哈尔滨工业大学导航原理大作业报告

Assignment of Principles of Navigation 《导航原理》作业(惯性导航部分2016 秋)
My Chinese Name
My Class No.
My Student No.
Task description
In an fictitious mission, a spaceship is to be lifted from a launching site located at 19o
37' NL and 110o
57' EL, into a circular orbit 400 kilometers high along the equator. The spaceship is equipped with a strapdown INS whose three gyros, G X , G Y , G Z , and three accelerometers, A X , A Y , A Z , are installed respectively along the axes X b , Y b , Z b of the body frame.
Case 1: Stationary test
During a pre-launching ground test, the body frame of the spaceship initially coincides with the local geographical frame, with its pitching axis X b pointing to the east, rolling axis Y b to the north, and heading axis Z b upwards. Then the body of the spaceship is made to rotate in 3 steps: (1) 80o
around X b
(2) 90o
around Y b (3) 170o
around Z b
After that, the body of the spaceship stops rotating. You are required to compute the final outputs of the three accelerometers in the spaceship, using quaternion and ignoring the device errors. It is assumed that the magnitude of gravity acceleration at the ground level is g 0 = 9.79m/s 2
.
Case 2: The launching process
The spaceship is installed on the top of an vertically erected rocket. Its initial heading, pitching and rolling angles with respect to the local geographical frame are -90, 90 and 0 degrees respectively. The default rotation sequence is heading → pitching → rolling. The top of the rocket is initially 100m above the sea level. Then the rocket is fired up.
The outputs of the gyros and accelerometers in the spaceship are both pulse numbers. Each gyro pulse is an angular increment of 0.01 arcsec, and each accelerometer pulse is 1e -7
g 0, with g 0 = 9.79m/s 2
. The gyro output fre-
quency is 100Hz, and the accelerometer’s is 5Hz.
The outputs of the gyros and accelerometers within 1460s are stored in a MATLAB data file named mission.mat, con-taining matrices GGM of 146000×3 from gyros and AAM of 7300×3 from accelerometers respectively. The format of the data in the two matrices is as shown in the tables, with 10 rows of each matrix selected. Each row represents the outputs of the type of sensors at a sampling time.
The Earth can be seen as an ideal sphere, with radius R = 6371.00km and spinning rate is 7.292×10
-5
rad/s, The errors of the gyros and ac- celerometers can be ignored, but the effect of
height on the magnitude of gravity has to be taken into account. The gravity acceleration at the sea level of the near equator region can be chosen as g 0=9.79 m/s 2
, and the magnitude of
gravity acceleration at a height of h can be computed as
Besides, the influence of height on the angular rates of the geographical frame and the changing rates of
latitude and longitude should also be considered.
The outputs of the gyros can be integrated every 0.01s. The rotation of the geographical frame is to be up- dated every 0.2s, so are the velocities and positions of the spaceship. You are required to:
(1) compute the final attitude quaternion, longitude, latitude, height, and east, north, vertical velocities of the spaceship.
(2) draw the latitude-versus-longitude trajectory of the spaceship, with horizontal longitude axis. (3) draw the curve of the height of the spaceship, with horizontal time axis. (4) draw the curves of the attitude angles of the spaceship, with horizontal time axis.
g h =g 0
R 2
(R +h )2
一 任务一
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) 0 0]; q2=[cos(45/360*pi) 0 sin(45/360*pi) 0]; q3=[cos(85/360*pi) 0 0 sin(85/360*pi)]; q4=quatmultiply(q1,q2); q=quatmultiply(q4,q3); g=[0 0 0 9.79]; qr=quatinv(q);
A0=quatmultiply(qr,g); A=quatmultiply(A0,q)； A=A'； norm(A) ans =
9.7900
二 任务二
1 由题目得导航解算流程图如下所示
A e '=q -1Aq
2 matlab解算代码
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); %定义东北天加速度矩阵
load mission.mat %导入已知数据
for N=1:K %开始位置，速度迭代
q1=zeros(21,4);
q1(1,:)=Q(N,:); %定义一个矩阵来存储姿态迭代时的最终结果
for n=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=[FX FY FZ];
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(Latit ude(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,:)=[0 0 0 ]; %计算载体姿态信息，分别为heading,pitching,rolling for N=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)
hold on
grid on
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);
hold on
grid on
xlabel('time/s');
ylabel('heading/circ');
title('heading information');
%俯仰角曲线
figure(4)
hold on
grid on
plot((1:7300),Eul(:,2)*180/pi);
xlabel('time/s');
ylabel('pitching/circ');
title('pitching information');
%横滚角曲线
figure(5)
hold on
grid on
plot((1:7300),Eul(:,3)*180/pi);
xlabel('time/s');
ylabel('rolling/circ');
title('rolling information');
%输出迭代计算结果
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 篇幅在一页之内即可。