捷联惯导Matlab程序
捷联惯导可视化仿真系统设计与实现
Value Engineering• 229•捷联惯导可视化仿真系统设计与实现Design and Implementation of Visual Simulation System for Strapdown Inertial Navigation高扬G A O Y a n g;徐景硕X U Jin g-sh u o(海军航空工程学院青岛校区，青岛266041 )(Navel Aeronautical Engineering In stitute Q ingdao Branch,Q ingdao266041, C hina)摘要:捷联惯导可视化仿真系统采用面向对象、模块化设计思想，建立了飞行轨迹发生器和惯性器件的数学模型，完成了在不同飞行状态下、不同惯性器件精度条件下捷联惯导系统的实时仿真,仿真输出的数据在虚拟多功能显示器显示。
亥系统可有效地降低捷联惯导系统的实验成本,缩短研制周期，对研究捷联惯导系统的算法性能、实验室完成综合航电系统交联实验验证等任务具有重要的工程实际意义。
Abstract:Visual sim ulation system for strapd ow n inertial is based on object-oriented th ou gh t and m odular design,ou tp u ts SIN S sim ulation d ata in different flight conditions and precisions,and sh ow s sim ulation d ata o n virtual m u lti function display.The system can be used to reduce effectively th e experim ental expense and sh orten th e developing period of steapdow n inertial navigation system,and is of en orm ou s engineering significance to verify th e inertial navigation system algorithm,and com plete th e integrated avionics system s cross- linking experim ents in laboratory.关键词：捷联惯导；可视化仿真；导航解算；飞行轨迹Key words:SINS；visual sim ulation；navigation solution；flight trajectory中图分类号:TP273 文献标识码:A0引言随着光学陀螺仪技术的成熟和计算机技术的发展，在 常规武器上激光陀螺捷联惯导系统正逐步取代挠性陀螺的 平台惯导系统，成为惯性导航系统发展的主流。
基于Matlab的SINS算法仿真
• 95•为了使实际SINS(捷联惯导系统)算法仿真程序编写更加方便，列写了一些与SINS 更新算法有关的函数计算公式。
对SINS 算法进行了仿真，仿真结果与理论分析结论吻合。
1.引言SINS 算法的核心部分是姿态更新解算，由于四元数算法的优良特性，它在工程上常被采用。
本文总结了与SINS 算法有关的函数计算公式，对SINS 算法进行了仿真，为SINS/GPS 组合导航的研究打下基础。
2.SINS算法本文选用地理坐标系为导航坐标系，记为n 系，载体坐标系记为b 系。
2.1 SINS更新算法SINS 更新的基本理念是，把前一时刻的姿态、速度和位置等导航参数作为初值，利用前一时刻至当前时刻的惯性器件采样输出，解算此时刻的导航参数，作为下一时刻SINS 解算初值，如此反复。
惯性器件采样经过误差补偿后获得等效旋转矢量和比力速度增量，再经过以下三步骤便可实现SINS 更新，计算公式为：（1）速度更新算法位置更新算法（3）姿态更新算法基于Matlab 的SINS算法仿真沈阳理工大学 王海生图1 陀螺漂移对导航精度的影响• 96•图2 捷联惯导算法仿真2.2 SINS速度与位置误差分析速度与位置是积分关系，二者紧密相连，在误差上也是递推的。
速度误差的递推关系式是：位置误差：3.SINS仿真与分析以ENU 坐标系为导航坐标系，在41.5纬度下仿真东、北、天三方向的初始姿态误差、初始位置误差和加速度计零偏误差对导航精度的影响，结果如图1所示。
由仿真图可知，东向速度精度受初始姿态偏差北向投影和初始北向零偏影响较大；经度精度受初始东向零偏影响较大；纬度精度受初始北向零偏和天向零偏影响较大。
而且由于地球自转和地球加速度的影响，速度和姿态都会发生略微偏差。
分别把初始平台的误差角设置为，对静态导航进行仿真，仿真1h 导航参数结果如图2所示。
图2中，速度误差和位置误差小图中，高度通道的误差为红色发散曲线，而水平平台误差、经纬度和水平速度误差呈现振荡趋势，方位平台误差变化比较小。
基于Matlab的低成本捷联惯导的仿真系统研究
基于Matlab的低成本捷联惯导的仿真系统研究作者：刘智平,邵小兵来源：《现代电子技术》2010年第12期摘要:低成本捷联惯导系统的研制和开发已成为导航领域的主要发展趋势之一,在选定了低成本的惯性器件后,系统预期的性能指标和最后样机的性能测试结果之间往往存在着一定的差距,如何降低开发成本并缩短开发时间,就成了低成本捷联惯导系统研制中的一个亟待解决的问题。
这里基于Matlab设计了一个仿真系统,将捷联惯导的软件和硬件模块化,该系统不仅可以完成对系统性能的预测和评估,也可完成对算法和器件选择的优化。
关键词:Matlab; 陀螺; 捷联惯导系统; 仿真系统中图分类号:TP274 文献标识码:A文章编号:1004-373X(2010)12-0078-03Simulation System of Low Cost Strapdown Inertial Navigation System Based on MatlabLIU Zhi-ping, SHAO Xiao-bing(Co llege of Computer, Xi’an Technological University, Xi’an 710032,China)Abstract:The research and development of low cost strapdown inertial navigation system has been the tendency in the field of navigation. The discrepancy between the expected performance and the test data was inevitable with the usage of inertial sensors. How to reduce the cost and shorten the development period is a hot issue in the development of low cost strapdown inertial navigation system.A simulation system based on MATLAB was designed and constructed, the software and hardware of strapdown inertial navigation were modularized, the optimization of various algorithms and selection of sensors should be more convenient and feasible.Keywords:Matlab; gyro; strapdown inertial navigation; simulation system0 引言捷联惯导系统由于体积小、成本低、可靠性好而大量应用在航空、航天、航海、兵器等领域[1-3]。
捷联惯导Matlab程序
捷联惯导程序，依据加表和陀螺仪的输出数据来求解飞行器的姿态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 ; -Myx 1 -Myz ; -Mzx -Mzy 1 ];% 加速度计模型参数标定如下：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 ; -Iyx 1 -Iyz ; -Izx -Izy 1 ];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 ]; %导航坐标系中的东北天初始速度都为0Vx = 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的特殊性决定了其数据个数为12001L_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 %求解Q1endif(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 %求解Q2endif(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 %求解Q3endQ0 = 0.5*sqrt(1-Q1*Q1-Q2*Q2-Q3*Q3); %求解Q0Q = [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*Q3 2*(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实为不妥，不过为了程序的完善性，只能这样了endendif ( 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 ;endendend%以下存储姿态角到三个矩阵里面，为画图做准备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) 0 0 ; tan( LL )/(Rn + h) 0 0];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_z 0 -Delta_Theta_x ; -Delta_Theta_y Delta_Theta_x 0 ]*Delta_V ) + Delta_t*(G-W_X*V);Vx = V(1);Vy = V(2);Vz = V(3);V e_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;endendLamda_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;。
imu惯导matlab解算
imu惯导matlab解算
在MATLAB中解算惯性导航（IMU）可以使用以下步骤：
1. 准备数据：你需要收集IMU传感器的数据，包括加速度计和陀螺仪的测量值。
这些数据通常以时间序列的形式提供。
2. 读取数据：使用MATLAB的文件读取函数（例如`readmatrix`）将数据读取到MATLAB的工作空间中。
3. 数据预处理：IMU数据通常需要进行预处理，例如去除噪声、校准传感器和对数据进行滤波。
可以使用MATLAB的信号处理工具箱进行这些预处理步骤。
4. 设计滤波器：在IMU数据预处理之后，可能需要设计一个滤波器来进一步减少噪声和提高数据质量。
MATLAB的信号处理工具箱提供了多种滤波器设计方法。
5. 解算姿态：使用IMU的加速度计和陀螺仪测量值，可以通过解算算法来估计姿态。
常见的解算算法包括加速度计积分法、四元数解算、互补滤波器等。
可以使用MATLAB中的数学函数和算法来实现这些解算算法。
6. 可视化结果：可以使用MATLAB的绘图函数（例如`plot`）将解算结果可视化，以便更好地理解IMU的姿态估计。
请注意，解算IMU的精确性和准确性可能需要对算法进行调优，并进行对比和评估。
IMU还可能受到环境因素和传感器误差的影响，因此在解算IMU数据时需要考虑到这些因素。
捷联惯导MATLAB程序
%位置矩阵仅仅与经纬度有关系，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) ) ];
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);
%*********************************************************************
Theta_Main = asin( T(3,2) );
Gama_Main = atan来自 -T(3,1) / T(3,3));
Fai_Main = atan( -T(1,2) / T(2,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);
捷联惯导系统仿真器的设计与实现
随着 惯性 导航 技 术 和 计算 机 技 术 的发展 ， 联 捷
惯 导系统 已成为 当前 惯 性 导 航 系统 发 展 的趋势 ， 并
仿 真 系统 的 正确性 和精 度 。
设 定姿 态 角随 时 问变 化 的 函数 ， 进 一 步 得 到 可 姿态 角一 阶导数 随 时 间变 化 的 函数 ， 由姿 态 角 和 再 姿态 角 的一 阶 导 数 算 出载 体 下 相 对 于 地 理 系 的 转
（ 泗洪 县建设局 测绘 队 ， 江苏 宿迁 ２ ３ ０ ， 江苏省测绘工程 院， ２ ９０ 。 江苏 南京 ２ ０ １ ） １０ ３ 摘 要 本 文研 究了捷联惯性导航 系统的仿 真原 理， 然后 以 Ｍａｔｂ为平 台， 对捷 联惯 性导航 系统 仿 真的基 础 ｔ ａ 在
上， 分析 了在纯捷 联惯导情况下导航 系统的导航精度 。实验结果表明捷联惯性导航 系统 的导航精度 随时 间的积 累
惯 性 器 件 由陀 螺 仪 和 加 速 度 计 两 部 分 组 成 。 陀螺 仪是 敏感载 体 角 运 动 的 元 件 ， 由于 陀 螺仪 本 身 存在 误差 ， 因此 陀螺 仪 的输 出为 ：
蕊 一诜 ＋ （） ３
由于条 件 限制 ， 法 得 到 实 际 飞 行 轨 迹 数 据 ， 无 而捷联 惯 导 仿 真 系 统 测 试 验 证 又 必 须 要 有 飞行 轨 迹 数据 。为此 ， 们 采 用 一 种 纯数 学 方 法 产 生 捷联 我 惯 导仿 真 系统测 试 用 飞行 轨 迹 数 据［ ］ ４ 。测 试 用 飞
对 惯 导系统 的仿真是 非 常有必 要 的 。
动角 速率 在 载 体 系 中 的 投 影 。姿 态 角 随 时 间 变 化
捷联惯导的解算程序
%====本程序为捷联惯导的解算程序(由惯性器件的输出解算出飞行器的位置、速度、姿态信息)======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('时间(秒)');%------------------------------绘图结束-------------------------------。