基于螢火蟲算法動態未知環境的路徑規劃

李鳳玲，陳 珊，范興江，劉 源

（長沙理工大學 汽車與機械工程學院，長沙410114）

路徑規劃是移動機器人實現自主導航的關鍵技術之一。路徑規劃是指在確定性環境或未知環境中，移動機器人根據自身攜帶的傳感器感知周圍環境，在避開障礙物的前提下，按照設定的路徑規劃算法尋找一條從起始點到目標點的無碰撞最優路徑[1]。根據路徑規劃環境是否隨時間變化來分，可以將移動機器人的路徑規劃分為靜態已知環境的路徑規劃和動態未知環境的路徑規劃，靜態路徑規劃也被稱作全局路徑規劃，動態路徑規劃被稱作局部路徑規劃[2]。動態未知環境中的移動機器人路徑規劃，一直是機器人路徑規劃領域的重點與難點，因為需要機器人系統具有較強的自適應不確定環境的能力。目前，國內外常應用于動態環境的局部路徑規劃方法主要有：人工勢場法[3]、強化學習[4]、神經網絡[5]、模糊算法[6]和其它智能算法[7]等。雖然這些方法大部分可以合理地解決路徑規劃問題，但普遍存在算法復雜、內存開銷大、實時性差等問題。

螢火蟲算法FA 是一種自然啟發式算法，依據自然界螢火蟲的發光特性而來[8～9]。目前，螢火蟲算法憑借算法參數較少、操作簡單、且具有較好的穩定性等優勢已廣泛應用到移動機器人的路徑規劃中。文獻[10]提出一種自適應螢火蟲算法，為保證初始的全局搜索能力與后續的局部搜索能力，將FA的吸引系數與隨機系數根據迭代次數而改變，并證明了算法的效率，但此文獻將螢火蟲算法應用于靜態已知環境的路徑規劃中，沒有考慮動態環境的情況。文獻[11]提出一種基于文化算法框架的螢火蟲優化算法，并應用于足球機器人的動態路徑規劃中。文中建立機器人與障礙物之間的碰撞預測模型，通過仿真實驗證明所提出的算法具備有效性，但沒有將該算法嵌入實際機器人系統中，算法的實時性有待驗證。文獻[12]提出一種基于螢火蟲算法的不確定環境下的路徑規劃方法，并驗證了該方法能較好地追尋動態目標點，及避開動態障礙物的情況。但沒有考慮環境中同時存在靜態障礙物與動態障礙物的復雜情形，并且實驗內容也沒有涉及動態障礙物的場景。

總之，智能螢火蟲算法在移動機器人的路徑規劃中有很好的應用前景，但目前還存在算法嵌入機器人系統的實時性、缺乏實際實驗驗證等問題。本文采用單目相機檢測機器人周圍環境信息，并設計螢火蟲算法的適應度函數，讓機器人在避開障礙物的同時能搜索較優路徑。最后，通過Matlab 仿真平臺與樹莓派機器人系統，驗證了螢火蟲算法在動態未知環境中路徑規劃的有效性與實時性。

1 動態路徑規劃方法設計

1.1 螢火蟲算法基本原理

螢火蟲算法的靈感來源于螢火蟲的閃爍行為，其閃光行為被當作一個特殊信號，并憑借此信號進行螢火蟲個體間的覓食、求偶、警戒等信息交流。螢火蟲算法的核心思想為在螢火蟲的感知范圍內，螢火蟲被亮度比它大的螢火蟲所吸引，并根據螢火蟲算法的位置更新公式，得到螢火蟲運動的方向與距離，使螢火蟲移動到新的位置；而種群中亮度最大的螢火蟲則隨機移動。其標準螢火蟲算法的數學模型，包括以下幾個關鍵公式。

螢火蟲的相對亮度公式：

式中：Iij為第i 只螢火蟲與第j 只螢火蟲之間的相對亮度；Ii為第i 只螢火蟲在r=0 處的亮度，稱為第i只螢火蟲的絕對亮度；γ 為光吸收系數，一般為常數；rij為第i 只螢火蟲與第j 只螢火蟲之間的距離。

螢火蟲的吸引度公式：

式中：β0為最大吸引度因子，一般為常數。

螢火蟲的移動公式：

式中：xi、xj分別表示第i 只螢火蟲與第j 只螢火蟲的空間位置；α 為步長因子，一般為常數；rand∈［0，1］之間的隨機數。

1.2 障礙物的檢測與識別

移動機器人動態環境下的路徑規劃，是路徑規劃研究領域中的重要組成部分。移動機器人需要通過自身傳感器檢測周圍環境，識別環境中的障礙物并計算與障礙物之間的距離。在移動機器人路徑規劃的仿真實驗中，機器人對障礙物的檢測分為以下步驟：

步驟1設置機器人的檢測范圍。當機器人處于未知環境中時，將機器人的運動方向設定為正面朝向，然后設定角度θ，θ 為機器人運動方向與最大檢測角度范圍的夾角，則機器人的障礙物檢測范圍如圖1所示。

圖1 移動機器人障礙物的檢測Fig.1 Detection of obstacles in mobile robots

步驟2計算在檢測范圍內與障礙物的距離。在指定的范圍內搜索障礙物，一旦檢測到障礙物的存在，便返回機器人與障礙物的距離信息。

步驟3判斷是否需要啟動螢火蟲算法。通過步驟2 檢測到障礙物，并返回機器人與障礙物的距離，當它們之間的距離小于設定閾值時，則啟動螢火蟲算法進行避障處理；反之則機器人向目標點移動。

1.3 適應度函數的設計

在絕大多數智能算法中，其適應度函數的設計直接影響算法的收斂速度，甚至影響算法是否能找到最優解，所以，智能算法中適應度函數的合理設計具有重要意義。在基于螢火蟲算法動態環境的路徑規劃中，在檢測到障礙物的附近隨機產生一定數量的螢火蟲個體，并且基于螢火蟲亮度在種群中選擇更亮的螢火蟲個體，使其與最近的障礙物保持最大的安全距離。螢火蟲個體的亮度大小通過適應度函數計算得到，所以適應度函數的合理設計顯得尤為重要。其適應度函數的設計包括以下幾個關鍵公式。

螢火蟲個體與最近障礙物之間的距離公式：

式中：Dfo為螢火蟲與離它最近的障礙物之間的歐幾里得距離；xfi與yfi分別表示螢火蟲fi在地圖坐標系中x 與y 的值；x0與y0分別表示距離該螢火蟲最近的障礙物的x 與y 值。

螢火蟲個體與目標點之間的距離公式：

式中：Dfg為螢火蟲與目標點之間的距離；xfi與yfi分別表示螢火蟲fi在地圖坐標系中x 與y 的值；xg與yg分別表示目標點的x 與y 值。

未知環境中螢火蟲算法的適應度函數公式：

式中：Dfo為螢火蟲與離它最近的障礙物之間的距離；Dfg為螢火蟲與目標點之間的距離；K1與K2分別為適應度參數，一般為常數。

2 仿真與分析

2.1 仿真環境的設計

為驗證螢火蟲算法在動態環境中路徑規劃的有效性，在Matlab 軟件中對其進行仿真實驗。首先建立仿真環境模型：忽略移動機器人和障礙物的高度信息，建立二維環境地圖模型，橫坐標以向左為正方向，縱坐標以向下為正方向，地圖的大小為500 cm*500 cm。并設置移動機器人的起點位置為坐標（50，50）處，目標點位置為坐標（450，450）處。地圖模型中黑色區域為不可行障礙物，機器人只能在地圖內運動，不可溢出地圖邊界。螢火蟲算法中，設置螢火蟲的種群大小為N=50，迭代次數為10 次。

2.2 動態目標點環境

為驗證螢火蟲算法在動態目標點情形下路徑規劃的有效性，在地圖環境中設置動態目標點。動態目標點的起始位置為坐標（450，450）處，其運動方向為180°，在二維坐標系中從右至左。在移動機器人的動態環境局部路徑規劃中，無障礙環境下目標運動的路徑規劃結果如圖2所示，其目標點的運動步長為2 cm；有障礙物環境下目標運動的路徑規劃結果如圖3所示，其目標點的運動步長為1 cm。

圖2 無障礙物環境下目標運動的路徑規劃Fig.2 Path planning of target motion in non-obstacle environment

圖3 有障礙物環境下目標運動的路徑規劃Fig.3 Path planning of target motion in obstacle environment

圖2 為無障礙物情形下的仿真圖，機器人隨著目標點位置的改變，不斷修正自身的運動角度，最終到達動態的目標點位置，其路徑長度為509.36 cm。圖3 為有障礙物情形下的仿真圖，最開始機器人也是向著目標點運動，當檢測到障礙物后，便啟動螢火蟲算法進行避障處理，同時根據目標點位置的改變，修正自身運動角度，最終抵達目標點，其路徑長度為632.78 cm。

2.3 動態障礙物環境

為驗證螢火蟲算法在動態障礙物情形下路徑規劃的有效性，在地圖環境中設置動態障礙物。設置動態障礙物的初始中心位置在坐標（300，360）處，邊長為20 cm 的矩形，其運動方向為180°，在二維坐標系中從右至左。圖4 為僅存在動態障礙物的環境下，移動機器人的導航軌跡，其中，動態障礙物的運動步長為2 cm。

圖4 動態障礙物環境下的路徑規劃Fig.4 Path planning in dynamic obstacle environment

在圖4 中，最初，機器人與動態障礙物存在一定距離，沒有檢測到動態障礙物，所以直接向著目標點運動；當機器人運動到一定位置，檢測到動態障礙物時，便啟動螢火蟲算法進行避障處理；最后，當機器人避開障礙物或當障礙物走遠，機器人再次朝著目標點移動到達終點。其路徑長度為574.29 cm。

為驗證螢火蟲算法在復雜環境下路徑規劃的有效性，在存在靜態障礙物的地圖中設置動態障礙物。動態障礙物的初始中心位置在坐標（300，350）處，邊長為20 cm 的矩形，其運動方向為180°，在二維坐標系中從右至左。圖5 為復雜環境下移動機器人的運動軌跡，仿真的圖例順序為（a）→（b）→（c）→（d），其中，動態障礙物的運動步長為2 cm。

機器人在向著目標點運動的過程中，不僅檢測到了靜態障礙物，對其進行避障處理，而且對檢測到的動態障礙物，同樣有效避開。其中，圖5 中移動機器人的導航軌跡長度為590.83 cm。由上述仿真結果可知，基于螢火蟲算法的路徑規劃，能在動態復雜環境中執行，并取得較好的局部路徑規劃結果。

3 機器人系統與實驗

3.1 基于樹莓派的機器人系統

移動機器人是由各種基礎設備相結合而構造的，其主要基礎設備包括微控制芯片、傳感器、驅動裝置、電源模塊、電路連接等。在本文基于樹莓派系統的機器人中，機器人主要通過單目攝像頭檢測周圍障礙物的距離與方位信息，然后將檢測到的障礙物信息傳入樹莓派系統，經過主控芯片對外界信息的處理計算，將執行信號輸出至驅動模塊，驅動移動機器人的電機運動。機器人的系統構架如圖6所示。

圖5 復雜環境下的路徑規劃Fig.5 Path planning in complex environment

圖6 機器人系統構架Fig.6 Architecture of the robot system

3.2 單目相機的成像原理

傳感器是機器人對周圍環境檢測的工具，機器人依靠檢測到的信息控制機器人運動。在移動機器人中，一般使用單目相機、雙目相機、深度相機、或者激光雷達作為主要傳感器。單目相機以價格低廉的優勢，廣泛地應用于低成本的移動機器人中。其中，單目相機的距離測量原理如圖7所示。

主要以小孔成像原理，根據相似三角形比例計算出機器人與障礙物的距離。其中可以直接測量的數據包括相機中心的實際點到相機的水平距離d、相加的安裝高度h、相機焦距f、障礙物在成像坐標系中到中心的距離pix。其中，中心光軸與地面的夾角公式為

中心光軸與障礙物光軸的夾角公式為

圖7 單目相機的測距原理Fig.7 Principle of ranging for monocular cameras

障礙物光軸與地面的夾角公式為

由三角形原理得出機器人與障礙物之間的距離公式為

通過上述公式，便可以通過單目相機測出機器人與障礙物的距離。

3.3 實驗測試

在基于樹莓派系統的輪式移動機器人上，對動態環境中的路徑規劃進行驗證。受機器人硬件設備和場地的限制，將環境地圖簡化，即縮小環境地圖大小，并將障礙物設置稀疏。機器人的大小為15．5 cm*25．5 cm，實際場景的大小為120 cm*120 cm，障礙物的放置位置如圖8所示，其中，靜態障礙物的大小為13 cm*13 cm，動態障礙物的運動軌跡橫穿整個環境地圖。并且，只考慮二維地圖情況下的路徑規劃，不考慮輪式機器人的高度與障礙物的高度。在輪式移動機器人中，其車輪的直徑為6．5 cm，光電編碼器上的光柵盤平均分布著20 個光柵縫隙，則機器人的移動距離公式為

式中：d 為車輪直徑；N 為光柵盤上的柵格數，本實驗中為20；n 為檢測到的脈沖信號的數量。即光電編碼器每輸出一個脈沖，機器人的移動距離為1．021 cm。

這里對螢火蟲算法的動態路徑規劃進行實驗驗證。在圖8 中，分別展示了輪式機器人路徑導航過程中的4 個時刻，從時間的先后排序為1→2→3→4，可以得出，移動機器人能安全繞過靜態障礙物與動態障礙物，并選擇一條較優路徑順利到達目標點。

圖8 機器人運動過程示意Fig.8 Schematic diagram of the robot motion process

上述實驗中，仿真路徑長度為164．853 cm，實際光電編碼器電頻變換次數為170 次，即實際路徑長度為173．57 cm，實際路徑長度比仿真路徑長度增加了5．29%。由于輪式機器人設備原因及車輪直徑測量誤差，導致實際路徑長度與仿真路徑長度存在的較小差距，均可在接受范圍內。驗證了基于螢火蟲算法的動態路徑規劃是有效可行的。

4 結語

文中提出一種基于螢火蟲算法動態未知環境的路徑規劃方法。采用單目相機作為檢測距離的傳感器，感知移動機器人周圍障礙物信息；并設計一種在避開障礙物的同時搜索最優路徑的適應度函數。仿真和實驗結果均表明，移動機器人在遇到靜態障礙物與動態障礙物時，都能有效的避開環境周圍的障礙物，并搜索出一條較優的路徑達到目標點。