張利民 李 茜 柴旭朝 路向陽 劉若陽

1. 中原工學院電子信息學院，鄭州 451191 2. 天津工業(yè)大學機械工程學院，天津 300387

0 引言

四旋翼無人機由于其機械結構簡單、操控靈活方便等優(yōu)點，近年受到了很多學者的關注[1-3]，四旋翼無人機多機協(xié)同航路規(guī)劃是多架無人機協(xié)同作戰(zhàn)任務規(guī)劃系統(tǒng)中的關鍵技術，近年來有很多相關研究成果，比如，莫宏偉等[4]在蟻群算法改進的基礎上，提出了變主方向搜索策略和簡化的四旋翼無人機航跡策略；Chen等[5]針對多四旋翼無人機協(xié)同裝配任務中的軌跡優(yōu)化問題，提出了一種基于離線路徑規(guī)劃過程和在線軌跡規(guī)劃過程的分層優(yōu)化策略；Zhang等[6]提出了一種基于優(yōu)先級的RRT算法，該算法為每個具有避障功能的四旋翼機生成一條在線全局路徑，在障礙物密集的環(huán)境中實現(xiàn)多個四旋翼自主導航的分散運動規(guī)劃；Iswanto等[7]結合模糊邏輯算法和單元分解算法，使四旋翼能夠通過尋找最短路徑快速移動到目標位置；Turpin等[8]控制一組微型四旋翼飛行器在三維環(huán)境中快速移動，同時保持緊密編隊。

但目前的文獻中少有對三維空間有約束情況下，利用四旋翼無人機編隊隊形信息的航跡優(yōu)化研究。本文針對此問題，首先，建立四旋翼無人機動力學模型，分析其飛行過程中的約束條件，分析領航主機的約束問題，然后將跟隨四旋翼無人機的約束轉換為領航主機的約束，將多機的航跡優(yōu)化問題轉換為領航主機的單機三維空間航跡優(yōu)化，并利用Gauss偽譜法(Gauss pseudo spectrum method，GPM)對領航主機進行單機航跡優(yōu)化。

1 多四旋翼無人機編隊約束分析

1.1 單四旋翼無人機的非線性模型

本文采用+字型四旋翼飛無人機，其結構簡圖如圖1所示[4]。

圖1 四旋翼無人機結構示意圖

四旋翼無人機的非線性模型[5-6]為：

(1)

本文采用一致性理論定義各四旋翼無人機之間的隊形保持問題，將編隊系統(tǒng)轉化為網(wǎng)絡拓撲結構，通過一致性算法使得無人機的狀態(tài)信息與期望信息保持一致，從而實現(xiàn)多無人機自主編隊運動控制，本文對一致性控制不做研究。

1.2 領航四旋翼無人機約束條件分析

1)初始約束

四旋翼無人機在起始點t=0時需滿足的初始位置和姿態(tài)約束，即：

(2)

2)終端約束

四旋翼無人機在到達目標點時需滿足的終端位置和終端姿態(tài)約束為：

(3)

3)最小轉彎半徑

四旋翼無人機的自身特性決定了在需要拐彎時拐彎半徑的最小值，另外，較大的轉彎半徑還夠避免四旋翼無人機編隊在改變隊形時相互碰撞。一般情況下此半徑大小需滿足如下要求：

(4)

式中：V1為領航四旋翼無人機速度，R1為領航無人機轉彎半徑，g為重力加速度，n1max為領航無人機最大法向過載。

4)控制約束

Uimin≤Ui≤Uimax,i=1,2,3,4

(5)

5)最低飛行高度

四旋翼無人機一般在飛行過程中會遇到較高的地形或者敵方火力范圍，因此在進行路徑規(guī)劃時要設定四旋翼無人機實時飛行的最低高度，確保所有的無人機在安全高度以上飛行。

z1(t)≥hmin(t),hmin(t)>0

(6)

其中：z1(t)為編隊中領航無人機的實時飛行高度，hmin(t)為根據(jù)地形等數(shù)據(jù)得到的實時最低飛行高度要求。

6)水平方向約束

如果四旋翼無人機在一定的高度飛行時，可以在水平方向上要求其距離障礙物的外圍輪廓保持一段安全距離，防止無人機因為慣性或者機動性不足而與障礙物發(fā)生碰撞，另外還可以利用障礙物的阻擋減小被敵方偵查到的概率。本文均以圓盤表示在一定飛行高度上對應的水平障礙物約束：

(x1(t)-xj)2+(y1(t)-yj)2>Rj,j=1,2,…，m

(7)

式中：(x1(t)，y1(t))表示領航無人機的實時位置，(xj,yj)和Rj分別表示第j個禁飛區(qū)圓盤的圓心和半徑，m表示禁飛區(qū)總數(shù)。

1.3 跟隨四旋翼無人機約束轉換

在本文研究的多無人機編隊航跡優(yōu)化中，我們將跟隨四旋翼無人機(從機)特性作為領航主機存在的約束條件，其轉換為約束的處理如下：

1)從機位置約束轉換：按照從機和領航主機在編隊中的位置信息，可得到從機在慣性坐標系下的位置表示：

(8)

其中：(xi(t),yi(t),zi(t))為第i個從機的實時位置， (x1(t),y1(t),z1(t))為領航主機的實時位置，(xi1,yi1,zi1)為第i個從機和領航主機之間在隊形中的相對位置，為已知值，從其表達式可以看出，對從機的位置約束處理時，可以將其變換為對領航主機的位置約束，于是，進一步，對于從機的最低飛行高度和水平方向的約束，我們可以做下面的分析。

2)從機最低飛行高度約束轉換：

當從機飛行時要求的最低飛行高度約束為zi(t)=z1(t)-zi1≥himin(t)時，其中himin(t)>0，我們可以將其轉換為領航主機的飛行高度約束，即

z1(t)≥hmin(t)+zi1

(9)

3) 從機水平方向約束轉換：

和高度約束轉換方法一樣，如果我們要求從機在水平方向上距離障礙物某個點的距離要求(xi(t)-xj)2+(yi(t)-yj)2>Rj,j=1,2,…,m，其中Rj表示從機i距離第j個障礙物表面的最近距離，則我們可以將其轉換為領航主機的水平方向約束，即

(x1(t)-xi1-xj)2+(y1(t)-yi1-yj)2>Rj

(10)

4)最小轉彎半徑

和領航無人機一樣，從機的轉彎半徑大小也需滿足如下要求：

(11)

(12)

于是可得從機與主機之間的速度關系：

(13)

式(11)左邊可變?yōu)椋?/p>

(14)

如果假設四旋翼無人機編隊在轉彎時，轉彎中心的相對領航主機的位置坐標為(0, 0, 0)，則有：

(15)

即式(11)可變?yōu)椋?/p>

(16)

這樣，從機的最小轉彎半徑可轉換為領航主機的最小轉彎半徑、速度和位置相關約束。

1.4 性能指標函數(shù)

本文采用編隊能耗最小作為性能指標，即

(17)

2 基于GPM的四旋翼編隊航跡優(yōu)化

2.1 Gauss偽譜法原理[9-11]

1)時域變換

采用Gauss偽譜法時，需要將時間區(qū)間[t0,tf]轉換為[-1,1]，即

(18)

2)狀態(tài)變量與控制變量的近似

(19)

其中：

(20)

(21)

令式(19)中節(jié)點處的近似狀態(tài)值和實際狀態(tài)值相等，得Xi=X(τi)=x(τi)。

對于控制變量采用同樣的方法離散，有

u(τk)=U(τk),k=1,…,K

(22)

即:

(23)

3)動力學微分方程約束

對式(19)求節(jié)點上微分：

(24)

其中：微分矩陣可以離線確定，即:

(25)

從而動力學微分方程約束轉換為代數(shù)約束:

(26)

其中：k= 1,…,K。

4)終端狀態(tài)約束的處理

狀態(tài)變量在終端時刻節(jié)點需要單獨處理，終端狀態(tài)應滿足如下動力學方程約束:

(27)

將式(27) Gauss積分公式近似得:

(28)

5)性能指標函數(shù)的近似

將性能指標函數(shù)中的積分項采用Gauss積分公式近似得：

(29)

6)Gauss偽譜法的一般描述

根據(jù)上述數(shù)學變換，Gauss偽譜法求解最優(yōu)控制問題的原理可以描述為：求離散節(jié)點上的狀態(tài)變量Xi，i=0,…,K和控制變量Uk，k= 1,…,K，以及初末時刻t0和tf，使得性能指標式(29)最小，并滿足終端狀態(tài)約束式(26)，以及原最優(yōu)控制問題的邊界條件式(27)，以及原最優(yōu)控制問題的邊界條件

φ(X0,t0,Xf,tf)=0

(30)

C(Xk,Uk,τk;t0,tf)≤0,k=1,…，K

(31)

從而將原最優(yōu)控制問題轉化成非線性規(guī)劃問題(NLP)，即:

(32)

其中:y為設計變量，包含控制變量、狀態(tài)變量和兩個端點時間。目前已有很多種有效方法求解非線性規(guī)劃問題，其中序列二次規(guī)劃算法(SQP)以其局部超一次收斂性、整體收斂性得到廣泛應用，Gauss偽譜法采用SQP算法處理轉換而來的NLP問題。

2.2 四旋翼無人機編隊最優(yōu)控制問題轉換分析

從四旋翼無人機的模型可以看出，四旋翼無人機的航跡優(yōu)化屬于強非線性的最優(yōu)控制問題，其求解比較復雜，本文借鑒Gauss偽譜法GPM思想，將最優(yōu)控制問題轉化為一系列具有代數(shù)約束的參數(shù)優(yōu)化問題，即非線性規(guī)劃問題。

經(jīng)過本文的分析，四旋翼無人機編隊最優(yōu)控制問題的最優(yōu)航跡規(guī)劃問題可描述為：在滿足動力學微分方程約束式(1)、領航主機的邊界條件式(2)和(3)、最小轉彎半徑約束式(4)、控制約束式(5)、最低飛行高度約束式(6)、水平方向約束下式(7)和跟隨四旋翼無人機轉換為領航主機的約束式(8)((10)和(12)，尋求最優(yōu)控制量u(t)，使性能指標式(17)最小。

3 仿真校驗

實驗針對6架四旋翼無人機組成的多機編隊，起飛時以等邊三角形編隊，編隊保持策略是領航跟隨法。規(guī)定領航四旋翼無人機要完成的任務是：在指定出發(fā)點(x0,y0,z0)=(2,1,3)m，以初始速度矢量(vx0,vy0,vz0)=(2,2,0)m/s，給定的初始姿態(tài)角(ψ0,θ0,φ0)=(45°,0°,0°)初始出發(fā)，穿過分布了多個山峰的非平地區(qū)域，以指定的速度矢量(vxf,vyf,vzf)=(0,0,0)m/s、(ψf,θf,φf)=(30°,0°,0°)姿態(tài)角，在規(guī)定的時間t=330s時從較高的高度到達目的地點(1200,1000,2)m。5架跟隨無人機的起飛位置分別為(0,0,3)m，(0,3,3)m，(1,0.5,3)m，(0,0.5,3)m，(1,1.5,3)m，起飛速度矢量、姿態(tài)角、末端速度矢量和姿態(tài)角與領航無人機相同。地形約束模型采用由式(33)生成的地面高度和式(34)生成的山峰地形數(shù)據(jù)[12-13]。

(33)

(34)

實驗中使用的四旋翼無人機其他相關參數(shù)如表1和式(35)所示。

表1 仿真參數(shù)設置

(35)

在采用領航跟隨策略的無人機編隊中，首先要為領航者指定參考航跡，使用本文方法生成了領航者無人機的參考航跡，用GPM生成的航跡以節(jié)點形式給出，通過自適應選點的B樣條平滑策略平滑處理，并加以最大值約束。其結果如圖2所示。

圖2 生成的領航者航跡

圖2展示了領航者無人機在分布了多個障礙物環(huán)境下的航跡，可以看出該航跡與所有障礙物都未發(fā)生碰撞，并能從起點到達指定終點。圖中實心點代表無人機編隊示意圖。從圖2可以看出規(guī)劃的飛行路徑平滑，均滿足約束條件。在飛行末端無人機編隊從較高位置逐漸降落在指定高度2m。在飛行過程中，飛行路徑先后經(jīng)過上升-下降-上升-下降4個過程，這個過程為消耗多余時間而進行的飛行。

領航主機的航向角、俯仰角、橫滾角的變化曲線分別如圖3～5所示。

圖3 領航主機航向角

從圖3可以看出，領航主機經(jīng)過4次航向變化，這部分對應的正是穿越山峰區(qū)域的時間，通過航向的變化可以在山峰直接穿越。中間有一段保持航向不變的飛行時間，此段時間為山谷間的平地飛行(或地面高度值不大)。從圖4可以看出，領航主機經(jīng)過2次爬升和2次降落過程。爬升過程均為翻越較低山峰準備，第1次降落過程為越過山峰后的過程，第2次降落為到達目的地后，根據(jù)要求從較高的高度逐漸降低到目標高度的過程。從圖5可以看出，領航主機在飛行過程中通過小角度翻滾帶領從機規(guī)避障礙物或地形，最終到達目的地時回到無翻滾狀態(tài)。

圖4 領航主機俯仰角

圖5 領航主機橫滾角

圖6～7分別為控制量U1、U2、U3和U4的變化曲線圖。

圖6 領航主機控制量U1

從圖6可以看出，通過U1改變3個空間方向的速度，實現(xiàn)無人機軌跡的變化，達到領航主機規(guī)避障礙物和消耗多余時間的要求。從圖7可以看出，三個姿態(tài)角在飛行過程中微小的變化，是為了改變領航主機的飛行方向和姿態(tài)穩(wěn)定性或者規(guī)避障礙物。

圖7 領航主機控制量U2、U3和U4

4 結論

本文對四旋翼無人機編隊問題進行了研究，首先采用了領航-跟隨法的策略，并基于一致性理論將編隊系統(tǒng)轉化為網(wǎng)絡拓撲結構，將編隊看成一個虛擬剛體。在此基礎上，首先分析了領航四旋翼無人機自身的一些約束條件，然后將編隊中的跟隨無人機信息進行分析，轉換為領航主機的約束，將編隊問題單機化，最后，給出了四旋翼無人機編隊航跡優(yōu)化問題的分析，提出基于Gauss偽譜法將航跡優(yōu)化的最優(yōu)控制問題全部轉換到非線性規(guī)劃的求解框架之中。最后的仿真結果表明了方法的有效性。