北航慣性導航綜合實驗四實驗報告.

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

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

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

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

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

6、et_i np_logic(AXS_AX,1);/i n的輸入信號邏輯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_chang

7、e(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() / 停止運動/ TODO: Add your control notification handler code here USB_sd_stop(AXS_AX);vo

8、id 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ù)由實驗原理可知，慣性測量單元（IMU ）可以通過自身獨立的測量結(jié)果進行積分，計算出目標運

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

10、2657,1);%計算位移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(時間 /秒),ylabel(速度 米/秒);figureplot(u/100,sx);xlabel(時間 /秒),ylabel(位移 米);5, 實驗結(jié)果分析從原理上講IMU做往返運動，位移應(yīng)該出現(xiàn)增大和減小的趨勢，但是由于各 種誤差角，而且滑軌也不能保證當?shù)厮?，在計算過程中，也未減去有害加速 度。

11、所以誤差很大。而且根據(jù)所采集的數(shù)據(jù)可知加速度計并沒有感知方向，在 實驗過程中應(yīng)該根據(jù)計算脈沖與時間，自己計算方向時間慣性導航系統(tǒng)半物理仿真實驗、實驗?zāi)康倪M行慣導系統(tǒng)半物理仿真實驗，以驗證慣性器件真實誤差特性情況下慣性 導航系統(tǒng)的性能。二、實驗內(nèi)容 將采集到陀螺儀與加速度計的真實誤差數(shù)據(jù)疊加到軌跡發(fā)生器產(chǎn)生的導航 參數(shù)真值上，進行慣導解算，并分析誤差特性。三、實驗系統(tǒng)組成真實的陀螺儀、加速度計或 IMU ，數(shù)據(jù)采集系統(tǒng)和數(shù)據(jù)處理計算機。四、實驗步驟（1）采集實驗數(shù)據(jù)（2）處理采集的實驗數(shù)據(jù)，生成半物理的慣性器件誤差數(shù)據(jù)（3）生成半物理的導航數(shù)據(jù)，進行導航解算（4）對導航解算結(jié)果進行分析（5）完成

12、實驗報告五、實驗內(nèi)容及結(jié)果（1）半物理仿真數(shù)據(jù)的生成：a） 應(yīng)用前面 IMU 實驗或慣性導航系統(tǒng)動態(tài)實驗中采集的陀螺儀與加速度計的 靜態(tài)數(shù)據(jù) DATAb） 對以上采集的靜態(tài)數(shù)據(jù)求取均方差 X （結(jié)果為 X 度 /小時或 Xg ）c） 將 DATA 中數(shù)據(jù)去掉均值生成新的數(shù)據(jù) DATA1 （器件噪聲）d）自己設(shè)定要仿真的陀螺或加速度計的精度 丫（度/小時或g）e）將DATA1中數(shù)據(jù)乘以Y/X生成新的數(shù)據(jù)DATA2 （半物理仿真噪聲）f）從 DATA2 中讀取數(shù)據(jù)并疊加到軌跡發(fā)生器產(chǎn)生的標準數(shù)據(jù)（不含噪聲 ）上，進行導航解算。（如初始采集的數(shù)據(jù)長度不夠，可以將 DATA2 中數(shù)據(jù)重復(fù) 利用，即將生

13、成一個幾倍長度于 DATA2和數(shù)據(jù)文件DATA3，并從DATA3 中讀取半物理數(shù)據(jù)并疊加到軌跡發(fā)生器產(chǎn)生的標準數(shù)據(jù)上）（ 2）加半物理仿真噪聲數(shù)據(jù)的導航結(jié)果：軌跡笈注器的航向角誤差(3)疊加噪聲的導航結(jié)果:疊加哩聲后的東向速度誤差170 r60 -SO -403020，0 |QIj10 -j51015疊加嗥聲后的的北向速度誤差X Tfl疊加嚨聲后的的航向莆誤差(4)結(jié)果分析：由實驗結(jié)果可見，疊加的仿真噪聲數(shù)據(jù)對姿態(tài)的解算影響很大；但由于所加噪 聲較小，所以噪聲數(shù)據(jù)對位移和速度的解算影響不大。六，源程序clear,clcin vout=load(慣導實驗數(shù)據(jù)第四次實驗第四部分半物理仿真數(shù)據(jù)生成方

14、法及數(shù)據(jù)格式說明IMU數(shù)據(jù)invout.dat);CaijiShuju=load(慣導實驗數(shù)據(jù)第四次實驗第四部分半物理仿真數(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(:,2)-W_pingjun(1,2);wz=W(:,3)-W_pingjun(1,3); fx=F(:,1)-F_pingjun(1,1); fx=F(:,2)-F_

15、pingjun(1,2); fx=F(:,3)-F_pingjun(1,3);%求%陀%螺%均%方%差 % N=size(W);n=N(1,1);%陀%螺%的精度設(shè)為 0.5 度/ 小時 % w_jingdu=0.5/3600*pi/180%0.5 度 / 小時轉(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 陀螺的均方差wx1=w_jingdu/wx_junfangcha;Wx=wx*wx1%半物理仿真噪聲 Wxsx=0;for i=1:nsx=sx+(W(i,2)-W_pi

16、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=wz*wz3%半物理仿真噪聲 Wz%求%加%計 的 均 方差%f_jingdu=1/1000*9.8%加計的精度為 1mgsx=0;for i=1:nsx=sx+(F(i,

17、1)-F_pi ngjun (1,1)人2endfx_junfan gcha=sqrt(sx/ n);%x加 計的均方差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);%丫加計的均方差fx2= f_jingdu/fy_junfangcha;Fy=fx*fx2;sx=0;for i=1:nsx=sx+(F(i,3)-F_pi ngjun (1,3)人2endfz_junfan gcha=sqrt(sx/ n);%乙力口

18、計的均方差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);%緯度Jingdu_invout=invout(:,9);%經(jīng)度Height_invout=invout(:,10);%高度%開%始%疊%加 %N1=size(invo

19、ut);n1=N1(1,1);%應(yīng)為采樣點數(shù)大于軌跡發(fā)生器的個數(shù)，所以以軌跡發(fā)生器的個數(shù)為準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;Fibb=Fxx,Fyy,Fzz;q0=zeros(n1,1);q1=zeros(n1,1);q2=zeros(n1,1);q3=zeros(n1,1);Phai=zer

20、os(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(Thita(1)/2)*cos(Gama(1)/2)+sin(- Phai(1)/2)*sin(Thita(1)/2)*sin(Gama(1)/2);q1(1)=cos(-Pha

21、i(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(Thita(1)/2)*sin(Gama(1)/2)-sin(-Phai(1)/2)*cos(Thita(1)/2)*cos(Gama(1)/2);Wie=0.000072

22、921151467;%已經(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)*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;c

23、23=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/c22);endif abs(c22)0.0000000000001 & c210Phai(k)=pi/2;endif abs(c22)0.0000000000001 & c210

24、.0000000000001 & c220Phai(k)=atan(-c21/c22);endif abs(c22)0.0000000000001 & c220 & c210Phai(k)=atan(c21/c22)+pi;endif abs(c22)0.0000000000001 & c220 & c210.0000000000001Phai(k)=atan(-c21/c22);endif abs(c22)0.0000000000001 & c210Phai(k)=pi/2;endif abs(c22)0.0000000000001 & c210.0000000000001 & c220 P

25、hai(k)=atan(-c21/c22);endif abs(c22)0.0000000000001 & c220 & c210Phai(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(

26、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=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);nmda(k+1)=0.5*T*(Vxt(k+1)+Vxt(k)/Rxt*sec(L(k)+nmda(k);%課本 86 頁 4.2-38Wenn=-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*Winn;Wtbb=Wibb(k,:)