北航惯性导航综合实验五实验报告

1惯性导航技术综合实验实验五惯性基组合导航及应用技术实验2惯性/卫星组合导航系统车载实验一、实验目的①掌握捷联惯导/GPS组合导航系统的构成和基本工作原理；②掌握采用卡尔曼滤波方法进行捷联惯导/GPS组合的基本原理；③掌握捷联惯导/GPS组合导航系统静态性能；④掌握动态情况下捷联惯导/GPS组合导航系统的性能。二、实验内容①复习卡尔曼滤波的基本原理（参考《卡尔曼滤波与组合导航原理》第二、五章）；②复习捷联惯导/GPS组合导航系统的基本工作原理（参考以光衢编著的《惯性导航原理》第七章）；三、实验系统组成①捷联惯导/GPS组合导航实验系统一套；②监控计算机一台。③差分GPS接收机一套；④实验车一辆；⑤车载大理石平台；⑥车载电源系统。四、实验内容1）实验准备①将IMU紧固在车载大理石减振平台上，确认IMU的安装基准面紧靠实验平台；②将IMU与导航计算机、导航计算机与车载电源、导航计算机与监控计算机、GPS接收机与导航计算机、GPS天线与GPS接收机、GPS接收机与GPS电3池之间的连接线正确连接；③打开GPS接收机电源，确认可以接收到4颗以上卫星；④打开电源，启动实验系统。2）捷联惯导/GPS组合导航实验①进入捷联惯导初始对准状态，记录IMU的原始输出，注意5分钟内严禁移动实验车和IMU；②实验系统经过5分钟初始对准之后，进入导航状态；③移动实验车，按设计实验路线行驶；④利用监控计算机中的导航软件进行导航解算，并显示导航结果。五、实验结果及分析(一)理论推导捷联惯导短时段（1分钟）位置误差，并用1分钟惯导实验数据验证。1、一分钟惯导位置误差理论推导：短时段内（t5min），忽略地球自转0ie，运动轨迹近似为平面1/0R，此时的位置误差分析可简化为：（1）加速度计零偏引起的位置误差：210.88022txm（2）失准角0引起的误差：2020.92182gtxm（3）陀螺漂移引起的误差：330.01376gtxm可得1min后的位置误差值1231.8157mxxxx2、一分钟惯导实验数据验证结果：（1）纯惯导解算1min的位置及位置误差图：4010002000300040005000600029.1429.1529.1629.1729.18lat0.01s度0100020003000400050006000121.345121.35121.355121.36121.365lon0.01s度纯惯导解算GPS理论真值0100020003000400050006000-3-2-101北向位移误差0.01sm0100020003000400050006000-10-505东向位移误差0.01sm（2）纯惯导解算1min的速度及速度误差图：50100020003000400050006000-100-50050Vx0.01sm/s0100020003000400050006000020406080Vy0.01sm/s纯惯导解算GPS理论真值0100020003000400050006000-0.4-0.3-0.2-0.10Vx误差0.01sm/s0100020003000400050006000-0.1-0.0500.050.1Vy误差0.01sm/s实验结果分析：纯惯导解算短时间内精度很高，1min的惯导解算的北向最大位移误差-2.668m，东向最大位移误差-8.231m，可见实验数据所得位置误差与理论推导的位置误差在同一数量级，结果不完全相同是因为理论推导时做了大量简化，而且实验时视GPS为真实值也会带来误差；另外，可见1min内纯惯导解算的东向速度最大误差-0.2754m/s，北向速度最大误差-0.08027m/s。(二)选取IMU前5分钟数据进行对准实验。将初始对准结果作为初值完成1小时捷联惯导和组合导航解算，对比1小时捷联惯导和组合导航结果。61、5minIMU数据的解析粗对准结果：-1.1960-0.9998-0.00840.9979-1.1990-0.01630.00620.11980.9991Cnbattitude=[219.7700-0.93180.4828]2、5minIMU数据的Kalman滤波精对准结果：00.511.522.53x104219.5220X:2.797e+04Y:220航向角0.01秒度00.511.522.53x104-1-0.9-0.8俯仰角0.01秒度00.511.522.53x1040.40.60.8横滚角0.01秒度fai=219.9744642380873*pi/180;theta=-0.895865732956914*pi/180;gama=0.640089448357591*pi/180;0123x104-2024x10-4北向速度误差0.01秒米/秒0123x10400.51x10-3北向速度误差方差0.01秒米/秒0123x1040246x10-4东向速度误差0.01秒米/秒0123x10400.51x10-3东向速度误差方差0.01秒米/秒70123x104-202x10-3北向失准角0.01秒度0123x10422.53x10-3北向失准角方差0.01秒度0123x104-101x10-3东向失准角0.01秒度0123x10422.53x10-3东向失准角方差0.01秒度0123x104-505x10-5天向失准角0.01秒度0123x1042.833.2x10-3天向失准角方差0.01秒度3、一小时IMU/GPS数据的组合导航结果图及估计方差P阵图：00.511.522.533.54x10539.84040.2纬度0.01s°00.511.522.533.54x105116116.5117经度0.01s°00.511.522.533.54x105050100高度0.01sm组合滤波GPS00.511.522.533.54x105-20020东向速度0.01sm/s00.511.522.533.54x105-20020北向速度0.01sm/s00.511.522.533.54x105-202天向速度0.01sm/s组合滤波GPS800.511.522.533.54x105100200300航向角0.01s°00.511.522.533.54x105-505俯仰角0.01s°00.511.522.533.54x105-505横滚角0.01s°00.511.522.533.54x105024x10-4P航向角误差0.01s度00.511.522.533.54x105024x10-4P俯仰角误差0.01s度00.511.522.533.54x10500.51x10-3P横滚角误差0.01s度00.511.522.533.54x10500.51x10-3P东向速度误差0.05sm/s00.511.522.533.54x10500.51x10-3P北向速度误差0.05sm/s00.511.522.533.54x10500.51x10-3P天向速度误差0.01sm/s900.511.522.533.54x105024x10-8P纬度误差0.01s度00.511.522.533.54x105024x10-8P经度误差0.01s度00.511.522.533.54x10500.10.2P高度误差0.01sm4、一小时IMU数据的捷联惯导解算结果与组合滤波、GPS输出对比图：00.511.522.533.54x105304050lat0.01s度00.511.522.533.54x105110115120lon0.01s度00.511.522.533.54x105-202x104height0.01sm纯惯导解算INS/GPS组合滤波GPS输出5、结果分析：由滤波结果图可以看出：（1）由组合后的速度、位置的P阵可以看出滤波之后载体的速度和位置比GPS输出的精度高。（2）短时间内SINS的精度较高，初始阶段的导航结果基本和GPS、组合导航结果重合，1小时后的捷联惯导解算结果很差，纬度、经度、高度均发散。（3）INS/GPS组合滤波的结果和GPS的输出结果十分近似，因为1小时的导航GPS的精度比SINS导航的精度高很多，Kalman滤波器中GPS信号的权重更大。10（4）总体看来，SINS/GPS组合滤波的结果优于单独用SINS或GPS导航的结果，起到了协调、超越、冗余的作用，使导航系统更可靠。六、SINS/GPS组合导航程序%%%%%%%%%%%%%%%%%%%%%%%INS/GPS组合导航跑车1h实验%%%%%%%%%%%%%%%%%%%%%%%%%%%该程序为15维状态量，6维观测量的kalman滤波程序，惯性/卫星组合松耦合的数学模型clearclccloseall%%初始量定义wie=0.000072921151467;Re=6378135.072;g=9.7803267714;e=1.0/298.25;T=0.01;%IMU频率100hz,此程序中GPS频率100hzdatanumber=360000;%数据时间3600sa=load('imu_1h.dat');w=a(:,3:5)'*pi/180/3600;%陀螺仪输出的角速率信息单位由°/h化为rad/sf=a(:,6:8)';%三轴比力输出，单位ga=load('gps_1h_new.dat');gps_pos=a(:,3:5);%GPS输出的纬度、经度、高度信息gps_pos(:,1:2)=gps_pos(:,1:2)*pi/180;%纬经单位化为弧度gps_v=a(:,6:8);%GPS输出的东北天速度信息%捷联解算及卡尔曼相关v=zeros(datanumber,3);%组合后的速度信息atti=zeros(datanumber,3);%组合后的姿态信息pos=zeros(datanumber,3);%组合后的位置信息gyro=zeros(3,1);acc=zeros(3,1);x_kf=zeros(datanumber,15);p_kf=zeros(datanumber,15);lat=40.0211142228246*pi/180;%组合导航的初始位置、姿态、速度lon=116.3703629769560*pi/180;height=43.0674;fai=219.9744642380873*pi/180;theta=-0.895865732956914*pi/180;gama=0.640089448357591*pi/180;11Vx=gps_v(1,1);Vy=gps_v(1,2);Vz=gps_v(1,3);X_o=zeros(15,1);%X的初值选为0X=zeros(15,1);%Q=diag([(50e-6*g)^2,(50e-6*g)^2,(50e-6*g)^2,(0.1*pi/180/3600)^2,(0.1*pi/180/3600)^2,(0.1*pi/180/3600)^2,0,0,0,0,0,0,0,0,0]);%随机Q=diag([(0.008*pi/180/3600)^2,(0.008*pi/180/3600)^2,(0.008*pi/180/3600)^2,(50e-6*g)^2,(50e-6*g)^2,(50e-6*g)^2,0,0,0,0,0,0,0,0,0]);R=diag([(0.01)^2,(0.01)^2,(0.01)^2,(0.1)^2,(0.1)^2,(0.15)^2]);P=zeros(15);P_k=diag([(0.00005*pi/180)^2,(0.00005*pi/180)^2,(0.00005*pi/180)^2,0.00005^2,0.00005^2,0.00005^2,2^2,2^2,2^2,(0.001*pi/180/3600)^2,(0.001*pi/180/3600)^2,(0.001*pi/180/3600)^2,(50e-6*g)^2,(50e-6*g)^2,(5