CN116022163A - 基于超局部子图的自动驾驶车辆扫描匹配和雷达姿态估计器 - Google Patents

Abstract

一种用于确定自动驾驶车辆的最终雷达姿态的扫描匹配和雷达姿态估计器，包括自动驾驶控制器，其被指示来基于预定数量的连续聚合的过滤后数据点云扫描及相关联的姿态估计确定超局部子图。自动驾驶控制器通过基于迭代最近点(ICP)对齐算法将最新聚合的过滤后数据点云扫描与最近超局部子图对齐的方式，来确定初始估计姿态。自动驾驶控制器基于最近超局部子图和邻近雷达点云扫描确定姿态图，并且执行多视图非线性ICP算法来以移动窗口方式调整与邻近雷达点云扫描相对应的初始估计姿态，以确定局部调整姿态。

Description

基于超局部子图的自动驾驶车辆扫描匹配和雷达姿态估计器

技术领域

本公开涉及一种-基于超局部子图用于自动驾驶车辆的扫描匹配和雷达姿态估计器的系统和方法，其中，超本地子图基于预定数量的连续聚合过滤数据点云扫描及其最新的相关姿势估计。

背景技术

自动驾驶车辆可以使用各种车载技术和传感器，在有限的人为干预或没有人为干预的情况下，从起始点驾驶至预定目的地。自动驾驶车辆包括各种自主传感器，诸如，但不限于，摄像机、雷达、激光雷达、全球定位系统(GPS)和用于探测车辆外部环境和状况的惯性测量单元(IMU)。然而，当自动驾驶车辆进行维修时，或驾驶时发生意外、或经过明显路面凹坑或障碍时，如果摄像机或雷达从其支架上移开，则摄像机或雷达传感器需要重新校准，这是一个手动且通常很麻烦的过程。此外，如果自动驾驶车辆经历车轮定位，则摄像机和雷达也需要重新校准。这是因为车辆的车轮确定行驶方向，这影响了摄像机和雷达瞄准。

毫米波(mmWave)雷达是一种自动驾驶车辆可以使用的特定技术。例如，毫米波雷达可以用于预警前方碰撞和后方碰撞，以执行自适应式巡航控制和自动泊车，以及在街道和公路上进行自动驾驶。应当认识到，相对于其他传感器系统，毫米波雷达的优势在于它可以在大多数天气和光暗条件下工作。毫米波雷达可以测量移动物体的距离、角度和多普勒(径向速度)。可以根据基于各种聚类和跟踪算法的毫米波雷达采集的数据，确定雷达点云(可以用于确定物体的位置、速度和轨迹)。然而，基于毫米波雷达采集的数据的雷达点云，尤其是低成本的基于信号片上系统(SoC)的毫米波雷达，可能会过于嘈杂和稀疏，而不能用于动态校准目的需要的鲁棒和准确的姿态估计。

因此，虽然当前用于自动驾驶车辆的雷达姿态估计方法达到了其预期目的，但是在本领域内仍存需要一种改进车辆雷达姿态估计的方法。

发明内容

根据几个方面，公开了一种用于确定自动驾驶车辆最终雷达姿态的扫描匹配和雷达姿态估计器。扫描匹配和雷达姿态估计器包括自动驾驶控制器，其被指示来基于预定数量的连续聚合的过滤后数据点云扫描及相关联的姿态估计确定超局部子图。预定数量的连续聚合的过滤后数据点云扫描是基于由安装在自动驾驶车辆上的单独雷达传感器采集的数据来确定的聚合的过滤后数据点云扫描的。自动驾驶控制器被指示来基于迭代最近点(ICP)对齐算法将最新聚合的过滤后数据点云扫描与最近超局部子图对齐，以确定初始估计姿态。自动驾驶控制器被指示来基于最近超局部子图和邻近雷达点云扫描确定姿态图。自动驾驶控制器被指示来执行多视图非线性ICP算法，以移动窗口方式调整与邻近雷达点云扫描相对应的初始估计姿态，以确定局部调整姿态。局部调整姿态是最终雷达姿态。

在一个方面中，自动驾驶控制器执行指示，以执行环路检测算法，来检测到自动驾驶车辆当前位于先前访问的位置，其中，当自动驾驶车辆当前正在先前访问的位置上时，检测环路。

在另一个方面中，响应于检测环路，自动驾驶控制器被指示来执行非线性优化例程，以对环路闭合姿态图进行全局姿态调整，来确定环路调整的雷达姿态，并且将环路调整的雷达姿态设置为最终雷达姿态。

在又一方面中，最近超局部子图是固定约束，并且邻近雷达点云扫描相对于彼此以及相对于最近超局部子图进行调整。

在一个方面中，在每个邻近雷达点云扫描之间确定边缘约束。

在另一个方面中，在邻近雷达点云扫描中的每一个与最近超局部子图之间确定边缘约束。

在又一方面中，自动驾驶控制器通过调整ICP对齐算法的最大距离阈值参数来确定初始估计姿态，以确定聚合的过滤后数据点云扫描与位于最近超局部子图上的点之间的对应关系。

在一个方面中，预定数量的聚合的过滤后数据点云扫描和相关联的姿态估计取决于单个雷达传感器的采样速率。

在另一个方面中，使用初始估计姿态来替代相关联的姿态估计以确定超局部子图，相关联的姿态估计是基于预定数量的聚合的过滤后数据点云扫描的。

在又一方面中，使用局部调整姿态来替代相关联的姿态估计以确定超局部子图，相关联的姿态估计是基于预定数量的聚合的过滤后数据点云扫描的。

在一个方面中，自动驾驶控制器被指示通过确定表示聚合的过滤后数据点云扫描相关联的最后两个连续姿态估计之间的相对运动的相对变换，基于与聚合的过滤后数据点云扫描相关联的最后两个连续姿态估计之间的时间差将最后两个连续姿态估计之间的相对移动的相对变换转换为速度，以及基于最后一个姿态估计与当前姿态估计之间的时间差将相对变换从速度转换为位置，来确定预测姿态。最后一个姿态估计与当前姿态估计之间的位置上的区别就是预测姿态。

在一个方面中，提供了一种用于确定自动驾驶车辆的最终雷达姿态的方法。方法包括基于预定数量的连续聚合的过滤后数据点云扫描及相关联的姿态估计来确定超局部子图。预定数量的连续聚合的过滤后数据点云扫描是基于一个聚合的过滤后数据点云扫描的，并且基于由安装在自动驾驶车辆上的单独雷达传感器采集的数据来确定该聚合的过滤后数据点云扫描。方法包括通过基于ICP对齐算法将最新聚合的过滤后数据点云扫描与最近超局部子图对齐的方式，来确定初始估计姿态。方法还包括基于最近超局部子图和邻近雷达点云扫描确定姿态图。最后，方法还包括执行多视图非线性ICP算法，以移动窗口方式调整与邻近雷达点云扫描相对应的初始估计姿态，以确定局部调整姿态，其中，局部调整姿态是最终雷达姿态。

在另一个方面中，方法包括执行环路检测算法，以检测到自动驾驶车辆当前位于先前访问的位置，其中，当自动驾驶车辆当前正在先前访问的位置上时，检测环路。

在又一方面中，响应于检测环路，方法包括执行非线性优化例程，以对环路闭合姿态图进行全局姿态调整，来确定环路调整的雷达姿态，并且将环路调整的雷达姿态设置为最终雷达姿态。

在一个方面中，方法包括确定每个邻近雷达点扫描之间的边缘约束。

在另一个方面中，方法包括在邻近雷达点云扫描中的每一个与最近超局部子图之间确定边缘约束。

在又一方面中，方法包括通过调整ICP对齐算法的最大距离阈值参数来确定初始估计姿态，以确定聚合的过滤后数据点云扫描与位于最近超局部子图上的点之间的对应关系。

在一个方面中，方法包括使用初始估计姿态来替代相关联的姿态估计以确定超局部子图，相关联的姿态估计是基于预定数量的聚合的过滤后数据点云扫描。

在一个方面中，方法包括使用初始估计姿态来替代相关联的姿态估计以确定超局部子图，相关联的姿态估计是基于预定数量的聚合的过滤后数据点云扫描的。

在另一个方面中，公开了一种用于确定自动驾驶车辆最终雷达姿态的扫描匹配和雷达姿态估计器。扫描匹配和雷达姿态估计器包括自动驾驶控制器，其被指示来基于预定数量的连续聚合的过滤后数据点云扫描及相关联的姿态估计确定超局部子图。预定数量的连续聚合的过滤后数据点云扫描是基于由安装在自动驾驶车辆上的单个雷达传感器采集的数据的。自动驾驶控制器被指示通过基于ICP对齐算法将最新聚合的过滤后数据点云扫描与最近超局部子图对齐的方式，来确定初始估计姿态。自动驾驶控制器被指示来基于最近超局部子图和邻近雷达点云扫描确定姿态图。自动驾驶控制器被指示来执行多视图非线性ICP算法，以移动窗口方式调整与邻近雷达点云扫描相对应的初始估计姿态，来确定局部调整姿态。在一个方面中，自动驾驶控制器被指示来执行环路检测算法，以检测到自动驾驶车辆当前位于先前访问的位置，其中，当自动驾驶车辆当前正在先前访问的位置上时，检测环路。响应于检测环路，自动驾驶控制器执行非线性优化例程，以对环路闭合姿态图进行全局姿态调整，来确定环路调整的雷达姿态。自动驾驶控制器被指示将环路调整的雷达姿态设置为最终雷达姿态。

从下文提供的详细描述中，其它适用范围将变得显而易见。应当理解，描述和具体实施例仅仅是为了说明的目的，而不是为了限制本公开的范围。

附图说明

本文所述附图仅仅是为了说明的目的，而不是为了以任何形式限制本公开的范围。

图1是根据示例性实施例的包括多个雷达传感器和自动驾驶控制器的自动驾驶车辆的示意图，其中，自动驾驶控制器包括用于确定校准坐标的姿态估计管道；

图2是根据示例性实施例的示出作为图1所示姿态估计管道的一部分的扫描匹配和雷达姿态估计器模块的框图；

图3是根据示例性实施例的包括固定超局部子图和多个邻近雷达点扫描的姿态图；

图4是根据示例性实施例的示出用于用图2中所示扫描匹配和雷达姿态估计器模块46确定最终雷达姿态的方法的流程图。

具体实施方式

以下描述本质上仅仅是示例性的，并不意图限制本公开、应用或用途。

参考图1，示出了示例性自动驾驶车辆10。自动驾驶车辆10具有自主驾驶系统12，其包括与多个车载自主传感器22和多个车辆系统24电子通信的自动驾驶控制器20。在如图1所示示例中，多个车载自主传感器22包括一个或多个雷达传感器30、一个或多个摄像机32、惯性测量单元(IMU)34、全球定位系统(GPS)36和激光雷达38；然而，应当认识到，也可以使用额外的传感器。多个雷达传感器30可以被安装在自动驾驶车辆10的前部14、后部16和/或侧部18，以检测自动驾驶车辆10周围环境中的物体。每个雷达传感器30对自动驾驶车辆10周围环境进行多次单独扫描，以获得包括多个检测点的雷达点云扫描形式的数据。

自动驾驶控制器20包括姿态估计管道40，包括扫描聚合器和过滤器42、惯性导航系统(INS)模块44、扫描匹配和雷达姿态估计器模块46和校准模块48。如下所解释的，扫描聚合器和过滤器42基于自动驾驶车辆10周围环境的单独雷达传感器30采集到的数据来确定聚合的过滤后数据点云扫描50。聚合的过滤后数据点云扫描50发送至扫描匹配和雷达姿态估计器模块46。与聚合的过滤后数据点云扫描50相关联的扫描时间戳被发送至INS模块44。INS模块44确定发送至校准模块48的IMU姿态52，并且扫描匹配和雷达姿态估计器模块46确定发送至校准模块48的最终雷达姿态54。校准模块48确定包括x、y和z坐标以及自动驾驶车辆10的滚动φ、俯仰θ和偏摇Ψ的六自由度(6DoF)变量56。在一个实施例中，六自由度(6DoF)变量56以雷达传输至被采用用以将雷达传感器30与自动驾驶车辆10的重心G进行自动对齐的车辆校准参数。然而，应当认识到，公开的扫描聚合器和过滤器42以及扫描匹配和雷达姿态估计器模块46不限于将6DoF变量以雷达传输至车辆校准参数，并且也可以用于其他应用。例如，在另一实施例中，扫描聚合器和过滤器42和扫描匹配和雷达姿态估计器模块46可以用于基于三维雷达的同时定位与建图(SLAM)应用程序。

应当认识到，由雷达传感器30获得的雷达点云可以是稀疏的，并且在一些实例中包括噪声和抖动数据、鬼影检测、反射和杂波。如下所解释的，尽管雷达点云是稀疏的，但是公开的扫描匹配和雷达姿态估计器模块46采用了超局部子图，用于来确定精确度和鲁棒性提高的最终雷达姿态54。具体地，如下所解释的，超局部子图被采用用于迭代最近点(ICP)扫描匹配、多视图非线性ICP调整，以及用于姿态图环路闭合，这降低了稀疏的雷达点云的影响。

自动驾驶车辆10可以是任何类型的车辆，诸如，但不限于，轿车、卡车、运动型多用途车、小型货车或房车。在一个非限制性实施例中，自动驾驶车辆10是完全自动驾驶车辆，包括执行全部驾驶任务的自动驾驶系统(ADS)。可替代地，在另一实施例中，自动驾驶车辆10是半自动驾驶车辆，包括用于以转向、制动和/或加速来辅助驾驶人的高级驾驶辅助系统(ADAS)。自动驾驶控制器20确定自动驾驶特征，诸如，自动驾驶车辆10的感知、计划、定位、建图和控制。虽然图1示出了作为单个控制器的自动驾驶控制器20，但是应当认识到，也可以包括多个控制器。多个车辆系统24包括，但不限于，制动系统70、转向系统72、动力系统74和悬架系统76。自动驾驶控制器20将车辆控制命令发送至多个车辆系统24，以引导自动驾驶车辆10。

雷达传感器30可以是用于探测距离自动驾驶车辆10约1米至约20米的物体的近程雷达、用于探测距离自动驾驶车辆10约1米至约60米的物体的中程雷达、或者用于探测距离自动驾驶车辆10高达至约260米的物体的远程雷达。在一个实施例中，雷达传感器30中的一个或多个包括毫米波(mmWave)雷达传感器，并且在特定低成本的基于信号片上系统(SoC)的毫米波雷达中具有有限视场。在另一实施例中，雷达传感器30包括一个或多个360度旋转的雷达传感器。

现参考图2，示出了示出扫描匹配和雷达姿态估计器模块46的框图，其中，扫描匹配和雷达姿态估计器模块46包括迭代最近点(ICP)扫描匹配子模块60、多视图非线性ICP调整子模块62、子图子模块64、姿态图环路闭合子模块66和一个或多个空间数据库68。来自扫描聚合器和过滤器42(在图1中可见)聚合的过滤后数据点云扫描50被发送至扫描匹配和雷达姿态估计器模块46的子模块60、62、64和66中的每一个。ICP扫描匹配子模块60确定被发送至剩余的子模块62、64和66的最终估计姿态80。多视图非线性ICP调整子模块62确定被发送至剩余的子模块60、64和66的局部调整姿态82。子图子模块64确定被发送至剩余的子模块60、62和66的超局部子图84。最后，如果适用的话，姿态图环路闭合子模块66确定被发送至剩余的子模块60、62和64的环路调整的雷达姿态86。

同时参考图1和图2，被发送至校准模块48的最终雷达姿态54或者是由多视图非线性ICP调整子模块62确定的局部调整姿态82，或者是由姿态图环路闭合子模块66确定的环路调整的雷达姿态86。具体地，在姿态图环路闭合子模块66检测到环路闭合的事件中，最终雷达姿态54则是环路调整的雷达姿态86，否则，最终雷达姿态54则是由多视图非线性ICP调整子模块62确定的局部调整姿态82。如下面所解释的，局部调整姿态82和环路调整的雷达姿态86是基于由子图子模块64确定的超局部子图84的。通常，超局部子图84是基于预定数量N的连续聚合的过滤后数据点云扫描50和相关联的姿态估计来确定的；然而，在连续聚合的过滤后数据点云扫描50的可用数量小于连续聚合的过滤后数据点云扫描50的预定数量N的事件中，超局部子图84则可以是基于目前可用的可用数量的连续聚合的过滤后数据点云扫描50来确定的。

参考图2，子图子模块64确定基于预定数量N的连续聚合的过滤后数据点云扫描50和相关联的姿态估计的超局部子图84；或者，在可替代方案中，如果连续聚合的过滤后数据点云扫描50的可用数量小于预定数量N，则超局部子图84可以是基于目前可用的可用数量的连续聚合的过滤后数据点云扫描50来确定的，其中，聚合的过滤后数据点云扫描50是基于由单独雷达传感器30采集到的数据来确定的。基于与相关联的姿态估计相结合的、自扫描聚合器和过滤器42接收到的预定数量N的聚合的过滤后数据点云扫描50，子图子模块64初始确定超局部子图84。每个历史聚合的过滤后数据点云扫描50都与相关联的姿态估计组合保存。应当认识到，自子模块60、62和66中的一个接收到的最新姿态估计可以被替代使用以确定超局部子图84。具体地，来自ICP扫描匹配子模块60的初始估计姿态80优先于基于聚合的过滤后数据点云扫描50的相关联的姿态估计，由多视图非线性ICP调整子模块62确定的局部调整姿态82优先于初始估计姿态80，并且由姿态图环路闭合子模块66确定的环路调整的雷达姿态86优先于初始估计姿态80。

超局部子图84示出由预定数量N的连续聚合的过滤后数据点云扫描50和相关联的姿态估计构建而成的图。预定数量N的聚合的过滤后数据点云扫描50和相关联的姿态估计取决于雷达传感器30(图1)的采样速率。在一个实施例中，预定数量N是雷达传感器30的采样速率的一半。例如，在一个实施例中，雷达传感器30的采样速率为约20帧每秒(fps)，因此，预定数量N是10。

扫描匹配和雷达姿态估计器模块46的ICP扫描匹配子模块60从扫描聚合器和过滤器42(图1)接收最新聚合的过滤后数据点云扫描50以及从子图子模块64接收最近超局部子图84作为输入，并且确定最新聚合的过滤后数据点云扫描50的初始估计姿态80。在连续聚合的过滤后数据点云扫描50的可用数量小于预定数量N的事件中，则可以基于可用数量的聚合的过滤后数据点云扫描50确定最近超局部子图84。

由于扫描间达到时间不一致，所以应当认识到，直接使用两个连续扫描之间的相对姿态改变来预测初始估计姿态80，导致了用ICP扫描匹配产生的不精确和收敛问题。因此，ICP扫描匹配子模块60通过首先确定预测姿态来确定初始估计姿态80，下面将进行更加详细的描述。应当认识到，预测姿态是ICP扫描匹配子模块60的内部预测姿态。然后，ICP扫描匹配子模块60调整ICP对齐算法的最大距离阈值参数，以确定聚合的过滤后数据点云扫描50与最近超局部子图84之间的对应关系。应当认识到，是基于自动驾驶车辆10(图1)的线速度和角速度两者来调整最大距离阈值的。最后，通过基于ICP对齐算法将最新聚合的过滤后数据点云扫描50与最近超局部子图84对齐，ICP扫描匹配子模块60确定用作起始点的初始估计姿态80以确定最终雷达姿态54。

在一个实施例中，通过首先确定示出与最新聚合的过滤后数据点云扫描50相关联的最后两个连续姿态估计之间的相对运动的相对变换，ICP扫描匹配子模块60确定预测姿态。然后，基于与最新雷达点扫描相关联的最后两个连续姿态估计之间的时间差，与最新聚合的过滤后数据点云扫描50相关联的最后两个连续姿态估计之间的相对变换被转换为速度。具体地，基于李代数对数函数，使用与最新聚合的过滤后数据点云扫描50相关联的最后两个连续姿态估计之间的时间差，将相对变换转换为se(3)空间。然后，基于最后一个姿态估计与当前姿态估计之间的时间差，将相对变换从速度转换为位置，其中，最后一个姿态估计与当前姿态估计之间的位置上的区别就是预测姿态估计。具体地，基于李代数指数函数，使用最后一个姿态估计与当前姿态估计之间的时间差，将相对变换转换为特殊的欧几电德群SE(3)空间，其中，se(3)是特殊的欧几电德群SE(3)的切空间。

同时参考图2和图3，多视图非线性ICP调整子模块62确定包括最近超局部子图84和邻近雷达点云扫描104的姿态图100(参见图3)。多视图非线性ICP调整子模块62将多个邻近雷达点云扫描104同时与彼此以及与最近超局部子图84进行对齐。应当认识到，最近超局部子图是固定约束，然而，邻近雷达点云扫描104相对于彼此以及相对于最近超局部子图84进行调整。

参考图3，最近超局部子图84由预定数量N的连续聚合的过滤后数据点云扫描50和与ICP扫描匹配子模块60确定的最新点云扫描S11相对应的对应初始估计姿态80构建而成。应当认识到，对应最终雷达姿态54已经被存储在存储器内，用于先前雷达点云扫描S1-S10。在如图3所示示例中，预定数量N是10，并且最近超局部子图84包括10个连续雷达点云扫描，S1-S10。姿态图100还包括N+1个数量的邻近雷达点云扫描104。换而言之，当与最近超局部子图84相比时，姿态图100包括一个额外的邻近雷达点云扫描104。在如图3所示示例中，姿态图100包括邻近雷达点云扫描S1、S2、S3、S4、S5、S6、S7、S8、S9、S10和S11，其中，S11表示来自雷达传感器30的最新雷达点云扫描(图1)。应当认识到，是基于邻近雷达点云扫描104的对应最新姿态估计来测量其接近度的。

同时参考图2和图3，基于邻近雷达点云扫描104的对应初始估计姿态80，通过首次确定邻近雷达点云扫描104，多视图非线性ICP调整子模块62确定姿态图100。多视图非线性ICP调整子模块62还基于K-最近邻(kNN)技术来确定邻近雷达点云扫描104之间的接近度。具体地，可以使用kNN技术以确定邻近雷达点云扫描104之间的邻域矩阵或邻接矩阵。

然后，通过基于最大对应距离阈值针对邻近雷达点云扫描104与最近超局部子图84中的每一个计算点对点对应关系，多视图非线性ICP调整子模块62确定姿态图100。在一个非限制性实施例中，最大对应距离阈值是约1.5米；然而，应当认识到，最大对应距离阈值是基于自动驾驶车辆10(图1)的线速度和角速度两者来调整的。多视图非线性ICP调整子模块62确定边缘约束108，用于连接表示邻近雷达点云扫描104的节点。边缘约束108定义节点与测量的不确定度之间的相对姿态。在如图3所示示例中，变量k等于3。因此，三个边缘约束108在各自的邻近雷达点云扫描104与剩余邻近雷达点云扫描104中的一个之间被确定。边缘约束110还在邻近雷达点云扫描104中的每一个与最近超局部子图84之间被确定。换而言之，最近超局部子图84与邻近雷达点云扫描104中的每一个邻近。然后，多视图非线性ICP调整子模块62针对边缘约束108、110中的每一个分配权重，其中，权重是任意两个邻近雷达点云扫描104之间或邻近雷达点云扫描104与最近超局部子图84之间发现的各个点对应关系之间的距离的测量值。在一个实施例中，距离测量值是全部对应距离与最大对应距离的中位数的平均值。应当认识到，最近超局部子图是包括多个扫描的点云。

一旦姿态图100被构建，那么多视图非线性ICP调整子模块62执行多视图非线性ICP算法来以移动窗口方式调整与邻近雷达点云扫描104相对应的初始估计姿态80，以确定局部调整姿态82。应当认识到，移动窗口是移动跨越N+1个最后扫描(包括最后扫描)的全局窗口，并且每引入新扫描一次就会由单个雷达点云扫描104进行递增。可以使用的多视图非线性ICP算法的一个示例是列文伯格-马夸尔特ICP，但是，应当认识到，也可以使用其他方法。一旦多视图非线性ICP调整子模块62执行多视图非线性ICP调算法以调整与邻近雷达点云扫描104相对应的初始评估姿态80，则只有与N+1个扫描的最旧扫描S1相关联的初始估计姿态80被最终确定，并被保持作为它的局部调整姿态。应当认识到，在多视图非线性ICP算法的下一个移动窗口期间，跳过最旧扫描S1。

再次参考图2，通过首次执行环路检测算法，扫描匹配和雷达姿态估计器模块46的姿态图环路闭合子模块66确定环路调整的雷达姿态86，以确定自动驾驶车辆10(图1)当前是否位于先前已经访问过的位置。当自动驾驶车辆10当前位于先前已经访问过的位置时，这被认为是检测环路。响应于检测环路，姿态图环路闭合子模块66执行非线性优化例程，以确定环路调整的雷达姿态86。用于确定环路调整的雷达姿态86的非线性优化例程的一个示例是非线性最小二乘法例程，诸如，但不限于，列文伯格-马夸尔特算法。

如在图2所见，姿态图环路闭合子模块66与一个或多个空间数据库68进行通信。随着自动驾驶车辆10(图1)的行进，姿态图环路闭合子模块66确定由子图子模块64确定的超局部子图84的二维特征描述符(被存储于一个或多个空间数据库68内)。例如，超局部子图84的多视图二维投影(M2DP)特征描述符被存储于一个或多个空间数据库68内。可以使用的空间数据库68的一个示例是k-d树，其是用于组织k维空间中的点的空间划分数据结构。在实施例中，只有预选或关键超局部子图84的二维特征描述符可以被存储于一个或多个空间数据库68内，其中，关键空间数据库68是基于连续超局部子图84之间发生的阈值运动量被定义的。

随着自动驾驶车辆10(图1)的行进，ICP扫描匹配子模块60接收来自扫描聚合器和过滤器42(图1)的最后数据点云扫描。随着ICP扫描匹配子模块60处理最后数据点云扫描，姿态图环路闭合子模块66确定与最后过滤后数据点云扫描50相对应的超局部子图84的二维特征描述符，并且找到存储于一个或多个空间数据库68内的匹配的二维特征描述符。响应于确定存储于一个或多个空间数据库68内匹配的二维特征描述符是在预定的阈值内，则执行环路闭合ICP扫描匹配算法。当尝试找到匹配，预定的阈值内的一个或多个空间数据库68内的全部候选匹配之间最接近的匹配被选定。具体地，基于放宽的对应距离阈值，在最后数据点云扫描与对应存储于一个或多个空间数据库68内的匹配的二维特征描述符的扫描之间执行环路闭合ICP扫描匹配算法。预定的阈值是根据经验确定的，并且，在一个实施例中，当采用M2DP时，预定的阈值是0.35(预定的阈值没有单位)。放宽的对应距离阈值也是根据经验确定的，并且取决于配置。在一个实施例中，放宽的对应距离阈值为约5米。

响应于确定环路闭合重叠得分和均方根误差(RMSE)落入预定义阈值内，将最后雷达点云扫描和对应的环路闭合变换添加至环路闭合姿态图。在一个实施例中，环路闭合重叠得分为至少0.75(至少75％重叠)，并且RMSE为约3米。由聚合的过滤后数据点扫描50及它们相关联的姿态构建的姿态图环路闭合子模块66构建的环路闭合姿态图，并且环路闭合姿态图的边缘约束是基于相对运动约束的。姿态图环路闭合子模块66执行非线性优化例程，以对环路闭合姿态图进行全局姿态调整，来确定环路调整的雷达姿态86。如上所述，用于确定环路调整的雷达姿态86的非线性优化例程的一个示例是非线性最小二乘法例程，诸如，但不限于，列文伯格-马夸尔特算法。

图4是示出用于通过扫描匹配和雷达姿态估计器模块46(图2)确定最终雷达姿态54(图2)的过程流程图。总体参考图1-4，方法200在方框202处开始。在方框202中，扫描匹配和雷达姿态估计器模块46的子图子模块64初始确定基于与相关联的姿态估计相结合的、自扫描聚合器和过滤器42接收到的预定数量N的聚合的过滤后数据点云扫描50的超局部子图84。在替代方案中，如果连续聚合的过滤后数据点云扫描50的可用数量小于预定数量N，则可以基于目前可用的可用数量的聚合的过滤后数据点云扫描50确定最近超局部子图84。如上所述，子图子模块64基于预定数量N的聚合的过滤后数据点云扫描50和相关联的姿态估计(接收自扫描聚合器和过滤器42)来初始确定超局部子图84，但是，应当认识到，子图子模块64利用自子模块60、62和66中的一个接收到的最新姿态估计。然后，方法200可进入方框204。

在方框204中，通过基于ICP对齐算法将最新聚合的过滤后数据点云扫描50与最近超局部子图84对齐，扫描匹配和雷达姿态估计器模块46的ICP扫描匹配子模块60确定初始估计姿态80。具体地，ICP扫描匹配子模块60确定预姿态，用过子方框204A-204C中的轮廓。

在子方框204A中，ICP扫描匹配子模块60确定表示与最新聚合的过滤后数据点云扫描50相关联的最后两个连续姿态估计之间的相对运动的相对变换。然后，方法200可进入子方框204B。在子方框204B中，然后，基于与最新雷达点扫描相关联的最后两个连续姿态估计之间的时间差，与最新聚合的过滤后数据点云扫描50相关联的最后两个连续姿态估计之间的相对变换被转换为速度。在子方框204C中，基于最后一个姿态估计与当前姿态估计之间的时间差，将相对变换从速度转换为位置，其中，最后一个姿态估计与当前姿态估计之间的位置上的区别就是预测姿态。然后，方法200可进入方框206。

在方框206中，基于最近超局部子图84和邻近雷达点云扫描104，通过首次确定姿态图100(参见图3)，多视图非线性ICP调整子模块62确定局部调整姿态82，并且随后执行多视图非线性ICP算法来以移动窗口方式调整与邻近雷达点云扫描104相对应的初始估计姿态80，以确定局部调整姿态82。在一个实施例中，姿态图环路闭合模块66不检测环路闭合。因此，最终雷达姿态54是局部调整姿态82。然后，方法200可结束或返回到方框202。然而，在环路闭合被姿态图环路闭合方框66检测的事件中，方法200可进入方框208。

在方框208中，姿态图环路闭合方框66执行环路检测算法，以检测自动驾驶车辆10当前是否位于先前访问的位置。如上所述，当自动驾驶车辆10当前位于先前访问的位置时，检测环路。然后，方法200可进入方框210。

在方框210中，响应于检测环路，姿态图环路闭合子模块66执行非线性优化例程，以对环路闭合姿态图进行全局姿态调整，从而确定环路调整的雷达姿态86。如上所述，用于确定环路调整的雷达姿态86的非线性优化例程的一个示例是非线性最小二乘法例程，诸如，但不限于，列文伯格-马夸尔特算法。应当认识到，最终雷达姿态54是环路调整的雷达姿态86。然后，方法200可结束。

总体参考附图，公开的用于自动驾驶车辆的扫描匹配和雷达姿态估计器提供各种技术效果和益处。具体地，本公开提供如果自动驾驶车辆的雷达传感器采集到的雷达点云是稀疏的，则公开的扫描匹配和雷达姿态估计器模块采用超局部子图，用于确定精确度增强的最终雷达姿态。具体地，公开的扫描匹配和雷达姿态估计器采用超局部子图，这降低了稀疏的雷达点云的影响。

控制器可以指的是电子电路、组合逻辑电路、现场可程式闸阵列(FPGA)、执行代码的处理器(共享、专用或组)或上述中的一些或全部的组合，诸如，片上系统，或者是上述的一部分。此外，控制器可以是基于微处理器(诸如，具有至少一个处理器、存储器(RAM和/或ROM)电脑和相关联的输入和输出总线的计算机)的。处理器可以在驻留在存储器内的操作系统的控制下进行操作。操作系统可以管理计算机资源，使得呈现为一个或多个计算机软件应用(诸如，驻留在存储器内的应用程序)的计算机程序代码可以具有被处理器执行的指令。在替代实施例中，处理器可以直接执行应用程序，在这种情况下，可以忽略操作系统。

本公开的描述本质上仅仅是示例性的，并且不背离本公开要点的变化旨在包括在本公开的范围内。这些变化不被认为是脱离本公开的方案和范围。

Claims (10)

1.一种用于自动驾驶车辆的扫描聚合器和过滤器，所述扫描聚合器和过滤器包括：

安装在自动驾驶车辆上的多个雷达传感器，其中，每个单独雷达传感器都执行周围环境的多个单独扫描，以获得包括多个检测点的雷达点云形式的数据；以及

自动驾驶控制器，所述自动驾驶控制器与所述多个雷达传感器电子通信，其中，所述自动驾驶控制器被指示以：

过滤所述单独扫描中的每一个以定义感兴趣的空间区域；

过滤所述单独扫描中的每一个以基于第一异常值-鲁棒模型估计算法移除表示移动物体的所述雷达点云的检测点；

基于运动补偿聚合技术将预定数量的单独扫描聚合在一起，以生成聚合数据扫描；以及

应用多个基于密度的聚类算法以过滤所述聚合数据扫描，来确定过滤后聚合数据扫描。

2.根据权利要求1所述的扫描聚合器和过滤器，其中，所述自动驾驶控制器执行指令以：

基于雷达截面积值过滤所述单独扫描中的每一个，其中，表示阈值尺寸的目标物体的上述雷达点云的多个监测点被保留。

3.根据权利要求1所述的扫描聚合器和过滤器，其中，所述自动驾驶控制器执行指令以：

基于第二异常值-鲁棒模型估计算法过滤所述过滤后聚合数据扫描，以确定聚合的过滤后数据点云。

4.根据权利要求1所述的扫描聚合器和过滤器，其中，所述第一异常值-鲁棒模型估计算法是随机抽样一致(RANSAC)算法或随机抽样最大似然估计(MLESAC)算法。

5.根据权利要求1所述的扫描聚合器和过滤器，其中，所述预定数量的单独扫描取决于所述雷达传感器的采样速率。

6.根据权利要求1所述的扫描聚合器和过滤器，其中，所述预定数量的单独扫描大于或等于3。

7.根据权利要求1所述的扫描聚合器和过滤器，其中，所述运动补偿扫描聚合技术是运动补偿RANSAC技术。

8.根据权利要求1所述的扫描聚合器和过滤器，其中，所述多个基于密度的聚类算法包括基于雷达截面积值过滤所述聚合数据扫描。

9.根据权利要求1所述的扫描聚合器和过滤器，其中，所述多个基于密度的聚类算法包括基于与各自邻近检测点相距的距离过滤所述聚合数据扫描。

10.根据权利要求1所述的扫描聚合器和过滤器，其中，所述多个基于密度的聚类算法是基于密度的带噪声应用空间聚类(DBSCAN)。

Images