惯导导航算法原理..doc
文本预览下载声明
导航算法
1坐标系定义
1.1地理坐标系
取北、天、东地理坐标系为初始对准过程中的基准系，记为。
1.2弹体坐标系
记为，各轴对应弹体的滚转轴、偏航轴、俯仰轴。
1.3目标坐标系
记为，由地理坐标系绕轴旋转角得到。
2角度定义
：导弹纵轴相对目标轴线的航向角，左偏为正。
：目标方位相对北向夹角，即轴相对轴的夹角，绕轴转过的角度，偏西为正。
：导弹纵轴相对北向的水平夹角（真北夹角），弹轴偏西为正，即导弹航向角（Yaw）。
：导弹俯仰角，抬头为正。（Pitch）
：导弹倾斜角，右倾为正。（Roll）
3常数设定
导航计算周期：
导弹所处纬度：
导航所处经度：
地球半径：
地球自转角速率：
重力加速度：
4导航算法
4.1导航解算初始条件
由初始对准形成，为弹体系和地理系之间的转换矩阵，分三次转动：第一次绕转动角得，第二次绕转动角得到，第三次绕转动角得到弹体坐标系。
设四元数：
、、可由封装数据得到；
、、可由封装数据得到；
初始高度由封装数据得到；
在导航计算前要进行陀螺和加速度计的系数标定，且在导航过程中要进行动静态的误差补偿。
4.2四元数与姿态阵的更新
四元数更新的解析式为：
其中：为阶单位矩阵；
、、为到时刻弹体系相对地理系的角增量分量，即三个陀螺输出的时刻到时刻的角增量。
四元数归一化（单位向量）
解四元数解析方程一般有两种方法：增量算法和龙格-库塔法
增量算法：
记，
将和按泰勒级数展开分别得到一阶到四阶算法
一阶算法：
二阶算法：
三阶算法：
四阶算法：
姿态矩阵的更新计算：
4.3角增量计算
地理坐标系相对惯性空间旋转角速度由地球自转分量和弹体运动产生的旋转角速度分量合成，角增量计算表达式如下：
展开得
4.4姿态角计算
令：
则可从中提取姿态角：
俯仰角
倾斜角
航向角
注：当俯仰角接近时解会产生奇异，此种方法失效，要用其他方法，或者避免使俯仰角接近。
俯仰角定义在区间内，不存在多值问题；滚动角、航向角均定义在区间内，需根据表1和表2进行象限判断。
表1滚动角象限表
象限 + - + + - - - + 表2航向角象限表
象限 + - + + - - - + 导弹相对目标系的航向角：
（注：取值范围定义在）
4.5速度计算
地理系上速度矢量算法为：
其中为加速计输出值在地理系中的投影，。
展开写成离散的计算形式为
目标系的速度分量为：
4.6位置计算
经度、纬度计算：
目标系方位角计算：
目标系内位置计算：
显示全部