捷联式惯性导航系统原理

1、方向余弦表cos cos sin sin sin sin cos cos sin sin cos sin sin cos cos cos sin cos sin sin sin cos sin sin cos sin cos cos cos C ψϕψθϕψϕψθϕθϕψθψθθψϕψθϕψϕψθϕθϕ-+-⎡⎤⎢⎥=-⎢⎥⎢⎥+-⎣⎦（1.0.1）X E Y C N Z ζ⎡⎤⎡⎤⎢⎥⎢⎥=⎢⎥⎢⎥⎢⎥⎢⎥⎣⎦⎣⎦（1.0.2） 在列写惯导方程需要采用方向余弦表，因为错误！未找到引用源。

α较小，经常采用两个假设，即：cos 1sin 1αα≈≈ （1.0.3）式中 α-两坐标系间每次相对转动的角度。

由于在工程实践中可以使其保持很小，所以进一步可以忽略如下形式二阶小量，即：sin sin 0αβ≈ （1.0.4）式中β-两坐标系间每次相对转动的角度。

可以将C 近似写为：111C ψϕψθϕθ-⎡⎤⎢⎥=-⎢⎥⎢⎥-⎣⎦（1.0.5） 2、用四元素表示坐标变换对于四元素123q p i p j p k λ=+++，可以表示为如下形式cossincos sincos sincos 2222q i j k θθθθαβγ=+++ （2.0.1）式（2.0.1）的四元数称为特殊四元数，它的范数1q =。

1'R q Rq -= （2.0.2）式中''''R xi yj zk R x i y j z k=++=++ （2.0.3）将q 和1q -的表达式及式（2.0.3）带入（2.0.2），然后用矩阵表示为：()()()()()()()()()22221231231322222123213231222213223131222''22'22p p p p p p p p p x x y p p pp p p p p p yz z p p p p p p p p p λλλλλλλλλ⎡⎤+--+-⎡⎤⎡⎤⎢⎥⎢⎥⎢⎥⎢⎥=-+--+⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎣⎦⎣⎦⎢⎥+-+--⎣⎦（2.0.4）由四元素到方向余弦表的建立123cos cos22sin cos22sin sin22cos sin22p p p θψϕλθψϕθψϕθψϕ-=-=-=+= （2.0.5） 将式（2.0.5）带入式（2.0.4），有cos cos sin cos sin cos sin sin cos cos sin sin sin cos cos cos sin sin sin cos cos cos cos sin sin sin sin cos cos C ϕψϕθψϕψϕθψϕθϕψϕθψϕψϕθψϕθθψθψθ-+⎡⎤⎢⎥=---+⎢⎥⎢⎥-⎣⎦（2.0.6）3、四元数转动公式的进一步说明采用方向余弦矩阵描述飞行器姿态运动时，需要积分姿态矩阵微分方程式，即C C =Ω （3.0.1）式中 C -动坐标系相对参考坐标系的方向余弦阵Ω-动坐标系相对参考坐标系角速度ω的反对称矩阵表达式 其中C 为公式（1.0.5）提供000z y zx y xωωωωωω⎡⎤-⎢⎥Ω=-⎢⎥⎢⎥-⎣⎦（3.0.2）采用（3.0.1）计算需要列写9个一阶微分方程式，计算量大。

在采用四元数法时，要求解四元数姿态微分方程式12q q ω=（3.0.3） 其中0x y z i j j ωωωω=+++，可以求解b q q =Ω得1122330222022202220222yxz y x zy x zyxz p p p p p p ωωωλλωωωωωωωωω⎡⎤---⎢⎥⎢⎥⎡⎤⎡⎤⎢⎥⎢⎥-⎢⎥⎢⎥⎢⎥⎢⎥=⎢⎥⎢⎥⎢⎥⎢⎥-⎢⎥⎢⎥⎢⎥⎢⎥⎣⎦⎣⎦⎢⎥⎢⎥-⎢⎥⎣⎦（3.0.4） 可见四元数姿态矩阵微分方程式只要解4个一阶微分方程式就可以了，比方向余弦姿态微分方程式计算量有明显的减少。

第七章捷联式惯性导航系统基本算法和系统误差传播特性1、捷联式惯导算法概述 1.1系统初始化系统初始化包括3项任务1）给定飞行器的初始位置和初始速度等初始信息。

2）数学平台的初始校准，确定姿态矩阵的初始值，是在计算机中用对准程序来完成的。

在物理概念上就是把“数学平台”坐标系和导航坐标系相重合，称其为对准。

3）惯性仪表的校准，对陀螺的标度因数进行标定，对陀螺的漂移进行标定，对加速度计的标度因数进行标定。

1.2惯性仪表的误差补偿1.3姿态矩阵计算 1.4导航计算1.5导航和控制信息的提取2、姿态矩阵的计算 2.1欧拉角方程航向角ψ，俯仰角θ，滚动角γ表示飞行器坐标系和地理坐标系之间关系。

从地理坐标系到飞行器坐标系的三次旋转顺序为γθψ→→，其方向余弦矩阵表达式为：cos cos sin cos sin cos sin sin sin cos sin sin sin cos cos cos sin cos sin cos sin sin sin sin cos cos sin cos cos b E C C C C γθψψθψθθψθγψγψθγψγθγψθγψγψθγψγθγ-⎡⎤⎢⎥==-+⎢⎥⎢⎥+-⎣⎦00000010sin 0cos sin cos 0sin cos cos Tb bb bEb EbX EbY EbZ C C C γθγγωωωωθψθγγγθθγγθψ⎡⎤⎡⎤⎡⎤⎢⎥⎢⎥⎢⎥⎡⎤==++=⎣⎦⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎣⎦⎣⎦⎣⎦-⎡⎤⎡⎤⎢⎥⎢⎥⋅⎢⎥⎢⎥⎢⎥⎢⎥-⎣⎦⎣⎦110sin 0cos sin cos 0sin cos cos cos sin cos cos sin 10cos cos sin cos cos 0sin cos b EbX b EbY b EbZ bEbX b EbY b EbZ γθωθγγθωψγγθωθγθγθωθγγθωθγγω-⎡⎤-⎡⎤⎡⎤⎢⎥⎢⎥⎢⎥==⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥-⎣⎦⎣⎦⎣⎦⎡⎤⎡⎤⎢⎥⎢⎥-⋅⎢⎥⎢⎥⎢⎥⎢⎥⎣⎦⎣⎦式为欧拉角微分方程式，式中的bEbX ω、bEbY ω、bEbZ ω三个角速度分量可由直接安装在飞行器上的三个角速度陀螺测量值与导航参数计算值得到，可认为是已知量。

但是在使用时应该注意，由于在方程式中存在三角函数，结实时计算带来困难，且当θ等于90︒时，方程出现奇点，这种现象等效于三环式平台的闭锁现象，因此，用欧拉角微分方程确定姿态的方法不能用于全姿态飞行器上。

2.3四元数微分方程及其解 前面已知b q q =Ω式中 b Ω-飞行器坐标系相对地理坐标系的旋转角速度的斜对称矩阵。

将上式展开为1122330222022202220222yxz y x zy x zyxz p p p p p p ωωωλλωωωωωωωωω⎡⎤---⎢⎥⎢⎥⎡⎤⎡⎤⎢⎥⎢⎥-⎢⎥⎢⎥⎢⎥⎢⎥=⎢⎥⎢⎥⎢⎥⎢⎥-⎢⎥⎢⎥⎢⎥⎢⎥⎣⎦⎣⎦⎢⎥⎢⎥-⎢⎥⎣⎦采用和矩阵微分方程式求解相似的方法，有()[][]sin 2cos 02o o o q t I q θθθθ∆⎧⎫⎪⎪∆=+∆⎨⎬∆⎪⎪⎩⎭且有[]210000x y z t xz y o b t y z x zyxdt θθθθθθθθθθθθθ-∆-∆-∆⎡⎤⎢⎥∆∆-∆⎢⎥∆=Ω=⎢⎥∆-∆∆⎢⎥∆∆-∆⎢⎥⎣⎦⎰2.4姿态角和航向角的计算 有前面已知下面公式：cos cos sin cos sin cos sin sin sin cos sin sin sin cos cos cos sin cos sin cos sin sin sin sin cos cos sin cos cos b E C C C C γθψψθψθθψθγψγψθγψγθγψθγψγψθγψγθγ-⎡⎤⎢⎥==-+⎢⎥⎢⎥+-⎣⎦可表示为111213212223313233bE T T T C T T T T T T ⎡⎤⎢⎥=⎢⎥⎢⎥⎣⎦由上面两式可知()1131233311211sin tan tan T T T T T θγψ---=-⎛⎫=⎪⎝⎭⎛⎫=⎪⎝⎭将其带入第一章四元数公式，可求得()()()222211123121232323122223331213132222T p p p T p p p T p p p T p p p T p p p λλλλλ=+--=+=+=+--=-再带入求角度公式，求得相应的欧拉角度。

同样，利用表则可以判断函数的真值。

3、姿态矩阵的实时计算 3.1增量算法3.1.1矩阵微分方程计算()()1111z y E E b b zx y xC n C n θθθθθθ⎡⎤-∆∆⎢⎥+=∆-∆⎢⎥⎢⎥-∆∆⎣⎦由前面可得112131122232132333E b T T T C T T T T T T ⎡⎤⎢⎥=⎢⎥⎢⎥⎣⎦带入可得()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()11112131212131113131112112122232222232123232122213132333232311111111z y x z y x z y x z y x z y T n T n T n T n T n T n T n T n T n T n T n T n T n T n T n T n T n T n T n T n T n T n T n T n T n T n T n T n T n T n θθθθθθθθθθθθθθ+=+∆-∆+=+∆-∆+=+∆-∆+=+∆-∆+=+∆-∆+=+∆-∆+=+∆-∆+=()()()()()()3313333313231x z y xT n T n T n T n T n T n θθθθ+∆-∆+=+∆-∆ 3.1.2四元数微分方程的计算 一阶算法为()()12221222112221222y xz y x z y x z y x z q n q n θθθθθθθθθθθθ∆⎡⎤∆∆---⎢⎥⎢⎥∆∆∆⎢⎥-⎢⎥+=⎢⎥∆∆∆⎢⎥-⎢⎥⎢⎥∆∆∆⎢⎥-⎢⎥⎣⎦ 将上式展开，得()()()()()()()()()()()()()()()()()()()()1231123221333121111222111122211112221111222x y z x z y y z x z y x n n p n p n p n p n p n n p n p n p n p n n p n p n p n p n n p n p n λλθθθθλθθθλθθθλθθ+=-∆-∆-∆+=+∆+∆-∆+=+∆-∆+∆+=+∆+∆-∆3.2数值积分算法 矩阵微分方程式()()()C t C t t =Ω其一阶龙格-库塔法的解的形式为()()()()C t T C t TC t t +=+Ω对于四元数微分方程b q q =Ω其一阶龙格-库塔法的解的形式为()()()()b q t T q t T t q t +=+Ω展开成元素的表达式，有系统误差方程。