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

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

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

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

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

1 汽車位姿補償

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

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

2 軌跡跟蹤

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

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

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

其中T1為參考軌跡選擇時間長度。

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

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

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

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

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

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

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

由式21-式23可知：

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

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

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

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

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

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

3 仿真實驗

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

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

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

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

4 結(jié)語

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

參考文獻

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

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

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

[4] 伏雨旋，李孝銀，吳鵬.基于時變?nèi)斯輬鯬ID的智能汽車軌跡跟蹤控制[J].時代汽車，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ù)瞄最優(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] 張炳力，呂敏煜，程進，等.兩點預(yù)瞄軌跡跟蹤橫向控制系統(tǒng)[J].電子測量與儀器學(xué)報，2019，33（05）：158-163.

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

[18] 沈智鵬，張曉玲.帶擾動補償?shù)囊苿訖C器人軌跡跟蹤反演控制[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.