基于 ROS 機(jī)器人的相機(jī)與激光雷達(dá)融合技術(shù)研究

張青春 何孝慈 姚勝 郭振久

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

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

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

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

移動(dòng)機(jī)器人可以自主導(dǎo)航也可以接受上位機(jī)控制移動(dòng)，并在未知環(huán)境中實(shí)時(shí)構(gòu)建地圖，移動(dòng)機(jī)器人機(jī)箱中攜帶的微計(jì)算機(jī)和5G 模塊，機(jī)器人使用5G 信號(hào)從本地計(jì)算機(jī)接收控制命令，進(jìn)而通過決策層微控制器配備的 Ubuntu操作系統(tǒng)攜帶的 ROS 框架，以計(jì)算每種算法并規(guī)劃路徑，并控制中間層（MCU），控制板卡將 PWM 擴(kuò)展到驅(qū)動(dòng)電機(jī)以使其旋轉(zhuǎn)，完成機(jī)器人的移動(dòng)并在測(cè)試場(chǎng)景中前進(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)建和地圖更新避障等操作。

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

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

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

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

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

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

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

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

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

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

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

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

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

2.3直線與平面擬合算法

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

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

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

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

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

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

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

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

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

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

2.5路徑規(guī)劃

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

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

3實(shí)驗(yàn)結(jié)果與分析

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

3.1 SLAM 方法選擇

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

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

3.2數(shù)據(jù)融合實(shí)驗(yàn)

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

4結(jié)束語(yǔ)

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

參考文獻(xiàn)

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

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

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

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

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

[6]楊博， 張緩緩， 江忠順.智能車輛的避障路徑規(guī)劃與跟蹤控制仿真分析[J/OL].中國(guó)測(cè)試：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]曾奇.激光測(cè)距與視覺融合的室內(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.

（編輯：徐柳）