3. 运动学与动力学基础：正逆运动学、雅可比矩阵、拉格朗日动力学建模
各位同学，欢迎来到第三章。说实话，这一章是后面所有力控算法的“地基”。你想想看，模仿学习要控制机器人去柔顺地接触环境，你总得知道机器人当前在哪儿、关节怎么动、力怎么传吧？
我个人习惯把运动学和动力学比作“骨架”和“肌肉”。运动学管的是位置、速度、加速度这些几何关系；动力学管的是力、力矩、质量、惯量这些物理量。两者缺一不可。
3.1 正运动学：从关节空间到任务空间
正运动学，说白了就是：给定每个关节的角度，求末端执行器在笛卡尔空间的位置和姿态。
我记得刚入行时，带我的老工程师丢给我一本《机器人学导论》，说：“你把DH参数搞明白，就算入门了。” 我当时还不太理解，后来做项目才发现，DH参数确实是正运动学的核心。
3.1.1 DH参数法
Denavit-Hartenberg参数法，用四个参数来描述相邻连杆之间的坐标变换：
- ai-1：连杆长度，沿Xi-1轴从Zi-1到Zi的距离
- αi-1：连杆扭转角，绕Xi-1轴从Zi-1到Zi的角度
- di：连杆偏距，沿Zi轴从Xi-1到Xi的距离
- θi：关节角，绕Zi轴从Xi-1到Xi的角度
有了这四个参数，相邻连杆的齐次变换矩阵就是：
i-1T_i = Rot(X, αi-1) · Trans(X, ai-1) · Rot(Z, θi) · Trans(Z, di)把每个关节的变换矩阵连乘起来，就得到末端相对于基座的位姿：
0T_n = 0T_1 · 1T_2 · ... · n-1T_n3.2 逆运动学：从任务空间回到关节空间
逆运动学正好反过来：已知末端位姿，求各关节角度。这比正运动学难多了，因为解不唯一，甚至可能无解。
为什么会这样？你想想看，一个6轴机器人到达同一个位置，可能有8组不同的关节角度（肘上/肘下、腕翻转等）。
3.2.1 解析法与数值法
| 方法 | 优点 | 缺点 | 适用场景 |
|---|---|---|---|
| 解析法（封闭解） | 速度快、精度高、所有解都能找到 | 只适用于满足Pieper准则的机器人 | 工业6轴机器人（后三轴交于一点） |
| 数值法（迭代法） | 通用性强，任意构型都能用 | 速度慢、可能陷入局部最优、需要初值 | 冗余机器人、非球形腕机器人 |
我在项目中遇到过一个问题：用数值法做逆解时，初值给得不好，迭代半天不收敛。后来我改成先用解析法算一个近似解，再用数值法微调，效果好了很多。
3.3 雅可比矩阵：速度与力的“翻译官”
雅可比矩阵，是连接关节空间和任务空间的桥梁。它把关节速度映射到末端速度，也把末端力映射到关节力矩。
3.3.1 几何雅可比
对于旋转关节，雅可比矩阵的每一列可以这样计算：
Ji = [ zi-1 × (pn - pi-1) ]
[ zi-1 ]其中zi-1是关节轴方向，pn是末端位置，pi-1是关节原点位置。
有了雅可比矩阵，速度映射就是：
ẋ = J · q̇力映射就是（利用虚功原理）：
τ = JT · F3.3.2 奇异性分析
当雅可比矩阵不满秩时，机器人处于奇异位形。这时候，某些方向的运动能力会丧失，关节速度会趋于无穷大。
我记得有一次调试焊接机器人，在某个姿态下末端突然剧烈抖动。一查，原来是腕部接近奇异点，雅可比矩阵的条件数太大了。
3.4 拉格朗日动力学建模
动力学，就是研究力和运动的关系。拉格朗日法从能量角度出发，比牛顿-欧拉法更系统化，尤其适合复杂机器人系统。
3.4.1 拉格朗日方程
拉格朗日函数定义为系统动能减去势能：
L = K - P拉格朗日方程：
d/dt (∂L/∂q̇) - ∂L/∂q = τ展开后，得到标准形式：
M(q)q̈ + C(q, q̇)q̇ + G(q) = τ其中：
- M(q)：惯性矩阵，对称正定
- C(q, q̇)：科里奥利力和离心力矩阵
- G(q)：重力向量
- τ：关节力矩向量
3.4.2 建模步骤
- 定义广义坐标：通常选关节角度q
- 计算各连杆的动能和势能：需要知道质心位置、质量、惯性张量
- 构造拉格朗日函数：L = K - P
- 代入拉格朗日方程：求偏导，整理成标准形式
3.5 知识体系总览
下面这张图，把本章的核心知识点串起来了。你可以把它当作一个“导航图”，随时回来对照。
3.6 本章小结
这一章的内容，说实话有点“硬”。但这些都是后面做模仿学习和柔顺力控绕不开的基础。
- 正运动学：让你知道机器人“在哪儿”
- 逆运动学：让你知道机器人“怎么去”
- 雅可比矩阵：让你知道“速度怎么映射、力怎么传递”
- 拉格朗日动力学：让你知道“力怎么产生运动”
我个人建议，学完这一章后，找一台真实的机器人（或者仿真环境），把DH参数标出来，写代码算一遍正逆解，再算一遍雅可比矩阵。只有亲手写过代码，这些公式才能真正变成你自己的东西。