智能车辆弯道换道路径规划算法与跟踪控制

吴树凡，魏民祥，滕德成，汪 䶮，邢德鑫，任师通

（1.南京航空航天大学 能源与动力学院，南京 210016；2.东南大学 机械工程学院，南京 211189）

道路交通事故是所有国家都面临的问题。其中，每公里弯曲道路上发生的交通事故数量明显大于在直线道路上发生的交通事故数量［1-2］，而大多数的交通事故都是人为操作不当造成的。智能车辆能减少交通事故的发生，是当今的研究热点。国内外对于智能车辆直道换道的研究较多，但是对于弯道的研究较少，因此对于智能车辆弯道换道技术的研究具有重要意义。其中，如何快速准确地进行换道路径规划并有效跟踪期望路径，是实现智能车辆车道变换功能的基础，也是提高车辆主动安全性的关键技术之一［3］。

国内外存在大量的关于智能车辆车道变换的研究。在路径规划方面，先进的智能车辆路径规划算法有：A*算法、D*算法、Dijkstra算法、人工势场法等，其中，A*算法和D*算法常用于全局路径规划［4］，而Dijkstra算法、人工势场法用于局部路径规划时，虽规划速度较快但存在规划轨迹不理想且可能陷入局部最优解的问题。先进的智能车辆路径规划算法还有：蚁群算法、遗传算法、神经网络算法等，具有规划路径良好但计算量巨大难以满足实时性的问题［5］。传统的车道变换方法有：正弦法（车辆期望位置满足正弦函数）、圆弧法、多项式以及梯形加速度理论（期望侧向加速度能够满足正反梯形约束）。其中多项式规划具有简单、运算量小的特点，能满足高速行驶时的时间要求，且具有通用性和实用性强的优点，但是没有考虑车辆基于该路径行驶时是否满足车辆稳定性的问题，本文将考虑智能车辆在不同的车速下的路径规划以及稳定性的问题。蔡英凤等［6］采用可拓优度评价理论设计了一种横向轨迹跟踪控制系统，对基于预瞄偏差的PID反馈控制器和基于道路曲率的PID前馈-反馈控制器在不同工况下进行实时的选择，具有较好的效果，本文将基于可拓优度评价进行最优换道路径规划的研究。

路径跟踪控制算法主要有最优控制［7］、PID算法［8］、鲁棒控制［9］、模糊控制［10］、滑模控制［11］和模型预测控制［12］（MPC）等。基于MPC的控制器，由于考虑了车身自身结构特征的限制与车辆动力学参数的一些约束限制，以及拥有较强的处理多约束的能力被广泛使用［13］。胡均平等［14］考虑到在复杂路况下车辆避障会发生稳定性与避障的行为冲突，提出了一种车辆避障控制算法。该算法基于MPC并通过将路径跟踪、避障和稳定性三者集成在一起，设定优先级以协调三者的行为冲突，较为准确地表示出了车辆的避障路径并进行跟踪。任玥等［15］针对主动避撞问题提出一种分层避撞方法，路径规划层使用人工势场函数描述车辆碰撞风险并基于5次多项式规划局部避撞路径，路径跟踪根据MPC算法给出最优方向盘转角进行路径跟踪控制。本文中提出一种智能车辆的弯道换道系统，可用于弯道避撞路径规划，采用可拓优度评价理论进行弯道路径规划，考虑了道路利用率（换道距离）以及车辆稳定性的因素，再基于MPC算法设计路径跟踪控制器对最优换道路径进行跟踪控制。

1 车辆动力学建模

1.1 路径规划所需的车辆动力学模型

由于本文路径规划算法考虑了车辆在换道时的稳定性，需要考虑的量为侧向加速度、横摆角速度、质心侧偏角，因此基于图1建立车辆2自由度模型［16］：

式中：ωr为横摆角速度；a、b为质心至前轴和后轴距离；K1、K2为前轮和后轮的侧偏刚度；Iz为绕z轴的转动惯量；V为纵向车速；m为汽车总质量；δf为前轮转角；β为质心侧偏角。

图1 2自由度车辆模型示意图

1.2 路径跟踪所需的车辆动力学模型

考虑汽车纵向、侧向、横摆运动，建立如图2所示的车辆单轨模型，用于基于模型预测控制算法的路径跟踪控制器设计。车辆为前轮驱动和转向，忽略悬架与转向系统以及空气动力学的影响。

图2 车辆单轨模型示意图

基于图2，其中跟踪控制使用车辆动力学模型如下［17］：

式中：Fcf、Fcr为前后轮胎所受侧向力；Flf、Flr为前后轮胎所受纵向力；Fxf、Fxr为前后轮胎在x方向所受力；Fyf、Fyr为前后轮胎在y方向所受力。

假设车辆轮胎工作在线性区域，轮胎力用线性函数近似表示。

式中：Clf、Clr为前后轮胎的纵向刚度；sf、sr为前后轮胎的滑移率。

在式（2），假设车辆前轮转角和轮胎侧偏角较小，则

根据式（2）～（7）可得：

2 弯道换道路径规划

弯道换道系统结构框图如图3所示，主要分成4部分，第1部分为环境感知系统，得知车辆位置信息（X，Y）、道路曲率半径（R）、与前方障碍物距离（Lreal）、前方障碍物速度（vq）等信息。第2部分为决策层，在确定前方障碍物异常的情况下，在弯道左侧无障碍物的情况下，直接进行换道。假设第1、第2部分的信息已知，路径决策部分已作出换道指令。第3部分为路径规划层，得到最优轨迹的纵、横坐标以及横摆角速度。主要分为：上层路径生成器和下层路径选择器。其中上层路径生成器根据不同的横向距离由5次多项式生成一系列弯道换道路径，下层路径选择器包括车辆状态预估模型和可拓优度评价控制器。第4部分为路径跟踪控制器，输出前轮转角（δf）控制车辆跟踪规划的路径。

图3 弯道换道系统结构框图

2.1 道路模型

道路模型分为直线模型以及曲线模型两大类，在之前研究中的道路用曲线模型中的抛物线模型来描述道路［18］。本文考虑到抛物线模型无法正确表示后方区域的道路，且本文的研究对象为弯道工况，且在一段时间内弯道的曲率不会有很大的改变，因此在假设弯道曲率在一定时间内不变的前提下，采用圆弧方程对道路进行拟合。如下：

式中：x是车辆在大地坐标系下的纵向距离；R是当前道路曲率半径；ylane为车辆中心到车辆中心起始点的横向距离。

2.2 上层路径生成器

上层路径生成器，用于生成一系列路径，本文采用5次多项式进行换道路径生成［15］。图4为车辆弯道换道的位置变换示意图，以车辆换道初始时刻的质心位置向外车道做垂线，交点为坐标原点，以原点向行驶方向做外车道边缘切线，即为X轴正方向。

图4 换道位置变换示意图

假设：弯道在某一时间段内曲率不变，为R，车辆的纵向速度为定值V，路面附着系数为定值μ。

由式（9）以及图4，得外车道边缘：

设定车道宽度为dw，则车辆所在原路径公式为：

目标换道路径为：

其中车辆的换道轨迹可以设为：

由5次多项式换道规则，车辆在换道初始位置x＝0和结束位置x＝xd需满足如下方程［19］：

式中，Rz为车道线中心线的曲率半径。

由式（11）（12）（13）（14）推导可得：

由于纵向速度变化较小，此处，假设纵向速度恒定，得纵向换道距离xd＝V·t，因此得到换道轨迹：

2.3 下层路径选择器

本文中主要针对最优换道路径无法确定的问题，采用蔡文教授提出的可拓学理论［20］，提出了使用可拓优度评价选取最优路径的方法。可拓优度评价方法是可拓学中评价一个对象（事物、方法等）优劣的基本方法，根据实际情况制定出符合相关要求的评价标准，确定衡量指标，从而知道其利弊的过程与变化，最后可评价该对象的优劣。下层路径选择器利用可拓优度评价选择器来评价所有路径的优劣，从而选择出优度较高的路径输出值。

可拓优度评价选择器［6］的流程如图5所示。

图5 优度评价选择器流程框图

1）评价指标

想要评价一个特定对象的优劣，需要先确定一个评价指标。规定不同的评价指标会得到不同的结果，因此选择合适的评价指标才能得到合适的选择结果。为得到某评价指标下的最优换道路径，除了将换道路径纵向距离和侧向加速度作为参考指标外，将反映车辆稳定性的横摆角速度与质心侧偏角也同样作为影响指标。根据上层路径生成器生成的变道轨迹，纵向速度视为定值，计算得到理想的侧向加速度［21-22］，输入到车辆状态预估模型中，根据转向逆动力学模型得到方向盘转角（δsw），输入到二自由度车辆动力学模型中，得到对应的车辆状态预估值：横摆角速度（ωr）和质心侧偏角（β），加上轨迹原本已知的侧向加速度（ay）和换道距离（l），以该4个值作为衡量指标，第1项反映换道效率，第2、3、4项反映换道稳定性与安全性。

2）权系数

评价路径优劣的各个指标具有轻重之分，使用权值系数表示每个衡量指标的重要程度，分别给每个指标赋予0到1之间的值。权值系数记为：

3）关联函数

以车辆的质心侧偏角β为例，建立关联函数。选择特征量最优状态点均为原点S0＝（0，0）。质心侧偏角与最优点S0＝（0，0）的加权可拓距为：

质心侧偏角经典域界可拓距为：

质心侧偏角可拓域界可拓距为：

4）计算关联度

衡量指标为MI＝｛l，ay，wr，β｝，权值系数为w＝（w1，w2，w3，w4），根据各个衡量指标的要求，把不同纵向距离xd对应的换道路径关于各个衡量指标MIi的关联函数值简记为Ki＝（Sj），则各个对象关于MIi的关联度为：

将上述关联度进行规范化：

不同纵向距离对应的换道路径关于MIi的规范关联度为：

5）计算优度

对象y（xdj）关于各个衡量指标MI1、MI2、MI3和MI4的规范关联度为：

对象Zj的优度为：

6）选取优度较高方案

对于y（xdj）的优度进行比较，若：

则y（xd0）为较优路径，xd0便是限制条件下最优路径对应的换道纵向距离。

优度评价方法如表1所示。

表1 优度评价方法

其中横摆角速度和质心侧偏角的获取采用逆动力学模型［22］和式（1）的2自由度模型获得，其中转向逆动力学模型为：

式中：δsw为期望方向盘转角；i为转向系传动比。

依据文献［24］，稳定性基本约束为：

式中β为质心侧偏角。

3 路径跟踪控制器的设计与参数优化

3.1 路径跟踪控制器的设计

本文中基于MPC（模型预测控制）进行路径跟踪控制器设计［25］，由于使用线性时变模型进行模型预测控制具有计算简单、实时性好等优点，因此首先对式（8）进行线性化。

对式（30）采用1阶差分法进行离散化，采样时间为T，得到状态方程：

将离散状态变量x（k）与控制变量u（k）组合成新的状态变量：

即可得到新的状态空间方程：

式中：yd＝[ φdYd]由路径规划算法计算得到；Q和R为加权矩阵；Np和Nc为预测时域和控制时域。

基于动力学模型的路径跟踪控制器在每个控制周期需求解以下约束问题：

式中：yh为硬约束输出（不能放宽约束范围）；ys为软约束输出（可以通过松弛因子ε对约束范围进行调 整）；yh，min、yh，max为 硬约 束极 限 值；ys，min、ys，max为软约束极限值。

求解出k时刻的一组控制增量序列：

k时刻控制变量可表示成k-1时刻控制变量叠加k时刻的控制增量，即：

3.2 控制器的参数优化

通常MPC控制器主要是根据工程经验和试凑调整参数，属于寻优过程。本文为得到合适的预测步长Np和控制步长Nc，设计目标函数：

该目标函数第1项为实际换道横向距离Y与期望换道横向距离Yref的差值，第2项为实际横摆角φ与期望横摆角φref的差值，设计车辆在一段路径上行驶，得到最优的预测步长和控制步长。

4 仿真与分析

4.1 控制器参数和仿真工况

为验证弯道避障系统的可行性，使用Matlab／Simulink与Carsim联合仿真，搭建仿真系统结构，如图6所示。

图6 弯道避障系统仿真结构示意图

4.1.1 路径规划相关参数

假设自车纵向车速为V＝14 m／s，dw＝3.75，由于我国高速公路弯道设计标准，在平原和丘陵地带，高速公路的弯道最小曲率半径为650 m。此处选择R＝650 m进行最优轨迹规划分析。整车参数如表2所示。

表2 整车参数

设定式（17）中的权值系数为：w1＝0.25，w2＝0.25，w3＝0.25，w4＝0.25，即在选择最优路径时，各个因素视为相同权重，仿真计算得到车辆最优换道路径和最优换道纵向距离。

4.1.2 路径跟踪控制器参数

采用某型前轮驱动的轿车作为实验仿真车，路面附着系数为μ＝0.85，在弯道曲率半径为R＝650 m的单向双车道公路上行驶，由于弯道换道具有一定危险性，因此此处实验车速主要考虑中低速的情况，以速度为11、14、18 m／s验证弯道换道路径跟踪控制器的性能。仿真步长T＝0.02 s，V＝11、14、18 m／s时分别对应的仿真周期设为6.5、5、3.7 s。

路径跟踪控制算法相关参数如下：

此外，预测控制加权矩阵Q和R选定为：

控制器约束设定为：

4.1.3 路径跟踪仿真工况设计

图7所示为本次实验道路路径，用于验证路径跟踪控制算法的有效性。该路径主要分为2段，第1段为换道部分路径，换道路径来自路径规划控制层得到的最优换道路径，为1段5次多项式型曲线，第2段为换道结束后在内侧车道中心线上行驶的部分，为1段圆弧。

图7 弯道避障换道路径示意图

4.2 仿真结果分析

4.2.1 最优路径确定分析

图8为特定速度和道路曲率下生成的一系列换道路径（8、10、12、…、54 m）以及最优换道路径。由图8可知，在车速为14 m／s、弯道半径为650 m的工况下，最优换道距离为38 m。图9为不同纵向车速下的最优换道纵向距离，在弯道半径为650 m的工况下，车速与最优换道路径纵向距离基本成正比关系，根据最小二乘法，可得到特定曲率半径的弯道工况下，不同速度对应的纵向换道距离的关系式为：xd-best＝2.93V-3.33，从而知道特定速度所对应的最优换道路径。

图8 V＝14 m／s车辆换道路径与最优换道路径示意图

图9 不同纵向车速下的最优换道纵向距离

4.2.2 路径跟踪仿真分析

由图10、11可知，当速度为11、14、18 m／s时，最优纵向换道距离分别为29、38和49 m。在3种工况下，车辆能基本贴合两段组合路线行驶，跟踪效果较好，并且在两段路径节点能够平滑过渡，说明了路径规划层规划的合理性以及MPC路径跟踪控制器在不同速度下控制的稳定性。

图10 V＝14 m／s时的路径跟踪效果

图11 V＝11 m／s和V＝18 m／s时的路径跟踪效果

由图12可知每一时刻的路径跟踪误差，根据图中显示，该路径跟踪控制器在换道时表现出了较好的跟踪效果，尤其在前半段效果更加明显，误差基本保持在0.05 m以内。在5次多项式的后半段路径，误差低于0.3 m，在较高的车速下也能较好地跟踪路径，且在两段路径的连接点跟踪误差较小。

图12 3种工况下的路径跟踪误差

由图展示出该控制器的一个特点，较高的车速下拥有更小的跟踪误差，通常情况下，较高的车速和较低的车速跟踪同一条路径时，高速下的跟踪误差较大，此处变小是因为智能车辆以不同速度行驶时规划的最优换道路径不同，更高的速度下跟踪误差没有变差，也反映了路径规划器的优势。在跟踪圆弧路段时，路径跟踪误差较小且比较稳定，并维持在0.08 m以下。

由图13、14、15可知质心侧偏角最大值分别为1.2°、0.6°、0.4°（小于10°），横摆角速度最大值为13、12、11（°）／s（小于26.5（°）／s），横向加速度最大值为0.26g、0.27g、0.29g（小于0.4g），皆在稳定域内。当速度升高时，横向加速度最大值升高，显示出弯道换道时需要限制车速。质心侧偏角、横摆角速度以及侧向加速度在5次多项式换道路径部分，跟踪时稳定性会有较大的波动，而在圆弧路径部分，跟踪趋于稳定，且3个指标的数值趋于零，车辆稳定性较高。

图13 3种工况下的质心侧偏角

图14 3种工况下的横摆角速度

图15 3种工况下的横向加速度

由图16可知，MPC路径跟踪控制器输出的方向盘转角都在合理范围内，且在5次多项式路径部分，方向盘转角需要进行一定的调整，绝对值的最大值分别为66°、48°、38°，方向盘转动合理，在圆弧路径部分，方向盘转角进行细微调整后维持在一定值，稳定行驶，说明了MPC路径跟踪控制器的合理性。

图16 3种工况下的方向盘转角

5 结论

针对智能车辆弯道路径规划与跟踪控制的问题，提出了一种弯道换道系统，包括基于可拓优度评价的弯道换道路径规划算法与基于模型预测控制的路径跟踪控制方法。搭建了Carsim／Matlab联合仿真平台，对不同车速下的车辆避障路径规划与跟踪控制性能进行了仿真验证。仿真结果表明：该弯道换道系统权衡了换道纵向距离与车辆稳定性因素，能根据车辆当前状态（车速）和道路信息（弯道曲率）合理地规划出最优换道路径。MPC路径跟踪控制器考虑车辆动力学特性，能准确地预测车辆未来状态并优化求得最优方向盘转角，准确、稳定地跟踪换道路径，同时路径跟踪控制器的仿真效果也证明了路径规划部分规划出路径的合理性以及整个换道系统的合理性。

本文仅考虑了智能车辆匀速行驶条件下的弯道换道，未考虑车辆变速情况下的换道情况，下一步的工作将考虑车辆在前方障碍物不同的运动状态下，纵向速度实时改变的换道控制，实现车辆实时换道路径规划与变速路径跟踪控制器的设计。