基于兩點(diǎn)法的移動(dòng)機(jī)器人局部路徑規(guī)劃算法

宋 莉，李彩虹,朱寶艷，王小宇

(山東理工大學(xué) 計(jì)算機(jī)科學(xué)與技術(shù)學(xué)院，山東 淄博 255049)

基于兩點(diǎn)法的移動(dòng)機(jī)器人局部路徑規(guī)劃算法

宋 莉，李彩虹,朱寶艷，王小宇

(山東理工大學(xué) 計(jì)算機(jī)科學(xué)與技術(shù)學(xué)院，山東 淄博 255049)

針對(duì)傳統(tǒng)移動(dòng)機(jī)器人局部路徑規(guī)劃存在死區(qū)或陷阱區(qū)域等問(wèn)題，提出了一種新的兩點(diǎn)法算法.在路徑規(guī)劃過(guò)程中，首先利用兩點(diǎn)法確定機(jī)器人面向目標(biāo)的旋轉(zhuǎn)角度，進(jìn)而判斷出在前進(jìn)路徑中遇到障礙物后的路徑，并推導(dǎo)出在不同環(huán)境信息下最優(yōu)或次優(yōu)的規(guī)劃算法.在前進(jìn)過(guò)程中機(jī)器人只需要處理當(dāng)前狀態(tài)下的傳感器數(shù)據(jù)，不需要處理全局的復(fù)雜環(huán)境信息，節(jié)省了機(jī)器人存儲(chǔ)空間，提高了規(guī)劃效率.最后用RobotBasic仿真平臺(tái)對(duì)所推導(dǎo)的算法進(jìn)行仿真.結(jié)果驗(yàn)證了算法的有效性和可靠性.

移動(dòng)機(jī)器人；局部路徑規(guī)劃；兩點(diǎn)法；RobotBasic

機(jī)器人局部路徑規(guī)劃[1-2]具有魯棒性明顯、實(shí)時(shí)性顯著，能靈敏地處理外部信息隨時(shí)變化的狀態(tài)，應(yīng)用范圍較為廣泛.其缺點(diǎn)是只能依靠局部信息，容易出現(xiàn)局部極值點(diǎn)和震蕩，以至機(jī)器人無(wú)法順利到達(dá)目標(biāo)點(diǎn)，故需要合適的算法來(lái)解決這些問(wèn)題.局部路徑方法主要有神經(jīng)網(wǎng)絡(luò)算法[3]、模糊邏輯算法[4]、人工勢(shì)場(chǎng)法[5]、遺傳算法[6]等.

利用機(jī)器人本體上配備的緩沖器、紅外傳感器感知周圍障礙物信息，利用GPS和指南針?lè)治鰴C(jī)器人在所處環(huán)境中的位置.在算法推導(dǎo)中首先確定機(jī)器人面向目標(biāo)點(diǎn)的旋轉(zhuǎn)角度，然后在前進(jìn)路徑中獲取實(shí)時(shí)環(huán)境信息，判斷在遇到障礙物后的最優(yōu)或次優(yōu)路徑，并利用攝像頭尋找目標(biāo)點(diǎn)，提出了改進(jìn)后的兩點(diǎn)法算法.該算法的數(shù)學(xué)方法簡(jiǎn)單，計(jì)算量小，解決了移動(dòng)機(jī)器人陷入死區(qū)、停滯不前和只能找到可行性局部路徑的問(wèn)題.

1 移動(dòng)機(jī)器人傳感器

本文移動(dòng)機(jī)器人配備有緩沖器、紅外傳感器、攝像頭、GPS和指南針4種類型的傳感器[6].

機(jī)器人本體前后的緩沖器分別形成130°弧，兩側(cè)緩沖器形成50°弧.緩沖器安裝在簡(jiǎn)單的折疊開(kāi)關(guān)上，當(dāng)機(jī)器人撞上一個(gè)物體時(shí)，壓力使得一個(gè)或多個(gè)開(kāi)關(guān)關(guān)閉.緩沖器系統(tǒng)將每個(gè)傳感器的邏輯“1”(探測(cè)到障礙物)或“0”(無(wú)障礙)發(fā)送至計(jì)算機(jī)的對(duì)應(yīng)位.四個(gè)緩沖器狀態(tài)分別用二進(jìn)制表示如圖1所示.紅外傳感器使用紅外LED(發(fā)光二極管)，機(jī)器人向周圍發(fā)送紅光線.如果紅光線被反射回機(jī)器人，則晶體管電路可檢測(cè)到，推測(cè)附近有物體.5個(gè)紅外傳感器分別相隔45°度裝在機(jī)器人前面，如圖2所示.編碼的數(shù)字用來(lái)表示紅外傳感器的狀況.機(jī)器人右邊的傳感器是二進(jìn)制數(shù)字中最小的.每個(gè)傳感器按順時(shí)針旋轉(zhuǎn)進(jìn)行配位.攝像頭可用于探測(cè)機(jī)器人與物體之間的距離，為機(jī)器人提供圖像以供分析.GPS用來(lái)確定機(jī)器人和目標(biāo)點(diǎn)的位置，指南針確定機(jī)器人的當(dāng)前方向.

2 局部路徑規(guī)劃算法設(shè)計(jì)

2.1 基本兩點(diǎn)法的設(shè)計(jì)思路

兩點(diǎn)法的基本思想是在全局路徑規(guī)劃的基礎(chǔ)上，機(jī)器人首先通過(guò)衛(wèi)星定位確定機(jī)器人的當(dāng)前位置.假設(shè)A點(diǎn)為機(jī)器人的出發(fā)點(diǎn)，B點(diǎn)為終點(diǎn).然后利用裝在機(jī)器人周圍的傳感器感知障礙物和目標(biāo)點(diǎn)的位置信息.在沒(méi)有檢測(cè)到障礙物存在時(shí)，機(jī)器人沿著起點(diǎn)和終點(diǎn)的直線前進(jìn).在檢測(cè)到障礙存在時(shí)，機(jī)器人會(huì)確定子目標(biāo)點(diǎn)C的位置，然后沿直線朝著C點(diǎn)運(yùn)動(dòng).到達(dá)C點(diǎn)后再將C點(diǎn)當(dāng)作機(jī)器人的新的起點(diǎn)，檢測(cè)到CB之間有障礙物，再確定子目標(biāo)點(diǎn)D的位置.到達(dá)D點(diǎn)后再將D點(diǎn)當(dāng)作機(jī)器人的新的起點(diǎn)，尋找直線DB之間的無(wú)障礙路徑，以此類推，直到機(jī)器人到達(dá)最終需要到達(dá)的點(diǎn)[7]，如圖1所示.

圖1 基本的兩點(diǎn)法示意圖Fig.1 A schematic diagram of the basic two-point method

2.2 改進(jìn)后的兩點(diǎn)法的設(shè)計(jì)思路

該算法首先利用GPS和指南針獲取移動(dòng)機(jī)器人當(dāng)前位置(Rx,Ry)、機(jī)器人當(dāng)前方向CH和目標(biāo)點(diǎn)的位置(Tx,Ty)，建立坐標(biāo)系如圖2所示.

圖2 機(jī)器人面向信標(biāo)旋轉(zhuǎn)角度Fig.2 The rotation angle of robot oriented beacon

則機(jī)器人橫坐標(biāo)與目標(biāo)之差為

dX=Tx-Rx

(1)

機(jī)器人縱坐標(biāo)與目標(biāo)之差為

dY=Ty-Ry

(2)

機(jī)器人和目標(biāo)之間的距離R為

(3)

水平軸和機(jī)器人中心與目標(biāo)中心連線之間所形成的角度為

TA=π-arctan(dX/dY)

(4)

此角度用于計(jì)算需要旋轉(zhuǎn)的方向和度數(shù)dA，可使機(jī)器人朝向目標(biāo).指南針的基準(zhǔn)方向?yàn)楸?，?60°角，以東向?yàn)榛鶞?zhǔn)測(cè)量的機(jī)器人角度值TA是±360°角，機(jī)器人通常認(rèn)為東向?yàn)?°，為使機(jī)器人上的指南針能被機(jī)器人識(shí)別，故將TA轉(zhuǎn)換為以北為基準(zhǔn)，再加上90°，使TA準(zhǔn)換為指南針指向.再減去機(jī)器人上指南針指向的角度，就是機(jī)器人朝向目標(biāo)需要旋轉(zhuǎn)的角度dA，該角度為

dA=SA×180/π+90-CH

(5)

改進(jìn)后的兩點(diǎn)法的原理是首先利用GPS確定機(jī)器人和目標(biāo)點(diǎn)的當(dāng)前位置，分析機(jī)器人面向目標(biāo)點(diǎn)應(yīng)旋轉(zhuǎn)的角度，然后用傳感器確定機(jī)器人前進(jìn)路徑上是否有障礙物的存在.機(jī)器人在未遇到障礙物之前沿出發(fā)點(diǎn)與目標(biāo)點(diǎn)之間的直線向目標(biāo)點(diǎn)前進(jìn)[8]，檢測(cè)到障礙物之后，先確定此時(shí)機(jī)器人的方向，機(jī)器人在此方向的基礎(chǔ)上再通過(guò)向順時(shí)針和逆時(shí)針旋轉(zhuǎn)一定角度，判斷沿障礙物左側(cè)還是右側(cè)前進(jìn)的路徑更優(yōu)，然后沿確定好的最優(yōu)路徑前進(jìn)，邊行駛邊檢測(cè)是否有障礙物，調(diào)整機(jī)器人前進(jìn)方向.若檢測(cè)到機(jī)器人左邊有障礙物，則向右隨機(jī)旋轉(zhuǎn)0°～135°；若檢測(cè)到機(jī)器人右邊有障礙物，則向左隨機(jī)旋轉(zhuǎn)0°～135°，防止機(jī)器人陷入死區(qū).在單個(gè)障礙物環(huán)境中，前進(jìn)路徑中當(dāng)機(jī)器人上的攝像頭看到目標(biāo)點(diǎn)時(shí)，機(jī)器人沿直線行駛向目標(biāo)點(diǎn).在離散障礙物群中，機(jī)器人將恰好感知到第二個(gè)障礙物的點(diǎn)看做起點(diǎn)，在起點(diǎn)和目標(biāo)點(diǎn)之間用兩點(diǎn)法重新進(jìn)行路徑規(guī)劃，依次類推直到安裝在機(jī)器人上的攝像頭看到目標(biāo)點(diǎn)，則機(jī)器人沿直線行駛向目標(biāo)點(diǎn).機(jī)器人在每個(gè)障礙區(qū)存在的局部環(huán)境中找到局部最優(yōu)路徑，綜合起來(lái)即可以找到局部最優(yōu)路徑.

如圖3所示，為機(jī)器人遇到某個(gè)不規(guī)則障礙物局部最優(yōu)路徑選擇的算法分析.X為起點(diǎn)，Y為目標(biāo)點(diǎn)，假設(shè)起始點(diǎn)與目標(biāo)點(diǎn)之間的連線XY與水平線之間的夾角為α.若XY上有障礙物存在，沿逆時(shí)針?lè)较蛐D(zhuǎn)角度θ，得到XY1.若XY1上也有障礙物，由線段XY1沿順時(shí)針?lè)较蛐D(zhuǎn)2θ，得到XY2，以此類推.最先得到的無(wú)障礙物的直線，說(shuō)明那個(gè)方向的障礙物繞行路徑最短.故機(jī)器人返回到與水平線之間的夾角為α的朝向，用緩沖器和紅外傳感器感應(yīng)障礙物繞其確定好的方向前進(jìn).在前進(jìn)過(guò)程中用攝像頭隨時(shí)感應(yīng)判斷目標(biāo)點(diǎn)，一旦緩沖器和紅外傳感器未感應(yīng)到障礙物存在，并且攝像頭看到目標(biāo)點(diǎn)，則沿障礙物前進(jìn)沿用兩點(diǎn)法確定好的直線駛向目標(biāo)點(diǎn)，在距目標(biāo)點(diǎn)較近處停下來(lái)，防止機(jī)器人與目標(biāo)點(diǎn)相撞.改進(jìn)后的兩點(diǎn)法算法適合起始點(diǎn)和目標(biāo)點(diǎn)相距相對(duì)較遠(yuǎn)，且障礙物邊長(zhǎng)在機(jī)器人傳感器都能檢測(cè)到的范圍內(nèi).

圖3 機(jī)器人路徑規(guī)劃圖Fig.3 The map of robot path planning

2.3 出現(xiàn)的問(wèn)題及解決方案

表1 最優(yōu)局部路徑可行性分析
Tab.1 The feasibility analysis of the optimal local path

機(jī)器人位置目標(biāo)位置理論局部路徑實(shí)際局部路徑最優(yōu)局部路徑可行性可得到的局部路徑編號(hào)上部上部順時(shí)針順時(shí)針可行①中部順時(shí)針或逆時(shí)針逆時(shí)針可行②下部逆時(shí)針逆時(shí)針可行③中部上部順時(shí)針順時(shí)針可行④中部順時(shí)針逆時(shí)針不可行⑤下部逆時(shí)針逆時(shí)針可行⑥下部上部順時(shí)針順時(shí)針可行⑦中部逆時(shí)針順時(shí)針不可行⑧下部逆時(shí)針逆時(shí)針可行⑨

由表1中可得局部路徑編號(hào)為①②③④⑥⑦⑨的最優(yōu)局部路徑可行，利用新的兩點(diǎn)法算法能找到最優(yōu)路徑，局部路徑編號(hào)為⑤⑧的最優(yōu)局部路徑不可行，說(shuō)明新的兩點(diǎn)法算法在⑤⑧這種特殊環(huán)境下只能找到可行性路徑，因此在上述分析的基礎(chǔ)上對(duì)可行路徑進(jìn)行改進(jìn)，力求規(guī)劃出最優(yōu)局部路徑.

對(duì)⑤⑧兩種特殊環(huán)境下的機(jī)器人規(guī)劃出的可行性局部路徑進(jìn)行分析，如圖4中(a)(b)所示.利用新的兩點(diǎn)法算法，機(jī)器人在圖(a)中，根據(jù)理論性分析，機(jī)器人距離障礙物上端的距離較長(zhǎng)，因此最優(yōu)路徑是沿順時(shí)針?lè)较蝰傁蛑虚g目標(biāo)點(diǎn)，但是利用新的兩點(diǎn)法算法得出的最優(yōu)路徑是沿順時(shí)針?lè)较蚯斑M(jìn)，因此利用上述算法得出的是可行性局部路徑；圖(b)中利用新的兩點(diǎn)法算法沿順時(shí)針?lè)较蛞?guī)劃路徑，根據(jù)理論性分析機(jī)器人的最優(yōu)路徑是沿逆時(shí)針?lè)较蝰傁蛑虚g目標(biāo)點(diǎn)，因此規(guī)劃出的路徑也是可行性路徑.

(a) (b) (c)圖4 機(jī)器人與目標(biāo)點(diǎn)的位置分布示意圖Fig.4 A schematic diagram of the position distribution of robot and target point

(a)一般環(huán)境 (b)特殊環(huán)境圖5 兩點(diǎn)法算法流程圖Fig.5 The low chart of two-points method

針對(duì)上述環(huán)境信息中出現(xiàn)的問(wèn)題，利用幾何知識(shí)進(jìn)行分析，如圖4(c)所示，虛線圖形為機(jī)器人未檢測(cè)到障礙物的位置，實(shí)線為障礙物恰好被機(jī)器人傳感器檢測(cè)到的位置，當(dāng)機(jī)器人和目標(biāo)點(diǎn)位于的障礙物的兩端時(shí)，即當(dāng)直線AC與障礙物的端點(diǎn)相切于M點(diǎn)時(shí)，機(jī)器人能夠檢測(cè)到的障礙物的邊長(zhǎng)最長(zhǎng)為MNmax.

(6)

在三角形ANM中，

(7)

MN=AN×tanα

(8)

在三角形ABC和ANM中，利用三角形相似定理，

(9)

(10)

根據(jù)以上分析過(guò)程，針對(duì)不同環(huán)境信息所設(shè)計(jì)的算法流程圖如圖5所示.

3 路徑規(guī)劃的仿真與實(shí)驗(yàn)

仿真結(jié)果表明改進(jìn)后的兩點(diǎn)法算法算法簡(jiǎn)單、可行，充分利用多傳感器融合技術(shù)，減小了計(jì)算量，能成功規(guī)劃出機(jī)器人的較優(yōu)或次優(yōu)路徑.

(a) 順時(shí)針繞行復(fù)雜障礙物 (b) 逆時(shí)針繞行復(fù)雜障礙物 (c) 順時(shí)針繞行凹形障礙物

(d) 逆時(shí)針繞行凹形障礙物 (e) 順時(shí)針繞行復(fù)雜障礙物 (f) 順時(shí)針繞行復(fù)雜障礙物圖6 移動(dòng)機(jī)器人路徑仿真結(jié)果Fig.6 The simulation results of mobile robot path

4 結(jié)束語(yǔ)

本文利用改進(jìn)后的兩點(diǎn)法對(duì)機(jī)器人進(jìn)行了局部路徑規(guī)劃.基于RobotBasic仿真平臺(tái)，對(duì)所設(shè)計(jì)的規(guī)劃算法進(jìn)行了仿真.在一般的復(fù)雜環(huán)境中，能規(guī)劃出相對(duì)較優(yōu)的路徑，尤其適合當(dāng)起始點(diǎn)和目標(biāo)點(diǎn)距離相對(duì)較遠(yuǎn)、障礙物較小和數(shù)量較多的情景.針對(duì)在特殊環(huán)境信息下只能規(guī)劃出可行性路徑的問(wèn)題，利用幾何知識(shí)對(duì)該算法進(jìn)行了改進(jìn).該方法簡(jiǎn)單易懂，計(jì)算量少，解決了機(jī)器人在路徑規(guī)劃中存在的死區(qū)、在陷阱區(qū)容易左右徘徊以及在特殊環(huán)境信息只能規(guī)劃出可行性路徑的問(wèn)題.仿真結(jié)果表明，改進(jìn)后的兩點(diǎn)法算法能有效規(guī)劃出不同環(huán)境下機(jī)器人躲避障礙的局部最優(yōu)路徑.

[1] PARK J H,HUH U Y.Local path planning for mobile robot using artificial neural network-Potential field algorithm[J].Transactions of the Korean Institute of Electrical Engineer,2015,64(10):1 479-1 485.

[2] YU J J, DU H W,WANG G W,ed al. Research about local path planning of moving robot based on improved artificial potential field[C]//2013 25th Chinese Control and Decision Conference.Beijing:IEEE Computer Society,2013:2 861-2 865.

[3]蔡曉慧，李艷君，吳鐵軍.基于智能算法的移動(dòng)機(jī)器人路徑規(guī)劃研究[D].杭州：浙江大學(xué)，2007.

[4]林正鵬，關(guān)新平.多移動(dòng)機(jī)器人路徑規(guī)劃算法及實(shí)驗(yàn)研究[D].秦皇島：燕山大學(xué)，2013.

[5]WANG M, LIU J N K.Fuzzy logic based robot path planning in unknown environment[C]//International Conference on Machine Learnning and Cybernetics.Hong Kong: Institute of Electrical and ElectronicsEngineers Computer Society,2005:813-818.

[6] BLANKENSHIP J,MISHAL S.機(jī)器人編程設(shè)計(jì)與實(shí)現(xiàn)[M].卜遲武，唐慶菊,譯.北京：科學(xué)出版社，2010:23-124.

[7]李彩虹，張子間.基于兩點(diǎn)法的機(jī)器人路徑規(guī)劃[J].山東理工大學(xué)學(xué)報(bào)(自然科學(xué)版)，2005,19(1):17-20.

[8]康亮，趙春霞，郭劍輝.未知環(huán)境下改進(jìn)的基于BUG算法的移動(dòng)機(jī)器人路徑規(guī)劃[J].系統(tǒng)仿真學(xué)報(bào)，2009,21(17):5 414-5 422.

[9]顧德祥.ZKRT-300型機(jī)器人的RobotBasic編程與控制[J].電子世界，2013(24):242-243.

[10]朱映輝.RobotBasic應(yīng)用于人工智能課程的實(shí)踐教學(xué)研究[J].現(xiàn)代計(jì)算機(jī)(專業(yè)版)，2012(3):23-25.

[11] UEYAMA T, FUKUDA T. Cooperative search using genetic algorithm based on local information - path planning for structure configuration of cellular robot[C]//1993 International Conference on Intelligent Robots and Systems. Japan:Publ by IEEE,1993:1 110-1 118.

Researchonlocalpathplanningalgorithmforthemobilerobotbasedontwo-dotmethod

SONG Li , LI Cai-hong, ZHU Bao-yan, WANG Xiao-yu

(School of Computer Science and Technology, Shandong University of Technology, Zibo 255049, China)

This paper proposed an improved two-point method for the local path planning of the mobile robot for solving the tradition problems in unknown environment, such as dead zone or trap area. In the path planning process, the rotation angle of the robot target oriented is firstly determined by two-point method, then the path after encountering obstacles is judged in the forward path,and finally the optimal or sub-optimal planning path is derived in different environmental information. In the process of moving forward, the robot only needs to deal with the sensor data in the current state instead of the whole complex environmental information, which saves the storage space of the robot and improves the efficiency of the planning. Finally, the RobotBasic simulation platform is used to simulate the derived algorithm.The simulation results show that the algorithm is effective and reliable, and can adapt to the unknown local environment information.

mobile robot; local path planning; two-dot method; RobotBasic

2016-12-04

國(guó)家自然科學(xué)基金項(xiàng)目(61473179)；山東省自然科學(xué)基金項(xiàng)目(ZR2014FM007)

宋莉, 女, 18369905833@163.com;

李彩虹, 女, lich@sdut.edu.cn

1672-6197(2018)01-0010-05

TP242

A

(編輯：劉寶江)