CN109506642A - 一种机器人多相机视觉惯性实时定位方法及装置 - Google Patents

Abstract

本发明公开了一种机器人多相机视觉惯性实时定位方法及装置，包括获取机器人的当前多目图像以及惯性传感器数据，根据当前图像提取图像特征点，估计当前机器人位姿，根据当前机器人位姿重建出3D点云，存储历史及当前点云数据对视觉点云地图进行维护，根据惯性传感器数据完成初始化估计出传感器偏置值，预积分得到当前机器人的速度、角度，根据视觉点云地图、惯性传感器预积分对当前位姿进行优化等步骤，本发明中提及的多目相机利用多个视角的信息提供一个更广阔的视野，多个相机朝向不同，很难出现全部视野被遮挡的状况，并且多个相机提供的视觉特征更为丰富，几乎可以保证满足完成定位需要的特征要求。

Description

一种机器人多相机视觉惯性实时定位方法及装置

技术领域

本发明涉及机器人导航技术，具体地说，涉及一种机器人多相机视觉惯性实时定位方法及装置。

背景技术

目前，越来越多不同类型的机器人出现在生产生活的方方面面，对于仓储物流、巡检监控等领域而言，其工作需要机器人能够在一个相对固定的环境内实现长期稳定的运行，且能实现精确的自定位。在相机视野被障碍物遮挡，视觉特征不明显，特征纹理重复性高难以匹配的情况下，往往会发生定位跟丢的事件。另外，当机器人运动速度过快以致成像模糊时，已有的纯视觉定位方法也难以胜任。多目相机利用多个视角(重叠或非重叠)的信息提供一个更广阔的视野，可以很大程度上解决以上问题。而IMU(惯性测量单元)的引入增强了系统的鲁棒性，使其在快速运动时定位不易丢失，从而达到较好的防抖动效果。多传感器的融合使用，可以获得更多的数据关联，在算法中充分发挥各个传感器的优势信息，从而到达提升定位精度、鲁棒性的效果。

发明内容

为了克服现有技术的不足，本发明的目的在于提供一种机器人多相机视觉惯性实时定位方法及装置，本发明是通过以下技术方案来实现的：

本发明公开了一种机器人多相机视觉惯性实时定位方法，包括以下步骤：

获取机器人的当前多目图像以及惯性传感器数据；

根据当前图像提取图像特征点，估计当前机器人位姿；根据当前机器人位姿重建出3D点云，存储历史及当前点云数据对视觉点云地图进行维护；

根据惯性传感器数据完成初始化估计出传感器偏置值，预积分得到当前机器人的速度、角度；

根据视觉点云地图、惯性传感器预积分对当前位姿进行优化。

作为进一步地改进，本发明所述的多目图像包括多个图像特征，所述的多目图像通过多目相机获取。

作为进一步地改进，本发明所述视觉点云地图具体为通过以下步骤构建：

获取工作环境的多目图像序列；计算各相机图像特征点的重投影误差来估计当前相机系统位姿；所述视觉点云地图由机器人位姿计算得到各相机位姿之后再进行点云重建，所述相机特征点与相机位姿的对应关系，具体为根据下式进行计算：

其中为第i个相机相对于世界坐标系的位姿变换，p_j为第j个世界坐标系下的空间3D点，为第i个相机的相机坐标系相对于像素坐标系的投影变换，u_ji为投影之后获得的第ji个像素点。

作为进一步地改进，本发明所述的相机的位姿更新具体为：

其中，e^μ为多目相机系统的位姿更新的se(3)李代数表示，为第i个相机相对于世界坐标系的位姿变换，E_i1为第i个相机相对于多目相机系统的位姿变换，为位姿更新后的第i个相机相对于世界坐标系的位姿变换。

作为进一步地改进，本发明所述获取机器人的当前多目图像以及当前位姿的具体步骤如下：

获取机器人的当前多目图像、以及机器人的前一时刻的多目图像和前一时刻的位姿；

根据所述当前多目图像、前一时刻的多目图像计算位姿变化量；

根据所述位姿变化量和前一时刻的位姿计算机器人的当前位姿。

作为进一步地改进，本发明所述根据视觉点云地图、惯性传感器预积分对当前位姿进行优化时，也对历史关键位姿进行优化。

作为进一步地改进，本发明所述的定位方法通过以下模块执行：

获取模块，用于获取机器人的当前多目图像以及当前惯性传感器数据；

第一计算模块，用于根据所述当前图像估计当前多目相机系统的位姿；

第二计算模块，用于根据所述当前惯性传感器数据得到当前机器人的角度、速度；

优化模块，用于根据视觉点云地图、惯性传感器预积分对当前位姿进行优化。

本发明还公开了一种机器人多相机视觉惯性实时定位装置，所述的装置包括多目相机和惯性测量单元，多目相机用于获取多目图像，惯性测量单元用于完成初始化估计出传感器偏置值，根据当前传感器数据进行预积分得到当前机器人的速度、角度。

作为进一步地改进，本发明所述多个相机之间刚性连接，各相机之间的位姿相对关系已经事先标定得知；

所述多目相机只优化机器人位姿，其余相机的实时位姿由相对于机器人的关系计算获得；

所述多目相机可有多个双目或单目相机组成，对相机类型不加限制。

作为进一步地改进，本发明所述的定位装置还包括处理器、存储器以及存储在存储器中的程序，所述计算机程序被处理器执行时实现如权利要求1或2或4或6中任一项所述的机器人多相机视觉惯性实时定位方法的步骤。

相比现有技术，本发明的有益效果在于：

本发明公开了一种多相机视觉惯性实时定位方法及装置，获取机器人的当前多目图像以及惯性传感器数据；根据当前图像提取图像特征点，估计当前机器人位姿；根据当前机器人位姿重建出3D点云；根据存储的历史及当前点云数据对点云地图进行维护；根据惯性传感器数据完成初始化估计出传感器偏置值；根据当前传感器数据进行预积分得到当前机器人的速度、角度；根据视觉点云地图、惯性传感器预积分对当前位姿进行优化。

目前移动机器人的主要使用领域在仓储物流、巡检监控等，他们对于机器人的要求是能够在一个相对固定的环境内实现长期稳定的运行，且能实现精确的自定位。

在相机视野被障碍物遮挡，视觉特征不明显，特征纹理重复性高难以匹配的情况下，往往会发生定位跟丢的事件。另外，当机器人运动速度过快以致成像模糊时，已有的纯视觉定位方法也难以胜任。本发明中提及的多目相机利用多个视角(重叠或非重叠)的信息提供一个更广阔的视野，可以很大程度上解决以上问题。多目相机的多个相机朝向不同，很难出现全部视野被遮挡的状况，并且多个相机提供的视觉特征更为丰富，几乎可以保证满足完成定位需要的特征要求。当机器人行走在特征纹理重复性高的场景中(例如左右两侧为白墙的走廊)时，多目相机大概率可以保证有一个视野的特征纹理重复性较低，从而较好的解决这个问题。多目相机的引入可以大大增强不同时刻的机器人观测到同一物体特征的可能性，例如当机器人向前前进5米，又向后倒退5米，只安装机器人朝前的相机是很难识别出机器人返回到原地这一事实的。如果安装上朝后的相机，那么出发时的朝前相机和后退时的朝后相机可以观测到同一物体的多个特征，这就形成了更丰富的数据关联，可以大大提高机器人定位的精度和地图构建的精度。

而IMU(惯性测量单元)的引入增强了系统的鲁棒性，使其在快速运动时定位不易丢失，从而达到较好的防抖动效果。IMU的测量数据与外界环境无关，只与自身的运动有关，因此融合进IMU的数据，可以大大减弱恶劣观测环境对定位精度的影响，从而保证长期定位的稳定性。通过引入惯性传感器数据在对应的时刻对位姿估计值进行优化，能够在机器人运行的过程中不断修正位姿估计的误差、提高定位精度。

本发明提供的多相机视觉惯性实时定位方法，在根据视觉点云地图、惯性传感器预积分对当前位姿进行优化时，也对历史关键位姿进行优化，并且存储历史及当前点云数据对视觉点云地图进行维护。维护点云地图及历史关键位姿，对机器人在一个固定的场景里长期稳定的运行至关重要。在初始的运行一圈构建好整个场景的点云地图之后，本发明提供的算法可以在已有的点云地图上进行定位，从而可以完成高精度的实时定位。

多相机视觉惯性实时定位方法的主要难点，并不在于如何想到，而是在于如何实现他的实时性。多相机提供了更大的视野，与此同时也带来了新的技术难点。更大的视野意味着更多的特征点，更多的环境信息，但也意味着更多的处理时间。提取特征的时间增大可能还会导致之后的建图优化出问题。因此要对多相机视觉惯性实时定位方法做一次提速，本发明在算法中对多相机的处理给出了方案。并不是时时刻刻处理多相机的数据，而是在每一时刻进行判断，之后再决定在当前时刻使用一个、两个、三个或多个相机的特征和信息。这样的处理一举多得，即达到了实时性的要求，又融合进了足够多的信息。多相机视觉惯性实时定位的难点还在于如何在算法中处理好数据关联，每一个相机的图像，它带来的特征、信息，计算出来的该相机位姿，如何与机器人系统匹配，如何与惯性传感器匹配。只有处理好这些，达成完善的传感器信息融合，才能完成算法的实时运行。

多传感器的融合使用，可以获得更多的数据关联，在算法中充分发挥各个传感器的优势信息，从而到达提升定位精度、鲁棒性的效果。

附图说明

图1为本发明机器人多相机视觉惯性实时定位方法及的流程示意图；

图2为图1中优化过程的示意图。

具体实施方式

下面，结合附图以及具体实施方式，对本发明的技术方案作进一步地描述：

图1为本发明机器人多相机视觉惯性实时定位方法及的流程示意图，本发明公开了一种机器人多相机视觉惯性实时定位方法，包括以下步骤：

获取机器人的当前多目图像以及惯性传感器数据；

根据当前图像提取图像特征点，估计当前机器人位姿；根据当前机器人位姿重建出3D点云，存储历史及当前点云数据对视觉点云地图进行维护；

根据惯性传感器数据完成初始化估计出传感器偏置值，预积分得到当前机器人的速度、角度；

根据视觉点云地图、惯性传感器预积分对当前位姿进行优化，图2为图1中优化过程的示意图，从左往右6个系统关键帧代表机器人随时间改变而不断移动，每个时刻之间的定位靠IMU预积分约束和视觉里程计约束进行确定。不同时刻的机器人看到不同的环境，环境由地图上的路标3D点表示，机器人看见环境中的路标点就会产生观测数据，根据这些观测数据可以完成地图的构建和机器人定位，多目机器人的优势就是提供更多的观测，因为机器人看的视野更广阔了，并且可以更容易在不同时刻看见同一路标点，这对于建图和定位都是大大有帮助的。

其中，多目图像包括多个图像特征，多目图像通过多目相机获取；视觉点云地图具体为通过以下步骤构建：

获取工作环境的多目图像序列；计算各相机图像特征点的重投影误差来估计当前相机系统位姿；所述视觉点云地图由机器人位姿计算得到各相机位姿之后再进行点云重建，所述相机特征点与相机位姿的对应关系，具体为根据下式进行计算：

其中为第i个相机相对于世界坐标系的位姿变换，p_j为第j个世界坐标系下的空间3D点，为第i个相机的相机坐标系相对于像素坐标系的投影变换，u_ji为投影之后获得的第ji个像素点。

相机的位姿更新具体为：

其中，e^μ为多目相机系统的位姿更新的se(3)表示，为第i个相机相对于世界坐标系的位姿变换，E_i1为第i个相机相对于多目相机系统的位姿变换，为位姿更新后的第i个相机相对于世界坐标系的位姿变换。

获取机器人的当前多目图像以及当前位姿的具体步骤如下：

获取机器人的当前多目图像、以及机器人的前一时刻的多目图像和前一时刻的位姿；

根据所述当前多目图像、前一时刻的多目图像计算位姿变化量；

根据所述位姿变化量和前一时刻的位姿计算机器人的当前位姿。

根据视觉点云地图、惯性传感器预积分对当前位姿进行优化时，也对历史关键位姿进行优化。

定位方法是通过以下模块执行：

获取模块，用于获取机器人的当前多目图像以及当前惯性传感器数据；

第一计算模块，用于根据所述当前图像估计当前多目相机系统的位姿；

第二计算模块，用于根据所述当前惯性传感器数据得到当前机器人的角度、速度；

优化模块，用于根据视觉点云地图、惯性传感器预积分对当前位姿进行优化。

本发明还公开了一种机器人多相机视觉惯性实时定位装置，装置包括多目相机和惯性测量单元，多目相机用于获取多目图像，惯性测量单元用于完成初始化估计出传感器偏置值，根据当前传感器数据进行预积分得到当前机器人的速度、角度，多个相机之间刚性连接，各相机之间的位姿相对关系已经事先标定得知，多目相机只优化机器人位姿，其余相机的实时位姿由相对于机器人的位姿关系计算获得；多目相机可由多个双目或单目相机组成，对相机类型不加限制，定位装置还包括处理器、存储器以及存储在存储器中的程序，计算机程序被处理器执行时实现机器人多相机视觉惯性实时定位方法的步骤。

从多目相机中获取多目图像；从惯性测量单元(IMU，包括陀螺仪和加速度计)中获取加速度和角加速度，步骤中的提取特征点，重建点云，预积分获得速度、角度，机器人位姿优化等都属于算法，在机器人携带的处理器或电脑上完成。

机器人的位姿可以表示为空间中的x,y,z坐标和朝向，这些可以从预积分获得的速度和角度方便的获得，程序中计算的是机器人系统的位姿，而每一个相机在世界坐标系下的位姿则通过相机相对于机器人系统的外参计算得到，外参为相对位置关系，包括空间中的旋转和平移，外参被认为为不变量，事先标定获得。每一时刻的机器人系统可以重建出当前视野看见的点云地图。在机器人移动的过程中，获得机器人系统的实时位置，可以将实时的点云地图拼接、融合成当前环境的点云地图，完成点云重建。

本实施例中的方法与前述实施例中的装置是基于同一发明构思下的两个方面，本领域技术人员可根据前述描述清楚地了解本实施中的系统的结构及实施过程。

为了描述的方便，描述以上装置时以功能分为各种模块分别描述。当然，在实施本发明时可以把各模块的功能在同一个或多个软件和/或硬件中实现。

通过以上的实施方式的描述可知，本领域的技术人员可以清楚地了解到本发明可借助软件加必需的通用硬件平台的方式来实现。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品可以存储在存储介质中，如ROM/RAM、磁碟、光盘等，包括若干指令用以使得一台计算机设备执行本发明各个实施例或者实施例的某些部分所述的方法，计算机设备可以是个人计算机，服务器，或者网络设备等。

描述的装置实施例仅仅是示意性的，其中所述作为分离部件说明的模块或单元可以是或者也可以不是物理上分开的，作为模块或单元示意的部件可以是或者也可以不是物理模块，既可以位于一个地方，或者也可以分布到多个网络模块上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。本领域普通技术人员在不付出创造性劳动的情况下，即可以理解并实施。

本发明可用于众多通用或专用的计算系统环境或配置中。例如：个人计算机、服务器计算机、手持设备或便携式设备、平板型设备、多处理器系统、基于微处理器的系统、机顶盒、可编程的消费电子设备、网络PC、小型计算机、大型计算机、包括以上任何系统或设备的分布式计算环境等等。

本发明的装置可以包括存储器200、处理器300以及存储在存储器200中的程序，所述程序被配置成由处理器300执行，处理器300执行所述程序时实现上述机器人多相机视觉惯性实时定位方法的步骤。

上述实施方式仅为本发明的优选实施方式，不能以此来限定本发明保护的范围，本领域的技术人员在本发明的基础上所做的任何非实质性的变化及替换均属于本发明所要求保护的范围。

Claims (10)

1.一种机器人多相机视觉惯性实时定位方法，其特征在于，包括以下步骤：

获取机器人的当前多目图像以及惯性传感器数据；

根据当前图像提取图像特征点，估计当前机器人位姿；根据当前机器人位姿重建出3D点云，存储历史及当前点云数据对视觉点云地图进行维护；

根据惯性传感器数据完成初始化估计出传感器偏置值，预积分得到当前机器人的速度、角度；

根据视觉点云地图、惯性传感器预积分对当前位姿进行优化。

2.如权利要求1所述的机器人多相机视觉惯性实时定位方法，其特征在于，

所述的多目图像包括多个图像特征，所述的多目图像通过多目相机获取。

3.如权利要求1或2所述的机器人多相机视觉惯性实时定位方法，其特征在于，所述视觉点云地图具体为通过以下步骤构建：

获取工作环境的多目图像序列；计算各相机图像特征点的重投影误差来估计当前相机系统位姿；所述视觉点云地图由机器人位姿计算得到各相机位姿之后再进行点云重建，所述相机特征点与相机位姿的对应关系，具体为根据下式进行计算：

其中为第i个相机相对于世界坐标系的位姿变换，p_j为第j个世界坐标系下的空间3D点，为第i个相机的相机坐标系相对于像素坐标系的投影变换，u_ji为投影之后获得的第ji个像素点。

4.如权利要求3所述的机器人多相机视觉惯性实时定位方法，其特征在于，相机的位姿更新具体为：

其中，e^μ为多目相机系统的位姿更新的se(3)李代数表示，为第i个相机相对于世界坐标系的位姿变换，E_i1为第i个相机相对于多目相机系统的位姿变换，为位姿更新后的第i个相机相对于世界坐标系的位姿变换。

5.如权利要求1或2或4中任一项所述的机器人多相机视觉惯性实时定位方法，其特征在于，所述获取机器人的当前多目图像以及当前位姿的具体步骤如下：

获取机器人的当前多目图像、以及机器人的前一时刻的多目图像和前一时刻的位姿；

根据所述当前多目图像、前一时刻的多目图像计算位姿变化量；

根据所述位姿变化量和前一时刻的位姿计算机器人的当前位姿。

6.如权利要求5中任一项所述的机器人多相机视觉惯性实时定位方法，其特征在于，所述根据视觉点云地图、惯性传感器预积分对当前位姿进行优化时，也对历史关键位姿进行优化。

7.如权利要求1或2或4或6所述的机器人多相机视觉惯性实时定位方法，其特征在于，所述的定位方法通过以下模块执行：

获取模块，用于获取机器人的当前多目图像以及当前惯性传感器数据；

第一计算模块，用于根据所述当前图像估计当前多目相机系统的位姿；

第二计算模块，用于根据所述当前惯性传感器数据得到当前机器人的角度、速度；

优化模块，用于根据视觉点云地图、惯性传感器预积分对当前位姿进行优化。

8.一种机器人多相机视觉惯性实时定位装置，其特征在于，所述的装置包括多目相机和惯性测量单元，所述的多目相机用于获取多目图像，所述的惯性测量单元用于完成初始化估计出传感器偏置值，根据当前传感器数据进行预积分得到当前机器人的速度、角度。

9.根据权利要求8所述的机器人多相机视觉惯性实时定位装置，其特征在于，所述多个相机之间刚性连接，各相机之间的位姿相对关系已经事先标定得知；

所述多目相机只优化机器人位姿，其余相机的实时位姿由该相机相对于机器人的关系计算获得；

所述多目相机可由多个双目或单目相机组成。

10.根据权利要求8或9所述的机器人多相机视觉惯性实时定位装置，其特征在于，所述的定位装置还包括处理器、存储器以及存储在存储器中的程序，所述计算机程序被处理器执行时实现如权利要求1或2或4或6中任一项所述的机器人多相机视觉惯性实时定位方法的步骤。