基于多約束條件的果園噴霧機(jī)器人路徑規(guī)劃方法

沈 躍 劉子涵 劉 慧 杜 偉

(江蘇大學(xué)電氣信息工程學(xué)院, 鎮(zhèn)江 212013)

0 引言

隨著農(nóng)業(yè)生產(chǎn)的現(xiàn)代化、智能化發(fā)展[1],新興的農(nóng)業(yè)生產(chǎn)設(shè)備與完善的配套算法成為了新的研究趨勢(shì)。自主行走的噴霧機(jī)器人被廣泛地應(yīng)用[2],有效減輕了果農(nóng)的勞動(dòng)強(qiáng)度,提高了作業(yè)效率與質(zhì)量。

路徑規(guī)劃[3]是果園噴霧機(jī)器人的研究熱點(diǎn)和重要問(wèn)題,即在復(fù)雜的果園環(huán)境下能夠?qū)ふ业揭粭l符合機(jī)器人運(yùn)動(dòng)要求的連續(xù)路徑。路徑點(diǎn)搜索[4]是路徑規(guī)劃的基礎(chǔ),基于柵格的路徑規(guī)劃算法是目前的主流方法,如Dijkstra 算法、A*算法等[5]。但傳統(tǒng)的路徑規(guī)劃算法具有路徑結(jié)構(gòu)復(fù)雜、曲率大,不便于跟蹤行駛等問(wèn)題[6]。在實(shí)際的果園環(huán)境應(yīng)用中,除了考慮路徑代價(jià)盡可能小、碰撞安全之外,還需保證行駛軌跡符合機(jī)器人的運(yùn)動(dòng)學(xué)約束。因此,一些學(xué)者提出了將路徑規(guī)劃與軌跡優(yōu)化結(jié)合的方法,對(duì)規(guī)劃好的路徑進(jìn)行軌跡優(yōu)化處理[7]。陳成等[8]采用四階貝塞爾曲線來(lái)表述軌跡形狀,并結(jié)合轉(zhuǎn)向結(jié)構(gòu)的約束與速度約束保證軌跡的可行性,但貝塞爾曲線的局限性限制了軌形的可塑性,不適用于復(fù)雜路面情況。曹如月等[9]用貝塞爾曲線對(duì)轉(zhuǎn)角處路徑進(jìn)行平滑處理,以“直線—曲線—直線”的方式表示全局路徑,以分步的形式優(yōu)化全局路徑,但分段方法無(wú)法保證整體路徑的連續(xù)性。呂恩利等[10]利用三次均勻B樣條曲線描述叉車(chē)路徑,優(yōu)化了整體軌跡形狀,效果很好,但此方法主要是針對(duì)短距離路線的簡(jiǎn)單作業(yè)情況,對(duì)于復(fù)雜路面情況的應(yīng)用仍然存在一定的局限性。

本文通過(guò)分析果園作業(yè)環(huán)境,根據(jù)實(shí)際地圖利用A*算法進(jìn)行全局路徑搜索,根據(jù)特征點(diǎn)曲率篩選出分段點(diǎn)與轉(zhuǎn)彎曲線,以轉(zhuǎn)彎處首末端點(diǎn)約束、機(jī)器人轉(zhuǎn)彎半徑約束與轉(zhuǎn)向機(jī)構(gòu)延遲約束構(gòu)建約束條件,以轉(zhuǎn)彎曲線曲率均值最小為目標(biāo)建立B樣條曲線并進(jìn)行參數(shù)求解。將采用三次非均勻B樣條(Non-uniform rational B-splines, NURBS)曲線表示全局路徑,并通過(guò)仿真與試驗(yàn)驗(yàn)證算法的可行性。

1 果園噴霧機(jī)器人作業(yè)分析

1.1 作業(yè)環(huán)境

果園場(chǎng)景下噴霧機(jī)器人的路徑規(guī)劃問(wèn)題需在滿足噴霧機(jī)運(yùn)動(dòng)學(xué)約束條件的前提下,以最小化損耗為目標(biāo)生成安全平滑的全局路線。除此以外,整體行駛路線還需滿足機(jī)器人的噴霧作業(yè)要求,為果園噴霧機(jī)器人提供一個(gè)安全可靠、節(jié)能高效的規(guī)劃路徑。

果園噴霧機(jī)器人的路徑規(guī)劃方案需結(jié)合噴霧作業(yè)的實(shí)際要求。果園環(huán)境具有“長(zhǎng)廊式”的特點(diǎn),如圖1所示。在果園噴霧機(jī)器人的行駛過(guò)程中,其噴霧作業(yè)設(shè)備會(huì)對(duì)兩側(cè)檢測(cè)到的果樹(shù)進(jìn)行噴霧施藥。因此,在樹(shù)行間行駛時(shí)需要盡可能與兩側(cè)果樹(shù)保持相同的施藥距離以保證噴霧作業(yè)效果,即以樹(shù)行中心線為目標(biāo)路線。同時(shí),果園噴霧機(jī)器人中心與樹(shù)行邊緣存在一個(gè)極限最小距離以保證行駛路線的安全無(wú)碰撞。在基于經(jīng)緯度信息建立的全局坐標(biāo)系下,果園噴霧機(jī)器人從第一排樹(shù)行外側(cè)為起點(diǎn),在樹(shù)行末端做連續(xù)的“U”形運(yùn)動(dòng),以最后一排樹(shù)行外側(cè)末端為目標(biāo)點(diǎn),在二維平面內(nèi)建立一條從起始點(diǎn)到目標(biāo)點(diǎn)、符合果園噴霧機(jī)器人運(yùn)動(dòng)學(xué)約束的安全平滑全局路徑。

圖1 果園噴霧機(jī)器人路徑規(guī)劃模型示意圖

1.2 路徑規(guī)劃問(wèn)題描述

基于北斗導(dǎo)航系統(tǒng)獲取起始點(diǎn)與目標(biāo)點(diǎn)的精準(zhǔn)全局坐標(biāo),并通過(guò)激光傳感器獲得樹(shù)行位置信息建立全局柵格地圖為前提,研究果園環(huán)境下的路徑規(guī)劃問(wèn)題。本文的研究?jī)?nèi)容包括路徑點(diǎn)搜索后的處理以及生成全局路徑。

路徑規(guī)劃整體流程如圖2所示。在已經(jīng)獲取樹(shù)行位置信息與目標(biāo)點(diǎn)坐標(biāo)的前提下,果園噴霧機(jī)器人首先通過(guò)北斗導(dǎo)航系統(tǒng)獲取自身位姿信息并以自身位置為起點(diǎn),利用A*算法進(jìn)行全局規(guī)劃,生成一條最優(yōu)或較優(yōu)的全局路徑,其結(jié)果是一系列離散的路徑點(diǎn)。將獲得的路徑點(diǎn)提取為特征點(diǎn),并結(jié)合果園實(shí)際環(huán)境信息與作業(yè)要求對(duì)特征點(diǎn)進(jìn)行初優(yōu)化處理。結(jié)合果園噴霧機(jī)器人運(yùn)動(dòng)學(xué)約束,采用三次非均勻B樣條曲線來(lái)進(jìn)行軌跡優(yōu)化并獲得軌跡曲線。最后通過(guò)純跟蹤算法進(jìn)行軌跡跟蹤,完成果園內(nèi)自主行駛的任務(wù)。

圖2 果園噴霧機(jī)器人路徑規(guī)劃流程圖

2 基于多約束條件的路徑規(guī)劃策略

2.1 A*算法路徑點(diǎn)搜索

A*算法[11]是一種啟發(fā)式搜索算法與深度優(yōu)先算法結(jié)合的算法,具有導(dǎo)向性,是靜態(tài)環(huán)境中用于求解最優(yōu)路徑最有效的直接搜索算法。A*算法通過(guò)一個(gè)估價(jià)函數(shù)f(n)來(lái)確定搜索方向[12],從起始點(diǎn)向周?chē)鷶U(kuò)展,同時(shí)通過(guò)啟發(fā)函數(shù)h(n)來(lái)計(jì)算周?chē)鱾€(gè)節(jié)點(diǎn)到目標(biāo)點(diǎn)的估計(jì)代價(jià)[13],以此來(lái)選擇最小代價(jià)節(jié)點(diǎn)作為下一個(gè)擴(kuò)展節(jié)點(diǎn),不斷重復(fù)這一過(guò)程直至到達(dá)目標(biāo)點(diǎn)。這一過(guò)程中搜索出的所有節(jié)點(diǎn)均為最小代價(jià)節(jié)點(diǎn)[14],將這些節(jié)點(diǎn)以路徑點(diǎn)的形式保存,生成最終路徑,此路徑為最小代價(jià)路徑。A*算法的代價(jià)函數(shù)f(n)表示為[15]

f(n)=g(n)+h(n)

(1)

式中n——待擴(kuò)展的節(jié)點(diǎn)

g(n)——從起點(diǎn)到當(dāng)前節(jié)點(diǎn)的實(shí)際路徑代價(jià)

h(n)——啟發(fā)函數(shù),前節(jié)點(diǎn)到目標(biāo)點(diǎn)估計(jì)代價(jià)值

對(duì)于代價(jià)函數(shù)f(n),當(dāng)g(n)=0時(shí),A*算法成為單純的貪心算法,計(jì)算速度快但不一定得到最優(yōu)解;當(dāng)h(n)=0時(shí),式(1)成為單純的Dijkstra算法,計(jì)算量大,效率低。A*算法在搜索過(guò)程中同時(shí)計(jì)算g(n)和h(n),在考慮搜索效率的同時(shí)保證得到最優(yōu)路徑。

A*算法在執(zhí)行路徑規(guī)劃任務(wù)時(shí)主要用2個(gè)表實(shí)現(xiàn)節(jié)點(diǎn)的擴(kuò)展和最優(yōu)點(diǎn)的選擇,即通過(guò)Openlist表和Closelist表來(lái)記錄節(jié)點(diǎn)。其中Openlist表是為了保存搜索過(guò)程中遇到的擴(kuò)展節(jié)點(diǎn),同時(shí)將這些節(jié)點(diǎn)按代價(jià)進(jìn)行排序,選出其中f(n)值最小的節(jié)點(diǎn),作為下一個(gè)擴(kuò)展節(jié)點(diǎn);然后又將該節(jié)點(diǎn)的所有鄰近節(jié)點(diǎn)存入Openlist表中,直至目標(biāo)點(diǎn)也被存入[16]。

2.2 特征點(diǎn)初優(yōu)化策略

2.2.1行間優(yōu)化

結(jié)合果園噴霧機(jī)器人實(shí)際工作情況,要求果園噴霧機(jī)器人行駛在樹(shù)行中心線上,為噴霧作業(yè)效果與行駛安全提供保障。因此,在通過(guò)A*算法獲得全局路徑點(diǎn)后需要進(jìn)行優(yōu)化處理。

將路徑點(diǎn)提取為特征點(diǎn)后,首先對(duì)獲得的路徑點(diǎn)進(jìn)行行間判斷,判斷特征點(diǎn)是否在兩排樹(shù)行當(dāng)中,并將處于行間的特征點(diǎn)投影至樹(shù)行中心線。為了方便計(jì)算,首先建立一個(gè)相對(duì)坐標(biāo)系。假設(shè)所有樹(shù)行均平行且與全局坐標(biāo)系Y軸夾角為θ,則將全局坐標(biāo)系旋轉(zhuǎn)θ至樹(shù)行與Y軸平行,如圖3所示。此時(shí)樹(shù)行亦與X軸垂直,利用旋轉(zhuǎn)矩陣計(jì)算出各路徑點(diǎn)坐標(biāo)以及樹(shù)行邊界點(diǎn)坐標(biāo),即

圖3 坐標(biāo)轉(zhuǎn)換示意圖

(2)

完成坐標(biāo)轉(zhuǎn)換后,此時(shí)便可通過(guò)特征點(diǎn)與樹(shù)行邊界的X軸坐標(biāo)來(lái)判斷其是否在樹(shù)行之中,并可以通過(guò)改變X軸坐標(biāo)將路徑點(diǎn)投影到樹(shù)行中心線上,無(wú)需進(jìn)行投影計(jì)算,其判斷方法為

(i=0,1,…,n;j=0,1,…,m)

(3)

式中i——路徑點(diǎn)序號(hào),共有n+1個(gè)路徑點(diǎn)

j——樹(shù)行邊界序號(hào),共有m+1個(gè)樹(shù)行邊界

Xj——樹(shù)行邊界X軸坐標(biāo)

此時(shí)行間的特征點(diǎn)已全部投影到了樹(shù)行中心線上,再將相對(duì)坐標(biāo)系中的特征點(diǎn)轉(zhuǎn)換回全局坐標(biāo)系下,即將坐標(biāo)系反方向旋轉(zhuǎn)θ計(jì)算出特征點(diǎn)坐標(biāo),即

(4)

2.2.2路徑點(diǎn)分段優(yōu)化

將特征點(diǎn)初步優(yōu)化處理后,為更好地發(fā)揮三次非均勻B樣條曲線在表達(dá)曲線形狀特征方面的優(yōu)勢(shì),在進(jìn)行插值前仍需對(duì)特征點(diǎn)進(jìn)行處理[17]。由于路徑點(diǎn)的曲率信息能夠較好地反映曲線的形狀特征且對(duì)于后續(xù)生成的全局路徑的實(shí)用性有較大參考價(jià)值,把特征點(diǎn)與曲率關(guān)聯(lián)起來(lái)進(jìn)行優(yōu)化處理,可以更好地顯示曲線的形狀特征。

(1)曲率計(jì)算

采用“三點(diǎn)法”來(lái)計(jì)算初始特征點(diǎn)曲率[18],即用相鄰3個(gè)特征點(diǎn)形成近似圓弧的方法來(lái)對(duì)各個(gè)初始特征點(diǎn)的曲率進(jìn)行求解。當(dāng)存在n+1個(gè)路徑點(diǎn)Qi(i=0,1,…,n),其對(duì)應(yīng)的曲率半徑為ρi(i=0,1,…,n),近似求解公式為

(5)

曲率ki為

(6)

(2)曲率分段點(diǎn)計(jì)算

初步優(yōu)化后的特征點(diǎn)組成的線段按照曲率大小可劃分為若干個(gè)大曲率段和若干個(gè)小曲率段。大曲率段曲線是路徑中的轉(zhuǎn)彎部分,彎曲程度較大,需要更加細(xì)化的計(jì)算來(lái)確定最終特征點(diǎn)的位置;小曲率段相對(duì)而言較為平直,即表示在經(jīng)過(guò)初步優(yōu)化后的行間直行部分,在三次非均勻B樣條曲線中,可通過(guò)特征點(diǎn)的位置限制拉伸為直線。

當(dāng)?shù)趇個(gè)特征點(diǎn)Qi對(duì)應(yīng)的曲率ki與鄰近的路徑點(diǎn)之間滿足以下條件之一時(shí),則可將Qi初步選作曲率分段點(diǎn)[19],即

(7)

上述條件可保證特征點(diǎn)Qi一側(cè)的曲率大于該點(diǎn)處曲率。但由于特征點(diǎn)曲率可能會(huì)出現(xiàn)跳躍,因此僅滿足單點(diǎn)大小判斷條件不能保證特征點(diǎn)Qi兩側(cè)的鄰近特征點(diǎn)中一側(cè)的曲率遠(yuǎn)大于另一側(cè)特征點(diǎn)的曲率。為保證分段點(diǎn)的準(zhǔn)確性,還需要根據(jù)不同情況對(duì)特征點(diǎn)進(jìn)行再篩選并最終確定分段點(diǎn)。

當(dāng)?shù)趇個(gè)特征點(diǎn)Qi對(duì)應(yīng)的曲率ki滿足ki-j>ki時(shí),計(jì)算其向前差分與向后差分的值并進(jìn)行判斷,其公式為

(8)

(9)

(10)

同理,當(dāng)?shù)趇個(gè)路徑點(diǎn)Qi對(duì)應(yīng)的曲率ki滿足ki+j>ki時(shí)有

(11)

(12)

(13)

在每個(gè)轉(zhuǎn)彎曲線內(nèi)存在一個(gè)曲率極大值點(diǎn),曲率極大值點(diǎn)對(duì)于曲線形狀的影響有著重要的作用,對(duì)假設(shè)段內(nèi)存在d個(gè)路徑點(diǎn),則曲率極大值為

kmax=max(k1,k2,…,kd)

(14)

該點(diǎn)即為曲率極大值點(diǎn)。

在使用A*算法搜索路徑點(diǎn)的過(guò)程中,由于存在啟發(fā)函數(shù)h(n)使得搜索過(guò)程具有方向性,一部分路徑點(diǎn)會(huì)向著目標(biāo)點(diǎn)方向偏移。這種偏移會(huì)導(dǎo)致部分路徑形狀特征復(fù)雜、不利于果園噴霧機(jī)器人的實(shí)際跟蹤行駛。提取特征點(diǎn)后通過(guò)行間優(yōu)化,行間特征點(diǎn)的偏移問(wèn)題得到解決。在確定分段點(diǎn)位置后,對(duì)起始點(diǎn)與首個(gè)分段點(diǎn)之間的路徑點(diǎn)進(jìn)行矯正,投影至包含起始點(diǎn)的平行于樹(shù)行的直線上,其效果如圖4所示。在已知果樹(shù)位置的情況下,根據(jù)果樹(shù)直徑與整體樹(shù)行的長(zhǎng)度對(duì)整體樹(shù)行做擬合處理并依次將樹(shù)行延長(zhǎng)至果園邊界形成“U”形走廊。

圖4 特征點(diǎn)初優(yōu)化對(duì)比

2.3 多約束條件下三次非均勻B樣條軌跡優(yōu)化

本文研究對(duì)象為阿克曼底盤(pán)果園噴霧機(jī)器人,其具有非完整性約束的特點(diǎn),若要改變航向,需要滿足曲率約束條件。因此在換行轉(zhuǎn)彎情況下,對(duì)于2.2節(jié)中確定的初始特征點(diǎn),需對(duì)其中轉(zhuǎn)彎曲率不滿足機(jī)器人轉(zhuǎn)彎條件的特征點(diǎn)進(jìn)行修正,優(yōu)化流程如圖5所示。

圖5 軌跡優(yōu)化算法流程圖

2.3.1果園噴霧機(jī)器人運(yùn)動(dòng)學(xué)模型建立

果園噴霧機(jī)器人在果園執(zhí)行噴霧作業(yè)任務(wù)時(shí)運(yùn)動(dòng)速度較為緩慢,在低速行駛時(shí)受到的側(cè)向力較小,假設(shè)其不發(fā)生側(cè)滑現(xiàn)象,因此在進(jìn)行路徑規(guī)劃時(shí)只考慮機(jī)器人運(yùn)動(dòng)學(xué)模型。

假設(shè)果園噴霧機(jī)器人車(chē)體為剛性,車(chē)輪變形可忽略不計(jì),且車(chē)輪與地面的每個(gè)接觸點(diǎn)僅產(chǎn)生滾動(dòng)無(wú)滑動(dòng),其模型如圖6所示。由于兩個(gè)前輪的轉(zhuǎn)向角不同造成了兩個(gè)后輪旋轉(zhuǎn)半徑也不相同,于是做出了相應(yīng)的簡(jiǎn)化,在前軸中心擬合出一個(gè)中心前輪,其方向表示機(jī)器人整體的運(yùn)動(dòng)方向。因此轉(zhuǎn)向角計(jì)算式為

圖6 果園噴霧機(jī)器人底盤(pán)運(yùn)動(dòng)學(xué)模型

(15)

式中δ——簡(jiǎn)化后車(chē)輛前輪轉(zhuǎn)向角

δl——左側(cè)前輪轉(zhuǎn)角

δr——右側(cè)前輪轉(zhuǎn)角

簡(jiǎn)化后的模型稱為自行車(chē)模型,計(jì)算式為

(16)

(17)

式中l(wèi)——前后軸之間距離

R——旋轉(zhuǎn)半徑vr——后輪速度

ω——后輪與目標(biāo)點(diǎn)夾角

2.3.2三次非均勻B樣條曲線

一條p次NURBS曲線可以表示為[20]

(18)

式中Pi——NURBS曲線的控制點(diǎn)

ωi——控制點(diǎn)的權(quán)因子

控制點(diǎn)個(gè)數(shù)為n+1個(gè),所有控制點(diǎn)的連線為NURBS曲線的特征多邊形,節(jié)點(diǎn)個(gè)數(shù)為m+1,其中m=n+p+1,Ni,k(u)即B樣條基函數(shù),可由De Boor-Cox遞推公式推導(dǎo)得出[21]

(19)

選用累積弦長(zhǎng)參數(shù)化法進(jìn)行特征點(diǎn)的參數(shù)化處理[22]。累積弦長(zhǎng)參數(shù)化法的節(jié)點(diǎn)矢量參數(shù)計(jì)算公式為

(20)

其中

式中 ΔQi——向前差分矢量

s——所有特征點(diǎn)的相鄰距離總和

為了簡(jiǎn)化軌跡曲線計(jì)算過(guò)程,將式(18)中的權(quán)因子ωi均設(shè)為1,即將NURBS曲線簡(jiǎn)化為非均勻B樣條曲線[23],其表達(dá)式為

(21)

式(21)表示的線性方程組由n+1個(gè)矢量方程組成,包含n+3個(gè)控制點(diǎn),未知控制點(diǎn)數(shù)大于方程數(shù)。因此,還需要添2個(gè)邊界條件來(lái)求解方程組。采用切失條件作為邊界條件。整理可得

(22)

其中

ei=(Δi+2+Δi+1)Qi-1=(ui+3-ui+1)Qi-1

(1≤i≤n)

取首末重復(fù)度為4,三次非均勻B樣條曲線的首末控制頂點(diǎn)即為特征點(diǎn),即Q0=P0,Qn=Pn+2。且首末處有切失條件[24]

(23)

式中q0、qn——首末切失條件

由式(20)可計(jì)算出節(jié)點(diǎn)矢量,代入式(22)可計(jì)算出所有控制點(diǎn),將控制點(diǎn)代入式(21)即可求出三次非均勻B樣條曲線。

2.3.3基于多約束條件的三次非均勻B樣條曲線

在果園噴霧機(jī)器人的路徑規(guī)劃過(guò)程中對(duì)于軌跡曲線應(yīng)有以下要求:

(1)曲率連續(xù)約束[25]。為避免噴霧機(jī)行駛過(guò)程中出現(xiàn)急轉(zhuǎn)、急停等現(xiàn)象,需要確保軌跡曲線的曲率連續(xù)。采用三次非均勻B樣條曲線,滿足C2連續(xù)性質(zhì)。

(3)轉(zhuǎn)向機(jī)構(gòu)延遲約束。對(duì)于絕大多數(shù)農(nóng)業(yè)機(jī)器人,其前輪轉(zhuǎn)向機(jī)構(gòu)具有一定的時(shí)間延遲,即從指令下發(fā)到轉(zhuǎn)向機(jī)構(gòu)做出角度轉(zhuǎn)變需要一定的轉(zhuǎn)換時(shí)間,這段時(shí)間差會(huì)使得機(jī)器人在實(shí)際跟蹤行駛過(guò)程中出現(xiàn)誤差。這種延遲帶來(lái)的誤差可以通過(guò)兩方面來(lái)減少。一方面是優(yōu)化轉(zhuǎn)向機(jī)構(gòu)減少反應(yīng)時(shí)間,這種方式直接有效但會(huì)增加成本且無(wú)法根本解決問(wèn)題;另一方面可以將反應(yīng)時(shí)間加入軌跡的優(yōu)化過(guò)程中,通過(guò)增加轉(zhuǎn)向角的角速度約束來(lái)保證給機(jī)器人留有足夠的反應(yīng)時(shí)間從而減少延遲帶來(lái)的誤差。

對(duì)于兩個(gè)分段中的轉(zhuǎn)彎曲線,可以通過(guò)調(diào)整其特征點(diǎn)的方式來(lái)改變曲線的形狀,其中特征點(diǎn)即為位置約束點(diǎn),默認(rèn)將前面優(yōu)化后的路徑點(diǎn)作為特征點(diǎn)。特征點(diǎn)與曲線關(guān)系示意圖如圖7所示。

圖7 轉(zhuǎn)彎曲線特征點(diǎn)與曲線關(guān)系

圖中點(diǎn)Q1、Q5為分段點(diǎn)、曲率極大值點(diǎn)。在已知分段點(diǎn)、曲率極大值點(diǎn)的條件下,通過(guò)改變點(diǎn)Q2和Q4的位置可以改變曲線的形狀,其中與兩點(diǎn)設(shè)定為關(guān)于點(diǎn)Q3對(duì)稱,簡(jiǎn)化了計(jì)算且符合轉(zhuǎn)彎邏輯。因此,通過(guò)求解多約束條件下點(diǎn)Q2的坐標(biāo)即可得到最優(yōu)的轉(zhuǎn)彎軌跡。點(diǎn)Q0與Q6分別為兩側(cè)分段點(diǎn)的前后一個(gè)特征點(diǎn),分別形成兩條直線,即部分行間軌跡。在優(yōu)化計(jì)算的過(guò)程中,代入兩個(gè)特征點(diǎn)Q0與Q6可以保證轉(zhuǎn)角軌跡與行間軌跡曲率的約束以及連續(xù)性。

如圖7所示,轉(zhuǎn)角軌跡優(yōu)化的初始已知條件為:噴霧機(jī)起始位置為Q0(x0,y0)、目標(biāo)位置為Q6(x6,y6)、轉(zhuǎn)角頂點(diǎn)為Q3(x3,y3),變量點(diǎn)Q2(x2,y2),可得Q4坐標(biāo)為

(24)

該軌跡上任一點(diǎn)曲率為[28]

(25)

式中u——三次B樣條曲線的節(jié)點(diǎn)向量

機(jī)器人底盤(pán)的最小轉(zhuǎn)彎半徑為Rmin,軌跡曲線應(yīng)滿足約束條件

(26)

機(jī)器人底盤(pán)跟蹤軌跡曲線時(shí),其前輪轉(zhuǎn)向角為

(27)

前輪轉(zhuǎn)角不能超過(guò)機(jī)器人底盤(pán)的最大前輪轉(zhuǎn)角。

當(dāng)選擇變量Q2的坐標(biāo)時(shí),易見(jiàn)得Q2應(yīng)處于Q1與Q3連線的上方,否則軌跡曲率會(huì)變化較大,不利于跟蹤。因此有約束條件

(28)

對(duì)于果園噴霧機(jī)器人,其前輪轉(zhuǎn)向角度差為

δmax=2θ

(29)

式中θ——轉(zhuǎn)向機(jī)構(gòu)單向最大轉(zhuǎn)角

因此果園噴霧機(jī)器人最大轉(zhuǎn)向角速度為

(30)

式中ts——轉(zhuǎn)向機(jī)構(gòu)動(dòng)作反應(yīng)時(shí)間

在果園噴霧機(jī)器人的軌跡規(guī)劃過(guò)程中應(yīng)該考慮到其曲率所反映的前輪轉(zhuǎn)角變化速度應(yīng)小于轉(zhuǎn)向機(jī)構(gòu)的最大角速度。

對(duì)于轉(zhuǎn)向機(jī)構(gòu)的角速度約束,則需要轉(zhuǎn)向角參數(shù)和時(shí)間參數(shù)。由式(27)和自行車(chē)模型下的幾何公式得轉(zhuǎn)向角為

(31)

(32)

式中l(wèi)d——前視距離

α——機(jī)器人位置與目標(biāo)點(diǎn)夾角

在進(jìn)行角速度規(guī)劃時(shí),如果曲線長(zhǎng)度未知,則很難進(jìn)行時(shí)間規(guī)劃,因此,可以通過(guò)積分來(lái)計(jì)算曲線長(zhǎng)度[29]

(33)

通過(guò)路徑長(zhǎng)度計(jì)算出每一段的預(yù)期時(shí)間td為

(34)

式中vs——期望速度

n——B樣條曲線段數(shù)

因此對(duì)于每一段路徑曲線都有轉(zhuǎn)向角速度約束

(35)

其中dt的分布范圍為[0,ntd]。

2.3.4參數(shù)最優(yōu)化求解

假設(shè)轉(zhuǎn)角軌跡上任一點(diǎn)的曲率為κ(u)。用三次非均勻B樣條曲線將軌跡表示后,使用最優(yōu)化的方法來(lái)求解在滿足多約束條件下的曲線參數(shù)。最優(yōu)化的目標(biāo)為轉(zhuǎn)角軌跡曲線的曲率均值,即

(36)

通過(guò)控制變量Q2的坐標(biāo),改變曲線形狀,使得曲率均值最小,獲得光滑連續(xù)的軌跡曲線[30]。同時(shí),最優(yōu)化過(guò)程有以下約束條件

(37)

3 仿真及驗(yàn)證分析

3.1 全局路徑仿真與評(píng)估

在第2節(jié)建立轉(zhuǎn)彎曲線的數(shù)學(xué)模型后,以式(36)為目標(biāo)函數(shù),點(diǎn)Q2的坐標(biāo)為變量,式(37)為約束條件,調(diào)用PSO函數(shù),可求得在轉(zhuǎn)彎場(chǎng)景下的最優(yōu)曲線參數(shù)[31]。將各轉(zhuǎn)彎曲線的特征點(diǎn)參數(shù)代入式(21)可計(jì)算出最終用三次非均勻B樣條曲線表示的全局路徑如圖8所示。

圖8 最終三次非均勻B樣條全局路徑曲線

圖8中綠色圓點(diǎn)為模擬果樹(shù),黑色矩形為障礙物信息。實(shí)際果園中,同一樹(shù)行的果樹(shù)存在間距但噴霧機(jī)不應(yīng)從中穿過(guò),因此以果樹(shù)的寬度與樹(shù)行的長(zhǎng)度整體生成矩形障礙物。同時(shí),為保證噴霧機(jī)以“U”形運(yùn)動(dòng)軌跡遍歷果園,分別將障礙物延伸至果園邊界,形成“U”形長(zhǎng)廊式結(jié)構(gòu),不同方法的優(yōu)化結(jié)果如圖9、10所示。

圖9 軌跡優(yōu)化方法對(duì)比

圖10 U形環(huán)境軌跡曲率對(duì)比

3.2 軌跡跟蹤仿真驗(yàn)證

在Matlab中根據(jù)果園噴霧機(jī)器人底盤(pán)模型編寫(xiě)跟蹤代碼進(jìn)行跟蹤仿真試驗(yàn)。選用純跟蹤算法,設(shè)定軸距為0.614 m,前視距離為1 m。將生成的全局路徑導(dǎo)入跟蹤算法的參考路徑中,觀察其橫向誤差、航向角以及前輪轉(zhuǎn)角。

分別對(duì)傳統(tǒng)無(wú)約束B(niǎo)樣條函數(shù)優(yōu)化的軌跡曲線和本文基于多約束條件優(yōu)化生成的軌跡曲線進(jìn)行跟蹤仿真,其結(jié)果如圖11、12所示。無(wú)約束路徑點(diǎn)存在較大轉(zhuǎn)角,由于軸距和前輪轉(zhuǎn)角的限制無(wú)法精準(zhǔn)跟蹤,產(chǎn)生較大的前輪轉(zhuǎn)角波動(dòng),影響果園噴霧機(jī)器人行駛穩(wěn)定性。跟蹤橫向誤差如圖13和表1所示,原始折線路徑由于存在尖角其誤差最大。通過(guò)增加三次B樣條函數(shù)優(yōu)化與約束條件,可以有效縮小橫向誤差。

表1 仿真機(jī)器人跟蹤軌跡誤差

圖11 無(wú)約束B(niǎo)樣條優(yōu)化軌跡跟蹤結(jié)果

圖12 多約束條件下B樣條優(yōu)化軌跡跟蹤結(jié)果

圖13 軌跡跟蹤仿真誤差對(duì)比

4 跟蹤試驗(yàn)驗(yàn)證

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

本文算法驗(yàn)證使用阿克曼底盤(pán)的無(wú)人噴霧機(jī)器人作為試驗(yàn)平臺(tái)[32],如圖14所示。無(wú)人噴霧機(jī)器人主要由機(jī)器人底盤(pán)(hunter2.0,深圳市松靈機(jī)器人)、中央計(jì)算機(jī)(英特爾酷睿八代i7-8565u)、組合導(dǎo)航系統(tǒng)(高精度組合導(dǎo)航系統(tǒng)X1,北云科技)和噴霧作業(yè)機(jī)構(gòu)組成。

圖14 果園噴霧機(jī)器人

本研究使用中央計(jì)算機(jī)裝載Ubuntu18.04LTS操作系統(tǒng),并基于機(jī)器人操作系統(tǒng)(Robot operating system,ROS)進(jìn)行開(kāi)發(fā)。基于ROS平臺(tái)編寫(xiě)機(jī)器人通訊與行駛代碼,并將規(guī)劃后的路徑載入預(yù)編好的容器中,通過(guò)可視化軟件RVIZ可觀察規(guī)劃路徑與實(shí)際行駛軌跡。

4.2 試驗(yàn)設(shè)計(jì)

試驗(yàn)場(chǎng)地位于江蘇大學(xué)果園(32.218 879 1°N, 119.509 957 2°E)。在該試驗(yàn)場(chǎng)地中,共有5列樹(shù)行,行距平均為3.3 m,株距平均為0.8 m,每列樹(shù)行平均長(zhǎng)10.5 m,如圖15所示。主要考慮果園環(huán)境特征為連續(xù)的U形走廊,將果樹(shù)樹(shù)行視為障礙物后搜索出從起始點(diǎn)到末行末尾的路徑,并采用不同的優(yōu)化算法進(jìn)行處理。

圖15 果園環(huán)境試驗(yàn)場(chǎng)景

采用純跟蹤算法控制果園噴霧機(jī)器人對(duì)不同的軌跡進(jìn)行跟蹤行駛,并實(shí)時(shí)計(jì)算橫向誤差作為評(píng)價(jià)指標(biāo)。純跟蹤算法的前視距離與速度會(huì)影響跟蹤效果,為避免跟蹤時(shí)出現(xiàn)振蕩不穩(wěn)定的情況,前視距離設(shè)置為1.0 m。同時(shí)為貼合實(shí)際工作情況,行駛速度設(shè)置為起始速度0.5 m/s并勻速加速至最大速度1.2 m/s。

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

通過(guò)對(duì)不同算法生成的路徑進(jìn)行跟蹤試驗(yàn),過(guò)程通過(guò)北斗導(dǎo)航系統(tǒng)中采集噴霧機(jī)器人實(shí)時(shí)位置信息并記錄為文本文件,將該文本文件導(dǎo)入Matlab中[33]得到果園環(huán)境下的跟蹤結(jié)果如圖16所示。由圖16可看出,當(dāng)進(jìn)行直線跟蹤時(shí)其實(shí)際行駛軌跡與目標(biāo)路徑較為貼合,橫向誤差較小;當(dāng)進(jìn)行轉(zhuǎn)彎行駛時(shí),皆與目標(biāo)路徑產(chǎn)生了較大差距,即橫向誤差變大。轉(zhuǎn)彎處的橫向誤差主要有2個(gè)原因:①純跟蹤算法的固有缺陷。②目標(biāo)路徑不符合果園噴霧機(jī)器人的行駛要求,機(jī)器人無(wú)法行駛到該路徑的位置點(diǎn)上。在圖16a中A*算法生成的路徑點(diǎn)折線段的曲率較大,機(jī)器人無(wú)法跟蹤到其線段的轉(zhuǎn)折點(diǎn)上進(jìn)而出現(xiàn)橫向誤差;在圖16b中,無(wú)約束均勻B樣條曲線兩側(cè)轉(zhuǎn)彎處的軌跡形狀復(fù)雜,且中間兩處曲率較大,由于果園噴霧機(jī)器人底盤(pán)存在軸距與前輪轉(zhuǎn)角的限制,其整體轉(zhuǎn)彎角存在最大角度,當(dāng)目標(biāo)路徑點(diǎn)的曲率較大時(shí),機(jī)器人無(wú)法從當(dāng)前位置點(diǎn)行駛至下一個(gè)目標(biāo)點(diǎn),從而出現(xiàn)了較大橫向誤差;在圖16c中,增加約束條件的優(yōu)化軌跡其曲率較小,且無(wú)較大波動(dòng),因此從3種算法的跟蹤行駛效果中可以較為直觀地觀測(cè)出多約束條件下非均勻B樣條優(yōu)化算法生成的目標(biāo)路徑更符合機(jī)器人的行駛要求。

圖16 果園環(huán)境下規(guī)劃路徑跟蹤結(jié)果

如圖17與表2所示,在起始時(shí)由于距離目標(biāo)軌跡較遠(yuǎn)造成誤差較大。在機(jī)器人行駛到目標(biāo)軌跡直線部分時(shí),誤差開(kāi)始收斂,此時(shí)開(kāi)始計(jì)算平均誤差。

表2 果園環(huán)境下軌跡曲率及機(jī)器人跟蹤效果

圖17 果園環(huán)境中規(guī)劃路徑跟蹤誤差對(duì)比

轉(zhuǎn)彎處3種算法生成軌跡的跟蹤誤差差距較為明顯,經(jīng)多約束條件下三次非均勻B樣條優(yōu)化后的軌跡曲線其曲率最小,跟蹤誤差最小,行駛過(guò)程中最大曲率為0.31 m-1,橫向誤差標(biāo)準(zhǔn)差為0.031 m,符合果園噴霧機(jī)器人作業(yè)要求。

5 結(jié)論

(1)針對(duì)傳統(tǒng)A*算法在果園環(huán)境中出現(xiàn)的路徑點(diǎn)偏移不適合直接代入優(yōu)化的問(wèn)題,提出了一種初優(yōu)化策略,根據(jù)樹(shù)行位置信息矯正行間路徑點(diǎn)。根據(jù)曲率分段劃分路徑點(diǎn),并結(jié)合始末端點(diǎn)矯正首末樹(shù)行外側(cè)路徑點(diǎn)。

(2)針對(duì)果園環(huán)境中噴霧機(jī)器人自主行駛過(guò)程,提出了一種基于三次非均勻B樣條曲線的果園行駛路徑規(guī)劃方法。結(jié)合噴霧機(jī)器人最小轉(zhuǎn)彎半徑約束、轉(zhuǎn)彎處首末端點(diǎn)連續(xù)約束、轉(zhuǎn)向機(jī)構(gòu)延遲約束等,建立多約束條件,以曲率均值最小為優(yōu)化目標(biāo)建立數(shù)學(xué)模型并求解。生成軌跡的最大曲率為0.31 m-1,平均曲率為0.15 m-1,相對(duì)于初始A*算法和三次均勻B樣條算法大大縮小了軌跡曲率,符合果園噴霧機(jī)器人運(yùn)動(dòng)學(xué)約束。

(3)跟蹤試驗(yàn)表明,果園噴霧機(jī)器人對(duì)果園行駛路徑規(guī)劃算法生成的路徑可以較好地進(jìn)行跟蹤,平均橫向誤差為0.225 m,標(biāo)準(zhǔn)差為0.031 m,滿足果園噴霧機(jī)器人行駛精度要求。