CN113702941A - 一种基于改进icp的点云测速方法 - Google Patents

一种基于改进icp的点云测速方法 Download PDF

Info

Publication number
CN113702941A
CN113702941A CN202110908246.9A CN202110908246A CN113702941A CN 113702941 A CN113702941 A CN 113702941A CN 202110908246 A CN202110908246 A CN 202110908246A CN 113702941 A CN113702941 A CN 113702941A
Authority
CN
China
Prior art keywords
point
point cloud
points
distance
average
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
Application number
CN202110908246.9A
Other languages
English (en)
Other versions
CN113702941B (zh
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.)
Harbin Engineering University
Original Assignee
Harbin Engineering University
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 Harbin Engineering University filed Critical Harbin Engineering University
Priority to CN202110908246.9A priority Critical patent/CN113702941B/zh
Publication of CN113702941A publication Critical patent/CN113702941A/zh
Application granted granted Critical
Publication of CN113702941B publication Critical patent/CN113702941B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • 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
    • G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
    • G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
    • G01S7/4802—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00 using analysis of echo signal for target characterisation; Target signature; Target cross-section
    • 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/02—Systems using the reflection of electromagnetic waves other than radio waves
    • G01S17/50—Systems of measurement based on relative movement of target
    • G01S17/58—Velocity or trajectory determination systems; Sense-of-movement determination systems
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T17/00—Three dimensional [3D] modelling, e.g. data description of 3D objects
    • G06T17/20—Finite element generation, e.g. wire-frame surface description, tesselation
    • 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

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Electromagnetism (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Computer Graphics (AREA)
  • Geometry (AREA)
  • Software Systems (AREA)
  • Theoretical Computer Science (AREA)
  • Length Measuring Devices With Unspecified Measuring Means (AREA)
  • Length Measuring Devices By Optical Means (AREA)

Abstract

本发明公开了一种基于改进ICP的点云测速方法，包括：点云预处理：去除点云数据中的噪声点，通过聚类及检测手段，移除点云数据中的动态物体；点云的匹配：获得采集当前一帧点云时载体的位姿，并以此位姿为基础，利用改进的ICP方法，进一步优化当前一帧点云所对应的载体位姿；局部图优化：对包括最新计算所得位姿在内的给定时间间隔内的所有位姿进行局部图优化；计算速度：利用角轴法对载体经历的位姿变化进行表示，然后计算出载体目前的速度。本发明的三维点云预处理可去除初始点云数据中的离群点和毛刺点，大大减轻噪声点对后续匹配步骤的影响。采用的小区域平面投影至大区域平面，极大的使投影效果贴近真实情况，使得配准结果更为准确。

Description

一种基于改进ICP的点云测速方法
技术领域
本发明属于导航技术领域，具体涉及一种基于改进ICP的点云测速方法。
背景技术
随着激光雷达等传感器技术的快速发展，点云数据在建图、导航和避障等领域的应用已经成为研究热点之一，而如果将点云数据用在测量载体速度方面，那么就可以更加充分的利用点云信息。对于仅拥有采集点云信息传感器的载体来说，利用点云数据来测得载体速度可以说是唯一的测速手段。同时，在状态估计中，通过提供更多的信息有利于导航系统对所估计状态更好的估计。此外，ICP(Iterative Closest Point)在传统点云配准方法中有着较为重要的意义，但普通ICP方法存在引入误差过大的问题，故对普通ICP方法进行了改进。
发明内容
针对上述现有技术，本发明要解决的技术问题是提供一种配准结果更为准确的基于改进ICP的点云测速方法，用于为装有采集点云数据传感器的载体提供一种测量速度的方法。
为解决上述技术问题，本发明的一种基于改进ICP的点云测速方法，包括以下步骤：
步骤1、点云预处理：去除点云数据中的噪声点，通过聚类及检测手段，移除点云数据中的动态物体；
步骤2：点云的匹配：获得采集当前一帧点云时载体的位姿，并以此位姿为基础，利用改进的ICP方法，进一步优化当前一帧点云所对应的载体位姿；
步骤3：局部图优化：对包括最新计算所得位姿在内的给定时间间隔内的所有位姿进行局部图优化；
步骤4：计算速度：利用角轴法对载体经历的位姿变化进行表示，然后计算出载体目前的速度。
本发明还包括：
1.步骤1中点云预处理具体为：
设k时刻在载体坐标系Lk下采集到的一帧三维点云数据为
Figure BDA0003202656610000011
其中
Figure BDA0003202656610000012
表示点云数据中的第i个点，
Figure BDA0003202656610000013
分别表示点云数据中的第i个点在载体坐标系Lk下的坐标；
在
Figure BDA0003202656610000014
中，通过kd-tree算法计算出距离每个点最近的m个点以及该点到距离其最近的m个点的距离；
对于第i个点
Figure BDA0003202656610000021
距离其最近的m个点与点
Figure BDA0003202656610000022
之间的距离分别为：di1，di2，…，dim；
计算出这m个距离的平均值di-average，di-average为局部平均距离；
求出点
Figure BDA0003202656610000023
的m个临近点的局部平均距离di1-average，di2-average，…，dim-average，从di1-average，di2-average，…，dim-average中选出最小值dimin-average，然后比较di-average和dimin-average的大小；若di-average＞Kdimin-average，则判定
Figure BDA0003202656610000024
为噪声点；其中，K是一个给定参数；
预处理后得到的三维点云数据记为：
Figure BDA0003202656610000025
2.步骤2中点云的匹配具体为：
使用里程计或者RANSAC方法获得载体坐标系Lk在世界系W下的初始估计的位姿Tk-start；
设点云采样时刻k的上一采样时刻为o；
采集到的点云数据记为
Figure BDA0003202656610000026
预处理后的点云数据记为
Figure BDA0003202656610000027
时刻o对应的载体坐标系为Lo；
载体在世界系下的位姿为To；
利用改进的ICP方法对
Figure BDA0003202656610000028
和
Figure BDA0003202656610000029
处理，得到优化后的Tk-start，记为Tk；
所述改进的ICP方法具体为：
利用初始估计的位姿Tk-start和To将
Figure BDA00032026566100000210
变换到世界坐标系W下，对应的点云数据表示为
Figure BDA00032026566100000211
从
Figure BDA00032026566100000212
中选出位于平面上的点；
使用非极大值抑制的方法使选取的点分布均匀；
得到的标准点集记为
Figure BDA00032026566100000213
在
Figure BDA00032026566100000214
中搜索距离
Figure BDA00032026566100000215
中的每个点最近的λ个点；
从这λ个点中再筛选出距离
Figure BDA0003202656610000031
中相应点小于给定距离D1的点，得到点集
Figure BDA0003202656610000032
其中，l′表示上述筛选操作后，点集的数量；
从
Figure BDA0003202656610000033
中选出距离点
Figure BDA0003202656610000034
最近的λ个点，记为
Figure BDA0003202656610000035
再从
Figure BDA0003202656610000036
中选出距离点
Figure BDA0003202656610000037
小于给定距离D1的点集
Figure BDA0003202656610000038
其中γi表示
Figure BDA0003202656610000039
中点的数量；
其中距离
Figure BDA00032026566100000310
最近的一个点记为pnst-nbor，比较pnst-nbor与
Figure BDA00032026566100000311
两点处的法向量，若两法向量的夹角在区间[θt,180°-θt]，则将
Figure BDA00032026566100000312
从
Figure BDA00032026566100000313
中舍去，其中θt为给定值；
计算
Figure BDA00032026566100000314
中所有点与pnst-nbor的距离，若存在距离小于给定距离R1的情况，则将
Figure BDA00032026566100000315
从
Figure BDA00032026566100000316
中舍去；
计算
Figure BDA00032026566100000317
中每个点是否位于平面上；
若没有位于平面上，则将对应的点集剔除出
Figure BDA00032026566100000318
得到
Figure BDA00032026566100000319
其中，l″表示
Figure BDA00032026566100000320
中点集的数量；
对于
Figure BDA00032026566100000321
中的每个点集，计算这些点集中的点在
Figure BDA00032026566100000322
中的最近邻，若最近邻没有位于平面上，且最近邻距离
Figure BDA00032026566100000323
中相应点的距离超过给定距离R2，则将相应点集从
Figure BDA00032026566100000324
剔除；
且R2＞R1；
得到
Figure BDA00032026566100000325
相应的最近邻点集记为
Figure BDA00032026566100000326
Figure BDA00032026566100000327
每个点对应的法向量记为
Figure BDA00032026566100000328
然后将
Figure BDA00032026566100000329
中的点投影至标准点集
Figure BDA00032026566100000330
的对应点中，得到投影点集
Figure BDA00032026566100000331
计算
Figure BDA00032026566100000332
与
Figure BDA00032026566100000333
中对应点的距离，得到误差函数error(Tk-start)；
通过非线性优化的方法对error(Tk-start)进行优化，得到ΔT；
对Tk-start更新，即Tk-start＝ΔT·Tk-start；
其中，ΔT即为更新量；
而后继续对新的Tk-start进行迭代优化，当||ΔT||2＜Tmin或已到达迭代次数nt，则停止迭代，并将最终得到的Tk-start记为Tk，其中，Tmin和nt为给定值。
3.位于平面上的判断方法具体为：
对于点集P，取其中一点pi，计算以pi为中心，以R3为半径的球体所包含的所有点
Figure BDA0003202656610000041
其中，R3＞R2；
计算出μi、Σi；
其中，
Figure BDA0003202656610000042
Figure BDA0003202656610000043
对Σi进行奇异值分解得到
Figure BDA0003202656610000044
定义曲率为：
Figure BDA0003202656610000045
若α＜αthreshold，且λ1＜δλ2，则认为pi位于平面上，αthreshold为给定的曲率阈值；
否则，认为pi没有位于平面上；
其中，0＜δ＜1。
4.将
Figure BDA0003202656610000046
中的点投影至标准点集
Figure BDA0003202656610000047
的对应点中具体为：
对于
Figure BDA0003202656610000048
的第i个点集
Figure BDA0003202656610000049
从
Figure BDA00032026566100000410
中取出第j个点
Figure BDA00032026566100000411
Figure BDA00032026566100000412
在
Figure BDA00032026566100000413
中的最近邻为
Figure BDA00032026566100000414
处的法向量为
Figure BDA00032026566100000415
计算
Figure BDA00032026566100000416
距离
Figure BDA00032026566100000417
点邻域所形成平面的距离
Figure BDA00032026566100000418
得到
Figure BDA0003202656610000051
在
Figure BDA0003202656610000052
中的投影
Figure BDA0003202656610000053
所述距离
Figure BDA0003202656610000054
点邻域所形成平面的距离：
设
Figure BDA0003202656610000055
邻域的点集为
Figure BDA0003202656610000056
则
Figure BDA0003202656610000057
其中：
Figure BDA0003202656610000058
5.误差函数error(Tk-start)满足：
对于
Figure BDA0003202656610000059
其对应的误差部分为
Figure BDA00032026566100000510
则
Figure BDA00032026566100000511
6.步骤4中计算速度具体为：
通过Tk和To可求出从Lo向Lk的变换矩阵Tk←o；
若用角轴法和平移向量tk←o来表示Tk←o，则有旋转轴nk←o、旋转角度θk←o；
设两帧点云数据间的时间间隔为Δτ；
则有旋转速度为：
Figure BDA00032026566100000512
平移速度：
Figure BDA00032026566100000513
本发明的有益效果：与现有技术相比，本发明具有如下的有益效果：
1、本发明的三维点云预处理可有效的去除初始点云数据中的离群点和毛刺点，大大减轻噪声点对后续匹配步骤的影响。
2、本发明所采用的小区域平面投影至大区域平面，极大的使投影效果贴近真实情况，使得配准结果更为准确。
3、本发明利用点云数据对载体的速度进行了估计。
附图说明
图1为本发明测速方法的流程图；
图2为关于离群点和毛刺的介绍。
具体实施方式
下面结合说明书附图和具体实施方式对本发明做进一步说明。
以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变化和改进。这些都属于本发明的保护范围。
如图1所示，一种基于改进ICP的点云测速方法，包括如下步骤：
点云预处理步骤：去除点云数据中的噪声点，通过聚类及检测手段，移除点云数据中的动态物体；
点云的匹配步骤：通过其他手段初步获得采集当前一帧点云时，载体的位姿，并以此位姿为基础，利用改进的ICP方法，进一步优化当前一帧点云所对应的载体位姿；
局部图优化步骤：对包括最新计算所得位姿在内的一定时间间隔内的所有位姿进行优化；
计算速度步骤：利用角轴法对载体经历的位姿变化进行表示，然后计算出载体目前的速度。
实施例1：三维点云预处理步骤
设k时刻在载体坐标系Lk下采集到的一帧三维点云数据为
Figure BDA0003202656610000061
其中
Figure BDA0003202656610000062
表示点云数据中的第i个点，
Figure BDA0003202656610000063
分别表示点云数据中的第i个点在载体坐标系Lk下的坐标；
在
Figure BDA0003202656610000064
中，通过kd-tree算法计算出距离每个点最近的m个点以及该点到距离其最近的m个点的距离；
对于第i个点
Figure BDA0003202656610000065
距离其最近的m个点与点
Figure BDA0003202656610000066
之间的距离分别为：di1，di2，…，dim；
计算出这m个距离的平均值di-average，可称di-average为局部平均距离；
同理也可求出点
Figure BDA0003202656610000067
的m个临近点的局部平均距离di1-average，di2-average，…，dim-average。从di1-average，di2-average，…，dim-average中选出最小值dimin-average，然后比较di-average和dimin-average的大小；若di-average＞Kdimin-average，则判定
Figure BDA0003202656610000068
为噪声点；
其中，K是一个待调节的参数；
预处理后得到的三维点云数据记为：
Figure BDA0003202656610000069
实施例2：三维点云的匹配
使用里程计或者RANSAC方法获得载体坐标系Lk在世界系W下的初始估计的位姿Tk-start；
设点云采样时刻k的上一采样时刻为o；
采集到的点云数据记为
Figure BDA0003202656610000071
预处理后的点云数据记为
Figure BDA0003202656610000072
时刻o对应的载体坐标系为Lo；
载体在世界系下的位姿为To；
利用改进的ICP方法对
Figure BDA0003202656610000073
和
Figure BDA0003202656610000074
处理，得到优化后的Tk-start，记为Tk。
实施例3：改进的ICP
使用里程计或者RANSAC方法获得载体坐标系Lk在世界系W下的初始估计的位姿Tk-start；
设点云采样时刻k的上一采样时刻为o；
采集到的点云数据记为
Figure BDA0003202656610000075
预处理后的点云数据记为
Figure BDA0003202656610000076
时刻o对应的载体坐标系为Lo；
载体在世界系下的位姿为To；
利用Tk-start和To将
Figure BDA0003202656610000077
变换到世界坐标系W下，对应的点云数据表示为
Figure BDA0003202656610000078
从
Figure BDA0003202656610000079
中选出位于平面上的点；
使用非极大值抑制的方法使选取的点分布均匀；
得到的标准点集记为
Figure BDA00032026566100000710
在
Figure BDA00032026566100000711
中进行搜索操作，对于
Figure BDA00032026566100000712
中的任意一点pj，在
Figure BDA00032026566100000713
中找到距离pj最近的λ个点；
从这λ个点中再筛选出距离pj小于D1的点；
经由上述两步筛选操作后，得到点集
Figure BDA0003202656610000081
其中，l′表示上述筛选操作后，点集的数量；
从
Figure BDA0003202656610000082
中选出距离点
Figure BDA0003202656610000083
最近的λ个点，记为
Figure BDA0003202656610000084
再从
Figure BDA0003202656610000085
中选出距离点
Figure BDA0003202656610000086
小于D1的点集
Figure BDA0003202656610000087
其中γi表示
Figure BDA0003202656610000088
中点的数量；
其中距离
Figure BDA0003202656610000089
最近的一个点记为pnst-nbor，比较pnst-nbor与
Figure BDA00032026566100000810
两点处的法向量，若两法向量的夹角在[θt,180°-θt]之间，则将
Figure BDA00032026566100000811
从
Figure BDA00032026566100000812
中舍去，其中θt为给定值，θt可以取值20°；
计算
Figure BDA00032026566100000813
中所有点与pnst-nbor的距离，若存在距离小于R1的情况，则将
Figure BDA00032026566100000814
从
Figure BDA00032026566100000815
中舍去；
计算
Figure BDA00032026566100000816
中每个点是否位于平面上；
若没有位于平面上，则将对应的点集剔除出
Figure BDA00032026566100000817
得到
Figure BDA00032026566100000818
其中，l″表示
Figure BDA00032026566100000819
中点集的数量；
对于
Figure BDA00032026566100000820
中的每个点集，还应计算这些点集中的点在
Figure BDA00032026566100000821
中的最近邻，若最近邻没有位于平面上，且最近邻距离
Figure BDA00032026566100000822
中相应点的距离超过R2，则将相应点集从
Figure BDA00032026566100000823
剔除；
且R2＞R1；
得到
Figure BDA00032026566100000824
相应的最近邻点集记为
Figure BDA00032026566100000825
Figure BDA00032026566100000826
每个点对应的法向量记为
Figure BDA00032026566100000827
然后将
Figure BDA00032026566100000828
中的点投影至标准点集
Figure BDA00032026566100000829
的对应点中，得到投影点集
Figure BDA00032026566100000830
计算
Figure BDA0003202656610000091
与
Figure BDA0003202656610000092
中对应点的距离，得到误差函数error(Tk-start)；
通过非线性优化的方法对error(Tk-start)进行优化，得到ΔT；
对Tk-start更新，即Tk-start＝ΔT·Tk-start；
其中，ΔT即为更新量；
而后继续对新的Tk-start利用改进的ICP方法进行迭代优化；
当||ΔT||2＜Tmin或已到达迭代次数nt，则停止迭代，并将最终得到的Tk-start记为Tk；
其中，Tmin和nt为给定值。
实施例4：位于平面上
对于点集P，取其中一点pi，计算以pi为中心，以R3为半径的球体所包含的所有点
Figure BDA0003202656610000093
其中，R3＞R2；
计算出μi、Σi；
其中，
Figure BDA0003202656610000094
Figure BDA0003202656610000095
对Σi进行奇异值分解得到
Figure BDA0003202656610000096
定义曲率为：
Figure BDA0003202656610000097
若α＜αthreshold，且λ1＜δλ2，则认为pi位于平面上；
否则，认为pi没有位于平面上；
其中，0＜δ＜1。
实施例5：所述点投影至标准点集
对于
Figure BDA0003202656610000101
的第i个点集
Figure BDA0003202656610000102
从
Figure BDA0003202656610000103
中取出第j个点
Figure BDA0003202656610000104
Figure BDA0003202656610000105
在
Figure BDA0003202656610000106
中的最近邻为
Figure BDA0003202656610000107
处的法向量为
Figure BDA0003202656610000108
计算
Figure BDA0003202656610000109
距离
Figure BDA00032026566100001010
点邻域所形成平面的距离
Figure BDA00032026566100001011
得到
Figure BDA00032026566100001012
在
Figure BDA00032026566100001013
中的投影
Figure BDA00032026566100001014
所述距离
Figure BDA00032026566100001015
点邻域所形成平面的距离：
设
Figure BDA00032026566100001016
邻域的点集为
Figure BDA00032026566100001017
则
Figure BDA00032026566100001018
其中：
Figure BDA00032026566100001019
实施例6：误差函数
对于
Figure BDA00032026566100001020
其对应的误差部分为
Figure BDA00032026566100001021
则
Figure BDA00032026566100001022
实施例7：局部图优化
对于从k往前的ρ帧有效激光数据，可得到ρ个位姿Tk-ρ+1,...,Tk；
对Tk-ρ+1,...,Tk建立位姿图；
对位姿图进行优化，得到优化后的Tk-ρ+1,...,Tk。
实施例8：计算速度
通过Tk和To可求出从Lo向Lk的变换矩阵Tk←o；
若用角轴法和平移向量tk←o来表示Tk←o，则有旋转轴nk←o、旋转角度θk←o；
设两帧点云数据间的时间间隔为Δτ；
则有旋转速度为：
Figure BDA00032026566100001023
平移速度：
Figure BDA00032026566100001024

Claims (7)

1.一种基于改进ICP的点云测速方法，其特征在于，包括以下步骤：
步骤1、点云预处理：去除点云数据中的噪声点，通过聚类及检测手段，移除点云数据中的动态物体；
步骤2：点云的匹配：获得采集当前一帧点云时载体的位姿，并以此位姿为基础，利用改进的ICP方法，进一步优化当前一帧点云所对应的载体位姿；
步骤3：局部图优化：对包括最新计算所得位姿在内的给定时间间隔内的所有位姿进行局部图优化；
步骤4：计算速度：利用角轴法对载体经历的位姿变化进行表示，然后计算出载体目前的速度。
2.根据权利要求1所述的一种基于改进ICP的点云测速方法，其特征在于：步骤1所述点云预处理具体为：
设k时刻在载体坐标系Lk下采集到的一帧三维点云数据为
Figure FDA0003202656600000011
其中
Figure FDA0003202656600000012
表示点云数据中的第i个点，
Figure FDA0003202656600000013
分别表示点云数据中的第i个点在载体坐标系Lk下的坐标；
在
Figure FDA0003202656600000014
中，通过kd-tree算法计算出距离每个点最近的m个点以及该点到距离其最近的m个点的距离；
对于第i个点
Figure FDA0003202656600000015
距离其最近的m个点与点
Figure FDA0003202656600000016
之间的距离分别为：di1，di2，…，dim；
计算出这m个距离的平均值di-average，di-average为局部平均距离；
求出点
Figure FDA0003202656600000017
的m个临近点的局部平均距离di1-average，di2-average，…，dim-average，从di1-average，di2-average，…，dim-average中选出最小值dimin-average，然后比较di-average和dimin-average的大小；若di-average＞Kdimin-average，则判定
Figure FDA0003202656600000018
为噪声点；其中，K是一个给定参数；
预处理后得到的三维点云数据记为：
Figure FDA0003202656600000019
3.根据权利要求1或2所述的一种基于改进ICP的点云测速方法，其特征在于：步骤2所述点云的匹配具体为：
使用里程计或者RANSAC方法获得载体坐标系Lk在世界系W下的初始估计的位姿Tk-start；
设点云采样时刻k的上一采样时刻为o；
采集到的点云数据记为
Figure FDA0003202656600000021
预处理后的点云数据记为
Figure FDA0003202656600000022
时刻o对应的载体坐标系为Lo；
载体在世界系下的位姿为To；
利用改进的ICP方法对
Figure FDA0003202656600000023
和
Figure FDA0003202656600000024
处理，得到优化后的Tk-start，记为Tk；
所述改进的ICP方法具体为：
利用初始估计的位姿Tk-start和To将
Figure FDA0003202656600000025
变换到世界坐标系W下，对应的点云数据表示为
Figure FDA0003202656600000026
从
Figure FDA0003202656600000027
中选出位于平面上的点；
使用非极大值抑制的方法使选取的点分布均匀；
得到的标准点集记为
Figure FDA0003202656600000028
在
Figure FDA0003202656600000029
中搜索距离
Figure FDA00032026566000000210
中的每个点最近的λ个点；
从这λ个点中再筛选出距离
Figure FDA00032026566000000211
中相应点小于给定距离D1的点，得到点集
Figure FDA00032026566000000212
其中，l′表示上述筛选操作后，点集的数量；
从
Figure FDA00032026566000000213
中选出距离点
Figure FDA00032026566000000214
最近的λ个点，记为
Figure FDA00032026566000000215
再从
Figure FDA00032026566000000216
中选出距离点
Figure FDA00032026566000000217
小于给定距离D1的点集
Figure FDA00032026566000000218
其中γi表示
Figure FDA00032026566000000219
中点的数量；
其中距离
Figure FDA00032026566000000220
最近的一个点记为pnst-nbor，比较pnst-nbor与
Figure FDA00032026566000000221
两点处的法向量，若两法向量的夹角在区间[θt,180°-θt]，则将
Figure FDA00032026566000000222
从
Figure FDA00032026566000000223
中舍去，其中θt为给定值；
计算
Figure FDA00032026566000000224
中所有点与pnst-nbor的距离，若存在距离小于给定距离R1的情况，则将
Figure FDA00032026566000000225
从
Figure FDA00032026566000000226
中舍去；
计算
Figure FDA0003202656600000031
中每个点是否位于平面上；
若没有位于平面上，则将对应的点集剔除出
Figure FDA0003202656600000032
得到
Figure FDA0003202656600000033
其中，l″表示
Figure FDA0003202656600000034
中点集的数量；
对于
Figure FDA0003202656600000035
中的每个点集，计算这些点集中的点在
Figure FDA0003202656600000036
中的最近邻，若最近邻没有位于平面上，且最近邻距离
Figure FDA0003202656600000037
中相应点的距离超过给定距离R2，则将相应点集从
Figure FDA0003202656600000038
剔除；
且R2＞R1；
得到
Figure FDA0003202656600000039
相应的最近邻点集记为
Figure FDA00032026566000000310
Figure FDA00032026566000000311
每个点对应的法向量记为
Figure FDA00032026566000000312
然后将
Figure FDA00032026566000000313
中的点投影至标准点集
Figure FDA00032026566000000314
的对应点中，得到投影点集
Figure FDA00032026566000000315
计算
Figure FDA00032026566000000316
与
Figure FDA00032026566000000317
中对应点的距离，得到误差函数error(Tk-start)；
通过非线性优化的方法对error(Tk-start)进行优化，得到ΔT；
对Tk-start更新，即Tk-start＝ΔT·Tk-start；
其中，ΔT即为更新量；
而后继续对新的Tk-start进行迭代优化，当||ΔT||2＜Tmin或已到达迭代次数nt，则停止迭代，并将最终得到的Tk-start记为Tk，其中，Tmin和nt为给定值。
4.根据权利要求3所述的一种基于改进ICP的点云测速方法，其特征在于：所述位于平面上的判断方法具体为：
对于点集P，取其中一点pi，计算以pi为中心，以R3为半径的球体所包含的所有点
Figure FDA00032026566000000318
其中，R3＞R2；
计算出μi、Σi；
其中，
Figure FDA0003202656600000041
Figure FDA0003202656600000042
对Σi进行奇异值分解得到
Figure FDA0003202656600000043
定义曲率为：
Figure FDA0003202656600000044
若α＜αthreshold，且λ1＜δλ2，则认为pi位于平面上，αthreshold为给定的曲率阈值；
否则，认为pi没有位于平面上；
其中，0＜δ＜1。
5.根据权利要求3所述的一种基于改进ICP的点云测速方法，其特征在于：所述将
Figure FDA0003202656600000045
中的点投影至标准点集
Figure FDA0003202656600000046
的对应点中具体为：
对于
Figure FDA0003202656600000047
的第i个点集
Figure FDA0003202656600000048
从
Figure FDA0003202656600000049
中取出第j个点
Figure FDA00032026566000000410
Figure FDA00032026566000000411
在
Figure FDA00032026566000000412
中的最近邻为
Figure FDA00032026566000000413
Figure FDA00032026566000000414
处的法向量为
Figure FDA00032026566000000415
计算
Figure FDA00032026566000000416
距离
Figure FDA00032026566000000417
点邻域所形成平面的距离
Figure FDA00032026566000000418
得到
Figure FDA00032026566000000419
在
Figure FDA00032026566000000420
中的投影
Figure FDA00032026566000000421
所述距离
Figure FDA00032026566000000422
点邻域所形成平面的距离：
设
Figure FDA00032026566000000423
邻域的点集为
Figure FDA00032026566000000424
则
Figure FDA00032026566000000425
其中：
Figure FDA00032026566000000426
6.根据权利要求3所述的一种基于改进ICP的点云测速方法，其特征在于：所述误差函数error(Tk-start)满足：
对于
Figure FDA0003202656600000051
其对应的误差部分为
Figure FDA0003202656600000052
则
Figure FDA0003202656600000053
7.根据权利要求1或2所述的一种基于改进ICP的点云测速方法，其特征在于：步骤4所述计算速度具体为：
通过Tk和To可求出从Lo向Lk的变换矩阵Tk←o；
若用角轴法和平移向量tk←o来表示Tk←o，则有旋转轴nk←o、旋转角度θk←o；
设两帧点云数据间的时间间隔为Δτ；
则有旋转速度为：
Figure FDA0003202656600000054
平移速度：
Figure FDA0003202656600000055
CN202110908246.9A 2021-08-09 2021-08-09 一种基于改进icp的点云测速方法 Active CN113702941B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110908246.9A CN113702941B (zh) 2021-08-09 2021-08-09 一种基于改进icp的点云测速方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110908246.9A CN113702941B (zh) 2021-08-09 2021-08-09 一种基于改进icp的点云测速方法

Publications (2)

Publication Number Publication Date
CN113702941A true CN113702941A (zh) 2021-11-26
CN113702941B CN113702941B (zh) 2023-10-13

Family

ID=78651954

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110908246.9A Active CN113702941B (zh) 2021-08-09 2021-08-09 一种基于改进icp的点云测速方法

Country Status (1)

Country Link
CN (1) CN113702941B (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114511673A (zh) * 2022-01-26 2022-05-17 哈尔滨工程大学 一种基于改进icp的海底局部环境初步构建方法

Family Cites Families (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104851094A (zh) * 2015-05-14 2015-08-19 西安电子科技大学 一种基于rgb-d的slam算法的改进方法
CN106600639B (zh) * 2016-12-09 2019-06-25 江南大学 遗传算法结合自适应阈值约束的icp位姿定位技术
CN107220995B (zh) * 2017-04-21 2020-01-03 西安交通大学 一种基于orb图像特征的icp快速点云配准算法的改进方法
CN109961506B (zh) * 2019-03-13 2023-05-02 东南大学 一种融合改进Census图的局部场景三维重建方法
CN112966542A (zh) * 2020-12-10 2021-06-15 武汉工程大学 一种基于激光雷达的slam系统和方法
CN113012212B (zh) * 2021-04-02 2024-04-16 西北农林科技大学 一种基于深度信息融合的室内场景三维点云重建方法和系统

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114511673A (zh) * 2022-01-26 2022-05-17 哈尔滨工程大学 一种基于改进icp的海底局部环境初步构建方法
CN114511673B (zh) * 2022-01-26 2022-12-09 哈尔滨工程大学 一种基于改进icp的海底局部环境初步构建方法

Also Published As

Publication number Publication date
CN113702941B (zh) 2023-10-13

Similar Documents

Publication Publication Date Title
JP5539680B2 (ja) 線形特徴を用いた対象追跡
CN108762309B (zh) 一种基于假设卡尔曼滤波的人体目标跟随方法
CN111521195B (zh) 一种智能机器人
CN107169972B (zh) 一种非合作目标快速轮廓跟踪方法
CN109727273B (zh) 一种基于车载鱼眼相机的移动目标检测方法
CN110487286B (zh) 基于点特征投影与激光点云融合的机器人位姿判断方法
JP2015522200A (ja) 人顔特徴点の位置決め方法、装置及び記憶媒体
CN112052802A (zh) 一种基于机器视觉的前方车辆行为识别方法
CN113012197B (zh) 一种适用于动态交通场景的双目视觉里程计定位方法
CN110187337B (zh) 一种基于ls和neu-ecef时空配准的高机动目标跟踪方法及系统
CN111914832B (zh) 一种rgb-d相机在动态场景下的slam方法
CN116643291A (zh) 一种视觉与激光雷达联合剔除动态目标的slam方法
Naujoks et al. Fast 3D extended target tracking using NURBS surfaces
Qing et al. A novel particle filter implementation for a multiple-vehicle detection and tracking system using tail light segmentation
CN113689459B (zh) 动态环境下基于gmm结合yolo实时跟踪与建图方法
CN113702941A (zh) 一种基于改进icp的点云测速方法
CN113191427B (zh) 一种多目标车辆跟踪方法及相关装置
CN108469729B (zh) 一种基于rgb-d信息的人体目标识别与跟随方法
KR20200102108A (ko) 차량의 객체 검출 장치 및 방법
CN113362358A (zh) 一种动态场景下基于实例分割的鲁棒性位姿估计方法
CN113029185A (zh) 众包式高精度地图更新中道路标线变化检测方法及系统
CN112308917A (zh) 一种基于视觉的移动机器人定位方法
CN111474560A (zh) 一种障碍物定位方法、装置及设备
CN109903266B (zh) 一种基于样本窗的双核密度估计实时背景建模方法及装置
CN115359089A (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
GR01 Patent grant
GR01 Patent grant