[發(fā)明專利]一種基于聚類和膜計算的改進FastSLAM方法

有效

• 申請?zhí)枺?/td>: 201910973115.1
• 申請日: 2019-10-14
• 公開（公告）號: CN110598804B
• 公開（公告）日: 2023-05-09
• 發(fā)明（設計）人: 韓濤;黃友銳;徐善永;陳亮;凌六一;唐超禮;許家昌
• 申請（專利權(quán)）人: 安徽理工大學
• 主分類號: G06F18/23213
• 分類號: G06F18/23213;G01C21/20
• 代理公司: 暫無信息
• 代理人: 暫無信息

1.一種基于聚類和膜計算的改進FastSLAM方法，其特征在于，所述方法包括下述步驟：

(1)初始化，獲取機器人當前時刻t的位姿x_t，環(huán)境地圖特征map_t，從t時刻到t+1時刻，機器人運動量u_t，觀測量z_t，觀測量噪聲協(xié)方差矩陣H_t；

(2)采樣，在x_t周圍隨機設置L個粒子，每個粒子代表一個機器人可能的位姿，再根據(jù)提議概率分布其中(k)表示第k個位姿，對機器人在t+1時刻可能的位姿進行采樣，越接近提議分布的粒子權(quán)重越大，得到Q個粒子，組成粒子集φ_t，設每個粒子權(quán)重為w^(k)，k∈[1,2,…,Q]：

(3)聚類，根據(jù)每個粒子的權(quán)重，將粒子集φ_t中所有粒子聚類為M個族群；

(3a)聚類初始化，粒子集φ_t中共有Q個粒子，隨機選取M個粒子，作為M個族群的初始族群中心，記為(m)表示第m個族群，n表示迭代次數(shù)，設迭代次數(shù)初始值為n＝0，最大迭代次數(shù)設為N_k-means；

(3b)聚類，計算φ_t中其他粒子分別對的權(quán)重距離dis^(km)＝||w^(k)-w^(m)||²，其中dis^(km)表示粒子k對粒子m的權(quán)重距離，w^(m)為第m個族群中心粒子的權(quán)值，w^(k)為φ_t中除了族群中心粒子外的第k個粒子權(quán)值，每個粒子選擇距離族群中心權(quán)重距離最小的族群，成為其族群成員；

(3c)聚類終止，若M個族群中的粒子成員沒有變化，則聚類結(jié)束，轉(zhuǎn)到步驟(4)，否則，若n＝＝N_k-means，則聚類結(jié)束，轉(zhuǎn)到步驟(4)，否則，迭代次數(shù)n＝n+1，分別計算這M個族群中所有粒子的均值作為新的族群中心返回步驟(3b)；

(4)膜計算優(yōu)化，對聚類后的粒子集φ_t采用膜計算優(yōu)化算法使粒子集里的粒子分布更接近于實際機器人位姿的概率分布；

(4a)優(yōu)化初始化，構(gòu)建細胞型膜系統(tǒng)

Π＝(V,T,μ,Q,W₀,W₂,…,W_M,R₀,R₁,…,R_M,i_o)

其中，V表示細胞內(nèi)所有粒子對象，即粒子集φ_t中所有的粒子，T表示輸出的對象合集，即膜計算優(yōu)化獲得的所有可能的最優(yōu)粒子，μ表示細胞膜型的膜結(jié)構(gòu)為[₀[₁]₁,[₂]₂,···,[_M]_M]₀，W₀～W_M表示每個細胞膜，其中包括一個表層膜W₀，M個基本膜W_m，(m∈[1,2,…,M])，R_m(m∈[0,1,…,M])為每個膜的進化規(guī)則，i_o表示模型運行完成表層膜W₀最終的輸出結(jié)果,設粒子集φ_t聚類后的M個族群分別放到M個基本膜中，第m個基本膜中有D_m,m∈[1,2,...,M]個粒子，設定學習因子c₁、c₂，取(0,1)之間的隨機數(shù)為r₁和r₂,設置膜計算初始迭代數(shù)n＝0，最大迭代次數(shù)為N，設每個粒子速度為v^(md)，位姿為x^(md)，其中d∈[1,2,…,D_m]，m∈[1,2,…,M]，(md)表示第m個基本膜內(nèi)的第d個粒子，設多樣性閾值δ_div，適應度閾值δ_f；

(4b)所有基本膜W_m，(m∈[1,2,…,M])，同時對各自膜內(nèi)所有粒子搜索，尋找權(quán)值最優(yōu)粒子，記為pbest^(m),(m∈[1,2,…,M])，并將其輸送到表層膜W₀中；

(4c)表層膜對所有的pbest^(m),(m∈[1,2,…,M])，進行權(quán)值最優(yōu)粒子搜索，將搜索到的最優(yōu)粒子記為gbest，并將gbest送回到每個基本膜W_m,(m∈[1,2,…,M])中；

(4d)每個基本膜W_m，(m∈[1,2,…,M])，根據(jù)公式分別計算其成員粒子的多樣性div_m，(m∈[1,2,…,M])，再根據(jù)pbest^(m)，(m∈[1,2,…,M])和gbest的值通過公式：

進行速度v^(md)和位姿x^(md)的更新；

(4e)根據(jù)公式和計算適應度值，其中表示迭代到n+1次時機器人位姿的估計值，表示迭代到n+1次時機器人在位姿上觀測量的估計值，其大小由和t時刻的地圖信息map_t決定，f_fitness表示優(yōu)化后的粒子的應度值，如果f_fitness≥δ_f則優(yōu)化結(jié)束，轉(zhuǎn)到步驟(5)，如果f_fitness＜δ_f，則判斷迭代數(shù)n是否達到最大迭代次數(shù)N，如果n＝＝N，則優(yōu)化結(jié)束，轉(zhuǎn)到步驟(5)，如果nN，則n＝n+1，返回(4b)，進入下一次的優(yōu)化迭代；

(5)計算權(quán)重，計算在t+1時刻每個粒子的權(quán)重為：權(quán)重值越大，表明該粒子接近真實值得概率越高；

(6)計算位姿并更新地圖，選擇權(quán)重最大的粒子作為機器人t+1時刻最接近真實值的位姿x_t+1，根據(jù)p(map_t+1|x_t+1,z_t)計算的值，更新t+1時刻環(huán)境地圖特征map_t+1；

(7)重采樣，根據(jù)計算粒子退化程度，若N_eff≥Q/2，則不要進行粒子集重采樣，將粒子集φ_t中權(quán)值較大的粒子復制到新粒子集中，組成t+1時刻的粒子集φ_t+1；若N_eff＜Q/2，則需要重采樣，采樣過程與步驟(2)相同，獲得t+1時刻的粒子集φ_t+1，在獲得的φ_t+1中進行機器人后續(xù)的位姿的估計和環(huán)境地圖的更新。