基于Frenet坐標(biāo)系和控制延時補償?shù)闹悄苘囕v路徑跟蹤

王威， 陳慧巖， 馬建昊， 劉凱， 龔建偉

(北京理工大學(xué) 機械與車輛學(xué)院, 北京 100081)

0 引言

智能車輛是智能交通系統(tǒng)的重要環(huán)節(jié)，它能夠有效減少交通事故、交通阻塞和環(huán)境污染等問題，也因此成為了近年來的研究熱點[1]。運動控制是智能車輛的核心技術(shù)之一。所謂運動控制是指根據(jù)參考軌跡輸入和控制律生成車輛執(zhí)行器(如方向盤、電子油門、制動器、換擋機構(gòu)等)的控制指令，產(chǎn)生影響車輛運動的力或力矩[2]，使車輛能夠最終收斂于參考軌跡。

智能車輛運動控制包括速度控制和轉(zhuǎn)向控制，二者存在耦合關(guān)系[3]。其中，轉(zhuǎn)向控制與路徑跟蹤問題有關(guān)，即根據(jù)參考路徑和車輛狀態(tài)生成轉(zhuǎn)向控制量，保證車輛穩(wěn)定在參考路徑上。目前常用的方法主要有純跟蹤算法、Stanley算法、前饋- 反饋控制器、線性二次型調(diào)節(jié)器和模型預(yù)測控制(MPC)等[4-6]。其中MPC控制器能夠考慮車輛系統(tǒng)模型并對未來一段時間內(nèi)系統(tǒng)的行為進行預(yù)測，通過求解帶約束的最優(yōu)控制問題，得到使預(yù)測時域內(nèi)跟蹤誤差最小的最優(yōu)控制序列[2]。同時，MPC控制器還具備較好的可擴展性，可以方便地應(yīng)用在運動控制和運動規(guī)劃中[7-8]。

由于傳感器和執(zhí)行器的數(shù)據(jù)傳輸和控制器數(shù)據(jù)處理占用時間，延時現(xiàn)象在智能車輛控制系統(tǒng)中是不可避免的。一般情況下延時的存在會引起控制器性能下降，甚至發(fā)生振蕩[9-12]，影響智能車輛的穩(wěn)定性和行駛安全性。為了解決控制延時對控制效果的影響，本文對轉(zhuǎn)向控制延時進行建模，基于實車數(shù)據(jù)完成參數(shù)辨識并仿真獲得等效的控制延時效果。之后，將轉(zhuǎn)向控制延時加入到車輛運動模型并應(yīng)用于MPC控制器，在Frenet坐標(biāo)系下實現(xiàn)并驗證算法的有效性。

1 基于Frenet坐標(biāo)系的車輛運動模型

本文基于Frenet坐標(biāo)系研究路徑跟蹤問題。對于智能車輛路徑跟蹤場景，可以忽略垂向運動，即將其簡化為平面運動，此時Frenet坐標(biāo)系指的是曲線切向量T，法向量N構(gòu)成沿基準(zhǔn)曲線移動的右手直角坐標(biāo)系。該坐標(biāo)系的特點是位置坐標(biāo)(s,d)采用沿基線移動的切向距離s(即從基線起點開始走過的弧長)和法向位移d(即相對基線法向偏差)來描述，F(xiàn)renet坐標(biāo)系中的坐標(biāo)(s,d)和笛卡爾坐標(biāo)(x,y)可以通過基線坐標(biāo)相互轉(zhuǎn)化[13]。Frenet坐標(biāo)系中曲線的切向量T和法向量N方向之間的關(guān)系如下[14]：

(1)

式中：κ為當(dāng)前點的曲率。

為了在路徑跟蹤場景中應(yīng)用Frenet坐標(biāo)系，首先需要將笛卡爾坐標(biāo)表示為弧長的向量值函數(shù)，即r(s)=x(s)i+y(s)j. 車輛在Frenet坐標(biāo)系下t時刻的坐標(biāo)可表示為

z(t)=r(s(t))+dN(s(t)).

(2)

假設(shè)車輛側(cè)向速度為0，不考慮載荷轉(zhuǎn)移，則Frenet坐標(biāo)系下3自由度車輛運動學(xué)模型[15]為

(3)

式中：v為車輛絕對速度；φ為車輛相對于投影點的航向偏差，即φ=θ-θr，θ為車輛航向角，θr為路徑航向角；κv和κr分別為車輛轉(zhuǎn)向曲率和參考點曲率，對于阿克曼轉(zhuǎn)向輪式車輛，忽略轉(zhuǎn)向不足特性，有近似式κv=tanδ/L，δ為前輪轉(zhuǎn)角，L為軸距。

Frenet坐標(biāo)系下3自由度車輛動力學(xué)模型為

(4)

式中：vx和vy分別為車輛質(zhì)心縱向和側(cè)向速度；ω為車輛橫擺角速度；ax為車輛質(zhì)心縱向加速度；m為車輛總質(zhì)量；Iz為車輛橫擺轉(zhuǎn)動慣量；lf和lb分別為質(zhì)心到前后軸距離；Fy,f和Fy,b分別為前后輪側(cè)向力，基于線性輪胎模型假設(shè)，有

(5)

Cf和Cb分別為前后輪側(cè)偏剛度。

圖1 Frenet坐標(biāo)系下的編隊行駛表述[16]Fig.1 A formation described by Frenet coordinates[16]

由上述可見，F(xiàn)renet坐標(biāo)可以直接表示車輛相對于參考路徑的位置，相比之下，笛卡爾坐標(biāo)系下表示這一相對位置需要在運動模型基礎(chǔ)上進行誤差模型推導(dǎo)。此外，F(xiàn)renet坐標(biāo)系下推導(dǎo)的車輛運動模型還可以直接用于多車編隊行駛問題中，用以表示編隊車輛相對于領(lǐng)航車的位置關(guān)系[16-17]。Frenet坐標(biāo)系下的編隊行駛表述如圖1所示。

2 電控轉(zhuǎn)向系統(tǒng)建模

2.1 電控轉(zhuǎn)向系統(tǒng)

本文所用試驗車輛為2006年款豐田陸地巡洋艦4700，其電控轉(zhuǎn)向由加裝在轉(zhuǎn)向軸的無刷直流電機實現(xiàn)。具體實現(xiàn)方式是，電機控制器接收期望方向盤轉(zhuǎn)角以及實際方向盤轉(zhuǎn)角，產(chǎn)生梯形速度曲線，如圖2所示。

圖2 電控轉(zhuǎn)向控制效果示意圖Fig.2 Electronic steering control performance

假設(shè)方向盤轉(zhuǎn)角δw與前輪轉(zhuǎn)角δ為線性關(guān)系，即δw=kδ，其中k為傳動比。經(jīng)過理論推導(dǎo)和實車測試驗證，該試驗平臺的轉(zhuǎn)向系統(tǒng)性能及約束如表1所示。

表1 試驗車輛轉(zhuǎn)向系統(tǒng)性能及約束(k=18.57)

2.2 基于實車數(shù)據(jù)的轉(zhuǎn)向延時模型

在實車測試過程中發(fā)現(xiàn)，電控轉(zhuǎn)向存在一個較大的延時，期望前輪轉(zhuǎn)角曲線和實際前輪轉(zhuǎn)角曲線時間相位相差近1 s，因此必須對延時建立模型以獲得更好的跟蹤精度。

國內(nèi)外一些學(xué)者對車輛電控系統(tǒng)的延時問題進行了研究[10-12,18]。其中：文獻(xiàn)[18]研究的問題是縱向速度控制，將加速度表述為1階慣性延時模型；文獻(xiàn)[11-12]研究的問題則是4輪獨立驅(qū)動車輛的橫擺角速度控制問題，將橫擺力矩表述為純滯后和1階慣性延時的串聯(lián)結(jié)構(gòu)模型；文獻(xiàn)[10]基于TORCS賽車仿真環(huán)境研究了智能車輛轉(zhuǎn)向控制延時問題，其核心思想是預(yù)測延時時間內(nèi)跟蹤誤差的變化，并基于預(yù)測值計算轉(zhuǎn)向控制量。本文考慮將前輪轉(zhuǎn)角表述為純滯后和1階慣性延時的串聯(lián)結(jié)構(gòu)模型(如圖3所示，其中δe(t)為t時刻車輛期望前輪轉(zhuǎn)角)，并在Simulink中搭建仿真模型，以實車測試中MPC計算得到的期望前輪轉(zhuǎn)角序列作為模型的輸入，估計實車測試過程中實際前輪轉(zhuǎn)角的響應(yīng)曲線，以實車測試數(shù)據(jù)為對比基準(zhǔn)，優(yōu)化仿真系統(tǒng)參數(shù)，解決如下優(yōu)化問題：

(6)

式中：‖·‖2表示取2范數(shù)；是指延時模型預(yù)測的實際前輪轉(zhuǎn)角序列；δr是指實車測量的實際前輪轉(zhuǎn)角序列；τ1是1階慣性時間常數(shù)；τp是純滯后時間。

圖3 轉(zhuǎn)向控制延時模型Fig.3 Electric steering control delay model

優(yōu)化結(jié)果如表2及圖4所示(其中δf為車輛預(yù)測前輪偏角)。除啟動階段(t<2.0 s，期望前輪轉(zhuǎn)角和實際前輪轉(zhuǎn)角差值超過10°)延時模型的預(yù)測效果較差之外，后續(xù)預(yù)測效果非常接近實車數(shù)據(jù)，則電控轉(zhuǎn)向系統(tǒng)的模型可以表示為

(7)

表2 延時模型優(yōu)化結(jié)果

圖4 延時模型優(yōu)化辨識結(jié)果對比Fig.4 Comparison of expected, actual and predicted front wheel angles

2.3 仿真模擬

為了能夠更方便地研究延時模型，本文在V-REP中基于Lincoln MKZ車型(詳細(xì)參數(shù)見4.1節(jié))搭建仿真車輛平臺，在仿真環(huán)境中建立等效的延時模型。系統(tǒng)框架如圖5所示。

圖5 基于V-REP和ROS的仿真環(huán)境Fig.5 Simulation environment based on V-REP and ROS

提取仿真數(shù)據(jù)中的期望前輪轉(zhuǎn)角作為上述延時模型的輸入，將輸出作為下一時刻實際前輪轉(zhuǎn)角的預(yù)測值，并與仿真測試的實際前輪轉(zhuǎn)角進行對比如表3及圖6所示。從中可以看到，仿真環(huán)境中的轉(zhuǎn)向系統(tǒng)延時同樣適用于上述模型，即在仿真中成功建立了等效的延時模型。下面將基于該仿真環(huán)境對存在延時的路徑跟蹤控制算法進行研究與測試。

表3 仿真環(huán)境控制量預(yù)測效果

圖6 仿真環(huán)境延時模型預(yù)測結(jié)果Fig.6 Predicted result of delayed control model in simulation

3 基于Frenet坐標(biāo)系的MPC路徑跟蹤算法

3.1 不考慮控制延時的MPC路徑跟蹤算法

對于運動學(xué)MPC，將(3)式作為參考模型，對其進行離散化：

(8)

式中：T為MPC控制器采樣周期。

設(shè)z=(s,d,φ)T為狀態(tài)量，u=δ為控制量，假設(shè)車速v為定值。

對于動力學(xué)MPC，將(4)式和(5)式作為參考模型，離散化得

s(k+1)=s(k)+λ(k)T，
d(k+1)=[vx(k)sinφ(k)+vy(k)cosφ(k)]T，
φ(k+1)=ω(k)-λ(k)κr(k)，
vx(k+1)=vy(k)ω(k)+ax(k)，
vy(k+1)=-vx(k)ω(k)+[Fy,f(k)+Fy,b(k)]/m，
ω(k+1)=[lfFy,f(k)-lbFy,b(k)]/Iz，

(9)

狀態(tài)量變?yōu)閦=(s,d,φ,vx,vy,ω)T，控制量仍為u=δ，模型中假設(shè)ax為0. 代價函數(shù)為

(10)

式中：wd、wφ、wu、wdu分別為橫向跟蹤誤差、航向角誤差、轉(zhuǎn)向控制量、轉(zhuǎn)向控制量變化率代價項的權(quán)重系數(shù)，本文中設(shè)置為[wd,wφ,wu,wdu]=[1,8,1,1 000]；Np為預(yù)測時域內(nèi)的離散狀態(tài)數(shù)量。

則路徑跟蹤問題可以表述為如下優(yōu)化問題：

(11)

式中：f(z,u)表示采用的車輛運動模型；δmax為前輪轉(zhuǎn)角最大值.

3.2 考慮控制延時的MPC路徑跟蹤算法

采用前述基于Frenet坐標(biāo)系的車輛運動學(xué)模型，同時考慮轉(zhuǎn)向系統(tǒng)控制模型，對(7)式進行離散化處理：

(12)

此時，將前輪轉(zhuǎn)角作為狀態(tài)量，期望前輪轉(zhuǎn)角作為控制量，對于運動學(xué)模型，狀態(tài)量變?yōu)閦=(s,d,φ,δ)T，控制量變?yōu)閡=δe；對于動力學(xué)模型，狀態(tài)量變?yōu)閦=(s,d,φ,vx,vy,ω,δ)T，控制量變?yōu)閡=δe. 代價函數(shù)不變，優(yōu)化問題的約束增加對期望前輪轉(zhuǎn)角的約束：

|δe|≤δmax.

(13)

對于1階慣性延時，在MPC參考模型中已經(jīng)考慮，但是純滯后延時意味著MPC計算得到的期望前輪轉(zhuǎn)角經(jīng)過τp時間后才可得到執(zhí)行。因此，在MPC求解之前，首先基于歷史期望前輪轉(zhuǎn)角數(shù)據(jù)對車輛狀態(tài)進行采樣預(yù)測，將預(yù)測得到的狀態(tài)作為MPC輸入的初始狀態(tài)，流程圖如圖7所示。

圖7 延時補償流程圖Fig.7 Electric steering control strategy

4 仿真實驗與分析

4.1 仿真環(huán)境搭建

本文基于ROS[19]和V-REP[20]搭建仿真驗證平臺(如圖8、圖9所示)，該平臺的優(yōu)勢在于：一方面，ROS是一個基于消息通信的分布式進程框架，擁有龐大的基礎(chǔ)庫和可視化調(diào)試工具，可快速搭建系統(tǒng)原型、快速實現(xiàn)算法開發(fā)和驗證；另一方面，V-REP擁有多種物理引擎以及用戶與場景實時交互的能力，并設(shè)有ROS通信接口，可以快速部署V-REP與ROS的聯(lián)合測試[21]。盡管V-REP存在運算資源占有率高[22]和構(gòu)建復(fù)雜動力學(xué)模型難度較大的問題，但是通過配置高性能計算機，以及基于實車試驗數(shù)據(jù)構(gòu)建車輛近似模型，V-REP可以作為算法驗證的理想環(huán)境。

圖8 VREP仿真環(huán)境Fig.8 VREP simulation environment

圖9 ROS節(jié)點及消息示意圖(橢圓代表ROS節(jié)點，矩形框代表節(jié)點發(fā)布和訂閱話題的消息內(nèi)容)Fig.9 ROS nodes and messages diagram (ellipse represents ROS node, and rectangle represents ROS message published and subscribed in a relative topic)

本文仿真環(huán)境設(shè)置和硬件配置如下：V-REP完成場景和車輛模型搭建和仿真；MPC控制器封裝為ROS節(jié)點，作為仿真車輛的路徑跟蹤控制器；全局路徑生成器用來采集和發(fā)布參考路徑；采用官方推薦的ROSInterface方案實現(xiàn)V-REP與MPC控制器之間的通信。上述MPC控制器在安裝Ubuntu 16.04 LTS系統(tǒng)的PC機上采用c++語言實現(xiàn)，PC機使用CPU型號Intel i7-7700HQ 2.80 GHz，內(nèi)存16 GB. 選擇非線性求解器Ipopt作為MPC優(yōu)化問題的求解器。仿真車輛選擇Lincoln MKZ，其參數(shù)如表4所示。

表4 仿真車輛參數(shù)

為了充分驗證上述算法在不同速度工況下對變曲率及大曲率路段的跟蹤效果，通過人工駕駛仿真車輛的方式以3種車速工況采集參考路徑，路徑形狀和基本特征統(tǒng)計如圖10及表5所示。

表5 參考路徑設(shè)計參數(shù)

圖10 全局參考路徑Fig.10 Global reference path

4.2 不考慮控制延時的MPC路徑跟蹤實驗

首先考察系統(tǒng)無延時情況下，基于Frenet坐標(biāo)系的MPC路徑跟蹤控制效果。車輛速度設(shè)置與人工采集速度相同，參考路徑A、B、C的跟蹤速度分別設(shè)置為5 m/s、10 m/s、20 m/s. 跟蹤控制效果如圖11及表6所示。從中可以看到，對于存在連續(xù)變曲率和大曲率路段的參考路徑，采用與人工駕駛相近的車速工況，本文所設(shè)計的MPC路徑跟蹤控制器取得了很好的跟蹤效果。

圖11 系統(tǒng)不存在延時情況下的跟蹤效果圖Fig.11 Tracking performances without delayed control inputs

表6 系統(tǒng)無延時情況下的跟蹤效果

4.3 考慮控制延時的MPC路徑跟蹤低速實驗

首先考察5 m/s車速對參考路徑A的跟蹤效果。在系統(tǒng)存在控制延時的情況下，對比有無控制延時模型的跟蹤控制效果，以及所用延時參數(shù)對跟蹤效果的影響，實驗結(jié)果按照平均跟蹤誤差升序，如表7所示。從中可以看到：1)在使用延時模型的情況下，跟蹤效果基本都能好過使用相同車輛模型、不考慮控制延時的MPC控制器，特別是在最大跟蹤誤差和航向偏差指標(biāo)上提升較多，但如果延時參數(shù)設(shè)置不當(dāng)會更差；2)延時參數(shù)設(shè)置為辨識結(jié)果的跟蹤效果最佳，對于運動學(xué)MPC，相比不考慮延時模型得到了較大提升，平均跟蹤誤差降低了83.7%，最大跟蹤誤差降低了74.4%. 由此驗證了在MPC路徑跟蹤控制器中應(yīng)用本文所述控制延時模型的有效性。

表7 系統(tǒng)存在延時情況下的跟蹤效果(5 m/s)

注：控制器0為無延時模型的MPC控制器；控制器1～9均為有延時模型的運動學(xué)MPC控制器；控制器10和控制器11分別為無/有延時模型的動力學(xué)MPC控制器；控制器9效果發(fā)散，無法跑完全程。

圖12對比展示控制器0、控制器1、控制器10、控制器11的跟蹤效果總覽和狀態(tài)量。

4.4 考慮控制延時的MPC路徑跟蹤高速實驗

為了進一步驗證算法在高速工況下的效果，針對參考路徑B和C繼續(xù)開展實驗。根據(jù)參考路徑A低速實驗場景的實驗結(jié)果，這里只選擇控制器0、控制器1、控制器10、控制器11進行實驗。

首先是參考路徑B，車速設(shè)置為10 m/s，在系統(tǒng)存在控制延時的情況下進行對比測試。4個控制器除了不考慮延時的運動學(xué)MPC(控制器0)在通過第一個彎道之后便振蕩至側(cè)翻之外，其余3個均安全跑完全程。從表8統(tǒng)計結(jié)果及圖13對比效果來看，考慮延時的MPC控制器在航向偏差和最大跟蹤誤差指標(biāo)上表現(xiàn)優(yōu)異，其中控制器11相比控制器10分別有約70%和18%的提升。值得注意的是，不考慮延時的動力學(xué)MPC(控制器10)在0.9 s控制響應(yīng)延時影響下仍然能夠保持較高的穩(wěn)定性和跟蹤精度，而考慮延時的動力學(xué)MPC(控制器11)相比之下在平均跟蹤誤差指標(biāo)上反而表現(xiàn)更差。分析狀態(tài)曲線推測原因為，動力學(xué)模型需要更多的車輛狀態(tài)量反饋，特別是側(cè)向速度、橫擺角速度等涉及側(cè)向運動的狀態(tài)量，相比運動學(xué)模型對噪聲更加敏感，這會引起對預(yù)測車輛狀態(tài)的更多干擾。

在車速設(shè)置為20 m/s的參考路徑C試驗中，如果完全按照實車延時效果設(shè)置系統(tǒng)仿真，會導(dǎo)致所有控制器在彎道處發(fā)生振蕩至側(cè)翻，無法跑完全程，因此設(shè)置低延時仿真實驗，響應(yīng)延時改為0.3 s，同時將純滯后延時修改為0.3 s. 此外，由于參考軌跡是人工駕駛以接近極限工況的狀態(tài)采集的，因此為了穩(wěn)定車輛在延時系統(tǒng)下的控制量，增加了前輪轉(zhuǎn)角變化量約束|Δδ|≤0.1 rad. 仍然對上述4種控制器進行對比實驗，由圖14及表9發(fā)現(xiàn)只有考慮延時的動力學(xué)MPC控制器可以安全跑完全程，其余3個控制器均在同一地點發(fā)生了側(cè)翻。這個結(jié)果進一步體現(xiàn)了動力學(xué)模型在高速工況下的優(yōu)勢，同時也進一步驗證了本文構(gòu)建的延時模型有效性。

圖14 系統(tǒng)存在延時情況下的跟蹤效果圖(20 m/s)Fig.14 Tracking performances with delayed control inputs (20 m/s)

表9 系統(tǒng)存在延時情況下的跟蹤效果(20 m/s)

4.5 計算耗時分析

最后，對本文涉及仿真實驗中4種控制器(控制器0、控制器1、控制器10、控制器11)計算耗時進行統(tǒng)計分析，如圖15所示?？刂破鞯倪\行周期設(shè)置為50 ms，在使用非線性優(yōu)化的情況下，運動學(xué)MPC(控制器0和控制器1)在運行周期內(nèi)可以完成計算得到控制量，動力學(xué)MPC(控制器10和控制器11)絕大部分情況能夠在50 ms內(nèi)運算完成。

圖15 計算耗時Fig.15 Computation Cost

5 結(jié)論

1)本文對智能車輛路徑跟蹤問題中存在的控制延時問題開展了研究。通過V-REP和ROS搭建了與實車轉(zhuǎn)向延時特性等效的仿真車輛模型，并在Frenet坐標(biāo)系下基于運動學(xué)和動力學(xué)模型構(gòu)建了不考慮和考慮控制延時兩種MPC路徑跟蹤控制器。

2)在5 m/s、10 m/s、20 m/s車速下的無延時車輛系統(tǒng)路徑跟蹤實驗中，不考慮控制延時的MPC控制器獲得了平均跟蹤誤差低于0.22 m的控制效果；而大延時車輛系統(tǒng)路徑跟蹤實驗表明，考慮控制延時的MPC控制器相比不考慮控制延時的MPC控制器取得了較大的效果提升，特別是在最大跟蹤誤差和航向誤差指標(biāo)上表現(xiàn)優(yōu)異，平均跟蹤誤差降低了83.7%，最大跟蹤誤差降低了74.4%.

3)通過分析實驗結(jié)果發(fā)現(xiàn)，對于高延時系統(tǒng)，低速工況下考慮延時的運動學(xué)MPC表現(xiàn)更好，而高速工況下動力學(xué)MPC表現(xiàn)出了更加穩(wěn)定的跟蹤性能。特別地，在20 m/s車速下的低延時實驗中，僅考慮控制延時的動力學(xué)MPC控制器安全地跑完了全程，進一步驗證了動力學(xué)模型和延時模型在高速工況下的優(yōu)勢。