CN116086484A - 基于地平面约束和回环检测的激光雷达里程计算方法 - Google Patents

基于地平面约束和回环检测的激光雷达里程计算方法 Download PDF

Info

Publication number
CN116086484A
CN116086484A CN202310062082.1A CN202310062082A CN116086484A CN 116086484 A CN116086484 A CN 116086484A CN 202310062082 A CN202310062082 A CN 202310062082A CN 116086484 A CN116086484 A CN 116086484A
Authority
CN
China
Prior art keywords
point cloud
plane
point
points
laser radar
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.)
Pending
Application number
CN202310062082.1A
Other languages
English (en)
Inventor
郑彬
刘宁
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Chongqing Institute of Green and Intelligent Technology of CAS
Original Assignee
Chongqing Institute of Green and Intelligent Technology of CAS
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Chongqing Institute of Green and Intelligent Technology of CAS filed Critical Chongqing Institute of Green and Intelligent Technology of CAS
Priority to CN202310062082.1A priority Critical patent/CN116086484A/zh
Publication of CN116086484A publication Critical patent/CN116086484A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C22/00—Measuring distance traversed on the ground by vehicles, persons, animals or other moving solid bodies, e.g. using odometers, using pedometers
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88—Lidar systems specially adapted for specific applications
    • G01S17/89—Lidar systems specially adapted for specific applications for mapping or imaging
    • 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
    • Y02A—TECHNOLOGIES FOR ADAPTATION TO CLIMATE CHANGE
    • Y02A90/00—Technologies having an indirect contribution to adaptation to climate change
    • Y02A90/10—Information and communication technologies [ICT] supporting adaptation to climate change, e.g. for weather forecasting or climate simulation

Abstract

本发明涉及一种基于地平面约束和回环检测的激光雷达里程计算方法，属于机器人技术领域。本发明通过改进RANSAC随机采样一致性算法进行地平面分割，能够更加准确快速地对地面点云进行提取。本发明引入ScanContext描述子进行激光雷达里程计的回环检测，相比以往的基于邻域半径搜索方式的回环检测更加有效；本发明能够保证在激光里程计算法框架结构不变的情况下，提高里程计的实时性和鲁棒性。激光雷达里程计在地面特征丰富的环境下精度和运行时间主要取决于地平面分割算法和回环检测算法，因此本发明不仅能够提高激光雷达里程计精度，而且缩短了算法整体框架的运行时间。

Description

基于地平面约束和回环检测的激光雷达里程计算方法
技术领域
本发明属于机器人技术领域，涉及基于地平面约束和回环检测的激光雷达里程计算方法。
背景技术
近年来，随着无人驾驶和自主移动机器人领域热度提高，同步定位与建图(SLAM)也越来越受到关注。与视觉传感器相比，激光雷达能够获取目标的深度信息，具有精度高、范围大、抗干扰能力强的特性。而基于激光雷达的SLAM算法也具有相对更高的定位精度和鲁棒性。激光SLAM一般分为四部分：前端扫描匹配部分负责利用前后相邻帧之间的匹配关系估计当前帧的位姿；后端优化部分负责增量式优化里程计及地图信息；回环检测部分负责通过检测闭环而减少全局地图的漂移现象，以便生成全局一致性地图；地图构建部分负责生成和维护全局地图。随着SLAM算法的长时间运行，姿态估计的累计误差逐渐增大，而回环检测算法可通过优化全局地图来减少累积误差，但是回环检测一般缺乏实时性，计算要求很高。基于激光雷达的SLAM算法与视觉SLAM相比，激光点云缺乏对环境的纹理和色彩信息，只有点云位置和强度信息，因此对于不同环境的适应性提出更高要求。
现有的激光里程计算法会把草丛、树叶提取为边缘点特征(草丛和树叶是不稳定特征的主要来源)，因此在非平滑连续(比如颠簸运动)的场景下，采集的数据将会失真，难以在连续帧之间获取准确的匹配，这些地面特征约束将会对结果造成影响。而且大部分算法使用半径搜索的方式进行回环检测，根据当前关键帧的距离和时间戳信息选择关键帧，找到距离当前关键帧一定半径范围并且相隔较远的关键帧作为候选帧进行ICP(IterativeClosest Point)匹配。这种方法在累计误差不大的情况下可以完成回环，但是在大场景下由于不断累计误差，可能导致无法通过距离搜索回环帧，无法完成回环检测，进而影响激光里程计算法整体的精度。因此，需要结合实际问题，深入、系统的对激光雷达里程计的地面平分割和回环检查模块进行研究，并设计出快速准确的激光雷达里程计算法。
发明内容
有鉴于此，本发明的目的在于提供一种基于地平面约束和回环检测的激光雷达里程计算方法。采用了改进的随机一致性采样RANSAC的地平面分割算法，以减少帧间的匹配误差和提高特征提取速度。引入了回环检测Scan Context算法，以更有效地执行回环检测，从而优化全局姿态。
为达到上述目的，本发明提供如下技术方案：
基于地平面约束和回环检测的激光雷达里程计算方法，该方法包括以下步骤：
S1：给定从待测试环境中采集的激光雷达点云集P'；
S2：记平面方程为ax+by+cz＝d，其中a2+b2+c2＝1,d>0，(a,b,c)是平面法向量，d是激光雷达传感器到平面的距离；从点云集P'中选取集合P，集合P属于激光雷达高度h-0.2和h-0.3之间；
S3：在点云数据P中随机选择三个点P1(x1,y1,z1),P1(x2,y2,z2),P3(x3,y3,z3)；
S4：平面H由S3随机选取的三个点P1(x1,y1,z1),P2(x2,y2,z2),P3(x3,y3,z3)求解得到。参数a,b,c由下面方程组决定：
Figure BDA0004061369140000021
S5：计算剩余点云集Pl到平面H的距离dp，统计内点在点云集中的占比。其中dp＝|axi+byi+czi-d|，如果dp小于点到平面距离阈值ε，则标记为内点，否则标记为外点；
S6：重复S3～S5运行k次，比较并选取拟合平面中含有内点数最多的点云集合，采用最小二乘法对平面参数微调使得最后的参数更符合拟合模型，得到最后的平面模型参数。最终算法结束运行条件为：达到k次运行次数，其中k为设定的循环运行最大次数，或者内点数达到阈值ξ。得到最后的拟合平面，该拟合平面为地面点云集，除拟合平面以外的点为非地面点云集。
为计算雷达的运动位姿，得到相邻帧间的姿态变换关系，采用面特征和边缘特征匹配方式来进行完整的数据帧匹配。
S7：采用每个点的前后五个点计算曲率，然后根据曲率来提取面特征和边缘特征，曲率c采用如下计算公式：
Figure BDA0004061369140000022
式中，k表示一次扫描周期，Pk为第k次扫描点云；L为当前雷达坐标系，i表示第k次扫描点云的第i个点，
Figure BDA0004061369140000023
代表三维向量位置，第k次扫描的坐标系为L，点云中的第i个点在L中的位置；M为点
Figure BDA0004061369140000024
附近制定规则数量的点，包括前后左右点，0.25度间隔方向的点。
S8：在时间t和t-1帧之间进行点云粗匹配从而求解对两帧之间的位姿
Figure BDA0004061369140000025
将获取到的地平面点云、边缘点云和平面点云特征从连续两帧估计位姿；
[pz,θroll,θpitch]由点到边和点到平面的距离决定。若地平面约束丢失，则
Figure BDA0004061369140000031
都由点到边和点到平面的距离决定。将计算得到的位姿
Figure BDA0004061369140000032
传递到特征子地图模块来更新特征子地图，同时全局优化模块建立位姿图。
SC(ScanContext,SC)将三维点云从点云中心开始根据等步长的的航向角和半径距离分割为不同的扇形栅格，每个栅格的值是其中所有点的最大高度值，对于没有点的栅格，它的值为零。
S9：开辟一个单独线程对每一帧点云先进行SC的计算；
S10：从该帧点云的SC中提取出一个N维的向量用于在KD树中搜索相近的关键帧；
S11：将搜索得到的参考帧的SC和待匹配的当前帧进行比较，若得分高于一定阈值，则认为找到回环对于两帧SC，即匹配到回环帧，计算两帧的得分描述相似程度。
S12：由匹配到的回环帧对S8的位姿进行全局优化。
可选的，所述S11中，将搜索得到的参考帧的SC和待匹配的当前帧进行比较具体为：
一列一列的比较ScanContext，公式如下：
Figure BDA0004061369140000033
本发明的有益效果在于：本发明通过改进RANSAC随机采样一致性算法进行地平面分割，能够更加准确快速地对地面点云进行提取。本发明引入ScanContext描述子进行激光雷达里程计的回环检测，相比以往的基于邻域半径搜索方式的回环检测更加有效；本发明能够保证在激光里程计算法框架结构不变的情况下，提高里程计的实时性和鲁棒性。激光雷达里程计在地面特征丰富的环境下精度和运行时间主要取决于地平面分割算法和回环检测算法，因此本发明不仅能够提高激光雷达里程计精度，而且缩短了算法整体框架的运行时间。
本发明的其他优点、目标和特征在某种程度上将在随后的说明书中进行阐述，并且在某种程度上，基于对下文的考察研究对本领域技术人员而言将是显而易见的，或者可以从本发明的实践中得到教导。本发明的目标和其他优点可以通过下面的说明书来实现和获得。
附图说明
为了使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明作优选的详细描述，其中：
图1为本发明整体系统图；
图2为本发明所述的ScanContext回环检测流程图。
具体实施方式
以下通过特定的具体实例说明本发明的实施方式，本领域技术人员可由本说明书所揭露的内容轻易地了解本发明的其他优点与功效。本发明还可以通过另外不同的具体实施方式加以实施或应用，本说明书中的各项细节也可以基于不同观点与应用，在没有背离本发明的精神下进行各种修饰或改变。需要说明的是，以下实施例中所提供的图示仅以示意方式说明本发明的基本构想，在不冲突的情况下，以下实施例及实施例中的特征可以相互组合。
其中，附图仅用于示例性说明，表示的仅是示意图，而非实物图，不能理解为对本发明的限制；为了更好地说明本发明的实施例，附图某些部件会有省略、放大或缩小，并不代表实际产品的尺寸；对本领域技术人员来说，附图中某些公知结构及其说明可能省略是可以理解的。
本发明实施例的附图中相同或相似的标号对应相同或相似的部件；在本发明的描述中，需要理解的是，若有术语“上”、“下”、“左”、“右”、“前”、“后”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此附图中描述位置关系的用语仅用于示例性说明，不能理解为对本发明的限制，对于本领域的普通技术人员而言，可以根据具体情况理解上述术语的具体含义。
请参阅图1～图2，为基于地平面约束和回环检测的激光雷达里程计算方法。图1中，五个虚线框表示五个并行处理模块。
本实施例中，采用了公共数据集KITTIOdometry序列00、02、05、07、09来评估的实验结果，其中序列02、07是崎岖不平的山区环境，能够验证本文提出方法的地面分割效果更突出，序列05、07包含更多环路，能够验证回环检测算法的效果更好，测试的实验设备是在HP台式机，配置为GTX1660，6GB内存，i55540，ROS1melodic版本。
本实施例中，所提出的方法主要用于在环路场景条件下测试激光雷达里程计算法，因此KITTI数据集中包含回环的序列00、05用于评估所提出方法的定位精度和计算效率。为了进行比较，通过使用相同的数据序列，将所提出的发明与当前开源KITTI数据集排前列的激光里程计算法框架LeGO-LOAM进行了比较，采用开源SLAM评估工具evo来对本文框架性能进行评估。
本实施例的具体操作，包括如下步骤：
步骤一：采用rosbag工具启动KITTI数据集包，发布velodyne激光雷达话题；
步骤二：在ROS平台下启动激光雷达里程计算法；
步骤三：采用kittievo评估工具对结果进行测试；
对比了本发明与LeGO-LOAM在各序列的绝对轨迹误差的最大值、最小值、标准差、均值、中值、均方根误差等结果如表1所示(本发明英文记为FAST-GSC-LO)，从表中可以看出，本发明各方面的性能指标更优。其中最大值和最小值的误差更小代表本发明在最极端情况下相比LeGO-LOAM不会出现非常大的轨迹漂移，均值和中值代表本发明整体上误差更小，均方根误差更小代表本发明对于异常敏感数据的处理更有效，标准差更小代表本发明与真实的轨迹更加切合。总之，通过指标测试本发明各方面性能更加优越。
表1LeGO-LOAM和本实施例在KITTI上各序列的绝对轨迹误差统计结果
Figure BDA0004061369140000051
为了验证本发明的实时效果性能，统计了在KITTI数据集上地平面分割算法和回环检测算法的平均运行时间，以及算法整体运行时间，如表2所示。从中可以看出地平面分割算法相比LeGO-LOAM实时性更高，推测是由于采用本发明新算法提取到的地面点云集更少；回环检测模块的运行时间也相比LeGO-LOAM速度更快，推测是由于采用SC全局描述符查找回环帧效率更高。算法在KITTI数据集上的平均运行时间提升了37％。因此，本发明不仅在定位误差上更加精确，而且整体算法实时性更高。
表2本实施例与LeGO-LOAM运行时间对比实验测试结果
Figure BDA0004061369140000061
最后说明的是，以上实施例仅用以说明本发明的技术方案而非限制，尽管参照较佳实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或者等同替换，而不脱离本技术方案的宗旨和范围，其均应涵盖在本发明的权利要求范围当中。

Claims (2)

1.基于地平面约束和回环检测的激光雷达里程计算方法，其特征在于：该方法包括以下步骤：
S1：给定从待测试环境中采集的激光雷达点云集P'；
S2：记平面方程为ax+by+cz＝d，其中a2+b2+c2＝1,d>0，(a,b,c)是平面法向量，d是激光雷达传感器到平面的距离；从点云集P'中选取集合P，集合P属于激光雷达高度h-0.2和h-0.3之间；
S3：在点云数据P中随机选择三个点P1(x1,y1,z1),P1(x2,y2,z2),P3(x3,y3,z3)；
S4：平面H由S3随机选取的三个点P1(x1,y1,z1),P2(x2,y2,z2),P3(x3,y3,z3)求解得到；参数a,b,c由下面方程组决定：
Figure FDA0004061369130000011
S5：计算剩余点云集Pl到平面H的距离dp，统计内点在点云集中的占比；其中dp＝|axi+byi+czi-d|，如果dp小于点到平面距离阈值ε，则标记为内点，否则标记为外点；
S6：重复S3～S5运行k次，比较并选取拟合平面中含有内点数最多的点云集合，采用最小二乘法对平面参数微调使得最后的参数更符合拟合模型，得到最后的平面模型参数；最终算法结束运行条件为：达到k次运行次数，其中k为设定的循环运行最大次数，或者内点数达到阈值ξ；得到最后的拟合平面，该拟合平面为地面点云集，除拟合平面以外的点为非地面点云集；
为计算雷达的运动位姿，得到相邻帧间的姿态变换关系，采用面特征和边缘特征匹配方式来进行完整的数据帧匹配；
S7：采用每个点的前后五个点计算曲率，然后根据曲率来提取面特征和边缘特征，曲率c采用如下计算公式：
Figure FDA0004061369130000012
式中，k表示一次扫描周期，Pk为第k次扫描点云；L为当前雷达坐标系，i表示第k次扫描点云的第i个点，
Figure FDA0004061369130000013
代表三维向量位置，第k次扫描的坐标系为L，点云中的第i个点在L中的位置；M为点
Figure FDA0004061369130000014
附近制定规则数量的点，包括前后左右点，0.25度间隔方向的点；
S8：在时间t和t-1帧之间进行点云粗匹配从而求解对两帧之间的位姿
Figure FDA0004061369130000015
将获取到的地平面点云、边缘点云和平面点云特征从连续两帧估计位姿；
[pz,θroll,θpitch]由点到边和点到平面的距离决定；若地平面约束丢失，则
Figure FDA0004061369130000021
都由点到边和点到平面的距离决定；将计算得到的位姿
Figure FDA0004061369130000022
传递到特征子地图模块来更新特征子地图，同时全局优化模块建立位姿图；
SC(Scan Context,SC)将三维点云从点云中心开始根据等步长的的航向角和半径距离分割为不同的扇形栅格，每个栅格的值是其中所有点的最大高度值，对于没有点的栅格，它的值为零；
S9：开辟一个单独线程对每一帧点云先进行SC的计算；
S10：从该帧点云的SC中提取出一个N维的向量用于在KD树中搜索相近的关键帧；
S11：将搜索得到的参考帧的SC和待匹配的当前帧进行比较，若得分高于一定阈值，则认为找到回环对于两帧SC，即匹配到回环帧，计算两帧的得分描述相似程度；
S12：由匹配到的回环帧对S8的位姿进行全局优化。
2.根据权利要求1所述的基于地平面约束和回环检测的激光雷达里程计算方法，其特征在于：所述S11中，将搜索得到的参考帧的SC和待匹配的当前帧进行比较具体为：
一列一列的比较Scan Context，公式如下：
Figure FDA0004061369130000023
CN202310062082.1A 2023-01-18 2023-01-18 基于地平面约束和回环检测的激光雷达里程计算方法 Pending CN116086484A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310062082.1A CN116086484A (zh) 2023-01-18 2023-01-18 基于地平面约束和回环检测的激光雷达里程计算方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310062082.1A CN116086484A (zh) 2023-01-18 2023-01-18 基于地平面约束和回环检测的激光雷达里程计算方法

Publications (1)

Publication Number Publication Date
CN116086484A true CN116086484A (zh) 2023-05-09

Family

ID=86202166

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310062082.1A Pending CN116086484A (zh) 2023-01-18 2023-01-18 基于地平面约束和回环检测的激光雷达里程计算方法

Country Status (1)

Country Link
CN (1) CN116086484A (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116358532A (zh) * 2023-05-31 2023-06-30 小米汽车科技有限公司 回环检测方法、装置、存储介质以及车辆
CN117761717A (zh) * 2024-02-21 2024-03-26 天津大学四川创新研究院 一种自动回环三维重建系统及运行方法

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116358532A (zh) * 2023-05-31 2023-06-30 小米汽车科技有限公司 回环检测方法、装置、存储介质以及车辆
CN116358532B (zh) * 2023-05-31 2023-09-26 小米汽车科技有限公司 回环检测方法、装置、存储介质以及车辆
CN117761717A (zh) * 2024-02-21 2024-03-26 天津大学四川创新研究院 一种自动回环三维重建系统及运行方法

Similar Documents

Publication Publication Date Title
CN110533722B (zh) 一种基于视觉词典的机器人快速重定位方法及系统
CN109345574B (zh) 基于语义点云配准的激光雷达三维建图方法
CN110322511B (zh) 一种基于物体和平面特征的语义slam方法和系统
CN116086484A (zh) 基于地平面约束和回环检测的激光雷达里程计算方法
CN101673403B (zh) 复杂干扰场景下的目标跟踪方法
CN111781608B (zh) 一种基于fmcw激光雷达的运动目标检测方法及系统
Yang et al. Automated extraction of building outlines from airborne laser scanning point clouds
CN103488972B (zh) 基于深度信息的指尖检测方法
CN111540005B (zh) 基于二维栅格地图的回环检测方法
CN103455797A (zh) 航拍视频中运动小目标的检测与跟踪方法
CN104700398A (zh) 一种点云场景物体提取方法
CN112484746B (zh) 一种基于地平面的单目视觉辅助激光雷达里程计方法
CN102799646B (zh) 一种面向多视点视频的语义对象分割方法
CN112084871B (zh) 一种基于弱监督学习的高分遥感目标边界提取方法
CN113269825A (zh) 基于地基激光雷达技术林木胸径值提取的方法
CN103578105A (zh) 一种基于区域特征的多源异类图像配准方法
CN113160292B (zh) 一种基于智能移动终端的激光雷达点云数据三维建模装置及方法
Fan et al. UAV image crop classification based on deep learning with spatial and spectral features
CN105303566A (zh) 一种基于目标轮廓裁剪的sar图像目标方位角估计方法
CN110110687A (zh) 基于颜色信息和三维轮廓信息的树上水果自动识别方法
CN113406658B (zh) 一种基于点线特征扫描匹配的移动机器人定位方法
CN112784869B (zh) 一种基于注意力感知与对抗学习的细粒度图像识别方法
CN112733971A (zh) 扫描设备的位姿确定方法、装置、设备及存储介质
CN112130166A (zh) 基于反光板网络的agv定位方法及装置
CN103927782A (zh) 一种用于深度图像曲面拟合的方法

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