[发明专利]一种基于点线融合的视觉SLAM方法

权利要求书

1.一种基于点线融合的视觉SLAM方法，其特征在于：利用帧间时序性关系提取特征信息进行地图重建；设某个三维路标为X，相机在0,1,…,t-2,t-1,t时刻拍摄了t+1张图像，则最近三帧图像所对应的相机位姿可表示为T_t-2,T_t-1,T_t，该三维路标X所对应的特征点为x_t-2,x_t-1,x_t，将帧间时序性关系与通用的视觉SLAM问题的数学模型相结合，得到贝叶斯概率表示为P(x_t|T_t,X,x_t-1)的特征观测模型；基于得到的贝叶斯概率表示为P(x_t|T_t,X,x_t-1)的特征观测模型确定计算流程；整个系统的计算流程分为特征跟踪、局部建图、回环闭合三个模块。

2.根据权利要求1所述的一种基于点线融合的视觉SLAM方法，其特征在于：利用帧间时序性关系提取及优化特征信息进行地图重建的具体步骤为：

步骤1，输入图像:使用相机拍摄图像，并将每一帧图像依次输入系统中；

步骤2，特征跟踪:

步骤2.1，预测相机位姿，并提取特征点和特征线:根据前两帧的相机位姿，预测当前帧的相机位姿T_t：

其中，为第t-2帧相机到第t-1帧相机的相对位姿，假设帧间运动是平缓的，并将该项作为第t-1帧到第t帧的相对位姿的估计值,由此得出当前帧的相机位姿；提取图像中的特征点，同时获得特征描述子,将特征点集合记为p_t；利用当前帧的特征线的时序关系和预测的相机位姿辅助提取图像中的特征线；对于第一帧图像和初始化或重定位之后的5帧图像，采用LSD算法对图像提取特征线；对于之后输入的图像，利用当时已经存在的特征线序列辅助提取线段，每个特征线序N_l条分别来自不同图像的二维线段，这些线段来自于同一条三维线段的不同投影；对于上一帧得到的每一条特征线，线段的每个参数相对于时间的变化量为使用特征线的长度、角度、中心点的X和Y坐标四个参数值描述一条线段，那么在当前图像中该线段的预测结果为：

其中，l_t-1为上一幅图像中的二维线段，同时使用重投影的方式对线段进行预测；记l_t-1反投影的三维线段为L，根据投影几何关系，预测当前图像中该线段为：

最后对两种预测结果进行融合，得到最终的预测结果：

在预测结果上采样最少N_p个点作为种子，并在图像中提取多条直线，将合并后的直线作为最终的提取结果；

步骤2.2，匹配前后帧的特征点和特征线：对于特征点，利用局部搜索加快匹配速度，首先由前后帧的几何关系，可知

其中，π_p表示点的投影函数；将p_t-1反投影到三维空间中，再投影到当前图像，得到p_t-1对应特征点的估计位置p′_t；然后在以p′_t为中心，5×5的窗口内搜索匹配点；最终使用双向匹配策略，将互为最佳匹配点的特征集合作为匹配结果；

对于特征线，将上一幅图像中的特征线重投影到当前图像中，得到估计的特征线

其中，π_l表示线的投影函数；然后以一定的角度范围作为搜索窗口，匹配二维特征线；最终得到匹配特征线的集合；同时，也根据匹配结果更新特征线序列，将匹配失败的特征线序列从当前帧的记录中移除，将新检测到的特征线序列加入当前帧；

步骤2.3，优化相机位姿：在新图像的几何特征和上一幅图像进行关联之后，对步骤2.1中估计得到的相机位姿，进行优化，重投影得到的特征点p_t′应尽可能和匹配的特征点p_t重合，特征点的重投影误差采用两点的欧式距离度量；而特征线l_t′也应和匹配的特征线l_t重合，特征线的重投影误差采用端点到直线的距离进行度量；本方法融合点和线定义位姿优化的最小二乘问题：

其中，e_p和e_l分别为特征点和特征线的重投影误差，其计算公式为：

e_p＝p_t-π_p(p_t-1,T_t-1) (9)

式(10)中，和分别为直线l_t的两个端点的齐次坐标形式，从而计算出直线l_t的端点到重投影直线的距离之和；式(8)中，为特征点的置信度，其表示为：

其中，n为该特征被检测到的次数，t₁为可调系数,Σ_p为该特征点的协方差矩阵，该矩阵和特征点所在ORB金字塔的级别直接相关；相应地，特征线的置信度为：

其中，σ为特征线长度分布的方差值，t₂和t₃为可调系数，Σ_l为该特征线的协方差矩阵；迭代优化式(8)，直到误差收敛，所得结果即为当前帧的相机位姿；

步骤2.4，优化二维特征线：采用重投影的方式对线段进行优化矫正；首先，将每个特征线序列中的线段反投影到三维空间中：

得到三维线段L′_i,i表示某一个时刻；假设线段的端点服从高斯分布，分别统计两个端点的方差σ_L和均值然后依据σ_L和对三维线段进行调整，

其中，α为优化系数，决定单次优化的力度；换言之，L′_i和均值的距离越大，优化力度越大；而端点分布的方差越大，优化力度越小；最后将优化后的线段投影到图像平面上完成对二维特征线的优化:

步骤2.5，判断是否设为关键帧,若为关键帧，则将其加入地图，否则返回步骤1；

步骤2.5.1，插入关键帧之后已处理多于10帧图像，且当前局部建图模块处于空闲状态，则判断为关键帧；

步骤2.5.2，上一次插入关键帧之后已处理超过20帧图像，则判断为关键帧；

步骤2.5.3，跟踪丢失并重定位后，已处理图像不得少于20帧，否则不能作为关键帧；

步骤2.5.4，当前图像上匹配的特征点不少于30个，且匹配的特征线不少于5条，否则不能作为关键帧；

步骤2.5.5，当前图像上跟踪的特征相比最近关键帧上跟踪的特征少于90％，则判断为关键帧；

若当前帧判断为关键帧，则生成关键帧并传递给局部建图模块以进行后续计算；当前跟踪模块返回步骤1，继续处理下一幅图像；

步骤3，局部建图：

步骤3.1，剔除冗余特征并插入新特征信息：对于新插入的关键帧，首先更新共视图和生成树，加入新关键帧节点；然后对局部地图中的三维特征进行裁剪和更新，由于被观测次数太少的特征有极大的可能是外点，所以裁剪掉三维地图中被观测次数少于三次的特征点和特征线，最后将该关键帧中匹配的新特征，使用三角化方法恢复特征深度信息，生成新三维特征并插入到地图中；

步骤3.2，优化局部地图：完整地图更新之后，对当前关键帧和与之关联的关键帧进行联合优化，主要优化对象为相关关键帧的相机位姿以及三维特征坐标；优化过程中所采用的重投影误差表达式和式(8)相同；在特征匹配时，不可避免地会引入一些外点，因此，在迭代优化10次后，将重投影误差过大的外点移除，再继续迭代优化，如此重复4次，以尽可能地清除外点，并最大程度地优化相机位姿；

步骤3.3，裁剪冗余关键帧：将关键帧之间特征的重合度大于90％的关键帧剔除，以精简共视图；

步骤4，回环检测：

步骤4.1，检测是否形成回环：首先，计算当前关键帧和相邻关键帧之间的词袋相似度，记相似度最小值为s_min，然后在运行时所维护的关键帧数据库中查询，若不存在相似度小于s_min的关键帧，则返回处理下一个关键帧；反之，则视为检测到回环；

步骤4.2，闭合回环：使用随机采样一致性算法计算两个关键帧之间的相似变换，然后将相似变换应用到当前关键帧与相邻关键帧上，完成闭环融合；

步骤4.3，优化全局地图，消除累积误差：对所有关键帧和三维特征进行优化，以消除累积误差，优化过程中，重投影误差的计算方式采用公式(8)所示内容。