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

代玉梅，張瑞玲，馬黎

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

0 引言

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

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

為此，通過引入人工勢(shì)場(chǎng)法改進(jìn)了A*算法的效率，并設(shè)計(jì)了采摘機(jī)器人的路徑規(guī)劃方法，然后利用狀態(tài)觀測(cè)器估計(jì)出系統(tǒng)狀態(tài)，并通過設(shè)計(jì)的終端滑模控制律準(zhǔn)確跟蹤路徑指令，從而提高了機(jī)械臂的控制精度。

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

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

圖1 采摘機(jī)器人建模Fig.1 Modeling of picking robot

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

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

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

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

m——采摘機(jī)器人的慣性矩陣；

n——科氏矩陣；

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

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

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

則采摘機(jī)器人的運(yùn)動(dòng)模型

式中：A——變換矩陣。

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

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

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

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

2.1 A*算法描述

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

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

Step2：針對(duì)起點(diǎn)方格建立Open集和Close集，將起點(diǎn)方格緊挨的8個(gè)方格列入Open集，然后分別計(jì)算起點(diǎn)方格到Open集各點(diǎn)的代價(jià)值，代價(jià)值的表達(dá)式為

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

h(c)——Open集各點(diǎn)至終點(diǎn)的代價(jià)值。

Step3：比較Open集中各點(diǎn)的代價(jià)值，選取代價(jià)值最小的點(diǎn)作為新的子起點(diǎn)，并將該點(diǎn)的上一級(jí)起點(diǎn)列入Close集。

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

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

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

2.2 人工勢(shì)場(chǎng)法

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

采摘機(jī)器人的引力勢(shì)場(chǎng)

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

dy——采摘機(jī)器人末端與終點(diǎn)之間的距離。

采摘機(jī)器人的障礙斥力勢(shì)場(chǎng)

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

dc——采摘機(jī)器人末端與障礙物間的空間長(zhǎng)度；

d0——障礙對(duì)于采摘機(jī)器人的斥力半徑。

則采摘機(jī)器人的合力勢(shì)場(chǎng)

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

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

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

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

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

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

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

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

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

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

Step5：將該點(diǎn)作為子起點(diǎn)。

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

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

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

3 終端滑模控制律

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

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

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

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

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

——s2的估計(jì) 值；

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

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

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

——采摘機(jī)器人摩擦干擾d的估計(jì)值。

利用RBF神經(jīng)網(wǎng)絡(luò)[19]估計(jì)摩擦干擾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]；

Γ——正定矩陣。

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

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

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

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

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

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

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

將式（17）和式（11）代入式（16），化簡(jiǎn)可得

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

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

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

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

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

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

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

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

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

ξ1和ξ2——滑模切換項(xiàng)。

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

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

在積分終端滑模面的基礎(chǔ)上，設(shè)計(jì)了積分終端滑模控制律如式(25)。

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

3.3 穩(wěn)定性分析

將終端滑模控制律式（25）代入式（24）可以得到

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

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

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

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

進(jìn)一步得

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

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

4 仿真試驗(yàn)結(jié)果

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

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

4.1 仿真結(jié)果

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

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

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

為了驗(yàn)證本文終端滑模控制方法的有效性，以實(shí)線為路徑指令(xAd,yAd)進(jìn)行跟蹤控制仿真，并參考文獻(xiàn)[23]的滑模控制方法比較，得到路徑跟蹤曲線如圖5揚(yáng)示。

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

由仿真圖5可看出，文獻(xiàn)[23]的滑模控制方法能夠使移動(dòng)車大致跟蹤指令信號(hào)，但是跟蹤誤差較大，最大跟蹤誤差達(dá)到了1.6 m，跟蹤效果不好；而揚(yáng)設(shè)計(jì)的終端滑模控制方法能夠使移動(dòng)車精確跟蹤指令信號(hào)，其最大跟蹤誤差僅為0.2 m，跟蹤效果較好。

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

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

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

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

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

為了驗(yàn)證本文終端滑模控制方法的有效性，以實(shí)線為路徑指令(xBd，yBd，zBd)進(jìn)行跟蹤控制仿真，并與文獻(xiàn)[23]的方法比較，得到的路徑跟蹤結(jié)果如圖7揚(yáng)示。其中，圖7（a）為路徑跟蹤曲線，圖7（b）為跟蹤誤差。

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

由仿真圖7可看出，文獻(xiàn)[23]的滑模控制方法可使機(jī)械臂大致跟蹤指令信號(hào)，誤差較大，最大跟蹤誤差為0.54 m，跟蹤效果不好；而本文的終端滑模控制方法能夠使機(jī)械臂精確跟蹤指令信號(hào)，最大跟蹤誤差僅為0.04 m，跟蹤效果較好。

5 結(jié)論

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

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

2）設(shè)計(jì)的終端滑模控制方法比滑模控制方法具有更優(yōu)的控制效果，對(duì)移動(dòng)車最大跟蹤誤差為0.2 m，對(duì)機(jī)械臂的最大跟蹤誤差僅為0.04 m。

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