1、i儀器曲光口學(xué)院school of ioiw scimi tommies EXGinc基于運(yùn)動(dòng)規(guī)劃的慣性導(dǎo)航系統(tǒng)動(dòng)態(tài)實(shí)驗(yàn)二零一三年六月十日2實(shí)驗(yàn) 4.1 慣性導(dǎo)航系統(tǒng)運(yùn)動(dòng)軌跡規(guī)劃與設(shè)計(jì)實(shí)驗(yàn)一、實(shí)驗(yàn)?zāi)康臑檫M(jìn)行動(dòng)態(tài)下簡化慣性導(dǎo)航算法的實(shí)驗(yàn)研究，進(jìn)行路徑和運(yùn)動(dòng)狀態(tài)規(guī)劃， 以驗(yàn)證不同運(yùn)動(dòng)狀態(tài)下慣導(dǎo)系統(tǒng)的性能。通過實(shí)驗(yàn)掌握步進(jìn)電機(jī)控制方法，并 產(chǎn)生不同運(yùn)動(dòng)路徑和運(yùn)動(dòng)狀態(tài)。二、實(shí)驗(yàn)內(nèi)容學(xué)習(xí)利用 6045B 控制板對(duì)步進(jìn)電機(jī)進(jìn)行控制的方法，并控制電機(jī)使運(yùn)動(dòng)滑 軌產(chǎn)生定長運(yùn)動(dòng)和不同加速度下的定長運(yùn)動(dòng)。三、實(shí)驗(yàn)系統(tǒng)組成USB_PCL6045控制板（評(píng)估板）、運(yùn)動(dòng)滑軌和控制計(jì)算機(jī)組成。四、實(shí)驗(yàn)原理IMU 安裝誤差

2、系數(shù)的計(jì)算方法USB_PCL6045控制板采用了 USB串行總線接口通信方式，不必拆卸計(jì)算 機(jī)箱就可以在臺(tái)式機(jī)或筆記本電腦上進(jìn)行運(yùn)動(dòng)控制芯片PCL6045B的學(xué)習(xí)和評(píng)估。USB_PCL6045評(píng)估板采用 USB串行總線方式實(shí)現(xiàn)評(píng)估板同計(jì)算機(jī)的數(shù)據(jù) 交換，由評(píng)估板的 FIFO 控制回路完成步進(jìn)電機(jī)以及伺服電機(jī)的高速脈沖控 制，任意 2 軸的圓弧插補(bǔ)， 2-4 軸的直線插補(bǔ)等運(yùn)動(dòng)控制功能。 USB_PCL6045B 評(píng)估板上配置了全部 PCL6045B芯片的外部信號(hào)接口和增量編碼器信號(hào)輸入接 口。由USB_PCL6045評(píng)估測(cè)試軟件可以進(jìn)行 PCL6045B芯片的主要功能的評(píng)估測(cè) 試。g圖4-1-

3、1USB_PCL6045B評(píng)估板原理框圖如圖4-1-1所示，CN11接口主要用于外部電源連接，可以選擇 DC5V單一 電源或DC5V/24V電源。CN12接口是USB信號(hào)接口，用于 USB_PCL6045評(píng)估 板同計(jì)算機(jī)的數(shù)據(jù)交換。USB_PCL6045評(píng)估板已經(jīng)完成對(duì)PCL6045B芯片的底層程序開發(fā)和硬件資 源與端口的驅(qū)動(dòng)，并封裝成156個(gè)API接口函數(shù)。用戶可直接在 VC環(huán)境下利 用API接口函數(shù)進(jìn)行編程。五、實(shí)驗(yàn)內(nèi)容1、操作步驟1）檢查電機(jī)驅(qū)動(dòng)電源（24V）2）檢查USB_PCL6045控制板與上位機(jī)及電機(jī)驅(qū)動(dòng)器間的連接電纜3）啟動(dòng)USB_PCL6045控制板評(píng)估測(cè)試系統(tǒng)檢查系統(tǒng)是否正

4、常工作。4）運(yùn)行編寫的定長運(yùn)動(dòng)程序，并比較實(shí)際位移與設(shè)定位移。5） 修改程序設(shè)定不同運(yùn)動(dòng)長度，并重復(fù)執(zhí)行步驟4）。6）對(duì)記錄實(shí)驗(yàn)數(shù)據(jù)，并進(jìn)行誤差分析。2、實(shí)驗(yàn)數(shù)據(jù)處理基于VC的控制界面：本次實(shí)驗(yàn)必須先設(shè)計(jì)控制系統(tǒng)的上位機(jī)，通過上位機(jī)的串口向下位機(jī)發(fā)送控制命令，下位機(jī)接收到命令后，產(chǎn)生PWM波，控制電機(jī)的正反轉(zhuǎn)以此達(dá)到控4#制導(dǎo)軌運(yùn)動(dòng)的目的。系統(tǒng)的控制界面如圖 1所示:InputDistance0Speed0OKOutputGeiSpeedCurrertSpeed0Control定長運(yùn)動(dòng)停止運(yùn)動(dòng)#圖1系統(tǒng)的控制界面控制導(dǎo)軌運(yùn)動(dòng)，運(yùn)動(dòng)采取正向運(yùn)動(dòng)，再返回，即IMU的實(shí)際運(yùn)行位移為零。并保存數(shù)據(jù)控制

5、界面的應(yīng)用程序源程序僅寫出VC中按鈕的響應(yīng)程序：void CAaaDlg:O nli ne()/ 定長運(yùn)動(dòng)/ TODO: Add your con trol no tificati on han dler code hereUSB_i nitial();USB_default_set();USB_set_orgogic(AXS_AX,0);原點(diǎn)開關(guān)的邏輯， 負(fù)邏輯USB_set_el_logic(AXS_AX,0);/硬極限輸入邏輯，低電平使能USB_set_sd_logic(AXS_AX,0);減速開關(guān)的輸入邏輯，負(fù)邏輯USB_set_almogic(AXS_AX, 1);/ 報(bào)警輸入信號(hào)邏

6、輯USB_set_i np_logic(AXS_AX,1);/i n的輸入信號(hào)邏輯USB_ez_logic(AXS_AX,0);/Z相的輸入邏輯USB_set_pls_outmode(AXS_AX,1);USB_set_out_enable(AXS_AX,1);/ 脈沖輸出使能/USB_jog_continue(AXS_AX,150,20000,20,20,20,20,1,30000);USB_start_tr_move(AXS_AX, m_dist, 0, m_inspeed, 5000, 5000);/USB_tv_move(AXS_AX, 150, 2000, 3000);/*USB_v

7、_change(AXS_AX, 5000, 5000);while(1)USB_get_speed(AXS_AX, &m_speed); UpdateData(FALSE);MSG msg; while(PeekMessage(&msg,0,0,100,PM_REMOVE) TranslateMessage(&msg); DispatchMessage(&msg);Sleep(100);*/void CAaaDlg:OnButton1() / 停止運(yùn)動(dòng)/ TODO: Add your control notification handler code here

8、USB_sd_stop(AXS_AX);void CAaaDlg:OnGetSpeed() / 獲得速度/ TODO: Add your control notification handler code here USB_get_speed(AXS_AX, &m_speed); UpdateData(FALSE); void CAaaDlg:OnButton3() /OK 按鈕程序 / TODO: Add your con trol no tificati on han dler code here UpdateData(true);3, 處理數(shù)據(jù)由實(shí)驗(yàn)原理可知，慣性測(cè)量單元（IMU

9、 ）可以通過自身獨(dú)立的測(cè)量結(jié)果進(jìn)行積分，計(jì)算出目標(biāo)運(yùn)動(dòng)的角度和位移等量。本次實(shí)驗(yàn)就是利用IMU的加速度計(jì)的某一敏感軸測(cè)量導(dǎo)軌運(yùn)行的加速度，通過加速度兩次積分得到物體的位 移，計(jì)算結(jié)果如圖2所示：實(shí)驗(yàn)經(jīng)過往返，從原理上講位移應(yīng)該為零。 處理結(jié)果：位移曲線：速度曲線:20406080100120時(shí)間解6 53 21404, 源程序：慣性器件綜合實(shí)驗(yàn)我的作業(yè)實(shí)驗(yàn)四X300000_V10000.txt');T=1/200; %單位為秒g=9.78;Ax=A(:,4)*g/1OOO; %提取加速度計(jì)的值轉(zhuǎn)化為mQ2Ax=Ax*(1.0009)-0.0036595*g;vx=zeros(12657

10、,1);sx=zeros(12657,1);u=zeros(12657,1);%計(jì)算位移for i=2:12657vx(i)=vx(i-1)+(Ax(i-1)+Ax(i-1)*T /2;sx(i)=sx(i-1)+(vx(i-1)+ vx(i)/2*T+0.5*A(i-1)*T*T;u(i)=i;endfigureplot(u/100,vx);xlabel('時(shí)間 /秒'),ylabel('速度 米/秒');figureplot(u/100,sx);xlabel('時(shí)間 /秒'),ylabel('位移 米');5, 實(shí)驗(yàn)結(jié)果分析從

11、原理上講IMU做往返運(yùn)動(dòng)，位移應(yīng)該出現(xiàn)增大和減小的趨勢(shì)，但是由于各 種誤差角，而且滑軌也不能保證當(dāng)?shù)厮剑谟?jì)算過程中，也未減去有害加速 度。所以誤差很大。而且根據(jù)所采集的數(shù)據(jù)可知加速度計(jì)并沒有感知方向，在 實(shí)驗(yàn)過程中應(yīng)該根據(jù)計(jì)算脈沖與時(shí)間，自己計(jì)算方向時(shí)間慣性導(dǎo)航系統(tǒng)半物理仿真實(shí)驗(yàn)、實(shí)驗(yàn)?zāi)康倪M(jìn)行慣導(dǎo)系統(tǒng)半物理仿真實(shí)驗(yàn)，以驗(yàn)證慣性器件真實(shí)誤差特性情況下慣性 導(dǎo)航系統(tǒng)的性能。二、實(shí)驗(yàn)內(nèi)容 將采集到陀螺儀與加速度計(jì)的真實(shí)誤差數(shù)據(jù)疊加到軌跡發(fā)生器產(chǎn)生的導(dǎo)航 參數(shù)真值上，進(jìn)行慣導(dǎo)解算，并分析誤差特性。三、實(shí)驗(yàn)系統(tǒng)組成真實(shí)的陀螺儀、加速度計(jì)或 IMU ，數(shù)據(jù)采集系統(tǒng)和數(shù)據(jù)處理計(jì)算機(jī)。四、實(shí)驗(yàn)步驟（1）采

12、集實(shí)驗(yàn)數(shù)據(jù)（2）處理采集的實(shí)驗(yàn)數(shù)據(jù)，生成半物理的慣性器件誤差數(shù)據(jù)（3）生成半物理的導(dǎo)航數(shù)據(jù)，進(jìn)行導(dǎo)航解算（4）對(duì)導(dǎo)航解算結(jié)果進(jìn)行分析（5）完成實(shí)驗(yàn)報(bào)告五、實(shí)驗(yàn)內(nèi)容及結(jié)果（1）半物理仿真數(shù)據(jù)的生成：a）應(yīng)用前面 IMU 實(shí)驗(yàn)或慣性導(dǎo)航系統(tǒng)動(dòng)態(tài)實(shí)驗(yàn)中采集的陀螺儀與加速度計(jì)的 靜態(tài)數(shù)據(jù) DATAb）對(duì)以上采集的靜態(tài)數(shù)據(jù)求取均方差 X （結(jié)果為 X 度 /小時(shí)或 Xg ）c）將 DATA 中數(shù)據(jù)去掉均值生成新的數(shù)據(jù) DATA1 （器件噪聲）d）自己設(shè)定要仿真的陀螺或加速度計(jì)的精度 丫（度/小時(shí)或g）e）將DATA1中數(shù)據(jù)乘以Y/X生成新的數(shù)據(jù)DATA2 （半物理仿真噪聲）f）從 DATA2 中讀取數(shù)據(jù)

13、并疊加到軌跡發(fā)生器產(chǎn)生的標(biāo)準(zhǔn)數(shù)據(jù)（不含噪聲 ）上，進(jìn)行導(dǎo)航解算。（如初始采集的數(shù)據(jù)長度不夠，可以將 DATA2 中數(shù)據(jù)重復(fù) 利用，即將生成一個(gè)幾倍長度于 DATA2和數(shù)據(jù)文件DATA3，并從DATA3 中讀取半物理數(shù)據(jù)并疊加到軌跡發(fā)生器產(chǎn)生的標(biāo)準(zhǔn)數(shù)據(jù)上）（ 2）加半物理仿真噪聲數(shù)據(jù)的導(dǎo)航結(jié)果：9軌跡笈注器的航向角誤差10(3)疊加噪聲的導(dǎo)航結(jié)果:11疊加哩聲后的東向速度誤差170 r60 -SO -403020，0 |QIj10 -j51015疊加嗥聲后的的北向速度誤差X Tfl疊加嚨聲后的的航向莆誤差13(4)結(jié)果分析：由實(shí)驗(yàn)結(jié)果可見，疊加的仿真噪聲數(shù)據(jù)對(duì)姿態(tài)的解算影響很大；但由于所加噪 聲

14、較小，所以噪聲數(shù)據(jù)對(duì)位移和速度的解算影響不大。六，源程序clear,clcin vout=load(慣導(dǎo)實(shí)驗(yàn)數(shù)據(jù)第四次實(shí)驗(yàn)第四部分半物理仿真數(shù)據(jù)生成方法及數(shù)據(jù)格式說明IMU數(shù)據(jù)invout.dat');CaijiShuju=load(慣導(dǎo)實(shí)驗(yàn)數(shù)據(jù)第四次實(shí)驗(yàn)第四部分半物理仿真數(shù)據(jù)生成方法及數(shù)據(jù)格式說明IMU數(shù)據(jù)data2.txt' );W=CaijiShuju(:,3:5);F=CaijiShuju(:,9:11);W_p ingjun=mea n(W);F_pingjun=mean(F); %生%成%噪%聲% wx=W(:,1)-W_pingjun(1,1);%器件噪聲wy=W

15、(:,2)-W_pingjun(1,2);wz=W(:,3)-W_pingjun(1,3); fx=F(:,1)-F_pingjun(1,1); fx=F(:,2)-F_pingjun(1,2); fx=F(:,3)-F_pingjun(1,3);%求%陀%螺%均%方%差 % N=size(W);n=N(1,1);%陀%螺%的精度設(shè)為 0.5 度/ 小時(shí) % w_jingdu=0.5/3600*pi/180%0.5 度 / 小時(shí)轉(zhuǎn)成弧度sx=0;for i=1:nsx=sx+(W(i,1)-W_pi ngju 門(1,1)人2endwx_ju nfan gcha=sqrt(sx/n);%x 陀

16、螺的均方差wx1=w_jingdu/wx_junfangcha;Wx=wx*wx1%半物理仿真噪聲 Wxsx=0;for i=1:nsx=sx+(W(i,2)-W_pi ngju 門(1,2)人2endwy_ju nfan gcha=sqrt(sx/n);%y 陀螺的均方差wy2=w_jingdu/wy_junfangcha;Wy=wy*wy2%半物理仿真噪聲 Wysx=0;for i=1:nsx=sx+(W(i,3)-W_pi ngjun (1,3)人2endwz_junfan gcha=sqrt(sx/ n);%z 陀螺的均方差wz3=w_jingdu/wz_junfangcha;Wz=w

17、z*wz3%半物理仿真噪聲 Wz%求%加%計(jì) 的 均 方差%f_jingdu=1/1000*9.8%加計(jì)的精度為 1mgsx=0;for i=1:nsx=sx+(F(i,1)-F_pi ngjun (1,1)人2endfx_junfan gcha=sqrt(sx/ n);%x加 計(jì)的均方差fx1= f_jingdu/fx_junfangcha;Fx=fx*fx1;sx=0;for i=1:nsx=sx+(F(i,2)-F_pi ngjun (1,2)人2endfy_junfan gcha=sqrt(sx/ n);%丫加計(jì)的均方差fx2= f_jingdu/fy_junfangcha;Fy=fx

18、*fx2;sx=0;for i=1:nsx=sx+(F(i,3)-F_pi ngjun (1,3)人2endfz_junfan gcha=sqrt(sx/ n);%乙力口計(jì)的均方差fx3= f_jingdu/fz_junfangcha;加噪Fz=fx*fx3;%軌%跡% 發(fā)生 器 數(shù) 據(jù) 疊聲%Wx_invout=invout(:,5);Wy_invout=invout(:,6);Wz_invout=invout(:,7);Fx_invout=invout(:,2);Fy_invout=invout(:,3);Fz_invout=invout(:,4);L_invout=invout(:,8)

19、;%緯度Jingdu_invout=invout(:,9);%經(jīng)度Height_invout=invout(:,10);%高度%開%始%疊%加 %N1=size(invout);n1=N1(1,1);%應(yīng)為采樣點(diǎn)數(shù)大于軌跡發(fā)生器的個(gè)數(shù)，所以以軌跡發(fā)生器的個(gè)數(shù)為準(zhǔn)Wxx=Wx(1:n1,1)+Wx_invout;Wyy=Wy(1:n1,1)+Wy_invout ;Wzz=Wz(1:n1,1)+Wz_invout;Wibb=Wxx,Wyy,Wzz;Fxx=Fx(1:n1,1)+Fx_invout;Fyy=Fy(1:n1,1)+Fy_invout ;Fzz=Fz(1:n1,1)+Fz_invout;

20、Fibb=Fxx,Fyy,Fzz;q0=zeros(n1,1);q1=zeros(n1,1);q2=zeros(n1,1);q3=zeros(n1,1);Phai=zeros(n1,1);Thita=zeros(n1,1);Gama=zeros(n1,1);Phai(1)=0/180*pi;%偏航初始角Thita(1)=(0)*pi/180;%俯仰初始角Gama(1)=(0)*pi/180;%橫滾初始角L=zeros(n1,1);nmda=zeros(n1,1);Vxt=zeros(n1+1,1);Vyt=zeros(n1+1,1);q0(1)=cos(-Phai(1)/2)*cos(Thit

21、a(1)/2)*cos(Gama(1)/2)+sin(- Phai(1)/2)*sin(Thita(1)/2)*sin(Gama(1)/2);q1(1)=cos(-Phai(1)/2)*sin(Thita(1)/2)*cos(Gama(1)/2)+sin(- Phai(1)/2)*cos(Thita(1)/2)*sin(Gama(1)/2);q2(1)=cos(-Phai(1)/2)*cos(Thita(1)/2)*sin(Gama(1)/2)-sin(- Phai(1)/2)*sin(Thita(1)/2)*cos(Gama(1)/2);q3(1)=cos(-Phai(1)/2)*sin(T

22、hita(1)/2)*sin(Gama(1)/2)-sin(-Phai(1)/2)*cos(Thita(1)/2)*cos(Gama(1)/2);Wie=0.000072921151467;%已經(jīng)是弧度制L(1)=40/180*pi;nmda(1)=116.0/180*pi;T=0.01; %采樣頻率為 100HzVxt(1)=0;Vyt(1)=0;Re=6378245+80;%加高度 80 米e=1/298.3;for k=1:n1cl仁 q0(k)A2+q1(k)A2-q2(k)A2-q3(k)A2;c12=2*(q1(k)*q2(k)+q0(k)*q3(k);c13=2*(q1(k)*q

23、3(k)-q0(k)*q2(k);c21=2*(q1(k)*q2(k)-q0(k)*q3(k);c22=q0(k)A2-q1(k)A2+q2(k)A2-q3(k)A2;c23=2*(q2(k)*q3(k)+q0(k)*q1(k);c31=2*(q1(k)*q3(k)+q0(k)*q2(k);c32=2*(q2(k)*q3(k)-q0(k)*q1(k);c33=q0(k)A2-q1(k)A2-q2(k)A2+q3(k)A2;Cnb=c11,c12,c13c21,c22,c23c31,c32,c33;if abs(c22)>0.0000000000001Phai(k)=atan(-c21/c

24、22);endif abs(c22)>0.0000000000001 & c21>0Phai(k)=pi/2;endif abs(c22)>0.0000000000001 & c21<0Phai(k)=-pi/2;endif abs(c22)>0.0000000000001 & c22>0Phai(k)=atan(-c21/c22);endif abs(c22)>0.0000000000001 & c22>0 & c21>0Phai(k)=atan(c21/c22)+pi;endif abs(c22)

25、>0.0000000000001 & c22>0 & c21<0Phai(k)=atan(-c21/c22)-pi;endThita(k)=asin(c23);Gama(k)=-atan(c13/c33);Cbn=inv(Cnb);Aibn=Cbn*Fibb(k,:)'Rxt=Re/(1-e*(sin(L(k)*sin(L(k);axt=Aibn(1,1)+2*Wie*sin(L(k)*Vyt(k)+Vyt(k)*Vxt(k)*tan(L(k)/Rxt; ayt=Aibn(2,1)-2*Wie*sin(L(k)*Vxt(k)-Vxt(k)*Vxt(k)

26、*tan(L(k)/Rxt;Vxt(k+1)=axt*T+Vxt(k);Vyt(k+1)=ayt*T+Vyt(k);Ryt=Re/(1+2*e-3*e*(sin(L(k)*sin(L(k);L(k+1)=0.5*T*(Vyt(k+1)+Vyt(k)/Ryt+L(k);%課本 86 頁 4.2-38nmda(k+1)=0.5*T*(Vxt(k+1)+Vxt(k)/Rxt*sec(L(k)+nmda(k);Wenn=-Vyt(k)/Ryt;Vxt(k)/Rxt;Vxt(k)/Rxt*tan(L(k);式Winn=Wenn+0;Wie*cos(L(k);Wie*sin(L(k);Winb=Cnb*W

27、inn;Wtbb=Wibb(k,:)'-Winb;dltaTitaO_fa ng=(Wtbb(1,1)*T)A2+(Wtbb(2,1)*T)A2+(Wtbb(3,1)*T)A2;dltaTita=0,-Wtbb(1,1)*T,-Wtbb(2,1)*T,-Wtbb(3,1)*T;Wtbb(1,1)*T,0,Wtbb(3,1)*T,-Wtbb(2,1)*T;Wtbb(2,1)*T,-Wtbb(3,1)*T,0,Wtbb(1,1)*T;Wtbb(3,1)*T,Wtbb(2,1)*T,-Wtbb(1,1)*T,0Q=(1- dltaTita0_fang/8)*eye(4)+0.5*dltaTi

28、ta)*q0(k);q1(k);q2(k);q3(k); q0(k+1)=Q(1);q1(k+1)=Q(2);q2(k+1)=Q(3);q3(k+1)=Q(4);end figure hold on i=1:n1;subplot(1,2,1),plot(i,Vxt(i)%速度誤差title(' 疊加噪聲后的東向速度誤差' )subplot(1,2,2),plot(i,Vyt(i)title(' 疊加噪聲后的的北向速度誤差 ' ) figure hold on i=1:n1;%位置誤差subplot(1,2,1),plot(i,L(i)*180/pi)title(

29、' 疊加噪聲后的的緯度誤差' )subplot(1,2,2),plot(i,nmda(i)*180/pi)title(' 疊加噪聲后的的經(jīng)度誤差' ) figure hold on i=1:n1;plot(i,Phai(i)*180/pi)%姿態(tài)角誤差 subplot(1,3,1),title( ' 疊加噪聲后的的航向角誤差 ' ) figure hold on i=1:n1;plot(i,Thita(i)*180/pi)%subplot(1,3,2),title( ' 疊加噪聲后的俯仰角誤差 ' )figure hold on

30、i=1:n1;plot(i,Gama(i)*180/pi) %subplot(1,3,3), title( ' 疊加噪聲后的橫滾角誤差 ' )% %軌%跡%發(fā)%生%器的數(shù)據(jù)處理 % clear,clc invout=load( 慣導(dǎo)實(shí)驗(yàn)數(shù)據(jù) 第四次 實(shí)驗(yàn) 第四部分半物理仿真數(shù)據(jù)生成方法 及數(shù)據(jù)格式說明 IMU 數(shù)據(jù) invout.dat' );Wx_invout=invout(:,5);Wy_invout=invout(:,6);Wz_invout=invout(:,7);Wibb=Wx_invout,Wy_invout,Wz_invout;Fx_invout=invo

31、ut(:,2);Fy_invout=invout(:,3);Fz_invout=invout(:,4);Fibb=Fx_invout,Fy_invout,Fz_invout;L_invout=invout(:,8);%緯度Jingdu_invout=invout(:,9);%經(jīng)度Height_invout=invout(:,10);%高度N1=size(invout);n1=N1(1,1);q0=zeros(n1,1);q1=zeros(n1,1);q2=zeros(n1,1);q3=zeros(n1,1);Phai=zeros(n1,1);Thita=zeros(n1,1);Gama=zer

32、os(n1,1);Phai(1)=0/180*pi;%偏航初始角Thita(1)=0*pi/180;%俯仰初始角Gama(1)=0*pi/180;%橫滾初始角L=zeros(n1,1);nmda=zeros(n1,1);Vxt=zeros(n1+1,1);Vyt=zeros(n1+1,1);q0(1)=cos(-Phai(1)/2)*cos(Thita(1)/2)*cos(Gama(1)/2)+sin(-20Phai(1)/2)*sin(Thita(1)/2)*sin(Gama(1)/2);q1(1)=cos(-Phai(1)/2)*sin(Thita(1)/2)*cos(Gama(1)/2)

33、+sin(- Phai(1)/2)*cos(Thita(1)/2)*sin(Gama(1)/2);q2(1)=cos(-Phai(1)/2)*cos(Thita(1)/2)*sin(Gama(1)/2)-sin(- Phai(1)/2)*sin(Thita(1)/2)*cos(Gama(1)/2);q3(1)=cos(-Phai(1)/2)*sin(Thita(1)/2)*sin(Gama(1)/2)-sin(-Phai(1)/2)*cos(Thita(1)/2)*cos(Gama(1)/2);Wie=0.000072921151467;%已經(jīng)是弧度制L(1)=40/180*pi;nmda(1

34、)=116.0/180*pi;T=0.01; %采樣頻率為 100HzVxt(1)=0;Vyt(1)=0;Re=6378245+80;%加高度 80 米e=1/298.3;for k=1:n1cl仁 q0(k)A2+q1(k)A2-q2(k)A2-q3(k)A2;c12=2*(q1(k)*q2(k)+q0(k)*q3(k);c13=2*(q1(k)*q3(k)-q0(k)*q2(k);c21=2*(q1(k)*q2(k)-q0(k)*q3(k);c22=q0(k)A2-q1(k)A2+q2(k)A2-q3(k)A2;c23=2*(q2(k)*q3(k)+q0(k)*q1(k);c31=2*(q

35、1(k)*q3(k)+q0(k)*q2(k);c32=2*(q2(k)*q3(k)-q0(k)*q1(k);c33=q0(k)A2-q1(k)A2-q2(k)A2+q3(k)A2;Cnb=c11,c12,c13c21,c22,c23c31,c32,c33;if abs(c22)>0.0000000000001Phai(k)=atan(-c21/c22);endif abs(c22)>0.0000000000001 & c21>0Phai(k)=pi/2;endif abs(c22)>0.0000000000001 & c21<0Phai(k)=-p

36、i/2;endif abs(c22)>0.0000000000001 & c22>0 Phai(k)=atan(-c21/c22);endif abs(c22)>0.0000000000001 & c22>0 & c21>0Phai(k)=atan(c21/c22)+pi;endPhai(k)=atan(-c21/c22)-pi;endThita(k)=asin(c23);Gama(k)=-atan(c13/c33);Cbn=inv(Cnb);Aibn=Cbn*Fibb(k,:)'Rxt=Re/(1-e*(sin(L(k)*sin(L(k);axt=Aibn(1,1)+2*Wie*sin(L(k)*Vyt(k)+Vyt(k)*Vxt(k)*tan(L(k)/Rxt; ayt=Aibn(2,1)-2*Wie*sin(L(k)*Vxt(k)-Vxt(k)*Vxt(k)*tan(L(k)/Rxt;Vxt(k+1)=axt*T+Vxt(k);Vyt(k+1)=ayt*T+Vyt(k);Ryt