基于環(huán)境語義信息的移動機器人重定位增強

向 超，蔣 林，雷 斌，朱建陽

(1.武漢科技大學機器人與智能系統(tǒng)研究院，湖北 武漢，430081；2.武漢科技大學冶金裝備及其控制教育部重點實驗室，湖北 武漢，430081)

為了實現(xiàn)移動機器人自主性的導航規(guī)劃任務，需要進行環(huán)境建模即地圖構建。移動機器人導航領域中常用的有拓撲地圖[1]、特征地圖[2]、柵格地圖[3]等，其中柵格地圖是將環(huán)境劃分為若干個相同大小的小柵格，用三種不同的顏色信息表示障礙物區(qū)域、無障礙物區(qū)域以及未知區(qū)域，它已成為目前應用于導航中的主流地圖形式。研究人員開發(fā)并開源了基于粒子濾波[4]、圖優(yōu)化[5]等方法的多種即時定位和柵格地圖構建方法。

柵格地圖雖然已能滿足機器人的基礎定位與導航要求，但它只表達了環(huán)境結構信息，機器人根據柵格地圖進行全局定位時，能夠利用的信息還比較少。在機器人重定位過程中，往往采用粒子濾波算法，通過采樣粒子與激光雷達信息逐步迭代完成定位，但在沒有人為設置條件及其他知識的情況下，粒子濾波方法的全局粒子撒布較為隨機，重定位往往會出現(xiàn)錯誤收斂或短時間內無法收斂的問題。

針對上述問題，胡章芳等[6]通過在環(huán)境中布局二維碼約束粒子采樣，增強了定位效果；黃鶴等[7]則通過在環(huán)境內布局多攝像頭監(jiān)控，在多攝像頭采集的圖像中提取ORB特征匹配檢測機器人所在區(qū)域，提高了機器人被綁架后重定位的時間效率；張啟彬等[8]通過分析機器人到障礙物的距離以及傳感器感知面積這兩個特征來快速確定機器人可能所在的區(qū)域；Park等[9]利用激光雷達信息訓練用于位置識別的分類器，完成機器人的粗定位，提高了姿態(tài)恢復的成功率。無論是通過布局二維碼等人工標記，還是利用視頻監(jiān)控獲取機器人位置，這些增強定位效果的方法都不夠直接、自主；而在僅通過激光雷達信息完成粗定位時，所利用的柵格地圖比較簡單，信息不夠豐富，同時，利用激光雷達信息單獨訓練粗定位分類器也較為麻煩。

近些年來，深度學習方法在目標檢測領域取得了矚目的成就[10]。基于卷積神經網絡的目標檢測方法在計算機視覺技術中得到實際應用，可為機器人提供豐富的語義信息，使得機器人能像人一樣思考環(huán)境和理解環(huán)境，補充柵格地圖未能表達的信息，為機器人的定位、導航提供支持。

本文結合深度學習與粒子濾波算法，針對室內環(huán)境提出一種加載環(huán)境語義信息的移動機器人重定位增強方法，同樣將重定位過程分為粗定位與精定位兩個階段，但不同點在于其采用豐富的語義信息初步估計機器人位置，從而在一定程度上增強機器人重定位的準確性與快速收斂性。

1 本文方法的框架結構

本文提出的移動機器人重定位增強方法如圖1所示。在建立機器人工作環(huán)境地圖時，同時獲取二維柵格地圖與環(huán)境內的語義信息，完成遍歷后根據柵格地圖將環(huán)境劃分為各個功能區(qū)塊，獲取其各自的語義信息作為功能區(qū)特征，建立子區(qū)域物體類別表。將語義信息用于環(huán)境匹配，完成機器人粗定位，并將該定位結果作為粒子濾波器的先驗知識，在子區(qū)域中利用粒子濾波器對機器人的姿態(tài)進行精確估計。

圖1 移動機器人重定位方法的框架結構

Fig.1 Framework of relocalization method for mobile robots

2 增強先驗的全局定位粒子濾波方法

2.1 粒子濾波重定位方法及其相關問題

機器人定位可描述為連續(xù)時間概率計算問題：bel(Xk)=p(X0∶k|m,z1∶k,u1∶k)。通過貝葉斯方法與馬爾科夫假設，將該問題進一步表示為：

bel(Xt)=ηp(zt|Xt,m)p(Xt|Xt-1,ut)bel(Xt-1)

(1)

式中：bel(Xt)為置信度；Xt為機器人t時刻的狀態(tài)估計；ut為控制量；zt為激光雷達測量數(shù)據；η為歸一化因子；m為全局地圖。

式(1)中包含了預測與更新兩個過程，p(Xt|Xt-1,ut)用于生成先驗概率分布來預測機器人狀態(tài)，p(zt|Xt,m)為測量似然概率，兩者結合后生成后驗概率分布，更新機器人狀態(tài)估計。

在機器人通過粒子濾波全局重定位時，如果沒有先驗知識，機器人初始位姿的不確定性較大，往往需要產生足夠多的粒子覆蓋整個狀態(tài)空間以獲得較好的結果，計算規(guī)模與占用內存都會呈指數(shù)級增長。同時，在環(huán)境結構存在一定相似性、二維環(huán)境特征簡單、粒子樣本數(shù)量不足或撒布不均時，由于可利用的信息較為單一，會經常出現(xiàn)錯誤收斂的情況。為此，本文通過劃分室內環(huán)境子區(qū)域、提取環(huán)境內語義信息并采用極大似然估計方法估計機器人所處的子區(qū)域，來完成其粗定位，從而約束粒子濾波器的初始粒子分布。

2.2 室內環(huán)境區(qū)域分塊

實際室內環(huán)境可依據功能分為臥室、客廳、廚房等，而各功能區(qū)域包含的可觀測物體類型會有一定的區(qū)別，顯然，這種區(qū)別有助于機器人定位。在利用功能區(qū)的不同進行定位前，首先要對其進行分離。通常各功能區(qū)被墻體所隔，又以地圖中的窄通道“門”來連接，因此在“門”的位置加入虛擬墻來分離各個房間。

在圖2所示測試環(huán)境中，將紙箱作為墻體，分隔出各個房間，在房中擺放不同類型或不同個數(shù)物體，以模擬實際環(huán)境各功能區(qū)域物體種類、數(shù)量的不同，其對應的柵格地圖見圖3(a)。圖3描述了區(qū)域分離的整個過程：根據柵格地圖求得環(huán)境中的大輪廓如圖3(b)所示(環(huán)境真實墻體一般為環(huán)境中的大連續(xù)障礙物)，并得到如圖3(c)所示的墻體以內區(qū)域；將輪廓圖腐蝕、膨脹以連接墻體之間窄通道，結果見圖3(d)；提取骨架后的結果見圖3(e)，其基本表達了真實墻體與虛擬墻體，也可表達成如圖3(f)所示的建議分塊區(qū)域；將建議分塊區(qū)域與圖3(c)做差值，可得到如圖3(g)所示誤差區(qū)域；最后將建議分塊的每個連續(xù)區(qū)域分別單獨加載到誤差圖對應位置，并獲取對應區(qū)域連續(xù)域，從而獲得某一完整區(qū)域，例如將圖3(f)左上角區(qū)域加載到誤差圖中得到圖3(h)，獲取連續(xù)域后得到圖3(i)所示完整區(qū)域；組合以上各個區(qū)域分塊結果并去除區(qū)域連接處重復線(虛擬墻，如圖3(j)所示)后則獲得區(qū)域最終分塊結果，如圖3(k)所示，圖中不同顏色區(qū)域代表不同功能區(qū)，從左至右、自上而下分別定義為區(qū)域1～6。

圖2 測試環(huán)境全景

(a) (b) (c) (d)

(e)

(f) (g) (h) (i) (j)

(k)

圖3 區(qū)域分塊結果

Fig.3 Results of regional separation

2.3 環(huán)境語義特征獲取與子區(qū)域物體類別表建立

在計算機視覺檢測識別領域，使用選擇性搜索的方法生成候選區(qū)域以及基于卷積神經網絡的深度學習方法已逐漸替代基于滑動窗口與人工特征提取的傳統(tǒng)檢測識別方法。當前較為典型的卷積神經網絡模型有R-CNN[11]、fast R-CNN[12]、faster R-CNN[13]、YOLO[14]、SSD[15]等，其中SSD結合了YOLO的網格化回歸思想和faster R-CNN的Anchor機制，同時還加入了多尺度特征圖檢測，可兼顧檢測速率與檢測準確率，有利于機器人對環(huán)境的語義信息提取。筆者收集并標注了包含室內常見物體在多視角、多視距、多亮度觀測條件下的12 000張室內場景圖片,用于SSD模型的訓練與測試，得到了室內物體檢測識別模型，綜合檢測精度為0.81。

飲食行為影響因素中，可看到產婦及其配偶文化程度高是部分食物攝入的保護因素，父母/公婆的建議是產婦對相關知識獲得的主要途徑且會影響產婦對食物的選擇，而從醫(yī)生/護士、網絡等獲取知識的產婦目前只是少部分。往后的工作中應廣泛開展營養(yǎng)健康教育，從而提高產婦及其家屬的營養(yǎng)保健知識水平，以降低對女性健康的不利影響。

為了區(qū)分環(huán)境內的物體類別與個數(shù)，本文選用美國微軟公司的kinectV2相機獲取彩色圖與深度圖，在標定配準深度相機與彩色相機后，將采集的彩色圖像輸入至SSD識別模型中得到檢測框與類別，并將彩色圖像輸入至GrabCut方法[16]中得到分割結果，通過深度圖像與相機模型獲取檢測框內剩余點云數(shù)據，最后同步SLAM算法的定位信息完成坐標轉換并得到檢測框內點云的世界坐標，柵格化后得到柵格坐標。將檢測到的每個物體都以(label、cells、pose)的形式存儲，其中，label為物體類別，cells為柵格坐標系下的物體柵格坐標，pose為第一次檢測識別到該物體時機器人的姿態(tài)。在遍歷環(huán)境過程中，如識別到同類物體時，將物體柵格坐標進行連續(xù)性判斷與重復性匹配，如無直接連續(xù)性或重復性較低，則認為其為環(huán)境中的另一物體，與此同時記錄下檢測到該物體時機器人的位姿，保存物體信息，表示機器人于當前位置可觀測物體。

由于本文測試環(huán)境中的紙箱高度較低，無法如真實墻體一樣遮擋相機視野，因此在遍歷環(huán)境過程中，根據激光雷達掃描距離適當選取相機視距，只獲取視距以內物體。在遍歷整體環(huán)境后，將檢測到物體時機器人的姿態(tài)pose投影到各個功能區(qū)中，如圖4(a)中所示，綠點為機器人當前位置，藍色區(qū)域為相機當前觀測范圍，紅色區(qū)域為當前機器人所位于的功能區(qū)，其對應檢測結果如圖4(b)所示，表示在當前功能區(qū)當前位姿下檢測到door與chair。

(a)機器人位姿

(b)檢測結果

圖4 機器人位姿與當前檢測結果

Fig.4 Robot’s pose and current observations

將全部檢測結果與機器人位姿投影到各區(qū)域后，得到子區(qū)域中的物體類別和數(shù)量，將其作為各區(qū)域特征，用于匹配區(qū)分各功能區(qū)。表1所示即為根據圖2中測試環(huán)境得到的對應信息。

表1 子區(qū)域中的物體類別和數(shù)量

2.4 基于極大似然估計的語義粗定位

盡管當前采用深度學習方法已經取得較高的檢測識別精度，但由于光照等因素還是會產生一定程度上的漏檢與誤檢。為了降低漏檢、誤檢對區(qū)域估計的影響，在實際應用中，根據對某一類物體在應用環(huán)境中的檢出與識別正確率來適當增加或減少該類物體對于區(qū)域判斷的貢獻值，以使粗定位估計更加準確。例如在本文搭建的測試環(huán)境中，由于模擬墻體的紙箱與語義庫中的柜子外觀較為相似，檢測識別時偶爾會將紙箱識別成柜子，故本文適當減少了檢測到柜子時對區(qū)域判斷的貢獻值。

(2)

在完成粗定位之后，將粒子約束在子區(qū)域中，相比于全局分布粒子，排除了語義小概率粒子與其他柵格地圖區(qū)域對地圖匹配的干擾，使機器人于語義高概率區(qū)域中通過粒子濾波器估計其位姿。理論上，通過語義信息增強先驗知識后的粒子濾波方法能有效提高粒子收斂速度并降低錯誤收斂概率。

3 重定位實驗結果對比分析

圖5為采用傳統(tǒng)粒子濾波方法的粒子撒布與重定位結果。圖5(a)為粒子全局撒布形式，圖5(b)為在圖5(a)基礎上機器人旋轉后的重定位狀態(tài)，隨著機器人的移動與粒子濾波器的迭代，粒子逐漸收斂，最終收斂于區(qū)域5，而此時機器人實際位于區(qū)域2內，圖中彩色點為激光雷達在真實位置掃描結果，在該錯誤收斂結果下掃描值與地圖重合度似乎較高，但實際上是因為環(huán)境結構有一定的相似性且柵格地圖信息不夠豐富導致收斂錯誤，機器人重定位失敗。

(a)粒子分布方式

(b)重定位結果

圖5 采用傳統(tǒng)粒子濾波方法的重定位結果

Fig.5 Relocalization result by traditional particle filter

在本文方法中，機器人首先利用相機觀測周圍環(huán)境，其中幾幀觀測結果如圖6所示，檢測到可視范圍內包含了椅子與垃圾桶，未檢測到門，且由于在局部地圖中兩幀觀測結果的椅子的點云重復度為0，機器人此時認為該區(qū)域中存在兩把椅子。根據子區(qū)域物體類別表，機器人更相信自身位于2#房間內，得到語義高概率區(qū)域，于是將粒子重點分布在該房間，如圖7(a)所示。機器人通過粒子濾波器精確定位后，定位結果如圖7(b)所示，粒子收斂于區(qū)域2內且激光數(shù)據與障礙物基本重合，表明此時定位成功。

(a)

(c)

圖6 觀測結果

Fig.6 Observation results

(a)粒子分布方式

(b)重定位結果

圖7 采用本文方法的重定位結果1

Fig.7 Relocalization result 1#by the proposed method

對于應用環(huán)境存在多個功能類似區(qū)域的情況，為了防止機器人粗定位出錯而導致機器人無法成功精定位，在區(qū)域估計中出現(xiàn)多區(qū)域有相同高概率時，則在環(huán)境中的多個子區(qū)域分布粒子。例如，在重定位實驗中，機器人實際位于測試環(huán)境區(qū)域6，相機觀測周圍環(huán)境只檢測到柜子、椅子，本文方法估計出區(qū)域5和區(qū)域6有相同高概率，則將粒子分布到這兩個子區(qū)域，如圖8(a)所示，同樣可減少粒子分布范圍，去除了語義小概率區(qū)域對粒子濾波器的干擾，通過粒子濾波器迭代收斂后，最后收斂結果如圖8(b)所示，定位成功。

(a)粒子分布方式

(b)重定位結果

圖8 采用本文方法的重定位結果2

Fig.8 Relocalization result 2#by the proposed method

為了分析本文方法對于重定位準確率的提升效果，采用傳統(tǒng)粒子濾波方法和本文方法在同等粒子數(shù)量條件下分別重復進行25次實驗，結果如表2中第1、2行所示。由表2可見：粒子全局分布方式的定位成功率為52%；本文同粒子數(shù)局部分布方式的定位成功率為88%，其中，3個失敗案例是由于出現(xiàn)了漏檢與誤檢使得粗定位估計不準確，最終導致精定位失敗，而對于其他粗定位成功的22個案例，隨后的精定位全部成功。

表2 實驗結果對比

綜合圖5、圖7與表2可知，在采用同等粒子數(shù)量的條件下，通過查詢先驗知識的局部粒子撒布方法的重定位更為準確，而且此時僅僅利用了為數(shù)不多的語義特征。如果環(huán)境中物體類別更多，語義庫、語義特征更加豐富，各區(qū)域的區(qū)分性就會更高，相應地機器人粗定位的估計準確率也會更高。

為了驗證本文方法在粒子數(shù)量方面的優(yōu)勢，在機器人粗定位之后，適量降低機器人重定位時粒子撒布數(shù)量。如圖9所示，粗定位估計機器人位于區(qū)域2內，由于區(qū)域2約占整體環(huán)境1/4的區(qū)域，將粒子數(shù)量降低為全局撒布粒子方式的1/4。同樣重復進行25次重定位實驗，結果如表2中第3行所示，重定位成功19次，失敗6次，成功率為76%。由于粒子只撒布在局部區(qū)域，一定程度上去除了其他區(qū)域對于數(shù)據匹配的干擾，盡管粒子數(shù)量降低為原來的1/4，本文方法的重定位成功率依然比全局均勻撒布粒子方式要高。

(a)粒子分布方式

(b)重定位結果

圖9 采用本文方法的重定位結果3

Fig.9 Relocalization result 3#by the proposed method

為了驗證本文方法在收斂速度上的優(yōu)勢，從上述同粒子數(shù)量條件下的實驗中獲取重定位成功案例，分別記錄重定位過程中粒子平均權重的變化，如圖10所示。從本文方法的平均權重變化曲線可見，迭代更新13次后，粒子快速收斂，更新21次后平均權重穩(wěn)定在較高值；從全局撒布粒子方式的平均權重變化曲線可見，由于粒子分布初值較差，更新過程中平均權重增長較慢，在更新31次后平均權重才達到0.02以上。該實驗結果表明，本文方法使機器人在同等粒子數(shù)量條件下的全局重定位收斂速度得到明顯提高。

圖10 粒子平均權重變化曲線

4 結語

本文針對室內環(huán)境下基于粒子濾波的機器人重定位收斂失敗、收斂速度慢等問題，提出一種通過提取語義信息來增強定位效果的方法。在機器人建立環(huán)境地圖時就獲取環(huán)境語義信息，并根據柵格地圖將環(huán)境劃分為多個子區(qū)域。在機器人自主重定位過程中，本文方法先通過機器人觀測四周環(huán)境，利用環(huán)境內的語義信息進行語義節(jié)點匹配，從語義上確定自身可能位于的子區(qū)域。之后，在語義高概率子區(qū)域初始化粒子，大大減少了粒子撒布范圍，降低了粒子濾波器的誤匹配概率。相較于傳統(tǒng)的全局分布粒子濾波重定位方法，加載了語義信息之后，在相同甚至減少粒子數(shù)量的情況下，本文方法的機器人重定位準確性更高，定位收斂速度更快。