[发明专利]一种基于FBPF-EIKF-FastSLAM的分布式多源融合定位方法

权利要求书

1.本发明特征在于：(1)确定均匀PDF的加权和；(2)预测k+1时刻的概率密度；(3)确定状态后验PDF；(4)更新箱粒子的荧光亮度；(5)更新全局最优箱粒子；(6)更新箱粒子位置；(7)确定新息和区间Kalman增益；具体包括以下七个步骤：

步骤一：确定均匀PDF的加权和p(x)：

式中，x表示随机变量，i表示计数单位，N表示混合均匀PDF的数目，w_i表示第i个箱粒子对应的概率密度，[x]表示箱粒子，U_[x]表示以箱粒子为支持集的PDF，[xⁱ]表示第i个箱粒子，

步骤二：预测k+1时刻的概率密度p(x_k+1|z_1,k)：

在k时刻，假设x_k状态下的PDF表示为：

根据时间更新步骤，对k+1时刻的概率密度：

式中，k表示为时刻，x_k为系统状态向量，z_k为观测向量，为第i个箱粒子在k时刻的概率密度，为第i个箱粒子在k时刻的状态向量，为第i个箱粒子在k时刻为支持集的PDF，表示支撑集箱粒子为的均匀PDF，[f]为包含函数，[V_k]为k+1时刻的状态转移噪声对应的箱粒子，[U_k]为控制输入箱粒子；

步骤三：确定状态后验PDFp(x_k+1|z_1,k+1)：

式中，η_k+1为归一化系数，[Z_k+1]表示k+1时刻实际观测箱粒子，表示为一个CSP，表示为预测箱粒子通过观测函数与实际观测箱粒子的关系进行约束，用来消除原箱粒子中多余的部分；为第i个箱粒子在k+1时刻的状态值，为以进行约束，为以约束的箱粒子x_k+1为支撑集的PDF，为进行约束后的新箱粒子，|[X]|为箱粒子的体积；

选择合适的阈值，执行随机子划分重采样，根据重采样的次数，将当前时刻得到的箱粒子随机地选取其某一维状态子区间进行均匀划分，使得箱粒子保持一个合适的尺寸；

步骤四：更新箱粒子的荧光亮度

式中，为k+1时刻第i个箱粒子的荧光亮度，为k+1时刻第i个箱粒子的预测观测，[Z_k+1]为实际观测；利用实际观测值和每个箱粒子的预测观测值进行对比代替每个箱粒子之间荧光亮度值的对比；

步骤五：更新全局最优箱粒子[g_k+1]：

确定箱粒子间的吸引度β：

式中，N(0,1)为均值为0，方差为1的高斯分布随机向量，为随机权值项，β_m为最大吸引度，γ为光强吸收系数，d_i为k+1时刻箱粒子与全局最优箱粒子之间的空间距离；当完成位置更新之后，计算并对比箱粒子的荧光亮度值，更新全局最优箱粒子[g_k+1]：

式中，I([X])为箱粒子荧光亮度；

步骤六：更新箱粒子位置

式中，rand为某个服从均匀分布的随机数；通过对箱粒子的位置进行更新，利用更新过程中吸引度发挥的引导作用来指导箱粒子朝着全局最优箱粒子所在的位置进行移动；设定最大迭代次数，当荧光亮度函数值大于设置的迭代终止阈值时，算法迭代停止，否则继续进行迭代，直至达到最大的迭代次数；

步骤七：确定新息和区间Kalman增益：

式中，为新息，I为单位矩阵，为实际观测，在被观测之前是一个不确定的区间矢量，在被观测之后是一个普通的矢量，为观测包含函数，表示区间Kalman增益，为区间聚合操作，为区间的雅克比矩阵，表示观测噪声区间协方差矩阵；对环境地图构建进行更新：

式中，为约束后的区间位置向量；完成环境特征估计，同时对传感器收集到的数据进行观测和数据关联，利用EIKF进行更新，进行地图信息的完善。