跟蹤算法下的果樹修剪機(jī)器人路徑規(guī)劃設(shè)計(jì)

方小菊,黃亦其

(1.廣西職業(yè)技術(shù)學(xué)院,南寧 530226;2.廣西大學(xué),南寧 530000)

0 引言

果樹修剪機(jī)器人作業(yè)過程中要求具有較高的作業(yè)覆蓋率和較低的作業(yè)重復(fù)率,同時(shí)要求作業(yè)過程中能根據(jù)設(shè)定的路徑進(jìn)行跟蹤作業(yè),達(dá)到較高的作業(yè)效率[1-2]。為此,以跟蹤算法為基礎(chǔ),設(shè)計(jì)一種橫向偏差路徑跟蹤規(guī)劃算法,對(duì)修剪機(jī)器人的轉(zhuǎn)向過程進(jìn)行控制,為果樹修剪機(jī)器人路徑跟蹤控制過程提供參考。

1 修剪機(jī)器人運(yùn)動(dòng)學(xué)模型

修剪機(jī)器人在果園中進(jìn)行作業(yè)時(shí),果園地面相對(duì)平坦,進(jìn)行轉(zhuǎn)向時(shí),假設(shè)機(jī)器人輪胎不發(fā)生滑移,由此可建立果樹修剪機(jī)器人四輪運(yùn)動(dòng)學(xué)模型,如圖1所示。

圖1 修剪機(jī)器人運(yùn)動(dòng)學(xué)模型示意圖Fig.1 Kinematic model of pruning robot

在笛卡爾坐標(biāo)系中,假設(shè)修剪機(jī)器人的實(shí)時(shí)位置為H(x,y),L表示軸距,θ表示航向角,R表示瞬時(shí)轉(zhuǎn)彎半徑,δ表示轉(zhuǎn)向輪轉(zhuǎn)角,ω表示修剪機(jī)器人行走系統(tǒng)監(jiān)測顯示轉(zhuǎn)角。修剪機(jī)器人的運(yùn)動(dòng)學(xué)方程可表示為

顯示轉(zhuǎn)角ω可表示為

2 修剪機(jī)器人路徑規(guī)劃算法

果樹修剪機(jī)器人在作業(yè)過程中的行走路線主要為長直線,依靠修剪機(jī)構(gòu)的空間位移執(zhí)行修剪動(dòng)作,因此修剪機(jī)器人行走路徑可假設(shè)為兩行果樹之間的條形區(qū)域[3-4]。果樹直徑一般小于修剪機(jī)器人的工作有效區(qū),修剪機(jī)器人經(jīng)過果樹時(shí),受到空間限制會(huì)形成不同的作業(yè)條件[5-7]。圖2所示為修剪機(jī)器人作業(yè)條件示意圖。其中,圖2(a)為修剪機(jī)器人行走在兩行果樹中間;圖2(b)為修剪機(jī)器人行走路徑靠近右側(cè)果樹;圖2(c)為修剪機(jī)器人行走路徑靠近左側(cè)果樹,圖2(d)為修剪機(jī)器人行走在兩行果樹中間,且修剪機(jī)器人有效工作空間無法達(dá)到果樹空間。在前3種工作條件下,修剪機(jī)器人能夠圍繞果樹進(jìn)行避障,避免修剪過程中出現(xiàn)漏剪的概率較小;在第4種工作條件下,修剪機(jī)器人會(huì)導(dǎo)致果樹一側(cè)無法進(jìn)行有效修剪,當(dāng)需要對(duì)另一側(cè)果樹進(jìn)行修剪時(shí),需繞行至果樹另外一側(cè)進(jìn)行修剪[8-9]。在進(jìn)行果樹修剪機(jī)器人路徑規(guī)劃時(shí),采用區(qū)域分割的方法將果園按照從左至右的順序進(jìn)行分解[10],則修剪區(qū)域可表示為

其中,r表示果樹半徑;d表示果樹圓心至果園左側(cè)邊界距離;w表示果樹所在區(qū)域邊界到果園左側(cè)邊界的距離。

圖2 修剪機(jī)器人作業(yè)條件示意圖Fig.2 Schematic diagram of working conditions of pruning robot

將修剪機(jī)器人四輪運(yùn)動(dòng)學(xué)模型進(jìn)行簡化,可得出修剪機(jī)器人的二自由度運(yùn)動(dòng)學(xué)模型,如圖3所示。以修剪機(jī)器人后軸為切點(diǎn),改變前輪方向,使修剪機(jī)器人可以沿設(shè)定路徑圓弧運(yùn)動(dòng)。

圖3 修剪機(jī)器人二自由度運(yùn)動(dòng)學(xué)模型Fig.3 Two degree of freedom kinematic model of pruning robot

假設(shè)修剪機(jī)器人目標(biāo)點(diǎn)P(gx,gy),ld為修剪機(jī)器人的前視距離,α為修剪機(jī)器人和目標(biāo)點(diǎn)之間的夾角,在跟蹤算法條件下,修剪機(jī)器人的后輪轉(zhuǎn)向可近似為一條圓弧[11-12]。根據(jù)正弦定理可以得出

修剪機(jī)器人前輪轉(zhuǎn)角δ在時(shí)刻的跟蹤算法控制表達(dá)式可表示為

在實(shí)際運(yùn)動(dòng)過程中,采用跟蹤算法計(jì)算得出修剪機(jī)器人的轉(zhuǎn)角預(yù)期值,隨著修剪過程位置的變化,需要對(duì)修剪機(jī)器人的轉(zhuǎn)角進(jìn)行實(shí)時(shí)更新,將修剪機(jī)器人的前視距離表示為修剪機(jī)器人縱向行進(jìn)速度的線性函數(shù)[13-14]。因此,可以得出

ld=kv(x)+Lfc

其中,k表示修剪機(jī)器人預(yù)瞄準(zhǔn)速度增益量;Lfc表示修剪機(jī)器人靜態(tài)預(yù)瞄準(zhǔn)距離;v(x)表示修剪機(jī)器人縱向行進(jìn)速度。

圖4所示為修剪機(jī)器人前視距離選擇策略示意圖。假設(shè)修剪機(jī)器人的目標(biāo)路徑左側(cè)和右側(cè)存在點(diǎn)O1(x1,y1)和O2(x2,y2),f表示修剪機(jī)器人當(dāng)前橫向誤差值,P0、P1、P2和P3為目標(biāo)路徑上的不同位置點(diǎn),則路徑上任意點(diǎn)求解公式為

圖4 修剪機(jī)器人前視距離選擇策略示意圖Fig.4 Schematic diagram of forward looking distance selection strategy for pruning robot

假設(shè)修剪機(jī)器人作業(yè)路徑由無數(shù)點(diǎn)(xi,yi)組成,則

當(dāng)修剪機(jī)器人位于左側(cè)點(diǎn)O1(x1,y1)時(shí),假設(shè)S0為修剪機(jī)器人前視距離選擇條件集合參數(shù),點(diǎn)O1、P0和P1組成三角形,與P1對(duì)應(yīng)的比值為S1,點(diǎn)O1、P0、P1和P2組成四角形,與P2對(duì)應(yīng)的比值為S2,點(diǎn)O1、P0、P1、P2和P3組成五角形,與P3對(duì)應(yīng)的比值為S3,以此類推可以得到多邊形與Pn對(duì)應(yīng)的比值為Sn。當(dāng)比值Sn大于S0時(shí),選擇Pn為前視距離點(diǎn),則前視距離點(diǎn)選擇策略可表示為

在前視距離選擇過程中,為避免橫向誤差過小導(dǎo)致Pn選取過大,將前視距離橫向偏差進(jìn)行調(diào)整[15]。當(dāng)橫向偏差e大于規(guī)定距離s時(shí),則采用Pn的選取策略;當(dāng)橫向偏差e小于規(guī)定距離s時(shí),則采用預(yù)瞄準(zhǔn)點(diǎn)的選取策略。綜上可以得出

3 仿真實(shí)驗(yàn)分析

在進(jìn)行修剪機(jī)器人路徑規(guī)劃算法仿真實(shí)驗(yàn)時(shí),設(shè)定實(shí)驗(yàn)區(qū)域底部長度為24m,左側(cè)邊長為9m,右側(cè)邊長為10m,上邊長為7m,如圖5所示。實(shí)驗(yàn)區(qū)域內(nèi)果樹間距為4m。

圖5 修剪機(jī)器人實(shí)驗(yàn)區(qū)域示意圖Fig.5 Schematic diagram of pruning robot experimental area

分別利用傳統(tǒng)往復(fù)式路徑規(guī)劃算法和跟蹤算法對(duì)修剪機(jī)器人的工作路徑進(jìn)行規(guī)劃。圖6(a)所示為修剪機(jī)器人傳統(tǒng)往復(fù)式路徑規(guī)劃軌跡,圖6(b)所示為修剪機(jī)器人跟蹤算法路徑規(guī)劃軌跡。

傳統(tǒng)往復(fù)式路徑規(guī)劃算法所得修剪機(jī)器人路徑軌跡包含19條直線,6次進(jìn)行果樹規(guī)避,規(guī)避過程中行走路線為曲線,在整個(gè)修剪區(qū)域內(nèi)掉頭24次。跟蹤算法所得路徑軌跡與傳統(tǒng)往復(fù)式路徑規(guī)劃算法相比具有較高的相似度,在行走過程中,規(guī)避動(dòng)作行走曲線更加圓滑,對(duì)目標(biāo)軌跡具有更高的擬合效果。經(jīng)分析統(tǒng)計(jì),跟蹤算法所得路徑軌跡作業(yè)過程中漏剪率降低3%,重復(fù)率降低2.5%,作業(yè)過程轉(zhuǎn)角范圍增大36°,橫向軌跡誤差降低600mm。

4 結(jié)論

跟蹤算法所得到的修剪機(jī)器人作業(yè)路徑軌跡,能夠有效降低修剪過程中出現(xiàn)的漏剪概率,同時(shí)降低修剪重復(fù)率,并在降低修剪機(jī)器人軌跡橫向偏差的同時(shí),提高行走過程轉(zhuǎn)角范圍,保持修剪過程中轉(zhuǎn)彎穩(wěn)定性。