捷聯(lián)慣導(dǎo)解算作業(yè)(共11頁)

1、慣性導(dǎo)航原理第二次作業(yè)報告姓名：蘇憲飛 學(xué)號：S14124007 電話：183107467511、題目分析本題已知的信息是陀螺儀和加速度計在各個時刻的采樣值，以及初始經(jīng)緯度、高度、姿態(tài)角和速度值。需要求解指北方位捷聯(lián)系統(tǒng)的運動軌跡和姿態(tài)角變化。根據(jù)這些已知信息以及捷聯(lián)式慣導(dǎo)系統(tǒng)的基本力學(xué)編排方程可知基本求解過程如下：第一步，先根據(jù)初試的姿態(tài)角確定初始的四元數(shù)值，進而可以列寫出本體系相對于導(dǎo)航系的方向余弦矩陣，然后將加速度計的測量信息經(jīng)過余弦陣由導(dǎo)航系變換到本體系中。第二步，根據(jù)變換后的比力信息和初始的經(jīng)緯度、高度和速度值進行指北方位系統(tǒng)的運動解算，可以求出速度信息、指令角速度信息和位置信息。第

2、三步，根據(jù)第二步中求解出來的指令角速度信息，對其經(jīng)過余弦矩陣變換到本體系中，陀螺儀測量到的信息和變換后的指令角速度相減得到本體系相對于導(dǎo)航系的角速度在本體系中的分解。第四步，根據(jù)四元數(shù)的運動學(xué)微分方程求解出下一時刻的四元數(shù)，根據(jù)四元數(shù)和歐拉角之間的關(guān)系求解出新的姿態(tài)角。接著進行以上兩步的運算。經(jīng)過以上幾步就可以對指北方位捷聯(lián)慣導(dǎo)系統(tǒng)進行解算。2、解算原理和公式(1)、初始姿態(tài)矩陣的確定：根據(jù)初始姿態(tài)角求四元數(shù)： 根據(jù)四元數(shù)求方向余弦矩陣： (2)、指北方位系統(tǒng)的運動解算：地理坐標(biāo)系相對地球坐標(biāo)系的角速度為： 加速度計獲得的比力信息為載體坐標(biāo)系中各個軸向的比力，而我們需要的比力為地理坐標(biāo)系中各個

3、軸向的比力，它們之間應(yīng)用矩陣做變換： 根據(jù)比力信息可以求出各個方向上的加速度：因此可以求得速度為：載體所在位置的地理緯度L、經(jīng)度可由下列方程求得：(3)、四元數(shù)姿態(tài)矩陣的更新：式中，為陀螺所測角速度。用畢卡逼近法更新的值， (4)、姿態(tài)角的求解：姿態(tài)角與姿態(tài)矩陣的關(guān)系：式中,分別為俯仰角，橫滾角和偏航角。如果記則由以上兩式即可解算出姿態(tài)角： 3、作業(yè)結(jié)果圖1：經(jīng)緯度位置曲線圖2：東西方向速度曲線圖3：南北方向速度曲線圖4：俯仰角隨時間變化曲線圖5：橫滾角隨時間變化曲線圖6：偏航角隨時間變化曲線4、程序流程圖圖7：程序流程圖5、學(xué)習(xí)小結(jié)學(xué)習(xí)慣導(dǎo)大約兩個月了，總體感覺研究生階段學(xué)習(xí)比本科階段有所提

4、高，本科接觸的慣導(dǎo)只是皮毛，而研究生階段則是充分剖析各個細節(jié)使我對慣導(dǎo)有了總體的把握。在學(xué)習(xí)中也看到了很多不足之處：第一：對各個公式推導(dǎo)還是不能全面掌握。第二：對所學(xué)知識還沒有融會貫通。第三：專業(yè)基礎(chǔ)太淺導(dǎo)致上課難以理解，還好老師講的比較詳細。針對這些問題，采取了一些自學(xué)手段首先對書上的公式按照老師的方法進行推導(dǎo)，努力回顧上課時老師教給我們的方法，而不是按照書本生搬硬套。6、源程序clear;clc;format long;%輸入常值%Re=6378245;e=1/298.3;w_ie=7.292115147e-5; %單位為弧度dt=0.01; %步長為0.01g0=9.7803267714

5、;gk1=0.00193185138639;gk2=0.00669437999013;%輸入初值%lumda0=116.344695283; %初始位置L0=39.975172; %單位為度h0=30;Vx0=-9.993908270; %初始速度Vy0=0;Vz0=0.348994967;xita0=2*pi/180; %初始姿態(tài)角gama0=1*pi/180; %單位為度fai0=-90*pi/180; %初始四元數(shù)中以順時針為正，所以此處應(yīng)該把航向角取負%數(shù)組初始化%L=zeros(1,60002);lumda=zeros(1,60002);Vx=zeros(1,60002);Vy=ze

6、ros(1,60002);Vz=zeros(1,60002);xita=zeros(1,60002);gama=zeros(1,60002);fai=zeros(1,60002);L(1,1)=L0;lumda(1,1)=lumda0;Vx(1,1)=Vx0;Vy(1,1)=Vy0;xita(1,1)=xita0*180/pi;gama(1,1)=gama0*180/pi;fai(1,1)=-fai0*180/pi; %初始四元數(shù)中以順時針為正，所以此處應(yīng)該把航向角取負%初始四元數(shù)%公式中以順時針為正%Lamu0=cos(fai0/2)*cos(xita0/2)*cos(gama0/2)+si

7、n(fai0/2)*sin(xita0/2)*sin(gama0/2);Lamu1=cos(fai0/2)*sin(xita0/2)*cos(gama0/2)+sin(fai0/2)*cos(xita0/2)*sin(gama0/2);Lamu2=cos(fai0/2)*cos(xita0/2)*sin(gama0/2)-sin(fai0/2)*sin(xita0/2)*cos(gama0/2);Lamu3=cos(fai0/2)*sin(xita0/2)*sin(gama0/2)-sin(fai0/2)*cos(xita0/2)*cos(gama0/2);Lamu=Lamu0;Lamu1;L

8、amu2;Lamu3;%導(dǎo)入測量值%load jlfw.mat%開始循環(huán)%lumda_t=lumda0*pi/180;L_t=L0*pi/180; %單位為弧度h_t=h0;Vx_t=Vx0;Vy_t=Vy0;Vz_t=Vz0;for i=1:1:60001; Lamu0=Lamu(1,1); Lamu1=Lamu(2,1); Lamu2=Lamu(3,1); Lamu3=Lamu(4,1); Ct_b=Lamu02+Lamu12-Lamu22-Lamu32,2*(Lamu1*Lamu2+Lamu0*Lamu3),2*(Lamu1*Lamu3-Lamu0*Lamu2); 2*(Lamu1*La

9、mu2-Lamu0*Lamu3),Lamu02-Lamu12+Lamu22-Lamu32,2*(Lamu2*Lamu3+Lamu0*Lamu1); 2*(Lamu1*Lamu3+Lamu0*Lamu2),2*(Lamu2*Lamu3-Lamu0*Lamu1),Lamu02-Lamu12-Lamu22+Lamu32; %捷聯(lián)姿態(tài)陣的逆 f_b=f_INSc(1,i);f_INSc(2,i);f_INSc(3,i); f_t=Ct_bf_b; Rxt_re=1/Re*(1-e*(sin(L_t)2); Ryt_re=1/Re*(1+2*e-3*e*(sin(L_t)2); Vy_t=Vy_t+dt

10、*(f_t(2,1)-Vx_t*(2*w_ie*sin(L_t)+Vx_t*Rxt_re*tan(L_t)+Vy_t*Ryt_re*Vz_t); Vx_t=Vx_t+dt*(f_t(1,1)+Vy_t*(2*w_ie*sin(L_t)+Vx_t*Rxt_re*tan(L_t)-Vz_t*(2*w_ie*cos(L_t)+Vx_t*Rxt_re); c33=sin(L_t); g=g0*(1+gk1*c332)*(1-2*h_t/Re)/sqrt(1-gk2*c332); Vz_t=Vz_t+dt*(f_t(3,1)+Vx_t*(-2*w_ie*cos(L_t)+Vx_t*Rxt_re)+Vy_

11、t*Vx_t*Ryt_re-g); witx_t=-Vy_t*Ryt_re; L_t=L_t+Vy_t*Ryt_re*dt; wity_t=Vx_t*Rxt_re+w_ie*cos(L_t); witz_t=Vx_t*Rxt_re*tan(L_t)+w_ie*sin(L_t); lumda_t=lumda_t+Vx_t*Rxt_re*sec(L_t)*dt; h_t=h_t+dt*Vz_t; wit_t=witx_t,wity_t,witz_t' wit_b=Ct_b*wit_t; wib_b=wib_INSc(1,i);wib_INSc(2,i);wib_INSc(3,i); wtb

12、_b=wib_b-wit_b; d_Lamu=1/2*0,-wtb_b(1,1),-wtb_b(2,1),-wtb_b(3,1); wtb_b(1,1),0,wtb_b(3,1),-wtb_b(2,1); wtb_b(2,1),-wtb_b(3,1),0,wtb_b(1,1); wtb_b(3,1),wtb_b(2,1),-wtb_b(1,1),0*Lamu; Lamu=Lamu+dt*d_Lamu; xita_t=asin(Ct_b(2,3); gama_t=atan(-Ct_b(1,3)/Ct_b(3,3); fai_t=atan(Ct_b(2,1)/Ct_b(2,2); L(1,i+1)

13、=L_t*180/pi; lumda(1,i+1)=lumda_t*180/pi; xita(1,i+1)=xita_t*180/pi; gama(1,i+1)=gama_t*180/pi; fai(1,i+1)=-fai_t*180/pi; Vx(1,i+1)=Vx_t; Vy(1,i+1)=Vy_t;endt=linspace(0,601,60002);plot(lumda,L);title('經(jīng)緯度位置曲線');xlabel('經(jīng)度/°');ylabel('緯度/°');figureplot(t,Vx);title('東西方向速度');xlabel('時間/s');ylabel('速度/m/s');figureplot(t,Vy);title('南北方向速度');xlabel('時間/s');ylabel('速度/m/s');figureplot(t,xita);title('俯仰角');xlabel('時間/s