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

权利要求书

1.一种适用于水下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

Wy＝S_xyω_x+S_yyω_y+S_zyω_z

Wz＝S_xzω_x+S_yzω_y+S_zzω_z (1)

Ax＝g_xxa_x+g_yxa_y+g_zxa_z

Ay＝g_xya_x+g_yya_y+g_zya_z

Az＝g_xza_x+g_yza_y+g_zza_z

其中W_x、W_y、W_z为航向、横滚、俯仰角速率，A_x、A_y、A_z为航向、横滚、俯仰加速度，ω_x、ω_y、ω_z为三个角速率陀螺仪测量值，a_x、a_y、a_z为三个加速度计测量值，S_*x、S_*y、S_*z角速率陀螺在航向、横滚、俯仰轴上的投影系数，g_*x、g_*y、g_*z加速度计在航向、横滚、俯仰轴上的投影系数，各投影系数通过转台标定测试获得；

所述的步骤8包括：

设定当前时刻的解算航向、横滚、俯仰角为Ψ_i、Ф_i、θ_i，则前一时刻的解算航向角为Ψ_i-1，规定解算航向角初始值为0°，航向角0～360°范围，横滚角和俯仰角±80°范围，当前时刻的解算横滚和俯仰角计算公式见(7)和(8)；

Ф_i＝arctg(A_z/A_x) (7)

θ_i＝arctg(A_y/A_x) (8)

航向角为Ψ_i解算采用四元数进行，四元数解算首先按公式组(9)计算当前时刻四元系数q₀、q₁、q₂、q₃，角度和角速率计算时采用单位为弧度；

按步骤7公式组(6)进行各轴角速率零偏补偿后，根据当前四元数系数q₀、q₁、q₂、q₃值按公式组见(10)～(13)计算解算中间变量值k1、k2、k3、k4；

k1[0]＝(Wx*q₁+Wy*q₂+Wz*q₃)/(-200)

k1[1]＝(Wy*q₃-Wx*q₀-Wz*q₂)/(-200) (10)

k1[2]＝(Wz*q₁-Wy*q₀-Wx*q₃)/(-200)

k1[3]＝(Wx*q₂-Wy*q₁-Wz*q₀)/(-200)

根据解算中间变量值k1、k2、k3、k4按公式组(14)迭代计算当前四元数系数q₀、q₁、q₂、q₃；

接上一步，根据当前四元数系数q₀、q₁、q₂、q₃按公式组(15)计算当前航向角为Ψ_i，并进行航向角4个象限拓展计算；