基于势场化改进A*算法的履带式车辆路径规划算法

王 涛，黎玉康，刘文学

（陆军炮兵防空兵学院，合肥 230000）

0 引言

履带式车辆由于与地面有更大的接触面积，比轮式车辆更能适应环境复杂恶劣的路面，因此，其工作路面大多条件较为恶劣。工作环境的恶劣性使得履带车更需要无人化改造。复杂的环境、崎岖的路面给无人履带车的路径规划带来了更大的挑战。文献［1-3］提出了针对于履带式车辆的路径规划方法，保证履带式车辆行驶的平稳性、安全性；文献［4-6］通过增加路径的平滑性，减少转弯次数的方式减缓履带式车辆拟合路径的难度，缩短路径长度；文献［7］引入新增节点与删除节点准则，改善了路径质量，并加入了动态窗口思想使之能够实时局部规划避障，但该方法参数选择困难；文献［8］改变传统A* 算法子节点拓展方式，其子节点位置根据拓展步长、车辆最小转弯半径以及前轮转角离散数量确定，保证了路径符合轮式车辆运动学特点；文献［9］提出的关键点提取法大大减少了复杂环境下A*算法内存的开销，但不具备局部路径规划能力。

履带式车辆通过两侧履带差速运动时拟合连续曲线时，车身易产生滑转滑移，以至于产生较大的横向偏差。针对以上问题，提出一种基于改进A*算法的履带式车辆路径规划算法。将人工势场引入环境中，生成局部势能最小值为子目标，为A*算法规划局部路径指引方向；提出删除冗余节点的方法，减少了路径转折，缩短了路径长度；引入了Reeds-Shepp 曲线，约束了路径的曲率使之适用于履带车辆的行驶。

1 改进A*算法

由于履带车辆的转向机构操作相对较为困难，难以连续调节转向半径，使得履带车辆运动轨迹显示出曲率变化少、曲率易突变的特点。而传统方法规划的路径曲率连续变化且变化多，不仅导致跟踪效果差，还会使得的拉动操纵杆动作过于频繁，大大缩短转向机构的寿命。为了符合履带车运动规律，且易于控制，减少操纵杆的动作，履带式车辆的运动路径应有着较大的直线占比，转弯部分以固定曲率半径的圆弧连接。

传统A*算法为全局启发式算法，有着良好的搜索最短路径的能力、但只能作为静态全局算法，且路径转折点过多，计算量过大。为了使A*算法所规划路径更满足履带式车辆的实际运动路径需要，本文将对A*算法作出一定的改进。

1.1 A＊算法势场化

A*算法属于全局静态规划算法，在把握全局环境的基础上，拥有出色的最短路径搜索能力，但是复杂的环境使得A*算法的计算效率急剧下降，且A*算法不具备未知环境下的局部路径搜索能力，而人工势场算法作为典型的局部路径搜索算法，具有结构简单，响应迅速的特点。

假设T 时刻时，履带式车辆前进方向上有半径为r，角度为90°的扇形观察域，此观察域随着履带车辆的前进方向改变而改变，人工势场初步感知出观察域内的障碍与非障碍区域，非障碍区域每个点相对终点的势能为：

相对障碍的势能为：

其中，k 为引力系数，m 为斥力系数，d1，d2分别为非障碍区域点与终点、障碍的距离，P0为斥力影响距离。

观察域内每个非障碍点的势能为：

将T 时刻时人工势场计算所得的势能最低点设为当前A*算法当前的子目标点。如图1 所示。

图1 某时刻观察域与势能分布Fig.1 The distribution of observation domain and potential energy at a certain time

引入人工势场后，复杂的环境相当于被分割为若干简单环境，提高了A*算法规划效率。人工势场为A*算法指出了当前的子目标，使得A*算法具备了未知环境的路径搜索能力。传统人工势场所规划路径质量差，易于陷入局部极值。A*算法缩短了路径长度，且可作为陷入极值点时的代替算法，跳出局部极值点。

1.2 优化节点

传统A*算法为求最短路径，在复杂环境障碍的影响下，会得到多折线组成的路径。其原由是路径中冗余节点的存在，这些节点还造成了一些不必要的转折。转折过多不利于履带车辆的行驶，不仅会造成车辆转向机构的磨损，还会大大减缓行进速度，消耗更多的时间。因此，需要对A*算法得到的路径进行优化以适应履带车辆的正常行驶。

由于文献［7］中删除冗余节点的策略需对参数α 进行调整，地图大小不同，复杂度不同，参数也不同，实际运用中限制较大。文献［10］中通过简化算子来减少拐点的方式简化路径。本文基于这两种策略，作出调整，提出了自己的删除冗余节点的策略。

将某次局部规划观察域内障碍物所处坐标定义为危险区，危险区不可通行。在某次局部规划完一条路径后，假设该规划包括起点终点在内一共有X 个节点，对所得路径进行二次规划：从起点开始依次与第2 节点，第3 节点直线连接，直到与第N+1 点连接时，该直线经过危险区，则停止，并删除起点到第N 点之间的点，起点与第N 点的连线即为新的路径。再从第N 点开始继续上述步骤。如图2 所示。

图2 删除冗余节点流程图Fig.2 Flow chart of deleting redundant nodes

经过该方法优化路径后，路径长度将大大缩短，减少了不必要的转弯与曲线路径。优化效果如图3 所示。

图3 路径优化效果Fig.3 Optimization effects of paths

1.3 Reeds-Shepp 曲线引入

传统路径规划算法所得曲线路径质量较差，除了拐角较多以外，还存在拐角角度大，难以满足履带车辆的运动特点的现象。拐角角度大意味着履带车辆运动方向变化大，在实际运动中，大角度的变向运动会引起较大路径跟踪误差，极大影响了跟踪精度。文献［2-7］采用了B 样条曲线以及删除、增加节点的方法使得曲线变得平滑，易于跟踪。但是在复杂的环境中，过于拥挤的障碍环境导致以上两种方法平滑后的曲线依然难以满足履带车辆的极限转弯半径所限的运动轨迹。履带式车辆的转向运动往往会产生突变性的曲率变化，且由于机械结构的限制，难以实现多次调节转向半径用于拟合曲率连续变化的曲线。

Reeds-Shepp 曲线由圆弧和直线组成，曲线的曲率可根据实际环境驾驶中的常用转弯半径而定。该曲线的特点正符合了车辆运动的理想路径的特点。将算法所得路径的转角利用Reeds-Shepp 曲线平滑化，所得曲线不仅符合履带车辆的运动规律，且为众多由该圆弧与直线段组成的路径中最短的一条。圆弧与直线连接的路径，既保证了转向开始时曲率的突变，又满足了转向过程中曲率保持不变的特点。

2 实验分析

为验证本文算法优越性，在不同环境下将传统算法与本文算法进行实验仿真比较。实验在CPU 为I7-7700HQ，RAM 为8 G 的计算机上运行，算法通过MATLAB 编程仿真实现。

2.1 与传统A＊算法对比

履带车辆的运动特性使得其跟踪曲线路径时会产生较大的误差，跟踪精度不理想。理想情况下的路径应长度相对短，直线路径占比高，转弯曲线曲率满足其转弯半径极限，如此保证了其运动的快速性、平稳性。

传统A*算法具有高效性，且寻路能力强。但传统A*算法因其固有原因，所得路径往往有着过多的转角，不适合履带式车辆的行驶。本文算法以A*算法为核心，为了避免上述缺陷，经过改进，加入了删除冗余节点的策略，大大减少了转弯次数，减少了履带式车辆在行驶中因过多转弯导致的转向机构磨损以及减速。如下页图4 所示。

图4 A*算法与本文算法路径对比Fig.4 Path comparison of A*algorithm with the proposed algorithmalgorithm

图5 不同障碍占比地图Fig.5 Maps of different obstacle proportions

由表1 中数据可看出，优化后的路径相对于原路径，缩短了19.6%，且大大减少了转弯次数，仅为原本转弯次数的1/6，平均转弯角度也大大减小。相比于传统A*算法，本算法所得路径通过删除冗余节点二次规划后，得到了更适应履带车辆行驶的路径。

表1 传统A*算法与本文算法数据对比Table 1 Data comparison of traditional A*algorithm and the proposed algorithm

2.2 与人工势场算法对比

人工势场算法作为一种典型的未知环境路径搜索算法，具有反应迅速，结构简单等特点。但往往路径质量较差，不适合履带车辆的行驶。实际的行驶路径中，直线路径占比越高，平均直线路径长度越长，越有利于车辆的快速行驶。本文算法运用了人工势场，并结合了改进A*算法令路径得到了优化。将传统人工势场算法与本文算法进行算法效率和路径质量上的对比。

为了消除其他复杂因素的干扰，现运用人工势场算法与本文算法在同样的计算机上，规划出4 种环境复杂度的可行路径。该地图为100 m×100 m 大小，每个栅格为1 m×1 m 障碍占比依据环境复杂度的增加而逐渐增加为5.84%、11.85%、15.70%和21.32%。为了防止因车辆体积原因导致与障碍相碰，对全地图进行膨化处理，膨胀比为1.2。履带车辆车体为3.3 m×6 m，无障碍分辨率为1 m。起点、终点均设为（3，3），（97，97）。

如图6 所示，以障碍占比21.32%的环境为例，从直观的路径形貌来看，本文所提出的算法显然较之传统人工势场算法所得路径质量更好，路径转弯次数更少，解决了传统路径转弯半径过小且路径不够平滑的问题。

图6 人工势场算法与本文算法路径对比Fig.6 Path comparison of artificial potential field algorithm with the proposed algorithm

由表2 以及图7～图10 实验数据可明显表明本文算法的优越性：1）论路径长度，本文算法所得路径更短，且随着环境障碍占比的增大，本文算法所得路径长度优势更加明显；2）论算法效率，由于本文算法采用先探索势能分布，再规划局部路径的方式，其运算时间必然较之传统的单一算法较长。但本文算法在环境大小未变，复杂度增大的情况下，得到全部未知环境的路径的时间较人工势场法相比差距不大；且由于观察域范围有限，其中，环境较之完整地图环境较为简单，改进A*算法的运算速度极快，对算法的实时反应速度影响较小；3）论直线路径占比，本算法所得路径拥有较大的直线路径长度占比率，相比于传统人工势场算法，本算法所得直线路径占比率具有显著优势，且平均直线路径长度较长。环境复杂度越高，优势越明显。4）论路径转弯次数，本文算法转折数相对较少。随着环境障碍占比率的增大，本文算法所得路径转弯次数的增加并不明显，而人工势场算法所得路径的转弯次数明显大大增加。

表2 本文算法与人工势场算法在不同复杂度环境下所得路径对比Table 2 The obtained path comparison of the proposed algorithm and artificial potential field algorithm in differently complex environment

图7 路径长度随障碍占比变化趋势Fig.7 Path length with obstacle proportions

图8 计算时间随障碍占比变化趋势Fig.8 Variation trend of time calculation with obstacle proportions

图9 直线路径长度占比随障碍占比变化趋势Fig.9 Variation trend of proportion of straight path length with obstacle proportions

图10 路径转折数随障碍占比变化趋势图Fig.10 Variation trend of path turning number with obstacle proportions

3 结论

传统的关于履带式车辆的路径规划方法多关注于得到平滑的曲线路径，而忽略了履带式车辆跟踪多曲率变化路径时不易提升跟踪精度的问题。本文提出了适合履带车辆行驶的路径规划方法。1）针对履带车辆转向操作不易，行驶路径曲率变化较少的特点，运用删除冗余节点策略，减少了转向，引入了Reeds-Shepp 曲线，约束了路径的曲率，再加上人工势场对未知环境的探索，提出了基于势场化改进A* 算法的履带式车辆路径规划算法。2）根据履带式车辆的行驶路径，提出了评价路径优劣性的路径直线占比、平均直线路径长度以及转向次数这3 个性能指标。高的直线占比率与平均直线路径长度降低了控制难度，有利于提高跟踪精度；较少的转弯次数使得在履带车辆跟踪路径的过程中，操纵杆的操作频次大大减少，有利于减少转向机构不稳定性带来的跟踪误差。3）最终本文算法所得路径总长度短，平均直线路径长度，直线路径占比高，在障碍占比20%的复杂环境下也可使得直线路径占比高达75%，路径相对传统A*算法缩短了19.6%，转弯次数相对于传统人工势场减少了63%。