专利名称：即时定位与地图构建方法和设备的制作方法
技术领域：
本发明涉及即时定位与地图构建方法和设备。更具体地说，涉及通过采用陀螺仪作为附加传感器来加速定位处理的、基于矢量的即时定位与地图构建方法和设备。
背景技术：
即时定位与地图构建(SimultaneousLocalization and Mapping, SLAM)是目前在机器人定位方面的热门研究课题。所谓SLAM就是将移动机器人定位与环境地图创建融为一体，即机器人在运动过程中根据自身位姿估计和传感器对环境的感知构建增量式环境地图，同时利用该地图实现自身的定位。在SLAM中，存在两种坐标系:机器人坐标系和全局坐标系。机器人的位姿是指它在全局坐标系下的绝对坐标，例如三维环境中记为X = (X，y，Ζ，Θ )，其中X，y, Z表示在全局坐标系下的坐标，Θ表示在该点的姿态(或，视角)。在初始状态下，机器人坐标系与全局坐标系重合。移动机器人在场景中运动，不需要任何先验知识，利用双目立体视觉来感知周围环境信息，并提取稳定的图像特征点来表征3D空间实际物理点，以此作为自然路标，来构建环境的几何地图，同时通过与当前时刻之前所创建的环境地图(自然路标库)中的路标进行匹配，估计机器人当前位姿并更新自然路标库，从而实现移动机器人的SLAM过程。在现有的2D或3D SLAM应用中，迭代最近点法(Iterative Closest Point, ICP)是经常使用的算法。该算法通过迭代优化矩阵，在每次迭代过程中，对目标点集上的每个点，在参考点集中寻找最近点，并利用这样的对应点，计算相应的旋转矩阵和平移向量，将其用于目标点集上，得到新的目标点集并进入下次迭代过程，最终得到优秀的转换矩阵，实现两点集的精确配准。然而，该算法的缺点在于:1)深度数据的噪声频繁地导致最近点的不匹配；2)尤其对于大尺度平面的偏移来说，地图校正能力很差；3)定位结果不精确。另外，基于尺度 不变特征变换(Scale-1nvariantfeature transform, SIFT)的SLAM也是已知的。SIFT是一种基于特征的配准方法。SIFT特征匹配算法是DavidG.Lowe在2004年总结了现有的基于不变量技术的特征检测方法的基础上，提出的一种基于尺度空间的、对图像缩放、旋转甚至仿射变换保持不变性的特征匹配算法。该算法匹配能力较强，能提取稳定的特征，可以处理两幅图像之间发生平移、旋转、仿射变换、视角变换、光照变换情况下的匹配问题，甚至在某种程度上对任意角度拍摄的图像也具备较为稳定的特征匹配能力，从而可以实现差异较大的两幅图像之间的特征的匹配。但是，这种方法的缺点在于对于环境亮度和随机出现的路标具有很强的依赖性。可见，在使用通过范围检测设备(例如，单视场主动红外相机或立体视觉相机)产生的深度数据的2D/3D SLAM中，大多通过点匹配来进行特征匹配。但是由于点云数据(Point Cloud Data，PCD)是海量的，从而使得处理非常耗时。此外，在本申请人于之前提交的另一专利申请中，还提出了基于环境矢量的SLAM方法。在所述方法中，以法向矢量唯一地表示与其垂直的平面，然后通过全局坐标系下与机器人坐标系下表示相同或相近平面的面匹配(即，法向矢量与特征矢量的匹配)来确定机器人当前的位姿。与采用点匹配的技术相比，由于基于环境矢量的SLAM方法中采用面匹配且在面匹配之前对相同或相近平面进行合并从而进一步减少了需要进行匹配的平面数量，因此大幅度地减小了待处理的数据量，极大地减小了 SLAM的计算负荷，并且提高了处理速度。另外，还可以提高包括地图和定位结果在内的输出精度。然而，在这种方法中，由于需要通过在多个特征矢量中寻求匹配来计算旋转矩阵，因此定位速度有待进一步提高。
发明内容
鉴于以上情形，本发明在基于环境矢量的SLAM的基础上，提出了采用陀螺仪作为附加传感器的一种新的SLAM的算法和工作流程。这些特征矢量表示在待开发的地区或空间中可能频繁出现的平面。根据本发明实施例的一个方面，提供了一种基于矢量的即时定位与地图构建方法，其用于未知环境中的3D建模与地图构建，包括如下步骤:使一设置有陀螺仪的设备以一未知视角获取一帧点云数据；将所获取的点云数据进行三角形网格化，以形成多个平面，获得与多个平面中的每一个对应的法向矢量，并合并接近的法向矢量以获得法向矢量列表；获得已知环境地图的环境特征矢量表；通过读取所述设备上的陀螺仪的相应参数，获得所述设备在该未知视角处的姿态，并根据该姿态获得旋转矩阵；将旋转矩阵应用于法向矢量列表中的各法向矢量，从而获得旋转后的法向矢量表;将旋转后的法向矢 量列表与所述环境特征矢量表中的各矢量进行匹配；将未能在环境特征矢量表中找到相匹配的特征矢量的法向矢量列表中的法向矢量添加到环境特征矢量表中以更新环境特征矢量表；利用相互匹配的矢量，计算位移；以及根据所述姿态和位移，完成当前帧与已知环境地图的合并。优选地，在根据本发明实施例的方法中，如果所述环境特征矢量表不存在，则通过使用该帧的法向矢量表来初始化环境特征矢量表。优选地，在根据本发明实施例的方法中，每一个法向矢量Qi由四个元素(α、，α\, Qi2, Qi3)构成，其中Cii3表示坐标原点到与法向矢量a1垂直的平面的距离，α、、
Qi2分别表示X、y、ζ坐标的归一化元素。优选地，在根据本发明实施例的方法中，基于最小均方误差进行所述匹配。优选地，在根据本发明实施例的方法中，通过如下公式计算位移D’:
N-1Dy=-^m1-D1 ,
i=0其中，N表示交叉点的数量，ω 是归一化的加权因子，Di是第i个交叉点的位移，所述交叉点是指在所述法向矢量表中由法向矢量表示的互不平行且不轴旋转对称的三个平面的交点。优选地，在根据本发明实施例的方法中，所述设备为机器人、手机、笔记本电脑或平板电脑。
根据本发明实施例的另一个方面，提供了一种基于矢量的即时定位与地图构建设备，其用于未知环境中的3D建模与地图构建,包括:点云数据获取单元，用于以一未知视角获取一帧点云数据；法向矢量获取单元，用于将所获取的点云数据进行三角形网格化，以形成多个平面，并获得与多个平面中的每一个对应的法向矢量；合并单元，用于合并接近的法向矢量以获得法向矢量列表；存储单元，用于存储已知环境地图的环境特征矢量表；环境特征矢量表获取单元，用于从所述存储单元获取所述环境特征矢量表；陀螺仪，用于获得在该未知视角处的姿态；旋转单元，用于根据该姿态获得旋转矩阵，将旋转矩阵应用于法向矢量列表中的各法向矢量，从而获得旋 转后的法向矢量表；匹配单元，用于将旋转后的法向矢量列表与所述环境特征矢量表中的各矢量进行匹配；更新单元，用于将未能匹配的法向矢量列表中的法向矢量添加到环境特征矢量表中以更新环境特征矢量表；计算单元，用于利用相互匹配的矢量，计算位移；以及3D定位与地图构建单元，用于根据旋转矩阵和位移的计算结果，完成当前帧与已知环境地图的合并。优选地，根据本发明实施例的设备进一步包括:初始化单元，用于如果所述环境特征矢量表不存在，则通过使用该帧的法向矢量表来初始化环境特征矢量表。优选地，在根据本发明实施例的设备中，每一个法向矢量Cii由四个元素(α1。，α\, Qi2, Qi3)构成，其中Cii3表示坐标原点到与法向矢量a1垂直的平面的距离，α、、
Qi2分别表示X、y、ζ坐标的归一化元素。优选地，在根据本发明实施例的设备中，所述匹配单元基于最小均方误差进行所述匹配。优选地，在根据本发明实施例的设备中，所述计算单元通过如下公式计算位移D’:
N-1Dy=-^mrDi ,
i=0其中，N表示交叉点的数量，ω 是归一化的加权因子，Di是第i个交叉点的位移，所述交叉点是指在所述法向矢量表中由法向矢量表示的互不平行且不轴旋转对称的三个平面的交点。在根据本发明实施例的基于矢量的即时定位与地图构建方法和设备中，通过采用陀螺仪作为附加传感器，能够进一步加速定位处理，减小SLAM的计算负荷，并提高包括地图和定位结果的输出精度。
图1示出了根据本发明实施例的基于矢量的即时定位与地图构建方法的流程图；图2A-2C是分别示出了获取点云数据、获得法向矢量以及合并接近的法向矢量的示意图；以及图3是示出了根据本发明实施例的基于矢量的即时定位与地图构建设备的配置的框图。
具体实施例方式下面将参照附图对本发明的各个优选的实施方式进行描述。提供以下参照附图的描述，以帮助对由权利要求及其等价物所限定的本发明的示例实施方式的理解。其包括帮助理解的各种具体细节，但它们只能被看作是示例性的。因此，本领域技术人员将认识到，可对这里描述的实施方式进行各种改变和修改，而不脱离本发明的范围和精神。而且，为了使说明书更加清楚简洁，将省略对本领域熟知功能和构造的详细描述。首先，将参照图1描述根据本发明实施例的基于矢量的即时定位与地图构建方法，其用于未知环境中的3D建模与地图构建。图1示出了根据本发明实施例的基于矢量的即时定位与地图构建方法的流程图。假设待测环境包括静态的、易于识别的多个平面，如墙壁和屋顶。机器人可以频繁地观测到这些平面中的一些或全部，并通过利用它们进行快速定位。因此，将加速的SLAM处理设计为包括图1中所示的如下步骤:首先，在步骤S101，使一设置有陀螺仪的设备以一未知视角获取一帧点云数据。其中，该帧中至少包括三个屏幕。例如，所述设备可以为机器人、手机、笔记本电脑或平板电脑。该步骤的处理结果如图2A所示。需要注意的是，在图2A中，仅示意性地以附图标记“X”示出了点云数据中的几个点。然而，实际上，点云数据是海量的。另外，点云数据是三维分布的。这里，为了便于绘制，仅以二维形式示出。然后，在步骤S102，将所获取的点云数据进行三角形网格化，以形成多个平面，获得与多个平面中的每一个对应的法向矢量，该步骤的处理结果如图2B所示。需要注意的是，在图2B中示出了以两个“ X ”点连接的线段以及与该线段垂直的法向矢量(其以箭头表示)，但是该二维线段所要表示的是以多个点(至少三个点)确定的平面，且以箭头表示的法向矢量垂直于该线段所表示的平面。为了便于绘制，在图2B中仅以二维形式表示。由于与确定平面垂直的法向矢量是唯一的，因此可以每一个法向矢量均代表一个平面，即与其垂直的那个平面。假设一个平面的方程为Ax+By+Cz+D = 0，与其垂直的法向矢量Qi由四个元素(Qi0^i1, Cii2, Cii3)构成，即通过四个参数来限定该唯一的平面。其中Qi3表示坐标原点到与法向矢量α 1垂直的平面的距离，即
权利要求
1.一种基于矢量的即时定位与地图构建方法，其用于未知环境中的3D建模与地图构建，包括如下步骤: 使一设置有陀螺仪的设备以一未知视角获取一帧点云数据； 将所获取的点云数据进行三角形网格化，以形成多个平面，获得与多个平面中的每一个对应的法向矢量，并合并接近的法向矢量以获得法向矢量列表； 获得已知环境地图的环境特征矢量表； 通过读取所述设备上的陀螺仪的相应参数，获得所述设备在该未知视角处的姿态，并根据该姿态获得旋转矩阵； 将旋转矩阵应用于法向矢量列表中的各法向矢量，从而获得旋转后的法向矢量表； 将旋转后的法向矢量列表与所述环境特征矢量表中的各矢量进行匹配； 将未能在环境特征矢量表中找到相匹配的特征矢量的法向矢量列表中的法向矢量添加到环境特征矢量表中以更新环境特征矢量表； 利用相互匹配的矢量，计算位移；以及 根据所述姿态和位移，完成当前帧与已知环境地图的合并。
2.根据权利要求1所述的方法，其中如果所述环境特征矢量表不存在，则通过使用该中贞的法向矢量表来初始化环境特征矢量表。
3.根据权利要求1所述的方法，其中每一个法向矢量α1由四个元素(α α \，aQi3)构成，其中Cii3表示坐标原点到与法向矢量Cii垂直的平面的距离，a; α\、a ^分别表示X、1、ζ坐标的归一化元素。
4.根据权利要求1所述的方法，其中基于最小均方误差进行所述匹配。
5.根据权利要求1所述的方法，其中通过如下公式计算位移D’: N-1i=0 其中，N表示交叉点的数量，Oi是归一化的加权因子，Di是第i个交叉点的位移，所述交叉点是指在所述法向矢量表中由法向矢量表示的互不平行且不轴旋转对称的三个平面的交点。
6.根据权利要求1所述的方法，其中所述设备为机器人、手机、笔记本电脑或平板电脑。
7.一种基于矢量的即时定位与地图构建设备，其用于未知环境中的3D建模与地图构建，包括: 点云数据获取单元，用于以一未知视角获取一帧点云数据； 法向矢量获取单元，用于将所获取的点云数据进行三角形网格化，以形成多个平面，并获得与多个平面中的每一个对应的法向矢量； 合并单元，用于合并接近的法向矢量以获得法向矢量列表； 存储单元，用于存储已知环境地图的环境特征矢量表； 环境特征矢量表获取单元，用于从所述存储单元获取所述环境特征矢量表； 陀螺仪，用于获得在该未知视角处的姿态； 旋转单元，用于根据该姿态获得旋转矩阵，将旋转矩阵应用于法向矢量列表中的各法向矢量，从而获得旋转后的法向矢量表；匹配单元，用于将旋转后的法向矢量列表与所述环境特征矢量表中的各矢量进行匹配； 更新单元，用于将未能匹配的法向矢量列表中的法向矢量添加到环境特征矢量表中以更新环境特征矢量表； 计算单元，用于利用相互匹配的矢量，计算位移；以及 3D定位与地图构建单元，用于根据旋转矩阵和位移的计算结果，完成当前帧与已知环境地图的合并。
8.根据权利要求7所述的设备，进一步包括: 初始化单元，用于如果所述环境特征矢量表不存在，则通过使用该帧的法向矢量表来初始化环境特征矢量表。
9.根据权利要求7所述的设备，其中每一个法向矢量Cii由四个元素(a:Qi1, Qi2,Qi3)构成，其中Cii3表示坐标原点到与法向矢量Cii垂直的平面的距离，a; α\、a ^分别表示X、1、ζ坐标的归一化元素。
10.根据权利要求7所述的设备，其中所述匹配单元基于最小均方误差进行所述匹配。
11.根据权利要求7所述的设备，其中所述计算单元通过如下公式计算位移D’:
全文摘要
一种基于矢量的即时定位与地图构建方法，其用于未知环境中的3D建模与地图构建，包括如下步骤使一设置有陀螺仪的设备以一未知视角获取一帧点云数据；将点云数据三角形网格化，形成多个平面，获得与多个平面中的每一个对应的法向矢量，合并接近的法向矢量以获得法向矢量列表；获得已知环境地图的环境特征矢量表；通过读取陀螺仪的相应参数，获得设备在该未知视角处的姿态，并根据该姿态获得旋转矩阵；将旋转矩阵应用于法向矢量列表中的各法向矢量，获得旋转后的法向矢量表；将旋转后的法向矢量列表与环境特征矢量表中的各矢量进行匹配；更新环境特征矢量表；利用相互匹配的矢量，计算位移；以及根据姿态和位移，完成当前帧与已知环境地图的合并。
文档编号G06T17/05GK103247225SQ20121003184
公开日2013年8月14日 申请日期2012年2月13日 优先权日2012年2月13日
发明者李南君, 张贺 申请人:联想(北京)有限公司