惯导第二次大作业.docxVIP

《惯性导航原理》第二次大作业原理分析指北方位平台式惯导解算在利用方向余弦方法对惯性导航系统进行测算时，刚体空间位置用固连于刚体的动坐标系对固定参考系各轴的九个方向余弦来确定，九个方向余弦角存在六个约束条件，计算比较繁琐，模型也比较复杂。如果在计算过程中引入四元数，则可以通过坐标系的一次转动，实现方向余弦方法中的三次坐标旋转。原理图如下：加速度计fbftλ、L、hVx、Vy、Vz姿态速率微分方程陀螺组件+—从原理图可以清楚的看出，通过捷联姿态矩阵可以将任意姿态的平台坐标系下的比力数据转换到地理坐标系下，然后通过指北方位平台式惯导解算的方法即可以得到任意时刻载体的位置和速度信息。关键在于捷联姿态矩阵的求解。在这里应用四元数知识进行解算。捷联姿态矩阵的求解因为平台的初始姿态角都是已知的，则可以先求解四元数中各元的初值。平台坐标系相对于地理坐标系的三次旋转可以由四元数的乘积得到。用四元数表示这三次转动为（逆时针为正）： 对三者进行四元素乘法运算： 结果与四元数Λ=中的各元素相对应，就可以从已知的平台初始姿态求解出四元数的各元初值。带入四元数姿态矩阵即可得到捷联姿态矩阵：通过此矩阵可以将地理坐标系的参数转移到平台坐标系。此矩阵的逆矩阵平台坐标系转移到地理坐标系的姿态矩阵。此外此矩阵还要在四元数的迭代计算中使用。下面进行四元数的迭代计算。四元数微分方程的矩阵形式为式中 其中陀螺仪所测量的角速度值，为平台的指令角速度，为地球的自转角速率与地理坐标系相对于地球坐标系的角速率之和，即地球的自转角速率为：地理坐标系相对于地球坐标系的角速率为：=上述四元数方程的解和下面矩阵方程的解类似：记q(t)=T，由角增量法可得迭代公式（取四阶算法）：q(n+1)=q(n)（n=0,1,2，……）式中q(0)即为解算出的初值，带入迭代公式即可得到任意时刻的捷联姿态矩阵。通过捷联姿态矩阵可以将任意姿态的平台坐标系下的比力数据转换到地理坐标系下，然后通过指北方位平台式惯导解算的方法求解任意时刻载体的位置和速度信息即可。2、指北方位平台式惯导求解方位和速度载体相对地球运动时，加速度计测得的比力表达式，称为比力方程，方程如下：在指北方案中，平台模拟地理坐标系，将上式中平台坐标系用地理坐标系代入得：系统中测量的是比力分量，将上式写成分量形式=- +将的表达式带入上式，即可得到如下方程组： （6）作业要求只考虑水平通道，因此只需要计算正东、正北两个方向的速度即可。理论上计算得到、后，再积分一次可得到速度值，即但在本次计算过程中，三个方向的速度均是从零开始在各时间节点上的累加，并不是t的函数，因此速度计算可以由以下方程组实现此方程组表示了从第i个采集点到第（i+1）个采集点的速度递推公式。方程中Rx表示卯酉圈的曲率半径，Ry表示子午圈的曲率半径，计算方法如下：Rx=Re/(1-esin2L);Ry=Re/(1+2e-3esin2L);由于平台在运动中纬度L也在不断变化，因此，计算过程中应当追踪两个半径的变化。另外方程组中g表征平台所处纬度下的重力加速度： 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(纬度); 为了尽可能减小计算的累积误差，计算过程中可以对g的变化进行追踪。 3.经纬度计算 载体所在位置的地理纬度L，经度λ可由下列方程求得：与速度的计算相同，经纬度也不是t的函数，可以由累加得到：二、程序流程图根据初始姿态角求解四元数的初值陀螺组件+ -四元数微分方程（迭代法）求逆加速度计指北方位的平台式惯导解算输出经纬度，东向北向的速度三、导航结果1.系统位置坐标曲线图2.系统东向速度随时间变化曲线图3.系统北向速度随时间变化曲线图4．系统纬度、经度、东向速度、北向速度的终点值经度（度）纬度（度）东向速度（m/s）北向速度（m/s）116.344639.9751-0.0608-0.0655四、小结本次的运行结果太小，觉得可能不对。检查了几天，把公式都重新推了一遍，也反复检查了程序的写法，但一直找不到问题所在，也有可能是我的想法有问题，希望老师在评阅时可以帮我找出其中的问题所在，予以指点，谢谢老师！五、源程序clear all;clc;Vx(1)=0.000048637; %初始化变量Vy(1)=0.000206947;Vz(1)=0;pi=3.141592654;fai(1)=91.637207*pi/180;sit(1)=0.120992605*pi/180;gam(1)=0.0