哈工大四系导航原理 大作业 INS部分.docx

Assignment ofPrinciples of Navigation《导航原理》作业（惯性导航部分）My NameMy Class1104105Student No.目录：一、题目内容1二、解答31. 第一种情形31. 方向余弦法32. 四元数法42. 第二种情形6三、对计算与仿真结果的分析与心得12一、题目内容一架战斗机采用捷联惯性导航系统，三个速率陀螺仪和三个加速度计的敏感轴分别沿着着战斗机（载体）坐标系的轴。初始时刻该战斗机处在北纬35度，东经122度。第一种情形：对战斗机进行地面静态测试初始时刻战斗机坐标系和地理坐标系重合，如图所示，战斗机的轴指东，轴指北，轴指天。此后战斗机坐标系相对地理坐标系的转动如下：(1)战斗机绕（俯仰轴）转过 10 度； (2)战斗机绕（滚动轴）转过 30 度； (3)战斗机绕（方位轴）转过 -50 度； 最后战斗机相对地面停止旋转。east请分别用方向余弦矩阵和四元数两种方法计算：战斗机经过三次旋转并停止之后，战斗机上三个加速度计的输出。忽略设备误差。取重力加速度的大小g = 9.8。第二种情形：战斗机正在飞行中初始时刻战斗机坐标系滚动轴为北偏东45度，并且与跑道平行，俯仰轴与滚动轴都在水平平面内；且战斗机初始高度25m，初始北向、东向速度和垂直速度都为零。然后沿跑道加速并开始起飞。陀螺仪和加速度计都以脉冲数形式输出，陀螺输出的每个脉冲代表 大小为0.1角秒的角增量。加速度计输出的每个脉冲代表g，g = 9.8。陀螺仪的输出频率为10Hz；加速度计的输出频率为1Hz。在5400秒内三个陀螺仪和三个加速度计的输出存在了数据文件gout.mat和aout.mat中，各含一矩阵变量gm，am，其中gm有54000行、3列；am有5400行、3列。每一行中的数据各代表每个采样时刻三个陀螺和三个加速度计的输出的脉冲数。形式如下图所示：将地球视为理想的球体，半径 6368.00公里，自转角速率为。不考虑仪表误差，也不考虑战斗机高度对重力加速度的影响。选取战斗机的姿态计算周期为0.1秒，速度和位置的计算周期为1秒。?计算最终战斗机的姿态四元数，战斗机最终到达的经纬度和高度，东向、北向和垂直速度；?计算最终战斗机总共的水平移动距离；?绘制出总体战斗机的经、纬度变化曲线（以经度为横轴）；?绘制出总体战斗机的高度变化曲线（以时间为横轴）。解答第一种情形方向余弦法方向余弦矩阵表达式为：由于均为单位向量，求内积的结果实际上大小即为对应的余弦值。若要求转动前相对转动后的方向余弦矩阵，则对上式求逆即可。1.绕X轴转过10度方向余弦矩阵为：绕Y轴转过30度方向余弦矩阵为：绕Z轴转过-50度方向余弦矩阵为：把g写成沿k轴向量的形式，即：那么三次旋转过后的g在新坐标轴的分量即为：通过计算得到：对A取模发现结果仍然为9.8，说明结果正确。MATLAB代码如下：%======第一次转动======%C1=[1 0 00 cos(10/180*pi) sin(10/180*pi)0 -sin(10/180*pi) cos(10/180*pi)]; %======第二次转动======%C2=[cos(30/180*pi) 0 -sin(30/180*pi)0 1 0sin(30/180*pi) 0 cos(30/180*pi)];%======第三次转动======%C3=[cos(-50/180*pi) sin(-50/180*pi) 0-sin(-50/180*pi) cos(-50/180*pi) 00 0 1];%======计算三次转动后战斗机的姿态======%A=C3*C2*C1*[0;0;9.8] %======检验转动后的合成矢量是否与转动前相同======%norm(A) 四元数法1.绕X轴转过10度得到第一次旋转四元数：2.绕Y轴转过30度得到第二次旋转四元数：3.绕Z轴转过-50度得到第三次旋转四元数：由于采用的映像形式，所以直接通过左乘得到四元数乘积：由四元数坐标系旋转的公式：计算得到G在三次旋转之后的坐标系中的四元数为：计算A得范数为9.8，证明计算正确。与方向余弦算法比较，得到了相同的结果。MATLAB代码如下：%======第一次转动======%q1=[cos(10/360*pi) sin(10/360*pi) 0 0];%======第二次转动======%q2=[cos(30/360*pi) 0 sin(30/360*pi) 0];%======第三次转动======%q3=[cos(-50/360*pi) 0 0 sin(-50/360*pi)];%利用MATLAB自带的四元数相乘函数计算四元数乘积。（四元数写成横向量形式）r=quatmultiply(q1,q2);q=quatmul