基于Frenet坐標系和控制延時補償的智能車輛路徑跟蹤

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

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

0 引言

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

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

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

1 基于Frenet坐標系的車輛運動模型

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

(1)

式中：κ為當前點的曲率。

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

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

(2)

假設車輛側向速度為0，不考慮載荷轉移，則Frenet坐標系下3自由度車輛運動學模型[15]為

(3)

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

Frenet坐標系下3自由度車輛動力學模型為

(4)

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

(5)

Cf和Cb分別為前后輪側偏剛度。

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

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

2 電控轉向系統建模

2.1 電控轉向系統

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

圖2 電控轉向控制效果示意圖Fig.2 Electronic steering control performance

假設方向盤轉角δw與前輪轉角δ為線性關系，即δw=kδ，其中k為傳動比。經過理論推導和實車測試驗證，該試驗平臺的轉向系統性能及約束如表1所示。

表1 試驗車輛轉向系統性能及約束(k=18.57)

2.2 基于實車數據的轉向延時模型

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

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

(6)

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

圖3 轉向控制延時模型Fig.3 Electric steering control delay model

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

(7)

表2 延時模型優化結果

圖4 延時模型優化辨識結果對比Fig.4 Comparison of expected, actual and predicted front wheel angles

2.3 仿真模擬

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

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

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

表3 仿真環境控制量預測效果

圖6 仿真環境延時模型預測結果Fig.6 Predicted result of delayed control model in simulation

3 基于Frenet坐標系的MPC路徑跟蹤算法

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

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

(8)

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

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

對于動力學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)

狀態量變為z=(s,d,φ,vx,vy,ω)T，控制量仍為u=δ，模型中假設ax為0. 代價函數為

(10)

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

則路徑跟蹤問題可以表述為如下優化問題：

(11)

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

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

采用前述基于Frenet坐標系的車輛運動學模型，同時考慮轉向系統控制模型，對(7)式進行離散化處理：

(12)

此時，將前輪轉角作為狀態量，期望前輪轉角作為控制量，對于運動學模型，狀態量變為z=(s,d,φ,δ)T，控制量變為u=δe；對于動力學模型，狀態量變為z=(s,d,φ,vx,vy,ω,δ)T，控制量變為u=δe. 代價函數不變，優化問題的約束增加對期望前輪轉角的約束：

|δe|≤δmax.

(13)

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

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

4 仿真實驗與分析

4.1 仿真環境搭建

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

圖8 VREP仿真環境Fig.8 VREP simulation environment

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

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

表4 仿真車輛參數

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

表5 參考路徑設計參數

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

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

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

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

表6 系統無延時情況下的跟蹤效果

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

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

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

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

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

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

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

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

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

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

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

4.5 計算耗時分析

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

圖15 計算耗時Fig.15 Computation Cost

5 結論

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

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

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