基于改進(jìn)勢場法的無人機(jī)編隊(duì)協(xié)同避障控制算法

孫翊君 代冀陽 應(yīng) 進(jìn) 聶 航

(南昌航空大學(xué)信息工程學(xué)院 江西 南昌 330063)

0 引 言

無人機(jī)編隊(duì)協(xié)同飛行控制研究已成為近年來的一個(gè)前沿課題，其不僅可以發(fā)揮單架無人機(jī)具有的機(jī)動(dòng)性能優(yōu)勢，還可以多架無人機(jī)協(xié)同合作共同完成較為復(fù)雜的任務(wù)，是一種典型的多智能體系統(tǒng)[1]。而這些協(xié)同控制任務(wù)的實(shí)現(xiàn)均建立在多智能體系統(tǒng)的一致性問題研究框架內(nèi)，因此針對無人機(jī)編隊(duì)協(xié)同避障控制策略的進(jìn)一步研究是具有重大意義的。

目前，人工勢場法因其原理簡單、實(shí)時(shí)性好等優(yōu)點(diǎn)在無人機(jī)編隊(duì)控制、航跡規(guī)劃領(lǐng)域得到了廣泛應(yīng)用，但其極易因障礙物影響出現(xiàn)死鎖以及目標(biāo)不可達(dá)問題等[2-3]。文獻(xiàn)[4]通過將斥力偏轉(zhuǎn)一定角度來避免傳統(tǒng)勢場法死鎖問題；文獻(xiàn)[5]使用雙向人工勢場的概念使控制對象避開局部極點(diǎn)；文獻(xiàn)[6]引入回環(huán)力、無人機(jī)間作用力來解決死鎖以及編隊(duì)安全問題。但上述算法未明確編隊(duì)間距與架構(gòu)通信問題，且未考慮無人機(jī)狀態(tài)一致問題?；趫D論法則是研究一致性問題的重要工具[7-8]。文獻(xiàn)[9]在文獻(xiàn)[6]基礎(chǔ)上分別設(shè)計(jì)了三個(gè)代價(jià)函數(shù)進(jìn)行優(yōu)化控制，但無人機(jī)相對位置不明確，未能保證固定的編隊(duì)結(jié)構(gòu)；文獻(xiàn)[10]提出了單一領(lǐng)導(dǎo)者的多智能體系統(tǒng)一致性算法，通過勢場函數(shù)避免機(jī)間自碰撞，有效地使智能體速度達(dá)成一致，但忽略了路徑避障問題；文獻(xiàn)[11]在假設(shè)領(lǐng)導(dǎo)者路途無障礙的前提下提出基于一致性算法和人工勢場法的編隊(duì)避障控制律，但構(gòu)造的勢場函數(shù)較為簡單，易出現(xiàn)死鎖問題；文獻(xiàn)[12]在此基礎(chǔ)上基于多虛擬領(lǐng)導(dǎo)者進(jìn)行編隊(duì)控制，增加指向領(lǐng)導(dǎo)者任一側(cè)的勢場旋轉(zhuǎn)力跳出死鎖，但實(shí)際環(huán)境較為復(fù)雜，一旦與領(lǐng)導(dǎo)者失去通信，編隊(duì)將難以維持，且其模擬環(huán)境較為理想化，實(shí)際應(yīng)用價(jià)值較低。

鑒于此，本文針對分布式無人機(jī)編隊(duì)，采用分段思想、增加指向目標(biāo)側(cè)的旋轉(zhuǎn)力、機(jī)間力改進(jìn)勢場函數(shù)以解決死鎖等問題，并將得到的勢場精確控制輸出作為期望狀態(tài)值，結(jié)合一致性理論對航跡位置、速度進(jìn)行誤差修正，提出一種圖論法和勢場法相結(jié)合的無人機(jī)編隊(duì)協(xié)同避障控制策略，實(shí)現(xiàn)編隊(duì)航行順利以及各無人機(jī)間的狀態(tài)一致性。利用系統(tǒng)的動(dòng)能與勢能構(gòu)造Hamilton函數(shù)討論編隊(duì)收斂性，利用反證法證明編隊(duì)穩(wěn)定性，并以不同形狀的障礙物模擬復(fù)雜環(huán)境，通過仿真驗(yàn)證該算法的有效性以及收斂性。

1 問題描述

1.1 無人機(jī)編隊(duì)建模

首先，考慮二維空間中具有n架無人機(jī)的編隊(duì)模型，第i架無人機(jī)運(yùn)動(dòng)學(xué)方程設(shè)為二階積分器模型為：

(1)

式中：xi=(xix,xiy)為無人機(jī)i的位置狀態(tài)；vi=(vix,viy)為其速度狀態(tài)；ui=(uix,uiy)表示其控制輸入。

則可定義以下向量作為所有n架無人機(jī)的積分狀態(tài)向量：

(2)

在無人機(jī)編隊(duì)的一致性控制中，假設(shè)每一架無人機(jī)上均安裝有信息接收發(fā)送裝置，并且無人機(jī)間的數(shù)據(jù)傳輸準(zhǔn)確、無延遲、無數(shù)據(jù)丟包等問題，那么在圖論理論中，無人機(jī)間信息的互換網(wǎng)絡(luò)拓補(bǔ)結(jié)構(gòu)可通過圖G=(V,E)來表示。V={1,2,…,n}表示其節(jié)點(diǎn)的非空集合，每一個(gè)節(jié)點(diǎn)都表示一架無人機(jī)。E={(i,j)|i,j∈N,i≠j}表示其邊集，(i,j)∈E表示無人機(jī)i可以獲取無人機(jī)j的狀態(tài)信息[13]，但反之不可。若無人機(jī)i與j之間可雙向通信，則表示圖G為無向圖。其拉普拉斯矩陣定義為L=D(G)-A(G)。其中:D(G)=diag(d(i))表示其入度矩陣，d(i)為向節(jié)點(diǎn)i傳輸信息的節(jié)點(diǎn)數(shù)目；A(G)=[aij]表示其鄰接矩陣，aij為邊(i,j)的權(quán)值。

(3)

假設(shè)由固定無向連通圖來表示無人機(jī)編隊(duì)的通信傳輸方式，那么四無人機(jī)編隊(duì)的網(wǎng)絡(luò)拓補(bǔ)結(jié)構(gòu)圖如圖1所示。

圖1 無人機(jī)編隊(duì)通信拓補(bǔ)圖

1.2 編隊(duì)控制目標(biāo)

對于上述無人機(jī)編隊(duì)系統(tǒng)，不論其初始狀態(tài)如何，若能夠滿足式(4)，則稱當(dāng)t→∞時(shí)，多無人機(jī)可以按照期望的速度以及編隊(duì)隊(duì)形軌跡收斂，達(dá)到了一致性：

(4)

式中：ρa(bǔ)表示無人機(jī)i與無人機(jī)j之間的相對最佳間距，以此可保持預(yù)期編隊(duì)隊(duì)形。若ρa(bǔ)為0，則表示為普通的無人機(jī)編隊(duì)系統(tǒng)一致性控制要求。

本文將采用普通的系統(tǒng)一致性控制協(xié)議，至于無人機(jī)間相對最佳間距與位置的保持將以人工勢場法為基礎(chǔ)的機(jī)間勢場力來替代，以求實(shí)現(xiàn)精確控制；另外，無人機(jī)編隊(duì)的避障問題同樣以改進(jìn)的人工勢場法進(jìn)行控制實(shí)現(xiàn)。即若無人機(jī)編隊(duì)系統(tǒng)滿足式(5)，則稱多無人機(jī)可以在保證自身安全穩(wěn)定的前提下完成任務(wù)。

(5)

式中：xk(t)表示無人機(jī)i和障礙物k幾何中心的連線與障礙物表面相交所得交點(diǎn)的空間位置。ρ(xi(t),xj(t))、ρ(xi(t),xk(t))則分別表示無人機(jī)i與無人機(jī)j、障礙物k之間的相對距離。

2 改進(jìn)人工勢場法

2.1 傳統(tǒng)人工勢場理論

傳統(tǒng)人工勢場法定義是基于物理學(xué)中的勢能場演變而來，無人機(jī)在整個(gè)環(huán)境內(nèi)受到目標(biāo)位置的引力勢場能作用以及障礙物對其的斥力勢場能作用，而后無人機(jī)將在所受勢場能產(chǎn)生的合力作用下以合力方向向目標(biāo)位置移動(dòng)。其原理圖如圖2所示。

圖2 傳統(tǒng)勢場法原理圖

引力場與斥力場函數(shù)定義如下：

Uit(xi)=0.5αρ2(xi,xt)

(6)

(7)

式中：α、β分別表示引力、斥力增益系數(shù)；ρk max則表示無人機(jī)i所受障礙物k影響的最大范圍。

但應(yīng)用傳統(tǒng)勢場法進(jìn)行航跡規(guī)劃時(shí)，無人機(jī)在航跡前方具有規(guī)律排布的障礙附近時(shí)，極易因所受的合力勢場作用變?yōu)榱愣萑刖植繕O小點(diǎn)，造成停滯飛行的情況；無人機(jī)在即將抵達(dá)目標(biāo)位置時(shí)，若目標(biāo)前存在一定的障礙物布置，則此時(shí)的無人機(jī)所受斥力勢場極有可能大于引力勢場，造成不能到達(dá)目標(biāo)位置的可能。因此需要將傳統(tǒng)勢場函數(shù)加以改進(jìn)后應(yīng)用，故在文獻(xiàn)[6]的基礎(chǔ)上采用分段思想、增加旋轉(zhuǎn)力、機(jī)間勢場力等手段將傳統(tǒng)勢場法加以改進(jìn)后應(yīng)用于本文所提無人機(jī)編隊(duì)模型。

2.2 勢場函數(shù)改進(jìn)

首先應(yīng)用分段思想對傳統(tǒng)勢場函數(shù)進(jìn)行改進(jìn)：

(8)

(9)

式中：α1、α2表示引力增益系數(shù)，且α1>α2，即當(dāng)其小于無人機(jī)間的最佳間距時(shí)，以通過增加引力增益系數(shù)的手段增大引力場作用，解決目標(biāo)不可達(dá)問題；β表示斥力增益系數(shù)。

對其求負(fù)梯度可得到如下的引力與斥力模型：

(10)

fik(xi)=-▽Uik(xi)=

(11)

式中：θit為無人機(jī)i所受引力的角度；θik為無人機(jī)i所受障礙物k斥力的角度。

另外，為解決局部極小值問題，在無人機(jī)所受合力計(jì)算中引入一個(gè)附加旋轉(zhuǎn)力，定義在無人機(jī)受到斥力作用的垂直方向上，它的指向與目標(biāo)側(cè)一致，表達(dá)式如下：

fic(xi)=[fic(xi)cosθic,fic(xi)sinθic]

(12)

式中：θic為無人機(jī)i所受旋轉(zhuǎn)力的角度；ζ表示旋轉(zhuǎn)力增益系數(shù)；η表示旋轉(zhuǎn)力啟動(dòng)參數(shù)；ρk min則表示障礙物k影響的最小范圍。

無人機(jī)間的相對最佳間距保持則以機(jī)間勢場力來精確控制實(shí)現(xiàn)，定義為無人機(jī)接近時(shí)，機(jī)間勢場力表現(xiàn)為斥力；當(dāng)無人機(jī)遠(yuǎn)離時(shí)，勢場則表現(xiàn)為引力。根據(jù)圖1所示編隊(duì)結(jié)構(gòu)，具體表達(dá)式如下：

fij(xi)=-▽Uij(xi)=

(13)

式中：θij為無人機(jī)i與j之間勢場力的角度；ε表示無人機(jī)間勢場力增益函數(shù)；ρa(bǔ) max、ρa(bǔ) min則表示無人機(jī)間距允許的最大值和最小值。

因此無人機(jī)i所受的勢場合力控制輸入如下：

fi=fit(xi)+fio(xi)+fic(xi)+fia(xi)

(14)

考慮被控對象為質(zhì)點(diǎn)模型，那么無人機(jī)的速度vi=fit，則預(yù)規(guī)劃得到無人機(jī)下一時(shí)刻的位置為xi(t+1)=xi(t)+viΔt，Δt表示時(shí)間步長。

但人工勢場法只能保證編隊(duì)的相對架構(gòu)，不能實(shí)現(xiàn)狀態(tài)的一致性。文獻(xiàn)[11]以領(lǐng)導(dǎo)者狀態(tài)作為期望狀態(tài)值以實(shí)現(xiàn)編隊(duì)協(xié)同控制，然而領(lǐng)導(dǎo)者一旦出現(xiàn)問題編隊(duì)將難以為繼。因此本文將采用預(yù)規(guī)劃得到的勢場精確輸出作為期望狀態(tài)值，結(jié)合一致性理論進(jìn)行狀態(tài)誤差修正，達(dá)到編隊(duì)狀態(tài)收斂控制目標(biāo)。

3 無人機(jī)編隊(duì)協(xié)同避障控制器設(shè)計(jì)

3.1 編隊(duì)協(xié)同避障控制器設(shè)計(jì)

對于編隊(duì)中處于任意狀態(tài)的無人機(jī)，為了保證在安全避碰避障到達(dá)目標(biāo)的前提下還能夠?qū)崿F(xiàn)無人機(jī)編隊(duì)的一致性，提出應(yīng)用于多無人機(jī)系統(tǒng)的編隊(duì)協(xié)同避障控制協(xié)議如下：

i∈{1,2,…,n}

(15)

(16)

3.2 穩(wěn)定性分析

將式(15)代入式(1)可得無人機(jī)i運(yùn)動(dòng)學(xué)模型為：

i∈{1,2,…,n}

(17)

(18)

(19)

其中多機(jī)系統(tǒng)的拉普拉斯矩陣如下:

(20)

因此無人機(jī)編隊(duì)系統(tǒng)的運(yùn)動(dòng)學(xué)模型表示為:

(21)

首先考慮以下齊次方程的穩(wěn)定性:

(22)

選取編隊(duì)系統(tǒng)的總能量構(gòu)造Hamilton函數(shù)，其包括勢能以及動(dòng)能函數(shù)，具體定義如下:

(23)

對其求導(dǎo)得:

vT·(-f-γ0Lx-γ1Lv+f+γ0Lx)=

(24)

非齊次方程一般解為通解與其特解之和，那么在t→∞時(shí)，式(21)的解漸變收斂為[x′,v′]T。

綜上：當(dāng)增益設(shè)置為γ0>0,γ1>0，編隊(duì)內(nèi)所有無人機(jī)的狀態(tài)值將趨于最大不變集中，且所有無人機(jī)的狀態(tài)值都能夠收斂到期望位置和期望速度并趨于一致，機(jī)間間距能夠收斂到最佳距離并漸進(jìn)保持穩(wěn)定，形成編隊(duì)隊(duì)形。

接下來采用反證法進(jìn)行無人機(jī)機(jī)間勢場力能夠有效避免機(jī)間碰撞的證明。

當(dāng)無人機(jī)的初始狀態(tài)(x(0),v(0))∈LH(c)時(shí)，假設(shè)t=t1>0時(shí)刻至少存在兩架不同的無人機(jī)發(fā)生碰撞，定義兩架無人機(jī)分別為k、l，那么此時(shí)機(jī)間勢場能量Ukl(xk(t1))→∞，那么：

Ukl(xk(t1))→∞

(25)

這與H(x(t),v(t))≤c的事實(shí)相悖，即?t≥0，反證法的假設(shè)條件是不成立的，可得無人機(jī)之間不會(huì)有相互碰撞的情況發(fā)生。

對于無人機(jī)編隊(duì)在飛行途中能否有效躲避障礙物的證明同樣可以采用反證法進(jìn)行相似的分析驗(yàn)證。由此無人機(jī)編隊(duì)系統(tǒng)的穩(wěn)定性與一致收斂性得以證明。

4 仿真分析

為了驗(yàn)證所提出算法的有效性，模擬實(shí)際環(huán)境中無人機(jī)可能遇到的復(fù)雜障礙物，分別以傳統(tǒng)勢場法與協(xié)同避障控制算法引導(dǎo)無人機(jī)編隊(duì)進(jìn)行航跡仿真。實(shí)驗(yàn)考慮四架可相互通信的無人機(jī)編隊(duì)系統(tǒng)，其通信拓補(bǔ)圖如圖1所示，則其拉普拉斯矩陣為：

(26)

首先設(shè)置編隊(duì)控制協(xié)議相應(yīng)的參數(shù)：n=4，γ0=0.1，γ1=10，α1=1.5，α2=3.0，β=3.0，ζ=0.05，η=350，ε=4 000。選取代表性的圓、正六邊形、凸形、L形模擬真實(shí)障礙，盡可能表示無人機(jī)飛行中可能遇到的各類障礙邊界，其質(zhì)心坐標(biāo)位置分別為(40,50)、(45,30)、(21.5,36.5)、(28,15)，ρk max=20，ρk min=0.1。四架無人機(jī)的位置狀態(tài)信息如表1所示，ρa(bǔ)=3，ρa(bǔ) max=10，ρa(bǔ) min=0，期望速度設(shè)為5 m/s。

表1 無人機(jī)位置信息

采用傳統(tǒng)勢場法引導(dǎo)式(1)所描述的四架無人機(jī)在上述復(fù)雜飛行環(huán)境中，由初始位置出發(fā)所得運(yùn)動(dòng)軌跡如圖3所示，而采用式(14)-式(16)所示的無人機(jī)編隊(duì)協(xié)同避障控制策略所得仿真結(jié)果如圖4-圖7所示。

圖3 傳統(tǒng)勢場法作用下的無人機(jī)編隊(duì)運(yùn)動(dòng)軌跡

圖4 改進(jìn)算法作用下的無人機(jī)編隊(duì)運(yùn)動(dòng)軌跡

圖5 各架無人機(jī)間的相對距離

圖6 各架無人機(jī)x軸速度分量

圖7 各架無人機(jī)y軸速度分量

由圖3可看出，傳統(tǒng)勢場法在作用于無人機(jī)編隊(duì)時(shí)，各無人機(jī)之間不能保持相對間距且在障礙物間隙陷入死鎖，未能完成飛行任務(wù)。而圖4中，在本文所提出的編隊(duì)控制策略作用下，四架無人機(jī)能夠良好實(shí)現(xiàn)編隊(duì)避障，并最終到達(dá)目標(biāo)位置。結(jié)合圖5可得，無人機(jī)間距始終不為零，說明控制策略能夠保證無人機(jī)的飛行安全，良好實(shí)現(xiàn)機(jī)間避碰；當(dāng)編隊(duì)安全通過障礙物區(qū)域之后，四架無人機(jī)在18 s左右能夠迅速地穩(wěn)定到期望的編隊(duì)隊(duì)形。

由圖6、圖7分析可得，在控制策略作用下的各架無人機(jī)在x軸與y軸方向上的速度分量能夠在較短的時(shí)間內(nèi)收斂至期望速度，并趨于一致。另外在即將抵達(dá)目標(biāo)位置時(shí)，四架無人機(jī)的速度呈現(xiàn)出一定的波動(dòng)，這說明在無人機(jī)所受目標(biāo)引力勢場作用上進(jìn)行的改進(jìn)手段是具有有效性的。

綜上，利用本文提出的無人機(jī)編隊(duì)協(xié)同避障控制策略，當(dāng)無人機(jī)編隊(duì)在飛往目的地的過程中能夠很好地保證自身的安全穩(wěn)定，并能夠在避障避碰的前提下以較快的速度收斂至期望的編隊(duì)隊(duì)形以及期望航行速度，實(shí)現(xiàn)位置以及速度的一致收斂，從而達(dá)到預(yù)期設(shè)計(jì)目標(biāo)，驗(yàn)證了所提出算法的優(yōu)越性。

5 結(jié) 語

本文針對采用二階積分器模型的無人機(jī)，通過圖論描述無人機(jī)編隊(duì)之間的通信關(guān)系，以人工勢場產(chǎn)生的精確控制輸入和一致性理論為基礎(chǔ)，提出一種基于圖論法和人工勢場法相結(jié)合的無人機(jī)編隊(duì)協(xié)同避障控制策略，利用LaSalle不變集原理討論了系統(tǒng)的穩(wěn)定性，證明了所提出控制算法的收斂一致性。最后通過仿真模擬來印證此控制策略的有效性與優(yōu)越性，結(jié)果表明無人機(jī)編隊(duì)在控制策略作用下能夠成功避開障礙，以較短的時(shí)間收斂至期望速度以及編隊(duì)隊(duì)形，并保持穩(wěn)定到達(dá)目標(biāo)位置。