一种基于视觉的离线地图构建及定位方法与流程

本发明涉及机器人定位技术，尤其涉及一种基于视觉的离线地图构建及定位方法。

背景技术：

当前，随着移动机器人技术的发展，越来越多的移动机器人产品进入消费品市场和商用市场。在商业应用的市场领域中，移动机器人主要用来导引、导购、讲解、介绍、迎宾、物品输送等。移动机器人作为一种移动平台，可移动性是其基本的功能，而移动机器人的定位功能则是移动机器人实现自主化的关键技术之一，移动机器人只有了解了自己在当前环境中所处的位置，才能决定下一步要去什么地方？以及怎么样到达目的地？当前移动机器人的定位方法有很多种，诸如：wifi定位、rfid定位、uwb定位、视觉定位、ibeacon定位、无线传感器定位、激光定位和超声波定位等，这些方法各有各的优缺点。其中视觉定位和激光定位技术以其良好的部署特性而越来越受服务机器人企业的重视和使用。

其中在一些专利申请中都给出了一些基于视觉定位的技术，如在专利cn201610997735.5提出了一种基于视觉定位的机器人自主导航方法及系统，该专利虽然提出了用视觉定位来实现机器人来定位，但是并没有较详细地描述如何用视觉定位算法来进行定位，其本质上还只是一种单传感器的视觉里程计的方法，并没有给出如何消除视觉定位的缺点所带来的技术问题。而在专利cn201510400786.0中，公开了一种基于kinect的机器人自定位方法，其方法是通过kinect获取环境的rgb图像和深度图像，通过融合视觉和里程计的信息，来估计机器人的相对运动量，并根据上一时刻机器人位姿，实现位姿跟踪，再将深度信息转换成三维点云，在点云中提取出地面，并根据地面自动标定kinect相对于地面的高度和俯仰角，从而将三维点云投影到地面，得到类似于激光数据的二维点云，将其与事先构建的环境格栅地图进行匹配，从而修正机器人跟踪过程中的累积误差，估计机器人的位姿。该发明本质上还是激光雷达进行定位的方法，并不是真正的视觉定位算法。

而实际上，视觉定位在移动机器人的定位功能中的应用主要源于其成本相对较低，部署相对较容易，但是视觉定位受环境影响非常严重，并且计算量也比较大，对cpu的性能要求比较高，尤其是要在实时定位的场合。目前的视觉定位技术的定位精度相对比较差，速度比较慢，而且经常出现定位失败的情况。当前，将3d摄像头用于服务机器人的定位和导航方面非常普遍，这主要是由于用3d摄像头可以很方便地获取环境的深度信息；但是用3d摄像头进行定位方法各不相同，性能也不一样。有些在定位的时候将3d摄像头当激光雷达的方式来使用，有些虽然是用视觉来定位，但是定位精度差，定位速度慢，算法的鲁棒性也比较差。因此为了改进现有视觉定位中存在的技术问题，急需一种提高机器人视觉定位精度的方法。

技术实现要素：

本发明目的是提供一种基于视觉的离线地图构建及定位方法，基于本发明提出的地图构建方法，移动机器人在定位时是基于视觉、imu(inertialmeasurementunit，惯性测量单元)以及里程计三者结合的定位方法，解决机器人视觉定位失败后，无法获知自己位置而无法进行导航的技术问题，并提高视觉定位的精度。

本发明解决技术问题采用如下技术方案：一种地图的构建方法，所述地图应用于移动机器人的视觉定位技术中，至少包括以下步骤：

a.通过3d摄像头采集机器人应用场合中的rgb图像以及深度图像，计算三维点云的参数；

b.提取所采集图像的orb特征以及描述子，拼接周围环境的三维点云的图像，生成rgb以及深度图像的关键帧；

c.根据所提取的orb特征以及描述子生成词袋，进行聚类，生成orb特征树；

d.分析所述三维点云中所处的空间方向，获得三维点云空间的投影参数，按照需求将与地面垂直方向的点云投影到地面，形成2d地图；

e.获得移动机器人视觉定位所需要的定位地图以及地图的关键帧。

其中，在所述步骤a中，通过离线或在线方式采集图像数据进行三维点云计算；在所述步骤b中，通过特征匹配获得每相邻帧之间的平移和旋转量，并采集每帧图像的imu数据以及里程计数据，计算后获得相机运动量，并构建所述相机的运动特征转移矩阵。

3、根据权利要求2所述的地图的构建方法，其特征在于，在所述步骤b中采用flann算法对两帧图像进行特征匹配计算，并采用卡尔曼滤波算法融合特征匹配算出相机的运动量。

其中，在所述步骤b中，仅对关键帧的图像帧拼接，并采用ba算法对点云地图进行闭环优化。

其中，在所述步骤c中，采用k-means聚类算法构造词袋，生成特征树。

其中，在所述步骤d中，所述三维点云空间的方向采用pca方法分析获得。

其中，所述的地图可离线或者在线构建。

本发明还提供一种移动机器人的定位方法，采用上述各权利要求所述的地图构建方法，包括以下步骤：

实时采集获得环境的rgb以及深度图像；

对采集获得的图像数据进行预处理，并与构建所述地图生成的关键帧进行匹配比较，获得匹配度最高的图片；

对所获得的图片进行计算，获得所采集的图像与匹配最高的图片的匹配帧之间的绝对位置和相对位置信息；

根据地图中由三维点云到2d地图的投影变化计算获得移动机器人定位的2d位置。

其中，还包括获取imu以及里程数据参数，并计算获得所述机器人的位置增量值，校准所述移动机器人的定位。

其中所述机器人的位置增量值的计算包括：采用卡尔曼滤波算法获得机器人姿态欧拉角；以及采用里程计算模型数据计算机器人的位置增量以及姿态角。

本发明具有如下有益效果：

在本发明的地图构建方法中，构建了关键帧的keypoint和descriptor的orb特征树和数据库，提高了所构建的三维地图精度，在投影生成2d地图时，选取了限定范围内的三维点云进行投影，减少天花板物体的投影对定位和导航的影响，提高采用该地图进行定位的准确性。并且在采用该地图构建方法的定位方法中，提出了将视觉定位的结果与航向角、里程计数据进行融合的算法，进一步提高定位的精度和定位的成功率。

附图说明

图1为本发明的具体实施例中地图构建方法中的3d点云地图构建流程示意图；

图2为本发明的具体实施例中地图构建方法中的特征树生成流程示意图；

图3为本发明的具体实施例中地图构建方法中的2d地图生成的投影示意图；

图4为本发明的具体实施例中视觉定位计算过程示意图；

图5为本发明的具体实施例中定位融合算法的过程示意图。

具体实施方式

在本发明中，提出了一种地图的构建方法，主要用于在视觉定位领域中，用来生成周围环境的keyframe(关键帧)，进而实现有效的视觉定位。尽管实时建图、定位和导航是当下的热门，但是实时建图建立的三维点云图不够精确，而且随着地图的增大容易出现很多的问题，因此在本发明的实施例中，给出了在离线状态进行地图构建的方法，采用该离线地图，并在本实施例中，提出了一种视觉定位融合imu和里程计数据的定位方法，解决视觉定位失败的时候机器人的定位问题，具体是通过将地图构建中生成的图片关键帧，生成一个keypoint和descriptor的查找树，提高视觉定位的速度，并对地图构建阶段所形成的环境的三维点云图，通过pca(principalcomponentanalysis)主成分分析，分析出点云空间的方向，计算出三维点云的投影参数，从而生成一个投影到地面的用于机器人定位以及路径规划的参考二维地图。本发明的实施例是基于intel公司的realsense深度摄像头进行建图、定位和导航，同时融合imu和里程计数据进行辅助定位，从而提高整个系统定位算法的鲁棒性，使得机器人在任何时候都不会定位失败。

下面结合实施例及附图对本发明的技术方案作进一步阐述。

本发明提供地图的构建方法，所述地图应用于移动机器人的视觉定位技术中，至少包括以下步骤：

a.通过3d摄像头采集机器人应用场合中的rgb图像以及深度图像，计算三维点云的参数；

b.提取所采集图像的orb特征以及描述子，拼接周围环境的三维点云的图像，生成rgb以及深度图像的关键帧；

c.根据所提取的orb特征以及描述子生成词袋，进行聚类，生成orb特征树；

d.分析所述三维点云中所处的空间方向，获得三维点云空间的投影参数，按照需求将与地面垂直方向的点云投影到地面，形成2d地图；

e.获得移动机器人视觉定位所需要的定位地图以及地图的关键帧。

在本发明的实施例中，在所述步骤a中，通过离线或在线方式采集图像数据进行三维点云计算；在所述步骤b中，通过特征匹配获得每相邻帧之间的平移和旋转量，并采集每帧图像的imu数据以及里程计数据，计算后获得相机运动量，并构建所述相机的运动特征转移矩阵。在所述步骤b中采用flann算法对两帧图像进行特征匹配计算，并采用卡尔曼滤波算法融合特征匹配算出相机的运动量。在所述步骤b中，仅对关键帧的图像帧拼接，并采用ba算法对点云地图进行闭环优化。在所述步骤c中，采用k-means聚类算法构造词袋，生成特征树。在所述步骤d中，所述三维点云空间的方向采用pca方法分析获得。

在本发明中，所述的地图可离线或者在线构建，本实施例中以采用离线构建地图的方法进行具体说明，因为在本发明的实施例中，离线构建地图有助于构建精确完整的环境地图，在构建地图时，采用提取图片orb特征的方式，然后将采集到的图片三维点云拼接成整个环境的三维点云模型。参考图1，地图的构建步骤如下：在本发明中的三维点云地图是离线构建的，利用3d摄像头采集rgb图像和深度图像，计算三维点云，提取图片的orb特征和描述子，拼接周围环境的三维点云图，生成rgb和深度的关键帧。

具体为，在离线的3d点云地图构建算法中，由于视觉定位本质是采用视觉里程计的实现，因此预先构建环境的一张3d点云图或者是一边构建3d点云图进行定位，但是在本实施例中先单独构建环境的3d点云图，在3d地图构建的时候，本发明在离线构建地图方法中加入了里程计数据的进行地图构建，进一步参考图1所示。为了构建精确的三维点云地图，对通过特征匹配计算出的相邻两帧之间的平移量和旋转量，同时采集每帧图像的imu数据和里程计数据，采用卡尔曼滤波算法融合特征匹配算出的运动量和传感器所采集的运动量，得到一个比较准确相机的运动量，构建一个相机的运动特征转移矩阵。在特征匹配时，采用flann算法(该算法为现有技术的内容，在此不再进行赘述)进行特征匹配计算，对于两帧图像的匹配，可能存在完全无法匹配的情况，也可能存在有大量的特征匹配点，其中不乏错误的匹配点，而我们不需要这些特征匹配点；或者匹配特征点之后，通过灰度直方图相似性的方式、最大匹配和最小匹配等方式去掉大部分的不合理的特征匹配点。在进行点云拼接时，只对被选定为keyframe的图像帧进行拼接，拼接的转移矩阵则是融合了多传感器所得到的转移矩阵。为了减少拼接时的误差，可采用稀疏的ba算法对拼接的点云地图进行了闭环优化。

进一步，为了减少机器人定位的时间，本实施例中，在三维点云提取的特征和描述子生成一个词袋，然后利用k-means聚类算法构造一个词袋，并生成orb特征树，这个词袋和特征树在闭环优化和定位阶段都会用到。

在实际使用的过程中，首先提取并产生一本特征点和描述子的词典，内部的单词就是一些特征聚类的结果。所形成的词袋生成一个orb特征树，如图2所示。

另外，构建周围环境的三维点云地图是定位和导航的基础，但在实际应用中，机器人进行定位和导航所使用的地图是环境的二维地图，二维地图的生成是在三维地图的基础上朝某一个平面投影所致，很明显投影的这个平面是机器人所行走的地面这个平面，2d地图的生成过程如下：首先用pca方法分析出三维点云空间的方向，计算出三维点云空间的投影参数；为了减少三维点云空间天花板点云对地面投影所产生的影响，在本实施例中，只把与地面垂直方向的的某些范围点云投影到地面，从而形成2d地图，如图3所示。

本发明还提出一种采用上述地图构建方法的移动机器人的定位方法，包括以下步骤：

实时采集获得环境的rgb以及深度图像；

对采集获得的图像数据进行预处理，并与构建所述地图生成的关键帧进行匹配比较，获得匹配度最高的图片；

对所获得的图片进行计算，获得所采集的图像与匹配最高的图片的匹配帧之间的绝对位置和相对位置信息；

根据地图中由三维点云到2d地图的投影变化计算获得移动机器人定位的2d位置。

在本发明的方案中，还包括获取imu以及里程数据参数，并计算获得所述机器人的位置增量值，校准所述移动机器人的定位。其中所述机器人的位置增量值的计算包括：采用卡尔曼滤波算法获得机器人姿态欧拉角；以及采用里程计算模型数据计算机器人的位置增量以及姿态角。

下面采用具体实施例进行说明，本发明中所述的视觉定位算法主要是，移动机器人实时采集环境的rgb和depth图，图片进行过预处理后与地图构建时生成的keyframe进行匹配，然后得出机器人相机姿态的位置。图片的预处理主要包括depth图的平滑处理，图片的特征提取等。图片的匹配算法是将采集的图片与keframe中每一帧进行比较，计算出8帧与之比较相似候选图片，最后在这8帧图片中选出匹配程度最好的图片，从而计算出采集到的图片和匹配帧之间的绝对位置和相对位置，根据从3d到2d的投影变化计算出所定位出来的2d位置，如图4所示。

由于视觉定位时，受环境光线、深度信息、纹理特征信息等影响，视觉定位时可能定位出来的位置有偏差或者是定位不到位置，这时机器人在环境中，就会迷失方向，从而不会向下一个目标位置移动。为了解决这个问题，在本发明的方案中，将惯性测量单元计算出来机器人的航向角和里程计所记录的编码器的数据融合进机器人的定位过程中。其中在本发明的实施例中，航向角解算是指：将从9轴imu单元都出来的原始的各方向的数据，利用卡尔曼滤波算法解算出机器人的四元数的姿态表示值，从而计算出机器人姿态的欧拉角。里程计模型计算：在本发明中，定位算法不是每时每刻都进行的，只在需要定位的时候才开始定位。因此，自从上一次定位完成，从移动机器人底盘采集到下一次需要定位时的4个电机的编码器的增量值，通过全向轮运动学解算，算出机器人在x方向和y方向的运动增量(有关机器人位置确定的计算过程为现有技术的内容，在此不再详细描述)。根据航向角的值和里程计模型数据算出机器人从上一次定位成功之后，本次定位的位置增量和姿态角。如果在本次视觉定位成功的情况，将imu和里程计算出来的位置增量值与视觉定位结果进行融合，校准本次视觉定位的结果，从而得出比较准确的定位结果；如果在本次视觉定位失败的情况下，那么根据上次imu和里程计定位的结果，再加上本次imu和里程计的计算值，算出机器人最终位置作为本次定位的位置，从而提高机器人的定位效率，具体定位过程如图5所示。

综上所述，本发明的方案有如下有益的技术效果：

1、在构建三维点云地图时，提出了一种离线的地图构建方法，并且融合里程计数据进行建图，提高建立的三维地图的准确性。

2、在构建三维点云地图时，将生成的图像的keyframe所提取的keypoint和descriptor以k-means聚类生成orb特征树，提高机器人的定位速度。

3、在投影生成二位地图时，采用了将距离地面和天花板一定距离内的所有点云同时向某一个平面进行投影的方法，从而避免将天花板物体投影到地面对路径规划所产生的负面影响，同时也将地面上物体投影到天花板投影的平面，从而有利于机器人在路径规划时，避开地面障碍物，提高避障的准确率。

4、在定位时，提出了将视觉定位的结果与航向角、里程计数据进行融合的算法，提高定位的精度和定位的成功率。

以上实施例的先后顺序仅为便于描述，不代表实施例的优劣。

最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。