CN111814286B - 一种面向自动驾驶的车道级地图几何模型建立方法 - Google Patents

Abstract

本发明属于自动驾驶导航技术领域，具体的说是一种面向自动驾驶的车道级地图几何模型建立方法。该几何模型建立方法为使用分段三次多项式来构建路网几何模型。本发明使得车道级地图能够同时满足厘米级精度，存储量小及可用性强的条件，同时保证了位置和切向量上的全局连续性。

Description

一种面向自动驾驶的车道级地图几何模型建立方法

技术领域

本发明属于自动驾驶导航技术领域，具体的说是一种面向自动驾驶的车道级地图几何模型建立方法。

背景技术

近些年来，自动驾驶迅速发展，以求带来更高水平的安全和便利。为此，需要精确的车道级地图来提供各种自动驾驶车辆应用，如车辆路径规划和高精度定位。此外，各种先进的驾驶员辅助系统(Advanced Driver Assistance Systems简称ADAS)，包括车道保持、变道辅助和燃料管理系统，都得益于精确的车道级地图。

车道级地图需要设计几何模型，需要具备三个条件：精确表达几何形状达到厘米级精度，存储量小及可用性强(具体指包括坐标、切线角度或曲率等在内的道路几何信息所需的计算量应该很低，以便车辆实时应用)，同时考虑到车辆的运动无论在位置还是速度上都是连续的，地图路径在位置和切向量上都应该具有全局连续性(又称C1连续性)，如此，地图路线将更好地近似于相应的车辆轨迹，从而使许多基于地图的应用受益。

现有车道级地图的几何模型包括如下模型：

1.折线模型：在允许误差较大的道路级地图中可以实现，对于车道级路网，折线模型难以表达准确的几何形状，在复杂弯道几何表达中，需要存储过多的形状点，使得模型实用性较差。

2.弧线样条曲线(Arc Spline)模型：利用直线和圆弧来代替传统的折线，线型简单，不需要存储多余的参数，同时实用性强，全段切向量连续，但是弧线和直线都是曲率恒定的线形，难以表达道路中众多曲率变化的路段。

3.回旋线模型：贴合在道路设计中广泛使用的线型，能够实现曲率一阶连续，但是这种线型的原始表达是菲涅尔(Fresnel)积分形式的超越方程，对于智能驾驶车辆应用所需要的信息，例如车辆与地图中的车道之间的距离和相对角度，这种几何模型需要复杂的计算，实际应用过程中很难直接利用。

4.三次B样条曲线模型：其好处在于曲线局部的修改不会影响整体，这使得路网可维护性强，但由于曲线方程不够直观，较难计算函数的一阶和二阶导数，较难提取出例如切线方向和曲率这种道路几何信息。

5.分段三次多项式模型：三次多项式曲线表达直观，由于可以方便地计算其导数，易得出道路切向和曲率的几何信息，现有研究构建的分段三次多项式模型能够满足拟合精度，存储性和实用性多方面需求，但是具有指数级或O(n3)计算复杂度，不适合于拟合大量的道路几何数据，或者没有考虑分段曲线之间的C1连续性，而连续的切向向量信息对于智能驾驶是至关重要的。

综合现阶段车道级地图的相关研究，在车道级几何模型构建中，各种线型各有优劣，未能同时满足拟合精度，存储性，实用性和曲线之间切向向量连续多方面需求。在本发明中，使用分段三次多项式来构建路网几何模型，使得模型构建计算简单，考虑了分段曲线之间的C1连续性。

发明内容

本发明提出了一种面向自动驾驶的车道级地图几何模型建立方法，该方法使得车道级地图能够同时满足厘米级精度，存储量小及可用性强的条件，同时保证了位置和切向量上的全局连续性。

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

一种面向自动驾驶的车道级地图几何模型建立方法，具体包括以下步骤：

步骤一、对于一系列的道路数据点，估计每一点的切线方向；

步骤二、设定每一段的起点i和终点j，起始使用四个点进行拟合；

步骤三、将上一轮拟合的终点即新一轮拟合的起点平移至坐标系原点得到一个新的数据集；

步骤四、利用起点和终点的值根据公式得出三次多项式的四个系数；公式具体如下：

针对分段三次曲线，以第一段和第二段来说明考虑的分段的连续性约束；

三次多项式曲线表达式为y＝a₀+a₁·x+a₂·x²+a₃·x³ (1)

其导数表达式为y’＝a₁+2a₂·x+3a₃·x² (2)

第一段曲线的起点设在原点，即x(0)＝0,y(0)＝0；

代入(1)中可得a₀＝0；；

设起点的斜率为k₀，即y’(0)＝k₀；

代入(2)中可得a₁＝k₀；

这样公式(1)可以写成y＝y(0)+k₀·x+a₂·x²+a₃·x³ (3)

公式(2)可以写成y’＝k₀+2a₂·x+3a₃·x² (4)

对于第一段曲线上一点M(x(m),y(m))在该点处斜率为k_m,代入公式(3)(4)中得出如下矩阵方程式

步骤五、根据步骤四中求出的系数算出：

y1(k)＝a₀+a₁·x(k)+a₂·x(k)²+a₃·x(k)³k＝i+1,…,j-1 (8)

设定判断量ε如下：

ε＝|y1(k)-y(k)|k＝i+1,…,j-1 (9)

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

步骤六、设定阈值δ，设计启发式判定准则：设定计数变量num，当ε大于判断阈值δ时，计数增加1，当计数num超过了计数阈值时，进行分段，将上一段的终点赋值给下一段的起点，进行步骤七；否则，继续判断下一个数据点，返回到步骤四；

步骤七、与之前拟合段转换到同一坐标系；使用下面公式进行坐标变换：

y＝a₀+a₁·(x-x(i))+a₂·(x-x(i))²+a₃·(x-x(i))³+y(i) (10)

然后返回到步骤二进行新一段曲线的拟合。

所述步骤一中估计每一点的切线方向的具体方法如下：

11)对于同样的道路数据点，采用最小二乘法进行分段拟合；设立分段点为p；

12)从第一段曲线开始拟合，设每一段的拟合终点为q，起始使用四个点进行拟合；

13)对于每一段，用最小二乘法拟合成三次多项式，设定判断值如下：

其中，y2是将数据点的横坐标代入最小二乘法拟合成的三次多项式计算得到的纵坐标值，y是数据点的真实纵坐标值；

将阈值设置为δ₁，当判断值大于阈值时，将q的值分配给p作为新的分段点，否则q的值增加1并返回步骤13)直到最后一个点；

14)根据拟合的分段三次多项式方程计算相应点的导数，即每个点的估计斜率值。

本发明的有益效果为：

本发明能使车道级地图能够同时满足厘米级精度，存储量小及可用性强的条件，同时保证了位置和切向量上的全局连续性。

附图说明

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

图2为所构建的分段三次多项式曲线拟合数据点的曲线图；

图3显示了图2中的局部数据点及曲线的放大图；

图4为每个数据点的真实纵坐标值与构建的曲线模型的纵坐标值之间的误差值曲线图。

具体实施方式

下面结合附图和实施例对本发明作进一步的详细说明。

参阅图1，一种面向自动驾驶的车道级地图几何模型建立方法，该方法为使用分段三次多项式来构建路网几何模型，得出道路切向和曲率的几何信息。

针对分段三次曲线，以第一段和第二段来说明考虑的分段的连续性约束。

三次多项式曲线表达式为y＝a₀+a₁·x+a₂·x²+a₃·x³ (1)

其导数表达式为y′＝a₁+2a₂·x+3a₃·x² (2)

第一段曲线的起点设在原点，即x(0)＝0,y(0)＝0。

代入(1)中可得a₀＝0。

设起点的斜率为k₀，即y′(0)＝k₀。

代入(2)中可得a₁＝k₀。

这样公式(1)可以写成y＝y(0)+k₀·x+a₂·x²+a₃·x³ (3)

公式(2)可以写成y′＝k₀+2a₂·x+3a₃·x² (4)

对于第一段曲线上一点M(x(m),y(m))在该点处斜率为k_m,代入公式(3)(4)中得出如下矩阵方程式

分段三次多项式构建模型的方法如下：

步骤一、一系列的道路数据点给出：X＝{x(n)|n＝1,2,…,N},Y＝{y(n)|n＝1,2,…,N}，估计每一点的切线方向；

所述步骤一中估计每一点的切线方向的具体方法如下：

11)对于同样的道路数据点：X＝{x(n)|n＝1,2,…,N},Y＝{y(n)|n＝1,2,…,N}。采用最小二乘法进行分段拟合；设立分段点为p；

12)从第一段曲线开始拟合，起始点设置为第一个点，即p的初始值为1。设每一段的拟合终点为q，起始使用四个点进行拟合，从第p个数据点开始往后选取三个点进行初次拟合，即q的初值设置为q＝p+3；

13)对于每一段，用最小二乘法拟合成三次多项式，设定判断值如下：

其中，y2是将数据点的横坐标代入最小二乘法拟合成的三次多项式计算得到的纵坐标值，y是数据点的真实纵坐标值；

将阈值设置为δ₁，当判断值大于阈值时，将q的值分配给p作为新的分段点，否则q的值增加1并返回步骤13)直到最后一个点；

14)根据拟合的分段三次多项式方程计算相应点的导数，即每个点的估计斜率值。

步骤二、设每一段的起始点为i,，每一段的拟合终点为j，起始使用四个点进行拟合，从第i个数据点开始往后选取三个点进行初次拟合，即j的初值设置为j＝i+3；

步骤三、由公式(1)和(2)可知，只有当x＝0时，a₀和a₁才能表示为y值及其导数值，即a₀＝y(0)＝0,a₁＝y'(0)；所以为了拟合和计算方便，使得上一轮拟合的终点即新一轮拟合的起点平移至坐标系原点得到一个新的数据集，即

x＝x-x(i) (6)

y＝y-y(i) (7)

步骤四、利用i点和j点的值根据公式(1)-(5)得出三次多项式的四个系数a₀,a₁,a₂,a₃；

步骤五、根据步骤四中求出的系数值算出：

y1(k)＝a₀+a₁·x(k)+a₂·x(k)²+a₃·x(k)³ k＝i+1,…,j-1 (8)

设定判断量ε如下：

ε＝|y1(k)-y(k)|k＝i+1,…,j-1 (9)

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

步骤六、设定阈值δ，由于数据中包含随机误差，即使所有数据点都属于一个多项式曲线，最大距离误差有时也会超过阈值。在这种情况下，忽略偶然的误差，能减少分段，该曲线段应该继续。为此，对曲线过渡设计如下启发式判定准则：设定计数num，初始值为0，如果当ε大于判断阈值δ时，计数增加1，当计数num超过了计数阈值时，j点作为分段点将值赋给i，进行步骤七；否则，继续判断下一个数据点，令j＝j+1，返回到步骤四。

步骤七、在这里，要考虑到与之前拟合段转换到同一坐标系。使用下面公式进行变换：

y＝a₀+a₁·(x-x(i))+a₂·(x-x(i))²+a₃·(x-x(i))³+y(i) (10)

然后返回到步骤二进行新一段曲线的拟合。

实施例

在我们的测试中，采集车配备了RTK-GPS和INS设备，定位精度可以达到厘米级。在开阔环境下，实际驾驶采集车沿车道中心线行驶，沿途采集GPS数据。

为了说明本发明的有效性，我们选择了采集的连续的约1000个数据点，使用提出的算法来构建车道级地图的几何模型。由于在本发明中考虑了分段曲线分段点处位置和切向量相等的约束，模型能够保证全局的C1连续性。在图2中显示了真实数据点和构造的分段三次多项式曲线。为了看的更清楚，图3放大了图2中的局部数据点及曲线，从图中可以看出所构建的分段三次多项式曲线可以很好地拟合数据点，曲线平滑且连续。图4显示了每个数据点的真实纵坐标值与构建的曲线模型中数据点的横坐标相对应的纵坐标值之间的误差值，可以看出，超过阈值的点的误差很小，最大误差为0.16m，非常接近0.1m，误差的平均值为0.0139m，远小于0.1m，，满足车道级地图厘米级精度要求。此外，对于约1000个GPS数据点所在的长度大于10000米的道路，本发明仅分段为76段，分段数非常小，满足了车道级地图存储量小的要求。

Claims (1)

1.一种面向自动驾驶的车道级地图几何模型建立方法，其特征在于，该方法为使用分段三次多项式来构建路网几何模型，具体包括以下步骤：

步骤一、对于一系列的道路数据点，估计每一点的切线方向；

步骤二、设定每一段的起点i和终点j，起始使用四个点进行拟合；

步骤三、将上一轮拟合的终点即新一轮拟合的起点平移至坐标系原点得到一个新的数据集；

步骤四、利用起点和终点的值根据公式得出三次多项式的四个系数；公式具体如下：

针对分段三次曲线，以第一段和第二段来说明考虑的分段的连续性约束；

三次多项式曲线表达式为y＝a₀+a₁·x+a₂·x²+a₃·x³ (1)

其导数表达式为y’＝a₁+2a₂·x+3a₃·x² (2)

第一段曲线的起点设在原点，即x(0)＝0，y(0)＝0；

代入(1)中可得a₀＝0；

设起点的斜率为k₀，即y’(0)＝k₀；

代入(2)中可得a₁＝k₀；

这样公式(1)可以写成y＝y(0)+k₀·x+a₂·x²+a₃·x³ (3)

公式(2)可以写成y’＝k₀+2a₂·x+3a₃·x² (4)

对于第一段曲线上一点M(x(m)，y(m))在该点处斜率为k_m，代入公式(3)(4)中得出如下矩阵方程式

步骤五、根据步骤四中求出的系数算出：

y1(k)＝a₀+a₁·x(k)+a₂·x(k)²+a₃·x(k)³k＝i+1，...，j-1 (8)

设定判断量ε如下：

ε＝|y1(k)-y(k)| k＝i+1，...，j-1 (9)

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

步骤六、设定判断阈值δ，设计启发式判定准则：设定计数变量num和计数阈值，当ε大于判断阈值δ时，计数增加1，当计数num超过了计数阈值时，进行分段，将上一段的终点赋值给下一段的起点，进行步骤七；否则，继续判断下一个数据点，返回到步骤四；

步骤七、与之前拟合段转换到同一坐标系；使用下面公式进行坐标变换：

y＝a₀+a₁·(x-x(i))+a₂·(x-x(i))²+a₃·(x-x(i))³+y(i) (10)

然后返回到步骤二进行新一段曲线的拟合；

所述步骤一中估计每一点的切线方向的具体方法如下：

11)对于同样的道路数据点，采用最小二乘法进行分段拟合；设立分段点为p；

12)从第一段曲线开始拟合，设每一段的拟合终点为q，起始使用四个点进行拟合；

13)对于每一段，用最小二乘法拟合成三次多项式，

设定判断值如下：

其中，y2是将数据点的横坐标代入最小二乘法拟合成的三次多项式计算得到的纵坐标值，y是数据点的真实纵坐标值；

将阈值设置为δ₁，当判断值大于阈值时，将q的值分配给p作为新的分段点，否则q的值增加1并返回步骤13)直到最后一个点；

14)根据拟合的分段三次多项式方程计算相应点的导数，即每个点的估计斜率值。

Images