CN115855082A - 一种基于点云先验地图的双模式快速重定位方法 - Google Patents
一种基于点云先验地图的双模式快速重定位方法 Download PDFInfo
- Publication number
- CN115855082A CN115855082A CN202211561710.2A CN202211561710A CN115855082A CN 115855082 A CN115855082 A CN 115855082A CN 202211561710 A CN202211561710 A CN 202211561710A CN 115855082 A CN115855082 A CN 115855082A
- Authority
- CN
- China
- Prior art keywords
- point cloud
- registration
- ndt
- pose
- icp
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 33
- 238000005259 measurement Methods 0.000 claims abstract description 27
- 239000011159 matrix material Substances 0.000 claims abstract description 18
- 230000001133 acceleration Effects 0.000 claims abstract description 14
- 238000010606 normalization Methods 0.000 claims abstract description 3
- 238000012545 processing Methods 0.000 claims abstract description 3
- 230000003068 static effect Effects 0.000 claims description 9
- 230000008569 process Effects 0.000 claims description 8
- 230000009466 transformation Effects 0.000 claims description 8
- 230000006870 function Effects 0.000 claims description 7
- 230000000007 visual effect Effects 0.000 claims description 7
- 238000005070 sampling Methods 0.000 claims description 6
- 238000007781 pre-processing Methods 0.000 claims description 4
- 238000001914 filtration Methods 0.000 claims description 3
- 230000005484 gravity Effects 0.000 claims description 3
- 239000013256 coordination polymer Substances 0.000 claims 1
- 238000013519 translation Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000000284 resting effect Effects 0.000 description 1
Images
Classifications
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Navigation (AREA)
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、利用所述旋转不变性描述子,计算初始重定位位姿Tsc,以Tsc为初值,对匹配上的所述关键帧点云与所述source点云进行ICP算法匹配,得到优化后的位姿Ticp=[Ricp|ticp],再进行NDT算法配准,获得配准结果,若所述配准结果收敛,则重定位成功,输出重定位位姿为Tndt=[Rndt|tndt];反之,则重复执行所述S4.1-S4.3,同时使用计数器flag计算重复执行的次数,当所述flag大于预设阈值,则认为当前实时点云不足以匹配得到重定位位姿,***将切换到手动提示配准模式。
优选地,进行所述自动配准的过程包括:
S5.1、对原始点云进行z轴方向矫正后,使用体素滤波降采样,再使用半径滤波器剔除离群点,设置搜索半径、最小邻点数目,获得source点云;
S5.2、使用静态变量cloud_counter记录接收的点云帧数,当所述cloud_counter大于20时,更新source点云为当前实时点云,更新完成后,cloud_counter重新设为0,开始下一轮计数;
S5.3、遍历所有关键帧点云,计算旋转不变性描述子,将所述旋转不变性描述子与source点云的描述子对比,计算相似度得分,得分最高且大于自定义阈值的为匹配上的关键帧,利用哈希表查询到对应的位姿Tsc=[Rsc|tsc];
S5.4、构建一个Kdtree,输入全局地图,查询tsc位置半径内的点云,构建子图;
S5.5、设置ICP配准最大匹配距离、最大迭代次数、两次变换矩阵之间的差值、均方误差及初值为,将source点云与所述S5.4得到的所述子图配准,得到位姿Ticp=[Ricp|ticp];
S5.6、设置NDT配准两次变换矩阵之间的差值、栅格分辨率及初值,将source点云与所述子图配准,若收敛,则得到最终的重定位位姿Tndt=[Rndt|tndt]。
优选地,更新所述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匹配,得到优化位姿Tndt=[Rndt|tndt],再以Tndt为初值进行ICP算法配准,若结果收敛,则得到重定位位姿为Ticp=[Ricp|ticp];反之,重复执行手动配准操作,若执行次数大于预设阈值,重定位失败;
步骤6.3、若重定位成功,***将在终端打印重定位位姿信息;反之,***提示重定位失败,提示用户需要移动机体后,重新执行重定位程序。
优选地,所述手动提示配准过程,包括:
S7.1、***在终端打印提示内容,提示用户在可视化界面上选取2D PoseEstimate,在地图上点击自车所处的位置,发布“initialpose”话题,得到一个初始猜测位姿Tguess=[I|tguess];
S7.2、创建Kdtree,输入全局地图,查询tguess位置半径内的子图,构建子图;
S7.3、预处理实时点云,获得矫正后的source点云;
S7.4、使用NDT配准source点云与所述S7.2得到的子图进行配准,配置NDT参数,得到初始位姿Tndt=[Rndt|tndt];
S7.5、设置ICP配准参数与ICP参数一致,设Tndt为初值,将source点云与所述子图配准,如果收敛,得到重定位位姿为Ticp=[Ricp|ticp]。
本发明的有益效果为:
(1)本发明基于激光雷达点云先验地图,提出了两种不同的点云配准模式,并通过设置合理的阈值,实现了两种模式的切换,保证在自动搜索配准失效的情况下,***仍可以根据手动提示,增强了定位***的鲁棒性、实用性;
(2)本发明还根据两种模式提供配准初值的特点,采用ICP、NDT相结合的精配准方法,并调整了ICP算法和NDT算法的前后顺序,以达到快速收敛的效果,提高了重定位***的快速性、准确性。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案,下面将对实施例中所需要使用的附图作简单地介绍,显而易见地,下面描述中的附图仅仅是本发明的一些实施例,对于本领域普通技术人员来讲,在不付出创造性劳动性的前提下,还可以根据这些附图获得其他的附图。
图1为本发明实施例的模式切换流程图;
图2为本发明实施例的激光雷达、车体坐标系z轴对齐示意图;
图3为本发明实施例的自动模式点云配准流程图;
图4为本发明实施例的手动模式点云配准流程图。
具体实施方式
下面将结合本发明实施例中的附图,对本发明实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅仅是本发明一部分实施例,而不是全部的实施例。基于本发明中的实施例,本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例,都属于本发明保护的范围。
为使本发明的上述目的、特征和优点能够更加明显易懂,下面结合附图和具体实施方式对本发明作进一步详细的说明。
本实施例提供一种基于点云先验地图的双模式快速重定位方法,具体包括:
步骤1、将激光雷达和IMU固定在车辆顶部的支架上,使得激光雷达的视野开阔,并保证激光雷达和IMU固连,且z轴对齐。
步骤2、将车辆静置在平地,可将车体z轴视为与重力方向对齐,则车体的实际三轴加速度归一化后为n1=[0,0,1]T,通过ROS***的rosbag工具录制大约30秒的IMU的数据包,计算IMU的平均测量结果,并归一化,得到n2=[ax,ay,az]T。
步骤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、得到的先验位姿为,利用旋转不变性描述子得到的Tsc在旋转方向上对重定位位姿给出了估计,而平移方向上的误差更大,因此首先以此为初值,对匹配上的关键帧点云与source点云进行ICP算法匹配,得到优化后的位姿Ticp=[Ricp|ticp],再进行NDT算法配准,如果结果收敛,则重定位成功,输出重定位位姿为Tndt=[Rndt|tndt];反之,程序将重复执行步骤4.1到步骤4.3,同时使用计数器flag计算重复执行的次数,当flag大于阈值3,则认为当前实时点云不足以匹配得到重定位位姿,***将切换到手动提示配准模式,如图1所示。
步骤4.4、当***切换至手动提示配准模式后,终端将会打印信息,提示用户在ROS***的可视化界面Rviz上,估计自车所处的大致位置,点击发布“/initialpose”话题,***接收到话题之后,将会在所选位置半径50m内构建子图,以加速搜索。
步骤4.5、用户发布的“/initialipose”包含平移方向上的初始估计位姿,缺少旋转方向上的估计,因此,首先对实时点云和子图进行NDT匹配,得到优化位姿Tndt=[Rndt|tndt],再以Tndt为初值进行ICP算法配准,如果结果收敛,则得到重定位位姿为Ticp=[Ricp|ticp];反之,重复执行手动配准操作,直至执行次数大于阈值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点云的描述子对比,计算相似度得分,得分最高且大于自定义阈值的为匹配上的关键帧,利用哈希表快速查询到对应的位姿Tsc=[Rsc|tsc],如图3所示。
步骤4、构建一个Kdtree,输入全局地图,查询tsc位置半径50m内的点云,构建一个子图,加速搜索。
步骤5、设置ICP配准最大匹配距离为0.05m,最大迭代次数为100,两次变换矩阵之间的差值为1e-6,均方误差为1e-6,初值为Tsc,将source点云与步骤4得到的子图配准,得到位姿Ticp=[Ricp|ticp]。
步骤6、设置NDT配准两次变换矩阵之间的差值为0.01,栅格分辨率为1.0m,初值为Ticp,将source点云与步骤4得到的子图配准,如果收敛,则得到最终的重定位位姿Tndt=[Rndt|tndt]。
所述步骤4.4-4.5的手动提示配准过程包括以下具体步骤:
步骤1、***在终端打印提示内容,提示用户在Rviz可视化界面上选取“2D PoseEstimate”,在地图上点击自车所处的位置,发布“initialpose”话题,得到一个初始猜测位姿Tguess=[I|tguess]。
步骤2、创建Kdtree,输入全局地图,查询tguess位置半径100m内的子图,构建一个子图。
步骤3、预处理实时点云,方法与权利要求2步骤1一致,获得矫正后的source点云。
步骤4、由于“initialpose”缺少旋转方向上的估计,首先使用NDT配准source点云与步骤2得到的子图进行配准,参数配置与权利要求3中的NDT参数一致,得到初始位姿Tndt=[Rndt|tndt],如图4所示。
步骤5、设置ICP配准参数与权利要求3中的ICP参数一致,设Tndt为初值,将source点云与步骤2得到的子图配准,如果收敛,得到重定位位姿为Ticp=[Ricp|ticp]。
若重定位成功,***将在终端打印重定位位姿信息;反之,***提示重定位失败,需要移动机体后,重新执行重定位程序。
以上所述的实施例仅是对本发明优选方式进行的描述,并非对本发明的范围进行限定,在不脱离本发明设计精神的前提下,本领域普通技术人员对本发明的技术方案做出的各种变形和改进,均应落入本发明权利要求书确定的保护范围内。
Claims (10)
1.一种基于点云先验地图的双模式快速重定位方法,其特征在于,包括:
获取车体静置三轴加速度、惯性测量单元IMU的数据包,基于所述数据包计算所述惯性测量单元IMU的平均测量结果,并对所述测量结果进行归一化处理;
结合所述车体静置三轴加速度与归一化后的所述测量结果,计算激光雷达坐标系和车体坐标系的z轴夹角θ,获得旋转矩阵;
加载先验地图、关键帧点云及位姿信息,通过所述旋转矩阵,存储所述关键帧的点云和对应的位姿,进入自动配准模式,基于所述旋转矩阵进行定位,若定位成功,则输出重定位位姿;若所述自动配准模式的执行次数大于预设阈值,则切换为手动提示配准模式,获得重定位位姿。
2.根据权利要求1所述的基于点云先验地图的双模式快速重定位方法,其特征在于,取所述车体静置三轴加速度,包括:
将所述激光雷达和所述惯性测量单元IMU固定在车辆顶部的支架上,所述激光雷达和所述惯性测量单元IMU固连,且z轴对齐,将车体z轴与重力方向对齐,获得车体实际三轴加速度,对所述车体实际三轴加速度进行归一化,获得所述车体静置三轴加速度。
3.根据权利要求1所述的基于点云先验地图的双模式快速重定位方法,其特征在于,获得归一化后的所述测量结果,包括:
通过ROS***的rosbag工具录制所述惯性测量单元IMU的数据包,计算IMU的平均测量结果,并归一化,得到归一化后的所述测量结果。
5.根据权利要求4所述的基于点云先验地图的双模式快速重定位方法,其特征在于,进入所述自动配准模式,包括:
S4.1、启动激光雷达驱动程序,读取点云数据,接收到实时点云后,按所述旋转矩阵将所述点云进行旋转,将对齐后的点云进行降采样、剔除离群点后作为source点云;
S4.2、遍历所述关键帧点云,计算每帧点云的旋转不变性描述子,将所述描述子与所述source点云的描述子进行对比,得到每个关键帧的得分,得分最高且大于阈值的视为匹配上的关键帧;
S4.3、利用所述旋转不变性描述子,计算初始重定位位姿Tsc,以Tsc为初值,对匹配上的所述关键帧点云与所述source点云进行ICP算法匹配,得到优化后的位姿Ticp=[Ricp|ticp],再进行NDT算法配准,获得配准结果,若所述配准结果收敛,则重定位成功,输出重定位位姿为Tndt=[Rndt|tndt];反之,则重复执行所述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点云的描述子对比,计算相似度得分,得分最高且大于自定义阈值的为匹配上的关键帧,利用哈希表查询到对应的位姿Tsc=[Rsc|tsc];
S5.4、构建一个Kdtree,输入全局地图,查询tsc位置半径内的点云,构建子图;
S5.5、设置ICP配准最大匹配距离、最大迭代次数、两次变换矩阵之间的差值、均方误差及初值为,将source点云与所述S5.4得到的所述子图配准,得到位姿Ticp=[Ricp|ticp];
S5.6、设置NDT配准两次变换矩阵之间的差值、栅格分辨率及初值,将source点云与所述子图配准,若收敛,则得到最终的重定位位姿Tndt=[Rndt|tndt]。
7.根据权利要求6所述的基于点云先验地图的双模式快速重定位方法,其特征在于,更新所述source点云,包括:
创建个互斥量cloud_mutex,需要更新所述source点云时,订阅点云的回调函数线程给cloud_mutex加锁,自动配准线程等待,当更新完成后,点云回调函数线程给cloud_mutex解锁,自动配准线程给cloud_mutex加锁,获得访问source点云内存的权限,并声明一个新的变量存储更新后的source点云,然后给cloud_mutex解锁,完成更新。
8.根据权利要求1所述的基于点云先验地图的双模式快速重定位方法,其特征在于,切换到所述手动提示配准模式,包括:
确定静态变量registration_counter并初始化为0,计算自动模式下的配准次数,在所述自动配准模式下每次配准失败后registration_counter增加1;
当所述registration_counter大于所述预设阈值时,***切换到所述手动提示配准模式。
9.根据权利要求1所述的基于点云先验地图的双模式快速重定位方法,其特征在于,进行所述手动提示配准模式,包括:
步骤6.1、通过终端打印信息,提示用户通过ROS***的可视化界面Rviz上,估计自车所处的位置,并发布“/initialpose”话题,接收到话题之后,在所确定的位置半径内构建子图,进行搜索;
步骤6.2、对实时点云和所述子图进行NDT匹配,得到优化位姿Tndt=[Rndt|tndt],再以Tndt为初值进行ICP算法配准,若结果收敛,则得到重定位位姿为Ticp=[Ricp|ticp];反之,重复执行手动配准操作,若执行次数大于预设阈值,重定位失败;
步骤6.3、若重定位成功,***将在终端打印重定位位姿信息;反之,***提示重定位失败,提示用户需要移动机体后,重新执行重定位程序。
10.根据权利要求9所述的基于点云先验地图的双模式快速重定位方法,其特征在于,所述手动提示配准过程,包括:
S7.1、***在终端打印提示内容,提示用户在可视化界面上选取2D Pose Estimate,在地图上点击自车所处的位置,发布“initialpose”话题,得到一个初始猜测位姿Tguess=[I|tguess];
S7.2、创建Kdtree,输入全局地图,查询tguess位置半径内的子图,构建子图;
S7.3、预处理实时点云,获得矫正后的source点云;
S7.4、使用NDT配准source点云与所述S7.2得到的子图进行配准,配置NDT参数,得到初始位姿Tndt=[Rndt|tndt];
S7.5、设置ICP配准参数与ICP参数一致,设Tndt为初值,将source点云与所述子图配准,如果收敛,得到重定位位姿为Ticp=[Ricp|ticp]。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202211561710.2A CN115855082B (zh) | 2022-12-07 | 2022-12-07 | 一种基于点云先验地图的双模式快速重定位方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202211561710.2A CN115855082B (zh) | 2022-12-07 | 2022-12-07 | 一种基于点云先验地图的双模式快速重定位方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN115855082A true CN115855082A (zh) | 2023-03-28 |
CN115855082B CN115855082B (zh) | 2023-10-20 |
Family
ID=85670595
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202211561710.2A Active CN115855082B (zh) | 2022-12-07 | 2022-12-07 | 一种基于点云先验地图的双模式快速重定位方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN115855082B (zh) |
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN116442286A (zh) * | 2023-06-15 | 2023-07-18 | 国网瑞嘉(天津)智能机器人有限公司 | 机器人作业对象定位***、方法、装置、机器人及介质 |
CN117589154A (zh) * | 2024-01-19 | 2024-02-23 | 深圳竹芒科技有限公司 | 自移动设备的重定位方法、自移动设备和可读存储介质 |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20190234753A1 (en) * | 2018-01-31 | 2019-08-01 | Hitachi, Ltd. | Information processing apparatus and automatic driving track management system |
CN112444246A (zh) * | 2020-11-06 | 2021-03-05 | 北京易达恩能科技有限公司 | 高精度的数字孪生场景中的激光融合定位方法 |
CN113432600A (zh) * | 2021-06-09 | 2021-09-24 | 北京科技大学 | 基于多信息源的机器人即时定位与地图构建方法及*** |
CN114236552A (zh) * | 2021-11-12 | 2022-03-25 | 苏州玖物互通智能科技有限公司 | 基于激光雷达的重定位方法及*** |
CN115127543A (zh) * | 2022-07-22 | 2022-09-30 | 上海于万科技有限公司 | 一种激光建图优化中异常边剔除方法及*** |
-
2022
- 2022-12-07 CN CN202211561710.2A patent/CN115855082B/zh active Active
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20190234753A1 (en) * | 2018-01-31 | 2019-08-01 | Hitachi, Ltd. | Information processing apparatus and automatic driving track management system |
CN112444246A (zh) * | 2020-11-06 | 2021-03-05 | 北京易达恩能科技有限公司 | 高精度的数字孪生场景中的激光融合定位方法 |
CN113432600A (zh) * | 2021-06-09 | 2021-09-24 | 北京科技大学 | 基于多信息源的机器人即时定位与地图构建方法及*** |
CN114236552A (zh) * | 2021-11-12 | 2022-03-25 | 苏州玖物互通智能科技有限公司 | 基于激光雷达的重定位方法及*** |
CN115127543A (zh) * | 2022-07-22 | 2022-09-30 | 上海于万科技有限公司 | 一种激光建图优化中异常边剔除方法及*** |
Non-Patent Citations (3)
Title |
---|
FURONG PENG 等: "Street view cross-sourced point cloud matching and registration", 《2014 IEEE INTERNATIONAL CONFERENCE ON IMAGE PROCESSING (ICIP)》, pages 1 - 5 * |
李彦 等: "提高钢厂物料定位精度的改进配准算法研究及应用", 《冶金自动化》, pages 129 - 134 * |
王勇;唐靖;饶勤菲;袁巢燕;: "采用点云重心距离进行边界检测的点云数据配准", 小型微型计算机***, no. 09, pages 2096 - 2101 * |
Cited By (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN116442286A (zh) * | 2023-06-15 | 2023-07-18 | 国网瑞嘉(天津)智能机器人有限公司 | 机器人作业对象定位***、方法、装置、机器人及介质 |
CN116442286B (zh) * | 2023-06-15 | 2023-10-20 | 国网瑞嘉(天津)智能机器人有限公司 | 机器人作业对象定位***、方法、装置、机器人及介质 |
CN117589154A (zh) * | 2024-01-19 | 2024-02-23 | 深圳竹芒科技有限公司 | 自移动设备的重定位方法、自移动设备和可读存储介质 |
CN117589154B (zh) * | 2024-01-19 | 2024-05-24 | 深圳竹芒科技有限公司 | 自移动设备的重定位方法、自移动设备和可读存储介质 |
Also Published As
Publication number | Publication date |
---|---|
CN115855082B (zh) | 2023-10-20 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN115855082A (zh) | 一种基于点云先验地图的双模式快速重定位方法 | |
CN112179330B (zh) | 移动设备的位姿确定方法及装置 | |
Cai et al. | Practical optimal registration of terrestrial LiDAR scan pairs | |
KR102068419B1 (ko) | 포인트 클라우드 데이터 수집 궤적을 조정하기 위한 방법, 장치 및 컴퓨터 판독 가능한 매체 | |
CN110930495A (zh) | 基于多无人机协作的icp点云地图融合方法、***、装置及存储介质 | |
CN109141364A (zh) | 障碍物检测方法、***及机器人 | |
JP5716433B2 (ja) | 形状認識装置、形状認識方法、および、そのプログラム | |
CN115290097B (zh) | 基于bim的实时精确地图构建方法、终端及存储介质 | |
US10540813B1 (en) | Three-dimensional point data alignment | |
WO2014077272A1 (ja) | 3次元物体認識装置および3次元物体認識方法 | |
CN112183171A (zh) | 一种基于视觉信标建立信标地图方法、装置 | |
CN113587933B (zh) | 一种基于分支定界算法的室内移动机器人定位方法 | |
CN109872350A (zh) | 一种新的点云自动配准方法 | |
US20230114588A1 (en) | Warehouse robot control method and apparatus, robot, and warehouse system | |
CN111400830B (zh) | 一种三维毛坯工件的加工校准方法及装置 | |
CN111754579A (zh) | 多目相机外参确定方法及装置 | |
CN110097598B (zh) | 一种基于pvfh特征的三维物***姿估计方法 | |
Chen et al. | Determining pose of 3D objects with curved surfaces | |
CN112484746A (zh) | 一种基于地平面的单目视觉辅助激光雷达里程计方法 | |
CN113706381A (zh) | 一种三维点云数据的拼接方法及装置 | |
WO2020014941A1 (zh) | 一种建立地图的方法、定位方法、装置、终端及存储介质 | |
Pradalier et al. | Concurrent matching, localization and map building using invariant features | |
CN116466356A (zh) | 一种多激光全局定位方法和*** | |
JPH07146121A (ja) | 視覚に基く三次元位置および姿勢の認識方法ならびに視覚に基く三次元位置および姿勢の認識装置 | |
CN115131208A (zh) | 一种结构光3d扫描测量方法及*** |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |