基于Scan Context與NDT-ICP相融合的果園建圖方法研究*

耿麗杰，顧健，別曉婷，冉維旭，蘭玉彬, 2, 3

(1. 山東理工大學農業(yè)工程與食品科學學院，山東淄博，255000；2. 山東理工大學生態(tài)無人農場研究院，山東淄博，255000；3. 山東理工大學國際精準農業(yè)航空應用技術研究中心，山東淄博，255000)

0 引言

近年來，隨著國家對智慧農業(yè)的大力支持，中國首個無人農場落地，進一步推動了智能農業(yè)裝備技術的快速發(fā)展[1-2]。機器人技術隨著計算機、激光雷達等技術的進步，獲得了快速發(fā)展。即時定位與地圖構建(Simultaneous Localization and Mapping，SLAM)是機器人導航技術的關鍵技術之一，也是機器人自動導航的重要基礎[3]。傳感器技術的不斷發(fā)展，尤其是近年來三維激光雷達的快速發(fā)展，使得即時定位與地圖構建技術在建圖精度及魯棒性上有了很大程度的提升。根據(jù)傳感器類型可以將SLAM技術分為視覺SLAM與激光SLAM兩大類[4]。其中視覺SLAM借助視覺傳感器獲取環(huán)境信息，成本低、結構簡單，但運算量大，易受光照影響，不適用于農田等光照變化明顯的環(huán)境[5-6]。激光SLAM技術相對成熟，測距準確，受光照影響較小，且激光雷達相比相機、超聲波、紅外傳感器等具有抗干擾能力強、精度高、測量范圍廣等優(yōu)勢，更適用于果園環(huán)境[7-9]。目前，農業(yè)機器人搭載激光雷達實現(xiàn)局部定位導航的方法不能提供全局的定位信息[10]，非實時動態(tài)測量技術(Real Time Kinematic，RTK)全局定位的方法主要通過三維建圖以及重定位來實現(xiàn)。實現(xiàn)建圖的主要算法以雷達里程計和實時建圖(Lidar Odometry and Mapping in Real-time，LOAM)以及輕量級地面優(yōu)化的雷達里程計和實時建圖(Lightweight and Ground-Optimized Lidar Odometry and Mapping on Variable Terrain，LeGO-LOAM)為主。LOAM算法，缺少回環(huán)檢測與后端圖優(yōu)化，運行在果園復雜環(huán)境中精度缺失嚴重。在此基礎上LeGO-LOAM算法增加了回環(huán)檢測與圖優(yōu)化部分。然而，回環(huán)檢測[11-13]與圖優(yōu)化部分不可避免地引入了果園環(huán)境地圖誤匹配及累計誤差大等問題。

回環(huán)檢測，又稱閉環(huán)檢測，是機器人識別曾到達某場景，使地圖閉環(huán)的能力，通常使用3D描述符的匹配，如快速點特征直方圖(FPFH)、快速激光興趣區(qū)域變換(FLIRT)和方向直方圖簽名(SHOT)等算法[14-16]。若回環(huán)檢測成功，可以顯著減小累計誤差，幫助機器人更精準、快速地實現(xiàn)避障導航工作。因此，回環(huán)檢測成為近年來機器人研究領域中的熱點之一。隨著深度學習的發(fā)展，從點云圖像中可以很方便地獲取語義信息[17]，用于輔助SLAM回環(huán)檢測。例如基于3D點云分割匹配的方法SegMatch[18]，以及基于surf的方法SUMA++[19]，通過動態(tài)去除，在高速公路場景中實現(xiàn)了驚人的準確性。

除回環(huán)檢測外，點云配準也是機器人避障導航中的重要步驟[20]。常用三維激光雷達點云配準方法有兩種，迭代最近點(Iterative Closest Point，ICP)和正態(tài)分布變換(Normal Distributions Transform，NDT)[21-24]。其中，ICP算法配準速度較慢，但配準精度高。NDT算法配準速度較快但配準精度較低。

針對LeGO-LOAM算法引入的果園環(huán)境地圖誤匹配及累計誤差大等問題，本文使用履帶底盤搭建了軟硬件系統(tǒng)，在機器人操作系統(tǒng)(Robot Operating System，ROS)下，以基于Scan Context的回環(huán)檢測方法為基礎進行改進，加入更優(yōu)的NDT-ICP點云配準方法進行建圖。最后通過在葡萄園進行實地建圖試驗，驗證所提出建圖方法的性能。

1 果園環(huán)境地圖建圖軟硬件系統(tǒng)

果園環(huán)境地圖構建的移動機器人的軟硬件系統(tǒng)主要包括履帶式差速轉向移動底盤、嵌入式人工智能處理器、三維激光雷達、Linux操作系統(tǒng)、ROS機器人操作系統(tǒng)以及建圖算法。

1.1 移動機器人組成

圖1為移動機器人與機體坐標系{V}。

圖1 移動機器人與機體坐標系

移動機器人使用履帶式兩驅差速轉向運動底盤，定義滿足右手定則的機體坐標系{V}。以三維激光雷達中心為坐標原點O，x軸指向機器人的正前方，y軸平行于機器人的輪軸且指向左方，z軸垂直指向正上方。

1.2 硬件組成

果園環(huán)境地圖構建硬件系統(tǒng)主要由三維激光雷達(OS1-64)、裝有Ubuntu18.04操作系統(tǒng)的Jetson AGX Xavier。

OS1-64為一款64線數(shù)字激光雷達，安裝在移動底盤正上方，其水平視場角為360°，轉速為10～20 Hz，垂直視場角為±15.8°，垂直角分辨率為0.52°，水平角分辨率為0.09°，點云數(shù)量高達131萬點/s，最大測距為120 m，測距精度為±3 cm，內置6軸IMU，包括3軸陀螺儀和3軸加速度計，每秒采樣數(shù)1 000個。

1.3 軟件組成

系統(tǒng)的軟件模塊框圖如圖2所示。

圖2 軟件模塊框圖

首先，將3D Lidar點云圖編碼到Scan Context中。然后，計算每一環(huán)最大高度的均值，組成一個向量，記為Ring key。用Ring key構造KD Tree，進行快速地上層搜索。找到最相似的候選幀，而后才將少數(shù)候選幀與當前幀進行相似度評分，檢測是否回環(huán)。確認回環(huán)后將兩幀點云進行NDT-ICP匹配，將得到的矩陣作為約束進行圖優(yōu)化。

2 果園環(huán)境地圖建圖方法

果園環(huán)境地圖構建方法包括點云掃描與處理、SLAM回環(huán)檢測、點云配準以及圖優(yōu)化。

2.1 點云掃描

果園環(huán)境地圖構建中，首先進行環(huán)境掃描。OS1-64激光雷達安裝在履帶式移動底盤上，通過手動遙控，以0.5～1 m/s的速度在葡萄園中行進，三維激光雷達以10 Hz的頻率，錄制點云數(shù)據(jù)。

2.2 回環(huán)檢測方法

在獲得點云數(shù)據(jù)后，首先進行雷達點云幀數(shù)據(jù)掃描，進行環(huán)境比對。本文選用Scan Context進行回環(huán)檢測。

首先，對單幀果園點云數(shù)據(jù)進行區(qū)域分割。將單幀果園點云數(shù)據(jù)進行徑向與環(huán)向劃分，如圖3所示。假設，激光雷達射線掃描的最遠距離為Lmax，則徑向區(qū)域分辨率為2π/Ns、環(huán)向區(qū)域分辨率為Lmax/Nr。圖3中Pij表示第i環(huán)，第j扇區(qū)內的點云集，當前幀點云區(qū)域劃分表達式為

圖3 沿環(huán)向和徑向的點云塊劃分

(1)

式中：P——當前幀點云；

i——環(huán)序號；

j——扇序號；

Nr——環(huán)向區(qū)域數(shù)量；

Ns——徑向區(qū)域數(shù)量。

果園點云數(shù)據(jù)確定間隔劃分，遠離距離傳感器劃分區(qū)域比靠近距離傳感器劃分區(qū)域具有更大的果園物理面積。然而，兩者同等編碼到Scan Context的單個像素中。因此，Scan Context補償了由遠點的稀疏性引起的信息量不足，并將附近果園動態(tài)對象視為稀疏噪聲。

果園當前點云幀分區(qū)后，受果園內復雜環(huán)境影響，使用點云的最大高度為區(qū)域賦值，數(shù)學表達式為

(2)

式中：p——第i環(huán)，第j扇內的區(qū)域點云集坐標；

z(·)——點集p的z軸坐標值函數(shù)；

Φ(Pij)——Pij點云集中最大的z值。

最終果園點云數(shù)據(jù)的Scan Context可以表達為Nr×Ns的矩陣

I=(bij)∈Nr×Ns

(3)

其中，bij=Φ(Pij)。

(4)

式中：Iq——待匹配點云幀；

Ic——候選點云幀；

因果園中雷達視點發(fā)生變化，同一地點的點云幀將隨之發(fā)生變化，候選Scan Context的扇向量也可能會發(fā)生改變，為避免該現(xiàn)象引入更多未知狀況，本文遍歷計算點云扇偏移的Scan Context。則最佳匹配扇位移數(shù)n*與最短距離d(Iq,Ic)的數(shù)學表達式為

(5)

(6)

由于Scan Context距離計算量過大，無法對所有矩陣進行計算，本文使用Ring key，進行兩階段分層搜索。首先利用Ring key進行果園點云候選幀挑選，然后，對Scan Context矩陣進行計算。

通過使用環(huán)的L0范式‖ri‖0，函數(shù)ψ被賦予單個實值，數(shù)學表達式為

(7)

其中，ψ:ri→。

式中：ri——每一環(huán)的最大高度均值。

向量k第一個實值來自離LiDAR最近的圓環(huán)，隨后，實值按序來自下一個環(huán)。因此，Nr維向量Ring key數(shù)學表達式為

k=(ψ(r1),…,ψ(rNr))

(8)

將向量k使用KD Tree存儲。將待匹配點云幀的Ring key在KD Tree中查找n個相似值及其相應掃描索引。然后將選定的n個候選幀的Scan Context與待匹配點云的Scan Context計算距離值。滿足閾值的最短距離候選幀留下，表達式為

(9)

式中：c*——確定為回環(huán)位置的索引；

C——KD Tree中候選索引集合；

cn——第n個候選幀索引值；

Icn——選定的第n個候選幀；

τ——給定的閾值為0.5。

2.3 點云配準算法

為了得到匹配點云幀與回環(huán)點云幀的相對位姿矩陣，進行后端圖優(yōu)化，確認回環(huán)位置后，本文選用NDT-ICP將果園匹配點云幀與候選點云幀進行點云配準。

將果園匹配點云幀空間分為若干相同立方體，并滿足立方體內至少有5個點，每個匹配點云幀立方體內點的均值μ和協(xié)方差矩陣C數(shù)學表達式為

(10)

(11)

式中：m——匹配點云幀空間立方體的點云數(shù)；

k——點云序號，k=1，2，…，m；

果園匹配點云幀立方體中每個點位的概率密度

(12)

將匹配點云幀映射到回環(huán)檢測幀坐標系中，每個點映射變換后的正態(tài)分布

(13)

將每個點的概率密度相加，評估坐標變換參數(shù)數(shù)學表達式為

(14)

使用Hessian矩陣法優(yōu)化s(p)。然后，重新映射到回環(huán)檢測幀坐標系中，直到滿足收斂條件為止。優(yōu)化后的回環(huán)點集為Ic*′，匹配點集Iq。用E(R,t)來表示果園原點集Ic*′在變換矩陣(R，t)下與果園目標點集Iq之間的誤差。則求解最優(yōu)變換矩陣的問題轉化為滿足minE(R，t)的最優(yōu)解(R，t)。其中E(R，t)稱為目標函數(shù)，它表示兩個點集之間的差異程度。該目標函數(shù)表達式為

(15)

R——旋轉參數(shù)；

t——平移參數(shù)。

為了最小化目標函數(shù)，求解最優(yōu)變換矩陣，即R和t。首先，計算果園目標點集Ic*′中每一個點在Iq點集中的對應最近點。利用SVD分解求得旋轉參數(shù)R和平移參數(shù)t，使得變換矩陣E(R，t)最小。對點集Ic*′使用上一步求得的旋轉參數(shù)R和平移參數(shù)t，得到新變換點集Ic*″。如果變換后點集Ic*″與Iq點集滿足目標函數(shù)要求，即兩點集的平均距離小于給定閾值，則停止迭代計算；否則重新計算新點集Ic*″作為新點集Ic*′繼續(xù)迭代，直到滿足收斂條件為止，得到最優(yōu)解(R，t)與相對位姿矩陣。

3 試驗結果與分析

為驗證本文提出的基于Scan Context與NDT-ICP的果園環(huán)境建圖方法，使用標準化種植葡萄園環(huán)境數(shù)據(jù)集與KITTI公開數(shù)據(jù)集進行試驗。其中，果園環(huán)境數(shù)據(jù)集全長1 707.645 m，持續(xù)時間為1 167 s。KITTI數(shù)據(jù)集00序列，全長3 724.187 m，持續(xù)470 s。

3.1 回環(huán)檢測試驗

在果園回環(huán)檢測試驗中，對比了半徑搜索回環(huán)方式與ICP匹配算法相融合的算法以及Scan Context搜索回環(huán)方式與ICP匹配算法。試驗結果如圖4所示。其中圖4(a)與圖4(b)分別表示半徑搜索回環(huán)方式與Scan Context搜索回環(huán)方式的局部效果圖。

(a) 原算法局部放大圖

從圖4(a)可以看出，使用半徑搜索回環(huán)點云模糊，且植株行中離群點較多。從圖4(b)可以看出，Scan Context搜索回環(huán)對拐角處影響較大，圖4(b)拐角處明顯比圖4(a)輪廓清晰。

表1為兩種不同的回環(huán)搜索方式所檢測到的回環(huán)準確率結果，可以看出，在果園數(shù)據(jù)集環(huán)境下，使用Scan Context搜索回環(huán)檢測方式進行檢測的回環(huán)次數(shù)更多，因其環(huán)路檢測不受激光雷達視點變化的影響，所以在反向重訪和拐角等地方檢測環(huán)路。半徑搜索回環(huán)檢測的回環(huán)個數(shù)為196，Scan Context回環(huán)檢測的回環(huán)個數(shù)為261，回環(huán)數(shù)提高了33.2%。在KITTI數(shù)據(jù)集環(huán)境下，半徑搜索回環(huán)檢測的回環(huán)個數(shù)為42，Scan Context回環(huán)檢測的回環(huán)個數(shù)為51，回環(huán)數(shù)提高了21.4%。

表1 果園檢測回環(huán)準確率

3.2 點云匹配試驗

3.2.1 軌跡對比試驗

為了評估本文提出的基于Scan Context與NDT-ICP的建圖方法在KITTI公開數(shù)據(jù)集中效果，將其與基于Scan Context與ICP最終的建圖軌跡進行對比。

使用GNSS持續(xù)獲取經(jīng)緯度信息作為標準軌跡信息。由圖5可知基于Scan Context與NDT-ICP最終的建圖軌跡更加貼近標準軌跡。

圖5 軌跡對比圖

3.2.2 試驗指標

均方根誤差(Root-mean-square error，RMSE)用來衡量觀測值同真值之間偏差，可以表征建圖軌跡精準度，其值越小表明建圖軌跡距GNSS軌跡越近，數(shù)學表達式為

(16)

式中：X′——真實值；

Xa——測量值；

d——位姿數(shù)；

a——位姿序號，a=1,2,…,d。

標準差(Standard Deviation)用來衡量觀測值同真值之間偏差，能反映建圖軌跡誤差離散程度，其值越小表明建圖軌跡距GNSS軌跡越近，數(shù)學表達式為

(17)

表2為不同匹配方式下回環(huán)檢測方法的誤差評價，結果表明，NDT-ICP點云匹配方法優(yōu)于ICP點云匹配方法，點云建圖誤差率縮小至原來的80%。即本文提出的點云匹配優(yōu)化方法能大大減少果園環(huán)境點云建圖產生的累積漂移誤差。

表2 LeGO-LOAM系統(tǒng)誤差表

4 結論

本文針對在果園環(huán)境同時定位與建圖中誤匹配、累計誤差大等問題，提出了一種基于Scan Context與NDT-ICP相融合的果園建圖方法。并分別在KITTI數(shù)據(jù)集與果園環(huán)境中進行相關試驗。

1) 通過利用Ring key進行快速地上層搜索，得到少量候選幀，隨后對候選幀與當前幀進行相似度評分，通過兩階段搜索算法來有效地檢測回環(huán)以減少誤匹配；同時使用一種基于NDT粗配準與ICP精確配準融合的點云配準方法來降低建圖的累計誤差。

2) 在果園數(shù)據(jù)集環(huán)境下，半徑搜索回環(huán)個數(shù)為196，Scan Context回環(huán)個數(shù)為261，回環(huán)數(shù)提高了33.2%。

3) 在KITTI數(shù)據(jù)集環(huán)境下，半徑搜索回環(huán)個數(shù)為42，Scan Context回環(huán)個數(shù)為51，回環(huán)數(shù)提高了21.4%。本文提出的NDT-ICP點云匹配方法的各項誤差評價參數(shù)都小于ICP點云匹配方法的對應誤差評價參數(shù)。表明本文改進的系統(tǒng)具有足夠的回環(huán)數(shù)和低誤差率，可以基本滿足果園環(huán)境下點云建圖。