捷联式姿态解算过程

（1）加速度记测得载体相对惯性空间比力b ib a ，经过误差补偿之后的b

ib a 经过姿态矩阵n b C 的变换得到n ib a ：n n b ib b ib =C a a

将n ib a 进行误差补偿之后通过积分运算得到速度分量n en V 。n en V 一方面作

为系统的输出，一方面作为输入用来求解位置角速率n en W 。 （2）陀螺仪测得载体相对惯性空间角速率b i b W ， 首先通过速度分量n en V 求得位移角速率n en W ，因为n eny W =0，

n

enx n eny W W ⎡⎤⎢⎥⎢⎥⎣⎦=22yt xt 22y yt xt yt t x x t t sin cos ()R R sin cos R 11sin cos R R 11sin cos R R R αααααααα⎡⎤

-⎢⎥⎢⎥⎢⎥⎢⎥

-⎢⎥⎣-++⎦

-（）（）n x n y V V ⎡⎤

⎢⎥⎢⎥⎣⎦

这样就根据陀螺仪测得的角速率b i b W ，上面所求得的位移角速率n

en W ，

加上已知的地球角速率e i e W 来求的姿态角速率b n b W 。 b b n b n e i e n i e n e i e W =C W =C C W 且 b b n e n n e n W =C W b b b b n b i b i e e n W =W W W --=b i b W b n e n n e i e e n C C W +W -（）

。 然后根据姿态角速率利用四元数微分方程求出四元数中的元素a,b,c,d.

四元数描述了一个坐标系或一个矢量相对某一个坐标系的旋转。a 是标量部分表示了转角的一半余弦值，b ，c ，d 是矢量部分表示瞬时转轴的方向，瞬时转动轴与参考坐标系轴间方向的余弦值。 表达式A=a+bi+cj+dk 。

四元数微分方程：.

b

n b A =1/2AW

..x

..001/200b

b b

nbx nby

nbz

b b b nb nbz

nby

b b b nby

nbz

nbx

b b b nbz nby

nbz

a w w w a w w w

b b w w w

c c w w w

d d ⎡⎤⎢⎥⎡⎤---⎡⎤⎢⎥⎢⎥⎢⎥-⎢⎥⎢⎥⎢⎥=⎢⎥⎢⎥⎢⎥-⎢⎥⎢⎥⎢⎥-⎢⎥⎢⎥⎣⎦

⎣⎦⎢⎥⎣⎦

这样求出四元数中a ，b ，c ，d 四个元数。

这里的四元数是用来描述载体坐标系相对游动方位坐标系的转动，即：

2222

b n 2222

n b 2222n b x x a b c d 2bc ad 2bd+ac y =2bc+ad a b c d 2cd-ab y 2bd ab 2cd+ab a -b -c d z z ⎡⎤+---⎡⎤

⎡⎤⎢⎥⎢⎥⎢⎥-+-⎢⎥⎢⎥⎢⎥⎢⎥⎢

⎥⎢⎥-+⎣⎦⎣⎦⎣⎦（）

（）（

）（）（

）（）

这样就可方向余弦矩阵与四元数姿态矩阵是完全等效的，即

2222

n 2222

b 2222a b

c

d 2bc ad 2bd+ac C =2bc+ad a b c d 2cd-ab 2bd ab 2cd+ab a -b -c d ⎡⎤

+---⎢⎥

-+-⎢

⎥⎢⎥-+⎣⎦

（）（）（）（）（

）

（）

而n b cos sin sin cos sin cos cos sin sin cos sin s sin sin sin C =sin sin in cos cos sin sin cos sin cos co sin s sin cos cos γψγθψ

θψγψγθψγψγθψθψγψγθψγθθ

γθ-⎡⎤

⎢⎥⎢⎥⎢⎥-⎣

+⎦

-+-

这样利用姿

态矩阵和四元数中各对应项相等的原则得出姿态矩阵中的各个元素，然后就能求出姿态参数，即三个姿态角航向角ψ、俯仰角θ、滚转角γ。