CN109059944B - 基于驾驶习惯学习的运动规划方法 - Google Patents

Abstract

本发明涉及基于驾驶习惯学习的运动规划方法，该方法包括以下步骤：步骤1、云处理端采集人工驾驶轨迹，通过图像化处理，提取出带有道路交通属性的预瞄点；步骤2、云处理端根据自动驾驶车辆的状态实时下发车辆前方的预瞄点；步骤3、自动驾驶车辆根据设定的路径生成算法，生成车到各预瞄点之间备选轨迹簇，并根据代价函数选择一条最优路径。与现有技术相比，本发明具有以下优点：使用人工驾驶轨迹产生局部规划路径，摆脱对高精度地图的依赖；预瞄点增加新的属性，且生成方式的改变,使得局部规划的路径更符合人的驾驶习惯。

Description

基于驾驶习惯学习的运动规划方法

技术领域

本发明涉及无人驾驶领域，尤其是涉及基于驾驶习惯学习的运动规划方法。

背景技术

无人驾驶汽车是通过车载传感系统感知道路环境，自动规划行车路线并控制车辆到达预定目标的智能汽车。其中，局部路径规划是环境感知和车辆控制的桥梁，规划算法的性能优劣直接影响无人车能否在复杂交通场景中的自主行驶。

如今，城市道路结构越来越复杂、道路上车辆越来越多，路径规划的问题越来越重要。目前的规划方法需要依赖高精度地图，地图信息不完善时就无法适应错综复杂的道路结构及交通规则、实时变化的交通状况以及大量频繁的路径规划。

1、现有技术一

依靠地图中心线的局部路径规划技术方案：

高精度地图提供道路中心线，车辆实时匹配到道路中心线上的某点，如图1所示。再向前一段距离(一次规划的长度)找道路中心线上的点，并做横向偏移生成均匀的预瞄点。车辆所在位置与各预瞄点之间分别生成平滑的曲线，从而得到一个轨迹簇，如图2所示。最后通过代价函数从所有的备选轨迹中选出一条最优路径。

缺点：

(1)该现有技术主要存在以下两个缺点：

(2)该路径规划方法依赖于高精度的地图数据；

(3)道路中心线和其横向偏移生成的预瞄点，难以描述复杂的道路结构及交通规则。

2、现有技术二

基于直接映射的局部路径规划技术方案：

这种方法的输入是当前无人车视场中的图像，输出是当前的转向角度。其需要的训练数据是人驾驶车辆采集的一段图像，同时记录人为控制时转向的角度，通过深度学习获得转向角模型，即可从实时图像输入获得转向角，如图3所示。

缺点：

(1)通过图像直接映射出方向盘转角的轨迹没法与实际轨迹完全吻合，还可能出现误匹配和匹配不上的情况；如图4所示，预测的轨迹与实际轨迹存在偏差：图中蓝色为记录真实轨迹，绿色为通过该技术规划的轨迹。

(2)道路中心线和其横向偏移生成的预瞄点，难以描述复杂的道路结构及交通规则。

发明内容

本发明的目的就是为了克服上述现有技术存在的缺陷而提供基于驾驶习惯学习的运动规划方法。

本发明的目的可以通过以下技术方案来实现：

基于驾驶习惯学习的运动规划方法，该方法包括以下步骤：

步骤1、云处理端采集人工驾驶轨迹，通过图像化处理，提取出带有道路交通属性的预瞄点；

步骤2、云处理端根据自动驾驶车辆的状态实时下发车辆前方的预瞄点；

步骤3、自动驾驶车辆根据设定的路径生成算法，生成车到各预瞄点之间备选轨迹簇，并根据代价函数选择一条最优路径。

优选地，所述的步骤1中的云处理端采集的轨迹来自于城市出租车系统、车载导航系统或数据采集车的采集设备采集的不同精度水平的轨迹信息。

优选地，所述的预瞄点的属性包括：

(1)ID：n1,n2,…；

(2)时间：生成该预瞄点的轨迹的时间戳；

(3)坐标x，y：全局坐标系的位置坐标；

(4)头指向取值：在设定范围内的离散化取值，每个值对应一个权重；

(5)曲率取值：在设定范围内的离散化取值，每个值对应一个权重；

(6)速率取值：在设定范围内的离散化取值，每个值对应一个权重；

(7)点之间的拓扑关系：与该点存在连通关系的点的ID序列。

优选地，所述的步骤1中的图像化处理包括以下步骤：

步骤1.1、根据轨迹点的属性，为各条轨迹上的点分配相应的权重值，将带有不同权重的轨迹投影到同一图像平面上；各条轨迹不同的权重值用不同的颜色表示；

步骤1.2、通过matlab描出轨迹的图形，保存为图像；

步骤1.3、用高斯卷积模板对图像进行平滑处理，得到轨迹密度热力图。

优选地，所述的步骤1中的提取预瞄点具体包括：在设定的位置取横截面，得到横截面上密度热力值最高的几个点，作为预瞄点，其中设定位置包括里程、曲率或路口的关键位置。

优选地，所述的预瞄点属性生成具体包括：得到预瞄点的位置坐标后，以预瞄点位置为圆心，半径为0.5米画圆，统计落在圆内的轨迹点属性，生成预瞄点的属性。

优选地，

若θ为头指向，假设落在圆内的轨迹点有N个，这些轨迹点的最大和最小头指向分别为max(θ_i)和min(θ_i)，i＝1,2,…,N，预瞄点的头指向是对这个区间的轨迹点头指向的离散化取值，用m表示划分的段数，j＝0,1,...,m，则预瞄点的头指向为

其中头指向对应的权重值

P_i是轨迹点所在轨迹经过最小二乘拟合后得到的权重值，如果

小于设定的阈值，则认为θ_j为粗差，将其剔除；

分别用曲率k和速率v代替上面公式中的θ，可以计算得到预瞄点曲率及速率的取值及其对应的权重值；

如果，两预瞄点的搜索区域内有10％的轨迹点在同一条轨迹上，则认为这两个预瞄点相互连通。

优选地，所述的步骤2中的自动驾驶车辆的状态包括位置、速度、头指向状态信息。

优选地，所述的步骤3具体包括以下步骤：

步骤3.1、通过驾驶轨迹得到带有各类属性的预瞄点后，用一个矢量代表车辆状态[x,y,θ,k],其中(x,y)代表车辆的位置；根据车辆当前状态P_init＝[x_I,y_I,θ_I,k_I]和任一预瞄点的状态P_goal＝[x_G,y_G,θ_G,k_G]，生成一条确定的平滑曲线；

步骤3.2、预瞄点的头指向和曲率在各自允许的范围内取不同的值，这样到达同一个终点位置可得到多条不同走法的路径；

步骤3.3、计算每条轨迹的代价值，选取代价值最小的路径，代价函数为：

其中，C_col表示障碍物的碰撞代价，C_s是路径的里程代价，C_θ为头指向和头指向的变化率过大产生的代价，C_k表示曲率和曲率的变化率过大产生的代价，

分别为选取的头指向和曲率对应的权重值倒数。

与现有技术相比，本发明具有以下优点：

1、充分利用不同精度的轨迹数据，使得规划不再依赖于高精度地图；只需要下发预瞄点，而不是大量地图和轨迹数据，大大减小数据传输量和数据冗余。

2、通过海量轨迹生成的预瞄点反应了人工驾驶最集中经过的点，以及驶过该点时的头指向、曲率。速度等信息，由此生成的规划路线更符合人工驾驶习惯；通过聚类的从驾驶轨迹中提取出预瞄点，而不是直接使用驾驶轨迹指导局部规划，解决了海量轨迹数据精度不一和误差噪声的影响。

3、生成的预瞄点的头指向、曲率、速度是在一定范围内的变化值，而不是某个固定值，由此可以生成更加灵活多样的路径以供选择，使得局部规划更加多变，以应对错综复杂的道路和交通状况。

附图说明

图1为现有技术一的车辆实时匹配到道路中心线上的某点示意图；

图2为现有技术一的基于道路中心线生成轨迹簇示意图；

图3为现有技术二的基于直接映射的局部路径规划技术方案示意图；

图4为现有技术二的预测的轨迹与实际轨迹存在偏差示意图；

图5为本发明的方法流程框架图；

图6为本发明的轨迹图像图；

图7为本发明的轨迹密度热力图；

图8为本发明的预瞄点选取位置示意图；

图9为本发明的预瞄点的属性生成示意图；

图10为本发明的多条路径曲线生成图；

图11为本发明的实施例一的人工驾驶轨迹分段示意图；

图12为本发明的实施例一的轨迹点最小二乘拟合示意图；

图13为本发明的实施例一的轨迹图像图；

图14为本发明的实施例一的处理后图像图；

图15为本发明的实施例一的横截面密度热力图；

图16为本发明的实施例二的十字路口轨迹密度热力图。

具体实施方式

下面将对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明的一部分实施例，而不是全部实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都应属于本发明保护的范围。

本发明所要解决的技术问题

现有的局部路径规划方法主要基于高精度地图，然而高精度地图制作成本高，制作周期长，制作难度大。在没有高精度地图的情况下，规划算法受到了极大的限制。另外，智能汽车需要面对越来越复杂的道路结构和交通环境，现有的规划方案无法有效应对如此复杂多样化的实际场景。然而在各种场景下都会有大量的人工驾驶轨迹，只要采集这些人工驾驶轨迹并作合适的处理，就可以很好的知道智能驾驶。因此，本专利设计一种基于大量驾驶轨迹生成预瞄点的局部路径规划方法，通过众包人工驾驶轨迹从而摆脱对高精度地图的依赖，并通过图像处理的方法得到人工驾驶最频繁经过的点以及点的状态，从而在各种复杂的情况下，都能规划出更合理的行车轨迹。

本发明的主要发明要点概述

1)不再依赖于高精度地图，而是采用众包的方式采集大量人工驾驶轨迹，处理产生局部规划路径。

2)采用图像处理的方式分析处理大量轨迹数据，直接提取出带有道路交通属性的预瞄点。

3)预瞄点添加了其他属性，能反应道路结构和拓扑关系，以及交通规则。

本发明应用的系统架构或场景

云处理端收集来自于城市出租车系统、车载导航系统或高精度采集车等采集设备采集的不同精度水平的轨迹信息。这些海量的轨迹数据包含了丰富的道路信息和交通动态信息。

云处理端从海量的人工驾驶轨迹中，通过图像化处理，提取出带有道路交通属性的预瞄点(反应道路结构和拓扑关系，以及交通规则)。

云处理端根据自动驾驶车辆的状态(位置、速度、头指向等)实时下发车辆前方的预瞄点，用以单车的局部规划。

自动驾驶车辆根据一定的路径生成算法，生成车到各预瞄点之间备选轨迹簇，并根据代价函数选择一条最优路径。

本发明的具体技术方案

1.预瞄点属性

1)ID：n1,n2,…

2)时间：生成该预瞄点的轨迹的时间戳

3)坐标x，y：全局坐标系的位置坐标

4)头指向取值：在一定范围内的离散化取值，每个值对应一个权重。

5)曲率取值：在一定范围内的离散化取值，每个值对应一个权重。

6)速率取值：在一定范围内的离散化取值，每个值对应一个权重。

7)点之间的拓扑关系：与该点存在连通关系的点的ID序列。

2.轨迹图像化处理

海量的轨迹数据，由于精度不一，存在不同程度的噪声。根据轨迹点的属性，为各条轨迹上的点分配相应的权重值。将带有不同权重的轨迹投影到同一图像平面上。如图6所示，图中同一个颜色的虚线为同一辆的轨迹，不同颜色给定了轨迹的不同权值，通过matlab描出轨迹的图形，保存为图像，接着对图像平滑处理，得出轨迹密度热力图。

其中，利用二维高斯函数生成20*20的卷积模板，对图像做步长为1的卷积运算，得到如图7所示的轨迹密度热力图。

3.预瞄点的位置生成

在合适的位置(里程、曲率、路口等关键位置)取横截面，得到横截面上密度热力值最高的几个点，作为预瞄点。

4.预瞄点的属性生成

得到预瞄点的位置坐标后，以预瞄点位置为圆心，半径为0.5米画圆，如图9所示。统计落在圆内的轨迹点属性，生成预瞄点的属性。

以头指向为例，假设落在圆内的轨迹点有N个，头指向为

m表示划分的段数，j＝0,1,...,m。头指向对应的权重值

如果

小于一定的阈值，则认为θ_j为粗差，将其剔除。

类似地，可以确定预瞄点的曲率和速度的取值及其对应的权重值。如果，两预瞄点的搜索区域内有10％的轨迹点在同一条轨迹上，则认为这两个预瞄点相互连通。

5.路径生成

通过驾驶轨迹得到带有各类属性的预瞄点后，根据车辆当前状态P_init＝[x_I,y_I,θ_I,k_I]和任一预瞄点的状态P_goal＝[x_G,y_G,θ_G,k_G]，可以生成一条确定的平滑曲线。

预瞄点的头指向和曲率在各自允许的范围内取不同的值，这样到达同一个终点位置可以得到多条不同走法的路径，如图10所示。从而使得路径规划更加灵活，以应对复杂的路况。

最后计算每条轨迹的代价值，选取代价值最小的路径。代价函数如下所示：

其中，C_col表示障碍物的碰撞代价，C_s是路径的里程代价，C_θ为头指向和头指向的变化率过大产生的代价，C_k表示曲率和曲率的变化率过大产生的代价，

分别为选取的头指向和曲率对应的权重值倒数。

本发明实施例一

(1)具体步骤：

由于采集的人工驾驶轨迹可能很长，所以先对轨迹进行分段，如图11所示。

对每段轨迹进行最小二乘拟合：

y＝a₀+a₁x+a₂x²+...+a_mx^m

并求每段轨迹的中误差：

轨迹点最小二乘拟合如图12所示。

同理，可以每段轨迹头指向，曲率，速度等信息的中误差，那么，该段轨迹上每个点的权重值

σ_xy表示行驶轨迹A的轨迹点的中误差；σ_θ表示行驶轨迹A的头指向的中误差；σ_k表示行驶轨迹A的曲率的中误差；σ_v表示行驶轨迹A的速度的中误差；∝、β、γ、

为常数。

将不同权重的轨迹投影到一个平面内，各条轨迹不同的权重值用不同的颜色表示。保存图形为轨迹图像，如图13所示。

用高斯卷积模板对图像进行平滑处理，效果如图14所示，颜色反应了点的密集程度。

选取一条权重最高的轨迹，如图13中红色曲线所示，计算轨迹上各点的里程和曲率，根据里程和曲率对图像进行分割，求横截面上密度热力值，如图15所示。取波峰位置的点作为预瞄点。

接着计算这些预瞄点的属性，生成车到各预瞄点之间备选轨迹簇，并根据代价函数选择一条最优路径。

(2)本发明实施例一的技术效果：

(2.1)智能驾驶不再依赖于高精度地图，而是通过对人工驾驶轨迹的处理生成最优路径。

(2.2)预瞄点不再是通过中心线上点的横向偏移生成，而是由人工驾驶最常经过的点产生，更符合人的驾驶习惯。

(3)本发明实施例一区别于现有技术的改进之处：

(3.1)使用人工驾驶轨迹产生局部规划路径，摆脱对高精度地图的依赖。

(3.2)预瞄点增加新的属性，且生成方式的改变,使得局部规划的路径更符合人的驾驶习惯。

2、本发明实施例二

我们还可以通过对轨迹图像的处理，得到路口等信息。如图16所示，轨迹密度热力值最高的区域为十字路口区域，我们可以通过膨胀、边缘检测的图像操作提取出路口的区域，如图红色方框所示。

具体步骤：以该边界线为横截面，提取出路口等关键位置的预瞄点。后续过程与实施例一中一致。

3、本发明关键技术点概括

(1)众包采集人工驾驶轨迹的方法和流程，对不同来源的轨迹分配不同的权重值，并把轨迹点投影到像素平面上，通过图形处理得到预描点的位置分布。

(2)预描点的定义包括预描点属性及生成方法和步骤，预瞄点除坐标外还包含头指向、曲率、速率及其变化范围和对应的权重值，以及点之间的拓扑关系等属性，可以直接通过人工驾驶轨迹得到更加合理的预瞄点及其属性，用作后续的最优路径选择。

以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到各种等效的修改或替换，这些修改或替换都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以权利要求的保护范围为准。

Claims (7)

1.基于驾驶习惯学习的运动规划方法，其特征在于，该方法包括以下步骤：

步骤1、云处理端采集人工驾驶轨迹，通过图像化处理，提取出带有道路交通属性的预瞄点；

步骤2、云处理端根据自动驾驶车辆的状态实时下发车辆前方的预瞄点；

步骤3、自动驾驶车辆根据设定的路径生成算法，生成车到各预瞄点之间备选轨迹簇，并根据代价函数选择一条最优路径；

所述的预瞄点属性生成具体包括：得到预瞄点的位置坐标后，以预瞄点位置为圆心，半径为0.5米画圆，统计落在圆内的轨迹点属性，生成预瞄点的属性；

若θ为头指向，假设落在圆内的轨迹点有N个，这些轨迹点的最大和最小头指向分别为max(θ_i)和min(θ_i)，i＝1,2,…,N，预瞄点的头指向是对这个区间的轨迹点头指向的离散化取值，用m表示划分的段数，j＝0,1,...,m，则预瞄点的头指向为

其中头指向对应的权重值

P_i是轨迹点所在轨迹经过最小二乘拟合后得到的权重值，如果

小于设定的阈值，则认为θ_j为粗差，将其剔除；

分别用曲率k和速率v代替上面公式中的θ，可以计算得到预瞄点曲率及速率的取值及其对应的权重值；

如果，两预瞄点的搜索区域内有10％的轨迹点在同一条轨迹上，则认为这两个预瞄点相互连通。

2.根据权利要求1所述的基于驾驶习惯学习的运动规划方法，其特征在于，所述的步骤1中的云处理端采集的轨迹来自于城市出租车系统、车载导航系统或数据采集车的采集设备采集的不同精度水平的轨迹信息。

3.根据权利要求1所述的基于驾驶习惯学习的运动规划方法，其特征在于，所述的预瞄点的属性包括：

(1)ID：n1,n2,…；

(2)时间：生成该预瞄点的轨迹的时间戳；

(3)坐标x，y：全局坐标系的位置坐标；

(4)头指向取值：在设定范围内的离散化取值，每个值对应一个权重；

(5)曲率取值：在设定范围内的离散化取值，每个值对应一个权重；

(6)速率取值：在设定范围内的离散化取值，每个值对应一个权重；

(7)点之间的拓扑关系：与该点存在连通关系的点的ID序列。

4.根据权利要求1所述的基于驾驶习惯学习的运动规划方法，其特征在于，所述的步骤1中的图像化处理包括以下步骤：

步骤1.1、根据轨迹点的属性，为各条轨迹上的点分配相应的权重值，将带有不同权重的轨迹投影到同一图像平面上；各条轨迹不同的权重值用不同的颜色表示；

步骤1.2、通过matlab描出轨迹的图形，保存为图像；

步骤1.3、用高斯卷积模板对图像进行平滑处理，得到轨迹密度热力图。

5.根据权利要求4所述的基于驾驶习惯学习的运动规划方法，其特征在于，所述的步骤1中的提取预瞄点具体包括：在设定的位置取横截面，得到横截面上密度热力值最高的几个点，作为预瞄点，其中设定位置包括里程、曲率或路口的关键位置。

6.根据权利要求1所述的基于驾驶习惯学习的运动规划方法，其特征在于，所述的步骤2中的自动驾驶车辆的状态包括位置、速度、头指向状态信息。

7.根据权利要求1所述的基于驾驶习惯学习的运动规划方法，其特征在于，所述的步骤3具体包括以下步骤：

步骤3.1、通过驾驶轨迹得到带有各类属性的预瞄点后，用一个矢量代表车辆状态[x,y,θ,k],其中(x,y)代表车辆的位置；根据车辆当前状态P_init＝[x_I,y_I,θ_I,k_I]和任一预瞄点的状态P_goal＝[x_G,y_G,θ_G,k_G]，生成一条确定的平滑曲线；

步骤3.2、预瞄点的头指向和曲率在各自允许的范围内取不同的值，这样到达同一个终点位置可得到多条不同走法的路径；

步骤3.3、计算每条轨迹的代价值，选取代价值最小的路径，代价函数为：

其中，C_col表示障碍物的碰撞代价，C_s是路径的里程代价，C_θ为头指向和头指向的变化率过大产生的代价，C_k表示曲率和曲率的变化率过大产生的代价，

分别为选取的头指向和曲率对应的权重值倒数。

Images