CN104865826A - 一种基于合作预测控制的多机器人环境监测方法 - Google Patents

Abstract

本发明涉及一种基于合作预测控制的多机器人环境监测方法。本发明采用径向基函数网络建立环境质量参数模型，对于每一个机器人，在每一时刻，都可以使用自己和他的邻居机器人通过网络传输过来的新的质量参数数据来更新建立的环境质量参数模型；再在环境质量参数模型的基础上，建立环境质量参数优化问题，采用预测控制方法，获得机器人理想的最优运动轨迹；最后建立多机器人合作控制优化问题，基于机器人的理想的最优运动轨迹，采用预测控制方法，产生机器人实际最优的控制序列，并采用实际最优控制序列中第一个控制输入到机器人，控制机器人的运动。本发明在保证多机器人追踪环境质量参数最优值的同时，可以保持多机器人运动避碰以及速度相差有界。

Description

一种基于合作预测控制的多机器人环境监测方法

技术领域

本发明属于自动化技术领域，涉及一种面向环境质量参数监测的多机器人合作控制方法。

背景技术

环境质量对人类安全有着非常重要的意义，如海洋环境中的盐分浓度分布、Ph值分布、温度分布，陆地上的有毒气体浓度分布等等。这些环境参数能够很好地表征环境的质量特征，因此，统称为环境质量参数。如何快速有效地追踪环境质量参数中的最优值，是建立环境质量参数分布模型的一个极其重要的问题。然而，环境质量参数的分布在不同的环境下，呈现出不同的特点。通常的情况，可以采用梯度的方法控制多机器人***追踪环境质量参数的最优值，从而建立合适的环境质量参数分布模型。然而，实际的环境中，由于洋流、温度、风力等不同因素的影响，使得环境质量参数分布极其复杂，采用梯度的方法由于不确定因素的存在，使得多机器人***建立的环境质量参数模型仅具有局部特征。在这一背景下，本发明弥补了现有技术的不足。

发明内容

本发明的目标是针对现有技术的不足之处，提供了一种有效地环境质量参数监测方法，使得建立的环境质量参数模型具有全局特征。首先采用径向基函数网络建立环境质量参数模型，对于每一个机器人，在每一时刻，都可以使用自己和他的邻居机器人通过网络传输过来的新的质量参数数据来更新建立的环境质量参数模型；然后，在环境质量参数模型的基础上，建立环境质量参数优化问题，采用预测控制方法，获得机器人理想的最优运动轨迹；最后，建立多机器人合作控制优化问题，基于机器人的理想的最优运动轨迹，采用预测控制方法，产生机器人实际最优的控制序列，并采用实际最优控制序列中第一个控制输入到机器人，控制机器人的运动。本发明弥补了传统控制的不足，设计的合作预测控制的环境监测方法，在保证多机器人追踪环境质量参数最优值的同时，可以保持多机器人运动避碰以及速度之间的协调。本发明采用的控制方法可以有效地保证多机器人更好地监测环境质量参数，从而更准确地建立环境质量参数模型。

本发明方法的步骤包括：

第一步：采用径向基函数网络建立环境质量参数模型，具体步骤如下：

a、对于第i个机器人的环境质量参数模型f_i(x)，即径向基函数网络如(1)式所示，其中：i＝1，2，...，n，n是机器人的数量；

$f_i\left(x\right)=\underset{\mathrm{kk}=1}{\overset{m}{\mathrm{\Σ}}}{\mathrm{\α}}_{\mathrm{kk}}{p}_{\mathrm{kk}}\left(x\right)---\left(1\right)$

其中：α_kk是径向基函数的权重；m是径向基函数的个数；x是机器人的位置；f_i(x)表示在机器人在位置x，径向基函数网络输出的环境质量参数预测值；p_kk(x)是第kk个径向基函数，具体如(2)式所示。

$p_{\mathrm{kk}}\left(x\right)=\frac{1}{\mathrm{\γ}}\exp \left(\frac{-{\left|\right|x-u_{\mathrm{kk}}\left|\right|}^2}{2{\mathrm{\σ}}_{\mathrm{kk}}^2}\right)---\left(2\right)$

其中：γ是归一化常数；u_kk是径向基函数中心；σ_kk是径向基函数的宽度；exp(·)是指数函数；||·||表示2范数。

b、对于第i个机器人的所有邻居，即可以和第i个机器人通信的机器人集合用表示，那么第i个机器人的环境质量参数模型中径向基函数的权重α_kk(kk＝1，2，...，m)根据(3)式更新。

其中：min表示取最小值；|·|表示绝对值；由于第j个机器人在第i个机器人的邻域内，即因此，能和第i个机器人通信，可以将第j个机器人的位置x_j和环境实际质量参数数值z(x_j)(z(x_j)表示第j个机器人在位置x_j的环境质量参数实际数值)发送给第i个机器人；f_i(x_j)表示第i个机器人的环境质量参数模型对第j个机器人的位置x_j上的环境质量参数预测值；(3)式的目的是找到环境质量参数模型权重的一组值使得环境质量参数模型的预测值和环境质量参数实际数值的均方误差最小。

第二步：基于环境质量参数模型，建立环境质量参数优化问题，采用预测控制方法产生机器人的理想的最优运动轨迹，具体步骤如下：

a、在环境质量参数模型的基础上，建立的环境质量参数优化问题如下：

$s.t.:\left\{\begin{array}{c}{\hat{x}}_i\left(l\right|k)={\hat{x}}_i(l-1|k)+{\hat{v}}_i(l-1|k)\\ {\hat{v}}_i\left(l\right|k)={\hat{v}}_i(l-1|k)+{\hat{u}}_i\left(l\right|k)\end{array}\right.---\left(4\right)$

其中：max表示取最大值；表示第i个机器人在k时刻预测的位置序列N是预测的长度；是关于位置序列的成本函数；s.t.表示位置序列应满足的条件，这个条件就是机器人的动力学方程；是第i个机器人在k时刻的位置；是第i个机器人在k时刻的速度；是第i个机器人在k时刻预测的在第k+l时刻的位置；是第i个机器人在k时刻预测的在第k+l时刻的速度。是第i个机器人在k时刻需要求得的在第k+l时刻的控制输入；表示第i个机器人在k时刻需要求得的控制序列

b、求解环境质量参数优化问题(4)，获得理想的最优预测控制序列，即：

$s.t.:\left\{\begin{array}{c}{\hat{x}}_i\left(l\right|k)={\hat{x}}_i(l-1|k)+{\hat{v}}_i(l-1|k)\\ {\hat{v}}_i\left(l\right|k)={\hat{v}}_i(l-1|k)+{\hat{u}}_i\left(l\right|k)\end{array}\right.---\left(5\right)$

其中：表示第i个机器人在k时刻获得的理想的最优控制序列对应于该理想的最优控制序列，我们可以得到第i个机器人在k时刻理想的最优位置序列和理想的最优速度序列如果用(上标T表示矩阵的转置)表示在k时刻获得的在第k+l时刻的最优状态，那么对应于理想的最优控制序列，我们可以将理想的最优位置序列和理想的最优速度序列表示成理想的最优运动轨迹(状态序列)将第i个机器人在k时刻获得最优运动轨迹发给他的邻居，同时，获得他的邻居发给他的最优运动轨迹

第三步：建立多机器人合作控制优化问题，基于理想的最优运动轨迹，获得实际的最优控制序列，并产生实际的最优运动轨迹，具体步骤如下：

a、建立多机器人合作能量函数，如(6)式所示。

其中：d表示相邻机器人之间的理想距离；s_i(l|k)＝[x_i(l|k)，v_i(l|k)]^T；V_i(s_i(l|k))表示第i个机器人在状态s_i(l|k)时的能量；能量函数用来约束多机器人***之间的距离和速度，即在理想的情况下，相邻机器人之间的距离为d；并且速度相等 $v_i\left(l\right|k)={\hat{v}}_j^{*}\left(l\right|k).$

b、建立多机器人合作控制优化问题，如(7)式所示。

$\underset{u_i\left(k\right)}{\min }\underset{l=1}{\overset{N}{\mathrm{\Σ}}}{V}_i\left(s_i\left(l\right|k)\right)$

$s.t.:\left\{\begin{array}{c}{x}_i\left(l\right|k)=x_i(l-1|k)+v_i(l-1|k)\\ v_i\left(l\right|k)=v_i(l-1|k)+u_i\left(l\right|k)\\ \left|\right|s_i\left(l\right|k)-{\hat{s}}_i^{*}\left(l\right|k)\left|\right|<\mathrm{\&gamma;}\end{array}\right.---\left(7\right)$

其中：γ是距离约束参数用来约束产生的实际运动轨迹s_i(1|k)，...，s_i(N|k)和理想的运动轨迹之间的距离。优化的目的是要求产生的实际运动轨迹在接近理想运动轨迹的情况下，即保持多机器人***搜索环境质量参数最优数值的情况下，使得多机器人避免碰撞并速度相差有界；u_i(k)表示第i个机器人在k时刻需要求得的控制序列u_i(1|k)，...，u_i(N|k)。

c、求解多机器人合作控制优化问题(7)，获得实际的最优预测控制序列。

$u_i^{*}\left(k\right)=\arg \underset{u_i\left(k\right)}{\min }\underset{l=1}{\overset{N}{\mathrm{\&Sigma;}}}{V}_i\left(s_i\left(l\right|k)\right)$

$s.t.:\left\{\begin{array}{c}{x}_i\left(l\right|k)=x_i(l-1|k)+v_i(l-1|k)\\ v_i\left(l\right|k)=v_i(l-1|k)+u_i\left(l\right|k)\\ \left|\right|s_i\left(l\right|k)-{\hat{s}}_i^{*}\left(l\right|k)\left|\right|<\mathrm{\&gamma;}\end{array}\right.---\left(8\right)$

其中：表示第i个机器人在k时刻获得的实际的最优控制序列

第四步：将第i个机器人在k时刻获得的实际的最优控制序列中的第一个最优控制施加给第i个机器人。

第五步：如果终止条件满足，例如：给定的最大搜索时间已经达到，则第i个机器人停止运行，并将他的环境模型f_i(x)输出；如果终止条件没有满足，则返回第一步继续执行。

本发明提出的一种基于合作预测控制的多机器人环境监测方法，该方法弥补了传统方法的不足，能够有效地协调多机器人***追踪环境质量参数的最优值，并能够建立具有全局特征的环境质量参数模型。

具体实施方式

以海洋环境盐分浓度分布为例，设定需监测的环境范围长为200米，宽为200米，建立坐标***，环境监测范围可表示成[-100，100]×[-100，100]。采用10个机器人(n＝10)合作采集环境的盐分浓度，建立盐分浓度分布模型。

对于机器人群体中的第i个机器人的具体实施步骤如下

第一步，初始化环境质量参数模型的参数，包括：径向基函数个数m为20；径向基函数中心u_kk(kk＝1，2，...，m)在[-100，100]×[-100，100]内均匀分布；径向基函数宽度σ_kk在[80，130]内均匀分布；γ为1；径向基函数的初始权重在[1，70]内均匀分布。初始化机器人的参数，包括：机器人的初始位置，机器人的初始速度设为0，最大速度限制则根据实际使用的机器人类型设定，控制的输入也是根据实际使用的机器人类型设定，预测长度N为6，理想的机器人间距离d为10米，机器人的通信范围为20米，距离约束参数γ为2米

第二步：采用径向基函数网络建立环境质量参数模型，具体步骤如下：

a、对于第i个机器人的环境质量参数模型，即径向基函数网络如(1)式所示。

$f_i\left(x\right)=\underset{\mathrm{kk}=1}{\overset{m}{\mathrm{\&Sigma;}}}{\mathrm{\&alpha;}}_{\mathrm{kk}}{p}_{\mathrm{kk}}\left(x\right)---\left(1\right)$

其中：α_kk是径向基函数的权重；m是径向基函数的个数；x是机器人的位置；f_i(x)表示在机器人位置x，径向基函数网络输出的环境质量参数预测值；p_kk(x)是第kk个径向基函数，具体如(2)式所示。

$p_{\mathrm{kk}}\left(x\right)=\frac{1}{\mathrm{\&gamma;}}\exp \left(\frac{-{\left|\right|x-u_{\mathrm{kk}}\left|\right|}^2}{2{\mathrm{\&sigma;}}_{\mathrm{kk}}^2}\right)---\left(2\right)$

其中：γ是归一化常数；u_kk是径向基函数中心；σ_kk是径向基函数的宽度；exp(·)是指数函数；||·||表示2范数。

b、对于第i个机器人的所有邻居，即可以和第i个机器人通信的机器人集合用表示，那么第i个机器人的环境质量参数模型中径向基函数的权重α_kk(kk＝1，2，...，m)根据(3)式更新。

其中：min表示取最小值；|·|表示绝对值；由于第j个机器人在第i个机器人的邻域内，即因此，能和第i个机器人通信，可以将第j个机器人的位置x_j和环境实际质量参数数值z(x_j)(z(x_j)表示第j个机器人在位置x_j的环境质量参数实际数值)发送给第i个机器人；f_i(x_j)表示第i个机器人的环境质量参数模型对第j个机器人的位置x_j上的环境质量参数预测值；(3)式的目的是找到环境质量参数模型权重的一组值使得环境质量参数模型的预测值和环境质量参数实际数值的均方误差最小。

第三步：基于环境质量参数模型，建立环境质量参数优化问题，采用预测控制方法产生机器人的理想的最优运动轨迹，具体步骤如下：

a.在环境质量参数模型的基础上，建立的环境质量参数优化问题如下：

$s.t.:\left\{\begin{array}{c}{\hat{x}}_i\left(l\right|k)={\hat{x}}_i(l-1|k)+{\hat{v}}_i(l-1|k)\\ {\hat{v}}_i\left(l\right|k)={\hat{v}}_i(l-1|k)+{\hat{u}}_i\left(l\right|k)\end{array}\right.---\left(4\right)$

其中：max表示取最大值；表示第i个机器人在k时刻预测的位置序列N是预测的长度；是关于位置序列的成本函数；s.t.表示位置序列应满足的条件，这个条件就是机器人的动力学方程；是第i个机器人在k时刻的位置；是第i个机器人在k时刻的速度；是第i个机器人在k时刻预测的在第k+l时刻的位置；是第i个机器人在k时刻预测的在第k+l时刻的速度。是第i个机器人在k时刻需要求得的在第k+l时刻的控制输入；表示第i个机器人在k时刻需要求得的控制序列

b.求解环境质量参数优化问题(4)，获得理想的最优预测控制序列，即：

$s.t.:\left\{\begin{array}{c}{\hat{x}}_i\left(l\right|k)={\hat{x}}_i(l-1|k)+{\hat{v}}_i(l-1|k)\\ {\hat{v}}_i\left(l\right|k)={\hat{v}}_i(l-1|k)+{\hat{u}}_i\left(l\right|k)\end{array}\right.---\left(5\right)$

其中：表示第i个机器人在k时刻获得的理想的最优控制序列对应于该理想的最优控制序列，我们可以得到第i个机器人在k时刻理想的最优位置序列和理想的最优速度序列如果用(上标T表示矩阵的转置)表示在k时刻获得的在第k+l时刻的最优状态，那么对应于理想的最优控制序列，我们可以将理想的最优位置序列和理想的最优速度序列表示成理想的最优运动轨迹(状态序列)将第i个机器人在k时刻获得最优运动轨迹发给他的邻居，同时，获得他的邻居发给他的最优运动轨迹

第四步：建立多机器人合作控制优化问题，基于理想的最优运动轨迹，获得实际的最优控制序列，并产生实际的最优运动轨迹，具体步骤如下：

a.建立多机器人合作能量函数，如(6)式所示。

其中：d表示相邻机器人之间的理想距离；s_i(l|k)＝[x_i(l|k)，v_i(l|k)]^T；V_i(s_i(l|k))表示第i个机器人在状态s_i(l|k)时的能量；能量函数用来约束多机器人***之间的距离和速度，即在理想的情况下，相邻机器人之间的距离为d；并且速度相等

b.建立多机器人合作控制优化问题，如(7)式所示。

$\underset{u_i\left(k\right)}{\min }\underset{l=1}{\overset{N}{\mathrm{\&Sigma;}}}{V}_i\left(s_i\left(l\right|k)\right)$

$s.t.:\left\{\begin{array}{c}{x}_i\left(l\right|k)=x_i(l-1|k)+v_i(l-1|k)\\ v_i\left(l\right|k)=v_i(l-1|k)+u_i\left(l\right|k)\\ \left|\right|s_i\left(l\right|k)-{\hat{s}}_i^{*}\left(l\right|k)\left|\right|<\mathrm{\&gamma;}\end{array}\right.---\left(7\right)$

其中：γ是距离约束参数用来约束产生的实际运动轨迹s_i(1|k)，...，s_i(N|k)和理想的运动轨迹之间的距离。优化的目的是要求产生的实际运动轨迹在接近理想运动轨迹的情况下，即保持多机器人***搜索环境质量参数最优数值的情况下，使得多机器人避免碰撞并速度相差有界；ui(k)表示第i个机器人在k时刻需要求得的控制序列u_i(1|k)，...，u_i(N|k)。

c.求解多机器人合作控制优化问题(7)，获得实际的最优预测控制序列。

$u_i^{*}\left(k\right)=\arg \underset{u_i\left(k\right)}{\min }\underset{l=1}{\overset{N}{\mathrm{\&Sigma;}}}{V}_i\left(s_i\left(l\right|k)\right)$

$s.t.:\left\{\begin{array}{c}{x}_i\left(l\right|k)=x_i(l-1|k)+v_i(l-1|k)\\ v_i\left(l\right|k)=v_i(l-1|k)+u_i\left(l\right|k)\\ \left|\right|s_i\left(l\right|k)-{\hat{s}}_i^{*}\left(l\right|k)\left|\right|<\mathrm{\&gamma;}\end{array}\right.---\left(8\right)$

其中：表示第i个机器人在k时刻获得的实际的最优控制序列

第五步：将第i个机器人在k时刻获得的实际的最优控制序列中的第一个最优控制施加给第i个机器人。

第六步：如果终止条件满足，例如：给定的最大搜索时间已经达到，则第i个机器人停止运行，并将他的环境模型f_i(x)输出；如果终止条件没有满足，则返回第二步继续执行。

Claims (1)

1.一种基于合作预测控制的多机器人环境监测方法，其特征在于，该方法具体包括以下步骤：

第一步：采用径向基函数网络建立环境质量参数模型，具体步骤如下：

a、对于第i个机器人的环境质量参数模型f_i(x)，即径向基函数网络如(1)式所示，其中：i＝1，2，...，n，n是机器人的数量；

$f_i\left(x\right)=\underset{\mathrm{kk}=1}{\overset{m}{\mathrm{\&Sigma;}}}{\mathrm{\&alpha;}}_{\mathrm{kk}}{p}_{\mathrm{kk}}\left(x\right)---\left(1\right)$

其中：α_kk是径向基函数的权重；m是径向基函数的个数；x是机器人的位置；f_i(x)表示在机器人在位置x，径向基函数网络输出的环境质量参数预测值；p_kk(x)是第kk个径向基函数，具体如(2)式所示；

$p_{\mathrm{kk}}\left(x\right)=\frac{1}{\mathrm{\&gamma;}}\exp \left(\frac{-{\left|\right|x-u_{\mathrm{kk}}\left|\right|}^2}{2{\mathrm{\&sigma;}}_{\mathrm{kk}}^2}\right)---\left(2\right)$

其中：γ是归一化常数；u_kk是径向基函数中心；σ_kk是径向基函数的宽度；exp(·)是指数函数；||·||表示2范数；

b、对于第i个机器人的所有邻居，即可以和第i个机器人通信的机器人集合用表示，那么第i个机器人的环境质量参数模型中径向基函数的权重α_kk(kk＝1，2，...，m)根据(3)式更新；

其中：min表示取最小值；|·|表示绝对值；由于第j个机器人在第i个机器人的邻域内，即因此，能和第i个机器人通信，可以将第j个机器人的位置x_j和环境实际质量参数数值z(x_j)发送给第i个机器人；z(x_j)表示第j个机器人在位置x_j的环境质量参数实际数值，f_i(x_j)表示第i个机器人的环境质量参数模型对第j个机器人的位置x_j上的环境质量参数预测值；

第二步：基于环境质量参数模型，建立环境质量参数优化问题，采用预测控制方法产生机器人的理想的最优运动轨迹，具体步骤如下：

a、在环境质量参数模型的基础上，建立的环境质量参数优化问题如下：

$s.t.:\left\{\begin{array}{c}{\hat{x}}_i\left(l\right|k)={\hat{x}}_i(l-1|k)+{\hat{v}}_i(l-1|k)\\ {\hat{v}}_i\left(l\right|k)={\hat{v}}_i(l-1|k)+{\hat{u}}_i\left(l\right|k)\end{array}\right.---\left(4\right)$

其中：max表示取最大值；表示第i个机器人在k时刻预测的位置序列N是预测的长度；是关于位置序列的成本函数；s.t.表示位置序列应满足的条件，这个条件就是机器人的动力学方程；是第i个机器人在k时刻的位置；是第i个机器人在k时刻的速度；是第i个机器人在k时刻预测的在第k+l时刻的位置；是第i个机器人在k时刻预测的在第k+l时刻的速度；是第i个机器人在k时刻需要求得的在第k+l时刻的控制输入；表示第i个机器人在k时刻需要求得的控制序列 ${\hat{u}}_i\left(l\right|k),...,{\hat{u}}_i\left(N\right|k);$

b、求解环境质量参数优化问题(4)，获得理想的最优预测控制序列，即：

$s.t.:\left\{\begin{array}{c}{\hat{x}}_i\left(l\right|k)={\hat{x}}_i(l-1|k)+{\hat{v}}_i(l-1|k)\\ {\hat{v}}_i\left(l\right|k)={\hat{v}}_i(l-1|k)+{\hat{u}}_i\left(l\right|k)\end{array}\right.---\left(5\right)$

其中：表示第i个机器人在k时刻获得的理想的最优控制序列对应于该理想的最优控制序列，我们可以得到第i个机器人在k时刻理想的最优位置序列和理想的最优速度序列如果用上标T表示矩阵的转置，表示在k时刻获得的在第k+l时刻的最优状态，那么对应于理想的最优控制序列，我们可以将理想的最优位置序列和理想的最优速度序列表示成理想的最优运动轨迹将第i个机器人在k时刻获得最优运动轨迹发给他的邻居，同时，获得他的邻居发给他的最优运动轨迹

第三步：建立多机器人合作控制优化问题，基于理想的最优运动轨迹，获得实际的最优控制序列，并产生实际的最优运动轨迹，具体步骤如下：

a、建立多机器人合作能量函数，如(6)式所示；

其中：d表示相邻机器人之间的理想距离；s_i(l|k)＝[x_i(l|k)，v_i(l|k)]^T；V_i(s_i(l|k))表示第i个机器人在状态s_i(l|k)时的能量；能量函数用来约束多机器人***之间的距离和速度，即在理想的情况下，相邻机器人之间的距离为d；并且速度相等

b、建立多机器人合作控制优化问题，如(7)式所示；

$\underset{u_i\left(k\right)}{\min }\underset{l=1}{\overset{N}{\mathrm{\&Sigma;}}}{V}_i\left(s_i\left(l\right|k)\right)$

$s.t.:\left\{\begin{array}{c}{x}_i\left(l\right|k)=x_i(l-1|k)+v_i(l-1|k)\\ v_i\left(l\right|k)=v_i(l-1|k)+u_i\left(l\right|k)\\ \left|\right|s_i\left(l\right|k)-{\hat{s}}_i^{*}\left(l\right|k)\left|\right|<\mathrm{\&gamma;}\end{array}\right.---\left(7\right)$

其中：γ是距离约束参数用来约束产生的实际运动轨迹s_i(l|k)，...，s_i(N|k)和理想的运动轨迹之间的距离；优化的目的是要求产生的实际运动轨迹在接近理想运动轨迹的情况下，即保持多机器人***搜索环境质量参数最优数值的情况下，使得多机器人避免碰撞并速度相差有界；u_i(k)表示第i个机器人在k时刻需要求得的控制序列u_i(l|k)，...，u_i(N|k)；

c、求解多机器人合作控制优化问题(7)，获得实际的最优预测控制序列；

$u_i^{*}\left(k\right)=\arg \underset{u_i\left(k\right)}{\min }\underset{l=1}{\overset{N}{\mathrm{\&Sigma;}}}{V}_i\left(s_i\left(l\right|k)\right)$

$s.t.:\left\{\begin{array}{c}{x}_i\left(l\right|k)=x_i(l-1|k)+v_i(l-1|k)\\ v_i\left(l\right|k)=v_i(l-1|k)+u_i\left(l\right|k)\\ \left|\right|s_i\left(l\right|k)-{\hat{s}}_i^{*}\left(l\right|k)\left|\right|<\mathrm{\&gamma;}\end{array}\right.---\left(8\right)$

其中：表示第i个机器人在k时刻获得的实际的最优控制序列 $u_i^{*}\left(l\right|k),...,u_i^{*}\left(N\right|k);$

第四步：将第i个机器人在k时刻获得的实际的最优控制序列中的第一个最优控制施加给第i个机器人；

第五步：如果终止条件满足，则第i个机器人停止运行，并将他的环境模型f_i(x)输出；如果终止条件没有满足，则返回第一步继续执行。