改進的APF算法在采摘機械手運動規劃中的應用

姚國林，王合闖

(1.河南農業職業學院機電工程學院，河南鄭州 451450；2.華北水利水電大學信息工程學院，河南鄭州 450045)

0 前言

采摘機器人一直是智能農業裝備領域的研究熱點，由于機械手的可操作性和精度較高，一般采用機械手作為采摘機器人的主要執行部件[1]。采用機械手作為采摘機器人的主要執行部件，首先需要解決機械手的運動規劃問題。運動規劃是指機械手從初始構型移動到目標構型的過程中，各個關節的運動軌跡規劃。在這個過程中，必須保證機械手不與工作空間內的任何障礙物或本身發生碰撞。

國內外研究學者對機器人的運動規劃問題開展了大量研究，現有的運動規劃算法包括快速隨機搜索樹(Rapidly Random Trees，RRT)[2]、概率路線圖法(Probabilistic Roadmap Method，PRM)[3]、A*算法[4]、神經網絡模型[5]、人工勢場(Artificial Potential Field，APF)[6]。由于采摘機器人工作的環境是非結構化的、非常復雜的、障礙物很多的環境，機械手的C空間構造將花費極長的時間。從現有文獻來看，RRT、PRM和A*算法需要構建C空間，因此無法滿足拾取機器人的實時性要求。而神經網絡算法不僅需要長時間的自我訓練，對硬件的要求也更高，因此也無法用于拾取機器人的實時運動規劃。

APF算法通過笛卡爾空間建立虛擬勢場來規劃運動，其中運動物體在從初始位置移動到目標位置的過程中會受到引力和斥力兩種力的影響，并避免碰撞障礙物。與RRT、PRM等全局規劃算法不同，APF是一種算法復雜度低、對計算機資源依賴性低、實時性強的局部運動規劃算法。但是，APF存在局部最小缺陷(Local Minimum Trap，LMT)和目標不可達問題(Target Unreachable Problem，TNRP)等問題[7]。MATOUI 等[8]對APF算法進行了改進，設計了一種非最小速度算法，改進后的APF能有效避免出現LMT現象。彭鵬等人[9]提出一種改進的APF和RRT結合的機械臂避碰路徑規劃算法，該算法有效避免了機械臂運動規劃陷入LMT問題。李卓等人[10]在傳統的智能車運動規劃方面，將速度和加速度等因素導入了引力勢場，通過仿真表明該算法能有效避免TNRP問題。由于采摘機器人對運動規劃算法的實時性要求較高，上述研究均耗時較長，無法滿足采摘機器人的實時性要求。

在上述研究的基礎上，本文作者分別對APF的勢場和力的計算模型進行了改進，在規劃機械手運動時直接計算機械手關節空間的吸引力矩，提出了改進的人工勢場 (IAPF)優化算法，有效地減少機械手運動規劃的計算時間，同時避免LMT和TPNR問題。最后通過實驗結果，驗證了所提優化算法在采摘機械手無碰撞運動規劃中的實時性和準確性。

1 采摘機器人的結構模型

圖1為本文作者所研究的智能采摘機器人的功能模型，主要由3個部件組成：負責果園內運動的自動移動車輛，并作為搭載所有其他部件的平臺；用于采摘水果的六連桿機械手；安裝在車輛上用于放置機器人控制柜的框架結構，包括水果貯藏箱、移動電源以及用于檢測和定位水果的立體攝像機視覺系統。

圖1 智能采摘機器人實物模型

2 采摘機械手的運動規劃模型

2.1 采摘機械手的運動學模型

機械手運動學模型的建立是運動規劃的前提工作，本文作者采用初等變換序列對六連桿機械手進行運動學建模[11]，如圖2所示，di(i=1，2，…，6)表示連桿偏移量，αi(i=1，2，…，6)表示連桿扭轉角，ai(i=1，2，…，6)表示連桿長度，θi(i=1，2，…，6)表示關節角度。從初等變換序列模型中，可得到機械手上每一點的雅可比矩陣。

圖2 六連桿機械手的初等變換序列模型

圖3所示為采摘水果的3個步驟：

圖3 采摘機械手運動規劃流程

步驟1，采摘機械手從初始位置移動到采摘位置，目標水果由末端執行器采摘分離。在規劃運動時，同時設計和計算采摘構型。

步驟2，采摘機械手從采摘構型移動到釋放構型，將水果放入存儲箱中。

步驟3，采摘機械手從釋放構型移動到初始構型，等待下一個采摘周期。在采摘實驗中，初始構型和釋放構型都是預先設定的，以進一步提高實時性。

2.2 機械手運動規劃算法

圖4所示為六連桿機械手的運動規劃概念圖。

圖4 六連桿機械手的運動規劃概念圖

APF算法是通過建立虛擬勢能場來規劃運動軌跡，以避免與工作空間中的任何障礙物發生碰撞[12]。引力勢場和引力的計算公式為

(1)

Fa=-?Ua=kada?da

(2)

式中：Ua∈R為引力勢場；Fa為引力；ka為引力系數；da為目標距離。斥力勢場和斥力計算公式為

(3)

(4)

式中：Ur∈R為斥力勢場；Fr為斥力；kr為斥力系數；dr為障礙物距離；d0為距離的閾值。合力勢場Us和合力Fs計算公式為

Us=Ua+Ur

(5)

Fs=Fa+Fr

(6)

與移動車輛不同，APF的應用和機械手的無碰撞運動規劃更加復雜。移動車輛可以看作是工作空間中的一個點進行運動規劃，但對于機械手來說，需要考慮如何對整個機械手進行無碰撞運動規劃，而不是將其作為單個點。機械手的路徑規劃可以在其工作空間或C空間中進行。由于C空間的計算量大，構建耗時長，所以通常采用工作空間中的路徑規劃。由于機械手上每個點的運動不能單獨規劃，因此必須在整體機械臂結構的背景下考慮機械手上的點。在此研究中，引力作用于所有初等變換序列坐標系的原點，斥力作用于每個初等變換序列線段上距離障礙物最近的點，如圖5所示。通過引力作用，機械手可以從初始構型變為目標構型。通過斥力作用，可以避免在工作空間中移動時撞到任何障礙物。

圖5 施加在機械手上的IAPF力示意

當工作空間中有多個障礙物時，合力勢場的合力可以計算為

(7)

(8)

為了控制機械手，通過雅可比矩陣的轉換，最終將合力映射到關節力矩上，計算關節力矩的公式為

(9)

2.3 運動規劃優化策略

APF中由于存在局部最小缺陷(LMT)、目標位置不可達(TNRP)等問題，將直接導致兩個問題，即Fa隨著da的減小而線性減小，當da=0時，Fa減小到零，但當dr趨于零時Fr趨于增加，當dr=0時，Fr增大到無窮大。首先，如果在目標位置附近有障礙物，其斥力會遠遠大于引力。當物體接近目標位置時，會不斷被斥力反彈，導致路徑振蕩甚至失效，這就是TNRP問題。其次，機械手關節所能承受的力矩有限，在運動規劃過程中無法實現無限的斥力。為了解決上述兩個問題，本文作者對勢場和力模型進行了優化。

為了避免TNRP問題，需要在接近目標位置時仍然有較大的梯度，以確保Fa足夠大，能夠克服Fr的影響，連續引導物體到達目標位置。同時，為了使路徑規劃算法收斂，當da=0時，Fa也應該趨近于0。為此，IAPF算法在APF算法原有的引力勢場計算模型的基礎上，通過加入Sigmoid函數分量以提升算法的收斂性[13]，即IAPF算法的引力勢場和引力計算為

(10)

(11)

式中：ka1和ka2為Sigmoid函數的控制因子，為避免Fr隨dr減小而不可控的問題，IAPF算法改變了原有的斥力勢場模型。APF算法的斥力勢場模型是分段函數，不僅需要在分段接點處連續可微，而且當dr=0時還會發生梯度爆炸。為確保IAPF算法的斥力不會無限增加，并且隨著dr的減小仍然是連續可微的，本文作者將IAPF算法的斥力變為一個反S的Sigmoid函數[14]，IAPF算法的斥力函數不僅在遠離障礙物時趨近于零，而且隨著dr的減小而增大，仍然是連續可微的。IAPF算法的Fr會逐漸增大，并趨于最大斥力，不會出現斥力爆炸的問題。IAPF算法的斥力勢場和斥力計算為

(12)

(13)

式中：kr1和kr2為反S型Sigmoid函數的控制因子；kr3為Ur的補償系數。為了消除LMT帶來的影響，IAPF算法選擇在合力勢場梯度消失時，增加臨時力Ft，打破斥力和引力的平衡。Ft不僅決定了IAPF算法結果的好壞，也決定了IAPF算法是否會再次陷入LMT。通過仿真實驗，Ft同時垂直于Fa和Fr時效果較好。在文中，當IAPF算法即將進入LMT時，將出現Ft，其值與Fa相等，其方向如圖6所示。

圖6 避免LMT的臨時力示意

通過雅可比矩陣的轉換，將工作空間中的力直接映射到關節空間中的力矩，仍然需要大量的計算，因此為了進一步優化，IAPF算法直接計算關節空間中的引力矩，計算公式為

(14)

(15)

3 采摘機械手運動規劃的仿真對比

文中的仿真都是在Windows 10操作系統下通過Python 3在Visual Studio Code中實現的。

3.1 勢場面與力曲線的仿真

APF和IAPF算法的勢場面和力曲線分別如圖7和圖8所示，表1給出了APF和IAPF算法的勢場模型和力模型的具體參數，其中Pg為目標位置，Pc為當前位置，P0為障礙物位置。

表1 第3.1節仿真使用的具體參數

圖7 APF和IAPF的勢場面

圖8 APF和IAPF的力曲線

可以看出：APF的引力勢場梯度在目標位置附近逐漸消失，而IAPF算法的梯度并沒有消失而是增加。因此，與APF相比，IAPF對TNRP的抵抗力更強，收斂速度更快。由于APF斥力勢場中存在梯度爆炸現象，當APF靠近障礙物時，斥力勢場的梯度遠大于引力勢場的坡度，導致合成勢場的引力勢場表面缺乏特征。然而，IAPF算法可以有效地克服以上問題，其合成勢場表面具有引力勢場面和斥力勢場面的共同特征，這使得IAPF算法不僅具有更快的收斂速度，還具有較強的無碰撞能力。

3.2 驗證TNRP的仿真對比

當目標位置附近有障礙物時，APF會出現TNRP問題。因此，為了驗證IAPF算法能夠有效避免TNRP問題，進行了下面的仿真實驗。仿真區域為10×10矩形，具體參數如表2所示，各障礙物的動作閾值相等。仿真結果如圖9所示，仿真曲線如圖10所示。

表2 第3.2節仿真使用的具體參數

圖9 TNRP的仿真結果

圖10 TNRP的仿真曲線

從圖9可以看出：當目標位置附近有障礙物時，機械手不斷靠近目標位置，但被障礙物的斥力推開，來回擺動，無法通過APF到達目標位置，因此APF無法收斂。從圖10可以看出：IAPF算法在第20次迭代附近接近目標位置，獲得了增強的引力，避免了TNRP的發生。仿真結果表明，IAPF算法可以有效避免TNRP的發生，保證路徑規劃的成功率。

3.3 驗證LMT的仿真對比

為了驗證IAPF算法能夠避免陷入LMT問題，設計了如下仿真實驗：當目標與初始位置之間的直線上存在障礙物時，容易陷入LMT，因此在這種情況下，APF和IAPF算法同時進行路徑規劃。模擬區域為一個10×10的矩形，具體參數見表3，各障礙物的動作閾值相等，仿真結果如圖11所示，仿真曲線如圖12所示。

表3 第3.3節仿真使用的具體參數

圖11 LMT的仿真結果

圖12 LMT的仿真曲線

從仿真結果可以看出：APF迅速陷入LMT，并一直保持這種狀態，直到路徑規劃失敗；IAPF算法沒有落入LMT，并迅速到達目標位置；IAPF算法在第22次迭代附近到達目標位置，而APF仍然停留在LMT中。仿真結果表明，IAPF算法能夠有效克服LMT問題，保證路徑規劃的成功率。

3.4 機械手運動規劃的仿真對比

采用APF和IAPF算法對機械臂進行路徑規劃。實驗參數如表4所示，其中初始構型為Θi，目標構型為Θg，障礙物坐標為POi，障礙物半徑均為120 mm。仿真結果如表5所示，機械手各初等變換序列線段與障礙物的關節角曲線和距離曲線如圖13所示。

表4 第3.4節仿真使用的具體參數

表5 IAPF和APF算法的路徑規劃結果

圖13 采摘機械手運動規劃的實驗結果

仿真結果表明：IAPF算法優于APF算法。IAPF算法的關節角曲線更平滑，變化率幾乎相同；APF算法的關節角曲線變化比較明顯，變化率相差較大，關節角θ3經過一個反向過程后開始收斂，而關節角θ2則出現波峰。APF算法的收斂速度非常慢，IAPF算法在第238次直接到達目標構型，而APF在第549次才收斂。與APF算法相比，IAPF算法在運行時間上減少了55.12%，在總關節角誤差上減少了46.21%。

4 運動規劃的實驗驗證

為進一步驗證IAPF算法的有效性和可行性，對智能采摘機器人進行了運動規劃實驗，實驗結果如表6所示。目標果樹及其簡化模型如圖14所示，首先用部分球面包絡(PSE)方法對目標果樹進行建模，該方法類似于一個包圍球算法[15-16]。PSE方法僅包含球面和線段模型，可以大大減少碰撞檢測過程中的計算量，進一步保證IAPF算法能夠滿足智能采摘機器人的實時性要求。PSE通過球體包絡果樹的所有非采摘部分，通過直線分段對當前采摘部分的分支進行建模，智能采摘機器人的采摘模擬和軌跡曲線如圖15所示，應用于智能采摘機器人的IAPF算法實驗結果如圖16所示。

表6 IAPF應用于智能采摘機器人的模擬實驗結果

圖14 目標果樹(a)及其PSE模型(b)

圖15 智能采摘機器人采摘仿真時的軌跡曲線

圖16 應用于智能采摘機器人的IAPF實驗結果

IAPF算法在智能采摘機器人采摘目標果實的路徑規劃過程中分別運動0.38、1.08、1.11 s，在采摘、釋放和初始化階段的關節總誤差分別為1.29°、1.08°和1.21°。智能采摘機器人規劃果實采摘路徑耗時約2.56 s，由于機械手運動所花費的時間比路徑規劃所花費的時間要長得多，因此IAPF算法可以在智能采摘機器人采摘過程中充分規劃下一個果實采摘路徑。從實驗結果可以得出結論：IAPF算法具有較快的求解速度和精度，能夠滿足智能采摘機器人的實時性要求。

5 結論

針對六連桿采摘機械手的無碰撞運動規劃，提出了改進的人工勢場(IAPF)優化算法，得出的主要結論如下：

(1)采用初等變換序列方法對機械手進行運動學建模，該模型可方便地得到機械手上各點相對于基底坐標系的雅可比矩陣。

(2)IAPF算法在APF算法原有的引力勢場計算模型的基礎上加入了Sigmoid函數，使得路徑規劃算法收斂；在斥力勢場中引入了反S的Sigmoid函數，防止出現斥力爆炸現象，有效地避免了局部最小缺陷問題和目標位置不可達問題。

(3)與傳統的APF算法相比，IAPF算法的計算時間和關節總誤差分別減少了55.12%和46.21%。最后通過實驗結果，驗證了所提優化算法在采摘機械手無碰撞運動規劃中的實時性和準確性。