捷联惯导的解算程序

捷联惯导初始对准以及姿态解算

第三部分：基于“存储数据与迭代计算对准”罗经法对准
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

b Eby

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
多传感器融合技术
随着科技的发展和应用需求的提高，捷联惯性导航系统需要进一步提高精度和可靠性，以满足更高层次的应用需求。
为了克服单一传感器的局限性，可以采用多传感器融合技术，将捷联惯性导航系统与其他传感器进行融合，提高导航系统的性能和鲁棒性。

基于四元数法的捷联式惯性导航系统的姿态解算

b EbX cos H sin C sin H cos C sin H X b 0 cos H cosC - sin C cos H X EbY .
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
பைடு நூலகம்

捷联惯导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;。

捷联惯导的解算程序

%====本程序为捷联惯导的解算程序(由惯性器件的输出解算出飞行器的位置、速度、姿态信息)======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为捷联式惯导系统的原理图，陀螺仪和加速度计输出分别送入姿态矩阵计算和由载体坐标系至平台坐标系的方向余弦矩阵的计算。

有了姿态矩阵，其一可以实现把载体坐标系轴向加速度信息变换到导航坐标系轴，进而可以进行所需的导航参数计算，其二利用姿态矩阵的元素，提取方位和姿态信息。

图1. 捷联式惯导系统的原理图姿态速率微分方程为:12b tbωΛ=Λ (1)其中；()b b b t ttb ib t ie et C ωωωω=-- (2)bib ω为陀螺仪测量经补偿后的值；0cos sin t iex tt ieiey ie t ie iez L L ωωωωωω⎡⎤⎡⎤⎢⎥⎢⎥⎢⎥==⎢⎥⎢⎥⎢⎥⎢⎥⎣⎦⎣⎦，为地球自转角速率； tan tety t yt etx t tt etx etety xtt etz tetxxt 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 tt C ψϕψθϕψϕψθϕθϕψθψθθψϕψθϕψϕψθϕθϕ-+-⎡⎤⎢⎥=-⎢⎥⎢⎥+-⎣⎦(3)对应的四元素初值为:0123cos cos cos sin sin sin 222222cos sin cos sin cos sin 222222cos cos sin sin sin cos 222222cossinsinsincoscos222222ψθϕψθϕλψθϕψθϕλψθϕψθϕλψθϕψθϕλ⎧=-⎪⎪⎪=-⎪⎨⎪=+⎪⎪⎪=+⎩(4)四元素姿态矩阵为：2222012312031302222212030123230122221302230101232()2()2()2()2()2()bt C λλλλλλλλλλλλλλλλλλλλλλλλλλλλλλλλλλλλ⎡⎤+++++⎢⎥=--+-+⎢⎥⎢⎥----+⎣⎦ (5)将姿态速率微分方程展开成矩阵形式：0112233001020bbbtbxtbytbx b bb tbx tbz tby b b btby tbztbx b bbtbz tbytbxλλωωωλλωωωωωωλλωωωλλ⎡⎤⎡⎤⎡⎤---⎢⎥⎢⎥⎢⎥-⎢⎥⎢⎥⎢⎥=⎢⎥⎢⎥⎢⎥-⎢⎥⎢⎥⎢⎥-⎢⎥⎢⎥⎢⎥⎣⎦⎣⎦⎣⎦(6)该微分方程的解可用比卡逼近法求解，解得[]000s i n 2()c o s (0)2t I λλλλλλ∆⎧⎫⎪⎪∆=+∆⎨⎬∆⎪⎪⎩⎭ (7)其中：22202((()))b b b tbx tby tbz ωωωλ+∆+=，220I λλ∆=-∆将0cos2λ∆和00sin2λλ∆∆展成级数并提取有限项，得到姿态矩阵的算法为：[]2420001(1)(1)()()8384248n I n λλλλλλ⎧⎫∆∆∆+=-++-∆⎨⎬⎩⎭ (8)根据比力方程比力方程：f a G =-(2)t t tttti e e tV f V gωω=-+⨯+(9)写为分量形式：0(2)(2)0(2)0(2)0(2)(2)0t t tt t t tx xx iez etz iey ety t t t t t tt y y iez etz iex etx y t t t t t t t iey ety iex etx z z z V f V V f V g f V V ωωωωωωωωωωωω⎡⎤⎡⎤⎡⎤⎡⎤ -+ + ⎡⎤⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥=- +-++⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥-+ +-⎣⎦⎣⎦⎣⎦⎣⎦⎣⎦ (10)从而有：(2s i n )(2c o s )(2s i n )(2c o s )ttt tt txx x x ie yiezxtxt tt yt tt txy y ie xzxt ytttyt t ttxz x iex y xtytV V V f L tgL V L V R R V V V f L tgL V V R R V V V f L V V gR R ωωωω⎧=++-+⎪⎪⎪⎪=-++⎨⎪⎪⎪=+++-⎪⎩(11)其中：22Re(1sin ())Re(1(23sin ()))Rxt e L Ryt e L =+=-- (12)天向速度为零，高度通道保持不变，上式可写为：2(s i n )t a n2(s i n )t a nt tx y t t tx x ie y xt t tt t t x xy y ie x ytV V V f LV L R V V V f L V L R ωω⎧=++⎪⎪⎨⎪=--⎪⎩(13)将加速度对时间积分可以得到X 、Y 两个方向的速度：00y 0(2sin tan )(2sin tan )tt t t tx xie x x xttt tttxy ie y xtV V L L V dt V R V V L L V dt V R ωω=++=++⎰⎰(14)X 、Y 两个方向的速度除以对应的地球曲率半径，再次积分得到载体的经纬度位置：(0)(0)c o s tt yyttt xxt V L dt L R V dt R Lλλ=+=+⎰⎰(15)二、程序流程图：三、导航结果：不考虑天向速度及飞行高度的导航结果如下：1.位置坐标曲线：2.系统东向速度曲线：3.系统北向速度曲线：4.系统终点值：四、小结：1、对捷联式惯导系统有了一定的了解。

捷联惯导结算原理

0 cos sin ， Rz sin 0 cos
sin cos 0
0 0 1
cos cos sin sin sin cos cos sin sin cos sin cos T11 T12 T13 Ry Rx Rz cos sin cos cos sin T21 T22 T23 sin cos cos sin sin sin sin cos sin cos cos cos T T T 31 32 33 b 由姿态矩阵 C n 反解飞行器姿态欧拉角：
（5）速度的计算
t t t t t 0 2iez etz ety 2iey Vxt Vx 0 t t b t t t t 0 2iex etx Vyt 0 Vy Cb f 2iez etz t Vz g Vzt 2 t t 2 t t 0 iey ety iex etx
o o sin 1 T23 , 90 , 90
tg 1
T13 180o , 180o , T33
tg 1
T21 o o , 180 , 180 T 22
图 6 东向北向速度变化曲线
阶段总结：1.学习了平台式和捷联式惯导的惯导解算方法并进行了仿真计算。 2.平台式惯导物理平台时刻跟踪当地水平东北天地理系，加速计的比力信息直接投影在导航系中，可直接进行导航速度和位置解算。载体的姿态可直接从平台框架直接得出；而捷联式惯导用数学平台取代实体的物理平台，通过求

用四元数法的捷联惯性导航姿态解算程序

;no dirg ;)')s/m( /yticoleV'(lebaly ;)')sm01( / emiT'(lebalx ;)'noitaluclaC yticoleV'(eltit ;)'Z :etanidrooC noitagivaN','Y :etanidrooC noitagivaN','X :etanidrooC noitagivaN'(dnegel )yticolev,mt(tolp erugif ;no dirg ;)')eerged( /legnA'(lebaly ;)')sm01( / emiT'(lebalx ;)'noitaluclaC erutseG'(eltit ;)'legnA waY','legnA lloR','legnA hctiP'(dnegel ;)'.-b',):,3(ELGNA,mt,'.g',):,2(ELGNA,mt,'-r',):,1(ELGNA,mt(tolp ;ip/081*ELGNA=ELGNA erugif dne
值比度速加/压电的向轴个三计度速加% ?=za_orez% ?=ya_orez% ?=xa_orez% 压电点零的向轴个三计度速加%
*********************************************************% % 值度速加的应相为换转据数将% 3*4201/cca_atad=cca_atad ;]z_cca;y_cca;x_cca[=cca_atad 压电拟模为换转字数度速加% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% ;)'txt.zetarelecca'(daol=z_cca ;)'txt.yetarelecca'(daol= y_cca ;)'txt.xetarelecca'(daol= x_cca ;4201/3=etar_cca% **************据数的计度速加入读****************% ;)'txt.zoryg'(daol=z_oryg ;)'txt.yoryg'(daol=y_oryg ;)'txt.xoryg'(daol=x_oryg 据数的仪螺陀入读**********% 据数入读****************************% ;']g-,0,0[=G 2^s/m8.9 位单%;24108.9=g 量矢度速加的生产力重% ;lla raelc ;lla esolc

用四元数法的捷联惯性导航姿态解算程序

用四元数法的捷联惯性导航姿态解算程序close all;clear all;%重力产生的加速度矢量g=9.80142;%单位9.8m/s^2G=[0,0,-g]';%****************************读入数据%**********读入陀螺仪的数据gyro_x=load('gyrox.txt');gyro_y=load('gyroy.txt');gyro_z=load('gyroz.txt');%****************读入加速度计的数据**************%acc_rate=3/1024;acc_x =load('acceleratex.txt');acc_y =load('acceleratey.txt');acc_z=load('acceleratez.txt'); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %加速度数字转换为模拟电压data_acc=[acc_x;acc_y;acc_z];data_acc=data_acc/1024*3%将数据转换为相应的加速度值%%*********************************************************%加速度计三个轴向的零点电压%zero_ax=?%zero_ay=?%zero_az=?%加速度计三个轴向的电压/加速度比值%rate_ax=? %单位是m/s^2/V%rate_ay=?%rate_az=?%acc_x=acc_x*acc_rate;%acc_y=acc_y*acc_rate;%acc_z=acc_z*acc_rate;aver_acc_x=mean(acc_x)aver_acc_y=mean(acc_y)aver_acc_z=mean(acc_z)%采样时间dtime=0.01;tm=0:dtime:0.01* (size(gyro_x,2)-1);%个数numn_point=size(gyro_x,2);%图1figureplot(tm,data_acc(1,:),'-',tm,data_acc(2,:),'.',tm,data_acc(3,:),'-.'); title('加速度计的采样曲线');legend('x_ACC','Y_ACC','Z_ACC');xlabel('Time / (10ms)');ylabel('Accelerate/ (m/s'')');grid on;%plot(tm,acc_x,'-',tm,acc_y,'.',tm,acc_z,'-.');%title('加速度的计的采样曲线'):%对采样曲线进行低通滤波a=[1,2,4,2,1];%gyro_x=filter(a/sum(a),1,gyro_x);%gyro_y=filter(a/sum(a),1,gyro_y);%gyro_z=filter(a/sum(a),1,gyro_z);%比例变换gyro_x=gyro_x/1024*3/0.6;gyro_y=gyro_y/1024*3/0.6;gyro_z=gyro_z/1024*3/0.6;%零点电压--陀螺仪，取前80个数的平均电压zero_gx=sum(gyro_x(1:80))/80zero_gy=sum(gyro_y(1:80))/80zero_gz=sum(gyro_z(1:80))/80%减去零点gyro_x=(gyro_x-zero_gx)/0.0125/180*pi; gyro_y=(gyro_y-zero_gy)/0.0125/180*pi; gyro_z=(gyro_z-zero_gz)/0.0125/180*pi;%gyro_x=(gyro_x-2.5)/0.0125/180*pi;%gyro_y=(gyro_y-2.5)/0.0125/180*pi;%gyro_z=(gyro_z-2.5)/0.0125/180*pi;%测试数据accelerate=zeros(3,n_point);accelerate(1,1:100)=10;accelerate(1,101:200)=-10;accelerate(1,201:300)=0;%陀螺仪数据gyro_x=zeros(1,n_point);gyro_y=zeros(1,n_point);gyro_z=zeros(1,n_point);gyro_z(1:100)=pi/3;gyro_z(101:200)=-pi/3;%重力轴始终有加速度accelerate(3,:)=accelerate(3,:)+9.8;figureplot(tm,accelerate(1,:),'-',tm,accelerate(2,:),'.',tm,accelerate(3,:),'-.');title('加速度计的采样曲线');legend('x_ACC','Y_ACC','Z_ACC');xlabel('Time / (10ms)');ylabel('Accelerate/ (m/s'')');grid on;%画出陀螺仪的采样曲线figureplot(tm,gyro_x,'r-',tm,gyro_y,'g.',tm,gyro_z,'b-.');title('陀螺仪的采样曲线');legend('x_Gyro','Y_Gyro','Z_Gyro');xlabel('Time / (10ms)');ylabel('Angel_rate/ (degree/s)');grid on;%size(gyro_x)%size(gyro_y)%size(gyro_z)data_gyro=[gyro_x;gyro_y;gyro_z];%转移矩阵--即方向余弦矩阵T=eye(3); %T是3*3的单位矩阵,初始转移矩阵%四元数矩阵，存储每步更新之后的四元数，方便以后绘图Q=zeros(4,n_point);%四元数的初始值确定，假定一开始导航坐标系与载体坐标系是重合的，因此方向余弦矩阵，是单位矩阵，利用它们之间的关系确定四元数的初始值。

捷联惯性导航系统的解算方法

b nbz
CC
RP
0 0
H.
C
R
P0. 0
R0.
0
2010-03-19
整理课件
Yb'' Yb Yb' H
N
17
b nbx b nby
cos
0
R
b nbz
sin R
P. cos R 0
R.
0
1
H. sin R 0
0 sin R cos P P.
1
sin P R.
kb jn
ib j
kn k
b n kb kn
2010-03-19
21
整理课件
固定矢量的坐标变换
矢量的坐标变换
固定矢量的坐标变换是一个在空间大小和方向都不变的矢量在两个不同方位的坐标系轴向分量之间的变换关系，也即同一个矢量在两个不同的坐标系轴向投影之间的变换关系。
旋转矢量的坐标变换
是指一个矢量大小不变，但在方向上转动了一个位置，这个矢量转动前和转动后在同一个坐标系轴向分量之间的变换关系。
2010-03-19
22
整理课件
固定矢量的坐标变换
一个矢量r，写成载体坐标系轴向分量形式：
r X bib Yb jb Z b kb r bT b
rb
X Y
b
b
ib j
b
Zb
kbb
b:载体坐标系 n:地理坐标系
同一个矢量r，如果写成地理坐标系轴向分量形式：
r X nin Yn jn Z n k n r nT n 由于r是同一个矢量，故
惯性导航系统原理
1
整理课件
3 捷联式惯导系统

捷联惯性导航总结

z
0 x
y
姿态矩阵计算方法
3 四元数微分方程及其解
1 qk 1 exp( )qk Mq k qk rk 2
rk ac as x i as y j as z k
(0.5 )2 (0.5 )4 ac cos( ) 1 2 2! 4!
tk b nb
t
v f dt
b tk
t
速度微分方程求解
2 速度积分算法
v f dt [2ω ωen ] v dt g1 dt
n n 0 0 n ie n n n 0
t
t
t
v
n k 1
v u gt
n k n
b
v
n l 1
[ I 2Ωie tl Ωen tl ]v
v f dt
b
t
1 tk 1 αk 1 α ωdt 2 tk
α ωdt
tk
tk
t
捷联导航算法
捷联惯导误差分析
主要误差源

1 初始对准误差
2 惯性器件误差
3 计算误差
误差传播方程：误差源与系统性能的相互关系的方程，主要用来阐述各种类型误差随时间的传播过程。
捷联惯导误差分析
sin( / 2) (0.5 ) (0.5 ) as 0.5(1 ) 3! 5!
2 4
姿态矩阵计算方法
4 圆锥误差
σ αk 1 αk 1
α ωdt
tk t
1 tk 1 αk 1 α ωdt 2 tk
姿态矩阵计算方法
5 姿态角计算
b nbz

捷联惯性导航系统算法程序汇总

捷联惯性导航系统算法
1.经典捷联惯性导航算法(毕卡逼近、旋转矢量、四阶龙格库塔算法)，使用C语言编写，在
实际的系统中得到验证；
2.组合导航算法，包括：速度匹配、位置匹配、姿态角匹配等；
3.捷联惯性导航系统初始对准算法，粗对准方法：经典解析法、基于惯性系抗晃动基座解析
法，精对准方法：基于Kalman滤波的速度匹配、位置匹配精对准方法；
4.捷联惯导系统姿态算法研究，包括：四阶龙格库塔算法、旋转矢量算法，在典型圆锥运动
环境下对姿态解算算法系数进行优化；
5.利用Allan方差分析对光纤陀螺随机误差进行分析，为了抑制随机误差采用Kalman滤波
器对其进行滤波；
6.单轴旋转捷联惯导系统(SINS)多位置初始对准算法以及导航解算方法；
以上所有算法均采用C语言编写，且已经在实际的惯性导航系统中进行了充分的验证，如果需要交流，可以进一步进行联系！。

捷联惯导

G 格网航向角，俯仰角，倾斜角＝ G , 取＝0,所以＝ G
得到矩阵T后，沿机体坐标系测量的比力就可以转换到平台坐标系上，得到：
f T f
p
b
由姿态矩阵T确定飞行器姿态角
根据矩阵T中的元素可以确定各角的主值：
主＝sin 1 T32
T31 主＝ tan T33 1 T12 G主 tan T 22
t VEt、VN 地理坐标系下的东向和北向速度
方向余弦矩阵（位置矩阵）Ce
平台坐标系与地球坐标系转动关系为：
p
Xp Xe p Y C Y p e e Z Z e p
其中
C11 C12 C13 sin sin L cos cos sin sin sin L sin cos cos sin cos L Cep C21 C22 C23 cos sin L cos sin sin cos sin L sin sin cos cos cos L C C C33 cos L cos cos L sin sin L 32 31
C32 cos L sin tan 1 C31 cos L cos
由于在L的定义域内cosL永远为正，所以 cos 与C31同号
利用 C31，主的正负值可确定真值：
主 = 主 180 主 180
C31 0 C31 0, 主 0 C31 0, 主 0
T11 T12 T13 cos cos G sin sin sin G cos sin G cos cos G cos sin sin G T T21 T22 T23 cos sin G sin sin cos G cos cos G sin sin G cos sin cos G T T T sin cos sin cos cos 31 32 33

P15捷联惯导系统算法导航原理教学课件

舰船导航应用
舰船导航概述
01
舰船在航行过程中需要精确的导航信息，以确保航行安全和任
务执行。
舰船导航应用案例
02
介绍了P15捷联惯导系统在舰船导航中的实际应用案例，包括海
上巡逻、救援行动等。
舰船导航优势
03
详细阐述了P15捷联惯导系统在舰船导航中的优势，如高精度、
稳定性、可靠性高等。
其他领域应用
其他领域概述
系统初始化
01
02
03
初始化流程
系统上电后，首先进行硬件和软件的初始化，包括传感器、微处理器、存储器等。
初始参数设置
根据系统要求和导航需求，设置初始参数，如初始位置、初始速度、地球模型等。
校准与标定
对系统中的传感器进行校准和标定，确保其测量精度和可靠性。
数据采集与预处理
ห้องสมุดไป่ตู้
数据采集
通过传感器采集原始数据，如加速度、角速度等。
算法验证
通过模拟实验和实际测试，验证算法的正确性和有效性。
导航解算与
导航解算
根据算法处理后的数据，进行导航解算，包括位置、速度、姿态
等计算。
数据融合
将捷联惯导系统与其他导航系统（如GPS）的数据进行融合，进
一步提高导航精度。
输出结果
将解算得到的导航信息输出，为其他系统或设备提供准确的导航
服务。
除了无人机、车辆和舰船等应用领域，P15捷联惯导系统还广泛应用于其他领域。
其他领域应用案例
列举了P15捷联惯导系统在其他领域中的实际应用案例，如机器人、航空航天等。
其他领域应用优势
详细阐述了P15捷联惯导系统在其他领域应用中的优势，如高精度、稳定性、可靠性高等。

捷联式惯性导航系统原理

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