李二超，王玉華

蘭州理工大學(xué) 電氣工程與信息工程學(xué)院，蘭州 730050

隨著人工智能的快速發(fā)展，移動(dòng)機(jī)器人被廣泛應(yīng)用于工業(yè)、軍事以及醫(yī)學(xué)等領(lǐng)域。而路徑規(guī)劃算法被廣泛地應(yīng)用在移動(dòng)機(jī)器人的導(dǎo)航中，機(jī)器人如何從所處的環(huán)境中搜索一條從初始位置到終點(diǎn)的安全、無(wú)碰撞的最優(yōu)路徑是路徑規(guī)劃的關(guān)鍵[1]。常用的算法有A*算法、蟻群算法（ant colony optimization，ACO）、Dijkstra算法、遺傳算法（genetic algorithm，GA）、快速隨機(jī)搜索樹(shù)法（rapidlyexploring random tree，RRT）、粒子群算法（particle swarm optimization，PSO）和人工勢(shì)場(chǎng)法（artificial potential field，APF）等[2]。其中，A*算法引入了啟發(fā)函數(shù)避免了路徑規(guī)劃問(wèn)題中產(chǎn)生大量的無(wú)效搜索路徑；ACO是一種啟發(fā)式隨機(jī)搜索算法，存在計(jì)算量大、收斂速度慢等問(wèn)題[3]；Dijkstra算法主要特點(diǎn)是由中心向外擴(kuò)散，直到擴(kuò)展到終點(diǎn)為止，但其遍歷計(jì)算節(jié)點(diǎn)較多，效率較低[4]GA得到的路徑符合現(xiàn)實(shí)中路徑選擇需求，但運(yùn)算量大且耗時(shí)長(zhǎng)，無(wú)法滿足實(shí)時(shí)性的需求[5]；RRT是一種基于采樣的概率算法，其特點(diǎn)主要是在空間中隨機(jī)生成新節(jié)點(diǎn)并在新節(jié)點(diǎn)上繼續(xù)向目標(biāo)點(diǎn)延伸，但存在路徑不平滑，搜索路徑一直變化的情況[6]；PSO同GA都是迭代算法，相較于GA，PSO收斂速度較快，但粒子同一化后，收斂速度較慢，且收斂精度不夠。

人工勢(shì)場(chǎng)法在路徑規(guī)劃中只需要根據(jù)當(dāng)前機(jī)器人的位姿以及障礙物來(lái)確定下一步的運(yùn)動(dòng)，算法本身具有計(jì)算量小、反應(yīng)速度快、生成路徑平滑等優(yōu)點(diǎn)被廣泛用于運(yùn)動(dòng)規(guī)劃系統(tǒng)的移動(dòng)機(jī)器人中。Rostami等[7]在傳統(tǒng)人工勢(shì)場(chǎng)函數(shù)中加入調(diào)節(jié)因子來(lái)繞過(guò)障礙物從而克服局部最小值和目標(biāo)不可達(dá)問(wèn)題；Yao等[8]從強(qiáng)化學(xué)習(xí)的視角來(lái)解決復(fù)雜環(huán)境問(wèn)題，將黑洞人工勢(shì)場(chǎng)和強(qiáng)化學(xué)習(xí)結(jié)合來(lái)解決局部穩(wěn)定點(diǎn)問(wèn)題，但在路徑規(guī)劃過(guò)程中會(huì)出現(xiàn)振蕩現(xiàn)象；趙明等[9]提出域來(lái)解決多目標(biāo)點(diǎn)任務(wù)的局部穩(wěn)定點(diǎn)問(wèn)題，在機(jī)器人陷入局部極小值時(shí)通過(guò)域場(chǎng)逃離局部極小點(diǎn)，但未考慮多復(fù)雜環(huán)境下機(jī)器人能否到達(dá)目標(biāo)點(diǎn)；何乃峰等[10]基于模擬退火算法，重新構(gòu)建斥力勢(shì)場(chǎng)函數(shù)模型，從而設(shè)計(jì)逃離局部極小值控制器進(jìn)行全局路徑，全局路徑平滑，但所規(guī)劃路徑并不是最優(yōu)路徑；徐小強(qiáng)等[11]針對(duì)陷阱區(qū)域和局部極小值問(wèn)題，提出提前預(yù)測(cè)人工勢(shì)場(chǎng)法，但對(duì)于較多障礙物的復(fù)雜環(huán)境而言，仍不能完整規(guī)劃路徑；王迪等[12]提出基于虛擬目標(biāo)點(diǎn)和環(huán)境判斷的改進(jìn)方法，實(shí)現(xiàn)移動(dòng)機(jī)器人的局部路徑規(guī)劃，但在遇到陷入型障礙物時(shí)規(guī)劃出“沿壁走”的路徑存在消耗程距的問(wèn)題；曹凱等[13]設(shè)計(jì)了一種由人工勢(shì)場(chǎng)引導(dǎo)RRT*進(jìn)行路徑規(guī)劃的方法來(lái)解決障礙物密集、通道狹窄的環(huán)境，但面對(duì)陷入型環(huán)境問(wèn)題時(shí)，偏向區(qū)域搜索會(huì)導(dǎo)致可行區(qū)域的收斂性，使路徑規(guī)劃采樣點(diǎn)不完整，無(wú)法完成路徑的完整性；高志偉等[14]提出將軌跡跟蹤和機(jī)器人動(dòng)力學(xué)控制模型相結(jié)合，實(shí)現(xiàn)全局路徑規(guī)劃點(diǎn)作為局部目標(biāo)點(diǎn)來(lái)得到能耗最優(yōu)軌跡，但這種規(guī)劃方式面對(duì)更復(fù)雜環(huán)境時(shí)全局路徑規(guī)劃點(diǎn)和局部路徑規(guī)劃點(diǎn)的判斷無(wú)疑加大了計(jì)算時(shí)間和耗能。

針對(duì)上述算法的不足，本文提出了一種簡(jiǎn)化障礙物預(yù)測(cè)碰撞人工勢(shì)場(chǎng)法（simplified obstacles and predict collision of artificial potential field method，SOPC-APF）。首先，加入預(yù)測(cè)碰撞思想，幫助機(jī)器人在未知環(huán)境中通過(guò)傳感器探測(cè)進(jìn)行提前預(yù)判，避免機(jī)器人陷入局部最優(yōu)；其次簡(jiǎn)化機(jī)器人受障礙物影響范圍內(nèi)的障礙物——即只考慮目標(biāo)點(diǎn)一側(cè)障礙物斥力的影響；最后通過(guò)虛擬目標(biāo)點(diǎn)的加入，采用改進(jìn)斥力因子影響范圍的斥力函數(shù)引導(dǎo)機(jī)器人快速、平穩(wěn)、無(wú)碰撞地到達(dá)目標(biāo)點(diǎn)。最后在仿真平臺(tái)對(duì)SOPC-APF進(jìn)行可行性的驗(yàn)證。

1 APF算法基本原理

人工勢(shì)場(chǎng)法作為路徑規(guī)劃算法中廣泛應(yīng)用的一種算法，最早由Khatib[15]所提出。在人工勢(shì)場(chǎng)算法中，將機(jī)器人的運(yùn)動(dòng)環(huán)境看作為一個(gè)虛擬力場(chǎng)，目標(biāo)點(diǎn)和障礙物在機(jī)器人運(yùn)動(dòng)過(guò)程中產(chǎn)生引力和斥力，通過(guò)兩者的合力作為機(jī)器人運(yùn)動(dòng)的加速度來(lái)控制機(jī)器人向目標(biāo)點(diǎn)移動(dòng)。

假設(shè)移動(dòng)機(jī)器人在二維有限空間內(nèi)運(yùn)動(dòng)受力分析如圖1所示，且機(jī)器人在二維有限空間中的位置表示為q=(x,y)T，目標(biāo)點(diǎn)位置表示為qtarg=(xT,yT)T，障礙物位置表示為qobs=(xo,yo)T。

圖1 傳統(tǒng)人工勢(shì)場(chǎng)法受力分析Fig.1 Force analysis of traditional APF

目標(biāo)點(diǎn)對(duì)機(jī)器人產(chǎn)生的引力場(chǎng)為Uatt(q)，障礙物對(duì)機(jī)器人產(chǎn)生的斥力場(chǎng)為Urep(q)，即機(jī)器人最終所受到的合力場(chǎng)為Utotal(q)，則可表達(dá)為：

式中，α為斥力勢(shì)場(chǎng)增益系數(shù)，‖q-qobs‖為機(jī)器人與障礙物之間的歐幾里德距離，ρo為障礙物斥力場(chǎng)對(duì)機(jī)器人產(chǎn)生斥力作用的影響距離，若‖q-qobs‖小于該值，受斥力作用，反之不受作用。

引力勢(shì)場(chǎng)函數(shù)的負(fù)梯度為機(jī)器人所受到的引力為：

當(dāng)機(jī)器人處于各障礙物影響距離內(nèi)時(shí)，機(jī)器人所受到的合斥力為，即機(jī)器人所受到的合力Ftotal為：

2 SOPC-APF算法

針對(duì)APF算法存在的缺陷，本文提出了SOPC-APF算法，該算法能夠在未知場(chǎng)景中根據(jù)傳感器探測(cè)出的環(huán)境信息，提高APF算法的全局搜索能力，減小某些局部震蕩產(chǎn)生的局部路徑消耗。SOPC-APF算法首先對(duì)機(jī)器人前方的障礙物進(jìn)行預(yù)測(cè)碰撞處理，從而簡(jiǎn)化影響范圍內(nèi)的受限障礙物（機(jī)器人當(dāng)前位置與目標(biāo)點(diǎn)之間存在的障礙物稱為受限障礙物）；在簡(jiǎn)化后的受限障礙物中通過(guò)旋轉(zhuǎn)角度來(lái)設(shè)置最優(yōu)的虛擬目標(biāo)點(diǎn)；最后結(jié)合虛擬目標(biāo)點(diǎn)的軌跡找尋真實(shí)目標(biāo)點(diǎn)，從而生成一條路徑平滑且連續(xù)的最優(yōu)路徑，從而使移動(dòng)機(jī)器人沿著該全局優(yōu)化路徑快速、平穩(wěn)、安全地到達(dá)目標(biāo)點(diǎn)。SOPC-APF算法的大體流程圖示意如圖2所示。

圖2 SOPC-APF流程圖Fig.2 Flow chart of SOPC-APF

針對(duì)移動(dòng)機(jī)器人在多障礙物影響范圍內(nèi)時(shí)受多個(gè)障礙物各個(gè)方向的影響出現(xiàn)合力為零或者機(jī)器人在某一范圍內(nèi)來(lái)回振蕩的情況，本文提出一種簡(jiǎn)化障礙物的思想，即移動(dòng)機(jī)器人只考慮在目標(biāo)點(diǎn)一側(cè)障礙物的斥力函數(shù)影響，如圖3所示。在簡(jiǎn)化過(guò)程中，令移動(dòng)機(jī)器人在當(dāng)前位置Xj處剔除(O1,O5,O6)，若保留障礙物(O1,O5)，其產(chǎn)生的斥力F1,5對(duì)逃離(O2,O3,O4)組成的受限障礙物的斥力有較大影響，導(dǎo)致與目標(biāo)點(diǎn)產(chǎn)生的引力發(fā)生不必要的振蕩，使后續(xù)的路徑規(guī)劃算法效率降低，所有在當(dāng)前位置Xj處，考慮(O1,O5)是完全沒(méi)有必要的。

圖3 簡(jiǎn)化障礙物圖Fig.3 Map of simplified obstacles

定義1機(jī)器人處于n個(gè)障礙物影響距離內(nèi)時(shí)，機(jī)器人只受m(m∈n)個(gè)障礙物斥力作用，則移動(dòng)機(jī)器人所即機(jī)器人所受到的合力受到的合斥力為Ftotal為：

2.1 碰撞預(yù)測(cè)

當(dāng)移動(dòng)機(jī)器人通過(guò)傳感器檢測(cè)出地圖存在陷阱區(qū)域時(shí)，如U型障礙物等復(fù)雜障礙物，傳統(tǒng)的人工勢(shì)場(chǎng)法由于算法自身的缺陷會(huì)出現(xiàn)震蕩或停止的現(xiàn)象，以致于不能完整地規(guī)劃出路徑。因此本節(jié)引入了預(yù)測(cè)距離和虛擬目標(biāo)點(diǎn)來(lái)引導(dǎo)移動(dòng)機(jī)器人繞開(kāi)陷阱區(qū)域，規(guī)劃完整路徑。

2.1.1 預(yù)測(cè)距離

針對(duì)陷阱區(qū)域的提前預(yù)知，如U型，陷入型障礙物，傳統(tǒng)算法會(huì)在陷阱區(qū)域內(nèi)發(fā)生震蕩或者停止，無(wú)法規(guī)劃出完整路徑。因此引出預(yù)測(cè)距離Rp，通過(guò)比較移動(dòng)機(jī)器人到簡(jiǎn)化障礙物之間的距離r0來(lái)判斷自己的下一步位置，若小于，通過(guò)偏轉(zhuǎn)角度來(lái)設(shè)置虛擬目標(biāo)點(diǎn)；否則按照初始方向向目標(biāo)點(diǎn)靠近。對(duì)于偏轉(zhuǎn)角度的實(shí)現(xiàn)以移動(dòng)機(jī)器人當(dāng)前位置為圓心，m個(gè)障礙物距離機(jī)器人最遠(yuǎn)的障礙物為半徑偏轉(zhuǎn)角度γ來(lái)繞開(kāi)m個(gè)障礙物。使每次旋轉(zhuǎn)3°直到簡(jiǎn)化后的障礙物在機(jī)器人安全范圍之外停止旋轉(zhuǎn)。則偏轉(zhuǎn)角度可表示為：

2.1.2 虛擬目標(biāo)點(diǎn)

通過(guò)偏轉(zhuǎn)角度γ，在m個(gè)障礙物中距離機(jī)器人最遠(yuǎn)的障礙物為半徑的另一端設(shè)置虛擬目標(biāo)點(diǎn)，在虛擬目標(biāo)點(diǎn)的引導(dǎo)下，使機(jī)器人向虛擬目標(biāo)點(diǎn)移動(dòng)，到達(dá)虛擬目標(biāo)點(diǎn)后主動(dòng)撤銷虛擬目標(biāo)點(diǎn)。重復(fù)以上步驟，直到機(jī)器人到達(dá)目標(biāo)點(diǎn)。找尋最優(yōu)虛擬目標(biāo)點(diǎn)的偽代碼如算法1。

算法1設(shè)置虛擬目標(biāo)點(diǎn)算法

2.2 改進(jìn)斥力函數(shù)

在二維有限空間中，改進(jìn)斥力場(chǎng)函數(shù)為：

改進(jìn)受力分析如圖4所示。其中，F(xiàn)R1是m個(gè)障礙物對(duì)機(jī)器人的斥力分量，F(xiàn)R2是目標(biāo)點(diǎn)對(duì)機(jī)器人的引力分量。

圖4 改進(jìn)勢(shì)場(chǎng)函數(shù)受力圖Fig.4 Force analysis of improved APF

算法2改進(jìn)斥力場(chǎng)函數(shù)算法

3 算法仿真及結(jié)果分析

為驗(yàn)證SOPC-APF算法的可行性和路徑規(guī)劃的效果，在實(shí)驗(yàn)平臺(tái)中進(jìn)行仿真研究，研究過(guò)程中，將移動(dòng)機(jī)器人視為一個(gè)質(zhì)點(diǎn)，分別對(duì)傳統(tǒng)人工勢(shì)場(chǎng)法和改進(jìn)人工勢(shì)場(chǎng)法在目標(biāo)不可達(dá)、局部極小值點(diǎn)以及復(fù)雜環(huán)境下地圖中做出對(duì)比，從而驗(yàn)證出SOPC-APF的優(yōu)越性。

3.1 基礎(chǔ)地圖算法對(duì)比分析

若將初始參數(shù)設(shè)置如下：機(jī)器人初始位置Xo=( 0,0)，目標(biāo)點(diǎn)位置Xg=(1 0,10)，引力系數(shù)ks=15，斥力1系數(shù)α=1.1，斥力2系數(shù)M=2，障礙物影響范圍ρo=2.5，預(yù)測(cè)碰撞距離Rp=2，步長(zhǎng)L=0.1，障礙物半徑R=0.5。圖5所示為移動(dòng)機(jī)器人-障礙物-目標(biāo)點(diǎn)目標(biāo)點(diǎn)不可達(dá)問(wèn)題。

圖5 目標(biāo)不可達(dá)問(wèn)題Fig.5 Problem of unreachable goals

圖5（a）為三點(diǎn)共線問(wèn)題，障礙物處于移動(dòng)機(jī)器人和目標(biāo)點(diǎn)兩點(diǎn)之間的連接線上，傳統(tǒng)算法在A點(diǎn)時(shí)斥力引力產(chǎn)生的合力在某一范圍震蕩，導(dǎo)致傳統(tǒng)算法無(wú)法逃出該范圍到達(dá)目標(biāo)點(diǎn)；SOPC-APF在B點(diǎn)碰撞預(yù)測(cè)后發(fā)現(xiàn)障礙物，及時(shí)偏轉(zhuǎn)角度脫離合力震蕩范圍，從而到達(dá)目標(biāo)點(diǎn)。圖5（b）中目標(biāo)點(diǎn)處于障礙物的影響范圍內(nèi)且距離障礙物較近，因此傳統(tǒng)算法會(huì)受到的斥力比較大，引力較小，會(huì)出現(xiàn)在目標(biāo)點(diǎn)附近抖動(dòng)現(xiàn)象；SOPC-APF則在B點(diǎn)處對(duì)障礙物的預(yù)判，從而通過(guò)偏轉(zhuǎn)角度設(shè)置虛擬目標(biāo)點(diǎn)，到達(dá)虛擬目標(biāo)點(diǎn)后重新規(guī)劃到達(dá)目標(biāo)點(diǎn)的路徑。

圖6 局部極小點(diǎn)問(wèn)題Fig.6 Problem of local minimum

設(shè)置參數(shù)如下：機(jī)器人初始位置Xo=(5 .5,2.5)；目標(biāo)點(diǎn)位置Xg=(1 0.5,10.5)。圖7為陷入陷阱區(qū)域問(wèn)題，機(jī)器人起點(diǎn)被障礙物包圍，傳統(tǒng)算法在到達(dá)A點(diǎn)處受到合斥力影響，在A點(diǎn)處產(chǎn)生振蕩，無(wú)法逃脫半包圍障礙物；SOPC-APF在起點(diǎn)處通過(guò)預(yù)測(cè)距離判斷出移動(dòng)機(jī)器人與目標(biāo)點(diǎn)之間已存在障礙物，則在全局障礙物中找出所有與該障礙物相連的障礙物，即受限障礙物，對(duì)受限障礙物及時(shí)旋轉(zhuǎn)角度設(shè)置虛擬目標(biāo)點(diǎn)B，從而逃出半包圍障礙物，規(guī)劃處達(dá)到目標(biāo)點(diǎn)路徑。

圖7 陷入問(wèn)題Fig.7 Problem of trapping

仿真數(shù)據(jù)結(jié)果如表1所示，驗(yàn)證了SOPC-APF對(duì)于目標(biāo)不可達(dá)和局部極小值點(diǎn)問(wèn)題路徑規(guī)劃的可行性和安全性。

表1 基礎(chǔ)地圖算法對(duì)比數(shù)據(jù)Table 1 Basic map algorithm compares data

3.2 復(fù)雜地圖算法對(duì)比分析

若將基礎(chǔ)參數(shù)設(shè)置如下：機(jī)器人初始位置Xo=( 0.5,0.5)，目標(biāo)點(diǎn)位置Xg=(1 9.5,19.5)，引力系數(shù)ks=15，斥力1系數(shù)α=1.1，障礙物影響范圍ρo=2.5，預(yù)測(cè)碰撞距離Rp=4，步長(zhǎng)L=0.2。在不同復(fù)雜障礙物下對(duì)本文算法、傳統(tǒng)人工勢(shì)場(chǎng)法、文獻(xiàn)[16]中改進(jìn)的蟻群算法（記作A-ACO）、文獻(xiàn)[7]改進(jìn)的人工勢(shì)場(chǎng)法（記作BAPF）、文獻(xiàn)[11]改進(jìn)的人工勢(shì)場(chǎng)法（記作C-APF）進(jìn)行多次比較，從而驗(yàn)證本文算法可以克服人工勢(shì)場(chǎng)在復(fù)雜障礙物不適用等問(wèn)題。傳統(tǒng)人工勢(shì)場(chǎng)法由于自身的缺陷，在復(fù)雜環(huán)境中會(huì)出現(xiàn)振蕩現(xiàn)象，無(wú)法成功完成路徑規(guī)劃；A-ACO算法屬于全局路徑規(guī)劃算法可以很好地避障從而完成路徑規(guī)劃，但相比較人工勢(shì)場(chǎng)法運(yùn)行時(shí)間較長(zhǎng)；B-APF在遇到障礙物時(shí)通過(guò)繞過(guò)受限障礙物來(lái)規(guī)劃路徑，這樣導(dǎo)致路徑過(guò)長(zhǎng)；C-APF引入預(yù)測(cè)距離和機(jī)器人安全距離，在障礙物受限之前作出反應(yīng)，通過(guò)設(shè)置虛擬目標(biāo)點(diǎn)規(guī)劃出來(lái)的路徑較短；SOPC-APF在C-APF基礎(chǔ)上對(duì)受限障礙物進(jìn)行簡(jiǎn)化處理，在斥力函數(shù)中加入目標(biāo)點(diǎn)和機(jī)器人的相對(duì)距離，通過(guò)障礙物影響范圍內(nèi)M取值情況的分類，從而使機(jī)器人的規(guī)劃路徑最短且平滑。

3.2.1 隨機(jī)離散障礙物地圖

若障礙物環(huán)境是離散分布的，幾種算法在同一仿真環(huán)境下運(yùn)行，設(shè)置M=0.5，R=0.65。仿真對(duì)比結(jié)果如圖8（a）所示。圖8（b）為SOPC-APF的引力和斥力能量圖，引力的大小趨于0，表示移動(dòng)機(jī)器人已到達(dá)目標(biāo)點(diǎn)，從圖8（b）中可知機(jī)器人抵達(dá)目標(biāo)點(diǎn)步數(shù)不超過(guò)150步，且機(jī)器人合力方向較平穩(wěn)，說(shuō)明生成路徑較光滑。從表2數(shù)據(jù)對(duì)比可以看出，SOPC-APF相較于A-ACO算法速度提高99.05%，轉(zhuǎn)折數(shù)量減少了1個(gè)；相較于B-APF算法路徑縮短了37.78%，算法速度提高了67.95%；相較于C-APF算法轉(zhuǎn)折數(shù)量減少了1個(gè)。

圖8 離散障礙物中SOPC-APF路徑規(guī)劃Fig.8 SOPC-APF path planning in discrete obstacles

3.2.2 L型、離散障礙物地圖

若障礙物環(huán)境中存在L型障礙物和離散障礙物，幾種算法在同一仿真環(huán)境下運(yùn)行，設(shè)置M=2，R=0.9。仿真對(duì)比結(jié)果如圖9（a）所示，傳統(tǒng)算法未達(dá)成功到達(dá)目標(biāo)點(diǎn)。圖9（b）為SOPC-APF的引力和斥力能量圖。從表3數(shù)據(jù)對(duì)比可以看出，SOPC-APF相較于A-ACO算法路徑縮短了5.48%，速度提高了99.05%，轉(zhuǎn)折數(shù)量減少了6個(gè)；相較于B-APF算法路徑縮短了3.50%，算法速度提高了30.0%；相較于C-APF算法路徑縮短了2.13%。

圖9 L型、離散障礙物中SOPC-APF路徑規(guī)劃Fig.9 SOPC-APF path planning in L-shaped and discrete obstacles

表3 L型、離散障礙物路徑規(guī)劃算法數(shù)據(jù)對(duì)比Table 3 Comparison of path planning algorithms for L-shaped and discrete obstacles

3.2.3 U型、離散障礙物地圖

若障礙物環(huán)境中存在U型障礙物和離散障礙物，幾種算法在同一仿真環(huán)境下運(yùn)行，設(shè)置M=1.5，R=1。仿真對(duì)比結(jié)果如圖10（a）所示，（b）為SOPC-APF的引力和斥力能量圖。從表4數(shù)據(jù)對(duì)比可以看出，SOPC-APF相較于A-ACO算法路徑縮短了2.05%，速度提高了99.06%，轉(zhuǎn)折數(shù)量減少了10個(gè)；B-APF算法在該地圖中由于陷入障礙物太深，無(wú)法通過(guò)B-APF算法繞出，因此路徑規(guī)劃失敗；相較于C-APF算法路徑縮短了2.72%。

圖10 U型、離散障礙物中SOPC-APF路徑規(guī)劃Fig.10 SOPC-APF path planning in U-shaped and discrete obstacles

3.2.4 復(fù)雜障礙物地圖

（1）若存在密集障礙物等復(fù)雜環(huán)境下，幾種算法在同一仿真環(huán)境下運(yùn)行，設(shè)置M=1.5，R=0.5，Rp=6。仿真對(duì)比結(jié)果如圖11（a），圖11（b）為SOPC-APF的引力和斥力能量圖。

圖11 復(fù)雜障礙物中SOPC-APF路徑規(guī)劃Fig.11 SOPC-APF path planning in complex obstacles

從表5數(shù)據(jù)對(duì)比可以看出，SOPC-APF相較于AACO算法路徑縮短了4.83%，速度提高了99.06%，轉(zhuǎn)折數(shù)量減少了7個(gè)；B-APF算法路徑縮短了56.15%，速度提高了94.61%；C-APF算法由于該地圖過(guò)于復(fù)雜無(wú)法完整的規(guī)劃出路徑。

表5 復(fù)雜路徑規(guī)劃算法數(shù)據(jù)對(duì)比Table 5 Comparison of complex obstacles path planning algorithm data

（2）若移動(dòng)機(jī)器人起點(diǎn)處于半包圍障礙物等復(fù)雜環(huán)境下，幾種算法在同一仿真環(huán)境下運(yùn)行，設(shè)置M=1.5，R=0.5，Rp=4。仿真對(duì)比結(jié)果如圖12（a）所示，圖12（b）為對(duì)應(yīng)的的引力和斥力能量圖。

圖12 復(fù)雜障礙物中SOPC-APF路徑規(guī)劃Fig.12 SOPC-APF path planning in complex obstacles

從表6數(shù)據(jù)對(duì)比可以看出，SOPC-APF相較于AACO算法路徑縮短了1.05%，速度提高了98.52%，轉(zhuǎn)折數(shù)量減少了8個(gè)；B-APF和C-APF算法由于陷入障礙物內(nèi)部，無(wú)法完整地規(guī)劃出路徑。

表6 復(fù)雜路徑規(guī)劃算法數(shù)據(jù)對(duì)比Table 6 Comparison of complex obstacles pathplanning algorithm data

圖13為上述環(huán)境下移動(dòng)機(jī)器人的合力方向的對(duì)比分析，而移動(dòng)機(jī)器人面對(duì)目標(biāo)點(diǎn)的角度問(wèn)題也可以看作移動(dòng)機(jī)器人相對(duì)路徑平穩(wěn)性的評(píng)估，從對(duì)比圖中不難看出：在相同仿真環(huán)境下，傳統(tǒng)算法面對(duì)目標(biāo)不可達(dá)、局部極小值以及陷入問(wèn)題時(shí)出現(xiàn)的震蕩問(wèn)題，使移動(dòng)機(jī)器人路徑波動(dòng)大，穩(wěn)定性不理想；SOPC-APF在設(shè)定迭代次數(shù)內(nèi)可到達(dá)目標(biāo)點(diǎn)，且運(yùn)行路徑較平滑，無(wú)震蕩現(xiàn)象出現(xiàn)。

圖13 合力方向?qū)Ρ确治鯢ig13 Comparative analysis of total force direction

通過(guò)以上仿真實(shí)驗(yàn)結(jié)果得到如下結(jié)論：當(dāng)環(huán)境復(fù)雜，存在陷阱區(qū)域、初始位置陷入環(huán)境以及密集障礙物時(shí)，SOPC算法可以安全地逃離或避開(kāi)陷阱區(qū)域和穿越密集環(huán)境，且軌跡平滑；當(dāng)?shù)貓D中存在較為簡(jiǎn)單的目標(biāo)不可達(dá)和局部極小點(diǎn)問(wèn)題的障礙物環(huán)境時(shí)SOPC算法也可以規(guī)劃出相比人工勢(shì)場(chǎng)法更加簡(jiǎn)潔的軌跡，且無(wú)震蕩軌跡發(fā)生。

4 結(jié)論

針對(duì)傳統(tǒng)人工勢(shì)場(chǎng)法目標(biāo)不可達(dá)，易陷入局部最優(yōu)，且不適用于復(fù)雜環(huán)境下等問(wèn)題導(dǎo)致移動(dòng)機(jī)器人無(wú)法規(guī)劃出完整路徑，因此本文提出一種改進(jìn)——基于簡(jiǎn)化障礙物預(yù)測(cè)碰撞人工勢(shì)場(chǎng)法。首先，在機(jī)器人移動(dòng)前對(duì)前方路徑進(jìn)行預(yù)測(cè)判斷；在預(yù)測(cè)到存在障礙物后，簡(jiǎn)化受限障礙物，即機(jī)器人只受影響范圍內(nèi)目標(biāo)一側(cè)障礙物的斥力影響；隨后在簡(jiǎn)化后的障礙物附近設(shè)置合理的虛擬目標(biāo)點(diǎn)，經(jīng)改進(jìn)的斥力函數(shù)引導(dǎo)機(jī)器人在復(fù)雜環(huán)境中快速生成一條路徑平滑、平穩(wěn)、無(wú)碰撞的路徑。

仿真實(shí)驗(yàn)表明，SOPC-APF有效解決了人工勢(shì)場(chǎng)法不適用于多障礙物復(fù)雜環(huán)境的問(wèn)題，以及傳統(tǒng)算法容易陷入陷阱區(qū)域和局部極小點(diǎn)問(wèn)題。下一步將考慮對(duì)于動(dòng)態(tài)環(huán)境和突發(fā)情況下，機(jī)器人如何進(jìn)行實(shí)時(shí)避障，為路徑規(guī)劃的真實(shí)性和適用性做近一步的研究。