張恒， 徐萬紅， 張禹

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

0 引 言

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

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

1 傳感器介紹

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

圖1 2D 激光雷達(dá)

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

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

圖2 Kinect V1

表2 Kinect參數(shù)表

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

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

圖3 光束模型

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

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

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

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

圖4 針孔相機(jī)模型[5]

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

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

將αf合并為fx、將βf合并為fy，對(duì)上式化簡(jiǎn)后寫為矩陣形式得：

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

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

2.1 點(diǎn)云匹配方法

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

圖7 turtlebot2移動(dòng)機(jī)器人

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

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

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

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

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

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

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

4 結(jié) 語

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