基于多幀局部地圖的室內(nèi)TSDF 建圖算法*

曹一波 張智輝 趙鵬飛 朱海文

（華南師范大學(xué)軟件學(xué)院 佛山 538200）

1 引言

隨著機(jī)器人工業(yè)的快速發(fā)展，越來(lái)越多的移動(dòng)機(jī)器人走入人們的生活，目前比較常見(jiàn)的包括家用的掃地機(jī)器人，餐廳的送餐機(jī)器人等。不管是哪一種移動(dòng)機(jī)器人，它們都有避免碰撞-感知周圍障礙物，同時(shí)利用障礙物地圖來(lái)規(guī)劃自己運(yùn)動(dòng)的需求。相關(guān)的問(wèn)題最早在20 世紀(jì)80 年代被提出，要求機(jī)器僅利用自身攜帶的傳感器，可以在陌生環(huán)境中構(gòu)建出環(huán)境地圖，同時(shí)確定自己在地圖中的位置。這個(gè)問(wèn)題也被叫做同時(shí)定位與建圖（Simultaneous Localization and Mapping，SLAM）［1］。

由于SLAM 要求建模周圍的障礙物信息，所以機(jī)器人上面必須要裝備一個(gè)可以獲取深度的傳感器，常用的傳感器包括超聲波、激光雷達(dá)、相機(jī)、RGBD相機(jī)等［2］。激光雷達(dá)是目前在室內(nèi)使用的比較廣泛的傳感器，其特點(diǎn)為水平視場(chǎng)角大、能夠直接獲取到精確的測(cè)距數(shù)據(jù)。通過(guò)快速旋轉(zhuǎn)的激光雷達(dá)，我們可以獲取到一幀表征周圍障礙物的點(diǎn)云，而為了獲取到相對(duì)較大范圍內(nèi)的環(huán)境地圖，我們必須將這些點(diǎn)云正確地對(duì)齊到一起。因此，基于激光雷達(dá)的SLAM 問(wèn)題的核心是點(diǎn)云對(duì)準(zhǔn)的問(wèn)題［3］。

最早的對(duì)準(zhǔn)方法是基于概率的，這些方法通過(guò)擴(kuò)展卡爾曼濾波（Extended Kalman Filter，EKF）或粒子濾波（Rao-Blackwellized Particle Filter，RBPF）［4］計(jì)算得到最有可能的機(jī)器人位姿，然后基于位姿來(lái)對(duì)點(diǎn)云進(jìn)行添加。卡爾曼濾波是較早的基于概率的方法，由于它只能模擬高斯分布，不能很好地表示位姿的概率分布，因此后續(xù)被表現(xiàn)更好的粒子濾波替代。粒子濾波用大量的粒子來(lái)表示機(jī)器人位姿分布，由于粒子的靈活性，粒子濾波可以模擬任意形狀的概率分布，且模擬的準(zhǔn)確度隨粒子數(shù)量的增加而增加。在粒子濾波的使用過(guò)程中，人們發(fā)現(xiàn)了一個(gè)很直覺(jué)的現(xiàn)象：隨著傳感器精度的上升，機(jī)器人定位的精度反而下降甚至出現(xiàn)定位丟失，這就是所謂的粒子耗散問(wèn)題［5］。

為了避免粒子耗散問(wèn)題，人們開(kāi)始使用非概率的純匹配方法，最著名的純匹配方法是用在谷歌開(kāi)源算法Cartographer［6］中的CSM 加非線性優(yōu)化的組合，其中CSM 使用暴力搜索來(lái)實(shí)現(xiàn)粗配準(zhǔn)，非線性優(yōu)化則用于實(shí)現(xiàn)精確配準(zhǔn)。另一種被廣泛使用的非概率配準(zhǔn)方法則是ICP（迭代最近點(diǎn)）［7］。ICP 是點(diǎn)云處理的常見(jiàn)算法，目的是為了解決兩組形狀相似、位置上有所偏差的點(diǎn)云的配準(zhǔn)問(wèn)題。

ICP 能以較高的精度配準(zhǔn)激光雷達(dá)先后獲取到兩幀點(diǎn)云，這正好是基于激光雷達(dá)的SLAM 所需要的［8］。PL-ICP 是ICP 的優(yōu)化版本［9］，不同于ICP使用點(diǎn)與點(diǎn)的距離作為優(yōu)化函數(shù)，PL-ICP 使用點(diǎn)線距離作為優(yōu)化函數(shù)，這不僅提高了它的精度，還讓它有比普通ICP 更快的收斂速度。但是不管是ICP還是PL-ICP，它們都有一個(gè)很嚴(yán)重的缺點(diǎn)。由于ICP 一般是直接配準(zhǔn)相鄰的兩幀，新的一幀位置僅僅受上一幀的位置所影響，這就導(dǎo)致誤差很容易會(huì)隨著匹配幀數(shù)量的增加而線性增長(zhǎng)。機(jī)器人靜止的時(shí)候使用ICP 匹配，過(guò)一陣子發(fā)現(xiàn)機(jī)器人的位置已經(jīng)發(fā)生了偏移，這便是這種情況的真實(shí)寫(xiě)照。更糟糕的是，由于無(wú)法使用歷史幀來(lái)進(jìn)行糾正，如果某一幀的匹配效果比較差，那么后續(xù)所有幀的位置都會(huì)收到影響，從而嚴(yán)重降低最后得到地圖的效果［10］。

為了增加ICP 匹配的信息量，本文章沒(méi)有采用上一幀作為參考幀，而是選擇創(chuàng)建局部地圖，并以局部地圖作為參考幀。這樣做的一個(gè)顯著好處是利用之前多幀點(diǎn)云的融合信息，有效提升了新一幀配準(zhǔn)結(jié)果的全局一致性。對(duì)于普通的占用柵格地圖來(lái)說(shuō)，由于其中的障礙物邊界比較模糊，使得我們將其用于上層的導(dǎo)航模塊比較困難。為了解決這個(gè)問(wèn)題，本篇文章引入TSDF 建圖方法［11］，TSDF借用插值方法可以精確地恢復(fù)出物體表面相關(guān)信息，從而得到更加準(zhǔn)確的環(huán)境地圖。

2 基于多幀局部地圖的配準(zhǔn)方法

2.1 PL-ICP算法

ICP 是常見(jiàn)的點(diǎn)云匹配算法，其核心思路在于找到兩組點(diǎn)云中的匹配點(diǎn)，通過(guò)匹配點(diǎn)的間的距離差建立誤差函數(shù)，最后通過(guò)最小化誤差函數(shù)得到原始點(diǎn)云到目標(biāo)點(diǎn)云的變換矩陣。與傳統(tǒng)ICP 算法不同，PL-ICP 采用點(diǎn)到線得距離作為誤差量。這種誤差形式更加符合實(shí)際情況，使得PL-ICP 能夠達(dá)到比普通ICP 更高的精度與更快的收斂速度。但是PL-ICP 相較于ICP 對(duì)于初值更加敏感，一般需要配合里程計(jì)一起使用［7］。

圖1 PL-ICP示意圖

考慮前后兩幀相鄰的點(diǎn)云：參考幀X與當(dāng)前幀P。我們將遍歷每一個(gè)當(dāng)前幀的點(diǎn)，并在參考幀中找到兩個(gè)距離其最近的點(diǎn)，用這兩個(gè)點(diǎn)建立一條直線，并將點(diǎn)線距離作為我們需要優(yōu)化的誤差量。

用pi表示點(diǎn)云P中的點(diǎn)，用xj1i，xj2i表示點(diǎn)云集合X中距離pi最近的兩個(gè)點(diǎn)，再由ni表示點(diǎn)xj1i，xj2i組成直線的法向量，則PL-ICP 的目標(biāo)函數(shù)可以描述如下：

其中有

PL-ICP的流程可以描述如下：

1）將當(dāng)前幀的數(shù)據(jù)pi根據(jù)當(dāng)前的位姿估計(jì)Tk進(jìn)行變換，得到新的坐標(biāo)表示Tkpi；

2）對(duì)于當(dāng)前幀的每一個(gè)點(diǎn)Tkpi，在X中找到最近的兩個(gè)點(diǎn)xj1i，xj2i，并計(jì)算法向量ni；

3）計(jì)算Tkpi到直線的距離，去除距離過(guò)大的點(diǎn)；

4）最小化誤差函數(shù)［7］，求得新的變換Tk+1；

5）若達(dá)到收斂條件或檢測(cè)到循環(huán)，將Tk+1作為結(jié)果輸出，否則跳轉(zhuǎn)到第一步。

2.2 局部地圖匹配

局部地圖區(qū)別與全局地圖，是由短時(shí)間內(nèi)獲取的一定幀組合而成的小范圍地圖。局部地圖對(duì)于定位和建圖任務(wù)來(lái)說(shuō)好處是明顯的，第一，作為匹配的參與者之一，它的規(guī)模不是很大，因此不用花費(fèi)過(guò)多的資源對(duì)其進(jìn)行處理；其次，局部地圖由于綜合了最近幀的所有信息，因此它的準(zhǔn)確度要明顯好于單獨(dú)使用某一幀進(jìn)行匹配。

在局部地圖的實(shí)現(xiàn)方面，本文使用重合的局部地圖策略，即某一幀會(huì)同時(shí)被兩個(gè)相鄰的局部地圖包含，分別出現(xiàn)在第一個(gè)局部地圖靠后部分和第二個(gè)局部地圖靠前部分。下面是三張部分重疊的局部地圖的示意圖。

圖2 重疊的局部地圖示意圖

局部地圖的創(chuàng)建流程如圖3。

圖3 局部地圖創(chuàng)建流程圖

圖4 單條光線TSDF值示意圖

圖5 TSDF場(chǎng)示意圖（左上柵格為障礙物，右下柵格為空閑區(qū)域，中間的線條為擬合的障礙物的表面）

圖6 實(shí)驗(yàn)使用的機(jī)器人仿真模型

圖7 實(shí)驗(yàn)使用的仿真環(huán)境

圖8 相同數(shù)據(jù)下兩個(gè)算法的建圖效果

也就是說(shuō)，每一個(gè)局部地圖中都會(huì)包含20 幀點(diǎn)云。其中前10 幀將會(huì)與上一個(gè)局部地圖進(jìn)行匹配，同時(shí)被添加到上一個(gè)局部地圖中；而后10 幀則會(huì)與當(dāng)前局部地圖進(jìn)行匹配，這10 幀同時(shí)也是下一個(gè)局部地圖的前10幀。

使用重疊的局部地圖策略一個(gè)顯著的好處便是出了剛開(kāi)始的10 幀以外，后續(xù)的每一幀都能與一個(gè)至少有10 幀數(shù)據(jù)的局部地圖進(jìn)行匹配，且這個(gè)匹配是增量式的，即匹配的結(jié)果會(huì)被添加到被匹配的局部地圖當(dāng)中。

3 基于TSDF的建圖方法

截?cái)嗑嚯x符號(hào)函數(shù)TSDF（Truncated Signed Distance Function）是物體表面重構(gòu)的經(jīng)典算法。其核心思想是基于柵格地圖建立一個(gè)符號(hào)距離函數(shù)場(chǎng)，某一個(gè)柵格的函數(shù)值的大小表示柵格與表面的距離［12］。由于障礙物表面內(nèi)外的距離值符號(hào)不同，因此構(gòu)建出函數(shù)場(chǎng)后我們可以由函數(shù)符號(hào)的跳轉(zhuǎn)恢復(fù)出曲面［13］。由于距離表面較遠(yuǎn)的點(diǎn)不用考慮，因此我們可以用1和-1約束距離符號(hào)函數(shù)的大小。

距離符號(hào)函數(shù)sdf（x）的定義：

其中l(wèi)aser（x）表示本次激光測(cè)量距離，dist（x）表示柵格離傳感器原點(diǎn)的距離。

而tsdf（x）的定義為

可以看到，tsdf（x）在sdf（x）的基礎(chǔ)上將值約束到了-1～1之間。

一般來(lái)說(shuō)，構(gòu)建一個(gè)TSDF 場(chǎng)需要綜合多次激光觀測(cè)，最后每個(gè)柵格的TSDF值如下所示：

其中wi(x)表示最新一次計(jì)算的權(quán)重，Wi-1(x)表示歷史的權(quán)重和。多次的觀測(cè)不斷按上式進(jìn)行融合，即可構(gòu)建出整個(gè)地圖的TSDF場(chǎng)，從地圖的TSDF場(chǎng)中可以重構(gòu)得到障礙物表面。

4 實(shí)驗(yàn)與分析

4.1 實(shí)驗(yàn)環(huán)境搭建

實(shí)驗(yàn)基于ROS（Robot Operating System）平臺(tái)進(jìn)行構(gòu)建，ROS是當(dāng)前機(jī)器人領(lǐng)域使用較多的開(kāi)源社區(qū)，里面包含硬件抽象、設(shè)備控制、進(jìn)程間通信等各種機(jī)器人編程常用到的功能［14］，從而為機(jī)器人研究開(kāi)發(fā)提供強(qiáng)力的代碼復(fù)用支持。本文中使用ROS中自帶的Gazebo 軟件進(jìn)行仿真，Gazebo 是一款3D動(dòng)態(tài)模擬器，通過(guò)其內(nèi)部自帶的物理引擎提供高保真度的環(huán)境仿真，同時(shí)提供了一整套傳感器模型，通過(guò)這些傳感器模型我們可以實(shí)現(xiàn)對(duì)現(xiàn)實(shí)世界中機(jī)器人的仿真［15］。

本文在Gazebo中創(chuàng)建了一個(gè)裝備了2D激光雷達(dá)和里程計(jì)的機(jī)器人。其中2D激光雷達(dá)的掃描范圍為360°，每一幀的點(diǎn)數(shù)為360 個(gè)，頻率為5.5Hz，最小范圍為0.2m，最大范圍為10m。為了模擬真實(shí)環(huán)境，激光雷達(dá)的測(cè)量距離被加上了N（0，0.02m）的高斯誤差。同樣的，為了讓里程計(jì)的表現(xiàn)更接近真實(shí)環(huán)境下的表現(xiàn)，每次里程計(jì)獲取的相對(duì)位姿都被加上了N（0，0.01m）的平移誤差和N（0，1deg）的旋轉(zhuǎn)誤差。

實(shí)驗(yàn)的運(yùn)行平臺(tái)配置為Intel Core i5 3320M +8G DDR3。仿真機(jī)器人所在的虛擬環(huán)境是一個(gè)約60m×45m 大小的多房間環(huán)境，通過(guò)控制機(jī)器人在此環(huán)境中行走，同時(shí)開(kāi)啟激光雷達(dá)與里程計(jì)，來(lái)獲得我們所需要的數(shù)據(jù)。實(shí)驗(yàn)所在的仿真環(huán)境如下。

4.2 實(shí)驗(yàn)結(jié)果與分析

為了證明本文算法的有效性，本文使用了普通的幀間匹配方法作為對(duì)比方法，在此方法中當(dāng)前幀只與上一幀進(jìn)行匹配。為了避免數(shù)據(jù)差異帶來(lái)的不確定性，兩個(gè)算法使用相同的數(shù)據(jù)集，這個(gè)數(shù)據(jù)集由仿真小車?yán)@仿真環(huán)境一圈收集得到。在相同數(shù)據(jù)下兩個(gè)算法的建圖效果如下。

可以看到兩者在建圖效果上有著明顯差異，為了進(jìn)一步比對(duì)兩者在定位精度上的差距，本文選取軌跡中的10 組共20 個(gè)點(diǎn)，并基于這10 組點(diǎn)的相對(duì)位置（包含測(cè)量值與真值）得到以下數(shù)據(jù)。

表1 兩種算法定位精度對(duì)比

本文使用了三個(gè)定位精度評(píng)價(jià)指標(biāo)，分別是平均誤差（Mean Error）、標(biāo)準(zhǔn)差（Standard Deviation）和均方根誤差（Root Mean Squared Error）。從這三個(gè)指標(biāo)的表現(xiàn)可以看出，本文中基于多幀局部地圖的匹配方法能夠有效提升機(jī)器人的定位精度。

5 結(jié)語(yǔ)

本文提出了一種基于局部地圖匹配的機(jī)器人定位建圖方法，對(duì)于普通的幀間匹配方法來(lái)說(shuō)，由于匹配時(shí)使用的信息過(guò)少?gòu)亩鴮?dǎo)致匹配的誤差較大，而使用局部地圖能顯著提升匹配時(shí)用到的信息量，從而顯著提升定位與建圖的精度。為了驗(yàn)證該方法本文基于ROS進(jìn)行了仿真實(shí)驗(yàn)，并在實(shí)驗(yàn)中證明了其有效性。實(shí)驗(yàn)表明，通過(guò)將普通幀間匹配替換成地圖匹配，能顯著減小定位算法的定位的誤差，同時(shí)增強(qiáng)所建立地圖的全局一致性。

本文的后續(xù)研究方向包括增加傳感器適用類型，例如IMU、深度相機(jī)等；減少算法的計(jì)算與內(nèi)存消耗，從而將算法用到更大的環(huán)境當(dāng)中；進(jìn)一步提升算法的定位精度與魯棒性；增加導(dǎo)航系統(tǒng)等。