[發明專利]一種基于FBPF-EIKF-FastSLAM的分布式多源融合定位方法

在審

• 申請號: 202110327231.3
• 申請日: 2021-03-26
• 公開（公告）號: CN113077055A
• 公開（公告）日: 2021-07-06
• 發明（設計）人: 胡燕祝;王松;賀琬婧
• 申請（專利權）人: 北京郵電大學
• 主分類號: G06N20/00
• 分類號: G06N20/00;G01C21/20
• 代理公司: 暫無信息
• 代理人: 暫無信息

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進行更新，進行地圖信息的完善。