基于三次B樣條曲線擬合的智能車軌跡跟蹤算法

張永華，杜 煜，潘 峰，魏 岳

(1.北京聯合大學 智慧城市學院，北京100101; 2.北京聯合大學 機器人學院，北京100101;3.保定學院物理與電子工程系，河北保定071000)

(*通信作者電子郵箱duyu@buu.edu.cn)

0 引言

軌跡跟蹤算法是智能車輛控制的一項關鍵技術，通過對車輛轉向系統的控制，使車輛按照規劃的行駛路線行駛，并滿足一定的舒適性、安全性和穩定性，近年來國內外許多高校和科研院所對此都進行了廣泛和深入的研究。以經典比例積分微分(Proportion-Integration-Differentiation，PID)控制理論為基礎，早期研究人員由基于前方路徑曲率的前饋控制和PID反饋控制器疊加組合的橫向復合式控制系統，構建出非線性PID控制器［1］。以此為基礎，研究人員又進一步提出了具有學習功能的自適應軌跡跟蹤算法［2］和基于慣導航向角的非線性PID軌跡跟蹤算法［3］。為了解決基于PID的軌跡跟蹤算法的魯棒性問題，研究人員提出了采用由期望方位偏差估計器和魯棒PID反饋控制器組成的橫向控制系統［4］和通過前輪主動轉向和直接橫擺力矩協調控制，實現了基于廣義比例積分(Proportion-Integration，PI)控制的線性參變(Linear Parameter Varying，LPV)魯棒增益調度控制器［5］。此外，基于橫向位置誤差和橫擺率，研究人員提出了嵌套PID控制器［6］。PID控制器雖然可以處理智能車輛的軌跡跟蹤問題，但由于其控制參數通過試湊法獲取，因此難以實現對橫向運動的最優控制。

除了基于PID控制理論的軌跡跟蹤算法以外，針對車輛的運動學特性，提出了基于幾何模型的軌跡跟蹤算法。文獻［7］提出了一種車輛側滑約束下的滿足車輛動力學特性的幾何軌跡跟蹤算法。而文獻［8］結合模糊控制算法提出了一種改進的純追蹤控制算法。在此基礎上，又進一步提出了在純追蹤控制算法中引入主動偏航控制的算法，從而實現了較高速度下穩定的軌跡跟蹤［9］。

路徑生成算法用于生成滿足車輛動力學約束的平滑避障路徑。文獻［10］中將貝塞爾曲線用于純追蹤模型當中，實現了避免碰撞下的平滑路徑的軌跡跟蹤。文獻［11］將貝塞爾曲線應用于具有回廊約束下的軌跡生成，通過軌跡的分段拼接生成具有C2光滑度的避障軌跡線。為了獲取更加平滑的軌跡線，文獻［12］中通過對四階貝塞爾曲線的三個控制參數的最優化求解，從而得到滿足車輛運動學約束和曲率連續約束的平滑軌跡線。更進一步地，文獻［13］中提出了采用五次貝塞爾曲線分段拼接的方式來獲取平滑路徑。為了克服貝塞爾曲線需要分段拼接的問題，進一步提出了基于B樣條的平滑路徑生成算法。文獻［14］將B樣條曲線和反推控制器相結合，實現了滿足車輛動力學約束下的平滑軌跡跟蹤。此外文獻［15］又進一步提出了根據前輪轉向實現具有非線性約束的車輛轉向控制。采用B樣條曲線擬合的方式生成的平滑軌跡線，相比貝塞爾曲線，在相同階次下，路徑軌跡更加平滑，且改變個別控制點不會改變整條軌跡線，更有利于路徑跟蹤的平穩性［16］

為了應對多變的復雜實際駕駛場景，設計控制簡單、易于調節、適應性和魯棒性更強的軌跡跟蹤算法，在現有幾何軌跡跟蹤算法［17］的基礎之上，本文提出了一種實用的基于三次B樣條曲線擬合的幾何軌跡跟蹤算法，使得擬合后的路徑具有C2連續性，從而滿足軌跡跟蹤對平滑軌跡的要求，有利于高速下的穩定軌跡跟蹤。該算法通過對先驗地圖中的離散軌跡點進行擬合，并通過擬合得到的軌跡方程重新生成帶有切向角參數的離散路點，從而擺脫了現有幾何軌跡跟蹤算法在計算切向角時對高精度慣性導航設備的依賴。相比傳統的基于PID的軌跡跟蹤算法和現有幾何軌跡跟蹤算法，本文提出的算法不僅具有一定的軌跡優化能力，使得軌跡跟蹤過程更加平穩;而且在有效避免了傳統PID跟蹤算法參數不易調節和現有幾何跟蹤算法對慣性導航設備依賴的問題的同時，能夠適用于多傳感器融合軌跡在較高速度下的軌跡跟蹤，具有更好的適應性和跟蹤精度。

1 理論基礎

1.1 車輛運動學模型

在不考慮側向滑動問題的前提下，汽車的運動學模型可以采用線性二自由度前輪轉向模型來描述，如圖1所示。

圖1 前輪轉向模型Fig.1 Front wheel steering model

車輛運動學模型如式(1)～(2)所示:

其中:(x，y)表示車輛的二維坐標;ψ表示航向角;k表示曲率;L表示前后輪軸距;r表示轉彎半徑;α表示前輪轉向角;v表示車輛實時速度。

1.2 幾何軌跡跟蹤算法

本文采用和文獻［17］相同的幾何軌跡跟蹤算法作為本文算法的基礎理論部分。如圖2所示，縱軸左側曲線為先驗地圖軌跡線，(cx，cy)為到前軸中心點所在橫軸與先驗軌跡線相交的軌跡點。δ(t)為車輪實際轉角，由橫向偏差角θfa(t)和切向角θe(t)兩部分構成，即:

其中橫向偏差角θfa(t)是橫向偏差efa(t)的非線性函數，即:

其中:橫向偏差efa(t)是前軸中心點到軌跡點(cx，cy)之間的距離;vy(t)指的是車輛的實際行駛速度v(t)的縱向分量;k為增益參數;切向角θe(t)指的是軌跡點(cx，cy)的切線正方向與縱軸正方向的夾角。方向盤的轉角φ(t)由車輪偏角和車輪轉角到方向盤轉角的映射函數μ(t)共同得到，即:

當增益參數k和縱向速度分量vy(t)為常數時，式(3)中橫向偏差角θfa(t)在車輪實際轉角δ(t)中所占的比重越小，表明軌跡跟蹤的精度就越高，此時橫向偏差efa(t)也就越小。因此可以用橫向偏差efa(t)絕對值的大小來評估軌跡跟蹤的效果。

式(4)中，在不同縱向速度分量vy(t)時，增益參數k用于調節橫向偏差efa(t)對橫向偏差角θfa(t)的貢獻大小。當縱向速度由低到高逐漸增大時，為了保證智能車行駛過程中及時、平穩、安全地進行橫向誤差矯正，k值呈現非線性遞增變化。

圖2 幾何軌跡跟蹤算法示意圖Fig.2 Schematic diagram of geometric path tracking algorithm

1.3 三次B樣條曲線擬合原理

如圖3 所示，設四個離散點為:P0、P1、P2、P3，其坐標分別為:(x0，y0)、(x1，y1)、(x2，y2)、(x3，y3); 設中點 M1=(P0+P2)/2、M2=(P1+P3)/2。要使擬合后的曲線起于始點P0，止于終點P3，需在起始端和終止端處分別增加一個點Ps=2P0－ P1和Pe=2P3－ P2，然后以新的點序列Ps、P0、P1、P2、P3、Pe為控制點繪制三次B樣條曲線。

圖3 三次B樣條曲線擬合示意圖Fig.3 Schematic diagram of cubic B-spline curve fitting

其中:

分量式為:

曲線的斜率為:

S點位于ΔP0P1P2的中線PM1上，距P1點P1M1/3處;E點位于ΔP0P1P2的中線P2M2上，距P2點P2M2/3處;S點切線平行于P0P2;E點切線平行于P1P3;擬合曲線以平均通過式的方式經過控制點。

當有 n個平面離散點時，記為 Pi(i=0，1，…，n －1)。以P0、P1、P2、P3繪制第1 條三次 B 樣條曲線;以 P1、P2、P3、P4繪制第2條三次B樣條曲線;以P2、P3、P4、P5繪制第3條三次B樣條曲線;以此類推，以Pn－4、Pn－3、Pn－2、Pn－1繪制第n － 3 條三次B樣條曲線。用該方法繪制的曲線自然銜接，具有C2級連續性。

1.4 軌跡點切向角獲取算法

基于曲線擬合的切向角計算如式(11)所示:

其中:k表示擬合軌跡線的斜率;atan(k)表示擬合軌跡線上的軌跡點的切線正方向與坐標橫軸正方向的夾角。

軌跡點切向角獲取算法流程如圖4所示，主要分為如下步驟:

1)初始化參與擬合的離散軌跡點，并判斷是否滿足擬合條件，若滿足條件則執行步驟2)，否則執行步驟4)。

2)循環計算前n－3個軌跡點的切向角。

3)計算最后三個軌跡點的切向角。

4)結束算法。

圖4 軌跡點切向角獲取算法流程Fig.4 Flow chart of path point tangential angle acquisition algorithm

2 實驗平臺

本文采用和文獻［17］相同的實驗平臺和實驗場地。如圖5所示，該實驗平臺為北汽紳寶C70改裝的智能車，其車頭安裝有一個LUX4激光雷達和一個毫米波雷達，用于檢測車前遠距離的動態和靜態障礙物;車頂裝有一個Velodyne64線激光雷達，用于檢測車輛周圍360°范圍內的動態和靜態障礙物;車尾有兩個側后方檢測系統(Rear and Side Detection System，RSDS)毫米波雷達用于檢測車輛左后和右后方向的動態車輛。該車還安裝有組合導航系統一套，定位精度達到厘米級。車內安裝兩個同盟視覺科技派克(Allied Vision Technologies-PIKE，AVT-PIKE)相機，用于檢測車道線、停止線、斑馬線和紅綠燈。

圖5 北汽紳寶C70智能車實驗平臺Fig.5 Beiqi Saab C70 intelligent vehicle experimental platform

該智能車實驗平臺的車輪轉角δ(t)取值范圍為±33.7°。函數μ(δ(t))為車輛偏角到方向盤轉角的傳動比。由于汽車傳動系統的限制，傳動比函數是車輪偏角的非線性函數，因此，本文采用離散映射表的方式來近似逼近傳動比函數。整個軌跡跟蹤算法的最終輸出為方向盤的轉角φ(t)，取值范圍為±540°。

實驗路線如圖6所示，實驗地點選擇在園博西二路、長順三路、園博西一路和長順二路組成的環形路線，各個路段分別用 L1、L2、L3和 L4表示，L1和 L2、L2 和 L3、L3 和 L4、L4 和L1之間的路口分別用C1、C2、C3、C4表示。實驗路線中各個路段的信息和路口信息如表1～2所示。

由于實際場景中智能車的行駛速度和道路的曲率都在實時發生變化，因此根據不同的速度，選擇一組合適的k值，對于軌跡跟蹤的精確度和穩定性至關重要。本文經過實際道路的測試，得到了幾何軌跡跟蹤算法的k系數的離散映射值，速度為0 km/h～20 km/h、20 km/h～40 km/h、40 km/h～60 km/h的 k 值分別為 1/5、1/4.5、1/4.2。

由于考慮到算法實時性的要求，本文采用混合軌跡的方式來生成可行駛軌跡線，即車前30 m和車后10 m范圍內采用三次B樣條曲線分段擬合方式和六次貝塞爾曲線擬合方式生成具有切向角屬性的平滑軌跡線，范圍外的軌跡線仍然采用原始高精度地圖軌跡線。通過采用這種策略，既能夠保證軌跡跟蹤對切向角的需要，同時又能夠節約計算資源，提高系統的實時性。

圖6 實驗路線示意圖Fig.6 Schematic diagram of experimental path

表1 實驗路段信息Tab.1 Experimental road information

表2 實驗路口信息Tab.2 Experimental crossroad information

3 實驗結果及分析

測試實驗分為軌跡擬合仿真實驗和軌跡跟蹤測試實驗兩個部分。第一部分，對三次B樣條曲線擬合與文獻［7］中采用的貝塞爾曲線擬合方法進行了軌跡擬合仿真實驗，用于測試兩種曲線擬合方法的特性，同時對兩種擬合算法生成的路徑采用基于線性二自由度的汽車模型進行了軌跡跟蹤仿真實驗，用于測試其跟蹤效果。第二部分進行了真實車輛平臺的軌跡跟蹤測試實驗，并與文獻［3］中提出的基于航向角的軌跡跟蹤算法和文獻［7］中提出的基于貝塞爾曲線擬合的軌跡跟蹤算法進行了對比實驗。根據實驗路段的特點，該部分實驗分為兩個子實驗。首先對由L1、L2、L3和L4組成的環形實驗路線進行了20 km/h繞圈軌跡跟蹤測試實驗，用于測試算法在低速時不同曲率道路下的跟蹤效果。然后以L1實驗路段為測試路段，進行了60 km/h直道軌跡跟蹤測試實驗，用于測試算法在較高速度小曲率道路下的跟蹤效果。

3.1 軌跡擬合仿真實驗

3.1.1 軌跡擬合特性仿真

如圖7所示，三次B樣條曲線具有很好的平滑性，而且相比六次貝塞爾曲線，擬合結果更接近原始軌跡線，在實際的路徑規劃中，尤其是對于曲率變化比較大的軌跡線，擬合效果整體優于高階貝塞爾曲線。另一方面，對于n點曲線擬合來說，采用三次B樣條曲線擬合時，整條軌跡線的階次保持不變，此外當若干個軌跡點發生變化時，只有擬合軌跡線的局部位置發生變化，擬合算法的穩定性和計算復雜性都較低。而當使用貝塞爾曲線進行擬合時，對于n個軌跡點，需要n－1次貝塞爾曲線，而且當若干個軌跡點發生變化時，整條擬合軌跡線都會發生變化，擬合算法的穩定性和計算復雜性相比三次B樣條曲線擬合都較差。

3.1.2 軌跡跟蹤仿真實驗

基于車輛運動學模型，對三次B樣條曲線和六次貝塞爾曲線進行軌跡跟蹤仿真實驗，實驗結果如圖8所示。當原始軌跡線是一個曲率急劇變化的三角波路徑時，采用三次B樣條曲線進行軌跡擬合時，平均跟蹤誤差為3.05 m，最大跟蹤誤差為7.62 m;而采用六次貝塞爾曲線進行軌跡擬合時，平均跟蹤誤差為5.27 m，最大跟蹤誤差為11.98 m。采用六次貝塞爾曲線擬合相比采用三次B樣條曲線擬合，其平均跟蹤誤差要高出72.79%，最大跟蹤誤差高出57.22%，實驗結果表明采用基于三次B樣條曲線擬合的軌跡跟蹤算法在軌跡曲率變化較大的情況下跟蹤效果優于基于貝塞爾曲線擬合的軌跡跟蹤算法。

圖7 曲線擬合仿真示意圖Fig.7 Schematic diagram of curve fitting simulation

圖8 軌跡跟蹤仿真示意圖Fig.8 Schematic diagram of path tracking simulation

3.2 軌跡跟蹤測試實驗

3.2.1 20 km/h環形路線軌跡跟蹤測試

如圖9(a)、(b)所示，三種軌跡跟蹤方式的切向角曲線和方向盤轉角曲線整體上都具有很好的連續性和平滑性。其中基于航向角的軌跡跟蹤算法的切向角曲線θe(t)和方向盤轉角曲線φ(t)分別存在0.15°和5.00°以內的局部微小震蕩。基于三次B樣條曲線擬合的軌跡跟蹤算法的切向角曲線θe(t)和方向盤轉角曲線φ(t)分別存在0.11°和3.80°以內的局部微小震蕩;基于六次貝塞爾曲線擬合的軌跡跟蹤算法的切向角曲線θe(t)和方向盤轉角曲線φ(t)分別存在0.09°和3.20°以內的局部微小震蕩。實驗表明智能車在低速時不同道路曲率下行駛時三種軌跡跟蹤方式跟蹤的平穩性良好。如圖9(c)所示，基于航向角的軌跡跟蹤算法、基于三次B樣條的軌跡跟蹤算法和基于六次貝塞爾曲線擬合的軌跡跟蹤算法的跟蹤誤差的絕對值的平均值分別為0.029 m、0.034 m和0.065 m，最小跟蹤誤差為都為零，最大軌跡跟蹤誤差分別為0.30 m、0.30 m和0.50 m。實驗表明低速時當軌跡線的曲率變化較大時，基于三次B樣條曲線擬合的軌跡跟蹤算法相比基于航向角方式的軌跡跟蹤算法跟蹤精度差異不大，都具有較好的跟蹤精度，而相比基于六次貝塞爾曲線擬合的軌跡跟蹤算法在跟蹤精度方面則具有明顯的優勢。實驗表明三次B樣條曲線擬合得到的平滑軌跡線相比六次貝塞爾曲線擬合得到的平滑軌跡線與原先驗軌跡線的貼合程度更好。

圖9 20 km/h環形路線軌跡跟蹤測試數據分析圖Fig.9 Data analysis diagram of path tracking test for ring path at 20 km/h

3.2.2 60 km/h 直道軌跡跟蹤測試

如圖10(a)、(b)所示:基于航向角的軌跡跟蹤算法的切向角曲線θe(t)和方向盤轉角曲線φ(t)分別存在0.23°和4.80°以內的局部微小震蕩;基于三次B樣條曲線擬合的軌跡跟蹤算法的切向角曲線θe(t)和方向盤轉角曲線φ(t)分別存在0.20°和3.80°以內的局部微小震蕩;基于六次貝塞爾曲線擬合的軌跡跟蹤算法的切向角曲線θe(t)和方向盤轉角曲線φ(t)分別存在0.25°和4.20°以內的局部微小震蕩。實驗表明在較高速度時智能車在小曲率道路下三種軌跡跟蹤方式跟蹤的平穩性良好。如圖10(c)所示，基于航向角的軌跡跟蹤算法、基于三次B樣條的軌跡跟蹤算法和基于六次貝塞爾曲線擬合的軌跡跟蹤算法的跟蹤誤差的絕對值的平均值分別為0.186 m、0.231 m 和0.338 m，最小跟蹤誤差為都為 0.1 m，最大軌跡跟蹤誤差分別為0.30 m、0.30 m 和0.40 m。實驗表明:較高速度下進行小曲率軌跡跟蹤時，基于三次B樣條曲線擬合的軌跡跟蹤算法和基于航向角方式的軌跡跟蹤算法跟蹤精度差異不大，都具有很好的跟蹤精度，而相比基于六次貝塞爾曲線擬合的軌跡跟蹤算法跟蹤精度方面則略有優勢。

圖10 60 km/h直道軌跡跟蹤測試數據分析圖Fig.10 Data analysis diagram of path tracking test for straight path at 60 km/h

實驗表明:基于三次B樣條曲線擬合的軌跡跟蹤算法在低速大曲率和較高速度小曲率時，相比基于航向角方式的軌跡跟蹤算法跟蹤效果差異不大;但是，相比基于航向角方式的軌跡跟蹤算法，該算法解除了對高精度慣導設備的依賴，實現了基于不同傳感器軌跡跟蹤算法的統一。實驗還表明，在低速大曲率時，本文提出的算法相比基于六次貝塞爾曲線擬合的軌跡跟蹤算法具有明顯的優勢，在較高速度小曲率時也有一定的優勢，總體上本文提出的基于三次B樣條曲線擬合的軌跡跟蹤算法優于基于六次貝塞爾曲線擬合的軌跡跟蹤算法。

4 結語

本文介紹了基于三次B樣條曲線擬合方式的幾何軌跡跟蹤算法。與傳統的基于慣導航向角的軌跡跟蹤算法和基于六次貝塞爾曲線擬合的軌跡跟蹤算法相比，本文算法具有如下優點:該算法在不損失軌跡跟蹤性能的前提下，擺脫了對高精度慣導設備的依賴，降低了研發成本;由于該算法的切向角不依賴硬件設備，因此該算法能夠實現多傳感器融合的路徑跟蹤;該算法能夠對行駛軌跡進行平滑處理，具有一定的軌跡優化能力。

采用了本文算法的智能車駕駛平臺參加了2016年中國智能車未來挑戰賽，在簡單的城市環境下以最高40 km/h和簡單高速環境下以最高60 km/h的速度順利完成比賽，并取得了較好名次。比賽結果充分證明了該算法的適應性和準確性。但是，針對本算法的驗證測試還不夠充分，需要開展更多的、更廣泛的測試實驗來進一步驗證算法的適應性和準確性。