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

代玉梅，張瑞玲，馬黎

(商丘職業技術學院軟件學院，河南商丘，476000)

0 引言

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

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

為此，通過引入人工勢場法改進了A*算法的效率，并設計了采摘機器人的路徑規劃方法，然后利用狀態觀測器估計出系統狀態，并通過設計的終端滑模控制律準確跟蹤路徑指令，從而提高了機械臂的控制精度。

1 建立采摘機器人數學模型

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

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

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

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

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

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

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

n——科氏矩陣；

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

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

Q——位置坐標，Q=［xA,yA,xB,yB,zB］T，m。

則采摘機器人的運動模型

式中：A——變換矩陣。

令s1=Q，?，則采摘機器人的數學模型可以化簡為

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

2 改進A*算法的路徑規劃算法

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

2.1 A*算法描述

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

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

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

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

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

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

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

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

當采摘空間劃分的足夠小時，該算法可以求得采摘機器人路徑的最優解，但是求解過程的計算量太大，導致路徑規劃的效率偏低。

2.2 人工勢場法

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

采摘機器人的引力勢場

式中：ky——引力勢場的比例系數；

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

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

式中：kc——斥力勢場的比例系數；

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

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

則采摘機器人的合力勢場

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

當目標點在障礙物的斥力空間內時，傳統人工勢場法容易陷入局部最優，如圖2揚示。

圖2 目標點和障礙物接近Fig.2 Target point is close to obstacle

當采摘機器人在接近目標點的過程中，合力勢場會在障礙物和目標點之間的某一點（非目標點）減小到0，最終得到局部最優解，導致路徑規劃結果錯誤。

2.3 改進A*路徑規劃算法步驟

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

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

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

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

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

Step5：將該點作為子起點。

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

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

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

3 終端滑模控制律

本文設計了終端滑模控制律來確保采摘機器人快速、準確跟蹤改進A*路徑規劃算法輸出的采摘路徑，系統結構如圖3揚示。

圖3 系統結構Fig.3 System structure

3.1 狀態觀測器設計

針對采摘機器人的數學模型式（3），設計狀態觀測器[18]。

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

——s2的估計 值；

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

ρ1、ρ2——正常數；

sign（）——符號函數；0＜α＜1；

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

利用RBF神經網絡[19]估計摩擦干擾d，則神經網絡自適應律描述

式中：?——RBF神經網絡的權重系數；

?(s1,s2)——RBF神經網絡的高斯基函數[20]；

Γ——正定矩陣。

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

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

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

構建Lyapunov函數

對式（15）求導可以得到

根據Young不等式[21]，可以得到

式中：k1、k2——正常數。

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

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

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

3.2 終端滑模控制律設計

采摘機器人的位置坐標跟蹤誤差

式中：s1d——改進A*路徑規劃算法輸出的采摘路徑規劃結果，即采摘機器人揚期望的位置坐標。

采摘機器人的轉動角速度跟蹤誤差

式中：s2d——期望的轉動角速度，。

設計如下積分終端滑模面[22]

式中：β1和β2——正常數；

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

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

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

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

式中：φ1、γ1和κ——正常數。

3.3 穩定性分析

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

構建Lyapunov函數

式中：γ2、和μ——正常數。

對式（27）求導可以得到

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

進一步得

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

由Lyapunov穩定性定理得：變量E和終端滑模面h可以穩定收斂到0，即采摘機器人的位置坐標跟蹤誤差e1和轉動角速度跟蹤誤差e2可以收斂到0，設計的積分終端滑模控制律式（25）能夠確保采摘機器人穩定跟蹤改進A*路徑規劃算法輸出的采摘路徑。

4 仿真試驗結果

為了驗證本文設計的方法能夠實現對采摘機器人的路徑規劃和精確控制，分別采用傳統A*算法進行路徑規劃、采用文獻[23]的滑模控制方法進行跟蹤控制，并與本文方法進行了Matlab仿真對比。設置控制系統參數如表1揚示。

表1 控制系統參數Tab.1 Control system parameters

4.1 仿真結果

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

圖4 移動車路徑規劃結果Fig.4 Path planning results of mobile vehicle

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

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

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

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

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

4.2 機械臂仿真結果

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

圖6 機械臂路徑規劃結果Fig.6 Path planning results of manipulator

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

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

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

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

5 結論

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

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

2）設計的終端滑模控制方法比滑模控制方法具有更優的控制效果，對移動車最大跟蹤誤差為0.2 m，對機械臂的最大跟蹤誤差僅為0.04 m。

3）通過改進A*路徑規劃算法和終端滑模控制方法有機結合，實現了對采摘機器人精確路徑規劃和跟蹤控制，有效提高了農業采摘的智能化和自主化水平。