郭淵博,李 琦,閔博旭,高 劍,陳依民

(西北工業(yè)大學 航海學院,陜西 西安,710072)

0 引言

自主水下航行器(autonomous undersea vehicle,AUV)作為認識和開發(fā)海洋資源的重要工具,具有隱蔽性較好、作業(yè)精度高、任務(wù)重構(gòu)能力強等優(yōu)勢,目前在軍民領(lǐng)域都有著極為廣泛的應(yīng)用[1]。多AUV 編隊由于具有功能冗余、空間分布的特點,彌補了單個AUV 運載能力小、能源動力有限、探索效率偏低的缺陷[2]。

現(xiàn)有編隊控制策略通常包括領(lǐng)航跟隨法[3-6]、虛擬結(jié)構(gòu)法[7-10]和一致性法[11-13]等方法。其中,領(lǐng)航跟隨法是最常見的隊形控制方法,領(lǐng)航者通過不斷調(diào)整自己的行為,引導著隊列往期望位置前行,跟隨者通過領(lǐng)航者的位置和方向求解控制量以達到隊形控制的目的。該方法的局限性是領(lǐng)航者需要與所有的AUV 保持通信關(guān)系,若領(lǐng)航者運動及控制狀態(tài)出現(xiàn)問題,編隊則會無法進行。虛擬結(jié)構(gòu)法是將多AUV 編隊視為一個剛性結(jié)構(gòu),各AUV 作為此剛性結(jié)構(gòu)中的一個頂點,當進行編隊運動時,AUV計算此剛性體的虛擬點位置,根據(jù)虛擬點跟蹤控制器實現(xiàn)隊形保持,缺點是缺乏靈活性。一致性法基于相鄰智能體之間的狀態(tài)偏差,采用一致性控制策略使得群體系統(tǒng)產(chǎn)生一致公共輸出,并在理論上證明達到一致性所需輪數(shù)的上界和下界相同,目前該理論研究仍處于發(fā)展階段,許多相關(guān)理論仍需深入探討。

編隊控制結(jié)構(gòu)可分為集中式控制和分布式控制。集中式控制是對系統(tǒng)的全局優(yōu)化和協(xié)同控制,需考慮所有AUV 的狀態(tài)進行優(yōu)化求解,存在計算量大、可靠性低等問題。相比之下,分布式控制方式僅僅利用局部信息交互,適用于通信帶寬和計算資源受限的情況,通過將整體優(yōu)化問題分配到各個子系統(tǒng),顯著降低了計算量,并提高了系統(tǒng)的容錯性和魯棒性,能夠充分發(fā)揮AUV 的自治性。鑒于此,文中基于分布式控制方式進行編隊研究。

模型預(yù)測控制(model predictive control,MPC)算法是一種先進的反饋控制策略,核心思想在于每一采樣時刻對優(yōu)化控制問題的構(gòu)建,并在線實時計算最優(yōu)控制律,控制系統(tǒng)有能力在線處理各種擾動和不確定性帶來的負面影響[14]。分布式模型預(yù)測控制(distributed model predictive control,DMPC)是一種將MPC 與分布式控制思想有機結(jié)合起來的控制方法,其具備顯式處理約束、抑制干擾和滾動優(yōu)化等MPC 方法的優(yōu)勢,且兼具分布式控制結(jié)構(gòu)容錯性高、結(jié)構(gòu)靈活性強等特點,特別適用于處理多輸入、多參數(shù)和多約束的優(yōu)化問題,在編隊研究中應(yīng)用廣泛[15]。

顧海艷等[16]提出了一種基于DMPC 的多無人機在線協(xié)同航跡規(guī)劃方法,利用MPC 將時空協(xié)同問題轉(zhuǎn)化為滾動優(yōu)化問題,通過對空間協(xié)同問題和同時到達問題設(shè)定不同的優(yōu)先級,實現(xiàn)2 種問題的解耦,從而可同時處理避碰問題和同時到達問題。Cai 等[17-18]提出了一種基于虛擬目標制導的DMPC 框架,可完成多無人機編隊和避障。針對復雜海洋環(huán)境中通信拓撲有限且不可靠問題,Liu 等[19]建立了一種新型的分布式制導系統(tǒng),僅利用部分通信鏈路,基于領(lǐng)導者-跟隨者策略,采用MPC 設(shè)計船舶編隊控制器,解決了通信受限下的編隊問題。

受上述文獻成果啟發(fā),文中設(shè)計了一種基于DMPC 的欠驅(qū)動AUV 編隊控制方法,基于局部鄰居信息為每一個AUV 控制器構(gòu)建預(yù)測控制的代價函數(shù)和約束條件。同時,針對編隊系統(tǒng)可能存在的障礙物避碰問題和通信時延問題,分別設(shè)計了基于距離和相對視線差的避障方法,以及在接收到所有鄰居信息后再求解的等待機制。仿真結(jié)果表明,文中方法可使得多航行器編隊在障礙及通信時延條件下保持隊形穩(wěn)定。

1 模型建立

1.1 單航行器模型

假定AUV 具有良好的上下、左右和前后對稱性,忽略俯仰運動和橫滾運動的影響。AUV 三維空間內(nèi)運動模型可分解為垂直面和水平面,垂直面做定深控制,AUV 的水平運動學模型如圖1 所示。

圖1 單AUV 系統(tǒng)模型示意圖Fig.1 Model of single AUV system

圖2 多AUV 直線隊形示意圖Fig.2 Multiple AUVs’ straight line formation configuration

記z(t)=[x,y,ψ,u,v,r]T為AUV 在時刻t的狀態(tài)向量,u(t)=[X0N]T為控制輸入向量,此模型為非線性控制模型,則有

式中,t0為系統(tǒng)初始時刻。

1.2 編隊模型

設(shè)多AUV 系統(tǒng)由N個AUV 組成,第i個AUV記為AUVi。記多AUV 系統(tǒng)聯(lián)合狀態(tài)向量Z=[z1,z2,···,zN],聯(lián)合控制向量U=[u1,u2,···,uN],得編隊系統(tǒng)的非線性模型為

假設(shè)多AUV 系統(tǒng)具有理想的通信條件,多個水下航行器之間進行局部信息交互,通信拓撲結(jié)構(gòu)可以用圖G={V,E}描述。其中,V={1,2,···,n}為節(jié)點集合,每個水下航行器對應(yīng)一個節(jié)點。E={(i,j)∈V×V}為邊集,表示節(jié)點間的通信關(guān)系。若節(jié)點i和j均能從對方獲取信息,則為一條無向邊,否則為有向邊。若(i,j)∈E,那么對于AUVi∈ E,則有(j,i)∈E且(i,i)?E。將AUVi的鄰居集合設(shè)為Ni,則Ni={j|(j,i)∈E}。文中所有拓撲均為無向、時不變結(jié)構(gòu)。

2 DMPC 控制器設(shè)計

AUV 編隊控制的控制目標為驅(qū)使AUV 以給定的構(gòu)型航行,且具有一定的應(yīng)變復雜環(huán)境的能力。此外,文中做出如下假設(shè)。

假設(shè)1: 所有航行器均預(yù)知期望隊形,但其中僅有1 個航行器預(yù)知路徑信息,每個AUV 只與其相鄰的AUV 共享信息。

假設(shè)2: 已知障礙物的位置信息,障礙物可以用最大直徑描述。

假設(shè)3: 不考慮干擾因素,如水流、噪聲等對航行器狀態(tài)的影響。

2.1 理想編隊控制

記TP＞0表示一個預(yù)測時域,δT為控制周期,在tc=t0+δTc,c∈{0,1,2,···}時刻,s∈[tc,tc+TP]。

為了求解AUV 狀態(tài)約束和終端約束下的最優(yōu)控制輸入,設(shè)計如下優(yōu)化問題

同時需滿足以下等式條件及約束

優(yōu)化問題中代價函數(shù)定義為

估計控制輸入如式(12)所示,包含2 個獨立的部分。第1 部分是在上一個預(yù)測周期s∈[tc-1,tc-1+TP)內(nèi)的最優(yōu)控制輸入序列;第2 部分是預(yù)測周期s∈[tc-1+TP,tc+TP]內(nèi)的控制輸入序列,取值為s=tc-1+TP時的最優(yōu)控制輸入。

2.2 避障及避碰控制

考慮到實際湖海環(huán)境的復雜性,引入避障算法。根據(jù)假設(shè)2,障礙物可以由1 個以障礙物最大長度為直徑的圓形代替,如圖3 所示,其半徑為Ri,AUV與圓形障礙物之間的距離為di,AUV 航向相對于圓形障礙物中心的角度σi∈(-π,π)。

圖3 考慮避障區(qū)域約束的AUV 幾何示意圖Fig.3 Geometry diagram of AUV considering obstacle avoidance zone constraints

設(shè)計di和 σi作為AUV 編隊中成員做出避障反應(yīng)的觸發(fā)條件,即認為當任何AUV 進入避障區(qū)(di＜dsafe)并且靠近避障區(qū)中心(|σi|＜π/2)時,增加的避障控制代價函數(shù)為

式中:Ji為編隊隊形控制時的適應(yīng)度函數(shù);D(di,σi)為避障觸發(fā)條件;μ和 ρ為權(quán)重系數(shù)。

如果通信拓撲為“2-1-3”,僅考慮隊形的保持和避障可能會導致AUV2和AUV3出現(xiàn)碰撞,如圖4 所示,此時就要考慮成員之間的避碰。設(shè)計編隊避碰控制的代價函數(shù)為

圖4 AUV 編隊與障礙物碰撞示意圖Fig.4 Schematic of AUV formation colliding with obstacles

2.3 時延補償控制

假設(shè)在tk時刻,所有的AUV 都成功求解了自身的最優(yōu)化問題,得到相應(yīng)的控制輸入序列和狀態(tài)序列,并將它們的信息發(fā)送給鄰居。系統(tǒng)的采樣周期為δ,在tk+δ時刻,每個AUV 都檢測系統(tǒng)的狀態(tài)。文中方法都是假設(shè)在tk+δ時鄰居間完成了通信,然而,實際中可能由于通信延遲傳輸,無法在時間tk+δ內(nèi)接收其鄰居的信息。

3 仿真驗證與結(jié)果分析

以3 條AUV 組成的編隊為例,使用文中方法對其隊形進行仿真。控制輸入約束條件及預(yù)設(shè)軌跡速度如下: 推進器推力Fi=±37.5 N,轉(zhuǎn)向力矩Ni=±19.16 N·m,單位時間速度改變量 Δv=±0.02 m/s,單位時間力矩改變量 ΔN=±1.92 N·m,預(yù)設(shè)軌跡速度vexp=3 m/s。

3.1 理想編隊控制仿真結(jié)果

設(shè)定算法中 δT=0.1 s,預(yù)測步長P=50。通信拓撲結(jié)構(gòu)設(shè)計為“1-2-3”,d1=d2=30 m。。

圖5 為多AUV 編隊跟蹤直線的隊形保持仿真結(jié)果。圖6 為編隊誤差變化圖,也即成員間實際距離與期望距離的差值。由圖可知,12 s 時直線隊形的編隊距離誤差開始達到期望距離。圖7 展示了多AUV 編隊保持過程中一些物理量的變化,包括推進器推力、轉(zhuǎn)向力矩、前向速度和角速度,可以看出控制輸入都在約束以內(nèi),且趨向穩(wěn)定。由速度變化圖可看到,AUV 隊列的速度最終達到穩(wěn)定,說明編隊最終能跟隨預(yù)設(shè)軌跡航行。

圖5 AUV 編隊直線隊形軌跡圖Fig.5 Trajactories of AUV straight line formation

圖6 AUV 成員間編隊誤差圖Fig.6 Formation errors between AUV members

圖7 各AUV 控制輸入和狀態(tài)曲線Fig.7 Curves of each AUV’s control inputs and states

3.2 避障及避碰控制仿真

當遇到障礙物時,AUV 編隊控制目標為: 在保持隊形的同時繞開障礙物,之后恢復至原預(yù)設(shè)軌跡繼續(xù)航行,直到航行結(jié)束。選取通信拓撲為“2-1-3”,障礙物信息如表1 所示。

表1 障礙物信息Table 1 Obstacle information

在避障的情況下,圖8 為AUV 編隊避碰前軌跡圖,軌跡出現(xiàn)交集時需要考慮避碰。

圖8 無避障策略時AUV 編隊軌跡Fig.8 AUV formation trajectories without obstacle avoidance strategy

圖9 為避碰后軌跡圖,圖10 為AUV 編隊成員2、3 距離變化圖,可以看出AUV2和AUV3的最小距離為3.2 m。在繞開障礙物后,87 s 恢復至原預(yù)設(shè)軌跡繼續(xù)航行,直到航行結(jié)束,說明隊列成功進行了避碰。

圖9 有避障策略時AUV 編隊軌跡Fig.9 AUV formation trajectories with obstacle avoidance strategy

圖10 AUV2 和AUV3 之間距離變化曲線Fig.10 Distance variation curve between AUV2 and AUV3

3.3 時延補償控制仿真

圖11～圖14 展示了具有0.1～0.4 s 隨機通信時延(τk=0.4 s)的情況下,未作處理和加入等待機制后的避障情況,此時求解周期為0.5 s。圖11 和圖13為軌跡圖,圖12 和圖14 為成員誤差圖。

圖11 無時延補償下AUV 編隊軌跡Fig.11 AUV formation trajectories without time delay compensation

圖12 無時延補償下AUV 成員之間編隊誤差Fig.12 Formation errors between AUV members without time delay compensation

圖13 有時延補償下AUV 編隊軌跡Fig.13 AUV formation trajectories with time delay compensation

圖14 有時延補償下AUV 成員之間編隊誤差Fig.14 Formation errors between AUV members with time delay compensation

由圖11 和圖12 可以看出,未處理時延會使得編隊機動能力變差。從圖13 和圖14 可以看出,多AUV 系統(tǒng)在加入等待機制后可以使編隊誤差在非避障階段達到穩(wěn)定。等待機制能夠有效地處理時延,但是加大了DMPC 的求解周期,在具體實施時需要綜合考慮。

4 結(jié)束語

文中設(shè)計了一種分布式模型預(yù)測控制框架,每個AUV 只與其相鄰的AUV 共享信息且只有1 個航行器知道預(yù)設(shè)的路徑信息。每個AUV 在編隊控制過程中只需考慮鄰居的狀態(tài)信息,并以此構(gòu)建預(yù)測控制的代價函數(shù)和約束條件。針對編隊系統(tǒng)可能存在的障礙物威脅問題,設(shè)計了一種基于距離和相對視線差的避障方法,并考慮了隊列內(nèi)避碰。針對可能存在的時延問題,設(shè)計了一種等待機制,使每個AUV 在接收到所有鄰居信息后再求解,一定程度上減少了時延的影響。最后,通過仿真驗證了文中算法的有效性。