CN110048693A - 基于四元数分布的并行高斯粒子滤波数据处理方法 - Google Patents

Abstract

本发明提出的一种基于四元数分布的并行高斯粒子滤波数据处理方法，属于数字滤波和多传感器数据融合技术领域，主要用于解决粒子滤波器的高维状态产生巨大的计算工作量的问题，该方法以四元数粒子滤波为框架，融合陀螺仪、加速度计及磁力计数据，使用新四元数分布作为单位超球面上的四元数的标准分布，用以估计状态的后验分布。本发明只需在新四元数分布中采样，仅通过线性变换计算四元数分布的二阶矩，加快了计算速度，适用于姿态估计、数据融合等应用场合。

Description

基于四元数分布的并行高斯粒子滤波数据处理方法

技术领域

本发明为了解决非线性粒子滤波算法在载体姿态估计中的计算量大的缺陷，提出了一种改进算法，属于数据处理和多传感器数据融合技术领域，适用于姿态估计、数据融合等应用场合。

背景技术

序贯姿态估计是导航系统的关键部分，目前姿态估计的成熟方案是用陀螺仪，磁力计和加速度计测量的数据来确定飞机的姿态。由于四元数具有非奇异的特性，因此广泛应用于动力学方程。近几十年来，人们已经进行了大量研究，使用各种新算法来提高估计精度。标准扩展卡尔曼滤波(Extended Kalman Filter，EKF)算法用于估计飞行姿态，然而EKF内的归一化处理没有达到最佳估计结果。为了应对强非线性系统，提出了一种无迹卡尔曼滤波(Unscented Kalman Filter，UKF)算法。同样，UKF也需要归一化来保持四元数的标准化约束。此外，UKF受随机过程的统计分布影响比较大。

修正的罗德里格斯参数(Modified Rodrigues Parameters，MRP)的粒子滤波器(Particle Filter，PF)解决了四元数的归一化问题和非线性非高斯问题，然而高维状态下该粒子滤波器会产生巨大的计算工作量。因此，四元数粒子滤波(Quaternion ParticleFilter，QPF)应运而生，该方法直接使用姿态四元数来工作。虽然QPF比其他算法表现更好，但随着粒子数量的增加，它也有较大的计算负荷。计算负荷的主要部分是由重采样过程引起的。因此，随着多核多线程技术的发展，四元数粒子滤波器的并行实现是必要的。

发明内容

在本发明中，我们介绍了一种新的四元数粒子滤波器，称为并行四元数滤波器(Parallel Quaternion Particle Filter，PQPF)。所提出的滤波器使用新的四元数分布作为单位超球面上的四元数的标准分布，用以估计状态的后验分布。PQPF只需在新的四元数分布中采样，且仅通过线性变换计算四元数分布的二阶矩。因此，所提出的滤波器通过并行计算和线性变换加快了计算速度。

本发明具体过程如下：

步骤1：根据加速度计和磁力计的初始测量值对四元数粒子进行初始化；

步骤2：以四元数为系统状态量，采用陀螺仪数据建立四元数系统状态方程；以加速度计及磁力计的输出为观测量建立量测方程；

步骤3：根据步骤2中建立的状态方程得到四元数的二阶矩，采用高斯分布函数抽样得到四元数粒子并进行状态更新；

步骤4：利用步骤2中的量测方程对四元数粒子进行量测更新，采用并行计算的方式计算新的权值；

步骤5：计算最优四元数估计值，根据四元数值计算姿态角。

进一步地，步骤1中定义四元数为q_k≡[q_0k ρ_k]^T，其中 是旋转轴，是旋转角度；

初始四元数粒子集可以由以下两式根据加速度计和磁力计的测量数据进行初始化：

其中，q₀、q₁、q₂、q₃表示四元数的四个参数；y_a0x、y_a0y分别表示初始时刻加速度计在x、y轴方向的输出；y_m0x、y_m0y、y_m0z分别表示初始时刻磁力计在x、y、z轴方向的输出；θ，γ和ψ分别是俯仰角，横滚角和偏航角。

进一步地，步骤2中记q_k为k时刻的四元数，q_k+1为k+1时刻四元数，则离散形式的姿态动力学微分方程可以写成：

q_k+1＝Φ(ω_k)q_k

其中，ω_k是时间k的角速度矢量且在采样间隔Δt内恒定，则正交矩阵Φ(ω_k)如下；

其中，-[ω_k×]为ω_k的反对称矩阵；

观测向量y_k由加速度计和磁力计的测量值组成，相应的矢量r_k称为参考矢量，由局部重力加速度和局部地球磁场值组成；因此，观测模型可以定义如下：

其中是四元数姿态矩阵，η_nk是测量高斯白噪声矢量。

进一步地，步骤3中包括以下步骤：

Step1：根据公式更新四元数的概率密度函数；

其中q是四元数，π是圆周率，q^T是q的转置，P_qq是四元数的二阶矩，是P_qq的逆，det(P_qq)是矩阵P_qq的行列式；

Step2：由于：

因此，根据离散四元数，改写状态方程为：

其中q_k+1表示k+1时刻离散四元数，和分别表示来自陀螺仪的测量角速度矢量和相应的噪声矢量，表示中间变量且服从新的四元数分布其中，P_qqk表示k时刻四元数的二阶矩；

然后根据改写过后的状态方程更新四元数的二阶矩：

其中，表示k+1时刻四元数二阶矩的估计量；

Step3：从新四元数分布中采样得到四元数粒子，首先将P_qq进行Cholesky分解，得到下三角矩阵L，并计算L^TL的最大特征值，记为(λ_LTL)_max；接着从四维高斯分布中采样得到矢量然后将矢量归一化最后比较γ^TL^TLγ/(λ_LTL)_max和z的值，如果γ^TL^TLγ/(λ_LTL)_max＞z，则使用获取随机四元数，否则从四维高斯分布中继续采样；其中L是P_qqCholesky分解后得到的下三角矩阵，L^T是L的转置矩阵，(λ_LTL)_max是L^TL的最大特征值，I_4×4是四阶单位阵，z是在区间(0，1)内服从均匀分布的随机值，是尚未进行归一化采样得到的矢量，γ是均匀分布在单位超球面上的随机向量，γ^T是γ的转置矩阵；

Step4：得到抽样粒子后，根据公式对四元数粒子进行状态更新；其中Z_k表示在时间k上由观测矢量y_k和参考矢量r_k构成的测量，因此Z_1：k表示从开始到时间k的测量。

进一步地，步骤4中将y_k其在时间k的滤波分布写为：

记代表相关独立单位四元数粒子的权重，其中样本来自重要性抽样函数；各个权重通过下式得出：

其中P_qqk是四元数的二阶矩，可由下式表示：

进一步地，步骤5中计算最优四元数：

其中最优四元数是对应于K的最大特征值的特征向量，根据最优四元数的值解算出滤波后姿态角：

θ＝arcsin(2(q₃q₄+q₁q₂))

本发明具有如下优点：并行四元数粒子滤波器提供了有效的姿态确定估计方法，这一新算法属于高斯滤波方法，用粒子更新滤波分布；随机四元数生成中不需要额外的归一化处理，PQPF能够实现并行采样，减少了计算时间；因此，本发明加快了计算速度，为姿态估计、数据融合等提供了新方法。

附图说明

图1并行四元数粒子滤波算法的框架

图2初始姿态误差较小的姿态误差

图3具有较大初始姿态和偏差误差的姿态误差

图4 PQPF和QPF的性能比较：计算时间与粒子数M＝500，1000，1500，2000，5000

具体实施方式

下面详细描述本发明的实施例，本实施例是示例性的，仅用于解释本发明，而不能理解为对本发明的限制。参照说明书附图对本发明的一种基于四元数分布的并行高斯粒子滤波数据处理方法作以下详细地说明：

为更好体现本发明的具体步骤实施及效果，搭建如下仿真实验：通过仿真工作检查PQPF算法的性能。仿真中，飞机配备三轴磁力计(TAM)，陀螺仪和加速度计。

陀螺仪测量模型：其中和ω(t)表示来自陀螺仪的连续时间测量角速度矢量和真实角速度矢量，η_v(t)和η_w(t)是独立的零均值高斯白噪声过程，将该模型离散化后有

加速度计测量模型：其中A_k是一个在时刻k的真实的姿态矩阵，是参考坐标系重的比力，η_ak是加速度计测量噪声；

磁力计测量模型：其中η_mk是磁力计测量噪音，符号由定义，其中α是倾角。

仿真参数设置如下：TAM传感器的标准偏差为50nT；陀螺仪的标准差为σ_u＝3.1623×10^-4μrad/s^3/2，初始偏差协方差设置为(0.2°/hr)²，每个轴初始偏置设置为0.1deg/h；加速度计的标准偏差为2.8935×10^-4g。所有传感器的采样间隔设置为10秒，初始姿态误差分别设定为-10°，10°和60°，初始姿态协方差设定为(50°)²，本实施中算法的粒子数设置为1500。

针对以上仿真实验数据，根据图1滤波算法结构框图，具体步骤实施如下：

步骤1：根据加速度计和磁力计的初始测量值对四元数粒子进行初始化

定义四元数为q_k≡[q_0k ρ_k]^T，其中 是旋转轴，是旋转角度；

初始四元数粒子集可以由以下两式根据加速度计和磁力计的测量数据进行初始化：

其中，q₀、q₁、q₂、q₃表示四元数的四个参数；y_a0x、y_a0y分别表示初始时刻加速度计在x、y轴方向的输出；y_m0x、y_m0y、y_m0z分别表示初始时刻磁力计在x、y、z轴方向的输出；θ，γ和ψ分别是俯仰角，横滚角和偏航角，本实施例中这三个角度分别为-10°，10°和60°。

步骤2：以四元数为系统状态量，采用陀螺仪数据建立四元数系统状态方程、以加速度计及磁力计的输出为观测量建立量测方程；

记q_k为k时刻的四元数，q_k+1为k+1时刻四元数，则离散形式的姿态动力学微分方程可以写成：

q_k+1＝Φ(ω_k)q_k

其中，ω_k是时间k的角速度矢量且在采样间隔Δt内恒定，本实施例中Δt＝10s，则正交矩阵Φ(ω_k)如下；

其中，-[ω_k×]为ω_k的反对称矩阵；

观测向量y_k由加速度计和磁力计的测量值组成，相应的矢量r_k称为参考矢量，由局部重力加速度和局部地球磁场值组成。因此，观测模型可以定义如下：

其中是四元数姿态矩阵，η_nk是测量高斯白噪声矢量。

步骤3：根据步骤2中建立的状态方程得到四元数的二阶矩，采用高斯分布函数抽样得到四元数粒子并进行状态更新

由于：

因此可根据离散四元数，改写状态方程为：

其中，q_k+1表示k+1时刻离散四元数，和分别表示来自陀螺仪的测量角速度矢量和相应的噪声矢量，表示中间变量且服从新的四元数分布其中，P_qqk表示k时刻四元数的二阶矩；

并将下式代入改写过后的状态方程：

其中，q是四元数，γ是均匀分布在单位超球面上的随机向量，L是从Choleshy分解得到的下三角矩阵P_qq，γ^T是γ的转置，L^T是L的转置；并且是一个恒定的正交矩阵，有：

然后根据改写过后的状态方程更新四元数的二阶矩：

其中，表示k+1时刻四元数二阶矩的估计量；

Step3：从新四元数分布中采样得到四元数粒子，首先将P_qq进行Cholesky分解，得到下三角矩阵L，并计算L^TL的最大特征值，记为(λ_LTL)_max；接着从四维高斯分布中采样得到矢量然后将矢量归一化最后比较γ^TL^TLγ/(λ_LTL)_max和z的值，如果γ^TL^TLγ/(λ_LTL)_max＞z，则使用获取随机四元数，否则从四维高斯分布中继续采样；其中(λ_LTL)_max是L^TL的最大特征值，I_4×4是四阶单位阵，z是在区间(0，1)内服从均匀分布的随机值，是尚未进行归一化采样得到的矢量；

Step4：得到抽样粒子后，对四元数粒子进行状态更新：

其中，Z_k表示在时间k上由观测矢量y_k和参考矢量r_k构成的测量，因此Z_1：k表示从开始到时间k的测量。

步骤4：利用步骤2中的量测方程对四元数粒子进行量测更新，采用并行计算的方式计算新的权值

将y_k其在时间k的滤波分布写为：

记代表相关独立单位四元数粒子的权重，其中样本来自重要性抽样函数；各个权重通过下式得出：

其中P_qqk是四元数的二阶矩，可由下式表示：

步骤5：计算最优四元数估计值，根据四元数值计算姿态角

计算最优四元数：

其中最优四元数是对应于K的最大特征值的特征向量，根据最优四元数的值解算出滤波后姿态角：

θ＝arcsin(2(q₃q₄+q₁q₂))

另外，本实施例中，计算姿态估计误差为：

δα＝2arccos(δq₁)

其中，δq₁是由下式定义的错误标量元件：

其中，q和分别是实数四元数和估计四元数的倒数，表示四元数乘积算子。

综上所述，对该方法进行效果分析，如图2所示，三种方法都是收敛的，EKF收敛慢，QPF和PQPF具有相似性能，收敛较快且效果良好；如图3所示，当每个轴的初始姿态误差被设定为-50、50和160时，PQPF和QPF的收敛效果依旧相似且良好，而EKF发散；图2和图3充分说明PQPF可以达到和QPF相似的优越性能，而EKF不一定能达到最佳估计结果。如图4所示当粒子数相同时，PQPF所需时间比QPF少，并且粒子数越多，PQPF计算所需时间少的越多，表示在粒子数量较多的情况下PQPF优越性较明显。本发明中的基于四元数分布的并行高斯粒子滤波数据处理方法具有良好的姿态估计性能，具有一定的实用价值。

Claims (2)

1.一种基于四元数分布的并行高斯粒子滤波数据处理方法，其特征在于，具体包括以下步骤：

步骤1：根据加速度计和磁力计的初始测量值对四元数粒子进行初始化；

步骤2：以四元数为系统状态量，采用陀螺仪数据建立四元数系统状态方程；以加速度计及磁力计的输出为观测量建立量测方程；

步骤3：根据步骤2中建立的状态方程得到四元数的二阶矩，采用高斯分布函数抽样得到四元数粒子并进行状态更新；

步骤4：利用步骤2中的量测方程对四元数粒子进行量测更新，采用并行计算的方式计算新的权值；

步骤5：计算最优四元数估计值，根据四元数值计算姿态角。

2.根据权利要求1所述基于四元数分布的并行高斯粒子滤波数据处理方法中的步骤3中根据步骤2建立的状态方程得到四元数的二阶矩，采用高斯分布函数抽样得到四元数粒子并进行状态更新；其特征在于过程如下：

Step1：根据公式更新四元数的概率密度函数；

其中q是四元数，π是圆周率，q^T是q的转置，P_qq是四元数的二阶矩，是P_qq的逆，det(P_qq)是矩阵P_qq的行列式；

Step2：根据离散四元数，改写状态方程为：

其中q_k+1表示k+1时刻离散四元数，和分别表示来自陀螺仪的测量角速度矢量和相应的噪声矢量，表示中间变量且服从新的四元数分布其中，P_qqk表示k时刻四元数的二阶矩；

然后根据改写过后的状态方程更新四元数的二阶矩：

其中，表示k+1时刻四元数二阶矩的估计量；

Step3：从新四元数分布中采样得到四元数粒子，首先将P_qq进行Cholesky分解，得到下三角矩阵L，并计算L^TL的最大特征值，记为(λ_LTL)_max；接着从四维高斯分布由采样得到矢量然后将矢量归一化最后比较γ^TL^TLγ/(λ_LTL)_max和z的值，如果γ^TL^TLγ/(λ_LTL)_max＞z，则使用获取随机四元数，否则从四维高斯分布中继续采样；其中L是P_qq Cholesky分解后得到的下三角矩阵，L^T是L的转置矩阵，(λ_LTL)_max是L^TL的最大特征值，I_4×4是四阶单位阵，z是在区间(0，1)内服从均匀分布的随机值，是尚未进行归一化采样得到的矢量，γ是均匀分布在单位超球面上的随机向量，γ^T是γ的转置矩阵；

Step4：得到抽样粒子后，根据公式对四元数粒子进行状态更新；其中Z_k表示在时间k上由观测矢量y_k和参考矢量r_k构成的测量，因此Z_1：k表示从开始到时间k的测量。