自動駕駛（五十五）四解軌跡規劃

前面已經三次講過軌跡規劃，還是如隔靴搔癢，詩不著題。所以想少一些理論，多一些實踐的角度寫軌跡規劃，就從常見的場景展開：

1. 問題引入：

已知條件車身感測器：車速、yawrate、方向盤轉角；車身周圍車輛資訊；車道線資訊；乙個模糊的目標點。求一條軌跡能到達目標點的位置和角度，軌跡要滿足車輛運動學，舒適性，避障等要求。

怎麼理解模糊的目標點？目標位置和角度不是很精確，目標狀態的位置和角度隨著不斷地靠近，可能會不斷的收斂，例如十字路口轉彎，在轉彎過程中攝像頭看不到入口的車道線，高精地圖只能給出大概的位置和角度，在這樣模糊的資訊下，需要先規劃出一條車道線，引導車輛轉彎，在過程中看到車道線，不斷收斂目標點的位置和方向。

2. 解決思路:

初始化：初始軌跡規劃採用選取多個關鍵點，利用三次樣條曲線生成軌跡。

更新：車身定位採用klaman濾波收斂，考慮車身的狀態和舒適性，結合收斂的目標位置和角度，重新生成三次樣條曲線。

輸出：利用生成的三次樣條曲線，選取一定長度的軌跡，生成軌跡多項式（三次）,滿足舒適性；

錯誤處理：一旦車身偏離軌道，或者目標的位置和狀態達不到，需要一套邏輯處理，或者直接退出軌跡規劃模式。

3. 三次樣條差值：

在車輛控制中，要通過n個點，為了滿足舒適性要求，一般是當前點角度為零作為約束，即b0=0，，最後乙個點一般也有角度要求：bn = p；輸出為同一座標系下的分段三次函式。具體推導過程省略，主要講實現思路：

1. 已知n個點：

2. 分段三次函式表示為：

假設：3. 通過對分段函式本身、一階導、二階導在分段點上的連續性，聯立方程組，解方程組從而求出分段函式的係數：

4. 首位一般有約束，即開始點一階導為車身yawrate下對應的橫擺角，即保持當前車身的變化姿勢，即使歸0也可能不舒適，也可以描述為開始點的二階導為0。

5. 末位也有類似的約束，二階導為0。

6. 則通過二階導可以組成以下方程組：

7. 最後通過以下關係求出其他係數：

8. 封裝好的c++**：

struct poly_coef ;
//自然邊界的三次樣條曲線函式
void cubic_getval(vectorpxy, vectoropcs) 
//指定係數
for (int i = 0; i < n - 3; i++)
//指定常數d
for (int i = 0; i < n - 3; i++) 
//求解三對角矩陣，結果賦值給e
tdma(e, n - 3, a, b, c, d);
m[0] = 0;     //自然邊界的首端m為0
m[n - 1] = 0; //自然邊界的末端m為0
for (int i = 1; i < n - 1; i++) 
//?算三次?條曲?的係數
for (int i = 0; i < n - 1; i++) 
free(h);    free(a);
free(b);    free(c);
free(d);    free(e);
free(m);    free(ai);
free(bi);    free(ci);
free(di);
}void tdma(real_t *x, const int_t n, real_t *a, real_t *b, real_t *c, real_t *d)
//直接求出x的最後乙個值
x[n - 1] = d[n - 1];
//逆向迭代， 求出x
for (int i = n - 2; i >= 0; i--)
}

4. 生成軌跡由於上一步選點，生成分段三次曲線，能不能直接把0點對應的三次曲線作為軌跡線輸出呢？我認為不行，首先，生成的分段函式長度是由分段點決定的，不一定滿足要求；再次，兩幀之間是強調擬合，沒有考慮其舒適性和車輛運動學模型。因此我們需要重新擬合當前場景下合適的軌跡線。

bool curvefitting(cvseq* indataset, float curveparam[4])
*((float*)cv_mat_elem_ptr(*cmatrix, 1, 0)) = *((float*)cv_mat_elem_ptr(*cmatrix, 0, 1));
*((float*)cv_mat_elem_ptr(*cmatrix, 1, 1)) = *((float*)cv_mat_elem_ptr(*cmatrix, 0, 2));
*((float*)cv_mat_elem_ptr(*cmatrix, 1, 2)) = *((float*)cv_mat_elem_ptr(*cmatrix, 0, 3));
*((float*)cv_mat_elem_ptr(*cmatrix, 2, 0)) = *((float*)cv_mat_elem_ptr(*cmatrix, 0, 2));
*((float*)cv_mat_elem_ptr(*cmatrix, 2, 1)) = *((float*)cv_mat_elem_ptr(*cmatrix, 1, 2));
*((float*)cv_mat_elem_ptr(*cmatrix, 2, 2)) = *((float*)cv_mat_elem_ptr(*cmatrix, 1, 3));
*((float*)cv_mat_elem_ptr(*cmatrix, 3, 0)) = *((float*)cv_mat_elem_ptr(*cmatrix, 0, 3));
*((float*)cv_mat_elem_ptr(*cmatrix, 3, 1)) = *((float*)cv_mat_elem_ptr(*cmatrix, 1, 3));
*((float*)cv_mat_elem_ptr(*cmatrix, 3, 2)) = *((float*)cv_mat_elem_ptr(*cmatrix, 2, 3));
cvmat* pcmatrix = cvcreatemat(4, 4, cv_32fc1);
cvzero(pcmatrix);
cvmat result = cvmat(4, 1, cv_32fc1, curveparam);
cvzero(&result);
cvinvert(cmatrix, pcmatrix, cv_lu);
cvmatmul(pcmatrix, cstmatrix, &result);
cvreleasemat(&pcmatrix);
cvreleasemat(&cstmatrix);
return true;
}

同樣的多項式擬合沒有考慮幀間的舒適性，也沒有考慮車輛運動學，所以我們需要新的思路解決問題。這裡我介紹一種物理含**決以上問題，如圖：

圖示：l1為前面幾個點擬合的直線，c0為車中心到l1的距離，l2為最後幾個點擬合的直線，l3為l2的法線，l4為l1的法線，計算l3和l4的交點。

對於以上點得到的軌跡多項式 y=a+bx+cx^2+dx^3，其中c直接為c0，b是l1的角度，c是 l3和l4的圓的半徑倒數，d是c的變換率，一般在中間在選一組點，計算c的變化率。

5. 優化更新

上面生成的軌跡，沒有考慮舒適性和車輛運動學模型。在保證舒適性的方法中，視窗濾波是最直接和簡單的方法，但是有明顯的確定，產生延遲。這裡介紹一種新的思路，供大家思考。

我們把上一幀計算的軌跡離散成一串點。

通過本次車身姿態和位置變化，仿射變換計算出上一幀軌跡離散點。

對新觀察的離散點，和上一幀推算的離散點進行kalman濾波。

對濾波結果安裝第4步重新計算新的軌跡。

6. 錯誤檢測

對輸出的軌跡進行檢測，對計算的c0、c1、c2、c3進行數值約束。對不滿足要求的結果進行退出操作

自動駕駛（五十五）四解軌跡規劃

自動駕駛汽車與自動駕駛網路

自動駕駛相關

自動駕駛相關

自動駕駛（五十五） 四解軌跡規劃

自動駕駛汽車與自動駕駛網路

自動駕駛相關

自動駕駛相關

相關推薦

自動駕駛（五十五）四解軌跡規劃