北航惯性导航作业二.doc

惯性导航作业一、数据说明：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/s2)、陀螺仪角速率信息wib_INSc(单位rad/s)，排列顺序为 一三行分别为X、Y、Z向信息.4: 航向角以逆时针为正。5:地球椭球长半径re=6378245;地球自转角速度wie=7.292115147e-5;重力加速度g=g0*(1+gk1*c332)*(1-2*h/re)/sqrt(1-gk2*c332)；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、姿态角的求解：姿态角与姿态矩阵的关系：式中,分别为俯仰角，滚转角和偏航角，以逆时针为正方向，而课本上是以顺时针为正，因此需要对课本上的公式进行修改，将代入原公式可得现公式。如果记则由以上两式即可解算出姿态角： 四、程序流程图五、结果六、小结这次作业是捷联惯导的解算，是利用上次作业的结果对数据进行处理。和上次不同，这次遇到了较多的问题。首先，对捷联惯导的基本原理理解的不够深刻，比如坐标系的转换，四元数微分方程的求解。其次，由于课本的姿态角是以顺时针为正，而原始数据是以逆时针为正，因此需要对书上的公式进行修改，在这个过程中就出现了许多问题，比如正负号问题。总之，这次作业弥补了学习上的不足，使我对基本原理理解更为深刻，也初步了解惯导的基本操作。七、程序clccleara=load(C:UsersAdministratorDocumentsMATLAB/jlfw.dat);Wib_INSc=a(:,2:4);f_INSc=a(:,5:7);%第一列：数据包序号 第二至四列：分别为东、北、天向陀螺仪角速率信息wib_INSc（单位：rad/s）%第五至七列：分别为东、北、天向比力信息f_INSc（单位：m/s2）.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); %定义四阶矩阵L(1,1)=39.975172/180*pi;%纬度初始值 单位：弧度Lambda(1,1)=116.344695283/180*pi;%经度初始值 单位：弧度Vx(1,1)=-9.993908270;%初始速度x方向分量Vy(1,1)=0;%初始速度y方向分量Vz(1,1)=0.348994967;%初始速度z方向分量Wibx(1,:)=a(:,2); %提取陀螺正东方向角速率并定义Wiby(1,:)=a(:,3); %提取陀螺正北方向角速率并定义 Wibz(1,:)=a(:,4); %提取陀螺天向角速率并定义 fibbx(1,:)=a(:,5);%x方向的比力数据fibby(1,:)=a(:,6);%y方向的比力数据fibbz(1,:)=a(:,7);%z方向的比力数据g0=9.7803267714;Wie=7.292115147e-5;%地球自转角速度Re=6378245;%长半径e=1/298.3;%椭圆度t=0.01;%采样时间psi(1,1)=90/180*pi;%偏航角初始值 单位：弧度theta(1,1)=2/180*pi;%俯仰角初始值 单位：弧度gamma(1,1)=1/180*pi;%滚转角初始值 单位：弧度gk1=0.00193185138639;gk2=0.00669437999013;H=30;%高度%求解四元数系数q0,q1,q2,q3的初值q(1,1)=cos(psi(1,1)/2)*cos(theta(1,1)/2)*cos(gamma(1,1)/2)-sin(psi(1,1)/2)*sin(theta(1,1)/2)*sin(gamma(1,1)/2); %q0q(2,1)=cos(psi(1,1)/2)*sin(theta(1,1)/2)*cos(gamma(1,1)/2)-sin(psi(1,1)/2)*cos(theta(1,1)/2)*sin(gamma(1,1)/2); %q1q(3,1)=cos(psi(1,1)/2)*cos(theta(1,1)/2)*sin(gamma(1,1)/2)+sin(psi(1,1)/2)*sin(theta(1,1)/2)*cos(gamma(1,1)/2); %q2q(4,1)=cos(psi(1,1)/2)*sin(theta(1,1)/2)*sin(gamma(1,1)/2)+sin(psi(1,1)/2)*cos(theta(1,1)/2)*cos(gamma(1,1)/2); %q3for i=1:60000g=g0*(1+gk1*sin(L(i)2)*(1-2*H/Re)/sqrt(1-gk2*sin(L(i)2);%计算重力加速度Rx(i)=Re/(1-e*(sin(L(i)2);%根据纬度计算卯酉圈曲率半径Ry(i)=Re/(1+2*e-3*e*(sin(L(i)2);%根据纬度计算子午圈曲率半径%求解四元数姿态矩阵q0=q(1,i);q1=q(2,i);q2=q(3,i);q3=q(4,i);Ctb=q02+q12-q22-q32,2*(q1*q2+q0*q3),2*(q1*q3-q0*q2); 2*(q1*q2-q0*q3),q22-q32+q02-q12,2*(q2*q3+q0*q1); 2*(q1*q3+q0*q2),2*(q2*q3-q0*q1),q32-q22-q12+q02;Cbt=Ctb;fibt=Cbt*fibbx(i);fibby(i);fibbz(i);%比力数据fibtx(i)=fibt(1,1);fibty(i)=fibt(2,1);fibtz(i)=fibt(3,1);Vx(1,i+1)=(fibtx(i)+(2*Wie*sin(L(i)+Vx(i)*tan(L(i)/Rx(i)*Vy(i)-(2*Wie*cos(L(i)+Vx(i)/Rx(i)*Vz(i)*t+Vx(i);%计算速度x方向分量Vy(1,i+1)=(fibty(i)-(2*Wie*sin(L(i)+Vx(i)*tan(L(i)/Rx(i)*Vx(i)+Vy(i)*Vz(i)/Ry(i)*t+Vy(i);%计算速度y方向分量Vz(1,i+1)=(fibtz(i)+(2*Wie*cos(L(i)+Vx(i)/Rx(i)*Vx(i)+Vy(i)*Vy(i)/Ry(i)-g)*t+Vz(i);%计算速度z方向分量Witt=-Vy(i)/Ry(i); Wie*cos(L(i)+Vx(i)/Rx(i); Wie*sin(L(i)+Vx(i)*tan(L(i)/Rx(i);%求出平台指令角速度值Wibb=Wibx(i);Wiby(i);Wibz(i);Wtbb=Wibb-Ctb*Witt;%将指令角速度转换到平台坐标系,并求解WtbbL(1,i+1)=t*Vy(i)/Ry(i)+L(i);Lambda(1,i+1)=t*Vx(i)/(Rx(i)*cos(L(i)+ Lambda(i);x=Wtbb(1,1)*t;y=Wtbb(2,1)*t;z=Wtbb(3,1)*t; %求取迭代矩阵中的各thetaA=0 -x -y -z;x 0 z -y;y -z 0 x;z y -x 0;%求取迭代矩阵thetaT=x2+y2+z2; % 计算theta2的q(:,i+1)=(1-T/8+T2/384)*I+(1/2-T/48)*A)*q(:,i);%求q0theta(i+1)=asin(Ctb(2,3); if(Ctb(2,2)=0) psi(i+1)=atan(-Ctb(2,1)/Ctb(2,2); elseif(Ctb(2,1)0) psi(i+1)=atan(-Ctb(2,1)/Ctb(2,2)+pi; else psi(i+1)=atan(-Ctb(2,1)/Ctb(2,2)-pi; end if(Ctb(3,3)0) gamma(i+1)=atan(-Ctb(1,3)/Ctb(3,3); elseif(Ctb(1,3)0) gamma(i+1)=atan(-Ctb(1,3)/Ctb(3,3)+pi; else gamma(i+1)=atan(-Ctb(1,3)/Ctb(3,3)-pi; end endfigure(1)plot(L*180/pi,Lambda*180/pi);title(经纬度位置曲线);xlabel(经度/);ylabel(纬度/);grid ont=0:0.01:600;figure(2)plot(t,Vx);title(东西方向速度);xlabel(时间/s);ylabel(速度/m/s);grid onfigure(3)plot(t,Vy);title(南北方向速度)