CN109974714B - 一种Sage-Husa自适应无迹卡尔曼滤波姿态数据融合方法 - Google Patents

Abstract

本发明提出的一种Sage‑Husa自适应无迹卡尔曼滤波姿态数据融合方法，属于数字滤波和多传感器数据融合技术领域，主要用于提高载体姿态估计的精度。该方法以误差四元数无迹卡尔曼滤波作为框架，融合陀螺仪、加速度计及磁力计数据，通过对传感器误差分析建立较为准确的传感器测量模型，并结合误差四元数的方法以及UKF滤波算法实现传感器的数据融合。本发明适用于非线性姿态测量系统，具有良好的抗干扰性和姿态解算精度，可以有效解决复杂环境下载体姿态解算问题。

Description

一种Sage-Husa自适应无迹卡尔曼滤波姿态数据融合方法

技术领域

本发明提供的一种Sage-Husa自适应无迹卡尔曼滤波姿态数据融合方法，属于数字滤波和多传感器数据融合技术领域，本发明提供的方法适用于非线性姿态测量系统。

背景技术

载体的姿态测量系统实际上为一个非线性系统，为了实现对姿态的精确解算，需要采用非线性滤波融合陀螺仪、加速度计及磁力计的数据，但是由于传感器本身容易受到外界干扰导致传感器输出数据包含了较多的噪声，过程噪声和量测噪声都是未知且具有时变特性，错误的参数估计可能导致滤波发散，所以需要使用合适的滤波算法。因此，一种Sage-Husa自适应无迹卡尔曼滤波算法应运而生，该方法能够实现数据融合自适应调整量测噪声大小，抑制传感器随机漂移误差，自适应的补偿了非重力加速度对姿态估计检测的精度，相比较于传统算法，该算法具有更高的姿态解算精度。

传统的无迹卡尔曼滤波(Unscented Kalman Filter，UKF)算法通过无迹变换(Unscented Transformation，UT)，用采样点的分布状态来近似估计概率密度的分布，这些点的均值和方差等于原状态分布的均值和方差。然而该方法容易引起方差矩阵非正定，且噪声变化较大时滤波效果很差，同时由于周围环境噪声变化等因素会导致噪声统计出现偏差，最终导致滤波误差较大，甚至失败。此外在载体实际运动过程中，受到外界环境影响，过程噪声和量测噪声都是未知的，因此本发明针对上述问题，设计了一种Sage-Husa自适应无迹卡尔曼滤波姿态数据融合方法，选用误差四元数方法构建系统的状态方程和量测方程，根据Sage-Husa自适应滤波算法对系统量测噪声矩阵进行自适应估计，并利用自适应估计得到的量测噪声矩阵对协方差矩阵进行自适应更新。

发明内容

本发明的目的在于针对上述存在的问题和不足，提出一种Sage-Husa自适应无迹卡尔曼滤波姿态数据融合方法，该算法能够对传感器误差进行分析并建立较为准确的传感器测量模型，对量测噪声矩阵进行自适应估计，获得精确的姿态角，保证载体运动过程中姿态的精确测量和有效控制。

本发明包括以下步骤：

步骤1：根据初始姿态角确定初始四元数值，选取四元数作为系统状态量并进行初始化，确定滤波初始化参数；

步骤2：根据陀螺仪输出和陀螺仪漂移模型建立误差四元数无迹卡尔曼滤波状态方程；

步骤3：以加速度计和磁力计输出作为观测量，建立误差四元数无迹卡尔曼滤波量测方程；

步骤4：根据Sage-Husa自适应滤波算法对系统量测噪声矩阵进行自适应估计，利用自适应估计得到的量测噪声矩阵及步骤2、步骤3得到的状态方程、量测方程进行系统状态更新和量测更新，得到量测更新后的误差四元数值；

步骤5：根据步骤4中得到的误差四元数值求得四元数滤波值并对其进行归一化处理，利用四元数滤波值姿态解算方法求解得到姿态角。

进一步地，步骤1中定义真实四元数q＝[q₀ q₁ q₂ q₃]^T，定义误差四元数

其中q_e＝[q_e1 q_e2 q_e3]^T，定义四元数估计值为/>

误差四元数、四元数估计值和真实四元数之间的关系为：/>

真实四元数q与三轴陀螺仪角速度的关系为：

表示四元数的乘法，ω表示三轴理想角速度，定义Φ_ω内容如下：

利用毕卡逼近法求解四元数微分方程的结果如下：

其中t表示当前时刻，T表示采样时间间隔；

初始四元数值q₀可以由初始姿态角确定：

其中，θ表示俯仰角，γ表示横滚角，ψ表示航向角；

导航系与载体系之间的变换关系可用如下姿态变换矩阵

表示：

进一步地，步骤2中陀螺仪输出为角速度、零漂误差、白噪声的总和：

ω_o＝ω+ε_g+n_g

其中，ω_o表示陀螺仪的输出，ω表示实际角速度，ε_g表示陀螺仪的零漂误差，n_g表示零均值白噪声；

陀螺仪漂移模型如下：

其中，n_εg为陀螺仪漂移零均值白噪声；

由此选取系统状态量如下：

x＝[q_e1 q_e2 q_e3 ε_gx ε_gy ε_gz]^T

其中ε_gx、ε_gy和ε_gz分别表示陀螺仪零漂误差ε_g在三轴的分量；

建立误差四元数无迹卡尔曼滤波状态方程如下：

x_k＝f(x_k-1)+w_k-1

其中，k为仿真时刻，x_k为k时刻的状态向量，f(x_k-1)为系统的状态转换方程，w_k-1为k-1时刻的系统过程噪声矩阵，具体公式如下：

其中[ω₀×]表示反对称矩阵，I表示单位矩阵。

进一步地，步骤3中加速度计输出为重力分量、零均值白噪声的总和：

其中，f_a表示加速度计的输出，

为导航系到载体系的姿态转换矩阵，q为真实四元数，gⁿ表示当地重力分量，n_a表示加速度计零均值白噪声；

磁力计输出为地磁场和零均值白噪声的总和：

其中，f_m表示磁强计输出，Mⁿ为当地地磁场强度分量，n_m表示磁强计零均值白噪声；

由此选取系统观测量如下：

z＝[f_a f_m]^T

建立误差四元数无迹卡尔曼滤波量测方程如下：

z_k＝h(x_k)+v_k

其中，z_k为k时刻的量测向量，h(x_k)为系统的量测转换方程，v_k为k时刻的系统量测噪声矩阵，具体公式如下：

v_k＝[n_a n_m]^T

进一步地，步骤4中利用自适应估计得到的量测噪声矩阵和步骤2得到的状态方程及步骤3得到的量测方程进行系统状态更新和量测更新，主要过程为：

(1)初始化：根据输入变量确定k＝0时刻的状态量均值

和协方差P₀；

(2)状态更新：

根据对称采样策略生成Sigma点：

其中χ_k表示sigma点集，

为状态量均值，n为状态向量x的维数，λ为尺度参数，由公式λ＝α²(n+κ)-n计算得到，其中α为待确定参数，κ为尺度参数，P_k为k时刻的协方差矩阵；

计算Sigma点状态一步预测，对每个Sigma点进行非线性变换，即：

χ_k+1，k＝f(χ_k)

对非线性变换后的Sigma点进行加权处理得到下一步预测

和协方差矩阵P_k+1，k：

其中

和/>

表示计算均值和方差时所用的加权值，其计算公式为：

其中β为待确定参数，Q_k表示k时刻的系统噪声矩阵；

(3)根据Sage-Husa自适应滤波算法对系统量测噪声矩阵R_k+1进行自适应估计，并利用自适应估计得到的量测噪声矩阵对量测输出协方差矩阵

进行Sage-Husa自适应量测更新，具体步骤为：

Step1：根据Sage-Husa自适应滤波算法对系统量测噪声矩阵R_k+1进行自适应估计：

r_k+1＝(1-d_k)r_k+d_kε_k+1

其中，r_k和r_k+1分别为k时刻和k+1时刻的量测噪声均值，R_k和R_k+1为k时刻和k+1时刻的量测噪声矩阵，H_k+1为k+1时刻的量测矩阵，P_x，k+1，k为k时刻至k+1时刻的一步预测协方差矩阵，d_k为加权值，由公式d_k＝(1-b)/(1-b^k)进行计算，b是常数，为遗忘因子，b越大，噪声矩阵对上一时刻的依赖越小，ε_k+1为k+1时刻的系统残差，由公式ε_k+1＝z_k+1-h(x_k+1)进行计算；

Step2：在系统量测更新过程中，对量测输出协方差矩阵

进行Sage-Husa自适应更新：

R_k+1＝(1-d_k)R_k+d_k(ε_k+1-r_k+1)(ε_k+1-r_k+1)^T

其中，

为k+1时刻的量测输出协方差矩阵，/>

为计算协方差时的加权值，z_i，k+1，k为由k时刻估计得到的k+1时刻的量测矩阵，/>

为k+1时刻的观测值的估计值，R_k+1表示k+1时刻的量测噪声矩阵，根据公式：

计算得到，其中P_x，k+1，k会随着滤波器收敛逐渐趋向于0，

矩阵对量测噪声矩阵的影响忽略不计，量测噪声矩阵R_k+1计算公式改写成：

R_k+1＝(1-d_k)R_k+d_k(ε_k+1-r_k+1)(ε_k+1-r_k+1)^T

(4)量测更新：

根据非线性观测方程对Sigma点集χ_k进行非线性变换：

z_k+1，k＝h(χ_k+1，k)

使用加权计算观测值预测为：

其中

为计算均值时用到的加权值；

对协方差矩阵

进行更新：

对卡尔曼增益矩阵K_k+1进行更新：

状态更新后滤波值更新：

后验方差矩阵更新：

进一步地，步骤5中首先根据步骤4中得到的误差四元数值计算四元数滤波值，根据步骤2中可知，四元数滤波值可以由公式

计算，其中四元数的估计值/>

可由公式

计算；其次对四元数滤波值q＝[q₀ q₁ q₂ q₃]^T进行归一化处理：

最后，利用四元数姿态解算方法求解姿态角：

本发明具有如下优点：本发明在传统无迹卡尔曼滤波算法的基础上，使用误差四元数方法建模，采用Sage-Husa自适应滤波算法对量测噪声值进行自适应估计，并根据实际情况对Sage-Husa自适应滤波算法进行改进，保证了滤波算法的稳定性，可以有效提高姿态估计精度，在系统受到干扰的情况下，仍能获得精确的姿态角，因此，本发明具有良好的抗干扰性，能够为载体运动器的稳定运动提供重要保障。

附图说明

图1Sage-Husa自适应无迹卡尔曼滤波姿态数据融合算法流程框图

图2姿态解算结果对比

图3姿态解算角度误差对比

具体实施方式

下面详细描述本发明的实施例，本实施例是示例性的，仅用于解释本发明，而不能理解为对本发明的限制。参照说明书附图对本发明的一种Sage-Husa自适应无迹卡尔曼滤波姿态数据融合方法作以下详细地说明。

为更好体现本发明的具体步骤实施及效果，搭建如下仿真实验：使用MATLAB仿真平台检查算法的性能。该实验环境中，可搭建以下测量模型：

陀螺仪姿态测量模型和误差模型为

其中ω_o和ω分别表示陀螺仪连续时间测量的角速度矢量和真实角速度矢量，n_g和n_εg分别表示零均值白噪声和陀螺仪漂移零均值白噪声。

加速度计和磁强计姿态测量模型为

其中/>

为导航系到载体系的姿态转换矩阵，gⁿ和Mⁿ分别表示当地重力加速度分量和当地地磁场强度分量，n_a和n_m分别表示加速度计零均值白噪声和磁强计零均值白噪声。

仿真参数设置如下：仿真次数为600次，采样时间间隔T＝0.01s，俯仰角θ、横滚角γ、航向角ψ均初始化为0°，当地重力分量gⁿ＝[0 0 g]^T，g为重力加速度，当地地磁场强度分量Mⁿ＝[0 10 10]^T，α＝0.01，系统量测噪声矩阵初始化R₀＝0.0001I_3×3，系统噪声矩阵初始化

α＝0.01，遗忘因子b＝0.97，β＝2。

针对以上仿真实验数据，根据图1Sage-Husa自适应无迹卡尔曼滤波姿态数据融合算法流程框图，具体步骤实施如下：

步骤1：根据初始姿态角确定初始四元数值，选取四元数作为系统状态量并进行初始化，确定滤波初始化参数；

定义真实四元数q＝[q₀ q₁ q₂ q₃]^T，定义误差四元数

其中q_e＝[q_e1q_e2 q_e3]^T，定义四元数估计值为/>

误差四元数、四元数估计值和真实四元数之间的关系为：

真实四元数q与三轴陀螺仪角速度的关系为：/>

表示四元数的乘法，ω表示三轴理想角速度，定义Φ_ω内容如下：

利用毕卡逼近法求解四元数微分方程的结果如下：

其中t表示当前时刻，本例中取总仿真次数为600次，T表示采样时间间隔，本例中取T＝0.01s。

初始四元数值q₀可以由初始姿态角确定：

其中，θ表示俯仰角，γ表示横滚角，ψ表示航向角，本例中三个姿态角均初始化为0°；

导航系与载体系之间的变换关系可用如下姿态变换矩阵

表示：

步骤2：根据陀螺仪输出和陀螺仪漂移模型建立误差四元数无迹卡尔曼滤波状态方程；

陀螺仪输出为角速度、零漂误差、白噪声的总和：

ω_o＝ω+ε_g+n_g

其中，ω_o表示陀螺仪的输出，ω表示实际角速度，ε_g表示陀螺仪的零漂误差，n_g表示零均值白噪声；

陀螺仪漂移模型如下：

其中，n_εg为陀螺仪漂移零均值白噪声；

由此选取系统状态量如下：

x＝[q_e1 q_e2 q_e3 ε_gx ε_gy ε_gz]^T

其中ε_gx、ε_gy和ε_gz分别表示陀螺仪零漂误差ε_g在三轴的分量；

建立误差四元数无迹卡尔曼滤波状态方程如下：

x_k＝f(x_k-1)+w_k-1

其中，k为仿真时刻，x_k为k时刻的状态向量，f(x_k-1)为系统的状态转换方程，w_k-1为k-1时刻的系统过程噪声矩阵，具体公式如下：

其中[ω₀×]表示反对称矩阵，I表示单位矩阵。

步骤3：以加速度计和磁力计输出作为观测量，建立误差四元数无迹卡尔曼滤波量测方程；

加速度计输出为重力分量、零均值白噪声的总和：

其中，f_a表示加速度计的输出，

为导航系到载体系的姿态转换矩阵，q为真实四元数，gⁿ表示当地重力分量，n_a表示加速度计零均值白噪声；

磁力计输出为地磁场和零均值白噪声的总和：

其中，f_m表示磁强计输出，Mⁿ为当地地磁场强度分量，n_m表示磁强计零均值白噪声；

由此选取系统观测量如下：

z＝[f_a f_m]^T

建立误差四元数无迹卡尔曼滤波量测方程如下：

z_k＝h(x_k)+v_k

其中，z_k为k时刻的量测向量，h(x_k)为系统的量测转换方程，v_k为k时刻的系统量测噪声矩阵，具体公式如下：

v_k＝[n_a n_m]^T

步骤4：根据Sage-Husa自适应滤波算法对系统量测噪声矩阵进行自适应估计，利用自适应估计得到的量测噪声矩阵及步骤2、步骤3得到的状态方程、量测方程进行系统状态更新和量测更新，得到量测更新后的误差四元数值，主要过程为：

(1)初始化：根据输入变量确定k＝0时刻的状态量均值

和协方差P₀；

(2)状态更新：

根据对称采样策略生成Sigma点：

其中χ_k表示sigma点集，

为状态量均值，n为状态向量x的维数，λ为尺度参数，由公式λ＝α²(n+κ)-n计算得到，其中α为待确定参数，本例中取α＝0.01，κ为尺度参数，本例中取值为κ＝3-n，P_k为k时刻的协方差矩阵；

计算Sigma点状态一步预测，对每个Sigma点进行非线性变换，即：

χ_k+1，k＝f(χ_k)

对非线性变换后的Sigma点进行加权处理得到下一步预测和协方差矩阵P_k+1，k：

其中

和/>

表示计算均值和方差时所用的加权值，其计算公式为：

其中β为待确定参数，本例中取β＝2，Q_k表示k时刻的系统噪声矩阵，本例中取Q_k的初值为/>

(3)根据Sage-Husa自适应滤波算法对系统量测噪声矩阵R_k+1进行自适应估计，并利用自适应估计得到的量测噪声矩阵对量测输出协方差矩阵

进行Sage-Husa自适应量测更新，具体步骤为：

Step1：根据Sage-Husa自适应滤波算法对系统量测噪声矩阵R_k+1进行自适应估计：

r_k+1＝(1-d_k)r_k+d_kε_k+1

其中，r_k和r_k+1分别为k时刻和k+1时刻的量测噪声均值，R_k和R_k+1为k时刻和k+1时刻的量测噪声矩阵，本例中取R_k的初值为R₀＝0.0001I3×3，H_k+1为k+1时刻的量测矩阵，P_x，k+1，k为k时刻至k+1时刻的一步预测协方差矩阵，d_k为加权值，由公式d_k＝(1-b)/(1-b^k)进行计算，b是常数，为遗忘因子，b越大，噪声矩阵对上一时刻的依赖越小，本例中取b＝0.97，ε_k+1为k+1时刻的系统残差，由公式ε_k+1＝z_k+1-h(x_k+1)进行计算；

Step2：在系统量测更新过程中，对协方差矩阵

进行Sage-Husa自适应更新：

R_k+1＝(1-d_k)R_k+d_k(ε_k+1-r_k+1)(ε_k+1-r_k+1)^T

其中，

为k+1时刻的量测输出协方差矩阵，/>

为计算协方差时的加权值，z_i，k+1，k为由k时刻估计得到的k+1时刻的量测矩阵，/>

为k+1时刻的观测值的估计值，R_k+1表示k+1时刻的量测噪声矩阵，根据公式：

计算得到，其中P_x，k+1，k会随着滤波器收敛逐渐趋向于0，

矩阵对量测噪声矩阵的影响忽略不计，量测噪声矩阵R_k+1计算公式改写成：

R_k+1＝(1-d_k)R_k+d_k(ε_k+1-r_k+1)(ε_k+1-r_k+1)^T

(4)量测更新：

根据非线性观测方程对Sigma点集χ_k进行非线性变换：

z_k+1，k＝h(χ_k+1，k)

使用加权计算观测值预测为：

其中

为计算均值时用到的加权值；

对协方差矩阵

进行更新：

对卡尔曼增益矩阵K_k+1进行更新：

状态更新后滤波值更新：

后验方差矩阵更新：

步骤5：根据步骤4中得到的误差四元数值求得四元数滤波值并对其进行归一化处理，利用四元数滤波值姿态解算方法求解得到姿态角；

首先根据步骤4中得到的误差四元数值计算四元数滤波值，根据步骤2中可知，四元数滤波值可以由公式

计算，其中四元数的估计值/>

可由公式/>

计算；其次对四元数滤波值q＝[q₀ q₁ q₂ q₃]^T进行归一化处理：

最后，利用四元数姿态解算方法求解姿态角：

对该方法进行效果分析，如图2所示，在仿真过程中50～100次、200～250次和400～450次这三个阶段外界干扰较大，使用误差四元数无迹卡尔曼滤波算法解算得到的俯仰角、横滚角和航向角波动均比较大，而Sage-Husa自适应无迹卡尔曼滤波算法通过自适应调整系统的量测噪声协方差矩阵，对外界干扰有较好的抑制效果，提高了姿态解算的精度。

如图3所示，误差四元数无迹卡尔曼滤波算法姿态解算结果误差较大，而Sage-Husa自适应无迹卡尔曼滤波算法明显优于误差四元数无迹卡尔曼滤波算法，可以获得精确度较高的运动姿态数据。

Claims (1)

1.一种Sage-Husa自适应无迹卡尔曼滤波姿态数据融合方法，其特征在于，具体包括以下步骤：

步骤1：根据初始姿态角确定初始四元数值，选取四元数作为系统状态量并进行初始化，确定滤波初始化参数；

步骤2：根据陀螺仪输出和陀螺仪漂移模型建立误差四元数无迹卡尔曼滤波状态方程；

步骤3：以加速度计和磁力计输出作为观测量，建立误差四元数无迹卡尔曼滤波量测方程；

步骤4：根据Sage-Husa自适应滤波算法对系统量测噪声矩阵进行自适应估计，得到k+1的时刻的量测噪声矩阵R_k+1，利用自适应估计得到的量测噪声矩阵及步骤2、步骤3得到的状态方程、量测方程进行系统状态更新和量测更新，对k+1时刻的量测输出协方差矩阵

进行Sage-Husa自适应量测更新，得到量测更新后的误差四元数值，量测噪声矩阵估计的具体步骤如下：

Step1：根据Sage-Husa自适应滤波算法对系统量测噪声矩阵R_k+1进行自适应估计：

r_k+1＝(1-d_k)r_k+d_kε_k+1

其中，r_k和r_k+1分别为k时刻和k+1时刻的量测噪声均值，R_k和R_k+1为k时刻和k+1时刻的量测噪声矩阵，H_k+1为k+1时刻的量测矩阵，P_x，k+1，k为k时刻至k+1时刻的一步预测协方差矩阵，d_k为加权值，由公式d_k＝(1-b)/(1-b^k)进行计算，b是常数，为遗忘因子，b越大，噪声矩阵对上一时刻的依赖越小，ε_k+1为k+1时刻的系统残差，由公式ε_k+1＝z_k+1-h(x_k+1)进行计算；

Step2：在系统量测更新过程中，对量测输出协方差矩阵

进行Sage-Husa自适应更新：

R_k+1＝(1-d_k)R_k+d_k(ε_k+1-r_k+1)(ε_k+1-r_k+1)^T

其中，

为k+1时刻的量测输出协方差矩阵，W_i ^c为计算协方差时的加权值，z_i，k+1，k为由k时刻估计得到的k+1时刻的量测矩阵，/>

为k+1时刻的观测值的估计值，R_k+1表示k+1时刻的量测噪声矩阵，根据公式：

计算得到，P_x，k+1，k会随着滤波器收敛逐渐趋向于0，

矩阵对量测噪声矩阵的影响忽略不计，量测噪声矩阵R_k+1计算公式改写成：

R_k+1＝(1-d_k)R_k+d_k(ε_k+1-r_k+1)(ε_k+1-r_k+1)^T；

步骤5：根据步骤4中得到的误差四元数值求得四元数滤波值并对其进行归一化处理，利用四元数滤波值姿态解算方法求解得到姿态角。

Images