第4章 数学基础回顾（二）：齐次变换矩阵与DH参数法
好，咱们接着聊。上一章我们把向量、旋转矩阵这些基础工具捋了一遍。这一章，我们要解决一个更实际的问题：怎么用数学描述一个机械臂的各个关节和连杆。
你想想看，一个六轴机械臂，从基座到末端执行器，中间经过了六个旋转关节和六根连杆。每个关节一转，末端的位置和姿态就变了。怎么把这种“关节角度 → 末端位姿”的关系用数学表达出来？
答案就是两个东西：齐次变换矩阵 和 DH参数法。这两个工具，是我做机器人运动学绕不开的坎。当年我刚入门时，对着书本看了三天没看懂，后来在项目里摔了几次跟头，才真正明白它们到底在干什么。
核心一句话：齐次变换矩阵是“描述坐标系之间相对位置和姿态”的数学工具；DH参数法是“用四个参数标准化描述相邻连杆坐标系”的建模方法。两者结合，就能从基座一路算到末端。
4.1 齐次变换矩阵：把旋转和平移装进一个盒子里
先说说齐次变换矩阵。说白了，它就是一个4×4的矩阵，能把旋转和平移打包在一起。为什么是4×4？因为3×3的旋转矩阵只能处理旋转，平移得单独加一个向量。齐次坐标引入后，我们用四个分量 (x, y, z, w) 表示一个点，w=1 时就是普通的三维点。
齐次变换矩阵的标准形式长这样：
| R11 R12 R13 tx |
| R21 R22 R23 ty |
| R31 R32 R33 tz |
| 0 0 0 1 |左上角3×3是旋转矩阵R，右上角3×1是平移向量t。最后一行固定是 [0 0 0 1]，这是齐次坐标的“身份标识”。
我的习惯：在STM32上实现时，我不会真的用4×4二维数组去存——太占内存。我一般用一个结构体，里面放一个3×3的旋转矩阵和一个3×1的平移向量，计算时再临时组装成4×4。省下来的RAM，留给其他任务不香吗？
齐次变换矩阵的威力在于链式乘法。假设我们有三个坐标系：基座 {0}、连杆1 {1}、连杆2 {2}。那么从 {0} 到 {2} 的变换就是：
T02 = T01 * T12其中 T01 是 {0} 到 {1} 的变换，T12 是 {1} 到 {2} 的变换。乘出来的结果 T02，直接给出了 {2} 相对于 {0} 的位置和姿态。这就是正运动学的数学本质——一连串矩阵乘到底。
4.2 DH参数法：给每个连杆发一张“身份证”
好，现在问题来了：怎么得到每个相邻坐标系之间的变换矩阵 T(i-1)i？
DH参数法就是干这个的。它用四个参数来描述相邻两个连杆坐标系之间的关系：
| 参数 | 符号 | 物理含义 | 我的理解 |
|---|---|---|---|
| 关节角 | θ | 绕Z轴旋转的角度 | 对于旋转关节，这就是电机转的角度 |
| 连杆偏距 | d | 沿Z轴平移的距离 | 两个关节轴之间的偏移量 |
| 连杆长度 | a | 沿X轴平移的距离 | 连杆本身的长度，从当前Z轴到下一个Z轴 |
| 连杆扭角 | α | 绕X轴旋转的角度 | 两个Z轴之间的夹角 |
有了这四个参数，相邻坐标系的变换矩阵 T(i-1)i 就可以直接写出来：
T(i-1)i = Rot(z, θ) * Trans(z, d) * Trans(x, a) * Rot(x, α)展开后就是：
| cosθ -sinθ·cosα sinθ·sinα a·cosθ |
| sinθ cosθ·cosα -cosθ·sinα a·sinθ |
| 0 sinα cosα d |
| 0 0 0 1 |我曾经踩过的坑：DH参数有两种约定——标准DH和改进DH。标准DH把坐标系建在连杆的前一个关节上，改进DH建在后一个关节上。我刚开始做项目时没注意这个区别，直接套用别人的DH参数表，结果算出来的末端位置差了十万八千里。后来花了整整两天才排查出来。所以，拿到DH参数表第一件事：确认是标准DH还是改进DH。
4.3 建立DH坐标系的步骤
我个人习惯按以下步骤来建立DH坐标系，每一步都有明确的目的：
- 标出所有关节轴：画出每个关节的旋转轴（Z轴）方向。
- 确定Z轴：每个关节的Z轴沿旋转轴方向。对于旋转关节，Z轴就是电机转轴。
- 确定X轴：X轴沿当前Z轴和下一个Z轴的公垂线方向，从当前Z轴指向下一个Z轴。
- 确定Y轴：用右手定则，Y = Z × X。
- 读取DH参数：根据坐标系之间的几何关系，读出θ、d、a、α四个值。
一个简单的例子：假设一个两连杆平面机械臂，两个关节都是旋转关节，连杆长度分别为a1和a2。DH参数表如下：
| 连杆 | θ | d | a | α |
|---|---|---|---|---|
| 1 | θ1 | 0 | a1 | 0 |
| 2 | θ2 | 0 | a2 | 0 |
代入公式，两个变换矩阵相乘，就能得到末端相对于基座的位姿。这就是正运动学的核心计算。
4.4 在STM32上实现时的注意事项
把DH参数法搬到STM32上，有几个地方要特别留意：
- 浮点精度：STM32的FPU是单精度的，float够用。但矩阵乘法累积误差会放大，我一般会在每个控制周期结束时做一次归一化，防止旋转矩阵漂移。
- 三角函数查表：如果控制频率很高（比如1kHz以上），每次计算都调用sin/cos函数会占用不少时间。我习惯在初始化时生成一个1024点的sin表，运行时查表加线性插值，速度能快3-5倍。
- 参数存储：DH参数是固定的（除了关节角θ），我把它放在const数组里，编译时直接烧进Flash，不占RAM。
我的代码习惯：我会把DH参数定义成一个结构体数组，每个元素包含θ、d、a、α四个float。然后写一个通用函数，输入DH参数和当前关节角，输出4×4变换矩阵。这样不管是6轴还是7轴机械臂，改一下参数表就行，代码不用动。
4.5 本章小结
齐次变换矩阵和DH参数法，是机器人运动学的两大基石。齐次变换矩阵给了我们一个统一的数学框架，DH参数法给了我们一个标准化的建模流程。两者结合，就能从机械臂的物理结构出发，一步步推导出正运动学方程。