基于优化的离散空间轨迹规划算法

宋春雷，张嘉轩，田晓春，徐建华，吴晓晖，张钰荣

（1.北京理工大学 自动化学院，北京 100081；2.北京自动化控制设备研究所，北京 100074）

随着传感器和计算设备等相关技术的发展与进步，自动驾驶技术近些年也得到了飞速发展。轨迹规划是自动驾驶技术中关键的一环，依据感知模块获取周围环境信息，规划出一条时空平滑的、安全的、舒适的行驶轨迹。

目前常见的轨迹规划算法主要包含两类：基于离散优化的轨迹规划算法[1]和基于机器学习的轨迹规划算法[2]。基于机器学习的轨迹规划算法往往具有端到端的特点，即将任务要求、车辆状态、环境状态等作为输入，根据训练好的神经网络等获得输入输出的映射关系，进而直接获得自动驾驶汽车所需的轨迹规划结果。常见的基于学习的轨迹规划方法如深度学习[3]、强化学习[4]和深度强化学习[5]等，此类方法的缺点是模型可解释性差，稳定性不高以及无法处理未训练过的场景。因此基于离散优化的轨迹规划算法应用更广泛，其采用离散状态方程描述被控车辆的运动学模型，并将车辆所在的横向-纵向空间离散，根据代价函数从多条可行路径中获得最优轨迹[6]。Chu 等人将空间离散为许多的路径点，根据安全成本、平滑度和一致性等约束条件选择出最优的路径点，然后利用三次多项式对离散点进行拟合，从而得到平滑的最优路径[7]。但该方法只考虑了静态障碍物的避障任务，而没有考虑动态障碍物对自动驾驶汽车的影响。Wu 等人考虑了行人和其他车辆等动态障碍物，根据驾驶员的舒适度和最小延时对路径进行优化，并在仿真中验证了所提算法[8]，但该算法的缺点是在变道时对驾驶员的感受缺乏考虑。Martinsen 等人提出了一种混合轨迹规划算法，将离散线路图搜索和凸优化相结合获得了全局最优的轨迹[9]，但由于其将障碍物考虑十分复杂，对几何形状描述较为精确，因此需要消耗很大的算力，规划时间较长，实时性较差。Jin 等人在传统的离散优化方法基础上，引入安全检查的概念，保证了无人车在极端行驶条件下的安全并且计算负担降低[10]，但该算法给出的速度曲线并不平滑。王富奎在局部路径规划时将安全性、舒适性、拟人性等约束条件引入目标函数，并采用图优化的方法进行求解[11]，但由于约束条件过于复杂导致规划前期速度变化剧烈。文献[12][13]采用采样和优化相结合的方法，解决了三维空间搜索计算量大的问题，仿真结果表明所提算法的效率比较高且规划时间是可接受的，但并未研究明显加减速场景中的速度规划问题。

针对离散空间轨迹规划问题，本文提出一种基于优化的规划方法。该规划方法可以使无人车得到安全无碰且曲率变化小的路径规划结果，以及让乘客舒适且平滑连续的速度规划结果。首先，将轨迹规划问题拆分为路径规划和速度规划两个模块，将纵向-横向空间（S-L空间）和纵向-时间空间（S-T空间）分别离散化后得到待规划的区域。然后，基于动态规划（Dynamic Programming,DP）和二次规划（Quadratic Programming,QP）在离散S-L空间中搜索出一条可以静态避障的路径。百度Apollo 提出的EMPlanner 在S-T空间中仍使用一般的动态规划，即对S-T空间中下一列的每个点均需搜索并计算，因此计算时间较长[14]。本文在S-T空间中提出一种改进的动态规划方法，对速度曲线进行初步搜索。该方法根据道路速度限制和不可倒车约束改进动态规划算法，并引入启发函数加快对规划终点的搜索速度，减少算法计算量，提高运行效率并且实现动态避障。最后，在Matlab/Simulink、Prescan 和Carsim 联合仿真平台对本文所提算法进行验证，通过设计静态障碍物和多种动态障碍物验证算法的有效性和优越性。

1 基于离散S-L 空间的路径规划

1.1 动态规划算法

对于车道行驶的场景，无人车完成任务需要全局路径信息，而本文的研究重点是用于实现静态避障和动态避障的轨迹规划，因此全局路径信息可直接使用基于传统算法（如A*、RRΤ、人工势场等）规划出的结果。将在全局路径信息中提取出的参考线作为待规划的S-L空间中心线[14]，根据所需路径规划长度确定纵向采样间隔，根据障碍物的大小确定横向采样间隔，从而获得待规划的离散S-L空间。由于无人车对静态障碍物进行避障时可以选择从左绕行或从右绕行，因此该问题并不是一个凸优化问题，存在多种解，非凸问题示意图如图1 所示。而动态规划算法可以给出一个粗略的解，并且该解把非凸问题转化为凸问题，进而可以利用二次规划求解器进行求解。

图1 非凸问题示意图Fig.1 Schematic diagram of non-convex problem

动态规划算法是将一个大问题拆解为多个相同的小问题，通过将每一个小问题求解出最优解进而得到整体大问题的最优解，从而降低算法复杂度，提高算法效率。动态规划的示意图如图2 所示。

图2 动态规划示意图Fig.2 Schematic diagram of dynamic programming

无人车路径动态规划问题可以描述为在离散S-L空间中根据设定的代价函数，从规划起始点逐步搜索代价最低的路径点直到规划终点，再从后向前遍历得到规划的路径。其中连接各个路径点的线并不是图2中的直线，而是根据初末状态约束求得的最优曲线。以连接路径点(1,1)和(2,2)的曲线为例，初末状态的约束如式(1)所示。

其中，g(s)为待定的最优曲线；g′(s)和g′(s)分别为g(s)的一阶和二阶导数；s和l是离散S-L空间中路径点的坐标。

由式(1)可知，每段路径的约束条件有6 个，根据泛函分析可知最优的曲线形式应该为五次多项式，因此每个路径点之间都通过五次多项式连接。在确定路径点之间的连接方式之后，需要根据代价函数计算每段路径的代价，其中代价函数如式(2)所示。

其中，costcollison为碰撞代价；costsmooth为平滑代价；costref为参考线距离代价。上述三个代价的具体函数如式(3)-(5)所示。

其中，ω collision为碰撞代价权重；ω ref为参考代价权重；ωdl、ω ddl、ω dddl分别为路径各阶导数代价权重；mind为无人车距离最近障碍物的距离；l为路径距参考线的距离，l′、l′、l′为l的各阶导数。

由式(3)-(5)可知，无人车距障碍物越近、路径越不平滑、距离参考线越远则代价越大。对于每个路径点均需计算其与下一列所有路径点之间的代价，并选取其中的最小值，然后记录该最小值对应的路径点，重复上述步骤直到搜索到规划终点，最后从终点反向索引即可得到动态规划的结果。

获得动态规划结果后还需要生成凸空间，即确定无人车遇到障碍物时执行左绕行还是右绕行。通过确定无人车在横向空间的上下限lmin和lmax与障碍物在纵向空间中的位置lobs之间的关系确定绕行方式，即执行左绕行时lmin＞lobs，而执行右绕行时lmax＜lobs。由此确定了无人车绕行障碍物的方式，并获得了二次规划所需的凸空间。

1.2 路径二次规划算法

根据动态规划获得的结果和凸空间，可以利用二次规划的方法对粗结果进行平滑和优化，进而获得最终的路径规划结果。二次规划算法的特点是可以在凸空间中针对最小值问题求解得到全局最优解，因此在约束相较于动态规划更严格的情况下，二次规划可以使路径更加平滑、更加贴近全局路径。带约束的二次规划问题如式(6)所示。

其中，x为待求解的状态变量，包括待求解路径的s、l、l′、l′；A和b为碰撞约束所需的矩阵；Aeq和beq为jerk 约束所需的矩阵；xlb和xub为规划起点的约束条件；H为参考线约束、路径的斜率和曲率约束以及路径末端约束；Τf为中心线以及期望终点的状态矩阵。

在获得上述约束之后，即可通过二次规划求解器对该问题进行求解。

2 基于离散S-T 空间的速度规划

2.1 改进的动态规划算法

与路径动态规划算法相似，改进的动态规划算法也需要在离散空间中进行。根据需要规划的纵向距离和常见障碍物的大小确定纵向采样间隔，根据规划时间的长短确定时间采样间隔。与路径动态规划不同的是，在S-L空间中进行下一步路径点的选择时是任意的，而在S-T空间中下一个点的选择需要考虑最大速度限制和不可倒车约束，即无人车在道路行驶过程中不得超过道路限速以及不能在行驶过程中倒车。根据这两点特性，可以对动态规划算法进行优化，减少动态规划的计算量。改进的动态规划算法在离散S-T空间中搜索的示意图如图3 所示。

图3 改进动态规划搜索示意图Fig.3 Improved dynamic programming search diagram

图3 表明搜索范围是红线和绿线之间，其中绿线代表对无人车的不可倒车约束，即搜索绿线以下部分意味着下一时刻车辆执行倒车；红线代表对无人车最大行驶速度的限制，即无人车的速度（S-T图中的斜率）不可超过道路规定的最大速度。搜索点的数量由纵向采样间隔、时间采样间隔和道路限速决定，其计算公式为：

其中，n为搜索点数量；speedmax为道路限速；ΔT为时间采样间隔；ΔS为纵向采样间隔。

在确定了搜索点数量后，根据设计的代价函数计算当前点到下一列点的代价最小值，并记录最小值对应的点，重复上述步骤直到搜索到规划终点。与路径规划不同的是，速度规划中各点之间的连接并不需要为五次多项式，只需为直线段即可。因为时间采样间隔较小，短时间内可以认为无人车在一个采样间隔内做匀速运动，故而采用直线段连接。各点连接的代价函数也与路径规划算法不同，速度规划中的代价函数除了计算到下一点的代价，还引入了下一点到搜索终点的启发函数，具体形式如式(8)所示。

其中，costobs为障碍物代价；costaccel为加速度代价；costrefv为参考速度代价；ωH为启发函数的权重；H(s,t)为节点到规划终点的启发函数。

其中，m为传感器探测到的障碍物数量；d(i)min为无人车到第i个障碍物的最小距离；ω obs为障碍物代价权重。

当无人车距障碍物最近距离大于1.5 m 时，障碍物代价为0；小于等于0.5 m 时，障碍物代价为恒值；其余情况障碍物代价随距离增大而减小。

加速度代价为：

其中，a为当前无人车的加速度，其具体范围与真车实际参数保持一致；ω accel为加速度代价权重。当加速度超过车辆动力学限制时，加速度代价急剧增大。

参考速度代价为：

其中，speed为无人车当前速度；speedref为参考速度；ωvref为参考速度代价权重。无人车当前速度越接近参考速度则代价越小。

当前节点到目标节点的启发函数为：

用数学知识解决数学问题的意义不仅仅在于结果，更在于过程中能够通过不断的尝试和探索体验到解决实际问题的快乐。小学生由于年龄较小，比较好动，注意力不能长时间集中，但是对于生活中的事物却有着很高的探索欲望，所以在解决问题的时候应让学生通过多动手、多尝试、画图、逆向思维、先易后难等策略，使学生充分体验到“直观”到“抽象”之间的过度与过程，引导学生进行由动作思维→形象思维→抽象思维的转变过程，让学生真正掌握题中的关系，进而掌握新的解题策略。

其中，H(s)为纵向启发函数；H(t)为时间启发函数；scur为当前纵向搜索点；s target为目标纵向搜索点；s end为纵向搜索终点；tcur为当前时间搜索点；ttarget为目标时间搜索点；tend为时间搜索终点；ωs为以多个纵向距离为目标点的启发函数权重；ωt为以多个时间为目标点的启发函数权重；m为目标点总数。

根据所需规划的速度不同，可以调节sω和ωt的大小，以改进不同的目标点对启发函数的影响。当ωs较大时，无人车规划出的速度更大；当ωt较大时，则无人车规划出的速度较为缓慢。

与路径规划相同，在获得了改进的动态规划结果后还需开辟凸空间，即确定无人车遇到动态障碍物时是执行加速超车还是执行减速避让。在某一固定时间段内，执行避让时smin≤sobs，而执行超车时smax≥sobs。由此确定了超车和避让的策略，获得了可以用于二次规划的凸空间。

2.2 速度二次规划算法

与路径二次规划算法相似，速度二次规划算法的公式表达与式(6)相同，不同的是式中各约束矩阵的含义。其中x为待求解的纵向距离、速度、加速度；A和b为不可倒车约束矩阵；Aeq和beq为连续性约束矩阵；x ub和xlb为凸空间约束矩阵；H为参考速度代价、加速度代价、jerk 代价矩阵；f为参考速度矩阵。根据二次规划求解器即可对速度二次规划问题求解。

在获得了路径二次规划结果和速度二次规划结果后，将所得路径和速度根据顺序进行融合以获得轨迹规划结果。在完成一个规划周期后，根据定位信息、上一周期的规划结果以及控制周期，可以将上周期的轨迹和本周期的轨迹进行拼接，进而发送给控制模块实现无人车的轨迹控制。路径规划和速度规划的迭代流程图如图4 所示。

图4 算法迭代流程图Fig.4 Algorithm iteration flowchart

3 仿真及结果分析

3.1 仿真平台

本文使用硬件配置为Intel 酷睿i5 12600KF 处理器，内存为16 G。软件使用Windows10 操作系统、Carsim2019.1 仿真平台、Matlab2020a、Prescan8.5 以及Visual Studio2019，其中Carsim 是为了获得无人车的数学模型，Matlab/Simulink 用于编写具体的规划决策程序以及搭建相关的定位模块、控制模块等，Prescan 用于设置具体的仿真环境如无人车、道路和障碍物等，Visual Studio 用于Carsim 和Simulink 联合仿真的编译。

3.2 静态避障实验

本文采用Prescan 作为仿真环境，通过引入无人车模型、理想传感器、道路模型以及障碍物模型等搭建基本仿真环境，通过设置障碍物的速度建立静态和动态避障实验环境。基本仿真环境如图5 所示，其中无人车和理想传感器固连，理想传感器主要负责采集障碍物的速度和航向；绿色阴影部分为传感器探测范围，设置为半径60 m 的圆形；红色直线为根据全局规划算法得到的全局路径，即车道中心线。

图5 基本仿真环境Fig.5 Basic simulation environment

算法中各代价权重、最大速度、距障碍物最小距离等参数的具体设置如表1 所示。障碍物设置为多个1 m×1 m×1 m 的立方体，无人车的参考速度设置为15 m/s，并且运动场景设置为无人车在Y 字路口转弯时遇到多个障碍物。静态避障结果如图6 所示。

图6 静态避障实验结果Fig.6 Static obstacle avoidance test results

由图6 可知，当无人车初次探测到障碍物时，动态规划算法会规划出一个十分安全但偏离全局路径很远的结果，并且路径的曲率较大。二次规划的优化给无人车规划出一个比动态规划更平滑、更靠近全局路径的结果并且也可以实现避障。当完成静态避障任务后，动态规划和二次规划的结果重合，表明在无障碍物时二者的规划结果一致。从图6(f)可以看出，当无人车在8 s 左右进行右转弯并遇到障碍物时，有明显的减速过程，并且这个过程较为平滑；无人车在16 s后完成避障动作，又以平滑的加速度将速度调整回参考速度15 m/s。当无人车进行避障动作以及完成避障后进行加速动作时，由表2 可知，算法规划出的路径曲率的平均值很小，并且标准差小，说明路径不仅平滑，而且波动变化较小，适合无人车的控制；当无人车回到全局路径且没有障碍物阻挡时，算法规划的路径与车道中心线完全重合，因此其曲率很小，说明路径规划结果非常平滑。

表2 静态避障路径平滑程度Tab.2 Smoothness of static obstacle avoidance path

3.3 动态避障实验

动态避障实验中障碍物的尺寸与静态避障实验相同，不同的是障碍物具有速度，并且在道路两边往复穿梭。各障碍物距起点的距离和自身速度如表3 所示，表中各速度依次对应典型的交通障碍如：行人、非机动车、低速机动车、高速机动车等。动态避障实验中的算法参数与静态避障实验中的参数保持一致。动态避障结果如图7 所示。

表3 动态障碍物参数Tab.3 Dynamic obstacle parameters

图7 动态避障实验结果Fig.7 Dynamic obstacle avoidance test results

由图7 可知，当无人车遇到第一个障碍物时，由于障碍物的速度较慢，无人车根据改进的动态规划算法规划出了一个超车的动作，然后二次规划使得这个结果更加远离障碍物，保证了无人车安全地实现超车。行驶至任务中段时，无人车再次遇到两个障碍物，但根据障碍物的速度和位置，无人车判断该障碍物无需超车或避让，继续保持原有速度即可安全通行，因此忽略了两个障碍物的影响，并且以参考速度安全通过。当无人车遇到最后一个障碍物时，由于障碍物的速度很快，无人车根据改进的动态规划算法规划出了一个减速避让的动作，然后二次规划又规划出了一个更低的速度，保证了无人车安全地执行减速避让动作。从图7(f)可以看出，超车时无人车从参考速度15 m/s 开始以较为平稳的加速度加速至19.5 m/s，在完成超车动作后又平稳减速至参考速度。同样，在执行减速避让动作时速度变化也较为平稳。将改进DP 算法和传统的DP 算法相对比，可得如表4 所示的结果。

表4 算法对比结果Tab.4 Algorithm comparison results

由表4 可知，两种算法的平均加速度几乎相当，但本文提出的改进DP 算法的加速度标准差更小，证明本文算法使无人车的速度变化更加平稳。在算法运行时间方面，考虑进行一次动态规划所需的计算时间，并对完整的任务过程求其平均值得到单次规划时间。与传统的DP 算法相比，本文改进DP 算法的运算时间减少了77.13%，算法效率得到了提高并使得规划更加满足无人车实时性的需求。

4 结论

本文针对无人车轨迹规划问题，提出了基于优化的离散空间轨迹规划方法，使得无人车可以平稳、安全、高效地完成任务。搭建了Matlab/Simulink，Prescan和Carsim 联合仿真平台，设计了静态避障实验和动态避障实验。仿真实验结果表明，本文提出的离散轨迹规划方法具有轨迹曲率更小、速度曲线更平滑的优点。与传统的DP 算法相比，本文提出的改进DP 算法的加速度标准差更小、单次规划时间更短、算法效率更高，能够满足无人车的实时性要求。