视觉SLAM十四讲(v2)-4.1.2罗德格里斯公式推导中的R(t)微分方程求解步骤

视觉SLAM十四讲(v2)-4.1.2罗德格里斯公式推导中的R(t)微分方程求解步骤

在罗德里格斯公式的推导中，求解关于旋转矩阵R(t)\mathbf{R}(t)R(t)的微分方程是一个关键步骤。以下是详细的求解过程：

1. 微分方程的建立

根据罗德里格斯公式的推导，旋转矩阵R(t)\mathbf{R}(t)R(t)满足以下微分方程：
R˙(t)=ϕ0∧R(t)\dot{\mathbf{R}}(t) = \phi_0^\wedge \mathbf{R}(t)R˙(t)=ϕ0∧​R(t)
其中：
-R˙(t)\dot{\mathbf{R}}(t)R˙(t)是R(t)\mathbf{R}(t)R(t)对时间ttt的导数。
-ϕ0∧\phi_0^\wedgeϕ0∧​是由初始旋转向量ϕ0\phi_0ϕ0​构造的反对称矩阵，形式为：
ϕ0∧=[0−ϕ0zϕ0yϕ0z0−ϕ0x−ϕ0yϕ0x0]\phi_0^\wedge = \begin{bmatrix} 0 & -\phi_{0z} & \phi_{0y} \\ \phi_{0z} & 0 & -\phi_{0x} \\ -\phi_{0y} & \phi_{0x} & 0 \end{bmatrix}ϕ0∧​=​0ϕ0z​−ϕ0y​​−ϕ0z​0ϕ0x​​ϕ0y​−ϕ0x​0​​

• 初始条件为R(0)=I\mathbf{R}(0) = IR(0)=I，即t=0t = 0t=0时旋转矩阵为单位矩阵。

2. 微分方程的解的形式

该微分方程是一个一阶线性常微分方程，其通解形式为矩阵指数函数：
R(t)=exp⁡(ϕ0∧t)R(0)\mathbf{R}(t) = \exp(\phi_0^\wedge t) \mathbf{R}(0)R(t)=exp(ϕ0∧​t)R(0)
由于R(0)=I\mathbf{R}(0) = IR(0)=I，解简化为：
R(t)=exp⁡(ϕ0∧t)\mathbf{R}(t) = \exp(\phi_0^\wedge t)R(t)=exp(ϕ0∧​t)

3. 矩阵指数的泰勒展开

矩阵指数函数exp⁡(ϕ0∧t)\exp(\phi_0^\wedge t)exp(ϕ0∧​t)可以通过泰勒级数展开：
exp⁡(ϕ0∧t)=I+ϕ0∧t+(ϕ0∧t)22!+(ϕ0∧t)33!+⋯\exp(\phi_0^\wedge t) = I + \phi_0^\wedge t + \frac{(\phi_0^\wedge t)^2}{2!} + \frac{(\phi_0^\wedge t)^3}{3!} + \cdotsexp(ϕ0∧​t)=I+ϕ0∧​t+2!(ϕ0∧​t)2​+3!(ϕ0∧​t)3​+⋯

4. 反对称矩阵的性质

反对称矩阵ϕ0∧\phi_0^\wedgeϕ0∧​具有以下重要性质：
-(ϕ0∧)T=−ϕ0∧(\phi_0^\wedge)^T = -\phi_0^\wedge(ϕ0∧​)T=−ϕ0∧​

• 对于三维反对称矩阵，存在一个单位向量a\mathbf{a}a和标量θ\thetaθ使得ϕ0∧=θa∧\phi_0^\wedge = \theta \mathbf{a}^\wedgeϕ0∧​=θa∧，其中a∧\mathbf{a}^\wedgea∧是a\mathbf{a}a的反对称矩阵。
• 进一步有(ϕ0∧)3=−θ2ϕ0∧(\phi_0^\wedge)^3 = -\theta^2 \phi_0^\wedge(ϕ0∧​)3=−θ2ϕ0∧​（当ϕ0∧\phi_0^\wedgeϕ0∧​对应于旋转时）。

5. 利用性质简化泰勒级数

利用(ϕ0∧)3=−θ2ϕ0∧(\phi_0^\wedge)^3 = -\theta^2 \phi_0^\wedge(ϕ0∧​)3=−θ2ϕ0∧​（这里θ\thetaθ是旋转角，ϕ0=θa\phi_0 = \theta \mathbf{a}ϕ0​=θa），可以将泰勒级数中的高阶项进行归纳：

• 奇数次幂：(ϕ0∧t)2k+1(\phi_0^\wedge t)^{2k+1}(ϕ0∧​t)2k+1可以表示为(ϕ0∧t)(\phi_0^\wedge t)(ϕ0∧​t)乘以一个标量系数。
• 偶数次幂：(ϕ0∧t)2k(\phi_0^\wedge t)^{2k}(ϕ0∧​t)2k可以表示为(ϕ0∧t)2(\phi_0^\wedge t)^2(ϕ0∧​t)2乘以一个标量系数。

具体地，泰勒级数可以重写为：
exp⁡(ϕ0∧t)=I+sin⁡(θt)a∧+(1−cos⁡(θt))(a∧)2\exp(\phi_0^\wedge t) = I + \sin(\theta t) \mathbf{a}^\wedge + (1 - \cos(\theta t)) (\mathbf{a}^\wedge)^2exp(ϕ0∧​t)=I+sin(θt)a∧+(1−cos(θt))(a∧)2
其中θ=∥ϕ0∥\theta = \|\phi_0\|θ=∥ϕ0​∥，a=ϕ0θ\mathbf{a} = \frac{\phi_0}{\theta}a=θϕ0​​。

6. 验证初始条件

当t=0t = 0t=0时：
exp⁡(ϕ0∧⋅0)=I+0+(1−1)⋅(a∧)2=I\exp(\phi_0^\wedge \cdot 0) = I + 0 + (1 - 1) \cdot (\mathbf{a}^\wedge)^2 = Iexp(ϕ0∧​⋅0)=I+0+(1−1)⋅(a∧)2=I
满足初始条件R(0)=I\mathbf{R}(0) = IR(0)=I。

7. 最终解

因此，旋转矩阵R(t)\mathbf{R}(t)R(t)的解为：
R(t)=exp⁡(ϕ0∧t)=I+sin⁡(θt)a∧+(1−cos⁡(θt))(a∧)2\mathbf{R}(t) = \exp(\phi_0^\wedge t) = I + \sin(\theta t) \mathbf{a}^\wedge + (1 - \cos(\theta t)) (\mathbf{a}^\wedge)^2R(t)=exp(ϕ0∧​t)=I+sin(θt)a∧+(1−cos(θt))(a∧)2
其中：
-θ=∥ϕ0∥\theta = \|\phi_0\|θ=∥ϕ0​∥是旋转角的大小。
-a=ϕ0θ\mathbf{a} = \frac{\phi_0}{\theta}a=θϕ0​​是旋转轴方向的单位向量。
-a∧\mathbf{a}^\wedgea∧是a\mathbf{a}a的反对称矩阵。

总结

通过建立微分方程、利用矩阵指数的泰勒展开以及反对称矩阵的特殊性质，我们得到了旋转矩阵R(t)\mathbf{R}(t)R(t)的解析解。这一解在三维旋转的表示中具有重要意义，广泛应用于机器人学、计算机视觉和航空航天等领域。