第4章姿态误差度量

第4章：姿态误差度量

各位同学，欢迎来到《高动态姿态跟踪控制实战》的第四讲。

前面几章我们把姿态描述的基础打牢了。今天聊一个更实际的问题——姿态误差到底怎么算？

你可能会想，误差不就是期望值减实际值吗？嗯，在欧几里得空间里确实是这样。但姿态这东西，它生活在SO(3)群上，不是线性空间。直接相减，结果毫无意义。我刚开始做飞控时就在这上面栽过跟头——用欧拉角直接做差当误差，结果控制器在奇异点附近直接炸了。

所以，今天我们来系统梳理一下：旋转矩阵误差、四元数误差、MRP误差，以及它们各自的脾气秉性。

核心观点：姿态误差不是简单的减法，而是“从一个姿态到另一个姿态的旋转”。这个旋转越小，误差越小。

4.1 姿态误差的本质定义

先搞清楚一件事：姿态误差 = 从当前姿态到期望姿态所需的旋转。

假设当前姿态为 \(R_a\)，期望姿态为 \(R_d\)。那么误差旋转矩阵 \(R_e\) 满足：

R_e = R_d^T * R_a    （或者 R_e = R_a * R_d^T，取决于定义习惯）

我个人习惯用 \(R_e = R_d^T R_a\)。为什么？因为当 \(R_a = R_d\) 时，\(R_e = I\)，误差为零，很直观。

注意，这里的 \(R_e\) 本身也是一个旋转矩阵。它描述的是：从期望坐标系到当前坐标系的旋转。如果这个旋转是零，说明姿态完全对齐。

小技巧：在实际代码中，我通常用轴角表示法来提取误差的大小。旋转矩阵的迹（trace）直接给出了旋转角：\(\cos\theta = (tr(R_e) - 1)/2\)。当 \(\theta\) 很小时，可以直接用 \(\theta\) 作为误差标量。

4.2 旋转矩阵误差

旋转矩阵误差是最“原始”的误差形式。它直接保留了全部姿态信息，没有奇异性问题。

定义：

R_e = R_d^T * R_a

当 \(R_e = I\) 时，误差为零。但实际控制中，我们很少直接用 \(R_e\) 作为反馈信号，因为它是9个元素，冗余度太高。

常用的误差向量：

从 \(R_e\) 中提取的误差向量通常有两种形式：

so(3) 映射： \(\Psi = (R_e - R_e^T)^\vee\)，即取反对称部分的三个独立元素
轴角映射： \(\mathbf{e} = \sin\theta \cdot \mathbf{n}\)，其中 \(\theta\) 和 \(\mathbf{n}\) 是 \(R_e\) 的旋转角和轴

我在项目中遇到过一个问题：当误差角接近180°时，\(\sin\theta\) 很小，误差向量趋近于零，但实际误差很大。这就是所谓的“大误差小信号”问题。解决办法？用 \(\theta \cdot \mathbf{n}\) 代替 \(\sin\theta \cdot \mathbf{n}\)，但要注意 \(\theta\) 的符号判断。

注意：旋转矩阵误差在 \(\theta = \pi\) 时存在歧义。两个不同的旋转轴可能对应同一个旋转矩阵。这在连续控制中需要特别处理。

4.3 四元数误差

四元数误差是工程中最常用的形式。为什么？因为它计算简单，没有奇异性。

定义：

q_e = q_d^{-1} ⊗ q_a

其中 \(q_d\) 是期望四元数，\(q_a\) 是当前四元数，⊗ 表示四元数乘法。

展开写：

q_e = [q_d0 * q_a - q_d^T * q_a_vec;
       q_d0 * q_a_vec + q_a0 * q_d_vec + q_d_vec × q_a_vec]

嗯，看起来有点复杂。但实际代码里就一行的事。

误差向量提取：

通常取四元数的矢量部分作为误差信号：

e = q_e_vec = [q_e1, q_e2, q_e3]^T

当误差很小时，\(q_e0 ≈ 1\)，矢量部分 ≈ \(\frac{1}{2}\theta \mathbf{n}\)。所以直接用矢量部分乘以2，就近似得到了轴角误差。

我的经验：四元数误差的矢量部分在 \(\theta < 90^\circ\) 时表现很好。但如果误差超过90°，矢量部分开始“饱和”——它不再随误差线性增长。这时候我建议用 \(2 \cdot \text{sign}(q_e0) \cdot q_e_vec\) 来补偿。

代码示例：

// 四元数误差计算
Quaternion q_error = q_desired.conjugate() * q_actual;

// 提取误差向量（用于反馈）
Vector3d error_vec = q_error.vec();  // 取虚部

// 处理大误差情况
if (q_error.w() < 0) {
    error_vec = -error_vec;  // 保证最短路径
}

// 小角度近似：误差角 ≈ 2 * ||error_vec||
double angle_error = 2.0 * asin(error_vec.norm());

避坑指南：我曾经在四元数误差的符号上吃过亏。记住，\(q\) 和 \(-q\) 表示同一个姿态。所以计算误差时，要确保 \(q_e0 \geq 0\)，否则取负。这样可以保证误差沿着最短路径收敛。

4.4 修正罗德里格斯参数（MRP）误差

MRP 是姿态表示中的“小众选手”，但在某些场景下非常好用。

定义：

MRP 由旋转轴 \(\mathbf{n}\) 和旋转角 \(\theta\) 定义：

σ = tan(θ/4) * n

MRP 误差的计算方式与四元数类似：

σ_e = σ_d 与 σ_a 的 MRP 合成（通过四元数中转）

具体步骤：

将 MRP 转为四元数：\(q = \frac{1}{\sqrt{1+|\sigma|^2}} [1, \sigma^T]^T\)
计算四元数误差：\(q_e = q_d^{-1} q_a\)
将 \(q_e\) 转回 MRP：\(\sigma_e = \frac{q_e_vec}{1+q_e0}\)

MRP 的独特优势：

当 \(\theta < 180^\circ\) 时，MRP 是唯一的
误差信号在 \(\theta\) 较小时近似线性
计算量比四元数略小（3个参数 vs 4个参数）

但要注意：当 \(\theta \to 360^\circ\) 时，MRP 会趋于无穷大。所以实际使用中，通常配合“影子 MRP”来切换。

我的建议：如果你做的是小角度跟踪（比如卫星姿态保持），MRP 是个不错的选择。但如果是大角度机动，我建议用四元数，更稳妥。

4.5 误差特性分析

最后，我们来对比一下这三种误差度量的特性。

特性	旋转矩阵误差	四元数误差	MRP 误差
参数数量	9（冗余）	4（归一化约束）	3（无约束）
奇异性	无	无	θ = 360°
小角度线性度	好（so(3)映射）	好（矢量部分）	好
大角度表现	需处理符号歧义	需处理双值性	需切换影子
计算效率	低	中	高
工程常用度	低（理论分析用）	高（实际飞控用）	中（特定场景用）

关键结论：

如果你在做理论推导，旋转矩阵误差最干净，SO(3)上的几何结构一目了然
如果你在写实际飞控代码，四元数误差是最稳妥的选择，没有之一
如果你追求极致计算效率且工作在小角度范围，MRP 值得一试

个人经验：我在做无人机竞速控制时，曾经尝试用 MRP 代替四元数。结果在大角度翻滚时，MRP 直接飞到了无穷大，控制器瞬间失稳。后来我加了一个影子 MRP 切换逻辑，才解决问题。但说实话，为了省那一点点计算量，不值得这么折腾。最后还是老老实实用四元数。

好了，关于姿态误差的度量，今天就聊到这里。记住一句话：没有最好的误差度量，只有最适合你场景的误差度量。理解每种度量的脾气，你才能在控制中游刃有余。

📚 课程目录