CN108151736A - 一种适用于水下mems航向陀螺航向角解算方法 - Google Patents

Abstract

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

Description

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

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加速度计在航向、横滚、俯仰轴上的投影系数，各投影系数可通过转台标定测试获得。

所述的步骤4包括：

初始航向为Ψ₀，航向角积分计算包括公式(2)～(4)，公式(2)为首次积分计算，由于数据处理周期为0.01s，所以角速率积分系数为0.01；公式(3)为循环积分计算；公式(4)为航向角360°范围求余计算，防止航向角超范围，Ψ₀为初始航向设定值；

ψ_o1＝0.01W_x1 (2)

ψ_oi＝ψ_oi-1+0.01W_xi (3)

ψ_oi＝(ψ_o+ψ_oi)％360 (4)

其中Ψ₀为设定航向角，Ψo_i为当前输出航向角。

所述的步骤5包括：进行各轴角速率零位误差补偿计算，计算周期为0.01s，角速率零位误差值取5s内的采样数据进行平均,具体按公式组(5))计算航向、横滚、俯仰角速率零位误差补偿值，

其中为i为5s时间采样值得数量(i＝500)，航向、横滚、俯仰角速率零位误差补偿值。

所述的步骤7包括：

按公式组(6)进行各轴角速率零偏补偿，角速率Wx、Wy、Wz转换为弧度后参与后续计算；

所述的步骤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)

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

k4[1]＝(Wy*(q₃+k3[3])-Wx*(q₀+k3[0])-Wz*(q₂+k3[2]))/(-200) (13)

k4[2]＝(Wz*(q₁+k3[1])-Wy*(q₀+k3[0])-Wx*(q₃+k3[3]))/(-200)

k4[3]＝(Wx*(q₂+k3[2])-Wy*(q₁+k3[0])-Wz*(q₀+k3[1]))/(-200)

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

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

所述的步骤11包括：

在启动时间等于6秒时计算航向角零位误差，航向角零位误差与启动后5-6秒时间内航向角解算值有关，航向角零位误差系数Δψ按公式(16)计算，

Δψ＝57.2957795ψ₁₀₀％360 (16)

其中为Δψ为航向角误差系数，ψ₁₀₀为5～6秒时间结束时的航向角值，即解算开始1秒钟后的第100个航向角值。

所述的步骤12是入水后航向角解算前对各轴角速率零位误差补偿修参照步骤7公式(6)计算；所述的步骤13是入水后航向角的四元数解算计算参照步骤8公式(7)～(14)计算。

所述的步骤14是入水后是航向角4象限拓展计算参照步骤8公式(15)计算。

所述的步骤15是入水后进行航向角弧度转角度整理计算，包括：

入水后航向陀螺输出航向角用四元数解算得到的航向角替代，按照公式(17)进行航向角弧度转角度整理计算，

ψ_oi＝(ψ_o-Δψ+57.2957795ψ_i)％360 (17)

其中Ψo_i为当前输出航向角，Ψ₀为设定航向角，ψ_i为当前航向角解算值。

本发明中所述的循环和输出周期为0.01s。

本发明的优点是：由于MEMS陀螺仪为角速率传感器，不能直接用于航向角度测量，所以采用本发明的解算方法进行一系列的积分、空间坐标关系转换和解算，以满足水下航行器控制的快速航向建立及精确航向测量需要。

附图说明

下面结合实施例附图对本发明作进一步说明：

图1是一种水下MEMS航向陀螺航向角解算方法流程图。

具体实施方式

如图1所示，一种适用于水下MEMS航向陀螺航向角解算方法，至少包括:

步骤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加速度计在航向、横滚、俯仰轴上的投影系数，各投影系数可通过转台标定测试获得。

步骤4：启动后进行航向角速率积分计算包括：

初始航向为Ψ₀，航向角积分计算包括公式(2)～(4)，公式(2)为首次积分计算，由于数据处理周期为0.01s，所以角速率积分系数为0.01；公式(3)为循环积分计算；公式(4)为航向角360°范围求余计算，防止航向角超范围，Ψ₀为初始航向设定值；

ψ_o1＝0.01W_x1 (2)

ψ_oi＝ψ_oi-1+0.01W_xi (3)

ψ_oi＝(ψ_o+ψ_oi)％360 (4)

其中Ψ₀为设定航向角，Ψo_i为当前输出航向角。

步骤5：角速率零位误差值计算包括：

在发射到入水过程中航向陀螺(航向解算)软件同时在后台进行各轴角速率零位误差补偿计算，计算周期为0.01s，角速率零位误差值取5s内的采样数据进行平均,具体按公式组(5))计算航向、横滚、俯仰角速率零位误差补偿值。

其中为i为5s时间采样值得数量(i＝500)，航向、横滚、俯仰角速率零位误差补偿值。

步骤7：各轴角速率零位误差补偿修正计算包括：

按公式组(6)进行各轴角速率零偏补偿，角速率Wx、Wy、Wz转换为弧度后参与后续计算。

步骤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₃。需要说明的是C语言软件角度和角速率计算时采用单位为弧度(取1弧度＝57.2957795°)。

接上一步，按步骤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)

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

k4[1]＝(Wy*(q₃+k3[3])-Wx*(q₀+k3[0])-Wz*(q₂+k3[2]))/(-200) (13)

k4[2]＝(Wz*(q₁+k3[1])-Wy*(q₀+k3[0])-Wx*(q₃+k3[3]))/(-200)

k4[3]＝(Wx*(q₂+k3[2])-Wy*(q₁+k3[0])-Wz*(q₀+k3[1]))/(-200)

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

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

步骤8计算获得航向角值不输出，陀螺航向角输出仍然采用步骤4计算得到的积分航向值。

步骤11：航向角零位误差值计算包括：

在启动时间等于6秒时计算航向角零位误差，航向角零位误差与启动后5-6秒时间内航向角解算值有关，航向角零位误差系数Δψ按公式(16)计算。

Δψ＝57.2957795ψ₁₀₀％360 (16)

其中为Δψ为航向角误差系数，ψ₁₀₀为5～6秒时间结束时的航向角值，即解算开始1秒钟后的第100个航向角值。

步骤12：(入水后)航向角解算前对各轴角速率零位误差补偿修参照步骤7公式(6)计算。

步骤13：(入水后)航向角的四元数解算计算参照步骤8公式(7)～(14)计算。

步骤14：(入水后)航向角4象限拓展计算参照步骤8公式(15)计算；

步骤15：(入水后)进行航向角弧度转角度整理计算包括：

入水后航向陀螺输出航向角用四元数解算得到的航向角替代，按照公式(17)进行航向角弧度转角度整理计算。

ψ_oi＝(ψ_o-Δψ+57.2957795ψ_i)％360 (17)

其中Ψo_i为当前输出航向角，Ψ₀为设定航向角，ψ_i为当前航向角解算值。

所述的循环解算和输出周期为0.01s。

Claims (10)

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所述的一种适用于水下MEMS航向陀螺航向角解算方法，其特征是：所述的步骤2包括：

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

其中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加速度计在航向、横滚、俯仰轴上的投影系数，各投影系数可通过转台标定测试获得。

3.根据权利要求1所述的一种适用于水下MEMS航向陀螺航向角解算方法，其特征是：所述的步骤4包括：

初始航向为Ψ₀，航向角积分计算包括公式(2)～(4)，公式(2)为首次积分计算，由于数据处理周期为0.01s，所以角速率积分系数为0.01；公式(3)为循环积分计算；公式(4)为航向角360°范围求余计算，防止航向角超范围，Ψ₀为初始航向设定值；

ψ_o1＝0.01W_x1 (2)

ψ_oi＝ψ_oi-1+0.01W_xi (3)

ψ_oi＝(ψ_o+ψ_oi)％360 (4)

其中Ψ₀为设定航向角，Ψo_i为当前输出航向角。

4.根据权利要求1所述的一种适用于水下MEMS航向陀螺航向角解算方法，其特征是：所述的步骤5包括：进行各轴角速率零位误差补偿计算，计算周期为0.01s，角速率零位误差值取5s内的采样数据进行平均,具体按公式组(5))计算航向、横滚、俯仰角速率零位误差补偿值，

其中为i为5s时间采样值得数量(i＝500)，航向、横滚、俯仰角速率零位误差补偿值。

5.根据权利要求1所述的一种适用于水下MEMS航向陀螺航向角解算方法，其特征是：所述的步骤7包括：

按公式组(6)进行各轴角速率零偏补偿，角速率Wx、Wy、Wz转换为弧度后参与后续计算；

6.根据权利要求1所述的一种适用于水下MEMS航向陀螺航向角解算方法，其特征是：所述的步骤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、k2、k3、k4按公式组(14)迭代计算当前四元数系数q₀、q₁、q₂、q₃。

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

7.根据权利要求1所述的一种适用于水下MEMS航向陀螺航向角解算方法，其特征是：所述的步骤11包括：

在启动时间等于6秒时计算航向角零位误差，航向角零位误差与启动后5-6秒时间内航向角解算值有关，航向角零位误差系数Δψ按公式(16)计算，

Δψ＝57.2957795ψ₁₀₀％360 (16)

其中为Δψ为航向角误差系数，ψ₁₀₀为5～6秒时间结束时的航向角值，即解算开始1秒钟后的第100个航向角值。

8.根据权利要求1所述的一种适用于水下MEMS航向陀螺航向角解算方法，其特征是：所述的步骤12是入水后航向角解算前对各轴角速率零位误差补偿修参照步骤7公式(6)计算；所述的步骤13是入水后航向角的四元数解算计算参照步骤8公式(7)～(14)计算。

9.根据权利要求1所述的一种水下MEMS航向陀螺航向角解算方法设计，其特征是：所述的步骤14是入水后是航向角4象限拓展计算参照步骤8公式(15)计算。

10.根据权利要求1所述的一种适用于水下MEMS航向陀螺航向角解算方法，其特征是：所述的步骤15是入水后进行航向角弧度转角度整理计算，包括：

入水后航向陀螺输出航向角用四元数解算得到的航向角替代，按照公式(17)进行航向角弧度转角度整理计算，

ψ_oi＝(ψ_o-Δψ+57.2957795ψ_i)％360 (17)

其中Ψo_i为当前输出航向角，Ψ₀为设定航向角，ψ_i为当前航向角解算值。