基于分布式模型預(yù)測(cè)控制的欠驅(qū)動(dòng)AUV 編隊(duì)控制

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

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

0 引言

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

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

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

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

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

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

1 模型建立

1.1 單航行器模型

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

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

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

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

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

1.2 編隊(duì)模型

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

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

2 DMPC 控制器設(shè)計(jì)

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

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

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

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

2.1 理想編隊(duì)控制

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

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

同時(shí)需滿足以下等式條件及約束

優(yōu)化問(wèn)題中代價(jià)函數(shù)定義為

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

2.2 避障及避碰控制

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

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

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

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

如果通信拓?fù)錇椤?-1-3”,僅考慮隊(duì)形的保持和避障可能會(huì)導(dǎo)致AUV2和AUV3出現(xiàn)碰撞,如圖4 所示,此時(shí)就要考慮成員之間的避碰。設(shè)計(jì)編隊(duì)避碰控制的代價(jià)函數(shù)為

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

2.3 時(shí)延補(bǔ)償控制

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

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

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

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

設(shè)定算法中 δT=0.1 s,預(yù)測(cè)步長(zhǎng)P=50。通信拓?fù)浣Y(jié)構(gòu)設(shè)計(jì)為“1-2-3”,d1=d2=30 m。。

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

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

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

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

3.2 避障及避碰控制仿真

當(dāng)遇到障礙物時(shí),AUV 編隊(duì)控制目標(biāo)為: 在保持隊(duì)形的同時(shí)繞開(kāi)障礙物,之后恢復(fù)至原預(yù)設(shè)軌跡繼續(xù)航行,直到航行結(jié)束。選取通信拓?fù)錇椤?-1-3”,障礙物信息如表1 所示。

表1 障礙物信息Table 1 Obstacle information

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

圖8 無(wú)避障策略時(shí)AUV 編隊(duì)軌跡Fig.8 AUV formation trajectories without obstacle avoidance strategy

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

圖9 有避障策略時(shí)AUV 編隊(duì)軌跡Fig.9 AUV formation trajectories with obstacle avoidance strategy

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

3.3 時(shí)延補(bǔ)償控制仿真

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

圖11 無(wú)時(shí)延補(bǔ)償下AUV 編隊(duì)軌跡Fig.11 AUV formation trajectories without time delay compensation

圖12 無(wú)時(shí)延補(bǔ)償下AUV 成員之間編隊(duì)誤差Fig.12 Formation errors between AUV members without time delay compensation

圖13 有時(shí)延補(bǔ)償下AUV 編隊(duì)軌跡Fig.13 AUV formation trajectories with time delay compensation

圖14 有時(shí)延補(bǔ)償下AUV 成員之間編隊(duì)誤差Fig.14 Formation errors between AUV members with time delay compensation

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

4 結(jié)束語(yǔ)

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