北航惯性导航综合实验五实验报告.docx
- 文档编号:8340256
- 上传时间:2023-01-30
- 格式:DOCX
- 页数:19
- 大小:69.79KB
北航惯性导航综合实验五实验报告.docx
《北航惯性导航综合实验五实验报告.docx》由会员分享,可在线阅读,更多相关《北航惯性导航综合实验五实验报告.docx(19页珍藏版)》请在冰豆网上搜索。
北航惯性导航综合实验五实验报告
惯性导航技术综合实验
实验五惯性基组合导航及应用技术实验
惯性/卫星组合导航系统车载实验
一、实验目的
1掌握捷联惯导/GPS组合导航系统的构成和基本工作原理:
2掌握采用卡尔曼波波方法进行捷联惯导/GPS组合的基本原理:
3掌握捷联惯导/GPS组合导航系统静态性能:
4掌握动态情况下捷联惯导/GPS组合导航系统的性能。
二、实验内容
1复习卡尔曼滤波的基本原理(参考《卡尔曼滤波与组合导航原理》第二、五章);
2复习捷联惯导/GPS组合导航系统的基本工作原理(参考以光衢编著的《惯性导航原理》第七章);
三、实验系统组成
1捷联惯导/GPS组合导航实验系统一套:
2监控计算机一台。
3差分
GPS接收机一套:
4实验车一辆:
5车载大理石平台:
6车载电源系统。
四、实验内容
D实验准备
1将IMU紧固在车载大理石减振平台上,确认IMU的安装基准面紧靠实验平台;
2将IMU与导航计算机、导航计算机与车载电源、导航计算机与监控计算机、GPS接收机与导航计算机、GPS天线与GPS接收机、GPS接收机与GPS电池之间的连接线正确连接;
3打开GPS接收机电源,确认可以接收到4颗以上卫星;
4打开电源,启动实验系统。
2)捷联惯导/GPS组合导航实验
1进入捷联惯导初始对准状态,记录IMU的原始输出,注意5分钟内严禁移动实验车和IMU;
2实验系统经过5分钟初始对准之后,进入导航状态;
3移动实验车,按设计实验路线行驶;
4利用监控计算机中的导航软件进行导航解算,并显示导航结果。
五、实验结果及分析
(一)理论推导捷联惯导短时段(1分钟)位置误差,并用1分钟惯导实验数据验证。
1、一分钟惯导位置误差理论推导:
短时段内(t<5min),忽略地球自转绍。
=0,运动轨迹近似为平面1/R=O,此时的位置误差分析可简化为:
Vr2
(1)加速度计零偏V引起的位置误差:
Jx,=—=0.8802m
2
(2)失准角我引起的误差:
3x2=^-=0.9218m
(3)陀螺漂移£引起的误差:
=0.0137m
6
可得1min后的位置误差值5x=+J.r2+5x.=1.8157m
2、一分钟惯导实验数据验证结果:
(1)纯惯导解算1min的位置及位置误差图:
(2)纯惯导解算1min的速度及速度误差图:
vy80
60
|40
20
00100020003000400050006000
0.01s
实验结果分析:
纯惯导解算短时间内精度很高,1min的惯导解算的北向最大位移误差-2.668m,东向最大位移误差-8.231m,可见实验数据所得位置误差与理论推导的位置误差在同一数量级,结果不完全相同是因为理论推导时做了大量简化,而且实验时视GPS为真实值也会带来误差;另外,可见1min内纯惯导解算的东向速度最大误差-0.2754m/s,北向速度最大误差-0.08027m/So
(-)选取IMU前5分钟数据进行对准实验。
将初始对准结果作为初值完成1小时捷联惯导和组合导航解算,对比1小时捷联惯导和组合导航结果。
1、5niinlMU数据的解析粗对准结果:
-1.1960
-0.9998
-0.0084
Cnh=
0.9979
-1.1990
-0.0163
0.0062
0.1198
0.9991
attitude=[219.7700-0.93180.4828]
fai=219.9744642380873*pi/180;
theta=-0.895865732956914*pi/180;
gama=0.640089448357591*pi/l80;
1
遂0.5
X
*
0
xio北问速度误差方差
2
*0
0
xi(/北向速度误差4
23
0.01秒x104
xio来向速度误差方差
•20123
3
侧2.5
o
12
0.01秒X面
C
)123
00秒X104
X103北向失准角
X10’北向失准角方差
2
0
8S
3、一小时
3
侧2.5
2
,
xioF向失准角方差
o
8S
0
X103条向失准角
23
。
・。
1秒x104
x10’天向失准角方差
23
0.01秒*104
东向速度
任〜
-201[1[[1c[
00.511.522.533.54
001SX105
北向i®度
20
.20[[[LL_
00.511.522.5
0.01S
天向速度
300
0.01Sx105
俯仰角
°_.—\%A(<«*'S^/-'>zA/YJp>yV
■5ccIccIc
00.511.522.533.54
0.01s
xio^Pffi®角误差
41111
X1031
P乐向速度误差
1I
1■,1■■
0.5-
■
irrirr
u■
00.5
11.522.533.54
0.05Sv奇
X10
Xio'3
P北向速度误差
1T
0.5•
■
nr
irrirr
u
00.5
11.522.533.54
0.05SX105
x10'1
P天向速度误差
1I
1iV1€€
0.5•
.
nr
irrirr
u
00.5
11.522.533.54
0.01sX105
w
°00.511.522.533.54
°.。
估*105
4、一小时IMU数据的捷联惯导解算结果与组合滤波、GPS输出对比图:
0.01Sheight
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实
A)A)A)/UA)/UA)A)A)A)/UA)/UA)A)A)A)A)A)/UA)/U
%该程序为15维状态量,6维观测量的kalman滤波程序,惯性/卫星组合松耦合的数学模型
clear
clc
closeall
%%初始量定义
\vie=0.1467;
Re=6378135.072;
g=9.7803267714;
%IMU频率100hz,此程序中GPS频率100hz
%数据时间3600s
e=1.0/298.25;
T=0.01;
datanumber=360000;
%组合后的速度信息
%组合后的姿态信息
%组合后的位置信息
%组合导航的初始位置、姿态、速度
%捷联解算及卡尔曼相关
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);
[at=40.46*pi/180;
Ion=116.60*pi/180;
height=43.0674;
fai=219.73*pi/180;theta=-0.6914*pi/180;
gama=0.7591*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)A2,(50e-6*g)A2,(50e-6*g)A2,(0.1*pi/180/3600)A2,(0.1*pi/180/3600)A2
(0.1*pi/180/3600)A2,0,0,0,0,0,0,0,0,0]);%随机
Q=diag([(0.008*pi/180/3600)A2,(0.008*pi/180/3600)A2,(0.008*pi/180/3600)A2,(50e-6*g)
A2,(50e-6*g)A2,(50e-6*g)A2,0,0,0,0,0,0,0,0,0]);
R=diag([(0.01)A2,(0.01)A2,(0.01)A2,(0.1)A2,(0.1)A2,(0.15)A2]);
P=zeros(15);
P_k=diag([(0.00005*pi/180)A2,(0.00005*pi/180)A2,(0.00005*pi/180)A2,0.00005A2,0.0000
5A2,0.00005A2,2A2,2A2,2A2,(0.001*pi/180/3600)A2,(0.001*pi/180/3600)A2,(0.001*pi/18
0/3600)A2,(50e-6*g)A2,(50e-6*g)A2,(50e-6*g)A2]);%
K=zeros(15,6);
Z=zeros(6,1);
l=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;v/ie*cos(lat);vzie*sin(lat)];
V/enn=[-Vy/Rmh;Vx/Rnh;Vx*tan(lat)/Rnh];
Winn=Wien+Wenn;
V/inb=Cnb*Winn;
forj=1:
3
gyro(j,1)=v/(j,i);
acc(j,1)=f(j,i)*g;%加速度信息,单位化为m/s"2
end
angle=(gyro-Winb)*T;
fn=Cnb1*acc;
difVx=fn
(1)+(2.0*v/ie*sin(lat)+Vx*tan(lat)/Rnh)*Vy;
difVy=fn
(2)-(2.0*v/ie*sin(lat)+Vx*tan(lat)/Rnh)*Vx;
difVz=fn(3)+(2.0*v/ie*cos(lat)+Vx/Rnh)*Vx+Vy*Vy/Rmh-g;
Vx=difVx*T+Vx;
Vy=difVy*T+Vy;
Vz=difVz*T+Vz;
lat=lat+Vy*T/Rmh;
Ion=Ion+Vx*T/Rnh/cos(lat);
height=height+Vz*T;
M=[0,-angle(l),-angleQ),・angle(3);
angle.),0,angle(3),-angle
(2);
angle
(2),-angle(3),0,angle
(1);
angle⑶,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⑷),
2*(q
(2)P⑷・q⑴P⑶);
2*(q
(2)*q(3)-q
(1)*q(4)),q
(1)*q⑴-q
(2)*q
(2)+q(3)*q(3)-q⑷*q⑷,
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)];
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;%以上为纯惯导解算%%
F1=[0wie*sin(lat)+v(i,1)*tan(lat)/(Rnh)-(v/ie*cos(lat)+v(i,1)/(Rnh))0-1/(Rmh)0000;
-(\vie*sin(lat)+v(i,1)*tan(lat)/(Rnh))0-v(i,2)/(Rmh)1/(Rnh)00
-wie*sin(lat)00;
v/ie*cos(lat)+v(i,1)/(Rnh)v(i,2)/(Rmh)0
ran(srr)、(Rnh)00wie*cos(srr)+v(L2)*sec=at:
rsec(_at:
)/(Rnh)00;
0,m(3)fnsv(L2)*ran=arv(Rmh),v(L3M(Rmh)2zMQSm=at:
)+v(£:
)*t:
aMat:
)、(Rnh)・(2*wiQcosaa-)+v(Ll)、(Rnh))
fns0—fn3,2* )+v(Lq)*ran(srr)、(Rnh)),v(L3)、(Rmh).v(L2)、(Rmh)—(2*wi%cos(srrf)+v(Llr$c(srr)*sec(5rt: )、(Rnh)) fn (2) 2*(wie*cos? r)+v(LlM(RM)),2J<.e*sm(_at: )*v(Ll)05? 000 000- 000 0 000 000 h GH【cnb.Neros(3);zerossnnbLzeros? 6)L fn30 2 0MRmh)0 sec(_ac/(Rnh) nIL卜q=卜qcvzeros(3)Neros(3)kag(【Rmh》Rnh*cos? t: r】)Neros(3》6)】;决HI奏F2"nnb.Neros(3);zeros(3)nn5 zeros(3)Neros(3)】; F"FLF2j zeros? 」5二;次ka-man繇需藏厩#愁fhf*t“流蛰薄尖remplHeye(15);disFHeye(15)jforjn1: 10 remsHF*rems一" disFndisF+remp。 ;sd Empl"Q*T; disQHrempl"forjH2三 gmp2HF Empqn(remp2+remp2-)、j; wd・・ 4J 对123456 『/V ezzzzzz disQ=disQ+tempi; Vx-gps_v(i,1);%试测量为纯惯导与GPS的速度差、位置差 Vy-gps_v(i,2); Vz•gps_v(i,3); (lat-gps_pos(i,1))*Rmh;%纬经度化为位移,单位m (Ion・gps_pos(i,2))*Rnh*cos(lat); height.gps_pos(i,3); X=disF*X_o;%kalman滤波五个公式 P=disF*P_k*disF+disQ; K=P*H7(H*P*H,+R); X_k=X+K*(Z-H*X); P_k=(I-K*H)*P; x_kf(i,1)=X_k (1)/pi*180; x_kf(i,2)=X_k (2)/pi*180; x_kf(i,3)=X_k(3)/pi*18O; x_kf(i,4)=X_k(4); x_kf(i,5)=X_k(5); x_kf(i,6)=X_k(6); x_kf(i,7)=X_k(7); x_kf(i,8)=X_k(8); x_kf(i,9)=X_k(9); x_kf(i,1O)=X_k(1O)/pi*180*3600; x^kf(i,11)=X.k(11)/pi*180*3600; x_kf(i,12)=X.k(12)/pi*180*3600; x_kf(i,13)=X_k(13)"0人6/g; x_kf(i,14)=X_k(14)*10A6/g; x_kf(i,15)=X_k(15)r(T6/g; %平台误差角 %速度误差 %位置误差 %陀螺随机常值漂移,单位°/h %加计随机常值偏置,单位Ug P_kf(iJ)=sqrt(abs(P_k(1,1)))/pi*180; p_kf(i,2)=sqrt(abs(P_k(2,2)))/pi*180; p_kf(i,3)=sqrt(abs(P_k(3,3)))/pi*180; p_kf(i,4)=sqrt(abs(P_k(4,4))); p_kf(i,5)=sqrt(abs(P_k(5,5))); p_kf(i,6)=sqrt(abs(P_k(6,6))); p_kf(i,7)=sqrt(abs(P_k(7,7))); p_kf(i,8)=sqrt(abs(P_k(8,8))); p_kf(i,9)=sqrt(abs(P_k(9,9))); p_kf(i,10)=sqrt(abs(P_k(10,10)))/pi*180*3600; p_kf(i,11)=sqrt(abs(P_k(11,11)))/pi*180*3600; p_kf(i,12)=sqrt(abs(P_k(12,12)))/pi*180*3600; p_kf(i,13)=sqrt(abs(P_k(13,13)))*10A6/g; p_kf(i,14)=sqrt(abs(P_k(14,14)))*10A6/g; p_kf(iJ5)=sqrt(abs(P_k(15,15)))*10A6/g; Vx=Vx-X_k⑷;%速度校正 Vy=Vy・X_k(5); Vz=Vz•X_k(6); v(i,: )=[Vx,Vy,Vz]; lat=lat-X_k⑺; Ion=Ion-X_k(8); height=height-X_k(9); pos(i,: )=[lat,Ion,height]; Atheta=X_k (1); Agama=X_k (2); Afai=X_k⑶; Ctn=[1,Afai,-Agama; -Afai,1,Atheta; Agama,-Atheta,1]; Cnb=Cnb*Ctn; fai=atan(・Cnb(2,1)/Cnb(2,2)); theta=asin(Cnb(2,3)); gama=atan(・Cnb(1,3)/Cnb(3,3)); if(Cnb(2,2)<0) fai=fai+pi; elseif(fai<0) fai=fai+2*pi; end %位置校正 %kalman滤波估计得出的失准角theta%kalman滤波估II•得出的失准角gania%kalman滤波估讣得出的失准角fai %更新姿态阵 if(Cnb(3,3)<0) if(gama>0) gama=gama-pi; else gama=gama+pi; end end atti(i,: )=[fai/pi*180,theta/pi*180,gama/pi*180]; q (2)=sqrt(abs(1+Cnb(1,1)・Cnb(2,2)-Cnb(3,3)))/2; q(3)=sqrt(abs(1-Cnb(1,1)+Cnb(2,2)-Cnb(3,3)))/2; q(4)=sqrt(abs(1•Cnb(1,1)-Cnb(2,2)+Cnb(3,3)))/2; q (1)=sqrt(abs(1-q (2)*q (2)-q(3)*q(3)-q(4)*q(4))); if
- 配套讲稿:
如PPT文件的首页显示word图标,表示该PPT已包含配套word讲稿。双击word图标可打开word文档。
- 特殊限制:
部分文档作品中含有的国旗、国徽等图片,仅作为作品整体效果示例展示,禁止商用。设计者仅对作品中独创性部分享有著作权。
- 关 键 词:
- 北航 惯性 导航 综合 实验 报告
![提示](https://static.bdocx.com/images/bang_tan.gif)