基于零空間方法的四旋翼無人機避障與協同編隊控制

楊鐘煜，余自權，程月華，徐貴力

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

0 引言

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

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

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

1 問題描述與預備知識

1.1 四旋翼無人機動力學模型

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

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

四旋翼無人機是1個含有4個系統輸入、6個系統輸出的典型欠驅動系統。因此，根據四旋翼無人機動力學模型和坐標系轉換矩陣，定義虛擬控制分量，使四旋翼無人機系統成為1 個6 個控制輸入和6 個控制輸出的全驅動系統。定義虛擬控制量：

進一步推導可得：

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

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

式（9）兩邊對pi求導得：

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

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

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

式（13）中，下標表示任務的優先級。

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

四旋翼無人機編隊飛行時，可將任務執行分解為3種任務：避障避撞任務、協同編隊任務和目標趨向任務。

2.1 避障避撞任務

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

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

則避障避撞任務的輸出為：

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

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

2.2 協同編隊任務

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

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

則協同編隊任務的輸出為：

式（19）中，λf,i為協同編隊增益系數。

2.3 目標趨向任務

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

則目標趨向任務的輸出為：

式（21）～（23）中：λd,i為目標趨向增益系數；vmax為無人機最大飛行速度；變量rd為趨向目標任務的調節因子。

2.4 基于零空間方法的任務融合

得到避障避撞任務、協同編隊任務和目標趨向任務的輸出后，考慮避障避撞任務優先級最高，其次為協同編隊任務優于目標趨向任務，采取NSB的方法將其綜合，可以得到最終的速度輸出為：

2.5 無人機控制算法設計

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

式（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為待設計的參數對角矩陣，且其元素均為正常數。最后由式（3）～（6），即可計算得到控制輸入U1i、U2i、U3i、U4i。

3 仿真結果及分析

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

表1 無人機相關參數表Tab.1 Related parameters of UAV

圖1 展示了各無人機的飛行軌跡。可以看出，各無人機在遭遇障礙物時，可以及時進行避障，保證自身安全。1號無人機在避障后靠近4號無人機，4號無人機同時遠離1號無人機保持飛行距離以避免相互碰撞。4架無人機在避障前后均可以維持設定的隊形進行飛行，并向各自目標點趨近。

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

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

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

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

圖4 各無人機的滾轉角Fig.4 Roll angle of UAVs

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

4 結論

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