改進遺傳算法與領航跟隨法的機器人編隊方法

江 濤，張志安，程 志，李金芝，陸 靜

南京理工大學 機械工程學院，南京210094

1 引言

近年來，隨著機器人技術、通信技術和自動控制技術的不斷發展，多機器人協同控制研究引起了眾多領域研究者的關注，并在軍事、空間探索、交通控制與服務行業等領域展現了廣闊的應用前景[1]。作為多機器人系統協同控制中最基礎和最重要的研究問題之一，編隊控制要求多個移動機器人同時運動到目標區域，并在運動過程中保持設定的隊形，同時能安全地避開障礙物[2]。

移動機器人的路徑規劃是指：在障礙物環境中，根據評價指標（如移動時間、路徑長度、能量消耗等）和周圍環境信息自主地搜索出一條從起始點到目標點的安全無碰撞且較優的可行路徑[3]。合理的路徑規劃算法是移動機器人研究的基礎性問題，不僅是研究的核心，更是制約移動機器人發展的瓶頸[4]。目前移動機器人路徑規劃常用的優化算法主要有人工勢場法、A*算法、粒子群算法和遺傳算法等[5]。遺傳算法是一種采用種群搜索機制的全局性尋優算法，由于遺傳算法具有并行性、全局搜索能力等優點，使得求取最優解的可能性和效率大大提高，因此被國內外學者廣泛應用到求解路徑規劃問題中[6]。

多機器人編隊控制是指多個機器人向目標點運動的過程中，既能夠收斂于特定的隊形，又能適應環境約束的控制技術[7]。目前多機器人的編隊控制方法主要有虛擬結構法、領航跟隨方法、人工勢場法、基于行為法等[8]。由于領航跟隨法控制簡單，易于工程實現，因此被廣泛應用于編隊控制中。

文獻[9]利用啟發式方法和隨機方法相結合的策略加快了收斂速度；文獻[10]對遺傳算法進行自適應改進，計算出能夠隨時適應的遺傳算子，以改進基本遺傳算法陷入局部最優問題；文獻[11]提出了一種基于障礙物的路徑搜索方法，有效地縮小了算法的搜索空間；文獻[12]提出了一種多次隨機交叉法來維持種群的多樣性，提高了算法尋找全局最優解的概率。

本文采用的是遺傳算法與領航跟隨法相結合的方法進行編隊。傳統遺傳算法收斂速度較慢，且規劃的路徑沒有充分考慮路徑平滑度要素，因此規劃出的路徑轉彎多，不利于機器人運動。本文對遺傳算法進行了改進，在適應度函數中加入了路徑平滑度因素，使轉彎次數少的路徑具有較高的適應度函數值，在進行選擇操作時更容易被選中，然后通過交叉操作將優良基因傳到下一代群體中保留下來，提高了路徑的順滑度；在種群選擇時加入了精英保留策略，適應度函數值最大的個體可以不進行交叉變異操作，而是直接復制到下一代種群個體中，防止破壞父代個體的優良基因，提高算法收斂速度。通過MATLAB軟件建立柵格地圖進行仿真，仿真結果表明：與傳統遺傳算法相比，改進后的算法可以更快地收斂到最短路徑，且路徑平滑度較好。規劃出最優路徑后，使用領航跟隨法控制跟隨機器人的運動，首先根據領航機器人的位姿信息以及隊形實時計算每臺虛擬機器人的位置，然后控制每臺跟隨機器人運動到虛擬機器人的位置從而形成設定的隊形。

2 遺傳算法路徑規劃

2.1 遺傳算法模型

遺傳算法是模仿自然界生物進化“優勝劣汰，適者生存”法則構建起的一種隨機全局搜索和優化方法，具有良好的并行性和可擴展性。圖1為遺傳算法路徑規劃流程圖。

圖1 遺傳算法路徑規劃流程圖

遺傳算法在求解最優路徑時，先將可行解通過某種方式形成染色體，隨機選擇一部分個體生成初始種群；然后通過一定方式將目標函數轉化為適應度函數，并通過計算個體的適應度函數值的大小來對個體進行選擇；最后再對個體進行交叉和變異操作，以產生適應度函數值更大的個體；通過不斷的繁衍后代，使后代更加適應環境，直到滿足期望的終止條件，從而形成種群的最優解。

2.1.1 路徑編碼

本文采用柵格法建立移動機器人的地圖模型。用正方形表示機器人的行走空間，有障礙物的柵格稱為障礙物柵格，用黑色填充；無障礙物的柵格稱為自由柵格，用白色填充[13]。

常用的柵格標記法有坐標法和序號標記法[14]。坐標法標號時每一個柵格用(x,y)的坐標形式表示。序號標記法由柵格左下角為0開始，按照由左到右，由下到上的順序依次加一進行標記。序號N和坐標(x,y)的轉換公式為：

其中，N為使用序號標記法標記的柵格序號，Gsize為每一行所包含的柵格數量，int為取整操作，%為取余操作。

圖2柵格地圖中所示的折線表示為一條能使機器人從點（1，1）運行到點（6，6）的可行路徑，路徑編碼為（0，6，12，18，25，32，33，34，35）。

圖2 可行路徑

2.1.2適應度函數計算

在遺傳算法中，適應度函數決定了一個個體被遺傳到下一代的概率。路徑規劃的目的是使機器人從起始點運動到目標點的距離最短，因此在進行適應度函數的設計時要滿足機器人運行軌跡越短適應度值越大。在一個路徑個體中，路徑總長度Length的計算公式如下：

其中，end為機器人行走的步數。

路徑適應度函數為：

由上式可以看出，路徑越短，適應度值越大，則該路徑被保留下來的概率就越大。

2.1.3 選擇操作

選擇的目的是將滿足要求的個體保留，不滿足要求的個體剔除。選擇方法基于上一步中計算出的適應度函數值，采用輪盤賭法。

輪盤賭的方式保證了部分非最優的個體，可以有效地防止算法陷入局部最優解[15]。

2.2 遺傳算法改進

2.2.1 適應度函數加入路徑平滑度

由于運動學和動力學的約束，機器人行進時拐彎不宜過大，相對平滑的路徑更加有利于機器人的行駛[16]。在使用傳統遺傳算法進行路徑規劃時，機器人只是把路徑最長度作為適應度函數的決定因素，造成生成的路徑拐彎次數過多，機器人運動時間和耗能大大增加，同時還有撞擊障礙物的危險。因此本文在計算路徑適應度函數時考慮了路徑平滑度因素。因此對適應度函數做出了修改，新的適應度函數如下：

其中，適應度函數的第一部分為傳統遺傳算法的適應度函數：

由幾何關系可知，路徑越平滑，相鄰三點形成的角度越大，角度越大相鄰三點之間的距離越大，因此需要計算路徑中所有相鄰三點的距離，計算公式如下：

d3值大小的不同可表示路徑中連續三點的夾角為180°、鈍角、直角和銳角這四種情況，其中夾角為180°時三點連成一條直線，平滑度最好，鈍角和直角次之。由于機器人動力學的約束，本文算法中設定不允許出現銳角的情況，所以分別對鈍角、直角和銳角給予5、30和1 000的懲罰值Punish。整條路徑的懲罰值總和為：

適應度函數的第二部分fit2=1 Punish總，根據路徑長度和平滑度之間的權重關系，適應度函數的兩部分需要各取一個權重參數a和b。

2.2.2 選擇使加入精英保留策略

精英保留策略的基本思想是對種群中適應度函數值最高的個體進行保留，不對其進行遺傳操作而直接將其遺傳到下一代之中。在遺傳算法中，遺傳操作在隨機進行的過程中可能對優良個體進行遺傳操作進而破壞其優秀的染色體結構，進而降低種群的平均適應度值，對算法的運算效率和收斂性都將產生不好的影響，因此希望擁有最高適應度值的個體盡可能地遺傳到下一代。為了實現這一目標，采用精英選擇方法來執行選擇操作。通過使用這種選擇方法，遺傳過程中某一代的最優解可以不參加遺傳操作而直接遺傳到下一代，保證其不被交叉和變異等遺傳操作破壞。

3 領航跟隨法編隊控制

相比于單臺機器人而言，多機器人協同編隊在靈活性、可拓展性、魯棒性等方面具有更好的優越性。目前機器人編隊中最常用的算法主要有領航跟隨法、基于行為法、虛擬結構法等方法[17]。領航跟隨法采用鏈式的拓撲結構，整個隊形由領航機器人和跟隨機器人組成，在實施過程中只需要使跟隨者獲得領航者的運動狀態信息并對領航者的運動軌跡進行跟蹤便可實現編隊控制[18]。領航跟隨法由于控制器的設計相對簡單，被廣泛應用于編隊算法中。

本文中編隊控制采用的算法是領航跟隨法。首先需要選定一臺機器人作為領航者，主要負責整個編隊的路徑規劃任務，根據地圖信息規劃出一條從起點到終點的無碰最優路徑。其余機器人則被視為跟隨者，跟隨機器人負責跟蹤領航者進行運動，并盡可能與領航機器人之間保持隊形所需要的距離和角度。多臺機器人形成編隊時，編隊中的每臺機器人不僅要避開障礙物安全抵達目的地，而且要避免與隊形中其他成員發生碰撞[19]。

領航跟隨法主要有l-l控制和l-φ控制兩種控制模式。l-l控制即每個跟隨機器人以固定的距離l跟隨兩個領航機器人，從而保持期望的隊形；l-φ控制即每個跟隨機器人以一定的距離和角度跟隨領航機器人，實現期望隊形[20]。本文采用l-φ控制方法。算法實現步驟如下：

步驟1領航機器人根據地圖信息使用改進遺傳算法規劃出自身的最優路徑。

步驟2領航機器人沿著最優路徑向目標點運行，并利用設計好的算法進行避障，同時，領航機器人實時檢測自身的位姿信息，根據設定的隊形使用l-φ控制法生成虛擬機器人的軌跡，并發送給跟隨機器人。

步驟3跟隨機器人實時接收領航機器人發送的軌跡，調整自己的速度和運動方向，使沿著虛擬機器人的軌跡運行。同時，跟隨機器人實時檢測自身周圍的障礙物信息，當跟隨機器人遇到障礙物時，會在原地進行一定時間的等待，當領航機器人通過后，跟隨機器人會運動到領航機器人上一時刻的位置，通過障礙物后，跟隨機器人再回到虛擬機器人的軌跡上，恢復設定隊形。

4 仿真

4.1 路徑規劃仿真

為了驗證本文改進的遺傳算法地有效性，建立20×20的柵格地圖環境模型對該算法進行仿真，并與傳統遺傳算法進行比較。設定機器人起始點坐標為（0，0），目標點坐標為（20，20），種群規模為100，最大迭代次數為50代，交叉概率為0.9，變異概率為0.002。實驗結果如圖3、圖4所示。

從圖3（a）和圖4（a）可以看出，改進后遺傳算法規劃出的路徑轉彎次數明顯減少，路徑平滑度比改進前有很大改善。從圖3（b）和圖4（b）可以看出，傳統遺傳算法和改進遺傳算法都收斂到了最短路徑，路徑長度都為29.799，但傳統遺傳算法用了15次迭代，改進后的遺傳算法6次迭代就收斂到了最短路徑。

為了避免偶然性因素，如圖5（a）所示，在柵格地圖左下角4×4區域內隨機選取自由柵格作為起始點，在地圖右上角4×4區域內隨機選取自由柵格作為目標點，將傳統遺傳算法和改進遺傳算法分別運行100次進行仿真對比，觀察各個算法收斂至最短路徑所需的迭代次數。

圖3 傳統遺傳算法

圖4 改進遺傳算法

圖5 算法改進前后收斂至最短路徑迭代次數

由圖5（b）可知，傳統遺傳算法需要經過13～24次迭代收斂到最短路徑，平均迭代次數為17.85次；但使用改進后的遺傳算法，只需5～15次便能收斂到最短路徑，平均迭代次數為9.93次。

本文又選取了文獻[9]、文獻[10]中所述的遺傳算法與傳統遺傳算法和改進后的遺傳算法進行對比，觀察四種遺傳算法的收斂速度。

將四種算法運行100次統計平均迭代次數與最終路徑的平均長度，得到的數據如表1所示。

表1 各算法迭代次數與路徑長度對比

從圖6和表1中可以看出，文獻[9]中所用改進的遺傳算法收斂速度非常快，能夠迅速收斂到接近最優解的值，但最終并沒有收斂到最優解；本文改進的遺傳算法比傳統遺傳算法和文獻[10]中改進的遺傳算法都收斂到了最優解，但本文平均只用了9.93次便收斂到了最優解，速度更快。

圖6 四種遺傳算法性能對比

實驗證明，與傳統遺傳算法、文獻[9]中遺傳算法和文獻[10]遺傳算法相比，改進的遺傳算法具有較快的收斂速度，且規劃出的路徑平滑度較好，能夠縮短規劃路徑所需的時間和機器人運動所需的時間，且運行更加安全，綜合性能較優。

4.2 多機器人編隊仿真

本文以三個機器人組成三角形隊形為例進行驗證。設定領航機器人的起始點坐標為（2，2），目標點坐標為（20，20），兩個跟隨機器人的起始點坐標分別為（1，2）和（2，1），目標點坐標分別為（19，20）和（20，19）。

仿真1領航者和跟隨者都沒有遇到障礙物，機器人沿直線直接從起點運動到了終點，所有機器人全程保持了三角形隊形，軌跡圖如圖7所示。

仿真2領航機器人遇到障礙物，跟隨者沒有遇到障礙物。在起點與終點的連線上增加了一個障礙物，當領航機器人運動到障礙物面前會繞過障礙物尋找一條新的路徑到達目標點，且跟隨機器人會跟隨著機器人繞開障礙物，且運動過程中始終保持著三角形隊形運動到目標點，軌跡圖如圖8所示。

圖7 無障礙環境下路徑規劃軌跡圖

圖8 領航者遇到障礙環境下路徑規劃軌跡圖

仿真3領航者沒有遇到障礙物，跟隨者遇到障礙物。當跟隨機器人的路徑上出現了障礙物，編隊會進行短時間的變換隊形，跟隨機器人會在障礙物面前原地等待一段時間，待到領航機器人通過后，跟隨機器人會移動到領航機器人上一時刻的位置，當跟隨者繞過障礙物以后，跟隨機器人重新回到虛擬機器人的軌跡上，恢復設定隊形，軌跡圖如圖9所示。

圖9 跟隨者遇到障礙環境下路徑規劃軌跡圖

仿真4只能單個機器人通過時，兩個跟隨機器人會在障礙物前等待，當領航機器人通過狹窄通道后，跟隨機器人會依次運動到領航機器人的位置，呈“一”字形挨個通過狹窄通道，當所有機器人通過狹窄區域后，再恢復至設定隊形，軌跡圖如圖10所示。

實驗表明，本文設計的領航跟隨法能使所有機器人保持隊形安全無碰撞地運行到目標位置。當機器人遇到障礙物時，能夠及時地變換隊形來避開障礙物，完成避障任務后再重新恢復隊形。

圖10 狹窄環境下路徑規劃軌跡圖

5 結束語

本文將遺傳算法和領航跟隨法結合，并對遺傳算法進行一定的改進，將平滑度要素加入到適應度函數中，并在選擇操作時加入了精英保留策略，在存在障礙物的柵格地圖中使用改進后的遺傳算法規劃出了最優路徑，實驗證明，通過多次迭代，最優路徑長度迅速收斂并最終趨于穩定，而且相比于傳統遺傳算法，改進后的遺傳算法收斂更快，規劃出的路徑也更加平滑。

從隊形保持、隊形變換來看，當沒有遇到障礙物時，多機器人可以始終保持隊形從起始點運動到目標位置，當機器人碰到狹窄通道或者遇到障礙物時，會進行適當的隊形變換以躲避障礙物，通過狹窄通道或者繞開障礙物后，多機器人又恢復成設定的隊形向目標點前進，最終到達指定位置。