一种多线激光雷达稀疏点云的表面法向量估计方法

1.本发明属于雷达数据处理技术领域，具体涉及一种多线激光雷达稀疏点云的表面法向量估计方法。


背景技术：

2.激光雷达通过向目标发射激光束并接收目标反射回的光线，经信号处理后，得到目标的方位、距离和反射率等信息。激光雷达具有测距精度高、受光照影响小、可昼夜工作等优点，目前已广泛应用于自动驾驶领域的研究。自动驾驶汽车一般搭载多线激光雷达对周围环境进行扫描，以获取汽车周围三维空间的点云信息。通过对点云进行分析处理，自动驾驶系统可获得周围环境的可通行区域、障碍物等信息。
3.点云的表面法向量是一种十分重要的点云特征信息，其可用于三维点云的分析处理，如点云分割、表面重建、障碍物识别等。传统的点云表面法向量估计方法一般通过构建kdtree(k-d树)将无序点云有序化，基于k近邻搜索算法，找到待估计点的某一距离阈值内最近邻点，再基于搜索到的最近邻点，构建点云表面的mesh网络进而计算曲面的法向量，或计算由最近邻点生成的协方差矩阵的特征向量和特征值，从而得到待估计点的表面法向量。然而，上述方法仅适用于点云分布较为均匀且稠密的点云数据。自动驾驶汽车所搭载的多线激光雷达的水平和垂直方向角分辨率差别很大，其获取到的点云数据分布不均匀且较为稀疏，将上述点云表面法向量估计方法用于多线激光雷达稀疏点云的表面法向量估计，效果往往较差。


技术实现要素：

4.针对上述问题，本发明提供了一种多线激光雷达稀疏点云的表面法向量估计方法，具体包含以下步骤：
5.s1.获取多线激光雷达对周围环境扫描所产生的一帧点云数据，将数据按按扫描线进行分组，对于同一条扫描线的数据，根据其扫描时间的先后顺序进行排序；对于不同组的数据，按所在扫描线的高低角度，由低到高进行排序。排序完成的数据如下：
6.p
1，1
，p
1，2
，...，p
1，n
，p
2，1
，p
2，2
，...，p
2，n
，...，p
m，1
，p
m，2
，...，p
m，n
7.其中，m为多线激光雷达的线数，n为一帧中每条扫描线的点数，p
l，i
为第l扫描线上的第i个点，对没有反射的点，存储为无效值nan。
8.s2.对于一帧点云数据中每一个有效测量点p
l，i
，计算扫描线l在点p
l，i
处的切线向量，具体步骤为：
9.s2.1以待计算点p
l,i
为中心，分别沿扫描线向后向前查找点其中，其中，应满足如下条件：(1)与p
l，i
的空间距离大于阈值d
threshold
；(2)；(2)与p
l，i
的下标距离|i-i
prev/next
|小于等于i
threshold
，或与p
l，i
的空间距离小于等于5d
threshold
。若无法所搜到满足条件1和条件2的点，则
或记为nan；阈值i
threshold
建议取值2～3。阈值d
threshold
根据激光雷达测测距误差取值，建议设置为测距误差的1.5～2倍；
10.s2.2根据s2.1的返回结果，分别计算p
l，i
与和构成的切线向量构成的切线向量和其中，normalize(
·
)为归一化函数。若或为nan，则对应的v1或v2为nan；
11.s2.3通过v1和v2计算扫描线l在点p
l，i
处的切线向量(1)若v1和v2均为nan，则p
l，i
的切向量无法计算，结束该点的表面法向量计算，返回s2继续计算下一个点的法向量；(2)若v1有效且v2为nan，则(3)若v2有效且v1为nan，则(4)若v1和v2都不为nan，首先计算向量v1和v2的夹角θ＝acos(v1·
v2)，如果θ小于阈值θ
threshold
，则否则，阈值θ
threshold
的值取决于期望对边缘点的表面法向量进行平滑的程度，通常情况下，建议取值范围为30～50
°
。
12.s3在相邻扫描线上查找p
l，i
的2个近邻点，并通过近邻点计算p
l，i
的2条副切线向量，具体步骤为：
13.s3.1记当前扫描线序号j
←
l-1；
14.s3.2判断扫描线j上与p
l，i
同一水平扫描方向的点p
j,i
是否为有效值，若无效，转步骤s3.3，否则，并转步骤s3.4；
15.s3.3在扫描线j上，以p
j，i
中心，搜索前后i
threshold
个点内的有效点p
j,k
，若找到，转步骤s3.4，否则记为nan并步骤s3.6；
16.s3.4判断p
l，i
与的空间距离是否满足条件若不满足，转步骤s3.5，否则，转s3.6。
17.s3.5j
←
j-1，重复步骤s3.2-s3.4；
18.s3.6若不为nan，则点p
l，i
的第一副切线向量否则，v
⊥1记为nan；
19.s3.7记当前扫描线序号j
←
l+1，执行s3.2-s3.6类似的步骤，查找并计算点p
l，i
的第二副切线向量v
⊥2，区别在于：1)步骤s3.2-s3.6中的变为2)步骤s3.5的j
←
j-1变为j
←
j+1；3)第二副切向量计算：
20.s4将s3得到的点p
l，i
的2条副切线向量v
⊥1、v
⊥2与p
l，i
的切线向量进行叉乘计算，得到p
l，i
的2条表面法向量估计若v
⊥1或v
⊥2为nan，则对应的n1或n2为nan。
21.s5检查法向量估计值n1、n2的方向，若其与雷达的激光发射方向夹角小于90度，则取反方向。
22.s6若n1和n2都不为nan，计算n1、n2的夹角θ＝acos(n1
·
n2)，若θ＜θ
threshold
，则p
l，i
的表面法向量n
l，i
＝normalize(n1+n2)，否则，n
l，i
←
n1；若n1和n2其中一个为nan，在n
l，i
取值n1或n2有效的向量；若n1和n2全部为nan，在n
l，i
不可估计，记为nan。
23.本发明的有益效果是：
24.(1)本发明所提供方法充分利用了激光雷达扫描的结构信息，而不是把激光雷达当成无序点云，通过kd-tree有序化后滤波，一方面计算方法更加简单高效，另一方面算法具备搜索距离自适应功能，在不同距离、水平方向和垂直方向使用了不同的搜索距离，因此在多线稀疏点云上可获取更好的估计结果；
25.(2)所提供方法在计算过程中，考虑了激光的测量误差对向量方向估计带来的影响，故可实现更好的估计结果；
26.(3)所提供方法在计算时考虑了对边缘点表面法向量的处理，当检测到某个点两侧的切向量或者法向量方向角度差超过阈值时，仅保留其中一侧的结果，避免了平均对边缘造成的平滑处理，故可较好的保留边缘点表面法向量不连续的特性。
附图说明
27.图1是本发明实施例的方法流程图
28.图2是本发明实施例的扫描线切线向量、副切线向量及表面法线示意图
具体实施方式
29.下面结合附图，并以velodyne生产的16线激光雷达vlp-16为例，对本发明作进一步说明，构成本发明的一部分。需要注意的是，本发明所提供的方法可用于各类型号的多线激光雷达，本实施例以vlp-16为例对算法进行补充说明，并不表示本发明所提供的方法仅适用于vlp-16型号的激光雷动。如图1所示，本发明实施例提供一种3d激光雷达稀疏点云的表面法向量估计方法，技术流程如下：
30.s1.获取多线激光雷达对周围环境扫描所产生的一帧点云数据，在接收激光雷达发送的原始数据包，并进行解析时，记录每个激光点所属的扫描线，并将数据按按扫描线进行分组，对于同一条扫描线的数据，根据其扫描时间的先后顺序进行排序；对于不同组的数据，按所在扫描线的高低角度，由低到高进行排序。排序完成的数据如下：
31.p
1,1
，p
1,2
，...，p
1,n
，p
2，1
，p
2,2
，...，p
2,n
，...，p
m，1
，p
m,2
，...，p
m,n
32.其中，m为多线激光雷达的线数，n为一帧中每条扫描线的点数p
l，i
为扫描线l上的第i个点，对没有反射的点，存储为无效值nan。本实施例中使用16线激光雷达，故m＝16；激光雷达转速约600
±
3rpm，每旋转一圈产生75.35个数据包，向上取整得到76，每个数据包含24组循环发射的数据，故一帧数据中，每条扫描线的点数n＝76
×
24＝1824；需要注意的是，vlp-16的16个激光发射头的发射顺序并不是按照高低角度依次发射，在对点云按扫描线高低角进行排序时，需依据velodyne提供的vlp-16用户手册中所述的激光发射头id号与高低角度的关系，对点云数据按下标进行正确的抽取和排序。
33.s2.如图2所示，对于一帧点云数据中每一个有效测量点p
l，i
，计算扫描线l在点p
l，i
处的切线向量，具体步骤为：
34.s2.1以待计算点p
l，i
为中心，分别沿扫描线向后向前查找点其中，其中，应满足如下条件：1)与p
l，i
的空间距离大于阈值d
threshold
；2)与p
l，i
的下标距离|i-i
prev/next
|小于等于i
threshold
，或与p
l，i
的
空间距离小于等于5d
threshold
。若无法所搜到满足条件1和条件2的点，则或记为nan。阈值i
threshold
建议取值2。阈值d
threshold
根据激光雷达测测距误差取值，建议为测距误差的1.5～2倍。本实施例中，vlp-16的测距误差为
±
0.03m，d
threshold
取值0.05m；
35.s2.2根据s2.1的返回结果，分别计算p
l，i
与和构成的切线向量构成的切线向量和其中，normalize(
·
)为归一化函数。若或为nan，则对应的v1或v2为nan；
36.s2.3通过v1和v2计算扫描线l在点p
l，i
处的切线向量1)若v1和v2均为nan，则p
l，i
的切向量无法计算，结束该点的表面法向量计算，返回s2继续计算下一个点的法向量；2)若v1有效且v2为nan，则3)若v2有效且v1为nan，则3)若v1和v2都不为nan，首先计算向量v1和v2的夹角θ＝acos(v1·
v2)，如果θ小于阈值θ
threshold
，则否则，阈值θ
threshold
的值取决于期望对边缘点的表面法向量进行平滑的程度，通常情况下，建议取值范围为30～50
°
。注意，为了避免反复调用反三角函数进行计算，在具体实施过程中，提前计算c
θ
＝cos(θ
threshold
)，并直接计算并判断v1·
v2≥c
θ
，以较少计算量并提高算法实时性。
37.s3在相邻扫描线上查找2个满足条件的p
l,i
近邻点，通过近邻点估计p
l，i
的2条副切线向量，具体步骤为：
38.s3.1记当前扫描线序号j
←
l-1；
39.s3.2判断扫描线j上与p
l，i
同一水平扫描方向的点p
j,i
是否为有效值，若无效，转步骤s3.3，否则，并转步骤s3.4；
40.s3.3在扫描线j上，以p
j，i
中心，搜索前后i
threshold
个点内的有效点p
j,k
，若找到，转步骤s3.4，否则记为nan并步骤s3.6；
41.s3.4判断p
l,i
与的空间距离是否满足条件若不满足，转步骤s3.5，否则，转s3.6；
42.s3.5j
←
j-1，重复步骤s3.2-s3.4；
43.s3.6若不为nan，则点p
l，i
的第一副切线向量否则，v
⊥1记为nan；
44.s3.7记当前扫描线序号j
←
l+1，执行s3.2至s3.6类似的步骤，查找并计算点p
l，i
的第二副切线向量v
⊥2，区别在于：1)步骤s3.2至s3.6中的变为2)步骤s3.5的j
←
j-1变为j
←
j+1；3)第二副切向量计算：
45.s4如图2所示，将s3得到的点p
l，i
的2个副切线向量v
⊥1、v
⊥2与p
l，i
的切线向量进行叉乘计算，得到p
l，i
的2条表面法向量估计若v
⊥1或v
⊥2为nan，则对应的n1或n2为nan。
46.s5检查法向量估计值n1、n2的方向，若其与雷达的激光发射方向夹角小于90度，则取反方向。具体实现方法：定义雷达的激光发射方向为r
io
，则有r
io
＝normalize(p
l，i
)，计算
ni·rio
，若结果大于0，则对应ni取反方向，前述中，i为1或2。
47.s6若n1和n2都不为nan，则如图2所示，计算n1、n2的夹角θ＝acos(n1
·
n2)，若θ＜θ
threshold
，则p
l，i
的表面法向量n
l，i
＝normalize(n1+n2)，否则，n
l，i
←
n1；若n1和n2其中一个为nan，在n
l，i
取值n1或n2有效的向量；若n1和n2全部为nan，在n
l，i
不可估计，记为nan。同样的，具体实施时，角度判断使用s2.3同样的技巧以提高算法实时性。
48.在具体实施过程中，会碰到一些边界点，如第一条扫描线并没有对应的上一条扫描线，即无法查找并计算v
⊥1，对这些边界点的处理方法如下：
49.(1)对于第1条扫描线的点，跳过步骤s3.1-s3.7，并将v
⊥1记为nan。
50.(2)对于第16条扫描线上的点，跳过步骤s3.8，并记v
⊥2为nan。
51.(3)对于每条扫描线的第一个点，在步骤s2.1或s3.1中以其为中心查找往前相关的点时，跳过相关查找流程记步骤s2.1或s3.1中对应的或p
j，k
为nan。
52.(4)对于每条扫描线的最后一个点，在步骤s2.1或s3.1中以其为中心查找往后相关的点时，跳过相关查找流程并记步骤s2.1或s3.1中对应的或p
j，k
为nan。
53.需要说明的是，上述边界点处理流程中，对于每条扫描线的第一个点或最后一个点，若扫描线为一圈360
°
的数据，理论上可以回环到描线最后一个点p
l,n
或第一个点p
l，1
进行查找，并获得更加精确的估计结果，但在具体实施时，该处理方式存在一定的问题，原因如下：1)vlp-16的水平分辨率约为0.199
°
，一圈360
°
每条扫描线产生的数据点数为360/0.199+1＝1810个，步骤s1中，获取激光雷达数据时，取整包数据，每条扫描线有1824个点，因此，扫描线最后一个点与第一个点在水平角度上并不连续；2)搭载激光雷达的移动平台在行进过程中会转弯，也会导致激光雷达旋转一周实际扫描过的水平角度不足(左转)或超过(右转)360
°
，即扫描线最后一个点与第一个点在水平角度上并不连续，因此无法形成回环读取并进行相关计算的条件。
54.应当理解的是，上述说明示出并描述了本发明的具体实施个例，并不用于限定本发明的保护范围，不应看作是对其他实例的排除。本发明的方法/实施例/权利要求中记载的特征可以进行多种组合或/或结合，即使这样的组合或结合没有明确记载于本发明说明中。特别地，在不脱离本公开的精神和和教导的情况下所做的任何等同替换或改进等，均应包含在本发明的保护范围之内。