哈尔滨工业大学导航原理大作业报告.docx
《哈尔滨工业大学导航原理大作业报告.docx》由会员分享,可在线阅读,更多相关《哈尔滨工业大学导航原理大作业报告.docx(11页珍藏版)》请在冰豆网上搜索。
![哈尔滨工业大学导航原理大作业报告.docx](https://file1.bdocx.com/fileroot1/2023-1/26/61644765-5521-48e7-88ae-7b8ed96e911d/61644765-5521-48e7-88ae-7b8ed96e911d1.gif)
哈尔滨工业大学导航原理大作业报告
AssignmentofPrinciplesofNavigation
《导航原理》作业(惯性导航部分2016秋)
MyChineseName
MyClassNo.
MyStudentNo.
Taskdescription
Inanfictitiousmission,aspaceshipistobeliftedfromalaunchingsitelocatedat19o37'NLand110o57'EL,intoacircularorbit400kilometershighalongtheequator.ThespaceshipisequippedwithastrapdownINSwhosethreegyros,GX,GY,GZ,andthreeaccelerometers,AX,AY,AZ,areinstalledrespectivelyalongtheaxesXb,Yb,Zbofthebodyframe.
Case1:
Stationarytest
Duringapre-launchinggroundtest,thebodyframeofthespaceshipinitiallycoincideswiththelocalgeographicalframe,withitspitchingaxisXbpointingtotheeast,rollingaxisYbtothenorth,andheadingaxisZbupwards.Thenthebodyofthespaceshipismadetorotatein3steps:
(1)80oaroundXb
(2)90oaroundYb
(3)170oaroundZb
Afterthat,thebodyofthespaceshipstopsrotating.Youarerequiredtocomputethefinaloutputsofthethreeaccelerometersinthespaceship,usingquaternionandignoringthedeviceerrors.Itisassumedthatthemagnitudeofgravityaccelerationatthegroundlevelisg0=9.79m/s2.
Case2:
Thelaunchingprocess
Thespaceshipisinstalledonthetopofanverticallyerectedrocket.Itsinitialheading,pitchingandrollingangleswithrespecttothelocalgeographicalframeare-90,90and0degreesrespectively.Thedefaultrotationsequenceisheading→pitching→rolling.Thetopoftherocketisinitially100mabovethesealevel.Thentherocketisfiredup.
Theoutputsofthegyrosandaccelerometersinthe
spaceshiparebothpulsenumbers.Eachgyropulseisan
angularincrementof0.01arcsec,andeachaccelerometer
pulseis1e-7g0,withg0=9.79m/s2.Thegyrooutputfre-
quencyis100Hz,andtheaccelerometer’sis5Hz.The
outputsofthegyrosandaccelerometerswithin1460sare
storedinaMATLABdatafilenamedmission.mat,con-tainingmatricesGGMof146000×3fromgyrosand
AAMof7300×3fromaccelerometersrespectively.Theformatofthedatainthetwomatricesisasshowninthetables,with10rowsofeachmatrixselected.Eachrowrepresentstheoutputsofthetypeofsensorsatasamplingtime.
TheEarthcanbeseenasanidealsphere,withradiusR=6371.00kmandspinningrateis7.292×10-5rad/s,Theerrorsofthegyrosandac-celerometerscanbeignored,buttheeffectofheightonthemagnitudeofgravityhastobetakenintoaccount.Thegravityaccelerationatthesealevelofthenearequatorregioncanbechosenasg0=9.79m/s2,andthemagnitudeofgravityaccelerationataheightofhcanbecomputedas
Besides,theinfluenceofheightontheangularratesofthegeographicalframeandthechangingratesof
latitudeandlongitudeshouldalsobeconsidered.
Theoutputsofthegyroscanbeintegratedevery0.01s.Therotationofthegeographicalframeistobeup-datedevery0.2s,soarethevelocitiesandpositionsofthespaceship.
Youarerequiredto:
(1)computethefinalattitudequaternion,longitude,latitude,height,andeast,north,verticalvelocitiesofthespaceship.
(2)drawthelatitude-versus-longitudetrajectoryofthespaceship,withhorizontallongitudeaxis.
(3)drawthecurveoftheheightofthespaceship,withhorizontaltimeaxis.
(4)drawthecurvesoftheattitudeanglesofthespaceship,withhorizontaltimeaxis.
一任务一
1利用四元数计算飞船上加速度计的最终输出
初始时刻三个加速度计的输出A=[0,0,9.79]。
由四元数的定义可得对于第一次旋转
第二次旋转
第三次旋转
并且由于采用映像的方法,得到三次旋转后的四元数q为
。
再由
可得出最终加速度计的输出。
经过matlab编程计算可得最终加速计的输出为A=[0,5.8068,5.8313,5.3030]’,且A的模等于重力加速度g0。
2任务一matlab代码
q1=[cos(40/360*pi)sin(40/360*pi)00];
q2=[cos(45/360*pi)0sin(45/360*pi)0];
q3=[cos(85/360*pi)00sin(85/360*pi)];
q4=quatmultiply(q1,q2);
q=quatmultiply(q4,q3);
g=[0009.79];
qr=quatinv(q);
A0=quatmultiply(qr,g);
A=quatmultiply(A0,q);
A=A';
norm(A)
ans=
9.7900
二任务二
1由题目得导航解算流程图如下所示
2matlab解算代码
k=20;%姿态迭代次数n
T=0.2;%速度位置迭代单次运行时间
K=7300;%速度位置迭代次数
R=6371000;
We=7.292e-5;%地球半径,自转角速度
Q=zeros(7301,4);%定义姿态四元数
Longitude=zeros(1,7301);
Latitude=zeros(1,7301);%定义经纬度变化矩阵
H=zeros(1,7301);%定义高度变化矩阵
Q0=[cos(-45/180*pi),0,0,sin(-45/180*pi)];
Q1=[cos(45/180*pi),sin(45/180*pi),0,0];
Q(1,:
)=quatmultiply(Q0,Q1);%由题目所给导弹初始状态计算初始姿态四元数
Longitude
(1)=110.95;
Latitude
(1)=19.62;
H=zeros(1,7301);
H
(1)=100;%初始经纬度,高度信息
Ve=zeros(1,7301);
Vn=zeros(1,7301);
Vu=zeros(1,7301);%定义导弹的东北天速度矩阵
Ve
(1)=0;
Vn
(1)=0;
Vu
(1)=0;%初始速度为0
FE=zeros(1,7301);
FN=zeros(1,7301);
FU=zeros(1,7301);%定义东北天加速度矩阵
loadmission.mat%导入已知数据
forN=1:
K%开始位置,速度迭代
q1=zeros(21,4);
q1(1,:
)=Q(N,:
);%定义一个矩阵来存储姿态迭代时的最终结果
forn=1:
k%开始姿态迭代
WX=((0.01/3600)/180*pi)*GGM((N-1)*20+n,1);%将角秒换算成弧度,得到陀螺仪测量的角增量
WY=((0.01/3600)/180*pi)*GGM((N-1)*20+n,2);
WZ=((0.01/3600)/180*pi)*GGM((N-1)*20+n,3);
w=[WX,WY,WZ];
normw=norm(w);
W=[0,-w
(1),-w
(2),-w(3);w
(1),0,w(3),-w
(2);w
(2),-w(3),0,w
(1);w(3),w
(2),-w
(1),0];%四阶近似矩阵
I=eye(4);%为便于计算而定义一个对角阵
S=1/2-normw^2/48;
C=1-normw^2/8+normw^4/384;
q1(n+1,:
)=((C*I+S*W)*q1(n,:
)')';%四阶近似求毕卡解
end
Q(N+1,:
)=q1(n+1,:
);
WE=-Vn(N)/R;
WN=Ve(N)/R+We*cos(Latitude(N)/180*pi);
WH=Ve(N)/R*tan(Latitude(N)/180*pi)+We*sin(Latitude(N)/180*pi);%地理坐标系的转动角速度分量
gama=[WE,WN,WH]*T;%利用地理坐标系的转动四元数修正载体姿态四元数
normgama=norm(gama);
n=gama/normgama;
QG=[cos(normgama/2),sin(normgama/2)*n];%地理坐标系四元数
Q(N+1,:
)=quatmultiply(quatinv(QG),Q(N+1,:
));%姿态四元数修正
FX=1e-7*9.79*AAM(N,1);
FY=1e-7*9.79*AAM(N,2);
FZ=1e-7*9.79*AAM(N,3);
%加速度计测得的比力
Fb=[FXFYFZ];
F1=quatmultiply(Q(N+1,:
),[0,Fb]);
F=quatmultiply(F1,quatinv(Q(N+1,:
)));
FE(N)=F
(2);
FN(N)=F(3);
FU(N)=F(4);
%计算载体相对加速度
VED(N)=FE(N)+Ve(N)*Vn(N)/R*tan(Latitude(N)/180*pi)-(Ve(N)/R+2*We*cos(Latitude(N)/180*pi))*Vu(N)+2*Vn(N)*We*sin(Latitude(N)/180*pi);
VND(N)=FN(N)-2*Ve(N)*We*sin(Latitude(N)/180*pi)-Ve(N)*Ve(N)/R*tan(Latitude(N)/180*pi)-Vn(N)*Vu(N)/R;
VUD(N)=FU(N)+2*Ve(N)*We*cos(Latitude(N)/180*pi)+(Ve(N)^2+Vn(N)^2)/R-9.79*R^2/((R+H(N))^2);
%积分计算载体的相对速度
Ve(N+1)=VED(N)*T+Ve(N);
Vn(N+1)=VND(N)*T+Vn(N);
Vu(N+1)=VUD(N)*T+Vu(N);
%位置更新
Longitude(N+1)=0.5*(Ve(N)+Ve(N+1))/R/cos(Latitude(N)/180*pi)*T/pi*180+Longitude(N);
Latitude(N+1)=0.5*(Vn(N)+Vn(N+1))/R*T/pi*180+Latitude(N);
H(N+1)=0.5*(Vu(N)+Vu(N+1))*T+H(N);
End
Eul(1,:
)=[000];%计算载体姿态信息,分别为heading,pitching,rolling
forN=1:
7300
Eul(N,:
)=q2eul(Q(N,:
));
end
%绘制图像
%绘制导弹的经纬度变化曲线
figure
(1)
hold on
grid on
plot(Longitude,Latitude,'LineWidth',2);
xlabel('longitude');
ylabel('latitude');
title('latitude-versus-longitude trajectory');
%绘制导弹的高度变化曲线
figure
(2)
holdon
gridon
plot((0:
7300),H);
title('the curve of the height');
xlabel('time/s');
ylabel('height/m');
%绘制导弹的姿态角变化曲线
%偏航角曲线
figure(3)
plot((1:
7300),Eul(:
1)*180/pi);
holdon
gridon
xlabel('time/s');
ylabel('heading/circ');
title('headinginformation');
%俯仰角曲线
figure(4)
holdon
gridon
plot((1:
7300),Eul(:
2)*180/pi);
xlabel('time/s');
ylabel('pitching/circ');
title('pitchinginformation');
%横滚角曲线
figure(5)
holdon
gridon
plot((1:
7300),Eul(:
3)*180/pi);
xlabel('time/s');
ylabel('rolling/circ');
title('rollinginformation');
%输出迭代计算结果
str1='final longitude:
';
fprintf(1,'%s%f\n',str1,Longitude(7301));
final longitude:
168.197173
str2='final latitude:
';
fprintf(1,'%s%f\n',str2,Latitude(5401));
final latitude:
-0.189239
str3='final height:
';
fprintf(1,'%s%f\n',str3,H(7301));
final height:
401269.745925
fprintf(1,'%s%f\n',str2,Latitude(7301));
final latitude:
0.003872
str4='final velocity of east:
';
fprintf(1,'%s%f\n',str4,Ve(7301));
final velocity of east:
7366.633430
str5='final velocity of north:
';
fprintf(1,'%s%f\n',str5,Vn(7301));
final velocity of north:
-0.300354
str6='final velocity of vertical:
';
fprintf(1,'%s%f\n',str6,Vu(7301));
final velocity of vertical:
0.790571
str7='final attitude quaternion:
';
fprintf(1,'%s%f\n',str7,Q(N,:
));
final attitude quaternion:
7.070875e-01
3.915839e-04
-3.621620e-04
-7.071258e-01
3仿真结果
(1)最终的姿态四元数为Q(7301,:
)=[0.707,0.00039,-0.00036,--0.707]’。
最后飞船所在的位置为-0.19NL,168.20EL。
高度为401.27Km。
东北天速度分别为7366.63m/s,-0.30m/s,0.79m/s。
(2)以经度为横轴,纬度为纵轴的变化曲线:
(3)飞船高度随时间的变化曲线:
(4)飞船偏航角随时间变化曲线:
(5)飞船俯仰角随时间变化曲线:
(6)飞船横滚角随时间变化曲线:
4结果分析
由最终结果可得,飞船最后的位置纬度为零左右,说明已经到达赤道上方,且东向速度为7366.63m/s,而北向和天向速度几乎为零,说明飞船在绕着赤道运行,与题干描述一致。
从高度曲线上看,飞船从开始时不断上升,速度先快后慢,到3200s左右时高度达到400Km左右,到达赤道上方,之后不再上升。
从经纬度曲线上看,飞船不断地向南飞行,直至到达赤道上方。
从飞船地姿态角变化来看,首先需明白飞船坐标系和地理坐标系重合,如图所示,
飞船的X轴(俯仰)Y轴(横滚)Z轴(偏航)分别指向地理坐标系的东北天方向。
而在任务二中,飞船的初始姿态角为(90,0,-90),即飞船头部指向东,俯仰角为90度,垂直向上飞行,飞行刚开始偏航角迅速变为-180度,即头部朝向南飞行,即向赤道方向飞行,在大约800s时偏航角又开始逐渐变回-90度,而俯仰角逐渐由90度变为0度,说明飞船在到达赤道上方后开始向东飞行。
三个人陈述
1此处可以陈述在大作业的完成过程中遇到的问题,你自己的思考,以及解决方案。
2篇幅在一页之内即可。