智能車輛彎道換道路徑規(guī)劃算法與跟蹤控制

吳樹凡，魏民祥，滕德成，汪 ?，邢德鑫，任師通

（1.南京航空航天大學 能源與動力學院，南京 210016；2.東南大學 機械工程學院，南京 211189）

道路交通事故是所有國家都面臨的問題。其中，每公里彎曲道路上發(fā)生的交通事故數量明顯大于在直線道路上發(fā)生的交通事故數量［1-2］，而大多數的交通事故都是人為操作不當造成的。智能車輛能減少交通事故的發(fā)生，是當今的研究熱點。國內外對于智能車輛直道換道的研究較多，但是對于彎道的研究較少，因此對于智能車輛彎道換道技術的研究具有重要意義。其中，如何快速準確地進行換道路徑規(guī)劃并有效跟蹤期望路徑，是實現智能車輛車道變換功能的基礎，也是提高車輛主動安全性的關鍵技術之一［3］。

國內外存在大量的關于智能車輛車道變換的研究。在路徑規(guī)劃方面，先進的智能車輛路徑規(guī)劃算法有：A*算法、D*算法、Dijkstra算法、人工勢場法等，其中，A*算法和D*算法常用于全局路徑規(guī)劃［4］，而Dijkstra算法、人工勢場法用于局部路徑規(guī)劃時，雖規(guī)劃速度較快但存在規(guī)劃軌跡不理想且可能陷入局部最優(yōu)解的問題。先進的智能車輛路徑規(guī)劃算法還有：蟻群算法、遺傳算法、神經網絡算法等，具有規(guī)劃路徑良好但計算量巨大難以滿足實時性的問題［5］。傳統的車道變換方法有：正弦法（車輛期望位置滿足正弦函數）、圓弧法、多項式以及梯形加速度理論（期望側向加速度能夠滿足正反梯形約束）。其中多項式規(guī)劃具有簡單、運算量小的特點，能滿足高速行駛時的時間要求，且具有通用性和實用性強的優(yōu)點，但是沒有考慮車輛基于該路徑行駛時是否滿足車輛穩(wěn)定性的問題，本文將考慮智能車輛在不同的車速下的路徑規(guī)劃以及穩(wěn)定性的問題。蔡英鳳等［6］采用可拓優(yōu)度評價理論設計了一種橫向軌跡跟蹤控制系統，對基于預瞄偏差的PID反饋控制器和基于道路曲率的PID前饋-反饋控制器在不同工況下進行實時的選擇，具有較好的效果，本文將基于可拓優(yōu)度評價進行最優(yōu)換道路徑規(guī)劃的研究。

路徑跟蹤控制算法主要有最優(yōu)控制［7］、PID算法［8］、魯棒控制［9］、模糊控制［10］、滑模控制［11］和模型預測控制［12］（MPC）等。基于MPC的控制器，由于考慮了車身自身結構特征的限制與車輛動力學參數的一些約束限制，以及擁有較強的處理多約束的能力被廣泛使用［13］。胡均平等［14］考慮到在復雜路況下車輛避障會發(fā)生穩(wěn)定性與避障的行為沖突，提出了一種車輛避障控制算法。該算法基于MPC并通過將路徑跟蹤、避障和穩(wěn)定性三者集成在一起，設定優(yōu)先級以協調三者的行為沖突，較為準確地表示出了車輛的避障路徑并進行跟蹤。任玥等［15］針對主動避撞問題提出一種分層避撞方法，路徑規(guī)劃層使用人工勢場函數描述車輛碰撞風險并基于5次多項式規(guī)劃局部避撞路徑，路徑跟蹤根據MPC算法給出最優(yōu)方向盤轉角進行路徑跟蹤控制。本文中提出一種智能車輛的彎道換道系統，可用于彎道避撞路徑規(guī)劃，采用可拓優(yōu)度評價理論進行彎道路徑規(guī)劃，考慮了道路利用率（換道距離）以及車輛穩(wěn)定性的因素，再基于MPC算法設計路徑跟蹤控制器對最優(yōu)換道路徑進行跟蹤控制。

1 車輛動力學建模

1.1 路徑規(guī)劃所需的車輛動力學模型

由于本文路徑規(guī)劃算法考慮了車輛在換道時的穩(wěn)定性，需要考慮的量為側向加速度、橫擺角速度、質心側偏角，因此基于圖1建立車輛2自由度模型［16］：

式中：ωr為橫擺角速度；a、b為質心至前軸和后軸距離；K1、K2為前輪和后輪的側偏剛度；Iz為繞z軸的轉動慣量；V為縱向車速；m為汽車總質量；δf為前輪轉角；β為質心側偏角。

圖1 2自由度車輛模型示意圖

1.2 路徑跟蹤所需的車輛動力學模型

考慮汽車縱向、側向、橫擺運動，建立如圖2所示的車輛單軌模型，用于基于模型預測控制算法的路徑跟蹤控制器設計。車輛為前輪驅動和轉向，忽略懸架與轉向系統以及空氣動力學的影響。

圖2 車輛單軌模型示意圖

基于圖2，其中跟蹤控制使用車輛動力學模型如下［17］：

式中：Fcf、Fcr為前后輪胎所受側向力；Flf、Flr為前后輪胎所受縱向力；Fxf、Fxr為前后輪胎在x方向所受力；Fyf、Fyr為前后輪胎在y方向所受力。

假設車輛輪胎工作在線性區(qū)域，輪胎力用線性函數近似表示。

式中：Clf、Clr為前后輪胎的縱向剛度；sf、sr為前后輪胎的滑移率。

在式（2），假設車輛前輪轉角和輪胎側偏角較小，則

根據式（2）～（7）可得：

2 彎道換道路徑規(guī)劃

彎道換道系統結構框圖如圖3所示，主要分成4部分，第1部分為環(huán)境感知系統，得知車輛位置信息（X，Y）、道路曲率半徑（R）、與前方障礙物距離（Lreal）、前方障礙物速度（vq）等信息。第2部分為決策層，在確定前方障礙物異常的情況下，在彎道左側無障礙物的情況下，直接進行換道。假設第1、第2部分的信息已知，路徑決策部分已作出換道指令。第3部分為路徑規(guī)劃層，得到最優(yōu)軌跡的縱、橫坐標以及橫擺角速度。主要分為：上層路徑生成器和下層路徑選擇器。其中上層路徑生成器根據不同的橫向距離由5次多項式生成一系列彎道換道路徑，下層路徑選擇器包括車輛狀態(tài)預估模型和可拓優(yōu)度評價控制器。第4部分為路徑跟蹤控制器，輸出前輪轉角（δf）控制車輛跟蹤規(guī)劃的路徑。

圖3 彎道換道系統結構框圖

2.1 道路模型

道路模型分為直線模型以及曲線模型兩大類，在之前研究中的道路用曲線模型中的拋物線模型來描述道路［18］。本文考慮到拋物線模型無法正確表示后方區(qū)域的道路，且本文的研究對象為彎道工況，且在一段時間內彎道的曲率不會有很大的改變，因此在假設彎道曲率在一定時間內不變的前提下，采用圓弧方程對道路進行擬合。如下：

式中：x是車輛在大地坐標系下的縱向距離；R是當前道路曲率半徑；ylane為車輛中心到車輛中心起始點的橫向距離。

2.2 上層路徑生成器

上層路徑生成器，用于生成一系列路徑，本文采用5次多項式進行換道路徑生成［15］。圖4為車輛彎道換道的位置變換示意圖，以車輛換道初始時刻的質心位置向外車道做垂線，交點為坐標原點，以原點向行駛方向做外車道邊緣切線，即為X軸正方向。

圖4 換道位置變換示意圖

假設：彎道在某一時間段內曲率不變，為R，車輛的縱向速度為定值V，路面附著系數為定值μ。

由式（9）以及圖4，得外車道邊緣：

設定車道寬度為dw，則車輛所在原路徑公式為：

目標換道路徑為：

其中車輛的換道軌跡可以設為：

由5次多項式換道規(guī)則，車輛在換道初始位置x＝0和結束位置x＝xd需滿足如下方程［19］：

式中，Rz為車道線中心線的曲率半徑。

由式（11）（12）（13）（14）推導可得：

由于縱向速度變化較小，此處，假設縱向速度恒定，得縱向換道距離xd＝V·t，因此得到換道軌跡：

2.3 下層路徑選擇器

本文中主要針對最優(yōu)換道路徑無法確定的問題，采用蔡文教授提出的可拓學理論［20］，提出了使用可拓優(yōu)度評價選取最優(yōu)路徑的方法。可拓優(yōu)度評價方法是可拓學中評價一個對象（事物、方法等）優(yōu)劣的基本方法，根據實際情況制定出符合相關要求的評價標準，確定衡量指標，從而知道其利弊的過程與變化，最后可評價該對象的優(yōu)劣。下層路徑選擇器利用可拓優(yōu)度評價選擇器來評價所有路徑的優(yōu)劣，從而選擇出優(yōu)度較高的路徑輸出值。

可拓優(yōu)度評價選擇器［6］的流程如圖5所示。

圖5 優(yōu)度評價選擇器流程框圖

1）評價指標

想要評價一個特定對象的優(yōu)劣，需要先確定一個評價指標。規(guī)定不同的評價指標會得到不同的結果，因此選擇合適的評價指標才能得到合適的選擇結果。為得到某評價指標下的最優(yōu)換道路徑，除了將換道路徑縱向距離和側向加速度作為參考指標外，將反映車輛穩(wěn)定性的橫擺角速度與質心側偏角也同樣作為影響指標。根據上層路徑生成器生成的變道軌跡，縱向速度視為定值，計算得到理想的側向加速度［21-22］，輸入到車輛狀態(tài)預估模型中，根據轉向逆動力學模型得到方向盤轉角（δsw），輸入到二自由度車輛動力學模型中，得到對應的車輛狀態(tài)預估值：橫擺角速度（ωr）和質心側偏角（β），加上軌跡原本已知的側向加速度（ay）和換道距離（l），以該4個值作為衡量指標，第1項反映換道效率，第2、3、4項反映換道穩(wěn)定性與安全性。

2）權系數

評價路徑優(yōu)劣的各個指標具有輕重之分，使用權值系數表示每個衡量指標的重要程度，分別給每個指標賦予0到1之間的值。權值系數記為：

3）關聯函數

以車輛的質心側偏角β為例，建立關聯函數。選擇特征量最優(yōu)狀態(tài)點均為原點S0＝（0，0）。質心側偏角與最優(yōu)點S0＝（0，0）的加權可拓距為：

質心側偏角經典域界可拓距為：

質心側偏角可拓域界可拓距為：

4）計算關聯度

衡量指標為MI＝｛l，ay，wr，β｝，權值系數為w＝（w1，w2，w3，w4），根據各個衡量指標的要求，把不同縱向距離xd對應的換道路徑關于各個衡量指標MIi的關聯函數值簡記為Ki＝（Sj），則各個對象關于MIi的關聯度為：

將上述關聯度進行規(guī)范化：

不同縱向距離對應的換道路徑關于MIi的規(guī)范關聯度為：

5）計算優(yōu)度

對象y（xdj）關于各個衡量指標MI1、MI2、MI3和MI4的規(guī)范關聯度為：

對象Zj的優(yōu)度為：

6）選取優(yōu)度較高方案

對于y（xdj）的優(yōu)度進行比較，若：

則y（xd0）為較優(yōu)路徑，xd0便是限制條件下最優(yōu)路徑對應的換道縱向距離。

優(yōu)度評價方法如表1所示。

表1 優(yōu)度評價方法

其中橫擺角速度和質心側偏角的獲取采用逆動力學模型［22］和式（1）的2自由度模型獲得，其中轉向逆動力學模型為：

式中：δsw為期望方向盤轉角；i為轉向系傳動比。

依據文獻［24］，穩(wěn)定性基本約束為：

式中β為質心側偏角。

3 路徑跟蹤控制器的設計與參數優(yōu)化

3.1 路徑跟蹤控制器的設計

本文中基于MPC（模型預測控制）進行路徑跟蹤控制器設計［25］，由于使用線性時變模型進行模型預測控制具有計算簡單、實時性好等優(yōu)點，因此首先對式（8）進行線性化。

對式（30）采用1階差分法進行離散化，采樣時間為T，得到狀態(tài)方程：

將離散狀態(tài)變量x（k）與控制變量u（k）組合成新的狀態(tài)變量：

即可得到新的狀態(tài)空間方程：

式中：yd＝[ φdYd]由路徑規(guī)劃算法計算得到；Q和R為加權矩陣；Np和Nc為預測時域和控制時域。

基于動力學模型的路徑跟蹤控制器在每個控制周期需求解以下約束問題：

式中：yh為硬約束輸出（不能放寬約束范圍）；ys為軟約束輸出（可以通過松弛因子ε對約束范圍進行調 整）；yh，min、yh，max為 硬約 束極 限 值；ys，min、ys，max為軟約束極限值。

求解出k時刻的一組控制增量序列：

k時刻控制變量可表示成k-1時刻控制變量疊加k時刻的控制增量，即：

3.2 控制器的參數優(yōu)化

通常MPC控制器主要是根據工程經驗和試湊調整參數，屬于尋優(yōu)過程。本文為得到合適的預測步長Np和控制步長Nc，設計目標函數：

該目標函數第1項為實際換道橫向距離Y與期望換道橫向距離Yref的差值，第2項為實際橫擺角φ與期望橫擺角φref的差值，設計車輛在一段路徑上行駛，得到最優(yōu)的預測步長和控制步長。

4 仿真與分析

4.1 控制器參數和仿真工況

為驗證彎道避障系統的可行性，使用Matlab／Simulink與Carsim聯合仿真，搭建仿真系統結構，如圖6所示。

圖6 彎道避障系統仿真結構示意圖

4.1.1 路徑規(guī)劃相關參數

假設自車縱向車速為V＝14 m／s，dw＝3.75，由于我國高速公路彎道設計標準，在平原和丘陵地帶，高速公路的彎道最小曲率半徑為650 m。此處選擇R＝650 m進行最優(yōu)軌跡規(guī)劃分析。整車參數如表2所示。

表2 整車參數

設定式（17）中的權值系數為：w1＝0.25，w2＝0.25，w3＝0.25，w4＝0.25，即在選擇最優(yōu)路徑時，各個因素視為相同權重，仿真計算得到車輛最優(yōu)換道路徑和最優(yōu)換道縱向距離。

4.1.2 路徑跟蹤控制器參數

采用某型前輪驅動的轎車作為實驗仿真車，路面附著系數為μ＝0.85，在彎道曲率半徑為R＝650 m的單向雙車道公路上行駛，由于彎道換道具有一定危險性，因此此處實驗車速主要考慮中低速的情況，以速度為11、14、18 m／s驗證彎道換道路徑跟蹤控制器的性能。仿真步長T＝0.02 s，V＝11、14、18 m／s時分別對應的仿真周期設為6.5、5、3.7 s。

路徑跟蹤控制算法相關參數如下：

此外，預測控制加權矩陣Q和R選定為：

控制器約束設定為：

4.1.3 路徑跟蹤仿真工況設計

圖7所示為本次實驗道路路徑，用于驗證路徑跟蹤控制算法的有效性。該路徑主要分為2段，第1段為換道部分路徑，換道路徑來自路徑規(guī)劃控制層得到的最優(yōu)換道路徑，為1段5次多項式型曲線，第2段為換道結束后在內側車道中心線上行駛的部分，為1段圓弧。

圖7 彎道避障換道路徑示意圖

4.2 仿真結果分析

4.2.1 最優(yōu)路徑確定分析

圖8為特定速度和道路曲率下生成的一系列換道路徑（8、10、12、…、54 m）以及最優(yōu)換道路徑。由圖8可知，在車速為14 m／s、彎道半徑為650 m的工況下，最優(yōu)換道距離為38 m。圖9為不同縱向車速下的最優(yōu)換道縱向距離，在彎道半徑為650 m的工況下，車速與最優(yōu)換道路徑縱向距離基本成正比關系，根據最小二乘法，可得到特定曲率半徑的彎道工況下，不同速度對應的縱向換道距離的關系式為：xd-best＝2.93V-3.33，從而知道特定速度所對應的最優(yōu)換道路徑。

圖8 V＝14 m／s車輛換道路徑與最優(yōu)換道路徑示意圖

圖9 不同縱向車速下的最優(yōu)換道縱向距離

4.2.2 路徑跟蹤仿真分析

由圖10、11可知，當速度為11、14、18 m／s時，最優(yōu)縱向換道距離分別為29、38和49 m。在3種工況下，車輛能基本貼合兩段組合路線行駛，跟蹤效果較好，并且在兩段路徑節(jié)點能夠平滑過渡，說明了路徑規(guī)劃層規(guī)劃的合理性以及MPC路徑跟蹤控制器在不同速度下控制的穩(wěn)定性。

圖10 V＝14 m／s時的路徑跟蹤效果

圖11 V＝11 m／s和V＝18 m／s時的路徑跟蹤效果

由圖12可知每一時刻的路徑跟蹤誤差，根據圖中顯示，該路徑跟蹤控制器在換道時表現出了較好的跟蹤效果，尤其在前半段效果更加明顯，誤差基本保持在0.05 m以內。在5次多項式的后半段路徑，誤差低于0.3 m，在較高的車速下也能較好地跟蹤路徑，且在兩段路徑的連接點跟蹤誤差較小。

圖12 3種工況下的路徑跟蹤誤差

由圖展示出該控制器的一個特點，較高的車速下擁有更小的跟蹤誤差，通常情況下，較高的車速和較低的車速跟蹤同一條路徑時，高速下的跟蹤誤差較大，此處變小是因為智能車輛以不同速度行駛時規(guī)劃的最優(yōu)換道路徑不同，更高的速度下跟蹤誤差沒有變差，也反映了路徑規(guī)劃器的優(yōu)勢。在跟蹤圓弧路段時，路徑跟蹤誤差較小且比較穩(wěn)定，并維持在0.08 m以下。

由圖13、14、15可知質心側偏角最大值分別為1.2°、0.6°、0.4°（小于10°），橫擺角速度最大值為13、12、11（°）／s（小于26.5（°）／s），橫向加速度最大值為0.26g、0.27g、0.29g（小于0.4g），皆在穩(wěn)定域內。當速度升高時，橫向加速度最大值升高，顯示出彎道換道時需要限制車速。質心側偏角、橫擺角速度以及側向加速度在5次多項式換道路徑部分，跟蹤時穩(wěn)定性會有較大的波動，而在圓弧路徑部分，跟蹤趨于穩(wěn)定，且3個指標的數值趨于零，車輛穩(wěn)定性較高。

圖13 3種工況下的質心側偏角

圖14 3種工況下的橫擺角速度

圖15 3種工況下的橫向加速度

由圖16可知，MPC路徑跟蹤控制器輸出的方向盤轉角都在合理范圍內，且在5次多項式路徑部分，方向盤轉角需要進行一定的調整，絕對值的最大值分別為66°、48°、38°，方向盤轉動合理，在圓弧路徑部分，方向盤轉角進行細微調整后維持在一定值，穩(wěn)定行駛，說明了MPC路徑跟蹤控制器的合理性。

圖16 3種工況下的方向盤轉角

5 結論

針對智能車輛彎道路徑規(guī)劃與跟蹤控制的問題，提出了一種彎道換道系統，包括基于可拓優(yōu)度評價的彎道換道路徑規(guī)劃算法與基于模型預測控制的路徑跟蹤控制方法。搭建了Carsim／Matlab聯合仿真平臺，對不同車速下的車輛避障路徑規(guī)劃與跟蹤控制性能進行了仿真驗證。仿真結果表明：該彎道換道系統權衡了換道縱向距離與車輛穩(wěn)定性因素，能根據車輛當前狀態(tài)（車速）和道路信息（彎道曲率）合理地規(guī)劃出最優(yōu)換道路徑。MPC路徑跟蹤控制器考慮車輛動力學特性，能準確地預測車輛未來狀態(tài)并優(yōu)化求得最優(yōu)方向盤轉角，準確、穩(wěn)定地跟蹤換道路徑，同時路徑跟蹤控制器的仿真效果也證明了路徑規(guī)劃部分規(guī)劃出路徑的合理性以及整個換道系統的合理性。

本文僅考慮了智能車輛勻速行駛條件下的彎道換道，未考慮車輛變速情況下的換道情況，下一步的工作將考慮車輛在前方障礙物不同的運動狀態(tài)下，縱向速度實時改變的換道控制，實現車輛實時換道路徑規(guī)劃與變速路徑跟蹤控制器的設計。