@捷联惯导系统姿态解算模块的实现---具体实现过程
捷联式惯性导航系统
1 绪论随着计算机和微电子技术的迅猛发展，利用计算机的强大解算和控制功能代替机电稳定系统成为可能。
于是，一种新型惯导系统--捷联惯导系统从20世纪60年代初开始发展起来，尤其在1969年，捷联惯导系统作为"阿波罗"-13号登月飞船的应急备份装置，在其服务舱发生爆炸时将飞船成功地引导到返回地球的轨道上时起到了决定性作用，成为捷联式惯导系统发展中的一个里程碑。
捷联式惯性导航(strap-down inertial navigation)，捷联（strap-down）的英语原义是“捆绑”的意思。
因此捷联式惯性导航也就是将惯性测量元件（陀螺仪和加速度计）直接装在飞行器、舰艇、导弹等需要诸如姿态、速度、航向等导航信息的主体上，用计算机把测量信号变换为导航参数的一种导航技术。
现代电子计算机技术的迅速发展为捷联式惯性导航系统创造了条件。
惯性导航系统是利用惯性敏感器、基准方向及最初的位置信息来确定运载体的方位、位置和速度的自主式航位推算导航系统。
在工作时不依赖外界信息，也不向外界辐射能量，不易受到干扰破坏。
它完全是依靠载体自身设备独立自主地进行导航，它与外界不发生任何光、声、磁、电的联系，从而实现了与外界条件隔绝的假想的“封闭”空间内实现精确导航。
所以它具有隐蔽性好，工作不受气象条件和人为的外界干扰等一系列的优点，这些优点使得惯性导航在航天、航空、航海和测量上都得到了广泛的运用[1]1.1 捷联惯导系统工作原理及特点惯导系统主要分为平台式惯导系统和捷联式惯导系统两大类。
惯导系统（INS）是一种不依赖于任何外部信息、也不向外部辐射能量的自主式导航系统，具有隐蔽性好，可在空中、地面、水下等各种复杂环境下工作的特点。
捷联惯导系统（SINS）是在平台式惯导系统基础上发展而来的，它是一种无框架系统，由三个速率陀螺、三个线加速度计和微型计算机组成。
平台式惯导系统和捷联式惯导系统的主要区别是：前者有实体的物理平台，陀螺和加速度计置于陀螺稳定的平台上，该平台跟踪导航坐标系，以实现速度和位置解算，姿态数据直接取自于平台的环架；后者的陀螺和加速度计直接固连在载体上作为测量基准，它不再采用机电平台，惯性平台的功能由计算机完成，即在计算机内建立一个数学平台取代机电平台的功能，其飞行器姿态数据通过计算机计算得到，故有时也称其为"数学平台"，这是捷联惯导系统区别于平台式惯导系统的根本点。
捷联惯导初始对准以及姿态解算
第三部分：基于“存储数据与迭代计算对准”罗经法对准
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 解算方法。
这些方法利用神经网络等深度学习模型，结合原始传感器数据进行端到端的学习和预测，以实现更高精度的位置和姿态估计。
综上所述，捷联惯性导航系统的解算方法主要包括初始对准、运动状态估计和航位推算三个过程。
其中，运动状态估计过程利用滤波算法对传感器的测量值进行处理，得到物体的加速度和角速度的估计。
捷联惯导系统算法.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
姿态矩阵的计算 假设数学坐标系模拟地理坐标系 飞行器姿态的描述：
航向角ψ、俯仰角θ、滚动角γ 一、欧拉微分方程
从地理坐标系到载体坐标系 的旋转顺序：
Ψ →θ →γ
捷联惯导的解算程序
%====本程序为捷联惯导的解算程序(由惯性器件的输出解算出飞行器的位置、速度、姿态信息)======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种方法：方向余弦法、欧拉-克雷罗夫角法以及四元数法。
捷联惯性导航积分算法设计--第一部分：姿态算法
为 安装 在 坐标 ； Ｊ
∞ ： 坐标 系 Ａ 相对 坐标 系 Ａ。 ＝ 的运 动角速 率 ； 当 Ａ。 为惯 性 系 ｉ ， 时
，
上 的角速 率传感 器所 测得 到 的角速率 。
修 回 日期 ：２ １ － ７ ３ ０ １０ — ０
角速 率／口 力 速度 效 应 （ 姿态 更新 中的 圆锥 效应 ，速 度更 新 中的划船 效 应 ，高分 辨 率位 置更 新 中的 涡卷 效 应 （ 由作 者定 义 的术语 ） ； 另一路 中速 算法 用于 参数 积分 算法 （ 态 ，速度 或位 置 ） ） 姿 ，其
中前 一路 的输 出为后 一路 的输 入。 算 法的设 计步 骤是 对捷 联 惯 导 系统传 感器 测 量得 到 的角速 率／ 比力进 行运 算 ，又 是姿 态 基准 用 导航 坐标 系旋 转及 速度 的积 分运 算 。本 文作 为 第 一部 分 ．定 义 了全 面 的捷联 惯 导 系统 积分 算 法 的设计 需 求 ，给 出 了方 向余 弦矩 阵形 式和 四元数 形 式 的姿 态 更 新算 法。 第二部 分 （ ａａｅ ． ． 捷联 惯 性 导航 积 分算 法设 计第 一部 分 ：姿态 算 法” ｏｒａ ｏ Ｓ ｖｇ ， Ｇ，“ Ｐ ，Ｊｕｎ ｌ ｆ Ｇ ｉａ ｃ ，Ｃ ｎｒｌａ ｄ Ｄ ｎ ｍ ｃ 即将 发 表 ） ｕｄ ｎ ｅ ｏ ｔ ｎ ｙ ａ ｉｓ（ ｏ ）论 述 了速 度 、位 置 积分 算 法 的设 计 方 法 。 第 一 、
， 单位 阵 ： ＿
ｇ 将某 一 四元数 向量 的各 分 量从 Ａ： 坐标 系 转换 到 Ａ 坐标 系的姿 态 四元 数 ；
ＡＩ ＊
，
Ａ
ｇ ：ｇ
Ｉ共 元 ， 一 元 与ｇ的 一 元 相 ， －个 素 对 元 的 轭四 数 第 个 素 ： 第 个 素 同 第２ 元 是ｑ的 应 ４ Ａ ， Ｉ
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
收稿日期 :2008208207 ; 修订日期 :2008209205. 基金项目 : 吉林省杰出青年基金资助项目 ( No . 20060115) ; 中国科学院 “三期创新” 平台资助项目
© 1994-2009 China Academic Journal Electronic Publishing House. All rights reserved.
摘要 : 提出了一种实现捷联惯性导航系统姿态解算模块的方法 。基于 DSP 的硬件平台和四元数的数学平台 ,设计了捷 联惯性导航系统的姿态解算模块 。介绍了捷联惯性导航系统的工作原理和姿态解算的基本算法 , 并给出了四元数法的 四阶龙格2库塔数值解法 。设计了姿态解算模块的硬件电路和软件实现程序 。实验测试结果表明 ,在增量角 < 5° 的情况 下 ,用四阶龙格2库塔法进行姿态解算 , 误差 < 0. 005 3 % ; 应用 TMS320C6713B 进行硬件电路设计 , 每次解算时间 <
和调试 。综上所述 ,DSP 系统具有速度快 、 精度 高、 体积小 、 成本低 、 可靠性高以及抗干扰能力强 等优点 ,可以满足捷联式惯性导航系统对硬件平 台的要求 [ 122 ] 。
© 1994-2009 China Academic Journal Electronic Publishing House. All rights reserved.
,
( 5)
2
K23 T
2
如下 : ψ θ γ ψ θ γ 0 0 0 0 0 0 cos cos cos + sin sin sin 2 2 2 2 2 2 λ (0) ψ θ γ ψ θ γ 0 0 0 0 0 0 cos cos sin - sin sin cos p1 (0) 2 2 2 2 2 2 = , ψ θ γ ψ θ γ p2 (0) 0 0 0 0 0 0 cos sin cos + sin cos sin 2 2 2 2 2 2 p3 (0) ψ θ γ ψ θ γ 0 0 0 0 0 0 sin cos cos - cos sin sin 2 2 2 2 2 2
第 16 卷 第 10 期
2008 年 10 月
Optics and Precision Engineering Oct . 2008
光学 精密工程
Vol. 16 No . 10
文章编号 10042924X ( 2008) 10219562 ,等 : 捷联惯导系统姿态解算模块的实现
1957
1 引 言
捷联惯性导航技术是一门综合性技术 , 用于 对运动体的姿态 、 速度和位置参数的确定 ,该项技 术广泛应用于航天 、 航空 、 航海和大地测量等领 域 。由于捷联惯性导航不受外界电磁和气象环境 干扰 , 不依赖于外界条件 , 并且具有良好的隐蔽 性 ,真正地实现了自主导航 ,成为自主式测量的最 佳手段 ,使得惯性导航技术在军事上具有特殊的 应用价值 。目前 ,在发达国家 ,捷联惯性导航系统 广泛用于机器人 、 无人飞机和精确制导炸弹等飞 行器的导航控制中 。 捷联惯导系统是通过提取陀螺和加速度计的 测量值实时地计算姿态矩阵 , 进而从姿态矩阵的 元素中获得运动载体的姿态和航向信息来进行导 航计算的 。姿态解算算法是捷联惯导系统算法的 核心 ,也是影响捷联惯导系统精度的主要因素之 一 ,因此设计和采用合理的姿态解算算法就成为 需要研究的课题 。 目前 , 国内的飞行器采用的计算机大多 是 冯・ 诺伊曼结构的通用型微处理器 , 这些通用型 微处理器的乘法用软件实现 , 常常需要若干个机 器周期才能完成 ,而且由于同时运行多个进程 ,使 数据处理速度较慢 。并且采用这些微处理器的器 件体积大 ,功耗大 ,电路结构复杂 ,可靠性低 ,成本 高 ,无法更好地满足当代捷联惯性导航系统对小 型化 、 高精度 、 实时性的要求 。 对于嵌入式数字信号处理系统 ,基于数字信 号处理器 ( DSP ) 的实时信号处理系统具有速度 快、 可靠性高 、 成本低 、 开发周期短等特点 。DSP 是专为高速数字信号处理而设计的微处理器 , 其 改进的哈佛结构 、 先进的多总线和多级流水线机 制、 专用的硬件乘法器 、 高效的指令集 , 使其易于 处理嵌入式各种信号的滤波 、 信息融合算法 ,而不 会牺牲系统的实时性 。此外 ,DSP 芯片中集成了
36 μs ,能够满足捷联惯性导航系统对精度和速度的要求 。
关 键 词 : 捷联系统 ; 惯性导航 ; 姿态解算 ; 四元数 中图分类号 :V448. 22 文献标识码 :A
Realization of attitude algorithm module in strapdown inertial guidance system
1958 3. 1 四元数法
光学 精密工程
K20 = K21 =
第 16 卷
四元数 ( 四维数) 的概念是 1843 年由哈密顿 首先提出的 , 他是代数学中的内容之一 。近些年 来 ,随着控制理论 、 惯性技术 、 计算技术 ,特别是捷 联惯性导航技术的发展 , 为了更简便地描述刚体 的角运动 ,设计控制系统 ,人们采用了四元数这个 数学工具 ,用它来描述刚体角运动的 3 个欧拉角 参数在设计控制系统时的不足 [ 3 ] 。 四元数是指由 1 个实数单位λ和 3 个虚数单 位 p1 、 p2 、 p3 组成的数 , 表示为 :
2 . Gra d uate U ni versi t y of Chi nese A ca dem y of S ciences , B ei j i n g 100039 , Chi na; 3 . D aqi n g V ocational Col le ge , D aqi n g 163255 , Chi na; 4 . S chool of Com m unication En gi neeri n g , J i l i n U ni versi t y , Chan gch un 130012 , Chi na) Abstract : A met hod to realize at tit ude algorit hm module in st rap down inertial guidance is p resented. Based o n t he digital signal p rocessor and quaternio ns , an at tit ude algorit hm module in st rap down iner2 tial guidance is designed. The working p rinciple of t rap down inertial guidance , t he at tit ude algorit hm and t he numerical met hod are int ro duced. The circuit and soft ware of at tit ude algorit hm are designed. The test result s show t hat t he relative erro r is less t han 0. 005 3 % wit h 4 o rder Runge2 Kut ta when t he increment is less t han 5° ; it can meet t he demands of t rap down inertial guidance , when t he algorit hm time is less t han 36 μs wit h TMS320C6713B. Key words : st rap down system ; inertial guidance ; at tit ude algorit hm ; quaternio n
捷联惯导系统姿态解算模块的实现
杜海龙1 ,2 ,张荣辉1 ,2 ,刘 平3 ,郑喜凤1 ,贾宏光1 ,马海涛4 ,1
(1. 中国科学院 长春光学精密机械与物理研究所 ,吉林 长春 130033 ; 2. 中国科学院 研究生院 ,北京 100039 ; 3. 大庆职业学院 ,黑龙江 大庆 163255 ;4. 吉林大学 通信工程学院 ,吉林 长春 130012 )
1 T T T K22 = [ωy ( t + ) A 0 - ωz ( t + ) A 1 ( t) + ωx ( t + ) A 3 ( t) ] 2 2 2 2
K23 =
,
1 T T T [ωz ( t + ) A 0 + ωy ( t + ) A 1 ( t) - ωx ( t + ) A 2 ( t) ] 2 2 2 2
RAM 存储器和各种外设接口 , 便于系统的设计
2 捷联惯性导航系统的工作原理
捷联惯性导航系统的工作是把捷联惯性测 量装置测量的运载体信息处理成导航所需要的各 种信息 ,形成控制指令 ,控制运载体按预定轨道运 行。 惯性导航的基本原理是以牛顿力学定律为基 础 ,在运载体内用加速度表和陀螺测量运载体加 速度和角速度 ,通过积分运算获得运载体姿态 、 速 度和位置的信息 ,利用这些信息进行导航 。 捷联惯性导航系统的关键是高精度的惯性测 量仪表和高性能的计算机以及软件设计 。捷联惯 性导航系统的特点是把惯性测量元件加速度表和 陀螺直接固连在运载体上 , 这些元件测量出沿载 体坐标系三轴的运载体的角速度和线加速度 , 计 算机实时计算出姿态矩阵 , 通过姿态矩阵把加速 度计测量的载体坐标系的轴向加速度信息变换到 导航坐标系 。捷联惯性导航原理框图如图 1 所 示。
( 4) ( t) + B 0 =λ K20 T
2
K21 T
B 1 = P1 ( t) + B 2 = P2 ( t) + B 3 = P3 ( t) + K30 = K31 =