激光雷達與深度相機融合的SLAM技術(shù)研究

張恒， 徐萬紅， 張禹

（沈陽工業(yè)大學(xué) 機械工程學(xué)院，沈陽110870）

0 引 言

近年來，國內(nèi)外對無人車技術(shù)的研究取得了極大的進展，在很多結(jié)構(gòu)化環(huán)境或人員稀少的環(huán)境中均已開始投入使用。無人車的結(jié)構(gòu)體系大體上可分為環(huán)境感知、定位導(dǎo)航、規(guī)劃決策和車輛控制4個模塊[1]。其中定位導(dǎo)航模塊中的SLAM（Simultaneous Localization and Mapping,SLAM）技術(shù)采用邊定位邊建圖的方法，即同時定位與建圖，它是指搭載特定傳感器的主體，在沒有環(huán)境先驗信息的條件下，在運動過程中建立環(huán)境地圖，利用所創(chuàng)建的局部環(huán)境地圖來估計自身的位姿[2]。

目前，按搭載傳感器的方式分類主要可分為搭載激光雷達的激光SLAM和搭載相機的視覺SLAM[3]。在實際使用情況中，激光雷達和相機等單一的傳感器都存在一定的局限性。激光雷達精度高，但多線激光雷達價格昂貴，極大地增加了產(chǎn)品成本；單線激光雷達的掃描范圍僅為一個水平面，難以在復(fù)雜環(huán)境中單獨使用；視覺SLAM在無紋理或光照弱的環(huán)境中效果較差。針對上述問題，本文提出了一種將單線激光雷達與視覺相機相融合的解決方案。通過將多個傳感器采集的信息進行互補、豐富數(shù)據(jù)，能有效提高定位精度和建圖的觀測區(qū)域。

1 傳感器介紹

本文中采用Hokuyo URG-04LX-UG01 2D激光雷達，如圖1所示，具有精度高、分辨率高、工作不受光線影響等優(yōu)點，其具體參數(shù)如表1所示?？捎糜跈C器人自主定位與導(dǎo)航、路徑規(guī)劃與避障等。

圖1 2D 激光雷達

表1 2D激光雷達參數(shù)表

Kinect V1是微軟公司于2010年6月推出的一款RGB-D視覺傳感器，如圖2所示，中間位置是可獲取彩色信息的RGB 攝像頭，左右兩側(cè)攝像頭分別紅外發(fā)射器與接收器，用來獲取與彩色信息對應(yīng)的深度信息。Kinect掃描速度快，并可以同時獲得一定范圍內(nèi)的3D 信息，信息量豐富，具體參數(shù)如表2所示。

圖2 Kinect V1

表2 Kinect參數(shù)表

激光雷達有較高的精度但仍存在一定噪聲，激光雷達常用數(shù)學(xué)模型有光束模型和似然場模型。

光束模型假設(shè)每幀激光xi是由k個激光點構(gòu)成xi=（x1,x2, ……,xk）; 每個激光點在擊中物體過程中會受到4種干擾，如圖3所示。

圖3 光束模型

光束模型具有直觀的物理意義，但求解每個激光點的期望值均需進行一次光線跟蹤，一幀激光則需要計算k次，計算量較大；傳感器在非結(jié)構(gòu)化的環(huán)境中輕微的運動也會導(dǎo)致期望值發(fā)生較大變化，嚴重影響觀測結(jié)果。所以采用文獻[4]中提到的似然場模型對激光束進行改進：

式中:phit為測量誤差；prand為隨機誤差；pmax為最大測量距離；zhit、zrand、zmax分別為對應(yīng)的權(quán)重，似然場模型將圖像高斯平滑后使其獲得相對于位姿更加平滑的期望值，可用于結(jié)構(gòu)或非結(jié)構(gòu)化環(huán)境且計算量低。

本文采用針孔相機模型對將現(xiàn)實世界中的三維坐標(biāo)點映射到二維平面的過程進行描述，如圖4所示。

圖4中O-x-y-z為相機坐標(biāo)系，O為攝像機的光心。實際環(huán)境中一點P，經(jīng)過O投影之后，落在成像平面O′-x′-y′-z′上，成像點為P′。P點坐標(biāo)為[X,Y,Z]T,P′點坐標(biāo)為[X′,Y′,Z′]T,根據(jù)三角形相似并化簡后得：

圖4 針孔相機模型[5]

儲存圖象信息時使用原點位于物理成像平面左上角上計算機圖像坐標(biāo)系O-u-v。將針孔模型坐標(biāo)系中P′=[X′,Y′,Z′]T轉(zhuǎn)換為計算機圖像坐標(biāo)系P′=[u,v]T得：

式中：α、β分別表示在u軸上放縮了α倍、在v軸放縮了β倍；cx、cy表示原點平移距離。

將αf合并為fx、將βf合并為fy，對上式化簡后寫為矩陣形式得：

其中，K即為相機的內(nèi)參矩陣，可通過K實現(xiàn)相機標(biāo)定。

2 多傳感器數(shù)據(jù)融合

2.1 點云匹配方法

SLAM在進行前端匹配時常用迭代最近點（Iterative Closest Point，ICP）算法來進行幀間匹配[6]，ICP方法是通過在不同位姿下觀察同一物體，得到同一物體的兩個不同點云即目標(biāo)點云和源點云，按照一定約束條件找到最鄰近點（pi，qi）,然后計算出旋轉(zhuǎn)矩陣R和平移矩陣t，使得誤差函數(shù)最小，如圖5所示。誤差函數(shù)E（R，t）為

式中：n為最鄰近點對的個數(shù)；pi為目標(biāo)點云P 中的一點；qi為源點云Q 中與pi對應(yīng)的最近點；R為旋轉(zhuǎn)矩陣；t為平移向量。

由于傳統(tǒng)的ICP算法進行匹配時采用點到點的距離作為誤差方程往往會造成較大的隨機誤差，本文采用PLICP算法進行匹配[7]，PL-ICP采用點到線的距離作為誤差方程，如圖5所示，誤差方程為

其中，ni表示曲面對應(yīng)點的法向量。

PL-ICP算法更加符合實際情況，降低了ICP算法中的誤差，提高了匹配精度。

2.2 多傳感器數(shù)據(jù)融合方法

單一傳感器具有一定局限性，將兩種傳感器數(shù)據(jù)融合來保證測量精度并增加測量范圍。根據(jù)融合階段可將數(shù)據(jù)融合分為像素級融合、特征級融合和決策級融合[8]。

圖5 ICP 與PL-ICP 誤差函數(shù)區(qū)別

卡爾曼濾波是一種遞歸的估計，它的估計過程只與上一個相關(guān)狀態(tài)的估計值和當(dāng)前狀態(tài)的實際值有關(guān)?？柭鼮V波數(shù)據(jù)融合算法信息丟失較少，所以卡爾曼濾波常用來對原始數(shù)據(jù)進行融合[9]。

式（9）～式（13）是卡爾曼濾波的核心內(nèi)容。式（9）中：X（k|k-1）是利用上一狀態(tài)預(yù)測的結(jié)果；X（k-1|k-1）是上一狀態(tài)最優(yōu)的結(jié)果；U（k）為現(xiàn)在狀態(tài)的控制量。式（10）中：P（k|k-1）是X（k|k-1）對應(yīng)的協(xié)方差；P（k-1|k-1）是X（k-1|k-1）對應(yīng)的協(xié)方差；A′表示A的轉(zhuǎn)置矩陣，Q是系統(tǒng)過程的協(xié)方差；現(xiàn)在狀態(tài)（k）的最優(yōu)化估算值為X（k|k）；Kg為卡爾曼增益（Kalman Gain）。

使用卡爾曼濾波進行數(shù)據(jù)處理前應(yīng)先設(shè)定好過程噪聲Q與協(xié)方差矩陣R。式中A為狀態(tài)矩陣，因為是一階，所以取A=1，得:

將貝葉斯估計用于融合多傳感器的柵格地圖時，可得到改進的融合公式：

為增加數(shù)據(jù)可靠性、豐富地圖細節(jié)、提高數(shù)據(jù)準(zhǔn)確性，本文在數(shù)據(jù)級采用卡爾曼濾波融合激光數(shù)據(jù)與深度數(shù)據(jù)，在決策級采用貝葉斯估計融合激光雷達與深度數(shù)據(jù)生成的局部二維柵格地圖，融合流程如圖6所示。

圖6 激光雷達與Kinect數(shù)據(jù)融合流程圖

3 實 驗

本文中實驗硬件設(shè)備如圖7所示，采用Turtlebot2作為移動平臺，搭載Hokuyo URG-04LX-UG01 激光雷達、Microsoft Kinect V1 傳感器、聯(lián)想拯救者Y-7000筆記本電腦。軟件平臺為Ubuntu16.04上安裝的ROS Kinetic操作系統(tǒng)。

圖7 turtlebot2移動機器人

使用Kinect對環(huán)境進行掃描建圖可以獲得更豐富的地圖信息，能夠?qū)⑤^小的障礙物也在柵格地圖中體現(xiàn)出來，但柵格地圖精度較差，如圖8所示。

圖8 Kinect數(shù)據(jù)柵格地圖

僅使用激光雷達作為傳感器對環(huán)境進行掃描建圖，如圖9所示，使用激光雷達所建地圖精度較高，但激光雷達掃描平面以外的信息沒有被采集到，對后續(xù)導(dǎo)航工作有一定的影響。

圖9 激光數(shù)據(jù)柵格地圖

將兩種傳感器數(shù)據(jù)在數(shù)據(jù)級與決策級進行兩次融合后生成柵格地圖，如圖10所示。

圖10 激光和Kinect數(shù)據(jù)融合柵格地圖

可以看出，數(shù)據(jù)融合后生成的地圖既保留了激光雷達建圖精度高的特點，同時也通過三維投影采集到多個水平面的環(huán)境信息，得到信息更加豐富的柵格環(huán)境地圖，降低了機器人與其他障礙物發(fā)生碰撞的風(fēng)險。為后續(xù)的導(dǎo)航、避障工作提供了更加準(zhǔn)確的環(huán)境地圖。

4 結(jié) 語

為彌補2D激光雷達或Kinect單一傳感器進行SLAM存在的缺陷，本文采用將激光雷達數(shù)據(jù)與Kinect數(shù)據(jù)進行兩次融合的方法構(gòu)建環(huán)境地圖。在數(shù)據(jù)級使用卡爾曼濾波融合激光數(shù)據(jù)與Kinect的深度數(shù)據(jù)，在決策級使用貝葉斯估計融合柵格地圖。經(jīng)過兩次融合后得到更加接近真實環(huán)境的柵格地圖，可使移動機器人在復(fù)雜環(huán)境中更有效地工作，同時還提高了地圖精度，也為后續(xù)的定位、導(dǎo)航、避障工作提供了更完善的地圖支持。進一步的研究方向可進行多機的SLAM柵格地圖融合，以便在廣闊的環(huán)境中提高建圖效率。