基于生物触角模型的多机器人水下目标搜寻方法及装置

摘要

本发明涉及一种基于生物触角模型的多机器人水下目标搜寻方法及装置，载有多台水下机器人的水下探测船到达目标搜寻区域后，各水下机器人利用生物触角模型驱动运动，将探测的环境信息数据发给水下探测船；水下探测船根据环境信息数据实时更新栅格地图，并广播给各水下机器人；当某水下机器人发现目标时广播该信息给其他水下机器人和水下探测船；水下探测船到达目标位置后利用机械手抓取目标，其他水下机器人运动到目标位置后登上水下探测船。本发明提出一种生物触角模型驱动机器人运动的方法和装置来完成多机器人在未知环境中的目标搜寻任务，可以大大的提高水下机器人目标搜寻的效率。

说明书

技术领域

本发明属于水下未知环境中多机器人目标搜寻领域，是机器人技术与 生物感知技术相结合的应用，特别是涉及一种基于生物触角模型的多机器 人水下目标搜寻方法和装置。

背景技术

水下目标搜寻是水下作业的重要内容之一，水下环境恶劣，要想实现 水下目标搜寻一般的设备很难完成。因此，人们将目光放在了机器人身上， 由于水下区域广阔，单个机器人搜寻效率低下，水下多机器人技术的研究 随之发展起来。

水下环境具有时间可变性，随着时间的推移，由于水流的影响会使得 环境中的物体发生移动而改变环境的状态。这样不仅增加了对机器人自主 避障的要求，还使得机器人不能准确的完成对水下环境的地图构建，从而 影响了机器人路径规划的效率。在这样的复杂多变环境下如何实现多机器 人的目标搜寻工作，成为水下机器人技术的研究热点，也是难点问题。

发明内容

本发明提出了一种基于生物触角模型的多机器人水下目标搜寻方法及 设备。通过生物触角模型对环境进行探索，并建立环境栅格地图，然后利 用信息融合技术实时更新水下环境栅格地图信息，指导机器人进行目标搜 索，发现目标后，利用扩散方程进行路径规划，指导水下探测船运动到目 标进行目标抓取，并最终完成水下目标搜索任务。本发明提供了一种能够 提高水下目标搜寻效率的基于生物触角模型的多机器人水下目标搜寻方法 及设备。

本发明实现上述发明目的的技术方案是：提供一种基于生物触角模型 的多机器人水下目标搜寻方法及装置。

基于生物触角模型的多机器人水下目标搜寻方法，其特征在于，包括 如下步骤：

(1)接到目标搜寻任务后，搜寻人员根据目标已知的大概位置信息，将 载有多台小型水下机器人的水下探测船从该处投入海底；

(2)水下探测船下沉到海底后，以水下探测船中心为原点，建立平面坐 标系；各水下机器人建立自身的运动学模型，第i台机器人的状态变量为 (x_i,y_i,θ_i)，其中(x_i,y_i)为机器人在平面坐标系中的位置坐标，θ_i为机器人运 动方向角；水下探测船释放水下机器人。

(3)各水下机器人利用生物触角模型驱动机器人不断往前运动，进行目 标搜索，同时利用自身携带的声呐传感器实时进行环境探测，并将声呐传 感器获取的环境信息数据通过自身携带的水下通信设备发给水下探测船；

(4)水下探测船建立水下环境动态栅格地图，根据接收到的各水下机器 人环境信息数据，实时更新栅格地图，并利用自身携带的水下通讯设备广 播该地图信息给各水下机器人；

(5)当某水下机器人在其侦测范围内根据已知目标的有关特征和信息 发现目标时，标记其位置信息，并广播该信息给其他水下机器人和水下探 测船；

(6)其他水下机器人和水下探测船在得到目标位置信息后，根据栅格地 图，利用扩散方程进行路径规划，计算各自到达目标的最佳路径，水下探 测船到达目标位置，利用携带的机械手进行目标抓取，其他水下机器人运 动到目标位置，然后所有水下机器人集合登上水下探测船，水下探测船上 浮，完成目标搜索任务。

所述步骤(3)中，生物触角模型的建立步骤为：

生物触角模型是将Bug算法与神经行为学结合得到的仿生条件反射导 航算法，即在水下机器人本身一些特定区域借助声呐传感器探测到的触觉 信息，实时激励触发反射行为来实现对未知环境中目标的搜寻；

该模型中设置五个生物触角模型分别为：位于前方的半圆环形触角 Dng_s与Dng_b，其半径分别为R_s与R_b；位于左右主动轮位置的矩形触角Wng_l与 Wng_r，其大小为d_W×c，其中d_w和c分别为矩形触角的长和宽；位于正前方 由大小为2R_s×a的矩形加半径为R_s的半圆形组成的宽面积型触角Lng，其中 2R_s为矩阵的长，a为矩形的宽；左右主动轮位置的矩形触角Dng_s和Dng_b用 于绕开障碍物边缘行为的实现，且R_r≤R_s≤R_b，R_r是水下机器人的外接圆半 径；Wng_l与Wng_r用于记录当前行走过程中左右主动轮运动的实时位移s_l和 s_r；Lng用于检测当前传感器探测范围内无障碍直线行走的最大距离s_max。

步骤（3）中，基于生物触角模型的水下机器人行走过程如下：

机器人从起点s出发，初始阶段采用随机搜索策略，即机器人运动方 向角为：

${\left({\theta }_r\right)}_{t+1}={\left({\theta }_r\right)}_t\pm \frac{\pi }{4}ϵ$

其中，(θ_r)_t+1是机器人在t+1时刻的运动方向角，(θ_r)_t是机器人t时刻的 运动方向角，ε是一个（0,1）之间的随机数；

在行走过程中通过触角模型不断判断各种运动条件是否满足来调整机 器人的运动方向角：

首先利用相遇条件判断是否遇到障碍物，如果遇到障碍物，则利用避 开条件判断是否进行避障，如果满足避开条件，则采用弧线运动避开障碍 物，并利用脱离条件判断是否已安全避开障碍物，如果避开障碍物则采用 随机搜索，否则继续进行避障；在运动过程中实时利用终止条件判断是否 停止机器人运动。

相遇条件为：d_min≤R_s，其中d_min表示声呐传感器探测范围内机器人与障 碍物之间的最近距离；

避开条件为：d_min≥R_b，其中d_min表示声呐传感器探测范围内机器人与障 碍物之间的最近距离；

脱离条件为：(|θ_r-θ_XT|≤θ_ε & d_XT≤s_max)OR(|θ_r-θ_XT|≤θ_ε & s_max≥s_step)，其 中θ_e为预先设定的对准角度容差，θ_r为当前水下机器人的运动方向角，θ_XT是 水下机器人当前位置与目标位置之间的角度，d_XT为水下机器人当前位置与 目标之间的距离，s_step为预先设定的脱离阈值，s_max为水下机器人当前位置 与目标方向上可无障碍行走的最大距离；

终止条件为：d_XT≤d_e，其中d_e为预先设定的终点距离容差。

所述步骤(4)水下探测船建立水下环境动态栅格地图、更新栅格地图的 过程为：

（4a）以水下探测船为原点，以水下机器人最大勘测距离为半径的圆 形环境区域内建立直角坐标系；该直角坐标系以水下探测船水平向右方向 为X轴正方向，正前方方向为Y轴正方向，并以1×1m²尺度将环境区域进行 基本的栅格地图划分，使得每一个栅格均有唯一的直角坐标(x,y)来标识， 环境地图用一个二维数矩阵m(i,j)表示：

（4b）水下探测船将所接收到的由各个水下机器人声纳传感器获取的 环境信息数据转换成信度函数值，作为对各栅格是否占有障碍物的一种评 估；

（4c）水下探测船将环境数据转换为栅格障碍物信度分配值后，再利 用D-S证据理论将所得到的信度分配值相融合来实时更新栅格地图。

步骤（4b）中数据转换方式如下：

在R-d≤r≤R+d区间内该概率值：

$m\left(O\right)=\frac{{{\left(\frac{\alpha -\left|\theta \right|}{\alpha }\right)}^2+\left(\frac{d-|R-r|}{d}\right)}^2}{2}$

m(E)=0

m({O,E})=1.00-m(O)

在R_min≤r≤R-d区间内该概率值：

m(O)=0

$m\left(E\right)=\frac{{{\left(\frac{\alpha -\left|\theta \right|}{\alpha }\right)}^2+\left(\frac{R-r-d}{R-d}\right)}^2}{2}$

m({O,E})=1.00-m(E)

其中:m(O)是栅格占有障碍物的信度分配值，m(E)是栅格为非障碍物的信度 函数分配值，m({O,E})是栅格状态模式不确定的信度函数分配值，r是障碍 物与机器人中心的距离，R是声波辐射量程，d是辐射误差，θ是辐射角度， α是散射波束角度的一半，R_min是水下机器人最小的安全距离。

步骤（4c）中融合的方法有两种，分别为不同时间上的信息融合和不 同机器人间的信息融合；

不同时间上的信息融合是指将水下机器人当前时刻所得到的信度函数 分配值与上一时刻得到的信度函数分配值相融合，融合公式为：

$m_O^t(i,j)=\frac{m_O^{t-1}(i,j)S_O^t(i,j)+m_O^{t-1}(i,j)(1-S_E^t(i,j)-S_O^t(i,j))+(1-m_E^{t-1}(i,j)-m_O^{t-1}(i,j))S_O^t(i,j)}{1-m_E^{t-1}(i,j)S_O^t(i,j)-m_O^{t-1}(i,j)S_E^t(i,j)}$

$m_E^t(i,j)=\frac{m_E^{t-1}(i,j)S_E^t(i,j)+m_E^{t-1}(i,j)(1-S_E^t(i,j)-S_O^t(i,j))+(1-m_E^{t-1}(i,j)-m_O^{t-1}(i,j))S_E^t(i,j)}{1-m_E^{t-1}(i,j)S_O^t(i,j)-m_O^{t-1}(i,j)S_E^t(i,j)}$

其中：分别为t时刻栅格(i,j)占有障碍物和非障碍物的信度函 数分配值，分别是(t-1)时刻存储的占有障碍物和非障碍物 信度函数分配值，分别是融合后占有障碍物和非障碍物信度 函数分配值。

不同机器人间的信息融合是指将多台水下机器人得到的同一栅格的信 度函数分配值相融合，而不同栅格的信度函数分配值则保持不变，融合公 式为：

$m_O^t(i,j)=\frac{\underset{k=1}{\overset{n}{\Sigma }}{m}_{\mathrm{Ok}}^t(i,j)}{n}$

$m_E^t(i,j)=\frac{\underset{k=1}{\overset{n}{\Sigma }}{m}_{\mathrm{Ek}}^t(i,j)}{n}$

其中n是探测到同一栅格的水下机器人个数，分别是第 k台机器人t时刻对同一栅格(i,j)融合后的占有障碍物和非障碍物信度函数 分配值。

所述步骤(6)其他水下机器人和水下探测船在得到目标位置信息后，根 据栅格地图，利用扩散方程进行路径规划的步骤为：

（6a）水下机器人和水下探测船根据所构建的栅格地图，确定各自的 起始坐标位置和目标坐标位置；栅格地图中每个栅格由(x,y,u)表示，(x,y)是 该栅格的地理位置信息，u为扩散方程在该栅格的浓度值，由下面扩散方程 模型公式计算：

其中M为邻近栅格的个数，u_k+1;r、u_k;r分别表示第k+1、k时刻第r个栅 格的浓度值，u_k;m为第k时刻第m个邻近栅格的浓度值，τ表示步长间隔，g 表示衰变速率且为很小的正数；

（6b）根据扩散方程模型计算每个栅格的动态浓度值，使得在障碍物 区域栅格的浓度值始终最小，在目标点栅格的浓度值始终最大；水下机器 人根据每个栅格的动态浓度值的大小实时计算出到达目标的最佳路径；

机器人返回时，最佳路径实时计算过程为：

(θ_r)_t+1=angle(p_r,p_n)

$p_n\Leftarrow u_{p_n}=\max \{u_m,m=\mathrm{1,2}...,M\}$

其中(θ_r)_t+1为水下机器人下一步运动的方向角，p_r为当前水下机器人的 位置，p_n为所有邻近栅格中浓度最大的栅格位置；u_m是p_r位置的邻近栅格 浓度值；是求得的p_r邻近栅格中的最大浓度值；angle（）是计算两个位 置夹角的函数，max（）是计算最大值函数；根据该运动规则，水下机器人 规划出一条从起点位置自动避开障碍物、快速运动到目标位置的最优路径。

基于生物触角模型的多机器人水下目标搜寻装置，其特征在于：包括 一水下探测船和多台小型水下机器人；水下探测船前部装有机械手；

所述水下探测船和各水下机器人上载有可相互通信的水下通信设备；

水下探测船和各水下机器人均携带有存储设备、中心处理器、声呐传 感器；

载有多台小型水下机器人的水下探测船下沉到目标搜寻区域后，释放 水下机器人进行目标搜索；

水下探测船以水下探测船中心为原点，建立平面坐标系；各水下机器 人建立自身的运动学模型；

各水下机器人利用生物触角模型驱动机器人不断往前运动，同时利用 自身携带的声呐传感器实时进行环境探测，并将声呐传感器获取的环境信 息数据通过自身携带的水下通信设备发给水下探测船；

水下探测船建立水下环境动态栅格地图，根据接收到的各水下机器人 环境信息数据，实时更新栅格地图，并利用自身携带的水下通讯设备广播 该地图信息给各水下机器人；

当某水下机器人在其侦测范围内根据已知目标的有关特征和信息发现 目标时，标记其位置信息，并广播该信息给其他水下机器人和水下探测船；

其他水下机器人和水下探测船在得到目标位置信息后，根据栅格地图， 利用扩散方程进行路径规划，计算各自到达目标的最佳路径，水下探测船 到达目标位置，利用携带的机械手进行目标抓取，其他水下机器人运动到 目标位置，然后所有水下机器人集合登上水下探测船，水下探测船上浮， 完成目标搜索任务。

本发明具有如下优点：

（1）本发明利用水下探测船搭载小型水下机器人进行目标搜索，然后 利用水下探测船进行水下目标抓取，将多机器人系统用于水下未知环境的 目标搜寻中，并使机器人以不同的方向角进行目标搜寻，能够大大提高目 标搜索的效率。

（2）本发明提出一种生物触角模型驱动机器人运动的方法来完成多机 器人在未知环境中的目标搜寻任务，可以大大的提高水下机器人目标搜寻 的效率。

（3）本发明将声纳传感器采集的数据与D-S信息融合算法相结合，用 于实时更新水下环境栅格地图，能够高效、快速、准确的实现栅格地图环 境的构建和动态更新。

（4）本发明利用扩散方程进行路径规划，一旦小型机器人发现目标， 则其他水下机器人和水下探测船采用扩散方程完成对目标位置的导航，该 方法能够高效、快速的规划出一条安全无碰撞到达目标的最优路径，高效 完成水下目标搜索和抓取任务。

附图说明

图1为本发明的硬件设备组成方框图；

图2为本发明中基于生物触角模型的多机器人水下目标搜寻方法流程 图；

图3为本发明中生物触角模型流程图；

图4为本发明中基于生物触角模型的多机器人水下目标搜寻示意图。

具体实施方式

以下结合附图对本发明作进一步的详细说明。

实施本发明的一种硬件设备组成框图如图1所示，包括一个水下探测 船和四台水下机器人，其中四台水下机器人分别搭载在水下探测船的四个 边缘，水下探测船和水下机器人上都载有水下通讯设备、中心处理器、存 储设备、声呐传感器，水下探测船前端装有机械手。水下机器人通过声呐 传感器探测环境实时信息，利用水下通讯设备将环境信息发给水下探测船， 水下探测船接收信息并在中心处理器上完成栅格地图的构建，并利用存储 设备存储地图，同时将该栅格地图信息发给各个水下机器人，水下机器人 将接收到的地图信息存储。当有水下机器人发现目标时，将该目标信息发 给其他水下机器人及水下探测船，并在已有的栅格地图中规划出各自到达 目标的最佳路径。其他水下机器人和水下探测船到达目标位置，水下探测 船完成目标抓取，所有水下机器人登上水下探测船，并上浮至水面，完成 目标搜寻任务。

本发明是基于生物触角模型的多机器人水下目标搜寻方法，其具体流 程如图2所示，包括如下步骤：

(1)水下探测船上载有4台小型水下机器人，探测船和水下机器人都载有 水下通信设备、存储设备、中心处理器、声呐传感器。探测船前部装有机 械手。

(2)接到目标搜寻任务后，搜寻人员根据目标已知的大概位置信息，将水下 探测船从该处投入海底，释放水下机器人进行目标搜索；

(3)水下探测船下沉到海底后，立即以水下探测船中心为原点，建立平面坐 标系。各机器人建立自身的运动学模型，第i台机器人的状态变量为 (x_i,y_i,θ_i)，其中(x_i,y_i)为机器人在平面坐标系中的位置坐标，θ_i为机器人运 动方向角，各机器人的初始运动方向角分别为

(4)各水下机器人离开水下探测船利用生物触角模型驱动机器人不断往前 运动，同时利用声呐传感器实时进行环境探测，并将声呐传感器获取的环 境信息数据发给水下探测船；

(5)水下探测船建立水下环境动态栅格地图，根据接收到的各机器人环境信 息数据，利用D-S证据理论实时更新栅格地图，并利用水下通讯设备广播 该地图信息给各水下机器人；

(6)当某水下机器人在其侦测范围内根据已知目标的有关特征和信息发现 目标时，标记其位置信息，并广播该信息给其他水下机器人和水下探测船；

(7)其他水下机器人和水下探测船在得到目标位置信息后，根据栅格地图， 利用扩散方程进行路径规划，计算各自到达目标的最佳路径，水下探测船 到达目标位置，并利用机械手进行目标抓取，其他机器人运动到目标位置， 然后所有机器人集合登上水下探测船，水下探测船上浮，完成目标搜索任 务。

生物触角模型是将Bug算法与神经行为学结合得到的仿生条件反射导 航算法，即在机器人本身一些特定区域借助传感器探测到的触觉信息，实 时激励触发反射行为来实现对未知环境中目标的搜寻。该模型中设置五个 生物触角模型分别为：位于前方的半圆环形触角Dng_s与Dng_b，其半径分别 为R_s与R_b；位于左右主动轮位置的矩形触角Wng_l与Wng_r，其大小为d_W×c， 其中d_w和c分别为矩形触角的长和宽；位于正前方由大小为2R_s×a的矩形加 半径为R_s的半圆形组成的宽面积型触角Lng，其中2R_s为矩阵的长，a为矩 形的宽；左右主动轮位置的矩形触角Dng_s和Dng_b用于绕开障碍物边缘行为 的实现，且R_r≤R_s≤R_b，R_r是水下机器人的外接圆半径；Wng_l与Wng_r用于记 录当前行走过程中左右主动轮运动的实时位移s_l和s_r；Lng用于检测当前传 感器探测范围内无障碍直线行走的最大距离s_max。

本发明中利用生物触角模型驱动机器人不断往前运动，其具体工作流 程如图3所示，包括如下步骤：

（1）机器人从起点s出发，初始阶段采用随机搜索策略，即机器人运 动方向角为：

${\left({\theta }_r\right)}_{t+1}={\left({\theta }_r\right)}_t\pm \frac{\pi }{4}ϵ$

其中，(θ_r)_t+1是机器人在t+1时刻的运动方向角，(θ_r)_t是机器人t时刻的运动 方向角，ε是一个（0,1）之间的随机数。

（2）在行走过程中通过触角模型不断判断各种运动条件是否满足来调 整机器人的运动方向角。首先利用相遇条件判断是否遇到障碍物，如果遇 到障碍物，则利用避开条件判断是否进行避障，如果满足避开条件，则采 用弧线运动避开障碍物，并利用脱离条件判断是否已安全避开障碍物，如 果避开障碍物则采用随机搜索，否则继续进行避障。在运动过程中实时利 用终止条件判断是否停止机器人运动。这里相遇条件为：d_min≤R_s，其中d_min表示声呐传感器探测范围内机器人与障碍物之间的最近距离；避开条件为： d_min≥R_b；脱离条件为：(|θ_r-θ_XT|≤θ_ε & d_XT≤s_max)OR(|θ_r-θ_XT|≤θ_ε & s_max≥s_step)， 其中θ_e为预先设定的对准角度容差，θ_r为当前水下机器人的运动方向角，θ_XT是水下机器人当前位置与目标位置之间的角度，d_XT为水下机器人当前位置 与目标之间的距离，s_step为预先设定的脱离阈值，s_max为水下机器人当前位 置与目标方向上可无障碍行走的最大距离；终止条件为：d_XT≤d_e，其中d_e为 预先设定的终点距离容差。

水下探测船建立水下环境动态栅格地图、更新栅格地图的过程为：

（4a）以水下探测船为原点，以水下机器人最大勘测距离为半径的圆 形环境区域内建立直角坐标系；该直角坐标系以水下探测船水平向右方向 为X轴正方向，正前方方向为Y轴正方向，并以1×1m²尺度将环境区域进行 基本的栅格地图划分，使得每一个栅格均有唯一的直角坐标(x,y)来标识， 环境地图用一个二维数矩阵m(i,j)表示：

（4b）水下探测船将所接收到的由各个水下机器人声纳传感器获取的 环境信息数据转换成信度函数值，作为对各栅格是否占有障碍物的一种评 估；

（4c）水下探测船将环境数据转换为栅格障碍物信度分配值后，再利 用D-S证据理论将所得到的信度分配值相融合来实时更新栅格地图。

步骤（4b）中数据转换方式如下：

在R-d≤r≤R+d区间内该概率值：

$m\left(O\right)=\frac{{{\left(\frac{\alpha -\left|\theta \right|}{\alpha }\right)}^2+\left(\frac{d-|R-r|}{d}\right)}^2}{2}$

m(E)=0

m({O,E})=1.00-m(O)

在R_min≤r≤R-d区间内该概率值：

m(O)=0

$m\left(E\right)=\frac{{{\left(\frac{\alpha -\left|\theta \right|}{\alpha }\right)}^2+\left(\frac{R-r-d}{R-d}\right)}^2}{2}$

m({O,E})=1.00-m(E)

其中:m(O)是栅格占有障碍物的信度分配值，m(E)是栅格为非障碍物的信度 函数分配值，m({O,E})是栅格状态模式不确定的信度函数分配值，r是障碍 物与机器人中心的距离，R是声波辐射量程，d是辐射误差，θ是辐射角度， α是散射波束角度的一半，R_min是水下机器人最小的安全距离。

步骤（4c）中融合的方法有两种，分别为不同时间上的信息融合和不 同机器人间的信息融合；

不同时间上的信息融合是指将水下机器人当前时刻所得到的信度函数 分配值与上一时刻得到的信度函数分配值相融合，融合公式为：

$m_O^t(i,j)=\frac{m_O^{t-1}(i,j)S_O^t(i,j)+m_O^{t-1}(i,j)(1-S_E^t(i,j)-S_O^t(i,j))+(1-m_E^{t-1}(i,j)-m_O^{t-1}(i,j))S_O^t(i,j)}{1-m_E^{t-1}(i,j)S_O^t(i,j)-m_O^{t-1}(i,j)S_E^t(i,j)}$

$m_E^t(i,j)=\frac{m_E^{t-1}(i,j)S_E^t(i,j)+m_E^{t-1}(i,j)(1-S_E^t(i,j)-S_O^t(i,j))+(1-m_E^{t-1}(i,j)-m_O^{t-1}(i,j))S_E^t(i,j)}{1-m_E^{t-1}(i,j)S_O^t(i,j)-m_O^{t-1}(i,j)S_E^t(i,j)}$

其中：分别为t时刻栅格(i,j)占有障碍物和非障碍物的信度函 数分配值，分别是(t-1)时刻存储的占有障碍物和非障碍物 信度函数分配值，分别是融合后占有障碍物和非障碍物信度 函数分配值。

不同机器人间的信息融合是指将多台水下机器人得到的同一栅格的信 度函数分配值相融合，而不同栅格的信度函数分配值则保持不变，融合公 式为：

$m_O^t(i,j)=\frac{\underset{k=1}{\overset{n}{\Sigma }}{m}_{\mathrm{Ok}}^t(i,j)}{n}$

$m_E^t(i,j)=\frac{\underset{k=1}{\overset{n}{\Sigma }}{m}_{\mathrm{Ek}}^t(i,j)}{n}$

其中n是探测到同一栅格的水下机器人个数，分别是第 k台机器人t时刻对同一栅格(i,j)融合后的占有障碍物和非障碍物信度函数 分配值。

其他水下机器人和水下探测船在得到目标位置信息后，根据栅格地图， 利用扩散方程进行路径规划的步骤为：

（6a）水下机器人和水下探测船根据所构建的栅格地图，确定各自的 起始坐标位置和目标坐标位置；栅格地图中每个栅格由(x,y,u)表示，(x,y)是 该栅格的地理位置信息，u为扩散方程在该栅格的浓度值，由下面扩散方程 模型公式计算：

其中M为邻近栅格的个数，u_k+1;r、u_k;r分别表示第k+1、k时刻第r个栅 格的浓度值，u_k;m为第k时刻第m个邻近栅格的浓度值，τ表示步长间隔，g 表示衰变速率且为很小的正数；

（6b）根据扩散方程模型计算每个栅格的动态浓度值，使得在障碍物 区域栅格的浓度值始终最小，在目标点栅格的浓度值始终最大；水下机器 人根据每个栅格的动态浓度值的大小实时计算出到达目标的最佳路径；

机器人返回时，最佳路径实时计算过程为：

(θ_r)_t+1=angle(p_r,p_n)

$p_n\Leftarrow u_{p_n}=\max \{u_m,m=\mathrm{1,2}...,M\}$

其中(θ_r)_t+1为水下机器人下一步运动的方向角，p_r为当前水下机器人的 位置，p_n为所有邻近栅格中浓度最大的栅格位置；u_m是p_r位置的邻近栅格 浓度值；是求得的p_r邻近栅格中的最大浓度值；angle（）是计算两个位 置夹角的函数，max（）是计算最大值函数；根据该运动规则，水下机器人 规划出一条从起点位置自动避开障碍物、快速运动到目标位置的最优路径。

本发明中基于生物触角模型的多机器人水下目标搜寻方法示意图如图 4所示。用生物触角模型驱动机器人运动实现未知环境的导航，机器人既可 以自主无碰撞的运动，又能够高效快速的完成目标搜寻任务。

本发明利用水下探测船搭载小型水下机器人进行目标搜索，然后利用 水下探测船进行水下目标抓取，将多机器人系统用于水下未知环境的目标 搜寻中，并使机器人以不同的方向角进行目标搜寻，能够大大提高目标搜 索的效率。利用一种生物触角模型驱动机器人运动的方法来完成多机器人 在未知环境中的目标搜寻任务，可以大大的提高水下机器人目标搜寻的效 率。本发明将声纳传感器采集的数据与D-S信息融合算法相结合，用于实 时更新水下环境栅格地图，能够高效、快速、准确的实现栅格地图环境的 构建和动态更新。本发明利用扩散方程进行路径规划，一旦小型机器人发 现目标，则其他水下机器人和水下探测船采用扩散方程完成对目标位置的 导航，该方法能够高效、快速的规划出一条安全无碰撞到达目标的最优路 径，高效完成水下目标搜索和抓取任务。本发明提高了复杂动态水下环境 中目标搜索效率，具有较高的实际应用价值。