北航2013年捷联惯导作业报告

1、惯性导航原理捷联惯导大作业姓名:埃保常学号：SY1317000联系电话：131-2345-67891计算原理分析本次系统选择的是指北方位的捷联惯导系统。捷联惯导系统与第一次作业中的平台式惯导系统的区别就在于捷联惯导是将惯性器件直接固连在载体上，没有实体的惯导平台。在导航计算中，由于惯性器件直接安装在载体上，惯性器件测量的是载体轴相对惯性空间的角速率和加速度分量，将测量信息送入由载体坐标系至平台坐标系的方向余弦矩阵就可以将捷联惯导转换为平台式惯导，从而方便解算。系统的解算原理图如下图。+-初始姿态角初始四元素加速度计组件指北方位平台惯导 ,L ,h陀螺仪组件 捷联惯导系统原理图2计算流程2.1首

2、先由初始姿态角 求得初始四元数。航向角以逆时针为正，公式如下：2.2将初始四元数带入四元素微分方程。在计算机上四元素微分方程由角增量法三阶算法实现。上式是四元素微分方程的解析解，在实际量测过程中，无法得到的连续的值，只能离散处理，所以将，展开成有限项，本算法采用三阶算法，即：2.3由四元素求出方向余弦矩阵，即本体系到地理坐标系的转换矩阵。因为地理系和本体系三个坐标轴都是相互正交的，所以方向余弦矩阵是正交矩阵，而且与互为转置关系。2.4将加速度计测量得到的本体系三个轴方向对惯性空间的比力矢量转换到地理坐标系。2.5得到矢量就可以在平台式惯导中进行速度，角速度，经纬度进行解算。2.5.1平台指令角

3、速度计算平台式惯导中平台指令角速度其中地球自转角速度地理坐标系相对地球坐标系的角速率为式中,分别为卯酉圈主曲率半径，计算公式为为地心纬度与地理纬度L近似相等，为了增加导航计算的精确性，本算法进行了转换2.5.2速度解算由比力方程 写成分量形式：将上面计算得到的和带入比力方程即得到东向、北向、天向加速度的计算式式中计算得到了每一时刻的加速度，在计算速度时，由于采样数据的离散性，需要对公式离散化处理，得到本算法中东北天方向的速度计算式：式中T为采样周期，本题中T=0.01s。2.5.3经纬度计算类似的，经纬度计算公式离散化处理为本算法使用的公式如下：2.6姿态角计算本系统的航向角以逆时针为正，所以

4、由3-1-2欧拉角得到的地理系到本体系的方向余弦矩阵表示为：（定义俯仰角为，横滚角为，航向角为）在程序计算中，以表示中第i行第j列元素则可以解算出姿态角的。因为俯仰角定义在-90°，+90°区间，与反正弦函数主值一致，因此不存在多值得问题。横滚角定义在-180°,+180°和航向角0°,360°的定义区间与反正切函数的主值不一致，这时候需要根据中元素其他值辅助判断，才确定横滚角和航向角的真值。判断依据下表。横滚角真值判断表-=横滚角所在象限横滚角真值+第一象限=主+-第二象限=+-第三象限=+-+第四象限=主航向角真值判断表-=航向角

5、所在象限航向角真值+第一象限=主+-第二象限=主+-第三象限=主+-+第四象限 =主+22.7程序流程图3计算结果3.1系统位置曲线图3.2东向北向速度曲线图3.3姿态角随时间变化4小结1）由于俞老师在课堂上已经将第二次作业的流程讲的比较清楚了，所以在做作业的过程中比较顺利。在航向角以逆时针为正的地方出了一些问题，因为课本上姿态矩阵与我们的定义不相同，需要从新推导才可以。2）本次作业把一些转换过程用函数实现了，希望可以达到突出主要流程和结构的目的。3）第二次作业只是一定程度上完成了导航计算，计算的精度和准确程度没有办法保证，没有误差分析，只是把捷联惯导系统的计算流程熟悉了一下，关于误差及对比实

6、验方面缺乏更深入的思考。4）现阶段的学习状态还比较满意，老师讲的也比较清楚，能跟得上老师的进度。附源程序clear all;load G:学习北航研一惯性导航原理第二次作业jlfw%变量初始化%初始经度为116.344695283度、纬度为39.975172度,高度h为30米。v0=-9.993908270;0.000000000;0.348994967Vx=zeros(1,60001);Vy=zeros(1,60001);Vz=zeros(1,60001);Vx(1)=-9.993908270;Vy(1)=0;Vz(1)=0.348994967;J=zeros(1,60001);L=zero

7、s(1,60001);Lc=zeros(1,60001);%L地理纬度，Lc地心纬度，J经度h=30;L(1)=39.975172*pi/180; J(1)=116.344695283*pi/180;%初始经纬度Rx=zeros(1,60001);Ry=zeros(1,60001);Re=6378245;Wie=7.292115147e-5;e=1/298.3;t=1/100;Lc(1)=L(1)*(1-e*sin(2*L(1);%由地理纬度L计算地心纬度LcRx(1)=Re/(1-e*sin(Lc(1)2); %卯酉面主曲率半径Ry(1)=Re/(1+2*e-3*e*sin(Lc(1)2);

8、%子午面主曲率半径%重力加速度的作用g0=9.7803267714;gk1=0.00193185138639;gk2=0.00669437999013;%Wtit=zeros(1,60001);Wbit=zeros(1,60001);Wtitx=zeros(1,60001);Wtity=zeros(1,60001);Wtitz=zeros(1,60001);Wbitx=zeros(1,60001);Wbity=zeros(1,60001);Wbitz=zeros(1,60001);%初始角速度Wtitx(1)=-Vy(1)/Ry(1);Wtity(1)=Vx(1)/Rx(1)+Wie*cos(

9、L(1);Wtitz(1)=Wie*sin(L(1)+(Vx(1)*tan(L(1)/Rx(1);%初始姿态角，X表示俯仰角，Y横滚，Z表示航向角X=zeros(1,60001);Y=zeros(1,60001);Z=zeros(1,60001);X(1)=2*pi/180;Y(1)=1*pi/180;Z(1)=90*pi/180;%由姿态角计算四元数初始值Q0=angle2q(X(1),Y(1),Z(1);At2b=q2cos(Q0);%读取数据文件%Fbx=f_INSc(1,:);Fby=f_INSc(2,:);Fbz=f_INSc(3,:);Wx=wib_INSc(1,:);Wy=wib

10、_INSc(2,:);Wz=wib_INSc(3,:);%我觉的可以开始导航计算了for i=1:60000%将b系比力矢量转换为t系.用列向量Fb=Fbx(i);Fby(i);Fbz(i);Ab2t=(At2b');Ft=Ab2t*Fb;%左乘%指北方位系统导航计算了g=g0*(1+gk1*sin(L(i)2)*(1-2*h/Re)/sqrt(1-gk2*sin(L(i)2);%计算该采样时刻加速度Ax=Ft(1)+(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);Ay=Ft(2)

11、-(2*Wie*sin(L(i)+Vx(i)*tan(L(i)/Rx(i)*Vx(i)+Vy(i)*Vz(i)/Ry(i);Az=Ft(3)+(2*Wie*cos(L(i)+Vx(i)/Rx(i)*Vx(i)+Vy(i)*Vy(i)/Ry(i)-g;%计算下一时刻时刻速度可以修改！（认为到采样时刻之前一直保持V（i），a保持该大小）Vx(i+1)=Ax*0.01+Vx(i);Vy(i+1)=Ay*0.01+Vy(i);Vz(i+1)=Az*0.01+Vz(i);%计算从上一时刻到该采样时刻的经纬度（认为速度在此期间保持v（i）L(i+1)=0.01*Vy(i)/Ry(i)+L(i);J(i+1

12、)=0.01*Vx(i)/(Rx(i)*cos(L(i)+J(i); %计算该采样时刻的曲率半径（因为Rx（1）是初始值，所以第一个采样时刻对应的是Rx（1+1=2）Rx(i+1)=Re/(1-e*sin(L(i+1)2); Ry(i+1)=Re/(1+2*e-3*e*sin(L(i+1)2);%计算到采样时刻时的角速度在t系的表达 %当前采样时刻速度等于历史值，经纬度和半径更新了,所以采样时刻的角速度变了Wtitx(i+1)=-Vy(i)/Ry(i+1) ; Wtity(i+1)=Vx(i)/Rx(i+1)+Wie*cos(L(i+1);Wtitz(i+1)=Wie*sin(L(i+1)+(

13、Vx(i)*tan(L(i+1)/Rx(i+1);Wtit=Wtitx(i+1);Wtity(i+1);Wtitz(i+1);%计算Wit在本体系的表达%Ab_t=inv(At2b);Wbit=(At2b)*Wtit;Wbitx(i)=Wbit(1);Wbity(i)=Wbit(2);Wbitz(i)=Wbit(3);%四元素三阶毕卡逼近法附件3角增量法Q1=(1-vector_Norm(Wx(i)-Wbitx(i),Wy(i)-Wbity(i),Wz(i)-Wbitz(i)/8)*eye(4)+(0.5-vector_Norm(Wx(i)-Wbitx(i),Wy(i)-Wbity(i),Wz

14、(i)-Wbitz(i)/48)*w2Mw(Wx(i)-Wbitx(i),Wy(i)-Wbity(i),Wz(i)-Wbitz(i)*Q0'Q0=Q1'%存储历史值%更新姿态矩阵P86 (4.2-36)At2b=q2cos(Q1);%更新欧拉角。姿态角angle=cos2angle(At2b);X(i+1)=angle(1);Y(i+1)=angle(2);Z(i+1)=angle(3);end%画图和读取数据plot(J*180/pi,L*180/pi);xlabel('经度');ylabel('纬度');title('系统位置'

15、;)tx=1:0.01:601;figure;plot(tx,Vx);xlabel('时间');ylabel('东向速度 单位 m/s');title('东向速度随时间的变化');figure;plot(tx,Vy);xlabel('时间');ylabel('北向速度 单位 m/s');title('北向速度随时间的变化');figure;plot(tx,Vz);xlabel('时间');ylabel('天向速度 单位 m/s');title('北向速度随时间

16、的变化');figure;plot(tx,X*180/pi);xlabel('时间');ylabel('俯仰角 单位 °');title('俯仰角随时间的变化');figure;plot(tx,Y*180/pi);xlabel('时间');ylabel('横滚角 单位 °');title('横滚随时间的变化');figure;plot(tx,Z*180/pi);xlabel('时间');ylabel('航向角 单位 °');tit

17、le('航向角随时间的变化');q2cos.m文件function Acos = q2cos( q )%由四元数推到方向余弦Acos= q(1)2+q(2)2-q(3)2-q(4)2 2*(q(2)*q(3)+q(1)*q(4) 2*(q(2)*q(4)-q(1)*q(3); 2*(q(2)*q(3)-q(1)*q(4) q(1)2-q(2)2+q(3)2-q(4)2 2*(q(3)*q(4)+q(1)*q(2); 2*(q(2)*q(4)+q(1)*q(3) 2*(q(3)*q(4)-q(1)*q(2) q(1)2-q(2)2-q(3)2+q(4)2 ;endangle2q.

18、m文件function q = angle2q( x,y,z )%由姿态角求四元数% 输入姿态角输出四元数 q(1)=cos(z/2)*cos(x/2)*cos(y/2)-sin(z/2)*sin(x/2)*sin(y/2); q(2)=cos(z/2)*sin(x/2)*cos(y/2)-sin(z/2)*cos(x/2)*sin(y/2); q(3)=cos(z/2)*cos(x/2)*sin(y/2)+sin(z/2)*sin(x/2)*cos(y/2); q(4)=cos(z/2)*sin(x/2)*sin(y/2)+sin(z/2)*cos(x/2)*cos(y/2);endangl

19、e2q.m文件function q = angle2q( x,y,z )%由姿态角求四元数% 输入姿态角输出四元数 q(1)=cos(z/2)*cos(x/2)*cos(y/2)-sin(z/2)*sin(x/2)*sin(y/2); q(2)=cos(z/2)*sin(x/2)*cos(y/2)-sin(z/2)*cos(x/2)*sin(y/2); q(3)=cos(z/2)*cos(x/2)*sin(y/2)+sin(z/2)*sin(x/2)*cos(y/2); q(4)=cos(z/2)*sin(x/2)*sin(y/2)+sin(z/2)*cos(x/2)*cos(y/2);end

20、cos2angle.m文件function angle = cos2angle( At2b )%姿态矩阵到欧拉角的转换 % 主要是处理多值的问题%俯仰角 x轴 不存在多值的问题angle(1)=asin(At2b(2,3); %横滚角 y轴 if(abs(At2b(3,3)>0.001) angle(2)=atan(-At2b(1,3)/At2b(3,3);%横滚角 主值 if(At2b(3,3)>0) angle(2)=atan(-At2b(1,3)/At2b(3,3);%横滚角 else if(-At2b(1,3)>0) angle(2)=atan(-At2b(1,3)/

21、At2b(3,3)+pi; else angle(2)=atan(-At2b(1,3)/At2b(3,3)-pi; end end else if(-At2b(1,3)>0) angle(2)=pi/2; else angle(2)=-pi/2; end end %航向角Z 轴 但是航向角是定义在0到360°上面的而且航向角以逆时针为正 课本上的P79页 姿态矩阵不能用 if(abs(At2b(2,2)>0.0001) angle(3)=atan(-At2b(2,1)/At2b(2,2);%横滚angle主值 if(At2b(2,2)>0) if(At2b(2,1)>0) angle(3)=atan(-At2b(2,1)/At2b(2,2)+2*pi;%角在第4象限 else angle(3)=atan(-At2b(2,1)/At2b(2,2);%角在第1象限 end else %A(2,2)<0 angle(3)=atan(-At2b(2,1)/At2b(2,2)+pi;%角在第二或第三象限 %if(At2b(2,1)>0) % angle(3)=atan(At2b(2,1)/At2b(2,2)+pi;