北航慣性導(dǎo)航作業(yè)二

1、慣性導(dǎo)航作業(yè)一、數(shù)據(jù)說明：1：慣導(dǎo)系統(tǒng)為指北方位的捷連系統(tǒng)。初始經(jīng)度為116.344695283度、緯度為39.975172度,高度h為30米。初速度v0=-9.993908270;0.000000000;0.348994967。2：jlfw中為600秒的數(shù)據(jù)，陀螺儀和加速度計采樣周期分別為為1/100秒和1/100秒。3：初始姿態(tài)角為2 1 90（俯仰，橫滾，航向,單位為度）,jlfw.mat中保存的為比力信息f_INSc(單位m/s2)、陀螺儀角速率信息wib_INSc(單位rad/s)，排列順序?yàn)?一三行分別為X、Y、Z向信息.4: 航向角以逆時針為正。5:地球橢球長半徑re=63782

2、45;地球自轉(zhuǎn)角速度wie=7.292115147e-5;重力加速度g=g0*(1+gk1*c332)*(1-2*h/re)/sqrt(1-gk2*c332)；g0=9.7803267714;gk1=0.00193185138639;gk2=0.00669437999013;c33=sin(lat緯度);二、作業(yè)要求：1：可使用 MATLAB語言編程，用MATLAB編程時可使用如下形式的語句讀取數(shù)據(jù)：load D:.文件路徑.jlfw,便可得到比力信息和陀螺儀角速率信息。用角增量法。2：(1) 以系統(tǒng)經(jīng)度為橫軸，緯度為縱軸（單位均要轉(zhuǎn)換為：度）做出系統(tǒng)位置曲線圖； (2) 做出系統(tǒng)東向速度和北

3、向速度隨時間變化曲線圖（速度單位：m/s，時間單位：s）； (3) 分別做出系統(tǒng)姿態(tài)角隨時間變化曲線圖（俯仰，橫滾，航向,單位轉(zhuǎn)換為：度，時間單位：s）；以上結(jié)果均要附在作業(yè)報告中。3：在作業(yè)報告中要寫出“程序流程圖、現(xiàn)階段學(xué)習(xí)小結(jié)”，寫明聯(lián)系方式。 (注意程序流程圖不是課本上的慣導(dǎo)解算流程，而是你程序分為哪幾個模塊、是怎樣一步步執(zhí)行的,什么位置循環(huán)等,讓別人根據(jù)該流程圖能夠編出相應(yīng)程序) (學(xué)習(xí)小結(jié)按條寫，不用寫套話)4：作業(yè)以紙質(zhì)報告形式提交，附源程序。三、基本原理和公式1、初始姿態(tài)矩陣的確定：根據(jù)初始姿態(tài)角求四元數(shù)：再根據(jù)四元數(shù)求方向余弦矩陣的初始矩陣：2、指北方位系統(tǒng)的運(yùn)動解算：“平臺

4、”指令角速度為：加速度計獲得的比力信息為載體坐標(biāo)系中各個軸向的比力，而我們需要的比力為地理坐標(biāo)系中各個軸向的比力，它們之間應(yīng)用矩陣做變換：根據(jù)比力信息可以求出各個方向上的加速度：因此可以求得速度為：載體所在位置的地理緯度L、經(jīng)度可由下列方程求得：3、四元數(shù)姿態(tài)矩陣的更新：式中，為陀螺所測得的角速度。用畢卡逼近法更新的值，T為采樣時間 4、姿態(tài)角的求解：姿態(tài)角與姿態(tài)矩陣的關(guān)系：式中,分別為俯仰角，滾轉(zhuǎn)角和偏航角，以逆時針為正方向，而課本上是以順時針為正，因此需要對課本上的公式進(jìn)行修改，將代入原公式可得現(xiàn)公式。如果記則由以上兩式即可解算出姿態(tài)角： 四、程序流程圖五、結(jié)果六、小結(jié)這次作業(yè)是捷聯(lián)慣導(dǎo)的

5、解算，是利用上次作業(yè)的結(jié)果對數(shù)據(jù)進(jìn)行處理。和上次不同，這次遇到了較多的問題。首先，對捷聯(lián)慣導(dǎo)的基本原理理解的不夠深刻，比如坐標(biāo)系的轉(zhuǎn)換，四元數(shù)微分方程的求解。其次，由于課本的姿態(tài)角是以順時針為正，而原始數(shù)據(jù)是以逆時針為正，因此需要對書上的公式進(jìn)行修改，在這個過程中就出現(xiàn)了許多問題，比如正負(fù)號問題?？傊@次作業(yè)彌補(bǔ)了學(xué)習(xí)上的不足，使我對基本原理理解更為深刻，也初步了解慣導(dǎo)的基本操作。七、程序clccleara=load('C:UsersAdministratorDocumentsMATLAB/jlfw.dat');Wib_INSc=a(:,2:4)'f_INSc=a(:

6、,5:7)'%第一列：數(shù)據(jù)包序號 第二至四列：分別為東、北、天向陀螺儀角速率信息wib_INSc（單位：rad/s）%第五至七列：分別為東、北、天向比力信息f_INSc（單位：m/s2）.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);%定義存放卯酉圈曲率半徑數(shù)據(jù)的矩陣Ry(1,:)=zeros(1,60001);%定義存放子午圈曲率半徑數(shù)據(jù)的矩陣psi(

7、1,:)=zeros(1,60001);%定義存放偏航角數(shù)據(jù)的矩陣theta(1,:)=zeros(1,60001);%定義存放俯仰角數(shù)據(jù)的矩陣gamma(1,:)=zeros(1,60001);%定義存放滾轉(zhuǎn)角數(shù)據(jù)的矩陣I=eye(4); %定義四階矩陣L(1,1)=39.975172/180*pi;%緯度初始值 單位：弧度Lambda(1,1)=116.344695283/180*pi;%經(jīng)度初始值 單位：弧度Vx(1,1)=-9.993908270;%初始速度x方向分量Vy(1,1)=0;%初始速度y方向分量Vz(1,1)=0.348994967;%初始速度z方向分量Wibx(1,:)=

8、a(:,2); %提取陀螺正東方向角速率并定義Wiby(1,:)=a(:,3); %提取陀螺正北方向角速率并定義 Wibz(1,:)=a(:,4); %提取陀螺天向角速率并定義 fibbx(1,:)=a(:,5);%x方向的比力數(shù)據(jù)fibby(1,:)=a(:,6);%y方向的比力數(shù)據(jù)fibbz(1,:)=a(:,7);%z方向的比力數(shù)據(jù)g0=9.7803267714;Wie=7.292115147e-5;%地球自轉(zhuǎn)角速度Re=6378245;%長半徑e=1/298.3;%橢圓度t=0.01;%采樣時間psi(1,1)=90/180*pi;%偏航角初始值 單位：弧度theta(1,1)=2/1

9、80*pi;%俯仰角初始值 單位：弧度gamma(1,1)=1/180*pi;%滾轉(zhuǎn)角初始值 單位：弧度gk1=0.00193185138639;gk2=0.00669437999013;H=30;%高度%求解四元數(shù)系數(shù)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)-si

10、n(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=g

11、0*(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);%根據(jù)緯度計算卯酉圈曲率半徑Ry(i)=Re/(1+2*e-3*e*(sin(L(i)2);%根據(jù)緯度計算子午圈曲率半徑%求解四元數(shù)姿態(tài)矩陣q0=q(1,i);q1=q(2,i);q2=q(3,i);q3=q(4,i);Ctb=q02+q12-q22-q32,2*(q1*q2+q0*q3),2*(q1*q3-q0*q2); 2*(q1*q2-q0*q3),q22-q32+q02-q12,2*(q2*q3+q0*q1); 2

12、*(q1*q3+q0*q2),2*(q2*q3-q0*q1),q32-q22-q12+q02;Cbt=Ctb'fibt=Cbt*fibbx(i);fibby(i);fibbz(i);%比力數(shù)據(jù)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*s

13、in(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*Wit

14、t;%將指令角速度轉(zhuǎn)換到平臺坐標(biāo)系,并求解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;%求取迭代矩陣thetaT=x2+y2+z2; % 計算theta2的q(:,i+1)=(1-T/8+T2/384)*I+(1/2-T/48)*A)*q(:,i);%求q0theta(i+1)=asi

15、n(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; else psi(i+1)=atan(-Ctb(2,1)/Ctb(2,2)-pi; end if(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; else gamma(i+1)

16、=atan(-Ctb(1,3)/Ctb(3,3)-pi; end endfigure(1)plot(L*180/pi,Lambda*180/pi);title('經(jīng)緯度位置曲線');xlabel('經(jīng)度/°');ylabel('緯度/°');grid ont=0:0.01:600;figure(2)plot(t,Vx);title('東西方向速度');xlabel('時間/s');ylabel('速度/m/s');grid onfigure(3)plot(t,Vy);title('南北方向速度');xlabel('時間/s');ylabel('速度/m/s');grid onfigure(4)plot(t,theta*180/pi);title('俯仰