CN109684921B - 一种基于三维激光雷达的道路边界检测与跟踪方法 - Google Patents

Abstract

本发明是一种基于三维激光雷达的道路边界检测与跟踪方法。包括：将激光雷达扫描点的极坐标数据转换成直角坐标数据；利用随机采样一致性平面拟合方法和主成分分析法提取地面点；采用一种多特征、宽阈值、分层次的特征点提取方法粗提取道路边界特征点；借鉴线性判别分析的思想，采用基于投影的方法，将粗提取的道路边界特征点划分为左右道路边界特征点两类；利用随机采样一致性方法分别对上述左右道路边界点进行滤波得到最终的道路边界点；采用二次多项式随机采样一致性方法分别对左右道路边界线进行拟合；采用基于幅值滤波的卡尔曼滤波方法对道路边界线进行跟踪。本发明能够处理多种复杂场景和工况，可以广泛应用于无人驾驶。

Description

一种基于三维激光雷达的道路边界检测与跟踪方法

技术领域

本发明属于无人驾驶车辆环境感知技术领域，具体的说是一种基于三维激光雷达的道路边界检测与跟踪方法。

背景技术

自动无人驾驶诞生以来，基于多种传感器的环境感知技术就吸引了众多学者和研究机构的目光。其中道路边界检测是无人驾驶环境感知中的重要研究内容。道路边界可以将道路区域和非道路区域分开，从而为无人驾驶车辆决策规划提供重要的依据；通过道路边界可以将道路边界范围之的无效传感器数据滤除，提高后续障碍物检测的效率；由于城市环境中树木、天桥和隧道等对GPS信号的遮挡，道路边界特征也可以用来估计无人车的位置。此外在没有车道线的道路环境下，可以依赖可靠的道路边界检测实现无人驾驶的车道保持和跟踪。

根据采用传感器不同，出现了很多不同的道路边界检测与跟踪技术，主要有基于相机(单目和立体)和激光雷达的(2D和3D)。

相机体积小，价格低能提供丰富的纹理颜色信息，但是容易受到光照天气的影响，而且单目相机没有深度信息。立体相机虽然具有深度信息，但是视场角小且需要计算视差，实时性较差。2D激光雷达虽然不受外界光照变化影响，可以直接获取深度信息，但是只能在一个固定角度扫描，数据量小，很容易受到观测噪声的影响。而3D激光雷达探测范围广，数据量大，精度高，可以准确的检测道路边界信息同时抑制噪声的干扰。

三维激光雷达虽然获取数据速度快，精度高，但是数据量庞大且具有无序性，这对处理三维激光雷达数据的算法提出了很高的要求。孙朋朋等人通过三个特征粗提取道路边界特征点，然后直接采用K最近邻算法对边界点聚类分割提取道路边界点。该方法复杂且在边界特征点不连续或者道路弯曲的情况下很容易将有效的边界特征点滤除，且容易受到道路内的障碍物干扰。Kunlei Hu等人通过曲率特征和高度差特征提取特征点，然后直接通过RANSAC直线拟合道路边界，该方法容易受到道路内外与道路边界有相同特征的障碍物影响，例如周围存在与道路同向的铁路轨道就会产生严重干扰。

发明内容

本发明提供了一种基于三维激光雷达的道路边界检测与跟踪方法，该方法可以处理弯曲道路，不受道路内动态障碍物的影响，同时不受道路外与道路边界有相似特征的障碍物影响，具有很好的鲁棒性，由于采幅值滤波的卡尔曼滤波跟踪方法，本方法可以很好的处理误检、漏检等状况，解决了现有自动无人驾驶中道路边界检测存在的上述不足。

本发明技术方案结合附图说明如下：

一种基于三维激光雷达的道路边界检测与跟踪方法，该方法包括以下步骤：

步骤一、获取激光雷达扫描数据并转换到激光雷达直角坐标系下；

步骤二、根据GPS/IMU提供的位姿变化信息，对点云数据进行畸变矫正；

步骤三、采用随机采样一致性和主成分分析法提取地面点；

步骤四、采用多特征、宽阈值、分层次的特征点提取方法对边界特征点进行粗提取；

步骤五、将粗提取的边界特征点分成左右边界点并进行精提取；

步骤六、采用随机采样一致性方法对精提取的边界点进行曲线拟合；

步骤七、采用幅值滤波的卡尔曼滤波方法对边界线进行跟踪。

所述的步骤一的具体方法为：

11)以三维激光雷达的中心为坐标原点O_l，过原点O_l且指向无人车前进方向为x轴，过原点O_l且垂直指向上方为z轴，过原点且垂直于x轴和z轴所构成的平面指向无人车前进方向左侧的方向为y轴,建立三维激光雷达的直角坐标系O_lxyz；

12)获取三维激光雷达扫描一周返回的所有数据作为一帧点云数据，并将该帧点云极坐标数据转换到三维激光雷达的直角坐标系下，具体包括以下步骤：

公式(1)将三维激光雷获取的点云的极坐标转换到三维激光雷达的直角坐标系下：

其中，i为激光雷达扫描点在一帧点云中的序号，γ_i为第i个扫描点距离激光雷达原点的极坐标距离，α_i为第i个扫描点的垂直方位角，β_i为第i个扫描点的水平方位角。

所述的步骤二的具体方法为：

根据GPS/IMU提供的每一帧位姿变化信息，包括翻滚角θ_Xframe，俯仰角θ_Yframe，航向角θ_Zframe以及位移x_frame，y_frame，z_frame，对每帧点云进行畸变矫正，包括以下步骤：

21)根据每个点P_i的水平方位角β_i，计算该点相对于扫描起始位置的时间偏移t_i，假设当前帧的扫描起始位置对应的水平方位角为β₀，那么通过式(2)计算雷达扫描β_i所需要的时间：

t_i＝C×(β_i-β₀)/2π (2)

其中C为激光雷达扫描一周的时间；

22)假设在同一帧内车辆的位姿近似线性变化，因此每一时刻车辆的位姿可以通过相邻两帧的位姿插值获得；根据GPS/IMU提供的每一帧车辆位姿变换信息，采用线性插值计算变换矩阵如下：

[θ_Xi θ_Yi θ_Zi]＝(-t_i/C)[θ_Xframe θ_Yframe θ_Zframe] (3)

其中T_i为帧起始扫描点到该时刻车辆的位移，θ_Xi，θ_Yi，θ_Zi分别为帧起始扫描点到该时刻车辆的翻滚角，俯仰角和航向角。

23)根据上述所计算得到的变换矩阵，采用公式(4)将点P_i

矫正如下，点P′_i为矫正后的点：

R_i＝R_z(θ_Zi)R_y(θ_Yi)R_x(θ_Xi),

P′_i＝R_iP_i+T_i (4)

其中，R_i为从起始扫描点到P_i对应时刻车辆位姿的旋转矩阵，R_z(θ_Zi)，R_y(θ_Yi)，R_x(θ_Xi)分别为绕z,y,x三轴的旋转矩阵。

所述的步骤三的具体方法为：

对畸变矫正后的点云进行地面提取，滤除掉地面以上的障碍物，包括以下步骤：

31)采用随机采样一致性平面拟合方法粗提取地面点，具体包括以下步骤：

311)随机从矫正的点云中任选三个点，得到通过这三个点的平面模型；

312)用得到的平面模型去测试其他点，由点到平面的距离阈值T₀确定其他点是否为局内点或者为局外点；

313)重复步骤311-312，直到找到具有最多内点的平面模型并将其作为“最佳”平面模型，并根据该模型按照步骤312将局外点作为非地面点滤除，得到粗提取的地面点。

32)对所述粗提取的地面点，通过对每个点的球形邻域进行主成分分析(PCA)，判定每个点是否具有平面特征来滤除高度介于路面和道路边界的障碍物点，具体方法为：

321)采用公式(5)对每个点的球形邻域点计算协方差矩阵C和相应的规范化特征值

λ′_j(j＝1，2，3，λ′₁≥λ′₂≥λ′₃)

其中，K为球形邻域点的个数，P_i为第i个邻域点的坐标位置向量，

为球形邻域点的均值向量，λ_j为协方差矩阵的特征值，λ′_j为规范化后的特征值；

322)根据上述计算得到的规范化特征值判定每个点是否具有平面特征，如果满足以下条件则将该点保留，否则将剔除：

T₁≥λ′₁≥λ′₂≥T₂ T₃≥λ′₃≥T₄ T₂＞T₃ (6)

其中T_i(i＝1，2，3，4)为平面特征点阈值。

所述的步骤四的具体方法为：

对步骤三提取的地面点，采用一种多特征，宽阈值，分层次的提取方法分别粗提取每个扫描线的道路边界特征点，包括以下步骤：

41)根据每个特征条件对每个点进行判定，并标记；具体包括以下步骤：

411)对该扫描线中的第i个扫描点，在同一扫描线上提取该点的邻域中的最大高度和最小高度，并计算它们的差值作为该扫描点i的邻域高度差特征值，如果高度差大于T₅并且小于T₆，那么该点满足邻域高度差特征条件，被选为候选特征点；

412)对该扫描线中的第i个扫描点，在同一扫描线采用公式(7)计算该点的邻域平滑特征值s，如果该平滑值特征值大于阈值T₇,那么该点满足邻域平滑特征条件，被选为候选特征点；

其中M表示第i个扫描点P_i的邻域点集合，集合中有一半点在P_i的左边，有一半点在P_i的右边，|M|表示集合M的势。

413)对该扫描线中的第i个扫描点，在同一扫描线采用公式(8)计算扫描线在该点的切向量夹角特征值θ_i，如果该特征值小于阈值T₈，被选为候选特征点：

其中，m_θ为用于计算扫描线在该点切向量的邻域点个数，V_a和V_b分别表示该扫描点沿着两侧扫描线的切向量，x,y分别扫描点的横纵方向坐标；

414)对该扫描线中第i个扫描点，提取同一扫描线上该点所有邻域点的高度值，并计算它们的方差作为扫描线在该点的邻域高度方差特征值，如果特征值大于阈值T₉，被选为候选特征点；

415)对该扫描线中第i个扫描点，计算该点与同一扫描线的相邻点的水平距离作为扫描线在该点的邻域水平距离特征值，如果该特征值小于阈值T₁₀，被选为候选特征点，该阈值T₁₀可通过公式(9)计算得到：

其中，H_s为该扫描点的高度的绝对值，θ_l为该扫描线的垂直方位角，θ_a为激光雷达的水平分辨率；

416)对该扫描线中的第i个扫描点，计算该点的法向量与激光雷达y坐标的夹角作为该点的法向量夹角特征值，如果该特征值小于阈值T₁₁，则被选为候选特征点；

42)对该扫描线提取满足所有特征条件的点作为粗提取的道路边界点。

所述的步骤五的具体方法为：

对粗提取的道路边界特征点，分成左右道路边界特征点并进行精提取得到精提取的左右道路边界点，包括以下步骤：

51)借鉴线性判别分析的思想，采用基于投影的左右道路边界点分类方法将将粗提取的道路边界特征点划分为两组，分别表示粗提取的左道路边界特征点和右道路边界特征点；具体包括以下步骤：

511)首先将所有粗提取的特征点投影到xy平面，并假设激光雷达坐标系x轴与道路边界夹角为θ，-50≤θ≤50，θ为整数，将粗提取的边界特征点绕z旋转θ角度进行变换，使道路边界与激光雷达x轴平行，然后将变换后的边界特征点投影到y轴上，得到投影后每个特征点的坐标；

512)根据投影后的每个特征点的y坐标正负将投影后的特征点分为两类，根据公式(10)计算分类标准得分值：

C_r＝|E(P_θ，L)-E(P_θ，R)|/(S(P_θ，L)+S(P_θ，R)) (10)

其中，E(P_θ，L)和E(P_θ，R)分别为投影后左道路边界特征点和右道路边界特征点的均值，S(P_θ，L)和S(P_θ，R)分别为投影后左道路边界特征点和右道路边界特征点的方差；

513)对θ假设范围内的每个角度按照步骤511和512分别计算出相应的分类标准得分值C_r，使得分值最大的角度θ_optimal即为车辆与道路边界的夹角；

514)根据上述得到的车辆与道路边界的夹角θ_optimal，首先将步骤四中粗提取的道路边界特征点进行绕z旋转θ_optimal角进行变换，使道路边界与激光雷达x轴平行，然后根据变换后的点的y坐标正负将粗提取道路边界特征点分为两组，然后再将两组特征点绕z轴旋转-θ_optimal角度变换，得到左道路边界特征点和右道路边界特征点；

52)对上述两组边界特征点，对每个扫描线在激光雷达yz平面上半空间和下半空间分别提取横向距离车辆最近的两个点，构成横向距离最近左右道路边界特征点；

53)对上述新的左右道路边界特征点，分别采用二次多项式随机采样一致性滤波方法滤除路内障碍物干扰点，得到精提取后的左右道路边界点，具体包括以下步骤：

531)随机地从新的左道路边界特征点中任选三个点，按照公式(11)拟合得到通过这三个点的二次多项式曲线：

y＝ax²+bx+c

z＝mean(z_i)i＝1…n (11)

其中z_i表示每个边界特征点的z方向坐标值，mean表示对所有特征点的z坐标取均值。

532)用得到的二次曲线模型去测试其他点，由点到曲线的距离阈值T₁₂确定其他点是否为局内点或者为局外点。

533)重复步骤531-532，直到找到具有最多内点的曲线模型并将其作为“最佳”曲线模型，并根据该模型按照步骤532将局外点作为干扰点滤除，得到精提取后的左道路边界点，更换新的右道路边界特征点，按照步骤531-533，滤除右边界点中的干扰点，得到精提取后的右道路边界特征点。

所述的步骤六的具体方法为：

对精提取的左右道路边界点，采用随机采样一致性二次多项式拟合方法分别对左右道路边界点进行曲线拟合，即得到道路边界线，其中对精提取的左右道路边界点，改变距离阈值T₁₂采用步骤531-533的方法，得到“最佳”的左右道路边界曲线模型，如公式(12)所示：

y＝a_optimalx²+b_optimalx+c_optimal (12)

其中a_optimal，b_optimal，c_optimal为通过随机采样一致性拟合方法得到的最佳的边界曲线模型参数。

所述的步骤七的具体方法为：

根据GPS/IMU提供的帧间姿态变换信息，利用幅值滤波卡尔曼滤波算法对所述的边界线进行跟踪，包括以下步骤：

71)判断是否检测到道路边界；

72)如果没有检测到道路边界，则根据上一次最终的道路边界的位置利用公式(13)对当前道路边界进行预测，预测结果就作为算法最终输出的道路边界线位置；

x(k+1)＝A(k)x(k)+B(k)u(k) (13)

其中x(k)为边界线上的坐标点，γ_frame为相邻两帧之间车辆的航向角变化量，x_frame,y_frame分别为相邻两帧车辆沿x,y方向的位移，A(k),B(k)分别表示状态变化矩阵和输入矩阵，u(k)表示输入向量；；

73)如果检测到道路边界，采用边界线参数幅值滤波器移除假正检测，然后将通过幅值滤波器的道路边界线作为真正的测量值；首先利用卡尔曼滤波器对道路边界线进行一次预测，结合本次的测量值更新道路边界线的位置，其中采用边界线参数幅值滤波器移除假正检测包括：计算当前检测的边界线参数与上一帧获得的边界线参数的差值，如果三个差值都小于各自的阈值，则将当前检测的道路边界线作为真正的测量值。

本发明的有益效果为：

1、本发明首先采用基于随机采样一致性拟合平面的方法快速的粗提取地面点，排除了道路内外具有明显高度的障碍物的影响；然后采用主成分分析法对每个点的球形邻域判定是否具有平面特征，有效排除了道路内外高度介于路面和路沿之间的非面特征障碍点的影响；

2、本发明在提取的地面点基础上，采用一种多特征、宽阈值、分层次的道路边界点的提取方法，该方法采用了邻域高度差特征，邻域平滑特征，切向量夹角特征，邻域高度方差特征，邻域水平距离特征，法向量夹角特征共六个特征，每个特征阈值较宽泛，分别对每个激光扫描线(层)提取道路边界特征点，可靠性高，适用于多种路况；

3、本发明在粗提取的道路边界点上，利用线性判别分析的分类思想，采用基于投影的左右道路边界点分类方法，将粗提取的道路边界点分为两类。相比于简单地根据特征点的横向坐标区别左右边界点的方法，该方法对弯曲道路、车辆转弯等工况有很好的适应性；

4、本发明在粗提取的左右道路边界点上，对每个扫描线在激光雷达x轴正方向和负方向分别提取横向距离车辆最近的两个点，构成新的左右道路边界特征点，该方法可有效排除在道路外存在于道路边界有相似障碍物的路况。例如，存在与道路同方向铁路轨道，多条道路并排的工况等；

5、本发明在新的左右道路边界特征点基础上，通过采用随机采样一致性的方法进行滤波，再此基础上再一次采用随机采样一致性进行拟合道路边界线，通过两次随机采样一致性方法对道路内障碍点有很强的抗干扰性，同时采样拟合模型利用二次多项式，对弯曲道路有很好的适应性；

6、本发明在检测的基础上，采用卡尔曼滤波算法对道路边界线进行了跟踪，同时在跟踪的更新过程中，采用了幅值滤波器滤除了假正检测，保证了算法的稳定性和可靠性，在针对误检、漏检等情况有较强的适应性。

附图说明

图1为本发明的流程图；

图2为本发明中一帧激光雷达数据在直角坐标系下的显示图；

图3为本发明中地面点提取结果图；

图4为本发明中切向量夹角特征和邻域水平距离特征的示意图；

图5为本发明中道路边界特征点的粗提取结果图；

图6为本发明中横向最近边界特征点提取结果图；

图7为本发明中道路边界点精提取结果图；

图8为本发明中精提取道路边界点在原始点云中的高亮显示结果图；

图9为本发明中道路边界线拟合结果图。

具体实施方式

实施例

本实施例提供一种基于激光雷达扫描的道路边界检测方法，可以用于多种道路工况下检测道路边界。本实施方式不限于某一款激光雷达，为了具体描述本方法，我们以三维激光雷达Velodyne HDL-64E为例对本方法进行分析。该传感器由64个激光发射单元组成，分成上下两层，各32个发射单元，垂直视角为-24.8°-+2°，垂直分辨率为0.4°，水平视场角为360°，水平分辨率为0.09°，以10HZ的频率水平旋转，每秒产生100多万个点。将雷达旋转一周采集的数据作为一帧，雷达输出的数据是以球体坐标的方式表示的，每个点参数有垂直方位角α，水平方位角β和点到激光雷达中心的距离γ。

参阅图1，本实施例有如下步骤：

步骤1以三维激光雷达的中心为坐标原点O_l，过原点且指向无人车前进方向为x轴，过原点且垂直指向上方为z轴，过原点且垂直于x轴和z轴所构成的平面指向无人车前进方向左侧的方向为y轴,建立三维激光雷达的直角坐标系O_lxyz；获取三维激光雷达扫描一周返回的所有数据作为一帧点云数据，并将该帧点云极坐标数据转换到三维激光雷达的直角坐标系下，如图2所示为变换到直角坐标系下的一帧原始点云；

为了尽可能的减少遮挡和增大激光雷达的探测范围，本实施例中将HDL-64E三维激光雷达安装在我们无人驾驶车的车顶位置，并且雷达的旋转轴线与地面垂直。

按照式(一)将三维激光雷达扫描点的极坐标转换到无人车的直角坐标系下：

其中，i为激光雷达扫描点在一帧点云中的序号，γ_i为第i个扫描点的极坐标距离，α_i为第i个扫描点的所在扫描线的垂直方位角，β_i为第i个扫描点的水平方位角。

步骤2，由于在1个扫描周期内，车辆的运动会使雷达原点的位姿发生显著的移动。此外当无人车在起伏的道路上行驶时，车辆的位姿会出现起伏，左右晃动，这也会改变传感器的坐标原点。因此需要根据GPS/IMU提供的每一帧位姿变化信息，包括翻滚角α_frame，俯仰角β_frame，航向角γ_frame以及位移x_frame，y_frame，z_frame，对每帧点云进行畸变矫正。

具体步骤包括：

步骤2-1，根据每个点P_i的水平方位角β_i，计算该点相对于扫描起始位置的时间偏移t。假设当前帧的扫描起始位置对应的水平方位角为β₀，那么通过式(二)计算雷达扫描到θ所需要的时间：

t_i＝0.1×(β_i-β₀)/2π (二)

其中激光雷达扫描一周的时间为0.1s。

步骤2-2，假设在同一帧内车辆的位姿近似线性变化，因此每一时刻车辆的位姿可以通过相邻两帧的位姿插值获得。根据GPS/IMU提供的每一帧车辆位姿变换信息，采用线性插值计算变换矩阵如下：

[θ_Xi θ_Yi θ_Zi]＝(-t_i/0.1)[θ_Xframe θ_Yframe θ_Zframe] (三)

步骤2-3，根据上述所计算得到的变换矩阵，采用公式(四)将点P_i矫正如下，点P′_i为矫正后的点：

R_i＝R_z(θ_Xi)R_y(θ_Yi)R_x(θ_Zi),

P_i′＝R_iP_i+T_i (四)

步骤3，当提取道路边界点时，路面内的存在行人，车辆，树木，建筑等会影响边界特征点的提取，增加边界检测的难度，道路边界属于地面的一部分，因此可以通过地面提取的方法将路面以上的干扰障碍物滤除。本发明采用先通过随机采样一致性方法粗提取，然后通过对每个点邻域主成分分析的方法滤除过，来进一步滤除高度介于路面和路沿之间的障碍点，提取的地面点如图3所示。

具体步骤包括：

步骤3-1，采用随机采样一致性平面拟合方法粗提取地面点。

步骤3-1所述的采用随机采样一致性平面拟合方法粗提取地面点包括：

步骤3-1-1，随机从矫正的点云中任选三个点，计算通过这三个点的平面模型参数；

步骤3-1-2，用得到的平面模型去测试其他点，由点到平面的距离阈值T₀确定其他点是否为局内点或者为局外点，本实施例中该阈值设置为0.2m。

步骤3-1-3，如果局内点足够多，并且局内点多于原有的“最佳”局内点，那么将这次迭代后的所有局外点作为干扰点进行滤除；

步骤3-2，对所述粗提取的地面点，通过对每个点的球形邻域进行主成分分析(PCA)，判定每个点是否具有平面特征来滤除高度介于路面和道路边界的障碍物点。

步骤3-2，对所述粗提取的地面点，通过对每个点的球形邻域进行主成分分析(PCA)，判定每个点是否具有平面特征来滤除高度介于路面和道路边界的障碍物点。

步骤3-2-1，采用公式(五)对每个点的球形邻域点计算协方差矩阵C和相应的规范化特征值λ′_j(j＝1，2，3，λ′₁≥λ′₂≥λ′₃):

其中，K为球形邻域点的个数，本实施例中邻域点个数K为50，P_i为第i个点的坐标位置向量，

为球形邻域点的均值向量，λ_j为协方差矩阵的特征值，λ′_j为规范化后的特征值。

步骤3-2-2，根据上述计算得到的规范化特征值判定每个点是否具有平面特征，如果满足以下条件则将该点保留，否则将剔除：

T₁≥λ′₁≥λ′₂≥T₂

T₃≥λ′₃≥T₄ (六)

其中本实施例中T₁为0.63，T₂为0.3，T₃为0.1，T₄为0。

步骤4，对上述提取的地面点，采用本发明提出的多特征，宽阈值，分层次的道路边界提取方法，分别每个扫描线粗提取道路边界点，粗提取的边界点如图5所示，从图中可以看出由于道路内部车辆，道路外部铁路轨道影响，粗提取的特征点包含多种干扰点；

步骤4-1，采用多特征，宽阈值，分层次的道路边界特征点提取方法分别对当前扫描线每个点根据下列六个特征进行判定，并标记，具体包括以下步骤：

步骤4-1-1，在理想的道路模型中，路沿和路面之间总是会有一个高度差，选用邻域高度差作为道路边界的一个特征。对该扫描线中的第i个扫描点，在同一扫描线上提取该点的邻域中的最大高度和最小高度，并计算它们的差值作为该扫描点i的邻域高度差特征值，如果高度差大于T₅并且小于T₆，那么该点满足邻域高度差特征条件，被选为候选特征点，本实施例中T₅为0.1，T₆为0.3；

步骤4-1-2，对该扫描线中的第i个扫描点，在同一扫描线采用公式(7)计算该点的邻域平滑特征值s，如果该平滑值特征值大于阈值T₇,那么该点满足邻域平滑特征条件，被选为候选特征点；

其中M表示第i个扫描点P_i的邻域点集合，集合中有一半点在P_i的左边，有一半点在P_i的右边，|M|表示集合M的势，本实施例中T₇为0.2，M为10。

步骤4-1-3，如图4所示，B点位于路面，两侧的切向量夹角将近180度，点A为道路边界点，两侧切向量小于180度，因此选取扫描点切向量夹角作为道路边界的一个特征。对该扫描线中的第i个扫描点，在同一扫描线采用公式(八)计算扫描线在该点的切向量夹角特征值θ_i，如果该特征值小于阈值T₈，被选为候选特征点：

其中，m_θ为用于计算扫描线在该点切向量的邻域点个数，V_a和V_b分别表示该扫描点沿着两侧扫描线的切向量，本实施例中m_θ为5，T₈为160；

步骤4-1-4，在同一扫描线中，连续的道路边界点的高度会呈现递增的趋势，因此边界点高度方向较分散，因此将邻域高度方差作为道路边界的一个特征。对该扫描线中第i个扫描点，提取同一扫描线上该点所有邻域点的高度值，并计算它们的方差作为扫描线在该点的邻域高度方差特征值，如果特征值大于阈值T₉，被选为候选特征点，本实施例中T₉为0.2；

步骤4-1-5，如图4所示，道路边界上的点A与相邻的下一个道路边界点的水平距离接近为0，在理想水平路面上的点B与相邻的下一个道路边界点的水平距离可依据该点的垂直方位角和高度值计算出来。因此将邻域水平距离作为道路边界点的一个特征。对该扫描线中第i个扫描点，计算该点与同一扫描线的相邻点的水平距离作为扫描线在该点的邻域水平距离特征值，如果该特征值小于阈值T₁₀，被选为候选特征点，该阈值T₁₀可通过公式(9)计算得到：

其中，H_s为该扫描点的高度的绝对值，θ_l为该扫描线的垂直方位角，θ_a为激光雷达的水平分辨率，在本实施例中θ_a为0.09；

步骤4-1-6，如图4所示，道路边界点C的法向量n_c与激光雷达坐标系y轴近似平行，而地面点B的法向量n_b与y轴近似垂直，因此将法向量夹角作为道路边界点的一个特征。对该扫描线中的第i个扫描点，计算该点的法向量与激光雷达y坐标的夹角作为该点的法向量夹角特征值，如果该特征值小于阈值T₁₁，则被选为候选特征点，本实施例中考虑到弯曲道路工况，将T₁₁设置为50；

步骤4-2，对每条扫描线提取同时满足上述六个特征条件(邻域高度差特征，邻域平滑特征，切向量夹角特征，邻域高度方差特征，邻域水平距离特征，法向量夹角特征)的点作为粗提取的道路特征点。

步骤5，粗提取后的边界点还存在多种障碍干扰点，包括道路内车辆、行人，道路外建筑物、铁轨、并排的相邻道路等。所以需要采用本发明提出的道路特征点精提取方法对上述粗提取的道路边界点进行精提取。

步骤5-1，受线性判别分析思想的启发，本发明提出了基于投影的左右道路边界点分类方法将将粗提取的道路边界特征点划分为两组，分别表示左道路边界特征点和右道路边界特征点，具体包括以下步骤；

步骤5-1-1，首先将所有粗提取的特征点投影到xy平面，并假设激光雷达坐标系x轴与道路边界夹角为θ(考虑到一般转弯和弯道工况，夹角较小，设置为-50≤θ≤50，θ为整数)，将粗提取的边界特征点绕z旋转θ角度进行变换，使道路边界与激光雷达x近似平行，然后将变换后的边界特征点投影到y轴上，得到投影后每个特征点的坐标；

步骤5-1-2，根据投影后的每个特征点的y坐标正负将投影后的特征点分为两类，根据公式(10)计算分类标准得分值：

C_r＝|E(P_θ，L)-E(P_θ，R)|/(S(P_θ，L)+S(P_θ，R)) (十)

其中，E(P_θ，L)和E(P_θ，R)分别为投影后左道路边界特征点和右道路边界特征点的均值，S(P_θ，L)和S(P_θ，R)分别为投影后左道路边界特征点和右道路边界特征点的方差；

步骤5-1-3，对θ假设范围内的每个角度按照步骤5-1-1和5-1-2计算出相应的分类标准得分值C_r，使得分值最大的角度θ_optimal即为车辆与道路边界的夹角；

步骤5-1-4，根据上述得到的车辆与道路边界的夹角θ_optimal，首先将步骤4中粗提取的道路边界特征点进行绕z旋转θ_optimal角进行变换，然后根据变换后的点的y坐标正负将粗提取道路边界特征点分为两组，然后再将两组特征点绕z轴旋转-θ_optimal角度变换，得到左道路边界特征点和右道路边界特征点；

步骤5-2，粗提取后的特征点中包括了很多道路外障碍干扰点，例如建筑物，并排的其他道路，铁路轨道等，实际工况中道路边界往往时横向距离车辆最近的，因此从上述左右道路边界特征点中选取横向距离主车最近的边界特征点作为新的左右道路边界特征点。对上述两组边界特征点，对每个扫描线在激光雷达达yz平面上半空间和下半空间分别提取横向距离车辆最近的两个点，构成左右横向最近道路边界特征点，如图6所示通过该方法滤除了大部分由于铁路轨道造成的干扰点，但是道路内部还存在由于其他车辆造成的干扰点；

步骤5-3，通过步骤5-2滤除了道路外的障碍干扰点的影响，但是再道路内存在车辆行人时，同样会造成干扰，因此需要采用本发明提出的基于随机采样一致性的方法滤除道路内障碍干扰点的影响。对上述新的左右道路边界特征点，分别采用二次多项式随机采样一致性滤波方法滤除路内障碍物干扰点，得到精提取后的左右道路边界点，如图7所示，通过上述两步基本将道路内外的其他干扰点都滤除了。具体步骤包括：

步骤5-3-1，随机地从新的左道路边界特征点中任选三个点，按照公式(十一)拟合得到通过这三个点的二次多项式曲线，由于边界线特征点的高度变化较小，而且对无人车安全行驶的影响不大，因此在拟合滤波的过程中只考虑特征点xy坐标，将z坐标统一为所有特征点z坐标的均值：

y＝ax²+bx+c

z＝mean(z_i)i＝1…n (十一)

其中n为新的左道路边界特征点的个数。

步骤5-3-2，用得到的二次曲线模型去测试其他点，由点到曲线的距离阈值T₁₂确定其他点是否为局内点或者为局外点，该是实施例中阈值T₁₂为0.2。

步骤5-3-3，重复步骤5-3-1和5-3-2，直到找到具有最多内点的曲线模型并将其作为“最佳”曲线模型，并根据该模型按照步骤5-3-2将局外点作为干扰点滤除，得到精提取后的左道路边界点；

更换新的右道路边界特征点，按照步骤5-3-1到5-3-3，滤除右边界点中的干扰点，得到精提取后的右道路边界特征点。

步骤6，对所述精提取的道路边界点，采用随机采样一致性二次多项式拟合方法分别对左右道路边界点进行拟合，即改变距离阈值T₁₂采用步骤5-3-1到5-3-3的方法，得到“最佳”的左右道路边界曲线模型，如公式(十二)所示，此时设置阈值T₁₂为0.05，拟合边界曲线如图9所示：

y＝a_optimalx²+b_optimalx+c_optimal (十二)

其中a_optimal，b_optimal，c_optimal为通过随机采样一致性拟合方法得到的最佳的边界曲线模型参数。

步骤7，根据GPS/IMU提供的帧间姿态变换信息，利用卡尔曼滤波算法对上述的左右道路边界线进行跟踪，具体步骤如下：

步骤7-1，判断是否检测到道路边界；

步骤7-2，如果没有检测到道路边界，则根据上一次最终的道路边界的位置利用公式(十三)对当前道路边界进行预测，预测结果就作为算法最终输出的道路边界线位置；

x(k+1)＝A(k)x(k)+B(k)u(k) (十三)

其中x(k)为边界线上的坐标点，由于在忽略高度的影响，通过三点就可以拟合一条曲线，因此在本实施例中只跟踪拟合的边界线上的三个固定点。

步骤7-3，如果检测到道路边界，采用边界线参数幅值滤波器移除假正检测，然后将通过幅值滤波器的道路边界线作为真正的测量值。首先利用卡尔曼滤波器对道路边界线进行一次预测，结合本次的测量值更新道路边界线的位置。

步骤7-3所述采用边界线参数幅值滤波器移除假正检测包括：计算当前检测的边界线参数与上一帧获得的边界线参数的差值，如果三个差值都小于各自的阈值，则将将当前检测的道路边界线作为真正的测量值。a，b，c三个参数的阈值分别为0.1，0.2，0.5。

综上，本发明能够处理多种复杂场景和工况，包括弯曲道路，转弯，道路内外存在干扰障碍物等。同时基于幅值滤波的卡尔曼滤波方法可以很好的处理误检和漏检等特殊情况，增强了算法的鲁棒性和适应性，同时算法具有良好的实时性，可以广泛应用于无人驾驶。

Claims (7)

1.一种基于三维激光雷达的道路边界检测与跟踪方法，其特征在于，该方法包括以下步骤：

步骤一、获取激光雷达扫描数据并转换到激光雷达直角坐标系下；

步骤二、根据GPS/IMU提供的位姿变化信息，对点云数据进行畸变矫正；

步骤三、采用随机采样一致性和主成分分析法提取地面点；

步骤四、采用多特征、宽阈值、分层次的特征点提取方法对边界特征点进行粗提取；

步骤五、采用基于投影的方法将粗提取边界特征点分成左右两类，并采用随机采样一致性方法对边界特征点进行精提取；

步骤六、采用随机采样一致性方法对精提取的边界点进行曲线拟合；

步骤七、采用幅值滤波的卡尔曼滤波方法对边界线进行跟踪；

所述的步骤二的具体方法为：

根据GPS/IMU提供的每一帧位姿变化信息，包括翻滚角θ_Xframe，俯仰角θ_Yframe，航向角θ_Zframe以及位移x_frame，y_frame，z_frame，对每帧点云进行畸变矫正，包括以下步骤：

21)根据每个点P_i的水平方位角β_i，计算该点相对于扫描起始位置的时间偏移t_i，假设当前帧的扫描起始位置对应的水平方位角为β₀，那么通过式(2)计算雷达扫描到β_i所需要的时间：

t_i＝C×(β_i-β₀)/2π (2)

其中C为激光雷达扫描一周的时间；

22)假设在同一帧内车辆的位姿近似线性变化，因此每个点P_i对应时刻，车辆的位姿可以通过相邻两帧的位姿插值获得；根据GPS/IMU提供的每一帧车辆位姿变换信息，采用线性插值计算变换矩阵如下：

[θ_Xi θ_Yi θ_Zi]＝(-t_i/C)[θ_Xframe θ_Yframe θ_Zframe] (3)

其中T_i为帧起始扫描点到该时刻车辆的位移，θ_Xi，θ_Yi，θ_Zi分别为帧起始扫描点到该时刻车辆的翻滚角，俯仰角和航向角；

23)根据上述所计算得到的变换矩阵，采用公式(4)将点P_i

矫正如下，点P′_i为矫正后的点：

R_i＝R_z(θ_Zi)R_y(θ_Yi)R_x(θ_Xi),

P′_i＝R_iP_i+T_i (4)

其中，R_i为从起始扫描点到P_i对应时刻车辆位姿的旋转矩阵，R_z(θ_Zi)，R_y(θ_Yi)，R_x(θ_Xi)分别为绕z,y,x三轴的旋转矩阵。

2.根据权利要求 1 所述的一种基于三维激光雷达的道路边界检测与跟踪方法，其特征在于， 所述的步骤一的具体方法为：

11)以三维激光雷达的中心为坐标原点O_l，过原点O_l且指向无人车前进方向为x轴，过原点O_l且垂直指向上方为z轴，过原点且垂直于x轴和z轴所构成的平面指向无人车前进方向左侧的方向为y轴,建立三维激光雷达的直角坐标系O_lxyz；

12)获取三维激光雷达扫描一周返回的所有数据作为一帧点云数据，并将该帧点云极坐标数据转换到三维激光雷达的直角坐标系下，具体包括以下步骤：

公式(1)将三维激光雷获取的点云的极坐标转换到三维激光雷达的直角坐标系下：

其中，i为激光雷达扫描点在一帧点云中的序号，γ_i为第i个扫描点距离激光雷达原点的极坐标距离，α_i为第i个扫描点的垂直方位角，β_i为第i个扫描点的水平方位角。

3.根据权利要求1所述的一种基于三维激光雷达的道路边界检测与跟踪方法，其特征在于，所述的步骤三的具体方法为：

对畸变矫正后的点云进行地面提取，滤除掉地面以上的障碍物，包括以下步骤：

31)采用随机采样一致性平面拟合方法粗提取地面点，具体包括以下步骤：

311)随机从矫正的点云中任选三个点，得到通过这三个点的平面模型；

312)用得到的平面模型去测试其他点，由点到平面的距离阈值T₀确定其他点是否为局内点或者为局外点；

313)重复步骤311-312，直到找到具有最多内点的平面模型并将其作为“最佳”平面模型，并根据该模型按照步骤312将局外点作为非地面点滤除，得到粗提取的地面点；

32)对所述粗提取的地面点，通过对每个点的球形邻域进行主成分分析(PCA)，判定每个点是否具有平面特征来滤除高度介于路面和道路边界的障碍物点，具体方法为：

321)采用公式(5)对每个点的球形邻域点计算协方差矩阵C和相应的规范化特征值λ′_j(j＝1,2,3，λ′₁≥λ′₂≥λ′₃):

其中，K为球形邻域点的个数，P_i为第i个邻域点的坐标位置向量，

为球形邻域点的均值向量，λ_j为协方差矩阵的特征值，λ′_j为规范化后的特征值；

322)根据上述计算得到的规范化特征值判定每个点是否具有平面特征，如果满足以下条件则将该点保留，否则将剔除：

T₁≥λ′₁≥λ′₂≥T₂ T₃≥λ′₃≥T₄ T₂>T₃ (6)

其中T_i(i＝1,2,3,4)为平面特征点阈值。

4.根据权利要求1所述的一种基于三维激光雷达的道路边界检测与跟踪方法，其特征在于，所述的步骤四的具体方法为：

对步骤三提取的地面点，采用一种多特征，宽阈值，分层次的提取方法分别粗提取每个扫描线的道路边界特征点，包括以下步骤：

41)根据每个特征条件对每个点进行判定，并标记；具体包括以下步骤：

411)对扫描线中的第i个扫描点，在同一扫描线上提取该点的邻域中的最大高度和最小高度，并计算它们的差值作为该扫描点i的邻域高度差特征值，如果高度差大于高度差特征上界域值T₅并且小于高度差特征下界域值T₆，那么该点满足邻域高度差特征条件，被选为候选特征点；

412)对该扫描线中的第i个扫描点，在同一扫描线采用公式(7)计算该点的邻域平滑特征值s，如果该平滑值特征值大于阈值T₇,那么该点满足邻域平滑特征条件，被选为候选特征点；

其中M表示第i个扫描点P_i的邻域点集合，集合中有一半点在P_i的左边，有一半点在P_i的右边，|M|表示集合M的势；

413)对该扫描线中的第i个扫描点，在同一扫描线采用公式(8)计算扫描线在该点的切向量夹角特征值θ_i，如果该特征值小于阈值T₈，被选为候选特征点：

其中，m_θ为用于计算扫描线在该点切向量的邻域点个数，V_a和V_b分别表示该扫描点沿着两侧扫描线的切向量，x,y分别扫描点的横纵方向坐标；

414)对该扫描线中第i个扫描点，提取同一扫描线上该点所有邻域点的高度值，并计算它们的方差作为扫描线在该点的邻域高度方差特征值，如果特征值大于阈值T₉，被选为候选特征点；

415)对该扫描线中第i个扫描点，计算该点与同一扫描线的相邻点的水平距离作为扫描线在该点的邻域水平距离特征值，如果该特征值小于阈值T₁₀，被选为候选特征点，该阈值T₁₀可通过公式(9)计算得到：

其中，H_s为该扫描点的高度的绝对值，θ_l为该扫描线的垂直方位角，θ_a为激光雷达的水平分辨率；

416)对该扫描线中的第i个扫描点，计算该点的法向量与激光雷达y坐标的夹角作为该点的法向量夹角特征值，如果该特征值小于阈值T₁₁，则被选为候选特征点；

42)对该扫描线提取满足所有特征条件的点作为粗提取的道路边界点。

5.根据权利要求1所述的一种基于三维激光雷达的道路边界检测与跟踪方法，其特征在于，所述的步骤五的具体方法为：

对粗提取的道路边界特征点，采用基于投影的方法将粗提取的边界特征点分成左右两类，提取距离主车横向最近特征点，并利用随机采样一直性方法滤波，得到精提取得到精提取的左右道路边界点，包括以下步骤：

51)借鉴线性判别分析的思想，采用基于投影的左右道路边界点分类方法将将粗提取的道路边界特征点划分为两组，分别表示粗提取的左道路边界特征点和右道路边界特征点；具体包括以下步骤：

511)首先将所有粗提取的特征点投影到xy平面，并假设激光雷达坐标系x轴与道路边界夹角为θ，-50≤θ≤50，θ为整数，将粗提取的边界特征点绕z旋转θ角度进行变换，使道路边界与激光雷达x轴平行，然后将变换后的边界特征点投影到y轴上，得到投影后每个特征点的坐标；

512)根据投影后的每个特征点的y坐标正负将投影后的特征点分为两类，根据公式(10) 计算分类标准得分值：

C_r＝|E(P_θ,L)-E(P_θ,R)|/(S(P_θ,L)+S(P_θ,R)) (10)

其中，E(P_θ,L)和E(P_θ,R)分别为投影后左道路边界特征点和右道路边界特征点的均值，S(P_θ,L)和S(P_θ,R)分别为投影后左道路边界特征点和右道路边界特征点的方差；

513)对θ假设范围内的每个角度按照步骤511和512分别计算出相应的分类标准得分值C_r，使得分值最大的角度θ_optimal即为车辆与道路边界的夹角；

514)根据上述得到的车辆与道路边界的夹角θ_optimal，首先将步骤四中粗提取的道路边界特征点进行绕z旋转θ_optimal角进行变换，使道路边界与激光雷达x轴平行，然后根据变换后的点的y坐标正负将粗提取道路边界特征点分为两组，然后再将两组特征点绕z轴旋转-θ_optimal角度变换，得到左道路边界特征点和右道路边界特征点；

52)对上述两组边界特征点，对每个扫描线在激光雷达yz平面上半空间和下半空间分别提取横向距离车辆最近的两个点，构成横向距离最近左右道路边界特征点；

53)对上述新的左右道路边界特征点，分别采用二次多项式随机采样一致性滤波方法滤除路内障碍物干扰点，得到精提取后的左右道路边界点，具体包括以下步骤：

531)随机地从新的左道路边界特征点中任选三个点，按照公式(11)拟合得到通过这三个点的二次多项式曲线：

y＝ax²+bx+c

z＝mean(z_i) i＝1…n (11)

其中z_i表示每个边界特征点的z方向坐标值，mean表示对所有特征点的z坐标取均值；

532)用得到的二次曲线模型去测试其他点，由点到曲线的距离阈值T₁₂确定其他点是否为局内点或者为局外点；

533)重复步骤531-532，直到找到具有最多内点的曲线模型并将其作为“最佳”曲线模型，并根据该模型按照步骤532将局外点作为干扰点滤除，得到精提取后的左道路边界点，更换新的右道路边界特征点，按照步骤531-533，滤除右边界点中的干扰点，得到精提取后的右道路边界特征点。

6.根据权利要求5所述的一种基于三维激光雷达的道路边界检测与跟踪方法，其特征在于，所述的步骤六的具体方法为：

对精提取的左右道路边界点，采用随机采样一致性二次多项式拟合方法分别对左右道路边界点进行曲线拟合，即得到道路边界线，其中对精提取的左右道路边界点，改变距离阈值T₁₂采用步骤531-533的方法，得到“最佳”的左右道路边界曲线模型，如公式(12)所示：

y＝a_optimalx²+b_optimalx+c_optimal (12)

其中a_optimal，b_optimal，c_optimal为通过随机采样一致性拟合方法得到的最佳的边界曲线模型参数。

7.根据权利要求1所述的一种基于三维激光雷达的道路边界检测与跟踪方法，其特征在于，所述的步骤七的具体方法为：

根据GPS/IMU提供的帧间姿态变换信息，利用幅值滤波卡尔曼滤波算法对所述的边界线进行跟踪，包括以下步骤：

71)判断是否检测到道路边界；

72)如果没有检测到道路边界，则根据上一次最终的道路边界的位置利用公式(13)对当前道路边界进行预测，预测结果就作为算法最终输出的道路边界线位置；

x(k+1)＝A(k)x(k)+B(k)u(k) (13)

其中x(k)为边界线上的坐标点，γ_frame为相邻两帧之间车辆的航向角变化量，x_frame,y_frame分别为相邻两帧车辆沿x,y方向的位移，A(k),B(k)分别表示状态变化矩阵和输入矩阵，u(k)表示输入向量；

73)如果检测到道路边界，采用边界线参数幅值滤波器移除假正检测，然后将通过幅值滤波器的道路边界线作为真正的测量值；首先利用卡尔曼滤波器对道路边界线进行一次预测，结合本次的测量值更新道路边界线的位置，其中采用边界线参数幅值滤波器移除假正检测包括：计算当前检测的边界线参数与上一帧获得的边界线参数的差值，如果三个差值都小于各自的阈值，则将当前检测的道路边界线作为真正的测量值。

Images