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=Cbirb(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˙birb+Cbir˙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˙birb+Cbi(−ωibb×rb)即 C˙birb=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[r1br2br3b]T=Cbi(ωibb×)[r1br2br3b]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
[r1br2br3b]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×)Cibri(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)+∫0tC(τ)Ω(τ)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τ1C(τ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τ1C(τ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τ1C(τ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−1tmω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)iCb(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+ΔθmsinΔθm[Δθm×]+Δθm1−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.