殷建軍 董文龍 梁利華 謝偉東 項(xiàng)祖豐

(浙江工業(yè)大學(xué)機(jī)械工程學(xué)院， 杭州 310023)

0 引言

路徑規(guī)劃是移動機(jī)器人研究的基礎(chǔ)[1-2]，是指在障礙環(huán)境下遵循特定的評價(jià)標(biāo)準(zhǔn)，尋找一條從起始點(diǎn)到目標(biāo)點(diǎn)的無碰撞移動路徑[3-4]，利用路徑規(guī)劃算法進(jìn)行不同方向優(yōu)化也是移動機(jī)器人研究的熱點(diǎn)問題[5-7]。

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

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

1 路徑規(guī)劃的損耗問題

1.1 距離-能量損耗模型

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

已知x、y兩點(diǎn)的坐標(biāo)，則x點(diǎn)到y(tǒng)點(diǎn)的損耗量可以表示為

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

(1)

式中D(x,y)——x、y兩點(diǎn)間的距離函數(shù)

E(x,y)——x、y兩點(diǎn)間的能量損耗函數(shù)

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

(2)

式中d(x,y)——x、y兩點(diǎn)間的實(shí)際距離函數(shù)，采用歐氏距離進(jìn)行計(jì)算

α(x,y)——x、y兩點(diǎn)間的仰角函數(shù)

αmax——兩點(diǎn)之間仰角的最大臨界值

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

(3)

式中m——輪式機(jī)器人與機(jī)載質(zhì)量

g——重力加速度

μ——機(jī)器人與地面間的動摩擦因數(shù)

在本文使用的算法中，兩點(diǎn)的實(shí)際距離采用歐氏距離進(jìn)行計(jì)算。

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

計(jì)算路徑的損耗量時(shí)，假設(shè)在地圖中存在一條可以從點(diǎn)ps到點(diǎn)pf的路徑，則可將路徑的總損耗量函數(shù)表示為

(4)

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

當(dāng)s≤i

(5)

式中pi——路徑ηpspf所遍歷的所有點(diǎn)

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

(6)

1.2 距離-能耗約束

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

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

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

(7)

式中cd——距離的約束值

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

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

(8)

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

2 算法改進(jìn)

2.1 傳統(tǒng)A*算法

傳統(tǒng)的A*算法[21-22]是一種啟發(fā)式搜索算法，具有最優(yōu)性、完備性和高效性等優(yōu)點(diǎn)[23]。A*算法的代價(jià)函數(shù)為

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

(9)

式中f(n)——當(dāng)前節(jié)點(diǎn)總啟發(fā)式代價(jià)

g(n)——起始點(diǎn)到當(dāng)前點(diǎn)的實(shí)際代價(jià)

h(n)——當(dāng)前點(diǎn)與目標(biāo)點(diǎn)的估計(jì)代價(jià)

該算法的原理是從起始點(diǎn)開始，對周圍的節(jié)點(diǎn)進(jìn)行擴(kuò)散，通過啟發(fā)函數(shù)計(jì)算得到具有最小啟發(fā)代價(jià)的點(diǎn)作為子節(jié)點(diǎn)，并將子節(jié)點(diǎn)移入到Close_list中，而其他已搜索到非最優(yōu)的子節(jié)點(diǎn)則移至Open_list中，不斷重復(fù)該過程直到搜索到目標(biāo)點(diǎn)，最后通過回溯得到一條最優(yōu)路徑。

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

2.2 改進(jìn)算法

假設(shè)要從地圖上一點(diǎn)ps到達(dá)地圖上的一點(diǎn)pf，已經(jīng)搜索到的中間路徑表示為ηn，引入距離-能量模型后，當(dāng)s≤i

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

(10)

其中

G(ηn)=L(ηn)

(11)

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

(12)

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

pn——當(dāng)前節(jié)點(diǎn)

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

H(pn,pf)——當(dāng)前節(jié)點(diǎn)到終點(diǎn)估計(jì)損耗量

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

與傳統(tǒng)A*算法類似，使用Open_list與Close_list記錄地圖節(jié)點(diǎn)的兩種狀態(tài)。Open_list記錄搜索到但沒有擴(kuò)展的節(jié)點(diǎn)及路徑， Close_list記錄已經(jīng)擴(kuò)展了的節(jié)點(diǎn)及路徑。每個(gè)節(jié)點(diǎn)pn通過數(shù)據(jù)結(jié)構(gòu)ε(pn)記錄4種不同的屬性，分別為當(dāng)前的節(jié)點(diǎn)pn、當(dāng)前路徑ηpn、G(ηpn)和F(ηpn)。

ECA*算法流程：

(1)完成運(yùn)算前的準(zhǔn)備工作，初始化各個(gè)變量，并將初始節(jié)點(diǎn)放入Open_list中。

(2)在Open_list中計(jì)算選擇最優(yōu)節(jié)點(diǎn)，放入Close_list中。在選擇過程中當(dāng)路徑和能量損耗處于矛盾時(shí)，優(yōu)先選擇路徑最優(yōu)。如果最優(yōu)節(jié)點(diǎn)為目標(biāo)點(diǎn)，則進(jìn)行路徑回溯獲取最終路徑。

(3)對最優(yōu)節(jié)點(diǎn)進(jìn)行擴(kuò)展，對各個(gè)節(jié)點(diǎn)的屬性值進(jìn)行計(jì)算，擴(kuò)展的同時(shí)剔除同一節(jié)點(diǎn)在Open_list和Close_list處于劣勢的路徑，并在列表中更新當(dāng)前的最優(yōu)路徑。若在兩個(gè)列表中都未記錄節(jié)點(diǎn)，則將最新擴(kuò)展的節(jié)點(diǎn)記錄于Open_list列表中。

ECA*算法改進(jìn)的地方在于以路徑為主的思想，路徑?jīng)Q定了行進(jìn)的能耗，所以在列表中記錄的不止有每個(gè)擴(kuò)展點(diǎn)的屬性，還記錄了擴(kuò)展點(diǎn)所在的路徑和當(dāng)前路徑的損耗量。其中每一個(gè)擴(kuò)展點(diǎn)有可能存在多個(gè)路徑經(jīng)過。當(dāng)某一擴(kuò)展點(diǎn)為當(dāng)前計(jì)算的最優(yōu)節(jié)點(diǎn)時(shí)，將同一個(gè)擴(kuò)展點(diǎn)的路徑進(jìn)行對比，選擇出距離最短、使用能量最少的路徑進(jìn)行再次擴(kuò)展，將其他處于劣勢的路徑及時(shí)進(jìn)行剔除，減少算法的計(jì)算量，提高了算法的計(jì)算效率。

3 仿真實(shí)驗(yàn)

3.1 環(huán)境建模

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

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

3.2 簡化因素

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

3.3 不同算法的能耗對比

本次仿真過程中將使用3種算法進(jìn)行路徑規(guī)劃，以展示ECA*算法的改進(jìn)過程與結(jié)果。第1種算法為傳統(tǒng)A*算法；第2種算法在A*算法的基礎(chǔ)上設(shè)置相鄰節(jié)點(diǎn)的仰角臨界值，即αmax，若仰角超過臨界值則表示兩點(diǎn)間路徑損耗超過預(yù)設(shè)值，則對路徑進(jìn)行剔除，是傳統(tǒng)A*算法與ECA*算法的一種過渡算法；第3種算法即為ECA*算法。

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

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

3.4 αmax對ECA*算法的影響

在ECA*算法中，αmax的實(shí)際意義是移動機(jī)器人的最大爬坡角，代表了機(jī)器人的最大驅(qū)動能力。但在實(shí)際規(guī)劃中，αmax不宜過大，通?？梢詼p小αmax的值，以增大部分路程為代價(jià)(由上文可知此代價(jià)通?？梢院雎圆挥?jì))，進(jìn)而選更優(yōu)的路徑，但是同時(shí)αmax不能取值過小，否則無法得到真實(shí)的最優(yōu)路徑，圖4、5為αmax對能量損耗的影響，表2為不同αmax完成路徑的能量損耗。因此在機(jī)器人工作時(shí)需要根據(jù)實(shí)際工況，選擇一個(gè)合適的αmax進(jìn)行計(jì)算。

圖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*算法

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

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

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

在沒有約束的情況下，算法在執(zhí)行從Open_list選擇最優(yōu)子節(jié)點(diǎn)這一步驟時(shí)，以距離最短作為選擇條件；當(dāng)在有約束的條件下，算法則可以選擇出路徑更長但能耗更少的路徑，得到進(jìn)一步優(yōu)化。從仿真可以看出，ECA*算法可以通過增大行進(jìn)路程為代價(jià)進(jìn)而尋找到能耗較低的路徑。

4 結(jié)束語

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