CN103123727B - 即时定位与地图构建方法和设备 - Google Patents

Abstract

公开了基于矢量的即时定位与地图构建方法和设备。所述方法用于未知环境中的3D定位与地图构建，包括：以未知视角获取一帧点云数据；将所获取的点云数据进行三角形网格化，以形成多个平面，并获得与多个平面中的每一个对应的法向矢量；合并接近的法向矢量以获得法向矢量列表；获得已知环境地图的环境特征矢量表；将法向矢量列表与所述环境特征矢量表中的各矢量进行匹配；将未能匹配的法向矢量列表中的法向矢量添加到环境特征矢量表中以更新环境特征矢量表；利用相互匹配的矢量，计算旋转矩阵，进而计算位移；根据旋转矩阵和位移的计算结果，完成当前帧与已知环境地图的合并。

Description

即时定位与地图构建方法和设备

技术领域

本发明涉及即时定位与地图构建方法和设备。更具体地说，涉及能够减小计算量并提高精度的即时定位与地图构建方法和设备。

背景技术

即时定位与地图构建(SimultaneousLocalizationandMapping，SLAM)是目前在机器人定位方面的热门研究课题。所谓SLAM就是将移动机器人定位与环境地图创建融为一体，即机器人在运动过程中根据自身位姿估计和传感器对环境的感知构建增量式环境地图，同时利用该地图实现自身的定位。

在SLAM中，存在两种坐标系：机器人坐标系和全局坐标系。机器人的位姿是指它在全局坐标系下的绝对坐标，例如三维环境中记为X＝(x，y，z，θ)，其中x，y，z表示在全局坐标系下的坐标，θ表示在该点的姿态(或，视角)。在初始状态下，机器人坐标系与全局坐标系重合。移动机器人在场景中运动，不需要任何先验知识，利用双目立体视觉来感知周围环境信息，并提取稳定的图像特征点来表征3D空间实际物理点，以此作为自然路标，来构建环境的几何地图，同时通过与当前时刻之前所创建的环境地图(自然路标库)中的路标进行匹配，估计机器人当前位姿并更新自然路标库，从而实现移动机器人的SLAM过程。

在现有的2D或3DSLAM应用中，迭代最近点法(IterativeClosestPoint，ICP)是经常使用的算法。该算法通过迭代优化矩阵，在每次迭代过程中，对目标点集上的每个点，在参考点集中寻找最近点，并利用这样的对应点，计算相应的旋转矩阵和平移向量，将其用于目标点集上，得到新的目标点集并进入下次迭代过程，最终得到优秀的转换矩阵，实现两点集的精确配准。然而，该算法的缺点在于：1)深度数据的噪声频繁地导致最近点的不匹配；2)尤其对于大尺度平面的偏移来说，地图校正能力很差；3)定位结果不精确。

另外，基于尺度不变特征变换(Scale-invariantfeaturetransform，SIFT)的SLAM也是已知的。SIFT是一种基于特征的配准方法。SIFT特征匹配算法是DavidG.Lowe在2004年总结了现有的基于不变量技术的特征检测方法的基础上，提出的一种基于尺度空间的、对图像缩放、旋转甚至仿射变换保持不变性的特征匹配算法。该算法匹配能力较强，能提取稳定的特征，可以处理两幅图像之间发生平移、旋转、仿射变换、视角变换、光照变换情况下的匹配问题，甚至在某种程度上对任意角度拍摄的图像也具备较为稳定的特征匹配能力，从而可以实现差异较大的两幅图像之间的特征的匹配。但是，这种方法的缺点在于对于环境亮度和随机出现的路标具有很强的依赖性。

可见，在使用通过范围检测设备(例如，单视场主动红外相机或立体视觉相机)产生的深度数据的2D/3DSLAM中，大多通过点匹配来进行特征匹配。但是由于点云数据(PointCloudData，PCD)是海量的，从而使得处理非常耗时。

发明内容

鉴于以上情形，本发明提出了一种使用环境特征矢量的新的SLAM的算法和工作流程。这些特征矢量表示在待开发的地区或空间中可能频繁出现的平面。

根据本发明实施例的一个方面，提供了一种基于矢量的即时定位与地图构建方法，其用于未知环境中的3D建模与地图构建，包括如下步骤：以一未知视角获取一帧点云数据；将所获取的点云数据进行三角形网格化，以形成多个平面，并获得与多个平面中的每一个对应的法向矢量；合并接近的法向矢量以形成法向矢量列表；获得已知环境地图的环境特征矢量表；将法向矢量列表与所述环境特征矢量表中的各矢量进行匹配；将未能匹配的法向矢量表中的法向矢量添加到环境特征矢量表中以更新环境特征矢量表；以及利用相互匹配的矢量，计算旋转矩阵，进而计算位移；以及根据旋转矩阵和位移的计算结果，完成当前帧与已知环境地图的合并。

此外，在根据本发明实施例的方法中，如果所述特征矢量表不存在，则通过使用该帧的法向矢量表来初始化环境特征矢量表。

此外，在根据本发明实施例的方法中，每一个法向矢量αⁱ由四个元素(αⁱ ₀，αⁱ ₁，αⁱ ₂，αⁱ ₃)构成，其中αⁱ ₃表示坐标原点到与法向矢量αⁱ垂直的平面的距离，αⁱ ₀、αⁱ ₁、αⁱ ₂分别表示x、y、z轴的归一化元素。

此外，在根据本发明实施例的方法中，基于最小均方误差进行所述匹配。

根据本发明实施例的另一个方面，提供了一种基于矢量的即时定位与地图构建设备，其用于未知环境中的3D建模与地图构建，包括：点云数据获取单元，用于以一未知视角获取一帧点云数据；法向矢量获取单元，用于将所获取的点云数据进行三角形网格化，以形成多个平面，并获得与多个平面中的每一个对应的法向矢量；合并单元，用于合并接近的法向矢量以获得法向矢量列表；存储单元，用于存储已知环境地图的环境特征矢量表；特征矢量表获取单元，用于从所述存储单元获取环境特征矢量表；匹配单元，用于将法向矢量列表与所述环境特征矢量表中的各矢量进行匹配；更新单元，用于将未能匹配的法向矢量表中的法向矢量添加到环境特征矢量表中以更新环境特征矢量表；计算单元，用于利用相互匹配的矢量，计算旋转矩阵，进而计算位移；以及3D定位与地图构建单元，用于根据旋转矩阵和位移的计算结果，完成当前帧与已知环境地图的合并。

此外，根据本发明实施例的设备进一步包括：初始化单元，用于如果所述特征矢量表不存在，则通过使用该帧的法向矢量表来初始化特征矢量表。

此外，在根据本发明实施例的设备中，每一个法向矢量αⁱ由四个元素(αⁱ ₀，αⁱ ₁，αⁱ ₂，αⁱ ₃)构成，其中αⁱ ₃表示坐标原点到与法向矢量αⁱ垂直的平面的距离，αⁱ ₀、αⁱ ₁、αⁱ ₂分别表示x、y、z轴的归一化元素。

此外，在根据本发明实施例的设备中，所述匹配单元基于最小均方误差进行所述匹配。

在根据本发明的上述各实施例中，公开了通过将实时扫描的表面法向矢量与动态管理的环境特征矢量表进行匹配，执行由移动对象(例如，机器人或手持设备)使用的SLAM的算法。由于根据现有技术的SLAM中使用点匹配来估计机器人的当前位姿，而在根据本发明实施例的算法中使用面匹配来估计机器人的当前位姿，因此可以极大地减小SLAM的计算负荷。另外，还可以提高输出精度，包括地图和定位结果。

附图说明

图1是图示根据本发明实施例的基于矢量的即时定位与地图构建方法的过程的流程图；

图2A-2C是示意性地示出了帧预处理阶段的各步骤的点云图；

图3是示意性地示出了在图1的步骤S106的匹配处理之前的点云图；

图4是示意性地示出了在图1的步骤S106的匹配处理之后的点云图；以及

图5是图示根据本发明实施例的基于矢量的即时定位与地图构建设备的配置的框图。

具体实施方式

下面将参照附图对本发明的各个优选的实施方式进行描述。提供以下参照附图的描述，以帮助对由权利要求及其等价物所限定的本发明的示例实施方式的理解。其包括帮助理解的各种具体细节，但它们只能被看作是示例性的。因此，本领域技术人员将认识到，可对这里描述的实施方式进行各种改变和修改，而不脱离本发明的范围和精神。而且，为了使说明书更加清楚简洁，将省略对本领域熟知功能和构造的详细描述。

首先，将参照图1描述根据本发明实施例的基于矢量的即时定位与地图构建方法，其用于未知环境中的3D建模与地图构建。

图1图示了基于环境特征矢量的SLAM(environmenteigenvector-basedSLAM，EE-SLAM)的2阶段工作流程。

如图1所示，所述基于矢量的即时定位与地图构建方法包括两个阶段：1)帧预处理；2)基于矢量的SLAM。图2A-2C是示意性地示出了帧预处理阶段的各步骤的处理的图。

阶段1：帧预处理：

首先，在步骤S101，以一未知视角获取一帧点云数据(深度帧)。该步骤的处理结果如图2A所示。需要注意的是，在图2A中，仅示意性地以附图标记“×”示出了点云数据中的几个点。然而，实际上，点云数据是海量的。另外，点云数据是三维分布的。这里，为了便于绘制，仅以二维形式示出。

然后，在步骤S102，将在步骤S101所获取的点云数据进行三角形网格化，以形成多个平面，并获得与多个平面中的每一个对应的法向矢量。该步骤的处理结果如图2B所示。需要注意的是，在图2B中示出了以两个“×”点连接的线段以及与该线段垂直的法向矢量(其以箭头表示)，但是该二维线段所要表示的是以多个点(至少三个点)确定的平面，且以箭头表示的法向矢量垂直于该线段所表示的平面。为了便于绘制，在图2B中仅以二维形式表示。由于与确定平面垂直的法向矢量是唯一的，因此可以每一个法向矢量均代表一个平面，即与其垂直的那个平面。

假设一个平面的方程为Ax+By+Cz+D＝0，与其垂直的法向矢量αⁱ由四个元素(αⁱ ₀，αⁱ ₁，αⁱ ₂，αⁱ ₃)构成，即通过四个参数来限定该唯一的平面。其中αⁱ ₃表示坐标原点到与法向矢量αⁱ垂直的平面的距离，

即 $\frac{\left|D\right|}{\sqrt{A^2+B^2+C^2}},$

αⁱ ₀、αⁱ ₁、αⁱ ₂分别表示x、y、z坐标的归一化元素，

即 ${\mathrm{\α}}_0^i=\frac{A}{\sqrt{A^2+B^2+C^2}},$ ${\mathrm{\α}}_1^i=\frac{B}{\sqrt{A^2+B^2+C^2}},$ ${\mathrm{\α}}_2^i=\frac{C}{\sqrt{A^2+B^2+C^2}}.$

接着，在步骤S103，合并接近的法向矢量以获得法向矢量列表。例如，以一对预先限定的角度和偏移的阈值(例如，分别为δθ和δd)来合并接近的法向矢量。然后将相关联的点投射到新的合并后的平面上。该步骤的处理结果如图2C所示。

然后，在步骤S104，判断是否存在已知环境地图的环境特征矢量表。如果在步骤S104的判断结果表示不存在已知环境地图的环境特征矢量表，换言之，在步骤S101获得的帧是第一帧点云数据且环境地图为空，则处理进行到步骤S105。

在步骤S105，通过使用在步骤S103获得的法向矢量表来创建地图的特征矢量表。例如，地图特征矢量表由{βⁱ}(i＝0，1，......，m-1，m∈N)表示。使用该帧的法向矢量{αⁱ}(i＝0，1，......，n-1，n∈N且n≤m)来初始化{βⁱ}。

如果在步骤S104的判断结果表示已经存在已知环境地图的环境特征矢量表，换言之，在步骤S101获得的帧不是第一帧点云数据且已经存在环境地图，则处理进行到下一阶段：基于矢量的SLAM。

阶段2：基于矢量的SLAM：

在步骤S106，将法向矢量表与环境特征矢量表中的各矢量进行匹配。换言之，在特征矢量表和环境特征矢量表中寻找表示相同或近乎相同的平面的矢量。例如，可以基于最小均方误差进行上述匹配。当然，这并不是限制性的。本领域的技术人员可以理解，任何其他匹配方法也是可能的。

图3是示意性地示出了在图1的步骤S106的匹配处理之前的点云图；而图4是示意性地示出了在图1的步骤S106的匹配处理之后的点云图。在图3中，以“×”表示获取的一帧点云数据，以表示在经过步骤S103的合并处理之后的法向矢量，以“○”表示已知环境地图中的点云数据，并且以表示已知环境地图的特征矢量表中的特征矢量。另外，表示前一视角，并且以虚线包围的区域表示新视角(即，当前视角)的估计区域。

然后，在步骤S107，将未能在环境特征矢量表中找到相匹配的特征矢量的法向矢量表中的法向矢量添加到环境特征矢量表中以更新环境特征矢量表。例如，可有条件地将未能匹配的法向矢量添加到环境特征矢量表中。

接着，在步骤S108，利用相互匹配的矢量，即表示相同平面的法向矢量表中的法向矢量和环境特征矢量表中的特征矢量，计算旋转矩阵。所谓旋转矩阵是指将当前机器人所在的机器人坐标系转换到全局坐标系下的转换矩阵。本质上，SLAM的过程所要解决的问题就是如何将在一新视角处的机器人坐标系转换到全局坐标系的问题。优选地，通过至少三个匹配最好的矢量(其中，各矢量互不平行)，求得上述旋转矩阵，进而计算位移。

最后，在步骤S109，根据旋转矩阵和位移的计算结果，完成当前帧与已知环境地图的合并。

到此为止，已经完成了一帧点云数据的处理。接下来，将继续进入下一帧的扫描和处理。

在上文中已经描述了根据本发明实施例的即时定位与地图构建方法。在下文中，将参照图5描述根据本发明实施例的即时定位与地图构建设备。

图5是图示根据本发明实施例的基于矢量的即时定位与地图构建设备的配置的框图。

根据本发明实施例的基于矢量的即时定位与地图构建设备用于未知环境中的3D建模与地图构建。如图5所示，所述设备500包括点云数据获取单元501、法向矢量获取单元502、合并单元503、存储单元504、环境特征矢量表获取单元505、匹配单元506、更新单元507、计算单元508和3D定位与地图构建单元509。

点云数据获取单元501以一未知视角获取一帧点云数据。例如，点云数据获取单元501可以是单视场主动红外相机或立体视觉相机。

法向矢量获取单元502从点云数据获取单元501接收点云数据，并将所获取的点云数据进行三角形网格化，以形成多个平面。即，通过三个点确定一个平面。然后获得与多个平面中的每一个对应的法向矢量。

合并单元503将法向矢量获取单元502得到的多个法向矢量中接近或完全相同的法向矢量进行合并，从而减少待处理的平面数量。

存储单元504用于存储已知环境地图的环境特征矢量表。环境特征矢量表获取单元505通过访问存储单元504来获取已知环境地图的环境特征矢量表。

匹配单元506从合并单元503和环境特征矢量表获取单元504接收数据，并将法向矢量表与环境特征矢量表中的各矢量进行匹配以在环境特征矢量表中寻找与法向矢量表中的法向矢量表示相同平面的特征矢量。如上所述，这里的匹配可以基于最小均方误差来进行。当然，这并不是限制性的，任何其他的匹配方法也是可能的。

更新单元507将未能在环境特征矢量表中找到相匹配的特征矢量的法向矢量表中的法向矢量添加到环境特征矢量表中以更新环境特征矢量表。

计算单元508利用相互匹配的矢量，计算旋转矩阵，进而计算位移。

3D定位与地图构建单元509根据旋转矩阵和位移的计算结果，完成当前帧与已知环境地图的合并。

另外，设备500还包括初始化单元510，用于当进行首次测量，即获取第一帧点云数据时(此时所述环境特征矢量表不存在)，通过使用合并单元503获得的该帧的法向矢量表来初始化环境特征矢量表。

在上文中，已经描述了根据本发明实施例的即时定位与地图构建方法和设备。在根据本发明实施例的即时定位与地图构建方法和设备中，以法向矢量唯一地表示与其垂直的平面，然后通过全局坐标系下与机器人坐标系下表示相同或相近平面的面匹配(即，法向矢量与特征矢量的匹配)来确定机器人当前的位姿。与采用点匹配的现有技术相比，由于本发明中采用面匹配且在面匹配之前对相同或相近平面进行合并从而进一步减少了需要进行匹配的平面数量，因此大幅度地减小了待处理的数据量，极大地减小了SLAM的计算负荷，并且提高了处理速度。另外，还可以提高包括地图和定位结果在内的输出精度。

另外，由于在本发明中采用的是面匹配，因此在包括多个平面的室内环境中采用根据本发明的即时定位与地图构建方法和设备是特别优选的。

需要说明的是，在本说明书中，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括......”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。

最后，还需要说明的是，上述一系列处理不仅包括以这里所述的顺序按时间序列执行的处理，而且包括并行或分别地、而不是按时间顺序执行的处理。

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

以上对本发明进行了详细介绍，本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本发明的限制。

Claims (8)

1.一种基于矢量的即时定位与地图构建方法，其用于未知环境中的3D建模与地图构建，包括如下步骤：

以一未知视角获取一帧点云数据；

将所获取的点云数据进行三角形网格化，以形成多个平面，并获得与多个平面中的每一个对应的法向矢量；

合并接近的法向矢量以获得法向矢量列表；

获得已知环境地图的环境特征矢量表；

将法向矢量列表与所述环境特征矢量表中的各矢量进行匹配；

将未能在环境特征矢量表中找到相匹配的特征矢量的法向矢量列表中的法向矢量添加到环境特征矢量表中以更新环境特征矢量表；

利用相互匹配的矢量，计算旋转矩阵，进而计算位移；以及

根据旋转矩阵和位移的计算结果，完成当前帧与已知环境地图的合并。

2.根据权利要求1所述的方法，其中如果所述环境特征矢量表不存在，则通过使用该帧的法向矢量列表来初始化环境特征矢量表。

3.根据权利要求1所述的方法，其中每一个法向矢量αⁱ由四个元素(αⁱ ₀，αⁱ ₁，αⁱ ₂，αⁱ ₃)构成，其中αⁱ ₃表示坐标原点到与法向矢量αⁱ垂直的平面的距离，αⁱ ₀、αⁱ ₁、αⁱ ₂分别表示x、y、z坐标的归一化元素。

4.根据权利要求1所述的方法，其中基于最小均方误差进行所述匹配。

5.一种基于矢量的即时定位与地图构建设备，其用于未知环境中的3D建模与地图构建，包括：

点云数据获取单元，用于以一未知视角获取一帧点云数据；

法向矢量获取单元，用于将所获取的点云数据进行三角形网格化，以形成多个平面，并获得与多个平面中的每一个对应的法向矢量；

合并单元，用于合并接近的法向矢量以获得法向矢量列表；

存储单元，用于存储已知环境地图的环境特征矢量表；

环境特征矢量表获取单元，用于从所述存储单元获取所述环境特征矢量表；

匹配单元，用于将法向矢量列表与所述环境特征矢量表中的各矢量进行匹配；

更新单元，用于将未能匹配的法向矢量列表中的法向矢量添加到环境特征矢量表中以更新环境特征矢量表；

计算单元，用于利用相互匹配的矢量，计算旋转矩阵，进而计算位移；以及

3D定位与地图构建单元，用于根据旋转矩阵和位移的计算结果，完成当前帧与已知环境地图的合并。

6.根据权利要求5所述的设备，进一步包括：

初始化单元，用于如果所述环境特征矢量表不存在，则通过使用该帧的法向矢量列表来初始化环境特征矢量表。

7.根据权利要求5所述的设备，其中每一个法向矢量αⁱ由四个元素(αⁱ ₀，αⁱ ₁，αⁱ ₂，αⁱ ₃)构成，其中αⁱ ₃表示坐标原点到与法向矢量αⁱ垂直的平面的距离，αⁱ ₀、αⁱ ₁、αⁱ ₂分别表示x、y、z坐标的归一化元素。

8.根据权利要求5所述的设备，其中所述匹配单元基于最小均方误差进行所述匹配。