捷联惯性导航系统的解算方法
捷联惯导初始对准以及姿态解算
第三部分：基于“存储数据与迭代计算对准”罗经法对准
3.2 罗经法对准过程中的调整策略（以北向通道为例）
g
y
f
p N
1
VN
1
s
R
-
K1
K2 R
K3 s
Control algorithm
cE -
x
1
s
x
-
ie cos L
z
实线所示的北向通道：本质上是一个休拉回路，失准角作无阻尼振荡。
采取的策略：1）引入内反馈环节（虚线）实现衰减振荡；2）引入前馈环节（点画 线）缩短振荡周期；3）引入积分环节（双点画线）消除罗经项的影响。
3.5 SINS罗经法对准如何实现迭代计算？
fˆNn -
b ib
fb cU
Cˆbn
Cˆbn
b ib
Cnbine
Cnbc
cN
fˆ n Cˆbn f b
1
VN
s
1
cE
R
K1
K2 R
K3 s
Control algorithm
上述过程中，可以实现迭代计算。
Page 15
第三部分：基于“存储数据与迭代计算对准”罗经法对准
导航坐标系 n （b）SINS
GINS中的测量数据直接反映失准角的大小； SINS中的测量数据不直接反映失准角；只有投影数据能够反映失准角的大小；相同 的测量数据经过不同的姿态矩阵进行投影，可以获取不同的投影数据。 注：上述均不考虑仪表误差。
对于SINS而言，分析一种理想的情况：仪表无误差，载体无机动，此时在整个对准 过程中，仪表测量数据均相等。整个对准过程，其实只用了一组仪表参数。
3.6 SINS罗经法对准中存储数据如何使用？
捷联惯性导航系统的解算方法
捷联惯性导航系统的解算方法捷联惯性导航系统（Inertial Navigation System，简称INS）是一种利用陀螺仪和加速度计等惯性测量单元测量物体的加速度和角速度，然后通过对这些测量值的积分计算出物体的速度和位置的导航系统。
INS广泛应用于航空航天、无人驾驶车辆和船舶等领域，具有高精度和自主性等特点。
INS的解算方法一般分为初始对准、运动状态估计和航位推算三个主要过程。
初始对准是指在启动导航系统时，通过利用外部辅助传感器（如GPS）或静态校准等方法将惯性传感器的输出与真实姿态和位置进行初次校准。
在初始对准过程中，需要获取传感器的初始偏差和初始姿态，一般采用标定或矩阵运算等方法进行。
运动状态估计是指根据惯性传感器的测量值，使用滤波算法对物体的加速度和角速度进行实时估计。
常用的滤波算法包括卡尔曼滤波、扩展卡尔曼滤波和粒子滤波等。
其中，卡尔曼滤波是一种最优估计算法，通过对观测值和状态进行线性组合，得到对真实状态的最佳估计。
扩展卡尔曼滤波则是基于卡尔曼滤波的非线性扩展，可以应用于非线性INS系统。
粒子滤波是一种利用蒙特卡洛采样技术进行状态估计的方法，适用于非高斯分布的状态估计问题。
航位推算是指根据运动状态估计的结果，对物体的速度和位置进行推算。
INS最基本的航位推算方法是利用加速度值对速度进行积分，然后再对速度进行积分得到位置。
但是，在实际应用中，由于传感器本身存在噪声和漂移等误差，导致航位推算过程会出现积分漂移现象。
为了解决这个问题，通常采用辅助传感器（如GPS）和地图等数据对INS的输出进行校正和修正。
当前，还有一些先进的INS解算方法被提出，如基于深度学习的INS 解算方法。
这些方法利用神经网络等深度学习模型，结合原始传感器数据进行端到端的学习和预测，以实现更高精度的位置和姿态估计。
综上所述，捷联惯性导航系统的解算方法主要包括初始对准、运动状态估计和航位推算三个过程。
其中，运动状态估计过程利用滤波算法对传感器的测量值进行处理，得到物体的加速度和角速度的估计。
捷联惯性导航原理
2.捷联惯导力学编排方程
姿态角定义： ψ航向角----载体纵轴在水平面的投影与地理子午线之间 的夹角，用ψ表示，规定以地理北向为起点，偏东方向 为正，定义域0~360°。 θ俯仰角----载体纵轴与纵向水平轴之间的夹角，用θ表 示，规定以纵向水轴为起点，向上为正，向下为负，定 义域-90 ° ~+90 ° 。 γ横滚角----载体纵向对称面与纵向铅垂面之间的夹角， 用γ表示，规定从铅垂面算起，右倾为正，左倾为负， 定义域-180 ° ~+180° 。（载体纵向对称面和 纵轴空 间 铅垂面）
捷联惯性导航原理
2010.11.30 北航通信导航与自动测试实验室
如果载体真实地理位置以纬度、经度、高度 表示，则与此对应的载体在地球坐标系中的 真实位置(x，y，z)可通过下式求得:
地球各点重力加速度近似计算公式: g=g0(1-0.00265cos&)/1+(2h/R) g0:地球标准重力加速度9.80665(m/平方秒) &:测量点的地球纬度 h:测量点的海拔高度 R: 地球的平均半径(R=6370km) s:时间 ？？？？？？？？？？？？？？？？？？？？
f 为地球椭球模型的椭圆度，f= 1/298.257223563
R1 RN h R2 RM h
注意从瞬时速度过来那条线，用来计算w（enn）
3、捷联惯导系统的算法
3.1 姿态更新算法 四元数法：
Q(q0 , q1 , q2 , q3 ) q0 q1i q2 j q3k
1. 惯性导航中的常用坐标系
yt
yb
z e zi
北
xb
zb
zt
xt
O
东
Oe
捷联惯导系统算法.ppt
cos
b Ebz
注意事项：当 θ= 90 度时，方程出现奇点
姿态计算 矩阵方程精确解1
二、方向余弦矩阵微分方程及其解 C C
其中
C bE
CbE
b Eb
0
b Eb
z
z
0
y
x
y x
0
由于陀螺仪直接测得的是载体 相对惯性空间的角速度，所以：
CbE
b ib
E iE
C
E b
或四元数微分方程：
q(t)
(
b ib
b iE
)q(t)
注意事项： 1、上述两个方程中的角速度表达式不一样 2、方程第二项较小，计算时速度可以低一些
增量算法 矩阵方程精确解
一、角增量算法
角增量：陀螺仪数字脉冲输出，每个脉冲代表一个角增量
一个采样周期内，陀螺输出脉冲数对应的角增量为：
C
0
0
c os
0 0 0 sin
sin
sin
c os
cos cos
求解欧拉角速率得
1 0
0
cos
0 sin
惯性器件的误差补偿
姿态计算 欧拉角微分方程1
姿态矩阵的计算 假设数学坐标系模拟地理坐标系 飞行器姿态的描述：
航向角ψ、俯仰角θ、滚动角γ 一、欧拉微分方程
从地理坐标系到载体坐标系 的旋转顺序：
Ψ →θ →γ
捷联惯性导航系统的解算方法课件
02
CATALOGUE
捷联惯性导航系统组成及工作 原理
主要组成部分介绍
惯性测量单元
包括加速度计和陀螺仪，用于测量载体在三个正交轴上的加速度 和角速度。
导航计算机
用于处理惯性测量单元的测量数据，解算出载体的姿态、速度和 位置信息。
控制与显示单元
用于实现人机交互，包括设置导航参数、显示导航信息等。
工作原理简述
学生自我评价报告
知识掌握情况
学生对捷联惯性导航系统的基本原理、解算 方法和实现技术有了深入的理解和掌握。
实践能力提升
通过实验和仿真，学生的动手实践能力得到了提升 ，能够独立完成相关的实验和仿真验证。
团队协作能力
在课程项目中，学生之间的团队协作能力得 到了锻炼和提升，能够相互协作完成项目任 务。
对未来发展趋势的预测和建议
捷联惯性导航系统的解算 方法课件
CATALOGUE
目 录
• 捷联惯性导航系统概述 • 捷联惯性导航系统组成及工作原理 • 捷联惯性导航系统解算方法 • 误差分析及补偿策略 • 实验验证与结果展示 • 总结与展望
01
CATALOGUE
捷联惯性导航系统概述
定义与基本原理
定义
捷联惯性导航系统是一种基于惯性测量元件（加速度计和陀螺仪）来测量载体（如飞机、导弹等）的加速度和角 速度，并通过积分运算得到载体位置、速度和姿态信息的自主导航系统。
01
高精度、高可靠性
02
多传感器融合技术
随着科技的发展和应用需求的提高， 捷联惯性导航系统需要进一步提高精 度和可靠性，以满足更高层次的应用 需求。
为了克服单一传感器的局限性，可以 采用多传感器融合技术，将捷联惯性 导航系统与其他传感器进行融合，提 高导航系统的性能和鲁棒性。
基于四元数法的捷联式惯性导航系统的姿态解算
0
sin C
cos C
b X EbZ
( 1) 根据欧拉角微分方程, 由角速度可 以求解 3 个姿态角。欧拉角微分方程式只有 3 个, 但每个 方程 Û x = f ( co s x , sin x ) X 都含有三角函数的运 算, 计算速度慢 , 且方程会出现/ 奇点0 , 方程式退 化, 故不能全姿态工作。 2. 2 方向余弦矩阵微分方程式 当一个坐标系相对另一个坐标系做一次或多 次旋转后可得到另外一个新的坐标系 , 前者往往 被称为参考坐标系或固定坐标系, 后者被称为动 坐标系, 他们之间的相互关系可用方向余弦表来 表示。方向余弦矩阵微分方程式可写为载体坐标 系相对导航坐标系旋转角速度的斜对称矩阵表达 式, 方向余弦表是对这两种坐标系相对转动的一 种数学描述。
E b C ÛE b = Cb 8 Eb , b Eb
2
姿态矩阵的计算方法
由于载体的姿态方位角速率较大 , 所以针对
姿态矩阵的实时计算提出了更高的要求。通常假 定捷联系统/ 数学平台0 模拟地理坐标系 , 即导航 坐标系; 而确定载体的姿态矩阵即为研究载体坐 标系 ( b) 和导航坐标系 ( E ) 的空间转动关系 , 一般
第 16 卷
第 10 期
光学 精密工程
O pt ics and Precision Eng ineer ing
2008 年 10 月 文章编号 1004 -924X( 2008) 10 - 1963 - 08
V ol. 16 N o. 10 Oct. 2008
பைடு நூலகம்
捷联惯导的解算程序
%====本程序为捷联惯导的解算程序(由惯性器件的输出解算出飞行器的位置、速度、姿态信息)======clear all;close all;clc;deg_rad=pi/180; %由度转化成弧度rad_deg=180/pi; %由弧度转化成度%-------------------------------从源文件中读入数据----------------------------------fid_read=fopen('IMUout.txt','r'); %path1_Den.dat 是由轨迹发生器产生的数据[AllDataNumofAllData]=fscanf(fid_read,'%g %g %g %g %g %g %g %g %g %g %g %g %g %g %g %g',[17 inf]);AllData=AllData';NumofEachData=round(NumofAllData/17);Time=AllData(:,1);longitude=AllData(:,2); %经度单位：弧度latitude=AllData(:,3); %纬度单位：弧度High=AllData(:,4); %高度单位：米Ve=-AllData(:,6); % 东向、北向、天向速度单位：米/妙Vn=AllData(:,5);Vu=AllData(:,7);fb_x=AllData(:,9); %比力（fx，fy，fz）fb_y=AllData(:,8); %指向右机翼方向为x正方向，指向机头方向为y正向，z轴与x轴和y轴构成右手坐标系单位：米/秒2fb_z=-AllData(:,10); %右前上pitch=AllData(:,11); %俯仰角（向上为正）单位:弧度head=-AllData(:,13); %偏航角（偏西为正）roll=AllData(:,12); %滚转角（向右为正）omigax=AllData(:,15); %陀螺输出（单位：弧度/秒，坐标轴的定义与比力的相同）omigay=AllData(:,14);omigaz=-AllData(:,16);%-------------------------------程序初始化--------------------------------------latitude0=latitude(1);longitude0=longitude(1); %初始位置High0=High(1);Ve0=Ve(1);Vn0=Vn(1); %初始速度Vu0=Vu(1);pitch0=pitch(1);head0=head(1); %初始姿态roll0=roll(1);TimeEach=0.005; %周期和仿真总时间TimeAll=(NumofEachData-1)*TimeEach;Omega_ie=0.7292115147E-4;%0.00007272205216643040; %地球自转角速度单位：弧度每妙g0=9.78;%------------------------------导航解算开始--------------------------------------%假设没有初始对准误差pitch_err0=pitch0+0*deg_rad;head_err0=head0+0*deg_rad;roll_err0=roll0+0*deg_rad;%初始捷联矩阵的计算《捷联惯导系统》P63 旋转顺序 head - pitch - roll %导航坐标系n为东北天方向载体坐标系b为右前上偏航角北偏西为正Tbn(1,1)=cos(roll_err0)*cos(head_err0)-sin(roll_err0)*sin(pitch_err0) *sin(head_err0);Tbn(1,2)=cos(roll_err0)*sin(head_err0)+sin(roll_err0)*sin(pitch_err0) *cos(head_err0);Tbn(1,3)=-sin(roll_err0)*cos(pitch_err0);Tbn(2,1)=-cos(pitch_err0)*sin(head_err0);Tbn(2,2)=cos(pitch_err0)*cos(head_err0);Tbn(2,3)=sin(pitch_err0);Tbn(3,1)=sin(roll_err0)*cos(head_err0)+cos(roll_err0)*sin(pitch_err0) *sin(head_err0);Tbn(3,2)=sin(roll_err0)*sin(head_err0)-cos(roll_err0)*sin(pitch_err0) *cos(head_err0);Tbn(3,3)=cos(roll_err0)*cos(pitch_err0);Tnb=Tbn';%位置矩阵的初始化《捷联惯导系统》P46 其中游动方位角 a＝0 假使初始经纬度确知Cne(1,1) = - sin(longitude0);Cne(1,2) = cos(longitude0);Cne(1,3) = 0;Cne(2,1) = - sin(latitude0) * cos(longitude0);Cne(2,2) = - sin(latitude0) * sin(longitude0);Cne(2,3) = cos(latitude0);Cne(3,1) = cos(latitude0) * cos(longitude0);Cne(3,2) = cos(latitude0) * sin(longitude0);Cne(3,3) = sin(latitude0);Cen=Cne';%初始四元数的确定《捷联惯导系统》 P151-152 方法本身保证了q1^2+q2^2+q3^2+q4^2=1q(2,1) = sqrt(abs(1.0 + Tnb(1,1) - Tnb(2,2) - Tnb(3,3))) / 2.0; q(3,1) = sqrt(abs(1.0 - Tnb(1,1) + Tnb(2,2) - Tnb(3,3))) / 2.0; q(4,1) = sqrt(abs(1.0 - Tnb(1,1) - Tnb(2,2) + Tnb(3,3))) / 2.0;q(1,1) = sqrt(abs(1.0 - q(2,1) ^2 - q(3,1) ^2 - q(4,1) ^2));% 判断q(1,1)的符号flag_q11=cos(head_err0/2.0)*cos(pitch_err0/2.0)*cos(roll_err0/2.0)-sin(head_err0/2.0)*sin(pitch_err0/2.0)*sin(roll_err0/2.0);if (flag_q11 >0) %此时q(1,1)取正if (Tnb(3,2) < Tnb(2,3))q(2,1) = - q(2,1);endif (Tnb(1,3) < Tnb(3,1))q(3,1) = - q(3,1);endif (Tnb(2,1) < Tnb(1,2))q(4,1) = - q(4,1);endelse %此时q(1,1)取负或0q(1,1) = - q(1,1);if (Tnb(3,2) > Tnb(2,3))q(2,1) = - q(2,1);endif (Tnb(1,3) > Tnb(3,1))q(3,1) = - q(3,1);endif (Tnb(2,1) > Tnb(1,2))q(4,1) = - q(4,1);endend%-------------------------迭代推算用到的参数的初始化------------------------Wiee_e = 0;Wiee_n = 0;Wiee_u = Omega_ie;Wiee = [Wiee_e Wiee_n Wiee_u]'; %地球速率在地球系中的投影东－北－天Lat_err(1)=latitude0;Lon_err(1)=longitude0;High_err(1)=High0;Ve_err(1)=Ve0;Vn_err(1)=Vn0;Vu_err(1)=Vu0;pitch_err(1)=pitch_err0;head_err(1)=head_err0;roll_err(1)=roll_err0;Re=6378137.0;%6378245.0; %地球长轴《惯性导航系统》 P28e=0.0033528106647474807198455286185206; %地球扁率精确值ee=0.00669437999014131699614;%----------------------------迭代推算开始-----------------------------------for i=1:NumofEachData%----------------------------惯性仪表数据的获得------------------------Wibb(1,1)=omigax(i); %指向右机翼方向为x正方向，指向机头方向为y正向，z轴与x轴和y轴构成右手坐标系Wibb(2,1)=omigay(i); %单位：弧度/妙Wibb(3,1)=omigaz(i); %右前上fb(1,1)=fb_x(i); %指向右机翼方向为x正方向，指向机头方向为y正向，z轴与x轴和y轴构成右手坐标系fb(2,1)=fb_y(i); %单位：米/秒2fb(3,1)=fb_z(i); %右前上%--------计算在姿态矩阵和位置矩阵更新时用到的参数------------------ RM=Re*(1.0-2.0*e+3.0*e*Cne(3,3)^2)+High_err(i); %《捷联惯导系统》P233 P235RN=Re*(1.0+e*Cne(3,3)^2)+High_err(i);% RN=Re*(1-ee)/(sqrt(1-ee*sin(Lat_err(i))))^3+High_err(i);% RM=Re/sqrt(1-ee*sin(Lat_err(i)))+High_err(i);%实验当地重力加速度计算《捷联惯导系统》P150 《惯性导航系统》 P35g=g0*((1.0+0.0052884*Cne(3,3)^2)-0.0000059*(1-(1-2*Cne(3,3)^2)^2))*(1 .0-2.0*High_err(i)/Re);tmp_slat=sin(Lat_err(i))*sin(Lat_err(i));Wien = Cne * Wiee; %地球速率在导航系中的投影Wenn(1,1) = -Vn_err(i)/RM;Wenn(2,1) = Ve_err(i)/RN; % <<惯性导航系统>> P45 考虑了地球转动的影响.Wenn(3,1) = Ve_err(i)*tan(Lat_err(i))/RN; %计算Wenn(不太精确)，更新速度和位置矩阵时用Winn=Wien+Wenn;Winb=Tbn*Winn;Wnbb=Wibb-Winb; %姿态速率在姿态更新时用到fn=Tnb*fb; % x-y-z 东－北－天% 速度的更新《捷联惯导系统》 P30 33 东－北－天difVe_err=fn(1,1)+(2*Wien(3,1)+Wenn(3,1))*Vn_err(i)-(2*Wien(2,1)+Wenn (2,1))*Vu_err(i);difVn_err=fn(2,1)-(2*Wien(3,1)+Wenn(3,1))*Ve_err(i)+(2*Wien(1,1)+Wenn (1,1))*Vu_err(i);difVu_err=fn(3,1)+(2*Wien(2,1)+Wenn(2,1))*Ve_err(i)-(2*Wien(1,1)+Wenn (1,1))*Vn_err(i)-g;Ve_err(i+1)=Ve_err(i)+difVe_err*TimeEach;Vn_err(i+1)=Vn_err(i)+difVn_err*TimeEach;Vu_err(i+1)=Vu_err(i)+difVu_err*TimeEach;High_err(i+1)=High_err(i)+Vu_err(i)*TimeEach;% 位置矩阵的实时更新《惯性导航系统》 P190Cne(1,1)=Cne(1,1)+TimeEach*(Wenn(3,1)*Cne(2,1)-Wenn(2,1)*Cne(3,1)); Cne(1,2)=Cne(1,2)+TimeEach*(Wenn(3,1)*Cne(2,2)-Wenn(2,1)*Cne(3,2)); Cne(1,3)=Cne(1,3)+TimeEach*(Wenn(3,1)*Cne(2,3)-Wenn(2,1)*Cne(3,3)); Cne(2,1)=Cne(2,1)+TimeEach*(-Wenn(3,1)*Cne(1,1)+Wenn(1,1)*Cne(3,1)); Cne(2,2)=Cne(2,2)+TimeEach*(-Wenn(3,1)*Cne(1,2)+Wenn(1,1)*Cne(3,2)); Cne(2,3)=Cne(2,3)+TimeEach*(-Wenn(3,1)*Cne(1,3)+Wenn(1,1)*Cne(3,3)); Cne(3,1)=Cne(3,1)+TimeEach*(Wenn(2,1)*Cne(1,1)-Wenn(1,1)*Cne(2,1)); Cne(3,2)=Cne(3,2)+TimeEach*(Wenn(2,1)*Cne(1,2)-Wenn(1,1)*Cne(2,2)); Cne(3,3)=Cne(3,3)+TimeEach*(Wenn(2,1)*Cne(1,3)-Wenn(1,1)*Cne(2,3));% Mat_Wenn(1,1)=0;% Mat_Wenn(1,2)=Wenn(3,1);% Mat_Wenn(1,3)=-Wenn(2,1); %Wenn的反对阵矩阵取负% Mat_Wenn(2,1)=-Wenn(3,1); %这里位置矩阵的及时修正为:dCne/dt=Mat_Wenn*Cne% Mat_Wenn(2,2)=0;% Mat_Wenn(2,3)=Wenn(1,1);% Mat_Wenn(3,1)=Wenn(2,1);% Mat_Wenn(3,2)=-Wenn(1,1);% Mat_Wenn(3,3)=0;%% Mat_Wenn=Mat_Wenn*Cne*TimeEach;% Cne=Cne+Mat_Wenn;Cen=Cne';% 计算经纬度Lat_err(i+1)=asin(Cne(3,3));Lon_err(i+1)=atan(Cne(3,2)/Cne(3,1)); %这是经度的主值if (Cne(3,1) < 0)if (Lon_err(i+1) > 0)Lon_err(i+1) = Lon_err(i+1) - pi;elseLon_err(i+1) = Lon_err(i+1) + pi;endend% 四元数的及时修正《惯性导航系统》 P194% Mat_Wnbb=[ 0, -Wnbb(1,1), -Wnbb(2,1), -Wnbb(3,1); % Wnbb(1,1), 0, Wnbb(3,1), -Wnbb(2,1); % Wnbb(2,1), -Wnbb(3,1), 0, Wnbb(1,1); % Wnbb(3,1), Wnbb(2,1), -Wnbb(1,1), 0];% q=q+Mat_Wnbb*q*TimeEach/2.0;q(1,1)=q(1,1)+TimeEach*(-Wnbb(1,1)*q(2,1)-Wnbb(2,1)*q(3,1)-Wnbb(3,1)* q(4,1))/2.0;q(2,1)=q(2,1)+TimeEach*(Wnbb(1,1)*q(1,1)+Wnbb(3,1)*q(3,1)-Wnbb(2,1)*q (4,1))/2.0;q(3,1)=q(3,1)+TimeEach*(Wnbb(2,1)*q(1,1)-Wnbb(3,1)*q(2,1)+Wnbb(1,1)*q (4,1))/2.0;q(4,1)=q(4,1)+TimeEach*(Wnbb(3,1)*q(1,1)+Wnbb(2,1)*q(2,1)-Wnbb(1,1)*q (3,1))/2.0;% 四元数归一化处理q_norm=sqrt(sum(q.*q));q=q/q_norm;% 计算姿态矩阵 TnbTnb(1,1) = q(1,1) ^2 + q(2,1) ^2 - q(3,1)^2 - q(4,1)^2;Tnb(1,2) = 2.0 * (q(2,1) * q(3,1) - q(1,1) * q(4,1));Tnb(1,3) = 2.0 * (q(2,1) * q(4,1) + q(1,1) * q(3,1));Tnb(2,1) = 2.0 * (q(2,1) * q(3,1) + q(1,1) * q(4,1));Tnb(2,2) = q(1,1)^2 - q(2,1)^2 + q(3,1)^2 - q(4,1)^2;Tnb(2,3) = 2.0 * (q(3,1) * q(4,1) - q(1,1) * q(2,1));Tnb(3,1) = 2.0 * (q(2,1) * q(4,1) - q(1,1) * q(3,1));Tnb(3,2) = 2.0 * (q(3,1) * q(4,1) + q(1,1) * q(2,1));Tnb(3,3) = q(1,1)^2 - q(2,1)^2 - q(3,1)^2 + q(4,1)^2;Tbn=Tnb';flag_pitch=asin(Tnb(3,2));flag_roll=atan(-Tnb(3,1)/Tnb(3,3));flag_head=atan(-Tnb(1,2)/Tnb(2,2));if(Tnb(3,3)<0)if(flag_roll<0)flag_roll=flag_roll+pi;endif(flag_roll>0)flag_roll=flag_roll-pi;endend% 偏航角范围 -180度——180度北偏西为正if(Tnb(2,2)<0)if(flag_head<0)flag_head=flag_head+pi;endif(flag_head>0)flag_head=flag_head-pi;endend% 姿态角更新pitch_err(i+1)=flag_pitch;head_err(i+1)=flag_head;roll_err(i+1)=flag_roll;% 解算完毕由对准结果、陀螺、加表的输出解算出载体的位置、速度、姿态%----------------------计算解算误差------------------ddLat(i)=(Lat_err(i)-latitude(i))*rad_deg; %纬度误差单位：度ddLog(i)=(Lon_err(i)-longitude(i))*rad_deg; %经度误差单位：度ddHigh(i)=High_err(i)-High(i); %高度误差单位：米ddVe(i)=Ve_err(i)-Ve(i);ddVn(i)=Vn_err(i)-Vn(i); % 速度误差单位：米/妙2ddVu(i)=Vu_err(i)-Vu(i);ddpitch(i)=(pitch_err(i)-pitch(i))*rad_deg*3600; %姿态误差单位：度ddhead(i)=(head_err(i)-head(i))*rad_deg*3600;ddroll(i)=(roll_err(i)-roll(i))*rad_deg*3600;endfclose(fid_read);%---------------------------绘图开始--------------------------------- figure(1)plot(Time,ddLog)ylabel('经度误差(度)'),xlabel('时间(秒)');figure(2)plot(Time,ddLat)ylabel('纬度误差(度)'),xlabel('时间(秒)');figure(3)plot(Time,ddHigh);ylabel('高度误差(米)'),xlabel('时间(秒)');figure(4)plot(Time,ddhead)ylabel('偏航角误差(角妙)'),xlabel('时间(秒)'); figure(5)plot(Time,ddpitch)ylabel('俯仰角误差(角妙)'),xlabel('时间(秒'); figure(6)plot(Time,ddroll);ylabel('滚转角误差(角妙)'),xlabel('时间(秒)'); figure(7)plot(Time,ddVe);ylabel('东向速度误差(米/秒)'),xlabel('时间(秒)'); figure(8)plot(Time,ddVn)ylabel('北向速度误差(米/秒)'),xlabel('时间(秒)'); figure(9)plot(Time,ddVu)ylabel('天向速度误差(米/秒)'),xlabel('时间(秒)');%------------------------------绘图结束-------------------------------。
捷联惯导
坐标系的定义
1. 地理坐标系（下标为t）—— OXtYtZt ：O 取载体质心，Xt 轴指向东，Yt 轴指向北，Zt 轴沿垂线指向天。 2. 导航坐标系（下标为n）—— OX nYnZn ：O 取载体质心，Zn与 Zt 重合，Xn 与 Xt，Yn 与 Yt 相差一个游动方
C13
C23
C33
位置速率
p ep
位置速率是由飞行器地速的水平分量引起的，由于平台坐标系与地理坐标系相差 一个游动方位角，
可得：
VVENtt
cos sin
sin cos
VEp VNp
p ep
可写成
p epE
C32 C31
180 ,180
1.求纬度的真值L
L L 反正弦函数的主值域与L的定义域一致，因此：
主
2.求经度的真值
反正切函数的主值域是 90 ,90 与 的定义域不一致，因此需要在 的定义域内确定经度的真值。
由： 主
tan 1
C32 C31
tan 1
cos L sin cos L cos
其中：
.
V ep 平台系相对地球的加速度向量
f 加速度计测量的比力向量
2ie ep V ep 无明显物理意义，又称有害加速度
g 重力加速度向量
整理上式可得：
.
VEp
.
VNp
.
VUp
f
p E
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
四参数法 1843年发明的，首先在数学中引入四元数， 以 后用在刚体定位问题。凯里.克莱茵（ CayleyKlein）参数法，是在1897年提出的。
2010-03-19 九参等数效法转基动于矢方量向法余弦的概念，也称 方向余弦法。 13
2010-03-19
4
接联式惯导的算法的基本内容
（1）系统的启动和自检测 （2）系统初始化 （3）惯性仪表的误差补偿 （4）姿态矩阵的计算 （5）导航计算 （6）制导和控制信息的提取
2010-03-19
5
（1）系统的启动和自检测
系统启动后，各个部分的工作是否正常，要 通过自检测程序加以检测，其中包括电源、惯 性 仪表、计算机以及计算机软件。
用欧拉角表示的姿态矩阵
X
' b
Yb'
Z
' b
cos H sin H － 0
sin H cos H
0 v-
0E
0N
_1__U
Zb Z b'' U ' PZb H.
CH
X b'' Yb''
Z
'' b
1 00 －
0 cos P sinvP-
CP
0
sin cos
PP___
f
b ib
捷联式惯导算法
, L,VE,VN
根据捷联式惯导的应用和功能要求不同，计算的内容和要 求，有很大的差别。常有
SINS——Strapdown Inertial Navigation Systems SVRU—— Strapdown Vertical Reference Uint SAHRS——Strapdown Attitude and Heading Reference Systems IMU——Inertial measurement Unit
R
sin
H
sin R cos H sin P cos R sin H
cos R sin H sin P sin R cos H cos P cos H
sin R sin H sin P cos R cos H
3.2 姿态矩阵的计算
3.2.1 欧拉角法 3.2.2 方向余弦法 3.2.3 四元数法 3.2.4 等效转动矢量法
2010-03-19
14
3.2.1 欧拉角法
一个动坐标系相对参考坐标系的方位，完全可以由动坐 标系依次绕3个不同的轴转动的3个转角来确定。
如把OXbYbZb作为动坐标系， ENU作为参考坐标系，则航向 角H，纵摇角（俯仰角）P和横 摇角（横滚角、倾斜角）R。就 是一组欧拉角。
惯性仪表的校准 Calibration
捷联式 姿态矩阵的初始值
陀螺仪 标度系数 漂移 进行测定
加速度计
偏置
2010-03-19
7
（3）惯性仪表的误差补偿
对捷联式惯导系统来说，由于惯性仪表直接安装 在载体上，因此，载体的线运动和角运动都引起 较大的误差。
为了保证系统的精度，必须对惯性仪表的误差进 行 补偿，最好的补偿方法是计算机补偿。
在计算机中通过专用的软件来实现误差补偿。
2010-03-19
8
（4）姿态矩阵的计算
姿态矩阵的计算是捷联式惯导算法中最重要的一 部分，也是捷联式系统所特有的。
不管捷联式惯导应用和功能要求如何，姿态矩 阵 的计算却是不可少的。姿态矩阵算法是本章 重点 讨论的内容。
2010-03-19
9
（5）导航计算
初始条件 导航计算机
H Pt R
VE VN
ibb
P, R, H
捷联式惯导算法
f ibb
, L,VE ,VN
捷联式惯导航系统是一个信息处理系统，就是将载体上安装的惯性 仪表所测量的载体运动信息，经过计算处理成所需要的导航信息。
2010-03-19
3
捷联式惯性导航系统=信息处理系统
b ib
P, R, H
惯性导航系统原理
3 捷联式惯导系统 程向红
2 0 10 .0 3 .19
3 捷联式惯导系统
3.1 捷联式惯导算法概述 3.2 姿态矩阵的计算 3.3 姿态矩阵计算机执行算法
2010-03-19
2
3.1 捷联式惯导算法概述
加速度计组
S
b FΒιβλιοθήκη 陀 螺 仪 组 ibbCbn
S
n F
b in
姿态矩 阵计 算
通过自检测，发现有不正常，则发出告警信息(或 故障码)。系统的自检测是保证系统进入导航状态 后能正常工作、提高系统可靠性的措施。
2010-03-19
6
（2）系统初始化 为何要初始化？
给定载体（舰船、飞行器、车辆等）的初始位置 （经度和纬度）和初始速度等初始信息。
导航平台的初始对准 平台式 用物理的方法来实现
X
' b
Yb' Z'
b
X b cos R
Yb 0
Zb
sin R －
0 1
sin 0
R
X
'' b
Yb''
0v -
cos
R___
Z
'' b
CR
ER
X b'
X b''
Xb
O P .
HPR
010-03-19
Cnb
2
cos
R
cos H sin P sin cos P sin H
2010-03-19
11
捷联式惯导系统算法流程图
启动
自检测
初始化
姿态阵计算
迭 代 次 数 NO
YES
导航计算
2010-03-19
返回9
控制信息提取
12
3.2 姿态矩阵的计算
捷联式惯导中，载体地理位置就是地理坐标系相 对 地球坐标系的方位。而载体的姿态和航向则是载 体 坐标系相对于地理坐标系的方位关系。确定两 个坐 标系的方位关系问题，是力学中的刚体定点 转到理 论。在刚体定点转动理论中，描述动坐标系 相对参 考坐标系方位关系的方法有多种。
Zb
Z
'' b
U
Z
' b
P
欧拉角没有严格的定义，根 据需要，可以选用不同的欧拉 角组。第一次转动，可以绕三 个轴中的任一个转动，故有3种 可能，第二次有2种可能，第三 次也有2种可能。总共有12种可 能。
2010-03-19
ER
X
'' b
X
' b
Xb
H. R.
O P.
Yb'' Yb Yb' H N
15
导航计算就是把加速度计的输出信息变换到导航 坐 标系，然后，计算载体速度、位置等导航信息。
2010-03-19
10
（6）制导和控制信息的提取
制导和控制信息的提取，载体的姿态既可用 来 显示也是控制系统最基本的控制信息。
此外，载体的角速度和线速度信息也都是控 制 载体所需要的信息。
这些信息可以从姿态矩阵的元素和陀螺加速 度 计的输出中提取出来。