基于預(yù)瞄的智能車位姿補(bǔ)償軌跡跟蹤算法

王軍雷 李健明 吳學(xué)易

摘 要：針對(duì)單點(diǎn)預(yù)瞄、模糊控制等軌跡跟蹤算法魯棒性低以及模型預(yù)測(cè)控制算法計(jì)算量大、參數(shù)不易確定的問(wèn)題，提出基于預(yù)瞄的智能車位姿補(bǔ)償軌跡跟蹤算法。通過(guò)建立目標(biāo)車輛的運(yùn)動(dòng)學(xué)模型，在軌跡跟蹤建模時(shí)進(jìn)行位姿補(bǔ)償，降低執(zhí)行機(jī)構(gòu)延時(shí);使用預(yù)瞄算法，構(gòu)建智能車轉(zhuǎn)向模型，求解最優(yōu)方向盤轉(zhuǎn)角。實(shí)車測(cè)試結(jié)果表明，所提軌跡跟蹤算法通過(guò)對(duì)位姿進(jìn)行實(shí)時(shí)補(bǔ)償，可以進(jìn)一步減小橫向誤差，具有實(shí)時(shí)性高，魯棒性好的特點(diǎn)，能夠滿足智能網(wǎng)聯(lián)汽車實(shí)際運(yùn)行需要。

關(guān)鍵詞：軌跡跟蹤;兩點(diǎn)預(yù)瞄;位姿預(yù)測(cè);位姿補(bǔ)償;最優(yōu)轉(zhuǎn)向角;智能網(wǎng)聯(lián)汽車

中圖分類號(hào)：U462 ?文獻(xiàn)標(biāo)識(shí)碼：A ?文章編號(hào)：1671-7988（2020）21-40-06

Abstract： Aiming at the problems of low robustness of trajectory tracking algorithms such as single-point preview and fuzzy control， large amount of calculation and difficult determination of parameters of model predictive control algorithm， an intelligent vehicle position and attitude compensation trajectory tracking algorithm based on preview is proposed. The kinematic model of the target vehicle is established， and the position and attitude compensation is carried out during the track tracking modeling to reduce the actuator delay. Using preview algorithm， the steering model of intelligent vehicle is built to solve the optimal steering wheel angle. The experimental results show that the proposed trajectory tracking algorithm can further reduce the lateral error by real-time compensation of the pose. It has the characteristics of high real-time performance and good robustness， and can meet the actual operation needs of the intelligent connected vehicle.

Keywords： Trajectory tracking; Two-point preview; Pose prediction; Pose compensation; Optimum steering angle; Intelligent connected vehicle

CLC NO.： U462 ?Document Code： A ?Article ID： 1671-7988（2020）21-40-06

引言

軌跡跟蹤作為智能網(wǎng)聯(lián)汽車軟件架構(gòu)中重要組成部分，直接關(guān)系到智能網(wǎng)聯(lián)汽車是否按照軌跡規(guī)劃層規(guī)劃出的軌跡進(jìn)行行駛，以及是否會(huì)與其它道路交通參與者發(fā)生碰撞。軌跡跟蹤效果的好壞不僅體現(xiàn)在實(shí)際軌跡與期望軌跡之間的誤差，而且體現(xiàn)在智能網(wǎng)聯(lián)汽車的安全性、平順性以及舒適性。常用軌跡跟蹤有純軌跡跟蹤、單點(diǎn)預(yù)瞄、雙點(diǎn)預(yù)瞄、模型預(yù)測(cè)控制、模糊控制、深度學(xué)習(xí)等方法。秦萬(wàn)軍等人基于無(wú)人車輪胎受力情況建立二自由度模型，從而進(jìn)行軌跡跟蹤。刁勤晴等人提出一種基于雙預(yù)瞄點(diǎn)的智能車大曲率路徑橫向模糊控制方法。梁政濤等人基于車輛運(yùn)動(dòng)學(xué)模型，采用模型預(yù)測(cè)控制方法進(jìn)行車輛橫向控制。伏雨旋等人使用時(shí)變參數(shù)的人工勢(shì)場(chǎng)與PID協(xié)調(diào)控制的軌跡跟蹤方法，系統(tǒng)使用橫向誤差和航向誤差的時(shí)變參數(shù)人工勢(shì)場(chǎng)力對(duì)前輪轉(zhuǎn)角進(jìn)行控制。Chen等人使用端到端的深度學(xué)習(xí)方式進(jìn)行智能車控制，系統(tǒng)直接輸入圖片，輸出結(jié)果直接為汽車橫向控制參數(shù)。Norouzi Armin等人提出了一種基于元啟發(fā)式優(yōu)化算法的新型李雅普諾夫魯棒控制器。上述方法雖取得一定研究成果，但在軌跡跟蹤效果方面還存在一定的提升空間。

為更好的智能網(wǎng)聯(lián)汽車軌跡跟蹤問(wèn)題，本文提出基于滾動(dòng)優(yōu)化的智能車軌跡跟蹤算法。算法分為三步驟，首先，通過(guò)建立汽車運(yùn)動(dòng)學(xué)模型，對(duì)執(zhí)行機(jī)構(gòu)延時(shí)產(chǎn)生的位姿變化量進(jìn)行補(bǔ)償;其次，使用改進(jìn)的預(yù)瞄算法，構(gòu)建智能車轉(zhuǎn)向模型并求解最優(yōu)方向盤轉(zhuǎn)角;最后，使用PID控制進(jìn)行誤差調(diào)節(jié)，使得實(shí)際方向盤轉(zhuǎn)角快速穩(wěn)定的跟蹤上期望方向盤轉(zhuǎn)角。實(shí)車實(shí)驗(yàn)結(jié)果表明，本文使用算法實(shí)時(shí)性高，魯棒性好，滿足實(shí)際測(cè)試需要。

1 汽車位姿補(bǔ)償

無(wú)人駕駛汽車在軌跡跟蹤過(guò)程中，由于執(zhí)行機(jī)構(gòu)延時(shí)問(wèn)題，控制程序發(fā)出執(zhí)行命令到執(zhí)行機(jī)構(gòu)開(kāi)始響應(yīng)相關(guān)控制命令時(shí)，無(wú)人駕駛汽車已經(jīng)移動(dòng)一段距離，當(dāng)無(wú)人駕駛汽車高速行駛時(shí)，其延時(shí)問(wèn)題對(duì)汽車平順性和控制精度的影響尤為明顯。為解決執(zhí)行機(jī)構(gòu)延時(shí)問(wèn)題，本文建立汽車運(yùn)動(dòng)學(xué)模型，研究在延時(shí)時(shí)間段無(wú)人駕駛汽車位姿的改變量，使用當(dāng)前位置加上位姿改變量量得到一個(gè)虛擬位姿，使用虛擬位姿計(jì)算當(dāng)前需要跟蹤參考軌跡的控制量，示意圖如圖1所示。S為無(wú)人駕駛汽車參考軌跡，Pt為無(wú)人駕駛汽車t時(shí)刻在參考軌跡上選擇的預(yù)瞄目標(biāo)點(diǎn)，Xt（xt， yt， ψt）為t時(shí)刻無(wú)人駕駛汽車的姿態(tài)信息，Xt'（xt'， yt'， ψt'）執(zhí)行機(jī)構(gòu)開(kāi)始執(zhí)行t時(shí)刻控制指令時(shí)無(wú)人駕駛汽車的姿態(tài)信息。無(wú)人駕駛汽車執(zhí)行機(jī)構(gòu)執(zhí)行延時(shí)時(shí)間假設(shè)為t0，在t0時(shí)間段無(wú)人駕駛汽車前進(jìn)的距離△dt'為：

為得到無(wú)人駕駛汽車在t0時(shí)間段其位姿改變量，建立如圖2所示的智能車運(yùn)動(dòng)學(xué)簡(jiǎn)化模型，簡(jiǎn)化模型假設(shè)無(wú)人駕駛汽車在運(yùn)動(dòng)過(guò)程中前后軸左右輪側(cè)偏剛度保持一致，且在短時(shí)間內(nèi)無(wú)人駕駛汽車可以看作一恒定加速度在運(yùn)動(dòng)。因此，將無(wú)人駕駛汽車四輪簡(jiǎn)化成如圖所示的兩輪自行車模型。δf、δr分別為前后輪轉(zhuǎn)角，lf、lr分別為汽車重心到前后軸之間的距離，υt為智能車t時(shí)刻速度，ψt為智能車t時(shí)刻航向角，β為智能車速度方向與智能車縱軸方向間的夾角，at為智能車t時(shí)刻加速度，dt為時(shí)間改變量。

2 軌跡跟蹤

在軌跡跟蹤控制過(guò)程中，最優(yōu)的控制目標(biāo)是無(wú)人駕駛汽車當(dāng)前位置與參考軌跡之間無(wú)橫向誤差，如圖3所示，Pt為參考軌跡S到無(wú)人駕駛汽車當(dāng)前位置之間橫向距離最小參考點(diǎn)，最小距離用△dt表示，且參考軌跡在參考點(diǎn)Pt點(diǎn)的方向與無(wú)人駕駛汽車當(dāng)前航向角ψt之間的誤差盡可能小。然而由于執(zhí)行機(jī)構(gòu)延時(shí)問(wèn)題的存在，因此，當(dāng)前t時(shí)刻計(jì)算控制量應(yīng)計(jì)算如圖4所示的無(wú)人駕駛汽車位姿Xt'所對(duì)應(yīng)的控制量，Xt到Xt'的位姿變化量由式2-式6給出。許多學(xué)者對(duì)軌跡跟蹤多有研究，文獻(xiàn)[6]為最為經(jīng)典的單點(diǎn)預(yù)瞄控制模型，但單點(diǎn)預(yù)瞄沒(méi)有考慮汽車運(yùn)動(dòng)學(xué)模型對(duì)控制的影響，且沒(méi)有預(yù)見(jiàn)性，一般適用于車速較低的場(chǎng)景。模型預(yù)測(cè)控制基于汽車運(yùn)動(dòng)學(xué)模型，求解汽車未來(lái)一段時(shí)間內(nèi)的姿態(tài)，根據(jù)姿態(tài)量和參考軌跡構(gòu)建優(yōu)化函數(shù)，模型預(yù)測(cè)都遵循預(yù)測(cè)模型、滾動(dòng)優(yōu)化和反饋控制，從而得到最優(yōu)的控制量，但模型預(yù)測(cè)控制計(jì)算量大，對(duì)計(jì)算資源要求較高。本文提出基于兩點(diǎn)預(yù)瞄的控制模型，如圖4所示，基于位姿補(bǔ)償Xt'，給出兩個(gè)預(yù)瞄點(diǎn)Q1、Q2，預(yù)瞄點(diǎn)Q2到Xt'的直線距離dt1為：

其中T0為預(yù)瞄時(shí)間，υt'為t時(shí)刻位姿補(bǔ)償后的智能車速度。

預(yù)瞄點(diǎn)Q2為參考軌跡最大曲率控制點(diǎn)，如圖5所示，Pt'為Xt'到參考軌跡距離最小的點(diǎn)，Pt''為實(shí)時(shí)參考軌跡約束點(diǎn)，將Pt'點(diǎn)至Pt''點(diǎn)間參考軌跡作為t時(shí)刻研究對(duì)象，Pt'點(diǎn)至Pt''點(diǎn)間距離為：

其中T1為參考軌跡選擇時(shí)間長(zhǎng)度。

將Pt'點(diǎn)至Pt''點(diǎn)連接成直線，構(gòu)成直線方程，得到Pt'點(diǎn)至Pt''點(diǎn)間參考軌跡偏離直線方程最大的軌跡點(diǎn)Q2。如圖5所示，在參考軌跡Q2附近截取一段距離，構(gòu)成點(diǎn)集，使用最小二乘法擬合成圓，得到圓心Ot（x，y）。

公式9至公式17中，xi、yi為點(diǎn)集中點(diǎn)的橫縱坐標(biāo)，xo、yoi、R分別為擬合圓的圓心坐標(biāo)和半徑。根據(jù)式9-式16可得到擬合圓的中心的半徑。M11、M12、M21、M22、H1、H2、A、B、C為中間變量，無(wú)實(shí)際含義。

υQ2為根據(jù)Q2點(diǎn)處根據(jù)擬合圓半徑R得到的最大車速， kQ2為半徑調(diào)節(jié)系數(shù)，dQ2為智能車位姿補(bǔ)償后點(diǎn)Xt'到Q2點(diǎn)之間的距離。當(dāng)t時(shí)刻時(shí)刻實(shí)際車速大于安全車速時(shí)，系統(tǒng)應(yīng)向駕駛員提供預(yù)警;實(shí)際車速小于安全車速時(shí)，無(wú)人駕駛汽車執(zhí)行速度規(guī)劃層規(guī)劃的速度。

根據(jù)“Ackermann”幾何關(guān)系。汽車軌跡曲率與方向盤轉(zhuǎn)角成正比：

式中，θ為方向盤轉(zhuǎn)角，i為方向盤傳動(dòng)比，L為智能車軸距，R't為t時(shí)刻位姿補(bǔ)償后智能車轉(zhuǎn)彎半徑，為智能車側(cè)向加速度。

根據(jù)圖6，經(jīng)過(guò)時(shí)間T0之后，則無(wú)人駕駛汽車的橫向位移關(guān)系式為：

式中為智能車側(cè)向速度。

由式21-式23可知：

“Aeklman”幾何關(guān)系的最優(yōu)方向盤轉(zhuǎn)角為：

在單點(diǎn)預(yù)瞄運(yùn)動(dòng)分析模型中，將無(wú)人駕駛汽車簡(jiǎn)化為一個(gè)點(diǎn)，無(wú)人駕駛汽車軌跡跟蹤的優(yōu)劣由到跟蹤軌跡的最小橫向誤差決定，然而由于無(wú)人駕駛汽車與跟蹤軌跡之間存在航向誤差，即使無(wú)橫向誤差，航向誤差的存在將時(shí)汽車在下一時(shí)刻偏離跟蹤軌跡，因此，最優(yōu)方向盤轉(zhuǎn)角應(yīng)考慮航向誤差的影響，本文建立如式25所示的最優(yōu)方向盤模型：

k2為“Aeklman”幾何關(guān)系的最優(yōu)方向盤轉(zhuǎn)角權(quán)重，k3為航向誤差權(quán)重。

然而由于執(zhí)行機(jī)構(gòu)執(zhí)行精度的問(wèn)題，最優(yōu)方向盤轉(zhuǎn)角與實(shí)際方向盤轉(zhuǎn)角之間存在誤差，因此，通過(guò)PID控制進(jìn)行誤差補(bǔ)償。

式中：kp、ki、kd分別為比例、積分、微分環(huán)節(jié)的系數(shù)， e（t）為t時(shí)刻最優(yōu)方向盤轉(zhuǎn)角與實(shí)際方向盤轉(zhuǎn)角之間的誤差，μ（t）為t時(shí)刻發(fā)送給方向盤的實(shí)際控制量，T為考慮的歷史數(shù)據(jù)時(shí)長(zhǎng)。

根據(jù)式25-式27，可求得每一時(shí)刻的請(qǐng)求方向盤轉(zhuǎn)角。

3 仿真實(shí)驗(yàn)

本文為驗(yàn)證基于預(yù)瞄的智能車位姿補(bǔ)償軌跡跟蹤算法，使用MATALB和Carsim進(jìn)行聯(lián)合仿真，并對(duì)比純跟蹤算法、本文所提算法、模型預(yù)測(cè)控制算法（MFC）在跟蹤效果和實(shí)時(shí)性等方面的因素。如圖7、8、9所示分別為純跟蹤算法、本文所提算法、MPC算法仿真效果，圖中黑色點(diǎn)劃線為參考軌跡，紅色實(shí)線為跟蹤參考軌跡時(shí)的跟蹤效果，藍(lán)色實(shí)線為橫向誤差，從圖7-圖9中可以看出，本文所提算法橫向誤差控制在0.7m范圍內(nèi)，滿足智能車對(duì)軌跡跟蹤橫向誤差精度要求。表1為三種控制方法的仿真時(shí)長(zhǎng)，設(shè)置車速為30km/h，仿真參數(shù)一致，從圖和表中可知，本文所提算法實(shí)時(shí)性較高，控制精度好，MPC雖然橫向誤差更小，但是實(shí)時(shí)性較差。

為驗(yàn)證本文所提局部路徑規(guī)劃算法，使用東風(fēng)柳汽S50EV電動(dòng)車作為實(shí)車實(shí)驗(yàn)平臺(tái)，實(shí)驗(yàn)車搭載一套GNSS定位系統(tǒng)根，一個(gè)64線激光雷達(dá)，兩個(gè)16線激光雷達(dá)，一個(gè)4線激光雷達(dá)一個(gè)毫米波雷達(dá)和2個(gè)不同焦距的攝像頭，實(shí)車圖像如圖10所示。

本文實(shí)車實(shí)驗(yàn)時(shí)設(shè)定巡航速度為30km/h，實(shí)車測(cè)試結(jié)果如圖11所示，圖中黑色實(shí)線為參考軌跡，參考軌跡為事先采集的GPS數(shù)據(jù)，黑色虛線為沒(méi)有進(jìn)行位姿補(bǔ)償時(shí)軌跡跟蹤效果，黑色點(diǎn)劃線為進(jìn)行位姿補(bǔ)償后的軌跡跟蹤效果，圖12中點(diǎn)劃線為沒(méi)有進(jìn)行位姿補(bǔ)償時(shí)軌跡跟蹤橫向誤差，實(shí)線為進(jìn)行位姿補(bǔ)償后軌跡跟蹤橫向誤差，圖13為每個(gè)GPS點(diǎn)對(duì)應(yīng)的速度規(guī)劃結(jié)果。

從圖11-圖13可以看出，本文提出基于預(yù)瞄的智能車位姿補(bǔ)償軌跡跟蹤算法，在實(shí)時(shí)對(duì)位姿進(jìn)行補(bǔ)償后，橫向誤差進(jìn)一步縮小，滿足軌跡跟蹤要求，且滿足汽車實(shí)時(shí)性要求，并且在進(jìn)入彎道前提前減速，駛出彎道都能夠立即加速，速度響應(yīng)快，控制精度高。

4 結(jié)語(yǔ)

本文針對(duì)智能網(wǎng)聯(lián)汽車的軌跡跟蹤問(wèn)題，提出了一種基于預(yù)瞄的智能車位姿補(bǔ)償軌跟蹤算法。算法首先利用車輛動(dòng)力學(xué)模型預(yù)測(cè)車輛在執(zhí)行機(jī)構(gòu)延時(shí)時(shí)間內(nèi)的位姿變化量，在軌跡建模時(shí)對(duì)車輛進(jìn)行位姿補(bǔ)償以降低執(zhí)行機(jī)構(gòu)延時(shí)問(wèn)題;其次，本文利用預(yù)瞄算法，在車輛軌跡上搜索轉(zhuǎn)彎半徑最小的參考點(diǎn)，在其附近截取一段參考點(diǎn)進(jìn)行圓擬合，得到轉(zhuǎn)彎半徑，使用轉(zhuǎn)彎半徑和到其點(diǎn)的距離求解當(dāng)前時(shí)刻期望速度。根據(jù)當(dāng)前車速選擇軌跡上一個(gè)預(yù)瞄點(diǎn)，構(gòu)建智能車轉(zhuǎn)向模型，根據(jù)轉(zhuǎn)向模型求解最優(yōu)方向盤轉(zhuǎn)角;最后利用PID控制對(duì)車輛實(shí)際方向盤轉(zhuǎn)角進(jìn)行誤差調(diào)節(jié)，使其能高效的跟蹤期望方向盤轉(zhuǎn)角。實(shí)驗(yàn)結(jié)果表明，本文所提算法既能夠滿足軌跡跟蹤的實(shí)時(shí)性與準(zhǔn)確性要求，又具有控制精度高，響應(yīng)速度快的特點(diǎn)。

參考文獻(xiàn)

[1] 秦萬(wàn)軍，徐友春，李明喜，等.基于二自由度模型的無(wú)人駕駛車輛軌跡跟蹤控制研究[J].軍事交通學(xué)院學(xué)報(bào)，2014，16（11）：31-35.

[2] 刁勤晴，張雅妮，朱凌云.雙點(diǎn)預(yù)瞄式智能車大曲率路徑的橫向控制[J].重慶理工大學(xué)學(xué)報(bào)（自然科學(xué)），2019，33（04）：32-40.

[3] 梁政燾，趙克剛，裴鋒，等.基于MPC的智能車軌跡跟蹤算法[J].機(jī)械與電子，2019，37（01）：66-70.

[4] 伏雨旋，李孝銀，吳鵬.基于時(shí)變?nèi)斯?shì)場(chǎng)PID的智能汽車軌跡跟蹤控制[J].時(shí)代汽車，2019（07）：17-18.

[5] Norouzi Armin， Masoumi Milad， Barari Ali， et al. Lateral control of an autonomous vehicle using integrated backstepping and sliding mode controller[J]. Proceedings of the Institution of Mechanical Engineers，2019，233（1）：141-151.

[6] 郭孔輝.駕駛員—汽車閉環(huán)系統(tǒng)操縱運(yùn)動(dòng)的預(yù)瞄最優(yōu)曲率模型[J].汽車工程，1984（03）：1-16.

[7] Hongde Qin，Chengpeng Li， Yanchao Sun， et al. Finite-time trajectory tracking control of unmanned surface vessel with error constraints and input saturations[J]. Journal of the Franklin Institute，2019.

[8] Guo Zhao，Sun Jiantao，Ling Jie，et al.Robust Trajectory Tracking Control for Variable Stiffness Actuators With Model Perturbations [J].Frontiers in Neurorobotics， 2019， 13：35.

[9] Qun Cao， Zhongqi Sun， Yuanqing Xia， et al. Self-triggered MPC for trajectory tracking of unicycle-type robots with external disturbance [J]. Journal of the Franklin Institute， 2019， 356（11）： 5593-5610.

[10] Aleksandr Andreev， Olga Peregudova. Trajectory tracking control for robot manipulators using only position measurements[J]. Inter -national Journal of Control，2019，92（7）：1490-1496.

[11] Ma Tianyu，Song Zhibin， Xiang Zhongxia， et al. Trajectory Tracking Control for Flexible-Joint Robot Based on Extended Kalman Filter and PD Control[J]. Frontiers in Neurorobotics， 2019， 13：25.

[12] Hang Peng， Chen Xinbo. Integrated chassis control algorithm design for path tracking based on four-wheel steering and direct yaw-moment control[J].Proceedings of the Institution of Mechani -cal Engineers， Part I： Journal of Systems and Control Engineering， 2019， 233（6）： 625-641.

[13] Ning Wang，Xinxiang Pan，Shun-Feng Su.Finite-Time Fault-Tolerant Trajectory Tracking Control of an Autonomous Surface Vehicle[J]. Journal of the Franklin Institute，2019.

[14] Souad Bezzaoucha Reba?，Holger Voos，Mohamed Darouach. Attack- tolerant control and observer-based trajectory tracking for Cyber- Physical Systems[J]. European Journal of Control，2019，47：30-46.

[15] Zecai Lin，Xin Wang，Jian Yang. Trajectory tracking control of robotic transcranial magnetic stimulation[J]. International Journal of Intelligent Computing and Cybernetics，2019，12（2）：245-259.

[16] 張炳力，呂敏煜，程進(jìn)，等.兩點(diǎn)預(yù)瞄軌跡跟蹤橫向控制系統(tǒng)[J].電子測(cè)量與儀器學(xué)報(bào)，2019，33（05）：158-163.

[17] 秦霞，李德釗，鄧華.基于模糊PD型輸入迭代學(xué)習(xí)的工業(yè)機(jī)器人軌跡跟蹤控制[J].傳感器與微系統(tǒng)，2019，38（04）：66-68+72.

[18] 沈智鵬，張曉玲.帶擾動(dòng)補(bǔ)償?shù)囊苿?dòng)機(jī)器人軌跡跟蹤反演控制[J].控制工程，2019，26（03）：398-404.

[19] K. Chaib Draa， A. Zemouche， M. Alma，H. Voos， et al. A Nonlinear observer-based trajectory tracking method applied to an anaerobic digestion process[J]. Journal of Process Control，2019，75：120-135.