CN106546996A - 基于四线激光雷达的道路检测与跟踪方法 - Google Patents

Abstract

本发明公开了一种基于四线激光雷达的道路检测与跟踪方法，包括以下步骤：S1：将车载四线激光雷达进行标定；S2：对激光雷达扫到的四组雷达数据分别进行数据分割获得道路轮廓，并融合得到完整的道边线段；S3：提取道边高度；S4：提取出岔口线段，融合得到完整地岔口线段，并结合道边线段提取道路信息；S5：构造三条直线函数，对下一帧的四组扫描数据进行初步滤波，快速获得道边和岔口线段，并融合得到完整道边与岔口线段；S6：由三条直线函数构造扩展卡尔曼滤波器，对得到的道边与岔口线段进行滤波估计，获得最终的道边与岔口线段，并提取道路信息。本发明采用四线激光雷达来实现道路的检测与跟踪，提高了道路检测与跟踪的准确性和实时性。

Description

基于四线激光雷达的道路检测与跟踪方法

技术领域

本发明涉及一种基于四线激光雷达的道路检测与跟踪方法。

背景技术

在室外环境下，智能机器人时刻需要感知环境，做出规划决策，确保在复杂的环境中安全行驶并最终到达预定目标。故而道路检测与跟踪作为自主导航技术的一个核心课题，越发受到研究人员的重视。基于图像的道路检测与跟踪因其信息量大、分辨率高等特点而受到广泛应用。但是图像易受外部环境的影响，如光照、阴影等，有时会导致检测与跟踪效果不佳，而激光雷达却不会受到光照、阴影的影响，不仅探测范围远、测距准确，而且还可以在恶劣天气条件下工作。于是基于激光雷达的道路检测与跟踪受到研究人员的关注。

发明内容

本发明要解决的技术问题是提供一种基于四线激光雷达的道路检测与跟踪方法。

基于四线激光雷达的道路检测与跟踪方法，包括以下步骤：

S1：将车载四线激光雷达进行标定；

S2：对激光雷达扫到的四组雷达数据分别进行数据分割获得道路轮廓，并根据道边与道路的空间位置关系筛选出道边直线段并融合得到完整的道边线段；

S3：由检测到的道边对扫描数据滤波提取出地面数据点拟合地平面，计算检测道边的端点到地平面的距离，即道边高度；

S4：用检测到的道边根据岔口与道边的空间位置关系在候选直线段中提取出岔口线段，融合得到完整地岔口线段，并结合道边线段提取道路信息；

S5：由道路检测获得的道路信息去构造三条直线函数，对下一帧的四组扫描数据进行初步滤波，快速获得道边和岔口线段，并融合得到完整道边与岔口线段；

S6：由三条直线函数构造扩展卡尔曼滤波器，对得到的道边与岔口线段进行滤波估计，获得最终的道边与岔口线段，并提取道路信息。

进一步的，所述车载激光雷达的标定方法如下：

S1-1：将空间任意测量点P从激光雷达坐标系转换到车体坐标系公式表示为：

其中，T＝[ΔX ΔY ΔZ]^T，

其中，[X_L Y_L Z_L]^T为P点在车体坐标系下的坐标，[X_v Y_v Z_v]^T为P点在雷达坐标系下的坐标，雷达标定就是要求解旋转矩阵R和平移向量T；

S1-2：将标定矩阵分解，分别求解X_L，Y_L，Z_L，则得到下列公式：

X_L＝R₁₁X_V+R₁₂Y_V+R₁₃Z_V+ΔX，

Y_L＝R₂₁X_V+R₂₂Y_V+R₂₃Z_V+ΔY，

Z_L＝R₃₁X_V+R₃₂Y_V+R₃₃Z_V+ΔZ，

S1-3：在平坦的地面竖直放置一条条状物体，记下物体右侧边缘在车体坐标下的坐标并采集该物体在雷达坐标系下右侧边缘的坐标则：

四线激光雷达扫描的数据为三维数据点，此方程中则有8个未知数，只需从激光雷达中最低激光线扫描到的数据中任取4组数据点就可以求解此方程；接着可求解S1-2中的方程；

S1-4：雷达扫描平面为水平平面，雷达扫描平面与地面相交于同一直线，因为地面平坦，该直线上的扫描点在车体坐标系下的Z坐标值可以近似为0，则S1-2中方程Z_L＝R₃₁X_V+R₃₂Y_V+R₃₃Z_V+ΔZ可以写成：

0＝R₃₁X_V+R₃₂Y_V+R₃₃Z_V+ΔZ，此方程式有4个未知数，只需要4组数据就可以求解这个方程式；

S1-5：结合S1-3和S1-4两次求解，便能得到旋转矩阵R和平移向量T。

进一步的，所述数据分割方法和提取道边方法如下：

S2-1：假设从第一个数据点开始的m个数据点共线，则由第一个与第m个点确定该直线Ax+By+C＝0，再对这m个点逐个计算点到直线的距离是否小于阈值，当大于阈值的点的个数多于3个时，则认为这m个点不共线，此时便从第二个点开始再检测m个数据点是否共线，若仍不共线再从下一个点继续检测直到遇到有m个点共线进入步骤S2-2；

S2-2：由步骤S2-1得到共线的直线去检测后一个点，即该m个点外的下一个点是否共线，即该点到直线的距离小于阈值，共线则继续检测下一个点，直到该点不在线段上，即该线段的结束，此时就由该点开始检测m个点共线去分割新的线段，执行步骤S2-1；

S2-3：根据如下的道边与道路的空间位置关系从中获取道边：

1)道边大致平行小车的航向，所以道边应与竖直方向近似平行；

2)左右两条道边近似平行并有一定距离，且道边有一定的长度；

S2-4：同样的数据分割和提取道边步骤分别对四组雷达扫描数据处理，即可获得四组道边线段，进行融合就可以得到一条完整的初步道边检测结果。

进一步的，所述岔口线段的提取方法如下：

S3-1：在道边检测后，根据岔口线段的特性对取得的道路轮廓线段来提取岔口线段：

1)岔口线段的y坐标离道边上端点的距离不小于道宽；

2)岔口线段的长度不能小于一定的阈值；

3)岔口线段应与道边近似垂直；

S3-2：每组获得的岔口线段分为两部分，在原点坐标左端和原点坐标右端，将这两部分分别融合(y坐标取均值，x坐标上线段左端取最小值，右端取最大值)得到岔口左右两部分，以此来判断T型口还是十字型口：若岔口左右两部分距离不小于道宽，即岔口的竖直朝向可通行，则为十字型口，否则是T型口，将岔口左右两部分结合在一起得到完整的T型岔口；若检测不到岔口线段，则认为还未到岔口。

本发明的有益效果是：

本发明采用四线激光雷达来实现道路的检测与跟踪，充分利用四线激光雷达本身的特点来提高道路检测与跟踪的准确性和实时性；也设计一种简单的算法实现道路跟踪，方法简单有效，能够实时准确地跟踪道路。

具体实施方式

以下具体实施例对本发明作进一步阐述，但不作为对本发明的限定。

基于四线激光雷达的道路检测与跟踪方法，包括以下步骤：

S1：将车载四线激光雷达进行标定；

S2：对激光雷达扫到的四组雷达数据分别进行数据分割获得道路轮廓，并根据道边与道路的空间位置关系筛选出道边直线段并融合得到完整的道边线段；

S3：由检测到的道边对扫描数据滤波提取出地面数据点拟合地平面，计算检测道边的端点到地平面的距离，即道边高度；

S4：用检测到的道边根据岔口与道边的空间位置关系在候选直线段中提取出岔口线段，融合得到完整地岔口线段，并结合道边线段提取道路信息；

S5：由道路检测获得的道路信息去构造三条直线函数，对下一帧的四组扫描数据进行初步滤波，快速获得道边和岔口线段，并融合得到完整道边与岔口线段；

S6：由三条直线函数构造扩展卡尔曼滤波器，对得到的道边与岔口线段进行滤波估计，获得最终的道边与岔口线段，并提取道路信息。

所述车载激光雷达的标定方法如下：

S1-1：将空间任意测量点P从激光雷达坐标系转换到车体坐标系公式表示为：

其中，T＝[ΔX ΔY ΔZ]^T，

其中，[X_L Y_L Z_L]^T为P点在车体坐标系下的坐标，[X_v Y_v Z_v]^T为P点在雷达坐标系下的坐标，雷达标定就是要求解旋转矩阵R和平移向量T；

S1-2：将标定矩阵分解，分别求解X_L，Y_L，Z_L，则得到下列公式：

X_L＝R₁₁X_V+R₁₂Y_V+R₁₃Z_V+ΔX，

Y_L＝R₂₁X_V+R₂₂Y_V+R₂₃Z_V+ΔY，

Z_L＝R₃₁X_V+R₃₂Y_V+R₃₃Z_V+ΔZ，

S1-3：在平坦的地面竖直放置一条条状物体，记下物体右侧边缘在车体坐标下的坐标并采集该物体在雷达坐标系下右侧边缘的坐标则：

四线激光雷达扫描的数据为三维数据点，此方程中则有8个未知数，只需从激光雷达中最低激光线扫描到的数据中任取4组数据点就可以求解此方程；接着可求解S1-2中的方程；

S1-4：雷达扫描平面为水平平面，雷达扫描平面与地面相交于同一直线，因为地面平坦，该直线上的扫描点在车体坐标系下的Z坐标值可以近似为0，则S1-2中方程Z_L＝R₃₁X_V+R₃₂Y_V+R₃₃Z_V+ΔZ可以写成：

0＝R₃₁X_V+R₃₂Y_V+R₃₃Z_V+ΔZ，此方程式有4个未知数，只需要4组数据就可以求解这个方程式；

S1-5：结合S1-3和S1-4两次求解，便能得到旋转矩阵R和平移向量T。

所述数据分割方法和提取道边方法如下：

S2-1：假设从第一个数据点开始的m个数据点共线，则由第一个与第m个点确定该直线Ax+By+C＝0，再对这m个点逐个计算点到直线的距离是否小于阈值，当大于阈值的点的个数多于3个时，则认为这m个点不共线，此时便从第二个点开始再检测m个数据点是否共线，若仍不共线再从下一个点继续检测直到遇到有m个点共线进入步骤S2-2；

S2-2：由步骤S2-1得到共线的直线去检测后一个点，即该m个点外的下一个点是否共线，即该点到直线的距离小于阈值，共线则继续检测下一个点，直到该点不在线段上，即该线段的结束，此时就由该点开始检测m个点共线去分割新的线段，执行步骤S2-1；

S2-3：根据如下的道边与道路的空间位置关系从中获取道边：

1)道边大致平行小车的航向，所以道边应与竖直方向近似平行；

2)左右两条道边近似平行并有一定距离，且道边有一定的长度；

S2-4：同样的数据分割和提取道边步骤分别对四组雷达扫描数据处理，即可获得四组道边线段，进行融合就可以得到一条完整的初步道边检测结果。

所述岔口线段的提取方法如下：

S3-1：在道边检测后，根据岔口线段的特性对取得的道路轮廓线段来提取岔口线段：

1)岔口线段的y坐标离道边上端点的距离不小于道宽；

2)岔口线段的长度不能小于一定的阈值；

3)岔口线段应与道边近似垂直；

S3-2：每组获得的岔口线段分为两部分，在原点坐标左端和原点坐标右端，将这两部分分别融合(y坐标取均值，x坐标上线段左端取最小值，右端取最大值)得到岔口左右两部分，以此来判断T型口还是十字型口：若岔口左右两部分距离不小于道宽，即岔口的竖直朝向可通行，则为十字型口，否则是T型口，将岔口左右两部分结合在一起得到完整的T型岔口；若检测不到岔口线段，则认为还未到岔口。

Claims (4)

1.基于四线激光雷达的道路检测与跟踪方法，其特征在于，包括以下步骤：

S1：将车载四线激光雷达进行标定；

S2：对激光雷达扫到的四组雷达数据分别进行数据分割获得道路轮廓，并根据道边与道路的空间位置关系筛选出道边直线段并融合得到完整的道边线段；

S3：由检测到的道边对扫描数据滤波提取出地面数据点拟合地平面，计算检测道边的端点到地平面的距离，即道边高度；

S4：用检测到的道边根据岔口与道边的空间位置关系在候选直线段中提取出岔口线段，融合得到完整地岔口线段，并结合道边线段提取道路信息；

S5：由道路检测获得的道路信息去构造三条直线函数，对下一帧的四组扫描数据进行初步滤波，快速获得道边和岔口线段，并融合得到完整道边与岔口线段；

S6：由三条直线函数构造扩展卡尔曼滤波器，对得到的道边与岔口线段进行滤波估计，获得最终的道边与岔口线段，并提取道路信息。

2.根据权利要求1所述的道路检测与跟踪方法，其特征在于，所述车载激光雷达的标定方法如下：

S1-1：将空间任意测量点P从激光雷达坐标系转换到车体坐标系公式表示为：

其中，T＝[ΔX ΔY ΔZ]^T，

其中，[X_L Y_L Z_L]^T为P点在车体坐标系下的坐标，[X_v Y_v Z_v]^T为P点在雷达坐标系下的坐标，雷达标定就是要求解旋转矩阵R和平移向量T；

S1-2：将标定矩阵分解，分别求解X_L，Y_L，Z_L，则得到下列公式：

X_L＝R₁₁X_V+R₁₂Y_V+R₁₃Z_V+ΔX，

Y_L＝R₂₁X_V+R₂₂Y_V+R₂₃Z_V+ΔY，

Z_L＝R₃₁X_V+R₃₂Y_V+R₃₃Z_V+ΔZ，

S1-3：在平坦的地面竖直放置一条条状物体，记下物体右侧边缘在车体坐标下的坐标并采集该物体在雷达坐标系下右侧边缘的坐标则：

$\left\{\begin{array}{c}{X}_L^A=R_{11}{X}_L^A+R_{12}{Y}_L^A+R_{13}{Z}_L^A+\mathrm{\Δ}X\\ Y_L^A=R_{21}{X}_L^A+R_{22}{Y}_L^A+R_{23}{Z}_L^A+\mathrm{\Δ}Y\end{array}\right.;$

四线激光雷达扫描的数据为三维数据点，此方程中则有8个未知数，只需从激光雷达中最低激光线扫描到的数据中任取4组数据点就可以求解此方程；接着可求解S1-2中的方程；

S1-4：雷达扫描平面为水平平面，雷达扫描平面与地面相交于同一直线，因为地面平坦，该直线上的扫描点在车体坐标系下的Z坐标值可以近似为0，则S1-2中方程Z_L＝R₃₁X_V+R₃₂Y_V+R₃₃Z_V+ΔZ可以写成：

0＝R₃₁X_V+R₃₂Y_V+R₃₃Z_V+ΔZ，此方程式有4个未知数，只需要4组数据就可以求解这个方程式；

S1-5：结合S1-3和S1-4两次求解，便能得到旋转矩阵R和平移向量T。

3.根据权利要求1所述的道路检测与跟踪方法，其特征在于，所述数据分割方法和提取道边方法如下：

S2-1：假设从第一个数据点开始的m个数据点共线，则由第一个与第m个点确定该直线Ax+By+C＝0，再对这m个点逐个计算点到直线的距离是否小于阈值，当大于阈值的点的个数多于3个时，则认为这m个点不共线，此时便从第二个点开始再检测m个数据点是否共线，若仍不共线再从下一个点继续检测直到遇到有m个点共线进入步骤S2-2；

S2-2：由步骤S2-1得到共线的直线去检测后一个点，即该m个点外的下一个点是否共线，即该点到直线的距离小于阈值，共线则继续检测下一个点，直到该点不在线段上，即该线段的结束，此时就由该点开始检测m个点共线去分割新的线段，执行步骤S2-1；

S2-3：根据如下的道边与道路的空间位置关系从中获取道边：

1)道边大致平行小车的航向，所以道边应与竖直方向近似平行；

2)左右两条道边近似平行并有一定距离，且道边有一定的长度；

S2-4：同样的数据分割和提取道边步骤分别对四组雷达扫描数据处理，即可获得四组道边线段，进行融合就可以得到一条完整的初步道边检测结果。

4.根据权利要求1所述的道路检测与跟踪方法，其特征在于，所述岔口线段的提取方法如下：

S3-1：在道边检测后，根据岔口线段的特性对取得的道路轮廓线段来提取岔口线段：

1)岔口线段的y坐标离道边上端点的距离不小于道宽；

2)岔口线段的长度不能小于一定的阈值；

3)岔口线段应与道边近似垂直；

S3-2：每组获得的岔口线段分为两部分，在原点坐标左端和原点坐标右端，将这两部分分别融合(y坐标取均值，x坐标上线段左端取最小值，右端取最大值)得到岔口左右两部分，以此来判断T型口还是十字型口：若岔口左右两部分距离不小于道宽，即岔口的竖直朝向可通行，则为十字型口，否则是T型口，将岔口左右两部分结合在一起得到完整的T型岔口；若检测不到岔口线段，则认为还未到岔口。