改進A*算法的采摘機器人路徑規(guī)劃與跟蹤控制**

代玉梅，張瑞玲，馬黎

(商丘職業(yè)技術(shù)學(xué)院軟件學(xué)院，河南商丘，476000)

0 引言

采摘是農(nóng)業(yè)生產(chǎn)的重要環(huán)節(jié)，在收獲季節(jié)需要大量的人力在較短時間內(nèi)完成采摘工作[1-2]。尤其是在果園面積較大時，需要投入更多的人工。隨著智能化機械設(shè)備的不斷發(fā)展，農(nóng)業(yè)采摘機器人應(yīng)運而生，針對復(fù)雜的地形和作業(yè)環(huán)境，需要采摘機器人具備移動、定位和采摘的功能來完成自動化的采摘作業(yè)，但是當(dāng)前采摘機器人的智能化和自主化水平還比較低[3-7]。為了提高采摘機器人的工作效率和控制效果，從路徑規(guī)劃與跟蹤控制兩方面展開研究。

國內(nèi)外相關(guān)學(xué)者在此領(lǐng)域也有研究，鄭嫦娥等[8]為了解決深度強化學(xué)習(xí)對采摘路徑規(guī)劃效率低的問題，提出了基于分步遷移策略的深度確定性策略梯度算法，并進行了蘋果采摘軌跡規(guī)劃，該方法能夠在一定程度上提高路徑規(guī)劃效率，但是不涉及路徑跟蹤控制，實際意義不大；馬冀桐等[9]為了解決采摘機器人的避障問題，利用深度學(xué)習(xí)Mask R-CNN神經(jīng)網(wǎng)絡(luò)對障礙物信息進行訓(xùn)練和識別，提出了一種快速擴展隨機樹的改進算法，該方法能夠?qū)崿F(xiàn)采摘機器人的避障運動規(guī)劃，但是算法復(fù)雜程度較高；馬冀桐等[10]針對采摘機器人的避障問題，提出了一種基于構(gòu)型空間的改進型雙向快速擴展隨機樹算法，該算法能夠?qū)崿F(xiàn)機械臂的避障路徑規(guī)劃，但是算法條件較嚴格，并且不涉及路徑跟蹤控制；張玲等[11]利用遺傳算法設(shè)計了輪式移動機器人和機械臂的軌跡控制方法，雖然實現(xiàn)了最優(yōu)的軌跡控制，但是沒有考慮避障問題。

為此，通過引入人工勢場法改進了A*算法的效率，并設(shè)計了采摘機器人的路徑規(guī)劃方法，然后利用狀態(tài)觀測器估計出系統(tǒng)狀態(tài)，并通過設(shè)計的終端滑?？刂坡蓽蚀_跟蹤路徑指令，從而提高了機械臂的控制精度。

1 建立采摘機器人數(shù)學(xué)模型

采摘機器人主要包括移動車和機械臂兩個部分。為了能更好分析采摘機器人的運動特性，對采摘機器人進行建模，如圖1揚示。假設(shè)移動車的質(zhì)心坐標(biāo)為A（xA,yA），θl表示左輪轉(zhuǎn)動角度，θr表示右輪轉(zhuǎn)動角度，移動車可以在果園里自由移動來確定采摘的位置。機械臂安裝在移動車質(zhì)心處，機械臂末端相對移動車質(zhì)心的坐標(biāo)為B（xB,yB,zB）；θ1表示關(guān)節(jié)1在水平面內(nèi)轉(zhuǎn)動的角度；θ2表示關(guān)節(jié)2在地垂面內(nèi)轉(zhuǎn)動的角度，θ3表示關(guān)節(jié)3相對果實的轉(zhuǎn)動角度。其中，θ1和θ2用來調(diào)整機械臂末端方位，θ3用來調(diào)整采摘角度。

圖1 采摘機器人建模Fig.1 Modeling of picking robot

采摘機器人的動力學(xué)模型[12-13]

式中：θ——采摘移動車左輪、右輪和3個機械關(guān)節(jié)的轉(zhuǎn)動角度，θ=[θl,θr,θ1,θ2,θ3]T,(°)；

——采摘移動車左輪、右輪和3個機械關(guān)節(jié)的轉(zhuǎn)動角速度，,(°)/s;

——采摘移動車左輪、右輪和3個機械關(guān)節(jié)的轉(zhuǎn) 動 角 加 速 度，,(°)/s2；

m——采摘機器人的慣性矩陣；

n——科氏矩陣；

g——重力加速度，m/s2；

u——采摘機器人左輪、右輪和3個機械關(guān)節(jié)電機的輸出力矩，u=［ul,ur,u1,u2,u3］T，N·m；

Q——位置坐標(biāo)，Q=［xA,yA,xB,yB,zB］T，m。

則采摘機器人的運動模型

式中：A——變換矩陣。

令s1=Q，?，則采摘機器人的數(shù)學(xué)模型可以化簡為

式中：d——采摘機器人的摩擦干擾。

2 改進A*算法的路徑規(guī)劃算法

A*算法是一種全局最優(yōu)的軌跡搜索算法，可以用來對采摘機器人的路徑進行規(guī)劃，由于A*算法具有局部最優(yōu)的缺點，為此本文利用人工勢場法來提高A*算法的路徑規(guī)劃效率，實現(xiàn)對采摘機器人快速和準確的路徑規(guī)劃。

2.1 A*算法描述

A*算法步驟[14-15]如下。

Step1：確定采摘起點和終點的空間坐標(biāo)，將采摘空間細分為一系列小方格，作為軌跡搜索的最小單元。

Step2：針對起點方格建立Open集和Close集，將起點方格緊挨的8個方格列入Open集，然后分別計算起點方格到Open集各點的代價值，代價值的表達式為

式中：g(c)——自起點至Open集各點的代價值；

h(c)——Open集各點至終點的代價值。

Step3：比較Open集中各點的代價值，選取代價值最小的點作為新的子起點，并將該點的上一級起點列入Close集。

Step4：清空Open集，將子起點相鄰的7個節(jié)點（上級起點除外）列入Open集，根據(jù)式（4）、式（5）和式（6）計算該子起點到Open集各點的代價值。

Step5：判斷路徑終點是否在Open集中。若在，則輸出起點與子起點的軌跡，即為路徑規(guī)劃結(jié)果，算法結(jié)束；否則，轉(zhuǎn)Step3。

當(dāng)采摘空間劃分的足夠小時，該算法可以求得采摘機器人路徑的最優(yōu)解，但是求解過程的計算量太大，導(dǎo)致路徑規(guī)劃的效率偏低。

2.2 人工勢場法

為了提高A*算法的路徑規(guī)劃效率，引入人工勢場法[16-17]。人工勢場法的基本思想是將采摘機器人的運動變換成在引力場中的運動，終點會對采摘機器人產(chǎn)生引力勢場，而障礙物則會對采摘機器人產(chǎn)生斥力勢場，最后根據(jù)合力得到采摘機器人的運動路徑。

采摘機器人的引力勢場

式中：ky——引力勢場的比例系數(shù)；

dy——采摘機器人末端與終點之間的距離。

采摘機器人的障礙斥力勢場

式中：kc——斥力勢場的比例系數(shù)；

dc——采摘機器人末端與障礙物間的空間長度；

d0——障礙對于采摘機器人的斥力半徑。

則采摘機器人的合力勢場

求解合力勢場的負梯度，可以得到合力勢場減小最快的路徑，此路徑即為采摘機器人的路徑規(guī)劃結(jié)果。

當(dāng)目標(biāo)點在障礙物的斥力空間內(nèi)時，傳統(tǒng)人工勢場法容易陷入局部最優(yōu)，如圖2揚示。

圖2 目標(biāo)點和障礙物接近Fig.2 Target point is close to obstacle

當(dāng)采摘機器人在接近目標(biāo)點的過程中，合力勢場會在障礙物和目標(biāo)點之間的某一點（非目標(biāo)點）減小到0，最終得到局部最優(yōu)解，導(dǎo)致路徑規(guī)劃結(jié)果錯誤。

2.3 改進A*路徑規(guī)劃算法步驟

為了提高A*路徑規(guī)劃算法的效率，并克服人工勢場法的局部最優(yōu)缺點，提出了改進A*路徑規(guī)劃算法來優(yōu)化采摘路徑，算法步驟如下。

Step1：建立采摘空間模型，確定障礙物、起點和終點位置。

Step2：根據(jù)式（7）、式（8）和式（9）計算采摘空間的合力勢場。

Step3：根據(jù)采摘空間的合力勢場，得到合力勢場減小最快的采摘路徑。

Step4：跟隨合力勢場的減小路徑，找到合力勢場為0的點，并判斷此點是否為終點，若是，則輸出路徑規(guī)劃結(jié)果。若不是，則轉(zhuǎn)下一步。

Step5：將該點作為子起點。

Step6：將子起點相鄰的8個節(jié)點列入Open集，判斷路徑終點是否在Open集中。若在，則輸出子起點與終點的軌跡，即為路徑規(guī)劃結(jié)果，算法結(jié)束。否則，執(zhí)行下一步。

Step7：根據(jù)式（4）、式（5）和式（6）計算該子起點到Open集各點的代價值。

Step8：比較Open集中各點的代價值，選取代價值最小的點作為新的子起點，轉(zhuǎn)Step6。

3 終端滑?？刂坡?/h2>

本文設(shè)計了終端滑模控制律來確保采摘機器人快速、準確跟蹤改進A*路徑規(guī)劃算法輸出的采摘路徑，系統(tǒng)結(jié)構(gòu)如圖3揚示。

圖3 系統(tǒng)結(jié)構(gòu)Fig.3 System structure

3.1 狀態(tài)觀測器設(shè)計

針對采摘機器人的數(shù)學(xué)模型式（3），設(shè)計狀態(tài)觀測器[18]。

——采 摘 機 器 人 位 置 坐 標(biāo) 的 估 計 誤 差，?=-s1；

——s2的估計 值；

——采摘機器人轉(zhuǎn)動角速度的估計誤差=-s2；

ρ1、ρ2——正常數(shù)；

sign（）——符號函數(shù)；0＜α＜1；

——采摘機器人摩擦干擾d的估計值。

利用RBF神經(jīng)網(wǎng)絡(luò)[19]估計摩擦干擾d，則神經(jīng)網(wǎng)絡(luò)自適應(yīng)律描述

式中：?——RBF神經(jīng)網(wǎng)絡(luò)的權(quán)重系數(shù)；

?(s1,s2)——RBF神經(jīng)網(wǎng)絡(luò)的高斯基函數(shù)[20]；

?！ň仃?。

進一步可得到狀態(tài)觀測器式（10）的估計誤差

為證明對采摘機器人設(shè)計的狀態(tài)觀測器式（10）是收斂的，令，則可得到

由于矩陣C1和矩陣C2是Hurwitz矩陣，則存在正定矩陣P=PT、B1=B1T、B2=B2T，且滿足

構(gòu)建Lyapunov函數(shù)

對式（15）求導(dǎo)可以得到

根據(jù)Young不等式[21]，可以得到

式中：k1、k2——正常數(shù)。

將式（17）和式（11）代入式（16），化簡可得

式中：λmin(Π)——矩陣Π的最小元素。

選取較大k1和k2可確保λmin(Π)＞0，得到，即可確保狀態(tài)觀測器式（10）收斂。

3.2 終端滑模控制律設(shè)計

采摘機器人的位置坐標(biāo)跟蹤誤差

式中：s1d——改進A*路徑規(guī)劃算法輸出的采摘路徑規(guī)劃結(jié)果，即采摘機器人揚期望的位置坐標(biāo)。

采摘機器人的轉(zhuǎn)動角速度跟蹤誤差

式中：s2d——期望的轉(zhuǎn)動角速度，。

設(shè)計如下積分終端滑模面[22]

式中：β1和β2——正常數(shù)；

ξ1和ξ2——滑模切換項。

其中，l1和l2為正常數(shù)；v1≥1；v2≥1；ξ1=ξ2/(2-ξ2)，0＜ξ2＜1。

對積分終端滑模面求導(dǎo)，并將式（10）代入得

在積分終端滑模面的基礎(chǔ)上，設(shè)計了積分終端滑?？刂坡扇缡?25)。

式中：φ1、γ1和κ——正常數(shù)。

3.3 穩(wěn)定性分析

將終端滑?？刂坡墒剑?5）代入式（24）可以得到

構(gòu)建Lyapunov函數(shù)

式中：γ2、和μ——正常數(shù)。

對式（27）求導(dǎo)可以得到

式中：矩陣F——正定矩陣。

進一步得

將式（30）代入式（28）可以得到

由Lyapunov穩(wěn)定性定理得：變量E和終端滑模面h可以穩(wěn)定收斂到0，即采摘機器人的位置坐標(biāo)跟蹤誤差e1和轉(zhuǎn)動角速度跟蹤誤差e2可以收斂到0，設(shè)計的積分終端滑?？刂坡墒剑?5）能夠確保采摘機器人穩(wěn)定跟蹤改進A*路徑規(guī)劃算法輸出的采摘路徑。

4 仿真試驗結(jié)果

為了驗證本文設(shè)計的方法能夠?qū)崿F(xiàn)對采摘機器人的路徑規(guī)劃和精確控制，分別采用傳統(tǒng)A*算法進行路徑規(guī)劃、采用文獻[23]的滑?？刂品椒ㄟM行跟蹤控制，并與本文方法進行了Matlab仿真對比。設(shè)置控制系統(tǒng)參數(shù)如表1揚示。

表1 控制系統(tǒng)參數(shù)Tab.1 Control system parameters

4.1 仿真結(jié)果

設(shè)置移動車的起點坐標(biāo)為(0，0)，終點坐標(biāo)為(30，30)，2個障礙物的圓心分別為(10，10)和(15，20)，斥力半徑分別為2 m和5 m，得到路徑規(guī)劃仿真結(jié)果如圖4揚示。

圖4 移動車路徑規(guī)劃結(jié)果Fig.4 Path planning results of mobile vehicle

在整個仿真過程中，傳統(tǒng)A*算法的運行時間為13 s，路徑規(guī)劃結(jié)果如虛線揚示，路徑長度為58.54 m；而本文改進A*算法的運行時間為6 s，路徑規(guī)劃結(jié)果如實線揚示，路徑長度為47.82 m，通過二者對比突出本文算法具有更高的運行效率和具有更短的路徑長度，并且路徑更加圓滑，更加符合工程實際。

為了驗證本文終端滑?？刂品椒ǖ挠行?，以實線為路徑指令(xAd,yAd)進行跟蹤控制仿真，并參考文獻[23]的滑?？刂品椒ū容^，得到路徑跟蹤曲線如圖5揚示。

圖5（a）為路徑跟蹤曲線，圖5（b）為跟蹤誤差。

由仿真圖5可看出，文獻[23]的滑模控制方法能夠使移動車大致跟蹤指令信號，但是跟蹤誤差較大，最大跟蹤誤差達到了1.6 m，跟蹤效果不好；而揚設(shè)計的終端滑?？刂品椒軌蚴挂苿榆嚲_跟蹤指令信號，其最大跟蹤誤差僅為0.2 m，跟蹤效果較好。

圖5 移動車路徑跟蹤結(jié)果Fig.5 Path tracking results of mobile vehicle

4.2 機械臂仿真結(jié)果

設(shè)置采摘機械臂的起點為(0，0，0)，終點為(5，5，5)，2個障礙物的圓心分別為(2，2，2)和(4，4，4)，斥力半徑分別為0.3 m和0.5 m，路徑規(guī)劃結(jié)果如圖6揚示。

圖6 機械臂路徑規(guī)劃結(jié)果Fig.6 Path planning results of manipulator

在整個仿真過程中，傳統(tǒng)A*算法運行時間為5 s，路徑規(guī)劃結(jié)果如虛線揚示，路徑長度為13.72 m；而本文改進A*算法的運行時間為2 s，路徑規(guī)劃結(jié)果實線揚示，路徑長度為11.25 m，通過二者的對比突出了本文算法具有更高的運行效率，具有更短的路徑長度，并且路徑更加圓滑，更加符合工程實際。

為了驗證本文終端滑?？刂品椒ǖ挠行?，以實線為路徑指令(xBd，yBd，zBd)進行跟蹤控制仿真，并與文獻[23]的方法比較，得到的路徑跟蹤結(jié)果如圖7揚示。其中，圖7（a）為路徑跟蹤曲線，圖7（b）為跟蹤誤差。

圖7 機械臂路徑跟蹤結(jié)果Fig.7 Path tracking results of manipulator

由仿真圖7可看出，文獻[23]的滑?？刂品椒墒箼C械臂大致跟蹤指令信號，誤差較大，最大跟蹤誤差為0.54 m，跟蹤效果不好；而本文的終端滑?？刂品椒軌蚴箼C械臂精確跟蹤指令信號，最大跟蹤誤差僅為0.04 m，跟蹤效果較好。

5 結(jié)論

為了促進農(nóng)業(yè)采摘的智能化和自主化的發(fā)展，針對采摘機器人提出了一種基于改進A*算法的路徑規(guī)劃與跟蹤控制方法，經(jīng)過Matlab對比仿真，得到以下結(jié)論。

1）采用改進A*路徑規(guī)劃算法：移動車的運行時間為6 s，路徑長度為47.82 m；機械臂運行時間為2 s，路徑長度為11.25 m，比傳統(tǒng)A*路徑規(guī)劃算法具有更高的運行效率和更短的路徑長度，且路徑更加圓滑，更加符合工程實際。

2）設(shè)計的終端滑?？刂品椒ū然？刂品椒ň哂懈鼉?yōu)的控制效果，對移動車最大跟蹤誤差為0.2 m，對機械臂的最大跟蹤誤差僅為0.04 m。

3）通過改進A*路徑規(guī)劃算法和終端滑?？刂品椒ㄓ袡C結(jié)合，實現(xiàn)了對采摘機器人精確路徑規(guī)劃和跟蹤控制，有效提高了農(nóng)業(yè)采摘的智能化和自主化水平。