復雜環境下農業機器人路徑規劃優化方法

殷建軍 董文龍 梁利華 謝偉東 項祖豐

(浙江工業大學機械工程學院， 杭州 310023)

0 引言

路徑規劃是移動機器人研究的基礎[1-2]，是指在障礙環境下遵循特定的評價標準，尋找一條從起始點到目標點的無碰撞移動路徑[3-4]，利用路徑規劃算法進行不同方向優化也是移動機器人研究的熱點問題[5-7]。

機器人工作工況與所處環境緊密相關，不同的環境下使用的工作策略也不盡相同。當移動機器人進行室外作業時，考慮到室外地形大都不平坦[8]，機器人在復雜地形環境下通常會受到自身功率和能量限制，局部產生不穩定[9]，降低作業效率與工作完成率。傳統的路徑規劃算法通常以距離為成本生成最優路徑，但是在機器人實際運動過程中，這種最優路徑由于變化率相對較高，反而會造成機器人的能耗過大。對于復雜環境下作業的機器人，通過能耗限制策略尋找最優路徑極為重要，因此也出現了許多針對能量優化的室外路徑規劃算法。文獻[10]提出了一種物理模型，可計算出在各種外力作用下移動機器人的能量損耗，并考慮了重力效應、功率限制等不平坦路面必須解決的問題。文獻[11]改進了能量成本模型，確定了垂直軸理想錐體表面摩擦和重力作用的近似最佳路徑。文獻[12]引入了地形面重量概念，捕獲一些基于位置的地形參數，并提出了一種多項式時間近似算法，用于尋找最短的各向異性路徑。文獻[13]提出了一種將估計能耗加入總行駛能耗的迭代算法，應用于強干擾環境。文獻[14]提出了一種基于約束-感知的啟發式路徑規劃算法，使用鋸齒形路徑模式估計不平坦地形上的成本。文獻[15-16]對一些傳統的算法根據能耗模型估算進行優化。文獻[17-18]對機器人軌跡控制優化，降低了能耗。

本文提出一種Energy Constraint A*算法，是一種改進啟發式搜索路徑規劃算法，簡稱ECA*算法。與文獻[8]添加比例因子的方法不同，本文算法將利用距離-能量損耗模型作為路徑搜索啟發代價，在給定約束條件下，使用最佳優先搜索策略尋找能量損耗最優路徑。

1 路徑規劃的損耗問題

1.1 距離-能量損耗模型

為了計算移動機器人在行進過程中的能量損耗，構造了兩點之間距離-能量損耗模型，通過迭代累計和一定的約束限制，即可計算出移動機器人在整條路徑中的能耗。

已知x、y兩點的坐標，則x點到y點的損耗量可以表示為

L(x,y)=(D(x,y),E(x,y))

(1)

式中D(x,y)——x、y兩點間的距離函數

E(x,y)——x、y兩點間的能量損耗函數

考慮機器人移動時所克服的重力與摩擦力的影響，D(x,y)的計算公式為[19]

(2)

式中d(x,y)——x、y兩點間的實際距離函數，采用歐氏距離進行計算

α(x,y)——x、y兩點間的仰角函數

αmax——兩點之間仰角的最大臨界值

E(x,y)的計算公式為[19]

(3)

式中m——輪式機器人與機載質量

g——重力加速度

μ——機器人與地面間的動摩擦因數

在本文使用的算法中，兩點的實際距離采用歐氏距離進行計算。

從式(2)、(3)中可以看出，當兩點之間的角度超過最大臨界值時，損耗量將變成無窮，所以為了減少能量的損耗，在搜索路徑的過程中會把上升坡度變化率過高的路徑剔除。

計算路徑的損耗量時，假設在地圖中存在一條可以從點ps到點pf的路徑，則可將路徑的總損耗量函數表示為

(4)

式中ηpspf——行進中所走過路徑(點的集合)[20]

當s≤i

(5)

式中pi——路徑ηpspf所遍歷的所有點

將式(4)代入到式(1)中，可得到

(6)

1.2 距離-能耗約束

使用距離-能量損耗模型搜索路徑時，距離的成本為行進時間，路徑越長則耗費的時間越多，能量的成本為輪式機器人的電量。若只考慮距離的影響，則可能會造成電池能大量的浪費，若只考慮能量的損耗，則可能會大大增長完成路徑的時間。所以在路徑搜索過程中要兼顧兩者。

在點ps到點pf的路徑搜索過程中，定義距離-能耗約束為

C(ps,pf)=(cd,ce)

(7)

式中cd——距離的約束值

ce——能量損耗約束值，為供電量最大值

假設算法搜索到路徑ηpspf，路徑損耗如式(4)所示，當且僅當

(8)

成立時，路徑ηpspf得到保留，否則將此路徑剔除。

2 算法改進

2.1 傳統A*算法

傳統的A*算法[21-22]是一種啟發式搜索算法，具有最優性、完備性和高效性等優點[23]。A*算法的代價函數為

f(n)=g(n)+h(n)

(9)

式中f(n)——當前節點總啟發式代價

g(n)——起始點到當前點的實際代價

h(n)——當前點與目標點的估計代價

該算法的原理是從起始點開始，對周圍的節點進行擴散，通過啟發函數計算得到具有最小啟發代價的點作為子節點，并將子節點移入到Close_list中，而其他已搜索到非最優的子節點則移至Open_list中，不斷重復該過程直到搜索到目標點，最后通過回溯得到一條最優路徑。

雖然傳統A*算法能夠高效地找出最短路徑，但是并沒有考慮到機器人實際的運動和消耗，最短路徑同時也意味著機器人在行進過程中需要經歷快速持續的地形變化以及大功率的輸出，反而會造成更多的能量損耗。

2.2 改進算法

假設要從地圖上一點ps到達地圖上的一點pf，已經搜索到的中間路徑表示為ηn，引入距離-能量模型后，當s≤i

F(ηn)=G(ηn)+H(pn,pf)

(10)

其中

G(ηn)=L(ηn)

(11)

H(pn,pf)=L(pn,pf)

(12)

式中F(ηn)——總的啟發式損耗量

pn——當前節點

G(ηn)——走過中間路徑ηn的損耗量

H(pn,pf)——當前節點到終點估計損耗量

ECA*算法的框架為

Initialize variables and lists

insertε(ps) into Open_list

while Open_list is not empty

calculate the minε(pn) from Open_list

if exist other entries

select the best one

remove the minε(pn) from Open_list

insert the minε(pn) into Close_list

ifε(pn) isε(pf)

trace back and get the path

else

for eachε(pn+1) ofε(pn)

calculateε(pn+1)

ifε(pn+1) is out ofC(ps,pe)

remove the path

continue

if existε′(pn+1) in Open_list or Close_list

compare and choose the best one

remove the paths

continue

insertε(pn+1) into Open_list

與傳統A*算法類似，使用Open_list與Close_list記錄地圖節點的兩種狀態。Open_list記錄搜索到但沒有擴展的節點及路徑， Close_list記錄已經擴展了的節點及路徑。每個節點pn通過數據結構ε(pn)記錄4種不同的屬性，分別為當前的節點pn、當前路徑ηpn、G(ηpn)和F(ηpn)。

ECA*算法流程：

(1)完成運算前的準備工作，初始化各個變量，并將初始節點放入Open_list中。

(2)在Open_list中計算選擇最優節點，放入Close_list中。在選擇過程中當路徑和能量損耗處于矛盾時，優先選擇路徑最優。如果最優節點為目標點，則進行路徑回溯獲取最終路徑。

(3)對最優節點進行擴展，對各個節點的屬性值進行計算，擴展的同時剔除同一節點在Open_list和Close_list處于劣勢的路徑，并在列表中更新當前的最優路徑。若在兩個列表中都未記錄節點，則將最新擴展的節點記錄于Open_list列表中。

ECA*算法改進的地方在于以路徑為主的思想，路徑決定了行進的能耗，所以在列表中記錄的不止有每個擴展點的屬性，還記錄了擴展點所在的路徑和當前路徑的損耗量。其中每一個擴展點有可能存在多個路徑經過。當某一擴展點為當前計算的最優節點時，將同一個擴展點的路徑進行對比，選擇出距離最短、使用能量最少的路徑進行再次擴展，將其他處于劣勢的路徑及時進行剔除，減少算法的計算量，提高了算法的計算效率。

3 仿真實驗

3.1 環境建模

針對路徑規劃的能耗問題，在三維柵格地圖中運行路徑規劃算法并得到仿真結果。三維地形圖選取學校周邊高程圖進行建模仿真，其中，地圖采用WGS84坐標系進行構建，長半軸為6 378 137，扁率為298.26，采樣地理位置為東經120.03°、北緯30.23°。為了降低計算量，地圖的采樣精度約為9.55 m(L15)，仿真的三維地圖如圖1所示。兩種算法均在Visual Stutio中實現，在仿真中算法采用的參數數值分別為：質量m為50 kg、重力加速度g為9.81 m/s2、摩擦因數μ為0.25、兩點之間仰角的最大臨界值αmax為0.15°。

圖1 仿真中使用的三維地形圖Fig.1 3D map of terrain used in simulation

3.2 簡化因素

在構造距離-能耗模型時，為了簡化模型、提高算法計算的效率，并且考慮到路面屬性的未知性，同時，控制變量摩擦因數一定，仿真的過程也將更加簡便，結果也將更為直觀，所以將模型中的滑動摩擦因數設定為常數，并忽略了機器人在行進過程中與地面的滑轉率等相關的影響因素。因此仿真結果也將與真實值之間產生相對應的誤差，但是與總的能量消耗相比，滑轉率等因素產生的誤差可以保持在一個較低的水平。而且控制不同的算法采用相同的能耗計算方式，也將體現出本文算法的有效性。

3.3 不同算法的能耗對比

本次仿真過程中將使用3種算法進行路徑規劃，以展示ECA*算法的改進過程與結果。第1種算法為傳統A*算法；第2種算法在A*算法的基礎上設置相鄰節點的仰角臨界值，即αmax，若仰角超過臨界值則表示兩點間路徑損耗超過預設值，則對路徑進行剔除，是傳統A*算法與ECA*算法的一種過渡算法；第3種算法即為ECA*算法。

將點S(50，200，65.04)m作為路徑的起點，將點F(900，1 000，75.35)m作為路徑的終點，爬升的高度為10 m。為了能夠更清晰地觀察路徑變化，以下將分別在三維柵格圖和平面云圖中展示規劃的路徑，如圖2、3所示，完成路徑所需要行進的路程和消耗的能量對比如表1所示。

圖2 不同算法的路徑在三維柵格圖中的對比Fig.2 Path comparison in grids with different algorithms

圖3 不同算法平面云圖中的路徑對比Fig.3 Path comparison in cloud map with different algorithms

表1 不同算法完成的能量損耗對比Tab.1 Path energy loss comparison with different algorithms

能量損耗主要由行進過程中與路面的滑動摩擦和爬升高度決定。由圖2、3可以看出，通過預設αmax的過渡算法能夠避免行進至坡度過大的路徑，進而避免由于不停爬升和下坡導致的能量浪費，所以優先選擇較為平整的路面進行計算和規劃。ECA*算法對過渡算法進一步改進，通過節點的擴展與路徑損耗量的積累與啟發計算，得到局部能耗最少的點組成的路徑。ECA*算法在路程能量消耗上比傳統A*算法的能量損耗減少14.87%，而實際增加的行程卻可以忽略不計。仿真結果表明，ECA*算法通過一些局部路徑的優化使得在路徑少量增加的同時，能量損耗卻大幅度降低。

3.4 αmax對ECA*算法的影響

在ECA*算法中，αmax的實際意義是移動機器人的最大爬坡角，代表了機器人的最大驅動能力。但在實際規劃中，αmax不宜過大，通常可以減小αmax的值，以增大部分路程為代價(由上文可知此代價通常可以忽略不計)，進而選更優的路徑，但是同時αmax不能取值過小，否則無法得到真實的最優路徑，圖4、5為αmax對能量損耗的影響，表2為不同αmax完成路徑的能量損耗。因此在機器人工作時需要根據實際工況，選擇一個合適的αmax進行計算。

圖4 不同αmax的路徑在三維柵格圖中的對比Fig.4 Path comparison in grids with different αmax

圖5 不同αmax的路徑在平面云圖中的對比Fig.5 Path comparison in cloud map with different αmax

αmax/(°)路程/m能量損耗/J1.811792468161.511811924941.211971815990.912161823130.614162112960.51689230209

3.5 添加能量約束后的ECA*算法

上述仿真實驗都是建立在沒有損耗約束的情況下完成的，即C(ps,pf)=(∞,∞)，而在移動機器人實際室外作業中，其能量總量通常是受限制的，因此在設定能量約束后，即C(ps,pf)=(∞,ce)，不同ce對應的路徑如圖6、7所示。

圖6 不同約束下三維柵格圖的路徑對比Fig.6 Path comparison in grids with different ce

圖7 不同約束下生成的路徑對比Fig.7 Path comparison with different ce

在沒有約束的情況下，算法在執行從Open_list選擇最優子節點這一步驟時，以距離最短作為選擇條件；當在有約束的條件下，算法則可以選擇出路徑更長但能耗更少的路徑，得到進一步優化。從仿真可以看出，ECA*算法可以通過增大行進路程為代價進而尋找到能耗較低的路徑。

4 結束語

針對在室外復雜環境下作業的移動機器人存在因能量受限而降低工作完成率的問題，提出了一種基于改進的啟發式搜索的ECA*路徑規劃算法，建立了一種距離-能量模型，并將模型與啟發式搜索函數相融合，提出了新的代價函數，通過迭代、擴展、選擇最優或次優子節點，剔除處于劣勢的路徑，進而尋找到能量損耗最優路徑。仿真實驗結果表明，相比于傳統的A*算法，優化改進的路徑可在仿真環境下節省14.87%的能量消耗，而且通過分析參數αmax的作用，添加相應的能量約束，ECA*算法可以得到不同的符合需求的路徑，驗證了其有效性。