基于改進(jìn)鴿群算法與領(lǐng)航跟隨法的無人車編隊(duì)方法

支奕琛，谷玉海，龍伊娜，徐小力

(1.北京信息科技大學(xué)現(xiàn)代測控技術(shù)教育部重點(diǎn)實(shí)驗(yàn)室，北京 100192；2.北京信息科技大學(xué)機(jī)電工程學(xué)院，北京 100192)

0 前言

由于單個(gè)無人車在自主建圖、自主導(dǎo)航等方面的能力有限，很難獨(dú)自完成如協(xié)作運(yùn)輸、集群勘探等工作任務(wù)，所以近年來多無人車系統(tǒng)協(xié)同工作成為行業(yè)研究的熱點(diǎn)。無人技術(shù)范疇中多無人車協(xié)同編隊(duì)發(fā)揮的作用非常關(guān)鍵，如協(xié)同反恐、偵查和聯(lián)合搜救等，因此多無人車系統(tǒng)編隊(duì)成為關(guān)鍵的課題。對于多無人車系統(tǒng)編隊(duì)來說，無人車在執(zhí)行任務(wù)的過程中需要實(shí)現(xiàn)自主協(xié)同并形成編隊(duì)隊(duì)形，同時(shí)保持隊(duì)形向目標(biāo)點(diǎn)行進(jìn)。結(jié)合任務(wù)具體要求，隊(duì)形可以是直線、正方形或者三角形等多種。

路徑規(guī)劃是指在有障礙物的環(huán)境下向機(jī)器人提供從起點(diǎn)到終點(diǎn)途中的道路信息并規(guī)劃出一條較優(yōu)路徑。對于移動機(jī)器人，規(guī)劃出一條良好的路徑與在路徑上成功避障十分重要，不合適的路徑可能會導(dǎo)致與其他物體碰撞致使機(jī)器人損壞或浪費(fèi)大量時(shí)間。Voronoi圖法、遺傳算法、蟻群算法、粒子群算法等都是應(yīng)用較為廣泛的路徑規(guī)劃算法，具有很強(qiáng)的代表性?？当热嗽O(shè)計(jì)禁忌柵格，對螞蟻的線路進(jìn)行引導(dǎo)、控制，并結(jié)合時(shí)空數(shù)據(jù)及時(shí)更新調(diào)整，獲得了全新的蟻群算法。

結(jié)合當(dāng)前文獻(xiàn)資料，控制多無人車編隊(duì)是以虛擬結(jié)構(gòu)法(Virtual-Structure)、基于行為的方法(Behavior-based)、領(lǐng)航-跟隨型法、基于路徑跟蹤的方法、人工勢場法、圖論法等為主。領(lǐng)航-跟隨型編隊(duì)控制方法的優(yōu)勢在于分析難度小，并且更加精準(zhǔn)，在多無人車編隊(duì)控制中適用性較強(qiáng)。領(lǐng)航-跟隨型編隊(duì)控制法中，通常領(lǐng)航者可以是一個(gè)，也可以是多個(gè)，除此以外都屬于跟隨者。在編隊(duì)的過程中，領(lǐng)航者負(fù)責(zé)選擇最優(yōu)路徑向目標(biāo)點(diǎn)前進(jìn)，并在前進(jìn)過程實(shí)時(shí)避障，而跟隨者利用領(lǐng)航者當(dāng)前的信息調(diào)整各自的狀態(tài)，實(shí)現(xiàn)與領(lǐng)航者保持一定的距離和角度，從而達(dá)到編隊(duì)效果。

文中編隊(duì)是利用改進(jìn)鴿群算法與領(lǐng)航跟隨法融合實(shí)現(xiàn)的。對鴿群算法進(jìn)行改進(jìn)，在鴿群算法的前期設(shè)置初始解使其初始化，能夠提升算法的效率。另外，在算法的地磁算子中加入相應(yīng)的權(quán)重指標(biāo)，提升算法在整體路徑中的規(guī)劃效率。通過MATLAB進(jìn)行仿真，結(jié)果表明：與PSO算法、量子粒子群優(yōu)化(Quantum Particle Swarm Optimization,QPSO)算法相比，改進(jìn)的鴿群算法規(guī)劃的路徑及算法運(yùn)行時(shí)間比其他算法更短，驗(yàn)證了改進(jìn)鴿群算法的優(yōu)越性。路徑規(guī)劃完成后，通過領(lǐng)航跟隨方法控制無人車編隊(duì)，以領(lǐng)航者無人車位置與編隊(duì)隊(duì)形為依據(jù)，完成所有虛擬機(jī)器人位置信息的計(jì)算；控制所有跟隨者無人車向虛擬機(jī)器人運(yùn)動，使隊(duì)形得以實(shí)現(xiàn)。

1 鴿群算法

1.1 鴿群行為說明

鴿子是一種十分常見的鳥，世界各地都在廣泛飼養(yǎng)。古時(shí)候在沒有手機(jī)等智能通信設(shè)備的情況下，人們都采用鴿子進(jìn)行遠(yuǎn)程書信交流，鴿子的記憶能力可見一斑。此外，它還是一種依賴習(xí)慣的動物，很多鴿子都可以利用一些建筑物或者標(biāo)記點(diǎn)進(jìn)行飛行，甚至還可以結(jié)合磁場信息、太陽的運(yùn)動等，以順利達(dá)到指定位置。

1.2 鴿群算法

在飛行過程中，鴿群具備非常突出的導(dǎo)航能力以及辨識方向的能力。研究者在對它們進(jìn)行長時(shí)間的研究后，受到啟發(fā)，創(chuàng)建了相應(yīng)的鴿群算法(Pigeon-Inspired Optimization，PIO)。PIO算法重點(diǎn)根據(jù)兩個(gè)方面形成，即地磁算子、地標(biāo)算子。不斷地對二者進(jìn)行迭代處理，可以快速運(yùn)算得到其中代表性粒子的速度數(shù)據(jù)，從而明確種群的最優(yōu)數(shù)據(jù)。

引進(jìn)相應(yīng)的地磁算子。地磁算子迭代更新的方法：以和表示第只鴿子所在的位置和速度。在每輪迭代中，鴿群中的個(gè)體都可以用公式(1)(2)計(jì)算并更新個(gè)體的速度和位置。

()=(-1)e-+[-(-1)]

(1)

()=(-1)+()

(2)

式中：表示地磁算子；表示隨機(jī)數(shù)；表示當(dāng)前全局最優(yōu)位置。

引進(jìn)相應(yīng)的地標(biāo)算子，鴿群按照其當(dāng)前的分布情況，評估目前的坐標(biāo)與目的地是否相同。若方位相似或者相同，鴿群繼續(xù)前進(jìn)，直奔最終地點(diǎn)；若相反，則令群體的中心位置為目標(biāo)，調(diào)整自身的行為。在不斷迭代后，種群的規(guī)模會下降。以第次迭代為例，地標(biāo)算子的更新方法如下：

(3)

(4)

()=(-1)+[()-(-1)]

(5)

式中：為第次迭代時(shí)種群的數(shù)量；()為第次迭代后第只鴿子的位置；[()]為()位置的適應(yīng)度。

結(jié)合地標(biāo)算子的運(yùn)算及分析，可以更高效率地確定目標(biāo)位置，使鴿群更直接、穩(wěn)定地到達(dá)目的地。

2 改進(jìn)鴿群算法

(1)改進(jìn)鴿群算法

鴿群算法的不足之處是收斂速率偏低，且局部最優(yōu)出現(xiàn)率偏高。本文作者在原算法的前提下，在前期設(shè)置一個(gè)初始解，在進(jìn)行初步處理后，能夠簡化算法，減少計(jì)算時(shí)間，從而能夠提高計(jì)算效率。同時(shí)，在算法的地磁算子中加入相應(yīng)的權(quán)重指標(biāo)，提升算法在整體路徑中的規(guī)劃效率。權(quán)重指標(biāo)的公式為

式中:、分別表示慣性權(quán)重系數(shù)的最小值和最大值；和分別表示當(dāng)前鴿群內(nèi)部適應(yīng)度的最小值和平均值。

(2)改進(jìn)鴿群算法路徑規(guī)劃流程

改進(jìn)鴿群算法路徑規(guī)劃流程如圖1所示。

圖1 改進(jìn)鴿群算法路徑規(guī)劃流程

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

多無人車編隊(duì)與單無人車進(jìn)行比較，其優(yōu)勢主要表現(xiàn)為魯棒性更好、擴(kuò)展能力更強(qiáng)且更加靈活。以行為為基礎(chǔ)的方法、虛擬結(jié)構(gòu)法和領(lǐng)航跟隨法是多無人車編隊(duì)控制法中應(yīng)用最為廣泛的幾種。其中，領(lǐng)航跟隨法控制方法難度更小且效果更精準(zhǔn)、穩(wěn)定性更強(qiáng)，在多無人車編隊(duì)控制中適用性較好。

該控制方法原理：編隊(duì)內(nèi)全部無人車除了領(lǐng)航者都是跟隨者。領(lǐng)航者以地圖信息為依據(jù)對最優(yōu)路徑進(jìn)行確定并完成避障，將控制指令發(fā)送給領(lǐng)航者進(jìn)而對編隊(duì)整體進(jìn)行控制，跟隨者時(shí)刻與領(lǐng)航者保持相應(yīng)的角度與間距，使隊(duì)形得到保證。多無人車編隊(duì)完成后，所有無人車都要避障，并且不能與編隊(duì)內(nèi)其他無人車出現(xiàn)碰撞現(xiàn)象。橫縱隊(duì)形和三角隊(duì)形是目前最常見的編隊(duì)，具體如圖2所示。

圖2 常見的多無人車編隊(duì)隊(duì)形

領(lǐng)航跟隨法有-控制和-控制2種控制模式。-控制方式即每個(gè)跟隨者無人車以一定的距離和角度跟隨領(lǐng)航者無人車，從而實(shí)現(xiàn)編隊(duì)隊(duì)形。-控制方式即每個(gè)跟隨者無人車以固定的距離跟隨2個(gè)領(lǐng)航者無人車?，F(xiàn)采用-控制方式，其實(shí)現(xiàn)步驟如下：

步驟1，領(lǐng)航者根據(jù)地圖信息使用改進(jìn)鴿群算法進(jìn)行路徑規(guī)劃，規(guī)劃出供無人車行駛的最優(yōu)路徑；

步驟2，領(lǐng)航者無人車沿著最優(yōu)路徑行駛，并利用避障算法進(jìn)行避障，同時(shí)領(lǐng)航者無人車實(shí)時(shí)檢測自身的位姿信息，根據(jù)使用的隊(duì)形用-控制方式生成虛擬機(jī)器人的軌跡發(fā)送給跟隨者無人車；

步驟3，跟隨者無人車不斷地接收領(lǐng)航者機(jī)器人發(fā)送的運(yùn)動控制指令和軌跡，調(diào)整自身的速度和運(yùn)動方向，沿著虛擬機(jī)器人的軌跡運(yùn)動。跟隨者無人車實(shí)時(shí)探測周圍的環(huán)境信息，當(dāng)遇到障礙物時(shí)，在原地等待一段時(shí)間，當(dāng)領(lǐng)航者無人車通過后，跟隨者無人車會行駛至領(lǐng)航者無人車上一時(shí)刻的位置，避開障礙物以后，跟隨者恢復(fù)原本軌跡，編隊(duì)隊(duì)形得以恢復(fù)。

結(jié)合改進(jìn)鴿群算法和領(lǐng)航跟隨法的編隊(duì)方法流程如圖3所示。

圖3 編隊(duì)方法流程

4 仿真實(shí)驗(yàn)與結(jié)果分析

4.1 路徑規(guī)劃仿真

為進(jìn)一步驗(yàn)證算法的實(shí)際效果，通過MATLAB進(jìn)行仿真。分析過程中，柵格尺寸確定為20×20。對相應(yīng)算法進(jìn)行初始化處理，借助算法獲得整體最優(yōu)路線。為分析算法的效果，利用PSO算法、QPSO算法進(jìn)行比較分析。結(jié)果如圖4所示。3種算法的收斂結(jié)果如圖5所示，數(shù)據(jù)對比如表1所示。

圖4 3種算法的全局路徑規(guī)劃

圖5 3種算法的收斂比較

表1 3種算法的數(shù)據(jù)比較

由圖4、圖5、表1可以看出：PSO算法和QPSO算法雖然都可以規(guī)劃出一條路徑，但是從算法的運(yùn)行時(shí)間、收斂性、路徑長度分析，改進(jìn)的鴿群算法更具有優(yōu)越性。

4.2 多無人車編隊(duì)仿真

以3個(gè)無人車組成三角形隊(duì)形為例進(jìn)行仿真驗(yàn)證。設(shè)定領(lǐng)航者、跟隨者起始位置和目標(biāo)點(diǎn)位置，領(lǐng)航者與跟隨者在行駛途中沒有遇到障礙物，無人車沿直線從起點(diǎn)行駛至目標(biāo)點(diǎn)，所有無人車全程保持三角隊(duì)形，行駛軌跡如圖6所示。

圖6 無障礙環(huán)境下無人車軌跡

在起點(diǎn)與終點(diǎn)的連線上增加一個(gè)障礙物，這時(shí)領(lǐng)航者無人車遇到障礙物但跟隨者無人車沒有遇到障礙物，當(dāng)領(lǐng)航者無人車行駛至障礙物前時(shí)會避開障礙物行駛至一條新的路徑到達(dá)終點(diǎn)，且跟隨者無人車會與領(lǐng)航者無人車保持三角隊(duì)形繞開障礙物并運(yùn)動到目標(biāo)點(diǎn)，如圖7所示。

圖7 領(lǐng)航者遇到障礙物下的無人車軌跡

當(dāng)領(lǐng)航者無人車行駛的路線上沒有障礙物但跟隨者無人車行駛路線上出現(xiàn)障礙物，編隊(duì)會進(jìn)行短暫的隊(duì)形變換，跟隨者無人車會在障礙物前原地等待一段時(shí)間，領(lǐng)航者通過障礙物以后，跟隨者會重復(fù)領(lǐng)航者行跡以實(shí)現(xiàn)避障，繞過障礙物后跟隨者無人車會返回原虛擬無人車的軌跡上，恢復(fù)三角隊(duì)形，如圖8所示。

圖8 跟隨者遇到障礙物下的無人車軌跡

當(dāng)?shù)缆分荒芡ㄟ^一個(gè)無人車時(shí)，跟隨者無人車會在障礙物前等待一段時(shí)間，當(dāng)領(lǐng)航者無人車通過狹窄的通道后，跟隨者無人車會輪流運(yùn)動到領(lǐng)航者的位置，呈縱行隊(duì)依次通過狹窄的道路，當(dāng)所有的無人車通過狹窄的通道后，再恢復(fù)至設(shè)定的隊(duì)形，如圖9所示。

圖9 狹窄環(huán)境下無人車路徑軌跡

5 算法實(shí)際應(yīng)用

為進(jìn)一步驗(yàn)證該編隊(duì)算法的有效性，將其運(yùn)用到真實(shí)的無人車上，實(shí)現(xiàn)無人車的編隊(duì)控制。

如圖10所示，障礙物位于3臺小車周圍，領(lǐng)航者無人車使用改進(jìn)鴿群算法規(guī)劃出一條直線路徑向前行駛，運(yùn)用領(lǐng)航跟隨法進(jìn)行編隊(duì)控制使后面兩輛跟隨者無人車以一定的距離和角度跟隨領(lǐng)航者無人車以三角隊(duì)形從起點(diǎn)運(yùn)動到終點(diǎn)。

圖10 無障礙環(huán)境下的編隊(duì)過程

如圖11(a)所示，在路徑的起點(diǎn)和終點(diǎn)設(shè)置一個(gè)障礙物，領(lǐng)航者遇到障礙物，而跟隨者未遇到，領(lǐng)航者小車會重新規(guī)劃出一條路徑繞開障礙物，此時(shí)跟隨者小車依舊會與領(lǐng)航者小車保持隊(duì)形繞開障礙物，并到達(dá)終點(diǎn)，過程如圖11(b)—(e)所示。

圖11 領(lǐng)航者遇到障礙物時(shí)的編隊(duì)過程

如圖12(a)所示，領(lǐng)航者無人車行駛路徑上沒有出現(xiàn)障礙物，但是跟隨者無人車行駛的路徑上出現(xiàn)障礙物，此時(shí)會進(jìn)行短暫的編隊(duì)隊(duì)形變換，跟隨者無人車在障礙物前等待一段時(shí)間，等待領(lǐng)航者無人車通過障礙物后，跟隨者小車運(yùn)動到領(lǐng)航者小車上一時(shí)刻的位置從而繞過障礙物，跟隨者小車?yán)@過障礙物后，恢復(fù)三角隊(duì)形到達(dá)終點(diǎn)，過程如圖12(b)—(h)所示。

圖12 跟隨者遇到障礙物時(shí)的編隊(duì)過程

如圖13(a)所示，當(dāng)無人車編隊(duì)通過狹窄道路環(huán)境只能通過一輛無人車時(shí)，跟隨者無人車會在障礙物前等待一段時(shí)間，等待領(lǐng)航者無人車通過狹窄通道，跟隨者小車依次運(yùn)動到領(lǐng)航者位置，呈一字形隊(duì)形或縱行隊(duì)形通過狹窄通道，全部無人車都順利從狹窄的通道通過以后，隊(duì)形恢復(fù)并向終點(diǎn)行進(jìn)，具體如圖13(b)—(e)所示。

圖13 存在狹窄環(huán)境下的編隊(duì)過程

6 結(jié)語

本文作者將鴿群算法和領(lǐng)航跟隨者算法相結(jié)合，通過引進(jìn)相應(yīng)的權(quán)重系數(shù)對鴿群算法進(jìn)行改進(jìn)。在仿真中，改進(jìn)的鴿群算法規(guī)劃的路徑和運(yùn)行時(shí)間比其他算法更短，驗(yàn)證了改進(jìn)鴿群算法的優(yōu)越性。領(lǐng)航跟隨者算法在隊(duì)形保持、隊(duì)形變換方面都十分出色，多無人車編隊(duì)在并未遇到障礙物的情況下，能夠以最初編隊(duì)隊(duì)形一直向目標(biāo)點(diǎn)行進(jìn)，如果中途有障礙物存在或者路徑過于狹窄，編隊(duì)隊(duì)形會適當(dāng)變化以順利通過或者避開障礙物，當(dāng)所有的無人車通過狹窄環(huán)境或避開障礙物時(shí)，多無人車編隊(duì)又會恢復(fù)設(shè)定的隊(duì)形繼續(xù)運(yùn)動到終點(diǎn)。結(jié)果表明：所提編隊(duì)方法可以控制多無人車以一定的隊(duì)形有效地從起點(diǎn)行駛至目標(biāo)點(diǎn)，并可以成功地避開障礙物和通過狹窄環(huán)境，該編隊(duì)方法可靠、有效。研究結(jié)果為多無人車編隊(duì)提供了參考。