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

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

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

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

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

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

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

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

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

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

1 系統(tǒng)框架

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

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

圖1 整體系統(tǒng)框架

2 高動態(tài)物體的處理

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

2.1 延遲對比法

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

圖2中，坐標(biāo)系表示機器人當(dāng)前的位置，黃色點表示上一幀激光數(shù)據(jù)，洋紅色的多邊形表示上一幀激光數(shù)據(jù)對應(yīng)的視野范圍，藍色點表示當(dāng)前幀激光數(shù)據(jù)，紅色點表示當(dāng)前幀中被檢測為動態(tài)物體的數(shù)據(jù)，該數(shù)據(jù)實際上是一個運動的人，紅色點位于上一幀的視野范圍之內(nèi)，因此其被認為是動態(tài)物體。

圖2 對比法工作原理圖

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

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

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

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

2.2 跟蹤法

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

2.3 延遲對比法和跟蹤法的結(jié)合

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

為了克服彼此的缺點，本文將延遲對比法和跟蹤法兩種方法結(jié)合起來，對于每一個延遲對比法檢測出來的動態(tài)障礙物都分配一個濾波器進行跟蹤，并且在不同幀之間進行數(shù)據(jù)關(guān)聯(lián)，實現(xiàn)動態(tài)障礙物的實時檢測和穩(wěn)定跟蹤的效果。

3 半靜態(tài)物體的處理

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

3.1 位姿圖優(yōu)化

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

圖4 位姿示意圖

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

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

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

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

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

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

3.2 地圖更新

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

圖5 地圖更新流程圖

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

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

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

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

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

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

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

4 實 驗

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

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

圖7 FANBOT原型機

4.1 高動態(tài)物體測試

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

圖8 無動態(tài)物體濾除

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

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

圖9 動態(tài)物體濾除

4.2 地圖更新測試

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

圖10 靜態(tài)地圖

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

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

圖11 對比圖

4.3 長時間定位測試

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

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

圖12 機器人運動的地圖

4.4 機器人定位可靠性分析

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

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

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

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

5 結(jié)束語

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