北航惯性导航作业二综述.docx
《北航惯性导航作业二综述.docx》由会员分享，可在线阅读，更多相关《北航惯性导航作业二综述.docx（14页珍藏版）》请在冰点文库上搜索。
北航惯性导航作业二综述
惯性导航作业
一、数据说明：
1：
惯导系统为指北方位的捷连系统。
初始经度为116.344695283度、纬度为39.975172度,高度h为30米。
初速度v0=[-9.993908270;0.000000000;0.348994967]。
2：
jlfw中为600秒的数据，陀螺仪和加速度计采样周期分别为为1/100秒和1/100秒。
3：
初始姿态角为[2190]（俯仰，横滚，航向,单位为度）,jlfw.mat中保存的为比力信息f_INSc（单位m/s^2）、陀螺仪角速率信息wib_INSc（单位rad/s），排列顺序为一~三行分别为X、Y、Z向信息.
4:
航向角以逆时针为正。
5:
地球椭球长半径re=6378245;地球自转角速度wie=7.292115147e-5;重力加速度g=g0*（1+gk1*c33^2）*（1-2*h/re）/sqrt（1-gk2*c33^2）；g0=9.7803267714;gk1=0.00193185138639;gk2=0.00669437999013;c33=sin（lat纬度）;
二、作业要求：
1：
可使用MATLAB语言编程，用MATLAB编程时可使用如下形式的语句读取数据：
loadD:
\...文件路径...\jlfw,便可得到比力信息和陀螺仪角速率信息。
用角增量法。
2：
（1）以系统经度为横轴，纬度为纵轴（单位均要转换为：
度）做出系统位置曲线图；
（2）做出系统东向速度和北向速度随时间变化曲线图（速度单位：
m/s，时间单位：
s）；
（3）分别做出系统姿态角随时间变化曲线图（俯仰，横滚，航向,单位转换为：
度，时间单位：
s）；
以上结果均要附在作业报告中。
3：
在作业报告中要写出“程序流程图、现阶段学习小结”，写明联系方式。
（注意程序流程图不是课本上的惯导解算流程，而是你程序分为哪几个模块、是怎样一步步执行的,什么位置循环等,让别人根据该流程图能够编出相应程序）
（学习小结按条写，不用写套话）
4：
作业以纸质报告形式提交，附源程序。
三、基本原理和公式
1、初始姿态矩阵的确定：
根据初始姿态角求四元数：
再根据四元数求方向余弦矩阵的初始矩阵：
2、指北方位系统的运动解算：
“平台”指令角速度为：
加速度计获得的比力信息
为载体坐标系中各个轴向的比力，而我们需要的比力
为地理坐标系中各个轴向的比力，它们之间应用矩阵
做变换：
根据比力信息可以求出各个方向上的加速度：
因此可以求得速度为：
载体所在位置的地理纬度L、经度
可由下列方程求得：
3、四元数姿态矩阵的更新：
式中，
为陀螺所测得的角速度。
用毕卡逼近法更新
的值，T为采样时间
4、姿态角的求解：
姿态角与姿态矩阵的关系：
式中
分别为俯仰角，滚转角和偏航角，以逆时针为正方向，而课本上是以顺时针为正，因此需要对课本上的公式进行修改，将
代入原公式可得现公式。
如果记
则由以上两式即可解算出姿态角：
四、程序流程图
五、结果
六、小结
这次作业是捷联惯导的解算，是利用上次作业的结果对数据进行处理。
和上次不同，这次遇到了较多的问题。
首先，对捷联惯导的基本原理理解的不够深刻，比如坐标系的转换，四元数微分方程的求解。
其次，由于课本的姿态角是以顺时针为正，而原始数据是以逆时针为正，因此需要对书上的公式进行修改，在这个过程中就出现了许多问题，比如正负号问题。
总之，这次作业弥补了学习上的不足，使我对基本原理理解更为深刻，也初步了解惯导的基本操作。
七、程序
clc
clear
a=load（'C:
\Users\Administrator\Documents\MATLAB/jlfw.dat'）;
Wib_INSc=a（:
2:
4）';f_INSc=a（:
5:
7）';
%第一列：
数据包序号第二至四列：
分别为东、北、天向陀螺仪角速率信息wib_INSc（单位：
rad/s）
%第五至七列：
分别为东、北、天向比力信息f_INSc（单位：
m/s^2）.
L（1,:
）=zeros（1,60001）;
Lambda（1,:
）=zeros（1,60001）;
Vx（1,:
）=zeros（1,60001）;
Vy（1,:
）=zeros（1,60001）;
Vz（1,:
）=zeros（1,60001）;
Rx（1,:
）=zeros（1,60001）;%定义存放卯酉圈曲率半径数据的矩阵
Ry（1,:
）=zeros（1,60001）;%定义存放子午圈曲率半径数据的矩阵
psi（1,:
）=zeros（1,60001）;%定义存放偏航角数据的矩阵
theta（1,:
）=zeros（1,60001）;%定义存放俯仰角数据的矩阵
gamma（1,:
）=zeros（1,60001）;%定义存放滚转角数据的矩阵
I=eye（4）;%定义四阶矩阵
L（1,1）=39.975172/180*pi;%纬度初始值单位：
弧度
Lambda（1,1）=116.344695283/180*pi;%经度初始值单位：
弧度
Vx（1,1）=-9.993908270;%初始速度x方向分量
Vy（1,1）=0;%初始速度y方向分量
Vz（1,1）=0.348994967;%初始速度z方向分量
Wibx（1,:
）=a（:
2）;%提取陀螺正东方向角速率并定义
Wiby（1,:
）=a（:
3）;%提取陀螺正北方向角速率并定义
Wibz（1,:
）=a（:
4）;%提取陀螺天向角速率并定义
fibbx（1,:
）=a（:
5）;%x方向的比力数据
fibby（1,:
）=a（:
6）;%y方向的比力数据
fibbz（1,:
）=a（:
7）;%z方向的比力数据
g0=9.7803267714;
Wie=7.292115147e-5;%地球自转角速度
Re=6378245;%长半径
e=1/298.3;%椭圆度
t=0.01;%采样时间
psi（1,1）=90/180*pi;%偏航角初始值单位：
弧度
theta（1,1）=2/180*pi;%俯仰角初始值单位：
弧度
gamma（1,1）=1/180*pi;%滚转角初始值单位：
弧度
gk1=0.00193185138639;
gk2=0.00669437999013;
H=30;%高度
%求解四元数系数q0,q1,q2,q3的初值
q（1,1）=cos（psi（1,1）/2）*cos（theta（1,1）/2）*cos（gamma（1,1）/2）-sin（psi（1,1）/2）*sin（theta（1,1）/2）*sin（gamma（1,1）/2）;%q0
q（2,1）=cos（psi（1,1）/2）*sin（theta（1,1）/2）*cos（gamma（1,1）/2）-sin（psi（1,1）/2）*cos（theta（1,1）/2）*sin（gamma（1,1）/2）;%q1
q（3,1）=cos（psi（1,1）/2）*cos（theta（1,1）/2）*sin（gamma（1,1）/2）+sin（psi（1,1）/2）*sin（theta（1,1）/2）*cos（gamma（1,1）/2）;%q2
q（4,1）=cos（psi（1,1）/2）*sin（theta（1,1）/2）*sin（gamma（1,1）/2）+sin（psi（1,1）/2）*cos（theta（1,1）/2）*cos（gamma（1,1）/2）;%q3
fori=1:
60000
g=g0*（1+gk1*sin（L（i）^2）*（1-2*H/Re）/sqrt（1-gk2*sin（L（i）^2）））;%计算重力加速度
Rx（i）=Re/（1-e*（sin（L（i）））^2）;%根据纬度计算卯酉圈曲率半径
Ry（i）=Re/（1+2*e-3*e*（sin（L（i）））^2）;%根据纬度计算子午圈曲率半径
%求解四元数姿态矩阵
q0=q（1,i）;q1=q（2,i）;q2=q（3,i）;q3=q（4,i）;
Ctb=[q0^2+q1^2-q2^2-q3^2,2*（q1*q2+q0*q3）,2*（q1*q3-q0*q2）;
2*（q1*q2-q0*q3）,q2^2-q3^2+q0^2-q1^2,2*（q2*q3+q0*q1）;
2*（q1*q3+q0*q2）,2*（q2*q3-q0*q1）,q3^2-q2^2-q1^2+q0^2;];
Cbt=Ctb';
fibt=Cbt*[fibbx（i）;fibby（i）;fibbz（i）];%比力数据
fibtx（i）=fibt（1,1）;fibty（i）=fibt（2,1）;fibtz（i）=fibt（3,1）;
Vx（1,i+1）=（fibtx（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））*t+Vx（i）;%计算速度x方向分量
Vy（1,i+1）=（fibty（i）-（2*Wie*sin（L（i））+Vx（i）*tan（L（i））/Rx（i））*Vx（i）+Vy（i）*Vz（i）/Ry（i））*t+Vy（i）;%计算速度y方向分量
Vz（1,i+1）=（fibtz（i）+（2*Wie*cos（L（i）+Vx（i））/Rx（i））*Vx（i）+Vy（i）*Vy（i）/Ry（i）-g）*t+Vz（i）;%计算速度z方向分量
Witt=[-Vy（i）/Ry（i）;
Wie*cos（L（i））+Vx（i）/Rx（i）;
Wie*sin（L（i））+Vx（i）*tan（L（i））/Rx（i）];%求出平台指令角速度值
Wibb=[Wibx（i）;Wiby（i）;Wibz（i）];
Wtbb=Wibb-Ctb*Witt;%将指令角速度转换到平台坐标系,并求解Wtbb
L（1,i+1）=t*Vy（i）/Ry（i）+L（i）;
Lambda（1,i+1）=t*Vx（i）/（Rx（i）*cos（L（i）））+Lambda（i）;
x=Wtbb（1,1）*t;y=Wtbb（2,1）*t;z=Wtbb（3,1）*t;%求取迭代矩阵中的各Δtheta
A=[0-x-y-z;x0z-y;y-z0x;zy-x0];%求取迭代矩阵[Δtheta]
T=x^2+y^2+z^2;%计算[Δtheta]^2的
q（:
i+1）=（（1-T/8+T^2/384）*I+（1/2-T/48）*A）*q（:
i）;%求q0
theta（i+1）=asin（Ctb（2,3））;
if（Ctb（2,2）>=0）
psi（i+1）=atan（-Ctb（2,1）/Ctb（2,2））;
elseif（Ctb（2,1）>0）
psi（i+1）=atan（-Ctb（2,1）/Ctb（2,2））+pi;
else
psi（i+1）=atan（-Ctb（2,1）/Ctb（2,2））-pi;
end
if（Ctb（3,3）>0）
gamma（i+1）=atan（-Ctb（1,3）/Ctb（3,3））;
elseif（Ctb（1,3）<0）
gamma（i+1）=atan（-Ctb（1,3）/Ctb（3,3））+pi;
else
gamma（i+1）=atan（-Ctb（1,3）/Ctb（3,3））-pi;
end
end
figure
（1）
plot（L*180/pi,Lambda*180/pi）;title（'经纬度位置曲线'）;xlabel（'经度/°'）;ylabel（'纬度/°'）;gridon
t=0:
0.01:
600;
figure
（2）plot（t,Vx）;title（'东西方向速度'）;xlabel（'时间/s'）;ylabel（'速度/m/s'）;gridon
figure（3）plot（t,Vy）;title（'南北方向速度'）;xlabel（'时间/s'）;ylabel（'速度/m/s'）;gridon
figure（4）plot（t,theta*180/pi）;title（'俯仰角'）;xlabel（'时间/s'）;ylabel（'度'）;gridon
figure（5）plot（t,gamma*180/pi）;title（'滚转角'）;xlabel（'时间/s'）;ylabel（'度'）;gridon
figure（6）plot（t,psi*180/pi）;title（'偏航角'）;xlabel（'时间/s'）;ylabel（'度'）;gridon
升级会员 