CN112683269A - 一种附有运动加速度补偿的marg姿态计算方法 - Google Patents

Abstract

本发明公开了本发明提出一种附有运动加速度补偿的MARG姿态计算方法，属于导航领域。该方法首先利用陀螺仪进行姿态一步预测，然后利用一步预测值对加速度计与磁力计进行检测，最后针对不同检测结果执行相应的数据融合算法。通过使用本方法进行MARG传感器姿态估计，有效对载体运动加速度与磁场干扰进行检测；针对运动载体的动态特征，引入滑动窗口数据拟合运动加速度变化规律并进行相应的模型补偿；推导重磁(加速度和磁感应强度测量值)矢量对的在Wahba问题下的解析解从而加快姿态求解速度，显著提高MARG姿态估计效率。

Description

一种附有运动加速度补偿的MARG姿态计算方法

技术领域

本发明属于导航领域，具体是附有滑动递推窗口运动加速度补偿的重磁矢量对快速解析的MARG姿态估计方法。

背景技术

近几十年来，随着微电子机械(Micro-Electro-Mechanical System,MEMS)技术的发展，MEMS惯性传感器被广泛应用于工业领域和科学研究，用以提供位置姿态信息。由三轴陀螺仪和三轴加速度计构成的MEMS惯性传感器，搭配三轴磁传感器的组合模块，被称为MARG(magnetic,angular rate and gravity,MARG)传感器。为了最佳地融合MARG传感器数据，发展了众多数据融合方法，主要分为两大类。其中一类是加速度计和陀螺仪互补滤波的方法，基于加速度计和陀螺仪输出信息在频域具有互补特性。偏航角可通过磁传感器进行进一步修正。互补滤波具有计算时间短，易于在硬件平台应用等特点，然而其只能提供次优估计。为获得更加准确的姿态，另一类方法——线性卡尔曼滤波(Kalman filter,KF)被用于数据融合，其可获得最小均方差意义下的最优估计。此外，针对非线性模型，发展了各类非线性滤波器如扩展KF、无迹KF和粒子KF，用以估计载***置与姿态。然而，MARG传感器姿态估计易受非重力加速度及磁场干扰的影响。这些干扰将导致***不再满足滤波假设模型，引起滤波估计有偏甚至发散。为解决这些问题，需建立加速度计与磁力计检测准则，进行异常数据检测。

发明内容

针对上述问题，本发明提出一种附有运动加速度补偿的MARG姿态计算方法；该方法首先利用陀螺仪进行姿态一步预测，然后利用一步预测值对加速度计与磁力计进行检测，最后针对不同检测结果执行相应的数据融合算法。通过使用本方法进行MARG传感器姿态估计，有效对载体运动加速度与磁场干扰进行检测；针对运动载体的动态特征，引入滑动窗口数据拟合运动加速度变化规律并进行相应的模型补偿；推导重磁(加速度和磁感应强度测量值)矢量对的在Wahba问题下的解析解从而加快姿态求解速度，显著提高MARG姿态估计效率。

本发明的技术方案为：一种附有运动加速度补偿的MARG姿态计算方法，具体包括以下步骤：

步骤1：利用陀螺仪进行一步预测；

步骤2：利用一步预测值，对加速度计与磁力计进行检测，通过滑动窗口多项式拟合，对运动加速度进行补偿；

步骤3：针对不同的检测结果执行相应的融合算法：

无磁场干扰，无运动加速度(或运动加速度已补偿)：转至步骤4；

存在磁干扰，无运动加速度(或运动加速度已补偿)：转步骤5；

无磁干扰，存在运动加速度且运动加速度未补偿，转步骤6；

存在磁干扰，存在运动加速度且运动加速度未补偿：将陀螺推算的一步预测结果作为姿态估计值，等待下一时刻的传感器数据。

步骤4：建立加速度和磁场强度的矢量对方程，通过快速线性四元数估计方法求解获得量测信息，基于卡尔曼滤波进行姿态估计。

步骤5：利用加速度计数据建立量测模型，基于非线性滤波实现状态估计，并对磁干扰进行估计。

步骤6：利用磁力计数据建立量测模型，基于非线性滤波实现状态估计。

本发明公开了一种附有滑动递推窗口运动加速度补偿的重磁矢量对快速解析的MARG姿态估计方法。该方法首先利用陀螺仪进行姿态一步预测，然后利用一步预测值对加速度计与磁力计进行检测，最后针对不同检测结果执行相应的数据融合算法。通过使用本方法进行MARG传感器姿态估计，有效对载体运动加速度与磁场干扰进行检测；针对运动载体的动态特征，引入滑动窗口数据拟合运动加速度变化规律并进行相应的模型补偿；推导重磁(加速度和磁感应强度测量值)矢量对的在Wahba问题下的解析解从而加快姿态求解速度，显著提高MARG姿态估计效率。

附图说明

图1是工作模式流程图。

具体实施方式

下面结合附图，对本发明中运动加速度补偿与磁场干扰检测的MARG姿态估计方法进行详细说明：

步骤1：利用陀螺仪进行一步预测：

建立***状态模型：

x_k＝Φ_k-1,kx_k-1+w_k (1)

其中，下标k表示t_k时刻，x_k表示t_k时刻的状态，此模型中取姿态四元数q作为状态，即取x＝q；w_k为状态模型噪声；由t_k-1时刻到t_k时刻的状态转移矩阵Φ_k-1,k为：

其中，I₄表示4维单位阵，T_s表示采样间隔，M(ω_k)定义如下：

其中，ω_x，ω_y，ω_z分别表示x，y，z方向上的角速率；ω_k表示3维角速率矢量，即ω_k＝[ω_x ω_y ω_z]^T；上标T表示矩阵转置；

进行一步预测：

P_k|k-1＝Φ_k-1,kP_k-1(Φ_k-1,k)^T+Q_k (5)

其中，上标^表示矢量为预测量，

表示从t_k-1时刻到t_k时刻的状态一步预测值，P_k|k-1表示一步预测

的方差，P_k-1表示t_k-1时刻状态最优估计

的方差，Q_k表示状态模型噪声w_k的方差；

步骤2：利用一步预测值，对加速度计与磁力计进行检测，具体如下：

步骤2-1：借助角速率姿态更新方程(4)，得t_k时刻姿态的一步预测值

(即

)，其对应的载体坐标系(b系)到导航坐标系(n系)的方向余弦矩阵为

简记为

其中方向余弦矩阵与四元数的关系如下：

其中，q₀，q₁，q₂，q₃分别表示姿态四元数q中的四个分量，即q＝[q₀,q₁,q₂,q₃]^T.

步骤2-2：对磁场干扰进行检测：

磁力计t_k时刻输出记为

有磁力计输出模型：

其中，上标b表示载体坐标系；r_mag(k)表示地磁矢量；b_mag(k)表示磁场干扰量；ε_mag(k)为磁力计噪声，其方差记为

其中diag表示对角矩阵，

分别表示三轴磁场噪声的方差；计算磁场干扰检测量δr_k：

上标n表示导航坐标系，

表示导航坐标系下的地磁矢量；根据3σ原则，当δr_k三轴分量满足：

认为无磁场干扰；反之，则磁场干扰存在；

步骤2-3：对运动加速度进行检测，当运动加速度持续时间大于设定阈值时，进行运动加速度补偿：

加速度计输出模型为：

其中，

表示加速度计的输出值，g表示重力加速度；a_k表示载体非重力加速度，ε_acc(k)为加速度计噪声；

计算运动加速度检测量δa_k：

表示由(4)算得的姿态一步预测值，

表示加速度计的输出；根据3σ原则，当δa_k三轴分量满足：

认为无运动加速度；反之，则运动加速度存在；其中σ_acc(x),σ_acc(y),σ_acc(z)分别表示加速度计三轴噪声标准差；

当运动加速度持续时间大于设定阈值时，通过滑动窗口进行运动加速度补偿：

对窗口长度为n的运动加速度数据{δa_k-n,…,δa_k-1}进行多项式曲线拟合，拟合阶数选择0阶、或1阶、或2阶、或高阶；多项式拟合阶数和窗口长度根据载体实际运动快慢情况及加速度计采样率进行调整；

借助拟合多项式计算得t_k时刻运动加速度拟合值

根据方差传播定理确定运动加速度拟合值

的方差

将拟合值补偿至

补偿后有：

经补偿后，用于滤波姿态解算，方差记为

表示加速度计噪声的方差；

步骤3：针对不同的检测结果执行相应的融合算法：

无磁场干扰，无运动加速度或运动加速度已补偿：转至步骤4；

存在磁干扰，无运动加速度或运动加速度已补偿：转步骤5；

无磁干扰，存在运动加速度且运动加速度未补偿，转步骤6；

存在磁干扰，存在运动加速度且运动加速度未补偿：将陀螺推算的一步预测结果作为姿态估计值，等待下一时刻的传感器数据；

步骤4：建立加速度和磁场强度的矢量对方程，通过快速线性四元数姿态估计方法求解获得量测信息，基于卡尔曼滤波进行姿态估计；具体如下：

步骤4-1：建立加速度和磁感应强度的矢量对方程：

若选取n系为东北天坐标系，则有gⁿ＝[0 0 -g]^T，mⁿ＝[0 m_N m_U]^T，其中m_N和m_U分别表示北向与天向的地磁强度，其中，q表示姿态四元数矢量，y_acc表示加速度矢量(无运动加速度时取加速度计的输出

运动加速度已补偿时取补偿后的加速度

)，y_mag表示磁感应强度(无磁干扰情况下即磁力计的输出

)，ε_acc表示加速度噪声，ε_mag表示磁感应强度噪声；

步骤4-2：通过快速解算获得量测信息

从四元数与方向余弦矩阵关系出发，建立四元数特征值与特征向量方程，给出特征值数值计算公式，基于特征值接近1的数值特性，快速选出真实解；随后，进行矩阵初等变换，得到矢量对方程的四元数解；

计算中间变量矩阵

其中，H_x，H_y，H_z，W为计算过程中的中间变量矩阵，I₃表示3维单位阵，H_x1，H_x2，H_x3分别表示3维矩阵H_x的第1、2、3个分量；H_y1，H_y2，H_y3分别表示3维矩阵H_y的第1、2、3个分量，H_z1，H_z2，H_z3分别表示3维矩阵H_z的第1、2、3个分量；

计算中间参数：

τ₂＝8(H_x3H_y2H_z1-H_x2H_y3H_z1-H_x3H_y1H_z2+H_x1H_y3H_z2+H_x2H_y1H_z3-H_x1H_y2H_z3)

τ₃＝det(W)

其中，det表示求矩阵行列式。

计算特征值：

从(17)中选取最接近1的数作为最终的特征值λ，即

λ＝argmin|λ_i-1|,(i＝1,2,3,4) (18)

其中，argmin表示使得后面式子达到最小时自变量的取值；||表示取绝对值。进行初等变换，

其中，χ，ρ，υ，ζ表示完成初等变换后的矩阵最后一列的四个元素；

获得归一化的四元数

步骤4-3：建立滤波量测模型：

其中，z_k表示量测矢量，取

量测转移矩阵为H_k＝I₄，v_k为量测噪声；

步骤4-4：基于卡尔曼滤波进行姿态估计：

P_k＝(I₄-K_kH_k)P_k|k-1 (24)

其中，K_k表示滤波增益，R_k表示量测噪声v_k的方差，P_k|k-1和P_k分别表示状态一步预测

和状态最优估计

的方差；上标-1表示矩阵求逆运算；

步骤5：利用加速度计数据建立量测模型，基于无迹卡尔曼滤波实现状态估计，并对磁干扰进行估计；具体如下：

步骤5-1：利用加速度计数据建立量测模型：

y_acc＝-C^T(q)gⁿ+ε_acc (25)

步骤5-2：针对***非线性模型，采用非线性滤波(如UKF)进行状态估计：

步骤6：利用磁力计数据建立量测模型，基于无迹卡尔曼滤波实现状态估计；具体如下：

步骤6-1：利用磁力计数据建立量测模型：

y_mag＝C^T(q)mⁿ+ε_mag (26)

步骤6-2：针对***非线性模型，采用非线性滤波进行状态估计。

Claims (2)

1.一种附有运动加速度补偿的MARG姿态计算方法，该方法包括：

步骤1：利用陀螺仪进行一步预测：

建立***状态模型：

x_k＝Φ_k-1,kx_k-1+w_k (1)

其中，下标k表示t_k时刻，x_k表示t_k时刻的状态，此模型中取姿态四元数q作为状态，即取x＝q；w_k为状态模型噪声；由t_k-1时刻到t_k时刻的状态转移矩阵Φ_k-1,k为：

其中，I₄表示4维单位阵，T_s表示采样间隔，M(ω_k)定义如下：

其中，ω_x，ω_y，ω_z分别表示x，y，z方向上的角速率；ω_k表示3维角速率矢量，即ω_k＝[ω_x ω_y ω_z]^T；上标T表示矩阵转置；

进行一步预测：

P_k|k-1＝Φ_k-1,kP_k-1(Φ_k-1,k)^T+Q_k (5)

其中，上标

表示矢量为预测量，

表示从t_k-1时刻到t_k时刻的状态一步预测值，P_k|k-1表示一步预测

的方差，P_k-1表示t_k-1时刻状态最优估计

的方差，Q_k表示状态模型噪声w_k的方差；

步骤2：利用一步预测值，对加速度计与磁力计进行检测，具体如下：

步骤2-1：借助角速率姿态更新方程(4)，得t_k时刻姿态的一步预测值

(即

)，其对应的载体坐标系(b系)到导航坐标系(n系)的方向余弦矩阵为

简记为

其中方向余弦矩阵与四元数的关系如下：

其中，q₀，q₁，q₂，q₃分别表示姿态四元数q中的四个分量，即q＝[q₀,q₁,q₂,q₃]^T.

步骤2-2：对磁场干扰进行检测：

磁力计t_k时刻输出记为

有磁力计输出模型：

其中，上标b表示载体坐标系；r_mag(k)表示地磁矢量；b_mag(k)表示磁场干扰量；ε_mag(k)为磁力计噪声，其方差记为

其中diag表示对角矩阵，

分别表示三轴磁场噪声的方差；计算磁场干扰检测量δr_k：

上标n表示导航坐标系，

表示导航坐标系下的地磁矢量；根据3σ原则，当δr_k三轴分量满足：

认为无磁场干扰；反之，则磁场干扰存在；

步骤2-3：对运动加速度进行检测，当运动加速度持续时间大于设定阈值时，进行运动加速度补偿：

加速度计输出模型为：

其中，

表示加速度计的输出值，g表示重力加速度；a_k表示载体非重力加速度，ε_acc(k)为加速度计噪声；

计算运动加速度检测量δa_k：

表示由(4)算得的姿态一步预测值，

表示加速度计的输出；根据3σ原则，当δa_k三轴分量满足：

认为无运动加速度；反之，则运动加速度存在；其中σ_acc(x),σ_acc(y),σ_acc(z)分别表示加速度计三轴噪声标准差；

当运动加速度持续时间大于设定阈值时，通过滑动窗口进行运动加速度补偿：

对窗口长度为n的运动加速度数据{δa_k-n,…,δa_k-1}进行多项式曲线拟合，拟合阶数选择0阶、或1阶、或2阶、或高阶；多项式拟合阶数和窗口长度根据载体实际运动快慢情况及加速度计采样率进行调整；

借助拟合多项式计算得t_k时刻运动加速度拟合值

根据方差传播定理确定运动加速度拟合值

的方差

将拟合值补偿至

补偿后有：

经补偿后，用于滤波姿态解算，方差记为

表示加速度计噪声的方差；

步骤3：针对不同的检测结果执行相应的融合算法：

无磁场干扰，无运动加速度或运动加速度已补偿：转至步骤4；

存在磁干扰，无运动加速度或运动加速度已补偿：转步骤5；

无磁干扰，存在运动加速度且运动加速度未补偿，转步骤6；

存在磁干扰，存在运动加速度且运动加速度未补偿：将陀螺推算的一步预测结果作为姿态估计值，等待下一时刻的传感器数据；

步骤4：建立加速度和磁场强度的矢量对方程，通过快速线性四元数姿态估计方法求解获得量测信息，基于卡尔曼滤波进行姿态估计；

步骤5：利用加速度计数据建立量测模型，基于无迹卡尔曼滤波实现状态估计，并对磁干扰进行估计；具体如下：

步骤5-1：利用加速度计数据建立量测模型：

y_acc＝-C^T(q)gⁿ+ε_acc (25)

步骤5-2：针对***非线性模型，采用非线性滤波进行状态估计：

步骤6：利用磁力计数据建立量测模型，基于无迹卡尔曼滤波实现状态估计；具体如下：

步骤6-1：利用磁力计数据建立量测模型：

y_mag＝C^T(q)mⁿ+ε_mag (26)

步骤6-2：针对***非线性模型，采用非线性滤波进行状态估计。

2.如权利要求1所述的一种附有运动加速度补偿的MARG姿态计算方法，其特征在于，所述步骤4的具体方法为：

步骤4-1：建立加速度和磁感应强度的矢量对方程：

若选取n系为东北天坐标系，则有gⁿ＝[0 0 -g]^T，mⁿ＝[0 m_N m_U]^T，其中m_N和m_U分别表示北向与天向的地磁强度，其中，q表示姿态四元数矢量，y_acc表示加速度矢量(无运动加速度时取加速度计的输出

运动加速度已补偿时取补偿后的加速度

)；

y_mag表示磁感应强度(无磁干扰情况下即磁力计的输出

)，

表示加速度噪声，ε_mag表示磁感应强度噪声；

步骤4-2：通过快速解算获得量测信息

从四元数与方向余弦矩阵关系出发，建立四元数特征值与特征向量方程，给出特征值数值计算公式，基于特征值接近1的数值特性，快速选出真实解；随后，进行矩阵初等变换，得到矢量对方程的四元数解；

计算中间变量矩阵

其中，H_x，H_y，H_z，W为计算过程中的中间变量矩阵，I₃表示3维单位阵，H_x1，H_x2，H_x3分别表示3维矩阵H_x的第1、2、3个分量；H_y1，H_y2，H_y3分别表示3维矩阵H_y的第1、2、3个分量，H_z1，H_z2，H_z3分别表示3维矩阵H_z的第1、2、3个分量；

计算中间参数：

其中，det表示求矩阵行列式；

计算特征值：

从(17)中选取最接近1的数作为最终的特征值λ，即

λ＝argmin|λ_i-1|,(i＝1,2,3,4) (18)

其中，argmin表示使得后面式子达到最小时自变量的取值；| |表示取绝对值；进行初等变换，

其中，χ，ρ，υ，ζ表示完成初等变换后的矩阵最后一列的四个元素；

获得归一化的四元数

步骤4-3：建立滤波量测模型：

其中，z_k表示量测矢量，取

量测转移矩阵为H_k＝I₄，v_k为量测噪声；

步骤4-4：基于卡尔曼滤波进行姿态估计：

P_k＝(I₄-K_kH_k)P_k|k-1 (24)

其中，K_k表示滤波增益，R_k表示量测噪声v_k的方差，P_k|k-1和P_k分别表示状态一步预测

和状态最优估计

的方差；上标-1表示矩阵求逆运算。

Images