基于運(yùn)動(dòng)學(xué)模型的軌跡跟蹤方法研究*

王國(guó)棟，張華強(qiáng)*，蘇慶華

（1.山東理工大學(xué) 機(jī)械工程學(xué)院，山東 淄博255049；2.北京物資學(xué)院 信息學(xué)院，北京101149）

我國(guó)作為世界第二大經(jīng)濟(jì)體，目前正處于科技大發(fā)展的重要時(shí)期，無(wú)人駕駛技術(shù)是實(shí)現(xiàn)科技強(qiáng)國(guó)的重要一環(huán)，而預(yù)設(shè)軌跡的跟蹤是實(shí)現(xiàn)無(wú)人駕駛技術(shù)的關(guān)鍵[1]。

常用的預(yù)設(shè)軌跡跟蹤控制方法通常分為兩種：一種為傳統(tǒng)的基于控制理論的控制方法，主要包括PID控制、模型預(yù)測(cè)控制等方法；另一種則為基于相關(guān)模型的控制方法，常用的有純追蹤算法[2-3]。PID控制方法通過(guò)獲得的誤差數(shù)據(jù)反饋給系統(tǒng)進(jìn)而消除誤差，華南農(nóng)業(yè)大學(xué)的羅錫文院士通過(guò)分析農(nóng)機(jī)的運(yùn)動(dòng)學(xué)模型，采用改進(jìn)PID控制方法設(shè)計(jì)直線路徑跟蹤控制器，取得了不錯(cuò)的路徑追蹤效果[4]；陳慧巖等人設(shè)計(jì)了魯棒PID控制器用于路徑追蹤，通過(guò)實(shí)時(shí)航向角度偏差進(jìn)行反饋調(diào)節(jié)，以車速為控制變量設(shè)置固定增益，通過(guò)試驗(yàn)表明，車輛路徑跟蹤效果較好[5]。PID控制算法應(yīng)用廣泛，優(yōu)點(diǎn)突出，魯棒性優(yōu)異，但是在復(fù)雜的非線性系統(tǒng)中，其不易實(shí)現(xiàn)控制，難以保障控制精度；衛(wèi)榮慕和金世俊對(duì)無(wú)人車的側(cè)滑問(wèn)題進(jìn)行了大量的研究，利用模糊控制預(yù)測(cè)無(wú)人車的非線性系統(tǒng)，通過(guò)李雅普諾夫算法求解模糊控制系統(tǒng)中參數(shù)的自適應(yīng)規(guī)律，利用H∞來(lái)補(bǔ)償無(wú)人車系統(tǒng)中的側(cè)滑量，并設(shè)計(jì)了自適應(yīng)模糊控制器應(yīng)用在無(wú)人車系統(tǒng)中[6]。模型預(yù)測(cè)控制方法在保證車輛追蹤預(yù)設(shè)軌跡的同時(shí)，能保證車輛狀態(tài)的穩(wěn)定，但是模型控制方法實(shí)現(xiàn)過(guò)程較為復(fù)雜，需要進(jìn)行大量的迭代求解，效率較低。純追蹤算法是最早應(yīng)用在無(wú)人車路徑追蹤中的方法，基于車輛的運(yùn)動(dòng)學(xué)模型，通過(guò)具體公式表達(dá)出前輪轉(zhuǎn)向角與橫向誤差和航向誤差的關(guān)系[7-11]，從而控制車輛轉(zhuǎn)向輪實(shí)現(xiàn)對(duì)預(yù)設(shè)軌跡的跟蹤。

本文通過(guò)車輛的運(yùn)動(dòng)學(xué)模型對(duì)純追蹤算法進(jìn)行構(gòu)建，設(shè)計(jì)路徑追蹤方法，并以常用車輛參數(shù)信息設(shè)計(jì)仿真實(shí)驗(yàn)，對(duì)曲線、直線軌跡進(jìn)行追蹤。

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

精確的車輛運(yùn)動(dòng)學(xué)模型是實(shí)現(xiàn)軌跡跟蹤的關(guān)鍵因素，車輛的運(yùn)動(dòng)學(xué)模型通過(guò)車輛的簡(jiǎn)單運(yùn)動(dòng)規(guī)律建立速度和位置等之間的聯(lián)系，因此在理想狀態(tài)下，只考慮車輛的平面運(yùn)動(dòng)，忽略側(cè)向滑動(dòng)對(duì)車輛的影響，在平面坐標(biāo)系下建立運(yùn)動(dòng)學(xué)模型，如圖1所示。

圖1 車輛運(yùn)動(dòng)學(xué)模型

圖中，（xs，y）s表示車輛前輪軸心坐標(biāo)；（xd，yd）表示車輛后輪軸心坐標(biāo)；α表示車輛的航向角；δs表示車輛前輪轉(zhuǎn)角；vs表示車輛前輪速度；vd表示車輛后輪速度；R表示車輛的轉(zhuǎn)向半徑；L表示車輛的軸距。

根據(jù)前后輪幾何約束可得車輛運(yùn)動(dòng)學(xué)模型為：

2 構(gòu)建純追蹤算法

在本節(jié)中，以自行車模型為基礎(chǔ)分析純追蹤算法，即假設(shè)：

（1）車輛行駛速度恒定，只考慮橫擺和橫向運(yùn)動(dòng)兩個(gè)自由度。

（2）不考慮空氣動(dòng)力對(duì)車輛的影響。

（3）不考慮切向力對(duì)車輛輪胎的影響。

（4）車輛在平面內(nèi)行駛。

純追蹤算法是一種模擬人類駕駛習(xí)慣的幾何追蹤模型，根據(jù)車輛的行駛速度和路徑信息設(shè)定軌跡上的一個(gè)路徑點(diǎn)，計(jì)算車輛當(dāng)前位置距離預(yù)設(shè)目標(biāo)點(diǎn)所需轉(zhuǎn)過(guò)的角度，控制車輛向路徑點(diǎn)行駛。純追蹤算法適應(yīng)性強(qiáng)，不需要考慮車輛復(fù)雜的動(dòng)力學(xué)模型，適應(yīng)于多種復(fù)雜軌跡的追蹤。

純追蹤算法中前視距離的大小將會(huì)直接影響車輛的軌跡跟蹤效果，前視距離較大時(shí)，車輛將會(huì)以較小的曲率向路徑行駛，但系統(tǒng)響應(yīng)較慢，可能會(huì)產(chǎn)生“抄近路”的問(wèn)題，影響控制精度；當(dāng)前視距離較小時(shí)，車輛將會(huì)以較大曲率向路徑行駛，車輛會(huì)因此頻繁轉(zhuǎn)向，產(chǎn)生較大的振蕩，降低車輛的穩(wěn)定性。因此，前視距離的大小將會(huì)影響軌跡跟蹤的效果。

純追蹤算法的幾何示意圖如圖2所示。

圖2 純追蹤算法示意圖

圖中，m為預(yù)設(shè)軌跡，A（xg，y）g為預(yù)設(shè)軌跡上的目標(biāo)點(diǎn)，d為車輛的橫向誤差，θ為車輛的航向誤差，Ld為純追蹤算法中的前視距離，k表示曲率，2α表示車輛到達(dá)目標(biāo)點(diǎn)所轉(zhuǎn)過(guò)的角度。

由運(yùn)動(dòng)學(xué)模型可得前輪轉(zhuǎn)向角為：

由圖2中目標(biāo)點(diǎn)橫坐標(biāo)與實(shí)時(shí)誤差幾何關(guān)系得：

根據(jù)式（2）、式（3）推導(dǎo)車輛前輪偏角為：

3 試驗(yàn)驗(yàn)證及分析

為進(jìn)一步驗(yàn)證本文所設(shè)計(jì)基于運(yùn)動(dòng)學(xué)模型的軌跡跟蹤方法的有效性，以低速農(nóng)機(jī)為仿真實(shí)驗(yàn)平臺(tái)，農(nóng)機(jī)參數(shù)信息見表1。

表1 農(nóng)機(jī)參數(shù)信息

基于車輛運(yùn)動(dòng)學(xué)模型設(shè)計(jì)MATLAB仿真實(shí)驗(yàn)，設(shè)置車輛軸距為2.3m，初始偏差為0.5m。在MATLAB中通過(guò)純追蹤算法對(duì)預(yù)設(shè)路徑進(jìn)行仿真追蹤，仿真結(jié)果如圖3、圖4所示。

圖3 直線軌跡追蹤效果圖

圖4 曲線軌跡追蹤效果圖

由上仿真結(jié)果可知，車輛在軌跡跟蹤的初始階段由于初始誤差的影響，需經(jīng)過(guò)一段時(shí)間進(jìn)行上線校正，校正后能夠?qū)︻A(yù)設(shè)軌跡實(shí)現(xiàn)有效的跟蹤，其中直線軌跡跟蹤效果最好，在曲線軌跡的跟蹤中，實(shí)際軌跡在曲線的拐點(diǎn)處發(fā)生輕微偏離，但依然能夠?qū)崿F(xiàn)曲線軌跡的有效跟蹤。因此，本文所設(shè)計(jì)的方法無(wú)論是直線預(yù)設(shè)軌跡還是曲線預(yù)設(shè)軌跡，都有較好的路徑追蹤效果。

4 結(jié)束語(yǔ)

針對(duì)無(wú)人駕駛的軌跡跟蹤問(wèn)題，設(shè)計(jì)了一種基于運(yùn)行學(xué)模型的軌跡跟蹤方法，以低速農(nóng)機(jī)參數(shù)信息設(shè)計(jì)MATLAB仿真試驗(yàn)，實(shí)驗(yàn)結(jié)果表明，所設(shè)計(jì)的方法無(wú)論對(duì)于直線軌跡還是對(duì)于曲線軌跡都有較好的軌跡跟蹤效果。