北航惯性导航作业二Word下载.docx
- 文档编号:22276168
- 上传时间:2023-02-03
- 格式:DOCX
- 页数:14
- 大小:315.68KB
北航惯性导航作业二Word下载.docx
《北航惯性导航作业二Word下载.docx》由会员分享,可在线阅读,更多相关《北航惯性导航作业二Word下载.docx(14页珍藏版)》请在冰豆网上搜索。
\...文件路径...\jlfw,便可得到比力信息和陀螺仪角速率信息。
用角增量法。
(1)以系统经度为横轴,纬度为纵轴(单位均要转换为:
度)做出系统位置曲线图;
(2)做出系统东向速度和北向速度随时间变化曲线图(速度单位:
m/s,时间单位:
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,:
Vx(1,:
Vy(1,:
Vz(1,:
Rx(1,:
%定义存放卯酉圈曲率半径数据的矩阵
Ry(1,:
%定义存放子午圈曲率半径数据的矩阵
psi(1,:
%定义存放偏航角数据的矩阵
theta(1,:
%定义存放俯仰角数据的矩阵
gamma(1,:
%定义存放滚转角数据的矩阵
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,:
3);
%提取陀螺正北方向角速率并定义
Wibz(1,:
4);
%提取陀螺天向角速率并定义
fibbx(1,:
5);
%x方向的比力数据
fibby(1,:
6);
%y方向的比力数据
fibbz(1,:
7);
%z方向的比力数据
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;
%滚转角初始值单位:
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)>
gamma(i+1)=atan(-Ctb(1,3)/Ctb(3,3));
elseif(Ctb(1,3)<
gamma(i+1)=atan(-Ctb(1,3)/Ctb(3,3))+pi;
gamma(i+1)=atan(-Ctb(1,3)/Ctb(3,3))-pi;
end
figure
(1)
plot(L*180/pi,Lambda*180/pi);
title('
经纬度位置曲线'
xlabel('
经度/°
'
ylabel('
纬度/°
gridon
t=0:
0.01:
600;
figure
(2)plot(t,Vx);
东西方向速度'
时间/s'
速度/m/s'
figure(3)plot(t,Vy);
南北方向速度'
figure(4)plot(t,theta*180/pi);
俯仰角'
度'
figure(5)plot(t,gamma*180/pi);
滚转角'
figure(6)plot(t,psi*180/pi);
偏航角'
- 配套讲稿:
如PPT文件的首页显示word图标,表示该PPT已包含配套word讲稿。双击word图标可打开word文档。
- 特殊限制:
部分文档作品中含有的国旗、国徽等图片,仅作为作品整体效果示例展示,禁止商用。设计者仅对作品中独创性部分享有著作权。
- 关 键 词:
- 北航 惯性 导航 作业
![提示](https://static.bdocx.com/images/bang_tan.gif)