[发明专利]一种适用于水下MEMS航向陀螺航向角解算方法

摘要

本发明涉及一种适用于水下MEMS航向陀螺航向角解算方法，其特征是：MEMS航向陀螺从航行器发射启动、入水、发动机启动供电、稳定航行等过程中的传感器测量数据在坐标轴上矢量投影、航向角速率积分、航向对准、四元数解算、航向角象限拓展、航向角误差系数等计算方法、公式与步骤。该方法满足水下航行器控制的快速航向建立及精确航向测量需要。

主权项

一种适用于水下MEMS航向陀螺航向角解算方法，其特征是：至少包括：步骤1：MEMS航向陀螺启动；步骤2：各传感器测量数据在坐标轴上矢量投影计算；步骤3：第一时间延时判断，小于6秒，执行下一步；大于等于6秒，执行步骤10；步骤4：启动后6秒内进行航向角速率积分计算，积分结果作为输出航向角数据，以便快速建立水下航行器的航向基准；步骤5：进行角速率零位误差值计算；步骤6：第二时间延时判断，小于5秒，执行步骤9；大于等于5秒，执行下一步；步骤7：进行各轴角速率零位误差补偿修正；步骤8：后台进行航向角的四元数解算，解算结果不输出；步骤9：将准备好的航向角数据输出，重返步骤2；步骤10：第三时间延时判断，等于6秒，执行下一步；大于6秒，执行步骤12；步骤11：根据步骤8计算的前5秒航向角进行航向角零位误差值估算；步骤12：进行正式航向角解算前对各轴角速率零位误差补偿修正；步骤13：进行航向角的四元数解算；步骤14：进行航向角4象限拓展计算；步骤15：进行航向角弧度转角度整理计算，准备输出航向角数据，重返步骤9。