基于虛擬障礙物法的無震蕩航路規(guī)劃

陳天德， 黃炎焱， 沈煒

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

0 引言

路徑規(guī)劃在移動機器人工程領域，如仿生機器人[1]、無人車[2]、無人機[3]、以及工業(yè)機械臂[4]等，是十分重要的研究內容。同時，在災害、突發(fā)事故應急系統(tǒng)與軍事輔助決策中也不乏有路徑規(guī)劃的身影[5-6]。目前，常用的路徑規(guī)劃方法有A*算法、人工勢場法、神經網絡法等[7]。人工勢場法[8]作為路徑規(guī)劃的一種算法，其數(shù)學描述簡潔、美觀，在環(huán)境少知、甚至未知的情況下，可以實時求解航路點且所得航路較為平滑[9]，在許多工程領域中的成功運用也證明該方法的有效性[10-12]。因此，在路徑規(guī)劃中，人工勢場法舉足輕重，受到了很大的重視。

然而由于算法本身的設計思想，局部極小值陷阱仍是傳統(tǒng)人工勢場法的嚴重缺點。目前，已有多種跳出局部極小值陷阱的方法，這些改進方法主要是從勢函數(shù)模型本身入手，通過改變勢函數(shù)模型來克服缺陷，如引入速度因素[13]、波動函數(shù)[14]、啟發(fā)式搜索[15]、混沌算法[16]以及切換勢函數(shù)法[17]等。這種改進思路類似于教室關門聲音大，就對門進行改造來降低聲音，雖然在一定程度上可以達到預期效果，但是對傳統(tǒng)勢函數(shù)模型進行了較大的改變，有的甚至已經失去了勢函數(shù)法的基本思想以及算法簡潔且易于實現(xiàn)的優(yōu)勢，改進后的算法需設定的參數(shù)較多且算法執(zhí)行也較為復雜，而且對路徑規(guī)劃中存在的震蕩問題并沒有很好的解決。

震蕩問題的出現(xiàn)會使得所解算出來的航路不夠平滑，出現(xiàn)反復多次的“之”字路徑[18]，這嚴重影響了避碰效果和趨向目標的效率。震蕩問題的出現(xiàn)主要是由于勢函數(shù)法執(zhí)行離散化所引起的，不是算法思想本身的問題。因此，從勢函數(shù)本身出發(fā)，很難解決震蕩問題。目前，針對航路點震蕩問題，解決的方法還較少，現(xiàn)有方法多數(shù)都是通過參數(shù)設定來降低震蕩程度，而并沒有徹底解決震蕩問題。

本文在不改變“門”的前提下，從“門”和“人”的關系入手，即人工勢場法和路徑規(guī)劃任務的關系，對人工勢場法存在的缺陷進行分析，相比已有的大部分文獻，本文為保留人工勢場法的優(yōu)點，在不改變勢函數(shù)模型的情況下，提出虛擬障礙物法和過濾震蕩點法，在算法執(zhí)行機制上克服人工勢場法的缺陷。

1 傳統(tǒng)人工勢場法及其缺點

由于算法本身，人工勢場法存在5個常見問題：1)由于局部極小值而存在陷阱區(qū)域；2)在相近障礙物之間不能識別路徑；3)在障礙物前可能出現(xiàn)震蕩；4)在狹窄通道中出現(xiàn)震蕩；5)GNRON問題(障礙物在目標附近，目標不可及)。

出現(xiàn)問題1和問題3的根本原因是斥力勢能非全局變量，可能會出現(xiàn)非目標點的零勢能梯度點，即局部極小值點。由于勢函數(shù)法編程實現(xiàn)時，一定會涉及到步長的確定，這就導致解算出的航路點不會正好為局部極小值點，而是在局部極小值點兩側來回震蕩，即陷入局部極小值陷阱。當機器人、障礙物和目標點在一條直線上時，這種情形最有可能出現(xiàn)。也正是因為步長的存在，導致在機器人、障礙物和目標點不在一條直線上時，很可能在障礙物附近解算出的航路點是來回震蕩前進的，這種震蕩不是無休止的，當脫離障礙物影響范圍時，也就擺脫了震蕩狀態(tài)。當然也可能由于步長設置與編譯精度的原因，使得本應陷入局部極小值陷阱的航路點解算緩解為在障礙物前震蕩多次后擺脫震蕩的狀態(tài)。如圖1所示：障礙物影響距離ρ0為1 m，當步長l為0.2 m時，航路點解算陷入局部極小值陷阱；當步長l為0.1 m時，航路點解算在障礙物前震蕩多次后擺脫震蕩并到達了目標點。

圖1 步長設定的影響Fig.1 Simulated results of different step sizes

問題2相比其他問題，影響較小，主要是由于在斥力與引力的博弈中，斥力處于優(yōu)勢所引起的，只要勢函數(shù)模型中步長、障礙物影響距離等相關參數(shù)設定得當，該問題能夠避免。但是問題2沒有處理好可能會升級為問題3甚至是問題1。

問題4的出現(xiàn)歸根到底是由于算法執(zhí)行的離散化，即步長設置所引起的。當然計算機精度也是離散化的標志，只是精度過高可以忽略離散的實質。因此問題4不是算法本身的問題，或者說不是算法思想的問題，僅為通過計算機語言實現(xiàn)算法時所引起的問題。換而言之，只要使用編程語言實現(xiàn)算法，該問題肯定會出現(xiàn)，只是嚴重程度不同而已。

因此以上問題可以歸結為算法本身局部極小值以及算法執(zhí)行所必須離散化所引起的。也就是說解決這些問題主要分為兩步：第1步，在算法執(zhí)行過程中，發(fā)現(xiàn)并解決局部極小值所引起的問題；第2步，在路徑解算完后，遍歷所有航路點，過濾掉因為算法執(zhí)行離散化所引起的震蕩航路點。

文獻[19]提出虛擬障礙物的概念，當機器人陷入局部極小值陷阱時，引入虛擬障礙物，以此打破當前的力平衡狀態(tài)，使得機器人擺脫局部極小值陷阱，該方法執(zhí)行簡單且有效。但文獻[19]僅把虛擬障礙物放置在局部極小值點位置，這使得該方法在某些情況下不能高效地完成路徑規(guī)劃甚至有再次陷入局部極小值陷阱的危險，且也沒有討論如何求解局部極小值點，除此之外航路點震蕩問題依舊沒有被解決。

對于問題5，文獻[20]通過在斥力勢能函數(shù)中引入機器人與目標位置的距離，巧妙解決了GNRON問題，該模型被廣泛應用。為了簡化，假設機器人是一個質點且在二維空間內運動。機器人的位置坐標設為qr=(xR,yR)T，目標點的位置設置為qg，其引力勢能函數(shù)為

(1)

式中：ξ為一個正比例系數(shù)；ρm(qr,qg)是機器人與目標點距離的m次方。斥力勢能函數(shù)如下：

(2)

式中：η為一個正比例系數(shù)；ρ(qr,qo)為機器人到附近障礙物的最短距離，qo為在該障礙物上的一個點，機器人到該點的距離是機器人到該障礙物的最短距離。但該方法僅僅解決了GNRON問題，其他4個勢函數(shù)固有問題仍然存在。

本文以該勢函數(shù)模型為規(guī)劃模型，在文獻[19]所提出的虛擬障礙物概念基礎上，所做的具體工作如下：1)提出陷入和逃出局部極小值陷阱的判斷方法，增加虛擬障礙物放置位置的確定方法，以此形成改進型虛擬障礙物法，該算法自動識別航路點解算陷入或逃出局部極小值陷阱，綜合考慮航路點解算陷入局部極小值陷阱時障礙物的當前態(tài)勢并放置虛擬障礙物，以此來解決局部極小值陷阱問題，即問題1；2)對航路點震蕩的原因進行分析，提出識別震蕩航路點的方法，并針對航路點震蕩的兩種情形分別提出新航路點生成的具體方法，以此形成過濾震蕩點法，對規(guī)劃好的路徑，該算法自動識別其震蕩位置并過濾掉震蕩航路點，以此消除震蕩問題，即問題3和問題4.

2 虛擬障礙物法

2.1 虛擬障礙物法的原理

在一個未知環(huán)境中，使用勢函數(shù)法來對移動機器人進行路徑規(guī)劃，當航路點解算陷入局部極小值陷阱時，由于局部極小值點是一個勢場梯度為0的點，所以在該點移動機器人所受到的虛擬合外力為0，即機器人不能向任何方向移動。此時，在局部極小值點附近設置一個虛擬障礙物，它對機器人施加一個附加的斥力，以此打破當前的力平衡狀態(tài)，使得陷入局部極小值陷阱的航路點解算擺脫局部極小值陷阱，從而繼續(xù)進行接下來的航路點解算。

虛擬障礙物法嵌入到勢函數(shù)法中后，勢函數(shù)法的執(zhí)行流程如圖2所示。先判斷當前航路點位置是否是目標點，如果是目標點，則結束路徑規(guī)劃，如果不是則判斷當前航路點是否陷入局部極小值陷阱。如果沒有陷入局部極小值陷阱，則直接進行下一個航路點的解算；如果陷入局部極小值陷阱，則放置虛擬障礙物，再進行下一個航路點的解算，并判斷該航路點是否跳出局部極小值陷阱。若未跳出局部極小值陷阱，則繼續(xù)放置虛擬障礙物；若跳出則撤除所放置的所有虛擬障礙物并進行下一個航路點的解算，以此循環(huán)執(zhí)行下去。其中每次解算出的航路點都要先判斷是否已到達目標位置，如果已到達目標位置，則進行震蕩航路點的過濾并結束整個路徑規(guī)劃。

圖2 引入虛擬障礙物法后的算法流程Fig.2 Flow chart of algorithm combined with the virtual obstacle method

2.2 虛擬障礙物法的步驟

2.2.1 判斷是否陷入或逃出局部極小值陷阱

在靜態(tài)二維空間中，由于航路點解算大體方向是指向目標位置的，即當前位置與目標點的連線方向，因此算法所得到的航路點應是不斷靠近目標點的。而實際解算中偏離該方向的航路點則是由于障礙物的影響所造成的。這種影響可能會造成解算出的航路點遠離目標點，而航路點解算陷入局部極小值陷阱同樣也存在該現(xiàn)象。

目前航路點解算是否陷入局部極小值陷阱的判斷方法已有一些，文獻[19]給出的一個判斷方法雖簡單可行，但判斷效率不高，會執(zhí)行很多無謂的判斷。本文在該方法的基礎上進行了改進細化，提出一個判斷算法執(zhí)行是否陷入或逃出局部極小值陷阱的新方法。

當出現(xiàn)解算出的航路點開始遠離目標位置時，則表明航路點解算可能陷入了局部極小值陷阱，此時執(zhí)行更進一步的判斷，否則當航路點持續(xù)靠近目標位置時，則不進行是否陷入局部極小值陷阱的判斷，這樣大大提高了判斷的效率。由于算法執(zhí)行的離散化，解算出的航路點不可能正好落在局部極小值點位置，而是在局部極小值點附近的區(qū)域內進行徘徊，因此判斷是否陷入局部極小值陷阱的步驟如下：

步驟1解算第k步航路點的位置qk并計算該點與目標點的距離ρ(qk,qg)。

步驟2將計算得到的距離與上一步中航路點與目標點的距離進行比較，若ρ(qk,qg)<ρ(qk-1,qg)，則令k=k+1并跳到步驟1，否則跳到步驟3.

步驟3繼續(xù)解算航路點，得到第k+m步的航路點，m為算法執(zhí)行的跨度值，m取20，計算第k+m步與第k步航路點之間的距離。若該距離ρ(qk+m,qk)≤d，d為一個較小的值，這里d取步長的5倍，則認為航路點解算陷入局部極小值陷阱，進行步驟4，否則令k=k+m+1并跳到步驟1.

步驟4根據(jù)當前態(tài)勢，放置虛擬障礙物。

步驟5繼續(xù)解算航路點，得到k+2m步航路點，計算該航路點與k+m步航路點距離以及這兩個航路點與目標點的距離。若ρ(qk+2m,qk+m)>d且ρ(qk+2m,qg)<ρ(qk+m,qg)，則認為航路點解算已跳出局部極小值陷阱，撤出虛擬障礙物，令k=k+2m+1并跳到步驟1，否則令k=k+m并繼續(xù)進行步驟4.

步驟1到步驟3用來判斷是否陷入局部極小值陷阱，步驟5用來判斷是否逃出局部極小值陷阱，步驟4為放置虛擬障礙物。

2.2.2 放置虛擬障礙物

將機器人當作為一個圓，其圓心是機器人當前位置的坐標，記為qr，半徑為機器人外形大小的一種體現(xiàn)，記為r. 虛擬障礙物放置在該圓上，即將虛擬障礙物對機器人施加的力看作機器人自身為擺脫局部極小值陷阱而采取的行動。

為了有效地使航路點解算跳出局部極小值陷阱，虛擬障礙物和當前航路點的連線應當與當前航路點和目標位置的連線相互垂直。如圖3所示，虛擬障礙物位置1與位置2就是虛擬障礙物的備選位置。令機器人指向障礙物的向量為nR,O，機器人指向目標位置的向量為nR,G. 當航路點解算陷入局部極小值陷阱時，虛擬障礙物的放置需考慮實際障礙物的位置。需考慮的實際障礙物，其nR,O與nR,G的夾角為(0°,90°]，根據(jù)方向不同將威脅區(qū)分成順時針威脅區(qū)與逆時針威脅區(qū)。

圖3 機器人建模及威脅區(qū)確定示意圖Fig.3 Schematic diagram of robot modeling and identification of threat zone

確定虛擬障礙物位置的具體過程如下：

步驟1判斷是否陷入局部極小值陷阱，如果陷入，跳步驟2，否則結束放置。

步驟2篩選障礙物，分別計算逆時針威脅區(qū)與順時針威脅區(qū)障礙物的個數(shù)nL、nR，以及兩個威脅區(qū)中障礙物離機器人最近的距離dLmin、dRmin.

步驟3確定虛擬障礙物位置：1)nL>nR，虛擬障礙物放置在位置1；2)nLdRmin，虛擬障礙物放置在位置2；4)nL=nR且dLmin≤dRmin，虛擬障礙物放置在位置1.

(3)

若虛擬障礙物放置在位置2，即順時針90°位置，則

(4)

式中：方位角θ為90°.

3 過濾震蕩航路點

通過勢函數(shù)法解算出的路徑由于算法執(zhí)行離散化的影響不可避免地會出現(xiàn)一定程度的震蕩。即使通過虛擬障礙物法使得陷入局部極小值陷阱的航路點解算擺脫了局部極小值陷阱，但大多數(shù)情況下，虛擬障礙物法僅能夠將局部極小值陷阱問題減輕為在障礙物前出現(xiàn)有限次震蕩問題。大部分震蕩問題基本都是由于步長與障礙物影響距離設置不當所引起的，使得解算出的航路點一下子從障礙物影響距離以外進入影響距離以內，而后受到障礙物很大的斥力又一下子逃離障礙物的影響范圍，就這樣反復多次形成很大的震蕩。為了算法能較好地執(zhí)行，步長一定要遠小于障礙物的影響距離，這樣就可以降低大部分震蕩的程度。若算法執(zhí)行能夠連續(xù)化，則斥力與引力充分博弈，所獲得的航路點曲線將是非常平滑的。但由于算法程序化，離散是不可避免的，這也就導致震蕩問題一定會出現(xiàn)，通過參數(shù)設置，可以減緩震蕩的程度，但不能消除震蕩。

為此，本文提出過濾震蕩點法，不論震蕩是由什么原因引起的，只要發(fā)生震蕩，算法就會自動識別出震蕩位置，并過濾震蕩點。要過濾震蕩航路點，首先要捕捉震蕩的開始點qb與結束點qe. 設算法步長為l，k時刻的航路點位置為qk. 依次掃描所有航路點，當檢測到k時刻的航路點與k-2時刻的航路點距離ρ(qk,qk-2)l，記qe=qk-1.

當震蕩的開始點與結束點位置確定后，則開始進行震蕩點過濾。震蕩點過濾分以下兩種情況處理：

(5)

圖4 異側震蕩示意圖Fig.4 Schematic diagram of oscillation points on the different side

2)qe-qb為偶數(shù)，即qb與qe位于震蕩的同側，如圖5所示，總體航路方向是朝向目標位置的，發(fā)生震蕩即突變的點定是相對更靠近障礙物的。當ρ(qe,qb)l時，處理方式與情況1相同，將直線L(qb,qe)進行n等分，獲得n+1個新航路點。

圖5 同側震蕩示意圖Fig.5 Schematic diagram of oscillation points on the same side

通過過濾震蕩航路點，所獲得的新航路長度為

stot=(總航路點數(shù)-1)×l+∑md+∑ms,

(6)

式中：總航路點包括起始點與目標點。

4 仿真結果與分析

假設機器人以恒定速度移動，虛擬力僅改變其運動方向。在一個10 m×10 m的開放空間中，有若干個圓形障礙物，機器人出發(fā)位置為(0 m, 0 m)，目標點位置為(10 m, 10 m)。勢函數(shù)中參數(shù)設定為：障礙物的影響距離ρ0為1 m，算法步長l為0.2 m，m=n=2，ξ=1，η=0.1.

當采用傳統(tǒng)人工勢場法時，如圖6和圖7所示，航路點解算陷入局部極小值陷阱，由于步長的存在即航路解算離散化，因此所解算出的航路點在局部極小值點附近來回震蕩而無法到達目標位置。

圖6 傳統(tǒng)人工勢場法的仿真結果Fig.6 Simulated result of path planning by traditional artificial potential field method

圖7 傳統(tǒng)勢函數(shù)法執(zhí)行過程中與目標點距離的變化Fig.7 Change of distance from the target point during the implementation of traditional potential function method

采用結合虛擬障礙物法的人工勢場法進行航路規(guī)劃，如圖8和圖9所示。算法自動識別出航路點解算已陷入局部極小值陷阱，再根據(jù)當前態(tài)勢，確定虛擬障礙物的位置并放置虛擬障礙物。由于虛擬障礙物的出現(xiàn)，導致原本的力平衡狀態(tài)被打破，航路點解算成功逃離局部極小值陷阱。但由于算法執(zhí)行的離散化，虛擬障礙物法僅將局部極小值陷阱問題減輕為在障礙物前出現(xiàn)有限次震蕩問題，此時該算法所得路徑的長度為25.8 m.

圖8 結合虛擬障礙物法的勢函數(shù)法仿真結果Fig.8 Simulated result of path planning by potential function method combined with virtual obstacle method

圖9 使用虛擬障礙物法后與目標點距離的變化Fig.9 Change of distance from target point by virtual obstacle method

在使用虛擬障礙物法去解決局部極小值陷阱問題的同時，采用震蕩點過濾的方法，如圖10和圖11所示，震蕩點被成功消除，該算法解算出的路徑長度為17.1 m.

圖10 過濾震蕩點后的仿真結果Fig.10 Simulated result of path planning after filtering the oscillation points

圖11 過濾震蕩航路點后的與目標點距離變化Fig.11 Change of distance from target point after filtering the oscillation points

仿真結果表明，虛擬障礙物法能夠有效地使陷入局部極小值陷阱的航路點解算成功逃脫陷阱并順利到達目標位置，但是由于算法執(zhí)行離散化，航路點震蕩問題仍然不可避免，通過本文提出的過濾震蕩點法，可以有效地消除震蕩航路點，使規(guī)劃的路徑更為平滑。

5 結論

本文針對人工勢場法存在的4個問題，對其進行原因分析，總結出導致這4個問題的根本原因是算法自身不可避免地存在局部極小值以及算法執(zhí)行不可避免地離散化。針對兩個根本原因，本文在不改變傳統(tǒng)人工勢場法的基礎上：1)提出航路點解算陷入、逃出局部極小值陷阱的判斷標準；2)引入虛擬障礙物概念，提出虛擬障礙物位置確定的標準；3)提出過濾震蕩點法，對解算出的路徑進行震蕩點過濾，得到相對平滑的路徑。在相同仿真環(huán)境下，對傳統(tǒng)勢函數(shù)法、引入虛擬障礙物的勢函數(shù)法以及結合過濾震蕩點的虛擬障礙物勢函數(shù)法進行Matlab仿真。仿真結果證明，虛擬障礙物法和過濾震蕩點法能夠很好地解決局部極小值陷阱問題與航路點震蕩問題。相比改變勢函數(shù)模型來克服缺陷的改進方法，本文提出的方法在保留最純粹勢函數(shù)模型的基礎上，僅從算法執(zhí)行機制入手對人工勢場法進行改進，保留了人工勢場法數(shù)學描述簡潔且易于執(zhí)行的優(yōu)勢。