3. 运动学基础（二）：正运动学求解（以单腿为例）、DH参数法
好，我们继续。上一章聊了四足机器人的基本构型和自由度分配，算是打了个底。这一章，咱们得动真格的了——正运动学求解。
说白了，正运动学就是：给定关节角度，算出腿末端（足端）在空间中的位置和姿态。你给电机发指令，关节转到某个角度，脚到底踩在哪？这就是正运动学要回答的问题。
我个人习惯，每次做仿真之前，先把单腿的正运动学模型搭好。腿算对了，整机才能跑起来。你想想看，一条腿都算不准，四条腿一起动，那画面太美我不敢看。
3.1 为什么需要DH参数法？
刚接触机器人学时，我也有个困惑：描述连杆之间的位姿关系，直接用几何法不行吗？
行，当然行。但问题是——不通用。每条腿的构型一变，几何推导就得重来一遍。DH参数法（Denavit-Hartenberg）就是来解决这个问题的。
核心思想：用4个参数，唯一确定相邻两个连杆之间的坐标变换关系。不管你的腿是串联还是并联，是3自由度还是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（Modified DH），因为它的坐标系附着在连杆的近端，更符合直觉。
避坑指南：我曾经在仿真里把标准DH和改进DH的参数混着用，结果足端位置算出来差了半个机身。排查了整整一天才发现是坐标系附着位置搞反了。所以，选定一种，从头到尾保持一致。
3.2 单腿DH参数建模（以髋关节横滚-髋关节俯仰-膝关节俯仰为例）
咱们以最常见的3自由度单腿为例：髋关节横滚（Hip Roll）→ 髋关节俯仰（Hip Pitch）→ 膝关节俯仰（Knee Pitch）。
先画个简图，把坐标系标出来：
对应的DH参数表（改进DH）：
| 连杆 i | ai-1 | αi-1 | di | θi |
|---|---|---|---|---|
| 1（髋横滚） | 0 | 0° | 0 | θ1 |
| 2（髋俯仰） | L1 | 90° | 0 | θ2 |
| 3（膝关节） | L2 | 0° | 0 | θ3 |
其中 L1 是大腿长度，L2 是小腿长度。θ1、θ2、θ3 就是三个关节的实时角度。
3.3 从DH参数到齐次变换矩阵
有了DH参数，下一步就是把它变成数学上能算的东西——齐次变换矩阵。
改进DH的相邻连杆变换矩阵公式：
i-1T_i = RotX(α_{i-1}) · TransX(a_{i-1}) · RotZ(θ_i) · TransZ(d_i)
展开成4x4矩阵：
[ cosθ_i -sinθ_i 0 a_{i-1} ]
[ sinθ_i·cosα_{i-1} cosθ_i·cosα_{i-1} -sinα_{i-1} -d_i·sinα_{i-1} ]
[ sinθ_i·sinα_{i-1} cosθ_i·sinα_{i-1} cosα_{i-1} d_i·cosα_{i-1} ]
[ 0 0 0 1 ]
看着有点吓人，对吧？别怕，咱们一条腿一条腿地算。
3.4 单腿正运动学代码实现
纸上谈兵没意思，直接上代码。我个人习惯用NumPy做矩阵运算，清晰又好调试。
import numpy as np
def dh_transform(a, alpha, d, theta):
"""
改进DH参数 -> 4x4齐次变换矩阵
a: 连杆长度
alpha: 连杆扭角（弧度）
d: 连杆偏距
theta: 关节角（弧度）
"""
ct = np.cos(theta)
st = np.sin(theta)
ca = np.cos(alpha)
sa = np.sin(alpha)
T = np.array([
[ct, -st, 0, a],
[st*ca, ct*ca, -sa, -d*sa],
[st*sa, ct*sa, ca, d*ca],
[0, 0, 0, 1]
])
return T
def leg_forward_kinematics(theta1, theta2, theta3, L1, L2):
"""
单腿正运动学求解
theta1: 髋横滚角（弧度）
theta2: 髋俯仰角（弧度）
theta3: 膝关节角（弧度）
L1: 大腿长度
L2: 小腿长度
返回：足端在基坐标系下的位置 (x, y, z)
"""
# 连杆1：髋横滚
T01 = dh_transform(0, 0, 0, theta1)
# 连杆2：髋俯仰（扭角90°）
T12 = dh_transform(L1, np.pi/2, 0, theta2)
# 连杆3：膝关节
T23 = dh_transform(L2, 0, 0, theta3)
# 从基座到足端的总变换
T03 = T01 @ T12 @ T23
# 足端位置（平移部分）
x = T03[0, 3]
y = T03[1, 3]
z = T03[2, 3]
return x, y, z
# 示例：所有关节角度为0
x, y, z = leg_forward_kinematics(0, 0, 0, 0.3, 0.35)
print(f"足端位置: x={x:.3f}, y={y:.3f}, z={z:.3f}")
# 输出：足端位置: x=0.650, y=0.000, z=0.000
小技巧：调试的时候，先把所有关节角设成0，看看足端位置是不是在预期位置（大腿+小腿长度方向）。如果不对，先检查DH参数表，再检查矩阵乘法顺序。我每次搭新模型都这么干，能省不少排查时间。
3.5 验证与可视化
代码写完了，怎么知道对不对？我的做法是：给一组已知角度，手算一遍，再跟代码结果对比。
举个例子：
- θ1 = 0°（髋横滚不转）
- θ2 = 30°（髋俯仰向前摆30°）
- θ3 = -60°（膝关节向后弯60°）
- L1 = 0.3 m, L2 = 0.35 m
手算一下足端在X-Z平面上的投影：
大腿末端位置（相对髋关节）：
x_hip = L1 * cos(30°) = 0.3 * 0.866 = 0.260 m
z_hip = L1 * sin(30°) = 0.3 * 0.500 = 0.150 m
小腿末端位置（相对膝关节，注意膝关节角度是相对于大腿的）：
膝关节相对水平面的角度 = 30° + (-60°) = -30°
x_knee = L2 * cos(-30°) = 0.35 * 0.866 = 0.303 m
z_knee = L2 * sin(-30°) = 0.35 * (-0.500) = -0.175 m
足端总位置：
x = 0.260 + 0.303 = 0.563 m
z = 0.150 + (-0.175) = -0.025 m
y = 0（髋横滚为0）
再用代码跑一遍：
x, y, z = leg_forward_kinematics(
np.radians(0),
np.radians(30),
np.radians(-60),
0.3, 0.35
)
print(f"足端位置: x={x:.3f}, y={y:.3f}, z={z:.3f}")
# 输出：足端位置: x=0.563, y=0.000, z=-0.025
对上了。嗯，心里踏实了。
注意：手算时一定要搞清楚角度正负方向。我见过不少新手把膝关节的弯曲方向搞反，结果仿真里腿直接往天上翘。建议统一约定：俯仰角向前为正，膝关节弯曲向后为负。
3.6 正运动学的工程意义
你可能会问：算个足端位置，有那么重要吗？
太重要了。正运动学是后面所有控制算法的基础：
- 步态规划：你得知道脚在什么位置，才能规划下一步踩哪
- 逆运动学：正运动学是逆运动学的反向验证工具
- 力控制：足端位置变化率就是速度，再求导就是加速度，这些是阻抗控制的前提
- 碰撞检测：实时算足端位置，判断是否穿透地面或碰到障碍物
说白了，正运动学就是四足机器人仿真的地基。地基没打牢，上面盖什么楼都得塌。
3.7 本章小结
| 知识点 | 要点 |
|---|---|
| DH参数法 | 4个参数（a, α, d, θ）唯一确定相邻连杆变换 |
| 改进DH vs 标准DH | 坐标系附着位置不同，选定一种后不要混用 |
| 齐次变换矩阵 | 4x4矩阵，包含旋转和平移信息 |
| 正运动学求解 | 关节角 → 足端位置，矩阵连乘即可 |
| 验证方法 | 手算一组已知角度，与代码结果对比 |
这一章的内容就到这。代码和公式都有了，建议你打开编辑器，把上面的代码敲一遍，换几组角度试试。只有亲手跑通了，才算真正掌握。