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;