如何基于Axis-Angle或Rotation Matrix构建PID控制器（MPU-6050场景）

完全可以用这些姿态表示来构建PID控制器，核心是绕开欧拉角的万向节锁死问题，通过正确计算姿态误差给PID提供输入。结合你用MPU-6050的场景，下面分几种方案说明：

最便捷的方案：基于四元数计算误差 #

MPU-6050搭配传感器融合算法（比如Mahony、Madgwick）可以直接输出四元数，这是最优选择，无需额外转换：

• 首先定义目标姿态四元数（比如机器人要保持的水平姿态，对应四元数q_target = [1, 0, 0, 0]）和传感器输出的当前姿态四元数q_current。
• 计算误差四元数：将目标四元数乘以当前四元数的共轭（共轭操作是取虚部符号翻转：q_conj = [q_current.w, -q_current.x, -q_current.y, -q_current.z]），即 q_error = q_target * q_conj。
• 提取PID可用的误差值：
  • 小角度误差场景下（机器人姿态偏差不大），误差四元数的虚部(x,y,z)乘以2，近似等于绕三个正交轴（roll/pitch/yaw）的角度误差（单位：弧度），直接输入到对应轴的PID控制器即可：

    // 示例伪代码
    float error_roll = 2 * q_error.x;
    float error_pitch = 2 * q_error.y;
    float error_yaw = 2 * q_error.z;
    

  • 大角度误差场景下，把误差四元数转换成轴角形式，得到误差角度θ和旋转轴向量，再根据机器人的控制逻辑（比如需要绕三个轴分别修正），将θ分解到对应控制轴上，作为PID的误差输入。

轴角的误差计算方案 #

如果已经有当前轴角和目标轴角，推荐先把两者转换成四元数，再按上面的四元数误差方法计算——直接计算轴角间的误差容易引入精度问题（尤其是旋转轴方向差异大时）。转换后得到的误差可以再转回轴角，或者分解到三个控制轴的角度误差。

旋转矩阵的误差计算方案 #

旋转矩阵的误差计算需要先得到误差旋转矩阵：用目标旋转矩阵乘以当前旋转矩阵的逆矩阵（正交矩阵的逆等于转置）。然后把误差旋转矩阵转换成四元数或轴角，再提取角度误差给PID，本质和前面的方案一致，只是多了一次矩阵转换步骤，效率略低。

注意事项 #

• PID控制器的结构（位置式/增量式）和用欧拉角时完全一致，差异仅在于误差的计算方式，彻底规避了万向节锁死。
• 如果你的PID是控制角速度而非角度，可直接用MPU-6050的陀螺仪数据，或者对融合后的姿态数据（四元数/轴角）做微分得到角速度，再计算角速度误差。

内容的提问来源于stack exchange，提问作者Linux Man