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

Abstract

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

Description

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

技术领域

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

背景技术

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

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

发明内容

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

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

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

(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({\mathrm{\θ}}_r\right)}_{t+1}={\left({\mathrm{\θ}}_r\right)}_t\±\frac{\mathrm{\π}}{4}\mathrm{\ϵ}$

其中，(θ_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{\mathrm{\α}-\left|\mathrm{\θ}\right|}{\mathrm{\α}}\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{\mathrm{\α}-\left|\mathrm{\θ}\right|}{\mathrm{\α}}\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}{\mathrm{\Σ}}}{m}_{\mathrm{Ok}}^t(i,j)}{n}$

$m_E^t(i,j)=\frac{\underset{k=1}{\overset{n}{\mathrm{\Σ}}}{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\⇐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({\mathrm{\θ}}_r\right)}_{t+1}={\left({\mathrm{\θ}}_r\right)}_t\±\frac{\mathrm{\π}}{4}\mathrm{\ϵ}$

其中，(θ_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{\mathrm{\α}-\left|\mathrm{\θ}\right|}{\mathrm{\α}}\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{\mathrm{\α}-\left|\mathrm{\θ}\right|}{\mathrm{\α}}\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}{\mathrm{\Σ}}}{m}_{\mathrm{Ok}}^t(i,j)}{n}$

$m_E^t(i,j)=\frac{\underset{k=1}{\overset{n}{\mathrm{\Σ}}}{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\⇐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信息融合算法相结合，用于实时更新水下环境栅格地图，能够高效、快速、准确的实现栅格地图环境的构建和动态更新。本发明利用扩散方程进行路径规划，一旦小型机器人发现目标，则其他水下机器人和水下探测船采用扩散方程完成对目标位置的导航，该方法能够高效、快速的规划出一条安全无碰撞到达目标的最优路径，高效完成水下目标搜索和抓取任务。本发明提高了复杂动态水下环境中目标搜索效率，具有较高的实际应用价值。

Claims (7)

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

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

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

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

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

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

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

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

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

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

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

2.根据权利要求1所述的基于生物触角模型的多机器人水下目标搜寻方法，其特征在于：所述步骤(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.根据权利要求2所述的基于生物触角模型的多机器人水下目标搜寻方法，其特征在于：步骤(3)中，基于生物触角模型的水下机器人行走过程如下：

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

${\left({\mathrm{\θ}}_r\right)}_{t+1}={\left({\mathrm{\θ}}_r\right)}_t\±\frac{\mathrm{\π}}{4}\mathrm{\ϵ}$

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

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

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

4.根据权利要求3所述的基于生物触角模型的多机器人水下目标搜寻方法，其特征在于：

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

避开条件为：d_min≥R_b；

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

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

5.根据权利要求1所述的基于生物触角模型的多机器人水下目标搜寻方法，其特征在于：步骤(4b)中数据转换方式如下：

在R-d≤r≤R+d区间内：

$m\left(O\right)=\frac{{\left(\frac{\mathrm{\α}-\left|\mathrm{\θ}\right|}{\mathrm{\α}}\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{\mathrm{\α}-\left|\mathrm{\θ}\right|}{\mathrm{\α}}\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是水下机器人最小的安全距离。

6.根据权利要求1所述的基于生物触角模型的多机器人水下目标搜寻方法，其特征在于：步骤(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\left)\right)+(1-m_E^{t-1}(i,j)-m_O^{t-1}(i,j\left)\right)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\left)\right)+(1-m_E^{t-1}(i,j)-m_O^{t-1}(i,j\left)\right)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}{\Σ}}{m}_{Ok}^t(i,j)}{n}$

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

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

7.根据权利要求1所述的基于生物触角模型的多机器人水下目标搜寻方法，其特征在于：所述步骤(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\⇐u_{p_n}=max\{u_m,m=1,2,...,M\}$

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