基于螢火蟲算法動態(tài)未知環(huán)境的路徑規(guī)劃

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

（長沙理工大學(xué) 汽車與機(jī)械工程學(xué)院，長沙410114）

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

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

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

1 動態(tài)路徑規(guī)劃方法設(shè)計

1.1 螢火蟲算法基本原理

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

螢火蟲的相對亮度公式：

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

螢火蟲的吸引度公式：

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

螢火蟲的移動公式：

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

1.2 障礙物的檢測與識別

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

步驟1設(shè)置機(jī)器人的檢測范圍。當(dāng)機(jī)器人處于未知環(huán)境中時，將機(jī)器人的運(yùn)動方向設(shè)定為正面朝向，然后設(shè)定角度θ，θ 為機(jī)器人運(yùn)動方向與最大檢測角度范圍的夾角，則機(jī)器人的障礙物檢測范圍如圖1所示。

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

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

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

1.3 適應(yīng)度函數(shù)的設(shè)計

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

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

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

螢火蟲個體與目標(biāo)點(diǎn)之間的距離公式：

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

未知環(huán)境中螢火蟲算法的適應(yīng)度函數(shù)公式：

式中：Dfo為螢火蟲與離它最近的障礙物之間的距離；Dfg為螢火蟲與目標(biāo)點(diǎn)之間的距離；K1與K2分別為適應(yīng)度參數(shù)，一般為常數(shù)。

2 仿真與分析

2.1 仿真環(huán)境的設(shè)計

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

2.2 動態(tài)目標(biāo)點(diǎn)環(huán)境

為驗證螢火蟲算法在動態(tài)目標(biāo)點(diǎn)情形下路徑規(guī)劃的有效性，在地圖環(huán)境中設(shè)置動態(tài)目標(biāo)點(diǎn)。動態(tài)目標(biāo)點(diǎn)的起始位置為坐標(biāo)（450，450）處，其運(yùn)動方向為180°，在二維坐標(biāo)系中從右至左。在移動機(jī)器人的動態(tài)環(huán)境局部路徑規(guī)劃中，無障礙環(huán)境下目標(biāo)運(yùn)動的路徑規(guī)劃結(jié)果如圖2所示，其目標(biāo)點(diǎn)的運(yùn)動步長為2 cm；有障礙物環(huán)境下目標(biāo)運(yùn)動的路徑規(guī)劃結(jié)果如圖3所示，其目標(biāo)點(diǎn)的運(yùn)動步長為1 cm。

圖2 無障礙物環(huán)境下目標(biāo)運(yùn)動的路徑規(guī)劃Fig.2 Path planning of target motion in non-obstacle environment

圖3 有障礙物環(huán)境下目標(biāo)運(yùn)動的路徑規(guī)劃Fig.3 Path planning of target motion in obstacle environment

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

2.3 動態(tài)障礙物環(huán)境

為驗證螢火蟲算法在動態(tài)障礙物情形下路徑規(guī)劃的有效性，在地圖環(huán)境中設(shè)置動態(tài)障礙物。設(shè)置動態(tài)障礙物的初始中心位置在坐標(biāo)（300，360）處，邊長為20 cm 的矩形，其運(yùn)動方向為180°，在二維坐標(biāo)系中從右至左。圖4 為僅存在動態(tài)障礙物的環(huán)境下，移動機(jī)器人的導(dǎo)航軌跡，其中，動態(tài)障礙物的運(yùn)動步長為2 cm。

圖4 動態(tài)障礙物環(huán)境下的路徑規(guī)劃Fig.4 Path planning in dynamic obstacle environment

在圖4 中，最初，機(jī)器人與動態(tài)障礙物存在一定距離，沒有檢測到動態(tài)障礙物，所以直接向著目標(biāo)點(diǎn)運(yùn)動；當(dāng)機(jī)器人運(yùn)動到一定位置，檢測到動態(tài)障礙物時，便啟動螢火蟲算法進(jìn)行避障處理；最后，當(dāng)機(jī)器人避開障礙物或當(dāng)障礙物走遠(yuǎn)，機(jī)器人再次朝著目標(biāo)點(diǎn)移動到達(dá)終點(diǎn)。其路徑長度為574.29 cm。

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

機(jī)器人在向著目標(biāo)點(diǎn)運(yùn)動的過程中，不僅檢測到了靜態(tài)障礙物，對其進(jìn)行避障處理，而且對檢測到的動態(tài)障礙物，同樣有效避開。其中，圖5 中移動機(jī)器人的導(dǎo)航軌跡長度為590.83 cm。由上述仿真結(jié)果可知，基于螢火蟲算法的路徑規(guī)劃，能在動態(tài)復(fù)雜環(huán)境中執(zhí)行，并取得較好的局部路徑規(guī)劃結(jié)果。

3 機(jī)器人系統(tǒng)與實驗

3.1 基于樹莓派的機(jī)器人系統(tǒng)

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

圖5 復(fù)雜環(huán)境下的路徑規(guī)劃Fig.5 Path planning in complex environment

圖6 機(jī)器人系統(tǒng)構(gòu)架Fig.6 Architecture of the robot system

3.2 單目相機(jī)的成像原理

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

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

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

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

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

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

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

3.3 實驗測試

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

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

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

圖8 機(jī)器人運(yùn)動過程示意Fig.8 Schematic diagram of the robot motion process

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

4 結(jié)語

文中提出一種基于螢火蟲算法動態(tài)未知環(huán)境的路徑規(guī)劃方法。采用單目相機(jī)作為檢測距離的傳感器，感知移動機(jī)器人周圍障礙物信息；并設(shè)計一種在避開障礙物的同時搜索最優(yōu)路徑的適應(yīng)度函數(shù)。仿真和實驗結(jié)果均表明，移動機(jī)器人在遇到靜態(tài)障礙物與動態(tài)障礙物時，都能有效的避開環(huán)境周圍的障礙物，并搜索出一條較優(yōu)的路徑達(dá)到目標(biāo)點(diǎn)。