北航惯性导航作业二综述.docx
《北航惯性导航作业二综述.docx》由会员分享,可在线阅读,更多相关《北航惯性导航作业二综述.docx(14页珍藏版)》请在冰豆网上搜索。
![北航惯性导航作业二综述.docx](https://file1.bdocx.com/fileroot1/2023-5/17/b2b120d1-c57c-4e44-92df-a43f97f65602/b2b120d1-c57c-4e44-92df-a43f97f656021.gif)
北航惯性导航作业二综述
惯性导航作业
一、数据说明:
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