一種室內(nèi)高動態(tài)環(huán)境的機(jī)器人定位方法

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

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

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

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

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

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

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

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

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

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

1 系統(tǒng)框架

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

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

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

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

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

2.1 延遲對比法

對比法是用當(dāng)前激光幀數(shù)據(jù)z(t)和上一幀z(t-1)或者上幾幀激光數(shù)據(jù)z(t-k)進(jìn)行對比，如果當(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)系表示機(jī)器人當(dāng)前的位置，黃色點(diǎn)表示上一幀激光數(shù)據(jù)，洋紅色的多邊形表示上一幀激光數(shù)據(jù)對應(yīng)的視野范圍，藍(lán)色點(diǎn)表示當(dāng)前幀激光數(shù)據(jù)，紅色點(diǎn)表示當(dāng)前幀中被檢測為動態(tài)物體的數(shù)據(jù)，該數(shù)據(jù)實(shí)際上是一個運(yùn)動的人，紅色點(diǎn)位于上一幀的視野范圍之內(nèi)，因此其被認(rèn)為是動態(tài)物體。

圖2 對比法工作原理圖

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

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

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

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

2.2 跟蹤法

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

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

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

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

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

跟高動態(tài)物體不同，半靜態(tài)物體能提供豐富的定位信息，因此不能直接視為噪聲進(jìn)行濾除，而是需要充分利用半靜態(tài)物體提供的信息來實(shí)現(xiàn)機(jī)器人的長時間穩(wěn)定定位[14]。本文在進(jìn)行半靜態(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)化

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

圖4 位姿示意圖

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

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

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

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

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

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

3.2 地圖更新

機(jī)器人進(jìn)行定位時，假設(shè)已經(jīng)具備環(huán)境的靜態(tài)地圖。當(dāng)環(huán)境中不存在半靜態(tài)物體，即環(huán)境沒有發(fā)生變化時，直接用靜態(tài)地圖進(jìn)行匹配即可以實(shí)現(xiàn)機(jī)器人的穩(wěn)定定位。當(dāng)環(huán)境發(fā)生變化時，靜態(tài)地圖與實(shí)際環(huán)境不匹配，會導(dǎo)致定位失敗。因此本文用激光雷達(dá)數(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)地圖進(jìn)行匹配得到位姿。當(dāng)環(huán)境發(fā)生變化時，會觸發(fā)匹配失敗，此時位姿只能依賴激光雷達(dá)數(shù)據(jù)的幀間匹配，而無法被地圖修正。地圖匹配失敗說明環(huán)境已經(jīng)改變，靜態(tài)地圖跟真實(shí)環(huán)境不再匹配，此時需要用當(dāng)前的觀測數(shù)據(jù)對地圖進(jìn)行更新，因此定位系統(tǒng)會緩存匹配失敗的數(shù)據(jù)，形成一個單鏈的位姿圖。當(dāng)機(jī)器人重新進(jìn)入已知區(qū)域時，回環(huán)檢測成功，形成一個全局的回環(huán)約束，然后通過位姿圖優(yōu)化來修正機(jī)器人的軌跡，最后進(jìn)行地圖的融合。

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

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

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

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

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

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

4 實(shí) 驗

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

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

圖7 FANBOT原型機(jī)

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

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

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

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

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

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

4.2 地圖更新測試

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

圖10 靜態(tài)地圖

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

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

圖11 對比圖

4.3 長時間定位測試

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

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

圖12 機(jī)器人運(yùn)動的地圖

4.4 機(jī)器人定位可靠性分析

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

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

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

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

5 結(jié)束語

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