基于零空間方法的四旋翼無人機(jī)避障與協(xié)同編隊(duì)控制

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

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

0 引言

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

2.1 避障避撞任務(wù)

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

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

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

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

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

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

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

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

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

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

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

目標(biāo)趨向任務(wù)控制無人機(jī)向目標(biāo)點(diǎn)飛行。定義目標(biāo)點(diǎn)位置為pd,i=(xd,i,yd,i,zd,i)T，可控的任務(wù)變量為δd,i=pi。引入四旋翼無人機(jī)最大飛行速度、實(shí)時(shí)位置坐標(biāo)和飛行目標(biāo)位置坐標(biāo)，對(duì)增益系數(shù)進(jìn)行重新定義，保證規(guī)劃得到的最終速度始終在無人機(jī)飛行能力范圍內(nèi)。對(duì)可控變量δ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é)同編隊(duì)任務(wù)和目標(biāo)趨向任務(wù)的輸出后，考慮避障避撞任務(wù)優(yōu)先級(jí)最高，其次為協(xié)同編隊(duì)任務(wù)優(yōu)于目標(biāo)趨向任務(wù)，采取NSB的方法將其綜合，可以得到最終的速度輸出為：

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

采用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è)計(jì)的參數(shù)對(duì)角矩陣，且其元素均為正常數(shù)。最后由式（3）～（6），即可計(jì)算得到控制輸入U(xiǎn)1i、U2i、U3i、U4i。

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

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

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

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

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

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

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

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

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

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

4 結(jié)論

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