CN116147624A - 一种基于低成本mems航姿参考系统的船舶运动姿态解算方法 - Google Patents

Abstract

本发明公开一种基于低成本MEMS航姿参考系统的船舶运动姿态解算方法，涉及惯性导航技术领域，包括：S1：获取航姿参考系统信息并进行误差补偿；S2：构建陀螺仪误差模型；S3：建立导航坐标系与载体坐标系的转换关系；S4：构建系统状态方程及量测方程；S5：计算船舶姿态角的初值及确定滤波参数初值；S6：更新滤波器参数；S7：得到更新后的船舶姿态角。所构建的误差模型涵盖了随机常值误差以及一阶马尔可夫随机漂移误差等，误差模型精度更高；采用CKF算法具有很好的滤波精度以及稳定性，计算速率更快；同时，基于强跟踪滤波方法调节预测状态误差协方差矩阵，基于Huber鲁棒滤波方法调节测量噪声协方差矩阵，提高了滤波器的鲁棒性。

Description

一种基于低成本MEMS航姿参考系统的船舶运动姿态解算方法

技术领域

本发明涉及惯性导航技术领域，具体涉及一种基于低成本MEMS航姿参考系统的船舶运动姿态解算方法。

背景技术

船舶在海上航行时，受到海浪、海风以及洋流等不确定因素的影响，会使船舶被动地产生沿着三个坐标轴的平移运动:横荡、纵荡、升沉和旋转运动:横摇、纵摇、艏摇，给舰船补给、海上设施维护以及海洋作业机器人的收放等正常作业带来重大影响。为了保障船舶海上作业的正常开展，需要对以上六自由度运动进行准确的测量,同时计算船舶姿态角。

船舶的姿态通常是利用陀螺仪测量的角速度进行积分解算得到的，由于陀螺仪测量的信号中含有零偏、噪声等干扰，随着时间的积累通过积分运算的误差不断放大，导致纯积分解算出来的姿态角出现无限漂移。在静态情况下，加速度计测量的是重力加速度，通过解算可以获取准确的俯仰角和横滚角。但是当载体处在动态时，加速度测量信息中还包含了载体的线加速度，直接解算的姿态信息变得不可靠，因此加速度动态性能差。在没有磁场干扰的情况下，磁力计测量的是地球磁场强度，可以用于校准偏航角信息，但是易受磁干扰导致测量异常。对于低成本航姿参考系统，通常结合上述三类仪器的测量信息，利用数据融合算法实现较高精度的姿态测量。

目前，针对姿态解算的数据融合的方法通常被分为两类：互补滤波器和卡尔曼滤波器。通过互补方式融合陀螺仪、加速度计以及磁力计的数据来评估姿态，由于基于频率的特性，这通常更简单，更快。然而研究发现，普通互补滤波器的性能在长期和高度动态的情况下会恶化。随着计算能力的提高，卡尔曼滤波算法也被用于姿态的解算，它们在姿态估计精度方面往往表现得更好。

西宁泰里霍利智能科技有限公司公开了一种姿态角获取方法及装置(发明专利申请号：201810649078.4，发明名称：姿态角获取方法及装置)，该方法融合了加速度计、磁力计以及陀螺仪数据，采用传统卡尔曼滤波器，基于四元数法获取姿态角，动态响应快，可以捕捉高于5Hz的人体动作，静态误差在±0.1°，对人体动作跟随效果好。

立得空间信息技术股份有限公司公开了一种航姿参考系统的扩展卡尔曼滤波姿态解算方法，(发明专利申请号：202210889240.6，发明名称：一种航姿参考系统的扩展卡尔曼滤波姿态解算方法)，该方法基于四元数法建立拓展卡尔曼滤波的状态方程和量测方程，并且采用平滑滤波算法对加速度和磁力计数据进行平滑滤波处理，同时可以自适应估计加速度数据的量测方差以及判断是否采用磁力计数据，可以获得比较准确的姿态信息。

东南大学公开了一种应用于农用无人机植保作业的姿态解算方法，(发明专利申请号：202210247916.1，发明名称：一种应用于农用无人机植保作业的姿态解算方法)，该方法采用扩展卡尔曼滤波算法作为融合算法，并且采用一种自适应因子修正系统噪声协方差矩阵以及量测噪声协方差矩阵，在获得较高姿态解算精度的同时对状态突变具有一定的鲁棒性。

以上公开的专利所采用的方式，对陀螺仪的误差建模并不准确，陀螺仪解算模型的适应性不强，同时采用扩展卡尔曼滤波(EKF)算法引入了高阶截断误差以及复杂的雅克比矩阵运算，增加了计算的难度和复杂性，姿态解算精度受限并且降低了滤波效率。同时在遇到各种干扰引起的状态突变以及测量异常时，缺少强有效的手段抑制这些异常信息，鲁棒性不够好。

现有技术的主要缺点包括：1、对陀螺仪的误差建模并不精确，尤其对于低精度陀螺仪而言，忽略了一些重要误差，影响了滤波精度；2、采用扩展卡尔曼滤波作为数据融合算法，引入了高阶截断误差以及复杂的雅克比矩阵运算，姿态解算精度受限并且解算效率低；3、难以在复杂的海洋环境中保障解算效果，滤波器在遇到诸多干扰的情况下鲁棒性不强。

发明内容

为解决上述问题的一个或多个，本发明提出了一种基于低成本MEMS航姿参考系统的船舶运动姿态解算方法，所构建的陀螺仪误差模型涵盖了随机常值误差以及一阶马尔可夫随机漂移误差等，误差模型精度更高。

采用CKF(Cubature Kalman filter，即容积卡尔曼滤波)算法作为基础数据融合算法，不仅避免了现有技术中采用扩展卡尔曼滤波引入的高阶截断误差以及复杂的雅克比矩阵运算，并且对于高维非线性系统具有很好的滤波精度以及稳定性，计算速率更快。

由于海洋环境是十分复杂的，获取航姿参考系统信息时会受到各种因素的干扰，导致卡尔曼滤波器的参数值发生突变以及测量异常。本发明基于强跟踪滤波方法调节预测状态误差协方差矩阵，基于Huber鲁棒滤波方法调节测量噪声协方差矩阵，提高了滤波器的鲁棒性，使卡尔曼滤波器面对复杂海洋环境时同样能保证姿态解算精度。

根据本发明的一个方面，提供了一种基于低成本MEMS航姿参考系统的船舶运动姿态解算方法，包括：S1：获取航姿参考系统信息并进行误差补偿；

S2：构建陀螺仪的误差模型；

S3：运用四元数矩阵表示导航坐标系与载体坐标系的转换关系；

S4：结合从陀螺仪获取的航姿参考系统信息与陀螺仪误差模型构建系统状态方程，结合从加速度计以及磁力计获取的航姿参考系统信息构建量测方程；

S5：计算船舶姿态角的初值及确定卡尔曼滤波器的参数初值；

S6：采用CKF算法作为融合算法并进行鲁棒性改进；

S7：计算更新后的船舶姿态角。

在一些实施方式中，S1包括：通过陀螺仪、加速度计以及磁力计获取航姿参考系统信息，并对标定出来的误差进行补偿。由此，可以具有补偿系统误差的效果。

在一些实施方式中，S2包括：根据陀螺仪的随机误差种类，建立陀螺仪误差模型；所述陀螺仪误差模型为：

ε＝b_ω+ε_r+w_g；

其中，ε表示陀螺仪的主要误差，b_ω表示随机常值漂移，ε_γ表示陀螺仪一阶马尔可夫随机漂移，w_g表示陀螺仪解算姿态角的白噪声，w_b表示随机常值漂移的白噪声，τ_g表示相关时间常数，w_r表示马尔可夫过程附加的白噪声。由此，还可以具有补偿随机误差的效果。

在一些实施方式中，S3包括：运用四元数矩阵表示导航坐标系与载体坐标系的转换关系；

运用四元数矩阵表示导航坐标系与载体坐标系的转换关系具体包括：定义单位四元数向量q＝[q₀,q₁,q₂,q₃]^T，利用四元数坐标变换矩阵

表示导航坐标系n系与载体坐标系b系之间的坐标转换关系；

在一些实施方式中，S4包括：根据磁力计的输出模型与加速度计的输出模型，得到量测方程；

加速度计的输出模型为：

其中，a^b表示加速度计在b系测量的加速度信号，g表示地球引力常数的绝对值，v_a为加速度计的加性传感器噪声；

磁力计的输出模型为：

其中

表示磁力计在b系输出的磁场信号,v_h表示磁力计的噪声；[E_x0E_z]表示当地的地球磁场。

在一些实施方式中，S5包括：通过从加速度计以及磁力计获取的航姿参考系统信息计算船舶姿态角的初值，确定卡尔曼滤波器的初值；

所述初始化卡尔曼滤波器参数包括：通过以下公式根据船舶姿态角的初值求得四元数向量的初值：

其中，γ、θ、ψ分别表示船舶的横滚角、俯仰角以及偏航角。

在一些实施方式中，船舶运动姿态解算方法包括：结合所述系统状态方程和量测方程构建卡尔曼滤波器，根据所述卡尔曼滤波器计算船舶姿态角。

在一些实施方式中，S6包括：采用CKF算法作为融合算法并进行鲁棒性改进；

所述采用CKF算法作为融合算法进行鲁棒性改进包括：基于强跟踪滤波方法调节预测状态误差协方差矩阵；基于Huber鲁棒滤波方法调节测量噪声协方差矩阵。S6还包括：通过测量误差协方差矩阵和互协方差矩阵计算卡尔曼滤波器的增益。无论是复杂海洋环境引起的状态突变还是测量异常，都有可能造成姿态解算异常。由此，还可以具有对异常值进行修正，那么滤波精度更高，鲁棒性更好的效果。

在一些实施方式中，所述基于强跟踪滤波方法调节预测状态误差协方差矩阵包括：在预测状态误差协方差矩阵中引入多衰因子矩阵Λ_k+1：

Λ_k+1＝diag(λ_1,k+1,λ_2,k+1,…,λ_n,k+1)；

λ_1,k+1＝μ_ic_k；

其中，λ_i,k+1表示第i个状态分量对应的衰落因子，μ_i表示第i个状态分量相对应的衰落因子的权重，c_k为标准因子；

所述基于Huber鲁棒滤波方法调节测量噪声协方差矩阵包括：引入代价函数，修正测量噪声协方差矩阵的权重。

在一些实施方式中，S7包括：根据更新后的系统状态及航姿参考系统信息得到更新后的四元数向量，根据更新后的四元数向量计算更新后的船舶姿态角；

根据以下公式计算更新后的船舶姿态角：

θ＝-arcsin(2(q₁q₃-q₀q₂))；

根据本发明的另一个方面，提供了一种存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现上述任一项的船舶运动姿态解算方法的步骤。

附图说明

图1为本发明的一种基于低成本MEMS航姿参考系统的船舶运动姿态解算方法的流程示意图；

图2为卡尔曼滤波器的算法流程示意图。

具体实施方式

下面结合附图对本发明作进一步详细的说明。

图1示意性地显示了根据本发明的一种实施方式的一种基于低成本MEMS航姿参考系统的船舶运动姿态解算方法的流程。如图1所示，该方法包括：S1：获取航姿参考系统信息并进行误差补偿；

S2：构建陀螺仪误差模型；

S3：建立导航坐标系与载体坐标系的转换关系；

S4：构建系统状态方程及量测方程；

S5：计算船舶姿态角的初值及确定滤波参数初值；

S6：更新滤波器参数；

S7：得到更新后的船舶姿态角；

还包括：根据系统状态方程及量测方程构建卡尔曼滤波器，根据卡尔曼滤波器计算船舶姿态角。

其中，S1包括：通过低成本MEMS航姿参考系统中的陀螺仪、加速度计以及磁力计获取航姿参考系统信息，接着对航姿参考系统信息进行误差标定，具体可通过实验设备中的转台设置特定的姿态，将所采集的数据将确定性的误差进行误差标定，并将标定出来的误差进行补偿。通过误差标定可以补偿大部分的确定性误差，而随机误差需要通过后续的处理进行补偿。

S2包括：根据陀螺仪的随机误差种类，建立精确的陀螺仪误差模型；

陀螺仪的主要误差模型建立如下：

ε＝b_ω+ε_r+w_g；

其中，ε表示陀螺仪的主要误差，b_ω表示随机常值漂移，ε_γ表示陀螺仪一阶马尔可夫随机漂移，w_g表示陀螺仪解算姿态角的白噪声，w_b表示随机常值漂移的白噪声，τ_g表示相关时间常数，w_r表示马尔可夫过程附加的白噪声。

S3包括：利用四元数矩阵表示导航坐标系与载体坐标系的转换关系；

首先定义单位四元数向量q＝[q₀,q₁,q₂,q₃]^T,所述导航坐标系n系与载体坐标系b系之间的坐标转换关系可以利用四元数坐标变换矩阵

表示如下：

S4包括：结合从陀螺仪获取的航姿参考系统信息与误差模型构建系统状态方程，结合从加速度计以及磁力计获取的航姿参考系统信息构建量测方程；

具体地，定义陀螺仪测量的角速度向量

可表示为角速度估计向量

和陀螺仪误差ε的叠加。对四元数向量进行微分：

其中

为载体坐标系b系相对于导航坐标系n系的角速度矩阵，表示如下：

进一步的，定义系统状态向量x＝[q b_ωxb_ωyb_ωzε_rxε_ryε_rz]^T,从而获取系统状态更新的微分方程如下：

其中b_ωx，b_ωy，b_ωz分别表示陀螺仪x,y,z轴的随机常值漂移；ε_rx，ε_ry，ε_rz，分别表示陀螺仪x,y,z轴的一阶马尔可夫随机漂移；τ_gx，τ_gy，τ_gz分别表示陀螺仪x,y,z轴一阶马尔可夫随机漂移对应的相关时间常数。

卡尔曼滤波器通常采用离散状态空间模型，因此引入采样时间间隔T对微分方程离散化有：

其中x_k+1表示第k+1时刻系统状态的值。

进一步的，加速度计在b系测量的加速度信号

为：

其中，g表示地球引力常数的绝对值，v_a为加速度计的加性传感器噪声。

同样地，磁力计的输出模型为：

其中

表示磁力计在b系输出的磁场信号,v_h表示磁力计的噪声；

[E_x0E_z]可以表示当地的地球磁场，求取如下：

定义磁力计在b系的实际测量值

则磁力计测量值在n系的投影

其中

所以水平面上的地磁E_x和垂直面上的地磁E_z为：

根据磁力计与加速度计的输出模型，得到量测方程h(x)如下：

S5包括：利用加速度计和磁力计测量的航姿参考系统信息解算船舶姿态角的初值，具体来说，可将加速度计和磁力计安装在船舶上，通过四元数向量和姿态角之间的联系得到四元数向量初值，并根据测量的航姿参考系统信息以及理论分析确定卡尔曼滤波器中的滤波参数的初值；

船舶姿态角初值的计算方法如下：

其中，γ、θ、ψ分别表示船舶的横滚角、俯仰角以及偏航角。

这里根据欧拉角法选择的坐标变换顺序为偏航角、俯仰角、横滚角，可得欧拉角与四元数向量的转换关系：

将上述步骤所得到的船舶姿态角初值代入即可求得四元数向量的初值q₀，系统状态

的其他初值b_ω,0和ε_γ,0可以取0，然后根据陀螺仪、加速度计和磁力计的参数指标确定预测状态误差协方差矩阵初始值P₀，过程噪声协方差矩阵初始值Q₀以及测量噪声协方差矩阵R₀。

S6包括：采用自适应鲁棒容积卡尔曼滤波作为融合算法，引入多衰因子修正预测状态误差协方差矩阵以及基于Huber鲁棒滤波方法修改测量噪声协方差矩阵的权重；具体来说，为了避免扩展卡尔曼滤波算法引入的高阶截断误差以及复杂的雅克比矩阵运算，采用CKF算法作为滤波算法具有更高的姿态解算精度以及滤波效率。

同时为了使卡尔曼滤波器在复杂环境下保持良好的滤波效果，需要进行鲁棒性改进，基于强跟踪滤波方法引入一种自适应因子修正预测状态误差协方差矩阵以提高系统对状态突变的鲁棒性，基于Huber鲁棒滤波方法修正测量噪声协方差矩阵提高卡尔曼滤波器抑制测量信号采样序列中离群点的能力。图2示意性地显示了本实施例中的卡尔曼滤波器的算法流程。自适应容积卡尔曼滤波如下：

一、预测更新：从k＝0时刻开始计算方差的平方根S_k以及容积点x_i,k：

其中，ξ_i表示容积点集，P_k表示在k时刻的误差协方差矩阵。对于函数f(x)，代入容积点，可得估计值

接下来，定义容积点个数m＝2n，n为系统状态维数，本实施例中取10。计算状态预测值

以及引入自适应因子前的预测状态误差协方差矩阵

其中Q_k表示k时刻过程噪声协方差矩阵；

计算

的平方根

以及对应的容积点

然后传播对应的测量容积点

并计算测量的估计值

因此可以获取引入自适应因子前的残差

计算引入自适应因子前测量值的互协方差矩阵

同时，可以获得对应测量方程的雅克比矩阵

在预测状态误差协方差矩阵中引入多衰因子矩阵Λ_k+1：

Λ_k+1＝diag(λ_1,k+1,λ_2,k+1,…,λ_n,k+1)；

λ_1,k+1＝μ_ic_k；

其中，λ_i,k+1表示第i个状态分量对应的衰落因子，μ_i表示第i个状态分量相对应的衰落因子的权重，可以为系统的默认值。基于强跟踪滤波方法，标准因子c_k通过以下步骤获取：

其中tr(·)表示矩阵的迹，d表示测量信息的维数，本实施例中取6，

表示M_k+1的第i行第i列元素。N_k+1、M_k+1求取方式如下：

其中V_k+1被定义为

在k＝0时刻时

R_k表示k时刻测量噪声协方差矩阵，α为遗忘因子，本实施例中取0.95,β表示弱化因子，避免过调节以及使状态估计值更平滑，可以通过多次实验获取，本实施例中取3。因此，可以获取更新后的预测状态误差协方差矩阵P_k+1|k：

二、测量更新：计算P_k+1|k的平方根S_k+1|k以及

对应的容积点X_i,k+1:

P_k+1|k＝S_k+1|k(S_k+1|k)^T；

然后计算传播测量值的容积点y_i,k+1并计算测量的估计值

y_i,k+1＝h(X_i,k+1)；

接下来进行鲁棒修正，定义归一化残差向量

通过Huber鲁棒滤波方法引入代价函数ρ·有：

其中，r是调优参数，本实施例设置为1.345，其中ζ_j,k+1表示第j个测量信息对应的归一化残差。定义测量噪声协方差矩阵的权重矩阵Ξ＝diag(ρ（ζ_j,k+1))。

因此，修正后的测量噪声协方差矩阵

为：

计算测量误差协方差矩阵

和互协方差矩阵

计算卡尔曼滤波器的增益矩阵K_k+1：

k时刻为当前时刻，k+1时刻为下一时刻；

更新k+1时刻的系统状态

以及预测状态误差协方差矩阵P_k+1：

S7包括：更新下一时刻的系统状态以及航姿参考系统信息，并将更新的四元数向量进行姿态解算，得到更新后的船舶姿态角；

具体来说，在经过卡尔曼滤波算法一次迭代以后，可以得到更新后的系统状态，系统状态中包含四元数向量，经过姿态转换可以获取滤波后的姿态角信息。

四元数向量的姿态解算如下：

θ＝-arcsin(2(q₁q₃-q₀q₂))；

以上所述的仅是本发明的一些实施方式。对于本领域的普通技术人员来说，在不脱离本发明创造构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。

Claims (10)

1.一种基于低成本MEMS航姿参考系统的船舶运动姿态解算方法，其特征在于，包括：S1：获取航姿参考系统信息并进行误差补偿；

S2：构建陀螺仪误差模型：

S3：运用四元数矩阵表示导航坐标系与载体坐标系的转换关系；

S4：结合从陀螺仪获取的航姿参考系统信息与陀螺仪误差模型构建系统状态方程，结合从加速度计以及磁力计获取的航姿参考系统信息构建量测方程；

S5：计算船舶姿态角的初值及确定卡尔曼滤波器的参数初值；

S6：采用CKF算法作为融合算法并进行鲁棒性改进；

S7：计算更新后的船舶姿态角；

所述船舶运动姿态解算方法还包括：结合所述系统状态方程和量测方程构建卡尔曼滤波器，根据所述卡尔曼滤波器计算船舶姿态角。

2.根据权利要求1所述的一种基于低成本MEMS航姿参考系统的船舶运动姿态解算方法，其特征在于，所述S1包括：通过陀螺仪、加速度计以及磁力计获取航姿参考系统信息，并对标定出来的误差进行补偿。

3.根据权利要求1所述的一种基于低成本MEMS航姿参考系统的船舶运动姿态解算方法，其特征在于，所述S2包括：根据陀螺仪的随机误差种类，建立陀螺仪误差模型；

所述陀螺仪误差模型为：

ε＝b_ω+ε_r+w_g；

其中，ε表示陀螺仪的主要误差，b_ω表示随机常值漂移，ε_γ表示陀螺仪一阶马尔可夫随机漂移，w_g表示陀螺仪解算姿态角的白噪声，w_b表示随机常值漂移的白噪声，τ_g表示相关时间常数，w_r表示马尔可夫过程附加的白噪声。

4.所述根据权利要求1所述的一种基于低成本MEMS航姿参考系统的船舶运动姿态解算方法，其特征在于，所述S3具体包括：定义单位四元数向量q＝[q₀，q₁，q₂，q₃]^T，利用四元数坐标变换矩阵

表示导航坐标系n系与载体坐标系b系之间的坐标转换关系；

5.根据权利要求1所述的一种基于低成本MEMS航姿参考系统的船舶运动姿态解算方法，其特征在于，所述S4包括：根据磁力计的输出模型与加速度计的输出模型，得到量测方程；

加速度计的输出模型为：

其中，

表示四元数坐标变换矩阵，a^b表示加速度计在b系测量的加速度信号，g表示地球引力常数的绝对值，v_a为加速度计的加性传感器噪声；

磁力计的输出模型为：

其中

表示磁力计在b系输出的磁场信号，v_h表示磁力计的噪声；[E_x 0 E_z]表示当地的地球磁场。

6.根据权利要求1所述的一种基于低成本MEMS航姿参考系统的船舶运动姿态解算方法，其特征在于，所述S5包括：通过从加速度计以及磁力计获取的航姿参考系统信息计算船舶姿态角的初值，初始化卡尔曼滤波器参数；

所述初始化卡尔曼滤波器参数包括：通过以下公式根据船舶姿态角的初值求得四元数向量的初值：

其中，γ、θ、ψ分别表示船舶的横滚角、俯仰角以及偏航角。

7.根据权利要求1所述的一种基于低成本MEMS航姿参考系统的船舶运动姿态解算方法，其特征在于，所述S6包括：基于强跟踪滤波方法调节预测状态误差协方差矩阵；基于Huber鲁棒滤波方法调节测量噪声协方差矩阵。

8.根据权利要求7所述的一种基于低成本MEMS航姿参考系统的船舶运动姿态解算方法，其特征在于，所述基于强跟踪滤波方法调节预测状态误差协方差矩阵包括：在预测状态误差协方差矩阵中引入多衰因子矩阵∧_k+1：

∧_k+1＝diag(λ_1，k+1，λ_2，k+1，…，λ_n，k+1)；

λ_1，k+1＝μ_ic_k；

其中，λ_i，k+1表示第i个状态分量对应的衰落因子，μ_i表示第i个状态分量相对应的衰落因子的权重，c_k是标准因子；

所述基于Huber鲁棒滤波方法调节测量噪声协方差矩阵包括：引入代价函数，修正测量噪声协方差矩阵的权重。

9.根据权利要求1所述的一种基于低成本MEMS航姿参考系统的船舶运动姿态解算方法，其特征在于，所述S7包括：根据更新后的系统状态及航姿参考系统信息得到更新后的四元数向量，根据更新后的四元数向量计算更新后的船舶姿态角；

根据以下公式计算更新后的船舶姿态角：

θ＝-arcsin(2(q₁q₃-q₀q₂))；

10.一种存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现权利要求1-9任一项所述的船舶运动姿态解算方法的步骤。

Images