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

本发明涉及控制系统领域，具体涉及使用模型控制算法在分布式框架下实现编队协同控制的一种新型方法。

背景技术：

近年来，随着多水下自主航行器(Autonomous Underwater Vehicle,AUV)技术在深海资源探测、海底绘图、协同护航、海洋搜救等方面应用需求的不断增加，对多AUV协同控制的研究，特别是在集群、合作、跟踪和编队等方面得到了快速的发展。AUV编队协同控制指多个AUV在执行任务的过程中，适应实际环境的约束，达到保持某种规定队形作业的一种控制技术。这种控制技术可以完成复杂任务的分布化：将各项任务分散到大量低成本，功能单一的子AUV中，通过各个子AUV系统之间的协作来完成原本复杂的任务。编队协同技术对提高多AUV控制效率、节约系统成本和保证系统可靠性也具有重要的意义。

模型预测控制(Model predictive control,MPC)是近年来广泛研究的一种反馈控制策略，是一种除PID控制算法以外，工业界应用最多的一种新型高级控制方法。这个算法是一种基于模型的闭环优化控制策略，其算法具有控制效果好、鲁棒性强等优点，可以有效地克服过程的不确定性、非线性和耦合性，并能方便的处理被控变量和操纵变量中的各种约束。模型预测控制的机理可以描述为：在每一采样时刻，根据获取系统当前的状态和输入信息，在线求解一个有限时域开环优化问题，并且将得到的控制序列的第一个元素作用于系统的控制对象，在下一采样时刻，重复上述过程，不断用新的预测值刷新优化问题并求得最新解。随着计算机计算性能的不断提升，模型预测控制的实时性变的更强，具有应用到AUV控制的广泛前景。

AUV编队控制主要解决AUV群组在某一设定速度下，并且沿着一个期望的多路径编队运动问题。这个控制问题主要可以分为个体的路径跟踪和不同个体之间实现协同控制两个问题。对于单个AUV的路径跟踪问题在许多文献中都被研究。如Breivik和Fossen在研究中考虑了欠驱动三自由度的单个航行器，提出其控制策略，并且在模型仿真和试验中均得到验证。Fredriksen等人同样提出了基于Line-of-sight(LOS)导引的控制策略，证明了稳定性，并在后来一个实验中也证实了该理论成果。对多AUV协同控制方面的研究，Skjetne等人在全驱动的航行器的研究中提出了一种集成编队控制策略的操纵控制方法。Pascoal等人对一种全驱动的水下航行器的平面运动问题，提出了分散编队控制思想。这种控制策略是将解耦控制问题的思想应用于个体的路径跟踪及不同个体信息协同中。Scattolini中研究了一种被动的同步路径跟踪控制方法。Lapierre等人考虑两个欠驱动水下航行器沿着平行平面路径，并且保持一个恒定距离的控制问题，提出了一种leader-follower控制策略。

分布式控制策略相比之前学者研究的集中式控制策略能够减少通信和计算成本，提高系统的控制性能；对于控制系统采用分层结构可以较少系统的优化频率，降低优化次数，而且更加便于实际工程的应用；模型预测算法控制率是一种先进的控制算法，相比其他算法具有显示地处理物理约束和优化轨迹的优势。比如在舵机性能和输入受限的实际情况。

技术实现要素：

要解决的技术问题

为了避免现有技术的不足之处，本发明提出一种基于分层分布式模型预测控制的AUV编队协同控制方法。

技术方案

为了实现AUV编队的协调控制，本发明提供一种AUV编队的分层分布式模型预测控制协同策略。通过分析编队之间的运动学特性和动力学特性，建立上层和下层串联结构的分层控制体系结构：设计分布式模型预测控制算法实现多AUV上层系统的协同控制，设计非线性模型预测控制算法实现AUV下层系统的跟踪控制。最后串联实现在整个AUV编队的协同控制，达到预定多路径规划。

一种基于分层分布式模型预测控制的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)式的终端范围约束。

有益效果

本发明提出的一种基于分层分布式模型预测控制的AUV编队协同控制方法，有益效果为：①提高系统控制的实时性。分层可减少上层系统优化的频率，并进一步减少整个优化次数。②便于工程应用。采用分布式结构和分层的优化框架更加便于实际应用。③两层控制系统均采用模型预测控制算法可处理实际系统的约束。比如在舵机输入受限的实际情况。④优化计算方法，可以在极大程度上提高系统的优化控制性能。

附图说明

图1为分层分布式模型预测控制的流程图

具体实施方式

现结合实施例、附图对本发明作进一步描述：

基于分层分布式模型预测控制算法，结合上层系统和下层系统的控制方案，对于实现编队协同中每个AUV之间的信息交换、优化计算和协同控制策略，我们设计以下算法：

离线部分：首先，在上层系统中，对于第i个AUV系统的运动学方程以周期T_i进行线性离散化。设计上层系统优化目标函数的状态量、控制量和终端状态的影响系数和以及各AUV的协同合作指标项的影响系数设计控制器的终端的约束集合Ω_i；在此设计上层系统的预测域长度和控制域的循环次数为N和N_u(且N≥N_u)；目标协同轨迹参考矢量R_i＝(R_xi,R_yi,R_zi,0,0)；设计每个邻居AUV的对进行优化的预测系数K_j。其次，在下层控制器中，以T_d＝T_u/N_d为周期建立下层系统离散的动力学方程，设计控制器优化目标函数的状态量、控制量和终端状态的影响参数α、ρ和β；在此设计下层系统的预测域长度和控制域的循环次数为N和N_d，(且N≥N_d)；设计控制器终端的约束集合

1)在上层系统在线计算部分，控制域的循环次数为N_u，执行以下过程：

a.在初始时刻，假设编队中每个AUV的初始状态都已经获得，选取第i个AUV的初始状态z_i(k)进行优化；

b.第i个AUV接受相邻j(j≠i)发来当下的预测状态

c.发送自己当下状态的预测值给周围的邻居AUV；

d.求解优化问题J_i(k,z_i,u_i)，求解得到满足约束的最优控制输入序列

e.将最优控制输入值传递给下层控制系统。

2)在下层系统在线计算部分，控制时域的循环次数为N_d，执行以下过程：

a.接受上层系统传递的信息，记作为下层系统在优化过程的参考值；

b.获取下层系统的状态初始值u(l)；

d.求解优化问题J(l,u,w,u_r)，求解得到满足约束的最优控制输入序列w^*(l)；

e.将最优的控制输入值w^*(l|l)，应用到系统控制器；

f.令l＝l+1，循环计算执行N步后，返回上层控制系统。

对分层分布式模型预测控制方法的设计包括以下步骤：

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

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

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

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

在此公式中，A_i为系统矩阵，B_i为系统的控制输入矩阵。

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

具体如下所示，

在上式子(4)中，其中R_i＝(R_xi,R_yi,R_zi,0,0)表示全局空间坐标下控制路径的期望航路点矢量。我们定义在编队中，对每个AUV在某一确定距离的半径范围内，并且可以保证与之互相通信的其余AUV为其邻居。记邻居j AUV的预测状态在上式子(6)中，我们设计K_j为一个可收敛的N*5阶矩阵。和(i,j∈M，且i≠j)表示在空间坐标系下，每个AUV和邻居AUV之间需要协同保证的各个方位的距离信息。公式(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)式终端范围约束。

通过优化计算得到第i个AUV上层系统控制器最优的输入控制量，将其传递给下层系统，作为下层系统整个优化过程的参考轨迹即也就是下层控制器在[l*T,(l+N_d-1)*T]时间区间内的设定值。由于下层系统并无邻居之间的协同关系，因此为了书写方便，对于每个对应优化的AUV，在下层系统优化过程中将去掉每个相关参数的下标，并作此说明。

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

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

6)将下层系统状态方程以采样周期T_d＝T_u/N_d(N_d为下层系统控制域的循环次数)离散化。得到离散的状态方程：

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

在此公式中，A为系统矩阵，B为系统的控制输入矩阵。

7)对上述系统构造二次性能指标：

具体如下所示：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))表示系统二次性能指标中的终端成本函数。

8)求解最优的控制序列：

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)式的终端范围约束。