CN108897215B - 基于分布式模型预测控制的多海洋机器人协同环形扫描方法 - Google Patents

Abstract

本发明涉及一种基于分布式模型预测控制的多海洋机器人协同环形扫描方法，属于多海洋机器人控制领域。本发明主要用于多海洋机器人协同环形扫描，其特征为：首先利用实际目标对轨迹上运动虚拟目标的跟踪，实现实际目标对于各自轨迹的跟随。再考虑协同环形扫描过程中虚拟目标之间行进角度的关系，引入协同性能指标，利用分布式模型预测控制算法在线优化得到最优控制序列。本发明利用分布式预测控制算法，大大减少系统间通信压力，同时利用一个控制器解决路径跟随与协同控制两个问题，降低控制系统的设计难度。

Description

基于分布式模型预测控制的多海洋机器人协同环形扫描方法

技术领域

本发明设计属于工业自动控制领域，具体涉及一种基于分布式模型预测控制的多海洋机器人协同环形扫描方法。

背景技术

中国拥有约300万平方公里海洋国土，海洋资源非常丰富。海洋的探测以及海洋资源的开发和利用已经成为建设海洋强国的关键内容。

在海洋探测和绘图过程中，往往需要对某一区域进行高效的扫描搜索，从而确定海域内的资源分布或者采集海底数据。海洋机器人是进行海底区域扫描和搜索的不可替代的工具。但是，单个海洋机器人一般搜索扫描能力有限，不能对大面积海域进行快速高效作业，或者作业时间长。

多海洋机器人协同作业是解决这一问题的有效途径。多海洋机器人协同作业一般包括集中式和分布式模式。但是集中式模式要求一个特殊的协同中心节点与每一个海洋机器人通信，实现成本高，通信负担重，可靠性较差。因此，利用分布式控制方法实现多海洋机器人的协同作业是一种必然趋势和实际选择。

多海洋机器人协同扫描和搜索本质上可以描述成一种分布式协同路径跟踪控制问题。近些年有众多学者对于协同路径跟随问题进行了研究。A.P.Aguiar和A.M.Pascoal在(IEEE Conference on Decision&Control,2007:1473-1479)中针对欠驱动水下机器人通过分别设计路径跟随控制器、协同控制器和基于逻辑的通信系统来解决协同路径跟随问题,但是他们没有考虑协同过程中的优化控制性能，而且对于解决有约束问题不太适用。Alessandro Rucco等在(Developments in Model-Based Optimization and Control,pp.140-160)中将协同路径跟随问题一分为二，运用模型预测控制算法解决路径跟随问题，利用反馈控制算法解决协同问题。但是这种方法将动力学和运动学行为分开来考虑，需要设计两部分控制器，协同控制精度和性能有限，并且不能实现协同环形扫描。

而多海洋机器人环形扫描一般对特定海洋区域扫描效率较高，能有效防止遗漏扫描区域，并且扫描后容易对图像进行拼接，具有重要的工程价值。因此，我们提出一种具有优化特性的多海洋机器人协同环形扫描方法。

发明内容

要解决的技术问题

为了解决多海洋机器人扫描搜索过程中效率问题，提高对于大面积海域的扫描能力，本发明提供一种基于分布式模型预测控制的多海洋机器人环形协同扫描方法。

技术方案

一种基于分布式模型预测控制的多海洋机器人协同环形扫描方法，其特征在于步骤如下：

步骤1：建立系统中每个海洋机器人的误差数学模型：

其中，e_x和e_y表示在地面坐标系下海洋机器人与参考轨迹在x轴和y轴方向的误差，ψ表示机器人运动方向与参考轨迹上虚拟目标运动方向的夹角，v表示海洋机器人运动速度，ω表示海洋机器人角速度，γ表示虚拟目标在轨迹上移动的距离，δ表示参考轨迹的曲率，u₁表示海洋机器人加速度，u₂表示角加速度，u₃表示虚拟目标速度；将上述误差数学模型离散化，去采样时间为T，可得如下离散状态空间模型：

步骤2：建立每个海洋机器人的二次性能指标：

在k时刻，假定预测时域和控制时域同为N；式中x_i(k+i|k)表示系统状态在k时刻对k+i时刻的预测，u_i(k+i|k)表示系统输入控制在k时刻对k+i时刻的预测，Q_i和R_i分别表示系统状态量和控制量的加权矩阵，

和

分别表示状态量和控制量的参考值，

表示对状态量的惩罚，

是对控制量的惩罚，Q_i,R_i,P_i,Q_ij为加权矩阵，σ_i表示曲线的曲率；

通过建立以上的性能指标，则每个海洋机器人对于各自轨迹的跟随转变为解决以下优化问题：

x_i(k+i+1|k)＝f(x_i(k+i|k),u_i(k+i|k)), (4)

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

其中，U_i ^*(k)＝[u_i ^*T(k|k),…,u_i ^*T(k+N-1|k)]^T表示控制输入最优序列，u_min表示控制输入允许的最小值输入量，u_max表示控制输入允许的最大值输入量，式(5)表示控制变量的约束，式(4)为式(2)的简化；

将机器人之间的位置关系信息引入性能指标中，得到如下的航行器协同优化目标：

x_i(k+i+1|k)＝f(x_i(k+i|k),u_i(k+i|k)), (6)

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

其中g_i,j(k)是与邻居节点相关的协同性能指标；

步骤3：建立虚拟目标运动学模型：

其中，x_d和y_d表示虚拟目标在地面坐标系下的坐标，x_d表示虚拟目标的偏航角，σ表示参考轨迹的曲率，

表示虚拟目标速度；

上述模型离散化：

通过式(9)可以得到虚拟目标角度与位移之间的关系，当x_d和γ初始值同为0时，满足：

χ_d(k)＝δγ(k) (10)

根据以上分析我们建立海洋机器人协同性能指标：

其中，

表示为i的邻居节点，Q_ij协同加权矩阵，σ表示曲线曲率，γ表示虚拟目标的位移；

通过在k时刻对于每一海洋机器人i求解(6)和(7)优化问题得到最优控制序列U_i(k)＝[u_i ^T(k|k),…,u_i ^T(k+N-1|k)]^T，将序列的第一个值u_i ^T(k|k)作为输入量作用于系统，其余值用于计算x_i(k+1|k),...,x_i(k+N|k)，将x_i(k+1|k)作为k+1时刻初始值带入上述过程中进行滚动迭代计算。

有益效果

本发明提出的一种基于分布式模型预测控制的多海洋机器人协同环形扫描方法，利用分布式算法，系统间通信量减少，只需要进行局部通信，节约了一定成本；利用同一控制器解决路径跟随与协同控制两个问题，简化了控制系统复杂程度；利用模型预测控制算法可以有效的解决有约束参数优化问题，提高系统的优化协同性能。

附图说明

图1：未引入协同性能指标g_i,j(k)，海洋机器人路径跟随图

图2：引入协同性能指标g_i,j(k)，海洋机器人协同路径跟随图

图3：算法流程图

图4：海洋机器人环形协同运动示意图

具体实施方式

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

本发明涉及的是一种基于分布式模型预测控制的多海洋机器人协同环形扫描方法，具体包括以下步骤：1)分别建立每个海洋机器人与各自参考轨迹的误差数学模型；2)考虑邻居节点信息，分别建立每个海洋机器人的性能指标，在路径跟随的同时保证环形协同扫描可行；3)提出分布式模型预测控制算法，根据性能指标计算出当前时刻最优输入。通过不断迭代优化，使得多目标系统收敛于平衡状态，达到预期目标。

基于分布式模型预测控制的多海洋机器人环形协同扫描方法，包括以下步骤：

首先，建立系统中每个海洋机器人的误差数学模型：

上式中我们引入虚拟目标概念，即参考轨迹上存在虚拟目标移动，我们目的即是使得实际目标跟踪上虚拟目标，从而跟随上参考轨迹。e_x和e_y表示在地面坐标系下海洋机器人与参考轨迹在x轴和y轴方向的误差，ψ表示机器人运动方向与参考轨迹上虚拟目标运动方向的夹角，v表示海洋机器人运动速度，ω表示海洋机器人角速度，γ表示虚拟目标在轨迹上移动的距离，δ表示参考轨迹的曲率，u₁表示海洋机器人加速度，u₂表示角加速度，u₃表示虚拟目标速度。将上述数学模型离散化，去采样时间为T，可得如下离散状态空间模型：

接着，建立每个海洋机器人的二次性能指标：

在k时刻，假定预测时域和控制时域同为N。式中x_i(k+i|k)表示系统状态在k时刻对k+i时刻的预测，u_i(k+i|k)表示系统输入控制在k时刻对k+i时刻的预测，Q_i和R_i分别表示系统状态量和控制量的加权矩阵，

和

分别表示状态量和控制量的参考值，

表示对状态量的惩罚，

是对控制量的惩罚。

通过建立以上的性能指标，则每个海洋机器人对于各自轨迹的跟随转变为解决以下优化问题：

x_i(k+i+1|k)＝f(x_i(k+i|k),u_i(k+i|k)), (4)

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

其中U_i ^*(k)＝[u_i ^*T(k|k),…,u_i ^*T(k+N-1|k)]^T表示控制输入最优序列，u_min表示控制输入允许的最小值输入量，u_max表示控制输入允许的最大值输入量，(5)式表示控制变量的约束，(4)式为(2)式的简化。

利用非线性模型预测控制算法解决上述问题，可以得到最优控制输入，使得每个海洋机器人能够实现跟踪虚拟目标，同时由于虚拟目标在参考轨迹上运动，从而间接的跟随上参考轨迹。但是多海洋机器人的协同显然无法通过上述方法简单实现，每个机器人除了考虑自身的性能指标外，还要考虑与其它目标满足一定的位置关系，也就是说要将这种位置关系信息引入性能指标中。

根据上述的考虑我们得到如下的航行器协同优化目标：

x_i(k+i+1|k)＝f(x_i(k+i|k),u_i(k+i|k)), (6)

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

其中g_i,j(k)是与邻居节点相关的协同性能指标。

最后，提出协同分布式模型预测控制算法：

在基于局部协同的分布式模型预测算法中，考虑到实际情况中通信联系的距离限制，海洋机器人之间的通信联系在局部进行。在上面步骤(2)中已经解决了对于轨迹的跟随问题，同时提出了与邻居节点相关的协同性能指标g_i,j(k)。由于实际航行器跟踪虚拟目标，并最终与虚拟目标同步，所以我们考虑实现虚拟目标之间的协同即可满足海洋机器人之间的协同。在实现同心圆半径方向环形搜索中关键的是实现角度协同，当虚拟目标走过的角度相同，即可实现环形协同。综上所述，协同性能指标g_i,j(k)中所惩罚的量必然与虚拟目标角度相关。

虚拟目标运动学模型：

其中x_d和y_d表示虚拟目标在地面坐标系下的坐标，x_d表示虚拟目标的偏航角，σ表示参考轨迹的曲率，

表示虚拟目标速度。

上述模型离散化：

通过式(9)可以得到虚拟目标角度与位移之间的关系，当x_d和γ初始值同为0时，满足：

χ_d(k)＝δγ(k) (10)

根据以上分析我们建立海洋机器人协同性能指标：

其中

表示为i的邻居节点，Q_ij协同加权矩阵，σ表示曲线曲率，γ表示虚拟目标的位移，则海洋机器人的协同问题转化为求解(6)和(7)的优化问题。

在k时刻，海洋机器人i通过求解优化问题得到控制量γ_i(k|k)，…，γ_i(k+N|k)，注意到γ_i(k|k)为初始已知值，将后面N个控制量信息发送给邻居节点i，作为邻居节点i代价函数中γ_j(k+i|k)的参考值。注意到上述过程中i的邻居节点i参考了i在k时刻预测的k+1至k+N时刻的γ值，而并不是k至k+N-1时刻的值。主要是考虑进行优化仿真时，特别是当多目标系统趋于稳定时，γ_i(k|k)与γ_i(k+1|k)相差并不大，而这样做可以适当简化仿真复杂度。

通过在k时刻对于每一海洋机器人i求解(6)和(7)优化问题得到最优控制序列U_i(k)＝[u_i ^T(k|k),…,u_i ^T(k+N-1|k)]^T，将序列的第一个值u_i ^T(k|k)作为输入量作用于系统，其余值用于计算x_i(k+1|k),...,x_i(k+N|k)，将x_i(k+1|k)作为k+1时刻初始值带入上述过程中进行滚动迭代计算。

本发明构思为：根据实际机器人对于虚拟海洋机器人目标的跟踪完成对于轨迹的跟随，同时根据环形协同中满足的角度关系建立协同性能指标g_i,j(k)，完成机器人在同心圆半径方向协同跟随。

步骤3)中，此方案将引入性能指标J_i(k)和g_i,j(k)，保证多目标系统路径跟随的同时保持环形协同。

式中Q_i,R_i,P_i,Q_ij为加权矩阵，σ_i表示曲线的曲率。

步骤3)中，考虑环形同步运动中所满足的角度关系，通过运动角度的相同实现协同，数学描述如下：

式中

表示i的邻接节点，χ_d(k)表示虚拟目标偏航角，σ_i表示曲线曲率，y_i(k)表示虚拟目标的位移。

具体实施例：

步骤1.根据海洋机器人误差数学模型，建立离散的非线性状态空间表达式：

上式中离散周期T取0.1s，本例中海洋机器人个数选取三个，相互之间都可以保持通信，相应的参考轨迹曲率分别为δ₁＝1，δ₂＝1/2，δ₃＝1/3。

步骤2.建立目标函数：

其中初始值

参考值

Q₁＝diag([8,12,25,1,0.01,0]),Q₂＝diag([8,8,20,1,0.01,0]),Q₃＝diag([10,8,30,1,0.01,0])

P₁＝diag([8,12,25,1,0.01,0]),P₂＝diag([8,8,20,1,0.01,0]),P₃＝diag([10,8,30,1,0.01,0])

R₁＝diag([0.01,0.01,5]),R₂＝diag([0.02,0.02,5]),R₃＝diag([0.02,0.02,9])

Q₁₂＝Q₁₃＝10,Q₂₁＝Q₂₃＝12,Q₃₁＝Q₃₂＝14

协同指标中γ_j初始值设为0

控制变量满足以下约束条件：

-5<u₁<5,-0.3*pi<u₂<0.3*pi，0<u₃

步骤3.求解最优控制序列：

→最优的控制序列u_i*(k)

假定预测时域N＝6,则可通过目标函数求得每次最优控制序列u_i*(k),记为u_i*(k)＝[u(k|k),…,u(k+5|k)]^T,计算得状态估计值x_i(k+1|k),...,x_i(k+6|k)，将其传送给其它海洋机器人。从上述状态参数中可以得到γ_i(k+1|k),...,γ_i(k+6|k)值，将其与曲率半径σ_j乘积σ_jγ_j作为协同性能指标g_i,j(k)中参考值。同时将最优控制序列u_i*(k)第一个解保留，作为下一时刻滚动迭代初始值。

步骤4.根据步骤3.计算其它海洋机器人的最优控制序列u_i*(k)并保留第一个解，同时传递状态估计值中γ_i(k+1|k),...,γ_i(k+6|k)值。本次仿真实验中海洋机器人个数为三，即经过三次循环后，完成k时刻海洋机器人系统的优化计算。

步骤5.在k+1时刻，将保留的最优控制序列u_i*(k)优化结果带入步骤1.中重复上述过程，进行滚动迭代计算。

Claims (1)

1.一种基于分布式模型预测控制的多海洋机器人协同环形扫描方法，其特征在于步骤如下：

步骤1：建立系统中每个海洋机器人的误差数学模型：

其中，e_x和e_y表示在地面坐标系下海洋机器人与参考轨迹在x轴和y轴方向的误差，ψ表示机器人运动方向与参考轨迹上虚拟目标运动方向的夹角，v表示海洋机器人运动速度，ω表示海洋机器人角速度，γ表示虚拟目标在轨迹上移动的距离，δ表示参考轨迹的曲率，u₁表示海洋机器人加速度，u₂表示角加速度，u₃表示虚拟目标速度；将上述误差数学模型离散化，去采样时间为T，可得如下离散状态空间模型：

步骤2：建立每个海洋机器人的二次性能指标：

在k时刻，假定预测时域和控制时域同为N；式中x_i(k+i|k)表示系统状态在k时刻对k+i时刻的预测，u_i(k+i|k)表示系统输入控制在k时刻对k+i时刻的预测，Q_i和R_i分别表示系统状态量和控制量的加权矩阵，

和

分别表示状态量和控制量的参考值，

表示对状态量的惩罚，

是对控制量的惩罚，Q_i,R_i,P_i,Q_ij为加权矩阵，σ_i表示曲线的曲率；

通过建立以上的性能指标，则每个海洋机器人对于各自轨迹的跟随转变为解决以下优化问题：

x_i(k+i+1|k)＝f(x_i(k+i|k),u_i(k+i|k)), (4)

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

其中，U_i ^*(k)＝[u_i ^*T(k|k),…,u_i ^*T(k+N-1|k)]^T表示控制输入最优序列，u_min表示控制输入允许的最小值输入量，u_max表示控制输入允许的最大值输入量，式(5)表示控制变量的约束，式(4)为式(2)的简化；

将机器人之间的位置关系信息引入性能指标中，得到如下的航行器协同优化目标：

x_i(k+i+1|k)＝f(x_i(k+i|k),u_i(k+i|k)), (6)

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

其中g_i,j(k)是与邻居节点相关的协同性能指标；

步骤3：建立虚拟目标运动学模型：

其中，x_d和y_d表示虚拟目标在地面坐标系下的坐标，x_d表示虚拟目标的偏航角，σ表示参考轨迹的曲率，

表示虚拟目标速度；

上述模型离散化：

通过式(9)可以得到虚拟目标角度与位移之间的关系，当χ_d和γ初始值同为0时，满足：

χ_d(k)＝δγ(k) (10)

根据以上分析我们建立海洋机器人协同性能指标：

其中，

表示为i的邻居节点，Q_ij协同加权矩阵，σ表示曲线曲率，γ表示虚拟目标的位移；

通过在k时刻对于每一海洋机器人i求解(6)和(7)优化问题得到最优控制序列U_i(k)＝[u_i ^T(k|k),…,u_i ^T(k+N-1|k)]^T，将序列的第一个值u_i ^T(k|k)作为输入量作用于系统，其余值用于计算x_i(k+1|k),...,x_i(k+N|k)，将x_i(k+1|k)作为k+1时刻初始值带入上述过程中进行滚动迭代计算。

Images