無人車輛行駛路徑的分層協調最優跟蹤控制

包套圖，曾 岑

（1.渤海船舶職業學院，遼寧 葫蘆島 125000；2.大連理工大學，遼寧 大連 116024）

1 引言

汽車保有量的提高嚴重增加了交通擁堵和交通事故的概率，自動駕駛技術的提出在一定程度上解決了由于飲酒、疲勞等人為因素導致的事故［1］。車輛運動控制是自動駕駛的關鍵與末端技術，是自動駕駛的主體行為［2］。因此，研究車輛運動控制技術對促進自動駕駛技術的成熟和落地具有重要意義。

車輛跟蹤控制一般解耦為縱向控制和橫向控制問題，縱向控制用于消除縱向的速度偏差，橫向控制用于消除車輛質心與期望軌跡點的位置偏差［3］。橫向控制是車輛跟蹤控制的核心問題，分為無預瞄控制和預瞄控制兩個大方面。無預瞄控制中Stanley方法［4］最為經典，它以前軸中心與參考軌跡最短距離為參考，計算前輪轉角。但是由于輪胎側偏的存在，使得橫向控制與橫向位移存在延遲，這種延遲在車速較高時會引起控制波動和駕駛軌跡振蕩。預瞄控制分為單點預瞄和多點預瞄兩種情況［5］，單點預瞄在路徑前方選擇一個預瞄點，繪制一條當前位置與預瞄點相切的軌跡并跟蹤。文獻［6］以加權方式將無預瞄和單點預瞄控制方法結合，實現了車輛在不同工況下的路徑跟蹤。多點預瞄是指在路徑點上選擇多個參考點，通過加權將各點橫向偏差進行綜合的方法，常用的多點預瞄方法包括線性二次規劃和模型預測控制等。文獻［7］以自動清掃車為研究對象，基于多點預瞄方法對車輛運動進行控制，經實驗驗證該方法能夠滿足清掃車輛控制精度需要。上述方法能夠得到較為精確的路徑跟蹤控制結果，但是控制過程中沒有關注能耗、終點控制精度等因素，因此有必要進行進一步研究。

針對智能車輛的路徑跟蹤控制問題，建立了車輛的動力學模型和單點預瞄控制系統模型，設計了分層協調最優控制的路徑跟蹤方法，其中上層控制器基于系統狀態求出最優控制律，下層控制器實現對最優控制律的準確跟蹤，最終實現了車輛在不同工況下對路徑高精度、低能耗的跟蹤控制。

2 車輛跟蹤系統建模

2.1 汽車動力學建模

為了建立車輛的動力學模型，首先作以下4點假設：

（1）忽略車輛的俯仰和滾動運動；

（2）車輛前輪負責轉向，后輪沒有轉向能力；

（3）忽略車輛行駛過程中的空氣組合和坡道阻力；

（4）左右車輪均工作在線性區域，且特性相同。

在以上4點假設的前提下，車輛可以被簡化為具有3個自由度的單軌模型，如圖1所示。

圖1 車輛簡化模型Fig.1 Simplified Vehicle Model

圖中：OXY—道路坐標系；（Xc，Yc）—車輛質心在道路坐標系的坐標；oxyz—車輛坐標系；x軸—車輛縱向前進方向；z軸—天向；y軸—按右手準則確定的方向；vx—車輛縱向速度；vy—車輛橫向速度；lf—車輛前軸距；lr—車輛后軸距；δf—前輪轉角；φ—車輛橫擺角。根據力平衡公式和力矩平衡公式，得到車輛動力學模型為：

式中：m—車輛質量；ax—車輛縱向加速度；Cαf、Cαr—前后輪側偏剛度；Iz—車輛繞Z軸的轉動慣量。

2.2 預瞄模型距離設定

與非預瞄控制比，預瞄控制具有魯棒性好、控制精度高等優點［8］，因此這里使用單點預瞄控制方式。單點預瞄控制模型，如圖2所示。

圖2 預瞄跟蹤控制模型Fig.2 Preview Tracking Control Model

圖中：L—預瞄距離；ye—橫向距離誤差；εe—方向誤差。

預瞄距離的設定對路徑跟蹤控制效果的影響具有一定規律性：當預瞄距離較大時，路徑跟蹤系統的穩定性較好；當預瞄距離較小時，路徑跟蹤精度較高。為了兼顧系統的穩定性和跟蹤精確性，經過不斷實驗和調整，將預瞄距離設置為橫向速度的分段函數，為：

2.3 預瞄模型及線性化

根據圖2所示的單點預瞄跟蹤控制示意圖，得到單預瞄點的跟蹤模型為：

式中：ωr—車輛橫擺角速度；KL—對應于預瞄點的轉換系數。

結合式（1）和式（3），選擇路徑跟蹤控制系統的狀態量為X={φ，vx，vy，ye，εe}，控制量u={ax，δf}，可以得到系統狀態空間方程為：

由式（4）表達式可知，路徑跟蹤系統的狀態空間方程為非線性形式。在(X0(t)，u0(t))位置進行泰勒展開，得：

式中：A—系統矩陣；B—控制矩陣，且有：

3 分層協調最優控制器設計

3.1 最優控制器結構

這里的控制目標關注控制精度和能量消耗兩個方面，為了實現此控制目標，設計了分層協調最優控制器，包括上層控制器和下層控制器兩個控制器，如圖3所示。

圖3 最優控制器結構Fig.3 Structure of Optimal Controller

圖3 中虛線框內為分層協調最優控制器，上層控制器使用LQR最優控制方法，得到控制精度、能量消耗綜合最優意義下的控制量uop={ }arop，δfop。下層控制使用參數智能整定PID控制器，以剎車氣動壓力Pb、油門節氣門開度αth為控制量，實現車輛對最優控制量uop={arop，δfop}的精確跟蹤。

3.2 LQR最優控制器設計

按照LQR最優控制器設計原理，目標函數應包括終端誤差、過程累積誤差、控制量約束等方面。但是考慮到路徑跟蹤控制過程可以視為無限時間的線性二次型系統，因此目標函數設計為［9］：

式中：J—目標函數；X—狀態量；U—控制量；t0—時間起點；Q—狀態量加權矩陣；R—控制量加權矩陣。

當權值矩陣Q、R為單位對角陣時，各狀態量和控制量的重視程度一致。但是在車輛行駛過程中，當車速較小時，車輛操縱穩定性較好，此時應更加關注車輛對給定路徑的跟蹤精度；當車速較大時，車輛操縱穩定性變差，此時應降低對跟蹤精度的關注，適當降低車速而提高車輛操作穩定性和行駛安全性。基于上述考慮，將權值矩陣Q、R設置為車速v的函數，為：

3.3 參數智能整定PID控制器

對于下層控制器，使用參數智能整定PID控制器進行設計。基于LQR得到的最優控制量arop，得到車輛的縱向控制邏輯，如圖4所示。將車輛實際加速度計為ar，設計一個變量a?r=ar-arop，當a?r≤0即實際加速度小于最優加速度時，則使用發動機提供動力，進一步增大加速度；當a?r＞0即實際加速度大于最優加速度時，則使用制動器提供阻力，從而減小加速度。

圖4 縱向控制邏輯Fig.4 Vertical Control Logic

使用參數智能整定PID控制器，基于變量a?r得到發動機節氣門開度αth或剎車氣動壓力Pb。由于兩個PID控制器的設計原理完全一致，因此以參數智能整定PID控制器1為例對控制器設計原理進行介紹。參數智能整定PID控制器原理，如圖5所示。

圖5 參數智能整定PID控制器Fig.5 Parameters Intelligence Calibration PID Controller

基于PID控制的節氣門開度αth計算方法為［10］：

式中：αth(t)—t時刻的節氣門開度；kp—比例系數；ki—積分系數；kd—微分系數。

傳統PID控制器使用固定的比例系數、積分系數、微分系數，但是控制對象狀態和工況是實時變化的，固定的PID參數難以對被控對象具有實時適應性。為了解決這一問題，這里使用粒子群算法實時整定PID參數。粒子群算法原理較為簡單，這里不再介紹，僅對粒子編碼方式和優化目標函數進行明確。粒子使用十進制3維度編碼方式，即粒子編碼為(kp，ki，kd)。優化的目標函數設置為：

式中：f—目標函數；j—時刻；t—當前時刻。式（9）表示優化的目標為車輛實際加速度與最優加速度的累積誤差最小。按照上述設置方法，可以得到具有實時適應性的PID控制器參數kp，ki，kd。

4 路徑跟蹤驗證與分析

4.1 車輛參數設置

為了驗證分層協調最優控制器的路徑跟蹤控制性能，在Carsim環境中建立車輛模型，并基于Matlab搭建控制器模型，車輛參數，如表1所示。

表1 車輛參數Tab.1 Vehicle Parameters

車輛常用行駛工況為變道-直線行駛、小曲率轉彎、大曲率轉彎等工況，因此這里通過這3個工況對控制器進行驗證。

4.2 變道-直線跟蹤

給定一個直線路徑，如圖6所示。圖中實線為道路邊界線，虛線為道路中心線，該道路為同向雙車道路徑。黑色矩形表示行駛車輛，車輛行駛過程中發現前方障礙物，規劃并跟蹤避障路徑，避障路徑按照文獻［11］方法規劃得到，圖6中細實線為避障路徑，實心點為避障路徑結束點，避障結束后沿直線行駛。

圖6 變道-直線路徑Fig.6 Lane Change-Straight Path

分別使用這里分層協調最優控制器（控制器1）、LQR最優控制+傳統PID控制（控制器2）、文獻［12］的優化控制方法（控制器3）進行路徑跟蹤，統計3種控制器的最大橫向偏差，結果如下：控制器1跟蹤的最大橫向偏差為0.027m，控制器2跟蹤的最大橫向偏差為0.036m，控制器3跟蹤的最大橫向偏差為0.033m。

跟蹤誤差表明：控制器1、控制器2、控制器3在變道-直線行駛工況中，都能夠對給定路徑進行跟蹤。從跟蹤精度看，控制器1的跟蹤精度高于控制器2 和控制器3，這是因為控制器1 中的LQR最優控制綜合考慮了控制精度和控制耗能，反應在控制過程中，表現為控制精度更高、控制量幅值更小，因此控制器1的控制精度整體優于控制器2和控制器3。

4.3 小曲率轉彎

給定一個U形軌跡，如圖7所示。圖中虛線為軌道的中心線，為本節跟蹤的路徑；虛線兩側的實線為道路邊界線。

圖7 U形路徑Fig.7 U-Shaped Path

分別使用控制器1、控制器2、控制器3進行路徑跟蹤，其中控制器1和控制器2得到的最優控制律一致，3種控制器得到的控制量，如圖8所示。

圖8 不同控制方法的控制量Fig.8 Control Quantity of Different Control Method

圖8（a）中，縱坐標數值為正時表示發動機節氣門開度，數值為負時表示剎車氣動壓力。

由圖8可以看出，這里分層協調最優控制器得到的控制量在幅值上整體小于控制器3，這是因為使用LQR最優控制方法計算控制律時，平衡了控制精度和控制量能耗，因此控制律在整體幅值上小于控制器3的優化控制方法。

統計控制器1、控制器2和控制器3對給定路徑跟蹤的最大橫向偏差和最大方向偏差結果，如表2所示。

表2 最大偏差Tab.2 Maximum Error

由表2種數據可知，控制器1的最大橫向誤差和最大方向誤差最小，表示控制器1的跟蹤控制精度最高，控制器3的跟蹤控制精度次之，控制器2的跟蹤控制精度最差。這是因為，控制器1和控制器2相比，控制器2使用傳統PID控制，控制器1使用的是參數智能整定PID控制，參數智能整定PID控制能夠根據系統狀態實時調節PID參數，使PID控制器對系統狀態和工況具有實時適應性，因此控制器1比控制器2的精度高。

綜合控制器1和控制器3，控制器1的控制精度好于控制器3，且控制律幅值也整體小于控制器3，這是因為控制器1 中的LQR最優控制綜合考慮了控制精度和控制耗能，反應在控制過程中，表現為控制精度更高、控制量幅值更小，因此控制器1的控制精度和幅值整體優于控制器3。

4.4 大曲率轉彎

為了驗證分層協調最優控制器的魯棒性，設計一個具有連續轉彎、大曲率的復雜道路工況，如圖9所示。圖中實線為道路邊界線，虛線為道路中心線，為這里需要跟蹤的路徑。圖中矩形位置為路徑起點，箭頭方向為機器人前進方向。

圖9 復雜道路工況Fig.9 Complex Road Conditions

分別使用控制器1和控制器3對復雜道路進行跟蹤，其中控制器1能夠實現對給定復雜路徑的跟蹤，而控制器3在第1個大曲率轉彎時對路徑跟蹤失敗，控制器1和控制器3的復雜路徑跟蹤結果，如圖10所示。圖10（a）中道路中心虛線為待跟蹤路徑，藍色實線為控制器1的實際跟蹤結果；圖10（b）中粗實線為控制器3的跟蹤結果。

圖10 兩種控制器的跟蹤結果Fig.10 Tracking Result of the Two Controllers

由圖10可以明顯看出，控制器1實現了對連續轉彎、大曲率等復雜道路的精確跟蹤，在道路轉彎處的跟蹤誤差略大于直線位置。控制器3在第1個大曲率轉彎位置，實際跟蹤路徑就超出了道路邊界，說明無法實現對大曲率轉彎路徑的跟蹤。經統計，控制器1對復雜路徑跟蹤的最大橫向偏差為0.103m、最大方向偏差為1.987°，實驗數據和實驗結果表明，控制器1對復雜道路也具有較強的高精度跟蹤能力，即分層協調最優控制器具有較強的跟蹤能力和較好的魯棒性。

5 結論

這里研究了無人車輛行駛路徑跟蹤控制問題，建立了車輛動力學模型和單點預瞄控制系統狀態空間方程，設計了分層協調最優控制器用于路徑跟蹤。經實驗驗證得出以下結論：（1）在變道跟蹤直線路徑和U型路徑轉彎中，分層協調最優控制器、LQR最優控制+傳統PID控制、文獻［12］優化控制都能夠實現路徑跟蹤；但是分層協調最優控制器的跟蹤精度優于LQR最優控制+傳統PID控制、文獻［12］優化控制；（2）對于具有連續轉彎、大曲率的復雜道路，分層協調最優控制器實現了精度較高的路徑跟蹤控制，文獻［12］的優化控制無法跟蹤上述復雜道路工況；（3）從變道跟蹤、U形轉彎和大曲率連續轉彎等工況可以看出，分層協調最優控制器具有較強的跟蹤能力和較好的魯棒性。