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

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

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

0 引言

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

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

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

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

1 理論基礎(chǔ)

1.1 車輛運動學(xué)模型

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

圖1 前輪轉(zhuǎn)向模型Fig.1 Front wheel steering model

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

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

1.2 幾何軌跡跟蹤算法

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

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

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

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

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

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

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

如圖3 所示，設(shè)四個離散點為:P0、P1、P2、P3，其坐標(biāo)分別為:(x0，y0)、(x1，y1)、(x2，y2)、(x3，y3); 設(shè)中點 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;擬合曲線以平均通過式的方式經(jīng)過控制點。

當(dāng)有 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級連續(xù)性。

1.4 軌跡點切向角獲取算法

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

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

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

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

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

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

4)結(jié)束算法。

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

2 實驗平臺

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

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

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

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

由于實際場景中智能車的行駛速度和道路的曲率都在實時發(fā)生變化，因此根據(jù)不同的速度，選擇一組合適的k值，對于軌跡跟蹤的精確度和穩(wěn)定性至關(guān)重要。本文經(jīng)過實際道路的測試，得到了幾何軌跡跟蹤算法的k系數(shù)的離散映射值，速度為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范圍內(nèi)采用三次B樣條曲線分段擬合方式和六次貝塞爾曲線擬合方式生成具有切向角屬性的平滑軌跡線，范圍外的軌跡線仍然采用原始高精度地圖軌跡線。通過采用這種策略，既能夠保證軌跡跟蹤對切向角的需要，同時又能夠節(jié)約計算資源，提高系統(tǒng)的實時性。

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

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

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

3 實驗結(jié)果及分析

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

3.1 軌跡擬合仿真實驗

3.1.1 軌跡擬合特性仿真

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

3.1.2 軌跡跟蹤仿真實驗

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

圖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環(huán)形路線軌跡跟蹤測試

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

圖9 20 km/h環(huán)形路線軌跡跟蹤測試數(shù)據(jù)分析圖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)和方向盤轉(zhuǎn)角曲線φ(t)分別存在0.23°和4.80°以內(nèi)的局部微小震蕩;基于三次B樣條曲線擬合的軌跡跟蹤算法的切向角曲線θe(t)和方向盤轉(zhuǎn)角曲線φ(t)分別存在0.20°和3.80°以內(nèi)的局部微小震蕩;基于六次貝塞爾曲線擬合的軌跡跟蹤算法的切向角曲線θe(t)和方向盤轉(zhuǎn)角曲線φ(t)分別存在0.25°和4.20°以內(nèi)的局部微小震蕩。實驗表明在較高速度時智能車在小曲率道路下三種軌跡跟蹤方式跟蹤的平穩(wěn)性良好。如圖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樣條曲線擬合的軌跡跟蹤算法和基于航向角方式的軌跡跟蹤算法跟蹤精度差異不大，都具有很好的跟蹤精度，而相比基于六次貝塞爾曲線擬合的軌跡跟蹤算法跟蹤精度方面則略有優(yōu)勢。

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

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

4 結(jié)語

本文介紹了基于三次B樣條曲線擬合方式的幾何軌跡跟蹤算法。與傳統(tǒng)的基于慣導(dǎo)航向角的軌跡跟蹤算法和基于六次貝塞爾曲線擬合的軌跡跟蹤算法相比，本文算法具有如下優(yōu)點:該算法在不損失軌跡跟蹤性能的前提下，擺脫了對高精度慣導(dǎo)設(shè)備的依賴，降低了研發(fā)成本;由于該算法的切向角不依賴硬件設(shè)備，因此該算法能夠?qū)崿F(xiàn)多傳感器融合的路徑跟蹤;該算法能夠?qū)π旭傑壽E進行平滑處理，具有一定的軌跡優(yōu)化能力。

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