果園作業(yè)機(jī)器人的自主行間導(dǎo)航系統(tǒng)研究※

李會(huì)賓，韓 偉，史 云

（中國農(nóng)業(yè)科學(xué)院農(nóng)業(yè)資源與農(nóng)業(yè)區(qū)劃研究所/農(nóng)業(yè)農(nóng)村部農(nóng)業(yè)信息技術(shù)重點(diǎn)實(shí)驗(yàn)室，北京100081）

0 引言

我國作為水果生產(chǎn)大國，果樹種植、管理和果實(shí)采收等工序存在自動(dòng)化程度低、設(shè)備單一、勞動(dòng)強(qiáng)度大和生產(chǎn)成本高等問題。針對(duì)這一現(xiàn)狀，研究符合我國國情的果園機(jī)器人相關(guān)理論、開發(fā)適合于果園作業(yè)的機(jī)器人勢(shì)在必行［1］。現(xiàn)代化矮化密植果園是機(jī)器人非常具有應(yīng)用前景的領(lǐng)域。此類果園中，果樹以平行直線為行的標(biāo)準(zhǔn)方式進(jìn)行種植，果樹密度和果樹行寬度也滿足作業(yè)機(jī)器人自主行走的空間需求和激光雷達(dá)等多傳感器的應(yīng)用條件［2-3］。果園作業(yè)機(jī)器人自動(dòng)作業(yè)的關(guān)鍵是具備自主導(dǎo)航能力，即可以按照優(yōu)化的作業(yè)路徑自主行走并完成多類生產(chǎn)任務(wù)，如挖溝、施肥、噴灑殺菌劑和殺蟲劑、割草、采摘、運(yùn)輸?shù)热蝿?wù)。高精度高性能的自主導(dǎo)航系統(tǒng)能減少重復(fù)作業(yè)區(qū)和遺漏作業(yè)區(qū)的面積，提高機(jī)器人的作業(yè)質(zhì)量和效率［4］。

目前國內(nèi)外關(guān)于果園作業(yè)機(jī)器人的自主導(dǎo)航研究取得了眾多成果。根據(jù)導(dǎo)航使用的傳感器不同，可以將導(dǎo)航方式分為激光導(dǎo)航、GPS導(dǎo)航、SLAM導(dǎo)航、圖像導(dǎo)航和基于多傳感器融合的導(dǎo)航等。戚樹藤等使用激光雷達(dá)采集果樹位置信息并采用霍夫變換算法獲取導(dǎo)航路徑，根據(jù)機(jī)器人的航向偏差和橫向偏差控制機(jī)器人沿導(dǎo)航路徑自動(dòng)行走，但是單純采用霍夫變換算法獲取導(dǎo)航路徑，難以保證導(dǎo)航路徑的精度和穩(wěn)定性［5］。Underwood等首先制作果園地圖，然后結(jié)合隱馬爾科夫模型實(shí)現(xiàn)機(jī)器人的導(dǎo)航控制，但是該方法需要地圖數(shù)據(jù)庫實(shí)時(shí)更新，才能解決果樹外觀隨時(shí)間變化對(duì)定位精度影響的問題［6］。Bargoti等采用SLAM方法識(shí)別果園內(nèi)的人工路標(biāo)和粗壯的樹木進(jìn)行導(dǎo)航，該方法對(duì)果園樹干要求較高，樹干和周圍環(huán)境要有明顯的特征差異。由于果樹特征四季變化明顯，若實(shí)現(xiàn)多場(chǎng)景和多季節(jié)的導(dǎo)航需要多個(gè)地圖支撐，而地圖的生產(chǎn)需要大量的采集工作［7］。一些學(xué)者還采用RTK-BDS或RTK-DGPS的方式為果園機(jī)器人導(dǎo)航，該類導(dǎo)航方式嚴(yán)重依賴定位信息的質(zhì)量，在寬闊的大田環(huán)境中能滿足接收多衛(wèi)星信號(hào)的需求，但是在果園環(huán)境中，存在衛(wèi)星信號(hào)遮擋、多路徑效應(yīng)和射頻干擾等問題，因而GPS導(dǎo)航設(shè)備無法有效地用于導(dǎo)航［8-10］。圖像導(dǎo)航方式依靠機(jī)器人上的攝像頭實(shí)時(shí)獲取圖像，通過特征聚類、閾值分割或者路徑直線提取等方法獲取機(jī)器人相對(duì)于果樹行的位置和姿態(tài)，最后生成機(jī)器人運(yùn)動(dòng)和轉(zhuǎn)向的控制信號(hào)，該類方法需要果樹和地面背景特征對(duì)比明顯，但是果樹底部普遍生長(zhǎng)著高密的雜草，使得樹木和地面的交界產(chǎn)生偏移甚至無法檢測(cè)，同時(shí)該類方法對(duì)光照變化適應(yīng)性差，計(jì)算工作量大，不適用多時(shí)段下不同光照環(huán)境，也嚴(yán)重影響了導(dǎo)航的實(shí)時(shí)性和魯棒性［11-12］。一些學(xué)者將視覺和激光傳感器作為自主導(dǎo)航的主要傳感器，兩者結(jié)合使用使機(jī)器人自主導(dǎo)航和目標(biāo)檢測(cè)的魯棒性得到提升。Subramanian等研制了一種基于機(jī)器視覺和激光雷達(dá)引導(dǎo)的自主導(dǎo)航系統(tǒng)，并采用旋轉(zhuǎn)編碼器提供機(jī)器人轉(zhuǎn)向角度的反饋，導(dǎo)航系統(tǒng)可以引導(dǎo)機(jī)器人自動(dòng)通過直線和曲線路徑，然而單純依靠編碼器來獲取機(jī)器人的角度會(huì)存在累計(jì)誤差，是一個(gè)固有難題，難以為機(jī)器人提供精準(zhǔn)的角度控制［13］。

針對(duì)以上難題和現(xiàn)代矮化密植果園的導(dǎo)航需求，文章提出了基于激光雷達(dá)和編碼器融合的果園行間導(dǎo)航系統(tǒng)。該系統(tǒng)以中國農(nóng)業(yè)科學(xué)院自主研發(fā)的果園作業(yè)機(jī)器人裝備為驗(yàn)證平臺(tái)進(jìn)行實(shí)驗(yàn)研究，首先通過VLP16激光雷達(dá)設(shè)備實(shí)時(shí)獲取果園行場(chǎng)景的點(diǎn)云，然后分割出左右果樹行點(diǎn)云，之后通過隨機(jī)抽樣一致（RANSAC）算法、拓展卡爾曼濾波（EKF）算法設(shè)計(jì)直線優(yōu)化算法并逐步獲取準(zhǔn)確穩(wěn)定的果樹行直線，進(jìn)而獲取果園作業(yè)機(jī)器人相對(duì)于果園行中心位置的方向和位置偏移，最后采用基于差速運(yùn)動(dòng)學(xué)模型和純跟蹤控制方法設(shè)計(jì)果園自主導(dǎo)航算法及系統(tǒng)，控制機(jī)器人沿果園行中心位置自主行駛，當(dāng)行駛到果園行尾時(shí)，能夠按照調(diào)頭策略準(zhǔn)確進(jìn)入下一行繼續(xù)工作。

1 果園作業(yè)機(jī)器人行間自主導(dǎo)航方法

1.1 果園作業(yè)機(jī)器人架構(gòu)

1.1.1 硬件架構(gòu)

果園作業(yè)機(jī)器人如圖1和圖2所示，由動(dòng)力系統(tǒng)、傳感器供電系統(tǒng)、計(jì)算系統(tǒng)、傳感系統(tǒng)、通信系統(tǒng)以及燈光系統(tǒng)等硬件系統(tǒng)組成。（1）動(dòng)力系統(tǒng)包括2 kW的雙伺服電機(jī)和具有PID調(diào)節(jié)能力的驅(qū)動(dòng)器，具有5 h續(xù)航能力的動(dòng)力電池以及具有強(qiáng)通過性能差速履帶底盤，支持ROS速度控制模式，支持遙控器控制模式。（2）傳感器供電系統(tǒng)提供5～48 V多種電壓供電，保證多類型傳感器正常工作，物理支持所有設(shè)備一鍵啟動(dòng)和緊急停止。（3）計(jì)算系統(tǒng)包括提供導(dǎo)航計(jì)算的工控機(jī)、顯示一體機(jī)等，支持導(dǎo)航數(shù)據(jù)實(shí)時(shí)計(jì)算。（4）傳感系統(tǒng)包括定位相機(jī)，雙目相機(jī)，變焦識(shí)別相機(jī)，2個(gè)16線的VLP-16激光雷達(dá)，雙GPS天線，超聲波模組和碰撞檢測(cè)開關(guān)。（5）通信系統(tǒng)支持4G、Wi-Fi、數(shù)傳通信，支持大數(shù)據(jù)網(wǎng)絡(luò)交換。（6）燈光系統(tǒng)包括左右轉(zhuǎn)彎燈、大燈、工作指示燈等。

圖1 果園作業(yè)機(jī)器人內(nèi)視圖Fig.1 Inside view of orchard operation robot

圖2 果園作業(yè)機(jī)器人側(cè)視圖Fig.2 Side view of orchard operation robot

1.1.2 硬件工作原理

果園作業(yè)機(jī)器人硬件工作原理如圖3所示。果園作業(yè)機(jī)器人目前支持2種工作模式，分別是遙控模式和自主導(dǎo)航模式。遙控模式和自主導(dǎo)航模式都能夠通過數(shù)傳天線將控制信號(hào)下發(fā)給工控機(jī)，經(jīng)由工控機(jī)將控制信號(hào)轉(zhuǎn)化為電機(jī)轉(zhuǎn)速信號(hào)，并將其通過串口下發(fā)給左右電機(jī)驅(qū)動(dòng)器實(shí)現(xiàn)驅(qū)動(dòng)左右電機(jī)運(yùn)動(dòng)。將左右電機(jī)上的編碼器讀數(shù)和差速模型結(jié)合，計(jì)算出果園作業(yè)機(jī)器人的里程信息。傳感器供電系統(tǒng)中配置了緊急停止按鈕，用于實(shí)現(xiàn)果園作業(yè)機(jī)器人的安全駕駛控制。為了實(shí)現(xiàn)通信避障功能，與計(jì)算系統(tǒng)工控機(jī)相連的網(wǎng)絡(luò)交換機(jī)連接了雙碰撞條和超聲波。傳感系統(tǒng)的水平和傾斜雷達(dá)主要用于建圖和導(dǎo)航，定位相機(jī)和識(shí)別相機(jī)則用于獲取周圍的圖像信息。所有的數(shù)據(jù)都實(shí)時(shí)下發(fā)到網(wǎng)絡(luò)交換機(jī)，工控機(jī)可以根據(jù)IP地址獲取需要的數(shù)據(jù)。網(wǎng)絡(luò)交換機(jī)也連接在Wi-Fi端，用于數(shù)據(jù)的傳輸和通信。

圖3 果園機(jī)器人硬件工作原理Fig.3 Hardware working principle of orchard operation robot

1.2 行間自主導(dǎo)航系統(tǒng)軟件架構(gòu)

果園作業(yè)機(jī)器人上安裝了多個(gè)傳感器，在行間自主導(dǎo)航過程中，主要采用3D水平激光雷達(dá)來獲取果園行內(nèi)的點(diǎn)云，在具有兩側(cè)分布特征的果園行點(diǎn)云中，首先分割左右樹行的點(diǎn)云，然后采用RNSANC算法獲取代表兩側(cè)樹木的直線［14］，并通過里程計(jì)的瞬時(shí)變化信息和EKF算法對(duì)獲取的直線進(jìn)行濾波，以獲取波動(dòng)較小、較為準(zhǔn)確的兩側(cè)直線。獲取果樹行直線后，根據(jù)里程計(jì)實(shí)時(shí)計(jì)算果園作業(yè)機(jī)器人的運(yùn)動(dòng)方向和兩側(cè)直線的航向偏差和橫向偏差，并參照兩個(gè)偏差通過純跟蹤控制算法修正其位姿，使其向果園行尾直行。果園機(jī)器人在導(dǎo)航過程中，通過檢測(cè)自身前方的點(diǎn)云數(shù)量，根據(jù)調(diào)頭判斷閾值判斷其位置是否到達(dá)了行尾，當(dāng)機(jī)器人判斷出到達(dá)行尾時(shí)，系統(tǒng)根據(jù)計(jì)算出的果園行平均寬度確定拐彎半徑，并根據(jù)拐彎行駛的長(zhǎng)度和當(dāng)前的運(yùn)行速度計(jì)算拐彎所需的時(shí)間。自主導(dǎo)航系統(tǒng)采用該調(diào)頭策略使果園作業(yè)機(jī)器人進(jìn)入下一行，果園作業(yè)機(jī)器再重新通過激光雷達(dá)數(shù)據(jù)檢測(cè)下一行數(shù)據(jù)并進(jìn)行導(dǎo)航，重復(fù)執(zhí)行直至完成整個(gè)果園作業(yè)。果園行間自主導(dǎo)航流程如圖4所示。

1.2.1 RANSAC直線擬合算法

果園行直線獲取方法通常是在獲取當(dāng)前幀的果園行點(diǎn)云后，將該幀點(diǎn)云投影到世界坐標(biāo)系中，然后將該幀點(diǎn)云分割成左右樹行的點(diǎn)云集，再從左右樹行點(diǎn)云集中擬合出兩側(cè)果樹的直線。常用的獲取方法有RANSAC算法和霍夫變化算法，從橫向偏差、航向偏差和算法耗時(shí)等方面對(duì)2個(gè)算法進(jìn)行比較，RANSAC直線提取算法相比較霍夫變化算法雖然具有細(xì)微的橫向偏差，但是在較大行距的果園中導(dǎo)航精度更高且算法實(shí)時(shí)性也具有絕對(duì)優(yōu)勢(shì)［15］。

圖4 果園行間自主導(dǎo)航流程Fig.4 Inter-row autonomous navigation process of orchard

如圖5所示，RANSAC算法執(zhí)行步驟如下。

（1）將果園行激光點(diǎn)云當(dāng)前幀PtCi，根據(jù)左右閾值進(jìn)行分割，獲取左右2個(gè)果樹行點(diǎn)云，分別為左邊行點(diǎn)云集合PtCil和右邊行點(diǎn)云PtCir。

（2）由于矮化密植果園一般按照平行直線種植，該文中設(shè)定2條默認(rèn)的左右平行線Ll和Lr，分別為：

式（1）～（2）中，k指果園行直線的斜率，bl和br分別指Ll和Lr的截距。

（3）從PtCil和PtCir隨機(jī)選擇2點(diǎn)，由這2個(gè)點(diǎn)確定構(gòu)成直線的參數(shù)。

（4）根據(jù)閾值T，確定與直線L的幾何距離小于T的點(diǎn)云集D（L），即為L(zhǎng)的一致集。

（5）重復(fù)n次隨機(jī)選擇，得到直線L1，L2，…，Ln和相應(yīng)的一致集D（L1），D（L2），…，D（Ln）。

（6）確定最大一致集，最后將最大一致集里的點(diǎn)利用最小二乘法擬合成一條直線即為最佳直線L，利用PtCil和PtCir獲取的最佳直線分別為L(zhǎng)l和Lr，即獲取到最佳的kl，kr，bl和br。

圖5 RANSAC直線擬合算法示意圖Fig.5 Algorithm diagram of RANSAC straight line fitting

1.2.2 EKF直線濾波算法

為了減少噪聲對(duì)Lr和Ll結(jié)果準(zhǔn)確性的影響，該文借助EKF算法對(duì)已經(jīng)獲取的直線進(jìn)行濾波，生成較為穩(wěn)定的左右行直線結(jié)果。在算法實(shí)現(xiàn)中將里程計(jì)的瞬時(shí)信息應(yīng)用到預(yù)測(cè)和更新直線參數(shù)的計(jì)算中［2］。由于左右直線的獲取方法相同，該文對(duì)右直線的獲取過程進(jìn)行詳細(xì)介紹。在EKF算法中需要右直線的兩種結(jié)果，分別是第n次已經(jīng)預(yù)測(cè)的狀態(tài)Lr（n|n）和已經(jīng)更新的狀態(tài)Lr（n|n-1）。已經(jīng)預(yù)測(cè)的直線狀態(tài)關(guān)鍵在于直線的斜率和截距，計(jì)算公式為：

式（3）～（6）中，Δx，Δy，Δtha分別是果園作業(yè)機(jī)器人根據(jù)里程計(jì)確定的位置移動(dòng)和角度旋轉(zhuǎn)，K 代表卡爾曼的增益系數(shù)，kr（n|n），kr（n|n-1）和kr（n-1|n-1）分別是Lr（n|n），Lr（n|n-1）和Lr（n-1|n-1）直線的斜率，br（n|n），br（n|n-1）和br（n-1|n-1）分別是Lr（n|n），Lr（n|n-1）和Lr（n-1|n-1）直線的截距，kr和br是當(dāng)前的直線測(cè)量結(jié)果。

為了縮小預(yù)測(cè)結(jié)果和實(shí)際結(jié)果之間的差異，設(shè)定一個(gè)協(xié)方差矩陣∑k|k-1，這個(gè)協(xié)方差矩陣通過EKF算法得到。利用Mahalanobis平方距離d作為評(píng)估指標(biāo)，該距離的計(jì)算公式為：

如果d大于閾值，判斷Lr（n|n）是錯(cuò)誤的計(jì)算結(jié)果，相應(yīng)的EKF更新結(jié)果也會(huì)被忽略。

1.2.3 行間自主導(dǎo)航和行尾調(diào)頭策略

果園作業(yè)機(jī)器人自主導(dǎo)航的2個(gè)重要任務(wù)是行間引導(dǎo)和行尾轉(zhuǎn)向［16］。果園作業(yè)機(jī)器人在通過EKF算法獲取到樹行兩側(cè)較為精準(zhǔn)穩(wěn)定的直線，其目的在于自動(dòng)識(shí)別樹行。在左右路徑直線擬合后，可以獲取2條直線的中心位置和履帶車中心的方向角tha和距離d，根據(jù)這2個(gè)變量實(shí)現(xiàn)對(duì)果園作業(yè)機(jī)器人方向和運(yùn)動(dòng)的控制。果園作業(yè)機(jī)器人沿果樹行直行和調(diào)頭方式如圖6所示。

圖6 果園作業(yè)機(jī)器人自主導(dǎo)航直行和調(diào)頭過程Fig.6 Direct and head-adjusting processes in autonomous navigation of orchard operation robot

果園作業(yè)機(jī)器人在果樹行間自主導(dǎo)航的方式，表現(xiàn)為做“弓字型”運(yùn)動(dòng)。在自主導(dǎo)航過程中，果園作業(yè)機(jī)器人會(huì)根據(jù)EKF算法檢測(cè)到的左右直線，通過對(duì)比機(jī)器人位姿和果樹行中心位置的橫向和航向偏差，實(shí)時(shí)調(diào)整自身的運(yùn)動(dòng)角度和位移，并且進(jìn)行調(diào)頭判斷和調(diào)頭操作。機(jī)器人根據(jù)橫向偏移量l和偏移角度tha實(shí)時(shí)調(diào)整自身的位姿，該過程如圖

7所示，其中tha和l的計(jì)算公式為：

圖7 果園作業(yè)機(jī)器人沿行中心位置直行Fig.7 Warking straight along the row center of orchard operation robot

果園作業(yè)機(jī)器人通過改進(jìn)純追蹤算法來確定前視距離，最終將車體的位姿轉(zhuǎn)化為左右輪的速度，驅(qū)動(dòng)果園作業(yè)機(jī)器人沿果樹行中心位置行駛，這樣保障了果園作業(yè)機(jī)器人導(dǎo)航的準(zhǔn)確性，并為機(jī)器人的運(yùn)動(dòng)提供了較大且合理的運(yùn)動(dòng)空間。

果園作業(yè)機(jī)器人在自主導(dǎo)航過程中，存在2種運(yùn)動(dòng)狀態(tài)，沿著果樹行中心位置行走（行跟蹤）和到達(dá)行尾轉(zhuǎn)彎到下一行繼續(xù)行跟蹤。一般果園的樹行兩端的特征很明顯，即行頭和行尾都不存在果樹，因此在行頭和行尾存在一定的調(diào)頭空間，所以當(dāng)激光雷達(dá)檢測(cè)到樹行突然出現(xiàn)大塊空缺信息且穩(wěn)定存在的時(shí)候，可以判斷機(jī)器人確實(shí)在行兩端附近，需要執(zhí)行轉(zhuǎn)彎的動(dòng)作。果樹行尾調(diào)頭策略如下，首先果園作業(yè)機(jī)器人在果樹行內(nèi)行走的過程中，左右兩側(cè)存在較大數(shù)量的點(diǎn)云，且位于果園作業(yè)機(jī)器人的正前方；同時(shí)果園作業(yè)機(jī)器人會(huì)計(jì)算出果樹行的平均寬度和調(diào)頭半徑，當(dāng)果園作業(yè)機(jī)器人行進(jìn)到果園行尾后，果園作業(yè)機(jī)器人兩側(cè)和前方的點(diǎn)云數(shù)量將急劇減少，出現(xiàn)這一現(xiàn)象時(shí)，果園作業(yè)機(jī)器人會(huì)根據(jù)所需的調(diào)頭行走的距離和角速度確定調(diào)頭需要的時(shí)間并完成進(jìn)入下一行的操作。當(dāng)果園作業(yè)機(jī)器人調(diào)頭完畢后，會(huì)向前繼續(xù)行進(jìn)一段時(shí)間，保證其深入進(jìn)入下一行，然后繼續(xù)執(zhí)行行跟蹤。

2 實(shí)驗(yàn)結(jié)果及分析

2.1 實(shí)驗(yàn)場(chǎng)景及配置

為驗(yàn)證果園作業(yè)機(jī)器人自主導(dǎo)航系統(tǒng)的實(shí)際工作效果，在中國農(nóng)業(yè)大學(xué)工學(xué)院附近樹林區(qū)域模擬果園導(dǎo)航實(shí)驗(yàn)。由于該樹林樹木較為稀疏，不能滿足果園行的實(shí)際場(chǎng)景，所以需增強(qiáng)該片樹林區(qū)域的果園密集特征，即在樹與樹之間添加了交通錐桶代替樹木，如圖8所示。樹林種植行間距為3 m，壟長(zhǎng)15 m，機(jī)器人整體寬度為0.9 m，長(zhǎng)度為1.6 m。由于該區(qū)域調(diào)頭空間有限，只能選取圖中單樹行，以0.4 m/s的速度，進(jìn)行20次實(shí)驗(yàn)驗(yàn)證自主導(dǎo)航中的直行功能，并記錄實(shí)驗(yàn)數(shù)據(jù)。果園作業(yè)機(jī)器人在自主導(dǎo)航實(shí)驗(yàn)中，采用了1個(gè)16線VLP水平激光雷達(dá)，左右輪的1024線編碼器，Nvidia jetson tx2工控機(jī)，2個(gè)2 kW伺服電機(jī)和對(duì)應(yīng)的驅(qū)動(dòng)器。

圖8 果園作業(yè)機(jī)器人導(dǎo)航實(shí)驗(yàn)區(qū)域Fig.8 Navigation experiment area of orchard operation robot

2.2 行間自主導(dǎo)航結(jié)果及分析

2.2.1 果園行間原始數(shù)據(jù)

樹行中地面雜草叢生，兩側(cè)是樹木和交通錐桶，由于該場(chǎng)景樹木稀疏，交通錐桶用于模擬果園中的樹干。該場(chǎng)景的兩側(cè)樹木具備了類墻的特征，如圖9所示。圖10是在該場(chǎng)景下獲取的點(diǎn)云場(chǎng)景，其中紅色和白色的點(diǎn)表示果園作業(yè)機(jī)器人左右兩側(cè)的果樹點(diǎn)云和交通錐桶點(diǎn)云。根據(jù)圖10中點(diǎn)云的分布可以看出，這些點(diǎn)云在宏觀上保持了果樹和交通錐桶按一條直線排列的特征。根據(jù)點(diǎn)云所隱含的直線特征可以通過RANSAC算法提取出左右的果樹行直線，然后通過兩條果樹行直線計(jì)算出果園作業(yè)機(jī)器人的中心導(dǎo)航位置，引導(dǎo)果園作業(yè)機(jī)器人在果樹行內(nèi)行駛。

圖9 果園實(shí)驗(yàn)區(qū)域Fig.9 Experiment area of orchard rows

圖10 實(shí)驗(yàn)區(qū)域點(diǎn)云Fig.10 Pointcloud of experiment area

2.2.2 果園行間直線擬合

該文采取RANSANC算法進(jìn)行兩側(cè)果樹行直線提取實(shí)驗(yàn)，在通過RANSAC算法擬合直線前，首先通過直通濾波算法去除地面，然后將左右行點(diǎn)云進(jìn)行分割，在獲取的左右樹木點(diǎn)云中，使用RANSAC算法分別進(jìn)行直線提取，算法提取結(jié)果如圖11所示，2條綠色直線代表果樹行的兩側(cè)。RANSAC算法的實(shí)時(shí)性能滿足導(dǎo)航需求，但直線擬合結(jié)果在航向和橫向方向存在一定的偏差且波動(dòng)比較大，這主要是由于周圍樹林環(huán)境的復(fù)雜性導(dǎo)致的。

圖11 RANSAC直線擬合Fig.11 Straight line fitting by RANSAC

2.2.3 果園行間直線EKF濾波

為了減少RANSAC算法擬合的直線存在的橫向和航向偏差，提升左右行直線檢測(cè)的穩(wěn)定性，該文采取EKF算法對(duì)擬合結(jié)果進(jìn)行優(yōu)化，結(jié)果如圖12所示，紅色雙直線代表EKF算法優(yōu)化直線后的結(jié)果，該結(jié)果的穩(wěn)定性更好，擬合直線精度更高。將EKF算法直線優(yōu)化的結(jié)果和RANSAC直線擬合結(jié)果在橫向偏差和航向偏差方向進(jìn)行對(duì)比實(shí)驗(yàn)，實(shí)驗(yàn)過程中，兩個(gè)算法存在結(jié)果相似的情況，如圖13所示，也存在RANSCAC算法結(jié)果較差的情況，如圖14、圖15所示。實(shí)驗(yàn)中通過人工標(biāo)定出藍(lán)色真值結(jié)果，進(jìn)行對(duì)比，如表1所示。

圖12 果園行實(shí)驗(yàn)區(qū)域Fig.12 Experiment area of orchard rows

圖13 實(shí)驗(yàn)區(qū)域點(diǎn)云Fig.13 Pointcloud of experiment area

圖14 果園行實(shí)驗(yàn)區(qū)域Fig.14 Experiment area of orchard rows

圖15 實(shí)驗(yàn)區(qū)域點(diǎn)云Fig.15 Pointcloud of experiment area

表1 直線擬合精度對(duì)比Table 1 Accuracy comparison of linear fitting

續(xù)表1

為了更直觀地表示2種算法的差異性，分別從橫向偏差、航向偏差2個(gè)方面作圖比較。

圖16 橫向偏差對(duì)比圖Fig.16 Contradistinction of lateral deviation

圖17 航向偏差對(duì)比圖Fig.17 Contradistinction of course deviation

從表1與圖16和圖17中可以得知：（1）RANSAC擬合直線算法與EKF濾波算法在橫向偏差均值分別為0.188 5 m與0.103 m，標(biāo)準(zhǔn)差分別為0.086 6 m與0.048 m；（2）RANSAC擬合直線算法與EKF濾波算法在航向偏差均值分別為2.488°與1.036 5°，標(biāo)準(zhǔn)差分別為1.817°與0.83 °。

由對(duì)比結(jié)果可知，EKF算法相比較RANSAC算法在航向偏差均值上提升了58.34%，在航向標(biāo)準(zhǔn)差方面提升54.32%，在橫向偏差均值上提升45.36%，在橫向標(biāo)準(zhǔn)差方面提升44.57%。EKF算法相比較RANSAC算法在結(jié)果的精度上大幅提升，并且能夠獲取的橫向和航向偏差結(jié)果更加平滑，魯棒性更強(qiáng)。該精度在3～5 m行距的果園中，能夠滿足自主導(dǎo)航的需求。

2.2.4 果園作業(yè)機(jī)器人行尾調(diào)頭

由于樹林的實(shí)驗(yàn)場(chǎng)景不具備調(diào)頭的特征，所以通過交通錐桶圍成類果園行的場(chǎng)景，在該場(chǎng)景中進(jìn)行調(diào)頭實(shí)驗(yàn)。圖18為果園作業(yè)機(jī)器人的導(dǎo)航起點(diǎn)，圖19為果園作業(yè)機(jī)器人自動(dòng)導(dǎo)航進(jìn)入交通錐桶行，圖20為果園作業(yè)機(jī)器人進(jìn)行出行操作，圖21為果園作業(yè)機(jī)器人檢測(cè)到出行特征后根據(jù)行半徑和調(diào)頭時(shí)間進(jìn)調(diào)頭操作，如圖22～23為果園機(jī)器人進(jìn)入下一行開始下一行工作，依次循環(huán)直至完成果園遍歷，結(jié)束導(dǎo)航。該調(diào)頭模擬的過程中，果園作業(yè)機(jī)器人能夠準(zhǔn)確地獲取調(diào)頭的行尾位置，并順利完成調(diào)頭操作。

圖18 機(jī)器人起始位置Fig.18 Start position of the robot

圖19 機(jī)器人進(jìn)入行Fig.19 Moving in rows

圖20 機(jī)器人出行Fig.20 Moving out of the row

圖21 機(jī)器人開始調(diào)頭Fig.21 Turning

圖22 機(jī)器人進(jìn)入下一行Fig.22 Entering the next row

圖23 機(jī)器人繼續(xù)在下一行行駛Fig.23 Running in next row

3 結(jié)論

現(xiàn)代化矮化密植的果園中，果園作業(yè)機(jī)器人自主導(dǎo)航能力是在果園中完成多類任務(wù)的關(guān)鍵。果樹的標(biāo)準(zhǔn)化種植，使得果園形成一道道“果樹墻”，該特征能夠被三維激光雷達(dá)獲取。針對(duì)這種場(chǎng)景，該文提出了一種果園機(jī)器人行間自主導(dǎo)航系統(tǒng)，依靠RANSAC算法對(duì)激光雷達(dá)數(shù)據(jù)進(jìn)行直線擬合、通過EKF算法優(yōu)化檢測(cè)到的兩側(cè)樹行直線，然后根據(jù)果園作業(yè)機(jī)器人距離果樹行中心的橫向偏差和航向偏差，通過純跟蹤算法驅(qū)動(dòng)機(jī)器人自主行駛并能夠完成自主調(diào)頭工作。該系統(tǒng)通過真實(shí)的樹行數(shù)據(jù)驗(yàn)證表明，在具備現(xiàn)代矮化密植果園的環(huán)境中，獲取的引導(dǎo)直線的精度和調(diào)頭策略能夠滿足果園作業(yè)機(jī)器人在果園行中的導(dǎo)航需求。未來的工作將側(cè)重于在現(xiàn)代化果園中存在缺樹或者其它不規(guī)則場(chǎng)景，提出更加精準(zhǔn)和普適的算法輔助果園作業(yè)機(jī)器人自主導(dǎo)航，進(jìn)而提升果園作業(yè)機(jī)器人自動(dòng)化果園生產(chǎn)作業(yè)的能力。