一种基于多特征视觉里程计和图优化模型的VSLAM方法与流程

本发明属于机器人slam(simultaneouslocalizationandmapping)领域。结合改进的lsd(linesegmentdetector)算法，完成对采集图像中特征线段的提取，尤其涉及一种基于图优化模型的三维地图创建方法。

背景技术：

近年来，随着计算机视觉设备及方法的飞速发展，基于视觉的同时定位与地图创建(visualslam)方法被广泛应用在机器人人机交互、无人驾驶、无人机、虚拟现实以及增强现实等诸多领域。vslam以图像作为环境感知信息源估计摄像机位姿，通过多视图几何理论重构三维地图。

视觉里程计(visualodometry)算法是一种重要的位姿估计方法，仅依靠视觉信息，通过连续配准前后两帧数据获得机器人(相机)的绝对位姿。目前广泛使用图像点特征来配准前后两帧数据。但是，基于点特征的视觉里程计计算量较大，以三维点云作为环境地图描述，精度不高且存储地图所需空间较大。此外，点特征很容易受到光线变化及阴影等环境因素的影响，基于点特征重构的三维地图也不利于后续的场景理解及人机交互等应用。而在结构化的人造环境中，线段特征非常丰富，与点特征相比，其受环境影响较小。线段特征能够很好地刻画环境结构信息，建立更高层次的环境地图，且同时能有效减少特征数量，降低计算复杂度。针对特征的提取与描述计算耗时等问题，近年来出现了基于直接法的视觉里程计，其利用像素梯度变化得到相机位姿，省去了计算特征和描述子的时间，可在特征缺失的场合下使用。但是，当机器人运动较大时，依靠基于梯度搜索的直接法计算获取的机器人位姿可靠性较低，所以直接法只适用于机器人运动较小的场合。此外，基于视觉里程计的vslam算法经过长时间帧与帧的配准，得到的机器人位姿会有累积误差。传统消除累积误差的方法通常基于概率框架(如卡尔曼滤波)，但其计算量较大且主要应用于中小场景中。本发明在gtsam(georgiatechsmoothingandmapping)图优化模型下，首先利用点、线特征估计机器人位姿，然后构建基于点、线特征的观测模型，最后建立用特征描述约束的点、线段特征数据关联方法及系统状态更新模型。最终，融合位姿图与相应帧的三维点完成机器人的自主定位与高精度建图。

技术实现要素：

本发明的目的是提供一种准确且高效的图像集三维地图创建方法。

本发明采用如下的技术方案：

为了充分利用人造环境的结构信息和提高机器人在环境纹理特征少或图像模糊等情况下的定位精度，提出了基于多特征的视觉里程计方法。该方法首先利用fast(featuresfromacceleratedsegmenttest)和改进的lsd算法提取彩色图像中的点、线特征，进而使用不同的描述符描述特征，之后进行特征匹配，最后使用改进的nicp(normaliterativeclosestpoint)算法和pnl(perspectivenline)算法估计机器人初始位姿。对图像提取线特征扩大了算法的应用场景，且得到了较好的机器人初始位姿。随后将多特征视觉里程计表示成贝叶斯网络，在贝叶斯网络基础上得到因子图，并利用最大后验概率估计因子图中机器人的全局位姿，使用高斯-牛顿法求解最大后验概率得到更新后的位姿图。最后，融合位姿图与相应帧的三维点得到重构的三维地图。

本发明的技术特征如下：

(1)基于多特征的视觉里程计

在环境纹理特征少或图像模糊情况下，提取图像特征点不足以得到机器人的位姿，为此，同时提取人造环境中丰富的线段信息以得到较好的机器人位姿。在提取特征时，首先，建立原始图像的5层高斯金字塔，并在其中选取区域极值点或区域内方向一致的线作为所提特征。然后，利用区域极值点或线邻域内像素构成特征的描述子。其次，根据特征的描述子相似性得到匹配的特征。最后，依据多视图几何理论得出匹配特征的旋转平移矩阵，即机器人位姿。基于多特征的视觉里程计具体包括以下四个步骤：

步骤一，提取图像的点、线特征。采用fast算法提取特征点。而采用改进的lsd算法检测图像线特征，其算法步骤为：

1)对原始图像进行高斯降采样，长宽缩放为原来的0.8倍；

2)计算每个像素点的梯度值及梯度方向；

3)根据梯度值的大小对所有像素点排序，建立状态列表，所有像素点的状态均设为0即未使用；

4)将状态列表中梯度值小于2的像素点的状态设置为1即被使用；

5)取出状态列表中梯度最大的像素点作为种子点，并设置其状态为1即被使用，并做以下计算：

a)以种子点为起点，搜索周围状态为0且梯度方向为正负22.5度内的点，更改状态为1；

b)生成包含所有满足点的矩形；

c)判断矩形内的点密度是否大于阈值0.7，若不满足则分割原矩形为多个小矩形框，直至满足该条件；

d)计算nfa(numberoffalsealarms)：

nfa(r)＝(nm)^5/2γb(n(r),k,g)(1)

其中，r代表计算nfa值的矩形框，n和m是采样后图像的行和列，

γ为改变概率阈值的次数，b(n，k，g)符合二项分布，n(r)为矩形框内像素个数，k为满足矩形主方向阈值内的像素个数，g为概率阈值。

e)改变矩形框使nfa的值更小直至小于阈值1，将矩形框加入输出列表。

步骤二，分别使用brief(binaryrobustindependentelementaryfeatures)算法、lbd(linebanddescriptor)算法描述步骤一所提取的点与线特征。

步骤三，计算两帧特征描述符的汉明距离，使用bf(bruteforce)和knn(knearestneighbors)算法获得匹配的特征。根据帧间特征的汉明距离，先利用bf算法进行特征粗匹配，之后使用knn算法获得特征最终匹配结果。

步骤四，计算帧间位姿。根据匹配的特征集合求出3×3的旋转矩阵r和3×1的位移矢量t,使得对于任意的t存在如式(2)的关系。

pi＝rqi+t(2)

其中，pi,qi代表第i组匹配的三维空间特征。然而，在实际中由于误差的存在，需要通过最小化误差来求解r,t。

其中，n为特征最大匹配数量。最终，位姿表示为4×4的矩阵t。

对于匹配的点特征集合，使用改进的nicp算法求解位姿矩阵t，该算法为每个三维空间特征点增添一个用lab颜色空间表示的向量，即特征点表示为p＝(cefαlαaαb)^t，其中，c,e,f分别为特征点的x,y,z轴坐标值，l表示特征点的亮度，a表示特征点从洋红色至绿色变化的颜色值,b表示特征点从黄色至蓝色变化的颜色值。，α为颜色权重，α的取值范围为0.006≤α≤0.03。第k次迭代后的最佳位姿满足以下关系：

其中，pm和qn为第m与n个匹配后用lab颜色空间约束的三维空间特征点，ωmn为pm与qn之间的6×6信息矩阵,是将qn进行位姿矩阵t变换的运算符号。

对于匹配的线特征，使用pnl算法，即通过优化直线投影误差来获得机器人位姿。

(2)基于gtsam模型优化位姿图及创建三维地图

slam的本质是根据观测数据，计算机器人的运动轨迹与地图。然而，在现实中，误差影响了机器人的自定位与建图精度。为了消除误差的影响，本方法将vslam表示为gtsam模型下的图优化问题。首先，假设机器人的运动与测量模型服从高斯概率分布，之后将机器人运动与测量过程表示成贝叶斯网络。其次，用因子图表示贝叶斯网络，并且使用最大化后验概率优化因子图，之后，利用高斯-牛顿法求解最大后验概率，并更新位姿图。最后，融合位姿图与三维点形成最终的三维地图。具体优化位姿图及创建三维地图步骤如下：

步骤一，将多特征视觉里程计表示成贝叶斯网络，其网络结点有三种类型：机器人位置点、路标点、观测点。网络边表示结点之间的联系，网络边能够由机器人的运动与观测模型得到。假设多特征视觉里程计运动与测量模型服从高斯概率分布，则得贝叶斯网络概率p的计算公式如式(6)所示。

其中，xt为第t时刻机器人的位置，x0为机器人初始位置，ut为第t时刻机器人运动控制输入量，lk为第h处机器人测量值，zk为第h处机器人实际观测量，p(xt|xt-1,ut)表示机器人由位置xt-1处在ut控制下移动到位置xt的概率，p(zh|xth,lh)表示机器人在位置xth处通过测量值lh获得观测值zh的概率，m,h表示机器人运动时刻及观测点最大值。

步骤二，将贝叶斯网络表示成因子图，图结点为机器人位置点或路标点，路标点与位置点之间的边由观测模型给出，位置点间的边由运动模型给出。

步骤三，利用最大化后验概率估计因子图中机器人的位姿，并使用非线性最小二乘法中的高斯-牛顿法求解最大后验概率，得到更新的位姿图。

步骤四，将更新后的位姿图与相应帧的三维点融合形成较高精度的三维地图。设空间点a(x,y,z)与其已知像素坐标b(u,v,d)，则根据针孔相机模型推导可得空间点a(x,y,z)计算公式如式(7)所示。

其中，u,v为像素点的图像坐标值，d为像素点的深度数据，fx,fy表示相机在x,y轴上的焦距，cx,cy表示相机在x,y轴上的光圈中心，s表示深度图的缩放因子。

附图说明

图1基于多特征视觉里程计和图优化模型的vslam流程图。

图2图像点、线特征提取与匹配效果示意图。

图3基于多特征视觉里程计和图优化模型的vslam实际建图效果示意图。

具体实施方式

下面结合附图和具体实施方式对本发明作进一步说明。

本发明针对目前vslam算法中存在的图像点特征缺失及估计位姿误差累积的问题，提出了一种基于多特征视觉里程计和图优化模型的vslam方法。该方法首先利用fast和改进的lsd算法提取彩色图像中的点、线特征，进而使用不同的描述符描述特征，之后进行特征匹配，最后使用改进的nicp和pnl算法估计机器人初始位姿。在图像中提取线特征扩大了算法应用场景，且得到了较好的机器人初始位姿。之后将多特征视觉里程计表示成贝叶斯网络，并在贝叶斯网络基础上得到因子图，进而利用最大化后验概率估计因子图中机器人的全局位姿，并使用高斯-牛顿法求解最大化后验概率得到更新后的位姿图。最后，融合位姿图与对应的三维点得到重构的三维地图。所涉及的方法整体流程如附图1所示，具体实施过程分为以下步骤：

步骤一，利用kinect相机得到环境的彩色与深度图像集。

步骤二，使用fast算法、改进的lsd算法提取彩色图像中的点与线特征。

步骤三，使用brief算法、lbd算法对提取的点与线特征进行描述，进而得到特征的描述子。

步骤四，先利用bf算法粗匹配提取的特征，之后使用knn算法得到较精确的匹配特征。

步骤五，使用改进的nicp算法、pnl算法计算匹配的点与线段特征得到机器人初始位姿。

步骤六，在gtsam图优化模型下优化机器人位姿，得到更新后的位姿图。

步骤七，融合更新后的全局位姿图与相应帧的三维点，得到最终的三维地图。

下面为本发明在vslam领域中的一个应用实例。

在实验室环境下对本发明所提方法进行实验验证，实验设备为架设有kinectv1相机的先锋3机器人移动平台。首先，利用fast及改进的lsd算法提取彩色图像中的点、线特征。按照上述具体实施步骤得到图像特征的描述符，随后使用bf算法得到两帧图像特征粗匹配结果，最后使用knn算法得到最终匹配结果。图像点、线特征提取与匹配效果示意图参见说明书附图2。

按照具体实施步骤可以计算得到机器人的初始位姿，之后构建以图像帧空间坐标为节点，帧间估计位姿为边的因子图，并使用高斯-牛顿法求解因子图的最大后验概率得到更新后的机器人全局位姿。最后，融合更新后的位姿图与相应帧的三维点云得到最终的三维地图，完成三维地图创建。对实验室场景使用基于多特征视觉里程计算法和图优化模型的vslam方法进行地图创建，所得实际效果示意图参见说明书附图3。