哈尔滨工业大学导航原理大作业报告.docx
《哈尔滨工业大学导航原理大作业报告.docx》由会员分享，可在线阅读，更多相关《哈尔滨工业大学导航原理大作业报告.docx（11页珍藏版）》请在冰豆网上搜索。
哈尔滨工业大学导航原理大作业报告
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篇幅在一页之内即可。
升级会员 