@捷联惯导系统姿态解算模块的实现---具体实现过程

1 绪论随着计算机和微电子技术的迅猛发展，利用计算机的强大解算和控制功能代替机电稳定系统成为可能。

于是，一种新型惯导系统--捷联惯导系统从20世纪60年代初开始发展起来，尤其在1969年，捷联惯导系统作为"阿波罗"-13号登月飞船的应急备份装置，在其服务舱发生爆炸时将飞船成功地引导到返回地球的轨道上时起到了决定性作用，成为捷联式惯导系统发展中的一个里程碑。

捷联式惯性导航(strap-down inertial navigation)，捷联（strap-down）的英语原义是“捆绑”的意思。

因此捷联式惯性导航也就是将惯性测量元件（陀螺仪和加速度计）直接装在飞行器、舰艇、导弹等需要诸如姿态、速度、航向等导航信息的主体上，用计算机把测量信号变换为导航参数的一种导航技术。

现代电子计算机技术的迅速发展为捷联式惯性导航系统创造了条件。

惯性导航系统是利用惯性敏感器、基准方向及最初的位置信息来确定运载体的方位、位置和速度的自主式航位推算导航系统。

在工作时不依赖外界信息，也不向外界辐射能量，不易受到干扰破坏。

它完全是依靠载体自身设备独立自主地进行导航，它与外界不发生任何光、声、磁、电的联系，从而实现了与外界条件隔绝的假想的“封闭”空间内实现精确导航。

所以它具有隐蔽性好，工作不受气象条件和人为的外界干扰等一系列的优点，这些优点使得惯性导航在航天、航空、航海和测量上都得到了广泛的运用[1]1.1 捷联惯导系统工作原理及特点惯导系统主要分为平台式惯导系统和捷联式惯导系统两大类。

惯导系统（INS）是一种不依赖于任何外部信息、也不向外部辐射能量的自主式导航系统，具有隐蔽性好，可在空中、地面、水下等各种复杂环境下工作的特点。

捷联惯导系统（SINS）是在平台式惯导系统基础上发展而来的，它是一种无框架系统，由三个速率陀螺、三个线加速度计和微型计算机组成。

平台式惯导系统和捷联式惯导系统的主要区别是：前者有实体的物理平台，陀螺和加速度计置于陀螺稳定的平台上，该平台跟踪导航坐标系，以实现速度和位置解算，姿态数据直接取自于平台的环架；后者的陀螺和加速度计直接固连在载体上作为测量基准，它不再采用机电平台，惯性平台的功能由计算机完成，即在计算机内建立一个数学平台取代机电平台的功能，其飞行器姿态数据通过计算机计算得到，故有时也称其为"数学平台"，这是捷联惯导系统区别于平台式惯导系统的根本点。

捷联惯性导航系统的解算方法捷联惯性导航系统（Inertial Navigation System，简称INS）是一种利用陀螺仪和加速度计等惯性测量单元测量物体的加速度和角速度，然后通过对这些测量值的积分计算出物体的速度和位置的导航系统。

INS广泛应用于航空航天、无人驾驶车辆和船舶等领域，具有高精度和自主性等特点。

INS的解算方法一般分为初始对准、运动状态估计和航位推算三个主要过程。

初始对准是指在启动导航系统时，通过利用外部辅助传感器（如GPS）或静态校准等方法将惯性传感器的输出与真实姿态和位置进行初次校准。

在初始对准过程中，需要获取传感器的初始偏差和初始姿态，一般采用标定或矩阵运算等方法进行。

运动状态估计是指根据惯性传感器的测量值，使用滤波算法对物体的加速度和角速度进行实时估计。

常用的滤波算法包括卡尔曼滤波、扩展卡尔曼滤波和粒子滤波等。

其中，卡尔曼滤波是一种最优估计算法，通过对观测值和状态进行线性组合，得到对真实状态的最佳估计。

扩展卡尔曼滤波则是基于卡尔曼滤波的非线性扩展，可以应用于非线性INS系统。

粒子滤波是一种利用蒙特卡洛采样技术进行状态估计的方法，适用于非高斯分布的状态估计问题。

航位推算是指根据运动状态估计的结果，对物体的速度和位置进行推算。

INS最基本的航位推算方法是利用加速度值对速度进行积分，然后再对速度进行积分得到位置。

但是，在实际应用中，由于传感器本身存在噪声和漂移等误差，导致航位推算过程会出现积分漂移现象。

为了解决这个问题，通常采用辅助传感器（如GPS）和地图等数据对INS的输出进行校正和修正。

当前，还有一些先进的INS解算方法被提出，如基于深度学习的INS 解算方法。

这些方法利用神经网络等深度学习模型，结合原始传感器数据进行端到端的学习和预测，以实现更高精度的位置和姿态估计。

综上所述，捷联惯性导航系统的解算方法主要包括初始对准、运动状态估计和航位推算三个过程。

其中，运动状态估计过程利用滤波算法对传感器的测量值进行处理，得到物体的加速度和角速度的估计。

%====本程序为捷联惯导的解算程序(由惯性器件的输出解算出飞行器的位置、速度、姿态信息)======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('时间(秒)');%------------------------------绘图结束-------------------------------。

捷联式惯性导航积分算法设计（连载）上篇：姿态计算
卢军政
【期刊名称】《江南航天科技》
【年(卷),期】2001(000)002
【摘要】本论文分上下两篇，给用于现代捷联惯导系统的主要软件算法设计提供
一个严密的综合方法：将角速率积分成姿态角，将加速度变换或积分成速度以及将速度积分成位置。

该算法是用两速修正法构成的，而两速修正法是具有一定创新程度的新颖算法，是为姿态修正而开发出来的，在姿态修正中，以中速运用精密解析方程去正积分参数（姿态，速度或位置），其输入是由在参数修正（姿态锥化修正，速度遥橹修正以及高分辨率位置螺旋修正）时间间隔内计算运动角速度和加速度的高速算法提供的，该设计方法考虑了通过捷联系统惯性传感器对角速度或比力加速度所进行的测量以及用于姿态其准和矢量速度积分的导航系旋转问题。

本论文上篇定义了捷联惯导积分函数的总体设计要求，并开发出了用于姿态修正算法的方向余弦法和四元数法，下篇着重讨论速度和位置积分算法的设计。

尽管上下两篇讨论中常常涉及到基本的惯性导航概念。

然而，本论文是为那些已对基础惯导概念很熟悉的实际工作者而写的。

【总页数】20页(P14-33)
【作者】卢军政
【作者单位】江南航天集团302所
【正文语种】中文
【中图分类】V249.322
【相关文献】
1.惯性导航系统航向姿态计算算法研究
2.捷联式惯性导航计算机控制系统
3.捷联惯性导航积分算法设计--第一部分：姿态算法
4.捷联惯性导航积分算法设计
5.捷联惯性导航积分算法设计
因版权原因，仅展示原文概要，查看原文内容请购买。

2013年第6期 导 弹 与 航 天 运 载 技 术 No.6 2013 总第329期 MISSILES AND SPACE VEHICLES Sum No.329收稿日期：2012-10-21；修回日期：2013-08-12基金项目：民用航天专业技术预先研究项目（D010101）作者简介：刘生炳（1986-），男，工程师，主要从事导航、制导与控制专业研究文章编号：1004-7182(2013)06-0060-04 DOI ：10.7654/j.issn.1004-7182.20130614捷联惯导系统全姿态初始对准方法刘生炳，魏宗康，陈东生，吴 涛（北京航天控制仪器研究所，北京100039）摘要：捷联惯导系统开始导航解算时需要初始对准，工程中常用的静基座初始对准方法有基于克雷洛夫角的静基座初始对准、基于克雷洛夫角的四元数初始对准。

前者由于需要首先求解克雷洛夫角，因此存在与旋转顺序相关和不能全姿态工作的问题；后者需要首先求解克雷洛夫角，然后求解四元数，因此同样存在不能全姿态工作的问题。

针对上述问题，提出了一种避免求解克雷洛夫角，直接通过四元数姿态变换矩阵求得姿态四元数的初始对准方法。

仿真验证结果表明：四元数直接求解初始对准方法可以完成捷联系统静基座初始对准，克服与旋转顺序相关的问题，并且可以实现全姿态初始对准。

关键词：捷联惯导系统；静基座初始对准；克雷洛夫角；四元数 中图分类号：V448.22 文献标识码：AAll Attitude Initial Alignment of Strapdown Inertial Navigation SystemLiu Shengbing, Wei Zongkang, Chen Dongsheng, Wu Tao(Beijing Institute of Aerospace Control Devices, Beijing, 100039)Abstract: It is necessary to perform initial alignment before using SINS, immobile platform initial alignment based on Krylov angle is usually used in project. However, the method is associated with order of rotation, and is unable to be used in all attitude calculation. The other method used in attitude calculation is quaternion initial alignment based on Krylov angle. As it needs to calculate the Krylov angle at the first step, and then calculate quaternion, therefore this method is also unable to be used in all attitude calculation. In order to solve this problem, a new method that can avoid calculating Krylov angle and directly obtain quaternion through attitude transform matrix is proposed. The simulation result shows that the new method can complete initial alignment in SINS and is independent of order rotation, and also can be used in all attitude initial alignment.Key Words: Strapdown inertial navigation system (SINS); Immobile platform initial alignment; Krylov angle; Quaternion0 引 言目前，在捷联惯性系统姿态角解算中主要有以下3种方法：方向余弦法、欧拉-克雷罗夫角法以及四元数法。

组合导航系统是将载体( 飞机、舰船等) 上导航设备组合成一个统一系统，利用两种或两种以上设备提供多重信息，构成一个多功能、高精度冗余系统。

组合导航系统有利于充分利用各导航系统进展信息互补与信息合作, 成为导航系统开展方向。

在所有组合导航系统中，以北斗与惯性导航系统INS 组合系统最为理想, 而深组合方式是北斗与惯性导航系统( INS) 组合最优方法。

鉴于GPS 不可依赖性，北斗卫星导航系统与INS 组合是我国组合导航系统开展趋势，我国自主研制北斗/INS深组合导航系统需要解决关键技术。

1北斗/惯导深组合导航算法深组合导航算法是由INS导航结果推算出伪距、伪距率，与北斗定位系统观测得到伪距、伪距率作差得到观测量。

通过卡尔曼滤波对INS误差和北斗接收机误差进展最优估计，并根据估计出INS误差结果对INS进展反应校正, 使INS保持高精度导航。

同时利用校正后INS 速度信息对北斗接收机载波环、码环进展辅助跟踪, 消除载波跟踪环和码跟踪环中载体大局部动态因素, 以降低载波跟踪环和码跟踪环阶数，从而减小环路等效带宽, 增加北斗接收机在高动态或强干扰环境下跟踪能力。

其组合方式如图1所示，图中只画出了北斗一个通道，其他通道均一样。

图 1 深组合方式框图组合导航参数估计是组合导航系统研究关键问题之一。

经典Kalman 滤波方法是组合导航系统中使用最广泛滤波方法，但由于动态条件下组合导航系统状态噪声和量测噪声统计信息不准确，常导致滤波精度下降，影响组合导航性能。

滤波初值选取与方差矩阵初值对滤波结果无偏性和稳定性有较大影响，不恰中选择可能导致滤波过程收敛速度慢，甚至有可能发散。

另外系统误差模型不准确也会导致滤波过程不稳定。

渐消记忆自适应滤波方法通过调节新量测值对估计值修正作用来减小系统误差模型不准确对滤波过程影响。

当系统模型不准确时，增强旧测量值对估计值修正作用，减弱新测量值对估计值修正作用。

因此我们提出了以模糊控制规那么为根底渐消记忆自适应卡尔曼滤波方法。

1.初始对准得到载体的姿态角后，由欧拉角得到四元数初始值：其中q=a+bi+cj+dk，[a,b,c,d]也即[q0,q1,q2,q3]。

2.由四元数初始值构造姿态转移矩阵nc——载体坐标系b系到导航b坐标系（也即地理坐标系）n系的转移矩阵其中，俯仰角θ，滚转角φ，航向角ψ3.由n b C求出e b Cn b e n e b C C C =其中：其中，ϕ和λ分别为纬度和经度。

4. 由eb C 反求e 系下四元数的初始值[a,b,c,d]也即[q0,q1,q2,q3]。

5. 由四元数按时间传递的特性：也即：⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡⎥⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎢⎣⎡------=⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡32103210000021q q q q q q q q xyzx zyy zx z yx ωωωωωωωωωωωω （2-24） 利用毕卡逼近法求解可得：()()()*120M dt q t e q t ω⎰= （2-25）令：[]()*0000x y z xz y y z x zyxM dt θθθθθθθωθθθθθθ-∆-∆-∆⎡⎤⎢⎥∆∆-∆⎢⎥∆==⎢⎥∆-∆∆⎢⎥∆∆-∆⎢⎥⎣⎦⎰ （2-26） （2-25）可简写为：()[]()120q t e q θ∆= （2-27）将[]12eθ∆展开可得：()[][][][]()231111022!23!2!2nq t I q n θθθθ⎡⎤⎛⎫⎛⎫⎛⎫∆∆∆⎢⎥=+∆+++++ ⎪ ⎪ ⎪⎢⎥⎝⎭⎝⎭⎝⎭⎣⎦（2-28） 由于：[][][][]224222203040x y z I IIθθθθθθθθθθ⎡⎤∆==-∆+∆+∆=-∆⎣⎦∆=-∆∆∆=-∆ （2-29） 将式（2-29）代入式（2-28）整理可得：[]000sin 2()cos (0)2q t I q θθθθ∆⎧⎫⎪⎪∆=+∆⎨⎬∆⎪⎪⎩⎭（2-30） 在实际解算中，把0cos 2θ∆和0sin 2θ∆展为级数形式并取有限项，得四元数的各阶近似算法。

捷联惯导系统的算法研究及其仿真实现Study and Simulation of Strapdown Inertial Navigation System1.1.3捷联惯导系统的发展趋势捷联式惯导系统是从20世纪60年代初开始发展起来的。

20世纪70年代以来，作为捷联系统的核心部件—惯性测量装置和计算机技术有了很大发展，而电子技术、计算机技术、现代控制理论的不断进步，为捷联惯性技术的发展创造了有利条件。

在硬件方面，新一代惯性器件如激光陀螺、光纤陀螺的成功研制，为捷联惯导的飞速发展打下了物质基础。

进入20世纪80-90年代，在航天飞机、宇宙飞船、卫星等民用领域及各种战略、战术导弹、军用飞机、反潜武器、作战舰艇等军事领域开始采用动力调谐式陀螺、激光陀螺和光纤式陀螺的捷联惯导系统。

其中激光陀螺和光纤式陀螺是捷联惯导系统的理想器件。

激光陀螺具有角速率动态范围宽、对加速度和震动不敏感、不需温控、启动时间特别短和可靠性高等优点。

激光陀螺惯导系统己在波音757/767、A310民机以及F-20战斗机上试用，精度达到 1.85km/h 的量级。

20世纪90年代，激光陀螺惯导系统估计占到全部惯导系统的一半以上，其价格与普通惯导系统差不多，但由于增加了平均故障间隔时间，其寿命期费用只有普通惯导系统的15%-20%。

光纤陀螺实际上是激光陀螺中的一种，其原理与环型激光陀螺相同，它克服了由激光陀螺闭锁带来的负效应，具有检测灵敏度和分辨率极高、启动时间极短、动态范围极宽、结构简单、零部件少体积小、造价低、可靠性高等优点。

采用光纤陀螺的捷联航姿系统已用于战斗机的机载武器系统及波音777飞机中。

波音777由于采用了光纤陀螺的捷联惯导系统，其平均故障间隔时间可高达20000h。

采用光纤陀螺的捷联惯导系统被认为是一种极有发展前途的导航系统。

而随着航空航天技术的发展及新型惯性器件关键技术的陆续突破，捷联惯导系统的可靠性、精度将会更高。

捷联式惯性导航原理捷联式惯性导航（Inertial Navigation System，简称INS）是一种基于惯性测量装置的导航系统。

它通过测量线性加速度和角速度来得出加速度、速度和位置信息，从而实现航海、航空和航天等领域的精确导航和定位。

捷联式惯性导航系统由多个惯性传感器组成，包括加速度计和陀螺仪。

加速度计用于测量线性加速度，而陀螺仪则用于测量角速度。

这些传感器安装在导航系统的载体上，并与导航系统的计算单元相连。

捷联式惯性导航系统的原理可分为两个主要步骤：传感器测量和姿态解算。

传感器测量是指测量加速度计和陀螺仪输出的信号。

加速度计通过测量导航系统相对于载体的线性加速度来估计速度和位移。

陀螺仪则通过测量导航系统相对于载体的角速度来估计转角和航向。

这些测量值由传感器输出，并发送给导航系统的计算单元进一步处理。

姿态解算是指根据传感器测量值计算导航系统相对于载体的三维方向。

这个过程基于四元数算法和方向余弦矩阵等数学模型。

根据加速度计的测量值，可以得到系统的重力矢量，从而计算出系统相对于地球的姿态。

陀螺仪的测量值则用于校正角速度误差和姿态的漂移。

通过不断地积分和更新测量值，导航系统可以保持准确的姿态信息。

捷联式惯性导航系统的优势在于其自主性和抗干扰能力。

由于不依赖于外部信号源，如卫星或地面控制点，INS可以在任何环境中进行导航。

同时，由于惯性传感器对外部扰动的响应速度很快，导航系统可以及时纠正估计误差，从而实现高精度的导航和定位。

然而，捷联式惯性导航系统也存在一些缺点。

由于惯性传感器存在漂移和积分误差，INS的导航信息随着时间的推移会变得不准确。

此外，惯性传感器的准确性和稳定性也会受到温度、振动和电磁干扰等因素的影响。

为了解决这些问题，通常需要与其他导航系统，如全球定位系统（GPS）或地面测量系统（如激光测距仪），进行组合导航。

总的来说，捷联式惯性导航系统是一种基于惯性传感器测量的导航系统。

它通过测量线性加速度和角速度，计算出加速度、速度和位置信息。

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个一阶微分方程式，计算量大。

捷联惯导系统姿态算法比较
邢质皙;王爱民
【期刊名称】《舰船电子工程》
【年(卷),期】2009(029)007
【摘要】姿态算法是捷联惯导系统算法中的一个重要组成部分,解算姿态阵相当于建立起数学平台,其精度对捷联惯导系统的精度影响很大.该文就实际应用,对四元数毕卡迭代法、四元数四阶龙格-库塔算法、圆锥补偿算法和一种改进的圆锥补偿算法进行了分析和比较,并在典型圆锥运动下进行了仿真,仿真结果表明,四阶龙格-库塔算法具有很高的工程实用价值.
【总页数】4页(P81-84)
【作者】邢质皙;王爱民
【作者单位】东南大学仪器科学与工程学院,南京,210096;东南大学仪器科学与工程学院,南京,210096
【正文语种】中文
【中图分类】U666.12
【相关文献】
1.红外旋转弹导引头捷联惯导系统的姿态算法研究 [J], 王业卿;黄建新;杨雨;槐超
2.基于双轴转台的捷联惯导系统姿态精度评定算法 [J], 张和杰;李京书
3.枪炮弹微型惯导系统捷联姿态控制算法研究 [J], 高凤;李正喜
4.捷联惯导系统姿态更新旋转矢量算法的优化研究 [J], 马得草
5.捷联惯导系统姿态算法比较 [J], 孙丽;秦永元
因版权原因，仅展示原文概要，查看原文内容请购买。