2. 机器人运动学基础：坐标变换、旋转矩阵、欧拉角、四元数、齐次变换矩阵

做机器人控制这些年，我最大的感触就是：运动学是根，坐标系是魂。你想想看，一个机械臂要抓杯子，它得先知道杯子在哪，自己的手在哪，然后才能算怎么过去。这背后全是坐标变换的活儿。今天咱们就把这块硬骨头啃下来。

2.1 坐标变换——机器人看世界的“语言”

说白了，坐标变换就是解决“同一个点，在不同坐标系下怎么描述”的问题。我刚开始做项目时，经常被坐标系搞晕——明明算好的位置，机器人就是抓不准。后来才发现，是基坐标系和工具坐标系没对齐。

举个例子：
你在世界坐标系下看到杯子在 (1, 2, 3) 处，但机器人的手有自己的坐标系。你得把杯子的坐标“翻译”成手坐标系下的值，机器人才能听懂。这个翻译过程，就是坐标变换。

核心公式：

点 P 在坐标系 {A} 中的描述为 ^AP，在坐标系 {B} 中的描述为 ^BP。如果知道 {B} 相对于 {A} 的位置和姿态，那么：

^AP = ^AR_B · ^BP + ^AP_Borg

其中 ^AR_B 是旋转矩阵，^AP_Borg 是平移向量。

2.2 旋转矩阵——描述“朝向”的硬核工具

旋转矩阵，我习惯叫它“方向描述器”。它是一个 3×3 的正交矩阵，行列式为 +1。每一列代表被旋转坐标系的一个轴在参考坐标系下的投影。

我记得有一次调试六轴机器人，末端姿态总差几度。查了半天，发现是旋转矩阵的数值精度问题——浮点数累积误差导致矩阵不再正交。从那以后，我每次用旋转矩阵都会做一次正交化处理。

// 绕 Z 轴旋转 θ 角的旋转矩阵
R_z(θ) = | cosθ  -sinθ  0 |
         | sinθ   cosθ  0 |
         | 0      0     1 |

// 绕 X 轴旋转 φ 角的旋转矩阵
R_x(φ) = | 1   0      0    |
         | 0  cosφ  -sinφ |
         | 0  sinφ   cosφ |

// 绕 Y 轴旋转 ψ 角的旋转矩阵
R_y(ψ) = | cosψ  0  sinψ |
         | 0     1  0    |
         | -sinψ 0  cosψ |

我的小技巧：

旋转矩阵的逆等于它的转置（R^-1 = R^T）。这个性质在求逆变换时特别好用，不用真的去算矩阵求逆。

2.3 欧拉角——直观但“有坑”的姿态表示

欧拉角用三个角度来描述姿态，比如 (α, β, γ)。听起来很直观对吧？但这里有个大坑——万向锁。

我曾经在写运动学逆解时，用欧拉角做插值。结果机器人走到某个位置突然“抽搐”了一下。排查后发现，是欧拉角遇到了万向锁——中间角度接近 90° 时，两个旋转轴重合了，自由度丢失了一个。

常见的欧拉角约定有几种：

约定名称	旋转顺序	常见应用
ZYX（RPY）	绕 Z → 绕 Y → 绕 X	移动机器人、无人机
ZYZ	绕 Z → 绕 Y → 绕 Z	工业机器人
XYZ（固定角）	绕 X → 绕 Y → 绕 Z	视觉系统标定

避坑指南：

我曾经在路径规划中直接用欧拉角做线性插值，结果轨迹不平滑。后来改用四元数球面线性插值（SLERP），问题就解决了。所以，做插值别用欧拉角，用四元数。

2.4 四元数——平滑旋转的“秘密武器”

四元数，说白了就是一个“带方向的旋转”。它由四个数组成：q = (w, x, y, z)，其中 w 是实部，(x, y, z) 是虚部。它没有万向锁问题，而且插值平滑。

我第一次接触四元数是在做视觉伺服时。相机采集到的姿态数据是四元数格式，我得把它转成机器人能理解的旋转矩阵。当时手算了一遍转换公式，才真正理解了它的几何意义。

// 四元数转旋转矩阵
R = | 1-2(y²+z²)   2(xy-wz)   2(xz+wy) |
    | 2(xy+wz)     1-2(x²+z²) 2(yz-wx) |
    | 2(xz-wy)     2(yz+wx)   1-2(x²+y²) |

// 旋转矩阵转四元数
w = sqrt(1 + R[0][0] + R[1][1] + R[2][2]) / 2
x = (R[2][1] - R[1][2]) / (4*w)
y = (R[0][2] - R[2][0]) / (4*w)
z = (R[1][0] - R[0][1]) / (4*w)

  为什么用四元数？
  无万向锁——旋转自由，不会卡死
插值平滑——SLERP 插值轨迹自然
计算高效——乘法比矩阵乘法快
存储紧凑——4个浮点数 vs 9个

2.5 齐次变换矩阵——把旋转和平移“打包”

齐次变换矩阵，就是把旋转矩阵和平移向量拼成一个 4×4 的矩阵。这样，一次矩阵乘法就能完成旋转+平移，特别方便。

// 齐次变换矩阵 T
T = | R   P |
    | 0   1 |

// 其中 R 是 3×3 旋转矩阵，P 是 3×1 平移向量
// 最后一行是 [0 0 0 1]

// 使用示例：将点 (x, y, z) 从坐标系 B 变换到坐标系 A
| ^AP |   | R   P | | ^BP |
|  1  | = | 0   1 | |  1  |

我习惯用齐次变换矩阵来管理机器人的运动链。每个关节对应一个 T 矩阵，串联起来就是正运动学。比如六轴机器人，从基座到末端就是 T₁ · T₂ · T₃ · T₄ · T₅ · T₆。

实用技巧：

求齐次变换矩阵的逆，不用真的算 4×4 矩阵求逆。利用旋转矩阵的性质：

T^-1 = | R^T -R^T·P |

这个公式在路径规划中经常用到，比如求工具坐标系到基坐标系的逆变换。

2.6 知识体系总览

下面这张图是我自己梳理的运动学基础结构，帮你把今天讲的内容串起来：

嗯，这张图把今天的内容都串起来了。坐标变换是基础，旋转矩阵、欧拉角、四元数是描述旋转的三种方式，齐次变换矩阵则是它们的“集大成者”。

我个人建议，做路径规划时优先用四元数做姿态插值，用齐次变换矩阵做坐标链管理。这样既避免了万向锁，又方便计算。当然，如果你只是做简单的二维移动，欧拉角也够用——但记得避开 90° 附近。

可可精品资料库