CN113720346B - 基于势能场和隐马尔可夫模型的车辆路径规划方法及系统 - Google Patents

Abstract

本发明属于汽车领域，涉及一种基于势能场和隐马尔可夫模型的车辆路径规划方法及系统；所述方法包括自主车辆进行环境感知采集障碍车辆信息和道路信息，并建立出道路势场模型和障碍车辆势场模型；根据障碍车辆的运动状态，利用隐马尔科夫模型进行识别观测，预测得到自主车辆在下一时刻的动作，并作出安全决策或避障决策；确定出障碍车辆横向的最小势场力点，结合换道决策进行路径规划；确定出障碍车辆纵向的最小势场力点，结合跟驰决策进行路径规划；本发明能够有效解决在考虑道路环境和自主车辆特性条件下的避撞问题和路径规划问题，通过HMM模型可使车辆规划出合理路径使车辆安全避障，有助于提高车辆避障效率与稳定性。

Description

基于势能场和隐马尔可夫模型的车辆路径规划方法及系统

技术领域

本发明涉及汽车路径领域，具体涉及一种基于势能场和隐马尔可夫模型的车辆路径规划方法及系统。

背景技术

随着汽车保有量不断攀升，城市拥堵、能源消耗、交通事故等问题日益突出，为了改善城市交通的运行状况，提高城市交通的运行效率，自动驾驶汽车已经成为汽车行业发展的趋势。

自动驾驶汽车使用主动与被动传感器(如光学雷达(Lidar)与毫米波雷达(Radar))持续做大范围的感测，具有360度视野。在行驶期间自动驾驶汽车需要根据车身与环境动态的参考信息作为系统输入，利用一些路径规划算法和控制算法来借此规划出车辆行驶轨迹。

国内外学者在自主车辆的路径规划和控制方法方面已经有了许多研究成果，常见的轨迹规划算法包括基于轨迹追踪的随机搜索法，基于特定函数的轨迹规划法，基于优化轨迹的模型预测法和人工势场法等。人工势场法作为较早的轨迹规划算法，通过对车辆以及目标点配置合理的势场函数，为物体规划出一条无碰撞的路径。由于其在安全阈值的动态建立和环境描述方面具有明显的优势，许多学者已将人工势场法用于智能车辆避障路径规划中。但此方法主要应用于自动驾驶汽车的运动规划和特定交通场景中的驾驶员行为建模，缺少对风险因素的考量，没有对复杂的路况以及对车辆-道路相互作用进行规划建模，导致现有的路径模型难以适应交通环境和车辆状态的相互作用和动态变化。

发明内容

有鉴于此，针对现有的避障路径规划方法未考虑到复杂多变的环境因素及车辆自身状态对自动驾驶车辆避障路径的影响，难以适应交通环境和车辆状态的相互作用和动态变化，本发明提供了一种基于势能场和HMM模型的车辆路径规划系统。本发明首先由车辆和道路交通要素引起的驾驶风险，建立了潜在的虚拟力场模型。在改善引力的同时，利用各点的势场力的大小进行路径规划。此外，HMM被用来进行路径决策，使所产生的路径在复杂情况下仍然遵守安全和道路法规原则。

在本发明的第一方面，本发明提供了一种基于势能场和隐马尔可夫模型的车辆路径规划方法，所述方法包括：

自主车辆使用主动与被动传感器进行环境感知，采集障碍车辆信息和道路信息；所述障碍车辆信息包括障碍车辆矢量，所述道路信息包括道路情况和道路规则；

根据道路中的障碍车辆矢量、道路情况和道路规则，建立出道路势场模型和障碍车辆势场模型；

根据道路中的障碍车辆矢量、道路情况和道路规则确定出障碍车辆的当前运动状态，利用隐马尔科夫模型进行识别观测，预测得到自主车辆在下一时刻的动作，自主车辆执行对应的安全决策或避障决策；

在安全决策下，自主车辆按照自由行驶进行路径规划；在避障决策下，对建立出的势场模型进行势场力分解，构建出势场力模型；根据障碍车辆的横向势场力大小，通过采样确定出横向的最小势场力点，自主车辆按照换道决策进行路径规划；根据障碍车辆的纵向势场力的大小，通过采样确定出纵向的最小势场力点，自主车辆按照跟驰决策进行路径规划。

在本发明的第二方面，本发明还提供了一种基于势能场和隐马尔可夫模型的车辆路径规划系统，所述系统包括：

采集模块，用于通过主动与被动传感器进行环境感知，采集障碍车辆信息和道路信息；

势场模型，用于根据道路中的障碍车辆矢量、道路情况和道路规则，建立出道路势场模型和障碍车辆势场模型；

隐马尔科夫模型决策模块，用于根据道路中的障碍车辆矢量、道路情况和道路规则确定出障碍车辆的当前运动状态，利用隐马尔科夫模型进行识别观测，预测得到自主车辆在下一时刻的动作，自主车辆执行对应的安全决策或避障决策；

势场力分解模块，用于对建立出的势场模型进行势场力分解，构建出势场力模型；

规划模块，用于在安全决策下，对自主车辆按照自由行驶进行路径规划；在避障决策下，根据障碍车辆的横向势场力大小，通过采样确定出横向的最小势场力点，并按照换道决策进行路径规划；根据障碍车辆的纵向势场力的大小，通过采样确定出纵向的最小势场力点，并按照跟驰决策进行路径规划。

本发明的有益效果：

本发明通过建立一个考虑道路环境和车辆特征的综合势场模型，能够预测出动态工况下的安全避撞路径点，实现动态工况下车辆自主避撞，并规划出避障后的路径；能够有效解决在考虑道路环境和自主车辆特性条件下的避撞问题和路径规划问题，通过HMM模型可使车辆在无法避障等复杂情况下依旧能够规划出合理路径使车辆安全避障，有助于提高车辆避障效率与稳定性。

附图说明

图1是本发明实施例中的车辆路径规划架构图；

图2是本发明实施例中的车辆路径规划流程图；

图3是本发明实施例中的车道势能场图；

图4是本发明实施例中的车辆势场图；

图5是本发明实施例中的隐马尔科夫模型图；

图6是本发明实施例中的动态工况下避障规划路径示意图；

图7是本发明实施例中的避障规划路径仿真图。

具体实施方式

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

图1是本发明实施例中的车辆路径规划架构图，如图1所示，所述路径规划的规程包括：

100、多源信息采集和处理；

在本实施例中，自主车辆需要完成多源信息的采集过程和处理过程，具体的：

101、环境感知；

自主车辆通过安装各类传感器来进行环境感知，所述传感器包括主动传感器和被动传感器，例如摄像头传感器、雷达传感器、激光雷达传感器等。通过这些传感器来感知自主车辆周围的环境信息。

102、障碍车辆和道路信息；

由于各类传感器采集到不同的环境信息，本实施例需要对采集到的环境信息进行处理，得到自主车辆周围的障碍车辆信息和自主车辆行驶过程中的道路信息，这里的障碍车辆信息包括自主车辆与障碍车辆之间的距离，障碍车辆的位置，障碍车辆的大小等；这里的道路信息包括自主车辆当前行驶车道的道路信息以及其他车道的信息，具体包括车道宽度、车道数量，车道直线距离和车道弯曲角度等等。

可以理解的是，所述障碍车辆也可以作为自主车辆，而所述自主车辆也可以作为障碍车辆。

200、交通状况评估；

在本实施例中，对于交通状况评估，主要是通过隐马尔科夫模型来确定出自主车辆的行驶状态；结合障碍车辆势场力模型和道路势场力模型来分析得出下一步的路径决策，具体的：

201、障碍车辆势场力模型和道路势场模型；

在本实施例中，需要分别建立出障碍车辆势场模型和道路势场模型，通过两个模型组合成综合势场模型。

202、势场力的分解；

在本实施例中，需要对上述综合势场模型进行势场力的分解，主要分解为横向势场力和纵向势场力。

203、隐马尔科夫模型；

在本实施例中，根据障碍车辆的运动状态，利用隐马尔科夫模型进行识别观测，预测得到自主车辆在下一时刻的动作；由于交通状态是一个连续的状态，因此本实施例中，根据综合势场力模型得到新的路径规划，这些规划出来的路径结果将作为下一时刻的动作状态继续作为隐马尔科夫模型的输入，预测出自主车辆在后续时刻的动作。

204、避障；

在本实施例中，根据隐马尔科夫模型计算得出自主车辆前方存在障碍车辆并且该障碍车辆影响该车辆自由行驶；因此自主车辆作出避障决策，并采用横向运动或者纵向运动，即自主车辆在下一时刻可以向左换道或者向右换道或者跟驰；根据具体的换道状态确定出避障状态。

205、安全；

在本实施例中，当检测出自主车辆前方障碍车辆不存在时或者该障碍车辆不影响车辆正常行驶，则可以保持自主车辆安全行驶。

300、路径决策和规划。

在本实施例中，在安全决策下，自主车辆按照自由行驶进行路径规划；而在避障决策下，则需要结合隐马尔科夫模型预测得到的状态和势场力分解得到的结果确定出具体的路径规划。即根据障碍车辆的横向势场力大小，通过采样确定出横向的最小势场力点，并按照换道决策进行路径规划；根据障碍车辆的纵向势场力的大小，通过采样确定出纵向的最小势场力点，并按照跟驰决策进行路径规划。

301、换道行驶

根据隐马尔科夫模型得到计算得出自主车辆处于避障状态中的可换道，结合横向势场力的最小势场力点，得到换道行驶后的规划路径。

302、保持跟驰状态

根据隐马尔科夫模型得到计算得出自主车辆处于避障状态中的不可换道，即没有其他车道可以更换，或者其他车道上也存在不可避开的障碍车辆，因此如果不满足换道，应该减速跟随前方车辆。

303、自由行驶

根据隐马尔科夫模型得到计算得出自主车辆处于安全状态，因此自主车辆可以自由行驶。

图2本发明实施例中的车辆路径规划流程图，如图2所示，所述车辆路径规划包括：

S1、自主车辆使用主动与被动传感器进行环境感知，采集障碍车辆信息和道路信息；所述障碍车辆信息包括障碍车辆矢量，所述道路信息包括道路情况和道路规则；

在本发明实施例中，自主车辆通过安装各类传感器来进行环境感知，所述传感器包括主动传感器和被动传感器，例如摄像头传感器、雷达传感器、激光雷达传感器等。通过这些传感器来感知自主车辆周围的环境信息。本实施例需要对采集到的环境信息进行处理，得到自主车辆周围的障碍车辆信息和自主车辆行驶过程中的道路信息，这里的障碍车辆信息包括障碍车辆矢量例如自主车辆与障碍车辆之间的距离，障碍车辆的位置，障碍车辆的大小等；这里的道路信息包括道路情况和道路规则；例如自主车辆当前行驶车道的道路信息以及其他车道的信息，具体还可以包括车道宽度、车道数量，车道直线距离和车道弯曲角度等等。

S2、根据道路中的障碍车辆矢量、道路情况和道路规则，建立出道路势场模型和障碍车辆势场模型；

在本发明实施例中，首先根据道路中物体的矢量以及道路的情况来建立出障碍车辆势场模型和道路势场模型，其中所述道路势场模型包括道路边界势场模型和车道势场模型。

所述道路边界势场模型E_road表示为：

E_road＝G_roadexp(-αy+y_e,j)

其中，G_road为道路边界势场比例因子，决定边界势场高度，根据车辆势场进行调节，要确保边界势场要远大于车辆势场，α为曲率因子，确定车道边缘势能上升/下降的速度以及车道边界线到车道边缘的宽度；y_e,j为第j条道路边界线的位置；

所述车道势场模型E_lane表示为：

其中，G_lane为车道标记类型，根据不同车道优先级确定；y_e,i为第i条车道划分的横向位置，σ_e确定车道势能上升/下降的速度以及车道能的宽度，根据车辆道路宽度确定。

图3是本发明实施例中的道路势能场图；由道路边界势场和车道势场组合而成。

在本发明实施例中，所述车辆势场模型可以表示如下：

其中，E_{K_q}表示车辆势场模型；G_e表示不确定常数，用于校正模型；M_e表示道路中物体e的等效质量，C_e表示物体所在位置处的道路条件因子；d_e＝(x-x_e，y-y_e)表示物体周围各位置(x，y)到物体所在位置(x_e，y_e)的距离矢量；λ₁和λ₂表示为势场的增益系数；v_e表示运动物体速度，θ表示物体速度方向与d_e形成的夹角，顺时针方向为正；K_e表示道路曲率因子，±表示车辆拐弯方向，+表示车辆拐弯方向为左，-表示车辆拐弯方向为右。

对于上述模型，道路条件因子C_e可表示为：

其中γ是能见度系数。μ是附着力系数。δ是道路坡度系数。此外，β₁和β₂为修正系数，根据该路段发生的事故数据进行修正。γ^*为能见度系数的标准值，μ^*为道路附着力系数的标准值。这两个参数是根据当地的道路状况确定的，一般数值为1。

等效质量M_e可表示为：

M_e＝(4.523+0.496Δv_e)m_eG_eH_e

其中m_e是物体e的质量，G_e是物体的类型，H_e是物体的结构形状，Δv_e为车辆速度与当前道路平均速度的标准差。

图4是本发明实施例中的车辆势场图，如图4所示，势能场中心是车辆的质心，越接近障碍车辆，势场能就越大，产生的势场力也就越大，符合实际车辆驾驶的风险特点。

S3、根据道路中的障碍车辆矢量、道路情况和道路规则确定出障碍车辆的当前运动状态，利用隐马尔科夫模型进行识别观测，预测得到自主车辆在下一时刻的动作，自主车辆执行对应的安全决策或避障决策；

在本实施例中，利用隐马尔科夫模型(HMM)观测车辆的运动状态，预测自主车辆下一步动作，自主车辆随后做出安全决策或避障决策；所述隐马尔科夫模型表示为：

其中，p(i|n)是当前时刻n自主车辆在运动状态i的概率；p(i|n+1)是下一时刻n+1自主车辆在运动状态i的概率；O(t_n)是观测变量矩阵，λ_ji表示是障碍车辆在运动状态j下，自主车辆为运动状态i对应的行驶状态转移概率矩阵；z_i表示自主车辆的观测概率分布矩阵；p(j|n)表示当前时刻n障碍车辆在运动状态j的概率；i,j∈{S₁,S₂,S₃,S₄}；S₁表示自由行驶状态；S₂表示右换道状态；S₃表示左换道状态；S₄表示跟驰状态，j表示障碍车辆的运动状态，这里的j可以包括加速状态、减速状态、匀速状态、换道状态等等，本实施例对此不作具体的限制。

根据所述隐马尔科夫模型计算得到自主车辆运动的避障决策系数，表示为：

其中，q_i表示自主车辆运动的避障决策系数，即q_i＝1表示自主车辆横向运动即作出换道决策，q_i＝0表示自主车辆纵向运动即作出跟驰决策；

表示自主车辆下一时刻n+1在运动状态i的概率矩阵，S_i表示自主车辆的行驶状态；m＝2,3表示横向运动，包括左换道和右换道。

可以理解的是，本实施例通过遍历自主车辆在所有运动状态，如果其中该概率矩阵中的概率最大值为横向运动对应的状态，则表明该车辆可以采用横向运动，如果该概率矩阵中的概率最大值没有横向运动对应的状态，则表明该车辆可以采用纵向运动。

图5是本发明实施例中的隐马尔科夫模型图，如图5所示，通过观测到障碍车辆状态来调整自主车辆在下一时刻的运动规划状态，自主车辆包括四种状态，包括{S₁,S₂,S₃,S₄}，这四种状态可以根据图5所示的状态转移图进行不同状态的转换。图中k_ij表示状态转换量，其中i,j＝1,2,3,4。

S4、对建立出的势场模型进行势场力分解，构建出势场力模型；

通过上述势场模型，将势场力分解为纵向引力，纵向斥力和横向斥力。

所述纵向引力为主车沿道路行驶的前端，所示纵向引力表达为：

F_a＝k₁|x_end-x_start|

其中，k₁为引力系数，x_end为主车前端，x_strat为道路起始端。

在本发明实施例中，所述纵向斥力表示为：

其中，F_max表示最大的横向势场力；q_i表示自主车辆运动的决策系数；d_max和d_min分别为势场力作用的最远距离和势场力最大值的作用距离；G_e表示不确定常数；C_e表示物体所在位置处的道路条件因子；W_e为道路条件因子；λ₁和λ₂表示为势场的增益系数；θ表示物体速度方向与d_e形成的夹角，顺时针方向为正；

为速度v与x轴的夹角；d_e＝(x-x_e，y-y_e)表示物体周围各位置(x，y)到物体所在位置(x_e，y_e)的距离矢量。

因此，所述纵向势场力F_V为纵向斥力与纵向引力之和，表示为：

F_V＝F_a+F_b

在实施例中，横向斥力包括模型在横向上产生的势场力，道路边界和车道势场模型在横向上产生的势场力，道路边界势场力为：

F_c1＝-αG_roadexp(-αy+y_e,j)

所述道路势场力为：

所述障碍车辆势场模型横向势场力为：

因此，所述横向势场力F_c为三个横向斥力之和，表示为：

F_c＝F_c1+F_c2+F_c3

S51、在安全决策下，自主车辆按照自由行驶进行路径规划；

在本发明实施例中，在安全决策下，即自主车辆前方没有车辆或者障碍车辆不影响车辆自主行驶，自主车辆可以按照自由行驶的方式进行路径规划。

S52、在避障决策下，根据障碍车辆的横向势场力大小，通过采样确定出横向的最小势场力点，自主车辆按照换道决策进行路径规划；根据障碍车辆的纵向势场力的大小，通过采样确定出纵向的最小势场力点，自主车辆按照跟驰决策进行路径规划。

在本发明实施例中，由于自主车辆是否能够执行换道主要取决于障碍车辆的横向势场力，在横向受力影响后，自主车辆会采取横向避障进入其他车道，其他车道的横向力会让该自主车辆保持在车道行驶。

在本发明实施例中，由于自主车辆在无法横向避障的情况下，需要通过纵向受力来规划出的路径使该自主车辆进行减速跟随，避免与前车发生碰撞，因此按照纵向势场力来确定出对应的路径规划。

可以理解的是，在本发明实施例中，无论是横向换道还是纵向跟驰，都需要对势场力进行采样，找出其中一系列的最小势场力点，这些最小势场力点所对应的物理位置即为安全避障点，通过对每一时刻安全避障点进行拟合形成的路径就可作为路径输出，用于给自主车辆提供对应的规划路径。

在一些实施例中，本发明还提供了一种基于势能场和隐马尔可夫模型的车辆路径规划系统，所述系统包括：

采集模块，用于通过主动与被动传感器进行环境感知，采集障碍车辆信息和道路信息；

所述采集模块安装在自主车辆上，并可以通过一些通信设备传输至服务器，由服务器来处理采集到的数据。所述采集模块可以包括主动传感器和被动传感器，例如摄像头传感器、雷达传感器、激光雷达传感器等。通过这些传感器来感知自主车辆周围的环境信息。本实施例中的服务器需要对采集到的环境信息进行处理，得到自主车辆周围的车辆信息和自主车辆行驶过程中的道路信息，这里的车辆信息包括自主车辆与障碍车辆之间的距离，障碍车辆的位置，障碍车辆的大小等；这里的道路信息包括自主车辆当前行驶车道的道路信息以及其他车道的信息，具体包括车道宽度、车道数量，车道直线距离和车道弯曲角度等等。

势场模型，用于势场模型，用于根据道路中的障碍车辆矢量、道路情况和道路规则，建立出道路势场模型和障碍车辆势场模型；

隐马尔科夫模型决策模块，用于根据道路中的障碍车辆矢量、道路情况和道路规则确定出障碍车辆的当前运动状态，利用隐马尔科夫模型进行识别观测，预测得到自主车辆在下一时刻的动作，自主车辆执行对应的安全决策或避障决策；

势场力分解模块，用于对建立出的势场模型进行势场力分解，构建出势场力模型；

规划模块，用于在安全决策下，对自主车辆按照自由行驶进行路径规划；在避障决策下，根据障碍车辆的横向势场力大小，通过采样确定出横向的最小势场力点，并按照换道决策进行路径规划；根据障碍车辆的纵向势场力的大小，通过采样确定出纵向的最小势场力点，并按照跟驰决策进行路径规划。

在一个具体的实施例中，本实施例采用如表1所示的势场模型参数，基于该模型参数可以得到车辆的路径规划效果。

表1势场模型参数

本发明提出的基于势能场和HMM模型的车辆路径规划方法和系统，所测试的案例如图6所示，对上述势场模型参数计算，可以得到如图7所示的仿真结果图，图7展示了本发明的避障规划路径图。

可以理解的是，本发明的基于势能场和隐马尔科夫模型的车辆路径规划方法和基于势能场和隐马尔科夫模型的车辆路径规划系统属于本发明的同一构思，其对应特征可以相互引用，本发明不再一一例举。

在本发明的描述中，需要理解的是，术语“同轴”、“底部”、“一端”、“顶部”、“中部”、“另一端”、“上”、“一侧”、“顶部”、“内”、“外”、“前部”、“中央”、“两端”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。

在本发明中，除非另有明确的规定和限定，术语“安装”、“设置”、“连接”、“固定”、“旋转”等术语应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或成一体；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通或两个元件的相互作用关系，除非另有明确的限定，对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本发明中的具体含义。

尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由所附权利要求及其等同物限定。

Claims (4)

1.一种基于势能场和隐马尔可夫模型的车辆路径规划方法，其特征在于，所述方法包括：

自主车辆使用主动与被动传感器进行环境感知，采集障碍车辆信息和道路信息；所述障碍车辆信息包括障碍车辆矢量，所述道路信息包括道路情况和道路规则；

根据道路中的障碍车辆矢量、道路情况和道路规则，建立出道路势场模型和障碍车辆势场模型；

所述道路势场模型包括道路边界势场模型和车道势场模型；

所述道路边界势场模型E_road表示为：

E_road＝G_roadexp(-αy+y_e,j)

其中，G_road为道路边界势场比例因子；α为曲率因子；y_e,j为第j条道路边界线的位置；

所述车道势场模型E_lane表示为：

其中，G_lane为车道标记类型；y_e,i为第i条车道划分的横向位置；σ_e表示车道势能因子；

所述障碍车辆势场模型E_{K_e}表示为：

其中，G_e表示不确定常数；M_e表示道路中物体e的等效质量；C_e表示物体所在位置处的道路条件因子；d_e＝(x-x_e，y-y_e)表示物体周围各位置(x，y)到物体所在位置(x_e，y_e)的距离矢量；λ₁和λ₂表示为障碍车辆势场的增益系数；v_e表示运动物体速度，θ表示物体速度方向与d_e形成的夹角，顺时针方向为正；K_e表示道路曲率因子，±表示车辆拐弯方向，+表示车辆拐弯方向为左，-表示车辆拐弯方向为右；

根据道路中的障碍车辆矢量、道路情况和道路规则确定出障碍车辆的当前运动状态，利用隐马尔科夫模型进行识别观测，预测得到自主车辆在下一时刻的动作，自主车辆执行对应的安全决策或避障决策；

在安全决策下，自主车辆按照自由行驶进行路径规划；在避障决策下，对建立出的势场模型进行势场力分解，构建出势场力模型；根据障碍车辆的横向势场力大小，通过采样确定出横向的最小势场力点，自主车辆按照换道决策进行路径规划；根据障碍车辆的纵向势场力的大小，通过采样确定出纵向的最小势场力点，自主车辆按照跟驰决策进行路径规划；

所述横向势场力表示为：

F_c＝F_c1+F_c2+F_c3

F_c1＝-αG_roadexp(-αy+y_e,j)

其中，F_c表示横向势场力，F_c1表示障碍车辆势场模型在横向上产生的势场力，F_c2表示道路边界模型在横向上产生的势场力，F_c3表示车道势场模型在横向上产生的势场力；F_max表示最大的横向势场力；q_i表示自主车辆运动的决策系数；d_max和d_min分别为势场力作用的最远距离和势场力最大值的作用距离；

为速度v与x轴的夹角；

所述纵向势场力为纵向斥力与纵向引力之和，表示为：

F_V＝F_a+F_b

F_a＝k₁|x_end-x_start|

其中，F_V表示纵向势场力，F_a表示纵向引力，F_b表示纵向斥力；k₁为引力系数，x_end为主车前端，x_strat为道路起始端；q_i表示自主车辆运动的决策系数。

2.根据权利要求1所述的一种基于势能场和隐马尔可夫模型的车辆路径规划方法，其特征在于，所述隐马尔科夫模型表示为：

其中，p(i|n)是当前时刻n自主车辆在运动状态i的概率；p(i|n+1)是下一时刻n+1自主车辆在运动状态i的概率；O(t_n)是观测变量矩阵，λ_ji表示是障碍车辆在运动状态j下，自主车辆为运动状态i对应的行驶状态转移概率矩阵；z_i表示自主车辆的观测概率分布矩阵；p(j|n)表示当前时刻n障碍车辆在运动状态j的概率；i∈{S₁,S₂,S₃,S₄}；S₁表示自由行驶状态；S₂表示右换道状态；S₃表示左换道状态；S₄表示跟驰状态，j表示障碍车辆的运动状态。

3.根据权利要求2所述的一种基于势能场和隐马尔可夫模型的车辆路径规划方法，其特征在于，所述在避障决策下还包括根据所述隐马尔科夫模型计算得到自主车辆运动的避障决策系数，表示为：

其中，q_i表示自主车辆运动的避障决策系数，即q_i＝1表示自主车辆横向运动即做出换道决策，q_i＝0表示自主车辆纵向运动即做出跟驰决策；

表示自主车辆下一时刻n+1在运动状态i的概率矩阵，m＝2,3表示横向运动，包括左换道和右换道。

4.一种基于势能场和隐马尔可夫模型的车辆路径规划系统，其用于实现如权利要求1～3任一所述的一种基于势能场和隐马尔可夫模型的车辆路径规划方法，其特征在于，所述系统包括：

采集模块，用于通过主动与被动传感器进行环境感知，采集障碍车辆信息和道路信息；

势场模型，用于根据道路中的障碍车辆矢量、道路情况和道路规则，建立出道路势场模型和障碍车辆势场模型；

隐马尔科夫模型决策模块，用于根据道路中的障碍车辆矢量、道路情况和道路规则确定出障碍车辆的当前运动状态，利用隐马尔科夫模型进行识别观测，预测得到自主车辆在下一时刻的动作，自主车辆执行对应的安全决策或避障决策；

势场力分解模块，用于对建立出的势场模型进行势场力分解，构建出势场力模型；

规划模块，用于在安全决策下，对自主车辆按照自由行驶进行路径规划；在避障决策下，根据障碍车辆的横向势场力大小，通过采样确定出横向的最小势场力点，并按照换道决策进行路径规划；根据障碍车辆的纵向势场力的大小，通过采样确定出纵向的最小势场力点，并按照跟驰决策进行路径规划。

Images