CN109976321A - 一种用于智能驾驶系统的轨迹规划方法及智能驾驶系统 - Google Patents

Abstract

一种用于智能驾驶系统的轨迹规划方法，其包括：步骤一、根据获取到的车辆的当前车速确定最优瞄准距离；步骤二、获取位于车辆前方的预设数量的位置点的点坐标，并分别计算各个位置点的曲率半径；步骤三、根据各个位置点的曲率半径和最优瞄准距离，确定预瞄点；步骤四、基于预设曲线拟合模型，根据预瞄点确定车辆的规划轨迹。本方法将智能驾驶车辆的车辆状态作为轨迹规划的参考信息，其能够使得得到规划轨迹更大可能地满足车辆动力学模型，使得智能驾驶系统的轨迹跟踪过程更加平稳安全。

Description

一种用于智能驾驶系统的轨迹规划方法及智能驾驶系统

技术领域

本发明涉及智能驾驶技术领域，具体地说，涉及一种用于智能驾驶系统的轨迹规划方法及智能驾驶系统。

背景技术

目前，智能驾驶系统在减小驾驶员工作强度、提高公路交通安全以及降低违法驾驶等方面都有着巨大的应用市场。其中，辅助驾驶系统已成为部分发达国家法律规定的汽车必备，同时也是世界各大汽车厂商及科技公司的研发重点。

随着智能驾驶技术的发展，配置有智能驾驶系统的自动驾驶车辆已开始逐步代替驾驶员的位置。为了使车辆自动驾驶至目的地，智能驾驶系统通常会搜索出一条最短期望路径。然而，直接对期望路径的跟踪通常是不符合车辆运动学的，因此需要在期望路径中选择一点，然后对该点进行轨迹规划，从而得出车辆能够跟踪的轨迹。现有的轨迹规划的拟合规划策略的实现过程较为复杂，无法满足智能驾驶系统的应用需要。

发明内容

为解决上述问题，本发明提供了一种用于智能驾驶系统的轨迹规划方法，所述方法包括：

步骤一、根据获取到的车辆的当前车速确定最优瞄准距离；

步骤二、获取位于车辆前方的预设数量的位置点的点坐标，并分别计算各个位置点的曲率半径；

步骤三、根据所述各个位置点的曲率半径和所述最优瞄准距离，确定预瞄点；

步骤四、基于预设曲线拟合模型，根据所述预瞄点确定所述车辆的规划轨迹。

根据本发明的一个实施例，在所述步骤一中，将所述当前车速与预设车速阈值进行比较，并根据比较结果确定所述最优瞄准距离。

根据本发明的一个实施例，在所述步骤一中，如果所述当前车速小于预设车速阈值，则将所述车辆的最小转弯半径作为所述最优瞄准距离。

根据本发明的一个实施例，在所述步骤一中，如果所述当前车速大于或等于预设速度阈值，则计算所述当前车速与预瞄距离增益的乘积，并将得到的乘积结果作为所述最优瞄准距离。

根据本发明的一个实施例，所述步骤二包括：

步骤a、分别计算各个位置点距离所述车辆的当前位置的距离；

步骤b、分别计算各个位置点所对应的距离线段与车辆坐标系X轴的航向角；

步骤c、根据所述航向角分别计算所述各个位置点在车辆行驶方向的横向距离；

步骤d、根据所述各个位置点所对应的横向距离分别计算各个位置点的曲率半径。

根据本发明的一个实施例，在所述步骤b中，根据如下表达式计算航向角：

其中，表示位置点(x_i,y_i)与车辆的当前位置(x₀,y₀)之间的距离线段d_i与车辆坐标系X轴的航向角。

根据本发明的一个实施例，在所述步骤c中，根据如下表达式计算所述横向距离：

其中，表示位置点(x_i,y_i)与车辆的当前位置(x₀,y₀)之间的距离线段d_i与车辆坐标系X轴的航向角，γ_vehicle表示车辆偏航角度，m_i表示位置点(x_i,y_i)在车辆行驶方向的横向距离。

根据本发明的一个实施例，在所述步骤三中，从所述各个位置点中选取曲率半径大于或等于最优瞄准距离并且曲率半径与所述最优瞄准距离的差值最小的位置点作为所述预瞄点。

根据本发明的一个实施例，所述预设曲线拟合模型包括：

根据预瞄点坐标计算所述车辆的当前偏移量；

根据所述车辆的当前偏移量确定所述车辆的规划轨迹。

根据本发明的一个实施例，根据如下表达式计算所述车辆的当前偏移量：

y_offset＝Y_fcosθ+X_fsinθ

其中，y_offset表示当前偏移量，(X_f,Y_f,θ)表示预瞄点坐标。

根据本发明的一个实施例，根据如下表达式计算所述车辆的规划轨迹：

Y(X)＝a₀+a₁X+a₂X²+a₃X³

a₀＝a₁＝0

其中，Y(X)表示与X相关的车辆的规划轨迹，a₀、a₁、a₂和a₃均表示计算系数，y_offset表示当前偏移量，(X_f,Y_f,θ)表示预瞄点坐标。

本发明还提供了一种智能驾驶系统，其特征在于，所述系统采用如上任一项所述的方法进行车辆的轨迹规划。

本发明所提供的用于智能驾驶系统的轨迹规划方法将智能驾驶车辆的车辆状态作为轨迹规划的参考信息，其能够使得得到规划轨迹更大可能地满足车辆动力学模型，进而使得智能驾驶系统的轨迹跟踪过程更加平稳安全。

同时，本方法还通过三次方曲线拟合来确定规划轨迹，相较于现有的轨迹规划方法，本方法有效降低了曲线拟合难度，这样也就能够有效节省拟合过程所耗费的时间，从而提高了轨迹规划的实时性。

本发明的其它特征和优点将在随后的说明书中阐述，并且，部分地从说明书中变得显而易见，或者通过实施本发明而了解。本发明的目的和其他优点可通过在说明书、权利要求书以及附图中所特别指出的结构来实现和获得。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要的附图做简单的介绍：

图1是根据本发明一个实施例的用于智能驾驶系统的轨迹规划方法的实现流程示意图；

图2是根据本发明一个实施例的确定最优瞄准距离的实现流程示意图；

图3是根据本发明一个实施例的计算各个位置点的曲率半径的实现流程示意图；

图4是根据本发明一个实施例的航向角的示意图；

图5是根据本发明一个实施例的目标偏移量的示意图；

图6是根据本发明一个实施例的不同预瞄点的状态角度所对应的规划轨迹示意图；

图7是根据本发明一个实施例的不同预瞄点的状态角度下的规划轨迹分析示意图。

具体实施方式

以下将结合附图及实施例来详细说明本发明的实施方式，借此对本发明如何应用技术手段来解决技术问题，并达成技术效果的实现过程能充分理解并据以实施。需要说明的是，只要不构成冲突，本发明中的各个实施例以及各实施例中的各个特征可以相互结合，所形成的技术方案均在本发明的保护范围之内。

同时，在以下说明中，出于解释的目的而阐述了许多具体细节，以提供对本发明实施例的彻底理解。然而，对本领域的技术人员来说显而易见的是，本发明可以不用这里的具体细节或者所描述的特定方式来实施。

另外，在附图的流程图示出的步骤可以在诸如一组计算机可执行指令的计算机系统中执行，并且，虽然在流程图中示出了逻辑顺序，但是在某些情况下，可以以不同于此处的顺序执行所示出或描述的步骤。

本发明提供了一种新的用于智能驾驶系统的轨迹规划方法以及应用该方法进行轨迹规划的智能驾驶系统，该方法能够有效减低轨迹规划难度，有助于提高轨迹规划的实时性。

图1示出了本实施例所提供的智能驾驶系统的轨迹规划方法的实现流程示意图。

本实施例所提供的轨迹规划方法首先需要确定最优轨迹终点。具体地，如图1所示，该方法首先在步骤S101中根据获取到的车辆的当前车速确定最优瞄准距离。由于车辆的横向与纵向具有强耦合关系，因此预瞄距离(即最优瞄准距离)与车辆的当前车速有关。

具体地，本实施例中，该方法在步骤S101中根据如下表达式来确定最优瞄准距离L_a：

其中，L_amin表示最小转弯半径，K_la表示预瞄距离增益，v表示当前车速，v_change表示预设车速阈值。

通过表达式(1)可以看出，如图2所示，该方法在确定最优瞄准距离时，会在步骤S201获取车辆的当前车速v，随后在步骤S202中将当前车速v与预设车速阈值v_change进行比较。其中，如果当前车速v小于预设车速阈值v_change，那么该方法则会在步骤S203中将车辆的最小转弯半径L_amin作为该车的最优瞄准距离；而如果当前车速v大于或等于预设车速阈值v_change，那么该方法则会在步骤S204中计算当前车速v与预瞄距离增益K_la的乘积K_lav，并将该乘积作为该车的最优瞄准距离。

本实施例中，最小转弯半径L_amin能够表征智能驾驶车辆遇到紧急情况时安全做出反应所需要的最小距离。例如，如果智能驾驶车辆的前路偏角的极限范围为[38°,42°]，由车辆运动学模型可知前轮偏转角与转弯半径之间的关系，取前轮偏角的最大值42°，假设车辆的轴距为5.9m，那么则可以确定出该车辆的最小转弯半径L_amin为6.55m。

而K_lav则能够表征出智能驾驶车辆在一较大速度下的预瞄距离。本实施例中，预瞄距离增益K_la表征一个在速度范围内的调节参数，在高速环境下可以减小预瞄距离增益K_la，从而使得预瞄距离增益K_la受车辆速度影响减弱，这样也就可以保证当存在一个较小的距离偏差时，智能驾驶系统不会过度修正。

假设预瞄距离增益K_la取值为2.87，那么根据表达式(1)可以得到：

这样对该智能驾驶车辆来说，也就存在：

本实施例中，对于一智能驾驶车辆来说，表达式(1)中的最小转弯半径L_amin、预瞄距离增益K_la以及预设速度阈值v_change是已知的，因此根据车辆的当前车速v也就可以确定出该车辆的最优瞄准距离L_a。

当然，在本发明的其它实施例中，该方法还可以采用其它合理方式来根据获取到的车辆的当前车速v确定最优瞄准距离L_a，本发明不限于此。

再次如图1所示，本实施例中，该方法会在步骤S102中获取位于车辆前方的预设数据量的位置点的点坐标，并在步骤S103中分别计算各个位置点的曲率半径。

需要指出的是，在本发明的不同实施例中，该方法在步骤S102中所获取到的位置点的数量可以根据实际需要配置为不同的合理值，本发明并不对所获取到的位置点的数量进行限定。

图3示出了本实施例中计算各个位置点的曲率半径的实现流程示意图。

如图3所示，在得到各个位置点的点坐标后，该方法会在步骤S301中分别计算各个位置点距离车辆的当前位置的距离。具体地，本实施例中，该方法优选地根据如下表达式来计算各个位置点距离车辆的当前位置的距离：

其中，d_i表示第i位置点距离车辆的当前位置(x₀,y₀)的距离，(x_i,y_i)表示第i位置点的点坐标。

在得到各个位置点位置点距离车辆的当前位置的距离后，该方法会在步骤S302中分别计算各个位置点所对应的距离线段与车辆坐标系X轴的航向角。

具体地，如图4所示，本实施例中，该方法优选地根据如下表达式计算航向角：

其中，表示位置点(x_i,y_i)与车辆的当前位置(x₀,y₀)之间的距离线段d_i与车辆坐标系X轴的航向角。

在得到各个位置点所对应的距离线段与车辆坐标系X轴的航向角后，该方法会在步骤S303中根据上述航向角来分别计算各个位置点在车辆行驶方向的横向距离。

具体地，本实施例中，各个位置点在车辆行驶方向的横向距离优选地可以根据如下表达式计算得到：

其中，m_i表示位置点(x_i,y_i)在车辆行驶方向的横向距离，γ_vehicle表示车辆偏航角度(即车辆行驶方向)。

在得到各个位置点在车辆行驶方向的横向距离后，该方法将在步骤S304中根据各个位置点所对应的横向距离来分别计算各个位置点的曲率半径。

具体地，本实施例中，该方法优选地根据如下表达式计算各个位置点的曲率半径：

R_i＝d_i ²/(2×m_i) (7)

其中，R_i表示位置点(x_i,y_i)的曲率半径。

当然，在本发明的其它实施例中，该方法还可以采用其它合理方式来确定各个位置点的曲率半径，本发明不限于此。

再次如图1所示，在得到各个位置点的曲率半径后，该方法将在步骤S104中根据步骤S101中所得到的最优瞄准距离L_a和步骤S103中所得到的各个位置点的曲率半径来确定预瞄点。

具体地，本实施例中，该方法在步骤S104中优选地将各个位置点的曲率半径与最优瞄准距离L_a进行比较，从各个位置点中选取曲率半径大于或等于最优瞄准距离L_a并且曲率半径与最优瞄准距离L_a的差值最小的位置点作为预瞄点。如图5所示，本实施例中，该方法在步骤S104中得到的预瞄点的坐标可以表示为(X_f,Y_f,θ)。其中，图5中所示的曲线2优选地根据所获取到的位置点的坐标进行曲线拟合得到(例如采用最小二乘法拟合得到三次项系数)，θ表示曲线2与车辆当前行驶方向之间的夹角。

本实施例中，如果上述位置点的曲率半径均小于最优瞄准距离L_a，那么该方法则会将最后一个位置点作为预瞄点。同时，如果上述位置点的数量小于预设位置点数量阈值(例如3个)，那么为了保证结果的准确性和可靠性，该方法将会根据最后一个位置点的坐标确定预瞄点的坐标。

具体地，本实施例中，如果位置点的数量小于预设位置点数量阈值，那么该方法优选地将预瞄点坐标确定为(x_final+k,y_final,0)，其中，(x_final,y_final)表示所获取的位置点中最后一个位置点的坐标，k表示调整系数。调整系数k的取值可以根据实际需要配置为不同的合理值(例如10或[7，13]中的其它值等)。

当然，在本发明的其它实施例中，该方法还可以采用其它合理方式来根据最优瞄准距离L_a和各个位置点的曲率半径来确定预瞄点，本发明不限于此。

同时，还需要指出的是，本发明并不对确定最优瞄准距离L_a和确定各个位置点的曲率半径的顺序进行限定，即在本发明的不同实施例中，该方法既可以先确定最优瞄准距离L_a再确定各个位置点的曲率半径，也可以先确定各个位置点的曲率半径再确定最优瞄准距离L_a，还可以同时确定各个位置点的曲率半径和最优瞄准距离L_a，本发明同样不限于此。

如图1所示，在得到预瞄点后个，该方法会在步骤S105中基于预设曲线拟合模型，来根据上述预瞄点确定车辆的规划轨迹。

具体地，本实施例中，该方法优选地采用三次方曲线拟合的方式来确定车辆的规划轨迹。其中，车辆的规划轨迹可以采用如下表达式表示：

Y(X)＝a₀+a₁X+a₂X²+a₃X³ (8)

其中，Y(X)表示与车辆横坐标X相关的车辆的规划轨迹，a₀、a₁、a₂和a₃均表示计算系数。

根据约束条件，计算系数a₀、a₁、a₂和a₃存在如下关系：

a₀＝a₁＝0 (9)

其中，y_offset表示车辆的当前偏移量。

因此为了确定车辆的规划轨迹，也就需要确定表达式(8)中各个计算系数a₀、a₁、a₂和a₃的取值，这样也就需要确定车辆的当前偏移量y_offset。本实施例中，结合图5可以看出，该方法可以根据如下表达式计算车辆的当前偏移量y_offset：

y_offset＝Y_fcosθ+X_fsinθ (12)

需要指出的是，在本发明的其它实施例中，该方法还以采用其它合理方式来根据预瞄点确定各个计算系数a₀、a₁、a₂和a₃的取值，本发明不限于此。

图6示出了本实施例中不同预瞄点的状态角度所对应的规划轨迹示意图，图7则示出了不同预瞄点的状态角度下的规划轨迹分析示意图。根据图6和图7可以看出，对于不同的预瞄点的状态角度，本实施例所提供的方法均能够得到较好的规划轨迹。

从上述描述中可以看出，本发明所提供的用于智能驾驶系统的轨迹规划方法将智能驾驶车辆的车辆状态作为轨迹规划的参考信息，其能够使得得到规划轨迹更大可能地满足车辆动力学模型，使得智能驾驶系统的轨迹跟踪过程更加平稳安全。

同时，本方法还通过三次方曲线拟合来确定规划轨迹，相较于现有的轨迹规划方法，本方法有效降低了曲线拟合难度，这样也就能够有效节省拟合过程所耗费的时间，从而提高了轨迹规划的实时性。

应该理解的是，本发明所公开的实施例不限于这里所公开的特定结构或处理步骤，而应当延伸到相关领域的普通技术人员所理解的这些特征的等同替代。还应当理解的是，在此使用的术语仅用于描述特定实施例的目的，而并不意味着限制。

说明书中提到的“一个实施例”或“实施例”意指结合实施例描述的特定特征、结构或特性包括在本发明的至少一个实施例中。因此，说明书通篇各个地方出现的短语“一个实施例”或“实施例”并不一定均指同一个实施例。

虽然上述示例用于说明本发明在一个或多个应用中的原理，但对于本领域的技术人员来说，在不背离本发明的原理和思想的情况下，明显可以在形式上、用法及实施的细节上作各种修改而不用付出创造性劳动。因此，本发明由所附的权利要求书来限定。

Claims (12)

1.一种用于智能驾驶系统的轨迹规划方法，其特征在于，所述方法包括：

步骤一、根据获取到的车辆的当前车速确定最优瞄准距离；

步骤二、获取位于车辆前方的预设数量的位置点的点坐标，分别计算各个位置点的曲率半径；

步骤三、根据所述各个位置点的曲率半径和所述最优瞄准距离，确定预瞄点；

步骤四、基于预设曲线拟合模型，根据所述预瞄点确定所述车辆的规划轨迹。

2.如权利要求1所述的方法，其特征在于，在所述步骤一中，将所述当前车速与预设车速阈值进行比较，并根据比较结果确定所述最优瞄准距离。

3.如权利要求2所述的方法，其特征在于，在所述步骤一中，如果所述当前车速小于预设车速阈值，则将所述车辆的最小转弯半径作为所述最优瞄准距离。

4.如权利要求2或3所述的方法，其特征在于，在所述步骤一中，如果所述当前车速大于或等于预设速度阈值，则计算所述当前车速与预瞄距离增益的乘积，并将得到的乘积结果作为所述最优瞄准距离。

5.如权利要求1～4中任一项所述的方法，其特征在于，所述步骤二包括：

步骤a、分别计算各个位置点距离所述车辆的当前位置的距离；

步骤b、分别计算各个位置点所对应的距离线段与车辆坐标系X轴的航向角；

步骤c、根据所述航向角分别计算所述各个位置点在车辆行驶方向的横向距离；

步骤d、根据所述各个位置点所对应的横向距离分别计算各个位置点的曲率半径。

6.如权利要求5所述的方法，其特征在于，在所述步骤b中，根据如下表达式计算航向角：

其中，表示位置点(x_i,y_i)与车辆的当前位置(x₀,y₀)之间的距离线段d_i与车辆坐标系X轴的航向角。

7.如权利要求5或6所述的方法，其特征在于，在所述步骤c中，根据如下表达式计算所述横向距离：

其中，表示位置点(x_i,y_i)与车辆的当前位置(x₀,y₀)之间的距离线段d_i与车辆坐标系X轴的航向角，γ_vehicle表示车辆偏航角度，m_i表示位置点(x_i,y_i)在车辆行驶方向的横向距离。

8.如权利要求1～7中任一项所述的方法，其特征在于，在所述步骤三中，从所述各个位置点中选取曲率半径大于或等于最优瞄准距离并且曲率半径与所述最优瞄准距离的差值最小的位置点作为所述预瞄点。

9.如权利要求1～8中任一项所述的方法，其特征在于，所述预设曲线拟合模型包括：

根据预瞄点坐标计算所述车辆的当前偏移量；

根据所述车辆的当前偏移量确定所述车辆的规划轨迹。

10.如权利要求9所述的方法，其特征在于，根据如下表达式计算所述车辆的当前偏移量：

y_offset＝Y_fcosθ+X_fsinθ

其中，y_offset表示当前偏移量，(X_f,Y_f,θ)表示预瞄点坐标。

11.如权利要求10所述的方法，其特征在于，根据如下表达式计算所述车辆的规划轨迹：

Y(X)＝a₀+a₁X+a₂X²+a₃X³

a₀＝a₁＝0

其中，Y(X)表示与X相关的车辆的规划轨迹，a₀、a₁、a₂和a₃均表示计算系数，y_offset表示当前偏移量，(X_f,Y_f,θ)表示预瞄点坐标。

12.一种智能驾驶系统，其特征在于，所述系统采用如权利要求1～11中任一项所述的方法进行车辆的轨迹规划。