CN109855639A - 基于障碍物预测与mpc算法的无人驾驶轨迹规划方法 - Google Patents

Abstract

本发明公开了一种基于障碍物预测与MPC算法的无人驾驶轨迹规划方法：周围车辆运动轨迹预测；碰撞预测分析与最小安全距离的计算；考轨迹与约束条件的生成；利用模型预测控制(MPC)进行轨迹优化，得到最终轨迹输出。本发明能够利用障碍物运动预测与模型预测控制(MPC)原理的轨迹最优化进行无人驾驶车辆的运动决策与轨迹规划。

Description

基于障碍物预测与MPC算法的无人驾驶轨迹规划方法

技术领域

本发明属于无人驾驶决策规划领域，更具体的说，是涉及一种基于障碍物预测与MPC算法的无人驾驶轨迹规划方法。

背景技术

随着社会的发展，人们对生活品质的要求越来越高，汽车成为人类生活不可或缺的交通工具，然而汽车数量的增加以及驾驶员自身安全意识的薄弱导致全球交通事故发生率急剧增长。面对日益严峻的交通安全与交通拥堵问题，构建智能交通系统的任务变得更加急切。无人驾驶车辆作为构建智能交通系统的关键部分，近年来受到各方的关注。无人驾驶车辆融合了环境感知与定位、决策规划与运动控制等多项功能，从而代替驾驶员的眼、脑和手，具有反应迅速、行驶安全可靠等优点。目前美国、英国和德国等一些国家的无人驾驶技术已发展较为成熟，我国的无人驾驶技术起步较晚，在一些关键技术的发展方面距离世界先进水平还有一定的差距。

模型预测控制(MPC)是20世纪70年代产生于工业过程控制领域的一类计算机控制算法，其基本要素为：预测模型、滚动优化、反馈校正和参考轨迹。MPC通过不断滚动的局部优化，使模型失配、畸变、干扰等引起的不确定性及时得到弥补，从而取得较好的动态控制性能。MPC最明显的优点是对模型的精确度要求不高并能在控制过程中增加多种约束，因此非常适合用在无人驾驶的规划和控制中。在近几年无人驾驶领域的发展中，MPC得到了很好的应用，例如加拿大滑铁卢大学采用多约束模型预测控制进行车辆避障与轨迹跟踪，瑞典沃尔沃汽车集团采用模型预测控制进行换道策略规划与车辆控制。在无人驾驶领域，常采用的预测模型有物理运动学规律、车辆运动学模型和车辆动力学模型等，由于运动规划对于车辆本身的特性要求不高，在本发明中采用物理运动学规律作为预测模型。

之前出现的一些基于MPC的规划算法只考虑了应用于直路的避障换道策略，或仅为给定路径的跟踪算法，本发明在根据周围环境做出轨迹预测后，除了考虑避障换道部分的轨迹规划外，还考虑路口转弯部分的轨迹规划。

发明内容

本发明的目的是为了克服现有技术中的不足，提供一种基于障碍物预测与MPC算法的无人驾驶轨迹规划方法，能够利用障碍物运动预测与模型预测控制(MPC)原理的轨迹最优化进行无人驾驶车辆的运动决策与轨迹规划。

本发明的目的是通过以下技术方案实现的。

本发明的基于障碍物预测与MPC算法的无人驾驶轨迹规划方法，包括以下步骤：

第一步：周围车辆运动轨迹预测

周围车辆包括同车道前方车辆、相邻车道前方和后方的车辆，通过车载传感器设备及相应的图像识别算法得到周围车辆的运动速度与方向；假设在预测时域内，周围车辆沿当前车道按照当前速度行驶，得到周围车辆运动轨迹；

第二步：碰撞预测分析与最小安全距离的计算

无人驾驶车辆V_A，当前运动速度为v_A，由决策层输出的参考速度v_des和运动指令可以估计车辆的大概行驶轨迹；为避免无人驾驶车辆V_A与周围车辆发生碰撞，进行碰撞预测分析与最小安全距离(MSS)的计算；换道场景坐标系为建立在无人驾驶车辆后轴中心的车载坐标系，换道场景中包括同车道前方车辆V₁，速度为v₁，与V_A的距离为L₁；目标车道前方同方向行驶的车辆V₂，速度为v₂，与V_A的距离为L₂；目标车道后方同方向行驶的车辆V₃，速度为v₃，与V_A的距离为L₃。V_A换道过程从时刻t₀＝0开始准备换道，到时刻t_adj为施加横向加速度前的调整时间，t_lat为施加横向加速度的时间，故时刻t_adj+t_lat施加横向加速度结束，V_A完全到达目标车道，t_finish为换道策略预测时间；

①V_A与V₁碰撞分析：

对V_A施加横向加速度，其右前角通过V₁左侧纵向切线之前易于V₁发生碰撞，两车之间的最小安全距离为：

其中，a_A和a₁分别为V_A与V₁的加速度，为V_A施加横向加速度开始到其右前角通过V₁左侧纵向切线的时间；

②V_A与V₂碰撞分析：

对V_A施加横向加速度，其左前角通过V₂右侧纵向切线之后易于V₂发生碰撞，两车之间的最小安全距离为：

其中，a₂为V₂的加速度，为V_A施加横向加速度开始到其左前角通过V₂右侧纵向切线的时间；

③V_A与V₃碰撞分析：

对V_A施加横向加速度，其左后角通过V₃右侧纵向切线之后易于V₃发生碰撞，两车之间的最小安全距离为：

其中，a₃为V₃的加速度，为V_A施加横向加速度开始到左后角通过V₃右侧纵向切线的时间；

第三步：参考轨迹与约束条件的生成

采用换道场景坐标系，分为纵向和横向两部分，约束条件分别为位置、速度、加速度、加加速度的限制，其中加速度与加加速度的约束为确定范围，根据车辆的一般特性确定；考虑直路避障与十字路口转弯两种情况：

①直路避障：

在本发明中均设定为纵向匀速换道，一般换道过程航向角φ为3～5°，故得到施加横向加速度的时间为：

其中，l为车道宽度；

在时刻t_adj前参考轨迹为V_A沿当前车道的匀速和匀减速过程，对应横向位置约束为当前车道左右两条车道线；时间段部分的换道轨迹用一段与纵向夹角为3～5°的直线代替，对应横向位置约束为当前车道和目标车道两侧的车道线；在时刻t_adj+t_lat之后为沿目标车道的匀加速和匀速过程，对应横向位置约束为目标车道的左右两条车道线；向左换道的参考轨迹与横向位置约束中包括横向位置最小值y_min和横向位置最大值y_max；由决策层输出参考速度v_des，产生换道行为一般是在当前车道前方车辆速度较小时，故在时刻t_adj前V_A的纵向速度约束为[v₁,v_des]；到达目标车道后V_A为保持与V₂和V₃的安全距离，故纵向速度最大值为v_xmax＝min{v_des,v₂}，最小值为v_xmin＝max{v₁,v₃}；V_A的横向速度约束由t_lat决定；

②十字路口转弯：

车辆需要在低速情况下进行转弯较安全，故在转弯前需要进行一段减速过程，减速至安全转弯速度v_turn，故参考轨迹即为沿当前车道中心线的匀速和匀减速过程，对应的横向位置约束为当前车道的左右两侧车道线；转弯时采用一段圆弧，可根据转弯所到达的车道初始点确定转弯半径和圆弧圆心；转弯结束后，车辆先进行匀加速过程，直至速度达到v_des，对应位置约束仍为车道两侧车道线。由于V_A整体的速度大小范围为[v_turn,v_des]，根据各时刻车辆运动方向确定其纵横向速度方向；十字路口右转的参考轨迹与位置约束中包括转弯前横向位置最小值y_min和横向位置最大值y_max，由于转弯后车辆沿横向行驶，还包括转弯后纵向位置最小值x_min、纵向位置最大值x_max以及转弯部分位置约束；

第四步：利用模型预测控制(MPC)进行轨迹优化，得到最终轨迹输出

本发明采用的模型为物理运动学规律为：

其中，x₀和v₀分别表示初始时刻车辆的位置和速度，x和v分别表示预测时刻t的车辆位置和速度，a为相应加速度；利用MPC算法进行优化时，纵向和横向分别进行计算，采用相同的模型，下面以纵向为例介绍计算过程：

①选取状态量为x_x、v_x、a_x分别为纵向位置、纵向速度和纵向加速度，控制量为u(k)＝Δa_x(k)，也即Δa_x为纵向加加速度，故得到新的状态空间表达式为：

相应的

②由状态方程得到未来时刻的输出为：

③选取代价函数为：

其中，I为单位矩阵，N_p为预测时域，N_c为控制时域，x_ref为参考轨迹转换到当前车载坐标系下的纵向坐标，a和b均为正的权重系数。约束条件为：

其中，x_max和x_min分别为纵向位置的约束，v_xmax和v_xmin分别为纵向速度的约束，a_xmax和a_xmin分别为纵向加速度的约束，Δa_xmax和Δa_xmin分别为纵向加加速度的约束；

④通过二次规划求解得到最优控制量为U^*＝Δa(t|t)；

⑤将得到的最优控制量作为下一步预测的输入，重复①-④的过程，直到得到t₀～t_finish所有时刻的轨迹输出。

设定V_A进行纵向匀速换道，故第二步中得到的三个最小安全距离为

先计算v_A＝v_des是否满足换道条件，如不满足则V_A需要提前进行减速至v_A＝v₁后按t＝t₀重新进行计算。

与现有技术相比，本发明的技术方案所带来的有益效果是：

本发明采用MPC算法进行轨迹规划，对模型的精确度要求不高，对于车辆运动约束可以较明确地体现在MPC求解的约束条件中。本发明在进行车辆的轨迹规划时考虑了周围障碍物的运动轨迹影响，保证了轨迹的安全性。由MPC算法得到的轨迹输出满足了平滑性的要求。本发明将车辆运动分为纵向和横向分别进行规划，采用不同的预测时域，由于纵向运动主要为速度规划，故采用较大的预测时域使得速度变化较平缓，而横向运动主要为位置规划，主要体现在换道部分，故采用较小的预测时域，使得换道动作反应灵敏。在每一步的MPC预测求解时均利用上一步预测得到的位置点与速度建立新的车载坐标系，由此方便约束条件的处理，并且得到的纵横向速度和加速度更为直观。除换道策略外，本发明考虑了十字路口转弯部分的轨迹规划，得到较好的规划结果。本发明将轨迹规划与跟踪合为一体，简化了无人驾驶车辆规划层的工作。

附图说明

图1为换道场景示意图。

图2为车辆换道过程示意图。

图3为向左换道的参考轨迹与位置约束示意图。

图4为十字路口右转的参考轨迹与位置约束示意图。

图5为MPC求解过程中坐标系转换示意图。

图6为向左换道仿真结果图。

图7为十字路口右转仿真结果图。

具体实施方式

下面结合附图对本发明作进一步的描述。

无人驾驶车辆的决策规划层的三个基本要求为：安全性、实时性、舒适性。面对实时交通环境，为满足以上三点要求，本发明做出以下几个方面的研究：(1)周围运动车辆的轨迹预测；(2)换道策略：碰撞预测分析与最小安全距离的计算；(3)十字路口车辆转弯的轨迹规划；(4)模型预测控制算法参数设置。

本发明的基于障碍物预测与MPC算法的无人驾驶轨迹规划方法，包括以下步骤：

第一步：周围车辆运动轨迹预测

周围车辆包括同车道前方车辆、相邻车道前方和后方的车辆，通过车载传感器设备及相应的图像识别算法可以得到周围车辆的运动速度与方向。假设在预测时域内，周围车辆沿当前车道按照当前速度行驶，得到周围车辆运动轨迹。

第二步：碰撞预测分析与最小安全距离的计算

无人驾驶车辆V_A，当前运动速度为v_A，由决策层输出的参考速度v_des和运动指令可以估计车辆的大概行驶轨迹。为避免无人驾驶车辆V_A与周围车辆发生碰撞，需要进行碰撞预测分析与最小安全距离(MSS)的计算。换道场景如图1所示，坐标系为建立在无人驾驶车辆后轴中心的车载坐标系。换道场景中包括同车道前方车辆V₁，速度为v₁，与V_A的距离为L₁；目标车道前方同方向行驶的车辆V₂，速度为v₂，与V_A的距离为L₂；目标车道后方同方向行驶的车辆V₃，速度为v₃，与V_A的距离为L₃。V_A换道过程如图2所示，从时刻t₀＝0开始准备换道，到时刻t_adj为施加横向加速度前的调整时间，t_lat为施加横向加速度的时间，故时刻t_adj+t_lat施加横向加速度结束，V_A完全到达目标车道，t_finish为换道策略预测时间。

①V_A与V₁碰撞分析：

对V_A施加横向加速度，其右前角通过V₁左侧纵向切线之前易于V₁发生碰撞，两车之间的最小安全距离为：

其中，a_A和a₁分别为V_A与V₁的加速度，为V_A施加横向加速度开始到其右前角通过V₁左侧纵向切线的时间。

②V_A与V₂碰撞分析：

对V_A施加横向加速度，其左前角通过V₂右侧纵向切线之后易于V₂发生碰撞，两车之间的最小安全距离为：

其中，a₂为V₂的加速度，为V_A施加横向加速度开始到其左前角通过V₂右侧纵向切线的时间。

③V_A与V₃碰撞分析：

对V_A施加横向加速度，其左后角通过V₃右侧纵向切线之后易于V₃发生碰撞，两车之间的最小安全距离为：

其中，a₃为V₃的加速度，为V_A施加横向加速度开始到左后角通过V₃右侧纵向切线的时间。

本发明中设定V_A进行纵向匀速换道，故可以得到的三个最小安全距离为：

先计算v_A＝v_des是否满足换道条件，如不满足则V_A需要提前进行减速至v_A＝v₁后按t＝t₀重新进行计算。

第三步：参考轨迹与约束条件的生成

此部分坐标系为图1所示的采用换道场景坐标系，分为纵向和横向两部分，约束条件分别为位置、速度、加速度、加加速度的限制，其中加速度与加加速度的约束为确定范围，根据车辆的一般特性确定。在本发明中考虑直路避障与十字路口转弯两种情况，以下将分别进行说明。

①直路避障：

在本发明中均设定为纵向匀速换道，一般换道过程航向角φ为3～5°，故可以得到施加横向加速度的时间为：

其中，l为车道宽度。

在时刻t_adj前参考轨迹为V_A沿当前车道的匀速和匀减速过程，对应横向位置约束为当前车道左右两条车道线。时间段部分的换道轨迹用一段与纵向夹角为3～5°的直线代替，对应横向位置约束为当前车道和目标车道两侧的车道线。在时刻t_adj+t_lat之后为沿目标车道的匀加速和匀速过程，对应横向位置约束为目标车道的左右两条车道线。向左换道的参考轨迹与横向位置约束如图3所示，其中虚线为横向位置最小值y_min，点划线为横向位置最大值y_max。由决策层输出参考速度v_des，产生换道行为一般是在当前车道前方车辆速度较小时，故在时刻t_adj前V_A的纵向速度约束为[v₁,v_des]。到达目标车道后V_A为保持与V₂和V₃的安全距离，故纵向速度最大值为v_xmax＝min{v_des,v₂}，最小值为v_xmin＝max{v₁,v₃}。V_A的横向速度约束由t_lat决定。

②十字路口转弯：

车辆需要在低速情况下进行转弯较安全，故在转弯前需要进行一段减速过程，减速至安全转弯速度v_turn，故参考轨迹即为沿当前车道中心线的匀速和匀减速过程，对应的横向位置约束为当前车道的左右两侧车道线。转弯时采用一段圆弧，可根据转弯所到达的车道初始点确定转弯半径和圆弧圆心。转弯结束后，车辆先进行匀加速过程，直至速度达到v_des，对应位置约束仍为车道两侧车道线。由于V_A整体的速度大小范围为[v_turn,v_des]，根据各时刻车辆运动方向确定其纵横向速度方向。如图4所示为十字路口右转的参考轨迹与位置约束，其中水平虚线为转弯前横向位置最小值y_min，水平点划线为横向位置最大值y_max，由于转弯后车辆沿横向行驶，竖直虚线为转弯后纵向位置最小值x_min，竖直点划线为纵向位置最大值x_max，转弯部分位置约束如虚线所示。

第四步：利用模型预测控制(MPC)进行轨迹优化，得到最终轨迹输出

本发明采用的模型为物理运动学规律为：

其中，x₀和v₀分别表示初始时刻车辆的位置和速度，x和v分别表示预测时刻t的车辆位置和速度，a为相应加速度。由于换道过程中航向角较小，横向与纵向间速度相互影响不计。利用MPC算法进行优化时，纵向和横向分别进行计算，采用相同的模型，下面以纵向为例介绍计算过程。

①选取状态量为x_x、v_x、a_x分别为纵向位置、纵向速度和纵向加速度，控制量为u(k)＝Δa_x(k)，也即Δa_x为纵向加加速度，故得到新的状态空间表达式为：

相应的

②由状态方程可以得到未来时刻的输出为：

③选取代价函数为：

其中，I为单位矩阵，N_p为预测时域，N_c为控制时域，x_ref为参考轨迹转换到当前车载坐标系下的纵向坐标，a和b均为正的权重系数。约束条件由第三步得到为：

其中，x_max和x_min分别为纵向位置的约束，v_xmax和v_xmin分别为纵向速度的约束，a_xmax和a_xmin分别为纵向加速度的约束，Δa_xmax和Δa_xmin分别为纵向加加速度的约束。

④通过二次规划求解得到最优控制量为U^*＝Δa(t|t)。

求解时令

故代价函数可以整理为：

接下来根据二次规划的思想进行求解即可。

⑤将得到的最优控制量作为下一步预测的输入，重复①-④的过程，直到得到t₀～t_finish所有时刻的轨迹输出。在每一步MPC求解时均需转换为当前位置的车载坐标系，如图5所示。原坐标系中任意点(x_before,y_before)与转换到新坐标系中的坐标值(x_after,y_after)的关系为：

其中(x₁,y₁)为上一时刻计算得到的位置点，也即转换坐标系后的原点，θ为其航向角。每一时刻得到的位置点都需转换到原始坐标系中进行输出，对于速度、加速度和加加速度无需进行转换。

在纵向和横向分别进行规划时采用不同的预测时域，由于纵向运动主要为速度规划，故采用较大的预测时域使得速度变化较平缓，而横向运动主要为位置规划，主要体现在换道部分，故采用较小的预测时域，使得换道动作反应灵敏。

多车道向左换道场景和十字路口转弯场景仿真结果如图6和图7所示。其中点状线为规定好的车道线，虚线为参考轨迹，实线为MPC优化后的轨迹。

尽管上面结合附图对本发明的功能及工作过程进行了描述，但本发明并不局限于上述的具体功能和工作过程，上述的具体实施方式仅仅是示意性的，而不是限制性的，本领域的普通技术人员在本发明的启示下，在不脱离本发明宗旨和权利要求所保护的范围情况下，还可以做出很多形式，这些均属于本发明的保护之内。

Claims (2)

1.一种基于障碍物预测与MPC算法的无人驾驶轨迹规划方法，其特征在于，包括以下步骤：

第一步：周围车辆运动轨迹预测

周围车辆包括同车道前方车辆、相邻车道前方和后方的车辆，通过车载传感器设备及相应的图像识别算法得到周围车辆的运动速度与方向；假设在预测时域内，周围车辆沿当前车道按照当前速度行驶，得到周围车辆运动轨迹；

第二步：碰撞预测分析与最小安全距离的计算

无人驾驶车辆V_A，当前运动速度为v_A，由决策层输出的参考速度v_des和运动指令可以估计车辆的大概行驶轨迹；为避免无人驾驶车辆V_A与周围车辆发生碰撞，进行碰撞预测分析与最小安全距离(MSS)的计算；换道场景坐标系为建立在无人驾驶车辆后轴中心的车载坐标系，换道场景中包括同车道前方车辆V₁，速度为v₁，与V_A的距离为L₁；目标车道前方同方向行驶的车辆V₂，速度为v₂，与V_A的距离为L₂；目标车道后方同方向行驶的车辆V₃，速度为v₃，与V_A的距离为L₃。V_A换道过程从时刻t₀＝0开始准备换道，到时刻t_adj为施加横向加速度前的调整时间，t_lat为施加横向加速度的时间，故时刻t_adj+t_lat施加横向加速度结束，V_A完全到达目标车道，t_finish为换道策略预测时间；

①V_A与V₁碰撞分析：

对V_A施加横向加速度，其右前角通过V₁左侧纵向切线之前易于V₁发生碰撞，两车之间的最小安全距离为：

其中，a_A和a₁分别为V_A与V₁的加速度，为V_A施加横向加速度开始到其右前角通过V₁左侧纵向切线的时间；

②V_A与V₂碰撞分析：

对V_A施加横向加速度，其左前角通过V₂右侧纵向切线之后易于V₂发生碰撞，两车之间的最小安全距离为：

其中，a₂为V₂的加速度，为V_A施加横向加速度开始到其左前角通过V₂右侧纵向切线的时间；

③V_A与V₃碰撞分析：

对V_A施加横向加速度，其左后角通过V₃右侧纵向切线之后易于V₃发生碰撞，两车之间的最小安全距离为：

其中，a₃为V₃的加速度，为V_A施加横向加速度开始到左后角通过V₃右侧纵向切线的时间；

第三步：参考轨迹与约束条件的生成

采用换道场景坐标系，分为纵向和横向两部分，约束条件分别为位置、速度、加速度、加加速度的限制，其中加速度与加加速度的约束为确定范围，根据车辆的一般特性确定；考虑直路避障与十字路口转弯两种情况：

①直路避障：

在本发明中均设定为纵向匀速换道，一般换道过程航向角φ为3～5°，故得到施加横向加速度的时间为：

其中，l为车道宽度；

在时刻t_adj前参考轨迹为V_A沿当前车道的匀速和匀减速过程，对应横向位置约束为当前车道左右两条车道线；时间段部分的换道轨迹用一段与纵向夹角为3～5°的直线代替，对应横向位置约束为当前车道和目标车道两侧的车道线；在时刻t_adj+t_lat之后为沿目标车道的匀加速和匀速过程，对应横向位置约束为目标车道的左右两条车道线；向左换道的参考轨迹与横向位置约束中包括横向位置最小值y_min和横向位置最大值y_max；由决策层输出参考速度v_des，产生换道行为一般是在当前车道前方车辆速度较小时，故在时刻t_adj前V_A的纵向速度约束为[v₁,v_des]；到达目标车道后V_A为保持与V₂和V₃的安全距离，故纵向速度最大值为v_xmax＝min{v_des,v₂}，最小值为v_xmin＝max{v₁,v₃}；V_A的横向速度约束由t_lat决定；

②十字路口转弯：

车辆需要在低速情况下进行转弯较安全，故在转弯前需要进行一段减速过程，减速至安全转弯速度v_turn，故参考轨迹即为沿当前车道中心线的匀速和匀减速过程，对应的横向位置约束为当前车道的左右两侧车道线；转弯时采用一段圆弧，可根据转弯所到达的车道初始点确定转弯半径和圆弧圆心；转弯结束后，车辆先进行匀加速过程，直至速度达到v_des，对应位置约束仍为车道两侧车道线。由于V_A整体的速度大小范围为[v_turn,v_des]，根据各时刻车辆运动方向确定其纵横向速度方向；十字路口右转的参考轨迹与位置约束中包括转弯前横向位置最小值y_min和横向位置最大值y_max，由于转弯后车辆沿横向行驶，还包括转弯后纵向位置最小值x_min、纵向位置最大值x_max以及转弯部分位置约束；

第四步：利用模型预测控制(MPC)进行轨迹优化，得到最终轨迹输出本发明采用的模型为物理运动学规律为：

其中，x₀和v₀分别表示初始时刻车辆的位置和速度，x和v分别表示预测时刻t的车辆位置和速度，a为相应加速度；利用MPC算法进行优化时，纵向和横向分别进行计算，采用相同的模型，下面以纵向为例介绍计算过程：

①选取状态量为x_x、v_x、a_x分别为纵向位置、纵向速度和纵向加速度，控制量为u(k)＝Δa_x(k)，也即Δa_x为纵向加加速度，故得到新的状态空间表达式为：

相应的

②由状态方程得到未来时刻的输出为：

③选取代价函数为：

其中，I为单位矩阵，N_p为预测时域，N_c为控制时域，x_ref为参考轨迹转换到当前车载坐标系下的纵向坐标，a和b均为正的权重系数。约束条件为：

其中，x_max和x_min分别为纵向位置的约束，v_xmax和v_xmin分别为纵向速度的约束，a_xmax和a_xmin分别为纵向加速度的约束，Δa_xmax和Δa_xmin分别为纵向加加速度的约束；

④通过二次规划求解得到最优控制量为U^*＝Δa(t|t)；

⑤将得到的最优控制量作为下一步预测的输入，重复①-④的过程，直到得到t₀～t_finish所有时刻的轨迹输出。

2.根据权利要求1所述的，其特征在于，设定V_A进行纵向匀速换道，故第二步中得到的三个最小安全距离为

先计算v_A＝v_des是否满足换道条件，如不满足则V_A需要提前进行减速至v_A＝v₁后按t＝t₀重新进行计算。