用于执行定位的方法和系统
技术领域
本发明涉及用于在三维(3D)环境中执行定位的方法和系统。该方法和系统具体应用于移动机器人和自动驾驶车辆导航。
背景技术
本说明书中对任何现有出版物(或从其得出的信息)或任何已知事项的引用,均不是也不应该视为是对现有出版物(或从其得出的信息)或已知事项形成本说明书所涉及的研究领域中的公知常识的一部分的承认或认可或任何形式的暗示。
全局定位是自动机器人导航的关键要素。它用于诸如机器人“唤醒”问题、“迷失或遭绑架”机器人问题、以及即时定位与地图构建(SLAM)中的环路闭合的任务。全局定位任务由两个主要元素组成:i)识别先前访问过的地点,以及ii)相对于重新访问的地点的坐标系的位置和方向(位姿)估计。为了实现全面的全局定位,需要地点识别和位姿估计。通常,在执行全局定位时,将通过扫描环境中的某个地点(局部扫描)获得的小点云与先前获得的扫描数据(全局地图)进行比较,并计算相对转换。
解决全局定位任务的经典方法是使用递归估计方法,例如卡尔曼滤波器或蒙特卡洛定位(粒子滤波器方法)。这些算法是在机器人传感器获得的数据有限且嘈杂时适时开发的。通过静态观察周围环境无法唯一确定机器人在全局地图中的位置。因此,在这些方法中,机器人必须首先四处行驶,观察环境并更新处于特定位置的可能性,直到确定性增加到可接受的水平。
也可以使用外部传感器基础设施(例如,全球定位系统(GPS)或外部信标)执行定位,尽管这样做会受到限制,因为在自动驾驶车辆或机器人可能会在其中导航的许多环境中(例如,在无法使用GPS的室内位置),所述外部传感器基础设施不可用。摄像机也可以用于地点识别,但是在不利的照明条件下,它们的性能会降低,并且无法在黑暗中进行定位。
已经提出了几种在3D点云中进行全局定位的方法,例如使用3D光成像、检测和测距(LiDAR)传感器。已知使用全局描述符(基于几何信息为整个局部扫描计算单个统计信息)可通过点高程直方图描述地点。替选的方法是通过将点云划分为三次网格来创建描述符,并计算描述形状的密度函数,这些形状随后被收集到直方图中。全局描述符的一个问题是,全局地图被解释为沿着轨迹的一组离散地点,并且每个地点都有一个定义的原点,形成了类似图形的结构。在这种情况下,唯一可能的机器人位置在图形顶点中。当机器人位于顶点之间或偏离原始轨迹时,无法确定确切位置。
还已知基于点云中的几何特征使用局部描述符来执行定位。从点云中选择多个关键点,并在它们周围计算局部描述符(半径远小于点云的局部描述符)。通过在不同的云之间找到最相似的描述符来执行识别。这种类型的几何识别过程效率低下且计算复杂,尤其是对于大型地图。
虽然可以使用局部几何描述符来获得变换和准确的位姿,但基于此方式的定位方法在计算上效率低下,不适合实时应用。全局描述符的使用是有用的,因为到目前为止,它为降维和对应搜索提供了一种高效的方法,但是它们不能提供相对的变换和位姿。
因此,需要一种更高效的定位算法,该算法能够在没有GPS基础设施等的非结构化环境中以及在任何照明条件下快速定位。
此外,能够根据固定扫描定位并且仅使用诸如3D LiDAR的激光传感器将是有利的。
正是基于这种背景以及与之相关的问题和困难,开发了本发明。
发明内容
在广义上,本发明的一方面试图提供一种用于在三维(3D)环境中执行定位的方法,该方法包括,在一个或更多个电子处理设备中:
a)至少部分地基于从局部扫描获得的局部点云中的点返回到至少一个激光传感器的辐射功率,根据由至少一个激光传感器执行的局部扫描确定强度数据;
b)使用强度数据计算局部点云的第一强度描述符;
c)检索多个先前计算的第二强度描述符,每个第二强度描述符分别与3D环境的地图的各个部分相关联;
d)将第一强度描述符与第二强度描述符中的至少一些进行比较;以及,
e)至少部分地根据比较结果来确定相对于地图的位置。
在一个实施方案中,所述方法包括,在一个或更多个电子处理设备中,基于用于强度数据在局部点云中的分布的至少一个统计度量来计算第一强度描述符。
在一个实施方案中,所述方法包括,在一个或更多个电子处理设备中通过以下方式计算第一强度描述符:
a)将局部点云划分为多个空间分布的局部点云区段;
b)对于每个局部点云区段,基于用于局部点云区段内的强度数据的分布的统计度量来计算第一强度描述符区段;以及,
c)使用每个第一强度描述符区段计算第一强度描述符。
在一个实施方案中,统计度量指示多个强度值中的每一个的频率的分布。
在一个实施方案中,局部点云区段具有至少部分球形的结构。
在一个实施方案中,局部点云区段包括以下中的至少一个:
a)多个同心球形区域;以及,
b)至少两个分割成八分圆的同心球形区域。
在一个实施方案中,外部球形区域的半径是基于传感器的范围的,并且选择一个或更多个内部球形区域的半径,使得每个球形区域包含相似数量的局部点云点。
在一个实施方案中,第二强度描述符是使用与对应于3D环境的地图的点云中的点相关联的强度数据来预先计算的,该强度数据至少部分地基于在先前对3D环境执行的扫描中,从点云中的点返回到至少一个激光传感器的辐射功率。
在一个实施方案中,每个第二强度描述符包括针对与地图的各个部分相对应的点云的一部分的点云区段而获得的第二强度描述符区段。
在一个实施方案中,所述多个第二强度描述符存储在数据存储器中,并且其中,所述方法包括,在一个或更多个电子处理设备中,从数据存储器中检索多个第二强度描述符。
在一个实施方案中,所述方法包括,在一个或更多个电子处理设备中:
a)通过以下方式将第一强度描述符与第二强度描述符进行比较:
i)比较第一强度描述符区段和第二强度描述符区段;以及,
ii)根据比较结果确定相似度值;
b)基于相似度值选择一个或更多个第二强度描述符。
在一个实施方案中,该方法包括,在一个或更多个电子处理设备中:
a)对于第一强度描述符和第二强度描述符之间的每个比较:
i)确定第二强度描述符的多个相对方向;以及,
ii)对于第二强度描述符的多个相对方向中的每个相对方向,将第一强度描述符区段与第二强度描述符区段进行比较;
b)确定每个方向的相似度值;以及,
c)选择最小相似度值。
在一个实施方案中,所述方法包括:在一个或更多个电子处理设备中,使用第一强度描述符与各个第二强度描述符之间的相似度值来将地图的部分排序为潜在位置。
在一个实施方案中,所述方法包括,在一个或更多个电子处理设备中:
a)使用基于地图的部分的排序选择的至少一个地图的部分作为潜在位置来生成子地图;以及,
b)在子地图中执行几何识别。
在一个实施方案中,所述方法包括,在一个或更多个电子处理设备中,通过以下方式执行几何识别:
a)从局部点云中提取局部关键点;
b)为每个局部关键点计算局部几何描述符;
c)检索先前计算的子地图几何描述符,该子地图几何描述符是使用与子地图中包括的地图的部分相关联的子地图关键点计算得出的;
d)确定局部几何描述符和子地图几何描述符之间的对应关系;
e)基于几何一致性对对应关系进行聚类以形成一个或更多个候选簇;以及,
f)确定每个候选簇的转换。
在一个实施方案中,所述方法包括,在一个或更多个电子处理设备中:
a)选择对应关系数量最高的候选簇;
b)将选择的候选簇中的对应关系数量与第一阈值进行比较;
c)以下中的至少一个:
i)如果对应关系数量低于第一阈值,则确定出选择的候选簇为错误匹配;
以及,
ii)如果对应关系数量高于第一阈值,则确定出选择的候选簇指示最可能的位置。
在一个实施方案中,对于指示最可能的位置的候选簇,所述方法包括,在一个或更多个电子处理设备中,优化变换以将局部点云和与子地图相关联的子地图点云对齐。
在一个实施方案中,所述方法包括,在一个或更多个电子处理设备中,对局部点云和子地图点云的至少一部分执行迭代最近点(ICP),以优化变换并确定位姿。
在一个实施方案中,所述方法包括,在一个或更多个电子处理设备中:
a)确定适应度得分,该适应度得分指示局部点云和子地图点云的对齐程度;
b)比较适应度得分与第二阈值;以及
c)根据比较结果选择性地验证位置。
在一个实施方案中,所述方法包括,在一个或更多个电子处理设备中,如果以下中的一个发生,则生成新的子地图:
a)对应关系数量低于第一阈值;以及,
b)适应度得分高于第二阈值。
在一个实施方案中,所述方法包括,在一个或更多个电子处理设备中,至少部分地使用基于地图的部分的排序而选择的地图的其他部分作为潜在位置来生成新的子地图。
在一个实施方案中,所述方法包括,在一个或更多个处理设备中:
a)确定子地图中已经包括多少个潜在位置;以及,
b)以下中的至少一个:
i)如果潜在位置的数量已经达到定义的最大值,则终止定位方法;以及,
ii)如果潜在位置的数量低于定义的最大值,则增加子地图的尺寸。
在一个实施方案中,局部扫描为以下中的至少一个:
a)固定扫描;
b)与移动机器人或自动驾驶汽车相关的唤醒扫描;以及,
c)环路闭合扫描。
在一个实施方案中,所述至少一个激光传感器是光成像、检测和测距(LiDAR)传感器。
在一个实施方案中,沿着地图构建轨迹从地图中提取地图的部分。
在一个实施方案中,将地图构建轨迹划分为预定长度的区段,并且将地图的每个部分定义为从至少一个传感器沿着地图构建轨迹的相应区段行进时获得的点云。
在另一种广义形式中,本发明的一方面试图提供一种用于在三维(3D)环境中执行定位的系统,所述系统包括一个或更多个电子处理设备,所述一个或更多个电子处理设备配置为:
a)至少部分地基于从局部扫描获得的局部点云中的点返回到至少一个激光传感器的辐射功率,根据由至少一个激光传感器执行的局部扫描确定强度数据;
b)使用强度数据计算局部点云的第一强度描述符;
c)检索多个先前计算的第二强度描述符,多个先前计算的第二强度描述符分别与3D环境的地图的各个部分相关联;
d)将第一强度描述符与第二强度描述符中的至少一些进行比较;以及,
e)至少部分地根据比较结果来确定相对于地图的位置。
在一个实施方案中,所述系统包括至少一个激光传感器,所述至少一个激光传感器配置为执行局部扫描。
在一个实施方案中,所述至少一个传感器是光成像、检测和测距(LiDAR)传感器。
在一个实施方案中,该系统进一步包括包含多个第二强度描述符的数据存储器,并且其中一个或更多个电子处理设备可访问该数据存储器以检索多个第二强度描述符。
在一个实施方案中,所述至少一个激光传感器、一个或更多个电子处理设备和数据存储器位于移动机器人或自动驾驶车辆上。
应当理解,本发明的广义形式及其各自的特征可以结合地,可互换地和/或独立地使用,并且引用单独的广义形式不旨在进行限制。
附图说明
现在将参考附图描述本发明的非限制性示例,其中:-
图1是用于在3D环境中执行定位的方法的示例的流程图;
图2是用于在3D环境中执行定位的系统的示例的示意图;
图3是图2的处理系统的示例的示意图;
图4A至图4C提供了在3D环境中执行定位的具体过程的示例的流程图;
图5是计算与局部扫描相关的第一强度描述符的示例的流程图;
图6A是强度描述符的结构的示例;
图6B是示例性的唤醒扫描的示例,该扫描被划分为空间分布的区段;
图7是用于完全不同地点的强度描述符之间的比较的示例图;
图8是第二强度描述符的示例,其示出了以四种可能的方式定向的坐标系,以与第一强度描述符进行比较;
图9是执行3D环境的地图的预处理的示例的流程图;
图10A是进行实验的地点的卫星照片;
图10B是通过地图构建图10A的位置而获得的点云的示例,其中用白色示出地图构建轨迹;
图10C是定位成功率与先验地点数的关系的示意图;
图10D是在距地图构建轨迹不同距离处的唤醒位置的定位成功率的示意图;
图10E是成功定位在距地图构建轨迹不同距离处的唤醒位置所需的地点候选数量的示意图;
图10F是与仅使用传统的基于几何的方法相比,使用本方法进行定位的计算时间的对数图;以及,
图10G通过使用基于强度的识别示出从地图提取的地点候选,示出了使用系统获得的搜索空间减少的示例。
具体实施方式
现在将参考图1描述用于在三维(3D)环境中执行定位的方法的示例。
为了说明的目的,假设该方法至少部分地使用一个或更多个电子处理设备来执行,一个或更多个电子处理设备形成与自动驾驶车辆或移动机器人(以下称为“机器人”)相关联的一个或更多个处理系统的一部分。所述一个或更多个电子处理设备配置为从至少一个激光传感器接收数据,激光传感器诸如安装在机器人上并且能够执行机器人周围环境扫描的光成像、检测和测距(LiDAR)传感器。
在该示例中,在步骤100,该方法包括:至少部分地基于从局部扫描获得的局部点云中的点返回到至少一个激光传感器的辐射功率,根据由至少一个激光传感器执行的局部扫描来确定强度数据。应当理解,局部扫描是3D环境的一部分的扫描,例如在“唤醒”时对机器人的临近周围环境的扫描。可以在机器人静止时执行局部扫描,以使机器人能够在移动之前确定其在3D环境中的位置。
由激光传感器执行的局部扫描包括机器人周围环境的局部点云。局部点云包括用来确定环境结构的空间信息。除了范围信息,激光传感器还可以提供每个点的强度度量,即从表面反射后返回的强度。更准确地说,强度是指激光器发射的功率与返回到激光器的功率之比。尽管在整个公开内容中通常使用术语强度,但是应该理解,诸如反射率的相关参数也应认为落入术语强度的范围内。反射率是通过补偿传感器的固有参数以及诸如距物体的距离、入射角、空气密度等外部特征而根据强度获得的表面的属性。
应该理解的是,如整个说明书所使用的术语“缺定”、或相关术语(例如,“判定”)的范围应理解为包括“获得”、“接收”、“计算”或“得出”必要的数据或信息。因此使用术语“确定”并不意味着需要进行任何处理来判断数据。
在步骤110,该方法包括使用强度数据计算局部点云的第一强度描述符。以这种方式,为整个局部点云计算单个描述符,以降低数据集的维数(这里,局部点云中每个点的强度值可以包含大约50万个点),并且同时保持独特的信息和描述性。第一强度描述符的结构将在下面进一步详细描述。
在步骤120,该方法包括检索多个先前计算的第二强度描述符,多个先前计算的第二强度描述符分别与3D环境的地图的各个部分相关联,如将在下面进一步详细描述的。
应当理解,第一强度描述符和第二强度描述符是编码/描述它们所基于的点云的全局描述符。第一强度描述符是从局部扫描获得的局部点云的全局描述符。每个第二强度描述符是与地图的各个部分相关联的点云的全局描述符。在这方面,并且如将在下面进一步描述的,3D环境的地图可以认为是全局地图(即,全局点云),其被分割成多个较小的点云,多个较小的点云分别对应于地图上的唯一地点。
因此,重要的是要认识到,该方法依赖于3D环境的现有地图(即全局点云),该地图先前已在使用激光传感器进行的地图构建运用中确定,例如在基于SLAM的环境地图构建中。在地图构建后,将地图分割为多个部分,每个部分都有各自的点云,并为地图的每个部分或每个区段确定第二强度描述符。与地图的每个部分相关联的点云也可以称为在该部分的中心具有原点的“地点”。第二强度描述符可以存储在数据存储器中并且由处理设备加载以便定位在地图中。
在步骤130,该方法包括将第一强度描述符与第二强度描述符中的至少一些进行比较。通常,这是简单的线性对应搜索,该搜索在局部扫描的第一强度描述符和从地图提取的所有部分(即,地点)的第二强度描述符之间执行(暴力方法)。虽然可以将地图中所有部分的第二强度描述符用于比较,但是如果达到确定正确地点的置信度,则该算法可以在使用每个第二强度描述符之前终止比较过程。
最终,在步骤140,该方法包括至少部分地根据比较结果来确定相对于地图的位置。比较步骤的输出是选择最可能的地点,该地点对应于与局部扫描具有最高相似度(基于强度数据)的地图的部分。在一些示例中,仅基于强度的识别可以足以实现地图中的定位。但是,应该理解,位置的精度不会比地图分割的分辨率更好。尽管可以基于强度描述符的坐标系之间的旋转差异来估计更准确的位置和方向(即位姿),但是由于云中的噪声会影响坐标系估计的精度,因此精度仍然受到限制。在其他示例中,还可以使用根据基于强度的识别所确定的最可能的位置来执行几何识别。如将在下面进一步详细描述的,可以使用几何识别来确定点云之间的变换,以使得能够准确地确定位姿。此外,几何识别可以用来验证基于强度的识别是正确的。
上述方法提供了许多优点。
首先,该方法能够仅使用来自激光传感器(例如,LiDAR)的数据执行全局定位。该方法不依赖于来自附加传感器(例如,摄像机)的信息,并且可以在缺少外部传感器基础设施(例如,GPS或信标)的非结构化环境中执行。作为示例,该方法可以用于在不能使用GPS的室内环境中或在GPS遭受严重的多径并且变得不可用的工业区域中的导航。该方法还可以在不利的照明条件下和例如在传统摄像机无效的黑暗环境中执行。
由于使用强度描述符来确定局部扫描相对于地图的最可能的位置,因此该方法在计算上是高效的。以这种方式,基于强度的识别能够用于将搜索空间减小到地图的一小部分,这可以称为机器人位置的“先验”估计。然后可以在缩小的搜索空间上执行几何识别以确定准确的位姿,而不是在整个搜索空间上执行几何识别(这是具有巨大计算复杂性的传统几何地点识别算法的情况)。因此,该方法可用作确定地图上的大致位置或显著减少用于几何识别的搜索空间以确定准确位姿的快速而高效的方式。
上述方法对于操作开始时的定位或在迷失轨道或路径的情况下可能特别有用。这些任务通常称为唤醒和迷失机器人问题。类似地,该方法可以用在其他场景中,例如需要确定机器人是否已经返回到先前访问过的地点的环路闭合。有利地,该方法可用于定位固定机器人,而不需要其四处行驶并收集附加数据。出于安全原因,这可能很重要,因为有时在唤醒时,机器人必须沿精确指定的方向移动,否则会损坏环境的某些部分(或自身)。
现在将描述许多其他特征。
在一个示例中,该方法包括,在一个或更多个电子处理设备中,基于用于强度数据在局部点云中的分布的至少一个统计度量来计算第一强度描述符。必须选择统计度量以减少局部点云中强度数据的维数,同时保持足够的唯一信息和描述性,以便与其他强度描述符进行比较。
在这方面,通常,该方法包括,在一个或更多个电子处理设备中,通过将局部点云划分为多个空间分布的局部点云区段来计算第一强度描述符。对于每个局部点云区段,该方法进一步包括:基于用于局部点云区段内的强度数据的分布的统计度量来计算第一强度描述符区段;以及,使用每个第一强度描述符区段计算第一强度描述符。局部点云的结构化划分通过将统计信息与空间位置相关联来确保描述符的唯一性。
应当指出,单个统计描述符可以用于整个局部点云,但是降维可能太大,以至于描述符失去唯一性。
在一个示例中,统计度量指示多个强度值中的每个强度值的频率的分布。以非常紧凑的形式编码点分布的一种方法是使用直方图。因此,每个第一强度描述符区段是局部点云中空间的特定区域中的点的强度直方图,因此整个第一强度描述符是一组直方图,它们一起以紧凑高效的方式唯一地描述了局部点云。
现在将描述第一强度描述符的结构。通常,局部点云区段具有至少部分球形的结构。局部点云区段包括以下中的至少一个:多个同心球形区域;以及,至少两个分割成八分圆的同心球形区域。在多个同心球形区域的情况下,描述符的性能将优于没有空间分割的描述符,但是,它可能无法区分具有相似对象的云,所述相似对象位于离原点相同的距离处,但在不同的侧面。通过将球形区域划分为八分圆(即,将球形划分为半球形,然后应用方位角划分),可以定义在点云中何处是“左”、“右”、“上”和“下”。
在一个示例中,外部球形区域的半径是基于传感器的范围的,并且选择一个或更多个内部球形区域的半径,使得每个球形区域包含相似数量的局部点云点。
类似于以上对第一强度描述符的描述,应当理解,第二强度描述符是使用与对应于3D环境的地图的点云中的点相关联的强度数据来预先计算的,该强度数据至少部分地基于在先前对3D环境执行的扫描中,从点云中的点返回到至少一个激光传感器的辐射功率。此外,每个第二强度描述符包括针对与地图的各个部分相对应的点云的一部分的各个点云区段而获得的第二强度描述符区段。因此,应当理解,第二强度描述符具有与第一强度描述符相同的结构,从而可以容易地比较描述符。
通常,多个第二强度描述符存储在数据存储器中,并且该方法包括,在一个或更多个电子处理设备中,每当执行定位方法时从数据存储器中检索多个第二强度描述符。如前所述的,当3D环境被地图构建以获得地图并且执行离散化以提取从中计算描述符的地图的部分时,更早地计算了第二强度描述符。
通常,该方法包括,在一个或更多个电子处理设备中,通过将第一强度描述符区段与第二强度描述符区段进行比较,将第一强度描述符与第二强度描述符进行比较;以及,根据比较结果确定相似度;以及,根据相似度值选择一个或更多个第二强度描述符。在一个示例中,该方法包括将第一强度描述符的直方图与每个第二强度描述符的相应直方图进行比较。可以使用卡方检验比较各个直方图,卡方检验提供了直方图之间的相似度度量。然后可以确定所有直方图比较的平均值,以提供强度描述符之间的相似度度量。然后可以基于相似度值和需要多大先前的搜索区域来选择一个或更多个最相似的第二强度描述符。
应当理解,可以使用诸如主成分分析(PCA)的技术来估计点云的坐标系。这是进行比较的重要考虑因素,因为有必要确保比较各个点云的相应描述符区段。虽然PCA可以确定正交矢量以形成坐标系,但它不能确定矢量的符号。因此,这个方向的消歧需要仔细考虑。
因此,在一个示例中,该方法包括:在一个或更多个电子处理设备中,针对第一强度描述符和第二强度描述符之间的每个比较,确定第二强度描述符的多个相对方向;以及,对于第二强度描述符的多个相对方向中的每个相对方向,将第一强度描述符区段与第二强度描述符区段进行比较。然后可以针对每个方向确定相似度值,并且可以选择最小相似度值作为相应的第一强度描述符和第二强度描述符之间的相似度的度量。
一旦确定了所有相似度值,该方法包括,在一个或更多个电子处理设备中,使用第一强度描述符和各个第二强度描述符之间的相似度值来将地图的部分排序为潜在位置。以这种方式,基于强度的识别能够根据从地图提取的与局部扫描相对应的地点的相似度来确定排序顺序。
然后可以选择缩小的搜索区域的尺寸(即,机器人在地图中位置的“先验”估计值),因此该方法可以包括,在一个或更多个电子处理设备中,使用基于地图的部分作为潜在位置的排序所选择的至少一个地图的部分来生成子地图,以及,在子地图中执行几何识别。应当理解,可以通过合并与基于相似度排序所选择的地图的部分相关联的点云来形成子地图或“先验”,并且取决于地图如何离散,相邻部分的点云之间可能存在显著重叠。
在一个示例中,该方法包括,在一个或更多个电子处理设备中,通过以下方式执行几何识别:从局部点云中提取局部关键点,为每个局部关键点计算局部几何描述符,检索先前计算的子地图几何描述符,所述子地图几何描述符是利用与子地图中包括的地图的部分相关联的子地图关键点计算的,确定局部几何描述符和子地图几何描述符之间的对应关系,基于几何一致性来聚类对应关系以形成一个或更多个候选簇,以及,确定每个候选簇的转换。该阶段的输出是一组局部扫描的候选地点以及到地图的转换。
应当理解,在3D环境被地图构建并存储在数据存储器中之后,子地图几何描述符与该地图的第二强度描述符一起计算。针对从地图提取的每个部分计算几何描述符,因此,当将地图的部分合并到子地图中时,检索相应的几何描述符以用于执行几何识别。
通常,该方法进一步包括:在一个或更多个电子处理设备中,选择具有最高对应关系数量的候选簇,将选择的候选簇中的对应关系数量与第一阈值进行比较;以及以下项中的至少一个:如果对应关系数量低于第一阈值,则确定出选择的候选簇为错误匹配;以及,如果对应关系数量高于第一阈值,则确定出选择的候选簇指示最可能的位置。因此,对应关系数量是基于必须满足的几何特征的第一个验证条件,否则将地点(地图的部分)作为不正确的匹配项丢弃。
在确定了指示最可能的位置的候选簇之后,该方法然后通常包括,在一个或更多个电子处理设备中,优化变换以将局部点云和与子地图相关联的子地图点云对齐。在一个示例中,这是通过在局部点云和子地图点云的至少一部分上执行迭代最近点(ICP)过程以优化变换并确定位姿来实现的。在优化后,点云被精确对齐。如果ICP成功收敛,则输出估计的变换矩阵。
作为进一步的验证步骤,该方法可以进一步包括:在一个或更多个电子处理设备中,确定适应度得分,该适应度得分指示局部点云和子地图点云的对齐程度,将适应度得分与第二阈值进行比较;以及,根据比较结果选择性地验证位置。如果适应度得分超过可接受的水平(即高于第二阈值),则该位置视为错误匹配。
错误匹配可能有多种原因,例如环境的动态程度(在地图构建过程中看到的对象可能不再可见或已移动)。此外,机器人可能只是远离原始地图构建轨迹,在这种情况下,局部扫描可能看起来与从地图提取的任何部分都不相似。
通常,如果任一验证条件失败,则该方法看起来会增加子地图(即先前的搜索区域)的尺寸。相应地,在一个示例中,该方法包括,在一个或更多个电子处理设备中,如果满足以下项之一,则生成新的子地图:对应关系数量低于第一阈值;以及,适应度得分高于第二阈值。可以至少部分地利用基于地图的部分作为潜在位置的排序所选择的地图的其他部分来生成新的子地图(根据基于强度的识别过程)。
在这方面,应当理解,应当组合在一起以形成子地图或先验的地图的提取部分的数量存在限制。这个的一个原因是,随着子地图的增长,几何描述符的数量也随之增加,因此定位过程变得更慢。由于所述系统因为减少了计算复杂性而具有优势,因此应该将子地图限制为这样一个尺寸,使得所使用的几何描述符的总量不会导致处理时间比传统方法(即,在整个地图中基于几何对应关系搜索)更长。
因此,在一个示例中,该方法包括,在一个或更多个处理设备中:确定子地图中已经包括多少个潜在位置;以及,以下项中的至少一个:如果潜在位置的数量已经达到定义的最大值,则终止定位方法;以及,如果潜在位置的数量低于定义的最大值,则增加子地图的尺寸。
在上述方法中,沿着地图构建轨迹从地图中提取地图的部分(即“地点”)。以此方式,将地图构建轨迹划分为预定长度的区段,并且将地图的每个部分定义为从至少一个传感器沿着地图构建轨迹的相应区段行进时获得的点云。发现这种分割地图的方法比其他方法(例如,基于半径的方式)更具有鲁棒性。这种方式基于选择地图中的位置并基于传感器范围提取点云。但是,这是不正确的,因为提取的扫描将包含从机器人位置看不到的点,这是因为它们被更靠近传感器的物体所遮挡。因此,最好沿轨迹提取地点,因为这样可以确保提取的地点与局部扫描相对应,因为它们仅包含机器人实际上可以从该位置看到的特征。
在进一步的广义形式中,本发明提供了一种用于在三维(3D)环境中执行定位的系统,所述系统包括一个或更多个电子处理设备,所述一个或更多个电子处理设备配置为:至少部分地基于从局部扫描获得的局部点云中的点返回到至少一个激光传感器的辐射功率,根据由至少一个激光传感器执行的局部扫描确定强度数据;使用强度数据计算局部点云的第一强度描述符;检索多个先前计算的第二强度描述符,多个先前计算的第二强度描述符分别与3D环境的地图的各个部分相关联;将第一强度描述符与第二强度描述符中的至少一些进行比较;以及,至少部分地根据比较结果来确定相对于地图的位置。
通常,所述系统包括至少一个激光传感器,所述至少一个激光传感器配置为执行局部扫描。在一个示例中,所述至少一个激光传感器是光成像、检测和测距(LiDAR)传感器。有利地,不需要额外的传感器即可实现完整的全局定位。
在一个示例中,所述系统进一步包括包含多个第二强度描述符的数据存储器,并且其中一个或更多个电子处理设备可访问该数据存储器以检索多个第二强度描述符。
优选地,所述至少一个激光传感器、一个或更多个电子处理设备和数据存储器位于移动机器人或自动驾驶车辆上。然而,在其他布置中,处理设备可以与远程数据存储器无线通信,以便检索所需的第二强度描述符。
现在将参考图2描述用于在3D环境中执行定位的系统的示例。
在该示例中,系统200包括位于自动驾驶车辆或机器人205上的至少一个电子处理设备210。电子处理设备210连接到数据存储器230,数据存储器230包含地图信息,诸如与地图相关联的全局点云、地图构建轨迹、第二强度描述符和几何描述符。激光传感器220安装在机器人205上,并且配置为对机器人205周围的环境进行扫描。在一个示例中,激光传感器是3D LiDAR传感器,例如由威力登(Velodyne)生产的VLP-16 3D LiDAR。为了增加从传感器获得的信息,传感器可以安装为绕竖直轴线旋转,此外,传感器可以与竖直方向成45度角倾斜。在一个示例中,局部扫描认为是由上述传感器的两次旋转生成的点云。
电子处理设备210的示例在图3中示出。在该示例中,处理设备210包括经由总线304互连的至少一个微处理器300、存储器301、可选的输入/输出设备302(例如,键盘和/或显示器)以及外部接口303,如图所示。在该示例中,外部接口303可以用于将处理设备210连接到外围设备,诸如通信网络、数据存储器230、其他存储设备等。尽管示出了单个外部接口303,但这仅出于示例的目的,并且实际上可以提供使用各种方法(例如,以太网、串行、USB、无线等)的多个接口。
在使用中,微处理器300执行以应用软件的形式存储在存储器301中的指令,以使得执行所需的处理,包括与数据存储器230进行通信以检索与提取的地图的部分相关联的第二强度描述符和几何描述符,从激光传感器220接收包括强度数据和范围数据的传感器数据,并执行基于强度的识别和几何识别。应用软件可以包括一个或更多个软件模块,并且可以在诸如操作系统环境等的合适的执行环境中执行。
相应地,将理解的是,电子处理设备210可以由任何适当的处理系统形成,诸如安装在机器人205上的适当编程的计算机系统,PC,膝上型计算机或手持式PC,诸如智能电话、平板电脑等。在一个具体示例中,电子处理设备210是标准处理系统,例如基于英特尔架构的处理系统,其执行存储在非易失性(例如,硬盘)存储器上的软件应用,尽管这不是必需的。然而,还将理解的是,处理系统可以是任何电子处理设备,例如微处理器、微芯片处理器、逻辑门配置、与诸如FPGA(现场可编程门阵列)的实现逻辑可选地相关联的固件,或任何其他电子设备、系统或布置。
现在将参考图4A至图4C描述用于在3D环境中执行定位的具体过程。此流程图中描述的过程描述了一种优选的两阶段定位流水线,包括第一阶段和第二阶段,第一阶段包括基于强度的地点识别,第二阶段包括几何地点识别以验证基于强度的匹配的正确性并提供所需的变换和位姿。
该过程开始于当使用3D LiDAR传感器执行环境的局部扫描时的步骤400。局部扫描可以是“唤醒”扫描,“唤醒”扫描是机器人在唤醒时或在没有关于其位置的先验信息的环境中初始化时执行的扫描。局部扫描会生成包括传感器可以看到的所有点的局部3D点云。
在步骤405,确定对应于局部点云中所有点的强度的强度数据。如前所讨论的,该信息对应于取决于被扫描表面的反射率特性的激光返回强度的度量。这样,强度数据高度描述了机器人所处的环境。在一个示例中,简单地从来自传感器的消息中读取强度数据。
在步骤410,使用局部点云中的点的强度数据来计算第一强度描述符。该强度描述符包括一个球体,该球体在径向上分为同心球,然后在水平方向上分成半球,然后再沿方位角方向每90度进行分段。生成的描述符具有16个空间分布的区段。然后计算落入16个区段中的每个区段中的点的强度的直方图,使得第一强度描述符包括16个强度的直方图。直方图的使用降低了数据集的维数,而描述符的结构化划分通过将统计信息与空间位置相关联来保持唯一性。强度描述符的结构的示例在图6A中示出,并且示例性的唤醒扫描划分为在图6B中示出的空间分布的区段。
在步骤415,计算局部几何描述符。为了实现这一点,首先对点云进行降采样以降低云的密度,然后将异常值去除,以最小化噪声对描述符的影响。然后根据随机选择从点云中提取关键点,并形成围绕其计算几何描述符的点。然后计算云中所有点的法线,最后计算出描述符。合适的几何描述符称为SHOT描述符,它表示Signature of Histograms ofOrientations(方向直方图的特征),并且具有较强的抗噪性,同时具有很高的描述性。SHOT描述符具有球形支撑结构,该结构分为32个空间分布的区段。对于每个区段,计算法线直方图,以提供鲁棒性,此外,每个区段的空间位置是固定的,从而确保了描述性。然而,应当理解,SHO描述符是可以使用的局部描述符的示例性示例,但是可以使用任何其他局部描述符,如本领域技术人员将理解的那样。
在步骤420,将可能的位置候选的数量设置为“n”。最初,“n”等于1,这意味着使用最相似的基于强度的地点候选形成执行几何识别的“先验”或子地图。
在步骤425,从数据存储器中检索多个第二强度描述符并将其加载到处理器的存储器中。
在步骤430,将第一强度描述符与第二强度描述符进行比较。这是通过将第一强度描述符的每个区段的强度直方图与第二强度描述符的每个对应区段的强度直方图进行比较来实现的。使用卡方检验比较各个直方图:
其中:
描述符A=第一强度描述符;描述符B=第二强度描述符;k=直方图的组(bin)数;m=直方图中的组个数;A(k)=描述符A的第k个组的值;以及,B(k)=描述符B的第k个组的值。
比较的输出是相似度值
获得两个描述符的相似度度量S
AB,作为所有区段的卡方检验结果的平均值。图7显示了不同地点的两个强度描述符的直方图之间的比较的直观图示。第一个地点“2”对应于密集的云,表示具有建筑物的结构化环境。第二个地点“917”对应于稀疏的云,表示树木和灌木丛的非结构化环境。
还应注意,通常使用主成分分析(PCA)为强度描述符计算坐标系。由于不可能确定包括坐标系的矢量的符号,因此必须考虑这一点,以确保将描述符的相应区段的直方图彼此比较。因此,将第一强度描述符与每个第二强度描述符的四个不同版本进行比较。通过不同的版本,应该理解区段的不同的序列,这些序列是通过考虑描述符的坐标系的不同方向而获得的,如图8所示。所述方法基本上假定坐标系定向的所有四个组合都是可能的。为四个比较中的每个比较确定一个相似度值SAB,并且描述符之间的相似度的总体度量定义为:
SAB=min{1SAB,2SAB,3SAB,4SAB}
在步骤435,根据比较结果(即,基于描述符之间的相似度度量),选择“n”个最相似的位置候选。在步骤440,通过组合各个地点的点云,将位置候选合并为子地图或“先验”。
在步骤440,从数据存储器中检索子地图几何描述符,所述子地图几何描述符基于为“先验”中的地图的每个部分确定的几何描述符。
在步骤450,确定几何对应关系。针对从局部扫描获得的每个局部几何描述符,查询每个子地图几何描述符。
对于子地图中的每个关键点,只能从局部扫描中选择一个关键点(最相似)。还有一个附加的约束条件:相似度水平必须低于定义的阈值。kd树用作搜索方法,其维数符合描述符维数的数量。
输出是单个最近邻值,如果相似度低于上述阈值,则将其视为对应点。对应关系被收集到一个向量中,该向量存储关键点对的系数(indice)。通常,局部扫描中的单个关键点可以在子地图中具有多个对应的关键点,其中包括多个实例的可能性。因此,相似度阈值很重要,因为它的值会影响识别的严格性。通过增加相似度阈值,系统对场景的变化变得不那么保守,而对噪点更具有鲁棒性,另一方面,太大的值将导致错误的匹配。平衡参数必须通过实验找到。
在步骤455,基于几何一致性将对应关系聚类。对应关系搜索步骤的结果是子地图中的点与局部扫描中的点之间的一组匹配。为了确保找到的对应关系属于局部扫描的实际实例,对它们施加了几何约束。基本假设是云中的点是刚性连接的。这允许将关键点聚类为满足几何一致性标准的集合。在这种情况下,如果子地图中的两个点的差的2范数与局部扫描中的对应点的差的2范数之间的差小于定义的共识阈值,则认为子地图中的两个点在几何上是一致的。几何一致性共识是系统的另一个重要参数。根据经验,它必须大于降采样后云的分辨率(点位置的差异可以达到分辨率)。但是,由于不能保证关键点在两个云中都处于完全相同的位置(由于通过降采样选择),并且由于噪声的缘故,共识值应该更大(数个云分辨率)。
此外,在最小对应关系数量上有一个条件,在该最小对应关系数量以下,簇被丢弃。之后,使用RANSAC算法进一步消除不一致的对应关系。在将以上约束施加于对应关系之后,然后在步骤460确定一组候选簇。
在步骤465,对于每个候选簇,使用绝对定向算法来计算变换矩阵以获得位姿。重要的是,要注意,此变换是根据关键点集的相对位置而不是整个点云来估计的,其准确性取决于簇中关键点的数量。该阶段的输出是一组局部扫描的位置候选,以及到子地图的转换。
在步骤470,将具有最高数量的对应关系的候选簇选择为最可能的位置候选。
在步骤475,以两步过程执行验证。首先,将最可能的位置候选中的对应关系数量与定义的阈值进行比较。对应关系数量必须大于阈值,否则将位置候选作为错误匹配丢弃。
然后,使用迭代最近点(ICP)算法优化所获得的最可能位置的变换矩阵。ICP应用于子地图和局部扫描中具有最高对应关系数量的簇的每个点。优化后,点云将精确对齐。如果ICP成功收敛,则输出估计的变换矩阵。
此外,ICP输出适应度得分,即点云对齐程度的度量。这形成了第二验证条件的一部分,该第二验证条件检查适应度得分是否超过可接受的水平(超过定义的阈值)。如果是,则将该地点视为错误匹配。
在步骤480,该方法检查是否满足上述验证条件。如果满足上述验证条件,则认为该机器人已定位并且该方法在步骤485处终止。如果不满足一个或两个验证条件,则在步骤490,该方法检查是否已经达到先验的最大的地点候选数量“n”。如果是这样,则不执行进一步的搜索,并且算法在步骤496终止,认为不可能找到该位置。否则,在步骤495增加先验的尺寸,并通过考虑其他地点候选来扩展子地图,并且该方法返回到步骤435。
现在将参考图5描述计算与局部扫描相关联的第一强度描述符的示例的流程图。
在该示例中,在步骤500,根据3D LiDAR扫描来确定局部点云中的点的强度数据。
在步骤510,例如,如图6A所示,将局部点云划分为多个空间分布的局部点云区段。然后在步骤520,计算每个点云区段中的点的强度分布的统计度量。在优选的实施方式中,在步骤530,为形成第一强度描述符区段的每个点云区段确定强度的直方图。在其他示例中,可以使用替代统计度量,例如,点云区段中的点的强度值的平均值或强度值的方差。
最后,在步骤540,使用第一强度描述符区段来定义第一强度描述符。当使用直方图时,第一强度描述符定义了一组强度直方图,这些直方图以紧凑而独特的形式表征了局部扫描的外观。
现在将参考图9描述执行地图的预处理的方法的示例的流程图。
在该示例中,在步骤900,通过在3D LiDAR传感器扫描的同时在3D环境中驾驶自动驾驶车辆或驱动机器人来获得地图。SLAM算法可用于获取环境的地图。
还记录了地图构建轨迹,并且在步骤910,该方法包括获得地图构建轨迹。在步骤920,沿着地图构建轨迹提取地图的部分。所述提取基于距离,相应地,通过将轨迹分段为长度为“l”的块(例如,l=2m),并根据沿每个块可见的所有点创建点云(云的原点位于中间),将地图进行划分。
在步骤930,针对每个提取部分的云中的点确定强度数据(如前所述,也称为“地点”)。
在步骤940,使用与先前为第一强度描述符所描述的相同技术,为每个提取的部分计算第二强度描述符。在步骤950,还使用先前描述的用于局部几何描述符的技术(例如,降采样、异常值去除、关键点选择、法线计算等)为每个提取的部分计算几何描述符。最终,在步骤960,将用于从地图提取的每个部分的第二强度描述符存储在数据存储器中,以供电子处理设备随后进行检索以用于定位。
实验与验证
在澳大利亚布里斯班的昆士兰高级技术中心现场进行了实验,以验证所述系统。图10A中显示了带有标明唤醒位置的测试环境的卫星照片。使用SLAM算法对整个区域进行地图构建,该算法提供了l地图(即,全局点云)和相应的地图构建轨迹,如图10B所示。
测试环境具有多个特征。存在完全结构化本质的地点(例如,建筑物)以及完全非结构化区域(由树木或灌木丛组成)。显然,存在包含两种类型的元素的区域,这种区域是结构化和非结构化环境的组合。此外,在整个地图中,不同地点的点的空间分布也有很大变化。建筑物之间的点通常非常受限且紧密,这可以防止在其中获得的扫描包含大量的点,但是这些点通常非常独特且易于区分。另一方面,也存在开放空间位置,只有零星的树木和灌木丛,这些树木和灌木丛缺乏明显的区别性特点,但经常包含大量点(以从地面反射所测量的)。所有这些使该地点成为非常难以识别的环境,尤其是要求仅基于LiDAR数据的情况。
为了验证系统,生成了各种唤醒扫描(即局部扫描)的集合。整个地点总共测试了101个位置,所有这些都可以在图10A中看到。可以注意到,这些点在地图上相当均匀地分布,从而保留了地点特征的多样性,并确保了结果的统计意义。而且,扫描是在距原始地图构建轨迹不同距离处生成的,以验证对变化的鲁棒性。同样重要的是要提到,整个集合是在多天内生成的。这意味着存在多种光照条件和温度,可以验证它们对基于强度的描述符的影响。
根据先前描述的方法,将地图分为一组地点。轨迹长约4公里,因此,在连续云的分辨率设置为2m的情况下,地图被划分为2055个地点。
所述系统的验证基于在多特征环境中建立识别的成功率。在提出的系统中,如果算法比经典基于几何的方法能够更快地找到正确的位姿,则认为识别成功。在以下部分中,仅基于几何的流水线将被称为经典方法,而基于强度和几何的两阶段识别将被称为组合流水线。更具体地说,如果满足如先前所定义的正确性(即,验证)条件,则认为该地点是正确的匹配。通过执行各种实验来调整此标准的参数。
对于某些扫描,通过将找到的位姿解析到导航流水线来直接评估正确性。如果该过程可以开始(正确对齐),则表示已成功找到该位姿。但是,在大多数情况下,全局定位测试是与导航分开进行的,因为两个流水线同时开发,并且通常无法执行相互测试。
在这种情况下,首先使用经典方法(即在地图中定位为单个云)进行识别,然后将其结果视为基本事实。如果组合流水线找到了相同坐标的地点(可允许的标准误差小于1m),则认为该地点正确。
对于某些扫描(整个集合总共进行了八次扫描),在包括单个云的地图中基于几何的定位失败。在这种情况下,经典方法的参数发生了变化(即,提高了降采样的分辨率以获得更多的描述符)。对于五个地点,更改的参数没有作用,仅基于几何的方法失败。在这些情况下,通过目视检查地图中找到的地点的位置和点云的对齐方式来评估组合方法的正确性。
为了确保组合流水线的性能比经典方法快,形成先验的地点的最大数量限制为49。这个数字也是通过实验确定的,以使处理时间短于经典方法的50%。但是,如实验所示,将先验扩展到25个地点以上并不会使得增加找到正确位姿的可能性。
总体而言,所述系统非常成功,总体成功率达到97%。如此之高的率源于鲁棒的算法结构,即在基于几何的验证不能确定识别的情况下,增加先验(即,地点候选的数目)的可能性。图10C中所示的示意图可视化了先验尺寸的增加对整体定位成功率的影响。在这个示意图中,下一个先验尺寸的成功率包括来自先前先验尺寸的成功率和通过增加先验尺寸而识别出的其他地点。绘制所有实验的结果(即,101个唤醒位置)。
在大多数情况下(82%),系统能够仅使用一个位置正确定位。从该图可以清楚地看到,将先验尺寸增加到5和25个位置可以提高成功率,但是,先验尺寸的进一步增加示出未提供任何更好的识别度改进(而不可避免地导致处理时间的恶化)。
在许多全局定位算法中,基本假设是机器人“重新访问该地点”(即,其当前可以看到的地点之前已经被访问过)。这意味着识别能力仅限于轨迹的周边。所提出的算法是通用的,并且可以在合理范围内操纵机器人到轨迹的任意距离处的位置。所述系统可以应对距离轨迹多达5米的机器人,这意味着它可以位于围绕地图构建路径10m宽半径内,并且仍然可以鲁棒地定位。此外,对于更远的距离,成功率保持在令人满意的70%的水平。这是一个极好的结果,证明了系统的鲁棒性。
参照图10D,示出了距轨迹不同距离的定位成功率的曲线图。条形图描绘了在不同距离范围内的唤醒位置相对于地图构建轨迹的比例。在图10E中,示出了成功定位在距地图构建轨迹的不同距离处所需的地点候选数量(先验尺寸的分布)的示意图。
正如人们所期望的那样,随着距轨迹的距离增加,基于强度的识别的确定性降低。这是由于机器人可以看到原始轨迹中不可见的对象而引起的。此外,远离地图构建路径,可以从不同的入射角度看到相同的对象,并且由于没有对其进行补偿,因此在强度直方图中引入了不确定性。很明显,当机器人离轨迹更远时,需要考虑更大的先验。原因之一是,从轨迹外的位置完全可见的对象只能从轨迹上的各个地点部分可见。因此,通过组合局部视图,可以创建包含足够信息的云,以执行成功的基于几何的定位。
此外,可以注意到,对于少数接近轨迹的扫描情况,也需要增加先验尺寸。这是由于环境是动态的,并且在唤醒时可能会出现某些对象,而这些对象在地图构建期间不可见。这导致基于强度的识别确定性降低。另一方面,这是对先验尺寸扩展功能的需求的另一种证明,这使算法能够应对环境中的动态变化。
开发系统的最重要动机之一就是要求减少处理时间。在经典方法中,当在整个全局地图点云中搜索对应关系时,该过程将花费不可接受的时间量。根据云的特征(在建筑物情况下结构化和紧凑的,或者在开放空间/灌木丛情况下非结构化和广阔的),搜索可能花费3到20分钟。对于必须执行某些任务的多用途车,不能容忍等待这么长的时间才开始运行。
所提出的算法完美地适应了时间要求。使用基于强度的先验,搜索空间的限制是巨大的(例如,如图10G所示)。使用经典方法和组合方法的处理时间的比较如图10F所示。显然要注意的,在大多数情况下,时间减少达到了约99%(纵轴为对数)。仅在包括25个地点的先验的情况下,减少的情况差一些,但仍达到90%左右。结果非常令人满意,并证明,在大多数情况下,该解决方案可以视为实时全局定位算法。
因此,将理解的是,在至少一个示例中,上述方法和系统提供了一种用于执行全局定位的高效算法,并且在一些示例中,提供了对于唤醒或迷失机器人问题(机器人不得不在没有任何先验知识的情况下,在已知地图中估计其位姿)的高效解决方案。从前述内容将理解的是,所述系统可以在需要全局定位的其他应用中使用,例如环路闭合等。所述系统能够仅使用从机载3D LiDAR传感器获得的数据执行全局定位,而无需依赖任何外部信标、GPS、IMU数据等。本文呈现的两阶段定位流水线使用一种新颖的基于强度的描述符来确定机器人位置的先验估计值,然后进行基于几何的验证,执行所述基于几何的验证以确定位姿和基于强度的匹配的正确性。整个过程可以由完全静止的机器人执行,并且不需要位置估计的运动。实验数据表明,该方法和系统对环境变化具有鲁棒性,并表明所述系统适用于现实世界中的自主导航。
在整个本说明书和随后的权利要求中,除非上下文另有要求,否则词语“包括”以及诸如“包括有”或“包含”的变体将被理解为暗示包括所述整数或一组整数或步骤,但是不排除任何其他整数或整数组。
本领域技术人员将理解的是,许多变化和修改将变得显而易见。对于本领域技术人员而言显而易见的所有这些变化和修改,都应认为落入本发明在之前广泛描述的精神和范围内。