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

惯性导航技术综合实验 实验三 惯性导航综合实验 实验3.1 初始对准实验 一、实验目的 结合已经采集并标定好的惯性传感器数据进行粗对准，了解实现对准的过程；
通过比较不同传感器数据的对准结果，进一步认识惯性传感器性能在导航系统中的重要地位。为在实际工程设计中针对不同应用需求下采取相应的导航系统方案提供依据。

二、实验内容 利用加速度计输出计算得到系统的初始姿态，利用陀螺输出信号计算航向角。对比利用不同的惯性传感器数据计算所得的不同结果。

三、实验系统组成 MEMS IMU（或其他类型IMU）、稳压电源、数据采集系统与分析系统。

四、实验原理 惯导系统在开始进行导航解算之前需要进行初始对准，水平对准的本质是将重力加速度作为对准基准，其对准精度主要取决于两个水平加速度计的精度，加速度计的零位输出将会造成水平对准误差；
方位对准最常用的方位是罗经对准，其本质是以地球自转角速度作为对准基准，影响对准精度的主要因素是等效东向陀螺漂移。

(1) 其中，分别为当前时刻的俯仰角和横滚角计算值。

水平对准误差和方位对准误差如下所示：
， (2) 五、实验步骤及结果 1、实验步骤：
采集静止状态下水平加速度计输出，按下式计算其平均值。

(3) 其中，为前n个加计输出均值；
为前n-1个加计输出均值；
为当前时刻加计输出值。

利用加计平均值来计算系统初始俯仰角和横滚角 (4) 其中，分别为当前时刻的俯仰角和横滚角计算值。

2、实验结果与分析：
2.1、用MIMS IMU的加速度计信息计算 （1）俯仰角和横滚角图：
（2）失准角：
2.2、实验结果分析 以上计算是基于MIMS IMU静止时data2进行的初始对准，与data2给定的初始姿态角相差不大。

六、源程序 clear clc g = 9.7803267714; a=load(＇E:＼郭凤玲＼chushiduizhun＼data2.txt＇); ax=a(:,4)＇; ay=a(:,5)＇; az=a(:,6)＇; %初始值ax0(1)=ax(1)/1000*g; %%%%转化单位，由mg转化为m/s^2 ay0(1)=ay(1)/1000*g; az0(1)=az(1)/1000*g; theta(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)); end detfaix=mean(ay0)/g; detfaiy=mean(-ax0)/g; t=1:120047; plot(t,theta,＇r＇,t,gama,＇b＇) title(＇俯仰角和横滚角＇);ylabel(＇弧度（rad）＇); legend(＇俯仰角＇,＇横滚角＇) 实验3.2 惯性导航静态实验 一、实验目的 1、掌握捷联惯导系统基本工作原理 2、掌握捷联惯导系统捷联解算方法 3、了解捷联惯导系统误差传递规律和方程 二、实验原理 捷联惯性导航系统（SINS）的导航解算流程如图1所示。在程序初始化部分，主要是获得SINS的初始姿态阵、初始位置矩阵以及初值四元数；
并读取SINS数据更新频率等SINS的工作参数。

图1 惯性导航原理 这里，、λ分别为当地纬度和经度ψ、θ、γ分别为载体航向、俯仰、横滚角。地理坐标系为东-北-天坐标系。

1．姿态计算 姿态矩阵为：
(1) (2) 位置矩阵为：
(3) 其中：
(4) 使用姿态四元数来更新姿态。四元数微分方程为：
(5) 简写为：
(6) 其中：
解四元数微分方程：
(7) 式中：
(8) 其中T为导航解算周期。

归一化四元数，有更新后的姿态矩阵：
(9) 提取姿态角：
(10) 2．速度计算 由下列速度方程进行速度的更新 （11）
式中 ， (12) 速度更新 (13) 由式（11）求出加速度，则。在实际程序中，为了进一步提高解算的精度，也可以在姿态阵更新前后分别计算两次加速度，用梯形法求得速度的更新值。

3．位置矩阵更新与位置计算 (14) 式中：
(17) 解上述微分方程使用如下解法：
(15) 提取经纬度：
(16) 四、主要实验设备 ① 捷联惯性导航实验系统一套；

② 监控计算机一台。

五、实验内容及结果 1．MIMS IMU系统导航计算 ① 将IMU固定在夹具上，将IMU连同夹具一起静置于桌面；

② 调整稳压电源的输出电压为+8V，关闭电源。连接稳压电源与IMU供电输入端，连接IMU信号线与USB-232转接线至监控计算机；

③ 打开监控计算机中的监控软件；

④ 打开捷联惯导实验系统电源，捷联惯导实验系统开始启动；

⑤ 保持捷联惯性导航系统静止600秒，并记录实时输出数据；

⑥ 停止记录数据，利用捷联解算方法计算纯惯性导航误差。

MIMS IMU系统纯惯导导航结果：
（1）速度误差 （2）姿态误差 （2）位置误差 2．中低精度惯性导航系统导航仿真 ① 由实验老师给定一组中低精度IMU的静态IMU采样数据，初始姿态由数据中前300秒的加速度计采样计算得到，初始航向由GPS双天线数据给出；

② 利用捷联解算方法计算纯惯性导航误差。

中低精度惯性导航系统导航仿真结果：
（1）
位置误差：
（2）
姿态误差：
（3）
速度误差：
3．导航仿真结果分析 1，MIMS IMU系统的经度误差、fai误差、theta误差、Vx误差大，在调试程序的过程中，选取安装误差阵变化很微小的情况下，姿态误差，速度误差变化很大，说明准确的安装误差补偿阵很重要，上面MIMS IMU的图形是在没有补偿的情况下得到的图形，图形未列出。上面图形是中精度导航系统的误差小，而中低精度导航系统的纬度误差、gama误差、Vy误差比MIMS IMU的误差小。

六，实验源程序 1，MIMS的静态捷联结算 clear all ; clc ; Q=load(＇E:＼惯性器件综合实验＼我的作业＼初始对准＼data2.txt＇); %惯性信息 Fbb Wib_bb %%%%%%%%%%%%%%%%%%%%下面为捷联解算初始值计算 re=6378393;%地球半径 e=1/298.25;%地球扁率 g0 = 9.7803267714; wie=15.04107*pi/180/3600;%地球自转角速率 vv=[0;0;0];%初始速度 latitude0=116.3438*pi/180;%初始经度 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.0001 -0.0020 ; -0.0001 0.9988 0.0004 ; 0.0033 -0.0015 0.9988 ]; ka0= [0.0018; 0.0014; 0.0009]; %%%%%%%%%%求初始姿态矩阵%%%% 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); 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; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%设置四元素的初始值 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))); end if T(1,3)>T(3,1) q2=0.5*sqrt(1-T(1,1)+T(2,2)-T(3,3)); else q2=-(0.5*sqrt(1-T(1,1)+T(2,2)-T(3,3))); end if 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))); end q0=sqrt(1-q1^2-q2^2-q3^2); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%位置阵的初始值。

Cte=[-sin(latitude0),cos(latitude0),0; -sin(longitude0)*cos(latitude0),-sin(longitude0)*sin(latitude0),cos(longitude0); cos(longitude0)*cos(latitude0),cos(longitude0)*sin(latitude0),sin(longitude0)]; rn=6356803; rm=6378254; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %2.循环解算 delt=0.005; Ti=0.005; %初始 %速度 vx=zeros(120047,1); vy=zeros(120047,1); vz=zeros(120047,1); %姿态 theta2=zeros(120047,1);%俯仰 psi2=zeros(120047,1);%偏航 gamma2=zeros(120047,1);%滚转 %位置经纬度 l2=zeros(120047,1);%经度 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;%%%%%转换至当地地理坐标系 %g更新 g=g0*(1+0.0052884*sin(latitude0)*sin(latitude0)-0.0000059*sin(2*latitude0)*sin(2*latitude0)); difVx = fp(1) + (2.0 * wie * 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=wib_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)/Cte(3,1))-pi; end Q=[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; %求捷联矩阵修正四元法 T=[q0^2+q1^2-q2^2-q3^2,2*(q1*q2-q0*q3),2*(q1*q3+q0*q2); 2*(q1*q2+q0*q3),q0^2-q1^2+q2^2-q3^2,2*(q2*q3-q0*q1); 2*(q1*q3-q0*q2),2*(q2*q3+q0*q1),q0^2-q1^2-q2^2+q3^2]; %下面根据捷联矩阵T计算姿态角yaw, y，roll theta0=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; end end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% if T(3,3)>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); %姿态 theta2(i)=theta0;%俯仰 psi2(i)=fai0;%偏航 gamma2(i)=gama0;%滚转 %位置经纬度 l2(i)=longitude0;%经度 a2(i)=latitude0;%纬度 end %%%%%%%%%%%%%%%%%%%%%%%%%%%% t=0:1/200:120046 /200; figure;% hold on plot(t,vx);grid on; xlabel(＇时间/秒＇),ylabel(＇东向速度 米/秒＇); hold off figure; hold on plot(t,vy);grid on; xlabel(＇时间/秒＇),ylabel(＇北向速度 米/秒＇); hold off figure; hold on plot(t,vz);grid on; xlabel(＇时间/秒＇),ylabel(＇天向速度 米/秒＇); hold off %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% figure;% hold on plot(t,theta2*180/pi);grid on; xlabel(＇时间/秒＇),ylabel(＇俯仰角 度＇); hold off figure; hold on plot(t,psi2*180/pi);grid on; xlabel(＇时间/秒＇),ylabel(＇偏航角 度＇); hold off figure; hold on plot(t,gamma2*180/pi);grid on; xlabel(＇时间/秒＇),ylabel(＇滚转角 度＇); hold off %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% figure;% hold on plot(t,l2*180/pi);grid on; xlabel(＇时间/秒＇),ylabel(＇经度 度＇); hold off figure; hold on plot(t,a2*180/pi);grid on; xlabel(＇时间/秒＇),ylabel(＇纬度 度＇); hold off 2 中低精度的源程序 %%%%%%%%%%%%%纯惯导解算中精度IMU静态数据%%%%%%%%%%%%% clear clc close all tic; %%初始量定义 pi = 3.1415926535897931; wie = 0.000072921151467; Re = 6378135.072; g = 9.7803267714; e = 1.0 / 298.25; %%初始地理位置和速度 lat_s =39.97885*pi/180; %初始纬度 lon_s =116.346824*pi/180; % 经度 Vx_s = 0; % 东速 Vy_s =0; % 北速 %可调部分 datanumber=174400; %为了与MIMS IMU导航结果对比，截取相同长度的数据进行处理 T=0.01; %%已知角度(初始姿态) 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^2 w0 = a(:,27:29)＇; %陀螺仪输出的角速率信息单位由脉冲数rp/10ms %% 陀螺标定 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; %脉冲数 kgz2=0.0006019*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*(GztF) + 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.53961364243771e-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; %% 惯导解算 Cnb = [ cos(gama_s)*cos(fai_s)-sin(gama_s)*sin(theta_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) * 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(theta_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); %纯惯导的结算轨迹 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 * 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.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); %经度 a1(i/5,2)=a(i,23); %纬度 a1(i/5,3)=a(i,19); %航向 a1(i/5,4)=a(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]; 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)); gama_s = atan(-Cnb(1,3) / Cnb(3,3)); if (Cnb(2,2) < 0) fai_s = fai_s + pi; elseif (fai_s < 0) fai_s = fai_s + 2*pi; end if (Cnb(3,3) < 0) if(gama_s > 0) gama_s = gama_s - pi; else gama_s = gama_s + pi; end end if(mod(i,5)==0) atti_s(i/5,:) = [fai_s/pi*180, theta_s/pi*180, gama_s/pi*180]; end i end %绘图 t=1:datanumber/5; figure(1) subplot(211) plot(t,pos_s(:,1)*180/pi-a1(:,2),＇b＇); title(＇纬度误差＇);xlabel(＇0.05s＇);ylabel(＇度＇); subplot(212) plot(t,pos_s(:,2)*180/pi- a1(:,1),＇b＇); title(＇经度误差＇);xlabel(＇0.05s＇);ylabel(＇度＇); figure(2) subplot(311) plot(t,atti_s(:,1)-a1(:,3),＇b＇); title(＇fai误差＇);xlabel(＇0.05s＇);ylabel(＇度＇); subplot(312) plot(t,atti_s(:,2)-a1(:,4),＇b＇); title(＇theta误差＇);xlabel(＇0.05s＇);ylabel(＇度＇); subplot(313) plot(t,atti_s(:,3)-a1(:,5),＇b＇); title(＇gama误差＇);xlabel(＇0.05s＇);ylabel(＇度＇); figure(3) subplot(211) plot(t,v_s(:,1),＇b＇); title(＇Vx＇);xlabel(＇0.05s＇);ylabel(＇m/s＇); subplot(212) plot(t,v_s(:,2),＇b＇); title(＇Vy＇);xlabel(＇0.05s＇);ylabel(＇m/s＇); toc;