一种基于合作控制机制的机器人滚动优化控制方法与流程

本发明属于自动化技术领域，涉及一种基于合作控制机制的机器人滚动优化控制方法。

背景技术：

路径规划问题是机器人领域中的基础问题之一，相应的机器人路径规划技术则成为机器人运动控制中的一项基础技术。该项技术主要是控制机器人能够安全有效地从初始位置到达指定的目标位置。根据环境信息是否已知，可分为全局路径规划技术和局部路径规划技术，特别是局部路径规划技术，由于涉及到了未知动态环境，使得该技术一直以来受到全世界的强烈关注。传统的局部路径规划技术将“规划”和“控制”分开，也就是说，先完成机器人感知范围内的路径设定，然后采用轨迹跟踪控制方法控制机器人沿着路径运动。但此类技术计算相对复杂，不能有效控制机器人工作在动态不确定环境中，使得机器人可能会“陷入”环境中非目标位置，在此背景下，本发明弥补了此技术的不足之处。

技术实现要素：

本发明的目标是针对现有技术的不足之处，提出一种基于合作控制机制的机器人滚动优化控制方法，具体是引入虚拟机器人，使得该虚拟机器人可以沿着障碍物的边缘运动；在此基础上，基于合作控制理论，在滚动优化的框架下，建立成本函数；求解成本函数，获得最优控制序列，仅使用最优控制序列中第一个控制信号控制机器人运动。该方法能够在每一次的采样过程中求解控制序列，并仅使用第一个控制信号，这样就能有效的控制机器人在动态不确定环境中运动，并降低传统方法运算的复杂性。

本发明方法的步骤包括：

(一)建立实际机器人与虚拟机器人的动力学模型。具体方法是：

①建立实际机器人的动力学模型：

其中：u(k)是实际机器人在第k时刻的控制信号；x(k)是实际机器人在第k时刻的位置；x(k+1)是实际机器人在第k+1时刻的位置；v(k)是实际机器人在第k时刻的速度；v(k+1)是实际机器人在第k+1时刻的速度。

②建立虚拟机器人动力学模型：

其中：是虚拟机器人在第k时刻的位置；是虚拟机器人在第k时刻的速度；p是2×2转换矩阵；i是2×2单位矩阵；yk是实际机器人检测范围内障碍物边缘上的任意一点位置；ak是过点yk的单位法向量；上标t表示矩阵的转置。

(二)基于合作控制理论，在滚动优化的框架下，建立成本函数，具体方法是：

①基于合作控制理论，建立实际机器人和虚拟机器人之间安全距离的成本函数：

其中：d是预先给定的安全距离；是第j个虚拟机器人在第k时刻的位置；||·||表示2范数；m是虚拟机器人的个数。

②基于合作控制理论，建立实际机器人与虚拟机器人速度一致性成本函数：

其中：是第j个虚拟机器人在第k时刻的速度。

③建立实际机器人与目标位置的成本函数：

||x(k)-g||(5)

其中：g是目标位置。

④基于式(3)、(4)、(5)，在滚动优化框架下，建立实际机器人运动控制的成本函数：

其中：x(l|k)是实际机器人在第k时刻预测的第k+l时刻的位置；是第j个虚拟机器人在第k时刻预测的第k+l时刻的位置；v(l|k)是实际机器人在第k时刻预测的第k+l时刻的速度；是第j个虚拟机器人在第k时刻预测的第k+l时刻的速度；x(n|k)是实际机器人在第k时刻预测的第k+n时刻的位置；n是预测长度。

(三)基于合作控制机制的滚动优化控制器结构如下：

其中：是第k时刻获得的最优控制信号序列；u^*(0|k)表示实际机器人在第k时刻获得的最优控制信号；u^*(n-1|k)表示实际机器人在第k时刻获得的k+n-1时刻的最优控制信号；u(0|k)表示实际机器人在第k时刻预测的控制信号；u(n-1|k)表示实际机器人在第k时刻预测的k+n-1时刻的控制信号；min表示取最小值；s.t.表示以下为约束条件。

(四)将实际机器人在k时刻获得的最优的控制序列中的第一个最优控制信号u^*(0|k)施加给实际机器人。

本发明提出的基于合作控制机制的机器人滚动优化控制方法弥补了传统的局部路径规划技术的不足,能够控制机器人安全有效地运行在动态不确定的环境中，避免了机器人“陷入”环境中非目标位置。

具体实施方式

以一具体移动机器人为例，基于合作控制机制的机器人滚动优化控制方法的实施步骤如下：

(一)在第k＝0时刻，初始化实际机器人系统参数，安全距离d＝2m，实际机器人初始位置x(0)＝[0m，0m]^t，实际机器人初始速度v(0)＝[0m/s，0m/s]，目标位置g＝[100m，100m]^t。

(二)建立实际机器人与虚拟机器人的动力学模型。具体方法是：

①建立实际机器人的动力学模型：

其中：u(k)是实际机器人在第k时刻的控制信号；x(k)是实际机器人在第k时刻的位置；x(k+1)是实际机器人在第k+1时刻的位置；v(k)是实际机器人在第k时刻的速度；v(k+1)是实际机器人在第k+1时刻的速度。

②建立虚拟机器人动力学模型：

其中：是虚拟机器人在第k时刻的位置；是虚拟机器人在第k时刻的速度；p是2×2转换矩阵；i是2×2单位矩阵；yk是实际机器人检测范围内障碍物边缘上的任意一点位置；ak是过点yk的单位法向量；上标t表示矩阵的转置。从式(2)可以看出，根据实际机器人在第k时刻的位置x(k)和速度v(k)，以及机器人检测到的障碍物边缘的任意一点yk和过该点的法向量ak，可以获得第k时刻虚拟机器人的位置和速度

(三)基于合作控制理论，在滚动优化的框架下，建立成本函数，具体方法是：

①基于合作控制理论，建立实际机器人和虚拟机器人之间安全距离的成本函数：

其中：d是预先给定的安全距离；是第j个虚拟机器人在第k时刻的位置；||·||表示2范数；m是虚拟机器人的个数。在实际机器人的检测范围内，存在m个障碍物，每一个障碍物边缘都存在一个虚拟机器人沿着障碍物边缘运动，也就存在m个虚拟机器人。

②基于合作控制理论，建立实际机器人与虚拟机器人速度一致性成本函数：

其中：是第j个虚拟机器人在第k时刻的速度。

③建立实际机器人与目标位置的成本函数：

||x(k)-g||(5)

其中：g是目标位置。

④基于式(3)、(4)、(5)，在滚动优化框架下，建立实际机器人运动控制的成本函数：

其中：x(l|k)是实际机器人在第k时刻预测的第k+l时刻的位置；是第j个虚拟机器人在第k时刻预测的第k+l时刻的位置；υ(l|k)是实际机器人在第k时刻预测的第k+l时刻的速度；是第j个虚拟机器人在第k时刻预测的第k+l时刻的速度；x(n|k)是实际机器人在第k时刻预测的第k+n时刻的位置；n是预测长度。

(四)基于合作控制机制的滚动优化控制器如下：

其中：是实际机器人在第k时刻获得的最优控制信号序列；u^*(0|k)表示实际机器人在第k时刻获得的最优控制信号；u^*(n-1|k)表示实际机器人在第k时刻获得的k+n-1时刻的最优控制信号；u(0|k)表示实际机器人在第k时刻预测的控制信号；u(n-1|k)表示实际机器人在第k时刻预测的k+n-1时刻的控制信号；min表示取最小值；s.t.表示以下为约束条件。对于每一个在第k时刻预测的控制信号序列u(0|k)，...，u(n-1|k)，实际机器人预测的位置和速度可以采用式(1)计算，相应的虚拟机器人预测的位置和速度可以采用式(2)计算，因此可以计算实际机器人运动控制成本函数(6)。执行式(7)获得使实际机器人运动控制成本函数(6)最小的最优控制序列

(五)根据实际要求，在每个采样时间上，根据(7)式计算实际机器人在第k时刻获得的最优控制信号序列并将第一个控制信号u^*(0|k)控制实际机器人，最终控制机器人安全到达目标位置。