復雜環境下果園機器人路徑規劃方法研究*

楊保海，任全會，李海生

(1. 廣西民族師范學院數理與電子信息工程學院，廣西崇左，532200； 2. 河南省智慧教育與智能技術應用工程技術研究中心，鄭州市，451460)

0 引言

目前，水果生產在農業經濟中占有非常重要的地位，和其它農業產業相比果園生產機械化程度比較低，果園的生產管理還主要靠人工完成[1]。主要是因為果樹不但樹冠較大，而且冠層還低，因此普通的農機很難進行具體的操作。人工操作的成本高，水果的價格不穩定等問題都會影響果農的積極性。隨著人工智能、物聯網以及自動化技術的快速發展，果園機器人也得到了廣泛的應用，這樣就可以大大提高生產效率，同時還降低了種植果樹的成本[2]。

果園機器人熱點和重要的問題就是路徑規劃，也就是在果園這個復雜的環境下能夠很好地尋找從起點到目標點的無碰撞移動路徑[3]。國內外的學者針對路徑規劃的問題進行了大量的研究，目前研究路徑規劃的算法有很多，比如有學者提出的人工勢場算法，這種算法具有數學模型簡單、計算量小、易于實現的優點，不過該算法應用到環境復雜的果園中就會造成鎖死的現象，而且迭代次數大大增加；還有學者提出了蟻群算法，此種算法的平均角度偏差較大，而且需要更多的迭代次數；其它算法的還有粒子群算法以及神經網絡算法等等，此類算法如果應用到果園機器人上會出現參數不確定以及計算量大等諸多問題[4]。

為實現復雜環境下果園機器人路徑規劃，本文提出了一種新的方法，也就是在基本人工勢場算法的基礎上進行優化[5]。引入使用了障礙物檢測算法，此算法剔除了果園機器人的斥力作用，引入一個障礙物有關的邊界條件參數，這樣不但避免了死鎖現象，還消除斥力的影響從而得到更優的移動路徑。

1 基本人工勢場算法

人工勢場算法由美國斯坦福大學教授Khatib提出的[6]。假設果園機器人的初始位置是S=[xy]，終點位置是SE=[xeye]。引力勢場函數可以表示成

(1)

式中:br——力場的增益系數，并且其值是正數。

假設引力勢場函數的負梯度[7]可表示成

(2)

式中:φ1——果園機器人起始點之間的距離，φ1=‖S-SE‖；

β1——單位向量。

通過式(1)和式(2)可以看出，引力和初始點的距離的關系是線性的[8]，因此就可以把障礙物產生的斥力場函數表示出來。

(3)

式中:φ——果園機器人到障礙物最短距離；

φ0——障礙物影響的長度；

b0——增益系數，并且其值是正數。

通過式(3)可以看出，參數φ0受障礙物斥力影響比較大，但是，如果不在這個方位內，則其斥力不再影響此參數。此時的斥力場函數的負梯度就可以表示成

(4)

果園機器人受的合力就可以表示成

F(S)=FH(S)+F0(S)

(5)

通過式(1)～式(5)可以看出，果園機器人路徑規劃和避障的時候缺點非常的明顯，主要表現在三個方面：其一，如果出現障礙物的斥力相加之后和引力的值一樣，并且相位相反，此時會出現死鎖現象；其二，因為斥力的存在，會出現無法到達終點的情況；其三，在復雜環境下，計算效率會大大下降，同時需要更多的迭代次數[9]。

2 算法改進

基于對人工勢場算法原理和其存在的問題的分析，主要從兩個方面進行改進，首先使用障礙物檢測算法識別路徑，引入邊界條件這個參數，降低障礙物的受力影響[10]。

2.1 重構識別路徑

路徑重構主要使用障礙物檢測算法實現，此算法的核心就是計算出一個科學的路徑中間點[11]。

假設果園機器人的初始位置是SQ=[xqyq]，終點位置是SD=[xdyd]，因此起始點的直線∑z的方程可以表示成

(6)

式中:round——取整函數。

仿真環境橫向方格數為Mh，縱向的方格數為Mz，果園機器人的活動范圍就可以設定成{(x，y)|0≤x≤Mh同時0≤y≤Mz}，方格到∑z的方程距離可以表示成

(7)

式中:m、n——仿真方格中的縱、橫坐標；

B——常數，并且其取值為大的正整數。

通過對式(7)分析可以看出，l的值存在正負值兩種可能。假設ψ為障礙物方格的邊沿，因此就有一個點到∑z的距離是正最大值，這個點的坐標設定成Smax,δ=[xmax,δymax,δ]，同時還有一個點到∑z的距離是負最小值，這個點的坐標設定成Smin,δ=[xmin,δymin,δ]。因此，就可以把ψ周邊的路徑中間點坐標表示出來。

(8)

(9)

2.2 優化斥力作用

仿真環境中，果園機器人可能移動的方向有8種，所以，果園機器人在移動的時候需要承受各方向上的作用力。為了便于分析，可以通過3×3的矩陣來存儲，其名稱也可以稱為力向矩陣[12]。

通過對基本人工勢場算法的分析可知，在果園機器人移動的過程中會受到障礙物的斥力作用，因此，果園機器人在移動的時候不能實現貼近障礙物移動，這就會造成果園機器人移動路徑變大。消除斥力的影響就可以得到更優的移動路徑，為此，本文使用了障礙物邊界條件的概念，也可以理解為障礙物對果園機器人作用力一直是0。這就可以實現果園機器人在移動的時候只受兩個因素的影響，一個是終點產生的引力，另外一個就是外部環境。假設果園機器人在S位置的受力方向矩陣

(10)

式中:In m——環境約束矩陣；

*——卷積運算。

In m代表的是仿真環境中果園機器人周邊方格的情況，可以把Inm表示成

(11)

(12)

如果要實現最優分解，就必須要符合

(13)

式中:j、i——行、列代號，其取值只能是{-1，0，1}；

βn m——方向向量，其取值范圍是(j，i)；

τn m——分解系數[13]。

果園機器人哪個方向受力最大，其會向哪個方向移動，因此就可以把路徑點的迭代表示成

S′=S+ηn m

(14)

式中:ηn m——最大引力分量方向向量；

S′——下一路徑點位置向量。

2.3 改進算法流程

改進算法的流程主要分為四步進行：第一步，對各個參數進行初始化包括仿真地圖信息矩陣Da和搜索矩陣Db；第二步，通過障礙物檢測算法找出兩個參數，這兩個參數是中間點與有效障礙物，與此同時將中間點作為局部路徑的終點，重復這個過程，確定沒有其它的有效障礙物和中間點，這樣就得到了第一個有效的中間點，同時把最后一個有效障礙物方格的特點數據進行移動，也就是把此數據從Da復制到Db；第三步，通過優化斥力作用的原理檢索Da的力向矩陣，接著檢索出受力最大的方向，同時按此方向從起點重新檢索，這樣就可以得到一個局部最優路徑；第四步，把終點設置為新的起點，把最終的目標點設為新的終點，按第二步和第三步重新進行檢索，如果出現了最終的目標點變成了起點，就停止搜索，這樣路徑規劃就結束了，最終的全局路徑就是由各個局部最優路徑組成[14]。

此算法對路徑進行的重構，因為各個局部路徑之間沒有新的障礙物和中間點，所以就避免了基本人工勢場算法中的死鎖現象；因為優化了斥力作用，這樣果園機器人就能夠緊貼障礙物進行移動，所以就能得到更短的局部路徑，進而實現最優的全局路徑。

3 測試結果

為了驗證本文算法的正確性，本文在復雜環境下進行了對比測試，仿真軟件使用Matlab[15]。

果園機器人的移動環境使用柵格法進行構建，設定黑色柵格為障礙物，左下角為果園機器的起點，右上角為果園機器的目標點，起點坐標是(0，0)，終點的坐標是(l，h)，每一個小方格的尺寸是a×a，兩個方向總的方格數可表示成

(15)

式中:Nh——縱向小方格的數量；

Nl——橫向小方格的數量；

ceil——向上取整函數。

構建的環境如圖1所示，測試結果如圖2所示。從圖2可以看出，初始狀態下，兩種方法路徑規劃相差不大，隨著障礙物的增多，基本人工勢場算法陷入局部最優解。本文算法用障礙物檢測算法識別路徑計算出一個科學的路徑中間點，同時優化了斥力作用，避免了局部最優解，充分使用收斂次數構建出反饋通道，這樣就可以大大減少了迭代過程中的波動性。和基本人工勢場算法相比，本文算法收斂過程更加平穩。

圖1 仿真環境Fig. 1 simulation environment

復雜環境下路徑長度迭代對比如圖3所示。兩種算法測試結果對比如表1所示。通過圖3、表1可以看出，基本人工勢場算法得到的規劃路徑長度為29.873 m，本文算法得到的規劃路徑長度為29.287 m，最優路徑長度有了一定的減少；基本人工勢場算法需要27次迭代才能收斂到最優路徑，但本文算法收斂到最優路徑僅需要8次迭代就能實現，迭代次數減少了19次，大大減少了迭代次數；基本人工勢場算法運行時間12.127 s，本文算法運行時間3.465 s，運行時間減少了近9 s。基本人工勢場算法波動較大，對算法的收斂速度產生不利的影響，而本文算法在迭代過程中的波動較小，收斂過程非常平穩，因此收斂速度較快。

(a) 基本人工勢場算法

(b) 本文算法

圖3 復雜環境下路徑長度迭代對比圖Fig. 3 Comparison of path length iterations in hybrid environment

表1 兩種算法測試結果對比Tab. 1 Comparison of test results of two algorithms

為使路徑搜索能力更清楚地量化，采用所尋路徑對環境的覆蓋率這個參數量化，其表達式為

(16)

式中:Mp——算法檢索到的方格數量；

Mf——自由方格的數量。

通過式(15)可以把兩種算法的所尋路徑對環境的覆蓋率計算出來，基本人工勢場算法的計算結果為56.89%，本文算法的計算結果是73.59%，本文算法所尋路徑對環境的覆蓋率提高了16.7個百分比。所尋路徑對環境的覆蓋率越大說明路徑規劃時搜索的范圍越廣，通過對比計算結果可知，本文算法得到最優路徑的可能性最大，也就是說規劃最優路徑的能力更突出。

4 結論

1) 復雜環境下果園機器人使用的傳統的基本人工勢場算法進行路徑規劃存在死鎖問題，本文算法使用障礙物檢測算法識別路徑計算出一個科學的路徑中間點，同時優化了斥力作用，避免了局部最優解，可以得到更優的路徑規劃，所尋路徑對環境的覆蓋率提高了16.7個百分點，覆蓋率越高，機器人全局尋優能力越強。

2) 本文算法充分使用收斂次數構建出反饋通道，這樣就可以大大減少了迭代過程中的波動性，和基本人工勢場算法相比，收斂過程更加平穩，大大減少了迭代次數，迭代次數減少了19次。

3) 本文算法不需要計算無效障礙物和斥力，大大降低了算法的運行時間，運行時間減少了近9 s，果園機器人的工作效率大大提高。