CN113110455A - 一种未知初始状态的多机器人协同探索方法、装置及系统 - Google Patents

Abstract

本发明提供了一种未知初始状态的多机器人协同探索方法、装置及系统，涉及机器人探索技术领域。本发明所述的未知初始状态的多机器人协同探索方法，包括：获取多机器人的融合环境信息，对所述融合环境信息进行目标提取，建立目标节点；对所述目标节点进行优化和筛选，建立机器人状态与目标节点图；根据所述机器人状态与目标节点图，采用统一的协同策略进行任务决策，完成机器人对目标区域的实时导航控制任务；根据机器人相互通信实现任务的迭代，完成探索任务。本发明所述的技术方案，通过多机器人数据关联实现机器人状态图的估计，填补多机器人探索领域中系统状态初始化的空白，解决了需要人为干预的弊端，提供了一套可靠有效的探索方法。

Description

一种未知初始状态的多机器人协同探索方法、装置及系统

技术领域

本发明涉及机器人探索技术领域，具体而言，涉及一种未知初始状态的多机器人协同探索方法、装置及系统。

背景技术

移动机器人具有机动灵活，重量轻，体积小且可以携带多种传感器进行环境感知的优点，一直都是机器人研究的重点。

现有技术中，通常假设多机器人的状态已知，并对每个机器人使用算法进行单个机器人的状态更新估计，从而对多个机器人的信息进行递增式的估计。然而这样的假设在实际场景以及相关环境的应用上会带来严重的问题。例如救援行动中，利用多机器人实施地下勘探行动时，实际的场景对人与机器人而言都是未知的，在将机器人放到地下环境中后，并不能人为地告知机器人初始状态的信息。因此如何在完全未知的环境下使多个机器人确定自身的状态，以及相对其他机器人的状态成为了目前需要解决的问题。

发明内容

本发明解决的问题是如何实现未知初始状态下的机器人探索任务。

为解决上述问题，本发明提供一种未知初始状态的多机器人协同探索方法，包括：获取多机器人的融合环境信息，对所述融合环境信息进行目标提取，建立目标节点；对所述目标节点进行优化和筛选，建立机器人状态与目标节点图；根据所述机器人状态与目标节点图，采用统一的协同策略进行任务决策，完成机器人对目标区域的实时导航控制任务；根据机器人相互通信实现任务的迭代，完成探索任务。

本发明所述的未知初始状态的多机器人协同探索方法，通过多机器人数据关联实现机器人状态图的估计，填补多机器人探索领域中系统状态初始化的空白，解决了需要人为干预的弊端，为安全救援、反恐、搜救等任务提供了一套可靠有效的探索方法。

可选地，所述获取多机器人的融合环境信息包括：确定每一对机器人之间的相对位姿，根据所述相对位姿确定机器人状态估计图；根据所述机器人状态估计图确定局部地图相对于全局地图的转换矩阵，以根据所述转换矩阵将所述全局地图的栅格位置作为索引，从所有所述局部地图提取所述融合环境信息。

本发明所述的未知初始状态的多机器人协同探索方法，通过相对位姿确定机器人状态估计图以确定局部地图相对于全局地图的转换矩阵，从而从所有局部地图地图信息获得融合环境信息，提高了地图准确性，也提高了探索任务执行效率。

可选地，所述对所述融合环境信息进行目标提取，建立目标节点包括：根据所述融合环境信息确定目标边界；根据所述目标边界确定轮廓图像的几何矩；根据所述几何矩确定边界轮廓的目标点信息，以建立所述目标节点。

本发明所述的未知初始状态的多机器人协同探索方法，根据融合环境信息确定目标边界以确定轮廓图像的几何矩，根据几何矩确定边界轮廓的目标点信息以建立目标节点，获得代表性的目标节点，提升目标点搜索效率，且保证全局搜索完整性，使地图的小角落不会因为生长树采样点过少而丢失。

可选地，所述对所述融合环境信息进行目标提取，建立目标节点还包括：采用基于RRT改进的搜索树方法寻找已知环境和未知环境的边界点。

本发明所述的未知初始状态的多机器人协同探索方法，通过为每个自主移动机器人设置一个搜索树，保障机器人能尽快找到距离其较近的边界点，提高边界点寻找的效率与个数。

可选地，所述对所述目标节点进行优化和筛选包括：根据聚类收敛结果确定局部区域的最优目标，根据所述最优目标更新当前的目标节点以作为所述机器人状态与目标节点图的输入。

本发明所述的未知初始状态的多机器人协同探索方法，根据聚类收敛结果确定局部区域的最优目标更新当前的目标节点以作为机器人状态与目标节点图的输入，在每一时刻进行信息的优化，为机器人团体减小协同策略的计算压力，保障系统的高效性。

可选地，所述根据所述机器人状态与目标节点图，采用统一的协同策略进行任务决策，完成机器人对目标区域的实时导航控制任务包括：根据所述目标节点确定可获得信息，并确定所述目标节点与状态节点的距离以及所述状态节点相对于所述目标节点的转向消耗，根据所述可获得信息、所述距离和所述转向消耗确定所述目标节点与机器人节点的边的权重，选择权重最大的边所对应的节点信息完成所述实时导航控制任务。

本发明所述的未知初始状态的多机器人协同探索方法，通过统一的协同策略进行任务决策，完成机器人对目标区域的实时导航控制任务，有效提高探索任务效率。

可选地，所述根据机器人相互通信实现任务的迭代，完成探索任务包括：根据机器人团队的自身状态和环境信息完成多次信息迭代和策略计算，更新所述机器人状态与目标节点图，发布机器人导航控制任务以完成指定区域的探索任务。

本发明所述的未知初始状态的多机器人协同探索方法，根据机器人团队的自身状态和环境信息完成多次信息迭代和策略计算，更新所述机器人状态与目标节点图，发布机器人导航控制任务以完成指定区域的探索任务，有效提高探索任务效率。

本发明还提供一种未知初始状态的多机器人协同探索装置，包括：节点模块，用于获取多机器人的融合环境信息，对所述融合环境信息进行目标提取，建立目标节点；优化筛选模块，用于对所述目标节点进行优化和筛选，建立机器人状态与目标节点图；导航控制模块，用于根据所述机器人状态与目标节点图，采用统一的协同策略进行任务决策，完成机器人对目标区域的实时导航控制任务；探索模块，用于根据机器人相互通信实现任务的迭代，完成探索任务。所述未知初始状态的多机器人协同探索装置与上述未知初始状态的多机器人协同探索方法相对于现有技术所具有的优势相同，在此不再赘述。

本发明还提供一种未知初始状态的多机器人协同探索系统，包括存储有计算机程序的计算机可读存储介质和处理器，所述计算机程序被所述处理器读取并运行时，实现如上所述的未知初始状态的多机器人协同探索方法。所述未知初始状态的多机器人协同探索系统与上述未知初始状态的多机器人协同探索方法相对于现有技术所具有的优势相同，在此不再赘述。

本发明还提供一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器读取并运行时，实现如上所述的未知初始状态的多机器人协同探索方法。所述计算机可读存储介质与上述未知初始状态的多机器人协同探索方法相对于现有技术所具有的优势相同，在此不再赘述。

附图说明

图1为本发明实施例的未知初始状态的多机器人协同探索方法的示意图；

图2为本发明实施例的生长树的程序流程图。

具体实施方式

在本发明中，探索的含义是指针对某一应用场景，移动机器人能够自主地对环境信息进行覆盖式的搜索，建立完整的地图信息。例如在地质勘探领域，据相关的数据统计，世界上每年至少有数千人死于矿难，中国每年在煤矿事故中的死亡人数更是常年位居世界前列。使用地质探索机器人对地质环境进行提前的勘探，不仅能减小施工人员碰到意外事件的概率，而且可以在发生矿难之后，探察地下环境，提高被困人员的生存几率。移动机器人自主探索的研究在军事领域也有着愈发重要的作用，发生战争时，如果使自主移动机器人首先对城市地下空间环境进行勘探，绘制战略地图，将对战争的形势起着决定性作用。

对于目前大多数探索任务而言，单个机器人工作的效率往往是很低的。受机器人性能和功能的限制，许多场景下，单个机器人都很难独自高效地完成任务。探索区域较大或者空间分布较为复杂的探索场景，例如城市地下环境，大型矿难事故现场等，单个机器人的能力具有明显的局限性。在这些场景中，多机器人系统比单个机器人具有明显的优势，既可以在面对复杂任务时缩短任务执行的时间，也可以通过协同合作提高任务完成效率。

多机器人利用信息共享，完成协作的前提，就是对自身状态以及相对状态的确定。在过去的研究成果中，研究人员们均是首先假设多机器人的状态已知，并对每个机器人使用例如gmapping或者cartographer等slam算法进行单个机器人的状态更新估计，从而对多个机器人的信息进行递增式的估计。然而这样的假设在实际场景以及相关环境的应用上会带来严重的问题。例如救援行动中，利用多机器人实施地下勘探行动时，实际的场景对人与机器人而言都是未知的。在将机器人放到地下环境中后，并不能人为地告知机器人初始状态的信息。为了解决初始状态未知的问题，在本发明中，提出了基于多机器人之间数据关联，使用优化的方法进行多机器人状态图的估计，从而解决未知初始状态下机器人状态估计图的初始化与运行时检测更新问题。

自主探索的过程本质上是每个机器人根据环境信息与目标信息进行自主选择与导航控制的过程。目标点和各个机器人处在空间中不同的位置却又相互影响。为了建立统一的决策过程，达到提高合理性与探索效率的目的，在本发明中建立机器人状态与目标节点图来描述这种空间关系与决策影响过程。在机器人状态与目标节点图中,机器人节点Nodes包含机器人的状态，目标节点Nodet包含目标点的信息，每个节点都将存储其当前位置信息，边Edgess表示机器人与机器人之间的相对关系；边Edgest表示目标点相对于机器人的信息，边Edgett表示目标点与目标点的相对关系。机器人协同决策的过程就是完成各个节点存储信息的更新与每条边对应权重计算。

为使本发明的上述目的、特征和优点能够更为明显易懂，下面结合附图对本发明的具体实施例做详细的说明。

如图1所示，本发明实施例提供一种未知初始状态的多机器人协同探索方法，包括：获取多机器人的融合环境信息，对所述融合环境信息进行目标提取，建立目标节点；对所述目标节点进行优化和筛选，建立机器人状态与目标节点图；根据所述机器人状态与目标节点图，采用统一的协同策略进行任务决策，完成机器人对目标区域的实时导航控制任务；根据机器人相互通信实现任务的迭代，完成探索任务。

具体地，在本实施例中，未知初始状态的多机器人协同探索方法包括：获取多机器人的融合环境信息，对融合环境信息进行目标提取，建立目标节点。对环境进行协同感知是机器人团体完成协同任务的基础，协同感知又需要单个机器人的环境感知能力以及每个机器人之间的相对位姿估计与融合环境信息。为了获得融合环境信息，在本实施例中，提出了一种新的地图融合方式，即在状态估计图基础之上创建一张以全局地图栅格位置为索引，从所有局部地图提取信息的方式。对每一时刻多机器人融合环境信息进行目标提取，建立目标节点。操作人员只需指定机器人探索任务的区域，然后系统就可以自动利用多机器人数据关联，完成优化的位姿估计方法，在任务目标范围内开始初始位姿的解算以及机器人状态估计图与融合环境信息的初始化。

对目标节点进行优化和筛选，建立机器人状态与目标节点图。在目标节点的迭代计算中，会不断产生冗余的目标信息。为了保障系统的高效性，在每一时刻进行信息的优化，为机器人团体减小协同策略的计算压力。

根据机器人状态与目标节点图，采用统一的协同策略进行任务决策，完成机器人对目标区域的实时导航控制任务。

根据机器人相互通信实现任务的迭代，完成探索任务。本实施例提出了一种基于多机器人数据关联对机器人状态图进行估计的方法，弥补了未知初始状态下多机器人探索领域中系统状态初始化的空白，即提供了未知初始状态下，全自主的多机器人探索技术。解决了需要人为干预的弊端，为安全救援、反恐、搜救等任务提供了一套可靠有效的探索方法及系统，并可以为系统提供状态检测方法，提高了机器人状态估计的鲁棒性。

在本实施例中，通过多机器人数据关联实现机器人状态图的估计，填补多机器人探索领域中系统状态初始化的空白，解决了需要人为干预的弊端，为安全救援、反恐、搜救等任务提供了一套可靠有效的探索方法。

可选地，所述获取多机器人的融合环境信息包括：确定每一对机器人之间的相对位姿，根据所述相对位姿确定机器人状态估计图；根据所述机器人状态估计图确定局部地图相对于全局地图的转换矩阵，以根据所述转换矩阵将所述全局地图的栅格位置作为索引，从所有所述局部地图提取所述融合环境信息。

具体地，在本实施例中，获取多机器人的融合环境信息包括：确定每一对机器人之间的相对位姿，根据相对位姿确定机器人状态估计图；根据机器人状态估计图确定局部地图相对于全局地图的转换矩阵，以根据转换矩阵将全局地图的栅格位置作为索引，从所有局部地图提取融合环境信息。其中，融合环境信息指的是多机器人融合的环境信息。

当机器人团队开始工作任务时，单个机器人通过gmapping算法能够得到局部地图。设机器人的数量为N,每个机器人得到的局部地图为map_i(i＝1，2...N)，map_i的参考坐标系为frame_i(i＝1，2...N)，此时每个机器人之间的相对状态未知。设初始化时第i个机器人的环境信息为map_i,坐标为frame_i，第j个机器人的激光点云数量为K，每个点在frame_j的中的坐标为

则机器人j相对于机器人i的最优位姿解ξ_ij ^*可以表示为：

其中，M_i(x)表示坐标x的地图占用概率，由map_i经过双立方插值平滑而得。

为根据ξ_ij计算的转换矩阵，可以表示为：

此优化问题求解的目标为，给出初始位姿ξ₀,定义并估计Δξ,并使得机器人j的观测在机器人i的局部信息中观测误差最小，即

其中，初始位姿ξ₀的估计在此优化问题中至关重要，较差的初值可能使优化问题陷入局部极值。较好的初值则会使优化过程更快地收敛到最优解。

机器人j相对于机器人i的初始位姿ξ₀的计算可以表示为：

其中M_{i_nearest}(x)表示map_i中距离坐标点x最近栅格点的值，w表示ξ₀可能存在的范围，即设定的x_ij,y_ij,θ_ij的范围。

为根据ξ_ij计算的转换矩阵。

通过比较所有可行解来求解最优初始位姿ξ₀，即根据x_ij,y_ij,θ_ij的范围与搜索的步长，从机器人j相对于机器人i的相对位姿ξ_ij的所有可行解中选出最优解。由于这是一个从可行解空间进行搜索，有约束条件的最优化问题，所以可用分枝限定树的数据结构加快ξ₀的计算。

最优初始位姿ξ₀由于步长设置的限制，导致它的最优解的精度限制在搜索空间分割的精度。因此以ξ₀为初值进行优化，既避免陷入局部极值，又能得到更精确的机器人j相对于机器人i的最优位姿解ξ_ij ^*。

根据估计出的位姿初值，将机器人j的观测在机器人i的局部信息中观测误差f(ξ+Δξ)通过一阶泰勒展开，得：

由观测误差的泰勒展开结果对Δξ进行求导，并令其等于0，通过化简后为：

因此可以得到变量ξ迭代的更新值为：

其中，H的计算公式为：

设第j个机器人的第k个点云在frame_j坐标系中x,y坐标分别表示为

和

则

的计算公式为：

通过对于目标函数的优化，当相对位姿的增量Δξ在经过多次迭代变得足够小时，就可以得到最终优化结果ξ_ij ^*,即机器人j相对于机器人i的最优位姿解。按照上述方式解出每一对机器人之间的相对位姿，从而得到机器人团队的状态估计图。

为了获得融合环境信息，在本实施例中提出了一种新的地图融合方式，即在状态估计图基础之上创建一张以全局地图栅格位置为索引，从所有局部地图提取信息的方式。全局地图M₀的表示方式为：

其中，x_ij表示全局地图中坐标为(i，j)的栅格点,N为机器人的数量,M_i(i＝1，2...N)为各个局部地图，T_0i为局部地图相对于全局地图的转换矩阵，由机器人状态估计图得出。此方法基于机器人状态估计图，相比于基于特征点法的优势在于目的性更强，处理速度更快且避免了基于特征点法产生误匹配的弊端。

在本实施例中，通过相对位姿确定机器人状态估计图以确定局部地图相对于全局地图的转换矩阵，从而从所有局部地图地图信息获得融合环境信息，提高了地图准确性，也提高了探索任务执行效率。

可选地，所述对所述融合环境信息进行目标提取，建立目标节点包括：根据所述融合环境信息确定目标边界；根据所述目标边界确定轮廓图像的几何矩；根据所述几何矩确定边界轮廓的目标点信息，以建立所述目标节点。

具体地，在本实施例中，对融合环境信息进行目标提取，建立目标节点包括：根据融合环境信息确定目标边界；根据目标边界确定轮廓图像的几何矩；根据几何矩确定边界轮廓的目标点信息，以建立目标节点。

首先根据ROS机器人操作系统的信息传输格式将融合图像转换为障碍物图像的表示方式，假设每一时刻ROS传输的数据为data,则转化得到的图像image可以表示为：

目标边界是指已知空闲环境和未知环境之间的边界。处理中剔除其他不是目标边界的边缘信息，为此，将地图图像进行如下处理：

对图像信息中的第i行，第j列元素，进行障碍物提取：

对预处理对图像进行障碍物的膨胀，在图像形态学操作中，利用A⊕B表示集合A用结构元素B膨胀，定义为：

对障碍物进行膨胀操作的目的在于区分障碍物产生的边界与目标边界。将障碍物膨胀后，由障碍物产生的边界位置发生变化，便于后续的分离。由于一个障碍物四周都会产生边界区域，所以必须用一个向四周扩展的卷积核对图像进行处理。所以取卷积核B为以下形式，然后作用于障碍物图像信息。

一般而言，障碍物与环境中已知和未知区域都会出现梯度值较大的信息，

所以本实施例中选择Canny算子对原图像边缘信息进行提取。经过上述处理步骤之后，再对障碍物膨胀后各像素取反，再与地图所有边界信息取交集等简单操作，就可以得到目标边界。

为了使移动机器人能够尽可能的探索更大面积的未知区域，从每个边界区域中计算几何中心点，使目标点处于未知区域的中心入口。

对目标边界计算质心的计算步骤为：

计算轮廓图像的几何矩：

其中，x，y分别表示图像中的像素横坐标与纵坐标位置，p+q＜4，表示是图像的几何矩((p+q)阶矩)，f(x，y)表示轮廓的像素点。

计算轮廓的质心坐标(x_o，y_o)即可得到该边界轮廓的目标点信息。

利用以上直接处理融合信息的方式，可以快速获得代表性的目标节点，且节点稀疏地分布在边界区域。

在本实施例中，根据融合环境信息确定目标边界以确定轮廓图像的几何矩，根据几何矩确定边界轮廓的目标点信息以建立目标节点，获得代表性的目标节点，提升目标点搜索效率，且保证全局搜索完整性，使地图的小角落不会因为生长树采样点过少而丢失。

可选地，所述对所述融合环境信息进行目标提取，建立目标节点还包括：采用基于RRT改进的搜索树方法寻找已知环境和未知环境的边界点。

具体地，在本实施例中，对所述融合环境信息进行目标提取，建立目标节点还包括：采用基于RRT改进的搜索树方法寻找已知环境和未知环境的边界点。结合图2所示，在系统初始化成功后，初始化随机树，输入初始节点q_start，当机器人正常工作时，则进入循环。进入生长树循环，在地图空间中生成一个随机节点q_rand，在生长树已有的节点中找到距离其最近的节点q_near，沿q_new和q_rand方向上生成与q_near节点距离为η的节点q_new。对q_new进行边界点检测。若检测为边界点，则将其用话题信息发布出去，且检测到一次就对该机器人重新初始化生长树。若检测不为边界点，对q_near到q_new做障碍物检测，如果没有生长到障碍物区域，则将q_new作为一个节点添加到树中，如此循环，直至遍历整个环境空间。通过为每个自主移动机器人设置一个搜索树，保障机器人能尽快找到距离其较近的边界点，提高边界点寻找的效率与个数，与上述目标边界点提取方法相互协调，提升目标点搜索效率。根据实验场景的大小，可设置生长参数η＝0.3，保证实验的速度与场景的精度。

在本实施例中，通过为每个自主移动机器人设置一个搜索树，保障机器人能尽快找到距离其较近的边界点，提高边界点寻找的效率与个数。

可选地，所述对所述目标节点进行优化和筛选包括：根据聚类收敛结果确定局部区域的最优目标，根据所述最优目标更新当前的目标节点以作为所述机器人状态与目标节点图的输入。

具体地，在本实施例中，对目标节点进行优化和筛选包括：根据聚类收敛结果确定局部区域的最优目标，根据最优目标更新当前的目标节点以作为机器人状态与目标节点图的输入。在目标点的寻找过程中，可能出现目标点的过分冗余，正确的筛选过程可以降低机器人状态与目标节点图的复杂度。Mean Shift Clustering算法是一种非参数的聚类算法，它假设不同簇类的数据集分别符合各自的概率密度分布。样本密度高的区域是该分布的对应最大值，通过使样本密度向增大最快的方向移动，最终收敛到局部密度最大值，且收敛到相同局部最大值的点被认为是同一簇类的成员。此算法用核函数来估计样本的密度。它在数据集每一个样本点上都设置一个核函数，然后进行累加得到数据集的核密度估计。

利用Mean Shift Clustering算法，采用如下K(x)进行核密度估计。

数据集中的每一个样本在迭代中都将向它所在的聚类中心移动，直到稳定地收敛到同一个值。所有聚类到同一中心m(x)的点都属于同一簇类。m(x)的公式为：

将最后收敛的结果作为局部区域的最优目标，更新当前的目标节点，为机器人状态与目标节点图提供输入。核函数的参数h，称为核密度估计的带宽。采用不同的带宽，概率密度将有不同的局部最大值个数，由此产生不同的聚类中心个数，影响聚类的结果。通常而言，当带宽越小时，产生的聚类中心也将越多。因此可以通过合适的核函数模型与带宽参数，对机器人目标节点进行合理筛选，根据环境的大小以及分布情况可调整聚类核函数的带宽h＝0.7m。

在本实施例中，根据聚类收敛结果确定局部区域的最优目标更新当前的目标节点以作为机器人状态与目标节点图的输入，在每一时刻进行信息的优化，为机器人团体减小协同策略的计算压力，保障系统的高效性。

可选地，所述根据所述机器人状态与目标节点图，采用统一的协同策略进行任务决策，完成机器人对目标区域的实时导航控制任务包括：根据所述目标节点确定可获得信息，并确定所述目标节点与状态节点的距离以及所述状态节点相对于所述目标节点的转向消耗，根据所述可获得信息、所述距离和所述转向消耗确定所述目标节点与机器人节点的边的权重，选择权重最大的边所对应的节点信息完成所述实时导航控制任务。

具体地，在本实施例中，根据机器人状态与目标节点图，采用统一的协同策略进行任务决策，完成机器人对目标区域的实时导航控制任务包括：根据目标节点确定可获得信息，并确定目标节点与状态节点的距离以及状态节点相对于目标节点的转向消耗，根据可获得信息、距离和转向消耗确定目标节点与机器人节点的边的权重，选择权重最大的边所对应的节点信息完成实时导航控制任务。

对于目标节点t_i而言，完成空间探索任务时，它是引导机器人前往的动力。在具体定义这个节点时，应包含机器人前往此处可获得的信息量。然而此信息量将受到边Edgett，即其他目标点的影响。例如当一个区域只有一个目标点时，它应该是优先级更高的点；当它周围有很多与它接近的目标点时，它的优先级就会下降。根据机器人传感器的范围，设定一个可获得信息半径R。则节点t_i存储的可获得信息(I_i)由参数R及环境状态决定，其计算公式为:

其中，y_i表示以目标节点t_i为圆心，R为半径的圆形区域，N表示机器人的个数，n表示环境信息未知的区域，S(x)表示区域x内的栅格数量，resolution为设定的地图分辨率大小。由此达到目标节点共同作用，相互影响的目的。

对于机器人状态节点s_i而言，相对于每个目标节点的距离将是影响其决策重要因素，距离较近的目标点应该具有更高的优先级。因此s_i将存储到每个目标点的距离值D_i。设机器人位置坐标为(r₀，r₁)，目标点为(f₀，f₁)。则机器人状态节点s_i与目标节点t_j距离大小(distance)可表示为：

公式中的D_max，为环境中距离可取得的最大值(不同的目标节点具有不同的角标表示)。

如果决策过程只考虑可获得信息与距离因素，可能会造成1)经历减速和大角度转向的过程；2)快速转向造成丢失关键帧而丢失位姿，需要重新估计位姿。因此，在机器人状态节点s_i中引入了当前的速度，对转向消耗进行计算。考虑机器人是动态的，航向会随导航路径和目标点变化而实时变动。在ROS中坐标系角度旋转的变换表示为四元数形式，设Z-Y-X欧拉角中，绕固定坐标系的X-Y-Z旋转的角度分别是α，β，γ，则四元数信息q＝(w，x，y，z)转化为欧拉角的公式为：

取Z轴的转向大小γ即可得到速度方向，从而根据速度大小可得到速度信息。设机器人的速度向量为(v_x，v_y)，机器人位置为(r₀，r₁)，目标点为(f₀，f₁)。机器人状态节点s_i相对于目标节点t_j转向消耗为T_ij，则T_ij的计算公式为：

t＝arccos(ct)

目标点节点与机器人节点的边Edgest将按周期计算该边对应的权重。从机器人状态节点s_i中获得D_ij与T_ij的值，从目标节点t_j获得I_j的值，计算该边的权重：

weight_ij＝I_j-D_ij-T_ij

例如设置可获得信息半径R＝1m，可根据设置的任务范围向量，以及信息半径的大小，计算出目标点量化指标的最大值：

D_max＝||(x_max，y_max)-(x_mimy_mim)||₂

T_max＝π

I_max＝2πR²

最终将在循环的决策过程中，挑选权重最大的边对应的节点信息进行导航控制任务。

在确定每个机器人的目标信息后，选用全局路径规划算法和局部路径规划算法对机器人进行导航。将局部路径规划算法输出的速度指令发送给机器人下位机，控制底盘进行机器人导航，完成机器人对于目标点导航的控制任务。例如选取A*算法完成全局路径规划，启发函数h(n)的选择曼哈顿距离，满足可纳性、一致性和目标状态的h(n)＝0，保证这一路线一定是最优解。

ManhattanDistance(N1，N2)＝|N1_x-N2_x|+|N1_y-N2_y|

选用DWA算法作为局部路径规划算法，通过采样路径的打分，选择轨迹评价中获得最小值的一组轨迹与速度样本，将速度指令发送给机器人下位机，控制底盘进行机器人导航，完成机器人的导航控制任务。

机器人协同决策的过程就是完成各个节点存储信息的更新与每条边对应权重计算。

在本实施例中，通过统一的协同策略进行任务决策，完成机器人对目标区域的实时导航控制任务，有效提高探索任务效率。

可选地，所述根据机器人相互通信实现任务的迭代，完成探索任务包括：根据机器人团队的自身状态和环境信息完成多次信息迭代和策略计算，更新所述机器人状态与目标节点图，发布机器人导航控制任务以完成指定区域的探索任务。

具体地，在本实施例中，根据机器人相互通信实现任务的迭代，完成探索任务包括：根据机器人团队的自身状态和环境信息完成多次信息迭代和策略计算，更新机器人状态与目标节点图，发布机器人导航控制任务以完成指定区域的探索任务。在实例中，机器人团队根据动态的自身状态与环境信息完成多次的信息迭代与策略计算。采用ROS操作系统统一的消息格式对各个信息完成统一度量，在任务执行中更新机器人状态与目标节点图，发布机器人导航控制任务，并周期性判断是否完成了指定区域的探索。最终实现机器人协同探索的任务。

在本实施例中，根据机器人团队的自身状态和环境信息完成多次信息迭代和策略计算，更新所述机器人状态与目标节点图，发布机器人导航控制任务以完成指定区域的探索任务，有效提高探索任务效率。

本发明另一实施例提供一种未知初始状态的多机器人协同探索装置，包括：节点模块，用于获取多机器人的融合环境信息，对所述融合环境信息进行目标提取，建立目标节点；优化筛选模块，用于对所述目标节点进行优化和筛选，建立机器人状态与目标节点图；导航控制模块，用于根据所述机器人状态与目标节点图，采用统一的协同策略进行任务决策，完成机器人对目标区域的实时导航控制任务；探索模块，用于根据机器人相互通信实现任务的迭代，完成探索任务。

本发明另一实施例提供一种未知初始状态的多机器人协同探索系统，包括存储有计算机程序的计算机可读存储介质和处理器，所述计算机程序被所述处理器读取并运行时，实现如上所述的未知初始状态的多机器人协同探索方法。

本发明另一实施例提供一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器读取并运行时，实现如上所述的未知初始状态的多机器人协同探索方法。

虽然本发明公开披露如上，但本发明公开的保护范围并非仅限于此。本领域技术人员在不脱离本发明公开的精神和范围的前提下，可进行各种变更与修改，这些变更与修改均将落入本发明的保护范围。

Claims (10)

1.一种未知初始状态的多机器人协同探索方法，其特征在于，包括：

获取多机器人的融合环境信息，对所述融合环境信息进行目标提取，建立目标节点；

对所述目标节点进行优化和筛选，建立机器人状态与目标节点图；

根据所述机器人状态与目标节点图，采用统一的协同策略进行任务决策，完成机器人对目标区域的实时导航控制任务；

根据机器人相互通信实现任务的迭代，完成探索任务。

2.根据权利要求1所述的未知初始状态的多机器人协同探索方法，其特征在于，所述获取多机器人的融合环境信息包括：

确定每一对机器人之间的相对位姿，根据所述相对位姿确定机器人状态估计图；

根据所述机器人状态估计图确定局部地图相对于全局地图的转换矩阵，以根据所述转换矩阵将所述全局地图的栅格位置作为索引，从所有所述局部地图提取所述融合环境信息。

3.根据权利要求1所述的未知初始状态的多机器人协同探索方法，其特征在于，所述对所述融合环境信息进行目标提取，建立目标节点包括：

根据所述融合环境信息确定目标边界；

根据所述目标边界确定轮廓图像的几何矩；

根据所述几何矩确定边界轮廓的目标点信息，以建立所述目标节点。

4.根据权利要求3所述的未知初始状态的多机器人协同探索方法，其特征在于，所述对所述融合环境信息进行目标提取，建立目标节点还包括：

采用基于RRT改进的搜索树方法寻找已知环境和未知环境的边界点。

5.根据权利要求1所述的未知初始状态的多机器人协同探索方法，其特征在于，所述对所述目标节点进行优化和筛选包括：

根据聚类收敛结果确定局部区域的最优目标，根据所述最优目标更新当前的目标节点以作为所述机器人状态与目标节点图的输入。

6.根据权利要求1所述的未知初始状态的多机器人协同探索方法，其特征在于，所述根据所述机器人状态与目标节点图，采用统一的协同策略进行任务决策，完成机器人对目标区域的实时导航控制任务包括：

根据所述目标节点确定可获得信息，并确定所述目标节点与状态节点的距离以及所述状态节点相对于所述目标节点的转向消耗，根据所述可获得信息、所述距离和所述转向消耗确定所述目标节点与机器人节点的边的权重，选择权重最大的边所对应的节点信息完成所述实时导航控制任务。

7.根据权利要求1所述的未知初始状态的多机器人协同探索方法，其特征在于，所述根据机器人相互通信实现任务的迭代，完成探索任务包括：

根据机器人团队的自身状态和环境信息完成多次信息迭代和策略计算，更新所述机器人状态与目标节点图，发布机器人导航控制任务以完成指定区域的探索任务。

8.一种未知初始状态的多机器人协同探索装置，其特征在于，包括：

节点模块，用于获取多机器人的融合环境信息，对所述融合环境信息进行目标提取，建立目标节点；

优化筛选模块，用于对所述目标节点进行优化和筛选，建立机器人状态与目标节点图；

导航控制模块，用于根据所述机器人状态与目标节点图，采用统一的协同策略进行任务决策，完成机器人对目标区域的实时导航控制任务；

探索模块，用于根据机器人相互通信实现任务的迭代，完成探索任务。

9.一种未知初始状态的多机器人协同探索系统，其特征在于，包括存储有计算机程序的计算机可读存储介质和处理器，所述计算机程序被所述处理器读取并运行时，实现如权利要求1至7任一项所述的未知初始状态的多机器人协同探索方法。

10.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器读取并运行时，实现如权利要求1至7任一项所述的未知初始状态的多机器人协同探索方法。

Images