一种基于双目视觉和激光融合定位导航系统的制作方法

本发明属于机器人技术领域，具体涉及一种机器人定位与地图构建系统。

背景技术：

同时定位与地图构建(slam)的技术是机器人导航领域一个重要的问题，slam问题可以描述为：机器人能够对探索过的环境建立一个全局的地图，同时能够在任何时候，利用这个地图去推测出自身的位置。机器人通过带有传感器设备在环境中自由的移动，通过采集到的信息对自身位置进行定位，同时在定位的基础上进行地图构建，实现机器人的同时定位和建图。有两个主要的因素影响着slam问题的求解，分别是传感器的数据特征和观测数据相关性，如果能提高数据关联的鲁棒性和准确性并且能够提高传感器数据的利用率，将会提高机器人的定位和地图构建的精度。

目前slam系统中最主流的传感器为相机和激光传感器，但是目前不管是以相机为主的视觉slam还是以激光为主的激光slam其本身都存在一些精度上的问题，所以如何利用多个传感器的数据融合得到更精确的数据定位是当前slam的一个主流研究方向。

技术实现要素：

为了克服已有机器人定位与地图构建系统的鲁棒性较差、准确性较低、适应性较差的不足，本发明提供一种鲁棒性好、准确性高、适应性强的基于双目视觉特征和2d激光传感器信息的机器人定位与地图构建系统。

本发明解决其技术问题所采用的技术方案是：

一种基于双目视觉和激光融合定位导航系统，所述系统包括双目信息采集、特征提取与匹配模块，2d激光点提取、匹配以及位姿估计模块，视觉slam算法初始化和跟踪模块与局部建图模块，视觉slam位姿与2d激光位姿融合模块；

所述双目信息采集、特征提取与匹配模块包括双目orb特征提取子模块、双目特征匹配子模块，所述双目orb特征提取子模块通过接受机器人移动过程中双目相机拍摄的图片，使用orb特征提取算法提取左右图像中的orb特征，将提取的结果存储；所述双目特征匹配子模块对左右图片提取的特征点进行匹配，得到特征点的深度信息；

所述2d激光点提取、匹配以及位姿估计模块包括2d激光数据提取子模块、激光点与2d地图匹配子模块、位姿优化子模块，所述2d激光数据提取子模块从激光传感器提取每一帧激光的数据，并将结果传递给激光点与2d地图匹配子模块；所述激光点与2d地图匹配子模块接受来自激光传感器的数据并对其进行坐标变换后投影到现有的地图中，然后与地图中存在的信息匹配得到机器人可能存在的最佳位姿，该位姿下一步将会作为所述位姿优化子模块的的初始位姿，采用最小二乘迭代算法多次迭代后，计算出一个更优的位姿结果；

所述视觉slam位姿与2d激光位姿融合模块包括视觉slam位姿提取子模块、2d激光位姿提取子模块、位姿判断子模块、位姿输出子模块和建图子模块，所述视觉slam位姿提取子模块将会提取来自视觉slam的位姿增量，所述2d激光位姿提取子模块将会提取来自激光slam的位姿增量，这两个量会输入所述位姿判断子模块，通过引入角度权重与速度权重，判断来自于视觉slam的位姿增量与来自于激光slam的位姿增量哪个更接近于实际值，然后位姿输出子模块将该位姿增量作为最终的位置增量输出给建图子模块，最后更新地图。

本发明创新性的利用双目视觉信息和激光传感器信息以及控制器信息，弥补了单个传感器在某些情况下的信息误差问题，提高了slam算法的鲁棒性。在此基础上，构建出了具备真实尺度信息的地图。

同时定位与地图创建(slam)的技术是机器人领域比较经典的问题，slam问题可以描述为:机器人从某一未知位置开始在未知环境中运动,在移动过程中估计自身位姿和建立环境地图，实现机器人的自主定位和建图。影响slam系统的两个因素有观测数据的相关性和环境噪声，周围环境观测的正确性依赖于好的的数据相关性，进而影响到环境地图的构建。

本发明的技术构思为：本发明提出的机器人slam系统，是基于双目视觉和2d激光信息融合slam系统的一种解决方案。本发明首先利用纯视觉位姿信息对机器人位姿变化进行估计；在视觉位姿中，使用高效率的特征提取算法，对图像提取丰富的描述子，然后利用提取的特征点进行匹配，获取其深度信息；再利用视觉几何知识，对机器人当前位置进行更加精确的估计；再利用纯激光位姿信息对机器人位姿变化进行估计；在激光slam中，使用最新的激光数据与现有的2d地图匹配得到位姿估计，再利用最小二乘迭代优化位姿。再利用来自于激光slam的与视觉slam分别得到的位姿增量，针对直线运动与旋转运动过程中激光slam与视觉slam中存在的一些问题，引入角度权重，以提高不同情况下视觉slam与激光slam的置信度，以提高系统的鲁棒性。

本发明的有益效果主要表现在：采用了视觉信息和2d激光信息融合的方法，能够很好的解决视觉slam中的问题，并提高slam系统的鲁棒性；准确性高，适应性强。

附图说明

图1是本发明系统结构图示。

图2是本发明系统流程图示。

具体实施方式

下面结合附图对本发明作进一步描述。

参照图1和图2，一种基于双目视觉和激光融合定位导航系统，包括双目信息采集、特征提取与匹配模块1、2d激光点提取、匹配以及位姿估计模块2、视觉slam位姿与2d激光位姿融合模块3；所述双目信息采集、特征提取与匹配模块1包括：双目orb特征提取子模块1.1和双目特征匹配子模块1.2；2d激光点提取、匹配以及位姿估计模块2包括：2d激光数据提取子模块2.1、激光点与2d地图匹配子模块2.2和位姿优化子模块2.3；视觉slam位姿与2d激光位姿融合模块3包括：视觉slam位姿提取子模块3.1、2d激光位姿提取子模块3.2、位姿判断子模块3.3、位姿输出子模块3.4和建图子模块3.5。

所述双目信息采集、特征提取和匹配模块1中，双目orb特征提取子模块1.1通过接受机器人移动过程中双目相机拍摄的图片，使用orb特征提取算法提取左右图像中的orb特征，将提取的结果存储；双目特征匹配子模块1.2对左右图片提取的特征点进行匹配，得到特征点的深度信息。

所述2d激光点提取、匹配以及位姿估计模块2中，2d激光数据提取子模块2.1从激光传感器提取每一帧激光的数据，并将结果传递给激光点与2d地图匹配子模块2.2；激光点与2d地图匹配子模块接受来自激光传感器的数据并对其进行坐标变换后投影到现有的地图中，然后与地图中存在的信息匹配得到机器人可能存在的最佳位姿，该位姿下一步将会作为位姿优化子模块2.3的的初始位姿，采用最小二乘迭代算法多次迭代后，计算出一个更优的位姿结果。

所述视觉slam位姿与2d激光位姿融合模块3中，视觉slam位姿提取子模块3.1将会提取来自视觉slam的位姿增量，2d激光位姿提取子模块3.2将会提取来自激光slam的位姿增量，这两个量会输入位姿判断子模块3.3，通过引入角度权重与速度权重，判断来自于视觉slam的位姿增量与来自于激光slam的位姿增量哪个更接近于实际值，然后位姿输出子模块3.4将该位姿增量作为最终的位置增量由位姿输出子模块3.4输出给建图模块位姿输出子模块3.5，最后更新地图。

下面进一步具体介绍对模块的执行情况。

所述双目信息采集、特征提取和匹配模块1中：

双目orb特征提取子模块1.1，在每次机器人采集到左右图像后，用于对图像进行orb特征的提取，选取orb特征作为图像特征的原因在于orb特征具有较好的视角光照不变性，并且orb特征作为一种二进制的视觉特征，具有提取速度快和匹配速度快的特点，很适合要求实时性的slam系统。提取后orb特征后，将提取的结果存储。

双目特征匹配子模块1.2，由于左右图像经过极线校正，搜索匹配点时只需在同一行进行匹配，在每次机器人采集到图像提取特征点后，利用特征点的描述子匹配，利用双目视觉的三角法可计算特征点的绝对尺度，计算公式如下：

在上述公式中，f为相机的焦距，b为两摄像机光心之间的距离；d为视差，即左右图片同一特征点的距离差值，z为特征点的深度；

通过特征点的信息，将其与上一帧或者关键帧中的特征点进行匹配，然后运用重投影误差，建立误差函数，最后通过解最小化误差函数求出相机位姿的变化量。

所述2d激光点提取、匹配以及位姿估计模块2中，2d激光数据提取子模块2.1从激光传感器提取每一帧激光的数据，并将结果传递给激光点与2d地图匹配子模块2.2；激光点与2d地图匹配子模块接受来自激光传感器的数据并对其进行坐标变换后投影到现有的地图中，首先将来自激光传感器的数据由极坐标系转化为直角坐标系下的坐标点：

其中r为每一个激光数据返回得到的距离信息，θ为每一个激光束的角度信息，(si，x，si，y)^t为转化后以激光为中心的每个激光点的直角坐标系下的坐标；

然后，将该坐标由激光为中心的坐标系转变为世界坐标系下的坐标：

其中为机器人当前世界坐标系下的坐标，si(δ)为激光点si＝(si，x，si，y)^t的世界坐标；

然后与地图中存在的信息匹配得到机器人可能存在的最佳位姿，该位姿下一步将会作为位姿优化子模块的的初始位姿，采用最小二乘迭代算法多次迭代后，计算出一个更优的位姿结果；

这里采用的是占用栅格地图，地图中每一个栅格点都以概率表示，对于每一个激光点si(δ)，其概率值用周围四个整数点的概率值通过双线性差值获得：

同时，该点的偏导数表示为：

通过每个点的栅格概率值，我们可以得到一个对应的误差函数：

该式子为最小二乘的形式，最后，通过求解最小二乘的方式，解得该式子的一个最优解。

所述视觉slam位姿与2d激光位姿融合模块3中，视觉slam位姿提取子模块3.1将会提取来自视觉slam的位姿增量，这里先记为然后从2d激光位姿提取子模块3.2将会提取来自激光slam的位姿增量，这里记为考虑我们只是在水平移动的情况下，上述位姿中的(δzca，δαca，δβca)基本不会发生变化，所以同样考虑三个量的位姿变化，这两个量会输入位姿判断子模块3.3，通过引入角度权重，判断来自于视觉slam的位姿增量与来自于激光slam的位姿增量哪个更接近于实际值；

如何根据这两个位姿的变化量得到最终的位姿估计，一种简单的方式是去平均：

考虑到激光与视觉在一些场景下的精度问题：纯激光传感器由于只有距离信息，所以在长廊这种行走过程中距离信息变化不明显或者基本不会发生变化的场景下容易引起自身的定位失败，而视觉由于可以采集空间中的大量特征点，所以即便是在这种周围障碍物距离变化不明显的场景也可以通过测量每个特征点相对位置的变化从而计算自身的位姿变化，从而达到一个较为准确的位姿估计；视觉虽然在长廊这种直线场景下仍然保持较好的特征跟踪性能，但是其在旋转过程中容易出现特征匹配错误或者匹配失败等问题，导致位姿的跟踪错误，然而激光由于只使用距离信息，在旋转过程中每个点对应的距离会产生变化，所以即便只有少数的特征点，其在旋转的情况下依然能够保持较好的跟踪状态。

基于以上在直线行走以及转弯过程中激光slam与视觉slam分别存在的优缺点，引入一个角度权重判断，在直线行走时，为避免出现激光slam中的跟踪错误，所以可以提高视觉的位姿判断的置信度，而在转弯的过程中，更倾向于提高来自激光slam的数据。

最终可以得到位姿增量为：

然后将该位姿增量作为最终的位置增量由位姿输出子模块3.4输出给建图模块位姿输出子模块3.5，最后更新地图。

以在配备双目相机和2d激光传感器的机器人的设备为例，详细介绍本实施在此设备上的执行过程。

首先在双目信息采集和深度信息获取模块1中，对双目视觉信息进行特征提取及特征匹配，这部分将获取的传感器信息传入后续模块，因为机器人在空间中运动，这个部分将不断的对传感器新得到的数据进行采集和提取。利用双目相机采集图像，将图像送至特征提取算法，将提取到的图像特征的位置与描述子保存，同时利用提取到左右图片的特征点，进行匹配计算特征点的深度并保存。然后，利用这些特征点与先前的图像中的特征点匹配，通过最小化重投影误差求得视觉slam下所估计的机器人位姿。

然后在2d激光点提取、匹配以及位姿估计模块(2)中，对激光传感器的数据进行提取，在机器人运动过程中，激光数据是以一定频率实时发送的，该数据将会被2d激光slam所接收。在接收到来自传感器的数据后首先会将该数据转化为以激光坐标为中心的直角坐标系下，然后通过机器人在世界坐标系下的位姿关系将该坐标转化为世界坐标系下的坐标，最后，通过高斯-牛顿方法使激光束与地图对齐，构建最小二乘问题，并通过最小二乘方法的一般解形式求得最后的结果，也就是来自激光slam的位姿。

最后在视觉slam位姿与2d激光位姿融合模块(3)中，视觉slam位姿提取子模块与激光slam位姿提取子模块分别提取来自视觉slam与激光slam的估计位姿，并充分考虑视觉slam与激光slam的优缺点，引入角度权重，使系统在不同情况下分别提高激光或者视觉的位姿估计的置信度，从而提高系统的建图精度。

本说明书的实施例所述的内容仅仅是对发明构思的实现形式的列举，仅作说明用途。本发明的保护范围不应当被视为仅限于本实施例所陈述的具体形式，本发明的保护范围也及于本领域的普通技术人员根据本发明构思所能想到的等同技术手段。