北航惯性导航综合实验四实验报告.

1、仪器曲光口学院school of ioiw scimi tommies EXGinc基于运动规划的惯性导航系统动态实验二零一三年六月十日实验 4.1 惯性导航系统运动轨迹规划与设计实验一、实验目的为进行动态下简化惯性导航算法的实验研究，进行路径和运动状态规划， 以验证不同运动状态下惯导系统的性能。通过实验掌握步进电机控制方法，并 产生不同运动路径和运动状态。二、实验内容学习利用 6045B 控制板对步进电机进行控制的方法，并控制电机使运动滑 轨产生定长运动和不同加速度下的定长运动。三、实验系统组成USB_PCL6045控制板（评估板）、运动滑轨和控制计算机组成。四、实验原理IMU 安装误差系数

2、的计算方法USB_PCL6045控制板采用了 USB串行总线接口通信方式，不必拆卸计算 机箱就可以在台式机或笔记本电脑上进行运动控制芯片PCL6045B的学习和评估。USB_PCL6045评估板采用 USB串行总线方式实现评估板同计算机的数据 交换，由评估板的 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评估 板同计算机的数据交换。USB_PCL6045评估板已经完成对PCL6045B芯片的底层程序开发和硬件资 源与端口的驱动，并封装成156个API接口函数。用户可直接在 VC环境下利 用API接口函数进行编程。五、实验内容1、操作步骤1）检查电机驱动电源（24V）2）检查USB_PCL6045控制板与上位机及电机驱动器间的连接电缆3）启动USB_PCL6045控制板评估测试系统检查系统是否正常工作

4、。4）运行编写的定长运动程序，并比较实际位移与设定位移。5） 修改程序设定不同运动长度，并重复执行步骤4）。6）对记录实验数据，并进行误差分析。2、实验数据处理基于VC的控制界面：本次实验必须先设计控制系统的上位机，通过上位机的串口向下位机发送控制命令，下位机接收到命令后，产生PWM波，控制电机的正反转以此达到控制导轨运动的目的。系统的控制界面如图 1所示:InputDistance0Speed0OKOutputGeiSpeedCurrertSpeed0Control定长运动停止运动图1系统的控制界面控制导轨运动，运动采取正向运动，再返回，即IMU的实际运行位移为零。并保存数据控制界面的应用程

5、序源程序仅写出VC中按钮的响应程序：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);原点开关的逻辑， 负逻辑USB_set_el_logic(AXS_AX,0);/硬极限输入逻辑，低电平使能USB_set_sd_logic(AXS_AX,0);减速开关的输入逻辑，负逻辑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, 处理数据由实验原理可知，惯性测量单元（IMU ）可以通过自身独立的测量结果进行积分，计算出目标运

9、动的角度和位移等量。本次实验就是利用IMU的加速度计的某一敏感轴测量导轨运行的加速度，通过加速度两次积分得到物体的位 移，计算结果如图2所示：实验经过往返，从原理上讲位移应该为零。 处理结果：位移曲线：速度曲线:20406080100120时间解6 53 21404, 源程序：惯性器件综合实验我的作业实验四X300000_V10000.txt);T=1/200; %单位为秒g=9.78;Ax=A(:,4)*g/1OOO; %提取加速度计的值转化为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, 实验结果分析从原理上讲IMU做往返运动，位移应该出现增大和减小的趋势，但是由于各 种误差角，而且滑轨也不能保证当地水平，在计算过程中，也未减去有害加速 度。

11、所以误差很大。而且根据所采集的数据可知加速度计并没有感知方向，在 实验过程中应该根据计算脉冲与时间，自己计算方向时间惯性导航系统半物理仿真实验、实验目的进行惯导系统半物理仿真实验，以验证惯性器件真实误差特性情况下惯性 导航系统的性能。二、实验内容 将采集到陀螺仪与加速度计的真实误差数据叠加到轨迹发生器产生的导航 参数真值上，进行惯导解算，并分析误差特性。三、实验系统组成真实的陀螺仪、加速度计或 IMU ，数据采集系统和数据处理计算机。四、实验步骤（1）采集实验数据（2）处理采集的实验数据，生成半物理的惯性器件误差数据（3）生成半物理的导航数据，进行导航解算（4）对导航解算结果进行分析（5）完成

12、实验报告五、实验内容及结果（1）半物理仿真数据的生成：a） 应用前面 IMU 实验或惯性导航系统动态实验中采集的陀螺仪与加速度计的 静态数据 DATAb） 对以上采集的静态数据求取均方差 X （结果为 X 度 /小时或 Xg ）c） 将 DATA 中数据去掉均值生成新的数据 DATA1 （器件噪声）d）自己设定要仿真的陀螺或加速度计的精度 丫（度/小时或g）e）将DATA1中数据乘以Y/X生成新的数据DATA2 （半物理仿真噪声）f）从 DATA2 中读取数据并叠加到轨迹发生器产生的标准数据（不含噪声 ）上，进行导航解算。（如初始采集的数据长度不够，可以将 DATA2 中数据重复 利用，即将生

13、成一个几倍长度于 DATA2和数据文件DATA3，并从DATA3 中读取半物理数据并叠加到轨迹发生器产生的标准数据上）（ 2）加半物理仿真噪声数据的导航结果：轨迹笈注器的航向角误差(3)叠加噪声的导航结果:叠加哩声后的东向速度误差170 r60 -SO -403020，0 |QIj10 -j51015叠加嗥声后的的北向速度误差X Tfl叠加嚨声后的的航向莆误差(4)结果分析：由实验结果可见，叠加的仿真噪声数据对姿态的解算影响很大；但由于所加噪 声较小，所以噪声数据对位移和速度的解算影响不大。六，源程序clear,clcin vout=load(惯导实验数据第四次实验第四部分半物理仿真数据生成方

14、法及数据格式说明IMU数据invout.dat);CaijiShuju=load(惯导实验数据第四次实验第四部分半物理仿真数据生成方法及数据格式说明IMU数据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);%陀%螺%的精度设为 0.5 度/ 小时 % w_jingdu=0.5/3600*pi/180%0.5 度 / 小时转成弧度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;%轨%迹% 发生 器 数 据 叠声%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);%经度Height_invout=invout(:,10);%高度%开%始%叠%加 %N1=size(invo

19、ut);n1=N1(1,1);%应为采样点数大于轨迹发生器的个数，所以以轨迹发生器的个数为准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;%已经是弧度制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,:)