1、慣性導(dǎo)航技術(shù)綜合實驗 實驗三 慣性導(dǎo)航綜合實驗 實驗3.1 初始對準(zhǔn)實驗一、實驗?zāi)康慕Y(jié)合已經(jīng)采集并標(biāo)定好的慣性傳感器數(shù)據(jù)進行粗對準(zhǔn)，了解實現(xiàn)對準(zhǔn)的過程；通過比較不同傳感器數(shù)據(jù)的對準(zhǔn)結(jié)果，進一步認(rèn)識慣性傳感器性能在導(dǎo)航系統(tǒng)中的重要地位。為在實際工程設(shè)計中針對不同應(yīng)用需求下采取相應(yīng)的導(dǎo)航系統(tǒng)方案提供依據(jù)。二、實驗內(nèi)容利用加速度計輸出計算得到系統(tǒng)的初始姿態(tài)，利用陀螺輸出信號計算航向角。對比利用不同的慣性傳感器數(shù)據(jù)計算所得的不同結(jié)果。三、實驗系統(tǒng)組成MEMS IMU（或其他類型IMU）、穩(wěn)壓電源、數(shù)據(jù)采集系統(tǒng)與分析系統(tǒng)。四、實驗原理慣導(dǎo)系統(tǒng)在開始進行導(dǎo)航解算之前需要進行初始對準(zhǔn)，水平對準(zhǔn)的本質(zhì)是將重力

2、加速度作為對準(zhǔn)基準(zhǔn)，其對準(zhǔn)精度主要取決于兩個水平加速度計的精度，加速度計的零位輸出將會造成水平對準(zhǔn)誤差；方位對準(zhǔn)最常用的方位是羅經(jīng)對準(zhǔn)，其本質(zhì)是以地球自轉(zhuǎn)角速度作為對準(zhǔn)基準(zhǔn)，影響對準(zhǔn)精度的主要因素是等效東向陀螺漂移。 (1)其中，分別為當(dāng)前時刻的俯仰角和橫滾角計算值。水平對準(zhǔn)誤差和方位對準(zhǔn)誤差如下所示：， (2)五、實驗步驟及結(jié)果1、實驗步驟：采集靜止?fàn)顟B(tài)下水平加速度計輸出，按下式計算其平均值。 (3)其中，為前n個加計輸出均值；為前n-1個加計輸出均值；為當(dāng)前時刻加計輸出值。利用加計平均值來計算系統(tǒng)初始俯仰角和橫滾角 (4)其中，分別為當(dāng)前時刻的俯仰角和橫滾角計算值。2、實驗結(jié)果與分析：2.

3、1、用MIMS IMU的加速度計信息計算（1）俯仰角和橫滾角圖：（2）失準(zhǔn)角：2.2、實驗結(jié)果分析 以上計算是基于MIMS IMU靜止時data2進行的初始對準(zhǔn)，與data2給定的初始姿態(tài)角相差不大。六、源程序clearclcg = 9.7803267714;a=load('E:郭鳳玲chushiduizhundata2.txt');ax=a(:,4)'ay=a(:,5)'az=a(:,6)'%初始值ax0(1)=ax(1)/1000*g; %轉(zhuǎn)化單位，由mg轉(zhuǎn)化為m/s2ay0(1)=ay(1)/1000*g;az0(1)=az(1)/1000*g;t

4、heta(1)=asin(ay(1)/az(1);gama(1)=-asin(ax(1)/az(1);for i=2:120047 ax0(i)=ax0(i-1)+(ax(i)-ax0(i-1)/i; ay0(i)=ay0(i-1)+(ay(i)-ay0(i-1)/i; az0(i)=az0(i-1)+(az(i)-az0(i-1)/i; theta(i)=asin(ay0(i)/az0(i); gama(i)=-asin(ax0(i)/az0(i);enddetfaix=mean(ay0)/g;detfaiy=mean(-ax0)/g;t=1:120047;plot(t,theta,'

5、;r',t,gama,'b')title('俯仰角和橫滾角');ylabel('弧度（rad）');legend('俯仰角','橫滾角')實驗3.2 慣性導(dǎo)航靜態(tài)實驗一、實驗?zāi)康?、掌握捷聯(lián)慣導(dǎo)系統(tǒng)基本工作原理2、掌握捷聯(lián)慣導(dǎo)系統(tǒng)捷聯(lián)解算方法3、了解捷聯(lián)慣導(dǎo)系統(tǒng)誤差傳遞規(guī)律和方程二、實驗原理捷聯(lián)慣性導(dǎo)航系統(tǒng)（SINS）的導(dǎo)航解算流程如圖1所示。在程序初始化部分，主要是獲得SINS的初始姿態(tài)陣、初始位置矩陣以及初值四元數(shù)；并讀取SINS數(shù)據(jù)更新頻率等SINS的工作參數(shù)。圖1 慣性導(dǎo)航原理這里，、分別為當(dāng)?shù)鼐暥?/p>

6、和經(jīng)度、分別為載體航向、俯仰、橫滾角。地理坐標(biāo)系為東-北-天坐標(biāo)系。1姿態(tài)計算姿態(tài)矩陣為： (1) (2) 位置矩陣為： (3) 其中：(4)使用姿態(tài)四元數(shù)來更新姿態(tài)。四元數(shù)微分方程為：(5)簡寫為： (6) 其中：解四元數(shù)微分方程： (7)式中：(8) 其中T為導(dǎo)航解算周期。歸一化四元數(shù)，有更新后的姿態(tài)矩陣： (9)提取姿態(tài)角： (10)2速度計算由下列速度方程進行速度的更新 （11）式中， (12)速度更新 (13)由式（11）求出加速度，則。在實際程序中，為了進一步提高解算的精度，也可以在姿態(tài)陣更新前后分別計算兩次加速度，用梯形法求得速度的更新值。3位置矩陣更新與位置計算 (14)式中：

7、 (17)解上述微分方程使用如下解法： (15)提取經(jīng)緯度： (16)四、主要實驗設(shè)備 捷聯(lián)慣性導(dǎo)航實驗系統(tǒng)一套； 監(jiān)控計算機一臺。五、實驗內(nèi)容及結(jié)果1MIMS IMU系統(tǒng)導(dǎo)航計算 將IMU固定在夾具上，將IMU連同夾具一起靜置于桌面； 調(diào)整穩(wěn)壓電源的輸出電壓為+8V，關(guān)閉電源。連接穩(wěn)壓電源與IMU供電輸入端，連接IMU信號線與USB-232轉(zhuǎn)接線至監(jiān)控計算機； 打開監(jiān)控計算機中的監(jiān)控軟件； 打開捷聯(lián)慣導(dǎo)實驗系統(tǒng)電源，捷聯(lián)慣導(dǎo)實驗系統(tǒng)開始啟動； 保持捷聯(lián)慣性導(dǎo)航系統(tǒng)靜止600秒，并記錄實時輸出數(shù)據(jù)； 停止記錄數(shù)據(jù)，利用捷聯(lián)解算方法計算純慣性導(dǎo)航誤差。MIMS IMU系統(tǒng)純慣導(dǎo)導(dǎo)航結(jié)果：（1）速

8、度誤差（2）姿態(tài)誤差（2）位置誤差2中低精度慣性導(dǎo)航系統(tǒng)導(dǎo)航仿真 由實驗老師給定一組中低精度IMU的靜態(tài)IMU采樣數(shù)據(jù)，初始姿態(tài)由數(shù)據(jù)中前300秒的加速度計采樣計算得到，初始航向由GPS雙天線數(shù)據(jù)給出； 利用捷聯(lián)解算方法計算純慣性導(dǎo)航誤差。中低精度慣性導(dǎo)航系統(tǒng)導(dǎo)航仿真結(jié)果：（1） 位置誤差：（2） 姿態(tài)誤差：（3） 速度誤差：3導(dǎo)航仿真結(jié)果分析1，MIMS IMU系統(tǒng)的經(jīng)度誤差、fai誤差、theta誤差、Vx誤差大，在調(diào)試程序的過程中，選取安裝誤差陣變化很微小的情況下，姿態(tài)誤差，速度誤差變化很大，說明準(zhǔn)確的安裝誤差補償陣很重要，上面MIMS IMU的圖形是在沒有補償?shù)那闆r下得到的圖形，圖形未

9、列出。上面圖形是中精度導(dǎo)航系統(tǒng)的誤差小，而中低精度導(dǎo)航系統(tǒng)的緯度誤差、gama誤差、Vy誤差比MIMS IMU的誤差小。六，實驗源程序1，MIMS的靜態(tài)捷聯(lián)結(jié)算clear all ;clc ;Q=load('E:慣性器件綜合實驗我的作業(yè)初始對準(zhǔn)data2.txt'); %慣性信息 Fbb Wib_bb%下面為捷聯(lián)解算初始值計算re=6378393;%地球半徑e=1/298.25;%地球扁率g0 = 9.7803267714;wie=15.04107*pi/180/3600;%地球自轉(zhuǎn)角速率vv=0;0;0;%初始速度latitude0=116.3438*pi/180;%初始經(jīng)度

10、longitude0=39.977584*pi/180;%初始緯度% h0=0;%初始高度Fibb=Q(:,4:6)*pi/3600/180;Wibb=Q(:,1:3)/1000*g0;fai0 = 87.16*pi/180; % 航向theta0 =-0.158*pi/180; % 俯仰gama0 =-0.325*pi/180; %陀螺儀安裝誤差補償陣Kg=0.9933 0.0013 -0.0020 ; 0.0033 0.9950 -0.0026 ; -0.0010 0.0022 0.9912 ;kg0=0.0201; -0.0537; 0.0810;%加速度計誤差補償Ka=0.9987 0

11、.0001 -0.0020 ; -0.0001 0.9988 0.0004 ; 0.0033 -0.0015 0.9988 ;ka0= 0.0018; 0.0014; 0.0009;%求初始姿態(tài)矩陣%Ctb = cos(gama0)*cos(fai0)-sin(gama0)*sin(theta0)*sin(fai0), cos(gama0)*sin(fai0)+sin(gama0)*sin(theta0)*cos(fai0), -sin(gama0)*cos(theta0); -cos(theta0)*sin(fai0), cos(theta0)*cos(fai0), sin(theta0);

12、 sin(gama0)*cos(fai0)+cos(gama0)*sin(theta0)*sin(fai0), sin(gama0)*sin(fai0)-cos(gama0)*sin(theta0)*cos(fai0), cos(gama0) * cos(theta0);cbt=Ctb'T=cbt;%設(shè)置四元素的初始值if T(3,2)>T(2,3) q1=0.5*sqrt(1+T(1,1)-T(2,2)-T(3,3);else q1=-(0.5*sqrt(1+T(1,1)-T(2,2)-T(3,3);endif T(1,3)>T(3,1) q2=0.5*sqrt(1-T(

13、1,1)+T(2,2)-T(3,3); else q2=-(0.5*sqrt(1-T(1,1)+T(2,2)-T(3,3);endif T(2,1)>T(1,2) q3=0.5*sqrt(1-T(1,1)-T(2,2)+T(3,3); else q3=-(0.5*sqrt(1-T(1,1)-T(2,2)+T(3,3);endq0=sqrt(1-q12-q22-q32);%位置陣的初始值。 Cte=-sin(latitude0),cos(latitude0),0; -sin(longitude0)*cos(latitude0),-sin(longitude0)*sin(latitude0)

14、,cos(longitude0); cos(longitude0)*cos(latitude0),cos(longitude0)*sin(latitude0),sin(longitude0); rn=6356803; rm=6378254;% %2.循環(huán)解算 delt=0.005; Ti=0.005;%初始%速度vx=zeros(120047,1);vy=zeros(120047,1);vz=zeros(120047,1);%姿態(tài)theta2=zeros(120047,1);%俯仰psi2=zeros(120047,1);%偏航gamma2=zeros(120047,1);%滾轉(zhuǎn)%位置經(jīng)緯度l

15、2=zeros(120047,1);%經(jīng)度a2=zeros(120047,1);%緯度 % for i=1:120047 fb=Fibb(i,:); fb=fb' fb=Ka*fb+ka0; wib_b=Wibb(i,:); wib_b=wib_b' wib_b=Kg* wib_b+kg0; fp=T*fb;%轉(zhuǎn)換至當(dāng)?shù)氐乩碜鴺?biāo)系 %g更新 g=g0*(1+0.0052884*sin(latitude0)*sin(latitude0)-0.0000059*sin(2*latitude0)*sin(2*latitude0); difVx = fp(1) + (2.0 * wie

16、* sin(latitude0) + vv(1) * tan(latitude0) / rn) * vv(2); difVy = fp(2) - (2.0 * wie * sin(latitude0) + vv(1)* tan(latitude0) / rn) * vv(1); dvv=fp-0;0;g+ difVx ; difVy ;0; vv=vv+dvv*delt;%積分法 wett=-vv(2)/(rn);vv(1)/(rm);vv(1)*tan(latitude0)/(rm); Wiet=0;wie*cos(latitude0);wie*sin(latitude0); wtbb=wi

17、b_b-inv(T)*(wett+Wiet); %下面進行位置矩陣修正 dc=Cte*0,-wett(3),wett(2); wett(3),0,-wett(1); -wett(2),wett(1),0; Cte=Cte+dc*delt; %位置計算 latitude0=asin(Cte(3,3); if Cte(3,1)>0 longitude0=atan(Cte(3,2)/Cte(3,1); elseif Cte(3,2)>0%等同于書上條件 longitude0=atan(Cte(3,2)/Cte(3,1)+pi; else longitude0=atan(Cte(3,2)/

18、Cte(3,1)-pi; endQ=q0;q1;q2;q3;dQ=1/2*0,-wtbb(1),-wtbb(2),-wtbb(3); wtbb(1),0,wtbb(3),-wtbb(2); wtbb(2),-wtbb(3),0,wtbb(1); wtbb(3),wtbb(2),-wtbb(1),0*Q;Q=Q+dQ*delt;qq=(sqrt(Q(1)2+Q(2)2+Q(3)2+Q(4)2);q0=Q(1)/qq;q1=Q(2)/qq;q2=Q(3)/qq;q3=Q(4)/qq;%求捷聯(lián)矩陣修正四元法T=q02+q12-q22-q32,2*(q1*q2-q0*q3),2*(q1*q3+q0*q

19、2); 2*(q1*q2+q0*q3),q02-q12+q22-q32,2*(q2*q3-q0*q1); 2*(q1*q3-q0*q2),2*(q2*q3+q0*q1),q02-q12-q22+q32;%下面根據(jù)捷聯(lián)矩陣T計算姿態(tài)角yaw, y，rolltheta0=asin(T(3,2);yawm=atan(-T(1,2)/T(2,2);rollm=atan(-T(3,1)/T(3,3);% % if T(2,2)<0 fai0=yawm+pi;else if yawm>0 fai0=yawm;%-1/2*pi; else fai0=yawm; endend% if T(3,3)

20、>0 gama0=rollm; else if rollm<0 gama0=rollm+pi; else gama0=rollm-pi; end end%收集信息%速度vx(i)=vv(1);vy(i)=vv(2);vz(i)=vv(3);%姿態(tài)theta2(i)=theta0;%俯仰psi2(i)=fai0;%偏航gamma2(i)=gama0;%滾轉(zhuǎn)%位置經(jīng)緯度l2(i)=longitude0;%經(jīng)度a2(i)=latitude0;%緯度 end%t=0:1/200:120046 /200;figure;%hold onplot(t,vx);grid on;xlabel(

21、9;時間/秒'),ylabel('東向速度 米/秒');hold offfigure;hold on plot(t,vy);grid on;xlabel('時間/秒'),ylabel('北向速度 米/秒');hold offfigure;hold on plot(t,vz);grid on;xlabel('時間/秒'),ylabel('天向速度 米/秒');hold off%figure;%hold onplot(t,theta2*180/pi);grid on;xlabel('時間/秒')

22、,ylabel('俯仰角 度');hold offfigure;hold on plot(t,psi2*180/pi);grid on;xlabel('時間/秒'),ylabel('偏航角 度');hold offfigure;hold on plot(t,gamma2*180/pi);grid on;xlabel('時間/秒'),ylabel('滾轉(zhuǎn)角 度');hold off%figure;%hold onplot(t,l2*180/pi);grid on;xlabel('時間/秒'),ylab

23、el('經(jīng)度 度');hold offfigure;hold on plot(t,a2*180/pi);grid on;xlabel('時間/秒'),ylabel('緯度 度');hold off2 中低精度的源程序%純慣導(dǎo)解算中精度IMU靜態(tài)數(shù)據(jù)%clearclcclose alltic;%初始量定義pi = 3.1415926535897931;wie = 0.000072921151467; Re= 6378135.072;g = 9.7803267714;e = 1.0 / 298.25;%初始地理位置和速度lat_s =39.97885

24、*pi/180; %初始緯度lon_s =116.346824*pi/180; % 經(jīng)度 Vx_s = 0; % 東速Vy_s =0; % 北速%可調(diào)部分datanumber=174400; %為了與MIMS IMU導(dǎo)航結(jié)果對比，截取相同長度的數(shù)據(jù)進行處理T=0.01;%已知角度(初始姿態(tài))fai_s = 86.813325*pi/180; % 航向theta_s = -0.153277*pi/180; % 俯仰gama_s =-0.237267*pi/180; % 橫滾a = load('data4.txt');f = a(:,9:11)' %三軸比力輸出，單位m/s

25、2w0 = a(:,27:29)' %陀螺儀輸出的角速率信息單位由脈沖數(shù)rp/10ms% 陀螺標(biāo)定 tg=a(:,6:8)' GxtF=tg(1,:); GytF=tg(2,:); GztF=tg(3,:); kgx2=-0.0003066*0.00655288964; kgx1=0.03026*0.00655288964; kgx0=-0.0268*0.00655288964; kgy2=0.0008726*0.0067110686; kgy1=-0.04716*0.0067110686; kgy0=0.0494*0.0067110686; %脈沖數(shù) kgz2=0.00060

26、19*0.00656632715; kgz1=-0.005303*0.00656632715; kgz0=-0.4919*0.00656632715 - 0.0040385312531699899; for i=1:datanumber Gxt=GxtF(i)*GxtF(i); Gyt=GytF(i)*GytF(i); Gzt=GztF(i)*GztF(i); end bias_gx = (kgx2*Gxt + kgx1*(GxtF) + kgx0); bias_gy = (kgy2*Gyt+ kgy1*(GytF) + kgy0); bias_gz = (kgz2*Gzt+ kgz1*(Gz

27、tF) + kgz0); ww0 = w0(1,:) - bias_gx; ww1 = w0(2,:) - bias_gy; ww2 = w0(3,:) - bias_gz; para = pi/180.0; egxx=0.000423887215768231; %(°/rp) egyy=0.000413897587790739; egzz=0.000423033091235588; egxy=2.63723878994696e-006; %單位：弧度 egxz=9.57100499531451e-007; egyx=-2.03721501416524e-006; egyz=-8.5

28、3961364243771e-007; egzx=-4.39813070640871e-007; egzy=-1.81987993694005e-007; w(1,:)= (ww0*egxx + egxy*ww1 + egxz*ww2)*para/0.01; %單位rad/s w(2,:)= (ww1*egyy + egyx*ww0 + egyz*ww2)*para/0.01; w(3,:)= (ww2*egzz + egzx*ww0 + egzy*ww1)*para/0.01; % 慣導(dǎo)解算Cnb = cos(gama_s)*cos(fai_s)-sin(gama_s)*sin(theta_

29、s)*sin(fai_s), cos(gama_s)*sin(fai_s)+sin(gama_s)*sin(theta_s)*cos(fai_s), -sin(gama_s)*cos(theta_s); -cos(theta_s)*sin(fai_s), cos(theta_s)*cos(fai_s), sin(theta_s); sin(gama_s)*cos(fai_s)+cos(gama_s)*sin(theta_s)*sin(fai_s), sin(gama_s)*sin(fai_s)-cos(gama_s)*sin(theta_s)*cos(fai_s), cos(gama_s) *

30、 cos(theta_s);q = cos(fai_s/2)*cos(theta_s/2)*cos(gama_s/2) - sin(fai_s/2)*sin(theta_s/2)*sin(gama_s/2); cos(fai_s/2)*sin(theta_s/2)*cos(gama_s/2) - sin(fai_s/2)*cos(theta_s/2)*sin(gama_s/2); cos(fai_s/2)*cos(theta_s/2)*sin(gama_s/2) + sin(fai_s/2)*sin(theta_s/2)*cos(gama_s/2); cos(fai_s/2)*sin(thet

31、a_s/2)*sin(gama_s/2) + sin(fai_s/2)*cos(theta_s/2)*cos(gama_s/2); gyro=zeros(3,1);acc=zeros(3,1);pos_s = zeros(datanumber/5,2); %純慣導(dǎo)的結(jié)算軌跡atti_s = zeros(datanumber/5,3);v_s = zeros(datanumber/5,2);for i=1:1:datanumber Rmh = Re * (1.0 - 2.0 * e + 3.0 * e * sin(lat_s) * sin(lat_s); Rnh = Re * (1.0 + e

32、* sin(lat_s) * sin(lat_s); Wien = 0; wie * cos(lat_s); wie * sin(lat_s); Wenn = -Vy_s / Rmh; Vx_s / Rnh; Vx_s * tan(lat_s) / Rnh; Winn = Wien + Wenn; Winb = Cnb * Winn; for j=1:3 gyro(j,1) = w(j,i); %陀螺信息 acc(j,1) = f(j,i); %加速度信息 end angle = (gyro - Winb) * T; fn = Cnb'* acc; difVx = fn(1) + (2

33、.0 * wie * sin(lat_s) + Vx_s * tan(lat_s) / Rnh) * Vy_s; difVy = fn(2) - (2.0 * wie * sin(lat_s) + Vx_s * tan(lat_s) / Rnh) * Vx_s; Vx_s = difVx * T + Vx_s; Vy_s = difVy * T + Vy_s; if(mod(i,5)=0) v_s(i/5,:)=Vx_s,Vy_s; a1(i/5,1)=a(i,22); %經(jīng)度 a1(i/5,2)=a(i,23); %緯度 a1(i/5,3)=a(i,19); %航向 a1(i/5,4)=a(

34、i,20); %俯仰 a1(i/5,5)=a(i,21); %橫滾 end lat_s = lat_s + Vy_s * T / Rmh;lon_s = lon_s + Vx_s * T / Rnh / cos(lat_s); if(mod(i,5)=0) pos_s(i/5,:)=lat_s,lon_s; end 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;

35、 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(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); fai_s= atan(-Cnb(2,1) / Cnb(2,2);theta_s = asin(Cnb(2,3);