CN117962886B - 一种基于他车轨迹预测的车辆自适应巡航控制方法 - Google Patents

Abstract

本发明公开了一种基于他车轨迹预测的车辆自适应巡航控制方法，包括以下步骤：A、车道线数据拟合；B、车道目标筛选；C、目标轨迹预测；D、跟踪目标信息输出。本发明能够改进现有技术的不足，提高自适应巡航控制系统的行驶安全性及舒适性。

Description

一种基于他车轨迹预测的车辆自适应巡航控制方法

技术领域

本发明涉及车辆辅助驾驶技术领域，尤其是一种基于他车轨迹预测的车辆自适应巡航控制方法。

背景技术

近年来，随着汽车感知能力及芯片计算能力的提升，汽车高级辅助驾驶系统（Advanced Driving Assistant System，ADAS）已成为当前汽车智能辅助系统的研究热点。其中自适应巡航控制（Adaptive Cruise Control，ACC）根据当前行驶工况主动调节车辆制动压力及驱动扭矩，极大程度上减少驾驶员控制加速踏板或制动踏板的频率，缓解驾驶疲劳，因此该功能的上车率正逐年增加。该算法核心逻辑为当自车行驶车道前方有可跟踪的目标车辆时，自车以一定的跟车时距跟随目标车辆行驶；若本车道内无可跟踪的目标车辆，则自车以预设的巡航速度行驶。

传统ACC对邻车道内车辆运动意图的预测方法主要分为两类，一类是只关注当前周期内传感器对他车的位姿及速度信息的监测来进行意图的判断，包含目标横向速度，横向加速度及与自车的相对横向距离，纵向距离等。一类是考虑连续几帧周期内他车相对位姿的变化来进行他车轨迹预测，进而获取他车的运动意图。以上两种策略均未考虑结合车辆自车行驶特性来进行他车的意图预测，这就使得在相对复杂的工况条件（如拥堵城市工况，弯道路况等）下出现ACC主目标筛选错误或者筛选不及时的情况，进一步影响ACC控制性能，出现误刹或减速不及时或制动剧烈或急加再急减等情况的发生，导致驾乘体验感降低，更严重的，在高速或者弯道等道路上行驶时错误的意图预判还存在相当的安全隐患。

发明内容

本发明要解决的技术问题是提供一种基于他车轨迹预测的车辆自适应巡航控制方法，能够解决现有技术的不足，提高自适应巡航控制系统的行驶安全性及舒适性。

为解决上述技术问题，本发明所采取的技术方案如下。

一种基于他车轨迹预测的车辆自适应巡航控制方法，包括以下步骤：

A、车道线数据拟合；

B、车道目标筛选；

C、目标轨迹预测；

D、跟踪目标信息输出。

作为优选，步骤A中，车道线数据拟合包括以下步骤，

A1、算法参数初始化；设置车道线拟合函数形式为一元三次多项式，

（1）

其中，x和y分别表示传感器识别到的车道线上的点在自车的车身坐标系下的纵向和横向坐标值，a、b、c和d为车道线拟合系数；

A2、车道线数据点输入；读取车道线传感器识别出的车道线点数据，将传感器坐标系转换成车身坐标系，并记录下对应车道线点数据，记为Data，

（2），

，，m表示每条识别到的车道线的点的个数，n表示识别的车道线的条数；

A3、最小二乘法拟合车道线；结合式（1）的拟合函数，考虑拟合函数与实际数据值的均方误差最小，对任一条识别到的车道线的均方误差表示为如下形式，

（3），

分别对系数求偏导得到，

（4），

整理式（4）得到任一条车道线最小二乘法拟合的表达形式，

（5），

据式（5）计算得到车道线拟合系数；

A4、识别当前自车行驶车道的左右侧车道线；在车道宽已知的情况下，结合拟合函数系数d的值识别出当前自车行驶车道的左右侧车道线系数，

（6），

其中，其中D为车道宽，和分别表示自车行驶车道对应的左右侧车道线的常数拟合系数，根据该系数得到其对应的车道线参数拟合系数，

（7），

和分别表示自车当前行驶车道的左侧，右侧车道线拟合系数；根据式（7）得到当前自车行驶车道的车道中心线的拟合函数，

（8），

其中，，，，。

作为优选，步骤B中，车道目标筛选包括以下步骤，

B1、获取目标传感器识别数据；读取目标识别传感器识别到的其他车辆信息，记录为ObsData，

（9），

其中，、、、、分别表示第n个识别到的车辆在当前自车的车身坐标系下的纵坐标、横坐标、与自车的相对速度、与自车的相对航向、识别到车辆的ID；

B2、区分其他行驶车辆所在车道；结合当前自车行驶车道的车道中心线拟合函数，如式（8）所示，识别出当前本车道及相邻左侧车道及相邻右侧车道内的其他行驶车辆信息；

B3、确定主目标车及次目标车；将本车道内与自车最近的其他行驶车辆作为主目标车，将相邻车道内与自车行驶方向一致的最近的其他行驶车辆作为次目标车。

作为优选，步骤C中，目标轨迹预测包括以下步骤，

C1、当本车道内主目标车存在且车速小于等于自车预设巡航速度时，

且当相邻车道内的次目标车存在且满足以下条件时，

（10），

其中，表示次目标车与自车的相对速度，表示次目标车与自车的相对纵向距离，表示主目标车与自车的相对纵向距离；

获取相邻车道内的次目标车在当前自车的车身坐标系下的信息，

（11），

其中，、、、、、分别表示次目标车的纵向坐标、横向坐标、纵向速度、横向速度、纵向加速度、横向加速度；

计算相邻的次目标车在自车的车道中心线上的投影点信息；找到该目标离车道中心线最近的点并记作匹配点，利用匹配点的信息计算次目标对应的投影点信息，包括投影点坐标，曲率及航向；

结合式（8）得到匹配点处路径横向对纵向的一阶导数及二阶导数，其表达形式如下，

（12），

由式（12）计算出匹配点处的航向角及匹配点处的曲率，

（13），

次目标车位姿与匹配点的纵向距离表示为，

（14），

综合式（13）、（14）计算出次目标车在车道中心线上投影点的航向、投影点的纵向坐标、横向坐标，

（15）；

将当前时刻下次目标车的位姿信息转换至以自车在本车道中心线的投影点为原点，车道中心线为纵轴s轴，正方向为自车行驶方向的自然坐标系；该自然坐标系上的点均遵循右手坐标系；插值找到当前次目标位置在自然坐标系上的纵向弧长，记作，计算次目标车偏离对应投影点的位移，

（16），

根据式（16）计算当前次目标车在自然坐标系下的速度，

（17），

计算当前次目标车横向偏移量对纵向弧长的一阶导数，

（18），

综合式（16）~（18）计算当前次目标车的纵向加速度，

（19），

根据式（17）~（19）得到当前次目标车横向偏离量对纵向弧长的二阶导数，

（20），

当前时刻次目标在所述自然坐标系上的坐标为，

（21）；

C2、当本车道内不存在主目标车或车速大于自车预设巡航速度时，

且相邻车道内的次目标车存在且满足以下条件，

（22），

其中表示预设的相邻车道内的次目标车与自车的最远距离，重复步骤C1的步骤，得到相邻车道内的次目标车在自然坐标系下的坐标信息；

C3、不满足步骤C1或C2的情况均输出空信息；

C4、预测次目标轨迹；由于车辆的横向运动均为纵向运动诱发的，预测次目标车在未来t秒内的运动轨迹；认为次目标车在终点状态与初始时刻的运动状态一致，由此得次目标在自然坐标系下的终点状态，

（23），

终点状态下的、、、与初始时刻相同；

由于横纵向终点状态均为未知量，因此纵向轨迹及横向轨迹均用4次多项式连接，

（24），

纵向轨迹终点状态，

（25），

其中，和均为函数参数。

作为优选，步骤D中，跟踪目标信息输出包括以下步骤，

结合式（25）判断相邻车道内的次目标在预测时域内的横向位置是否切入本车道，若存在切入本车道的次目标车，则将与自车最近的目标车作为跟踪目标，否则跟踪目标为本车道内的主目标车。

采用上述技术方案所带来的有益效果在于：本发明建立自车行驶车道的自然坐标系的基础上，充分利用传感器对他车的信息监测，通过将他车在自车的车身坐标系下的位姿状态及速度信息转化到所建立的自然坐标下，并结合车辆行为特性预测他车在未来时间内的运动轨迹，根据所预测的他车轨迹判断是否有切入本车行驶域的意图。本发明所提方法充分结合感知信息和车辆行驶特性预测未来时间内车辆的行驶轨迹，基于对他车的轨迹预测结果调整巡航控制策略，可在一定程度上提高驾乘舒适性及驾驶信心，此外，可进一步提高在复杂行驶工况下的行驶安全性。

附图说明

图1是本发明一个具体实施方式的流程图。

图2是本发明一个具体实施方式的详细框架图。

图3是单目标场景仿真测试场景示意图。

图4是相邻车道内车辆的轨迹预测结果。

图5a是相邻车道其他车辆速度信息。

图5b是相邻车道其他车辆加速度信息。

图6是相邻车道内其他车辆的横向位移信息。

图7a是单目标场景下本发明与传统方法自车与跟踪目标的相对纵向弧长对比。

图7b是单目标场景下本发明与传统方法的自车速度对比。

图7c是单目标场景下本发明与传统方法的自车加速度对比。

图8a是多目标场景下3辆其他车辆相对于自车的横向轨迹。

图8b是多目标场景下3辆其他车辆的加速度值。

图8c是多目标场景下3辆其他车辆的速度值。

图9a是多目标场景下本发明与传统方法的自车与跟踪目标的相对纵向弧长对比。

图9b是多目标场景下本发明与传统方法的自车速度对比。

图9c是多目标场景下本发明与传统方法的自车加速度对比。

图9d是多目标场景下3辆其他车辆相对自车的纵向位移。

具体实施方式

参照图1-图2，本实施例包括以下步骤：

A、车道线数据拟合；

A1、算法参数初始化；设置车道线拟合函数形式为一元三次多项式，

（1）

其中，x和y分别表示传感器识别到的车道线上的点在自车的车身坐标系下的纵向和横向坐标值，a、b、c和d为车道线拟合系数；

A2、车道线数据点输入；读取车道线传感器识别出的车道线点数据，将传感器坐标系转换成车身坐标系，并记录下对应车道线点数据，记为Data，

（2），

，，m表示每条识别到的车道线的点的个数，n表示识别的车道线的条数；

A3、最小二乘法拟合车道线；结合式（1）的拟合函数，考虑拟合函数与实际数据值的均方误差最小，对任一条识别到的车道线的均方误差表示为如下形式，

（3），

分别对系数求偏导得到，

（4），

整理式（4）得到任一条车道线最小二乘法拟合的表达形式，

（5），

据式（5）计算得到车道线拟合系数；

A4、识别当前自车行驶车道的左右侧车道线；在车道宽已知的情况下，结合拟合函数系数d的值识别出当前自车行驶车道的左右侧车道线系数，

（6），

其中，其中D为车道宽，和分别表示自车行驶车道对应的左右侧车道线的常数拟合系数，根据该系数得到其对应的车道线参数拟合系数，

（7），

和分别表示自车当前行驶车道的左侧，右侧车道线拟合系数；根据式（7）得到当前自车行驶车道的车道中心线的拟合函数，

（8），

其中，，，，。

B、车道目标筛选；

B1、获取目标传感器识别数据；读取目标识别传感器识别到的其他车辆信息，记录为ObsData，

（9），

其中，、、、、分别表示第n个识别到的车辆在当前自车的车身坐标系下的纵坐标、横坐标、与自车的相对速度、与自车的相对航向、识别到车辆的ID；

B2、区分其他行驶车辆所在车道；结合当前自车行驶车道的车道中心线拟合函数，如式（8）所示，识别出当前本车道及相邻左侧车道及相邻右侧车道内的其他行驶车辆信息；

B3、确定主目标车及次目标车；将本车道内与自车最近的其他行驶车辆作为主目标车，将相邻车道内与自车行驶方向一致的最近的其他行驶车辆作为次目标车。

C、目标轨迹预测；

C1、当本车道内主目标车存在且车速小于等于自车预设巡航速度时，

且当相邻车道内的次目标车存在且满足以下条件时，

（10），

其中，表示次目标车与自车的相对速度，表示次目标车与自车的相对纵向距离，表示主目标车与自车的相对纵向距离；

获取相邻车道内的次目标车在当前自车的车身坐标系下的信息，

（11），

其中，、、、、、分别表示次目标车的纵向坐标、横向坐标、纵向速度、横向速度、纵向加速度、横向加速度；

计算相邻的次目标车在自车的车道中心线上的投影点信息；找到该目标离车道中心线最近的点并记作匹配点，利用匹配点的信息计算次目标对应的投影点信息，包括投影点坐标，曲率及航向；

结合式（8）得到匹配点处路径横向对纵向的一阶导数及二阶导数，其表达形式如下，

（12），

由式（12）计算出匹配点处的航向角及匹配点处的曲率，

（13），

次目标车位姿与匹配点的纵向距离表示为，

（14），

综合式（13）、（14）计算出次目标车在车道中心线上投影点的航向、投影点的纵向坐标、横向坐标，

（15）；

将当前时刻下次目标车的位姿信息转换至以自车在本车道中心线的投影点为原点，车道中心线为纵轴s轴，正方向为自车行驶方向的自然坐标系；该自然坐标系上的点均遵循右手坐标系；插值找到当前次目标位置在自然坐标系上的纵向弧长，记作，计算次目标车偏离对应投影点的位移，

（16），

根据式（16）计算当前次目标车在自然坐标系下的速度，

（17），

计算当前次目标车横向偏移量对纵向弧长的一阶导数，

（18），

综合式（16）~（18）计算当前次目标车的纵向加速度，

（19），

根据式（17）~（19）得到当前次目标车横向偏离量对纵向弧长的二阶导数，

（20），

当前时刻次目标在所述自然坐标系上的坐标为，

（21）；

C2、当本车道内不存在主目标车或车速大于自车预设巡航速度时，

且相邻车道内的次目标车存在且满足以下条件，

（22），

其中表示预设的相邻车道内的次目标车与自车的最远距离，重复步骤C1的步骤，得到相邻车道内的次目标车在自然坐标系下的坐标信息；

C3、不满足步骤C1或C2的情况均输出空信息；

C4、预测次目标轨迹；由于车辆的横向运动均为纵向运动诱发的，预测次目标车在未来t秒内的运动轨迹；认为次目标车在终点状态与初始时刻的运动状态一致，由此得次目标在自然坐标系下的终点状态，

（23），

终点状态下的、、、与初始时刻相同；

由于横纵向终点状态均为未知量，因此纵向轨迹及横向轨迹均用4次多项式连接，

（24），

纵向轨迹终点状态，

（25），

其中，和均为函数参数。

D、跟踪目标信息输出；

结合式（25）判断相邻车道内的次目标在预测时域内的横向位置是否切入本车道，若存在切入本车道的次目标车，则将与自车最近的目标车作为跟踪目标，否则跟踪目标为本车道内的主目标车。

仿真测试

图3为测试场景一，如图所示，自车在当前道路最右侧车道行驶，自车相邻左侧车道内存在同向行驶的低速次目标车辆。自车预设巡航速度为90kph。根据自车感知系统信息，基于本发明算法思想，在某一时刻下，预测的相邻车道内的次目标车在未来T秒内与自车的相对位置轨迹如图4所示。从图中可明显看出，在未来T秒后该次目标将切入自车行驶车道，且相对纵向距离逐渐减小，表明次目标车速度小于自车行驶速度。图5a，图5b分别表示在整个运行周期内次目标车的速度信息和加速度信息。从图5a可以看出目标车的最大车速为54kph，远小于自车巡航车速90kph，综合图5a和图5b可以看出在整个仿真周期内，次目标车的速度先做匀速运动后连续做了两段较为激烈的变速运动减速至静止状态。图6为次目标车在以自车的车道中心线为纵轴的自然坐标系下，横向位移随时间的变化关系，综合图5a和图5b，图6 信息可知该次目标车从自车左侧车道低速切入后做变速运动减速至静止。此外，该次目标的初始位置与自车的相对纵向弧长为40m。图7a、图7b和图7c为使用本发明所提方法和传统跟踪目标方法自车与跟踪目标的纵向相对弧长，自车速度及自车加速度随时间变化的关系曲线。从图中可明显看出，采用本发明所提方法可提前将即将切入本车道的次目标选做跟踪目标，控制器可根据跟踪目标信息提前做出响应策略，由于切入的次跟踪目标速度远小于本车的巡航车速，在0.7秒时本发明所提方法预测出该次目标将切入本车道并将其选做自车的跟车目标并将相关信息发送给控制器，由于选中的跟踪目标速度远小于自车巡航车速，因此自车ACC控制器会更早的决策出减速指令，在整个测试过程中，自车与前车的距离均保持在相对安全的范围内。传统策略为当次目标切入本车道后再将其选做跟车目标，在当前测试场景下，在2秒时才将该侵入本车道的次目标车选为跟踪目标，此时自车与跟踪目标的相对纵向距离只有15m而自车处在巡航速度下，远大于跟踪目标速度，受限于ACC控制器能力，自车无法应对低速近距离切入的目标，在4秒左右时已然发生安全事故。

图8a、图8b和图8c为测试场景2，自车行驶车道及左右两侧车道内均存在其他行驶车辆且左右两侧车道内的其他车辆均低速切入本车道。在整个仿真周期内以自车行驶车道的车道中心线为轴，所述的其余3辆其他车辆的横向轨迹如图8a所示，目标车1切入时间早于目标车2，目标车3始终保持在自车行驶车道内行驶。所述3辆他车的加速度信息及速度信息分别如图8b，图8c所示，目标车3初始速度与自车巡航速度相同，随后减速至72kph保持一段时间后做变减速运动至静止状态。目标车1从54kph变加速切入至本车道内后做匀速运动，最后做变减速度运动。目标车2加速切入本车道内行驶，其匀速行驶速度小于目标车1。此外，目标车1，目标车2，目标车3的初始位置与自车的纵向距离分别是52m，38m，47m。图9a为基于本发明所提方法和传统方法下自车与跟踪目标的相对纵向弧长信息。基于本发明所提的算法其纵向相对弧长与自车的最短距离为10m左右，传统方法为3m左右，相比传统方法本发明所提方法在相同的场景下具备更大的安全距离阈量。图9b，图9c分别为基于本发明所提算法及传统算法自车的响应速度及响应加速度随时间的变化关系。结合图9a、图9b和图9c可看出本发明所提算法更早的将切入的低速目标车作为自车的跟踪目标，因此相较于传统策略会提前做出减速响应。图9d为基于本发明所提方法，所述测试场景下的其他车辆相对于自车相对纵向距离，结合图9a和图9d可看出基于本发明所提算法及传统算法的跟踪目标顺序是相同的，即，目标车3，目标车1，目标车2，主要区别在于本发明所提算法提前预测了目标车的切入时间，提前将切入目标作为跟踪目标并将其信息发给控制器。

在本发明的描述中，需要理解的是，术语“纵向”、“横向”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。

以上显示和描述了本发明的基本原理和主要特征和本发明的优点。本行业的技术人员应该了解，本发明不受上述实施例的限制，上述实施例和说明书中描述的只是说明本发明的原理，在不脱离本发明精神和范围的前提下，本发明还会有各种变化和改进，这些变化和改进都落入要求保护的本发明范围内。本发明要求保护范围由所附的权利要求书及其等效物界定。

Claims (1)

1.一种基于他车轨迹预测的车辆自适应巡航控制方法，其特征在于包括以下步骤：

A、车道线数据拟合；车道线数据拟合包括以下步骤，

A1、算法参数初始化；设置车道线拟合函数形式为一元三次多项式，

（1）

其中，x和y分别表示传感器识别到的车道线上的点在自车的车身坐标系下的纵向和横向坐标值，a、b、c和d为车道线拟合系数；

A2、车道线数据点输入；读取车道线传感器识别出的车道线点数据，将传感器坐标系转换成车身坐标系，并记录下对应车道线点数据，记为Data，

（2），

，，m表示每条识别到的车道线的点的个数，n表示识别的车道线的条数；

A3、最小二乘法拟合车道线；结合式（1）的拟合函数，考虑拟合函数与实际数据值的均方误差最小，对任一条识别到的车道线的均方误差表示为如下形式，

（3），

分别对系数求偏导得到，

（4），

整理式（4）得到任一条车道线最小二乘法拟合的表达形式，

（5），

据式（5）计算得到车道线拟合系数；

A4、识别当前自车行驶车道的左右侧车道线；在车道宽已知的情况下，结合拟合函数系数d的值识别出当前自车行驶车道的左右侧车道线系数，

（6），

其中，其中D为车道宽，和分别表示自车行驶车道对应的左右侧车道线的常数拟合系数，根据该系数得到其对应的车道线参数拟合系数，

（7），

和分别表示自车当前行驶车道的左侧，右侧车道线拟合系数；根据式（7）得到当前自车行驶车道的车道中心线的拟合函数，

（8），

其中，，，，；

B、车道目标筛选；车道目标筛选包括以下步骤，

B1、获取目标传感器识别数据；读取目标识别传感器识别到的其他车辆信息，记录为ObsData，

（9），

其中，、、 、、分别表示第n个识别到的车辆在当前自车的车身坐标系下的纵坐标、横坐标、与自车的相对速度、与自车的相对航向、识别到车辆的ID；

B2、区分其他行驶车辆所在车道；结合当前自车行驶车道的车道中心线拟合函数，如式（8）所示，识别出当前本车道及相邻左侧车道及相邻右侧车道内的其他行驶车辆信息；

B3、确定主目标车及次目标车；将本车道内与自车最近的其他行驶车辆作为主目标车，将相邻车道内与自车行驶方向一致的最近的其他行驶车辆作为次目标车；

C、目标轨迹预测；目标轨迹预测包括以下步骤，

C1、当本车道内主目标车存在且车速小于等于自车预设巡航速度时，

且当相邻车道内的次目标车存在且满足以下条件时，

（10），

其中，表示次目标车与自车的相对速度，表示次目标车与自车的相对纵向距离，表示主目标车与自车的相对纵向距离；

获取相邻车道内的次目标车在当前自车的车身坐标系下的信息，

（11），

其中，、、、、、分别表示次目标车的纵向坐标、横向坐标、纵向速度、横向速度、纵向加速度、横向加速度；

计算相邻的次目标车在自车的车道中心线上的投影点信息；找到该目标离车道中心线最近的点并记作匹配点，利用匹配点的信息计算次目标对应的投影点信息，包括投影点坐标，曲率及航向；

结合式（8）得到匹配点处路径横向对纵向的一阶导数及二阶导数，其表达形式如下，

（12），

由式（12）计算出匹配点处的航向角及匹配点处的曲率，

（13），

次目标车位姿与匹配点的纵向距离表示为，

（14），

综合式（13）、（14）计算出次目标车在车道中心线上投影点的航向、投影点的纵向坐标、横向坐标，

（15）；

将当前时刻下次目标车的位姿信息转换至以自车在本车道中心线的投影点为原点，车道中心线为纵轴s轴，正方向为自车行驶方向的自然坐标系；该自然坐标系上的点均遵循右手坐标系；插值找到当前次目标位置在自然坐标系上的纵向弧长，记作，计算次目标车偏离对应投影点的位移，

（16），

根据式（16）计算当前次目标车在自然坐标系下的速度，

（17），

计算当前次目标车横向偏移量对纵向弧长的一阶导数，

（18），

综合式（16）~（18）计算当前次目标车的纵向加速度，

（19），

根据式（17）~（19）得到当前次目标车横向偏离量对纵向弧长的二阶导数，

（20），

当前时刻次目标在所述自然坐标系上的坐标为，

（21）；

C2、当本车道内不存在主目标车或车速大于自车预设巡航速度时，

且相邻车道内的次目标车存在且满足以下条件，

（22），

其中表示预设的相邻车道内的次目标车与自车的最远距离，重复步骤C1的步骤，得到相邻车道内的次目标车在自然坐标系下的坐标信息；

C3、不满足步骤C1或C2的情况均输出空信息；

C4、预测次目标轨迹；由于车辆的横向运动均为纵向运动诱发的，预测次目标车在未来t秒内的运动轨迹；认为次目标车在终点状态与初始时刻的运动状态一致，由此得次目标在自然坐标系下的终点状态，

（23），

终点状态下的、 、、与初始时刻相同；

由于横纵向终点状态均为未知量，因此纵向轨迹及横向轨迹均用4次多项式连接，

（24），

纵向轨迹终点状态，

（25），

其中，和均为函数参数；

D、跟踪目标信息输出；跟踪目标信息输出包括以下步骤，

结合式（25）判断相邻车道内的次目标在预测时域内的横向位置是否切入本车道，若存在切入本车道的次目标车，则将与自车最近的目标车作为跟踪目标，否则跟踪目标为本车道内的主目标车。