一种异构机器人空地协同搜索救援的方法

技术领域

本公开实施例涉及异构机器人空地协同技术领域，尤其涉及一种异构机器人空地协同搜索救援的方法。

背景技术

相较于单机器人系统，多机器人协同系统具有功能强大、空间分布广、任务执行效率高、容错能力强等优势，现已成功应用于搜索救援、安防巡逻、军事作战、智慧农业等场景。空地协同是一种典型的多机器人系统，空中机器人(如无人机)往往具有机动灵活的优势，具有广阔的视角与探测范围，能够快速覆盖大面积区域；地面机器人(如无人车)负载较大、续航时间长，可携带更多的高算力机载设备执行任务。将空中与地面互补的优势结合得到空地异构多机器人协作系统，其协作效率更高、鲁棒性更强，更加适应复杂动态变换的环境。搜索与救援(search and rescue，SAR)一直以来均受到人类的关注，研究环境自适应能力更强的空地协同系统是目前的研究热点。

空地协同系统是多机器人协同系统中备受关注的研究领域，主要包括三种类型：无人车支援无人机、无人机支援无人车、无人机和无人车共同执行任务。第一种是无人车支援无人机问题，无人机在无人车上的着陆为其中重点研究方向。第二种是无人机支援无人车问题，无人机使用自身的传感器来提供无人车所需的功能辅助。第三种是无人机/无人车共同作业，需要将无人机和无人车的传感器优势综合起来，协同完成任务。目前，无人机和无人车在不同视角下观察的目标信息不同，导致目标物识别的准确度不够准确。

发明内容

为了避免现有技术的不足之处，本申请提供一种异构机器人空地协同搜索救援的方法，用以解决现有技术中存在无人机和无人车在不同视角下观察的目标信息不同，导致目标物识别的准确度不够准确的问题。

根据本公开实施例的第一方面，提供一种异构机器人空地协同搜索救援的方法，该方法包括：

利用空中机器人感知地面环境，以得到初始全局地图；

边缘节点利用SAM算法对所述初始全局地图进行语义分割，以得到目标全局地图；

所述边缘节点对所述目标全局地图进行处理，以转化成栅格地图；

基于所述栅格地图，针对地面机器人依次利用AMCL算法进行自主定位、利用A*算法进行全局路径规划、利用DWA算法进行局部路径规划和在所述栅格地图上添加膨胀层以对所述地面机器人的转向进行保护，以使所述搜索救援能够完成搜索救援。

所述SAM算法的核心模块包括：

任务、模型和数据；

其中，所述任务为通过提供图片以及多个提示来分割出所需区域，所述提示至少包含描述文本，空间中的点、方框，以及掩码。

所述模型包括：

Image Encoder、Prompt Encoder和Mask Encoder；

所述Image Encoder的内部是用MAE训练的ViT模型，所述Image Encoder每幅图像运行一次；

所述Prompt Encoder先将所述提示分为稀疏的提示和密集的提示，对于所述稀疏的提示，使用位置编码结合学习到的embedding，对于所述密集的提示，通过卷积嵌入并于图像元素相加；

所述Mask Encoder有效的将图像embedding、提示embedding和输出token映射到所述掩码。

所述模型的解码器基于Transformer的解码器块修改，在所述解码器后添加了动态掩码预测头，所述解码器块使用了提示自注意力和双向的交叉注意力来更新所有的embedding。

所述边缘节点对所述目标全局地图进行处理，以转化成栅格地图的步骤中，还包括：

根据所述目标全局地图和所述栅格地图建立映射关系：

其中，g(x,y)表示坐标(x,y)处的栅格地图单元值，m为第m个像素；

所述栅格地图存储格式为.pgm，所述目标全局地图存储格式为.png，这两种格式都是从左上角开始存储数据，将所述栅格地图的每个单元与所述目标全局地图中每个像素一一对应，则所述栅格地图的每个单元存储的状态值为：

g′(k)＝i*g(x,y)+j

其中，g′(k)表示栅格地图在一维存储下的值，k为一维存储下的索引，i为二维存储下的行数，j为列数；

在所述栅格地图中找到参照物并明确所述参照物的实际尺寸。

所述针对地面机器人依次利用AMCL算法进行自主定位的步骤中，包括：

预测、更新和重采样；

在所述预测中，根据所述地面机器人上一时刻的状态、控制信息和运动模型，预测所述地面机器人当前时刻的状态；

在所述更新中，使用传感器信息和地图信息，计算每个粒子的权重；

在所述重采样中，根据所述粒子的权重，选择一部分粒子并对其进行复制和修正，以维持粒子集大小不变；

多次进行所述更新和所述重采样的循环，所述粒子的权重则更符合实际情况，最终得到所述地面机器人的位置估计结果。

所述利用A*算法进行全局路径规划的步骤中，包括：

使用一个评估函数来为每个搜索节点赋予一个值，该值代表从起始点到目标点的预估成本；所述评估函数通常表示为：

f(n)＝g(n)+h(n)

其中：f(n)是节点n的总成本，g(n)是从起始点到节点n的实际成本，h(n)是从节点n到目标点的估计成本；

所述A*算法的工作原理如下：

1)将起始节点放入一个开放列表(通常使用优先队列实现)；

2)从开放列表中选择具有最低f值的节点；

3)对该节点进行扩展，即查找其所有邻居节点；

4)对于每个邻居节点，计算其g值和h值，并更新其f值；

5)如果邻居节点不在开放列表和关闭列表中，将其添加到开放列表中；

6)如果邻居节点已经在开放列表中，并且新的g值更小，则更新其g值；

7)如果当前节点是目标节点，算法结束；

8)否则，将当前节点移动到关闭列表，并返回步骤2)。

所述利用DWA算法进行局部路径规划的步骤中，包括：

动态窗口：定义所述地面机器人在一个预设的时间段能够达到的速度范围；

速度采样：在所述动态窗口内，对线速度和角速度进行采样，生成若干个速度命令；

评估函数：对于每一个所述速度命令，使用一个所述评估函数计算评估得分，以评估所述速度命令的优劣性；

选择最佳速度命令：选择所述评估得分最高的所述速度命令作为下一个动作；

反馈控制：所述地面机器执行选择的所述速度命令，并根据环境的反馈进行调整。

所述评估函数标准如下：

到目标的距离：所述速度命令使所述地面机器人更接近目标；

机器人的速度：所述速度命令使所述地面机器人保持在一个预设的速度；

避障：所述速度命令使所述地面机器人能够避免与与障碍物碰撞。

本公开的实施例提供的技术方案可以包括以下有益效果：

本公开的实施例中，通过上述异构机器人空地协同搜索救援的方法，一方面，在搜救范围确定的前提下，空中机器人利用全局的视野优势来感知地面环境，向下俯视拍摄得到全局范围的初始全局地图，空中机器人将初始全局地图传输给边缘节点，边缘节点利用SAM算法对初始全局地图进行语义分割以得到目标全局地图，边缘节点再对目标全局地图进行处理最终转换成栅格地图，并将栅格地图传输给地面机器人。地面机器人根据栅格地图，利用AMCL算法进行自主定位、利用A*算法进行全局路径规划、利用DWA Local Planner算法(即DWA算法)进行局部路径规划和在栅格地图上添加膨胀层以对地面机器人的转向进行保护，以选定前往搜救的目标点进行路径规划，并利用其局部精准定位优势导航达到目标点完成搜救。另一方面，空中机器人和地面机器人在不同视角下观察的目标信息不同，通过视角的变换以及几何的相关性实现二者信息互补，大大地提高了目标物识别的准确度。

附图说明

此处的附图被并入说明书中并构成本说明书的一部分，示出了符合本公开的实施例，并与说明书一起用于解释本公开的原理。显而易见地，下面描述中的附图仅仅是本公开的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1示出本公开示例性实施例中一种异构机器人空地协同搜索救援的方法的步骤图；

图2示出本公开示例性实施例中空地协同框架图；

图3示出本公开示例性实施例中使用SAM算法进行基于提示的分割任务；

图4示出本公开示例性实施例中模型分割状态结点构建阶段两台设备下构建的模型分割状态图。

具体实施方式

现在将参考附图更全面地描述示例实施方式。然而，示例实施方式能够以多种形式实施，且不应被理解为限于在此阐述的范例；相反，提供这些实施方式使得本公开将更加全面和完整，并将示例实施方式的构思全面地传达给本领域的技术人员。所描述的特征、结构或特性可以以任何合适的方式结合在一个或更多实施方式中。

此外，附图仅为本公开实施例的示意性图解，并非一定是按比例绘制。图中相同的附图标记表示相同或类似的部分，因而将省略对它们的重复描述。附图中所示的一些方框图是功能实体，不一定必须与物理或逻辑上独立的实体相对应。

本示例实施方式中首先提供了一种异构机器人空地协同搜索救援的方法。参考图1中所示，该异构机器人空地协同搜索救援的方法可以包括：步骤S101和步骤S104。

步骤S101：利用空中机器人感知地面环境，以得到初始全局地图；

步骤S102：边缘节点利用SAM算法对所述初始全局地图进行语义分割，以得到目标全局地图；

步骤S103：所述边缘节点对所述目标全局地图进行处理，以转化成栅格地图；

步骤S104：基于所述栅格地图，针对地面机器人依次利用AMCL算法进行自主定位、利用A*算法进行全局路径规划、利用DWA算法进行局部路径规划和在所述栅格地图上添加膨胀层以对所述地面机器人的转向进行保护，以使所述搜索救援能够完成搜索救援。

通过上述异构机器人空地协同搜索救援的方法，一方面，在搜救范围确定的前提下，空中机器人利用全局的视野优势来感知地面环境，向下俯视拍摄得到全局范围的初始全局地图，空中机器人将初始全局地图传输给边缘节点，边缘节点利用SAM算法对初始全局地图进行语义分割以得到目标全局地图，边缘节点再对目标全局地图进行处理最终转换成栅格地图，并将栅格地图传输给地面机器人。地面机器人根据栅格地图，利用AMCL算法进行自主定位、利用A*算法进行全局路径规划、利用DWA Local Planner进行局部路径规划和在栅格地图上添加膨胀层以对地面机器人的转向进行保护，以选定前往搜救的目标点进行路径规划，并利用其局部精准定位优势导航达到目标点完成搜救。另一方面，空中机器人和地面机器人在不同视角下观察的目标信息不同，通过视角的变换以及几何的相关性实现二者信息互补，大大地提高了目标物识别的准确度。

下面，将参考图1至图4对本示例实施方式中的上述异构机器人空地协同搜索救援的方法的各个步骤进行更详细的说明。

在步骤S101中，构建空地协同框架：

如图2所示，该空地协同框架包括空中机器人、地面机器人和边缘节点。其中，在搜救范围确定的前提下，无人机利用全局的视野优势来感知地面环境，向下俯视拍摄得到全局范围的地图。将全局地图传输给边缘节点，边缘节点对全局地图进行处理，并最终转换成栅格地图，传输给地面的无人车。无人车根据地图，选定前往搜救的目标点进行路径规划，利用其局部精准定位优势导航达到目标点完成搜救。

在步骤S102中，空中机器人构建目标全局地图：

步骤S102在边缘节点上进行。采用Segment Anything Model(SAM)算法来对无人机航拍图像进行语义分割，能够实现没有先验输入的情况下也能正确的识别出障碍物和可通行道路。

SAM具有超强的零样本泛化能力。SAM的核心模块分别是任务、模型和数据，下面将对任务和模型进行介绍。

SAM的任务是通过提供图片以及各种各样的提示(prompt)来分割出所需区域，提示可以包含：描述文本，空间中的点、方框等。在提示不明确的情下，可能存在多个对象，比如：在空间中点一个点，这个点落在衣服上，所以对象有可能是衣服、穿衣服的人或衣服上的图案。模型会被要求至少能合理的分割其中一个对象，任务和模型如图3所示。在预训练阶段，将这些提示和图片输入到模型中训练得到分割结果，然后和真实的结果进行对比。这种训练方法十分有效，生成了具有零样本泛化能力的图像分割器，甚至能够处理模糊的提示。同时，想要得到十分好的效果，还需要独特的模型结构。

SAM的模型结构由三部分组成：Image Encoder、Prompt Encoder和Mask Encoder。Image Encoder内部是用MAE训练的ViT模型，该图像编码器每幅图像运行一次，在提示到来之前就可以使用。对于Prompt Encoder，首先将提示分为稀疏的(点、方框、文本)和密集的(掩码)。对于点和方框，使用位置编码结合学习到的embedding；对于文本使用CLIP编码器；对于密集的提示(掩码)，通过卷积嵌入并于图像元素相加。Mask Encoder可以有效的将图像embedding、提示embedding和输出token映射到掩码。该模型的解码器基于Transformer的解码器块修改，在解码器后添加了动态掩码预测头。解码器块使用了提示自注意力和双向的交叉注意力(prompt-to-image embedding，image-to-prompt embedding)来更新所有的embedding。在经过两个解码器块之后，对图像进行上采样，再使用多层感知机将输出映射到动态线性分类器上，最终得出每个图像位置的掩码前景概率。对于模糊的提示，会输出三种有效的掩码供用户选择。在训练时，只反向传播损失函数最小的掩码。模型会输出置信分数(如IoU)来对掩码效果进行排名。

在无人机航拍照片上均匀选取32*32个点(prompt)，总共生成3072(32*32*3)个掩码，再通过筛选掩码之间的重叠度高于阈值和掩码与真实图像的IoU低于阈值的掩码，最终得到65个掩码。观察生成的各种掩码图，发现第十九和第二十张照片正好对应道路的右侧和左侧(由于原图道路左右侧的连接处有大片阴影，因此SAM并没有将整个道路分割出来)，将这两张图片进行合并得到完成的全局地图，并和使用传统图像处理得到的全局地图进行对比，如图4所示。该方法解决了传统的基于二值化的全局地图构建容易受复杂背景元素和光照的变化而变化的问题。

在步骤S103中，目标全局地图栅格化：

仅区分出图像里的障碍物和路径不能够满足路径规划的需求，还需要将这些信息转换成无人车可以理解的地图格式。本文综合考虑无人机的航拍地图为二维，而且使用单线(二维)激光雷达进行避障，最终选用栅格地图进行导航，并根据之前的图像处理设计栅格地图的构建算法，为后续路径规划提供可靠的尺度和障碍物信息。将全局地图转换为栅格地图，还需要注意以下几点：

(1)将地图进行灰度化处理后，黑色像素对应的地图值为0，白色为255。而ROS中黑色为100，白色为0，因此需要将值进行对应的转换。

(2)一张图片里面，像素值的存储是二维的，而在ROS中地图数据是一维的，且数据是以行为主要递增方向的.也就是说遍历的时候要先遍历第一行的所有数据，然后再遍历第二行的所有数据。

(3)由于无人机采用单目相机拍摄图像，所以没有尺度信息，而无人车导航过程中需要知道障碍物和道路的实际大小，因此需要确定尺度信息。

对于第一点，可以建立如下映射关系，其中g(x,y)表示坐标(x,y)处的栅格地图单元值：

对于第二点，ROS中的栅格地图存储格式为.pgm，全局地图存储格式为.png，这两种格式都是从左上角开始存储数据。将栅格地图的每个单元与全局地图中每个像素一一对应，这样栅格地图每个单元存储的状态值可以用下面的公式表示：

g′(k)＝i*g(x,y)+j

其中，g′(k)表示栅格地图在一维存储下的值，k为一维存储下的索引，i为二维存储下的行数，j为列数。

对于第三点，需要在地图中找到参照物并明确参照物的实际尺寸。因此本文使用了谷歌地图中的“比例尺”功能，使用谷歌地图找到无人机航拍区域，用比例尺测出右侧道路的宽度d

因此可得尺度因子scale为：

带入上面测量的值，计算出scale＝0.057(米/像素)。

在步骤S104中，使用栅格地图进行路径规划：

机器人的自主定位是指机器人在未知环境中能够准确地确定自身位置的能力，而无需依赖外部信号或人为标记。SLAM中的定位是在导航之前进行的，而自主定位是用于导航中，机器人根据目标点按照设定的路线运动，通过定位算法来判断机器人的实际轨迹是否符合预期。

本申请采用AMCL算法进行机器人的自主定位。AMCL算法的核心思想是使用一组粒子(particle)来表示机器人可能所在的位置。每个粒子都有一个状态，包括机器人所在的位置、方向和速度等信息，以及一个权重。AMCL算法需要对每个粒子的权重进行更新和重采样，以使其代表机器人所处的实际位置和状态。粒子权重的计算通常使用贝叶斯滤波器框架，基于机器人的传感器信息和地图信息，不断更新每个粒子的状态和权重。

AMCL算法包含三个关键步骤：预测、更新和重采样。预测步骤根据机器人上一时刻的状态、控制信息和运动模型，预测机器人当前时刻的状态；更新步骤使用传感器信息和地图信息，计算每个粒子的权重；重采样步骤根据粒子的权重，选择一部分粒子并对它们进行复制和修正，以维持粒子集大小不变。这样，经过多次更新和重采样的循环，粒子的权重会越来越符合实际情况，最终可以得到机器人的位置估计结果。

在实际场景中，是无法直接使用栅格地图的，原因有两点：

(1)语义分割构建的栅格地图是一个静态地图，而在导航过程中障碍物信息是可变的，这意味着可能有一些障碍物被移动或添加。因此，为了实时获取障碍物的信息，在导航期间需要进行实时障碍物检测。

(2)当机器人接近障碍物边缘时，即使该区域是未被占用的，机器人如果进入该区域，可能由于惯性或者机器人形状不规则等因素，转弯时与障碍物产生碰撞风险。为了确保机器人的安全，在靠近地图中的障碍物边缘处设置一个警戒区域，以尽量避免机器人进入此区域。

所以，静态地图无法直接应用于导航，其基础之上需要添加一些辅助信息的地图，即global costmap(全局代价地图)和local costmap(本地代价地图)，前者用于全局路径规划，后者用于局部路径规划。

global costmap可以理解成在静态地图上进行的路径规划，采用A*算法。A*算法的核心思想是使用一个评估函数来为每个搜索节点赋予一个值，这个值代表从起始点到目标点的预估成本。评估函数通常表示为：

f(n)＝g(n)+h(n)

其中：f(n)是节点n的总成本，g(n)是从起始点到节点n的实际成本，h(n)是从节点n到目标点的估计成本(也称为启发式函数)。

A*算法的工作原理如下：

1)将起始节点放入一个开放列表(通常使用优先队列实现)；

2)从开放列表中选择具有最低f值的节点；

3)对该节点进行扩展，即查找其所有邻居节点；

4)对于每个邻居节点，计算其g值和h值，并更新其f值；

5)如果邻居节点不在开放列表和关闭列表中，将其添加到开放列表中；

6)如果邻居节点已经在开放列表中，并且新的g值更小，则更新其g值；

7)如果当前节点是目标节点，算法结束；

8)否则，将当前节点移动到关闭列表，并返回步骤2)。

而local costmap用于对机器人的即时周围环境进行处理，当机器人按照全局规划的路径前进，遇上障碍物时，local costmap可以动态生成局部路径来规避障碍物。本申请选择DWA Local Planner作为局部路径规划算法。以下是DWALocal Planner的基本概念和工作原理：

动态窗口：考虑到机器人的动态约束(如最大加速度和最大速度)，动态窗口定义了机器人在一个短时间内可以达到的速度范围。

速度采样：在动态窗口内，DWA对可能的线速度和角速度进行采样，生成一系列可能的速度命令。

评估函数：对于每一个速度命令，DWA使用一个评估函数来评估其优劣。这个评估函数通常基于以下三个标准：

到目标的距离：速度命令应该使机器人更接近目标。

机器人的速度：速度命令应该使机器人保持一个合适的速度。

避障：速度命令不应该导致机器人与障碍物碰撞。

选择最佳速度命令：DWA选择评估函数值最高的速度命令作为下一个动作。

反馈控制：机器人执行选择的速度命令，并根据环境的反馈进行调整。

最后在地图上添加膨胀层来对机器人的转向进行保护，以避免机器人的外壳撞上障碍物。在Rviz中对配置好的全局代价地图和局部代价地图进行测试。

通过上述异构机器人空地协同搜索救援的方法，一方面，在搜救范围确定的前提下，空中机器人利用全局的视野优势来感知地面环境，向下俯视拍摄得到全局范围的初始全局地图，空中机器人将初始全局地图传输给边缘节点，边缘节点利用SAM算法对初始全局地图进行语义分割以得到目标全局地图，边缘节点再对目标全局地图进行处理最终转换成栅格地图，并将栅格地图传输给地面机器人。地面机器人根据栅格地图，利用AMCL算法进行自主定位、利用A*算法进行全局路径规划、利用DWA Local Planner进行局部路径规划和在栅格地图上添加膨胀层以对地面机器人的转向进行保护，以选定前往搜救的目标点进行路径规划，并利用其局部精准定位优势导航达到目标点完成搜救。另一方面，空中机器人和地面机器人在不同视角下观察的目标信息不同，通过视角的变换以及几何的相关性实现二者信息互补，大大地提高了目标物识别的准确度。

在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本公开的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不必须针对的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。此外，本领域的技术人员可以将本说明书中描述的不同实施例或示例进行结合和组合。

本领域技术人员在考虑说明书及实践这里公开的发明后，将容易想到本公开的其它实施方案。本申请旨在涵盖本公开的任何变型、用途或者适应性变化，这些变型、用途或者适应性变化遵循本公开的一般性原理并包括本公开未公开的本技术领域中的公知常识或惯用技术手段。说明书和实施例仅被视为示例性的，本公开的真正范围和精神由所附的权利要求指出。