一种基于确定性采样的网格四元数粒子滤波数据处理方法技术

本发明专利技术提出的一种基于确定性采样的网格四元数粒子滤波数据处理方法，属于数字滤波和多传感器数据融合技术领域，主要用于解决粒子滤波器在姿态估计中初始姿态不清楚，造成较大初始协方差和收敛时间过长的问题，该方法以粒子滤波为框架，融合陀螺仪、加速度计，利用确定性采样采样方法来求得初始四元数集。本发明专利技术将采样确定性采样方法，对高密度区采用很少的粒子来近似，对低密度区域的随机样本进行忽略，该方法在估计精度和惯性测量单元姿态估计计算量之间取得了良好的平衡，适用于姿态估计、数据融合等应用场合。数据融合等应用场合。数据融合等应用场合。

【技术实现步骤摘要】
一种基于确定性采样的网格四元数粒子滤波数据处理方法


[0001]本专利技术为了解决粒子滤波算法在飞行器姿态估计中存在庞大的计算负担问题，提出了一种以粒子滤波为框架的确定性采样直接姿态估计算法，属于数据处理和多传感器数据融合
，适用于姿态估计、数据融合等应用场合。

技术介绍

[0002]对于姿态估计，可以分为两个分支：间接估计法和直接估计法。间接估计策略将SO(3)上的姿态估计问题转化为其切空间。四元数姿态估计的间接滤波算法依赖于假设噪声项为高斯分布的非线性的局部线性化，例如乘法扩展卡尔曼滤波器(MEKF)，无迹四元数估计器(USQUE)，以及间接高斯粒子滤波器。遗憾的是，这种近似方法受到从局部参数化和线性化继承而来的基本问题的限制。首先，在运动学方程线性化中只考虑姿态误差的一阶或二阶。因此，系统噪声必须足够小，线性化才能有效。目前姿态估计的成熟方案是基于陀螺仪、加速度计及磁力计的矢量观测来确定飞机的姿态。克服间接估计器缺点的一个直接方法是使用直接在SO(3)上定义的概率分布，而不是高斯分布进行局部参数化来模拟姿态的不确定性。例如，基于马尔可夫链蒙特卡罗方法来近似真实的概率密度函数，Oshman和Carmi提出的遗传算法四元数粒子滤波器(GA
‑
QPF)。粒子滤波器使用加权粒子以非参数方式近似底层概率密度。如果粒子的数量足够大，概率分布随机地反映了潜在的不确定性，比在SO(3)上定义的分布更真实。然而，使用重采样和正则化过程来避免粒子退化和样本不足将随着粒子数量的增加而导致计算负担加重。
>[0003]关于姿态估计的估计精度和收敛速度，特别是在高非线性和强系统噪声下，直接方法优于间接方法，但代价是增加了计算量。一种可行的方法是在现有的四元数滤波器中采用确定性建模而不是随机方式。确定性方法的优点是可以将样本放置在有代表性的位置，这样用很少的样本就可以很好地表示实际密度，可以避免粒子退化和样本不足。此外，与随机方案相比，确定性方案可以保证结果的可重复性。虽然上面提到的USQUE应用了UT(无迹变换)来获得确定性高斯西格玛点，但是它不在流形上移动。然而，基于非参数方式确定性采样流形上的直接四元数滤波器的研究还很少。

技术实现思路

[0004]在本专利技术中，提出了一种基于高密度不确定性匹配的确定性采样策略的直接四元数估计器——离散四元数粒子滤波器(Discrete Quaternion Particle Filter)，用于估计姿态和陀螺偏差。单位四元数直接在滤波器中用作粒子，以保持固有的范数约束。位于高密度区域的离散确定性样本通过遵循指数映射的一致切线空间生成。然后，狄拉克混合物近似于超球形样本的流形上高密度概率密度函数。通过与GA
‑
QPF算法和USQE算法的比较，验证了DQPF在估计精度和惯性测量单元姿态估计的计算成本之间取得了良好的平衡。
[0005]本专利技术的具体过程如下：
[0006]步骤1：利用陀螺仪输出建立系统状态方程，将加速度计及磁力计的输出作为观测
量建立系统观测方程，根据初始加速度y0＝[a
x0 a
y0 a
z0
]T
和初始重力加速度r0＝[0 0
ꢀ‑
g]T
得到中心四元数利用确定性采样方法得到初始化四元数粒子其中，i表示粒子个数索引，N表示每层粒子数，I表示层数，下标0表示0时刻，T表示矩阵转置；
[0007]步骤2：计算下一时刻先验粒子，由k时刻四元数粒子k+1时刻的陀螺仪输出可以得到先验粒子其中，M表示粒子数；
[0008]步骤3：转移粒子到新的四元数，由步骤2得到的先验粒子和k时刻的权重计算k+1中心四元数再由k时刻的粒子k时刻的中心四元数和k+1中心四元数可以得到k+1时刻四元数粒子其中，下标k+1表示k+1时刻；
[0009]步骤4：更新权重，据步骤3得到的再由下一时刻的观测值y
k+1
经过计算得到权重，将权重线性化得到
[0010]步骤5：计算最优四元数；根据步骤3中计算得到k+1时刻四元数粒子和步骤4中得到线性化后的权重可以得到最优四元数并根据最优四元数计算姿态角，其中，姿态角包括俯仰角θ、横滚角γ、航向角ψ；
[0011]步骤6：根据步骤3得到的k+1时刻的四元数粒子和k+1时刻的中心四元数步骤4得到的k+1时刻线性化后的权重循环执行步骤2、3、4、5得到下一时刻滤波估计值。
[0012]进一步地，步骤1中定义四元数为q≡[q
1 ρ
T
]T
，其中，T表示矩阵转置；q1＝cos(θ/2)，是旋转轴，θ是旋转角度。估计值到真值q的旋转四元数定义为误差四元数δq≡[δq
1 δρ
T
]T
。由于误差四元数对应于小的姿态旋转，所以它的标量δq1→
1，姿态信息主要包含在矢量部分δρ，因此定义状态量：
[0013]用表示四元数乘法，则误差四元数δq和估计值及真值q的关系为：
[0014][0015]记q
k
为k时刻的四元数，q
k+1
为k+1时刻四元数，则系统状态方程为：
[0016][0017]k时刻观测量y
k
由加速度计输出和磁力计输出组成，系统的观测方程为：
[0018][0019]其中，为导航坐标系到机体坐标系的姿态矩阵，g
n
为当地重力加速度，m
n
为当地磁场分量；η
a
、η
m
分别为加速度计零均值白噪声、磁力计零均值白噪声。
[0020]利用确定性采样方法得到初始化四元数粒子，如下：
[0021]Step1：利用递归分区等面积算法得到
[0022]Step2：利用指数映射将S2平面的映射到S3平面，公式如下：
[0023][0024]其中，等式右边的表示之前在S2平面，表示的范数；
[0025]Step3：利用以下公式可以得到在q
central
周围N
×
1的确定性采样粒子
[0026][0027][0028][0029]其中，i是从1到I，m是从1到N，R是指高密度范围，
←
表示形成一个四元数粒子集。
[0030]高密度范围R通过如下公式获得：
[0031][0032]其中，表示的标准差，σ通过如下步骤获得：
[0033]step1：以初始四元数为中心进行单位高斯采样得到M个随机样本；
[0034]step2：根据公式求所需概率区间的协方差，所的协方差便是σ，其中，表示维度为D的向量，则是这些向量的平均值，D是维度，在这里是4维。
[0035]进一步地，步骤2得到k+1时刻先验粒子可以通过如下公式：
[0036][0037]其中是在k+1时刻陀螺角速度输出矢量，表示的反对称矩阵，exp(
·
)表示指数本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于确定性采样的网格四元数粒子滤波数据处理方法，其特征在于，具体包括以下步骤：步骤1：根据初始加速度y0＝[a
x0 a
y0 a
z0
]
T
和初始重力加速度r0＝[0 0
ꢀ‑
g]
T
得到中心四元数利用确定性采样方法得到初始化四元数粒子其中，i表示粒子个数索引，N表示每层粒子数，I表示层数，下标0表示0时刻，T表示矩阵转置，利用确定性采样方法得到初始化四元数粒子，具体包括如下子步骤：子步骤1：利用递归分区等面积算法得到球面上均匀分布的点子步骤2：利用指数映射将S2平面的映射到S3平面，公式如下：其中，等式右边的表示在S2平面，表示的范数，cos()和sin()均表示三角函数，表示初始中心四元数子步骤3：利用以下公式可以得到在q
central
周围N
×
I的确定性采样四元数粒子集I的确定性采样四元数粒子集I的确定性采样四元数粒子集I的确定性采样四元数粒子集其中，i是从1到I，m是从1到N；R是选取网格四元数高密度范围的边界值，
←
表示形成一个四元数粒子集；步骤2：计算下一时刻先验粒子，由k时刻四元数粒子k+1时刻的陀螺仪输出可以得到先验粒子其中，M表示粒子数；步骤3：转移粒子到新的四元数，由步骤2得到的先验粒子和...

【专利技术属性】
技术研发人员：周翟和，李永斌，曾传伟，
申请(专利权)人：南京航空航天大学秦淮创新研究院，
类型：发明
国别省市：