尹辰柯 胡家輝 王靖宇

摘 ?要：常用定位及避障傳感器中，LIDAR不受光照影響、創(chuàng)建地圖精度高，但數(shù)據(jù)量較少;Kinect深度相機(jī)成本低、數(shù)據(jù)量豐富，但受環(huán)境影響較大。在此基礎(chǔ)上，提出一種將激光雷達(dá)與Kinect深度相機(jī)數(shù)據(jù)融合的方法。該方法預(yù)處理深度圖像數(shù)據(jù)，轉(zhuǎn)換為激光數(shù)據(jù)，從而投影到極坐標(biāo)上，得到的非線性數(shù)據(jù)，在處理上引入了雅可比矩陣。接著通過EKF算法，將線性激光雷達(dá)信息和變換非線性深度相機(jī)信息融合。通過對(duì)比，EKF融合數(shù)據(jù)相較單獨(dú)LIDAR數(shù)據(jù)或Kinect深度相機(jī)數(shù)據(jù)能夠在大部分場(chǎng)景中更好地還原真實(shí)路徑。

關(guān)鍵詞：深度圖像 ?極坐標(biāo) ?障礙物檢測(cè) ?路徑規(guī)劃

中圖分類號(hào)：TP391 ? ? ? ? ? ? ? ? ? ? ? ? ? 文獻(xiàn)標(biāo)識(shí)碼：A ? ? ? ? ? ? ? ? ? 文章編號(hào)：1672-3791（2020）10（b）-0033-03

Abstract： Among the commonly used location and obstacle avoidance sensors， lidar is not affected by illumination and has high accuracy in creating maps， but the amount of data is small; Kinect depth camera has low cost and rich data， but it is greatly affected by the environment. On this basis， a method of data fusion between lidar and Kinect depth camera is proposed. In this method， the depth image data is preprocessed， converted into laser data， and then projected onto the polar coordinates. The Jacobian matrix is introduced in the processing of nonlinear data. Then， the linear lidar information and transform nonlinear depth camera information are fused by EKF algorithm. Compared with lidar data or Kinect depth camera data， EKF fusion data can restore the real path better in most scenes.

Key Words： Depth image; Polar coordinates; Obstacle detection; Path planning

傳感器是連接被測(cè)環(huán)境與實(shí)際測(cè)量的橋梁，是環(huán)境狀態(tài)精準(zhǔn)測(cè)量中最初始和最重要的部分。M. Zohdy等人發(fā)現(xiàn)不精確的傳感器讀數(shù)會(huì)導(dǎo)致冗余的環(huán)境測(cè)量，產(chǎn)生不恰當(dāng)甚至錯(cuò)誤的決策[1]。因此，選擇合適的傳感器做合適的測(cè)量，再將被測(cè)環(huán)境與實(shí)際測(cè)量聯(lián)系起來(lái)顯得格外重要。如果單一傳感器無(wú)法提供必要的數(shù)據(jù)，就需要用到傳感器融合技術(shù)。使用多個(gè)傳感器提供冗余信息，可以減少錯(cuò)誤的測(cè)量的機(jī)會(huì)[2]。從各個(gè)傳感器獲取有關(guān)參考的數(shù)據(jù)，融合數(shù)據(jù)并將冗余數(shù)據(jù)利用起來(lái)。通過傳感器融合技術(shù)，可以將兩個(gè)或者多個(gè)傳感器用特定的方式結(jié)合起來(lái)，并能夠互補(bǔ)優(yōu)勢(shì)，去其劣勢(shì)[3]。

1 ?LIDAR與深度相機(jī)對(duì)比

單個(gè)傳感器看來(lái)，LIDAR能夠獲得高清的三維環(huán)境感知信息且工作狀態(tài)與自然光無(wú)關(guān)，利用3D反射精確確定對(duì)象，在障礙物的檢測(cè)、分類、跟蹤上有廣泛應(yīng)用，Lin， C.H等人[10]為使用SVM分類器，對(duì)加權(quán)協(xié)方差矩陣進(jìn)行特征提取，進(jìn)行LIDAR檢測(cè)。但是對(duì)于距離較遠(yuǎn)的物體，LIDAR無(wú)法精確檢測(cè)，從而在精確檢測(cè)上存在一定問題，并且因?yàn)槌杀据^高，無(wú)法大范圍使用[3]。LIDAR檢測(cè)到目標(biāo)物體，并拒絕其他物體，給出目標(biāo)物體的位置（單位為m），方向角（單位為。）以及判別值。

與LIDAR不同的是，深度相機(jī)根據(jù)豐富的紋理、形狀、顏色來(lái)做出決策。它可以用于檢測(cè)物體并估計(jì)物體位置，并且單個(gè)RGB相機(jī)可以獨(dú)立用于圖像定位。雖然深度相機(jī)可以用圖像的深度學(xué)習(xí)對(duì)物體進(jìn)行檢測(cè)，以改進(jìn)結(jié)果[7-9]。但是與LIDAR相比較，Kinect深度相機(jī)的定位效果較差，在多霧、下雨等環(huán)境中效果不佳[4-6]，但LIDAR則不受這些因素影響[3]。Kinect深度相機(jī)將檢測(cè)的結(jié)果表示為圖像中的相對(duì)邊框的位置，并且劃分各個(gè)區(qū)域的可信度。對(duì)兩個(gè)數(shù)據(jù)進(jìn)行預(yù)處理，變換深度相機(jī)的數(shù)據(jù)映射，得到x、y、z方向以及角度θ估計(jì)值。

2 ?融合技術(shù)

眾多攝像機(jī)與LIDAR的融合技術(shù)中，一個(gè)較為精確的例子，融合了稀疏的3D LIDAR和密集的3D圖像點(diǎn)云。但是這種方法中，3D圖像中對(duì)應(yīng)點(diǎn)的匹配在計(jì)算上很復(fù)雜，如果圖像的紋理很少，則可能無(wú)法很好的工作。該文提出一種實(shí)時(shí)、精確的方法，同時(shí)利用LIDAR和Kinect深度相機(jī)來(lái)準(zhǔn)確定位對(duì)象，并且在成本上，因?yàn)槭褂冒斯馐腖IDAR，所以更加便宜。該文的重點(diǎn)在于Kinect深度相機(jī)和LIDAR二者所搜集信息的融合EKF算法研究及路徑規(guī)劃，并做了相應(yīng)的路徑還原程度和純LIDAR或者深度相機(jī)路徑還原的比較，驗(yàn)證了算法結(jié)果。實(shí)驗(yàn)所用數(shù)據(jù)均來(lái)于自建機(jī)器人平臺(tái)。

3 ?算法設(shè)計(jì)

算法主要有3個(gè)模塊組成，包括數(shù)據(jù)映射轉(zhuǎn)換模塊、擴(kuò)展卡爾曼濾波模塊、定位決策制定模塊。

3.1 深度圖像映射轉(zhuǎn)換

在Kinect深度相機(jī)得到深度圖像之后，經(jīng)過一系列的圖像預(yù)處理，我們將圖像中轉(zhuǎn)換窗口的每個(gè)像素列上深度值最小的像素點(diǎn)距離和角度提取出來(lái)，形成一系列的虛擬激光點(diǎn)。

根據(jù)數(shù)據(jù)的對(duì)應(yīng)關(guān)系，m為深度圖像中任意一點(diǎn)，M為RGB圖像中與m對(duì)應(yīng)一點(diǎn)。Zk為m點(diǎn)的深度值，r為光心O與目標(biāo)點(diǎn)M的虛擬激光點(diǎn)距離，A、C為中間虛擬激光點(diǎn)。下面列了各個(gè)必要參數(shù)的計(jì)算方法。

（2）二維激光點(diǎn)標(biāo)定：將Kinect深度相機(jī)的視角范圍規(guī)定為，在該范圍內(nèi)所取得的激光點(diǎn)序列號(hào)記為i，所取得數(shù)量記為N：。

3.2 擴(kuò)展卡爾曼濾波

卡爾曼濾波（KF）被廣泛應(yīng)用于傳感器測(cè)量數(shù)據(jù)的濾波以及傳感器數(shù)據(jù)融合。由于該系統(tǒng)所處理數(shù)據(jù)非線性，所以，可以用擴(kuò)展卡爾曼濾波（EKF）應(yīng)用到數(shù)據(jù)的處理中，它可以將協(xié)方差和均值線性化。擴(kuò)展卡爾曼濾波的有關(guān)等式如下所示。

（1）目標(biāo)狀態(tài)預(yù)測(cè)：，式中：xk為狀態(tài)向量（目標(biāo)和速度）。

（3）計(jì)算卡爾曼濾波增益：，式中：H為測(cè)量向量。真實(shí)測(cè)量zk為：，式中：vk為測(cè)量噪聲，服從均值為0、方差R的正太分布。

3.3 MATLAB實(shí)現(xiàn)

該系統(tǒng)算法基于MATLAB運(yùn)行調(diào)試實(shí)現(xiàn)。系統(tǒng)在開始階段，首先從外部文件中讀取攝像頭參數(shù)，并將參數(shù)初始化，然后圖像采集節(jié)點(diǎn)不斷以5Hz的刷新頻率更新深度相機(jī)采集的場(chǎng)景圖像。在得到深度圖像與LIDAR信息之后，EKF節(jié)點(diǎn)與模糊系統(tǒng)節(jié)點(diǎn)同時(shí)運(yùn)行并訂閱數(shù)據(jù)信息，在EKF節(jié)點(diǎn)工作結(jié)束后，將采集信息作為參考加入模糊邏輯系統(tǒng)中，作為決策參考因素。EKF節(jié)點(diǎn)在完成系統(tǒng)的初始化并且針對(duì)輸入的圖像進(jìn)行目標(biāo)狀態(tài)的預(yù)測(cè)、卡爾曼濾波增益計(jì)算、數(shù)據(jù)更新3個(gè)線程，從而一方面糾正相機(jī)的狀態(tài)，同時(shí)進(jìn)行定位的決策。在擴(kuò)展卡爾曼濾波節(jié)點(diǎn)上，對(duì)目標(biāo)狀態(tài)預(yù)測(cè)中，包含對(duì)深度相機(jī)以及LIDAR參數(shù)進(jìn)行檢查，獲取正確的參數(shù)條件之下，執(zhí)行接下來(lái)的算法步驟，最終實(shí)現(xiàn)障礙物的檢測(cè)并將該信息傳遞給下面步驟。當(dāng)未檢測(cè)到深度相機(jī)或者LIDAR傳遞有效數(shù)據(jù)時(shí)，系統(tǒng)關(guān)閉。

4 ?實(shí)驗(yàn)

該文采用的實(shí)驗(yàn)數(shù)據(jù)樣本來(lái)自互聯(lián)網(wǎng)爬蟲獲取的KITTI數(shù)據(jù)集，使用數(shù)據(jù)對(duì)系統(tǒng)準(zhǔn)確性進(jìn)行測(cè)試。實(shí)驗(yàn)設(shè)備為一臺(tái)配備Intel core i5處理器的筆記本電腦，操作環(huán)境Ubuntu 16.04，MATLAB版本2018B。得到的各個(gè)數(shù)據(jù)子集的定位均方根誤差對(duì)比如表1所示。

通過進(jìn)行實(shí)驗(yàn)并對(duì)結(jié)果進(jìn)行分析，得出擴(kuò)展卡爾曼濾波算法（EKF）的定位精度顯著優(yōu)于深度相機(jī)，并且優(yōu)于LIDAR的定位精度，這是因?yàn)樵撐乃捎玫乃惴ㄊ菍⑸疃认鄼C(jī)和LIDAR二者在EKF算法下融合實(shí)現(xiàn)的，因此能夠保持較高甚至在某些場(chǎng)合之下保持更高的定位精度。

5 ?結(jié)語(yǔ)

該文提出一種將激光雷達(dá)與Kinect深度相機(jī)數(shù)據(jù)融合的方法，將深度圖像數(shù)據(jù)處理為非線性激光數(shù)據(jù)，通過卡爾曼濾波算法（EKF）融合數(shù)據(jù)，并將融合結(jié)果與LIDAR和深度相機(jī)分別單獨(dú)作用下均方差誤差對(duì)比。通過多傳感器數(shù)據(jù)融合，可以有效提高數(shù)據(jù)精確度，提高復(fù)雜環(huán)境下場(chǎng)景識(shí)別能力。該研究可廣泛應(yīng)用于自動(dòng)駕駛、運(yùn)輸、服務(wù)機(jī)器人等領(lǐng)域。

參考文獻(xiàn)

[1] 閆九祥，趙永國(guó)，張艷芳，等.基于單目視覺的工業(yè)機(jī)器人拆垛系統(tǒng)設(shè)計(jì)與實(shí)驗(yàn)[J].機(jī)床與液壓，2020，48（3）：18-23.

[2] 鄒斌，劉康，王科未.基于三維激光雷達(dá)的動(dòng)態(tài)障礙物檢測(cè)和追蹤方法[J].汽車技術(shù)，2017（8）：19-25.

[3] 李啟東.基于單目視覺的機(jī)器人動(dòng)態(tài)障礙物檢測(cè)與壁障方法研究[D].吉林大學(xué)，2016.

[4] 曹潔.SINS/GPS組合導(dǎo)航自適應(yīng)濾波算法研究[D].華東師范大學(xué)，2018.

[5] 王博.基于MIMU的姿態(tài)解算研究[D].中北大學(xué)，2018.

[6] 仲江濤.基于UWB室內(nèi)定位算法的研究與實(shí)現(xiàn)[D].深圳大學(xué)，2017.

[7] 朱文浩.基于多傳感器融合的移動(dòng)機(jī)器人系統(tǒng)設(shè)計(jì)與建圖導(dǎo)航研究[D].哈爾濱工業(yè)大學(xué)，2018.

[8] 楊思睿，薛朝輝，張玲，等.高光譜與LiDAR數(shù)據(jù)融合研究——以黑河中游張掖綠洲農(nóng)業(yè)區(qū)精細(xì)作物分類為例[J].國(guó)土資源遙感，2018，30（4）：33-40.

[9] 郎朗，馮曉蓉，劉浪.基于LiDAR的農(nóng)田地形環(huán)境三維重建方法設(shè)計(jì)與研究[J].中國(guó)農(nóng)機(jī)化學(xué)報(bào)，2020，41（1）：161-166.

[10] 楊杰.基于LiDAR數(shù)據(jù)的DSM和DEM制作研究[J].科技資訊，2019，17（22）：6-7.

[11] 許志華.基于低空影像和地面LiDAR數(shù)據(jù)的建筑物損毀評(píng)估方法研究[J].地理與地理信息科學(xué)，2018，34（4）：126.

[12] 李偉.基于回波次數(shù)的點(diǎn)云強(qiáng)度濾波方法在處理LiDAR數(shù)據(jù)中的研究與應(yīng)用[J].經(jīng)緯天地， 2019（3）：61-65.