CN112070170B - 一种动态残差阈值自适应四元数粒子滤波姿态解算数据融合方法 - Google Patents

Abstract

本发明提出的一种动态残差阈值自适应四元数粒子滤波姿态解算数据融合方法，属于数字滤波和多传感器数据融合技术领域，主要用于提高载体姿态估计的精度。该方法以标准粒子滤波算法作为框架，融合陀螺仪、加速度计及磁力计数据，通过对传感器误差分析建立较为准确的传感器测量模型，并选用四元数作为姿态参数，对残差的合理范围进行预测判断，继而自适应调整***的量测噪声矩阵，实现传感器的数据融合。本发明适用于非线性姿态测量***，具有良好的抗干扰性和姿态解算精度，可以有效解决复杂环境下载体姿态解算问题。

Description

一种动态残差阈值自适应四元数粒子滤波姿态解算数据融合 方法

技术领域

本发明提供的一种动态残差阈值自适应四元数粒子滤波姿态解算数据融合方法，属于数字滤波和多传感器数据融合技术领域，本发明提供的方法适用于非线性姿态测量***。

背景技术

载体的姿态测量***实际上为一个非线性***，为了实现对姿态的精确解算，需要采用非线性滤波融合陀螺仪、加速度计及磁力计的数据，但是由于加速度计和磁力计本身容易受到非重力加速度和磁场干扰导致其输出数据包含了较多的噪声，过程噪声和量测噪声都是未知且具有时变特性，错误的参数估计可能导致滤波发散，所以需要使用合适的滤波算法。因此，一种动态残差阈值自适应四元数粒子滤波姿态解算数据融合方法应运而生，该方法能够有效判别误差较大的传感器测量数据，并自适应调整量测噪声大小，抑制了非重力加速度和磁场对姿态估计的干扰，相比较于传统算法，该算法具有更高的姿态解算精度。

在载体实际运动过程中，加速度计和磁力计容易受到非重力加速度和磁力计的影响，导致其输出数据误差较大，从而影响滤波效果，因此本发明针对上述问题，设计了一种动态残差阈值自适应四元数粒子滤波姿态数据融合方法，选用四元数作为姿态参数，利用四元数运动学微分方程构建***的状态方程，利用加速度计和陀螺仪输出模型建立***的量测方程，对***的动态残差阈值范围进行预测，若残差超出范围，则对***量测噪声矩阵进行自适应估计，并利用自适应估计得到的量测噪声矩阵进行量测更新和状态自适应估计。

发明内容

本发明的目的在于针对上述存在的问题和不足，提出一种动态残差阈值自适应四元数粒子滤波姿态解算数据融合方法(RDT-QPF)，该算法能够对传感器误差进行分析并建立较为准确的传感器测量模型，对残差阈值范围进行预测，如果残差超出该范围则调整***量测噪声矩阵，获得精确的姿态角，保证载体运动过程中姿态的精确测量和有效控制。

本发明包括以下步骤：

步骤1：选择四元数q＝[q₀ q_v]^T作为四元数粒子滤波状态量，即x＝q，其中q₀和q_v分别表示四元数标量和矢量部分，根据陀螺仪输出和陀螺仪误差模型建立四元数粒子滤波状态方程，根据加速度计输出f^b和磁力计输出m^b作为观测量，建立四元数粒子滤波量测方程；

步骤2：滤波器初始化，根据初始姿态角求得初始四元数q₀，采样得到一系列四元数粒子集初始化每个粒子权值为1/N，其中上标i表示粒子索引，下标0表示初始时刻，N为粒子总数；

步骤3：时间更新，根据建立的动态残差阈值自适应四元数粒子滤波状态方程对k时刻的四元数粒子集进行更新得到k+1时刻的预测四元数粒子集/>

步骤4：根据k时刻的残差均值μ_k和方差σ_k+1 ²信息，预测k+1时刻动态残差阈值区间，判断k+1时刻残差值r_k+1是否超出动态残差阈值区间S～[μ_k+1-εσ_k+1，μ_k+1+εσ_k+1]，若残差值超出动态残差阈值区间，则自适应调整***k+1时刻的量测噪声矩阵R_k+1；其中ε表示相关系数，μ_k+1和σ_k+1表示k+1时刻残差的均值和标准差；

步骤5：利用自适应估计得到的量测噪声矩阵及步骤1的量测方程进行***量测更新，计算k+1时刻粒子的重要性权值并归一化；

步骤6：重采样，若有效粒子数则对粒子集进行随机重采样操作，其中/>表示k+1时刻第i个粒子的权值，N_threhold表示重采样粒子阈值；

步骤7：对量测更新后的粒子集进行状态估计得到最优四元数估计值并进行归一化处理，利用四元数滤波值姿态解算方法求解得到俯仰角θ，横滚角γ和航向角ψ；

步骤8：循环步骤3～步骤7，直到滤波结束。

进一步地，步骤1中定义四元数为q＝[q₀ q_v]^T＝[q₀ q₁ q₂ q₃]^T，其中q₀和q_v分别表示四元数标量和矢量部分，q₁～q₃为四元数的三个矢量元，四元数运动学微分方程为：

其中，表示四元数的乘法，q(t)表示t时刻的四元数矩阵，ω(t)表示t时刻的角速度，此处假设角速度矢量为四元数形式，Φ_ω(t)为角速度组成的四元数乘法交换矩阵；

对四元数运动学微分方程求通解并离散化得：

定义Θ(T)内容如下：

其中，θ(T)表示[0，T]时段内的三轴角速度增量，即：

由此选取***状态量x如下：

x＝q＝[q₀ q₁ q₂ q₃]^T

建立动态残差阈值自适应四元数粒子滤波状态方程如下：

其中，ΔT表示采样时间间隔；

加速度计输出数据f^b与四旋翼飞行器的运动条件有关，其输出模型可表示为：

式中，为由导航坐标系(n系)到载体坐标系(b系)的姿态转换矩阵，gⁿ为当地重力分量，e为非重力加速度，n_f为加速度计高斯白噪声，/>具体形式如下：

磁力计输出为m^b，其模型可表示为：

式中，mⁿ为当地磁场强度分量，n_m为磁力计高斯白噪声，

选取***观测量z如下：

其中，分别表示加速度计和磁力计的三轴输出分量；

建立动态残差阈值自适应四元数粒子滤波观测方程如下：

其中，h(·)表示***观测方程的非线性函数，v_k表示***的观测噪声；

协方差矩阵：

其中，R_f和R_m分别为加速度计和磁力计的噪声协方差矩阵，0_3×3为3×3的零矩阵；

进一步地，步骤2中根据初始姿态角求得初始四元数q₀，采样得到一系列四元数粒子集初始化每个粒子权值为1/N，其中上标i表示粒子索引，下标0表示初始时刻，N为粒子总数；

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

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

进一步地，步骤3中根据建立的动态残差阈值自适应四元数粒子滤波状态方程对k时刻的四元数粒子集进行更新得到k+1时刻的预测四元数粒子集/>

进一步地，步骤4中具体包含以下步骤：

Step1：根据k时刻的残差均值μ_k和方差σ_k ²信息，预测k+1时刻动态残差阈值区间：

k+1时刻预测的残差均值μ_k+1和方差σ_k+1 ²计算公式为：

其中，r_k+1为k+1时刻残差值，为k+1时刻第i个粒子的残差值：

其中，z_k+1为k+1时刻***的量测值，为k+1时刻第i个粒子的预测量测值；

Step2：判断k+1时刻残差值r_k+1是否超出动态残差阈值区间S～[μ_k+1-εσ_k+1，μ_k+1+εσ_k+1]，其中ε表示相关系数，μ_k+1和σ_k+1表示k+1时刻残差的均值和标准差；

Step3：若残差值超出动态残差阈值区间，则自适应调整***k+1时刻的量测噪声矩阵R_k+1，引入滤波窗口h内的残差信息对R_k+1进行调整：

其中，h表示滤波窗口的宽度；

进一步地，步骤5中利用自适应估计得到的量测噪声矩阵及步骤1的量测方程进行***量测更新，k+1时刻粒子的重要性权值可以由下式进行计算：

粒子权值归一化：

进一步地，步骤6中定义有效粒子数若N_eff＜N_threhold，则对粒子集进行随机重采样操作，其中/>表示k+1时刻第i个粒子的权值，N_threhold表示重采样粒子阈值；

进一步地，步骤7中对量测更新后的粒子集进行状态估计得到最优四元数估计值并进行归一化处理，利用四元数滤波值姿态解算方法求解得到姿态角。

采用以下公式来求得最优四元数的估计值

该公式表示最优四元数估计值就是矩阵M的最大特征值对应的特征向量；

由最优四元数估计值，可求得俯仰角θ、横滚角γ和航向角计算公式如下：

进一步地，步骤8，循环步骤3～步骤7，直到滤波结束。

本发明具有如下优点：本发明在标准粒子滤波算法的基础上，选择四元数作为姿态参数，通过对残差的阈值进行预测，然后自适应调整***的量测噪声矩阵，保证了滤波算法的稳定性，可以有效提高姿态估计精度，在加速度计和磁力计受到非重力加速度和磁场干扰的情况下，仍能获得精确的姿态角，因此，本发明具有良好的抗干扰性，能够为载体运动器的稳定运动提供重要保障。

附图说明

图1一种动态残差阈值自适应四元数粒子滤波姿态解算数据融合方法流程框图

图2残差与动态阈值判别结果

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

具体实施方式

下面详细描述本发明的实施例，本实施例是示例性的，仅用于解释本发明，而不能理解为对本发明的限制。参照说明书附图对本发明的一种动态残差阈值自适应四元数粒子滤波姿态解算数据融合方法作以下详细地说明。

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

陀螺仪输出模型可表示为ω^b＝ω+ε，式中ω为真实角速度，ε为陀螺仪的输出误差，主要包含三种分量：ε＝ε_b+ε_r+n_g，其中，ε_b为随机常值漂移，ε_r为陀螺仪一阶马尔科夫过程，T_g为相关时间，ω_r为马尔科夫过程白噪声，n_g为陀螺仪高斯白噪声。

加速度计和磁力计的输出模型为：式中/>为由导航坐标系(n系)到载体坐标系(b系)的姿态转换矩阵，gⁿ为当地重力分量，e为非重力加速度，n_f为加速度计高斯白噪声，mⁿ为当地磁场强度分量，n_m为磁力计高斯白噪声。

仿真参数设置如下：粒子数为N＝1000，仿真时长为60s，传感器采样频率设置为100Hz，三轴陀螺仪噪声系数为σ_ω＝0.2778，三轴加速度计噪声系数为σ_f＝0.124，三轴磁力计噪声系数为σ_m＝0.244。

针对以上仿真实验数据，根据图1一种动态残差阈值自适应四元数粒子滤波姿态解算数据融合方法流程框图，具体步骤实施如下：

步骤1：选择四元数q＝[q₀ q_v]^T作为四元数粒子滤波状态量，即x＝q，其中q₀和q_v分别表示四元数标量和矢量部分，根据陀螺仪输出和陀螺仪误差模型建立四元数粒子滤波状态方程，根据加速度计输出f^b和磁力计输出m^b作为观测量，建立四元数粒子滤波量测方程；

定义四元数为q＝[q₀ q_v]^T＝[q₀ q₁ q₂ q₃]^T，其中q₀和q_v分别表示四元数标量和矢量部分，q₁～q₃为四元数的三个矢量元，四元数运动学微分方程为：

其中，表示四元数的乘法，q(t)表示t时刻的四元数矩阵，ω(t)表示t时刻的角速度，此处假设角速度矢量为四元数形式，Φ_ω(t)为角速度组成的四元数乘法交换矩阵；

对四元数运动学微分方程求通解并离散化得：

定义Θ(T)内容如下：

其中，θ(T)表示[0，T]时段内的三轴角速度增量，即：

由此选取***状态量x如下：

x＝q＝[q₀ q₁ q₂ q₃]^T

建立动态残差阈值自适应四元数粒子滤波状态方程如下：

其中，ΔT表示采样时间间隔；

加速度计输出数据f^b与四旋翼飞行器的运动条件有关，其输出模型可表示为：

式中，为由导航坐标系(n系)到载体坐标系(b系)的姿态转换矩阵，gⁿ为当地重力分量，e为非重力加速度，n_f为加速度计高斯白噪声，/>具体形式如下：

磁力计输出为m^b，其模型可表示为：

式中，mⁿ为当地磁场强度分量，n_m为磁力计高斯白噪声，

选取***观测量z如下：

其中，分别表示加速度计和磁力计的三轴输出分量；

建立动态残差阈值自适应四元数粒子滤波观测方程如下：

其中，h(·)表示***观测方程的非线性函数，v_k表示***的观测噪声；

协方差矩阵：

其中，R_f和R_m分别为加速度计和磁力计的噪声协方差矩阵，0_3×3为3×3的零矩阵；

步骤2：滤波器初始化，根据初始姿态角求得初始四元数q₀，采样得到一系列四元数粒子集初始化每个粒子权值为1/N，其中上标i表示粒子索引，下标0表示初始时刻，N为粒子总数；

本例中，初始姿态角设置为Λ₀＝[0° 0° 30°]^T，初始四元数值q₀可以由初始姿态角确定：

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

步骤3：时间更新，根据建立的动态残差阈值自适应四元数粒子滤波状态方程对k时刻的四元数粒子集进行更新得到k+1时刻的预测四元数粒子集/>

步骤4：根据k时刻的残差均值μ_k和方差σ_k ²信息，预测k+1时刻动态残差阈值区间，判断k+1时刻残差值r_k+1是否超出动态残差阈值区间S～[μ_k+1-εσ_k+1，μ_k+1+εσ_k+1]，若残差值超出动态残差阈值区间，则自适应调整***k+1时刻的量测噪声矩阵R_k+1；其中ε表示相关系数，μ_k+1和σ_k+1表示k+1时刻残差的均值和标准差，具体包含以下步骤：

Step1：根据k时刻的残差均值μ_k和方差σ_k+1 ²信息，预测k+1时刻动态残差阈值区间：

k+1时刻预测的残差均值μ_k+1和方差σ_k+1 ²计算公式为：

其中，r_k+1为k+1时刻残差值，为k+1时刻第i个粒子的残差值：

其中，z_k+1为k+1时刻***的量测值，为k+1时刻第i个粒子的预测量测值；

Step2：判断k+1时刻残差值r_k+1是否超出动态残差阈值区间S～[μ_k+1-εσ_k+1，μ_k+1+εσ_k+1]，其中ε表示相关系数，μ_k+1和σ_k+1表示k+1时刻残差的均值和标准差；

Step3：若残差值超出动态残差阈值区间，则自适应调整***k+1时刻的量测噪声矩阵R_k+1，引入滤波窗口h内的残差信息对R_k+1进行调整：

其中，h表示滤波窗口的宽度；

步骤5：利用自适应估计得到的量测噪声矩阵及步骤1的量测方程进行***量测更新，计算k+1时刻粒子的重要性权值并归一化；

k+1时刻粒子的重要性权值可以由下式进行计算：

粒子权值归一化：

步骤6：重采样，若有效粒子数则对粒子集进行随机重采样操作，其中/>表示k+1时刻第i个粒子的权值，N_threhold表示重采样粒子阈值，本例中取N_threhold＝2N/3；

步骤7：对量测更新后的粒子集进行状态估计得到最优四元数估计值并进行归一化处理，利用四元数滤波值姿态解算方法求解得到俯仰角θ，横滚角γ和航向角ψ；

采用以下公式来求得最优四元数的估计值

该公式表示最优四元数估计值就是矩阵M的最大特征值对应的特征向量；

由最优四元数估计值，可求得俯仰角θ、横滚角γ和航向角计算公式如下：

步骤8：循环步骤3～步骤7，直到滤波结束。

对该方法进行效果分析，仿真过程中在25～30s向加速度计Y轴加入非重力加速度干扰，向磁力计三轴加入磁场干扰。如图2所示，y轴的acc-x，acc-y，acc-z，mag-x，mag-y，mag-z分别表示加速度计和磁力计的三轴输出残差值，在对应时间段的残差值均超过预测阈值范围，说明此时传感器测量数据误差较大，如图3所示，使用无迹卡尔曼滤波算法(UKF)和标准粒子滤波算法(QPF)解算得到的俯仰角pitch、横滚角roll和航向角yaw波动均比较大，而一种动态残差阈值自适应四元数粒子滤波姿态数据融合方法(RDT-QPF)通过自适应调整***的量测噪声协方差矩阵，滤波效果明显优于其余两种算法，对外界干扰有较好的抑制效果，提高了姿态解算的精度。

Claims (2)

1.一种动态残差阈值自适应四元数粒子滤波姿态解算数据融合方法，其特征在于，具体包括以下步骤：

步骤1：选择四元数q＝[q₀ q_v]^T作为四元数粒子滤波状态量，即x＝q，其中q₀和q_v分别表示四元数标量和矢量部分，根据陀螺仪输出和陀螺仪误差模型建立四元数粒子滤波状态方程，根据加速度计输出f^b和磁力计输出m^b作为观测量，建立四元数粒子滤波量测方程；

步骤2：滤波器初始化，根据初始姿态角求得初始四元数q₀，采样得到一系列四元数粒子集初始化每个粒子权值为1/N，其中上标i表示粒子索引，下标0表示初始时刻，N为粒子总数；

步骤3：时间更新，根据建立的动态残差阈值自适应四元数粒子滤波状态方程对k时刻的四元数粒子集进行更新得到k+1时刻的预测四元数粒子集/>

步骤4：根据k时刻的残差均值μ_k和方差σ_k ²信息，预测k+1时刻动态残差阈值区间，判断k+1时刻残差值r_k+1是否超出动态残差阈值区间S～[μ_k+1-εσ_k+1，μ_k+1+εσ_k+1]，若残差值超出动态残差阈值区间，则自适应调整***k+1时刻的量测噪声矩阵R_k+1；其中ε表示相关系数，μ_k+1和σ_k+1表示k+1时刻残差的均值和标准差；

步骤5：利用自适应估计得到的量测噪声矩阵及步骤1的量测方程进行***量测更新，计算k+1时刻粒子的重要性权值并归一化；

步骤6：重采样，若有效粒子数则对粒子集进行随机重采样操作，其中/>表示k+1时刻第i个粒子的权值，N_threhold表示重采样粒子阈值；

步骤7：对量测更新后的粒子集进行状态估计得到最优四元数估计值并进行归一化处理，利用四元数滤波值姿态解算方法求解得到俯仰角θ，横滚角γ和航向角ψ；

步骤8：循环步骤3～步骤7，直到滤波结束。

2.根据权利要求1所述的一种动态残差阈值自适应四元数粒子滤波姿态解算数据融合方法，其特征在于，步骤4中根据k时刻的残差均值μ_k和方差σ_k ²信息，预测k+1时刻动态残差阈值区间，判断k+1时刻残差值r_k+1是否超出动态残差阈值区间S～[μ_k+1-εσ_k+1，μ_k+1+εσ_k+1]，若残差值超出动态残差阈值区间，则自适应调整***k+1时刻的量测噪声矩阵R_k+1；具体包含以下步骤：

Step1：根据k时刻的残差均值μ_k和方差σ_k ²信息，预测k+1时刻动态残差阈值区间：

k+1时刻预测的残差均值μ_k+1和方差σ_k+1 ²计算公式为：

其中，为k+1时刻第i个粒子的残差值：

其中，z_k+1为k+1时刻***的量测值，为k+1时刻第i个粒子的预测量测值；

Step2：判断k+1时刻残差值r_k+1是否超出动态残差阈值区间S～[μ_k+1-εσ_k+1，μ_k+1+εσ_k+1]，其中ε表示相关系数，μ_k+1和σ_k+1表示残差的均值和标准差；

Step3：若残差值超出动态残差阈值区间，则自适应调整***k+1时刻的量测噪声矩阵R_k+1，引入滤波窗口h内的残差信息对R_k+1进行调整：

其中，h表示滤波窗口的宽度。