张一鸣，周 兵，吴晓建，崔庆佳，柴 天

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

前言

根据国家统计局数据，我国2017年交通事故死亡63 772人，其中汽车交通事故死亡46 817人，占比73.4%且较往年呈现上升趋势［1］，因此，智能车主动安全系统受到越来越多人的关注。运动规划算法作为主动安全系统中重要一环，能够应对不同交通场景规划出合理轨迹且满足环境约束、车辆运动学约束和车辆动力学约束，从而减少或者避免事故的发生。

传统的运动规划更多考虑获取环境静态信息，通过不断感知更新规划路径。但在高速紧急避撞情况下，短时间内车辆状态会发生较大改变，此时前车的状态信息变化对主车规划轨迹影响较大，如果不能提前判断前车轨迹，仅根据前车静态环境信息更新主车规划轨迹会造成主车在紧急情况下过多调整行驶轨迹变化，难以完成有效避撞。Hu等［2］利用多项式方法生成路径簇，通过构建考虑动静态安全性和舒适性的代价函数选取最佳路径，但动态障碍物的运动模型考虑较为简单。Song等［3］结合势场法和弹性绳理论，能在多种道路环境下完成路径规划，并保证轨迹的平滑性，但是未考虑车辆在初始时刻航向角的变化过程。Han等［4］采用构造Bezier曲线方法通过调整控制点位置实现躲避障碍物。Chen等［5］在构造Bezier曲线时对控制点位置采用序列二次规划方法进行优化，规划得到曲率最优的运动路径，该方法能够保证生成满足车辆行驶的平滑轨迹，但是对于目标点位置的选取都未做进一步研究。Wang等［6］使用模型预测的方法构造引导势场和危险势场的代价函数，使用序列二次规划方法进行最优求解得到规划路径。黄建根等［7］提出基于D-S证据推理算法构造不同传感器的基本概率分配函数，该方法可以在每个预测周期内选择最佳的汽车运动模型，但是并不适合长期轨迹预测。Houenou等［8］结合基于最优化方法和恒角速度恒加速度运动模型的方法，对不同时刻两种预测方法的权重系数进行调整，达到对车辆短期和长期行驶轨迹的有效预测。周兵等［9］基于运动模型对交通车轨迹进行预测，并考虑运动轨迹不确定性对高速车辆采取制动避撞和转向避撞方式进行决策，该方法能够对未来碰撞概率进行判断，但在高速情况下单一运动模型预测方法会造成运动后期误差较大。Xie等［10］将基于模型和基于意图的预测轨迹不确定性进行融合，从而减小运动模型后期预测误差较大造成的影响。

可以看出现有文献中着重研究车辆的轨迹预测或者轨迹规划，但基于前车轨迹预测的主车轨迹规划关注不足，因此本文中在考虑前车未来一段时间内运动轨迹前提下，采用5阶Bezier曲线方法对高速车辆避撞进行轨迹规划。融合基于车辆运动模型和考虑驾驶意图对前车轨迹进行预测，并建立运动过程中碰撞安全距离模型，结合车辆运动学约束与动力学约束，采用SQP方法对Bezier曲线参数进行求解。目标函数考虑前车预测轨迹的不确定性，采用卡尔曼滤波方法求解轨迹概率密度分布函数，对运动过程中碰撞风险概率进行计算，并结合车辆运动过程中速度变化特点，规划出保证车辆安全性与稳定性的运动轨迹。

1 贝塞尔曲线

n阶Bezier曲线的参数化表达式为

式中：B（τ）为Bezier曲线点的坐标矩阵；Pi为第i+1个控制点的坐标矩阵。

图1为Bezier曲线示意图，X方向为道路纵向，Y方向为道路横向，P、P′和 P″分别表示 B、B1和 B2曲线的控制点，P0既为曲线B初始控制点，也为曲线B1、B2初始控制点，P5为曲线B、B2目标控制点，曲线B1目标控制点前移至P5′位置，从图中可以看出不同位置控制点对曲线形状的影响。

图1 Bezier曲线示意图

对于路径规划，Bezier曲线通过确定初始两个控制点和末尾两个控制点位置，可以控制初末时刻轨迹切线方向，且曲线经过第1个和第n+1个控制点，可以将轨迹导向目标位置，使曲线始终位于控制点构成的凸包内，保证轨迹方向变化趋势。另外Bezier曲线规划方法计算量小，能够满足高速工况下轨迹规划实时性要求，且曲率具有连续性，计算公式为

本文中选取5阶Bezier曲线构造车辆避障轨迹，即通过设置6个控制点位置规划出相应轨迹。结合车辆位置信息，将曲线控制点的坐标表示为

式中：P0和P5分别为初始位置和目标位置坐标，纵向坐标分别用y0和yT表示；d1和d4分别为P0点与P1点和 P4点与P5点之间距离；κ（0）和 κ（T）分别为初始时刻和结束时刻曲率；v0和vT分别为初始时刻与结束时刻速度方向矢量；i表示车道线方向单位矢量。

2 前车轨迹预测

2.1 基于运动模型的轨迹预测

运动模型主要有恒速模型（CV）、恒加速度模型（CA）、恒角速度模型（CTR）和恒角速度恒加速度模型（CTRA）。CV模型为加速度为0时的CA模型，CTR模型为加速度为0时的CTRA模型，因此本文中将上述4种模型简化为CA模型和CTRA模型。转向角速度对于高速行驶车辆影响较大，不同的运动模型预测轨迹会有较大的差别，因此在规划之初需要根据前车当前状态对运动模型进行合理决策。给定门限值ω0，当角速度大于ω0时选用CTRA模型，当角速度小于ω0时选用CA模型［11］。

采用运动学模型预测前车未来行驶轨迹时，高速车辆运动状态的不确定性会造成模型在预测过程中的偏差，加入高斯噪声以反映上述不确定性，从而得到预测轨迹的概率分布。利用扩展卡尔曼滤波对CA模型中车辆状态进行递归估计，将状态空间表示为 x（t）＝［x y vxvyaxay］T，其中 x、y分别为车辆在全局坐标系下X轴和Y轴位置，vx、vy分别为X轴和Y轴方向的速度分量，ax、ay分别为X轴和Y轴方向加速度分量。则t+1时刻的期望值x（t+1）和协方差矩阵 Σ（t+1）为

式中：A为状态转移矩阵；ωt为过程噪声矩阵；Q为过程噪声的协方差矩阵。

不同于CA模型，引入横摆角速度变量，CTRA模型中位置坐标为

可以看出CTRA模型是一个高度非线性模型，此时通过扩展卡尔曼滤波方法一方面会造成计算结果严重离散，另一方面计算过程也较为复杂。无损卡尔曼滤波（UKF）摒弃了对非线性函数进行线性化的传统做法，使用无迹变换来处理均值和协方差的非线性传递，因此选取UKF对轨迹进行预测，车辆未来轨迹预测不确定性结果如图2所示。

图2 预测轨迹不确定性

2.2 考虑驾驶意图的轨迹预测

考虑驾驶意图的轨迹预测首先要根据车辆信息判断出前车的驾驶意图，目前对于驾驶意图的判断方法主要有隐马尔科夫法［12］、支持向量机法［13］和神经网络［14］等，本文中侧重对前车轨迹进行预测，在此暂不对前车驾驶意图实时判断做深入讨论，而是将前车驾驶意图假定为车道保持和换道两种，将其对轨迹预测的影响加以考虑。换道轨迹在满足车辆动力学约束前提下还需保证轨迹的平顺性和连续性以及保证车辆换道效率，为了得到最优轨迹，给定代价函数：

式中：θi（t-1）和 θi（t）分别为车辆前后时刻的航向角；si为轨迹长度；i为备选轨迹路径序号；w1和w2分别为评价车辆平顺性和换道效率部分的系数。

相同运动距离条件下，车辆航向角变化值累加量越大，则车辆平顺性越差，而运动距离变大则会增加车辆平顺性，但同时降低了车辆避撞通过效率。式（12）中不同的权重代表不同的驾驶员风格，本文中在此不做过多研究，只讨论通过该方法对长期轨迹进行预测。通过代价函数最小化，优化得到考虑前车驾驶意图的局部最优预测轨迹。

2.3 轨迹预测融合

假定基于运动模型的预测轨迹为Tmdl，考虑驾驶意图的预测轨迹为Tman，基于模型的预测轨迹能够反映车辆短时间内运动趋势，考虑意图的轨迹预测方法可以反映长期预测轨迹，因此构造两种预测方法随时间变化的权重函数，权重函数采用三次Bezier曲线形式表示，设置0～1.5 s内Tmdl权重值较高，1.5～4 s内 Tman权重值较高，权重函数 f（t）如图3所示。

图3 权重函数

加权融合后的预测轨迹为

3 碰撞概率计算

将预测轨迹离散化，定义主车与前车轨迹分别为 xSE（xse，yse，θse）和 xTR（xtr，ytr，θtr），如图4所示，分别对两车纵向距离与侧向距离进行计算。

图4 安全距离模型示意图

不发生碰撞的安全条件为

式中：l和w分别为车长和车宽；ls和ws分别为纵向和侧向安全距离。

当车辆间距小于该安全距离时，判定车辆处于危险范围，不再考虑轨迹不确定性；大于该安全距离时，需要考虑不确定性进一步计算碰撞概率。采用蒙特卡洛法求解近似概率，将某一时刻前车状态变量根据概率密度函数离散采样，通过以下公式计算碰撞概率P（Ct）期望值：

当 IC（xSE，xTR）＝1时，说明主车和前车几何图形所占空间存在交集，即发生碰撞；当IC（xSE，xTR）＝0时，表示不会发生碰撞。

4 主车轨迹规划

考虑到前车未来的运动轨迹，若主车目标点在道路纵向位置距离较大，在运动过程中会增大与前车碰撞概率，而目标点在道路纵向位置距离较小时减小了碰撞概率，但是会增大车辆侧向加速度，极易造成车辆侧向失稳。图5所示为P5点在Y方向距离P0点3.75 m时，前车保持直道前行时目标点位置对于车辆最大加速度和碰撞概率影响，此时侧向加速度绝对值变化不大，随着P5点在X方向坐标位置变大，纵向加速度绝对值减小较为明显。图6和图7为P5点在X方向距离P0点72 m，道路纵向坐标位置52 m时横向距离对侧向加速度绝对值和纵向加速度绝对值与碰撞概率的影响。可以看出若目标点在道路横向位置距离P0点较大，此时侧向加速度受影响变化较为明显，容易造成高速行驶车辆侧滑失稳，在碰撞概率较小的安全裕度内，可以通过适当减小目标点纵向位置，既保证车辆稳定，又保证车辆不发生碰撞。

图5 P0点纵向位置对加速度与最大碰撞概率影响

图6 P0点横向位置对侧向加速度与最大碰撞概率影响

图7 P0点横向位置对纵向加速度与最大碰撞概率影响

考虑在高速紧急避撞过程中，车辆进行制动操作不可避免，而避撞规划的目的又是在车辆尽可能减少速度变化的同时保证安全，这样既可以提高车辆运动效率，又可以减小车辆纵向加速度，提高车辆稳定性，基于此构造关于车辆运动速度的目标函数，采用序列二次规划方法对参数 P5、d4和x3进行求解。

车辆侧向加速度ay常用来表征侧向稳定性，假定预期最大侧向加速度值为aymax，则其表达式为

ay＝vt2κ≤aymax（18）式中 aymax＝min（μg，ayuser），ayuser为驾驶员期望最大侧向加速度值。

主车在变道过程中速度发生变化，纵向及横向加速度受到轮胎和地面摩擦力的限制：

式中：Ft为轮胎力；m为车辆质量；ax为车辆纵向加速度；μ为地面附着率；axuser为驾驶员期望最大纵向加速度。

根据实际驾驶经验，在高速紧急避撞工况下，如果车辆发现有碰撞风险，会最大程度采取制动操作，在短时间内充分利用地面附着力进行避撞，躲避危险之后降低减速度，使得车辆快速通过危险区域。本文中采取两种避撞策略，主车与前车行驶意图不同时，主车会在避撞前半段采取较大制动减速度，后半段降低减速度；主车与前车行驶意图相同时，主车全程采取较大制动减速度来避免发生碰撞。另外考虑前车运动轨迹的不确定性，在可行目标点区域内对车辆避障过程中的碰撞概率进行计算，并且车辆的横纵向加速度大小影响到乘客舒适性与稳定性。对规划轨迹在后半段均匀选取n个点，给出求解的目标函数形式为式中：i为n个采用点序号；w1为速度规划权值；w2为最大纵向加速度alon权值；w3为避撞过程中最大碰撞概率Pcom的权值。

I＝0表示主车与前车直行或换道意图不同，此时 t∈［0，T／2］，若意图相同，则 t∈［T／2，T］。对该非线性优化问题采用序列二次规划方法进行求解，求解步骤如下［15］：

（1）初始化Hessian矩阵以及变量x0；

（2）利用主动集法求解子二次规划获得求解搜寻方向dk；

（3）通过 xk+1＝xk+αkdk获取下一个迭代值xk+1，其中αk需要保证目标函数能充分减小，重复直至Δφ（xk）＜τ；

（4）采用拟牛顿法更新Hessian矩阵。

5 仿真分析

道路场景选取单向双车道，针对高速行驶车辆面临前方障碍车辆变道和直行两种典型工况，采用Matlab对本文中提出方法进行仿真验证。

5.1 前车直行工况

主车车速为 30 m／s，前车速度为 20 m／s，两车距离为20 m，前车加速度为0，横摆角速度为0，因此运动模型选用CA运动模型，根据对前车未来的预测轨迹规划出主车避障运动轨迹如8所示。

图8 主车避撞轨迹规划

仿真前车直行工况下，主车若继续减速直行，从图9看出，在23 m之后碰撞概率开始上升并在26 m处达到100%，可以肯定会与前车发生碰撞，此时选择换道避撞方式降低了碰撞风险。如图10所示，主车在换道躲避碰撞过程中速度持续减小，换道前期纵向加速度绝对值持续增大，后期开始减小，从而保证安全前提下快速通过危险区域，侧向加速度绝对值和纵向加速度绝对值波峰错开，分配地面附着力以提高车辆稳定性，仅在车道纵向位置27 m处有最大2%的碰撞概率，保证了车辆通过安全性。

图9 主车直行与换道碰撞概率

图10 主车换道加速度与速度规划

5.2 前车变道工况

主车车速为 30 m／s，前车速度为 20 m／s，两车距离为20 m，前车加速度为0.85 m／s2，横摆角速度为0.04 rad／s，此时车辆运动模型选用CTRA模型。从图11中可以看出，基于运动模型的方法在短距离0～18 m内能够较好接近实际轨迹，考虑驾驶意图的方法则能够较准确地预测长期运动轨迹，融合两种方法能够较准确地预测前车未来运动轨迹，图12为主车轨迹规划结果。

图11 前车轨迹预测

图12 主车避撞轨迹规划

若此时主车采取换道方式，全程采取较大制动减速度，从图13看出在道路纵向位置23 m处碰撞概率达到100%。而通过提前预判前车未来运动轨迹，如图14所示，前半段制动减速度开始上升，当躲避前车后降低制动减速度，快速脱离危险区域，提高了车辆通过效率，碰撞概率仅在道路纵向位置38 m处达到4%，保证了车辆安全性，制动加速度在允许范围之内，避免了转向产生的侧向加速度，从而在侧向具有稳定性。

图13 主车换道与直行碰撞概率

图14 主车直行加速度与速度规划

6 结论

考虑到高速情况下主车执行避撞动作受前车状态变化影响较大，提出基于前车轨迹预测的主车运动规划算法。构造目标函数包括主车规划轨迹与前车在运动过程中碰撞概率以及主车在运动过程中加速度变化特点，并结合车辆动力学与运动学约束，采用SQP方法对Bezier曲线控制点位置进行求解，能够规划出主车最优避撞运动轨迹。使用Matlab仿真前车直行和换道两种意图下主车避撞运动规划，结果验证了基于前车轨迹预测的主车运动规划方法的可行性与有效性。