慣性導(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)角為［2190］（俯仰，橫滾，航向，單位為度），jlfw.mat中保存的為比力信息f_INSc（單位m/sA2）、陀螺儀角速率信息wib」NSc（單位rad/s），排列順序為一?三行分別為X、Y、Z向信息.4:航向角以逆時針為正。5:地球橢球長半徑re=6378245;地球自轉(zhuǎn)角速度wie=7.292115147e-5;重力加速度g=g0*（1+gk1*c33A2）*（1-2*h/re）/sqrt（1-gk2*c33A2）；g0=9.7803267714;gk1=0.00193185138639;gk2=0.00669437999013;c33=sin（lat緯度）;二、作業(yè)要求：1：可使用MATLAB語言編程，用MATLAB編程時可使用如下形式的語句讀取數(shù)據(jù)：loadD:\…文件路徑…\jlfw,便可得到比力信息和陀螺儀角速率信息。用角增量法。2：（1）以系統(tǒng)經(jīng)度為橫軸，緯度為縱軸（單位均要轉(zhuǎn)換為：度）做出系統(tǒng)位置曲線圖；（2）做出系統(tǒng)東向速度和北向速度隨時間變化曲線圖（速度單位：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ù)：V9Y.V.0.Yq=cos-^bcos—cos——sin-^bsin—sin—TOC\o"1-5"\h\z0 2 2 2 2 2 2V 9Y V 9Yq=cos—absin—cos——sin—abcos—sin—1 2 2 2 2 2 2V 9Y V 9Yq=cos-^bcos—sin—+sin~^bsin—cos—2 2 2 2 2 2 2V 9Y V 9Y22q=cos—absin—sin—+sin-^bcos—cos—223 2 2 2 2再根據(jù)四元數(shù)求方向余弦矩陣的初始矩陣：q2+q2q2+q2-q2-q20(12)32(qq-qq)(1 2 03)2(qq+qq)

1 3 022(qq+qq)12q2-q2+q-qq23 01)32(qq-qq)13 02、(qq+qq)2301q2-q2-q2+q201232、指北方位系統(tǒng)的運動解算：“平臺”指令角速度為：Vt——Ryt()Vt31=

it3ie3cos31=

it3ieie Rxtsin(L)+Xt-tan(L)Rxt加速度計獲得的比力信息戶為載體坐標(biāo)系中各個軸向的比力，而我們需ib要的比力ft為地理坐標(biāo)系中各個軸向的比力，它們之間應(yīng)用矩陣。做變換：it bft=CtXfb

itbibCt=(Cb)T=(Cb)-1

bt t根據(jù)比力信息可以求出各個方向上的加速度:Vt=ft+Vt=ft+(23sin(L)+xibx ieVt=ft—(23sin(L)+yiby ieVt=ft+(23cos(L)+zibz ieVtx-tan(L))Vt—(23cosR y iextVt Vt—tan(L))Vt+-^VtR xRzxt ytVt Vt—x—)Vt+—y—Vt—gRxRyxt yt(L)+VtA)VtRzxt因此可以求得速度為:Vt=JVdt+Vtx 0因此可以求得速度為:Vt=JVdt+Vtx 0x x0Vt=ftVtdt+Vt

y 0y y0載體所在位置的地理緯度L、經(jīng)度入可由下列方程求得:V,TL=J-2_dt+L0R 0yt入=J匕^sec(L)dt+入0R 0xt3、四元數(shù)姿態(tài)矩陣的更新:3b=3b—Cb31tbibtit式中，3b為陀螺所測得的角速度。ib用畢卡逼近法更新Cb的值，T為采樣時間

tA0=Tx3bib-0—A0—A0A0xyz[ao]=A0x0A0z—A0A0—A00A0yzxA0A0—A00匚 zyxA02=A02+A02+A020 xyzq(n+q(n+1)=(A0)X——I+384)(A0)2)——48)[a0〕;q(n)4、姿態(tài)角的求解:4、姿態(tài)角的求解:姿態(tài)角與姿態(tài)矩陣的關(guān)系:cosycosW—sincosycosW—sinysin0sinwab ab一cos0sinwabsinycosw+cosysin0sinwab abcosysinw+sinysin0coswab abcos0coswab一sinysinw—cosysin0coswab ab一sinycos0

sin0

cosycos0T33①=tan-iIT22式中0,y,w分別為俯仰角，滾轉(zhuǎn)角和偏航角，以逆時針為正方向，而課本ab上是以順時針為正，因此需要對課本上的公式進(jìn)行修改，將呷劭代入原公式可得現(xiàn)公式。如果記rtTT]111213Cb=TTTt212223TTTL313233則由以上兩式即可解算出姿態(tài)角：0=sin-i(T)23(

y=tan-1(四、程序流程圖五、結(jié)果經(jīng)緯度位髭曲線經(jīng)度產(chǎn)12南北方向速度時間總懣轉(zhuǎn)角時間總六、小結(jié)這次作業(yè)是捷聯(lián)慣導(dǎo)的解算，是利用上次作業(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:\Users\Administrator\Documents\MATLAB/jlfw.dat');Wib_INSc=a(:,2:4)';f_INSc=a(:,5:7)';%第一列：數(shù)據(jù)包序號第二至四列：分別為東、北、天向陀螺儀角速率信息wib_INSc(單位：rad/s)%第五至七列：分別為東、北、天向比力信息f_INSc(單位：m/s^2).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(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,:)=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/180*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)-sin(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);%q3fori=1:60000g=g0*(1+gk1*sin(L(i)A2)*(1-2*H/Re)/sqrt(1-gk2*sin(L(i)A2)));%計算重力加速度Rx(i)=Re/(1-e*(sin(L(i)))人2);%根據(jù)緯度計算卯酉圈曲率半徑Ry(i)=Re/(1+2*e-3*e*(sin(L(i)))A2);%根據(jù)緯度計算子午圈曲率半徑%求解四元數(shù)姿態(tài)矩陣q0=q(1,i);q1=q(2,i);q2=q(3,i);q3=q(4,i);Ctb=[q0A2+q1A2-q2A2-q3A2,2*(q1*q2+q0*q3),2*(q1*q3-q0*q2);2*(q1*q2-q0*q3),q2A2-q3A2+q0A2-q1A2,2*(q2*q3+q0*q1);2*(q1*q3+q0*q2),2*(q2*q3-q0*q1),q3A2-q2A2-q1A2+q0A2;];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*sin(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*Witt;%將指令角速度轉(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;%求取迭代矩陣中的各AthetaA=[0-x-y-z;x0z-y;y-z0x;zy-x0];%求取迭代矩陣[Atheta]T=xA2+yA2+zA2;%計算[Atheta]A2的q(:,i+1)=((1-T/8+TA2/384)*I+(1/2-T/48)*A)*q(:,i);%求q0theta(i+1)=asin(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;elsepsi(i+1)=atan(-Ctb(2