基于LiDAR场景建模的智能车路径规划*

王志强 胡钊政 ▲ 袁 凯 陶倩文

（1.武汉理工大学智能交通系统研究中心 武汉 430063；2.武汉理工大学能源与动力工程学院 武汉 430063）

0 引 言

近年来，由于摄像头、激光雷达等传感器的更新换代和大数据、人工智能等高科技在汽车领域的广泛应用，汽车智能化的程度越来越高[1]。无人驾驶智能车是基于机器人技术结合车辆工程研发设计的，涉及计算机视觉与人工智能等交叉学科，其作为新兴科学技术的载体平台，引领了目前科学技术的重要发展方向。

自主导航是智能车的一项最基本、最重要的功能，具体涉及到感知、决策以及控制等关键技术。具备感知能力，并且能够构建出驾驶场景模型，即环境地图，是智能车实现自动驾驶、自主导航的一个大前提。目前，用于智能车环境感知和障碍物检测的传感器主要包括激光雷达（LiDAR）[2]、毫米波雷达[3]、单双目视觉[4]，以及超声波传感器[5]等。LiDAR基于飞行时间法原理，即激光遇到障碍物后的折返时间，计算目标与自身的相对距离，并绘制出3D环境地图，精度可达到厘米级别。此外，基于LiDAR的同时定位与建图（simultaneous localization and mapping，SLAM）[6]在感知决策方面也具有非常重要的实际用途。驾驶场景建模是指建立智能车所处环境中的各种物体（如，障碍物、道路等）准确的空间位置描述，即建立地图[7]。地图同样是实现智能车自主导航的一个重要因素，发展至今，主要的地图可以分为3类：①尺度地图（metric map）；②语义地图（semantic map）[8]；③拓扑地图（topological map）。由于尺度地图具有直观明了的特点，因此被广泛运用。尺度地图又主要包含了2种形式的地图，即：栅格地图（grid map）和特征地图（feature-based map）[9]。占据栅格地图由Elfes[10]提出并应用于机器人感知中，目前被广泛应用于基于栅格法的路径规划研究邻域。占据栅格地图将环境离散化，形成了一系列相互独立的正方形栅格，每个栅格的状态只有自由（free）和被占据（occupied）2种状态。对于建模环境相对稳定不变的场景，占据栅格地图的有关算法比较成熟，但是对于真实的环境（存在车辆、行人等临时出现在场景中的障碍物），场景建模会存在一定的问题[11]。

从规划决策算法角度来看，现有的智能车路径规划算法起源于移动机器人领域，然后逐渐应用于不同环境下的智能车。移动机器人的路径规划问题是移动机器人导航研究领域的热点问题，可以描述为：移动机器人依据某个或某些性能指标（如工作代价最小、行走路线最短、行走时间最短等），在运动空间中找到1条从起始状态到目标状态、可以避开障碍物的最优或者接近最优的路径[12]。根据机器人对环境信息的掌握程度来分类，路径规划算法一般可以分为完全掌握地图信息的全局路径规划和不完全掌握地图信息的局部路径规划[13-14]。全局路径规划要求机器人路径规划前完全掌握作业环境的地图信息，进而规划出1条从起始位置到目标位置的最优路径。常用的全局路径规划算法有启发式搜索算法，如，Dijkstra算法、A*算法等，以及各种智能算法，如：遗传算法（genetic algorithm，GA）、蚁群算法（ant colony optimization，ACO）、粒子群优化算法（particle swarm optimization，PSO）等；局部路径规划是移动机器人在环境未知的情况下，根据传感器的信息实时规划出1条可通行的路径。常用的算法包括：人工势场法[15]（artificial potential field，APF）、滚动窗口法（dynamic window approach，DWA）[16]、D*算法等。近年来，众多研究者针对栅格环境下的路径规划算法进行改进，形成了很多启发式搜索算法。国外方面，Likhachev[17]提出一种focussed dynamic A*lite算法（D*lite），Dakulović等[18]提出一种two-way D*算法（TWD*），上述2种算法主要用于环境未知的动态场景，而环境已知的静态环境下使用A*算法往往速度更快。在国内方面，赵晓等[19]采用跳点搜索的方法，对A*算法加以改进，在搜索过程中优化搜索策略，通过筛选具有代表性的节点进行扩展，取代了原算法中对每个相邻节点的操作，降低了算法的整体计算时间。辛煜等[20]将传统A*算法的可搜索邻域拓展为无限个，使得改进后的算法可以沿任意方向进行搜索，不仅缩短了路径长度，并且降低了转折点的个数，但规划的路径中节点信息不便于储存，算法在栅格环境下的适用性不好。

综上所述全局路径规划在整个路径规划阶段起着至关重要的作用，它为车辆运动提供了指引，为后续局部路径规划奠定了基础。针对传统算法规划路径消耗内存、算法计算效率低等不足，本文通过搜索邻域、搜索方向2个方面对传统算法进行优化、改进，有效提高了路径规划的计算效率。

1 场景建模及算法分析

路径规划问题的核心研究内容一般包括场景建模方法和路径搜索算法。本文围绕智能车路径规划问题展开研究。

1）研究了一种基于LiDAR的智能车场景建模方法，实现了由三维激光点云到俯视投影的二维栅格地图建模。

2）在栅格地图环境下，对传统A*算法进行改进，研究了一种自适应搜索方向A*算法（adaptive search direction A*algorithm，ASD-A*），用于智能车全局路径规划，最后在虚拟栅格地图环境和由真实场景生成的栅格地图环境下进行了实验验证。

1.1 基于LiDAR的智能车场景建模

场景建模是实现智能车自主导航的前提，场景地图模型的优劣直接影响着导航系统的整体性能。由于激光SLAM技术具有高精度、高效率的特点，近年来广泛应用于智能车定位、环境感知等领域，通过多传感器信息融合来建立环境点云地图的方法，已经成为自动驾驶等领域的热门研究方向[21]。

本文研究一种基于LiDAR的智能车场景建模方法：主要以建模场景中的三维激光雷达点云数据为基础，通过对原始点云数据进行滤波、点云分割处理，将噪声点、离群点滤除，并将路面点云去除，保留场景中建筑物、树木等信息，然后把点云投影到XOY平面，根据XOY平面上的点云信息，实现了二维栅格地图制作。该环节的流程图见图1。

图1 场景建模流程图Fig.1 Flowchart of scene modeling

利用车载多线激光雷达数据采集平台进行激光原始数据的采集见图2。数据采集工作在校区某教学楼周围的路段完成，见图3。

图2 数据采集平台Fig.2 Data acquisition platform

图3 数据采集区域Fig.3 Data collection area

借鉴LOAM算法[22]，首先将采集到的激光雷达数据进行激光点云三维建模，建模工作在机器人操作系统（robot operating system，ROS）下完成，有效建图面积约30 000 m2，其中长约300 m，宽约100 m，三维激光点云地图见图4。

图4 三维激光点云地图Fig.4 3D LiDAR point cloud map

根据激光雷达的工作原理，距离激光雷达较远处的点云数据，其准确性往往较差。原始数据中点云散布范围过大，以及噪声数据的存在，导致原始点云数据不能直接用于智能车路径规划，为了把采集到的三维激光雷达点云数据应用到智能车路径规划中，需要对原始点云数据进行预处理。在预处理中，需要将噪声点、离群点剔除，同时还需要过滤掉场景中路面点云信息，只保留场景中除路面信息之外的固有静态障碍信息，如建筑物、树木等。

为了解决激光雷达点云数据散布范围过大的问题，采用直通滤波器过滤掉距离智能车较远处的点云数据，减少点云的数量，提高后续程序运行的效率。针对激光雷达点云的三维坐标信息，利用直通滤波器对点云进行直接过滤。假设原始激光点云中某一点的坐标pi( )xi，yi，zixi，yi，zi分 别 为 第i个点云的坐标，若该坐标满足式（1）则该坐标点被保留。

式中：X1，X2，Y1，Y2，Z1，Z2分别为激光点云坐标沿X，Y和Z轴方向设定的下限和上限阈值。

本文采用Statistical Outlier Removal滤波器移除激光点云数据中的噪声点。基于点云临近点距离分布，Statistical Outlier Removal滤波器计算每一个点到所有临近点的平均距离D，设标准差倍数为Nt，若指定点到其m个临近点的平均距离DM大于平均距离D超过Nt个标准差，那么该点将会被认为是1个噪声点而被去除。

在去除路面点云的过程中，基于高度阈值Zmin，对点云数据进行路面点云分割。在实验数据采集阶段，由于激光雷达传感器的安装高度为1.5 m，那么场景中的路面高度在激光雷达坐标系下应该为-1.5 m，将高度阈值设为Zmin=-1.5 m。若点云Pi（xi，yi，zi）的Z轴高度zi满足公式zi≥Zmin则将点云Pi保留，否则将其剔除经过点云预处理，得到点云地图见图5。

图5 处理后的点云地图Fig.5 Processed point cloud map

使用基于栅格地图的表示方法实现了从三维激光点云数据到二维俯视视角的栅格地图构建环节。首先，将预处理后的三维激光点云数据进行XOY平面的投影处理。然后将整个XOY平面均匀地划分为一定尺寸的栅格，本文使用的栅格尺寸为1 m×1 m，见图6。接着统计每1个栅格的状态，规定存在点云的栅格为占据状态，不存在点云的栅格为自由状态，将占据栅格用数字“1”表示，自由栅格用数字“0”表示，即

图6 划分栅格Fig.6 Dividing the grid

这样便可以根据所有栅格状态建立1个相对应的矩阵，该矩阵的元素由“0”和“1”组成，即逻辑值矩阵Mmatirx，最终形成了以逻辑值矩阵表征地图信息的栅格地图，图7为构建栅格地图的流程图。基于上述场景建模方法，得到栅格地图见图8。

图7 栅格地图构建流程Fig.7 Mapping process of grid map

图8 栅格地图Fig.8 Grid map

1.2 自适应搜索方向A*算法

基于栅格环境的全局路径规划算法中，A*算法具有函数表达式简单、容易编程实现、计算量小、规划路径较短等特点，目前被广泛应用于机器人或智能车研究领域。

A*算法由Dijkstra算法发展而来，并加入启发因子，形成了一种启发式搜索算法[23]。它由启发函数来评价地图中任意节点与目标节点间的距离，根据启发函数来选择最优的方向展开搜索。A*算法的启发函数表达式为

式中：f(n)为在当前节点n时，从初始节点到达目标节点的总代价函数；g(n)为从初始节点到达当前节点n实际代价值的大小；h(n)为从当前节点n到达目标节点的最小估计代价值的大小。

用来计算h(n)大小的公式主要有曼哈顿距离（Manhattan distance）和欧几里得距离（Euclidean distance），具体的表达式为

式中：MD为曼哈顿距离；ED为欧几里得距离；n)为当前节点的坐标；(xd，yd)为目标节点的坐标。

在路径搜索时，算法沿着当前节点的最小f(n)值所对应的临近节点进行搜索，传统A*算法的搜索方向一般为4个或8个，即沿着当前节点相邻的4个邻域或8个邻域展开搜索，见图9，以4邻域搜索为例，4个方向分别为当前节点的正上、正下、正左和正右方向。8邻域搜索则是在前者的基础之上增加了4个斜向方向。

图9 搜索邻域Fig.9 Search neighborhood

由图9可知，在相同栅格环境下，若设定的起点和终点相同，4邻域搜索策略在路径搜索过程中访问栅格的数量要少于8邻域搜索策略，这意味着前者在计算时间上要优于后者。但从所规划路线长度的角度分析，8邻域搜索策略要优于前者。仿真实验在Matlab 2018a软件平台上进行，图10为2种不同搜索策略下的仿真实验结果。

通过上述分析可以看出，4邻域搜索A*算法和8邻域搜索A*算法在计算效率和规划路线长度2个方面上各有所长，二者也各自都存在不足之处：①前者虽然在计算时间上优于后者，但在规划路线长度上过长；②后者虽然在规划路线长度上优于前者，但计算时间上却要比前者多，此外由于沿着8个方向进行路径搜索，其规划出的路线难免会出现斜着穿过障碍物栅格某一顶点的现象，见图11，对于智能车来讲，这种规划出的路径显然不合理，难以使智能车按照预定路线行驶。

本文从路径规划合理性和算法计算时间2个方面对传统A*算法进行改进。针对传统A*算法8邻域搜索策略存在规划路线不合理的问题，研究了1种自适应搜索邻域A*算法，有效解决了该问题，使规划出的路径更便于智能车的安全行驶；为缩短算法计算时间，在所提算法的基础上进行改进，研究了一种自适应搜索方向A*算法见图11。

图10 实验对比Fig.10 Experimental comparison

图11 斜穿障碍栅格顶点Fig.11 Diagonally cross vertice of obstacle grid

由图11可知，当从节点n到节点n+1的运动为斜向时，如果在由上述2个节点所在栅格及其相邻的2个栅格所组成的局部2×2栅格块中存在障碍栅格，便会出现斜穿障碍栅格顶点的现象。如果使用4邻域搜索策略，则可以避免该现象。本文结合A*算法的4邻域搜索策略和8邻域搜索策略，研究了一种自适应搜索邻域A*算法。图12为算法流程，算法以8邻域搜索策略为主，在从节点n到节点n+1的斜向运动中，算法将判断是否存在斜穿障碍物栅格顶点的现象，如果存在该现象，则切换为4邻域搜索策略，重新从节点n搜索路径；否则算法将继续以8邻域搜索策略进行路径规划。图12为算法流程图。

图12 算法流程图Fig.12 Algorithm flowchart

利用改进后的算法进行与图11中相同的仿真实验，仿真结果见图13。利用改进算法规划出的路径不存在斜穿障碍栅格顶点的现象，路径更加合理，便于智能车安全行驶。

图13 改进算法的实验结果Fig.13 Experimental results of improved algorithm

为了降低算法的计算时间，在基于自适应搜索邻域A*算法基础上，研究了一种自适应搜索方向A*算法，见图14。

如图14所示，ASD-A*算法将图9中的8邻域搜索方向划分为8个主方向，与其相邻的2个搜索方向共同构成1个搜索区域。

图14 搜索方向示意图Fig.14 Search direction diagram

在ASD-A*中一共有8个搜索区域，每个搜索区域有1个主方向，搜索区域的范围由与主搜索方向相邻的2个副搜索方向所确定，即每个搜索区域包含了3个搜索方向，搜索区域可表示为

式中：SAi为搜索区域；M-Directioni为主方向。

算法根据路径规划的起点和终点，计算出终点相对于起点的方向Ds。然后在8个主方向中寻找到与Ds方向最接近的1个主方向，即

找到主方向后，便可确定搜索区域。

考虑到算法的搜索方向被缩减至3个后，可能会出现当前节点被障碍栅格“包围”而不能搜索到下1个节点的问题，为克服算法的不足，将ASD-A*算法与本文提出的自适应搜索邻域A*算法相结合，当不能搜索到下1节点时，算法将采用自适应邻域搜索策略跳出“包围”，最终返回到ASD-A*算法。仿真实验采用图13中的栅格地图、起点和终点，实验结果见图15。

从实验结果可知，ASD-A*算法规划出的路径与自适应搜索邻域A*算法规划的路径一致，但算法所访问的栅格数量明显减少，对应计算时间将降低。

2 实验与分析

2.1 虚拟栅格地图环境下的实验

场景建模阶段的地图是以栅格法得到的，栅格地图的规模与实际场景的大小以及所采用的栅格尺寸三者之间密切相关。当建模场景固定时，合适的栅格尺寸对于真实场景的准确表示是至关重要的，而栅格尺寸的大小又直接影响着整个栅格地图的规模，若使用较小的栅格尺寸，那么栅格地图的整体精度将会提高，但栅格地图的规模必然会呈现出“指数性”的增长；当建模场景较大时，在保证地图精度的前提下，栅格地图的规模也同样会比较大。此时算法的计算量将会大幅度地增加，算法的计算效率将相应地下降，难以保证较高的实时性。此外，在较为简单的场景中，障碍物往往比较少，与之相对应的障碍栅格在栅格地图中的占有率（本文称作：障碍栅格占有率）也比较小；在复杂的场景中，障碍物的数量一般比较多且密集，在对应的栅格地图中，障碍栅格占有率也比较高。而障碍栅格占有率的大小显然会影响到算法的整体性能。通过上述分析可知，栅格地图的规模和障碍栅格占有率这2个因素影响着路径规划算法的整体性能。

图15 实验结果Fig.15 Experimental results

为了充分验证ASD-A*算法的整体性能，本节从栅格地图规模和障碍栅格占有率2个方面，在虚拟栅格地图环境下进行仿真实验，利用控制变量的方法分别对上述2个方面进行研究，选取了传统4邻域搜索A*算法、传统8邻域搜索A*算法、自适应搜索邻域A*算法以及自适应搜索方向A*算法4种算法进行了仿真对比试验，仿真实验在Matlab 2018a软件平台上进行。

2.1.1 不同规模栅格地图下的实验

选取传统4邻域搜索A*算法、8邻域搜索A*算法、自适应搜索邻域A*算法以及ASD-A*算法4种算法进行了20组仿真实验，并采用了5种不同规模的虚拟栅格地图环境（20×20，40×40，60×60，80×80和100×100，栅格尺寸为1 m×1 m），在每种栅格地图环境下均进行4组实验，最后统计了实验结果。表1列出了实验结果的统计信息。其中，1～4组实验的栅格规模为20×20；5～8组实验的栅格规模为40×40；9～12组实验的栅格规模为60×60；13～16组实验的栅格规模为80×80；17～20组实验的栅格规模为100×100。图16～18为从访问栅格、规划路线长度和算法计算时间3个方面对4种算法的实验结果进行对比分析的结果。

表1 对比实验结果Tab.1 Results of comparative experiment

图16 访问栅格数对比Fig.16 Comparison of the number of access grids

图17 规划路线长度对比Fig.17 Comparison of planned route lengths

图18 计算时间对比Fig.18 Comparison of calculation time

从对比结果可以看出，随着栅格地图规模的增大，4种算法在访问栅格数量和规划路线长度方面整体上均呈现上升趋势，但ASD-A*算法在每一次路径规划过程中所访问的栅格数量远小于其他3种算法，且在规划路线长度方面基本与传统A*算法（8邻域搜索）和自适应邻域搜索A*算法相持平；在算法计算时间方面，ASD-A*算法整体上明显比其他3种算法要少。这说明ASD-A*算法将搜索方向缩小至3个时，避免了一些不必要的计算量，使算法的计算时间缩短，提升了算法的计算效率。

2.1.2 不同障碍栅格占有率下的实验

在同一规模的栅格地图环境下进行了20组仿真实验，栅格地图规模设置为：40×40，并采用了5种不同的障碍栅格占有率，分别为：20%，25%，30%，35%和40%。栅格尺寸均设置为：1 m×1 m，在每种栅格地图环境中均进行4组实验，图19～21为对比分析结果。

图19 访问栅格数对比Fig.19 Comparison of the number of access grids

图20 规划路线长度对比Fig.20 Comparison of planned route lengths

图21 计算时间对比Fig.21 Comparison of calculation time

从对比结果可以看出，在栅格地图规模固定的前提下，随着地图中障碍栅格数量的增加，4种算法在进行路径规划时所访问的栅格数量和规划路线长度整体上均呈现上升趋势，其中传统A*算法（8邻域搜索）和自适应搜索邻域A*算法在每一次路径规划过程中访问的栅格数量较多，传统A*算法（4邻域搜素）次之，而ASD-A*算法则远小于其他3种算法；且ASD-A*算法在规划路线长度方面基本与传统A*算法（8邻域搜索）和自适应邻域搜索A*算法相持平；在算法计算时间方面，ASD-A*算法整体上明显比其他3种算法要少。

2.1.3 综合分析

前文利用控制变量的思想，分别从栅格地图规模和障碍栅格占有率2个方面对4种算法进行了仿真实验。从定性分析来看，本文所提出的ASD-A*算法较之其他3种算法具有较高的计算效率，且路径规划长度相对较短。为了更加清晰地研究所提算法的整体性能，对表1中的实验数据采用定量分析的方法做进一步的分析。将40组实验数据取平均值，表2统计了算法在访问栅格数量、规划路径长度和计算时间3个方面的信息。

表2 实验数据平均值Tab.2 Average of experimental data

由表2可见，相较于传统A*算法（4邻域搜索），ASD-A*算法在访问栅格数量方面降低了约39.2%，在路径长度方面降低了约15.5%，在计算时间上降低了约38.2%；对比传统A*算法（8邻域搜索），ASD-A*算法在访问栅格数量和计算时间上分别降低了约54.5%和47.2%，虽然在路径长度方面略高于前者，但融合了自适应搜索邻域A*算法后，ASD-A*算法规划出的路线更加合理，便于智能车安全行驶。综上所述，通过仿真实验可以证明本文提出的ASD-A*算法具有良好的适用性，能够在多种规模和不同障碍栅格占有率的栅格环境下保持良好的整体性能。

2.2 真实栅格地图环境下的实验

根据上述虚拟栅格环境下的实验结果分析，可以看出，本文所提出的ASD-A*算法较之传统A*算法具有计算效率高、规划路线更加合理的优点。为了进一步验证该算法在真实栅格地图环境下的整体性能，在图22所示的真实栅格地图环境下，分别选取不同的路径规划起点和终点，进行了5组实验测试。图22所示场景为1.1节中所对应的建模场景。其中，栅格尺寸为：1 m×1 m。表3统计了相关实验数据。

图22 真实栅格地图下的实验Fig.22 Experiments on real grid map

表3 实验数据Tab.3 Experimental data

可以看出，ASD-A*算法在5组实验中所规划的路径长度最短为63.885 m，最长为157.059 m，算法计算时间最短为2.379 ms，最长为3.71 ms。算法在真实栅格地图环境下所表现出的整体性能与虚拟栅格地图环境下基本一致。

3 结束语

针对智能车自主导航问题，围绕智能车路径规划问题展开研究工作。①研究了一种基于LiDAR的智能车场景建模方法，将数据量较大的三维激光点云进行“降维”处理，以二维栅格地图来表示智能车驾驶场景；②对传统A*算法进行改进，运用4邻域、8邻域自适应搜索策略优化了传统算法斜穿障碍栅格顶点的不合理路线，所提出的自适应搜索方向A*算法（ASD-A*）在计算时间上较之传统A*算法（8邻域搜索）降低了47.2%，很大程度上提升了计算效率。通过仿真实验可以证明，本文提出的自适应搜索方向A*算法具有良好的适用性，能够在多种规模和不同障碍栅格占有率的栅格环境下保持良好的整体性能。

此外，本文的研究工作也存在一些不足之处，如：①由于实验条件有限，本文的场景建模工作是在校区内进行的，后期将尝试在物流园区、港口码头等适合智能车应用的场景进行场景建模工作；②仅研究一种路径规划算法，未来将考虑结合2种或2种以上的算法，尝试研究路径规划融合算法。