3、运动学基础（一）：刚体变换与齐次坐标、旋转矩阵与欧拉角、四元数基础
各位同学，欢迎来到运动学基础的第一讲。
说实话，做机器人这么多年，我最大的体会就是：运动学是机器人的“世界观”。你想想看，一个机器人要导航、要抓取、要避障，它首先得知道自己“在哪儿”、“朝哪儿看”。这些问题的数学答案，就藏在我们今天要聊的这几个概念里。
我个人习惯把这一章叫做“坐标系的语言”。你听不懂这门语言，后面写轨迹跟踪代码时就会处处碰壁。我曾经带过一个项目，团队里的小伙子在融合IMU和里程计时，因为欧拉角万向锁没处理好，机器人在原地疯狂打转……嗯，从那以后，我要求所有新人都得先把这块啃透。
核心要点：本章所有内容都围绕一个目标——如何精确描述一个刚体在三维空间中的位置和姿态。这是导航与轨迹跟踪的数学基石。
3.1 刚体变换：从“点”到“物体”的认知升级
在中学物理里，我们描述一个点用 (x, y, z) 就够了。但机器人不是点，它是一个有体积、有朝向的刚体。我们需要同时描述它的位置和姿态。
刚体变换，说白了就是两个坐标系之间的映射关系。它包含两部分：
- 旋转：物体朝哪个方向？
- 平移：物体在哪儿？
我习惯用一个公式来概括：
世界坐标系下的点 = 旋转矩阵 × 物体坐标系下的点 + 平移向量但这样写有个麻烦——旋转和平移是分开的，运算起来不优雅。于是数学家们想了个聪明的办法：齐次坐标。
3.2 齐次坐标：把“加法”变成“乘法”
齐次坐标的核心思想很简单：给三维向量加一个维度，变成四维向量。
普通坐标 (x, y, z) → 齐次坐标 (x, y, z, 1)
方向向量 (x, y, z) → 齐次坐标 (x, y, z, 0) // 注意最后一位是0为什么要这么做？因为这样一来，旋转和平移就可以合并成一个 4×4 的变换矩阵：
| R(3×3) t(3×1) | · | 物体坐标 | = | 世界坐标 |
| 0 0 0 1 | | 1 | | 1 |你看，连续多次变换，只需要把矩阵乘起来就行。我在做多传感器标定时，经常要串联七八个坐标系，如果没有齐次坐标，代码会写得想哭。
我的小技巧：在ROS里，tf2 库底层用的就是齐次变换。你每次调用 lookupTransform()，拿到的其实就是一个 TransformStamped 消息，里面封装了平移和旋转。理解了这个，你就能看懂 tf2 的源码了。
3.3 旋转矩阵：最直观，但最“啰嗦”
旋转矩阵 R 是一个 3×3 的正交矩阵，行列式为 +1。它的每一列，代表物体坐标系的一个轴在世界坐标系下的投影。
举个例子，绕 Z 轴旋转 θ 角：
R_z(θ) = | cosθ -sinθ 0 |
| sinθ cosθ 0 |
| 0 0 1 |旋转矩阵的好处是直观、无歧义。但坏处也很明显——9 个参数，却有 6 个约束条件（正交+单位行列式），冗余度太高。在优化问题里，你很难保证迭代过程中矩阵始终保持正交。
避坑指南：我曾经在卡尔曼滤波里直接用旋转矩阵作为状态量，结果协方差矩阵很快就奇异了。后来才意识到，旋转矩阵的流形结构不允许你这样粗暴地做线性更新。记住：旋转矩阵不适合做插值和滤波。
3.4 欧拉角：直观但危险的“万向锁”
欧拉角用三个角度来描述旋转，比如常见的 ZYX 顺序：偏航 (Yaw) → 俯仰 (Pitch) → 横滚 (Roll)。
为什么说它危险？因为万向锁。
当俯仰角接近 ±90° 时，偏航和横滚的旋转轴会重合，导致丢失一个自由度。这时候你无论怎么调偏航角，机器人的朝向都不会变。
我记得有一次在调试双足机器人的上半身姿态时，俯仰角刚好卡在 90° 附近，结果肩关节的轨迹突然跳变，差点把机械臂甩出去。从那以后，我只要涉及大角度运动，就坚决不用欧拉角。
| 表示方法 | 参数数量 | 优点 | 缺点 |
|---|---|---|---|
| 旋转矩阵 | 9 | 无奇异性，运算方便 | 冗余，不适合插值 |
| 欧拉角 | 3 | 直观，参数少 | 万向锁，不连续 |
| 四元数 | 4 | 无奇异性，适合插值 | 不够直观 |
3.5 四元数基础：工程界的“最优解”
四元数，说白了就是一个实部加三个虚部：q = w + xi + yj + zk，其中 i² = j² = k² = ijk = -1。
别被这个数学形式吓到。在工程里，你只需要记住几个关键点：
- 单位四元数：
w² + x² + y² + z² = 1，用于表示旋转。 - 共轭：
q* = w - xi - yj - zk，相当于逆旋转。 - 乘法：两个四元数相乘，对应旋转的复合。
四元数最大的优势是可以平滑插值。SLERP（球面线性插值）算法能让你在两个姿态之间生成平滑的过渡轨迹。这在轨迹跟踪里太重要了——你总不希望机械臂的末端执行器在两点之间“抽风”吧？
工程建议：在ROS里，姿态消息 geometry_msgs/Pose 就是用四元数存储朝向的。你可以在代码里直接用 tf2::Quaternion 类进行操作。我个人习惯把所有姿态运算都放在四元数域，只在最后可视化时才转成欧拉角。
3.6 知识体系总览
下面这张图，是我自己梳理的本章知识脉络。你可以把它当作一个“导航地图”：
3.7 实战：ROS中的姿态转换
光说不练假把式。我们来看看在ROS里怎么用代码做姿态转换。下面这个例子，演示了如何把欧拉角转成四元数，再转成旋转矩阵：
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Matrix3x3.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
// 从欧拉角创建四元数（ZYX顺序：yaw, pitch, roll）
tf2::Quaternion q;
q.setRPY(roll, pitch, yaw); // 注意：参数顺序是 roll, pitch, yaw
// 四元数转旋转矩阵
tf2::Matrix3x3 rot_mat(q);
// 提取欧拉角（用于调试）
double r, p, y;
rot_mat.getRPY(r, p, y);
// 发布到ROS话题
geometry_msgs::Quaternion msg;
tf2::convert(q, msg);我的习惯：在写导航代码时，我通常用四元数做所有内部运算，只在打印日志或可视化时转成欧拉角。这样既避免了万向锁，又方便人眼阅读。
3.8 本章小结
好了，我们来捋一捋今天的内容：
- 刚体变换 = 旋转 + 平移，是描述机器人状态的基础。
- 齐次坐标把变换统一成矩阵乘法，让代码更简洁。
- 旋转矩阵直观但冗余，欧拉角参数少但有万向锁，四元数是工程上的最佳选择。
- 在ROS里，
tf2库帮你封装了所有底层运算，但理解原理才能用好它。