3、坐标系与姿态表示：机体坐标系、世界坐标系、欧拉角、旋转矩阵、四元数的定义与转换

在IMU（惯性测量单元）的工程应用中，姿态解算是一切控制逻辑的基石。无论是双轮自平衡机器人、四足机器人还是无人机，其平衡控制的本质都是“在世界坐标系中感知自身姿态，并在机体坐标系中施加控制力矩”。因此，理解坐标系定义与姿态表示方法，是连接传感器原始数据与运动控制指令之间的关键桥梁。本节将系统梳理机体坐标系与世界坐标系的物理含义，深入剖析欧拉角、旋转矩阵与四元数三种姿态表示方法的数学定义、工程适用场景及相互转换关系，并结合实际代码与调试经验，帮助读者建立完整的姿态数学框架。

上图清晰地展示了三种姿态表示方法之间的逻辑关系。世界坐标系为机器人提供了绝对的空间参考基准，而机体坐标系则随机器人运动而改变。欧拉角直观但存在万向锁问题，旋转矩阵计算稳定但参数冗余，四元数则兼顾了紧凑性与无奇异性。在工程实践中，我们通常使用四元数作为IMU内部解算的核心，而在控制层或用户界面中转换为欧拉角进行可视化与逻辑判断。

3.1 核心原理：坐标系的定义与姿态的数学本质

世界坐标系（World Frame），又称导航坐标系或惯性坐标系，是一个固定不动的参考系。在机器人平衡控制中，通常将世界坐标系的原点设定在机器人起始位置的地面投影点，Z轴垂直于水平面向上（与重力方向相反），X轴指向机器人初始前进方向，Y轴由右手定则确定。世界坐标系为机器人提供了“绝对位置”和“绝对姿态”的度量基准。

机体坐标系（Body Frame），则固定于机器人本体之上，原点位于IMU的几何中心或机器人的质心投影点。其X轴指向机器人正前方，Y轴指向机器人左侧（或右侧，取决于具体约定），Z轴垂直于机体平面向上。当机器人发生倾斜或旋转时，机体坐标系随之转动，而世界坐标系保持不动。

姿态表示的数学本质，就是描述机体坐标系相对于世界坐标系的旋转关系。这种旋转关系可以用一个3×3的正交旋转矩阵R来表示，满足R^TR = I且det(R) = 1。旋转矩阵的每一列分别表示机体坐标系的三个基向量在世界坐标系中的投影分量。例如，若机器人绕世界坐标系Z轴旋转角度ψ（偏航角），则旋转矩阵为：

Rz(ψ) = [cosψ  -sinψ  0;
          sinψ   cosψ  0;
          0      0     1]

同理，绕X轴的横滚角φ和绕Y轴的俯仰角θ也有对应的旋转矩阵Rx(φ)和Ry(θ)。完整的旋转矩阵R = Rz(ψ)·Ry(θ)·Rx(φ)，注意旋转顺序对结果有决定性影响，工程中必须明确约定旋转顺序（通常为Z-Y-X，即先偏航、再俯仰、最后横滚）。

3.2 判断方法与操作流程：三种姿态表示的选择策略

在实际工程中，选择哪种姿态表示方法取决于具体应用场景。下表对比了三种方法的特性：

特性维度	欧拉角	旋转矩阵	四元数
参数数量	3个（φ, θ, ψ）	9个（3×3矩阵）	4个（w, x, y, z）
直观性	高，物理意义明确	低，不直观	中，需理解四维空间
奇异性	存在万向锁（θ=±90°）	无奇异	无奇异
计算效率	高（三角函数较多）	中（矩阵乘法）	高（仅乘加运算）
插值平滑性	差，角度跳变	差，需正交化	优，球面线性插值
存储占用	3个浮点数	9个浮点数	4个浮点数

工程选择建议：

• 控制层（PID、LQR等）：优先使用四元数或旋转矩阵，避免欧拉角在接近±90°时出现的数值不稳定问题。例如，双轮平衡车在急加速或急刹车时，俯仰角可能瞬间接近90°，此时欧拉角表示会导致控制量突变。
• 用户界面与调试：使用欧拉角，因为工程师和操作员更习惯“横滚、俯仰、偏航”的直观描述。
• IMU内部解算（姿态融合算法）：四元数是绝对主流，因为互补滤波和卡尔曼滤波在四元数域内运算最为高效且无奇异。

转换操作流程：当需要从四元数转换为欧拉角时，可使用以下公式（Z-Y-X旋转顺序）：

roll  = atan2(2*(w*x + y*z), 1 - 2*(x*x + y*y))
pitch = asin(2*(w*y - z*x))
yaw   = atan2(2*(w*z + x*y), 1 - 2*(y*y + z*z))

注意，当pitch接近±90°时，asin函数的输入可能超出[-1,1]范围，需进行钳位处理。反向转换（欧拉角到四元数）则通过三个轴的旋转四元数相乘得到：

q = qz(yaw) * qy(pitch) * qx(roll)
其中：
qx(roll)  = [cos(roll/2),  sin(roll/2), 0, 0]
qy(pitch) = [cos(pitch/2), 0, sin(pitch/2), 0]
qz(yaw)   = [cos(yaw/2),   0, 0, sin(yaw/2)]

3.3 案例拆解：双轮自平衡机器人的姿态解算实战

我们以一台典型的双轮自平衡机器人为例，其IMU（MPU6050）输出原始加速度和角速度数据。目标是通过姿态解算获得机器人的实时俯仰角（pitch），用于平衡控制。

场景背景与约束：机器人质量5kg，轮子直径0.15m，电机最大扭矩2N·m。控制周期为5ms（200Hz）。IMU安装在机器人底盘中心，Z轴垂直向上。机器人最大运行速度1.5m/s，最大加速度2m/s²。初始状态为静止直立。

初始现象与目标：上电后，IMU输出的加速度计数据为(0.02, 0.01, 9.78) m/s²，陀螺仪数据为(0.01, -0.02, 0.03) rad/s。我们需要在10ms内计算出初始俯仰角，并持续输出平滑的姿态角。

关键输入数据与参数：

• 加速度计噪声密度：0.0002 m/s²/√Hz
• 陀螺仪零偏稳定性：0.01 °/s
• 互补滤波系数α = 0.98（加速度计权重0.02）

分析判断过程：首先，利用加速度计计算初始俯仰角：pitch_acc = atan2(ax, sqrt(ay² + az²))。代入数据得pitch_acc = atan2(0.02, sqrt(0.01² + 9.78²)) ≈ 0.002 rad ≈ 0.115°。由于机器人静止，该角度可信。但加速度计对振动敏感，因此需要与陀螺仪积分融合。

实际操作步骤：

1. 初始化四元数q = (1, 0, 0, 0)，对应欧拉角(0, 0, 0)。
2. 读取陀螺仪角速度(ωx, ωy, ωz)，转换为四元数导数：dq/dt = 0.5 * q ⊗ (0, ωx, ωy, ωz)。
3. 以5ms周期积分四元数，得到姿态预测值。
4. 读取加速度计数据，计算重力方向与预测姿态的重力投影之间的误差。
5. 用PI控制器修正陀螺仪零偏，并将修正后的角速度用于四元数更新。
6. 每50ms将四元数转换为欧拉角，用于控制逻辑和上位机显示。

结果对比与复盘：在静止状态下，纯陀螺仪积分在10秒内漂移约2.3°，而融合后的姿态角漂移小于0.1°。在机器人受到外部冲击（模拟碰撞）时，纯加速度计解算的角度出现±5°的跳变，而融合输出仅产生0.3°的平滑过渡。这验证了四元数+互补滤波方案的有效性。风险点在于：若α设置过大（如0.995），动态响应变慢，机器人可能无法及时纠正倾斜；若α过小（如0.9），则振动噪声会直接传递到控制量，导致电机抖动。经过多次调试，α=0.98在响应速度与噪声抑制之间取得了最佳平衡。

3.4 常见误区与总结复盘

误区一：混淆旋转顺序。很多初学者在将欧拉角转换为旋转矩阵时，默认使用R = Rx·Ry·Rz，但IMU数据手册通常约定的是Z-Y-X顺序。若顺序错误，在俯仰角较大时，横滚和偏航会出现交叉耦合，导致控制发散。

误区二：忽视万向锁问题。在俯仰角接近±90°时，欧拉角表示会丢失一个自由度，导致偏航角与横滚角无法区分。例如，当机器人垂直向上（pitch=90°）时，yaw和roll的旋转效果相同，此时欧拉角解算会产生奇异值。解决方法是切换到四元数表示，或在控制逻辑中限制俯仰角范围。

误区三：四元数未归一化。四元数必须满足w²+x²+y²+z²=1。在长时间积分后，由于数值误差，四元数模长会偏离1，导致旋转矩阵非正交，进而引起姿态漂移。因此，每次更新后必须进行归一化处理：q = q / ||q||。

误区四：坐标系轴方向约定不一致。不同IMU厂商对X、Y、Z轴的正方向定义可能不同。例如，有的传感器Z轴向下，有的向上。若未仔细阅读数据手册，直接将加速度计数据代入公式，会导致姿态角符号反转。建议在代码初始化阶段，通过静态校准确定轴方向。