刚体运动学基础：坐标系与变换、旋转矩阵、欧拉角、四元数

各位同学，欢迎来到第二章。这一章的内容，说白了就是人形机器人运动学的「语言」。你想想看，机器人要动，首先得知道自己在哪、胳膊腿在什么位置、脑袋朝哪个方向。这些信息怎么描述？靠的就是坐标系和旋转表达。

我刚开始做机器人那会儿，觉得这玩意儿不就是高中数学嘛，没什么大不了的。结果第一次调试双足步行，机器人直接来了个后空翻——不是表演，是摔的。后来一查，是欧拉角万向锁导致姿态解算炸了。嗯，从那以后我再也不敢小看这些基础了。

2.1 坐标系与刚体位姿描述

先聊坐标系。人形机器人身上到处都是坐标系：世界坐标系、基座坐标系、髋关节坐标系、膝关节坐标系、踝关节坐标系……每个关节都有自己的「小天地」。

一个刚体在空间中的状态，我们叫它位姿。位 = 位置，姿 = 姿态。位置用三维坐标 (x, y, z) 表示，姿态用旋转矩阵、欧拉角或四元数表示。

核心概念：刚体有6个自由度——3个平移自由度 + 3个旋转自由度。人形机器人全身几十个关节，每个关节又引入额外的自由度，但描述每个刚体本身，6个就够了。

坐标系之间的变换，我习惯用一个4×4的齐次变换矩阵搞定：

| R   t |
| 0   1 |

其中 R 是3×3旋转矩阵，t 是3×1平移向量。这个矩阵能把一个点从坐标系A变换到坐标系B。

我在项目中遇到过一个问题：两个工程师用了不同的坐标系约定——一个用右手系，一个用左手系。结果联合调试时，机器人手臂直接往反方向转。所以，团队内部必须统一坐标系约定，这是血的教训。

2.2 旋转矩阵

旋转矩阵是描述姿态最直接的方式。它是一个3×3的正交矩阵，行列式为+1。说白了，就是一组标准正交基。

绕X轴、Y轴、Z轴旋转θ角的基本矩阵长这样：

R_x(θ) = | 1    0       0    |
         | 0  cosθ  -sinθ   |
         | 0  sinθ   cosθ   |

R_y(θ) = | cosθ  0   sinθ   |
         | 0     1    0     |
         | -sinθ 0   cosθ   |

R_z(θ) = | cosθ  -sinθ  0   |
         | sinθ   cosθ  0   |
         | 0      0     1   |

旋转矩阵有几个重要性质：

正交性：R^TR = I，R^-1 = R^T
组合性：连续旋转可以用矩阵乘法叠加，R = R₂ · R₁
不可交换：矩阵乘法不满足交换律，R₁R₂ ≠ R₂R₁

我的习惯：在代码里，我一般用旋转矩阵做中间计算，因为它没有奇异性，加减乘除都方便。但不会用它做存储或通信——9个数字存一个姿态，太浪费了。

2.3 欧拉角

欧拉角是工程中最直观的旋转表达方式。它用三个角度来描述姿态：偏航(yaw)、俯仰(pitch)、滚转(roll)。你想想看，开飞机的时候，飞行员就说「左转30度，抬头10度，侧倾5度」，多好理解。

但欧拉角有个大坑——万向锁。当俯仰角达到±90°时，偏航和滚转的旋转轴会重合，丢失一个自由度。我在做人形机器人上楼梯时遇到过这个问题：机器人躯干前倾到90°，姿态解算直接炸了，角度跳变，机器人差点摔倒。

欧拉角类型	旋转顺序	常见应用
ZYX（偏航-俯仰-滚转）	先绕Z，再绕Y，最后绕X	航空、人形机器人躯干
ZYZ	先绕Z，再绕Y，再绕Z	机械臂逆运动学
XYZ（固定轴）	绕世界坐标系X-Y-Z	视觉伺服

避坑指南：我曾经在IMU姿态解算中直接用欧拉角做插值，结果角度从179°变到-179°时，插值路径绕了地球一大圈。后来改用四元数球面线性插值(SLERP)，问题才解决。记住：欧拉角适合人看，不适合机器算。

2.4 四元数

四元数，很多人一听就头大。其实没那么玄乎。它就是一个四维向量 q = (w, x, y, z)，其中 w 是实部，(x, y, z) 是虚部，满足 w² + x² + y² + z² = 1。

为什么用四元数？三个理由：

无万向锁——这是最大的优势
插值平滑——SLERP插值能得到最短路径的旋转
计算高效——比旋转矩阵少存5个数字，乘法也更快

四元数表示旋转的公式很简单：

q = cos(θ/2) + sin(θ/2) · (u_x i + u_y j + u_z k)

其中 u 是旋转轴单位向量，θ 是旋转角度。

我建议你在代码里这样用：

// C++ 伪代码：四元数旋转向量
Quaternion q = Quaternion::fromAxisAngle(axis, angle);
Vector3d v_rotated = q * v * q.inverse();  // 或者直接用重载运算符

实用技巧：在ROS和很多机器人框架中，四元数是姿态的标准存储格式。我习惯把四元数作为「内部交换格式」，只在显示和人工调试时转成欧拉角。

2.5 三种表达方式的对比与选择

说了这么多，到底该用哪个？我整理了一张对比表：

特性	旋转矩阵	欧拉角	四元数
参数数量	9	3	4
奇异性	无	有（万向锁）	无
插值	困难	有跳变风险	SLERP平滑
直观性	差	好	差
计算效率	中等	高	高
推荐场景	中间计算	人机交互	存储/通信/插值

我个人习惯的选型原则：

存储和通信：用四元数，省空间、无歧义
中间计算：用旋转矩阵，加减乘除方便
调试和显示：转成欧拉角，人看得懂
姿态插值：必须用四元数SLERP

2.6 知识体系总览

下面这张图，把本章的核心逻辑串起来了。你仔细看一遍，应该能明白坐标系变换和旋转表达之间的关系。

这张图你看懂了吗？从刚体位姿出发，位置和姿态是两个独立的分支。姿态又有三种表达方式，各有优劣。最后通过齐次变换矩阵，把位置和姿态统一起来，构成完整的坐标系变换工具链。

好了，第二章就到这里。这些基础概念会贯穿整个课程，后面的正运动学、逆运动学、轨迹规划，全都离不开它们。你先把坐标系变换和三种旋转表达吃透，后面就顺了。

📚 课程目录

刚体运动学基础：坐标系与变换、旋转矩阵、欧拉角、四元数

2.1 坐标系与刚体位姿描述

2.2 旋转矩阵

2.3 欧拉角

2.4 四元数

2.5 三种表达方式的对比与选择

2.6 知识体系总览