第二次实验报告.docx

第二次实验报告.docx

《第二次实验报告.docx》由会员分享，可在线阅读，更多相关《第二次实验报告.docx（12页珍藏版）》请在冰豆网上搜索。

第二次实验报告

《惯性导航原理》

第二次大作业

姓名：

ff

学号：

SY

邮箱：

w

一、题目分析

本题已知的信息是陀螺仪和加速度计在各个时刻的采样值，以及初始经纬度、高度、姿态角和速度值。

需要求解指北方位捷联系统的运动轨迹和姿态角变化。

在利用方向余弦方法对惯性导航系统进行测算时，刚体空间位置用固连于刚体的动坐标系对固定参考系各轴的九个方向余弦来确定，九个方向余弦角存在六个约束条件，计算比较繁琐，模型也比较复杂。

如果在计算过程中引入四元数，则可以通过坐标系的一次转动，实现方向余弦方法中的三次坐标旋转。

原理图如下：

从原理图可以清楚的看出，通过捷联姿态矩阵

可以将任意姿态的平台坐标系下的比力数据转换到地理坐标系下，然后通过指北方位平台式惯导解算的方法即可以得到任意时刻载体的位置和速度信息。

关键在于捷联姿态矩阵的求解。

在这里应用四元数知识进行解算。

二、程序流程框图

根据已知信息以及捷联式惯导系统的基本力学编排方程可知基本求解过程如下：

第一步，先根据初始的姿态角确定初始的四元数值，进而可以列写出本体系相对于导航系的方向余弦矩阵，然后将加速度计的测量信息经过余弦阵由导航系变换到本体系中。

第二步，根据变换后的比力信息和初始的经纬度、高度和速度值进行指北方位系统的运动解算，可以求出速度信息、指令角速度信息和位置信息。

第三步，根据第二步中求解出来的指令角速度信息，对其经过余弦矩阵变换到本体系中，陀螺仪测量到的信息和变换后的指令角速度相减得到本体系相对于导航系的角速度在本体系中的分解。

第四步，根据四元数的运动学微分方程求解出下一时刻的四元数，根据四元数和欧拉角之间的关系求解出新的姿态角。

接着进行以上两步的运算。

经过以上几步就可以对指北方位捷联惯导系统进行解算，详细流程如下图所示:

三、导航结果

1.系统位置坐标曲线图

2.系统速度随时间变化曲线图

3.系统俯仰角随时间变化曲线图

4.系统滚转角随时间变化曲线图

5.系统航向角随时间变化曲线图

6．系统纬度、经度、东向速度、北向速度的终点值

经度（度）

纬度（度）

东向速度（m/s）

北向速度（m/s）

116.3027

40.0080

0.1741

9.9944

四、现阶段学习小结

根据自己的惯导专业方面的知识基础，在这几个周中我对书中的推导公式进行了自行推导学习，同时在老师讲解的时候注重观察老师在推导公式的时候侧重点在哪里，以及对公式的推导需要注意的问题和各种技巧。

经过最近的学习，在老师讲解公式推导的时候能够很好的进行把握。

通过作业的练习，对书中公式的应用有了深刻的认识，加深了印象，对工程应用中惯导系统工作原理机制有了初步的认识。

此外，对书中理论的推导有了系统的认识。

该课程的学习需要扎实的力学、数学功底，这方面通过学习，必须融会贯通。

五、源程序

clc

closeall

%定义常量

dt=0.01;

total=60001;

Re=6378245;%地球椭球长半径

wie=7.292115147e-5;%地球自转角速度

e=1/298.3;%偏心率

pi=3.141592654;

g0=9.7803267714;

gk1=0.00193185138639;

gk2=0.00669437999013;

%定义初始值

lamda

（1）=116.344695283*pi/180;%经度

L

（1）=39.975172*pi/180;%纬度

h=30;%高度

v0=[-9.993908270;0.000000000;0.348994967];%初速度

att0=[2190]*pi/180;%俯仰，横滚，航向,单位为度

%加载数据

xls=load（'C:

\Users\blxh\Desktop\第二次作业程序资料\jlfw.mat'）;

f=xls.f_INSc;%f（m,n）f（3,60001）

wib=xls.wib_INSc;%wib（m,n）wib（3,60001）弧度每秒

Vx

（1）=v0

（1）;

Vy

（1）=v0

（2）;

Vz

（1）=v0（3）;

q0=cos（att0（3）/2）*cos（att0

（1）/2）*cos（att0

（2）/2）-sin（att0（3）/2）*sin（att0

（1）/2）*sin（att0

（2）/2）;%初始四元数

q1=cos（att0（3）/2）*sin（att0

（1）/2）*cos（att0

（2）/2）-sin（att0（3）/2）*cos（att0

（1）/2）*sin（att0

（2）/2）;

q2=cos（att0（3）/2）*cos（att0

（1）/2）*sin（att0

（2）/2）+sin（att0（3）/2）*sin（att0

（1）/2）*cos（att0

（2）/2）;

q3=cos（att0（3）/2）*sin（att0

（1）/2）*sin（att0

（2）/2）+sin（att0（3）/2）*cos（att0

（1）/2）*cos（att0

（2）/2）;

Q=[q0,q1,q2,q3]';

Atb=[q0^2+q1^2-q2^2-q3^2,2*（q1*q2+q0*q3）,2*（q1*q3-q0*q2）;...

2*（q1*q2-q0*q3）,q0^2-q1^2+q2^2-q3^2,2*（q2*q3+q0*q1）;...

2*（q1*q3+q0*q2）,2*（q2*q3-q0*q1）,q0^2-q1^2-q2^2+q3^2...

];%ctb

%计算流程

fori=1:

total

Rx（i）=（1/Re）*（1-e*sin（L（i））*sin（L（i）））;%此处Rx=1/Rxt

Ry（i）=（1/Re）*（1+2*e-3*e*sin（L（i））*sin（L（i）））;%此处Ry=1/Ryt

c33=sin（L（i））;

g=g0*（1+gk1*c33^2）*（1-2*h/Re）/sqrt（1-gk2*c33^2）;%重力加速度g

%计算Wtbb

Witt=[-Vy（i）*Ry（i）;wie*cos（L（i））+Vx（i）*Rx（i）;wie*sin（L（i））+Vx（i）*Rx（i）*tan（L（i））];

Witb=Atb*Witt;%watch

Wibb=wib（:

i）;%载入已知wib

Wtbb=Wibb-Witb;

theat=（Wtbb）*dt;%角增量

d_theat=theat

（1）^2+theat

（2）^2+theat（3）^2;

dt_theat=[0,-theat

（1）,-theat

（2）,-theat（3）;...

theat

（1）,0,theat（3）,-theat

（2）;...

theat

（2）,-theat（3）,0,theat

（1）;...

theat（3）,theat

（2）,-theat

（1）,0...

];

Q=（（1-d_theat/8-d_theat^2/384）*eye（4）+（0.5-d_theat/48）*dt_theat）*Q;

%Q=（（1-d_theat/8）*eye（4）+（0.5-d_theat/48）*dt_theat）*Q;

q0=Q（1,1）;q1=Q（2,1）;q2=Q（3,1）;q3=Q（4,1）;

Atb=[q0^2+q1^2-q2^2-q3^2,2*（q1*q2+q0*q3）,2*（q1*q3-q0*q2）;...

2*（q1*q2-q0*q3）,q0^2-q1^2+q2^2-q3^2,2*（q2*q3+q0*q1）;...

2*（q1*q3+q0*q2）,2*（q2*q3-q0*q1）,q0^2-q1^2-q2^2+q3^2...

];%ctb

%姿态角提取

att_theat（i）=asin（Atb（2,3））;%俯仰

att_gama1=atan（-Atb（1,3）/Atb（3,3））;%滚转

ifAtb（3,3）>0

att_gama（i）=att_gama1;

elseifatt_gama1<0

att_gama（i）=att_gama1+pi;

else

att_gama（i）=att_gama1-pi;

end

end

att_fe1=atan（-Atb（2,1）/Atb（2,2））;%偏航逆时针为正

ifAtb（2,2）<0

att_fe（i）=att_fe1+pi;

elseifatt_fe1>0

att_fe（i）=att_fe1;

else

att_fe（i）=att_fe1+2*pi;

end

end

ft（:

i）=inv（Atb）*f（:

i）;%系转换

%计算速度

Vx（i+1）=Vx（i）+（ft（1,i）+（2*wie*sin（L（i））+Vx（i）*tan（L（i））*Rx（i））*Vy（i）-（2*wie*cos（L（i））+Vx（i）*Rx（i））*Vz（i））*dt;

Vy（i+1）=Vy（i）+（ft（2,i）-（2*wie*sin（L（i））+Vx（i）*tan（L（i））*Rx（i））*Vx（i）+（Vy（i）*Ry（i））*Vz（i））*dt;

Vz（i+1）=Vz（i）+（ft（3,i）+（2*wie*cos（L（i））+Vx（i）*Rx（i））*Vx（i）+（Vy（i）*Ry（i））*Vy（i）-g）*dt;

%计算经纬度

L（i+1）=L（i）+（Vy（i）*Ry（i））*dt;

lamda（i+1）=lamda（i）+（Vx（i）*Rx（i）/cos（L（i）））*dt;

end

disp（'终点经度为：

'）

L（60001）*180/pi

disp（'终点纬度为：

'）

lamda（60001）*180/pi

disp（'东向速度为：

'）

Vx（60001）

disp（'北向速度为：

'）

Vy（60001）

%画图

figure

（1）;

plot（lamda*180/pi,L*180/pi）

text（lamda（60001）*180/pi,L（60001）*180/pi,[num2str（lamda（60001）*180/pi,'终点经纬度坐标\n（%.3f'）,',',num2str（L（60001）*180/pi,'%.3f'）,'）'],'EdgeColor','red','BackgroundColor',[.7.9.7],'VerticalAlignment','bottom'）

gridon

title（'位置变化'）

xlabel（'经度lamda/°'）

ylabel（'纬度L/°'）

t=0:

0.01:

600.01;

figure

（2）;

plot（t,Vx,'-'）

title（'速度变化'）

holdon

plot（t,Vy,':

'）

xlabel（'时间/秒'）,ylabel（'速度/m/s'）;

gridon

legend（'东向速度','北向速度',0）

holdoff

t=0:

0.01:

600;

figure（3）;

plot（t,att_theat*180/pi,'r'）

title（'俯仰角度变化'）

xlabel（'时间/秒'）,ylabel（'俯仰/度'）;

gridon

figure（4）;

plot（t,att_gama*180/pi,'g'）

title（'滚转角度变化'）

xlabel（'时间/秒'）,ylabel（'滚转/度'）;

gridon

figure（5）;

plot（t,att_fe*180/pi,'b'）

title（'偏航角度变化/度'）

xlabel（'时间/秒'）,ylabel（'偏航/度'）;

gridon