4. 运动学基础：刚体运动学、轮式机器人运动学模型、机械臂运动学
各位同学，咱们今天聊点硬核的——运动学。说实话，我入行那会儿，觉得运动学就是一堆公式来回倒腾。直到有一次，我设计的AGV小车在工厂里原地打转，怎么都走不直……嗯，从那以后我才真正明白，运动学不是纸上谈兵，它是轨迹规划的根基。
这一章，我会把刚体运动学、轮式模型和机械臂正逆解揉碎了讲。你想想看，搞懂了这些，后面做路径规划、避障、轨迹优化，心里才有底。
4.1 刚体运动学：位置、姿态与坐标变换
刚体运动学，说白了就是描述一个物体在空间里怎么“动”。它不关心力，只关心位置和姿态的变化。
一个刚体在三维空间里有6个自由度：3个平移（x, y, z），3个旋转（roll, pitch, yaw）。我个人习惯用齐次变换矩阵来统一描述，这样平移和旋转可以一起算，代码写起来也清爽。
核心概念：
- 位置：用三维向量 p = [x, y, z]^T 表示
- 姿态：用旋转矩阵 R ∈ SO(3) 表示，或者用四元数、欧拉角
- 齐次变换矩阵：T = [R p; 0 1]，4x4矩阵，一步到位
我在项目中遇到过一个问题：用欧拉角做插值时，出现了“万向锁”。当时小车机械臂在抓取时突然卡住，查了半天才发现是角度表示方式的问题。后来我全部改用四元数，再也没出过类似毛病。
我的小建议：做轨迹规划时，尽量用四元数表示旋转。插值平滑，没有奇异性。欧拉角只适合给人看，不适合给机器算。
4.2 轮式机器人运动学模型
轮式机器人是轨迹规划里最常见的载体。差速和阿克曼，这两种模型几乎覆盖了90%的移动机器人场景。
4.2.1 差速模型
差速模型，说白了就是两个轮子独立驱动，靠速度差来转弯。结构简单，控制也直观。
它的运动学方程是这样的：
v = (v_r + v_l) / 2 # 线速度
ω = (v_r - v_l) / L # 角速度，L为轮距其中 v_r 和 v_l 分别是左右轮的速度。你想想看，如果 v_r = v_l，小车直行；如果 v_r = -v_l，小车原地旋转。
注意：差速模型有一个隐含约束——它不能侧向移动。也就是说，它的瞬时速度方向永远指向车头方向。这在狭窄空间里是个大麻烦。
我曾经在一个仓库项目里，用差速小车做密集货架间的穿梭。一开始路径规划没考虑这个约束，结果小车老是撞到货架腿。后来我在规划层加了一个“非完整约束”的检查，才把问题解决。
4.2.2 阿克曼模型
阿克曼模型就是汽车转向模型。前轮转向，后轮驱动。它的特点是：转弯时所有轮子的轴线交于一点（瞬时转动中心）。
运动学方程稍微复杂一点：
v = v_rear # 后轴中心速度
ω = v * tan(δ) / L_wheelbase # 角速度，δ为前轮转角阿克曼模型的好处是：高速行驶时稳定性好。坏处是：转弯半径大，没法原地掉头。
两种模型对比：
| 特性 | 差速模型 | 阿克曼模型 |
|---|---|---|
| 转弯方式 | 速度差 | 前轮转向 |
| 最小转弯半径 | 0（可原地旋转） | 大于0 |
| 高速稳定性 | 差 | 好 |
| 典型应用 | 室内AGV、扫地机 | 无人车、物流车 |
4.3 机械臂运动学：正解与逆解
机械臂运动学，是轨迹规划里绕不开的坎。正解简单，逆解麻烦。但实际工程中，我们天天跟逆解打交道。
4.3.1 正运动学
正运动学，就是已知关节角度，求末端位姿。说白了，给每个关节一个角度，算出手在哪儿。
标准做法是用DH参数法。我习惯用改进DH，因为它在处理树形结构时更直观。
# 一个简单的2自由度机械臂正解
def forward_kinematics(theta1, theta2, L1, L2):
x = L1 * cos(theta1) + L2 * cos(theta1 + theta2)
y = L1 * sin(theta1) + L2 * sin(theta1 + theta2)
return x, y正解是唯一的，给一组关节角，只有一种末端位姿。这个没什么好说的，就是矩阵乘法。
4.3.2 逆运动学
逆运动学就热闹了。已知末端位姿，求关节角度。这玩意儿有多解，甚至可能无解。
我记得有一次做焊接机器人离线编程，末端轨迹明明很平滑，但逆解出来的关节角却跳来跳去。后来发现是逆解时选了不同的解分支。嗯，这里要注意：逆解一定要做“连续性检查”，保证相邻时刻的解是连续的。
避坑指南：我曾经在逆解时只求了一组解，结果机器人走到奇异点附近，关节速度瞬间爆表。后来我改用数值解法（比如迭代法），加上阻尼因子，才把这个问题压下去。
逆解的常用方法：
- 解析法：速度快，但只适用于特定构型（如PUMA 560）
- 数值法：通用性强，但需要迭代，实时性差一些
- 几何法：直观，适合低自由度机械臂
4.4 知识体系总览
下面这张图，是我自己梳理的运动学知识框架。你把它记在脑子里，后面学轨迹规划时就不会迷路。
4.5 本章小结
运动学是轨迹规划的“语言”。不懂运动学，你写的轨迹就是空中楼阁。我个人觉得，学运动学最好的方式就是动手算一遍。拿一个2自由度机械臂，手推正解和逆解，再用代码验证。搞通了，后面学动力学、控制、规划，都会顺很多。
好了，这一章就到这儿。记住：运动学不难，难的是把它用对地方。
公众号:蓝海资料掘金营，微信deep3321