技術(shù)特征：

1.一種基于分層分布式模型預(yù)測(cè)控制的AUV編隊(duì)協(xié)同控制方法，其特征在于步驟如下：

步驟1：假設(shè)編隊(duì)控制中有M個(gè)AUV，每個(gè)AUV的橫滾問題已被解決；通過分析第i個(gè)AUV的運(yùn)動(dòng)學(xué)特性，其中i∈M，建立上層運(yùn)動(dòng)學(xué)系統(tǒng)的狀態(tài)方程：

${\stackrel{\·}{z}}_i\left(t\right)=f(z_i\left(t\right),u_i\left(t\right))---\left(1\right)$

其中，z_i(t)表示上層系統(tǒng)的狀態(tài)矢量，即x_i(t)、y_i(t)和z_i(t)表示第i個(gè)AUV在全局空間下t時(shí)刻的X軸、Y軸和Z軸的坐標(biāo)位置，φ_i(t)和θ_i(t)表示AUV系統(tǒng)t時(shí)刻的偏航角和俯仰角；u_i(t)表示上層系統(tǒng)的控制矢量，u_i(t)、v_i(t)和w_i(t)表示AUV在全局空間下t時(shí)刻的每個(gè)坐標(biāo)相對(duì)應(yīng)的控制速度，和r_i(t)表示t時(shí)刻偏航角的角速度和俯仰角的角速度；

步驟2：將上層系統(tǒng)狀態(tài)方程以采樣周期T_u離散化，得到離散的狀態(tài)方程：

z_i(k+1)＝A_iz_i(k)+B_iu_i(k) (2)

其中，A_i為系統(tǒng)矩陣，B_i為系統(tǒng)的控制輸入矩陣，k為離散參數(shù)點(diǎn)；

步驟3：對(duì)上層運(yùn)動(dòng)學(xué)系統(tǒng)構(gòu)造二次性能指標(biāo)：

$J_i(k,z_i,u_i)=\underset{j=0}{\overset{N-1}{\Σ}}{L}_i(k+j|k,z_i,u_i)+G_i\left(z_i\right(k+N|k\left)\right)---\left(3\right)$

$L_i(k,z_i,u_i)={\mathrm{\α}}_i^x\left|\right|x_i\left(k\right)-R_{xi}|{|}^2+{\mathrm{\α}}_i^y||y_i\left(k\right)-R_{yi}|{|}^2+{\mathrm{\α}}_i^z\left|\right|z_i\left(k\right)-R_{zi}|{|}^2+{\mathrm{\α}}_i^{\mathrm{\φ}}|\left|{\mathrm{\φ}}_i\left(k\right)\right|{|}^2+{\mathrm{\α}}_i^{\mathrm{\θ}}\left|\right|{\mathrm{\θ}}_i\left(k\right)|{|}^2---\left(4\right)$

$+\underset{j\∈M}{\Σ}{\mathrm{\β}}_{ij}^x\left|\right|{\tilde{x}}_j\left(k\right)-x_i\left(k\right)-L_{ij}^x|{|}^2+\underset{j\∈M}{\Σ}{\mathrm{\β}}_{ij}^y||{\tilde{y}}_j\left(k\right)-y_i\left(k\right)-L_{ij}^y|{|}^2+\underset{j\∈M}{\Σ}{\mathrm{\β}}_{ij}^z\left|\right|{\tilde{z}}_j\left(k\right)-z_i\left(k\right)-L_{ij}^z|{|}^2---\left(6\right)$

$\begin{array}{c}{G}_i\left(z_i\right(k+N\left)\right)=q_i^x\left|\right|x_i(k+N)-R_{xi}|{|}^2+q_i^y||y_i(k+N)-R_{yi}|{|}^2+q_i^z\left|\right|z_i(k+N)-R_{zi}|{|}^2\\ +q_i^{\mathrm{\φ}}\left|\right|{\mathrm{\φ}}_i(k+N)|{|}^2+q_i^{\mathrm{\θ}}|\left|{\mathrm{\θ}}_i(k+N)\right|{|}^2\end{array}$

其中，和為上層系統(tǒng)優(yōu)化目標(biāo)函數(shù)的狀態(tài)量、控制量和終端狀態(tài)的影響系數(shù)，為各AUV的協(xié)同合作指標(biāo)項(xiàng)的影響系數(shù)，R_i＝(R_xi,R_yi,R_zi,0,0)表示全局空間坐標(biāo)下控制路徑的期望航路點(diǎn)矢量，為鄰居jAUV的預(yù)測(cè)狀態(tài)，在上式子(6)中，設(shè)計(jì)K_j為一個(gè)可收斂的N*5階矩陣，和表示在空間坐標(biāo)系下，每個(gè)AUV和鄰居AUV之間需要協(xié)同保證的各個(gè)方位的距離信息，其中i,j∈M，且i≠j；公式(6)為合作指標(biāo)項(xiàng)，保證第i個(gè)AUV的狀態(tài)與鄰居的保持編隊(duì)協(xié)同；G_i(z_i(k+N))表示系統(tǒng)二次性能指標(biāo)中的終端成本函數(shù)；

步驟4：求解最優(yōu)的控制序列：

$u_i^{*}\left(k\right)=\underset{u_i\left(k\right)}{\mathrm{argmin}}{J}_i(k,z_i,u_i),subjectto---\left(7\right)$

z_i(k+1)＝A_iz_i(k)+B_iu_i(k), (8)

u_min≤u_i(k+j|k)≤u_max,j＝0,…,N-1, (9)

z_i(k+N|k)∈Ω_i, (10)

其中，表示系統(tǒng)控制輸入最優(yōu)序列；u_min表示控制輸入允許的最小值輸入量，u_max表示控制輸入允許的最大值輸入量；(9)式按元素滿足不等式；對(duì)于終端狀態(tài)滿足(10)式終端范圍約束；

步驟5：通過優(yōu)化計(jì)算得到第i個(gè)AUV上層系統(tǒng)控制器最優(yōu)的輸入控制量，將其傳遞給下層系統(tǒng)，作為下層系統(tǒng)整個(gè)優(yōu)化過程的參考軌跡即也就是下層控制器在[l*T,(l+N_d-1)*T]時(shí)間區(qū)間內(nèi)的設(shè)定值；

步驟6：對(duì)于下層控制系統(tǒng)，通過分析單個(gè)AUV的動(dòng)力學(xué)特性，建立下層系統(tǒng)的狀態(tài)方程：

$\stackrel{\·}{u}\left(t\right)=h\left(u\right(t),w\left(t\right))---\left(11\right)$

其中u(t)表示下層系統(tǒng)的狀態(tài)矢量，即w(t)表示k時(shí)刻下層系統(tǒng)的控制矢量，即w(t)＝[τ_水(t),τ_垂(t),δ_水(t),δ_垂(t)]^T，τ_水(t)和δ_水(t)表示在空間X-Y平面坐標(biāo)系下，t時(shí)刻的航行器的推力和舵偏轉(zhuǎn)，τ_垂(t)和δ_垂(t)表示在與X-Y垂直的平面坐標(biāo)系下，t時(shí)刻的航行器的推力和舵偏轉(zhuǎn)；

步驟7：將下層系統(tǒng)狀態(tài)方程以采樣周期T_d＝T_u/N_d進(jìn)行離散化，其中N_d為下層系統(tǒng)控制域的循環(huán)次數(shù)；得到離散的狀態(tài)方程：

u(l+1)＝Au(l)+Bw(l) (12)

其中，A為系統(tǒng)矩陣，B為系統(tǒng)的控制輸入矩陣；

步驟8：對(duì)下層系統(tǒng)構(gòu)造二次性能指標(biāo)：

$J(l,u,w,u_r)=\underset{j=0}{\overset{N-1}{\Σ}}L(l+j|l,u,w,u_r)+G\left(u\right(l+N\left|l\right)---\left(13\right)$

L(l+j|l,u,w,u_r)＝α||u(l+j|l)-u_r||²+ρ||w(l+j|l)||²

G(u(l+N|l))＝β||u(l+N|l)-u_r||²

其中，G(u(l+T|l))表示系統(tǒng)二次性能指標(biāo)中的終端成本函數(shù)；

步驟9：求解最優(yōu)的控制序列：

$w^{*}\left(l\right)=\underset{w\left(l\right)}{\mathrm{argmin}}J(l,u,w,u_r),subjectto---\left(14\right)$

u(l+1)＝Au(l)+Bw(l), (15)

w_min≤w(l+j|l)≤w_max,j＝0,…,N-1, (16)

$w(l+N|l)\∈\stackrel{\‾}{\mathrm{\Ω}},---\left(17\right)$

其中，w^*(l)＝[w^*(l|l),…,w^*(l+N-1|l)]^T表示控制輸入最優(yōu)序列；w_min表示控制輸入允許的最小值輸入量，w_max表示控制輸入允許的最大值輸入量；(16)式按元素滿足不等式；對(duì)于終端狀態(tài)滿足(17)式的終端范圍約束。