基于粒子濾波的室內巡邏機器人SLAM技術研究

管晨曦，王淑營

(西南交通大學唐山研究生院,河北 唐山 063000)

0 引言

在生產(chǎn)生活中，安全一直是人們關心的問題。傳統(tǒng)的室內安防模式一般有兩種：一種是人力巡邏，由于人力成本高、監(jiān)測不到位、危險性高而逐漸被淘汰；另一種是視頻監(jiān)控，存在安裝成本過高、監(jiān)控存在死區(qū)、容易被破壞或躲避等問題。進入21世紀以來，隨著人工智能和機器人技術的不斷發(fā)展，人們迫切需要智能機器人替代人工完成巡邏工作。尤其是近幾年，全球掀起了應用在安防領域的巡邏機器人的研究熱潮。

目前，傳統(tǒng)的巡邏機器人主要以差速轉向底盤為主。石學文等提出了一種差分輪式機器人系統(tǒng)[1]。該機器人可以較好地進行運動。楊記周等設計了一種差速輪式機器人[2]。該機器人結構簡單，便于使用。但是這些機器人在實際室內場景移動時會出現(xiàn)轉向困難、容易碰撞等問題。徐文麗等設計了一種可以全向行駛的智能機器人。該機器人可以全向轉動，但是只能按照既定軌道行駛[3]。

室內巡邏機器人主要是基于構建好的地圖進行路徑規(guī)劃、運動控制與自主導航。其中，同步定位與地圖構建(simultaneous localization and mapping，SLAM)是核心技術[4]。Murphy和Doucet為了提高建圖準確性，首次把Rao-Blackwellized粒子濾波器(Rao-Blackwellized particle filters,RBPF)引入到SLAM問題[5]。陳國軍等提出了一種利用修正梯度細化算法對傳統(tǒng)的RBPF-SLAM算法進行改進的新方法，有效提高了構建地圖的準確性[6]。孫弋等針對RBPF-SLAM算法進行改進，結合退火參數(shù)優(yōu)化了提議分布，并引入遺傳算法進行重采樣，降低了計算時間[7]。張廷軍等針對重采樣過程引起的粒子多樣性降低的問題,提出部分粒子免疫優(yōu)化重采樣方法，提高了粒子的多樣性[8]。但是上述改進算法在應用于室內建圖環(huán)境時，很可能舍棄了更好的粒子，導致出現(xiàn)局部最優(yōu)的問題。

綜合上述原因，在硬件方面，本文為巡邏機器人選擇全向移動底盤，較好地避免了機器人在實際場景試驗時容易出現(xiàn)的轉向困難、容易碰撞等問題。在算法方面，針對傳統(tǒng)的RBPF-SLAM算法在重采樣之后導致的粒子退化問題，本文提出使用馬爾可夫鏈蒙特卡羅(Markov chain Monte Carlo,MCMC)移動步驟來增加粒子的多樣性。在計算提議分布時，本文提出添加激光雷達觀測模型，提高了地圖構建的精度。試驗證明，優(yōu)化的RBPF-SLAM算法相較于傳統(tǒng)算法具有更好的建圖效果。

1 全向巡邏機器人系統(tǒng)設計

1.1 全向巡邏機器人平臺

本文巡邏機器人的全向移動底盤數(shù)學模型如圖1所示。

圖1 全向底盤數(shù)學模型示意圖Fig.1 Schematic diagram of omnidirectional chassismathematical model

巡邏機器人使用的是可以進行全向行駛的麥克納姆全向移動底盤[9]。很多室內環(huán)境具有長廊、拐角等狹小部分。傳統(tǒng)的差速移動機器人靈活性差，容易發(fā)生碰撞，不能很好地完成轉向動作，所以不適合在室內進行巡邏工作。麥克納姆全向移動平臺可以很好地完成旋轉和平移等功能，具有較好的靈活性和機動性，可以充分發(fā)揮全向移動功能。規(guī)定x軸的正方向是巡邏機器人的向前移動方向，y軸的正方向是機器人的向左移動方向，逆時針是機器人的旋轉正方向。

設機器人底盤長度為2L、寬度為2S、車輪的半徑為Rω，輪轂軸和麥輪輥子軸線夾角為α，規(guī)定vx為x方向的移速、vy為y方向的移速，角速度為ωz。此外,設ωi(i=1,2,3,4)分別為四個車輪的旋轉角速度，Vi(i=1,2,3,4)分別為四個車輪的線速度，且Vi=Rω×ωi。正向分析全向巡邏機器人運動學，在已有vx、vy、ωz、α以及機器人尺寸的情況下,四個車輪的線速度Vi(i=1,2,3,4)可由式(1)得到：

(1)

在四個車輪的線速度Vi(i=1,2,3,4)已知的情況下，也可以逆向分析全向移動機器人運動學。全向移動機器人在坐標系中的速度和角速度可以通過四個車輪的線速度計算出：

(2)

1.2 機器人硬件系統(tǒng)設計

全向巡邏機器人的硬件主要包括：搭載STM32的麥克納姆輪底盤、樹莓派4B和各種環(huán)境傳感器。其中，巡邏機器人的前進、后退、旋轉與停止等運動通過全向移動底盤實現(xiàn)。樹莓派4B作為上位機可以控制整個巡邏機器人，使機器人實現(xiàn)各種巡邏功能。相比于大多數(shù)的機器人控制器，樹莓派4B具有更高的性能。環(huán)境傳感器可以較為準確地感知室內環(huán)境信息，并且可以和樹莓派相連傳遞信息。環(huán)境傳感器包括攝像頭、溫度傳感器、煙霧傳感器、激光雷達等。巡邏機器人硬件設計如圖2所示。

圖2 機器人硬件設計框圖Fig.2 Block diagram of robot hardware design

巡邏機器人在移動過程中，通過激光雷達發(fā)射激光來獲取周圍障礙物的具體位置信息和周圍環(huán)境的信息。機器人可以通過這些信息完成地圖的構建以及自主導航的功能。激光雷達通過USB接口與樹莓派相連，將外部環(huán)境的信息發(fā)送給樹莓派進行處理。巡邏機器人在巡邏過程中使用攝像頭實現(xiàn)監(jiān)控功能。攝像頭與樹莓派相連,可以實時上傳拍攝的視頻。機器人通過STM32運動控制器連接四個直流電機進行移動。慣性測量單元(inertial measurement unit,IMU)加速度陀螺儀搭載在STM32運動控制器上，將收集到的測量信息上傳到樹莓派。

1.3 機器人軟件架構設計

機器人軟件架構如圖3所示。

圖3 機器人軟件架構示意圖Fig.3 Schematic diagram of robot software architecture

對于全向巡邏機器人的軟件架構,采用開源機器人系統(tǒng)(robot operating system,ROS)作為底層的框架。2007年，ROS誕生于機器人公司W(wǎng)illow Garage的個人機器人項目和斯坦福大學人工智能實驗室的合作項目[10]。ROS是一種編寫機器人相關程序的靈活開源操作系統(tǒng)，為使用者提供了多種功能。在ROS中，用節(jié)點表示一個應用程序，節(jié)點與節(jié)點之間通過事先定義好的消息(topic)、服務(service)或動作(action)進行連接，通信信息從發(fā)送節(jié)點傳送到接收節(jié)點。點對點的設計可以分散計算機的運算壓力，而節(jié)點的松耦合特性使得ROS編程更靈活易修改。本文在充分發(fā)揮ROS性能的基礎上，將機器人的軟件架構分為四層，分別是可視層、節(jié)點層、驅動層、信息層。

可視層可以直觀地查看巡邏機器人的所有信息，并對機器人進行操控。比如：啟動視頻處理節(jié)點來處理攝像頭拍攝的錄像；啟動SLAM建圖節(jié)點來實現(xiàn)地圖構建功能；啟動底盤控制節(jié)點來實現(xiàn)機器人的人工底盤控制等。

節(jié)點層是把巡邏機器人的SLAM建圖、視頻處理、導航等功能進行模塊化處理，并編寫、設計成ROS節(jié)點。巡邏機器人通過節(jié)點層的各個節(jié)點實現(xiàn)相應的功能。

驅動層包含各功能模塊的驅動，用于控制傳感器的正常工作。傳感器采集到的信息屬于信息層。

信息層通過USB、Wi-Fi、局域網(wǎng)等進行數(shù)據(jù)傳遞，將機器人采集的各種環(huán)境信息發(fā)送到樹莓派。樹莓派將接收到的環(huán)境信息進行數(shù)據(jù)處理，最后執(zhí)行相應的任務。同時，可視層將處理的結果信息通過可視化界面展示出來。

2 RBPF-SLAM算法原理

傳統(tǒng)的SLAM算法利用激光雷達的觀測數(shù)據(jù)z1:t=z1,z2,...,zt和里程計中的控制信息u1:t-1=u1,u2,...,ut-1來表示機器人的后驗路徑分布p(x1:t,m|z1:t,u1:t-1)。基于粒子濾波的SLAM算法在尋找后驗概率分布的過程中，由于狀態(tài)空間維數(shù)的擴大，計算復雜度非常高。RBPF-SLAM算法將SLAM問題分解為兩點:一是機器人的定位;二是已知機器人的位姿進行地圖構建。RBPF-SLAM算法減少了采樣時的狀態(tài)空間維數(shù)，從而達到簡化計算的效果[11]。RBPF粒子濾波器分解后的公式如下:

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

(3)

RBPF算法用粒子來表示定位的概率分布(每個粒子包含機器人的姿態(tài))，然后結合觀測模型來更新地圖。總體步驟如下。

③權重計算:為了補償提議分布與目標后驗分布的差距，計算每個粒子的權重w，公式如下。

(4)

(5)

當式(5)計算的數(shù)值小于設定值時，意味著粒子退化嚴重，那么算法在重采樣時會丟棄權重較小的粒子，保留權重較大的粒子。

3 優(yōu)化的RBPF-SLAM算法

傳統(tǒng)的RBPF-SLAM算法重采策略是丟棄權重小的粒子，復制權重大的粒子。這樣會造成粒子貧乏問題。MCMC重采樣方法可以有效恢復粒子的多樣性[12]，從而解決粒子貧乏問題。本文引入了MCMC移動步驟來增加粒子的多樣性。

(6)

由此產(chǎn)生的新粒子更加接近真實目標分布。

①隨機產(chǎn)生一個在區(qū)間[0,1]上服從均勻分布的隨機數(shù)v。

(7)

在計算提議分布時，僅僅采用了機器人的里程計信息。這樣雖然方便，但是由于巡邏機器人在移動的過程中存在打滑等現(xiàn)象，并且里程計本身就不夠準確，當遠里程計信息的誤差遠大于激光雷達信息的誤差時，運動模型的似然區(qū)域遠大于觀測模型的似然可行區(qū)域L(i)，粒子具有較高的觀測后驗似然值，因而擁有較高的權重。所以僅僅使用里程計運動模型來計算RBPF的提議分布并不是最好的方法。為了解決上述問題，本文將激光雷達獲得的觀測信息zt添加到提議分布，并且盡量在觀測信息的似然函數(shù)可行域內進行采樣。在優(yōu)化后提議分布的采樣結果如下:

(8)

根據(jù)式(8)，可實現(xiàn)粒子的權重方差最小。由于使用了激光雷達，并且添加了觀測數(shù)據(jù)，因此優(yōu)化后的提議分布可以比較簡單地獲得似然函數(shù)的峰值。由于不能直接通過式(5)進行采樣計算，所以以式(9)進行采樣:

(9)

(10)

(11)

最終的粒子方差為：

(12)

在每個數(shù)組(ut-1,zt)有更新的時候，都要重新計算每個粒子的提議分布并且更新粒子。算法步驟如下。

④采樣提議分布q中的粒子。

⑤通過式(13)計算和更新粒子的權重，然后重新采樣；重采樣之后，進行MCMC移動步驟。

4 試驗分析

本文利用自行設計組裝的全向巡邏機器人在室內環(huán)境中對優(yōu)化后的SLAM建圖算法進行試驗驗證，試驗場地為室內機房及走廊。該巡邏機器人基于麥克納姆輪底盤，配備有思嵐A3激光雷達、里程計、Vizum雙目攝像機、慣性測量單元、樹莓派4B等元件。操作系統(tǒng)為基于Ubuntu16.04的ROS系統(tǒng)。通過Wi-Fi熱點與筆記本計算機建立聯(lián)系，通過計算機來控制機器人的運動。在ROS系統(tǒng)自帶的RVIZ可視化界面中顯示構建好的地圖。地圖使用柵格類型進行表示。

為了研究優(yōu)化后的RBPF-SLAM算法的有效性，分別采用傳統(tǒng)的RBPF-SLAM算法和優(yōu)化后的算法進行地圖構建試驗。試驗過程中控制機器人以0.3 m/s的速度環(huán)繞機房和走廊進行移動構建地圖。機房右上方與走廊相連接，機房內共有三排計算機及配套桌。傳統(tǒng)RBPF-SLAM算法構建的地圖如圖4所示。優(yōu)化RBPF-SLAM算法構建的地圖如圖5所示。

根據(jù)圖4和圖5可知，傳統(tǒng)RBPF-SLAM算法構建的柵格地圖出現(xiàn)了走廊內容缺失、整體輪廓變形、地圖比例不準確的情況。使用優(yōu)化算法構建的柵格地圖精確度更高、輪廓更清晰、效果更好。這是因為傳統(tǒng)的RBPF-SLAM算法在重采樣后導致了粒子的退化，從而影響了地圖的精確度。又由于計算提議分布時只使用了里程計的信息，隨著時間的增加，里程計讀數(shù)和真實信息之間的誤差疊加越來越大，導致算法的誤差變大、地圖精確度隨之降低。而優(yōu)化的RBPF-SLAM算法使用了MCMC移動步驟來增加粒子的多樣性，同時添加了激光雷達觀測模型，從而有效地提高了提議分布的準確率。試驗結果表明：優(yōu)化后的算法能夠構建更精確的地圖。

圖4 傳統(tǒng)RBPF-SLAM算法構建的地圖Fig.4 Map constructed by traditional RBPF-SLAM algorithm

圖5 優(yōu)化RBPF-SLAM算法構建的地圖Fig.5 Map constructed by optimized RBPF-SLAM algorithm

5 結論

首先，本文為巡邏機器人選擇全向移動地盤，使機器人轉向更靈活。其次，本文在傳統(tǒng)RBPF-SLAM算法的基礎上，提出加入MCMC移動步驟來保護粒子的多樣性。最后，針對提議分布不準確導致的建圖不準確問題，本文提出在計算提議分布添加激光雷達觀測模型。本文基于自行設計的巡邏機器人在室內環(huán)境進行了試驗。試驗結果表明，優(yōu)化后的RBPF-SLAM算法與原算法相比，減小了建圖時的誤差，取得了更好的建圖效果。本文的研究對于機器人的相關領域研究具有借鑒意義。