捷联惯导程序，依据加表和陀螺仪的输出数据来求解飞行器的姿态
clc;
clear;
format long;%设置数据精度为15位小数
Data=importdata('temp.txt');%导入实验所采集的数据，以矩阵形式赋给Data变量，temp.txt 必须与该M文件在同一个文件夹中
Px=Data(:,3);%Px,Py,Pz为陀螺仪的输出值
Py=Data(:,4);
Pz=Data(:,5);
Nx=Data(:,6);%Nx,Ny，Nz为加速度计的输出值
Ny=Data(:,7);
Nz=Data(:,8);
%陀螺仪模型参数标定如下：
Sx=-4.085903e-006;Sy=-4.085647e-006;Sz=-4.085170e-006;
Mxy=5.059527e-003;Mxz=-1.031103e-003;Myx=-3.355451e-003;
Myz=3.508468e-003;Mzx=-1.266671e-003;Mzy=-2.318244e-004;
Dx=-2.009710e-006;Dy=8.156346e-007;Dz=-5.749059e-007;
GyroCali_A=[1-Mxy-Mxz;-Myx1-Myz;-Mzx-Mzy1];
%加速度计模型参数标定如下：
Kx=9.272930e-004;Ky=9.065544e-004;Kz=9.443748e-004;
Ixy=6.533872e-003;Ixz=9.565992e-004;Iyx=-6.319376e-003;
Iyz=-6.902339e-004;Izx=-1.144549e-003;Izy=-3.857963e-004;
Bx=-3.400847e-002;By=-8.916341e-003;Bz=-9.947414e-003;
AccCali_A=[1-Ixy-Ixz;-Iyx1-Iyz;-Izx-Izy1];
Delta_t=0.05;%采样时间为0.05秒
Delta_Theta_x=0;
Delta_Theta_y=0;
Delta_Theta_z=0;%定义陀螺仪输出的角度增量
Delta_Vx=0;
Delta_Vy=0;
Delta_Vz=0;%定义加速度计输出的速度增量
L=zeros(1,12001);
L(1)=45.7328*pi/180;%纬度用L表示，纬度的初始值划为弧度形式，因为后面计算位置矩阵更新
L(2)=45.7328*pi/180;%时需要用到前两次的L值来计算当前L值，所以在此定义2个初始L值
Lamda=126.6287*pi/180;%经度用Lamda表示，经度的初始值划为弧度形式
h=136;%高度用h表示
V=[0;0;0];%导航坐标系中的东北天初始速度都为0
Vx=0;%方便后面的速度计算与速度更新
Vy=0;
Vz=0;
Theta=0;
Gama=0;
Fai=0;%初始姿态角（俯仰角/倾斜角/航向角）都为0,此处均为弧度
Re=6378254;Rp=6356803;%定义地球的半长轴与半短轴
e=(Re-Rp)/Re;%定义旋转椭球扁率（椭球度）
Wie=15.04107/180*pi;%定义地球自转角速度，地球坐标系相对于惯性坐标系的角速度
Theta_Matrix=zeros(1,12000);%定义姿态角矩阵，供画图用
Gama_Matrix=zeros(1,12000);
Fai_Matrix=zeros(1,12000);
L_Matrix=zeros(1,12001);%定义经纬度矩阵，供画图用，L的特殊性决定了其数据个数为12001
L_Matrix(1)=45.7328;
Lamda_Matrix=zeros(1,12000);
Ve_Matrix=zeros(1,12000);%定义速度矩阵，供画图用
Vn_Matrix=zeros(1,12000);
Vu_Matrix=zeros(1,12000);
%以下计算捷联矩阵的初始值，捷联矩阵的初始值仅仅由Theta，Gama，Fai的初始值决定T=[cos(Gama)*cos(Fai)-sin(Gama)*sin(Theta)*sin(Fai)-cos(Theta)*sin(Fai) sin(Gama)*cos(Fai)+cos(Gama)*sin(Theta)*sin(Fai);
cos(Gama)*sin(Fai)+sin(Gama)*sin(Theta)*cos(Fai)cos(Theta)*cos(Fai) sin(Gama)*sin(Fai)-cos(Gama)*sin(Theta)*cos(Fai);
-sin(Gama)*cos(Theta)sin(Theta) cos(Gama)*cos(Theta)];
%由捷联矩阵的初始值计算初始四元数值，为捷联矩阵的实时更新做准备
if(T(3,2)-T(2,3)>0)
Q1=0.5*sqrt(1+T(1,1)-T(2,2)-T(3,3));
else if(T(3,2)-T(2,3)==0)
Q1=0;
else Q1=-0.5*sqrt(1+T(1,1)-T(2,2)-T(3,3));
end%求解Q1
end
if(T(1,3)-T(3,1)>0)
Q2=0.5*sqrt(1-T(1,1)+T(2,2)-T(3,3));
else if(T(1,3)-T(3,1)==0)
Q2=0;
else Q2=-0.5*sqrt(1-T(1,1)+T(2,2)-T(3,3));
end%求解Q2
end
if(T(2,1)-T(1,2)>0)
Q3=0.5*sqrt(1-T(1,1)-T(2,2)+T(3,3));
else if(T(2,1)-T(1,2)==0)
Q3=0;
else Q3=-0.5*sqrt(1-T(1,1)-T(2,2)+T(3,3));
end%求解Q3
end
Q0=0.5*sqrt(1-Q1*Q1-Q2*Q2-Q3*Q3);%求解Q0
Q=[Q0;Q1;Q2;Q3];%四元数初始值
Q=Q/sqrt(Q0*Q0+Q1*Q1+Q2*Q2+Q3*Q3);%四元数的初始归一化，为得到最小漂移误差
%以下求位置矩阵的初始值，通过位置矩阵更新后，反过来算运载体所在的经纬度%位置矩阵仅仅与经纬度有关系，Ce2n表示把地球坐标系转换为导航坐标系的转换矩阵Ce2n=[-sin(Lamda)cos(Lamda)0;
-sin(L(1))*cos(Lamda)-sin(L(1))*sin(Lamda)cos(L(1));
cos(L(1))*cos(Lamda)cos(L(1))*sin(Lamda)sin(L(1))];
%大循环，共执行12000次，实时更新捷联矩阵，速度矩阵，位置矩阵，保存作图所需数据for k=1:12000;
GyroCali_B=[Sx*Px(k)-Dx*Delta_t;Sy*Py(k)-Dy*Delta_t;Sz*Pz(k)-Dz*Delta_t];
Delta_Theta=GyroCali_A*GyroCali_B;%计算陀螺仪输出的角度增量
Delta_Theta_x=Delta_Theta(1);
Delta_Theta_y=Delta_Theta(2);
Delta_Theta_z=Delta_Theta(3);
Delta_Theta_Module=sqrt(Delta_Theta_x*Delta_Theta_x+Delta_Theta_y* Delta_Theta_y+Delta_Theta_z*Delta_Theta_z);
AccCali_B=[Kx*Nx(k)-Bx*Delta_t;Ky*Ny(k)-By*Delta_t;Kz*Nz(k)-Bz*Delta_t];
Delta_V=AccCali_A*AccCali_B;%计算加速度计输出的速度增量
Delta_Vx=Delta_V(1);
Delta_Vy=Delta_V(2);
Delta_Vz=Delta_V(3);
Delta_V_Module=sqrt(Delta_Vx*Delta_Vx+Delta_Vy*Delta_Vy+Delta_Vz* Delta_Vz);
%使用毕卡法求解四元数更新矩阵，即捷联矩阵
Bika=zeros(4);
Bika(1,1)=cos(0.5*Delta_Theta_Module);
Bika(1,2)=-Delta_Theta_x/Delta_Theta_Module*sin(0.5*Delta_Theta_Module);
Bika(1,3)=-Delta_Theta_y/Delta_Theta_Module*sin(0.5*Delta_Theta_Module);
Bika(1,4)=-Delta_Theta_z/Delta_Theta_Module*sin(0.5*Delta_Theta_Module);
Bika(2,1)=-Bika(1,2);
Bika(2,2)=Bika(1,1);
Bika(2,3)=-Bika(1,4);
Bika(2,4)=Bika(1,3);
Bika(3,1)=-Bika(1,3);
Bika(3,2)=-Bika(2,3);
Bika(3,3)=Bika(1,1);
Bika(3,4)=-Bika(1,2);
Bika(4,1)=-Bika(1,4);
Bika(4,2)=-Bika(2,4);
Bika(4,3)=-Bika(3,4);
Bika(4,4)=Bika(1,1);
Q=Bika*Q;%每循环一次，更新一次四元素Q值，为求捷联矩阵
Q=Q/sqrt(Q0*Q0+Q1*Q1+Q2*Q2+Q3*Q3);%四元数的归一化，为得到最小漂移误差Q0=Q(1);
Q1=Q(2);
Q2=Q(3);
Q3=Q(4);
%捷联矩阵的四元数表达式
T=[Q0*Q0+Q1*Q1-Q2*Q2-Q3*Q32*(Q1*Q2-Q0*Q3) 2*(Q1*Q3+Q0*Q2)
2*(Q1*Q2+Q0*Q3)Q0*Q0-Q1*Q1+Q2*Q2-Q3*Q3 2*(Q2*Q3-Q0*Q1)
2*(Q1*Q3-Q0*Q2)2*(Q2*Q3+Q0*Q1) Q0*Q0-Q1*Q1-Q2*Q2+Q3*Q3];
%*********************************************************************
%********************求三个姿态角Theta，Gama和Fai******************** %*********************************************************************
Theta_Main=asin(T(3,2));
Gama_Main=atan(-T(3,1)/T(3,3));
Fai_Main=atan(-T(1,2)/T(2,2));
Theta=Theta_Main;
if(T(3,3)>0)
Gama=Gama_Main;
else if(T(3,3)<0&&Gama_Main>0)
Gama=Gama_Main+pi;
else Gama=Gama_Main-pi;%此处用else实为不妥，不过为了程序的完善性，只能这样了
end
end
if(T(2,2)<0)
Fai=Fai_Main+pi;
else if(T(2,2)==0)
Fai=pi/2;
else if(Fai_Main>0)
Fai=Fai_Main;
else Fai=Fai_Main+2*pi;
end
end
end
%以下存储姿态角到三个矩阵里面，为画图做准备
Theta_Matrix(k)=Theta*180/pi;%作图用矩阵,以角度表示
Gama_Matrix(k)=Gama*180/pi;%作图用矩阵,以角度表示
if(Fai<2*pi)
Fai_Matrix(k)=Fai*180/pi;
else Fai_Matrix(k)=Fai*180/pi-360;%作图用矩阵,以角度表示
end
%到此为止，姿态角的求解完毕，以下先求速度
%*********************************************************************
%********************求飞行器相对于东北天的速度************************* %*********************************************************************
Rm=Re*(1-2*e+3*e*sin(L(k+1))^2);
Rn=Re*(1+e*sin(L(k+1))^2);
LL=3/2*L(k+1)-1/2*L(k);
F=[0-1/(Rm+h)0;1/(Rn+h)00;tan(LL)/(Rn+h)00];
g=9.7803+0.051799*sin(L(k+1))^2-0.94114e-006*h;
G=[0;0;-g];
Wen2n=F*V;
Wie2n=[0;Wie*cos(L(k+1));Wie*sin(L(k+1))];
W=2*Wie2n+Wen2n;
W_X=[0-W(3)W(2);W(3)0-W(1);-W(2)W(1)0];%此式中W_X为2*Wie+Wen2n的反对称矩阵
V=V+T*(Delta_V+[0-Delta_Theta_z Delta_Theta_y;Delta_Theta_z0 -Delta_Theta_x;-Delta_Theta_y Delta_Theta_x0]*Delta_V)+Delta_t*(G-W_X*V);
Vx=V(1);
Vy=V(2);
Vz=V(3);
Ve_Matrix(k)=V(1);
Vn_Matrix(k)=V(2);
Vu_Matrix(k)=V(3);
%*********************************************************************
%********************求飞行器所在的经纬度****************************** %*********************************************************************
Epsilon=F*V*Delta_t;
Ce2n=(eye(3)-[0-Epsilon(3)Epsilon(2);Epsilon(3)0-Epsilon(1);-Epsilon(2) Epsilon(1)0])*Ce2n;%位置矩阵实时更新
%下面通过位置矩阵来实时更新经纬度
L(k+2)=asin(Ce2n(3,3));%由于L本来就是以矩阵形式定义的，下面定义一个把L用角度表示的矩阵
L_Matrix(k+1)=L(k+2)*180/pi;
Lamda_Main=atan(Ce2n(3,2)/Ce2n(3,1));%计算出来的L和Lamda都是弧度制的
if(Ce2n(3,1)>0)
Lamda=Lamda_Main;
else if(Lamda_Main<0)
Lamda=Lamda_Main+pi;
else Lamda=Lamda_Main-pi;
end
end
Lamda_Matrix(k)=Lamda*180/pi;%作图用矩阵，以角度表示
end
%**************************************************************************
%********************以下是画图程序***************************************** %**************************************************************************
k=1:1:12000;%绘制三轴姿态变化图线-绿色
figure(1);
plot(k/20,Theta_Matrix(k),'g');
xlabel('Time(second)');
ylabel('Angle(degree)');
title('Theta(俯仰角)');
grid on;
figure(2);
plot(k/20,Gama_Matrix(k),'g');
xlabel('Time(second)');
ylabel('Angle(degree)');
title('Gama（滚转角）');
grid on;
figure(3);
plot(k/20,Fai_Matrix(k),'m');
xlabel('Time(second)');
ylabel('Angle(degree)');
title('Fai（偏航角）');
grid on;
%绘制东北天个方向的速度变化曲线-红色figure(4);
plot(k/20,Ve_Matrix(k),'r');
xlabel('Time(second)');
ylabel('Velocity(m/s)');
title('Ve（东向速度）');
grid on;
figure(5);
plot(k/20,Vn_Matrix(k),'r');
xlabel('Time(second)');
ylabel('Velocity(m/s)');
title('Vn（北向速度）');
grid on;
figure(6);
plot(k/20,Vu_Matrix(k),'r');
xlabel('Time(second)');
ylabel('Velocity(m/s)');
title('Vu（天向速度）');
grid on;
%绘制飞行器所在经纬度曲线-蓝色figure(7);
plot(k/20,L_Matrix(k),'b');
xlabel('Time(second)');
ylabel('Degree');
title('Latitude L(纬度)');
grid on;
figure(8);
plot(k/20,Lamda_Matrix(k),'b');
xlabel('Time(second)');
ylabel('Degree');
title('Longitude Lamda(经度)');
grid on;
卡尔曼滤波实验报告 捷联惯导静基座初始对准实验 一、实验目的 ①掌握捷联惯导的构成和基本工作原理； ②掌握捷联惯导静基座对准的基本工作原理； ③了解捷联惯导静基座对准时的每个系统状态的可观测性； ④了解双位置对准时系统状态的可观测性的变化。 二、实验原理 选取状态变量为：[]T E N E N U x y x y z X V V δδεεε=ψψψ??，其
中导航坐标系选为东北天坐标系，E V δ为东向速度误差，N V δ为北向速度误差，E ψ为东向姿态误差角，N ψ为北向姿态误差角，U ψ为天向姿态误差角，x ?为东向加速度偏置，y ?为北向加速度偏置，x ε为东向陀螺漂移，y ε为北向陀螺漂移，z ε为天向陀螺漂移。则系统的状态模型为： X AX W =+ (1) 其中， 1112212211 12 1321222331323302sin 000002sin 000000000sin cos 0000sin 000000cos 0000000000000000000000000000000000000000000000000000 0L g C C L g C C L L C C C L C C C L C C C A Ω-? ? ??-Ω????Ω-Ω? ?-Ω????Ω=? ?????? ?????????? ? [00000]E N E N U T V V W W W W W W δδψψψ=，E D V W W δψ 为零均值高斯 白噪声，分别为加速度计误差和陀螺漂移的噪声成分，Ω为地球自转角速度，ij C 为姿态矩 阵n b C 中的元素，L 为当地纬度。 量测量选取两个水平速度误差：[ ]T E N Z V V δδ=，则量测方程为： 10000000000100000000E E N N V X V δηδη???? ??=+???????????? (2) 即Z HX η=+ 其中，H 为量测矩阵，[]T E N ηηη=为量测方程的随机噪声状态矢量，为零均值高 斯白噪声。 要利用基本卡尔曼滤波方程进行状态估计，需要将状态方程和量测方程进行离散化。 系统转移矩阵为： 2323/1111102!3!! n n k k k k k k n T T T I TA A A A n ∞ -----=Φ=++++=∑ (3)
第六章捷联惯导
6-1捷联惯导的原理?捷联惯导系统概述 ?捷联惯性技术的发展过程 ?捷联惯导系统与平台惯导系统的对比 ?捷联惯导系统的基本力学编排方程?捷联惯导系统的算法概述 ?捷联惯导系统原理框图的说明 ?姿态方程的解算 （1）姿态和航向角的计算 （2）姿态矩阵的微分方程 （3）四元数的运动学微分方程 （4）等效旋转矢量法及其微分方程 （5）位移角速率方程 （6）速度方程
?导航位置方程 （1）游动方位系与地球系之间的方向余弦矩阵 （2）载体位置计算 （3）方向余弦矩阵计算 ?垂直通道阻尼 ?捷联惯性器件的余度技术?单自由度陀螺仪的配置方案 （1）四陀螺仪配置方案 （2）六陀螺仪系统 ?二自由度陀螺仪的配置方案
?捷联惯导的数值计算方法?数值积分法 （1）欧拉法 （2）四阶龙格－库塔法 ?角速率信息的提取
“ 捷联（Strapdown）”这一术语的英文原义就是“捆绑”的意思。因此，所谓捷联惯性系统也就是将惯性敏感元件（陀螺与加速度计）直接“捆绑”在载体上，从而完成制导和导航任务的系统。 V-2导弹 “阿波罗-13”宇宙飞船 “海盗”火星降落器
从捷联技术的发展过程中我们已经看到捷联系统的优越性已越来越突出的显示出来，并在许多方面已日渐代替平台系统。为什么会出现这种情况呢？为了回答这一问题，这里从生产与使用的角度将捷联系统与平台系统做一对比。 （1）硬件和软件的复杂程度 由于捷联系统没有平台框架及相连的伺服装置，因而简化了硬件；代价是增加了计算机的负担，需要一个比较复杂的实时程序。 （2）可靠性 捷联系统的可靠性要比平台系统高，其原因是它的机械构件少，加之容易采用多敏感元件配置，实现余度技术。 （3）成本与可维护性 由于平台系统在机械结构上要复杂得多，而对于捷联系统只是算法复杂些，因而从制造成本上看捷联系统的成本要比平台系统低。从市场供应的情况来看，数字计算机的价格一直在下降，而平台系统的价格一直在上升。 此外，捷联系统比平台系统具有较长的平均故障间隔时间，加之模块设计简化了维修，从而捷联系统的可维护性比平台系统大为提高了。
第27卷 第1期航 空 学 报 Vol 127No 11 2006年 1月ACTA A ERONAU TICA ET ASTRONAU TICA SIN ICA J an. 2006 收稿日期:2004209230;修订日期:2005204227基金项目:国家自然科学基金(60234030)、国家杰出青年科学基金 (60225015)和教育部高校青年教师奖资助项目 文章编号:100026893(2005)0120098206 捷联惯导姿态算法中的圆锥误差与量化误差 练军想,胡德文,胡小平,吴文启 (国防科技大学机电工程与自动化学院自动控制系,湖南长沙 410073) R esearch on Coning E rror and Q uantization E rror of SINS Attitude Algorithm L IAN J un 2xiang ,HU De 2wen ,HU Xiao 2ping ,WU Wen 2qi (Department of Automatic Control ,College of Mechatronics and Automation ,National University of Denfense Technology ,Changsha 410073,China ) 摘 要:对捷联惯导系统的误差源进行了研究,利用几何方法分析了不可交换性误差和量化误差的形成机理,以及它们的相互影响。针对工程应用中激光陀螺输出脉冲采样量化条件,就多子样算法进行了讨论,并设计了基于MA TL AB/Simulink 的仿真。研究结果表明,当考虑量化误差的影响时,选取适当的量化因子,三子样等效旋转矢量算法比其它算法具有更好的综合性能。 关键词:捷联惯导;姿态算法;不可交换性误差;量化误差;多子样中图分类号:V249.3 文献标识码:A Abstract :In this paper ,the error sources of strapdown inertial navigation system (SINS )are researched.The mechanism of noncommutativity error and quantization error is illustrated f rom the geometrical point of view ,and the mutual influence between them is analyzed.Considering the sample condition of the output pulses of the ring laser gyroscope (RL G ),the multi 2sample attitude algorithm of SINS is discussed.The simu 2lation is carried out using MA TL AB/Simulink.It is concluded that the three 2sample rotation vector algorithm with the proper quantization factor outperforms others when the impact of quantization errors is taken into ac 2count. K ey w ords :SINS ;attitude algorithm ;noncommutativity error ;quantization error ;multi 2sample 对于捷联惯导系统姿态更新算法的研究,目前国内外同行们有很多成果。普遍的观点认 为[1,2]:四元数姿态更新算法比方向余弦和欧拉角等其它方法的计算量小、适应性好;双子样、三子样等效旋转矢量算法在抑制不可交换性误差方面比单子样有显著改善;将姿态更新分成快慢两个回路,即快速回路更新旋转矢量,慢速回路更新姿态参数,既能有效抑制不可交换性误差,还可以减少算法的计算量。但上述观点很少有结合捷联惯导系统工程实际来考虑的。本文针对工程中激光陀螺输出脉冲采样量化条件,就多子样算法展开研究,并得出了相应的结论。 1 误差源分析 评价算法优劣的最重要的标准之一是算法的 精度。要提高算法的精度就必须研究各种误差及其补偿、抑制方法。 在对惯导系统作原理性分析时,尚可以理想化地认为平台坐标系(平台式或捷联式)准确、无误差地模拟地理坐标系,但实际的惯导系统不可避免地受到各种误差的影响[3]。通常,可将误差分为源信息误差和解算方法误差。对捷联惯导系统,解算方法误差是指数值求解姿态矩阵和比力方程时产生的原理性误差,比如不可交换性误差、舍入误差、截断误差等。源信息误差可分为解析源信息误差和测量源信息误差。测量源信息误差就是惯性传感器的误差,此类误差因惯性器件的物理原理和仪表的精度不同而各不相同。比如陀螺的常值漂移和随机漂移、刻度因子误差、非线性误差、输出值的量化误差等。 下面重点对捷联姿态解算时不可交换性误差和陀螺采样的量化误差进行分析。111 不可交换性误差 在利用方向余弦矩阵微分方程或四元数微分方程进行姿态更新时,都会遇到角速度矢量的积分问题。由于刚体有限转动的不可交换性,当转
捷联惯导算法心得 1、四个概念：“地理”坐标系、“机体”坐标系、他们之间换算公式、换算公式用的系数。 地理坐标系：东、北、天，以下简称地理。在这个坐标系里有重力永远是（0,0,1g），地磁永远是（0,1,x）（地磁的垂直不关心）两个三维向量。 机体坐标系：以下简称机体，上面有陀螺、加计、电子罗盘传感器，三个三维向量。 换算公式：以下简称公式，公式就是描述机体姿态的表达方法，一般都是用以地理为基准，从地理换算到机体的公式，有四元数、欧拉角、方向余弦矩阵。 换算公式的系数：以下简称系数，四元数的q0123、欧拉角的ROLL/PITCH/YAW、余弦矩阵的9个数。系数就是描述机体姿态的表达方法的具体数值。 姿态，其实就是公式+系数的组合，一般经常用人容易理解的公式“欧拉角”表示，系数就是横滚xx度俯仰xx度航向xx度。 2、五个数据源：重力、地磁、陀螺、加计、电子罗盘，前两个来自地理，后三个来自机体。 3、陀螺向量：基于机体，也在机体上积分，因为地理上无参考数据源，所以很独立，直接在公式的老系数上积分，得到新系数。 狭义上的捷联惯导算法，就是指这个陀螺积分公式，也分为欧拉角、方向余弦矩阵、四元数，他们的积分算法有增量法、数值积分法（X阶龙格-库塔）等等 4、加计向量、重力向量：加计基于机体，重力基于地理，重力向量（0,0,1g）用公式换算到机体，与机体的加计向量算出误差。理论上应该没有误差，这误差逆向思维一下，其实就是换算公式的系数误差。所以这误差可用于纠正公式的系数（横滚、俯仰），也就是姿态。 5、电子罗盘向量、地磁向量：同上，只不过要砍掉地理上的垂直向量，因为无用。只留下地理水平面上的向量。误差可以用来纠正公式的系数（航向）。 6、就这样，系数不停地被陀螺积分更新，也不停地被误差修正，它和公式所代表的姿态也在不断更新。 如果积分和修正用四元数算法（因为运算量较少、无奇点误差），最后用欧拉角输出控制PID（因为角度比较直观），那就需要有个四元数系数到欧拉角系数的转换。常用的三种公式，它们之间都有转换算法。 再搞个直白一点的例子： 机体好似一条船，地理就是那地图，姿态就是航向（船头在地图上的方位），重力和地磁是地图上的灯塔，陀螺/积分公式是舵手，加计和电子罗盘是瞭望手。舵手负责估计和把稳航向，他相信自己，本来船向北开的，就一定会一直往北开，
捷联惯导系统初始对准技术的研究 摘要:初始对准是捷联惯导系统关键技术之一。初始对准精度直接影响捷联惯导系统的工作精度，初始对准时间也是反映武器系统快速反应能力的重要战术指标。 捷联惯性导航系统是将惯性器件陀螺仪、加速度计构成的惯性测量单元直接与载体固联，测量得到的载体角速度与线运动参数是沿载体固联的坐标轴上的分量。导航计算机通过计算“姿态矩阵”可以将加速度信息转换到惯性坐标系或当地地理坐标系，从而实现“数学平台”，然后再进行速度及位置计算。图1即为捷联式惯性导航系统原理框图。 捷联惯导系统的关键技术包括初始对准问题、有害加速度的消除及引力修正、惯性元件误差模型的建立和实时补偿、捷联矩阵的更新等。捷联惯性导航系统初始对准的目的是建立捷联矩阵的初始值。 1、捷联惯导系统初始对准基本概念 按对准阶段来分，初始对准一般分为两个阶段:第一阶段为粗对准，第二阶段为精对准。捷联系统粗对准的任务是得到粗略的捷联矩阵，为后续的精对准提供基础，此阶段精度可以低一些，但要求速度快。精对准是在粗对准的基础上进行的，通过处理惯性敏感元件的输出信息，精确校正真实导航坐标系与计算的导航坐标系之间的失准角，使之趋于零，从而得到精确的捷联矩阵。按照捷联惯性导航系统初始对准时载体的运行状态来分，可分为静基座对准和动基座对准。按
照初始对准时是否取得外部信息，可分为自对准和非自对准.惯性导航系统的自对准是利用重力矢量和地球自转角速率矢量通过解析的方法实现的初始对准，这种对准方法的优点是自主性强，缺点是所需的对准时间长。非自主式对准可以通过机电或光学方法将外部参考坐标系引入系统，实现惯性系统的初始对准.在捷联惯性导航系统的粗对准阶段，可以通过引入主惯导系统的航向姿态信息，通过传递对准，迅速将数学平台对准导航坐标系，减小初始失准角.在精对准阶段，可以通过组合导航的方法，利用其它导航设备(如GPS，计程仪)等提供的信息(如速度和位置)作为观测信息，通过卡尔曼滤波实现精确对准。目前有关初始对准问题的研究主要集中在误差模型的建立、模型求解方法和误差模型的可观性分析三个方面，本文正是针对这三个方面，对初始对准的国内外研究状况进行综述。 2、初始对准的误差模型 捷联惯导系统初始对准的误差模型及常用算法研究的基础模型有Ψ角误差模型和Φ角误差模型。前者基于导航坐标系，是指导航坐标系到计算坐标系(通常取计算的地理坐标系)的小角度误差；后者基于真实坐标系，是指导航坐标系到真实坐标系(通常取地理坐标系)的小角度误差。 3、求解误差模型的方法 3. 1古典方法 从频率域角度设计对准回路，并将对准分为水平对准和方位对准，对准过程首先是水平粗对准，然后是方位粗对准;在粗对准之后再精对准，首先是水平精对准，然后是方位精对准.由于对准回路频带低，响应慢，因此整个对准时间长。针对在静基座捷联惯导系统初始对准中，东向陀螺漂移没有估计效果，使得估计的方位失准角存在常值误差的问题，上海交大提出了，建立东向陀螺漂移估计位的修正方程，并对其进行修正，从而大大提高了方位失准角的估计精度。 3.2 Kalman滤波在初始对准中的应用 提高惯性导航系统初始对准精度的最佳途径之一是利用Kalman滤波这一重要数学工具，对于采用自主对准方式的惯导系统，一般采用卡尔曼滤波技术估计出系统的失调角和惯导系统误差源，然后采用一定的控制技术设计出控制角速率使失调角达到规定的要求。卡尔曼滤波的对象是用状态方程来描述随机线性系统，它按照估计误差方差最小的原则，从被污染的观测值中，实时估计出系统的各个
捷联惯导程序，依据加表和陀螺仪的输出数据来求解飞行器的姿态 clc; clear; format long; %设置数据精度为15位小数 Data=importdata(''); % 导入实验所采集的数据，以矩阵形式赋给Data变量，必须与该M文件在同一个文件夹中 Px=Data(:,3); % Px,Py,Pz为陀螺仪的输出值 Py=Data(:,4); Pz=Data(:,5); Nx=Data(:,6); % Nx,Ny，Nz为加速度计的输出值 Ny=Data(:,7); Nz=Data(:,8); % 陀螺仪模型参数标定如下： Sx = ; Sy = ; Sz = ; Mxy = ; Mxz = ; Myx = ; Myz = ; Mzx = ; Mzy = ; Dx = ; Dy = ; Dz = ; GyroCali_A = [ 1 -Mxy -Mxz ; -Myx 1 -Myz ; -Mzx -Mzy 1 ]; % 加速度计模型参数标定如下： Kx = ; Ky = ; Kz = ; Ixy = ; Ixz = ; Iyx = ; Iyz = ; Izx = ; Izy = ; Bx = ; By = ; Bz = ; AccCali_A = [1 -Ixy -Ixz ; -Iyx 1 -Iyz ; -Izx -Izy 1 ]; Delta_t = ; %采样时间为秒 Delta_Theta_x = 0; Delta_Theta_y = 0; Delta_Theta_z = 0; %定义陀螺仪输出的角度增量 Delta_Vx = 0; Delta_Vy = 0; Delta_Vz = 0; %定义加速度计输出的速度增量 L = zeros(1,12001); L(1)= *pi/180 ; %纬度用L表示，纬度的初始值划为弧度形式，因为后面计算位置矩阵更新 L(2)= *pi/180 ; %时需要用到前两次的L值来计算当前L值，所以在此定义2个初始L值Lamda = *pi/180 ; %经度用Lamda表示，经度的初始值划为弧度形式 h = 136 ; %高度用h表示
1、四个概念：“地理”坐标系、“机体”坐标系、他们之间换算公式、换算公式用的系数。 地理坐标系：东、北、天，以下简称地理。在这个坐标系里有重力永远是（0,0,1g），地磁永远是（0,1,x）（地磁的垂直不关心）两个三维向量。 机体坐标系：以下简称机体，上面有陀螺、加计、电子罗盘传感器，三个三维向量。 换算公式：以下简称公式，公式就是描述机体姿态的表达方法，一般都是用以地理为基准，从地理换算到机体的公式，有四元数、欧拉角、方向余弦矩阵。 换算公式的系数：以下简称系数，四元数的q0123、欧拉角的ROLL/PITCH/YAW、余弦矩阵的9个数。系数就是描述机体姿态的表达方法的具体数值。 姿态，其实就是公式+系数的组合，一般经常用人容易理解的公式“欧拉角”表示，系数就是横滚xx度俯仰xx度航向xx度。 2、五个数据源：重力、地磁、陀螺、加计、电子罗盘，前两个来自地理，后三个来自机体。 3、陀螺向量：基于机体，也在机体上积分，因为地理上无参考数据源，所以很独立，直接在公式的老系数上积分，得到新系数。 狭义上的捷联惯导算法，就是指这个陀螺积分公式，也分为欧拉角、方向余弦矩阵、四元数，他们的积分算法有增量法、数值积分法（X阶龙格-库塔）等等 4、加计向量、重力向量：加计基于机体，重力基于地理，重力向量（0,0,1g）用公式换算到机体，与机体的加计向量算出误差。理论上应该没有误差，这误差逆向思维一下，其实就是换算公式的系数误差。所以这误差可用于纠正公式的系数（横滚、俯仰），也就是姿态。 5、电子罗盘向量、地磁向量：同上，只不过要砍掉地理上的垂直向量，因为无用。只留下地理水平面上的向量。误差可以用来纠正公式的系数（航向）。 6、就这样，系数不停地被陀螺积分更新，也不停地被误差修正，它和公式所代表的姿态也在不断更新。 如果积分和修正用四元数算法（因为运算量较少、无奇点误差），最后用欧拉角输出控制PID（因为角度比较直观），那就需要有个四元数系数到欧拉角系数的转换。常用的三种公式，它们之间都有转换算法。 再搞个直白一点的例子： 机体好似一条船，地理就是那地图，姿态就是航向（船头在地图上的方位），重力和地磁是地图上的灯塔，陀螺/积分公式是舵手，加计和电子罗盘是瞭望手。 舵手负责估计和把稳航向，他相信自己，本来船向北开的，就一定会一直往北开，觉得转了90度弯，那就会往东开。 当然如果舵手很牛逼，也许能估计很准确，维持很长时间。不过只信任舵手，肯定会迷路，所以一般都有地图和瞭望手来观察误差。 瞭望手根据地图灯塔方位和船的当前航向，算出灯塔理论上应该在船的X方位。然而看到实际灯塔在船的Y方位，那肯定船的当前航向有偏差了，偏差就是ERR=X-Y。 舵手收到瞭望手给的ERR报告，觉得可靠，那就听个90%*ERR，觉得天气不好、地图误差大，那就听个10%*ERR，根据这个来纠正估算航向。。
捷联惯导系统快速罗经初始对准方法研究1 严恭敏1，严卫生1,2，徐德民1,2 1西北工业大学航海学院，西安（710072） 2水下信息处理与控制国家级重点实验室，西安（710072） E-mail：yangongmin@https://www.doczj.com/doc/f114472606.html, 摘要：在分析平台罗经初始对准原理基础上，提出了捷联罗经初始对准的原理并推导了适合于软件编程的算法。将捷联罗经对准的具体实现划分为四个阶段：方位角未知情况下的水平对准、粗略方位自对准、重新水平对准和罗经方位对准，通过对大方位误差角捷联惯导非线性误差方程的简化，推导了粗略方位自对准的算法公式。如果导航计算机存储容量足够大并且计算能力足够强，根据捷联惯导系统数学平台多样性和可进行逆向姿态控制的特点，设计了一种用于缩短捷联罗经初始对准时间的具体步骤。最后，试验表明快速捷联罗经对准方案是有效的。 关键词：捷联惯导系统，罗经效应，初始对准，逆向控制 中图分类号：V249.3 1. 引言 平台惯导系统罗经初始对准过程通常可分为两步，先是水平调平，然后是方位对准。方位对准在水平调平的基础上进行，一般采样罗经方位对准方法。方位罗经对准利用的是罗经效应，也就是，在正确的平台跟踪当地地理坐标系的角速率控制指令下，如果平台存在方位轴向的偏差角，平台将产生绕东向轴的倾斜，该倾斜能由北向加速度计感测到，利用北向加速度计的输出并设计适当的控制规律，控制平台方位轴朝减小方位偏差方向转动，实现平台自动寻北。捷联惯导系统初始对准通常可分为粗对准和精对准两个阶段：在粗对准阶段，利用地球自转角速度和重力加速度作为参考量，通过惯性器件的测量输出建立粗略的导航计算坐标系；在精对准阶段，通过现代控制理论最优估计方法估计出失准角，获得准确的姿态矩阵[1,2]。 捷联惯导系统经典解析式粗对准方法难以适应晃动干扰环境，有不少文献研究了晃动基座下的初始对准问题并且也出现一些应用实例，激光陀螺和光纤陀螺的发展和不断成熟为捷联罗经的研究注入了新的活力[3-6]。从本质上说，捷联惯导系统与平台惯导系统是相同的，前者以数学平台（利用姿态矩阵、四元数或欧拉角等数学工具）模拟后者的实体平台，描述捷联惯导系统相对于参考坐标系的空间方位。平台惯导系统中实体平台具有隔离外界干扰的作用，因而平台罗经能够实现晃动基座下的初始对准，同理，在捷联惯导系统初始对准中也可以根据平台罗经初始对准的特点，建立相应的数学平台隔离晃动影响。经典控制理论与现代最优估计方法相比，前者的优点之一是勿需精确的数学模型与噪声模型，应用经典控制理论进行罗经对准的设计方法已经非常成熟，为捷联罗经对准方案设计提供了大量的参考，然而初始对准时间长是平台罗经的一大缺点。快速初始对准是国内在捷联罗经对准方法研究中亟待解决的一个主要问题，该问题在某些西方国家已得到较好解决，例如法国iXSea公司的OctansIII型光纤陀螺罗经在动态环境下，能在3min内完成初始对准，达到0.2o×sec(L)的精度[5]，成为捷联罗经研究与应用中的佼佼者，它为我们的研究和工程开发目标提供了参考。 本文从分析平台罗经初始对准的原理出发，提出了捷联罗经初始对准的原理并推导了便于软件编程的算法，通过对大方位误差角捷联惯导非线性误差方程的简化，推导了粗略方位 1本课题得到水下信息处理与控制国家级重点实验室基金（9140C230206070C2306）的资助。
第27卷 第1期航 空 学 报 Vo l 27No 1 2006年 1月ACT A A ERON A U T ICA ET A ST RO N AU T ICA SIN ICA Jan. 2006 收稿日期:2004 09 30;修订日期:2005 04 27基金项目:国家自然科学基金(60234030)、国家杰出青年科学基金 (60225015)和教育部高校青年教师奖资助项目 文章编号:1000 6893(2005)01 0098 06捷联惯导姿态算法中的圆锥误差与量化误差 练军想,胡德文,胡小平,吴文启 (国防科技大学机电工程与自动化学院自动控制系,湖南长沙 410073) Research on C oning Error and Quantization Error of SINS Attitude Algorithm LIAN Jun x iang ,H U De w en,H U Xiao ping ,WU W en qi (Department of A utomatic Contr ol,Colleg e of M echat ronics and A utomation,N atio nal U niver sity of Denfense T echno log y,Chang sha 410073,China) 摘 要:对捷联惯导系统的误差源进行了研究,利用几何方法分析了不可交换性误差和量化误差的形成机理,以及它们的相互影响。针对工程应用中激光陀螺输出脉冲采样量化条件,就多子样算法进行了讨论,并设计了基于M A T L A B/Simulink 的仿真。研究结果表明,当考虑量化误差的影响时,选取适当的量化因子,三子样等效旋转矢量算法比其它算法具有更好的综合性能。 关键词:捷联惯导;姿态算法;不可交换性误差;量化误差;多子样中图分类号:V 249.3 文献标识码:A Abstract:In this paper,the er ror so urces o f str apdow n iner tial nav igat ion system (SIN S)are r esear ched.T he mechanism of nonco mmut ativity er ro r and quantizatio n er ro r is illustr ated fr om the g eometrical point of v iew,and the mutual influence betw een them is analy zed.Considering the sample quantizing co ndition of the o utput pulses of the r ing laser g yro sco pe (R LG ),the multi sample att itude alg or ithm o f SIN S is discussed.T he simu latio n is car ried out using M A T L A B/Simulink.It is co ncluded that the three sam ple rotat ion vector alg or ithm wit h t he pro per quant izat ion factor outper for ms o thers when the impact of quant izat ion err ors is taken into ac count. Key words:SI NS;attitude algo rithm;no nco mmutat ivit y er ror ;quantizatio n er ro r;multi sample 对于捷联惯导系统姿态更新算法的研究,目 前国内外同行们有很多成果。普遍的观点认为[1,2]:四元数姿态更新算法比方向余弦和欧拉角等其它方法的计算量小、适应性好;双子样、三子样等效旋转矢量算法在抑制不可交换性误差方面比单子样有显著改善;将姿态更新分成快慢两个回路,即快速回路更新旋转矢量,慢速回路更新姿态参数,既能有效抑制不可交换性误差,还可以减少算法的计算量。但上述观点很少有结合捷联惯导系统工程实际来考虑的。本文针对工程中激光陀螺输出脉冲采样量化条件,就多子样算法展开研究,并得出了相应的结论。 1 误差源分析 评价算法优劣的最重要的标准之一是算法的 精度。要提高算法的精度就必须研究各种误差及其补偿、抑制方法。 在对惯导系统作原理性分析时,尚可以理想化地认为平台坐标系(平台式或捷联式)准确、无误差地模拟地理坐标系,但实际的惯导系统不可避免地受到各种误差的影响[3]。通常,可将误差分为源信息误差和解算方法误差。对捷联惯导系统,解算方法误差是指数值求解姿态矩阵和比力方程时产生的原理性误差,比如不可交换性误差、舍入误差、截断误差等。源信息误差可分为解析源信息误差和测量源信息误差。测量源信息误差就是惯性传感器的误差,此类误差因惯性器件的物理原理和仪表的精度不同而各不相同。比如陀螺的常值漂移和随机漂移、刻度因子误差、非线性误差、输出值的量化误差等。 下面重点对捷联姿态解算时不可交换性误差和陀螺采样的量化误差进行分析。1 1 不可交换性误差 在利用方向余弦矩阵微分方程或四元数微分 方程进行姿态更新时,都会遇到角速度矢量的积分问题。由于刚体有限转动的不可交换性,当转
一、原理分析： 捷联式惯导系统是将惯性器件（陀螺仪和加速度计）直接固连在载体上的系统。图1为捷联式惯导系统的原理图，陀螺仪和加速度计输出分别送入姿态矩阵计算和由载体坐标系至平台坐标系的方向余弦矩阵的计算。有了姿态矩阵，其一可以实现把载体坐标系轴向加速度信息变换到导航坐标系轴，进而可以进行所需的导航参数计算，其二利用姿态矩阵的元素，提取方位和姿态信息。 图1. 捷联式惯导系统的原理图 姿态速率微分方程为: 12b tb ωΛ=Λ (1) 其中； () b b b t t tb ib t ie et C ωωωω=-- (2) b ib ω为陀螺仪测量经补偿后的值；
0cos sin t iex t t ie iey ie t ie iez L L ωωωωωω?? ? ????? ??==???????????? ，为地球自转角速率； tan t ety t yt etx t t t etx et ety xt t etz t etx xt V R V R V L R ωωωω??-?? ? ???? ????? ??==?????????????????? ，为地理坐标系相对地球坐标系的转动角速率； 导航坐标系到载体坐标系的姿态矩阵为： 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 t t C ψ?ψθ? ψ?ψθ? θ?ψθ ψθ θ ψ?ψθ? ψ?ψθ? θ?-+-?? ?? =-?? ??+-?? (3) 对应的四元素初值为: 0123cos cos cos sin sin sin 2 2 2 22 2 cos sin cos sin cos sin 2 2 2 2 2 2 cos cos sin sin sin cos 2 2 2 2 2 2 cos sin sin sin cos cos 2 2 2 2 2 2ψ θ ? ψ θ ? λψ θ ? ψθ ? λψ θ ? ψ θ ? λψ θ ? ψ θ ? λ? =-???=-???=+???=+? (4) 四元素姿态矩阵为： 22220123120313022 2 2 2 12030123 230122221302230101232() 2()2() 2()2() 2() b t C λλλλλλλλλλλλλλλλλλλλλλλλλλλλλλλλλλλλ?? +++++?? =--+-+????----+? ? (5) 将姿态速率微分方程展开成矩阵形式： 0112233001020b b b tbx tby tbx b b b tbx tbz tby b b b tby tbz tbx b b b tbz tby tbx λλωωωλλωωωωωωλλωωωλλ???? ??---??????-??????=??????-??????-???????????? (6)
捷联式惯导系统初始对准 惯性技术是惯导(惯性导航与惯性制导)技术、惯性仪表技术、惯性测量技术以及有关设备和装置技术的统称。惯性导航与惯性制导是当今非常重要的综合技术之一，它广泛用于航空、航海、航天及陆地各领域。惯性导航系统是和用陀螺与加速度计通过最初的方向基准和位置信息来确定运载体在一特定坐标系内的姿态、位置、速度和加速度的自主式导航系统。惯性制导系统是利用运载体内部的陀螺、加速度计测量其运动参数，经过计算机发出控制指令，从而把运载体按照预定的路线准确地引导到目的地的制导系统。自主性是惯性系统最重要的特点。确定运动对象导航参数的方法和仪器有许多，例如磁、天文、无线电、水声、全球卫星定位系统等等，然而它们都有一个致命的弱点，即不是自主的，不是要向外界发出信息，就是要依赖对外观测信息，而惯性系统与上述诸方法的基本区别就在于是完全自主的，即导弹、潜艇、飞船等可以在一个完全与外界条件以及电磁波隔绝的假想“封闭”空间内实现精确导航。因此，惯导系统具有隐蔽性好、抗干扰、不受任何气象条件限制的优点，且数据更新速率高，可以提供连续实时的导航参数。 惯性系统在国防科学技术中占有非常重要的地位，因而是世界各工业强国重点发展的技术领域之一。随着惯性技术的不断发展，许多国家已将其应用领域扩大到现代化交通运输，海洋开发，大地测量与勘探，石油钻井，矿井、隧道的掘进与贯通，机器人控制，现代化医疗器械，摄影技术以及森林防护，农业播种、施肥等民用领域。惯性技术的发展表明：从传统的机械转子型陀螺向固态陀螺仪(激光、光纤陀螺仪)转移，并进一步向以半导体硅为基本材料的微机械振动陀螺发展；从框架式平台系统向捷联系统转移，从纯惯性捷联系统向以惯性系统为基础的多体制组合导航系统发展，成为今后惯性技术发展的总趋势。 捷联式惯性导航系统，导航用的加速度计是直接捆绑在运载体上，它测量的是运载体坐标系轴向比力，只要把这个比力转换到惯性坐标系上，则其他计算就和空间稳定的平台式惯性导航系统一样，而比力转换的关键就是要实时地进行姿态基准计算来提供数学平台，即实时更新姿态矩阵b C，有些资料上称姿态矩阵 g 为捷联矩阵或方向余弦矩阵b C。一般选择地理坐标系为导航坐标系，那么捷联 g 矩阵也可表示为i C，其导航原理图如下所示： g
%====本程序为捷联惯导的解算程序(由惯性器件的输出解算出飞行器的位置、速度、姿态信息)====== clear all; close all; clc; deg_rad=pi/180; %由度转化成弧度 rad_deg=180/pi; %由弧度转化成度 %-------------------------------从源文件中读入数据 ---------------------------------- fid_read=fopen('IMUout.txt','r'); %path1_Den.dat 是由轨迹发生器产生的数据 [AllData NumofAllData]=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轴构成右手坐标系单位：米/秒2 fb_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); %初始位置
第二章 捷联惯导系统的初试对准 2.1引言 惯导系统是一种自主式导航系统。它不需要任何人为的外部信息，只要给定导航的初始条件（例如初始速度、位置等），便可根据系统中的惯性敏感元件测量的比力和角速率通过计算机实时地计算出各种导航参数。由于“平台”是测量比力的基准，因此“平台”的初始对准就非常重要。对于平台惯导系统，初试对准的任务就是要将平台调整在给定的导航坐标系的方向上。若采用游动方位系统，则需要将平台调水平---称为水平对准，并将平台的方位角调至某个方位角处---称为方位对准。对于捷联惯导系统，由于捷联矩阵T 起到了平台的作用，因此导航工作一开始就需要获得捷联矩阵T 的初始值，以便完成导航的任务。显然捷联惯导系统的初始对准就是确定捷联矩阵的初始值。在静基座条件下，捷联惯导系统的加速度计的输入量为---b g ，陀螺的输入量为地球自转角速率b ie ω。因此b g 与 b ie ω就成为初始对准的基准。将陀螺与加速度计的输入引出计算机，通过计算机 就可以计算出捷联矩阵T 的初始值。 由以上的分析可以看出，陀螺与加速度计的误差会导致对准误差；对准飞行器的干扰运动也是产生对准误差的重要因素。因此滤波技术对捷联系统尤其重要。由于初始对准的误差将会对捷联惯导系统的工作造成难以消除的影响，因此研究初始对准的误差传播方程也是非常必要的。 2.2 捷联惯导系统的基本工作原理 捷联式惯性导航系统，陀螺仪和加速度计直接与载体固联，加速度计测量是载体坐标系轴向比力，只要把这个比力转换到导航坐标系上，则其它计算就与平台式惯性导航系统一样，而比力转换的关键就是要实时地进行姿态基准计算来提供数学平台，即实时更新姿态矩阵n b C ，姿态矩阵也称为捷联矩阵。一般选择地理坐标系为导航坐标系，那么捷联矩阵n b C 也可表示为t b C ， 其导航原理图如图2.1所示。
1 绪论 随着计算机和微电子技术的迅猛发展，利用计算机的强大解算和控制功能代替机电稳定系统成为可能。于是，一种新型惯导系统--捷联惯导系统从20世纪60年代初开始发展起来，尤其在1969年，捷联惯导系统作为"阿波罗"-13号登月飞船的应急备份装置，在其服务舱发生爆炸时将飞船成功地引导到返回地球的轨道上时起到了决定性作用，成为捷联式惯导系统发展中的一个里程碑。 捷联式惯性导航(strap-down inertial navigation) ，捷联（strap-down）的英语原义是“捆绑”的意思。因此捷联式惯性导航也就是将惯性测量元件（陀螺仪和加速度计）直接装在飞行器、舰艇、导弹等需要诸如姿态、速度、航向等导航信息的主体上，用计算机把测量信号变换为导航参数的一种导航技术。现代电子计算机技术的迅速发展为捷联式惯性导航系统创造了条件。惯性导航系统是利用惯性敏感器、基准方向及最初的位置信息来确定运载体的方位、位置和速度的自主式航位推算导航系统。在工作时不依赖外界信息，也不向外界辐射能量，不易受到干扰破坏。它完全是依靠载体自身设备独立自主地进行导航，它与外界不发生任何光、声、磁、电的联系，从而实现了与外界条件隔绝的假想的“封闭”空间内实现精确导航。所以它具有隐蔽性好，工作不受气象条件和人为的外界干扰等一系列的优点，这些优点使得惯性导航在航天、航空、航海和测量上都得到了广泛的运用[1] 1.1 捷联惯导系统工作原理及特点 惯导系统主要分为平台式惯导系统和捷联式惯导系统两大类。惯导系统（INS）是一种不依赖于任何外部信息、也不向外部辐射能量的自主式导航系
统，具有隐蔽性好，可在空中、地面、水下等各种复杂环境下工作的特点。 捷联惯导系统（SINS）是在平台式惯导系统基础上发展而来的，它是一种无框架系统，由三个速率陀螺、三个线加速度计和微型计算机组成。平台式惯导系统和捷联式惯导系统的主要区别是：前者有实体的物理平台，陀螺和加速度计置于陀螺稳定的平台上，该平台跟踪导航坐标系，以实现速度和位置解算，姿态数据直接取自于平台的环架；后者的陀螺和加速度计直接固连在载体上作为测量基准，它不再采用机电平台，惯性平台的功能由计算机完成，即在计算机内建立一个数学平台取代机电平台的功能，其飞行器姿态数据通过计算机计算得到，故有时也称其为"数学平台"，这是捷联惯导系统区别于平台式惯导系统的根本点。由于惯性元器件有固定漂移率，会造成导航误差，因此，远程导弹、飞机等武器平台通常采用指令、GPS或其组合等方式对惯导进行定时修正，以获取持续准确的位置参数。如采用指令+捷联式惯导、GPS+惯导（GPS/INS）。美国的战斧巡航导弹采用了GPS+INS +地形匹配组合导航。 惯导系统基本工作原理是以牛顿力学定律为基础，通过测量载体在惯性参考系的加速度，将它对时间进行积分，之后将其变换到导航坐标系，得到在导航坐标系中的速度、偏航角和位置信息等。对捷联惯导系统而言，平台的作用和概念体现在计算机中，它是写在计算机中的方向余弦阵。直接安装在载体上的惯性元件测得相对惯性空间的加速度和角加速度是沿载体轴的分量，将这些分量经过一个坐标转换方向余弦阵，可以转换到要求的计算机坐标系内的分量。如果这个矩阵可以描述载体和地理坐标系之间的关系，那么载体坐标系测得的相对惯性空间的加速度和角速度，经过转换后便可得到沿地理坐标系的加速度和角速度分量，有了已知方位的加速度和角速度分量之后，导航计算机便
车载捷联惯导系统基本原理 一、捷联惯导系统基本原理 捷联惯导系统基本原理如图2-1所示： 图中陀螺和加速度计直接与载体系b固联，用来测量载体的角运动信息和线运动信息。导航解算的本质是根据初值进行积分的过程，通过求解姿态微分方程完成对姿态和航向角的积分，通过求解比力微分方程完成对速度的积分，通过求解位置微分方程实现对位置的积分。捷联惯导的姿态矩阵C n 相当于“数学平台”，取代了平台惯导中的实体平台，而ω?相当于对数学平台“施矩”的指令角速率。
二、捷联惯导微分方程 （一）姿态微分方程 在捷联惯导系统中，导航坐标系n 和载体坐标系b 之间的角位置关系通常用姿态矩阵、四元数和欧拉角表示，相应也存在姿态矩阵微分方程、四元数微分方程和欧拉角微分方程三种形式。 姿态矩阵微分方程的表达式为：
在欧拉角微分方程式(2.2-7)中，当俯仰角θ趋于90o时，cosθ趋于0，tanθ趋于无穷，方程存在奇异性，所以这种方法不能在全姿态范围内正常工作；姿态矩阵微分方程式(2.2-1)可全姿态工作，但姿态矩阵更新相当于求解包含9个未知量的线性微分方程组，计算量大；四元数微分方程式(2.2-6)同样可以全姿态工作，且更新算法只需求解4个未知量的线性微分方程组，计算量小，算法简单，是较实用的工程算法。 （二）速度微分方程 速度微分方程即比力方程，是惯性导航解算的基本关系式： 三、捷联惯性导航算法 捷联惯导解算的目的是根据惯性器件输出求解载体姿
态、速度和位置等导航信息，实际上就是求解三个微分方程的过程，相应存在姿态更新算法、速度更新算法和位置更新算法。 （一）姿态更新算法 求解微分方程式(2.2-6)可得四元数姿态更新算法为：