CN113515111B - 一种车辆避障路径规划方法及装置 - Google Patents

Abstract

本发明涉及一种车辆避障路径规划方法及装置，包括：获取车辆行驶预设轨迹；在车辆行驶预设轨迹上确定一个基准目标点，将该基准目标点变换到车辆坐标系下；根据车辆坐标系下的基准目标点确定备选目标点；根据基准目标点、车辆起始位置，拟合一条局部备选路径；对于任一个备选目标点、车辆起始位置，也对应拟合一条局部备选路径；将局部备选路径按照到车辆行驶预设轨迹的距离从小到大的顺序排序；按照顺序依次判断各局部备选路径是否满足路径选择设定条件，若某条局部备选路径满足路径选择设定条件，则将该条局部备选路径作为最终确定的局部路径。本发明不仅提高了确定局部路径的效率，而且可以保证最终确定的路径尽可能的与预设轨迹一致。

Description

一种车辆避障路径规划方法及装置

技术领域

本发明属于无人驾驶技术领域，具体涉及一种车辆避障路径规划方法及装置。

背景技术

路径规划分为全局规划和局部规划，是实现无人驾驶***导航和控制的最基本环节之一，其目的在于使车辆在未知的环境中能够基于一定的规则，搜索出一条从起始点到目标点的平滑、代价小且无障碍物的路径。目前已有多种智能控制算法来解决路径规划的问题，如遗传算法、蚁群算法、A*算法、Dijkstra算法等。这些算法都存在一定的缺陷，遗传算法运算的时间复杂度较高，无法满足实时规划的需求；蚁群算法容易陷入局部最优，限制了在实际中的应用；A*算法需要感知***提供栅格地图，且存在路径折线多、累积转角大的问题；Dijkstra算法考虑全局最优，运算时间长，且有可能出现权为负的边使算法失效。

为了实现局部路径规划，申请公布号为CN107992050A的中国专利申请文件公开了一种无人驾驶汽车局部路径运动规划方法，根据无人驾驶汽车的期望驾驶路径、当前车辆位置点、当前车辆航向、拟合得到局部路径轨迹簇，然后计算局部路径轨迹簇中各局部路径轨迹的代价函数值，将代价函数值最小的局部路径轨迹作为最佳局部运动路径。该方法虽然可以规划处一条可行驶的最佳局部运动路径，但是存在以下问题：1、将初始规划路径偏移到当前车道线上，需要借助高精度地图，进行整体数据映射，计算量大；2、在确定最佳局部运动路径时，需要计算局部路径轨迹簇中每一条曲线对应的代价函数值，计算量较大，速度慢；3、在计算局部路径轨迹簇中每一条曲线对应的代价函数值时，仅考虑绕障轨迹与障碍物之间、局部可选目标点与局部期望目标点之间以及当前局部可选目标点与前一个最佳局部可选目标点之间的横向距离，考虑较为片面，当绕障轨迹与障碍物之间的横向距离较大而车辆与障碍物之间的距离较小时，会导致最终选取出的最佳局部运动路径不合理。

总的来说，上述的无人驾驶汽车局部路径运动规划方法计算量大，效率低，而安全性不高；在实际应用中效果不佳。

发明内容

本发明提供了一种车辆避障路径规划方法及装置，用以解决现有技术在路径规划时效率低安全性差的问题。

为解决上述技术问题，本发明的技术方案包括：

本发明提供了一种车辆避障路径规划方法，步骤如下：

获取车辆行驶预设轨迹；

在所述车辆行驶预设轨迹上确定一个基准目标点，将该基准目标点进行坐标变换，变换到车辆坐标系下；

根据车辆坐标系下的所述基准目标点，确定备选目标点；

根据基准目标点、车辆起始位置确定若干控制点，拟合一条符合贝塞尔曲线标准的局部备选路径；对于任一个备选目标点、车辆起始位置确定相应的若干控制点，也对应拟合一条符合贝塞尔曲线标准的局部备选路径；从而形成一组局部备选路径；

将所述局部备选路径按照到所述车辆行驶预设轨迹的距离从小到大的顺序排序；

按照所述顺序依次判断各局部备选路径是否满足路径选择设定条件，若某条局部备选路径满足所述路径选择设定条件，则将该条局部备选路径作为最终确定的局部路径。

上述技术方案的有益效果为：只需要确定一个基准目标点，然后将基准目标点映射到车辆坐标系即可，不需要进行大规模的数据映射；通过对局部备选路径进行排序，并依次进行判断，当车辆局部备选路径满足条件时，则无需再进行判断，不仅提高了效率，而且可以保证最终确定的局部路径尽可能的与预设轨迹一致。

进一步的，为了提高最终确定的局部路径长度的合理性，根据车辆当前位置、车辆当前速度、车辆制动的最大减速度、车辆最小安全距离在车辆行驶预设轨迹上确定所述基准目标点。

进一步的，在车辆行驶预设轨迹上确定所述基准目标点的步骤包括：

根据车辆当前速度，车辆制动的最大减速度，车辆最小安全距离，计算基准距离：

其中，l₀为基准距离，v为车辆当前速度，T为程序运行周期，l_c为最小碰撞距离，l_s为最小安全距离，a为车辆制动的最大减速度；

根据车辆当前位置和所述基准距离，在所述车辆行驶预设轨迹上确定基准目标点，使得基准目标点到车辆当前位置的距离为所述基准距离。

进一步的，为了获取合适的备选路径，所述路径选择设定条件为：局部备选路径上没有障碍物，且障碍物到局部备选路径的侧向通过距离不小于允许车辆通过的最小侧向通过距离。

进一步的，为了降低计算复杂度，确定备选目标点的步骤包括：

将所述基准目标点作为车辆坐标系的原点，以车辆行驶预设轨迹在所述基准目标点处的航向作为车辆坐标系的x轴，在对应车辆坐标系的y轴上选定备选目标点；然后将各备选目标点的坐标进行转换，转换到以车辆当前位置为原点的车辆坐标系下。

进一步的，为了使拟合出的局部备选路径均匀分布在车辆行驶预设轨迹的两侧，所述备选目标点相对于基准目标点沿x轴对称。

进一步的，为了使拟合出的局部备选路径更加平衡，根据基准目标点、车辆起始位置确定的控制点包括：车辆当前位置的航向所在直线与所述基准目标点的航向所在直线的交点；

根据备选目标点、车辆起始位置确定的控制点包括：车辆当前位置的航向所在直线与所述备选目标点的航向所在直线的交点。

进一步的，为了获取最优的停车制动路径，还包括：当所有的局部备选路径均不满足路径选择设定条件时，则选取存在障碍物且障碍物到车辆当前位置的距离最大所对应的局部备选路径作为停车制动路径。

进一步的，为了提高获取、存储预设轨迹的效率，通过GPS数据获取所述车辆行驶预设轨迹，并且通过离散点的形式进行存储。

本发明还提供了一种车辆避障路径规划装置，包括处理器和存储器，所述处理器用于处理存储在所述存储器中的指令，以实现上述的车辆避障路径规划方法，并达到与方法相同的效果。

附图说明

图1是本发明的车辆避障路径规划方法的流程图；

图2是本发明的车辆避障路径规划方法构建的Bezier曲线组示意图；

图3是本发明的车辆避障路径规划装置的结构图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及具体实施例对本发明进行进一步详细说明。

方法实施例：

一种车辆避障路径规划方法，其对应的流程图如图1所示，包括以下步骤：

(1)采集GPS地图，根据车辆预设路线，离线处理地图数据，构建车辆行驶预设轨迹，并将车辆行驶预设轨迹以离散点的形式进行存储，这种方式获取、存储预设轨迹效率高，利于后期计算。

其中，可以根据实际情况，来构建对应的车辆行驶预设轨迹。例如，对于行驶路线确定的公交车，可以根据公交车的行驶路线，并结合GPS地图，来构建对应的车辆行驶预设轨迹；对于知道起始点和终点的小轿车，可以根据百度地图或者高德地图生成相应的行驶路线，并结合GPS地图，来构建对应的车辆行驶预设轨迹。

(2)根据车辆行驶预设轨迹和车辆当前位置，构建一组Bezier(贝塞尔)曲线，具体构建步骤如下：

(2-1)首先根据车辆当前速度，车辆制动的最大减速度，车辆最小安全距离，确定基准距离l₀，对应的确定公式如下：

其中，v为车辆当前速度，T为程序运行周期，该程序运行周期是指包括路径规划算法在内的整个***程序运行的一个步长，l_c为最小碰撞距离，l_s为最小安全距离，a为车辆制动的最大减速度。

(2-2)根据车辆行驶预设轨迹、车辆当前位置以及基准距离l₀，在车辆行驶预设轨迹上确定基准Bezier曲线的基准目标点Q₀。此时基准目标点Q₀沿车辆行驶预设轨迹，到车辆当前位置的距离为l₀。

(2-3)根据车辆当前位姿与车辆行驶预设轨迹上的基准目标点Q₀位姿在车辆坐标系下选取控制点P₀，P₁，…，P_n(n为偶数)，构建基准Bezier曲线，具体步骤如下：

(2-3-1)根据车辆当前位姿照运动学关系将该基准目标点Q₀的大地坐标系转化为车辆坐标系：

其中，(x₀，y₀)为车辆当前位置，为车辆当前位置的航向角，(x_global，y_global)为目标点Q₀的大地坐标，(x_vehicle，y_vehicle)为目标基准点Q₀的车辆坐标。

(2-3-2)如图2所示，以车辆当前位置(x₀，y₀)作为车辆坐标系的原点，取车辆当前位置为控制点P₀，此时有P₀(0,0)，取基准目标点Q₀为控制点P_n，此时有P_n(x_vehicle，y_vehicle)，取车辆当前航向与基准目标点Q₀处航向的交点为关键控制点P_n/2，其余(n-2)个控制点为在线段P₁P_n/2和线段P_n/2Pn上均匀取点得到。

(2-3-3)根据控制点P₀，P₁，…，P_n，依据曲线方程构建贝塞尔Bezier曲线：

通过上述的步骤(2-3-1)-(2-3-3)就可以获取基准Bezier曲线，且上述的取点方式至少可以取出5个控制点，可以保证生成的Bezier曲线与预设路径轨迹形态近似。

本实施例中，仅仅需要将基准目标点Q₀从大地坐标系转化到车辆坐标系，而不需要进行整个高精度地图的映射和处理，计算速度快，效率高。而且，本实施例中，车辆当前位置航向与基准目标点Q₀处航向的交点为关键控制点P_n/2，采用该关键控制点能够使得拟合的轨迹更加平衡，特别适合于转弯工况。

(2-4)根据基准目标点Q₀和车道线宽度width，确定其他的2m个备选目标点，进而确定其对应的Bezier曲线，具体步骤如下：

(2-4-1)如图2所述，将基准目标点Q₀作为车辆坐标系的原点，以基准目标点Q₀的航向作为车辆坐标系的x轴，在y轴上选定2m个关于x轴对称的备选目标点Q_i(x_i，y_i)，i＝1，2，…，2m，其中，x_i＝0，

(2-4-2)将备选目标点Q_i(x_i，y_i)转化为以车辆当前位置为原点的车辆坐标系下，对应得到备选目标点Q_i(x′_vehicle，y′_vehicle)，取车辆当前位置为控制点P₀，此时有P₀(0,0)，取备选目标点Q_i(x′_vehicle，y′_vehicle)为控制点P_n，此时有P_n(x′_vehicle，y′_vehicle)，将车辆行驶预设轨迹在基准目标点Q₀的航向作为各备选目标点的航向，按照步骤(2-3)的方法构建2m条Bezier曲线。

在步骤(2-4-1)中，通过将基准目标点Q₀作为车辆坐标系的原点，并将基准目标点Q₀的航向作为车辆坐标系的x轴，这样就可以在y轴上选定备选目标点，降低了计算复杂度。步骤(2-4-2)中将坐标系再次转化到了以车辆当前位置为原点的车辆坐标系。

作为其他实施方式，也可以始终在以车辆当前位置为原点的车辆坐标系中进行计算。

(2-5)对所有(2m+1)条Bezier曲线按照距离预设轨迹的距离从小到大进行排序，得到一组有序Bezier曲线并进行存储。

(3)根据障碍物与曲线的相对位置关系，从构建的Bezier曲线组中选择一条作为车辆的局部路径，具体过程如下：

(3-1)先获取距离预设轨迹的距离最小的Bezier曲线，并从前向感知***实时获取障碍物信息。

(3-2)找到距离该Bezier曲线最近的障碍物，并计算障碍物纵向距离D_long和侧向通过距离D_lat：

其中，d_long为Bezier曲线上的障碍物距离车辆的实际距离，d_lat为障碍物距离Bezier曲线的实际侧向通过距离，threshold为允许车辆通过的最小侧向通过距离。

(3-3)根据障碍物纵向距离D_long和侧向通过距离D_lat的计算情况，进行如下操作：

当D_long＝∞且D_lat≠0时，选择该Bezier曲线作为车辆的可行驶路径；

当D_long＝∞且D_lat＝0时，继续从(3-1)开始判断下一条Bezier曲线；

当D_long＝d_long时，继续从(3-1)开始判断下一条Bezier曲线。

(3-4)遍历所有Bezier曲线后，若存在可行驶路径，为保证行驶路径不偏离预设轨迹过远，选择第一条满足条件的Bezier曲线作为最优路径；若不存在可行驶路径，则选择d_long最大的曲线进行停车制动。

在上述的步骤(3)中，通过按照距离预设轨迹的距离从小到大的顺序，依次计算Bezier曲线对应的障碍物纵向距离D_long和侧向通过距离D_lat，当满足D_long＝∞且D_lat≠0时，就直接把该Bezier曲线作为车辆的可行驶路径，无需再进行后续Bezier曲线的相关计算，在保证可以选出车辆的可行驶路径的基础上，不仅提高了计算效率，而且还可以保证最终确定的路径尽可能的与预设轨迹一致。

在本实施例中，所构建的Bezier曲线组如图2所示，总共包括5条Bezier曲线。当然，作为其他的实施方式，可以根据车道线宽度，来确定备选目标点的数目，进而确定Bezier曲线组中的曲线条数。

需要说明的是，在上述的车辆避障路径规划方法中，是以先构建基准Bezier曲线，然后再根据基准目标点Q₀来确定2m个备选目标点，进而构建其他的2m条Bezier曲线为例进行说明的，作为其他的实施方式，也可以首先根据基准目标点Q₀来确定2m个备选目标点，然后根据基准目标点Q₀和2m个备选目标点，来同时构建基准Bezier曲线和其他的2m条Bezier曲线。

上述的车辆避障路径规划方法仅是给出了确定基准目标点、备选目标点以及控制点的一种具体实施方式，作为其他的实施方式，在保证生成的局部备选路径长度合理的情况下，也可以采用其他方式来确定基准目标点和备选目标点，在保证能够生成符合贝塞尔曲线标准的局部备选路径的情况下，也可以采用其他的形式来确定控制点。

例如，在确定基准目标点时，可以根据车辆的当前速度和最小安全距离来确定基准目标点到车辆当前位置的距离，进而确定基准目标点在车辆行驶预设轨迹上的位置；在确定备选目标点时，可以在对应车辆坐标系的y轴上均匀取点来得到；在确定控制点时，也可以采用背景技术中所提到的专利申请文件中所公开的方式。

另外，在保证最终确定的局部路径合理的情况下，路径选择设定条件也可以采用其他的实施方式。例如，路径选择设定条件为局部备选路径到距离其最近的障碍物的横向距离不小于允许车辆通过的最小侧向通过距离。

装置实施例：

本实施例提供了一种车辆避障路径规划装置，如图3所示，包括处理器、存储器，存储器中存储有可在处理器上运行的计算机程序，该处理器在执行计算机程序时实现上述方法实施例的方法。也说就是说，以上方法实施例中的方法应理解为可由计算机程序指令实现车辆避障路径规划方法的流程。

本实施例所指的处理器是指微处理器MCU或可编程逻辑器件FPGA等的处理装置；本实施例所指的存储器包括用于存储信息的物理装置，通常是将信息数字化后再以利用电、磁或者光学等方式的媒体加以存储。例如：利用电能方式存储信息的各式存储器，RAM、ROM等；利用磁能方式存储信息的的各式存储器，硬盘、软盘、磁带、磁芯存储器、磁泡存储器、U盘；利用光学方式存储信息的各式存储器，CD或DVD。当然，还有其他方式的存储器，例如量子存储器、石墨烯存储器等等。

通过上述存储器、处理器以及计算机程序构成的装置，在计算机中由处理器执行相应的程序指令来实现，处理器可以搭载各种操作***，如windows操作***、linux***、android、IOS***等。

Claims (7)

1.一种车辆避障路径规划方法，其特征在于，步骤如下：

获取车辆行驶预设轨迹；

在所述车辆行驶预设轨迹上确定一个基准目标点，将该基准目标点进行坐标变换，变换到车辆坐标系下；根据车辆当前位置、车辆当前速度、车辆制动的最大减速度、车辆最小安全距离在车辆行驶预设轨迹上确定所述基准目标点；在车辆行驶预设轨迹上确定所述基准目标点的步骤包括：

根据车辆当前速度，车辆制动的最大减速度，车辆最小安全距离，计算基准距离：

其中，l₀为基准距离，v为车辆当前速度，T为程序运行周期，l_c为最小碰撞距离，l_s为最小安全距离，a为车辆制动的最大减速度；

根据车辆坐标系下的所述基准目标点，确定备选目标点；

根据基准目标点、车辆起始位置确定若干控制点，拟合一条符合贝塞尔曲线标准的局部备选路径；对于任一个备选目标点、车辆起始位置确定相应的若干控制点，也对应拟合一条符合贝塞尔曲线标准的局部备选路径；从而形成一组局部备选路径；根据基准目标点、车辆起始位置确定的控制点包括：车辆当前位置的航向所在直线与所述基准目标点的航向所在直线的交点为关键控制点，其余控制点在基准目标点、车辆起始位置分别与关键控制点之间的线段上均匀取点得到；根据备选目标点、车辆起始位置确定的控制点包括：车辆当前位置的航向所在直线与所述备选目标点的航向所在直线的交点为关键控制点，其余控制点在备选目标点、车辆起始位置分别与关键控制点之间的线段上均匀取点得到；

将所述局部备选路径按照到所述车辆行驶预设轨迹的距离从小到大的顺序排序；

按照所述顺序依次判断各局部备选路径是否满足路径选择设定条件，若某条局部备选路径满足所述路径选择设定条件，则将该条局部备选路径作为最终确定的局部路径。

2.根据权利要求1所述的车辆避障路径规划方法，其特征在于，所述路径选择设定条件为：局部备选路径上没有障碍物，且障碍物到局部备选路径的侧向通过距离不小于允许车辆通过的最小侧向通过距离。

3.根据权利要求1所述的车辆避障路径规划方法，其特征在于，确定备选目标点的步骤包括：

将所述基准目标点作为车辆坐标系的原点，以车辆行驶预设轨迹在所述基准目标点处的航向作为车辆坐标系的x轴，在对应车辆坐标系的y轴上选定备选目标点；然后将各备选目标点的坐标进行转换，转换到以车辆当前位置为原点的车辆坐标系下。

4.根据权利要求3所述的车辆避障路径规划方法，其特征在于，所述备选目标点相对于基准目标点沿x轴对称。

5.根据权利要求1所述的车辆避障路径规划方法，其特征在于，还包括：当所有的局部备选路径均不满足路径选择设定条件时，则选取存在障碍物且障碍物到车辆当前位置的距离最大所对应的局部备选路径作为停车制动路径。

6.根据权利要求1所述的车辆避障路径规划方法，其特征在于，通过GPS数据获取所述车辆行驶预设轨迹，并且通过离散点的形式进行存储。

7.一种车辆避障路径规划装置，其特征在于，包括处理器和存储器，所述处理器用于处理存储在所述存储器中的指令，以实现权利要求1-6中任一项所述的车辆避障路径规划方法。