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

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

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

0 引言

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

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

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

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

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

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

1.1 移動機(jī)器人組成

圖1為移動機(jī)器人與機(jī)體坐標(biāo)系{V}。

圖1 移動機(jī)器人與機(jī)體坐標(biāo)系

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

1.2 硬件組成

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

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

1.3 軟件組成

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

圖2 軟件模塊框圖

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

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

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

2.1 點(diǎn)云掃描

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

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

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

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

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

(1)

式中：P——當(dāng)前幀點(diǎn)云；

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

j——扇序號；

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

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

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

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

(2)

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

z(·)——點(diǎn)集p的z軸坐標(biāo)值函數(shù)；

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

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

I=(bij)∈Nr×Ns

(3)

其中，bij=Φ(Pij)。

(4)

式中：Iq——待匹配點(diǎn)云幀；

Ic——候選點(diǎn)云幀；

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

(5)

(6)

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

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

(7)

其中，ψ:ri→。

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

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

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

(8)

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

(9)

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

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

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

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

τ——給定的閾值為0.5。

2.3 點(diǎn)云配準(zhǔn)算法

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

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

(10)

(11)

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

k——點(diǎn)云序號，k=1，2，…，m；

果園匹配點(diǎn)云幀立方體中每個點(diǎn)位的概率密度

(12)

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

(13)

將每個點(diǎn)的概率密度相加，評估坐標(biāo)變換參數(shù)數(shù)學(xué)表達(dá)式為

(14)

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

(15)

R——旋轉(zhuǎn)參數(shù)；

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

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

3 試驗(yàn)結(jié)果與分析

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

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

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

(a) 原算法局部放大圖

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

表1為兩種不同的回環(huán)搜索方式所檢測到的回環(huán)準(zhǔn)確率結(jié)果，可以看出，在果園數(shù)據(jù)集環(huán)境下，使用Scan Context搜索回環(huán)檢測方式進(jìn)行檢測的回環(huán)次數(shù)更多，因其環(huán)路檢測不受激光雷達(dá)視點(diǎ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)準(zhǔn)確率

3.2 點(diǎn)云匹配試驗(yàn)

3.2.1 軌跡對比試驗(yàn)

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

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

圖5 軌跡對比圖

3.2.2 試驗(yàn)指標(biāo)

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

(16)

式中：X′——真實(shí)值；

Xa——測量值；

d——位姿數(shù)；

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

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

(17)

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

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

4 結(jié)論

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

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

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點(diǎn)云匹配方法的各項(xiàng)誤差評價(jià)參數(shù)都小于ICP點(diǎn)云匹配方法的對應(yīng)誤差評價(jià)參數(shù)。表明本文改進(jìn)的系統(tǒng)具有足夠的回環(huán)數(shù)和低誤差率，可以基本滿足果園環(huán)境下點(diǎn)云建圖。