北航惯性导航综合实验三实验报告.docx
- 文档编号:25586882
- 上传时间:2023-06-10
- 格式:DOCX
- 页数:7
- 大小:19.63KB
北航惯性导航综合实验三实验报告.docx
《北航惯性导航综合实验三实验报告.docx》由会员分享,可在线阅读,更多相关《北航惯性导航综合实验三实验报告.docx(7页珍藏版)》请在冰豆网上搜索。
北航惯性导航综合实验三实验报告
北航惯性导航综合实验三实验报告
惯性导航技术综合实验实验三惯性导航综合实验实验3.1初始对准实验一、实验目的结合已经采集并标定好的惯性传感器数据进行粗对准,了解实现对准的过程;通过比较不同传感器数据的对准结果,进一步认识惯性传感器性能在导航系统中的重要地位。
为在实际工程设计中针对不同应用需求下采取相应的导航系统方案提供依据。
二、实验内容利用加速度计输出计算得到系统的初始姿态,利用陀螺输出信号计算航向角。
对比利用不同的惯性传感器数据计算所得的不同结果。
三、实验系统组成MEMSIMU(或其他类型IMU)、稳压电源、数据采集系统与分析系统。
四、实验原理惯导系统在开始进行导航解算之前需要进行初始对准,水平对准的本质是将重力加速度作为对准基准,其对准精度主要取决于两个水平加速度计的精度,加速度计的零位输出将会造成水平对准误差;方位对准最常用的方位是罗经对准,其本质是以地球自转角速度作为对准基准,影响对准精度的主要因素是等效东向陀螺漂移。
(1)其中,分别为当前时刻的俯仰角和横滚角计算值。
水平对准误差和方位对准误差如下所示:
,
(2)五、实验步骤及结果1、实验步骤:
采集静止状态下水平加速度计输出,按下式计算其平均值。
(3)其中,为前n个加计输出均值;为前n-1个加计输出均值;为当前时刻加计输出值。
利用加计平均值来计算系统初始俯仰角和横滚角(4)其中,分别为当前时刻的俯仰角和横滚角计算值。
2、实验结果与分析:
2.1、用MIMSIMU的加速度计信息计算
(1)俯仰角和横滚角图:
(2)失准角:
2.2、实验结果分析以上计算是基于MIMSIMU静止时data2进行的初始对准,与data2给定的初始姿态角相差不大。
六、源程序clearclcg=9.***-*****14;a=load('E:
\郭凤玲\chushiduizhun\data2.txt');ax=a(:
4)';ay=a(:
5)';az=a(:
6)';%初始值ax0
(1)=ax
(1)/1000*g;%%%%转化单位,由mg转化为m/s^2ay0
(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));fori=2:
120XX年7ax0(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:
120XX年7;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.MIMSIMU系统导航计算①将IMU固定在夹具上,将IMU连同夹具一起静置于桌面;②调整稳压电源的输出电压为+8V,关闭电源。
连接稳压电源与IMU供电输入端,连接IMU信号线与USB-232转接线至监控计算机;③打开监控计算机中的监控软件;④打开捷联惯导实验系统电源,捷联惯导实验系统开始启动;⑤保持捷联惯性导航系统静止600秒,并记录实时输出数据;⑥停止记录数据,利用捷联解算方法计算纯惯性导航误差。
MIMSIMU系统纯惯导导航结果:
(1)速度误差
(2)姿态误差
(2)位置误差2.中低精度惯性导航系统导航仿真①由实验老师给定一组中低精度IMU的静态IMU采样数据,初始姿态由数据中前300秒的加速度计采样计算得到,初始航向由GPS双天线数据给出;②利用捷联解算方法计算纯惯性导航误差。
中低精度惯性导航系统导航仿真结果:
(1)位置误差:
(2)姿态误差:
(3)速度误差:
3.导航仿真结果分析1,MIMSIMU系统的经度误差、fai误差、theta误差、Vx误差大,在调试程序的过程中,选取安装误差阵变化很微小的情况下,姿态误差,速度误差变化很大,说明准确的安装误差补偿阵很重要,上面MIMSIMU的图形是在没有补偿的情况下得到的图形,图形未列出。
上面图形是中精度导航系统的误差小,而中低精度导航系统的纬度误差、gama误差、Vy误差比MIMSIMU的误差小。
六,实验源程序1,MIMS的静态捷联结算clearall;clc;Q=load('E:
\惯性器件综合实验\我的作业\初始对准\data2.txt');%惯性信息FbbWib_bb%%%%%%%%%%%%%%%%%%%%下面为捷联解算初始值计算re=***-*****;%地球半径e=1/298.25;%地球扁率g0=9.***-*****14;wie=15.04107*pi/180/3600;%地球自转角速率vv=[0;0;0];%初始速度latitude0=116.3438*pi/180;%初始经度longitude0=39.******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.99330.0013-0.0020;0.00330.9950-0.0026;-0.00100.00220.9912];kg0=[0.0201;-0.0537;0.0810];%%%%%%%%%%%%%%%%%加速度计误差补偿Ka=[0.99870.0001-0.0020;-0.00010.99880.0004;0.0033-0.00150.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;%%%%%%%%%%%%%%%%%%%%%%%%%%%%%设置四元素的初始值ifT(3,2)T(2,3)q1=0.5*sqrt(1+T(1,1)-T(2,2)-T(3,3));elseq1=-(0.5*sqrt(1+T(1,1)-T(2,2)-T(3,3)));endifT(1,3)T(3,1)q2=0.5*sqrt(1-T(1,1)+T(2,2)-T(3,3));elseq2=-(0.5*sqrt(1-T(1,1)+T(2,2)-T(3,3)));endifT(2,1)T(1,2)q3=0.5*sqrt(1-T(1,1)-T(2,2)+T(3,3));elseq3=-(0.5*sqrt(1-T(1,1)-T(2,2)+T(3,3)));endq0=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=***-*****;rm=***-*****;%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%2.循环解算delt=0.005;Ti=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);%纬度%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%fori=1:
120XX年7fb=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.***-******sin(latitude0)*sin(latitude0)-0.***-******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));ifCte(3,1)0longitude0=atan(Cte(3,2)/Cte(3,1));elseifCte(3,2)0%等同于书上条件longitude0=atan(Cte(3,2)/Cte(3,1))+pi;elselongitude0=atan(Cte(3,2)/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;%求捷联矩阵修正四元法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,rolltheta0=asin(T(3,2));yawm=atan(-T(1,2)/T(2,2));rollm=atan(-T(3,1)/T(3,3));%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%ifT(2,2)0fai0=yawm+pi;elseifyawm0fai0=yawm;%-1/2*pi;elsefai0=yawm;endend%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%ifT(3,3)0gama0=rollm;elseifrollm0gama0=rollm+pi;elsegama0=rollm-pi;endend%%%%%%%%%%%%%%%%%%%%%%%%%收集信息%速度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:
120XX年6/200;figure;%holdonplot(t,vx);gridon;xlabel('时间/秒'),ylabel('东向速度米/秒');holdofffigure;holdonplot(t,vy);gridon;xlabel('时间/秒'),ylabel('北向速度米/秒');holdofffigure;holdonplot(t,vz);gridon;xlabel('时间/秒'),ylabel('天向速度米/秒');holdoff%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%figure;%holdonplot(t,theta2*180/pi);gridon;xlabel('时间/秒'),ylabel('俯仰角度');holdofffigure;holdonplot(t,psi2*180/pi);gridon;xlabel('时间/秒'),ylabel('偏航角度');holdofffigure;holdonplot(t,gamma2*180/pi);gridon;xlabel('时间/秒'),ylabel('滚转角度');holdoff%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%figure;%holdonplot(t,l2*180/pi);gridon;xlabel('时间/秒'),ylabel('经度度');holdofffigure;holdonplot(t,a2*180/pi);gridon;xlabel('时间/秒'),ylabel('纬度度');holdoff2中低精度的源程序%%%%%%%%%%%%%纯惯导解算中精度IMU静态数据%%%%%%%%%%%%%clearclcclosealltic;%%初始量定义pi=3.**********;wie=0.***-********-*****;Re=***-*****.072;g=9.***-*****14;e=1.0/298.25;%%初始地理位置和速度lat_s=39.******pi/180;%初始纬度lon_s=116.******pi/180;%经度Vx_s=0;%东速Vy_s=0;%北速%可调部分datanumber=*****;%为了与MIMSIMU导航结果对比,截取相同长度的数据进行处理T=0.01;%%已知角度(初始姿态)fai_s=86.******pi/180;%航向theta_s=-0.******pi/180;%俯仰gama_s=-0.******pi/180;%横滚a=load('data4.txt');f=a(:
9:
11)';%三轴比力输出,单位m/s^2w0=a(:
27:
29)';%陀螺仪输出的角速率信息单位由脉冲数rp/10ms%%陀螺标定tg=a(:
6:
8)';GxtF=tg(1,:
);GytF=tg(2,:
);GztF=tg(3,:
);kgx2=-0.***-******0.***-*****964;kgx1=0.03026*0.***-*****964;kgx0=-0.0268*0.***-*****964;kgy2=0.***-******0.***-*****86;kgy1=-0.04716*0.***-*****86;kgy0=0.0494*0.***-*****86;%脉冲数kgz2=0.***-******0.***-*****715;kgz1=-0.005303*0.***-*****715;kgz0=-0.4919*0.***-*****715-0.***-********-*****899;fori=1:
datanumberGxt=GxtF(i)*GxtF(i);Gyt=GytF(i)*GytF(i);Gzt=GztF(i)*GztF(i);endbias_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.***-********-*****31;%(°/rp)egyy=0.0004*****739;egzz=0.***-********-*****88;egxy=2.***-**********e-006;%单位:
弧度egxz=9.***-**********e-007;egyx=-2.***-**********e-006;egyz=-8.***-**********e-007;egzx=-4.398*****e-007;egzy=-1.***-**********e-007;w(1,:
)=(ww0*egxx+egxy*ww1+egxz*ww2)*para/0.01;%单位rad/sw(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);fori=1:
1:
datanumberRmh=Re*(1.0-2.0*e+3.0*e*sin(lat_s)*sin(lat_s));Rnh=Re*(1.0+
- 配套讲稿:
如PPT文件的首页显示word图标,表示该PPT已包含配套word讲稿。双击word图标可打开word文档。
- 特殊限制:
部分文档作品中含有的国旗、国徽等图片,仅作为作品整体效果示例展示,禁止商用。设计者仅对作品中独创性部分享有著作权。
- 关 键 词:
- 北航 惯性 导航 综合 实验 报告