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

1、仪器静与光社程学院»1 OF mext SCIENCE (OPKbraONICS ENGHG惯性导航技术综合实验实验五惯性基组合导航及应用技术实验惯性 /卫星组合导航系统车载实验实验目的 掌握捷联惯导 /GPS 组合导航系统的构成和基本工作原理；掌握采用卡尔曼滤波方法进行捷联惯导/GPS组合的基本原理;掌握捷联惯导 /GPS 组合导航系统静态性能； 掌握动态情况下捷联惯导 /GPS 组合导航系统的性能。二、实验内容复习卡尔曼滤波的基本原理（参考卡尔曼滤波与组合导航原理第二、五章） 复习捷联惯导 /GPS 组合导航系统的基本工作原理（参考以光衢编著的惯性导航原理 第七章）；三、实验系统

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

3、统经过5分钟初始对准之后，进入导航状态; 移动实验车，按设计实验路线行驶; 利用监控计算机中的导航软件进行导航解算，并显示导航结果。五、实验结果及分析（一）理论推导捷联惯导短时段（1分钟）位置误差，并用 1分钟惯导实验数据验证。1、分钟惯导位置误差理论推导:短时段内（t<5min），忽略地球自转ie0，运动轨迹近似为平面1/ R 0，此时的位置误差分析可简化为:(1)加速度计零偏引起的位置误差:Xi£0.8802 m22、可得失准角0引起的误差:陀螺漂移引起的误差:1min后的位置误差值x分钟惯导实验数据验证结果:X3X10t2.3g tX20.9218 m0.0137 mx3

4、1.8157m（1）纯惯导解算1min的位置及位置误差图:Ion121.365121.36121.355121.35121.3451000200030000.01s4000纯惯导解算 GPS理论真值50006000北向位移误差0.01s东向位移误差(2)纯惯导解算1min的速度及速度误差图:Vy8060m 40 m201000200030000.01s-0.40.1-0.1纯惯导解算GPS理论真值400050006000Vx误差-0.1-0.31000200040005000600030000.01s Vy误差m -0.200.05-0.051000200040005000600030000.

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

6、11980.9991attitude 二219.7700-0.93180.48282、5minIMU数据的Kalman滤波精对准结果:220219.500.511.50.01 秒俯仰角22.534x 10>1-LL1-rrrr100.511.522.530.01 秒4x 10横滚角LL4_ rr1r100.511.522.530.01 秒4x 10-0.8度-0.9-10.8度0.60.4 X: 2.797e+04Y: 220fai = 219.9744642380873* pi/180; theta = -0.895865732956914* pi/180; gama = 0.6400

7、89448357591* pi/180;5x 10-4北向速度误差-2 L00.52x 10 -北匕向速度误差方差0123O.。1 秒X 1041 20.01 秒30.01 秒44x 10秒米秒米x 10 -东向速度误差方差0.5341 20.01 秒02x 10x 10-2北向失准角度0-3x 103401 20.01 秒东向失准角2.53401 20.01 秒x 10-3东向失准角方差x 10-3北向失准角方差x 10x 10x 101-100度34-31 20.01 秒x 105-50202.5341 20.01 秒x 103.2度32.8天向失准角0-5x 10x 10-3天向失准角方

8、差1 20.01 秒34x 101 20.01 秒34x 103、一小时IMU/GPS数据的组合导航结杲图及估计方差P阵图:40.2hF4r1100.511.520.01s经度2.533.54x 105卜1Ar1r11r1100.511.520.01s高度2.533.54x 105L_jr-旨1*E rrhr_1r11r-1v-100.511.522.533.54纬度4039.8117116.511610000.01s组合滤波GPSm 50x 1020东向速度- sm-20frffifj00.511.522.533.540.01s5X 10北向速度2051 1Ttm 0''f亠

9、VW-20rrrrr00.511.522.533.540 -50.01s天向速度X 102s m0-2 L03001000.511.520.01s2.5组合滤波GPS3.545X 10航向角20000.511.52.533.520.01s45X 10俯仰角50-5 I00.511.520.01s2.533.545X 105-5横滚角00.511.52.533.520.01s450x104x 10rrt1J1 1 '00.511.522.533.540.01sX 105-4X 10P俯仰角误差fcS'1rrr111100.511.522.533.540.01s5X 10-3X 1

10、0P横滚角误差X r-ST.r1.F00.511.522.533.54P航向角误差2度04度201度0.500.01s5-4x 100.51p东向速度误差0.511.52.533.520.05s45x 10-3x 101P北向速度误差0.50.511.52.533.520.05s45x 10-3x 101P天向速度误差0.50.511.52.533.520.01s4x 10 5-3x 10x 10c* rIJLrE r11*r00.511.522.533.540.01sx 10-8x 10P经度误差rLETr00.511.522.533.540.01s5x 10P高度误差t1.!LLEL-rT

11、Itr00.511.522.533.54P纬度误差0400.2m 0.100.01sx105-8GPS输出对比图:4、一小时IMU数据的捷联惯导解算结果与组合滤波、50LLILLIrr1rrnT00.511.522.533.540.01s5x 10IonEL1LEDLntrJrrDTE00.511.522.533.540.01s5x 104x 104heightrr1rrDrB00.511.522 53354lat度4030120度1151102-20.01s05m 0纯惯导解算INS/GPS组合滤建GPS输出5、结果分析:由滤波结果图可以看出:(1)由组合后的速度、位置的 P阵可以看出滤波之

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

13、的数学模型clear clc close all%初始量定义wie = ;Re%IMU频率100hz,此程序中 GPS频率lOOhzdatanumber = 360000;数据时间 3600sa = load('');w = a(:,3:5)'*pi/180/3600;陀螺仪输出的角速率信息单位由°/h 化为 rad/sf = a(:,6:8)'三轴比力输出，单位 ga = load('');gps_pos = a(:,3:5);%GPS输出的纬度、经度、高度信息纬经单位化为弧度gps_pos(:,1:2) = gps_pos(:,1:

14、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 = *pi/180;组合导航的初始位置、姿态、速度lon =*pi/180;height =;fai = *pi

15、/180;theta = *pi/180;gama = *pi/180;Vx=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)A2,(50e-6*g)A2,(50e-6*g)A2,* 卩“180/3600)人2,* pi/180/3600)A2,*p随机i/180/3600)A2,0,0,0,0,0,0,0,0,0);%Q=diag(* 卩“180/3600)人2,* 卩“180/3600)人2,* pi/180/3600)A2,(50e-6*g)A2

16、,(50e-6*g)A2 ,(50e-6*g)A2,0,0,0,0,0,0,0,0,0);R=diag(F2,A2,A2,A2,A2,A2);P=zeros(15);P_k=diag(* pi/180)人2,* 卩“180)人2,* 卩“180)人2,人2,人2,人2,2人2,2人2,2人2,* 卩“180/3600)人2,*p i/180/3600)A2,* pi/180/3600)A2,(50e-6*g)A2,(50e-6*g)A2,(50e-6*g)A2);%K=zeros(15,6);Z=zeros(6,1);CnbI=eye(15);cos(gama)*cos(fai)-sin(ga

17、ma)*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), cos(gama) * cos(theta);q = cos(fai/2)*cos(theta/2)

18、*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(theta/2)*cos(gama/2);Cnb_s=Cnb;q_s=q;for i=1:1:datan

19、umberRmh = Re * - * e + * e * sin(lat) * sin(lat) + height;Rnh = Re * + 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;for j=1:3gyro(j,1) = w(j,i);acc(j,1) = f(j,i)*g; %加速度信息，单位化为m/sA2enda

20、ngle = (gyro - Winb) * T;fn = Cnb'* acc;difVx = fn(1) + * wie * sin(lat) + Vx * tan(lat) / Rnh) * Vy;difVy = fn(2) - * wie * sin(lat) + Vx * tan(lat) / Rnh) * Vx;difVz = fn(3) + * wie * cos(lat) + Vx / Rnh) * Vx + Vy * Vy / Rmh -g;Vx = difVx * T + Vx;Vy = difVy * T + Vy;Vz = difVz * T + Vz;lat =

21、 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), -angle(1), 0;q = (cos(norm(angle) / 2) * eye(4) + sin(norm(angle) / 2) / norm(angle) * M)q;q =

22、 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(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 * - * e + * e

23、* sin(lat) * sin(lat) + height;Rnh = Re * + e * sin(lat) * sin(lat) + height; %以上为纯惯导解算%-(wie*cos(lat)+v(i,1)/(Rnh)F1=0 wie*sin(lat)+v(i,1)*tan(lat)/(Rnh) -1/(Rmh)0 0 0;-(wie*sin(lat)+v(i,1)*tan(lat)/(Rnh)0 -v(i,2)/(Rmh)1/(Rnh)-wie*sin(lat) 0 0;wie*cos(lat)+v(i,1)/(Rnh)v(i,2)/(Rmh)tan(lat)/(Rnh)wie*

24、cos(lat)+v(i,2)*sec(lat)*sec(lat)/(Rnh) 0 0;fn(2)v(i,2)*tan(lat)/(Rmh)-v(i,3)/(Rmh)2*wie*sin(lat)+v(i,1)*tan(lat)/(Rnh)-fn(3)-(2*wie*cos(lat)+v(i,1)/(Rnh) (2*wie*cos(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)-fn(1)-2*(wie*sin(lat)+v(i,1)*tan(lat)/(Rnh)-v(i,3)/

25、(Rmh)-v(i,2)/(Rmh)-(2*wie*cos(lat)+v(i,1)*sec(lat)*sec(lat)/(Rnh)*v(i,1) 0 0;fn(1)2*v(i,2)/(Rmh)-fn(2)2*(wie*cos(lat)+v(i,1)/(Rnh)-2*wie*sin(lat)*v(i,1) 0 0;0 1/(Rmh) 00 0 0;sec(lat)/(Rnh)v(i,1)*sec(lat)*tan(lat)/(Rnh) 0 0;G=Cnb',zeros(3);zeros(3),Cnb'zeros(9,6);H=zeros(3),eye(3),zeros(3),ze

26、ros(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);F=F1,F2;zeros(6,15); %以上为 kalman 滤波模型参数离散化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:

27、11temp2 = F * temp1;temp1 = (temp2 + temp2')/j;disQ = disQ + temp1;endZ(1) = Vx - gps_v(i,1);%量测量为纯惯导与 GPS的速度差、位置差Z(2) = Vy - gps_v(i,2);Z(3) = Vz - gps_v(i,3);Z(4) = (lat - gps_pos(i,1) * Rmh;纬经度化为位移，单位 mZ(5) = (lon - gps_pos(i,2) * Rnh * cos(lat);Z(6) = height - gps_pos(i,3);X = disF * X_o;%ka

28、lman滤波五个公式P = 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)/pi*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

29、,8) = X_k(8);x_kf(i,9) = X_k(9);x_kf(i,10) = X_k(10)/pi*180*3600; %陀螺随机常值漂移 , 单位° /hx_kf(i,11) = X_k(11)/pi*180*3600;x_kf(i,12) = X_k(12)/pi*180*3600;x_kf(i,13) = X_k(13)*106/g;%加计随机常值偏置，单位 ugx_kf(i,14) = X_k(14)*10A6/g;x_kf(i,15) = X_k(15)*106/g;p_kf(i,1) = sqrt(abs(P_k(1,1)/pi*180;p_kf(i,2) =

30、 sqrt(abs(P_k(2,2)/pi*180;p_kf(i,3) = sqrt(abs(P_k(3,3)/pi*180;p_kf(i,4) = sqrt(abs(P_k(4,4);p_kf(i,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) =

31、 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(abs(P_k(13,13)*10A6/g;p_kf(i,14) = sqrt(abs(P_k(14,14)*106/g;p_kf(i,15) = sqrt(abs(P_k(15,15)*106/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 =

32、 lon - X_k(8);height = height - X_k(9);pos(i,:) = lat, lon, height;Atheta = X_k(1);%kalman滤波估计得出的失准角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(

33、2,3);gama = atan(-Cnb(1,3) / Cnb(3,3);if (Cnb(2,2) < 0)fai = fai + pi;elseif (fai < 0)fai = fai + 2*pi;end if (Cnb(3,3) < 0)if(gama > 0)gama = gama - pi;elsegama = gama + pi;end end atti(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

34、(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) + 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);end if (Cnb(3,1) < Cnb(1,3)q(3) = - q(3);endif(cnbcl2) Acnb(23)q(4) Hq(4=endxku 込)HaXIOH

35、XKendH 秸 fanumbecfiguressubp-of(3=)八p一of(LPOS(二)*180、PL-r-JygpsIPOS(二)*180、PL-b-)E_e(- -=x_abe_(=)迄abe三subp一Of(32xp_of(LPOSC2)*80、PL-rngpslposc2)*80、PL-b-)巻e (- Bw -=x_abe_(=)迄abe_(-subp一Of(313xp_of(LPOSC3)mLgpslposc3)b-)巻e (- MW -=x_abe_(=)迄abe_(-m-=_egend(- laM爺藩-GPS)figured)subp-of(3=)八p豆(Lv (二)x

36、ngpsv (二二 b-)E_e(- 杀可审滝-=x_abe_(£y_abe_(-m、s-=subp一Of(32xp豆(LVC2)xLgpsvc2)&-)巻e(- 亠昌审滝-=x_abe_(£y_abe_(-m/s-=subp一Of(313xp豆(LVC3)xLgpsvc3)&-)E_e(- 刃可审滝-=x_abe_(£y_abe_(-m、s-=_egend(- laM爺藩-GPS)figure(3)subp一of(3=)-p豆(Lsi(二)巻e(- -=x_abe_(=)迄abe_(-subp一Of(32yp_of(Lsic2)E_e(- -=x_abe_(=)迄abe_(-subp一Of(313xp豆(Lsic3)良理 -)-x_abe_(£y_abe_(-figure(4)subp一of(3=yp豆(Lpkr(二)subp一Of(32yp豆(Lpkrc2)巻e(-psubp-of(313)八p豆(Lpkrc3)巻e(-pfigure(5)subp互(3= =-=x_abe_(