关于自动驾驶车道线拟合算法，常用的方法有B样条、三次样条插值、Ransac、最小二乘法等等，但是针对于高精度地图的车道线拟合，由于车道线坐标点已知，所以不需要有控制点进行约束，那么B样条贝塞尔曲线等都不太适合；三次样条插值曲线每两个坐标点都拟合一组参数，如果高精度地图为20cm一个点的画，那么100m的道路一条车道线就将有500组参数，对于性能是不乐观的；而Ransac更适用于散点拟合，对于已知的有序点再进行多次迭代也是耗费性能的，所以目前还是以最小二乘法为主流方案。

关于普通最小二乘法（Ordinary Least Square，OLS）：所选择的回归函数应该使所有观察值的残差平方和达到最小。对于线性一次函数的车道线数据，在拟合函数时，可以先假定函数的通用表达式，一般车道线是以一系列的有序经纬度坐标点集组成。

[公式]

首先将数据转换为UTM84坐标系后，分为横坐标（自变量）为

，观测值（因变量）为：

目前的主流方法直接用最小二乘法做三次曲线拟合，但本文以引入为主，先以线性最小二乘法引入。假设：

则估计值

使得残差和 [公式] 最小，表达式如下:

而在自动驾驶中，需要的就是将曲线参数传入控制规划端用以描述道路形状，现阶段主流的拟合方程为一元三次方程，例如MobileEye以C0，C1，C2，C3传出，所以在我们目前的工作中，要做的就是针对点集数据，求出最优参数使得残差最小。由于残差和的表达式是二元函数，因此分别对参数 [公式] 求偏导，使其等于0时，得到残差和 [公式] 的最小值。

整理后得

具体推导可以自行百度，就不在此展开了。

在一次函数的情况下，函数拟合需要计算两个未知量。推广到高阶函数，就需要一个统一公式去做相关运算。

在高阶函数下

同理我们还是有[公式] 个自变量

和观测值

不妨将上式写成矩阵形式。

[公式]

上述矩阵方程记为

观测值 [公式] 写为向量的形式，记为 [公式]

同样，使得残差的平方和最小

将上式右半部展开，得：

根据矩阵转置相乘的转换关系，可得：

可以对上式的 所需要求的参数矩阵A 求偏导，使其偏导数为0即可得到最小残差和平方

[公式]

化简为： [公式] ;

根据矩阵的求导法则:

即： [公式]

因此残差平方 [公式] 和对各阶系数 [公式] 的偏导写为：

[公式]

化简后，我们得到高阶函数各项系数的最优取值

[公式]

面对线性数学模型，参数矩阵较大时，求逆矩阵的耗时较大，上式的直接求逆可能就略显鸡肋，一般需要通过QR分解、Cholesky分解、SVD分解进行求解。

面对非线性数学模型，无法直接写出导数或者导数过于复杂，也无法用上述矩阵分解的方式求解，所以一般会请来我们认识的雅克比、海塞来求解，目前主流的方法为：最速下降法、牛顿法、高斯牛顿法、LM等等进行迭代求解，也可以依赖ceres库、G2O库进行求解，后面也会依次介绍。

#include <iostream>
#include <Eigen/src/Cholesky/LDLT.h>

Eigen::VectorXd Ls(std::vector<double>& x,std::vector<double>&y)
{
Eigen::Map<Eigen::VectorXd> sampleX(x.data(), static_cast<long>(x.size()));
Eigen::Map<Eigen::VectorXd> sampleY(y.data(), static_cast<long>(y.size()));
Eigen::MatrixXd mtxVandermonde(x.size(), 4);
Eigen::VectorXd colVandermonde = sampleX;
for (int ii = 0; ii < 4; ++ii)
{
if (0 == ii) {
mtxVandermonde.col(0) = Eigen::VectorXd::Constant(static_cast<long>(x.size()), 1, 1);
continue;
}
if (1 == ii) {
mtxVandermonde.col(1) = colVandermonde;
continue;
}
colVandermonde = colVandermonde.array() * sampleX.array();
mtxVandermonde.col(ii) = colVandermonde;
}
Eigen::VectorXd result =(mtxVandermonde.transpose() * mtxVandermonde).inverse() * (mtxVandermonde.transpose()) * sampleY;


return result;
}//代码原文参见
https://zhuanlan.zhihu.com/p/268884807

输出结果后，就得到了数据点拟合曲线的相关参数，即可表达当前道路形状，用于后端控制。

具体使用哪种模型，还是要根据系统的实际需求，有时要求高速的车道线输出，曲率变化小，那用简单的线性模型就好；有些需要精确地道路模型，那就要求三次曲线或二次曲线；有些是视觉车到模型，那就要考虑引用Ransac或者Catmull_Rom样条曲线。总之，在做算法工作时，没有最好的算法，只有最适合的算法。