基于改進(jìn)人工勢(shì)場(chǎng)的無人機(jī)編隊(duì)避障控制研究

張佳龍,閆建國(guó),張普,王奔馳

(1.西北工業(yè)大學(xué)自動(dòng)化學(xué)院,710129,西安;2.陜西服裝工程學(xué)院經(jīng)濟(jì)管理學(xué)院,712046,西安;3.空軍工程大學(xué)航空工程學(xué)院,710038,西安)

隨著計(jì)算機(jī)、傳感器以及通信技術(shù)的快速發(fā)展,多機(jī)編隊(duì)系統(tǒng)在三維空間的控制已經(jīng)成為廣泛研究的課題,并具有重要的工程應(yīng)用價(jià)值,包括科學(xué)研究、交通運(yùn)輸、地質(zhì)勘查、執(zhí)行各種任務(wù)等[1-3]。

在多無人機(jī)編隊(duì)控制中,已提出了很多編隊(duì)結(jié)構(gòu)、不同的控制策略以及控制算法,這些方法主要有“長(zhǎng)機(jī)-僚機(jī)”法、基于行為法、虛擬結(jié)構(gòu)法以及基于一致性協(xié)同編隊(duì)避障控制算法,每一種控制方法都有自己的優(yōu)缺點(diǎn)。在“長(zhǎng)機(jī)-僚機(jī)”方法中,任意一架無人機(jī)均可作為長(zhǎng)機(jī),其余無人機(jī)作為僚機(jī),這種控制方法不足之處在于僚機(jī)對(duì)于長(zhǎng)機(jī)沒有實(shí)時(shí)的反饋。例如,在文獻(xiàn)[4]中,作者提出一種具有分布估計(jì)量的分布式控制方案,在三維噪聲環(huán)境中,僚機(jī)只需知道與長(zhǎng)機(jī)的相對(duì)位置,就可實(shí)現(xiàn)編隊(duì)的控制。基于一致性算法的協(xié)同編隊(duì)控制是一種分布式控制方法,具有網(wǎng)絡(luò)結(jié)構(gòu)靈活性優(yōu)勢(shì)[5-8],可實(shí)現(xiàn)多通道復(fù)合控制避障。研究多智能體系統(tǒng)協(xié)同控制問題,也主要應(yīng)用一致性算法解決[9-11],可極大地簡(jiǎn)化問題的復(fù)雜程度。

在研究無人機(jī)避障問題中,人工勢(shì)場(chǎng)方法是普遍應(yīng)用的算法,因?yàn)樗?jiǎn)單、實(shí)用、工程實(shí)踐性強(qiáng)。所謂人工勢(shì)場(chǎng)方法,即無人機(jī)與無人機(jī)之間被視為相互吸引的球體,當(dāng)兩架無人機(jī)靠近時(shí)相互吸引;當(dāng)它們遠(yuǎn)離時(shí)相互排斥,而且之間距離越小斥力越大,使得兩架無人機(jī)相互遠(yuǎn)離,直至達(dá)到平衡狀態(tài)。這里的勢(shì)場(chǎng)法采用梯度勢(shì)場(chǎng)法,也就是說勢(shì)場(chǎng)的負(fù)梯度作為作用在無人機(jī)上的虛擬力,障礙物對(duì)無人機(jī)產(chǎn)生斥力,長(zhǎng)機(jī)對(duì)兩僚機(jī)產(chǎn)生引力,僚機(jī)之間產(chǎn)生斥力,以至于無人機(jī)維持編隊(duì)形狀而且無人機(jī)與障礙物避免發(fā)生碰撞。當(dāng)兩架無人機(jī)和障礙物共線時(shí),其中一架無人機(jī)處于平衡位置,無法擺脫平衡狀態(tài),此時(shí)無人機(jī)編隊(duì)無法繞開障礙物,易發(fā)生碰撞。本文采用人工勢(shì)場(chǎng)的兩個(gè)方向的矢量場(chǎng),分別為x-y平面和y-z平面,無人機(jī)編隊(duì)接近障礙物周圍產(chǎn)生橢球性人工勢(shì)場(chǎng),兩個(gè)平面的矢量場(chǎng)會(huì)打破這種平衡,使無人機(jī)編隊(duì)以最優(yōu)的路徑避開障礙物。在研究人工勢(shì)場(chǎng)方法避障的過程中,存在的共性問題是無人機(jī)編隊(duì)、障礙物以及目標(biāo)之間存在局部最小值,即無人機(jī)接近障礙物時(shí),斥力處于無人機(jī)運(yùn)動(dòng)的反方向,使其滯留在最小值處,不能到達(dá)目標(biāo)位置[12-14]。解決辦法之一是在垂直方向或者水平方向加入外界干擾打破這種平衡,然而無人機(jī)的速度很難保持,尤其在多無人機(jī)編隊(duì)中。近年來,很多學(xué)者提出了有關(guān)多無人機(jī)編隊(duì)在二維平面跟隨和追蹤目標(biāo)以到達(dá)目標(biāo)點(diǎn)的改進(jìn)的人工勢(shì)場(chǎng)避障算法[15-16]。無人機(jī)避障算法能夠使編隊(duì)在避障過程中保持良好的穩(wěn)定性和魯棒性[17],例如無人機(jī)編隊(duì)在三維空間以平滑的轉(zhuǎn)彎半徑和最優(yōu)的路徑避障,追蹤目標(biāo)到達(dá)目標(biāo)點(diǎn)。同時(shí),一些學(xué)者提出無人機(jī)協(xié)同編隊(duì)模型預(yù)測(cè)控制方法,對(duì)任意形狀的障礙物進(jìn)行避障策略研究,這對(duì)無人機(jī)編隊(duì)的避障理論進(jìn)行了補(bǔ)充。

多無人機(jī)編隊(duì)系統(tǒng)預(yù)估運(yùn)動(dòng)目標(biāo)的軌跡、避開障礙物并完成追蹤運(yùn)動(dòng)的目標(biāo),是一個(gè)復(fù)雜的過程,也是待解決的核心問題之一。本文針對(duì)人工勢(shì)場(chǎng)方法進(jìn)行避障易陷于局部最小值的問題(當(dāng)無人機(jī)、目標(biāo)以及障礙物三者共線時(shí),無人機(jī)處于三者中間,受到目標(biāo)和障礙物的斥力等大反向,此時(shí)無人機(jī)處于局部最小位置處),提出一種三維空間復(fù)合矢量人工勢(shì)場(chǎng)法應(yīng)用到無人機(jī)編隊(duì)中,通過采用復(fù)合人工勢(shì)場(chǎng)方法,使無人機(jī)編隊(duì)避障后能夠保持無人機(jī)在飛行過程中良好的穩(wěn)定性和魯棒性。

1 問題描述

本文以3架無人機(jī)和一架虛擬長(zhǎng)機(jī)構(gòu)成的正三角形編隊(duì)作為控制目標(biāo),其中3架無人機(jī)在三角形的3個(gè)頂點(diǎn)處作為僚機(jī),虛擬長(zhǎng)機(jī)在三角形的幾何中心。無人機(jī)編隊(duì)中的單機(jī)在三維空間的動(dòng)力學(xué)質(zhì)點(diǎn)模型如下式[17]所示

(1)

圖1 無人機(jī)坐標(biāo)系統(tǒng)模型

無人機(jī)控制系統(tǒng)包括內(nèi)環(huán)和外環(huán),內(nèi)環(huán)控制系統(tǒng)的輸入信號(hào)分別為發(fā)動(dòng)機(jī)推力、升力以及傾斜角,外環(huán)系統(tǒng)將控制信號(hào)通過傳感器反饋給執(zhí)行元件做出相應(yīng)的機(jī)動(dòng)。式(1)通過反饋線性化,得到線性化模型如下

(2)

式中(μx,μy,μz)是虛擬加速度,作為控制輸入。虛擬控制輸入信號(hào)通過式(2)線性化模型定義,真實(shí)控制輸入由下式獲得

(3)

2 無人機(jī)編隊(duì)控制

本文以3架無人機(jī)和一架虛擬長(zhǎng)機(jī)構(gòu)成的編隊(duì)系統(tǒng)作為控制體,將所提出的改進(jìn)人工勢(shì)場(chǎng)算法應(yīng)用到無人機(jī)編隊(duì)控制中,實(shí)現(xiàn)了三維空間避障,其編隊(duì)模型如圖2所示。

圖2 無人機(jī)編隊(duì)控制模型結(jié)構(gòu)

2.1 單機(jī)模型避障控制算法

無人機(jī)編隊(duì)中每架無人機(jī)運(yùn)動(dòng)軌跡均是兩種矢量的控制力復(fù)合作用的結(jié)果。復(fù)合控制力由兩部分組成,如下式所示

μn=Fna+Fr

(4)

人工勢(shì)場(chǎng)的引力Fna能夠保證無人機(jī)保持隊(duì)形,而且控制無人機(jī)到達(dá)以虛擬長(zhǎng)機(jī)為球心的球體表面,可以表示為

Fna=(Fxna,Fyna,Fzna)

(5)

其中

(6)

式中:(xl,yl,zl)表示虛擬長(zhǎng)機(jī)的坐標(biāo);ks表示增益系數(shù)。

無人機(jī)所受人工勢(shì)場(chǎng)斥力的合力Fr能夠使得每架無人機(jī)均勻地分布在以虛擬長(zhǎng)機(jī)坐標(biāo)(xl,yl,zl)為球心的球體表面,每架無人機(jī)帶有正電荷或者負(fù)電荷,帶有相同電荷的無人機(jī)互相排斥,反之互相吸引。控制力可以保持無人機(jī)均勻分布在以虛擬長(zhǎng)機(jī)坐標(biāo)(xl,yl,zl)為球心、ra為半徑的球體表面,當(dāng)無人機(jī)所受到斥力的合力方向與球體表面相切時(shí),則該無人機(jī)處于平衡狀態(tài),此時(shí)所受的合力為0,表示每架無人機(jī)之間的相對(duì)距離相等,構(gòu)成了正三角形隊(duì)形。無人機(jī)之間的斥力定義如下

(7)

每架無人機(jī)所受斥力可以解耦成x、y、z三個(gè)坐標(biāo)軸方向上的分斥力,即

(8)

其中

(9)

同時(shí),復(fù)合控制力如下式所示

(10)

由式(8)和式(9)可知,無人機(jī)所受到的斥力Fr與1/rni成正比,并且通過選擇恰當(dāng)?shù)某饬杀苊鉄o人機(jī)之間發(fā)生碰撞。在式(10)中,控制力(μxn,μyn,μzn)使得無人機(jī)向球體表面移動(dòng)至平衡位置處。無人機(jī)運(yùn)動(dòng)方向如圖3所示。

P表示無人機(jī)初始位置;P′表示無人機(jī)移動(dòng)后的位置;Q2、Q1表示P、P′在x0-o-y0平面的投影;Q3表示P′在z0-o-Q2平面的投影圖3 無人機(jī)運(yùn)動(dòng)方向示意圖

2.2 虛擬長(zhǎng)機(jī)控制策略

無人機(jī)編隊(duì)在飛行過程中,虛擬長(zhǎng)機(jī)運(yùn)動(dòng)軌跡作為期望路徑,僚機(jī)跟蹤長(zhǎng)機(jī)構(gòu)成三角形編隊(duì),長(zhǎng)機(jī)受到目標(biāo)的引力作用向目標(biāo)點(diǎn)運(yùn)動(dòng)。長(zhǎng)機(jī)的位置pl=(xl,yl,yl),目標(biāo)的位置pt=(xt,yt,yt),無人機(jī)所受的引力如下式所示

ifr

(11)

else

(12)

當(dāng)長(zhǎng)機(jī)接近目標(biāo)且它們之間的相對(duì)速度增大時(shí),為防止發(fā)生碰撞,可通過控制長(zhǎng)機(jī)所受的阻力來實(shí)現(xiàn)避障的目的。定義阻力如下

(13)

式中:km為放大系數(shù)。每架無人機(jī)所受到的控制力是引力和阻力的合力,即

(14)

通過以上分析,對(duì)單架無人機(jī)和虛擬長(zhǎng)機(jī)的最優(yōu)控制,能夠確保無人機(jī)編隊(duì)系統(tǒng)安全避障且保持良好的穩(wěn)定性,同時(shí)選擇最優(yōu)路徑到達(dá)目標(biāo)點(diǎn)。

3 無人機(jī)避障策略

無人機(jī)編隊(duì)執(zhí)行作戰(zhàn)任務(wù)過程中,避障環(huán)節(jié)是關(guān)鍵因素之一。避障結(jié)束,快速且以最優(yōu)路徑集結(jié)成三角形編隊(duì),是本文提出避障算法的核心。本文提出一種三維空間復(fù)合勢(shì)場(chǎng)方法來解決這一問題。本節(jié)針對(duì)單架無人機(jī)和多機(jī)編隊(duì)系統(tǒng)分別提出兩種控制算法,能夠?qū)崿F(xiàn)快速避障的目的。

3.1 無人機(jī)避障方法

在三維空間中,單架無人機(jī)追蹤虛擬長(zhǎng)機(jī),其運(yùn)動(dòng)軌跡是設(shè)計(jì)避障算法的關(guān)鍵。假設(shè)無人機(jī)視覺傳感器感受到的障礙物近似為圓柱體,其表面半徑為r、高度為h,且圓柱體周圍產(chǎn)生的人工勢(shì)場(chǎng)近似為橢球形狀。人工勢(shì)場(chǎng)以最小體積包絡(luò)住障礙物,即包絡(luò)住圓柱體的最小橢球體。橢球體滿足以下方程

(15)

無人機(jī)編隊(duì)系統(tǒng)采用人工勢(shì)場(chǎng)避障,主要取決斥力。當(dāng)無人機(jī)編隊(duì)接近障礙物時(shí),無人機(jī)受到的斥力與其運(yùn)動(dòng)速度相反,無人機(jī)會(huì)處于局部最小位置,本文提出的復(fù)合人工勢(shì)場(chǎng)將導(dǎo)引無人機(jī)避開此位置。復(fù)合人工勢(shì)場(chǎng)能夠包絡(luò)住橢球體,使無人機(jī)編隊(duì)以最優(yōu)路徑繞過障礙物。復(fù)合人工勢(shì)場(chǎng)由分別平行于x-y平面和y-z平面的兩種人工勢(shì)場(chǎng)復(fù)合而成,其示意圖如圖4所示。

圖4 三維空間障礙物周圍旋轉(zhuǎn)矢量場(chǎng)

由圖4可知,無人機(jī)避障軌跡是由兩種旋轉(zhuǎn)矢量場(chǎng)疊加作用的結(jié)果,同時(shí)無人機(jī)以最優(yōu)軌跡快速避障,然后快速集結(jié)到達(dá)目標(biāo)點(diǎn)位置。此處φn和γn分別受兩個(gè)分矢量場(chǎng)影響,其表達(dá)式如下

(16)

無人機(jī)運(yùn)動(dòng)軌跡在x-y平面的投影曲線,只受與x-y平面平行的旋轉(zhuǎn)矢量人工勢(shì)場(chǎng)影響。旋轉(zhuǎn)矢量人工勢(shì)場(chǎng)分逆時(shí)針和順時(shí)針兩個(gè)方向。平行x-y平面的旋轉(zhuǎn)矢量場(chǎng)的運(yùn)動(dòng)學(xué)方程如下

(17)

(18)

3.2 無人機(jī)避障路徑優(yōu)化

無人機(jī)編隊(duì)進(jìn)入避障區(qū)域,避開局部最小值位置且以最短的路徑繞過障礙物。針對(duì)無人機(jī)以最優(yōu)路徑避障,本節(jié)提出無人機(jī)編隊(duì)避障優(yōu)化策略,同時(shí)分別以單架無人機(jī)和多架無人機(jī)編隊(duì)系統(tǒng)為研究對(duì)象,提出控制力概念。

當(dāng)單架無人機(jī)進(jìn)入障礙物區(qū)域或者接近障礙物時(shí),具有三維空間旋轉(zhuǎn)矢量的人工勢(shì)場(chǎng)使無人機(jī)以期望的路徑繞過障礙物。障礙物近似為球體,其最小球體半徑為r0,無人機(jī)與障礙物之間的相對(duì)距離滿足以下等式

(19)

無人機(jī)避障所需的控制力如下式所示

Fnr=(Fxnr,Fynr,Fznr)

(20)

則控制力關(guān)系如下

ifra

(21)

else

Fr=Fdes

(22)

對(duì)于虛擬長(zhǎng)機(jī),所需的控制力為

Fdes=(Fxl,Fyl,Fzl)

對(duì)于每架僚機(jī),所需的控制力為

Fdes=(Fxna+Fnr,Fyna+Fnr,Fzna+Fnr)

基于無人機(jī)在x-y平面的運(yùn)動(dòng)軌跡受旋轉(zhuǎn)矢量人工勢(shì)場(chǎng)的影響,則無人機(jī)避障所需的控制力定義如下

(23)

或者

(24)

式中k0是增益系數(shù)。平行于x-y平面的順時(shí)針和逆時(shí)針方向的人工勢(shì)場(chǎng)如圖5所示。

圖5 平行于x-y平面的人工勢(shì)場(chǎng)順時(shí)針和逆時(shí)針方向示意圖

φn、χn、ρn可由下式計(jì)算

(25)

障礙物周圍產(chǎn)生的旋轉(zhuǎn)矢量方向分兩種情況:當(dāng)φn≥ρn,則旋轉(zhuǎn)矢量是順時(shí)針方向;當(dāng)φn<ρn,則旋轉(zhuǎn)矢量是逆時(shí)針方向。

基于無人機(jī)在y-z平面的運(yùn)動(dòng)軌跡受旋轉(zhuǎn)矢量人工勢(shì)場(chǎng)的影響,則無人機(jī)避障所需的控制力定義如下

(26)

或者

(27)

平行于y-z平面的順時(shí)針和逆時(shí)針方向的人工勢(shì)場(chǎng)如圖6所示。

圖6 平行于y-z平面的人工勢(shì)場(chǎng)順時(shí)針和逆時(shí)針方向示意圖

γn、ζn和?n可由下式計(jì)算

(28)

障礙物周圍產(chǎn)生的旋轉(zhuǎn)矢量方向分兩種情況:當(dāng)γn≥?n,則旋轉(zhuǎn)矢量方向向下;當(dāng)γn

if |γn-ζn|<|φn-χn|,則

(29)

else

(30)

將無人機(jī)編隊(duì)系統(tǒng)避障控制力歸一化如下

(31)

4 仿真結(jié)果分析

本文使用半物理仿真平臺(tái)進(jìn)行仿真實(shí)驗(yàn)。仿真過程忽略外界因素干擾,其中包括側(cè)向風(fēng)和渦旋效應(yīng),同時(shí)認(rèn)為障礙物是運(yùn)動(dòng)狀態(tài)。設(shè)定初始條件為:4架無人機(jī)構(gòu)成正三角形編隊(duì),其中3架無人機(jī)和1架虛擬長(zhǎng)機(jī),且長(zhǎng)機(jī)與僚機(jī)之間信息雙向傳遞;在無人機(jī)編隊(duì)靠近運(yùn)動(dòng)的障礙物過程中,障礙物簡(jiǎn)化為圓柱體,即禁飛區(qū),其周圍的人工勢(shì)場(chǎng)近似為橢球體。基于無人機(jī)編隊(duì)模型和提出的復(fù)合矢量人工勢(shì)場(chǎng)算法,則無人機(jī)編隊(duì)系統(tǒng)在不同情形下的仿真圖如圖7～圖10所示。

圖7 無人機(jī)在不同算法下的避障軌跡圖

圖8 “長(zhǎng)機(jī)-僚機(jī)”控制策略的避障圖

圖9 無人機(jī)在兩種人工勢(shì)場(chǎng)作用下橫側(cè)向距離誤差對(duì)比圖

圖10 無人機(jī)編隊(duì)在兩種人工勢(shì)場(chǎng)作用下航向角變化對(duì)比圖

由圖7可知:一般避障算法應(yīng)用于無人機(jī)編隊(duì)避障是通過設(shè)計(jì)內(nèi)環(huán)控制器,來保證無人機(jī)編隊(duì)中靠近障礙物的僚機(jī)與障礙物之間的相對(duì)距離大于安全距離,但避障后自調(diào)整速度慢、路徑較遠(yuǎn);人工勢(shì)場(chǎng)算法最大的優(yōu)勢(shì)是避障路徑較短、動(dòng)態(tài)響應(yīng)較快、穩(wěn)定較好,但在連續(xù)避障過程中可能會(huì)出現(xiàn)遲滯現(xiàn)象;將具有三維空間旋轉(zhuǎn)矢量的人工勢(shì)場(chǎng)應(yīng)用到編隊(duì)避障過程,無人機(jī)編隊(duì)可連續(xù)避障,避開局部最小位置,且以最優(yōu)路徑避開障礙物,而后保持良好的穩(wěn)定性和魯棒性快速集結(jié)成正三角形編隊(duì)飛行。

由圖8可知,虛擬長(zhǎng)機(jī)按照期望的軌跡飛行,僚機(jī)跟隨其后構(gòu)成編隊(duì)有效地避開障礙物,之后又重組隊(duì)形飛行。圖中長(zhǎng)機(jī)和僚機(jī)的初始位置分別為(2,1,1.5)和(1,1,1.2),且障礙物中心位置為(2.2,1.5,0.8)。設(shè)置kr=4,ks=5,kt=2,km=1.5,D=2,ra=1,基于三維空間旋轉(zhuǎn)矢量的人工勢(shì)場(chǎng)算法,在虛擬結(jié)構(gòu)和“長(zhǎng)機(jī)-僚機(jī)”控制策略情形下,無人機(jī)編隊(duì)接近障礙物以平滑的軌跡避開障礙物,然后集結(jié)成三角形編隊(duì)追蹤目標(biāo)到達(dá)目標(biāo)點(diǎn)。在此過程中,無人機(jī)編隊(duì)保持良好的穩(wěn)定性,有效地驗(yàn)證了所提出算法的有效性和可行性。

由圖9可知,在具有三維空間旋轉(zhuǎn)矢量的人工勢(shì)場(chǎng)中,無人機(jī)編隊(duì)與障礙物之間保持安全距離且選擇最優(yōu)路徑進(jìn)行連續(xù)避障,它們之間的橫側(cè)向距離誤差變化幅值很小,動(dòng)態(tài)響應(yīng)快,隊(duì)形保持良好。

由圖10可知:在一般人工勢(shì)場(chǎng)作用下,無人機(jī)通過視覺傳感器感知目標(biāo)后,以最大的安全距離避開障礙物,而且小幅度平滑轉(zhuǎn)彎的地方很少,曲線振顫頻率低且沒有較大的波峰或者波谷,這種避障方式不足之處在于避障路徑較遠(yuǎn)、損耗較大、效率較低;在具有三維空間旋轉(zhuǎn)矢量的人工勢(shì)場(chǎng)作用下,無人機(jī)進(jìn)行連續(xù)平滑轉(zhuǎn)彎,選擇最優(yōu)路徑避障,即無人機(jī)基本貼近障礙物包絡(luò)線運(yùn)動(dòng),曲線呈現(xiàn)連續(xù)等幅振顫趨勢(shì)。這說明無人機(jī)在保持安全距離的情況下,運(yùn)動(dòng)軌線基本與障礙物的包絡(luò)線相切,則航向角呈現(xiàn)一種連續(xù)小幅振蕩趨勢(shì)。

5 總結(jié)與展望

基于虛擬“長(zhǎng)機(jī)-僚機(jī)”控制策略,本文提出一種具有三維空間旋轉(zhuǎn)矢量的人工勢(shì)場(chǎng)方法,解決了無人機(jī)編隊(duì)避障過程易陷局部最小位置的問題。復(fù)合人工勢(shì)場(chǎng)由平行于x-y平面和y-z平面的人工勢(shì)場(chǎng)復(fù)合而成,可以避開障礙物周圍每個(gè)局部最小位置,而且以最優(yōu)路徑快速繞過障礙物,集結(jié)編隊(duì)飛行到達(dá)目標(biāo)點(diǎn)。在此過程中,引力使3架無人機(jī)和虛擬長(zhǎng)機(jī)保持正三角形隊(duì)形,同時(shí)驅(qū)使無人機(jī)編隊(duì)朝向目標(biāo)運(yùn)動(dòng),而斥力能夠避免無人機(jī)之間發(fā)生碰撞,同時(shí)也避免無人機(jī)與障礙物發(fā)生碰撞,達(dá)到避障的目的。當(dāng)無人機(jī)速度過大或者障礙物周圍產(chǎn)生的球形人工勢(shì)場(chǎng)強(qiáng)度較弱時(shí),可能會(huì)發(fā)生碰撞,這種算法可能會(huì)失效,因此下一步工作需綜合考慮多種因素,盡可能貼近工程實(shí)際。