一种应用于矿山非结构化道路场景下的边界线滤波方法

1.本发明涉及一种边界线滤波方法，具体是一种应用于矿山非结构化道路场景下的边界线滤波方法。


背景技术：

2.激光雷达目前已经广泛用于无人驾驶，以及地理测绘中，其主要的优点是可以生成三维位置信息，以快速确定对象的位置，大小，外观和材料，同时获取数据以形成准确的数字模型。与摄像机等传感器相比，检测距离更长，精度更高，响应速度更灵敏，并且环境光不受影响。但是目前激光雷达产生的三维点云信息，往往包含几十万甚至上百万个三维点，这些三维点的信息并不都是有用的，目前无人驾驶所需的点主要是获取路面的点，并且由于产生的三维点过多，使得在对三维点的信息进行传输时还会增加带宽压力。因此为了将产生的三维点云去除无用点云，保留所需的点云则会对产生的三维点云进行滤波处理，目前运用的比较广泛的滤波方法主要是直通滤波和体素滤波，其中直通滤波适用于平行于坐标轴的场景滤波，对无人驾驶中弯道场景的适用性较差，在弯道处往往会丢失大量路面区域所需的点云；体素滤波无法将冗余场景剔除，其虽然可以减少三维点的数量，但是对所需的路面区域点云的提取效率仍然比较差。而矿山的非结构化道路与结构化道路相比，其具有弯道弧度大，边界线无规律的特点，因此上述两种滤波方式在非结构化道路场景下的滤波效果均较差。
3.因此，如何提出一种新的滤波方法，在非结构化道路场景下，能有效对弯道路面所需的三维点云进行提取，且具有较高的提取效率，并且能将无用的点云数据及冗余数据剔除，从而有效的减轻后续对点云数据进行传输时的带宽压力，是本行业的研究方向之一。


技术实现要素：

4.针对上述现有技术存在的问题，本发明提供一种应用于矿山非结构化道路场景下的边界线滤波方法，在非结构化道路场景下，能有效对弯道路面所需的三维点云进行提取，且具有较高的提取效率，并且能将无用的点云数据及冗余数据剔除，从而有效的减轻后续对点云数据进行传输时的带宽压力。
5.为了实现上述目的，本发明采用的技术方案是：一种应用于矿山非结构化道路场景下的边界线滤波方法，在矿用卡车上装配感知系统，所述感知硬件系统包括固态激光雷达和计算机，所述计算机包括pcl点云库屏幕选点工具、时间戳同步模块、边界线点云预处理模块、滤波处理模块和显示器，固态激光雷达用于将感知到的三维点云数据传递给计算机；pcl点云库屏幕选点工具用于从三维点云数据中选取道路边界点云获得边界线数据；之后计算机将获取到的边界线数据通过边界线点云预处理模块进行三次样条插值预处理，使得预处理后两条边界线点云密度大致相同；所述时间戳同步模块用于将固态激光雷达的点云与预处理后的边界线进行时间戳同步接收；所述滤波处理模块用于利用边界线信息对当前帧的激光雷达点云进行滤波，得到两条边界线之间的道路点云信息；最后将获取到的两
条边界线之间的道路点云信息传递给显示器进行显示，具体滤波步骤为：
6.a、对固态激光雷达传输数据与pcl点云库屏幕选点工具选取道路边界点云进行软同步，确保两者是同一个时间分别发布的激光雷达点云数据信息和边界线数据信息；
7.b、对步骤a获得的边界线数据信息进行三次样条插值预处理，使得道路两条边界线的点云密度大致相同；
8.c、计算机对步骤a获取的激光雷达点云数据信息与步骤b预处理之后的边界线数据信息进行时间戳同步接收；
9.d、将接收到的边界线数据信息与当前帧的激光雷达点云信息进行滤波处理，滤波处理过程依次为：求取包围盒、点云平移、栅格投影、光线投射、栅格内点云信息叠加，完成滤波处理后从而获得两条边界线之间的道路点云信息。
10.进一步，所述步骤a中软同步方式为：在ros系统中，固态激光雷达和pcl点云库屏幕选点工具分别进行数据发布之前先在时间戳上打上统一的格林尼治时间。
11.进一步，所述步骤b中对边界线数据信息进行三次样条插值预处理的具体过程为：
12.首先将边界线数据信息[a,b]分成n个区间，分别为[(x
0,
x1),(x
1,
x2),(x
2,
x3),
…
,(x
n-1
,xn)]，共有n+1个点，其中两个端点分别为x0＝a,xn＝b；三次样条即每个区间的曲线是一个三次方程，三次样条方程满足以下条件：
[0013]
(1)在每一个区间(x
i,
x
i+1
)上s(x)＝si(x)都是一个三次方程；
[0014]
(2)满足插值条件，即s(xi)＝yi，i＝0,1,2,3,
…
,n；
[0015]
(3)曲线光滑，即s(x),s
′
(x),s
″
(x)是连续的；
[0016]
通过上述条件，各个区间的三次方程能构造成如下形式y＝ai+bix+cix2+dix3，这种形式即为三次样条函数si(x)；对每个区间的未知系数(ai,bi,ci,di)进行求解，则n个区间共有4n个未知数，则需要4n个方程来进行求解，通过获取4n个方程，从而求得每一区间的四个未知系数，进而得出每个区间的三次样条函数si(x)，最后每个区间的三次样条函数si(x)按照设定的步长，对每一区间进行插值。
[0017]
进一步，所述4n个方程的具体获取过程为：
[0018]
通过上述(2)中的插值条件得出，除了两个端点，所有内部的n-1个内部点均满足si(x
i+1
)＝y
i+1
,s
i+1
(x
i+1
)＝y
i+1
，即每个内部点相邻两个区间内的三次方程，则有2(n-1)个方程，再加上两个端点分别满足第一个区间和最后一个区间的三次方程，则总共有2n个方程；其次，根据(3)中曲线光滑的条件，则n-1个内部点的一阶导数是连续的，即在第i区间的末点和第i+1区间的起点是同一个点，因此同一点的一阶导数相等，即si′
(x
i+1
)＝s
i+1
′
(x
i+1
)，则有n-1个方程；另外，内部点的二阶导数也是连续的，即si″
(x
i+1
)＝s
i+1
″
(x
i+1
)，也有n-1个方程，此时总共有4n-2个方程，剩余两个方程通过边界条件得到：
[0019]
自然边界(natural spline)：指定端点二阶导数为0，即s
″
(x0)＝0＝s
″
(xn)；
[0020]
固定边界(clamped spline):指定端点一阶导数，这里分别定为a和b，即s0′
(x0)＝a,s
n-1
′
(xn)＝b；
[0021]
非扭结边界(not-a-knot spline):强制第一个插值点的三阶导数值等于第二个点的三阶导数值，最后一个点的三阶导数值等于倒数第二个点的三阶导数值，即s0″′
(x0)＝s1″′
(x1)，s
n-2
″′
(x
n-1
)＝s
n-1
″′
(xn)；从而获得4n个方程。
[0022]
进一步，所述步骤d中滤波处理的具体过程为：
[0023]
①
求包围盒：将当前帧激光雷达点云中的点按照x,y,z的顺序进行排序，求出x方向上的最大值与最小值；y方向的最大值与最小值；z方向上的最大值与最小值，求出包围盒的尺寸如下：
[0024][0025]
上式中max
x
表示的是x方向的最大值，min
x
表示的是x方向的最小值；；maxy表示的是y方向的最大值，miny表示的是y方向的最小值；maxz表示的是z方向的最大值，minz表示的是z方向的最小值；abs表示取绝对值；
[0026]
②
点云平移：为了确保点云投射到栅格上后，点云索引为正，需要根据x，y的最小值，对点云进行平移，使得点云中点的坐标不存在负值，具体公式如下：
[0027][0028]
③
栅格投影：利用步骤
①
求出来包围盒尺寸以及设置的分辨率参数resolution，求点云中每个点对应的栅格索引以及序号，求栅格索引的公式如下：
[0029][0030]
其中的cell
x
与celly表示的x，y对应的栅格索引，ceil表示的是向上取整函数；利用如上函数求得每个三维点对应的栅格索引；计算出栅格索引之后，为了确保栅格索引在投射的范围之内，对超出范围的栅格进行删除，首先依据计算出的size
x
，sizey合分辨率计算出栅格的地图大小：
[0031][0032]
筛选的准则如下：
[0033][0034][0035]
按照以上的方式对栅格进行筛选，将符合条件的栅格保留，之后依据计算出来的栅格索引计算栅格序号：
[0036]
cell
num
＝cell
x
*leny+celly[0037]
按照以上进行栅格投影，即将三维点云投射到二维栅格上，并获得每个三维点对应的栅格索引以及栅格序号；
[0038]
④
光线投射：将两条边界线投影到二维栅格地图上之后，获取左右边界线之间的栅格，此时对左右边界线投射上去的栅格一一进行光线投射，完成后获得左右边界线之间的栅格索引；
[0039]
⑤
栅格内点云信息叠加：利用步骤
④
获取的两条边界线之间的栅格索引，将对应栅格内在步骤
③
获得的投影点进行叠加，从而获取叠加点云，由于在步骤
②
进行了坐标平
移，为了维持原始的坐标不变，将坐标通过如下公式变换回去：
[0040][0041]
坐标变换之后，保存点云结果，即得到滤波之后的道路点云数据。
[0042]
进一步，所述步骤
④
中光线投射具体步骤为：
[0043]
(1)给出左右边界线的两个栅格索引(left
x
,lefty),(right
x
,righty)，将左边的点放入点(x0，y0)中；
[0044]
(2)将(x0，y0)装入缓存容器中，得到第一个点；
[0045]
(3)计算常量δx，δy，2δy，2δy-2δx，其中δx，δy分别为首尾两个点的横纵坐标差的绝对值，并计算决策参数的第一个值，决策参数的公式如下：
[0046]
p0＝2δy-δx
[0047]
(4)从k＝0开始，在沿着路径的每个xk处进行如下的检测：
[0048]
如果pk《0，则下一个需要绘制的点是(xk+1,yk),并且
[0049]
p
k+1
＝pk+2δy
[0050]
否则，下一个需要绘制的点是(xk+1,yk+1)，并且
[0051]
p
k+1
＝pk+2δy-2δx
[0052]
(5)重复步骤(4)共δx-1次，从而获得左右边界线之间的栅格索引。
[0053]
与现有技术相比，本发明先对固态激光雷达传输数据与pcl点云库屏幕选点工具选取道路边界点云进行软同步，确保两者是同一个时间分别发布的激光雷达点云数据信息和边界线数据信息；然后对获取的边界线数据信息进行三次样条插值预处理，使得道路两条边界线的点云密度大致相同；获取的激光雷达点云数据信息与上述预处理之后的边界线数据信息进行时间戳同步接收；最后将接收到的边界线数据信息与当前帧的激光雷达点云信息进行滤波处理，完成滤波处理后从而获得两条边界线之间的道路点云信息；在非结构化道路场景下，利用提取的道路边界线对获取的点云图像进行滤波，从而能将两条边界线之间的道路点云数据筛选出来，获得所需的道路点云数据，并将其余无用的点云数据及冗余数据剔除，减少冗余点，提高下游算法处理速度，同时也避免再使用其他的去冗余算法，使整个处理过程效率较高能满足所需的数据处理要求，同时有效的减轻后续对点云数据进行传输时的带宽压力。
附图说明
[0054]
图1是本发明的整体流程图；
[0055]
图2是本发明在u型弯道的滤波效果图；
[0056]
图3是本发明在直道的滤波效果图。
具体实施方式
[0057]
下面将对本发明作进一步说明。
[0058]
如图1所示，本发明先在矿用卡车上装配感知系统，所述感知硬件系统包括固态激光雷达和计算机，所述计算机包括pcl点云库屏幕选点工具、时间戳同步模块、边界线点云预处理模块、滤波处理模块和显示器，固态激光雷达用于将感知到的三维点云数据传递给
计算机；pcl点云库屏幕选点工具用于工作人员手动从三维点云数据中选取道路边界点云获得边界线数据；之后计算机将获取到的边界线数据通过边界线点云预处理模块进行三次样条插值预处理，使得预处理后两条边界线点云密度大致相同；所述时间戳同步模块用于将固态激光雷达的点云与预处理后的边界线进行时间戳同步接收；所述滤波处理模块用于利用边界线信息对当前帧的激光雷达点云进行滤波，得到两条边界线之间的道路点云信息；最后将获取到的两条边界线之间的道路点云信息传递给显示器进行显示，具体滤波步骤为：
[0059]
a、对固态激光雷达传输数据与pcl点云库屏幕选点工具选取道路边界点云进行软同步，确保两者是同一个时间分别发布的激光雷达点云数据信息和边界线数据信息；所述软同步方式为：在ros系统中，固态激光雷达和pcl点云库屏幕选点工具选取分别进行数据发布之前先在时间戳上打上统一的格林尼治时间。
[0060]
b、对步骤a获得的边界线数据信息进行三次样条插值预处理，使得道路两条边界线的点云密度大致相同；三次样条插值预处理的具体过程为：
[0061]
首先将边界线数据信息[a,b]分成n个区间，分别为[(x
0,
x1),(x
1,
x2),(x
2,
x3),
…
,(x
n-1
,xn)]，共有n+1个点，其中两个端点分别为x0＝a,xn＝b；三次样条即每个区间的曲线是一个三次方程，三次样条方程满足以下条件：
[0062]
(1)在每一个区间(x
i,
x
i+1
)上s(x)＝si(x)都是一个三次方程；
[0063]
(2)满足插值条件，即s(xi)＝yi，i＝0,1,2,3,
…
,n；
[0064]
(3)曲线光滑，即s(x),s
′
(x),s
″
(x)是连续的；
[0065]
通过上述条件，各个区间的三次方程能构造成如下形式y＝ai+bix+cix2+dix3，这种形式即为三次样条函数si(x)；对每个区间的未知系数(ai,bi,ci,di)进行求解，则n个区间共有4n个未知数，则需要4n个方程来进行求解，4n个方程的具体获取过程为：
[0066]
通过上述(2)中的插值条件得出，除了两个端点，所有内部的n-1个内部点均满足si(x
i+1
)＝y
i+1
,s
i+1
(x
i+1
)＝y
i+1
，即每个内部点相邻两个区间内的三次方程，则有2(n-1)个方程，再加上两个端点分别满足第一个区间和最后一个区间的三次方程，则总共有2n个方程；其次，根据(3)中曲线光滑的条件，则n-1个内部点的一阶导数是连续的，即在第i区间的末点和第i+1区间的起点是同一个点，因此同一点的一阶导数相等，即si′
(x
i+1
)＝s
i+1
′
(x
i+1
)，则有n-1个方程；另外，内部点的二阶导数也是连续的，即si″
(x
i+1
)＝s
i+1
″
(x
i+1
)，也有n-1个方程，此时总共有4n-2个方程，剩余两个方程通过边界条件得到：
[0067]
自然边界(natural spline)：指定端点二阶导数为0，即s
″
(x0)＝0＝s
″
(xn)；
[0068]
固定边界(clamped spline):指定端点一阶导数，这里分别定为a和b，即s0′
(x0)＝a,s
n-1
′
(xn)＝b；
[0069]
非扭结边界(not-a-knot spline):强制第一个插值点的三阶导数值等于第二个点的三阶导数值，最后一个点的三阶导数值等于倒数第二个点的三阶导数值，即s0″′
(x0)＝s1″′
(x1)，s
n-2
″′
(x
n-1
)＝s
n-1
″′
(xn)；从而获得4n个方程，接着根据4n个方程求得每一区间的四个未知系数，进而得出每个区间的三次样条函数si(x)，最后每个区间的三次样条函数si(x)按照设定的步长，对每一区间进行插值。
[0070]
c、计算机对步骤a获取的激光雷达点云数据信息与步骤b预处理之后的边界线数据信息进行时间戳同步接收；
[0071]
d、将接收到的边界线数据信息与当前帧的激光雷达点云信息进行滤波处理，滤波处理的具体过程为：
①
求包围盒：将当前帧激光雷达点云中的点按照x,y,z的顺序进行排序，求出x方向上的最大值与最小值；y方向的最大值与最小值；z方向上的最大值与最小值，求出包围盒的尺寸如下：
[0072][0073]
上式中max
x
表示的是x方向的最大值，min
x
表示的是x方向的最小值；；maxy表示的是y方向的最大值，miny表示的是y方向的最小值；maxz表示的是z方向的最大值，minz表示的是z方向的最小值；abs表示取绝对值；
[0074]
②
点云平移：为了确保点云投射到栅格上后，点云索引为正，需要根据x，y的最小值，对点云进行平移，使得点云中点的坐标不存在负值，具体公式如下：
[0075][0076]
③
栅格投影：利用步骤
①
求出来包围盒尺寸以及设置的分辨率参数resolution，求点云中每个点对应的栅格索引以及序号，求栅格索引的公式如下：
[0077][0078]
其中的cell
x
与celly表示的x，y对应的栅格索引，ceil表示的是向上取整函数；利用如上函数求得每个三维点对应的栅格索引；计算出栅格索引之后，为了确保栅格索引在投射的范围之内，对超出范围的栅格进行删除，首先依据计算出的size
x
，sizey结合分辨率计算出栅格的地图大小：
[0079][0080]
筛选的准则如下：
[0081][0082][0083]
按照以上的方式对栅格进行筛选，将符合条件的栅格保留，之后依据计算出来的栅格索引计算栅格序号：
[0084]
cell
num
＝cell
x
*leny+celly[0085]
按照以上进行栅格投影，即将三维点云投射到二维栅格上，并获得每个三维点对应的栅格索引以及栅格序号；
[0086]
④
光线投射：将两条边界线投影到二维栅格地图上之后，获取左右边界线之间的栅格，此时对左右边界线投射上去的栅格一一进行光线投射，光线投射具体步骤为：
[0087]
(1)给出左右边界线的两个栅格索引(left
x
,lefty),(right
x
,righty)，将左边的点放入点(x0，y0)中；
[0088]
(2)将(x0，y0)装入缓存容器中，得到第一个点；
[0089]
(3)计算常量δx，δy，2δy，2δy-2δx，其中δx，δy分别为首尾两个点的横纵坐标差的绝对值，并计算决策参数的第一个值，决策参数的公式如下：
[0090]
p0＝2δy-δx
[0091]
(4)从k＝0开始，在沿着路径的每个xk处进行如下的检测：
[0092]
如果pk《0，则下一个需要绘制的点是(xk+1,yk),并且
[0093]
p
k+1
＝pk+2δy
[0094]
否则，下一个需要绘制的点是(xk+1,yk+1)，并且
[0095]
p
k+1
＝pk+2δy-2δx
[0096]
(5)重复步骤(4)共δx-1次，从而获得左右边界线之间的栅格索引。
[0097]
⑤
栅格内点云信息叠加：利用步骤
④
获取的两条边界线之间的栅格索引，将对应栅格内在步骤
③
获得的投影点进行叠加，从而获取叠加点云，由于在步骤
②
进行了坐标平移，为了维持原始的坐标不变，将坐标通过如下公式变换回去：
[0098][0099]
坐标变换之后，保存点云结果，如图2和3所示，即得到滤波之后的道路点云数据。
[0100]
以上所述仅是本发明的优选实施方式，应当指出：对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。