楊鐘煜，余自權(quán)，程月華，徐貴力

（南京航空航天大學(xué)自動化學(xué)院，江蘇南京 210016）

0 引言

無人機(jī)在民用和軍事[1-3]領(lǐng)域都發(fā)揮著越來越重要的作用，如電力線路檢查、水土保持、森林防火[4-5]等。搜索并跟蹤移動地面目標(biāo)也是無人機(jī)預(yù)測目標(biāo)意圖并采取主動措施所需的主要能力，具體應(yīng)用包括邊境巡邏[6-7]、空中監(jiān)視[8-9]和警察執(zhí)法等。四旋翼無人機(jī)為最常見的一類無人機(jī)，它具有可垂直起降、控制靈活和適應(yīng)性強等優(yōu)點，在民用和軍用領(lǐng)域都有廣泛應(yīng)用。

四旋翼無人機(jī)在編隊執(zhí)行任務(wù)中可能遭遇外界障礙物、群內(nèi)可能發(fā)生機(jī)間碰撞，造成巨大損失，影響無人機(jī)飛行安全。目前針對避障問題應(yīng)用較多的方法有人工勢場法[10-12]、動態(tài)窗口法[13-14]、基于行為法[15-17]等。但在無人機(jī)編隊執(zhí)行任務(wù)時，需要其進(jìn)行編隊飛行、目標(biāo)趨近和避障避撞等幾項行為，而這些行為之間常存在沖突，基于行為的方法能很好地調(diào)節(jié)系統(tǒng)行為，解決沖突問題。整個行為調(diào)節(jié)的效果主要取決于行為選擇機(jī)制。目前，行為選擇機(jī)制主要有行為抑制法、加權(quán)平均法和模糊邏輯法[18]等。文獻(xiàn)[19]提出的零空間行為法（NSB）即為模糊邏輯法的1種。在保持首要任務(wù)完成的前提下，可以充分利用零空間完成其他次要任務(wù)，具有實時性強、各子任務(wù)間零沖突的優(yōu)點。文獻(xiàn)[20]針對自主水下機(jī)器人在復(fù)雜海底環(huán)境駛向目標(biāo)點的過程中可能遇到動、靜態(tài)障礙物的問題，設(shè)計了基于零空間行為法的避障策略。文獻(xiàn)[21]提出了1 種基于零空間和滑模的移動機(jī)器人控制方法，實現(xiàn)了隊形保持和隊形內(nèi)避障的協(xié)調(diào)控制。文獻(xiàn)[22]采用基于零空間的行為控制對各無人機(jī)的行為進(jìn)行融合，解決了行為間的沖突問題。但現(xiàn)有文獻(xiàn)中研究對象大多為智能體，并未考慮對應(yīng)實際模型，且大多僅研究了路徑規(guī)劃上的避障策略，并未研究相應(yīng)具體的控制方法。

考慮到這一研究現(xiàn)狀，本文基于零空間方法，針對四旋翼無人機(jī)在編隊執(zhí)行任務(wù)時可能遭遇障礙物問題，提出了1 種四旋翼無人機(jī)避障與協(xié)同編隊控制算法。首先，建立四旋翼無人機(jī)動力學(xué)模型，并建立虛擬控制量簡化控制模型；然后，基于零空間方法進(jìn)行避障與協(xié)同編隊控制算法研究，將無人機(jī)任務(wù)執(zhí)行分解為目標(biāo)趨向任務(wù)、避障避撞任務(wù)和協(xié)同編隊任務(wù)，并根據(jù)優(yōu)先級進(jìn)行任務(wù)融合得到期望速度；最后，基于PID方法設(shè)計控制律。

1 問題描述與預(yù)備知識

1.1 四旋翼無人機(jī)動力學(xué)模型

本文采用四旋翼無人機(jī)，其動力學(xué)模型表示為[23]：

式（1）中：xi、yi、zi為無人機(jī)的位置分量；?i、θi、ψi為無人機(jī)的姿態(tài)角，分別為滾轉(zhuǎn)角、俯仰角和偏航角；J為旋翼轉(zhuǎn)動慣量；Ix Iy Iz為3個軸的轉(zhuǎn)動慣量；m為機(jī)身的質(zhì)量；g為重力加速度；Ωri=ω1i-ω2i+ω3i-ω4i為4 個旋翼轉(zhuǎn)速的殘差；ωji(j=1,2,3,4)為4 個旋翼的轉(zhuǎn)速；U1i為無人機(jī)的總升力；U2i、U3i、U4i分別為橫滾力矩、俯仰力矩和偏航力矩。

四旋翼無人機(jī)是1個含有4個系統(tǒng)輸入、6個系統(tǒng)輸出的典型欠驅(qū)動系統(tǒng)。因此，根據(jù)四旋翼無人機(jī)動力學(xué)模型和坐標(biāo)系轉(zhuǎn)換矩陣，定義虛擬控制分量，使四旋翼無人機(jī)系統(tǒng)成為1 個6 個控制輸入和6 個控制輸出的全驅(qū)動系統(tǒng)。定義虛擬控制量：

進(jìn)一步推導(dǎo)可得：

式（3）～（8）中，?d,i、θd,i、ψd,i表示姿態(tài)角的期望值。

1.2 基于零空間的行為控制方法

式（9）兩邊對pi求導(dǎo)得：

式（9）中，Ji(pi)為δi的雅可比矩陣。則可得到速度的最小二乘解為：

式（11）得到了單個任務(wù)的速度輸出。如果存在多個任務(wù)，采用NSB的方法將次要任務(wù)的速度輸出投影到優(yōu)先任務(wù)的零空間上。第q個任務(wù)向較高一級任務(wù)零空間的投影為可表示為：

式（12）中，I為單位矩陣。以3個任務(wù)為例，得到速度輸出為：

式（13）中，下標(biāo)表示任務(wù)的優(yōu)先級。

2 基于零空間方法的避障與協(xié)同編隊控制算法研究

四旋翼無人機(jī)編隊飛行時，可將任務(wù)執(zhí)行分解為3種任務(wù)：避障避撞任務(wù)、協(xié)同編隊任務(wù)和目標(biāo)趨向任務(wù)。

2.1 避障避撞任務(wù)

在無人機(jī)飛行過程中，避障避撞任務(wù)能保證無人機(jī)的安全和完整，為后續(xù)任務(wù)的實現(xiàn)提供基本的保障，故其擁有最高的優(yōu)先級。假設(shè)障礙物在前進(jìn)方向上，目的是保持無人機(jī)與障礙物和相鄰無人機(jī)之間的安全距離，為方便分析與算法設(shè)計，將相鄰無人機(jī)同樣 視 為 障 礙 物 ，定 義 障 礙 物 坐 標(biāo) 為po,m=(xo,m,yo,m,zo,m)T，m=1,2,…,M。無人機(jī)可控的任務(wù)變量為：

式（14）中，Do,i為無人機(jī)與障礙物之間的距離。定義安全距離為do，對可控變量δo,i求雅克比矩陣，得到：

則避障避撞任務(wù)的輸出為：

式（16）中，λo,i為避障避撞增益系數(shù)。

在機(jī)間避撞時，提出1種優(yōu)先級機(jī)間避撞策略，即在需要考慮將相鄰無人機(jī)視為障礙物時，采取按優(yōu)先級選擇障礙物進(jìn)行避障的方式：將各無人機(jī)進(jìn)行編號，在避撞時會選擇在避障范圍內(nèi)比自身序號小的無人機(jī)進(jìn)行優(yōu)先避障，如2 號無人機(jī)只對1 號無人機(jī)進(jìn)行避撞，3 號無人機(jī)只對1 號和2 號無人機(jī)進(jìn)行避撞，以此類推。此種機(jī)間避撞策略可以有效避免重復(fù)的雙向選擇，避免執(zhí)行避撞行為的2 架無人機(jī)產(chǎn)生越飛越遠(yuǎn)的現(xiàn)象。

2.2 協(xié)同編隊任務(wù)

多無人機(jī)編隊任務(wù)涉及各個無人機(jī)之間的相互協(xié)作。為實現(xiàn)無人機(jī)編隊，以各無人機(jī)與編隊隊形中心的位置偏差為輸入建立任務(wù)函數(shù)。定義多無人機(jī)編隊隊形中心位置坐標(biāo)為pc=(xc,yc,zc)T，取無人機(jī)可控的任務(wù)變量為：

式（17）中，Df,i為第i架無人機(jī)與編隊隊形中心點之間的坐標(biāo)偏差。定義第i架無人機(jī)當(dāng)前位置點與編隊隊形中心點的期望坐標(biāo)偏差為df,i，對可控變量δf,i求雅克比矩陣，得到：

則協(xié)同編隊任務(wù)的輸出為：

式（19）中，λf,i為協(xié)同編隊增益系數(shù)。

2.3 目標(biāo)趨向任務(wù)

目標(biāo)趨向任務(wù)控制無人機(jī)向目標(biāo)點飛行。定義目標(biāo)點位置為pd,i=(xd,i,yd,i,zd,i)T，可控的任務(wù)變量為δd,i=pi。引入四旋翼無人機(jī)最大飛行速度、實時位置坐標(biāo)和飛行目標(biāo)位置坐標(biāo)，對增益系數(shù)進(jìn)行重新定義，保證規(guī)劃得到的最終速度始終在無人機(jī)飛行能力范圍內(nèi)。對可控變量δd,i求雅克比矩陣，得到：

則目標(biāo)趨向任務(wù)的輸出為：

式（21）～（23）中：λd,i為目標(biāo)趨向增益系數(shù)；vmax為無人機(jī)最大飛行速度；變量rd為趨向目標(biāo)任務(wù)的調(diào)節(jié)因子。

2.4 基于零空間方法的任務(wù)融合

得到避障避撞任務(wù)、協(xié)同編隊任務(wù)和目標(biāo)趨向任務(wù)的輸出后，考慮避障避撞任務(wù)優(yōu)先級最高，其次為協(xié)同編隊任務(wù)優(yōu)于目標(biāo)趨向任務(wù)，采取NSB的方法將其綜合，可以得到最終的速度輸出為：

2.5 無人機(jī)控制算法設(shè)計

采用PID 控制方法作為無人機(jī)位置和姿態(tài)控制方法，首先定義如下速度跟蹤誤差與姿態(tài)跟蹤誤差：

式（27）（28）中：Θi=(?i,θi,ψi)T；Θd,i=(?d,i,θd,i,ψd,i)T。

則可得到控制律如下：

式 （29） （30） 中 ：Uv,i=(Uxi,Uyi,Uzi)T；Ua,i=(U?i,Uθi,Uψi)T；KP、KI、KD、kP、kI、kD為待設(shè)計的參數(shù)對角矩陣，且其元素均為正常數(shù)。最后由式（3）～（6），即可計算得到控制輸入U1i、U2i、U3i、U4i。

3 仿真結(jié)果及分析

本文通過仿真對提出的基于零空間方法的四旋翼無人機(jī)避障與協(xié)同編隊控制算法進(jìn)行有效性驗證?？紤]編隊中共有4架四旋翼無人機(jī)（見表1）。彼此之間均可進(jìn)行信息交互，即每架無人機(jī)均可獲得其他無人機(jī)的位置信息。

表1 無人機(jī)相關(guān)參數(shù)表Tab.1 Related parameters of UAV

圖1 展示了各無人機(jī)的飛行軌跡?？梢钥闯?，各無人機(jī)在遭遇障礙物時，可以及時進(jìn)行避障，保證自身安全。1號無人機(jī)在避障后靠近4號無人機(jī)，4號無人機(jī)同時遠(yuǎn)離1號無人機(jī)保持飛行距離以避免相互碰撞。4架無人機(jī)在避障前后均可以維持設(shè)定的隊形進(jìn)行飛行，并向各自目標(biāo)點趨近。

圖1 各無人機(jī)的飛行軌跡Fig.1 Flight path of UAVs

各無人機(jī)在x、y、z方向上的線速度見圖2；圖3為各無人機(jī)的控制信號U1i，由圖可知各無人機(jī)可以在遭遇障礙物時及時作出機(jī)動反應(yīng)；圖4 為各無人機(jī)的滾轉(zhuǎn)角。

圖2 各無人機(jī)的線速度分量Fig.2 Linear velocity components of UAVs

圖3 各無人機(jī)的控制信號U1iFig.3 Control signal U1i of UAVs

圖4 各無人機(jī)的滾轉(zhuǎn)角Fig.4 Roll angle of UAVs

從以上仿真結(jié)果和分析中可知，本文提出的方案可以使四旋翼無人機(jī)在編隊飛行時及時進(jìn)行避障以免相互避撞，保證了各無人機(jī)在遭遇障礙物時的飛行安全。

4 結(jié)論

本文針對四旋翼無人機(jī)在編隊飛行執(zhí)行任務(wù)時可能遭遇障礙物問題，考慮多無人機(jī)避障及機(jī)間避撞的需求，提出了1 種基于零空間方法的四旋翼無人機(jī)避障與協(xié)同編隊控制算法。首先，建立了四旋翼無人機(jī)動力學(xué)模型，并建立虛擬控制量簡化控制模型；然后，基于零空間方法進(jìn)行了避障與協(xié)同編隊控制算法研究，將無人機(jī)任務(wù)執(zhí)行分解為目標(biāo)趨向任務(wù)、避障避撞任務(wù)和協(xié)同編隊任務(wù)3 種任務(wù)，根據(jù)優(yōu)先級進(jìn)行任務(wù)融合得到期望速度，并基于PID 方法設(shè)計了控制律；最后，通過仿真驗證了所提控制算法的有效性。所提方法可保證四旋翼無人機(jī)在編隊飛行中遭遇障礙物時的飛行安全。