鄭 路 張 嘯 王建國 吳 悅 李海濤

(中國農(nóng)業(yè)大學(xué)工學(xué)院,北京 100083)

0 引言

隨著智慧農(nóng)業(yè)的不斷發(fā)展,農(nóng)業(yè)機器人作為解決農(nóng)村務(wù)農(nóng)人口驟減、農(nóng)業(yè)人力成本上漲的關(guān)鍵技術(shù)而備受關(guān)注[1]。高精度定位與導(dǎo)航是農(nóng)業(yè)機器人進行自主田間作業(yè)的先決條件,在導(dǎo)航作業(yè)中,機器人首先需確定自身的絕對或相對位置以進行后續(xù)路徑規(guī)劃和軌跡追蹤等各項工作[2-3]。

絕對位置多指機器人所在位置的全球定位系統(tǒng)坐標(biāo),其獲取主要依靠全球?qū)Ш叫l(wèi)星系統(tǒng)(Global navigation satellite system,GNSS)。隨著自動駕駛行業(yè)的蓬勃發(fā)展,搭載載波相位差分技術(shù)(Real time kinematic,RTK)的定位系統(tǒng)協(xié)同以卡爾曼濾波為代表的濾波器可將定位精度維持在分米級范圍,足以滿足田間作業(yè)需求[4-5]。相對位置是指作業(yè)過程中機器人相對于作物或障礙物等標(biāo)志的位置,對相對位置的獲取主要依靠以激光雷達、毫米波雷達和視覺系統(tǒng)等為主的多種傳感器,在條件允許的情況下定位精度可達厘米級甚至毫米級[6-8]。

位置信息得以獲取后,對農(nóng)業(yè)機器人作業(yè)路徑進行合理規(guī)劃是決定農(nóng)業(yè)機器人能否順利完成相關(guān)作業(yè)的另一重要環(huán)節(jié)。根據(jù)對所需信息和適用場景的不同,田間作業(yè)路徑又分為全局靜態(tài)路徑和局部動態(tài)路徑[9]。全局靜態(tài)路徑的規(guī)劃主要依托作業(yè)區(qū)域特征、作物種植特征等現(xiàn)有信息,以機器人運行總里程最少、路徑重復(fù)率最低為原則對田間作業(yè)道路進行靜態(tài)規(guī)劃,且規(guī)劃后路徑信息無法實時改變[10-11]。局部動態(tài)路徑的規(guī)劃依托相對位置信息,在一定區(qū)域以不與障礙物發(fā)生碰撞為前提規(guī)劃出最優(yōu)路徑,可減小環(huán)境中存在的未知障礙物對機器人作業(yè)的影響[12-13]。

針對田間作業(yè)環(huán)境,FAUST等[14]提出一種分層式機器人路徑規(guī)劃方法PRM-RL,通過強化學(xué)習(xí)代理完成短程障礙躲避的任務(wù),然后利用基于采樣的規(guī)劃器映射這些代理從而進行可靠的路徑規(guī)劃。JENSEN等[15]提出一種農(nóng)業(yè)作業(yè)中涉及田內(nèi)和田間運輸?shù)膯卧窂揭?guī)劃方法,結(jié)合時間或行駛距離的優(yōu)化準(zhǔn)則,生成一個“度量地圖”并通過Dijkstra算法解決了單源路徑規(guī)劃問題。魏爽等[16]提出了農(nóng)機自動導(dǎo)航路徑搜索方法和預(yù)瞄點搜索的純追蹤模型,可以得到較好的導(dǎo)航與路徑追蹤控制精度。劉剛等[17]提出了一種基于GNSS 的農(nóng)田平整自動導(dǎo)航路徑規(guī)劃方法,將全局路徑規(guī)劃和局部路徑規(guī)劃相結(jié)合進而實現(xiàn)路徑規(guī)劃。上述的路徑規(guī)劃方法在全局靜態(tài)路徑和局部動態(tài)路徑規(guī)劃上進行了區(qū)分,位置信息僅應(yīng)用于對應(yīng)路徑的規(guī)劃,無法將兩者進行融合。由于全局路徑對高精度測繪信息即絕對位置信息的依賴較大,測繪精度將直接影響規(guī)劃得到的路徑質(zhì)量,當(dāng)機器人位于田間作物行間特殊位置時,局部路徑規(guī)劃可能會導(dǎo)致機器人運行方向發(fā)生錯誤,造成重復(fù)作業(yè)甚至迷失方向的問題。因此,全局靜態(tài)路徑需要進行實時修正,局部動態(tài)路徑需要指明每一時刻的運動趨勢,通過對位置信息的交叉利用來保障機器人田間作業(yè)的正常運行。

本文提出 “全局+局部”的宏微融合路徑規(guī)劃模式。全局靜態(tài)路徑規(guī)劃為機器人運行指明關(guān)鍵方向與整體運動趨勢,動態(tài)規(guī)劃依托激光雷達對作業(yè)環(huán)境進行實時感知,通過對作業(yè)行與作物特征點的提取進行相對定位,配合一定的修正系數(shù)確定機器人行駛行兩側(cè)作物的相對位置并規(guī)劃局部動態(tài)路線。通過對兩種路徑的實時融合可避免因種植規(guī)范化程度低,測繪誤差大等因素所帶來的靜態(tài)規(guī)劃路線偏差較大的問題,為機器人指明運動方向并保障運動趨勢的正確性。最后,基于模型預(yù)測控制(Model predictive control,MPC)算法實現(xiàn)對機器人運行路徑的實時追蹤。

1 全局靜態(tài)路徑規(guī)劃

1.1 絕對位置(經(jīng)緯度)坐標(biāo)獲取

絕對位置的精確獲取是實現(xiàn)精準(zhǔn)作業(yè)的先決條件,包括路徑追蹤在內(nèi)的多個環(huán)節(jié)均對位置精度有較高要求。不同于海拔落差幅度較大的丘陵山地,大田作業(yè)地面平整度較高,因此海拔變化可忽略,僅保留經(jīng)度與緯度作為絕對位置。在忽略海拔變化的情況下,絕對位置坐標(biāo)表現(xiàn)為由經(jīng)度和緯度所組成的二維坐標(biāo)。組合導(dǎo)航作為在自動駕駛中常用的導(dǎo)航方式,被廣泛應(yīng)用于各種搭載定位系統(tǒng)的載具上,主要指利用GNSS和慣性測量單元(Inertial measurement unit,IMU)為代表的多種傳感器進行松/緊/深組合,通過不同傳感器之間的相互配合,獲得更為精準(zhǔn)的載體絕對位置信息,并由計算機對載具進行控制以完成制導(dǎo)[18]。GNSS頻率較低,但定位長期精度高,基本不受地域、時間限制,在空曠無遮擋,載體運動平穩(wěn)的理想環(huán)境下具有較好的穩(wěn)定性,可以提供可信度較高的位置信息[19],在引入RTK技術(shù)后理論定位精度可達厘米級[20]?？紤]到農(nóng)業(yè)作業(yè)場景的特殊性,農(nóng)業(yè)機器人常工作在遮擋較少的開闊環(huán)境,多徑效應(yīng)和材質(zhì)反射所帶來的影響更小,對RTK等定位設(shè)備更為友好[21]。IMU頻率高,短期累計誤差小,可獲取短時間內(nèi)較為準(zhǔn)確的位置信息。

在自動駕駛領(lǐng)域,兩種傳感器進行融合后通過頻率特性互補可以在一定程度上降低定位誤差,從而提升定位精度。何秀鳳等[22]提出了一種基于UD分解的快速卡爾曼濾波算法,給出了IMU/GPS組合系統(tǒng)的軟硬件設(shè)計和試驗結(jié)果,相較于單一傳感器定位,融合定位精度大幅提升。張百強等[23]設(shè)計了基于Rao-Blackwellised無跡卡爾曼濾波(RB-UKF)的組合導(dǎo)航算法,顯著提升了組合導(dǎo)航系統(tǒng)GPS信號中斷再恢復(fù)后的濾波精度,且易于工程實現(xiàn)。經(jīng)試驗驗證,利用現(xiàn)有組導(dǎo)算法足以滿足農(nóng)業(yè)作業(yè)精度需求。

1.2 作業(yè)區(qū)域路徑規(guī)劃

1.2.1路徑規(guī)劃范圍確定

組合導(dǎo)航大大降低了定位誤差,可以使機器人獲取到滿足作業(yè)要求的絕對位置信息。為明確機器人全局靜態(tài)路徑規(guī)劃的區(qū)域范圍,本文面向二維平面內(nèi)田間作業(yè)進行全局靜態(tài)路徑規(guī)劃,不考慮地勢變化對作業(yè)的影響。

圖1為作業(yè)平面及作業(yè)區(qū)域特征點選取示意圖,首先對作業(yè)區(qū)域特征點進行選取,并通過多種測繪手段相結(jié)合的方式,再對作業(yè)區(qū)域特征點的經(jīng)緯度信息進行獲取和確認,進而構(gòu)建多邊形作業(yè)區(qū)域并確認機器人作業(yè)范圍。

圖1 作業(yè)區(qū)域特征點及作業(yè)平面

受田間道路鋪設(shè)、灌溉系統(tǒng)搭建等因素的影響,在實際農(nóng)田中,作物種植區(qū)域周邊往往會存在一圈非種植區(qū)域,如圖2所示,此區(qū)域無法承載機器人的作業(yè)路徑。為此,考慮到地圖精度和潛在誤差,需將作業(yè)范圍劃分為種植區(qū)和非種植區(qū)兩部分[24]。如圖3所示,其中黃色區(qū)域為非種植區(qū),綠色區(qū)域為種植區(qū)。實際農(nóng)田中非種植區(qū)內(nèi)外兩邊的距離或內(nèi)外頂點間的距離多為定值,在路徑規(guī)劃中可作靜態(tài)參數(shù)直接參與規(guī)劃運算。通過定義“種植區(qū)域特征點”,即圖3中紅色點,與作業(yè)區(qū)域特征點相對應(yīng),最終確定機器人在種植區(qū)作業(yè)的全局靜態(tài)路徑規(guī)劃范圍。

圖2 實際農(nóng)田情況

圖3 種植區(qū)域與非種植區(qū)域

1.2.2路徑規(guī)劃安全區(qū)域確定

作業(yè)規(guī)劃區(qū)域范圍內(nèi)作物有序種植,種植行列之間均留有一定的間隙,可利用此間隙對路徑規(guī)劃范圍進行細分。以玉米大田為例,在種植區(qū)內(nèi),依托于北斗衛(wèi)星定位系統(tǒng)進行的精準(zhǔn)播種使得玉米行距和株距得以保持在一個特定值上下小幅度波動。因此種植區(qū)內(nèi)每列玉米可以近似擬合為矩形區(qū)域,如圖4中深綠色矩形所示。矩形長邊矢量方向即每列玉米與坐標(biāo)軸的夾角在不同種植要求的農(nóng)田中差別較大,需要通過參考點給出。每兩行玉米之間的距離稱為玉米種植行距。圖4對玉米大田進行了模擬,由種植區(qū)和矩形區(qū)域選擇出來的淺綠色部分即為機器人可以正常通過的安全區(qū)域。為保證機器人工作時對作物的影響或破壞程度最小,機器人行駛路徑必須限制在安全域內(nèi)。

圖4 玉米大田模型

機器人未工作時,假設(shè)其位于種植區(qū)域左下角點,該點可作為機器人運動和路徑規(guī)劃的出發(fā)點,在進行田間作業(yè)時,需要明確的主要參數(shù)為機器人的行駛方向和速度,綜合考慮作業(yè)精度和作物損傷程度,機器人應(yīng)沿矩形長邊延伸方向行駛,且在直線行駛過程中速度可近似認為是確定值。因此,路徑規(guī)劃應(yīng)結(jié)合作物矩形區(qū)域特征,通過給出機器人行駛方向,進而確定在圖4所示農(nóng)田中作業(yè)時的行駛軌跡,為保證作業(yè)效率,避免重復(fù)作業(yè)造成的能源浪費等問題,最佳行駛路徑重復(fù)率為0,即路徑中沒有二次經(jīng)過的區(qū)段,這也是將機器人運動和規(guī)劃的出發(fā)點定在左下角點的原因之一。

1.2.3全局靜態(tài)路徑規(guī)劃

在圈定機器人行駛的安全區(qū)域后,需對全局靜態(tài)路徑進行規(guī)劃。規(guī)劃原理為在安全區(qū)域內(nèi),標(biāo)定種植方向后在垂直方向上衍生一定數(shù)量的等距直線。如圖5所示,等距直線與種植區(qū)邊界會存在多個交點,這些點便是軌跡引導(dǎo)點,且軌跡引導(dǎo)點應(yīng)出現(xiàn)在安全區(qū)域內(nèi)。對點進行順序選擇后,利用折線連接即可得到規(guī)劃后的路徑。

圖5 等距直線與邊界線交點

種植方向決定了規(guī)劃后路徑直行段的方向,且種植區(qū)形狀同樣會影響規(guī)劃效果,如圖6所示,等距直線與種植區(qū)邊界線相交出現(xiàn)交點,對于種植區(qū)形成的多邊形而言,如果為凸多邊形,會與直線存在一個或兩個交點,當(dāng)直線上有兩個及以上的點落在凸多邊形內(nèi),便會出現(xiàn)兩個交點,當(dāng)直線上只有一點落在多邊形內(nèi)且剛好為頂點時便只有一個交點。但對于凹多邊形而言,第1種情況可能導(dǎo)致交點多于兩個,此時路徑規(guī)劃方法需要進行調(diào)整,以上便是作業(yè)區(qū)域形狀對規(guī)劃的影響。本文以凸多邊形作業(yè)區(qū)域為例進行規(guī)劃。

圖6 凸多邊形和凹多邊形區(qū)域

在確定第1行/列作物的種植方向后,基于后續(xù)種植方向均與第1行/列平行的假設(shè),需要計算給定區(qū)域最大允許作物種植行數(shù)即等距線條數(shù)。通過遍歷種植區(qū)域特征點,得到最大距離后與行距相除且向下取整即可得到最大允許行數(shù),計算式為

(1)

其中

Lmax=max(Li)

(2)

(3)

式中Li——特征點間距離

Lmax——特征點間最大距離

H——行距

N——給定區(qū)域最大允許作物種植行數(shù)

如圖7所示,為對點進行順序選擇,方便路徑形成,將等距線分為上行線和下行線兩類,上行線即路徑矢量方向在Y軸的分量為正值,下行線即路徑矢量方向在Y軸的分量為負值,為保證總路線長度最短,規(guī)劃后的路線應(yīng)當(dāng)為蛇形折線,即上行線和下行線交替出現(xiàn)。上下行線與邊界線的交點即為軌跡引導(dǎo)點,為確定折線路徑中交點出現(xiàn)的順序,利用上下行線斜率特征進行判斷。在圖7所示情況下,針對上行線,由于先出現(xiàn)的交點應(yīng)為上行線與下方邊界線的交點,后出現(xiàn)的是與上方邊界線的交點,即點1、5、9先出現(xiàn),2、6、10后出現(xiàn),下行線恰好相反。為計算交點,必須得知邊界線和上下行線的直線特征,具體表現(xiàn)為K和b值。

圖7 上行線和下行線

如圖8所示,邊界線由種植區(qū)特征點確定,在計算時有順時針計算和逆時針計算的區(qū)別,為達到交點有序,在計算交點坐標(biāo)時,與上行線特征匹配的是逆時針邊界線特征,與下行線特征匹配的是順時針邊界線特征。如此往復(fù),分別計算不同等距線的交點即可實現(xiàn)交點有序化。

圖8 順時針計算和逆時針計算

軌跡引導(dǎo)點確定后,路徑規(guī)劃便可以實現(xiàn)。以上提出的引導(dǎo)點有序化方法是在種植方向直線特征K為正值的前提下實現(xiàn)的,如果種植方向直線特征K為負值,則上下行特征與邊界線特征順逆匹配恰好相反。依據(jù)上述全局靜態(tài)路徑規(guī)劃原理,經(jīng)規(guī)劃后最終形成的路徑如圖9所示。

圖9 全局靜態(tài)路徑規(guī)劃演示

2 基于激光雷達的局部動態(tài)路徑規(guī)劃

針對實際農(nóng)田種植和生產(chǎn)要求,提出了基于GNSS+IMU組合導(dǎo)航的全局靜態(tài)路徑規(guī)劃方法,即依托高精度位置信息,獲取機器人作業(yè)范圍,結(jié)合實際農(nóng)藝,通過多種測繪手段獲得大田種植參數(shù)后,由系統(tǒng)自動規(guī)劃得到一條重復(fù)率最低的作業(yè)路徑,在誤差允許范圍內(nèi),全局靜態(tài)路徑規(guī)劃可以給定機器人運動的大致軌跡?？紤]到土壤起伏程度,作物種植行距波動,測繪精度等因素的影響,機器人在田間作業(yè)時依靠單一的全局靜態(tài)路徑規(guī)劃顯然不足以支撐自動駕駛的正常運行。其次,全局靜態(tài)規(guī)劃通常需要在已知環(huán)境中進行,即上述“人為劃定”,屬于一種事前規(guī)劃,可以找到最優(yōu)解,一旦環(huán)境發(fā)生變化,未能及時更新地圖,或由于對已知環(huán)境把控的誤差,便無法保證作業(yè)效果。以玉米大田為例,玉米行距維持在50～60 cm范圍內(nèi),擬定機器人寬度為40 cm,由于GNSS+IMU組合系統(tǒng)可以提供的精度范圍只能穩(wěn)定維持在分米級,隨著行駛過程中累計誤差的增大,可能出現(xiàn)機器人行駛軌跡與實際種植區(qū)重疊即發(fā)生碰撞進而導(dǎo)致局部作物損傷的不利情況。為解決這一問題,引入局部動態(tài)路徑規(guī)劃并對全局靜態(tài)路徑進行實時修正顯得尤為重要。

局部動態(tài)路徑規(guī)劃通常用在未知或部分未知的環(huán)境中,系統(tǒng)根據(jù)傳感器實時獲取到環(huán)境障礙物的信息,并做出相應(yīng)規(guī)劃,在設(shè)定的規(guī)劃閾值范圍內(nèi),參照全局靜態(tài)路徑規(guī)劃做出的初步判斷,進行再判斷,矯正糾偏,這對系統(tǒng)的實時計算處理能力有著較高的要求。但若缺乏全局環(huán)境信息,在一些特定場景中其規(guī)劃效果也同樣不盡人意。兩者的宏微結(jié)合,協(xié)同融合,可大幅提高機器人運行的精準(zhǔn)性和平穩(wěn)性。

2.1 點云數(shù)據(jù)剪切

基于現(xiàn)有的局部動態(tài)路徑規(guī)劃算法,本文提出一種基于激光雷達的規(guī)劃方法。在作物種植行間,通過單線激光雷達對某一合適高度的平面進行掃描,同時對不符合計算要求的區(qū)域進行剪切處理,通過保留的數(shù)據(jù)可以計算在當(dāng)前環(huán)境下機器人與作物莖稈的相對位置和自身位姿,對莖稈特征點(位置坐標(biāo))進行曲線擬合后,規(guī)劃出布局最優(yōu)路徑,協(xié)同全局靜態(tài)路徑并對其進行局部修正,做出最終規(guī)劃。單線激光雷達工作原理如圖10所示,激光雷達原始輸出經(jīng)處理后成為2個數(shù)組,分別記錄0°～360°范圍內(nèi)每一個環(huán)境反射點的距離和角度信息,經(jīng)可視化處理后最終顯示效果如圖10所示。

圖10 雷達掃描可視化

在實際應(yīng)用中,由于安放位置不同以及機器人外形限制,0°～360°的掃描信息往往有一部分需要剪切。假設(shè)雷達置于機器人最前方,故其掃描范圍為90°～270°,而270°～360°和0°～90°范圍的點云數(shù)據(jù)由于設(shè)備遮擋等因素被裁剪。在消除機器人自身障礙部分對雷達測距產(chǎn)生的影響的前提下,減小數(shù)據(jù)計算量,方便更快地做出規(guī)劃。

2.2 范圍閾值設(shè)定

點云數(shù)據(jù)經(jīng)過裁剪之后,還要設(shè)定掃描范圍閾值,因為雷達的掃描半徑遠大于作物種植行距,雷達發(fā)出的激光難免會透過作物株間的空隙,照射到非當(dāng)前行駛行的作物,采集異常信息,進而對局部動態(tài)規(guī)劃造成干擾。范圍閾值的設(shè)定保證了機器人進行行間作業(yè)時,局部動態(tài)路徑規(guī)劃的依據(jù)始終為左右兩側(cè)最近作物種植行/列而避免出現(xiàn)漂移和越界規(guī)劃行為,進而導(dǎo)致局部動態(tài)路徑規(guī)劃異常。如圖11所示,范圍閾值設(shè)定的計算過程如下:

圖11 相對位置及距離標(biāo)定

設(shè)L1為作物種植的行距,計算式為

L1=L2+2L4+L3

(4)

式中L2——作物擬合矩形寬度

L3——機器人寬度

L4——機器人邊界到作物擬合矩形內(nèi)邊距離

L4可以作為機器人運行安全距離。圖中機器人恰好位于左右兩側(cè)作物列中央,此時式(4)成立。在計算范圍閾值時,需要將機器人置于邊界位置,如圖12所示,范圍閾值由最大距離Lmax和最小距離Lmin組成,其計算式為

圖12 范圍閾值計算

(5)

(6)

假設(shè)在某一時刻機器人受到影響,行駛軌跡存在偏移,導(dǎo)致機器人偏離正常航線,緊貼右邊作物擬合矩形,即安全距離為0,存在碰撞和碾壓作物風(fēng)險。此時雷達被安置于機器人中線位置處,掃描范圍L應(yīng)設(shè)定在(Lmin,Lmax)區(qū)間內(nèi),參照式(4)和圖13 可將點云數(shù)據(jù)進行處理,可得

圖13 點云裁剪

L=rangecos(degree)

(7)

其中range、degree均為雷達內(nèi)置數(shù)據(jù)格式。

經(jīng)過范圍閾值設(shè)定后,最終呈現(xiàn)的點云分布如圖13所示,可以看到雷達只保留了兩側(cè)閾值范圍內(nèi)的點云數(shù)據(jù)并進行顯示,不合適的角度和點云均被剪切。

2.3 特征化識別

完成點的篩選后,當(dāng)前點云數(shù)據(jù)間除了位置信息以外沒有任何差別,沒有一個點可以作為局部動態(tài)路徑規(guī)劃的特征點或參考點參與運算,故需要對單一的點云數(shù)據(jù)進行再篩選,目的是找出符合規(guī)劃思路的特征點并標(biāo)定。

在實際的大田中以玉米為例,玉米機械化種植后存在規(guī)定的行距和株距,在進行全局靜態(tài)路徑規(guī)劃時依托玉米大田全局特征進行計算和匹配,可以得到圖14中的紅色路徑,但在進行局部動態(tài)路徑規(guī)劃時更關(guān)心圖中微觀特征。

圖14 玉米大田種植情況

在同一行/列內(nèi),每株玉米之間都有一定距離,稱為株距,雷達進行掃描時,株距的存在會導(dǎo)致掃描到非當(dāng)前行駛行的作物信息,對規(guī)劃造成干擾,可以通過雷達角度和點云裁剪對其進行修正。行駛行兩側(cè)的玉米會被雷達掃描到并可視化呈現(xiàn),圖14即為局部動態(tài)規(guī)劃時的微觀情況,紅色箭頭指向為行駛方向。

進行局部動態(tài)路徑規(guī)劃時,由于雷達掃描特性,在某一特定角度下會首先收到距離最近點的距離信息,規(guī)劃的首要目的是定位玉米莖稈的位置,但由于不同品種的玉米節(jié)距,葉片數(shù)量、分布、大小等情況均不相同,可能出現(xiàn)在掃描高度有葉片遮擋,進而無法準(zhǔn)確定位莖稈的問題。因此需要對多種玉米生長情況調(diào)研,并對生長特征類似的玉米進行三維模型重建,如圖15所示。

圖15 玉米三維掃描模型

如圖16所示,將雷達掃描高度的玉米生長情況擬合為雙同心圓,圖中紅黃色同心圓即代表作物,紅色表示莖稈位置,黃色表示玉米下部,莖稈周邊可能出現(xiàn)的葉片,只要機器人行駛過程中不會與紅色部分發(fā)生碰撞即可。該模型將葉片考慮在內(nèi),雷達掃描得到的點經(jīng)擬合認為是圓弧,通過計算找到圓心即莖稈位置,避免出現(xiàn)因葉片干擾導(dǎo)致莖稈定位失誤,造成規(guī)劃錯誤的問題。

圖16 掃描平面示意圖及特征圓擬合

機器人兩側(cè)矩形框為作物擬合范圍,淺綠色矩形框代表雷達掃描范圍,虛線箭頭代表雷達發(fā)出的激光,與雷達不同距離的植株經(jīng)掃描后,反饋的信息并不完全相同,以紫色弧線表示反饋信息,即掃描到的玉米特征信息。紫色弧線由多個點云數(shù)據(jù)組成,任選其中4個點可以擬合一個特征圓,取多個圓心均值標(biāo)定為玉米莖稈位置,即紅色同心圓圓心位置。

通過對作物三維建模著重對雷達掃描平面即玉米下半部分進行分析。由圖15b可知,在掃描平面內(nèi),玉米葉片可擬合成一個半徑為R1的圓,同理,莖稈區(qū)域也可以擬合為一個半徑為R2的圓,在理想情況下,兩圓的中軸線會重合,現(xiàn)實情況中往往會出現(xiàn)長度為d的間隙。在誤差允許的范圍內(nèi),d可不做考慮。

玉米莖稈位置確定之后便可以進行局部動態(tài)規(guī)劃,假設(shè)掃描后有2m個符合的特征點(莖稈位置)且分布在機器人左右兩側(cè),兩兩配對并取中點坐標(biāo),符合要求的中點有m個,采用最小二乘法對曲線進行擬合,選擇多項式擬合模式

y=a0+a1x+a2x2+…+amxm

(8)

離散的各點(特征點中點)到這條曲線的平方和F(a0,a1,…,am)為

F(a0,a1,…,am)=

(9)

對F(a0,a1,…,am)分別求對a4的偏導(dǎo)數(shù),得到m+1個方程

(10)

將其整理為針對a0,a1,…,am的正規(guī)方程組,最終結(jié)果為

(11)

利用高斯消元法求解即可得到a0,a1,…,am,擬合曲線如圖17所示。

圖17 最小二乘法擬合曲線

3 全局與局部規(guī)劃的宏微融合

基于測繪信息和雷達掃描信息,全局靜態(tài)路徑規(guī)劃與局部動態(tài)路徑規(guī)劃得以實現(xiàn),為優(yōu)化機器人田間作業(yè)效果,兩者的宏微結(jié)合是極為重要的環(huán)節(jié)。

顯然,全局靜態(tài)路徑與局部動態(tài)路徑的區(qū)別在于規(guī)劃范圍即路徑對機器人指導(dǎo)范圍的不同,全局靜態(tài)路徑更多的對機器人大范圍內(nèi)(如整塊作業(yè)田)的行駛方向進行限定,而局部動態(tài)路徑更傾向于對行間直行前提下的軌跡糾偏進行指導(dǎo)。受限于田間測繪信息的精確度,如果只將全局狀態(tài)下規(guī)劃得到的作業(yè)路徑直接投入使用會產(chǎn)生累積誤差進而影響作業(yè)質(zhì)量,同理,雖然局部動態(tài)路徑可以為機器人行間作業(yè)提供高精度路徑信息,卻無法在諸如起始行、轉(zhuǎn)角、終止行等特殊位置處給出合理路徑,且極易出現(xiàn)錯誤規(guī)劃的問題。因此通過全局靜態(tài)與局部動態(tài)路徑規(guī)劃的宏微融合,即作業(yè)過程中,機器人的作業(yè)行駛方向始終由宏觀狀態(tài)下的全局靜態(tài)路徑給出,而當(dāng)機器人順利通過特殊作業(yè)位置,行駛在作物行內(nèi)時,依托微觀狀態(tài)下的局部路徑對其進行更為精細,作物損傷率更低的行駛指導(dǎo),宏微融合從而完成高質(zhì)量田間作業(yè)。

在機器人操作系統(tǒng)(Robot operating system,ROS)中,路徑作為Path消息被發(fā)布并顯示在可視化工具(Robot visualization tool,RVIZ)中。按照種植規(guī)律首先放置虛擬作物,利用Python中高斯分布函數(shù)np.random.normal在保證不違背種植規(guī)律的前提下對作物位置進行微調(diào)以模擬實際作物種植情況。利用宏微結(jié)合算法,將宏觀路徑和微觀路徑進行融合后得到如圖18中的路徑效果。綠色實線表示規(guī)劃路徑,紅色帶箭頭虛線表示全局靜態(tài)規(guī)劃路徑。顯然,宏微結(jié)合路徑相較于全局靜態(tài)路徑和局部動態(tài)路徑而言更貼合實際田間作業(yè)場景,更符合實際生產(chǎn)需要。

圖18 路徑規(guī)劃仿真

田間作業(yè)往往會受到不同障礙物阻擋等情況,通過增大虛擬作物體積將其中某一作物定為如樹木、水井等的障礙物。在障礙物存在的情況下,若行間仍允許機器人通過,即當(dāng)前作業(yè)行間最小寬度大于機器人寬度,則出現(xiàn)如圖18的規(guī)劃路徑;若無法保證機器人安全通過,則綠色路徑會在障礙物處終止,而紅色路徑不受影響。由此可見宏微融合對實際作業(yè)中保證安全、高質(zhì)量作業(yè)的重要性。

4 基于MPC算法的路徑跟蹤

為保證機器人在實際作業(yè)時可以對宏微融合后的路徑進行有效追蹤,使其運行軌跡最大程度上貼合規(guī)劃后的路線[25-26],可基于MPC算法對這一環(huán)節(jié)進行實現(xiàn)。MPC算法的作用機理可以表述為,在每一個采用時刻,根據(jù)當(dāng)前獲得的測量信息,在線實時求解一個有限時間下開環(huán)優(yōu)化問題,并將得到的控制序列的第1個元素作用于被控對象。在下一個采樣時刻,重復(fù)上述過程:用新的測量值作為此時預(yù)測系統(tǒng)未來動態(tài)的初始條件,刷新優(yōu)化問題并重新求解。相對于線性二次型調(diào)節(jié)器(LQR)方法而言,MPC算法可以考慮空間狀態(tài)變量的各種約束,而LQR、PID等控制算法只能夠考慮輸入輸出變量的各種約束[27]。

試驗所用農(nóng)業(yè)機器人為兩輪差速機器人,其底盤為兩輪差速底盤,由兩個輪轂電機作為動力輪,后部由一個萬向輪支撐?；谧笥逸喫俣茸兓M而實現(xiàn)轉(zhuǎn)彎和前進動作。

在ROS獨有的兩輪差速模型中,機器人運動學(xué)模型描述為

(12)

(13)

(14)

θ——機器人航向角,rad

ω——機器人航向角速度,rad/s

為方便實際應(yīng)用,將上述連續(xù)的微分模型進行離散化處理,成為差分模型,差分間隔定義為dt。則k+1時刻的機器人橫向、縱向位移與航向角便可由k時刻的對應(yīng)量與差分間隔內(nèi)的微小變化量相加得到,可得

xk+1=xk+vkcosθkdt

(15)

yk+1=yk+vksinθkdt

(16)

θk+1=θk+ωkdt

(17)

式中xk——k時刻機器人橫向位移,m

yk——k時刻機器人縱向位移,m

ωk——k時刻機器人航向角,rad

θk——k時刻機器人航向角速度,rad/s

考慮到路面情況與控制誤差造成的位移偏差,定義橫向跟蹤誤差ek(m)與航向誤差e′k(rad)并假設(shè)機器人運行偏差敏感方向為Y方向,ek表示k時刻下MPC算法規(guī)劃的路徑點與真實路徑點距離差,e′k表示k時刻MPC算法規(guī)劃路徑上對應(yīng)點的航向角與真實航向角的差值,則有

ek=f(xk)-yk

(18)

e′k=arctan(f′(xk)-θk)

(19)

其中f(xk)為機器人運行到k時刻時真實路徑的位置在Y方向上的投影,相應(yīng)的,arctan(f′(xk))為k時刻的航向角,同樣在離散化處理后有

ek+1=ek+vksinθkdt

(20)

e′k+1=e′k+ωkdt

(21)

通過對機器人運動學(xué)模型進行分析并計算離散后的位移與誤差,再根據(jù)實際應(yīng)用場景對目標(biāo)函數(shù)進行設(shè)置與優(yōu)化,最終可以得到基于MPC的控制函數(shù),以目標(biāo)函數(shù)最小為原則,可得到N個預(yù)測節(jié)點以指導(dǎo)機器人的運行。

如圖19所示,在ROS中,利用C++編制了部署于虛擬機平臺的仿真環(huán)境。在引入機器人統(tǒng)一機器人描述格式(Unified robot description format,URDF)模型后,機器人運動基礎(chǔ)參數(shù)如輪徑、輪距等均被定義,之后導(dǎo)入機器人運動學(xué)模型與對應(yīng)的MPC控制函數(shù)并以圖18中的仿真路徑為基礎(chǔ)進行MPC算法仿真試驗。其中綠色路徑為融合規(guī)劃路徑,紅色軌跡為MPC算法對機器人前方1.5 m范圍內(nèi)實時規(guī)劃所得路徑。以機器人當(dāng)前位置為原點,將運行方向上前1.5 m內(nèi)的融合規(guī)劃路徑與MPC實時規(guī)劃路徑離散化后得到如表1所示的位置點,路徑效果圖如圖20所示。在MPC算法控制下,1.5 m內(nèi)的最大橫向誤差為0.028 m,1.0 m內(nèi)的最大橫向誤差為0.006 m,可見,合理選擇MPC算法中動態(tài)窗口可以保證良好的跟蹤效果。

圖20 MPC規(guī)劃路徑圖

5 宏微結(jié)合路徑規(guī)劃試驗

5.1 試驗平臺與場景

為驗證宏微結(jié)合下路徑規(guī)劃的實際效果,搭建了試驗平臺,設(shè)計了實物試驗。本試驗所搭建的平臺如圖21所示,試驗平臺為直徑50 cm的圓形兩輪差速機器人,以NVIDIA NX作為主控平臺,在Ubuntu的基礎(chǔ)上部署了ROS Melodic系統(tǒng)用以實現(xiàn)整個機器人的控制。為實現(xiàn)各項功能,試驗平臺上部署了GNSS系統(tǒng)和IMU單元等傳感器元件,兩者協(xié)同成為組合導(dǎo)航系統(tǒng),在空曠無遮擋,搭配無跡卡爾曼濾波算法的前提下其定位精度可達分米級,足以滿足一般田間作業(yè)精度要求。雷達被布署于試驗平臺前側(cè),離地高度30 cm。

圖21 試驗平臺

試驗場景如圖22所示,為模擬真實的大田作業(yè)環(huán)境,利用直徑為40 mm的PVC管材替代田間作物,管材高度均為40 cm,模擬作物種植間距45 cm,種植行距100 cm。

圖22 試驗場景

試驗過程中平臺內(nèi)部控制流程如圖23所示。平臺運行伊始,全局靜態(tài)路徑首先被確定。在運行過程中,雷達掃描得到的實時局部動態(tài)路徑將不斷與全局靜態(tài)路徑進行融合進而作為待追蹤路徑參與MPC下一時刻控制輸出。在此期間,試驗平臺不斷比對當(dāng)前位置與下一時刻位置,從而確定下一時刻的目標(biāo)點并在MPC算法的規(guī)劃下得到實時的航向角與速度變化指令。在CAN總線通訊下,主控平臺NX驅(qū)動兩側(cè)伺服輪轂電機運行,進而控制試驗平臺實現(xiàn)各項運動。如此循環(huán),在裝載作業(yè)器具后即可完成實際作業(yè)。

圖23 路徑規(guī)劃流程圖

5.2 路徑規(guī)劃效果評估標(biāo)準(zhǔn)

本文所提出的宏微結(jié)合下路徑規(guī)劃算法主要解決由于測繪等數(shù)據(jù)精度不足造成農(nóng)業(yè)作業(yè)機器人田間運行時隨著累積誤差的增大,可能出現(xiàn)的碾壓田壟、破壞作物等直接影響生產(chǎn)的問題。依據(jù)此條件,搭載本算法后試驗平臺運行過程中與模擬作物之間的最短距離即可作為評估算法有效性的重要參數(shù)。實際作物在正常生長過程中,枝葉會出現(xiàn)以莖稈為中心向四周延伸的現(xiàn)象,這也是圖16中黃色區(qū)域的產(chǎn)生原因。當(dāng)平臺所行駛路徑與黃色區(qū)域存在重疊現(xiàn)象時,也就意味著碰撞或者碾壓現(xiàn)象可能發(fā)生,作物可能出現(xiàn)不同程度的損傷。為此,需要在允許試驗平臺正常通過的前提下,在模擬作物周邊設(shè)定合理的枝葉延伸區(qū)域,并利用組合導(dǎo)航系統(tǒng)記錄的軌跡點擬合試驗平臺行駛路徑,計算重疊次數(shù)與重合度,進而對算法效果進行評估。

5.3 宏微結(jié)合路徑規(guī)劃試驗

為對比宏微結(jié)合下的路徑融合效果,首先進行了全局靜態(tài)路徑規(guī)劃試驗,結(jié)果如表2所示。在該試驗中,試驗平臺所追蹤路徑僅為依托全局測繪信息規(guī)劃后得到的全局靜態(tài)路徑,如圖24所示。

表2 全局靜態(tài)路徑規(guī)劃試驗結(jié)果

圖24 全局靜態(tài)路徑規(guī)劃下平臺移動路徑

為對規(guī)劃后路徑進行質(zhì)量考核,圖中紅色實心圓點代表作物實際位置,紅色實心圓點周邊黃色空心圈為作物枝葉生長范圍。其中黑色蛇形路徑為全局規(guī)劃后的靜態(tài)路徑,綠色空心圈為平臺追蹤路徑運行時每一時刻所處的位置。顯然,當(dāng)綠色空心圈與黃色空心圈有重疊部分時即代表發(fā)生一定程度的碰撞或碾壓。

為方便考量結(jié)果,量化了作物枝葉生長范圍,圖25為當(dāng)枝葉生長范圍半徑為0.15 m的覆蓋率,圖26為枝葉生長范圍半徑為0.20 m的覆蓋率。兩種范圍限定后留給平臺運行時調(diào)整的距離分別為0.2 m和0.1 m,可見兩種范圍限定下發(fā)生明顯碰撞的均為第5條路徑,最大覆蓋率和平均覆蓋率分別為6.381 6%、13.081 2%和1.276 3%、3.405 8%。

圖25 作物枝葉半徑為0.15 m的全局靜態(tài)路徑規(guī)劃覆蓋率

圖26 作物枝葉半徑為0.20 m的全局靜態(tài)路徑規(guī)劃覆蓋率

結(jié)果表明,隨著作物行/列的增多,測繪誤差會逐步累積,進而造成后續(xù)全局靜態(tài)路徑與作物覆蓋率驟增,可能會發(fā)生難以忽略的碰撞與碾壓問題。在實際作業(yè)過程中,田間行駛環(huán)境相較于本試驗更為惡劣,土壤起伏等原因會造成該問題更早發(fā)生且現(xiàn)象更為明顯。顯然全局靜態(tài)路徑無法長期指導(dǎo)田間作業(yè)。

基于上述試驗情況,再次進行宏微結(jié)合下的融合路徑規(guī)劃試驗。在該試驗過程中,首先按照上述全局靜態(tài)路徑規(guī)劃方法,人為輸入模擬大田的各項特性參數(shù)后,平臺首先擬合出全局靜態(tài)路徑,之后平臺從左下角出發(fā),按照蛇形路徑運行。在此期間局部動態(tài)路徑不斷與全局靜態(tài)路徑融合,指導(dǎo)平臺運行。融合路徑規(guī)劃試驗情景如圖27所示,其中不同元素的含義同上。可見在融合路徑規(guī)劃過程中,全局靜態(tài)路徑對平臺運行的作用主要體現(xiàn)在轉(zhuǎn)角處方向信息的指示,而行間作業(yè)路徑信息更多地由局部動態(tài)路徑規(guī)劃給出。在這樣的融合模式下,平臺運行過程中實時與環(huán)境進行感知,碰撞與碾壓的可能性得以降低,即一定程度上降低了作物損傷率。

圖27 融合路徑規(guī)劃下平臺移動路徑

基于全局靜態(tài)路徑試驗結(jié)果,隨著全局靜態(tài)路徑與實際情況的累積量逐步增大,局部動態(tài)路徑規(guī)劃對試驗平臺行間正常運行起到了越來越重要的作用。為衡量本文所提出的路徑規(guī)劃理論的實際效果,同樣設(shè)定枝葉延伸范圍半徑分別為0.15、0.2 m的試驗,結(jié)果如表3、4和圖28、29所示。

表3 作物枝葉半徑為0.15 m的融合路徑規(guī)劃覆蓋率

圖29 作物枝葉半徑為0.2 m的融合路徑規(guī)劃覆蓋率

可見在延伸半徑為0.15 m即平臺允許調(diào)整距離為0.2 m的情況下,平臺行駛路徑僅與一株作物發(fā)生了明顯重疊現(xiàn)象,此時最大覆蓋率為7.554 1%,平均覆蓋率為0.188 9%。同樣地,在延伸半徑為0.2 m情況下,平臺運行過程中的最大覆蓋率為 7.502 4%,平均覆蓋率為0.677 2%。相對于全局靜態(tài)路徑規(guī)劃的試驗結(jié)果,融合路徑規(guī)劃顯著提升了作業(yè)精度,降低了覆蓋率,減小了作物碾壓率。通過對比試驗結(jié)果發(fā)現(xiàn),局部動態(tài)路徑的實時更新與正確規(guī)劃是保證平臺與兩側(cè)作物留有安全距離,不發(fā)生碰撞的前提。

為進一步驗證局部動態(tài)路徑規(guī)劃所帶來的優(yōu)勢,設(shè)計了更為復(fù)雜的種植場景。如圖30所示,該路徑夸大了實際大田中兩側(cè)作物種植誤差即相對于中心的偏移距離,以對算法有更好的評估。在該環(huán)境中,全局靜態(tài)路徑規(guī)劃顯然已經(jīng)無法指導(dǎo)平臺正常運行,僅能給出最基本的前進方向,局部動態(tài)路徑規(guī)劃在其中起到了決定性作用。依照同樣的效果評估策略,利用雷達實時測量試驗平臺前方兩側(cè)作物距離并獲取記錄最小值,試驗結(jié)果如表5所示,擬合出最小距離隨縱向位移的變化曲線,如圖31所示。待運行結(jié)束后得到最小距離為12.858 6 cm,且作業(yè)過程未與作物發(fā)生碰撞,局部動態(tài)規(guī)劃效果良好。

表5 局部動態(tài)路徑規(guī)劃試驗結(jié)果

圖30 全局與局部動態(tài)路徑規(guī)劃試驗

圖31 局部動態(tài)路徑規(guī)劃下最小距離擬合

6 結(jié)束語

針對目前智慧農(nóng)業(yè)中農(nóng)業(yè)機器人的田間導(dǎo)航需求,提出了一種基于宏微結(jié)合的路徑規(guī)劃算法,該算法首先依托作業(yè)區(qū)域特征,作物種植特征等現(xiàn)有信息進行全局靜態(tài)路徑規(guī)劃,其次在作業(yè)期間依據(jù)雷達,同時根據(jù)特定作物品種擬合后的數(shù)學(xué)模型,從而確定機器人當(dāng)前行駛行兩側(cè)作物的相對位置,進行局部動態(tài)路線規(guī)劃并與全局靜態(tài)規(guī)劃路徑進行融合,并進行路徑融合得到最終運行路徑,之后在MPC等路徑追蹤算法的控制下完成田間作業(yè)。通過試驗,驗證了宏微結(jié)合下的機器人田間路徑規(guī)劃算法在實際作業(yè)中的可行性。在模擬環(huán)境中,當(dāng)機器人在田間作業(yè)時兩側(cè)可調(diào)整距離分別為0.2、0.1 m時,當(dāng)試驗平臺僅進行全局靜態(tài)路徑規(guī)劃時,平均作物損傷率為3.405 8%和1.276 3%,而通過本文提出的宏微結(jié)合下機器人田間路徑規(guī)劃算法規(guī)劃后,可將平均作物損傷率降低到0.677 2%和0.188 9%,說明該算法有效地阻止了農(nóng)業(yè)機器人在工作過程中對作物的碾壓,提高了作業(yè)精度。當(dāng)作物行間距為100 cm且種植規(guī)范的情況下,試驗平臺與作物距離最小為22 cm;當(dāng)種植規(guī)劃化程度較低時,其可實現(xiàn)最小距離為12.858 6 cm,證明了宏微結(jié)合下的機器人田間路徑規(guī)劃算法在作物不受到破壞的情況下,確保了機器人安全作業(yè),對田間環(huán)境表現(xiàn)出良好的環(huán)境適應(yīng)性。