第二次实验报告.docx

惯性导航原理》第二次大作业姓名：ff学号： SY邮箱： w一、题目分析本题已知的信息是陀螺仪和加速度计在各个时刻的采样值，以及初始经纬 度、高度、姿态角和速度值需要求解指北方位捷联系统的运动轨迹和姿态角变 化在利用方向余弦方法对惯性导航系统进行测算时，刚体空间位置用固连于刚 体的动坐标系对固定参考系各轴的九个方向余弦来确定，九个方向余弦角存在六 个约束条件，计算比较繁琐，模型也比较复杂如果在计算过程中引入四元数， 则可以通过坐标系的一次转动，实现方向余弦方法中的三次坐标旋转原理图如 下：从原理图可以清楚的看出，通过捷联姿态矩阵 可以将任意姿态的平台坐标系下的比力数据转换到地理坐标系下，然后通过指北方位平台式惯导解算的方法 即可以得到任意时刻载体的位置和速度信息关键在于捷联姿态矩阵的求解 在这里应用四元数知识进行解算二、程序流程框图根据已知信息以及捷联式惯导系统的基本力学编排方程可知基本求解过程 如下：第一步，先根据初始的姿态角确定初始的四元数值，进而可以列写出本体系 相对于导航系的方向余弦矩阵，然后将加速度计的测量信息经过余弦阵由导航系 变换到本体系中第二步，根据变换后的比力信息和初始的经纬度、高度和速度值进行指北方 位系统的运动解算，可以求出速度信息、指令角速度信息和位置信息。

第三步，根据第二步中求解出来的指令角速度信息，对其经过余弦矩阵变换 到本体系中，陀螺仪测量到的信息和变换后的指令角速度相减得到本体系相对于 导航系的角速度在本体系中的分解第四步，根据四元数的运动学微分方程求解出下一时刻的四元数，根据四元 数和欧拉角之间的关系求解出新的姿态角接着进行以上两步的运算经过以上几步就可以对指北方位捷联惯导系统进行解算，详细流程如下图所 示:o tit三、导航结果1.系统位置坐标曲线图速度变化时间，秒3. 系统俯仰角随时间变化曲线图20-2100500600-25o俯仰角度变化200 300 400时间俐、4. 系统滚转角随时间变化曲线图00100500600200 300 400时间俐、滚转角度变化5. 系统航向角随时间变化曲线图6．系统纬度、经度、东向速度、北向速度的终点值经度（度）纬度（度）东向速度（m/s）北向速度（m/s）116.302740.00800.17419.9944四、现阶段学习小结根据自己的惯导专业方面的知识基础，在这几个周中我对书中的推导公式进 行了自行推导学习，同时在老师讲解的时候注重观察老师在推导公式的时候侧重 点在哪里，以及对公式的推导需要注意的问题和各种技巧。

经过最近的学习，在 老师讲解公式推导的时候能够很好的进行把握通过作业的练习，对书中公式的应用有了深刻的认识，加深了印象，对工程 应用中惯导系统工作原理机制有了初步的认识此外，对书中理论的推导有了系 统的认识该课程的学习需要扎实的力学、数学功底，这方面通过学习，必须融 会贯通五、源程序clc close all %定义常量 dt=0.01;total=60001;Re=6378245;%地球椭球长半径 wie=7.292115147e-5;%地球自转角速度 e=l/298.3;%偏心率 pi=3.141592654;g0=9.7803267714; gk1=0.00193185138639;gk2=0.00669437999013;%定义初始值lamda(l)=116.344695283*pi/180;%经度L(l)=39.975172*pi/180;%纬度h=30;%高度v0=[-9.993908270;0.000000000;0.348994967];%初速度att 0=[2 1 90]*pi/180;%俯仰，横滚，航向，单位为度%加载数据xls=load('C:\Users\blxh\Desk top\第二次作业程序资料 \jlfw.ma t'); f=xls.f_INSc; %f(m，n) f(3，60001)wib=xls.wib_INSc;%wib(m，n) wib(3，60001) 弧度每秒Vx(1)=v0(1);Vy(1)=v0(2);Vz(1)=v0(3);q0=cos(att0(3)/2)*cos(att0(1)/2)*cos(att0(2)/2)-sin(att0(3)/2)*sin(att0(1)/2)*s in(a tt 0(2)/2);%初始四元数q1=cos(att0(3)/2)*sin(att0(1)/2)*cos(att0(2)/2)-sin(att0(3)/2)*cos(att0(1)/2)*s in(att0(2)/2);q2=cos(att0(3)/2)*cos(att0(1)/2)*sin(att0(2)/2)+sin(att0(3)/2)*sin(att0(1)/2)*c os(att0(2)/2);q3=cos(att0(3)/2)*sin(att0(1)/2)*sin(att0(2)/2)+sin(att0(3)/2)*cos(att0(1)/2)*c os(att0(2)/2);Q=[q0，q1，q2，q3]';At b=[q0"2+q「2-q2"2-q3"2,2*(ql*q2-q0*q3),2*(ql*q3+q0*q2), ];%c tb2*(q1*q2+q0*q3), q0 2-ql 2+q2 2-q3 2*(q2*q3-q0*ql),2*(q1*q3-q0*q2); ...2*(q2*q3+q0*q1); ...q0 2-ql 2-q2 2+q3 2...%计算流程iuljor+alRX(iY(l、Re)*(Te*sin(L(i))*sin(L(i)))八ek^Rxul'RX-t Ry (i) H (l、Re) * (l+2*e——3*e*sin (L (i)) *sin (L (i)))八ek^Ryul'RyH- c33usin(L(i))- gugo* (l+gkl*c33>2) * (T2*h、Re) 一 sqrr+(Tgk2*c33>2)八蛊Hus^g 手斗ww-tbb wir+r+丄——vy (i) *Ry (i)八 wie*cos (L (i)) +VX (i) *RX (i)八 wie*sin(L (i)) +VX (i) *RX (i) *tan(L (i)二八 wir+bHA<-+b*wi<-+5T手 war+ch wibbuwibri)^B>ru^wib wr+bbuwibb——wi<-+三r+hear+H (w-tbb) *d5T手>進*d——r+hea<-+H<-+hea<-+(1) >2+r+hear+(2) >2+r+hear+(3) >2 八 dr+uheaH-丄 0 “——<-+hear+(l) “——r+hea<-+(2) “——r+hea<-+(3)八•： r+hear+(1)“ 0-r+hear+(3) “ ——r+hea<-+(2)八•： r+hear+(2) “ ——r+heaH-(3) “ 0-r+hear+(1)八•： r+hear+(3) “r+hear+(2) “ ——r+heaH-(1)0•:%V((Td——r+hea<8) *eye (4) +p5——d——<-+hea<48)*dr+——<-+hea<-+)*Q-qoQp1) -qTQ(N 1) -q2zp1) -q3z4 1)- Ar+b丄qo>2+ql>2——q2>2——q3>2“ 2* (ql*q2+qo*q3) “ 2*(ql*q2——qo*q3) “ qo>2——ql>2+q2>2——q3>2. 2* (ql*q3+qo*q2) “ 2* (q2*q3——qo*ql) “T%c<-+bar+<-+——<-+hea<-+(i) Hasin (Ar+b (2“ 3))八ar+r+—gam 巴 Har+an(丄1±(1“ 3)、Ar+b (3“ 3))- if A&p3)〉0 a<-+<-+——gama (i) Har+<-+——ganlal - else if ar+<-+——ganlal〈o a<-+<-+——gama (i) Har+<-+——ganlal+pi -elsea<-+<-+——gama (i) Har+<-+——ganlaTpi - endendar+<-+——feTa<-+an(丄-tb(2“ 1) \A-tb (2“ 2))八％® 雪if Ar+b(N2)〈02*(ql*q3——qo*q2)八•： 2*(q2*q3+qo*ql)八•： qO 2——ql 2——q2 2+q3 2 :.att_fe(i)=att_fe1+pi;else if att_fe1>0att_fe(i)=att_fe1;elseatt_fe(i)=att_fe1+2*pi;endendft (:, i)二inv(Atb)*f (:, i);%系转换%计算速度Vx(i+1)= Vx(i) + (ft(1,i)+(2*wie*sin(L(i))+Vx(i)*tan(L(i))*Rx(i))*Vy(i)-(2*wie*cos(L(i))+Vx(i)*R x(i))*Vz(i))*dt;Vy(i+1)= Vy(i) +(ft(2,i)-(2*wie*sin(L(i))+Vx(i)*tan(L(i))*Rx(i))*Vx(i)+(Vy(i)*Ry(i))*Vz(i))*dt;Vz(i+1)= Vz(i) + (ft(3,i)+(2*wie*cos(L(i))+Vx(i)*Rx(i))*Vx(i)+(Vy(i)*Ry(i))*Vy(i)-g)*dt;%计算经纬度L(i+1)=L(i)+(Vy(i)*Ry(i))*dt; lamda(i+1)=lamda(i)+(Vx(i)*Rx(i)/cos(L(i)))*dt;enddisp('终点经度为：')L(60001)*180/pidisp ('终点纬度为：')lamda(60001)*180/pidisp ('东向速度为：')Vx(60001)disp ('北向速度为：')Vy(60001)%画图figure(1);plot(lamda*180/pi,L*180/pi)tex t(lamda(60001)*180/pi,L(60001)*180/pi,[num2s tr(lamda(60001)*180/pi,'终点经纬 度坐标\n(%.3f'),',',num2str(L(60001)*180/pi,'%.3f'),')'],'EdgeColor','red','BackgroundC olor',[.7 .9 .7],'VerticalAlignment','bottom')grid ontitle('位置变化')xlabel ('经度lamda/°')yl。