哈工大导航原理大作业剖析

《导航原理》作业

(惯性导航部分)

一、题目要求

A fighter equipped with SINS is initially at the position of ?35 NL and ?122 EL,stationary on a motionless carrier. Three gyros X G ,Y G ,Z G ,and three accelerometers, X A ,Y A ,Z A are installed along the axes b X ,b Y ,b Z of the body frame respectively.

Case 1:stationary onboard test

The body frame of the fighter initially coincides with the geographical frame, as shown in the figure, with its pitching axis b X pointing to the east,rolling axis b Y to the north, and azimuth axis b Z upward. Then the body of the fighter is made to rotate step by step relative to the geographical frame.

(1) ?10around b X (2) ?30around b Y (3) ?50-around b Z

After that, the body of the fighter stops rotating. You are required to compute the final output of the three accelerometers on the fighter, using both DCM and quaternion respectively,and ignoring the device errors. It is known that the magnitude of gravity acceleration is 2/8.9g s m =.

Case 2:flight navigation

Initially, the fighter is stationary on the motionless carrier with its board 25m above the sea level. Its pitching and rolling axes are both in the local horizon, and its rolling axis is ?45on the north by east, parallel with the runway onboard. Then the fighter accelerate along the runway and take off from the carrier.

The output of the gyros and accelerometers are both pulse numbers,Each gyro pulse is an angular increment of sec arc 1.0-,and each accelerometer pulse is

g 6e 1-,with 2/8.9g s m =.The gyro output frequency is 10 Hz,and the accelerometer’s

is 1Hz. The output of gyros and accelerometers within 5400s are stored in MATLAB data files named gout.mat and aout.mat, containing matrices gm of 35400? and am of 35400? respectively. The format of data as shown in the tables, with 10 rows of each matrix selected. Each row represents the out of the type of sensors at each sample time.

The Earth can be seen as an ideal sphere, with radius 6368.00km and spinning rate rad/s 10292.75-?, The errors of sensors are ignored, so is the effect of height on the magnitude of gravity. The output of the gyros are to integrated every 0.1s. The rotation of

geographical frame is to be updated every 1s, so are the velocities and position of the figure. You are required to:

(1)Compute the final attitude quaternion, longitude, latitude, height, and east, north, vertical velocities of the fighter.

(2)Compute the total horizontal distance traveled by the fighter.

(3)Draw the latitude -versus -longitude trajectory of the fighter, with horizontal longitude axis.

(4)Draw the curve of the height of fighter, with horizontal time axis.

二、Case1解答

2.1 方向余弦阵法

（1） 绕Xb 轴转过ψ?=10?

????

? ????-??=????? ??-=10cos 10sin 010sin 10cos 0001cos sin 0sin cos 0001?????C

（2） 绕Yb 轴转过?=30θ

????

? ?????-?=????? ??-=30cos 030sin 01030sin 030cos cos 0sin 010sin 0cos θθθθθC

（3） 绕Zb 轴转过?-=50ψ

????

? ???-?-?-?-=????? ??=1000）50（cos ）50（sin -0）50（sin ）50（cos 1000cos sin -0sin cos ψψψψψ

C 所以变换后的坐标

X E Y C C C N Z ?θψζ??

?? ?

?= ? ? ? ???

??

由于初始时刻有

009.8E N ζ???? ? ?= ? ? ? ?????

所以计算得

(

Z Y X )

=( 3581

.86027.2-4054

.4-)

三个加速度计的输出分别是 ,/4504.42

x s m A -=

,/6027.22y s m A -=

s m A /3581.8z =2

计算程序见附录一

2.2 四元数法

（1）绕Xb 轴转过ψ?=10? i 2

sin

2

cos q ?

?

?+=

（2）绕Yb 轴转过?=30θ

j 2

sin

2

cos

q θ

θ

θ+=

（3）绕Zb 轴转过?-=50ψ

k 2

sin

2

cos

q ψ

ψ

ψ+=

则合成四元数

)2

sin 2(cos )2sin 2(cos )2sin 2(cos

k j i q q q q ?+?+?+==ψψθθ??ψθ?

合成四元数的逆

1123q p i p j p k λ-=---

由公式 q N E q

Z Y

X ????

? ?

?=

????

?

?

?-ξ1

计算得 (Z Y X )=(3581

.86027.2-4054

.4-)

三个加速度计的输出分别是 ,/4504.42

x s m A -=

,/6027.22y s m A -=

s m A /3581.8z =2

由两种计算方法的计算结果可以看出，方向余弦阵法和四元数法的计算结果是一致的。 计算程序见附录二

三、Case2解答

3.1程序流程图

3.2 源程序详见附录三 3.3 仿真及结果

3.3.1 运行程序可得：

（1）战斗机相对当地地理坐标系的姿态四元数为：

[]4841.00023

.00054.08750

.0----=Q

（2）经度= ?8757.122

纬度=?35.0694

高度=?8061.24

（3）战斗机的东部，北部和垂直速度

s m V /7716.393e -=

s m V /5612.267n = s m V /1583.3u -=

（4）战斗机总水平距离为：m 103822.26?

3.3.2运行程序，绘制纬度-经度战斗机轨迹（以经度为横轴，纬度为纵轴）；如图：

3.3.3 运行程序，绘制战斗机的高度的曲线（以时间为横轴，高度为纵轴），如图：

四、心得体会

通过本次的大作业，我对比力、四元数等惯性导航技术中涉及的基本概念有了更深的理解，同时再次复习方向余弦矩阵法和四元数法求解捷联惯导系统，更加熟练地掌握其算法。

除此之外，本次大作业中所有程序均要涉及Matlab编程，在编写程序时，四元数的初始化非常重要，如果四元数的初始化错误，会导致所有的运算结果错误。在编程过程中，每当遇到问题，我就会上网查一些资料，并与周边同学进行深入讨论，学习并解决问题。这进一步提高了我的自学能力和对知识综合应用的能力。

五、参考/协助确认

本文是根据本人研究成果，并参考相关文献资料整合而成，非本人完全独立研究完成，编程部分很大程度上参考了百度百科及周边同学的研究成果。另外，报告形式上借鉴了上届学长学姐的形式。本人对作业中的某些结论不负完全责任，特此声明。

C1=[1,0,0

0,cos(10/180*pi),sin(10/180*pi)

0,-sin(10/180*pi),cos(10/180*pi)

]; %第一次转动方向余弦阵

C2=[ cos(30/180*pi),0,-sin(30/180*pi)

0,1,0

sin(30/180*pi),0,cos(30/180*pi)

];

%第二次转动方向余弦阵

C3=[ cos(-50/180*pi),sin(-50/180*pi),0

-sin(-50/180*pi),cos(-50/180*pi),0

0,0,1

];

%第三次转动方向余弦阵

A=C3*C2*C1*[0;0;9.8] %计算转动后加速度的输出

norm(A) %检验输出是否正确

附录二

q1=[cos(10/360*pi);0;0;sin(10/360*pi)];%第一次转动四元数q2=[cos(30/360*pi);sin(30/360*pi);0;0];%第二次转动四元数q3=[cos(-50/360*pi);0;sin(-50/360*pi);0];%第三次转动四元数r=quml(q1,q2);%计算四元数乘积

q=quml(r,q3);

A1=[q(1) q(2) q(3) q(4)

-q(2) q(1) q(4) -q(3)

-q(3) -q(4) q(1) q(2)

-q(4) q(3) -q(2) q(1)]

A2=[q(1) -q(2) -q(3) -q(4)

q(2) q(1) q(4) -q(3)

q(3) -q(4) q(1) q(2)

q(4) q(3) -q(2) q(1)]

A=A1*A2;%

G=A*[0;0;0;9.8] %计算转动后的弹体加速度计的输出

附录三

%已知参数

k=10;

T=1;

Gyro_pulse=0.1/3600/180*pi;

Acc_pulse=9.8/1000000;

R=6368000;

g=9.8;

W_earth=0.00007292;

%初始导航参数和地球参数

Q=zeros(5401,4); %定义变换四元数矩阵

Q(1,:)=[cos((-45/2)/180*pi),0,0,sin((-45/2)/180*pi)]; Longitude=zeros(1,5401); %定义长度为5400的经度数组

Latitude=zeros(1,5401); %定义长度为5400的纬度数组

Height=zeros(1,5401); %定义长度为5400的高度数组

Longitude(1)=122; %初始化经度，纬度，高度

Latitude(1)=35;

Height(1)=25;

%定义速度矩阵，供画图用

Ve=zeros(1,5401); %东向速度

Vn=zeros(1,5401); %北向速度

Vu=zeros(1,5401); %天向速度

X=zeros(1,5401);

%定义加速度矩阵

fe=zeros(1,5400);

fn=zeros(1,5400);

fu=zeros(1,5400);

FE=zeros(1,5400);

FN=zeros(1,5400);

FU=zeros(1,5400);

load gout.mat;

load aout.mat;

for N=1:K %位置、速度迭代

q=zeros(11,4);

q(1,:)=Q(N,:);

for n=1:k %姿态迭代

w=Gyro_pulse*gm((N-1)*10+n,1:3);

w_mod=quatmod([w,0]);

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-w_mod^2/48;

C=1-w_mod^2/8+w_mod^4/384;

q(n+1,:)=((C*I+S*W)*q(n,:)')';

end

Q(N+1,:)=q(n+1,:);

%地理坐标系的转动角速度分量

WE=-Vn(N)/R;

WN=Ve(N)/R+W_earth*cos(Latitude(N)/180*pi);

WU=Ve(N)/R*tan(Latitude(N)/180*pi)+W_earth*sin(Latitude(N)/180*pi );

Gama=[WE,WN,WU]*T; %用地理坐标系的转动四元数

Gama_mod=quatmod([WE,WN,WU,0]);

n=Gama/Gama_mod;

Qg=[cos(Gama_mod/2),sin(Gama_mod/2)*n];

Q(N+1,:)=quatmultiply(quatinv(Qg),Q(N+1,:));

%加速度计测得的比力

fb=Acc_pulse*am(N,1:3);

f1=quatmultiply(Q(N+1,:),[0,fb]);

fg=quatmultiply(f1,quatinv(Q(N+1,:)));

fe(N)=fg(2);

fn(N)=fg(3);

fu(N)=fg(4);

%计算载体的相对加速度

FE(N)=fe(N)+Ve(N)*Vn(N)/R*tan(Latitude(N)/180*pi)-(Ve(N)/R+2*W_ea rth*cos(Latitude(N)/180*pi))*Vu(N)+2*Vn(N)*W_earth*sin(Latitude(N )/180*pi);

FN(N)=fn(N)-2*Ve(N)*W_earth*sin(Latitude(N)/180*pi)-Ve(N)^2/R*tan (Latitude(N)/180*pi)-Vn(N)*Vu(N)/R;

FU(N)=fu(N)+2*Ve(N)*W_earth*cos(Latitude(N)/180*pi)+(Ve(N)^2+Vn(N )^2)/R-g;

%积分计算载体的相对速度

Ve(N+1)=FE(N)*T+Ve(N);

Vn(N+1)=FN(N)*T+Vn(N);

Vu(N+1)=FU(N)*T+Vu(N);

%积分计算载体的位置

Latitude(N+1)=(Vn(N)+Vn(N+1))/2*T/R/pi*180+Latitude(N);

Longitude(N+1)=(Ve(N)+Ve(N+1))/2*T/(R*cos(Latitude(N)/180*pi))/pi *180+Longitude(N);

Height(N+1)=(Vu(N)+Vu(N+1))/2*T+Height(N);

%computing the distance of horizon

Xe=(Ve(N)+Ve(N+1))/2*T;

Xn=(Vn(N)+Vn(N+1))/2*T;

X(N+1)=X(N)+sqrt(Xe^2+Xn^2);

End

%绘制经、纬度变化曲线（以经度为横轴，纬度为纵轴）；figure(1);

xlabel('经度/度');

ylabel('纬度/度');

grid on;

hold on;

plot(Longitude,Latitude);

%绘制高度变化曲线（以时间为横轴，高度为纵轴)

figure(2);

xlabel('时间/秒');

ylabel('高度/米');

grid on;

hold on;

plot((0:5400),Height);

%绘制总水平位移曲线

figure(3);

xlabel('时间/秒');

ylabel('航程/米');

grid on;

hold on;

plot((0:5400),X)