CN115855082B - 一种基于点云先验地图的双模式快速重定位方法 - Google Patents

Abstract

本发明涉及一种基于点云先验地图的双模式快速重定位方法，包括：通过激光雷达和惯性测量单元IMU，获得归一化后车体的实际三轴加速度，录制惯性测量单元IMU的数据包，基于数据包计算所述惯性测量单元IMU的平均测量结果，并对测量结果进行归一化处理，计算所述激光雷达的坐标系和车体坐标系的z轴夹角θ，获得旋转矩阵；加载先验地图、关键帧点云及位姿信息，通过旋转矩阵，并使用哈希表存储关键帧的点云和对应的位姿，进行一一对应，进入自动配准模式，若定位成功，则输出重定位位姿；若自动配准模式的执行次数大于预设阈值，则切换为手动提示配准模式，获得重定位位姿。本发明增强了定位***的鲁棒性、实用性。

Description

一种基于点云先验地图的双模式快速重定位方法

技术领域

本发明涉及自动驾驶技术领域，特别是涉及一种基于点云先验地图的双模式快速重定位方法。

背景技术

定位是自动驾驶领域的核心问题之一，是无人车辆运动规划、控制等自主能力的必要条件。目前工程上的主流定位方案为：首先通过多种高精度传感器融合，构建高精度点云地图，然后将实时的点云与先验点云地图进行匹配，来获取当前车辆的初始位置，因此如何快速、准确地得到车辆的初始位姿是定位***的难题。在现有的解决方案中，有的使用二维码、标志物等辅助装置来进行重定位，这种方法增加了成本，且适用性不强；有的采用基于NDT算法进行点云匹配，但当先验地图较大时，这种方法收敛较慢，十分耗时。

发明内容

本发明针对上述现有技术中的缺点，提出了基于点云先验地图的双模式快速重定位方法，该方法提供了自动配准和手动提示配准两种重定位模式，并可以根据匹配结果自主切换，在不需要外部辅助装置的情况下，实现无人***快速、准确的重定位。

为实现上述目的，本发明提供了如下方案：

一种基于点云先验地图的双模式快速重定位方法，包括：

获取车体静置三轴加速度、惯性测量单元IMU的数据包，基于所述数据包计算所述惯性测量单元IMU的平均测量结果，并对所述测量结果进行归一化处理；

结合所述车体静置三轴加速度与归一化后的所述测量结果，计算激光雷达坐标系和车体坐标系的z轴夹角θ，获得旋转矩阵；

加载先验地图、关键帧点云及位姿信息，通过所述旋转矩阵，存储所述关键帧的点云和对应的位姿，进入自动配准模式，基于所述旋转矩阵进行定位，若定位成功，则输出重定位位姿；若所述自动配准模式的执行次数大于预设阈值，则切换为手动提示配准模式，获得重定位位姿。

优选地，取所述车体静置三轴加速度，包括：

将所述激光雷达和所述惯性测量单元IMU固定在车辆顶部的支架上，所述激光雷达和所述惯性测量单元IMU固连，且z轴对齐，将车体z轴与重力方向对齐，获得车体实际三轴加速度，对所述车体实际三轴加速度进行归一化，获得所述车体静置三轴加速度。

优选地，获得归一化后的所述测量结果，包括：

通过ROS***的rosbag工具录制所述惯性测量单元IMU的数据包，计算IMU的平均测量结果，并归一化，得到归一化后的所述测量结果。

优选地，获得所述旋转矩阵，包括：

通过所述夹角θ，获取所述激光雷达坐标系的z轴与所述车体坐标系的z轴向量外积n_axis，通过所述向量外积n_axis，获得所述旋转矩阵：

其中，为旋转矩阵。

优选地，进入所述自动配准模式，包括：

S4.1、启动激光雷达驱动程序，读取点云数据，接收到实时点云后，按所述旋转矩阵将所述点云进行旋转，将对齐后的点云进行降采样、剔除离群点后作为source点云；

S4.2、遍历所述关键帧点云，计算每帧点云的旋转不变性描述子，将所述描述子与所述source点云的描述子进行对比，得到每个关键帧的得分，得分最高且大于阈值的视为匹配上的关键帧；

S4.3、利用所述旋转不变性描述子，计算初始重定位位姿T_sc，以T_sc为初值，对匹配上的所述关键帧点云与所述source点云进行ICP算法匹配，得到优化后的位姿T_icp＝[R_icp|t_icp]，再进行NDT算法配准，获得配准结果，若所述配准结果收敛，则重定位成功，输出重定位位姿为T_ndt＝[R_ndt|t_ndt]；反之，则重复执行所述S4.1-S4.3，同时使用计数器flag计算重复执行的次数，当所述flag大于预设阈值，则认为当前实时点云不足以匹配得到重定位位姿，***将切换到手动提示配准模式。

优选地，进行所述自动配准的过程包括：

S5.1、对原始点云进行z轴方向矫正后，使用体素滤波降采样，再使用半径滤波器剔除离群点，设置搜索半径、最小邻点数目，获得source点云；

S5.2、使用静态变量cloud_counter记录接收的点云帧数，当所述cloud_counter大于20时，更新source点云为当前实时点云，更新完成后，cloud_counter重新设为0，开始下一轮计数；

S5.3、遍历所有关键帧点云，计算旋转不变性描述子，将所述旋转不变性描述子与source点云的描述子对比，计算相似度得分，得分最高且大于自定义阈值的为匹配上的关键帧，利用哈希表查询到对应的位姿T_sc＝[R_sc|t_sc]；

S5.4、构建一个Kdtree，输入全局地图，查询t_sc位置半径内的点云，构建子图；

S5.5、设置ICP配准最大匹配距离、最大迭代次数、两次变换矩阵之间的差值、均方误差及初值为，将source点云与所述S5.4得到的所述子图配准，得到位姿T_icp＝[R_icp|t_icp]；

S5.6、设置NDT配准两次变换矩阵之间的差值、栅格分辨率及初值，将source点云与所述子图配准，若收敛，则得到最终的重定位位姿T_ndt＝[R_ndt|t_ndt]。

优选地，更新所述source点云，包括：

创建个互斥量cloud_mutex，需要更新所述source点云时，订阅点云的回调函数线程给cloud_mutex加锁，自动配准线程等待，当更新完成后，点云回调函数线程给cloud_mutex解锁，自动配准线程给cloud_mutex加锁，获得访问source点云内存的权限，并声明一个新的变量存储更新后的source点云，然后给cloud_mutex解锁，完成更新。

优选地，切换到所述手动提示配准模式，包括：

确定静态变量registration_counter并初始化为0，计算自动模式下的配准次数，在所述自动配准模式下每次配准失败后registration_counter增加1；

当所述registration_counter大于所述预设阈值时，***切换到所述手动提示配准模式。

优选地，进行所述手动提示配准模式，包括：

步骤6.1、通过终端打印信息，提示用户通过ROS***的可视化界面Rviz上，估计自车所处的位置，并发布“/initialpose”话题，接收到话题之后，在所确定的位置半径内构建子图，进行搜索；

步骤6.2、对实时点云和所述子图进行NDT匹配，得到优化位姿T_ndt＝[R_ndt|t_ndt]，再以T_ndt为初值进行ICP算法配准，若结果收敛，则得到重定位位姿为T_icp＝[R_icp|t_icp]；反之，重复执行手动配准操作，若执行次数大于预设阈值，重定位失败；

步骤6.3、若重定位成功，***将在终端打印重定位位姿信息；反之，***提示重定位失败，提示用户需要移动机体后，重新执行重定位程序。

优选地，所述手动提示配准过程，包括：

S7.1、***在终端打印提示内容，提示用户在可视化界面上选取2D PoseEstimate，在地图上点击自车所处的位置，发布“initialpose”话题，得到一个初始猜测位姿T_guess＝[I|t_guess]；

S7.2、创建Kdtree，输入全局地图，查询t_guess位置半径内的子图，构建子图；

S7.3、预处理实时点云，获得矫正后的source点云；

S7.4、使用NDT配准source点云与所述S7.2得到的子图进行配准，配置NDT参数，得到初始位姿T_ndt＝[R_ndt|t_ndt]；

S7.5、设置ICP配准参数与ICP参数一致，设T_ndt为初值，将source点云与所述子图配准，如果收敛，得到重定位位姿为T_icp＝[R_icp|t_icp]。

本发明的有益效果为：

(1)本发明基于激光雷达点云先验地图，提出了两种不同的点云配准模式，并通过设置合理的阈值，实现了两种模式的切换，保证在自动搜索配准失效的情况下，***仍可以根据手动提示，增强了定位***的鲁棒性、实用性；

(2)本发明还根据两种模式提供配准初值的特点，采用ICP、NDT相结合的精配准方法，并调整了ICP算法和NDT算法的前后顺序，以达到快速收敛的效果，提高了重定位***的快速性、准确性。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1为本发明实施例的模式切换流程图；

图2为本发明实施例的激光雷达、车体坐标系z轴对齐示意图；

图3为本发明实施例的自动模式点云配准流程图；

图4为本发明实施例的手动模式点云配准流程图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。

本实施例提供一种基于点云先验地图的双模式快速重定位方法，具体包括：

步骤1、将激光雷达和IMU固定在车辆顶部的支架上，使得激光雷达的视野开阔，并保证激光雷达和IMU固连，且z轴对齐。

步骤2、将车辆静置在平地，可将车体z轴视为与重力方向对齐，则车体的实际三轴加速度归一化后为n₁＝[O，0，1]^T，通过ROS***的rosbag工具录制大约30秒的IMU的数据包，计算IMU的平均测量结果，并归一化，得到n₂＝[a_x，a_y，a_z]^T。

步骤3、由步骤2所得的n₁、n₂，计算出激光雷达坐标系和车体坐标系的z轴夹角θ，两条z轴方向向量外积得到n_axis，如图2所示，最后得到使得激光雷达坐标系和车体坐标系z轴对齐的旋转矩阵

步骤4、加载先验地图、关键帧的点云及位姿信息，使用哈希表存储关键帧的点云和对应的位姿，使其一一对应，便于快速查找。

步骤4.1、启动激光雷达驱动程序，读取点云数据，在接收到实时点云后，按旋转矩阵将点云进行旋转，防止原始点云“翘起”或“趴下”，将对齐后的点云进行降采样、剔除离群点后作为source点云，为防止source点云匹配失败，导致程序死锁，需要每隔2秒钟更新source帧。

使用多线程实现source点云的更新与自动配准过程，创建一个互斥量cloud_mutex，当需要更新source点云时，订阅点云的回调函数线程给cloud_mutex加锁，自动配准线程等待，当更新完成后，点云回调函数线程给cloud_mutex解锁，自动配准线程给cloud_mutex加锁，获得访问source点云内存的权限，并声明一个新的变量存储更新后的source点云，然后给cloud_mutex解锁。

步骤4.2、使用哈希表结构快速遍历所有关键帧点云，计算每帧点云的“旋转不变性”描述子，将其与source点云的描述子对比，得到每个关键帧的得分，得分最高且大于阈值的视为匹配上的关键帧。

步骤4.3、得到的先验位姿为，利用旋转不变性描述子得到的T_sc在旋转方向上对重定位位姿给出了估计，而平移方向上的误差更大，因此首先以此为初值，对匹配上的关键帧点云与source点云进行ICP算法匹配，得到优化后的位姿T_icp＝[R_icp|t_icp]，再进行NDT算法配准，如果结果收敛，则重定位成功，输出重定位位姿为T_ndt＝[R_ndt|t_ndt]；反之，程序将重复执行步骤4.1到步骤4.3，同时使用计数器flag计算重复执行的次数，当flag大于阈值3，则认为当前实时点云不足以匹配得到重定位位姿，***将切换到手动提示配准模式，如图1所示。

步骤4.4、当***切换至手动提示配准模式后，终端将会打印信息，提示用户在ROS***的可视化界面Rviz上，估计自车所处的大致位置，点击发布“/initialpose”话题，***接收到话题之后，将会在所选位置半径50m内构建子图，以加速搜索。

步骤4.5、用户发布的“/initialipose”包含平移方向上的初始估计位姿，缺少旋转方向上的估计，因此，首先对实时点云和子图进行NDT匹配，得到优化位姿T_ndt＝[R_ndt|t_ndt]，再以T_ndt为初值进行ICP算法配准，如果结果收敛，则得到重定位位姿为T_icp＝[R_icp|t_icp]；反之，重复执行手动配准操作，直至执行次数大于阈值2，重定位失败。

步骤5、若重定位成功，***将在终端打印重定位位姿信息；反之，***提示重定位失败，提示用户需要移动机体后，重新执行重定位程序。

所述步骤4.3的自动配准切换到手动提示配准的过程包括以下具体步骤：

4.31、对实时点云进行预处理，启动自动配准模式。

4.32、如果自动配准模式下配准成功，则重定位成功；反之，声明静态变量registration_counter并初始化为0，用于计算自动模式下的配准次数，每次配准失败后，registration_counter增加1。

4.33、当registration_counter大于“最大允许自动配准次数”时，***切换到手动提示配准模式。

所述步骤4.1-4.3中自动配准过程包括下述具体步骤：

步骤1、对原始点云进行z轴方向矫正后，使用体素滤波降采样，最小体素单元设置为(0.2，0.2，0.2)，再使用半径滤波器剔除离群点，搜索半径设置为5m，最小邻点数目设置为30，最后得到source点云。

步骤2、在点云话题的回调函数中，使用静态变量cloud_counter记录接收的点云帧数，当cloud_counter大于20时，更新source点云为当前实时点云，防止匹配失败，程序死锁，更新完成后，cloud_counter重新设为0，开始下一轮计数。

步骤3、遍历所有关键帧点云，计算“旋转不变性”描述子，将其与source点云的描述子对比，计算相似度得分，得分最高且大于自定义阈值的为匹配上的关键帧，利用哈希表快速查询到对应的位姿T_sc＝[R_sc|t_sc]，如图3所示。

步骤4、构建一个Kdtree，输入全局地图，查询t_sc位置半径50m内的点云，构建一个子图，加速搜索。

步骤5、设置ICP配准最大匹配距离为0.05m，最大迭代次数为100，两次变换矩阵之间的差值为1e-6，均方误差为1e-6，初值为T_sc，将source点云与步骤4得到的子图配准，得到位姿T_icp＝[R_icp|t_icp]。

步骤6、设置NDT配准两次变换矩阵之间的差值为0.01，栅格分辨率为1.0m，初值为T_icp，将source点云与步骤4得到的子图配准，如果收敛，则得到最终的重定位位姿T_ndt＝[R_ndt|t_ndt]。

所述步骤4.4-4.5的手动提示配准过程包括以下具体步骤：

步骤1、***在终端打印提示内容，提示用户在Rviz可视化界面上选取“2D PoseEstimate”，在地图上点击自车所处的位置，发布“initialpose”话题，得到一个初始猜测位姿T_guess＝[I|t_guess]。

步骤2、创建Kdtree，输入全局地图，查询t_guess位置半径100m内的子图，构建一个子图。

步骤3、预处理实时点云，方法与将点云进行z轴方向的矫正，并进行降采样、剔除离群点操作一致，获得矫正后的source点云。

步骤4、由于“initialpose”缺少旋转方向上的估计，首先使用NDT配准source点云与步骤2得到的子图进行配准，参数配置与NDT参数一致，得到初始位姿T_ndt＝[R_ndt|t_ndt]，如图4所示。

步骤5、设置ICP配准参数与ICP参数一致，设T_ndt为初值，将source点云与步骤2得到的子图配准，如果收敛，得到重定位位姿为T_icp＝[R_icp|t_icp]。

若重定位成功，***将在终端打印重定位位姿信息；反之，***提示重定位失败，需要移动机体后，重新执行重定位程序。

以上所述的实施例仅是对本发明优选方式进行的描述，并非对本发明的范围进行限定，在不脱离本发明设计精神的前提下，本领域普通技术人员对本发明的技术方案做出的各种变形和改进，均应落入本发明权利要求书确定的保护范围内。

Claims (8)

1.一种基于点云先验地图的双模式快速重定位方法，其特征在于，包括：

获取车体静置三轴加速度、惯性测量单元IMU的数据包，基于所述数据包计算所述惯性测量单元IMU的平均测量结果，并对所述测量结果进行归一化处理；

结合所述车体静置三轴加速度与归一化后的所述测量结果，计算激光雷达坐标系和车体坐标系的z轴夹角θ，获得旋转矩阵；

加载先验地图、关键帧点云及位姿信息，通过所述旋转矩阵，存储所述关键帧的点云和对应的位姿，进入自动配准模式，基于所述旋转矩阵进行定位，若定位成功，则输出重定位位姿；若所述自动配准模式的执行次数大于预设阈值，则切换为手动提示配准模式，获得重定位位姿；

进入所述自动配准模式，包括：

S4.1、启动激光雷达驱动程序，读取点云数据，接收到实时点云后，按所述旋转矩阵将所述点云进行旋转，将对齐后的点云进行降采样、剔除离群点后作为source点云；

S4.2、遍历所述关键帧点云，计算每帧点云的旋转不变性描述子，将所述描述子与所述source点云的描述子进行对比，得到每个关键帧的得分，得分最高且大于阈值的视为匹配上的关键帧；

S4.3、利用所述旋转不变性描述子，计算初始重定位位姿T_sc，以T_sc为初值，对匹配上的所述关键帧点云与所述source点云进行ICP算法匹配，得到优化后的位姿T_icp＝[R_icp|t_icp]，再进行NDT算法配准，获得配准结果，若所述配准结果收敛，则重定位成功，输出重定位位姿为T_ndt＝[R_ndt|t_ndt]；反之，则重复执行所述S4.1-S4.3，同时使用计数器flag计算重复执行的次数，当所述flag大于预设阈值，则认为当前实时点云不足以匹配得到重定位位姿，***将切换到手动提示配准模式；

进行所述手动提示配准模式，包括：

步骤6.1、通过终端打印信息，提示用户通过ROS***的可视化界面Rviz上，估计自车所处的位置，并发布“/initialpose”话题，接收到话题之后，在所确定的位置半径内构建子图，进行搜索；

步骤6.2、对实时点云和所述子图进行NDT匹配，得到优化位姿T_ndt＝[R_ndt|t_ndt]，再以T_ndt为初值进行ICP算法配准，若结果收敛，则得到重定位位姿为T_icp＝[R_icp|t_icp]；反之，重复执行手动配准操作，若执行次数大于预设阈值，重定位失败；

步骤6.3、若重定位成功，***将在终端打印重定位位姿信息；反之，***提示重定位失败，提示用户需要移动机体后，重新执行重定位程序。

2.根据权利要求1所述的基于点云先验地图的双模式快速重定位方法，其特征在于，取所述车体静置三轴加速度，包括：

将所述激光雷达和所述惯性测量单元IMU固定在车辆顶部的支架上，所述激光雷达和所述惯性测量单元IMU固连，且z轴对齐，将车体z轴与重力方向对齐，获得车体实际三轴加速度，对所述车体实际三轴加速度进行归一化，获得所述车体静置三轴加速度。

3.根据权利要求1所述的基于点云先验地图的双模式快速重定位方法，其特征在于，获得归一化后的所述测量结果，包括：

通过ROS***的rosbag工具录制所述惯性测量单元IMU的数据包，计算IMU的平均测量结果，并归一化，得到归一化后的所述测量结果。

4.根据权利要求1所述的基于点云先验地图的双模式快速重定位方法，其特征在于，获得所述旋转矩阵，包括：

通过所述夹角θ，获取所述激光雷达坐标系的z轴与所述车体坐标系的z轴向量外积n_axis，通过所述向量外积n_axis，获得所述旋转矩阵：

其中，为旋转矩阵。

5.根据权利要求1所述的基于点云先验地图的双模式快速重定位方法，其特征在于，进行所述自动配准的过程包括：

S5.1、对原始点云进行z轴方向矫正后，使用体素滤波降采样，再使用半径滤波器剔除离群点，设置搜索半径、最小邻点数目，获得source点云；

S5.2、使用静态变量cloud_counter记录接收的点云帧数，当所述cloud_counter大于20时，更新source点云为当前实时点云，更新完成后，cloud_counter重新设为0，开始下一轮计数；

S5.3、遍历所有关键帧点云，计算旋转不变性描述子，将所述旋转不变性描述子与source点云的描述子对比，计算相似度得分，得分最高且大于自定义阈值的为匹配上的关键帧，利用哈希表查询到对应的位姿T_sc＝[R_sc|t_sc]；

S5.4、构建一个Kdtree，输入全局地图，查询t_sc位置半径内的点云，构建子图；

S5.5、设置ICP配准最大匹配距离、最大迭代次数、两次变换矩阵之间的差值、均方误差及初值为，将source点云与所述S5.4得到的所述子图配准，得到位姿T_icp＝[R_icp|t_icp]；

S5.6、设置NDT配准两次变换矩阵之间的差值、栅格分辨率及初值，将source点云与所述子图配准，若收敛，则得到最终的重定位位姿T_ndt＝[R_ndt|t_ndt]。

6.根据权利要求5所述的基于点云先验地图的双模式快速重定位方法，其特征在于，更新所述source点云，包括：

创建个互斥量cloud_mutex，需要更新所述source点云时，订阅点云的回调函数线程给cloud_mutex加锁，自动配准线程等待，当更新完成后，点云回调函数线程给cloud_mutex解锁，自动配准线程给cloud_mutex加锁，获得访问source点云内存的权限，并声明一个新的变量存储更新后的source点云，然后给cloud_mutex解锁，完成更新。

7.根据权利要求1所述的基于点云先验地图的双模式快速重定位方法，其特征在于，切换到所述手动提示配准模式，包括：

确定静态变量registration_counter并初始化为0，计算自动模式下的配准次数，在所述自动配准模式下每次配准失败后registration_counter增加1；

当所述registration_counter大于所述预设阈值时，***切换到所述手动提示配准模式。

8.根据权利要求1所述的基于点云先验地图的双模式快速重定位方法，其特征在于，所述手动提示配准过程，包括：

S7.1、***在终端打印提示内容，提示用户在可视化界面上选取2D Pose Estimate，在地图上点击自车所处的位置，发布“initialpose”话题，得到一个初始猜测位姿T_guess＝[I|t_guess]；

S7.2、创建Kdtree，输入全局地图，查询t_guess位置半径内的子图，构建子图；

S7.3、预处理实时点云，获得矫正后的source点云；

S7.4、使用NDT配准source点云与所述S7.2得到的子图进行配准，配置NDT参数，得到初始位姿T_ndt＝[R_ndt|t_ndt]；

S7.5、设置ICP配准参数与ICP参数一致，设T_ndt为初值，将source点云与所述子图配准，如果收敛，得到重定位位姿为T_icp＝[R_icp|t_icp]。