第一章 机器人运动学基础：位姿描述、齐次变换矩阵与DH参数法
各位同学，大家好。我是这门课的主讲工程师。今天咱们开始第一讲，也是整个轨迹规划的地基——机器人运动学基础。
说实话，很多新手一上来就急着写插补代码，结果算出来的轨迹机器人根本走不对。为什么？因为连机器人的“胳膊”在空间里到底怎么摆的都没搞清楚。所以，咱们先把这第一步踩实了。
1.1 位姿描述：机器人到底在哪儿？
要控制机器人，首先得知道它在哪儿、朝哪儿。这就是位姿描述要干的事。
位姿 = 位置 + 姿态。
- 位置：就是空间里的一个点。用三维坐标 (x, y, z) 表示。这个简单，初中几何就学过。
- 姿态：就是它“脸”朝哪个方向，“头顶”朝哪个方向。这个稍微复杂点，需要用旋转矩阵来描述。
我个人习惯把姿态想象成三个互相垂直的箭头，固定在机器人末端上。这三个箭头相对于世界坐标系的指向，就是姿态。
核心概念：一个刚体在空间中的自由度是6个（3个位置 + 3个姿态）。你想想看，如果只告诉机器人位置，不告诉它姿态，它可能“脸”朝下就把工件怼上去了，那不就出事故了吗？
1.2 齐次变换矩阵：把位置和姿态打包
有了位置和姿态，怎么把它们放在一起方便计算？答案就是齐次变换矩阵。
它是一个 4x4 的矩阵：
| R(3x3) P(3x1) |
| 0 0 0 1 |左上角 3x3 是旋转矩阵（姿态），右上角 3x1 是位置向量。最后一行是固定的 [0 0 0 1]。
为什么要加这一行？说白了，就是为了把旋转和平移统一成一次矩阵乘法。否则你得先旋转再平移，写两行代码，麻烦不说，还容易出错。
我在项目中遇到过一件事：有个同事用欧拉角算姿态，结果在奇异点附近机器人直接“抽搐”了一下。后来我建议他改用齐次变换矩阵，把姿态用旋转矩阵表示，问题就解决了。因为矩阵运算没有万向锁的问题。
我的小技巧：写代码时，我习惯把齐次变换矩阵定义成一个结构体，包含旋转矩阵 R 和平移向量 P。这样代码可读性高，也方便调试。
1.3 DH参数法：给机器人关节建个“身份证”
好了，现在我们知道怎么描述一个点的位姿了。但机器人有6个关节，每个关节都在动，怎么描述它们之间的关系？
这就轮到 DH参数法 登场了。Denavit 和 Hartenberg 这两位前辈想了个办法：用4个参数，就能唯一确定相邻两个连杆坐标系之间的变换关系。
这4个参数是：
| 参数 | 符号 | 含义 |
|---|---|---|
| 连杆长度 | ai-1 | 沿 Xi-1 轴，从 Zi-1 到 Zi 的距离 |
| 连杆扭角 | αi-1 | 绕 Xi-1 轴，从 Zi-1 转到 Zi 的角度 |
| 连杆偏距 | di | 沿 Zi 轴，从 Xi-1 到 Xi 的距离 |
| 关节角 | θi | 绕 Zi 轴，从 Xi-1 转到 Xi 的角度 |
嗯，这里要注意：DH参数有两种约定——标准DH和改进DH。区别在于坐标系建在连杆的前端还是后端。我个人习惯用改进DH，因为它在处理树形结构机器人时更直观。
1.4 建立连杆坐标系的步骤
光讲理论不行，咱们来走一遍实际步骤。我以最常见的六轴工业机器人为例：
- 找出所有关节轴：画出每个关节的旋转轴（Z轴）。
- 确定相邻轴的关系：相邻两个Z轴之间的公垂线，就是X轴的方向。
- 确定Y轴：用右手定则，Y = Z × X。
- 填写DH参数表：按照上面的表格，把 a、α、d、θ 填进去。
我曾经带过一个实习生，他画坐标系时把Z轴方向搞反了，结果算出来的正运动学全是错的。排查了整整一下午才发现。所以，坐标系方向一定要统一，我建议都用右手系。
避坑指南：我曾经在调试一个焊接机器人时，发现末端轨迹总是偏几毫米。查了半天，原来是DH参数里连杆长度 a 的单位搞错了——图纸上是毫米，我代码里写成了米。这种低级错误，一错就是千倍误差，大家千万注意单位统一。
1.5 从DH参数到齐次变换矩阵
有了DH参数，怎么得到相邻连杆的变换矩阵？公式如下：
T = Rot_x(α) * Trans_x(a) * Rot_z(θ) * Trans_z(d)展开成矩阵形式：
| cosθ -sinθ 0 a |
| sinθ*cosα cosθ*cosα -sinα -d*sinα |
| sinθ*sinα cosθ*sinα cosα d*cosα |
| 0 0 0 1 |你想想看，只要把每个关节的DH参数代进去，就能得到6个变换矩阵。把它们连乘起来，就是机器人末端相对于基座的位姿。这就是正运动学。
在实际项目中，我一般会写一个函数，输入DH参数，输出变换矩阵。然后循环调用6次，就得到末端位姿了。代码大概长这样：
def dh_transform(a, alpha, d, theta):
ct = cos(theta)
st = sin(theta)
ca = cos(alpha)
sa = sin(alpha)
T = np.array([
[ct, -st*ca, st*sa, a*ct],
[st, ct*ca, -ct*sa, a*st],
[0, sa, ca, d],
[0, 0, 0, 1]
])
return T这段代码我用了不下百次，从六轴机器人到协作机器人，再到AGV上的机械臂，都能用。说白了，DH参数法就是机器人运动学的“通用语言”。
1.6 本章小结
这一章咱们讲了三个核心概念：
- 位姿描述：位置+姿态，6个自由度。
- 齐次变换矩阵：把旋转和平移打包成4x4矩阵，方便计算。
- DH参数法：用4个参数描述相邻连杆关系，建立整个机器人的运动学模型。
这些是后续所有轨迹规划、插补算法的基础。下一章，咱们会讲正运动学与逆运动学，到时候你会看到，今天学的齐次变换矩阵和DH参数，是怎么让机器人“算”出自己该往哪儿动的。
好，今天就到这儿。有问题随时交流。