CN113593238B - 一种面向自动驾驶导航的路口虚拟车道建模方法 - Google Patents

Abstract

本发明是一种面向自动驾驶导航的路口虚拟车道建模方法。包括以下步骤：一、给定车辆在路口实际行驶的轨迹点；二、设置拟合的起终点；三、确定优化的目标函数；步骤四、初次拟合，将起终点斜率作为优化变量，分段拟合，将上一段终点斜率赋值给下一段起点斜率，并将下一段终点斜率作为优化变量，对目标函数进行最优求解；五、设置误差阈值，比较目标函数最小值是否大于阈值，如果大于，返回步骤二，反之，将终点的序号加1并返回步骤三。本发明采用三次曲线对路口虚拟车道进行建模，模型可用性强，保证C1连续性，能够很好的近似车辆实际行驶的轨迹，符合车辆动力学要求，更好地引导智能车辆在路口行驶，满足自动驾驶导航的需要。

Description

一种面向自动驾驶导航的路口虚拟车道建模方法

技术领域

本发明属于自动驾驶技术领域，具体的说是一种面向自动驾驶导航的路口虚拟车道建模方法。

背景技术

近些年来，自动驾驶迅速发展，以求带来更高水平的安全和便利，自动驾驶导航技术也随之逐渐发展起来，作为其中重要组成的全局路径规划，要求能够根据现有电子地图，规划从起点到终点的可行路径。现有很多成熟的导航软件都能够进行路径规划，但是它是针对人类驾驶员设计的，在规划路径时是基于道路层面的，且只是给驾驶员一个如何行驶的指令，例如前方路口左转，在这个过程中，驾驶员是主体，导航软件起辅助作用，并不适用于无人驾驶。

自动驾驶导航系统要求能够给出不考虑交通流干扰下的一条可行轨迹，对于车道内行驶，可以让车辆跟随车道中心线行驶，即将车道中心线作为智能车的参考轨迹，利用它来引导智能车，现有的车道保持系统就是如此。相比于在道路内的情况，车辆在路口行驶具有较大的难度，首先，路口缺少车道边界线及一些标志，没有可以遵循的中心线，其次，进入和离开路口的车道较多，且路口通常包含左转、直行、右转等多种行为，情况较复杂，如何合理的建立引导智能车在路口行驶的虚拟车道是自动驾驶导航的关键技术。

在路口内生成虚拟车道，是指从进入路口的车道端点到离开路口的车道端点之间生成用来引导车辆行驶的轨迹，它需要很好的近似车辆实际在路口的行驶轨迹，从而很好的满足车辆动力学约束，同时要有很好的可用性(具体指提取包括坐标、切线角度或曲率等在内的道路几何信息所需的计算量应该很低，以便车辆实时应用)，同时考虑到车辆的运动无论在位置还是速度上都是连续的，轨迹在位置和切向量上都应该具有连续性(又称C1连续性)，如此，虚拟车道将更好地引导智能车在路口行驶。

发明内容

本发明提供了一种面向自动驾驶导航的路口虚拟车道建模方法，考虑到使用三次多项式建模能够轻松获得位置坐标、切线角度或曲率等在内的道路几何信息，模型可用性好，该建模方法采用三次曲线对路口虚拟车道进行建模，同时考虑切向向量优化，以及位置和切向量上的连续性约束，让生成的虚拟车道能够很好的近似车辆实际行驶的轨迹，符合车辆动力学要求，更好地引导智能车辆在路口行驶，解决了自动驾驶导航在路口生成虚拟车道存在的上述问题。

本发明技术方案结合附图说明如下：

一种面向自动驾驶导航的路口虚拟车道建模方法，包括以下步骤：

步骤一、设定车辆在路口实际行驶的轨迹点；

步骤二、设置拟合的起点和初始终点；

步骤三、将轨迹点横坐标带入曲线方程确定优化的目标函数；

步骤四、初次拟合，将起点和终点的斜率作为优化变量，对目标函数进行最优求解；若需要进行分段拟合，将上一段终点斜率赋值给下一段起点斜率，并将下一段终点斜率作为优化变量对目标函数进行最优求解；

步骤五、设置误差阈值，比较目标函数最小值是否大于阈值，如果目标函数最小值大于阈值，返回步骤二，如果目标函数最小值小于阈值，将终点的序号加1并返回步骤三直到最后一个点。

所述步骤一的具体方法如下：

设车辆在路口行驶的一系列轨迹点为

其中，x_i,y_i分别为第i个轨迹点的横纵坐标；n为轨迹点数目。

所述步骤二的具体方法如下：

设拟合起点为第i个点，终点为第j个点，第一次拟合时，采用从起始点开始的4个点，即j＝i+3。

所述步骤三的具体方法如下：

31)建立三次多项式曲线，如下：

y＝a₀+a₁x+a₂x²+a₃x³ (1)

32)式(1)的导数表达式，如下：

y′＝a₁+2a₂·x+3a₃·x² (2)

33)路口虚拟轨迹的起点和终点分别为(x₀，y₀)，(x₁，y₁)，起点和终点处斜率分别为k₀和k₁；设路口轨迹的起点位于原点，代入公式(1)得a₀＝0；将起点斜率k₀代入公式(2)得a₁＝k₀；

34)将a₀＝0和a₁＝k₀代入公式(1)得：

y＝k₀·x+a₂·x²+a₃·x³ (3)

将a₁＝k₀代入公式(2)得：

y′＝k₀+2a₂·x+3a₃·x² (4)

将终点坐标(x₁，y₁)代入公式(3)得：

y₁＝k₀·x₁+a₂·x₁ ²+a₃·x₁ ³ (5)

将终点斜率k₁代入公式(4)得：

k₁＝k₀+2a₂·x₁+3a₃·x₁ ² (6)

由公式(5)和(6)得：

将k₀和k₁作为待优化的变量，a₂和a₃用关于k₀和k₁的表达式表达如式(7)(8)所示，将(7)和(8)代入(3)得y关于k₀和k₁的表达式(9)：

35)将从i到j的每个点的横坐标代入(9)中得每个点对应在曲线方程中的纵坐标值；设定目标函数为：

其中，y是将数据点的横坐标代入构建的曲线方程计算得到的纵坐标值；y₁是数据点的真实纵坐标值。

所述步骤四的具体方法如下：

第一次拟合时，将k₀和k₁作为优化变量，对目标函数(10)进行最优求解，求得使目标函数最小时对应的k₀和k₁值；若分段进行多次拟合，则将上一段终点的斜率k₁赋值给下一段的起点斜率k₀，将下一段的终点斜率k₁作为优化变量，对目标函数(10)进行最优求解，求得使目标函数最小时对应的k₁值。

所述步骤五的具体方法如下：

设定误差阈值为ε，当式(10)的最小值超过阈值时，将j的值分配给i作为新的分段点，返回步骤二，否则j的值增加1并返回步骤三直到最后一个点。

本发明的有益效果为：

1)本发明采用三次曲线对路口虚拟车道进行建模，让生成的虚拟车道满足位置和切向量上的连续性，并且能够很好的近似车辆实际行驶的轨迹，符合车辆动力学要求，更好地引导智能车辆在路口行驶；

2)本发明采用的三次多项式模型能够轻松获得位置坐标、切线角度或曲率等在内的道路几何信息，模型可用性好。

附图说明

为了更清楚地说明本发明实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本发明的某些实施例，因此不应被看作是对范围的限定，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他相关的附图。

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

图2为一段车辆在路口的真实行驶轨迹点以及建模的虚拟车道模型的对比图；

图3为另一段车辆在路口的真实行驶轨迹点以及建模的虚拟车道模型的对比图。

具体实施方式

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

参阅图1，一种面向自动驾驶导航的路口虚拟车道建模方法，包括以下步骤：

步骤一、设车辆在路口行驶的一系列轨迹点为

其中，x_i，y_i分别为第i个轨迹点的横纵坐标；n为轨迹点数目。

步骤二、设拟合起点为第i个点，终点为第j个点，第一次拟合时，采用从起始点开始的4个点，即j＝i+3。

步骤三、将轨迹点横坐标带入曲线方程确定优化的目标函数；

31)建立三次多项式曲线，如下：

y＝a₀+a₁x+a₂x²+a₃x³ (1)

32)式(1)的导数表达式，如下：

y′＝a₁+2a₂·x+3a₃·x² (2)

33)路口虚拟轨迹的起点和终点分别为(x₀，y₀)，(x₁，y₁)，起点和终点处斜率分别为k₀和k₁；为了计算方便，设路口轨迹的起点位于原点，代入公式(1)得a₀＝0；将起点斜率k₀代入公式(2)得a₁＝k₀；

34)将a₀＝0和a₁＝k₀代入公式(1)得：

y＝k₀·x+a₂·x²+a₃·x³ (3)

将a₁＝k₀代入公式(2)得：

y′＝k₀+2a₂·x+3a₃·x² (4)

将终点坐标(x₁，y₁)代入公式(3)得：

y₁＝k₀·x₁+a₂·x₁ ²+a₃·x₁ ³ (5)

将终点斜率k₁代入公式(4)得：

k₁＝k₀+2a₂·x₁+3a₃·x₁ ² (6)

由公式(5)和(6)得：

将k₀和k₁作为待优化的变量，a₂和a₃用关于k₀和k₁的表达式表达如式(7)(8)所示，将(7)和(8)代入(3)得y关于k₀和k₁的表达式(9)：

35)将从i到j的每个点的横坐标代入(9)中得每个点对应在曲线方程中的纵坐标值；设定目标函数为：

其中，y是将数据点的横坐标代入构建的曲线方程计算得到的纵坐标值；y₁是数据点的真实纵坐标值。

步骤四、第一次拟合时，将k₀和k₁作为优化变量，采用内点法对目标函数(10)进行最优求解，求得使目标函数最小时对应的k₀和k₁值；若分段进行多次拟合，则将上一段终点的斜率k₁赋值给下一段的起点斜率k₀，将下一段的终点斜率k₁作为优化变量，同样采用内点法对目标函数(10)进行最优求解，求得使目标函数最小时对应的k₁值。

步骤五、设定误差阈值为ε，此处要求厘米级精度，将ε取为0.1m，当式(10)的最小值超过阈值时，将j的值分配给i作为新的分段点，返回步骤二，否则j的值增加1并返回步骤三直到最后一个点。

实施例

在本实施例的测试中，采集车配备了RTK-GPS和INS设备，定位精度可以达到厘米级。在开阔环境下，实际驾驶采集车过路口，沿途采集GPS数据作为路口处车辆实际行驶的轨迹点。

为了说明本发明的有效性，选择了采集的路口处车辆实际行驶的轨迹点，使用提出的方法来建立用于自动驾驶导航的路口虚拟车道模型，并与实际车辆行驶轨迹进行对比。由于在本发明中考虑了分段曲线分段点处位置和切向量相等的约束，模型能够保证全局的C1连续性。在图2和图3中分别显示了两段车辆在路口的真实行驶轨迹点以及建模的虚拟车道，从图中可以看出，对于不同的车辆路口行驶轨迹，所构建的虚拟车道均可以很好地拟合轨迹点，曲线平滑且连续。由于误差阈值ε设为0.1m，保证了所有轨迹点的真实纵坐标值与构建的虚拟车道模型中轨迹点横坐标相对应的纵坐标值之间的误差均小于0.1m，满足厘米级精度要求。同时由于使用三次多项式建模，能够轻松通过模型获得位置坐标、切线角度或曲率等在内的道路几何信息，模型可用性好。

以上结合附图详细描述了本发明的优选实施方式，但是，本发明的保护范围并不局限于上述实施方式中的具体细节，在本发明的技术构思范围内，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，根据本发明的技术方案及其发明构思加以等同替换或改变，这些简单变型均属于本发明的保护范围。

另外需要说明的是，在上述具体实施方式中所描述的各个具体技术特征，在不矛盾的情况下，可以通过任何合适的方式进行组合，为了避免不必要的重复，本发明对各种可能的组合方式不再另行说明。

此外，本发明的各种不同的实施方式之间也可以进行任意组合，只要其不违背本发明的思想，其同样应当视为本发明所公开的内容。

Claims (1)

1.一种面向自动驾驶导航的路口虚拟车道建模方法，其特征在于，包括以下步骤：

步骤一、设定车辆在路口实际行驶的轨迹点；

步骤二、设置拟合的起点和初始终点；

步骤三、将轨迹点横坐标带入曲线方程确定优化的目标函数；

步骤四、初次拟合，将起点和终点的斜率作为优化变量，对目标函数进行最优求解；若需要进行分段拟合，将上一段终点斜率赋值给下一段起点斜率，并将下一段终点斜率作为优化变量对目标函数进行最优求解；

步骤五、设置误差阈值，比较目标函数最小值是否大于阈值，如果目标函数最小值大于阈值，返回步骤二，如果目标函数最小值小于阈值，将终点的序号加1并返回步骤三直到最后一个点；

所述步骤一的具体方法如下：

设车辆在路口行驶的一系列轨迹点为

其中，x_i，y_i分别为第i个轨迹点的横纵坐标；n为轨迹点数目；

所述步骤二的具体方法如下：

设拟合起点为第i个点，终点为第j个点，第一次拟合时，采用从起始点开始的4个点，即j＝i+3；

所述步骤三的具体方法如下：

31)建立三次多项式曲线，如下：

y＝a₀+a₁x+a₂x²+a₃x³ (1)

其中，a₀、a₁、a₂、a₃分别是三次多项式曲线的四个系数；

32)式(1)的导数表达式，如下：

y′＝a₁+2a₂·x+3a₃·x² (2)

33)路口虚拟轨迹的起点和终点分别为(x₀，y₀)，(x₁，y₁)，起点和终点处斜率分别为k₀和k₁；设路口轨迹的起点位于原点，代入公式(1)得a₀＝0；将起点斜率k₀代入公式(2)得a₁＝k₀；

34)将a₀＝0和a₁＝k₀代入公式(1)得：

y＝k₀·x+a₂·x²+a₃·x³ (3)

将a₁＝k₀代入公式(2)得：

y′＝k₀+2a₂·x+3a₃·x² (4)

将终点坐标(x₁，y₁)代入公式(3)得：

y₁＝k₀·x₁+a₂·x₁ ²+a₃·x₁ ³ (5)

将终点斜率k₁代入公式(4)得：

k₁＝k₀+2a₂·x₁+3a₃·x₁ ² (6)

由公式(5)和(6)得：

将k₀和k₁作为待优化的变量，a₂和a₃用关于k₀和k₁的表达式表达如式(7)(8)所示，将(7)和(8)代入(3)得y关于k₀和k₁的表达式(9)：

35)将从i到j的每个点的横坐标代入(9)中得每个点对应在曲线方程中的纵坐标值；设定目标函数为：

其中，y(k)是将数据点的横坐标代入构建的曲线方程计算得到的纵坐标值；y₁(k)是数据点的真实纵坐标值；k是索引值，范围从i到j；

所述步骤四的具体方法如下：

第一次拟合时，将k₀和k₁作为优化变量，对目标函数(10)进行最优求解，求得使目标函数最小时对应的k₀和k₁值；若分段进行多次拟合，则将上一段终点的斜率k₁赋值给下一段的起点斜率k₀，将下一段的终点斜率k₁作为优化变量，对目标函数(10)进行最优求解，求得使目标函数最小时对应的k₁值；

所述步骤五的具体方法如下：

设定误差阈值为ε，当式(10)的最小值超过阈值时，将j的值分配给i作为新的分段点，返回步骤二，否则j的值增加1并返回步骤三直到最后一个点。

Images