沈 躍 劉子涵 劉 慧 杜 偉

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

0 引言

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

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

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

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

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

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

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

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

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

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

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

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

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

2.1 A*算法路徑點搜索

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

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

(1)

式中n——待擴展的節(jié)點

g(n)——從起點到當前節(jié)點的實際路徑代價

h(n)——啟發(fā)函數(shù),前節(jié)點到目標點估計代價值

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

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

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

2.2.1行間優(yōu)化

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

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

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

(2)

完成坐標轉(zhuǎn)換后,此時便可通過特征點與樹行邊界的X軸坐標來判斷其是否在樹行之中,并可以通過改變X軸坐標將路徑點投影到樹行中心線上,無需進行投影計算,其判斷方法為

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

(3)

式中i——路徑點序號,共有n+1個路徑點

j——樹行邊界序號,共有m+1個樹行邊界

Xj——樹行邊界X軸坐標

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

(4)

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

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

(1)曲率計算

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

(5)

曲率ki為

(6)

(2)曲率分段點計算

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

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

(7)

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

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

(8)

(9)

(10)

同理,當?shù)趇個路徑點Qi對應(yīng)的曲率ki滿足ki+j>ki時有

(11)

(12)

(13)

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

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

(14)

該點即為曲率極大值點。

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

圖4 特征點初優(yōu)化對比

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

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

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

2.3.1果園噴霧機器人運動學(xué)模型建立

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

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

圖6 果園噴霧機器人底盤運動學(xué)模型

(15)

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

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

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

簡化后的模型稱為自行車模型,計算式為

(16)

(17)

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

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

ω——后輪與目標點夾角

2.3.2三次非均勻B樣條曲線

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

(18)

式中Pi——NURBS曲線的控制點

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

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

(19)

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

(20)

其中

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

s——所有特征點的相鄰距離總和

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

(21)

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

(22)

其中

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

(1≤i≤n)

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

(23)

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

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

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

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

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

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

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

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

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

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

(24)

該軌跡上任一點曲率為[28]

(25)

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

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

(26)

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

(27)

前輪轉(zhuǎn)角不能超過機器人底盤的最大前輪轉(zhuǎn)角。

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

(28)

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

δmax=2θ

(29)

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

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

(30)

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

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

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

(31)

(32)

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

α——機器人位置與目標點夾角

在進行角速度規(guī)劃時,如果曲線長度未知,則很難進行時間規(guī)劃,因此,可以通過積分來計算曲線長度[29]

(33)

通過路徑長度計算出每一段的預(yù)期時間td為

(34)

式中vs——期望速度

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

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

(35)

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

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

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

(36)

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

(37)

3 仿真及驗證分析

3.1 全局路徑仿真與評估

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

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

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

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

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

3.2 軌跡跟蹤仿真驗證

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

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

表1 仿真機器人跟蹤軌跡誤差

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

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

圖13 軌跡跟蹤仿真誤差對比

4 跟蹤試驗驗證

4.1 試驗平臺

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

圖14 果園噴霧機器人

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

4.2 試驗設(shè)計

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

圖15 果園環(huán)境試驗場景

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

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

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

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

如圖17與表2所示,在起始時由于距離目標軌跡較遠造成誤差較大。在機器人行駛到目標軌跡直線部分時,誤差開始收斂,此時開始計算平均誤差。

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

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

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

5 結(jié)論

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

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

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