CN104897168A - 基于道路危险评估的智能车路径搜索方法及系统 - Google Patents

Abstract

本发明公开一种基于道路危险评估的智能车路径搜索方法及系统，能够解决现有的智能车路径搜索方法在许多情况下不能实现路径搜索，或者不能实现准确的路径搜索的问题。所述方法包括：S1、将车辆行驶的道路环境建模成由边长相同的多个正方形栅格组成的二维模型；S2、以栅格的危险评估值最小为原则，采用启发式搜索算法搜索从车辆车头中心位置所处栅格至目的区域栅格的最优栅格路径；其中，栅格的危险评估值根据车辆传感器感知的环境信息计算得到。

Description

基于道路危险评估的智能车路径搜索方法及系统

技术领域

本发明涉及智能车辆路径规划技术领域，具体涉及一种基于道路危险评估的智能车路径搜索方法及系统。

背景技术

智能车路径搜索是指移动实体按照某一指标(如距离、时间、能量等)，在环境建模地图中寻找一条从起始状态到目标状态的行驶路径。路径搜索包括实时寻路和非实时寻路。实时寻路指的是在驾驶过程中，随着实时道路交通信息的不断变化而对寻路结果不断地进行重新计算修正，它依赖于实时道路交通信息采集的频率和所寻行驶路径的长度等因素，一般需要进行多次计算。非实时寻路指算法只在起始时刻进行一次搜索，不利用实时交通信息去进行修正和更新，在导航的整个过程中始终利用初始的计算结果。

由于在智能车自主驾驶过程中道路情况会实时变化，利用非实时寻路算法具有明显的缺陷。而在智能车环境感知的过程中，经常会碰到由于环境复杂，无法识别道路中障碍物，车道线，交通标志等信息。当只具备一类或几类环境信息时，智能车往往不知道怎样根据获得的一类或几类环境信息，生成一条当前感知能力条件下的最佳行驶路径，即寻路算法一般会失效。综上，现有的智能车路径搜索方法在许多情况下不能实现路径搜索，或者不能实现准确的路径搜索。

发明内容

本发明的目的在于，提供一种基于道路危险评估的智能车路径搜索方法及系统，能够解决现有的智能车路径搜索方法在许多情况下不能实现路径搜索，或者不能实现准确的路径搜索的问题。

为此目的，本发明提出一种基于道路危险评估的智能车路径搜索方法，包括如下步骤：

S1、将车辆行驶的道路环境建模成由边长相同的多个正方形栅格组成的二维模型；

S2、以栅格的危险评估值最小为原则，采用启发式搜索算法搜索从车辆车头中心位置所处栅格至目的区域栅格的最优栅格路径；其中，栅格的危险评估值根据车辆传感器感知的环境信息计算得到。

另一方面，本实施例公开一种基于道路危险评估的智能车路径搜索系统，包括：

模型建立单元，用于将车辆行驶的道路环境建模成由边长相同的多个正方形栅格组成的二维模型；

路径搜索单元，用于以栅格的危险评估值最小为原则，采用启发式搜索算法搜索从车辆车头中心位置所处栅格至目的区域栅格的最优栅格路径；其中，栅格的危险评估值根据车辆传感器感知的环境信息计算得到。

本发明实施例所述的基于道路危险评估的智能车路径搜索方法及系统，将车辆行驶的道路环境建模成由边长相同的多个正方形栅格组成的二维模型，并以栅格的危险评估值最小为原则，采用启发式搜索算法搜索从车辆车头中心位置所处栅格至目的区域栅格的最优栅格路径，而栅格的危险评估值根据车辆传感器感知的环境信息计算得到，这就使得相较于现有技术，当只具备一类或几类环境信息时，仍然能够利用已感知到的环境信息生成一条当前感知能力条件下的最佳行驶路径，从而能够克服现有技术寻路算法失效的缺陷，同时，本发明属于实时寻路算法，相较于现有的非实时寻路算法，能够克服现有的非实时寻路算法因只进行一次路径搜索所产生的不能根据实时道路环境信息进行实时路径修正、更新的缺陷，即本发明能够解决现有的智能车路径搜索方法在许多情况下不能实现路径搜索，或者不能实现准确的路径搜索的问题。

附图说明

图1为本发明基于道路危险评估的智能车路径搜索方法一实施例的流程示意图；

图2为本发明基于道路危险评估的智能车路径搜索方法另一实施例中把障碍物位置描述为其所在的栅格区域的示意图；

图3为本发明基于道路危险评估的智能车路径搜索方法又一实施例中待计算危险评估值的栅格及其附近障碍物的示意图；

图4为本发明基于道路危险评估的智能车路径搜索方法又一实施例中栅格搜索范围内的三个栅格的示意图；

图5为本发明基于道路危险评估的智能车路径搜索方法又一实施例中当具备障碍物和车道线信息时的仿真搜索结果示意图，*为最终路径示意图；

图6为本发明基于道路危险评估的智能车路径搜索方法又一实施例中真实城市环境的路径搜索结果示意图，*为最终路径示意图；

图7为本发明基于道路危险评估的智能车路径搜索系统一实施例的方框结构示意图。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

如图1所示，本实施例公开一种基于道路危险评估的智能车路径搜索方法(the search method based on the risk assessment，简称SRA)，包括如下步骤：

S1、将车辆行驶的道路环境建模成由边长相同的多个正方形栅格组成的二维模型；

S2、以栅格的危险评估值最小为原则，采用启发式搜索算法搜索从车辆车头中心位置所处栅格至目的区域栅格的最优栅格路径；其中，栅格的危险评估值根据车辆传感器感知的环境信息计算得到到。

本发明实施例的基于道路危险评估的智能车路径搜索方法，将车辆行驶的道路环境建模成由边长相同的多个正方形栅格组成的二维模型，并以栅格的危险评估值最小为原则，采用启发式搜索算法搜索从车辆车头中心位置所处栅格至目的区域栅格的最优栅格路径，而栅格的危险评估值根据车辆传感器感知的环境信息计算得到，这就使得相较于现有技术，当只具备一类或几类环境信息时，仍然能够利用已感知到的环境信息生成一条当前感知能力条件下的最佳行驶路径，从而能够克服现有技术寻路算法失效的缺陷，同时，本发明属于实时寻路算法，相较于现有的非实时寻路算法，能够克服现有的非实时寻路算法因只进行一次路径搜索所产生的不能根据实时道路环境信息进行实时路径修正、更新的缺陷，即本发明能够解决现有的智能车路径搜索方法在许多情况下不能实现路径搜索，或者不能实现准确的路径搜索的问题。

可选地，在本发明基于道路危险评估的智能车路径搜索方法的另一实施例中，所述S2包括：

S20、在启发式搜索算法开始时，将车辆车头中心位置所处栅格作为第一级父栅格，并将所述第一级父栅格存入预先建立的已搜索列表中，计算所述第一级父栅格搜索范围内的三个栅格的危险评估值，确定出此次计算出的危险评估值中的最小值，判断该最小值是否小于预设的第一阈值，若小于所述第一阈值，则确定该最小值对应的栅格为此次搜索的第二级父栅格，将此次搜索的第二级父栅格存入所述已搜索列表中，并将所述第一级父栅格作为此次搜索的第二级父栅格的父栅格，将此次搜索的第二级父栅格作为所述第一级父栅格的子栅格，建立所述第一级父栅格和此次搜索的第二级父栅格的父子关系；其中栅格搜索范围内的三个栅格为沿车头方向位于栅格前方的三个栅格；

S21、对于所述已搜索列表中作为父子链终端的第i级父栅格，判断该第i级父栅格是否是所述第一级父栅格，若不是所述第一级父栅格，则判断该第i级父栅格搜索范围内的三个栅格是否为目的区域栅格，若该三个栅格均不是目的区域栅格，则判断该三个栅格是否均存在于所述已搜索列表中，若该三个栅格中存在不存在于所述已搜索列表中的栅格，则计算该三个栅格中不存在于所述已搜索列表中的栅格的危险评估值，确定出此次计算出的危险评估值中的最小值，判断该最小值是否小于所述第一阈值，若小于所述第一阈值，则确定该最小值对应的栅格为此次搜索的第i+1级父栅格，将此次搜索的第i+1级父栅格存入所述已搜索列表中，并将所述第i级父栅格作为此次搜索的第i+1级父栅格的父栅格，将此次搜索的第i+1级父栅格作为所述第i级父栅格的子栅格，建立所述第i级父栅格和此次搜索的第i+1级父栅格的父子关系，否则，若该最小值不小于所述第一阈值，或者该三个栅格均存在于所述已搜索列表中，则取消所述已搜索列表中与所述第i级父栅格存在父子关系的第i-1级父栅格和所述第i级父栅格之间的父子关系，重新搜索所述第i-1级父栅格的子栅格，或者若该三个栅格中的一个栅格是目的区域栅格，则将从所述第一级父栅格开始至所述第i级父栅格的父子链以及目的区域栅格相应的路径确定为从车辆车头中心位置所处栅格至目的区域栅格的最优路径，或者若该第i级父栅格是所述第一级父栅格，则判断该第一级父栅格搜索范围内的三个栅格是否均存在于所述已搜索列表中，若该三个栅格中存在不存在于所述已搜索列表中的栅格，则计算该三个栅格中不存在于所述已搜索列表中的栅格的危险评估值，确定出此次计算出的危险评估值中的最小值，判断该最小值是否小于所述第一阈值，若小于所述第一阈值，则确定该最小值对应的栅格为此次搜索的第二级父栅格，将此次搜索的第二级父栅格存入所述已搜索列表中，并将所述第一级父栅格作为此次搜索的第二级父栅格的父栅格，将此次搜索的第二级父栅格作为所述第一级父栅格的子栅格，建立所述第一级父栅格和此次搜索的第二级父栅格的父子关系，否则，若该最小值不小于所述第一阈值，或者该三个栅格均存在于所述已搜索列表中，则将所述已搜索列表中的历史父子关系链中第一次距离目的区域栅格最近的历史父子关系链相应的路径确定为从车辆车头中心位置所处栅格至目的区域栅格所能达到的最远路径，i为正整数。

本申请中SRA搜索算法建立于以下基础：

1、道路中所检测的障碍物为静态，或速度较低，在智能车决策的一个周期内可以看成静态障碍物来处理。

2、已经利用环境感知方法准确获取以下全部或部分信息：

(1)障碍物大小、位置及分布；

(2)车道线位置及种类(如双黄线，白虚线等)；

(3)本车速度及安全转向角度；

(4)当前行驶任务；

(5)其他需要危险评估的已知信息。

3、智能车可以按照SRA搜索算法提供的路径行驶。

下面对本发明的一具体实施例进行详细说明。

本发明一种基于道路危险评估的智能车路径搜索方法的具体步骤如下：

(1)构造环境栅格图。

将道路环境抽象成边长相同的二维矩形栅格，栅格的边长根据环境识别精度和车辆控制精度选取(在实施例中选取的长度为20cm)。将栅格编号后，把本车自身位置，障碍物位置、车道中心线位置等都描述为其在栅格中的坐标位置。如图2所示，是把障碍物位置描述为其所在的栅格区域的示意图。

(2)确定栅格的危险估计值。

若智能车行驶到任意栅格所在的区域，都会面临来自环境和自身的危险，其危险评估方法如下：

f(n)＝g(n)+h(n)+i(n)+j(n)+k(n)，

其中，f(n)是当前栅格危险评估值，g(n)是障碍物危险评估值，h(n)是车速危险评估值，i(n)是车道线危险评估值，j(n)是任务危险评估值，k(n)是其他危险评估值。

即：当前危险评估值＝障碍物危险评估值+车速危险评估值+车道线危险评估值+任务危险评估值+其他危险评估值。

障碍物危险评估g(n)是指，道路中出现的障碍物对栅格区域危险程度的影响值。这里的障碍物包括正障碍(如车辆、行人)和负障碍(如水坑，悬崖)。根据常理我们可知，车辆距离障碍物越近其面临的危险程度越大，而离障碍物越远越安全。因此栅格障碍物危险评估值同该栅格与最近障碍物之间的距离成反比，于是可以构造如下公式：g(n)＝W-S·u，其中，w为该类方法中指定的最大危险值。S为当前栅格到最近障碍栅格的栅格距离，可以规定，两个相邻栅格的距离值为l，对角线方向相邻的栅格距离为以此类推。u为单位距离的第一危险值。

如图3所示，A，B为障碍物所在栅格，C，D为待求栅格。假设我们指定栅格的最大危险值为1000，单位距离第一危险值为100。那么对于栅格C，障碍A离它最近，且栅格距离为2，因此栅格C的障碍物危险评估值为g(C)＝1000-2·100＝800。

对于栅格D，障碍A离它最近，且栅格距离为因此栅格D的障碍物危险评估值为运用此种方法可以计算任意栅格的障碍物危险估计值g(n)。

车速危险评估h(n)是指，当本车处于任意栅格时，在当前车速和行驶方向的条件下，能够到达另一栅格的危险评估值。根据车体动力学我们可知，当车速较快时，转向角越大则离心力越大，车辆越面临翻车的危险；相反转向角越小越安全。因此栅格车速危险评估值同车辆当前位置、车速、转向角一定关系。于是可以构造如下公式：

其中，α为车辆行驶到该栅格需要转向的栅格角度，由于本文所述搜索算法只评估车辆所处栅格之后的三个栅格，因此转向的栅格角度只有可能为0度或45度，v为当前车速，k为一常数系数。

车道线危险评估i(n)是指，车道虚拟中心线对栅格区域危险程度的影响值。根据交通法规我们可知，车辆应该行驶于两车道线中间的虚拟中心线上，而长时间行驶在车道线上特别是双黄线，白实线上都属于违章行为。因此车道线危险评估同该栅格与车道线虚拟中心线之间的距离成正比，于是可以构造如下公式：

i(n)＝p·q，其中，p为待计算危险评估值的栅格到最近车道虚拟中心线的栅格距离，可以规定，两个相邻栅格的距离值为l，以此类推，q为预设的单位栅格距离的第二危险值。

任务危险评估j(n)是指，车辆当前执行的特殊任务对栅格区域危险程度的影响值。此类危险评估没有具体计算方式，当车辆正常行驶时，任务危险评估值为0；当车辆执行特殊任务时，如红灯停车，跟车行驶等，此时栅格任务危险评估值需要人为指定。

其他危险评估k(n)是指，其他可能的因素对栅格区域危险程度的影响值。此类危险评估没有具体计算方式，当车辆正常行驶时，其他危险评估值为0；当遇到恶劣天气，车辆自身问题，人工干预意向等，其影响因素都可以通过人为指定的方式构造其他危险评估值。

在车辆行驶过程中，当车辆传感器只能感知到一类或者几类环境信息时，可以令需要根据车辆传感器未感知到的环境信息计算得到的各项危险评估值为0，仅用已知环境信息来评价道路各区域的危险评估值，从而本发明所述的搜索方法仍然能够实现较优路径的搜索。

(3)搜索可行驶路径。

SRA路径搜索算法依据各栅格危险评估值，运用深度优先遍历的启发式搜索方法进行路径搜索。该算法依靠父子栅格、已搜索列表、搜索范围三个属性展开：如果从栅格A搜索到下一栅格B，则称A为B的父栅格，B为A的子栅格；搜索列表保存了所有已经搜索过的父栅格坐标；搜索范围是指能够产生子栅格的栅格范围。算法以到达目的区域或找不到子栅格而结束。其具体搜索步骤为：

1.算法开始时，把车辆自身位置作为父栅格，并把父栅格放入到已搜索列表。由于智能车只能向前方直线行驶或转向，而不能横向移动，本文也暂不考虑倒车情况，因此智能车所有可到达的相邻栅格只有正前方的3个栅格。如图4所示，图中A为车辆自身位置所在父栅格，则其搜索范围为B、C、D三个栅格。

2.通过对危险估计值的计算，找出这三个栅格中危险估计值最小的栅格，判断其危险估计值是否超过阈值。阈值是由车体大小和操作经验人为设定的，估计值超过阈值的栅格为不可行驶栅格，反之为可行驶栅格。若该栅格危险估计值小于阈值，则将其作为当前父栅格的子栅格，并作为下一次搜索的父栅格，放入已搜索列表。

3.开始下一次搜索，通过与已搜列表比对，判断本次父栅格搜索范围内所有栅格是否有未搜栅格。

4.若存在未搜栅格，并存在危险估计值小于阈值的栅格，则找出这几个未搜栅格中危险估计值最小的栅格，作为当前父栅格的子栅格，并作为下一次搜索的父栅格，放入已搜索列表。然后回到步骤3。

5.若不存在未搜栅格，或所有未搜栅格的危险估计值均大于阈值，则找到本次父栅格在上一次搜索时所对应的父栅格，取消它们之间的父子关系，并把上一次搜索所对应的父栅格作为下一次搜索的父栅格。然后回到步骤3。

6.在执行(3)、(4)、(5)的循环搜索过程中，如果搜索到目的区域的栅格，则搜索结束，生成从车辆自身栅格到目的区域栅格的父子关系链；如果在未到达目的区域栅格之前，搜索已经回退到车辆自身栅格，并且其搜索范围内不存在未搜栅格，则搜索结束，生成历史父子关系链中第一次到达距离目标区域最近的一条父子关系链。

本发明提出了一种智能车SRA路径搜索算法，在智能车路径规划的过程中，由于环境感知所得信息有限，往往不知道以什么标准来搜索最佳行驶路径。SRA路径搜索算法提供一种依据道路危险评估搜索最优路径的方法，在比较和实验多种方法的基础上，自主提出危险值计算方法和搜索方法，并能够依据所计算的道路危险值实时搜索可行路径。

在智能车路径规划的过程中，需要从车辆当前点规划最佳路线到达目标点。SRA搜索算法是发展人工智能而产生的路径规划十分有效的启发式搜索方法，算法采用深度优先遍历的方法，到达目标节点即停止搜索，若无法到达目标结点，算法可以给出最接近于目标点的路径。它首先利用二维栅格图对车辆行驶环境进行建模；然后采用危险评估函数来计算当前点的估计值，通过对车道线，道路边界，障碍物，车辆速度，任务规划等因素对当前栅格产生的危险进行评估，最终确定每个栅格的危险估计值；最后利用启发式的搜索方法，寻找一条从车辆当前栅格到目的地区域的危险值最小的栅格路径作为智能车当前的可行驶路径。

可选地，在本发明基于道路危险评估的智能车路径搜索方法的另一实施例中，栅格的危险评估值的计算公式为f(n)＝g(n)+h(n)+i(n)+j(n)+k(n)，其中，f(n)是待计算危险评估值的栅格的危险评估值，g(n)是障碍物危险评估值，h(n)是车速危险评估值，i(n)是车道线危险评估值，j(n)是任务危险评估值，k(n)是其他危险评估值。

可选地，在本发明基于道路危险评估的智能车路径搜索方法的另一实施例中，所述g(n)＝W-S·u，其中，W为预设的第二阈值，S为待计算危险评估值的栅格到最近障碍栅格的栅格距离，u为预设的单位栅格距离的第一危险值。

可选地，在本发明基于道路危险评估的智能车路径搜索方法的另一实施例中，所述其中，α为车辆行驶到待计算危险评估值的栅格需要转向的栅格角度，v为车辆当前速度，k为一常数系数。

可选地，在本发明基于道路危险评估的智能车路径搜索方法的另一实施例中，所述i(n)＝p·q，其中，p为待计算危险评估值的栅格到最近车道虚拟中心线的栅格距离，q为预设的单位栅格距离的第二危险值。

可选地，在本发明基于道路危险评估的智能车路径搜索方法的另一实施例中，当车辆正常行驶时，所述j(n)的值为0，当车辆执行特殊任务时，所述j(n)的值为常数。

可选地，在本发明基于道路危险评估的智能车路径搜索方法的另一实施例中，当车辆正常行驶时，所述k(n)的值为0，当车辆遇到特殊情况时，所述k(n)的值为常数。

如图7所示，本实施例公开一种基于道路危险评估的智能车路径搜索系统，包括：

模型建立单元1，用于将车辆行驶的道路环境建模成由边长相同的多个正方形栅格组成的二维模型；

路径搜索单元2，用于以栅格的危险评估值最小为原则，采用启发式搜索算法搜索从车辆车头中心位置所处栅格至目的区域栅格的最优栅格路径；其中，栅格的危险评估值根据车辆传感器感知的环境信息计算得到。

本发明实施例的基于道路危险评估的智能车路径搜索系统，将车辆行驶的道路环境建模成由边长相同的多个正方形栅格组成的二维模型，并以栅格的危险评估值最小为原则，采用启发式搜索算法搜索从车辆车头中心位置所处栅格至目的区域栅格的最优栅格路径，而栅格的危险评估值根据车辆传感器感知的环境信息计算得到，这就使得相较于现有技术，当只具备一类或几类环境信息时，仍然能够利用已感知到的环境信息生成一条当前感知能力条件下的最佳行驶路径，从而能够克服现有技术寻路算法失效的缺陷，同时，本发明属于实时寻路算法，相较于现有的非实时寻路算法，能够克服现有的非实时寻路算法因只进行一次路径搜索所产生的不能根据实时道路环境信息进行实时路径修正、更新的缺陷，即本发明能够解决现有的智能车路径搜索方法在许多情况下不能实现路径搜索，或者不能实现准确的路径搜索的问题。

可选地，在本发明基于道路危险评估的智能车路径搜索系统的另一实施例中，所述路径搜索单元包括：

第一栅格搜索子单元，用于在启发式搜索算法开始时，将车辆车头中心位置所处栅格作为第一级父栅格，并将所述第一级父栅格存入预先建立的已搜索列表中，计算所述第一级父栅格搜索范围内的三个栅格的危险评估值，确定出此次计算出的危险评估值中的最小值，判断该最小值是否小于预设的第一阈值，若小于所述第一阈值，则确定该最小值对应的栅格为此次搜索的第二级父栅格，将此次搜索的第二级父栅格存入所述已搜索列表中，并将所述第一级父栅格作为此次搜索的第二级父栅格的父栅格，将此次搜索的第二级父栅格作为所述第一级父栅格的子栅格，建立所述第一级父栅格和此次搜索的第二级父栅格的父子关系；其中栅格搜索范围内的三个栅格为沿车头方向位于栅格前方的三个栅格；

第一路径搜索子单元，用于对于所述已搜索列表中作为父子链终端的第i级父栅格，判断该第i级父栅格是否是所述第一级父栅格，若不是所述第一级父栅格，则判断该第i级父栅格搜索范围内的三个栅格是否为目的区域栅格，若该三个栅格均不是目的区域栅格，则判断该三个栅格是否均存在于所述已搜索列表中，若该三个栅格中存在不存在于所述已搜索列表中的栅格，则计算该三个栅格中不存在于所述已搜索列表中的栅格的危险评估值，确定出此次计算出的危险评估值中的最小值，判断该最小值是否小于所述第一阈值，若小于所述第一阈值，则确定该最小值对应的栅格为此次搜索的第i+1级父栅格，将此次搜索的第i+1级父栅格存入所述已搜索列表中，并将所述第i级父栅格作为此次搜索的第i+1级父栅格的父栅格，将此次搜索的第i+1级父栅格作为所述第i级父栅格的子栅格，建立所述第i级父栅格和此次搜索的第i+1级父栅格的父子关系，否则，若该最小值不小于所述第一阈值，或者该三个栅格均存在于所述已搜索列表中，则取消所述已搜索列表中与所述第i级父栅格存在父子关系的第i-1级父栅格和所述第i级父栅格之间的父子关系，重新搜索所述第i-1级父栅格的子栅格，或者若该三个栅格中的一个栅格是目的区域栅格，则将从所述第一级父栅格开始至所述第i级父栅格的父子链以及目的区域栅格相应的路径确定为从车辆车头中心位置所处栅格至目的区域栅格的最优路径，或者若该第i级父栅格是所述第一级父栅格，则判断该第一级父栅格搜索范围内的三个栅格是否均存在于所述已搜索列表中，若该三个栅格中存在不存在于所述已搜索列表中的栅格，则计算该三个栅格中不存在于所述已搜索列表中的栅格的危险评估值，确定出此次计算出的危险评估值中的最小值，判断该最小值是否小于所述第一阈值，若小于所述第一阈值，则确定该最小值对应的栅格为此次搜索的第二级父栅格，将此次搜索的第二级父栅格存入所述已搜索列表中，并将所述第一级父栅格作为此次搜索的第二级父栅格的父栅格，将此次搜索的第二级父栅格作为所述第一级父栅格的子栅格，建立所述第一级父栅格和此次搜索的第二级父栅格的父子关系，否则，若该最小值不小于所述第一阈值，或者该三个栅格均存在于所述已搜索列表中，则将所述已搜索列表中的历史父子关系链中第一次距离目的区域栅格最近的历史父子关系链相应的路径确定为从车辆车头中心位置所处栅格至目的区域栅格所能达到的最远路径，i为正整数。

本发明采用一套危险评估体系，根据当前感知的结果，形成可行驶区域中各部分的危险系数，从而搜索最佳行驶路径。与现有的技术比较，本发明的优点是:

一、有效融合了多传感器数据。

由于智能车使用的传感器种类并不相同，数据各有特点，融合十分困难。本算法提出了危险评估的层面，使得多种传感器数据能够在此层面得到统一，并且形成量化指标写入对应栅格图；同时，人为因素的影响也可以统一到栅格图，作为对环境的危险评估之一，从而指导最优路径的生成。

二、当环境信息不全时，也可以实现SRA搜索算法。

在智能车环境感知的过程中，经常会碰到由于环境复杂，无法识别道路中障碍物，车道线，交通标志等信息。当只具备一类或几类环境信息时，我们可以令未知信息的危险估计值为0，仅用已知信息来评价道路各区域的危险评估值，这样SRA搜索算法仍然能够实现当前感知条件下的较优路径的搜索。

三、目的区域不可到达时，SRA搜索算法可以给出最远路径。

当车辆前方的目的区域被堵死以至于无法到达时，现有的搜索算法一般会失效，而该算法会记录在深度优先搜索过程中最接近目的区域的位置，从而提供一条能够行驶得最靠近目的区域的较优路径。

虽然结合附图描述了本发明的实施方式，但是本领域技术人员可以在不脱离本发明的精神和范围的情况下做出各种修改和变型，这样的修改和变型均落入由所附权利要求所限定的范围之内。

Claims (10)

1.一种基于道路危险评估的智能车路径搜索方法，其特征在于，包括如下步骤：

S1、将车辆行驶的道路环境建模成由边长相同的多个正方形栅格组成的二维模型；

S2、以栅格的危险评估值最小为原则，采用启发式搜索算法搜索从车辆车头中心位置所处栅格至目的区域栅格的最优栅格路径；其中，栅格的危险评估值根据车辆传感器感知的环境信息计算得到。

2.根据权利要求1所述的基于道路危险评估的智能车路径搜索方法，其特征在于，所述S2包括：

S20、在启发式搜索算法开始时，将车辆车头中心位置所处栅格作为第一级父栅格，并将所述第一级父栅格存入预先建立的已搜索列表中，计算所述第一级父栅格搜索范围内的三个栅格的危险评估值，确定出此次计算出的危险评估值中的最小值，判断该最小值是否小于预设的第一阈值，若小于所述第一阈值，则确定该最小值对应的栅格为此次搜索的第二级父栅格，将此次搜索的第二级父栅格存入所述已搜索列表中，并将所述第一级父栅格作为此次搜索的第二级父栅格的父栅格，将此次搜索的第二级父栅格作为所述第一级父栅格的子栅格，建立所述第一级父栅格和此次搜索的第二级父栅格的父子关系；其中栅格搜索范围内的三个栅格为沿车头方向位于栅格前方的三个栅格；

S21、对于所述已搜索列表中作为父子链终端的第i级父栅格，判断该第i级父栅格是否是所述第一级父栅格，若不是所述第一级父栅格，则判断该第i级父栅格搜索范围内的三个栅格是否为目的区域栅格，若该三个栅格均不是目的区域栅格，则判断该三个栅格是否均存在于所述已搜索列表中，若该三个栅格中存在不存在于所述已搜索列表中的栅格，则计算该三个栅格中不存在于所述已搜索列表中的栅格的危险评估值，确定出此次计算出的危险评估值中的最小值，判断该最小值是否小于所述第一阈值，若小于所述第一阈值，则确定该最小值对应的栅格为此次搜索的第i+1级父栅格，将此次搜索的第i+1级父栅格存入所述已搜索列表中，并将所述第i级父栅格作为此次搜索的第i+1级父栅格的父栅格，将此次搜索的第i+1级父栅格作为所述第i级父栅格的子栅格，建立所述第i级父栅格和此次搜索的第i+1级父栅格的父子关系，否则，若该最小值不小于所述第一阈值，或者该三个栅格均存在于所述已搜索列表中，则取消所述已搜索列表中与所述第i级父栅格存在父子关系的第i-1级父栅格和所述第i级父栅格之间的父子关系，重新搜索所述第i-1级父栅格的子栅格，或者若该三个栅格中的一个栅格是目的区域栅格，则将从所述第一级父栅格开始至所述第i级父栅格的父子链以及目的区域栅格相应的路径确定为从车辆车头中心位置所处栅格至目的区域栅格的最优路径，或者若该第i级父栅格是所述第一级父栅格，则判断该第一级父栅格搜索范围内的三个栅格是否均存在于所述已搜索列表中，若该三个栅格中存在不存在于所述已搜索列表中的栅格，则计算该三个栅格中不存在于所述已搜索列表中的栅格的危险评估值，确定出此次计算出的危险评估值中的最小值，判断该最小值是否小于所述第一阈值，若小于所述第一阈值，则确定该最小值对应的栅格为此次搜索的第二级父栅格，将此次搜索的第二级父栅格存入所述已搜索列表中，并将所述第一级父栅格作为此次搜索的第二级父栅格的父栅格，将此次搜索的第二级父栅格作为所述第一级父栅格的子栅格，建立所述第一级父栅格和此次搜索的第二级父栅格的父子关系，否则，若该最小值不小于所述第一阈值，或者该三个栅格均存在于所述已搜索列表中，则将所述已搜索列表中的历史父子关系链中第一次距离目的区域栅格最近的历史父子关系链相应的路径确定为从车辆车头中心位置所处栅格至目的区域栅格所能达到的最远路径，i为正整数。

3.根据权利要求2所述的基于道路危险评估的智能车路径搜索方法，其特征在于，栅格的危险评估值的计算公式为f(n)＝g(n)+h(n)+i(n)+j(n)+k(n)，其中，f(n)是待计算危险评估值的栅格的危险评估值，g(n)是障碍物危险评估值，h(n)是车速危险评估值，i(n)是车道线危险评估值，j(n)是任务危险评估值，k(n)是其他危险评估值。

4.根据权利要求3所述的基于道路危险评估的智能车路径搜索方法，其特征在于，所述g(n)＝W-S·u，其中，W为预设的第二阈值，S为待计算危险评估值的栅格到最近障碍栅格的栅格距离，u为预设的单位栅格距离的第一危险值。

5.根据权利要求3所述的基于道路危险评估的智能车路径搜索方法，其特征在于，所述其中，α为车辆行驶到待计算危险评估值的栅格需要转向的栅格角度，v为车辆当前速度，k为一常数系数。

6.根据权利要求3所述的基于道路危险评估的智能车路径搜索方法，其特征在于，所述i(n)＝p·q，其中，p为待计算危险评估值的栅格到最近车道虚拟中心线的栅格距离，q为预设的单位栅格距离的第二危险值。

7.根据权利要求3所述的基于道路危险评估的智能车路径搜索方法，其特征在于，当车辆正常行驶时，所述j(n)的值为0，当车辆执行特殊任务时，所述j(n)的值为常数。

8.根据权利要求3所述的基于道路危险评估的智能车路径搜索方法，其特征在于，当车辆正常行驶时，所述k(n)的值为0，当车辆遇到特殊情况时，所述k(n)的值为常数。

9.一种基于道路危险评估的智能车路径搜索系统，其特征在于，包括：

模型建立单元，用于将车辆行驶的道路环境建模成由边长相同的多个正方形栅格组成的二维模型；

路径搜索单元，用于以栅格的危险评估值最小为原则，采用启发式搜索算法搜索从车辆车头中心位置所处栅格至目的区域栅格的最优栅格路径；其中，栅格的危险评估值根据车辆传感器感知的环境信息计算得到。

10.根据权利要求9所述的基于道路危险评估的智能车路径搜索系统，其特征在于，所述路径搜索单元包括：

第一栅格搜索子单元，用于在启发式搜索算法开始时，将车辆车头中心位置所处栅格作为第一级父栅格，并将所述第一级父栅格存入预先建立的已搜索列表中，计算所述第一级父栅格搜索范围内的三个栅格的危险评估值，确定出此次计算出的危险评估值中的最小值，判断该最小值是否小于预设的第一阈值，若小于所述第一阈值，则确定该最小值对应的栅格为此次搜索的第二级父栅格，将此次搜索的第二级父栅格存入所述已搜索列表中，并将所述第一级父栅格作为此次搜索的第二级父栅格的父栅格，将此次搜索的第二级父栅格作为所述第一级父栅格的子栅格，建立所述第一级父栅格和此次搜索的第二级父栅格的父子关系；其中栅格搜索范围内的三个栅格为沿车头方向位于栅格前方的三个栅格；

第一路径搜索子单元，用于对于所述已搜索列表中作为父子链终端的第i级父栅格，判断该第i级父栅格是否是所述第一级父栅格，若不是所述第一级父栅格，则判断该第i级父栅格搜索范围内的三个栅格是否为目的区域栅格，若该三个栅格均不是目的区域栅格，则判断该三个栅格是否均存在于所述已搜索列表中，若该三个栅格中存在不存在于所述已搜索列表中的栅格，则计算该三个栅格中不存在于所述已搜索列表中的栅格的危险评估值，确定出此次计算出的危险评估值中的最小值，判断该最小值是否小于所述第一阈值，若小于所述第一阈值，则确定该最小值对应的栅格为此次搜索的第i+1级父栅格，将此次搜索的第i+1级父栅格存入所述已搜索列表中，并将所述第i级父栅格作为此次搜索的第i+1级父栅格的父栅格，将此次搜索的第i+1级父栅格作为所述第i级父栅格的子栅格，建立所述第i级父栅格和此次搜索的第i+1级父栅格的父子关系，否则，若该最小值不小于所述第一阈值，或者该三个栅格均存在于所述已搜索列表中，则取消所述已搜索列表中与所述第i级父栅格存在父子关系的第i-1级父栅格和所述第i级父栅格之间的父子关系，重新搜索所述第i-1级父栅格的子栅格，或者若该三个栅格中的一个栅格是目的区域栅格，则将从所述第一级父栅格开始至所述第i级父栅格的父子链以及目的区域栅格相应的路径确定为从车辆车头中心位置所处栅格至目的区域栅格的最优路径，或者若该第i级父栅格是所述第一级父栅格，则判断该第一级父栅格搜索范围内的三个栅格是否均存在于所述已搜索列表中，若该三个栅格中存在不存在于所述已搜索列表中的栅格，则计算该三个栅格中不存在于所述已搜索列表中的栅格的危险评估值，确定出此次计算出的危险评估值中的最小值，判断该最小值是否小于所述第一阈值，若小于所述第一阈值，则确定该最小值对应的栅格为此次搜索的第二级父栅格，将此次搜索的第二级父栅格存入所述已搜索列表中，并将所述第一级父栅格作为此次搜索的第二级父栅格的父栅格，将此次搜索的第二级父栅格作为所述第一级父栅格的子栅格，建立所述第一级父栅格和此次搜索的第二级父栅格的父子关系，否则，若该最小值不小于所述第一阈值，或者该三个栅格均存在于所述已搜索列表中，则将所述已搜索列表中的历史父子关系链中第一次距离目的区域栅格最近的历史父子关系链相应的路径确定为从车辆车头中心位置所处栅格至目的区域栅格所能达到的最远路径，i为正整数。