【自动驾驶】车道线拟合算法---最小二乘法拟合直线
概覽
關(guān)于自動(dòng)駕駛車道線擬合算法，常用的方法有B樣條、三次樣條插值、Ransac、最小二乘法等等。
但是針對(duì)于高精度地圖的車道線擬合，由于車道線坐標(biāo)點(diǎn)已知，所以不需要有控制點(diǎn)進(jìn)行約束，那么B樣條、貝塞爾曲線等都不太適合；三次樣條插值曲線每?jī)蓚€(gè)坐標(biāo)點(diǎn)都擬合一組參數(shù)，如果高精度地圖為20cm一個(gè)點(diǎn)的畫，那么100m的道路一條車道線就將有500組參數(shù)，對(duì)于性能是不樂觀的；而Ransac更適用于散點(diǎn)擬合，對(duì)于已知的有序點(diǎn)再進(jìn)行多次迭代也是耗費(fèi)性能的，所以目前還是以最小二乘法為主流方案。
最小二乘法，又稱最小平方法，其實(shí)就是深度學(xué)習(xí)中的均方誤差。它通過最小化 誤差的平方和 尋找數(shù)據(jù)的最佳函數(shù)匹配。主要作用是從一堆相關(guān)數(shù)據(jù)中求解數(shù)據(jù)的一般性規(guī)律。在圖像處理方面多用于各種形狀的擬合。
最小二乘擬合直線，主要體現(xiàn)為找到一條直線，使得所有已知的點(diǎn)到這條直線的歐式距離的和最小（或者理解為點(diǎn)到直線的誤差平方和最小）。開始之前先引入一個(gè)誤差的概念。已知直線y=2x。現(xiàn)有點(diǎn)(x1,y1),求誤差是多少，答案就是y1 - 2 * x1。
具體使用哪種模型，還是要根據(jù)系統(tǒng)的實(shí)際需求：
- 有時(shí)要求高速的車道線輸出，曲率變化小，那用簡(jiǎn)單的線性模型就好；
- 有些需要精確地道路模型，那就要求三次曲線或二次曲線；
- 有些是視覺車到模型，那就要考慮引用Ransac或者Catmull_Rom樣條曲線。
總之，在做算法工作時(shí)，沒有最好的算法，只有最適合的算法。
車道線的數(shù)學(xué)模型相關(guān)知識(shí)點(diǎn) 可閱讀 blog
擬合線性方程
曲線擬合中最基本和最常用的是直線擬合。設(shè)x和y之間的函數(shù)關(guān)系為：
y＝a+bx此處a 對(duì)應(yīng)車道線里的 lateral deviation橫向偏移；斜率b 對(duì)應(yīng)車道線里的 heading angle車輛航向角 ；車輛航向角定義
式中有兩個(gè)待定參數(shù)，a代表截距，b代表斜率。對(duì)于等精度測(cè)量所得到的N組數(shù)據(jù)（xi，yi），i＝1，2……，N。xi值被認(rèn)為是準(zhǔn)確的，所有的誤差只聯(lián)系著yi。下面利用最小二乘法把觀測(cè)數(shù)據(jù)擬合為直線。
用最小二乘法估計(jì)參數(shù)時(shí)，要求觀測(cè)值yi的偏差的加權(quán)平方和為最小。對(duì)于等精度觀測(cè)值的直線擬合來說，可使下式的值最小：
上式分別對(duì)a、b求偏導(dǎo)得：
整理后得到方程組：
解上述方程組便可求得直線參數(shù)a和b的最佳估計(jì)值：
相關(guān)系數(shù)r:
最小二乘法處理數(shù)據(jù)除給出a、b外，常常還給出相關(guān)系數(shù)r, r定義為：
或者 有的公式 除以了 n：
- 當(dāng)r>0時(shí)，斜率 = X的標(biāo)準(zhǔn)差 / Y的標(biāo)準(zhǔn)差；
- 當(dāng)r<0時(shí)，斜率 = -X的標(biāo)準(zhǔn)差 / Y的標(biāo)準(zhǔn)差；
通常用SD線來直觀的表示數(shù)據(jù)的走向：
1）當(dāng)r<0時(shí),SD線的斜率小于0時(shí)，則說明數(shù)據(jù)負(fù)相關(guān)，即當(dāng)x增大時(shí)y減少。
2）當(dāng)r>0時(shí)，SD線的斜率大于0時(shí)，則說明數(shù)據(jù)正相關(guān)，此時(shí)當(dāng)x增大時(shí)y增大。
3）相關(guān)系數(shù)r的范圍在[-1,1]之間，當(dāng)r=0時(shí)表示數(shù)據(jù)相關(guān)系數(shù)為0(不相關(guān))。當(dāng)r=正負(fù)1時(shí)，表示數(shù)據(jù)負(fù)相關(guān)，此(x,y)點(diǎn)數(shù)據(jù)都在SD線上。
4）r的值越接近正負(fù)1說明(x,y)越靠攏SD線，說明數(shù)據(jù)相關(guān)性越強(qiáng)，r的值越接近0說明(x,y)點(diǎn)到SD線的散度越大（越分散），數(shù)據(jù)相關(guān)性越小。
代碼實(shí)現(xiàn)
void LineFitLeastSquares(float *data_x, float *data_y, int data_n) { float A = 0.0; float B = 0.0; float C = 0.0; float D = 0.0; float E = 0.0; float F = 0.0; for (int i=0; i<data_n; i++) { A += data_x[i] * data_x[i]; B += data_x[i]; C += data_x[i] * data_y[i]; D += data_y[i]; } // 計(jì)算斜率a和截距b float a, b, temp = 0; if( temp = (data_n*A - B*B) )// 判斷分母不為0 { a = (data_n*C - B*D) / temp; b = (A*D - B*C) / temp; } else { a = 1; b = 0; } // 計(jì)算相關(guān)系數(shù)r float Xmean, Ymean; Xmean = B / data_n; Ymean = D / data_n; float tempSumXX = 0.0, tempSumYY = 0.0; for (int i=0; i<data_n; i++) { tempSumXX += (data_x[i] - Xmean) * (data_x[i] - Xmean); tempSumYY += (data_y[i] - Ymean) * (data_y[i] - Ymean); E += (data_x[i] - Xmean) * (data_y[i] - Ymean); } F = sqrt(tempSumXX) * sqrt(tempSumYY); float r; r = E / F; }擬合一元三次方程（高階方程）
目前的主流方法直接用最小二乘法做三次曲線擬合，但本文以引入為主，上面先以線性最小二乘法引入。 y = ax + b
使殘差和最小：
在自動(dòng)駕駛中，需要的就是將曲線參數(shù)傳入控制規(guī)劃端用以描述道路形狀，現(xiàn)階段主流的擬合方程為一元三次方程，例如MobileEye以C0，C1，C2，C3傳出，所以在我們目前的工作中，要做的就是針對(duì)點(diǎn)集數(shù)據(jù)，求出最優(yōu)參數(shù)使得殘差最小。由于殘差和的表達(dá)式是二元函數(shù)，因此分別對(duì)參數(shù) a, b 求偏導(dǎo)，使其等于0時(shí)，得到殘差和的最小值：
整理后得
在一次函數(shù)的情況下，函數(shù)擬合需要計(jì)算兩個(gè)未知量。推廣到高階函數(shù)，就需要一個(gè)統(tǒng)一公式去做相關(guān)運(yùn)算。
在高階函數(shù)下：
同理我們還是有 n 個(gè)自變量
和觀測(cè)值
不妨將上式寫成矩陣形式。
上述矩陣方程記為:
觀測(cè)值 Y 寫為向量的形式，記為：
同樣，使得殘差的平方和最小：
將上式右半部展開，得：
根據(jù)矩陣轉(zhuǎn)置相乘的轉(zhuǎn)換關(guān)系，可得：
化簡(jiǎn)為：
根據(jù)矩陣的求導(dǎo)法則:
即：
因此 殘差平方和 對(duì) 各階系數(shù) 的偏導(dǎo)為：
化簡(jiǎn)后，我們得到高階函數(shù)各項(xiàng)系數(shù)的最優(yōu)取值：
面對(duì)線性數(shù)學(xué)模型，參數(shù)矩陣較大時(shí)，求逆矩陣的耗時(shí)較大，上式的直接求逆可能就略顯雞肋，一般需要通過QR分解、Cholesky分解、SVD分解進(jìn)行求解。
面對(duì)非線性數(shù)學(xué)模型，無法直接寫出導(dǎo)數(shù)或者導(dǎo)數(shù)過于復(fù)雜，也無法用上述矩陣分解的方式求解，所以一般會(huì)請(qǐng)來我們認(rèn)識(shí)的雅克比、海塞來求解，目前主流的方法為：最速下降法、牛頓法、高斯牛頓法、LM等等進(jìn)行迭代求解，也可以依賴ceres庫(kù)、G2O庫(kù)進(jìn)行求解，后面也會(huì)依次介紹。
代碼實(shí)現(xiàn)
#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輸出結(jié)果后，就得到了數(shù)據(jù)點(diǎn)擬合曲線的相關(guān)參數(shù)，即可表達(dá)當(dāng)前道路形狀，用于后端控制。
參考鏈接：
https://guyuehome.com/35243
https://blog.csdn.net/qq_36251561/article/details/88020558
https://blog.csdn.net/Naruto_ahu/article/details/8694366
總結(jié)
以上是生活随笔為你收集整理的【自动驾驶】车道线拟合算法---最小二乘法拟合直线的全部?jī)?nèi)容，希望文章能夠幫你解決所遇到的問題。
- 上一篇： div隐藏(设置div隐藏)
- 下一篇： PySpark