导航原理业（惯性导航部分）.docx

导航原理作业（惯性导航部分）姓名班号学号成绩批阅人一、题目内容一枚导弹采用捷联惯性导航系统，三个速率陀螺仪Gx, Gy, Gz 和三个加速度计 Ax, Ay, Az 的敏感轴分别沿着着弹体坐标系的Xb, Yb, Zb轴。初始时刻该导弹处在北纬45.75度，东经126.63度。第一种情形：正对导弹进行地面静态测试（导弹质心相对地面静止）。初始时刻弹体坐标系和地理坐标系重合，如图所示，弹体的Xb轴指东，Yb轴指北，Zb轴指天。此后弹体坐标系 Xb-Yb-Zb 相对地理坐标系的转动如下：首先，弹体绕 Zb（方位轴）转过 -10 度；接着，弹体绕 Xb（俯仰轴）转过 15 度；然后，弹体绕 Yb（滚动轴）转过 20 度；最后弹体相对地面停止旋转。请分别用方向余弦矩阵和四元数两种方法计算：弹体经过三次旋转并停止之后，弹体上三个加速度计Ax, Ay, Az的输出。取重力加速度的大小g = 9.8m/s2。第二种情形：导弹正在飞行中。初始时刻弹体坐标系仍和地理坐标系重合；且导弹初始高度200m，初始北向速度 1800 m/s，初始东向速度和垂直速度都为零。陀螺仪和加速度计的输出都为脉冲数形式，陀螺输出的每个脉冲代表 0.00001弧度的角增量。加速度计输出的每个脉冲代表1μg，1g = 9.8m/s2。陀螺仪和加速度计输出的采样频率都为10Hz，在200秒内三个陀螺仪和三个加速度计的输出存在了数据文件gaout.mat中，内含一矩阵变量ga，有2000行，6列。每一行中的数据代表每个采样时刻三个陀螺Gx, Gy, Gz和三个加速度计Ax, Ay, Az的输出的脉冲数。格式如下表（前10行）GxGyGzAxAyAz-30-3055600077000940977-20-3055600077000940977-20-3055599977000940977-10-3055600177000940977-2-1-3055600177001940978-30-3055600077000940977-20-3055600077001940978-20-3155600077000940976-10-3055600076999940976-20-3155600077000940977将地球视为理想的球体，半径 6371.00公里，且不考虑仪表误差，也不考虑弹体高度对重力加速度的影响。选取弹体的姿态计算周期为0.1秒，速度和位置的计算周期为1秒。请计算200秒后弹体到达的经纬度和高度，东向和北向速度；请计算200秒后弹体相对当地地理坐标系的姿态四元数；请绘制出200秒内导弹的经、纬度变化曲线（以经度为横轴，纬度为纵轴）；请绘制出200秒内导弹的高度变化曲线（以时间为横轴，高度为纵轴）。二、第一种情形解答2.1 方向余弦阵法先绕 轴转过再绕轴转过再绕轴转过所以变换后的坐标由于初始时刻有所以计算得弹体上三个加速度计的输出分别是，，计算程序见附录一2.2 四元数法（1）先绕 轴转过（2）再绕轴转过（3）再绕轴转过则合成四元数合成四元数的逆计算得由及得其中所以计算得弹体上三个加速度计的输出分别是，，由两种计算方法的计算结果可以看出，方向余弦阵法和四元数法的计算结果是一致的。 计算程序见附录二三、第二种情形解答3.1 程序流程图3.2 matlab程序见附录三3.3 程序运算结果及仿真图在200秒后，弹体到达的经度为128.149度，到达的纬度为48.897度，导弹所到达的高度为84.504米，200秒时的东向速度为1123.96米每秒，北向速度为1568.016米每秒。200秒后弹体相对当地地理坐标系的姿态四元数为：导弹飞行过程中的经纬度变化曲线如图1所示：图1 导弹飞行过程中经度纬度变化曲线导弹飞行过程中高度变化曲线如图2：图2 导弹飞行过程中高度变化曲线3.4 程序仿真中遇到的问题及体会在按照程序流程图编程时，由于把某些公式的正负号写错，导致在程序仿真时出现了图3的结果。图3 仿真遇到的问题在程序仿真中遇到的另一个问题是在计算四元数阵时，把四元数的维数弄错，导致程序运行出错。在编写程序时，四元数的初始化非常重要，如果四元数的初始化错误，会导致所有的运算结果错误。某些子函数的调用实参与虚参要一致，子函数的返回值要符合需要的格式。同时，这次编程，让我学会了很多以前没用到的matlab函数，在编程过程中，每当遇到问题，我就会上网查一些资料，学习并解决问题。这在很大程度上提高了我的自学能力。附录一suan.mC1=[cos(-10/180*pi) sin(-10/180*pi) 0 -sin(-10/180*pi) cos(-10/180*pi) 0 0 0 1]; %第一次转动方向余弦阵C2=[1 0 0 0 cos(15/180*pi) sin(15/180*pi)