(完整)北航惯性导航作业二.编辑整理:尊敬的读者朋友们:这里是精品文档编辑中心,本文档内容是由我和我的同事精心编辑整理后发布的,发布之前我们对文中内容进行仔细校对,但是难免会有疏漏的地方,但是任然希望((完整)北航惯性导航作业二.)的内容能够给您的工作和学习带来便利。
同时也真诚的希望收到您的建议和反馈,这将是我们进步的源泉,前进的动力。
本文可编辑可修改,如果觉得对您有帮助请收藏以便随时查阅,最后祝您生活愉快业绩进步,以下为(完整)北航惯性导航作业二.的全部内容。
惯性导航作业一、数据说明: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/s^2)、陀螺仪角速率信息wib_INSc(单位rad/s),排列顺序为一~三行分别为X、Y、Z向信息.4:航向角以逆时针为正.5:地球椭球长半径re=6378245;地球自转角速度wie=7。
292115147e-5;重力加速度g=g0*(1+gk1*c33^2)*(1-2*h/re)/sqrt(1—gk2*c33^2);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、指北方位系统的运动解算:“平台"指令角速度为:加速度计获得的比力信息为载体坐标系中各个轴向的比力,而我们需要的比力为地理坐标系中各个轴向的比力,它们之间应用矩阵做变换:根据比力信息可以求出各个方向上的加速度:0123coscos cos sin sin sin 222222cos sin cos sin cos sin222222cos cos sin sin sin cos222222cos sin sin sin cos cos222222ab ab ab ab ab ab ab ab q q q q ψψθγθγψψθγθγψψθγθγψψθγθγ=-=-=+=+()()()()()()222201231203130222221203012323012222130223010123222222b t q q q q q q q q q q q q C q q q q q q q q q q q q q q q q q q q q q q q q ⎡⎤+--+-⎢⎥=--+-+⎢⎥⎢⎥+---+⎣⎦()()co s sin tan ()ty yt tt xit ie xt tx ie xt V R V L R V L L R ωωω⎡⎤-⎢⎥⎢⎥⎢⎥⎢⎥=+⎢⎥⎢⎥⎢⎥+⎢⎥⎣⎦bib f titf tb C ()()1t t bit b ibT t b b bttf C f C C C -=⨯==因此可以求得速度为:载体所在位置的地理纬度L 、经度可由下列方程求得:3、四元数姿态矩阵的更新:式中,为陀螺所测得的角速度.用毕卡逼近法更新的值,T 为采样时间4、姿态角的求解:姿态角与姿态矩阵的关系:()()()()(2s in ta n ())(2c o s )(2s in ta n ())(2c o s )t t t t t t x xx ib xie y ie zx t x t t ty t tt t xy ib yie x zx t y tt ty t tt t xzib zie x y x t y tV V V f L L V L V R R V V V f L L V V R R V V V f L V V gR R ωωωω•••=++-+=-++=+++-0t t t t xx x t tt t y y y V V dt V V V dt V ••=+=+⎰⎰λ0000)sec(λλ+=+=⎰⎰dt L R V L dt RV L txt xttytytb b b t t b i b t i t C ωωω=-bib ωbt C bi b T θω∆=⨯[]0000x y z x z y y z x z y x θθθθθθθθθθθθθ-∆-∆∆⎡⎤⎢⎥∆∆-∆⎢⎥∆=⎢⎥∆-∆∆⎢⎥∆∆-∆⎢⎥⎣⎦22220x y z θθθθ∆=∆+∆+∆()()()()[]()2420001118384248q n I q n θθθθ⎧⎫⎛⎫⎛⎫∆∆∆⎪⎪+=-++-∆ ⎪ ⎪⎨⎬ ⎪ ⎪⎪⎪⎝⎭⎝⎭⎩⎭(完整)北航惯性导航作业二.式中,,分别为俯仰角,滚转角和偏航角,以逆时针为正方向,而课本上是以顺时针为正,因此需要对课本上的公式进行修改,将代入原公式可得现公式。
如果记则由以上两式即可解算出姿态角:四、程序流程图c o s c o s s i n s i n s i n c o s s i n s i n s i n c o s s i n c o s c o s s i n c o s c o s s i n s i n c o s c o s s i n s i n s i n s i n c o s s i n c o s c o s c o s a b a b a b a b b t a b a b a b a b a b a b C γψγθψγψγθψγθθψθψθγψγθψγψγθψγθ-+-⎡⎤⎢⎥=-⎢⎥⎢⎥+--⎣⎦θγab ψab ψ-111213212223313233b t T T T C T T T T T T ⎡⎤⎢⎥=⎢⎥⎢⎥⎣⎦()1231133312122s i n t a n t a n T T T T T θγϕ---=⎛⎫=- ⎪⎝⎭⎛⎫= ⎪⎝⎭五、结果六、小结这次作业是捷联惯导的解算,是利用上次作业的结果对数据进行处理。
和上次不同,这次遇到了较多的问题。
首先,对捷联惯导的基本原理理解的不够深刻,比如坐标系的转换,四元数微分方程的求解.其次,由于课本的姿态角是以顺时针为正,而原始数据是以逆时针为正,因此需要对书上的公式进行修改,在这个过程中就出现了许多问题,比如正负号问题.总之,这次作业弥补了学习上的不足,使我对基本原理理解更为深刻,也初步了解惯导的基本操作.七、程序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);%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=[q0^2+q1^2—q2^2-q3^2,2*(q1*q2+q0*q3),2*(q1*q3-q0*q2);2*(q1*q2—q0*q3),q2^2-q3^2+q0^2—q1^2,2*(q2*q3+q0*q1);2*(q1*q3+q0*q2),2*(q2*q3-q0*q1),q3^2-q2^2—q1^2+q0^2;];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];%求取迭代矩阵[Δtheta] T=x^2+y^2+z^2; %计算[Δtheta]^2的q(:,i+1)=((1—T/8+T^2/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,1)/Ctb(2,2))—pi;endif(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;elsegamma(i+1)=atan(—Ctb(1,3)/Ctb(3,3))-pi;endendfigure(1)plot(L*180/pi,Lambda*180/pi);title(’经纬度位置曲线’);xlabel('经度/°');ylabel('纬度/°’);grid ont=0:0。