导航原理大作业 哈工大.docx

导航原理大作业 哈工大.docx

《导航原理大作业 哈工大.docx》由会员分享，可在线阅读，更多相关《导航原理大作业 哈工大.docx（14页珍藏版）》请在冰豆网上搜索。

导航原理大作业哈工大

导航原理作业（惯性导航部分）

姓名

班号

学号

成绩

批阅人

Taskdescription

AfighterequippedwithSINSisinitiallyatthepositionof35oNLand122oEL,stationaryonamotionlesscarrier.Threegyros,GX,GY,GZ,andthreeaccelerometers,AX,AY,AZ,areinstalledalongtheaxesXb,Yb,Zbofitsbodyframerespectively.

Case1:

stationaryonboardtest

Thebodyframeofthefighterinitiallycoincideswiththegeographicalframe,asshowninthefigure,withitspitchingaxisXbpointingtotheeast,rollingaxisYbtothenorth,andazimuthaxisZbupward.Thenthebodyofthefighterismadetorotatestepbysteprelativetothegeographicalframe:

（1）10oaroundXb

（2）30oaroundYb

（3）–50oaroundZb

Afterthat,thebodyofthefighterstopsrotating.Youarerequiredtocomputethefinaloutputsofthethreeaccelerometersonthefighter,usingbothdirectioncosinematrixandquaternionrespectively,andignoringthedeviceerrors.Itisknownthatthemagnitudeofgravityaccelerationisg=9.8m/s2.

Case2:

flightnavigation

Initially,thefighterisstationaryonthemotionlesscarrierwithitsboard25mabovethesealevel.Itspitchingandrollingaxesarebothinthelocalhorizon,anditsrollingaxisis45onorthbyeast（北偏东）,parallelwiththerunwayonboard.Thenthefighteracceleratesalongtherunwayandtakesofffromthecarrier.

Theoutputsofthegyrosandaccelerometersarebothpulsenumbers.Eachgyropulseisanangularincrementof0.1arc-sec,andeachaccelerometerpulseis1e-6g,withg=9.8m/s2.Thegyrooutputfrequencyis10Hz,andtheaccelerometer’sis1Hz.Theoutputsofthegyrosandaccelerometerswithin5400sarestoredinMATLABdatafilesnamedgout.matandaout.mat,containingmatricesgmof54000×3andamof5400×3respectively.Theformatofthedataisasshowninthetables,with10rowsofeachmatrixselected.Eachrowrepresentstheoutputsofthetypeofsensorsateachsamplingtime.

TheEarthcanbeseenasanidealsphere,withradius6368.00kmandspinningrate

7.292×10-5rad/s,Theerrorsofthesensorsareignored,soistheeffectofheightonthemagnitudeofgravity.Theoutputsofthegyrosaretobeintegratedevery0.1s.Therotationofthegeographicalframeistobeupdatedevery1s,soarethevelocitiesandpositionsofthefighter.Youarerequiredto:

（1）computethefinalattitudequaternion,longitude,latitude,height,andeast,north,verticalvelocitiesofthefighter.

（2）computethetotalhorizontaldistancetraveledbythefighter.

（3）drawthelatitude-versus-longitudetrajectoryofthefighter,withhorizontallongitudeaxis.

（4）drawthecurveoftheheightofthefighter,withhorizontaltimeaxis.

AXAY

第一种情形：

正对导弹进行地面静态测试（导弹质心相对地面静止）。

初始时刻弹体坐标系和地理坐标系重合，如图所示，弹体的Xb轴指东，Yb轴指北，Zb轴指天。

此后弹体坐标系Xb-Yb-Zb相对地理坐标系的转动如下：

首先，弹体绕Zb（方位轴）转过10度；

接着，弹体绕Xb（俯仰轴）转过30度；

然后，弹体绕Yb（滚动轴）转过-50度；

最后弹体相对地面停止旋转。

请分别用方向余弦矩阵和四元数两种方法计算：

弹体经过三次旋转并停止之后，弹体上三个加速度计Ax,Ay,Az的输出。

取重力加速度的大小g=9.8m/s2。

解答：

（1）四元数法

开始时刻弹体上三个加速度计Ax,Ay,Az的输出为

（0,0，-g）=（0,0,-9.8）.

由四元数的定义结合原题可以得到：

对于第一次旋转：

，

，

第二次旋转：

，

，

第三次旋转：

，

，

在四元数q1,q2和q3中，单位矢量都表示成了映像的形式，所以可按照下式合成:

由四元数的乘法规则在Matlab中编写如下四元数乘法计算程序：

function[q]=qmul（q1,q2）

lm=q1

（1）;

p1=q1

（2）;

p2=q1（3）;

p3=q1（4）;

ML=[lm-p1-p2-p3

p1lm-p3p2

p2p3lm-p1

p3-p2p1lm];

q=ML*q2;

编写新的Matlab程序用于计算四元数的逆：

function[qi]=qinv（q）

qn=norm（q）;

q（2:

4）=-q（2:

4）;

qi=q/qn^2;

方向余弦矩阵

%方向余弦阵

g0=[0;0;9.8];%初始加速度

c1=[100;0cos（10/180*pi）sin（10/180*pi）;0-sin（10/180*pi）cos（10/180*pi）];%第一次绕xb旋转

c2=[cos（30/180*pi）0-sin（30/180*pi）;010;sin（30/180*pi）0cos（30/180*pi）];%第二次绕yb旋转

c3=[cos（-50/180*pi）sin（-50/180*pi）0;-sin（-50/180*pi）cos（-50/180*pi）0;001];%第三次绕zb旋转

c=c3*c2*c1;%合并余弦阵

g=c*g0

写出如下主程序进行整体计算过程：

%四元数

g0=[0009.8]';%四元数表示初始加速度

q1=[cos（10/2/180*pi）sin（10/2/180*pi）00]';%四元数表示第一次转动

q2=[cos（30/2/180*pi）0sin（30/2/180*pi）0]';%四元数表示第二次转动

q3=[cos（-50/2/180*pi）00sin（-50/2/180*pi）]';%四元数表示第三次转动

q=qmul（qmul（q1,q2）,q3）;%合并三次转动

qni=qinv（q）;%求四元数q的逆

g=qmul（qmul（qni,g0）,q）

得到新的坐标系中重力的四元数表示：

g=

-0.0000

-4.4054

-2.6027

8.3581

因此，在经过三次旋转后，弹体上三个加速度计Ax,Ay,Az的输出分别为，

，

，

。

（2）方向余弦矩阵方法

由题可知，开始时刻弹体上三个加速度计Ax,Ay,Az的输出为

（0,0，-g）=（0,0,-9.8）.

第一次旋转时弹体坐标系的改变得到首次旋转的方向余弦矩阵

第二次旋转时弹体坐标系的改变得到首次旋转的方向余弦矩阵

第三次旋转时弹体坐标系的改变得到首次旋转的方向余弦矩阵

因此，在经过三次旋转后，弹体上三个加速度计Ax,Ay,Az的输出，记作

可以表示为：

在经过三次旋转后，弹体上三个加速度计Ax,Ay,Az的输出分别为

。

分析：

对两种计算方法得到的结果进行对比，发现两组数据结果相同。

说明用这两种方法都可以进行正确的计算。

第二种情形：

导弹导航

程序设计

%初始化

we=7.292e-5;%地球自转角速度

Q=zeros（4,5401）;%初始化四元数矩阵

Q（:

1）=[cos（45/2/180*pi）;0;0;sin（45/2/180*pi）];%初始四元数

R=6368000;%地球半径

Longitude=zeros（1,5401）;

Longitude

（1）=122;%初始经度

Latitude=zeros（1,5401）;

Latitude

（1）=35;%初始纬度

h=zeros（1,5401）;

h

（1）=25;%初始高度

g=9.8;%重力加速度

VE=zeros（1,5401）;

VE

（1）=0;%初始东向速度

VN=zeros（1,5401）;

VN

（1）=0;%初始北向速度

VT=zeros（1,5401）;

VT

（1）=0;%初始垂直速度

t=0.1;%姿态计算周期

k=10;

T=k*t;%位、速计算周期

K=5400;

loadgout.mat;%数据读入

loadaout.mat;%数据读入

forN=1:

K%姿态子迭代

Q1=zeros（4,11）;%用于1秒内四元数更新

Q1（:

1）=Q（:

N）;%赋初值

forn=1:

k

w=0.1/3600/180*pi*gm（（N-1）*10+n,:

）;%角度增量

w=w';

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）;

WT=VE（N）/R*tan（Latitude（N）/180*pi）+we*sin（Latitude（N）/180*pi）;

%用地理坐标系的转动四元数修正载体姿态四元数

gama=[WE,WN,WT]'*T;

normgama=norm（gama）;

n=gama/normgama;

Qg=[cos（normgama/2）;sin（normgama/2）*n];%地理坐标系四元数

Q（:

N+1）=qmul（qinv（Qg）,Q（:

N+1））;%姿态四元数修正

%加速度计测得的比力

fx=1e-6*9.8*am（N,1）;

fy=1e-6*9.8*am（N,2）;

fz=1e-6*9.8*am（N,3）;

fb=[fxfyfz]';%加速度计比力向量

%加速度计比力从载体坐标系到地里坐标系

f=qmul（Q（:

N+1）,qmul（[0;fb],qinv（Q（:

N+1））））;

fe（N）=f

（2）;fn（N）=f（3）;ft（N）=f（4）;

%计算载体的相对加速度

d_VE（N）=fe（N）+VE（N）*VN（N）/R*tan（Latitude（N）/180*pi）-（VE（N）/R+2*we*cos（Latitude（N）/180*pi））*VT（N）+2*VN（N）*we*sin（Latitude（N）/180*pi）;

d_VN（N）=fn（N）-2*VE（N）*we*sin（Latitude（N）/180*pi）-VE（N）*VE（N）/R*tan（Latitude（N）/180*pi）-VN（N）*VT（N）/R;

d_VT（N）=ft（N）+2*VE（N）*we*cos（Latitude（N）/180*pi）+（VE（N）^2+VN（N）^2）/R-g;

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

VE（N+1）=d_VE（N）*T+VE（N）;

VN（N+1）=d_VN（N）*T+VN（N）;

VT（N+1）=d_VT（N）*T+VT（N）;

%位置更新

Longitude（N+1）=VE（N）/（R*cos（Latitude（N）/180*pi））*T/pi*180+Longitude（N）;

Latitude（N+1）=VN（N）/R*T/pi*180+Latitude（N）;

h（N+1）=VT（N）*T+h（N）;

end

%画经纬度曲线

figure

（1）

xlabel（'经度/度'）

ylabel（'纬度/度'）

holdon

plot（Longitude,Latitude）;

%画高度曲线

figure

（2）

xlabel（'时间/秒'）

ylabel（'高度/米'）

holdon

plot（[0:

5400],h）;

%显示200秒弹体到达的经纬度和高度

Longitude=Longitude（5401）

Latitude=Latitude（5401）

Height=h（5401）

%显示200秒后弹体到达的东向速度和北向速度

VE=VE（5401）

VN=VN（5401）

%显示200秒后弹体相对当地地理坐标系的姿态四元数

Quaternion=Q（:

5401）

运行结果：

Longitude=122.8785

Latitude=35.0691

Height=-604.1142

Distance=2.3820e+006

VE=-393.4743

VN=268.0711

VT=-5.4673

Quaternion=

-0.8750

-0.0054

-0.0023

-0.4841

解答：

（1）结果：

弹体经纬度——东经122.8757度，北纬35.0691度，飞行高度为

，东向速度

，北向速度

，垂直速度

；

（2）totalhorizontaldistancetraveledbythefighter

Distance=2.3820e+006，即2382公里。

（3）latitude-versus-longitudetrajectoryofthefighter

（4）thecurveoftheheightofthefighter

对导弹高度图形的分析

在0~2000秒的时间内，导弹的高度随时间的变化呈先上升而后波动趋势，最高达到约9000米；在2000~3500秒的时间内，导弹的高度随时间上升幅度较小且比较稳定；随后在3500~4500秒内其高度发生了一次较为剧烈的波动，最高达到了18000米，随后又下降至低于2000米；4500秒之后，其高度经过两次波动后达到最低点约为-600米，这时的状态很可能是在水下。

遇到的问题及体会：

这次的作业，让我对方向余弦矩阵和四元数法有了更系统和全面的了解，重新复习了老师上课时讲授的知识，同时也了解到了它们在工程实际中应用的方法。

在解决问题的过程中，对于我而言最大的难题在于编程。

由于对matlab的使用十分不熟练，所以请教了一些同学，也上网搜索到一些关于matlab的教程，这个过程对于我而言远远比一个结果来得更加有意义。

在程序方面，很诚实的说，这个程序并不是我一个人的劳动成果，而是和班里几个同学共同商量得出的结果，请老师多多谅解。

其次是对于老师所给的迭代计算流程图，开始我并不太理解其含义，也并不明白应该如何代入数据。

也是通过看相关书籍，结合老师上课讲的，以及和同学们之间的互相交流，最终才完成了本次作业。