[發(fā)明專利]基于螢火蟲優(yōu)化粒子濾波的機(jī)器人定位與建圖方法及裝置

有效

• 申請?zhí)枺?/td>: 201910473312.7
• 申請日: 2019-05-31
• 公開（公告）號: CN110263905B
• 公開（公告）日: 2021-03-02
• 發(fā)明（設(shè)計）人: 崔昊楊;張宇;劉昊;秦倫明;楊強(qiáng)強(qiáng);卞正蘭;唐忠
• 申請（專利權(quán)）人: 上海電力學(xué)院
• 主分類號: G06N3/00
• 分類號: G06N3/00;G06T7/70
• 代理公司: 上海科盛知識產(chǎn)權(quán)代理有限公司 31225
• 代理人: 蔡彭君

1.一種基于螢火蟲優(yōu)化粒子濾波的機(jī)器人定位與建圖方法，其特征在于，包括：

步驟S1：獲得采樣初始時刻粒子，

步驟S2：通過機(jī)器人運(yùn)動模型和觀測模型得到t時刻的目標(biāo)值，并通過重要性采樣得到t時刻的粒子集合，

步驟S3：利用改進(jìn)后的螢火蟲算法優(yōu)化采樣粒子的位置，分別得到所有粒子t+1時刻優(yōu)化后的位置，

步驟S4：計算優(yōu)化后的粒子權(quán)重并進(jìn)行歸一化，篩選有效粒子，并基于有效粒子進(jìn)行重采樣，直至粒子總數(shù)達(dá)到設(shè)定數(shù)目，

步驟S5：根據(jù)步驟S4得到的粒子群進(jìn)行機(jī)器人位姿與環(huán)境估計；

所述步驟S3具體包括：

步驟S31：根據(jù)重定義后的相對亮度公式計算t+1時刻粒子i、j的相對亮度：

其中：I為相對亮度，pdist(·)_euc為歐氏距離，為第i粒子的預(yù)測觀測值，為第i粒子的最新觀測值，為第i粒子的預(yù)測狀態(tài)值，為第i粒子的最新狀態(tài)值，

步驟S32：計算粒子t+1時刻粒子i、j的歐式距離，

步驟S33：根據(jù)改進(jìn)后的位置方程來更新粒子i的位置：

其中：x_i'為更新后的粒子i所在的空間位置，x_i為粒子i所在的空間位置，β₀為螢火蟲最大吸引度，γ為光強(qiáng)吸收系數(shù)，r_ij為粒子i、j的空間距離，x_j為粒子j所在的空間位置，R_t為觀測過程的噪聲值，I(i)為粒子i相對亮度，I(j)為粒子j相對亮度，rand為服從均值為0、方差為1的均勻分布隨機(jī)數(shù)。

2.根據(jù)權(quán)利要求1所述的一種基于螢火蟲優(yōu)化粒子濾波的機(jī)器人定位與建圖方法，其特征在于，所述步驟S4具體包括：

步驟S41：計算粒子權(quán)重并按升序排列，采用基于偏差修正的指數(shù)加權(quán)平均算法計算排序后粒子集的平均值δ，并將權(quán)重小于該平均值δ的粒子舍棄，并將剩余的粒子作為有效粒子；

步驟S42：按照有效粒子的權(quán)重大小，對各有效粒子進(jìn)行復(fù)制；

步驟S43：對各粒子權(quán)值降序排列，并利用高斯變異算子依次對粒子i位姿x_i進(jìn)行隨機(jī)擾動從而產(chǎn)生新粒子x′_i，直至滿足總數(shù)N。

3.根據(jù)權(quán)利要求2所述的一種基于螢火蟲優(yōu)化粒子濾波的機(jī)器人定位與建圖方法，其特征在于，所述步驟S42中，各粒子的復(fù)制次數(shù)為：

其中：n_i為粒子i的復(fù)制次數(shù)，g(·)為向上取整函數(shù)，為粒子i的權(quán)重，δ為粒子權(quán)重的平均值。

4.根據(jù)權(quán)利要求2所述的一種基于螢火蟲優(yōu)化粒子濾波的機(jī)器人定位與建圖方法，其特征在于，所述步驟S43中新添粒子公式如下：

x_i'＝x_i+x_i×μ×M(0,1)

其中：x_i'為加入隨機(jī)擾動后的粒子i的位置，x_i為加入隨機(jī)擾動前的粒子i的位置，μ為擾動控制因子，M(0,1)為均值為0、方差為1的高斯分布隨機(jī)變量。