CN110597257B - 一种基于道路曲率的常规行驶车速规划策略 - Google Patents

Abstract

本发明提供了一种基于道路曲率的常规行驶车速规划策略，包括以下步骤：通过本车环境感知系统和路径规划系统获取车辆前方一段弧长为L的目标路径上所有目标点的坐标信息及车辆自身位置信息；根据上述信息计算距离本身位置最近的最近点；根据预瞄距离确定预瞄点，进而计算汽车当前位置与预瞄点连线的直线方程；遍历最近点与预瞄点之间的所有目标点，计算这些点与上述直线的距离，进而确定最大距离点；计算最大距离点处的曲率半径，根据曲率半径计算此点目标车速，计算最大距离点与当前车辆位置间的距离，计算实现以当前车速减速到最大距离点期望车速的期望减速度；比较减速度与设定的舒适减速度，确定保持当前车速还是进行减速。

Description

一种基于道路曲率的常规行驶车速规划策略

技术领域

本发明属于自动驾驶车辆速度规划领域，尤其是涉及一种基于道路曲率的常规行驶车速规划策略。

背景技术

自动驾驶技术被视为交通安全事故、交通拥堵等问题的最终解决方案，自诞生以来一直备受各国政府、高等院校以及相关机构重视。环境感知、决策规划与运动控制被视为自动驾驶技术的三大核心问题。其中，决策规划系统负责根据环境感知系统获取的交通环境信息确定目标路径以及目标车速，而后传递给运动控制系统进行执行。对目标车速的规划是整个决策规划系统的基础也是重要组成部分，然而现有对自动驾驶目标车速的规划主要面向自适应巡航、紧急避撞或者某一类特定交通场景，或者以车辆的燃油经济性为前提进行规划，也有一些面向常规行驶工况(即前方无障碍物的一般工况)的车速规划研究，但却需要多个环境参数，对传感器要求较多，算法也相对复杂，导致系统实时性较差。

发明内容

有鉴于此，本发明旨在提出一种基于道路曲率的常规行驶车速规划策略，以解决上述背景技术中提到的问题。

为达到上述目的，本发明的技术方案是这样实现的：

一种基于道路曲率的常规行驶车速规划策略，包括以下步骤：

步骤S1：通过本车环境感知系统和路径规划系统获取车辆前方一段弧长为L的目标路径上所有目标点的坐标信息{(x₁,y₁)，(x₂,y₂)，… …}以及车辆自身位置信(X_v,Y_v)；

步骤S2：根据已知目标路径上所有目标点的坐标信息{(x₁,y₁)，(x₂,y₂)，… …}和本车位置信息(X_v,Y_v)，计算距离本身位置最近的最近点(x_n,y_n)；

步骤S3：根据预瞄距离l_d确定预瞄点(x_p,y_p)，进而计算汽车当前位置(X_v,Y_v)与预瞄点连线的直线方程f(x')；

步骤S4：遍历最近点与预瞄点之间的所有目标点，计算这些点与步骤S3中直线f(x')的距离d_i,(i＝1,2,…)，进而确定最大距离点(x_m,y_m)；

步骤S5：计算最大距离点(x_m,y_m)处的曲率半径R，根据R计算此点目标车速，当前采用计算公式为：

V_des＝a/(1+e^-b*(R-c)) (1)

其中，a,b,c为调节参数，a车速变化范围，b决定车速随曲率的变化率，c决定最低车速；

步骤S6：计算最大距离点(x_m,y_m)与当前车辆位置(X_v,Y_v)间的距离s，根据公式a_des＝(V_t ²-V_des ²)/(2*s)计算实现以当前车速V_t减速到最大距离点期望车速V_des的期望减速度；

步骤S7：比较步骤S6中减速度a_des与设定的舒适减速度a_c，若a_des≥a_c，则保持当前车速；若a_des＜a_c，则以a_des进行减速。

进一步的，所述步骤S3中，预瞄点(x_p,y_p)的确定方式如下：

1)根据如下公式确定预瞄距离：

式中，l_{d_min}为预瞄距离下限值；

2)根据步骤S1中车辆自身位置(X_v,Y_v)和步骤S1中最近点(x_n,y_n)，从最近点开始依次计算已知目标路径点与汽车位置间的距离l_i,(i＝1,2,…)，以满足误差限l_th的第一个点作为预瞄点，即满足|l_i-l_d|≤l_th，返回该点坐标；若无满足条件的目标点，则返回最后一个已知目标路径点的坐标。

进一步的，所述步骤S5中，关于最大距离点(x_m,y_m)处的曲率半径R的确定方式为：

步骤501：获取以最大距离点为中心点，前后距离s₀范围内的路径点；

步骤502：利用这些路径点，以最小二乘法进行圆弧拟合；

步骤503：根据拟合方程，得到最大距离点处的曲率半径R。

本发明的另一目的在于提出一种基于道路曲率的常规行驶车速规划装置，包括

目标数据获取装置，用于通过本车环境感知系统和路径规划系统获取车辆前方一段弧长为L的目标路径上所有目标点的坐标信息以及车辆自身位置信息；

最近点位置计算装置，用于根据已知目标路径上所有目标点的坐标信息和本车位置信息计算距离本身位置最近的最近点；

最大距离点计算装置，用于根据预瞄距离l_d确定预瞄点，进而计算汽车当前位置与预瞄点连线的直线方程；遍历最近点与预瞄点之间的所有目标点，计算这些点与直线的距离，进而确定最大距离点；

目标点车速计算装置，用于计算最大距离点处的曲率半径，根据曲率半径计算此点目标车速；

期望减速度获取装置，用于计算最大距离点与当前车辆位置间的距离，根据公式计算实现以当前车速减速到最大距离点期望车速的期望减速度；

结果执行装置，用于比较期望减速度与设定的舒适减速度从而控制车轮保持当前车速或者进行减速。

相对于现有技术，本发明所述的一种基于道路曲率的常规行驶车速规划策略具有以下优势：

(1)本发明对传感器依赖较少，只需获取目标路径上目标点位置信息以及本车位置信息，算法简单实用，实时性好；

(2)本发明利用车辆与预瞄点间连线的最大距离点曲率信息计算车速，这种方式可将车辆自身的位姿信息考虑在内，更为合理；

(3)本发明在车辆减速方面同时兼顾了对乘员舒适性问题的考虑。

附图说明

构成本发明的一部分的附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：

图1为基于道路曲率的常规行驶车速策略原理示意图。

图2为基于道路曲率的常规行驶车速策略计算流程图。

具体实施方式

需要说明的是，在不冲突的情况下，本发明中的实施例及实施例中的特征可以相互组合。

下面将参考附图并结合实施例来详细说明本发明。

如图1、图2所示，一种基于道路曲率的常规行驶车速规划策略，包括以下步骤：

S1、通过本车环境感知系统和路径规划系统获取车辆前方一段弧长为L的目标路径上所有目标点的坐标信息{(x₁,y₁)，(x₂,y₂)，… …}；通过本车环境感知系统获取车辆自身位置信息(X_v,Y_v)；目标路径的弧长L由本车的环境感知传感器参数以及路径规划算法决定；

S2、根据已知目标路径上所有目标点的坐标信息{(x₁,y₁)，(x₂,y₂)，… …}和本车位置信息(X_v,Y_v)，计算距离本身位置最近的最近点(x_n,y_n)；

S3、根据预瞄距离l_d确定预瞄点(x_p,y_p)，进而计算汽车当前位置(X_v,Y_v)与预瞄点连线的直线方程f(x')；

具体的，关于预瞄点(x_p,y_p)的确定方式如下：

1)根据如下公式确定预瞄距离：

式中，l_{d_min}为预瞄距离下限值；

2)根据步骤S1中车辆自身位置(X_v,Y_v)和步骤S1中最近点(x_n,y_n)，从最近点开始依次计算已知目标路径点与汽车位置间的距离l_i,(i＝1,2,…)，以满足误差限l_th的第一个点作为预瞄点，即满足|l_i-l_d|≤l_th，返回该点坐标；；若无满足条件的目标点，则返回最后一个已知目标路径点的坐标；

S4、遍历最近点与预瞄点之间的所有目标点，计算这些点与步骤S3中直线f(x')的距离d_i,(i＝1,2,…)，进而确定最大距离点(x_m,y_m)；

S5、计算最大距离点(x_m,y_m)处的曲率半径R，根据R计算此点目标车速，当前采用计算公式为：

V_des＝a/(1+e^-b*(R-c)) (1)

其中，a,b,c为调节参数，a车速变化范围，b决定车速随曲率的变化率，c决定最低车速；

具体的，关于最大距离点(x_m,y_m)处的曲率半径R的确定方式为：

1)获取以最大距离点为中心点，前后距离s₀范围内的路径点；

2)利用这些路径点，以最小二乘法进行圆弧拟合；

3)根据拟合方程，得到最大距离点处的曲率半径R。

S6、计算最大距离点(x_m,y_m)与当前车辆位置(X_v,Y_v)间的距离s，根据公式a_des＝(V_t ²-V_des ²)/(2*s)计算实现以当前车速V_t减速到最大距离点期望车速V_des的期望减速度；

S7、比较步骤S6中减速度a_des与设定的舒适减速度a_c，若a_des≥a_c，则保持当前车速；若a_des＜a_c，则以a_des进行减速。

本发明的另一目的在于提出一种基于道路曲率的常规行驶车速规划装置，包括

目标数据获取装置，用于通过本车环境感知系统和路径规划系统获取车辆前方一段弧长为L的目标路径上所有目标点的坐标信息以及车辆自身位置信息；

最近点位置计算装置，用于根据已知目标路径上所有目标点的坐标信息和本车位置信息计算距离本身位置最近的最近点；

最大距离点计算装置，用于根据预瞄距离l_d确定预瞄点，进而计算汽车当前位置与预瞄点连线的直线方程；遍历最近点与预瞄点之间的所有目标点，计算这些点与直线的距离，进而确定最大距离点；

目标点车速计算装置，用于计算最大距离点处的曲率半径，根据曲率半径计算此点目标车速；

期望减速度获取装置，用于计算最大距离点与当前车辆位置间的距离，根据公式计算实现以当前车速减速到最大距离点期望车速的期望减速度；

结果执行装置，用于比较期望减速度与设定的舒适减速度从而控制车轮保持当前车速或者进行减速。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (4)

1.一种基于道路曲率的常规行驶车速规划策略，其特征在于：包括以下步骤：

步骤S1：通过本车环境感知系统和路径规划系统获取车辆前方一段弧长为L的目标路径上所有目标点的坐标信息{(x₁,y₁)，(x₂,y₂)，…，(x_N,y_N)}以及车辆自身位置信息(X_v,Y_v)；

步骤S2：根据已知目标路径上所有目标点的坐标信息{(x₁,y₁)，(x₂,y₂)，…，(x_N,y_N)}和本车位置信息(X_v,Y_v)，计算距离本车位置最近的最近点(x_n,y_n)；

步骤S3：根据预瞄距离l_d确定预瞄点(x_p,y_p)，进而计算汽车当前位置信息(X_v,Y_v)与预瞄点连线的直线方程f(x')；

步骤S4：遍历最近点与预瞄点之间的所有目标点，计算这些点与步骤S3中直线f(x')的距离d_i，i＝1,2,…，进而确定最大距离点(x_m,y_m)；

步骤S5：计算最大距离点(x_m,y_m)处的曲率半径R，根据R计算最大距离点期望车速，当前采用计算公式为：

V_des＝a/(1+e^-b*(R-c)) (1)

其中，a,b,c为调节参数，a决定车速变化范围，b决定车速随曲率的变化率，c决定最低车速；

步骤S6：计算最大距离点(x_m,y_m)与当前车辆位置(X_v,Y_v)间的距离s，根据公式a_des＝(V_t ²-V_des ²)/(2*s)计算实现以当前车速V_t减速到最大距离点期望车速V_des的期望减速度；

步骤S7：比较步骤S6中减速度a_des与设定的舒适减速度a_c，若a_des≥a_c，则保持当前车速；若a_des＜a_c，则以a_des进行减速。

2.根据权利要求1所述的一种基于道路曲率的常规行驶车速规划策略，其特征在于：所述步骤S3中，预瞄点(x_p,y_p)的确定方式如下：

1)根据如下公式确定预瞄距离：

式中，l_{d_min}为预瞄距离下限值；

2)根据步骤S1中车辆自身位置(X_v,Y_v)和步骤S1中最近点(x_n,y_n)，从最近点开始依次计算已知目标路径点与汽车位置间的距离l_i，i＝1,2,…，以满足误差限l_th的第一个点作为预瞄点，即满足|l_i-l_d|≤l_th，返回该点坐标；若无满足条件的目标点，则返回最后一个已知目标路径点的坐标。

3.根据权利要求1所述的一种基于道路曲率的常规行驶车速规划策略，其特征在于：所述步骤S5中，关于最大距离点(x_m,y_m)处的曲率半径R的确定方式为：

步骤501：获取以最大距离点为中心点，前后距离s₀范围内的路径点；

步骤502：利用这些路径点，以最小二乘法进行圆弧拟合；

步骤503：根据拟合方程，得到最大距离点处的曲率半径R。

4.应用权利要求1所述的基于道路曲率的常规行驶车速规划策略的一种基于道路曲率的常规行驶车速规划装置，其特征在于：包括

目标数据获取装置，用于通过本车环境感知系统和路径规划系统获取车辆前方一段弧长为L的目标路径上所有目标点的坐标信息以及车辆自身位置信息；

最近点位置计算装置，用于根据已知目标路径上所有目标点的坐标信息和车辆自身位置信息计算距离本身位置最近的最近点；

最大距离点计算装置，用于根据预瞄距离l_d确定预瞄点，进而计算汽车当前位置与预瞄点连线的直线方程；遍历最近点与预瞄点之间的所有目标点，计算这些点与直线的距离，进而确定最大距离点；

目标点车速计算装置，用于计算最大距离点处的曲率半径，根据曲率半径计算此点期望车速；

期望减速度获取装置，用于计算最大距离点与当前车辆位置间的距离，根据公式计算实现以当前车速减速到最大距离点期望车速的期望减速度；

结果执行装置，用于比较期望减速度与设定的舒适减速度从而控制车轮保持当前车速或者进行减速。

Images