实施方案
[0032] 以一具体移动机器人为例，基于合作控制机制的机器人滚动优化控制方法的实施步骤如下：
[0033] (一)在第k＝0时刻，初始化实际机器人系统参数，安全距离d＝2m，实际机器人初始位置x(0)＝[0m，0m]T，实际机器人初始速度v(0)＝[0m/s，0m/s]，目标位置G＝[100m，100m]T。
[0034] (二)建立实际机器人与虚拟机器人的动力学模型。具体方法是：
[0035] ①建立实际机器人的动力学模型：
[0036]
[0037] 其中：u(k)是实际机器人在第k时刻的控制信号；x(k)是实际机器人在第k时刻的位置；x(k+1)是实际机器人在第k+1时刻的位置；v(k)是实际机器人在第k时刻的速度；v(k+1)是实际机器人在第k+1时刻的速度。
[0038] ②建立虚拟机器人动力学模型：
[0039]
[0040]
[0041]
[0042] 其中： 是虚拟机器人在第k时刻的位置； 是虚拟机器人在第k时刻的速度；P是2×2转换矩阵；I是2×2单位矩阵；yk是实际机器人检测范围内障碍物边缘上的任意一点位置；ak是过点yk的单位法向量；上标T表示矩阵的转置。从式(2)可以看出，根据实际机器人在第k时刻的位置x(k)和速度v(k)，以及机器人检测到的障碍物边缘的任意一点yk和过该点的法向量ak，可以获得第k时刻虚拟机器人的位置 和速度
[0043] (三)基于合作控制理论，在滚动优化的框架下，建立成本函数，具体方法是：
[0044] ①基于合作控制理论，建立实际机器人和虚拟机器人之间安全距离的成本函数：
[0045]
[0046] 其中：d是预先给定的安全距离； 是第j个虚拟机器人在第k时刻的位置；||·||表示2范数；M是虚拟机器人的个数。在实际机器人的检测范围内，存在M个障碍物，每一个障碍物边缘都存在一个虚拟机器人沿着障碍物边缘运动，也就存在M个虚拟机器人。
[0047] ②基于合作控制理论，建立实际机器人与虚拟机器人速度一致性成本函数：
[0048]
[0049] 其中： 是第j个虚拟机器人在第k时刻的速度。
[0050] ③建立实际机器人与目标位置的成本函数：
[0051] ||x(k)-G|| (5)
[0052] 其中：G是目标位置。
[0053] ④基于式(3)、(4)、(5)，在滚动优化框架下，建立实际机器人运动控制的成本函数：
[0054]
[0055] 其中：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是预测长度。
[0056] (四)基于合作控制机制的滚动优化控制器如下：
[0057]
[0058] 其中： 是实际机器人在第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)最小的最优控制序列
[0059] (五)根据实际要求，在每个采样时间上，根据(7)式计算实际机器人在第k时刻获得的最优控制信号序列 并将第一个控制信号u*(0|k)控制实际机器人，最终控制机器人安全到达目标位置。
