黃 山，黃洪鐘*，曾 奇，錢(qián)華明

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

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

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

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

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

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

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

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

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

1 系統(tǒng)框架

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

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

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

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

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

2.1 延遲對(duì)比法

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

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

圖2 對(duì)比法工作原理圖

相對(duì)于機(jī)器人來(lái)說(shuō)，動(dòng)態(tài)物體分為向機(jī)器人靠近和遠(yuǎn)離機(jī)器人兩種。從對(duì)比法的原理來(lái)說(shuō)，只能處理向機(jī)器人靠近的動(dòng)態(tài)物體，因?yàn)槿绻麆?dòng)態(tài)物體正在遠(yuǎn)離機(jī)器人，那么該動(dòng)態(tài)物體并不在上一幀數(shù)據(jù)的視野范圍，所以對(duì)比法無(wú)法檢測(cè)出遠(yuǎn)離機(jī)器人的動(dòng)態(tài)物體。

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

圖3 延遲對(duì)比法工作原理圖

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

2.2 跟蹤法

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

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

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

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

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

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

3.1 位姿圖優(yōu)化

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

圖4 位姿示意圖

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

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

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

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

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

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

3.2 地圖更新

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

圖5 地圖更新流程圖

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

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

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

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

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

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

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

4 實(shí) 驗(yàn)

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

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

圖7 FANBOT原型機(jī)

4.1 高動(dòng)態(tài)物體測(cè)試

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

圖8 無(wú)動(dòng)態(tài)物體濾除

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

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

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

4.2 地圖更新測(cè)試

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

圖10 靜態(tài)地圖

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

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

圖11 對(duì)比圖

4.3 長(zhǎng)時(shí)間定位測(cè)試

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

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

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

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

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

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

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

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

5 結(jié)束語(yǔ)

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