一种基于改进icp的点云测速方法
阅读说明：本技术 一种基于改进icp的点云测速方法 (Point cloud speed measurement method based on improved ICP (inductively coupled plasma) ) 是由 杜雪 闫泽博 胡俊生 管凤旭 孙岩 张勋 陈涛 郑岩 于 2021-08-09 设计创作，主要内容包括：本发明公开了一种基于改进ICP的点云测速方法,包括：点云预处理：去除点云数据中的噪声点,通过聚类及检测手段,移除点云数据中的动态物体；点云的匹配：获得采集当前一帧点云时载体的位姿,并以此位姿为基础,利用改进的ICP方法,进一步优化当前一帧点云所对应的载体位姿；局部图优化：对包括最新计算所得位姿在内的给定时间间隔内的所有位姿进行局部图优化；计算速度：利用角轴法对载体经历的位姿变化进行表示,然后计算出载体目前的速度。本发明的三维点云预处理可去除初始点云数据中的离群点和毛刺点,大大减轻噪声点对后续匹配步骤的影响。采用的小区域平面投影至大区域平面,极大的使投影效果贴近真实情况,使得配准结果更为准确。(The invention discloses a point cloud speed measurement method based on improved ICP (inductively coupled plasma), which comprises the following steps: point cloud pretreatment: removing noise points in the point cloud data, and removing dynamic objects in the point cloud data through clustering and detection means; matching point clouds: acquiring the pose of the carrier when the current frame of point cloud is acquired, and further optimizing the pose of the carrier corresponding to the current frame of point cloud by using an improved ICP method on the basis of the pose; local graph optimization: performing local graph optimization on all poses in a given time interval including the pose obtained by the latest calculation; calculating the speed: and (3) representing the pose change experienced by the carrier by using an angular axis method, and then calculating the current speed of the carrier. The three-dimensional point cloud preprocessing of the invention can remove outliers and burr points in the initial point cloud data, and greatly reduce the influence of noise points on the subsequent matching steps. The adopted small area plane is projected to the large area plane, so that the projection effect is greatly close to the real situation, and the registration result is more accurate.)
技术领域
本发明属于导航技术领域，具体涉及一种基于改进ICP的点云测速方法。
背景技术
随着激光雷达等传感器技术的快速发展，点云数据在建图、导航和避障等领域的应用已经成为研究热点之一，而如果将点云数据用在测量载体速度方面，那么就可以更加充分的利用点云信息。对于仅拥有采集点云信息传感器的载体来说，利用点云数据来测得载体速度可以说是唯一的测速手段。同时，在状态估计中，通过提供更多的信息有利于导航系统对所估计状态更好的估计。此外，ICP(Iterative Closest Point)在传统点云配准方法中有着较为重要的意义，但普通ICP方法存在引入误差过大的问题，故对普通ICP方法进行了改进。
发明内容
针对上述现有技术，本发明要解决的技术问题是提供一种配准结果更为准确的基于改进ICP的点云测速方法，用于为装有采集点云数据传感器的载体提供一种测量速度的方法。
为解决上述技术问题，本发明的一种基于改进ICP的点云测速方法，包括以下步骤：
步骤1、点云预处理：去除点云数据中的噪声点，通过聚类及检测手段，移除点云数据中的动态物体；
步骤2：点云的匹配：获得采集当前一帧点云时载体的位姿，并以此位姿为基础，利用改进的ICP方法，进一步优化当前一帧点云所对应的载体位姿；
步骤3：局部图优化：对包括最新计算所得位姿在内的给定时间间隔内的所有位姿进行局部图优化；
步骤4：计算速度：利用角轴法对载体经历的位姿变化进行表示，然后计算出载体目前的速度。
本发明还包括：
1.步骤1中点云预处理具体为：
设k时刻在载体坐标系Lk下采集到的一帧三维点云数据为其中表示点云数据中的第i个点，分别表示点云数据中的第i个点在载体坐标系Lk下的坐标；
在中，通过kd-tree算法计算出距离每个点最近的m个点以及该点到距离其最近的m个点的距离；
对于第i个点距离其最近的m个点与点之间的距离分别为：di1，di2，…，dim；
计算出这m个距离的平均值di-average，di-average为局部平均距离；
求出点的m个临近点的局部平均距离di1-average，di2-average，…，dim-average，从di1-average，di2-average，…，dim-average中选出最小值dimin-average，然后比较di-average和dimin-average的大小；若di-average＞Kdimin-average，则判定为噪声点；其中，K是一个给定参数；
预处理后得到的三维点云数据记为：
2.步骤2中点云的匹配具体为：
使用里程计或者RANSAC方法获得载体坐标系Lk在世界系W下的初始估计的位姿Tk-start；
设点云采样时刻k的上一采样时刻为o；
采集到的点云数据记为
预处理后的点云数据记为
时刻o对应的载体坐标系为Lo；
载体在世界系下的位姿为To；
利用改进的ICP方法对和处理，得到优化后的Tk-start，记为Tk；
所述改进的ICP方法具体为：
利用初始估计的位姿Tk-start和To将变换到世界坐标系W下，对应的点云数据表示为
从中选出位于平面上的点；
使用非极大值抑制的方法使选取的点分布均匀；
得到的标准点集记为
在中搜索距离中的每个点最近的λ个点；
从这λ个点中再筛选出距离中相应点小于给定距离D1的点，得到点集其中，l′表示上述筛选操作后，点集的数量；
从中选出距离点最近的λ个点，记为
再从中选出距离点小于给定距离D1的点集其中γi表示中点的数量；
其中距离最近的一个点记为pnst-nbor，比较pnst-nbor与两点处的法向量，若两法向量的夹角在区间[θt,180°-θt]，则将从中舍去，其中θt为给定值；
计算中所有点与pnst-nbor的距离，若存在距离小于给定距离R1的情况，则将从中舍去；
计算中每个点是否位于平面上；
若没有位于平面上，则将对应的点集剔除出
得到其中，l″表示中点集的数量；
对于中的每个点集，计算这些点集中的点在中的最近邻，若最近邻没有位于平面上，且最近邻距离中相应点的距离超过给定距离R2，则将相应点集从剔除；
且R2＞R1；
得到
相应的最近邻点集记为
每个点对应的法向量记为
然后将中的点投影至标准点集的对应点中，得到投影点集
计算与中对应点的距离，得到误差函数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为半径的球体所包含的所有点
其中，R3＞R2；
计算出μi、Σi；
其中，
对Σi进行奇异值分解得到
定义曲率为：
若α＜αthreshold，且λ1＜δλ2，则认为pi位于平面上，αthreshold为给定的曲率阈值；
否则，认为pi没有位于平面上；
其中，0＜δ＜1。
4.将中的点投影至标准点集的对应点中具体为：
对于的第i个点集从中取出第j个点
在中的最近邻为处的法向量为
计算距离点邻域所形成平面的距离
得到在中的投影
所述距离点邻域所形成平面的距离：
设邻域的点集为
则
其中：
5.误差函数error(Tk-start)满足：
对于其对应的误差部分为
则
6.步骤4中计算速度具体为：
通过Tk和To可求出从Lo向Lk的变换矩阵Tk←o；
若用角轴法和平移向量tk←o来表示Tk←o，则有旋转轴nk←o、旋转角度θk←o；
设两帧点云数据间的时间间隔为Δτ；
则有旋转速度为：
平移速度：
本发明的有益效果：与现有技术相比，本发明具有如下的有益效果：
1、本发明的三维点云预处理可有效的去除初始点云数据中的离群点和毛刺点，大大减轻噪声点对后续匹配步骤的影响。
2、本发明所采用的小区域平面投影至大区域平面，极大的使投影效果贴近真实情况，使得配准结果更为准确。
3、本发明利用点云数据对载体的速度进行了估计。
附图说明
图1为本发明测速方法的流程图；
图2为关于离群点和毛刺的介绍。
具体实施方式
下面结合说明书附图和具体实施方式对本发明做进一步说明。
以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变化和改进。这些都属于本发明的保护范围。
如图1所示，一种基于改进ICP的点云测速方法，包括如下步骤：
点云预处理步骤：去除点云数据中的噪声点，通过聚类及检测手段，移除点云数据中的动态物体；
点云的匹配步骤：通过其他手段初步获得采集当前一帧点云时，载体的位姿，并以此位姿为基础，利用改进的ICP方法，进一步优化当前一帧点云所对应的载体位姿；
局部图优化步骤：对包括最新计算所得位姿在内的一定时间间隔内的所有位姿进行优化；
计算速度步骤：利用角轴法对载体经历的位姿变化进行表示，然后计算出载体目前的速度。
实施例1：三维点云预处理步骤
设k时刻在载体坐标系Lk下采集到的一帧三维点云数据为其中表示点云数据中的第i个点，分别表示点云数据中的第i个点在载体坐标系Lk下的坐标；
在中，通过kd-tree算法计算出距离每个点最近的m个点以及该点到距离其最近的m个点的距离；
对于第i个点距离其最近的m个点与点之间的距离分别为：di1，di2，…，dim；
计算出这m个距离的平均值di-average，可称di-average为局部平均距离；
同理也可求出点的m个临近点的局部平均距离di1-average，di2-average，…，dim-average。从di1-average，di2-average，…，dim-average中选出最小值dimin-average，然后比较di-average和dimin-average的大小；若di-average＞Kdimin-average，则判定为噪声点；
其中，K是一个待调节的参数；
预处理后得到的三维点云数据记为：
实施例2：三维点云的匹配
使用里程计或者RANSAC方法获得载体坐标系Lk在世界系W下的初始估计的位姿Tk-start；
设点云采样时刻k的上一采样时刻为o；
采集到的点云数据记为
预处理后的点云数据记为
时刻o对应的载体坐标系为Lo；
载体在世界系下的位姿为To；
利用改进的ICP方法对和处理，得到优化后的Tk-start，记为Tk。
实施例3：改进的ICP
使用里程计或者RANSAC方法获得载体坐标系Lk在世界系W下的初始估计的位姿Tk-start；
设点云采样时刻k的上一采样时刻为o；
采集到的点云数据记为
预处理后的点云数据记为
时刻o对应的载体坐标系为Lo；
载体在世界系下的位姿为To；
利用Tk-start和To将变换到世界坐标系W下，对应的点云数据表示为
从中选出位于平面上的点；
使用非极大值抑制的方法使选取的点分布均匀；
得到的标准点集记为
在中进行搜索操作，对于中的任意一点pj，在中找到距离pj最近的λ个点；
从这λ个点中再筛选出距离pj小于D1的点；
经由上述两步筛选操作后，得到点集
其中，l′表示上述筛选操作后，点集的数量；
从中选出距离点最近的λ个点，记为
再从中选出距离点小于D1的点集
其中γi表示中点的数量；
其中距离最近的一个点记为pnst-nbor，比较pnst-nbor与两点处的法向量，若两法向量的夹角在[θt,180°-θt]之间，则将从中舍去，其中θt为给定值，θt可以取值20°；
计算中所有点与pnst-nbor的距离，若存在距离小于R1的情况，则将从中舍去；
计算中每个点是否位于平面上；
若没有位于平面上，则将对应的点集剔除出
得到
其中，l″表示中点集的数量；
对于中的每个点集，还应计算这些点集中的点在中的最近邻，若最近邻没有位于平面上，且最近邻距离中相应点的距离超过R2，则将相应点集从剔除；
且R2＞R1；
得到
相应的最近邻点集记为
每个点对应的法向量记为
然后将中的点投影至标准点集的对应点中，得到投影点集
计算与中对应点的距离，得到误差函数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为半径的球体所包含的所有点
其中，R3＞R2；
计算出μi、Σi；
其中，
对Σi进行奇异值分解得到定义曲率为：
若α＜αthreshold，且λ1＜δλ2，则认为pi位于平面上；
否则，认为pi没有位于平面上；
其中，0＜δ＜1。
实施例5：所述点投影至标准点集
对于的第i个点集从中取出第j个点
在中的最近邻为处的法向量为
计算距离点邻域所形成平面的距离
得到在中的投影
所述距离点邻域所形成平面的距离：
设邻域的点集为
则
其中：
实施例6：误差函数
对于其对应的误差部分为
则
实施例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；
设两帧点云数据间的时间间隔为Δτ；
则有旋转速度为：
平移速度：
- 上一篇：一种医用注射器针头装配设备
- 下一篇：一种基于数据驱动的无人机激光雷达