《北航惯性导航综合实验三实验报告.docx》由会员分享，可在线阅读，更多相关《北航惯性导航综合实验三实验报告.docx（15页珍藏版）》请在taowenge.com淘文阁网|工程机械CAD图纸|机械工程制图|CAD装配图下载|SolidWorks_CaTia_CAD_UG_PROE_设计图分享下载上搜索。
1、北航惯性导航综合实验三实验报告惯性导航技术综合实验 实验三 惯性导航综合实验 实验 3.1初始对准实验一、实验目的结合已经采集并标定好的惯性 传感器数据进行粗对准，了解实现对准的过程；通过比较不同传感器数据的对准结果，进一步认识惯性传感 器性能在导航系统中的重要地位。为在实际工程设计中针对不同 应用需求下采取相应的导航系统方案提供依据。二、实验内容利用加速度计输出计算得到系统的初始姿态, 利用陀螺输出信号计算航向角。对比利用不同的惯性传感器数据 计算所得的不同结果。三、实验系统组成MEMSIMU （或其他类型IMU）、稳压电 源、数据采集系统与分析系统。四、实验原理惯导系统在开始进行导航解算之
2、前需要进行 初始对准，水平对准的本质是将重力加速度作为对准基准，其对 准精度主要取决于两个水平加速度计的精度，加速度计的零位输 出将会造成水平对准误差；方位对准最常用的方位是罗经对准，其本质是以地球自转角 速度作为对准基准，影响对准精度的主要因素是等效东向陀螺漂 移。其中，分别为当前时刻的俯仰角和横滚角计算值。 end % t=0:l/200:120XX 年 6 /200; figure;% hold on plot（t,vx）;grid on; xlabe 时间/秒）,ylabel（东 向速度 米/秒）；hold off figure; hold on plot（t,vy）;grid on;
3、 xlabel（z 时间/秒）,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; hol
4、d on plot（t,gamma2*180/pi）;grid on; xlabel（z 时间/秒）,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; xlabe7时间/秒）,ylabel（,纬度度）; hold off 2中低精度的源程序%嬲%嬲纯惯导解算中精度 IMU 静态数据% clear cic close all tic;嬲初始量定义 pi
5、 = 3.* ;wie = 0. * 一 *Re =* 一 * .072; g=e = 1.0 / 298.25;厥初始地理位置和速度lat_s =39.*pi/180; %初始纬度 lon_s =116*pi/180; % 经度Vx_s0; %东速 Vy_s=0; %北速 %可调部分datanumber=*; %为了与MIMS IMU导航结果对比，截取相同 长度的数据进行处理T=0.01;嬲已知角度(初始姿态)fai_s = 86.*pi/180; % 航向 theta_s = -0 *pi/180; % 俯仰 gama_s =-0 *pi/180; % 横滚 a = load(, data
6、4.txt ); f = 三轴比力输出，单位m/s八2w0=a(:,27:29) ；陀螺仪输出的角速率信息单位由脉冲数rp/10ms %陀螺标定 tg=a(:,6:8)/; GxtF=tg(l,:); GytF=tg(2,:); GztF=tg(3,:); kgx2=-0 *-*0. *_*964;kgxl=0.03026*0,*-*964; kgxO=-O.O268*0.*_*964;kgy2=0.*_*0*_*86;kgyl=-0.04716*0 *_*86; kgyO=0.0494*0.*-*86; % 脉冲数kgz2=0.*-*0,*-*715; kgzl=-0,005303*0 *-
7、 *715; kgzO二-04919*0*一*715 - 0.*_*_*899; for i = l:datanumber Gxt=GxtF(i)*GxtF(i); Gyt=GytF(i)*GytF(i);Gzt=GztF(i)*GztF(i); end bias_gx = (kgx2*Gxt + kgxl*(GxtF) + kgxO); bias_gy = (kgy2*Gyt+ kgyl*(GytF) + kgyO); bias_gz = (kgz2*Gzt+ kgzl*(GztF) + kgzO); wwO = w0(l,:) - bias_gx; wwl = w0(2,:) - bias
8、_gy; ww2 = w0(3,:) - bias_gz; para = pi/180.0; egxx=0.*-*-*31; %(/rp) egyy=O.0004*739; egzz=0.*-*-*88; egxy=2.*-*e-006; %单位：弧度egxz=9.*_*e-007; egyx二 _2.*_*e_0O6;egyz=-8.*_*e_007; egzx=-4.398*e-007; egzy=- *一*e -007; w(l,:)= (wwOegxx + egxy*wwl + egxz*ww2)*para/0.01; % 单位 rad/s w(2,:)= (wwl*egyy + eg
9、yx*ww0 + egyz*ww2)*para/0.01; w(3,:)= (ww2*egzz + egzx*ww0 + egzy*wwl)*para/0.01; % 惯导解算 Cnb = cos(gama_s)*cos(fai_s)-sin(gama_s)*sin(theta_s)*sin(fai_s), cos(gama_s)*sin(faLs)+sin(gama_s)*sin(theta_s)*cos(faLs), 一 sin(gama_s)*cos(theta_s);- cos(t heta_s)*si n (fa i_s),cos(theta_s)*cos(fai_s),sin(th
10、eta_s);sin(gama_s)*cos(fai_s)+cos(gama_s)*sin(theta_s)*sin(faLs), 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
11、(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,l); acc=zeros(3,l); pos_s = zeros(datanumber/5,2); %纯惯导的结算 轨 迹 atti_s = zeros(datanumber/5,3)
12、; v_s = zeros(datanumber/5,2); for i = l:l: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 * W
13、inn; for j = l:3 gyro(j,l) = w(j,i); % 陀螺信息 acc(j,l)= f(j,i); %加速度信息 end angle = (gyro - Winb) * T; fn = Cnb，* acc; difVx = fn(l) + (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 = di
14、fVy * T + Vy_s; if(mod(i,5)=0) v_s(i/5,:)=Vx_s,Vy_s; al(i/5,l)=a(i,22); % 经度 al(i/5,2)=a(i,23); % 纬度 al(i/5,3)=a(i,19); % 航向 al(i/5,4)=a(i,20); %俯仰 al(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 =
15、 0, -angle(l),- angle(2), -angle(3); angle(l), 0, angle(3), -angle(2); angle(2),- angle(3), 0, angle(l); angle(3), angle(2), -angle(l), 0; q = (cos(norm(angle) / 2) * eye(4) + sin(norm(angle) / 2) / norm(angle)* M) * q; q = q / norm(q); Cnb = q(l)*q(l)+q(2)*q(2)-q(3)*q(3)- q(4)*q(4),2*(q(2)*q(3)+q(l
16、)*q(4),2*(q(2)*q -q *q(3);2*(q(2)*q(3)-q(l)*q(4), q(l)*q(l)-q(2)*q(2)+q(3)*q(3)-q(4)*q(4), 2*(q(3)*q(4)+q(l)*q(2); 2*(q(2)*q(4)+q(l)*q(3), 2*(q *q - q(l)*q(2), q(l)*q(l)-q(2)*q(2)-q(3)*q(3)+q(4)*q(4);fai_s = atan(- Cnb(2,l) / Cnb(2,2); theta_s = asin(Cnb(2,3); gama_s = atan(- Cnb(l,3) / Cnb(3,3); if
17、 (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=l:datanumber/5; figure(l) subplot(211) plot(t,
18、pos_s(:)l)*180/pi-al(:)2)/ b ); title(z纬度误 差);xlabel( 7 0,05s );ylabel(度)；subplot(212) plot(t,pos_s(:,2)*180/pi- b ); title(z经度误差);xlabel(, 0.05s);ylabel(77); figure(2) subplot(311) plot(t,atti_s(:,l)-al(:,3), bz ); title(z fai 误差);xlabel(, 0.05s7 );ylabel(,度，);subplot(312) plot(t,atti_s(:,2)-al(:,4
19、), b ); title( theta 误差);xlabel( 0.05sz );ylabel(,度);subplot(313) plot(t,atti_s(:,3)-al(:,5), b); title- gama 误差);xlabel( 0.05s7 );ylabel(,度);figure(3) subplot(211) plot(t,v_s(:,l)/ b); title- Vx );xlabel(z 0.05s7 );ylabel(,m/s，); subplot(212) plot(t,v_s(:,2)/ bz ); title- Vyz );xlabel(z 0.05sz );yl
20、abel(z m/sz ); toe;水平对准误差和方位对准误差如下所示：,(2)五、实验步骤及结果1、实验步骤：采集静止状态下水平加速度计输出，按下式计算其平均值。(3)其中，为前n个加计输出均值；为前n-1个加计输出均值；为当前时刻加计输出值。利用加计平均值来计算系统初始俯仰角和横滚角(4)其中, 分别为当前时刻的俯仰角和横滚角计算值。2、实验结果与分析：2.1 用MIMS IMU的加速度计信息计算(1)俯仰角和横滚 角图：(2)失准角：2.2 、实验结果分析 以上计算是基于MIMS IMU静止时data2 进行的初始对准，与data2给定的初始姿态角相差不大。六、源程序 clear cl
21、eg = 9 .*-*14; a = load(, E: 郭凤玲 chushiduizhun data2.txt7); ax=a(:,4),; ay=a(:,5),; az=a(:,6),; % 初始值ax0(l)=ax(l)/1000*g; %转化单位，由mg转化为m/sA2 ay0(l)=ay(l)/1000*g;az0(l)=az(l)/1000*g;theta(l)=asin(ay(l)/az(l); gama(l)=-asin(ax(l)/az(l); for i=2:120XX 年 7 axO(i)=axO(i-l)+(ax(i)-axO(i-l)/i; ayO(i)=ayO(i-
22、l)+(ay(i)-ayO(i-l)/i;azO(i)=azO(i-l)+(az(i)-azO(i-l)/i; theta(i)=asin(ayO(i)/azO(i); gama(i)=-asin(axO(i)/azO(i); end detfaix=mean(ayO)/g; detfaiy = mean(-axO)/g; t=l:120XX 年 7; plot(t,theta/ rz ,t,gama/ b ) title-俯仰角和横滚角);ylabel(, 弧度(rad) );legend(俯仰角，横滚角)实验3.2惯性导 航静态实验一、实验目的1、掌握捷联惯导系统基本工作原理 2、掌握捷联
23、惯导系统捷联解算方法3、了解捷联惯导系统误差 传递规律和方程 二、实验原理 捷联惯性导航系统(SINS)的导 航解算流程如图1所示。在程序初始化部分，主要是获得SINS 的初始姿态阵、初始位置矩阵以及初值四元数；并读取SINS数据更新频率等SINS的工作参数。图1惯性导航原理 这里，、人分别为当地纬度和经度中、0、Y分别为载体航向、俯仰、横滚角。地理坐标系为东-北-天坐标 系。1 .姿态计算 姿态矩阵为：(1) (2)位置矩阵为：(3)其中：(4)使用姿态四元数来更新姿态。四元数微分方程为：(5)简写为：(6)其中：解四元数微分方程：(7)式中:(8)其中T为导航解算周期。归一化四元数，有更新
24、后的姿态矩阵：(9)提取姿态角：(10) 2.速度计算 由下列速度方程进行速度的更新(11) 式中，(12)速度更新(13)由式(11)求出加速度，则。在实 际程序中，为了进一步提高解算的精度，也可以在姿态阵更新前 后分别计算两次加速度，用梯形法求得速度的更新值。3.位置矩阵更新与位置计算(14)式中：(17)解上述微分方程使用如下解法：(15)提取经纬度：(16)四、主要实验设备捷联惯性导航实验系统一套；监控计算机一台。五、实验内容及结果L MIMS IMU系统导航计算 将IMU 固定在夹具上，将IMU连同夹具一起静置于桌面； 调整稳压电源的输出电压为+8V,关闭电源。连接稳压电 源与IMU
25、供电输入端，连接IMU信号线与USB-232转接线至监 控计算机；打开监控计算机中的监控软件；打开捷联惯导实验系统电源，捷联惯导实验系统开始启动; 保持捷联惯性导航系统静止600秒，并记录实时输出数 据； 停止记录数据，利用捷联解算方法计算纯惯性导航误差。MIMS IMU系统纯惯导导航结果：(1)速度误差(2)姿态误差(2)位置误差2.中低精 度惯性导航系统导航仿真由实验老师给定一组中低精度 IMU的静态IMU采样数据，初始姿态由数据中前300秒的加速 度计采样计算得到，初始航向由GPS双天线数据给出；利用捷联解算方法计算纯惯性导航误差。中低精度惯性导航系统导航仿真结果：(1)位置误差：(2)
26、姿态误差：(3)速度误差：3.导航仿真结果分析1, MIMS IMU系统的经度误差、fai 误差、theta误差、Vx误差大，在调试程序的过程中，选取安装 误差阵变化很微小的情况下，姿态误差，速度误差变化很大，说 明准确的安装误差补偿阵很重要，上面MIMS IMU的图形是在没 有补偿的情况下得到的图形，图形未列出。上面图形是中精度导 航系统的误差小,而中低精度导航系统的纬度误差、gama误差、Vy误差比MIMS IMU的误差小。六，实验源程序1, MIMS的静态捷联结算clear all ; cic ; Q=load( E: 惯性器件综合实验 我的作业初始对准 data2.txt ); %惯性
27、信息 Fbb Wib_bb%下面 为捷联解算初始值计算re=*-*;%地球半径e=1/298.25;%地 球扁率 gO = 9.*-*14; wie=15.04107*pi/180/3600;%地球自 转角速率 w=0;0;0;%初始速度 latitude0=116.3438*pi/180;%J 始经度 longitude。=39.*pi/180;%初始纬度 % hO=0;%初始高 度 Fibb=Q(:,4:6)*pi/3600/180; Wibb=Q(:,l:3)/1000*g0; faiO 二 87.16*pi/180; % 航向 thetaO =-0,158*pi/180; % 俯仰 g
28、amaO =- 0.325*pi/180; %厥%幽%嬲瞅陀螺仪安 装误差 补偿阵 Kg=0.9933 0.0013 -0.0020 ; 0.0033 0.9950 -0.0026 ; -0.0010 0.00220.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 ; kaO= 0,0018; 0,0014; 0,0009; %厥嬲%求初始姿态矩阵% Ctb = cos(gamaO)*cos(faiO
29、)-sin(gamaO)*sin(thetaO)*sin(faiO),cos(gamaO)*sin(faiO)+sin(gamaO)*sin(thetaO)*cos(faiO),-sin(gamaO)*cos(thetaO);-cos(thetaO)*sin(faiO),cos(thetaO)*cos(faiO),sin(thetaO);sin(gamaO)*cos(faiO)+cos(gamaO)*sin(thetaO)*sin(faiO), sin(gamaO)*sin(faiO)-cos(gamaO)*sin(thetaO)*cos(faiO), cos(gamaO) * cos(the
30、taO); cbt=Ctb ;T=cbt;厥%嬲%嬲%设置四元素的初始值if T(3,2)T(2,3) ql=0.5*sqrt(l+T(l,l)-T(2,2)-T(3,3); else ql=- (0.5*sqrt(l+T(l,l)-T(2,2)-T(3,3); end if T(1,3)T(3,1) q2=0.5*sqrt(l-T(l,l)+T(2,2)-T(3,3); else q2=-(0.5*sqrt(l- T(l,l)+T(2,2)-T(3,3); end if T(2,1)T(1,2) q3=0,5*sqrt(l-T(l,l)-T(2,2)+T(3,3); else q3=-(0.
31、5*sqrt(l-T(l,l)-T(2,2)+T(3,3); end qO=sqrt(l-qlA2-q2A2-q3八 2); %位置阵的初始值。Cte=-sin(latitude0),cos(latitude0),0;-sin(longitudeO)*cos(latitudeO),- sin(longitudeO)*sin(latitudeO),cos(longitudeO); cos(longitudeO)*cos(latitudeO),cos(longitudeO)*sin(latitudeO),sin(l ongitudeO);m = *-*;rm=*-*; % %2.循环解算 (比二0
32、.005;不二0.005;%初始 % 速度 vx=zeros(120XX 年 7,1); vy=zeros(120XX 年 7,1); vz=zeros(120XX 年 7,1); %姿态 theta2=zeros(120XX 年 7,1);%俯仰psi2=zeros(120XX 年 7,1);%偏航 gamma2=zeros(120XX 年 7,1);% 滚转 位置经纬度 l2=zeros(120XX年7,1);%经度 a2=zeros(120XX年7,1);%纬度 % % for i=l:120XX 年 7 fb=Fibb(i,:); fb=fb ; fb=Ka*fb+kaO; wib_b
33、=Wibb(i,:); wib_b=wib_bz ; wib_b=Kg* wib_b+kgO; fp=转换至当 地地理坐标系 %g 更新g=g0*(l+0.*-*sin(latitudeO)*sin(latitudeO)-O .*-*sin(2*latitude0)*sin(2*latitude0); difVx = fp(l) + (2.0 * wie * sin(latitudeO) + w(l) * tan(latitudeO) / rn) * vv(2); difVy = fp(2)- (2.0 * wie * sin(latitudeO) + vv(l)* tan(latitudeO
34、) / rn) * vv(l); dw=fp-0;0;g + difVx ; difVy ;0; w=w+dw*delt积分法 wett=-vv(2)/(rn);vv(l)/(rm);vv(l)*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 ,0,-wett ； -wett ,wett ,0; Cte=Cte+dc*delt; % 位置计算 latitude
35、。=asin(Cte(3,3); if Cte(3,l)0 longitudeO=atan(Cte(3,2)/Cte(3,l); elseif Cte(3,2)0%等 同于书上条件 longitudeO=atan(Cte(3,2)/Cte(3,l)+pi; elselongitudeO=atan(Cte(3,2)/Cte(3,l)-pi; end Q=q0;ql;q2;q3; dQ=l/2*0,-wtbb(l),-wtbb(2),-wtbb(3);wtbb(l),0,wtbb(3),-wtbb ；wtbb(2),-wtbb(3),0,wtbb(l);wtbb(3),wtbb(2),-wtbb(
36、l),O*Q;Q=Q+dQ*delt;qq 二(sqrt(Q 八 2+Q A2+Q A2+Q 八 2);qO = Q /qq;ql=Q(2)/qq; q2=Q(3)/qq; q3=Q(4)/qq; %求捷联矩阵修正四元法 T=q0A2+qlA2-q2A2-q3A2,2*(ql*q2-q0*q3),2*(ql*q3+q0*q2); 2*(ql*q2+q0*q3),q0A2-qlA2+q2A2-q3A2,2*(q2*q3-q0*ql);2*(ql*q3-q0*q2),2*(q2*q3+q0*ql),q0A2-qlA2-q2A2+q3A2; % 下面根据捷联矩阵T计算姿态角yaw, y, roll
37、thetaO=asin(T(3,2); yawm=atan(-T(l,2)/T(2,2);rollm=atan(-T(3,l)/T(3,3); % % if T(2,2)0 faiO=yawm + pi; else if yawmO fai0=yawm;%-l/2*pi; else faiO=yawm;endend % if T(3,3)0 gamaO=rollm; else if rollmO gamaO=rollm + pi; else gamaO=rollm- pi; end end % 收 集信息 速度 vx(i)=vv(l); vy(i)=vv(2); vz(i)=vv(3); %姿态 theta2(i)=theta。;%俯仰 psi2(i)=fai01% 偏航 gamma2(i)=gama0;% 滚转 位置经纬度 I2(i)=longitude0;% 经 度 a2(i)=latitude0;% 纬 度