1. 引言
随着我国汽车工业的崛起,无论是从国家层面还是社会企业层面都加大了对智能车辆的研发和投入,智能化技术也在不断蓬勃发展。智能车辆是集环境感知、决策规划以及辅助驾驶等功能于一体的高度自治的系统,融合了传感器、计算机、通信、人工智能还有自动控制等多种技术 [1] 。车辆路径规划作为集中体现车辆智能化水平的重要模块功能,一直是行业内研究的焦点。
全局路径规划是指在由高精地图、车联网提供了已知行驶环境情况下,在全局地图上规划出一条从车辆起始位置到目标位置的点对点的高效、安全且舒适的行驶路径,使车辆能够跟随规划路线,实现行程目标。具体来说即在包含了静态障碍物和可行域信息的地图上 [2] ,搜索出一条从初始点出发到目标点通行效率最高的路线。全局路径规划主要包含两方面内容,一是环境的建模,二是路径点搜索算法。合适的环境建模,地图构建是进行路径规划的基础,要去除环境中的干扰因素,只保留会对车辆行驶造成影响的环境边界和障碍物信息,在提升规划质量的同时降低对计算资源的占用。本文假设全局环境信息已由高精地图获取,在已有环境建模的基础上对进行更进一步的处理。
常用的路径规划算法包括基于栅格图的搜索规划,如Dijkstra算法、D*算法、A*算法等,基于采样的规划有快速随机搜索树和概率路线图,还包括基于智能算法的规划,如粒子群算法、遗传算法、蚁群算法等。目前已有很多研究对A*算法进行了改进。文献 [3] 提出的任意时间树恢复加权A*算法是对加权A*算法的改进,该算法以牺牲部分最优性为代价加速了搜索过程,并通过重用以前的搜索信息减少了搜索时间。文献 [4] 提出了一种斜八邻域扩展策略,在原八邻域扩展策略的基础上,在x轴和y轴方向上多扩展一个节点,再将其和四邻域扩展方式结合,融合成一种新的双邻域选择扩展策略。文献 [5] 中使用了加权A*算法,通过增加了可调节的权重系数,增强了启发式函数的影响,从而提高了算法执行效率,但结果容易陷入局部最优。文献 [6] 针对A*算法中的启发式函数难以确定或不一致,导致算法性能不稳定的问题,提出了多启发式A*算法,通过不同启发式函数的组合来改变启发式函数的引导能力,从而简化了启发式函数的设计,提高了算法的性能,避免了局部极小值的出现。
本文的研究目标就是在不同行驶场景下根据确定的出发点,以到终点的行驶安全性和距离代价最小为目标进行全局规划。在参阅了相关文献后,发现现有研究对于A*算法多以目标点为导向进行应用和改进,而没有考虑障碍物对于算法搜索的潜在影响,因此加入行车风险势场对算法的节点扩展搜索方向进行引导。针对算法中陷入局部最优空间,搜索策略效率低,转折点多、路径不平滑等问题,对算法代价函数计算和规划结果分别进行优化改进。
2. 全局规划地图构建
2.1. 栅格地图建立
进行全局路径规划首先要确定环境信息,建立环境地图,根据得到的障碍物和可行域在方位上的布局,构建合理的障碍物分布图,然后在此基础上通过规划搜索算法进行路径寻优,合理的环境地图可以更好地帮助确定规划方法和搜索算法,最终以更少的资源代价规划出更优的路径。初步的环境建模首先对获取到的卫星地图进行预处理,将行驶环境映射成一个整合了环境特点和障碍物轮廓的地理表示。
常用的环境建模方法包括栅格法、几何法和拓扑法。文献 [7] 使用拓扑图来描述环境,增加了规划路径和障碍物之间的安全距离,但仍然存在拓扑信息来源困难以及需要额外碰撞检测的问题。还有研究 [8] 以圆形边界来包围车身轮廓以形成一个安全距离约束,只有满足了安全约束的路径点才会被使用,但这样的边界判定增加了计算量。栅格图中每个栅格都对应着真实工作环境中的具体位置,栅格大小都是确定的相同的,易于数据存储检索和处理。
获取到的卫星地图不能直接应用于路径搜索算法,首先要将环境模型转化为栅格图,忽略所有物体的高度信息,只保留长宽信息,过程主要是将全域环境以一定的精度/分辨率划分为大小相等的正方形区域,每一个方形区域就是栅格地图的基本计算单位,栅格的尺寸越小,栅格地图对环境的描述越精确。同时给每一块方形区域都赋予不同的属性。
栅格地图的数学模型 [9] 可以表示如式(1):
(1)
式中:A为整个环境地图的模型,m和n分别为环境地图栅格化后每行、每列的栅格数,
为栅格地图中
处的栅格位置,
表示栅格处可通行,
表示栅格处有障碍物,无法通行。
2.2. 增加安全空间的环境建模
由于在栅格地图中使用A*算法进行路径搜索时使用的是车辆点质量运动模型。而汽车不同于移动机器人,机器人体积相对较小,栅格的大小通常和机器人的轮廓体积相近,可保证机器人不和障碍物发生碰撞。但是车辆规划中如果采用和机器人同样的处理方法,必然会造成规划和实际的偏差。忽略车身体积,实际无法通过的路径可能会在规划中被视为可通行。不考虑转弯半径,规划中的拐点可能会造成实车的激进转向。因此需要在环境模型中补偿因忽略车身体积而造成的误差。
同时,实际行驶过程中还存在陷阱区域,即障碍物形成了一个内凹的可行驶区域,如果算法搜索的路径点进入区域后,算法可能会在陷阱区域内规划出激烈的转弯甚至掉头路径,又或者在较大的陷阱区域内行驶一段路程后再次回到起点。对安全空间进行处理,具体就是对障碍物的边界进行膨胀扩展,保留一定的安全行车空间。
对栅格地图进行安全空间处理易于计算,且不需要降低地图分辨率。本文将安全空间的优化分为必要安全空间和冗余安全空间两部分,前者是保证车辆和障碍物之间安全无碰撞的必要距离,同时为局部轨迹规划留有一定的空间裕度。后者是为了防止路径搜索算法在陷入陷阱区域后导致规划路径扭曲变形,造成车辆无法跟踪的情况发生。必要安全空间的设置步骤如图1(a)所示,在规划中采用车辆点质量模型,但实际中要考虑车身体积,图中黄色方框代表车辆。为了降低陷阱区域的影响,对障碍物再进行冗余安全空间设置。在栅格地图中填补陷阱区域,设置其为潜在障碍物。冗余安全空间的设置,让一些不能保证车辆安全通过的狭窄通道和陷阱空间被冗余空间覆盖,提升了规划路线的整体平滑性。具体安全空间设置示意如图1(b)所示。
![](//html.hanspub.org/file/37-2571602x12_hanspub.png?20240523082854088)
Figure 1.Schematic diagram of safety space setting
图1. 安全空间设置示意图
3. A*算法和行车风险场
目前A*算法在移动机器人和车辆规划中已经有了很多应用,本文在改进A*算法的同时,引入了障碍物对车辆产生的风险势场影响。
3.1. 传统A*算法
A*算法是在Dijkstra算法的基础上扩展而来的启发式算法 [10] ,在代价计算中,相比于Dijkstra算法,A*算法在计算实际代价的基础上,加入了启发式函数,从而定义了新的代价函数,使得A*算法在完成二维空间中的路径搜索和寻优时可以充分发挥效力。A*具体计算方式如式(2):
(2)
式中
表示从起始点到当前节点的实际行驶路径代价,
表示从当前节点到目标点的预估路径代价,也就是启发函数;
表示当前节点总的预估代价,用以评判当前节点是否代价最优。
首先要明确的就是启发函数的计算方式,通常的距离计算方式有四种:曼哈顿距离、欧几里得距离、切比雪夫距离和对角线距离 [11] 。对角线距离就是可以沿着栅格对角线移动的距离加上不能沿着对角线移动的距离,允许在八邻域方向上移动。如果四邻域搜索的代价为D,那么对角线代价
,具体对角线距离计算如式(3):
(3)
确定了启发函数就可以进行A*算法的具体实施,算法流程如图2所示:
3.2. 行车风险势场建立
车辆行驶过程中除了目标方位决定了行驶方向,同时还面临着障碍物、障碍车对行车方向的扰动影响,需要根据车辆定位和所处环境进行行驶风险的评估,提升行车安全性。合理的行车势场可以将障碍对于车辆的影响数值化,形成具体的势场力。参考文献 [12] ,类比物理电场建立行车势场,势场力的计算方式参考人工势场法 [13] ,具体如下。
目标点势场如式(4):
(4)
其中
为正比例增益系数,
为一个矢量,表示汽车的位置
和目标点位置
之间的距离矢量,大小为
,方向是从汽车的位置指向目标点的位置。
相应的引力
为目标势场场的负梯度如式(5):
(5)
决定障碍物风险势场的因素是汽车和障碍物之间的距离,当车辆不在障碍物的影响范围内,其受到的势能值为0,在汽车进入障碍物的影响范围后,两者之间的距离越大,汽车受到的势能就越小,距离越小,汽车受到的势能值就越大。风险势场的势场函数如式(6):
(6)
其中
为正比例系数,
为一常数,表示障碍物对汽车产生影响的最大距离。
相应的斥力
如式(7):
(7)
用行车势场对车辆形成的合力来预测行车方向,改进路径点的搜索机制,可以充分考虑到障碍物的影响,保证充足的避让空间的同时,又确保了前进的方向性。
4. 基于行车风险场的改进A*算法
A*算法进行路径寻优时搜索速度快,但同时也会有陷入局部最优的问题。并且随着环境复杂程度的增加,问题越凸显,算法的执行效率将受到很大的减益。
4.1. A*算法改进
4.1.1. 启发函数改进
启发函数决定着算法的搜索行为,如果只使用四方位的曼哈顿距离,那么会增加规划路径中的转折,降低路径效率,如果只采用全方位的欧式距离,那么规划出的路径可能会出现较大的拐点,这又不符合车辆运动特性。本文采用对角线距离作为启发函数的计算公式,并针对直线和对角线距离加上不同的优化处理,具体计算方式如下:
(8)
通过上式(8)判断出当前位置和目标点位置的横向和纵向距离差。
(9)
式(9)中
和
分别对应对角线距离中的计算公式,
用于判断车辆在当前位置的运动倾向。当
时,整个行程更偏向于斜向运动,启发函数计算如式(10):
(10)
当
时,表明整个行程更偏向于直线方向运动,启发函数计算如式(11):
(11)
通过调整启发函数中的权重值来改变A*算法的搜索行为,当两个权重值都为0时,算法就会退化为Dijkstra算法,规划得到的路径最短,但所花费的计算代价也最大,扩展次数多,计算时间长。随着两个权重值的增大,算法的扩展次数会呈现指数级下降,但过大的权重又会导致算法退化为BFS,导致路径最优性下降。因此,考虑到启发函数预估距离应该小于等于当前节点到路径目标点的真是距离,两个权重值之和不应该过大。改进后的启发函数代价取值为
,
。
4.1.2. 引入代价权重
确定好启发函数后,再对整体代价函数的计算方式进行优化。由以上不同情况下的路径搜索结果分析,以及所参考的文献,采用变权重代价函数来改进启发函数,优化后的具体的代价和权重计算公式如下式(12):
(12)
式中,
表示当前点到目标点距离,
表示起始点到目标点的距离,搜索起始的阶段,权重公式中
的值趋近于1,这时权重系数就相对大,有利于算法的搜索;当算法运行到行程最后,比值就趋近于0,实际路程代价和预估路程代价的权重就接近,更适合算法的寻优。
4.2. 引入势场导向的改进A*算法
常用的A*算法所采用的节点扩展方法都为4邻域或者8邻域扩展,为了进一步增加行车的安全性,在保证方向性的同时,还应该保证车辆行驶方向不会靠近障碍物。因此将行车势场和A*算法相结合,提出基于行车风险场的变邻域扩展A*路径搜索算法。进一步提升算法的搜索效率,减少搜索节点个数。改进后的算法在父节点进行搜索时判断势场合力方向,然后根据合力方向有针对性地搜索栅格。
根据当前点所受合力方向与地图坐标系的斜率k来确定算法的节点扩展方向,同时加入当前位置相对于目标点方位的考虑,当斜向距离更大,节点应尽可能斜向扩展,扩展方向如表1,示意如图3;当横向距离更大时,扩展应更偏向直线方向搜索,扩展方向如表2,示意如图3。
确定了扩展搜索的方向后,采用变邻域增扩搜索策略进一步对节点扩展方式进行改进,首先基于对角线距离判定当前车辆所处位置距离目标点是斜向距离大还是直线距离大。若斜向距离大,那么采用斜向增扩策略,根据合力方向判定所属区域,如表1所示,每一个区域都包含三个扩展栅格,除此以外,当斜向对角上第一个栅格没有障碍物时,进行斜向增扩,即再向外扩展一个对角栅格,如图4中(c)所示,否则保持原扩展搜索策略。若直线距离大,那么采用横纵向增扩策略,同样先根据合力方向判定所属区域,如表2所示,每一个区域也都包含三个扩展栅格,当前栅格的横纵方向上第一个扩展栅格没有障碍物时,进行横纵向增扩,即再向外扩展一个横向或纵向的栅格,具体如图4中(d)所示,否则保持原扩展搜索策略。
![](Images/Table_Tmp.jpg)
Table 1. Table of slope and direction correspondence during oblique direction
表1. 斜向时斜率和方向对应表
![](Images/Table_Tmp.jpg)
Table 2. Table of slope and direction correspondence in both horizontal and vertical directions
表2. 横纵向时斜率和方向对应表
![](//html.hanspub.org/file/37-2571602x60_hanspub.png?20240523082854088)
Figure 3. Schematic diagram of expanding search direction
图3. 扩展搜索方向示意图
5. 规划轨迹二次处理
由于栅格地图的限制,上述改进算法最终得到的规划路径仍是以栅格路径点为基础的折线,这不符合实车的行驶状况。如果直接发送给车辆控制器,会导致路径点地重复计算,浪费计算资源,同时也会加剧车辆运动的不稳定性。因此需要对规划完成的路径进行冗余点剔除和平滑优化的二次处理。
5.1. 冗余点剔除
冗余点在一定程度上保证了规划的连续性和路径的安全性,但同时,由于大量冗余点的存在,也导致了规划出的路径并非为最优路径 [14] 。去除多余的节点有利于车辆以规划路径为基准平稳地运行,具体步骤如下:
步骤1:将规划完成的路径点坐标依次放入集合
中,
。
步骤2:从起始点开始,连接
和
形成向量并判断
,如果角度为0,说明节点在同一直线上,那么
就是冗余点。如果角度不为0,则连接
,同时判断之间是否存在障碍物,如果没有,那么
依然判定为冗余点。
步骤3:循环步骤2,依次递推集合中的节点,直到集合中的点全部遍历完成剔除冗余点后的效果如图5所示。
![](//html.hanspub.org/file/37-2571602x69_hanspub.png?20240523082854088)
Figure 4. Improve and expand search strategies
图4. 改进扩展搜索策略
![](//html.hanspub.org/file/37-2571602x70_hanspub.png?20240523082854088)
Figure 5. Comparison chart before and after removing redundant points
图5. 剔除冗余点前后对比图
5.1. 路径平滑
在路径规划中,当运动方向发生了变化时,车辆随之也要进行转向动作,产生一定的转角。在转向时,由于方向突然改变,速度和加速度的方向也会改变。然而在栅格地图的路径规划中,车辆航向发生变化的转折点处都是折线,这就导致了转角突然的抖动,这不符合实际中车辆运动的特性,不满足车辆动力学约束,同时剧烈的转向也会降低乘车舒适性 [15] 。
在路径平滑方面,现有文献中有两种常见的方法来解决光栅地图的路径平滑问题。第一种是使用特定的曲线进行拟合,如Bezier曲线、B样条曲线等。该方法通常用于规划完整路径后的路径点拟合。由于曲线拟合不考虑障碍物,拟合后仍存在碰撞风险,因此需要在平滑后进行碰撞检测,对不合格的平滑参数重新调整容易造成重复平滑和碰撞检测。另一种平滑的方法是增加搜索维数或网格搜索范围。例如添加航向角状态,将状态空间从二维空间变为三维空间。这些方法可以在一定程度上平滑路径,但容易造成计算量的大量增加。
贝塞尔曲线结构简单、计算效率高 [16] 。B样条曲线具有贝塞尔曲线的所有优点,同时克服了贝塞尔曲线不能局部修改和连续条件难以获得的缺点 [17] 。动态切点调整算法通过动态调整路径上的切点来生成平滑且安全的路径,常用于移动机器人和自动驾驶车辆的路径规划,能够生成平滑且安全的路径,适应动态变化的环境,提高路径的实时性和鲁棒性 [18] 。本文采用动态切点调整算法来去除路径点中的部分不安全或不必要拐点,让路径在满足车辆运动几何特性的同时,保持连续的曲率,具体平滑步骤如下:
步骤1:将剔除冗余点的路径点存入集合
中,
,从
开始,依次遍历集合中的节点。
步骤2:比较
和
线段长度,以较短线段的未相交端点S作为起始点,作线段的垂线,和
的平分线相交于
。
步骤3:以相交点为圆心,端点和交点的连线段为半径作圆。
步骤4:判断圆是否和较长线段相交,如果有交点
,跳转步骤5,否则跳转步骤6。
步骤5:判断以圆两个交点为界的圆弧是否经过障碍物,如果存在障碍物,跳转步骤2,进行下一个点的判定,如果不存在障碍物,那么就用圆弧替换折线路径。
步骤6:将切点
移动到
,直到切点圆和较长线段有交点,再将
设置为初始点,并跳转步骤2,直到集合中的路径点全部遍历完成。
具体如图6所示。
![](//html.hanspub.org/file/37-2571602x82_hanspub.png?20240523082854088)
Figure 6. Improve and expand search strategies
图6. 改进扩展搜索策略
平滑后的路径效果如图7所示。
6. 仿真实验及结果分析
在MATLAB中进行仿真实验,并假设已经完成了从实际地图到栅格地图的建模和冗余空间的配置。为了更好验证本文改进算法的性能,建立了规则障碍物分布和随即障碍物分布两种仿真地图环境,并分别在30 × 30,60 × 60,100 × 100这三种地图大小下进行仿真验证,起始点设定为(1, 1),终止点分别设定为(30, 30),(60, 60)和(100, 100)。为了保证实验结果的有效性,将本文提出算法和使用曼哈顿距离的四邻域A*搜索算法,使用欧氏距离的8邻域搜索算法进行对比。仿真结果如图8和图9所示。图中蓝色圆圈表示起始点,红色圆圈代表终止点,黑色圆圈代表剔除冗余节点后的剩余路径节点,蓝色为规划路径,绿色为平滑优化的部分路径。
![](//html.hanspub.org/file/37-2571602x83_hanspub.png?20240523082854088)
Figure 7. Comparison before and after path smoothing
图7. 平滑前后路径效果对比
![](//html.hanspub.org/file/37-2571602x84_hanspub.png?20240523082854088)
Figure 8. Comparison of planning effects in a rule-based environment
图8. 规则环境下规划效果对比
![](//html.hanspub.org/file/37-2571602x85_hanspub.png?20240523082854088)
Figure 9. Comparison of planning effects in random environments
图9. 随机环境下规划效果对比
具体算法对比数据如下。
![](Images/Table_Tmp.jpg)
Table 3.Comparison of planning effect data
表3.规则环境下规划效果数据对比
![](Images/Table_Tmp.jpg)
Table 4.Comparison of planning effect data
表4.随机环境下规划效果数据对比
从表3和表4的数据对比可以看出,本文提出的改进A*算法节点搜索数大幅降低,且规划路径的长度也保持在较优的水平。表明本文所提出的改进算法能够在保证路径最优性的同时,降低算法的计算成本,提升车辆的路径规划效率。
7. 总结
针对传统A*算法在车辆全局路径规划应用中存在的不足,本文首先对环境建模方法进行了优化,增加了车身和障碍物之间的安全空间。其次,将周围障碍物对车辆运动方向的影响纳入考虑,引入行车风险势场来改善和引导A*算法路径节点的扩展搜索方向,通过对比实验验证了改进后的针对性路径点搜索在保证了节点扩展效率的同时,路径距离代价也保持在最优水平。最后,在优化A*算法的基础上,对规划路径进行冗余点剔除和路径平滑优化二次处理,提升路径规划的有效性。仿真实验得出的最终改进规划路线进一步降低了中间冗余节点数量,平滑后路径节点之间的过渡更加平缓,并规避障碍物。综上本文所提出的改进算法可以有效地提升A*算法在智能车辆路径规划中的应用效果。