北航惯性导航综合实验五实验报告Word文档下载推荐.doc
《北航惯性导航综合实验五实验报告Word文档下载推荐.doc》由会员分享,可在线阅读,更多相关《北航惯性导航综合实验五实验报告Word文档下载推荐.doc(18页珍藏版)》请在冰豆网上搜索。
![北航惯性导航综合实验五实验报告Word文档下载推荐.doc](https://file1.bdocx.com/fileroot1/2022-10/19/5e30e9c5-eec2-468a-a170-57a0fb235fca/5e30e9c5-eec2-468a-a170-57a0fb235fca1.gif)
GPS接收机一套;
④实验车一辆;
⑤车载大理石平台;
⑥车载电源系统。
四、实验内容
1)实验准备
①将IMU紧固在车载大理石减振平台上,确认IMU的安装基准面紧靠实验平台;
②将IMU与导航计算机、导航计算机与车载电源、导航计算机与监控计算机、GPS接收机与导航计算机、GPS天线与GPS接收机、GPS接收机与GPS电池之间的连接线正确连接;
③打开GPS接收机电源,确认可以接收到4颗以上卫星;
④打开电源,启动实验系统。
2)捷联惯导/GPS组合导航实验
①进入捷联惯导初始对准状态,记录IMU的原始输出,注意5分钟内严禁移动实验车和IMU;
②实验系统经过5分钟初始对准之后,进入导航状态;
③移动实验车,按设计实验路线行驶;
④利用监控计算机中的导航软件进行导航解算,并显示导航结果。
五、实验结果及分析
(一)理论推导捷联惯导短时段(1分钟)位置误差,并用1分钟惯导实验数据验证。
1、一分钟惯导位置误差理论推导:
短时段内(t<
5min),忽略地球自转,运动轨迹近似为平面,此时的位置误差分析可简化为:
(1)加速度计零偏引起的位置误差:
m
(2)失准角引起的误差:
(3)陀螺漂移引起的误差:
可得1min后的位置误差值
2、一分钟惯导实验数据验证结果:
(1)纯惯导解算1min的位置及位置误差图:
(2)纯惯导解算1min的速度及速度误差图:
实验结果分析:
纯惯导解算短时间内精度很高,1min的惯导解算的北向最大位移误差-2.668m,东向最大位移误差-8.231m,可见实验数据所得位置误差与理论推导的位置误差在同一数量级,结果不完全相同是因为理论推导时做了大量简化,而且实验时视GPS为真实值也会带来误差;
另外,可见1min内纯惯导解算的东向速度最大误差-0.2754m/s,北向速度最大误差-0.08027m/s。
(二)选取IMU前5分钟数据进行对准实验。
将初始对准结果作为初值完成1小时捷联惯导和组合导航解算,对比1小时捷联惯导和组合导航结果。
1、5minIMU数据的解析粗对准结果:
2、5minIMU数据的Kalman滤波精对准结果:
3、一小时IMU/GPS数据的组合导航结果图及估计方差P阵图:
4、一小时IMU数据的捷联惯导解算结果与组合滤波、GPS输出对比图:
5、结果分析:
由滤波结果图可以看出:
(1)由组合后的速度、位置的P阵可以看出滤波之后载体的速度和位置比GPS输出的精度高。
(2)短时间内SINS的精度较高,初始阶段的导航结果基本和GPS、组合导航结果重合,1小时后的捷联惯导解算结果很差,纬度、经度、高度均发散。
(3)INS/GPS组合滤波的结果和GPS的输出结果十分近似,因为1小时的导航GPS的精度比SINS导航的精度高很多,Kalman滤波器中GPS信号的权重更大。
(4)总体看来,SINS/GPS组合滤波的结果优于单独用SINS或GPS导航的结果,起到了协调、超越、冗余的作用,使导航系统更可靠。
六、SINS/GPS组合导航程序
%%%%%%%%%%%%%%%%%%%%%%%INS/GPS组合导航跑车1h实验%%%%%%%%%%%%%%%%%%%%%%%%%%
%该程序为15维状态量,6维观测量的kalman滤波程序,惯性/卫星组合松耦合的数学模型
clear
clc
closeall
%%初始量定义
wie=0.000072921151467;
Re =6378135.072;
g=9.7803267714;
e=1.0/298.25;
T=0.01;
%IMU频率100hz,此程序中GPS频率100hz
datanumber=360000;
%数据时间3600s
a=load('
imu_1h.dat'
);
w=a(:
3:
5)'
*pi/180/3600;
%陀螺仪输出的角速率信息单位由°
/h化为rad/s
f=a(:
6:
8)'
;
%三轴比力输出,单位g
gps_1h_new.dat'
gps_pos=a(:
5);
%GPS输出的纬度、经度、高度信息
gps_pos(:
1:
2)=gps_pos(:
2)*pi/180;
%纬经单位化为弧度
gps_v=a(:
8);
%GPS输出的东北天速度信息
%捷联解算及卡尔曼相关
v=zeros(datanumber,3);
%组合后的速度信息
atti=zeros(datanumber,3);
%组合后的姿态信息
pos=zeros(datanumber,3);
%组合后的位置信息
gyro=zeros(3,1);
acc=zeros(3,1);
x_kf=zeros(datanumber,15);
p_kf=zeros(datanumber,15);
lat=40.0211142228246*pi/180;
%组合导航的初始位置、姿态、速度
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=gps_v(1,3);
X_o=zeros(15,1);
%X的初值选为0
X=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]);
%随机
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.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.00005^2,0.00005^2,0.00005^2,2^2,2^2,2^2,(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(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=[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)];
Cnb_s=Cnb;
q_s=q;
fori=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;
forj=1:
3
gyro(j,1)=w(j,i);
acc(j,1)=f(j,i)*g;
%加速度信息,单位化为m/s^2
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*