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

1、仪養科学与光虹程学院sn OF INST腮NT SCIENCE IOPTHLOONICS ENGI»G惯性导航技术综合实验实验五惯性基组合导航及应用技术实验i惯性 /卫星组合导航系统车载实验一、实验目的 掌握捷联惯导 /GPS 组合导航系统的构成和基本工作原理； 掌握采用卡尔曼滤波方法进行捷联惯导 /GPS 组合的基本原理； 掌握捷联惯导 /GPS 组合导航系统静态性能； 掌握动态情况下捷联惯导 /GPS 组合导航系统的性能。二、实验内容 复习卡尔曼滤波的基本原理（参考卡尔曼滤波与组合导航原理第二、五章）； 复习捷联惯导 /GPS 组合导航系统的基本工作原理（参考以光衢编著的惯性导航

2、原理 第七章）；三、实验系统组成 捷联惯导 /GPS 组合导航实验系统一套； 监控计算机一台。 差分GPS 接收机一套； 实验车一辆； 车载大理石平台； 车载电源系统。四、实验内容1） 实验准备 将 IMU 紧固在车载大理石减振平台上，确认 IMU 的安装基准面紧靠实 验平台； 将 IMU 与导航计算机、 导航计算机与车载电源、 导航计算机与监控计算机、GPS接收机与导航计算机、GPS天线与GPS接收机、GPS接收机与GPS电池之间的连接线正确连接； 打开GPS接收机电源，确认可以接收到4颗以上卫星； 打开电源，启动实验系统。2）捷联惯导/GPS组合导航实验 进入捷联惯导初始对准状态，记录IM

3、U的原始输出，注意5分钟内严禁 移动实验车和IMU ； 实验系统经过5分钟初始对准之后，进入导航状态； 移动实验车，按设计实验路线行驶； 利用监控计算机中的导航软件进行导航解算，并显示导航结果。五、实验结果及分析（一）理论推导捷联惯导短时段（1分钟）位置误差，并用 1分钟惯导实验数据验证。短时段内（t<5min），忽略地球自转ie1、一分钟惯导位置误差理论推导：0，运动轨迹近似为平面 1/ R 0，此时的位置误差分析可简化为:(1)加速度计零偏引起的位置误差:£0.8802 m2(2)失准角0引起的误差:0t20.9218 m(3)陀螺漂移引起的误差:X3.3g t60.013

4、7 m可得1min后的位置误差值xX1X2x31.8157m2、一分钟惯导实验数据验证结果：（1）纯惯导解算1min的位置及位置误差图:21latIon121.365121.36纯惯导解算GPS理论真值-度 121.355121.35121.34501000200030004000500060000.01s北向位移误差东向位移误差(2)纯惯导解算1min的速度及速度误差图:VxVy80s m604020100020003000400050006000纯惯导解算GPS理论真值0.01ss mVx误差-0.1-0.2-0.3100020003000400050006000-0.40.01s0.01

5、s实验结果分析：纯惯导解算短时间内精度很高,1min的惯导解算的北向最大位移误差-2.668m，东向最大位移误差-8.231m，可见实验数据所得位置误差与理论推导的位置误差在同一数量级，结果不完全相同是因为理论推导时做了大量简化，而且实验时视GPS为真实值也会带来误差；另外，可见度最大误差-0.08027m/s。1min内纯惯导解算的东向速度最大误差-0.2754m/s，北向速（二）选取IMU前5分钟数据进行对准实验。 将初始对准结果作为初值完成 1小时捷联惯导和 组合导航解算，对比1小时捷联惯导和组合导航结果。1、5minIMU数据的解析粗对准结果:-1.1960-0.9998-0.0084

6、Cnb0.9979-1.1990-0.01630.00620.11980.9991attitude :=219.7700-0.93180.48282、5minIMU 数据的Kalman滤波精对准结果:220度-0.9-11X: 2.797e+04Y: 220rrtrt00.511.50.01 秒俯仰角22.534x 10cci.1 *1trtrt00.511.50.01 秒横滚角22.534x 10L1.ir.口Lrrrrr00.511.522.53航向角219.5-0.80.80.01 秒度°.60.44x 10fai = 219.9744642380873*pi/180; the

7、ta = -0.895865732956914*pi/180; gama = 0.640089448357591*pi/180;0.01 秒“4x 10x 10 -北向速度误差方差1 ,米0.50.。1 秒x 1040 1cc01230.01秒加x 10x 10 -东向速度误差方差1 -米0.50 101230.01 秒“4x 10度度0-20.01 秒东向失准角4x 10x 10 3 北向失准角2.5234x 100.01 秒x 10 3北向失准角方差0-150-5-3x 100.01 秒34x 10x 10 "东向失准角方差度2.5, 20.01 秒度3度J2 8123(12 :

8、x 10天向失准角x 100.01 秒4x 10x 10 "天向失准角方差0.01 秒4x 103、一小时 IMU/GPS数据的组合导航结杲图及估计方差P阵图:40.220纬度4039.8116.5116L1II1)I-irurri00.511.522.533.540.01s5x 10经度 ii-1I-tii00.511.522.533.540.01s5x 10咼度bii->7 i孑w-sA.、jp>1Iarrii1171005000.52.53.5x 10组合滤波GPS1.50.01s东向速度-20 L00.51.520.01s北向速度2.53.545x 10200-2

9、02.53.5451.5C1LLffrr0.50.01s天向速度x 10-2 L00.512.53.51.50.01s组合滤波GPS45x 10航向角30020010000.511.522.533.54mm0.01s5X 100-50.51.52.53.50.01s5横滚角X 1050-500.511.522.533.540.01s5x 10P航向角误差x 104度204度20I-一V-IiiTI1-IIiii00.511.522.533.540.01s5x 10-4x 10P俯仰角误差El1i-t1ii00.511.522.533.540.01s5x 10-3x 10P横滚角误差%1r>

10、;l一 .4i-00.511.522.533.5410.500.01s5x 10P东向速度误差10.510.51tiIi00.511.522.533.540.05s5x 10-3x 10P北向速度误差i1iLL-iii-00.511.522.533.540.05s5x 10-3x 10P天向速度误差IiIiLIiii00.511.522.533.54x 100.500.01s5x 10x 10P纬度误差度20-8度20Lr*rIrrLC1E-rr00.511.522.533.540.01s5x 10-8x 10P经度误差口1trITrE e-r*r=00.511.522.533.540.01s

11、x 105P高度误差口iFIsFIIGF=00.51 1.52 2.53 3.5440.20.01sm 0.10x 104、一小时IMU数据的捷联惯导解算结果与组合滤波、GPS输出对比图:50lat度4030度115110i-I-lLE*rrirrr- 一 _rir00.511.522.533.540.01s5x 10lonvii七r一.00.511.522.533.540.01s5x 104x 10heightI| rrrrr1202-20.53.5m 01.50.01s纯惯导解算INS/GPS组合滤波GPS输出5、结杲分析:由滤波结杲图可以看出：(1) 由组合后的速度、位置的 P阵可以看出

12、滤波之后载体的速度和位置比GPS输出的精度咼。(2) 短时间内SINS的精度较高，初始阶段的导航结果基本和GPS组合导航结果重合，1小时后的捷联惯导解算结果很差，纬度、经度、高度均发散。(3) INS/GPS组合滤波的结果和 GPS的输出结果十分近似，因为1小时的导航 GPS的精 度比SINS导航的精度高很多，Kalman滤波器中GPS信号的权重更大。(4)总体看来，SINS/GPS组合滤波的结果优于单独用SINS或GPS导航的结果，起到了协调、超越、冗余的作用，使导航系统更可靠。六、SINS/GPS组合导航程序%组合导航跑/GPSIh 实验 %该程序为 15维状态量， 6维观测量的 kalm

13、an 滤波程序，惯性 /卫星组合松耦合的数学模型clearclcclose all%初始量定义wie = 0.000072921151467; Re = 6378135.072;g = 9.7803267714;e = 1.0 / 298.25;T = 0.01;%IMUdatanumber = 360000; %频率100hz,此程序中 GPS频率100hz数据时间 3600sa = load('imu_1h.dat'); w = a(:,3:5)'*pi/180/3600; % f = a(:,6:8)' % a = load('gps_1h_new

14、.dat'); gps_pos = a(:,3:5);陀螺仪输出的角速率信息单位由° /h 化为 rad/s 三轴比力输出，单位 g%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)

15、; 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;组合导航的初始位置、姿态、速度Vx=gps_v(1,1);Vy=gps_v(1,2);Vz=gps_v(

16、1,3);X_o=zeros(15,1); %X 的初值选为 0 X=zeros(15,1);%Q=diag(50e-6*g)A2,(50e-6*gF2,(50e-6*gF2,(0.1*pi/180/3600)A2,(0.1*pi/180/36 00)人2,(0.1*卩“180/3600)人2,0,0,0,0,0,0,0,0,0);%随机Q=diag(0.008*pi/180/3600)A2,(0.008*pi/180/3600)A2,(0.008*pi/180/3600)A2,(50e- 6*g)A2,(50e-6*g)A2,(50e-6*g)A2,0,0,0,0,0,0,0,0,0);R=

17、diag(0.01)A2,(0.01)A2,(0.01)A2,(0.1)A2,(0.1)A2,(0.15)A2);P=zeros(15);P_k=diag(0.00005*pi/180)A2,(0.00005*pi/180)A2,(0.00005*pi/180)A2,0.00005A2,0. 00005A2,0.00005A2,2A2,2A2,2A2,(0.001*pi/180/3600)A2,(0.001*pi/180/3600)A2,(0.00 1*pi/180/3600)A2,(50e-6*g)A2,(50e-6*g)A2,(50e-6*g)A2);%K=zeros(15,6);Z=ze

18、ros(6,1);I=eye(15);Cnb =cos(gama)*cos(fai)-sin(gama)*sin(theta)*sin(fai),cos(gama)*sin(fai)+sin(gama)*sin(theta)*cos(fai), -sin(gama)*cos(theta);-cos(theta)*sin(fai), cos(theta)*cos(fai), sin(theta); sin(gama)*cos(fai)+cos(gama)*sin(theta)*sin(fai), sin(gama)*sin(fai)-cos(gama)*sin(theta)*cos(fai),

19、cos(gama) * cos(theta); q = cos(fai/2)*cos(theta/2)*cos(gama/2) - sin(fai/2)*sin(theta/2)*sin(gama/2);cos(fai/2)*sin(theta/2)*cos(gama/2)- sin(fai/2)*cos(theta/2)*sin(gama/2);cos(fai/2)*cos(theta/2)*sin(gama/2)+ sin(fai/2)*sin(theta/2)*cos(gama/2);cos(fai/2)*sin(theta/2)*sin(gama/2)+sin(fai/2)*cos(t

20、heta/2)*cos(gama/2);Cnb_s=Cnb;q_s=q;for i=1:1:datanumberRmh = Re * (1.0 - 2.0 * e + 3.0 * e * sin(lat) * sin(lat) + height; Rnh = Re * (1.0 + e * sin(lat) * sin(lat) + height;Wien = 0; wie * cos(lat); wie * sin(lat);Wenn = -Vy / Rmh; Vx / Rnh; Vx * tan(lat) / Rnh;Winn = Wien + Wenn;Winb = Cnb * Winn

21、;for j=1:3gyro(j,1) = w(j,i);加速度信息，单位化为m/s2acc(j,1) = f(j,i)*g; % endangle = (gyro - Winb) * T;fn = Cnb'* acc;difVx = fn(1) + (2.0 * wie * sin(lat) + Vx * tan(lat) / Rnh) * Vy;difVy = fn(2) - (2.0 * wie * sin(lat) + Vx * tan(lat) / Rnh) * Vx;difVz = fn(3) + (2.0 * wie * cos(lat) + Vx / Rnh) * Vx

22、 + Vy * Vy / Rmh -g;Vx = difVx * T + Vx;Vy = difVy * T + Vy;Vz = difVz * T + Vz;lat = lat + Vy * T / Rmh;lon = lon + Vx * T / Rnh / cos(lat); height = height + Vz * T;M = 0, -angle(1), -angle(2), -angle(3); angle(1), 0, angle(3), -angle(2); angle(2), -angle(3), 0, angle(1); angle(3), angle(2), -angl

23、e(1), 0;q = (cos(norm(angle) / 2) * eye(4) + sin(norm(angle) / 2) / norm(angle) * M)* q;q = q / norm(q);Cnb = q(1)*q(1)+q(2)*q(2)-q(3)*q(3)-q(4)*q(4), 2*(q(2)*q(3)+q(1)*q(4), 2*(q(2)*q(4)-q(1)*q(3);2*(q(2)*q(3)-q(1)*q(4), q(1)*q(1)-q(2)*q(2)+q(3)*q(3)-q(4)*q(4), 2*(q(3)*q(4)+q(1)*q(2);2*(q(2)*q(4)+q

24、(1)*q(3), 2*(q(3)*q(4)-q(1)*q(2), q(1)*q(1)-q(2)*q(2)-q(3)*q(3)+q(4)*q(4);Rmh = Re * (1.0 - 2.0 * e + 3.0 * e * sin(lat) * sin(lat) + height;以上为纯惯导解算-(wie*cos(lat)+v(i,1)/(Rnh)000 -v(i,2)/(Rmh)1/(Rnh)Rnh = Re * (1.0 + e * sin(lat) * sin(lat) + height; % %F1=0 wie*sin(lat)+v(i,1)*tan(lat)/(Rnh) -1/(R

25、mh) 0 0 0;-(wie*sin(lat)+v(i,1)*tan(lat)/(Rnh)-wie*sin(lat) 0 0;wie*cos(lat)+v(i,1)/(Rnh) v(i,2)/(Rmh) 0 tan(lat)/(Rnh) 0 0 wie*cos(lat)+v(i,2)*sec(lat)*sec(lat)/(Rnh) 0 0;0 -fn(3) fn(2) v(i,2)*tan(lat)/(Rmh)-v(i,3)/(Rmh) 2*wie*sin(lat)+v(i,1)*tan(lat)/(Rnh) -(2*wie*cos(lat)+v(i,1)/(Rnh) (2*wie*cos(

26、lat)*v(i,2)+v(i,1)*v(i,2)*sec(lat)*sec(lat)/(Rnh)+2*wie*sin(lat)*v(i,3) 0 0;fn(3)0-fn(1)-2*(wie*sin(lat)+v(i,1)*tan(lat)/(Rnh)-v(i,3)/(Rmh)-v(i,2)/(Rmh)-(2*wie*cos(lat)+v(i,1)*sec(lat)*sec(lat)/(Rnh)*v(i,1) 0 0;-fn(2)fn(1)02*(wie*cos(lat)+v(i,1)/(Rnh)2*v(i,2)/(Rmh)0-2*wie*sin(lat)*v(i,1) 0 0;0 0 00

27、1/(Rmh) 00 0 0;000sec(lat)/(Rnh)00v(i,1)*sec(lat)*tan(lat)/(Rnh) 0 0;0 0 00 0 10 0 0;G=Cnb',zeros(3);zeros(3),Cnb'zeros(9,6);H=zeros(3),eye(3),zeros(3),zeros(3,6); zeros(3),zeros(3),diag(Rmh,Rnh*cos(lat),1),zeros(3,6); % 量测阵F2=-Cnb',zeros(3);zeros(3),Cnb' zeros(3),zeros(3);以上为 kalman

28、 滤波模型参数 离散化F=F1,F2;zeros(6,15); %F = F * T;%temp1 = eye(15); disF = eye(15);for j = 1:10temp1 = F * temp1 / j; disF = disF + temp1;endtemp1 = Q * T;disQ = temp1;量测量为纯惯导与for j = 2:11temp2 = F * temp1;temp1 = (temp2 + temp2')/j; disQ = disQ + temp1;endZ(1) = Vx - gps_v(i,1); % Z(2) = Vy - gps_v(i,

29、2); Z(3) = Vz - gps_v(i,3);Z(4) = (lat - gps_pos(i,1) * Rmh; % Z(5) = (lon - gps_pos(i,2) * Rnh * cos(lat); Z(6) = height - gps_pos(i,3);X = disF * X_o; %kalmanP = disF * P_k * disF'+ disQ;K = P * H'/( H * P * H'+ R); X_k = X + K * (Z - H * X); P_k = (I - K * H) * P; x_kf(i,1) = X_k(1)/p

30、i*180; % x_kf(i,2) = X_k(2)/pi*180; x_kf(i,3) = X_k(3)/pi*180; x_kf(i,4) = X_k(4); % x_kf(i,5) = X_k(5); x_kf(i,6) = X_k(6);x_kf(i,7) = X_k(7); % x_kf(i,8) = X_k(8);x_kf(i,9) = X_k(9); x_kf(i,10) = X_k(10)/pi*180*3600; % x_kf(i,11) = X_k(11)/pi*180*3600; x_kf(i,12) = X_k(12)/pi*180*3600;x_kf(i,13) =

31、 X_k(13)*10A6/g;%x_kf(i,14) = X_k(14)*10A6/g;x_kf(i,15) = X_k(15)*10A6/g; p_kf(i,1) = sqrt(abs(P_k(1,1)/pi*180; p_kf(i,2) = sqrt(abs(P_k(2,2)/pi*180;GPS的速度差、位置差纬经度化为位移，单位 滤波五个公式平台误差角速度误差位置误差陀螺随机常值漂移 , 单位° /h加计随机常值偏置，单位 ugp_kf(i,3) = sqrt(abs(P_k(3,3)/pi*180;p_kf(i,4) = sqrt(abs(P_k(4,4);p_kf(i,

32、5) = sqrt(abs(P_k(5,5);p_kf(i,6) = sqrt(abs(P_k(6,6);p_kf(i,7) = sqrt(abs(P_k(7,7);p_kf(i,8) = sqrt(abs(P_k(8,8);p_kf(i,9) = sqrt(abs(P_k(9,9);p_kf(i,10) = sqrt(abs(P_k(10,10)/pi*180*3600; p_kf(i,11) = sqrt(abs(P_k(11,11)/pi*180*3600; p_kf(i,12) = sqrt(abs(P_k(12,12)/pi*180*3600;p_kf(i,13) = sqrt(ab

33、s(P_k(13,13)*10A6/g;p_kf(i,14) = sqrt(abs(P_k(14,14)*10A6/g;p_kf(i,15) = sqrt(abs(P_k(15,15)*10A6/g;Vx = Vx - X_k(4);%速度校正Vy = Vy - X_k(5);Vz = Vz - X_k(6);v(i,:) = Vx, Vy, Vz;lat = lat - X_k(7);%位置校正lon = lon - X_k(8);height = height - X_k(9);pos(i,:) = lat, lon, height;Atheta = X_k(1);%kalman滤波估计得

34、出的失准角thetaAgama = X_k(2);%kalman滤波估计得出的失准角gamaAfai = X_k(3);%kalman滤波估计得出的失准角faiCtn = 1, Afai, -Agama;-Afai, 1, Atheta;Agama, -Atheta, 1;Cnb = Cnb*Ctn;%更新姿态阵fai = atan(-Cnb(2,1) / Cnb(2,2);theta = asin(Cnb(2,3);gama = atan(-Cnb(1,3) / Cnb(3,3);if (Cnb(2,2) < 0)fai = fai + pi;elseif (fai < 0)fa

35、i = fai + 2*pi;endif (Cnb(3,3) < 0)if(gama > 0)gama = gama - pi;elsegama = gama + pi;endendatti(i,:) = fai/pi*180, theta/pi*180, gama/pi*180;q(2) = sqrt(abs(1 + Cnb(1,1) - Cnb(2,2) - Cnb(3,3) / 2; q(3) = sqrt(abs(1 - Cnb(1,1) + Cnb(2,2) - Cnb(3,3) / 2; q(4) = sqrt(abs(1 - Cnb(1,1) - Cnb(2,2) +

36、 Cnb(3,3) / 2; q(1) = sqrt(abs(1 - q(2) * q(2) - q(3) * q(3) - q(4) * q(4); if (Cnb(2,3) < Cnb(3,2)q(2) = - q(2);endif (Cnb(3,1) < Cnb(1,3)q(3) = - q(3);endif (Cnb(1,2) < Cnb(2,1)q(4) H , q(4)八endxlko9) H aXIOHXKiend%接函 % Hld.afan umber 八figuressubpof(3l)八POXLPOS (二)*180、PL5Lgpslpos (二)*180

37、、PL-b-)E_e (->w-)><一abe_(ool s-=y_abe_ (- subpof(312)八 p-of(LPOS(J'2)*180、PL5Lgpslpos(J'2)*180、PL-b-)E_e (-Bw-)><一abe_(ool s-=y_abe_ (- subpof(313)八 p_of(LPOSC3)mLgpslposc3)cr) 巻e (-Mw-)xabe_(-0Q s-=y_abe_(-m)_egend(- taM爺藩-GPS)figure(2)subpof(3l)八POXLV(二)<J 込 PSIV(二)CT) E_

38、e (- 杀可Bw-)xabe_(oos-)y_abe_(-m、s-= subpof(312)八 poXLVC-r-LgpsIVC-b-) 巻e (- 斗可Bw-)xabe_(-o.os-)y_abe_(-m、s-= subpof(313)八 poXLVCWr-LgpsIvcwb-) E_e (- 刃可Bw-)xabe_(oos-)y_abe_(-m、s-= _egend(- taM爺藩-GPS) figure(3) subpof(3l)八 poXLaf三二) E_e (-w可>-)><一abe_(oos-=y_abe_ (- ° -)八 subpof(312)八

39、poXLaf三J'2)E_e (- -)xabe_(oos-)y_abe_ (- ° -)八subpof(313)八 poXLaf三J'3) E_e (- -)xabe_(oos-)y_abe_ (- ° -)八figure(4)subpof(3l)八 p_of(Lplkf(二) E_e(-Pw可>沛m-)x_abe_(oos-=y_abe三w-= 亠6度');度');度');度');度/ 小时 ');度/ 小时 ');度/ 小时 ');subplot(312); plot(t,p_kf(:,2) title('P 俯仰角误差 ');xlabel('0.01s');ylabel