CN104166989B - 一种用于二维激光雷达点云匹配的快速icp方法 - Google Patents
一种用于二维激光雷达点云匹配的快速icp方法 Download PDFInfo
- Publication number
- CN104166989B CN104166989B CN201410327894.5A CN201410327894A CN104166989B CN 104166989 B CN104166989 B CN 104166989B CN 201410327894 A CN201410327894 A CN 201410327894A CN 104166989 B CN104166989 B CN 104166989B
- Authority
- CN
- China
- Prior art keywords
- point
- point cloud
- rays
- gamma
- distance
- 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.)
- Expired - Fee Related
Links
- 238000000034 method Methods 0.000 title claims abstract description 38
- 238000004458 analytical method Methods 0.000 claims abstract description 7
- 238000004364 calculation method Methods 0.000 claims description 5
- 238000005259 measurement Methods 0.000 claims description 4
- 238000010276 construction Methods 0.000 claims description 3
- 238000000354 decomposition reaction Methods 0.000 claims description 3
- 230000011218 segmentation Effects 0.000 claims description 3
- 230000009466 transformation Effects 0.000 claims 1
- 238000009616 inductively coupled plasma Methods 0.000 description 19
- 238000013519 translation Methods 0.000 description 5
- 239000011159 matrix material Substances 0.000 description 2
- 230000002159 abnormal effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000010586 diagram Methods 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
- 238000007781 pre-processing Methods 0.000 description 1
- 238000013024 troubleshooting Methods 0.000 description 1
Landscapes
- Optical Radar Systems And Details Thereof (AREA)
Abstract
本发明公开了一种用于二维激光雷达点云匹配的快速ICP方法,它涉及一种ICP方法。本发明的方法步骤为:步骤1:计算模型点集Rm的平面直角坐标表示Pm;步骤2:通过分段分析方法构建当前激光雷达点云数据Rc的有效射线集合Wc,以及点集Wc的平面直角坐标表示Pc。步骤3:初始化迭代次数k=0,T0=I3×3,误差阈值τ。步骤4:计算中的点在Pm中的最近点及其距离其中表示中第j点在Pm中的最近点编号为其距离为步骤5:应用配准,步骤6:如果dk‑1‑dk<τ,则停止迭代,输出Tk,否则,k=k+1,转第4步。本发明使得二维激光雷达原始点云数据配准速度大幅度提高,即将时间复杂度从经典ICP方法的O(DNPNX)降为O(DNP),其中NP为待配准点集的规模,NX为模型点集的规模,D为迭代次数。
Description
技术领域
本发明涉及的是一种ICP方法,具体涉及一种用于二维激光雷达点云匹配的快速ICP方法。
背景技术
二维激光雷达广泛应用于移动机器人领域,例如移动机器人避障、环境建模、目标跟踪、位姿估计等。在这些任务中,通常需要对激光雷达点云数据进行配准。如何快速地进行数据配准是实现移动机器人各类任务实时性的关键。
迭代最近点算法(ICP,Iterative Close Point)是实现点云数据匹配的一种经典方法。然而,ICP算法具有以下不足:(1)要求初始估计较为准确,(2)大量的点云数据使得ICP算法效率低,(3)ICP算法可能陷入局部最优解。
现有技术提出了一种经典的迭代最近点(Iterative Closest Point,ICP)技术,可以对两个点集进行配准,其基本过程如下:
1、令和分别代表代配准的两个点集(具体地,将数据点集P配准到模型点集X)。
2、初始化点集P0=P,配准向量迭代次数k=0,误差阈值τ。
3、计算最近点,Yk=Closest(Pk,X)(时间复杂度为O(NPNX))
4、计算配准及其误差,(时间复杂度为O(NP))
5、应用配准,(时间复杂度为O(NP))
6、如果dk-1-dk<τ,则停止迭代,输出否则,k=k+1,转第3步 迭代最近点(ICP)的主要缺点为:1)如果初始矩阵选择不当,可能导致陷入局部最优点;2)计算最近点的过程计算复杂度较大,为O(NPNX)。假设算法迭代次数为D,则算法复杂度为:O(DNPNX)。由于配准算法通常要被多次调用,而实际问题的数据点集规模也通常非常大,因此该方法难以获得实时结果。
现有技术还提出了一种在极坐标系下配准二维激光雷达点云数据的方法。其基本思路是利用激光雷达原始扫描数据的结构信息,利用扫描投影的方法计算当前扫描在参考坐标系中的期望点云。
该方法首先对原始点云数据进行适当的预处理以去除异常数据。此后,在初始配准估计的基础上,迭代地进行扫描投影(scan projection)、平移估计、旋转估计。具体技术方案如下。
令分别表示参考点云和当前点云, 表示当前点云C相对于参考点云R的初始位姿估计,k=0。
1、扫描投影,表示当前点云C按位姿
投射到参考点云R后,在参考点云方向的期望距离。
2、平移估计,目的是找到使目标函数最小的平移估计(xc,yc)。其中wi为权重。根据文献[3]有
其中,W为权重wi的对角矩阵。
3、方向估计,在平移估计的基础上,以1°的分辨率对[-20°,+20°]范围进行扫描投影,计算使目标函数 最小的
4、更新位姿估计计算k和k+1次迭代的目标函数值,当两者的差距小于设定阈值时终止,否则,k=k+1,转第1步。
该方法的主要缺点包括:1)要求初始估计较为准确,否则有可能陷入局部最优点。2)角度估计与平移估计分为两步,且角度估计仍需要对[-20°,+20°]范围的窗口进行扫描,降低了算法效率。
发明内容
针对现有技术上存在的不足,本发明目的是在于提供一种用于二维激光雷达点云匹配的快速ICP方法,使得二维激光雷达原始点云数据配准速度大幅度提高,即将时间复杂度从经典ICP方法的O(DNPNX)降为O(DNP),本发明对二维激光雷达原始点云数据的特点,在极坐标系下快速地计算两个点云之间的最近点方法。
为了实现上述目的,本发明是通过如下的技术方案来实现:一种用于 二维激光雷达点云匹配的快速ICP方法,其技术方案为:输入:(1)模型点集Rm设置为激光雷达原始时刻点云数据。
(2)当前激光雷达点云数据表示当前点集。
其中,表示Rm的第j条射线,表示Rc的第j条射线,,Lm表示点集Rm的射线总数,Lc表示点集Rc的射线总数,表示射线的测量距离,表示射线的测量距离,表示射线的方向,表示射线的方向,表示角度偏移,表示角度分辨率;
输出:当前激光雷达点云数据Rc相对于模型点集Rm的齐次变换T。
步骤1:计算模型点集Rm的平面直角坐标表示Pm
其中
步骤2:通过分段分析方法构建当前激光雷达点云数据Rc的有效射线集合Wc,以及有效射线集合Wc的平面直角坐标表示Pc。
步骤(2.1)通过式(2)对Rc进行分段分析,
其中,表示第k段点云数据,ct表示分段数目,
gap表示一个阈值,
步骤(2.2)通过式(3)计算有效射线集合,
其中,Wc表示Rc的有效射线集合,表示第k段射线数目,表示第k段射线平均长度,根据这两个特征,γ,λ2,λ1分别表示阈值。
步骤(2.3),计算有效射线集合Wc的平面直角坐标表示Pc。
其中
步骤3:初始化迭代次数k=0,T0=I3×3,误差阈值τ。计算中的点在Pm中的最近点及其距离j∈[1..|Pc|。其中表示中第j点在Pm中的最近点编号为其距离为具体过程如下:
(3.1)对于中的每一个点计算其方向
(3.2)计算方向在Pm中最近点编号
(3.3)根据距离阈值选择用于配准的点集
步骤4:利用奇异值分解(SVD,Singular Value Decomposition)方法计算配准及误差:
步骤5:应用配准,
步骤6:如果dk-1-dk<τ,则停止迭代,输出Tk,否则,k=k+1,转步骤4。
本发明设计了一种快速的方法来计算两个待配准的二维激光雷达点云数据的最近点。将常规的ICP方法在计算最近点时的平方复杂度(O(NPNX))降为线性复杂度(O(NP))(NP为当前点云数据个数,NX为模型点集数据个数)。
附图说明
下面结合附图和具体实施方式来详细说明本发明;
图1为本发明的实施例的四种方法的比较(航迹推算传感器正常时,故障前)图;
图2为本发明的实施例的四种方法的比较(航迹推算传感器故障时)图;
图3为本发明的实施例的四种方法的比较(航迹推算传感器正常时,故障排除后)图。
具体实施方式
为使本发明实现的技术手段、创作特征、达成目的与功效易于明白了解,下面结合具体实施方式,进一步阐述本发明。
参照图1-3,本具体实施方式采用以下技术方案:一种用于二维激光雷达点云匹配的快速ICP方法,其方法步骤为:
步骤1:计算模型点集Rm的平面直角坐标表示Pm
其中
步骤2:通过分段分析方法构建当前激光雷达点云数据Rc的有效射线集合Wc,以及有效射线集合Wc的平面直角坐标表示Pc。
步骤(2.1)通过式(2)对Rc进行分段分析,
其中,表示第k段,ct表示分段数目, gap表示一个阈值,
步骤(2.2)通过式(3)计算有效射线集合,
其中,Wc表示Rc的有效射线集合,表示第k段射线数目,表示第k段射线平均长度,根据这两个特征,γ,λ2,λ1分别表示阈值。
步骤(2.3),计算有效射线集合Wc的平面直角坐标表示Pc。
其中
步骤3:初始化迭代次数k=0,T0=I3×3,误差阈值τ。计算中的点在Pm中的最近点及其距离j∈[1..|Pc|。其中表示中第j点在Pm中的最近点编号为其距离为具体过程如下:
(3.1)对于中的每一个点计算其方向
(3.2)计算方向在Pm中最近点编号
(3.3)根据距离阈值选择用于配准的点集
步骤5:利用奇异值分解(SVD,Singular Value Decomposition)方法计算配准及误差:
步骤6:应用配准,
步骤7:如果dk-1-dk<τ,则停止迭代,输出Tk,否则,k=k+1,转步骤4。
实施例1:图1-3说明了在机器人导航过程中,激光雷达配准的结果比较。四种方法都以航迹推算为初始估计,图1、3为正常情况下的结果,图2为传感器异常(导致航迹推算有较大误差)的结果。
图-3说明快速ICP比常规ICP收敛速度快。同时,快速ICP的最近点计算过程为线性复杂度,而常规ICP的最近点计算过程为平方复杂度。
以上显示和描述了本发明的基本原理和主要特征和本发明的优点。本行业的技术人员应该了解,本发明不受上述实施例的限制,上述实施例和说明书中描述的只是说明本发明的原理,在不脱离本发明精神和范围的前提下,本发明还会有各种变化和改进,这些变化和改进都落入要求保护的本发明范围内。本发明要求保护范围由所附的权利要求书及其等效物界定。
Claims (2)
1.一种用于二维激光雷达点云匹配的快速ICP方法,其特征在于,其技术方案为:输入:(1)模型点集j∈[1..Lm],Rm设置为激光雷达原始时刻点云数据;
(2)当前激光雷达点云数据j∈[1..Lc],表示当前点集;
其中,表示Rm的第j条射线,表示Rc的第j条射线,,Lm表示点集Rm的射线总数,Lc表示点集Rc的射线总数,表示射线的测量距离,表示射线的测量距离,表示射线的方向,表示射线的方向, 表示角度偏移,表示角度分辨率;
输出:当前激光雷达点云数据Rc相对于模型点集Rm的齐次变换T;
步骤1:计算模型点集Rm的平面直角坐标表示Pm
其中
步骤2:通过分段分析方法构建当前激光雷达点云数据Rc的有效射线集合Wc,以及有效射线集合Wc的平面直角坐标表示Pc;
步骤(2.1)通过式(2)对Rc进行分段分析,
其中,表示第k段点云数据,ct表示分段数目,gap表示一个阈值,
步骤(2.2)通过式(3)计算有效射线集合,
其中,Wc表示Rc的有效射线集合,表示第k段射线数目,表示第k段射线平均长度,γ,λ2,λ1分别表示阈值;
步骤(2.3),计算有效射线集合Wc的平面直角坐标表示Pc;
其中
步骤3:初始化迭代次数k=0,T0=I3×3,误差阈值τ;计算中的点在Pm中的最近点及其距离j∈[1..|Pc|];其中表示中第j点在Pm中的最近点编号为其距离为
步骤4:利用奇异值分解(SVD,Singular Value Decomposition)方法计算配准及误差:
步骤5:应用配准,
步骤6:如果dk-1-dk<τ,则停止迭代,输出Tk,否则,k=k+1,转步骤4。
2.根据权利要求1所述的一种用于二维激光雷达点云匹配的快速ICP方法,其特征在于,所述的步骤3具体过程如下:
(3.1)对于中的每一个点计算其方向
(3.2)计算方向在Pm中最近点编号
(3.3)根据距离阈值选择用于配准的点集
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201410327894.5A CN104166989B (zh) | 2014-07-04 | 2014-07-04 | 一种用于二维激光雷达点云匹配的快速icp方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201410327894.5A CN104166989B (zh) | 2014-07-04 | 2014-07-04 | 一种用于二维激光雷达点云匹配的快速icp方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN104166989A CN104166989A (zh) | 2014-11-26 |
CN104166989B true CN104166989B (zh) | 2017-02-15 |
Family
ID=51910780
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201410327894.5A Expired - Fee Related CN104166989B (zh) | 2014-07-04 | 2014-07-04 | 一种用于二维激光雷达点云匹配的快速icp方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN104166989B (zh) |
Families Citing this family (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104657990A (zh) * | 2015-02-06 | 2015-05-27 | 北京航空航天大学 | 一种二维轮廓快速配准方法 |
CN104700451B (zh) * | 2015-03-14 | 2017-05-17 | 西安电子科技大学 | 基于迭代就近点算法的点云配准方法 |
CN104764457B (zh) * | 2015-04-21 | 2017-11-17 | 北京理工大学 | 一种用于无人车的城市环境构图方法 |
US10031231B2 (en) * | 2016-09-12 | 2018-07-24 | Delphi Technologies, Inc. | Lidar object detection system for automated vehicles |
CN109765569B (zh) * | 2017-11-09 | 2022-05-17 | 电子科技大学中山学院 | 一种基于激光雷达实现虚拟航迹推算传感器的方法 |
CN109545072B (zh) * | 2018-11-14 | 2021-03-16 | 广州广电研究院有限公司 | 地图构建的位姿计算方法、装置、存储介质和系统 |
CN116184417A (zh) * | 2018-12-10 | 2023-05-30 | 北京图森智途科技有限公司 | 一种挂车夹角的测量方法、装置及车辆 |
CN110705458B (zh) * | 2019-09-29 | 2022-06-28 | 北京智行者科技有限公司 | 边界检测方法及装置 |
Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
GB2389500A (en) * | 2002-04-20 | 2003-12-10 | Virtual Mirrors Ltd | Generating 3D body models from scanned data |
CN101082988A (zh) * | 2007-06-19 | 2007-12-05 | 北京航空航天大学 | 自动的深度图像配准方法 |
CN102169579A (zh) * | 2011-03-31 | 2011-08-31 | 西北工业大学 | 密集点云模型快速精确配准方法 |
CN103744086A (zh) * | 2013-12-23 | 2014-04-23 | 北京建筑大学 | 一种地面激光雷达与近景摄影测量数据的高精度配准方法 |
-
2014
- 2014-07-04 CN CN201410327894.5A patent/CN104166989B/zh not_active Expired - Fee Related
Patent Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
GB2389500A (en) * | 2002-04-20 | 2003-12-10 | Virtual Mirrors Ltd | Generating 3D body models from scanned data |
CN101082988A (zh) * | 2007-06-19 | 2007-12-05 | 北京航空航天大学 | 自动的深度图像配准方法 |
CN102169579A (zh) * | 2011-03-31 | 2011-08-31 | 西北工业大学 | 密集点云模型快速精确配准方法 |
CN103744086A (zh) * | 2013-12-23 | 2014-04-23 | 北京建筑大学 | 一种地面激光雷达与近景摄影测量数据的高精度配准方法 |
Also Published As
Publication number | Publication date |
---|---|
CN104166989A (zh) | 2014-11-26 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN104166989B (zh) | 一种用于二维激光雷达点云匹配的快速icp方法 | |
Lin et al. | Loam livox: A fast, robust, high-precision LiDAR odometry and mapping package for LiDARs of small FoV | |
CN107239076B (zh) | 基于虚拟扫描与测距匹配的agv激光slam方法 | |
CN104019813B (zh) | 目标即时定位和构建地图的方法与系统 | |
Zhou et al. | LiDAR SLAM with plane adjustment for indoor environment | |
CN112985416A (zh) | 激光与视觉信息融合的鲁棒定位和建图方法及系统 | |
CN104615880B (zh) | 一种三维激光雷达点云匹配的快速icp方法 | |
CN113776519B (zh) | 一种无光动态开放环境下agv车辆建图与自主导航避障方法 | |
CN110187337B (zh) | 一种基于ls和neu-ecef时空配准的高机动目标跟踪方法及系统 | |
CN108151713A (zh) | 一种单目vo快速位姿估计方法 | |
Yap et al. | A particle filter for monocular vision-aided odometry | |
Lowe et al. | Complementary perception for handheld slam | |
CN116429084A (zh) | 一种动态环境3d同步定位与建图方法 | |
Zhao et al. | Review of slam techniques for autonomous underwater vehicles | |
Wen et al. | GNSS/LiDAR integration aided by self-adaptive Gaussian mixture models in urban scenarios: An approach robust to non-Gaussian noise | |
Dhawale et al. | Fast monte-carlo localization on aerial vehicles using approximate continuous belief representations | |
Wang et al. | FEVO-LOAM: Feature Extraction and Vertical Optimized Lidar Odometry and Mapping | |
Zeng et al. | An Indoor 2D LiDAR SLAM and Localization Method Based on Artificial Landmark Assistance | |
Li et al. | A novel edge-enabled SLAM solution using projected depth image information | |
Wang et al. | SW-LIO: A Sliding Window Based Tightly Coupled LiDAR-Inertial Odometry | |
CN117471439A (zh) | 船闸禁停区域无重叠视野激光雷达的外参标定方法 | |
CN116577801A (zh) | 一种基于激光雷达和imu的定位与建图方法及系统 | |
CN116907477A (zh) | 一种基于视觉路标辅助的高精度激光slam方法及装置 | |
Kupervasser et al. | Robust positioning of drones for land use monitoring in strong terrain relief using vision-based navigation | |
Pan et al. | LiDAR-IMU Tightly-Coupled SLAM Method Based on IEKF and Loop Closure Detection |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
C06 | Publication | ||
PB01 | Publication | ||
C10 | Entry into substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
C14 | Grant of patent or utility model | ||
GR01 | Patent grant | ||
CF01 | Termination of patent right due to non-payment of annual fee |
Granted publication date: 20170215 Termination date: 20200704 |
|
CF01 | Termination of patent right due to non-payment of annual fee |