基于鴿群優化算法的實時避障算法

李霜琳，何家皓，敖海躍，劉燕斌

（南京航空航天大學 航天學院，南京210016）

路徑規劃是指在有障礙物的環境下向機器人提供有關達到特定目標信息的任務［1］。對于移動機器人，路徑規劃與避障十分重要，不合適路徑可能會產生碰撞導致機器人的損壞，或大量時間的浪費［2］。

一般來說，路徑規劃問題分為全局避障和實時避障（或稱局部路徑規劃）。全局避障是指在障礙物完全已知的環境中，尋找可行路徑。該領域已有許多較為成熟的算法，如人工勢場法、A*算法［3］及多種智能避障算法［4-7］等。實時避障算法是指在進行路徑規劃的過程中，根據傳感器感知到的環境信息，對地圖進行實時更新并連續規劃得到一條合乎要求的路徑［8］。與全局避障相比，實時避障的應用范圍更廣，國內外的許多學者已對實時避障做了大量研究。

現有的實時避障算法包括人工勢場法、模糊控制法、向量場直方圖（VFH）法、人機合作、圓形擴張（CSE+）法等。其中，人工勢場法通過引入與障礙物間的斥力來防止碰撞，但易陷入局部最小值而無法運動到終點［9］；利用Q學習算法進行路徑規劃，通過多次試錯，獲得最優軌跡。而后又出現了多種改進方法提高算法速度、改進路徑冗余問題［10-12］，但仍存在實時性差、運行時間受障礙物數量影響的問題；模糊控制法的實時性較好，但易陷入U型死鎖。之后人們對利用模糊控制法進行實時避障的改進在一般環境中的性能不好［13］，存在路徑冗余問題［14］。VFH法［15］以機器人為中心建立一維直方圖表示不同方向的行進代價，從而確定前進方向。之后的改進版本包括VFH*、VFH+［16-17］，考慮不同尺寸的機器人，并通過在算法中進行前期驗證，減少陷入死胡同的風險。但在障礙物分布稀松的環境中，算法對至多有3個候選方向［18］，會導致更優路徑丟失。張帥等［19］提出利用人機合作來保證實時避障過程中的安全性，但該方法的自主性差。圓形擴張（CSE+）法［20］通過圓形的不斷膨脹在遞歸過程中連續擴展圓扇形區域獲得安全路徑，當障礙物為點時效果很好。隨后提出的CSE+法［21］，考慮機器人尺寸，可在圓形障礙物之間尋找可行路徑。這2種算法在障礙物非常密集時效果較好，但障礙物分布松散時，只有一種選擇的方向，會造成更優的路徑丟失。

基于上述研究，本文提出了融合鴿群優化算法和CSE+法的實時避障算法，利用鴿群優化算法在障礙物分布稀松，安全范圍內尋找下一時刻位置從而確定行進方向，使得所提算法可找到當前位置的最優路徑，在保證安全性的同時，大大改進在障礙物分布稀松時規劃的路徑。

1 在線避障算法的實現

本節介紹基于鴿群優化算法的二維實時避障算法的實現方法，并對CSECSE+法與鴿群優化算法的實現過程進行了介紹。

1.1 CSE+法

CSE+［2］法的基本思想為：利用圓的擴張尋找并避免障礙物，如圖1所示。在由圓形障礙物隨機填充的環境中找到到達目標位置的可行路徑。通過尋找障礙物并從障礙物構成的通道中穿過，根據與目標的方位角選擇向左或向右通道，重復這一過程而不斷接近目標位置。

圖1顯示了被障礙物包圍的機器人，圖1（a）中機器人從起點開始膨脹尋找障礙物，其中藍色箭頭轉向為圓的膨脹方向；圖1（b）中沿與障礙物1的圓心方向膨脹確定障礙物2；圖1（c）中穿過障礙物1、2構成的通道繼續向前膨脹確定障礙物3；圖1（d）中進行左右轉向選擇，轉向后繼續膨脹確定新的障礙物，其中虛線為障礙物間構成的通道。

圖1 CSE+法避障原理Fig.1 Obstacle avoidance principle of CSE+method

1.2 鴿群優化算法

研究表明，鴿子可通過地磁場信息、太陽高度信息和地標信息這3個導引工具輕松歸巢。受這一現象啟發，段海濱和喬沛鑫［21］于2014年提出了鴿群優化算法，經驗證，該算法可有效解決數值設計、參數優化等問題［22-23］。

在鴿群優化算法中，第i只鴿子的位置可表示為Xi，速度為Vi，設路徑分為n-1段。

鴿群優化算法中，利用地圖羅盤算子與地標算子分別進行迭代更新以優化鴿群的速度和位置，最終選擇獲得種群中的最優信息。

式中：R為地圖羅盤算子；rand為［0，1］間的隨機數。

地標算子的更新方法為：根據適應度值大小將不同鴿子得到的路徑進行排序。具體的迭代更新方法如下：以第i只鴿子在第t次迭代中為例。

1.3 融合鴿群優化算法的二維實時避障算法

在該算法中，用一個半徑為Rv的圓來擬合移動機器人。與一般的CSE+［2］法相同，將以圓形障礙物作為輸入，忽略環境、數據通信等影響。從移動機器人的當前位置出發到達目標位置，在前進的過程中利用傳感器感測環境中的障礙物，并表示為半徑不同的圓。從出發穿過2個障礙物后，每次由最近的3個障礙物作為安全范圍，安全范圍表示方法如圖2所示，由虛線圍成的三角形為當前安全范圍。

圖2 安全范圍示意Fig.2 The graph of current safe area

當前障礙物分布密集時，即安全范圍較小時，按照原始CSE+［2］法，根據Apollonius相切問題確定相切圓，移動機器人運動到相切圓圓心。否則將利用鴿群優化算法在安全范圍內尋找下一目標位置。而后進行轉向判斷，膨脹尋找新的障礙物。不斷重復這一過程，直到機器人運動到達終點。

由于機器人有一定的半徑，對可通行寬度有一定要求。在轉向選擇時需對通道寬度進行判斷，判斷方法如下：

式中：L13和L23分別為障礙物1、3和障礙物2、3構成的通道；（x1，y1）、（x2，y2）和（x3，y3）分別為障礙物1、2、3的圓心坐標；r1、r2、r3分別為障礙物1、2、3的半徑；Lw為預設可通行的最小寬度。

當滿足式（10）時，選擇障礙物1和3所構成的通道前進；滿足式（11）時，選擇障礙物2和3所構成的通道前進；滿足式（12）時，根據文獻［12］中所提供的方法，根據與目標位置的方位角進行轉向判斷；滿足式（13）時，機器人將掉頭，嘗試上一次轉向中的另一通道，而算法將返回至搜索樹的上一級。

搜索樹的建立與使用方法如下：

移動機器人每到達一個位置，將存儲障礙物1、2、3的信息、機器人的位置信息以及轉向判斷信息到搜索樹中。當轉向判斷滿足式（12）時，轉向選擇結果記為“1”，表示存儲在搜索樹的左分支，可以選擇另一方向通行；當轉向選擇滿足式（10）或式（11），轉向選擇記為“0”，表示存儲在搜索樹的右分支，不可選擇另一方向通行。若算法需要返回搜索樹上一級時，需返回到轉向選擇為“1”的那一級，選擇與轉向判斷計算相反的通道，并將轉向結果更改為“0”。搜索樹使用方法如圖3所示，其中數字表示轉向選擇，箭頭表示返回方向，紅標數字表示轉向更改。

圖3 搜索樹返回示意圖Fig.3 Schematic diagram of return of search tree

圖4 基于鴿群優化算法的實時避障算法流程圖Fig.4 Flowchart of real-time obstacle avoidance algorithm based on pigeon-inspired optimization

步驟1從起點出發向前膨脹直到找到障礙物1，如圖1（a）所示；沿障礙物1與起點方向延長線進行膨脹確定障礙物2，如圖1（b）所示，若構成的通道不可通行，即L12＜Lw，調整膨脹方向重新尋找障礙物2。確定障礙物1、2后執行步驟2。

步驟2從障礙物1、2所構成的通道中穿過進行膨脹尋找障礙物3，如圖1（c）所示。并確定由障礙物構成的安全范圍A的大小。當SA＜Sref時進行步驟3，否則將進行步驟4。

步驟3經過障礙物1、2所圍成的通道到達與3個障礙物相切的相切圓的圓心，后執行步驟7。

步驟7判斷是否可直接達到目標位置，不可以時進行步驟8。

步驟8記錄當前位置與障礙物信息至搜索樹中。

步驟9進行轉向判斷。若可進行轉向，選擇構成當前通道的2個障礙物作為新的障礙1、2，并進行步驟3；若不可轉向，將進行步驟10。

步驟10標記當前通道不可行，機器人掉頭返回上一層搜索樹，選擇另一條通道，若仍不可行，則機器人再一次掉頭并返回搜索樹更上一層再次重新執行步驟10；若可行將進行步驟2。

將利用適應度函數ftc計算得到的適應度值作為目標位置的成本J，在使鴿群優化算法中以成本最小為指標選擇下一目標位置。將考慮包括障礙物對機器人產生的威脅成本，以及為得到更平滑的軌跡將考慮機器人的轉彎成本。

在進行下一目標位置選擇時，應先保證移動機器人的安全性，在進行路徑規劃時不能進入受當前障礙物影響的區域，否則將威脅值取為無窮大。而當移動機器人不在受障礙物影響的禁區時，機器人距離障礙物越遠越安全。當前檢測到的每一個障礙物對該點的威脅值計算方法如下：

當前已檢測到的障礙物對該點產生的威脅成本Jo的計算方法為

其中轉向角成本Jθ的計算方法為

式中：Zθ為運行到該點時所需轉向角。

因此，該點的成本為

2 仿真和實驗

在本節中利用對所提算法的仿真，在圓形障礙物分布的環境中對融入鴿群優化算法前后實時避障算法性能進行測試，并對所提算法在狹長通道中的路徑規劃能力與死角檢測進行驗證。

2.1 鴿群優化算法融入前后對比

以半徑為0.05m的移動機器人在圓形障礙物隨機填充的環境中從出發點（-50，0）m找到能到達目標位置（60，0）m的路徑。其中黑色填充圓形為障礙物，紅色與藍色軌跡分別為融入鴿群優化算法前后的移動機器人的路徑。

圖5為障礙物分布密集時，搜索得到的路徑。圖6為障礙物分布疏松時，搜索得到的路徑。表1為在不同障礙物分布的環境下，融入鴿群優化算法前后搜索得到的路徑對比。在該算法中以平均轉向角來衡量路徑的曲率。

從圖5（a）中可以得到，在障礙物分布密集的環境下，直接利用CSE+法可以規劃得到較好的路徑，但路徑中仍存在一些不必要的凸起，而造成路徑的冗余。而在圖5（b）中，融合鴿群優化算法后可在原路徑上進行優化，避免部分凸起，減少冗余與轉彎次數。從圖6（a）中可以得到，當障礙分布較為疏松時，直接利用CSE+法得到的路徑，不必要的凸起與路徑冗余較多，而在圖6（b）中幾乎可以修正所有不必要的冗余。與圖6（a）得到的軌跡相比，圖6（b）中的轉彎次數顯著減少。

從表1中可見，融入鴿群優化算法后進行路徑規劃的結果，路徑長度更短，平均轉向角更小。其中在障礙分布密集時，融入鴿群優化算法后路徑長度減少到原來的93.78%、平均轉向角減少到原來的77.90%；而當障礙物分布疏松時，路徑長度減少到原來的83.86%，而平均轉向角減少到原來的57.03%。

仿真結果顯示，融入鴿群優化算法后得到路徑冗余度減少且更平滑，在障礙物分布疏松時效果尤其明顯。

圖5 障礙物分布密集時路徑Fig.5 Circumatances when obstacles are densely distributed

圖6 障礙物分布疏松時路徑Fig.6 Circumstances when obstacles are loosely distributed

表1 不同環境下2種算法參數對比Table 1 Comparison of two methods’parameters in different environments

2.2 長廊運行與死角檢測

為驗證算法在長廊中的通行能力，設計如圖7所示的障礙物分布。利用算法半徑為0.01m的移動機器人，搜索得到出發點（0，0）m找到能到達目標位置（50，-60）m的路徑。

測試結果表明，所提算法能在長廊中通行，且可檢測并避免死角。利用所提算法可有效避免陷入U型死鎖，且可實現在連續障礙環境進行路徑規劃，這為算法在室內環境中的應用提供了可能性。

圖7 長廊檢測Fig.7 Test in corridor

3 結 論

本文提出的實時避障算法在原CSE+法的基礎上巧妙地融入智能優化算法，可適用于利用圓形物體進行建模的移動機器人在障礙物周圍找到可通行路徑，如以樹木石頭為主的障礙物。此外，可通過將墻壁等其他不規則障礙物視為由多個小的圓形障礙物組成，而實現室內的實時避障。

根據測試結果顯示，與原算法相比，本文算法能找到更好的路徑。算法的優點如下：

1）可避免大量路徑冗余。

2）尋找得到的路徑更平滑。

3）避障機器人的轉向次數大大減少。

4）可有效避免陷入U型死鎖，可應用于連續的障礙物環境。