北京航空大学本科惯性导航作业二.docVIP

惯性导航作业  一、数据说明： 1：惯导系统为指北方位的捷连系统。初始经度为116.344695283度、纬度为39.975172度,高度h为30米。初速度v0=[-9.993908270;0.000000000;0.348994967]。 2：jlfw中为600秒的数据，陀螺仪和加速度计采样周期分别为为1/100秒和1/100秒。 3：初始姿态角为[2 1 90]（俯仰，横滚，航向,单位为度）,jlfw.mat中保存的为比力信息f_INSc(单位m/s^2)、陀螺仪角速率信息wib_INSc(单位rad/s)，排列顺序为 一~三行分别为X、Y、Z向信息. 4: 航向角以逆时针为正。 5:地球椭球长半径re=6378245;地球自转角速度wie=7.292115147e-5;重力加速度g=g0*(1+gk1*c33^2)*(1-2*h/re)/sqrt(1-gk2*c33^2)；g0=9.7803267714;gk1=0.00193185138639;gk2=0.00669437999013;c33=sin(lat纬度); 二、作业要求： 1：可使用 MATLAB语言编程，用MATLAB编程时可使用如下形式的语句读取数据：load D:\...文件路径...\jlfw,便可得到比力信息和陀螺仪角速率信息。用角增量法。 2：(1) 以系统经度为横轴，纬度为纵轴（单位均要转换为：度）做出系统位置曲线图； (2) 做出系统东向速度和北向速度随时间变化曲线图（速度单位：m/s，时间单位：s）； (3) 分别做出系统姿态角随时间变化曲线图（俯仰，横滚，航向,单位转换为：度，时间单位：s）； 　　以上结果均要附在作业报告中。 3：在作业报告中要写出“程序流程图、现阶段学习小结”，写明联系方式。 (注意程序流程图不是课本上的惯导解算流程，而是你程序分为哪几个模块、是怎样一步步执行的,什么位置循环等,让别人根据该流程图能够编出相应程序) (学习小结按条写，不用写套话) 4：作业以纸质报告形式提交，附源程序。 三、基本原理和公式 1、初始姿态矩阵的确定： 根据初始姿态角求四元数： 再根据四元数求方向余弦矩阵的初始矩阵： 2、指北方位系统的运动解算： “平台”指令角速度为： 加速度计获得的比力信息为载体坐标系中各个轴向的比力，而我们需要的比力为地理坐标系中各个轴向的比力，它们之间应用矩阵做变换： 根据比力信息可以求出各个方向上的加速度： 因此可以求得速度为： 载体所在位置的地理纬度L、经度可由下列方程求得： 3、四元数姿态矩阵的更新： 式中，为陀螺所测得的角速度。 用毕卡逼近法更新的值，T为采样时间 4、姿态角的求解： 姿态角与姿态矩阵的关系： 式中,,分别为俯仰角，滚转角和偏航角，以逆时针为正方向，而课本上是以顺时针为正，因此需要对课本上的公式进行修改，将代入原公式可得现公式。如果记 则由以上两式即可解算出姿态角： 四、程序流程图 五、结果 六、小结 这次作业是捷联惯导的解算，是利用上次作业的结果对数据进行处理。和上次不同，这次遇到了较多的问题。首先，对捷联惯导的基本原理理解的不够深刻，比如坐标系的转换，四元数微分方程的求解。其次，由于课本的姿态角是以顺时针为正，而原始数据是以逆时针为正，因此需要对书上的公式进行修改，在这个过程中就出现了许多问题，比如正负号问题。总之，这次作业弥补了学习上的不足，使我对基本原理理解更为深刻，也初步了解惯导的基本操作。 七、程序 clc clear a=load(C:\Users\Administrator\Documents\MATLAB/jlfw.dat); Wib_INSc=a(:,2:4);f_INSc=a(:,5:7); %第一列：数据包序号 第二至四列：分别为东、北、天向陀螺仪角速率信息wib_INSc（单位：rad/s） %第五至七列：分别为东、北、天向比力信息f_INSc（单位：m/s^2）. L(1,:)=zeros(1,60001); Lambda(1,:)=zeros(1,60001); Vx(1,:)=zeros(1,60001); Vy(1,:)=zeros(1,60001); Vz(1,:)=zeros(1,60001); Rx(1,:)=zeros(1,60001);%定义存放卯酉圈曲率半径数据的矩阵 Ry(1,:)=zeros(1,60001);%定义存放子午圈曲率半径数据的矩阵 psi(1,:)=zeros(1,60001);%定义存放偏航角数据的矩阵 theta(1,:)=zeros(1,60001);%定义存放俯仰角数据的矩阵 gamma(1,:)=zeros(1,60001);%定义存放滚转角数据的矩阵 I=eye(4);