基于势能场虚拟力的智能网联车辆运动规划*

田洪清，丁 峰，郑讯佳，黄荷叶，王建强

（清华大学，汽车安全与节能国家重点实验室，北京 100084）

前言

智能网联车辆运动规划算法能提高车辆行驶安全性、优化行车轨迹和速度、节约能耗并改善行驶舒适性［1］。根据车辆当前状态和周围环境中其他车辆状态信息，规划出安全、高效、可行的参考轨迹［2］。运动规划方法可分为图搜索法、随机采样法、几何曲线法、最优化法、仿生学习法和势能场法。

众多运动规划算法各有其优势和缺点。图搜索法中A*算法以起始点为中心向外搜索，以通过路径为代价值，以与终点直线距离作为启发值，使搜索沿着距离最短方向进行［3］，该算法能规划最优路径，但其增量式计算方法导致计算复杂度高［4］；快速随机搜索树算法用随机采样方式生成一个扩展树［5］，在扩展树中搜索规划路径，速度较快，但规划路径为局部最优轨迹；几何曲线法通过拟合逼近控制点来形成［6］，该方法平滑性好，能适应车辆动力学约束条件，但难以实时避障；最优控制法通过最优控制理论找到可行的最优控制量［7］，缺点在于难以准确描述最优轨迹的特征；人工势能场法将环境信息抽象为引力场和斥力场函数，通过场函数来规划从起始点到目标点的无碰撞路径［8-9］，具有规划速度快、动态避障效果好的优点。但传统的人工势能场方法忽略了动态障碍物的运动速度，存在轨迹振荡等问题［10-11］。

针对现有运动规划算法存在的问题，本文中提出基于势能场虚拟力模型的车辆运动规划方法，该方法通过获取自车与周边车辆的运动状态，建立纵横向势能场虚拟力模型，提出了基于车辆运动状态与势能场虚拟力相互循环迭代的运动规划方法。

1 势能场虚拟力模型

智能网联交通环境中，车辆能够获取自车与周边车辆的位置和速度等运动状态信息，周边车辆因其相对于自车的运动状态而形成势能场，车辆行驶过程中产生的加减速和换道运动可视为因势能场作用产生的虚拟力，称之为势能场虚拟力。车辆的纵向加减速和横向换道等行为即可等效为在势能场虚拟力作用下的运动。

本文中基于车辆相对位置、速度和驾驶意图，采用人工势能场方法生成纵向和横向势能场虚拟力，用于车辆纵向与横向规划。按其相互作用关系可分为引力与斥力；按其产生机理可分为位置虚拟力与速度虚拟力。在图1（a）所示的道路中，相同车速情况下，势能场虚拟力的大小和方向由车距决定；如图1（b）所示，当车辆间距相同而速度不同时，虚拟力由两车的速度差决定，说明势能场虚拟力与车辆位置和速度均有关。因此，在本文中提出的势能场虚拟力综合考虑位置与速度的影响，分为位置虚拟力与速度虚拟力。

图1 势能场虚拟力模型框架

其中位置虚拟力表征由道路上障碍物和车辆等物体与自车之间的相对位置而生成的势能场，对自车运动产生影响的位置虚拟作用力；速度虚拟力表征由道路上障碍物、车辆等物体与自车之间的相对运动速度而生成的势能场，对自车运动产生影响的速度虚拟作用力。二者合成道路行驶环境中虚拟合力∑F，如式（1）所示。

势能场虚拟力模型框架如图2所示。

图2 势能场虚拟力模型框架

图2框架表明，势能场虚拟力按运动方向可划分为纵向和横向虚拟力。纵向方向的引力和斥力分别对应车辆的加速和减速运动，横向方向的引力则对应车辆的换道或车道线保持运动。

建立势能场虚拟力模型，在纵向运动控制方面，车距变小时产生纵向斥力，车距变大时产生纵向引力，从而保持安全高效的跟车距离。在横向运动控制方面，当邻车道路符合换道条件时生成换道引力，车辆进行横向换道运动；当不符合换道条件时生成车道保持引力，使得其保持在自车车道内行驶。势能场虚拟力兼顾虚拟力的距离反馈和速度反馈，提高运动规划的稳定性，消除运动规划过程中的振荡。

1.1 纵向势能场虚拟力模型

1.1.1 纵向人工势能场

人工势能场法是用于动态环境机器人导航的一种经典方法。文献［12］中定义的人工势能场如式（2）所示。

式中：Up为基于位置的人工势能；r为障碍物与自车之间的距离；rmax为势能场作用的最远距离；rmin为势能场最大值作用距离；n为位置梯度指数，决定了势能场变化梯度。

在跟车场景下势能场分布如图3所示。

图3 跟车场景下人工势能场分布

传统人工势能场依据车辆与静态障碍物之间的相对位置建模。在动态环境中，为体现车辆速度在势能场建模中的作用，采用式（3）来表示速度势能场。

式中：Uv为基于速度的人工势能；vx为自车速度；vt为目标车辆速度；kv为速度势能系数；m为速度梯度指数。

1.1.2 纵向斥力模型

车辆在人工势能场环境中的运动规划与虚拟力相关［13］，势能场虚拟力与人工势能场之间的关系由式（4）描述［14］。纵向斥力又可分为纵向位置斥力和纵向速度斥力。

（1）纵向位置斥力

车辆在势能场环境运动过程中受到目标引力和障碍物斥力作用。根据车辆位置得到基于势能场的纵向位置斥力模型：

将式（5）所得到的纵向位置斥力Fp，通过式（6）所示的归一化方法进行处理后可得到标准纵向位置斥力Fr。

式中：Fr为经归一化处理的标准纵向位置斥力；kr为纵向位置斥力系数；Fpmax为斥力最大值；Fpmin为斥力最小值。

将纵向位置斥力Fp进行归一化处理，并转换到道路坐标系后，道路坐标系下的标准纵向位置斥力如式（7）所示。

式中：(xb，yb)为障碍车辆位置坐标；(x，y)为自车位置坐标；Frmax为标准纵向位置斥力最大值。

由式（7）可知，由势能场虚拟力方法定义的纵向位置斥力模型具备有界性和可控性，即纵向位置斥力值在[0，Frmax]范围，位置梯度指数对势能场虚拟力的影响如图4所示。

图4 位置梯度指数对势能场虚拟力的影响

图4中：s1点为安全距离rmin；s2点为纵向位置斥力的最大作用距离rmax。在运动规划中按障碍物特征与自车性能选取位置梯度指数n，当n=6.2 时，自车与前车距离较远时曲线斜率小，斥力增长慢，能提高行驶舒适性；距离近时曲线斜率大，斥力增加较快，能避免发生碰撞。梯度系数起到兼顾自车行驶舒适性与安全性的作用。

（2）纵向速度斥力

针对传统势能场方法在运动规划中的轨迹振荡问题，本文中提出了速度势能场概念，考虑车辆的运动速度特征，归一化处理后得到标准速度斥力模型如式（8）所示。

式中：Fv为车辆的速度斥力；Δvmax为车辆之间的最大速度差，大于此速度差则速度斥力为最大值，Fvmax=kv；Δvmin为最小速度差，小于此速度则斥力为0；kv为速度斥力系数；m为速度梯度指数。

对势能场虚拟力影响如图5所示。

图5 速度梯度指数对势能场虚拟力的影响

图中设定Δvmin= 2 m/s，Δvmax= 30 m/s，自车速度vx= 32 m/s 时，当速度vt∈[0，30 m/s]范围内，车辆间速度差小则曲线斜率小，斥力增长慢，车辆行驶舒适性好；速度差大则曲线斜率大，斥力增长快，车辆紧急制动减速避撞。同时，由于速度虚拟力的阻尼作用，能消除因车间距快速变化而产生的轨迹振荡问题。

1.1.3 纵向引力模型

当车间距大于理想跟车距离时，运动规划采用引力模型，纵向引力模型可分为位置和速度引力模型。

（1）纵向位置引力

设定车辆理想跟车距离为目标点与自车的距离，如图6 所示，目标点位置采用驾驶员主观安全距离模型来测算［15］。

图6 纵向位置引力与车距关系

图中前车处于A位置，自车处于C位置，目标点为B位置。理想跟车距离为da，实际跟车距离为d，纵向位置引力模型如式（9）所示。

式中：Fa为位置引力；xa为目标点位置坐标；dmax为引力最大作用距离，此范围外引力取Famax；dmin为最小作用距离，小于此距离则引力为0；ka为位置引力系数。

（2）纵向速度引力

与纵向斥力模型相类似，纵向引力模型中的速度引力为

式中：Fb为纵向速度引力；vb为前车速度；Fbmax为最大速度引力；kb为速度引力系数。

1.2 横向势能场虚拟力模型

针对车辆横向势能场建模难题，本文中设计了基于隐马尔可夫模型（hidden Markov model，HMM）的换道意图识别方法，建立横向势能场虚拟力模型。

1.2.1 横向人工势能场

横向人工势能场建模参数在车辆位置和速度基础上，增加了横向运动决策系数。建立的横向人工势能场模型如式（11）所示。

式中：UH为横向人工势能；yt为目标车道中心位置；hi为横向运动决策系数。

1.2.2 横向换道意图识别

HMM 方法从可观察参数中确定该过程中的隐含参数。本文中将HMM 用于车辆行驶过程中的车道线保持或换道行为的横向运动意图识别。

车辆保持车道线或换道行驶的行为意图取决于自车及周边车辆的位置和速度等因素。如图7 所示，当一定范围内的车辆速度与距离满足条件时，用HMM方法可以识别其横向运动意图。

将上述方法用于车辆横向运动规划，以自车道与邻车道的车辆位置、速度、车间距等特征参数为观测变量，将车辆横向运动状态等效为横向势能场，设为隐状态，通过HMM 方法识别车辆横向运动意图。用HMM方法由观测变量对隐变量进行预测，能准确地识别车辆横向运动意图，规划车辆横向运动，避免发生碰撞。

车辆行驶过程中按HMM 方法可将行驶状态分为右 换 道、左换道、跟 驰 和自 由行驶[s1，s2，s3，s4]4 种状态，以自车速度、前车速度、侧前车辆速度、侧后车辆速度、前方车距、侧后方车距、侧方车辆间距[vC，vB，vD，vA，da，dh，d]为观测变量，建立基于HMM的有限状态机模型如图8所示。

图8 基于HMM的有限状态机

图中，A∈[a11，a12，…，a44]为行驶状态转移概率矩阵，B∈[b1，b2，b3，b4]为观测概率分布矩阵，车辆行驶状态概率由式（12）描述。

式中：pn(i) 为当前时刻运动状态的概率；pn+1(i)为下一时刻状态概率；O(tn)为观测变量矩阵；Pn+1=[pn+1(1)，pn+1(2)，pn+1(3)，pn+1(4)]为横向运动状态输出概率矩阵。由式（12）可预测车辆横向运动状态，即车辆驾驶意图［16］，用式（13）将其转换为横向运动决策系数。

1.2.3 横向引力模型

（1）横向位置引力

设定目标车道中心线为引力目标点，建立横向引力模型：

式中：Fc为车辆横向位置引力；[kd，kd，kc，kc]为位置势能系数矩阵；[hl，hr，hf，he]为决策系数矩阵，取决于行驶状态输出矩阵Pn+1；yc为自车车道线位置；yd为目标车道线位置；dh为车道宽度。

由HMM模型得到车辆横向运动状态，该方法对车辆驾驶意图进行识别，识别过程中考虑周边车辆的位置和速度信息，横向运动规划时避免了换道碰撞风险。

（2）横向速度引力

为抑制横向运动中的振荡，模型中引入横向速度引力模型：

式中：Fd为横向速度引力；vy为横向运动速度；vdmax为横向最大速度，大于此速度时引力为Fdmax；kd为横向速度引力系数。

2 车辆运动规划方法

基于势能场虚拟力的车辆运动规划用当前时刻自车和周边车辆状态，按势能场虚拟力模型计算势能场纵向虚拟力FL和横向虚拟力FH，以虚拟力为输入，计算得到下一时刻车辆的运动状态为运动规划输出，并将该输出反馈到输入端，如图9 所示，通过循环迭代方法进行车辆运动规划。

图9 基于势能场虚拟力的车辆运动规划框架

2.1 纵向运动规划

由势能场虚拟力模型框架可知，车辆在行驶过程中在纵向上受到位置斥力、位置引力、速度斥力和速度引力4种虚拟力的综合作用。

式中FL为纵向虚拟力合力。

纵向运动规划采用双积分运动学模型。

式中：a为加速度；M为自车质量。

将系统进行离散化处理后建立车辆运动状态空间方程［17］：

其中：

式中：X(k)为车辆当前时刻的运动状态；X(k+ 1)为下一时刻运动状态；u(k)为控制输入；A为状态矩阵；B为控制矩阵；Ts为采样周期。

纵向运动规划过程中，根据当前时刻自车与前车的位置和速度，计算纵向虚拟力FL，结合当前时刻车辆运动状态X(k)，规划下一时刻运动状态X(k+ 1)。

2.2 横向运动规划

车辆横向上受到位置引力和速度引力的综合作用。

式中FH为横向虚拟力合力。

横向运动规划采用双积分运动学模型，如式（17）和式（18）所示。采用HMM 方法计算自车受到的横向虚拟力FH，结合当前时刻运动状态Y(k)，规划下一时刻车辆的运动状态Y(k+ 1)。

2.3 车辆平面运动规划

综合车辆纵向与横向运动规划，建立基于势能场虚拟力的车辆平面运动规划模型：

式中：ξ(k)为状态量；U(k)为控制量；At为车辆运动状态矩阵；Bt为车辆运动控制矩阵；Ct为车辆运动状态输出矩阵；η(k+ 1)为下一时刻运动状态输出。

3 仿真验证

为验证该方法可行性，本文中通过仿真分析，并与传统势能场规划方法以及highD［18］真实自然驾驶数据集运动轨迹进行对比。

车辆运动规划仿真环境如图10 所示，自车前方存在一慢速车辆B，为避免发生碰撞，通过构建势能场虚拟力模型进行运动规划。

图10 基于势能场虚拟力的运动规划仿真环境

3.1 振荡抑制验证

跟车状态下，按传统势能场模型和本文中所述的势能场虚拟力模型分别对车辆的运动状态进行规划，仿真工况如表1所示，仿真结果的车间距和车速对比如图11所示。由图可见，传统势能场方法会产生振荡，而采用势能场虚拟力规划方法，仿真数据经过小幅振荡后，车间距稳定于60 m，速度稳定于20 m/s。

表1 跟车运动规划仿真参数

图11 振荡抑制仿真对比

3.2 跟车行驶仿真

跟车状态下的仿真参数及数值如表1 所示，通过仿真实验获取车间距、自车速度和加速度结果如图12 所示。由图可见，运动速度和加速度平滑，当车速增加时，车辆减速度逐渐增加直到达到饱和值，车间距逐渐减小，并保持在安全范围内。

图12 跟车行驶工况

为验证该规划方法的实用性，将运动规划数据与德国亚琛大学采集的车辆highD 数据集［18］进行对比，在相同条件下进行运动仿真，结果如图13 所示。由图可见，仿真数据与实车数据的行车间距、车速和加速度有相同的变化趋势，该运动规划结果与实车运动基本一致，行车间距比实车略大，加速度值相对实车数据较小，车辆行驶安全性与平顺性更优。

3.3 换道行驶仿真

在换道行驶工况下，构建纵横向势能场虚拟力模型并进行运动规划，仿真参数及数值如表2 所示。

图13 跟车运动对比

表2 车辆换道运动规划仿真参数

换道状态下车辆的运动轨迹、速度和加速度如图14 所示。由图可见，基于势能场虚拟力的仿真运动规划轨迹平滑，符合车辆动力学特性，能避免与前车发生碰撞，满足安全性要求。

表3 列出了6 种车速条件下车辆运动规划仿真实验数据，当车速增加时，行车轨迹曲率、横向加速度和转角增加，车间距变小，但在安全范围之内。

图14 车辆换道行驶工况图

表3 运动规划仿真实验数据

为验证该方法的实用性，将换道仿真数据与highD中的实车运动轨迹进行对比，在相同条件下进行运动仿真，结果如图15 所示。由图可见，仿真数据与实车数据的运动轨迹、轨迹曲率、横向运动速度、车辆转角具有相同的变化趋势，该规划方法与实车运动基本一致，且仿真轨迹更平滑，曲率小，车辆横向运动速度低，规划安全性更优。

图15 highD实车换道运动对比图

4 结论

（1）建立了智能网联交通环境条件下的势能场虚拟力模型框架，提出了位置和速度虚拟力的概念，反映了封闭交通系统中车辆间相互作用关系，阐明了车辆运动规划的内在驱动机理。

（2）提出了基于车辆位置、速度和驾驶意图的横纵向势能场虚拟力模型，采用归一化方法将车辆行驶安全与效率融合为势能场虚拟力模型。揭示了车辆状态、势能、虚拟力、运动之间的相互作用关系。

（3）根据势能场虚拟力模型框架，建立了基于车辆位置与速度的双反馈闭环运动规划系统，运用车辆状态空间方程，采用势能场虚拟力与车辆状态循环迭代的方法进行运动规划，为车辆在结构化道路条件下运用势能场理论进行自动驾驶运动规划提供了新的研究思路。

（4）仿真实验结果表明，采用该方法规划的运动轨迹、速度等指标与实车运动数据基本一致，表明实际道路中车辆驾驶操控机制与势能场虚拟力运动规划机理基本一致；通过仿真对比实验，表明该方法能够克服传统势能场方法在结构化道路环境条件下进行运动规划过程中存在的速度振荡问题，具有较好的实用性。