CN117250957A - 路径跟踪控制关键参考点选取方法、装置、设备和介质 - Google Patents

Abstract

本发明涉及一种路径跟踪控制关键参考点选取方法、装置、设备和介质，所述方法包括：在初始控制周期中，以移动装备与参考路径上的所有点的距离最小值对应的点作为关键参考点；在其它控制周期中，包括如下步骤：读取上一控制周期的关键参考点，以该点作为当前控制周期的搜索基准点，并基于该点确定基准距离；将当前控制周期的基准点之后的每一点与移动装备之间的距离与基准距离进行判断，得到该当前控制周期的关键参考点。本发明可以在等间隔离散点列的参考路径中实时、有效的获取关键参考点，解决现有技术选取关键参考点计算成本高、实时性较差的问题。

Description

路径跟踪控制关键参考点选取方法、装置、设备和介质

技术领域

本发明属于自主驾驶控制领域，具体涉及一种路径跟踪控制关键参考点选取方法、装置、设备和介质。

背景技术

路径跟踪控制，即是移动机器人、无人驾驶车辆等移动装备按照参考路径行驶的控制过程，是自主行驶控制系统的关键技术，其精度和可靠性直接影响到移动装备行驶的安全性。纯跟踪算法、模型预测控制等路径跟踪算法都需要在参考路径中选取关键参考点(即参考路径中距离移动装备最近的路径点)。目前主要有两种方法获取关键参考点，一种是基于几何关系的选取方法，另一种是基于全局最优的选取方法。

对于基于几何关系的选取方法，要求参考路径以参数方程给出，但当参考路径较为复杂时，参考路径通常设置为等间隔点列形式，该方法无法有效获取关键参考点。而基于全局最优的选取方法会遍历整个参考路径，获取与移动机器人最近的路径点，所以当参考路径较长时，会耗费大量地计算资源，使得路径跟踪控制的实时性恶化。现有文献中提出了一种关键参考点选取方法，通过在搜索基准点前后扩增，确定搜索范围，代替全局遍历，但是其计算量仍然较大，而且当移动装备距离参考路径较远时，难以确定初始关键参考点。

发明内容

为了克服现有技术存在的上述问题，本发明提供一种路径跟踪控制关键参考点选取方法、装置、设备和介质，用于目前存在的技术问题。

一种路径跟踪控制关键参考点选取方法，用于移动装备，所述方法包括：

S1.在初始控制周期中，以移动装备与参考路径上的所有点的距离最小值对应的点作为关键参考点；

在其它控制周期中，包括如下步骤：

S2.读取上一控制周期的关键参考点，以该点作为当前控制周期的搜索基准点，并基于该点确定基准距离；

S3.将当前控制周期的基准点之后的每一点与移动装备之间的距离与基准距离进行判断，得到该当前控制周期的关键参考点。

如上所述的方面和任一可能的实现方式，进一步提供一种实现方式，所述S1包括：

S11.给出点列形式的参考路径；

S12.确定移动装备与参考路径中的每个点之间的距离；

S13.将所有距离中最短距离对应的点作为关键参考点。

如上所述的方面和任一可能的实现方式，进一步提供一种实现方式，所述基准距离为所述基准点至所述移动装备当前位置的距离。

如上所述的方面和任一可能的实现方式，进一步提供一种实现方式，所述S12具体包括：参考路径以数组形式表示为：

式中，x_i表示第i个参考路径点在全局坐标系中的横坐标，y_i表示第i个参考路径点在全局坐标系中的纵坐标，n表示参考路径点的个数，所述移动装备与参考路径中的每个点之间的距离通过如下公式得到：

式中，x_a和y_a表示当前控制周期中移动装备实际位置的横坐标和纵坐标。

如上所述的方面和任一可能的实现方式，进一步提供一种实现方式，所述S3具体包括：

S31.计算所述基准点的下一点与所述移动装备当前位置的距离；

S32.若该距离小于等于该基准距离，则继续搜索，即计算下一点与所述移动装备当前位置的距离；

S33.若继续搜索得到的点与基准点的距离大于基准距离，则将基准距离对应的点作为关键参考点。

如上所述的方面和任一可能的实现方式，进一步提供一种实现方式，所述S32或者为：若该距离大于该基准距离，则将基准距离对应的点作为关键参考点。

如上所述的方面和任一可能的实现方式，进一步提供一种实现方式，所述基准点的下一点与所述移动装备当前位置的距离为：

其中，x_j+1表示第j+1个参考路径点在全局坐标系中的横坐标，y_j+1表示第j+1个参考路径点在全局坐标系中的纵坐标，x_a和y_a表示当前控制周期中移动装备实际位置的横坐标和纵坐标，j＝1,2,…,n。

本发明还提供了一种路径跟踪控制关键参考点选取装置，所述装置用于实现所述的方法，包括：

第一选取模块，用于在初始控制周期中，以移动装备与参考路径上的所有点的距离最小值对应的点作为关键参考点；

确定模块，用于在其它控制周期中，读取上一控制周期的关键参考点，以该点作为当前控制周期的搜索基准点，并基于该点确定基准距离；

第二选取模块，用于将当前控制周期的基准点之后的每一点与移动装备之间的距离与基准距离进行判断，得到该当前控制周期的关键参考点。

本发明还提供了一种计算机存储介质，所述介质上存储有计算机程序，所述计算机程序被处理器执行实现所述的方法。

本发明还提供了一种电子设备，所述电子设备包括：

存储器，存储有可执行指令；

处理器，所述处理器运行所述存储器中的所述可执行指令，以实现所述的方法。

本发明的有益效果

与现有技术相比，本发明有如下有益效果：

本发明的路径跟踪控制关键参考点选取方法，用于移动装备，所述方法包括：在初始控制周期中，以移动装备与参考路径上的所有点的距离最小值对应的点作为关键参考点；在其它控制周期中，包括如下步骤：读取上一控制周期的关键参考点，以该点作为当前控制周期的搜索基准点，并基于该点确定基准距离；将当前控制周期的基准点之后的每一点与移动装备之间的距离与基准距离进行判断，得到该当前控制周期的关键参考点。本发明可以在等间隔离散点列的参考路径中实时、有效的获取关键参考点，解决现有技术选取关键参考点计算成本高、实时性较差的问题。

附图说明

图1为本发明的方法流程图。

具体实施方式

为了更好的理解本发明的技术方案，本发明内容包括但不限于下文中的具体实施方式，相似的技术和方法都应该视为本发明保护的范畴之内。为使本发明要解决的技术问题、技术方案和优点更加清楚，下面将结合附图及具体实施例进行详细描述。

应当明确，本发明所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。

在本发明实施例中使用的术语是仅仅出于描述特定实施例的目的，而非旨在限制本发明。在本发明实施例和所附权利要求书中所使用的单数形式的“一种”、“所述”和“该”也旨在包括多数形式，除非上下文清楚地表示其他含义。

如图1所示，本发明的一种路径跟踪控制关键参考点选取方法，用于移动装备，所述方法包括：

S1.在初始控制周期中，以移动装备与参考路径上的所有点的距离最小值对应的点作为关键参考点；

在其它控制周期中，包括如下步骤：

S2.读取上一控制周期的关键参考点，以该点作为当前控制周期的搜索基准点，并基于该点确定基准距离；

S3.将当前控制周期的基准点之后的每一点与移动装备之间的距离与基准距离进行判断，得到该当前控制周期的关键参考点。

本发明的具体实现过程如下：

对于路径跟踪控制而言，初始控制周期的控制量还未发送给执行机构，特别是速度信号还未发送给执行机构，即在初始控制周期中，作为移动装备的移动机器人或无人驾驶车辆的行驶速度为零，因此在这个控制周期中，对移动装备的控制器的实时性要求很低，故可以设置与其他控制周期不同的关键参考点选取算法，以解决难以确定初始关键参考点的问题，初始控制周期指的是移动装备刚启动时的第一个控制周期，其他的控制周期为非初始控制周期。

首先，当路径规划系统给出的参考路径，其可以为直线，也可以为曲线，本发明不作限定，参考路径为点列形式时，其上包括多个路径参考点，即数组形式：

式中，x_i表示全局坐标系第i个参考路径点中的横坐标，y_i表示全局坐标系中第i个参考路径点的纵坐标，n表示参考路径点的个数，n为正整数。

然后，计算移动装备在初始控制周期中与参考路径各点之间的距离：

式中，x_a和y_a表示当前控制周期中移动装备实际位置的横坐标和纵坐标。

本发明与现有专利(CN112859863B遍历范围为预测获得的范围)的一个显著不同是遍历范围，本发明中，遍历范围为所有参考路径点列。

将n个参考路径点的坐标分别代入上式(1)，可得相应的n组距离，求取其中n组距离的最小值，并记录距离最小值对应的参考路径点的某个点的序号j，则以该参考路径中的第j个点作为初始控制周期中的关键参考点，即在初始控制周期，本发明进行初始化，即遍历参考路径寻找距离移动装备距离最近的路径点作为关键参考点；在初始控制周期结束后，进入下一控制周期，也称为其它控制周期或非初始控制周期。

在其他控制周期中，对移动装备的控制器实时性要求很高，当参考路径较长时，遍历所有参考路径点的方法耗费的计算资源过多，因此采用新的关键参考点选取模式。

首先，读取上一控制周期的关键参考点，以该点作为当前控制周期的搜索基准点，并记录该点在参考路径中的序号。

由于移动装备的控制周期较短，移动机器人通常不超过0.05s，无人驾驶车辆通常不超过0.01s，一个控制周期内移动装备驶过的距离通常不超过0.5m，并且提供给移动装备的参考路径曲率通常具有上限，如提供给无人驾驶小轿车的参考路径曲率上限不超过0.2m^-1，因为非初始控制周期不需要遍历整个参考路径，只需要从搜索基准点开始搜索，直到满足停止搜索条件(即大于基准距离)，并且一个控制周期消耗的时间很短，所以移动装备在该控制周期内不会行驶很长的距离，加上参考路径曲率较小，因此，在一个控制周期内，从基准点到满足停止搜索条件时对应路径点之间的参考路径可以被视为直线。

考虑到点到直线的距离为存在最小值的二次函数，即该二次函数开口向上，因此，在点和直线之间的距离没到最小值之前，距离会越来越小，也就是说在未达到最小值之前，该二次函数单调递减，因此采用搜索基准点R_j到移动装备当前位置的距离l_j作为基准进行判断。

令：

l_b＝l_j (2)

式中，l_b表示基准距离，l_j采用公式(1)计算得到，j＝1,2,…,n。

进一步地，计算该基准点的下一点与移动装备当前位置的距离：

若该距离l_j+1大于该基准距离l_b，则将基准距离l_b对应的点R_j作为关键参考点；如果该距离l_j+1小于等于基准距离l_b，如公式所示：l_b≥l_j ⁺¹(4)，则更新基准距离，以该距离l_j+1作为新的基准距离，即令l_b＝l_j+1，进行继续搜索，并将继续搜索的下一点R_j+2与移动装备当前位置的距离继续进行判断，直到搜索到距离大于基准距离l_b的点R_j+k，k＝1，2，…，即满足：

l_b＜l_j+k (5)

则将该基准距离对应的参考路径点作为该控制周期内的关键参考点。

关键参考点是参考路径距离移动装备最近的点，通常只有一个。对搜索基准点之后的点搜索，但当满足上面停止搜索条件后，意味着已经找到最近点，此时结束搜索。整个过程需要重复一直找，直到找到关键参考点为止。因为使用局部最优代替了全局最优，从而减少计算量，提高实时性。

作为公开的实施例，本发明还提供了一种路径跟踪控制关键参考点选取装置，所述装置用于实现所述的方法，包括：

第一选取模块，用于在初始控制周期中，以移动装备与参考路径上的所有点的距离最小值对应的点作为关键参考点；

确定模块，用于在其它控制周期中，读取上一控制周期的关键参考点，以该点作为当前控制周期的搜索基准点，并基于该点确定基准距离；

第二选取模块，用于将当前控制周期的基准点之后的每一点与移动装备之间的距离与基准距离进行判断，根据判断结果得到该当前控制周期的关键参考点。

作为公开的实施例，本发明还提供了一种计算机存储介质，所述介质上存储有计算机程序，所述计算机程序被处理器执行实现所述的方法。

作为公开的实施例，本发明还提供了一种电子设备，所述电子设备包括：

存储器，存储有可执行指令；

处理器，所述处理器运行所述存储器中的所述可执行指令，以实现所述的方法。

上述说明示出并描述了本发明的若干优选实施例，但如前所述，应当理解本发明并非局限于本文所披露的形式，不应看作是对其他实施例的排除，而可用于各种其他组合、修改和环境，并能够在本文所述申请构想范围内，通过上述教导或相关领域的技术或知识进行改动。而本领域人员所进行的改动和变化不脱离本发明的精神和范围，则都应在本发明所附权利要求书的保护范围内。

Claims (10)

1.一种路径跟踪控制关键参考点选取方法，用于移动装备，其特征在于，所述方法包括：

S1.在初始控制周期中，以移动装备与参考路径上的所有点的距离最小值对应的点作为关键参考点；

在其它控制周期中，包括如下步骤：

S2.读取上一控制周期的关键参考点，以该点作为当前控制周期的搜索基准点，并基于该点确定基准距离；

S3.将当前控制周期的基准点之后的每一点与移动装备之间的距离与基准距离进行判断，根据判断结果得到该当前控制周期的关键参考点，

所述移动装备为在所述参考路径上行驶的移动机器人或无人驾驶车辆。

2.根据权利要求1所述的路径跟踪控制关键参考点选取方法，其特征在于，所述S1包括：

S11.给出点列形式的参考路径；

S12.确定移动装备与参考路径中的每个点之间的距离；

S13.将所有距离中最短距离对应的点作为关键参考点。

3.根据权利要求2所述的路径跟踪控制关键参考点选取方法，其特征在于，所述基准距离为所述基准点至所述移动装备当前位置的距离。

4.根据权利要求2或3所述的路径跟踪控制关键参考点选取方法，其特征在于，所述S12具体包括：参考路径以数组形式表示为：

式中，x_i表示第i个参考路径点在全局坐标系中的横坐标，y_i表示第i个参考路径点在全局坐标系中的纵坐标，n表示参考路径点的个数，所述移动装备与参考路径中的每个点之间的距离通过如下公式得到：

式中，x_a和y_a表示当前控制周期中移动装备实际位置的横坐标和纵坐标。

5.根据权利要求2所述的路径跟踪控制关键参考点选取方法，其特征在于，所述S3具体包括：

S31.计算所述基准点的下一点与所述移动装备当前位置的距离；

S32.若该距离小于等于该基准距离，则继续搜索，即计算下一点与所述移动装备当前位置的距离；

S33.若继续搜索得到的点与所述移动装备当前位置的距离大于基准距离，则将基准距离对应的点作为关键参考点。

6.根据权利要求5所述的路径跟踪控制关键参考点选取方法，其特征在于，所述S32或者为：若该距离大于该基准距离，则将基准距离对应的点作为关键参考点。

7.根据权利要求6所述的路径跟踪控制关键参考点选取方法，其特征在于，所述基准点的下一点与所述移动装备当前位置的距离为：

式中，x_j+1表示第j+1个参考路径点在全局坐标系中的横坐标，y_j+1表示第j+1个参考路径点在全局坐标系中的纵坐标，x_a和y_a表示当前控制周期中移动装备实际位置的横坐标和纵坐标，j＝1,2,…,n。

8.一种路径跟踪控制关键参考点选取装置，其特征在于，所述装置用于实现权利要求1-7任一项所述的方法，包括：

第一选取模块，用于在初始控制周期中，以移动装备与参考路径上的所有点的距离最小值对应的点作为关键参考点；

确定模块，用于在其它控制周期中，读取上一控制周期的关键参考点，以该点作为当前控制周期的搜索基准点，并基于该点确定基准距离；

第二选取模块，用于将当前控制周期的基准点之后的每一点与移动装备之间的距离与基准距离进行判断，得到该当前控制周期的关键参考点。

9.一种计算机存储介质，其特征在于，所述介质上存储有计算机程序，所述计算机程序被处理器执行实现权利要求1-7任一项所述的方法。

10.一种电子设备，其特征在于，所述电子设备包括：

存储器，存储有可执行指令；

处理器，所述处理器运行所述存储器中的所述可执行指令，以实现权利要求1-7中任一项所述的方法。