陳天德,黃炎焱,李 琛

(南京理工大學自動化學院,江蘇南京 210094)

1 引言

集群航路規(guī)劃作為一個重要的研究課題,一直為集群任務(wù)規(guī)劃的研究重點[1-4].

目前,集群航路規(guī)劃的方法主要有領(lǐng)航跟隨法[5]、虛擬結(jié)構(gòu)法[6-7]、人工勢場法[8-9]以及基于行為法[10]等.

由于無人裝備的迅速發(fā)展,有人無人混合集群,甚至純無人集群將是未來集群行動的重要形式[11].因此,集群航路規(guī)劃的結(jié)果將從有人集群的參考路徑轉(zhuǎn)變成無人集群的行動依據(jù),這就對集群航路規(guī)劃提出了更高的要求.

單從航路規(guī)劃來說,人工勢場法數(shù)學模型簡單[12]、解算快速[13]、易于執(zhí)行且實時性良好[14],具有類人的感知特征[15].此外,所得路徑平滑[16],具有易于跟隨性[17],較為適用于對無人裝備的航路規(guī)劃[2,18-19].

集群行動必然是有構(gòu)型的行動,因此集群航路規(guī)劃是基于構(gòu)型對集群中各單元進行航路規(guī)劃.文獻[20]根據(jù)期望構(gòu)型中智能體之間的距離設(shè)計勢場函數(shù),可以保證智能體之間的距離最終達到期望值,而整個集群構(gòu)型的運動姿態(tài)則卻不能單靠勢函數(shù)法來保證,可見人工勢場法在隊形組織能力方面略顯不足.此外,當集群構(gòu)型改變時,勢函數(shù)參數(shù)需重新配置,從而使集群成員間保持所需距離,以此形成構(gòu)型,如文獻[21]將勢能場布置成“溝槽”的形狀,機器人運動到“溝槽”中形成隊形,而不同隊形需要不同形式的“溝槽”;再如文獻[22]基于人工勢場法研究無人機編隊的導(dǎo)航算法,設(shè)計出新的分叉勢場法,通過改變參數(shù)實現(xiàn)無人機編隊的隊形變換.因此,人工勢場法在應(yīng)對構(gòu)型變化時使用上不夠靈活[7,23].

文獻[24]將構(gòu)型控制與航路規(guī)劃分成兩個獨立模塊,構(gòu)型控制采用人工勢場的思想,通過控制集群成員間的距離來實現(xiàn);航路規(guī)劃采用周期進化算法獲得距離代價最小的全局路徑,再用類似于人工勢場法的速度矢量法進行局部修正.然而,該構(gòu)型控制方法,雖能形成并保持構(gòu)型,但不能很好地確定集群構(gòu)型的運動姿態(tài);該航路規(guī)劃算法的仿真結(jié)果表明,相比另一種以由周期性快速搜索遺傳算法與人工勢場法組合而成的聯(lián)合算法(periodic fast search genetic algorithm and artificial potential field,PFSGA-APF)[24],該算法在路徑距離上效果更優(yōu),但路徑平滑度卻相對差一些.

文獻[7]通過設(shè)置勢能場,使編隊參考點到達目標參考點,而集群在勢能場作用下,跟隨編隊參考點到達期望位置.在集群運動過程中,通過勢能場控制集群成員間以及成員與編隊參考點間的距離來實現(xiàn)構(gòu)型的形成與保持.由于該構(gòu)型控制中加入了編隊參考點對構(gòu)型的進一步約束,集群構(gòu)型的運動姿態(tài)問題得到了解決.然而,當構(gòu)型改變時,各成員所涉及的勢函數(shù)需要進行相應(yīng)的參數(shù)調(diào)整,因此該算法在使用上不夠靈活,且編隊參考點的個數(shù)影響對構(gòu)型的約束效果,而個數(shù)確定的依據(jù)與過程并未給出.

文獻[6]通過將人工勢場法與虛擬結(jié)構(gòu)相結(jié)合,以編隊參考點為中心形成期望的虛擬結(jié)構(gòu),在勢能場的作用下,集群各成員以對應(yīng)的虛擬結(jié)構(gòu)點為運動目標,在避碰避障的前提下,漸進形成隊形.由于該方法所用的是傳統(tǒng)的人工勢場法,因此,存在著人工勢場法固有的不足[16,25],且該方法僅針對編隊隊形的形成與保持,未涉及編隊的航路規(guī)劃.

人工勢場法的固有不足為狹窄通道無法識別、狹窄通道中震蕩、障礙在目標附近導(dǎo)致目標不可及(goals nonreachable with obstacles nearby,GNRON)[26]以及陷阱問題[16,25].根據(jù)文獻[25]的研究結(jié)果,由于斥力勢函數(shù)僅考慮與障礙物的距離因素,而導(dǎo)致在障礙物的影響距離范圍內(nèi)采取無差別避碰措施,即無謂避碰現(xiàn)象[27],進而產(chǎn)生人工勢場法的固有不足.雖然固有不足產(chǎn)生的原因嚴格上講并非唯一是無謂避碰,但若能克服無謂避碰,不僅可以在一定程度上減少固有不足問題的出現(xiàn),還能提高人工勢場法在路徑距離上的優(yōu)化效果.

針對無謂避碰問題,文獻[28]通過對碰撞角和避碰角進行比較,來預(yù)測障礙物的碰撞危險,存在碰撞危險的障礙物才對機器人產(chǎn)生斥力作用,該方法可以較好的避免無謂避碰,但未考慮碰撞的緊迫程度,存在一定避碰不及的情況.文獻[29]提出改進的斥力勢函數(shù),僅當機器人關(guān)于障礙物的相對速度在機器人指向障礙物的方向上的分量大于0且該分量減為0后機器人將移動到障礙物影響范圍內(nèi)時,障礙物才對機器人產(chǎn)生斥力作用,然而該方法能否較好的避免無謂避碰還取決于障礙物影響距離的設(shè)置.根據(jù)文獻[30]的研究結(jié)果,障礙物影響距離作為勢函數(shù)中一個重要參數(shù),其值設(shè)置過大會導(dǎo)致規(guī)劃效率低,即加劇無謂避碰的出現(xiàn),過小則會導(dǎo)致避碰不及.目前大多數(shù)對勢函數(shù)法的應(yīng)用以及各方面的改進,該參數(shù)基本都被設(shè)為一個固定值.然而,在實際應(yīng)用中,由于規(guī)劃環(huán)境的動態(tài)性,很難得到合適的影響距離值.

本文以文獻[6]的方法為基礎(chǔ),提出基于二重勢函數(shù)法的集群航路規(guī)劃算法,主要工作有:1) 借助虛擬結(jié)構(gòu)的思想,將集群航路規(guī)劃分為兩個階段,依次為針對集群目標的航路規(guī)劃與針對集群構(gòu)型的航路規(guī)劃,兩個階段的航路規(guī)劃均使用人工勢場法;2)針對人工勢場法存在的無謂避碰現(xiàn)象,構(gòu)建碰撞危險度評估模型以及障礙物影響距離確定模型,以此將障礙物影響距離從傳統(tǒng)勢函數(shù)中的固定值改為實時根據(jù)障礙物碰撞危險度所確定的動態(tài)值;3)針對人工勢場法存在的GNRON問題,根據(jù)造成該問題的障礙物特點,設(shè)計時間碰撞危險度評估模型;4)針對人工勢場法在動態(tài)規(guī)劃環(huán)境下存在的陷阱問題,根據(jù)該問題的特征,總結(jié)陷阱問題發(fā)生的條件,并采用虛擬障礙物的思想來阻止該問題的發(fā)生.

相比目前已有的采用人工勢場法進行集群航路規(guī)劃的方法,本文所提方法仍采用傳統(tǒng)人工勢場法的數(shù)學模型,僅從算法執(zhí)行機制入手,克服人工勢場法在集群航路規(guī)劃上的不足,并使傳統(tǒng)人工勢場法的固有不足得到改善,極大程度的保留了人工勢場法相比其他航路規(guī)劃算法的最大優(yōu)勢,即算法數(shù)學描述簡潔且易于執(zhí)行.

2 航路規(guī)劃算法

2.1 勢函數(shù)模型

為了方便描述,本文將航路規(guī)劃的對象統(tǒng)稱為機器人,而不針對任何特定的機器人類型以及具體的應(yīng)用場景.

人工勢場法是將機器人在環(huán)境中的運動視為一種在虛擬人工受力場中的運動[26].障礙物產(chǎn)生的斥力與目標產(chǎn)生的引力共同決定機器人的運動.在動態(tài)環(huán)境下,即運動的目標和障礙物,其引力勢能函數(shù)[29]如下:

式中:qrobot和qgoal分別為機器人和目標的位置坐標;vrobot和vglobal分別為機器人和目標的速度;ρ(qrobot,qgoal)為機器人與目標之間的距離;‖vgoal?vrobot‖為機器人與目標之間相對速度的大小;ξq為受距離影響的引力場增益系數(shù);ξv為受速度影響的引力場增益系數(shù);m和n為正常數(shù).當目標靜止時,ξv為0,該引力勢能函數(shù)就退化成傳統(tǒng)的引力勢能函數(shù)[26].對應(yīng)的引力函數(shù)Fatt(qrobot,vrobot)為引力勢能函數(shù)的負梯度.傳統(tǒng)斥力勢能函數(shù)[26]如下:

式中:η是斥力場增益系數(shù);ρ(qrobot,qobs)是機器人到障礙物的最短距離;qobs是障礙物上的一個點,機器人到該點的距離是機器人到該障礙物的最短距離;ρ0是障礙物有效影響距離.對應(yīng)的斥力函數(shù)Frep(qrobot)為斥力勢能函數(shù)的負梯度.

2.2 障礙物影響距離

2.2.1 障礙物模型化

本文中,兩個障礙物之間的距離默認可以安全通過一個機器人,若不能通過,則把這兩個障礙物當成一個障礙物來處理.為了便于計算機器人與障礙物的最短距離,本文將障礙物用一個圓形區(qū)域包圍,以此作為航路規(guī)劃的障礙區(qū).設(shè)qobstacle為障礙區(qū)中心位置坐標,Robstacle為障礙區(qū)半徑,則qobs為

由于本文對障礙物采用圓形建模,機器人與障礙物的距離相比實際距離很可能偏小,故通過下文將介紹的碰撞危險度模型所解算出的障礙物碰撞危險度相比實際可能偏大,這在一定程度上會削弱避免無謂避碰行為的效果.因此,為了更好地避免無謂避碰行為的發(fā)生,在實際應(yīng)用中,障礙物的建模應(yīng)盡可能與障礙物本身相符.本文為了計算方便,對機器人同樣采用圓形建模,由此式(1)中qrobot和qgoal則分別具體代表機器人中心的位置坐標和目標中心的位置坐標.

2.2.2 碰撞危險度模型

碰撞危險度分為空間碰撞危險度和時間碰撞危險度[31-32].空間碰撞危險度反映了碰撞的可能性大小,時間碰撞危險度是在有碰撞可能的情況下反映碰撞的緊迫程度.

1) 空間碰撞危險度.

空間碰撞危險度udT定義[32-33]為

式中:Dcpa為最近會遇距離;d1為最低安全會遇距離,d2為空間碰撞危險度零邊界,通常d2≈2d1[32-33].

針對機器人與圓形障礙區(qū)的碰撞危險度解算,定義d1:指機器人邊界與障礙區(qū)中心的最低安全會遇距離;定義d2:指空間碰撞危險度開始等于0時機器人邊界與障礙區(qū)中心的會遇距離;定義dsafe:指機器人邊界與障礙區(qū)邊界的最低安全距離;定義Dcpa:指機器人與障礙區(qū)會遇時,機器人邊界與障礙區(qū)中心最近的距離.因此最低安全會遇距離與空間碰撞危險度零邊界為

式中:r為機器人的半徑,其大小是機器人外形大小的一種抽象體現(xiàn);Tcpa為最小會遇時間[32-33].

2) 時間碰撞危險度.

在進行航路規(guī)劃中,機器人與目標的位置示意如圖1[34]所示,圖中各區(qū)域是相對于機器人當前位置而定的,即各區(qū)域的實際位置隨著航路規(guī)劃的不斷進行而發(fā)生改變.

圖1 GNRON問題位置分析Fig.1 Position analysis of the GNRON problem

由文獻[34]的分析可知:1)對機器人有較高碰撞危險的障礙物即區(qū)域1內(nèi)的障礙物和相對目標運動的障礙物,反而不容易造成GNRON問題,本研究稱這類障礙物為第1類障礙物;2)容易造成GNRON 問題的障礙物即位于區(qū)域2和3內(nèi)且相對于目標靜止的障礙物,卻對機器人沒有較高的碰撞危險,本研究稱這類障礙物為第2類障礙物.因此,時間碰撞危險度utT定義為

即針對第1類障礙物采用常規(guī)的時間碰撞危險度評估模型utT1[32-33],針對第2類障礙物采用特殊的時間碰撞危險度評估模型utT2.式中:vOG為障礙物相對于目標的速度;ρ(qrobot,qobstacle)為機器人中心與障礙區(qū)中心的距離;vOR為障礙物相對于機器人的航速;D1為最晚轉(zhuǎn)向距離[33-34];D2為d2所對應(yīng)的最晚轉(zhuǎn)向距離,即通過將d1替換成d2而求得的“D1”,具體計算細節(jié)見文獻[33].式(4)(8)以及式(9)中3.03的設(shè)置依據(jù)見文獻[32].相比utT1,utT2的不同主要為:1)若障礙物位于區(qū)域3即ρ(qrobot,qobs)>La,則該障礙物的時間碰撞危險度直接為0;2)位于區(qū)域2的障礙物,只要機器人與該障礙物的最短會遇距離不小于最低安全會遇距離即|Dcpa|≥d1,則該障礙物的時間碰撞危險度也為0.

3) 碰撞危險度總模型.

碰撞危險度總模型uT為

式中RT為機器人邊界與障礙區(qū)中心的距離.

2.2.3 障礙物影響距離的確定

為了確保當機器人與障礙物存在碰撞危險時,機器人在該障礙物影響范圍內(nèi),從而通過勢函數(shù)法采取相應(yīng)的避碰措施,障礙物的ρ0為

式中λ為大于1的數(shù).

2.3 陷阱問題

2.3.1 陷阱問題的分析

陷阱問題是局部極小值問題在動態(tài)環(huán)境下的表現(xiàn)形式[34],主要包括某區(qū)域內(nèi)持續(xù)徘徊甚至靜止不動、抖動性遠離目標以及抖動性接近目標.根據(jù)文獻[34]的研究結(jié)果,當存在障礙物在機器人的運動方向上且機器人繼續(xù)保持該運動方向時:1) 若障礙物靜止,則機器人很可能在某區(qū)域內(nèi)持續(xù)徘徊;2)若障礙物與機器人相向而行,則機器人很可能抖動性遠離目標;3)若障礙物與機器人同向而行且機器人存在追越障礙物的可能,則機器人很可能抖動性接近目標.

由于勢函數(shù)法是目標導(dǎo)向性的[25,34],故在判斷算法執(zhí)行是否可能發(fā)生陷阱問題時,僅考慮目標引力對機器人運動的影響.陷阱問題的發(fā)生需要機器人保持一個運動方向不變,在不考慮障礙物影響的情況下,當目標為靜止時,只要目標在機器人的運動方向上,則機器人就可以保持運動方向不變,當目標為運動時,僅當目標在機器人的運動方向上且機器人對目標的Dcpa=0,機器人才能保持運動方向不變.陷阱問題的實質(zhì)是機器人對障礙物采取的必要避碰行為,但由于勢函數(shù)法自身的問題,機器人的避碰并不是有效的,而機器人對障礙物進行避碰則表明該障礙物對機器人的碰撞危險度uT>0.

綜上所述,若同時滿足:1)θG=0?且機器人對目標的Dcpa=0,這里θG為目標相對于機器人運動方向,順時針計量的方位角度,即船舶領(lǐng)域中的舷角[32-33];2)障礙物對機器人的Dcpa=0且Tcpa>0;3)障礙物為靜止的或障礙物的航向與機器人的航向在同一條直線上;4)障礙物對機器人的碰撞危險度uT>0,則航路規(guī)劃很有可能出現(xiàn)陷阱問題.

2.3.2 虛擬障礙物

由陷阱問題發(fā)生的條件不難知道:qgoal,qobs和qrobot三點共線是陷阱問題發(fā)生的必要條件.因此,只要使得這三點不共線,陷阱問題就可以得到解決.

本文引入虛擬障礙物法,當航路規(guī)劃同時滿足陷阱問題發(fā)生的4個條件時,在機器人附近設(shè)置一個質(zhì)點型虛擬障礙物,它對機器人施加一個附加斥力,使得在接下來的航路規(guī)劃中qgoal,qobs和qrobot三點不再共線.為了防止所放置的虛擬障礙物增加陷阱問題發(fā)生的可能,附加斥力垂直于三點的連線[16,28]且指向障礙物較稀疏的方向[14,16].為了有效的破壞陷阱問題發(fā)生的必要條件,本文將虛擬障礙物與機器人中心的距離設(shè)為機器人的半徑r,虛擬障礙物的ρ0為一個默認值,即不通過碰撞危險度來確定.

3 二重勢能場

3.1 問題描述

在給定集群構(gòu)型的情況下,對集群進行航路規(guī)劃,需完成的4點功能按優(yōu)先級由高到底排序為:1)集群單元的避障以及單元間的避碰;2)集群到達目標位置;3)構(gòu)型的形成;4)構(gòu)型的保持.功能3)和4)分開列出的原因為集群行動時會根據(jù)實時態(tài)勢以及任務(wù)需要對構(gòu)型進行調(diào)整,若此時集群已經(jīng)形成原規(guī)劃的構(gòu)型并保持,則停止保持該構(gòu)型,而以新規(guī)劃的構(gòu)型為準并形成.

為實現(xiàn)上述功能,本研究采用如圖2的航路規(guī)劃方式.

圖2 集群航路規(guī)劃示意圖Fig.2 Schematic diagram of cluster route planning

根據(jù)給定的集群構(gòu)型選擇合適的構(gòu)型參考點Oref,并將Oref放置在合適的初始位置.在集群構(gòu)型不變的情況下,各個構(gòu)型點Ci相對于Oref的位置始終不變.通過對Oref進行航路規(guī)劃,得到Oref從初始位置到目標的航路,并實時根據(jù)Oref位置的變化確定Ci的位置.集群各單元Ui以對應(yīng)的Ci為動態(tài)目標,進行航路規(guī)劃.由于本文的研究重點是集群的航路規(guī)劃,故集群構(gòu)型的確定、構(gòu)型參考點的選擇及其初始位置的確定不在本研究的范圍之內(nèi).

根據(jù)本研究中碰撞危險度模型以及人工勢場法的特點,本文以正北方向為Y軸,正東方向為X軸,建立大地坐標系,坐標系的原點根據(jù)規(guī)劃空間的具體情況來選定,大地坐標系中所有角度按正北方向順時針計量.根據(jù)本研究中集群航路規(guī)劃的特點,本文以構(gòu)型參考點的位置為坐標原點,其運動方向為Y軸指向,X軸指向為Y軸指向順時針旋轉(zhuǎn)90?所得,建立構(gòu)型參考點的隨體坐標系,在該坐標系下,所有角度按Y軸指向順時針計量.

3.2 第一重勢能場

第一重勢能場的作用:通過對構(gòu)型參考點建立第一重勢能場,規(guī)劃出構(gòu)型參考點到目標的航路,以此根據(jù)構(gòu)型參考點在航路中的實時位置,解算出各個構(gòu)型點的實時位置.

第一重勢能場所面對的目標是根據(jù)集群的實際目標所選取的目標點,該目標點的選取既要考慮集群的任務(wù)類型,又要考慮構(gòu)型參考點相對于集群構(gòu)型的位置情況.例如,集群的任務(wù)是到達一個指定區(qū)域,則構(gòu)型參考點到達所選取的目標點時,集群的各構(gòu)型點均要在該指定區(qū)域內(nèi).再如,集群的任務(wù)是跟蹤某一目標,則構(gòu)型參考點到達所選取的目標點時,集群各單元在對應(yīng)的構(gòu)型點上可以保證整個集群對目標可觀測且與目標保持一定距離.

在不考慮目標點具體選取細節(jié)的情況下,構(gòu)型參考點的任務(wù)簡單地說就是到達目標點.目標點的狀態(tài)可分為靜止和運動,而構(gòu)型參考點對目標點所采用的到達方式分為“軟著陸”和“硬著陸”.對于靜止的目標點,構(gòu)型參考點采用“硬著陸”方式,即ξv=0;對于運動的目標點,構(gòu)型參考點通常采用“軟著陸”方式,即ξv0.

第一重勢能場為第二重勢能場提供各構(gòu)型點在大地坐標系下實時的位置以及速度.設(shè)構(gòu)型參考點在大地坐標系下的位置坐標為(xrefyref)T,速度為(vxrefvyref)T;設(shè)構(gòu)型點在構(gòu)型參考點隨體坐標系下的位置坐標為(xRCyRC)T;設(shè)構(gòu)型點在大地坐標系下的位置坐標為(xCyC)T,速度為(vxCvyC)T,則有

3.3 第二重勢能場

第二重勢能場的作用:將第一重勢能場所提供的構(gòu)型點作為集群中對應(yīng)單元的動態(tài)目標點,通過對集群各單元建立第二重勢能場,以此規(guī)劃出集群各單元到對應(yīng)構(gòu)型點的跟隨路徑,從而完成整個集群的航路規(guī)劃.

從集群航路規(guī)劃需完成第3.1節(jié)所提的4個功能上看,第一重勢能場主要是完成功能2),第二重勢能場主要是完成功能1),3)和4),而集群單元對構(gòu)型點的跟隨直接完成功能3)和4),即構(gòu)型形成與保持,因此集群單元對構(gòu)型點采用“軟著陸”到達方式.

第二重勢能場需要完成集群中各單元間的避碰,對某一個單元進行航路規(guī)劃時,需要考慮集群中其他單元的速度以及運動方向,而其他單元的速度以及運動方向同樣也是通過航路規(guī)劃所得,即存在集群單元間運動狀態(tài)相互影響的問題,這就使得第二重勢能場不易于實現(xiàn)集群單元間避碰的功能.對此,本文采用避碰優(yōu)先級的處理方式.首先,對集群中各單元進行避碰優(yōu)先級的評定并排序;然后,按照避碰優(yōu)先級由高到低的順序?qū)簡卧M行航路規(guī)劃.對集群單元進行航路規(guī)劃時,僅需考慮對避碰優(yōu)先級高的單元進行避碰.而避碰優(yōu)先級的評定與具體任務(wù)類型、集群單元屬性以及集群態(tài)勢變化等因素有關(guān),故不在本研究范圍內(nèi).

由于在第一重勢能場中,航路規(guī)劃的對象僅為構(gòu)型參考點,且規(guī)劃過程并不考慮各構(gòu)型點的位置.因此,根據(jù)構(gòu)型參考點實時位置所確定的構(gòu)型點有可能會在障礙區(qū)內(nèi),這樣的構(gòu)型點顯然是不可安全跟隨的,若集群中對應(yīng)的單元以該構(gòu)型點為目標,則存在碰撞的危險.對于這種情況,本研究采取的解決方法是:當集群單元所對應(yīng)的構(gòu)型點在障礙區(qū)內(nèi),則該單元以避碰優(yōu)先級比其高一級的集群單元為目標進行航路規(guī)劃,且速度大小與高一級單元保持一致,對于避碰優(yōu)先級最高的單元,其高一級單元為構(gòu)型參考點,此外,對應(yīng)的構(gòu)型點作為第一重勢能場中的障礙物,參與接下來的航路規(guī)劃,其uT設(shè)置為1,直至對應(yīng)構(gòu)型點離開障礙區(qū).

由于第二重勢能場所要完成的功能中優(yōu)先級最高的是功能1),因此只有構(gòu)型點不在障礙區(qū)內(nèi),且可以安全跟隨,則對應(yīng)的集群單元以該構(gòu)型點為目標,否則以高一級單元為目標.判斷構(gòu)型點是否可安全跟隨的方法為:若所有實際障礙物對構(gòu)型點的合斥力為0,則該構(gòu)型點可安全跟隨.

4 集群航路規(guī)劃總模型

將航路規(guī)劃算法與二重勢能場思想相結(jié)合,形成集群航路規(guī)劃總模型,其模型的總流程如圖3 所示(圖3見下一頁).

首先判斷整個集群是否達到目標位置,即在構(gòu)型參考點到達目標點的情況下,集群各單元到達對應(yīng)構(gòu)型點位置,若到達,且對于動態(tài)目標,在不需要繼續(xù)跟隨目標的情況下,結(jié)束集群航路規(guī)劃,否則繼續(xù)進行接下來的規(guī)劃過程.

圖3中的子流程為航路規(guī)劃算法的流程,如圖4所示.

由于航路規(guī)劃的目的是讓機器人在不與障礙物發(fā)生碰撞的情況下盡快到達目標,即航路規(guī)劃具有目標導(dǎo)向性[25,34],因此在進行碰撞危險度解算,以及判斷是否滿足陷阱問題發(fā)生條件的過程中,以目標引力的方向為機器人速度方向,且機器人速度大小不變.基于碰撞危險度確定障礙物的影響距離后,判斷是否滿足陷阱問題發(fā)生的條件,若滿足則進行虛擬障礙物的放置并進行航路規(guī)劃,否則直接進行航路規(guī)劃.航路規(guī)劃后,若放置了虛擬障礙物則撤除所放置的虛擬障礙物.

圖3中所涉及的初始化設(shè)置如圖5所示.在整個集群航路規(guī)劃的過程中,集群單元航路規(guī)劃的情況,如集群單元與對應(yīng)構(gòu)型點的距離,可能會影響集群單元的避碰優(yōu)先級.因此,在集群航路規(guī)劃過程中,集群單元的避碰優(yōu)先級可能要實時進行調(diào)整.此外,在集群航路規(guī)劃過程中,也可能進行集群構(gòu)型的調(diào)整.但由于本研究不涉及這些,故在總流程中未標出.

圖3 算法總流程Fig.3 Total algorithm flow

圖5 初始化設(shè)置Fig.5 Initial setting

圖4 子流程Fig.4 Sub process

5 想定驗證

引力勢函數(shù)模型中參數(shù)設(shè)定為m=n=2,ξq=0.1,ξv=0.05;斥力勢函數(shù)模型中參數(shù)設(shè)定為η=0.3,ρ0默認為2 m(傳統(tǒng)方法和虛擬障礙物使用);障礙物影響距離確定模型中參數(shù)設(shè)定為λ=2.為便于仿真,不考慮實際動態(tài)障礙物的半徑.

5.1 單機航路規(guī)劃

為了驗證本文所提出的勢函數(shù)法在陷阱問題、無謂避碰問題以及GNRON問題上的處理能力,設(shè)置單機航路規(guī)劃仿真實驗.假設(shè)機器人以恒定速度移動,虛擬力僅改變其運動方向.機器人參數(shù)設(shè)定為:半徑r=0.15 m,機器人速度vrobot=0.2 m/s.

5.1.1 陷阱問題

以抖動性遠離目標為例,在一個開放空間中,機器人出發(fā)位置為(0,0)m;目標靜止,其位置為(10,10)m;設(shè)置障礙物:位置為(5.5,5.5)m,速度為0.15m/s,航向為225?.

分別采用傳統(tǒng)的人工勢場法和本文改進的人工勢場法進行單機航路規(guī)劃.見圖6,當采用傳統(tǒng)人工勢場法時,由于陷阱問題的發(fā)生,機器人抖動性遠離目標.

圖6 抖動性遠離目標Fig.6 Jittering away from the goal

如圖7所示,當采用本文改進的人工勢場法時,由于滿足陷阱問題發(fā)生的條件,因此機器人在虛擬障礙物的斥力作用下不再與障礙物和目標三點共線,從而避免了陷阱問題的發(fā)生.

圖7 避免抖動性遠離目標Fig.7 Avoid the problem of jittering away from the goal

5.1.2 無謂避碰問題

參照文獻[34],設(shè)置動態(tài)仿真環(huán)境.動態(tài)目標初始位置為(10,10)m,其速度為0.112 m/s,航向為116.56?.初始化6個動態(tài)障礙物:Obs 1位置為(5,0)m,速度為0.12 m/s,航向為0?;Obs 2位置為(9.4,4.8)m,速度為0.29 m/s,航向為270?;Obs 3位置為(19,10)m,速度為0.082 m/s,航向為218?;Obs 4 位置為(10,0)m,速度為0.3 m/s,航向為315?;Obs 5位置為(15,12)m,速度為0.085 m/s,航向為180?;Obs 6位置為(0,3)m,速度為0.13 m/s,航向為45?,加速度為0.002 m/s2,加速度方向為90?.放置8 個靜態(tài)障礙物:Obs 7位置為(6.5,10.5)m,半徑為0.7 m;Obs 8 位置為(17.8,4.3)m,半徑為1.05 m;Obs 9 位置為(20,7)m,半徑為1.05 m;Obs 10 位置為(11,5.35)m,半徑為0.5 m;Obs 11位置為(10.5,7.5)m,半徑為0.5 m;Obs 12位置為(3,3.6)m,半徑為0.3 m;Obs 13 位置為(7.3,3.95)m,半徑為0.35 m;Obs 14位置為(10.5,10.5)m,半徑為0.4 m.機器人出發(fā)位置為(0,0)m.

分別采用傳統(tǒng)的人工勢場法和本文改進的人工勢場法進行單機航路規(guī)劃,如圖8和圖9所示.

圖8 無謂避碰Fig.8 Unnecessary collision avoidance

圖9 避免無謂避碰Fig.9 Avoid the problem of unnecessary collision avoidance

由圖8(a)和圖9(a)可知,相比傳統(tǒng)方法,本文方法使機器人在經(jīng)過Obs 12附近和與Obs 1會遇的過程中很好的避免了無謂避碰行為,確保機器人能夠抓住時機直接從Obs 1前橫越過去.由圖8(b)和圖9(b)可知,相比傳統(tǒng)方法,由于本文方法能夠很好的避免無謂避碰行為,故機器人在通過Obs 10與Obs 11之間和Obs 8與Obs 9之間時,路徑相對平滑,不存在路徑點抖動問題.相比傳統(tǒng)勢函數(shù)法用170 s到達目標,本文所提方法僅用130 s.

5.1.3 GNRON問題

在與第5.1.2節(jié)相同的仿真環(huán)境下,將動態(tài)目標改為靜態(tài)目標,分別采用傳統(tǒng)的人工勢場法和本文改進的人工勢場法進行單機航路規(guī)劃.如圖10所示,當采用傳統(tǒng)方法時,由于Obs 14在目標附近,導(dǎo)致GNRON問題的發(fā)生,使得機器人無法到達目標.如圖11所示,當采用本文方法時,由于時間碰撞危險度評估模型對障礙物進行有區(qū)別的評估,從而屏蔽了Obs 14對機器人的影響,避免了GNRON問題的發(fā)生,機器人順利到達目標.

圖10 GNRON問題Fig.10 The problem of GNRON

圖11 克服GNRON問題Fig.11 Overcome the problem of GNRON

5.2 集群航路規(guī)劃

對于集群來說,靜態(tài)目標可以看作是將動態(tài)目標的速度設(shè)為0,因此集群以指定構(gòu)型追上并跟隨動態(tài)目標更具有一般性.現(xiàn)設(shè)置想定:5個運動單元組成一個集群,集群以集群外的一個單元為領(lǐng)航者,形成指定構(gòu)型,當構(gòu)型形成時,在領(lǐng)航者的隨體坐標系中,單元U1位置為(?0.5,?0.866)m,單元U2位置為(0.5,?0.866)m,單元U3位置為(1,?1.7321)m,單元U4位置為(0,?1.7321)m,單元U5位置為(?1,?1.7321)m.集群各單元與領(lǐng)航者半徑r均為0.1 m,速度最大為0.2 m/s,領(lǐng)航者初始化數(shù)據(jù)與第5.1.2節(jié)的動態(tài)目標相同.

當采用文獻[6]中的方法時,由于不考慮集群單元的具體類型,因此不涉及文獻[6]中的目標跟蹤算法,則集群單元可視為文獻[6]中的虛擬結(jié)構(gòu)質(zhì)點,以領(lǐng)航者為編隊參考點,并設(shè)置虛擬結(jié)構(gòu),通過傳統(tǒng)人工勢場法形成隊形.集群單元間用于避碰的ρ0為0.4 m.

當采用本文所提的集群航路規(guī)劃法時,則將領(lǐng)航者視為目標,想定轉(zhuǎn)變?yōu)?集群以沒有領(lǐng)航者的非完整構(gòu)型,追上并跟隨領(lǐng)航者.集群單元避碰優(yōu)先級的關(guān)系設(shè)為:U1>U2>U3>U4>U5,構(gòu)型參考點的速度恒為0.175 m/s.

5.2.1 無障礙物環(huán)境

在無障礙環(huán)境下,初始化5 個集群單元的位置:U1為(0,0.5)m,U2為(0.5,0)m,U3為(1,0)m,U4為(0,0)m,U5為(0,1)m.現(xiàn)分別采用文獻[6]的方法與本文的方法,當采用本文的方法時,構(gòu)型參考點的位置初始化為(2,2)m,仿真結(jié)果如圖12和圖13所示.

如圖12所示,當采用文獻[6]方法時,由于集群單元各自為政,單元路徑發(fā)生非必要交錯,加劇碰撞發(fā)生的可能性,從而出現(xiàn)由避碰所產(chǎn)生的明顯避碰路徑;且由于集群單元間無避碰優(yōu)先級之分,因此采取避碰措施的單元并非一個;此外,由于避碰僅考慮距離因素,避碰路徑較為抖動.

圖12 無障礙環(huán)境下采用文獻[6]方法Fig.12 The method of reference [6]used in no-obstacle environment

相比之下,如圖13所示,當采用本文方法時,由于第二重勢能場的作用,使集群單元的行動從一開始就受到了約束與引導(dǎo),避免了單元路徑非必要交錯,各單元各行其道,在83 s時形成無領(lǐng)航者的非完整構(gòu)型,并以該構(gòu)型繼續(xù)前進,直到與領(lǐng)航者形成完整構(gòu)型,相比文獻[6]方法用時144 s,本文方法用時131 s.

圖13 無障礙環(huán)境下采用本文方法Fig.13 The proposed method used in no-obstacle environment

5.2.2 多障礙物環(huán)境

現(xiàn)設(shè)置障礙物初始化數(shù)據(jù)與第5.1.2 節(jié)相同,在該多障礙物環(huán)境下,初始化5 個集群單元的位置:U1為(?1.26,?0.76)m,U2為(?1.76,?0.76)m,U3為(?2.26,?0.76)m,U4為(?2.76,?0.76)m,U5為(?3.26,?0.76)m.現(xiàn)分別采用文獻[6]的方法與本文的方法,當采用本文的方法時,構(gòu)型參考點的位置初始化為(0,0)m,仿真結(jié)果如圖14和圖15所示.

圖14 多障礙環(huán)境下采用文獻[6]方法Fig.14 The method of reference [6]used in multi-obstacle environment

圖15 多障礙環(huán)境下采用本文方法Fig.15 The proposed method used in multi-obstacle environment

當采用本文方法時,集群各單元從行動開始就試圖組成無領(lǐng)航者的非完整構(gòu)型.如圖15(a),集群單元開始就散開試圖組成非完整構(gòu)型,雖然由于障礙物的威脅,集群不得不暫時收攏,但集群通過Obs 12 與Obs 13后就重新散開進行嘗試;再如圖15(b),集群通過Obs 10與Obs 11后,又重新散開試圖組成非完整構(gòu)型.由圖15(c)和圖15(d)可知,由于障礙物較多,集群未能在與領(lǐng)航者組成完整構(gòu)型前形成非完整構(gòu)型,但對比圖14,正因為這種貫穿始終的“試圖”,使得集群單元以一個整體的姿態(tài)行動,再加上依據(jù)障礙物碰撞危險度進行避障,以及集群單元間依據(jù)避碰優(yōu)先級有差別避讓,很好地提高了行動效率,相比文獻[6]方法用230 s形成構(gòu)型,本文方法用了172 s.

6 結(jié)論

針對人工勢場法在集群航路規(guī)劃中構(gòu)型組織能力的不足,提出基于二重勢函數(shù)法的集群航路規(guī)劃法,通過第一重勢能場形成集群到目標的可行路徑,第二重勢能場形成集群構(gòu)型,從而實現(xiàn)集群的航路規(guī)劃.此外,針對避障過程中人工勢場法僅考慮距離因素而導(dǎo)致無謂避碰現(xiàn)象的發(fā)生,引入碰撞危險度評估模型,根據(jù)障礙物的碰撞危險度來確定障礙物影響距離,以此避免無謂避碰現(xiàn)象的發(fā)生;針對人工勢場法在航路規(guī)劃中存在的GNRON問題,通過引入兩種時間碰撞危險度評估模型,實現(xiàn)對障礙物有區(qū)別的評估,從而克服GNORN問題;針對人工勢場法存在的陷阱問題,通過引入虛擬障礙物予以避免.MATLAB仿真結(jié)果證明了所提的改進人工勢場法以及以該人工勢場法所構(gòu)成集群航路規(guī)劃法的可行性.

基于二重勢函數(shù)法的集群航路規(guī)劃法為采用人工勢場法實現(xiàn)集群航路規(guī)劃提供了一種思路,其中一些本文未過多涉及的點,如集群構(gòu)型的確定、構(gòu)型參考點的確定和初始位置的選擇、集群單元避碰優(yōu)先級的設(shè)定以及實時調(diào)整等,同樣也舉足輕重,需要進一步去研究.