方向余弦阵微分方程及其求解

1.方向余弦阵微分方程
\qquad 假设 b b b系与 i i i系具有共同的原点， b b b系相对于 i i i系转动的角速度为 ω i b \omega_{ib} ωib​，从 i i i系到 b b b系的坐标系变换矩阵记作 C b i C_b^i Cbi​，为时变矩阵。假设 r r r为 i i i系中一固定矢量，在矢量 r r r在 b b b系与 i i i系之间的转换关系为: r i = C b i r b (1.1) r^i=C_b^ir^b \tag{1.1} ri=Cbi​rb(1.1) \qquad 将式(1.1)两边对时间微分得: r ˙ i = C ˙ b i r b + C b i r ˙ b (1.2) \dot{r}^i=\dot{C}_b^ir^b+C_b^i\dot{r}^b \tag{1.2} r˙i=C˙bi​rb+Cbi​r˙b(1.2) \qquad 其中 r r r为 i i i系中的固定矢量，则有 r ˙ i = 0 \dot{r}^i=0 r˙i=0。由于 b b b系相对于 i i i系转动的角速度为 ω i b \omega_{ib} ωib​，那么在 b b b系上观察 r r r的角速度应为 − ω i b -\omega_{ib} −ωib​，且 r ˙ b = − ω i b b × r b \dot{r}^b=-\omega_{ib}^b\times r^b r˙b=−ωibb​×rb，则式(2)可表示为: 0 = C ˙ b i r b + C b i ( − ω i b b × r b ) 即   C ˙ b i r b = C b i ( ω i b b × ) r b (1.3) 0=\dot{C}_b^ir^b+C_b^i(-\omega_{ib}^b\times r^b)即\ \dot{C}_b^ir^b=C_b^i(\omega_{ib}^b\times) r^b \tag{1.3} 0=C˙bi​rb+Cbi​(−ωibb​×rb)即 C˙bi​rb=Cbi​(ωibb​×)rb(1.3) \qquad 由于式(3)对 i i i系中任意固定矢量 r r r均成立，则任选三个不共面得到向量 r 1 , r 2 , r 3 r_1,r_2,r_3 r1​,r2​,r3​，则有: C ˙ b i [ r 1 b r 2 b r 3 b ] T = C b i ( ω i b b × ) [ r 1 b r 2 b r 3 b ] T (1.4) \dot{C}_b^i\begin{bmatrix}r_1^b&r_2^b&r_3^b \end{bmatrix}^T=C_b^i(\omega_{ib}^b\times) \begin{bmatrix}r_1^b&r_2^b&r_3^b \end{bmatrix}^T \tag{1.4} C˙bi​[r1b​​r2b​​r3b​​]T=Cbi​(ωibb​×)[r1b​​r2b​​r3b​​]T(1.4) \qquad 显然矩阵 [ r 1 b r 2 b r 3 b ] T \begin{bmatrix}r_1^b&r_2^b&r_3^b \end{bmatrix}^T [r1b​​r2b​​r3b​​]T可逆。所以总有: C ˙ b i = C b i ( ω i b × ) (1.5) \dot{C}_b^i=C_b^i(\omega_{ib}\times) \tag{1.5} C˙bi​=Cbi​(ωib​×)(1.5) \qquad 式(5)即方向余弦微分方程，或称姿态阵微分方程，它建立了动坐标系与参考坐标之间的方向余弦阵与动坐标系运动角速度之间的关系。
\qquad 矢量 r r r在 b b b系与 i i i中的线速度关系可表示为: ω i b i × r i = C b i ( ω i b b × r b ) (1.6) \omega_{ib}^i\times r^i=C_b^i(\omega_{ib}^b\times r^b) \tag{1.6} ωibi​×ri=Cbi​(ωibb​×rb)(1.6)
\qquad 进一步可推出: ω i b i × r i = C b i ( ω i b b × r b ) = C b i ( ω i b b × ) r b = C b i ( ω i b b × ) C i b r i (1.7) \omega_{ib}^i\times r^i=C_b^i(\omega_{ib}^b\times r^b)=C_b^i (\omega_{ib}^b\times) r^b= C_b^i (\omega_{ib}^b\times) C_i^b r^i\tag{1.7} ωibi​×ri=Cbi​(ωibb​×rb)=Cbi​(ωibb​×)rb=Cbi​(ωibb​×)Cib​ri(1.7) \qquad 则有: ( ω i b i × ) = C b i ( ω i b b × ) C i b (1.8) (\omega_{ib}^i\times)=C_b^i (\omega_{ib}^b\times) C_i^b \tag{1.8} (ωibi​×)=Cbi​(ωibb​×)Cib​(1.8) \qquad C b i C_b^i Cbi​为单位正交矩阵，可知 ( C b i ) − 1 = ( C b i ) T = C i b (C_b^i)^{-1}=(C_b^i)^T=C_i^b (Cbi​)−1=(Cbi​)T=Cib​。根据式(5)和式(8)可得，以下四种方向余弦阵微分方程相互等价: C ˙ b i = C b i ( ω i b b × ) (1.9) \dot{C}_b^i=C_b^i(\omega_{ib}^b\times) \tag{1.9} C˙bi​=Cbi​(ωibb​×)(1.9) C ˙ b i = ( ω i b i × ) C b i (1.10) \dot{C}_b^i=(\omega_{ib}^i\times)C_b^i \tag{1.10} C˙bi​=(ωibi​×)Cbi​(1.10) C ˙ i b = ( ω i b b × ) C i b (1.11) \dot{C}_i^b=(\omega_{ib}^b\times)C_i^b \tag{1.11} C˙ib​=(ωibb​×)Cib​(1.11) C ˙ i i = C i b ( ω i b i × ) (1.12) \dot{C}_i^i=C_i^b (\omega_{ib}^i\times) \tag{1.12} C˙ii​=Cib​(ωibi​×)(1.12)

2.方向余弦阵微分方程求解
\qquad 为了方便书写及运算推导，略去复杂的上下标，记反对称矩阵 Ω ( t ) = [ ω i b b ( t ) × ] \Omega(t)=[\omega_{ib}^b(t)\times] Ω(t)=[ωibb​(t)×]，姿态阵微分方程表示为: C ˙ ( t ) = C ( t ) Ω ( t ) (2.1) \dot{C}(t)=C(t)\Omega(t) \tag{2.1} C˙(t)=C(t)Ω(t)(2.1) \qquad 首先对式(2.1)在时间区间 [ 0 , t ] [0,t] [0,t]上积分，有: C ( t ) = C ( 0 ) + ∫ 0 t C ( τ ) Ω ( τ ) d τ (2.2) C(t)=C(0)+\int_0^tC(\tau)\Omega(\tau)d\tau \tag{2.2} C(t)=C(0)+∫0t​C(τ)Ω(τ)dτ(2.2) \qquad 由于式(2.2)右侧被积函数中依然含有待求项 C ( t ) C(t) C(t)，将式(2.2)重复带入积分号内，第一次带入有: C ( t ) = C ( 0 ) + ∫ 0 t [ C ( 0 ) + ∫ 0 τ C ( τ 1 ) Ω ( τ 1 ) d τ 1 ] Ω ( τ ) d τ = C ( 0 ) [ I + ∫ 0 t Ω ( τ ) d τ ] + ∫ 0 t ∫ 0 τ C ( τ 1 ) Ω ( τ 1 ) d τ 1 Ω ( τ ) d τ (2.3) \begin{aligned}C(t)&=C(0)+\int_0^t[C(0)+\int_0^\tau C(\tau_1)\Omega(\tau_1)d\tau_1]\Omega(\tau)d\tau \\ &=C(0)[I+\int_0^t\Omega(\tau)d\tau]+\int_0^t\int_0^\tau C(\tau_1)\Omega(\tau_1)d\tau_1 \Omega(\tau)d\tau \end{aligned} \tag{2.3} C(t)​=C(0)+∫0t​[C(0)+∫0τ​C(τ1​)Ω(τ1​)dτ1​]Ω(τ)dτ=C(0)[I+∫0t​Ω(τ)dτ]+∫0t​∫0τ​C(τ1​)Ω(τ1​)dτ1​Ω(τ)dτ​(2.3) \qquad 第二次带入有: C ( t ) = C ( 0 ) [ I + ∫ 0 t Ω ( τ ) d τ + ∫ 0 t ∫ 0 τ Ω ( τ 1 ) d τ 1 Ω ( τ ) d τ ] + ∫ 0 t ∫ 0 τ ∫ 0 τ 1 C ( τ 2 ) Ω ( τ 2 ) d τ 2 Ω ( τ 1 ) d τ 1 Ω ( τ ) d τ (2.4) C(t)=C(0)[I+\int_0^t\Omega(\tau)d\tau+\int_0^t\int_0^\tau \Omega(\tau_1)d\tau_1 \Omega(\tau)d\tau ]\\+\int_0^t\int_0^\tau \int_0^{\tau_1} C(\tau_2)\Omega(\tau_2)d\tau_2 \Omega(\tau_1)d\tau_1 \Omega(\tau)d\tau \tag{2.4} C(t)=C(0)[I+∫0t​Ω(τ)dτ+∫0t​∫0τ​Ω(τ1​)dτ1​Ω(τ)dτ]+∫0t​∫0τ​∫0τ1​​C(τ2​)Ω(τ2​)dτ2​Ω(τ1​)dτ1​Ω(τ)dτ(2.4) \qquad 经过不断迭代，可得到无限重积分，即毕卡级数: C ( t ) = C ( 0 ) [ I + ∫ 0 t Ω ( τ ) d τ + ∫ 0 t ∫ 0 τ Ω ( τ 1 ) d τ 1 Ω ( τ ) d τ ] + ∫ 0 t ∫ 0 τ ∫ 0 τ 1 C ( τ 2 ) Ω ( τ 2 ) d τ 2 Ω ( τ 1 ) d τ 1 Ω ( τ ) d τ + … … ] (2.5) C(t)=C(0)[I+\int_0^t\Omega(\tau)d\tau+\int_0^t\int_0^\tau \Omega(\tau_1)d\tau_1 \Omega(\tau)d\tau ]\\+\int_0^t\int_0^\tau \int_0^{\tau_1} C(\tau_2)\Omega(\tau_2)d\tau_2 \Omega(\tau_1)d\tau_1 \Omega(\tau)d\tau +…… ] \tag{2.5} C(t)=C(0)[I+∫0t​Ω(τ)dτ+∫0t​∫0τ​Ω(τ1​)dτ1​Ω(τ)dτ]+∫0t​∫0τ​∫0τ1​​C(τ2​)Ω(τ2​)dτ2​Ω(τ1​)dτ1​Ω(τ)dτ+……](2.5) \qquad 式(2.5)所示级数显然是收敛的，但一般情况下无法得出闭合解。
\qquad 假设对于任意时间 t , τ ∈ [ 0 , T ] t,\tau \in[0,T] t,τ∈[0,T]，转动角速度满足以下可交换条件: Ω ( t ) Ω ( τ ) = Ω ( τ ) Ω ( t ) (2.6) \Omega(t)\Omega(\tau)=\Omega(\tau)\Omega(t) \tag{2.6} Ω(t)Ω(τ)=Ω(τ)Ω(t)(2.6) \qquad 则有 ∫ 0 t Ω ( t ) Ω ( τ ) d τ = ∫ 0 t Ω ( τ ) Ω ( t ) d τ ⇒ Ω ( t ) ∫ 0 t Ω ( τ ) d τ = ∫ 0 t Ω ( τ ) d τ Ω ( t ) (2.7) \int_0^t\Omega(t)\Omega(\tau)d\tau=\int_0^t\Omega(\tau)\Omega(t)d\tau \rArr \Omega(t)\int_0^t\Omega(\tau)d\tau=\int_0^t\Omega(\tau)d\tau\Omega(t) \tag{2.7} ∫0t​Ω(t)Ω(τ)dτ=∫0t​Ω(τ)Ω(t)dτ⇒Ω(t)∫0t​Ω(τ)dτ=∫0t​Ω(τ)dτΩ(t)(2.7) \qquad 在式(2.6)成立的条件下，对下列微分有: d d t [ ∫ 0 t Ω ( τ ) d τ ] 2 = d d t [ ( ∫ 0 t Ω ( τ ) d τ ) ⋅ ( ∫ 0 t Ω ( τ ) d τ ) ] = Ω ( t ) ∫ 0 t Ω ( τ ) d τ + ∫ 0 t Ω ( τ ) d τ Ω ( t ) = 2 ∫ 0 t Ω ( τ ) d τ Ω ( t ) (2.8) \begin{aligned} \frac{d}{dt}[\int_0^t\Omega(\tau)d\tau]^2&=\frac{d}{dt}[(\int_0^t\Omega(\tau)d\tau)\cdot(\int_0^t\Omega(\tau)d\tau)]\\ &=\Omega(t)\int_0^t\Omega(\tau)d\tau+\int_0^t\Omega(\tau)d\tau\Omega(t) \\&=2\int_0^t\Omega(\tau)d\tau\Omega(t) \end{aligned} \tag{2.8} dtd​[∫0t​Ω(τ)dτ]2​=dtd​[(∫0t​Ω(τ)dτ)⋅(∫0t​Ω(τ)dτ)]=Ω(t)∫0t​Ω(τ)dτ+∫0t​Ω(τ)dτΩ(t)=2∫0t​Ω(τ)dτΩ(t)​(2.8) \qquad 由式(2.8)可知: ∫ 0 t ∫ 0 τ Ω ( τ 1 ) d τ 1 Ω ( τ ) d τ = 1 2 [ ∫ 0 t Ω ( τ ) d τ ] 2 (2.9) \int_0^t\int_0^\tau \Omega(\tau_1)d\tau_1 \Omega(\tau)d\tau =\frac{1}{2}[\int_0^t\Omega(\tau)d\tau]^2 \tag{2.9} ∫0t​∫0τ​Ω(τ1​)dτ1​Ω(τ)dτ=21​[∫0t​Ω(τ)dτ]2(2.9) \qquad 同理有: ∫ 0 t ∫ 0 τ ∫ 0 τ 1 C ( τ 2 ) Ω ( τ 2 ) d τ 2 Ω ( τ 1 ) d τ 1 Ω ( τ ) d τ = 1 6 [ ∫ 0 t Ω ( τ ) d τ ] 3 (2.10) \int_0^t\int_0^\tau \int_0^{\tau_1} C(\tau_2)\Omega(\tau_2)d\tau_2 \Omega(\tau_1)d\tau_1 \Omega(\tau)d\tau =\frac{1}{6}[\int_0^t\Omega(\tau)d\tau]^3 \tag{2.10} ∫0t​∫0τ​∫0τ1​​C(τ2​)Ω(τ2​)dτ2​Ω(τ1​)dτ1​Ω(τ)dτ=61​[∫0t​Ω(τ)dτ]3(2.10) \qquad 在满足式(2.6)的可交换条件下，式(2.5)所示毕卡级数可化为闭合解形式: C ( t ) = C ( 0 ) [ I + ∫ 0 t Ω ( τ ) d τ + 1 2 ! ( ∫ 0 t Ω ( τ ) d τ ) 2 + 1 3 ! [ ∫ 0 t Ω ( τ ) d τ ] 3 + … … ] = C ( 0 ) e ∫ 0 t Ω ( τ ) d τ (2.11) \begin{aligned}C(t)&=C(0)[I+\int_0^t\Omega(\tau)d\tau+\frac{1}{2!}(\int_0^t\Omega(\tau)d\tau)^2+\frac{1}{3!}[\int_0^t\Omega(\tau)d\tau]^3+……] \\ &=C(0)e^{\int_0^t\Omega(\tau)d\tau}\end{aligned} \tag{2.11} C(t)​=C(0)[I+∫0t​Ω(τ)dτ+2!1​(∫0t​Ω(τ)dτ)2+3!1​[∫0t​Ω(τ)dτ]3+……]=C(0)e∫0t​Ω(τ)dτ​(2.11) \qquad 对于时间区间 [ 0 , T ] [0,T] [0,T]，记角度增量 θ ( T ) = ∫ 0 T ω ( τ ) d τ \theta(T)=\int_{0}^T\omega(\tau)d\tau θ(T)=∫0T​ω(τ)dτ且模值 θ ( T ) = ∣ θ ( T ) ∣ \theta(T)=|\theta(T)| θ(T)=∣θ(T)∣，则有角度增量的矩阵指数函数为: e ∫ 0 T Ω ( τ ) d τ = e ( θ ( T ) × ) = I + s i n θ ( T ) θ ( T ) [ θ ( T ) × ] + 1 − c o s θ ( T ) θ ( T ) [ θ ( T ) × ] 2 (2.12) e^{\int_{0}^T\Omega(\tau)d\tau}=e^{(\theta(T)\times)}=I+\frac{sin\theta(T)}{\theta(T)}[\theta(T)\times]+\frac{1-cos\theta(T)}{\theta(T)}[\theta(T)\times]^2 \tag{2.12} e∫0T​Ω(τ)dτ=e(θ(T)×)=I+θ(T)sinθ(T)​[θ(T)×]+θ(T)1−cosθ(T)​[θ(T)×]2(2.12) \qquad 因此式(2.11)在 T T T时刻的解为: C ( t ) = C ( 0 ) C T 0 (2.13) C(t)=C(0)C_T^0 \tag{2.13} C(t)=C(0)CT0​(2.13) \qquad 其中 C T 0 = I + s i n θ ( T ) θ ( T ) [ θ ( T ) × ] + 1 − c o s θ ( T ) θ ( T ) [ θ ( T ) × ] 2 (2.14) C_T^0 =I+\frac{sin\theta(T)}{\theta(T)}[\theta(T)\times]+\frac{1-cos\theta(T)}{\theta(T)}[\theta(T)\times]^2 \tag{2.14} CT0​=I+θ(T)sinθ(T)​[θ(T)×]+θ(T)1−cosθ(T)​[θ(T)×]2(2.14) \qquad 则对于时间区间 [ t m − 1 , t m ] [t_{m-1},t_m] [tm−1​,tm​]，假设 t m − 1 t_{m-1} tm−1​时刻的方向余弦矩阵为 C b ( m − 1 ) i C_{b(m-1)}^i Cb(m−1)i​， [ t m − 1 , t m ] [t_{m-1},t_m] [tm−1​,tm​]时间段内的角度增量为 Δ θ m = ∫ t m − 1 t m ω i b b ( t ) d t \Delta\theta_m=\int_{t_{m-1}}^{t_m}\omega_{ib}^b(t)dt Δθm​=∫tm−1​tm​​ωibb​(t)dt且记模值 Δ θ m = ∣ Δ θ m ∣ \Delta\theta_m=|\Delta\theta_m| Δθm​=∣Δθm​∣，则 t m t_m tm​时刻的姿态阵 C b ( m ) i C_{b(m)}^i Cb(m)i​为: C b ( m ) i = C b ( m − 1 ) i C b ( m ) b ( m − 1 ) (2.15) C_{b(m)}^i=C_{b(m-1)}^iC_{b(m)}^{b(m-1)} \tag{2.15} Cb(m)i​=Cb(m−1)i​Cb(m)b(m−1)​(2.15) C b ( m ) b ( m − 1 ) = I + s i n Δ θ m Δ θ m [ Δ θ m × ] + 1 − c o s Δ θ m Δ θ m [ Δ θ m × ] 2 (2.16) C_{b(m)}^{b(m-1)}=I+\frac{sin\Delta\theta_m}{\Delta\theta_m}[\Delta\theta_m\times]+\frac{1-cos\Delta\theta_m}{\Delta\theta_m}[\Delta\theta_m\times]^2 \tag{2.16} Cb(m)b(m−1)​=I+Δθm​sinΔθm​​[Δθm​×]+Δθm​1−cosΔθm​​[Δθm​×]2(2.16) \qquad 式(2.15)和式(12.6)即姿态阵离散化更新递推公式，但式(16)成立的前提是系统在 [ t m − 1 , t m ] [t_{m-1},t_m] [tm−1​,tm​]时间段内做定轴转动。对比式(2.13)和式(2.16)，可以发现二者在形式上完全一致，因而定轴转动的角增量 Δ θ m \Delta\theta_m Δθm​是以 b t m − 1 b_{t_{m-1}} btm−1​​系为参考， b t m b_{t_m} btm​​系相对于 b t m − 1 b_{t_{m-1}} btm−1​​系转动的等效旋转矢量。当式(2.6)可交换性条件不成立时，如果我们依然简单的利用式(2.16)进行计算就会引起姿态求解的不可交换误差。对于非定轴转动条件下的姿态更新，主要通过等闲旋转矢量来进行不可交换误差的补偿。

参考文献
[1] 严恭敏, 翁浚. 捷联惯导算法与组合导航原理[M]. 西安:西北工业大学出版社, 2020.

标签：tau,0t,求解,int,qquad,余弦,tag,微分方程,Omega
来源： https://blog.csdn.net/Anchor_Yin/article/details/120296604