运动学基础：正运动学与逆运动学、D-H参数法、雅可比矩阵在轨迹跟踪中的作用
各位做机器人控制的朋友，咱们今天聊点硬核的。运动学，说白了就是研究机器人“怎么动”的学问。你给关节发指令，末端到底走到哪？反过来，你想让末端去抓杯子，关节该转多少度？这就是正逆运动学要解决的问题。
我刚开始接触人形机器人时，觉得运动学就是套公式，没啥了不起。直到有一次调试双足行走，机器人走着走着突然“劈叉”了——嗯，就是因为逆运动学求解时没处理好奇异点。从那以后，我再也不敢小看这些基础了。
1. 正运动学：从关节到末端
正运动学很简单：已知每个关节的角度，求末端执行器的位姿。用人话说就是——你拧了电机，手到底到了哪？
对于人形机器人，我们通常把身体分成多个链：左臂、右臂、左腿、右腿、躯干。每条链都是一个开链结构。正运动学就是沿着这个链，一级一级往下算。
核心公式：
T = T0 · T1 · T2 · … · Tn
其中每个 Ti 是相邻两个坐标系之间的齐次变换矩阵。
我习惯把正运动学比作“多米诺骨牌”——你推倒第一块，后面的连锁反应是确定的。只要关节角度给定，末端位姿就是唯一确定的。这一点在轨迹跟踪中特别重要：你规划了一条末端轨迹，得先能算出来每个时刻末端应该在哪，才能反推关节怎么动。
2. D-H参数法：给机器人建个“身份证”
D-H参数法，全称Denavit-Hartenberg参数法。说白了，就是给机器人的每个关节和连杆建立坐标系，然后用四个参数来描述它们之间的关系。
这四个参数分别是：
| 参数 | 符号 | 含义 | 我常犯的错 |
|---|---|---|---|
| 连杆长度 | ai | 沿xi轴，从zi-1到zi的距离 | 符号搞反，正负号没注意 |
| 连杆扭角 | αi | 绕xi轴，从zi-1到zi的角度 | 右手定则记混 |
| 连杆偏距 | di | 沿zi-1轴，从xi-1到xi的距离 | 移动关节和旋转关节搞混 |
| 关节角 | θi | 绕zi-1轴，从xi-1到xi的角度 | 初始零位没校准 |
有了这四个参数，相邻连杆的变换矩阵就能写出来了：
T_i = Rot(z, θ_i) · Trans(z, d_i) · Trans(x, a_i) · Rot(x, α_i)
写成矩阵形式：
[cosθ_i -sinθ_i·cosα_i sinθ_i·sinα_i a_i·cosθ_i]
[sinθ_i cosθ_i·cosα_i -cosθ_i·sinα_i a_i·sinθ_i]
[0 sinα_i cosα_i d_i ]
[0 0 0 1 ]
我的小技巧：建D-H表时，先把所有关节的坐标系画出来，标清楚z轴方向。坐标系画对了，参数自然就对了。我曾经因为一个z轴方向画反，排查了整整两天。
3. 逆运动学：从末端到关节
逆运动学就麻烦多了。已知末端位姿，求关节角度。说白了就是——手要摸到那个点，每个电机该转多少？
为什么麻烦？因为：
- 多解性：同一个末端位姿，可能有多种关节角度组合。比如你伸手够一个东西，可以肘部朝下，也可以肘部朝外。
- 无解性：目标点超出了机器人的工作空间。你想想看，让一个1米5的机器人去够2米高的柜子，怎么可能？
- 奇异点：某些位姿下，关节速度会突变。比如手臂完全伸直时，肘关节的微小变化会导致末端剧烈抖动。
我常用的逆运动学求解方法有两种：
| 方法 | 适用场景 | 优点 | 缺点 |
|---|---|---|---|
| 解析法 | 自由度≤6，结构简单 | 速度快，精度高 | 通用性差，每换结构要重推 |
| 数值法（迭代） | 任意自由度，复杂结构 | 通用性强 | 速度慢，可能不收敛 |
对于人形机器人，我建议优先用解析法。为什么？因为实时性要求高。你想想看，机器人走路时每秒钟要算几十次逆运动学，数值法根本扛不住。
避坑指南：我曾经在NAO机器人上用过数值法做逆运动学，结果走路时关节角度跳变，机器人直接摔了。后来换成解析法，配合关节限位约束，才稳定下来。记住：实时控制场景，能解析就别数值。
4. 雅可比矩阵：速度与力的桥梁
雅可比矩阵，这是轨迹跟踪的核心中的核心。它描述的是关节速度与末端速度之间的关系。
公式很简单：
v = J(q) · q̇
其中v是末端速度（线速度+角速度），q̇是关节速度，J就是雅可比矩阵。
但它的作用远不止于此：
- 速度映射：给定关节速度，算末端速度（正雅可比）。反过来，给定末端速度，算关节速度（逆雅可比）。
- 力映射：τ = JT · F，关节力矩与末端力的关系。你想想看，机器人要推一个重物，每个关节该出多少力？就是靠这个。
- 奇异点检测：当J的行列式为0时，机器人处于奇异位形。这时候某些方向的速度会失控。
轨迹跟踪中的关键用法：
在轨迹跟踪中，我们通常这样用雅可比矩阵：
- 规划末端轨迹，得到期望的末端位置和速度
- 用逆雅可比把末端速度映射到关节速度
- 把关节速度积分得到关节角度
- 用正运动学验证末端位置是否匹配
我习惯在代码里这样实现：
// 伪代码：基于雅可比矩阵的轨迹跟踪
void trackTrajectory(Trajectory traj) {
for (each time step) {
// 1. 获取期望末端位姿和速度
Pose desiredPose = traj.getPose(t);
Twist desiredTwist = traj.getTwist(t);
// 2. 计算当前末端位姿（正运动学）
Pose currentPose = forwardKinematics(jointAngles);
// 3. 计算位姿误差
PoseError error = desiredPose - currentPose;
// 4. 计算雅可比矩阵
Matrix J = computeJacobian(jointAngles);
// 5. 加入反馈项，计算期望关节速度
Vector qd_dot = pinv(J) * (desiredTwist + K * error);
// 6. 积分得到关节角度
jointAngles += qd_dot * dt;
}
}
我的经验：纯前馈（只用desiredTwist）不够稳，一定要加反馈项（K * error）。K矩阵我一般先设成对角阵，对角线取值在5-20之间。太小了跟踪慢，太大了会震荡。具体数值得根据机器人惯量和关节刚度来调。
5. 知识体系总览
说了这么多，咱们用一张图把整个运动学基础串起来：
这张图把咱们今天讲的内容串起来了。你想想看，D-H参数是地基，正逆运动学是柱子，雅可比矩阵是横梁，轨迹跟踪就是盖在上面的房子。地基不稳，房子就塌。