跟蹤算法下的果樹修剪機器人路徑規劃設計

方小菊,黃亦其

(1.廣西職業技術學院,南寧 530226;2.廣西大學,南寧 530000)

0 引言

果樹修剪機器人作業過程中要求具有較高的作業覆蓋率和較低的作業重復率,同時要求作業過程中能根據設定的路徑進行跟蹤作業,達到較高的作業效率[1-2]。為此,以跟蹤算法為基礎,設計一種橫向偏差路徑跟蹤規劃算法,對修剪機器人的轉向過程進行控制,為果樹修剪機器人路徑跟蹤控制過程提供參考。

1 修剪機器人運動學模型

修剪機器人在果園中進行作業時,果園地面相對平坦,進行轉向時,假設機器人輪胎不發生滑移,由此可建立果樹修剪機器人四輪運動學模型,如圖1所示。

圖1 修剪機器人運動學模型示意圖Fig.1 Kinematic model of pruning robot

在笛卡爾坐標系中,假設修剪機器人的實時位置為H(x,y),L表示軸距,θ表示航向角,R表示瞬時轉彎半徑,δ表示轉向輪轉角,ω表示修剪機器人行走系統監測顯示轉角。修剪機器人的運動學方程可表示為

顯示轉角ω可表示為

2 修剪機器人路徑規劃算法

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

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

圖2 修剪機器人作業條件示意圖Fig.2 Schematic diagram of working conditions of pruning robot

將修剪機器人四輪運動學模型進行簡化,可得出修剪機器人的二自由度運動學模型,如圖3所示。以修剪機器人后軸為切點,改變前輪方向,使修剪機器人可以沿設定路徑圓弧運動。

圖3 修剪機器人二自由度運動學模型Fig.3 Two degree of freedom kinematic model of pruning robot

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

修剪機器人前輪轉角δ在時刻的跟蹤算法控制表達式可表示為

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

ld=kv(x)+Lfc

其中,k表示修剪機器人預瞄準速度增益量;Lfc表示修剪機器人靜態預瞄準距離;v(x)表示修剪機器人縱向行進速度。

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

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

假設修剪機器人作業路徑由無數點(xi,yi)組成,則

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

在前視距離選擇過程中,為避免橫向誤差過小導致Pn選取過大,將前視距離橫向偏差進行調整[15]。當橫向偏差e大于規定距離s時,則采用Pn的選取策略;當橫向偏差e小于規定距離s時,則采用預瞄準點的選取策略。綜上可以得出

3 仿真實驗分析

在進行修剪機器人路徑規劃算法仿真實驗時,設定實驗區域底部長度為24m,左側邊長為9m,右側邊長為10m,上邊長為7m,如圖5所示。實驗區域內果樹間距為4m。

圖5 修剪機器人實驗區域示意圖Fig.5 Schematic diagram of pruning robot experimental area

分別利用傳統往復式路徑規劃算法和跟蹤算法對修剪機器人的工作路徑進行規劃。圖6(a)所示為修剪機器人傳統往復式路徑規劃軌跡,圖6(b)所示為修剪機器人跟蹤算法路徑規劃軌跡。

傳統往復式路徑規劃算法所得修剪機器人路徑軌跡包含19條直線,6次進行果樹規避,規避過程中行走路線為曲線,在整個修剪區域內掉頭24次。跟蹤算法所得路徑軌跡與傳統往復式路徑規劃算法相比具有較高的相似度,在行走過程中,規避動作行走曲線更加圓滑,對目標軌跡具有更高的擬合效果。經分析統計,跟蹤算法所得路徑軌跡作業過程中漏剪率降低3%,重復率降低2.5%,作業過程轉角范圍增大36°,橫向軌跡誤差降低600mm。

4 結論

跟蹤算法所得到的修剪機器人作業路徑軌跡,能夠有效降低修剪過程中出現的漏剪概率,同時降低修剪重復率,并在降低修剪機器人軌跡橫向偏差的同時,提高行走過程轉角范圍,保持修剪過程中轉彎穩定性。