基于宏微結(jié)合的田間作業(yè)機(jī)器人路徑規(guī)劃

鄭 路 張 嘯 王建國(guó) 吳 悅 李海濤

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

0 引言

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

圖4 玉米大田模型

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

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

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

圖5 等距直線與邊界線交點(diǎn)

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

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

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

(1)

其中

Lmax=max(Li)

(2)

(3)

式中Li——特征點(diǎn)間距離

Lmax——特征點(diǎn)間最大距離

H——行距

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

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

圖7 上行線和下行線

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

圖8 順時(shí)針計(jì)算和逆時(shí)針計(jì)算

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

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

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

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

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

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

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

圖10 雷達(dá)掃描可視化

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

2.2 范圍閾值設(shè)定

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

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

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

L1=L2+2L4+L3

(4)

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

L3——機(jī)器人寬度

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

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

圖12 范圍閾值計(jì)算

(5)

(6)

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

圖13 點(diǎn)云裁剪

L=rangecos(degree)

(7)

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

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

2.3 特征化識(shí)別

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

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

圖14 玉米大田種植情況

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

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

圖15 玉米三維掃描模型

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

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

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

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

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

y=a0+a1x+a2x2+…+amxm

(8)

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

F(a0,a1,…,am)=

(9)

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

(10)

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

(11)

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

圖17 最小二乘法擬合曲線

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

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

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

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

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

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

4 基于MPC算法的路徑跟蹤

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

試驗(yàn)所用農(nóng)業(yè)機(jī)器人為兩輪差速機(jī)器人,其底盤為兩輪差速底盤,由兩個(gè)輪轂電機(jī)作為動(dòng)力輪,后部由一個(gè)萬(wàn)向輪支撐。基于左右輪速度變化進(jìn)而實(shí)現(xiàn)轉(zhuǎn)彎和前進(jìn)動(dòng)作。

在ROS獨(dú)有的兩輪差速模型中,機(jī)器人運(yùn)動(dòng)學(xué)模型描述為

(12)

(13)

(14)

θ——機(jī)器人航向角,rad

ω——機(jī)器人航向角速度,rad/s

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

xk+1=xk+vkcosθkdt

(15)

yk+1=yk+vksinθkdt

(16)

θk+1=θk+ωkdt

(17)

式中xk——k時(shí)刻機(jī)器人橫向位移,m

yk——k時(shí)刻機(jī)器人縱向位移,m

ωk——k時(shí)刻機(jī)器人航向角,rad

θk——k時(shí)刻機(jī)器人航向角速度,rad/s

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

ek=f(xk)-yk

(18)

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

(19)

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

ek+1=ek+vksinθkdt

(20)

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

(21)

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

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

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

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

5.1 試驗(yàn)平臺(tái)與場(chǎng)景

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

圖21 試驗(yàn)平臺(tái)

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

圖22 試驗(yàn)場(chǎng)景

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

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

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

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

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

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

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

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

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

為方便考量結(jié)果,量化了作物枝葉生長(zhǎng)范圍,圖25為當(dāng)枝葉生長(zhǎng)范圍半徑為0.15 m的覆蓋率,圖26為枝葉生長(zhǎng)范圍半徑為0.20 m的覆蓋率。兩種范圍限定后留給平臺(tái)運(yùn)行時(shí)調(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é)果表明,隨著作物行/列的增多,測(cè)繪誤差會(huì)逐步累積,進(jìn)而造成后續(xù)全局靜態(tài)路徑與作物覆蓋率驟增,可能會(huì)發(fā)生難以忽略的碰撞與碾壓?jiǎn)栴}。在實(shí)際作業(yè)過(guò)程中,田間行駛環(huán)境相較于本試驗(yàn)更為惡劣,土壤起伏等原因會(huì)造成該問(wèn)題更早發(fā)生且現(xiàn)象更為明顯。顯然全局靜態(tài)路徑無(wú)法長(zhǎng)期指導(dǎo)田間作業(yè)。

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

圖27 融合路徑規(guī)劃下平臺(tái)移動(dòng)路徑

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

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

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

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

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

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

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

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

6 結(jié)束語(yǔ)

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