基于 ROS 機器人的相機與激光雷達融合技術研究

張青春 何孝慈 姚勝 郭振久

摘要：為提高智能移動機器人的感知能力，針對智能機器人相機與激光雷達融合提出一種基于直線與平面擬合算法，并將其應用于 ROS 智能移動機器人系統。基于 PPB 方法對相機圖像進行畸變校正獲得角點的二維坐標，采用點云反射強度（PCI）的方法獲得角點的三維坐標。通過解決 N 點透視位姿求解問題（PNP 問題）的方法來獲得三維坐標與二維坐標之間的變換，變換后完成相機與激光雷達的點云融合。將激光雷達與深度相機安裝與機器人頂部并通過比較選擇 ORB 算法作為機器人的 SLAM 方式，實驗結果表明融合后機器人能夠更有效地識別小體積障礙物，具有一定的應用前景。

關鍵詞： ROS 系統;深度相機;激光雷達;融合

中圖分類號： TP241.2;TB9文獻標志碼： A文章編號：1674–5124（2021）12–0120–04

Research on the fusion technology of camera and lidar based on ROS intelligent mobile robot

ZHANG Qingchun，HE Xiaoci，YAO Sheng，GUO Zhenjiu

（Automation Faculty， Huaiyin Institute of Technology， Huaian 223003， China）

Abstract： In order to improve the perception ability of intelligent mobile robots， an algorithm based on straight line and plane fitting is proposed for the fusion of intelligent robot camera and lidar， and it is applied to the ROS intelligent mobile robot system. Based on the PPB method， the camera image is subjected to distortion correctiontoobtainthetwo-dimensionalcoordinatesof thecornerpoints，andthepointcloudreflection intensity （PCI） methodisusedtoobtainthethree-dimensionalcoordinatesofthecornerpoints. The transformation between three-dimensional coordinates and two-dimensional coordinates is obtained by solving the N-point perspective pose problem （PNP problem）， and the point cloud fusion between the camera and the lidar is completed after the transformation. The lidar and depth camera are installed on the top of the robot， and the ORB algorithm is selected as the SLAM method of the robot through comparison. The experimental results show that the fusion robot can more effectively identify small-volume obstacles， which has certain application prospects.

Keywords： ROS system; depth camera; lidar; fusion

0引言

隨著科學技術的發展，機器人技術已廣泛應用于各個行業。相較于用于傳統工業的機器人，機器人領域的研究重點已經轉移到了智能移動機器人上，智能移動機器人的應用變得愈加廣泛，并能完成一部分人類的工作[1]。本文使用 ROS 系統，ROS 設計的主要目的是提高機器人研發中的代碼重用度，是一個分層架構。盡管 ROS 系統仍然存在一些缺點，但是對其系統體系結構的研究將對機器人的整體開發和設計帶來很大的幫助[2]。在智能移動機器人系統中，如何使用傳感器對獲得的數據進行更精確的映射，定位，感知和導航變得十分重要。自主移動機器人的傳感器技術是指通過攝像頭，激光雷達，毫米波雷達，超聲波雷達等傳感器與外界環境交互[3]。一種傳感器必定存在其自身的缺點，必須采取有效措施來杜絕其缺點，以便于機器人更好地完成環境感知的任務。

1系統方案及部分模塊選型

移動機器人可以自主導航也可以接受上位機控制移動，并在未知環境中實時構建地圖，移動機器人機箱中攜帶的微計算機和5G 模塊，機器人使用5G 信號從本地計算機接收控制命令，進而通過決策層微控制器配備的 Ubuntu操作系統攜帶的 ROS 框架，以計算每種算法并規劃路徑，并控制中間層（MCU），控制板卡將 PWM 擴展到驅動電機以使其旋轉，完成機器人的移動并在測試場景中前進、后退、轉彎等。通過激光雷達和深度相機傳感器獲取未知的環境信息和數據，最后由決策層的軟件系統獲取的環境數據進行處理，進一步進行路線軌跡，自主導航，地圖構建和地圖更新避障等操作。

該移動機器人的硬件系統由三部分組成。第一部分是底層，主要由電源、電機和各種傳感器組成。第二部分是中間層，中間層以 MCU 為主要控制器控制下層單元，并與上層交互。第三部分是上層， NVIDIA JESTON NANO 嵌入式智能開發板，可以接收下面的數據并發送控制命令，并實現人機交互。系統結構示意圖如圖1所示。

上層部分主控模塊選用英偉達 JETSON NANO 嵌入式智能開發板，激光雷達選擇 RPLIDAR A3激光雷達掃描測距儀，深度相機選擇英特爾 Intel RealSense 雙目深度攝像頭。

2相機與激光雷達數據融合

當前，多源傳感器的融合[4]是無人駕駛環境傳感的研究重點，必要將激光雷達和攝像頭融合起來，利用它們各自的優勢互相促進以改善系統環境感知的能力。相機數據和激光雷達數據的提取和矯正是進一步融合兩個傳感器的重要前提。

2.1相機圖像角點提取

采取多邊形平面板的方法，簡稱為PPB方法[5]來提取角點，在提取角度點的過程中，拍攝圖像并校正失真后，手動調整像素位置以直接選擇拐角點，對于角點標記的位置，程序會準確讀取角點坐標。

2.2激光雷達點云數據的提取

采用測量點云反射強度的方法來解決激光雷達點云數據的角點提取問題，激光雷達角點提取示意圖如圖2所示。

為了確定獲得的邊緣點之間互相的位置關系，可以使用點云數據精確地讀取每個點，并且可以使用該點的3D 坐標值計算其所屬的激光束的俯仰角，從而可以計算該點所屬的激光線的條數。假設所獲得的3D 點云數據中的一個點 P=（x，y，z），計算線束 n 的方法如下：

式中：Angle——激光線束的一個俯仰角且是由點的三維坐標所求得;

gene——比例因子且為一固定的數值;

n?所要求取的點 P 所位于的激光線束的條數。

可以求出每條激光線點云上與標定平面的交點或者距離平面幾何距離最小的點，并且可以獲得這些點的擬合直線，以獲得每個邊緣三維空間中的通用表達式。最后分別處理四個邊緣，并獲得3D 空間中兩條相鄰線的交點，即校準板和相機的對應四個角，傳統的方程組方法不一定能夠求得所需要的角點坐標，且由于調整后的直線不一定是空間中的共面直線因此需要使用別的方法來計算出所需的角點坐標。

2.3直線與平面擬合算法

為了能夠求出激光雷達數據的角點坐標，提出了一種直線與平面擬合的算法，具體方法如下：

知道空間直線的一般方程的情況下，設點 p1=（x1 ，y1 ，z1）和點p2=（x2 ，y2 ，z2）為直線L1上的兩點，點p3=（x3 ，y3 ，z3）和點p4=（x4 ，y4 ，z4）為直線L2上的兩點，則兩直線對應的方向向量分別表示為：

此時可以求得同時垂直于直線L1和L2的法向量：

根據空間解析幾何的原理可知，一條直線與直線外一點可以確定一個平面，所以借助直線L1上一個任意的點P1與直線的法向量 c 可以構造一個同時過直線L1和向量 c 的平面，記作：

通過式（5）所確定的平面與L2聯立一個線性方程組，可以求得一點Q1，記作Q1=（X1 ， Y1 ， Z1），該點則為平面與L2的交點。同樣的思想可以由直線L2上的一點p3與法向量 c 可以構造一個同時過直線L2和向量 c 的平面，記作：

通過式（6）所確定的平面與L1聯立一個線性方程組，可以求得一點Q2，記作Q2=（X2 ， Y2 ， Z2），則該點為平面與L1的交點。由于L1與L2為兩條互相異面的直線，則通過擬合所得到的直線交點估算為上面所求得的Q1與Q2的坐標平均值。

2.43D 與2D 投影的求解方法

在知道角點的3D 與2D 坐標之后，可以使用求解 PNP 問題的方法來求取它們之間的變換。

在軟件OpenCV3.4中，用solvePNP迭代法求解3維到2維的轉換中，設空間中的點為[X0 ， Y0 ， Z0 ， 1]T，二維圖像上的對應點為[x，y， 1]T，則三維點變換到二維點的求解原理為公式為：

其中，f1、f2、u0、v0是可以從相機參數中讀到的固定系數，那么才這個變換中就有 R11、R12、R13、R21、 R22、R23、R31、R32、R33、T1、T2、T3共12個未知數，角點的2D 和3D 結構提供了由X0 ， Y0 ， Z0 ， x，y確定的兩個方程。從數學方程式基本求解性質上可以得知6個未知數需要6個方程式聯立求解，具體算法流程圖如圖3所示。

2.5路徑規劃

本文在楊博等人[6]為了保障機器人安全的研究基礎上采用 A*算法將機器人 SLAM 構建出的地圖簡化成柵格地圖，地圖中的每一個柵格用0或1表示其特定的信息。若柵格的數值為1，則代表該區域可以通過;若柵格的數值為0，則代表該區域存在障礙物，機器人無法通過該單元格。在地圖中設置起點和終點，則機器人會找出從起點柵格到終點柵格將要經過的所有可以通過的柵格，這些柵格就是機器人起點到終點的行駛路徑。

在 A*算法中，搜索移動機器人當前位置所在位置的周圍節點，計算從起始點至當前位置周圍節點的實際值與該節點至目標位置和的最小值節點，將該節點作為機器人下一步運行方向。繼續重復進行上述過程，直到移動機器人到達目標位置，從而獲得從起始位置到目標位置的最終路徑[7]。

3實驗結果與分析

實驗使用基于 ROS 平臺的移動機器人。將激光雷達與深度相機安裝于機器人的頂端并且通過 USB 接口與決策層連接，采集到的數據直接由英偉達 JETSON NANO 嵌入式智能開發板的主處理器進行處理，使得機器人可以獨立進行環境感知。

3.1 SLAM 方法選擇

在本文中，構建了包含小障礙物的精確地圖，以提高檢測小障礙物的能力。使用Gmapping算法通過融合傳感器數據來構建地圖，該方法是一種有效的 Rao-Blackwellized粒子濾波算法，并且網格地圖是由傳感器使用該算法從周圍環境中獲取的數據構建而來。在 SLAM 中使用此粒子過濾時，每個粒子點代表一個單獨的環境圖[8]。找到什么樣的方法來減少粒子的數量成為了問題的關鍵，為了減少粒子數量，有Gmapping算法的研究人員提出了幾種算法包括 ORB 算法、SURF 算法以及 SIFT 算法[9]，通過對比算法平均匹配運行時間選出平均匹配時間最短的方法來進行接下來的數據融合實驗，實驗結果如表1所示。

通過表1可以看出本實驗中 ORB 算法所用的運行總時間以及平均匹配時間均少于另外兩種算法且精度也能達到要求，因此將使用 ORB 算法進行 slam 地圖構建。

3.2數據融合實驗

單線激光雷達無法掃描障礙物，此外還會嚴重影響移動機器人的地圖構建和導航避障，因此將融合單線激光和深度攝像頭的信息來構建地圖，該軟件平臺為 ROS。首先，使用 ROS 中的 PCL 庫將深度相機的3D 點云數據轉換為2D 點云數據，并將單線激光雷達中的Laserscan數據類型轉換為2D 點云，然后使用修改過的 IRA 工具包源代碼的 Laser 部分，使用它來合并深度相機和激光雷達數據，生成虛擬傳感器，然后轉換虛擬傳感器坐標，最后將融合到的數據流放入Gmapping包中。如圖4所示的實驗場景及其前后的結果，在融合之前無法用單線激光雷達在實驗場景中檢測到障礙物，融合后可以準確地檢測出低障礙物。

4結束語

為了使智能移動機器人能夠更好的進行環境識別，結合 PNP 問題求解方法，提出了一種基于直線與平面擬合的算法，完成了相機與激光雷達的融合。在實驗過程中采用Gmapping算法來構建網格地圖，大大地減少了Rao-Blackwellized粒子濾波的粒子數量，通過實驗選擇了Gmapping方法中運行時間較短且算法精度較高的 ORB 算法進行 slam，數據融合后成功地識別了較小障礙物，更好地完成了機器人的環境識別任務，具有一定的應用前景。

參考文獻

[1]陳志賢.面向復雜環境的服務機器人自主規劃方法研究[D].深圳：中國科學院大學， 2019.

[2]胡春旭. ROS 機器人開發實踐[M].北京：機械工業出版社，2018.

[3]王鵬飛.基于 ROS 的多傳感器信息融合自主導航控制系統設計與實現[D].南京：南京郵電大學， 2019.

[4] GROVES P D. GNSS 與慣性與多傳感器組合導航系統原理[M].練軍想， 唐康華， 譯.北京：國防工業出版社出版社， 2015：28-39.

[5]丁業峰.室內移動機器人激光雷達導航算法研究.[D]哈爾濱：哈爾濱工業大學， 2019.

[6]楊博， 張緩緩， 江忠順.智能車輛的避障路徑規劃與跟蹤控制仿真分析[J/OL].中國測試：1-8[2021-06-20]. http：//kns.cnki. net/kcms/detail/51.1714.TB.20201016.1753.015.html.

[7] GU J J， CAO Q X. Path planning for mobile robot in a 2.5- dimensionalgrid-basedmap[J]. IndustrialRobot： An International Journal， 2011（3）：315-321.

[8]曾奇.激光測距與視覺融合的室內導航系統研究[D].成都：電子科技大學， 2018.

[9] ZI J Y ， GIM HL .3DFeat-Net： weakly supervised local 3D features for point cloud registration[C]// European Conference on Computer Vision， 2018.

（編輯：徐柳）