CN118112935A - 车辆路径跟踪控制方法、装置、设备及存储介质 - Google Patents

Abstract

本申请提供一种车辆路径跟踪控制方法、装置、设备及存储介质，属于自动驾驶技术领域。该方法包括：得到配置参数；根据配置参数进行采样处理，得到采样数据；确定控制量‑时间关系[u,t]图；实时获取车辆在第一时刻的第一行驶信息和参考路径信息；初始化节点矩阵；更新节点矩阵；确定车辆的投影点为初始参考位姿；确定多个参考位姿；基于控制量迭代约束和状态转移方程，利用动态规划技术更新待解矩阵中每个节点的代价，得到目标控制序列；将目标控制序列中的第二个期望控制量转换为控制指令下发至车辆，以使车辆跟踪上参考路径信息。本申请能在降低技术实现成本的同时，提高路径跟踪控制的精度。

Description

车辆路径跟踪控制方法、装置、设备及存储介质

技术领域

本申请涉及车辆控制技术领域，尤其涉及自动驾驶技术领域，具体涉及一种车辆路径跟踪控制方法、装置、设备及存储介质。

背景技术

随着自动驾驶技术的高速发展，路径跟踪控制作为其核心模块，对最终的智能驾驶体验至关重要，因此路径跟踪控制也逐渐成为业内的研究热点。路径跟踪控制主要是通过跟踪上层规划好的参考路径，实现车辆的自主导航。控制的核心要求是高精确性与连续性，即在车辆物理约束的范围内，精确跟随上层规划的路径执行，且控制连续舒适。

目前，现有技术中，出于对技术实现成本的考虑，路径跟踪控制常用的技术为纯跟踪控制（pure pursuit,简称PP）。PP控制使用车辆的几何模型，对参考路径要求较低，在低速工况下表现出优异的性能。

然而，发明人发现，现有技术至少存在以下技术问题：在高速及大曲率路径等工况下存在控制精度较差的问题。

发明内容

本申请提供一种车辆路径跟踪控制方法、装置、设备及存储介质，能在降低技术实现成本的同时，提高路径跟踪控制的精度。

第一方面，本申请提供一种车辆路径跟踪控制方法，包括：

根据预设需求，进行参数配置处理，得到配置参数；

根据所述配置参数进行采样处理，得到采样数据，并根据所述采样数据，确定控制量-时间关系[u,t]图；

实时获取车辆在第一时刻的第一行驶信息和参考路径信息；

根据所述配置参数，初始化节点矩阵，其中所述节点矩阵包括多个节点，每个节点包括一组节点信息，并根据所述配置参数和所述第一时刻的上一时刻的控制指令，更新所述节点矩阵，得到第一个节点更新的待解矩阵；

根据所述第一行驶信息和参考路径信息，确定车辆的投影点为初始参考位姿；

根据预设运动模型、所述配置参数、所述第一行驶信息和所述待解矩阵，确定多个参考位姿，其中每个参考位姿按时间顺序对应所述[u,t]图中一个时刻；

基于控制量迭代约束和状态转移方程，利用动态规划技术更新所述待解矩阵中每个节点的代价，得到目标控制序列，其中所述目标控制序列包括多个按时间顺序排列的期望控制量，最后的期望控制量对应的节点的代价最小；

将所述目标控制序列中的第二个期望控制量转换为控制指令下发至车辆，以使所述车辆跟踪上所述参考路径信息，并返回所述实时获取车辆在第一时刻的第一行驶信息和参考路径信息的步骤。

在一种可能的实现方式中，所述配置参数包括预测时域、时间间隔、控制量间隔、最大控制量、最小控制量和最大控制速率；相应地，所述根据所述配置参数进行采样处理，得到采样数据，并根据所述采样数据，确定控制量-时间关系[u,t]图，包括：在所述预测时域内按所述时间间隔进行时间数据采样，得到多个时刻坐标；在所述最大控制量和所述最小控制量之间按所述控制量间隔进行控制量采样，得到多个控制量坐标，其中每个控制量坐标按时间顺序对应一个时刻坐标；根据多个时刻坐标和多个控制量坐标，确定[u,t]图。

在一种可能的实现方式中，所述配置参数包括控制量间隔；相应的，所述根据所述配置参数和所述第一时刻的上一时刻的控制指令，更新所述节点矩阵，得到第一个节点更新的待解矩阵，包括：根据所述第一时刻的上一时刻的控制指令，确定上一时刻的控制量；根据所述上一时刻的控制量和所述控制量间隔，确定第一时刻的控制量；根据所述第一时刻和所述第一时刻的控制量，确定节点矩阵中第一个节点的待更新矩阵信息；将所述待更新矩阵信息更新到所述节点矩阵中，得到待解矩阵。

在一种可能的实现方式中，所述配置参数还包括时间间隔，所述第一行驶信息包括所述车辆在第一时刻的速度、加速度、定位信息和车辆轴距，所述预设运动模型包括纵向模型和横向模型；相应地，所述根据预设运动模型、所述配置参数、所述第一行驶信息和所述待解矩阵，确定多个参考位姿，包括：根据所述车辆在第一时刻的速度、加速度和时间间隔和纵向模型，确定所述第一时刻之后的多个时刻的纵向距离的增量；根据所述定位信息，确定车辆在第一时刻的位姿信息；根据多个纵向距离的增量、所述车辆在第一个时刻的位姿信息、所述车辆轴距、所述待解矩阵和横向模型，确定所述第一时刻之后的多个时刻的参考位姿。

在一种可能的实现方式中，所述横向模型为运动学横向模型或动力学横向模型。

在一种可能的实现方式中，所述纵向模型为：

式中，i为时间方向的索引，为纵向距离的增量，为第一时刻之后的第i个 时刻的加速度，为第一时刻之后的第i个时刻的速度，OR表示纵向运动为匀速运动或匀 加速运动的运算结果，为配置参数中的时间间隔；

所述运动学横向模型为：

式中，i为时间方向的索引，为车辆位姿，其中x为车辆位姿的横向分 量，y为车辆位姿的纵向分量，为车辆位姿的偏航角分量，为第一时刻之后第i个时 刻对应的控制量，为车辆轴距。

在一种可能的实现方式中，所述配置参数包括最大控制速率和时间间隔；

相应地，所述控制量迭代约束为：

式中，为配置参数中的最大控制速率，i为时间方向的索引，j为控制 方向的索引，k为控制方向的索引增量，为节点u（i，j）对应的控制量，为配置参数 中的时间间隔；

状态转移方程为：

式中，为时间方向的索引，为控制方向的索引，为控制方向的索引增量，C(i +1,j+k)为待解矩阵中节点（i+1，j+k）的代价，C(i,j)为待解矩阵中节点u（i，j）的代价，为节点u（i，j）的跟踪误差代价，且：

式中，为参考路径信息， pos(i,j)为u（i，j）对应的车辆预测位姿，其中所述车辆预测位姿为基于所述预设运动模型推导所得，为跟踪误差权重，为pos(i,j)在参考路径上pos(ref)的投影误差的平方，基 于所述状态转移方程和所述控制量迭代约束，得到待解矩阵中的节点u（i，j）到节点u（i+1， j+k）的控制量变化代价，且：

式中，是时间方向的索引，是控制方向的索引，为控制方向的索引增量，为预设控制变化量权重，为控制变化量的平方。

在一种可能的实现方式中，还包括：实时获取车辆的上层规划发送的控制信息；当检测到所述车辆的上层规划发送的信息中不包含参考路径信息时，且所述车辆跟踪行驶完之前的参考路径信息时，则控制车辆停止。

第二方面，本申请提供一种车辆路径跟踪控制装置，包括：

参数配置模块，用于根据预设需求，进行参数配置处理，得到配置参数；

采样制图模块，用于根据所述配置参数进行采样处理，得到采样数据，并根据所述采样数据，确定控制量-时间关系[u,t]图；

获取模块，用于实时获取车辆在第一时刻的第一行驶信息和参考路径信息；

矩阵更新模块，用于根据所述配置参数，初始化节点矩阵，其中所述节点矩阵包括多个节点，每个节点包括一组节点信息，并根据所述配置参数和所述第一时刻的上一时刻的控制指令，更新所述节点矩阵，得到第一个节点更新的待解矩阵；

位姿确定模块，用于根据所述第一行驶信息和参考路径信息，确定车辆的投影点为初始参考位姿；

所述位姿确定模块，还用于根据预设运动模型、所述配置参数、所述第一行驶信息和所述待解矩阵，确定多个参考位姿，其中每个参考位姿按时间顺序对应所述[u,t]图中一个时刻；

动态规划模块，用于基于控制量迭代约束和状态转移方程，利用动态规划技术更新所述待解矩阵中每个节点的代价，得到目标控制序列，其中所述目标控制序列包括多个按时间顺序排列的期望控制量，最后的期望控制量对应的节点的代价最小；

路径跟踪控制模块，用于将所述目标控制序列中的第二个期望控制量转换为控制指令下发至车辆，以使所述车辆跟踪上所述参考路径信息，并返回所述实时获取车辆在第一时刻的第一行驶信息和参考路径信息的步骤。

第三方面，本申请提供了一种车辆路径跟踪控制设备，包括：至少一个处理器和存储器；

所述存储器存储计算机执行指令；

所述至少一个处理器执行所述存储器存储的计算机执行指令，使得所述至少一个处理器执行如上述第一方面描述的车辆路径跟踪控制方法。

第四方面，本申请提供了一种计算机可读存储介质，所述计算机可读存储介质中存储有计算机一项所执行指令，当处理器执行所述计算机执行指令时，实现如上述第一方面描述的车辆路径跟踪控制方法。

本申请提供的一种车辆路径跟踪控制方法、装置、设备及存储介质，先通过按预设需求设计配置参数，然后根据配置参数进行采样，形成控制量-时间关系[u,t]图以供后续的动态规划技术使用。再实时的获取车辆在第一时刻即当前时刻的第一行驶信息和参考路径信息。再根据配置参数，初始化节点矩阵，并利用第一时刻的上一时刻的控制指令更新节点矩阵，得到待解矩阵。再根据第一行驶信息和参考路径信息，确定初始参考位姿，并基于预设运动模型、配置参数、第一行驶信息和待解矩阵，确定未来时刻的多个参考位姿。利用可在低算力平台下高效运行的动态规划技术，兼容非线性的预设运动模型，并基于控制量迭代约束和状态转移方程，更新每个节点的代价，得到最后一个时刻对应的期望控制量的代价最小的目标控制序列，提高跟踪控制的精度，最终将能使跟踪误差最小的目标控制序列中的第二个时刻对应的期望控制量转换为控制指令下发至车辆，使车辆在车辆物理约束的范围内，以最快的速度准确的跟踪上参考路径，最终获得一条与参考路径误差越来越小的曲线，实现高精度的车辆路径跟踪控制的同时降低技术实现成本。

附图说明

为了更清楚地说明本申请实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作一简单地介绍，显而易见地，下面描述中的附图是本申请的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1为本申请实施例提供的车辆路径跟踪控制方法的应用场景示意图；

图2为本申请实施例提供的车辆路径跟踪控制方法的流程示意图；

图3为本申请实施例提供的控制量-时间关系[u,t]图；

图4为本申请实施例提供的车辆路径跟踪控制装置的结构示意图；

图5为本申请实施例提供的车辆路径跟踪控制设备的硬件结构示意图。

具体实施方式

为使本申请实施例的目的、技术方案和优点更加清楚，下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。

目前，在现有技术中，除了PP控制（pure pursuit，纯跟踪控制）外，还有MPC控制（model predictive control，模型预测控制）。发明人在使用MPC控制技术时，发现MPC控制由于其良好的跟踪预见性，能实现较高精度的控制，且场景适应性较强，在智驾领域也得到了一些发展，但MPC控制对硬件平台的要求较高，需要算力很高的平台才能支撑，因此技术实现成本较高。因此，亟需一种跟踪控制技术来实现技术实现成本低的同时，还能达到跟踪控制精度高的要求。

为解决上述技术问题，本申请实施例提供了以下解决问题的技术构思：首先，基于动态规划技术，显式的添加各种约束，调整各种代价函数，以满足各种车辆与路况的控制精度需求。而且可根据控制需求，调整离散化间隔及取值范围，进而调整算力消耗，使适应于部署在与PP控制技术一样的低算力平台上，降低技术实现成本。

图1为本申请实施例提供的车辆路径跟踪控制方法的应用场景示意图，如图1所示，包括：车辆101和车机端102。

其中，车辆101，可以是自动驾驶车，车辆路径跟踪控制的目的是为了通过跟踪上层规划好的参考路径，实现车辆101的自主导航。车辆101用于接收车机端102下发的控制指令，在车辆物理约束的范围内，精确的跟随上层规划的路径执行。车机端102，用于接收从上层规划如云端、其它服务器或移动端下发的参考路径信息，并获取车辆101的行驶信息，执行车辆路径跟踪控制方法，得到控制量并转换为控制指令下发给车辆101，以控制车辆在最快的速度准确的跟踪上参考路径。

图2为本申请实施例提供的车辆路径跟踪控制方法的流程示意图，本实施例的执行主体可以为图1所示实施例中的车机端102，也可以是其他的计算机的相关设备，对此实施例不作特别限制。

如图2所示，该车辆路径跟踪控制方法包括：

S201：根据预设需求，进行参数配置处理，得到配置参数。

本实施例中，预设需求指的是车辆路径跟踪控制需求，例如：预设需求可以是将控 制量的相关参数设置成等间隔控制或非等间隔控制。配置参数指的是与车辆路径跟踪控制 相关的控制参数。例如：本实施例中，得到的配置参数可以是预测时域、时间间隔；设 定最大控制量、最小控制量及控制量间隔等可以预先设定的参数。

具体的，在本申请一可选的实施例中，进行参数配置处理可以是设定预测时域、 时间间隔；设定最大控制量、最小控制量及控制量间隔（可以根据需求设 计成非等间隔），进而获取控制维度，设定最大控制速率，其中u为控制量，该控 制量可以是等效前轮偏角或方向盘转角。

本实施例中，预测时域可以是接收要控制的时间区域，时间间隔可以是控制时间 的增量，例如：=1秒，=0.1秒。最大控制量可以是等效前轮偏角或反向转角可控 制转动的最大角度值。例如：=30°。控制量间隔可以是控制量的递增量，例如：= 0.1°。控制维度为离散化采样时的数量，例如：=10×600。最大控制速率可以是 控制量在单位时间内的增长量，例如：=0.15°/0.1秒。

此外，根据需求信息，也可以设计成非等间隔的，例如：第一个0.1秒的控制量的递增量为0.1°，第二个0.1秒的控制量的递增量为0.15°，第三个0.1秒的控制量的递增量为0.2°。

S202：根据配置参数进行采样处理，得到采样数据，并根据采样数据，确定控制量-时间关系[u,t]图。

图3为本申请实施例提供的控制量-时间关系[u,t]图。

如图3所示，本实施例中，采样数据可以是[u,t]图中的坐标点中的横坐标和纵坐标。

以上述实施例为基础，在本申请一可选的实施例中，配置参数包括预测时域、时间间隔、控制量间隔、最大控制量、最小控制量和最大控制速率；相应地，步骤S202包括：

S202a：在预测时域内按时间间隔进行时间数据采样，得到多个时刻坐标。

S202b：在最大控制量和最小控制量之间按控制量间隔进行控制量采样，得到多个控制量坐标，其中每个控制量坐标按时间顺序对应一个时刻坐标。

S202c：根据多个时刻坐标和多个控制量坐标，确定[u,t]图。

本实施例中，[u,t]图可用于观察控制指令的变化趋势，[u,t]图的横坐标表示时 间，纵坐标表示控制量。时间数据采样和控制量采样可通过离散化的采样方式进行采样得 到。例如：对控制量u以控制量间隔进行离散化采样，离散化的数量可以根据控制维度 决定，将[-,]分为多个采样点采样，得到多个控制量坐标。同理，对时间t以时间 间隔进行离散化采样，将预测时域分为多个采样点采样，得到多个时刻坐标。根据多 个时刻坐标和多个控制量坐标，确定[u,t]图的方式可以是通过编程软件或绘图工具绘制 而成。例如：将离散化的多个时刻坐标和对应的多个控制量坐标整理成表格形式，然后使用 绘图软件将这些数据点绘制在二维平面上，这个绘图过程可以是自动完成的。

S203：实时获取车辆在第一时刻的第一行驶信息和参考路径信息。

本实施例中，第一时刻指的是车辆在向车辆下发控制指令之前的时刻，参考路径信息可以是上层规划下发的供车辆行驶的参考的路径点。第一行驶信息可以是与车辆相关的数据，例如：车辆的轴距、车辆的行驶速度和加速度。

S204：根据配置参数，初始化节点矩阵，其中节点矩阵包括多个节点，每个节点包括一组节点信息，并根据配置参数和第一时刻的上一时刻的控制指令，更新节点矩阵，得到第一个节点更新的待解矩阵。

本实施例中，节点矩阵可以是供动态规划技术的节点，节点信息可以包括控制量、 时间、代价及父亲节点等。待解矩阵指的是基于上一时刻的控制指令和配置参数，计算得到 第一个节点，并将第一个节点更新到节点矩阵中，得到第一个节点更新的待解矩 阵。

具体的，在本申请一可选的实施例中，步骤S204包括：

S204a：根据第一时刻的上一时刻的控制指令，确定上一时刻的控制量。

S204b：根据上一时刻的控制量和控制量间隔，确定第一时刻的控制量。

S204c：根据第一时刻和第一时刻的控制量，确定节点矩阵中第一个节点的待更新矩阵信息。

S204d：将待更新矩阵信息更新到节点矩阵中，得到待解矩阵。

本实施例中，第一时刻为，因为上一时刻的控制指令中包含上一时刻的控制量， 因此可从控制指令中识别出控制量。再根据上一时刻的控制量和控制量间隔，能得到 时刻的控制量，并得到初始化后的节点矩阵中的第一个节点。

S205：根据第一行驶信息和参考路径信息，确定车辆的投影点为初始参考位姿。

本实施例中，车辆在第一时刻的定位信息可以是位置坐标点，将该位置坐标点投 影到参考路径上，得到投影点，该投影点可以包括车辆在第一时刻的位移横向分量、位移纵 向分量和位移偏向角分量。例如：该投影点可以包括车辆在第一时刻的位姿x分量、位姿y分 量和偏航角分量。初始参考位姿对应的是待解矩阵中第一个节点对应的位姿。

S206：根据预设运动模型、配置参数、第一行驶信息和待解矩阵，确定多个参考位姿，其中每个参考位姿按时间顺序对应[u,t]图中一个时刻。

本实施例中，确定多个参考位姿的过程指的是以待解矩阵中的第一个节点对应的初始位姿为基础推导出的未来多个时刻对应的参考位姿。

具体的，以上述实施例为基础，在本申请一可选的实施例中，配置参数还包括时间间隔，第一行驶信息包括车辆在第一时刻的速度、加速度、定位信息和车辆轴距，预设运动模型包括纵向模型和横向模型，相应地，步骤S206包括：

S206a：根据车辆在第一时刻的速度、加速度和时间间隔和纵向模型，确定第一时刻之后的多个时刻的纵向距离的增量。

本实施例中，假设车辆在纵向上的移动为匀速运动或匀加速运动，则可根据纵向模型，计算出第一时刻之后的每个时刻的纵向距离增量、速度和加速度。

具体的，在本申请一可选的实施例中，步骤S206a中的纵向模型为：

式中，i为时间方向的索引，为纵向距离的增量，为第一时刻之后的第i个 时刻的加速度，为第一时刻之后的第i个时刻的速度，OR表示纵向运动为匀速运动或匀 加速运动的运算结果，为配置参数中的时间间隔。

本实施例中，当i=0时，则第一时刻对应的速度为，加速度为。由步骤S206a 中的公式可知，当车辆的纵向运动为匀速运动时，则下一个时刻对应的速度，，纵向距离增量。依次类推，可得到时刻、时刻…时 刻对应的纵向距离的增量。

S206b：根据定位信息，确定车辆在第一时刻的位姿信息。

S206c：根据多个纵向距离的增量、车辆在第一个时刻的位姿信息、车辆轴距、待解矩阵和横向模型，确定第一时刻之后的多个时刻的参考位姿。

本实施例中，通过使用横向模型，对多个纵向距离的增量、车辆在第一个时刻的位姿信息、车辆轴距和待解矩阵中包含的节点信息进行计算，可得到具体的参考位姿。

在本申请一可选的实施例中，横向模型可以是运动学横向模型或动力学横向模型。

具体的，在本申请一可选的实施例中，运动学横向模型为：

式中，i为时间方向的索引，为车辆位姿，其中x为车辆位姿的横向分 量，y为车辆位姿的纵向分量，为车辆位姿的偏航角分量，为第一时刻之后第i个时 刻对应的控制量，为车辆轴距。

本实施例中，T为采样周期即上述实施例中的。

S207：基于控制量迭代约束和状态转移方程，利用动态规划技术更新待解矩阵中每个节点的代价，得到目标控制序列，其中目标控制序列包括多个按时间顺序排列的期望控制量，最后的期望控制量对应的节点的代价最小。

本实施例中，利用动态规划技术的过程可以是将待解矩阵中的每个位置视为图上 的节点，然后对这些节点进行遍历，得到从第一个节点到最后一个节点之间路径最短的一 批节点。本实施例中，则是需要利用动态规划技术寻找待解矩阵中的最后一个节点的时刻代价最小的控制量。本实施例中，求解的过程受控制量迭代约束，对状态转移方程进 行求解，得到代价最小目标控制序列的结果。目标控制序列可以是 ，其中为求解得到的时刻代价最小的期望控制量，到之间的其它几个期望控制 量则为利用动态规划技术反向推导得出的。

在本申请一可选的实施例中，配置参数包括最大控制速率和时间间隔；相应地，控制量迭代约束为：

式中，为配置参数中的最大控制速率，i为时间方向的索引，j为控制 方向的索引，k为控制方向的索引增量，为节点u（i，j）对应的控制量，为配置参 数中的时间间隔；

状态转移方程为：

式中，为时间方向的索引，为控制方向的索引，为控制方向的索引增量，C(i +1,j+k)为待解矩阵中节点（i+1，j+k）的代价，C(i,j)为待解矩阵中节点u（i，j）的代价，为节点u（i，j）的跟踪误差代价，且：

式中，为参考路径信 息，pos(i,j)为u（i，j）对应的车辆预测位姿，其中车辆预测位姿为基于预设运动模型推导所得，为跟踪误差权重，为pos(i,j)在参考路径上pos(ref)的投影误差的平方，基 于所述状态转移方程和所述控制量迭代约束，得到待解矩阵中的节点u（i，j）到节点u（i+1， j+k）的控制量变化代价，且：

式中，是时间方向的索引，是控制方向的索引，为控制方向的索引增量，为预设控制变化量权重，为控制变化量的平方。

本实施例中，代价最小指的是跟踪误差代价、控制量变化代价和上一时刻的代价之和最小。其中，由状态转移方程可知，当跟踪误差代价、控制量变化代价和上一时刻的代价之和大于当前的时刻的代价时，则以当前的时刻的代价为最小。

S208：将目标控制序列中的第二个期望控制量转换为控制指令下发至车辆，以使车辆跟踪上参考路径信息，并返回实时获取车辆在第一时刻的第一行驶信息和参考路径信息的步骤。

本实施例中，目标控制序列中的第二个值对应上述实施例中的时刻对应的期望控制量/>。将期望控制量这一数值转换成控制指令后，即可下发至车辆，对车辆进行控制，使车辆的等效前轮偏角或方向盘转角转动到/>，以使车辆的实际行驶路径准确的跟踪上参考路径，之后再返回步骤S203以进行下一时刻的车辆跟踪控制过程。本实施例中，通过将目标控制序列中的第二个值对应的时刻/>对应的期望控制量/>转换为控制指令，提高路径跟踪的连续性和减小跟踪控制的时间间隔，提高了控制的稳定性和路径跟踪的准确性。

为使本申请实施例提供的车辆路径跟踪控制方法更容易被理解，以下将结合示例对该车辆路径跟踪控制方法进行说明。

第一步：根据预设需求，进行参数配置，设计成等间隔的控制。配置参数中，设定预 测时域为1秒，时间间隔为0.1秒，最大控制量为30°，最小控制量为0°，控制量间隔为0.1°， 控制维度=10×600，最大控制速率=0.15°/0.1秒。

第二步：对控制量u及时间t分别以0.1°和0.1秒进行离散化采样，得到、…、 和、、…、这些数据，基于这些数据可以得到、…、这些坐标点组成 的[u,t]图。

第三步：实时获取车辆在第一时刻的速度、加速度、定位信息和参考路径信 息。

第四步：根据第一步的配置参数，初始化节点矩阵，节点信息包括控制量、时间、代 价及父亲节点等，并基于上一时刻指令及控制量间隔0.1秒，计算，并更新节点矩 阵得到第一个节点为的待解矩阵。

第五步：根据定位信息，求出车辆在参考路径上的投影点，并作为时刻的参考位 姿；根据纵向模型，求出至对应的纵向距离至，进而根据横向模型求出至 的参考位姿至。

第六步：基于控制量迭代约束和状态转移方程，利用动态规划技术更新待解矩阵中每个节点的代价。

第七步：寻找时刻代价最小的期望控制量，再根据和动态规划技术反推 依次得出，并与一同组成目标控制序列[、、…、、 、]。

第八步：将目标控制序列中的第二个值转换为控制指令的形式，并下发至车辆，使车辆跟踪上参考路径。并返回第三步。

综上，本申请实施例提供的车辆路径跟踪控制方法，通过按预设需求进行参数配置处理，得到配置参数，然后根据配置参数进行采样，形成控制量-时间关系[u,t]图供后续的动态规划技术使用。再实时的获取车辆在第一时刻即当前时刻的第一行驶信息和参考路径信息。再根据配置参数，初始化节点矩阵，并利用第一时刻的上一时刻的控制指令更新节点矩阵，得到待解矩阵。再根据第一行驶信息和参考路径信息，确定初始参考位姿，并基于预设运动模型、配置参数、第一行驶信息和待解矩阵，确定未来时刻的多个参考位姿。利用可在低算力平台下高效运行的动态规划技术，兼容预设运动模型，并基于控制量迭代约束和状态转移方程，更新待解矩阵中每个节点的代价，得到最后一个时刻对应的期望控制量的代价最小的目标控制序列，提高跟踪控制的精度，最终将能使跟踪误差最小的第二期望控制量转换为控制指令下发至车辆，以完成车辆路径跟踪控制，再进行下次控制指令的下发过程。在实现更高精度的跟踪控制的同时，还能在低算力平台下运行，进而降低技术实现成本。

以上述实施例为基础，在本申请一可选的实施例中提供的车辆路径跟踪控制方法，还包括：

步骤A：实时获取车辆的上层规划发送的控制信息。

步骤B：当检测到车辆的上层规划发送的信息中不包含参考路径信息时，且车辆跟踪行驶完之前的参考路径信息时，则控制车辆停止。

本实施例中，上层规划发送的控制信息可以是参考路径和改变车辆行驶状态的指令信息。本实施例中的上层可以是云端或与车辆的车机端通信连接的远程控制端、移动端或人工智能设备等终端。当车机端检测到上层规划发送的控制信息不包含参考路径信息时，则表示车辆无需在进行路径跟踪控制，此时可控制车辆停止。

图4为本申请实施例提供的车辆路径跟踪控制装置的结构示意图，该装置包括：参数配置模块41、采样制图模块42、获取模块43、矩阵更新模块44、位姿确定模块45、动态规划模块46和路径跟踪控制模块47。

参数配置模块41，用于根据预设需求，进行参数配置处理，得到配置参数。

采样制图模块42，用于根据配置参数进行采样处理，得到采样数据，并根据采样数据，确定控制量-时间关系[u,t]图。

获取模块43，用于实时获取车辆在第一时刻的第一行驶信息和参考路径信息。

矩阵更新模块44，用于根据配置参数，初始化节点矩阵，其中节点矩阵包括多个节点，每个节点包括一组节点信息，并根据配置参数和第一时刻的上一时刻的控制指令，更新节点矩阵，得到第一个节点更新的待解矩阵。

位姿确定模块45，用于根据第一行驶信息和参考路径信息，确定车辆的投影点为初始参考位姿。

位姿确定模块45，还用于根据预设运动模型、配置参数、第一行驶信息和待解矩阵，确定多个参考位姿，其中每个参考位姿按时间顺序对应[u,t]图中一个时刻。

动态规划模块46，用于基于控制量迭代约束和状态转移方程，利用动态规划技术更新待解矩阵中每个节点的代价，得到目标控制序列，其中目标控制序列包括多个按时间顺序排列的期望控制量，最后的期望控制量对应的节点的代价最小。

路径跟踪控制模块47，用于将目标控制序列中的第二个期望控制量转换为控制指令下发至车辆，以使车辆跟踪上参考路径信息，并返回实时获取车辆在第一时刻的第一行驶信息和参考路径信息的步骤。

在本申请一可选的实施例中，配置参数包括预测时域、时间间隔、控制量间隔、最大控制量、最小控制量和最大控制速率；相应地，采样制图模块42，具体用于：

在预测时域内按时间间隔进行时间数据采样，得到多个时刻坐标；在最大控制量和最小控制量之间按控制量间隔进行控制量采样，得到多个控制量坐标，其中每个控制量坐标按时间顺序对应一个时刻坐标；根据多个时刻坐标和多个控制量坐标，确定[u,t]图。

在本申请一可选的实施例中，配置参数包括控制量间隔；相应地，矩阵更新模块44，具体用于：根据第一时刻的上一时刻的控制指令，确定上一时刻的控制量；根据上一时刻的控制量和控制量间隔，确定第一时刻的控制量；根据第一时刻和第一时刻的控制量，确定节点矩阵中第一个节点的待更新矩阵信息；将待更新矩阵信息更新到节点矩阵中，得到待解矩阵。

在本申请一可选的实施例中，配置参数还包括时间间隔，第一行驶信息包括车辆在第一时刻的速度、加速度、定位信息和车辆轴距，预设运动模型包括纵向模型和横向模型；相应地，位姿确定模块45，具体用于：根据车辆在第一时刻的速度、加速度和时间间隔和纵向模型，确定第一时刻之后的多个时刻的纵向距离的增量；根据定位信息，确定车辆在第一时刻的位姿信息；根据多个纵向距离的增量、车辆在第一个时刻的位姿信息、车辆轴距、待解矩阵，确定第一时刻之后的多个时刻的参考位姿。

在本申请一可选的实施例中，位姿确定模块45中使用的横向模型为运动学横向模型或动力学横向模型。

在本申请一可选的实施例中，位姿确定模块45中使用的，纵向模型为：

式中，i为时间方向的索引，为纵向距离的增量，为第一时刻之后的第i个 时刻的加速度，为第一时刻之后的第i个时刻的速度，OR表示纵向运动为匀速运动或匀 加速运动的运算结果，为配置参数中的时间间隔；

运动学横向模型为：

式中，i为时间方向的索引，为车辆位姿，其中x为车辆位姿的横向分 量，y为车辆位姿的纵向分量，为车辆位姿的偏航角分量，为第一时刻之后第i个时 刻对应的控制量，为车辆轴距。

在本申请一可选的实施例中，配置参数包括最大控制速率和时间间隔。相应地，动态规划模块46中使用的，控制量迭代约束为：

式中，为配置参数中的最大控制速率，i为时间方向的索引，j为控制 方向的索引，k为控制方向的索引增量，为节点u（i，j）对应的控制量，为配置参 数中的时间间隔；

状态转移方程为：

式中，为时间方向的索引，为控制方向的索引，为控制方向的索引增量，C(i +1,j+k)为待解矩阵中节点（i+1，j+k）的代价，C(i,j)为待解矩阵中节点u（i，j）的代价，为节点u（i，j）的跟踪误差代价，且：

式中，为参考路径 信息，pos(i,j)为u（i，j）对应的车辆预测位姿，其中车辆预测位姿为基于预设运动模型推导所得，为跟踪误差权重，为pos(i,j)在参考路径上pos(ref)的投影误差的平方，基 于状态转移方程和控制量迭代约束，得到待解矩阵中的节点u（i，j）到节点u（i+1，j+k）的控 制量变化代价，且：

式中，是时间方向的索引，是控制方向的索引，为控制方向的索引增量，为预设控制变化量权重，为控制变化量的平方。

在本申请一可选的实施例中，获取模块42，还用于：实时获取车辆的上层规划发送的控制信息。路径跟踪控制模块47，还用于：当检测到车辆的上层规划发送的信息中不包含参考路径信息时，且车辆跟踪行驶完之前的参考路径信息时，则控制车辆停止。

本实施例提供的车辆路径跟踪控制装置，可用于执行上述方法实施例的技术方案，其实现原理和技术效果类似，本实施例此处不再赘述。

图5为本申请实施例提供的车辆路径跟踪控制设备的硬件结构示意图，如图5所示，该设备统包括：至少一个处理器501以及存储器502。

其中，存储器502，用于存储计算机执行指令。

处理器501，用于执行存储器502存储的计算机执行指令，以实现上述方法实施例中所涉及的各个步骤。具体可以参见前述方法实施例中的相关描述。

可选的，存储器502既可以是独立的，也可以跟处理器501集成在一起。

当存储器502独立设置时，该设备还包括总线503，用于连接存储器502和处理器501。

本申请实施例还提供一种计算机可读存储介质，计算机可读存储介质中存储有计算机执行指令，当处理器执行计算机执行指令时，实现如上的车辆路径跟踪控制方法。

本申请实施例还提供一种计算机程序产品，包括计算机程序，该计算机程序被处理器执行时实现如上的车辆路径跟踪控制方法。

在本申请所提供的几个实施例中，应该理解到，所揭露的装置和方法，可以通过其它的方式实现。例如，以上所描述的装置实施例仅仅是示意性的，例如，上述模块的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个模块可以结合或者可以集成到另一个***，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，装置或模块的间接耦合或通信连接，可以是电性，机械或其它的形式。

上述作为分离部件说明的模块可以是或者也可以不是物理上分开的，作为模块显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案。

另外，在本发明各个实施例中的各功能模块可以集成在一个处理单元中，也可以是各个模块单独物理存在，也可以两个或两个以上模块集成在一个单元中。上述模块组成的单元既可以采用硬件的形式实现，也可以采用硬件加软件功能单元的形式实现。

上述以软件功能模块的形式实现的集成的模块，可以存储在一个计算机可读取存储介质中。上述软件功能模块存储在一个存储介质中，包括若干指令用以使得一台计算机设备（可以是个人计算机，服务器，或者网络设备等）或处理器执行本申请各个实施例的方法的部分步骤。

应理解，上述处理器可以是中央处理单元（Central Processing Unit，简称CPU），还可以是其他通用处理器、数字信号处理器（DigitalSignal Processor，简称DSP）、专用集成电路（Application Specific Integrated Circuit，简称ASIC）等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。结合发明所公开的方法的步骤可以直接体现为硬件处理器执行完成，或者用处理器中的硬件及软件模块组合执行完成。

存储器可能包含高速RAM存储器，也可能还包括非易失性存储NVM，例如至少一个磁盘存储器，还可以为U盘、移动硬盘、只读存储器、磁盘或光盘等。

总线可以是工业标准体系结构（Industry Standard Architecture，简称ISA）总线、外部设备互连（Peripheral Component Interconnect，简称PCI）总线或扩展工业标准体系结构（ExtendedIndustry Standard Architecture，简称EISA）总线等。总线可以分为地址总线、数据总线、控制总线等。为便于表示，本申请附图中的总线并不限定仅有一根总线或一种类型的总线。

上述存储介质可以是由任何类型的易失性或非易失性存储设备或者它们的组合实现，如静态随机存取存储器（SRAM），电可擦除可编程只读存储器（EEPROM），可擦除可编程只读存储器（EPROM），可编程只读存储器（PROM），只读存储器（ROM），磁存储器，快闪存储器，磁盘或光盘。存储介质可以是通用或专用计算机能够存取的任何可用介质。

一种示例性的存储介质耦合至处理器，从而使处理器能够从该存储介质读取信息，且可向该存储介质写入信息。当然，存储介质也可以是处理器的组成部分。处理器和存储介质可以位于专用集成电路(Application Specific Integrated Circuits，简称ASIC)中。当然，处理器和存储介质也可以作为分立组件存在于电子设备或主控设备中。

本领域普通技术人员可以理解：实现上述各方法实施例的全部或部分步骤可以通过程序指令相关的硬件来完成。前述的程序可以存储于一计算机可读取存储介质中。该程序在执行时，执行包括上述各方法实施例的步骤；而前述的存储介质包括：ROM、RAM、磁碟或者光盘等各种可以存储程序代码的介质。

以说明本申请的技术方案，而非对其限制；尽管参照前述各实施例对本申请进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本申请各实施例技术方案的范围。

Claims (11)

1.一种车辆路径跟踪控制方法，其特征在于，包括：

根据预设需求，进行参数配置处理，得到配置参数；

根据所述配置参数进行采样处理，得到采样数据，并根据所述采样数据，确定控制量-时间关系[u,t]图；

实时获取车辆在第一时刻的第一行驶信息和参考路径信息；

根据所述配置参数，初始化节点矩阵，其中所述节点矩阵包括多个节点，每个节点包括一组节点信息，并根据所述配置参数和所述第一时刻的上一时刻的控制指令，更新所述节点矩阵，得到第一个节点更新的待解矩阵；

根据所述第一行驶信息和参考路径信息，确定车辆的投影点为初始参考位姿；

根据预设运动模型、所述配置参数、所述第一行驶信息和所述待解矩阵，确定多个参考位姿，其中每个参考位姿按时间顺序对应所述[u,t]图中一个时刻；

基于控制量迭代约束和状态转移方程，利用动态规划技术更新所述待解矩阵中每个节点的代价，得到目标控制序列，其中所述目标控制序列包括多个按时间顺序排列的期望控制量，最后的期望控制量对应的节点的代价最小；

将所述目标控制序列中的第二个期望控制量转换为控制指令下发至车辆，以使所述车辆跟踪上所述参考路径信息，并返回所述实时获取车辆在第一时刻的第一行驶信息和参考路径信息的步骤。

2.根据权利要求1所述的方法，其特征在于，所述配置参数包括预测时域、时间间隔、控制量间隔、最大控制量、最小控制量和最大控制速率；

相应地，所述根据所述配置参数进行采样处理，得到采样数据，并根据所述采样数据，确定控制量-时间关系[u,t]图，包括：

在所述预测时域内按所述时间间隔进行时间数据采样，得到多个时刻坐标；

在所述最大控制量和所述最小控制量之间按所述控制量间隔进行控制量采样，得到多个控制量坐标，其中每个控制量坐标按时间顺序对应一个时刻坐标；

根据多个时刻坐标和多个控制量坐标，确定[u,t]图。

3.根据权利要求1所述的方法，其特征在于，所述配置参数包括控制量间隔；

相应的，所述根据所述配置参数和所述第一时刻的上一时刻的控制指令，更新所述节点矩阵，得到第一个节点更新的待解矩阵，包括：

根据所述第一时刻的上一时刻的控制指令，确定上一时刻的控制量；

根据所述上一时刻的控制量和所述控制量间隔，确定第一时刻的控制量；

根据所述第一时刻和所述第一时刻的控制量，确定节点矩阵中第一个节点的待更新矩阵信息；

将所述待更新矩阵信息更新到所述节点矩阵中，得到待解矩阵。

4.根据权利要求3所述的方法，其特征在于，所述配置参数还包括时间间隔，所述第一行驶信息包括所述车辆在第一时刻的速度、加速度、定位信息和车辆轴距，所述预设运动模型包括纵向模型和横向模型；

相应地，所述根据预设运动模型、所述配置参数、所述第一行驶信息和所述待解矩阵，确定多个参考位姿，包括：

根据所述车辆在第一时刻的速度、加速度和时间间隔和纵向模型，确定所述第一时刻之后的多个时刻的纵向距离的增量；

根据所述定位信息，确定车辆在第一时刻的位姿信息；

根据多个纵向距离的增量、所述车辆在第一个时刻的位姿信息、所述车辆轴距、所述待解矩阵和横向模型，确定所述第一时刻之后的多个时刻的参考位姿。

5.根据权利要求4所述的方法，其特征在于，所述横向模型为运动学横向模型或动力学横向模型。

6.根据权利要求5所述的方法，其特征在于，所述纵向模型为：

；

式中，i为时间方向的索引，为纵向距离的增量，/>为第一时刻之后的第i个时刻的加速度，/>为第一时刻之后的第i个时刻的速度，OR表示纵向运动为匀速运动或匀加速运动的运算结果，/>为配置参数中的时间间隔；

所述运动学横向模型为：

；

式中，i为时间方向的索引，为车辆位姿，其中x为车辆位姿的横向分量，y为车辆位姿的纵向分量，/>为车辆位姿的偏航角分量，/>为第一时刻之后第i个时刻对应的控制量，/>为车辆轴距。

7.根据权利要求1所述的方法，其特征在于，所述配置参数包括最大控制速率和时间间隔；

相应地，所述控制量迭代约束为：

；

式中，为配置参数中的最大控制速率，i为时间方向的索引，j为控制方向的索引，k为控制方向的索引增量，/>为节点u（i，j）对应的控制量，/>为配置参数中的时间间隔；

状态转移方程为：

；

式中，为时间方向的索引，/>为控制方向的索引，/>为控制方向的索引增量，C(i+1,j+k)为待解矩阵中节点（i+1，j+k）的代价，C(i,j)为待解矩阵中节点u（i，j）的代价，为节点u（i，j）的跟踪误差代价，且：

；

式中，为参考路径信息，pos(i,j)为u（i，j）对应的车辆预测位姿/>，其中所述车辆预测位姿为基于所述预设运动模型推导所得，/>为跟踪误差权重，为pos(i,j)在参考路径上pos(ref)的投影误差的平方，基于所述状态转移方程和所述控制量迭代约束，得到待解矩阵中的节点u（i，j）到节点u（i+1，j+k）的控制量变化代价/>，且：；

式中，是时间方向的索引，/>是控制方向的索引，/>为控制方向的索引增量，/>为预设控制变化量权重，/>为控制变化量的平方。

8.根据权利要求1至6任一项所述的方法，其特征在于，还包括：

实时获取车辆的上层规划发送的控制信息；

当检测到所述车辆的上层规划发送的信息中不包含参考路径信息时，且所述车辆跟踪行驶完之前的参考路径信息时，则控制车辆停止。

9.一种车辆路径跟踪控制装置，其特征在于，包括：

参数配置模块，用于根据预设需求，进行参数配置处理，得到配置参数；

采样制图模块，用于根据所述配置参数进行采样处理，得到采样数据，并根据所述采样数据，确定控制量-时间关系[u,t]图；

获取模块，用于实时获取车辆在第一时刻的第一行驶信息和参考路径信息；

矩阵更新模块，用于根据所述配置参数，初始化节点矩阵，其中所述节点矩阵包括多个节点，每个节点包括一组节点信息，并根据所述配置参数和所述第一时刻的上一时刻的控制指令，更新所述节点矩阵，得到第一个节点更新的待解矩阵；

位姿确定模块，用于根据所述第一行驶信息和参考路径信息，确定车辆的投影点为初始参考位姿；

所述位姿确定模块，还用于根据预设运动模型、所述配置参数、所述第一行驶信息和所述待解矩阵，确定多个参考位姿，其中每个参考位姿按时间顺序对应所述[u,t]图中一个时刻；

动态规划模块，用于基于控制量迭代约束和状态转移方程，利用动态规划技术更新所述待解矩阵中每个节点的代价，得到目标控制序列，其中所述目标控制序列包括多个按时间顺序排列的期望控制量，最后的期望控制量对应的节点的代价最小；

路径跟踪控制模块，用于将所述目标控制序列中的第二个期望控制量转换为控制指令下发至车辆，以使所述车辆跟踪上所述参考路径信息，并返回所述实时获取车辆在第一时刻的第一行驶信息和参考路径信息的步骤。

10.一种车辆路径跟踪控制设备，其特征在于，包括：至少一个处理器和存储器；

所述存储器存储计算机执行指令；

所述至少一个处理器执行所述存储器存储的计算机执行指令，使得所述至少一个处理器执行如权利要求1至8任一项所述的车辆路径跟踪控制方法。

11.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质中存储有计算机执行指令，当处理器执行所述计算机执行指令时，实现如权利要求1至8任一项所述的车辆路径跟踪控制方法。