捷联惯导结算原理

0 cos sin ， Rz sin 0 cos
sin cos 0
0 0 1
cos cos sin sin sin cos cos sin sin cos sin cos T11 T12 T13 Ry Rx Rz cos sin cos cos sin T21 T22 T23 sin cos cos sin sin sin sin cos sin cos cos cos T T T 31 32 33 b 由姿态矩阵 C n 反解飞行器姿态欧拉角：
0
b tbx

b tby
上述四元数微分方程的解和矩阵方程的解相类似，可用毕卡逼近法求解，当
M * b 和 M * b dt 相乘满足可交换律时，有
t1
Q q2 e 2
而
1
1
t1 M
t2
*
b dt
Q q1 e 2
1

Q q1
e2
2 q1q2 qo q3 2 2 2 qo q12 q2 q3 2 qo q1 q2 q3
2 qo q2 q1q3 2 qo q1 q2 q3 2 2 2 qo q12 q2 q3
b （3） 由姿态矩阵 C n 求解飞行器姿态信息
q3 qo q2 q1 q1 q2 qo q3
q1 qo q3 q2
q2 q3 qo q1
q3 0 q2 x q1 y qo z
2 2 2 qo q12 q2 q3 b Cn 2 q1q2 qo q3 2 qo q2 q1q3
b 解姿态矩阵 Cn 将 f b 转换成 f n ，再进行导航速度和位置解算。工程上普遍采用四
元数法求解姿态微分方程，求解出新的四元数，再构造出新的姿态矩阵，这样便 可迭代求解。
四 .源程序代码
load('wib_SINS.mat'); load('f_SINS.mat'); Vxt=-9.993908270;Vyt=0;Lo=116.344695283/180*pi;La=39.975172/180*pi; theta=2/180*pi;gamma=1/180*pi;psai=90/180*pi;wiet=zeros(3,1);wett=zeros(3,1);witt=zeros(3,1 ); V_xt=[];V_yt=[];Long=[];Lati=[]; theta_b=[];gamma_b=[];psai_b=[]; d_angle=zeros(3,1);matrix_angle=zeros(4); e=1/298.3; %（长半轴-短半轴）/长半轴=扁率 wie=7.292115147*10^(-5); Re=6378245; T=0.01; q0=cos(psai/2)*cos(theta/2)*cos(gamma/2)-sin(psai/2)*sin(theta/2)*sin(gamma/2); q1=cos(psai/2)*sin(theta/2)*cos(gamma/2)-sin(psai/2)*cos(theta/2)*sin(gamma/2); q2=sin(psai/2)*sin(theta/2)*cos(gamma/2)+cos(psai/2)*cos(theta/2)*sin(gamma/2); q3=sin(psai/2)*cos(theta/2)*cos(gamma/2)+cos(psai/2)*sin(theta/2)*sin(gamma/2); q=[q0;q1;q2;q3];Cnb=zeros(3);Cbn=zeros(3); q=q/sqrt(q'*q); for i=1:60001 V_xt=[V_xt,Vxt];V_yt=[V_yt,Vyt];Long=[Long,Lo];Lati=[Lati,La]; theta_b=[theta_b,theta];gamma_b=[gamma_b,gamma];psai_b=[psai_b,psai]; Cnb(1,1)=q(1)^2+q(2)^2-q(3)^2-q(4)^2;%%求解姿态矩阵 Cnb(1,2)=2*(q(1)*q(4)+q(2)*q(3)); Cnb(1,3)=2*(-q(1)*q(3)+q(2)*q(4)); Cnb(2,1)=2*(q(2)*q(3)-q(1)*q(4)); Cnb(2,2)=q(1)^2-q(2)^2+q(3)^2-q(4)^2; Cnb(2,3)=2*(q(1)*q(2)+q(3)*q(4)); Cnb(3,1)=2*(q(1)*q(3)+q(2)*q(4)); Cnb(3,2)=2*(-q(1)*q(2)+q(3)*q(4)); Cnb(3,3)=q(1)^2-q(2)^2-q(3)^2+q(4)^2; Cbn=Cnb'; %%%%%%%姿态解算程序 theta=asin(Cnb(2,3));
一 .捷联惯导结算原理
捷联惯导的惯性器件（IMU）直接安装在载体坐标系上，取消了一个复杂的 机电系统--物理平台，减轻了重量，体积，简化了结构。通过导航计算机实时计
b 算姿态矩阵 C n ，将载体系中测量得到的比力信息 f b 转换成导航系中的分量 f n
进行导航位置和速度解算。也就是说，捷联惯导系统，在导航计算机内构造出一 个 “数学平台” 代替平台惯导中真实的物理平台。 这样， 整个系统维修保障便捷， 故障率降低，但是惯性器件直接安装在载体上，所处环境恶劣，要求惯性器件具 有较高的精密度。本次作业中导航系仍为指北方位东-北-天当地水平坐标系。
二 .公式推导
（1） 由初始欧拉角求得初始四元数 题目中给出初始欧拉角如下：

90


2


1

按照 3-1-2 顺序旋转
q1 cos 2
0 0 sin ， q2 cos sin 0 0 ， q3 cos 0 sin 0 2 2 2 2 2
其中列表计算 和
T33
主
真 主 主
180o 主 180o 主
表格 1 滚转角计算图
范围
o o 0 , 90 o o 90 ,0 o o 180 , 90 o o 90 , 180
+ + — —
+ — + —
b （2） 由四元数生成姿态矩阵 C n
假设空间矢量在载体系 b 和导航系 t 中的投影分别为 r ' 和 r ，则存在
qo 0 q x' * r' Q rQ 1 q2 y ' z ' q3
所以
q1 qo q3 q2
q2 q3 qo q1
0 cos sin 0 2 sin cos sin cos 2 2 0 sin 2 0 0 cos 0 2 sin cos 0 0 0 0 cos sin


2

q q1 q2


2

2
2
2
0
0
2
简化得
cos 2 cos 2 cos 2 sin 2 sin 2 sin 2 q o cos sin cos sin cos sin 2 2 2 2 2 2 q1 q Q sin sin cos cos cos sin q2 2 2 2 2 2 2 q3 sin cos cos cos sin sin 2 2 2 2 2 2
（5） 速度的计算
t t t t t 0 2iez etz ety 2iey Vxt Vx 0 t t b t t t t 0 2iex etx Vyt 0 Vy Cb f 2iez etz t Vz g Vzt 2 t t 2 t t 0 iey ety iex etx
0 b 1 1 tbx b Q Q tb b 2 2 tby b tbz
t2
b tbx 0 b tbz b tby b tbz b qo tbz b tby q1 1 * M b Q b q2 2 tbx 0 q3
（6） 位置的计算
t t Vy dt Lo L 0 Ryt ，其中 t Vxt 0 R cos L dt o xt
1 1 2 R R 1 e sin L xt e 1 1 1 2e 3e sin 2 L R yt Re

2 cos 2 0 0 sin sin 2 2 0 cos 2 0
T
T
T
则
cos 2 0 q3 0 sin 2
T22
主
真 主 主
180o 主 180o 主
表格 2 偏航角计算图
范围
o o 0 , 90 o o 90 ,0 o o 180 , 90 o o 90 , 180
+ + — —
+ — + —
（4） 姿态速率微分方程
三 .程序流程图及仿真图像
以下给出程序解算流程图和仿真图像
o , o , o
求解四元 数初值
qo
求解姿态 矩阵
b Cn
四元数微分方程
q1
反解姿态欧拉角
fb
t t Lo , o ,Vxo ,Vyo
将比力转换到导航 系并进行速度和位 置计算
, ,
L, ,Vxt ,Vyt
图 1 程序流程图
o o sin 1 T23 , 90 , 90
tg 1
T13 180o , 180o , T33
tg 1
T21 o o , 180 , 180 T 22
图 6 东向北向速度变化曲线
阶段总结：1.学习了平台式和捷联式惯导的惯导解算方法并进行了仿真计算。 2.平台式惯导物理平台时刻跟踪当地水平东北天地理系， 加速计的比 力信息直接投影在导航系中，可直接进行导航速度和位置解算。载体的姿态可直 接从平台框架直接得出；而捷联式惯导用数学平台取代实体的物理平台，通过求
载体位置坐标曲线图 40.01
终点
40.005
40
39.995
L来自百度文库)
39.99 39.985 39.98
起点
39.975 116.3
116.305
116.31
116.315
116.32
116.325 ()
116.33
116.335
116.34
116.345
116.35
图 2 载体位置曲线
载体俯仰角变化曲线图 2 1.5 1 0.5 0
欧拉角法按照 3-1-2 旋转顺序得到当地水平地理系（东-北-天）到载体系的
b 姿态旋转矩阵 C n ，其中
cos Ry 0 sin
0 sin 0 1 1 0 ， Rx 0 cos 0 sin 0 cos

cos
o sin o , 2 2 2 I o x y z 2 o 2
2 4 2 o o 1 o I Q t1 Q t2 1 8 384 48 2
()
-0.5 -1 -1.5 -2 -2.5 0
100
200
300 时间 (s)
400
500
600
图 3 载体俯仰角变化曲线
载体滚转角变化曲线图 1
0.5
0
()
-0.5
-1
-1.5 0
100
200
300 时间 (s)
400
500
600
图 4 载体滚转角变化曲线
载体偏航角变化曲线图 90 80 70 60 50
()
40 30 20 10 0 -10 0 100 200 300 时间 (s) 400 500 600
图 5 载体偏航角变化曲线
载体东向和北向速度曲线图 10 8 6 4 2 东向速度 北向速度
速度(m/s)
0 -2 -4 -6 -8 -10 0 100 200 300 时间 (s) 400 500 600