改進(jìn)遺傳算法與領(lǐng)航跟隨法的機(jī)器人編隊(duì)方法

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

南京理工大學(xué) 機(jī)械工程學(xué)院，南京210094

1 引言

近年來，隨著機(jī)器人技術(shù)、通信技術(shù)和自動(dòng)控制技術(shù)的不斷發(fā)展，多機(jī)器人協(xié)同控制研究引起了眾多領(lǐng)域研究者的關(guān)注，并在軍事、空間探索、交通控制與服務(wù)行業(yè)等領(lǐng)域展現(xiàn)了廣闊的應(yīng)用前景[1]。作為多機(jī)器人系統(tǒng)協(xié)同控制中最基礎(chǔ)和最重要的研究問題之一，編隊(duì)控制要求多個(gè)移動(dòng)機(jī)器人同時(shí)運(yùn)動(dòng)到目標(biāo)區(qū)域，并在運(yùn)動(dòng)過程中保持設(shè)定的隊(duì)形，同時(shí)能安全地避開障礙物[2]。

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

多機(jī)器人編隊(duì)控制是指多個(gè)機(jī)器人向目標(biāo)點(diǎn)運(yùn)動(dòng)的過程中，既能夠收斂于特定的隊(duì)形，又能適應(yīng)環(huán)境約束的控制技術(shù)[7]。目前多機(jī)器人的編隊(duì)控制方法主要有虛擬結(jié)構(gòu)法、領(lǐng)航跟隨方法、人工勢場法、基于行為法等[8]。由于領(lǐng)航跟隨法控制簡單，易于工程實(shí)現(xiàn)，因此被廣泛應(yīng)用于編隊(duì)控制中。

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

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

2 遺傳算法路徑規(guī)劃

2.1 遺傳算法模型

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

圖1 遺傳算法路徑規(guī)劃流程圖

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

2.1.1 路徑編碼

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

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

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

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

圖2 可行路徑

2.1.2適應(yīng)度函數(shù)計(jì)算

在遺傳算法中，適應(yīng)度函數(shù)決定了一個(gè)個(gè)體被遺傳到下一代的概率。路徑規(guī)劃的目的是使機(jī)器人從起始點(diǎn)運(yùn)動(dòng)到目標(biāo)點(diǎn)的距離最短，因此在進(jìn)行適應(yīng)度函數(shù)的設(shè)計(jì)時(shí)要滿足機(jī)器人運(yùn)行軌跡越短適應(yīng)度值越大。在一個(gè)路徑個(gè)體中，路徑總長度Length的計(jì)算公式如下：

其中，end為機(jī)器人行走的步數(shù)。

路徑適應(yīng)度函數(shù)為：

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

2.1.3 選擇操作

選擇的目的是將滿足要求的個(gè)體保留，不滿足要求的個(gè)體剔除。選擇方法基于上一步中計(jì)算出的適應(yīng)度函數(shù)值，采用輪盤賭法。

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

2.2 遺傳算法改進(jìn)

2.2.1 適應(yīng)度函數(shù)加入路徑平滑度

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

其中，適應(yīng)度函數(shù)的第一部分為傳統(tǒng)遺傳算法的適應(yīng)度函數(shù)：

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

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

適應(yīng)度函數(shù)的第二部分fit2=1 Punish總，根據(jù)路徑長度和平滑度之間的權(quán)重關(guān)系，適應(yīng)度函數(shù)的兩部分需要各取一個(gè)權(quán)重參數(shù)a和b。

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

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

3 領(lǐng)航跟隨法編隊(duì)控制

相比于單臺機(jī)器人而言，多機(jī)器人協(xié)同編隊(duì)在靈活性、可拓展性、魯棒性等方面具有更好的優(yōu)越性。目前機(jī)器人編隊(duì)中最常用的算法主要有領(lǐng)航跟隨法、基于行為法、虛擬結(jié)構(gòu)法等方法[17]。領(lǐng)航跟隨法采用鏈?zhǔn)降耐負(fù)浣Y(jié)構(gòu)，整個(gè)隊(duì)形由領(lǐng)航機(jī)器人和跟隨機(jī)器人組成，在實(shí)施過程中只需要使跟隨者獲得領(lǐng)航者的運(yùn)動(dòng)狀態(tài)信息并對領(lǐng)航者的運(yùn)動(dòng)軌跡進(jìn)行跟蹤便可實(shí)現(xiàn)編隊(duì)控制[18]。領(lǐng)航跟隨法由于控制器的設(shè)計(jì)相對簡單，被廣泛應(yīng)用于編隊(duì)算法中。

本文中編隊(duì)控制采用的算法是領(lǐng)航跟隨法。首先需要選定一臺機(jī)器人作為領(lǐng)航者，主要負(fù)責(zé)整個(gè)編隊(duì)的路徑規(guī)劃任務(wù)，根據(jù)地圖信息規(guī)劃出一條從起點(diǎn)到終點(diǎn)的無碰最優(yōu)路徑。其余機(jī)器人則被視為跟隨者，跟隨機(jī)器人負(fù)責(zé)跟蹤領(lǐng)航者進(jìn)行運(yùn)動(dòng)，并盡可能與領(lǐng)航機(jī)器人之間保持隊(duì)形所需要的距離和角度。多臺機(jī)器人形成編隊(duì)時(shí)，編隊(duì)中的每臺機(jī)器人不僅要避開障礙物安全抵達(dá)目的地，而且要避免與隊(duì)形中其他成員發(fā)生碰撞[19]。

領(lǐng)航跟隨法主要有l(wèi)-l控制和l-φ控制兩種控制模式。l-l控制即每個(gè)跟隨機(jī)器人以固定的距離l跟隨兩個(gè)領(lǐng)航機(jī)器人，從而保持期望的隊(duì)形；l-φ控制即每個(gè)跟隨機(jī)器人以一定的距離和角度跟隨領(lǐng)航機(jī)器人，實(shí)現(xiàn)期望隊(duì)形[20]。本文采用l-φ控制方法。算法實(shí)現(xiàn)步驟如下：

步驟1領(lǐng)航機(jī)器人根據(jù)地圖信息使用改進(jìn)遺傳算法規(guī)劃出自身的最優(yōu)路徑。

步驟2領(lǐng)航機(jī)器人沿著最優(yōu)路徑向目標(biāo)點(diǎn)運(yùn)行，并利用設(shè)計(jì)好的算法進(jìn)行避障，同時(shí)，領(lǐng)航機(jī)器人實(shí)時(shí)檢測自身的位姿信息，根據(jù)設(shè)定的隊(duì)形使用l-φ控制法生成虛擬機(jī)器人的軌跡，并發(fā)送給跟隨機(jī)器人。

步驟3跟隨機(jī)器人實(shí)時(shí)接收領(lǐng)航機(jī)器人發(fā)送的軌跡，調(diào)整自己的速度和運(yùn)動(dòng)方向，使沿著虛擬機(jī)器人的軌跡運(yùn)行。同時(shí)，跟隨機(jī)器人實(shí)時(shí)檢測自身周圍的障礙物信息，當(dāng)跟隨機(jī)器人遇到障礙物時(shí)，會在原地進(jìn)行一定時(shí)間的等待，當(dāng)領(lǐng)航機(jī)器人通過后，跟隨機(jī)器人會運(yùn)動(dòng)到領(lǐng)航機(jī)器人上一時(shí)刻的位置，通過障礙物后，跟隨機(jī)器人再回到虛擬機(jī)器人的軌跡上，恢復(fù)設(shè)定隊(duì)形。

4 仿真

4.1 路徑規(guī)劃仿真

為了驗(yàn)證本文改進(jìn)的遺傳算法地有效性，建立20×20的柵格地圖環(huán)境模型對該算法進(jìn)行仿真，并與傳統(tǒng)遺傳算法進(jìn)行比較。設(shè)定機(jī)器人起始點(diǎn)坐標(biāo)為（0，0），目標(biāo)點(diǎn)坐標(biāo)為（20，20），種群規(guī)模為100，最大迭代次數(shù)為50代，交叉概率為0.9，變異概率為0.002。實(shí)驗(yàn)結(jié)果如圖3、圖4所示。

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

為了避免偶然性因素，如圖5（a）所示，在柵格地圖左下角4×4區(qū)域內(nèi)隨機(jī)選取自由柵格作為起始點(diǎn)，在地圖右上角4×4區(qū)域內(nèi)隨機(jī)選取自由柵格作為目標(biāo)點(diǎn)，將傳統(tǒng)遺傳算法和改進(jìn)遺傳算法分別運(yùn)行100次進(jìn)行仿真對比，觀察各個(gè)算法收斂至最短路徑所需的迭代次數(shù)。

圖3 傳統(tǒng)遺傳算法

圖4 改進(jìn)遺傳算法

圖5 算法改進(jìn)前后收斂至最短路徑迭代次數(shù)

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

本文又選取了文獻(xiàn)[9]、文獻(xiàn)[10]中所述的遺傳算法與傳統(tǒng)遺傳算法和改進(jìn)后的遺傳算法進(jìn)行對比，觀察四種遺傳算法的收斂速度。

將四種算法運(yùn)行100次統(tǒng)計(jì)平均迭代次數(shù)與最終路徑的平均長度，得到的數(shù)據(jù)如表1所示。

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

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

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

實(shí)驗(yàn)證明，與傳統(tǒng)遺傳算法、文獻(xiàn)[9]中遺傳算法和文獻(xiàn)[10]遺傳算法相比，改進(jìn)的遺傳算法具有較快的收斂速度，且規(guī)劃出的路徑平滑度較好，能夠縮短規(guī)劃路徑所需的時(shí)間和機(jī)器人運(yùn)動(dòng)所需的時(shí)間，且運(yùn)行更加安全，綜合性能較優(yōu)。

4.2 多機(jī)器人編隊(duì)仿真

本文以三個(gè)機(jī)器人組成三角形隊(duì)形為例進(jìn)行驗(yàn)證。設(shè)定領(lǐng)航機(jī)器人的起始點(diǎn)坐標(biāo)為（2，2），目標(biāo)點(diǎn)坐標(biāo)為（20，20），兩個(gè)跟隨機(jī)器人的起始點(diǎn)坐標(biāo)分別為（1，2）和（2，1），目標(biāo)點(diǎn)坐標(biāo)分別為（19，20）和（20，19）。

仿真1領(lǐng)航者和跟隨者都沒有遇到障礙物，機(jī)器人沿直線直接從起點(diǎn)運(yùn)動(dòng)到了終點(diǎn)，所有機(jī)器人全程保持了三角形隊(duì)形，軌跡圖如圖7所示。

仿真2領(lǐng)航機(jī)器人遇到障礙物，跟隨者沒有遇到障礙物。在起點(diǎn)與終點(diǎn)的連線上增加了一個(gè)障礙物，當(dāng)領(lǐng)航機(jī)器人運(yùn)動(dòng)到障礙物面前會繞過障礙物尋找一條新的路徑到達(dá)目標(biāo)點(diǎn)，且跟隨機(jī)器人會跟隨著機(jī)器人繞開障礙物，且運(yùn)動(dòng)過程中始終保持著三角形隊(duì)形運(yùn)動(dòng)到目標(biāo)點(diǎn)，軌跡圖如圖8所示。

圖7 無障礙環(huán)境下路徑規(guī)劃軌跡圖

圖8 領(lǐng)航者遇到障礙環(huán)境下路徑規(guī)劃軌跡圖

仿真3領(lǐng)航者沒有遇到障礙物，跟隨者遇到障礙物。當(dāng)跟隨機(jī)器人的路徑上出現(xiàn)了障礙物，編隊(duì)會進(jìn)行短時(shí)間的變換隊(duì)形，跟隨機(jī)器人會在障礙物面前原地等待一段時(shí)間，待到領(lǐng)航機(jī)器人通過后，跟隨機(jī)器人會移動(dòng)到領(lǐng)航機(jī)器人上一時(shí)刻的位置，當(dāng)跟隨者繞過障礙物以后，跟隨機(jī)器人重新回到虛擬機(jī)器人的軌跡上，恢復(fù)設(shè)定隊(duì)形，軌跡圖如圖9所示。

圖9 跟隨者遇到障礙環(huán)境下路徑規(guī)劃軌跡圖

仿真4只能單個(gè)機(jī)器人通過時(shí)，兩個(gè)跟隨機(jī)器人會在障礙物前等待，當(dāng)領(lǐng)航機(jī)器人通過狹窄通道后，跟隨機(jī)器人會依次運(yùn)動(dòng)到領(lǐng)航機(jī)器人的位置，呈“一”字形挨個(gè)通過狹窄通道，當(dāng)所有機(jī)器人通過狹窄區(qū)域后，再恢復(fù)至設(shè)定隊(duì)形，軌跡圖如圖10所示。

實(shí)驗(yàn)表明，本文設(shè)計(jì)的領(lǐng)航跟隨法能使所有機(jī)器人保持隊(duì)形安全無碰撞地運(yùn)行到目標(biāo)位置。當(dāng)機(jī)器人遇到障礙物時(shí)，能夠及時(shí)地變換隊(duì)形來避開障礙物，完成避障任務(wù)后再重新恢復(fù)隊(duì)形。

圖10 狹窄環(huán)境下路徑規(guī)劃軌跡圖

5 結(jié)束語

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

從隊(duì)形保持、隊(duì)形變換來看，當(dāng)沒有遇到障礙物時(shí)，多機(jī)器人可以始終保持隊(duì)形從起始點(diǎn)運(yùn)動(dòng)到目標(biāo)位置，當(dāng)機(jī)器人碰到狹窄通道或者遇到障礙物時(shí)，會進(jìn)行適當(dāng)?shù)年?duì)形變換以躲避障礙物，通過狹窄通道或者繞開障礙物后，多機(jī)器人又恢復(fù)成設(shè)定的隊(duì)形向目標(biāo)點(diǎn)前進(jìn)，最終到達(dá)指定位置。