本發(fā)明涉及控制系統(tǒng)領域，具體涉及使用模型控制算法在分布式框架下實現(xiàn)編隊協(xié)同控制的一種新型方法。
背景技術：
近年來，隨著多水下自主航行器(Autonomous Underwater Vehicle,AUV)技術在深海資源探測、海底繪圖、協(xié)同護航、海洋搜救等方面應用需求的不斷增加，對多AUV協(xié)同控制的研究，特別是在集群、合作、跟蹤和編隊等方面得到了快速的發(fā)展。AUV編隊協(xié)同控制指多個AUV在執(zhí)行任務的過程中，適應實際環(huán)境的約束，達到保持某種規(guī)定隊形作業(yè)的一種控制技術。這種控制技術可以完成復雜任務的分布化：將各項任務分散到大量低成本，功能單一的子AUV中，通過各個子AUV系統(tǒng)之間的協(xié)作來完成原本復雜的任務。編隊協(xié)同技術對提高多AUV控制效率、節(jié)約系統(tǒng)成本和保證系統(tǒng)可靠性也具有重要的意義。
模型預測控制(Model predictive control,MPC)是近年來廣泛研究的一種反饋控制策略，是一種除PID控制算法以外，工業(yè)界應用最多的一種新型高級控制方法。這個算法是一種基于模型的閉環(huán)優(yōu)化控制策略，其算法具有控制效果好、魯棒性強等優(yōu)點，可以有效地克服過程的不確定性、非線性和耦合性，并能方便的處理被控變量和操縱變量中的各種約束。模型預測控制的機理可以描述為：在每一采樣時刻，根據(jù)獲取系統(tǒng)當前的狀態(tài)和輸入信息，在線求解一個有限時域開環(huán)優(yōu)化問題，并且將得到的控制序列的第一個元素作用于系統(tǒng)的控制對象，在下一采樣時刻，重復上述過程，不斷用新的預測值刷新優(yōu)化問題并求得最新解。隨著計算機計算性能的不斷提升，模型預測控制的實時性變的更強，具有應用到AUV控制的廣泛前景。
AUV編隊控制主要解決AUV群組在某一設定速度下，并且沿著一個期望的多路徑編隊運動問題。這個控制問題主要可以分為個體的路徑跟蹤和不同個體之間實現(xiàn)協(xié)同控制兩個問題。對于單個AUV的路徑跟蹤問題在許多文獻中都被研究。如Breivik和Fossen在研究中考慮了欠驅(qū)動三自由度的單個航行器，提出其控制策略，并且在模型仿真和試驗中均得到驗證。Fredriksen等人同樣提出了基于Line-of-sight(LOS)導引的控制策略，證明了穩(wěn)定性，并在后來一個實驗中也證實了該理論成果。對多AUV協(xié)同控制方面的研究，Skjetne等人在全驅(qū)動的航行器的研究中提出了一種集成編隊控制策略的操縱控制方法。Pascoal等人對一種全驅(qū)動的水下航行器的平面運動問題，提出了分散編隊控制思想。這種控制策略是將解耦控制問題的思想應用于個體的路徑跟蹤及不同個體信息協(xié)同中。Scattolini中研究了一種被動的同步路徑跟蹤控制方法。Lapierre等人考慮兩個欠驅(qū)動水下航行器沿著平行平面路徑，并且保持一個恒定距離的控制問題，提出了一種leader-follower控制策略。
分布式控制策略相比之前學者研究的集中式控制策略能夠減少通信和計算成本，提高系統(tǒng)的控制性能；對于控制系統(tǒng)采用分層結構可以較少系統(tǒng)的優(yōu)化頻率，降低優(yōu)化次數(shù)，而且更加便于實際工程的應用；模型預測算法控制率是一種先進的控制算法，相比其他算法具有顯示地處理物理約束和優(yōu)化軌跡的優(yōu)勢。比如在舵機性能和輸入受限的實際情況。
技術實現(xiàn)要素：
要解決的技術問題
為了避免現(xiàn)有技術的不足之處，本發(fā)明提出一種基于分層分布式模型預測控制的AUV編隊協(xié)同控制方法。
技術方案
為了實現(xiàn)AUV編隊的協(xié)調(diào)控制，本發(fā)明提供一種AUV編隊的分層分布式模型預測控制協(xié)同策略。通過分析編隊之間的運動學特性和動力學特性，建立上層和下層串聯(lián)結構的分層控制體系結構：設計分布式模型預測控制算法實現(xiàn)多AUV上層系統(tǒng)的協(xié)同控制，設計非線性模型預測控制算法實現(xiàn)AUV下層系統(tǒng)的跟蹤控制。最后串聯(lián)實現(xiàn)在整個AUV編隊的協(xié)同控制，達到預定多路徑規(guī)劃。
一種基于分層分布式模型預測控制的AUV編隊協(xié)同控制方法，其特征在于步驟如下：
步驟1：假設編隊控制中有M個AUV，每個AUV的橫滾問題已被解決；通過分析第i個AUV的運動學特性，其中i∈M，建立上層運動學系統(tǒng)的狀態(tài)方程：
其中，zi(t)表示上層系統(tǒng)的狀態(tài)矢量，即xi(t)、yi(t)和zi(t)表示第i個AUV在全局空間下t時刻的X軸、Y軸和Z軸的坐標位置，φi(t)和θi(t)表示AUV系統(tǒng)t時刻的偏航角和俯仰角；ui(t)表示上層系統(tǒng)的控制矢量，ui(t)、vi(t)和wi(t)表示AUV在全局空間下t時刻的每個坐標相對應的控制速度，和ri(t)表示t時刻偏航角的角速度和俯仰角的角速度；
步驟2：將上層系統(tǒng)狀態(tài)方程以采樣周期Tu離散化，得到離散的狀態(tài)方程：
zi(k+1)＝Aizi(k)+Biui(k) (2)
其中，Ai為系統(tǒng)矩陣，Bi為系統(tǒng)的控制輸入矩陣，k為離散參數(shù)點；
步驟3：對上層運動學系統(tǒng)構造二次性能指標：
其中，和為上層系統(tǒng)優(yōu)化目標函數(shù)的狀態(tài)量、控制量和終端狀態(tài)的影響系數(shù)，為各AUV的協(xié)同合作指標項的影響系數(shù)，Ri＝(Rxi,Ryi,Rzi,0,0)表示全局空間坐標下控制路徑的期望航路點矢量，為鄰居jAUV的預測狀態(tài)，在上式子(6)中，設計Kj為一個可收斂的N*5階矩陣，和表示在空間坐標系下，每個AUV和鄰居AUV之間需要協(xié)同保證的各個方位的距離信息，其中i,j∈M，且i≠j；公式(6)為合作指標項，保證第i個AUV的狀態(tài)與鄰居的保持編隊協(xié)同；Gi(zi(k+N))表示系統(tǒng)二次性能指標中的終端成本函數(shù)；
步驟4：求解最優(yōu)的控制序列：
zi(k+1)＝Aizi(k)+Biui(k), (8)
umin≤ui(k+j|k)≤umax,j＝0,…,N-1, (9)
zi(k+N|k)∈Ωi, (10)
其中，表示系統(tǒng)控制輸入最優(yōu)序列；umin表示控制輸入允許的最小值輸入量，umax表示控制輸入允許的最大值輸入量；(9)式按元素滿足不等式；對于終端狀態(tài)滿足(10)式終端范圍約束；
步驟5：通過優(yōu)化計算得到第i個AUV上層系統(tǒng)控制器最優(yōu)的輸入控制量，將其傳遞給下層系統(tǒng)，作為下層系統(tǒng)整個優(yōu)化過程的參考軌跡即也就是下層控制器在[l*T,(l+Nd-1)*T]時間區(qū)間內(nèi)的設定值；
步驟6：對于下層控制系統(tǒng)，通過分析單個AUV的動力學特性，建立下層系統(tǒng)的狀態(tài)方程：
其中u(t)表示下層系統(tǒng)的狀態(tài)矢量，即w(t)表示k時刻下層系統(tǒng)的控制矢量，即w(t)＝[τ水(t),τ垂(t),δ水(t),δ垂(t)]T，τ水(t)和δ水(t)表示在空間X-Y平面坐標系下，t時刻的航行器的推力和舵偏轉，τ垂(t)和δ垂(t)表示在與X-Y垂直的平面坐標系下，t時刻的航行器的推力和舵偏轉；
步驟7：將下層系統(tǒng)狀態(tài)方程以采樣周期Td＝Tu/Nd進行離散化，其中Nd為下層系統(tǒng)控制域的循環(huán)次數(shù)；得到離散的狀態(tài)方程：
u(l+1)＝Au(l)+Bw(l) (12)
其中，A為系統(tǒng)矩陣，B為系統(tǒng)的控制輸入矩陣；
步驟8：對下層系統(tǒng)構造二次性能指標：
L(l+j|l,u,w,ur)＝α||u(l+j|l)-ur||2+ρ||w(l+j|l)||2
G(u(l+N|l))＝β||u(l+N|l)-ur||2
其中，G(u(l+T|l))表示系統(tǒng)二次性能指標中的終端成本函數(shù)；
步驟9：求解最優(yōu)的控制序列：
u(l+1)＝Au(l)+Bw(l), (15)
wmin≤w(l+j|l)≤wmax,j＝0,…,N-1, (16)
其中，w*(l)＝[w*(l|l),…,w*(l+N-1|l)]T表示控制輸入最優(yōu)序列；wmin表示控制輸入允許的最小值輸入量，wmax表示控制輸入允許的最大值輸入量；(16)式按元素滿足不等式；對于終端狀態(tài)滿足(17)式的終端范圍約束。
有益效果
本發(fā)明提出的一種基于分層分布式模型預測控制的AUV編隊協(xié)同控制方法，有益效果為：①提高系統(tǒng)控制的實時性。分層可減少上層系統(tǒng)優(yōu)化的頻率，并進一步減少整個優(yōu)化次數(shù)。②便于工程應用。采用分布式結構和分層的優(yōu)化框架更加便于實際應用。③兩層控制系統(tǒng)均采用模型預測控制算法可處理實際系統(tǒng)的約束。比如在舵機輸入受限的實際情況。④優(yōu)化計算方法，可以在極大程度上提高系統(tǒng)的優(yōu)化控制性能。
附圖說明
圖1為分層分布式模型預測控制的流程圖
具體實施方式
現(xiàn)結合實施例、附圖對本發(fā)明作進一步描述：
基于分層分布式模型預測控制算法，結合上層系統(tǒng)和下層系統(tǒng)的控制方案，對于實現(xiàn)編隊協(xié)同中每個AUV之間的信息交換、優(yōu)化計算和協(xié)同控制策略，我們設計以下算法：
離線部分：首先，在上層系統(tǒng)中，對于第i個AUV系統(tǒng)的運動學方程以周期Ti進行線性離散化。設計上層系統(tǒng)優(yōu)化目標函數(shù)的狀態(tài)量、控制量和終端狀態(tài)的影響系數(shù)和以及各AUV的協(xié)同合作指標項的影響系數(shù)設計控制器的終端的約束集合Ωi；在此設計上層系統(tǒng)的預測域長度和控制域的循環(huán)次數(shù)為N和Nu(且N≥Nu)；目標協(xié)同軌跡參考矢量Ri＝(Rxi,Ryi,Rzi,0,0)；設計每個鄰居AUV的對進行優(yōu)化的預測系數(shù)Kj。其次，在下層控制器中，以Td＝Tu/Nd為周期建立下層系統(tǒng)離散的動力學方程，設計控制器優(yōu)化目標函數(shù)的狀態(tài)量、控制量和終端狀態(tài)的影響參數(shù)α、ρ和β；在此設計下層系統(tǒng)的預測域長度和控制域的循環(huán)次數(shù)為N和Nd，(且N≥Nd)；設計控制器終端的約束集合
1)在上層系統(tǒng)在線計算部分，控制域的循環(huán)次數(shù)為Nu，執(zhí)行以下過程：
a.在初始時刻，假設編隊中每個AUV的初始狀態(tài)都已經(jīng)獲得，選取第i個AUV的初始狀態(tài)zi(k)進行優(yōu)化；
b.第i個AUV接受相鄰j(j≠i)發(fā)來當下的預測狀態(tài)
c.發(fā)送自己當下狀態(tài)的預測值給周圍的鄰居AUV；
d.求解優(yōu)化問題Ji(k,zi,ui)，求解得到滿足約束的最優(yōu)控制輸入序列
e.將最優(yōu)控制輸入值傳遞給下層控制系統(tǒng)。
2)在下層系統(tǒng)在線計算部分，控制時域的循環(huán)次數(shù)為Nd，執(zhí)行以下過程：
a.接受上層系統(tǒng)傳遞的信息，記作為下層系統(tǒng)在優(yōu)化過程的參考值；
b.獲取下層系統(tǒng)的狀態(tài)初始值u(l)；
d.求解優(yōu)化問題J(l,u,w,ur)，求解得到滿足約束的最優(yōu)控制輸入序列w*(l)；
e.將最優(yōu)的控制輸入值w*(l|l)，應用到系統(tǒng)控制器；
f.令l＝l+1，循環(huán)計算執(zhí)行N步后，返回上層控制系統(tǒng)。
對分層分布式模型預測控制方法的設計包括以下步驟：
1)假設編隊控制中有M個AUV，每個AUV的橫滾問題已被解決。通過分析第i(i∈M)個AUV的運動學特性，建立上層運動學系統(tǒng)的狀態(tài)方程：
其中zi(t)表示上層系統(tǒng)的狀態(tài)矢量，即xi,yi和zi表示第i個AUV在全局空間下的X軸、Y軸和Z軸的坐標位置，φi和θi表示AUV系統(tǒng)的偏航角和俯仰角；ui(t)表示上層系統(tǒng)的控制矢量，即ui,vi和wi表示AUV在全局空間下的每個坐標相對應的控制速度，和ri表示偏航角的角速度和俯仰角的角速度。
2)將上層系統(tǒng)狀態(tài)方程以采樣周期Tu離散化，得到離散的狀態(tài)方程：
zi(k+1)＝Aizi(k)+Biui(k) (2)
在此公式中，Ai為系統(tǒng)矩陣，Bi為系統(tǒng)的控制輸入矩陣。
3)對上層運動學系統(tǒng)構造二次性能指標：
具體如下所示，
在上式子(4)中，其中Ri＝(Rxi,Ryi,Rzi,0,0)表示全局空間坐標下控制路徑的期望航路點矢量。我們定義在編隊中，對每個AUV在某一確定距離的半徑范圍內(nèi)，并且可以保證與之互相通信的其余AUV為其鄰居。記鄰居j AUV的預測狀態(tài)在上式子(6)中，我們設計Kj為一個可收斂的N*5階矩陣。和(i,j∈M，且i≠j)表示在空間坐標系下，每個AUV和鄰居AUV之間需要協(xié)同保證的各個方位的距離信息。公式(6)為合作指標項，保證第i個AUV的狀態(tài)與鄰居的保持編隊協(xié)同。Gi(zi(k+N))表示系統(tǒng)二次性能指標中的終端成本函數(shù)。
4)求解最優(yōu)的控制序列：
zi(k+1)＝Aizi(k)+Biui(k), (8)
umin≤ui(k+j|k)≤umax,j＝0,…,N-1, (9)
zi(k+N|k)∈Ωi, (10)
其中表示系統(tǒng)控制輸入最優(yōu)序列；umin表示控制輸入允許的最小值輸入量，umax表示控制輸入允許的最大值輸入量；(9)式按元素滿足不等式；對于終端狀態(tài)滿足(10)式終端范圍約束。
通過優(yōu)化計算得到第i個AUV上層系統(tǒng)控制器最優(yōu)的輸入控制量，將其傳遞給下層系統(tǒng)，作為下層系統(tǒng)整個優(yōu)化過程的參考軌跡即也就是下層控制器在[l*T,(l+Nd-1)*T]時間區(qū)間內(nèi)的設定值。由于下層系統(tǒng)并無鄰居之間的協(xié)同關系，因此為了書寫方便，對于每個對應優(yōu)化的AUV，在下層系統(tǒng)優(yōu)化過程中將去掉每個相關參數(shù)的下標，并作此說明。
5)對于下層控制系統(tǒng)，通過分析單個AUV的動力學特性，建立下層系統(tǒng)的狀態(tài)方程：
其中u(t)表示下層系統(tǒng)的狀態(tài)矢量，即w(t)表示k時刻下層系統(tǒng)的控制矢量，即w(t)＝[τ水(t),τ垂(t),δ水(t),δ垂(t)]T。τ水和δ水表示在空間X-Y平面坐標系下，航行器的推力和舵偏轉。τ垂和δ垂表示在與X-Y垂直的平面坐標系下，航行器的推力和舵偏轉。
6)將下層系統(tǒng)狀態(tài)方程以采樣周期Td＝Tu/Nd(Nd為下層系統(tǒng)控制域的循環(huán)次數(shù))離散化。得到離散的狀態(tài)方程：
u(l+1)＝Au(l)+Bw(l) (12)
在此公式中，A為系統(tǒng)矩陣，B為系統(tǒng)的控制輸入矩陣。
7)對上述系統(tǒng)構造二次性能指標：
具體如下所示：L(l+j|l,u,w,ur)＝α||u(l+j|l)-ur||2+ρ||w(l+j|l)||2
G(u(l+N|l))＝β||u(l+N|l)-ur||2
這里G(u(l+T|l))表示系統(tǒng)二次性能指標中的終端成本函數(shù)。
8)求解最優(yōu)的控制序列：
u(l+1)＝Au(l)+Bw(l), (15)
wmin≤w(l+j|l)≤wmax,j＝0,…,N-1, (16)
其中w*(l)＝[w*(l|l),…,w*(l+N-1|l)]T表示控制輸入最優(yōu)序列；wmin表示控制輸入允許的最小值輸入量，wmax表示控制輸入允許的最大值輸入量；(16)式按元素滿足不等式；對于終端狀態(tài)滿足(17)式的終端范圍約束。