杜 晨, 杜 煜

(1.北京聯(lián)合大學 北京市信息服務(wù)工程重點實驗室，北京 100101；2.北京聯(lián)合大學 機器人學院，北京 100101)

0 引 言

作為機器人自主定位導(dǎo)航技術(shù)的關(guān)鍵，同時定位與地圖構(gòu)建(simultaneous localization and mapping，SLAM)技術(shù)也正成為關(guān)注焦點[1,2]。移動機器人的SLAM技術(shù)包括對機器人的狀態(tài)和環(huán)境信息的同時估計[3]。

Rao-Blackwellized粒子濾波器(Rao-Blackwellized particle filter,RBPF)是解決SLAM技術(shù)的方法之一。經(jīng)過多年的發(fā)展也出現(xiàn)了很多改進辦法，國內(nèi)的王田橙[4]、國外PopovlG[5]等人都在不同程度上對RBPF-SLAM算法做出了改進。

本文在RBPF-SLAM方法的基礎(chǔ)上，將采樣集中在觀測信息的可能性區(qū)域，減少了與目標分布的誤差，使得采樣的粒子更加符合真實環(huán)境狀態(tài)，建立更準確的地圖環(huán)境；并且引入分層重采樣優(yōu)化策略，通過控制閾值，維持盡可能多的粒子多樣性，有效地解決了粒子退化問題。最后，為了驗證算法的有效性，以Bulldog移動機器人平臺為基礎(chǔ)，開展了相關(guān)實驗。并針對實驗平臺，優(yōu)化了掃描處理步驟，加快了掃描處理時間，有效降低了實驗運行時間。

1 基于RBPF-SLAM算法的優(yōu)化

1.1 RBPF-SLAM原理

RBPF-SLAM問題可理解為:初始地圖m0與初始位姿x0已知的情況下，從最初到時刻t的傳感器觀測信息z1:t=z1,…,zt，與移動機器人里程計的控制信息u1:t-1=u1,…,ut-1，來估計p(x1:t,m|z1:t,u1:t-1)，即機器人軌跡x1:t=x1…,xt與地圖m的聯(lián)合后驗概率分布。通過貝葉斯濾波器可以對其進行分解

p(x1:t,m|z1:t,u1:t-1)=p(m|x1:t,z1:t)p(x1:t|z1:t,u1:t-1)

(1)

RBPF-SLAM算法采用粒子濾波器來估算機器人的位姿，其中，每一個粒子代表一條可能的軌跡。包含4個主要步驟：采樣、權(quán)重計算、粒子重采樣、地圖更新。在權(quán)重計算階段可以通過限制概率密度函數(shù)來進行

(2)

式中η=1/p(zt|z1:t-1,u1:t-1)為貝葉斯定律中歸一化因子，所有粒子擁有相同的η。

1.2 優(yōu)化的RBPF-SLAM

1.2.1 融合觀測信息的提議分布

機器人在實際移動過程中，激光雷達作為機器人的外部傳感器具有較高的精度，似然函數(shù)區(qū)域比較集中；而基于里程計的運動模型，因里程計自身誤差較大，似然函數(shù)區(qū)域比較分散，導(dǎo)致了兩者之間重合的地方很少。

融合觀測信息[6]，將采樣集中在觀測信息的可能性區(qū)域。可以得到關(guān)于粒子權(quán)重方差的最佳提議分布，減少與目標分布的誤差，使得采樣的粒子更加符合真實環(huán)境狀態(tài)，建立更準確的地圖環(huán)境。此時的提議分布

進一步更新式(2)，得到

在似然函數(shù)的峰值進行采樣，可以得到化簡之后的權(quán)重計算

1.2.2 重采樣優(yōu)化策略

引入有效粒子數(shù)Neff來估計當前粒子集代表目標函數(shù)的近似程度

為解決粒子退化問題[7，8]，本文引入了一種分層重采樣優(yōu)化策略。首先設(shè)置采樣粒子高權(quán)重閾值ωk=2/N和低權(quán)重閾值ωL=1/(2N)，其中N代表粒子數(shù)。根據(jù)設(shè)定的閾值，把粒子權(quán)值分為3個區(qū)間，較高權(quán)值區(qū)間、較低權(quán)值區(qū)間和中等權(quán)值區(qū)間。其中，對中等權(quán)值區(qū)間的粒子不做處理，對具有較高和較低權(quán)值區(qū)間的粒子按權(quán)值大小由高到低進行排序，之后再計算每個粒子被選擇的概率

p(i)={a+[rank(i)/(Nh+Nl-1)]·

(b-a)}/(Nh+Nl)

(7)

式中 rank(i)為粒子根據(jù)權(quán)重排序后的序號，a,b為系數(shù)，Nh和Nl分別為高權(quán)重粒子數(shù)和低權(quán)重粒子數(shù)。

1.2.3 優(yōu)化RBPF-SLAM算法流程

1)首先，估計機器人初始位姿；2)使用迭代最近鄰ICP算法進行點云匹配；3)進行采樣，計算目標分布，進而求得提議分布；4)在融合觀測信息的提議分布中采樣求得新粒子集，計算粒子權(quán)重；5)進行優(yōu)化重采樣策略更新粒子權(quán)值；6)地圖更新。

2 移動機器人系統(tǒng)

2.1 移動機器人硬件平臺

本實驗平臺為長90 cm、寬70 cm 、高80 cm的Bulldog機器人，實物圖如圖1(a)所示。

本實驗選取Rslidar激光雷達傳感器以及里程計進行實驗。Rslidar激光雷達集合了16個激光收發(fā)元器件，可以按照設(shè)定的頻率進行掃描。里程計能夠記錄機器人走過的路程以及方向等信息。其內(nèi)部存在一個用串口線連接的PC，外部PC連接Bulldog的路由，通過SSH協(xié)議實現(xiàn)通信。

Bulldog輪式機器人包含感知模塊、決策模塊和底盤控制模塊等。當控制層對Bulldog輪式機器人進行控制時，感知層中的傳感器會接收到自身以及周圍場景的信號。之后，將信號節(jié)點發(fā)布到內(nèi)部PC中，外部PC連接Bulldog的路由，實現(xiàn)與內(nèi)部PC的通信。在訂閱節(jié)點信息的同時，對這些數(shù)據(jù)進行處理，根據(jù)各自算法的需求，來完成相應(yīng)的實驗。

2.2 移動機器人軟件系統(tǒng)

機器人操作系統(tǒng)(robot operating system,ROS)是移動機器人研究的基礎(chǔ)，ROS的提出推動了機器人產(chǎn)業(yè)的發(fā)展。系統(tǒng)框架分為:節(jié)點(node)、節(jié)點管理器(master)、消息(message)、話題(topic)。其中，節(jié)點是核心，可以執(zhí)行簡單的程序語句[9]。

內(nèi)部PC為Linux(Ubuntu14.04)上運行的ROS，主要節(jié)點簡化圖如圖1(b)所示。

圖1 移動機器人及ROS的主要節(jié)點

在ROS框架下，激光雷達傳感器用激光雷達節(jié)點來實現(xiàn)激光雷達數(shù)據(jù)幀的讀取，由/scan話題完成激光數(shù)據(jù)的發(fā)布工作。里程計節(jié)點得到里程計信息，由/odom完成里程計信息的發(fā)布工作。所有話題都會發(fā)布到公共主題當中，等待其它節(jié)點訂閱。ROS中還提供了一個可視化工具，可以實現(xiàn)實時在線地圖構(gòu)建的顯示。

由于實驗平臺實現(xiàn)的功能較簡單，僅在移動機器人已經(jīng)行進了給定距離后才開始處理掃描數(shù)據(jù)，隨著掃描數(shù)據(jù)的增多，運行時間會加大。因而，在掃描處理上也做出了優(yōu)化改進，增加了限定時間的掃描處理條件，以便更加快速的處理掃描。

3 實驗環(huán)境與分析

3.1 實驗環(huán)境與結(jié)果

選取由簡單到復(fù)雜的兩個實驗場景，分別用傳統(tǒng)RBPF-SLAM算法即Gmapping算法、其它改進RBPF-SLAM算法[7]以及本文優(yōu)化的RBPF-SLAM算法進行實驗。

實驗場景一：近似地圖為80 m×2 m，環(huán)境比較簡單，包括長走廊，電梯口等，真實場景如圖2(a)所示。三種算法使用相同的粒子數(shù)，構(gòu)建地圖結(jié)果如圖2(b)、(c)、(d)所示。

圖2 三種算法分別構(gòu)圖結(jié)果

圖2(b)、(c)、(d)是移動機器人從A點到B點再到C點使用不同算法在RVIZ上顯示的構(gòu)圖情況。選取的柵格大小同樣為5 cm×5 cm。從構(gòu)建地圖的最終結(jié)果來看，隨著時間增加，圖2(b)中會出現(xiàn)邊緣地方不一致性現(xiàn)象，如圖中標出的地方；圖2(c)和圖2(d)構(gòu)圖效果較好，可以明顯地看出D點凹凸的輪廓。

在場景一中分別進行5次實驗，計算這兩種算法創(chuàng)建相同一致性地圖時，需要的平均運行時間。實驗時，機器人平均速度為0.4 m/s，改進算法需要258 s，本文優(yōu)化的RBPF-SLAM算法需要232 s，比改進算法更快。

實驗場景二：近似地圖為6 m×8 m，存在一些障礙物，如辦公桌椅、書柜、長桌等，如圖3(a)所示。同樣使用三種算法進行對比實驗。查看構(gòu)建一致性地圖時，三種算法使用粒子數(shù)的情況。圖3(b)是本文優(yōu)化的RBPF-SLAM算法構(gòu)建的地圖結(jié)果。

圖3 實驗場景及本文算法構(gòu)圖結(jié)果

當構(gòu)建的地圖結(jié)果中A點、B點和C點都能清晰看出輪廓時，記錄三種算法使用粒子數(shù)的情況，傳統(tǒng)RBPF-SLAM算法使用38個粒子，改進RBPF-SLAM算法使用20個粒子，本文優(yōu)化的RBPF-SLAM算法只需要18個粒子。

3.2 實驗結(jié)果分析

從構(gòu)建地圖結(jié)果中可以看出，當實驗環(huán)境較簡單(實驗場景一)，使用相同粒子數(shù)時，其它研究者的改進算法以及本文優(yōu)化的RBPF-SLAM算法構(gòu)圖結(jié)果更加可靠，地圖出現(xiàn)不一致性現(xiàn)象的情況較少。說明將采樣更集中在觀測信息的可行性區(qū)域，使得采樣的粒子更加符合真實環(huán)境的狀態(tài)，可以改善地圖構(gòu)建不一致性。而構(gòu)建相同精度的地圖時，與改進RBPF-SLAM算法相比較，本文優(yōu)化的RBPF-SLAM算法所需要的平均運行時間更短，算法的效率更高。

當實驗環(huán)境復(fù)雜一些(實驗場景二)，構(gòu)建相同清晰度的地圖時，本文優(yōu)化的RBPF-SLAM算法使用的粒子數(shù)更少，有效地降低了算法的計算量。

為了對算法的粒子多樣性進行驗證，同樣用三種算法在使用相同30個粒子時，查看重采樣過程中粒子多樣性變化情況，如圖4所示。

圖4 重采樣過程中粒子多樣性變化

由圖4可以看出：本文優(yōu)化的RBPF-SLAM算法，在重采樣過程中，不同粒子數(shù)明顯多于傳統(tǒng)的RBPF-SLAM算法；與改進的RBPF-SLAM算法相比較，粒子多樣性的波動范圍會更大一些。也說明了優(yōu)化算法不僅可以緩解粒子退化現(xiàn)象，而且粒子的多樣性更好一些。

4 結(jié) 論

本文優(yōu)化的Rao-Blackwellized粒子濾波的同時定位與地圖構(gòu)建算法，在提議分布中融合了激光雷達的觀測信息，將采樣集中在觀測信息的可能性區(qū)域，減少了與目標分布的誤差，提高了準確度，有效地減少了地圖不一致性。而通過控制權(quán)值的閾值，維持盡可能多的粒子多樣性的分層重采樣優(yōu)化策略，有效緩解了粒子退化現(xiàn)象。而進一步優(yōu)化的掃描處理，增加了限定時間的掃描處理條件，加快了處理速度，降低了運行時間。下一步工作，嘗試在更大的實驗環(huán)境下驗證優(yōu)化RBPF-SLAM算法的魯棒性。