一种基于启发式搜索算法来缩短智能汽车路径规划搜索时间的方法技术领域
本发明涉及自动驾驶汽车领域，特别是涉及一种缩短智能汽车路径规划搜索时间
的方法。
背景技术
自动驾驶系统在智能交通领域占有重要的地位，是计算机人工智能、机器人学、控
制理论和电子技术等多个技术学科交叉的产物。其根据各传感器得到的信息做出分析和判
断，把人从单一持久的驾驶活动中解放出来，减少驾驶行为差异对交通流稳定性的影响，有
利于提高现有道路网络的车辆通行率缓解交通拥堵，另一方面可以提高汽车行驶安全，降
低交通事故率改善交通安全，降低能源消耗和环境污染，对我国能源转型减少污染、缓解交
通拥堵以及汽车产业自主创业都有重大的战略意义。
无人驾驶汽车实时路径规划和导航是反映汽车自主能力的关键要素之一，也是较
难解决的问题之一。路径规划问题通常存在环境信息量大、障碍物多等约束，其已被证明是
NP-Hard(非确定性多项式困难)问题。特别是在近年来由于智能控制理论的建立和实践，越
来越多的学者和专家已经致力于结合智能控制来解决移动机器人的路径规划问题。例如遗
传算法(GA)、蚁群算法(Ant)、禁忌搜索(Taboo Search)等智能算法及其混合形式都被用来
解决路径规划问题。但这些智能算法目前还不太完善，存在着一些缺点，例如GA存在编码长
度变化范围大，求解效率低，求解规模小等问题；Dijkstra算法直接搜索全局空间而不考虑
目标信息，导致路径求解时间长，难以满足快速规划路径的需求。因此，智能汽车在遇到突
发状况时，路径求解时间减少，鲁棒性增强，将是实现无人驾驶安全的保障。
发明内容
针对现有技术中存在不足，本发明提供了一种基于启发式搜索算法来缩短智能汽
车路径规划搜索时间的方法。
本发明是通过以下技术手段实现上述技术目的的。一种基于启发式搜索算法来缩
短智能汽车路径规划搜索时间的方法，包括如下步骤：
(1)运用GPS、激光雷达工具采集环境信息；
(2)将环境信息均匀栅格化，建立包含障碍区域与自由区域的均匀栅格地图；
(3)取智能车发动位置为s点，手动设置目的地为g点。采用A*搜索算法在环境地图
中寻得s点到g点的最优路径X。选定路径X为智能车行驶路径；
(4)智能车开始行驶，监测车辆状态，导航引导过程中时刻检测智能车是否处于已
规划的路径上。一旦汽车偏离最优路径行驶，触发偏离机制，跳转步骤(5)；如一直安全到达
目的地，跳转步骤(10)；
(5)记当前汽车行驶位置为p点。根据设定的最高延误设置理论搜索最大时间T1，
智能汽车采用动态启发式搜索算法寻找当前汽车位置p到目的地g点的路径Y；
(6)检索路径Y是否与路径X有交集点q，如果有，转向步骤(7)；如果没有，转向步骤
(8)。
(7)路径Y中，从当前汽车点p到交集点q不变，将交集点q到目的地g点的路径转换
成路径X中q点到目的地g点的路径，并记此时从p点到g点的路径为路径Z。同时转向步骤
(9)；
(8)令路径Z等同于路径Y；
(9)选取路径Z更新为智能汽车此时的行驶路径。跳转步骤(4)；
(10)结束进程。
进一步，所述步骤(1)的具体过程为：首先导入应用差分定位技术的全球卫星定位
系统获得厘米级的起终点之间卫星地图，在先验地图已知的情况下，无人驾驶汽车可以根
据已知地图不断进行自身位置的校正，实现精确定位；在未知环境下，无人驾驶汽车可通过
激光雷达并采用SLAM的方法获取环境信息，经过信号处理抽取有效信息，以构建环境地图。
进一步，所述步骤(2)的具体过程为：用尺寸相同的栅格对智能汽车的二维工作空
间进行划分，栅格的大小以智能汽车自身的尺寸为准，若某个栅格范围内不含任何障碍物，
则称此栅格为自由栅格；反之，称为障碍栅格，自由空间和障碍物均可表示为栅格块的集
成，采用均匀栅格表示工作环境，起始栅格与目标栅格都是自由栅格，每个栅格都对应相应
坐标值，而坐标值就表示无人驾驶汽车在栅格地图内的当前位置，栅格节点间的典型连接
关系是八连接，启发函数选用欧几里德距离。
进一步，所述步骤(3)中，采用启发函数为f(s)＝g(s)+h(s)的A*算法。
进一步，所述步骤(4)中已规划路径为智能车此时选定的路径，如果之前没有触发
过偏离机制，则为路径X；否则为路径Z。
进一步，所述步骤(5)的动态启发式搜索算法，是将启发函数中添加一个权值系数
ε*，然后根据搜索总时间调整ε*，并且将搜索剩余时间加入到启发函数中，形成动态的启发
函数：
其中，T1为理论搜索最大时间，T为搜索消耗时间，单位ms；K为初始权值系数；B为
搜索极限速度系数；为触发搜索极限时间比率。
该发明的目的及效果为：
针对目前无人驾驶汽车面对突发情况(动态障碍物阻挡原始路径，原始道路施工
等)导致路径临时更改，而造成无人驾驶汽车路径生成延迟，发生待机的情况，优化路径规
划时间，增强无人驾驶汽车行驶的稳定性。
其中，步骤(4)的作用主要是时刻监测车辆是否处于规划的路径，为启用动态启发
函数的前提。
步骤(5)与现有技术方案相比，创造了动态的启发式函数，可以根据限定的搜索时
间，动态地完成此搜索时间下的全局路径规划。与现有的技术方案相比，避免了因环境地图
复杂导致的搜索时间过长，进一步引起智能汽车因无路径导航发生的无人驾驶汽车行驶方
向延迟，待机。
步骤(6)主要的作用是进一步优化动态搜索算法路径的最优性，更新由紧急情况
导致的智能汽车行驶路径上的最优性差异。
步骤(4)到步骤(9)的方法策略与现有技术方案相比，优化了智能汽车的鲁棒性
附图说明
图1为本发明所述的路径规划流程图。
图2为本发明所述的动态A*算法搜索流程图。
具体实施方式
下面结合附图以及具体实施例对本发明作进一步的说明，但本发明的保护范围并
不限于此。
本发明所述的一种基于启发式搜索算法来缩短智能汽车路径规划搜索时间的方
法，包括如下步骤：
(1)运用GPS、激光雷达工具采集环境信息：首先导入应用差分定位技术的全球卫
星定位系统获得厘米级的起终点之间卫星地图，在先验地图已知的情况下，无人驾驶汽车
可以根据已知地图不断进行自身位置的校正，实现精确定位。在未知环境下，无人驾驶汽车
可通过激光雷达并采用SLAM的方法获取环境信息，经过信号处理抽取有效信息，以构建环
境地图。
(2)将环境信息均匀栅格化，建立包含障碍区域与自由区域的均匀栅格地图：用尺
寸相同的栅格对智能汽车的二维工作空间进行划分，栅格的大小以智能汽车自身的尺寸为
准，若某个栅格范围内不含任何障碍物，则称此栅格为自由栅格；反之，称为障碍栅格。自由
空间和障碍物均可表示为栅格块的集成。采用均匀栅格表示工作环境。起始栅格与目标栅
格都是自由栅格。每个栅格都对应相应坐标值，而坐标值就表示无人驾驶汽车在栅格地图
内的当前位置。栅格节点间的典型连接关系是八连接，启发函数选用欧几里德距离。
(3)取智能车发动位置为s点，手动设置目的地为g点。采用A*搜索算法在环境地图
中寻得s点到g点的最优路径X。选定路径X为智能车行驶路径。
由于启动开车时间充裕，所以采用不计时间成本只保证最优路径的传统A*算法，
其启发函数为
f(s)＝g(s)+h(s) (1)
传统A*算法，可参考关泉珍，鲍泓，史志坚.基于A*算法的驾驶地图路径规划实现.
北京联合大学学报：自然科学版，2016，(2).的方法实施传统A*算法路径规划。
(4)智能车开始行驶，监测车辆状态，导航引导过程中时刻检测智能车是否处于已
规划的路径上。一旦汽车偏离最优路径行驶，触发偏离机制，跳转步骤(5)；如一直安全到达
目的地，跳转步骤(10)。偏离机制是指智能汽车由于突发情况例如避开动态障碍物而转向
行驶导致没有按照步骤(4)中智能车所选定的行驶路径的情形。这里面根据差分技术定位
的全球卫星定位时刻拟合智能汽车当前的行驶路径和环境地图中规划好的路径，一旦路径
偏差超过一定阈值，触发偏离机制。在实际应用中，触发偏离机制的因素有很多，例如交通
事故，道路施工，交通拥堵等。
(5)记当前汽车行驶位置为p点。根据设定的最高延误设置理论搜索最大时间T1，
智能汽车采用动态启发式搜索算法寻找当前汽车位置p到目的地g点的路径Y。
本动态启发式搜索算法是将启发函数中添加一个权值系数K和ε*，已此来调整搜
索最优性与搜索效率之间的自适应。然后根据搜索理论最大时间T1和搜索消耗时间T来调
整ε*(这里面的T1并不是实际智能车搜索路径时的最大时间，只是一个理论参考值，实际搜
索最大时间会有偏差)。并且将搜索剩余时间加入到启发函数中，形成动态的启发函数：
其中
g(s)是指起始点到s节点的真实距离；
g(Z，C)是指从节点Z到节点C的真实距离；
T1为理论搜索最大时间，T为搜索消耗时间；——单位可情况而定，这里暂定为ms。
K为初始权值系数；——影响搜索速度和路径的最优性。与速度成正比，与路径的
优化成反比。
B为搜索极限速度系数；——当搜索时间到达时，此时到搜索结束前一
直采用固定值B。
为触发搜索极限时间比率。——视情况决定，一般可取0.2～0.4。
所采用的动态启发式搜索算法在图2所示，图2中：
f值是公式(2)、(3)计算得出；
BEST是指当前节点且为OPEN中f值最小的节点；
SUC为BEST的子节点；
OLD为OPEN中已经计算过f值的节点。
(6)检索路径Y是否与路径X有交集点q，如果有，转向步骤(7)；如果没有，转向步骤
(8)。这里主要是指由动态启发式搜索算法生成的路径Y与由传统A*算法生成的路径X是否
有部分路径重合，记路径重合起始点为交集点q。
(7)路径Y中，从当前汽车点p到交集点q不变，将交集点q到目的地g点的路径转换
成路径X中q点到目的地g点的路径，并记此时从p点到g点的路径为路径Z。同时转向步骤
(9)。因经过传统A*算法得出的路径X由A*算法的性质可以被证明，为从起始点到终点全路
径及当前智能汽车行驶点到终点部分路径最优。所以当经过重新规划的路径Y中与传统A*
算法X有重合的部分，我们优先采用路径X。
(8)令路径Z等同于路径Y。
即将路径Y当前现有的路径信息复制给路径Z。
(9)选取路径Z更新为智能汽车此时的行驶路径。跳转步骤(4)。
即确定使用的路径，并命令智能车跟踪此路径继续行驶。
(10)结束进程。
综上，本发明的一种基于启发式搜索算法来缩短全局规划搜索时间的方法，该方
法能够应用在智能汽车由于突发情况而偏离最初设定最优路径的情况下。针对上述情况，
本发明提出了一种具有动态启发函数的路径搜索算法。首先需要通过差分技术的全球定位
系统和激光雷达实时地提取环境信息，然后通过将环境信息均匀栅格化，建立包含障碍区
域与自由区域的环境地图。之后在智能汽车发生偏离原定路径的情况时，触发偏离机制，调
取该动态搜索算法并快速实时地搜索可行路径。此发明可有效处理全局路径规划搜索路径
的最优性和搜索效率之间的矛盾，为无人驾驶的路径规划提供技术支持。
在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示意性实施例”、
“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结
构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的
示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特
点可以在任何的一个或多个实施例或示例中以合适的方式结合。
尽管已经示出和描述了本发明的实施例，本领域的普通技术人员可以理解：在不
脱离本发明的原理和宗旨的情况下可以对这些实施例进行多种变化、修改、替换和变型，本
发明的范围由权利要求及其等同物限定。