基于宏微結合的田間作業機器人路徑規劃

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

(中國農業大學工學院,北京 100083)

0 引言

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

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

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

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

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

1 全局靜態路徑規劃

1.1 絕對位置(經緯度)坐標獲取

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

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

1.2 作業區域路徑規劃

1.2.1路徑規劃范圍確定

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

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

圖1 作業區域特征點及作業平面

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

圖2 實際農田情況

圖3 種植區域與非種植區域

1.2.2路徑規劃安全區域確定

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

圖4 玉米大田模型

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

1.2.3全局靜態路徑規劃

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

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

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

圖6 凸多邊形和凹多邊形區域

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

(1)

其中

Lmax=max(Li)

(2)

(3)

式中Li——特征點間距離

Lmax——特征點間最大距離

H——行距

N——給定區域最大允許作物種植行數

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

圖7 上行線和下行線

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

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

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

圖9 全局靜態路徑規劃演示

2 基于激光雷達的局部動態路徑規劃

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

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

2.1 點云數據剪切

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

圖10 雷達掃描可視化

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

2.2 范圍閾值設定

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

圖11 相對位置及距離標定

設L1為作物種植的行距,計算式為

L1=L2+2L4+L3

(4)

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

L3——機器人寬度

L4——機器人邊界到作物擬合矩形內邊距離

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

圖12 范圍閾值計算

(5)

(6)

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

圖13 點云裁剪

L=rangecos(degree)

(7)

其中range、degree均為雷達內置數據格式。

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

2.3 特征化識別

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

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

圖14 玉米大田種植情況

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

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

圖15 玉米三維掃描模型

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

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

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

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

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

y=a0+a1x+a2x2+…+amxm

(8)

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

F(a0,a1,…,am)=

(9)

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

(10)

將其整理為針對a0,a1,…,am的正規方程組,最終結果為

(11)

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

圖17 最小二乘法擬合曲線

3 全局與局部規劃的宏微融合

基于測繪信息和雷達掃描信息,全局靜態路徑規劃與局部動態路徑規劃得以實現,為優化機器人田間作業效果,兩者的宏微結合是極為重要的環節。

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

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

圖18 路徑規劃仿真

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

4 基于MPC算法的路徑跟蹤

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

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

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

(12)

(13)

(14)

θ——機器人航向角,rad

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

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

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)并假設機器人運行偏差敏感方向為Y方向,ek表示k時刻下MPC算法規劃的路徑點與真實路徑點距離差,e′k表示k時刻MPC算法規劃路徑上對應點的航向角與真實航向角的差值,則有

ek=f(xk)-yk

(18)

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

(19)

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

ek+1=ek+vksinθkdt

(20)

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

(21)

通過對機器人運動學模型進行分析并計算離散后的位移與誤差,再根據實際應用場景對目標函數進行設置與優化,最終可以得到基于MPC的控制函數,以目標函數最小為原則,可得到N個預測節點以指導機器人的運行。

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

圖20 MPC規劃路徑圖

5 宏微結合路徑規劃試驗

5.1 試驗平臺與場景

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

圖21 試驗平臺

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

圖22 試驗場景

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

圖23 路徑規劃流程圖

5.2 路徑規劃效果評估標準

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

5.3 宏微結合路徑規劃試驗

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

表2 全局靜態路徑規劃試驗結果

圖24 全局靜態路徑規劃下平臺移動路徑

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

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

圖25 作物枝葉半徑為0.15 m的全局靜態路徑規劃覆蓋率

圖26 作物枝葉半徑為0.20 m的全局靜態路徑規劃覆蓋率

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

基于上述試驗情況,再次進行宏微結合下的融合路徑規劃試驗。在該試驗過程中,首先按照上述全局靜態路徑規劃方法,人為輸入模擬大田的各項特性參數后,平臺首先擬合出全局靜態路徑,之后平臺從左下角出發,按照蛇形路徑運行。在此期間局部動態路徑不斷與全局靜態路徑融合,指導平臺運行。融合路徑規劃試驗情景如圖27所示,其中不同元素的含義同上?？梢娫谌诤下窂揭巹澾^程中,全局靜態路徑對平臺運行的作用主要體現在轉角處方向信息的指示,而行間作業路徑信息更多地由局部動態路徑規劃給出。在這樣的融合模式下,平臺運行過程中實時與環境進行感知,碰撞與碾壓的可能性得以降低,即一定程度上降低了作物損傷率。

圖27 融合路徑規劃下平臺移動路徑

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

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

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

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

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

表5 局部動態路徑規劃試驗結果

圖30 全局與局部動態路徑規劃試驗

圖31 局部動態路徑規劃下最小距離擬合

6 結束語

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