CN114877858B - 一种高动态和磁干扰环境下的姿态估计算法 - Google Patents

Abstract

本发明提供一种高动态和磁干扰环境下的姿态估计算法，该姿态估计算法包括第一层姿态估计算法、第二层姿态估计算法，其中第一层算法解算俯仰、滚转角，选用方向余弦矩阵的最后一行作为俯仰、滚转角的状态变量，选用该状态变量的误差状态在EKF框架下进行更新与校正；第二层算法解算航向角，选用方向余弦矩阵的第一列的前两个元素作为航向角的状态变量，同时为了避免磁干扰对姿态解算的影响，使用磁力计模长乘以模块磁场倾角增量再取对数函数的方法表征磁干扰的强度，并将磁干扰的强度和观测噪声相关联。解决了姿态估计算法在高动态环境下精度低、易发散，且姿态角受磁干扰影响的问题，有效提高了姿态估计的精度和鲁棒性。

Description

一种高动态和磁干扰环境下的姿态估计算法

【技术领域】

本发明涉及一种姿态估计算法，具体涉及一种高动态和磁干扰环境下的姿态估计算法。

【背景技术】

卡尔曼滤波是姿态估计中应用最为广泛的一种方法，传统卡尔曼滤波只使用于线性***，而三维旋转运动由于其转动空间是流形，无论用哪种其数学描述方法都是在非线性空间中工作。为了适应非线性***，提出使用扩展卡尔曼滤波(EKF)进行姿态估计，其核心思想就是利用泰勒级数，将非线性***在工作点附近进行一阶泰勒展开并忽略高阶项。但是在高动态环境中，运动方程的模型噪声和陀螺仪噪声增大，观测方程的线性化点开始远离工作点，最终导致EKF的估计结果产生偏差或不一致，甚至会导致滤波器发散。

由于加速度计并未包含航向信息，要想获得无累计误差的航向角则需要引入磁力计，但载体在运动过程中，不可避免的会运动到有外部磁场干扰的环境，当距离干扰源比较近时干扰磁场的强度会远远大于地磁场强度，使得磁力计数据失去参考价值。

【发明内容】

为了解决上述问题，本发明针对传统的姿态估计算法在高动态环境下估计值容易发散，姿态角受磁干扰影响的问题，提出一种高动态和磁干扰环境下的姿态估计算法，其将俯仰角、滚转角和航向角分层解算，解决了姿态估计算法在高动态环境下精度低、易发散，且姿态角受磁干扰影响的问题，有效提高了姿态估计的精度和鲁棒性。

本发明是通过以下技术方案实现的，提供一种高动态和磁干扰环境下的姿态估计算法，包括以下步骤：

S1第一层姿态估计算法：基于线性化程度更高的误差状态解算俯仰角、滚转角，并选用方向余弦矩阵的最后一行作为俯仰角、滚转角状态变量，选用该状态变量的误差状态在EKF框架下进行更新与校正；

S2第二层姿态估计算法：选用方向余弦矩阵的第一列的前两个元素作为航向角的状态变量，并选用磁力计校正航向信息，使用磁力计模值和模块磁场倾角增量相乘再取对数函数的方法度量磁干扰强度。

特别的，所述S1具体按照以下步骤实施：

S11名义状态预测

选用从载体坐标系b系到导航坐标系n系的姿态旋转矩阵

的最后一行r作为表达俯仰θ、滚转角γ的状态变量，并将陀螺仪的零偏b_g当成状态变量估计，第一层的名义状态向量x₁按如下公式表示：

x₁＝[r b_g]   (1)，

公式(1)中，r按如下公式计算：

r＝[-sinθ sinγcosθ cosγcosθ]   (2)，

状态向量x₁的运动方程按如下公式表示：

公式(3)中，^bΦ_k为[t_k-1,t_k]时间段内的等效旋转矢量，(^bΦ_k)^{^}为^bΦ_k的反对称矩阵；

S12误差状态预测和校正

误差状态的运动方程为：

公式(4)中，δφ为r的误差状态，δb_g为b_g的误差状态，将公式(4)按矩阵形式表示为：

公式(5)中，F_k-1为误差状态δx₁的状态转移矩阵，其按照如下公式计算：

误差状态的先验协方差

的预测方程如下：

于公式(7)中，Q为预测方程的过程噪声矩阵；

引入加速度计对预测结果进行校正，观测方程按如下公式表示：

z_1,k＝h₁(x_1,k,v_k)  (8)，

公式(8)中，z_1,k＝a_m为加速度计测量值，v_k为加速度计测量噪声,由于状态变量是选用姿态旋转矩阵的最后一行，所以h₁(·)是一个线性函数，

根据链式求导法则，观测方程关于δx₁的雅克比矩阵如下：

公式(9)中，H_x1＝[I₃ 0₃]，

因此，误差状态的后验校正过程按如下公式表示：

公式(10)中，R₁为观测方程的噪声矩阵,I₆为6*6的单位矩阵；

S13误差状态的注入和复位

将经过加速度计校正后的误差状态按如下公式注入到名义状态中：

公式(11)中，Exp(δφ)是误差状态到上SO(3)的指数映射，其按照如下公式表示：

Exp(δφ)＝I₃+u^{^}sinφ+(1-cosφ)(u^{^})²  (12)，

将误差状态注入到名义状态之后，需要将误差状态清零，具体按如下公式操作：

特别的，所述S11中，需引入单子样+前一周期的等效旋转矢量补偿不可交换误差，具体按如下公式表示：

公式(14)中，Φ_k为等效旋转矢量，ΔΘ_k为角增量，ΔΘ_k＝ΔT·(^bω_m,k-b_g,k-1)^{^}。

特别的，所述S2具体按照以下步骤实施：

S21航向信息解算

选用从载体坐标系b系到导航坐标系n系的姿态旋转矩阵

的第一列的前两个元素

作为表达航向角

的状态变量，其中第二层的状态向量x₂为：

运动方程按如下公式表示：

公式(16)中，

由

和

计算所得，

状态x₂的先验协方差的预测方程按如下公式表示：

S22航向信息校正

由于磁力计数据并不能直接校正状态x₂，因此需将其转换到和状态x₂同一量纲，状态x₂的后验校正过程按如下公式表示：

公式(18)中，

S23磁干扰的检测

使用模块磁场倾角增量和磁力计模值来评价磁干扰强度，具体评价函数按如下公式表示：

m_len＝log(||^bm_m||sin(|dip-dip_ref|))   (19)，

公式(19)中，dip_ref为无磁干扰情况下的模块磁场倾角，dip为加速度计输出矢量和磁力计输出矢量的夹角，dip按如下公式进行计算：

公式(20)中，a_m为加速度计的测量值,m_m为磁力计的测量值。

特别的，所述S2中，为了减小磁干扰对航向信息的影响，按如下公式将磁干扰强度和观测矩阵R₂关联：

公式(21)中，σ_m为磁力计噪声的标准差，σ_d为磁干扰的基础标准差。

本发明提供一种高动态和磁干扰环境下的姿态估计算法，其将俯仰、滚转角的解算基于线性化程度更高的误差状态进行，并选用方向余弦矩阵的最后一行作为其状态变量，减小了***非线性；同时，磁力计只用来校正航向信息，彻底避免了磁干扰对俯仰、滚转角的影响；使用模块磁场倾角增量和磁场模值相乘再取对数函数的方法度量磁干扰强度，并用其动态调节观测噪声的大小，使得航向角在短时间的磁干扰环境中也不受影响。

【附图说明】

图1为本发明一种高动态和磁干扰环境下的姿态估计算法的计算框图；

图2为磁干扰检测图；

图3为本发明与EKF算法、ESKF算法在磁干扰环境下的对比结果图；

图4为本发明与EKF算法、ESKF算法在高动态环境下的对比结果图。

【具体实施方式】

为了使本发明的目的、技术方案及优点更加清楚明白，以下对本发明进一步详细说明。

请参阅图1，本发明针对传统的姿态估计算法在高动态环境下估计值容易发散，姿态角受磁干扰影响的问题，提出一种基于误差状态的双层解耦姿态估计算法，即一种高动态和磁干扰环境下的姿态估计算法，具体按照如下步骤实施：

S1第一层姿态估计算法

于本发明中，第一层姿态估计算法只对俯仰角θ、滚转角γ进行估计，且只用加速度计对预测值做校正，将磁力计和俯仰角、滚转角彻底解耦，从根本上避免了磁干扰对俯仰角、滚转角的影。为了使本发明提供的估计算法适应高动态环境，使用线性化程度更高的误差状态做更新和校正。将***真实状态分解成名义状态(nominal-state)和误差状态(error-state)，名义状态是大的状态量继承了***的非线性，但不考虑噪声，如此就不包含不确定度，误差状态是真实状态和名义状态之差，包含了传感器噪声和模型噪声。

S11名义状态预测

选用从载体坐标系(b系)到导航坐标系(n系)的姿态旋转矩阵

的最后一行r作为表达俯仰、滚转角的状态变量，并将陀螺仪的零偏b_g也当成状态变量估计。

其中，第一层的名义状态向量x₁按如下公式表示：

x₁＝[r b_g]   (1)，

公式(1)中，r按如下公式进行计算：

r＝[-sinθ sinγcosθ cosγcosθ]   (2)；

状态向量x₁的运动方程按如下公式表示：

公式(3)中，^bΦ_k为[t_k-1,t_k]时间段内的等效旋转矢量，(^bΦ_k)^{^}为^bΦ_k的反对称矩阵，在此处使用等效旋转矢量的目的是：在高动态的环境下载体不满足定轴转动的假设，若直接假设角增量ΔΘ_k≈ΔT·[^bω_k×]，则会引入不可交换误差降低姿态角的解算精度，因此，按如下公式引入单子样+前一周期的等效旋转矢量补偿不可交换误差：

公式(14)中，Φ_k为等效旋转矢量，ΔΘ_k为角增量，ΔΘ_k＝ΔT·(^bω_m,k-b_g,k-1)^{^}。

S12误差状态预测和校正

名义状态r和b_g的误差状态分别定义为δφ、δb_g，其中，误差状态的运动方程如下：

将公式(4)按矩阵形式表示为：

公式(5)中，F_k-1是误差状态δx₁的状态转移矩阵，按如下公式表示：

误差状态的先验协方差

的预测方程按如下公式表示：

公式(7)中，Q为预测方程的过程噪声矩阵；

由于陀螺仪中存在零偏，只进行预测会造成姿态漂移，因此此处引入加速度计对预测结果进行校正，观测方程按如下公式表示：

z_1,k＝h₁(x_1,k,v_k)   (8)，

公式(8)中，z_1,k＝a_m为加速度计测量值，v_k为加速度计测量噪声。由于状态变量是选用姿态旋转矩阵的最后一行，所以h₁(·)是一个线性函数；

根据链式求导法则，观测方程关于δx₁的雅克比矩阵如下：

公式(9)中，H_x1＝[I₃ 0₃]，

因此，误差状态的后验校正过程按如下公式表示：

公式(10)中，R₁为观测方程的噪声矩阵，I₆为6*6的单位矩阵。

S13误差状态的注入和复位

将经过加速度计校正后的误差状态按如下公式注入到名义状态中：

公式(11)中，Exp(δφ)是误差状态到上SO(3)的指数映射，其计算公式根据罗德里格斯公式给出，如下：

Exp(δφ)＝I₃+u^sinφ+(1-cosφ)(u^)²   (12)，

将误差状态注入到名义状态之后，需要将误差状态清零，具体按如下同时操作：

S2第二层姿态估计算法

第二层姿态估计算法使用Kalman滤波来估计航向角

使用磁力计信息校正预测值。

S21航向信息解算

选用从b系到n系的姿态旋转矩阵

的第一列的前两个元素

作为表达航向角的状态变量。其中第二层的状态向量x₂为：

运动方程按如下公式表示：

公式(16)中，

由

和

计算出来；

状态x₂的先验协方差的预测方程如下：

S22航向信息校正

校正过程需要引入磁力计对航向信息进行校正，而磁力计数据并不能直接校正状态x₂，因此需将其转换到和状态x₂同一量纲，状态x₂的后验校正过程按如下公式表示：

公式(18)中，

S22磁干扰的检测

模块磁场倾角是模块检测到的磁场通量线与地球表面之间的夹角，当磁干扰发生时模块磁场倾角会发生改变，但是其角度变化方向不确定，可能增大，也可能减小。磁力计模值可用来度量磁干扰的强度，其变化方向也不能完全确定，一般情况下，磁干扰越强，模值越大。

鉴于磁干扰发生的情况非常复杂，为了更精准的度量磁干扰强度，本发明使用模块磁场倾角增量和磁力计模值来评价磁干扰强度，具体评价函数按如下公式表示：

m_len＝log(||^bm_m||sin(|dip-dip_ref|))   (19)，

公式(19)中，dip_ref为无磁干扰情况下的模块磁场倾角，dip为加速度计输出矢量和磁力计输出矢量的夹角，按如下公式计算：

公式(20)中，a_m为加速度计的测量值，m_m为磁力计的测量值；

为了减小磁干扰对航向信息的影响，本发明将磁干扰强度和观测矩阵R₂按如下公式关联：

公式(21)中，σ_m为磁力计噪声的标准差，σ_d为磁干扰的基础标准差。

为了验证本发明提供的高动态和磁干扰环境下的姿态估计算法的有效性，选用XSENS公司的MTI-G-710模块对其进行验证，实验过程中记录传感器模块中加速度计、陀螺仪和磁力计的原始信号，传感器模块采样频率为100HZ，之后再将传感器原始信号导入到matlab进行分析和实验。以下通过磁干扰实验、高动态环境实验对本发明提供的姿态估计算法进行验证。

1、磁干扰实验

首先选用该实验可对本发明提到的使用模块磁场倾角增量和磁场模值相乘再取对数函数度量磁干扰强度的方法进行验证。具体实验方法如下：

将模块放置在无磁干扰的环境中并保持静止，然后将一块磁铁由远及近靠近模块再逐渐远离。分别使用模块磁场倾角、磁力计模值、本发明提到的方法去度量磁干扰程度，对比结果如图2所示。

由图2可知，在磁干扰实验中，模块磁场倾角没有及时反映磁干扰，而磁场强度出现了先下降而后上升的趋势，只有本发明提供的算法能及时并正确的反映磁干扰，因此本发明提供的方法明显由于前两者。

其次验证磁干扰对姿态角的影响，此处先将模块拿到空中做小幅度运动，然后将模块保持静止，再慢慢用磁力靠近，分别运行本文算法、EKF算法、ESKF算法，观察姿态角受影响程度，对比结果如图3所示。

由图3可知，EKF算法对磁干扰没有抵抗力，不仅航向角受到影响，俯仰、横滚角也受到了影响，而本发明提供的算法和ESKF算法则几乎不受磁干扰的影响，且由图3(3)可知，本发明提供的算法在磁铁靠近时抵抗力更优于ESKF算法。

2、高动态环境实验

将模块放置在三轴转台上，控制转台进行高动态运动，将转台输出的平台角度作为真值。使用本发明所提供的算法、EKF算法、ESKF算法分别进行姿态估计，并将解算出的姿态角和真值做差。对比结果如图4所示：

由图4可知，EKF算法在高动态环境下表现很差，在运动的初期就表现出了巨大的误差；ESKF算法和本发明在俯仰、滚转角上误差小于±2°，两者相差不大，在航向角上ESKF算法虽然在运动初期可以保持良好的精度，但随着高动态环境的持续，航向角出现了10°的漂移。

综上所述，本发明针对在高动态场景下由于非线性误差过大导致姿态估计算法容易发散和姿态角容易受到磁干扰影响的问题，提供了一种高动态和磁干扰环境下的姿态估计算法，其使用误差状态做更新和校正，利用误差状态是小量的特性，保证线性化一直有效；并使用旋转矩阵的最后一行作为状态变量，进一步降低了***的非线性，使得本发明提供的算法可以适应高动态环境；通过分层计算，俯仰角、滚转角不使用磁力计校正，彻底规避了磁力计干扰对这两个角度的影响，同时通过综合考虑磁力计模值和模块磁场倾角增量来度量磁干扰程度，并将其和第二层姿态估计算法的观测矩阵关联，使得航向角也能在短时间的磁干扰环境中保持一定的精度。

Claims (3)

1.一种高动态和磁干扰环境下的姿态估计算法，其特征在于，包括以下步骤：

S1第一层姿态估计算法：基于线性化程度更高的误差状态解算俯仰角、滚转角，并选用方向余弦矩阵的最后一行作为俯仰角、滚转角状态变量，选用该状态变量的误差状态在EKF框架下进行更新与校正；

S2第二层姿态估计算法：选用方向余弦矩阵的第一列的前两个元素作为航向角的状态变量，并选用磁力计校正航向信息，使用磁力计模值和磁倾角增量相乘再取对数函数的方法度量磁干扰强度；S21航向信息解算

选用从载体坐标系b系到导航坐标系n系的姿态旋转矩阵的第一列的前两个元素作为表达航向角的状态变量，其中第二层的状态向量x₂为：

运动方程按如下公式表示：

公式(16)中，由和计算所得，

状态x₂的先验协方差的预测方程按如下公式表示：

S22航向信息校正

由于磁力计数据并不能直接校正状态x₂，因此需将其转换到和状态x₂同一量纲，状态x₂的后验校正过程按如下公式表示：

公式(18)中，

S23磁干扰的检测

使用磁倾角和磁力计模值来评价磁干扰强度，具体评价函数按如下公式表示：

m_len＝log(||^bm_m||sin(|dip-dip_ref|))                  (19)，

公式(19)中，dip_ref为无磁干扰情况下的磁倾角，dip为加速度计输出矢量和磁力计输出矢量的夹角，dip按如下公式进行计算：

公式(20)中，a_m为加速度计的测量值,m_m为磁力计的测量值；

所述S2中，为了减小磁干扰对航向信息的影响，按如下公式将磁干扰强度和观测矩阵R₂关联：

公式(21)中，σ_m为磁力计噪声的标准差，σ_d为磁干扰的基础标准差。

2.根据权利要求1所述一种高动态和磁干扰环境下的姿态估计算法，其特征在于，所述S1具体按照以下步骤实施：

S11名义状态预测

选用从载体坐标系b系到导航坐标系n系的姿态旋转矩阵的最后一行r作为表达俯仰θ、滚转角γ的状态变量，并将陀螺仪的零偏b_g当成状态变量估计，第一层的名义状态向量x₁按如下公式表示：

x₁＝[r b_g]                           (1)，

公式(1)中，r按如下公式计算：

r＝[-sinθ sinγcosθ cosγcosθ]                       (2)，

状态向量x₁的运动方程按如下公式表示：

公式(3)中，^bΦ_k为[t_k-1,t_k]时间段内的等效旋转矢量，(^bΦ_k)^为^bΦ_k的反对称矩阵；

S12误差状态预测和校正

误差状态的运动方程为：

公式(4)中，δφ为r的误差状态，δb_g为b_g的误差状态，将公式(4)按矩阵形式表示为：

公式(5)中，F_k-1为误差状态δx₁的状态转移矩阵，其按照如下公式计算：

误差状态的先验协方差的预测方程如下：

于公式(7)中，Q为预测方程的过程噪声矩阵；

引入加速度计对预测结果进行校正，观测方程按如下公式表示：

z_1,k＝h₁(x_1,k,v_k)                        (8)，

公式(8)中，z_1,k＝a_m为加速度计测量值，v_k为加速度计测量噪声,由于状态变量是选用姿态旋转矩阵的最后一行，所以h₁(·)是一个线性函数，

根据链式求导法则，观测方程关于δx₁的雅克比矩阵如下：

公式(9)中，H_x1＝[I₃ 0₃]，

因此，误差状态的后验校正过程按如下公式表示：

公式(10)中，R₁为观测方程的噪声矩阵，I₆为6*6的单位矩阵；

S13误差状态的注入和复位

将经过加速度计校正后的误差状态按如下公式注入到名义状态中：

公式(11)中，Exp(δφ)是误差状态到上SO(3)的指数映射，其按照如下公式表示：

Exp(δφ)＝I₃+u^sinφ+(1-cosφ)(u^)²           (12)，

将误差状态注入到名义状态之后，将误差状态清零，具体按如下公式操作：

3.根据权利要求2所述一种高动态和磁干扰环境下的姿态估计算法，其特征在于，所述S11中，需引入单子样+前一周期的等效旋转矢量补偿不可交换误差，具体按如下公式表示：

公式(14)中，Φ_k为等效旋转矢量，ΔΘ_k为角增量，ΔΘ_k＝ΔT·(^bω_m,k-b_g,k-1)^。

Images