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

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

【技术实现步骤摘要】
一种基于点云先验地图的双模式快速重定位方法


[0001]本专利技术涉及自动驾驶
，特别是涉及一种基于点云先验地图的双模式快速重定位方法。

技术介绍

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

技术实现思路

[0003]本专利技术针对上述现有技术中的缺点，提出了基于点云先验地图的双模式快速重定位方法，该方法提供了自动配准和手动提示配准两种重定位模式，并可以根据匹配结果自主切换，在不需要外部辅助装置的情况下，实现无人系统快速、准确的重定位。
[0004]为实现上述目的，本专利技术提供了如下方案：
[0005]一种基于点云先验地图的双模式快速重定位方法，包括：
[0006]获取车体静置三轴加速度、惯性测量单元IMU的数据包，基于所述数据包计算所述惯性测量单元IMU的平均测量结果，并对所述测量结果进行归一化处理；
[0007]结合所述车体静置三轴加速度与归一化后的所述测量结果，计算激光雷达坐标系和车体坐标系的z轴夹角θ，获得旋转矩阵；
[0008]加载先验地图、关键帧点云及位姿信息，通过所述旋转矩阵，存储所述关键帧的点云和对应的位姿，进入自动配准模式，基于所述旋转矩阵进行定位，若定位成功，则输出重定位位姿；若所述自动配准模式的执行次数大于预设阈值，则切换为手动提示配准模式，获得重定位位姿。
[0009]优选地，取所述车体静置三轴加速度，包括：
[0010]将所述激光雷达和所述惯性测量单元IMU固定在车辆顶部的支架上，所述激光雷达和所述惯性测量单元IMU固连，且z轴对齐，将车体z轴与重力方向对齐，获得车体实际三轴加速度，对所述车体实际三轴加速度进行归一化，获得所述车体静置三轴加速度。
[0011]优选地，获得归一化后的所述测量结果，包括：
[0012]通过ROS系统的rosbag工具录制所述惯性测量单元IMU的数据包，计算IMU的平均测量结果，并归一化，得到归一化后的所述测量结果。
[0013]优选地，获得所述旋转矩阵，包括：
[0014]通过所述夹角θ，获取所述激光雷达坐标系的z轴与所述车体坐标系的z轴向量外积n_axis，通过所述向量外积n_axis，获得所述旋转矩阵：
[0015][0016]其中，为旋转矩阵。
[0017]优选地，进入所述自动配准模式，包括：
[0018]S4.1、启动激光雷达驱动程序，读取点云数据，接收到实时点云后，按所述旋转矩阵将所述点云进行旋转，将对齐后的点云进行降采样、剔除离群点后作为source点云；
[0019]S4.2、遍历所述关键帧点云，计算每帧点云的旋转不变性描述子，将所述描述子与所述source点云的描述子进行对比，得到每个关键帧的得分，得分最高且大于阈值的视为匹配上的关键帧；
[0020]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大于预设阈值，则认为当前实时点云不足以匹配得到重定位位姿，系统将切换到手动提示配准模式。
[0021]优选地，进行所述自动配准的过程包括：
[0022]S5.1、对原始点云进行z轴方向矫正后，使用体素滤波降采样，再使用半径滤波器剔除离群点，设置搜索半径、最小邻点数目，获得source点云；
[0023]S5.2、使用静态变量cloud_counter记录接收的点云帧数，当所述cloud_counter大于20时，更新source点云为当前实时点云，更新完成后，cloud_counter重新设为0，开始下一轮计数；
[0024]S5.3、遍历所有关键帧点云，计算旋转不变性描述子，将所述旋转不变性描述子与source点云的描述子对比，计算相似度得分，得分最高且大于自定义阈值的为匹配上的关键帧，利用哈希表查询到对应的位姿T
sc
＝[R
sc
|t
sc
]；
[0025]S5.4、构建一个Kdtree，输入全局地图，查询t
sc
位置半径内的点云，构建子图；
[0026]S5.5、设置ICP配准最大匹配距离、最大迭代次数、两次变换矩阵之间的差值、均方误差及初值为，将source点云与所述S5.4得到的所述子图配准，得到位姿T
icp
＝[R
icp
|t
icp
]；
[0027]S5.6、设置NDT配准两次变换矩阵之间的差值、栅格分辨率及初值，将source点云与所述子图配准，若收敛，则得到最终的重定位位姿T
ndt
＝[R
ndt
|t
ndt
]。
[0028]优选地，更新所述source点云，包括：
[0029]创建个互斥量cloud_mutex，需要更新所述source点云时，订阅点云的回调函数线程给cloud_mutex加锁，自动配准线程等待，当更新完成后，点云回调函数线程给cloud_mutex解锁，自动配准线程给cloud_mutex加锁，获得访问source点云内存的权限，并声明一个新的变量存储更新后的source点云，然后给cloud_mutex解锁，完成更新。
[0030]优选地，切换到所述手动提示配准模式，包括：
[0031]确定静态变量registration_counter并初始化为0，计算自动模式下的配准次数，
在所述自动配准模式下每次配准失败后registration_counter增加1；
[0032]当所述registration_counter大于所述预设阈值时，系统切换到所述手动提示配准模式。
[0033]优选地，进行所述手动提示配准模式，包括：
[0034]步骤6.1、通过终端打印信息，提示用户通过ROS系统的可视化界面Rviz上，估计自车所处的位置，并发布“/initialp本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于点云先验地图的双模式快速重定位方法，其特征在于，包括：获取车体静置三轴加速度、惯性测量单元IMU的数据包，基于所述数据包计算所述惯性测量单元IMU的平均测量结果，并对所述测量结果进行归一化处理；结合所述车体静置三轴加速度与归一化后的所述测量结果，计算激光雷达坐标系和车体坐标系的z轴夹角θ，获得旋转矩阵；加载先验地图、关键帧点云及位姿信息，通过所述旋转矩阵，存储所述关键帧的点云和对应的位姿，进入自动配准模式，基于所述旋转矩阵进行定位，若定位成功，则输出重定位位姿；若所述自动配准模式的执行次数大于预设阈值，则切换为手动提示配准模式，获得重定位位姿。2.根据权利要求1所述的基于点云先验地图的双模式快速重定位方法，其特征在于，取所述车体静置三轴加速度，包括：将所述激光雷达和所述惯性测量单元IMU固定在车辆顶部的支架上，所述激光雷达和所述惯性测量单元IMU固连，且z轴对齐，将车体z轴与重力方向对齐，获得车体实际三轴加速度，对所述车体实际三轴加速度进行归一化，获得所述车体静置三轴加速度。3.根据权利要求1所述的基于点云先验地图的双模式快速重定位方法，其特征在于，获得归一化后的所述测量结果，包括：通过ROS系统的rosbag工具录制所述惯性测量单元IMU的数据包，计算IMU的平均测量结果，并归一化，得到归一化后的所述测量结果。4.根据权利要求1所述的基于点云先验地图的双模式快速重定位方法，其特征在于，获得所述旋转矩阵，包括：通过所述夹角θ，获取所述激光雷达坐标系的z轴与所述车体坐标系的z轴向量外积n_axis，通过所述向量外积n_axis，获得所述旋转矩阵：其中，为旋转矩阵。5.根据权利要求4所述的基于点云先验地图的双模式快速重定位方法，其特征在于，进入所述自动配准模式，包括：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.根据权利要求5所述的基于点云先验地图的双模式快速重定位方法，其特征在于，进行所述自动配准的过程包括：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，输入全局...

【专利技术属性】
技术研发人员：魏超，曹兴雨，钟思维，丁萌，
申请(专利权)人：北京理工大学长三角研究院嘉兴，
类型：发明
国别省市：