移動機器人動態(tài)路徑規(guī)劃方法的研究與實現(xiàn)

史 進，董 瑤,3，白振東，崔澤晨，董永峰

(1.河北工業(yè)大學(xué) 控制科學(xué)與工程學(xué)院，天津 300401； 2. 河北工業(yè)大學(xué) 計算機科學(xué)與軟件學(xué)院，天津 300401；3.河北省大數(shù)據(jù)計算重點實驗室，天津 300401)

移動機器人動態(tài)路徑規(guī)劃方法的研究與實現(xiàn)

史 進1,2，董 瑤1,2,3，白振東2,3，崔澤晨2,3，董永峰2,3*

(1.河北工業(yè)大學(xué) 控制科學(xué)與工程學(xué)院，天津 300401； 2. 河北工業(yè)大學(xué) 計算機科學(xué)與軟件學(xué)院，天津 300401；3.河北省大數(shù)據(jù)計算重點實驗室，天津 300401)

針對在未知動態(tài)障礙物存在且目標點移動的環(huán)境下，采用人工勢場法規(guī)劃路徑時斥力影響半徑往往大于障礙物的半徑從而導(dǎo)致動態(tài)障礙物與機器人發(fā)生碰撞的問題，提出非完全等待策略與Morphine算法相結(jié)合的改進人工勢場法動態(tài)路徑規(guī)劃策略。當動態(tài)障礙物與機器人發(fā)生側(cè)面碰撞時采用非完全等待策略；當動態(tài)障礙物與機器人發(fā)生迎面碰撞時采用Morphine算法局部規(guī)劃路徑；同時引入滾動窗口理論提高躲避動態(tài)障礙物的精確度。通過仿真實驗，與傳統(tǒng)人工勢場作對比，提出的改進算法在發(fā)生側(cè)面碰撞時要縮短12步，在發(fā)生迎面碰撞時要縮短6步，由此可得提出改進算法在路徑平滑性和規(guī)劃步數(shù)方面效果更優(yōu)。

路徑規(guī)劃；人工勢場；Morphine算法；非完全等待策略；滾動窗口

0 引言

近年來，動態(tài)路徑規(guī)劃問題一直是機器人技術(shù)和人工智能領(lǐng)域的一個重要研究課題，同時也是機器人技術(shù)能夠更廣泛應(yīng)用到其他領(lǐng)域的一個重要前提條件。當前常用的路徑規(guī)劃算法主要有柵格法[1]、人工勢場法[2]、遺傳算法[3]、神經(jīng)網(wǎng)絡(luò)[4]等，其中，人工勢場法是目前比較成熟且高效的一種算法，但其主要用于解決靜態(tài)環(huán)境下機器人路徑規(guī)劃問題，而對于解決動態(tài)環(huán)境下的運動路徑規(guī)劃問題并不理想，常由于斥力影響半徑大于障礙物的半徑而導(dǎo)致動態(tài)障礙物與機器人發(fā)生碰撞。為此本文提出了非完全等待策略與Morphine算法相結(jié)合的改進人工勢場路徑規(guī)劃策略，從在單一動態(tài)環(huán)境和復(fù)雜動態(tài)環(huán)境下進行的實驗結(jié)果可以看出，本文提出的改進策略在路徑平滑性和規(guī)劃路徑步數(shù)上更優(yōu)。

1 機器人路徑規(guī)劃方法

1.1 改進人工勢場法

人工勢場法即根據(jù)地圖的障礙物、目標點位置分別構(gòu)建斥力場和引力場，通過斥力和引力相互作用構(gòu)建人工虛擬勢場，機器人在勢場中規(guī)劃出一條無碰撞路徑。

在二維空間中設(shè)機器人的坐標向量為X=(x,y)，目標點的坐標向量為Xd=(xd,yd)，總勢場Usum(X)可以表示為引力場函數(shù)Uatt(X)和斥力場函數(shù)Urep(X)之和，即：

Usum(X)=Uatt(X)+Urep(X)

(1)

傳統(tǒng)人工勢場法的引力場函數(shù)Uatt(X)和斥力場函數(shù)Urep(X)計算公式如下：

Uatt(X)=0.5αρ2(X,Xo)

(2)

Urep(X)=

(3)

其中：α為引力的增益系數(shù)，是正數(shù)；β為斥力的增益系數(shù)，是正數(shù)；ρ(X,Xo)為機器人與障礙物之間的最短距離;ρo是一個大于零的常數(shù)，表示障礙物影響的距離，在ρo之外的機器人便不受此障礙物的影響。

改進人工勢場法[5-7]引入機器人和目標點的相對位置和速度，公式如下：

Uatt(p,V)=δp‖p-pd‖2+δv‖V-Vd‖2

(4)

其中：p為機器人當前位置，V為機器人當前速度，δp為相對位置引力系數(shù)，pd為目標點位置，Vd為目標點速度，δv為相對速度引力系數(shù)，‖p-pd‖為機器人與目標點之間的歐氏距離，‖V-Vd‖為機器人與目標點之間的相對速度。由此得出機器人所受目標點的引力，公式如下：

Fatt(p,V)=-▽Uatt(p,V)=-▽pUatt(p,V)-

▽vUatt(p,V)

(5)

改進的斥力場函數(shù)可表示為：

Urep(p,V)=

(6)

其中：Rr為機器人半徑；β1與β2為可變因子；ρo為障礙物影響距離；ρro是指機器人與障礙物邊界的距離；Vro為障礙物與機器人的相對速度。由此推出斥力公式：

Frep(p,V)=

(7)

1.2 Morphine算法

Morphine算法[8]即通過機器人獲取環(huán)境信息后統(tǒng)計前方備選的多條弧線路徑信息，選出最優(yōu)通行路徑。

設(shè)已知機器人的起點s和目標點d的坐標,機器人的方向角α(s和d的連線與x軸的夾角)、某一備選路徑弧線的半徑r,如圖1所示，要畫出備選路徑弧線必須知道該弧線的圓心c的坐標及某一路徑點t的坐標，其計算公式如式(8)～(9)所示:

(8)

(9)

其中θ為路徑點t的圓心角。

圖1 弧線上某點坐標推導(dǎo)圖Fig. 1 Coordinates of a point on the Arc

設(shè)定機器人路徑尋優(yōu)評估函數(shù)為：

(10)

式中：Arc表示弧線，O表示障礙物，L為每條弧線路徑長度；G為路徑拐點參數(shù)；M為路徑點t到目標點d的歐氏距離；α1、α2、α3為各參數(shù)的權(quán)值。當f值為無窮大時，弧線經(jīng)過障礙物，機器人與障礙物碰撞，此路徑不可取，選取f值最小的路徑為機器人運行最優(yōu)路徑。

2 動態(tài)環(huán)境下機器人路徑規(guī)劃

室內(nèi)環(huán)境復(fù)雜多變，具有動態(tài)不確定性，移動機器人如何有效避障是完成路徑規(guī)劃的重點。本文在局部路徑規(guī)劃中采用滾動窗口預(yù)測障礙物，分析碰撞類型，分別采取不同避障策略，最終完成動態(tài)路徑規(guī)劃。

2.1 滾動窗口碰撞預(yù)測

機器人在運動過程中采用滾動窗口[9]對周圍的環(huán)境信息進行探測，預(yù)測在其規(guī)劃好的全局路徑中是否有障礙物出現(xiàn); 若存在動態(tài)障礙物，則測定其速度與方向，預(yù)測其運動軌跡，制定有效的避碰策略。滾動窗口碰撞預(yù)測的流程如圖2所示，具體步驟如下：

1)場景預(yù)測。機器人開始運動時，采用啟發(fā)式方法，將傳感器所探測到的局部環(huán)境信息轉(zhuǎn)化為局部子目標[10]，同時預(yù)測動態(tài)障礙物的運動軌跡，設(shè)定機器人每走一步用時Δt，通過預(yù)測Δt時間內(nèi)機器人與動態(tài)障礙物兩條運動軌跡之間的相對位置來預(yù)測二者是否會發(fā)生碰撞。

2)滾動窗口更新優(yōu)化。按照滾動窗口預(yù)測結(jié)果和環(huán)境信息，進行子目標的局部路徑規(guī)劃，采取相應(yīng)策略有效避障，重新更新滾動窗口。

3)反饋初始化。在新的滾動窗口范圍內(nèi)，根據(jù)此時傳感器采集的信息，重新更新窗口內(nèi)障礙物的運動狀況和環(huán)境信息，以此循環(huán)預(yù)測。

圖2 滾動窗口碰撞預(yù)測流程Fig. 2 Collision prediction flow by rolling window

2.2 動態(tài)避障策略

根據(jù)滾動窗口預(yù)測結(jié)果，分析機器人與動態(tài)障礙物運動軌跡是否存在交集，即是否存在碰撞，如圖3所示。機器人與動態(tài)障礙物相對運動的3種典型情況如下：

1)無碰撞。若兩條運動軌跡不存在交集，如圖3(a)、(b)、(e)所示，此時機器人與障礙物在Δt范圍內(nèi)不會發(fā)生碰撞。

2)側(cè)面碰撞。若兩條運動軌跡之間存在交集，如圖3(c)、(d)所示，機器人與動態(tài)障礙物將在Δt范圍內(nèi)發(fā)生側(cè)面碰撞，此時機器人采取非完全等待策略[11]。假設(shè)障礙物的半徑為R，障礙物斥力影響半徑為r，當機器人在r范圍內(nèi)則開始等待，大于r則繼續(xù)前進，稱其為“完全等待”；但若機器人未等障礙物完全經(jīng)過(還在影響半徑r范圍內(nèi))就繼續(xù)前進，稱其為“非完全等待”。

3)迎面碰撞。若兩條運動軌跡之間有交集，如圖3(f)所示，在Δt時間內(nèi)機器人與動態(tài)障礙物發(fā)生迎面碰撞，則此時機器人采用Morphin算法作局部路徑規(guī)劃，當機器人不在動態(tài)障礙物影響半徑r范圍內(nèi)時再用改進人工勢場法作全局路徑規(guī)劃。

圖3 機器人與動態(tài)障礙物碰撞的幾種典型情況Fig. 3 Several typical cases of collision between robot and dynamic obstacle

由于室內(nèi)環(huán)境復(fù)雜多變，傳統(tǒng)人工勢場法存在障礙物附近目標不可達、易發(fā)生碰撞、狹窄通道等問題，本文提出采用改進人工勢場法進行全局路徑規(guī)劃，當遇到狹窄通道時，采用Morphine算法進行局部路徑規(guī)劃。

2.3 動態(tài)路徑規(guī)劃

機器人動態(tài)路徑規(guī)劃流程具體步驟如下。

步驟1 構(gòu)建室內(nèi)環(huán)境地圖。

步驟2 采用改進人工勢場法進行全局路徑規(guī)劃，且其目標點即機器人的勢場全局最小點。

步驟3 機器人沿全局規(guī)劃路徑行走。

步驟4 通過滾動窗口預(yù)測動態(tài)障礙物，判斷此障礙物是否會產(chǎn)生影響：若無影響，轉(zhuǎn)步驟5；有影響，則轉(zhuǎn)步驟6。

步驟5 判斷是否到達目標點：若到達目標點路徑規(guī)劃結(jié)束；反之，返回步驟2。

步驟6 判斷動態(tài)障礙物的運動方向和狀態(tài)，采用相應(yīng)的避障策略。

步驟7 判斷是否避障成功：若成功返回步驟3；反之，返回步驟2。

3 實驗結(jié)果及分析

本文采用上述動態(tài)路徑規(guī)劃方法，分別設(shè)計單一和復(fù)雜兩種動態(tài)障礙物環(huán)境進行20組實驗驗證，每類實驗舉一例說明。

3.1 單一環(huán)境動態(tài)路徑規(guī)劃

3.1.1 側(cè)面碰撞

假設(shè)機器人與動態(tài)障礙物發(fā)生側(cè)面碰撞，其參數(shù)設(shè)定如表1所示。

表1 側(cè)面碰撞參數(shù)列表Tab. 1 Parameter list of side collision

1) 機器人采取“完全等待”策略。

機器人采取“完全等待”策略的動態(tài)路徑規(guī)劃結(jié)果如圖4所示，機器人到達目標點步數(shù)為144步，且路徑非常平滑。

圖4 完全等待動態(tài)路徑規(guī)劃Fig. 4 Completely waiting for dynamic path planning

2) 機器人采取“非完全等待 ”策略。

機器人采取“非完全等待”策略的動態(tài)路徑規(guī)劃結(jié)果如圖5所示，采取“非完全等待”策略的規(guī)劃路徑雖沒有“完全等待”平滑，但機器人到達目標點步數(shù)為123步，小于“完全等待”規(guī)劃步數(shù)。

圖5 非完全等待動態(tài)路徑規(guī)劃Fig. 5 Non-completely waiting for dynamic path panning

同時在設(shè)定機器人步長為1的情況下選取3組實驗數(shù)據(jù)作對比，結(jié)果如表2所示，采取“非完全等待”策略在規(guī)劃步數(shù)，即運動時間上要遠小于“完全等待”策略。

表2 兩種等待策略步數(shù)對比Tab. 2 Comparison of steps by two waiting strategies

為再次證明“非完全等待”策略的有效性，本文與文獻[12]方法作對比，實驗環(huán)境不變，參數(shù)設(shè)定如表1所示，實驗結(jié)果如圖6所示，其中圖6(a)是采用文獻[12]中的未采用等待策略的路徑規(guī)劃，圖6(b)是采用“非完全等待”策略的路徑規(guī)劃，圖6(c)是兩種路徑規(guī)劃方法的局部避障放大對比圖，由圖6(c)可以看出本文提出的非完全等待避障策略不會使機器人在受到巨大斥力情況下倒退行走，避障效果更優(yōu)。

3.1.2 迎面碰撞

假設(shè)機器人與動態(tài)障礙物發(fā)生迎面碰撞，采取Morphine算法進行避障。為證明本文算法優(yōu)越性，與文獻[12]算法作對比，其參數(shù)設(shè)定如表3所示，步長為1。

表3 迎面碰撞參數(shù)列表Tab. 3 Parameter list of face-to-face collision

動態(tài)路徑規(guī)劃結(jié)果如圖7所示，其中圖7(a)為采用文獻[12]人工勢場算法的路徑規(guī)劃，圖7(b)為采用Morphine算法的路徑規(guī)劃，圖7(c)為兩種方法局部避障放大對比圖。不難看出未使用 Morphine 算法的機器人在遇到障礙物后會出現(xiàn)倒退現(xiàn)象，其運動到目標點的步數(shù)為169 步，使用 Morphine算法的機器人運動步數(shù)為159 步。因此，本文算法在避障效果和路徑規(guī)劃步數(shù)上更優(yōu)。

圖6 在一個障礙物的情形下側(cè)面碰撞路徑規(guī)劃對比Fig. 6 Comparison of side collision path planning with one obstacle

圖7 在一個障礙物的情形下迎面碰撞路徑規(guī)劃圖Fig. 7 Comparison of face-to-face collision path planning with one obstacle

3.2 復(fù)雜環(huán)境動態(tài)路徑規(guī)劃

建立復(fù)雜環(huán)境，在120×100的環(huán)境地圖中設(shè)定8個靜態(tài)障礙物和1個動態(tài)障礙物，機器人起始點坐標為(0,0)，目標點坐標為(100,100)，步長為1。

3.2.1 側(cè)面碰撞

設(shè)定動態(tài)障礙物起始點坐標為(69,54)，并向左下方運動，其半徑為3，影響半徑為6，采用傳統(tǒng)人工勢場法進行路徑規(guī)劃，如圖8所示，機器人整個動態(tài)避障的步數(shù)為45。

圖8 傳統(tǒng)人工勢場法避免側(cè)面碰撞Fig. 8 Side collision avoidance by traditional artificial potential field method

采用滾動窗口預(yù)測到機器人行至(65,44)時將與動態(tài)障礙物發(fā)生側(cè)面碰撞，于是采用本文提出的非完全等待進行避障，其動態(tài)路徑規(guī)劃如圖9所示，機器人整個動態(tài)避障的步數(shù)為33，可以明顯看出采用非完全等待的避障策略的行進路徑比傳統(tǒng)避障方法的路徑更平滑，所用步數(shù)更少。

圖9 非完全等待避障策略避免側(cè)面碰撞Fig. 9 Side collision avoidance by non-completely waiting strategy

3.2.2 迎面碰撞

設(shè)定動態(tài)障礙物的起始點坐標為(89,78)，并向左下方運動，其半徑為3，影響半徑為6，如圖10所示。采用常規(guī)避障方法，整個動態(tài)避障過程需要46步。

圖10 傳統(tǒng)人工勢場法避免迎面碰撞Fig. 10 Face-to-face collision avoidance by traditional artificial potential field method

而在相同環(huán)境下，采用滾動窗口預(yù)測到機器人行至(74,59)時將與動態(tài)障礙物發(fā)生迎面碰撞，采取Morphine算法進行局部避障，如圖11所示，實線為動態(tài)避障過程，整個動態(tài)避障過程需要40步，效果要優(yōu)于傳統(tǒng)人工勢場法。

圖11 Morphine算法避免迎面碰撞Fig. 11 Face-to-face collision avoidance by Morphine algorithm

4 結(jié)語

本文在動態(tài)環(huán)境下提出非完全等待策略與Morphine算法相結(jié)合的改進人工勢場法路徑規(guī)劃，并引入滾動窗口理論，對未知動態(tài)障礙物進行碰撞預(yù)測，分析碰撞模型進而采取相應(yīng)的避障策略。通過對比實驗可知，本文提出的改進策略具有更高的避障性能，且規(guī)劃步數(shù)更少。

References)

[1] WANG X, JIN Y, DING Z. A path planning algorithm of raster maps based on artificial potential field[C]// Proceedings of the 2015 Chinese Automation Congress. Piscataway, NJ: IEEE, 2015:627-632.

[2] YE B, ZHAO M, WANG Y. Research of path planning method for mobile robot based on artificial potential field[C]// Proceedings of the 2011 International Conference on Multimedia Technology. Piscataway, NJ: IEEE, 2011: 3192-3195.

[3] OZDIKIS O. Genetic algorithms with random coordinates for route planning on a 3D terrain[C]// Proceedings of the 2011 5th International Conference on Genetic and Evolutionary Computing. Washington, DC: IEEE Computer Society, 2011:146-149.

[4] JIANG M, YU Y, LIU X, et al. Fuzzy neural network based dynamic path planning[C]// Proceedings of the 2012 International Conference on Machine Learning and Cybernetics. Piscataway, NJ: IEEE, 2012: 326-330.

[5] CHEN L, LIU C, SHI H, et al. New robot planning algorithm based on improved artificial potential field[C]// Proceedings of the 3rd International Conference on Instrumentation, Measurement, Computer, Communication and Control. Washington, DC: IEEE Computer Society, 2013:228-232.

[6] 羅勝華,劉國榮,蔣燕.一種基于改進人工勢場法的移動機器人路徑規(guī)劃[J]. 微計算機信息, 2009, 25(29):188-190.(LUO S H, LIU G R, JIANG Y. A path planning of mobile robot based on improved artificial potential field method[J]. Microcomputer Information, 2009, 25(29):188-190.)

[7] 羅乾又, 張華, 王姮, 等.改進人工勢場法在機器人路徑規(guī)劃中的應(yīng)用[J].計算機工程與設(shè)計, 2011, 32(4):1411-1413.(LUO Q Y, ZHANG H, WANG H, et al. Application of improved artificial potential field approach in local path planning for mobile robot[J]. Computer Engineering and Design, 2011, 32(4):1411-1413.)

[8] 萬曉風(fēng),胡偉,鄭博嘉,等.基于改進蟻群算法與Morphin算法的機器人路徑規(guī)劃方法[J].科技導(dǎo)報, 2015,33(3): 84-89.(WAN X F,HU W,ZHENG B J, et al. Robot path planning method based on improved ant colony algorithm and Morphin algorithm[J].Science & Technology Review, 2015,33(3):88-89.)

[9] 叢巖峰, 基于滾動優(yōu)化原理的路徑規(guī)劃方法研究[D].長春: 吉林大學(xué),2007.(CONG Y F, The path planning method research based on rolling optimization theory [D]. Changchun: Jilin University, 2007.)

[10] LI X, XU H, LI M. A memory-based complete local search method with variable neighborhood structures for no-wait job shops[J]. International Journal of Advanced Manufacturing Technology, 2013,87(5/6/7/8):1401-1408.

[11] BRANKE J, RGEN, MIDDENDORF M, et al. Waiting strategies for dynamic vehicle routing [J]. Transportation Science, 2005, 39(3): 298-312.

[12] LIU Z X, YANG L X, WANG J G. Soccer robot path planning based on evolutionary artificial field[J].Advanced Materials Research, 2012, 568(8): 955-958.

This work is partially supported by the Tianjin Natural Science Foundation Project (14JCYBJC18500), the Tianjin Application Foundation and Frontier Technology Research Program (13JCQNJC00200).

SHIJin, born in 1981, Ph. D. candidate, lecturer. His research interests include artificial intelligence, robot positioning navigation.

DONGYao, born in 1982, Ph. D. candidate, experimentalist. Her research interests include intelligent information processing, artificial intelligence.

BAIZhendong, born in 1989, M. S. candidate. His research interests include robot positioning navigation, artificial intelligence.

CUIZechen, born in 1989, M. S. candidate. His research interests include robot positioning navigation.

DONGYongfeng, born in 1977, Ph. D., professor. His research interests include intelligent information processing.

Researchandimplementationofmobilerobotpathplanningmethod

SHI Jin1,2, DONG Yao1,2,3, BAI Zhendong2,3, CUI Zechen2,3,DONG Yongfeng2,3*

(1.SchoolofControlScienceandEngineering,HebeiUniversityofTechnology,Tianjin300401,China;2.SchoolofComputerScienceandEngineering,HebeiUniversityofTechnology,Tianjin300401,China;3.HebeiProvinceKeyLaboratoryofBigDataCalculation,Tianjin300401,China)

In the environment with unknown dynamic obstacle moving and target point, the radius of the repulsive force is often larger than the radius of the obstacle when the path is planned by the artificial potential field method, which leads to the collision of the dynamic obstacle with the robot. An improved dynamic path planning strategy of artificial potential field based on Morphine algorithm and non-completely waiting strategy was proposed. The non-completely waiting strategy was adopted when the dynamic obstacle collided with the robot on a side. The Morphine algorithm was used to localize the path when the dynamic obstacle collided with the robot face to face. Moreover, the rolling window theory was introduced to improve the accuracy of avoiding dynamic obstacles. Through the simulation tests, compared with the traditional artificial potential field, the proposed algorithm is shortened by 12 steps in the event of a side collision and 6 steps in the event of a face-to-face collision. Therefore, the improved algorithm is more effective in path smoothness and planning steps.

path planning; artificial potential field; Morphine algorithm; non-completely waiting strategy; rolling window

2017- 05- 16;

2017- 06- 08。

天津市自然科學(xué)基金資助項目(14JCYBJC18500); 天津市應(yīng)用基礎(chǔ)與前沿技術(shù)研究計劃項目(13JCQNJC00200)。

史進 (1981—)，男，河北張家口人，講師，博士研究生，主要研究方向：人工智能、機器人定位導(dǎo)航； 董瑤(1982—)，女，河北石家莊人，實驗師，博士研究生，主要研究方向：智能信息處理、人工智能； 白振東(1989—)，男，河北邯鄲人，碩士研究生，主要研究方向：機器人定位導(dǎo)航、人工智能; 崔澤晨(1989—),男，北京人，碩士研究生，主要研究方向：機器人定位導(dǎo)航; 董永峰(1987—),男，河北定州人，教授，博士，主要研究方向：智能信息處理。

1001- 9081(2017)11- 3119- 05

10.11772/j.issn.1001- 9081.2017.11.3119

(*通信作者電子郵箱dongyongfeng@scse.hebut.edu.cn)

TP399

A