一種室內高動態環境的機器人定位方法

黃 山，黃洪鐘*，曾 奇，錢華明

(1.電子科技大學系統可靠性與安全性研究中心 成都611731；2.成都越凡創新科技有限公司 成都610097)

隨著移動機器人技術的發展，全自主移動機器人在倉儲物流、野外救援、園區送貨和商場服務等場景得到了越來越廣泛的應用。移動機器人的定位是移動機器人進行全自主運動的前提，定位技術是移動機器人的核心技術之一，因此開展移動機器人定位技術的研究對于實現移動機器人的全自主運動具有重要意義。

移動機器人定位方法可以分為兩大類[1]：基于無線電信號的定位(wireless-based)和基于地圖的定位(map-based localization)?；跓o線電的定位方法主要有衛星定位方法、UWB定位方法、藍牙定位方法、wifi定位方法和RFID定位方法等[2]。這些定位方法的優點是能實現相對于某一個基準坐標系的絕對定位，定位誤差不會累計，在有信號的情況下可以實現比較準確的定位。然而基于無線電定位的方法在受到遮擋時，定位質量會急劇下降，并且需要預先設置定位用的基站，因此嚴重地限制了其在移動機器人領域的應用?；诘貓D的定位方法即事先構建機器人運行環境的地圖，然后利用機器人當前傳感器的觀測數據和預先構建的地圖進行匹配，得到機器人位姿。由于該方法不需要對環境進行改造，適用于移動機器人的推廣，因此在移動機器人定位領域得到了廣泛的應用。按照傳感器類型進行分類，主要可以分為基于激光雷達的方法(lidar-based)和基于相機的方法(camera-based)[3]。由于激光雷達具有測距精度高、抗干擾能力強等優點，因此本文使用激光雷達進行定位，以下所說的定位方法均指激光雷達定位。

機器人定位的最大挑戰來自于環境中的動態物體，環境中動態物體分為高動態物體(highdynamic object)和半靜態物體(semi-static object)兩大類。高動態物體是指環境中位置時刻在發生變化的物體，特別是機器人觀測到該物體時，該物體處于運動狀態，如行人、手推車等；半靜態物體是指環境中某些位置會發生變動但是變動頻率極低的物體，特別是機器人觀測到該物體時，該物體處于靜止狀態，如家具、停車場中的汽車等?；诘貓D的定位方法的基本原理是用傳感器觀測到的數據和地圖進行匹配，從而修正機器人的位置。顯然高動態物體的位置在實時變動，因此其不能提供定位信息。而半靜態物體的位置在短時間內不發生改變，在其位置不變的時間段內，可以提供定位信息。因此對于這兩種物體需要有不同的處理方式，高動態物體直接進行過濾而半靜態物體需要更新到地圖中以提高機器人定位的穩定性。

目前移動機器人領域應用最知名、應用最廣泛的方法為文獻[4]提出的蒙特卡洛定位方法(Monte Carlo localizaiton,MCL)。該方法用粒子(particle)來表示機器人的位姿，然后用里程計數據進行粒子的傳播，用激光雷達觀測數據和地圖的匹配程度來作為測量值更新粒子的權重，通過粒子濾波器的迭代更新實現對于機器人位置的估計，MCL默認環境是靜態的，在動態環境中表現較差。文獻[5]在MCL的基礎上，提出通過比較激光雷達期望測距和實際測距的差來估計該束激光雷達數據是否來自動態物體，該方法在處理高動態物體時取得了不錯的效果。文獻[6]通過動態物體跟蹤來分辨激光雷達數據是否來自動態物體。以上方法雖然能比較好地處理高動態物體，但是無法處理半靜態物體。而實際環境中存在大量的半靜態物體，如商場中店鋪位置的改變、店鋪的裝修及家具位置的移動等，因此對于半靜態物體的處理也很重要。

對半靜態物體進行處理，基本的思路是實現地圖更新，對半靜態物體進行建模。文獻[7-8]擴展了原本的覆蓋柵格地圖(occupancy grid map)，把動態物體也包含到覆蓋柵格地圖中。文獻[9]提出了動態覆蓋柵格(dynamic occupancy grid)的概念，該方法用一個隱馬爾可夫模型來表示二維柵格的占用概率和動態物體的概率。文獻[10]使用RBPF(rao-blackwellized particle filter)來同時估計機器人位姿和環境狀態，實現地圖更新。由于以上方法都使用了RBPF來進行機器人位姿的估計和地圖更新的計算，因此要么計算量巨大，不能滿足實時性的要求；要么會消耗巨量的內存，在資源受限的情況下無法運行。

除了使用粒子濾波之外，文獻[11-12]使用位姿圖(pose-graph)來處理動態物體，本文工作也受其啟發。最大不同在于本文使用柵格地圖，而文獻[11-12]使用點云地圖。點云地圖對噪聲比較敏感并且容易受到動態物體的影響，而柵格地圖對于這些干擾具有一定的抵抗能力，因此抗干擾能力更強。

商用服務機器人的運行場景中充斥著大量的高動態物體和半靜態物體，因此商用機器人的長時間穩定應用離不開能同時處理高動態物體和半靜態物體的定位系統。同時由于成本的限制，商用服務機器人的計算資源和內存資源都比較受限，這決定了其定位系統不能耗費太多的計算和內存資源。

為了克服以上方法的缺點，同時實現機器人的長時間穩定運行，本文提出了一種能同時處理高動態物體和半靜態物體的定位方法，同時該方法能在奔騰處理器上實時運行。該方法通過高動態物體的檢測與跟蹤過濾掉激光雷達數據中的高動態物體數據，極大提高了機器人在高動態環境中運行的穩定性。同時由于半靜態物體能提供豐富的定位信息，因此本文對于半靜態物體的處理不是簡單地去除，而是通過地圖更新的方式把半靜態物體信息融入到靜態地圖中，并提出了一種新的地圖更新方法，該方法能快速地實現地圖更新。

1 系統框架

本文提出的定位算法整體的系統框架如圖1所示。

從圖1可以看出，定位算法主要分為3個模塊：高動態物體處理模塊、半靜態物體模塊和地圖更新模塊，使用的傳感器包括激光雷達和輪式里程計。高動態物體處理模塊接收激光雷達數據和輪式里程計數據，以里程計數據為真值，對動態物體進行檢測和跟蹤。高動態物體處理模塊檢測出激光數據中包含的高動態物體，并進行過濾，輸出一個不帶高動態物體的過濾激光幀；過濾激光幀和里程計數據輸入半靜態物體處理和地圖更新模塊，半靜態物體處理和地圖更新模塊實現對于機器人的定位和地圖更新，把半靜態物體更新到地圖中，并且輸出更新后的地圖，同時輸出機器人當前的位置。

圖1 整體系統框架

2 高動態物體的處理

高動態物體檢測的目的是過濾掉相對于機器人正在運動的物體，如行人、推車等。因為高動態物體不但無法提供定位信息，反而會降低定位的成功率。本文對高動態物體的處理采用對比法和跟蹤法相結合的方式。為了克服對比法的缺點，本文利用定位系統的特點，提出了一種延遲對比方法，大大提高了動態物體檢測的性能。

2.1 延遲對比法

對比法是用當前激光幀數據z(t)和上一幀z(t-1)或者上幾幀激光數據z(t-k)進行對比，如果當前激光幀z(t)中有數據落在過去激光幀的視野范圍內，則說明該數據是由動態物體造成的。因為落在上一幀數據的視野范圍內，說明上一幀數據在對應位置沒有觀測到該物體，而當前幀在對應位置觀測到了該物體，因此該物體只可能是動態物體。對比法的工作原理如圖2所示。

圖2中，坐標系表示機器人當前的位置，黃色點表示上一幀激光數據，洋紅色的多邊形表示上一幀激光數據對應的視野范圍，藍色點表示當前幀激光數據，紅色點表示當前幀中被檢測為動態物體的數據，該數據實際上是一個運動的人，紅色點位于上一幀的視野范圍之內，因此其被認為是動態物體。

圖2 對比法工作原理圖

相對于機器人來說，動態物體分為向機器人靠近和遠離機器人兩種。從對比法的原理來說，只能處理向機器人靠近的動態物體，因為如果動態物體正在遠離機器人，那么該動態物體并不在上一幀數據的視野范圍，所以對比法無法檢測出遠離機器人的動態物體。

為了克服對比法的問題，本文提出了延遲對比法。對于機器人來說，里程計數據的精度在較短的時間內可以認為是比較高的，因此定位系統實際上只需要定期修正里程計的漂移即可，不需要進行實時修正。即用激光數據進行定位時，并不需要用當前最新的激光數據z(t)，而可以用過去一段時間的激光數據z(t?1)，比如幾秒鐘之前的數據。如果用過去的數據z(t?1)來定位，那么用定位的數據z(t?1)不但可以和以前的數據z(t?2)進行對比來識別動態物體，同時也可以和未來的數據z(t)進行對比來識別動態物體，延遲對比法的工作原理如圖3所示。圖中洋紅色的多邊形表示z(t?1)的視野范圍，綠色多邊形表示z(t+1)的視野范圍，紅色的點表示一個正在遠離機器人的行人。顯然紅色的點在洋紅色多邊形外，因此對比法無法識別出其是一個動態物體，然而紅色的點在綠色多邊形的內部，因此用延遲對比法可以識別出該行人是一個動態物體。

圖3 延遲對比法工作原理圖

顯然，和過去數據進行對比可以識別出向機器人靠近的動態物體，而和未來數據進行對比則可以識別出遠離機器人的物體，因此延遲對比法能識別兩種類型的動態物體，克服了對比法只能識別向機器人靠近的物體的缺點，能大幅提高機器人對于動態物體的處理，增強機器人在高動態環境定位的穩定性。

2.2 跟蹤法

與對比法不同，跟蹤法[13]不利用激光幀視野范圍形成的空白區域，只利用激光幀本身觀測到的數據。跟蹤法首先把一幀激光雷達數據根據歐式距離進行聚類，從而把一幀數據分解成一個個不同的類，這些不同的類跟上一幀的數據進行數據關聯，確定是否為同一個物體，如果確定為同一個物體，則可以認為是跟蹤成功；如果沒有匹配上，則可以認為是新出現的物體，分配一個新的ID進行跟蹤。判斷兩個類是否為同一個物體可以根據兩個類的距離以及相似度等。每新出現一個物體，則分配一個卡爾曼濾波器進行跟蹤來計算該物體的位置和速度等參數，如果連續成功地跟蹤了好幾幀，則根據物體的速度判斷其是否為動態物體，速度超過某一個閾值則認為是動態物體。

2.3 延遲對比法和跟蹤法的結合

延遲對比法的優勢是可以在看到動態物體的瞬間就能進行分辨，但是沒有對不同幀之間的動態物體進行數據關聯，缺失上下文信息，在傳感器存在噪聲的情況下，存在漏檢的可能；而跟蹤法對不同幀之間的動態物體進行數據關聯讓動態物體跟蹤比較穩定，但是必須跟蹤物體一定時間之后才能判斷該物體是否為動態物體。

為了克服彼此的缺點，本文將延遲對比法和跟蹤法兩種方法結合起來，對于每一個延遲對比法檢測出來的動態障礙物都分配一個濾波器進行跟蹤，并且在不同幀之間進行數據關聯，實現動態障礙物的實時檢測和穩定跟蹤的效果。

3 半靜態物體的處理

跟高動態物體不同，半靜態物體能提供豐富的定位信息，因此不能直接視為噪聲進行濾除，而是需要充分利用半靜態物體提供的信息來實現機器人的長時間穩定定位[14]。本文在進行半靜態物體處理時，假設激光數據中的高動態物體已經被濾除，激光數據中只包含有靜態物體(跟當前地圖吻合的物體)和半靜態物體(跟當前地圖不吻合的物體)。本文在處理半靜態物體時，采用位姿圖優化[15]的形式更新地圖。

3.1 位姿圖優化

位姿圖是一種機器人狀態估計的描述方法，用節點表示機器人的位姿，用邊表示兩個節點之間的空間約束，如圖4所示。

圖4 位姿示意圖

圖4中，X i表示移動機器人位姿，本文中機器人在平面上運動，因此X i=(xi,yi,θi)。Z ij表示兩個節點之間的相對位姿關系，Z ij可以通過里程計或者掃描匹配得到，黑色的邊表示幀間約束，紅色的邊表示回環約束。本文使用文獻[16]的算法進行粗搜索，使用文獻[17]的算法進行細搜索的方式來計算兩個節點的相對位姿關系。在給定幀間約束和回環約束的前提下，通過后端優化能得到機器人軌跡的極大似然估計。已知機器人整條軌跡的位姿，則可以通過覆蓋柵格構圖算法[18]生成對應的覆蓋柵格地圖。

無論是里程計測量還是幀間匹配都有誤差，因此隨著機器人的行走，誤差會不斷累積。只有當機器人進入已知區域，形成回環檢測時才能消除誤差，圖4中紅色的邊表示回環約束，X5和X2觀測到環境中同一個特征，因此可以通過掃描匹配得到相對位姿Z52，從而構建出回環約束。當回環約束構建完畢之后，即可以通過優化求解機器人軌跡的極大似然估計。

節點之間相對位姿的觀測值用Z ij和Λij表示，

由于預測函數是一個非線性函數，因此機器人位姿估計問題是一個非線性最小二乘(nonlinear least square)問題。對于式(3)的求解，可以通過局部線性化，然后迭代更新。方程的非線性來自于誤差函數，因此可以把誤差函數進行泰勒展開，然后取一階近似實現整個目標函數的線性化：

從式(8)可得，線性化后的目標函數是關于待求解變量的二次函數，因此目標函數的極小值可以通過對自變量求導，然后令導數等于0的方式進行求解：

顯然式(9)是一個比較容易求解的線性方程組。這里得到的解是線性化目標函數的最小值，只在當前解的鄰域內成立，因此需要不斷迭代，直到解收斂。解收斂則可得到式(3)所示非線性方程組的解。為了簡單起見，本文使用G2o[19]求解該問題。

3.2 地圖更新

機器人進行定位時，假設已經具備環境的靜態地圖。當環境中不存在半靜態物體，即環境沒有發生變化時，直接用靜態地圖進行匹配即可以實現機器人的穩定定位。當環境發生變化時，靜態地圖與實際環境不匹配，會導致定位失敗。因此本文用激光雷達數據和地圖匹配是否失敗作為判斷環境是否發生變化的指示。如果當前數據和地圖匹配失敗，則說明環境發生變化，因此本文把匹配失敗作為觸發地圖更新的條件。本文地圖更新的流程如圖5所示。

圖5 地圖更新流程圖

從圖5中可以看出，當環境沒有發生變化時，用當前數據跟靜態地圖進行匹配得到位姿。當環境發生變化時，會觸發匹配失敗，此時位姿只能依賴激光雷達數據的幀間匹配，而無法被地圖修正。地圖匹配失敗說明環境已經改變，靜態地圖跟真實環境不再匹配，此時需要用當前的觀測數據對地圖進行更新，因此定位系統會緩存匹配失敗的數據，形成一個單鏈的位姿圖。當機器人重新進入已知區域時，回環檢測成功，形成一個全局的回環約束，然后通過位姿圖優化來修正機器人的軌跡，最后進行地圖的融合。

在位姿圖中，位姿誤差不斷累積，因此位姿圖中除了首尾節點之外，其他節點位姿都存在累積誤差。位姿圖優化就是為了消除幀間匹配的累積誤差，位姿圖優化的結果如圖6所示。

圖6 位姿圖矯正前后的誤差

從圖6中可以看出，機器人在一個辦公室場景中運行，該環境的靜態地圖被擦除一部分，因此機器人行走在被擦除部分對應的環境時，地圖匹配會失敗，地圖匹配失敗的軌跡為圖6中藍色的軌跡。藍色軌跡的位姿誤差是逐步累積的，最后一個節點的累積誤差最大。當機器人進入已知區域后，回環檢測程序會讓其最后一個節點和地圖匹配成功，最后一個位姿得到修正并且形成回環，圖6中黑色的邊即為回環約束。最后進行位姿圖優化，整條軌跡的誤差都得到了修正，綠色軌跡即為修正之后的軌跡。

由于位姿圖第一個節點位姿和最后一個節點位姿都是跟靜態地圖匹配得到，因此優化后的位姿圖坐標系跟靜態地圖坐標系是對齊的。本文把位姿圖理解成一個由半靜態物體構成的局部子圖，靜態地圖則表示全局地圖。地圖更新的過程即把局部子圖與全局地圖進行融合的過程。為了最大限度地減少地圖融合過程中出現歧義的情況，本文把融合分為清除和增加兩個部分，即先清除那些地圖中不存在的物體，然后再增加新出現的物體。

首先利用位姿圖的位姿和激光數據，按照覆蓋柵格建圖算法生成一個柵格地圖，該地圖標識了局部子圖中的空白區域和占用區域。對于全局地圖中的占用區域，如果它位于局部子圖中的空白區域中，則說明該物體在全局地圖中是存在的，但是并不存在于局部子圖中，其對應的情況是環境中的物體被移走。通過這一步驟可以清除全局地圖中那些已經不存在的物體。清除之后，把局部子圖中的物體加入到全局地圖中，為了防止由于定位誤差導致的融合地圖出現歧義的情況，如果局部子圖中的物體離全局地圖中的物體小于一個閾值，則認為該物體是由于定位誤差導致，并不是真實發生的改變，因此該物體直接忽略，不加入到全局地圖中。

本文的地圖更新方法首先利用位姿圖對機器人位姿進行優化，實現兩個坐標系的對齊，然后生成覆蓋柵格地圖，直接在覆蓋柵格地圖層面進行融合。位姿圖優化的復雜度跟節點的數量成正比，而實際環境中，環境都是緩慢變化的，不會發生大規模的突然變化，因此位姿圖中節點的個數一般小于100個。對于這個數量級的位姿圖優化，在奔騰處理器上的時間小于10 ms。已知位姿進行覆蓋柵格地圖生成基本上小于5 ms。由于局部子圖和全局地圖的坐標系對齊，因此進行柵格地圖融合只需要遍歷一遍局部子圖即可，在柵格地圖的分辨率為5 cm的情況下，柵格地圖融合的耗時在10～20 ms之間。因此本文的地圖更新方法總耗時在25～35 ms之間，完全能滿足實際運行的需要。同時地圖更新之后，存儲的是新的柵格地圖而不是激光的原始數據，內存的消耗僅僅是一張圖片的大小，不會超過10 Mb。因此本算法適用于計算資源和存儲資源都比較受限制的商用機器人。

4 實 驗

為了驗證本文提出的定位算法的有效性，本文用C++實現了全部的代碼，并且把本文的定位模塊和建圖模塊、規劃模塊以及控制模塊進行整合，形成一個完整的導航系統，對本文提出的定位算法在動態環境中的定位性能進行了充分測試。測試平臺為成都越凡創新公司的全自動售貨機器人FANBOT的原型機，如圖7所示。

圖7所示的原型機是一個差分驅動的輪式機器人，該機器人裝備有多種傳感器，包括RGBD傳感器、雙目相機和2D激光雷達等，本文只使用2D激光雷達數據。該原型機的處理器為一個奔騰四核處理器。

圖7 FANBOT原型機

4.1 高動態物體測試

高動態物體測試場景為機器人在環境中行走，同時有行人在機器人前方來回走動，既有遠離機器人的運動，也有靠近機器人的運動。在不進行動態物體濾除的情況下，機器人生成的點云地圖如圖8所示。

圖8 無動態物體濾除

從圖8可以看出，行人對機器人生成的點云地圖產生了嚴重的干擾，點云地圖中包含大量動態物體造成的數據，讓點云地圖變得比較雜亂無章，會嚴重影響機器人的定位性能。經過本文動態物體濾除算法過濾后的點云地圖如圖9所示。

從圖9可以看出，本文的高動態物體去除方法基本上過濾掉了動態物體，對生成的點云地圖質量有極大的提高。值得注意的是，除了過濾掉行人之外，有一些不屬于行人的激光點也被過濾掉了，這是因為FANBOT使用低成本激光雷達數據質量較差，正常數據中也會夾雜很多的數據噪點，因此被動態物體濾除算法當成動態物體一并過濾了。從圖8和圖9的對比可知，本文方法能成功地過濾掉高動態物體，極大地提高點云質量。

圖9 動態物體濾除

4.2 地圖更新測試

本文在一個辦公室場景進行半靜態物體的測試。首先構建該辦公室場景的靜態地圖，然后人為移動環境中的物體來模擬環境變化，讓機器人在變化后的環境中進行定位。為了讓對比更加明顯，特地使用點云地圖來進行顯示。構建的靜態地圖如圖10所示。

圖10 靜態地圖

為讓機器人在變化后的環境中運行，需定位系統能夠構建出環境變動的部分來更新地圖。讓機器人在辦公室中走5圈，最終構建出的地圖和靜態地圖的對比圖如圖11所示。

圖11中紅色點云為原始點云，與圖10是重合的；綠色點云為定位系統構建出來的點云。從圖11可以看出，在環境發生巨大變化的情況下，定位系統依然可以進行穩定的定位，并且自動把環境中變化的部分更新到地圖中。證明了該算法在環境發生改變之后，能自適應的更新地圖，讓地圖始終跟當前環境保持一致。因此可以處理動態環境中的低動態物體，讓機器人實現長時間穩定的定位。

圖11 對比圖

4.3 長時間定位測試

為了驗證本論文定位系統的長時間定位性能，本文讓測試樣機在圖12a所示的環境中進行長達一個月的不間斷運行，機器人運動的過程中環境在不斷的發生變化，在運行一個月之后，機器人的地圖更新為圖12b所示。

從圖12a和圖12b的對比中可以看出，盡管在機器人的運行過程中，環境發生了巨大改變，本文的定位方法依然可以實現機器人的穩定定位，同時可以重建出環境的變化。該實驗表明本文的定位算法對于動態環境的適應性，能在動態環境中實現長時間穩定的定位。

圖12 機器人運動的地圖

4.4 機器人定位可靠性分析

經過在商場環境下長時間的運行，發現對于機器人定位穩定性影響最大的3個因素為機器人輪胎磨損、機器人被人推動和激光雷達傳感器感知能力限制。

機器人輪胎直徑在出廠時會進行標定。然而隨著機器人運行時間的增長，輪胎的磨損加劇，導致機器人輪胎半徑發生變化，和出廠時的誤差越來越大，導致機器人的里程計精度越來越差。而里程計精度對于機器人的定位系統來說是一個重要的傳感器，其不但影響機器人在環境變化時的定位精度，也會影響用位姿圖優化時初始解的準確度，如果初始解太差，可能會導致地圖更新出錯。因此防止機器人輪胎磨損，或者在機器人輪胎磨損之后能自動標定輪胎直徑的功能對于機器人定位穩定性具有重要的意義。

機器人被人推動，又叫機器人綁架，是實際運行中影響機器人定位穩定性的另一個重要因素。由于行人對于機器人的好奇，經常會有人把機器人從一個地方推動到另外一個地方，導致機器人定位失敗。同時激光雷達數據信息量不夠豐富，導致機器人不能快速地進行全局定位，所以這時需要人為地介入才能解決這個問題。因此如何將激光雷達數據和信息量豐富的傳感器數據，比如視覺傳感器，融合起來解決機器人綁架問題也值得研究。

最后一個因素是激光雷達屬于光學傳感器，因此它看不到透明玻璃。然而商場環境中具有大量的透明玻璃，因此在商場中總是會出現激光雷達檢測不到物體的情況，此時相當于純靠機器人的里程計進行航跡推算，從而導致機器人定位漂移。因此融合多個傳感器數據，讓至少有一個傳感器能觀測到數據，對于機器人定位的穩定性具有積極影響。

5 結束語

本文提出了一種移動機器人的定位算法，該算法能同時處理環境中的高動態物體和半靜態物體，實現機器人在動態環境中長時間的穩定定位。本文利用定位系統不需要使用最新數據的特點，提出能同時處理遠離機器人和靠近機器人這兩種動態物體的延遲對比法。同時提出一個結合位姿圖優化和柵格地圖覆蓋的方法，實驗證明該方法能很好地處理環境中的半靜態物體，讓機器人的地圖保持跟當前環境的一致性。同時總結了實際運行過程中影響機器人定位可靠性的三大因素，后續工作主要是對這3個因素進行進一步的優化。