廣告
幾種常見(jiàn)的路徑跟蹤算法
2022-01-03 17:25:28· 來(lái)源：智能運(yùn)載裝備研究所 作者：杜二磊
一.路徑跟蹤簡(jiǎn)介在路徑跟蹤過(guò)程中，參考軌跡曲線與時(shí)間參數(shù)無(wú)關(guān)，跟蹤軌跡時(shí)可以假設(shè)無(wú)人車以當(dāng)前速度勻速行駛，以一定的代價(jià)規(guī)則形成行駛路徑趨近于參考路徑；
一.路徑跟蹤簡(jiǎn)介
在路徑跟蹤過(guò)程中，參考軌跡曲線與時(shí)間參數(shù)無(wú)關(guān)，跟蹤軌跡時(shí)可以假設(shè)無(wú)人車以當(dāng)前速度勻速行駛，以一定的代價(jià)規(guī)則形成行駛路徑趨近于參考路徑；而軌跡跟蹤，其參考路徑曲線與時(shí)間和空間均相關(guān)，并且要求無(wú)人車在規(guī)定時(shí)間內(nèi)達(dá)到某一預(yù)設(shè)好的參考路徑點(diǎn)。
圖1 智能車路徑跟蹤問(wèn)題
路徑跟蹤一般是通過(guò)控制轉(zhuǎn)向角度，實(shí)時(shí)對(duì)預(yù)測(cè)軌跡的跟蹤。不僅僅要求跟蹤效果的準(zhǔn)確性，從而保證智能車的行駛安全，還要求跟蹤過(guò)程中車輛的穩(wěn)定性和安全性?，F(xiàn)有路徑跟蹤控制方法包括：
（1）基于道路幾何原理的路徑跟蹤控制，如純跟蹤控制、Stanley控制等；
（2）基于經(jīng)典控制理論路徑跟蹤控制，如PID控制、線性反饋控制等；
（3）基于現(xiàn)代控制理論路徑跟蹤控制，如模型預(yù)測(cè)控制、最優(yōu)控制等。
本文在分析現(xiàn)有路徑跟蹤控制方法的基礎(chǔ)之上，選擇基于經(jīng)典控制理論的PID控制、基于道路幾何的Stanley控制以及基于現(xiàn)代控制理論的最優(yōu)控制三種方法完成智能車路徑跟蹤控制器設(shè)計(jì)。并針對(duì)設(shè)計(jì)的控制算法進(jìn)行仿真分析，對(duì)比三種控制算法的優(yōu)缺點(diǎn)，為路徑跟蹤控制進(jìn)一步研究提供一定的參考與借鑒意義。
二.基于PID控制算法的路徑跟蹤
1.PID算法簡(jiǎn)介
首先進(jìn)行期望值輸入，然后和實(shí)際值進(jìn)行偏差計(jì)算。所得到的偏差的比例kp、ki、kd的線性組合構(gòu)成控制量，在傳遞給被控對(duì)象，被控對(duì)象輸出實(shí)際值，詳細(xì)可參考公眾號(hào)文章：無(wú)人駕駛控制算法之PID控制和PID控制原理及參數(shù)調(diào)節(jié)。
圖2 PID算法原理
PID控制器為：
其中，u(t)為控制器控制量，Kp為控制器比例項(xiàng)系數(shù)，Kd為控制器微分項(xiàng)系數(shù)，Ki為控制器積分項(xiàng)系數(shù)，e(t)代表期望值與實(shí)際值之間的偏差。
2.車輛-道路相對(duì)位置關(guān)系
假設(shè)車輛預(yù)瞄前方道路上一點(diǎn)A，它在慣性坐標(biāo)系下的坐標(biāo)為，到點(diǎn)A參考路徑曲線的切線方向與車輛坐標(biāo)系x軸夾角為，車輛質(zhì)心位置為，車輛坐標(biāo)系x軸線與X軸夾角為，如圖所示。
圖3 車輛道路相對(duì)位置關(guān)系
在車輛坐標(biāo)系xoy下，可得到點(diǎn)A與車輛質(zhì)心的位置偏差及方向偏差，其相對(duì)位置幾何關(guān)系有：
其中上式中l(wèi)e為預(yù)瞄距離；de為車輛坐標(biāo)系下xoy車輛質(zhì)心與預(yù)瞄點(diǎn)A的橫向距離偏差；φe為車輛坐標(biāo)系xoy下車輛質(zhì)心與預(yù)瞄點(diǎn)A的航向偏差。
利用PID，車輛路徑跟蹤轉(zhuǎn)向過(guò)程中可通過(guò)計(jì)算橫向距離偏差與航向偏差從而得到前輪轉(zhuǎn)角：
上式Kpde、Kdde、Kide代表橫向偏差的比例、微分和積分因子；Kpφe、Kdφe、Kiφe代表航向偏差的比例、微分和積分因子；de表示車輛質(zhì)心與參考點(diǎn)之間的橫向偏差；φe為航向偏差。
三.基于Stanley控制算法路徑跟蹤
Stanley控制算法是一種基于橫向偏差的非線性反饋函數(shù)，并且能實(shí)現(xiàn)橫向偏差指數(shù)收斂于0。如圖4所示，橫向偏差de是車輛前輪到給定最近軌跡點(diǎn)p的距離；δe是給定軌跡上切線方向與車身航向之間的夾角；φe是前輪線速度與車身航向之間的夾角。
圖4 Stanley控制算法示意圖
在不考慮橫向誤差的情況之下，需要車輛跟隨期望軌跡運(yùn)動(dòng)，需要前輪轉(zhuǎn)角時(shí)刻保持與期望軌跡上點(diǎn)的切線方向一致，即前輪轉(zhuǎn)角為φe。
在不考慮航向誤差的情況之下，需要車輛跟隨期望軌跡運(yùn)動(dòng)，則前輪轉(zhuǎn)角需要消除橫向偏差，即前輪轉(zhuǎn)角為δe。
因此同時(shí)考慮橫向偏差和航向偏差，前輪轉(zhuǎn)角為：
根據(jù)Stanley示意圖可得：
其中，
所以，
當(dāng)橫向偏差很小時(shí)，
趨近于0。則
由k>0可見(jiàn)，橫向偏差de指數(shù)收斂于0，收斂速度取決于參數(shù)k。因此，
則前輪轉(zhuǎn)角為
四.基于LQR控制算法的路徑跟蹤
1.車輛運(yùn)動(dòng)學(xué)模型
車輛運(yùn)動(dòng)學(xué)模型如圖5所示。在慣性坐標(biāo)系XOY下，(Xf,Yf)和(Xr,Yr)分別是車輛前軸和后軸的軸心坐標(biāo)，φ為車輛的橫擺角，δf為前輪轉(zhuǎn)角，vr為車輛的后軸中心速度，L為汽車軸距。
圖5 車輛運(yùn)動(dòng)學(xué)模型
車輛運(yùn)動(dòng)學(xué)模型為
2.運(yùn)動(dòng)學(xué)狀態(tài)空間模型
根據(jù)車輛運(yùn)動(dòng)學(xué)方程，將車輛運(yùn)動(dòng)狀態(tài)系統(tǒng)看作一個(gè)輸入u=[v,δ]^T以及狀態(tài)參數(shù)X=[x,y,φ]^T的控制系統(tǒng)?？杀硎緸椋?/span>
在車輛控制系統(tǒng)中，期望軌跡可表示為：
上式中
將車輛運(yùn)動(dòng)學(xué)非線性模型在期望點(diǎn)(Xc,uc)處進(jìn)行泰勒式展開，忽略高階項(xiàng)，可得：
記
可得：
即
可得線性化之后的路徑跟蹤誤差模型，將其離散化可得：
其中，T=0.01s為采樣時(shí)間，系統(tǒng)矩陣為
3.LQR控制器設(shè)計(jì)
LQR推導(dǎo)過(guò)程本文不做詳細(xì)贅述，具體可參考公眾號(hào)文章：無(wú)人駕駛控制算法之LQR控制和連續(xù)系統(tǒng)與離散系統(tǒng)LQR問(wèn)題求解：以車輛二自由度模型為例。在建立車輛運(yùn)動(dòng)學(xué)模型之后，可得車輛的運(yùn)動(dòng)狀態(tài)空間方程，給定權(quán)重矩陣Q和R后進(jìn)而可以求得其LQR控制器。
五.對(duì)比分析
從安全性、舒適性、穩(wěn)定性三個(gè)方面來(lái)評(píng)價(jià)PID、Stanley、LQR三種控制算法，驗(yàn)證三種控制方法的有效性及優(yōu)缺點(diǎn)。
圖6 三種控制算法效果對(duì)比圖
評(píng)價(jià)結(jié)果如下表1所示?？梢?jiàn)，基于線性化運(yùn)動(dòng)學(xué)模型的LQR所得結(jié)果綜合性能優(yōu)于其他兩種方法。
表1 三種控制算法評(píng)價(jià)結(jié)果
點(diǎn)贊 1 反對(duì) 0 舉報(bào) 0
收藏 0
評(píng)論 0 分享 90
廣告 編輯推薦
最新資訊
-
聯(lián)合國(guó)法規(guī)R60對(duì)兩輪車操縱件與指示裝置的
2026-03-04 12:08
-
標(biāo)準(zhǔn)立項(xiàng)|《汽車異種材料鉚接接頭拉伸性能
2026-03-04 11:40
-
“汽車大角度座椅 第1部分：通用要求”將有
2026-03-04 11:39
-
“汽車大角度座椅 第2部分：可靠性要求”將
2026-03-04 11:39
-
電池耐久評(píng)價(jià)開始從“電池單體性能指標(biāo)”轉(zhuǎn)
2026-03-04 11:38
