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

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

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

0 引言

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

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

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

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

1 果園環境地圖建圖軟硬件系統

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

1.1 移動機器人組成

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

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

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

1.2 硬件組成

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

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

1.3 軟件組成

系統的軟件模塊框圖如圖2所示。

圖2 軟件模塊框圖

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

2 果園環境地圖建圖方法

果園環境地圖構建方法包括點云掃描與處理、SLAM回環檢測、點云配準以及圖優化。

2.1 點云掃描

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

2.2 回環檢測方法

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

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

圖3 沿環向和徑向的點云塊劃分

(1)

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

i——環序號；

j——扇序號；

Nr——環向區域數量；

Ns——徑向區域數量。

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

果園當前點云幀分區后，受果園內復雜環境影響，使用點云的最大高度為區域賦值，數學表達式為

(2)

式中：p——第i環，第j扇內的區域點云集坐標；

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

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

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

I=(bij)∈Nr×Ns

(3)

其中，bij=Φ(Pij)。

(4)

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

Ic——候選點云幀；

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

(5)

(6)

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

通過使用環的L0范式‖ri‖0，函數ψ被賦予單個實值，數學表達式為

(7)

其中，ψ:ri→。

式中：ri——每一環的最大高度均值。

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

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

(8)

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

(9)

式中：c*——確定為回環位置的索引；

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

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

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

τ——給定的閾值為0.5。

2.3 點云配準算法

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

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

(10)

(11)

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

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

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

(12)

將匹配點云幀映射到回環檢測幀坐標系中，每個點映射變換后的正態分布

(13)

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

(14)

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

(15)

R——旋轉參數；

t——平移參數。

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

3 試驗結果與分析

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

3.1 回環檢測試驗

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

(a) 原算法局部放大圖

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

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

表1 果園檢測回環準確率

3.2 點云匹配試驗

3.2.1 軌跡對比試驗

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

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

圖5 軌跡對比圖

3.2.2 試驗指標

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

(16)

式中：X′——真實值；

Xa——測量值；

d——位姿數；

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

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

(17)

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

表2 LeGO-LOAM系統誤差表

4 結論

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

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

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

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