CN105526942A - 基于威胁估计的智能车辆路径规划方法 - Google Patents

Abstract

本发明请求保护一种基于威胁估计的智能车辆路径规划方法，涉及智能车辆决策与控制技术领域。首先确定影响智能车辆威胁评估的多种因素如外部目标特性及环境参数等，建立基于贝叶斯网络的智能车辆威胁估计模型。在智能车辆运行过程中，采集威胁因素的实时数据，根据该模型计算外部环境中的各目标对智能车辆的威胁指数；基于各目标对智能车辆的威胁指数构造各目标对智能车辆的斥力势场模型，与智能车辆行驶的终点对智能车辆的引力势场模型融合，得到智能车辆的综合势场模型，求解后得到智能车辆的路径规划。本发明通过对影响智能车辆威胁评估的多种因素的考虑，形成对智能车辆驾驶环境的更充分的认知，以规划出更为合理、有效的行驶路径。

Description

基于威胁估计的智能车辆路径规划方法

技术领域

本发明属于计算机和自动化技术，特别是智能车辆决策与控制技术领域，具体涉及一种利用贝叶斯网络进行智能车辆威胁估计并藉此实现智能车辆路径规划的方法。

背景技术

路径规划是智能车辆的重要组成部分，是实现智能车辆自动驾驶控制的基础。与移动机器人相比，智能车辆由于其面临高速、动态的复杂环境、严格的实时性与安全性约束等诸多因素，从而需要在路径规划时充分考虑不同类型、不同运动特征的道路车辆、行人及障碍物细节，规划出合理的路径，以支持智能车辆实现安全、高效的自动驾驶。

在现有的路径规划方法中，文献“基于人工势场法的移动机器人最优路径规划”(《航空学报》2007年S1期)采用人工势场方法实现移动机器人的局部路径规划，但并没有考虑环境对移动机器人路径规划的影响。中国专利申请：一种危险源环境下的机器人全局路径规划方法(申请号：2011100048825)仅考虑了威胁源与机器人之间的距离所造成的威胁程度，对威胁源的运动状态、身份特征、尺寸等因素并没有考虑。中国专利申请：一种车辆路径规划方法、系统及一种车载导航设备(申请号：2011103339004)依据当前位置坐标、终点坐标和实时交通路况信息动态地规划路径，未考虑外部环境中的目标对路径规划的影响，本质上属于全局路径规划。中国专利申请：一种语音播报式智能车辆路径规划装置与实施方法(申请号：2014100504696)中仅考虑了障碍物的距离、角度，并未考虑其运动状态、身份特征、尺寸等因素。文献“Threat-awarePathPlanninginUncertainUrbanEnvironments”(2010IEEE/RSJInternationalConferenceonIntelligentRobots&Systems)提出了一种基于威胁评估模型的自主车辆路径规划方法，威胁评估模块由意图预测器和威胁评估器组成，首先采用威胁评估算法计算出其他每辆车辆的意图可达性集合，然后根据其他车辆的意图可达性集合计算出在主车行驶路径上的威胁值，最后利用闭环快速搜索随机树算法计算一条威胁最小的路径。但该方法没有考虑环境因素、目标车辆威胁能力、智能车辆车况等因素对车辆路径规划的影响，威胁评估模型不充分。

本发明针对现有车辆路径规划方法在评估外部环境中的道路车辆、行人及障碍物等目标的威胁时，未考虑目标类型、运动特征的问题，提出一种利用贝叶斯网络进行智能车辆威胁估计并藉此实现智能车辆路径规划的方法，该方法首先构造包含目标类型、运动特征及环境因素的贝叶斯网络模型，用以估计外部环境中的道路车辆、行人及障碍物等目标的威胁指数，然后建立基于威胁指数的外部环境中的目标对智能车辆的斥力势场模型，结合智能车辆行驶的终点对智能车辆的引力势场模型，求得智能车辆在当前位置的综合势场模型，从而规划出能兼顾安全性与效率的智能车辆路径，以支持自动驾驶。

发明内容

针对以下现有的不足，提出了一种可以形成对智能车辆驾驶环境的更充分的认知，以规划出合理的路径，对智能车辆实现安全、高效的自动驾驶的基于威胁估计的智能车辆路径规划方法方法。。本发明的技术方案如下：一种基于威胁估计的智能车辆路径规划方法，其包括以下步骤：

步骤1、根据智能车辆当前位置及终点位置进行全局路径规划，获得全局最优路径；

步骤2、建立基于贝叶斯网络的智能车辆威胁估计模型，计算外部环境中的各目标对智能车辆的威胁指数；

步骤3、建立基于威胁指数的外部环境中的各目标对智能车辆的斥力势场模型，与智能车辆行驶终点对智能车辆的引力势场模型融合，得到智能车辆的综合势场模型；

步骤4、对步骤3建立的综合势场模型求解，搜索原始局部最优路径；

步骤5、对原始局部最优路径进行优化处理，得到智能车辆最终的行驶路径。

进一步的，步骤1根据智能车辆当前位置及终点位置进行全局路径规划，获得全局最优路径的步骤具体为：利用卫星导航或/和惯性导航装置来实现智能车辆的定位和状态估计，并确定智能车辆行驶的终点，然后与地图数据库匹配，进行全局路径规划；采用路网地图模型实现环境构建，用A*最短路径搜索算法实现全局最优路径规划。

进一步的，步骤2包括了智能车辆威胁建模的步骤与威胁估计的步骤。

进一步的，智能车辆威胁建模的步骤主要包括：

(1)确定影响智能车辆威胁评估的因素，包括外部环境中的目标的类型与运动特征、环境参数；

(2)根据步骤(1)的上述影响因素，确定贝叶斯网络拓扑结构；

(3)分析各因素对智能车辆威胁的影响程度，确定局部条件概率表；

进一步的，所述智能车辆威胁估计的步骤主要包括：

(1)采集威胁因素的实时数据，经模糊化处理后为贝叶斯网络模型各输入节点提供证据信息；

(2)根据贝叶斯网络的推理算法计算威胁指数节点的后验概率，即各目标对智能车辆的威胁指数。

进一步的，所述步骤3智能车辆行驶终点对智能车辆的引力势场模型为：

终点对智能车辆的引力势场模型定义为

$U_{at}\left(q\right)=\frac{1}{2}{\mathrm{\ξ\ρ}}^2(q,q_{goal})---\left(1\right)$

式中：ξ为位置增益系数；q为智能车辆中心位置[x,y]^T；q_goal为终点的位置[x_g,y_g]^T；ρ(q,q_goal)为智能车辆当前位置与终点位置的相对距离；

外部环境中的目标对智能车辆的斥力势场模型确定：

第i个外部环境中的目标对智能车辆的斥力势场模型定义为

$U_{reps}\left(q_i\right)=\frac{1}{2}\mathrm{\η}{(\frac{1}{\mathrm{\ρ}(q,q_i)}-\frac{1}{{\mathrm{\ρ}}_0})}^2\·\frac{p_i}{1-p_i}---\left(2\right)$

式中：i∈(1,2,…,m)，m为外部环境中的目标的总数；η是位置增益系数；q_i为第i个目标的位置[x_i,y_i]^T；ρ(q,q_i)是智能车辆与第i个目标之间的距离；ρ₀是目标的临界影响半径，即在目标ρ₀范围之外不会受到目标斥力影响；p_i是目标的威胁指数。

进一步的，所述智能车辆的综合势场模型为：智能车辆在q位置的综合势场模型定义为m个外部环境中的目标对智能车辆的斥力势场与智能车辆行驶终点对智能车辆的引力势场的叠加

$U\left(q\right)=U_{at}+\underset{i=1}{\overset{m}{\Σ}}U\left(q_i\right)---\left(3\right)$

进一步的，步骤4对综合势场模型求解采用梯度下降法或随机搜索方法。

进一步的，所述步骤5对原始局部最优路径进行以下优化处理：

(1)路径危险点排除；(2)路径缩短化处理；(3)路径平滑处理。

本发明的优点及有益效果如下：

本发明现有智能车辆路径规划方法忽略外部环境及其中的目标的多样性、动态性等诸多因素，建立在对环境的不充分的认知上，造成规划的路径不完全合理。为此本发明提出一种基于威胁估计的智能车辆路径规划方法，构造出包含了道路车辆、行人及障碍物等目标的类型、运动特征以及环境等多种因素的贝叶斯网络模型来评估目标对智能车辆的威胁，根据威胁值构造势场模型来进行路径规划。通过对目标的类型、运动特征以及环境等多种因素的考虑，本发明可以形成对智能车辆驾驶环境的更充分的认知，以规划出合理的路径，对智能车辆实现安全、高效的自动驾驶具有重要意义与使用价值。

附图说明

图1是本发明提供优选实施例基于威胁估计的智能车辆路径规划方法的总体框架；

图2本发明基于贝叶斯网络的智能车辆威胁建模与评估过程；

图3本发明基于贝叶斯网络的智能车辆威胁估计模型；

图4本发明智能车辆威胁估计模型的贝叶斯网络局部条件概率表确定流程。

具体实施方式

以下结合附图，对本发明作进一步说明：

本发明提出一种基于威胁估计的智能车辆路径规划方法，该方法利用贝叶斯网络建立能包含多种因素的智能车辆威胁估计模型来计算外部环境中的目标对智能车辆的威胁指数，利用该威胁指数建立势场模型，以规划出更有效的智能车辆行驶路径。

如图1所示为本发明提出的基于威胁估计的智能车辆路径规划方法的总体框架，包含五个步骤：

步骤一，进行全局路径规划，获得全局最优路径。

利用卫星导航、惯性导航装置或其组合等方式来实现智能车辆的定位和状态估计，并确定智能车辆行驶的终点，然后与地图数据库匹配，进行全局路径规划；可以采用路网地图模型实现环境构建，用A*最短路径搜索算法实现全局最优路径规划。

步骤二，建立基于贝叶斯网络的智能车辆威胁估计模型，计算外部环境中的目标对智能车辆的威胁指数，包括了智能车辆威胁建模与威胁估计两个方面。

智能车辆威胁建模主要包括：

(1)确定影响智能车辆威胁评估的因素，包括外部环境中的目标的类型与运动特征、环境参数等；

(2)根据上述影响因素，确定贝叶斯网络拓扑结构；

(3)分析各因素对智能车辆威胁的影响程度，确定局部条件概率表；

智能车辆威胁估计主要包括：

(1)采集威胁因素的实时数据，经模糊处理后为贝叶斯网络模型各输入节点提供证据信息；

(2)根据贝叶斯网络的推理算法计算威胁指数节点的后验概率，即各目标对智能车辆的威胁指数；

步骤三，建立基于威胁指数的外部环境中的目标对智能车辆的斥力势场模型，与智能车辆行驶终点对智能车辆的引力势场模型融合，得到智能车辆在当前位置的综合势场模型。

(1)智能车辆行驶终点对智能车辆的引力势场模型确定：

终点对智能车辆的引力势场模型定义为

$U_{at}\left(q\right)=\frac{1}{2}{\mathrm{\ξ\ρ}}^2(q,q_{goal})---\left(1\right)$

式中：ξ为位置增益系数；q为智能车辆中心位置[x,y]^T；q_goal为终点的位置[x_g,y_g]^T；ρ(q,q_goal)为智能车辆当前位置与终点位置的相对距离。

(2)外部环境中的目标对智能车辆的斥力势场模型确定：

第i个外部环境中的目标对智能车辆的斥力势场模型定义为

$U_{reps}\left(q_i\right)=\frac{1}{2}\mathrm{\η}{(\frac{1}{\mathrm{\ρ}(q,q_i)}-\frac{1}{{\mathrm{\ρ}}_0})}^2\·\frac{p_i}{1-p_i}---\left(2\right)$

式中：i∈(1,2,…,m)，m为外部环境中的目标的总数；η是位置增益系数；q_i为第i个目标的位置[x_i,y_i]^T；ρ(q,q_i)是智能车辆与第i个目标之间的距离；ρ₀是目标的临界影响半径，即在目标ρ₀范围之外不会受到目标斥力影响；p_i是目标的威胁指数。

(3)智能车辆在当前位置的综合势场模型确定：

智能车辆在q位置的综合势场模型定义为m个外部环境中的目标对智能车辆的斥力势场与智能车辆行驶终点对智能车辆的引力势场的叠加

$U\left(q\right)=U_{at}+\underset{i=1}{\overset{m}{\Σ}}U\left(q_i\right)---\left(3\right)$

步骤四，智能车辆综合势场模型求解，获得原始局部最优路径。

利用但不限于梯度下降法、随机搜索方法等求解智能车辆综合势场模型，来获得原始局部最优路径。

步骤五，对局部路径进行优化处理，得到最终规划的智能车辆行驶路径。

原始局部最优路径一般需进一步进行以下优化处理：

(1)路径危险点排除

部分路径点因为处于外部环境中的目标的威胁范围内而变成危险点，这种路径点需要排除。本发明建立的势场是一种以目标中心为圆点，以威胁指数为半径的圆形势场，因此可判定：当路径点位于圆内或圆上时则该点为危险点，并进行排除；当路径点位于圆外时则该点为安全点，保留不做处理。

(2)路径缩短化处理

局部极小点区域集中了许多路径点，并且原始路径中也有许多冗余的路径，会造成大量的行车浪费，因此采用路径遍历拉直的方法对路径进行缩短化处理：通过遍历原始路径的任意两点，当两点之间不与目标相交时，即可对此区间的路径进行拉直处理，否则继续遍历其他的两点，直至路径处理完成。

(3)路径平滑处理

经过缩短化处理后的路径由于存在过多的折线段，路径平滑度不够，导致行驶路径的平均转向角过大，考虑到车辆的运动特性和机械特性，依然无法直接应用于实际。因此可采用内切圆进行曲线拟合的方法，对路径进行平滑处理，以保证路径的平滑度，提供智能车辆行驶的平滑性。

如图2所示为本发明基于贝叶斯网络的智能车辆威胁评估过程，包括了智能车辆威胁建模与威胁估计两个阶段。智能车辆威胁建模阶段具体过程如下：

(1)威胁因素提取

本发明主要考虑智能车辆自身状况和驾驶环境两部分的因素。智能车辆自身状况因素主要包括车辆的速度、加速度、刹车片磨损、机油压力、胎压、水温等，可通过车身网络获取。驾驶环境主要分为自然环境和智能车辆周围的车辆、行人及其他障碍物构成的环境中的目标，自然环境因素包括路面状况、可见度、车辆密度等，目标因素包括目标的速度、与智能车辆距离等。

(2)网络拓扑结构建模

根据上述影响因素，确定贝叶斯网络拓扑结构。

(3)局部条件概率表确定

分析各因素对智能车辆威胁的影响程度，确定局部条件概率表。

智能车辆威胁估计阶段，对外部环境的每一目标，分别采用贝叶斯网络模型估计该智能车辆的威胁，具体过程如下：

(1)模型节点证据更新

根据采集到的威胁因素的实时数据，进行模糊化等处理，使其符合贝叶斯网络模型各输入节点数据类型要求，作为更新的各节点证据信息；

(2)网络推理

更新证据节点后，利用贝叶斯网络的推理算法实现整个网络节点的后验概率更新，直至威胁指数节点，威胁指数节点的后验概率即为目标对智能车辆的威胁指数。

贝叶斯网络的推理可采用但不限于Polytree精确推理算法。以Polytree精确推理算法为例，智能车辆威胁估计的具体推理步骤：

分别用X_f＝{x_f1,x_f2,…,x_fn}和X_s＝{x_s1,x_s2,…,x_sm}表示贝叶斯网络模型中任意节点x_i的父节点和子节点集，令是通过子节点集X_s传递到x_i的证据信息，是通过父节点X_f传递到x_i的证据信息。

第一步，节点初始化：

令π(x_i)和λ(x_i)分别为x_i的父节点子集X_f和子节点子集X_s传递的信息，s_i为x_i的节点状态取值。初始化威胁证据节点，使其满足：

$\begin{array}{ccc}\left\{\begin{array}{c}\mathrm{\λ}\left(x_i\right)=1\\ \mathrm{\π}\left(x_i\right)=1\end{array}\right.& if& s_i=e_i\end{array}---\left(4\right)$

$\begin{array}{ccc}\left\{\begin{array}{c}\mathrm{\λ}\left(x_i\right)=0\\ \mathrm{\π}\left(x_i\right)=0\end{array}\right.& if& s_i\≠e_i\end{array}---\left(5\right)$

对于所有非证据节点x_i，如果x_i没有子节点，则λ(x_i)＝1，如果x_i没有父节点，则π(x_i)＝p(s_i)。

第二步，后验概率更新：

如果节点x_i收到来自其父节点集X_f的数据，则π(x_i)为：

$\mathrm{\π}\left(x_i\right)=\underset{x_{f1},...x_{fn}}{\Σ}\[p\left(x\right|x_{f1},...,x_{fn})\underset{i=1}{\overset{n}{\Π}}\mathrm{\π}\left(x_{fi}\right)\]---\left(6\right)$

如果x_i收到其子节点集X_s传递的信息，则λ(x_i)为：

$\mathrm{\λ}\left(x_i\right)=\underset{j=1}{\overset{m}{\Π}}\mathrm{\λ}\left(x_{si}\right)---\left(7\right)$

而节点x_i传给其父节点x_f的信息为：

${\mathrm{\λ}}_{xi}\left(x_{fj}\right)=\underset{i}{\Σ}P\left(x_i\right|x_{fj})\mathrm{\λ}\left(x_i\right)---\left(8\right)$

x_i传给其子节点x_sj的信息为：

${\mathrm{\π}}_{xi}\left(x_{sj}\right)=\mathrm{\α}\mathrm{\π}\left(x_i\right)\underset{k\≠j}{\Π}\mathrm{\λ}\left(x_{sk}\right)---\left(9\right)$

其中为归一化常数。

然后计算各节点的后验概率分布，可得到节点x_i在威胁信息E下的后验概率：

$p\left(x_i\right|E)=\mathrm{\α}p\left(x_i\right|e_X^{+})p\left(e_X^{-}\right|x_i)=\mathrm{\α}\mathrm{\π}\left(x_i\right)\mathrm{\λ}\left(x_i\right)---\left(10\right)$

重复第二步，直到所有的证据信息的影响传遍整个贝叶斯网络，可得到威胁指数节点的后验概率，即目标对智能车辆的威胁指数。

如图3所示即为本发明基于贝叶斯网络的智能车辆威胁估计模型。通过分析威胁因素之间的关系，可确定智能车辆威胁估计模型的贝叶斯网络的拓扑结构。

定义威胁指数节点为Xa，定义威胁等级的影响因素集Xb为

Xb＝{相对车速(rv)，车距(l)，环境的影响(en)，目标威胁能力(tc)和智能车辆车况(vc)}

其中，相对车速越高，车距越小，环境越差，目标威胁能力越大，智能车辆车况越坏，则智能车辆所受威胁程度也越高。相对车速模糊化为高速(HS)、正常速度(NS)和低速(LS)，车距模糊化为危险距离(RD)、适中距离(MD)和安全距离(SD)，智能车辆车况可从仪表盘提供的胎压，刹车片磨损，机油压力，水温等方面警报进行评估，当无故障警告提示时车况为好(GVC)、有一项警告提示时为中(MVC)、大于一项警告提示时为差(BVC)。

定义威胁能力大小的影响因素集Xc：

Xc＝{目标类型(vt)，目标状态(vs)}

其中，目标类型模糊化为大型(BC)、中型(MC)和小型(LC)，目标状态取值为正加速(IA)，负加速(NA)。目标越大，对智能车辆的威胁能力越高；根据目标加速度方向，目标车辆处于正加速状态对智能车辆的威胁能力大于负加速状态。

定义环境影响因素集Xd：

Xd＝{可见度(vb)，路况(rc)，车辆密集度(vd)}

其中，可见度分为正常(MV)、低(NV)，路况分为干路面(DR)、湿路面(WR)和冰雪路(IR)，车辆密集度模糊化为高(HD)、正常(ND)和低(LD)。可见度越低，路面越湿滑或者行车环境中车辆密集度越大则智能车辆行车环境越恶劣。车辆密集度与智能车辆速度有关，当智能车辆速度很高时，危险距离随之变大，同时威胁目标数量的检测范围变大，反之则检测范围变小。

如图4所示为本发明智能车辆威胁估计模型的贝叶斯网络局部条件概率表确定流程。

贝叶斯网络模型构建好之后，需要确定每个节点的条件概率表。条件概率表的获得通过参数学习获得，具体步骤如下：

(1)初始值设定：根据经验或者任意地初始化条件概率表，并设计一组场景，根据已有文献和专家经验构造训练数据；

(2)威胁指数评估：选择一个场景，选取单个威胁因素作为变量，计算在该威胁因素影响下智能车辆的威胁指数；

(3)参数比较和修正：将步骤(2)中的威胁指数与训练数据进行对比，逐步调整对应的条件概率表，得到可信度较高的条件概率数据；

(4)在步骤(2)简单模型中逐步添加其他的威胁因素节点形成新的评估模型，重复步骤(2)、(3)，直至整个网络的条件概率得到修正。

以上这些实施例应理解为仅用于说明本发明而不用于限制本发明的保护范围。在阅读了本发明的记载的内容之后，技术人员可以对本发明作各种改动或修改，这些等效变化和修饰同样落入本发明权利要求所限定的范围。

Claims (6)

1.一种基于威胁估计的智能车辆路径规划方法，其特征在于，包括以下步骤：

步骤1、根据智能车辆当前位置及终点位置进行全局路径规划，获得全局最优路径；

步骤2、建立基于贝叶斯网络的智能车辆威胁估计模型，计算外部环境中的各目标对智能车辆的威胁指数；

步骤3、建立基于威胁指数的外部环境中的各目标对智能车辆的斥力势场模型，与智能车辆行驶终点对智能车辆的引力势场模型融合，得到智能车辆的综合势场模型；

步骤4、对步骤3建立的综合势场模型求解，搜索原始局部最优路径；

步骤5、对原始局部最优路径进行优化处理，得到智能车辆最终的行驶路径。

2.根据权利要求1所述的基于威胁估计的智能车辆路径规划方法，其特征在于，步骤2包括了智能车辆威胁建模步骤与威胁估计步骤。

3.根据权利要求2所述的基于威胁估计的智能车辆路径规划方法，其特征在于，智能车辆威胁建模的步骤主要包括：

(1)确定影响智能车辆威胁评估的因素，包括外部环境中的目标的类型与运动特征、环境参数；

(2)根据步骤(1)的上述影响因素，确定贝叶斯网络拓扑结构；

(3)分析各因素对智能车辆威胁的影响程度，确定局部条件概率表。

4.根据权利要求2所述的基于威胁估计的智能车辆路径规划方法，其特征在于，所述智能车辆威胁估计的步骤主要包括：

(1)采集威胁因素的实时数据，经模糊化后作为贝叶斯网络模型各输入节点提供证据信息；

(2)对环境中的每一目标，根据贝叶斯网络的推理算法计算威胁指数节点的后验概率，即各目标对智能车辆的威胁指数。

5.根据权利要求4所述的基于威胁估计的智能车辆路径规划方法，其特征在于，所述步骤3智能车辆行驶终点对智能车辆的引力势场模型定义为

式中：ξ为位置增益系数；q为智能车辆中心位置[x,y]^T；q_goal为终点的位置[x_g,y_g]^T；ρ(q,q_goal)为智能车辆当前位置与终点位置的相对距离；

外部环境中的目标对智能车辆的斥力势场模型是根据威胁指数来构建的，第i个外部环境中的目标对智能车辆的斥力势场模型定义为

式中：i∈(1,2,…,m)，m为外部环境中的目标的总数；η是位置增益系数；q_i为第i个目标的位置[x_i,y_i]^T；ρ(q,q_i)是智能车辆与第i个目标之间的距离；ρ₀是目标的临界影响半径，即在目标ρ₀范围之外不会受到目标斥力影响；p_i是目标的威胁指数。

6.根据权利要求5所述的基于威胁估计的智能车辆路径规划方法，其特征在于，所述智能车辆的综合势场模型为：智能车辆在q位置的综合势场模型定义为m个外部环境中的目标对智能车辆的斥力势场与智能车辆行驶终点对智能车辆的引力势场的叠加