[发明专利]基于分层分布式模型预测控制的AUV编队协同控制方法

权利要求书

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

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

其中，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：对上层运动学系统构造二次性能指标：

其中，和分别为上层系统优化目标函数的状态量、控制量和终端状态的影响系数，为各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：求解最优的控制序列：

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的动力学特性，建立下层系统的状态方程：

其中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：对下层系统构造二次性能指标：

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：求解最优的控制序列：

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

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

其中，w^*(l)＝[w^*(l|l),…,w^*(l+N-1|l)]^T表示控制输入最优序列；w_min表示控制输入允许的最小值输入量，w_max表示控制输入允许的最大值输入量；(16)式按元素满足不等式；对于终端状态满足(17)式的终端范围约束。