哈工大导航原理大作业

《导航原理》作业(惯性导航部分)姓名班级1104103学号11104103一、题目要求AfighterequippedwithSINSisinitiallyatthepositionof35NLand122EL,stationaryonamotionlesscarrier.ThreegyrosXG,YG,ZG,andthreeaccelerometers,XA,YA,ZAareinstalledalongtheaxesbX,bY,bZofthebodyframerespectively.Case1:stationaryonboardtestThebodyframeofthefighterinitiallycoincideswiththegeographicalframe,asshowninthefigure,withitspitchingaxisbXpointingtotheeast,rollingaxisbYtothenorth,andazimuthaxisbZupward.Thenthebodyofthefighterismadetorotatestepbysteprelativetothegeographicalframe.(1)10aroundbX(2)30aroundbY(3)50-aroundbZAfterthat,thebodyofthefighterstopsrotating.Youarerequiredtocomputethefinaloutputofthethreeaccelerometersonthefighter,usingbothDCMandquaternionrespectively,andignoringthedeviceerrors.Itisknownthatthemagnitudeofgravityaccelerationis2/8.9gsm.Case2:flightnavigationInitially,thefighterisstationaryonthemotionlesscarrierwithitsboard25mabovethesealevel.Itspitchingandrollingaxesarebothinthelocalhorizon,anditsrollingaxisis45onthenorthbyeast,parallelwiththerunwayonboard.Thenthefighteracceleratealongtherunwayandtakeofffromthecarrier.Theoutputofthegyrosandaccelerometersarebothpulsenumbers,Eachgyropulseisanangularincrementofsecarc1.0,andeachaccelerometerpulseisg6e1,with2/8.9gsm.Thegyrooutputfrequencyis10Hz,andtheaccelerometer’sis1Hz.Theoutputofgyrosandaccelerometerswithin5400sarestoredinMATLABdatafilesnamedgout.matandaout.mat,containingmatricesgmof35400andamof35400respectively.Theformatofdataasshowninthetables,with10rowsofeachmatrixselected.Eachrowrepresentstheoutofthetypeofsensorsateachsampletime.TheEarthcanbeseenasanidealsphere,withradius6368.00kmandspinningraterad/s10292.75-,Theerrorsofsensorsareignored,soistheeffectofheightonthemagnitudeofgravity.Theoutputofthegyrosaretointegratedevery0.1s.Therotationofgeographicalframeistobeupdatedevery1s,soarethevelocitiesandpositionofthefigure.Youarerequiredto:(1)Computethefinalattitudequaternion,longitude,latitude,height,andeast,north,verticalvelocitiesofthefighter.(2)Computethetotalhorizontaldistancetraveledbythefighter.(3)Drawthelatitude-versus-longitudetrajectoryofthefighter,withhorizontallongitudeaxis.(4)Drawthecurveoftheheightoffighter,withhorizontaltimeaxis.二、Case1解答2.1方向余弦阵法（1）绕Xb轴转过ψ1010cos10sin010sin10cos0001cossin0sincos0001C（2）绕Yb轴转过3030cos030sin01030sin030coscos0sin010sin0cosC（3）绕Zb轴转过501000）50（cos）50（sin-0）50（sin）50（cos1000cossin-0sincosC所以变换后的坐标XEYCCCNZ由于初始时刻有009.8EN所以计算得(ZYX)=(3581.86027.2-4054.4-)三个加速度计的输出分别是,/4504.42xsmA,/6027.22ysmAsmA/3581.8z2计算程序见附录一2.2四元数法（1）绕Xb轴转过ψ10i2sin2cosq（2）绕Yb轴转过30j2sin2cosq（3）绕Zb轴转过50k2sin2cosq则合成四元数)2sin2(cos)2sin2(cos)2sin2(coskjiqqqq合成四元数的逆1123qpipjpk由公式qNEqZYX1计算得(ZYX)=(3581.86027.2-4054.4-)三个加速度计的输出分别是,/4504.42xsmA,/6027.22ysmAsmA/3581.8z2由两种计算方法的计算结果可以看出，方向余弦阵法和四元数法的计算结果是一致的。计算程序见附录二三、Case2解答3.1程序流程图3.2源程序详见附录三3.3仿真及结果3.3.1运行程序可得：（1）战斗机相对当地地理坐标系的姿态四元数为：4841.00023.00054.08750.0Q（2）经度=8757.122纬度=35.0694高度=8061.24（3）战斗机的东部，北部和垂直速度smV/7716.393esmV/5612.267nsmV/1583.3u（4）战斗机总水平距离为：m103822.263.3.2运行程序，绘制纬度-经度战斗机轨迹（以经度为横轴，纬度为纵轴）；如图：3.3.3运行程序，绘制战斗机的高度的曲线（以时间为横轴，高度为纵轴），如图：四、心得体会通过本次的大作业，我对比力、四元数等惯性导航技术中涉及的基本概念有了更深的理解，同时再次复习方向余弦矩阵法和四元数法求解捷联惯导系统，更加熟练地掌握其算法。除此之外，本次大作业中所有程序均要涉及Matlab编程，在编写程序时，四元数的初始化非常重要，如果四元数的初始化错误，会导致所有的运算结果错误。在编程过程中，每当遇到问题，我就会上网查一些资料，并与周边同学进行深入讨论，学习并解决问题。这进一步提高了我的自学能力和对知识综合应用的能力。五、参考/协助确认本文是根据本人研究成果，并参考相关文献资料整合而成，非本人完全独立研究完成，编程部分很大程度上参考了百度百科及周边同学的研究成果。另外，报告形式上借鉴了上届学长学姐的形式。本人对作业中的某些结论不负完全责任，特此声明。附录一C1=[1,0,00,cos(10/180*pi),sin(10/180*pi)0,-sin(10/180*pi),cos(10/180*pi)];%第一次转动方向余弦阵C2=[cos(30/180*pi),0,-sin(30/180*pi)0,1,0sin(30/180*pi),0,cos(30/180*pi)];%第二次转动方向余弦阵C3=[cos(-50/180*pi),sin(-50/180*pi),0-sin(-50/180*pi),cos(-50/180*pi),00,0,1];%第三次转动方向余弦阵A=C3*C2*C1*[0;0;9.8]%计算转动后加速度的输出norm(A)%检验输出是否正确附录二q1=[cos(10/360*pi);0;0;sin(10/360*pi)];%第一次转动四元数q2=[cos(30/360*pi);sin(30/360*pi);0;0];%第二次转动四元数q3=[cos(-50/360*pi);0;sin(-50/360*pi);0];%第三次转动四元数r=quml(q1,q2);%计算四元数乘积q=quml(r,q3);A1=[q(1)q(2)q(3)q(4)-q(2)q(1)q(4)-q(3)-q(3)-q(4)q(1)q(2)-q(4)q(3)-q(2)q(1)]A2=[q(1)-q(2)-q(3)-q(4)q(2)q(1)q(4)-q(3)q(3)-q(4)q(1)q(2)q(4)q(3)-q(2)q(1)]A=A1*A2;%G=A*[0;0;0;9.8]%计算转动后的弹体加速度计的输出附录三%已知参数k=10;K=5400;T=1;Gyro_pulse=0.1/3600/180*pi;Acc_pulse=9.8/1000000;R=6368000;g=9.8;W_earth=0.00007292;%初始导航参数和地球参数Q=zeros(5401,4);%定义变换四元数矩阵Q(1,:)=[cos((-45/2)/180*pi),0,0,sin((-45/2)/180*pi)];Longitude=zeros(1,5401);%定义长度为5400的经度数组Latitude=zeros(1,5401);%定义长度为5400的纬度数组Height=zeros(1,5401);%定义长度为5400的高度数组Longitude(1)=122;%初始化经度，纬度，高度Latitude(1)=35;Height(1)=25;%定义速度矩阵，供画图用Ve=zeros(1,5401);%东向速度Vn=zeros(1,5401);%北向速度Vu=zeros(1,5401);%天向速度X=zeros(1,5401);%定义加速度矩阵fe=zeros(1,5400);fn=zeros(1,5400);fu=zeros(1,5400);FE=zeros(1,5400);FN=zeros(1,5400);FU=zeros(1,5400);loadgout.mat;loadaout.mat;forN=1:K%位置、速度迭代q=zeros(11,4);q(1,:)=Q(N,:);forn=1: