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

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

【技术实现步骤摘要】
一种附有运动加速度补偿的MARG姿态计算方法


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

技术介绍

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

技术实现思路

[0003]针对上述问题，本专利技术提出一种附有运动加速度补偿的MARG姿态计算方法；该方法首先利用陀螺仪进行姿态一步预测，然后利用一步预测值对加速度计与磁力计进行检测，最后针对不同检测结果执行相应的数据融合算法。通过使用本方法进行MARG传感器姿态估计，有效对载体运动加速度与磁场干扰进行检测；针对运动载体的动态特征，引入滑动窗口数据拟合运动加速度变化规律并进行相应的模型补偿；推导重磁(加速度和磁感应强度测量值)矢量对的在Wahba问题下的解析解从而加快姿态求解速度，显著提高MARG姿态估计效率。
[0004]本专利技术的技术方案为：一种附有运动加速度补偿的MARG姿态计算方法，具体包括以下步骤：
[0005]步骤1：利用陀螺仪进行一步预测；
[0006]步骤2：利用一步预测值，对加速度计与磁力计进行检测，通过滑动窗口多项式拟合，对运动加速度进行补偿；
[0007]步骤3：针对不同的检测结果执行相应的融合算法：
[0008]无磁场干扰，无运动加速度(或运动加速度已补偿)：转至步骤4；
[0009]存在磁干扰，无运动加速度(或运动加速度已补偿)：转步骤5；
[0010]无磁干扰，存在运动加速度且运动加速度未补偿，转步骤6；
[0011]存在磁干扰，存在运动加速度且运动加速度未补偿：将陀螺推算的一步预测结果作为姿态估计值，等待下一时刻的传感器数据。
[0012]步骤4：建立加速度和磁场强度的矢量对方程，通过快速线性四元数估计方法求解获得量测信息，基于卡尔曼滤波进行姿态估计。
[0013]步骤5：利用加速度计数据建立量测模型，基于非线性滤波实现状态估计，并对磁干扰进行估计。
[0014]步骤6：利用磁力计数据建立量测模型，基于非线性滤波实现状态估计。
[0015]本专利技术公开了一种附有滑动递推窗口运动加速度补偿的重磁矢量对快速解析的MARG姿态估计方法。该方法首先利用陀螺仪进行姿态一步预测，然后利用一步预测值对加速度计与磁力计进行检测，最后针对不同检测结果执行相应的数据融合算法。通过使用本方法进行MARG传感器姿态估计，有效对载体运动加速度与磁场干扰进行检测；针对运动载体的动态特征，引入滑动窗口数据拟合运动加速度变化规律并进行相应的模型补偿；推导重磁(加速度和磁感应强度测量值)矢量对的在Wahba问题下的解析解从而加快姿态求解速度，显著提高MARG姿态估计效率。
附图说明
[0016]图1是工作模式流程图。
具体实施方式
[0017]下面结合附图，对本专利技术中运动加速度补偿与磁场干扰检测的MARG姿态估计方法进行详细说明：
[0018]步骤1：利用陀螺仪进行一步预测：
[0019]建立系统状态模型：
[0020]x
k
＝Φ
k
‑
1,k
x
k
‑1+w
k
ꢀꢀꢀꢀꢀ
(1)
[0021]其中，下标k表示t
k
时刻，x
k
表示t
k
时刻的状态，此模型中取姿态四元数q作为状态，即取x＝q；w
k
为状态模型噪声；由t
k
‑1时刻到t
k
时刻的状态转移矩阵Φ
k
‑
1,k
为：
[0022][0023]其中，I4表示4维单位阵，T
s
表示采样间隔，M(ω
k
)定义如下：
[0024][0025]其中，ω
x
，ω
y
，ω
z
分别表示x，y，z方向上的角速率；ω
k
表示3维角速率矢量，即ω
k
＝[ω
x ω
y ω
z
]T
；上标T表示矩阵转置；
[0026]进行一步预测：
[0027][0028]P
k|k
‑1＝Φ
k
‑
1,k
P
k
‑1(Φ
k
‑
1,k
)
T
+Q
k
ꢀꢀꢀꢀꢀꢀ
(5)
[0029]其中，上标^表示矢量为预测量，表示从t
k
‑1时刻到t
k
时刻的状态一步预测值，P
k|k
‑1表示一步预测的方差，P
k
‑1表示t
k
‑1时刻状态最优估计的方差，Q
k
表示状态模型噪声w
k
的方差；
[0030]步骤2：利用一步预测值，对加速度计与磁力计进行检测，具体如下：
[0031]步骤2
‑
1：借助角速率姿态更新方程(4)，得t
k
时刻姿态的一步预测值(即)，其对应的载体坐标系(b系)到导航坐标系(n系)的方向余弦矩阵为简记为其中方向余弦矩阵与四元数的关系如下：
[0032][0033]其中，q0，q1，q2，q3分别表示姿态四元数q中的四个分量，即q＝[q0,q1,q2,q3]T
.
[0034]步骤2
‑
2：对磁场干扰进行检测：
[0035]磁力计t
k
时刻输出记为有磁力计输出模型本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种附有运动加速度补偿的MARG姿态计算方法，该方法包括：步骤1：利用陀螺仪进行一步预测：建立系统状态模型：x
k
＝Φ
k
‑
1,k
x
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
为：其中，I4表示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,k
P
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系)的方向余弦矩阵为简记为其中方向余弦矩阵与四元数的关系如下：其中，q0，q1，q2，q3分别表示姿态四元数q中的四个分量，即q＝[q0,q1,q2,q3]
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：针对不同的检测结果执行相应的融合算法：无磁场干扰，无运动加速度或运动加速度已补...

【专利技术属性】
技术研发人员：周泽波，张泽亮，
申请(专利权)人：电子科技大学，
类型：发明
国别省市：