北航2013年捷聯(lián)慣導(dǎo)作業(yè)報(bào)告.

1、慣性導(dǎo)航原理捷聯(lián)慣導(dǎo)大作業(yè)姓名:埃保常學(xué)號(hào)：SY1317000聯(lián)系電話：131-2345-67891計(jì)算原理分析本次系統(tǒng)選擇的是指北方位的捷聯(lián)慣導(dǎo)系統(tǒng)。捷聯(lián)慣導(dǎo)系統(tǒng)與第一次作業(yè)中的平臺(tái)式慣導(dǎo)系統(tǒng)的區(qū)別就在于捷聯(lián)慣導(dǎo)是將慣性器件直接固連在載體上，沒有實(shí)體的慣導(dǎo)平臺(tái)。在導(dǎo)航計(jì)算中，由于慣性器件直接安裝在載體上，慣性器件測量的是載體軸相對(duì)慣性空間的角速率和加速度分量，將測量信息送入由載體坐標(biāo)系至平臺(tái)坐標(biāo)系的方向余弦矩陣就可以將捷聯(lián)慣導(dǎo)轉(zhuǎn)換為平臺(tái)式慣導(dǎo)，從而方便解算。系統(tǒng)的解算原理圖如下圖。+-初始姿態(tài)角初始四元素加速度計(jì)組件指北方位平臺(tái)慣導(dǎo) ,L ,h陀螺儀組件 捷聯(lián)慣導(dǎo)系統(tǒng)原理圖2計(jì)算流程2.1首

2、先由初始姿態(tài)角 求得初始四元數(shù)。航向角以逆時(shí)針為正，公式如下：2.2將初始四元數(shù)帶入四元素微分方程。在計(jì)算機(jī)上四元素微分方程由角增量法三階算法實(shí)現(xiàn)。上式是四元素微分方程的解析解，在實(shí)際量測過程中，無法得到的連續(xù)的值，只能離散處理，所以將，展開成有限項(xiàng)，本算法采用三階算法，即：2.3由四元素求出方向余弦矩陣，即本體系到地理坐標(biāo)系的轉(zhuǎn)換矩陣。因?yàn)榈乩硐岛捅倔w系三個(gè)坐標(biāo)軸都是相互正交的，所以方向余弦矩陣是正交矩陣，而且與互為轉(zhuǎn)置關(guān)系。2.4將加速度計(jì)測量得到的本體系三個(gè)軸方向?qū)T性空間的比力矢量轉(zhuǎn)換到地理坐標(biāo)系。2.5得到矢量就可以在平臺(tái)式慣導(dǎo)中進(jìn)行速度，角速度，經(jīng)緯度進(jìn)行解算。2.5.1平臺(tái)指令角

3、速度計(jì)算平臺(tái)式慣導(dǎo)中平臺(tái)指令角速度其中地球自轉(zhuǎn)角速度地理坐標(biāo)系相對(duì)地球坐標(biāo)系的角速率為式中,分別為卯酉圈主曲率半徑，計(jì)算公式為為地心緯度與地理緯度L近似相等，為了增加導(dǎo)航計(jì)算的精確性，本算法進(jìn)行了轉(zhuǎn)換2.5.2速度解算由比力方程 寫成分量形式：將上面計(jì)算得到的和帶入比力方程即得到東向、北向、天向加速度的計(jì)算式式中計(jì)算得到了每一時(shí)刻的加速度，在計(jì)算速度時(shí)，由于采樣數(shù)據(jù)的離散性，需要對(duì)公式離散化處理，得到本算法中東北天方向的速度計(jì)算式：式中T為采樣周期，本題中T=0.01s。2.5.3經(jīng)緯度計(jì)算類似的，經(jīng)緯度計(jì)算公式離散化處理為本算法使用的公式如下：2.6姿態(tài)角計(jì)算本系統(tǒng)的航向角以逆時(shí)針為正，所以

4、由3-1-2歐拉角得到的地理系到本體系的方向余弦矩陣表示為：（定義俯仰角為，橫滾角為，航向角為）在程序計(jì)算中，以表示中第i行第j列元素則可以解算出姿態(tài)角的。因?yàn)楦┭鼋嵌x在-90°，+90°區(qū)間，與反正弦函數(shù)主值一致，因此不存在多值得問題。橫滾角定義在-180°,+180°和航向角0°,360°的定義區(qū)間與反正切函數(shù)的主值不一致，這時(shí)候需要根據(jù)中元素其他值輔助判斷，才確定橫滾角和航向角的真值。判斷依據(jù)下表。橫滾角真值判斷表-=橫滾角所在象限橫滾角真值+第一象限=主+-第二象限=+-第三象限=+-+第四象限=主航向角真值判斷表-=航向角

5、所在象限航向角真值+第一象限=主+-第二象限=主+-第三象限=主+-+第四象限 =主+22.7程序流程圖3計(jì)算結(jié)果3.1系統(tǒng)位置曲線圖3.2東向北向速度曲線圖3.3姿態(tài)角隨時(shí)間變化4小結(jié)1）由于俞老師在課堂上已經(jīng)將第二次作業(yè)的流程講的比較清楚了，所以在做作業(yè)的過程中比較順利。在航向角以逆時(shí)針為正的地方出了一些問題，因?yàn)檎n本上姿態(tài)矩陣與我們的定義不相同，需要從新推導(dǎo)才可以。2）本次作業(yè)把一些轉(zhuǎn)換過程用函數(shù)實(shí)現(xiàn)了，希望可以達(dá)到突出主要流程和結(jié)構(gòu)的目的。3）第二次作業(yè)只是一定程度上完成了導(dǎo)航計(jì)算，計(jì)算的精度和準(zhǔn)確程度沒有辦法保證，沒有誤差分析，只是把捷聯(lián)慣導(dǎo)系統(tǒng)的計(jì)算流程熟悉了一下，關(guān)于誤差及對(duì)比實(shí)

6、驗(yàn)方面缺乏更深入的思考。4）現(xiàn)階段的學(xué)習(xí)狀態(tài)還比較滿意，老師講的也比較清楚，能跟得上老師的進(jìn)度。附源程序clear all;load G:學(xué)習(xí)北航研一慣性導(dǎo)航原理第二次作業(yè)jlfw%變量初始化%初始經(jīng)度為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經(jīng)度h=30;L(1)=39.975172*pi/180; J(1)=116.344695283*pi/180;%初始經(jīng)緯度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計(jì)算地心緯度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);%初始姿態(tài)角，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;%由姿態(tài)角計(jì)算四元數(shù)初始值Q0=angle2q(X(1),Y(1),Z(1);At2b=q2cos(Q0);%讀取數(shù)據(jù)文件%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,:);%我覺的可以開始導(dǎo)航計(jì)算了for i=1:60000%將b系比力矢量轉(zhuǎn)換為t系.用列向量Fb=Fbx(i);Fby(i);Fbz(i);Ab2t=(At2b');Ft=Ab2t*Fb;%左乘%指北方位系統(tǒng)導(dǎo)航計(jì)算了g=g0*(1+gk1*sin(L(i)2)*(1-2*h/Re)/sqrt(1-gk2*sin(L(i)2);%計(jì)算該采樣時(shí)刻加速度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;%計(jì)算下一時(shí)刻時(shí)刻速度可以修改！（認(rèn)為到采樣時(shí)刻之前一直保持V（i），a保持該大?。￢x(i+1)=Ax*0.01+Vx(i);Vy(i+1)=Ay*0.01+Vy(i);Vz(i+1)=Az*0.01+Vz(i);%計(jì)算從上一時(shí)刻到該采樣時(shí)刻的經(jīng)緯度（認(rèn)為速度在此期間保持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); %計(jì)算該采樣時(shí)刻的曲率半徑（因?yàn)镽x（1）是初始值，所以第一個(gè)采樣時(shí)刻對(duì)應(yīng)的是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);%計(jì)算到采樣時(shí)刻時(shí)的角速度在t系的表達(dá) %當(dāng)前采樣時(shí)刻速度等于歷史值，經(jīng)緯度和半徑更新了,所以采樣時(shí)刻的角速度變了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);%計(jì)算Wit在本體系的表達(dá)%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'%存儲(chǔ)歷史值%更新姿態(tài)矩陣P86 (4.2-36)At2b=q2cos(Q1);%更新歐拉角。姿態(tài)角angle=cos2angle(At2b);X(i+1)=angle(1);Y(i+1)=angle(2);Z(i+1)=angle(3);end%畫圖和讀取數(shù)據(jù)plot(J*180/pi,L*180/pi);xlabel('經(jīng)度');ylabel('緯度');title('系統(tǒng)位置'

15、;)tx=1:0.01:601;figure;plot(tx,Vx);xlabel('時(shí)間');ylabel('東向速度 單位 m/s');title('東向速度隨時(shí)間的變化');figure;plot(tx,Vy);xlabel('時(shí)間');ylabel('北向速度 單位 m/s');title('北向速度隨時(shí)間的變化');figure;plot(tx,Vz);xlabel('時(shí)間');ylabel('天向速度 單位 m/s');title('北向速度隨時(shí)間

16、的變化');figure;plot(tx,X*180/pi);xlabel('時(shí)間');ylabel('俯仰角 單位 °');title('俯仰角隨時(shí)間的變化');figure;plot(tx,Y*180/pi);xlabel('時(shí)間');ylabel('橫滾角 單位 °');title('橫滾隨時(shí)間的變化');figure;plot(tx,Z*180/pi);xlabel('時(shí)間');ylabel('航向角 單位 °');tit

17、le('航向角隨時(shí)間的變化');q2cos.m文件function Acos = q2cos( q )%由四元數(shù)推到方向余弦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 )%由姿態(tài)角求四元數(shù)% 輸入姿態(tài)角輸出四元數(shù) 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 )%由姿態(tài)角求四元數(shù)% 輸入姿態(tài)角輸出四元數(shù) 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 )%姿態(tài)矩陣到歐拉角的轉(zhuǎn)換 % 主要是處理多值的問題%俯仰角 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°上面的而且航向角以逆時(shí)針為正 課本上的P79頁 姿態(tài)矩陣不能用 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;