哈工大四系导航原理 大作业 INS部分.docx
《哈工大四系导航原理 大作业 INS部分.docx》由会员分享，可在线阅读，更多相关《哈工大四系导航原理 大作业 INS部分.docx（17页珍藏版）》请在冰豆网上搜索。
哈工大四系导航原理大作业INS部分
Assignmentof
PrinciplesofNavigation
《导航原理》作业
（惯性导航部分）
MyName
MyClass
1104105
StudentNo.
目录：
一、题目内容1
二、解答3
1.第一种情形3
1.方向余弦法3
2.四元数法4
2.第二种情形6
三、对计算与仿真结果的分析与心得12
一、题目内容
一架战斗机采用捷联惯性导航系统，三个速率陀螺仪
和三个加速度计
的敏感轴分别沿着着战斗机（载体）坐标系的
轴。
初始时刻该战斗机处在北纬35度，东经122度。
第一种情形：
对战斗机进行地面静态测试
初始时刻战斗机坐标系和地理坐标系重合，如图所示，战斗机的
轴指东，
轴指北，
轴指天。
此后战斗机坐标系相对地理坐标系的转动如下：
（1）战斗机绕
（俯仰轴）转过10度；
（2）战斗机绕
（滚动轴）转过30度；
（3）战斗机绕
（方位轴）转过-50度；
最后战斗机相对地面停止旋转。
east
请分别用方向余弦矩阵和四元数两种方法计算：
战斗机经过三次旋转并停止之后，战斗机上三个加速度计
的输出。
忽略设备误差。
取重力加速度的大小g=9.8
。
第二种情形：
战斗机正在飞行中
初始时刻战斗机坐标系滚动轴为北偏东45度，并且与跑道平行，俯仰轴与滚动轴都在水平平面内；且战斗机初始高度25m，初始北向、东向速度和垂直速度都为零。
然后沿跑道加速并开始起飞。
陀螺仪和加速度计都以脉冲数形式输出，陀螺输出的每个脉冲代表大小为0.1角秒的角增量。
加速度计输出的每个脉冲代表
g，g=9.8
。
陀螺仪的输出频率为10Hz；加速度计的输出频率为1Hz。
在5400秒内三个陀螺仪和三个加速度计的输出存在了数据文件gout.mat和aout.mat中，各含一矩阵变量gm，am，其中gm有54000行、3列；am有5400行、3列。
每一行中的数据各代表每个采样时刻三个陀螺
和三个加速度计
的输出的脉冲数。
形式如下图所示：
将地球视为理想的球体，半径6368.00公里，自转角速率为
。
不考虑仪表误差，也不考虑战斗机高度对重力加速度的影响。
选取战斗机的姿态计算周期为0.1秒，速度和位置的计算周期为1秒。
（1）计算最终战斗机的姿态四元数，战斗机最终到达的经纬度和高度，东向、北向和垂直速度；
（2）计算最终战斗机总共的水平移动距离；
（3）绘制出总体战斗机的经、纬度变化曲线（以经度为横轴）；
（4）绘制出总体战斗机的高度变化曲线（以时间为横轴）。
2、解答
1.第一种情形
1.方向余弦法
方向余弦矩阵表达式为：
由于
均为单位向量，求内积的结果实际上大小即为对应的余弦值。
若要求转动前相对转动后的方向余弦矩阵，则对上式求逆即可。
1.绕X轴转过10度
方向余弦矩阵为：
2.绕Y轴转过30度
方向余弦矩阵为：
3.绕Z轴转过-50度
方向余弦矩阵为：
把g写成沿k轴向量的形式，即：
那么三次旋转过后的g在新坐标轴的分量即为：
通过计算得到：
对A取模发现结果仍然为9.8，说明结果正确。
MATLAB代码如下：
%======第一次转动======%
C1=[100
0cos（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）
010
sin（30/180*pi）0cos（30/180*pi）];
%======第三次转动======%
C3=[cos（-50/180*pi）sin（-50/180*pi）0
-sin（-50/180*pi）cos（-50/180*pi）0
001];
%======计算三次转动后战斗机的姿态======%
A=C3*C2*C1*[0;0;9.8]
%======检验转动后的合成矢量是否与转动前相同======%
norm（A）
2.四元数法
1.绕X轴转过10度
得到第一次旋转四元数：
2.绕Y轴转过30度
得到第二次旋转四元数：
3.绕Z轴转过-50度
得到第三次旋转四元数：
由于采用的映像形式，所以直接通过左乘得到四元数乘积：
由四元数坐标系旋转的公式：
计算得到G在三次旋转之后的坐标系中的四元数为：
计算A得范数为9.8，证明计算正确。
与方向余弦算法比较，得到了相同的结果。
MATLAB代码如下：
%======第一次转动======%
q1=[cos（10/360*pi）sin（10/360*pi）00];
%======第二次转动======%
q2=[cos（30/360*pi）0sin（30/360*pi）0];
%======第三次转动======%
q3=[cos（-50/360*pi）00sin（-50/360*pi）];
%利用MATLAB自带的四元数相乘函数计算四元数乘积。
（四元数写成横向量形式）
r=quatmultiply（q1,q2）;
q=quatmultiply（r,q3）;
G=[0009.8];
N=[q
（1）q
（2）q（3）q（4）];
%利用MATLAB自带的四元数求逆函数计算四元数乘积。
（四元数写成横向量形式）
B=quatinv（N）;
%利用乘积函数计算坐标系转动的四元数坐标系变换结果。
M=quatmultiply（B,G）;A=quatmultiply（M,N）;A=A'
%======检验转动后的合成矢量是否与转动前相同======%
norm（A）
2.第二种情形
对飞行中战斗机的姿态进行解算，流程可由下图确定，其中NavtimeT的取值为5400s。
MATLAB代码如下：
先定义两个四元数操作函数：
1四元数求逆函数.qiuni
function[qi]=qiuni（q）
qn=norm（q）;
q（2:
4）=-q（2:
4）;
qi=q/qn^2;
End
2.四元数乘积函数qmul
function[q]=quml（q1,q2）
lm=q1
（1）;p1=q1
（2）;p2=q1（3）;p3=q1（4）;
q=[lm-p1-p2-p3;
p1lm-p3p2;
p2p3lm-p1;
p3-p2p1lm]*q2;
End
3.第二种情况计算用MATLAB程序如下：
%======已知参数======%
k=10;%姿态迭代次数n
T=1;%△T为1s
K=5400;%导航时间5400s
R=6368000;%地球半径
We=7.292e-5;%地球自转角速度
%======初始导航参数和地球参数======%
Q=zeros（4,5401）;%定义变换四元数矩阵
Longitude=zeros（1,5401）;%定义长度为5400的经度数组
Latitude=zeros（1,5401）;%定义长度为5400的纬度数组
H=zeros（1,5401）;%定义长度为5400的高度数组
Q（:
1）=[cos（22.5/180*pi）;0;0;-sin（22.5/180*pi）];%初始化四元数，由北偏东45度计算得，注意是惯性系相对载体
Longitude
（1）=122;%初始化经度
Latitude
（1）=35;%初始化纬度
H
（1）=25;%初始化高度
g=9.8;
%======定义速度矩阵，供画图用======%
Ve=zeros（1,5401）;%东向速度
Vn=zeros（1,5401）;%北向速度
Vu=zeros（1,5401）;%天向速度
Ve
（1）=0;%初始速度为0
Vn
（1）=0;
Vu
（1）=0;
%======定义加速度矩阵======%
FE=zeros（1,5401）;%东向加速度
FN=zeros（1,5401）;%北向加速度
FU=zeros（1,5401）;%天向加速度
loadgout.mat%导入数据
loadaout.mat
forN=1:
K%开始位置、速度迭代
q1=zeros（4,11）;
q1（:
1）=Q（:
N）;
forn=1:
k%开始姿态迭代
WX=（（0.1/3600）/180*pi）*gm（（N-1）*10+n,1）;%0.1角秒换算为弧度制进行迭代
WY=（（0.1/3600）/180*pi）*gm（（N-1）*10+n,2）;
WZ=（（0.1/3600）/180*pi）*gm（（N-1）*10+n,3）;
%======求解四元数的微分方程（DCM）======%
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/8normw^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）=quml（qiuni（QG）,Q（:
N+1））;
FX=1e-6*9.8*am（N*1,1）;%加速度计测得的比力
FY=1e-6*9.8*am（N*1,2）;
FZ=1e-6*9.8*am（N*1,3）;
Fb=[FXFYFZ]';
%======将比力从载体坐标系分解到地理坐标系======%
F=quml（Q（:
N+1）,quml（[0;Fb],qiuni（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-g;
%======计算载体的相对速度======%
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（Flatitude（N）/180*pi）*T/pi*180+Longitude（N）;
Flatitude（N+1）=0.5*（Vn（N）+Vn（N+1））/R*T/pi*180+Flatitude（N）;
H（N+1）=0.5*（Vu（N）+Vu（N+1））*T+H（N）;
end
%======画图部分======%
%======绘制战斗机的经、纬度变化曲线（以经度为横轴）======%
figure
（1）
holdon
gridon
plot（Longitude,Latitude,'LineWidth',2）;
xlabel（'longitude'）;
ylabel（'latitude'）;
title（'latitude-versus-longitudetrajectory'）;
%======绘制战斗机的高度变化曲线（以时间为横轴）======%
figure
（2）
holdon
gridon
plot（[0:
5400],H,'LineWidth',2）;
xlabel（'time'）;
ylabel（'height'）;
title（'thecurveoftheheight'）;
%======输出部分======%
str1='finallongitude:
';
fprintf（1,'%s%f\n',str1,Longitude（5401））;
str2='finallatitude:
';
fprintf（1,'%s%f\n',str2,Latitude（5401））;
str3='finalheight:
';
fprintf（1,'%s%f\n',str3,H（5401））;
str4='finalvelocityofeast:
';
fprintf（1,'%s%f\n',str4,Ve（5401））;
str5='finalvelocityofnorth:
';
fprintf（1,'%s%f\n',str5,Vn（5401））;
Str6='finalvelocityofvertical:
';
fprintf（1,'%s%f\n',str6,Vu（5401））;
Str7='finalattitudequaternion:
';
fprintf（1,'%s\n',str7）;
Q=Q（:
5401）
Str8='totalhorizontaldistance：
';
fprintf（1,'%s\n',str8）;
%======计算战斗机总共飞行过距离======%
DIS=0;%初始距离为零
fora=1:
5401%总体时间
DIS=DIS+（（（Ve（a））^2+（0+Vn（a））^2+（Vu（a））^2）^0.5*1）;%对三个方向速度平方开根号求合成速度与采样时间相乘得到采样时间内飞行距离
End
DIS=DIS+（（（Ve（5401））^2+（0+Vn（5401））^2+（Vu（5401））^2）^0.5*1）%对每段采样时间内的距离求和得到总体距离
最终输出结果为：
finallongitude:
122.875673
finallatitude:
35.069422
finalheight:
24.806096
finalvelocityofeast:
-393.771553
finalvelocityofnorth:
267.561194
finalvelocityofvertical:
-3.158305
finalattitudequaternion:
Q=
-0.8750
-0.0054
-0.0023
-0.4841
totalhorizontaldistance：
DIS=
2.3852e+06
latitude-versus-longitudetrajectory：
thecurveoftheheight：
3、对计算与仿真结果的分析与心得
1.对情形一的结果分析：
经过三次旋转之后，得到新的坐标系下，重力加速度在三个坐标轴方向上的分量，经过求模（范数），得到与旋转之前一致的结果。
在解决这种情形时，应该注意应该求取地理坐标系相对于载体坐标系的方向余弦矩阵和四元数，也就是求取载体相对于地理坐标系的方向余弦矩阵或四元数的逆，在这里曾经出现过错误；
这两段程序基本由自己独立完成，在利用MATLAB自带的函数quatinv（求逆）和quatconj（求共轭）以及quatmultiply（求乘积）求四元数相关运算的时候要注意写成[a1a2a3a4]的形式，不能缺少元素或者写成列矩阵的形式。
2.对情形二的结果分析：
情形二的程序流程与课堂讲授的惯性导航姿态解算流程完全相符，首先利用陀螺仪的输出量解四元数微分方程（DCM），再利用地球自转角速率对载体姿态进行修正，最后利用加速度计的输出计算比力进而计算载体相对地理坐标系的加速度，对载体的速度和位置进行迭代解算，得到结果。
在编写这段程序时借鉴了往年学长的程序，细致分析理解后进行了适当的应用并加以修改（对程序的理解已经标注在程序旁）。
运行后得到了仿真结果，发现战斗机运行后最终基本回到原位置，从经度-纬度图线以及高度-时间曲线上也不难看出这一点。
在计算水平距离时一开始计算成了始末两点之间的直线距离（由首末两点经纬度计算得到），后又更改为战斗机总共经过的曲线路程。
3.心得体会
这次作业让我对捷联惯导系统载体姿态解算算法有了更加深刻的认识，对解算的流程有了新的理解。
在完成作业的过程中，发现问题，不借助老师的力量而独立查找资料、阅读资料进而解决问题，使我不但加深了对课程学习知识的理解，而且锻炼了我自主动手、解决问题的能力，使我获益良多。
在复习巩固课堂所学知识的同时，也再次对MATLAB仿真、计算功能进行了更深的探索，对许多新的函数功能有了基本的认识，为以后的深入学习打下了基础。
升级会员 