激光雷达辅助驾驶道路参数计算方法研究

游安清，潘旭东，赵 平，潘文武

（1.中国工程物理研究院 高能激光科学与技术重点实验室，四川 绵阳 621900；2.中国工程物理研究院 应用电子学研究所，四川 绵阳 621900）

引言

在一些特定场合，比如特定的军事转移过程中，需要用一辆小型导引车对转移路线上的道路路况进行实时勘测，为后面大部队大型车辆的可通过性提供路线决策和辅助驾驶依据，包括道路宽度、限高、拐弯半径等因素是否支持大型车辆通过。为此，需要在前端导引车上配备激光雷达辅助驾驶系统[1-4]，实时扫描路况并快速计算道路参数。与二维图像不同，三维激光点云属于无序离散图像，其处理方法远不如二维图像成熟。本文设计一种基于点云垂直切片的路宽、限高计算方法，可以有效解决城区结构化道路的参数提取[5-11]问题。并设计一种基于载车位姿数据最小二乘拟合的拐弯半径全自动、快速计算方法，通过与Terrasolid 等经典激光点云处理软件相比，不仅效率上有大幅提高，而且精度也更高。

1 系统组成

本系统在硬件上采用“VLP 多线激光扫描头+惯性导航单元IMU + 数据处理单元DPU”的构架如图1 所示。激光扫描头完成对载车周围景物及道路路面的激光扫描；惯性导航单元实时测量系统所处的地理位置和在三维空间所处的姿态[12-13]；数据处理单元运行数据解算软件，完成激光点云数据预处理、三维重构、点云分割、道路提取、路面参数计算等所有算法功能[14-15]。

图1 车载辅助驾驶系统的系统Fig.1 Vehicle-mounted auxiliary driving system

2 点云重构

跟传统激光雷达一样，点云解算方法及其应用软件开发也非常重要。为了在车载平台上对道路进行三维扫描、并构建三维点云图，必须定义3 个坐标系。

1)扫描仪局部坐标系：如图1，y轴平行于VLP16 扫描仪的轴线、指向天顶；x轴在扫描仪的横截面内、垂直y轴、指向车右侧方向；z轴经过扫描仪的0°光束方向的反方向、指向车尾。

2)载车固连坐标系：取为车载惯导系统(IMU)的轴系。

3)世界坐标系：一个相对静止的全局坐标系，可以任意自定义，比如载车出发地的地平坐标系。

车载三维扫描时，扫描仪局部坐标系置于载车固连坐标系之上。由于存在安装关系，所以这两个坐标系并不平行、原点也不重合，设扫描仪局部坐标系相对载车固连坐标系有(α1，β1，γ1)的姿态角(这3 个角的绕转轴分别是y、x、z，可分别称为航向角、俯仰角、滚转角)，扫描仪局部标系相对载车固连坐标系的原点偏移为(u1，v1，w1)；同理，载车固连坐标系相对世界坐标系也有姿态角和原点偏移，设为(α2，β2，γ2)和(u2，v2，w2)。

有了这些姿态和位移量，就可以将扫描仪的测点坐标(r，θ)转换成全局点云坐标P(x，y，z)：

式中：Raxis(·)为3×3 的右手螺旋坐标旋转矩阵。

上式模型的12 个位姿参数中，(α1，β1，γ1，u1，v1，w1)与扫描仪在载车上的安装角和安装位置有关，提前标定获得；(α2，β2，γ2，u2，v2，w2)可由载车上的惯导系统实时给出。依此模型，就可在载车前进的同时，实现点云重构和整体三维场景的“逐渐生长”，图2 为示例。

图2 激光雷达道路场景扫描示例Fig.2 Scanning road with LIDAR

3 道路参数提取

3.1 道宽、限高计算与实验

本文设计一种通过点云法截面网格投影计算道路宽度、限高的方法，步骤如下：

1) 设定算法参数，包括网格尺寸s(等于所期望的精度水平比，如5 cm)、网格内非噪点的最小点数阈值Nmin(比如5 个点/格)、点云切片层厚度d(比如5 m)、单边路宽最大值a(比如20m)、上方限高物最大高度b(比如15 m)。这些参数的取值是通过事后离线数据分析、参数尝试和算法效果测试确定出来的，它们与激光雷达点频率、载车行进速度等作业配置有关。

2) 根据惯导给出的载车位姿数据序列，计算生成载车的轨迹曲线，如图3 所示；根据轨迹曲线，计算轨迹上每个位置点处载车行进的方向，以及该方向的法截面方程。

图3 道路点云与载车轨迹Fig.3 Road point cloud and truck track

3) 在每个轨迹点处，垂直于行进方向，切出厚度为d的点云层，并按尺度s作二维网格划分，如图4 所示。

图4 道路点云切片及网格化Fig.4 Point cloud slice and mesh

4) 从载车上扫描仪中心位置(图4 中粉色实心圆点)所在网格起，往上取若干格(比如至1.5 m 处为止)、往下取若干格(比如至-1 m 处为止)，组成一个竖直网格条。将该网格条逐渐向左平移，直至碰到内含点数大于Nmin的网格阻挡为止，该位置定为路的左边界；再将竖直网格条向右平移，直至碰到内含点数大于Nmin的网格阻挡为止，该位置定为路的右边界；计算左右边界间的距离，得路的宽度W。

5) 以中心网格及其左右边界之间的网格一起组成一个水平网格条，将该网格条自中心位置向上平移，直至碰到内含点数大于Nmin的网格阻挡为止，该位置定为路的上方净空高度H。路宽W和上方净高H，共同构成该路段的可通过空间(一个矩形范围)。

6) 几种常见情形：

a) 如果往左/右平移时始终未碰到有阻挡的网格，则以设定的最大单边路宽a作为该侧路宽。

b) 如果往上平移始终未碰到有阻挡的网格，则认为该段路上方是开阔空域、没有限高。

c)当载车经过上方有广告牌、交通灯横杆等物体时，会测量得到有效限高。

d)当载车从架空电线或桥梁下面经过时，会测量得到上方电线或桥梁的相对高度。

e)当载车从隧道内经过时，会同时扫描测到隧道的左右宽度和顶面高度。

在户外场景开展了初步的验证实验，在城区结构化道路上效果良好，如图5 所示，图中上方有线缆限高物，黄色门形框为车辆可通过的矩形范围。

相对文献中的方法，此方法的优点是不假设路边沿是直线或参数化曲线、不作直(曲)线拟合，完全借助当前截面的点云分布分析确定路宽与限高，所以计算量小、速度快，缺点是对沿路方向的关联信息利用不足、对路的结构化(即路沿的凸起性)依赖较大。

图5 上方有线缆和桥梁限高的路段Fig.5 Limited above by cables and bridge

3.2 拐弯半径计算

道路的拐弯半径可以通过对路道轨迹曲线作圆拟合、求取圆半径的方法得到。算法步骤如下：

1) 设定道路曲线拟合的每段路段长度为L(比如每25 m 拟合一个结果)。

2) 从惯导的位姿数据中取出载车行进过程中各个时刻的位置，得到离散的载车轨迹序列。

3) 对载车原始轨迹进行滤波，使轨迹变得平滑，抑制必然存在的位置噪声对后续曲线拟合的影响。

4)对离散的轨迹点序列进行等间距样条插值，得到均匀分布的位置点序列。

5)用路段上的位置点序列(x，y)拟合一个圆，得圆半径r和圆心位置(xc，yc)。为此，记圆的方程为：

展开后得：

记a=-2xc、b=-2yc、c=xc2+yc2-r2，则上式变成：

上式中(a，b，c)为待拟合参数，轨迹序列(x，y)已知，故上式是关于(a，b，c)的线性超定方程，可用最小二乘法求解，并进而得圆心位置(xc，yc)和圆半径r：

图6 是某段道路进行曲线拟合和拐弯半径分析的结果，其中绿色粗实线路段是拐弯半径小于80 m 的路段，以醒目方式标示，可以提醒驾驶人员小心驾驶，这就是辅助驾驶的意义所在。表1 给出了该路段上部分位置处拐弯半径的定量值，可以看出，自然路段的拐弯半径一般都在几十米量级。

图6 多弯路段的曲线拟合Fig.6 Curve fitting on winding road

表1 曲线路段拐弯半径r 的定量计算结果Table 1 Calculation of turnaround radius along curve road

3.3 对比实验

为了进行对比实验，本文选取了一段有4 个拐弯点的轨迹进行拐弯半径拟合，如图7(a)所示。载车从①点处出发，依次经过并在②、③、④号点处拐弯。表2 给出了各个拐弯点处用本算法算得的拐弯半径。部分点处其实不是一个单一的拐弯圆，比如，图7(b)对4 号点进行了放大，该路段明显呈现两个拐弯圆，算法很有效地拟合出了这两个结果，如表2 所示。

图7 四拐弯轨迹的拐弯半径拟合Fig.7 Turn radius fit along a 4-turn road

表2 专业软件方法与本方法拟合拐弯半径的结果对比Table 2 Comparison of radius fit between professional software and this method

作为对比，用专业的激光点云数据处理软件Terrasolid 也进行了拐弯半径拟合，结果也列于表2中。由于Terrasolid 是一种半自动、交互式的点云处理软件，其结果依赖于人工框选的轨迹弧段，所以效率并不高、结果也不稳定。从表2 可以看出，使用Terrasolid 需尝试多次量测，才能得到与本算法相近的结果。图8(a)也展示了这种多次测量的不稳定性(同一个点处，一次拟合得白色圆[小圆]，另一次拟合得绿色圆[大圆]，差别明显)，而本文算法不仅过程自动，而且结果也符合直观感受，如图8(b)所示，这说明本文方法确有一定优势或好处。

图8 3 号点处的拐弯半径的拟合Fig.8 radius fit at 3rd turn

4 结论

本文从特定场景辅助驾驶的需求出发，设计一套基于三维激光雷达点云的道路场景参数计算方法，包括道路宽度、限高、拐弯半径等，为判断特定车辆的可通过性提供依据。经测试，并与Terrasolid专业软件对比，本方法表现良好，精度很高。

进一步的改进包括：对路面障碍物的自动检测与识别、对乡村非结构化道路的参数提取，以进一步提高此系统和方法的适用范围和实用性。