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

儀器程學院SCBOOLOFlOOIENTSCIENCEIOHO-TOMNICS哪皿冊G基于運動規(guī)劃的慣性導航系統(tǒng)動態(tài)實驗二零一三年六月十日實驗4.1慣性導航系統(tǒng)運動軌跡規(guī)劃與設計實驗一、 實驗目的為進行動態(tài)下簡化慣性導航算法的實驗研究，進行路徑和運動狀態(tài)規(guī)劃，以驗證不同運動狀態(tài)下慣導系統(tǒng)的性能。通過實驗掌握步進電機控制方法，并產(chǎn)生不同運動路徑和運動狀態(tài)。二、 實驗內(nèi)容學習利用6045B控制板對步進電機進行控制的方法，并控制電機使運動滑軌產(chǎn)生定長運動和不同加速度下的定長運動。三、 實驗系統(tǒng)組成USB_PCL6045B控制板（評估板X運動滑軌和控制計算機組成。四、實驗原理IMU安裝誤差系數(shù)的計算方法USB_PCL6045B控制板采用了USB串行總線接口通信方式，不必拆卸計算機箱就可以在臺式機或筆記本電腦上進行運動控制芯片PCL6045B的學習和評估。USB_PCL6045B評估板采用USB串行總線方式實現(xiàn)評估板同計算機的數(shù)據(jù)交換，由評估板的FIFO控制回路完成步進電機以及伺服電機的高速脈沖控制，任意2軸的圓弧插補，2-4軸的直線插補等運動控制功能。USB_PCL6045B評估板上配置了全部PCL6045B芯片的外部信號接口和增量編碼器信號輸入接口。由USB_PCL6045B評估測試軟件可以進行PCL6045B芯片的主要功能的評估測試。圖4-1-1USB_PCL6045B評估板原理框圖如圖4-1-1所示，CN11接口主要用于外部電源連接，可以選擇DC5V單一電源或DC5V/24V電源。CN12接口是USB信號接口USB_PCL6045B評估板同計算機的數(shù)據(jù)交換。USB_PCL6045B評估板已經(jīng)完成對PCL6045B芯片的底層程序開發(fā)和硬件資源與端口的驅(qū)動，并封裝成156個API接口函數(shù)。用戶可直接在VC環(huán)境下利用API接口函數(shù)進行編程。五、實驗內(nèi)容1、 操作步驟1） 檢查電機驅(qū)動電源（24V）2） 檢查USB_PCL6045B控制板與上位機及電機驅(qū)動器間的連接電纜3） 啟動USB_PCL6045B控制板評估測試系統(tǒng)檢查系統(tǒng)是否正常工作。4） 運行編寫的定長運動程序，并比較實際位移與設定位移。5） 修改程序設定不同運動長度，并重復執(zhí)行步驟4）。6） 對記錄實驗數(shù)據(jù)，并進行誤差分析。2、 實驗數(shù)據(jù)處理基于VC的控制界面：本次實驗必須先設計控制系統(tǒng)的上位機，通過上位機的串口向下位機發(fā)送控制命令，下位機接收到命令后，產(chǎn)生PWM波，控制電機的正反轉(zhuǎn)以此達到控制導軌運動的目的。系統(tǒng)的控制界面如圖1所示：圖1系統(tǒng)的控制界面控制導軌運動，運動采取正向運動，再返回，即IMU的實際運行位移為零。并保存數(shù)據(jù)控制界面的應用程序源程序僅寫出VC中按鈕的響應程序：voidCAaaDlg::Online() ////定長運動(//TODO:AddyourcontrolnotificationhandlercodehereUSB_initial();USB_default_set();USB_set_org_logic(AXS_AX,0);//原點開關的邏輯， 負邏輯USB_set_el_logic(AXS_AX,0);//硬極限輸入邏輯，低電平使能USB_set_sd_logic(AXS_AX,0);//M速開關的輸入邏輯,負邏輯USB_set_alm_logic(AXS_AX,1);//報警輸入信號邏輯USB_set_inp_logic(AXS_AX,1);//in的輸入信號邏輯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_change(AXS_AX,5000,5000);while(1)(USB_get_speed(AXS_AX,&m_speed);UpdateData(FALSE);MSGmsg;while(PeekMessage(&msg,0,0,100,PM_REMOVE))(TranslateMessage(&msg);DispatchMessage(&msg);}Sleep(100);}*/}voidCAaaDlg::OnButton1()//////停止運動(//TODO:AddyourcontrolnotificationhandlercodehereUSB_sd_stop(AXS_AX);}voidCAaaDlg::OnGetSpeed()//////獲得速度(//TODO:AddyourcontrolnotificationhandlercodehereUSB_get_speed(AXS_AX,&m_speed);UpdateData(FALSE);voidCAaaDlg::OnButton3()///OK按鈕程序(//TODO:AddyourcontrolnotificationhandlercodehereUpdateData(true);}3，處理數(shù)據(jù)由實驗原理可知，慣性測量單元(IMU)可以通過自身獨立的測量結(jié)果進行積分，計算出目標運動的角度和位移等量。本次實驗就是利用IMU的加速度計的某一敏感軸測量導軌運行的加速度，通過加速度兩次積分得到物體的位移，計算結(jié)果如圖2所示：實驗經(jīng)過往返，從原理上講位移應該為零。處理結(jié)果：位移曲線：

oOO8oOO821oO64ooO208Q —一一-I I I I I I 0 20 40 60 80 100 120 140時間'?秒速度曲線：1_ / -qk_ i i i i i i 0 20 40 60 8。 100 120 140時間'?秒4，源程序：A=load('E:\慣性器件綜合實驗'我的作業(yè)'實驗四\X300000_V10000.txt');T=1/200;%%%%單位為秒g=9.78;Ax=A(:,4)*g/1000;%%%提取加速度計的值轉(zhuǎn)化為m/sA2Ax=Ax*(1.0009)-0.0036595*g;vx=zeros(12657,1);sx=zeros(12657,1);u=zeros(12657,1);%%%%%計算位移fori=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做往返運動，位移應該出現(xiàn)增大和減小的趨勢，但是由于各種誤差角，而且滑軌也不能保證當?shù)厮?，在計算過程中，也未減去有害加速度。所以誤差很大。而且根據(jù)所采集的數(shù)據(jù)可知加速度計并沒有感知方向，在實驗過程中應該根據(jù)計算脈沖與時間，自己計算方向時間慣性導航系統(tǒng)半物理仿真實驗一、實驗目的進行慣導系統(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） 完成實驗報告五、 實驗內(nèi)容及結(jié)果（1）半物理仿真數(shù)據(jù)的生成：a） 應用前面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） 自己設定要仿真的陀螺或加速度計的精度Y（度/小時或g）e） 將DATA1中數(shù)據(jù)乘以Y/X生成新的數(shù)據(jù)DATA2（半物理仿真噪聲）f） 從DATA2中讀取數(shù)據(jù)并疊加到軌跡發(fā)生器產(chǎn)生的標準數(shù)據(jù)（不含噪聲）上，

進行導航解算。（如初始采集的數(shù)據(jù)長度不夠，可以將DATA2中數(shù)據(jù)重復利用，即將生成一個幾倍長度于DATA2和數(shù)據(jù)文件DATA3，并從DATA3中讀取半物理數(shù)據(jù)并疊加到軌跡發(fā)生器產(chǎn)生的標準數(shù)據(jù)上）（2）加半物理仿真噪聲數(shù)據(jù)的導航結(jié)果:(3)疊加噪聲的導航結(jié)果:(4)結(jié)果分析：由實驗結(jié)果可見，疊加的仿真噪聲數(shù)據(jù)對姿態(tài)的解算影響很大；但由于所加噪聲較小，所以噪聲數(shù)據(jù)對位移和速度的解算影響不大。六，源程序clear,clcinvout=load('E:\慣導實驗數(shù)據(jù)'第四次'實驗4.3\第四部分半物理仿真數(shù)據(jù)生成方法及數(shù)據(jù)格式說明\IMU數(shù)據(jù)\invout.dat');CaijiShuju=load('E:\慣導實驗數(shù)據(jù)'第四次'實驗4.3\第四部分半物理仿真數(shù)據(jù)生成方法及數(shù)據(jù)格式說明\IMU數(shù)據(jù)\data2.txt');W=CaijiShuju(:,3:5);F=CaijiShuju(:,9:11);W_pingjun=mean(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_pingjun(1,2);fx=F(:,3)-F_pingjun(1,3);%%%%%%%%%%%%%%%%%%%%%%%%%%%求陀螺均方差％%%%%%%%%%%%%%%%%%%%N=size(W);n=N(1,1);%%%%%%%%%%%%%陀螺的精度設為0.5度/小時%%%%%%%%%%%%%%%%%%%w_jingdu=0.5/3600*pi/180%0.5度/小時轉(zhuǎn)成弧度sx=0;fori=1:nsx=sx+(W(i,1)-W_pingjun(1,1))A2endwx_junfangcha=sqrt(sx/n);%x陀螺的均方差wx1=w_jingdu/wx_junfangcha;Wx=wx*wx1%半物理仿真噪聲Wxsx=0;fori=1:nsx=sx+(W(i,2)-W_pingjun(1,2))A2endwy_junfangcha=sqrt(sx/n);%y陀螺的均方差wy2=w_jingdu/wy_junfangcha;sx=0;fori=1:nsx=sx+(W(i,3)-W_pingjun(1,3))A2endwz_junfangcha=sqrt(sx/n);%z陀螺的均方差wz3=w_jingdu/wz_junfangcha;Wz=wz*wz3%半物理仿真噪聲Wz%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%求加計的均方差%%%%%%%%%%%%%%%%%%%%%%%f_jingdu=1/1000*9.8%加計的精度為1mgsx=0;fori=1:nsx=sx+(F(i,1)-F_pingjun(1,1))A2endfx_junfangcha=sqrt(sx/n);%x加計的均方差fx1=f_jingdu/fx_junfangcha;Fx=fx*fx1;sx=0;fori=1:nsx=sx+(F(i,2)-F_pingjun(1,2))A2endfy_junfangcha=sqrt(sx/n);%y加計的均方差fx2=f_jingdu/fy_junfangcha;Fy=fx*fx2;sx=0;fori=1:nsx=sx+(F(i,3)-F_pingjun(1,3))A2endfz_junfangcha=sqrt(sx/n);%z加計的均方差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)度%%%%%%%%%%%%%%%開始疊加%%%%%%%%%%%%%%%%%%%%N1=size(invout);n1=N1(1,1);%%%%應為采樣點數(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=zeros(n1,1);Thita=zeros(n1,1);Gama=zeros(n1,1);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(-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(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)=116.0/180*pi;T=0.01;%采樣頻率為100HzVxt(1)=0;Re=6378245+80;%加高度80米e=1/298.3;fork=1:n1c11=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*(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];ifabs(c22)>0.0000000000001Phai(k)=atan(-c21/c22);endifabs(c22)>0.0000000000001&c21>0Phai(k)=pi/2;endPhai(k)=-pi/2;endifabs(c22)>0.0000000000001&c22>0Phai(k)=atan(-c21/c22);endifabs(c22)>0.0000000000001&c22>0&c21>0Phai(k)=atan(c21/c22)+pi;endifabs(c22)>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)*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);Wenn=[-Vyt(k)/Ryt;Vxt(k)/Rxt;Vxt(k)/Rxt*tan(L(k))];%課本86頁4.2-38式Winn=Wenn+[0;Wie*cos(L(k));Wie*sin(L(k))];Winb=Cnb*Winn;Wtbb=Wibb(k,:)'-Winb;dltaTita0_fang=(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,0]Q=((1-dltaTita0_fang/8)*eye(4)+0.5*dltaTita)*[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);endfigureholdoni=1:n1;subplot(1,2,1),plot(i,Vxt(i))%速度誤差title('疊加噪聲后的東向速度誤差')subplot(1,2,2),plot(i,Vyt(i))title('疊加噪聲后的的北向速度誤差')figureholdoni=1:n1;subplot(1,2,1),plot(i,L(i)*180/pi)%位置誤差title('疊加噪聲后的的緯度誤差')subplot(1,2,2),plot(i,nmda(i)*180/pi)title('疊加噪聲后的的經(jīng)度誤差')figureholdoni=1:n1;plot(i,Phai(i)*180/pi)%姿態(tài)角誤差subplot(1,3,1),title('疊加噪聲后的的航向角誤差')figureholdoni=1:n1;plot(i,Thita(i)*180/pi)%subplot(1,3,2),title('疊加噪聲后的俯仰角誤差')figureholdoni=1:n1;plot(i,Gama(i)*180/pi)%subplot(1,3,3),title('疊加噪聲后的橫滾角誤差')%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%軌跡發(fā)生器的數(shù)據(jù)處理％%%%%%%%%%%%%%%clear,clcinvout=load('E:\慣導實驗數(shù)據(jù)'第四次'實驗4.3\第四部分半物理仿真數(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=invout(:,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=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(-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(Thita(1)/2)*sin(Gama(1)/2)-sin(-Phai(1)/2)*cos(Thita(1)/2)*cos(Gama(1)/2);Wie=0.000072921151467;%已經(jīng)是弧度制nmda(1)=116.0/180*pi;T=0.01;%采樣頻率為100HzVxt(1)=0;Vyt(1)=0;Re=6378245+80;%加高度80米e=1/298.3;fork=1:n1c11=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*(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];ifabs(c22)>0.0000000000001Phai(k)=atan(-c21/c22);endifabs(c22)>0.0000000000001&c21>0Phai(k)=pi/2;endifabs(c22)>0.0000000000001&c21<0Phai(k)=-pi/2;endifabs(c22)>0.0000000000001&c22>0Phai(k)=atan(-c21/c22);endifabs(c22)>0.0000000000001&c22>0&c21>0Phai(k)=atan(c21/c22)+pi;endifabs(c22)>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,:)';卞踱L(⑵qqiM-'頊r￡)qqiM'oL(rDqqiM:l(r￡)qqiM-'L(r乙)qqiM-'L(九)qqiM-'o]=叩_l