原创力文档- 1、本文档共5页，可阅读全部内容。
- 2、原创力文档（book118）网站文档一经付费（服务费），不意味着购买了该文档的版权，仅供个人/单位学习、研究之用，不得用于商业用途，未经授权，严禁复制、发行、汇编、翻译或者网络传播等，侵权必究。
- 3、本站所有内容均由合作方或网友上传，本站不对文档的完整性、权威性及其观点立场正确性做任何保证或承诺！文档内容仅供研究参考，付费前请自行鉴别。如您付费，意味着您自己接受本站规则且自行承担风险，本站不退款、不进行额外附加服务；查看《如何避免下载的几个坑》。如果您已付费下载过本站文档，您可以点击 这里二次下载。
- 4、如文档侵犯商业秘密、侵犯著作权、侵犯人身权等，请点击“版权申诉”（推荐），也可以打举报电话：400-050-0827(电话支持时间：9:00-18:30)。
- 5、该文档为VIP文档，如果想要下载，成为VIP会员后，下载免费。
- 6、成为VIP后，下载本文档将扣除1次下载权益。下载后，不支持退款、换文档。如有疑问请联系我们。
- 7、成为VIP后，您将拥有八大权益，权益包括：VIP文档下载权益、阅读免打扰、文档格式转换、高级专利检索、专属身份标志、高级客服、多端互通、版权登记。
- 8、VIP文档为合作方或网友上传，每下载1次， 网站将根据用户上传文档的质量评分、类型等，对文档贡献者给予高额补贴、流量扶持。如果你也想贡献VIP文档。上传文档
查看更多
惯导导航算法原理.
导航算法
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位置计算
经度、纬度计算：
目标系方位角计算：
目标系内位置计算：
文档评论（0）