惯性导航作业

一、数据说明：1：惯导系统为指北方位的捷连系统。初始经度为116.344695283度、纬度为39.975172度，高度h为30米。初速度v0=［-9.993908270;0.000000000;0.348994967］。2：jlfw中为600秒的数据，陀螺仪和加速度计采样周期分别为为1/100秒和1/100秒。3：初始姿态角为［2190］（俯仰，横滚，航向，单位为度），jlfw.mat中保存的为比力信息f_INSc（单位m/sA2）、陀螺仪角速率信息wib」NSc（单位rad/s），排列顺序为一〜三行分别为X、Y、Z向信息.4:航向角以逆时针为正。5:地球椭球长半径re=6378245;地球自转角速度wie=7.292115147e-5;重力加速度g=g0*（1+gk1*c33A2）*（1-2*h/re）/sqrt（1-gk2*c33A2）；g0=9.7803267714;gk1=0.00193185138639;gk2=0.00669437999013;c33=sin（lat纬度）;二、作业要求：1：可使用MATLAB语言编程，用MATLAB编程时可使用如下形式的语句读取数据：loadD:\…文件路径…\jlfw,便可得到比力信息和陀螺仪角速率信息。用角增量法。2：（1）以系统经度为横轴，纬度为纵轴（单位均要转换为：度）做出系统位置曲线图；（2）做出系统东向速度和北向速度随时间变化曲线图（速度单位：m/s，时间单位：s）；（3）分别做出系统姿态角随时间变化曲线图（俯仰，横滚，航向,单位转换为：度，时间单位：s）；以上结果均要附在作业报告中。3：在作业报告中要写出“程序流程图、现阶段学习小结”，写明联系方式。（注意程序流程图不是课本上的惯导解算流程，而是你程序分为哪几个模块、是怎样一步步执行的,什么位置循环等,让别人根据该流程图能够编出相应程序）（学习小结按条写，不用写套话）4：作业以纸质报告形式提交，附源程序。三、基本原理和公式1、初始姿态矩阵的确定：根据初始姿态角求四元数：V9Y.V.0.Yq=cos-^bcos—cos——sin-^bsin—sin—TOC\o"1-5"\h\z0 2 2 2 2 2 2V 9Y V 9Yq=cos—absin—cos——sin—abcos—sin—1 2 2 2 2 2 2V 9Y V 9Yq=cos-^bcos—sin—+sin~^bsin—cos—2 2 2 2 2 2 2V 9Y V 9Y22q=cos—absin—sin—+sin-^bcos—cos—223 2 2 2 2再根据四元数求方向余弦矩阵的初始矩阵：q2+q2q2+q2-q2-q20(12)32(qq-qq)(1 2 03)2(qq+qq)

1 3 022(qq+qq)12q2-q2+q-qq23 01)32(qq-qq)13 02、(qq+qq)2301q2-q2-q2+q201232、指北方位系统的运动解算：“平台”指令角速度为：Vt——Ryt()Vt31=

it3ie3cos31=

it3ieie Rxtsin(L)+Xt-tan(L)Rxt加速度计获得的比力信息户为载体坐标系中各个轴向的比力，而我们需ib要的比力ft为地理坐标系中各个轴向的比力，它们之间应用矩阵。做变换：it bft=CtXfb

itbibCt=(Cb)T=(Cb)-1

bt t根据比力信息可以求出各个方向上的加速度:Vt=ft+Vt=ft+(23sin(L)+xibx ieVt=ft—(23sin(L)+yiby ieVt=ft+(23cos(L)+zibz ieVtx-tan(L))Vt—(23cosR y iextVt Vt—tan(L))Vt+-^VtR xRzxt ytVt Vt—x—)Vt+—y—Vt—gRxRyxt yt(L)+VtA)VtRzxt因此可以求得速度为:Vt=JVdt+Vtx 0因此可以求得速度为:Vt=JVdt+Vtx 0x x0Vt=ftVtdt+Vt

y 0y y0载体所在位置的地理纬度L、经度入可由下列方程求得:V,TL=J-2_dt+L0R 0yt入=J匕^sec(L)dt+入0R 0xt3、四元数姿态矩阵的更新:3b=3b—Cb31tbibtit式中，3b为陀螺所测得的角速度。ib用毕卡逼近法更新Cb的值，T为采样时间

tA0=Tx3bib-0—A0—A0A0xyz[ao]=A0x0A0z—A0A0—A00A0yzxA0A0—A00匚 zyxA02=A02+A02+A020 xyzq(n+q(n+1)=(A0)X——I+384)(A0)2)——48)[a0〕;q(n)4、姿态角的求解:4、姿态角的求解:姿态角与姿态矩阵的关系:cosycosW—sincosycosW—sinysin0sinwab ab一cos0sinwabsinycosw+cosysin0sinwab abcosysinw+sinysin0coswab abcos0coswab一sinysinw—cosysin0coswab ab一sinycos0

sin0

cosycos0T33①=tan-iIT22式中0,y,w分别为俯仰角，滚转角和偏航角，以逆时针为正方向，而课本ab上是以顺时针为正，因此需要对课本上的公式进行修改，将呷劭代入原公式可得现公式。如果记rtTT]111213Cb=TTTt212223TTTL313233则由以上两式即可解算出姿态角：0=sin-i(T)23(

y=tan-1(四、程序流程图五、结果经纬度位髭曲线经度产12南北方向速度时间总懑转角时间总六、小结这次作业是捷联惯导的解算，是利用上次作业的结果对数据进行处理。和上次不同，这次遇到了较多的问题。首先，对捷联惯导的基本原理理解的不够深刻，比如坐标系的转换，四元数微分方程的求解。其次，由于课本的姿态角是以顺时针为正，而原始数据是以逆时针为正，因此需要对书上的公式进行修改，在这个过程中就出现了许多问题，比如正负号问题。总之，这次作业弥补了学习上的不足，使我对基本原理理解更为深刻，也初步了解惯导的基本操作。七、程序clccleara=load('C:\Users\Administrator\Documents\MATLAB/jlfw.dat');Wib_INSc=a(:,2:4)';f_INSc=a(:,5:7)';%第一列：数据包序号第二至四列：分别为东、北、天向陀螺仪角速率信息wib_INSc(单位：rad/s)%第五至七列：分别为东、北、天向比力信息f_INSc(单位：m/s^2).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);%q3fori=1:60000g=g0*(1+gk1*sin(L(i)A2)*(1-2*H/Re)/sqrt(1-gk2*sin(L(i)A2)));%计算重力加速度Rx(i)=Re/(1-e*(sin(L(i)))人2);%根据纬度计算卯酉圈曲率半径Ry(i)=Re/(1+2*e-3*e*(sin(L(i)))A2);%根据纬度计算子午圈曲率半径%求解四元数姿态矩阵q0=q(1,i);q1=q(2,i);q2=q(3,i);q3=q(4,i);Ctb=[q0A2+q1A2-q2A2-q3A2,2*(q1*q2+q0*q3),2*(q1*q3-q0*q2);2*(q1*q2-q0*q3),q2A2-q3A2+q0A2-q1A2,2*(q2*q3+q0*q1);2*(q1*q3+q0*q2),2*(q2*q3-q0*q1),q3A2-q2A2-q1A2+q0A2;];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;%求取迭代矩阵中的各AthetaA=[0-x-y-z;x0z-y;y-z0x;zy-x0];%求取迭代矩阵[Atheta]T=xA2+yA2+zA2;%计算[Atheta]A2的q(:,i+1)=((1-T/8+TA2/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;elsepsi(i+1)=atan(-Ctb(2