智能汽車避障路徑規劃與跟蹤控制研究

呂 佳，邱建崗

（1.重慶建筑工程職業學院軌道與機電工程系，重慶 400072；2.北京汽車動力總成有限公司，北京 101106）

1 引言

道路交通事故造成的人員傷亡及經濟損失逐年遞增，面對如此嚴峻的交通安全問題，改善汽車安全性能迫在眉睫，無人駕駛技術是汽車主動安全的關鍵技術[1]，可以有效降低交通事故引發的人員損失和財產損失。

主要研究智能汽車避障路徑規劃與跟蹤兩個方面的內容。避障路徑規劃方法可分為傳統方法、圖形學方法和智能仿生算法三類，傳統方法包括人工勢場法、模擬退火法等，人工勢場法需解決“局部極值陷阱”問題[2]，模擬退火法存在尋優效率低、隨機性差的問題[3]。圖形學方法包括柵格法、可視圖空間法、voronoi圖法等，柵格法和可視圖空間法環境適應性差，環境改變時需重新建模[4-5]，voronoi圖不適用于大規模環境下路徑規劃。智能仿生算法是當前研究熱點，包括蟻群算法、粒子群算法、遺傳算法等[6]。在路徑跟隨控制方面，按照有無預瞄環節可以分為補償控制類模型和預瞄控制類模型，補償控制類模型在沒有前視作用前提下，根據當前時刻車輛狀態和期望狀態的誤差對車輛實施控制。根據預瞄點數量，預瞄控制類分為單點預瞄、兩點預瞄和多點預瞄，其中多點預瞄的魯棒性最好，跟蹤精度最高[7]。

本文以智能汽車避障過程為研究對象，在路徑規劃方面，提出了新的智能算法，即人工水滴算法，用于規劃車輛的避障路徑；在路徑跟蹤方法，提出了車輛轉向控制和速度自適應控制的綜合控制器。達到了智能汽車有效避開障礙物，且精確跟蹤避障路徑的目的。

2 避障路徑規劃

為了實現智能汽車避障路徑的智能實時規劃，提出了三維柵格模型與新型人工水滴算法結合的規劃方法。整體思路是：建立工作環境的三維柵格模型，柵格第三維為高度屬性，根據柵格與目標點距離確定，而后水滴根據地勢由起始點流到目標點。

2.1 三維柵格模型

參考傳統柵格模型，將避障環境劃分為若干柵格，根據后文人工水滴算法求解需要，建立三維柵格模型[8]，柵格第三維表示高度。柵格高度根據柵格與目標點距離確定，將目標柵格高度定義為0，其余柵格高度值為h=lnx+1，式中：h—柵格高度，x—柵格與目標柵格距離。分析柵格高度函數可知，與目標點距離越近，則高度下降梯度越大，可以加快算法收斂；與目標點距離越遠，則高度下降梯度越小，算法收斂慢，但是能夠增加路徑多樣性。

對于具有障礙物的工作環境，將障礙物所占柵格高度值賦為極大值，保證人工水滴能夠絕對避開障礙物柵格而選擇其他柵格。某一工作環境的三維柵格模型，如圖1所示。

圖1 三維柵格模型Fig.1 Three-Dimension Grid Model

2.2 人工水滴算法

模擬水往低處流、同時對地面進行沖刷而帶走部分泥沙的過程，本文提出了人工水滴算法。

（1）柵格選擇概率。記水滴所在柵格為i，則對于臨近的8個柵格，水滴的選擇概率為：

式中：hj—柵格j的高度；

pij—水滴選擇柵格j的概率。

分析式（1）可知，對于障礙物柵格，由于其高度為無窮大，則障礙物柵格被選擇概率為0。使用輪盤賭的方式選擇下一柵格，這樣既能保證較優柵格具有更大的選擇概率，同時其余柵格也有被選概率，保證路徑的多樣性。

（2）泥沙量更新（也可稱為高度更新）。由于水流的沖刷作用，水滴走過的柵格高度值會有所減小。當所有水滴按照式（1）完成路徑規劃，依據適應度值對路徑進行排序，然后對最優路徑經過的柵格高度進行更新，方法為：

式中：hj(k)—k時刻柵格j的高度；Δhj(k)—k時刻柵格j的高度變化量；Q—常數，代表被帶走泥沙總量；Lop—最優路徑長度，當最優路徑經過柵格j 時，則對柵格j 進行更新，否則不更新。

對步驟（1）（2）進行反復迭代，直至算法結束，輸出最優路徑。在柵格環境下，智能汽車避障路徑不可避免地出現曲折變向的情況，為了滿足車輛在運行狀態下動力學對安全性能的要求，本文使用B樣條曲線對原始路徑進行擬合。

2.3 動靜態環境下超車仿真驗證

為了驗證人工水滴算法在避障路徑規劃中的可行性，設置動靜態兩種仿真環境，完成智能汽車的避障路徑規劃。在動態環境下，每間隔0.3s刷新環境信息，根據動態障礙物運動情況和智能汽車行駛方法實時更新目標點并實時進行路徑規劃。人工水滴算法的水滴數量設置為30，算法最大迭代次數設置為100。

在長度為300m、寬度為7m的平直通向雙車道上，使用5m×1.4m大小的柵格將車道劃分為60×5的柵格，智能汽車探測前方150m范圍內的障礙物分布情況，每隔0.3s進行一次信息更新。

（1）靜態障礙物的超車路徑。智能汽車以30m/s的速度在道路上行駛，探測到同車道前方50m處存在靜態障礙物，使用人工水滴算法進行路徑規劃結果，如圖2所示。

圖2 靜態環境下避障路徑Fig.2 Obstacle Avoidance Path under Static Environment

使用Prescan的整車模型對規劃路徑進行跟蹤，跟蹤速度設置為30m/s。對避障路徑跟隨過程中，車輛的側向加速度峰值低于0.4g，橫擺角速度峰值低于15°/s，滿足約束條件，能夠保證車輛穩定行駛。

（2）動態車輛的超車路徑。智能汽車以30m/s的速度向前行駛，某一時刻探測到同車道前方50m存在一障礙車輛，障礙車的行駛速度為20m/s，每間隔0.3s更新一次周圍環境信息、目標點和行駛路徑，對動態車輛的避障路徑，如圖3所示。

圖3 動態環境下避障路徑Fig.3 Obstacle Avoidance Path under Dynamic Environment

圖3中黑色實心矩形為障礙車從t0～t9共10個時刻的刷新位置，空心矩形為智能汽車從t0～t9共10個時刻的刷新位置，紅色實線為動態超車軌跡。使用Prescan的整車模型對規劃路徑進行跟蹤，跟蹤速度設置為30m/s。對避障路徑跟隨過程中，車輛的側向加速度峰值低于0.4g，橫擺角速度峰值低于15°/s，滿足約束條件，能夠保證車輛穩定行駛。

3 模型預測轉向控制

為了實現避障路徑的跟蹤，本文建立了模型預測轉向控制和自適應速度控制的綜合控制器，兩者均在多點預瞄思想下實現，本節研究內容為模型預測轉向控制。

3.1 建立預測模型

模型預測控制對模型精度要求較低，而對預測實時性要求較高，因此本文選用線性二自由度模型[9]為預測模型。以前輪轉角為輸入量，側向位移和橫擺角為輸出量。首先給出建模使用的地面坐標系和車輛坐標系，如圖4所示。

圖4 線性二自由度車輛模型Fig.4 Linear Two Degree of Freedom Vehicle Model

圖中：OXY—地面坐標系，以道路前進方向為X軸，垂直方向為Y軸；Oxy—車輛坐標系，以車輛縱軸方向為x軸，車輛質心為原點，車輛橫軸方向為y軸；Lf—質心與車輛前軸距離；Lr—質心與后軸距離；δ—前輪轉角；φ—車輛橫擺角；β—車輛速度方向與車輛縱軸夾角。

選取系統狀態量為x(k)=[Vy(k)，ω(k)，y(k)，φ(k)]T，式中Vy(k)為k時刻側向速度；ω(k)橫擺角速度；y(k)為側向位移；φ(k)為車輛橫擺角。根據線性二自由度車輛的運動微分方程，得到狀態方程為：

式中：A—狀態轉移矩陣；B—輸入矩陣，且有

式中：Cf、Cr—前后輪的側偏剛度；

Vx—車輛縱向速度；

m—汽車質量；

I—車輛轉動慣量；

G—車輛自重。

選取側向位移y(k)和橫擺角φ(k)為輸出量，即z(k)=[y(k)，φ(k)]T，則輸出方程為：

記預測時域為Np，控制時域為Nu，要求Np≥Nu。記

則基于線形二自由度車輛模型的預測模型為：

3.2 滾動優化

滾動優化是指在一定約束條件下，通過某一性能指標最優化得到未來控制量。對于避障路徑跟蹤問題，一般以預測輸出與期望路徑方差最小為優化指標。另外，在路徑跟蹤過程中，希望使用較小的輪胎轉角就能夠跟蹤期望路徑，因此在優化指標中加入前輪轉角，得到滾動優化的目標函數為：

式中：W(k)=[w(k+1)，w(k+2)，…，w(k+Np)]T—預測時域內期望路徑；

Q=diag(q，q，…，q)Np—Np維的對角矩陣，為路徑誤差權重系數；

R=diag(r，r，…，r)Np—Np維的對角矩陣—輸入量權重系數。

通過調整q與r值可以實現路徑跟蹤誤差與輪胎轉角的權重分配，從而得到偏向于跟蹤精度或輪胎轉角變化平緩的跟蹤效果。

3.3 反饋控制

對式（6）所示的優化目標進行求解，得到控制時域內的最優控制序列為δop(k)=[δop(k)，δop(k+1)，…，δop(k+Nu-1)]T，對于滾動優化方法，在當前時刻只選擇δop(k)作為輪胎轉角控制量[10]。而后時間滾動至k+1時刻，重復進行模型預測、優化求解、反饋控制，直至車輛到達目的地。

4 自適應速度控制

根據駕駛經驗，經過曲率半徑小的路徑時需低速通過，經過曲率半徑大的路徑可高速通過。因此，行車速度應隨路徑曲率自適應變化，才能在保證行車安全的同時，提高路徑跟蹤精度。

4.1 速度規劃

參考單點預瞄模型，首先設置預瞄距離，而后對預瞄距離進行等間距分割，根據路徑曲率和車輛動力學約束設置約束條件，建立速度規劃目標函數，求解獲得速度曲線。

預瞄距離應足夠大，應大于等于安全預瞄距離，安全預瞄距離是指車輛以當前車速行駛時的最小剎車距離，計算方法為：

式中：Sa—安全預瞄距離；

v0—車輛當前速度；

amax—汽車最大制動減速度，即amax=-μg，μ-地面摩擦系數。

要求預瞄距離S大于安全預瞄距離Sa，即S＞Sa。確定預瞄距離S后，將其等間距地劃分為n段，每個等分點速度記為vi，i=1，2，，n。則速度規劃的目標函數為：

式中：k1—速度變化權重；

k2—速度大小權重。

分析式（8）可知，第一項優化目標是速度變化平穩，第二項優化目標是以最大速度行駛。

速度規劃的約束條件包括速度約束和加速度約束，將其描述為：

式中：vi，max—預瞄節點i處的最大速度；

ds—預瞄節點間距離；

ai—預瞄節點i處加速度；

ai，max—預瞄節點i處的最大加速度。

vi，max和ai，max由車輛性能、路面附著系數、路面曲率共同決定。求解以式（8）為優化目標，以式（9）為約束條件的二次規劃問題，可以得到各預瞄節點處的最優速度，而后使用B樣條曲線進行擬合，得到規劃的速度曲線。

4.2 速度跟蹤

車輛進行速度跟蹤的控制過程為：駕駛員根據當前車速與期望車速的大小情況，使用發動機進行加速或使用制動系統進行減速，最終使實際車速跟蹤期望車速。由以上過程可以看出，車速跟蹤過程中，加速度是連接動力系統和運動學系統的橋梁，因此選擇速度跟蹤分兩步進行：（一）根據期望速度軌跡，規劃出需要的加速度；（二）根據加速度值設計發動機工作點或制動系統工作點。

第一步：加速度規劃。速度跟蹤的目標是，使用最小的加速度，使車輛實際速度跟蹤期望速度，因此得到速度規劃的評價函數為：

式中：Jv—評價函數；w1、w2—誤差系數和速度變化系數；v0—當前時刻速度；ane—需求加速度；tp—預瞄時間；vi—預瞄點期望速度。

通過求解式（10）可以得到加速度最優需求值ane，而后進行第二步，根據加速度值確定發動機或制動系統工作點。

第二步：根據加速度需求確定動力系統工作點。當預瞄點加速度需求大于當前加速度時，需要發動機工作產生動力。根據牛頓第二定律和前輪轉動動力學方程，可以得到發動機轉矩需求為：

式中：Tnet—發動機需求轉矩；Ie—發動機轉動慣量；It—傳動系統轉動慣量；Iw—前輪轉動慣量；R—輪胎轉速與發動機轉速比；m—整車質量；reff—輪胎半徑；Ca—空氣阻力相關系數；ωe—發動機轉速；Rx—滾動阻力。根據求得的發動機轉矩需求和發動機轉速，通過查詢發動機萬有特性曲線可以確定節氣門開度。

當預瞄點加速度需求小于當前加速度時，需要制動系統工作產生制動力。制動系統工作過程為：駕駛員踏下制動踏板產生制動壓強Pb作用于制動主缸，制動主缸液壓系統將制動壓強分別放大ξ1和ξ2倍傳遞給前后輪缸，前后各輪缸按照比例將傳遞壓強轉化為制動力矩Tbf、Tbr，比例系數分別記為β1和β2，則

由式（12）得到制動系統的制動壓強為：

式（11）和式（13）即為速度跟蹤控制使用的控制量計算式。

5 仿真分析與驗證

5.1 繞樁實驗設計

避障路徑智能規劃方法的有效性在第二節中進行了驗證，本節只對模型預測轉向控制和自適應速度控制相結合的綜合控制方法進行驗證。人-車-路的閉環測試系統在Matlab 軟件中Simulink仿真平臺中搭建。

在長度為400m、寬為8m 的長直地面上，在（160，1）和（240，-1）兩點處存在兩個路樁，使用人工水滴算法規劃的繞樁路徑，如圖5所示。

圖5 繞樁路徑Fig.5 Path Around the Pile

圖5給出的繞樁路徑既有長直路徑，也包括曲率半徑較小的危險轉彎路徑，既能夠驗證前輪轉角控制效果，也能夠驗證速度隨路況自適應變化過程。車輛初始速度和終止速度均設置為30m/s，長直路徑階段要求進行定速巡航，遇到障礙物時使用綜合控制器避障。

5.2 跟蹤結果分析

現有的智能汽車軌跡跟蹤方法大都只對前輪轉角（即轉向）進行控制，汽車行駛速度通過認為設定驗證跟蹤效果。文獻[11]中將速度分別設置為1m/s、2m/s、3m/s進行軌跡跟蹤驗證，車速均處于較低水平，與汽車的實際行駛工況不符。為了形成對比效果，本文以前文繞樁路徑為跟蹤對象，分別使用本文的模型預測轉向控制加自適應速度控制的綜合控制與文獻[11]的方法進行軌跡跟蹤。

使用本文方法時，速度變化過程為：在繞樁轉彎前車輛以30m/s進行定速巡航，而后轉入自適應速度控制，繞樁結束后自適應地加速至30m/s定速巡航狀態。使用文獻[11]方法跟蹤路徑時，將車速設置為105km/h定速狀態。兩種方法的路徑跟蹤效果，如圖6所示。

圖6 繞樁路徑跟蹤效果Fig.6 Tracking Effect of Path Around the Pile

從圖6中可以直觀看出，使用本文的轉向加車速綜合控制方法，智能汽車實際行駛軌跡與規劃軌跡幾乎完全重合，而文獻[11]的軌跡控制方法在轉彎處具有明顯的跟蹤誤差。經統計，本文綜合控制方法的最大橫向誤差出現在155m處，最大橫向跟蹤誤差為0.1m；使用文獻[11]的軌跡跟蹤方法，最大橫向誤差出現在241m處，最大橫向跟蹤誤差為0.6m，是本文綜合控制方法的6倍。這是因為本文在轉向控制的基礎上制定了車速的自適應控制方法，使得車輛在探測到轉彎時，可以根據轉彎處的緩急程度自適應調整車速，實現車輛軌跡的高精度跟蹤控制。路徑跟蹤過程中，智能汽車前輪轉角和橫擺角速度變化曲線，如圖7所示。對比圖5給出的軌跡和圖7（a）給出的前輪轉角變化曲線，軌跡在110m處開始出現彎道，前輪轉角在（100～110）m之間轉彎角，與駕駛員實際駕駛情況一致，存在一定的預瞄距離，即智能汽車的轉向控制合理。根據駕駛經驗，駕駛速度越高時，轉角操作越頻繁，圖7（a）所示前輪轉角變化頻率較大，符合這一規律。

圖7 轉向控制結果Fig.7 Turning Control Result

另外，橫擺角速度極值不足0.15rad/s，在約束范圍內，符合國家標準的汽車穩定性要求。速度控制的規劃曲線及跟蹤過程中節氣門開度和制動系統的制動壓力曲線，如圖8所示。

圖8 速度控制結果Fig.8 Speed Control Result

分析圖8（a）可知，車輛在繞樁轉彎前為定速巡航狀態，在100m處左右，由于預瞄到轉彎車輛開始減速，在300m處車輛完成繞樁，車輛立即加速至定速巡航狀態，以目標速度行駛，此速度控制過程，既提現了轉彎時的自適應速度變化，低速安全通過轉彎區域，同時體現了轉彎結束后立即加速度前進，展現了很高的行駛效率。經計算，速度跟蹤誤差最大值為0.5m/s，跟蹤誤差均值為0.1m/s，誤差的出現是多方面因素導致的：（1）在式（10）中設定跟蹤目標時，不僅僅以跟蹤誤差最小為目標，而且加入了加速度最小的目標，這就必然導致根據式（10）解算出的控制量存在跟蹤誤差；（2）加速度跟蹤時，預瞄時間對跟蹤誤差影響較大，若預瞄時間過大則時效性差、跟蹤誤差大；若預瞄時間小則時效性好，但是遇到緊急狀況時反應時間不夠。對比圖8（b）圖8（c），兩者工作時間完全錯開，不存在工作時間的交集，與實際駕駛情況一致，說明速度控制過程科學合理。對比圖8（a）圖8（b）圖8（c）可知，速度變化過程與發動機工作時域、制動系統工作時域完全一致，說明了本文提出的控制方法對避障路徑跟蹤精度高且安全合理。

6 結論

研究了智能汽車避障路徑規劃和路徑跟蹤兩個方面的內容，分別提出了基于人工水滴算法的路徑規劃方法和路徑跟蹤的綜合控制方法。經仿真驗證，得到了以下結論：（1）在靜態環境下，人工水滴算法可以實時規劃避障路徑；在動態環境下，通過目標點的實時更新，人工水滴算法可以實現避障路徑動態規劃；（2）綜合控制器通過對速度自適應調整和對轉向的預測控制，可以實現對避障路徑的精確跟蹤控制。