视觉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−ϕ0z0ϕ0xϕ0y−ϕ0x0

初始条件为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的反对称矩阵。