領(lǐng)航主機約束化的多四旋翼無人機編隊航跡優(yōu)化方法*

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

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

0 引言

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

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

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

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

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

圖1 四旋翼無人機結(jié)構(gòu)示意圖

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

(1)

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

1.2 領(lǐng)航四旋翼無人機約束條件分析

1)初始約束

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

(2)

2)終端約束

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

(3)

3)最小轉(zhuǎn)彎半徑

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

(4)

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

4)控制約束

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

(5)

5)最低飛行高度

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

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

(6)

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

6)水平方向約束

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

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

(7)

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

1.3 跟隨四旋翼無人機約束轉(zhuǎn)換

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

1)從機位置約束轉(zhuǎn)換：按照從機和領(lǐng)航主機在編隊中的位置信息，可得到從機在慣性坐標(biāo)系下的位置表示：

(8)

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

2)從機最低飛行高度約束轉(zhuǎn)換：

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

z1(t)≥hmin(t)+zi1

(9)

3) 從機水平方向約束轉(zhuǎn)換：

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

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

(10)

4)最小轉(zhuǎn)彎半徑

和領(lǐng)航無人機一樣，從機的轉(zhuǎn)彎半徑大小也需滿足如下要求：

(11)

(12)

于是可得從機與主機之間的速度關(guān)系：

(13)

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

(14)

如果假設(shè)四旋翼無人機編隊在轉(zhuǎn)彎時，轉(zhuǎn)彎中心的相對領(lǐng)航主機的位置坐標(biāo)為(0, 0, 0)，則有：

(15)

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

(16)

這樣，從機的最小轉(zhuǎn)彎半徑可轉(zhuǎn)換為領(lǐng)航主機的最小轉(zhuǎn)彎半徑、速度和位置相關(guān)約束。

1.4 性能指標(biāo)函數(shù)

本文采用編隊能耗最小作為性能指標(biāo)，即

(17)

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

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

1)時域變換

采用Gauss偽譜法時，需要將時間區(qū)間[t0,tf]轉(zhuǎn)換為[-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)動力學(xué)微分方程約束

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

(24)

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

(25)

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

(26)

其中：k= 1,…,K。

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

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

(27)

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

(28)

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

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

(29)

6)Gauss偽譜法的一般描述

根據(jù)上述數(shù)學(xué)變換，Gauss偽譜法求解最優(yōu)控制問題的原理可以描述為：求離散節(jié)點上的狀態(tài)變量Xi，i=0,…,K和控制變量Uk，k= 1,…,K，以及初末時刻t0和tf，使得性能指標(biāo)式(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)控制問題轉(zhuǎn)化成非線性規(guī)劃問題(NLP)，即:

(32)

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

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

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

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

3 仿真校驗

實驗針對6架四旋翼無人機組成的多機編隊，起飛時以等邊三角形編隊，編隊保持策略是領(lǐng)航跟隨法。規(guī)定領(lǐng)航四旋翼無人機要完成的任務(wù)是：在指定出發(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時從較高的高度到達(dá)目的地點(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)角與領(lǐng)航無人機相同。地形約束模型采用由式(33)生成的地面高度和式(34)生成的山峰地形數(shù)據(jù)[12-13]。

(33)

(34)

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

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

(35)

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

圖2 生成的領(lǐng)航者航跡

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

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

圖3 領(lǐng)航主機航向角

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

圖4 領(lǐng)航主機俯仰角

圖5 領(lǐng)航主機橫滾角

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

圖6 領(lǐng)航主機控制量U1

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

圖7 領(lǐng)航主機控制量U2、U3和U4

4 結(jié)論

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