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

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

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

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

0 引言

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

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

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

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

1 理論基礎(chǔ)

1.1 車(chē)輛運(yùn)動(dòng)學(xué)模型

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

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

車(chē)輛運(yùn)動(dòng)學(xué)模型如式(1)～(2)所示:

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

1.2 幾何軌跡跟蹤算法

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

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

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

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

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

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

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

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

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

其中:

分量式為:

曲線的斜率為:

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

當(dāng)有 n個(gè)平面離散點(diǎn)時(shí)，記為 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級(jí)連續(xù)性。

1.4 軌跡點(diǎn)切向角獲取算法

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

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

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

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

2)循環(huán)計(jì)算前n－3個(gè)軌跡點(diǎn)的切向角。

3)計(jì)算最后三個(gè)軌跡點(diǎn)的切向角。

4)結(jié)束算法。

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

2 實(shí)驗(yàn)平臺(tái)

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

圖5 北汽紳寶C70智能車(chē)實(shí)驗(yàn)平臺(tái)Fig.5 Beiqi Saab C70 intelligent vehicle experimental platform

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

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

由于實(shí)際場(chǎng)景中智能車(chē)的行駛速度和道路的曲率都在實(shí)時(shí)發(fā)生變化，因此根據(jù)不同的速度，選擇一組合適的k值，對(duì)于軌跡跟蹤的精確度和穩(wěn)定性至關(guān)重要。本文經(jīng)過(guò)實(shí)際道路的測(cè)試，得到了幾何軌跡跟蹤算法的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。

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

圖6 實(shí)驗(yàn)路線示意圖Fig.6 Schematic diagram of experimental path

表1 實(shí)驗(yàn)路段信息Tab.1 Experimental road information

表2 實(shí)驗(yàn)路口信息Tab.2 Experimental crossroad information

3 實(shí)驗(yàn)結(jié)果及分析

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

3.1 軌跡擬合仿真實(shí)驗(yàn)

3.1.1 軌跡擬合特性仿真

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

3.1.2 軌跡跟蹤仿真實(shí)驗(yàn)

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

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

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

3.2 軌跡跟蹤測(cè)試實(shí)驗(yàn)

3.2.1 20 km/h環(huán)形路線軌跡跟蹤測(cè)試

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

圖9 20 km/h環(huán)形路線軌跡跟蹤測(cè)試數(shù)據(jù)分析圖Fig.9 Data analysis diagram of path tracking test for ring path at 20 km/h

3.2.2 60 km/h 直道軌跡跟蹤測(cè)試

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

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

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

4 結(jié)語(yǔ)

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

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