CN109816696A - 一种机器人定位与建图方法、计算机装置及计算机可读存储介质 - Google Patents

Abstract

本发明提供一种机器人定位与建图方法、计算机装置及计算机可读存储介质，该方法包括利用摄像装置获取的信息以及惯性测量单元采集的信息对惯性测量单元采集的信息进行预积分；前端模块利用预积分的相对位姿信息进行运动跟踪，并且判断是否出现视觉跟踪丢失的情况，如没有出现视觉跟踪丢失，则进行视觉与惯性测量融合操作，更新惯性测量单元的状态信息；在产生新的关键帧时，由后端模块获取前端模块所获取的关键帧的位姿信息以及特征点的对应关系信息，创建并优化因子图，利用所获取的位姿信息以及3D点的深度信息更新后端地图。本发明还提供实现上述方法的计算机装置及计算机可读存储介质。本发明能够高效、准确的实现机器人的定位与建图。

Description

一种机器人定位与建图方法、计算机装置及计算机可读存储 介质

技术领域

本发明涉及图像处理领域，具体地，是基于单目视觉和惯性测量单元的扫地机器人定位与建图方法，还涉及实现这种方法的计算机装置、计算机可读存储介质。

背景技术

随着智能控制技术的发展，无人控制的智能机器人已经开始广泛的应用，例如工业生产应用的运输机器人、家用使用的扫地机器人、擦窗机器人，还有小型的无人机等。由于这些机器人在工作过程中需要移动，有一些机器人需要快速的移动，为了实现机器人的运动控制，通常使用同步定位与建图(Simultaneous localization and mapping，SLAM)技术实现。

同步定位与建图技术是机器人根据传感器信息同时实现自身定位和环境地图构建的一种技术，该技术在自动驾驶、无人机、智能家居、虚拟现实、增强现实等领域中有广泛地应用。通常，在机器人内设置一个摄像装置，如CCD传感器，还设置一个惯性测量单元(IMU)，或者设置激光传感器用于测量前方障碍物的距离。

使用激光传感器的方案具有精度高、探测范围广，但机器人的生产成本高、功耗大，且无法识别丰富的特征。而采用SLAM技术的摄像装置主要分为单目相机、双目相机和RGB-D相机，单目相机无法直接获取图像深度信息，且对光强敏感；双目相机通过视差计算深度，计算量大，实时性较差；RGB-D相机可以直接获取环境的深度信息，但成本太高。惯性测量单元通常包含陀螺仪和加速度计，能够测量传感器本体的角速度和加速度，被认为与摄像装置具有明显的互补性，其可以确定尺度，并且可以估计摄像装置采集的两帧图像之间的相对位姿信息。在这种应用背景下，应用摄像装置采集的图像实现的视觉信息与惯性测量单元的信息融合，从而进行SLAM技术的研究成为当前SLAM技术的研究热点。

由于SLAM技术主要是采用硬件设备采集的信息并结合SLAM算法实现对机器人位姿的计算，从而设定机器人的运行轨迹。现有的主流SLAM算法可分为激光和视觉两大类，激光类SLAM算法研究较早，理论和工程均比较成熟。而视觉类的SLAM算法仍处于技术积累层面，尚未有大规模的商业应用。例如中国发明专利申请CN201710617653就公开了一种应用于无人机的基于视觉与惯性测量单元实现的无人机位姿估算方法。

现有基于单目相机的视觉SLAM算法的主要缺点是无法确定环境的绝对尺度；在相机运动速度较快时，相机会出现运动模糊，或者两帧之间的重叠区域太小以至于无法进行特征匹配，容易出现跟踪丢失问题。而现有单目视觉结合惯性测量单元实现的硬件框架主要分为两大类：松耦合框架和紧耦合框架，松耦合框架分别单独计算出惯性测量单元测量得到的状态信息和视觉里程计得到的状态信息，然后进行融合计算。紧耦合框架则将惯性测量单元和视觉约束信息放在一个非线性优化函数中去优化。松耦合框架和滤波融合的方法类似，精度较低；紧耦合框架使得惯性测量单元的数据可以对视觉里程计进行矫正，同时视觉里程计信息也可以矫正惯性测量单元的零点偏移的问题，因此紧耦合框架的定位精度较高，但计算复杂精度高，无法在嵌入式平台等计算能力比较低的平台实时运行。

发明内容

本发明的主要目的是提供一种算法简单且对机器人位姿计算准确的机器人定位与建图方法。

本发明的另一目的是提供一种实现上述机器人定位与建图方法的计算机装置。

本发明的再一目的是提供一种实现上述机器人定位与建图方法的计算机可读存储介质。

为实现本发明的主要目的，本发明提供的机器人定位与建图方法包括利用摄像装置获取的信息以及惯性测量单元采集的信息对惯性测量单元采集的信息进行预积分；前端模块利用预积分的相对位姿信息进行运动跟踪，并且判断是否出现视觉跟踪丢失的情况，如没有出现视觉跟踪丢失，则进行视觉与惯性测量融合操作，更新惯性测量单元的状态信息；在产生新的关键帧时，由后端模块获取前端模块所获取的关键帧的位姿信息以及特征点的对应关系信息，创建并优化因子图，利用所获取的位姿信息以及3D点的深度信息更新后端地图。

由上述方案可见，在产生关键帧以前，由前端模块来更新惯性测量单元的状态，只有产生关键帧以后，通过后端模块来对关键帧的数据进行处理，从而更新后端地图。通过前端模块与后端模块的协同工作，可以实现高精度定位的要求，且系统的运行速度较高，在嵌入式平台也能满足定位与建图实时性的要求。

一个优选的方案是，前端模块运行在第一线程，后端模块运行在第二线程。这样，前端模块与后端模块分别在两个不同的线程上运行，可以避免前端模块的运行影响后端模块的运行。由于前端模块并不是对关键帧进行处理，因此需要实时的对大量数据进行处理，而后端模块需要对关键帧的数据进行处理，且关键帧涉及机器人的定位与运动跟踪，通过一个独立的线程处理关键帧的数据并且更新后端地图，可以使得关键帧图像的处理速度更高，满足嵌入式系统的要求。

一个优选的方案是，前端模块进行运动跟踪前，判断机器人定位系统是否初始化成功，如没有初始化成功，还执行：由初始化模块对机器人定位系统进行初始化；其中，初始化模块运行在第一线程。

由此可见，初始化模块与前端模块设置在同一个线程中执行，可以避免初始化模块与后端模块在同一线程中执行，从而确保后端模块对关键帧图像的数据处理效率。

进一步的方案是，在确认出现视觉跟踪丢失时，初始化模块进行重新初始化，在重新初始化后，继续进行运动跟踪。

可见，在出现视觉跟踪丢失的情况时，系统通过重新初始化的方式来获取视觉跟踪丢失对应的一帧图像的特征点，并且根据重新获取的特征点来继续进行运动跟踪，确保完全视觉跟踪的连续性。

更进一步的方案是，判断是否产生新的关键帧包括：判断当前帧是否为关键帧，如确认当前帧为关键帧，则确认当前帧为新的关键帧。

由此可见，前端模块实时的对当前帧进行判断，在确认当前帧为新的关键帧时，即可以确定产生新的关键帧，继而由后端模块对关键帧进行处理。

更进一步的方案是，确认当前帧为新的关键帧包括：在历史关键帧中获取多个特征点，将多个特征点向当前帧进行投影，当投影在该当前帧上的特征点的数量大于第一预设数值时，将该关键帧添加至离当前帧最近的关键帧序列中；根据当前帧和关键帧序列中每一关键帧的位姿信息，对多个关键帧进行排序，选取离当前帧最近的多个关键帧，将所选取的多个关键帧上的特征点投影到当前帧上，如果投影到当前帧的特征点的数量小于第二预设数值时，确定当前帧为关键帧。

可见，经过特征点数据的投影与数量的判断后，对关键帧的判断更加精确，确保后续的定位与地图更新更加准确。

更进一步的方案是，初始化模块对机器人定位系统进行初始化包括：获取新的一帧图像后，将该帧图像划分成多个单元格，并且从该帧图像中提取特征点；判断所提取的特征点的数量大于特征点数量阈值时，对连续两帧图像进行稀疏光流跟踪，并在累计的帧数达到初始化窗口长度数量以后，判断初始化窗口长度内第一帧图像与最后一帧图像的相对位姿信息，并计算出首尾两帧图像中对应的特征点的深度值，实现视觉与惯性测量单元的对齐。

由此可见，在初始化的过程中实现了摄像装置的视角系统与惯性测量单元的位姿对齐，从而计算出机器人初始的位姿状态以及构建初始的地图，有利于后续的定位与建图。

更进一步的方案是，初始化模块进行重定位操作包括：保存视觉跟踪丢失对应的一帧图像对应的惯性测量单元的位姿信息，并在该帧图像中提取新的特征点；对惯性测量单元采集的信息进行预积分后，对视觉跟踪丢失对应的一帧图像进行光流跟踪；在当前帧图像与视觉跟踪丢失对应的一帧图像之间的视差大于预设的阈值时，根据惯性测量单元采集的信息的预积分结果确定当前帧图像与视觉跟踪丢失对应的一帧图像之间在原始世界坐标系下的相对位姿信息，计算对应特征点的深度值，继续返回前端模块进行跟踪。

可见，在出现视觉跟踪丢失的情况时，初始化模块进行重新初始化可以重新获取特征点的信息，并且可以继续由前端模块进行运动跟踪，避免跟踪丢失的情况。

为实现上述的另一目的，本发明提供的计算机装置包括处理器以及存储器，存储器存储有计算机程序，计算机程序被处理器执行时实现上述机器人定位与建图方法的各个步骤。

为实现上述的再一目的，本发明提供计算机可读存储介质上存储有计算机程序，计算机程序被处理器执行时实现上述机器人定位与建图方法的各个步骤。

附图说明

图1是本发明机器人定位与建图方法实施例的流程图。

图2是本发明机器人定位与建图方法实施例中进行视觉与惯性测量单元融合操作的流程图。

图3是应用本发明机器人定位与建图方法实施例第一次实验计算的机器人运行轨迹与实际轨迹的对比图。

图4是应用本发明机器人定位与建图方法实施例第二次实验计算的机器人运行轨迹与实际轨迹的对比图。

以下结合附图及实施例对本发明作进一步说明。

具体实施方式

本发明的机器人定位与建图方法应用在机器人上，具体的，该机器人是可以移动的机器人，如无人机、扫地机器人等，并且机器人设置有嵌入式的芯片，本发明的机器人定位与建图方法由该运行在嵌入式芯片的程序实现。进一步的，该嵌入型芯片可以具有处理器以及存储器，存储器上存储有计算机程序，处理器通过执行该计算机程序实现机器人定位与建图方法。

机器人定位与建图方法实施例：

本实施例是针对的是视觉领域的同步定位与建图(SLAM)算法。由于视觉SLAM算法可分为基于特征点的稀疏间接算法、基于光度误差最小化的稀疏直接算法、基于光流场匹配的稠密间接算法、基于先验几何信息及光度误差最小化的稠密直接算法，以及基于特征点但不计算描述子的半直接算法等。不同的计算方法采用不同的优化策略来消除算法对机器人位置定位过程中所累积误差，如光束平差法和因子图优化等优化方法。本实施例吸取上述这些方法中的优点，并结合其他传感器信息，构造出成本低、精度高、速度快、优化效率高的SLAM算法。

参见图1，本实施例的流程由三个模块分别执行，包括初始化模块11、前端模块12以及后端模块13，其中初始化模块11与前端模块12在一个线程内执行，即在第一线程内执行，而后端模块13在另外一个线程独立运行，如在第二线程内执行。

初始化模块11用于实现机器人定位与建图系统的初始化以及视觉跟踪丢失之后的重新初始化。前端模块12用于实现机器人的运动跟踪、视觉与惯性测量单元的融合操作，还用于实现对特征点或者3D点的深度滤波功能。其中运动跟踪用于实现相机等摄像装置所获取的信息并且实现相机的位姿估计、特征对齐、位姿及3D点深度优化。视觉与惯性测量单元的融合操作是将视觉与惯性测量单元获取的信息进行融合、矫正位姿以及更新惯性测量单元的状态。深度滤波用于更新3D点或者种子点的深度。同时，前端模块12还用于对每一帧图像进行判断，判断当前帧图像是否为关键帧，一旦产生新的关键帧，则启动后端模块13对关键帧进行处理。

后端模块13根据关键帧的位姿信息及特征点的对应关系，通过增量式图优化方法对关键帧位姿和特征点的深度进行优化，得到更加精确的位姿和深度值。

本实施例的方法需要对机器人定位与建图系统进行初始化，初始化操作是通过视觉计算得到相机相对位姿信息，并计算惯性测量单元的信息进行预积分，进行摄像装置所获取的信息与惯性测量单元所获取的特征进行对齐，根据惯性测量单元的坐标系和摄像装置与惯性测量单元的外部参数确定整个系统的世界坐标系，并将相机位姿和特征点深度转换到世界坐标系下。而重新初始化是在视觉跟踪丢失的情况下，利用惯性测量单元的预积分结果来确定当前帧图像与视觉跟踪丢失对应的一帧图像之间的位姿关系，在该两帧图像之间进行光流跟踪得到特征点的对应关系，通过位姿及对应的特征点并利用三角化的计算得到特征点的深度值，可保持整个系统的世界坐标系及尺度，实现继续跟踪。

前端模块11的运动跟踪过程首先对惯性测量单元采集的信息进行预积分，获取连续两帧通信之间的相对位姿信息，将上一帧图像中的特征点投影到三维空间，惯性测量单元的信息预积分结果作为优化的初始位姿信息，通过最小化光度误差优化得到当前帧的位姿信息。然后利用该位姿将距离当前帧最近的关键帧序列上的特征点以及深度滤波获得的成熟的3D候选点投影到当前帧进行特征对齐，优化3D点在当前帧图像上的二维坐标位置。最后，进一步优化当前帧图像的位姿信息与投影到当前帧上的特征点深度值。视觉与惯性测量单元的融合过程主要是将当前相机位姿转换到世界坐标系下，与惯性测量单元所采集的信息进行融合，创建因子图并进行优化，更新惯性测量单元的状态，包括位置、旋转、速度和偏置等状态信息。深度滤波可以使用深度滤波器实现，可选的，深度滤波器由均匀分布和高斯分布联合组成，在时间域上更新未收敛的特征点的深度，直到这些特征点的深度收敛，将其作为3D候选点插入地图中。

后端模块13仅在产生新的关键帧时执行，获取前端模块12所获取的关键帧的位姿以及特征点的对应关系，将关键帧的位姿转换为世界坐标系下的绝对位姿作为初始位姿因子，将关键帧的特征点的二维坐标位置和对应关系作为投影因子，将3D点的三维坐标位置以及惯性测量单元的因子添加到因子图中进行增量式优化，得到关键帧更加精确的位姿信息和特征点深度信息。

下面结合图1与图2详细介绍机器人定位与建图方法的具体流程。首先，获取相机等摄像装置所获取的信息，并且获取惯性测量单元所采集的信息。本实施例中，相机所获取的信息是一帧一帧的图像，相机运行过程中，按照一定的速率拍摄图像并且形成连续的多帧图像，因此，获取相机等摄像装置所获取的信息实际上是获取相机输出的多帧图像的信息。

因此，步骤S1中，获取相机等摄像装置所获取的信息，并且获取惯性测量单元所采集的信息后，对惯性测量单元所采集的信息进行预积分。由于通过单一的一帧图像难以实现图像的位姿估算，因此步骤S1需要在或者至少两帧图像以后才能执行。对惯性测量单元所采集的信息进行预积分的具体操作可以采用现有的技术进行，在此不再详细说明。

然后，执行步骤S2，判断机器人定位与建图系统初始化是否成功，如果已经初始化成功，则执行步骤S3，否则，执行步骤S4，由初始化模块对系统进行初始化操作。

具体的，执行步骤S4时，初始化的具体步骤包括：获取新的一帧图像后，将该帧图像划分成多个单元格，并且提取该帧图像的特征点。具体的，特征点可以是图像上具有特别定位效果的点，例如与周边的像素点具有明显的像素差或者亮度差等。然后，判断该帧图像中特征点的数量是否大于预设的特征点数量阈值N1，如果大于，则进行后续的判断，否则，重新获取该帧图像的特征点。本实施例中，其中特征点数量阈值N1可以设定为100个。

在确定该帧图像中特征点的数量大于预设的特征点数量阈值N1后，获取连续两帧图像，并且对两帧图像进行KLT稀疏光流跟踪，检测新的特征点以保证每幅图像有100个至300个特征点，并使用RANSAC算法对特征点进行离群点移除，即将分布在图像中的零散特征点进行移除，避免对图像跟踪造成干扰。

接着，继续接收新的一帧图像，并重复执行上述操作，并判断累计的连续接收的图像的帧数是否达到初始化窗口长度数量N2，如果没有达到初始化窗口长度数量N2，则继续接收新的一帧图像。本实施例中，初始化窗口长度数量N2可以是4帧。

如果累计的连续接收的图像的帧数达到初始化窗口长度数量N2，则进一步判断初始化窗口长度内的第一帧图像与最后一帧图像之间的对应的特征点的数量是否大于预设特征点数量阈值N3，并且视差是否在预设范围[D1,D2]之内，如果是，则进行下一步的操作，如果否，则清除初始化窗口长度内的所有帧图像，并重新开始初始化的操作，即重新获取新的一帧图像并划分单元格。本实施例中，预设特征点数量阈值N3为30，而预设范围[D1，D2]可以是[25，100]。

然后，利用五点法计算初始化窗口内第一帧图像与最后一帧图像之间的相对位姿信息，根据该位姿信息三角化得到这两帧图像上对应特征点的深度值，并基于这些三角化的特征信息，使用PnP(Perspective-n-Point)算法估计初始化窗口内其余帧图像的位姿信息，从而计算出所有特征点的深度值。

最后进行视觉与惯性测量单元的对齐操作，包括恢复尺度、重力、速度以及陀螺仪的偏置信息。具体的，首先根据相机与惯性测量单元的旋转一致性，得到陀螺仪的初始偏置信息；然后联合视觉与惯性测量单元信息，以及重力和尺度一起构建线性最小二乘问题并求解；之后修正重力向量，将重力向量旋转在z轴得到世界帧和第一个相机帧图像之间的旋转；最后把所有的变量从参考帧旋转到世界坐标系下。至此，即完成机器人定位与建图系统的初始化。

在初始化完成后，执行步骤S3，由前端模块12进行运动跟踪，具体的，进行运动跟踪时执行下面的步骤：首先，将惯性测量单元采集的信息进行预积分后得到的相对位姿信息加入直接法，通过最小化光度误差方法求解得到当前帧图像的初始位姿信息。然后，判断当前帧图像是否为最新关键帧的下一帧，如果是，利用光流跟踪方法从新关键帧图像中提取特征点，利用直接法得到的位姿信息与光流跟踪得到的对应点三角化新特征点，并且获取新的特征点的深度信息，为深度滤波器对新特征点进行深度滤波提供一个较准确的初始深度。如果当前帧图像不是最新关键帧的下一帧，则进行特征对齐、位姿优化以及3D点优化计算。其中，进行特征对齐是将相机采集的信息与惯性测量单元采集的信息进行匹配，即将相机采集的位姿信息、旋转速度等信息与惯性测量单元所采集的位姿信息、旋转速度等信息进行匹配。

然后，执行步骤S5，判断是否发生视觉跟踪丢失的情况，如果是，则执行步骤S6，系统进行重新初始化，如果没有出现跟踪丢失的情况，则执行步骤S7。本实施例中，判断是否出现跟踪丢失的情况，是判断连续两帧图像中对应的特征点的数量，如果连续两帧图像对应的特征点的数量过小，则难以实现后续的运动跟踪，此时可以认为出现视觉跟踪丢失的情况。例如，可以设定一个视觉跟踪丢失的特征点数量阈值，如果连续两帧图像对应的特征点数量小于视觉跟踪丢失的特征点数量阈值，则可以确认出现视觉跟踪丢失的情况。

在确认出现运动跟踪丢失时，执行步骤S6，机器人定位与建图系统进行重新初始化，具体的，执行以下步骤：首先，保存视觉跟踪丢失对应的一帧图像的惯性测量单元的位姿信息，并在该帧图像上提取新的特征点。然后，对惯性测量单元采集的信息进行预积分，对视觉跟踪丢失对应的一帧图像进行光流跟踪。然后，判断当前帧图像与视觉跟踪丢失对应一帧图像之间的视差数值是否大于阈值D3，如果视差数值不大于阈值D3，则返回重新对对惯性测量单元采集的信息进行预积分，如视差数值大于阈值D3，则确定当前帧图像与视觉跟踪丢失对应的一帧图像之间在原始世界坐标系下的相对位姿信息，计算对应特征点的深度值，在获取下一帧图像后将返回到进入前端模块12进行运动跟踪，即返回执行步骤S3。其中，阈值D3可以设定为20。

如果步骤S5确认没有出现视觉跟踪丢失，则执行步骤S7，进行视觉与惯性测量单元的融合操作，具体的执行步骤如图2所示。首先，执行步骤S21，初始化惯性测量单元状态和噪声模型，并创建因子图。然后，执行步骤S22，将惯性测量单元预积分的结果作为惯性测量单元的因子添加到因子图中。接着，执行步骤S23，将摄像装置的位姿信息通过外部参数转换到惯性测量单元坐标系，作为视觉因子添加到因子图中。然后，执行步骤S24，判断当前累计的图像帧数是否达到融合操作的窗口长度，如果是，则执行步骤S26，否则，执行步骤S25，删除因子图中的所有的旧结点。

在步骤S24如确认当前累计的图像帧数达到融合操作的窗口长度，则执行步骤S26，对因子图进行优化，并且执行步骤S27，更新惯性测量单元的状态信息，包括位置、旋转、速度和偏置等信息。

执行步骤S7后，则执行步骤S8，判断当前帧是否为新的关键帧，也就是判断是否产生了新的关键帧，如果是，则执行步骤S9，否则，执行步骤S10。本实施例中，判断当前帧是否诶新的关键帧可以通过以下步骤判断确定。

首先，在历史关键帧图像上选取多个特征点，例如在历史关键帧图像的左上、右上、中间、左下、右下5个位置上分别选取1个特征点，利用位姿信息将选取的5个特征点向当前帧图像进行投影。然后，如果投影在当前帧图像上的特征点的数量大于或者等于第一预设数值时，则将该关键帧添加至距离当前帧图像最近的关键帧序列中。优选的，第一预设数值可以设定为1。

接着，根据当前帧图像和关键帧序列中每个关键帧图像的位姿信息，对多帧关键帧图像进行排序，选择离当前帧图像最近的多张关键帧图像，优选的，可以选取4张关键帧图像。然后，将所选取的多张关键帧图像上的特征点投影到当前帧图像上，如果投影到当前帧图像的特征点的数量少于第二预设数值时，则判定当前帧图像为关键帧，优选的，第二预设数值为50。

如步骤S8确认当前帧图像为关键帧，则执行步骤S9，将新的关键帧划分为多个单元格，并且提取新的特征点，然后进入后端模块13对关键帧进行处理。

如步骤S8确认当前帧图像不是关键帧，则执行步骤S10，利用深度滤波器更新当前帧图像的种子点的深度和不确定性，优选的，本实施例中的深度滤波器是由均匀分布和高斯分布联合组成，在时间域上更新未收敛的特征点的深度，直到这些特征点的深度收敛，将其作为3D候选点插入地图中，也就是执行步骤S11，利用3D候选点更新前端地图，并返回执行步骤S3，重新进行运动跟踪。

当出现新的关键帧并且提出新的关键帧的特征点以后，后端模块13对新的关键帧进行处理，具体的，执行步骤S12，先获取前端模块12所获取的新关键帧的位姿信息以及特征点的对应关系，然后将新的关键帧的位姿信息转换为世界坐标系下的绝对位姿信息作为初始位姿因子，将特征点的二维坐标位置和对应关系作为投影因子，并将3D点的三维坐标位置及惯性测量单元因子添加到因子图中，从而完成因子图的创建。

然后，执行步骤S13，增量式优化因子图，得到更加精确的位姿信息和特征点的深度值，最后执行步骤S14，利用更新后的位姿信息以及特征点的深度值更新后端地图。

本实施例的方法通过初始化模块11、前端模块12以及后端模块13共同完成，且初始化模块11与前端模块12在一个线程内执行，而后端模块13在另外一个线程内独立运行，这样可以确保关键帧的处理效率，提高机器人定位与建图的准确性。由于后端模块13根据关键帧图像的位姿信息以及特征点的对应关系，通过增量式图优化方法对关键帧位姿信息和特征点的深度进行优化，得到更加精确的位姿信息和特征点的深度值。图3与图4示出了应用本实施例实现的扫地机器人的计算轨迹与实际轨迹，从两张图可见，应用本实施例的扫地机器人的计算的轨迹与实际轨迹很接近，能够实现机器人的定位与建图的需要。

并且，在系统初始化的过程中，将新的一帧图像划分为多个单元格，并且提取该帧图像的特征点，如果所提取的特征点的数量大于一个设定的阈值，则进行连续两帧图像之间的光流跟踪，并对惯性测量单元所采集的信息进行预积分。如果累计的帧数达到初始化窗口长度数量，且对应特征点的数量和视差均满足条件，则计算初始化窗口长度内第一帧图像与最后一帧图像之间的相对位姿信息；然后计算初始化窗口长度内所有帧图像的位姿信息以及所有特征点的深度值；最后进行视觉与惯性测量单元的特征对齐，确定世界坐标系，得到系统尺度以及惯性测量单元的初始状态。

另外，由于本实施例在视觉跟踪丢失后进行重新初始化，例如在出现视觉跟踪丢失的情况后，保存视觉跟踪丢失对应一帧图像对应的惯性测量单元的位姿信息，并在该帧图像上提取新的特征点；同时对惯性测量单元所采集的信息进行预积分，对视觉跟踪丢失对应的一帧图像进行光流跟踪。如果当前帧图像与视觉跟踪丢失对应的一帧图像之间的视差大于一定阈值，则根据惯性测量单元预积分结果确定当前帧与视觉跟踪丢失对应一帧图像之间在原始世界坐标系下的相对位姿信息，三角化计算对应特征点的深度值，继续进入前端模块12进行运动跟踪，可在一定程度上解决视觉跟踪丢失问题。

此外，本实施例优化了深度滤波器的初始深度。当产生新的关键帧时，在该关键帧图像上提取新的特征点，并用下一帧图像对该关键帧进行光流跟踪得到新的特征点的对应关系。并且，将惯性测量单元预积分结果作为直接法的初始位姿信息，最小化光度误差优化得到当前帧的位姿；根据该位姿信息及特征点的对应关系，三角化得到特征点的深度值并作为深度滤波器的初始深度，可以加快3D种子点的收敛速度，及时补充特征点的数量，降低视觉跟踪丢失的概率。

最后，由于本实施例通过后端模块对关键帧进行处理，即对关键帧的数据进行优化，且后端模块仅在产生新的关键帧时执行，并获取前端模块12的特征点的对应关系以及关键帧的位姿信息，将新产生的关键帧的姿转换为世界坐标系下的绝对位姿作为初始位姿因子，将新的特征点的二维坐标位置和对应关系作为投影因子，将3D点的三维坐标位置及惯性测量单元因子添加到因子图中进行增量式优化，得到更加精确的位姿和深度值。

可见，本发明方法通过三个模块分别独立的执行，架构清晰高效，在保证一定精度的条件下，实现该方法的程序复杂度较低，并且在整个系统的初始化模块确定世界坐标系以及尺度，并加入重新初始化的步骤，可以在一定程度上解决视觉跟踪丢失问题。此外，为深度滤波器初始化一个较准确的深度，有利于加快种子点的收敛速度，及时补充特征点的数量，降低视觉跟踪丢失的概率，最后，通过后端模块利用增量式因子图对关键帧进行优化，效率较高。

计算机装置实施例：

本实施例的计算机装置可以是应用于无人机、扫地机器人等可移动的机器人上的控制芯片，该芯片包括有处理器、存储器以及存储在存储器中并可在处理器上运行的计算机程序，处理器执行计算机程序时实现上述机器人定位与建图方法的各个步骤。

例如，计算机程序可以被分割成一个或多个模块，一个或者多个模块被存储在存储器中，并由处理器执行，以完成本发明的各个模块。一个或多个模块可以是能够完成特定功能的一系列计算机程序指令段，该指令段用于描述计算机程序在终端设备中的执行过程。

需要说明的是，该控制芯片可包括，但不仅限于，处理器、存储器。本领域技术人员可以理解，本发明的示意图仅仅是终端设备的示例，并不构成对终端设备的限定，可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件，例如终端设备还可以包括输入输出设备、网络接入设备、总线等。

本发明所称处理器可以是中央处理单元(Central Processing Unit，CPU)，还可以是其他通用处理器、数字信号处理器(Digital Signal Processor，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现成可编程门阵列(Field-Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等，处理器是终端设备的控制中心，利用各种接口和线路连接整个终端设备的各个部分。

存储器可用于存储计算机程序和/或模块，处理器通过运行或执行存储在存储器内的计算机程序和/或模块，以及调用存储在存储器内的数据，实现终端设备的各种功能。存储器可主要包括存储程序区和存储数据区，其中，存储程序区可存储操作系统、至少一个功能所需的应用程序(比如声音播放功能、图像播放功能等)等；存储数据区可存储根据手机的使用所创建的数据(比如音频数据、电话本等)等。此外，存储器可以包括高速随机存取存储器，还可以包括非易失性存储器，例如硬盘、内存、插接式硬盘，智能存储卡(SmartMedia Card,SMC)，安全数字(Secure Digital,SD)卡，闪存卡(Flash Card)、至少一个磁盘存储器件、闪存器件、或其他易失性固态存储器件。

计算机可读存储介质：

上述计算机所存储的计算机程序如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明实现上述实施例方法中的全部或部分流程，也可以通过计算机程序来指令相关的硬件来完成，该计算机程序可存储于一计算机可读存储介质中，该计算机程序在被处理器执行时，可实现上述机器人定位与建图方法的各个步骤。

其中，计算机程序包括计算机程序代码，计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。计算机可读介质可以包括：能够携带计算机程序代码的任何实体或装置、记录介质、U盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、电载波信号、电信信号以及软件分发介质等。需要说明的是，计算机可读介质包含的内容可以根据司法管辖区内立法和专利实践的要求进行适当的增减，例如在某些司法管辖区，根据立法和专利实践，计算机可读介质不包括电载波信号和电信信号。

最后需要强调的是，本发明不限于上述实施方式，初始化具体步骤或者重新初始化具体步骤的改变，这些改变也应该包括在本发明权利要求的保护范围内。

Claims (10)

1.机器人定位与建图方法，其特征在于，包括：

利用摄像装置获取的信息以及惯性测量单元采集的信息对所述惯性测量单元采集的信息进行预积分；

前端模块利用所述预积分的相对位姿信息进行运动跟踪，并且判断是否出现视觉跟踪丢失的情况，如没有出现视觉跟踪丢失，则进行视觉与惯性测量融合操作，更新所述惯性测量单元的状态信息；

在产生新的关键帧时，由后端模块获取前端模块所获取的所述关键帧的位姿信息以及特征点的对应关系信息，创建并优化因子图，利用所获取的位姿信息以及3D点的深度信息更新后端地图。

2.根据权利要求1所述的机器人定位与建图方法，其特征在于：

所述前端模块运行在第一线程，所述后端模块运行在第二线程。

3.根据权利要求2述的机器人定位与建图方法，其特征在于：

所述前端模块进行运动跟踪前，判断机器人定位系统是否初始化成功，如没有初始化成功，还执行：由初始化模块对所述机器人定位系统进行初始化；

其中，所述初始化模块运行在所述第一线程。

4.根据权利要求3所述的机器人定位与建图方法，其特征在于：

在确认出现视觉跟踪丢失时，所述初始化模块进行重新初始化，在重新初始化后，继续进行运动跟踪。

5.根据权利要求1至4任一项述的机器人定位与建图方法，其特征在于：

判断是否产生新的关键帧包括：判断当前帧是否为关键帧，如确认当前帧为关键帧，则确认当前帧为新的关键帧。

6.根据权利要求5所述的机器人定位与建图方法，其特征在于：

确认当前帧为新的关键帧包括：在历史关键帧中获取多个特征点，将多个所述特征点向当前帧进行投影，当投影在该当前帧上的特征点的数量大于第一预设数值时，将该关键帧添加至距离当前帧最近的关键帧序列中；

根据当前帧和所述关键帧序列中每一关键帧的位姿信息，对多个关键帧进行排序，选取离当前帧最近的多个关键帧，将所选取的多个关键帧上的特征点投影到当前帧上，如果投影到当前帧的特征点的数量小于第二预设数值时，确定当前帧为关键帧。

7.根据权利要求3所述的机器人定位与建图方法，其特征在于：

所述初始化模块对所述机器人定位系统进行初始化包括：获取新的一帧图像后，将该帧图像划分成多个单元格，并且从该帧图像中提取特征点；

判断所提取的特征点的数量大于特征点数量阈值时，对连续两帧图像进行稀疏光流跟踪，并在累计的帧数达到初始化窗口长度数量以后，判断初始化窗口长度内第一帧图像与最后一帧图像的相对位姿信息，并计算出首尾两帧图像中对应的特征点的深度值，实现视觉与惯性测量单元的对齐。

8.根据权利要求1或2所述的机器人定位与建图方法，其特征在于：

所述初始化模块进行重定位操作包括：

保存视觉跟踪丢失对应的一帧图像对应的惯性测量单元的位姿信息，并在该帧图像中提取新的特征点；

对所述惯性测量单元采集的信息进行预积分后，对视觉跟踪丢失对应的一帧图像进行光流跟踪；

在当前帧图像与视觉跟踪丢失对应的一帧图像之间的视差大于预设的阈值时，根据所述惯性测量单元采集的信息的预积分结果确定当前帧图像与视觉跟踪丢失对应的一帧图像之间在原始世界坐标系下的相对位姿信息，计算对应特征点的深度值，继续返回所述前端模块进行跟踪。

9.计算机装置，其特征在于，包括处理器以及存储器，所述存储器存储有计算机程序，所述计算机程序被处理器执行时实现如权利要求1至8中任意一项所述机器人定位与建图方法的各个步骤。

10.计算机可读存储介质，其上存储有计算机程序，其特征在于：所述计算机程序被处理器执行时实现如权利要求1至8中任意一项所述机器人定位与建图方法的各个步骤。