CN109579843B - 一种空地多视角下的多机器人协同定位及融合建图方法 - Google Patents

Abstract

本发明公开了一种空地多视角下的多机器人协同定位及融合建图方法，使用空中、地面多视角完整覆盖探测场景，融合由空中机器人、地面机器人采集的场景图像数据，通过视觉约束定位各机器人并还原三维场景信息。通过附着于机器人上的特定视觉特征，优化建图和定位系统三维点云地图和机器人六自由度位姿。基于视觉特征的位姿优化、地图融合算法显著地提高了的重建和定位精度，修正了地图尺度，使得各机器人的局部地图能够在多个异构机器人间共享，提高了三维重建的覆盖率，为任务规划、灾难环境搜救、军事反恐等情景快速提供可靠的环境信息。

Description

一种空地多视角下的多机器人协同定位及融合建图方法

技术领域

本发明属于计算机视觉技术领域，尤其涉及空地多视角下的多机器人协同定位及融合建图方法，适用于复杂场景下三维点云地图的构建、多机器人协同定位、自动任务规划、无人机测绘等技术领域。

背景技术

同时定位和建图(SLAM)技术近年来得到广泛而深入的研究，但是多机器人、跨视场的协同定位和建图问题一直没有很好地被解决。在协同定位方面，多机器人之间的相对位姿校正和信息感知成为一个难点，在建图过程中，机器人无法得知场景中的结构先验信息，地图构建主要靠机器人在场景中的运动完成，而机器人间的相对位姿关系在地图融合过程中起到关键作用。现有技术通过差分GPS等技术手段获得机器人相对位置关系从而使得机器人之间得以相对定位。

三维地图直观地反映了三维空间中的环境特征，所以三维地图作为三维环境的重要特征之一，一直以来都是国内外研究人员的研究重点。如果可以对三维场景进行结构的重建，就可以恢复场景信息，为人工智能设备对三维场景的理解和探索提供了直接信息。分散于环境中的多个机器人，通过通信和协作，可以使得整个系统在重建和定位的速度和精度都有较大提升。

三维重建问题就是确定某一三维环境的空间结构问题。三维重建估计在机器人视觉、场景理解和自动任务规划、自动路径规划等很多领域都有应用。随着计算机视觉技术的广泛应用，在场景环境固定的情况下，自由运动的相机拍摄场景，并对环境三维结构信息做出估计，已经成为一个重要的研究方向。目前现有技术在三维重建问题上有多种解决方案，例如基于双目相机、深度相机、激光雷达或基于SfM(Structure from Motion)技术来计算三维地图。

目前基于双目相机、深度相机、激光雷达或基于SFM等的单一机器人的方法恢复场景三维结构受视角局限等影响，在复杂场景下，无法完备地恢复场景中的三维信息，同时由于无法统一地图尺度，各机器人创建的地图无法在多个机器人之间共享。而差分GPS等定位方法精度受环境影响大，在有遮蔽的环境中定位精度较低，从而影响协同定位的总体精度，使建图结果出现偏移或形变。

发明内容

针对当前对于复杂环境下单一机器人建图视角受限、无法获取全局地图的问题，本发明借助空中机器人和地面机器人协同建图的空地多视角协同建图和定位方法，旨在高效、鲁棒地解决机器人间地图共享、融合和定位问题。

本发明的目的是提供一种基于多机器人的空地协同定位与建图方法，通过机器人上的标志物辅助定位与建图算法完成初始化，多个机器人维护和创建各自的局部地图，并由运算设备融合各机器人的局部地图形成尺度统一的全局地图并定位各终端在地图中的位置。

为了实现上述目的，本发明技术方案包括如下步骤：

一种空地多视角下的多机器人协同定位及融合建图方法，包括：

步骤1、机器人各自进行初始化，各自建立和维护一张局部地图；

步骤2、机器人检测所获取图像中的附着于其他机器人之上的二维码，将检测到二维码的机器人作为观测机器人，将附着有二维码的机器人作为被观测机器人，计算二维码相对于观测机器人的位姿，并通过二维码中携带的标识获取预先标定的二维码在被观测机器人坐标系下的位姿，计算机器人之间的位姿；

步骤3、通过引入的二维码位姿约束，优化各机器人的地图尺度和轨迹，使用局部光束平差法融合当前的全局地图和对应机器人的局部地图，生成新的全局地图；

步骤4、将全局地图、各机器人的地图尺度和轨迹广播给各机器人，各机器人根据收到的全局地图和轨迹通过局部光束平差法更新当前的局部地图和自身相对于世界坐标系的位姿，并根据地图尺度修正局部地图。

进一步地，所述步骤2，包括：

步骤2.1、观测机器人检测所获取图像中的附着于其他被观测机器人之上的二维码，获取二维码对应的二维码数据；

步骤2.2、提取二维码内部角点在图像平面上的坐标；

步骤2.3、根据二维码数据中印刷尺寸，恢复出二维码内部角点在被观测机器人坐标系下的三维坐标，作为PnP算法的物体原始坐标，求解PnP，获得二维码相对于观测机器人的位姿；

步骤2.4：根据二维码数据中预先标定的二维码在被观测机器人坐标系下的位姿，计算得到机器人之间的位姿。

进一步地，所述引入的二维码位姿约束，包括：机器人的地图尺度、机器人之间的位姿。

进一步地，所述优化各机器人的地图尺度，包括：

根据得到的机器人的地图尺度s_m，对于机器人i的局部地图中的三维点

匹配观测机器人m地图中的对应三维点

形成匹配的三维点集合

最小化如下函数求解机器人i的地图尺度

其中，s_i为所求机器人i的地图尺度，根据该值缩放对应机器人局部地图使各机器人的地图尺度统一，其中(·)表示i或m。

进一步地，所述优化各机器人的轨迹，包括：

通过ICP配准统一尺度后的各局部地图形成全局地图，在全局地图中通过最小化如下函数优化机器人i的轨迹：

其中，ρ是损失函数，K为匹配的特征点对，(x^k,X^k)为K中的二维点和三维点，π为重投影函数。

本发明提出的一种空地多视角下的多机器人协同定位及融合建图方法，与现有技术相比，本发明的有益效果如下：

空中机器人与地面机器人相互协作，在建筑物内部等空中机器人无法进入的场景可由地面机器人负责建图，同时，空中机器人为地面机器人提供俯视视角，为协同建图提供更多图像特征点，使得生成的三维点云地图更加完备，当无人机视角包含地面机器人时，地面机器人机身附着的二维码提供的附加几何约束可以有效地降低建图误差和定位漂移。并且，计算终端可以根据实时更新优化的全局地图统一规划机器人探索路径和所需执行的任务，合理调度机器人资源，为灾难环境搜救、军事反恐等情景快速提供可靠的环境信息。

附图说明

图1为本发明空地多视角下的多机器人协同定位及融合建图方法流程图；

图2为本发明实施例机器人所附着的二维码示例；

图3为本发明实施例协同定位与建图结果样本。

具体实施方式

下面结合附图和实施例，对本发明进行进一步的说明。应当理解，此处所描述的具体实施示例仅仅用于解释本发明，并不用于限定本发明；附图仅用于示例性说明，不能理解为对本专利的限制；为了更好地说明实施例，附图某些部件可能会有省略、放大或缩小，并不代表实际产品的尺寸；对于本领域技术人员来说，附图中的某些公知结构及其说明可能省略是可以理解的；附图中描述的位置关系仅用于示例性说明，不能理解为对本专利的限制。

基于空中机器人和地面机器人来实现空地多视角下的多机器人协同定位及融合建图，需要空中机器人、地面机器人和计算终端来联合实现。其中，空中机器人和地面机器人采集和计算本地局部地图，其上安装有用于采集场景视频流的摄像头以及相应的计算硬件设备和必须的控制程序；计算终端用于融合各机器人重建的局部地图以及计算各机器人于全局地图中的位姿。

如图1所示，给出了一种空地多视角下的多机器人协同定位及融合建图方法的实施例，包括如下步骤：

步骤1、机器人各自进行初始化，各自建立和维护一张局部地图。

本实施例在非动态场景下，部署由空中机器人和地面机器人组成的机器人群。各机器人上设置有相机(或者称为摄像头、摄像机等)，用于拍摄场景图像。各个机器人上还设置有各自的同时定位和建图系统，可以单独完成各自的同时定位和建图，本实施例将各个机器人建立的地图也称为局部地图。

各个机器人的初始化过程，包括：

步骤1.1、接收相机采集的图像，提取图像特征点。

一般认为，图像像素点与其8邻域差异足够大，该点就是特征点：

其中，I(x)为圆周circle(p)上一点的灰度，I(p)为圆心的灰度，ε为灰度差的阈值，如果N大于给定阈值，则认为p为特征点。

步骤1.2、通过最小化相机帧间重投影误差，求解相机帧间位姿R,t。

相机帧间重投影误差为：

ρ是损失函数，xⁱ和Xⁱ为关键帧和地图中匹配的二维和三维点，π为重投影函数，将空间点根据相机成像规律投影至图像平面。

求解相机帧间位姿R,t的过程即最小化重投影误差，可用最小二乘法求解：

步骤1.3：恢复图像特征点的三维信息。

已知相机内参K的的情况下，在帧间匹配的两个二维关键点的齐次坐标x₀＝(u₀,v₀,s₀)和x₁＝(u₁,v₁,s₁)间有如下约束：

其中，相机内参为：

f_x和f_y分别为相机焦距，c_x和c_y分别为相机光心的x和y坐标。

在初始化后，持续对每帧图像提取特征点，并根据帧间特征点的匹配信息完成增量建图过程。需要说明的是，各个机器人单独完成各自的同时定位和建图，属于比较成熟的现有技术，本发明对此不做限制。

在各个机器人完成各自的SLAM局部地图后，本实施例通过无线通信手段实时将局部地图更新至计算终端。此外，各机器人还将自身位姿(机器人相对于局部地图世界坐标系下的位姿)上传到计算终端。

步骤2、机器人检测所获取图像中的附着于其他机器人之上的二维码，将检测到二维码的机器人作为观测机器人，将附着有二维码的机器人作为被观测机器人，计算二维码相对于观测机器人的位姿，并通过二维码中携带的标识获取预先标定的二维码在被观测机器人坐标系下的位姿，计算机器人之间的位姿。

本实施例机器人包括空中机器人和地面机器人，在地面机器人上附着有形如图2所示的二维码，其上记载有唯一标识码。空中机器人可以采集到含有地面机器人的图像，地面机器人也可以采集到含有其他地面机器人的图像，在本实施例中将观测到其他机器人上面附着有二维码的机器人称为观测机器人，而将附着有二维码并被其他机器人观测到的机器人称为被观测机器人。

例如，机器人A采集到的图像中含有机器人B，机器人B上附着有二维码，则将机器人A称为观测机器人，而将机器人B称为被观测机器人。而当机器人B采集到的图像中含有机器人C，机器人C上附着有二维码，则将机器人B称为观测机器人，而将机器人C称为被观测机器人。机器人B观测别的机器人时是观测机器人，而在被别的机器人观测到的时候，是被观测机器人。被观测机器人上一定附着有二维码，即一定是地面机器人。

本实施例二维码记载有唯一标识码，本实施例预先设定了关系表，表中包括二维码唯一标识码、机器人编号、二维码在所附着机器人坐标系下的位姿、印刷尺寸。则在获知二维码对应的唯一标识码后，可以查表获取其他信息，即通过查表等方法可以获知该二维码所对应的机器人编号以及预先标定的二维码在所附着机器人坐标系下的位姿、印刷尺寸等二维码数据。

本实施例求解二维码相对于观测机器人的位姿的计算过程，包括：

步骤2.1、观测机器人检测所获取图像中的附着于其他被观测机器人之上的二维码，获取二维码对应的二维码数据。

由于本实施例仅在地面机器人上贴二维码，因此空中机器人、地面机器人在获取图像时，可以检测所获取图像中的附着于其他机器人之上的二维码。在本实施例中空中机器人、地面机器人各自的数量不限，空中机器人所获取图像中如果包括一个地面机器人，则可以检测到该地面机器人上的二维码；同样地面机器人所获取图像中如果包括另一个地面机器人，则可以检测到该另一个地面机器人上的二维码。

使用自适应阈值算法二值化获取的图像，查找图像中的凸四边形，提取四个角点，根据仿射变换将四边形投影至方形图像块内，根据编码规则判断得到二维码对应的唯一标识码，进而通过查表获取二维码数据。

步骤2.2、提取二维码内部角点在图像平面上的坐标。

二维码中内部角点在图像平面上的坐标，可以根据图像的尺寸获取，这里不再赘述。

步骤2.3、根据二维码数据中印刷尺寸，恢复出二维码内部角点在被观测机器人坐标系下的三维坐标，作为PnP算法的物体原始坐标，求解PnP，获得二维码相对于观测机器人的位姿。

在检测到二维码后，容易获得二维码对应的唯一标识码，然后通过查表获得二维码对应的二维码数据，二维码数据中包括印刷尺寸，从而可以恢复出二维码内部角点在被观测机器人坐标系下的三维坐标，作为PnP算法的物体原始坐标。

本实施例通过PnP算法来计算二维码相对于观测机器人的位姿，本实施例将获取图像的机器人也称为观测机器人，将图像中贴有二维码的机器人称为被观测机器人。

步骤2.4：根据二维码数据中预先标定的二维码在被观测机器人坐标系下的位姿，计算得到机器人之间的位姿。

本实施例二维码数据中包括预先标定的二维码在被观测机器人坐标系下的位姿。

机器人之间的位姿计算公式如下：

其中，

表示被观测机器人G相对于观测机器人A的位姿；二维码Q在观测机器人A坐标系下的位姿为

是预先标定的二维码在机器人G坐标系下的位姿，为查表所得，式中

为

的逆，即：

本实施例通过上述步骤计算得到了被观测机器人相对于观测机器人的位姿

由于观测机器人相对于被观测机器人的位姿

与

是互逆的关系，也就得到

需要说明的是，本实施例中可以在观测机器人中计算出二维码相对于观测机器人的位姿后，将二维码相对于观测机器人的位姿上传到计算终端，由计算终端计算机器人之间的位姿；也可以直接在观测机器人中计算，如果在观测机器人中计算，则将计算得到的机器人之间的位姿上传到计算终端。此外，机器人还可以检测每帧图像中出现的二维码，提取含有二维码的视频帧传送至统一的计算终端来完成步骤2中的操作。

步骤3、通过引入的二维码位姿约束，优化各机器人的地图尺度和轨迹，使用局部光束平差法融合当前的全局地图和对应机器人的局部地图，生成新的全局地图。

本发明技术方案引入了二维码位姿约束，包括：机器人的地图尺度、机器人之间的位姿。

其中机器人地图尺度s_m为真实尺寸与机器人建图尺寸的比值，可由IMU预积分估计得出。机器人地图尺度也可从二维码恢复：求得各二维码内部角点的空间坐标，计算各坐标间的空间距离，通过最小二乘法，求解机器人地图尺度。

式中d_map为地图坐标系下二维码内部角点的空间距离，d_tag为世界坐标系真实尺度下二维码内部角点的空间距离。

根据得到的机器人的地图尺度s_m，修正其他机器人的局部地图尺度：

对于机器人i的局部地图中的三维点

匹配观测机器人m地图中的对应三维点

形成匹配的三维点集合

最小化如下函数求解机器人i的地图尺度

上述公式中，s_i为所求机器人i的地图尺度，根据该值缩放对应机器人局部地图使各机器人的地图尺度统一，其中(·)表示i或m。

通过ICP(Iterative Closest Points)配准统一尺度后的各局部地图形成全局地图，在全局地图中通过最小化如下函数优化机器人i的轨迹：

其中，ρ是损失函数，K为匹配的特征点对，(x^k,x^k)为K中的二维点和三维点，π为重投影函数，将三维点根据针孔相机模型投影至图像平面；ICP配准算法使用步骤2中计算的机器人间相对位姿作为观测机器人和被测机器人局部地图间的初始转换矩阵，关于ICP配准算法属于现有成熟的技术，这里不再赘述。

在优化各机器人的轨迹后，使用局部光束平差法融合当前的全局地图和对应机器人的局部地图，生成新的全局地图。本实施例中，通过局部光束平差法来融合机器人的局部地图。由于机器人在运动中不断获取新的图像，各机器人也不断更新上传新的局部地图，计算终端在获取到新的局部地图后，使用局部光束平差法融合当前的全局地图和对应机器人的局部地图，生成新的全局地图，从而实现全局地图的不断更新。

在计算终端，可以获得步骤1上传的局部地图、机器人自身位姿，也可以获得步骤2计算得到的机器人之间的位姿，从而可以来融合生成新的全局地图。在一些可选的实施例中，当机器人附带有惯性测量单元(IMU)时，其创建的三维点云地图的尺度可以由IMU数据辅助确定，二维码几何信息计算得出的尺度转换关系可以与由IMU恢复的尺度信息进行融合。同时，如果GPS信号可用且精度达到合适指标，则通过GPS计算得出的机器人相对位姿亦关系可用于数据融合。

步骤4：将全局地图、地图尺度和各机器人的轨迹广播给各机器人，各机器人根据收到的全局地图和轨迹通过局部光束平差法更新当前的局部地图和自身相对于世界坐标系的位姿，并根据地图尺度修正局部地图。

上述局部光束平差法求解如下方程在姿态链约束下的最优解以优化机器人自身相对于世界坐标系的位姿和局部地图：

其中，

为能量函数，

为重叠视场关键帧

内的关键点，

为

中匹配的关键点对。

根据以上步骤，可以得到如图3所示的协同定位和融合建图结果，可以看出三维点云地图在融合后去除了部分重复的地图点，并优化了各机器人的轨迹。

以上实施例仅用以说明本发明的技术方案而非对其进行限制，在不背离本发明精神及其实质的情况下，熟悉本领域的技术人员当可根据本发明作出各种相应的改变和变形，但这些相应的改变和变形都应属于本发明所附的权利要求的保护范围。

Claims (5)

1.一种空地多视角下的多机器人协同定位及融合建图方法，其特征在于，所述协同定位及融合建图方法，包括：

步骤1、机器人各自进行初始化，各自建立和维护一张局部地图；

步骤2、机器人检测所获取图像中的附着于其他机器人之上的二维码，将检测到二维码的机器人作为观测机器人，将附着有二维码的机器人作为被观测机器人，计算二维码相对于观测机器人的位姿，并通过二维码中携带的标识获取预先标定的二维码在被观测机器人坐标系下的位姿，计算观测机器人与被观测机器人之间的位姿；

步骤3、通过引入的二维码位姿约束，优化各机器人的地图尺度和轨迹，使用局部光束平差法融合当前的全局地图和对应机器人的局部地图，生成新的全局地图；

步骤4、将全局地图、各机器人的地图尺度和轨迹广播给各机器人，各机器人根据收到的全局地图和轨迹通过局部光束平差法更新当前的局部地图和自身相对于世界坐标系的位姿，并根据地图尺度修正局部地图。

2.如权利要求1所述的空地多视角下的多机器人协同定位及融合建图方法，其特征在于，所述步骤2，包括：

步骤2.1、观测机器人检测所获取图像中的附着于其他被观测机器人之上的二维码，获取二维码对应的二维码数据；

步骤2.2、提取二维码内部角点在图像平面上的坐标；

步骤2.3、根据二维码数据中印刷尺寸，恢复出二维码内部角点在被观测机器人坐标系下的三维坐标，作为PnP算法的物体原始坐标，求解PnP，获得二维码相对于观测机器人的位姿；

步骤2.4：根据二维码数据中预先标定的二维码在被观测机器人坐标系下的位姿，计算得到机器人之间的位姿。

3.如权利要求1所述的空地多视角下的多机器人协同定位及融合建图方法，其特征在于，所述引入的二维码位姿约束，包括：机器人的地图尺度、机器人之间的位姿。

4.如权利要求3所述的空地多视角下的多机器人协同定位及融合建图方法，其特征在于，所述优化各机器人的地图尺度，包括：

根据得到的机器人的地图尺度s_m，对于机器人i的局部地图中的三维点

匹配观测机器人m地图中的对应三维点

形成匹配的三维点集合

最小化如下函数求解机器人i的地图尺度

其中，s_i为所求机器人i的地图尺度，根据该值缩放对应机器人局部地图使各机器人的地图尺度统一，其中(·)表示i或m，D_(·)表示D_(i)或D_(m)，

表示

或

表示

或

5.如权利要求3所述的空地多视角下的多机器人协同定位及融合建图方法，其特征在于，所述优化各机器人的轨迹，包括：

通过ICP配准统一尺度后的各局部地图形成全局地图，在全局地图中通过最小化如下函数优化机器人i的轨迹：

其中，ρ是损失函数，K为匹配的特征点对，(x^k,X^k)为K中的二维点和三维点，π为重投影函数，R_i,t_i表示机器人i的位姿。

Images