用于空间地图构建的空间不变量二次检测方法和设备

摘要

公开了用于空间地图构建的空间不变量二次检测方法和设备。所述方法包括：在通过利用一观测主体先后以第一位姿和第二位姿对未知空间进行两次观测而得到的一组观测图像中，获取已经初步检测到的n个空间不变量；分别计算两次观测中相应空间不变量的距离，其中对于第i个空间不变量，在第一次观测中与第j个空间不变量的距离为dij，在第二次观测中与第j个空间不变量的距离为dij’，其中i，j均小于等于n，且j≠i；基于预定投票策略，对特定数量的空间不变量进行投票；以及根据投票结果以及预定判定策略在n个空间不变量之中判定正确的N个空间不变量，从而过滤掉未正确匹配的空间不变量，其中N是自然数且小于或等于n。

说明书

技术领域

本发明涉及用于空间地图构建的空间不变量二次检测方法和设备以及机 器人快速位置估计方法和设备。更具体地说，涉及能够降低计算量并提高运 行速度的空间不变量二次检测方法和设备以及使用该空间不变量二次检测方 法和设备的机器人快速位置估计方法和设备。

背景技术

三维定位与地图构建是移动机器人在未知环境中进行运动导航、路径规 划、物体识别和语义理解的关键支撑技术之一。机器人在连续移动过程中， 通过匹配在不同观测位置的三维观测数据，可以计算出机器人的位置。匹配 观测数据与计算位置信息的核心是准确地找到两次观测结果中的空间不变 量，进而以空间不变量为基准点计算出两次观测点的位置变化量，完成机器 人的位置估计。

图1A、1B示出了基于两次观测数据进行位置姿态变化估计的示意图。 在图1A中，示出了机器人在(0，0，0，0，0，0)位置姿态下的观测数据。 在图1B中，示出了机器人移动到(x，y，z，roll，pitch，yaw)位置姿态下 的观测数据。其中，图1A、1B中的6个方框表示从两次观测数据中检测出 的6组空间不变量(6组三维点对)。这里需要说明的是，所谓空间不变量是 指在观测主体(即，这里的机器人)以不同位姿进行的两次观测中相同的空 间点。由于机器人的移动，在两次观测中，机器人相对6个空间不变点的位 置发生变化，基于变化可计算出(x，y，z，roll，pitch，yaw)的具体数值， 从而完成机器人的位置姿态估计。需要指出的是，虽然图1A、1B中以6组 空间不变量作为示例进行描述，但是在理论上，只需要通过3组在空间中不 共线的空间不变点，就可求得(x，y，z，roll，pitch，yaw)的具体数值。

由此可见，能够准确地检测出两次观测数据中的空间不变量(也可称为 特征点)是问题的关键。目前已经有很多研究成果能够较为有效地从二维灰 度图像或三维点云观测数据中检测出空间不变量(例如，通过灰度值匹配的 方式)，但仍存在较大误差，会发生错误检测的情况。

图2A、2B示出了不正确的空间不变量引发位置姿态错误估计的示意图。 在图2A、2B中，由于第4组与第6组空间不变量的不正确匹配，将导致机 器人位置姿态的错误估计。因此，现有解决方案普遍对已经初步检测出的空 间不变量进行后处理，进行空间不变量的二次检测，过滤掉错误数据。

现有的空间不变量二次检测使用RANSAC(RANdom SAmpling  Consensus)方法。该方法通过多次随机实验，来遴选空间不变量。在每次实 验中，随机选取其中三组空间不变量(A1，A2，A3)，这三组不变量计算出位置 姿态信息，并以该位置姿态信息为标准，判断其他的空间不变量是否正确， 将会找到所有与(A1，A2，A3)保持空间不变性质的准确的空间不变量 M1＝(A1，A2，A3，A4，...An)，并计算如此获得的该组空间不变量M1的误差。经 过多次随机实验，将得到多组遴选出的空间不变量(M1，M2，M3，...，Mn)。选取 其中误差最小的一组作为最终的过滤掉错误数据的空间不变量。

该方法在每次随机实验中需要多次计算机器人的位置姿态变化量，计算 量大，运行速度慢。另外，当实验次数较少时，不能找到最佳的空间不变量。

发明内容

鉴于以上情形，本发明提出了新的基于空间不变量选举检测策略的空间 不变量二次检测方法和设备以及使用该空间不变量二次检测方法和设备的机 器人快速位置估计的方法和设备，其不需要多次计算机器人的位置姿态变化 量，计算量小，运行速度快，经过测试与RANSAC方法相比能够提高1-2个 数量级。

根据本发明实施例的一个方面，提供了一种用于空间地图构建的空间不 变量二次检测方法，所述方法包括：在通过利用一观测主体先后以第一位姿 和第二位姿对未知空间进行两次观测而得到的一组观测图像中，获取已经初 步检测到的n个空间不变量，其中所述观测主体至少具备一图像采集单元以 获取观测图像，且n为自然数；分别计算两次观测中相应空间不变量的距离， 其中对于第i个空间不变量，在第一次观测中与第j个空间不变量的距离为 dij，在第二次观测中与第j个空间不变量的距离为dij’，其中i，j均小于等于 n，且j≠i；基于预定投票策略，对特定数量的空间不变量进行投票，其中对 于某一个空间不变量进行投票是指由所有其他空间不变量对于所述某一个空 间不变量进行投票；以及根据投票结果以及预定判定策略在n个空间不变量 之中判定正确的N个空间不变量，从而过滤掉未正确匹配的空间不变量，其 中N是自然数且小于或等于n。

优选地，在根据本发明实施例的方法中，所述空间不变量是指在观测主 体以不同位姿进行的两次观测中相同的空间点。

优选地，在根据本发明实施例的方法中，所述预定投票策略为：

判断|dij-dij’|/max(|dij|，|dij’|)是否小于预定阈值，如果|dij-dij’|/max(|dij|， |dij’|)小于所述预定阈值，则第i个空间变量与第j个空间变量互投赞成票， 否则互投反对票。

优选地，在根据本发明实施例的方法中，所述观测主体为机器人。

优选地，在根据本发明实施例的方法中，对所有空间不变量进行投票， 并且所述预定判定策略为：判定得到赞成票最多的N个空间不变量为正确的 空间不变量。

优选地，在根据本发明实施例的方法中，根据设计精度来确定N的数值。

优选地，在根据本发明实施例的方法中，任意选择一个空间不变量进行 投票，并且所述预定判定策略为：如果所选择的空间不变量得到大于n/2的 赞成票，则判定其为正确的空间不变量，并且将为其投赞成票的空间不变量 判定为正确的空间不变量，同时删除为其投反对票的空间不变量；而如果所 选择的空间不变量未得到大于n/2的赞成票，则判定其为错误的空间不变量， 并任意选取剩余的空间不变量之一，然后重复以上处理，直到得到判定为正 确的空间不变量为止。

根据本发明实施例的另一个方面，提供了一种机器人快速位置估计方法， 包括如下步骤：机器人分别以初始位姿和当前位姿对未知空间进行两次观测 从而获取一组观测图像；初步检测该组观测图像中表示相同空间点的空间不 变量；根据上述方法对空间不变量进行二次检测，以便过滤掉未正确匹配的 空间不变量；以及基于正确匹配的空间不变量获得机器人的当前位姿。

根据本发明实施例的再一个方面，提供了一种用于空间地图构建的空间 不变量二次检测设备，所述设备包括：获取单元，用于在通过利用一观测主 体先后以第一位姿和第二位姿对未知空间进行两次观测而得到的一组观测图 像中，获取已经初步检测到的n个空间不变量，其中所述观测主体至少具备 一图像采集单元以获取观测图像，且n为自然数；距离计算单元，用于分别 计算两次观测中相应空间不变量的距离，其中对于第i个空间不变量，在第 一次观测中与第j个空间不变量的距离为dij，在第二次观测中与第j个空间 不变量的距离为dij’，其中i，j均小于等于n，且j≠i；投票单元，用于基于 预定投票策略，对特定数量的空间不变量进行投票，其中对于某一个空间不 变量进行投票是指由所有其他空间不变量对于所述某一个空间不变量进行投 票；以及判定单元，用于根据投票结果以及预定判定策略在n个空间不变量 之中判定正确的N个空间不变量，从而过滤掉未正确匹配的空间不变量，其 中N是自然数且小于或等于n。

优选地，在根据本发明实施例的设备中，所述空间不变量是指在观测主 体以不同位姿进行的两次观测中相同的空间点。

优选地，在根据本发明实施例的设备中，所述预定投票策略为：判断 |djj-dij’|/max(|dij|，|dij’|)是否小于预定阈值，如果|dij-dij’|/max(|dij|，|dij’|) 小于所述预定阈值，则第i个空间变量与第j个空间变量互投赞成票，否则互 投反对票。

优选地，在根据本发明实施例的设备中，所述观测主体为机器人。

优选地，在根据本发明实施例的设备中，所述投票单元对所有空间不变 量进行投票，并且所述预定判定策略为：判定得到赞成票最多的N个空间不 变量为正确的空间不变量。

优选地，在根据本发明实施例的设备中，根据设计精度来确定N的数值。

优选地，在根据本发明实施例的设备中，所述投票单元任意选择一个空 间不变量进行投票，并且所述预定判定策略为：如果所选择的空间不变量得 到大于n/2的赞成票，则判定其为正确的空间不变量，并且将为其投赞成票 的空间不变量判定为正确的空间不变量，同时删除为其投反对票的空间不变 量；而如果所选择的空间不变量未得到大于n/2的赞成票，则判定其为错误 的空间不变量，并任意选取剩余的空间不变量之一，然后重复以上处理，直 到得到判定为正确的空间不变量为止。

根据本发明实施例的又一个方面，提供了一种机器人快速位置估计设备， 包括：图像采集单元，用于分别以初始位姿和当前位姿对未知空间进行两次 观测从而获取一组观测图像；空间不变量初步检测单元，初步检测图像采集 单元获取的该组观测图像中表示相同空间点的空间不变量；如上所述的空间 不变量二次检测设备，用于对空间不变量初步检测单元检测出的空间不变量 进行二次检测，以便过滤掉未正确匹配的空间不变量；以及位姿计算单元， 用于基于正确匹配的空间不变量获得机器人的当前位姿。

通过根据本发明实施例的空间不变量二次检测方法和设备，能够极大地 降低计算量并提升处理速度，同时提高空间不变量的匹配精度。此外，采用 上述空间不变量二次检测方法和设备的机器人快速位置估计方法和设备，能 够在降低计算量并提升处理速度的同时，更好地完成位姿估计。

附图说明

图1A-1B示出了基于两次观测数据进行位置姿态变化估计的示意图；

图2A-2B示出了不正确的空间不变量引发位置姿态错误估计的示意图；

图3A-3B分别示出了两次观测中各空间不变量之间的相对距离；

图4是示出了根据本发明实施例的用于空间地图构建的空间不变量二次 检测方法的流程的流程图；

图5是示出了根据本发明实施例的机器人快速位置估计方法的流程的流 程图；

图6是示出了根据本发明实施例的用于空间地图构建的空间不变量二次 检测设备的配置的框图；以及

图7是示出了根据本发明实施例的机器人快速位置估计设备的配置的框 图。

具体实施方式

下面将参照附图对本发明的各个优选的实施方式进行描述。提供以下参 照附图的描述，以帮助对由权利要求及其等价物所限定的本发明的示例实施 方式的理解。其包括帮助理解的各种具体细节，但它们只能被看作是示例性 的。因此，本领域技术人员将认识到，可对这里描述的实施方式进行各种改 变和修改，而不脱离本发明的范围和精神。而且，为了使说明书更加清楚简 洁，将省略对本领域熟知功能和构造的详细描述。

在根据本发明实施例的方法和设备中，以各空间不变量之间的相对距离 为判断依据，对各不变量进行投票选举，快速筛选出正确的空间不变量。图 3A-3B分别示出了两次观测中各空间不变量之间的相对距离。如图3A、3B 所示，对于正确的空间不变量(1，2，3，4，5)来说，在两次观测中它们之 间的相对距离保持不变。

图4是示出了根据本发明实施例的用于空间地图构建的空间不变量二次 检测方法的流程的流程图。

如图4所示，所述方法包括如下步骤：

首先，在步骤S401，在通过利用一观测主体先后以第一位姿和第二位姿 对未知空间进行两次观测而得到的一组观测图像中，获取已经初步检测到的 n个空间不变量，其中所述观测主体至少具备一图像采集单元以获取观测图 像，且n为自然数。例如，这里所述的观测主体可以是机器人，第一位姿可 以是(0，0，0，0，0，0)，第二位姿可以是(x，y，z，roll，pitch，yaw)。 另外，需要指出的是，所述空间不变量是指在观测主体以不同位姿进行的两 次观测中相同的空间点。

然后，在步骤S402，分别计算两次观测中相应空间不变量的距离，其中 对于第i个空间不变量，在第一次观测中与第j个空间不变量的距离为dij， 在第二次观测中与第j个空间不变量的距离为dij’，其中i，j均小于等于n， 且j≠i。

然后，在步骤S403，基于预定投票策略，对特定数量的空间不变量进行 投票。这里，根据进行投票的空间不变量的数量的不同，可以分为全投票和 部分投票两种实施例。需要指出的是，对于某一个空间不变量进行投票是指 由所有其他空间不变量对于所述某一个空间不变量进行投票。

例如，所述预定投票策略为：判断|dij-dij’|/max(|dij|，|dij’|)是否小于预 定阈值，如果|dij-dij’|/max(|dij|，|dij’|)小于所述预定阈值，则第i个空间变 量与第j个空间变量互投赞成票，否则互投反对票。

最后，在步骤S404，根据投票结果以及预定判定策略在n个空间不变量 之中判定正确的N个空间不变量，从而过滤掉未正确匹配的空间不变量，其 中N是自然数且小于或等于n。

下面，首先描述全投票实施例。在该实施例中，对所有空间不变量进行 投票。在这种情况下，上述步骤S404中的所述预定判定策略为：判定得到赞 成票最多的N个空间不变量为正确的空间不变量。

需要指出的是，这里的N可以根据设计精度来调整。例如，设计精度越 高，则上述判定策略中选取的空间不变量的数量越大，即N值越大。反之， 则N值越小。一般来说，N的取值范围为50～300。

如上所述，现有RANSAC方法的准确性受到实验次数的限制，仅能找到 较为准确的空间不变量。而通过根据本发明的该全投票实施例，能够找到最 为准确的空间不变量。而且，由于不需要多次计算观测主体(即，机器人) 的位置姿态变化量，所有计算只涉及简单的距离计算及大小判断，因此大幅 度地降低了计算量，处理速度得到提升。经测试，性能能够提升1～2个数量 级。

接下来，将描述部分投票实施例。该实施例与之前描述的全投票实施例 的不同之处在于，并非对全部空间不变量进行投票。替代地，首先任意选择 一个空间不变量进行投票。在这种情况下，上述步骤S404中的所述预定判定 策略为：如果所选择的空间不变量得到大于n/2的赞成票，则判定其为正确 的空间不变量，并且将为其投赞成票的空间不变量判定为正确的空间不变量， 同时删除为其投反对票的空间不变量；如果所选择的空间不变量未得到大于 n/2的赞成票，则判定其为错误的空间不变量，并任意选取剩余的空间不变量 之一，然后重复以上处理，直到得到判定为正确的空间不变量为止。

根据以上的描述可知，部分投票实施例在全投票实施例的基础上进一步 降低了运算量并提升了处理速度。但是，部分投票实施例与全投票实施例相 比，缺点在于精度不高。换言之，不能像全投票实施例那样能够找到最为准 确的空间不变量。因此，根据具体的设计要求，可以适当地选取全投票或部 分投票实施例。

以上描述了根据本发明实施例的空间不变量二次检测方法。显而易见地， 该方法可以应用于现有技术中的机器人快速位置估计。由于根据本发明实施 例的空间不变量二次检测方法能够在降低计算量并提升处理速度的同时找到 正确匹配的空间不变量，从而能够更好地完成机器人的位置姿态估计。

下面参照图5描述应用了根据本发明实施例的空间不变量二次检测方法 的机器人快速位置估计方法的流程。

如图5所示，机器人快速位置估计方法包括如下步骤：

首先，在步骤S501，机器人分别以初始位姿和当前位姿对未知空间进行 两次观测从而获取一组观测图像。

然后，在步骤S502，初步检测该组观测数据中表示相同空间点的空间不 变量。例如，如前所述，在现有技术中，目前已经有很多研究成果能够较为 有效地从二维灰度图像或三维点云观测数据中检测出空间不变量(例如，通 过灰度值匹配的方式)。这只是空间不变量的初步匹配。

接下来，在步骤S503，通过根据如上参照图4所述的空间不变量二次检 测方法对空间不变量进行二次检测，以便过滤掉未正确匹配的空间不变量。

最后，在步骤S504，基于正确匹配的空间不变量获得机器人的当前位姿。

下面，将参照图6描述根据本发明实施例的用于空间地图构建的空间不 变量二次检测设备。如图6所示，所述设备600包括获取单元601、距离计 算单元602、投票单元603和判定单元604。

获取单元601在通过利用一观测主体先后以第一位姿和第二位姿对未知 空间进行两次观测而得到的一组观测图像中，获取已经初步检测到的n个空 间不变量。其中，所述观测主体至少具备一图像采集单元以获取观测图像， 且n为自然数。

距离计算单元602在获取单元601获取的n个空间不变量中，分别计算 两次观测中相应空间不变量的距离，其中对于第i个空间不变量，在第一次 观测中与第j个空间不变量的距离为dij，在第二次观测中与第j个空间不变 量的距离为dij’，其中i，j均小于等于n，且j≠i。

投票单元603根据距离计算单元602计算出的各空间不变量之间的相对 距离，基于预定投票策略，对特定数量的空间不变量进行投票。

这里，根据进行投票的空间不变量的数量的不同，可以分为全投票和部 分投票两种实施例，并且对于某一个空间不变量进行投票是指由所有其他空 间不变量对于所述某一个空间不变量进行投票。

判定单元604根据投票单元603得到的投票结果以及预定判定策略在n 个空间不变量之中判定正确的N个空间不变量，从而过滤掉未正确匹配的空 间不变量，其中N是自然数且小于或等于n。

由于在根据本发明实施例的空间不变量二次检测设备中，有关“预定投 票策略”、“全投票实施例”、“部分投票实施例”和“预定判定策略”的内容 与前述空间不变量二次检测方法中的对应内容完全相同，故为了简明起见， 其细节不再赘述。

类似地，上述根据本发明实施例的空间不变量二次检测设备600也可以 应用于现有技术中的机器人快速位置估计设备。下面将参照图7描述应用了 根据本发明实施例的空间不变量二次检测设备的机器人快速位置估计设备 700。

如图7所示，机器人快速位置估计设备700包括：图像采集单元701、 空间不变量初步检测单元702、如上所述的空间不变量二次检测设备600以 及位姿计算单元703。

图像采集单元701分别以初始位姿和当前位姿对未知空间进行两次观测 从而获取一组观测图像。

空间不变量初步检测单元702在图像采集单元701获取的该组观测图像 中，初步检测表示相同空间点的空间不变量；

空间不变量二次检测设备600对空间不变量初步检测单元702检测出的 空间不变量进行二次检测，以便过滤掉未正确匹配的空间不变量；

位姿计算单元703基于经空间不变量二次检测设备600过滤后的正确匹 配的空间不变量获得机器人的当前位姿。

以上参照附图，描述了根据本发明实施例的空间不变量二次检测方法和 设备以及应用了其的机器人快速位置估计方法和设备。通过根据本发明实施 例的空间不变量二次检测方法和设备，能够极大地降低计算量并提升处理速 度，同时提高空间不变量的匹配精度。此外，采用上述空间不变量二次检测 方法和设备的机器人快速位置估计方法和设备，能够在降低计算量并提升处 理速度的同时，更好地完成位姿估计。

需要说明的是，在本说明书中，术语“包括”、“包含”或者其任何其他变 体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品 或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是 还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的 情况下，由语句“包括......”限定的要素，并不排除在包括所述要素的过程、 方法、物品或者设备中还存在另外的相同要素。

最后，还需要说明的是，上述一系列处理不仅包括以这里所述的顺序按 时间序列执行的处理，而且包括并行或分别地、而不是按时间顺序执行的处 理。

通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到本发 明可借助软件加必需的硬件平台的方式来实现，当然也可以全部通过软件来 实施。基于这样的理解，本发明的技术方案对背景技术做出贡献的全部或者 部分可以以软件产品的形式体现出来，该计算机软件产品可以存储在存储介 质中，如ROM/RAM、磁碟、光盘等，包括若干指令用以使得一台计算机设 备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例 或者实施例的某些部分所述的方法。

以上对本发明进行了详细介绍，本文中应用了具体个例对本发明的原理 及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法 及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在 具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容不应 理解为对本发明的限制。