捷联惯导解算作业(共11页).doc

《捷联惯导解算作业(共11页).doc》由会员分享，可在线阅读，更多相关《捷联惯导解算作业(共11页).doc（11页珍藏版）》请在淘文阁 - 分享文档赚钱的网站上搜索。

1、精选优质文档-倾情为你奉上惯性导航原理第二次作业报告姓名：苏宪飞 学号：S 电话：1、题目分析本题已知的信息是陀螺仪和加速度计在各个时刻的采样值，以及初始经纬度、高度、姿态角和速度值。需要求解指北方位捷联系统的运动轨迹和姿态角变化。根据这些已知信息以及捷联式惯导系统的基本力学编排方程可知基本求解过程如下：第一步，先根据初试的姿态角确定初始的四元数值，进而可以列写出本体系相对于导航系的方向余弦矩阵，然后将加速度计的测量信息经过余弦阵由导航系变换到本体系中。第二步，根据变换后的比力信息和初始的经纬度、高度和速度值进行指北方位系统的运动解算，可以求出速度信息、指令角速度信息和位置信息。第三步，根据第

2、二步中求解出来的指令角速度信息，对其经过余弦矩阵变换到本体系中，陀螺仪测量到的信息和变换后的指令角速度相减得到本体系相对于导航系的角速度在本体系中的分解。第四步，根据四元数的运动学微分方程求解出下一时刻的四元数，根据四元数和欧拉角之间的关系求解出新的姿态角。接着进行以上两步的运算。经过以上几步就可以对指北方位捷联惯导系统进行解算。2、解算原理和公式(1)、初始姿态矩阵的确定：根据初始姿态角求四元数： 根据四元数求方向余弦矩阵： (2)、指北方位系统的运动解算：地理坐标系相对地球坐标系的角速度为： 加速度计获得的比力信息为载体坐标系中各个轴向的比力，而我们需要的比力为地理坐标系中各个轴向的比力，

3、它们之间应用矩阵做变换： 根据比力信息可以求出各个方向上的加速度：因此可以求得速度为：载体所在位置的地理纬度L、经度可由下列方程求得：(3)、四元数姿态矩阵的更新：式中，为陀螺所测角速度。用毕卡逼近法更新的值， (4)、姿态角的求解：姿态角与姿态矩阵的关系：式中,分别为俯仰角，横滚角和偏航角。如果记则由以上两式即可解算出姿态角： 3、作业结果图1：经纬度位置曲线图2：东西方向速度曲线图3：南北方向速度曲线图4：俯仰角随时间变化曲线图5：横滚角随时间变化曲线图6：偏航角随时间变化曲线4、程序流程图图7：程序流程图5、学习小结学习惯导大约两个月了，总体感觉研究生阶段学习比本科阶段有所提高，本科接触

4、的惯导只是皮毛，而研究生阶段则是充分剖析各个细节使我对惯导有了总体的把握。在学习中也看到了很多不足之处：第一：对各个公式推导还是不能全面掌握。第二：对所学知识还没有融会贯通。第三：专业基础太浅导致上课难以理解，还好老师讲的比较详细。针对这些问题，采取了一些自学手段首先对书上的公式按照老师的方法进行推导，努力回顾上课时老师教给我们的方法，而不是按照书本生搬硬套。6、源程序clear;clc;format long;%输入常值%Re=;e=1/298.3;w_ie=7.e-5; %单位为弧度dt=0.01; %步长为0.01g0=9.;gk1=0.639;gk2=0.013;%输入初值%lumda

5、0=116.; %初始位置L0=39.; %单位为度h0=30;Vx0=-9.; %初始速度Vy0=0;Vz0=0.;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,6000

6、2);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(fai

7、0/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;%导入测量值%load jlfw.mat%开始循环%lumda_t=lumda0*pi/180;L_t=L0*pi/180; %单位为弧度h_t=h0;

8、Vx_t=Vx0;Vy_t=Vy0;Vz_t=Vz0;for i=1:1:60001; Lamu0=Lamu(1,1); Lamu1=Lamu(2,1); Lamu2=Lamu(3,1); Lamu3=Lamu(4,1); Ct_b=Lamu02+Lamu12-Lamu22-Lamu32,2*(Lamu1*Lamu2+Lamu0*Lamu3),2*(Lamu1*Lamu3-Lamu0*Lamu2); 2*(Lamu1*Lamu2-Lamu0*Lamu3),Lamu02-Lamu12+Lamu22-Lamu32,2*(Lamu2*Lamu3+Lamu0*Lamu1); 2*(Lamu1*Lamu

9、3+Lamu0*Lamu2),2*(Lamu2*Lamu3-Lamu0*Lamu1),Lamu02-Lamu12-Lamu22+Lamu32; %捷联姿态阵的逆 f_b=f_INSc(1,i);f_INSc(2,i);f_INSc(3,i); f_t=Ct_bf_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*(

10、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*c332)*(1-2*h_t/Re)/sqrt(1-gk2*c332); 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*

11、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=witx_t,wity_t,witz_t; wit_b=Ct_b*wit_t; wib_b=wib_INSc(1,i);wib_INSc(2,i);wib_INSc(3,i); wtb_b=wib_b-wit_b; d_Lamu=1/2*0,-wtb_b(1,1),-wtb_b(2,1),-wtb_b(3,1); wtb_b(1,1),0,wtb_b(3,1),-

12、wtb_b(2,1); wtb_b(2,1),-wtb_b(3,1),0,wtb_b(1,1); wtb_b(3,1),wtb_b(2,1),-wtb_b(1,1),0*Lamu; Lamu=Lamu+dt*d_Lamu; xita_t=asin(Ct_b(2,3); gama_t=atan(-Ct_b(1,3)/Ct_b(3,3); fai_t=atan(Ct_b(2,1)/Ct_b(2,2); L(1,i+1)=L_t*180/pi; lumda(1,i+1)=lumda_t*180/pi; xita(1,i+1)=xita_t*180/pi; gama(1,i+1)=gama_t*180

13、/pi; fai(1,i+1)=-fai_t*180/pi; Vx(1,i+1)=Vx_t; Vy(1,i+1)=Vy_t;endt=linspace(0,601,60002);plot(lumda,L);title(经纬度位置曲线);xlabel(经度/);ylabel(纬度/);figureplot(t,Vx);title(东西方向速度);xlabel(时间/s);ylabel(速度/m/s);figureplot(t,Vy);title(南北方向速度);xlabel(时间/s);ylabel(速度/m/s);figureplot(t,xita);title(俯仰角);xlabel(时间/s);ylabel(角度/);figureplot(t,gama);title(横滚角);xlabel(时间/s);ylabel(角度/);figureplot(t,fai);title(偏航角);xlabel(时间/s);ylabel(角度/);专心-专注-专业