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

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

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

0 引言

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

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

1 多四旋翼無人機(jī)編隊(duì)約束分析

1.1 單四旋翼無人機(jī)的非線性模型

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

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

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

(1)

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

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

1)初始約束

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

(2)

2)終端約束

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

(3)

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

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

(4)

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

4)控制約束

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

(5)

5)最低飛行高度

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

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

(6)

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

6)水平方向約束

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

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

(7)

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

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

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

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

(8)

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

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

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

z1(t)≥hmin(t)+zi1

(9)

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

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

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

(10)

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

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

(11)

(12)

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

(13)

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

(14)

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

(15)

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

(16)

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

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

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

(17)

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

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

1)時(shí)域變換

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

(18)

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

(19)

其中：

(20)

(21)

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

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

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

(22)

即:

(23)

3)動力學(xué)微分方程約束

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

(24)

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

(25)

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

(26)

其中：k= 1,…,K。

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

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

(27)

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

(28)

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

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

(29)

6)Gauss偽譜法的一般描述

根據(jù)上述數(shù)學(xué)變換，Gauss偽譜法求解最優(yōu)控制問題的原理可以描述為：求離散節(jié)點(diǎn)上的狀態(tài)變量Xi，i=0,…,K和控制變量Uk，k= 1,…,K，以及初末時(shí)刻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è)計(jì)變量，包含控制變量、狀態(tài)變量和兩個端點(diǎn)時(shí)間。目前已有很多種有效方法求解非線性規(guī)劃問題，其中序列二次規(guī)劃算法(SQP)以其局部超一次收斂性、整體收斂性得到廣泛應(yīng)用，Gauss偽譜法采用SQP算法處理轉(zhuǎn)換而來的NLP問題。

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

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

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

3 仿真校驗(yàn)

實(shí)驗(yàn)針對6架四旋翼無人機(jī)組成的多機(jī)編隊(duì)，起飛時(shí)以等邊三角形編隊(duì)，編隊(duì)保持策略是領(lǐng)航跟隨法。規(guī)定領(lǐng)航四旋翼無人機(jī)要完成的任務(wù)是：在指定出發(fā)點(diǎn)(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ī)定的時(shí)間t=330s時(shí)從較高的高度到達(dá)目的地點(diǎn)(1200,1000,2)m。5架跟隨無人機(jī)的起飛位置分別為(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)航無人機(jī)相同。地形約束模型采用由式(33)生成的地面高度和式(34)生成的山峰地形數(shù)據(jù)[12-13]。

(33)

(34)

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

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

(35)

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

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

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

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

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

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

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

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

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

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

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

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

4 結(jié)論

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