基于跟蹤誤差模型的智能車(chē)輛軌跡跟蹤方法*

潘世舉，李 華，蘇致遠(yuǎn)，徐友春

（陸軍軍事交通學(xué)院，天津 300161）

前言

隨著人工智能和機(jī)器人技術(shù)的不斷進(jìn)步，智能車(chē)輛技術(shù)迅速發(fā)展。其關(guān)鍵技術(shù)主要包括精確定位技術(shù)、環(huán)境感知技術(shù)、規(guī)劃決策和控制技術(shù)等［1-3］。軌跡跟蹤技術(shù)［4-7］主要是控制車(chē)輛的橫縱向運(yùn)動(dòng)，使車(chē)輛沿著參考軌跡行駛，其精確程度直接決定了智能車(chē)輛的行為表現(xiàn)。

近年來(lái)許多學(xué)者針對(duì)參考軌跡跟蹤問(wèn)題進(jìn)行了研究。文獻(xiàn)［8］中通過(guò)車(chē)載傳感器實(shí)時(shí)獲取行駛軌跡相對(duì)于最優(yōu)路徑的偏差信息，采用偏差融合技術(shù)的PID控制方法（proportion integration differentiation control），實(shí)現(xiàn)鏟運(yùn)車(chē)的無(wú)人駕駛控制。文獻(xiàn)［9］中采用改進(jìn)的有限時(shí)間最優(yōu)預(yù)瞄橫向控制算法，其車(chē)輛相對(duì)于軌跡的橫向偏差在±0.3 m內(nèi)，但預(yù)瞄時(shí)間的選取較為繁瑣。文獻(xiàn)［10］中采用基于模糊控制和自校正模型控制方法，實(shí)現(xiàn)插秧機(jī)以1 m/s行駛時(shí)，平均橫向偏差為8.7 cm。文獻(xiàn)［11］中基于迭代學(xué)習(xí)控制理論設(shè)計(jì)迭代學(xué)習(xí)控制算法，該算法實(shí)現(xiàn)了車(chē)輛期望路徑有限區(qū)間內(nèi)的高精度完全跟蹤控制，但僅適用于參考軌跡重復(fù)固定的情況。

隨著現(xiàn)代控制理論的發(fā)展，模型預(yù)測(cè)控制理論引起了人們的關(guān)注。文獻(xiàn)［12］中以車(chē)輛非線性運(yùn)動(dòng)學(xué)模型為基礎(chǔ)設(shè)計(jì)控制器，以低于1 m/s的速度跟蹤圓形路徑時(shí)，橫向最大誤差小于0.1 m。文獻(xiàn)［13］中采用車(chē)輛動(dòng)力學(xué)模型設(shè)計(jì)控制器，以15 km/h的速度跟蹤軌跡時(shí)，可將誤差控制在0.2 m范圍內(nèi)。

為進(jìn)一步提高智能車(chē)輛軌跡跟蹤精度，本文中采用基于車(chē)輛動(dòng)力學(xué)的跟蹤誤差模型設(shè)計(jì)模型預(yù)測(cè)控制器，在Carsim與MATLAB/Simulink構(gòu)建的聯(lián)合仿真平臺(tái)中進(jìn)行仿真實(shí)驗(yàn)，并進(jìn)行了實(shí)車(chē)驗(yàn)證，以15 m/s的速度跟蹤軌跡時(shí)，橫向最大誤差在0.52 m范圍內(nèi)，跟蹤效果較為理想。

1 車(chē)輛動(dòng)力學(xué)模型

采用車(chē)輛四輪模型分析車(chē)輛在運(yùn)動(dòng)過(guò)程中的受力情況，為簡(jiǎn)化計(jì)算，假設(shè)車(chē)輛在水平路面上行駛，車(chē)輛縱向速度恒定，不考慮橫向和縱向空氣阻力，前輪轉(zhuǎn)角較小。滿(mǎn)足以上條件的車(chē)輛動(dòng)力學(xué)模型如圖1所示，其中，xoy為車(chē)體坐標(biāo)系，坐標(biāo)原點(diǎn)o為車(chē)輛質(zhì)心所在點(diǎn)，x軸正方向?yàn)檐?chē)頭方向，y軸正方向?yàn)榇怪避?chē)體方向向左，z軸正方向?yàn)榇怪眡oy面向上，XOY為慣性坐標(biāo)系。

圖1 車(chē)輛模型

考慮車(chē)輛側(cè)向受力和橫擺運(yùn)動(dòng)，建立如下動(dòng)力學(xué)方程：

式中：m為車(chē)輛的質(zhì)量；φ為車(chē)輛在慣性坐標(biāo)系下的航向；Fyf，F(xiàn)yr分別為車(chē)輛前、后輪受到的側(cè)向力；Iz為車(chē)輛繞z軸的轉(zhuǎn)動(dòng)慣量；lf，lr分別為車(chē)輛前、后軸中心到質(zhì)心的距離。

在小角度假設(shè)下，前、后輪受到的側(cè)向力為

式中：Cf，Cr分別為前、后輪側(cè)偏剛度；δ為前輪轉(zhuǎn)角。聯(lián)合式（1）～式（4），可得

智能車(chē)輛的軌跡跟蹤問(wèn)題是指在慣性坐標(biāo)系中，設(shè)計(jì)軌跡跟蹤控制器，確定車(chē)輛控制輸入，使車(chē)輛在規(guī)定的時(shí)刻到達(dá)預(yù)設(shè)的軌跡點(diǎn)，并具備預(yù)設(shè)的狀態(tài)，包括位置、航向、速度、加速度等。因此，還應(yīng)考慮車(chē)輛的縱向控制。本文中采用如下車(chē)輛縱向運(yùn)動(dòng)學(xué)方程：

式中：a為車(chē)輛的加速度；ar為參考點(diǎn)的加速度；s為車(chē)輛相對(duì)于軌跡起點(diǎn)的位置；sr為參考點(diǎn)相對(duì)于軌跡起點(diǎn)的位置。

定義車(chē)輛在參考軌跡跟蹤過(guò)程中產(chǎn)生的跟蹤誤差：e1為車(chē)輛質(zhì)心與參考點(diǎn)的橫向誤差，e2為車(chē)輛與參考點(diǎn)的航向誤差［14］，e3為車(chē)輛與參考點(diǎn)的距離誤差，則

綜合式（5）～式（10），可得到基于參考軌跡的車(chē)輛模型為

可以看出，該系統(tǒng)為非線性系統(tǒng)，為提高模型預(yù)測(cè)控制計(jì)算速度，還需進(jìn)行線性化和離散化處理。

2 線性時(shí)變誤差模型

參考軌跡上的任意點(diǎn)均滿(mǎn)足式（11），即

其中，由于參考點(diǎn)處于參考軌跡上，則參考狀態(tài)量 ξr=［0 0 0 0 0 0］T。將式（12）在該點(diǎn)處進(jìn)行一級(jí)泰勒展開(kāi)，忽略高級(jí)項(xiàng)，得

式中：fξ，r為 f相對(duì)于 ξ的雅克比矩陣，fu，r為 f相對(duì)于u的雅克比矩陣，fw，r為f相對(duì)于w的雅克比矩陣。

將式（13）與式（12）相減得

假設(shè)預(yù)測(cè)時(shí)域?yàn)镹p，系統(tǒng)采樣時(shí)間為k=0，…，Np，系統(tǒng)采樣周期為T(mén)。車(chē)輛的底層控制器會(huì)在一個(gè)控制周期T內(nèi)保持控制量的恒定，因此采用零階保持進(jìn)行離散的方法能夠得到比較精準(zhǔn)的符合期望的控制輸入量。設(shè)計(jì)增廣向量z=［ξ u w］T，則式（14）可表示為

對(duì)Gz進(jìn)行矩陣指數(shù)運(yùn)算，采取零階保持可得

式中第一行即為車(chē)輛的線性時(shí)變誤差模型：

3 軌跡跟蹤控制器設(shè)計(jì)

3.1 預(yù)測(cè)模型

定義預(yù)測(cè)時(shí)域Np內(nèi)的車(chē)輛的預(yù)測(cè)輸出矩陣、控制量矩陣和附加變量矩陣為

根據(jù)式（17），可推導(dǎo)出系統(tǒng)的預(yù)測(cè)方程為

式中：系統(tǒng)狀態(tài)量預(yù)測(cè)參數(shù)為

控制量矩陣預(yù)測(cè)參數(shù)為

附加變量預(yù)測(cè)參數(shù)為

3.2 滾動(dòng)優(yōu)化

滾動(dòng)優(yōu)化是對(duì)系統(tǒng)狀態(tài)量和控制量的優(yōu)化，使得車(chē)輛能夠快速平穩(wěn)地實(shí)現(xiàn)軌跡跟蹤，通過(guò)目標(biāo)函數(shù)的最優(yōu)來(lái)實(shí)現(xiàn)。式（18）中，u～表示的系統(tǒng)控制量未知，可通過(guò)設(shè)定目標(biāo)函數(shù)進(jìn)行求解。采用控制增量作為優(yōu)化目標(biāo)函數(shù)的狀態(tài)量，同時(shí)引入松弛因子，保證系統(tǒng)存在可行解。目標(biāo)函數(shù)表達(dá)式為

式中：Q為預(yù)測(cè)時(shí)域權(quán)重矩陣；R為控制時(shí)域權(quán)重矩陣；ρ為權(quán)值系數(shù)；ε為松弛因子。目標(biāo)函數(shù)第1項(xiàng)表示對(duì)參考軌跡的快速跟蹤能力，第2項(xiàng)表示對(duì)控制量變化的優(yōu)化能力。將式（18）代入式（19），轉(zhuǎn)化為二次規(guī)劃問(wèn)題，并考慮系統(tǒng)狀態(tài)量、控制量和控制增量的約束條件，整理得

式中：Hk為正定Hessian矩陣；Gk為控制量系數(shù)矩陣；Pk為常數(shù)；E（k）為預(yù)測(cè)時(shí)域內(nèi)系統(tǒng)跟蹤誤差；ξmin和 ξmax為狀態(tài)量約束最值；umin和 umax為控制量約束最值；Δumin和Δumax為控制增量約束最值。

3.3 反饋校正

在采樣時(shí)刻k，采用有效集方法進(jìn)行求解，求取性能指標(biāo) J（ξ～（k），u～（k））的最小值，獲得最優(yōu)的控制序列｛u～（k|k），…，u～（k+Np-1|k）｝，取第 1項(xiàng)作為控制增量，計(jì)算控制輸入量：

在（k+1）時(shí)刻，重新計(jì)算最優(yōu)控制序列并將第1項(xiàng)控制增量作用于系統(tǒng)，從而實(shí)現(xiàn)最優(yōu)控制。

4 實(shí)驗(yàn)結(jié)果與分析

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

4.1.1 仿真平臺(tái)介紹

該仿真平臺(tái)由CarSim提供車(chē)輛動(dòng)力學(xué)模塊，通過(guò)MATLAB語(yǔ)言編寫(xiě) S-Function作為控制器，在Simulink中進(jìn)行整體結(jié)構(gòu)的搭建。

本文中CarSim車(chē)輛動(dòng)力學(xué)模型整車(chē)質(zhì)量為1 100 kg，質(zhì)心與前、后軸中心的距離分別為1 040和1 560 mm，質(zhì)心高度為540 mm，輪距為1 695 mm，路面附著系數(shù)為0.8，滾動(dòng)阻力系數(shù)為0.8。

4.1.2 參考軌跡生成

在軌跡跟蹤驗(yàn)證中通常使用雙移線進(jìn)行測(cè)試，但標(biāo)準(zhǔn)雙移線工況彎道不足以平滑，不易在仿真中應(yīng)用。本文中借鑒文獻(xiàn)［15］中的雙移線工況對(duì)所建立的控制器進(jìn)行驗(yàn)證，函數(shù)表達(dá)為

以10 m/s的速度跟蹤參考軌跡為例，進(jìn)行控制器參數(shù)調(diào)試。隨著預(yù)測(cè)時(shí)域的增大，橫向跟蹤誤差減小，主要是因?yàn)轭A(yù)測(cè)時(shí)域較大時(shí)，控制器能夠更好地預(yù)測(cè)系統(tǒng)未來(lái)輸出，根據(jù)當(dāng)前誤差及時(shí)修正系統(tǒng)輸入信號(hào)，但同時(shí)會(huì)增加計(jì)算量；控制時(shí)域的變化對(duì)控制器跟蹤效果影響較小，主要是因?yàn)榉答佇Ｕ刂破鲀H將控制增量的第1個(gè)元素作用于系統(tǒng)；隨著控制周期變長(zhǎng)，橫向跟蹤誤差逐漸減小。仿真控制器參數(shù)設(shè)置如表1所示。

4.1.3 軌跡跟蹤實(shí)驗(yàn)

在慣性坐標(biāo)系下，車(chē)輛初始狀態(tài)為：x=0，y=0，v=0，φ=0，δ=0。采用非線性MPC控制器和本文中設(shè)計(jì)的線性MPC控制器，以10 m/s的速度進(jìn)行軌跡跟蹤，仿真跟蹤結(jié)果如圖2所示。線性MPC控制器的橫向最大誤差為0.517 m，航向最大誤差為0.053 rad；非線性MPC控制器的橫向最大誤差為1.496 m，航向最大誤差為0.124 rad。保持車(chē)輛初始狀態(tài)不變，采用本文中設(shè)計(jì)的MPC控制器，分別以5，10和15 m/s的速度進(jìn)行軌跡跟蹤，仿真跟蹤結(jié)果如圖3和圖4所示，其中橫向最大誤差分別為0.452，0.517和0.292 m，航向最大誤差分別為0.059，0.053和0.067 rad，側(cè)向最大加速度分別為 0.619，2.585和6.298 m·s-2。

表1 仿真控制器參數(shù)

4.2 實(shí)車(chē)實(shí)驗(yàn)

4.2.1 實(shí)驗(yàn)平臺(tái)介紹

圖2 線性MPC與普通MPC跟蹤效果對(duì)比

圖3 軌跡跟蹤與橫向誤差

圖4 航向誤差與側(cè)向加速度

實(shí)車(chē)實(shí)驗(yàn)平臺(tái)為 JJUV8，該平臺(tái)改裝于長(zhǎng)城Wey。硬件架構(gòu)分為底層、交換層、感知層和計(jì)算層。底層主要包含執(zhí)行機(jī)構(gòu)驅(qū)動(dòng)電路、車(chē)輛信號(hào)控制電路和中控管理電路；交換層主要包含交換機(jī)；感知層包括64線激光雷達(dá)、16線激光雷達(dá)、組合慣導(dǎo)、攝像頭和毫米波雷達(dá)；計(jì)算層部分包含兩臺(tái)工控機(jī)。軟件架構(gòu)包括感知、預(yù)測(cè)、決策規(guī)劃和控制4部分，各部分并行計(jì)算，利用機(jī)器人操作系統(tǒng)的消息發(fā)布和訂閱機(jī)制進(jìn)行數(shù)據(jù)交互。

決策規(guī)劃部分根據(jù)感知、預(yù)測(cè)部分發(fā)送的環(huán)境信息和車(chē)輛自身狀態(tài)等，實(shí)時(shí)規(guī)劃可行駛軌跡，周期為0.1 s；控制部分根據(jù)車(chē)輛自身狀態(tài)與參考軌跡的誤差計(jì)算控制量，周期為0.02 s。初始時(shí)，車(chē)輛質(zhì)心位置與參考軌跡起點(diǎn)重合，航向與參考軌跡初始方向一致。實(shí)驗(yàn)平臺(tái)和顯示界面如圖5所示，其中方框?yàn)檐?chē)輛位置，曲線為參考軌跡。JJUV8實(shí)驗(yàn)平臺(tái)參數(shù)如表2所示。

表2 JJUV8實(shí)驗(yàn)平臺(tái)參數(shù)

圖5 實(shí)驗(yàn)平臺(tái)和軌跡顯示界面

4.2.2 軌跡跟蹤實(shí)驗(yàn)

軌跡跟蹤實(shí)驗(yàn)中，為保證軌跡跟蹤實(shí)車(chē)實(shí)驗(yàn)的可操作性和安全性，在天津空港經(jīng)濟(jì)區(qū)選取一條道路空曠、車(chē)輛和行人較少且路面情況滿(mǎn)足要求的道路進(jìn)行實(shí)驗(yàn)，如圖5所示。車(chē)輛以5和10 m/s的速度分別進(jìn)行3次實(shí)驗(yàn)取其平均值，實(shí)車(chē)實(shí)驗(yàn)跟蹤結(jié)果如圖 6所示，橫向最大誤差分別為 0.61和0.45 m，航向最大誤差分別為0.05和0.06 rad，側(cè)向最大加速度分別為0.54和3.72 m·s-2。

4.3 實(shí)驗(yàn)分析

（1）本文中設(shè)計(jì)的線性 MPC控制器與普通MPC控制器的仿真實(shí)驗(yàn)結(jié)果相比，橫向最大誤差減少0.98 m，航向最大誤差減少0.071 rad。主要原因是在求解最優(yōu)解方程的過(guò)程中，方程包含了多個(gè)最優(yōu)變量和非線性狀態(tài)約束，以及由控制量約束和狀態(tài)量約束組成的線性約束，對(duì)于非線性MPC，其求解的復(fù)雜程度隨系統(tǒng)狀態(tài)方程階數(shù)的增加而迅速增加，運(yùn)算時(shí)間增加，很難保證實(shí)時(shí)控制，從而導(dǎo)致跟蹤誤差增大。

圖6 實(shí)車(chē)軌跡跟蹤結(jié)果

（2）軌跡跟蹤仿真實(shí)驗(yàn)結(jié)果中，對(duì)于5和10 m/s跟蹤速度，橫向最大誤差隨速度增大而增大，航向最大誤差隨速度增大而減小，主要原因是轉(zhuǎn)彎時(shí)車(chē)輛質(zhì)心變化導(dǎo)致繞z軸的轉(zhuǎn)動(dòng)慣量發(fā)生變化、前輪轉(zhuǎn)角較大導(dǎo)致小角度假設(shè)不成立，控制器預(yù)測(cè)的車(chē)輛運(yùn)動(dòng)與實(shí)際情況差別增大。15 m/s跟蹤速度下橫向最大誤差較小，航向最大誤差較大，主要原因是轉(zhuǎn)彎處的側(cè)向最大加速度大于0.4g，輪胎側(cè)偏特性處于非線性范圍、左右車(chē)輪載荷變化導(dǎo)致的特性變化等。

（3）軌跡跟蹤實(shí)車(chē)實(shí)驗(yàn)結(jié)果與仿真實(shí)驗(yàn)相比，5 m/s跟蹤速度下的橫向最大誤差增加0.15 m，而航向最大誤差減小0.01 rad，主要原因是傳感器、執(zhí)行器和路面不平坦等不確定因素對(duì)系統(tǒng)的干擾。10 m/s跟蹤速度下的橫向最大誤差減少0.07 m，航向最大誤差增加0.08 rad，主要原因是在仿真環(huán)境中路面附著系數(shù)和滾動(dòng)阻力系數(shù)均為0.8，而實(shí)車(chē)實(shí)驗(yàn)道路環(huán)境相對(duì)較差。

5 結(jié)論

本文中以車(chē)輛動(dòng)力學(xué)模型為基礎(chǔ)，提出基于線性時(shí)變模型預(yù)測(cè)控制的軌跡跟蹤方法，設(shè)計(jì)控制器并進(jìn)行仿真實(shí)驗(yàn)與實(shí)車(chē)實(shí)驗(yàn)。仿真實(shí)驗(yàn)結(jié)果表明，該控制器與普通非線性MPC控制器相比，能夠滿(mǎn)足實(shí)時(shí)控制的要求，且跟蹤效果更優(yōu)。在雙移線軌跡跟蹤過(guò)程中，5 m/s時(shí)橫向最大誤差為0.452 m，10 m/s橫向最大誤差為0.517 m。

利用JJUV8平臺(tái)進(jìn)行實(shí)車(chē)驗(yàn)證，車(chē)輛以5 m/s速度跟蹤軌跡時(shí)，與仿真實(shí)驗(yàn)相比，橫向最大誤差增加0.15 m，航向最大誤差減小0.01 rad，基本滿(mǎn)足軌跡跟蹤的要求。車(chē)輛以10 m/s速度跟蹤軌跡時(shí)，橫向最大誤差為0.45 m，航向最大誤差為0.06 rad。本文中所設(shè)計(jì)的控制器效果較為理想，能夠滿(mǎn)足智能車(chē)輛的軌跡跟蹤要求。

下一步工作將繼續(xù)研究車(chē)輛在縱向速度變化、載荷轉(zhuǎn)移情況下的動(dòng)力學(xué)模型，分析低附著系數(shù)和低滾動(dòng)摩擦因數(shù)環(huán)境下的控制方法，兼顧車(chē)輛運(yùn)動(dòng)穩(wěn)定性，并展開(kāi)實(shí)車(chē)實(shí)驗(yàn)。