北航惯第一次大作业.docVIP

《惯性导航原理》第一次大作业 原理分析 惯导系统为指北方位的平台系统，则利用比力方程以及陀螺提供的东、北、天三个比力数据，即可计算得到在每个数据采集点的平台即时速度，再通过经纬度的计算公式，就可以得到每个数据采集点平台的即时经纬度，以每个数据采集点为下一个采集点的起点，即可对速度和经纬度进行累计计算，从而得到平台在运动过程中任意时刻的速度和位置情况。 1.模型公式的推导 载体相对地球运动时，加速度计测得的比力表达式，称为比力方程，方程如下： （1） 在指北方案中，平台模拟地理坐标系，将上式中平台坐标系用地理坐标系代入得： （2） 系统中测量的是比力分量，将上式写成分量形式 QUOTE = QUOTE - QUOTE + QUOTE （3） 又因为地球的自转角速率为： QUOTE (4) 地理坐标系相对于地球坐标系的角速率为： QUOTE = QUOTE (5) 将（4）（5）两个式子带入（3）式，即可得到如下方程组： QUOTE （6） 2.速度计算 作业要求只考虑水平通道，因此只需要计算正东、正北两个方向的速度即可。理论上计算得到、后，再积分一次可得到速度值，即 但在本次计算过程中，三个方向的速度均是从零开始在各时间节点上的累加，并不是t的函数，因此速度计算可以由以下方程组实现： QUOTE （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) 为了尽可能减小计算的累积误差，计算过程中可以对g的变化进行追踪。 3.经纬度计算 载体所在位置的地理纬度L，经度λ可由下列方程求得： 与速度的计算相同，经纬度也不是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); %读