CN115790571A - 基于异构无人系统相互观测的同时定位与地图构建方法 - Google Patents

Abstract

本发明涉及基于异构无人系统相互观测的同时定位与地图构建方法，智能交通。所述方法的一种实施方式是用于解决无人车和无人机在同一时空范围内联合执行任务(如巡检、运送货物、勘察)时，在不依赖于GNSS和RTK等外部信息，仅使用机载/车载感知设备的条件下，通过无人机与无人车之间对环境和彼此的观测来实现准确定位，使得无人车和无人机协作的定位精度优于单机定位精度，为执行进一步的高精度的操作提供定位信息。

Description

基于异构无人系统相互观测的同时定位与地图构建方法

技术领域

本公开涉及智能交通，尤其涉及一种基于异构无人系统相互观测的同时定位与地图构建方法。

背景技术

准确的环境感知与自身状态估计是无人系统自主执行各种高层次任务的重要基础。在光照正常、纹理丰富、结构特征明显的一般场景下，现有的同时定位与地图构建(simultaneous localization and mapping，SLAM)方法已经取得了非常稳定的性能表现，如在经典的室内数据集上，SLAM最高定位精度已经可以达到厘米级。然而，在感知退化环境下(如隧道、矿洞、狭长走廊等)进行机器人的同时定位与地图构建仍然是一个具有挑战性的问题。主要表现在：(1)与开阔空间之间存在遮挡，导致无人系统无法接收到GPS或RTK等外部定位信号；(2)高度相似性的结构增加了里程计错误匹配的概率，进而容易引起错误的回环检测，一旦将错误的回环引入地图优化，将会为全局地图带来严重的破坏；(3)较大的空间规模使里程计的累计误差随运行时间明显增加，即使采用回环修正的方法对精度的提升也非常有限。

为了实现在复杂环境下不依赖外部信息的精确定位，将无人机与无人车协同，发挥二者各自优势来解决环境感知条件退化与自身运动导致的定位丢失问题是一种有效的方案。

(1)公开号CN210377164U公布一项中国实用新型专利-一种空地协同作业系统，如图1所示。该实用新型公开了一种空地协同作业系统，包括：无人车，通过在地面上的移动构建目标场景的三维地图，并实时定位自身在三维地图的地图坐标系下的位姿；无人机，用于对作业目标进行作业；数据采集器，用于获取无人机与无人车的第一相对位姿以及作业目标与无人车的第二相对位姿；数据处理器，与无人车、无人机以及数据采集器均相连，用于根据无人车在地图坐标系下的位姿、第一相对位姿以及第二相对位姿，得到无人机与作业目标的第三相对位姿，并将第三相对位姿发送至无人机，以使无人机根据第三相对位姿调整位姿至预设作业范围内。该空地协同作业系统通过无人车协助无人机在目标场景中精确定位。该方法虽然同样可以达到无人车与无人机协同作业的效果，但是该方法仅在无人车上安放AprilTag标记，用于辅助无人机的定位，而没有考虑使用无人车和无人机相互的观测信息实现协同定位，对有效信息的利用不够充分。

(2)公开号CN217716438U公布一项中国实用新型专利一种无人车、无人机协同导航装置，如图2所示。该实用新型公开了一种无人车、无人机协同导航装置，包括无人机、QGC地面站和无人车，所述无人机安装有GPS定位装置、锂电池组、第一无线图传模块、双目相机、第二无线数传模块、支撑板、起落架和飞行控制器，所述飞行控制器包括主控制器、第一I/O控制器、第一IMU惯性导航模块和GPS模块，所述QGC地面站安装有第一无线数传模块，所述无人车设置有MC底层控制模块、上位机图形界面显示模块、速度控制器、转向控制器、第二I/0控制器和第二无线图传模块，所述MC底层控制模块由车载控制器和第二IMU惯性导航模块组成。该实用新型解决了传统导航装置识别范围小、延时较大、受GPS信号影响较大的问题，设计了一种无人车、无人机协同导航装置但该实用新型依赖GPS模块提供全局定位信息，在矿坑洞穴或高楼峡谷等GPS信号缺失的场景中无法使用。

(3)公开号CN114923477A公布一项中国发明专利基于视觉与激光SLAM技术的多维度空地协同建图系统和方法，如图3所示。该系统包括地面站、无人机和无人车。无人机包括整体物理架构、机载嵌入式电脑和图像传感器；整体物理架构包括机架、四旋翼和电机；机载嵌入式电脑包括通信模块与嵌入式处理器，图像传感器包括双目相机与RGB-D相机。无人车包括嵌入式处理器、激光雷达传感器和通信模块。地面站包括显示控件、控制控件和通信控件。所述方法利用地面站输出控制和建图指令，将无人机构建的3D点云地图和无人机构建的2D平面栅格地图通过SLAM技术进行融合。该发明通过远程控制采集环境信息，提高了相关作业的安全性，消除了GPS信号弱等因素造成的定位误差，提高了建图的精准度，但该方法在无人测和无人机的地图融合上仅使用场景中的结构信息，没有使用到无人机与无人车之间的相互观测，信息利用不够充分。且在感知退化环境下，容易出现由于无人机和无人车异构视角引起的地图匹配错误，导致整个定位系统出现严重飘移。

(4)公开号CN111707988A公布一项中国发明专利基于无人车车载UWB基站的无人器定位系统及定位方法，如图4所示。该发明公开了基于无人车车载UWB基站的无人器定位系统及定位方法，包括无人车车载的4个UWB锚节点、定位计算模块和无人机UWB标签，建立无人车车载坐标系，在无人机无人车编队运动过程中，无人车车载4个UWB锚节点分别实时获取无人机UWB标签的距离信息，依据三维空间欧氏距离计算方法，计算无人机在无人车车载坐标系中的位置，为编队队形的保持提供位置信息，但该方法需要在无人机和无人车上安装UWB信号收发模块，提升了硬件成本，限制了方法的应用场景。

(5)论文《Omni-Swarm：A Decentralized Omnidirectional Visual-Inertial-UWB State Estimation System for Aerial Swarms》使用多无人机之间的相互观测和UWB距离测量来进行无人机集群的定位，如图5所示。该文提出了一种用于航空群的分散式全向视觉惯性UWB状态估计系统Omni-Swarm。为了解决可观测性、复杂的初始化、精度不足和缺乏全局一致性的问题，引入了一个全向感知系统作为全向群的前端，该系统由包括立体鱼眼相机和超宽带(UWB)传感器在内的全向传感器和包括鱼眼视觉惯性里程计(VIO)在内的算法组成，以实现基于多无人机地图的定位和视觉对象检测器。基于图的优化和正向传播作为Omni-Swarm的后端工作，以融合前端的测量结果。根据实验结果，所提出的群系统分散状态估计方法在保证全局一致性的同时，达到了厘米级的相对状态估计精度。该方法虽然在定位中用到了无人机之间相互观测的信息，但该方法不具备自主性，即无法区别对待处于感知条件良好的无人机与处于感知条件恶劣的无人机的定位结果，存在集群中某一无人机定位结果漂移引起整个集群定位不稳定的缺点。

发明内容

针对上述现有技术，本发明主要解决的问题至少是无人车和无人机在同一时空范围内联合执行任务(如巡检、运送货物、勘察)时，在不依赖于GNSS和RTK等外部信息，仅使用机载/车载感知设备的条件下，如何通过无人机与无人车之间对环境和彼此的观测来实现准确定位，使得无人车和无人机协作的定位精度优于单机定位精度，为执行进一步的高精度的操作提供定位信息。

为了解决上述技术问题，本发明的技术方案如下。

第一方面，本发明提出了一种基于异构无人系统相互观测的同时定位与地图构建方法，所述方法包括下述步骤：

S100、使第一机器人静止于临界区域，使第二机器人位于探索区域，第一机器人具有第一双目相机，第二机器人具有第二双目相机；

S200、基于第一双目相机、第二双目相机两者的深度图像，建立占据栅格地图，检测探索区域的障碍物，并生成安全区域；

S300、在安全区域内进行路径规划，基于探索区域中第二双目相机的实时视野以及第二机器人与第一机器人之间的相对位置，调整路径，直至第二机器人移动至新的临界区域，完成探索区域的地图构建；

所述临界区域为第一机器人或第二机器人能够使自身相对所在环境处于静止状态的区域，所述探索区域为需要建立地图的区域。

在上述技术方案中，第一机器人、第二机器人是广义的机器人，可以是无人机、无人车、手持设备或其它随身便携设备。通过第一机器人和第二机器人协作定位以构建地图，以解决在无法利用外部信息进行定位或定位发生严重偏差而又需要获取某一区域的环境状况时，比如隧道、矿坑、无人区域等，通过实时获取彼此的相对位置避免单个机器人因视觉里程计的定位飘移与定位跟踪丢失。

作为上述技术方案的进一步改进，所述方法通过角色互换，直至完成通过感知退化区域的任务。具体地，还包括下述步骤：

S400、若还存在探索区域，则使第二机器人为第一机器人静止于临界区域，使第一机器人为第二机器人，进入新的探索区域，返回步骤S200。

在上述技术方案中，一种生成安全区域的实施方式如下：

基于第一双目相机和第二双目相机，建立周围环境的稠密点云；

将稠密点云转化为占据栅格地图M_ocp，占据栅格地图M_ocp具有三类体素，分别为已知障碍物体素V_obstacle、已知安全区域体素V_safe、未知体素V_unknow；

在M_ocp中，以第一机器人为圆心，在第一设定距离内，将所有空白的区域中与障碍物体素距离不超过第二设定距离的已知安全区域体素作为安全区域；

所述第一设定距离用于保证第一机器人和第二机器人距离之间的定位精度。

在上述技术方案中，所述S300采用基于图结构的探索路径规划方法进行路径规划。

在上述技术方案中，所述方法采用光流法使第一机器人静止于临界区域。

在上述技术方案中，第一机器人具有第一标识，第二机器人具有第二标识，第一标识和/或第二标识用于确定第一机器人与第二机器人之间的相对位置。

在上述技术方案中，所述第一机器人和第二机器人之间的相对位置，估算如下：

式中：

为动态权重平衡因子；

为第二机器人基于第一标识得到的自身相对第一机器人的相对位姿估计，

为第一机器人基于第二标识得到的自身相对第一机器人的相对位姿估计，T_exp-odom为第二机器人根据自身双目相机得到的关于自身位姿估计值。

在上述技术方案中，所述动态权重平衡因子采用下式估算：

式中：d_max为第一机器人和第二机器人之间的最大相对位置，N_Fast为第二机器人的双目相机当前视野范围内Fast特征点数量，d为第一机器人、第二机器人之间距离。

第二方面，本发明提出了一种计算机可读存储介质，其特征在于：存储有能够被处理器加载并执行如权利要求1至8中任一种方法的计算机程序。

第三方面，本发明提出了一种基于异构无人系统相互观测的同时定位与地图构建装置，所述装置包括第一机器人、第二机器人、地图构建模块；所述第一机器人，被配置位于临界区域静止，在第一机器人上配置第一双目相机，所述第二机器人，被配置位于探索区域能够移动，在第二机器人上配置第二双目相机；所述地图构建模块，被配置利用生成安全区域单元、路径规划单元，以完成探索区域的地图构建；所述生成安全区域单元，被配置用于：基于第一双目相机、第二双目相机两者的深度图像，建立占据栅格地图，检测探索区域的障碍物，并生成安全区域；所述路径规划单元，被配置用于：在安全区域内进行路径规划，基于探索区域中第二双目相机的实时视野以及第二机器人与第一机器人之间的相对位置，调整路径，直至第二机器人移动至新的临界区域；所述临界区域为第一机器人或第二机器人能够使自身相对所在环境处于静止状态的区域，所述探索区域为需要建立地图的区域。

附图说明

为了更清楚地说明本申请实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本申请的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1、背景技术中提及的空地协同作业系统的结构框图；

图2、背景技术中提及的无人车、无人机协同导航装置结构示意图；

图3、背景技术中提及的基于视觉与激光SLAM技术的多维度空地协同建图系统的总体结构框图；

图4、背景技术中提及的基于无人车车载UWB基站的无人器定位系统及定位方法的定位原理示意图；

图5、背景技术中提及的使用多无人机之间的相互观测和UWB距离测量来进行无人机集群的定位示意图；

图6、本发明一种实施方式下的相机结构示意图；

图7、本发明一种实施方式下的无人车与无人机相互观测的同时定位与地图构建方法架构示意图；

图8、本发明一种实施方式下的无人车与无人机相互观测的同时定位与地图构建方法应用场景示意图；

图9、本发明一种实施方式下的方法流程示意图。

具体实施方式

下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。

在一种实施方式中，第一机器人为无人机，第二机器人为无人车。无人机、无人车在同一时空范围内联合执行任务(如巡检、运送货物、勘察)。无人机和无人车都配备一个双目相机、一个鱼眼相机和多个AprilTag二维码标记，如图6所示。其中：双目相机用于感知周围环境以实现定位建图及避障，鱼眼相机用于无人机与无人车之间的相互观测，AprilTag二维码标记作为无人机或无人车上的标识，用于确定无人机和无人车之间的相对位置。

在感知条件良好的环境下，无人车可以搭载无人机行驶，仅依靠自身的双目相机执行定位建图和避障；当环境感知条件退化时，执行无人车与无人机相互观测的同时定位与地图构建方法，方法架构如图7所示。

其中，感知条件的退化主要表现在：

(1)与开阔空间之间存在遮挡，导致无人系统无法接收到GPS或RTK等外部定位信号；

(2)高度相似性的结构增加了里程计错误匹配的概率，进而容易引起错误的回环检测，一旦将错误的回环引入地图优化，将会为全局地图带来严重的破坏；

(3)较大的空间规模使里程计的累计误差随运行时间明显增加，即使采用回环修正的方法对精度的提升也非常有限。

参见图8所示的场景，一辆无人车与一架无人机从起点出发，经过一片感知条件退化的区域。无人机、无人车通过彼此的观测来协作定位与构建感知条件退化区域的地图，可避免在单机在经过感知退化区域时视觉里程计的定位飘移与跟踪丢失问题。在这个过程中，无人机与无人车时刻处于可相互观测的条件，双方交替作为彼此的参照，直到完成通过感知退化区域的任务。

基于图7的方法架构，具体步骤见下文步骤S101～S401，如图9所示：

S101、使参照机器人静止于临界区域，使探索机器人位于探索区域。

在这里，使用机器人来代指无人机或无人车，并依据所处的感知环境将集群中的机器人分类，规定位于临界区域的机器人为参照机器人，需要进入感知退化区域进行探索的机器人为探索机器人。初始角色分配随机进行。即无人机和无人车都有可能作为探索机器人和参照机器人。

探索区域为需要建立地图的区域，在当前实施方式中，为感知退化区域，探索机器人可通过融合与参照机器人的相互观测张贴的AprilTag与自身视觉里程计的结果来进行鲁棒定位。

临界位置，为第一机器人或第二机器人能够使自身相对所在环境处于静止状态的区域。在当前实施方式中，临界位置为该位置的感知条件能够满足机器人基于光流法确保自身处于静止的要求。可通过双目相机视野内的纹理丰富程度来度量某一地点是否满足临界位置的要求。在一种实施方式中，如果某一机器人视野内提取Fast特征点的数量超过某一阈值T_Fast，则该地点适合作为临界位置。Fast特征点提取方法为：遍历所有的像素点，判断当前像素点是不是特征点的唯一标准就是在以当前像素点为圆心以3像素为半径画个圆(圆上有16个点)，统计这16个点的像素值与圆心像素值相差比较大的点的个数。超过9个差异度很大的点那就认为圆心那个像素点是一个特征点。

S201、基于第一双目相机、第二双目相机两者的深度图像，建立占据栅格地图，检测探索区域的障碍物，并生成安全区域。

在这一步骤中，基于探索机器人和参照机器人双目相机的感知融合结果，检测待探索区域的障碍物，为探索机器人生成安全的探索区域。

具体地，无人机和无人车的双目相机通过双目测距原理建立周围环境的稠密点云，并将稠密点云转化为体素边长为d_ocp的占据栅格地图M_ocp，其中有三类体素，分别为已知障碍物体素V_obstacle、已知安全区域体素V_safe、未知体素V_unknow。随着机器人在空间中移动执行任务，M_ocp中代表障碍物的体素动态增加。

由于无人机和无人车各自携带一个双目相机，所以其产生的占据栅格图有各自独立的坐标，分别记为M_ocp-UAV和M_ocp-UGV。本实施方式中以M_ocp-UGV初始位置为M_ocp的位置，通过无人机与无人车之间使用AprilTag的相互定位来确定t时刻M_ocp-UAV和M_ocp-UGV相对于M_ocp的位姿变换。

在M_ocp中，以参照机器人为圆心，d_max距离内所有空白的区域中与障碍物体素距离不超过设定的安全区域距离d_safe的所有已知安全区域体素即为安全区域A_safe，机器人可在此区域内规划路径。d_max为参照机器人和探索机器人的最大相对位置，设定d_max的意义在于防止参照机器人和探索机器人距离过远导致的AprilTag定位精度下降。

S301、在安全区域内进行路径规划，基于探索区域中第二双目相机的实时视野以及参照机器人与探索机器人之间的相对位置，调整路径，直至探索机器人移动至新的临界区域，完成探索区域的地图构建。

在这一步骤中，一种实施方式是使用基于图结构的探索路径规划方法为探索机器人规划探索路径。在融合探索机器人和参照机器人的稠密点云到全局地图形成占据栅格地图后，确定障碍物体素和未知体素，在一定范围内划定安全区域，在安全区域内通过不高于某一密度的随机采样得到空间中一系列节点，按照阈值将节点连接形成图结构，使用迪杰斯特拉最短路径算法进行局部轨迹规划。具体地：

首先，确定局部路径规划的终点。定义某一地点的探索增益为该地点d_gain邻域内的未知体素的数量。步骤S200中A_safe内探索增益最大的地点即为局部路径规划的终点P_gain-max。

其次，生成图结构。在安全区域内生成随机采样点，过滤掉该随机采样点与最近邻点距离小于d_min的点，并将距离小于d_edge的点用无向边连接，最终在局部生成图结构。d_min、d_edge为设定的超参数。

最终，在图结构中执行迪杰斯特拉最短路径算法，起点为探索机器人的当前位置，终点为安全区域内确定的终点P_gain-max，并将经过的节点进行插值得到路径Traj，插值方法比如三次B样条，牛顿插值等。

将参照机器人的观测结果、探索机器人的观测结果、探索机器人视觉里程计的结果根据两机器人的距离以及探索机器人视野内环境感知条件进行线性融合，以获得构建的地图。

S401、若还存在探索区域，则使探索机器人为参照机器人并静止于临界区域，使参照机器人为探索机器人，进入新的探索区域，返回步骤S201。

在探索机器人执行探索任务的过程中，参照机器人静止于临界位置，即该位置的感知条件可以满足确保自身处于静止的要求，探索机器人通过观测参照机器人并结合参照机器人发送的对自身的观测来确定自身位姿，直到探索机器人达到下一临界位置，转变机器人角色继续进行上述步骤。

具体过程如下：

首先，根据步骤3中得到的轨迹Traj，使用比例-积分-微分(PID)方法控制探索机器人进行轨迹跟踪。

其次，在探索机器人执行轨迹跟踪的过程中，参照机器人处于临界位置，基于光流法确保自身位置静止于点P_ref，此时参照机器人的位姿为T_ref。

在探索机器人执行轨迹跟踪同时，两机器人相互观测对方的AprilTag，得到以下结果：探索机器人通过观测参照机器人AprilTag得到自身相对参照机器人的相对位姿估计

参照机器人观测探索机器人得到的自身相对探索机器人的位姿估计

探索机器人根据自身双目相机得到基于视觉里程计的自身位姿估计T_exp-odom.

最终，探索机器人的位姿可以表示为：

其中，

为动态权重平衡因子，其取值与两机器人之间距离d和探索机器人当前视野范围内Fast特征点数量N_Fast有关，具体表示为：

其中，d_max为两机器人之间的最大距离。

在探索机器人到达局部路径规划的重点后，判断该地点是否属于临界位置。若是，则由交换探索机器人与参照机器人身份，返回步骤S201，直到完成任务达到环境感知友好的区域。若否，则探索机器人沿原路径回退，直到到达某一属于临界位置的点，交换探索机器人与参照机器人身份，返回步骤S201，直至需要构建地图的区域全部建立起地图。

综上，上述实施方式仅仅依靠无人机和无人车搭载的双目相机和鱼眼相机实现感知恶劣环境下相互观测定位。以往解决感知退化环境下多机器人鲁棒定位问题的方法，大都依赖于增加传感器的冗余程度，或依赖外部定位信号。如，如通过增加主动光源实现弱光线环境下的同时定位与地图构建，通过增加有锚点的超宽带(Ultra-WideBand，UWB)定位实现矿坑下复杂环境的定位，通过基站发送伪GPS卫星信号实现隧道环境下的定位。上述方法虽然提升了感知退化环境下机器人同时定位与地图构建的能力，但增加机器人携带传感器和辅助设备的数量和种类会增加整个无人系统的复杂性，无人机/无人车等移动平台的载荷和运算能力往往会受到限制，从而限制了其应用的范围。而本方法基于低成本的双目相机、鱼眼相机、AprilTag二维码标签即可实现感知恶劣环境下的同时定位与建图。在无人机与无人车的自主轨迹规划考虑到了对定位的影响。以往轨迹规划方法侧重于考虑最短距离或最短时间，而上述实施方式采用的轨迹规划方法，综合考虑了机器人在场景中的定位能力。可以最大限度保证基于视觉里程计的定位不丢失或飘移，为复杂环境下机器人执行多种任务提供了精确的未知信息环境信息。

需注意的是：上述实施方式中的无人机或无人车上配置的双目相机数目可改变，而无人机或无人车的数目也可以改变，进一步改进成多机定位与地图构建。

需要注意的是：上述实施方式中，当无人机或无人车为一种手持设备或便携设备时，上述实施方法可用于人工辅助进行未知区域的定位和地图的构建，在上述方法的使用中，相应的位姿估计变为一种位置估计。

通过以上的实施方式的描述，所属领域的技术人员可以清楚地了解到本公开可借助软件加必需的通用硬件的方式来实现，当然也可以通过专用硬件包括专用集成电路、专用CPU、专用存储器、专用元器件等来实现。一般情况下，凡由计算机程序完成的功能都可以很容易地用相应的硬件来实现，而且，用来实现同一功能的具体硬件结构也可以是多种多样的，例如模拟电路、数字电路或专用电路等。但是，对本公开而言更多情况下，软件程序实现是更佳的实施方式。

尽管以上结合附图对本发明的实施方案进行了描述，但本发明并不局限于上述的具体实施方案和应用领域，上述的具体实施方案仅仅是示意性的、指导性的，而不是限制性的。本领域的普通技术人员在本说明书的启示下和在不脱离本发明权利要求所保护的范围的情况下，还可以做出很多种的形式，这些均属于本发明保护之列。

Claims (10)

1.基于异构无人系统相互观测的同时定位与地图构建方法，其特征在于，所述方法包括下述步骤：

S100、使第一机器人静止于临界区域，使第二机器人位于探索区域，第一机器人具有第一双目相机，第二机器人具有第二双目相机；

S200、基于第一双目相机、第二双目相机两者的深度图像，建立占据栅格地图，检测探索区域的障碍物，并生成安全区域；

S300、在安全区域内进行路径规划，基于探索区域中第二双目相机的实时视野以及第二机器人与第一机器人之间的相对位置，调整路径，直至第二机器人移动至新的临界区域，完成探索区域的地图构建；

所述临界区域为第一机器人或第二机器人能够使自身相对所在环境处于静止状态的区域，所述探索区域为需要建立地图的区域。

2.根据权利要求1所述的方法，其特征在于，所述方法还包括下述步骤：

S400、若还存在探索区域，则使第二机器人为第一机器人静止于临界区域，使第一机器人为第二机器人，进入新的探索区域，返回步骤S200。

3.根据权利要求1所述的方法，其特征在于，所述S200包括：

基于第一双目相机和第二双目相机，建立周围环境的稠密点云；

将稠密点云转化为占据栅格地图M_ocp，占据栅格地图M_ocp具有三类体素，分别为已知障碍物体素V_obstacle、已知安全区域体素V_safe、未知体素V_unknow；

在M_ocp中，以第一机器人为圆心，在第一设定距离内，将所有空白的区域中与障碍物体素距离不超过第二设定距离的已知安全区域体素作为安全区域；

所述第一设定距离用于保证第一机器人和第二机器人距离之间的定位精度。

4.根据权利要求1所述的方法，其特征在于，所述S300采用基于图结构的探索路径规划方法进行路径规划。

5.根据权利要求1所述的方法，其特征在于，所述方法采用光流法使第一机器人静止于临界区域。

6.根据权利要求1所述的方法，其特征在于，第一机器人具有第一标识，第二机器人具有第二标识，第一标识和/或第二标识用于确定第一机器人与第二机器人之间的相对位置。

7.根据权利要求6所述的方法，其特征在于，所述第一机器人和第二机器人之间的相对位置，估算如下：

式中：

为动态权重平衡因子；

为第二机器人基于第一标识得到的自身相对第一机器人的相对位姿估计，

为第一机器人基于第二标识得到的自身相对第一机器人的相对位姿估计，T_exp-odom为第二机器人根据自身双目相机得到的关于自身位姿估计值。

8.根据权利要求7所述的方法，其特征在于，所述动态权重平衡因子采用下式估算：

式中：d_max为第一机器人和第二机器人之间的最大相对位置，N_Fast为第二机器人的双目相机当前视野范围内Fast特征点数量，d为第一机器人、第二机器人之间距离。

9.一种计算机可读存储介质，其特征在于：存储有能够被处理器加载并执行如权利要求1至8中任一种方法的计算机程序。

10.基于异构无人系统相互观测的同时定位与地图构建装置，其特征在于：

所述装置包括第一机器人、第二机器人、地图构建模块；

所述第一机器人，被配置位于临界区域静止，在第一机器人上配置第一双目相机，所述第二机器人，被配置位于探索区域能够移动，在第二机器人上配置第二双目相机；

所述地图构建模块，被配置利用生成安全区域单元、路径规划单元，以完成探索区域的地图构建；

所述生成安全区域单元，被配置用于：基于第一双目相机、第二双目相机两者的深度图像，建立占据栅格地图，检测探索区域的障碍物，并生成安全区域；

所述路径规划单元，被配置用于：在安全区域内进行路径规划，基于探索区域中第二双目相机的实时视野以及第二机器人与第一机器人之间的相对位置，调整路径，直至第二机器人移动至新的临界区域；

所述临界区域为第一机器人或第二机器人能够使自身相对所在环境处于静止状态的区域，所述探索区域为需要建立地图的区域。

Images