北航惯导第一次大作业
- 格式：doc
- 大小：386.00 KB
- 文档页数：7
《惯性导航原理》第一次大作业
一、 原理分析
惯导系统为指北方位的平台系统，则利用比力方程以及陀螺提供的东、北、
天三个比力数据，即可计算得到在每个数据采集点的平台即时速度，再通过经纬
度的计算公式，就可以得到每个数据采集点平台的即时经纬度，以每个数据采集
点为下一个采集点的起点，即可对速度和经纬度进行累计计算，从而得到平台在
运动过程中任意时刻的速度和位置情况。
1.模型公式的推导
载体相对地球运动时，加速度计测得的比力表达式，称为比力方程，方程如
下：
gVVfepepieep)2( （1）
在指北方案中，平台模拟地理坐标系，将上式中平台坐标系用地理坐标系代
入得：
tttettiettgVfV)2( （2）
系统中测量的是比力分量，将上式写成分量形式
=-
+ （3）
又因为地球的自转角速率为：
(4)
地理坐标系相对于地球坐标系的角速率为：
= (5)
将（4）（5）两个式子带入（3）式，即可得到如下方程组：
（6）
2.速度计算
作业要求只考虑水平通道，因此只需要计算正东、正北两个方向的速度即可。理
论上计算得到txV、tyV后，再积分一次可得到速度值，即
ttytytyttxtxtxVdtVV
VdtVV
0
0
0
0
但在本次计算过程中，三个方向的速度均是从零开始在各时间节点上的累加，并
不是t的函数，因此速度计算可以由以下方程组实现：
（7）
此方程组表示了从第i个采集点到第（i+1）个采集点的速度递推公式。
方程中Rx表示卯酉圈的曲率半径，Ry表示子午圈的曲率半径，计算方法如下：
Rx=Re/(1-esin2L);
Ry=Re/(1+2e-3esin2L); (8)
由于平台在运动中纬度L也在不断变化，因此，计算过程中应当追踪两个半径的
变化，正如方程组（7）所示。
另外方程组中g表征平台所处纬度下的重力加速度：
g=g0(1+0.0052884sin2L-0.0000059sin2L) (9)
为了尽可能减小计算的累积误差，计算过程中可以对g的变化进行追踪。
3.经纬度计算
载体所在位置的地理纬度L，经度λ可由下列方程求得：
txttxtyttyLdtRVLdtRVL0000sec
与速度的计算相同，经纬度也不是t的函数，可以由累加得到：
二、程序流程图
三、导航结果
系统初始化,提取数据
并对常数赋值
开始
根据理论得到的模型编写
算法求解速度和经纬度
按要求输出结果，绘制图像
结束
1.系统位置坐标曲线图
2.系统东向速度随时间变化曲线图
3. 系统北向速度随时间变化曲线图
4．系统纬度、经度、东向速度、北向速度的终点值
经度（度） 纬度（度） 东向速度（m/s） 北向速度（m/s）
116.3386 40.1555 -2.6472 -5.2091
四、小结
程序运行结果显示本次平台的运动是一个以初始点位置为中心的往返运动，
从东向和北向速度的变化情况来看也是如此。
由于时间问题，自己有两种想法没有验证，不知道正确与否。一是把陀螺仪
角速率信息ω看做平台相对地球的角速率信息，从而可将方程组（3）直接建立
成模型计算。二是在本题目中，天向速度是存在的，我的模型计算结果是
-1.5554m/s，因此高度是有变化的，可以加入高度通道进行计算，不知道结果会
差多少。
惯性导航是比较有难度的课程，本来觉得很有压力，但是俞老师可以一个台
阶一个台阶引导我学习，并激发了我的学习兴趣，非常感谢。我非常喜欢本门课
程，对授课方式也非常满意，再次谢谢俞老师！
五、源程序
clear all;
clc;
Vx(1)=0;
%初始化变量
Vy(1)=0;
Vz(1)=0;
L(1)=40.162565402/180*pi; %将初始位置经纬度变换成弧度
J(1)=116.343692076/180*pi;
Wie=7.2921E-5; %给公式中的常数赋值
pi=3.141592654;
re=6378245;
e=1/298.3;
h=37.74319;
g0=9.78049;
load('E:\学习\惯性导航\2011秋《惯性导航原理》第一次作业\fw');
%读取文件中的数据
Fx=f(1,:); %提取正东方向比力数据并定义
Fy=f(2,:); %提取正北方向比力数据并定义
Fz=f(3,:); %提取天向比力数据并定义
Wx=w(1,:); %提取陀螺正东方向角速率并定义
Wy=w(2,:); %提取陀螺正北方向角速率并定义
Wz=w(3,:); %提取陀螺天向角速率并定义
i=1;
t=1E-2;
for i=1:120000
%共有120000个数据，利用for程序完成对数据
的循环处理
g=g0*(1+0.0052884*sin(L(i))^2-0.0000059*sin(2*L(i))^2);
%在纬度发生变化的情况下追踪重力加速度的变化，减小计算误差
Rx(i)=re/(1-e*sin(L(i))^2); %卯酉圈主曲率半径
Ry(i)=re/(1+2*e-3*e*sin(L(i))^2);
%子午圈主曲率半径
Vx(i+1)=(Fx(i)+(2*Wie*sin(L(i))+Vx(i)*tan(L(i))/Rx(i))*Vy(i)-(2*Wie*c
os(L(i))+Vx(i)/Rx(i))*Vz(i))*t+Vx(i);%正东方向速度计算
Vy(i+1)=(Fy(i)-(2*Wie*sin(L(i))+Vx(i)*tan(L(i))/Rx(i))*Vx(i)+Vy(i)*Vz
(i)/Ry(i))*t+Vy(i); %正北方向速度计算
Vz(i+1)=(Fz(i)+(2*Wie*cos(L(i)+Vx(i))/Rx(i))*Vx(i)+Vy(i)*Vy(i)/Ry(i)-
g)*t+Vz(i); %天向速度计算
L(i+1)=t*Vy(i)/Ry(i)+L(i);
%纬度计算
J(i+1)=t*Vx(i)/(Rx(i)*cos(L(i)))+J(i);%经度计算
i=i+1;
%i与1累加，直至将120000组数据处理完成循环结束
end
figure(1)
plot(L*180/pi,J*180/pi);xlabel('经度'),ylabel('纬度');
%以经度为横
轴，纬度为纵轴（单位为：度）作出系统位置坐标曲线图
T=0:0.01:1200;
figure(2)
plot(T,Vy);xlabel('时间/秒'),ylabel('东向速度/m/s');
%以时间为横
轴（单位：秒），东向速度为纵轴作出系统速度随时间变化曲线图
figure(3)
plot(T,Vx);xlabel('时间/秒'),ylabel('北向速度/m/s');
%以时间为横
轴（单位：秒），北向速度为纵轴作出系统速度随时间变化曲线图
Lzz=L(120001)*180/pi %输出最终位置的纬度
Jzz=J(120001)*180/pi %输出最终位置的经度
Vxzz=Vx(120001) %正东方向的速度最终值
Vyzz=Vy(120001) %正北方向的速度最终值
