捷联惯导解算作业

惯性导航原理第二次作业报告姓名：苏宪飞学号：S14124007电话：183107467511、题目分析本题已知的信息是陀螺仪和加速度计在各个时刻的采样值，以及初始经纬度、高度、姿态角和速度值。需要求解指北方位捷联系统的运动轨迹和姿态角变化。根据这些已知信息以及捷联式惯导系统的基本力学编排方程可知基本求解过程如下：第一步，先根据初试的姿态角确定初始的四元数值，进而可以列写出本体系相对于导航系的方向余弦矩阵，然后将加速度计的测量信息经过余弦阵由导航系变换到本体系中。第二步，根据变换后的比力信息和初始的经纬度、高度和速度值进行指北方位系统的运动解算，可以求出速度信息、指令角速度信息和位置信息。第三步，根据第二步中求解出来的指令角速度信息，对其经过余弦矩阵变换到本体系中，陀螺仪测量到的信息和变换后的指令角速度相减得到本体系相对于导航系的角速度在本体系中的分解。第四步，根据四元数的运动学微分方程求解出下一时刻的四元数，根据四元数和欧拉角之间的关系求解出新的姿态角。接着进行以上两步的运算。经过以上几步就可以对指北方位捷联惯导系统进行解算。2、解算原理和公式(1)、初始姿态矩阵的确定：根据初始姿态角求四元数：2cos2cos2sin2sin2sin2cos2cos2sin2sin2sin2cos2cos2sin2cos2sin2cos2sin2cos2sin2sin2sin2cos2cos2cos3210根据四元数求方向余弦矩阵：222201231203130222221203012323012222130223010123222222btC(2)、指北方位系统的运动解算：地理坐标系相对地球坐标系的角速度为：)tan(LRVRVRVzxtytxtxtytyttettetytetxtet加速度计获得的比力信息bibf为载体坐标系中各个轴向的比力，而我们需要的比力titf为地理坐标系中各个轴向的比力，它们之间应用矩阵tbC做变换：ttbitbibfCf根据比力信息可以求出各个方向上的加速度：gVVVfffVVVtetztetytetxtetxtiextetytieytetxtiextetztieztetytieytetztieztitztitytitxtetztetytetx0002)2()2(022)2(0因此可以求得速度为：tetyttetytetytetxttetxtetxVdtVVVdtVV0000载体所在位置的地理纬度L、经度可由下列方程求得：0000)sec(dtLRVLdtRVLtxtxttytyt(3)、四元数姿态矩阵的更新：bbbtttbibtetieC式中，bib为陀螺所测角速度。用毕卡逼近法更新btC的值，bibT0000xyzxzyyzxzyx22220xyz02sin2cos000qItq222201231203130222221203012323012222130223010123222222btC(4)、姿态角的求解：姿态角与姿态矩阵的关系：coscossinsinsincossinsincoscossinsincossinsinsincossincoscoscossinsinsincossincoscoscosbtC式中,,分别为俯仰角，横滚角和偏航角。如果记111213212223313233btTTTCTTTTTT则由以上两式即可解算出姿态角：1231133312122sinTTtgTTtgT3、作业结果116.3116.305116.31116.315116.32116.325116.33116.335116.34116.345116.3539.97539.9839.98539.9939.9954040.00540.01经纬度位置曲线经度/°纬度/°图1：经纬度位置曲线0100200300400500600700-10-8-6-4-202东西方向速度时间/s速度/m/s图2：东西方向速度曲线0100200300400500600700012345678910南北方向速度时间/s速度/m/s图3：南北方向速度曲线0100200300400500600700-2.5-2-1.5-1-0.500.511.52俯仰角时间/s角度/°图4：俯仰角随时间变化曲线0100200300400500600700-1-0.8-0.6-0.4-0.200.20.40.60.81横滚角时间/s角度/°图5：横滚角随时间变化曲线0100200300400500600700-100102030405060708090偏航角时间/s角度/°图6：偏航角随时间变化曲线4、程序流程图开始装载数据，初始化解算结束？YN根据初始姿态角计算初始的四元数值用四元数构造方向余弦阵将比力信息通过方向余弦阵进行转换对指北方位系统进行速度求解并保存根据速度值求解出指北系统的指令角速度根据速度值求解出经纬度值并保存利用四元数微分方程更新四元数利用方向余弦阵求解姿态角并保存绘图图7：程序流程图5、学习小结学习惯导大约两个月了，总体感觉研究生阶段学习比本科阶段有所提高，本科接触的惯导只是皮毛，而研究生阶段则是充分剖析各个细节使我对惯导有了总体的把握。在学习中也看到了很多不足之处：第一：对各个公式推导还是不能全面掌握。第二：对所学知识还没有融会贯通。第三：专业基础太浅导致上课难以理解，还好老师讲的比较详细。针对这些问题，采取了一些自学手段首先对书上的公式按照老师的方法进行推导，努力回顾上课时老师教给我们的方法，而不是按照书本生搬硬套。6、源程序clear;clc;formatlong;%%%%%%%%%输入常值%%%%%%Re=6378245;e=1/298.3;w_ie=7.292115147e-5;%单位为弧度dt=0.01;%步长为0.01g0=9.7803267714;gk1=0.00193185138639;gk2=0.00669437999013;%%%%%%%%%输入初值%%%%%%lumda0=116.344695283;%初始位置L0=39.975172;%单位为度h0=30;Vx0=-9.993908270;%初始速度Vy0=0;Vz0=0.348994967;xita0=2*pi/180;%初始姿态角gama0=1*pi/180;%单位为度fai0=-90*pi/180;%初始四元数中以顺时针为正，所以此处应该把航向角取负%%%%%%%%数组初始化%%%%%%%L=zeros(1,60002);lumda=zeros(1,60002);Vx=zeros(1,60002);Vy=zeros(1,60002);Vz=zeros(1,60002);xita=zeros(1,60002);gama=zeros(1,60002);fai=zeros(1,60002);L(1,1)=L0;lumda(1,1)=lumda0;Vx(1,1)=Vx0;Vy(1,1)=Vy0;xita(1,1)=xita0*180/pi;gama(1,1)=gama0*180/pi;fai(1,1)=-fai0*180/pi;%初始四元数中以顺时针为正，所以此处应该把航向角取负%%%%%%%%%初始四元数%%%%%%%%%%公式中以顺时针为正%%%%%%%Lamu0=cos(fai0/2)*cos(xita0/2)*cos(gama0/2)+sin(fai0/2)*sin(xita0/2)*sin(gama0/2);Lamu1=cos(fai0/2)*sin(xita0/2)*cos(gama0/2)+sin(fai0/2)*cos(xita0/2)*sin(gama0/2);Lamu2=cos(fai0/2)*cos(xita0/2)*sin(gama0/2)-sin(fai0/2)*sin(xita0/2)*cos(gama0/2);Lamu3=cos(fai0/2)*sin(xita0/2)*sin(gama0/2)-sin(fai0/2)*cos(xita0/2)*cos(gama0/2);Lamu=[Lamu0;Lamu1;Lamu2;Lamu3];%%%%%%%%%导入测量值%%%%%%%%%%loadjlfw.mat%%%%%%%%%开始循环%%%%%%%%%%%%lumda_t=lumda0*pi/180;L_t=L0*pi/180;%单位为弧度h_t=h0;Vx_t=Vx0;Vy_t=Vy0;Vz_t=Vz0;fori=1:1:60001;Lamu0=Lamu(1,1);Lamu1=Lamu(2,1);Lamu2=Lamu(3,1);Lamu3=Lamu(4,1);Ct_b=[Lamu0^2+Lamu1^2-Lamu2^2-Lamu3^2,2*(Lamu1*Lamu2+Lamu0*Lamu3),2*(Lamu1*Lamu3-Lamu0*Lamu2);2*(Lamu1*Lamu2-Lamu0*Lamu3),Lamu0^2-Lamu1^2+Lamu2^2-Lamu3^2,2*(Lamu2*Lamu3+Lamu0*Lamu1);2*(Lamu1*Lamu3+Lamu0*Lamu2),2*(Lamu2*Lamu3-Lamu0*Lamu1),Lamu0^2-Lamu1^2-Lamu2^2+Lamu3^2];%捷联姿态阵的逆f_b=[f_INSc(1,i);f_INSc(2,i);f_INSc(3,i)];f_t=Ct_b\f_b;Rxt_re=1/Re*(1-e*(sin(L_t)^2));Ryt_re=1/Re*(1+2*e-3*e*(sin(L_t)^2));Vy_t=Vy_t+dt*(f_t(2,1)-Vx_t*(2*w_ie*sin(L_t)+Vx_t*Rxt_re*tan(L_t))+Vy_t*Ryt_re*Vz_t);Vx_t=Vx_t+dt*(f_t(1,1)+Vy_t*(2*w_ie*sin(L_t)+Vx_t*Rxt_re*tan(L_t))-Vz_t*(2*w_ie*cos(L_t)+Vx_t*Rxt_re));c33=sin(L_t);g=g0*(1+gk1*c33^2)*(1-2*h_t/Re)/sqrt(1-gk2*c33^2);Vz_t=Vz_t+dt*(f_t(3,1)+Vx_t*(-2*w_ie*cos(L_t)+Vx_t*Rxt_re)+Vy_t*Vx_t*Ryt_re-g);witx_t=-Vy_t*Ryt_re;L_t=L_t+Vy_t*Ryt_re*dt;wity_t=Vx_t*Rxt_re+w_ie*cos(L_t);witz_t=Vx_t*Rxt_re*tan(L_t)+w_ie*sin(L_t);lumda_t=lumda_t+Vx_t*Rxt_re*sec(L_t)*dt;h_t=h_t+dt*Vz_t;wit_t=[w