紧急避撞工况下的路径规划与跟踪

周兵，万希†，吴晓建，陈晓龙，曾凡沂

（1.湖南大学汽车车身先进设计制造国家重点实验室，湖南长沙 410082；2.南昌大学机电工程学院，江西南昌 330031）

统计数据表明，人为驾驶不可避免地存在反应不及时、操控不精确、误操作等局限性，导致90%以上的交通事故与人为因素有关.智能车辆能够通过高效的环境感知、决策和精准的控制，有效地消除上述局限，提高行驶安全性.目前，智能车辆相关研究已成为热点.

局部路径规划是智能驾驶的基础和核心技术之一.根据规划的原理可以将其划分为3 大类[1]：基于特定函数的路径规划、基于搜索的路径规划和基于优化的路径规划.其中，基于特定函数的路径规划是一种采用梯形加速度曲线[2]、多项式[3-5]、B 样条曲线[6]等函数计算车辆避撞路径的方法.该方法具有计算高效、曲率变化连续平滑等优点，适用于本文研究的高速避撞这类极端工况.目前，围绕该方法已开展了大量研究工作.Shim 等[4]采用多项式函数建立6 次多项式方程，求解得出多项式系数，使路径满足起点和终点约束，但该方法未考虑车辆动力学约束；Gonzalez 等[6]采用贝塞尔曲线构造具有曲率连续的避撞路径，通过最大侧向加速度约束得出最大纵向速度，从而实现对纵向速度的规划；孙浩等[7]提出一种基于三次多项式曲线的最优换道轨迹，考虑了乘坐舒适性和安全性，但前提是车辆恒速行驶；陈成等[8]提出了一种由3 个参数确定的四阶贝塞尔曲线来规划路径，并考虑了加速度连续和加速度有界等约束，但忽略了纵侧向加速度耦合约束；金之熔等[9]使用B 样条曲线对Dijkstra 算法得到的最短路径进行平滑，并考虑时间最优，在规划路径的同时求解速度曲线，最终得到符合车辆动力学特性的最优运动路径；Suzuki等[10]将利用A*算法得到的B 样条节点进行平移生成曲率大于最小转弯半径的避障路径，并对控制点位置进行处理，以使车辆尽可能减少转向操作；Wu等[11]使用B 样条曲线对人工势场法规划结果进行平滑，实现了减小路径震荡的效果；Choi 等[12]在规划双臂机器人路径通过多次删选B 样条控制点以达到减少不必要路径抖动的效果；Wan 等[13]在B 样条曲线控制点连线平行方向增加控制点，使车辆能够通过指定路径点，从而更好地控制车辆可以通过狭窄通道.

学者们在基于特定函数的路径规划方法做了广泛而深入的探索，然而，仍有两个问题需要关注：1）为了保证生成路径的可行性，通常通过调整函数的参数值生成若干条备选路径，再通过目标函数从中选择.当确定轨迹函数的参数较多时，生成的备选轨迹会成倍增加，导致筛选耗时；2）目前，对该类方法的研究主要集中在中低速，且规划轨迹时考虑车辆约束不够全面，如缺少对纵向加速度的约束，以及对纵、侧向加速度耦合的约束等，而这些被忽略的约束在车辆高速避撞控制时往往至关重要.

针对以上问题，本文利用两段非均匀B 样条曲线构造出具有曲率连续、满足起（终）点的斜率与曲率约束的避撞路径模型，且其路径兼有梯形加速度曲线的优点，同时路径的形状仅由两个参数确定，减少备选轨迹的生成；而后通过构建乘员舒适性目标函数，添加车辆速度、加速度连续约束，防侧滑约束，纵、侧向加速度耦合约束等多约束，对生成的避撞路径簇进行筛选，最终完成车辆高速避撞路径规划和运动规划.

1 路径规划

1.1 非均匀B 样条曲线的介绍

1.1.1 非均匀B 样条曲线的递推式

相较于其它类型的B 样条曲线通过控制点和曲线次数改变曲线形状，非均匀B 样条曲线形状变化更加灵活，可以通过它的节点矢量加以改变.

k 次非均匀B 样条曲线定义为：

式中：{Pi}（Pi=（Xi，Yi））是决定样条曲线形状的控制点的位置坐标；{Ni，k（j）}是在非周期且非均匀节点矢量χ 上的k 次样条基函数.

式中的节点个数为m+1，非均匀B 样条曲线的次数k、控制点数（n+1）、节点数（m+1）之间的关系为：m=n+k+1.

k 次非均匀B 样条曲线基函数Ni，k（j）定义为在给定一个非递减的节点矢量χ=[j0，j1，…，jn]，有：

当式（3）中出现%的形式时，约定其比值为0.

一般地，路径规划采用3 次或3 次以上的非均匀B 样条函数，使生成的路径能够在整个区间具有连续的二阶导数，即具有连续的路径曲率.需要注意的是，增加样条函数次数可以提高曲线的自由度，但势必增加计算消耗.考虑到本文研究的高速紧急避撞工况对路径规划的实时性要求较高，在此，将采用3 次非均匀B 样条进行路径规划.

1.1.2 非均匀B 样条曲线控制点的确定

为使路径拥有较好的平滑性，文献[14]提出了一种非均匀B 样条曲线控制点确定方法.本文在该方法基础上，通过融入路径信息，进行路径规划.具体而言，如图1 所示，通过车辆的初始位置（X0，Y0，φ0）和目标位置（Xend，Yend，φend）确定两条相交的直线，构成过渡路径以配置样条曲线的控制点.为使路径起止点的方向与车辆方向相同，且保证从初始位置到目标位置之间的曲线曲率连续，本文选用6 个对称的控制点确定每段B 样条曲线，控制点的定位由式（4）确定.

式中：B 点为两直线的交点；a 和b 为两直线的单位向量；l1和l2为前、后转弯准备距离，其中，l1为车辆当前位置与交点B 的距离，l2为车辆目标点与交点B 的距离；τ 为控制点的定位参数.

当前、后转弯准备距离确定后，由参数τ 确定B样条曲线的控制点及对应的节点矢量χ.

图1 非均匀B 样条曲线控制点的确定Fig.1 Determination of control points of non-uniform B-spline curve

车辆的侧向加速度与道路曲率的关系满足ay=在车辆纵向速度变化不太大的情况下，应尽量使道路曲率的变化与车辆侧向加速度的变化趋势相同.本文选用的确定B 样条曲线控制点的方法能够达到这一目的.因车辆的加速度常采用梯形加速度模型，本文的B 样条曲线控制点，其曲率函数可表达为式（5），如图2 所示，根据式（5）得到的曲率变化曲线也呈现梯形.

图2 当l1=l2，τ=0.3 时，非均匀B 样条曲线的曲率变化Fig.2 The change of curvature of non-uniform B-spline curve when l1=l2 and τ=0.3

1.2 局部最优路径规划及速度规划

紧急避撞路径规划的工作原理可概括为图3.详细而言，为了使整段避撞路径的曲率呈梯形变化，路径采用两段B 样条曲线构成.同时，由于控制B 样条曲线的参数是可变的，因此可以生成一组路径簇.本文在避撞路径簇的基础上，进一步构建乘员舒适性目标函数，旨在满足避撞要求的情形下，选取产生一条舒适性最优的局部路径；与此同时，还将由目标函数得出车辆的最优纵向加速度.

图3 紧急避撞路径规划的框架Fig.3 The frame structure of path planning of emergency collision avoidance

1.2.1 场景信息的提取与紧急避撞路径模型的建立

构建如下紧急避撞场景：自车在平直的高速公路上以一恒定的车速行驶，突然发现前方道路存在障碍物，且在发现时刻车辆与障碍物的距离比较近，但周围无其他车辆，左邻车道能够正常行驶.自车可以考虑转向左邻道，进行紧急避撞.

该场景下，通过传感器可确定车辆的位置信息（X0，Y0，φ0）、障碍物的位置信息（Xend，Yend，φend）、障碍物的宽度、期望的安全距离Wobs、车宽Wv，如图4 所示.通过这些信息可以规划出图4 所示的两段虚线组成的过渡路径.为了简化计算，每段样条曲线的前、后转弯准备距离取相同值.通过式（6）可以得出两段样条曲线的转弯准备距离L1、L2与倾斜角θ 之间的关系.

图4 基于B 样条曲线的紧急避撞路径模型Fig.4 The model of emergency collision avoidance based on B-spline curve

式中：L1、L2为转弯准备距离；D=Sd+0.5Wv为车辆纵向轴线与障碍物边缘的最近距离，Sd为期望的安全距离，即期望的车辆与障碍物的最小距离.

1.2.2 基于非均匀B 样条曲线的路径簇生成

在得出两条B 样条曲线的转弯准备距离L1、L2与倾斜角θ 的关系后，随着倾斜角θ 的变化，可以得到多条过渡路径.图5 示出了τ=0.4，倾斜角分别为0.13 rad（路径1）和0.16 rad（路径2）时的过渡路径.与此同时，当确定转弯准备距离后，参数τ 可调整B样条曲线的控制点，从而控制B 样条曲线.图6 为倾斜角θ=0.14 rad，取τ 值分别为0.01 和0.30 时的过渡路径.

以上可知，每一条过渡路径可以规划出多条B样条曲线，由此确定一组避撞路径簇.

图5 不同倾斜角下的路径簇Fig.5 Path clusters with different inclinations

图6 不同控制τ 参数下的路径簇Fig.6 Path clusters with different control parameters of τ

1.2.3 路径规划中的多约束车辆模型

紧急工况下的车辆避撞控制需处理车辆稳定性和避撞安全性双重目标，可能同时涉及制动和转向两种操作.因此，在这两种操作同时发生时，为了能让车辆跟踪上轨迹，规划路径需要加入车辆模型和道路环境约束、车辆自身约束.

考虑到高速避撞对路径规划的实时性要求较高，车辆模型不宜太复杂，因而车辆模型采用点质量模型.车辆在t 时刻的状态量ξ 和控制量u 定义：

式中：x、vx和ax分别表示车辆纵向的位置、速度和加速度；y、vy和ay分别表示车辆侧向的位置、速度和加速度；jx、jy分别为纵、侧向加速度增量.

车辆的动态模型的方程如下：

将车辆点质量模型进行离散化处理，假设模型的控制量u 在离散时间间隔T 内是常量，从而得到离散车辆模型：

因此离散车辆点质量模型可表示为：

车辆的物理约束概括为：

式中：f 为任意值；jmin（m∈{x，y}，n∈{min，max}）为加速度变化增量，保证加速度变化连续.

1.2.4 路径的评估与优化

前已述及，根据车载传感器检测到的周围环境信息，采用非均匀B 样条曲线可以规划出一组避撞路径簇，在路径簇中抉择最优路径将成为多目标控制的关键.本文的路径优化思路为：设计目标函数，考虑式（11）建立的车辆物理约束，以纵向、侧向加速度的增量为点质量模型的输入，计算点质量模型沿路径簇行驶过程中的目标函数值，选择目标函数最小对应的路径即为最优路径，同时得出车辆在路径各位置点的最优纵向加速度值.在保证避撞安全性和车辆稳定性的条件下，本文提出以乘员的舒适为优化目标，使总加权加速度均方根值aw最小.设计乘员舒适性目标函数为：

离散化后的目标函数：

其中，w1、w2、w3为权重因子，目标函数的第一、二项为车辆整个行驶过程中的加速度大小的累加，其值关系到乘客的舒适度.函数的第三项是为了保证车辆与障碍物的距离，式中d 为障碍物与规划路径之间的最小距离.

需要注意的是，在目前的局部路径规划研究中，许多学者已将车辆约束加入到路径规划中，但很少把因路面附着产生的纵向、侧向作用力耦合效应加入到路径规划中.这对车辆低速跟踪路径影响甚微，但高速工况下，车辆中纵向力和侧向力存在此消彼长的耦合关系，在路径规划过程中不考虑这一约束，可能导致车辆无法准确地跟踪规划的路径，也可能导致路径跟踪过程中的车辆稳定性问题.本文将纵、侧向力耦合约束加入到路径规划中，且将纵、侧向力耦合约束转化为纵、侧向加速度耦合约束.

防侧滑约束：

纵、侧向加速度耦合约束：

式中：κ 为路径的曲率半径；μ 为路面附着系数，可通过状态估计得到；g 为重力加速度.

通过上述优化，可以选出最优路径和路径对应的最优纵向加速度.再由最优纵向加速度得出纵向期望速度.

1.3 一般环境下的避撞路径规划

对于曲线道路行驶时的避撞，避撞路径可利用一段非均匀B 样条曲线构成，如图7 所示.再结合1.2 节的规化方法，便可得到最优避撞路径.

需要说明的是，本文中，车辆在避撞过程中选用一段非均匀B 样条曲线还是两段非均匀B 样条曲线，取决于车辆即将实施避撞时刻的车辆姿态和车辆完成避撞时刻的姿态.即，取决于车辆即将实施避撞时刻的航向角φ0与车辆完成避撞时刻的航向角φend之差Δφ，当Δφ＜10°时，避撞路径由一段非均匀B 样条曲线构成，当Δφ≥10°时，避撞路径由两端段非均匀B 样条曲线构成.

图7 曲线道路的避撞路径规划Fig.7 Collision avoidance path planning for curve road

1.4 路径规划的仿真与分析

设车辆行驶于如下工况：车辆当前坐标位置为（0，0，0），车速V0=90 km/h，车宽wv=2 m，静止障碍物位置坐标为（40，0，0），障碍物的宽度wobs=2 m，车辆与障碍物的期望安全距离Sd=1 m.通过上述路径规划方法得出的避撞路径如图8 所示.

图8 过渡路径、最优避撞路径和次优路径Fig.8 The transition path，the optimal and suboptimal collision avoidance paths

图9 展示了最优、次优路径对应的纵、侧向加速度.实线为最优路径的纵、侧向加速度，虚线则为次优路径的纵、侧向加速度.最优路径的侧向加速度变化较快，但最大侧向加速度值小于次优路径的最大侧向加速度值.同时，最优路径的最大纵向加速度也小于次优路径的最大加速度.因此，从舒适性角度来说，最优路径的舒适性更好.

随后，通过最优路径的纵向加速度可得出纵向期望速度，如图10 所示.在该仿真工况中，由于车辆利用转向进行侧向避撞，轮胎的纵向制动力所需不大，故使车辆的纵向速度减小较少.图中，车辆的纵向速度由初始时刻的90 km/h 下降到完成避撞时的83.7 km/h，可以保持较高速度避障通过.

图9 最优的纵、侧向加速度和次优纵、侧向加速度Fig.9 The optimal longitudinal and lateral accelerations，and the suboptimal longitudinal and lateral accelerations

图10 纵向期望速度Fig.10 Desired longitudinal velocity

2 路径跟踪

路径规划后需通过路径跟踪完成避撞控制，本文路径跟踪控制的框架如图11 所示.由于高速避撞工况下，车辆的纵向动力学与侧向动力学存在复杂的耦合关系，车辆纵向动态对侧向动态产生影响，需要将车辆纵向控制器中输出的加速度传递到车辆侧向控制器中，得到更加准确的控制效果.

图11 路径跟踪的框架Fig.11 The frame structure of path tracking

车辆的纵向速度控制器采用PID 控制，侧向控制则采用模型预测控制方法.模型预测控制（model predictive control，MPC）方法是一种滚动优化控制方法，该方法能够系统地处理控制约束问题，非常适合用于车辆的轨迹跟踪控制.考虑到非线性模型的MPC 方法虽然控制效果好，但计算量大，实时性变差[15]，本文采用Falcone 等[16]提出的线性时变模型预测控制算法，将车辆模型在工作点进行局部线性化，可降低求解器的求解难度，加快求解器运算速度，同时保证模型的误差.

2.1 二自由度车辆模型

如图10 所示，纵向期望速度在避撞过程中变化很小，利用模型预测控制器控制车辆转向避撞时，在一个时长很短的预测时域内，车辆模型可采用纵向速度恒定的非线性二自由度模型，如图12 所示.

图12 二自由度车辆模型Fig.12 2-DOF vehicle model

车辆模型可以缩写为下式的微分方程：

2.2 线性时变模型预测控制

为了保证模型预测控制器能够实时地输出控制信号，需要简化模型预测控制器中使用的非线性车辆模型，降低控制器的求解计算量.先通过对非线性车辆模型进行离散化，得到非线性离散车辆模型.

再将非线性离散车辆模型在工作点（ξi，ut）处进行线性化处理后，得到线性时变车辆模型[17].

轨迹跟踪的优化问题可以表示为：

式中：ΔU（t）=[Δu（t|t），Δu（t+1|t），…，Δu（t+Nc-1|t）]T；Np和Nc分别为预测时域和控制时域；Q和R为权重矩阵；ξr为参考轨迹点.通过优化问题简化后可以重新表达为:

约束：MΔU≤N.

2.3 跟踪效果的仿真与分析

根据1.4 节所述工况下规划的避撞路径，采用Simulink-Carsim 联合进行路径跟踪仿真控制，车辆初试速度为90 km/h，路面附着系数为0.8，Carsim 中的部分车辆参数如表1 所示.

表1 部分仿真参数Tab.1 Partial simulation parameters

图13 为在1.4 节所述紧急避撞工况下，根据本文提出的方法规划出的期望速度和跟踪速度对比.图中，虚线为规划的期望速度，实线为车辆跟踪速度.可以发现：由于在路径规划过程中加入了加速度约束，车辆的期望速度没有出现突变；即使车辆在高速行驶，并且有大的转向时，车辆的速度变化仍能够跟上期望速度的变化.

图13 速度跟踪效果Fig.13 The result of velocity tracking

图14 为规划路径与跟踪路径的对比.总体上，车辆能够准确地跟踪规划的路径，由于控制器使用线性化处理的车辆模型，造成跟踪时出现小的偏差；其中，最大的侧向误差为0.185 m（高速时，此误差能够接受），车辆与障碍物的最小距离为0.353 m（考虑了自车的宽度），能够保证车辆避开障碍物.

图14 路径跟踪效果Fig.14 The result of path tracking

规划的车辆侧向加速度与跟踪侧向加速度的对比，如图15 所示.跟踪路径控制时的侧向加速度变化呈梯形加速度模型变化，其值与规划的侧向加速度比较接近，其中，后半段路径由于车辆的跟踪速度稍大于规划速度，造成跟踪的侧向加速度稍大于规划的侧向加速度.

图15 路径跟踪过程中侧向加速度的变化Fig.15 The change of lateral acceleration during path tracking

图16 和图17 为路径跟踪过程中的横摆角速度和质心侧偏角的变化.整个跟踪过程中，车辆的横摆角速度的绝对值小于16.34 deg/s，且质心侧偏角的绝对值小于1.55°，这表明车辆在路径跟踪过程中能够保持良好的稳定性.

图16 路径跟踪过程中横摆角速度的变化Fig.16 The change of yaw rate during path tracking

图17 路径跟踪过程中质心侧偏角的变化Fig.17 The change of sideslip angle during path tracking

综合以上分析，本文提出的多约束及多目标最优条件下紧急避撞路径规划方法，能够在保证避撞的前提下，尽可能的保证乘员的舒适性；同时，因在构造路径簇和选择最优路径时，充分考虑了车辆的动力学约束，可确保车辆能够良好地跟踪规划路径.

3 结论

1）提出了一种基于非均匀B 样条曲线构造紧急避撞路径的方法，充分利用传感器所得到的环境信息，得到一组少量且准确的避撞路径簇（备选路径）；在此基础上，利用构建的乘员舒适性目标函数和车辆纵、侧向加速度耦合约束等条件，从避撞路径簇中选取产生一条既能保证乘员舒适性又适合车辆高速跟踪的避撞路径.

2）Simulink-Carsim 联合进行路径跟踪控制结果表明：本文在路径规划时将车辆约束融入规划过程，能够确保车辆在高速紧急避撞时也能精确跟踪路径.