同步定位与建图方法与流程

本公开属于机器视觉领域，尤其是涉及一种同步定位和建图方法。
背景技术：
：现阶段机器人和/或自动驾驶交通工具的移动大多依靠人工路径规划，机器人和/或自动驾驶交通工具的自主导航能力依赖于同步定位与建图技术(SimultaneousLocalizationandMapping，以下简称为SLAM)。其旨在解决当机器人和/或自动驾驶交通工具进入未知工作环境时，利用传感器信息，对周围环境高效率且准确地进行构建(Mapping)，同时得到设备在空间中的位置与姿态(Localization)。除了可以应用在机器人领域外，虚拟现实与增强现实设备的空间追踪以及自动驾驶也同样可以使用SLAM技术。SLAM问题自提出以来已经过去了近30年，这期间SLAM问题所用的传感器与计算方法都发生了巨大的变化。现在的SLAM技术大多采用视觉为主的传感器，包括单目传感器，多目传感器，以及彩色图像及深度信息(RGB-D)的传感器等。现阶段主流的基于视觉的SLAM方案可根据优化方法分为两类，一类为使用滤波器的SLAM方法，另一类的是使用图优化的SLAM方法。使用滤波器的SLAM方案模型构建比较简单，但误差会逐渐累计不可修复。在这类方法中，Davison等人在文献“A.Davison,I.Reid,andN.Molton.MonoSLAM:Real-TimeSingleCameraSLAM.IEEETransactionsonPatternAnalysisandMachineIntelligence,pp.1052-1067,2007.”中提出的MonoSLAM方案是基于扩展卡尔曼滤波器(简称EKF)的单目传感器SLAM方案，通过在图像中构建路标点并使用EKF对SLAM进行模型构建而求解。是基于滤波器的SLAM中比较优秀的方案。使用图优化方法的SLAM方案由于要构建姿态图往往运算量比较大。在这类方法中，Artal等人在文献“R.Artal,J.Montiel,andJ.Tardos.ORB-SLAM:AVersatileandAccurateMonocularSLAMSystem.WorkshoponResearch,EducationandDevelopmentofUnmannedAerialSystems,pp241-248,2015.”中提出的ORB-SLAM是目前基于图优化方法的单目视觉SLAM最优方案。ORB-SLAM通过对图像提取ORB特征描述符(一种提取速度极快的特征描述符)，可以达到很高的速度。并且通过完善的图优化操作，得到的地图也具有很高的精确度。以上两种方法是目前视觉SLAM采用滤波器或图优化方法中比较有代表性的方案，也是实际使用中比较主流的方案。但现有方法仍有一定不足。首先单目SLAM方案难以估计现实世界的尺度，往往难以构建出准确规模的地图；其次SLAM方案生成的一般是三维点云地图，由空间中的散点构成，在空间上不具备连续性，无法用于导航；另外基于图优化方案的SLAM会将每帧图像加入姿态图进行优化，会导致姿态图过大优化速度慢。技术实现要素：本公开的实施例关于一种同步定位与建图方法，包括基于帧间运动的大小从多个图像帧中选出关键帧序列，基于关键帧序列的帧间匹配结果将多个图像帧导入姿态图，基于姿态图构建一个或多个有效检测回环以用于进行捆集调整以及基于所捆集调整的姿态图修正多个图像帧。在一些实施例中，多个图像帧包括彩色图像和深度图像两者。在一些实施例中，将修正的多个图像帧变换为三维点云并拼接为点云地图。在一些实施例中，基于八叉树建图将点云地图转化为三维栅格地图。在一些实施例中，关键帧序列包括多个图像帧的第一帧。在一些实施例中，基于帧间运动的大小选出关键帧序列包括当图像帧与关键帧序列中的最后一帧之间帧间运动的值在第一帧间运动阈值以上且在第二帧间运动阈值以下时，将该图像帧添加入关键帧序列作为新的最后一帧。在一些实施例中，当图像帧与关键帧序列中的最后一帧之间帧间运动的值小于第一帧间运动阈值或大于第二帧间运动阈值时，丢弃该图像帧并导入下一图像帧。在一些实施例中，姿态图包括代表相机姿态的状态变量的节点和代表不同姿态运动变换关系的连接节点的边。在一些实施例中，姿态图基于每一图像帧与其前后帧之间的运动变换关系而形成为线状。在一些实施例中，构建检测回环包括在具有至少一个可匹配特征点的非相邻节点之间建立运动变换关系。本公开的实施例引入深度图像准确估计环境的尺度，通过八叉树建图建立三维彩色栅格地图用于导航，并且提取关键帧加入姿态图，有助于压缩图优化规模从而提高优化速度和/或效率。该方法相比于同类算法有20％-50％的效率提升，通过引入深度图像信息获得了比较准确的现实环境地图尺度，以及使用八叉树建图得到的彩色三维栅格地图可以直接用于导航。附图说明本公开提供了附图以便于所公开内容的进一步理解，附图构成本申请的一部分，但仅仅是用于图示出体现发明概念的一些发明的非限制性示例，而不是用于做出任何限制。图1是根据本公开一些实施例的同步定位与建图方法的流程图。图2分别是根据本公开一些实施例的同步定位与建图方法所建立的地图在优化之前与之后的三维点云地图。图3是根据本公开一些实施例建立栅格地图的示意图。图4是根据本公开一些实施例在姿态图中构建检测回环的示意图。具体实施方式下文将使用本领域技术人员向本领域的其它技术人员传达他们工作的实质所通常使用的术语来描述本公开的发明概念。然而，这些发明概念可体现为许多不同的形式，因而不应视为限于本文中所述的实施例。提供这些实施例是为了使本公开内容更详尽和完整，并且向本领域的技术人员完整传达其包括的范围。也应注意这些实施例不相互排斥。来自一个实施例的组件、步骤或元素可假设成在另一实施例中可存在或使用。在不脱离本公开的实施例的范围的情况下，可以用多种多样的备选和/或等同实现方式替代所示出和描述的特定实施例。本申请旨在覆盖本文论述的实施例的任何修改或变型。对于本领域的技术人员而言明显可以仅使用所描述的方面中的一些方面来实践备选实施例。本文出于说明的目的，在实施例中描述了特定的数字、材料和配置，然而，领域的技术人员在没有这些特定细节的情况下，也可以实践备选的实施例。在其它情况下，可能省略或简化了众所周知的特征，以便不使说明性的实施例难于理解。此外，下文为有助于理解说明性的实施例，将各种操作依次描述为了多个离散的操作；然而，所描述的顺序不应当被认为是意味着这些操作必须依赖于该顺序执行。而是不必以所呈现的顺序来执行这些操作。下文中的“在一些实施例中”，“在一个实施例中”等短语可以或可以不指相同的实施例。术语“包括”、“具有”和“包含”是同义的，除非上下文中以其它方式规定。短语“A和/或B”意味着(A)、(B)或(A和B)。短语“A/B”意味着(A)、(B)或(A和B)，类似于短语“A和/或B”。短语“A、B和C中的至少一个”意味着(A)、(B)、(C)、(A和B)、(A和C)、(B和C)或(A、B和C)。短语“(A)B”意味着(B)或(A和B)，即A是可选的。参考图1所示的方法流程图，在一些实施例中，同步定位和建图需要先取得一系列的多个图像帧作为输入，如步骤S101所示。该多个图像帧包括彩色图像和深度图像，尤其是可以包括在时间上同步采集的彩色与深度图像序列。本领域技术人员将认识到可以使用同一时刻的彩色图像与深度图像生成该时刻的点云地图，如可选的步骤S102所示。由于彩色及深度图都是二维图像，对图像建立二维笛卡尔坐标系，则可以用(u,v)来表示图像上任意一个像素点p。对彩色图像中的任意一点可以提取出该点对应的颜色信息，即r,g,b颜色分量，也可以对深度图像中的该点提取出深度值d。对于x,y,z分量，利用小孔相机模型依靠求解u,v,d来获得。图像中的信息u,v,d关于空间点P的x,y,z的关系为：S·uvd=C(R·xyz+t)---(1)]]>其中C为相机的内参矩阵，由相机标定得出。相机可以包括彩色相机和/或深度相机。S是缩放矩阵，默认为单位阵。R为旋转矩阵，t为位移矩阵。对于单张图片来说，不存在视角的旋转和位移，所以在单帧图像转换时忽略这两个量。由此，可以将二维的图像信息转化到真实的三维世界坐标系中。对图像中所有的点全部进行变换，得到上述的点云。在可选的步骤S103、S104和S105中，可以利用可选步骤S102中得到的初步点云，对所有图像提取SIFT特征描述符(可替换为其他特征描述符，如ORB等)，该步骤可以使用本领域技术人员熟知的计算机视觉库OPENCV中的SIFT特征描述符提取函数来进行，本处不作详述。然后使用近似最近邻法(简称为FLANN)对每相邻两帧图像进行特征匹配，随后使用随机一致性估计(简称为RANSAC)估计两帧图像之间拍摄相机的姿态变化，下文中又将两帧之间相机的姿态变化简称为“帧间运动”，相机的姿态变化可以包括轨迹变化、俯仰变化、平移变化和/或旋转变化之类的任何相机与参考系相对姿态的变化。以上可选步骤利用得到的帧间运动估计对单幅点云进行拼接，可以得到初步的点云地图。该初步的点云地图是用于与经过下文所述的优化步骤后的最终点云地图进行比较，以进一步例示本公开实施例方法取得的效果。在步骤S106中，根据帧间运动估计大小来从一系列的输入图像帧中选取合适的关键帧，从而得到一个关键帧的序列。设关键帧序列为F，则可先将第一帧f0放入序列，之后导入图像序列的每一帧图像i，并计算每帧i与关键帧序列F中最后一帧(即，最近一帧)的帧间运动大小T。若T＜Tmin则认为图像i距离前一个关键帧过近，放弃并开始导入下一帧。若T＞Tmax则认为图像i运动过远，同样放弃并开始导入下一帧。Tmin与Tmax为所设定的第一阈值和第二阈值，分别代表关键帧间运动的最小运动距离与最大运动距离。若两帧图像无法匹配，则认为图像i存在问题，如模糊和/或大面积遮挡，则同样抛弃并开始导入下一帧。剩余的情况，即帧间运动大小为第一阈值和第二阈值之间时，则将图像i定义为关键帧Fi并加入关键帧序列F末尾。由此，最终得到的关键帧序列F内将包含所需要的所有关键帧。在步骤S107中，将关键帧的姿态信息加入姿态图。首先构建一个由节点(vertices)与边(edge)组成的姿态图，节点代表相机姿态的状态变量，而连接两个节点的边代表不同位姿下图像之间的约束关系，即运动变换关系。在上一步中，通过计算可以得到相机在帧间匹配后得到的运动估计旋转矩阵R与位移矩阵t。若将相机的姿态表示为P，也就是姿态图中的节点，将姿态之间的变换关系表示为T，也就是姿态图中的边，那么有：Pi=R3×3t3×1O1×31iTi,j=R3×3t3×1O1×31i,j---(2)]]>同时设相机第一帧图像姿态为P0，且为4×4大小的单位矩阵。则随着图像序列的导入有：Pj＝Ti,jPi(3)即第i帧图像利用与下一帧图像的帧间匹配结果T，将下一帧也就是第j帧关键帧的节点信息(姿态)加入姿态图，同时T也作为两个节点之间的约束条件，如图4中T1,2和/或T2,3所示。图中的Pi代表姿态节点，Ti,j代表不同姿态之间的运动变换约束，如P1代表第一帧图像的摄像机位姿，T1,2代表第一帧图像与第二帧图像之间的运动关系约束。当全部图像导入姿态图后，即可获得每帧与其前后帧相关的姿态图，该姿态图可理解为线状的，如上述可以完成姿态图的初步构建。相机运动过程中，如果有某些场景的图像与之前某时刻的图像匹配结果较为接近，则在某种程度上可以认为相机回到了之前来过的某个位置。这种情况下通过使用这组相似场景之间的运动变换得到对应相机姿态之间的运动变换，可以利用该运动变换对之前所估计的相机姿态进行修正，使修正后的各姿态轨迹更接近真实值。在本公开的实施例中，利用这种较为匹配的非相邻图像帧之间的运动变换建立运动变换关系T以用于结果的优化，在姿态图中这表现为线状姿态图中原非相邻节点之间回环状结构的建立，建立回环结构的非相邻节点虽然在时间上不连续，但相互具有相匹配的至少一个特征点。如图4中所示，P为不同帧对应的姿态，T为不同姿态之间的运动变换，图中姿态P1，P2，P3在没有加入回环检测T1,3时，之间的约束只有T1,2和T1,3，即如上文所述的线状。现假设非相邻图像帧的姿态P1与姿态P3中有一定数目可以匹配的特征点，则可以新添加一条运动变换约束T1,3，即构成了检测回环。类似地，可以如图4所示建立任意数量的检测回环以用于后续的优化步骤。如步骤S108所示，对姿态图进行捆集调整，得到优化后的姿态图。在根据应用要求已构建足够数量的检测回环之后，可以对姿态图进行全局的姿态优化，对于全局姿态的优化被称为捆集调整(Bundleadjustment)。可以利用非线性最小二乘法来实现全局误差的最小。这个过程使用Levenberg-Marquardt(LM)法来实现迭代策略。由于边中存在误差，使得所有边得到的数据并不一致，于是优化的是整个过程中的不一致性误差：minE=Σi,j||xi*-Ti,jxj*||22---(4)]]>x(t+1)*=x(t)*-η*▿xE---(5)]]>这里代表对优化变量xi的估计值，若上式收敛，就可以在优化结束后获得最小的误差E，即最接近真实值的姿态图。在本领域中存在多种用于捆集调整的方法，其中一种通过使用Kümmerle等人在文“R.Kümmerle,G.Grisetti,H.Strasdat,K.KonoligeandW.Burgard,“G2o:Ageneralframeworkforgraphoptimization,”IEEEInternationalConferenceonRoboticsandAutomation,pp.3607-3613,2011.”中提出的图优化工具g2o(GeneralFrameworkforGraphOptimization)来进行，此处不再赘述。在步骤S109中，利用捆集调整后的姿态图对彩色/深度图像帧，即点云进行重新拼接，以得到最终准确的点云地图。设每一帧图像生成的点云为pcli。通过下式可以利用较准确的相机姿态Pi将所有帧的点云合并到同一个坐标系中，得到最终环境的点云图像pclmap。pcloi＝Pi×pcli(6)pclmap=Σi=0Npcloi---(7)]]>可选地，在步骤S110-S111中，使用八叉树建图工具将点云地图转化为可用于导航的三维栅格地图。使用本领域中用于对点云进行处理的Octomap工具将地图转化为三维栅格地图，同时根据真实世界的色彩对栅格单位方格进行染色。完成转化后，本方法可以得到例如如图3所示的最终的三维彩色栅格地图。本公开实施例的同步定位与建图方法还在中央处理器为i5-45903.4GHzCPU、内存为8G、OS为Ubuntu操作系统的运行环境中进行了测试，在测试中用来构建地图的彩色与深度测试序列cafe来自于NYUv2数据集，该数据库由N.Silberman等人在文献“N.Silberman,D.Hoiem,andP.Kohli,IndoorSegmentationandSupportInferencefromRGBDImages,EuropeanConferenceonComputerVision,pp746-760,2012”中提出，为750对RGB彩色图像与深度图像，分辨率均为640*480，帧率为30Hz。将完整序列导入本公开构建的SLAM系统，并根据包括可选步骤的上述步骤生成初步的点云地图和优化后的点云地图，之后对比点云地图的细节准确程度，生成速度以及稳定性。图2为展示本公开优化前后的的点云地图。首先在整体上两幅地图规模近似，都对序列中的场景进行了比较完整的构建。但在细节部分可以发现两幅点云地图的准确程度有一定差距。如图中被框出部分，左图中视觉里程计的部分出现了明显的重叠与模糊，尤其是在木架部分，明显出现多条木架的重叠。墙面云点分布较为分散，不能准确的分布在同一个平面中，反映到图片中即为墙面亮度较低，空洞较多。而右图的本RGBDSLAM方法得到的点云地图，由于优化后的点云图误差较小，点云分布均匀平整，没有重影出现；并且在多帧图像的点云叠加后，小图中顶部黑板上方的菜单文字依然清晰可辨。在速度与稳定性上还可与另外一种使用彩色与深度信息源的RGBDSLAMv2进行对比。如以下表1所示，本公开的SLAM方案的运行速度(帧率)高于RGBDSLAMv2，为5.7fps，对比的RGBDSLAMv2的帧率仅为3.2fps。同时本公开的SLAM方法可以完整的运行整个图像序列，而作为对比的RGBDSLAMv2方法只能运行到序列的第400帧就出现错误不能继续执行运算，证明的本公开具有较好的稳定性，能够以较快速度建立准确的可用于导航的三维栅格地图。表格1算法运行速度比较方法序列帧数耗时每帧平均耗时处理帧率本SLAM方法750132.7s176ms5.7fpsRGBD-SLAMv2400126.0s315ms3.2fps本文中的部分方法步骤和流程可能需要由计算机执行，从而以硬件、软件、固件及其任何组合的方式来实施，并且可以包括计算机可执行的指令。该计算机可执行的指令可以以计算机程序产品的形式存储在机器可读介质上或者以从远程服务器下载的方式进行提供，并由通用计算机、专用计算机和/或其他可编程数据处理装置的一个或多个处理器读取和执行以实现方法步骤和流程中指明的功能/动作。机器可读介质包括但不限于软盘、光盘、压缩盘、磁光盘、只读存储器ROM、随机存取存储器RAM、可擦可编程ROM(EPROM)、电可擦可编程ROM(EEPROM)、存储卡、闪存和/或电、光、声以及其他形式的传播信号(例如载波、红外信号、数字信号等)。另外需注意，本文中的术语“和/或”可表示“和”、“或”、“异或”、“一个”、“一些但不是全部”、“两者皆不”和/或“两者皆是”，但在此方面并无限制。本文虽然已经示出和描述了本公开的具体实施例，但对本领域技术人员显然可以在不脱离所附权利要求书范围的情况下进行众多改变、变化和修改。另外，在上述具体实施方式中，可看到各种特征在单个实施例中组合在一起以便简化公开内容。此公开方式不应解释为反映要求保护的实施方式需要比每个权利要求项明确所述的具有更多特征。相反，如权利要求所反映的一样，本公开的主题依赖的是比单个公开实施方式所有特征更少的特征。因此，权利要求书的每个权利要求项本身保持为单独的完整的实施例。综上，本领域技术人员将认识到在不脱离本公开的范围和精神的情况下，可在更广阔的各方面中进行改变和修改。所附权利要求书在其范围内涵盖了落入本公开真实范围和精神内的所有此类改变、变化和修改。当前第1页1 2 3