孫鵬耀， 黃炎焱， 潘堯

(南京理工大學(xué) 自動化學(xué)院， 江蘇 南京 210094)

0 引言

隨著移動機器人技術(shù)的不斷發(fā)展，機器人自主執(zhí)行作戰(zhàn)任務(wù)已成為軍事領(lǐng)域的一個重要方向。在許多機器人作戰(zhàn)任務(wù)中，比如排雷、偵察、運送物資等，要想移動機器人順利完成作戰(zhàn)任務(wù)，準(zhǔn)確可靠的路徑規(guī)劃是必不可少的前提條件之一。目前，針對移動機器人路徑規(guī)劃常用方法有：勢場法(PFA)[1-2]、隨機樹搜索法[3]、粒子群優(yōu)化算法[4]、遺傳算法[5]、蟻群算法[6]、煙花算法[7]等，其中PFA因其模型簡單，計算快速與實時性強等優(yōu)點被廣泛研究。但戰(zhàn)場障礙環(huán)境往往具有復(fù)雜性與部分未知性的特點，PFA在復(fù)雜障礙環(huán)境下的應(yīng)用仍存在許多問題。現(xiàn)有研究主要將這些問題分為3類：局部極小陷阱、路徑振蕩、路徑不被識別。

局部極小陷阱是PFA最關(guān)鍵的問題，目前解決局部極小陷阱的方法主要可以分為消除法和逃離法兩大類[8-9]：

1)消除法[10-12]通過構(gòu)造特殊勢函數(shù)來消除局部極小點，從而保證目標(biāo)點為全局唯一極小點。但消除法需要基于全局環(huán)境信息充分已知前提下進(jìn)行預(yù)處理，無法滿足對實時性要求高的情況。

2)逃離法的思想是在機器人陷入局部極小陷阱后通過某種方法引導(dǎo)機器人逃離陷阱。常見的逃離法有搜索算法、多PFA、虛擬障礙物法、沿墻行走法等。搜索算法的思想是當(dāng)陷入局部極小陷阱后，搜索并移動到勢場值比當(dāng)前局部極小值更小的位置點，然后機器人繼續(xù)按勢場梯度下降法移動，目前常用的搜索算法如隨機搜索、模擬退火在缺少啟發(fā)信息時存在搜索效率的不確定性，可能導(dǎo)致搜索效率很低[13]。多PFA[14]的思想是設(shè)計多個全局最小點相同但局部極小點不同的勢場函數(shù)，當(dāng)機器人陷入局部極小陷阱時通過切換勢場函數(shù)保證規(guī)劃繼續(xù)進(jìn)行。多PFA需要對全局環(huán)境信息提前準(zhǔn)確了解并構(gòu)造勢場函數(shù)，且構(gòu)造多個滿足要求的勢函數(shù)難度大，容易使機器人重新陷入已逃離的局部極小陷阱，增加了規(guī)劃的復(fù)雜度與不確定性。虛擬障礙物法[15]的思想是當(dāng)機器人陷入局部極小陷阱時，在局部極小點附近加入虛擬障礙物，通過改變原有勢場分布引導(dǎo)或阻止機器人逃離或進(jìn)入局部極小陷阱，但在復(fù)雜障礙環(huán)境下要確定虛擬障礙物的位置較為復(fù)雜，且容易導(dǎo)致振蕩問題的產(chǎn)生。沿墻行走法[16]的思想是讓機器人沿著障礙區(qū)的邊緣逃離陷阱，簡單實用，但現(xiàn)有的一些沿邊行走方法過于簡單，不能適應(yīng)過于復(fù)雜的障礙環(huán)境。

振蕩問題的產(chǎn)生與勢場參數(shù)、機器人步長、障礙物信息等參數(shù)密切相關(guān)，目前解決振蕩問題的方法主要有兩類：一類是參數(shù)優(yōu)化，其思想是通過優(yōu)化算法不斷優(yōu)化各個參數(shù)來避免振蕩產(chǎn)生，現(xiàn)常用的優(yōu)化算法有量子粒子群優(yōu)化算法[17]、遺傳算法等，但是這類方法在面對復(fù)雜的障礙環(huán)境時，需要不斷地優(yōu)化調(diào)整參數(shù)，計算量較大，不夠簡潔高效；另一類是后消除方法[18-19]，該類方法的思想是在完成路徑規(guī)劃后找出路徑中的振蕩，并消除替換為平直路徑，該類方法不能邊走邊處理，不具有實時性。

路徑不被識別問題也與各參數(shù)息息相關(guān)，目前解決該問題的方法也主要分為兩類：一類是參數(shù)優(yōu)化[20]；另一類是基于行為的方法[21]，其思想是通過切換不同的機器人行為模式避免該問題，該類方法實用性強，但在復(fù)雜環(huán)境下，設(shè)計行為切換條件是難點。

綜上所述，PFA中存在的系列問題已受到研究者的廣泛關(guān)注[22-23]，但是依然存在一些不足：1)在復(fù)雜環(huán)境下，現(xiàn)有方法不能兼顧實時性與高效性；2)現(xiàn)有研究將振蕩分為障礙物前方和側(cè)方兩類，但并未給出準(zhǔn)確的分類標(biāo)準(zhǔn)，是一個研究盲點，模糊的分類邊界導(dǎo)致在應(yīng)用具體方法時難以設(shè)計準(zhǔn)確判據(jù)，并且現(xiàn)有方法都集中于前方振蕩，而忽視了側(cè)方振蕩，不具有一般性；3)現(xiàn)有研究在解決局部極小陷阱、路徑振蕩、路徑不被識別3類問題時，多采用逐個解決的思想，并未系統(tǒng)全面地將這些問題分析、總結(jié)與統(tǒng)一，并且所設(shè)計的障礙區(qū)形狀與環(huán)境往往比較簡單，在多種問題可能同時存在的復(fù)雜障礙環(huán)境下，這些方法不能夠簡潔高效地系統(tǒng)解決這些問題。

針對上述情況，考慮到戰(zhàn)場環(huán)境的復(fù)雜性，為系統(tǒng)全面地解決PFA在復(fù)雜障礙環(huán)境下存在的一系列問題，保障機器人實現(xiàn)實時路徑規(guī)劃，順利完成作戰(zhàn)任務(wù)，本文將PFA存在的問題細(xì)分為相近障礙區(qū)之間路徑不被識別(問題1)、多障礙區(qū)導(dǎo)致的振蕩(問題2)、多障礙區(qū)導(dǎo)致的局部極小陷阱(問題3)、單個障礙區(qū)導(dǎo)致的局部極小陷阱(問題4)、單障礙區(qū)導(dǎo)致的振蕩(問題5)，從問題的必要條件入手，提出一種結(jié)合多行為策略與可變影響范圍的PFA(CSVR-PFA)：

1)首先，分析問題1～問題3的必要條件，提出可變影響范圍算法解決該必要條件，規(guī)避問題1～問題3的出現(xiàn)；

2)其次，提出有效步進(jìn)與無效步進(jìn)的概念，結(jié)合內(nèi)部振蕩與邊緣振蕩，提出一種新的振蕩分類方法，并給出準(zhǔn)確分類標(biāo)準(zhǔn)；

3)再次，分析了問題4、問題5的必要條件，把對問題4、問題5的規(guī)避轉(zhuǎn)化為對無效步進(jìn)與有效振蕩的規(guī)避，提出多行為行動策略，并給出準(zhǔn)確的起止條件。機器人根據(jù)不同時刻面對的不同狀態(tài)，通過各行為起止條件之間的銜接，進(jìn)行行為的切換，以有效規(guī)避問題4、問題5；

4)最后，結(jié)合上述研究形成CSVR-PFA，通過大量復(fù)雜障礙環(huán)境下的仿真實驗與對比研究，驗證了本文所提方法的有效性、可行性與優(yōu)越性。

1 PFA基本原理

PFA的基本原理是將整個物理環(huán)境轉(zhuǎn)化為一個勢場模型，目標(biāo)點產(chǎn)生引力勢場吸引機器人移動，障礙產(chǎn)生斥力勢場阻止機器人向目標(biāo)移動，機器人在引力和斥力共同作用下從高勢場位置沿勢場的負(fù)梯度方向逐步向低勢場位置運動。文獻(xiàn)[1]把機器人與目標(biāo)點的距離引入到斥力函數(shù)中，確保目標(biāo)點是全局勢場中的勢能最低點，解決了目標(biāo)點與障礙過近從而不可達(dá)的問題。根據(jù)機器人的物理半徑對障礙做膨脹計算，機器人可看作質(zhì)點?，F(xiàn)給出勢函數(shù)與力函數(shù)如(1)式～(4)式所示。

引力勢能函數(shù)：

(1)

斥力勢能函數(shù)：

(2)

式中：qr、qg分別表示機器人、目標(biāo)點所在位置；qobs表示障礙區(qū)中與機器人當(dāng)前位置距離最近點的坐標(biāo)；ξ為引力增益系數(shù)；η為斥力增益系數(shù)；ρ(qr,qg)=‖qg-qr‖表示機器人所在位置與目標(biāo)點的直線距離；ρ(qr,qobs)表示機器人與障礙區(qū)最小直線距離；ρO為障礙區(qū)的影響范圍半徑。

引力函數(shù)為

(3)

斥力函數(shù)為

(4)

式中：

(5)

(6)

nOG表示由機器人指向目標(biāo)點的單位向量；nOR表示由障礙區(qū)指向機器人的單位向量；nRG表示由機器人指向目標(biāo)點的單位向量。

本文以此勢函數(shù)與力函數(shù)為基礎(chǔ)，對PFA進(jìn)行研究。

2 多障礙區(qū)問題的分析與規(guī)避

2.1 問題分析

2.1.1 問題1 相近障礙區(qū)之間路徑不被識別

圖1 問題1示意圖Fig.1 Problem 1

如圖1和圖2所示，機器人本該從某兩個障礙區(qū)之間的狹窄路徑通過，但由于路徑入口處被障礙區(qū)群影響范圍完全覆蓋且入口處任意一點的斥力勢場都很大，機器人在入口處所受合力指向障礙區(qū)群外側(cè)區(qū)域，此時機器人無法識別和進(jìn)入路徑，只能從障礙區(qū)群的外側(cè)繞行。

圖2 勢場值示意圖Fig.2 Schematic diagram of potential field value

綜上分析，狹窄路徑入口處影響范圍重疊或相切以及不合適的斥力系數(shù)是產(chǎn)生問題1的必要條件。

2.1.2 問題2 多障礙區(qū)導(dǎo)致的振蕩

該問題有兩種場景：1)振蕩出現(xiàn)在障礙區(qū)之間的狹窄通道中；2)振蕩出現(xiàn)在障礙區(qū)四周。

場景1又細(xì)分為兩種：

圖3 問題2示意圖Fig.3 Problem 2

1)如圖3所示，兩個障礙區(qū)影響范圍重疊且合斥力較小，合力指向障礙區(qū)之間的路徑，此時機器人可以穿過障礙區(qū)之間的通道。在通道區(qū)域，合勢場呈現(xiàn)U形的峽谷形狀，如圖4所示。機器人在穿過該峽谷通道時，理論上應(yīng)該沿著谷底的最低勢場線(藍(lán)色路徑)移動，但是由于計算機計算的離散性和不恰當(dāng)?shù)某饬ο禂?shù)，機器人很難嚴(yán)格處在這條理論線上，而是交替出現(xiàn)在這條線的兩側(cè)(紅色路徑點)，從而形成振蕩，如圖5所示。

圖5 理論與實際路徑對比圖Fig.5 Comparison of theoretical and practical paths

2)兩個障礙區(qū)的影響范圍不重疊但斥力極大，且存在某一區(qū)域，這兩個影響范圍的最小距離極小(小于一個步長)。當(dāng)機器人進(jìn)入該部分區(qū)域時，由于斥力極大且此時兩障礙區(qū)影響范圍極其接近，機器人剛進(jìn)入1號障礙區(qū)影響范圍就被排斥出去，緊接著進(jìn)入2號障礙區(qū)的影響范圍。接下來，機器人再被2號障礙區(qū)排斥出影響范圍，緊接進(jìn)入1號障礙區(qū)影響范圍。以此往復(fù)，形成振蕩，具體如圖6所示。若最小距離大于1個步長，機器人從1號影響范圍被排斥出去后不能立即進(jìn)入2號障礙區(qū)影響范圍，此時如果形成振蕩，并不是兩個障礙區(qū)連續(xù)排斥造成，只與某一個障礙區(qū)有關(guān)，不符合問題2的定義。

圖6 影響范圍過近導(dǎo)致的振蕩問題示意圖Fig.6 Oscillation caused by close influence range

場景2：機器人在經(jīng)過多障礙區(qū)影響范圍的重疊區(qū)域時，與問題1類似，由于計算的離散性和不恰當(dāng)?shù)某饬ο禂?shù)，機器人不能完全貼合理想路線行進(jìn)，合力出現(xiàn)突變從而導(dǎo)致振蕩。

綜上分析，障礙區(qū)影響范圍重疊或過于接近(小于一個步長)和不合適的斥力系數(shù)是產(chǎn)生問題2的必要條件。

2.1.3 問題3 多障礙區(qū)導(dǎo)致的局部極小陷阱

所謂局部極小陷阱，原理上，即在合勢場中存在某處，其合勢場強度為0，機器人在該處受到的合力為0，從而無法進(jìn)行下一步方向判斷，停滯不前，路徑中斷。局部極小陷阱是由PFA本身的設(shè)計引起的，是不可避免的。在實際應(yīng)用中，由于計算機計算的離散性，當(dāng)機器人陷入局部極小陷阱時，其表現(xiàn)為在圍繞著該局部極小陷阱的某一個較小區(qū)域內(nèi)徘徊，無法逃出該區(qū)域。具體表現(xiàn)如圖7所示。

圖7 問題3示意圖Fig.7 Problem 3

顯然，障礙區(qū)的影響范圍存在重疊區(qū)域，并且在重疊區(qū)域存在合勢場強度為0的局部極小點是產(chǎn)生問題3的必要條件。

2.2 可變影響范圍算法

通過上述分析可以發(fā)現(xiàn)，形成問題1～問題3的共同必要條件是各影響范圍之間存在最小距離小于1個步長的部分(最小距離小于0即為重疊)。因此，只要各個障礙區(qū)的影響范圍之間的最小距離大于1個步長，消除產(chǎn)生問題1～3的必要條件，即可提前避免問題1～問題3的產(chǎn)生。對此，本文提出一種障礙區(qū)可變影響范圍算法，對已知障礙區(qū)位置信息進(jìn)行處理，處理后各障礙區(qū)影響范圍之間最小距離不小于1個步長。算法步驟如下：

2) 計算每兩個障礙區(qū)間的最小距離，獲得對稱的距離矩陣Dn×n，矩陣元素dij=min [ρ(S(i),S(j))]；

3)計算可變率矩陣Cn×n，矩陣元素cij=(ρO(i)+ρO(j))÷(dij-Ls)。

上述算法步驟中，S(n)表示已知構(gòu)成障礙區(qū)Obs(n)的點集合，Ls表示機器人行進(jìn)步長，ε為一個微小的正值?？勺兟蔯反映了某一對障礙區(qū)影響范圍調(diào)整的緊急程度，c≥1時表示影響范圍間最小距離小于1個步長，需要進(jìn)行調(diào)整，c越大表示越緊急，c<1表示不需要調(diào)整。考慮到一次調(diào)整后會導(dǎo)致多組障礙區(qū)的c發(fā)生變化，故每次只調(diào)整最緊急的一對障礙區(qū)，然后重新計算可變率矩陣，再選擇下一對要調(diào)整的障礙區(qū)，直到任意一對障礙區(qū)c<1，從而達(dá)到算法目的。

在實際應(yīng)用中，障礙區(qū)的位置信息可能不完全已知，需要在機器人行進(jìn)過程中進(jìn)行探測。當(dāng)探測到新的障礙區(qū)或障礙區(qū)新的信息時，將探測到的內(nèi)容加入步驟1中的初始信息，重新計算即可。

3 單障礙區(qū)問題的分析與規(guī)避

3.1 問題分析

3.1.1 問題4 單障礙區(qū)局部極小陷阱

在分析單障礙區(qū)局部極小陷阱前，需要引入有效步進(jìn)與無效步進(jìn)的概念，現(xiàn)給出有效與無效步進(jìn)定義為：有步進(jìn)qr(s)qr(s+1),若ρ(qr(s+1),qg)<ρ(qr(s),qg)，則稱步進(jìn)qr(s)qr(s+1)為有效步進(jìn)；否則為無效步進(jìn)。

單障礙區(qū)局部極小陷阱的表現(xiàn)為障礙區(qū)擋在機器人與目標(biāo)點之間，機器人陷入局部極小點周圍的極小區(qū)域內(nèi)無法逃離，具體表現(xiàn)形式為機器人圍繞著局部極小點運動，這一過程中包含無效步進(jìn)。無效步進(jìn)為構(gòu)成單障礙區(qū)局部極小陷阱表現(xiàn)形式的必要條件。

3.1.2 問題5 單障礙區(qū)振蕩

從PFA的原理看，振蕩問題本不該出現(xiàn)，但由于實際計算的離散性，機器人每次行進(jìn)固定步長，所以往往不能嚴(yán)格準(zhǔn)確地沿著理論上的路徑移動，通常會或多或少地超出理論路徑點，導(dǎo)致合力的突變。此時機器人實際路徑點會交替出現(xiàn)在理論路線的兩側(cè)，形成振蕩。

在PFA應(yīng)用中，初始參數(shù)如引力系數(shù)、斥力系數(shù)、障礙區(qū)影響范圍、步長等設(shè)置的不同，振蕩出現(xiàn)的位置與形式也不相同。振蕩問題產(chǎn)生原因的多樣性導(dǎo)致了振蕩問題表現(xiàn)形式的復(fù)雜性，要準(zhǔn)確有效地處理振蕩問題，需要對振蕩問題進(jìn)行準(zhǔn)確分類與預(yù)判。現(xiàn)有的振蕩分類方法為障礙區(qū)前方與側(cè)方之分，這種方法的分類條件模糊，應(yīng)用時難以準(zhǔn)確選擇判據(jù)。本文提出新的單障礙區(qū)振蕩分類方法，將其分為影響范圍邊緣振蕩與影響范圍內(nèi)部振蕩兩類，具體分析如下：

圖8 邊緣振蕩示意圖Fig.8 Edge oscillation

1) 障礙區(qū)影響范圍邊緣振蕩。當(dāng)影響范圍邊緣斥力強度非常大時，如圖8所示：①機器人剛剛進(jìn)入障礙區(qū)范圍后，所受斥力極大，機器人立刻被排斥出障礙區(qū)影響范圍；②機器人剛被排斥出來，此時只受到引力作用，再次進(jìn)入障礙區(qū)影響范圍邊緣；③上述兩個步驟交替出現(xiàn)，形成振蕩。

設(shè)機器人當(dāng)前位于路徑點qr(s)，在計算出下一路徑點qr(s+1)后，預(yù)判是否會產(chǎn)生邊緣振蕩的方法如圖9所示。

圖9中，ρ(qr(s+1)qg,qobs(n))表示路徑點qr(s+1)指向目標(biāo)點qg的向量與障礙區(qū)Obs(n)的最近距離，ρO(n)表示障礙區(qū)Obs(n)的影響范圍。ρ(qr(s+1)qg,qobs(n))-ρO(n)<0表示向量qr(s+1)qg與障礙區(qū)Obs(n)有交點。

圖10 內(nèi)部振蕩示意圖Fig.10 Internal oscillation

2)障礙區(qū)影響范圍內(nèi)部振蕩。當(dāng)影響范圍邊緣斥力強度較小時，如圖10所示，機器人可以進(jìn)入影響范圍內(nèi)部：①機器人所受斥力較小，合力指向影響范圍內(nèi)部，機器人朝向障礙區(qū)行進(jìn)，越來越靠近障礙區(qū)，斥力的增長率急速增大，引力的變化平緩；②由于計算離散性，機器人所受斥力急劇增大，引起合力突變，排斥機器人遠(yuǎn)離障礙區(qū)；③機器人與障礙區(qū)距離增大后斥力急劇下降，再次引起合力突變，吸引機器人接近障礙區(qū)；④步驟2與步驟3行為交替出現(xiàn)，從而形成振蕩。

設(shè)機器人現(xiàn)位置為qr(s)，在計算出下一路徑點qr(s+1)后，預(yù)判是否會產(chǎn)生內(nèi)部振蕩的方法如圖11所示。

圖11 內(nèi)部振蕩預(yù)判方法流程圖Fig.11 Flow chart of internal oscillation prediction

圖11中：〈m,n〉表示向量n旋轉(zhuǎn)到m方向的角度，逆時針為正，順時針為負(fù)；λ為一個小于0的較小數(shù)值，一是用來判定是否產(chǎn)生振蕩，二是用來消除一些微弱干擾和誤差。

至此，振蕩可準(zhǔn)確分為內(nèi)部振蕩與邊緣振蕩。接下來，結(jié)合有效、無效步進(jìn)，引入有效振蕩與無效振蕩的概念

由有效步進(jìn)構(gòu)成的振蕩稱為有效振蕩，由無效步進(jìn)與有效步進(jìn)交替構(gòu)成或者全都由無效步進(jìn)構(gòu)成的振蕩稱為無效振蕩。PFA中的振蕩可以分割成這兩種振蕩的組合。

無效振蕩發(fā)生時，由于無效步進(jìn)使機器人遠(yuǎn)離目標(biāo)點，會導(dǎo)致機器人的行進(jìn)效率較低，從而降低整個PFA的收斂速度甚至導(dǎo)致無法收斂，同時機器人處于無效振蕩時，每次步進(jìn)的轉(zhuǎn)向角很大，容易造成姿態(tài)不穩(wěn)，嚴(yán)重危害機器人的行進(jìn)安全。有效振蕩對算法收斂速度與機器人行進(jìn)安全的影響相對較小。

結(jié)合內(nèi)部、邊緣振蕩與有效、無效振蕩兩種分類方式，最終將振蕩細(xì)分為4種：內(nèi)部- 有效振蕩、內(nèi)部- 無效振蕩、邊緣- 有效振蕩、邊緣- 無效振蕩。

對于內(nèi)部- 無效振蕩和邊緣- 無效振蕩而言，由其定義可知無效步進(jìn)是構(gòu)成這兩種振蕩的必要條件，這與構(gòu)成問題4單障礙區(qū)局部極小陷阱表現(xiàn)形式的必要條件相同。故內(nèi)部- 無效振蕩、邊緣- 無效振蕩以及問題4的解決可以轉(zhuǎn)化為對無效步進(jìn)的處理。而對于內(nèi)部- 有效振蕩和邊緣- 有效振蕩，只要處理好有效振蕩即可。為此，本文設(shè)計多行為行動策略。

3.2 多行為行動策略

本文的解決思路是：由于無效步進(jìn)或有效振蕩是構(gòu)成問題4、問題5的必要條件，當(dāng)判定下一步進(jìn)出現(xiàn)無效步進(jìn)或有效振蕩時，機器人跳出傳統(tǒng)PFA，根據(jù)所處環(huán)境狀態(tài)選擇其他行為方式，沿特定路線行動駛離問題區(qū)域，直至障礙物影響范圍外，以完成對問題4、問題5的規(guī)避，然后回到傳統(tǒng)PFA. 對此，本文設(shè)計了一種多行為方式組合的機器人行動策略，該策略包含沿等距線、直行和傳統(tǒng)PFA 3種行為方式及其起止條件。

直行行為針對預(yù)判出無效步進(jìn)或有效振蕩時，機器人當(dāng)前位置與目標(biāo)點的連線段不會穿過所在影響范圍對應(yīng)障礙區(qū)的情況(簡稱情況1)；沿等距線行為針對預(yù)判出無效步進(jìn)或有效振蕩時，機器人當(dāng)前位置與目標(biāo)點的連線段會穿過所在影響范圍對應(yīng)障礙區(qū)的情況(簡稱情況2)；傳統(tǒng)PFA行為作為整個算法的基礎(chǔ)行為，針對前兩種行為之外的其他所有情況(簡稱情況3)。

當(dāng)針對的情況出現(xiàn)時，機器人需要開始進(jìn)入對應(yīng)的行為，故各行為的起始條件即是其所針對情況的預(yù)判條件。當(dāng)機器人采用新的行為時，原本的行為結(jié)束，故原行為的中止條件即為新行為的起始條件。通過各行為起止條件之間的銜接，完成行為切換。起止條件應(yīng)滿足以下要求：1)各行為的起始條件間無交集(不沖突)，且并集包含所有情況；2)新行為的起始條件作為原行為的中止條件，必須確保原行為所針對的問題情況已消失。

3.2.1 直行行為

當(dāng)情況1出現(xiàn)時，此時機器人朝目標(biāo)直行行駛不會與對應(yīng)障礙物發(fā)生碰撞，機器人保持目標(biāo)方向直行至該影響范圍之外或抵達(dá)目標(biāo)點。

定理1機器人直行駛至障礙物影響范圍外時，原可能發(fā)生的問題4、問題5被規(guī)避。

證明設(shè)機器人在經(jīng)過障礙區(qū)Obs(n)影響范圍的部分連通域H時預(yù)判出現(xiàn)無效步進(jìn)或有效振蕩，此時可能發(fā)生問題4、問題5. 機器人朝目標(biāo)方向直線穿過H行駛至影響范圍邊界之外后，回到傳統(tǒng)PFA行為，此時機器人不受Obs(n)影響，只受目標(biāo)點引力作用，會繼續(xù)向目標(biāo)點直行，不再進(jìn)入H，故H中預(yù)判的無效步進(jìn)或有效振蕩不會出現(xiàn)，即原可能出現(xiàn)的問題4、問題5被規(guī)避。

起始條件：

ρ(qr(s),qobs(n))≤ρO(n)∧
{q|q∈qr(s)qg,q∈Obs(n)}=?，

(7)

式中：q為任意一點坐標(biāo)。

中止條件：

ρ(qr(s),qobs(n))>ρO(n)∨|qr(s)qg|

(8)

起始條件表示機器人進(jìn)入Obs(n)的影響范圍后在目標(biāo)點方向上不會與障礙物發(fā)生碰撞。該起始條件包含了上述中預(yù)判出無效步進(jìn)或有效振蕩的情況；另一方面，如果機器人正常避障不會出現(xiàn)問題，但也會因為避障導(dǎo)致一定程度的繞行，而機器人本可以朝目標(biāo)直行，這種多余的繞行可以避免。合并這兩種情況的預(yù)判條件即為直行行為起始條件。

中止條件表示機器人駛離Obs(n)的影響范圍或抵達(dá)目標(biāo)點。此時機器人已經(jīng)不受Obs(n)的影響，原可能發(fā)生的問題4、問題5被規(guī)避，所以結(jié)束直行行為。

根據(jù)直行行為過程可得該行為下路徑點計算公式為

(9)

式中：第1個公式確定方向一致性；第2個公式確定步長一致性。

3.2.2 沿等距線行為

當(dāng)情況2出現(xiàn)時，此時機器人必須按特定路線進(jìn)行規(guī)避繞行，機器人先按目標(biāo)方向直行，抵達(dá)等距線限定區(qū)域，然后沿著等距線行駛，直至切換至直行行為，離開問題區(qū)域。

本文所述的等距線，即在影響范圍內(nèi)，一條由到障礙區(qū)最短距離相等的點構(gòu)成且可以遍歷障礙區(qū)邊緣的閉合曲線。用點集E(ρe)n表示：

E(ρe)n={q|ρ(q,qobs(n))=ρe,ρe≤γexp≤ρO(n)}，

(10)

式中：ρe為等距線上的點要滿足的距離值。對于凸障礙區(qū)和一般凹障礙區(qū)，等距線是障礙區(qū)邊界線的放大，但是對于一些缺口較窄的凹障礙區(qū)，ρe過大會導(dǎo)致所得的等距線不能穿過缺口到達(dá)障礙區(qū)包圍的內(nèi)部區(qū)域，機器人無法搜索該內(nèi)部區(qū)域，如圖12所示。而且在實際中可能部分障礙區(qū)信息是未知的，無法預(yù)知缺口的大小，故參數(shù)ρe應(yīng)越小越好，理論上可以為0，但考慮到實際中存在誤差，所以引入一個誤差經(jīng)驗參數(shù)γexp對ρe進(jìn)行限定，確保在安全的前提下使ρe盡可能小，機器人能夠遍歷障礙物輪廓。

圖12 不同ρe的等距線示意圖Fig.12 Schematic diagram of isometric lines with different ρe

定理2障礙區(qū)Obs(n)邊界線上必存在一點，該點與目標(biāo)點的連線段不穿過Obs(n)，即Obs(n)邊界線上存在點O與目標(biāo)點G的有向連線段OG，除起點O外必與Obs(n)沒有交點。

證明假設(shè)Obs(n)邊界線上不存在這樣的點，即Obs(n)邊界線上任意一點O與目標(biāo)點G的有向連線段OG，除起點O外必與Obs(n)有交點。設(shè)該連線段由K個點組成，則該連線段可以表示為點集：{P1,P2,…,Pj,…,PK}，其中P1=O，PK=G，由上述假設(shè)得，必有Pj1∈Obs(n)的邊界線(1

定理3對于障礙區(qū)Obs(n)邊界線與等距線之間區(qū)域的任意一點P，必存在兩端端點O、Q分別位于Obs(n)邊界線與等距線上，且經(jīng)過該點的一條線段OQ，OQ不穿過Obs(n)，即線段與Obs(n)除端點O外沒有交點。

證明首先，根據(jù)本文等距線的定義可知，等距線是遍歷Obs(n)邊界且閉合包圍的，連接O、P并延長，與等距線必有交點Q，故必有經(jīng)過點P的線段OQ；接著，假設(shè)任意情況下的OQ，除端點O外與Obs(n)必有交點。設(shè)線段OQ由K個點組成，則該連線段可以表示為點集：{P1,P2,…,Pj,…,PK}，其中P1=O，PK=Q，根據(jù)假設(shè)必有Pj1屬于Obs(n)的邊界線(1

定理4在上述情況下，等距線上必存在一點，該點與目標(biāo)點的連線段不穿過障礙區(qū)Obs(n)，即滿足直行行為的起始條件。

證明當(dāng)G位于等距線外側(cè)，即ρ(Obs(n),G)≥ρe時，滿足定理2的連線段OG上的點到Obs(n)的距離范圍為[0,ρ(Obs(n),G)]，OG上有點P滿足ρ(Obs(n),P)=ρe，即P也位于等距線上，OG與Obs(n)無交點，故PG與Obs(n)無交點。

當(dāng)G位于等距線內(nèi)側(cè)，即ρ(Obs(n),G)<ρe時，將G看作定理3中的點P，則有滿足定理3的過點G的線段OQ，因為OQ除點O外與Obs(n)無交點，故GQ與Obs(n)無交點。

綜合上述兩類討論，定理4得證，即機器人必定能夠從沿等距線行為切換為直行行為，直至駛出影響范圍。再根據(jù)定理1可知，原本H中預(yù)判的無效步進(jìn)或有效振蕩不會出現(xiàn)，即原可能出現(xiàn)的問題4、問題5被規(guī)避。

起始條件：

ρ(qr(s),qobs(n))≤
ρO(n)∧{q|q∈qr(s)qg,q∈Obs(n)}≠?∧
qr(s)qr(s+1)?PROBLEM，

(11)

中止條件：

ρ(qr(s),qobs(n))≤ρO(n)∧
{q|q∈qr(s)qg,q∈Obs(n)}=?.

(12)

起始條件中的PROBLEM表示出現(xiàn)無效步進(jìn)或有效振蕩。起始條件即對情況2的判定；由于等距線位于影響范圍內(nèi)部，為使機器人能夠行駛到Obs(n)影響范圍外，將沿等距線行為與直行行為銜接起來，所以中止條件即為直行的起始條件。

根據(jù)沿等距線行為過程得該行為下路徑點計算公式：

當(dāng)ρ(qr(s),qobs(n))≤γexp時，

(13)

當(dāng)ρ(qr(s),qobs(n))>γexp時，

(14)

3.2.3 傳統(tǒng)PFA行為

傳統(tǒng)PFA行為作為整個算法的基礎(chǔ)行為，在除去情況1、情況2以及多余繞行的其他所有情況下，機器人按照傳統(tǒng)PFA，在勢場力的作用下移動。

起始條件：

(15)

中止條件：

ρ(qr(s),qobs(n))≤
ρO(n)∧{q|q∈qr(s)qg,q∈Obs(n)}=?∨
(ρ(qr(s),qobs(n))≤
ρO(n)∧{q|q∈qr(s)qg,q∈Obs(n)}≠?∧
qr(s)qr(s+1)?PROBLEM)∨|qr(s)qg|

(16)

該行為下路徑點計算公式為

(17)

根據(jù)3種行為各自的功能與特點，可以得到他們的切換路線圖，如圖13所示。

圖13 行為切換路線圖Fig.13 Roadmap of behavior switching

歸納得到行為切換有兩種路線：方式1，傳統(tǒng)PFA?直行?傳統(tǒng)PFA；方式2，傳統(tǒng)PFA?沿等距線?直行?傳統(tǒng)PFA. 由定理1和定理2可知這兩種路線均可以使機器人規(guī)避原來可能出現(xiàn)的問題4、問題5.

至此，本文分析了問題1～問題3的共同必要條件與問題4、問題5的共同必要條件，并提出對應(yīng)的可變影響范圍算法與多行為行動策略，結(jié)合傳統(tǒng)PFA，形成改進(jìn)后的CSVR-PFA. 問題分析思路總圖與完整算法流程圖分別如圖14和圖15所示。

圖14 問題分析思路總圖Fig.14 General diagram of problem analysis ideas

圖15 CSVR-PFA完整算法流程圖Fig.15 Flow chart of CSVR-PFA

4 仿真實驗及結(jié)果分析

為了驗證本文方法的有效性和可行性，在數(shù)學(xué)仿真軟件MATLAB 2017b平臺上進(jìn)行了仿真實驗。機器人簡化為半徑為0.5 m的圓以便于膨脹計算。同時，為了讓仿真接近現(xiàn)實，設(shè)定機器人探測得到的距離和角度誤差為5%.

4.1 U形空間仿真研究

在相同尺寸的U形空間中，通過改變機器人的探測范圍大小以及U形空間缺口大小，模擬機器人在多種尺度U形空間中的狀態(tài)。此U形陷阱環(huán)境中機器人的起點為(40 m,30 m)，目標(biāo)點為(70 m,65 m)，引力系數(shù)ξ=1，斥力系數(shù)η=2，步長Ls=1 m，抗干擾系數(shù)λ=-0.1，等距線限定參數(shù)γexp=1.5 m，障礙區(qū)影響范圍ρO=10 m，探測范圍R分別為2 m、10 m和50 m.

圖16和圖17分別是U形空間在大缺口和小缺口下，探測范圍分別為2 m、10 m和50 m時的仿真結(jié)果。由仿真結(jié)果可知，在模擬的各種尺度的U形空間中，本文所提算法都能夠成功規(guī)劃出一條較好的可行路徑，使機器人順利抵達(dá)目標(biāo)點，驗證了所提方法在多尺度U形空間內(nèi)的有效性。

圖16 不同R下的大缺口U形空間路徑對比圖Fig.16 Comparison diagram of U-shaped spatial path with large gap and different R

圖17 不同R下的小缺口U形空間路徑對比圖Fig.17 Comparison diagram of U-shaped spatial path with narrow gap and different R

4.2 參數(shù)不敏感性研究

在PFA中，引力系數(shù)ξ、斥力系數(shù)η和障礙區(qū)影響范圍ρO這3個核心參數(shù)的改變往往直接影響算法結(jié)果。要想通過PFA獲得較好的規(guī)劃結(jié)果，需要多次調(diào)整參數(shù)，較為繁瑣復(fù)雜。而本文所提的CSVR-PFA從問題的必要條件入手規(guī)避問題1～問題5，從而對這3個核心參數(shù)具有不敏感性。設(shè)置4組參數(shù)進(jìn)行仿真對比實驗，以驗證本文方法的參數(shù)不敏感性，仿真對比實驗結(jié)果如圖18所示。

圖18 不同ξ、η和ρO下的仿真對比圖Fig.18 Simulation contrast diagrams with different ξ, η and ρO

從仿真對比結(jié)果中可明顯看出，PFA在參數(shù)差異性較大時，規(guī)劃中出現(xiàn)的問題以及規(guī)劃結(jié)果差異較大。相比之下，本文所提方法在各種參數(shù)條件下均能夠順利規(guī)劃出較優(yōu)的路徑結(jié)果，同時各結(jié)果之間差異性很小，本文方法對引力系數(shù)ξ、斥力系數(shù)η和障礙區(qū)影響范圍ρO這3個核心參數(shù)具有很好的不敏感性。

4.3 綜合仿真研究

機器人在執(zhí)行軍事任務(wù)時面對的障礙環(huán)境往往較為復(fù)雜，為驗證本文所提算法在復(fù)雜環(huán)境中的有效性，說明并驗證算法中各模塊對各自針對的問題的作用，本節(jié)在130 m×130 m的區(qū)域內(nèi)設(shè)置多個形狀范圍不一的障礙區(qū)，模擬構(gòu)建復(fù)雜戰(zhàn)場環(huán)境。命令機器人從起點出發(fā)，分別使用PFA、可變影響范圍PFA(VR-PFA)以及CSVR-PFA自行規(guī)劃路徑(縱向?qū)Ρ?。機器人穿過該復(fù)雜區(qū)域抵達(dá)目標(biāo)點視作完成作戰(zhàn)任務(wù)。

設(shè)定起點為(0 m,0 m)，目標(biāo)點為(110 m,110 m)，引力系數(shù)ξ=1，斥力系數(shù)η=1，各障礙區(qū)的初始影響范圍為ρO(1)=…=ρO(7)=10 m，步長Ls=1 m，抗干擾系數(shù)λ=-0.1，算法最大可計算次數(shù)為500.

4.3.1 傳統(tǒng)PFA仿真結(jié)果

采用PFA進(jìn)行路徑規(guī)劃，結(jié)果如圖19和圖20所示。

圖19 PFA仿真結(jié)果Fig.19 Simulated results of PFA

圖20 PFA計算過程中的收斂趨勢Fig.20 Convergence trend in the calculation process of PFA

由圖19可以看出：細(xì)節(jié)1中出現(xiàn)問題2的場景一現(xiàn)象，機器人雖然順利從Obs(1)與Obs(2)之間的通道穿過，但在Obs(1)與Obs(2)的共同影響下出現(xiàn)了嚴(yán)重的振蕩；細(xì)節(jié)2中出現(xiàn)問題1現(xiàn)象以及問題2中的場景2現(xiàn)象，機器人本應(yīng)從Obs(3)與Obs(4)之間通道穿過，此時只能從兩個障礙區(qū)的外側(cè)繞行，同時，機器人在繞行過程中路徑發(fā)生嚴(yán)重振蕩；細(xì)節(jié)3中出現(xiàn)問題3現(xiàn)象，機器人被困于Obs(5)與Obs(6)影響范圍重疊區(qū)的某一極小區(qū)域無法逃脫。

如圖20所示，當(dāng)達(dá)到最大計算次數(shù)時，算法沒有收斂至0，此時機器人無法抵達(dá)目標(biāo)點，任務(wù)失敗。

4.3.2 VR-PFA仿真結(jié)果

將可變影響范圍算法與PFA結(jié)合，形成VR-PFA. 機器人路徑規(guī)劃結(jié)果如圖21和圖22所示。

圖21 VR-PFA仿真結(jié)果Fig.21 Simulated results of VR-PFA

圖22 VR-PFA計算過程中的收斂趨勢Fig.22 Convergence trend in the calculation process of VR-PFA

觀察圖21的細(xì)節(jié)1～細(xì)節(jié)3可發(fā)現(xiàn)，原本存在由多障礙區(qū)導(dǎo)致的問題1～問題3已被規(guī)避，但此時出現(xiàn)大量單障礙區(qū)振蕩，即問題5. 細(xì)節(jié)1中，存在輕微的內(nèi)部- 有效振蕩，因其對算法收斂速度影響甚小，所以在圖22中并不能明顯反映該處振蕩。細(xì)節(jié)2中，機器人剛進(jìn)入Obs(3)的影響范圍就被排斥出去且出現(xiàn)無效步進(jìn)，緊接著，與3.1.2中的分析相同，形成大量邊緣- 無效振蕩。細(xì)節(jié)3中也產(chǎn)生了一個邊緣- 無效振蕩。細(xì)節(jié)4中，機器人在躲避Obs(7)“5”字形復(fù)雜凹形障礙區(qū)時陷入單障礙區(qū)局部極小陷阱，無法逃離，同時產(chǎn)生大量無效步進(jìn)，即問題4.

如圖22所示，當(dāng)達(dá)到最大計算次數(shù)時，算法沒有收斂至0，此時機器人無法抵達(dá)目標(biāo)點，任務(wù)失敗。但同時，本小節(jié)結(jié)果驗證了本文提出的可變影響范圍算法對規(guī)避問題1～問題3的有效性。

4.3.3 CSVR-PFA仿真結(jié)果

將多行為行動策略與可變影響范圍算法、PFA結(jié)合，最終形成本文提出的CSVR-PFA. 機器人路徑規(guī)劃結(jié)果如圖23和圖24所示。

圖23 CSVR-PFA仿真結(jié)果Fig.23 Simulated results of CSVR-PFA

圖24 CSVR-PFA計算過程中收斂趨勢Fig.24 Convergence trend in the calculation process of CSVR-PFA

觀察細(xì)節(jié)1與細(xì)節(jié)3，機器人分別進(jìn)入Obs(2)、Obs(5)的影響范圍后，判定采用直行行為，從而規(guī)避了原有的內(nèi)部- 有效振蕩與邊緣- 無效振蕩。細(xì)節(jié)2中，機器人進(jìn)入Obs(3)影響范圍后，判定出下一步進(jìn)為無效步進(jìn)且不可直行，采用沿等距線行為。行進(jìn)一段距離后，機器人判定可以直行，轉(zhuǎn)為直行行為，直到離開Obs(3)的影響范圍，規(guī)避了原有的邊緣- 無效振蕩。細(xì)節(jié)4中，機器人進(jìn)入Obs(7)影響范圍后，判定不可直行且不存在問題4、問題5的必要條件，此時采用PFA行為。在即將陷入單障礙區(qū)局部極小陷阱時，機器人判定將出現(xiàn)無效步進(jìn)，轉(zhuǎn)用沿等距線行為直到判定可以直行，從而規(guī)避了該陷阱。最終機器人直行抵達(dá)目標(biāo)點，任務(wù)成功。此時算法收斂至0，計算次數(shù)(步數(shù))為290次。

本節(jié)的仿真結(jié)果表明，在復(fù)雜凹凸障礙環(huán)境下，本文提出的可變影響范圍算法能夠使機器人有效規(guī)避問題1～問題3；多行為行動策略能夠有效規(guī)避問題4、問題5. 最終，本文提出的以傳統(tǒng)PFA為基礎(chǔ)，結(jié)合多行為行動策略與可變影響范圍算法的CSVR-PFA算法，能夠有效綜合規(guī)避問題1～問題5，使機器人順利實現(xiàn)實時路徑規(guī)劃。

4.4 算法對比仿真研究

圖25 PFA、DWA、CSVR-PFA仿真結(jié)果對比圖Fig.25 Comparison of simulated results of PFA, DWA and CSVR-PFA

為進(jìn)一步驗證本文方法的有效性與優(yōu)越性，在復(fù)雜障礙環(huán)境下，將本文所提方法CSVR-PFA與PFA、動態(tài)窗口法(DWA)、A*算法、快速隨機樹(RRT) 算法進(jìn)行了20次仿真對比實驗。設(shè)有一個100 m×100 m的機器人移動空間，起點坐標(biāo)(0 m,0 m)，目標(biāo)點坐標(biāo)100 m×100 m，機器人需沿途規(guī)避多個凹凸障礙區(qū)域抵達(dá)目標(biāo)點完成任務(wù)。在此環(huán)境下，各算法仿真結(jié)果如圖25、圖26和表1所示。

圖26 A*算法、RRT算法、CSVR-PFA仿真結(jié)果對比圖Fig.26 Comparison of simulated results of A* algorithm, RRT algorithm and CSVR-PFA

表1 對比算法仿真結(jié)果Tab.1 Comparation of the algorithms’ simulated results

圖25是本文所提算法與PFA、DWA兩種經(jīng)典局部路徑規(guī)劃算法的對比結(jié)果；圖26是本文所提算法與A*、RRT兩種經(jīng)典全局路徑規(guī)劃算法的對比結(jié)果；表1是20次仿真實驗后各算法的平均規(guī)劃時間與平均路徑長度。

由圖25可以看出：PFA算法的結(jié)果中包含大量振蕩，所得路徑曲折嚴(yán)重，DWA算法的結(jié)果十分平滑，但這兩個算法在復(fù)雜凹凸障礙環(huán)境下皆陷入局部極小陷阱，機器人無法抵達(dá)目標(biāo)點；與之相比，本文提出的CSVR-PFA算法在傳感器存在一定的測量誤差情況下也能成功為機器人規(guī)劃出一條到達(dá)目標(biāo)點的路徑，且所得路徑較為平直光滑，具有較好的規(guī)劃性能與穩(wěn)定性。

由圖26看出，RRT算法和A*算法都成功規(guī)劃出到達(dá)目標(biāo)點的路徑，但兩者的路徑都較為曲折，不夠平直光滑。同時，由于RRT算法和A*算法需要對地圖進(jìn)行柵格化處理，所以對于一些形狀的障礙(如圓形)，柵格化的地圖對這些障礙的描述精度較低，這也導(dǎo)致RRT算法和A*算法的規(guī)劃結(jié)果在路徑精度方面的局限性。仿真中發(fā)現(xiàn)，若提高柵格化分辨率，雖然能夠提升規(guī)劃路徑的精度，但同時也會導(dǎo)致算法花費時間的急劇提高。與之相比，本文的CSVR-PFA算法規(guī)劃出的路徑更加平直光滑，并且由于不需要對地圖柵格化處理，對路徑點位置沒有限制，可以充分探測到各種形狀的障礙區(qū)邊緣，規(guī)劃出的路徑精度更好。結(jié)合表1可看出，本文算法在規(guī)劃時間與路徑長度上都明顯優(yōu)于RRT和A*算法(柵格分辨率為1 m)，驗證了本文算法的優(yōu)越性。

延長最右側(cè)長條形障礙，模擬出口十分狹窄的障礙環(huán)境。對RRT算法、A*算法和CSVR-PFA進(jìn)一步仿真對比，結(jié)果如圖27和圖28所示。

圖27 A*算法和CSVR-PFA結(jié)果對比圖Fig.27 Comparison of simulated results of A* algorithm and CSVR-PFA

圖28 RRT算法搜索過程圖Fig.28 Search process of RRT algorithm

對于RRT算法，由于出口過于狹窄，被搜索到的概率大大降低，最終經(jīng)過15 000次搜索后，未能尋找到抵達(dá)目標(biāo)點的路徑，規(guī)劃失敗。另一方面，與先前的仿真結(jié)果類似，A*算法能夠規(guī)劃出抵達(dá)目標(biāo)點的路徑，但依舊存在曲折不夠平滑、精度低、長度大等缺點。本文算法在出口狹窄的障礙環(huán)境下，依然能夠為機器人快速地規(guī)劃出一條較為平直光滑、精度高、長度較短的可行路徑。

5 結(jié)論

考慮到戰(zhàn)場環(huán)境的復(fù)雜性，為解決PFA在復(fù)雜障礙環(huán)境下存在的5個問題，保障機器人實現(xiàn)實時路徑規(guī)劃，順利完成作戰(zhàn)任務(wù)，本文提出了CSVR-PFA. 得出以下主要結(jié)論：

1)分析得到影響范圍之間存在最小距離小于1個步長的部分是路徑不識別、多障礙區(qū)導(dǎo)致的振蕩、多障礙區(qū)導(dǎo)致的局部極小陷阱3個問題的共同必要條件，并提出可變影響范圍算法進(jìn)行問題規(guī)避。

2)基于新的振蕩分類方法，分析單障礙區(qū)導(dǎo)致局部極小陷阱、單障礙區(qū)導(dǎo)致振蕩兩個問題的共同表現(xiàn)形式，把對這兩個問題的解決轉(zhuǎn)化為對無效步進(jìn)與有效振蕩的規(guī)避，對此提出了多行為行動策略，并最終形成本文的CSVR-PFA.

3)本文進(jìn)行了詳細(xì)的仿真實驗，驗證了CSVR-PFA在不同尺度U形空間內(nèi)的有效性、參數(shù)不敏感性；通過縱向分步仿真結(jié)果對比，驗證算法在復(fù)雜障礙環(huán)境下的有效性；將CSVR-PFA與PFA、DWA、A*算法、RRT算法進(jìn)行橫向?qū)Ρ?，驗證了本文算法的誤差穩(wěn)定性、規(guī)劃有效性與性能優(yōu)越性。

4)本文提出的CSVR-PFA從問題必要條件入手，能夠更系統(tǒng)全面地規(guī)避PFA的一系列問題，在實際應(yīng)用中，只需測距測角傳感器的數(shù)據(jù)，易于機器人系統(tǒng)開發(fā)與使用；同時也保留了PFA簡單高效、實時性強、路徑短且平滑、計算精度高的優(yōu)點。