2、机械臂运动学基础：刚体变换与齐次坐标、DH参数建模法、正运动学求解（以6轴机械臂为例）、逆运动学数值解法（迭代法）
各位同学，欢迎来到机械臂运动学这一章。说实话，很多做视觉SLAM的朋友一听到「运动学」就头大，觉得那是搞机械臂的人的事。但你想，我们要让机械臂跟视觉系统协同工作，总得知道它怎么动吧？这一章，我就带你把这些基础啃下来。
2.1 刚体变换与齐次坐标
先聊个最基本的：怎么描述一个物体在空间里的位置和姿态？
位置好办，三个坐标 (x, y, z) 就搞定了。但姿态呢？光说「它朝那边」可不行。我们得用数学语言精确描述。
常用的姿态表示方法有三种：
- 旋转矩阵：3x3 矩阵，正交且行列式为 1。直观，但参数冗余（9个参数，实际只有3个自由度）。
- 欧拉角：绕三个轴依次旋转的角度。直观，但有万向锁问题。我在做协作机器人时，就踩过这个坑——欧拉角在奇异点附近突然跳变，导致轨迹规划直接崩了。
- 四元数：4 个参数，无万向锁，插值平滑。SLAM 和机器人领域最常用。
那怎么把位置和姿态合在一起呢？齐次坐标登场了。
说白了，齐次坐标就是在三维坐标后面加一个维度 w。一个点 (x, y, z) 变成 (x, y, z, 1)，一个向量变成 (x, y, z, 0)。这样，旋转和平移就能统一写成一个 4x4 的齐次变换矩阵：
| R t |
| 0 1 |其中 R 是 3x3 旋转矩阵，t 是 3x1 平移向量。这个矩阵可以同时描述旋转和平移，而且可以连续乘起来表示坐标系的链式变换。
2.2 DH参数建模法
好，现在我们要给机械臂建模。6轴机械臂有6个关节，每个关节连接一个连杆。怎么用统一的参数描述它们之间的关系？
Denavit-Hartenberg 参数法，简称 DH 参数，就是干这个的。它用 4 个参数描述相邻两个坐标系之间的变换：
| 参数 | 符号 | 含义 |
|---|---|---|
| 连杆长度 | ai | 沿 xi 轴，从 zi-1 到 zi 的距离 |
| 连杆扭角 | αi | 绕 xi 轴，从 zi-1 到 zi 的转角 |
| 连杆偏距 | di | 沿 zi-1 轴，从 xi-1 到 xi 的距离 |
| 关节角 | θi | 绕 zi-1 轴，从 xi-1 到 xi 的转角 |
对于旋转关节，θi 是变量；对于移动关节，di 是变量。6轴机械臂通常全是旋转关节，所以 θ1 到 θ6 就是我们要控制的关节角度。
建立 DH 参数表的步骤：
- 给每个关节分配一个坐标系，z 轴沿关节轴线方向。
- 确定 x 轴方向：沿相邻 z 轴的公垂线。
- 按顺序填入 a, α, d, θ 四个参数。
下面我以常见的 6 轴工业机械臂为例，给出一组典型的 DH 参数表：
| 关节 i | ai (mm) | αi (rad) | di (mm) | θi |
|---|---|---|---|---|
| 1 | 0 | π/2 | d1 | θ1 |
| 2 | a2 | 0 | 0 | θ2 |
| 3 | a3 | 0 | 0 | θ3 |
| 4 | 0 | π/2 | d4 | θ4 |
| 5 | 0 | -π/2 | 0 | θ5 |
| 6 | 0 | 0 | d6 | θ6 |
有了这张表，我们就可以用统一的公式计算相邻坐标系的变换矩阵了。
2.3 正运动学求解
正运动学很简单：给定 6 个关节角，求末端执行器的位姿。
公式就是连乘：
T_06 = T_01 * T_12 * T_23 * T_34 * T_45 * T_56其中每个 T_(i-1,i) 由 DH 参数计算得到：
T_(i-1,i) = Rot(z, θ_i) * Trans(z, d_i) * Trans(x, a_i) * Rot(x, α_i)写成矩阵形式：
| cosθ_i -sinθ_i cosα_i sinθ_i sinα_i a_i cosθ_i |
| sinθ_i cosθ_i cosα_i -cosθ_i sinα_i a_i sinθ_i |
| 0 sinα_i cosα_i d_i |
| 0 0 0 1 |把 6 个矩阵乘起来，就得到末端位姿了。代码实现很简单，用 Eigen 或 ROS 的 tf2 库都能轻松搞定。
2.4 逆运动学数值解法（迭代法）
逆运动学就麻烦多了：给定末端位姿，求 6 个关节角。
为什么麻烦？因为：
- 可能有多个解（比如肘部向上或向下）。
- 可能无解（目标位姿超出工作空间）。
- 解析解只适用于特定构型的机械臂（比如有相邻三轴交于一点）。
对于通用 6 轴机械臂，我推荐用数值解法。最常用的是迭代法，核心思想是：
- 从初始关节角 θ 开始。
- 计算当前末端位姿与目标位姿的误差 Δx。
- 用雅可比矩阵 J 把 Δx 映射到关节角增量 Δθ。
- 更新 θ = θ + Δθ。
- 重复直到误差小于阈值。
雅可比矩阵 J 描述了关节角速度与末端速度的关系：
Δx = J * Δθ所以 Δθ = J-1 * Δx。但 J 不一定是方阵，或者可能奇异。这时我们用伪逆：
Δθ = J+ * Δx其中 J+ = JT * (J * JT)-1。
下面是一个简单的迭代逆运动学伪代码：
function inverse_kinematics(target_pose, initial_theta, max_iter=100, tol=1e-6):
theta = initial_theta
for i in range(max_iter):
current_pose = forward_kinematics(theta)
error = target_pose - current_pose
if norm(error) < tol:
return theta
J = compute_jacobian(theta)
delta_theta = pinv(J) * error
theta = theta + delta_theta
return theta # 可能未收敛数值解法的优点是通用，不依赖机械臂构型。缺点是计算量大，且可能陷入局部最优。实际应用中，我通常先用解析法求一个近似解，再用数值法精调。
本章知识体系
下面这张图总结了本章的核心逻辑：