现阶段乘用车在布建自动驾驶系统时,一方面需要高精度定位传感器感知自车运行状态及当前所在位置;另一方面采用高精度地图获取周边超视距信息,弥补传感器的不足。本文主要介绍高精度地图在配合自动驾驶控制系统工作时,需要将地图车道线信息传递给后端,以便规划控制系统对车辆行驶轨迹进行推算并进行横纵向控制。一般地,高精度地图常将车道线拟合成曲线方程,将方程参数传递给后端,常见的曲线拟合算法有:多项式曲线,样条曲线,贝塞尔曲线等,而在自动驾驶高精度地图应用中,最常见的则为一元三次多项式曲线中插值的算法,即为三次样条插值。

当已知地图经纬度坐标点而不知道具体方程时候,我们通常将各几何点进行分段插值,插值只是形似,每个几何点都必会穿过,但是高阶会出现龙格现象,所以我们一般采用三次样条插值。

一般来说,出于对车辆行驶安全的考虑,L3级自动驾驶系统安全条件可以认定为:需要保证任何情况下行驶300m内有以确保车辆行驶的安全。对于300m行驶距离,在高速道路最高限速120km/h的情况下,大约9秒的行驶时间。一旦组合导航系统失效和摄像头识别异常,高精度地图也可以提供最短9秒的安全接管时间。故我们在使用高精度地图车道线信息时,一般选取300m的车道线长度传给控制端,我们将300m区间[a,b]分成n个区间 :[公式] ,共有n+1个点,暂时先不考虑数据量问题,我们把300的车道线作为大区间,其中两个端点 [公式] ,每个车道线几何点作为小区间的两端点。三次样条就是说每个小区间的曲线是一个三次方程,三次样条方程一般满足以下条件:

1、在每个分段小区间 [公式] 上,每段都是一个三次方程;
2、车道线几何点全部在曲线上,即 [公式]
3, 曲线光滑,即 [公式] 连续 。

则这个三次方程可以构造成如下形式:
[公式] 这种形式,我们称这个方程为三次样条函数 [公式] 
 [公式] 可以看出每个小区间有四个未知数 [公式] ,有n个小区间,则有4n个未知数,其实我们的需求很简单,已知各几何(x,y),就是为了求这4n个参数,所以接下来我们看具体求解方式:
求解思路:

我们先把曲线方程写出来:

[公式]

根据条件1、2:所有点都在曲线上,即为[公式]  ;

当前区间段和下一个区间段,有一个连接点,即: [公式] 

根据条件3:

所有节点处一阶连续,即: [公式]

推导出 [公式] 

所有节点处二阶连续,即: [公式]

同理[公式] 

令:[公式]

所以现在我们有以下几个公式:

[公式] ①;

[公式] ②;

[公式] ③;

[公式] ④;

[公式] ⑤;

已知( [公式] ),求 [公式] ,我们先把四组参数得转换关系写出来设 [公式] ,由⑤可得 [公式]

由③得: [公式] 

 [公式] 代入 [公式] 可得 [公式] 这样我们可以构造一个以m为未知数的线性方程组。

另外还需要增加区间两端点约束条件,一般为三种:自然边界,固定边界,非节点边界

1,自然边界 ( Natural Spline ):指让端点的斜率自由的在某一位置保持平衡,使得曲线的摇摆最小,端点二阶导数为0,[公式]

2, 固定边界 ( Clamped Spline ): 指在端点处使其具有固定得斜率,端点一阶导数,这里分别定为A和B。即 [公式]

3, 非扭结边界( Not-A-Knot Spline ): 强制第一个插值点的三阶导数值等于第二个点的三阶导数值,最后第一个点的三阶导数值等于倒数第二个点的三阶导数值. 即 [公式]

假设300m区间内,设两端点为自由边界条件,则可得, [公式]

根据已知条件,求解出m,别的参数也都可以迎刃而解。

本文旨在介绍高精度地图车道线拟合得一种通用方法,后续文章将介绍上述三对角矩阵的不同求解形式和运算速度、采样区间的工程化选择以及不同拟合方法的表现效果等。

Best wish!!!