基于分层分布式模型预测控制的AUV编队协同控制方法与流程

技术特征：

1.一种基于分层分布式模型预测控制的AUV编队协同控制方法，其特征在于步骤如下：

步骤1：假设编队控制中有M个AUV，每个AUV的横滚问题已被解决；通过分析第i个AUV的运动学特性，其中i∈M，建立上层运动学系统的状态方程：

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

其中，z_i(t)表示上层系统的状态矢量，即x_i(t)、y_i(t)和z_i(t)表示第i个AUV在全局空间下t时刻的X轴、Y轴和Z轴的坐标位置，φ_i(t)和θ_i(t)表示AUV系统t时刻的偏航角和俯仰角；u_i(t)表示上层系统的控制矢量，u_i(t)、v_i(t)和w_i(t)表示AUV在全局空间下t时刻的每个坐标相对应的控制速度，和r_i(t)表示t时刻偏航角的角速度和俯仰角的角速度；

步骤2：将上层系统状态方程以采样周期T_u离散化，得到离散的状态方程：

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

其中，A_i为系统矩阵，B_i为系统的控制输入矩阵，k为离散参数点；

步骤3：对上层运动学系统构造二次性能指标：

$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}$

其中，和为上层系统优化目标函数的状态量、控制量和终端状态的影响系数，为各AUV的协同合作指标项的影响系数，R_i＝(R_xi,R_yi,R_zi,0,0)表示全局空间坐标下控制路径的期望航路点矢量，为邻居jAUV的预测状态，在上式子(6)中，设计K_j为一个可收敛的N*5阶矩阵，和表示在空间坐标系下，每个AUV和邻居AUV之间需要协同保证的各个方位的距离信息，其中i,j∈M，且i≠j；公式(6)为合作指标项，保证第i个AUV的状态与邻居的保持编队协同；G_i(z_i(k+N))表示系统二次性能指标中的终端成本函数；

步骤4：求解最优的控制序列：

$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)

其中，表示系统控制输入最优序列；u_min表示控制输入允许的最小值输入量，u_max表示控制输入允许的最大值输入量；(9)式按元素满足不等式；对于终端状态满足(10)式终端范围约束；

步骤5：通过优化计算得到第i个AUV上层系统控制器最优的输入控制量，将其传递给下层系统，作为下层系统整个优化过程的参考轨迹即也就是下层控制器在[l*T,(l+N_d-1)*T]时间区间内的设定值；

步骤6：对于下层控制系统，通过分析单个AUV的动力学特性，建立下层系统的状态方程：

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

其中u(t)表示下层系统的状态矢量，即w(t)表示k时刻下层系统的控制矢量，即w(t)＝[τ_水(t),τ_垂(t),δ_水(t),δ_垂(t)]^T，τ_水(t)和δ_水(t)表示在空间X-Y平面坐标系下，t时刻的航行器的推力和舵偏转，τ_垂(t)和δ_垂(t)表示在与X-Y垂直的平面坐标系下，t时刻的航行器的推力和舵偏转；

步骤7：将下层系统状态方程以采样周期T_d＝T_u/N_d进行离散化，其中N_d为下层系统控制域的循环次数；得到离散的状态方程：

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

其中，A为系统矩阵，B为系统的控制输入矩阵；

步骤8：对下层系统构造二次性能指标：

$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))表示系统二次性能指标中的终端成本函数；

步骤9：求解最优的控制序列：

$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表示控制输入最优序列；w_min表示控制输入允许的最小值输入量，w_max表示控制输入允许的最大值输入量；(16)式按元素满足不等式；对于终端状态满足(17)式的终端范围约束。