基于RGB-D相機(jī)和激光雷達(dá)的傳感器數(shù)據(jù)融合算法

孫 健，劉隆輝，李 智，楊佳玉，申 奧

（湖南工程學(xué)院 電氣與信息工程學(xué)院，湘潭 411104）

0 引言

傳感器作為機(jī)器人環(huán)境感知的基礎(chǔ)［1］，主要分為非視覺器件和視覺器件.非視覺器件（如激光雷達(dá)、超聲波或紅外傳感器）具有抗干擾能力強(qiáng)、可全天候工作的優(yōu)點(diǎn)，但該類傳感器不足之處是采集的數(shù)據(jù)單一，且價(jià)格昂貴；視覺器件（如單目相機(jī)、雙目相機(jī)和RGB-D相機(jī)）雖然成本更低，信息更豐富，但受環(huán)境影響大，檢測精度低，視野范圍有限.因單一傳感器數(shù)據(jù)無法滿足機(jī)器人不同場景的應(yīng)用需求，傳感器數(shù)據(jù)融合技術(shù)應(yīng)運(yùn)而生，并成為當(dāng)前機(jī)器人技術(shù)的研究焦點(diǎn)，如圖1所示.

圖1 視覺激光融合示意圖

近年隨著技術(shù)不斷的迭代更新與積累，目前已有許多關(guān)于傳感器數(shù)據(jù)融合算法的研究.文獻(xiàn)［1］通過EKF算法，將激光雷達(dá)信息和深度相機(jī)信息融合，但因其只對固定高度范圍內(nèi)的像素進(jìn)行轉(zhuǎn)換，縮小了RGB-D相機(jī)的感知范圍，不能根據(jù)機(jī)器人所處實(shí)際環(huán)境自適應(yīng)剔除地面點(diǎn)云.文獻(xiàn)［2］先獲取RGB-D相機(jī)點(diǎn)云中的地面點(diǎn)云，剔除后進(jìn)行投影，再通過ICP算法將RGB-D相機(jī)點(diǎn)云與激光雷達(dá)點(diǎn)云配準(zhǔn)融合，但算法精度和時(shí)效性較低，導(dǎo)致兩傳感器之間的位姿關(guān)系不能實(shí)時(shí)準(zhǔn)確更新，影響數(shù)據(jù)融合效果.

針對上述問題，本文在對RGB-D相機(jī)獲取的點(diǎn)云數(shù)據(jù)優(yōu)化之后，利用PL-ICP點(diǎn)云配準(zhǔn)實(shí)現(xiàn)數(shù)據(jù)融合，然后基于擴(kuò)展卡爾曼濾波（EKF）進(jìn)行精確融合，進(jìn)一步提高融合數(shù)據(jù)精準(zhǔn)度.

1 傳感器模型

為更好地研究傳感器數(shù)據(jù)融合的過程，使算法更具客觀性和可重復(fù)性，本文對核心器件激光雷達(dá)和RGB-D相機(jī)進(jìn)行建模.

1.1 RGB-D相機(jī)模型

RGB-D相機(jī)是一種應(yīng)用較廣泛的視覺器件，不僅可獲取環(huán)境的圖像信息，還可直接獲取環(huán)境對應(yīng)點(diǎn)的深度信息［3］，相比傳統(tǒng)的雙目相機(jī)依靠視差獲取深度信息的方式，RGB-D相機(jī)大大地降低了計(jì)算的復(fù)雜度.

本文選用的RGB-D相機(jī)為Kinect v1［4］，如圖2所示，硬件結(jié)構(gòu)主要包括外殼、底座、散熱器、4個(gè)麥克風(fēng)陣列、傳動電動機(jī)以及3個(gè)攝像頭從左到右分別為紅外投影機(jī)、顏色攝像機(jī)、紅外攝像機(jī)，表1是Kinect v1的主要參數(shù).

圖2 Kinect v1的實(shí)物圖

表1 Kinect v1主要參數(shù)

Kinect v1所獲得的彩色圖像和深度圖像用點(diǎn)云來描述，其中每一個(gè)點(diǎn)均由r，g，b，x，y，z共6個(gè)分量分別表示該點(diǎn)的顏色與空間位置.

針孔相機(jī)模型如圖3所示，對任意給定的一個(gè)深度圖像點(diǎn)m()u,v,d，其對應(yīng)的相機(jī)世界坐標(biāo)點(diǎn)M(x,y,z)，可通過下式進(jìn)行轉(zhuǎn)換：

圖3 針孔相機(jī)模型

其中fx、fy為相機(jī)在x、y兩個(gè)軸上的焦距，cx、cy為相機(jī)的光圈中心，s為深度圖的縮放因子.

為獲得更好的成像效果，在相機(jī)前安裝了透鏡，由于其對光線傳播產(chǎn)生影響，相應(yīng)的也影響了相機(jī)坐標(biāo)系與像素平面的一一對應(yīng).為消除這種影響，需對畸變進(jìn)行矯正.畸變可分為徑向畸變和切向畸變，通常影響作用較大的為徑向畸變，下面主要分析徑向畸變的矯正.

徑向畸變隨著與中心距離的增加而增加，可以用以下多項(xiàng)式函數(shù)描述：

式中：[x,y]T為歸一化平面的點(diǎn)的坐標(biāo)，[xdt,ydt]T為畸變糾正后點(diǎn)的坐標(biāo).對于像素平面中的某點(diǎn)，可以通過這些畸變系數(shù)k1、k2、k3得到該點(diǎn)在相機(jī)坐標(biāo)系上的正確坐標(biāo).

1.2 激光雷達(dá)模型

激光雷達(dá)能提供環(huán)境精確的測量信息，有較廣的感知范圍，是移動機(jī)器人環(huán)境感知和構(gòu)建地圖的核心器件之一［5］.圖4是本文所使用的激光雷達(dá)LS01B實(shí)物圖，表2為其主要參數(shù).

圖4 LS01B實(shí)物圖

表2 LS01B主要參數(shù)

LS01B采用三角測量法來獲取環(huán)境信息.三角測量原理如圖5所示，激光器向外發(fā)射激光，線性CCD接收障礙物反射回來的光線，根據(jù)反射原理，物體在CCD上的成像與物體和雷達(dá)之間的距離相關(guān)，因此在CCD上能夠顯示不同距離的物體的反射光.而激光器和探測器之間有一定間隔，根據(jù)三角測量公式，能計(jì)算出被測物體的距離.

圖5 三角測量原理示意圖

如圖6所示，激光雷達(dá)獲取的激光束與YL軸正向夾角為α，激光雷達(dá)坐標(biāo)原點(diǎn)到障礙物點(diǎn)的距離為r.則激光測距點(diǎn)二元組()α,r與障礙物坐標(biāo)(xL,yL)有如下轉(zhuǎn)換關(guān)系：

圖6 測距二元組與障礙物坐標(biāo)關(guān)系圖

2 深度相機(jī)模擬激光數(shù)據(jù)

根據(jù)上述激光雷達(dá)和RGB-D相機(jī)的模型，兩傳感器獲取的環(huán)境信息的表現(xiàn)形式和實(shí)際意義都不同.為實(shí)現(xiàn)視覺激光數(shù)據(jù)融合，本文的策略是先使用深度相機(jī)模擬激光數(shù)據(jù)，然后將模擬激光數(shù)據(jù)與激光傳感器數(shù)據(jù)融合.

2.1 點(diǎn)云優(yōu)化

RGB-D相機(jī)模型可將深度圖像轉(zhuǎn)換為點(diǎn)云，但RGB-D相機(jī)采集的點(diǎn)云數(shù)據(jù)中包含地面點(diǎn)云.地面點(diǎn)云不屬于機(jī)器人建圖時(shí)的障礙物信息，在點(diǎn)云投影時(shí)會作為障礙物引入，因此需將地面點(diǎn)云提取剔除.

本文針對隨機(jī)抽樣一致性算法［6］（RANSAC）在地面起伏不平的復(fù)雜場景下提取地面點(diǎn)云不完整的缺點(diǎn)，先采用改進(jìn)的RANSAC平面提取算法進(jìn)行地面粗提取，而后利用法向量差分特征（DoN）［7］對剩余點(diǎn)云進(jìn)行二次提取，達(dá)到提高地面點(diǎn)云提取完整的目的.具體步驟如下：

（1）原始點(diǎn)云數(shù)據(jù)利用體素化網(wǎng)格進(jìn)行降采樣濾波，在盡可能保持采樣前后點(diǎn)云一致性的基礎(chǔ)上，降低算法內(nèi)存占用和提高算法時(shí)效性.

（2）計(jì)算地面點(diǎn)云數(shù)據(jù)法向量.

（3）估計(jì)地面點(diǎn)云平面方程，實(shí)現(xiàn)地面點(diǎn)云的粗提取.

由地面法向量計(jì)算地面的擬合方程：

利用RANSAC算法隨機(jī)選擇數(shù)據(jù)點(diǎn)并計(jì)算該點(diǎn)所在的平面方程π(A,B,C,D)，計(jì)算點(diǎn)云數(shù)據(jù)中每個(gè)點(diǎn)到該平面的距離，利用距離閾值dmax統(tǒng)計(jì)出該閾值內(nèi)的點(diǎn)數(shù)Nin.重復(fù)隨機(jī)采樣更新平面方程，最終選擇點(diǎn)數(shù)最多的平面方程所對應(yīng)的點(diǎn)構(gòu)成集合，利用該集合最小化距離函數(shù)，求得最優(yōu)地面平面方程：

其中Dk表示集合中第k個(gè)點(diǎn) 到地面平面的距離.

（4）對剩余點(diǎn)云法向量計(jì)算差分特征（DoN），二次提取地面點(diǎn)云.

對于點(diǎn)云中的每一點(diǎn)q，在不同鄰域r1，r2（r1>r2）進(jìn)行法向量估計(jì)，得到兩個(gè)單位法向量n(q,r1),n(q,r2)，向量間的夾角設(shè)為θ，定義法向量差分算子如下：

（5）對地面點(diǎn)云平面進(jìn)行合并.

2.2 點(diǎn)云投影

剔除地面點(diǎn)云后的障礙物點(diǎn)云為Mi()xi,yi,zi，要實(shí)現(xiàn)對激光數(shù)據(jù)的模擬，需先將障礙物三維點(diǎn)云對地面進(jìn)行投影，得到二維點(diǎn)云mi()θi,di，然后將Kinect v1每條視線上距離光心O最近的點(diǎn)提取出來，便形成了一束模擬激光，如圖7所示.具體步驟如下：

圖7 Kinect v1模擬激光原理圖

（1）障礙物三維點(diǎn)云Mi()xi,yi,zi對地面進(jìn)行投影得到二維點(diǎn)云mi()θi,di，其中：

（2）若Kinect相機(jī)觀測角范圍為()βmin,βmax，激光束共細(xì)分為N份.假設(shè)m()θ,d點(diǎn)為OC視線上距離光心O最近的點(diǎn)，那么，點(diǎn)m()θ,d在數(shù)組laser中的索引值為：

（3）數(shù)組laser［n］表示模擬激光數(shù)據(jù)，若某視線上產(chǎn)生模擬激光序號是n，該視線上激光點(diǎn)距離值為d[θ]，那么形成的模擬激光為：

3 視覺激光數(shù)據(jù)融合

單一傳感器無法提供完整的環(huán)境測量數(shù)據(jù)，需要用到傳感器融合技術(shù).使用多個(gè)傳感器提供冗余信息，可以減少測量誤差.從相機(jī)和激光傳感器獲取傳感器數(shù)據(jù)，通過傳感器融合技術(shù)，可以將兩個(gè)傳感器用特定的方式結(jié)合起來，實(shí)現(xiàn)優(yōu)勢互補(bǔ).本文首先對兩組傳感器數(shù)據(jù)進(jìn)行點(diǎn)云配準(zhǔn)實(shí)現(xiàn)粗融合，然后通過擴(kuò)展卡爾曼濾波將兩組激光數(shù)據(jù)進(jìn)行精確融合.

3.1 點(diǎn)云配準(zhǔn)

移動機(jī)器人系統(tǒng)上多傳感器擁有多個(gè)坐標(biāo)系，算法需要將多個(gè)傳感器的數(shù)據(jù)統(tǒng)一到同一個(gè)基準(zhǔn)坐標(biāo)系進(jìn)行對齊.對已獲取的兩束激光數(shù)據(jù)，通過對激光雷達(dá)的點(diǎn)云數(shù)據(jù)和深度相機(jī)的圖像點(diǎn)云數(shù)據(jù)進(jìn)行配準(zhǔn)，實(shí)現(xiàn)數(shù)據(jù)的坐標(biāo)系對齊，達(dá)到數(shù)據(jù)粗融合的目的.

圖8所示為同一時(shí)刻分別處于激光雷達(dá)坐標(biāo)系(OL,XL,YL,ZL)與Kinect v1相機(jī)坐標(biāo)系(OK,XK,YK,ZK)中的點(diǎn)M，其坐標(biāo)分別為(xL,yL,zL)和(xK,yK,zK)，根據(jù)激光雷達(dá)坐標(biāo)系與相機(jī)坐標(biāo)系的位姿關(guān)系，得到坐標(biāo)轉(zhuǎn)換關(guān)系為：

圖8 坐標(biāo)系位置關(guān)系示意圖

式中，R是旋轉(zhuǎn)矩陣；T是平移矩陣.

如圖9（a）可知，激光點(diǎn)是對實(shí)際環(huán)境中曲面的離散采樣，激光點(diǎn)到實(shí)際曲面的距離是最優(yōu)的誤差尺度.常用的點(diǎn)云配準(zhǔn)算法經(jīng)典迭代最鄰近（Iterative Closest Point，ICP）算法［8］，如圖9（b）所示，將點(diǎn)到點(diǎn)的距離，作為誤差構(gòu)建誤差方程，容易造成誤匹配，增加迭代時(shí)間.本文采用改進(jìn)的PL-ICP（Point-to-Line）算法［9］，通過用點(diǎn)到其最近兩點(diǎn)連線的距離代替點(diǎn)到點(diǎn)的距離，如圖9（c）所示，構(gòu)建誤差方程，有利于縮減迭代時(shí)間，增加算法時(shí)效性.

圖9 PL-ICP原理圖

深度相機(jī)模擬的點(diǎn)云和對應(yīng)的激光雷達(dá)點(diǎn)云空間坐標(biāo)集合分別為：

算法的具體步驟為：

（1）給定一個(gè)初始的轉(zhuǎn)換矩陣Qk=()Rk,Tk，將當(dāng)前模擬點(diǎn)云的數(shù)據(jù)轉(zhuǎn)換到激光雷達(dá)坐標(biāo)系下，轉(zhuǎn)換方式為：

后面迭代計(jì)算所需的數(shù)據(jù)由上一次算法迭代計(jì)算得到.

（3）計(jì)算誤差并去除誤差過大的點(diǎn).

（4）構(gòu)建最小化誤差方程：

其中ni表示線的法向量.

（5）求解出位姿轉(zhuǎn)換矩陣Qk+1，將其用于下次迭代計(jì)算.

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

在點(diǎn)云配準(zhǔn)得到兩個(gè)傳感器之間的位姿關(guān)系后，可將兩束激光轉(zhuǎn)換到同一坐標(biāo)系下進(jìn)行數(shù)據(jù)精確融合.該過程常用濾波思想的方法實(shí)現(xiàn)，針對經(jīng)典傳感器濾波算法Kalman濾波［10］只局限用于系統(tǒng)線性、噪聲為高斯理想場景的不足，本文采用基于其演變的擴(kuò)展卡爾曼濾波［11］（EKF），在粗融合的基礎(chǔ)上進(jìn)行精細(xì)融合.

系統(tǒng)的狀態(tài)轉(zhuǎn)移方程以及傳感器的觀測方程分別為：

θk為估計(jì)值，zk為傳感器的觀測值，狀態(tài)轉(zhuǎn)移噪聲向量sk一般是服從多維高斯分布的，其協(xié)方差矩陣為Q；觀測噪聲向量vk一般也是服從多維高斯分布的，其協(xié)方差矩陣是R.

利用泰勒展開式對（13）式在一次估計(jì)值θk-1處展開得：

利用泰勒展開式對（14）式在一次估計(jì)值θ′k處展開得：

其中，F(xiàn)k-1和Hk分別表示f(θk-1)和h(θk)在θk-1和處的雅克比矩陣.

EKF分為預(yù)測和更新兩步：

預(yù)測：

其中為預(yù)測值，為預(yù)測值與真實(shí)值的協(xié)方差.

更新：

其中Kk為卡爾曼增益，θk為估計(jì)值，zk為觀測值，Pk為估計(jì)值與真實(shí)值之間的協(xié)方差.

如圖11所示，具體融合過程為：

圖10 融合算法示意圖

（1）在t時(shí)收到激光雷達(dá)數(shù)據(jù)時(shí)，根據(jù)t-1時(shí)刻的估計(jì)值θt-1和協(xié)方差Pt-1，利用公式（17）、（18）完成一次預(yù)測得到預(yù)測值和協(xié)方差.

（2）再根據(jù)t時(shí)刻的激光雷達(dá)的觀測數(shù)據(jù)、預(yù)測值和協(xié)方差，利用公式（19）、（20）、（21）實(shí)現(xiàn)測量值更新得到t時(shí)刻的狀態(tài)θt和協(xié)方差Pt.

（3）在t+1時(shí)刻收到Kinect v1模擬激光數(shù)據(jù)時(shí)，根據(jù)t時(shí)刻的狀態(tài)θt和協(xié)方差Pt完成一次預(yù)測得到預(yù)測值和協(xié)方差

（4）同理，根據(jù)t+1時(shí)刻的Kinect v1模擬激光的觀測數(shù)據(jù)、預(yù)測值和協(xié)方差，實(shí)現(xiàn)測量值更新得到t+1時(shí)刻的估計(jì)值θt+1和協(xié)方差Pt+1用于下一次迭代.

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

實(shí)驗(yàn)平臺如圖11所示基于Aimibot教育機(jī)器人，硬件包括NUC機(jī)載電腦、高清顯示器、激光雷達(dá)LS01B、RGB-D相機(jī)Kinect v1等，其中機(jī)載電腦搭載基于Linux（Ubuntu16.04）系統(tǒng)的ROS（Robot Operating System）操作系統(tǒng).

圖11 實(shí)驗(yàn)平臺

綜合考慮實(shí)驗(yàn)室的條件以及實(shí)驗(yàn)平臺的局限性，為驗(yàn)證視覺激光融合建圖算法在復(fù)雜環(huán)境下的建圖準(zhǔn)確性與可靠性，設(shè)置如圖12所示的實(shí)驗(yàn)場景.場景面積為8 m×5 m，主要包括三種不同障礙物，分別為組合體A、鏤空的凳子B、小木塊C.

圖12 實(shí)驗(yàn)場景

場景中長方體木塊的寬為10 cm，而實(shí)驗(yàn)平臺的單線激光雷達(dá)離地的高度為15 cm，因而場景中的一部分障礙物不在單線激光雷達(dá)的掃描平面.三種障礙物依次排開，與機(jī)器人形成的夾角為60°，而Kinect v1深度相機(jī)的視角為57°×43°，因此機(jī)器人無法感知到所有障礙物，如圖13所示.

圖13 機(jī)器人與障礙物相關(guān)位置關(guān)系

實(shí)驗(yàn)主要是對比采用不同傳感器數(shù)據(jù)建圖的結(jié)果，以驗(yàn)證本文算法的可行性和有效性.

如圖14為單個(gè)二維激光雷達(dá)的建圖結(jié)果，該結(jié)果表明二維激光雷達(dá)無法感知非激光掃描平面障礙物信息，對場景中障礙物的感知情況為：組合體A中比激光雷達(dá)位置低的部分和鏤空凳子B與激光雷達(dá)不在同一平面的部分均出現(xiàn)信息缺失.此地圖在機(jī)器人導(dǎo)航時(shí)會使其產(chǎn)生錯(cuò)誤的路徑規(guī)劃.

圖14 二維激光數(shù)據(jù)建圖結(jié)果

文獻(xiàn)［1］采用的視覺激光融合算法，在進(jìn)行RGB-D相機(jī)模擬激光數(shù)據(jù)時(shí)，處理地面點(diǎn)云時(shí)僅對深度圖像固定高度范圍內(nèi)像素進(jìn)行轉(zhuǎn)換，縮小了RGB-D相機(jī)的感知范圍，不能根據(jù)機(jī)器人所處實(shí)際環(huán)境自適應(yīng)剔除地面點(diǎn)云.因而機(jī)器人在移動建圖過程中對同一障礙物不能連續(xù)觀測，導(dǎo)致部分障礙物信息缺失，如圖15所示障礙物均出現(xiàn)缺損.

圖15 文獻(xiàn)［1］融合方法建圖結(jié)果

文獻(xiàn)［2］采用的傳統(tǒng)ICP算法進(jìn)行點(diǎn)云配準(zhǔn)，算法精度和時(shí)效性較低，導(dǎo)致兩傳感器之間的位姿關(guān)系不能實(shí)時(shí)準(zhǔn)確更新，使得融合過程數(shù)據(jù)參考坐標(biāo)系不統(tǒng)一，最終融合結(jié)果出現(xiàn)重影，如圖16所示，建圖精度降低，無法準(zhǔn)確還原障礙物的信息.

圖17 本文融合方法建圖結(jié)果

圖7為本文視覺激光融合數(shù)據(jù)建圖結(jié)果，對比單個(gè)二維激光雷達(dá)數(shù)據(jù)和文獻(xiàn)［1］［2］融合方法數(shù)據(jù)建圖結(jié)果，很好地還原了場景的障礙物信息，建圖的完整度大大提高，包含了更為豐富的環(huán)境信息，同時(shí)也保證了建圖的精度.

圖16 文獻(xiàn)[2]融合方法建圖結(jié)果

通過該實(shí)驗(yàn)的對比可以看出，融合多傳感器數(shù)據(jù)能夠有效彌補(bǔ)單一傳感器的不足，實(shí)現(xiàn)對環(huán)境的更精確、更完整的感知.利用此融合算法可以實(shí)現(xiàn)機(jī)器人在復(fù)雜環(huán)境下的精確定位和導(dǎo)航.

5 結(jié)語

本文分析了當(dāng)前移動機(jī)器人環(huán)境感知傳感器的優(yōu)缺點(diǎn)，針對當(dāng)前一些傳感器融合方案的不足，提出一種改進(jìn)RGB-D相機(jī)和激光雷達(dá)數(shù)據(jù)融合的算法.利用融合后的數(shù)據(jù)進(jìn)行建圖，并對比單個(gè)傳感器數(shù)據(jù)和已有傳感器融合方案的建圖結(jié)果，本文的融合算法在保證了傳感器數(shù)據(jù)精度的基礎(chǔ)上，實(shí)現(xiàn)了對環(huán)境信息更全面的感知.這對提高移動機(jī)器人建圖完整度和精準(zhǔn)度有重要意義.