第4章 单腿正运动学建模：基于D-H参数法的连杆坐标系建立与变换矩阵推导
好，咱们进入正题。上一章我们把四足机器人的单腿简化成了连杆模型，今天要干的事很明确——给这条腿建立坐标系，然后算出从髋关节到脚端的位姿变换。说白了，就是给腿装上“GPS”，让每个关节角度都能映射到脚在空间里的精确位置。
我个人习惯，做运动学建模前一定会先问自己三个问题：
1. 这条腿有几个自由度？
2. 每个关节是旋转还是平移？
3. 坐标系怎么放才能让后面的计算最省事？
想清楚这三点，D-H参数法就是个顺理成章的工具。
4.1 为什么选D-H参数法？
你可能见过其他建模方法，比如直接用几何法硬算。但四足机器人每条腿至少3个自由度，几何法到后面会算到你怀疑人生。D-H参数法的好处是：标准化、流程化、适合编程。
我在做第一个四足机器人原型时，一开始偷懒用了几何法，结果换腿型时所有公式都要重推。后来换成D-H法，改几个参数就行，省了大把时间。
核心思想： 每个连杆用一个4×4齐次变换矩阵描述，矩阵由4个参数决定——连杆长度a、连杆扭角α、关节距离d、关节转角θ。对于旋转关节，θ是变量；对于移动关节，d是变量。
4.2 单腿连杆坐标系建立规则
坐标系怎么放？Craig的经典规则我用了很多年，这里给你提炼成三步：
- 找关节轴： 每个关节的旋转轴（或移动方向）就是z轴。从基座开始，依次标z₀、z₁、z₂…
- 定原点： 相邻两关节轴的公垂线与当前z轴的交点，就是坐标系原点。
- 定x轴： 沿公垂线方向，从当前z轴指向下一个z轴。
嗯，这里要注意——如果两轴相交，x轴垂直于两轴所在平面；如果两轴平行，你可以自由选择原点位置，但通常选在方便的地方。
我的小技巧： 画坐标系时，先用粗线画出所有关节轴，再画连杆。这样公垂线一目了然，不容易出错。
4.3 以三自由度单腿为例
咱们拿最常见的四足机器人单腿结构来说——髋关节横滚（Hip Roll）、髋关节俯仰（Hip Pitch）、膝关节俯仰（Knee Pitch）。三个旋转关节，串联结构。
先画个简图：
4.4 D-H参数表
坐标系画好后，接下来就是填表。我习惯按这个顺序来：先看a（连杆长度），再看α（连杆扭角），然后d（关节距离），最后θ（关节转角）。
对于我们的三自由度腿，D-H参数表如下：
| 连杆 i | ai-1 (mm) | αi-1 (°) | di (mm) | θi (°) |
|---|---|---|---|---|
| 1 (髋横滚) | 0 | 0 | 0 | θ₁ |
| 2 (髋俯仰) | 0 | 90 | L₁ | θ₂ |
| 3 (膝俯仰) | L₂ | 0 | 0 | θ₃ |
注意： α=90° 表示相邻两关节轴垂直。这里髋横滚轴（z₀）与髋俯仰轴（z₁）垂直，所以α₁=90°。我曾经在这里搞反过符号，结果仿真时腿往反方向踢，排查了半天才发现是α的正负号问题。
4.5 变换矩阵推导
有了D-H参数，变换矩阵就是套公式的事。标准公式长这样：
ᵢ⁻¹Tᵢ = Rot(x, αᵢ₋₁) · Trans(x, aᵢ₋₁) · Rot(z, θᵢ) · Trans(z, dᵢ)
展开后：
ᵢ⁻¹Tᵢ =
[ cosθᵢ -sinθᵢ 0 aᵢ₋₁ ]
[ sinθᵢ·cosαᵢ₋₁ cosθᵢ·cosαᵢ₋₁ -sinαᵢ₋₁ -dᵢ·sinαᵢ₋₁ ]
[ sinθᵢ·sinαᵢ₋₁ cosθᵢ·sinαᵢ₋₁ cosαᵢ₋₁ dᵢ·cosαᵢ₋₁ ]
[ 0 0 0 1 ]把参数代进去，一步步算：
连杆1变换（基座→髋横滚）：
⁰T₁ =
[ cosθ₁ -sinθ₁ 0 0 ]
[ sinθ₁ cosθ₁ 0 0 ]
[ 0 0 1 0 ]
[ 0 0 0 1 ]连杆2变换（髋横滚→髋俯仰）：
¹T₂ =
[ cosθ₂ -sinθ₂ 0 0 ]
[ 0 0 -1 -L₁ ]
[ sinθ₂ cosθ₂ 0 0 ]
[ 0 0 0 1 ]连杆3变换（髋俯仰→膝俯仰）：
²T₃ =
[ cosθ₃ -sinθ₃ 0 L₂ ]
[ sinθ₃ cosθ₃ 0 0 ]
[ 0 0 1 0 ]
[ 0 0 0 1 ]4.6 正运动学总变换
从基座到脚端的总变换，就是把三个矩阵乘起来：
⁰T₃ = ⁰T₁ · ¹T₂ · ²T₃我一般不会手算这个乘法——太容易出错。用MATLAB或Python的符号计算库，几行代码搞定。但为了让你理解结构，我给出最终形式：
⁰T₃ =
[ r₁₁ r₁₂ r₁₃ pₓ ]
[ r₂₁ r₂₂ r₂₃ pᵧ ]
[ r₃₁ r₃₂ r₃₃ p_z ]
[ 0 0 0 1 ]
其中脚端位置：
pₓ = L₂·cosθ₁·cosθ₂·cosθ₃ - L₂·cosθ₁·sinθ₂·sinθ₃ + L₂·cosθ₁·cosθ₂
pᵧ = L₂·sinθ₁·cosθ₂·cosθ₃ - L₂·sinθ₁·sinθ₂·sinθ₃ + L₂·sinθ₁·cosθ₂
p_z = -L₁ + L₂·sinθ₂·cosθ₃ + L₂·cosθ₂·sinθ₃ + L₂·sinθ₂验证方法： 把θ₁=0, θ₂=0, θ₃=0代入，看看脚端位置是不是(0, 0, -L₁+L₂)。如果是，说明你的推导基本正确。我每次推完都会做这个“零位验证”，能筛掉80%的笔误。
4.7 编程实现要点
在实际代码中，我建议把D-H参数和变换矩阵封装成函数。给你一个Python风格的伪代码参考：
def dh_transform(a, alpha, d, theta):
"""根据D-H参数生成4x4齐次变换矩阵"""
ct = cos(theta)
st = sin(theta)
ca = cos(alpha)
sa = sin(alpha)
T = [
[ct, -st, 0, a],
[st*ca, ct*ca, -sa, -d*sa],
[st*sa, ct*sa, ca, d*ca],
[0, 0, 0, 1]
]
return T
# 定义D-H参数表
dh_params = [
{'a': 0, 'alpha': 0, 'd': 0, 'theta': theta1},
{'a': 0, 'alpha': pi/2, 'd': L1, 'theta': theta2},
{'a': L2, 'alpha': 0, 'd': 0, 'theta': theta3}
]
# 计算总变换
T = identity(4)
for param in dh_params:
T = T @ dh_transform(**param)
# 提取脚端位置
foot_pos = T[0:3, 3]调试建议： 先单独验证每个关节的变换矩阵，再组合。比如只动θ₁，看脚端是不是绕z₀轴画圆。这样逐关节排查，问题定位快得多。
4.8 常见坑与避坑指南
- α符号搞反： 右手定则，从当前z轴转向下一个z轴，逆时针为正。我曾经在髋关节这里搞反，结果仿真时腿往外撇而不是往下蹬。
- d和a混淆： d是沿z轴的距离，a是沿x轴的距离。记不住的话，想想“d for displacement along z, a for along x”。
- 矩阵乘法顺序： 从基座往末端乘，顺序不能乱。我见过有人写成T₃·T₂·T₁，结果脚飞到天上去了。
- 角度单位： 代码里统一用弧度，别混着用。我吃过这个亏，仿真时腿抖得像抽风。
好了，正运动学建模就到这儿。你拿着这章的D-H参数表和变换矩阵，已经可以算出任意关节角度下脚端的位置了。下一件事——怎么根据脚端位置反推关节角度？那就是逆运动学的事了。