[发明专利]用于空间地图构建的空间不变量二次检测方法和设备

说明书

公开了用于空间地图构建的空间不变量二次检测方法和设备。所述方法包括：在通过利用一观测主体先后以第一位姿和第二位姿对未知空间进行两次观测而得到的一组观测图像中，获取已经初步检测到的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)。选取其中误差最小的一组作为最终的过滤掉错误数据的空间不变量。

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

发明内容