CN111426318B - 基于四元数-扩展卡尔曼滤波的低成本ahrs航向角补偿方法 - Google Patents

Abstract

基于四元数‑扩展卡尔曼滤波的低成本AHRS航向角补偿方法，方法包括以下步骤：S1、采集MEMS惯性传感器信息；S2、建立MEMS惯性传感器随机误差模型；S3、利用四元数姿态算法建立运动学方程；S4、根据四元数微分方程和陀螺仪随机误差模型建立姿态测量系统状态方程，利用加速度计和磁力计的输出值构建系统的量测方程；S5、利用扩展卡尔曼滤波获取最佳四元数，解算出AHRS姿态角。利用基于四元数‑扩展卡尔曼滤波的低成本AHRS航向角补偿方法可提升AHRS姿态检测模块的航向角姿态精度，算法结构简单解算速度较快。相较于其他姿态测试方案，本方法采用加速度计和磁力计的三轴测量值作为系统的6维观测量，所得结构可信度更高，有效的提升低成本MEMS AHRS的航向角姿态精度。

Description

基于四元数-扩展卡尔曼滤波的低成本AHRS航向角补偿方法

技术领域

本发明涉及微惯性导航技术相关领域，具体涉及一种基于四元数-扩展卡尔曼滤波的低成本AHRS航向角补偿方法。

背景技术

低成本的MEMS惯性器件精度差、噪声高，无法满足载体位置、速度导航要求，而常常用于载体姿态测量。其具有体积小、成本低、重量轻,便于集成、携带等优点，既可应用于小型航空飞行器，车辆自主驾驶，人体关节姿态测量等领域，又可应用于军用领域，如低成本制导弹药、小型无人机、雷达系统等，具有广阔的应用前景。但是MEMS惯性器件误差随时间不断累加，短时间内姿态精度迅速恶化，甚至无法应用。

MEMS惯性器件组成的AHRS姿态检测模块由于MEMS惯性器件的固有属性，输出的姿态信息精度普遍较低，尤其是系统关键器件MEMS陀螺仪噪声大、随机漂移误差严重，很大程度影响系统的测量精度，并且会导致航向角严重漂移，输出姿态可信度不高。目前提高MEMSAHRS的精度主要有两种技术途径。一种是从制造工艺和生产原理入手，通过优化结构设计，从根本上提高器件的精度，降低随机漂移带来的影响，但这种方法必然会带来必然会带来整个系统成本的大幅提升，而且测量精度提高有限，研发时间长；另一种方法是通过引入其他传感器辅助参与陀螺仪的姿态计算，采用数据融合策略对各个传感器取长补短，修正陀螺仪随机漂移误差，实现姿态测量系统测量精度和稳定性的提升。

四元数微分方程在解算过程中涉及到积分运算，由于陀螺仪具有常值漂移，在没有其他辅助传感器对陀螺仪数据进行修正的情况下，直接更新计算四元数会产生较大的累积误差，最终造成姿态估计发散。加速度计和磁力计可共同解算出载体的全姿态，对于陀螺仪姿态测量的漂移误差可以起到修正作用。因此，将加速度计和磁力计的三轴测量值作为系统的6维观测量。本发明通过引入加速度计与磁力计校准陀螺仪姿态解算，利用四元数-扩展卡尔曼滤波算法实现AHRS姿态检测模块的高精度俯仰、航向、横滚角的输出。

发明内容

基于上述低成本MEMS AHRS姿态检测模块因为MEMS惯性传感器噪声大、随机漂移误差严重导致的姿态输出精度低的问题，本发明基于四元数-扩展卡尔曼滤波的低成本AHRS航向角补偿方法。

为实现上述目的本发明具体方案如下：

基于四元数-扩展卡尔曼滤波的低成本AHRS航向角补偿方法，所述方法包括以下步骤：

S1、采集MEMS惯性传感器信息；

S2、建立MEMS惯性传感器随机误差模型；

S3、利用四元数姿态算法建立运动学方程；

S4、根据四元数微分方程和陀螺仪随机误差模型建立姿态测量系统状态方程，利用加速度计和磁力计的输出值构建系统的量测方程；

S5、利用扩展卡尔曼滤波获取最佳四元数，解算出AHRS姿态角。

可选地，所述步骤S1中的采集MEMS惯性传感器信息包括：MEMS陀螺仪数据信息、MEMS加速度计数据信息、MEMS磁力计数据信息。

可选地，所述步骤S2中MEMS惯性传感器随机误差模型包括：MEMS陀螺仪随机误差模型、MEMS加速度计随机误差模型、MEMS磁力计随机误差模型，MEMS陀螺仪随机误差模型具体如下：

式中，下标x、y、z分别代表陀螺仪x、y、z轴；表示陀螺仪实际输出的角速率；/>表示陀螺仪输出的电信号；ω表示理想情况下陀螺仪输出的角速率；S_g表示标度因数误差；J_g表示陀螺仪敏感轴之间交叉藕合系数；C_g表示陀螺仪常值漂移；D_g表示陀螺仪加速度效应系数；ε_g表示陀螺仪随机漂移；u_g表示陀螺仪受到的白噪声；

MEMS加速度计随机误差模型具体如下：

式中，表示加速度计实际输出值；A表示理想情况下加速度计输出的加速度；B_a表示加速度计零偏误差；v_a表示加速度计受到的白噪声；

MEMS磁力计随机误差模型具体如下：

式中，表示磁力计实际输出值；M表示理想情况下磁力计输出的磁场强度；B_m表示磁力计罗差；w_m表示磁力计受到的白噪声。

可选地，所述步骤S3包括：

S3.1、采集陀螺仪的原始数据，通过方向余弦矩阵，求解出欧拉角的初始值，根据陀螺仪的数据来推算理论上的加速度计值，将四元数的四个数量值换算为方向余弦矩阵的第三列元素，代表载体坐标系的值；

S3.2、四元数初始化，其通用初始化方程如下：

S3.3、四元数的更新及规范化，有了初始化的四元数量，再结合姿态数据采样周期及采样数据，对四元数进行更新，从而实时更新数据，实现姿态解算，其更新方程表示为：

式中，ω_X，ω_Y，ω_Z表示陀螺仪在规定周期内完成的三个轴向的角速度采样，在各个采样周期中角速度是变量，将采样值与四元数初始值带入上式中，实现四元数的不断更新；

S3.4、四元数转换为欧拉角。

可选地，所述步骤S4姿态测量系统状态方程和量测方程具体为：

其中：

X(k)＝[q₀(k)，q₁(k)，q₂(k)，q₃(k)，ε_bx(k)，ε_by(k)，ε_bz(k)]；

上式是以四元数和陀螺仪常值漂移误差作为滤波器状态变量的7维状态变量；

上式将加速度计和磁力计的三轴测量值作为系统的6维观测量；

其中，状态转移矩阵为：

量测转移矩阵为：

噪声驱动矩阵为：

可选地，所述步骤S5包括：

S5.1初始化初始状态

初始化状态变量X(0)、量测值Z(0)；

S5.2时间更新，利用状态方程进行状态一步预测：

S5.3量测更新

估计状态变量：

计算滤波器增益：

K(k)＝P(k|k-1)H(k)^T[H(k)P(k|k-1)H(k)^T+R(k)]^-1；

估计均方误差：

P(k|k)＝[I-K(k)H(k)]P(k|k-1)[I-K(k)H(k)]^T+K(k)R(k)K(k)^T；

将经过扩展卡尔曼滤波算法所求得的四元数根据四元数计算方式列成下列矩阵：

至此，得到了只用四元数中的四个变量表示的载体姿态角公式：

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

上式中的γ、θ、ψ分别为系统输出的俯仰轴姿态角、横滚轴姿态角以及航向轴姿态角。

本发明的有益效果在于，利用基于四元数-扩展卡尔曼滤波的低成本AHRS航向角补偿方法可提升AHRS姿态检测模块的航向角姿态精度，算法结构简单解算速度较快。相较于其他姿态测试方案，本方法采用加速度计和磁力计的三轴测量值作为系统的6维观测量，所得结构可信度更高，有效的提升低成本MEMS AHRS的航向角姿态精度。

附图说明

图1为本发明姿态算法流程图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细描述。应当理解，此处所描述的具体实施例仅用于解释本发明，并不用于限定本发明，实施例中记载的前、后均以附图为准，仅用于明确位置关系，并不用于限定。

如图1所示，一种基于四元数-扩展卡尔曼滤波的低成本AHRS航向角补偿方法，所述方法包括以下步骤：

S1、采集MEMS惯性传感器信息，包括：MEMS陀螺仪数据信息、MEMS加速度计数据信息、MEMS磁力计数据信息，发明所涉及的MEMS惯性传感器均为数字型传感器，可直接通过STM32对其内部寄存器进行读写，通过STM32将数据进行采集与整理。

S2、建立MEMS惯性传感器随机误差模型，建立MEMS惯性传感器随机误差模型为后续扩展卡尔曼滤波参数做准备，所述MEMS惯性传感器随机误差模型包括：MEMS陀螺仪随机误差模型、MEMS加速度计随机误差模型、MEMS磁力计随机误差模型；

其中，MEMS陀螺仪随机误差模型具体如下：

式中，下标x、y、z分别代表陀螺仪x、y、z轴。表示陀螺仪实际输出的角速率；/>表示陀螺仪输出的电信号；ω表示理想情况下陀螺仪输出的角速率；S_g表示标度因数误差；J_g表示陀螺仪敏感轴之间交叉藕合系数；C_g表示陀螺仪常值漂移；D_g表示陀螺仪加速度效应系数；ε_g表示陀螺仪随机漂移；u_g表示陀螺仪受到的白噪声。

MEMS加速度计随机误差模型具体如下：

式中，表示加速度计实际输出值；A表示理想情况下加速度计输出的加速度；B_a表示加速度计零偏误差；v_a表示加速度计受到的白噪声。

MEMS磁力计随机误差模型具体如下：

式中，表示磁力计实际输出值；M表示理想情况下磁力计输出的磁场强度；B_m表示磁力计罗差；w_m表示磁力计受到的白噪声。

S3、利用四元数姿态算法建立运动学方程，该步骤包括：

S3.1、采集陀螺仪的原始数据，通过方向余弦矩阵，求解出欧拉角的初始值，根据陀螺仪的数据来推算理论上的加速度计值，将四元数的四个数量值换算为方向余弦矩阵的第三列元素，代表载体坐标系的值。

S3.2、四元数初始化，其通用初始化方程如下：

S3.3、四元数的更新及规范化，有了初始化的四元数量，再结合姿态数据采样周期及采样数据，即可对四元数进行更新，从而实时更新数据，实现姿态解算。

其更新方程可表示为：

式中，ω_X，ω_Y，ω_Z表示陀螺仪在规定周期内完成的三个轴向的角速度采样。在各个采样周期中角速度是变量，将采样值与四元数初始值带入上式中，就可以实现四元数的不断更新。

S3.4、四元数转换为欧拉角。

S4、根据四元数微分方程和陀螺仪随机误差模型建立姿态测量系统状态方程，利用加速度计和磁力计的输出值构建系统的量测方程；

方程具体为：

其中：

X(k)＝[q₀(k)，q₁(k)，q₂(k)，q₃(k)，ε_bx(k)，ε_by(k)，ε_bz(k)]；

上式是以四元数和陀螺仪常值漂移误差作为滤波器状态变量的7维状态变量。

上式将加速度计和磁力计的三轴测量值作为系统的6维观测量。

其中，状态转移矩阵为：

量测转移矩阵为：

噪声驱动矩阵为：

S5、利用扩展卡尔曼滤波获取最佳四元数，解算出AHRS姿态角，具体为：S5.1初始化初始状态

初始化状态变量X(0)、量测值Z(0)；

S5.2时间更新，利用状态方程进行状态一步预测：

S5.3量测更新

估计状态变量：

计算滤波器增益：

K(k)＝P(k|k-1)H(k)^T[H(k)P(k|k-1)H(k)^T+R(k)]^-1；

估计均方误差：

P(k|k)＝[I-K(k)H(k)]P(k|k-1)[I-K(k)H(k)]^T+K(k)R(k)K(k)^T；将经过扩展卡尔曼滤波算法所求得的四元数根据四元数计算方式列成下列矩阵：

至此，得到了只用四元数中的四个变量表示的载体姿态角公式：

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

上式中的γ、θ、ψ分别为系统输出的俯仰轴姿态角、横滚轴姿态角以及航向轴姿态角。

本发明的有益效果在于，利用基于四元数-扩展卡尔曼滤波的低成本AHRS航向角补偿方法可提升AHRS姿态检测模块的航向角姿态精度，算法结构简单解算速度较快。相较于其他姿态测试方案，本方法采用加速度计和磁力计的三轴测量值作为系统的6维观测量，所得结构可信度更高，有效的提升低成本MEMS AHRS的航向角姿态精度。

Claims (2)

1.基于四元数-扩展卡尔曼滤波的低成本AHRS航向角补偿方法，其特征在于，所述方法包括以下步骤：

S1、采集MEMS惯性传感器信息；

S2、建立MEMS惯性传感器随机误差模型；

S3、利用四元数姿态算法建立运动学方程；

S4、根据四元数微分方程和陀螺仪随机误差模型建立姿态测量系统状态方程，利用加速度计和磁力计的输出值构建系统的量测方程；

S5、利用扩展卡尔曼滤波获取最佳四元数，解算出AHRS姿态角；

所述步骤S2中MEMS惯性传感器随机误差模型包括：MEMS陀螺仪随机误差模型、MEMS加速度计随机误差模型、MEMS磁力计随机误差模型，MEMS陀螺仪随机误差模型具体如下：

式中，下标x、y、z分别代表陀螺仪x、y、z轴；表示陀螺仪实际输出的角速率；/>表示陀螺仪输出的电信号；ω表示理想情况下陀螺仪输出的角速率；S_g表示标度因数误差；J_g表示陀螺仪敏感轴之间交叉耦合系数；C_g表示陀螺仪常值漂移；D_g表示陀螺仪加速度效应系数；ε_g表示陀螺仪随机漂移；u_g表示陀螺仪受到的白噪声；

MEMS加速度计随机误差模型具体如下：

式中，表示加速度计实际输出值；A表示理想情况下加速度计输出的加速度；B_a表示加速度计零偏误差；v_a表示加速度计受到的白噪声；

MEMS磁力计随机误差模型具体如下：

式中，表示磁力计实际输出值；M表示理想情况下磁力计输出的磁场强度；B_m表示磁力计罗差；w_m表示磁力计受到的白噪声；

所述步骤S3包括：

S3.1、采集陀螺仪的原始数据，通过方向余弦矩阵，求解出欧拉角的初始值，根据陀螺仪的数据来推算理论上的加速度计值，将四元数的四个数量值换算为方向余弦矩阵的第三列元素，代表载体坐标系的值；

S3.2、四元数初始化，其通用初始化方程如下：

S3.3、四元数的更新及规范化，有了初始化的四元数量，再结合姿态数据采样周期及采样数据，对四元数进行更新，从而实时更新数据，实现姿态解算，其更新方程表示为：

式中，ω_X，ω_Y，ω_Z表示陀螺仪在规定周期内完成的三个轴向的角速度采样，在各个采样周期中角速度是变量，将采样值与四元数初始值带入上式中，实现四元数的不断更新；

S3.4、四元数转换为欧拉角；

所述步骤S4姿态测量系统状态方程和量测方程具体为：

其中：

X(k)＝[q₀(k)，q₁(k)，q₂(k)，q₃(k)，ε_bx(k)，ε_by(k)，ε_bz(k)]；

上式是以四元数和陀螺仪常值漂移误差作为滤波器状态变量的7维状态变量；

上式将加速度计和磁力计的三轴测量值作为系统的6维观测量；

其中，状态转移矩阵为：

量测转移矩阵为：

噪声驱动矩阵为：

所述步骤S5包括：

S5.1初始化初始状态

初始化状态变量X(0)、量测值Z(0)；

S5.2时间更新，利用状态方程进行状态一步预测：

S5.3量测更新

估计状态变量：

计算滤波器增益：

K(k)＝P(k|k-1)H(k)^T[H(k)P(k|k-1)H(k)^T+R(k)]^-1；

估计均方误差：

P(k|k)＝[I-K(k)H(k)]P(k|k-1)[I-K(k)H(k)]^T+K(k)R(k)K(k)^T；

将经过扩展卡尔曼滤波算法所求得的四元数根据四元数计算方式列成下列矩阵：

至此，得到了只用四元数中的四个变量表示的载体姿态角公式：

上式中的γ、θ、ψ分别为系统输出的俯仰轴姿态角、横滚轴姿态角以及航向轴姿态角。

2.根据权利要求1所述的基于四元数-扩展卡尔曼滤波的低成本AHRS航向角补偿方法，其特征在于，所述步骤S1中的采集MEMS惯性传感器信息包括：MEMS陀螺仪数据信息、MEMS加速度计数据信息、MEMS磁力计数据信息。