基于DMPC的多無人機軌跡跟蹤與避碰切換控制

趙超輪, 戴邵武*, 何云風, 劉 帥, 趙國榮

(1. 海軍航空大學, 山東 煙臺 264000; 2. 海軍航空大學青島校區(qū), 山東 青島 266000)

0 引 言

當前, 多無人機系統(tǒng)在眾多領域展現(xiàn)出巨大的應用潛力, 如搜索救援[1]、 海洋勘測[2]、 物資配送[3]、 移動邊緣計算[4]、 軍事作戰(zhàn)[5]等。 關于多無人機系統(tǒng)的研究越來越受到人們的重視。 多無人機系統(tǒng)的一個核心問題是編隊跟蹤控制, 它要求無人機在跟蹤參考軌跡的同時保持指定的隊形。 根據(jù)無人機間信息交互情況的不同, 編隊跟蹤控制算法可分為集中式、 分布式、 分散式; 根據(jù)編隊控制策略的不同, 其可分為領航跟隨法[6]、 虛擬結構法[7]、 基于行為法[8]、 虛擬領航法[9]等; 根據(jù)具體的生成控制量的方法不同, 其可分為反步法[10]、 滑?？刂芠11]、 PID控制[12]、 一致性算法[13]、 人工勢場法[14]、 模型預測控制(Model Predictive Control, MPC)[15]等。

然而, 對于多無人機系統(tǒng), 大多數(shù)控制方法在各種約束條件下, 如輸入飽和、 速度限制、 安全約束等, 不能達到最優(yōu)的控制性能。 相比之下, MPC[16]被認為是少數(shù)幾種能夠顯式處理約束并優(yōu)化控制性能的方法之一。 而分布式模型預測控制(Distributed Model Predictive Control, DMPC)[17-18]是MPC與分布式結構的有機結合, 它兼顧了MPC滾動優(yōu)化、 顯式處理約束的特點與分布式結構可擴展性強、 計算量小的優(yōu)勢。 因此, DMPC被廣泛應用于各種場景, 如船舶編隊[19]、 車輛隊列[20]、 無人機編隊[15]等。

在DMPC研究成果中, 對于編隊跟蹤或軌跡跟蹤問題, 一般是通過在目標函數(shù)中考慮軌跡跟蹤代價來解決的[21-28]。 為了保證編隊的安全, 編隊控制器設計時必須考慮避碰問題。 避碰主要包括編隊系統(tǒng)內部單機與單機間的避碰、 單機與外部障礙間的避碰, 這兩者的差異在于獲取避碰對象狀態(tài)的方式不同, 前者可利用通信的方式進行信息的雙向交互, 或利用探測的方式彼此感知, 而后者只能利用探測的方式單向感知。 對于避碰問題, 主要有兩種解決思路: 一是在目標函數(shù)中增加避碰代價[21-24]; 二是在約束條件中增加避碰約束[25-28]。 其中, 僅增加避碰代價的優(yōu)勢是計算成本低, 能解決大多數(shù)的避碰問題, 但由于其本質是一種軟約束, 故無法保證嚴格避碰; 僅增加避碰約束的優(yōu)勢是能保證嚴格避碰, 不足是計算成本較高。 本文采用第2種思路。

在DMPC方法中, 對于編隊內的單機而言, 其在每個采樣時刻控制量的求解一般需要用到本機和鄰機的狀態(tài)信息。 DMPC的控制量求解方式可分為順序式、 同步式, 信息交互方式可分為迭代式、 非迭代式。 考慮到無人機飛行速度快, 對計算時間要求高, 這里重點研究同步式、 非迭代的DMPC方法, 即在每個采樣周期內, 無人機間只交互一次信息, 各無人機并行求解一次最優(yōu)控制問題。

在同步式、 非迭代式DMPC的研究中, Wang等[26]在考慮智能體間避碰的情況下, 設計出一種能保證系統(tǒng)漸近穩(wěn)定的控制算法, 實現(xiàn)了編隊的軌跡跟蹤。 Yang等[27]則在文獻[26]的基礎上研究了智能體與靜止障礙間的避碰問題。 Dai等[28]則在考慮智能體間避碰、 智能體與靜止障礙避碰的情況下, 實現(xiàn)了多智能體定點編隊控制。 上述文獻[26-28]通過引入終端集約束、 相容性約束等, 實現(xiàn)了在滿足避碰條件下的系統(tǒng)漸近穩(wěn)定, 因為與系統(tǒng)穩(wěn)定性有關的參數(shù)是離線整定的, 所以上述文獻要求障礙是預先已知的, 且參考軌跡上不能有障礙。

然而, 在復雜空域中飛行的多無人機系統(tǒng)必然要面對預先未知的障礙, 且無法確保參考軌跡上沒有障礙。 為此, 在假設無人機具備實時探測能力的基礎上, 提出一種帶有模式切換的DMPC算法。 具體地, 算法中以多四旋翼無人機為研究對象, 以虛擬領航法為控制策略, 通過實時探測無人機與障礙間的距離, 判斷有無碰撞風險, 進而設計出兩種控制模式: 跟蹤模式和避碰模式, 無人機可根據(jù)風險情況進行模式的切換。 不同模式對應不同的DMPC控制器, 其中, 跟蹤模式, 即無人機在無碰撞風險時所采用的模式, 是以實現(xiàn)軌跡跟蹤為目標, 相應控制算法能保證編隊跟蹤控制系統(tǒng)漸近穩(wěn)定; 避碰模式, 是以保證無人機安全為首要目標, 相應控制算法中去掉了終端代價、 終端集約束, 增加了避碰約束、 位置相容性約束。 在避碰時, 通過暫時舍棄無人機跟蹤誤差的漸近收斂性來換取無人機的安全性。 仿真結果表明, 該算法能夠實現(xiàn)多無人機系統(tǒng)的軌跡跟蹤, 并且具備無人機之間、 無人機與預先未知障礙之間的避碰能力。

相比于文獻[26-28], 本文DMPC方法有以下優(yōu)勢:

(1) 使無人機不僅能夠有效規(guī)避系統(tǒng)內部無人機、 預先已知的障礙, 也能夠有效規(guī)避預先未知的靜態(tài)/動態(tài)障礙, 其中包括大型障礙;

(2) 采用切換控制的思路, 根據(jù)有無碰撞風險選擇合適的控制模式。 相比于單一控制模式, 降低了問題求解的保守性, 減少了計算時間。

1 系統(tǒng)運動模型

本文重點研究多四旋翼系統(tǒng)的運動協(xié)調問題, 且側重其三維平移運動過程, 因此, 在系統(tǒng)建模時對四旋翼模型合理簡化, 重點描述其平移運動的運動學特征。

基于地球固連坐標系oexeyeze研究多無人機系統(tǒng)相對地面的運動狀態(tài)。 在地球固連坐標系中, 將地球表面假設為平面, 原點oe為地面上指定的一點,oexe軸指向地理東,oeye軸指向地理北,oeze軸垂直地面向上。

1.1 單機運動模型

假設編隊中共有Nv(Nv={1, 2, …,Nv})架四旋翼無人機, 無人機間動力學解耦, 且忽略風的阻力。 假設無人機配備了自動駕駛儀, 該自動駕駛儀可實現(xiàn)無人機的速度控制, 即把速度指令輸入給自動駕駛儀后, 它能夠自動控制無人機跟蹤給定的速度指令。 在此基礎上, 第i架無人機的質心運動模型可以近似描述為如下連續(xù)時間線性時不變形式[29]:

(1)

(2)

采用零階保持法[30]對式(2)進行離散化, 得到

xi(k+1)=Gxi(k)+Hui(k)

(3)

狀態(tài)和控制輸入滿足如下約束:

(4)

(5)

在虛擬領航策略中, 虛擬領航機UAVr的作用是為所有真實無人機預先提供參考狀態(tài)序列和參考輸入, 即真實無人機可根據(jù)UAVr的狀態(tài)序列以及給定的期望相對狀態(tài), 得到自身期望狀態(tài)序列。 UAVr具有與真實無人機相同的數(shù)學模型, 即

xr(k+1)=Gxr(k)+Hur(k)

(6)

式中:xr=[(pr)T, (vr)T]T;xr,ur需分別滿足約束式(4)～(5)。

1.2 編隊運動模型

(7)

2 問題描述

假設1: 有真實無人機預知虛擬領航機的狀態(tài)信息、 參考輸入, 以及期望的相對狀態(tài)信息。

假設2: 無人機具備一定的探測能力和通信能力。 在一個采樣周期內, 真實無人機能夠實時獲取進入其探測范圍內的障礙物的當前位置、 速度信息, 能夠實時獲取進入其通信范圍的無人機所發(fā)送的信息。

假設3: 不考慮外界干擾、 噪聲、 空氣阻力等對無人機動態(tài)的影響。

控制目標: 無人機編隊系統(tǒng)在滿足自身狀態(tài)、 輸入約束的情況下, 能夠按照期望的相對位置和相對速度跟蹤虛擬領航機, 且具備與系統(tǒng)內部無人機、 系統(tǒng)外部預先未知靜態(tài)/動態(tài)障礙的避碰能力。

考慮到無人機計算資源有限, 且并不是空域內所有障礙物都會給無人機帶來威脅, 只有當障礙物與無人機的距離較近時, 才有發(fā)生碰撞的風險, 因此, 基于無人機能夠實時探測障礙物的能力, 根據(jù)有無碰撞風險, 設計出跟蹤、 避碰兩種控制模式及模式切換策略。

3 控制算法設計

圖1是以雙機為例的帶有模式切換的DMPC控制結構圖。 控制算法的實現(xiàn)是同步的、 非迭代的。

圖1 帶有模式切換的DMPC控制結構Fig.1 DMPC control structure with mode switching

3.1 模式切換策略

圖2 無人機的幾個半徑Fig.2 Several radii of the UAV

設k時刻UAVi所能探測到的障礙物集合為Nim, k={m|(Dim(k)-Rm)≤Rd,m∈No∪(Nv{i})},Nv{i}表示除UAVi以外的其他所有無人機集合,No={o1,o2, …}表示所有外部障礙的集合,o特指“外部障礙”。 設k時刻進入UAVi避碰觸發(fā)半徑Ra內的障礙物集合為Nia, k={m|(Dim(k)-Rm)≤Ra,m∈Nim, k}。

假設初始時刻Nia, 0=?, 則UAVi處于跟蹤模式; 隨著時間推移, 若在k時刻?m∈Nia, k, 則UAVi切換至避碰模式,Ra是UAVi由跟蹤模式切換至避碰模式的閾值。 為避免模式的頻繁切換, 設置UAVi由避碰模式切換至跟蹤模式的閾值為Ra+Rε, 即當無人機進入避碰模式后, 只有在滿足{m|(Dim(k)-Rm)≥Ra+Rε,m∈Nim, k}=?時, 才能重新切換到跟蹤模式。 模式切換策略如圖3所示, 其中, 橫坐標為時間、 縱坐標為障礙物m中心與UAVi中心間的距離。

圖3 模式切換策略示意圖Fig.3 Schematic diagram of mode switching strategy

3.2 跟蹤模式

DMPC本質上是一種優(yōu)化算法, 因此相應的問題被描述為優(yōu)化問題形式。 UAVi在沒有碰撞風險時, 采用跟蹤模式, 其優(yōu)化問題可描述為問題1。

問題1在采樣時刻k, UAVi從xi(k|k)出發(fā)求解預測時域長度為N的優(yōu)化控制問題, 可表示為

(8)

s.t.xi(k+l+1|k)=Gxi(k+l|k)+Hui(k+l|k),

l=0, …,N-1;xi(k|k)=xi(k);

xi(k+l|k)∈Xi,l=1, …,N;

ui(k+l|k)∈Ui,l=0, …,N-1;

xi(k+N|k)∈Ωi

(9)

求解問題1, 即要求解出滿足式(9)的控制輸入序列Ui(k)={ui(k|k),ui(k+1|k), …,ui(k+N-1|k)}, 使得目標函數(shù)式(8)最小。

目標函數(shù)式(8)中各項的具體形式如下:

(1)Ji, x為UAVi跟蹤參考狀態(tài)軌跡的代價, 形式為

(10)

(2)Ji, u為UAVi跟蹤參考輸入的代價, 形式為

(11)

式中:uir(k+l|k)=ui(k+l|k)-ur(k+l), 權重Si為對稱正定矩陣。 式(11)直觀上是使無人機的控制指令盡可能接近虛擬領航機的控制指令, 其作用是抑制控制量的劇烈變化。

(3)Jif為終端代價, 形式為

(12)

該項僅存在于跟蹤模式對應的問題1中, 是為保證跟蹤控制系統(tǒng)漸近穩(wěn)定而引入的, 用來近似k+N時刻以后優(yōu)化問題的性能指標。 其中, 權重Pi為對稱正定矩陣。

約束條件式(9)中第1項為無人機預測模型, 第2項為將k時刻當前狀態(tài)作為該時刻優(yōu)化問題的初始狀態(tài), 第3、 4項分別為無人機自身狀態(tài)、 輸入約束, 具體形式見式(4)～(5)。 第5項為終端集約束, 設計終端集為

(13)

注1:終端集約束是為保證系統(tǒng)漸近穩(wěn)定而引入的, 由其形式易知, 該約束要求UAVi終端狀態(tài)距離參考軌跡不能太遠, 而UAVi在避碰過程中經常會遇到需要遠離參考軌跡的情形, 終端集約束的引入將不可避免地增加優(yōu)化問題求解的保守性, 因此, 在避碰模式中去掉該約束后可增大解的可行域, 降低保守性。

值得注意的是, 在MPC方法中, 終端部分設計是保證系統(tǒng)漸近穩(wěn)定的關鍵, 其包括終端代價函數(shù)、 局部控制器(或局部反饋控制律)、 終端集約束等3個要素的設計。 其中, 終端代價函數(shù)見式(12), 終端集約束見式(13), 設計局部反饋控制律如下:

(14)

式中:Ki一般被稱為終端反饋增益; 上標κ表示進入終端集的狀態(tài)和相應的控制輸入。

下面給出針對問題1的定理1。

定理1在滿足上述假設1～3的條件下, 對多無人機系統(tǒng)式(7), 若單機目標函數(shù)采用式(8), 終端部分采用式(12)～(14), 通過設計Pi,Ki,δi, 使得對?l≥N, 滿足下列條件:

(15)

(16)

證明

(17)

(18)

(19)

(3) 對?l≥N, 證明終端集(13)為正不變集。

xir(k+1)=Gxir(k)+Huir(k)

(20)

將終端控制輸入式(14)代入式(20)得到

xir(k+1)=(G+HKi)xir(k)

(21)

若能夠找到滿足關系式(22)的正定對稱陣Pi, 則

(G+HKi)TPi(G+HKi)-Pi≤0

(22)

那么, 對于?l≥N,Ωi為系統(tǒng)式(21)的正不變集。 易知, 若式(15)成立, 則可以保證Ωi為正不變集。

(4) 對?l≥N, 證明關系式(16)成立。

若式(15)成立, 則式(16)成立。 式(15)可轉化為LMI問題, 通過選取合適的Qi,Si, 不難得到可行解。 用Xi,Yi代替Pi,Ki, 則式(15)的LMI形式為

(23)

定理1證畢。

為了獲取最大范圍的終端域, 可通過求解下述LMI優(yōu)化問題[26]得到Xi,Yi,δi, 從而得到Pi,Ki:

(24)

s.t. 式(18), (19), (23)

3.3 避碰模式

當UAVi有碰撞風險時, 采用避碰模式, 其優(yōu)化問題可初步描述為問題2, 在對約束條件完成具體設計后, 將該優(yōu)化問題最終描述為問題3。

問題2在采樣時刻k, UAVi從xi(k|k)出發(fā)求解預測時域長度為N的優(yōu)化控制問題, 可表示為

(25)

s.t.xi(k+l+1|k)=Gxi(k+l|k)+Hui(k+l|k),

l=0, …,N-1;xi(k|k)=xi(k);

xi(k+l|k)∈Xi,l=1, …,N;

ui(k+l|k)∈Ui,l=0, …,N-1;

?o∈No,l=1, 2, …,N-1;

?j∈Nv{i},l=1, 2, …,N-1

(26)

其中, 目標函數(shù)式(25)中兩項的具體形式與式(10)、 式(11)相同, 與問題1相比, 問題2的目標函數(shù)式(25)去掉了終端代價Jif。 約束條件式(26)前4項與問題1的式(9)相同, 不同的是增加了與系統(tǒng)外部障礙的避碰約束(式(26)第5項)以及與系統(tǒng)內部無人機的避碰約束(式(26)第6項), 去掉了終端集約束(式(9)第5項)。 其中,po,Ro分別為外部障礙o的位置與半徑,Rs為無人機安全半徑。

兩項避碰約束的細節(jié)設計如下。

(1) 與系統(tǒng)外部障礙的避碰約束

式(26)給出的UAVi與系統(tǒng)外部障礙的避碰約束為

?o∈No,l=1, 2, …,N-1

(27)

實際上, 正如模式切換策略所述, 只有當UAVi中心與障礙物o球面間的最短距離(Dio(k)-Ro)≤Ra時, 才引入UAVi與該障礙的避碰約束。 因此, 在算法實施時, 式(27)中可取o∈No∩Nia, k。po(k+l)是障礙物o在k+l時刻的實際位置值。 根據(jù)上述假設2, 對于外部障礙, 在k時刻, UAVi可通過主動探測獲得其當前位置po(k)與速度vo(k)。 為簡便起見, 假設外部障礙的速度是慢變或不變的, 且UAVi探測到的po(k)和vo(k)是準確的, 那么, 利用式(28)對k時刻外部障礙o的位置進行預測, 則

(28)

綜上, 在算法實施時, 用式(29)代替式(27)。

?o∈No∩Nia, k,l=1, 2, …,N-1

(29)

注2:在預測障礙物的位置變化時, 采用勻速假設, 這一預測方法較為簡單, 適用于障礙物速度不變或者變化較慢的情形。 對于速度變化較快的障礙物, 應采用符合目標運動特性的狀態(tài)模型以及狀態(tài)估計算法。

(2) 與系統(tǒng)內部無人機的避碰約束

約束式(26)所給出的UAVi與系統(tǒng)內部無人機的避碰約束為

?j∈Nv{i},l=1, 2, …,N-1

(30)

實際上, 正如上述模式切換策略所述, 只有當UAVi中心與UAVj球面間的最短距離(Dij(k)-Rs)≤Ra時, 才引入UAVi與UAVj間的避碰約束, 因此, 式(30)中可取j∈(Nv{i})∩Nia, k。

需要注意的是,pj(k+l|k)是UAVj在k時刻對自身的位置預測值, UAVi并不能在k時刻獲取該值。 為此, 引入假設預測狀態(tài)。 為避免混淆, 在預測時域[k,k+N]內, 所使用的UAVi狀態(tài)、 輸入符號命名見表1。

表1 輸入、 狀態(tài)符號表Table 1 Symbol table of inputs and states

一般地, 假設的預測控制輸入序列由上一時刻最優(yōu)預測控制輸入序列與終端控制輸入構成, 即

(31)

ur(k-1+N)

(32)

(33)

?j∈(Nv{i})∩Nia, k,l=1, 2, …,N-1

(34)

式中:μ(l)=λ·l,λ為常數(shù)。 注意, 若j∈(Nv{i})∩Nia, k, 則i∈(Nv{j})∩Nja, k。

另外, 為避免系統(tǒng)內部多個無人機同時執(zhí)行避碰操作時可能引起的沖突問題, 這里采用優(yōu)先級策略, 即按照某一規(guī)則, 為系統(tǒng)內的每個無人機賦予唯一的編號, 編號越小, 優(yōu)先級越高。 當某兩個無人機存在碰撞風險時, 優(yōu)先級高的無人機不進行規(guī)避操作, 即不引入避碰約束, 而優(yōu)先級低的無人機則引入避碰約束, 進行規(guī)避操作。

因此在算法實施時, 設計無人機間避碰約束為

?j∈Ni∩Nia, k,l=1, 2, …,N-1

(35)

式中:Ni={j|j

根據(jù)以上分析, 將避碰模式對應的優(yōu)化問題最終描述為問題3。

問題3在采樣時刻k, UAVi從xi(k|k)出發(fā)求解預測時域長度為N的優(yōu)化控制問題, 可表示為

(36)

s.t.xi(k+l+1|k)=Gxi(k+l|k)+Hui(k+l|k),

l=0, 1, …,N-1;

xi(k|k)=xi(k);

xi(k+l|k)∈Xi,l=1, 2, …,N;

ui(k+l|k)∈Ui,l=0, 1, …,N-1;

?o∈No∩Nia, k,l=1, 2, …,N-1;

?j∈Ni∩Nia, k,l=1, 2, …,N-1;

?j∈(Nv{i})∩Nia, k,l=1, 2, …,N-1

(37)

3.4 算法流程

帶有模式切換的DMPC算法見算法1。

4 性質分析

在對基于DMPC方法的控制系統(tǒng)進行穩(wěn)定性分析時, 一般是利用目標函數(shù)構造Lyapunov函數(shù)并證明其遞減。 為證明其遞減性, 一般需要構造可行解。 跟蹤模式下編隊控制系統(tǒng)的穩(wěn)定性分析見定理2。

定理2在滿足假設1～3且所有無人機均處于跟蹤模式的情況下, 對于編隊系統(tǒng)式(7)中的每個UAVi, 若在k時刻, 通過應用算法1, 問題1具有可行解, 那么對于后續(xù)時刻, 問題1均是可行的; 編隊系統(tǒng)式(7)中的所有無人機能夠在滿足自身狀態(tài)、 輸入約束的前提下, 漸近收斂于各自的期望狀態(tài), 即所有無人機能夠按照期望的相對狀態(tài)跟蹤虛擬領航機。

在k+1時刻, 按照式(31)構造控制輸入序列:

Ui(k+1)=ui(k+1|k+1), …,ui(k+N-1|k+1),

(38)

相應的狀態(tài)序列為

Xi(k+1)=xi(k+2|k+1), …,xi(k+N|k+1),

(39)

下面討論式(38)和式(39)所述Ui(k+1)和Xi(k+1)是否為可行解, 即其是否滿足約束(9)。

對于輸入、 狀態(tài)約束, 由構造方法知,Ui(k+1)和Xi(k+1)的前N-1項滿足這2個約束; 根據(jù)定理1知,Ui(k+1)和Xi(k+1)的第N項亦滿足這2個約束。

綜上,Ui(k+1)和Xi(k+1)滿足k+1時刻問題1的所有約束(9), 是問題1的一個可行解。 以此遞推, 對于后續(xù)時刻, 問題1均是可行的。

穩(wěn)定性證明將k時刻所有無人機最優(yōu)目標函數(shù)的和作為Lyapunov函數(shù), 即

用可行解Ui(k+1)和Xi(k+1)構造k+1時刻的目標函數(shù)之和, 即為JΣ(k+1)。

定理2證畢。

5 數(shù)值仿真分析

5.1 仿真參數(shù)設置

采用6架四旋翼無人機進行仿真實驗。

(1) 虛擬領航機UAVr初始狀態(tài)、 參考輸入為

xr(0)=[16, 0, 50, 0, 0, 0]T

(2) 真實無人機相關參數(shù): 初始速度均為vi(0)=[0, 0, 0]Tm/s, 初始位置pi(0)、 與虛擬領航機的期望相對位置dir, 見表2。

表2 無人機初始位置、 期望相對位置Table 2 Initial position and desired relative position of UAV

(3) 外部障礙物參數(shù): 共4個球形障礙物, 參數(shù)見表3。 障礙物o1為勻速運動, 速度為(0, 0.5, 0)Tm/s, 障礙物o2～o4靜止; 假設只有障礙物o2是預先已知的。

表3 外部障礙物初始位置、 半徑Table 3 Initial position and radius of external obstacles

(4) 其他參數(shù): 采樣周期T=0.5 s, 預測時域N=6, 仿真時間為60 s, 求解優(yōu)化問題的算法采用序列二次規(guī)劃法。

5.2 結果分析

以UAV2為例, 離線求解LMI問題(24)得到參數(shù):

δ2=998.36。

圖4為6架真實無人機及1架虛擬領航機UAVr的三維軌跡圖, 圖5為該三維軌跡的俯視圖。 用黃色虛線表示UAVr的軌跡pr, 其他顏色的虛線表示真實無人機UAVi的期望軌跡pides。 用實心方形表示無人機的初始位置, 實線為實際軌跡pi, 將無人機第2 s, 12 s, 42 s, 60 s所在的實際位置用小球表示。 3個深藍色球分別表示靜態(tài)障礙o2～o4, 2個橙色球表示動態(tài)障礙o1在第0 s和42s的位置。 從圖4～5中可見, 各無人機能夠在避開障礙物的情況下跟蹤上各自的期望軌跡, 并保持預定的隊形, 而且避障的路徑較短。

圖4 三維軌跡Fig.4 Three-dimensional trajectory

圖5 三維軌跡的俯視圖Fig.5 Top view of the three-dimensional trajectory

圖6 軌跡跟蹤誤差Fig.6 Error of track tracking

圖7 無人機模式切換圖Fig.7 Mode switching diagram of UAVs

圖8 兩兩無人機間的距離Fig.8 Distance between two UAVs

圖9 UAV1與UAV2間距離的局部放大圖Fig.9 Partial enlarged view of the distance between UAV1 and UAV2

圖10 各無人機與外部障礙物o1的距離Fig.10 Distance between UAV and external obstacle o1

圖11 各無人機與外部障礙o3的距離Fig.11 Distance between UAV and external obstacle o3

在圖8～11中有3條虛線, 其中, 紅色虛線為安全臨界線, 其值為Rm+Rs, 曲線在紅色虛線以上表明無人機未與障礙發(fā)生碰撞; 黑色虛線為無人機進入避碰模式的臨界線, 其值為Ra+Rm; 黃色虛線為退出避碰模式的臨界線, 其值為Ra+Rm+Rε。

結合上述模式切換策略, 以UAV2為例對圖6～11的仿真結果進行具體分析。 圖6中的綠色線表示UAV2的跟蹤誤差, 圖7中綠色線代表UAV2的模式切換過程, 可見, UAV2有兩個時間段處于避碰模式, 分別為9.5～16.5 s期間與38.5～48 s期間。 結合圖9與圖11分析可知, 在第9.5 s時, UAV2與障礙物o3的距離為11.49 m, 小于Ra+Ro3的12 m, 故UAV2由跟蹤模式切換至避碰模式, 此時引入與障礙物o3間的避碰約束式(35), 在第11 s時, UAV2與UAV1間的距離為5.48 m, 小于Ra+Rs的6 m, 此時引入與UAV1間的避碰約束式(35), 直到第16.5 s時, UAV2與障礙物o3間的距離為13.02 m, 大于Ra+Ro3+Rε的13 m, 達到退出避碰模式的閾值, 故UAV2重新切換至跟蹤模式。 在38.5～48 s期間, 由圖10分析可知, UAV2因規(guī)避障礙物o1而再次切換為避碰模式。 因此, 仿真結果符合本文模式切換策略的設計, 且不存在頻繁切換模式的現(xiàn)象。

以UAV2為例, 在9.5～16.5 s期間, 結合圖9和圖11可知, UAV2為規(guī)避UAV1以及障礙物o3, 而遠離期望軌跡; 在38.5～48 s期間, 結合圖10可知, UAV2因規(guī)避外部障礙物o1而再次遠離期望軌跡, 致使跟蹤誤差增大, 在避開障礙后無人機能夠再次跟蹤上各自的期望軌跡, 從圖6中第45～58 s的局部放大圖中可見, 在46 s后, UAV2的跟蹤誤差小于0.05 m。

圖12為UAV2三個軸向的控制輸入與實際速度曲線。 可見, 所設計的帶模式切換的DMPC算法沒有出現(xiàn)控制量劇烈抖動的現(xiàn)象。 所求解的控制輸入、 實際速度滿足所設置的約束, 均小于4 m/s。

圖12 UAV2控制輸入與實際速度Fig.12 Control input and actual velocity of UAV2

為說明本文算法優(yōu)勢, 與文獻[27]所述的單一控制模式下的DMPC算法進行對比實驗。 仿真參數(shù)設置不變。 圖13為對比實驗的三維軌跡俯視圖, 從圖中可看出, 多無人機系統(tǒng)能夠規(guī)避預先已知的障礙物o2, 也能規(guī)避預先未知的障礙物o1,o3,o4, 但是觀察UAV6軌跡可見, UAV6規(guī)避障礙物o4時的軌跡較為混亂, 其原因是障礙物o4較大且處于期望軌跡上, 此時終端集約束與避碰約束間的沖突使UAV6的優(yōu)化問題難以求解。 而在本文算法下, 避碰軌跡明顯平滑, 見圖5。

圖13 文獻[27]算法下的三維軌跡俯視圖Fig.13 Top view of the three-dimensional trajectory obtained by the algorithm of the literature [27]

采用處理器配置為Intel Core i7-9750H、 主頻2.60 GHz的計算機進行仿真, 兩種算法求解優(yōu)化問題的平均用時見表4。 用σ表示優(yōu)化求解的迭代終止門限, 即當目標函數(shù)值與上一次目標函數(shù)值的差值小于σ時, 迭代終止。 可見, 本文算法的求解時間明顯減少, 例如當σ=1×10-5時, 本文算法的求解時間相較對比實驗算法減少了46.81%。

表4 單機求解一次優(yōu)化問題的平均用時Table 4 Average time for single UAV to solve a single optimization problem