基于LIDAR與深度相機的EKF數據融合研究

尹辰柯 胡家輝 王靖宇

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

關鍵詞：深度圖像 ?極坐標 ?障礙物檢測 ?路徑規劃

中圖分類號：TP391 ? ? ? ? ? ? ? ? ? ? ? ? ? 文獻標識碼：A ? ? ? ? ? ? ? ? ? 文章編號：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

傳感器是連接被測環境與實際測量的橋梁，是環境狀態精準測量中最初始和最重要的部分。M. Zohdy等人發現不精確的傳感器讀數會導致冗余的環境測量，產生不恰當甚至錯誤的決策[1]。因此，選擇合適的傳感器做合適的測量，再將被測環境與實際測量聯系起來顯得格外重要。如果單一傳感器無法提供必要的數據，就需要用到傳感器融合技術。使用多個傳感器提供冗余信息，可以減少錯誤的測量的機會[2]。從各個傳感器獲取有關參考的數據，融合數據并將冗余數據利用起來。通過傳感器融合技術，可以將兩個或者多個傳感器用特定的方式結合起來，并能夠互補優勢，去其劣勢[3]。

1 ?LIDAR與深度相機對比

單個傳感器看來，LIDAR能夠獲得高清的三維環境感知信息且工作狀態與自然光無關，利用3D反射精確確定對象，在障礙物的檢測、分類、跟蹤上有廣泛應用，Lin， C.H等人[10]為使用SVM分類器，對加權協方差矩陣進行特征提取，進行LIDAR檢測。但是對于距離較遠的物體，LIDAR無法精確檢測，從而在精確檢測上存在一定問題，并且因為成本較高，無法大范圍使用[3]。LIDAR檢測到目標物體，并拒絕其他物體，給出目標物體的位置（單位為m），方向角（單位為。）以及判別值。

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

2 ?融合技術

眾多攝像機與LIDAR的融合技術中，一個較為精確的例子，融合了稀疏的3D LIDAR和密集的3D圖像點云。但是這種方法中，3D圖像中對應點的匹配在計算上很復雜，如果圖像的紋理很少，則可能無法很好的工作。該文提出一種實時、精確的方法，同時利用LIDAR和Kinect深度相機來準確定位對象，并且在成本上，因為使用八光束的LIDAR，所以更加便宜。該文的重點在于Kinect深度相機和LIDAR二者所搜集信息的融合EKF算法研究及路徑規劃，并做了相應的路徑還原程度和純LIDAR或者深度相機路徑還原的比較，驗證了算法結果。實驗所用數據均來于自建機器人平臺。

3 ?算法設計

算法主要有3個模塊組成，包括數據映射轉換模塊、擴展卡爾曼濾波模塊、定位決策制定模塊。

3.1 深度圖像映射轉換

在Kinect深度相機得到深度圖像之后，經過一系列的圖像預處理，我們將圖像中轉換窗口的每個像素列上深度值最小的像素點距離和角度提取出來，形成一系列的虛擬激光點。

根據數據的對應關系，m為深度圖像中任意一點，M為RGB圖像中與m對應一點。Zk為m點的深度值，r為光心O與目標點M的虛擬激光點距離，A、C為中間虛擬激光點。下面列了各個必要參數的計算方法。

（2）二維激光點標定：將Kinect深度相機的視角范圍規定為，在該范圍內所取得的激光點序列號記為i，所取得數量記為N：。

3.2 擴展卡爾曼濾波

卡爾曼濾波（KF）被廣泛應用于傳感器測量數據的濾波以及傳感器數據融合。由于該系統所處理數據非線性，所以，可以用擴展卡爾曼濾波（EKF）應用到數據的處理中，它可以將協方差和均值線性化。擴展卡爾曼濾波的有關等式如下所示。

（1）目標狀態預測：，式中：xk為狀態向量（目標和速度）。

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

3.3 MATLAB實現

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

4 ?實驗

該文采用的實驗數據樣本來自互聯網爬蟲獲取的KITTI數據集，使用數據對系統準確性進行測試。實驗設備為一臺配備Intel core i5處理器的筆記本電腦，操作環境Ubuntu 16.04，MATLAB版本2018B。得到的各個數據子集的定位均方根誤差對比如表1所示。

通過進行實驗并對結果進行分析，得出擴展卡爾曼濾波算法（EKF）的定位精度顯著優于深度相機，并且優于LIDAR的定位精度，這是因為該文所采用的算法是將深度相機和LIDAR二者在EKF算法下融合實現的，因此能夠保持較高甚至在某些場合之下保持更高的定位精度。

5 ?結語

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

參考文獻

[1] 閆九祥，趙永國，張艷芳，等.基于單目視覺的工業機器人拆垛系統設計與實驗[J].機床與液壓，2020，48（3）：18-23.

[2] 鄒斌，劉康，王科未.基于三維激光雷達的動態障礙物檢測和追蹤方法[J].汽車技術，2017（8）：19-25.

[3] 李啟東.基于單目視覺的機器人動態障礙物檢測與壁障方法研究[D].吉林大學，2016.

[4] 曹潔.SINS/GPS組合導航自適應濾波算法研究[D].華東師范大學，2018.

[5] 王博.基于MIMU的姿態解算研究[D].中北大學，2018.

[6] 仲江濤.基于UWB室內定位算法的研究與實現[D].深圳大學，2017.

[7] 朱文浩.基于多傳感器融合的移動機器人系統設計與建圖導航研究[D].哈爾濱工業大學，2018.

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

[9] 郎朗，馮曉蓉，劉浪.基于LiDAR的農田地形環境三維重建方法設計與研究[J].中國農機化學報，2020，41（1）：161-166.

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

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

[12] 李偉.基于回波次數的點云強度濾波方法在處理LiDAR數據中的研究與應用[J].經緯天地， 2019（3）：61-65.