3、坐标系与变换：刚体空间位姿描述、齐次变换矩阵、欧拉角与四元数、坐标系建立方法
各位同学，欢迎来到第三讲。
这一章，我们聊聊六自由度平台最核心的数学基础——坐标系与变换。说白了，就是怎么描述一个刚体在空间里“在哪”以及“怎么转”。
我刚开始做六自由度平台时，觉得这玩意儿不就是几个坐标嘛，有啥难的？结果第一次联调，平台直接撞了限位。查了半天，发现是坐标系定义反了。嗯，从那以后，我再也不敢小看坐标系了。
核心观点：六自由度平台的所有运动控制，本质上都是在做坐标系之间的变换。你搞懂了变换，就搞懂了平台的一半。
3.1 刚体空间位姿描述
一个刚体在空间里，有6个自由度。哪6个？三个平移（X、Y、Z），三个旋转（绕X、Y、Z轴）。
我习惯把位姿拆成两个部分来看：
- 位置（Position）：刚体上某个参考点（比如质心）在参考坐标系下的坐标。用向量 p = [x, y, z]^T 表示。
- 姿态（Orientation）：刚体自身的坐标系相对于参考坐标系的旋转关系。这个稍微复杂点，需要用旋转矩阵、欧拉角或四元数来描述。
举个例子。你手里拿个手机，手机屏幕朝上放在桌上。手机的位置就是它中心点的坐标，姿态就是屏幕朝上、摄像头朝左这种朝向关系。
我的经验：在六自由度平台中，我们通常把下平台（固定平台）的坐标系作为世界坐标系，上平台（动平台）的坐标系作为工具坐标系。所有控制指令，最终都要转换到这两个坐标系之间。
3.2 齐次变换矩阵
齐次变换矩阵，是描述刚体位姿最直观的工具。它把旋转和平移打包成一个4x4的矩阵。
形式是这样的：
T = [ R p ]
[ 0 1 ]其中，R 是3x3的旋转矩阵，p 是3x1的位置向量。最后一行是 [0 0 0 1]。
为什么要用齐次坐标？说白了，就是为了把旋转和平移统一成矩阵乘法。你想想看，如果不用齐次坐标，你要先旋转再平移，得写两个式子。用了齐次变换矩阵，一个矩阵乘法搞定。
我在项目中遇到过一个问题：两个坐标系之间的变换矩阵算错了，导致平台运动轨迹完全偏离。后来发现，是变换矩阵的乘法顺序搞反了。记住：左乘是相对于固定坐标系，右乘是相对于当前坐标系。这个坑，我踩过，你别踩。
注意：齐次变换矩阵的逆矩阵，不是简单的转置。它的形式是：
T^(-1) = [ R^T -R^T * p ]
[ 0 1 ]这个公式我建议你手推一遍，加深理解。
3.3 欧拉角与四元数
旋转矩阵有9个元素，但自由度只有3个。所以，我们通常用更紧凑的方式描述旋转——欧拉角和四元数。
3.3.1 欧拉角
欧拉角的核心思想：任何旋转都可以分解为绕三个坐标轴的依次旋转。常见的顺序有 ZYX、ZYZ、XYZ 等。
我最常用的是 ZYX 顺序（也叫RPY角）：
- Roll（横滚）：绕X轴旋转，角度记为 φ
- Pitch（俯仰）：绕Y轴旋转，角度记为 θ
- Yaw（偏航）：绕Z轴旋转，角度记为 ψ
欧拉角的好处是直观，三个角度一看就懂。但有个致命问题——万向锁。当 Pitch 接近 ±90° 时，Roll 和 Yaw 的旋转轴会重合，丢失一个自由度。
我曾经在调试一个六自由度平台时，遇到平台突然卡死。查了半天，发现是欧拉角计算时遇到了万向锁。从那以后，我只要涉及大角度运动，一律改用四元数。
3.3.2 四元数
四元数，说白了就是复数的推广。一个四元数 q = w + xi + yj + zk，其中 w 是实部，x、y、z 是虚部。
描述旋转时，我们使用单位四元数（模长为1）。绕单位向量 u = [ux, uy, uz]^T 旋转角度 θ 的四元数为：
q = [cos(θ/2), ux*sin(θ/2), uy*sin(θ/2), uz*sin(θ/2)]四元数的好处：
- 没有万向锁问题
- 插值平滑（可以用球面线性插值SLERP）
- 计算效率高（比旋转矩阵少乘法）
我个人的习惯是：人机交互用欧拉角，内部计算用四元数。这样既直观，又避免了万向锁。
实用技巧：欧拉角转四元数、四元数转旋转矩阵，这些转换公式我建议你写成工具函数，直接调用。别每次都手算，容易出错。
3.4 坐标系建立方法
坐标系怎么建？这没有标准答案，但有经验法则。
我总结了一套“三步法”：
- 定原点：通常选在平台的几何中心或关键铰点处。
- 定轴向：Z轴一般垂直向上（与重力方向一致），X轴和Y轴根据右手定则确定。
- 定参考：明确哪个是固定坐标系，哪个是运动坐标系。
在六自由度平台中，我通常这样建系：
| 坐标系名称 | 原点位置 | Z轴方向 | X轴方向 |
|---|---|---|---|
| 世界坐标系 {W} | 下平台几何中心 | 垂直向上 | 指向平台前方 |
| 动平台坐标系 {P} | 上平台几何中心 | 垂直上平台平面向上 | 指向平台前方 |
| 铰点坐标系 {B_i} | 第i个下铰点 | 垂直下平台平面向上 | 指向平台中心 |
为什么要这么建？因为这样，动平台的位姿变化就直接对应了 {P} 相对于 {W} 的齐次变换矩阵。控制起来非常方便。
避坑指南：我曾经在项目里，把动平台坐标系的原点定在了上平台边缘。结果每次计算铰点位置，都要做一次偏移变换，代码写得很痛苦。后来统一改到几何中心，清爽多了。所以，原点尽量选在对称中心，能省很多事。
3.5 知识体系总览
为了让你更直观地理解本章的知识结构，我画了一张图：
这张图把本章的知识点串起来了。你从上往下看，先理解刚体位姿是什么，再掌握三种描述方式，最后学会怎么建坐标系。每一步都是下一步的基础。
总结一下：
- 刚体位姿 = 位置 + 姿态
- 齐次变换矩阵是“瑞士军刀”，啥都能干，但有点重
- 欧拉角直观但有万向锁，四元数无奇点但不够直观
- 坐标系建立，原点选对称中心，轴向按右手定则
好了，这一章就到这里。这些概念你理解了，后面讲运动学正反解、动力学建模，就会轻松很多。
公众号:蓝海资料掘金营，微信deep3321