基于改進勢場法的移動機器人路徑規(guī)劃

孫鵬耀， 黃炎焱， 潘堯

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

0 引言

隨著移動機器人技術(shù)的不斷發(fā)展，機器人自主執(zhí)行作戰(zhàn)任務(wù)已成為軍事領(lǐng)域的一個重要方向。在許多機器人作戰(zhàn)任務(wù)中，比如排雷、偵察、運送物資等，要想移動機器人順利完成作戰(zhàn)任務(wù)，準確可靠的路徑規(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)用仍存在許多問題?，F(xiàn)有研究主要將這些問題分為3類：局部極小陷阱、路徑振蕩、路徑不被識別。

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

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

2)逃離法的思想是在機器人陷入局部極小陷阱后通過某種方法引導(dǎo)機器人逃離陷阱。常見的逃離法有搜索算法、多PFA、虛擬障礙物法、沿墻行走法等。搜索算法的思想是當(dāng)陷入局部極小陷阱后，搜索并移動到勢場值比當(dāng)前局部極小值更小的位置點，然后機器人繼續(xù)按勢場梯度下降法移動，目前常用的搜索算法如隨機搜索、模擬退火在缺少啟發(fā)信息時存在搜索效率的不確定性，可能導(dǎo)致搜索效率很低[13]。多PFA[14]的思想是設(shè)計多個全局最小點相同但局部極小點不同的勢場函數(shù)，當(dāng)機器人陷入局部極小陷阱時通過切換勢場函數(shù)保證規(guī)劃繼續(xù)進行。多PFA需要對全局環(huán)境信息提前準確了解并構(gòu)造勢場函數(shù)，且構(gòu)造多個滿足要求的勢函數(shù)難度大，容易使機器人重新陷入已逃離的局部極小陷阱，增加了規(guī)劃的復(fù)雜度與不確定性。虛擬障礙物法[15]的思想是當(dāng)機器人陷入局部極小陷阱時，在局部極小點附近加入虛擬障礙物，通過改變原有勢場分布引導(dǎo)或阻止機器人逃離或進入局部極小陷阱，但在復(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è)方兩類，但并未給出準確的分類標準，是一個研究盲點，模糊的分類邊界導(dǎo)致在應(yīng)用具體方法時難以設(shè)計準確判據(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存在的問題細分為相近障礙區(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)其次，提出有效步進與無效步進的概念，結(jié)合內(nèi)部振蕩與邊緣振蕩，提出一種新的振蕩分類方法，并給出準確分類標準；

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

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

1 PFA基本原理

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

引力勢能函數(shù)：

(1)

斥力勢能函數(shù)：

(2)

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

引力函數(shù)為

(3)

斥力函數(shù)為

(4)

式中：

(5)

(6)

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

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

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

2.1 問題分析

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

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

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

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

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

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

2)兩個障礙區(qū)的影響范圍不重疊但斥力極大，且存在某一區(qū)域，這兩個影響范圍的最小距離極小(小于一個步長)。當(dāng)機器人進入該部分區(qū)域時，由于斥力極大且此時兩障礙區(qū)影響范圍極其接近，機器人剛進入1號障礙區(qū)影響范圍就被排斥出去，緊接著進入2號障礙區(qū)的影響范圍。接下來，機器人再被2號障礙區(qū)排斥出影響范圍，緊接進入1號障礙區(qū)影響范圍。以此往復(fù)，形成振蕩，具體如圖6所示。若最小距離大于1個步長，機器人從1號影響范圍被排斥出去后不能立即進入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ù)，機器人不能完全貼合理想路線行進，合力出現(xiàn)突變從而導(dǎo)致振蕩。

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

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

所謂局部極小陷阱，原理上，即在合勢場中存在某處，其合勢場強度為0，機器人在該處受到的合力為0，從而無法進行下一步方向判斷，停滯不前，路徑中斷。局部極小陷阱是由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ū)位置信息進行處理，處理后各障礙區(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表示機器人行進步長，ε為一個微小的正值。可變率c反映了某一對障礙區(qū)影響范圍調(diào)整的緊急程度，c≥1時表示影響范圍間最小距離小于1個步長，需要進行調(diào)整，c越大表示越緊急，c<1表示不需要調(diào)整?？紤]到一次調(diào)整后會導(dǎo)致多組障礙區(qū)的c發(fā)生變化，故每次只調(diào)整最緊急的一對障礙區(qū)，然后重新計算可變率矩陣，再選擇下一對要調(diào)整的障礙區(qū)，直到任意一對障礙區(qū)c<1，從而達到算法目的。

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

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

3.1 問題分析

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

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

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

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

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

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

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

1) 障礙區(qū)影響范圍邊緣振蕩。當(dāng)影響范圍邊緣斥力強度非常大時，如圖8所示：①機器人剛剛進入障礙區(qū)范圍后，所受斥力極大，機器人立刻被排斥出障礙區(qū)影響范圍；②機器人剛被排斥出來，此時只受到引力作用，再次進入障礙區(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)指向目標點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所示，機器人可以進入影響范圍內(nèi)部：①機器人所受斥力較小，合力指向影響范圍內(nèi)部，機器人朝向障礙區(qū)行進，越來越靠近障礙區(qū)，斥力的增長率急速增大，引力的變化平緩；②由于計算離散性，機器人所受斥力急劇增大，引起合力突變，排斥機器人遠離障礙區(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方向的角度，逆時針為正，順時針為負；λ為一個小于0的較小數(shù)值，一是用來判定是否產(chǎn)生振蕩，二是用來消除一些微弱干擾和誤差。

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

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

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

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

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

3.2 多行為行動策略

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

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

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

3.2.1 直行行為

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

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

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

起始條件：

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

(7)

式中：q為任意一點坐標。

中止條件：

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

(8)

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

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

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

(9)

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

3.2.2 沿等距線行為

當(dāng)情況2出現(xiàn)時，此時機器人必須按特定路線進行規(guī)避繞行，機器人先按目標方向直行，抵達等距線限定區(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)致所得的等距線不能穿過缺口到達障礙區(qū)包圍的內(nèi)部區(qū)域，機器人無法搜索該內(nèi)部區(qū)域，如圖12所示。而且在實際中可能部分障礙區(qū)信息是未知的，無法預(yù)知缺口的大小，故參數(shù)ρe應(yīng)越小越好，理論上可以為0，但考慮到實際中存在誤差，所以引入一個誤差經(jīng)驗參數(shù)γexp對ρe進行限定，確保在安全的前提下使ρe盡可能小，機器人能夠遍歷障礙物輪廓。

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

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

證明假設(shè)Obs(n)邊界線上不存在這樣的點，即Obs(n)邊界線上任意一點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在上述情況下，等距線上必存在一點，該點與目標點的連線段不穿過障礙區(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ù)判的無效步進或有效振蕩不會出現(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)無效步進或有效振蕩。起始條件即對情況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，形成改進后的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平臺上進行了仿真實驗。機器人簡化為半徑為0.5 m的圓以便于膨脹計算。同時，為了讓仿真接近現(xiàn)實，設(shè)定機器人探測得到的距離和角度誤差為5%.

4.1 U形空間仿真研究

在相同尺寸的U形空間中，通過改變機器人的探測范圍大小以及U形空間缺口大小，模擬機器人在多種尺度U形空間中的狀態(tài)。此U形陷阱環(huán)境中機器人的起點為(40 m,30 m)，目標點為(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ī)劃出一條較好的可行路徑，使機器人順利抵達目標點，驗證了所提方法在多尺度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ù)進行仿真對比實驗，以驗證本文方法的參數(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ū)域抵達目標點視作完成作戰(zhàn)任務(wù)。

設(shè)定起點為(0 m,0 m)，目標點為(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進行路徑規(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可以看出：細節(jié)1中出現(xiàn)問題2的場景一現(xiàn)象，機器人雖然順利從Obs(1)與Obs(2)之間的通道穿過，但在Obs(1)與Obs(2)的共同影響下出現(xiàn)了嚴重的振蕩；細節(jié)2中出現(xiàn)問題1現(xiàn)象以及問題2中的場景2現(xiàn)象，機器人本應(yīng)從Obs(3)與Obs(4)之間通道穿過，此時只能從兩個障礙區(qū)的外側(cè)繞行，同時，機器人在繞行過程中路徑發(fā)生嚴重振蕩；細節(jié)3中出現(xiàn)問題3現(xiàn)象，機器人被困于Obs(5)與Obs(6)影響范圍重疊區(qū)的某一極小區(qū)域無法逃脫。

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

如圖22所示，當(dāng)達到最大計算次數(shù)時，算法沒有收斂至0，此時機器人無法抵達目標點，任務(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

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

為進一步驗證本文方法的有效性與優(yōu)越性，在復(fù)雜障礙環(huán)境下，將本文所提方法CSVR-PFA與PFA、動態(tài)窗口法(DWA)、A*算法、快速隨機樹(RRT) 算法進行了20次仿真對比實驗。設(shè)有一個100 m×100 m的機器人移動空間，起點坐標(0 m,0 m)，目標點坐標100 m×100 m，機器人需沿途規(guī)避多個凹凸障礙區(qū)域抵達目標點完成任務(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é)果中包含大量振蕩，所得路徑曲折嚴重，DWA算法的結(jié)果十分平滑，但這兩個算法在復(fù)雜凹凸障礙環(huán)境下皆陷入局部極小陷阱，機器人無法抵達目標點；與之相比，本文提出的CSVR-PFA算法在傳感器存在一定的測量誤差情況下也能成功為機器人規(guī)劃出一條到達目標點的路徑，且所得路徑較為平直光滑，具有較好的規(guī)劃性能與穩(wěn)定性。

由圖26看出，RRT算法和A*算法都成功規(guī)劃出到達目標點的路徑，但兩者的路徑都較為曲折，不夠平直光滑。同時，由于RRT算法和A*算法需要對地圖進行柵格化處理，所以對于一些形狀的障礙(如圓形)，柵格化的地圖對這些障礙的描述精度較低，這也導(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進一步仿真對比，結(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次搜索后，未能尋找到抵達目標點的路徑，規(guī)劃失敗。另一方面，與先前的仿真結(jié)果類似，A*算法能夠規(guī)劃出抵達目標點的路徑，但依舊存在曲折不夠平滑、精度低、長度大等缺點。本文算法在出口狹窄的障礙環(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個問題的共同必要條件，并提出可變影響范圍算法進行問題規(guī)避。

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

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

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