提高视觉里程计系统鲁棒性和降低算法计算消耗的方法技术方案

本发明专利技术公开了一种提高视觉里程计系统鲁棒性和降低算法计算消耗的方法，包括：步骤1：采用视觉里程计系统，通过图像传感器采集图像，获得帧p，初始化MatchBufp为空集合；步骤2：判定MatchBufp‑1是否为空，如是，设置相机位姿Twc为0，执行步骤6，如否，执行步骤3；步骤3：计算帧p‑1与帧p之间的相对位姿Tlc；步骤4：计算帧p在地图中的相机位姿Twcp，并构建MatchBufp；步骤5：更新FilterBuf，并判定帧p是否为关键帧；步骤6：构建新的关键帧Knew，并加入到SetK中；步骤7：使用特征匹配初始化新的三维点。本发明专利技术基于图像传感器的实时地图构建和自定位系统，该系统在实现精确鲁棒的构图和定位的同时，减小系统计算资源消耗。

【技术实现步骤摘要】
提高视觉里程计系统鲁棒性和降低算法计算消耗的方法
本专利技术涉及基于视觉的运动估计
，具体涉及一种提高视觉里程计系统鲁棒性和降低算法计算消耗的方法。
技术介绍
VO(VisualOdometry)系统是使用图像传感器作为信息采集工具，进行实时地图构建和自定位的系统。VO是指视觉里程计，视觉里程计VO的目标是根据拍摄的图像估计相机的运动，通过分析一系列图像序列，来确定机器人的朝向和位置。视觉里程计VO主要用于移动机器人的定位导航。VO系统根据原理的不同具体可以分为以下三类：(1)直接法。不需要提取特征点，通常使用所有像素点或者像素梯度值大于某一阈值的像素点进行图像间的匹配。该类方法不需要提取图像特征点，以及计算特征点的描述子，因而通常消耗的计算资源较小。然而因为该类方法使用像素灰度值作为图像匹配依据，因而对于环境光照变化鲁棒性差。典型方法如：LSD_SLAM(Large-ScaleDirectMonocularSLAM，基于单目直接法的大场景实时定位和地图构建)，DSO(DirectSparseOdometry，基于直接法和稀疏点的视觉里程计)等。(2)半直接法。该类方法需要提取特征点，不需要计算特征点的描述子。通常使用快速的特征点提取算法，例如FAST(FeaturesfromAcceleratedSegmentTest)算子。使用以特征点为中心的N*N像素块进行图像匹配。鲁棒性相对于直接法有一些提高，由于特征点的数量较少，因而计算效率相比于直接法也有一些提高。典型方法如：SVO(Semi-DirectVisualOdometry，基于半直接法的视觉里程计)等。(3)间接法(即特征点法)。该类方法针对每一幅图像提取特征点，并针对每个特征点计算描述子。以描述子的相似度作为特征点匹配的依据，完成图像间的匹配。该类方法计算效率底，然而对于环境变化的鲁棒性最好。该类方法虽然计算效率低，然而鲁棒性好，所以目前应用较广泛。典型方法如：ORB_SLAM(基于ORB(OrientedFASTandRotatedBRIEF)特征的视觉定位和地图构建)等。为了节省计算资源，同时获得较好的鲁棒性，本文所述方法将半直接法与间接法结合，完成构建基于关键帧和三维点的拓扑地图，从而同时节约计算资源，并获得更好的鲁棒性。
技术实现思路
本专利技术所要解决的技术问题是：针对现有的VO方法存在的问题，提出半直接法与间接法融合的VO系统，提供了一种提高视觉里程计系统鲁棒性和降低算法计算消耗的方法，基于图像传感器(单目，双目或者多目，目是指VO系统中的相机个数，例如单目是指VO系统中只有一个相机)的实时地图构建和自定位系统，该系统在实现精确鲁棒的构图和定位的同时，减小系统计算资源消耗。本专利技术所述地图包括关键帧集合与三维点集合Nk，NM分别表示两个集合的元素数目。K1表示关键帧集合SetK中第1个关键帧，表示关键帧集合SetK中第Nk个关键帧，Kik表示关键帧集合SetK中第ik个关键帧，关键帧Kik包含{I,Twc,SetF}，包含ik时刻相机采集到的图像I，相机位姿Twc(包含3×3旋转矩阵Rwc和3×1平移矩阵twc)和具有Nf个元素的特征集合Fi＝{xi,yi,si,Di,mi}，其中xi,yi表示第i个特征点在图像上的坐标，si表示该特征点的评分值，由特征点提取算法计算得到，Di表示该点的描述子，由描述子计算算法得到，mi∈{SetM，-1}表示特征点Fi对应的三维点，mi的取值为-1或者M∈SetM，若mi为-1，表示Fi在集合SetM中不存在对应的三维点。M1表示三维点集合SetM中第1个三维点，Mim表示三维点集合SetM中第im个三维点，表示三维点集合SetM中第NM个三维点。三维点Mim，包含{X,Y,Z,SetObs}，具有空间位置信息(X,Y,Z)和观测集合Oi＝{ki,ti}，ki∈SetK，O0＝{k0,t0}，k0∈SetK，k0为生成三维点Mim的关键帧，为生成三维点Mim的特征点，t0为Mim在关键帧k0中对应的特征标号，ti为Mim在关键帧ki中对应的特征标号，即表示Mim对应于中的第ti个特征。所述生成是指已知k0位姿坐标和深度由相机投影关系可以得到Mim的空间位置(Xim,Yim,Zim)，进而初始化三维点Mim。基于Graph的VO地图表示方法通常采用上述模式，在此不再赘述。本专利技术所示MatchBufp为MatchBufp下标p表示图像序列标号，其中，Buf包含信息(x,y,m,D)，m∈SetM，已知生成m的特征点为F={xF,yF,sF,DF,mF}，则m即为mF，D为DF，x,y表示三维点m在帧p中的坐标。Bufi包含信息(xi,yi,mi,Di)，mi为集合SetM中的任意元素，xi,yi表示三维点mi在帧p中的坐标，mi表示三维点，Di表示三维点mi的描述子。本专利技术所示FilterBuf为DFilteri即为SVO中的深度滤波器，在此不再赘述。SVO论文为现有技术，具体可参照文献“基于半直接法的单目视觉里程计”（C.Forster,M.Pizzoli,andD.Scaramuzza.SVO:Fastsemidirectmonocularvisualodometry.InInternationalConferenceonRoboticsandAutomation(ICRA),2014.）。本专利技术采用的技术方案如下：一种提高视觉里程计系统鲁棒性和降低算法计算消耗的方法，包括以下步骤：步骤1：采用视觉里程计系统，通过图像传感器采集图像，获得帧p，初始化MatchBufp为空集合，MatchBufp下标p表示图像序列标号；步骤1中，图像传感器采用单目、双目或多目；MatchBufp为MatchBufp下标p表示图像序列标号，其中，Bufi包含信息(xi,yi,mi,Di)，mi为三维点集合SetM中的任意元素，xi,yi表示三维点mi在帧p中的坐标，mi表示三维点，Di表示三维点mi的描述子；步骤2：判定MatchBufp-1是否为空，如是，设置相机位姿Twc为0，执行步骤6；如否，执行步骤3；步骤3：计算帧p-1与帧p之间的相对位姿Tlc；设置Tlc初始值为0，使用Lucas-Kanade光流算法，迭代优化Tlc，使得MatchBufp-1中的点投影到帧p中得到的灰度值误差最小，从而得到相对位姿Tlc；步骤3中，Lucas-Kanade光流算法可采用现有技术。步骤4：计算帧p在地图中的相机位姿Twcp，并构建MatchBufp；4.1由帧p-1在地图中的相机位姿Twcp-1，以及步骤3中得到的帧p-1与帧p之间的相对位姿Tlc，得到帧p在地图中的相机位姿Twcp：Twcp=Twcp-1*Tlc4.2对于Mi∈SetM，判定Mi在帧p中是否可见，将Mi投影到帧p中，得到Mi在帧p中的投影坐标(xp,yp)，若(xp,yp)在帧p对应的图像Ip区域内，构建Mi在帧p中的Bufi，生成Mi的关键帧为对应的特征为表示Mi在图像上的坐标，表示Mi评分值，由特征点提取算法计算得到，表示Mi点的特征描述子，由描述子计算算法得到，表示对应的三维点，若为-1，表示在SetM不存在对应的三维点；以像素为中心，本文档来自技高网...

【技术保护点】
1.一种提高视觉里程计系统鲁棒性和降低算法计算消耗的方法，其特征在于，包括以下步骤：步骤1：采用视觉里程计系统，通过图像传感器采集图像，获得帧p，初始化MatchBufp为空集合，MatchBufp下标p表示图像序列标号；步骤2：判定MatchBufp‑1是否为空，如是，设置相机位姿Twc为0，执行步骤6，如否，执行步骤3；步骤3：计算帧p‑1与帧p之间的相对位姿Tlc；设置Tlc初始值为0，使用Lucas‑Kanade光流算法，迭代优化Tlc，使得MatchBufp‑1中的点投影到帧p中得到的灰度值误差最小，从而得到相对位姿Tlc；步骤4：计算帧p在地图中的相机位姿Twcp，并构建MatchBufp；步骤5：更新FilterBuf，并判定帧p是否为关键帧；步骤5.1：已知帧p的位姿Twcp和帧p的图像，使用基于半直接法的视觉里程计中深度滤波器更新算法更新FilterBuf，该部分包含将收敛后的三维点加入到SetM，以及删除无效的DFilteri；步骤5.2：判定帧p是否为关键帧；即计算最新的关键帧Knew的位姿

【技术特征摘要】
1.一种提高视觉里程计系统鲁棒性和降低算法计算消耗的方法，其特征在于，包括以下步骤：步骤1：采用视觉里程计系统，通过图像传感器采集图像，获得帧p，初始化MatchBufp为空集合，MatchBufp下标p表示图像序列标号；步骤2：判定MatchBufp-1是否为空，如是，设置相机位姿Twc为0，执行步骤6，如否，执行步骤3；步骤3：计算帧p-1与帧p之间的相对位姿Tlc；设置Tlc初始值为0，使用Lucas-Kanade光流算法，迭代优化Tlc，使得MatchBufp-1中的点投影到帧p中得到的灰度值误差最小，从而得到相对位姿Tlc；步骤4：计算帧p在地图中的相机位姿Twcp，并构建MatchBufp；步骤5：更新FilterBuf，并判定帧p是否为关键帧；步骤5.1：已知帧p的位姿Twcp和帧p的图像，使用基于半直接法的视觉里程计中深度滤波器更新算法更新FilterBuf，该部分包含将收敛后的三维点加入到SetM，以及删除无效的DFilteri；步骤5.2：判定帧p是否为关键帧；即计算最新的关键帧Knew的位姿与当前帧p的位姿twcP之间的位姿关系，如果大于阈值dist_thres，则帧p为关键帧，执行步骤6，否则返回步骤1；步骤6：构建新的关键帧Knew，并加入到SetK中；6.1提取该帧p图像的特征点和对应的描述子，所有帧p的特征点与对应的描述子形成集合SetF，对于任意Fi∈SetF，初始mi在集合SetM中无对应点，设置为-1，得到关键帧Knew，并加入到SetK中；6.2更新MatchBufp，所述的MatchBufp为SetBufNbuf为SetBuf中元素的个数，Bufi包含信息(xi,yi,mi,Di)，mi为集合SetM中的任意元素，xi,yi表示三维点mi在帧p中的坐标，mi表示三维点，Di表示三维点mi的描述子；对于Bufi∈MatchBufp，遍历Fi∈SetF，在SetF中寻找特征Fmin，满足|xi-xmin|+|yi-ymin|≤r，其中r为给定阈值，且Dis(Di-Dmin)最小，其中Dis()用于度量两个描述子之间的相似程度，返回数值越小表示两个描述子越接近，Di为所述Bufi中的描述子，Dmin表示Fmin对应的描述子，若Fmin可以...

【专利技术属性】
技术研发人员：张清泉，王军南，余饶东，唐杭生，
申请(专利权)人：浙江大承机器人科技有限公司，
类型：发明
国别省市：浙江,33