基于SLAM的無人機(jī)飛行參數(shù)設(shè)定方法研究*

顧健，張榮昕，戴相龍，韓鑫，蘭玉彬, 2, 3，孔凡霞

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

0 引言

中國林業(yè)種植面積位居世界前列，然而落后的林業(yè)機(jī)械化水平無法滿足當(dāng)下林業(yè)生產(chǎn)需求。提高我國林木生產(chǎn)機(jī)械化與自動化成為亟待解決的問題。由于林間地理環(huán)境復(fù)雜，獲取高可靠度的林間作業(yè)環(huán)境，成為指導(dǎo)林間機(jī)械化、自動化設(shè)備作業(yè)的重要環(huán)節(jié)。多旋翼無人機(jī)高空作業(yè)[1]相對于無人車作業(yè)[2]進(jìn)行林間環(huán)境數(shù)據(jù)采集，具有很大的優(yōu)勢，因此成為林間環(huán)境采集主要工具。目前，林間無人機(jī)激光雷達(dá)掃描技術(shù)[3-5]是實(shí)現(xiàn)林間作業(yè)機(jī)械化、自動化、管理精細(xì)化的關(guān)鍵，是實(shí)現(xiàn)林間精準(zhǔn)作業(yè)中人機(jī)分離的基礎(chǔ)。相比于傳統(tǒng)的人工操作，該技術(shù)能夠有效解決林間復(fù)雜環(huán)境采集中的人力缺乏、勞動強(qiáng)度大、采集數(shù)據(jù)效果差、數(shù)據(jù)拼接時間長等問題。

激光雷達(dá)是近年來發(fā)展十分迅速的主動遙感技術(shù)[6-11]，在林木垂直結(jié)構(gòu)信息獲取方面具有其他遙感技術(shù)無可比擬的優(yōu)勢。很多學(xué)者提出了基于無人機(jī)機(jī)載三維激光雷達(dá)的諸多應(yīng)用，如基于無人機(jī)機(jī)載激光雷達(dá)點(diǎn)云數(shù)據(jù)的提取果樹單木樹冠信息系統(tǒng)[12]、使用機(jī)載激光雷達(dá)點(diǎn)云數(shù)據(jù)估測人工經(jīng)濟(jì)林單木參數(shù)[13]、使用激光雷達(dá)進(jìn)行作物葉面積指數(shù)定量反演[14]等。激光雷達(dá)點(diǎn)云數(shù)據(jù)在農(nóng)田環(huán)境障礙物檢測方面也具有重要的科研與應(yīng)用價值?；跉W式聚類的三維激光點(diǎn)云田間障礙物檢測方法[15]，使用無人機(jī)搭載激光雷達(dá)掃描儀以獲取機(jī)載點(diǎn)云，然后基于機(jī)載激光雷達(dá)點(diǎn)云提取樹木、電力塔、電力線等飛行障礙物[16]，為無人機(jī)安全飛行提供可靠數(shù)據(jù)。

但是，近年來在激光雷達(dá)應(yīng)用快速發(fā)展并呈現(xiàn)井噴趨勢的同時，在無人機(jī)激光雷達(dá)點(diǎn)云數(shù)據(jù)采集過程中采用不合理的飛行參數(shù)[17]，距植株飛行低于5 m、飛行速度大于10 m/s的情況時有發(fā)生，這不僅會造成采集激光雷達(dá)點(diǎn)云效果差、還會浪費(fèi)電池能量，降低點(diǎn)云數(shù)據(jù)利用率。因此植保無人機(jī)機(jī)載激光雷達(dá)的合理飛行參數(shù)的選擇與應(yīng)用，對行業(yè)發(fā)展有著重要意義。

由于缺少高效的點(diǎn)云數(shù)據(jù)質(zhì)量評價方法，導(dǎo)致機(jī)載激光雷達(dá)作業(yè)過程中飛行參數(shù)設(shè)定困難。原始數(shù)據(jù)的點(diǎn)云數(shù)據(jù)質(zhì)量評價方法主要包括基于航帶重疊區(qū)統(tǒng)計特征的點(diǎn)云數(shù)據(jù)相對精度評價方法、點(diǎn)云數(shù)據(jù)密度評價方法、點(diǎn)云數(shù)據(jù)覆蓋質(zhì)量評價方法。上述評價方法，在林間實(shí)際作業(yè)過程中，無法準(zhǔn)確全面評價林間無人機(jī)作業(yè)所需點(diǎn)云數(shù)據(jù)。因此，本文提出了一種基于同時定位與建圖(Simultaneous Localization and Mapping，SLAM)[18-20]的林間無人機(jī)參數(shù)設(shè)定方法。使用3D LiDAR采集林間點(diǎn)云數(shù)據(jù)信息進(jìn)行SLAM建圖，使用SLAM建圖軌跡與GNSS-RTK軌跡進(jìn)行對比分析，根據(jù)其均方根誤差，實(shí)現(xiàn)雷達(dá)點(diǎn)云數(shù)據(jù)質(zhì)量的評測和無人機(jī)的最佳飛行高度與速度的設(shè)定。

針對無人機(jī)上激光雷達(dá)在林間環(huán)境運(yùn)動的過程中會使點(diǎn)云產(chǎn)生運(yùn)動畸變，從而影響點(diǎn)云匹配正確率與SLAM建圖精度。本文使用SLAM算法中的LOAM算法，加強(qiáng)SLAM建圖過程中的位姿精度，提升建圖算法的效果。最終實(shí)現(xiàn)準(zhǔn)確評價激光雷達(dá)林間點(diǎn)云數(shù)據(jù)質(zhì)量，從而設(shè)定無人機(jī)最佳飛行參數(shù)。

1 林間環(huán)境建圖軟硬件系統(tǒng)

林間環(huán)境地圖構(gòu)建硬件系統(tǒng)主要包括，多旋翼無人機(jī)，嵌入式人工智能處理器，三維激光雷達(dá)，GNSS-RTK定位系統(tǒng)。軟件環(huán)境主要包括，Linux操作系統(tǒng)，ROS機(jī)器人操作系統(tǒng)以及LOAM建圖算法包。

1.1 無人機(jī)平臺組成

本試驗(yàn)所使用無人機(jī)為多旋翼無人機(jī)，定義滿足右手定則的機(jī)體坐標(biāo)系{V}。如圖1所示，以三維激光雷達(dá)中心為坐標(biāo)原點(diǎn)o，x軸平行于無人機(jī)的腳架且指向左方，y軸指向無人機(jī)的正前方，z軸垂直指向正上方。

圖1 植保無人機(jī)搭載激光雷達(dá)試驗(yàn)平臺Fig. 1 Plant protection UAV equipped with lidar test platform1.3D LiDAR 2.GNSS-RTK 3.多旋翼無人機(jī) 4.Jetson AGX Xavier

多旋翼無人機(jī)主要包括，動力模塊、控制模塊、機(jī)身模塊。動力模塊包括，4塊電池、8個電調(diào)、8只電機(jī)、8對螺旋槳?？刂颇K包括，主控器、PMU、LED 3個部分。機(jī)身模塊包括，控制板、機(jī)臂、腳支架。無人機(jī)的總體性能指標(biāo)如表1所示。

表1 無人機(jī)總體性能指標(biāo)Tab. 1 Overall performance index of UAV

1.2 系統(tǒng)硬件組成

林間環(huán)境地圖構(gòu)建系統(tǒng)主要有三維激光雷達(dá)(VLP-16)、裝有Ubuntu18.04操作系統(tǒng)的Jetson AGX Xavier嵌入式控制器。

VLP-16為一款16線機(jī)械激光雷達(dá)，安裝在無人機(jī)正下方，雷達(dá)水平視場角為360°，在幀頻率為5～20 Hz時水平角分辨率為0.1°～0.4°，垂直視場角為±15°，垂直角分辨率為2°，點(diǎn)數(shù)量高達(dá)30萬點(diǎn)/s，最大測距為100 m，測距精度為±3 cm，供電電壓為9～32 V，重量為830 g。

GNSS-RTK定位系統(tǒng)在固定解下的水平定位精度為±1 cm，該系統(tǒng)主要用于記錄無人機(jī)的實(shí)時軌跡，作為參考真值。

1.3 系統(tǒng)軟件組成

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

圖2 軟件模塊框圖Fig. 2 Software module block diagram

2 林間環(huán)境地圖建圖方法

林間環(huán)境地圖構(gòu)建方法主要框架由四部分組成即點(diǎn)云特征提取，里程計解算，雷達(dá)建圖，位姿變換整合。

2.1 林間環(huán)境點(diǎn)云特征提取

首先，為了提高林間點(diǎn)云配準(zhǔn)的效率，使用林間點(diǎn)云數(shù)據(jù)中提取出的特征點(diǎn)來代替完整林間數(shù)據(jù)幀。特征點(diǎn)提取通過曲率值來分類，曲率大于設(shè)定閾值為邊緣點(diǎn)，曲率小于設(shè)定閾值的點(diǎn)為平面點(diǎn)。曲率值計算數(shù)學(xué)表達(dá)式如式(1)所示。

(1)

式中：c——曲率值；

S——為點(diǎn)i周圍連續(xù)點(diǎn)的集合；

k——掃描周期；

L——雷達(dá)坐標(biāo)系；

i——第k幀點(diǎn)云中的點(diǎn)，i∈pk；

同時，對所取的點(diǎn)也有一定的限制，該點(diǎn)的周圍點(diǎn)盡量不被再取到，這樣可以使整體的特征點(diǎn)分布更加的平均，同時該點(diǎn)不能與雷達(dá)掃描束過于平行，這樣也會出現(xiàn)問題。

2.2 林間環(huán)境雷達(dá)里程計解算

圖3 時間節(jié)點(diǎn)圖Fig. 3 Time node graph

(2)

式中：dE——點(diǎn)到直線的距離；

j、l是k時刻點(diǎn)云中用來確定直線的點(diǎn)，i是k+1時刻點(diǎn)云中的點(diǎn)，j是i對應(yīng)的匹配點(diǎn)，l是點(diǎn)j周圍距離最小的特征點(diǎn)。

點(diǎn)到平面的距離數(shù)學(xué)表達(dá)式如式(3)所示。

(3)

式中：dH——點(diǎn)到面的距離；

i、j、m是k時刻點(diǎn)云中用來確定一個平面的點(diǎn)i是k+1時刻點(diǎn)云中的點(diǎn)，j是i對應(yīng)的匹配點(diǎn)，l是點(diǎn)m周圍距離最小的特征點(diǎn)。

當(dāng)獲取到dE和dH之后，需要最小化式(2)和式(3)的右邊部分，得到最小化的dE和dH，這樣就可以使用非線性優(yōu)化的方法來進(jìn)行求解了。

運(yùn)動估計中會通過非線性優(yōu)化的方式，獲得最小距離，從而計算出最優(yōu)位姿變換。

2.3 林間環(huán)境全局地圖構(gòu)建

由于上一步求解的是局部雷達(dá)觀測坐標(biāo)系下的結(jié)果，是為求解相鄰幀之間的變換，也就是TL，然而為了同時定位與建圖，需要求解的是全局坐標(biāo)系下的變換，也就是TW。因此，當(dāng)獲取了若干相鄰幀的姿態(tài)變換信息后，需要做的就是將其和全局地圖進(jìn)行匹配，并將其加入到全局地圖之中。

2.4 林間環(huán)境位姿變換整合

主要是將雷達(dá)里程計解算中得到的姿態(tài)信息和地圖構(gòu)建中得到的信息進(jìn)行位姿變換整合。并且通過rviz，方便觀看和處理。

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

為驗(yàn)證本文提出的基于同時定位與建圖的無人機(jī)林間環(huán)境飛行參數(shù)設(shè)定方法，使用采集的林間環(huán)境數(shù)據(jù)集進(jìn)行仿真試驗(yàn)。

3.1 激光雷達(dá)數(shù)據(jù)采集試驗(yàn)

試驗(yàn)區(qū)域?yàn)?30 m×10 m的大葉女貞樹地塊，株高6～7 m。進(jìn)行正交試驗(yàn)，試驗(yàn)高度為12 m、18 m、24 m，速度為2 m/s、4 m/s、6 m/s。

林間試驗(yàn)布置情況如圖4所示，測量地面長度130 m，設(shè)置于0 m處進(jìn)行起飛，在0～15 m處作為加速緩沖帶，在15 m處標(biāo)記點(diǎn)A，A點(diǎn)出達(dá)到試驗(yàn)速度，中間15～115 m處作為速度保持區(qū)，在115 m處標(biāo)記點(diǎn)B，開始降速，在最后的115～130 m處，作為減速區(qū)，在130 m處無人機(jī)下降，結(jié)束試驗(yàn)。

圖4 地面測量俯視圖Fig. 4 Ground survey top view

3.2 林間點(diǎn)云數(shù)據(jù)建圖分析

在林間環(huán)境中，此處展示了部分點(diǎn)云圖，無人機(jī)參數(shù)分別為高度12 m，速度2 m/s的數(shù)據(jù)集的點(diǎn)云圖，如圖5(a)所示；高度18 m，速度4 m/s的數(shù)據(jù)集的點(diǎn)云圖，如圖5(b)所示；高度24 m，速度6 m/s的數(shù)據(jù)集的點(diǎn)云圖，如圖5(c)所示。建圖試驗(yàn)結(jié)果：高度越高，速度越快的點(diǎn)云建圖漂移越大，且建圖效果越差，圖像模糊。其中12-2表示無人機(jī)的飛行高度為12 m，速度為2 m/s，18-4與24-6含義參照12-2。下文出現(xiàn)此種表達(dá)方式，參考此處。

圖6為飛行軌跡與建圖軌跡對比圖，飛行軌跡為GNSS-RTK記錄的軌跡，建圖軌跡為本文使用的LOAM算法生成的軌跡圖。此處展示了無人機(jī)參數(shù)分別為高度12 m，速度2 m/s的數(shù)據(jù)集的軌跡對比圖，如圖6(a)所示；高度18 m，速度4 m/s的數(shù)據(jù)集的軌跡對比圖，如圖6(b)所示；高度24 m，速度6 m/s的數(shù)據(jù)集的軌跡對比圖，如圖6(c)所示。

(a) 12-2點(diǎn)云圖

(b) 18-4點(diǎn)云圖

(c) 24-6點(diǎn)云圖圖5 激光雷達(dá)建圖Fig. 5 Lidar mapping

(a) 12-2軌跡對比圖

(b) 18-4軌跡對比圖

(c) 24-6軌跡對比圖圖6 軌跡對比Fig. 6 Track comparison

3.3 林間點(diǎn)云建圖數(shù)據(jù)分析

均方根誤差(Root mean square error，RMSE)用來衡量觀測值同真值之間的偏差，可以表征建圖軌跡的精準(zhǔn)度，其值越小表明林間環(huán)境雷達(dá)點(diǎn)云數(shù)據(jù)建圖軌跡距GNSS-RTK軌跡越近，飛行參數(shù)的設(shè)定值越優(yōu)。其數(shù)學(xué)表達(dá)式如式(4)所示。

(4)

式中：n——位姿數(shù)；

a——位姿點(diǎn)序號；

Xa——林間環(huán)境點(diǎn)云建圖軌跡點(diǎn)位姿點(diǎn)值；

X′——GNSS-RTK軌跡位姿點(diǎn)值。

表2 軌跡對比結(jié)果Tab. 2 Track comparison results

圖7 建圖軌跡均方根誤差分布Fig. 7 Mapping trajectory RMSE distribution

速度作為變量時，可以看出點(diǎn)云數(shù)據(jù)建圖軌跡的均方根誤差分別與飛行速度成正相關(guān)。說明在一定高度的情況下無人機(jī)的飛行速度對點(diǎn)云采集的質(zhì)量有明顯的影響，速度越慢，建圖軌跡精度越高，與GNSS-RTK軌跡越是接近，離散程度越低，點(diǎn)云采集的質(zhì)量越高。同時在實(shí)際運(yùn)用當(dāng)中飛行速度與工作面積有關(guān)，由于無人機(jī)續(xù)航問題，為滿足在續(xù)航時間內(nèi)完成作業(yè)，盡量地降低無人機(jī)的飛行速度，從而提高激光雷達(dá)數(shù)據(jù)采集的質(zhì)量。

高度作為變量時，可以看出點(diǎn)云數(shù)據(jù)建圖軌跡的均方根誤差分別與飛行高度成正相關(guān)。說明在一定速度的情況下無人機(jī)的飛行高度對點(diǎn)云采集的質(zhì)量有明顯的影響，高度越低，建圖軌跡精度越高，與GNSS-RTK軌跡越是接近，離散程度越低，點(diǎn)云采集的質(zhì)量越高。同時在實(shí)際運(yùn)用當(dāng)中飛行高度與植株和障礙物的高度有關(guān)，本文試驗(yàn)的場景植株高度在6～7 m，為保證無人機(jī)飛行安全，并且植保無人機(jī)風(fēng)場會吹動植株，導(dǎo)致植株晃動，會影響點(diǎn)云收集的質(zhì)量，水平飛行時要離植株障礙物5 m左右。所以要在合理高度范圍內(nèi)盡量的降低無人機(jī)的飛行高度。

圖8 建圖軌跡均方根誤差分布Fig. 8 Mapping trajectory RMSE distribution

4 結(jié)論

本文針對在林間環(huán)境中點(diǎn)云數(shù)據(jù)質(zhì)量難以直接評價導(dǎo)致的無人機(jī)機(jī)載激光雷達(dá)飛行參數(shù)設(shè)定不合理的問題，提出了一種基于同時定位與建圖的無人機(jī)林間環(huán)境飛行參數(shù)設(shè)定方法。對三維激光雷達(dá)所采集的林間點(diǎn)云數(shù)據(jù)進(jìn)行建圖，將建圖軌跡與GNSS-RTK數(shù)據(jù)軌跡進(jìn)行對比分析，評價點(diǎn)云數(shù)據(jù)的質(zhì)量，并根據(jù)其均方根誤差對無人機(jī)的最佳飛行高度與速度參數(shù)進(jìn)行設(shè)定。在林間數(shù)據(jù)集環(huán)境中進(jìn)行相關(guān)試驗(yàn)，主要結(jié)論匯總?cè)缦隆?/p>

1) 飛行速度固定時，機(jī)載激光雷達(dá)飛行軌跡的均方根誤差與飛行高度成正相關(guān)。高度越低林間建圖軌跡精度越高，與GNSS-RTK所獲得的參考軌跡越接近，且離散程度越低，林間點(diǎn)云采集的質(zhì)量越高。

2) 飛行高度固定時，機(jī)載激光雷達(dá)飛行軌跡的均方根誤差與飛行速度成正相關(guān)。速度越低，林間建圖軌跡精度越高，與GNSS-RTK所獲得的參考越接近且離散程度越低，林間點(diǎn)云采集的質(zhì)量越高。

3) 根據(jù)本文試驗(yàn)數(shù)據(jù)、電池容量及任務(wù)需求綜合考慮，林間環(huán)境數(shù)據(jù)采集無人機(jī)的最佳飛行參考參數(shù)為：平均飛行速度2 m/s，平均飛行高度12 m。此時，均方根誤差可接近1.262 m。