CN102034030B - 一种多机器人系统合作定位危险气味源方法 - Google Patents

Abstract

本发明涉及一种多机器人系统合作定位危险气味源方法。现有的定位方法成功率较低，系统耗能过多。本发明首先建立气味源的观测模型,然后获得气味源位置的估计值，气味源位置的先验概率分布；其次如果检测到气味，修正先验概率分布，得到后验概率分布，并对后验概率分布采样，获得该气味源位置的估计值，产生机器人的新位置；如果没有检测到气味，直接采样气味源位置的先验概率分布，获得该气味源位置的估计值，产生机器人的新位置；最后用一致性算法控制机器人向新位置移动。本发明方法弥补了传统方法的不足，并有效地提高气味源定位的精度，保证多机器人系统能量的较小消耗，同时满足实际中要求快速定位的要求。

Description

一种多机器人系统合作定位危险气味源方法

技术领域

本发明属于自动化技术领域，涉及一种多机器人系统合作定位危险气味源方法。

背景技术

危险气味源定位对人类安全有着非常重要的意义，如定位污染物的源头和化学工厂中有毒气体泄漏的源头等等。因此，如何快速有效地定位危险气味源是一个极其重要的问题。然而，危险气味源定位问题在不同的环境下，呈现出不同的特点。通常，在没有气流的条件下，气味分子的扩散是一个主要力量，它能够驱动气味分子远离气味源。最大的浓度将会在气味源的附近出现。因此，我们可以使用梯度的方法来定位气味源。然而，在真实的世界里，气流是一种影响气味扩散的主要力量，它通过影响气味分子的运动，而形成羽烟。在由高Reynolds数刻画的环境里，羽烟将会呈现出高度的不连续性和间歇性，这使得梯度方法变得不可行。此外，由于多机器人系统的可扩展性和鲁棒性，使得多机器人系统代替单机器人系统成为危险气味源定位的主要工具。目前，协调多机器人系统定位危险气味源的主要方法是群智能技术，但是，这种技术的定位成功率较低，也会使多机器人系统消耗过多的能量，一个主要的原因是，该方法主要是利用了浓度幅值信息，而浓度幅值信息的不稳定性，往往会使多机器人系统局部收敛。另外，在我国对于多机器人系统的合作定位方法的研究刚刚开始起步，仍然没有提出行之有效的方法。在这一背景下，本发明弥补了现有技术的不足。

发明内容

本发明的目标是针对现有技术的不足之处，提出多机器人系统合作定位危险气味源方法，具体是以最大熵粒子滤波理论为理论基础，设计多机器人合作定位危险气味源方法。该方法弥补了传统合作搜索方式的不足，保证多机器人系统具有较高的定位精度和稳定性的同时，也保证形式简单并减少定位过程中多机器人系统消耗的能量。

对于每一个机器人，本发明方法首先建立气味源的观测模型；然后，以预先估计的正态分布为背景分布，利用最大熵原理和其他机器人对气味源位置的估计值，获得气味源位置的先验概率分布；其次，如果检测到气味，利用观测模型获得实际测量值，修正气味源位置的先验概率分布，从而得到气味源位置的后验概率分布，并对气味源位置的后验概率分布采样，获得该气味源位置的估计值，进一步产生机器人的新位置；如果没有检测到气味，直接采样气味源位置的先验概率分布，获得该气味源位置的估计值，进一步产生机器人的新位置；最后，用一致性算法控制机器人向新位置移动。

本发明的技术方案是通过数据检测、在线优化、气味源位置概率分布评估等手段，确立了一种多机器人系统合作定位危险气味源的方法，利用该方法可有效提高气味源定位的精度。

本发明方法在第                                                

(

)个机器人上运行的步骤包括：

(1)利用气味分子的运动学模型，建立气味源位置的观测模型，具体方法是：

a.建立单个气味分子团的运动模型:

               

其中, 

指气味分子团在

时刻的位置；

指气味分子团在时刻位置的微分；是在时刻的均值风速度；

表示一个随机过程，该过程服从均值为零，方差为

的高斯分布。

对气味分子团的运动模型在时间段[

,

]（

）内进行积分，获得气味分子团在

时刻被气味源释放，在当前时刻

时的位置，如：

            

其中，[

,

]指积分的时间段，

指气味源释放气味分子团的时间，

指当前时间； 

指的是在当前时间

气味分子团的位置；

是在时刻

气味源的位置。

b.通过定义

和

，将该模型离散化。其中，离散时刻

是气味源释放气味分子团时间

的整数值；离散时刻

是当前时间

的整数值；

是指时间的微小变化。因此，气味分子团的离散模型为：

             

其中，气味源位置在一定时间内是静止的，所以有

；

表示气味源在

时刻释放气味分子团，在

时刻的位置；表示在时间段[

,

]内风推动气味分子团的移动距离；

表示随机过程，服从均值为零，方差

的正态分布。

    考虑到气味分子团释放时间的所有可能性，即

                       

                       

则气味分子团的离散运动模型进一步表示如下：

                     

其中，

为

时刻气味分子团的位置；

为时刻气味源的位置；

为

时刻风推动气味分子团移动的距离；

是测量噪声，并且服从零均值，方差的正态分布。

    c.通过定义

，可以得到气味源位置的观测模型：

                        

其中，

是第个机器人在时刻

对气味源位置

的测量值。

(2)基于最大熵粒子滤波原理，获得气味源位置的先验概率分布。如果在

时刻，检测到气味（气味浓度大于阈值气味浓度），使用测量值修正气味源位置的先验概率分布，从而得到气味源位置的后验概率分布，通过采样气味源位置的后验概率分布产生气味源位置新的估计值；然后，可以进一步获得第

个机器人的下一步的位置。如果在

时刻，没有检测到气味，则采样气味源位置的先验概率分布产生气味源位置新的估计值，并可进一步获得第个机器人的下一步的位置，具体方法是：

a.基于经验知识，建立关于气味源位置的背景分布

。

                

其中, 

表示气味源位置满足正态分布的随机变量；

是气味源位置的经验估计值；

是关于气味源位置的经验估计协方差矩阵；

表示正态分布。

    并根据各个机器人对气味源位置的估计值，求解下式：

               

               

其中，

表示机器人还没有获得测量值时的

时刻；

是第个机器人对气味源位置的先验估计值；

表示的是测量预测值的先验平均值，并被第

个机器人获得；

表示测量预测值的先验矩阵；

是机器人的个数。

b.基于最大熵原理，采用共轭梯度法求解下式：

      

其中：

         

         

   

         

         

通过求解上式，获得拉格朗日乘子

和

，从而得到气味源位置的先验概率分布

：

      

其中：

表示气味源位置满足正态分布的随机变量；

是气味源位置的均值；是关于气味源位置的协方差矩阵；

表示正态分布。              

c.根据第个机器人是否检测到气味，执行不同的步骤。如果检测到气味，则执行c-1，否则执行c-2。

c-1. 如果检测到气味，则执行以下两个步骤:

①使用测量值更新拉格朗日乘子

和

，得：

其中，

表示机器人获得测量值时的时刻；

是测量噪声协方差矩阵。    这样，就获得了气味源位置的后验概率分布

：

      

采样气味源位置的后验概率分布，第

个机器人得到气味源位置的最新估计值

。

②基于气味源位置的最新估计值

，则机器人下一步的位置为气味源位置的最新估计值，即

              

其中，

是时刻第

个机器人的位置。

c-2.如果没有检测到气味，则执行以下两个步骤：

①采样气味源位置的先验概率分布

，第个机器人得到气味源位置的最新估计值

。

②基于气味源位置的最新估计值

和没有检测到气味的周期数，计算机器人下一步的位置，执行不同的步骤。如果在两个以内周期，执行步骤ⅰ，否则执行ⅱ。

ⅰ 如果在两个运行周期内（2k以内）则机器人的下一步的位置为：

其中：和

分别是风在轴和

轴方向的速度；

是第

个机器人在

时刻对于气味源位置最新估计值在

轴方向的坐标；

第

个机器人在

时刻位置在

轴方向的坐标；

和

第

个机器人在

时刻位置在

轴和

轴方向的坐标。

ⅱ如果两个运行周期及以外（2k及2k以上）第

个机器人没有检测到气味，则机器人下一步位置是基于气味源位置最新估计值

（）产生。具体方法是：

               

               

其中，

和

分别是控制

轴和

轴方向搜索范围控制参数，在

范围内产生一个均匀分布的随机数；

是机器人的个数。

d. 当第

个机器人到达新的位置后，从第（2）步重新开始执行，直到群体机器人中，某一个机器人检测到的气味浓度达到预先设定值，或通过视觉传感器判断出气味源，然后通过无线网络通知第

个机器人，则第

个机器人停止搜索，并提供气味源的估计位置

。

本发明提出的一种基于最大熵粒子滤波理论的多机器人系统合作定位危险气味源的方法，该方法弥补了传统方法的不足，并有效地提高气味源定位的精度，保证多机器人系统能量的较小消耗，同时满足实际中要求快速定位的要求。

本发明提出的合作定位方法，能够有效地估计气味源的概率分布，更好地让安全人员了解目前危险气味扩散的情况，进一步弥补传统方法的不足。此外，在任何时刻，都可以知道危险气味源的可能位置。

具体实施方式

以工业气体输送管道泄漏为例：

这里以工业气体输送管道泄漏为例子加以描述。在可能气体泄漏点建立2维局部坐标系，并为机器人安装风速计测量局部风速（每隔1秒，记录一次风速，共记录100个。一旦风速信息的数量超过了100个，则以新的风速信息代替旧的风速信息），和里程计计量机器人在局部坐标系中的位置，同时为机器人安装工业有毒气体检测装置。并设定好机器人的最大线速度和角速度并指定气味源位置的背景分布。对于第

个机器人，开始执行以下步骤：

   1、搜索气味线索。

多机器人系统首先沿着和当前风速交叉的方向前进，来搜索气味线索，一旦气味线索检测到，则启动多机器人系统定位危险气味源方法，即执行第2步；否则继续执行第1步。

2、多机器人系统定位危险气味源方法开始。

（1）计算气味分子团的运动距离。

              

                  

其中，

是

时刻的风速，

。然后，计算测量噪声协方差矩阵。

                

其中，

是测量噪声协方差矩阵；标准差

可以基于经验事先指定。

  （2）基于最大熵理论，获得气味源的先验概率分布。

     第一步，根据各个机器人对气味源位置的估计值，求解下式：

               

               

其中，

表示机器人还没有获得测量值时的

时刻；

是第

个机器人对气味源位置的先验估计值；

表示的是测量预测值的先验平均值，并被第

个机器人获得；

表示测量预测值的先验矩阵；是机器人的个数。

 第二步，采用共轭梯度法求解下式：

其中：

         

         

   

         

         

通过求解上式，获得拉格朗日乘子

和

，从而得到气味源位置的先验概率分布

：

      

其中：

表示气味源位置满足正态分布的随机变量；是气味源位置的均值；是关于气味源位置的协方差矩阵；

表示正态分布。              

（3）如果气味检测事件发生，执行第一步，否则执行步骤第二步。

第一步，如果气味检测事件发生，即气味浓度检测值高于阈值，则开始估计气味源位置，然后计算机器人下一步的位置，并控制机器人行进到该位置。具体步骤如下：  

①基于机器人当前的位置和气味分子团的运动距离，计算气味源位置的观测值。

               

②则使用测量值更新拉格朗日乘子

和

，得：

其中，表示机器人还获得测量值时的

时刻；

是测量噪声协方差矩阵。

这样，就获得了气味源位置的后验概率分布

：

      

③机器人下一步的位置为气味源位置的最新估计值，即

              

其中，

是

时刻第

个机器人的位置。

    ④采用一致性算法作为控制律，控制机器人移动到新的位置，移动时间可定为20秒（如果机器人没有在20秒内移动到新位置，也将认为机器人完成运动过程）。

第二步：如果没有检测到气味，首先估计气味源的位置，然后计算机器人下一步的位置。

①采样气味源位置的先验概率分布

，第

个机器人得到气味源位置的最新估计值

。

②根据气味源位置的最新估计值

和没有检测到气味的周期数，计算机器人下一步的位置，并控制机器人移动到该位置。如果2个周期内没有检测到气味，执行步骤ⅰ，如果2个周期以上没有检测到气味源，执行ⅱ。

ⅰ如果在两个运行周期内（2k以内）则机器人的下一步的位置为：

  

  

其中：

和

分别是风在

轴和

轴方向的速度；

是第

个机器人在时刻对于气味源位置最新估计值在

轴方向的坐标；

第

个机器人在

时刻位置在

轴方向的坐标；和

第个机器人在

时刻位置在

轴和轴方向的坐标。在这种情况下，采用一致性算法作为控制律，控制机器人移动到新的位置，移动时间可定为40秒。

ⅱ如果两个运行周期及以上（2k及2k以上）第

个机器人没有检测到气味，则机器人下一步位置是基于气味源位置最新估计值产生。具体方法是：

               

               

其中，和

是搜索范围控制参数，

在

范围内产生一个均匀分布的随机数；

是机器人的个数。在这种情况下，采用一致性算法作为控制律，控制机器人移动到新的位置，移动时间可定为40秒。

（4）当第

个机器人到达新的位置后，从第（1）步重新开始执行，直到群体机器人中，某一个机器人检测到的气味浓度达到预先设定值，或通过视觉传感器判断出气味源，通过无线网络通知第

个机器人，则第个机器人停止搜索,然后提供气味源的估计位置

。

Claims (1)

1.一种多机器人系统合作定位危险气味源方法，其特征在于该方法包括以下步骤：

(1)利用气味分子的运动学模型，建立气味源位置的观测模型，具体方法是：

A.建立单个气味分子团的运动模型:

其中r(t)指气味分子团在t时刻的位置,

指气味分子团在t时刻位置的微分,u(t)是在t时刻的均值风速度,ρ(t)表示一个随机过程，该随机过程服从均值为零，方差为σ²的高斯分布；

对气味分子团的运动模型在时间段[t_l,t_k]内进行积分，获得气味分子团在t_l时刻被气味源释放，在当前时刻t_k时的位置：

$r(t_l,t_k)=\underset{t_l}{\overset{t_k}{\∫}}u\left(\mathrm{\τ}\right)\mathrm{d\τ}+\underset{t_l}{\overset{t_k}{\∫}}\mathrm{\ρ}\left(\mathrm{\τ}\right)\mathrm{d\τ}+r_s\left(t_l\right)$

其中[t_l,t_k]指积分的时间段，t_l指气味源释放气味分子团的时间，t_k指当前时间,r(t_l,t_k)指的是在当前时间t_k气味分子团的位置,r_s(t_l)是在时刻t_l气味源的位置；

B.通过定义 $v(l,k)=\underset{j=l}{\overset{k-1}{\mathrm{\Σ}}}u\left(j\right)\mathrm{dt}\≈\underset{t_l}{\overset{t_k}{\∫}}u\left(\mathrm{\τ}\right)\mathrm{d\τ}$ 和 $w(l,k)=\underset{t_l}{\overset{t_k}{\∫}}\mathrm{\ρ}\left(\mathrm{\τ}\right)\mathrm{d\τ},$ 将运动模型离散化；其中离散时刻l是气味源释放气味分子团时间t_l的整数值，离散时刻k是当前时间t_k的整数值，dt是指时间的微小变化，气味分子团的离散模型为：

r(l,k)=r_s(k)+v(l,k)+w(l,k)

其中r(l,k)表示气味源在l时刻释放气味分子团，在k时刻的位置；v(l,k)表示在时间段[l,k]内风推动气味分子团的移动距离；w(l,k)表示随机过程，该随机过程服从均值为零，方差(k-l)σ²的正态分布；

令 $\stackrel{\‾}{v}\left(k\right)=\frac{1}{k}\underset{l=0}{\overset{k-1}{\mathrm{\Σ}}}v(l,k),$ $\stackrel{\‾}{w}\left(k\right)=\frac{1}{k}\underset{l=0}{\overset{k-1}{\mathrm{\Σ}}}w(l,k)$

则气味分子团的离散运动模型进一步表示如下：

$r\left(k\right)=r_s\left(k\right)+\stackrel{\‾}{v}\left(k\right)+\stackrel{\‾}{w}\left(k\right)$

其中r(k)为k时刻气味分子团的位置,r_s(k)为k时刻气味源的位置,

为k时刻风推动气味分子团移动的距离,

是测量噪声并且服从零均值，方差的正态分布；

C.通过定义

得到气味源位置的观测模型：

$z^i\left(k\right)=r_s\left(k\right)+\stackrel{\‾}{w}\left(k\right)$

其中，zⁱ(k)是第i个机器人在时刻k对气味源位置r_s(k)的测量值；

(2)基于最大熵粒子滤波原理，获得气味源位置的先验概率分布；如果在k时刻，检测到气味，使用测量值修正气味源位置的先验概率分布，得到气味源位置的后验概率分布，通过采样气味源位置的后验概率分布产生气味源位置新的估计值；然后，获得第i个机器人的下一步的位置；如果在k时刻，没有检测到气味，则采样气味源位置的先验概率分布产生气味源位置新的估计值，获得第i个机器人的下一步的位置，具体方法是：

a.建立关于气味源位置的背景分布Q(r)；

Q(r)=N(r;ω_s,C_s)

其中,r表示气味源位置满足正态分布的随机变量,ω_s是气味源位置的经验估计值,C_s是关于气味源位置的经验估计协方差矩阵,N()表示正态分布；

根据各个机器人对气味源位置的估计值，求解下式：

${\mathrm{\η}}_s\left(k^{-}\right)=\frac{1}{n}\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{r_s}^{\left(j\right)}\left(k^{-}\right)$

$H_s\left(k^{-}\right)=\frac{1}{n}\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{r_s}^{\left(j\right)}\left(k^{-}\right){r_s}^{\left(j\right)T}\left(k^{-}\right)$

其中k^-表示机器人还没有获得测量值时的k时刻,r_s ^(j)(k^-)是第j个机器人对气味源位置的先验估计值,η_s(k^-)表示的是测量预测值的先验平均值，并由第i个机器人获得；H_s(k^-)表示测量预测值的先验矩阵,n是机器人的个数；

b.基于最大熵原理，采用共轭梯度法求解下式：

$H({\mathrm{\η}}_s\left(k^{-}\right),H_s\left(k^{-}\right))=\underset{\mathrm{\λ}\left(k^{-}\right),\mathrm{\Λ}\left(k^{-}\right)}{\sup }\{{\mathrm{\η}}_s\left(k^{-}\right)\·\mathrm{\λ}\left(k^{-}\right)+\frac{1}{2}{H}_s\left(k^{-}\right):\mathrm{\Λ}\left(k^{-}\right)-F(\mathrm{\λ}\left(k^{-}\right),\mathrm{\Λ}\left(k^{-}\right))\}$ 其中：F(λ(k^-),Λ(k^-))=logZ(λ(k^-),Λ(k^-))

$Z(\mathrm{\λ}\left(k^{-}\right),\mathrm{\Λ}\left(k^{-}\right))=\sqrt{\frac{\left|D_s\left(\mathrm{\Λ}\left(k^{-}\right)\right)\right|}{\left|C_s\right|}}\exp [\mathrm{\λ}{\left(k^{-}\right)}^T\stackrel{\‾}{w}\left(k\right)+\frac{1}{2}\stackrel{\‾}{w}{\left(k\right)}^T\mathrm{\Λ}\left(k^{-}\right)\stackrel{\‾}{w}\left(k\right)$

$-\frac{1}{2}{{\mathrm{\ω}}_s}^T{C_s}^{-1}{\mathrm{\ω}}_s+\frac{1}{2}{{\mathrm{\ξ}}_s}^T(\mathrm{\λ}\left(k^{-}\right),\mathrm{\Λ}\left(k^{-}\right)){D_s}^{-1}\left(\mathrm{\Λ}\left(k^{-}\right)\right){\mathrm{\ξ}}_s(\mathrm{\λ}\left(k^{-}\right),\mathrm{\Λ}\left(k^{-}\right))]$

D_s(Λ(k^-))=C_s+[C_s ^-1-Λ(k^-)]^-1Λ(k^-)C_s

${\mathrm{\ξ}}_s(\mathrm{\λ}\left(k^{-}\right),\mathrm{\Λ}\left(k^{-}\right))={\mathrm{\ω}}_s+[{C_s}^{-1}-\mathrm{\Λ}]{\left(k^{-}\right)}^{-1}(\mathrm{\λ}\left(k^{-}\right)+\mathrm{\Λ}\left(k^{-}\right)({\mathrm{\ω}}_s+\stackrel{\‾}{w}\left(k\right)))$

通过求解上式，获得拉格朗日乘子λ(k^-)和Λ(k^-)，从而得到气味源位置的先验概率分布P(r;ξ_s(λ(k^-),Λ(k^-))，D_s(Λ(k^-)))：

P(r;ξ_s(λ(k^-),Λ(k^-)),D_s(Λ(k^-)))=N(r;ξ_s(λ(k^-),Λ(k^-)),D_s(Λ(k^-)))

其中r表示气味源位置满足正态分布的随机变量，ξ_s(λ(k^-),Λ(k^-))是气味源位置的均值，D_s(Λ(k^-))是关于气味源位置的协方差矩阵，N()表示正态分布；

c.根据第i个机器人是否检测到气味，执行不同的步骤；如果检测到气味，则执行c-1，否则执行c-2；

c-1.检测到气味，执行以下两个步骤:

①使用测量值更新拉格朗日乘子λ(k^-)和Λ(k^-)，得：

λ(k⁺)=λ(k^-)+R^-1(k)zⁱ(k)

Λ(k⁺)=Λ(k^-)-R^-1(k)

其中k⁺表示机器人获得测量值时的k时刻，R(k)是测量噪声协方差矩阵，这样就获得了气味源位置的后验概率分布P(r;ξ_s(λ(k⁺),Λ(k⁺))，D_s(Λ(k⁺)))：

P(r;ξ_s(λ(k⁺),Λ(k⁺)),D_s(Λ(k⁺)))=N(r;ξ_s(λ(k⁺),Λ(k⁺)),D_s(Λ(k⁺)))

采样气味源位置的后验概率分布P(r;ξ_s(λ(k⁺),Λ(k⁺))，D_s(Λ(k⁺)))，得到气味源位置的最新估计值r_s ⁽ⁱ⁾(k⁺)；

②基于气味源位置的最新估计值r_s ⁽ⁱ⁾(k⁺)，则机器人下一步的位置为气味源位置的最新估计值，即ψ_i(k+1)=r_s ⁽ⁱ⁾(k⁺)，其中ψ_i(k+1)是k+1时刻第i个机器人的位置；

c-2.没有检测到气味，则执行以下两个步骤：

③采样气味源位置的先验概率分布P(r;ξ_s(λ(k^-),Λ(k^-)),D_s(Λ(k^-)))，得到气味源位置的最新估计值r_s ⁽ⁱ⁾(k^-)；

④基于气味源位置的最新估计值r_s ⁽ⁱ⁾(k^-)和没有检测到气味的周期数，计算机器人下一步的位置，如果在两个采样周期内没有检测到气味，执行步骤ⅰ，如果在两个采样周期及两个采样周期以上没有检测到气味，则执行ⅱ；

ⅰ机器人的下一步的位置为：

${\mathrm{\&psi;}}_{\mathrm{xi}}(k+1)=\left\{\begin{array}{c}({\mathrm{\&psi;}}_{\mathrm{xi}}\left(k\right)-{r_{\mathrm{xs}}}^{\left(i\right)}\left(k^{-}\right))/2+{r_{\mathrm{xs}}}^{\left(i\right)}\left(k^{-}\right),{\mathrm{\&psi;}}_{\mathrm{xi}}\left(k\right)\&GreaterEqual;{r_{\mathrm{xs}}}^{\left(i\right)}\left(k^{-}\right)\\ ({r_{\mathrm{xs}}}^{\left(i\right)}\left(k^{-}\right)-{\mathrm{\&psi;}}_{\mathrm{xi}}\left(k\right))/2+{r_{\mathrm{xs}}}^{\left(i\right)}\left(k^{-}\right),{\mathrm{\&psi;}}_{\mathrm{xi}}\left(k\right)<{r_{\mathrm{xs}}}^{\left(i\right)}\left(k^{-}\right)\end{array}\right.$

ψ_yi(k+1)=ψ_xi(k+1)u_y/u_x

其中u_x和u_y分别是风在x轴和y轴方向的速度,r_xs ⁽ⁱ⁾(k^-)是第i个机器人在k时刻对于气味源位置最新估计值在x轴方向的坐标,ψ_xi(k)是第i个机器人在k时刻位置在x轴方向的坐标,ψ_xi(k+1)和ψ_yi(k+1)分别是第i个机器人在k+1时刻位置在x轴和y轴方向的坐标；

ⅱ机器人下一步位置是基于气味源位置最新估计值r_s ^(j)(k^-)产生,具体方法是：

${\mathrm{\&psi;}}_{\mathrm{xi}}(k+1)=\frac{1}{n}\underset{j=1}{\overset{n}{\mathrm{\&Sigma;}}}{r_{\mathrm{xs}}}^{\left(j\right)}\left(k^{-}\right)+\mathrm{rand}\left(\right)\&times;2\mathrm{\&beta;}-\mathrm{\&beta;}$

${\mathrm{\&psi;}}_{\mathrm{yi}}(k+1)=\frac{1}{n}\underset{j=1}{\overset{n}{\mathrm{\&Sigma;}}}{r_{\mathrm{ys}}}^{\left(j\right)}\left(k^{-}\right)+\mathrm{rand}\left(\right)\&times;2\mathrm{\&gamma;}-\mathrm{\&gamma;}$

其中β和γ分别是控制x轴和y轴方向搜索范围控制参数，rand()是在[0,1]范围内产生一个均匀分布的随机数,n是机器人的个数；

d.当第i个机器人到达新的位置后，重复步骤a至步骤c，直到群体机器人中的某一个机器人检测到的气味浓度达到预先设定值，或通过视觉传感器判断出气味源，然后通过无线网络通知第i个机器人，则第i个机器人停止搜索，并提供气味源的估计位置r_s ⁽ⁱ⁾(k⁺)。