考虑路径平滑性和避撞稳定性的智能汽车弯道轨迹规划研究*

余嘉星，Arab Aliasghar，裴晓飞，过学迅

（1. 湖北省汽车零部件重点实验室，武汉理工大学，武汉 430070；2. 罗格斯大学机械与航空航天工程系，新泽西，皮斯卡塔韦 08854 美国）

前言

智能汽车通常采用包含感知、规划和控制的分层架构，其中规划层在安全性和舒适性方面起着承上启下的作用。当前智能汽车已经在低速、简单道路的封闭场景下实现落地，但是在复杂的高速场景下，由于规划问题导致的弯道行驶性能和类人性还有所欠缺。

通常轨迹规划分为解耦的路径规划和速度规划两个部分，常用的路径规划方法包括曲线拟合法、图搜索法、采样搜索法、势场法和基于优化的方法等。其中，曲线拟合法仅能表现有限几何的特征，缺少普适性。图搜索法如A*和Dijkstra 在动态环境中每个步长都要重新构建地图，从而造成过高的计算量。基于优化的轨迹规划法例如模型预测控制，必须将障碍物约束转化为模型约束，较难处理多障碍物。采样搜索法例如RRT，可快速搜索环境，构建无碰撞的路径，但路径不光滑，难以保证计算实时性要求。文献［5］中采用Dubin 曲线扩展RRT 树，再用三次样条平滑Dubin 曲线，提高路径平滑度。文献［6］中采用RRT搜索粗糙路径，再调用二次规划（QP）数字优化路径，以达到车辆模型约束要求，但是往往耗时过长。文献［7］中采用双树结构，加快第二个RRT 构建速度，返回C1 连续性的曲线，只能实现车辆低速避障。文献［8］中通过驾驶行为引导、高斯采样的方式，提高RRT 计算速度，返回曲率连续的路径，但其速度规划未考虑RRT 路径的曲率，忽略了弯道曲率对车速的影响，因此无法保证智能汽车的高速过弯性能。

速度规划对弯道行车和避撞性能也尤为重要。文献［5］中采用路径规划和速度规划解耦的方法，使用优化的方法基于路径求解速度规划曲线，这样更利于运用不同的方法限制约束量。百度阿波罗（Apollo）的速度规划分为两步，首先通过动态规划得到粗糙的参考速度曲线，然后将纵向加速度及其导数作为二次规划的代价函数光滑速度曲线，提高驾驶舒适性。文献［8］和文献［9］中设计了梯形速度规划方法，分别考虑动态障碍物的影响和车辆运动学要求。但无法保证车辆在复杂工况下的操纵稳定性。而文献［10］中的研究结果表明，在高速过弯时PGVC 能充分利用道路曲率和自车动态信息，提高车辆对弯道的跟踪能力。

针对现有文献在处理弯道避撞问题上的不足，本文中提出了一种考虑规划曲线平滑度和车辆稳定性的轨迹规划方法。在路径规划中构建满足车辆运动学模型的C2连续性曲线，同时利用并行计算提高RRT 计算效率。在速度规划中考虑自车车速、侧向加速度和规划路径的曲率，通过联合速度梯形和PGVC 弥补了文献［10］中PGVC 未考虑最高限速的问题。因此，本文首先给出了轨迹规划的总体框架，然后分别具体介绍RRT 路径规划和速度规划方法，最后通过仿真验证所提出方法的有效性。

1 总体规划框架

如图1 所示，轨迹规划方案采用了纵横向解耦的轨迹规划方式，从而更加灵活地给出速度规划的限制条件。全局路径不考虑是否有障碍物在车道内，并将其作为参考路径。局部路径规划RRT 在参考路径的车道和旁车道内采样，采样点与预估代价值最小的节点连接。然后在节点附近寻找是否有代价值更小的节点，优化预估代价值最小节点的路径，最后输出包括坐标点位置、横摆角、曲率和精确的路径长度的螺旋线路径。原始的速度规划采用路径上节点之间的线段距离总和，估计梯形速度规划的加速和减速时长，而通过精确的路径长度可得到精确的加速和减速时长。速度规划首先判断车辆应处于的加速、减速或匀速状态，生成速度梯形。然后用三次多项式优化速度梯形得到连续的纵向加速度曲线，最后利用速度决策融合速度梯形和PGVC 的加速度，输出期望纵向加速度。

图1 轨迹规划方案

轨迹跟踪作为下层控制器，由基于预瞄线性模型的模型预测控制器和加速度跟随控制器组成。前者将规划的路径和自车状态转化为跟踪误差，基于模型预测控制将此误差作为初始状态量构建二次规划，并把最佳控制序列的第一个值作为前轮转角控制量输出。而加速度跟随器预先定义了当前速度与期望加速度的关系，判断此时应给出制动压力还是节气门开度，在根据PID 反查表得出自车实际的制动压力或者节气门开度。

2 轨迹规划

2.1 RRT路径规划

本文从车辆模型约束和计算实时性两个方面对传统RRT 加以改进。文献［12］中指出只要规划曲线的曲率和曲率变化率低于门限值，曲线光滑度可满足车辆运动学模型。因此，本文采用G插值法生成具有C2连续性曲线，再通过限制其曲率和曲率变化率使得所构建的路径符合车辆运动学模型。此外，采用并行计算提高RRT 的计算速度，具体在3.3节中讨论。

改进的RRT 算法伪代码算法如表1 所示。首先，根据车辆位置和航向角对RRT 树初始化，并将初始化为False，表示还未找到可行解。Sample() 是采用探索和优化的方式在参考路径附近采样，为False 时采用预定义的路径，为True 时选择可行解路径或者预定义的路径作为参考路径。BestNearest采用深度神经网络作为度量函数近似两节点之间的曲率变化量，如图2 所示。Steer 使用G插值法生成螺旋线，返回曲率、曲率变化率和螺旋线的长度，并将从根节点至x的曲率变化量标记为。如果曲线没有与障碍物碰撞，且满足曲率和曲率变化量要求，则在附近搜索每个节点，记录下其曲率变化量。通过比较各节点曲率变化量和，找到最小的曲率变化量节点，输出对应曲线的中间节点。若有中间节点位于目标区域，则将改为True。

表1 改进的RRT

图2 BestNearest神经网络结构

2.2 梯形速度规划

在改进的RRT 获得无碰撞的目标路径基础上，通过速度规划进一步构建梯形速度曲线。根据RRT的规划路径长度和最高限速，确定梯形速度规划的加速和减速时长，此时可构建原始的速度曲线。由于在分段线组成的速度梯形中车辆加速度不连续，故为了得到连续的加速度曲线，基于三次多项式对梯形速度进行平滑处理。输出连续的加速度和速度曲线如式（1）所示，图3中对比光滑前后的梯形速度曲线。

图3 梯形速度曲线

式中：是当前加速度；是当前速度；是终端速度时刻；是终端加速度。

2.3 加速度决策

上述梯形速度曲线只考虑了RRT 的路径长度、当前车辆速度和加速度，忽略了速度对路径曲率的敏感程度，故未考虑车辆过弯时的操纵性能。PGVC针对不同曲率和曲率变化率，可动态调整相应的加/减速度，还可以通过速度变化率，动态调整相应的加/减速度，以维持车辆的过弯操纵性能。PGVC由速度预瞄跟踪器和GVC 组成，速度预瞄跟踪器基于预瞄点处曲率变化率判断出车辆的纵向加/减速度，如式（2）所示，GVC 基于车辆侧向加速度判断车辆的纵向加/减速度。为了除去因转向盘转角抖动造成的˙抖动，式（3）还定义一个开启GVC 的阈值。最终由PGVC 输出的加速度是两个部分叠加而成，即=+。式中：是速度预瞄跟踪器增益；是预瞄时间；˙是预瞄点处曲率变化率；a是车辆侧向加速度；˙是侧向加速度导数Jerk；C是GVC增益。

由PGVC 公式可知，是目标加速度，而式（1）也可产生理想的加速度曲线，两者在预瞄点曲率变化处会产生不同的期望加速度，故需要加速度控制逻辑给出唯一的期望加速度。加速度控制逻辑希望车辆安全行驶的同时保持速度优势。本文采用的加速度控制逻辑为：若PGVC 的减速度大于速度规划的减速度，或者PGVC 加速度大于速度规划的加速度且纵向车速小于最高车速时，将PGVC 的加速度作为最终的期望加速度，否则输出速度规划的加速度作为最终的期望加速度。梯形速度曲线和PGVC均能产生平滑的纵向加速度，因此线性加权后的期望加速度仍可保证平滑性和连续性。

3 仿真测试

本节不仅给出在S 弯道中智能汽车的轨迹规划效果，同时验证RRT 算法的实时性。本文中采用ROS、SIMULINK 和CARSIM 联合仿真的形式，台式机的配置是12 线程Intel i7-8700。RRT 路径规划和速度规划的控制频率均设定为10 Hz，而PGVC 和轨迹跟踪控制频率设为100 Hz。

3.1 RRT性能验证

本节将BestNearest 与常用的欧几里得度量、RRT*在如图4所示的环境中进行50次测试，对比其路径的平滑程度和收敛速度。为评估是否发生碰撞，采用将车辆的形状分解为多个连续的圆形，其圆形的半径设置为0.95 m。

图4 路径平滑性比较

图5 路径曲率变化量

图6 收敛速度

3.2 弯道稳定性测试

图7 弯道超车及避障轨迹

图7（f）展示了智能汽车弯道超车和避撞的全部轨迹，现分析本文轨迹规划方法的跟踪误差和速度优势。因弯道和换道最高限速是18 m/s，因此在0-2 s 内车速都有所下降，PGVC 介入使得车速下降至17 m/s，如图8（b）所示。而从图8（a）和图8（c）可看出，缺少PGVC 方案在自车第一次换道时（0-2 s）前轮已达到其最大转角17°，但侧向偏差依然达到0.9 m，缺少PGVC 方案的质心侧偏角达到10.9°。相比而言，本文的规划方法侧向偏差只有0.5 m，质心侧偏角只有8°，分别降低了80%和36.2%。

图8 路径跟踪和稳定性能

进入S 弯的第一个弯道时（3-5 s），由于PGVC输出减速度与梯形速度规划对比后，采用PGVC 减速度，故车速会继续下降。本文的规划方法路径跟踪误差相对较小，前轮转角稳定在4°左右，而非PGVC 控制的前轮转角接近于最大转角。而在车辆出弯时（6-10 s），由于速度规划和PGVC 联合作用，使得车辆加速至20.6 m/s 以较大的加速度出弯，从而获得速度优势。车辆在S 弯的第二个弯道时（12-15 s），本文规划方法的前轮转角稳定在4°左右，侧向偏差保持在0.1 m内，均远小于非PGVC控制的方案。因此，所提出的规划方法可维持智能汽车以较高的车速过弯，同时路径跟踪性能和操纵性能更好。

3.3 算法实时性验证

本文中的路径规划RRT基于C++和ROS系统实现。RRT首次订阅自车的姿态和周围车辆的姿态信息，并输出以自车为起点的路径。在下一次订阅时，将路径的第二个节点定为根节点，保留第一个节点至根节点的路径。若自车行驶过根节点，则删除第一个节点，第三个节点定为根节点，保留第二个节点至根节点的路径，以此反复循环。 RRT 的BestNearest 采用Open MP 并行计算，Open MP 是由#paragma 组成控制多线程执行程序，每次采样后BestNearest需要搜寻RRT树上的所有节点。由于各个节点之间没有数据依赖，Open MP 的For循环更适合运用于大量神经网络的计算。此外，速度规划也需要RRT 的路径信息，因此两者写入同一个节点内，并确定最终的期望加速度。

RRT 算法的BestNearest 采用深度神经网络，神经网络调用的频率随着节点数量增加而上升。因本文采用神经网络的层数和神经元个数较多，故BestNearest 会消耗较多的计算资源，需测试RRT 的实时性。图9 是分别进行640 次采样试验的测试结果。图9（a）是采用并行计算的更新频率，更新频率居中在6.25-7.25 Hz 之间；图9（b）是未采用并行计算，更新频率居中在0.8-1.6 Hz 之间，可看出并行计算大大提高了RRT算法的实时性。

图9 RRT计算效率

4 结论

本文提出一种智能汽车的轨迹规划方法，可实现车辆稳定、安全、快速地超过前车或避开障碍物。RRT 以牺牲少量最优解为代价，换取了快速的收敛速度，且保证了车辆路径的平滑程度。同时，引入并行计算提高了RRT 计算速度，解决了RRT 实时性问题。在有曲率不连续的S 弯道路上，RRT 返回曲率连续的路径，便于车辆控制。速度规划与PGVC 联合控制将路径的曲率考虑在内，输出的期望加速度更加符合过弯和换道要求，提高车辆过弯的操纵性能。未来考虑引入基于机器学习的决策，适用于更加复杂的道路环境。