北航慣性導(dǎo)航綜合實(shí)驗(yàn)五實(shí)驗(yàn)報告

1、慣性導(dǎo)航技術(shù)綜合實(shí)驗(yàn)實(shí)驗(yàn)五 慣性基組合導(dǎo)航及應(yīng)用技術(shù)實(shí)驗(yàn) 慣性/衛(wèi)星組合導(dǎo)航系統(tǒng)車載實(shí)驗(yàn)一、 實(shí)驗(yàn)?zāi)康恼莆战萋?lián)慣導(dǎo)/gps組合導(dǎo)航系統(tǒng)的構(gòu)成和基本工作原理； 掌握采用卡爾曼濾波方法進(jìn)行捷聯(lián)慣導(dǎo)/gps組合的基本原理；掌握捷聯(lián)慣導(dǎo) /gps組合導(dǎo)航系統(tǒng)靜態(tài)性能；掌握動態(tài)情況下捷聯(lián)慣導(dǎo) /gps組合導(dǎo)航系統(tǒng)的性能。二、實(shí)驗(yàn)內(nèi)容復(fù)習(xí)卡爾曼濾波的基本原理（參考卡爾曼濾波與組合導(dǎo)航原理第二、五章）； 復(fù)習(xí)捷聯(lián)慣導(dǎo)/gps組合導(dǎo)航系統(tǒng)的基本工作原理（參考以光衢編著的慣性導(dǎo)航原理第七章）；三、實(shí)驗(yàn)系統(tǒng)組成捷聯(lián)慣導(dǎo)/gps組合導(dǎo)航實(shí)驗(yàn)系統(tǒng)一套； 監(jiān)控計算機(jī)一臺。差分 gps接收機(jī)一套；實(shí)驗(yàn)車一輛；車載大理石平

2、臺；車載電源系統(tǒng)。四、實(shí)驗(yàn)內(nèi)容1） 實(shí)驗(yàn)準(zhǔn)備 將imu緊固在車載大理石減振平臺上，確認(rèn)imu的安裝基準(zhǔn)面緊靠實(shí)驗(yàn)平臺； 將imu與導(dǎo)航計算機(jī)、導(dǎo)航計算機(jī)與車載電源、導(dǎo)航計算機(jī)與監(jiān)控計算機(jī)、gps接收機(jī)與導(dǎo)航計算機(jī)、gps天線與gps接收機(jī)、gps接收機(jī)與gps電池之間的連接線正確連接； 打開gps接收機(jī)電源，確認(rèn)可以接收到4顆以上衛(wèi)星； 打開電源，啟動實(shí)驗(yàn)系統(tǒng)。2） 捷聯(lián)慣導(dǎo)/gps組合導(dǎo)航實(shí)驗(yàn) 進(jìn)入捷聯(lián)慣導(dǎo)初始對準(zhǔn)狀態(tài)，記錄imu的原始輸出，注意5分鐘內(nèi)嚴(yán)禁移動實(shí)驗(yàn)車和imu； 實(shí)驗(yàn)系統(tǒng)經(jīng)過5分鐘初始對準(zhǔn)之后，進(jìn)入導(dǎo)航狀態(tài)； 移動實(shí)驗(yàn)車，按設(shè)計實(shí)驗(yàn)路線行駛； 利用監(jiān)控計算機(jī)中的導(dǎo)航軟件進(jìn)行導(dǎo)

3、航解算，并顯示導(dǎo)航結(jié)果。五、 實(shí)驗(yàn)結(jié)果及分析(一) 理論推導(dǎo)捷聯(lián)慣導(dǎo)短時段（1分鐘）位置誤差，并用1分鐘慣導(dǎo)實(shí)驗(yàn)數(shù)據(jù)驗(yàn)證。1、一分鐘慣導(dǎo)位置誤差理論推導(dǎo)：短時段內(nèi)（t5min），忽略地球自轉(zhuǎn)，運(yùn)動軌跡近似為平面，此時的位置誤差分析可簡化為：（1） 加速度計零偏引起的位置誤差：m（2） 失準(zhǔn)角引起的誤差：m（3） 陀螺漂移引起的誤差：m可得1min后的位置誤差值2、一分鐘慣導(dǎo)實(shí)驗(yàn)數(shù)據(jù)驗(yàn)證結(jié)果：（1）純慣導(dǎo)解算1min的位置及位置誤差圖：（2）純慣導(dǎo)解算1min的速度及速度誤差圖：實(shí)驗(yàn)結(jié)果分析：純慣導(dǎo)解算短時間內(nèi)精度很高，1min的慣導(dǎo)解算的北向最大位移誤差-2.668m，東向最大位移誤差-8.2

4、31m，可見實(shí)驗(yàn)數(shù)據(jù)所得位置誤差與理論推導(dǎo)的位置誤差在同一數(shù)量級，結(jié)果不完全相同是因?yàn)槔碚撏茖?dǎo)時做了大量簡化，而且實(shí)驗(yàn)時視gps為真實(shí)值也會帶來誤差；另外，可見1min內(nèi)純慣導(dǎo)解算的東向速度最大誤差-0.2754m/s，北向速度最大誤差-0.08027m/s。(二) 選取imu前5分鐘數(shù)據(jù)進(jìn)行對準(zhǔn)實(shí)驗(yàn)。將初始對準(zhǔn)結(jié)果作為初值完成1小時捷聯(lián)慣導(dǎo)和組合導(dǎo)航解算，對比1小時捷聯(lián)慣導(dǎo)和組合導(dǎo)航結(jié)果。1、5minimu數(shù)據(jù)的解析粗對準(zhǔn)結(jié)果：2、5minimu數(shù)據(jù)的kalman濾波精對準(zhǔn)結(jié)果：3、一小時imu/gps數(shù)據(jù)的組合導(dǎo)航結(jié)果圖及估計方差p陣圖：4、一小時imu數(shù)據(jù)的捷聯(lián)慣導(dǎo)解算結(jié)果與組合濾波、g

5、ps輸出對比圖：5、結(jié)果分析：由濾波結(jié)果圖可以看出：（1） 由組合后的速度、位置的p陣可以看出濾波之后載體的速度和位置比gps輸出的精度高。（2） 短時間內(nèi)sins的精度較高，初始階段的導(dǎo)航結(jié)果基本和gps、組合導(dǎo)航結(jié)果重合，1小時后的捷聯(lián)慣導(dǎo)解算結(jié)果很差，緯度、經(jīng)度、高度均發(fā)散。（3） ins/gps組合濾波的結(jié)果和gps的輸出結(jié)果十分近似，因?yàn)?小時的導(dǎo)航gps的精度比sins導(dǎo)航的精度高很多，kalman濾波器中g(shù)ps信號的權(quán)重更大。（4） 總體看來，sins/gps組合濾波的結(jié)果優(yōu)于單獨(dú)用sins或gps導(dǎo)航的結(jié)果，起到了協(xié)調(diào)、超越、冗余的作用，使導(dǎo)航系統(tǒng)更可靠。六、 sins/gps

6、組合導(dǎo)航程序%ins/gps組合導(dǎo)航跑車1h實(shí)驗(yàn)%該程序?yàn)?5維狀態(tài)量，6維觀測量的kalman濾波程序，慣性/衛(wèi)星組合松耦合的數(shù)學(xué)模型clearclcclose all%初始量定義wie = 0.000072921151467; re= 6378135.072;g = 9.7803267714;e = 1.0 / 298.25;t = 0.01; %imu頻率100hz,此程序中g(shù)ps頻率100hzdatanumber = 360000; %數(shù)據(jù)時間3600s a = load(imu_1h.dat);w = a(:,3:5)*pi/180/3600; %陀螺儀輸出的角速率信息單位由/h化為

7、rad/sf = a(:,6:8); %三軸比力輸出，單位ga = load(gps_1h_new.dat); gps_pos = a(:,3:5); %gps輸出的緯度、經(jīng)度、高度信息gps_pos(:,1:2) = gps_pos(:,1:2)*pi/180; %緯經(jīng)單位化為弧度gps_v = a(:,6:8); %gps輸出的東北天速度信息%捷聯(lián)解算及卡爾曼相關(guān)v=zeros(datanumber,3); %組合后的速度信息atti = zeros(datanumber,3); %組合后的姿態(tài)信息pos = zeros(datanumber,3); %組合后的位置信息gyro=zeros

8、(3,1);acc=zeros(3,1);x_kf = zeros(datanumber,15); p_kf = zeros(datanumber,15);lat = 40.0211142228246*pi/180; %組合導(dǎo)航的初始位置、姿態(tài)、速度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=g

9、ps_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); %隨機(jī)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.

10、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.000052,0.000052,0.000052,22,22,22,(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,(50e-6*g)2); %k=zeros(15,6);z=zeros(6,1);i=eye(15);cnb = cos

11、(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), cos(gama) * cos(theta);q

12、= 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(theta/2)*cos(gama/2)

13、;cnb_s=cnb;q_s=q; for i=1:1:datanumber rmh = 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; for j=1:3 g

14、yro(j,1) = w(j,i); acc(j,1) = f(j,i)*g; %加速度信息，單位化為m/s2 end angle = (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 + vy * vy

15、 / 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), -angle(1),

16、 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

17、(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; rnh = re * (1.0 + e * sin(lat) * sin(lat) + height; %以上為純慣導(dǎo)解算 % f1=0 wie*sin(lat)+v(i,1)*tan(lat)/(rnh) -(wie*cos(lat)+v(i,1)/(rnh) 0 -1/(rmh) 0 0 0 0; -(wie

18、*sin(lat)+v(i,1)*tan(lat)/(rnh) 0 -v(i,2)/(rmh) 1/(rnh) 0 0 -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)/(

19、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) 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) 0 2*(wie*cos(lat)+v(i,1)/(rnh) 2*v(i,2)/(rmh) 0 -2*w

20、ie*sin(lat)*v(i,1) 0 0; 0 0 0 0 1/(rmh) 0 0 0 0; 0 0 0 sec(lat)/(rnh) 0 0 v(i,1)*sec(lat)*tan(lat)/(rnh) 0 0; 0 0 0 0 0 1 0 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),cn

21、b; zeros(3),zeros(3); f=f1,f2; zeros(6,15); %以上為kalman濾波模型參數(shù) f = f * t; %離散化 temp1 = eye(15); disf = eye(15); for j = 1:10 temp1 = f * temp1 / j; disf = disf + temp1; end temp1 = q * t; disq = temp1; for j = 2:11 temp2 = f * temp1; temp1 = (temp2 + temp2)/j; disq = disq + temp1; end z(1) = vx - gps_

22、v(i,1); %量測量為純慣導(dǎo)與gps的速度差、位置差 z(2) = vy - gps_v(i,2); z(3) = vz - gps_v(i,3); z(4) = (lat - gps_pos(i,1) * rmh; %緯經(jīng)度化為位移，單位m z(5) = (lon - gps_pos(i,2) * rnh * cos(lat); z(6) = height - gps_pos(i,3); x = disf * x_o; %kalman濾波五個公式 p = disf * p_k * disf+ disq; k = p * h/( h * p * h+ r); x_k = x + k * (

23、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,8) = x_k(8); x_kf(i,9) = x_k(9); x_kf(i,10) = x_k(10)/pi*180*3600; %陀螺隨機(jī)常值漂移,單

24、位/h x_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; %加計隨機(jī)常值偏置，單位ug x_kf(i,14) = x_k(14)*106/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) = sqrt(abs(p_k(2,2)/pi*180; p_kf(i,3) = sqrt(abs(p_k(3,3)/pi*180; p_kf(i,4) = sqr

25、t(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) = sqrt(abs(p_k(11,11)/pi*180*3600; p_kf(i,12) = sqrt(abs(p_k(12,12)/pi*1

26、80*3600; p_kf(i,13) = sqrt(abs(p_k(13,13)*106/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 = lon - x_k(8); height = height - x_k(9); pos(i,:) = lat, lo

27、n, height; atheta = x_k(1); %kalman濾波估計得出的失準(zhǔn)角theta agama = x_k(2); %kalman濾波估計得出的失準(zhǔn)角gama afai = x_k(3); %kalman濾波估計得出的失準(zhǔn)角fai ctn = 1, afai, -agama; -afai, 1, atheta; agama, -atheta, 1; cnb = cnb*ctn; %更新姿態(tài)陣 fai= atan(-cnb(2,1) / cnb(2,2);theta = asin(cnb(2,3);gama = atan(-cnb(1,3) / cnb(3,3); if (cn

28、b(2,2) 0) fai = fai + pi; elseif (fai 0) fai = fai + 2*pi; end if (cnb(3,3) 0) gama = gama - pi; else gama = 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(3) = sqrt(abs(1 - cnb(1,1) + cnb(2,2) - cnb(3,3) / 2; q(4) = sq

29、rt(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); end if (cnb(1,2) cnb(2,1) q(4) = - q(4); end x_k(1:9) = 0; x_o=x_k; iend%繪圖%t=1:datanumber;figure(1)subplot(311);p

30、lot(t,pos(:,1)*180/pi,r,t,gps_pos(:,1)*180/pi,b)title(緯度);xlabel(0.01s);ylabel();subplot(312);plot(t,pos(:,2)*180/pi,r,t,gps_pos(:,2)*180/pi,b)title(經(jīng)度);xlabel(0.01s);ylabel();subplot(313);plot(t,pos(:,3),r,t,gps_pos(:,3),b)title(高度);xlabel(0.01s);ylabel(m);legend(組合濾波,gps)figure(2)subplot(311);plot

31、(t,v(:,1),r,t,gps_v(:,1),b)title(東向速度);xlabel(0.01s);ylabel(m/s);subplot(312);plot(t,v(:,2),r,t,gps_v(:,2),b)title(北向速度);xlabel(0.01s);ylabel(m/s);subplot(313);plot(t,v(:,3),r,t,gps_v(:,3),b)title(天向速度);xlabel(0.01s);ylabel(m/s);legend(組合濾波,gps)figure(3)subplot(311);plot(t,atti(:,1)title(航向角);xlabel(0.01s);ylabel();subplot(312);plot(t,atti(:,2)title(俯仰角);xlabel(0.01s);ylabel();subplot(313);plot(t,atti(:,3)title(橫滾角);xlabel(0.01s);ylabel();figure(4)subplot(311);plo