姿态动力学Vol.4-罗德里格斯参数(CRPs)与修正参数(MRPs)的对偶性

引言

在前面的章节中，我们介绍了欧拉角（直观但有奇点）和四元数（无奇点但有冗余约束）。是否存在一种既无冗余（只有3个参数）又能在大部分工作空间内避免奇点的描述方式呢？

答案是肯定的。这就是本章要介绍的罗德里格斯参数 (Rodrigues Parameters) 家族。这一家族基于“轴-角”描述，通过巧妙的投影变换，将四元数的 4 个参数压缩为 3 个。其中，经典罗德里格斯参数 (CRPs) 和 修正罗德里格斯参数 (MRPs) 是最为重要的两个成员。MRPs 凭借其优越的数值性质，已成为现代高精度姿态控制（如卫星编队飞行）的首选参数。

---

1. 罗德里格斯旋转公式 (Rodrigues’ Rotation Formula)

一切的起源都可以追溯到 1840 年 Olinde Rodrigues 提出的旋转公式。该公式给出了矢量 $athbf{v}$ 绕单位轴 $athbf{k}$ 旋转角度 $ heta$ 后的新矢量 $athbf{v}_{rot}$ 的解析表达。

1.1 几何推导

将矢量 $athbf{v}$ 分解为平行于 $athbf{k}$ 的分量 $athbf{v}_{arallel}$ 和垂直于 $athbf{k}$ 的分量 $athbf{v}_{erp}$：

$$athbf{v} = athbf{v}_{arallel} + athbf{v}_{erp} = (athbf{v} dot athbf{k})athbf{k} + [athbf{v} - (athbf{v} dot athbf{k})athbf{k}]$$

旋转时，平行分量不变，垂直分量在垂直于 $athbf{k}$ 的平面内旋转 $ heta$ 角。

$$athbf{v}_{rot} = athbf{v}_{arallel} + athbf{v}_{erp} os heta + (athbf{k} imes athbf{v}_{erp}) in heta$$

整理后得到著名的罗德里格斯旋转公式：

$$athbf{v}_{rot} = athbf{v} os heta + (athbf{k} imes athbf{v}) in heta + athbf{k}(athbf{k} dot athbf{v})(1 - os heta)$$

1.2 矩阵形式

利用反对称矩阵 $athbf{K} = [athbf{k} imes]$，可以将上述公式写成矩阵形式 $athbf{v}_{rot} = athbf{R}(athbf{k}, heta) athbf{v}$，其中旋转矩阵 $athbf{R}$ 为：

$$athbf{R} = athbf{I} + (in heta)athbf{K} + (1-os heta)athbf{K}^2$$

这就是从轴-角到 DCM 的直接转换公式。

---

2. 经典罗德里格斯参数 (CRPs)

2.1 定义

经典罗德里格斯参数 $athbf{q}$（注意不要与四元数混淆，有时也记为 $oldsymbol{\rho}$）定义为：

$$athbf{q} = athbf{k} aneft(rac{ heta}{2}ight)$$

它是一个 3 维矢量，方向与旋转轴相同，模长为半角的正切。

2.2 与四元数的关系

利用四元数定义 $eta_0 = os( heta/2), oldsymbol{eta} = athbf{k}in( heta/2)$，可得：

$$athbf{q} = rac{oldsymbol{eta}}{eta_0}$$

这解释了 CRPs 的几何意义：它是四元数在超平面 $eta_0=1$ 上的球极投影 (Stereographic Projection)。

2.3 奇点与性质

• 奇点：当 $ heta o m 180^irc$ 时，$eta_0 o 0$，$ an( heta/2) o nfty$。这意味着 CRPs 在 $180^irc$ 旋转处存在奇点。
• 优点：无冗余约束（3个参数），且其运动学方程不含三角函数，计算极快。
• 缺点：无法描述 $180^irc$ 附近的姿态，限制了其在大角度机动中的应用。

---

3. 修正罗德里格斯参数 (MRPs)

为了克服 CRPs 在 $180^irc$ 处的奇点，Marandi 和 Modi 在 1987 年提出了修正罗德里格斯参数 (MRPs)。

3.1 定义

MRPs $oldsymbol{igma}$ 定义为：

$$oldsymbol{igma} = athbf{k} aneft(rac{ heta}{4}ight)$$

注意这里使用的是 1/4 角。

3.2 与四元数的关系

利用半角公式 $ an(rac{ heta}{4}) = rac{in( heta/2)}{1+os( heta/2)}$，可得：

$$oldsymbol{igma} = rac{oldsymbol{eta}}{1 + eta_0}$$

几何上，这是将四元数从超球面的“南极” $(-1, 0, 0, 0)$ 投影到赤道平面上的结果。

3.3 奇点分析

MRPs 的奇点发生在 $ heta = m 360^irc$ ($eta_0 = -1$)。 这意味着 MRPs 可以描述 $0$ 到 $360^irc$ 范围内的所有姿态！ 对于刚体旋转，$m 360^irc$ 等同于 $0^irc$。因此，MRPs 在物理上几乎是全局非奇异的。

---

4. 伴影集 (Shadow Set) 与对偶性

虽然 MRPs 的奇点在 $360^irc$，但当 $ heta$ 接近 $360^irc$ 时，$oldsymbol{igma}$ 的模长会变得非常大，导致数值精度下降。为了解决这个问题，我们引入了伴影集 (Shadow Set) 的概念。

4.1 伴影集定义

对于任意姿态，存在两组 MRPs 参数：

1. 原参数 $oldsymbol{igma}$：对应四元数 $(eta_0, oldsymbol{eta})$。
2. 伴影参数 $oldsymbol{igma}_S$：对应四元数 $(-eta_0, -oldsymbol{eta})$（代表同一物理姿态）。

根据定义：

$$oldsymbol{igma}_S = rac{-oldsymbol{eta}}{1 - eta_0}$$

两者满足对偶关系：

$$oldsymbol{igma}_S = -rac{oldsymbol{igma}}{oldsymbol{igma}^2}$$

4.2 模长性质

容易证明：如果 $oldsymbol{igma} > 1$，则 $oldsymbol{igma}_S < 1$。 $oldsymbol{igma} = 1$ 对应于 $ heta = 180^irc$。

4.3 切换策略 (Switching Strategy)

为了保证数值稳定性，我们在仿真或控制中采用如下策略： 始终保持 $oldsymbol{igma} e 1$。 当检测到 $oldsymbol{igma} > 1$ 时，立即切换到其伴影集：

$$oldsymbol{igma} eftarrow -rac{oldsymbol{igma}}{oldsymbol{igma}^2}$$

这一操作将姿态描述从“长路径”（$ heta > 180^irc$）切换到了“短路径”（$ heta’ = heta - 360^irc$）。 通过这种机制，MRPs 实现了全局无奇点、无数值发散的姿态描述，是目前最完美的 3 参数姿态描述法。

---

5. 运动学微分方程

MRPs 的运动学方程非常简洁，且不含三角函数：

$$ot{oldsymbol{igma}} = rac{1}{4} eft[ (1 - oldsymbol{igma}^T oldsymbol{igma}) athbf{I} + 2 [oldsymbol{igma} imes] + 2 oldsymbol{igma} oldsymbol{igma}^T ight] oldsymbol{mega}$$

或者写成矩阵形式 $ot{oldsymbol{igma}} = rac{1}{4} athbf{B}(oldsymbol{igma}) oldsymbol{mega}$。 这一方程是多项式形式，非常适合计算机快速求解。

---

结语

从 CRPs 到 MRPs，我们见证了数学家如何通过巧妙的变换，一步步逼近完美的姿态描述。MRPs 结合了 3 参数的简洁性和四元数的鲁棒性，配合伴影集切换策略，成为了现代航天动力学的“瑞士军刀”。

下一章，我们将从运动学转向动力学，推导描述刚体转动核心规律的——欧拉动力学方程。

三维旋转的本质

在机器人学和航空航天领域，精确描述和操作三维空间中的物体姿态是一项核心任务，一个物体从一个参考姿态到另一个姿态的变换，本质上是一次旋转。为了量化和计算这种旋转，数学家和工程师们发展了多种方法，如欧拉角、方向余弦矩阵和四元数。

其中，基于“轴-角”概念的描述方法以其物理直观性而被广泛应用。该方法指出：任何复杂的三维旋转都可以等效为绕空间中一个固定的轴（AKA主轴，Eigenaxis）旋转一个特定的角度，也即主角度，Principal Angle。这一思想由19世纪的法国数学家Olinde Rodrigues系统地阐述，并最终形成了著名的Rodrigues’ Rotation Formula，这是现代姿态动力学中一系列重要参数的基础。

这篇文章将从最核心的罗德里格斯旋转公式出发，详细推导其向量和矩阵形式，并进一步探讨由此衍生的两种重要的姿态参数——经典罗德里格斯参数CRPs和修正罗德里格斯参数MRPs，深入分析它们的数学性质、几何意义、优缺点以及在避免奇点问题上的巧妙解决方案。

---

罗德里格斯旋转公式：旋转的向量描述

罗德里格斯旋转公式提供了一个直接的方法来计算一个向量v绕一个单位向量k（定义了旋转轴）旋转角度$\theta$后得到的新向量$\mathbf{v}_{\text{rot}}$。

公式的最终形式如下：

$$\mathbf{v}_{\text{rot}} = \mathbf{v} \cos\theta + (\mathbf{k} \times \mathbf{v}) \sin\theta + \mathbf{k}(\mathbf{k} \cdot \mathbf{v})(1 - \cos\theta)$$

旋转被分解为三个具向量分量的和，每个分量都具有明确的几何意义。

公式推导

首先，将向量v分解为平行于旋转轴k的分量 $\mathbf{v}_{|}$ 和垂直于k的分量 $\mathbf{v}_{\perp}$

$$\mathbf{v} = \mathbf{v}_{\|} + \mathbf{v}_{\perp}$$

• 平行分量 $\mathbf{v}_{|}$： v在k上的投影。根据向量投影的定义，有：

  $$\mathbf{v}_{\|} = (\mathbf{v} \cdot \mathbf{k})\mathbf{k}$$

  由于这个分量本身就在旋转轴上，因此旋转对它没有影响，即 $\mathbf{v}_{|,\text{rot}} = \mathbf{v}_{|}$。

• 垂直分量 $\mathbf{v}_{\perp}$： 垂直分量可以通过从原向量v中减去平行分量得到：

  $$\mathbf{v}_{\perp} = \mathbf{v} - \mathbf{v}_{\|} = \mathbf{v} - (\mathbf{v} \cdot \mathbf{k})\mathbf{k}$$

  分量位于一个垂直于k的平面上，所有的旋转都发生在这个平面内。当$\mathbf{v}_{\perp}$旋转$\theta$角时，可以将其旋转后的向量$\mathbf{v}_{\perp,\text{rot}}$分解。

  为了描述这个平面内的旋转，我们需要一个与$\mathbf{v}_{\perp}$正交且长度相同的基向量。向量叉乘 $\mathbf{k} \times \mathbf{v}$ 恰好满足这个要求。由于k是单位向量且垂直于$\mathbf{v}_{\perp}$，所以 $|\mathbf{k} \times \mathbf{v}_{\perp}| = |\mathbf{k}||\mathbf{v}_{\perp}|\sin(90^\circ) = |\mathbf{v}_{\perp}|$。同时，我们知道 $\mathbf{k} \times \mathbf{v} = \mathbf{k} \times (\mathbf{v}_{|} + \mathbf{v}_{\perp}) = \mathbf{k} \times \mathbf{v}_{|} + \mathbf{k} \times \mathbf{v}_{\perp} = \mathbf{0} + \mathbf{k} \times \mathbf{v}_{\perp}$，所以我们可以使用 $\mathbf{k} \times \mathbf{v}$ 作为这个正交基向量。

  在由 $\mathbf{v}_{\perp}$ 和 $\mathbf{k} \times \mathbf{v}$ 构成的二维平面中，旋转后的向量 $\mathbf{v}_{\perp, \text{rot}}$ 可以表示为：

  $$\mathbf{v}_{\perp, \text{rot}} = \mathbf{v}_{\perp} \cos\theta + (\mathbf{k} \times \mathbf{v}) \sin\theta$$

.svg)

• 组合结果： 旋转后的总向量 $\mathbf{v}_{\text{rot}}$ 是其旋转后平行与垂直分量的和： $$\mathbf{v}_{\text{rot}} = \mathbf{v}_{\|,\text{rot}} + \mathbf{v}_{\perp, \text{rot}}$$ $$\mathbf{v}_{\text{rot}} = \mathbf{v}_{\|} + \mathbf{v}_{\perp} \cos\theta + (\mathbf{k} \times \mathbf{v}) \sin\theta$$ 将 $\mathbf{v}_{|} = (\mathbf{v} \cdot \mathbf{k})\mathbf{k}$ 和 $\mathbf{v}_{\perp} = \mathbf{v} - \mathbf{v}_{|}$ 代入上式： $$\mathbf{v}_{\text{rot}} = (\mathbf{v} \cdot \mathbf{k})\mathbf{k} + (\mathbf{v} - (\mathbf{v} \cdot \mathbf{k})\mathbf{k}) \cos\theta + (\mathbf{k} \times \mathbf{v}) \sin\theta$$ 整理后即可得到最终的罗德里格斯旋转公式： $$\mathbf{v}_{\text{rot}} = \mathbf{v} \cos\theta + (\mathbf{k} \times \mathbf{v}) \sin\theta + \mathbf{k}(\mathbf{k} \cdot \mathbf{v})(1 - \cos\theta)$$

矩阵形式

为了便于求解和计算，我们通常将旋转表示为矩阵形式

叉乘运算 $\mathbf{k} \times \mathbf{v}$ 可以用一个反对称矩阵 $\mathbf{K}$ 与向量 $\mathbf{v}$ 的乘积来表示：

$$\mathbf{k} \times \mathbf{v} = \mathbf{K}\mathbf{v}$$

如果 $\mathbf{k} = [k_x, k_y, k_z]^T$，则：

$$\mathbf{K} = \begin{bmatrix} 0 & -k_z & k_y \\ k_z & 0 & -k_x \\ -k_y & k_x & 0 \end{bmatrix}$$

将这个关系代入旋转公式：

$$\mathbf{v}_{\text{rot}} = \mathbf{v} \cos\theta + \mathbf{K}\mathbf{v} \sin\theta + (\mathbf{k}\mathbf{k}^T)\mathbf{v}(1 - \cos\theta)$$

注意到 $\mathbf{k}(\mathbf{k}\cdot\mathbf{v}) = (\mathbf{k}\mathbf{k}^T)\mathbf{v}$。同时，可以证明 $\mathbf{K}^2 = \mathbf{k}\mathbf{k}^T - \mathbf{I}$，其中 I 是3x3单位矩阵。因此，$\mathbf{k}\mathbf{k}^T = \mathbf{K}^2 + \mathbf{I}$。代入上式并整理：

$$\mathbf{v}_{\text{rot}} = \left( \mathbf{I}\cos\theta + \mathbf{K}\sin\theta + (\mathbf{K}^2+\mathbf{I})(1-\cos\theta) \right) \mathbf{v}$$ $$\mathbf{v}_{\text{rot}} = \left( \mathbf{I} + (\sin\theta)\mathbf{K} + (1-\cos\theta)\mathbf{K}^2 \right) \mathbf{v}$$

因此，旋转矩阵R可以表示为：

$$\mathbf{R} = \mathbf{I} + (\sin\theta)\mathbf{K} + (1-\cos\theta)\mathbf{K}^2$$

---

姿态运动学参数

虽然旋转矩阵R能够完全描述旋转，但它包含9个元素，而三维旋转只有3个自由度，这导致了冗余。为了寻求更紧凑的表示方法，运动学中引入了多种参数集，其中罗德里格斯参数及其变体是重要的一族。

1. 经典罗德里格斯参数 (CRP)

经典罗德里格斯参数CRP，有时也称为Gibbs Vector，直接源于轴-角表示法。

定义

CRP是一个三维向量 $\mathbf{q}$，其方向与主轴 $\mathbf{k}$ 相同，其模的大小与主角度 $\theta$ 的半角正切相关：

$$\mathbf{q} = \mathbf{k} \tan\left(\frac{\theta}{2}\right)$$

与欧拉四元数的关系

CRP与欧拉四元数 $(\beta_0, \boldsymbol{\beta})$ 有着简洁的关系。已知四元数定义为 $\beta_0 = \cos(\theta/2)$ 和 $\boldsymbol{\beta} = \mathbf{k}\sin(\theta/2)$，我们可以轻易推导出：

$$q_i = \frac{k_i \sin(\theta/2)}{\cos(\theta/2)} = \frac{\beta_i}{\beta_0} \quad (i=1,2,3)$$

反之，从CRP也可以求得四元数：

$$\beta_0 = \frac{1}{\sqrt{1 + \mathbf{q}^T\mathbf{q}}}, \quad \beta_i = \frac{q_i}{\sqrt{1 + \mathbf{q}^T\mathbf{q}}}$$

性质

• 唯一性: CRP与姿态具有唯一对应关系，克服了四元数中 $(\boldsymbol{\beta}, \beta_0)$ 和 $(-\boldsymbol{\beta}, -\beta_0)$ 描述同一旋转的模糊性。
• 奇点 (Singularity): CRP存在一个严重的奇点问题。当主旋转角 $\theta \to \pm\pi$ (180度) 时，$\tan(\theta/2) \to \infty$。这意味着围绕任意轴旋转180度的姿态都无法用有限的CRP值表示。
• 不可叠加性: 两次连续旋转的CRP不能通过简单的向量加法得到总旋转的CRP。

几何意义

CRP的几何意义可以通过球极投影（Stereographic Projection）来理解。想象一个四维单位超球面，欧拉四元数 $(\beta_0, \boldsymbol{\beta})$ 是球面上的一个点。CRP是这个点从北极点 $(\beta_0=1, \boldsymbol{\beta}=0)$ 投影到超平面 $\beta_0=0$ 上的影像。当旋转角度 $\theta \to \pi$ 时，$\beta_0 \to 0$，这意味着投影点趋于无穷远，从而导致了奇点。

2. 修正罗德里格斯参数 (MRP)

为了解决CRP在180度时的奇点问题，并获得在更大角度范围内表现更接近线性的参数，修正罗德里格斯参数（MRP）应运而生。

定义

MRP是一个三维向量 $\boldsymbol{\sigma}$，其定义与CRP非常相似，但使用了主角度的四分之一角：

$$\boldsymbol{\sigma} = \mathbf{k} \tan\left(\frac{\theta}{4}\right)$$

与欧拉四元数的关系

利用半角/四分之一角恒等式，可以推导出MRP与四元数的关系：

$$\sigma_i = k_i \tan\left(\frac{\theta}{4}\right) = k_i \frac{\sin(\theta/2)}{1+\cos(\theta/2)} = \frac{\beta_i}{1+\beta_0} \quad (i=1,2,3)$$

性质与优势

• 奇点位置: MRP的奇点发生在 $1+\beta_0=0$，即 $\beta_0=-1$ 时。这对应于主旋转角 $\theta = \pm 2\pi$ (360度)。对于大多数物理系统，旋转360度等同于没有旋转，这个奇点在实际应用中几乎不会遇到，这是一个巨大的优势。
• 线性范围更广: MRP在其定义域内比CRP更接近线性，尤其是在大角度旋转时，这对于控制算法的设计非常有利。

几何意义

MRP同样可以通过球极投影来解释。不同的是，MRP是将四元数点从南极点 $(\beta_0=-1, \boldsymbol{\beta}=0)$ 投影到超平面 $\beta_0=0$ 上的影像。这种投影方式巧妙地将奇点从180度移动到了360度。

3. 伴影集 (Shadow Set) 与奇点规避

尽管MRP已经将奇点移至一个通常无害的位置，但在数值计算中，当旋转角度接近360度时，MRP的模会变得非常大，可能导致计算不稳定。为此，引入了伴影集（Shadow Set）的概念来彻底解决这个问题。

定义

对于任意一个MRP $\boldsymbol{\sigma}$，其伴影集 $\boldsymbol{\sigma}^S$ 被定义为：

$$\boldsymbol{\sigma}^S = -\frac{\boldsymbol{\sigma}}{\|\boldsymbol{\sigma}\|^2}$$

可以证明，$\boldsymbol{\sigma}$ 和 $\boldsymbol{\sigma}^S$ 描述的是完全相同的物理姿态。它们的区别在于，一个对应于主角度 $\theta$，另一个则对应于 $2\pi-\theta$ 或 $2\pi+\theta$。

一个关键性质是，如果 $|\boldsymbol{\sigma}| > 1$，那么 $|\boldsymbol{\sigma}^S| < 1$。

切换策略

在进行姿态动力学积分或控制计算时，可以采用以下切换策略：

1. 始终使用模长小于或等于1的MRP集进行计算。
2. 当计算过程中发现 $|\boldsymbol{\sigma}| > 1$ 时，立即切换到其伴影集 $\boldsymbol{\sigma}^S$ 来继续后续的计算。
3. 由于 $|\boldsymbol{\sigma}| = 1$ 对应于主角度 $\theta = \pi$ (180度)，这个策略保证了所使用的姿态参数始终描述小于180度的最短旋转路径，完美地规避了所有奇点和数值溢出问题。

参数间的转换与运动学

CRP 与 MRP 转换

$$\mathbf{q} = \frac{2\boldsymbol{\sigma}}{1 - \|\boldsymbol{\sigma}\|^2}, \quad \boldsymbol{\sigma} = \frac{\mathbf{q}}{1 + \sqrt{1 + \|\mathbf{q}\|^2}}$$

运动学微分方程

姿态参数的时间导数与航天器角速度 $\boldsymbol{\omega}$ 之间的关系至关重要。

• CRP运动学方程:

  $$\dot{\mathbf{q}} = \frac{1}{2}\left( \mathbf{I} + [\tilde{\mathbf{q}}] + \mathbf{q}\mathbf{q}^T \right)\boldsymbol{\omega}$$

• MRP运动学方程:

  $$\dot{\boldsymbol{\sigma}} = \frac{1}{4}\left( (1-\|\boldsymbol{\sigma}\|^2)\mathbf{I} + 2[\tilde{\boldsymbol{\sigma}}] + 2\boldsymbol{\sigma}\boldsymbol{\sigma}^T \right)\boldsymbol{\omega}$$

其中 $[\tilde{\mathbf{a}}]$ 表示向量 $\mathbf{a}$ 对应的反对称叉乘矩阵。可以看到MRP的运动学方程略显复杂，但在引入伴影集后，其鲁棒性远超CRP