一种基于改进ICP的点云测速方法与流程

一种基于改进icp的点云测速方法
技术领域
1.本发明属于导航技术领域，具体涉及一种基于改进icp的点云测速方法。


背景技术：

2.随着激光雷达等传感器技术的快速发展，点云数据在建图、导航和避障等领域的应用已经成为研究热点之一，而如果将点云数据用在测量载体速度方面，那么就可以更加充分的利用点云信息。对于仅拥有采集点云信息传感器的载体来说，利用点云数据来测得载体速度可以说是唯一的测速手段。同时，在状态估计中，通过提供更多的信息有利于导航系统对所估计状态更好的估计。此外，icp(iterative closest point)在传统点云配准方法中有着较为重要的意义，但普通icp方法存在引入误差过大的问题，故对普通icp方法进行了改进。


技术实现要素：

3.针对上述现有技术，本发明要解决的技术问题是提供一种配准结果更为准确的基于改进icp的点云测速方法，用于为装有采集点云数据传感器的载体提供一种测量速度的方法。
4.为解决上述技术问题，本发明的一种基于改进icp的点云测速方法，包括以下步骤：
5.步骤1、点云预处理：去除点云数据中的噪声点，通过聚类及检测手段，移除点云数据中的动态物体；
6.步骤2：点云的匹配：获得采集当前一帧点云时载体的位姿，并以此位姿为基础，利用改进的icp方法，进一步优化当前一帧点云所对应的载体位姿；
7.步骤3：局部图优化：对包括最新计算所得位姿在内的给定时间间隔内的所有位姿进行局部图优化；
8.步骤4：计算速度：利用角轴法对载体经历的位姿变化进行表示，然后计算出载体目前的速度。
9.本发明还包括：
10.1.步骤1中点云预处理具体为：
11.设k时刻在载体坐标系l
k
下采集到的一帧三维点云数据为其中表示点云数据中的第i个点，分别表示点云数据中的第i个点在载体坐标系l
k
下的坐标；
12.在中，通过kd
‑
tree算法计算出距离每个点最近的m个点以及该点到距离其最近的m个点的距离；
13.对于第i个点距离其最近的m个点与点之间的距离分别为：d
i1
，d
i2
，
…
，d
im
；
14.计算出这m个距离的平均值d
i
‑
average
，d
i
‑
average
为局部平均距离；
15.求出点的m个临近点的局部平均距离d
i1
‑
average
，d
i2
‑
average
，
…
，d
im
‑
average
，从d
i1
‑
average
，d
i2
‑
average
，
…
，d
im
‑
average
中选出最小值d
imin
‑
average
，然后比较d
i
‑
average
和d
imin
‑
average
的大小；若d
i
‑
average
＞kd
imin
‑
average
，则判定为噪声点；其中，k是一个给定参数；
16.预处理后得到的三维点云数据记为：
17.2.步骤2中点云的匹配具体为：
18.使用里程计或者ransac方法获得载体坐标系l
k
在世界系w下的初始估计的位姿t
k
‑
start
；
19.设点云采样时刻k的上一采样时刻为o；
20.采集到的点云数据记为
21.预处理后的点云数据记为
22.时刻o对应的载体坐标系为l
o
；
23.载体在世界系下的位姿为t
o
；
24.利用改进的icp方法对和处理，得到优化后的t
k
‑
start
，记为t
k
；
25.所述改进的icp方法具体为：
26.利用初始估计的位姿t
k
‑
start
和t
o
将变换到世界坐标系w下，对应的点云数据表示为
27.从中选出位于平面上的点；
28.使用非极大值抑制的方法使选取的点分布均匀；
29.得到的标准点集记为
30.在中搜索距离中的每个点最近的λ个点；
31.从这λ个点中再筛选出距离中相应点小于给定距离d1的点，得到点集其中，l
′
表示上述筛选操作后，点集的数量；
32.从中选出距离点最近的λ个点，记为
33.再从中选出距离点小于给定距离d1的点集其中γi表示中点的数量；
34.其中距离最近的一个点记为p
nst
‑
nbor
，比较p
nst
‑
nbor
与两点处的法向量，若两法向量的夹角在区间[θ
t
,180
°‑
θ
t
]，则将从中舍去，其中θ
t
为给定值；
[0035]
计算中所有点与p
nst
‑
nbor
的距离，若存在距离小于给定距离r1的情况，则将
从中舍去；
[0036]
计算中每个点是否位于平面上；
[0037]
若没有位于平面上，则将对应的点集剔除出
[0038]
得到其中，l
″
表示中点集的数量；
[0039]
对于中的每个点集，计算这些点集中的点在中的最近邻，若最近邻没有位于平面上，且最近邻距离中相应点的距离超过给定距离r2，则将相应点集从剔除；
[0040]
且r2＞r1；
[0041]
得到
[0042]
相应的最近邻点集记为
[0043]
每个点对应的法向量记为
[0044]
然后将中的点投影至标准点集的对应点中，得到投影点集
[0045]
计算与中对应点的距离，得到误差函数error(t
k
‑
start
)；
[0046]
通过非线性优化的方法对error(t
k
‑
start
)进行优化，得到δt；
[0047]
对t
k
‑
start
更新，即t
k
‑
start
＝δt
·
t
k
‑
start
；
[0048]
其中，δt即为更新量；
[0049]
而后继续对新的t
k
‑
start
进行迭代优化，当||δt||2＜t
min
或已到达迭代次数n
t
，则停止迭代，并将最终得到的t
k
‑
start
记为t
k
，其中，t
min
和n
t
为给定值。
[0050]
3.位于平面上的判断方法具体为：
[0051]
对于点集p，取其中一点p
i
，计算以p
i
为中心，以r3为半径的球体所包含的所有点
[0052]
其中，r3＞r2；
[0053]
计算出μ
i
、σ
i
；
[0054]
其中，
[0055][0056]
对σ
i
进行奇异值分解得到
[0057]
定义曲率为：
[0058]
若α＜α
threshold
，且λ1＜δλ2，则认为p
i
位于平面上，α
threshold
为给定的曲率阈值；
[0059]
否则，认为p
i
没有位于平面上；
[0060]
其中，0＜δ＜1。
[0061]
4.将中的点投影至标准点集的对应点中具体为：
[0062]
对于的第i个点集从中取出第j个点
[0063]
在中的最近邻为处的法向量为
[0064]
计算距离点邻域所形成平面的距离
[0065]
得到在中的投影
[0066]
所述距离点邻域所形成平面的距离：
[0067]
设邻域的点集为
[0068]
则
[0069]
其中：
[0070]
5.误差函数error(t
k
‑
start
)满足：
[0071]
对于其对应的误差部分为
[0072]
则
[0073]
6.步骤4中计算速度具体为：
[0074]
通过t
k
和t
o
可求出从l
o
向l
k
的变换矩阵t
k
←
o
；
[0075]
若用角轴法和平移向量t
k
←
o
来表示t
k
←
o
，则有旋转轴n
k
←
o
、旋转角度θ
k
←
o
；
[0076]
设两帧点云数据间的时间间隔为δτ；
[0077]
则有旋转速度为：
[0078]
平移速度：
[0079]
本发明的有益效果：与现有技术相比，本发明具有如下的有益效果：
[0080]
1、本发明的三维点云预处理可有效的去除初始点云数据中的离群点和毛刺点，大大减轻噪声点对后续匹配步骤的影响。
[0081]
2、本发明所采用的小区域平面投影至大区域平面，极大的使投影效果贴近真实情况，使得配准结果更为准确。
[0082]
3、本发明利用点云数据对载体的速度进行了估计。
附图说明
[0083]
图1为本发明测速方法的流程图；
[0084]
图2为关于离群点和毛刺的介绍。
具体实施方式
[0085]
下面结合说明书附图和具体实施方式对本发明做进一步说明。
[0086]
以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变化和改进。这些都属于本发明的保护范围。
[0087]
如图1所示，一种基于改进icp的点云测速方法，包括如下步骤：
[0088]
点云预处理步骤：去除点云数据中的噪声点，通过聚类及检测手段，移除点云数据中的动态物体；
[0089]
点云的匹配步骤：通过其他手段初步获得采集当前一帧点云时，载体的位姿，并以此位姿为基础，利用改进的icp方法，进一步优化当前一帧点云所对应的载体位姿；
[0090]
局部图优化步骤：对包括最新计算所得位姿在内的一定时间间隔内的所有位姿进行优化；
[0091]
计算速度步骤：利用角轴法对载体经历的位姿变化进行表示，然后计算出载体目前的速度。
[0092]
实施例1：三维点云预处理步骤
[0093]
设k时刻在载体坐标系l
k
下采集到的一帧三维点云数据为其中表示点云数据中的第i个点，分别表示点云数据中的第i个点在载体坐标系l
k
下的坐标；
[0094]
在中，通过kd
‑
tree算法计算出距离每个点最近的m个点以及该点到距离其最近的m个点的距离；
[0095]
对于第i个点距离其最近的m个点与点之间的距离分别为：d
i1
，d
i2
，
…
，d
im
；
[0096]
计算出这m个距离的平均值d
i
‑
average
，可称d
i
‑
average
为局部平均距离；
[0097]
同理也可求出点的m个临近点的局部平均距离d
i1
‑
average
，d
i2
‑
average
，
…
，d
im
‑
average
。从d
i1
‑
average
，d
i2
‑
average
，
…
，d
im
‑
average
中选出最小值d
imin
‑
average
，然后比较d
i
‑
average
和d
imin
‑
average
的大小；若d
i
‑
average
＞kd
imin
‑
average
，则判定为噪声点；
[0098]
其中，k是一个待调节的参数；
[0099]
预处理后得到的三维点云数据记为：
[0100]
实施例2：三维点云的匹配
[0101]
使用里程计或者ransac方法获得载体坐标系l
k
在世界系w下的初始估计的位姿t
k
‑
start
；
[0102]
设点云采样时刻k的上一采样时刻为o；
[0103]
采集到的点云数据记为
[0104]
预处理后的点云数据记为
[0105]
时刻o对应的载体坐标系为l
o
；
[0106]
载体在世界系下的位姿为t
o
；
[0107]
利用改进的icp方法对和处理，得到优化后的t
k
‑
start
，记为t
k
。
[0108]
实施例3：改进的icp
[0109]
使用里程计或者ransac方法获得载体坐标系l
k
在世界系w下的初始估计的位姿t
k
‑
start
；
[0110]
设点云采样时刻k的上一采样时刻为o；
[0111]
采集到的点云数据记为
[0112]
预处理后的点云数据记为
[0113]
时刻o对应的载体坐标系为l
o
；
[0114]
载体在世界系下的位姿为t
o
；
[0115]
利用t
k
‑
start
和t
o
将变换到世界坐标系w下，对应的点云数据表示为
[0116]
从中选出位于平面上的点；
[0117]
使用非极大值抑制的方法使选取的点分布均匀；
[0118]
得到的标准点集记为
[0119]
在中进行搜索操作，对于中的任意一点p
j
，在中找到距离p
j
最近的λ个点；
[0120]
从这λ个点中再筛选出距离p
j
小于d1的点；
[0121]
经由上述两步筛选操作后，得到点集
[0122]
其中，l
′
表示上述筛选操作后，点集的数量；
[0123]
从中选出距离点最近的λ个点，记为
[0124]
再从中选出距离点小于d1的点集
[0125]
其中γi表示中点的数量；
[0126]
其中距离最近的一个点记为p
nst
‑
nbor
，比较p
nst
‑
nbor
与两点处的法向量，若两法向量的夹角在[θ
t
,180
°‑
θ
t
]之间，则将从中舍去，其中θ
t
为给定值，θ
t
可以取值20
°
；
[0127]
计算中所有点与p
nst
‑
nbor
的距离，若存在距离小于r1的情况，则将从中
舍去；
[0128]
计算中每个点是否位于平面上；
[0129]
若没有位于平面上，则将对应的点集剔除出
[0130]
得到
[0131]
其中，l
″
表示中点集的数量；
[0132]
对于中的每个点集，还应计算这些点集中的点在中的最近邻，若最近邻没有位于平面上，且最近邻距离中相应点的距离超过r2，则将相应点集从剔除；
[0133]
且r2＞r1；
[0134]
得到
[0135]
相应的最近邻点集记为
[0136]
每个点对应的法向量记为
[0137]
然后将中的点投影至标准点集的对应点中，得到投影点集
[0138]
计算与中对应点的距离，得到误差函数error(t
k
‑
start
)；
[0139]
通过非线性优化的方法对error(t
k
‑
start
)进行优化，得到δt；
[0140]
对t
k
‑
start
更新，即t
k
‑
start
＝δt
·
t
k
‑
start
；
[0141]
其中，δt即为更新量；
[0142]
而后继续对新的t
k
‑
start
利用改进的icp方法进行迭代优化；
[0143]
当||δt||2＜t
min
或已到达迭代次数n
t
，则停止迭代，并将最终得到的t
k
‑
start
记为t
k
；
[0144]
其中，t
min
和n
t
为给定值。
[0145]
实施例4：位于平面上
[0146]
对于点集p，取其中一点p
i
，计算以p
i
为中心，以r3为半径的球体所包含的所有点
[0147]
其中，r3＞r2；
[0148]
计算出μ
i
、σ
i
；
[0149]
其中，
[0150]
[0151]
对σ
i
进行奇异值分解得到定义曲率为：
[0152]
若α＜α
threshold
，且λ1＜δλ2，则认为p
i
位于平面上；
[0153]
否则，认为p
i
没有位于平面上；
[0154]
其中，0＜δ＜1。
[0155]
实施例5：所述点投影至标准点集
[0156]
对于的第i个点集从中取出第j个点
[0157]
在中的最近邻为处的法向量为
[0158]
计算距离点邻域所形成平面的距离
[0159]
得到在中的投影
[0160]
所述距离点邻域所形成平面的距离：
[0161]
设邻域的点集为
[0162]
则
[0163]
其中：
[0164]
实施例6：误差函数
[0165]
对于其对应的误差部分为
[0166]
则
[0167]
实施例7：局部图优化
[0168]
对于从k往前的ρ帧有效激光数据，可得到ρ个位姿t
k
‑
ρ 1
,...,t
k
；
[0169]
对t
k
‑
ρ 1
,...,t
k
建立位姿图；
[0170]
对位姿图进行优化，得到优化后的t
k
‑
ρ 1
,...,t
k
。
[0171]
实施例8：计算速度
[0172]
通过t
k
和t
o
可求出从l
o
向l
k
的变换矩阵t
k
←
o
；
[0173]
若用角轴法和平移向量t
k
←
o
来表示t
k
←
o
，则有旋转轴n
k
←
o
、旋转角度θ
k
←
o
；
[0174]
设两帧点云数据间的时间间隔为δτ；
[0175]
则有旋转速度为：
[0176]
平移速度：