一種基于Voronoi_Obstacle場(chǎng)的移動(dòng)機(jī)器人路徑規(guī)劃算法

龐風(fēng)麟，駱敏舟, 3，柳 聰, 柏永華, 徐孝彬

(1.河海大學(xué)機(jī)電工程學(xué)院，江蘇 常州 213022) (2.河海大學(xué)江蘇省特種機(jī)器人技術(shù)重點(diǎn)實(shí)驗(yàn)室，江蘇 常州 213022) (3.江蘇集萃智能制造技術(shù)研究所有限公司， 江蘇 南京 211800)

近年來，移動(dòng)機(jī)器人廣受社會(huì)各界關(guān)注，在工業(yè)、農(nóng)業(yè)、服務(wù)、搜救等領(lǐng)域得到廣泛應(yīng)用[1]。路徑規(guī)劃作為智能移動(dòng)機(jī)器人的關(guān)鍵技術(shù)之一，成為各個(gè)領(lǐng)域研究的熱點(diǎn)[2-4]。

路徑規(guī)劃算法按照不同的工作需求，可分為路徑最優(yōu)算法、安全距離最優(yōu)算法、能量最優(yōu)算法等。其中路徑最優(yōu)算法主要包括A*算法、D*算法、Dijkstra算法、RRT算法、遺傳算法、粒子群算法、Q-learning算法等[2-4]；安全距離最優(yōu)算法主要包括人工勢(shì)場(chǎng)法、Voronoi圖算法等[2-4]；能量最優(yōu)算法主要包括基于隨機(jī)動(dòng)態(tài)正交水平集的能量最優(yōu)算法、基于改進(jìn)A*算法的能耗最優(yōu)路徑規(guī)劃等[5-6]。本文主要討論安全距離最優(yōu)算法。

Khatib[7]于1986年提出人工勢(shì)場(chǎng)法(artificial potential field，APF)，該算法可規(guī)劃一條避開障礙物的安全路徑，但在計(jì)算過程中可能出現(xiàn)因引力勢(shì)場(chǎng)和斥力勢(shì)場(chǎng)相互抵消而陷入局部最小解的情況[8]。趙明等[9]提出了域-人工勢(shì)場(chǎng)法，通過加入自適應(yīng)的域勢(shì)場(chǎng)，幫助移動(dòng)機(jī)器人根據(jù)域勢(shì)場(chǎng)快速到達(dá)目標(biāo)點(diǎn)，避免陷入疊加勢(shì)場(chǎng)帶來的局部最小解。Fedorenko等[10]提出了基于Voronoi圖的路徑規(guī)劃算法，通過生成的Voronoi圖，得到一條最優(yōu)安全路徑，但其采用三段式廣度優(yōu)先搜索規(guī)劃路徑，計(jì)算時(shí)間長(zhǎng)，無法滿足實(shí)時(shí)計(jì)算的要求。任曉兵等[11]提出了使用Voronoi圖的節(jié)點(diǎn)信息進(jìn)行路徑規(guī)劃，減少了路徑規(guī)劃計(jì)算時(shí)間，但因計(jì)算Voronoi節(jié)點(diǎn)信息增加了計(jì)算負(fù)擔(dān)，延長(zhǎng)了規(guī)劃時(shí)間，并且只使用節(jié)點(diǎn)信息，無法獲得最優(yōu)安全路徑。Dolgov等[12]提出了基于Voronoi場(chǎng)的Hybrid A*算法，但Voronoi場(chǎng)計(jì)算復(fù)雜度高，計(jì)算時(shí)間長(zhǎng)，難以滿足實(shí)時(shí)計(jì)算的要求。

1 Voronoi_Obstacle場(chǎng)

Dolgov等[12]提出基于最近障礙物和最近Voronoi點(diǎn)信息構(gòu)建Voronoi場(chǎng)。在二維空間，使用K-D樹搜索最近障礙物和最近Voronoi點(diǎn)時(shí)，構(gòu)建Voronoi場(chǎng)的復(fù)雜度為O(n2.5)，當(dāng)機(jī)器人工作場(chǎng)景變大時(shí)，Voronoi場(chǎng)計(jì)算量變大，計(jì)算時(shí)間延長(zhǎng)。本文采用分層多步驟[13]的思想，其層次結(jié)構(gòu)如圖1所示。

圖1 層次結(jié)構(gòu)

1.1 基于障礙物的勢(shì)場(chǎng)

地圖膨脹廣泛應(yīng)用于移動(dòng)機(jī)器人導(dǎo)航，通過設(shè)置合適的膨脹值，可以規(guī)劃出遠(yuǎn)離障礙物的安全路徑, 降低碰撞的風(fēng)險(xiǎn)[13-15]。本文采用Lu等[13]提出的膨脹思想建立障礙物的勢(shì)場(chǎng)，考慮移動(dòng)機(jī)器人實(shí)際尺寸，勢(shì)場(chǎng)公式如下：

(1)

式中：κx為距離障礙物的危險(xiǎn)值，其會(huì)隨著遠(yuǎn)離障礙物的方向逐漸趨向于0；x為配置空間中的點(diǎn)；X為機(jī)器人的配置空間；pob為x處最近的障礙物；d(x,pob)為x與pob的距離；Drodot為機(jī)器人的尺寸；μ為衰減率。

1.2 基于Voronoi圖的勢(shì)場(chǎng)

Voronoi圖由俄國(guó)數(shù)學(xué)家格奧爾吉·沃羅諾伊提出，其將平面劃分為基于特定點(diǎn)子集的子區(qū)域，其中特定點(diǎn)子集(稱為種子)是預(yù)先指定的，并且每個(gè)種子所對(duì)應(yīng)的子區(qū)域中任一點(diǎn)到該種子的距離要比到其他任何種子的距離近，Voronoi圖的定義如下[16-17]：

Rk={x∈X|d(x,Pk)≤d(x,Pj) for all

j≠k}

(2)

式中：Rk為Voronoi區(qū)域；Pk，Pj為特定元組(種子)；d(x,Pk)為x到Pk的距離。

本文采用Lau等[18]提出的方法得到Voronoi圖。與1.1節(jié)算法思想一致，本文采用膨脹思想對(duì)生成的Voronoi圖建立Voronoi勢(shì)場(chǎng)，Voronoi勢(shì)場(chǎng)不考慮機(jī)器人的實(shí)際尺寸，勢(shì)場(chǎng)公式如下：

χx=e-vd(x,pvo)x∈X

(3)

式中：χx∈[0,1)，表示距離Voronoi的程度值，其會(huì)隨著遠(yuǎn)離Voronoi的方向逐漸趨向于0；pvo表示距離機(jī)器人最近的Voronoi點(diǎn)；d(x,pvo)為x到pvo的距離；v為增益率。

1.3 基于障礙物勢(shì)場(chǎng)和Voronoi圖勢(shì)場(chǎng)的Voronoi_ Obstacle場(chǎng)

傳統(tǒng)的Voronoi場(chǎng)的計(jì)算公式如下[12]：

(4)

傳統(tǒng)Voronoi場(chǎng)的復(fù)雜度為O(n2.5)，其復(fù)雜度高，計(jì)算時(shí)間長(zhǎng)。為解決此問題，本文提出采用分層多步驟的思想計(jì)算Voronoi_Obstacle場(chǎng)，計(jì)算公式如下：

(5)

式中：ρx為Voronoi_Obstacle場(chǎng)在x處的勢(shì)場(chǎng)值，當(dāng)κx≥κmax時(shí)，ρx為0，ρx會(huì)隨著遠(yuǎn)離Voronoi的方向逐漸趨向于1；κmax為最大衰減值，是常量值。

式(5)不考慮最近障礙物和最近Voronoi點(diǎn)的距離，而是通過κx和χx計(jì)算Voronoi_Obstacle場(chǎng)。在二維空間，本文提出的Voronoi_Obstacle場(chǎng)的復(fù)雜度為O(n2)，復(fù)雜度降低O(n0.5)。

基于障礙物勢(shì)場(chǎng)和Voronoi圖勢(shì)場(chǎng)的Voronoi_Obstacle場(chǎng)詳細(xì)的分層結(jié)構(gòu)如圖2所示。

圖2 Voronoi_Obstacle場(chǎng)的分層結(jié)構(gòu)圖

2 基于Voronoi_Obstacle場(chǎng)的啟發(fā)式搜索算法

A*算法是一種典型的啟發(fā)式搜索計(jì)算方法，其表達(dá)式如下：

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

(6)

式中：f(n)為從起始點(diǎn)到目標(biāo)點(diǎn)的估價(jià)函數(shù)；g(n)為從起始點(diǎn)到節(jié)點(diǎn)n的移動(dòng)代價(jià)；h(n)為從節(jié)點(diǎn)n到目標(biāo)節(jié)點(diǎn)估計(jì)代價(jià)，其表示啟發(fā)式代價(jià)。

本文采用改進(jìn)的A*算法對(duì)Voronoi_Obstacle場(chǎng)進(jìn)行啟發(fā)式搜索，在A*的基礎(chǔ)上加入啟發(fā)式權(quán)重值，可以人為更改啟發(fā)值的權(quán)重，并且對(duì)g(n)和h(n)重新進(jìn)行定義，表達(dá)式如下：

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

(7)

(8)

(9)

式中：w為啟發(fā)式權(quán)重值；ρi為配置空間i處的Voronoi場(chǎng)值；d(x,y)表示從x到y(tǒng)的距離。

其算法流程如圖3所示，updateState()函數(shù)和Main()函數(shù)與傳統(tǒng)的A*算法相同，此處不展開描述。

圖3 算法流程圖

3 實(shí)驗(yàn)分析

3.1 實(shí)驗(yàn)硬件配置及環(huán)境描述

3.1.1實(shí)驗(yàn)硬件配置描述

圖4為自主研發(fā)的差速輪移動(dòng)底盤，平臺(tái)上裝有慣性測(cè)量單元(inertial measurement unit, IMU)、輪式里程計(jì)、二維激光雷達(dá)等傳感器。IMU采用三馳慣性公司的SC-AHRS-100D4，采集頻率為100 Hz；二維激光雷達(dá)采用鐳神智能系統(tǒng)有限公司的N301激光雷達(dá)，量程為30 m，采集頻率為10 Hz；差速輪采用輪轂電機(jī)，編碼器采集頻率為50 Hz；工控機(jī)采用I7-7500U。

圖4 差速輪移動(dòng)底盤

3.1.2實(shí)驗(yàn)環(huán)境描述

為驗(yàn)證提出的基于Voronoi_Obstacle場(chǎng)的移動(dòng)機(jī)器人路徑規(guī)劃算法的可行性及優(yōu)越性，本文利用室內(nèi)真實(shí)環(huán)境進(jìn)行對(duì)比實(shí)驗(yàn)。實(shí)際場(chǎng)景(25 m×37 m)如圖5所示，對(duì)環(huán)境進(jìn)行二維地圖構(gòu)建，結(jié)果如圖6所示，本文采用Lau等[18]提出的方法生成Voronoi圖，結(jié)果如圖7所示。

圖5 實(shí)際場(chǎng)景

圖6 構(gòu)建的二維地圖

圖7 基于二維地圖的Voronoi圖

3.2 實(shí)驗(yàn)結(jié)果對(duì)比分析

本文進(jìn)行3組實(shí)驗(yàn)對(duì)比分析。

2)將傳統(tǒng)的基于Voronoi圖的路徑規(guī)劃算法[10]與基于Voronoi_Obstacle場(chǎng)的路徑規(guī)劃算法進(jìn)行對(duì)比實(shí)驗(yàn)。采用圖9中的Voronoi_Obstacle場(chǎng)進(jìn)行路徑規(guī)劃，并設(shè)起點(diǎn)為(2.3，-7.5)、終點(diǎn)為(15.8，-31.0)。實(shí)驗(yàn)結(jié)果如圖10，11所示，本文進(jìn)行30次實(shí)驗(yàn)，并取平均值，基于Voronoi圖的路徑規(guī)劃的平均時(shí)間為0.863 s，而基于Voronoi_Obstacle場(chǎng)的路徑規(guī)劃的平均時(shí)間為0.565 s，時(shí)間縮短34.53%，并得到一條安全距離路徑，保證了機(jī)器人的安全運(yùn)行。實(shí)驗(yàn)結(jié)果表明，所提出的基于Voronoi_Obstacle場(chǎng)的移動(dòng)機(jī)器人路徑規(guī)劃算法具有一定的可行性和優(yōu)越性。

圖8 傳統(tǒng)的Voronoi場(chǎng)圖

圖9 Voronoi_Obstacle場(chǎng)圖

圖10 傳統(tǒng)基于Voronoi圖的路徑規(guī)劃算法

圖11 基于Voronoi_Obstacle場(chǎng)的路徑規(guī)劃算法

3)將改進(jìn)的基于Voronoi圖的路徑規(guī)劃[11]與本文提出的基于Voronoi_Obstacle場(chǎng)的路徑規(guī)劃算法進(jìn)行對(duì)比實(shí)驗(yàn)。采用圖9中的Voronoi_Obstacle場(chǎng)，并設(shè)起點(diǎn)為(2.3，-7.5)、終點(diǎn)為(15.8，-31.0)。實(shí)驗(yàn)結(jié)果如圖12，13所示，本文進(jìn)行30次實(shí)驗(yàn)，并取平均值，改進(jìn)的基于Voronoi圖的路徑規(guī)劃的平均時(shí)間為0.828 s，而基于Voronoi_Obstacle場(chǎng)的路徑規(guī)劃的平均時(shí)間為0.565 s，時(shí)間縮短31.76%。改進(jìn)的基于Voronoi圖的路徑規(guī)劃計(jì)算Voronoi節(jié)點(diǎn)信息時(shí)間為0.804 s，占據(jù)總規(guī)劃時(shí)間的94.1%，表明計(jì)算Voronoi節(jié)點(diǎn)增加了計(jì)算負(fù)擔(dān)，延長(zhǎng)了規(guī)劃時(shí)間，并且如圖12中A、B所示，改進(jìn)的基于Voronoi圖的路徑規(guī)劃算法并未完全使用Voronoi點(diǎn)信息，只使用了Voronoi節(jié)點(diǎn)信息，其會(huì)產(chǎn)生一條偽最優(yōu)或者次優(yōu)的安全距離路徑。

圖12 改進(jìn)的基于Voronoi圖的路徑規(guī)劃算法

圖13 基于Voronoi_Obstacle場(chǎng)的路徑規(guī)劃算法

4 結(jié)束語

本文研究了Voronoi場(chǎng)計(jì)算復(fù)雜和基于Voronoi圖路徑規(guī)劃計(jì)算緩慢的問題。主要?jiǎng)?chuàng)新點(diǎn)是提出了一種基于Voronoi_Obstacle場(chǎng)的移動(dòng)機(jī)器人路徑規(guī)劃算法，從而縮短生成Voronoi_Obstacle場(chǎng)和路徑規(guī)劃的計(jì)算時(shí)間。傳統(tǒng)的基于Voronoi圖的路徑規(guī)劃，在空間變大、毛刺增多、柵格密度變大等情況下，Voronoi信息增多，規(guī)劃緩慢，并且傳統(tǒng)的Voronoi場(chǎng)計(jì)算復(fù)雜度高，計(jì)算緩慢，本文利用基于改進(jìn)的Voronoi_Obstacle場(chǎng)進(jìn)行啟發(fā)式搜索，大大減少了計(jì)算量，縮短了規(guī)劃時(shí)間。該方法適用于移動(dòng)機(jī)器人的路徑規(guī)劃，可提供可靠、實(shí)時(shí)、安全的導(dǎo)航路徑。本文在實(shí)際室內(nèi)環(huán)境進(jìn)行了測(cè)試驗(yàn)證，通過3組實(shí)驗(yàn)對(duì)比，表明本文提出的基于Voronoi_Obstacle場(chǎng)的移動(dòng)機(jī)器人路徑規(guī)劃算法具有一定的可行性和優(yōu)越性。