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

说明书

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

技术领域

本发明属于惯性测量技术领域，涉及一种适用于水下MEMS航向陀螺航向角解算方法。

背景技术

传统水下航行器的航向测量通常是采用双自由度方位状态的机械框架陀螺仪。在航行器发射过程中，由于航行器发动机未启动，无法给航向陀螺供电，所以在机械航向陀螺上设计有有发条驱动机构，发射时通过电脉冲信号点燃电爆管，电爆管爆炸推动陀螺发条解锁键，发条蓄能驱动陀螺转子高速启转并达到额定转速，在没有供电的情况下依靠陀螺效应的定轴性为水下航行器建立初始航向基准。入水后，航行器发动机启动产生陀螺供电电源，驱动陀螺电机转动，为航行器提供持续的航向角测量输出。

随着惯性技术的快速发展，使得采用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。

所述的步骤2包括：

采集角速率陀螺仪和加速度计信号，并按航行器坐标轴系定义将各传感器数据矢量折算到航向、横滚和俯仰轴上，折算包括测量值极性转换、标度系数换算和轴间安装耦合叠加，按如下公式组(1)计算航向、横滚、俯仰角速率、航向、横滚、俯仰加速度。

Wx＝S_xxω_x+S_yxω_y+S_zxω_z