2. 机械臂运动学与动力学基础：齐次变换矩阵、DH参数法、牛顿-欧拉与拉格朗日动力学建模
各位同学，大家好。今天我们聊点硬核的——机械臂的运动学和动力学基础。说实话，这部分内容我当年学的时候也觉得枯燥，一堆矩阵、一堆公式。但后来在实际项目中吃过亏，才明白这些基础有多重要。你想想看，如果连机器人怎么动、需要多大扭矩都算不清楚，那力位混合控制就是空中楼阁。
我个人习惯，讲运动学之前，先让大家脑子里有个画面：机械臂的每个关节，本质上就是一个坐标系在空间中的旋转和平移。我们要做的，就是把这些变换用数学语言描述清楚。
2.1 齐次变换矩阵：描述空间位姿的“通用语言”
说白了，齐次变换矩阵就是一个4x4的矩阵，它把旋转和平移打包在一起。我刚开始做项目时，总喜欢把旋转矩阵和平移向量分开算，结果代码越写越乱。后来发现，用齐次变换矩阵统一处理，清爽多了。
一个标准的齐次变换矩阵长这样：
T = [ R p ]
[ 0 1 ]其中，R是3x3的旋转矩阵，p是3x1的平移向量。最后一行是固定的[0 0 0 1]。
为什么叫“齐次”？因为它在三维坐标的基础上加了一个维度，把旋转和平移统一成线性变换。嗯，这里要注意：齐次坐标的最后一个分量是1，表示点；如果是0，表示方向向量。这个细节我见过不少新手搞混。
2.2 DH参数法：建立机械臂运动学模型的“标准套路”
DH参数法，全称是Denavit-Hartenberg参数法。我个人觉得，这是机械臂运动学建模最优雅的方法之一。它用四个参数——a、α、d、θ——就能描述相邻两个关节坐标系之间的变换关系。
这四个参数的含义：
- a：连杆长度，沿x轴方向的平移距离
- α：连杆扭转角，绕x轴旋转的角度
- d：连杆偏距，沿z轴方向的平移距离
- θ：关节角，绕z轴旋转的角度
我在项目中遇到过一个问题：用标准DH法建模时，如果相邻关节的轴线平行，会出现奇异情况。后来我改用改进DH法（也叫Craig版），把参数定义在连杆的远端，这个问题就解决了。所以，我建议大家在建模前，先想清楚用标准DH还是改进DH。
下面是一个六轴机械臂的DH参数表示例：
| 关节 i | ai-1 (mm) | αi-1 (rad) | di (mm) | θi (rad) |
|---|---|---|---|---|
| 1 | 0 | 0 | 0.3 | θ1 |
| 2 | 0.5 | -π/2 | 0 | θ2 |
| 3 | 0.4 | 0 | 0 | θ3 |
| 4 | 0 | π/2 | 0.2 | θ4 |
| 5 | 0 | -π/2 | 0.2 | θ5 |
| 6 | 0 | π/2 | 0.1 | θ6 |
有了DH参数，我们就可以通过连乘齐次变换矩阵，得到末端执行器相对于基座的位姿。这就是所谓的正运动学。
2.3 牛顿-欧拉动力学建模：从力到运动的“递推之路”
运动学只关心位置、速度和加速度，不关心力。但力位混合控制，核心就是力和位置的协调。所以，动力学建模是绕不开的。
牛顿-欧拉法，说白了就是基于牛顿第二定律和欧拉方程，对每个连杆进行受力分析。它分为两步：
- 向外递推：从基座到末端，计算每个连杆的角速度、角加速度、线加速度
- 向内递推：从末端到基座，计算每个关节的力和力矩
我曾经在一个协作机器人项目里，直接用牛顿-欧拉法做实时动力学计算。当时遇到一个坑：惯性张量参数没校准，导致计算出的关节力矩偏差很大。后来用实验辨识法重新标定了惯性参数，才把精度提上来。
这里给出一段简化的牛顿-欧拉递推代码（仅示意核心逻辑）：
// 向外递推：计算连杆的运动学量
for (i = 1; i <= n; i++) {
omega[i] = omega[i-1] + qd[i] * z[i-1];
alpha[i] = alpha[i-1] + qdd[i] * z[i-1] + cross(omega[i-1], qd[i] * z[i-1]);
a[i] = a[i-1] + cross(alpha[i], r[i]) + cross(omega[i], cross(omega[i], r[i]));
}
// 向内递推：计算关节力/力矩
for (i = n; i >= 1; i--) {
F[i] = m[i] * a[i];
N[i] = I[i] * alpha[i] + cross(omega[i], I[i] * omega[i]);
f[i] = f[i+1] + F[i];
tau[i] = tau[i+1] + cross(r[i], f[i]) + cross(r_com[i], F[i]) + N[i];
}2.4 拉格朗日动力学建模：从能量到运动的“全局视角”
拉格朗日法跟牛顿-欧拉法不同。它不关心每个连杆的受力细节，而是从系统的总动能和总势能出发，推导出运动方程。我个人觉得，拉格朗日法更适合理论分析，而牛顿-欧拉法更适合实时计算。
拉格朗日函数定义为：
L = K - P其中，K是总动能，P是总势能。然后，运动方程由下式给出：
d/dt (∂L/∂q̇) - ∂L/∂q = τ展开后，可以得到标准形式：
M(q) q̈ + C(q, q̇) q̇ + G(q) = τ这里，M(q)是惯性矩阵，C(q, q̇)是科里奥利力和离心力矩阵，G(q)是重力项。这个形式在自适应控制中非常有用，因为我们可以把未知参数分离出来，设计自适应律。
我在做力位混合控制时，就经常用到这个形式。比如，当机械臂末端与环境接触时，动力学方程变成：
M(q) q̈ + C(q, q̇) q̇ + G(q) = τ + J^T F_ext其中，J是雅可比矩阵，F_ext是末端受到的外力。这个方程，就是力位混合控制的基础。
2.5 本章知识体系总览
为了让大家更直观地理解本章的知识结构，我画了一张图。这张图展示了从运动学到动力学，再到力位混合控制的逻辑链条。
这张图里，运动学是基础，动力学是桥梁，力位混合控制是最终目标。三者环环相扣，缺一不可。
好了，这一章的内容就到这里。运动学和动力学是机械臂控制的基石，也是后续自适应控制设计的前提。希望大家能把这些矩阵和公式真正理解透，而不是死记硬背。下一章，我们会把这些知识用到力位混合控制中，看看它们到底怎么发挥作用。