运动学基础（上）：刚体变换、旋转矩阵、欧拉角、四元数
各位同学，欢迎来到《人形机器人轨迹跟踪实战》的第一章。
做机器人控制，尤其是人形机器人，你绕不开一个东西——运动学。说白了，就是研究机器人怎么动、动到哪里去。而运动学的根基，就是刚体变换。
我刚开始接触人形机器人时，觉得这玩意儿不就是坐标变换嘛，有啥难的？结果第一次调试双足平衡，机器人原地转了个圈，直接摔了个大马趴。嗯，从那以后，我再也不敢小看旋转矩阵和四元数了。
核心观点：刚体变换是描述机器人“位置+姿态”的数学工具。没有它，你连机器人的脚在哪儿都说不清楚。
1. 刚体变换：从“点”到“姿态”
先问一个问题：你如何告诉机器人“把手伸到桌子上的水杯处”？
光说“坐标(0.5, 0.3, 0.8)”是不够的。因为手爪还有个朝向——你是正着抓、侧着抓、还是反着抓？这个朝向，就是姿态。
刚体变换，就是用来同时描述位置和姿态的。它由两部分组成：
- 平移向量：描述从参考坐标系原点到目标坐标系原点的位移。
- 旋转矩阵：描述目标坐标系相对于参考坐标系的朝向。
我在项目中遇到过一个问题：两个传感器分别测量机器人的躯干和腿部，数据格式不一样。一个给的是欧拉角，一个给的是四元数。如果不做刚体变换统一到同一个坐标系下，那控制指令就是一团乱麻。
避坑指南：我曾经因为忘记把传感器坐标系对齐，导致机器人走路时一条腿“飘”在半空中。后来我养成了一个习惯：任何数据进来，第一件事就是确认它是在哪个坐标系下表达的。
2. 旋转矩阵：最直观，但最啰嗦
旋转矩阵是一个3x3的矩阵，用来描述一个坐标系相对于另一个坐标系的旋转关系。
它的好处是：数学性质好。正交矩阵，行列式为+1，求逆就是转置，计算方便。
它的坏处是：冗余。9个参数描述3个自由度，你想想看，这得多浪费计算资源？
举个例子，绕Z轴旋转θ角，旋转矩阵长这样：
R_z(θ) = [cosθ -sinθ 0]
[sinθ cosθ 0]
[0 0 1]绕X轴、Y轴类似。三个轴的旋转矩阵乘在一起，就能表达任意姿态。
但注意：乘法顺序很重要。先绕X轴转再绕Y轴转，和先绕Y轴再绕X轴，结果完全不同。我见过不少新手在这里栽跟头。
警告：旋转矩阵乘法不满足交换律。AB ≠ BA。在代码里写矩阵乘法时，一定要搞清楚哪个矩阵在左、哪个在右。
3. 欧拉角：直观，但有“万向锁”
欧拉角用三个角度（比如滚转、俯仰、偏航）来描述姿态。很直观，一看就懂。
比如“偏航30度，俯仰20度，滚转10度”，你脑子里能立刻想象出那个姿态。这是欧拉角最大的优点。
但它的致命缺陷是万向锁（Gimbal Lock）。
当俯仰角接近±90度时，滚转和偏航会“锁死”在一起，丢失一个自由度。你想想看，机器人正在走路，突然某个关节角度到了90度，姿态控制瞬间失效——这可不是闹着玩的。
我曾经在调试一个六轴机械臂时，遇到了万向锁。机器人末端执行器突然“卡住”了，怎么给指令都不动。排查了半天，才发现是欧拉角表示法在某个特定姿态下失效了。
经验之谈：如果你只是做简单的可视化、或者人机交互界面，用欧拉角没问题。但如果你在做实时控制、轨迹规划，我建议你远离欧拉角，改用四元数。
4. 四元数：不直观，但真香
四元数是什么？说白了，它是一个四维复数：q = w + xi + yj + zk。
听起来很抽象？没关系。你只需要记住：
- 四元数用4个参数表示旋转，没有冗余，没有万向锁。
- 插值平滑，适合做姿态的平滑过渡。
- 计算效率高，适合嵌入式实时控制。
它的缺点只有一个：不直观。你看着(0.707, 0, 0.707, 0)这个四元数，能想象出它对应什么姿态吗？反正我不能。
但在代码里，四元数非常好用。比如ROS2里，姿态就是用四元数表示的：
# Python示例：ROS2中的四元数
from geometry_msgs.msg import Quaternion
import tf_transformations
# 从欧拉角转四元数
q = tf_transformations.quaternion_from_euler(roll, pitch, yaw)
# q = [x, y, z, w]
# 发布姿态
pose_msg.orientation.x = q[0]
pose_msg.orientation.y = q[1]
pose_msg.orientation.z = q[2]
pose_msg.orientation.w = q[3]你看，代码里用四元数非常直接。ROS2的TF系统内部也是用四元数做运算的。
小技巧：我习惯在代码里统一用四元数做内部运算，只在可视化或人机交互时才转成欧拉角。这样既避免了万向锁，又保留了直观性。
5. 它们在机器人描述中的应用
好了，理论讲完了。这些东西到底怎么用？
在机器人描述文件（比如URDF、SDF）里，每个关节、每个连杆的位置和姿态，都是用刚体变换来描述的。
举个例子，一个简单的URDF片段：
<link name="base_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.2 0.3 0.1"/>
</geometry>
</visual>
</link>
<joint name="hip_joint" type="revolute">
<parent link="base_link"/>
<child link="thigh_link"/>
<origin xyz="0 0 0.05" rpy="0 0 0"/>
<axis xyz="0 1 0"/>
</joint>这里的rpy就是欧拉角（滚转、俯仰、偏航）。xyz是平移向量。整个描述就是一个刚体变换链。
当你做运动学正解时，就是从基座开始，沿着关节链一路做刚体变换，最终算出末端执行器的位姿。
当你做运动学逆解时，就是反过来：已知末端位姿，反推每个关节的角度。
嗯，这里要注意：URDF里默认用欧拉角，但ROS2内部计算时用的是四元数。所以你在写代码时，经常需要做欧拉角和四元数之间的转换。
总结一下：
- 刚体变换：位置+姿态，机器人描述的基石。
- 旋转矩阵：数学性质好，但冗余。
- 欧拉角：直观，但有万向锁。
- 四元数：无万向锁，适合计算，但不直观。
我的建议是：内部计算用四元数，人机交互用欧拉角，写文档用旋转矩阵。
这张图把本章的知识体系串起来了。你从中心出发，左边是平移，中间是旋转的三种表示，右边是它们在机器人领域的实际应用。记住这个结构，后面几章的内容都是在这个框架上展开的。
个人习惯：我每次做新项目，都会先画一张类似的图，把坐标系、变换关系理清楚。磨刀不误砍柴工，真的。
好了，这一章就到这里。刚体变换、旋转矩阵、欧拉角、四元数，这些概念你理解了，后面学运动学正解、逆解、轨迹规划就会轻松很多。
记住：坐标系是机器人的“世界观”。搞不清楚坐标系，机器人就不知道自己在哪儿、要去哪儿。