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

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

1 引言

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

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

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

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

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

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

2 基于多幀局部地圖的配準方法

2.1 PL-ICP算法

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

圖1 PL-ICP示意圖

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

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

其中有

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

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

2）對于當前幀的每一個點Tkpi，在X中找到最近的兩個點xj1i，xj2i，并計算法向量ni；

3）計算Tkpi到直線的距離，去除距離過大的點；

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

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

2.2 局部地圖匹配

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

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

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

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

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

圖4 單條光線TSDF值示意圖

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

圖6 實驗使用的機器人仿真模型

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

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

也就是說，每一個局部地圖中都會包含20 幀點云。其中前10 幀將會與上一個局部地圖進行匹配，同時被添加到上一個局部地圖中；而后10 幀則會與當前局部地圖進行匹配，這10 幀同時也是下一個局部地圖的前10幀。

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

3 基于TSDF的建圖方法

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

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

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

而tsdf（x）的定義為

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

一般來說，構(gòu)建一個TSDF 場需要綜合多次激光觀測，最后每個柵格的TSDF值如下所示：

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

4 實驗與分析

4.1 實驗環(huán)境搭建

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

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

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

4.2 實驗結(jié)果與分析

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

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

表1 兩種算法定位精度對比

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

5 結(jié)語

本文提出了一種基于局部地圖匹配的機器人定位建圖方法，對于普通的幀間匹配方法來說，由于匹配時使用的信息過少從而導(dǎo)致匹配的誤差較大，而使用局部地圖能顯著提升匹配時用到的信息量，從而顯著提升定位與建圖的精度。為了驗證該方法本文基于ROS進行了仿真實驗，并在實驗中證明了其有效性。實驗表明，通過將普通幀間匹配替換成地圖匹配，能顯著減小定位算法的定位的誤差，同時增強所建立地圖的全局一致性。

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