北航惯性导航综合实验四实验报告.docx
《北航惯性导航综合实验四实验报告.docx》由会员分享，可在线阅读，更多相关《北航惯性导航综合实验四实验报告.docx（20页珍藏版）》请在冰点文库上搜索。
北航惯性导航综合实验四实验报告
基于运动规划的惯性导航系统动态实验
二零一三年六月十日
实验4.1惯性导航系统运动轨迹规划与设计实验
一、实验目的
为进展动态下简化惯性导航算法的实验研究，进展路径和运动状态规划，以验证不同运动状态下惯导系统的性能。
通过实验掌握步进电机控制方法，并产生不同运动路径和运动状态。
二、实验容
学习利用6045B控制板对步进电机进展控制的方法，并控制电机使运动滑轨产生定长运动和不同加速度下的定长运动。
三、实验系统组成
USB_PCL6045B控制板〔评估板〕、运动滑轨和控制计算机组成。
四、实验原理
IMU安装误差系数的计算方法
USB_PCL6045B控制板采用了USB串行总线接口通信方式，不必拆卸计算机箱就可以在台式机或笔记本电脑上进展运动控制芯片PCL6045B的学习和评估。
USB_PCL6045B评估板采用USB串行总线方式实现评估板同计算机的数据交换，由评估板的FIFO控制回路完成步进电机以及伺服电机的高速脉冲控制，任意2轴的圆弧插补，2-4轴的直线插补等运动控制功能。
USB_PCL6045B评估板上配置了全部PCL6045B芯片的外部信号接口和增量编码器信号输入接口。
由
USB_PCL6045B评估测试软件可以进展PCL6045B芯片的主要功能的评估测试。
图4-1-1USB_PCL6045B评估板原理框图
如图4-1-1所示，11接口主要用于外部电源连接，可以选择DC5V单一电源或DC5V/24V电源。
12接口是USB信号接口，用于USB_PCL6045B评估板同计算机的数据交换。
USB_PCL6045B评估板已经完成对PCL6045B芯片的底层程序开发和硬件资源与端口的驱动，并封装成156个API接口函数。
用户可直接在VC环境下利用API接口函数进展编程。
五、实验容
1、操作步骤
1〕检查电机驱动电源〔24V〕
2〕检查USB_PCL6045B控制板与上位机及电机驱动器间的连接电缆
3〕启动USB_PCL6045B控制板评估测试系统检查系统是否正常工作。
4〕运行编写的定长运动程序，并比拟实际位移与设定位移。
5〕修改程序设定不同运动长度，并重复执行步骤4〕。
6〕对记录实验数据，并进展误差分析。
2、实验数据处理
基于VC的控制界面：
本次实验必须先设计控制系统的上位机，通过上位机的串口向下位机发送控制命令，下位机接收到命令后，产生PWM波，控制电机的正反转以此到达控制导轨运动的目的。
系统的控制界面如图1所示：
图1系统的控制界面
控制导轨运动，运动采取正向运动，再返回，即IMU的实际运行位移为零。
并保存数据
控制界面的应用程序
源程序仅写出VC中按钮的响应程序：
voidCAaaDlg:
:
Online（）////定长运动
{
//TODO:
Addyourcontrolnotificationhandlercodehere
USB_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）;//减速开关的输入逻辑,负逻辑
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:
Addyourcontrolnotificationhandlercodehere
USB_sd_stop（AXS_AX）;
}
voidCAaaDlg:
:
OnGetSpeed（）//////获得速度
{
//TODO:
Addyourcontrolnotificationhandlercodehere
USB_get_speed（AXS_AX,&m_speed）;
UpdateData（FALSE）;
}
voidCAaaDlg:
:
OnButton3（）///OK按钮程序
{
//TODO:
Addyourcontrolnotificationhandlercodehere
UpdateData（true）;
}
3，处理数据
由实验原理可知，惯性测量单元〔IMU〕可以通过自身独立的测量结果进展积分，计算出目标运动的角度和位移等量。
本次实验就是利用IMU的加速度计的某一敏感轴测量导轨运行的加速度，通过加速度两次积分得到物体的位移，计算结果如图2所示：
实验经过往返，从原理上讲位移应该为零。
处理结果：
位移曲线：
速度曲线：
4，源程序：
A=load（'E:
\惯性器件综合实验\我的作业\实验四\X300000_V10000.txt'）;
T=1/200;%%%%单位为秒
g=9.78;
Ax=A（:
4）*g/1000;%%%提取加速度计的值转化为m/s^2
Ax=Ax*（1.0009）-0.0036595*g;
vx=zeros（12657,1）;
sx=zeros（12657,1）;
u=zeros（12657,1）;
%%%%%计算位移
fori=2:
12657
vx（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;
end
figure
plot（u/100,vx）;
xlabel（'时间/秒'）,ylabel（'速度米/秒'）;
figure
plot（u/100,sx）;
xlabel（'时间/秒'）,ylabel（'位移米'）;
5，实验结果分析
从原理上讲IMU做往返运动，位移应该出现增大和减小的趋势，但是由于各种误差角，而且滑轨也不能保证当地水平，在计算过程中，也未减去有害加速度。
所以误差很大。
而且根据所采集的数据可知加速度计并没有感知方向，在实验过程中应该根据计算脉冲与时间，自己计算方向时间
惯性导航系统半物理仿真实验
一、实验目的
进展惯导系统半物理仿真实验，以验证惯性器件真实误差特性情况下惯性导航系统的性能。
二、实验容
将采集到陀螺仪与加速度计的真实误差数据叠加到轨迹发生器产生的导航参数真值上，进展惯导解算，并分析误差特性。
三、实验系统组成
真实的陀螺仪、加速度计或IMU，数据采集系统和数据处理计算机。
四、实验步骤
〔1〕采集实验数据
〔2〕处理采集的实验数据，生成半物理的惯性器件误差数据
〔3〕生成半物理的导航数据，进展导航解算
〔4〕对导航解算结果进展分析
〔5〕完成实验报告
五、实验容及结果
〔1〕半物理仿真数据的生成：
a）应用前面IMU实验或惯性导航系统动态实验中采集的陀螺仪与加速度计的静态数据DATA
b）对以上采集的静态数据求取均方差X〔结果为X度/小时或Xg〕
c）将DATA中数据去掉均值生成新的数据DATA1〔器件噪声〕
d）自己设定要仿真的陀螺或加速度计的精度Y〔度/小时或g〕
e）将DATA1中数据乘以Y/X生成新的数据DATA2〔半物理仿真噪声〕
f）从DATA2中读取数据并叠加到轨迹发生器产生的标准数据（不含噪声）上，进展导航解算。
〔如初始采集的数据长度不够，可以将DATA2中数据重复利用，即将生成一个几倍长度于DATA2和数据文件DATA3，并从DATA3中读取半物理数据并叠加到轨迹发生器产生的标准数据上〕
〔2〕加半物理仿真噪声数据的导航结果：
〔3〕叠加噪声的导航结果：
〔4〕结果分析：
由实验结果可见，叠加的仿真噪声数据对姿态的解算影响很大；但由于所加噪声较小，所以噪声数据对位移和速度的解算影响不大。
六，源程序
clear,clc
invout=load（'E:
\惯导实验数据\第四次\实验4.3\第四局部半物理仿真数据生成方法及数据格式说明\IMU数据\invout.dat'）;
CaijiShuju=load（'E:
\惯导实验数据\第四次\实验4.3\第四局部半物理仿真数据生成方法及数据格式说明\IMU数据\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度/小时转成弧度
sx=0;
fori=1:
n
sx=sx+（W（i,1）-W_pingjun（1,1））^2
end
wx_junfangcha=sqrt（sx/n）;%x陀螺的均方差
wx1=w_jingdu/wx_junfangcha;
Wx=wx*wx1%半物理仿真噪声Wx
sx=0;
fori=1:
n
sx=sx+（W（i,2）-W_pingjun（1,2））^2
end
wy_junfangcha=sqrt（sx/n）;%y陀螺的均方差
wy2=w_jingdu/wy_junfangcha;
Wy=wy*wy2%半物理仿真噪声Wy
sx=0;
fori=1:
n
sx=sx+（W（i,3）-W_pingjun（1,3））^2
end
wz_junfangcha=sqrt（sx/n）;%z陀螺的均方差
wz3=w_jingdu/wz_junfangcha;
Wz=wz*wz3%半物理仿真噪声Wz
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%求加计的均方差%%%%%%%%%%%%%%%%%%%%%%%
f_jingdu=1/1000*9.8%加计的精度为1mg
sx=0;
fori=1:
n
sx=sx+（F（i,1）-F_pingjun（1,1））^2
end
fx_junfangcha=sqrt（sx/n）;%x加计的均方差
fx1=f_jingdu/fx_junfangcha;
Fx=fx*fx1;
sx=0;
fori=1:
n
sx=sx+（F（i,2）-F_pingjun（1,2））^2
end
fy_junfangcha=sqrt（sx/n）;%y加计的均方差
fx2=f_jingdu/fy_junfangcha;
Fy=fx*fx2;
sx=0;
fori=1:
n
sx=sx+（F（i,3）-F_pingjun（1,3））^2
end
fz_junfangcha=sqrt（sx/n）;%z加计的均方差
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（invout）;
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=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.1467;%已经是弧度制
L
（1）=40/180*pi;
nmda
（1）=116.0/180*pi;
T=0.01;%采样频率为100Hz
Vxt
（1）=0;
Vyt
（1）=0;
Re=6378245+80;%加高度80米
e=1/298.3;
fork=1:
n1
c11=q0（k）^2+q1（k）^2-q2（k）^2-q3（k）^2;
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）^2-q1（k）^2+q2（k）^2-q3（k）^2;
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）^2-q1（k）^2-q2（k）^2+q3（k）^2;
b=[c11,c12,c13
c21,c22,c23
c31,c32,c33];
ifabs（c22）>0.01
Phai（k）=atan（-c21/c22）;
end
ifabs（c22）>0.01&c21>0
Phai（k）=pi/2;
end
ifabs（c22）>0.01&c21<0
Phai（k）=-pi/2;
end
ifabs（c22）>0.01&c22>0
Phai（k）=atan（-c21/c22）;
end
ifabs（c22）>0.01&c22>0&c21>0
Phai（k）=atan（c21/c22）+pi;
end
ifabs（c22）>0.01&c22>0&c21<0
Phai（k）=atan（-c21/c22）-pi;
end
Thita（k）=asin（c23）;
Gama（k）=-atan（c13/c33）;
Cbn=inv（b）;
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=b*Winn;
Wtbb=Wibb（k,:
）'-Winb;
dltaTita0_fang=（Wtbb（1,1）*T）^2+（Wtbb（2,1）*T）^2+（Wtbb（3,1）*T）^2;
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）;
end
figure
holdon
i=1:
n1;
subplot（1,2,1）,plot（i,Vxt（i））%速度误差
title（'叠加噪声后的东向速度误差'）
subplot（1,2,2）,plot（i,Vyt（i））
title（'叠加噪声后的的北向速度误差'）
figure
holdon
i=1:
n1;
subplot（1,2,1）,plot（i,L（i）*180/pi）%位置误差
title（'叠加噪声后的的纬度误差'）
subplot（1,2,2）,plot（i,nmda（i）*180/pi）
title（'叠加噪声后的的经度误差'）
figure
holdon
i=1:
n1;
plot（i,Phai（i）*180/pi）%姿态角误差subplot（1,3,1）,
title（'叠加噪声后的的航向角误差'）
figure
holdon
i=1:
n1;
plot（i,Thita（i）*180/pi）%subplot（1,3,2）,
title（'叠加噪声后的俯仰角误差'）
figure
holdon
i=1:
n1;
plot（i,Gama（i）*180/pi）%subplot（1,3,3）,
title（'叠加噪声后的横滚角误差'）
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%轨迹发生器的数据处理%%%%%%%%%%%%%%%
clear,clc
invout=load（'E:
\惯导实验数据\第四次\实验4.3\第四局部半物理仿真数据生成方法及数据格式说明\IMU数据\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）;%经度
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=zero
升级会员 