低速自動駕駛橫向跟蹤控制研究

唐 坤,曹志雄

(北京天隼圖像技術有限公司,北京 100089)

0 引 言

低速自動駕駛車輛與普通高速自動駕駛車輛相比，存在不同的技術特點：(1)低速自動駕駛車輛的行駛速度遠小于高速車輛，因此在障礙物的檢測距離、控制周期等方面均有較大不同；(2)低速車輛行駛空間相對較小、車道標識不夠明確、障礙物對象較為復雜，因此，其路徑跟蹤控制的精準度要求較高；(3)低速車輛的線控底盤多為自主研發，給控制增添了許多不確定因素[1]。

針對車輛的橫向運動控制，國內外研究機構進行了各種不同的研究。日產公司采用經典的PID控制構建橫向前饋-反饋控制器解決橫向控制實時性的問題[2]；Eom等[3]提出基于三自由度車輛模型的魯棒H∞橫向控制算法；Netto等[4]提出了基于視覺的智能車輛橫向自適應控制器；Falcone等[5]針對智能車輛主動轉向控制問題，提出了橫向模型預測控制算法；另外，模糊控制、滑模控制、神經網絡控制、迭代學習與深度學習等也在車輛橫向運動控制研究中有廣泛應用[6]。

結合低速自動駕駛車輛行駛環境和自身結構的特點，以及橫向控制的特點，本文設計了一種純跟蹤(PP)控制與模型預測控制(MPC)聯合的控制器，以此滿足橫向跟蹤控制的快速性、穩定性以及精確度。

1 系統模型的建立

1.1 試驗樣車介紹

本文的車輛仿真模型建立以及實車測試均以北京天隼圖像技術有限公司(簡稱“天隼”)自主研發的FELIS ACH201小型智能化線控底盤為核心，如圖1所示，該底盤架構與乘用車保持一致，采用CAN總線通信，由整車控制器對轉向系、驅動系以及制動系進行指令解析和轉發，實現整車控制總線化。

圖1 FELIS AHC201線控底盤

1.2 轉向控制系統模型

FELIS AHC201型線控底盤的轉向控制系統是一個由轉向電機及其底層控制器、霍爾元件、編碼器等模塊構成的三閉環控制系統，即位置環、速度環以及電流環，其結構如圖2所示。上層控制器輸出的轉向電機期望位置作為底層控制器輸入，霍爾元件及編碼器實時檢測轉向電機的位置、速度以及電流，底層控制器根據期望值和實時反饋值調節轉向電機的輸出位置，實現對轉向電機期望位置的實時動態跟蹤。

圖2 轉向控制系統結構

底層控制器3個閉環均采用增量PID控制，并對轉向位置、轉向速度以及電機電流進行有效安全約束。通過對控制參數的整定，該底層控制器具有良好的跟蹤效果，如圖3所示，最大期望位置為8 000°，對應車輪轉向角為30°，轉向所需時間為1.5 s，此過程最大延時時間為15 ms，最大穩態偏差為15°，誤差率為0.18%，滿足設計要求。同時設置安全約束條件：位置極限值為±9 130°，轉速極限為±1 400°/s，電流極限為±30 A。

圖3 位置跟蹤測試結果

1.3 車輛動力學模型

因為本文主要側重研究車輛對目標路徑的跟蹤能力，所以將車輛模型簡化為二自由度單車模型，其幾何關系如圖4所示。根據牛頓力學關系以及輪胎的側偏力與側偏角的線性關系，車輛的橫向動力學模型以及車身坐標與大地坐標系的轉換關系為

圖4 車輛單車模型

(1)

式中：m為整車質量；vx為車輛的縱向速度；vy為車輛的橫向速度；ψ為質點橫擺角；δf為車輛的前輪轉向角；lf、lr為質心到前后軸的距離；Cf、Cr為前后輪胎的側偏剛度；Iz為車輛繞z軸的轉動慣量；X、Y為車輛在大地坐標系OXY下車輛參考點位置。

1.4 車輛運動學模型

由于低速自動駕駛車輛的工作車速較低，其運動學特性比動力學特性更為明顯,且動力學特性較穩定。故基于車輛運動學模型設計的路徑跟蹤控制器具備可靠的控制性能。在此同樣選用圖4所示單車模型建立車輛的運動學模型[7]。以車輛后軸中心(Xr，Yr)為參考點，在低速工況下，質心側偏角β極小，可假設為0°，車速v近似等于車輛的縱向速度vx，后輪轉向角δr恒為0°，l為軸距。則在大地坐標系OXY下，車輛運動學方程[8]：

(2)

2 控制器設計

2.1 控制器整體結構

針對橫向運動控制研究的展開前提是車輛的初始位置處于目標路徑上，或處于目標路徑的附近。但在實際工況中，需要考慮車輛與目標路徑的初始位置存在較大跟蹤偏差的情況。

MPC作為一種具有多種模式約束的實時在線控制算法，能夠實現較高的控制精度。但當車輛與目標路徑跟蹤偏差較大時，其調節時間較長且波動性較大，尤其在低速自動駕駛控制中存在失控的可能。而PP控制能夠實現對目標路徑快速跟蹤，但在跟蹤精度要求較高的工況下，尤其曲率較大的路段，其控制精度明顯弱于MPC。因此，通過對上述控制算法特點的總結，并結合低速自動駕駛車輛工作環境和自身結構的特點，設計了基于MPC與PP控制相結合的聯合控制器，其整體結構如圖5所示。MPC模式以車輛運動學模型作為預測模型，根據目標路徑與車輛當前狀態信息，計算輸出前輪的期望轉向角δ1；PP控制模式根據目標路徑、車輛當前位置信息以及由當前車速確定的預瞄距離，計算輸出前輪的期望轉向角δ2；模式選擇器根據目標路徑與車輛當前位姿信息(位置與方向)，選擇相應的控制模式；穩定控制器根據模式標志mod判斷是否發生了模式切換，若發生模式的切換，為了防止控制器的輸出產生較大的躍變，造成系統產生劇烈振蕩，起動模糊控制器，對2種控制模式輸出的前輪期望轉向角進行加權計算，約束期望轉向角的輸出幅值，直至切換結束，從而實現平滑切換；根據阿克曼轉向總成的逆模型推導前輪轉向角與轉向電機位置之間的幾何關系，確定轉向電機的期望位置。

圖5 控制器整體結構框

2.2 基于MPC算法的橫向控制

針對低速自動駕駛車輛的行駛特點，本文選用車輛運動學模型作為預測模型，但是非線性的模型預測控制計算量龐大，求解速度相對緩慢，難以滿足系統對于實時性的要求，因此需要進行線性化處理，以保證控制的實時性[9-10]。

2.2.1 線性時變模型

由式(2)可知，車輛的非線性運動學模型的一般狀態方程可寫成：

(3)

式中：狀態量ξ=[XrYrψ]T，控制量u=[vδr]。

可以將目標路徑看作一組已知的車輛狀態信息，進而能夠應用運動學方程進行描述，將式(3)在目標路徑上的某一點(ξ0，u0)處采用泰勒級數表示并忽略高階項，結合該點處的狀態方程，即可求得線性化的誤差模型，即：

(4)

應用一階差商方法對式(4)進行離散化處理，即：

(5)

式中：Ak,t=I+TA(t);Bk,t=TB(t);T為采樣周期；I為單位矩陣。

2.2.2 預測模型

通過上述離散線性誤差模型，能夠計算車輛未來時刻的狀態量與輸出量。為了便于對控制增量進行計算，將式(5)進行如下轉換，令：

(6)

得到一個新的狀態空間方程：

(7)

通過式(7)可知，若是已知系統當前時刻的狀態量以及未來一段時域內非零的控制增量，進行迭代計算，能夠預測未來這段時域內的狀態量和輸出量。

2.2.3 優化求解

在實際控制中，系統的控制增量是未知的，可以通過設置目標函數，并對其進行滾動尋優，來獲得一段控制時域內的控制增量的序列。本文中目標函數設置為

(8)

式中：ηref為未來第i時刻的目標輸出；Np為預測時域；Nc為控制時域；R、Q為權重矩陣；ρ為權重因子；ε為松弛因子。

式(8)中：第一項反映了控制系統對目標路徑的跟蹤能力；第二項反映了前輪轉向過程的平穩性；第三項松弛因子的加入能夠保證在未來預測時域內的每一刻均有可行解。因此，在設定預測時域內，該目標函數能夠實現對目標路徑的精確、平穩跟蹤。

為了方便求解，需要將目標函數轉化為標準二次規劃的問題，將式(7)代入式(8)中，可得：

(9)

在實際控制過程中，為了增強跟蹤過程準確性、穩定性，往往需要考慮對控制量以及控制增量設置約束條件。在本文設計中，在控制周期設置為0.05 s的前提下，根據AHC201的縱向跟蹤能力測試，設定車速控制的約束條件為

車速偏差：-0.05 m/s

車速增量：-0.025 m/s<Δv<0.025 m/s。

根據AHC201的轉向能力測試，設定前輪轉向控制的約束條件為

(10)

式中：vt為期望車速；Δ為每個控制周期的增量。

綜上所述，模型預測的求解問題可以描述為在滿足約束條件的前提下，求解每一個采樣周期內使式(9)最小的最優控制增量序列：

將該序列的第一個元素用于橫向跟蹤系統的實際控制，進而求得期望控制量，即：

(11)

而后進入下一采樣周期，求取新的最優控制增量序列，最終實現對目標路徑的跟蹤控制。

2.3 基于PP算法的橫向控制

2.3.1 基于單車模型的PP算法原理

PP控制廣泛應用于車輛的路徑跟蹤控制中。文中PP控制基于單車模型，以車輛后軸為切點，車輛縱向車身為切線，通過控制前輪轉向角，控制車輛沿著一條經過預瞄點的圓弧行駛，如圖6所示[11-12]。

圖6 PP算法原理圖

圖6中,點(gx，gy)為目標路徑上的預瞄點，(cx，cy)為車輛的當前位置，δ為前輪的轉向角，α為預瞄角，ld為預瞄距離，L為軸距，R為車輛的轉向半徑。根據正弦定理，可得：

(12)

根據簡化的阿克曼轉向幾何，前輪轉向角與車輛轉彎半徑存在如下關系：

(13)

聯立式(12)與式(13)，可得基于PP控制的前輪期望轉向角為

(14)

2.3.2 預瞄距離的確定

預瞄距離的確定是PP控制算法能否實現路徑準確跟蹤的關鍵，如果預瞄距離過短，易造成系統產生持續的振蕩，系統的穩定性下降；如果預瞄距離過長，則跟蹤精度下降，尤其在跟蹤曲率較大處時，容易產生“抄近路”現象。經分析，預瞄距離與車輛實時的速度、加速度、初始預瞄距離以及轉彎半徑等參數存在一定的數學關系，通常用于計算預瞄距離的經驗公式為[11]

(15)

式中：v為車輛的行駛速度；amax為車輛最大制動加速度；k為速度系數，Rmin為車輛最小的行駛半徑。

式(15)中，第1項表示車輛制動距離；第2項表示車輛的反應行駛距離；AHC201的設計參數確定，其最大制動加速度為0.5 m/s2，k為0.5，Rmin為1.8 m。當車速處于3～8 km/h時，系統對于預瞄距離的變化反應較為明顯，因此，本文設定預瞄距離數學關系式為

(16)

2.3.3 預瞄點的確定

預瞄距離確定后，根據預瞄距離尋找目標路徑上的預瞄點成為整個算法實現的重要環節。本文采用弦長累加的方法確定目標點。具體預瞄點的確定方法如圖7所示。

圖7 預瞄點的確認方法

尋找目標路徑上距離車輛當前位置最近的點B0，然后將B0作為起點，順序求取每個路徑點與下一路徑點的距離并進行累加，此累加距離記為ls，即

(17)

當累加至點Bi時，ls(i)大于預瞄距離ld，終止累加，后比較ls(i)與ls(i-1)，選取與ld誤差較小的點，即為預瞄點。

2.4 切換控制

2.4.1 切換邏輯

當應用MPC模式進行跟蹤控制時，距離偏差與方向偏差過大，易造成控制系統產生振蕩，甚至失去跟蹤能力。并且不同車速下，距離偏差與方向偏差的影響不盡相同。

本文通過設置不同車速、初始距離偏差以及方向偏差，運用MPC方式控制AHC201跟蹤100 m直線路徑的測試方法，測試能夠保持系統穩定的最大距離偏差eymax及其對應的方向偏差eψmax，后進行數據擬合獲得不同車速v下，eymax與eψmax對應關系如圖8所示，由圖8可知，同一車速下，隨著eψmax增大，eymax逐漸減小；不同車速下，隨著車速的增大，eymax與eψmax允許范圍越來越小。

圖8 車速、位置偏差與方向偏差閾值對應關系

對于圖5所示的控制系統，本文選用位姿偏差(距離偏差與方向偏差)作為模式選擇的指標，根據車輛的當前車速以及圖8的對應關系，計算最大方向偏差eψmax為0時對應的最大距離偏差eymax(當前速度下最大的距離偏差)。再將當前的距離偏差ey與該值進行比較，若ey大于eymax，則選用PP控制模式，模式標志mod設為1；若ey小于eymax，則繼續根據圖8的對應關系以及當前的距離偏差ey。計算對應最大方向偏差eψmax，并將當前的方向偏差eψ與該值進行比較，若eψ大于eψmax，則選用PP控制模式，模式標志mod設為1；否則，在滿足MPC約束條件的情況下，選用MPC模式，模式標志mod設為2。

2.4.2 模式切換控制

在發生模式切換時，為了避免控制量的輸出產生躍變，引起系統發生瞬時振蕩和擾動，設計模式切換控制器對跟蹤控制器的輸出進行約束[13]。

在圖5的跟蹤控制器中，當模式切換控制器根據模式標志mod判定發生模式切換時，跟蹤控制器的期望輸出δref按照下式進行加權處理：

δref=c1δ1+c2δ2

(18)

式中：δ1為切換前控制模式(記為模式1)輸出的車輪期望轉向角；δ2為切換后控制模式(記為模式2)輸出的車輪期望轉向角；c1為模式1期望輸出的加權系數，c2為模式2期望輸出的加權系數。切換前，由模式1完全控制，即c1為1，c2為0；切換后，由模式2完全控制，即c1為0，c2為1，故c1的變化范圍為1～0，c2的變化范圍為0～1，在切換過程中，模式切換控制器依據設計的模糊控制器實時的調整c1、c2。

上述模糊控制器以加權系數c1、c2作為輸出，以切換前后2種控制模式期望輸出量的偏差絕對值|e|及其變化率ec作為輸入。根據AHC201的測量參數，模糊控制算法的輸入量|e|的基本論域為[0°，35°]，模糊論域為{0，1，2，3}，對應的模糊子集為{ZO，PS，PM，PB}；其變化率ec的基本論域為[-25°，25°]，模糊論域為{-1，0，1}，對應的模糊子集為{N，ZO，P}，隸屬度函數采用高斯型。模糊控制算法的輸出c1、c2的基本論域為[0，1]，模糊論域為{0，1，2，3}，對應的模糊子集為{ZO，PS，PM，PB}，隸屬度函數采用三角形。

該模糊控制對應的規則如表1和表2所示，當切換前后的偏差較大且有繼續增大的趨勢時，為了保證切換的平滑性，需要側重繼承切換前的控制模式；當切換前后的偏差較小時且有繼續減小的趨勢時，2種控制模式需要適當配合調整；當切換前后的偏差近似為零且能保持這種狀態時，說明切換過程結束，切換前控制模式可以退出，切換后控制模式完全接管控制。

表1 加權系數c1模糊規則表

表2 加權系數c2模糊規則表

3 控制效果測試

3.1 仿真測試

3.1.1 仿真平臺的介紹

為驗證上述聯合控制器在路徑跟蹤控制中的控制效果，本文采用MATLAB/Simulink與Carsim聯合仿真的方法進行仿真測試。以AHC201型線控底盤為參考模型，進行數據的標定，其部分參數如表3所示。

表3 FELIS AHC201整車參數

3.1.2 仿真分析

針對靜態障礙或者低速障礙進行避障換道是低速自動駕駛的一種典型工況[14-15]。本文采用一種避障換道路徑作為仿真的目標路徑，如圖9所示。

圖9 目標路徑

重點檢驗在存在較大初始位姿偏差和曲率實時變化的路況條件下，控制器的跟蹤效果，目標路徑的方程為

(19)

設置該路徑的附著系數為0.8，初始位置為(0 m，0 m)，初始方向為0°；車輛的初始位置為(-1.5 m，0 m)初始方向為-7°；車速設置為5.4 km/h。比較分析不同控制器作用下，跟蹤控制的精確度、穩定性以及快速性。

仿真結果如圖10所示，該結果表明，MPC的位姿實時處于不斷微調狀態，一旦位置或方向存在較大的偏差，其需要花費較長時間進行調整，在本次仿真中，由于初始位姿偏差較大，其位置調整長時間維持在-0.04～3.2 m，調整時間約為50 s，其方向調整長時間維持在0.01°～14.7°之間，調整時間約為30 s。PP控制雖實現了對目標路徑的及時平穩的跟蹤，但穩態偏差相對較大，尤其在曲率較大處，穩態位置偏差變化范圍為0.06～0.12 m。聯合控制很好地優化了上述2種控制的不足，在初始位姿偏差較大時，處于PP模式，實現了對目標路徑及時平穩跟蹤，待滿足切換條件后，切換為MPC控制模式，實現對目標路徑的精確跟蹤，且方向的微調波動減小。通過對仿真結果的分析，聯合控制算法相較于單一的MPC，跟蹤的及時性、穩定性均得到了較好的改善，相較于單一的PP控制，精確度得到了良好優化。

圖10 仿真效果對比

針對模式切換控制器控制效果驗證，分別應用加入與未加入模式切換控制器的2種聯合控制器進行了對比仿真，其仿真結果如圖11所示。仿真結果表明，未加入控制器的跟蹤路徑在切換點處產生較大波動，其轉向車輪橫擺角速度與車身的側向加速度產生了較為激烈的振蕩，橫擺角速度的波動范圍為-7°/s～4.5°/s，側向加速度的波動范圍為-2.8～2.2 m/s2；在加入控制器后，其橫擺角速度與側向加速度的波動范圍明顯減小，最大波動分別為-3.6°/s～-2°/s 以及-0.4～0.2 m/s2。對比結果表明，該模式切換控制器能夠較好的優化模式切換的魯棒性。

圖11 有無切換控制器控制效果對比

3.2 實車測試

為了驗證上述橫向跟蹤控制器的實際控制效果，借助天隼自主研發的FELIS自動駕駛開發平臺進行了實車測試，如圖12所示。該開發平臺以工控機為平臺，利用高精度的RTK-GPS導航模塊創建全局地圖，進行車輛的實時定位與局部路徑規劃；利用由激光雷達、毫米波雷達以及雙目視覺傳感器組成的感知模塊實時檢測路況信息(障礙物、行人及道路邊緣等)，完成SLAM建圖及局部路徑規劃；利用車體傳感器采集車速、加速度以及轉角等車體信息，后通過融合算法將上述模塊輸出的數據信息進行數據融合，完成運動路徑二次規劃以及行為決策，并通過CAN總線建立的分布式通信網絡發送給底層控制器，由底層控制器完成底層的路徑跟蹤。

圖12 FELIS自動駕駛開發平臺

本次實車測試中，以若干靜止的車輛作為障礙物，完成圖13(a)所示的避障路徑的標定，以此作為實車測試的目標路徑，測試環境為干燥瀝青路面，微風，路寬2.7 m，目標車速設定為5.4 km/h。測試結果如圖13所示，在整個測試過程中，車速穩定在4.9～5.8 km/h之間；由于存在較大的初始偏差，所以測試初期，控制器控制轉向電機迅速轉向，快速跟蹤上目標路徑，相應的位置偏差與方向偏差迅速減小；待達到穩定跟蹤后，位置偏差的波動范圍為-0.02～0.11 m，方向偏差的變化范圍為-1.4°～2.5°，達到了預期的控制目標。

圖13 實車測試結果

4 結 語

本文針對低速自動駕駛車輛橫向路徑跟蹤過程中，位姿偏差較大的情況下，單一的控制算法無法同時滿足精確度與穩定性的問題，結果MPC算法與PP控制算法的控制特點，設計了基于MPC算法與PP控制算法的聯合控制器。

由于MPC算法模式下，隨著車速的不同，對位置偏差和方向偏差容忍程度也不同，在不同車速條件下，橫向偏差與方向偏差的數據進行擬合，以此作為模式切換的依據。同時設計了基于模糊控制的模式切換控制器，保證切換過程的穩定平滑。

以FELIS AHC201型線控底盤作為模型，搭建了基于MATLAB/Simulink與Carsim的聯合仿真平臺，分別應用不同控制算法進行了仿真測試，并對仿真結果進行了對比分析。最后應用FELIS自動駕駛開發平臺進行了實車測試。仿真測試以及實車測試的結果均表明：本文所設計的聯合控制器兼有MPC算法控制精度高以及PP控制跟蹤速度快、穩定性好的特點。