CN108955718A - 一种视觉里程计及其定位方法、机器人以及存储介质 - Google Patents

Abstract

本申请公开了一种视觉里程计的定位方法，该方法包括：获取拍摄装置拍摄得到的目标图像；采用第一方法获得目标图像中用于确定拍摄装置的位姿信息的目标像素点；利用与第一方法对应的预设评估方式对获得的目标像素点进行评估；若评估结果为有效，则将采用第一方法确定的位姿信息作为拍摄装置拍摄目标图像时的位姿信息；若评估结果为无效，则采用第二方法获得目标图像中用于确定拍摄装置的位姿信息的目标像素点，以利用获得的目标像素点确定拍摄装置的位姿信息；其中，第一方法和第二方法的其中一个为直接法，另一个为特征点匹配法。本申请所提供的定位方法能够提高定位的鲁棒性。本申请还提供了一种视觉里程计、机器人及存储介质。

Description

一种视觉里程计及其定位方法、机器人以及存储介质

技术领域

本申请涉及视觉里程计领域，特别是涉及一种视觉里程计及其定位 方法、机器人以及存储介质。

背景技术

随着计算机视觉技术的发展，视觉里程计由于其较强的适应性成为 自主移动导航系统的重要选择。视觉里程计是根据拍摄的图像计算相机 的运动，现有技术中实现视觉里程计的方式主要是有特征点匹配法和直 接法。但是，特征点匹配法虽然可在噪声较大、相机运动太快的时工作， 但地图则是稀疏特征点，直接法虽能够建立稠密地图，但存在着计算量 大、鲁棒性不好的缺陷，这两种方法均无法应对场景复杂的应用场景。 所以现在急需一种可以较好应对复杂多变场景的视觉里程计的定位方 法。

发明内容

本申请主要解决的技术问题是提供一种视觉里程计及其定位方法、 机器人以及存储介质，能够提高定位的鲁棒性。

为解决上述技术问题，本申请采用的一个技术方案是：提供一种视 觉里程计的定位方法，所述方法包括：

获取拍摄装置拍摄得到的目标图像；

采用第一方法获得所述目标图像中用于确定所述拍摄装置的位姿 信息的目标像素点；

利用与所述第一方法对应的预设评估方式对所述获得的目标像素 点进行评估；

若所述评估结果为有效，则将采用所述第一方法确定的位姿信息作 为所述拍摄装置拍摄所述目标图像时的位姿信息；

若所述评估结果为无效，则采用第二方法获得所述目标图像中用于 确定所述拍摄装置的位姿信息的目标像素点，以利用所述获得的目标像 素点确定所述拍摄装置的位姿信息；

其中，所述第一方法和第二方法的其中一个为直接法，另一个为特 征点匹配法。

为解决上述技术问题，本申请采用的另一个技术方案是，提供一种 视觉里程计，所述视觉里程计包括：处理器，以及与所述处理器耦接的 拍摄装置和存储器；所述拍摄装置用于拍摄得到图像；所述存储器存储 程序数据；所述处理器运行所述程序数据时，用于执行如上所述的一种 视觉里程计的定位方法。

为解决上述技术问题，本申请采用的再一个技术方案是，提供一种 机器人，包括上述的视觉里程计。

为解决上述技术问题，本申请采用的又一个技术方案是，提供一种 存储介质，所述存储介质存储有程序数据，所述程序数据被执行时实现 如上所述的一种视觉里程计的定位方法。

以上方案，通过对直接法或特征点匹配法所获得的目标像素点进行 评估，以判断采用这种方法是否会获取到有效的目标像素点，并在当前 的这种方法评估为有效时，将由这种方法得到的位姿信息作为拍摄位置 的位姿信息，在当前的这种方法评估为无效时，切换至另一种方法重新 对目标图像进行处理以实现对拍摄位置的定位，故本方案能够灵活切换 不同的方法实现拍摄装置的定位，且在当前方法获得的目标像素点的评 估结果为无效时则进行切换，保证了对拍摄装置的准确定位，提高了定 位的鲁棒性。另外，由于特征点匹配法具有对较大位移与旋转的鲁棒性 跟踪定位的优点，直接法具有对纹理缺失条件下的良好跟踪效果且具有 良好的实时性，故本方案可兼备两种方法的有效，且根据不同的应用场 景选择不同的方法，可较好地应对复杂多变的应用场景。

附图说明

图1是本申请视觉里程计的定位方法一实施例的流程示意图；

图2是本申请视觉里程计的定位方法另一实施例的流程示意图；

图3是本申请视觉里程计的定位方法又一实施例的流程示意图；

图4是本申请视觉里程计的定位方法再一实施例的流程示意图；

图5是本申请视觉里程计的定位方法再一实施例的流程示意图；

图6是本申请视觉里程计的定位方法再一实施例的流程示意图；

图7是本申请视觉里程计的定位方法再一实施例的流程示意图；

图8是本申请视觉里程计的定位方法再一实施例的流程示意图；

图9是本申请视觉里程计的定位方法再一实施例的流程示意图；

图10是本申请视觉里程计的定位方法再一实施例的流程示意图；

图11是本申请一种视觉里程计一实施例的结构示意图；

图12是本申请一种机器人在一实施例中的结构示意图；

图13是本申请一种存储介质一实施例的结构示意图。

具体实施方式

下面将结合本申请实施例中的附图，对本申请实施例中的技术方案 进行清楚、完整地描述。可以理解的是，此处所描述的具体实施例仅用 于解释本申请，而非对本申请的限定。基于本申请中的实施例，本领域 普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例， 都属于本申请保护的范围。

本申请的描述中，“多个”的含义是至少两个，例如两个，三个等， 除非另有明确具体的限定。此外，术语“包括”和“具有”以及它们任 何变形，意图在于覆盖不排他的包含。例如包含了一系列步骤或单元的 过程、方法、系统、产品或设备没有限定于已列出的步骤或单元，而是 可选地还包括没有列出的步骤或单元，或可选地还包括对于这些过程、 方法、产品或设备固有的其它步骤或单元。

在本文中提及“实施例”意味着，结合实施例描述的特定特征、结 构或特性可以包含在本申请的至少一个实施例中。在说明书中的各个位 置出现该短语并不一定均是指相同的实施例，也不是与其它实施例互斥 的独立的或备选的实施例。本领域技术人员显式地和隐式地理解的是， 本文所描述的实施例可以与其它实施例相结合。

首先在此对下文即将阐述技术方案中的第一方法和第二方法进行 说明：第一方法和第二方法的其中一个为直接法，另一个为特征点匹配 法。在下文对于本申请技术方案的阐述中，首先以第一方法为主要角度 进行阐述。

请参阅图1，为本申请一种视觉里程计的定位方法在一实施例中的 流程示意图。本实施例中，该定位方法由视觉里程计执行，该视觉里程 计安装在机器人或者无人机等任意需要定位的设备上。该定位方法具体 包括以下步骤：

S10：获取拍摄装置拍摄得到的目标图像。

其中，该拍摄装置可设置在该视觉里程计上，或者为于视觉里程计 连接的独立装置。该拍摄装置可以是彩色相机或深度相机，又或包括上 述两种相机。对应地，该目标图像为彩色图像、深度图像或包括上述两 种图像。目标图像是指用于计算拍摄装置在拍摄期间的运动轨迹所需的 图像。

例如，目标图像包括彩色图像，记为I_k。又例如，目标图像包括深 度图像，或者目标图像包括双目拍摄的两帧彩色图像，利用该双目彩色 图像计算出深度图像。再利用该深度图像即可获得对应的深度信息，进 而可得到图像中像素点的空间位置信息，如空间三维坐标。需要说明的 是，步骤S10中获取的图像可以是拍摄装置实时拍摄所得的图像，也可以是拍摄装置预先拍摄所得并保存的图像，在需要时调用以执行本申请 所提供的技术方案。

S20：采用第一方法获得目标图像中用于确定拍摄装置的位姿信息 的目标像素点。

本申请所述的目标像素点为在目标图像中，采用特征匹配法或直接 法用于确定拍摄装置的位姿信息的像素点。可以理解的是，即便同一图 像，分别采用特征点匹配法和直接法获取的目标像素点可能是不同的像 素点。

视觉里程计可默认先采用该目标图像的前一帧图像最终采用的方 法作为该第一方法，例如，前一帧图像最终是采用特征匹配法确定得到 拍摄拍照装置的位姿信息，则在获得目标图像后，先采用特征匹配法来 获取该目标像素点。

例如，该第一方法为特征匹配法，该目标图像为彩色图像，视觉里 程计获取目标图像中的特征点，并从该特征点中查找出与历史图像的目 标像素点匹配的特征点作为该目标图像的目标像素点。其中，该历史图 像可以为该拍摄装置拍摄的在目标图像前一帧或前几帧的图像，该历史 图像的目标像素点也可采用本申请方法确定得到，即该历史图像作为目 标图像，利用本申请方法得到其目标像素点以及对应的拍摄装置的位姿 信息。

又例如，该第一方法为直接法，该目标图像为彩色图像，利用目标 图像的像素点的梯度情况，从目标图像中查找得到梯度情况符合设定要 求的像素点作为目标像素点。

S30：利用与第一方法对应的预设评估方式对获得的目标像素点进 行评估。

本实施例中，可以灵活选择采用第一方法或第二方法对拍摄装置进 行定位。其中，每种方法均对应有预设评估方式，以采用该预设评估方 式评估利用该方法进行定位是否合适。例如，在步骤S20利用第一方法 获得目标图像中用于确定摄像装置的位姿信息的目标像素点之后，会进 一步利用与第一方法对应的预设评估方式对步骤S20中获得的目标像素 点进行评估，以判断利用第一方法获得的目标像素点是否是有效的。若 评估结果为有效，则确定利用第一方法所得的目标像素点计算所得的拍 摄装置的位姿是准确的，或者所得的拍摄装置的位姿误差是在预设范围 内，故执行步骤S40。若评估结果为无效，则确定利用第一方法所得的 目标像素点计算所得的拍摄装置的位姿是不准确的，或者所得的拍摄装 置的位姿误差不在预设范围内，故执行S50。

其中，与第一方法对应的预设评估方式可以评估的是目标像素点的 数量，也可以是其他可以反映所得拍摄装置位姿是否准确有效的像素点 信息。

在一实施例中，当第一方法是特征点匹配法时，步骤S30则是利用 与特征点匹配法对应的预设评估方式，对步骤S20中所获得的目标像素 点进行评估。此时是将步骤S20中所获得的目标像素点数量与第一预设 阈值进行比较大小，若步骤S20所获得目标图像中的目标像素点的数量 大于或等于上述的第一预设阈值，则判断特征点匹配法对于目标图像的 跟踪和处理使有效的，即会输出评估结果有效；反之则输出评估结果无 效。其中，第一预设阈值是表示用于确定拍摄装置的位置信息的像素点 数量的经验性阈值。其中，该经验性阈值可以是经过对数据库中的图像 数据不断训练得到的，又或者是在后期使用的过程中，对初始经验性阈 值(系统出厂设置的阈值)不断优化后得到第一预设阈值。

在另一实施例中，当第一方法是直接法时，步骤S30则是利用与直 接法对应的评估方式，对步骤S20中所获得的目标像素点进行评估。此 时是计算步骤S20中所获得的目标图像中的目标像素点的数量与参考图 像中目标像素点数量之比，并将计算所得的比值与预设比值进行比较， 以此评估用当前的所获得像素点是否有效。若步骤S20中所获得的目标 图像中的目标像素点的数量与参考帧中的目标像素点数量之比大于或 等于预设比值时，则判断直接法对于目标图像的跟踪和处理是有效的， 则会输出评估结果有效；反之则输出评估结果是无效的。其中，参考图 像是指连续采用所述直接法确定拍摄装置的位姿信息的至少两帧图像 中首帧图像，即该目标图像的前两帧图像是采用特征匹配法确定拍摄装 置的位姿信息，在对目标图像的前一帧图像是采用直接法确定拍摄装置 的位姿信息，而当前对目标特征也采用直接法得到其目标像素点，在该 获得的目标像素点进行评估时，将该前一帧图像作为参考图像。

S40：若评估结果为有效，则将采用第一方法确定的位姿信息作为 拍摄装置拍摄所述目标图像时的位姿信息。

当步骤S30中评估得出的结果为有效，则会采用第一方法确定的位 姿信息作为拍摄装置拍摄目标图像时的位姿信息，同时继续采用第一方 法保持对于下一帧图像的跟踪，即会再次重复使用第一方法获得下一帧 目标图像中用于确定拍摄装置的位姿信息的目标像素点，并利用与第一 方法对应的预设评估方式对所获得的目标像素点进行评估。

其中，该采用第一方法确定位姿信息的步骤可以执行于本步骤S40 中，或者执行于上述步骤S30之前，即视觉里程计在采用第一方法获得 目标像素点并利用目标像素点确定位姿信息后，再执行上述评估。

S50：若评估结果为无效，则采用第二方法获得目标图像中用于确 定拍摄装置的位姿信息的目标像素点，以利用获得的目标像素点确定拍 摄装置的位姿信息。

当步骤S30中输出的评估结果无效，则会采用第二方法重新获得目 标图像中用于确定拍摄装置的位姿信息的目标像素点，并直接将利用第 二方法获得的目标像素点确定拍摄装置的位姿信息，或者类似于上述步 骤S30-S40，利用第二方法对应的预设评估方法对重新获得的目标像素 点进行评估，并在新评估结果为有效时，才将利用第二方法获得的目标 像素点确定拍摄装置的位姿信息。

本申请通过获取拍摄装置拍摄得到的目标图像，采用与第一方法对 应的预设评估方式(特征点匹配法或直接法)，对所获得的目标像素点 进行评估，若评估结果为有效，则将采用第一方法确定的位姿信息作为 拍摄装置拍摄目标图像时的位姿信息，若评估结果为无效时，则将采用 第二方法(当第一方法是特征点匹配法时，则第二方法则是直接法；若 第一方法是直接法，则第二方法是特征点匹配法)获得目标图像中用于 确定拍摄装置的位姿信息的目标像素点，以利用所获得的目标像素点确 定拍摄装置在拍摄目标图像时的位姿信息，最终通过所确定的拍摄装置 的位姿，进一步确定拍摄装置的运动轨迹。本申请所提供的技术方案提 供一种通过对于目标图像中的目标像素点的评估，并根据评估结果有效 与否，实现特征点匹配法与直接法之间的灵活转换，可以同时兼备特征 点匹配法地对于较大位移与旋转的鲁棒性跟踪的优点，同时也具备直接 法在纹理缺失条件下跟踪的优点，应对复杂的场景环境。

进一步的，请参见图2，为本申请视觉里程计的定位方法另一实施 例中步骤S20所包含的流程示意图。本实施例中，该第一方法为特征点 匹配法，上述步骤S20包括以下子步骤：

S21：提取目标图像中的特征点。

在采用特征点匹配法获得目标图像中用于确定拍摄装置位姿信息 的目标像素点时，会首先提取出目标图像中的特征点，再进一步对所提 取出的特征点做预设的处理分析。其中，在图像处理领域中，特征点指 的是图像灰度值发生剧烈变化的点或者在图像边缘上曲率较大的点(即 两个边缘的交点)。在当前实施例中，首先采用的是FAST算法获取角点， 再利用BEIEF算法提取所获取的角点的特征信息，最终将获得特征信息 的角点作为特征点。具体的，请参见图3，步骤S21进一步包括子步骤 S211至步骤S212。其中，

S211：利用Fast算法检测目标图像中的角点。

提取目标图像中的特征点首先要利用Fast算法检测目标图像中的角 点。其中，Fast算法的全称Features from accelerated segment test，是一 种用于角点检测的算法。具体的，请参见图4所示的流程示意图，利用 Fast算法检测目标图像中的角点具体包括下述子步骤S2111至步骤 S2114。

S2111：在目标图像中选取一个像素点作为中心像素点。

在利用Fast算法检测目标图像中的角点时，会首先在目标图像中选 取一个像素点作为中心像素点。在一实施例中，目标图像中的所有像素 点在检测角点的过程中，均会被选作为中心像素点的。换而言之，就是 会检测目标图像中的每一个像素点是否为角点。

S2112：以所选中的中心像素点为中心，选择预设数量且分布在一 圆周上的像素点。需要说明的是，请参见图5，所选中的预设数量的像 素点均在一圆周上R1。

当选好一个像素点作为中心像素点后，会以所选中的中心像素点为 中心，选中预设数量、且均分布在一个预设大小的圆周上的像素点。对 于所选中的像素点数量，或该圆周的大小均是依据经验值设定，也可以 根据实际的需要进行调整大小。

在一实施例中，以所选中的中心像素点P为中心，选择16个分布 在同一圆周上R1上的像素点。

在其他的实施例中，以所选中的中心像素点为中心，选择9个像素 点。

S2113：判断所选择的预设数量的像素点与中心像素点是否相同。

判断像素点与中心像素点是否相同的判断依据是，当圆周所穿过的 像素点与所选中的中心像素点的灰度值之差的绝对值大于或等于一预 设值t时，则判定这两个像素点不同；当圆周所穿过的像素点与所选中 的中心像素点的灰度值之差的绝对值小于预设值t时，则判定这两个像 素点相同。t是一表示像素点灰度值的量，具体可以为80～90之间任意一个值。

其中，在判断所选择的像素点与中心像素点是否相同之后，会进一 步对于判断结果进行计数。即统计圆周所穿过的像素点与中心像素点不 同的像素点的数量。

S2114：若圆周所穿过的预设数量的像素点中有大于等于m的像素 点与中心像素点不同，则判断中心像素点为一个角点。

将上述步骤S2113中所得的圆周所穿过的像素点与中心像素点不同 的像素点的数量，与预设的m进行比较大小，当圆周所穿过的像素点与 中心像素点不同的像素点的数量大于或等于m时，则判断该中心像素点 为角点。其中，预设的m根据实际的需要进行调整大小。

在一实施例中，当上述步骤S2112中，在中心像素点周围选择16 个像素点时，则会设定m＝12。即当以所选择的中心像素点为中心，在 其周围选择分布在一圆周上的16个像素点时，当判断有大于或等于12 个像素点与中心像素点不同时，则判断该中心像素点为角点，反之则判 断当前中心像素点不是角点。

采用Fast算法，可以快速且准确的检测出目标图像中的角点。在利 用Fast算法完成对于目标图像中角点的检测后，会进一步的利用Brief 算法获取角点的特征信息，以得到目标图像中的特征点。

S212：利用Brief算法获取角点的特征信息，并将得到特征信息的 角点作为目标图像中的特征点。其中，利用Brief算法获取的角点的特 征信息至少包括描述子。

在一实施例中，所述特征信息包括：所述角点的位置信息和所述角 点的描述子。可以理解的，在其他实施例中，步骤S212中提取的角点 的特征信息包括，角点的描述子、角点的位置信息以及角点的其他信息 等，具体根据实际的需要进行调整，在此不做任何的限定。

需要说明的是，角点的位置信息是指角点的二维空间的位置信息， 也可以是该角点的三维空间位置信息。可以理解的，当获得角点的二维 空间的位置信息或该角点的三维空间的位置信息中的任意一个，均可以 经过计算，最终得出另一种位置信息，所以具体在此对于特征信息中的 位置信息并不做任何限定。

在一实施例中，当特征信息为描述子时，利用Brief算法计算角点 的描述子的具体流程如下。首先以d为半径，该角点为圆心作圆。然后 以某一设定好的模式在圆内选取N个点对，记为：P₁(A，B)，P₂(A， B)…….P_N(A，B)。然后根据所定义二进制测试以获得描述子，二进制测 试如下所示：

其中，I_A,I_B代表像素点A、B的灰度值；

特征描述子定义为N个二进制测试的向量：

S22：将提取的特征点与历史图像的目标像素点进行匹配。

经过步骤S21中完成对特征点的提取之后，会进一步将所提取的特 征点与历史图像的目标像素点进行匹配，以得到一个匹配点对集。

进一步的，步骤S22包括：根据特征信息将目标图像中的特征点与 历史图像的目标像素点进行匹配。特征信息至少包括：描述子和位置信 息。

其中，目标图像中的特征点与历史图像中的目标像素点的匹配依据 是：特征点的描述子。即是根据目标图像中特征点的描述子与历史图像 中目标像素点描述子(即历史图像中的目标像素点对应的特征点的描述 子)的相似度，进一步判断该特征点与历史图像中的目标像素点是否匹 配。

例如，在一实施例中，特征点的描述子为二进制字串。如目标图 像中的特征点A和历史图像中的目标像素点B的描述子如下：A： 10111011，B：10111010。

进一步的，我们设定一个阈值，如75％，当特征点A和目标像素 点B的描述子的相似度大于等于75％的时候，则判断特征点A和目标 像素点B是匹配的。所以在此所举得例子中，由于A和B的相似度是 87.5％，大于设定的值75％，所以此时判断像素点A和像素点B匹配。

在另一实施例中，步骤S22中的匹配依据包括：特征点的描述子 以及其他的特征信息，如特征点的位置信息。设定将目标图像中的特 征点与目标图像的前一帧图像中的目标像素点进行比对。由于拍摄相 邻帧图像之间间隔时间非常短，所以相邻帧之间的匹配的特征点之间 的位置相距非常近。所以对目标图像的特征点与历史图像中的目标像 素点之间进行匹配计算时，同时参考特征点和目标像素点的描述子和 位置信息。

其中，历史图像为目标图像的前n帧目标图像，所述n至少为1。 可以理解的，在一些实施例中，可以根据实际的需要所需要的n的值。

S23：将目标图像中与历史图像中的目标像素点匹配成功的特征点 作为用于确定拍摄装置的位姿信息的目标像素点。

在一实施例中，步骤S23之后会进一步包括，计算目标像素点的三 维空间坐标。并根据所得的目标像素点的三维空间坐标与历史图像中的 匹配成功的特征点所对应的三维空间坐标，运用相关函数计算出目标图 像与历史图像之间的位移，从而计算出在拍摄历史图像与当前图像之间 的时间间隔期间，拍摄装置的位移。

在一实施例中，步骤S23之后进一步包括：采用RANSAC算法去 除目标图像中误匹配的特征点。可以理解的，在步骤S22将目标图像中 的提取到的特征点与历史图像中的目标像素点进行匹配时，会有误匹配 的特征点出现，而误匹配的特征点会对后续各帧图像中点的跟踪，又或 是对于一个准确的里程计实现均有一定的影响，所以进一步的去除目标图像中误匹配的特征点是非常有必要的。

RANSAC算法是通过多次随机抽取一定的样本对参数进行估计，先 得出初始参数模型，然后根据估计的参数将所有的数据进行分类，一部 分数据在设定的误差范围内，则称为内点，反之称为外点，经过多次迭 代计算出最优模型参数。

S24：根据所获得的目标像素点对应的空间位置信息及其匹配的历 史图像中的目标像素点对应的空间位置信息，确定拍摄装置的位姿信 息。

经过步骤S23确定了用于确定拍摄装置的位置信息的目标像素后， 会进一步根据该目标像素点对应的空间位置信息机器配的历史图像中 的目标像素点对应的空间位置信息，确定拍摄装置的位姿信息。其中， 拍摄装置的位姿信息包括：拍摄装置的相对于拍摄起点的位移量，拍摄 的角度、或拍摄装置旋转角度等信息。根据上述位姿信息，可以进一步计算得出拍摄装置在该时间段内的轨迹。

请参阅图6，现已知有n个点的3D位置和它们相应的像素坐标， 在EPnP中对应相应的解法。通常，选取世界坐标系xyz中的四个虚拟 的控制点然 后，通过n个3D点在相机(拍摄装置)平面ABDE的投影关系，以及与 这四个控制点的权重关系，构建12*12的方阵，求得其零空间特征向量， 可以得到虚拟控制点的相机(拍摄装置)平面ABDE上的坐标，然后再使 用POSIT算法即可求出拍摄装置的位姿，依据所求的位姿信息，可而得 到拍摄装置在该时间段内的轨迹。

进一步的，在一实施例中，当所获得目标像素点的位置信息是空缺 的，在步骤S24之前会进一步获取目标像素点的位置信息。具体的可以 结合目标像素点的特征信息计算得出该像素点的空间位置信息。当当前 实施例中的拍摄装置是深度相机时，可以结合目标像素点的特征信息和 目标图像对应的深度图像信息，进一步求解目标像素点的空间位置信 息。

需要说明的是，求解所得的目标像素点的空间位置信息进一步包 括：二维空间信息和三位空间信息。

进一步的，在一实施例中，在根据所获得的目标像素点对应的空间 位置信息及其匹配的历史图像中的目标像素点对应的空间位置信息，计 算得到拍摄装置的位姿信息的步骤之后，进一步包括：采用设定位姿优 化算法对所述计算得到的位姿信息进行调整得到优化后的摄像装置的 位姿信息。其中，在一实施例中，所设定的位姿优化算法可以是最小化 投影误差法。具体的，请参见图7。

考虑n个三维空间点P及其投影p，待优化的相机位姿R，t，它的 李代数表示为ξ。假设某空间点坐标为P_i＝[X_i,Y_i,Z_i]^T，其投影的像素 坐标为u_i＝[u_i,v_i]^T。根据其相机模型，可得其关系如下：

这里使用ξ表示相机位姿，写成矩阵形式就是：

s_iU_i＝Kexp(ξ^∧)P_i

式中隐式包含了齐次坐标到非齐次坐标的转换。现在，由于相机位 姿未知以及观测点的噪声，该等式存在误差。现在构建最小二乘问题， 然后寻找最好的相机位姿，使误差最小化，获得最优位姿：

该误差项，是将像素坐标与3D点按照当前估计的位姿进行投影得 到的位置相比较得到的误差，称为重投影误差。正如上图6所示的那样， p₁和p₂是同一个空间点P的投影。在初始值中，P的投影与实际的 p₂之间有一定的距离。于是，我们调整拍摄装置位姿，使得这个距离变 小，实现对所得的拍摄装置位姿的优化。

进一步的，请参见图8，为本申请一种视觉里程计的定位方法在一 实施例中的流程示意图。具体的，图8所示的示意图是当第一方法是特 征点匹配法时，步骤S30利用与第一方法对应的预设评估方式对获得的 目标像素点进行评估的流程示意图。由图可知，步骤S30进一步包括步 骤S31至步骤S33。

S31：将匹配的特征点对数量与第一预设阈值进行比对。

在当前实施例中，当第一方法为特征点匹配法时，步骤S20中完成 对于目标像素点的获取后，由于目标像素点即是目标图像中与历史图像 中的目标像素点成功匹配的像素点，所以目标图像中的特征点对的数量 则等于目标图像中的目标像素点的数量。那么步骤S31实质是将目标像 素点的数量与第一预设阈值进行比对，以判断使用特征点匹配法对目标 图像的跟踪是否是有效的。

其中，第一预设阈值是以经验性阈值，是一表示用于确定拍摄装置 的位置信息的像素点数量的经验性阈值，经过在数据库中计算优化得 到，又或者是以基于数据库中数据计算所得的初始阈值为基础，结合实 际应用场景中的使用进行优化所得的。可以理解的，在不同的应用场景 下，第一预设阈值的大小不相同。如在一相对空旷与一人流量很多物品拥挤的场所，第一预设阈值是不同的。

S32：当匹配的特征点对数量大于或等于所述第一预设阈值时，判 断所述获得的目标像素点是有效的。

当判断所获得的目标像素点是有效的，则基于所获得目标像素点可 以得出拍摄装置的准确位姿信息。则可以继续采用特征点匹配法对下一 帧图像继续进行处理，以完成视觉里程计。

S33：当匹配的特征点对的数量小于第一预设阈值时，则判断获得 的目标像素点是无效的。

可以理解的，当匹配的特征点对的数量小于第一预设阈值时，则特 征点匹配法在目标图像中的所获得目标像素点是无效的，则需要转换成 直接法重新获取目标图像中的目标像素点，以克服特征点匹配法的缺 点，并根据直接法获得目标像素点进一步确认拍摄装置的位姿信息。

其中，特征点匹配法对应的评估方式中所使用的评估函数具体如 下：

上述式中的θ₁是一个经验性的阈值，用以表示一帧图像中可以计算 出拍摄装置位姿像素点经验性的数量。这里的I_k表示的是第k帧图像， V(I_k)表示的是评估的结果，当V(I_k)＝True时，表示评估结果是有 效，即可以使用特征点匹配法所获取的目标像素点所确认的拍摄装置的 位姿信息，且还可以继续采用特征点匹配法继续获得下一帧图像中用于确认拍摄装置的位姿信息的目标像素点；当V(I_k)＝False时，评估结 果是无效，则判断当前的特征点匹配法所获得的目标像素点确认的拍摄 装置的位姿信息是不可用的，需要切换至直接法重新获取目标图像中的 用于确认拍摄装置位姿信息的目标像素点。

请参阅图9，为本申请视觉里程计的定位方法再一实施例的流程示 意图。具体的，采用直接法获得目标图像中用于确定拍摄装置的位姿信 息的目标像素点的步骤进一步包括步骤S61至步骤S63：

S61：对目标图像中的像素点进行梯度值测量。

首先会对目标图像中的每一个像素点进行梯度值测量，以选取具有 一定梯度值的像素点。像素点的梯度是基于相邻像素点间的灰度值的变 化产生的。

S62：确定梯度值大于或等于第二预设阈值的第一像素点。

由于本申请中的直接法是设定对于目标图像中的每一个像素点均 进行梯度检查，每一帧目标图像中均会有大量的普通的点(即不是本申 请中确定拍摄装置位姿信息的目标像素点)，所以会进一步设定第二预 设阈值进一步筛选第一像素点，可以在保证不会遗漏掉目标像素点的同 时，还可以提高计算速度，保证良好的实时性。其中，第一像素点是指 梯度值大于等于第二预设阈值的像素点。

其中，第二预设阈值是一个衡量像素点梯度的值。第二预设阈值的 大小可以根据实际的需要进行设定可调整大小，在此不做任何的限定。

S63：获取目标图像中在第一像素点的预设范围内的预设数量的第 二像素点作为目标像素点。

在确定完梯度值大于或等于第二预设阈值的第一像素点后，会进一 步获取当前确定的第一像素点预设范围内的预设数量的第二像素点。第 二像素点是第一像素点预设范围内像素点。然后将第一像素点与第二像 素点作为目标像素点，以基于目标像素点对应的图像信息计算拍摄装置 的位姿信息。

其中，需要说明的是，在一实施例中，当完成目标像素点的获取之 后，会进一步获取目标像素点的图像信息。其中，图像信息至少包括： 像素点的位置信息和像素点的灰度值。可以理解的，根据实际的需要， 还可以获取更多类型的像素点的图像信息，如亮度，具体在此不一一详 述。

进一步的，采用直接法利用所获得的目标像素点确定所述拍摄装置 的位姿信息，包括：利用目标图像中获得的目标像素点的图像信息，计 算得到所述拍摄装置的位姿信息。根据上述步骤中确定的目标像素点的 对应的图像信息，会进一步计算拍摄装置在拍摄当前在目标图像时的位 姿信息，这里的拍摄装置的位姿信息包括：拍摄装置的在拍摄参考图像 至拍摄目标图像期间内的位移量，以及拍摄目标图像时的拍摄装置的旋 转量。

其中，在一实施例中，利用所述目标图像中获得的目标像素点的图 像信息，计算得到所述拍摄装置的位姿信息的步骤进一步包括：由目标 图像中获得的目标像素点的图像信息构建得到优化问题，并对优化问题 求解得到拍摄装置的位姿信息。与特征点匹配法不同，直接法是直接基 于优化方法来计算拍摄装置的位姿。

再次参见图7，首先定义广度误差，即两个像素点之间的亮度误差：

e＝I_k(p₁)-I_k+1(p₂)

需要说明的是，上式中的k和k+1是表示目标图像(k帧)目标图 像和目标图像的下一帧(k+1帧)。由于目标图像中会有很多的个像素点，， 设定是N个像素点的情况下，最终基于目标图像中获得的目标像素点的 图像信息构建得到的需要优化的目标函数如下：

e_i＝I_k(p₁,i)-I_k+1(p₂,i)

为了求解上述目标函数，就需要知道e是如何随着拍摄装置位姿变 化的，即需要计算e与拍摄装置位姿ξ变化的，即需要计算他们的导数 关系。因此，使用李代数的扰动模型。

记，

这里的q为扰动分量在下一帧(k+1帧)目标图像中的相机坐标系下 的坐标，而u为它的像素坐标(即上述像素点的位置信息，是指二维坐 标)。利用一阶泰勒展开，有：

式中：

为u处的像素梯度；

为投影方程关于相机坐标系下的三维点的导数，记 q＝[X Y Z]^T，则导数为：

为变换后的三维点对变换的导数：

由于后两项只与三维点q有关，而与图像无关，把它合并之后为：

则误差相对于李代数的雅克比矩阵为：

在获得雅克比矩阵之后，然后使用高斯牛顿等方法计算增量，迭代 求解得到拍摄装置的位姿。

在获得雅克比矩阵之后，然后使用高斯牛顿等方法计算增量，具体 如下：

J(ξ)^TJ(ξ)Δξ＝-J(ξ)^Te(ξ)

J已经求得，将上述的式子改写如下：H·Δξ＝g，

那么，就可以使用代数的方法求解Δξ，然后更新所求的位姿ξ，当 Δξ足够小时，则停止迭代，最终得到当前拍摄装置最准确的位姿。

可选地，请参见图10，当第一方法是直接法时，利用与第一方法对 应的预设评估方式对获得的目标像素点进行评估的步骤具体包括步骤 S71至步骤S72。

S71：计算目标图像中第三像素点数量和参考图像中的目标像素点 数量间的比值。

其中目标图像中的第三像素点为目标图像中目标像素点中与所述 参考图像中的目标像素点对应的像素点。进一步的，统计目标图像中第 三像素点的数量后，计算第三像素点与参考图像中像素点数量间的比 值。即，比值＝(第三像素点数量)/(参考图像中目标像素点数量)。

S72：利用比值评估目标图像中所获得的目标像素点是否有效。

经过步骤S71中计算得到该比值后，会进一步利用该比值评估目标 图像中所获得的目标像素点是否是有效，以评估利用直接法获得的目标 图像中目标像素点确定的相机位姿是否是有效的。

其中，参考图像为连续采用直接法确定拍摄装置的位姿信息的至少 两帧图像中首帧图像。

进一步的，在一实施例中，当步骤S72评估得到，目标图像中第三 像素点数量和参考图像中的目标像素点数量间的比值大于或等于第三 预设阈值时，确定在目标图像中所获得的目标像素点是有效的。

在另一实施例中，当步骤S72评估得到该比值小于第三预设阈值时， 则确定目标图像中所获得的目标像素点是无效的。因为在参考帧中所确 定的目标像素点在目标图像中所占有的比例已经低于经验性阈值(第三 预设阈值)，所以可以确定的是，利用直接法获得的目标图像中的目标 像素点已经无法得出拍摄装置准确的位姿。

其中，直接法对应的评估方式中所采用的评估函数如下所示；

上述评估函数中的θ₂是一个分值，是一个经验性阈值。而M(I_k) 是目标图像中的目标像素点与参考图像帧原始像素点对应的像素点数 量(Sum(I_k))，与参考图像中目标像素点的数量(Sum(I_ref))之比。即

结合W(I_k)和θ₂可知，当目标图像中的目标像素点与参考图像帧 原始像素点对应的像素点数量(Sum(I_k))，与参考图像中目标像素点的 数量(Sum(I_ref))的比值大于等于θ₂时，W(I_k)则输出True，表示采 用直接法获得的目标图像中的目标像素点是有效的，即可以采用直接法 所获得的目标像素点所计算得出的拍摄装置的位姿信息，还可以继续采 用直接法获得下一帧目标图像的目标像素点；反之则表示采用直接法获 得的目标图像中的目标像素点是无效的，此时需要转换至特征点匹配法 重新获得目标图像中的目标像素点。

由上述各个实施例的阐述可知，本申请通过设定两个评估函数，对 应的对两种不同的方法所获得的像素点进行评估，以判断当前方法是否 有效，可以较好地实现根据当前图像的实际需要调整方法切换当前所使 用的方法。

其中，本申请所提供的技术方案中，若第一方法对应的评估方式得 到的评估结果为有效，则继续采用所述第一方法进行后一帧目标图像获 得用于确定所述摄像装置的位姿信息的像素点，以及利用与所述第一方 法对应的预设评估方式对所述获得的像素点进行评估。

进一步的，在一实施例中，评估得到使用特征点匹配法得到的目标 像素点是有效的，那么会在采用特征点匹配法确定的位姿信息作为拍摄 装置拍摄目标图像时的位姿信息，同时还会继续采用特征点匹配法获取 后一帧目标图像中用于确定拍摄装置的位姿信息的像素点，反之如果评 估结果是无效的，则会切换到直接法重新获取目标图像中用于确定拍摄 装置位置信息的目标像素点。需要说明的，在切换至直接法时的第一帧 图像，是不需要进行评估所获取的目标像素点，并会将目标图像作为参 考帧，用于采用直接法在获取后续各帧目标图像中目标像素点评估时做 参考。可以理解的，在不同的实施例中，在刚刚切换至直接法的第一帧 目标图像时，会进一步采用直接法获取第一帧目标图像的前一帧图像中 的目标像素点，作为参考图像，以实现对第一帧目标图像中的像素点进 行评估。

在另一实施例中，当评估得到使用直接法得到的目标像素点是有效 的，在采用直接法确定的拍摄装置的位姿信息的同时，会进一步采用直 接法获取下一帧目标图像中的目标像素点，以此确认拍摄装置拍摄时的 位姿信息，最终实现计算出拍摄装置在拍摄图像时的运动轨迹。

通过本申请所提供的技术方案中所定义的评估函数，通过评估得到 当前所采用的方法(特征点匹配法或直接法)对目标图像的处理是否有 效，实现根据目标图像的实际情况，在直接法和特征点匹配之间灵活切 换，提供了一种较好的视觉里程计的定位方法，可应对复杂多变的场景。

参见图11，本申请还提供一种视觉里程计100。其中，视觉里程计 100包括：处理器101，以及与处理器101耦接的拍摄装置103和存储 器102。其中，拍摄装置103用于拍摄得到图像；存储器102存储程序 数据和处理器101工作执行的结果。而处理器101在运行所存储的程序 数据时，用于执行如上述各个实施例中所述的视觉里程计的定位方法。

在一实施例中，拍摄装置103为普通的相机。此时处理器101可以 进一步执行获得普通相机拍摄所得图像所对应的深度图像的算法。

在另一实施例中，拍摄装置103为深度相机，可以在拍摄到图像后 直接通过相机获得图像所对应的深度图像及对应的深度图像信息，以供 通过深度图像信息，结合其他类别的信息，计算得出拍摄装置103的位 姿信息，从而完成视觉里程计的计算。

请参见图12，本申请还提供一种机器人200，该机器人200包括主 体202和如上所述的视觉里程计201.其中，视觉里程计201实现如上所 述的视觉里程计的定位方法。

参见图13，本申请还提供一种存储介质300。该存储介质存储有程 序数据，该程序数据被执行时实现如上所述视觉里程计的定位方法及各 个实施例中所描述的方法。具体的，上述具有存储功能的存储介质300 可以是存储器、个人计算机、服务器、网络设备，或者U盘等其中的一 种。

以上所述仅为本申请的实施方式，并非因此限制本申请的专利范 围，凡是利用本申请说明书及附图内容所作的等效结构或等效流程变 换，或直接或间接运用在其他相关的技术领域，均同理包括在本申请的 专利保护范围内。

Claims (15)

1.一种视觉里程计的定位方法，其特征在于，所述方法包括：

获取拍摄装置拍摄得到的目标图像；

采用第一方法获得所述目标图像中用于确定所述拍摄装置的位姿信息的目标像素点；

利用与所述第一方法对应的预设评估方式对所述获得的目标像素点进行评估；

若所述评估结果为有效，则将采用所述第一方法确定的位姿信息作为所述拍摄装置拍摄所述目标图像时的位姿信息；

若所述评估结果为无效，则采用第二方法获得所述目标图像中用于确定所述拍摄装置的位姿信息的目标像素点，以利用所述获得的目标像素点确定所述拍摄装置的位姿信息；

其中，所述第一方法和第二方法的其中一个为直接法，另一个为特征点匹配法。

2.根据权利要求1所述的视觉里程计的定位方法，其特征在于，所述采用特征点匹配法获得所述目标图像中用于确定所述拍摄装置的位姿信息的目标像素点，包括：

提取所述目标图像中的特征点；

将所述提取的特征点与历史图像的目标像素点进行匹配，其中，所述历史图像为所述目标图像的前n帧目标图像，所述n至少为1；

将所述目标图像中与所述历史图像中的目标像素点匹配成功的特征点作为所述用于确定所述拍摄装置的位姿信息的目标像素点；

所述采用特征点匹配法利用所述获得的目标像素点确定所述拍摄装置的位姿信息，包括：

根据所述获得的目标像素点对应的空间位置信息及其匹配的历史图像中的目标像素点对应的空间位置信息，确定所述摄像装置的位姿信息。

3.根据权利要求2所述的视觉里程计的定位方法，其特征在于，当所述第一方法为特征点匹配法时，所述利用与所述第一方法对应的预设评估方式对所述获得的目标像素点进行评估的步骤具体包括：

将所述匹配的特征点对数量与第一预设阈值进行比对；

当所述匹配的特征点对数量大于或等于所述第一预设阈值时，判断所述获得的目标像素点是有效的；

当所述匹配的特征点对的数量小于所述第一预设阈值时，则判断所述获得的目标像素点是无效的。

4.根据权利要求2所述的视觉里程计的定位方法，其特征在于，所述提取所述目标图像中的特征点，包括：

利用Fast算法检测所述目标图像中的角点；

利用Brief算法获取所述角点的特征信息，并将得到所述特征信息的角点作为目标图像中的特征点；

其中，所述特征信息包括：所述角点的位置信息和所述角点的描述子。

5.根据权利要求4所述的视觉里程计的定位方法，其特征在于，所述将所述提取的特征点与历史图像的目标像素点进行匹配包括：

根据所述特征信息将所述目标图像中的特征点与所述历史图像的目标像素点进行匹配。

6.根据权利要求2所述的视觉里程计的定位方法，其特征在于，将所述目标图像中与所述历史图像中的特征点匹配成功的特征点作为所述用于确定所述摄像装置的位姿信息的目标像素点之前，还包括：

采用RANSAC算法去除所述目标图像中误匹配的特征点。

7.根据权利要求2所述的视觉里程计的定位方法，其特征在于，所述根据所述获得的目标像素点对应的空间位置信息及其匹配的历史图像中的目标像素点对应的空间位置信息，确定所述摄像装置的位姿信息，包括：

根据所述获得的目标像素点对应的空间位置信息及其匹配的历史图像中的目标像素点对应的空间位置信息，计算得到所述拍摄装置的位姿信息；

采用设定位姿优化算法对所述计算得到的位姿信息进行调整得到优化后的摄像装置的位姿信息。

8.根据权利要求1所述的视觉里程计的定位方法，其特征在于，所述方法还包括：

所述采用直接法获得所述目标图像中用于确定所述拍摄装置的位姿信息的目标像素点，包括：

对所述目标图像中的像素点进行梯度值测量；

确定所述梯度值大于或等于所述第二预设阈值的第一像素点；

获取所述目标图像中在所述第一像素点的预设范围内的预设数量的第二像素点作为所述目标像素点；

所述采用直接法利用所述获得的目标像素点确定所述拍摄装置的位姿信息，包括：

利用所述目标图像中获得的目标像素点的图像信息，计算得到所述拍摄装置的位姿信息。

9.根据权利要求8所述的视觉里程计的定位方法，其特征在于，所述利用所述目标图像中获得的目标像素点的图像信息，计算得到所述拍摄装置的位姿信息，包括：

由所述目标图像中获得的目标像素点的图像信息构建得到优化问题，并对所述优化问题求解得到所述拍摄装置的位姿信息。

10.根据权利要求8所述的视觉里程计的定位方法，其特征在于，当所述第一方法为直接法时，所述利用与所述第一方法对应的预设评估方式对所述获得的目标像素点进行评估的步骤具体包括：

计算所述目标图像中第三像素点的数量和所述参考图像中的目标像素点数量间的比值；

利用所述比值评估所述目标图像中所获得的目标像素点是否有效；

其中，所述第三像素点为所述目标图像中与所述参考图像中的目标像素点对应的目标像素点，所述参考图像为连续采用所述直接法确定拍摄装置的位姿信息的至少两帧图像中首帧图像。

11.根据权利要求10所述的视觉里程计的定位方法，其特征在于，所述利用所述比值评估所述目标图像中所获得的目标像素点是否有效，具体包括：

当所述比值大于或等于所述第三预设阈值时，确定所述目标图像中所获得的目标像素点是有效的；

当所述比例值小于所述第三预设阈值时，确定所述目标图像中所获得的目标像素点是无效的。

12.根据权利要求1所述的视觉里程计的定位方法，其特征在于，若所述评估结果为有效，则继续采用所述第一方法进行后一帧目标图像获得用于确定所述拍摄装置的位姿信息的目标像素点，以及利用与所述第一方法对应的预设评估方式对所述获得的目标像素点进行评估。

13.一种视觉里程计，其特征在于，所述视觉里程计包括：处理器，以及与所述处理器耦接的拍摄装置和存储器；所述拍摄装置用于拍摄得到图像；所述存储器存储程序数据；所述处理器运行所述程序数据时，用于执行如权利要求1～12任一项所述的方法。

14.一种机器人，其特征在于，包括如权利要求13所述的视觉里程计。

15.一种存储介质，其特征在于，所述存储介质存储有程序数据，所述程序数据被执行时实现如权利要求1～12任一项所述的方法。