方向余弦法
方向余弦法
姿态更新的方向余弦法实质上就是直接求解姿态矩阵解微分方程. 设r为某一空间向量,b为机体坐标系,导航坐标系n取地理坐标系g,则根据哥氏定理,
drdr|n=|b+ωnb⨯r (9.5.2) dtdt
上式两边诸向量向b坐标系投影,
drbdrb|n=|b+(ωnb⨯r)b (9.5.3) dtdt
由于
drbb |b=r dt
b(ωnb⨯r)b=ωnb⨯rb
所以式(9.5.3)可写成
b= rdrbb|n-ωnb⨯rb (9.5.4) dt
另外,根据向量在不同坐标系的变换关系,有
bn rb=Cnr
上式两边对时间求导
brn+Cbr brn+Cb b=C n=C rnnnndrn|n dt
上式中, drndr|n是在n坐标系中的投影,经Cnb左乘后即为在b坐标系dtdtn
中的投影,所以
brn+Cb b=C rnndrb|n (9.5.5) dt
比较式(9.5.4) 和式(9.5.5),得
brn=-ωb⨯rb=-ωb⨯Cbrn=-ωbkCbrn Cnnbnbnnbn
所以
b=-ωbkCb Cnnbn
(9.5.6)
bkb式中, ωnb是由ωnb构造的反对称矩阵,即若
bbbb ωnb=[ωnbx ωnby ωnbz ] T
⎡0⎢bbk=⎢ωnbz则 ωnb
b⎢-ωnby⎣b-ωnbz0bωnbxb⎤ωnbyb⎥-ωnbx⎥ (9.5.7) 0⎥⎦
式(9.5.6)式姿态矩阵微分方程的一种形式,根据计算方便,姿态矩阵微分方程还可以改写成其他形式.
若式(9.5.2)两边诸向量向导航坐标系n投影,则有 n= rdrnn|b+ωnb⨯rn (9.5.8) dt
根据向量的坐标系变换公式:
nb rn=Cbr
对上式两边求时间导数,得:
b nrb+Cnr nrb+Cndr|b=C nrb+dr|n (9.5.9) n=C =C rbbbbbbbdtdt
比较(9.5.8)和(9.5.9),得
nrb=ωn⨯rn=ωnkCnrb Cbnbnbb
n=ωnkCn (9.5.10) 所以 Cbnbb
式中
nk ωnb⎡0⎢n=⎢ωnbz
n⎢-ωnby⎣n-ωnbz0nωnbxn⎤ωnbyn⎥-ωnbx⎥ (9.5.11) 0⎥⎦
nnnn ωnb ωnby ωnbz]T =[ ωnbx
若分别对式(9.5.6)和(9.5.10)两边求转置,则得姿态矩阵微分方程的
另外两种形式:
n=Cnωbk (9.5.12) Cbbnb b=-Cbωnk (9.5.13) Cnnnb