北航惯导第一次大作业.doc

惯性导航原理第一次大作业一、 原理分析惯导系统为指北方位的平台系统，则利用比力方程以及陀螺提供的东、北、天三个比力数据，即可计算得到在每个数据采集点的平台即时速度，再通过经纬度的计算公式，就可以得到每个数据采集点平台的即时经纬度，以每个数据采集点为下一个采集点的起点，即可对速度和经纬度进行累计计算，从而得到平台在运动过程中任意时刻的速度和位置情况。1.模型公式的推导载体相对地球运动时，加速度计测得的比力表达式，称为比力方程，方程如下： （1）在指北方案中，平台模拟地理坐标系，将上式中平台坐标系用地理坐标系代入得： （2）系统中测量的是比力分量，将上式写成分量形式Vzt = fxtfytfzt - 0-(2iezt+etzt)(2ieyt+etyt)(2iezt+etzt)0-(2iext+etxt)-2ieyt+etyt2iext+etxt0VxtVytVzt + 00g （3）又因为地球的自转角速率为： iet=iextieytiezt=0iecosLiesinL (4)地理坐标系相对于地球坐标系的角速率为： ett=etxtetytetzt = -VetytRytVetxtRxtVetxtRxttanL (5)将（4）（5）两个式子带入（3）式，即可得到如下方程组： Vzt=fzt +(2iecosL+VxtRxt)Vxt + VytRytVyt g （6）2.速度计算作业要求只考虑水平通道，因此只需要计算正东、正北两个方向的速度即可。理论上计算得到、后，再积分一次可得到速度值，即但在本次计算过程中，三个方向的速度均是从零开始在各时间节点上的累加，并不是t的函数，因此速度计算可以由以下方程组实现：Vxti+1=fxti+2iesinLi+VxtiRxtitanLiVyti-2iecosLi+VxtiRxtiVzt*0.01+VxtiVyti+1=fyti-2iesinLi+VxtiRxtitanLiVxti+VytiRytiVzt*0.01+VytiVzti+1=fzti+2iecosLi+VxtiRxtiVxti+VytiRytiVyt-g*0.01+Vzti（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，经度可由下列方程求得： 与速度的计算相同，经纬度也不是t的函数，可以由累加得到：二、程序流程图 结束按要求输出结果，绘制图像根据理论得到的模型编写算法求解速度和经纬度系统初始化,提取数据并对常数赋值 开始三、导航结果1.系统位置坐标曲线图2.系统东向速度随时间变化曲线图3. 系统北向速度随时间变化曲线图4系统纬度、经度、东向速度、北向速度的终点值经度（度）纬度（度）东向速度（m/s）北向速度（m/s）116.338640.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*cos(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组数据处理完成循环结束endfigure(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