張青春 何孝慈 姚勝 郭振久

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

關(guān)鍵詞： ROS 系統(tǒng);深度相機(jī);激光雷達(dá);融合

中圖分類號： TP241.2;TB9文獻(xiàn)標(biāo)志碼： 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引言

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

1系統(tǒng)方案及部分模塊選型

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

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

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

2相機(jī)與激光雷達(dá)數(shù)據(jù)融合

當(dāng)前，多源傳感器的融合[4]是無人駕駛環(huán)境傳感的研究重點，必要將激光雷達(dá)和攝像頭融合起來，利用它們各自的優(yōu)勢互相促進(jìn)以改善系統(tǒng)環(huán)境感知的能力。相機(jī)數(shù)據(jù)和激光雷達(dá)數(shù)據(jù)的提取和矯正是進(jìn)一步融合兩個傳感器的重要前提。

2.1相機(jī)圖像角點提取

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

2.2激光雷達(dá)點云數(shù)據(jù)的提取

采用測量點云反射強(qiáng)度的方法來解決激光雷達(dá)點云數(shù)據(jù)的角點提取問題，激光雷達(dá)角點提取示意圖如圖2所示。

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

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

gene——比例因子且為一固定的數(shù)值;

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

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

2.3直線與平面擬合算法

為了能夠求出激光雷達(dá)數(shù)據(jù)的角點坐標(biāo)，提出了一種直線與平面擬合的算法，具體方法如下：

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

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

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

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

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

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

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

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

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

2.5路徑規(guī)劃

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

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

3實驗結(jié)果與分析

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

3.1 SLAM 方法選擇

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

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

3.2數(shù)據(jù)融合實驗

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

4結(jié)束語

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

參考文獻(xiàn)

[1]陳志賢.面向復(fù)雜環(huán)境的服務(wù)機(jī)器人自主規(guī)劃方法研究[D].深圳：中國科學(xué)院大學(xué)， 2019.

[2]胡春旭. ROS 機(jī)器人開發(fā)實踐[M].北京：機(jī)械工業(yè)出版社，2018.

[3]王鵬飛.基于 ROS 的多傳感器信息融合自主導(dǎo)航控制系統(tǒng)設(shè)計與實現(xiàn)[D].南京：南京郵電大學(xué)， 2019.

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

[5]丁業(yè)峰.室內(nèi)移動機(jī)器人激光雷達(dá)導(dǎo)航算法研究.[D]哈爾濱：哈爾濱工業(yè)大學(xué)， 2019.

[6]楊博， 張緩緩， 江忠順.智能車輛的避障路徑規(guī)劃與跟蹤控制仿真分析[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]曾奇.激光測距與視覺融合的室內(nèi)導(dǎo)航系統(tǒng)研究[D].成都：電子科技大學(xué)， 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.

（編輯：徐柳）