連續工況下基于PID+LQR算法的自動駕駛車輛橫縱向耦合控制

余米森， 錢玉寶*， 黃華寶， 王紫涵

(1.長江大學機械工程學院， 荊州 434023； 2.長江大學新能源特車研究所， 荊州 434023)

隨著社會的發展，汽車保有量與日俱增，隨之而來的交通安全、效率等問題也日益嚴峻，自動駕駛技術成為比年來汽車工程領域的研究熱點[1-2]。車輛控制作為自動駕駛的關鍵技術之一[3]，相關研究主要包含縱向控制、橫向控制及橫縱向耦合控制[4]。

自動駕駛車輛上層控制器各功能的實現需要依托于底層控制器[5]，而底層控制主要通過對車輛的橫向和縱向來控制車輛。如何保證車輛能夠精確地沿著由決策規劃系統制定的路徑行駛，且跟蹤精度能夠達到期望值，這是自動駕駛車輛控制方面的一個難題[6]。若對車輛橫縱向運動進行單獨控制，由于運動方向之間的相互作用，縱向控制可能會帶來橫向擾動等，橫縱向運動的相互干涉會導致整車控制系統的控制精度下降，車輛主動安全性得不到保障[7]。

自動駕駛是一項典型的“智能”技術，但它非天生聰明[8]。隨著對自動駕駛車輛控制領域的進一步研究，出現了一些優化型橫縱向控制算法[9]。文獻[10]設計了一種自動駕駛車輛縱向自適應優化控制算法，而文獻[11]則提出了一種帶有前饋控制的改進型線性二次型調節(linear quadratic regulator，LQR)橫向運動控制方法。文獻[12]根據車輛與目標路徑的橫向誤差和角度偏差提出了基于速度自適應預瞄無模型轉向控制算法；文獻[13]則對大曲率且多變的混合道路工況下橫向控制系統的自適應性和控制精度不足的問題提出了一種橫向可拓預瞄切換控制方法。

文獻[14]為插電式混合動力汽車提出了模型預測控制(model predictive control，MPC)算法；而文獻[15]為實現多輛震源車的安全自動跟隨行駛，提出融合比例積分微分(proportional integral differential，PID)和MPC的控制算法；基于非線性模型預測控制算法(nonlinear model predictive control，NMPC)，文獻[16]提出一種路徑跟蹤器的設計及實現方案。隨著芯片計算能力的提高，文獻[17]采用無模型強化學習算法中的REINFORCE算法和神經網絡技術研究無人駕駛車輛變道控制策略。同時，為規避決策算法的復雜性及帶來的社會倫理問題，有研究人員開展了基于圖像的端到端深度學習模型來控制自動駕駛系統的研究[18-19]。

然而，以上文獻大都是對自動駕駛車輛的橫向運動和縱向運動進行單獨研究來實現對車輛的控制，對于車輛橫縱向耦合運動的控制策略、算法及效果的研究和驗證較少。因此，現基于PID+LQR算法來實現對自動駕駛車輛的橫縱向耦合控制，并借助聯合仿真測試平臺來驗證橫縱向耦合控制策略的效果。為自動駕駛車輛橫縱向耦合運動的控制策略及控制算法等研究提供參考。

1 模型簡化與動力學方程

依據物理系統建立的模型主要有車輛動力學模型和輪胎受力模型等。本文研究采用的是二自由度動力學模型。

1.1 汽車模型簡化

本文研究的對象是前輪轉向汽車。模型簡化過程：①忽略轉向系統的影響，直接以前輪轉角作為輸入；②忽略懸架作用，車身只做平行于地面的平面運動，即汽車沿z軸的位移，繞y軸的俯仰角以及繞x軸的側傾角均為零；③汽車的前進速度不變；④側向加速度ay<0.4g(g為重力加速度)，輪胎的側偏特性處于線性范圍；⑤忽略空氣動力學的作用；⑥忽略左右輪胎由于載荷變化而引起的輪胎特性的變化及輪胎回正力矩的作用。汽車被簡化為只有沿y軸的側向加速度和繞z軸的橫擺運動的線性二自由度兩輪汽車模型。簡化后的汽車模型如圖1所示。

1.2 車輛動力學方程

完全基于幾何關系而不考慮影響運動的力而建立的運動學模型不適用于高精度運動控制要求，所以需要研究用于車輛側向運動的動力學模型。

基于二自由度“自行車”模型，兩個自由度分別用車輛側向位移y和車輛橫擺角φ表示。忽略路面坡度，沿y軸方向應用牛頓第二定律：

may=Fyf+Fyr

(1)

式(1)中：ay為車輛在質心處沿y軸的慣性加速度。

(2)

(3)

由繞z軸的轉矩平衡可得車輛橫擺動力學方程為

(4)

當側偏角較小時，輪胎的側向力與側偏角成正比。輪胎的側偏角定義為輪胎平面方向與輪胎速度矢量之間的夾角。前輪側偏角為

αf=δ-θvf

(5)

αr=-(0-θvr)

(6)

式(5)中：θvf為車輛前輪速度矢量與車輛縱軸之間的夾角；θvr為后輪速度矢量與縱軸之間的夾角。

車輛的前輪側向力可表示為

Fyf=2Cαf(δ-θvf)

(7)

同理，后輪側向力為

Fyr=2Cαr(θvr)

(8)

式中：Cαf、Cαr分別為前、后輪側偏剛度。

(9)

(10)

(11)

(12)

式(12)中：X為狀態變量矩陣；A為狀態變量系數矩陣；B為控制變量系數矩陣；u為控制變量。因此，可以通過控制δ來實現對y和φ的控制。

2 橫縱向聯合控制

橫向控制是自動駕駛車輛控制中的難點，主要負責方向盤的控制。縱向控制主要是負責自動駕駛車輛油門和剎車的控制。

2.1 橫向控制策略

算法的輸出為

u=-Kerr+fc

(13)

控制過程主要分為4步：①向A、B計算模塊輸入整車參數和車輛縱向速度vx計算出矩陣A、B；再聯合對角矩陣Q、R，借助LQR計算模塊得出比例系數矩陣K；②根據車輛位置與狀態和軌跡規劃點求出車輛誤差矩陣err以及投影曲率k；③通過步驟①中求出的K和步驟②中求出的k以及整車參數和vx求出前饋控制量fc；④最后用K、err、αf，通過最終控制計算模塊輸出u(前輪轉角)。控制目標是選擇合適的u，使得誤差盡可能最小。流程圖如圖2所示。

圖2 橫向控制流程圖Fig.2 Lateral control flow chart

2.2 縱向控制策略

目前開發自動駕駛系統所采用的主要是純電動車輛，所以需要在CarSim中創建一個電機模型。電機模型的基本參數參考比亞迪-唐純電版，最大扭矩Tmax=385 N·m；最大功率Pmax=180 kW；電機最大轉速nmax=4 500 r/min。

自動駕駛汽車的決策通常是依據駕駛員行為規則或是采用自學習算法模仿駕駛員操作經驗來實現；有研究提出一種不依賴于大量標注駕駛數據的決策層強化學習方法[20]。本文研究中采用傳統的方法來制作油門、剎車標定表，即通過給加速踏板輸入不同的角度來得到不同的速度v和加速度a曲線，再整合速度和加速度即可得到v-a曲線。

縱向控制流程如圖3所示。

es為縱向誤差;vp為規劃車速；ap為規劃加速度；為基于Frenet坐標系的車速；thr(throttle)為節氣閥開度系數；bra(brake)為制動踏板壓力系數圖3 縱向控制流程Fig.3 Longitudinal control flow

縱向控制采用雙PID控制的策略，第一個PID為位置控制器，即處理車輛期望位置與實際位置之間的縱向誤差es；第二個PID為速度控制器，接受前PID控制器對縱向誤差處理結果的輸出和車輛當前位置速度及規劃模塊給出的速度，并將處理結果傳遞給下一個模塊。車輛油門剎車控制模塊依據制作的油門剎車標定表來控制車輛的加減速，從而實現對車輛的縱向控制。

2.3 橫縱向控制算法

2.3.1A、B計算模塊

將Cartesian(笛卡爾)坐標系變換為Frenet坐標系。自動駕駛控制算法理論起點公式為

(14)

(15)

=vycos(φ-θr)+vxsin(φ-θr)

(16)

認為φ-θr很小，根據等價無窮小可得

(17)

令ed=d,eφ=φ-θr，則可得

(18)

(19)

表達式可簡寫為

(20)

式(20)中：err為新的狀態量，表示車輛實際位置與規劃軌跡之間的橫縱向誤差；A、B分別為更新后的狀態量與控制量系數矩陣。

2.3.2 LQR模塊

為讓誤差盡可能最小，令J為代價函數。

(21)

(22)

代價函數最小，即式(22)在約束條件式(20)下最小。U為控制量u在控制序列中的數組，對角矩陣Q、R分別為誤差權重矩陣和控制量權重矩陣。

因為A、B只與整車參數和vx有關，而整車參數可近似認為不變，所以A、B只和vx有關。

K=lqr(A,B,Q,R)

(23)

經lqr數值計算可知，可得vx和K的對應表。

2.3.3err、k計算模塊

在規劃軌跡點的車輛狀態參數為

(24)

遍歷xr、yr找到與x、y最近的規劃點，并記該點的序列為dmin。

(25)

(26)

(27)

θr=θmin+kdmines

(28)

(29)

(30)

kr=kdmin

(31)

2.3.4 雙PID模塊

控制模塊中的兩個PID分別為位置PID和速度PID。比例系數Kp的作用是加快系統的響應，提高系統的調節精度；Kp越大車輛的縱向行駛軌跡就越快的逼近規劃軌跡，但Kp過大會產生超調，甚至會導致系統不穩定。積分系數Ki的作用是消除系統的穩態誤差，使縱向控制系數無限的逼近期望值；Ki越大系統的穩態誤差消除越快，但過大的Ki會在初期產生積分飽和現象，從而引起較大的超調。微分系數Kd的作用是改善系統的動態特性，抑制PI對車輛縱向控制時產生的超調。原理如圖4所示。

n(t)、u(t)、y(t)、e(t)分別為PID控制器t時刻的輸入、計算輸出、控制器最終輸出及誤差圖4 PID控制器原理Fig.4 Principle of PID control

離散PID控制算法為

(32)

式(33)中：t為采樣序號；T為采樣時間；e(t)為本次誤差；e(t-1)為上次誤差。

2.3.5 前饋控制及最終控制計算模塊

將整車參數，由LQR模塊得出的比例系數矩陣K，及誤差計算模塊得出的投影曲率k輸入到前饋控制計算模塊求得前饋控制量為

(33)

式(33)中：Kx為矩陣K的單元值。

通過最終控制模塊可求得前輪轉角為

δ=-Kerr+fc

(34)

3 基于CarSim/MATLAB橫縱向耦合控制仿真測試

3.1 仿真平臺及基本參數

CarSim-MATLAB/Simulink聯合仿真測試平臺的通信實現，采用CarSim2019.0版本，MATLAB 2020b版本。在CarSim中選擇C級倉背式乘用車，車輛的基本仿真參數如表1所示。仿真測試時間設置為35 s。

表1 車輛的基本仿真參數Table 1 Basic simulation parameters of the vehicle

3.2 聯合仿真及結果分析

仿真測試工況為變道停車連續工況。在MATLAB/Simulink中搭建車輛控制模塊，由控制模塊計算出對車輛加速踏板，制動踏板及方向盤的控制律，將控制律傳遞給CarSim中的動力學模型來實現對車輛的橫縱向耦合控制。

假設決策規劃系統的決策誤差在合理的范圍內，則可認為決策規劃系統向車輛控制系統輸出的規劃值都在期望值范圍內。決策規劃系統給出期望縱向位移xd、期望橫向位移yd、期望前輪轉角θd、期望車速vd、期望加速度ad。規劃層給出的期望車速和加速度關系如圖5所示。

基于雙PID控制算法下的車輛縱向控制，整車縱向跟蹤誤差es如圖7所示。在0～2.5 s時間段

圖5 期望車速與加速度Fig.5 Expected speed and acceleration

內，由于車輛啟動時初始速度的波動，縱向跟蹤誤差波動較大，之后誤差可穩定在±0.04 m范圍內。對規劃縱向位移xd的跟蹤效果如圖8所示。仿真測試結果表明基于雙PID控制算法的縱向控制策略具有較好的跟蹤控制精度。

基于LQR算法的車輛橫向控制，整車橫向跟蹤誤差ed如圖9所示，跟蹤誤差為(-4～4)×10-3m。

圖6 期望車速與實車速度Fig.6 Expected speed versus and actual speed

圖7 縱向跟蹤誤差Fig.7 Longitudinal tracking error

圖8 規劃縱向位移與實際縱向位移Fig.8 Planned and actual longitudinal displacements

在二自由度車輛動力學模型假設下，直接以前輪轉角作為輸入，圖10為規劃前輪轉角與仿真測試實際輸出前輪轉角關系圖，可見實際輸出前輪轉角仍存在延時。對規劃的橫向位移yd的跟蹤效果如圖11所示，跟蹤誤差可控制在毫米級范圍內，具有較高的跟蹤精度。

聯合仿真測試結果表明基于PID+LQR算法的橫縱向耦合控制策略，車輛可以較好地跟蹤決策規劃系統給出的期望軌跡，且跟蹤精度可控制在理想的范圍內。可為今后在實車測試平臺上對橫縱向耦合控制系統的有效性測試提供一定的參考價值。

圖9 橫向跟蹤誤差Fig.9 Lateral tracking error

圖10 規劃前輪轉角與實際前輪轉角Fig.10 Planning and actual front wheel angle

圖11 規劃橫向位移與實際橫向位移Fig.11 Planned and actual lateral displacements

4 結論

提出了橫縱向耦合控制策略，優化了控制算法的數學模型，最后運用聯合仿真軟件測試了控制策略對車輛的控制效果，得出如下結論。

(1)在連續工況下基于PID+LQR算法的橫縱向耦合控制策略具備可行性，PI值等縱向控制參數需要進一步優化以提高縱向跟蹤精度，橫向控制算法期待進一步簡化以提高控制的實時性。

(2)車速作為至關重要的耦合控制變量，直接影響著對車輛的橫縱向控制效果，為獲得更高的軌跡跟蹤控制精度，車速的控制算法需納入目標車輛的動力特性。

(3)為提高耦合控制系統的精度，今后對PID調參以及油門剎車等的標定問題可以基于深度學習模型，借助人工神經網絡對特征參數的訓練來得出理想的模型參數。