無人車輛軌跡規劃與跟蹤控制的統一建模方法

徐楊 陸麗萍 褚端峰 黃子超

統計顯示,90%以上的交通事故都是由駕駛員的失誤操作引起.近年來,隨著Google、百度等公司在無人駕駛方面的研發推動,使得人們熱切期待利用無人車輛來徹底解決交通事故問題.欲完全取代人類駕駛員,無人車輛的自動駕駛系統需足夠智能化,以應對復雜的道路交通場景.特別地,無人車輛需在全局路徑規劃得到的起點與終點之間的最優路徑基礎上,根據動態交通環境以及自車運動狀態,進行局部的軌跡規劃,以實現動態避障.軌跡規劃在機器人領域已被廣泛研究并用于障礙物的規避[1].無人車可看作是一種輪式機器人,因此可借鑒一些機器人領域的軌跡規劃方法.當然兩者的軌跡規劃也有很大的不同,例如,無人車的運行速度相比于普通機器人來說要快得多,行車環境也是動態變化的,無人車的軌跡規劃還需要考慮道路的結構信息以及一些交通規則.另外,無人車在軌跡規劃的過程中要考慮車輛的運動學、動力學特性以及滿足輪胎等因素的約束.目前已經有很多學者對無人車的軌跡規劃進行了研究[2?5],常見的無人車軌跡規劃算法包括基于特定函數的軌跡規劃方法[2]、基于搜索的軌跡規劃方法(隨機搜索法[3]、柵格法)、基于優化的軌跡規劃方法(模型預測方法[4]、人工勢場法等).

人工勢場是最具吸引力的路徑規劃算法之一,它最早被用于機器人的路徑規劃[6].通過為障礙物、道路結構以及目標點分配合理的勢場函數,人工勢場可為車輛規劃出一條通往目標點且無碰撞的路徑.人工勢場的主要優勢在于結構簡單、可用不同的勢場函數比較精確地描述各類障礙物、道路結構等影響因素.目前,已經有許多學者將人工勢場用于自動駕駛車輛的軌跡規劃.Wang等[7]基于勢場理論提出了一種駕駛安全勢場模型,包括靜止障礙物(靜止的車輛、道路邊界)勢場、運動障礙物勢場、駕駛員行為勢場,最后根據駕駛安全勢場模型可以實現車輛的軌跡規劃.Wolf等[8]提出了相應的勢場函數來描述行車環境中的各類影響因素,然后用梯度下降法來進行軌跡規劃.Cao等[9]根據障礙物、道路邊界、目標點的特征設計了諧波函數形式的勢場模型,通過梯度下降法得到最優的軌跡.基于這種架構,Ji等[10]先通過構建環境勢場,分別用三角函數和指數函數形式來描述道路邊界和障礙物,再進行軌跡規劃,最后通過模型預測控制實現軌跡跟蹤.然而,這些方法在產生最優軌跡時,并未考慮車輛的一些特性,這可能導致規劃出的軌跡并不適合于車輛進行跟隨.

模型預測控制也可用來進行軌跡規劃.Abbas等[11]基于障礙物與車輛的間距建立了避障目標函數,然后把它引入到模型預測算法的目標函數中,以實現障礙物的規避.Park等[12]把障礙物信息以視差的形式引入到模型預測控制的框架中進行避障.Mousavi等[13]把障礙物和道路結構以約束條件的形式引入到模型預測控制中實現障礙物規避.從上述文獻可知,基于模型預測控制的路徑規劃方法通常把障礙物、道路等影響因素當作是一種約束或用單一的目標函數來描述所有這些因素.當前的無人車輛或機器人的軌跡規劃與跟蹤控制通常分為兩個獨立的單元,即采用人工勢場等方法實現軌跡規劃,再通過魯棒控制、最優控制等方法實現對規劃出的軌跡進行跟蹤.Huang等[14]首次提出了一種基于人工勢場的模型預測控制器,兼具軌跡規劃與跟蹤控制的功能.然而,該研究采用較為簡單的車輛運動學模型,其跟蹤控制精度有待進一步提高.

為此,本文基于車輛動力學模型,利用人工勢場與模型預測控制相結合,通過模型預測控制中的優化算法來實現最優軌跡選擇,提出一種針對軌跡規劃與跟蹤的統一建模方法,即在無人車輛軌跡規劃的同時,實現對無人車輛的縱橫向耦合控制.

本文的結構如下:第1節介紹無人車輛軌跡規劃的框架,主要根據車道線、環境車、目標點等多影響因素建立勢場函數;第2節利用車輛動力學建模,并基于模型預測控制方法設計軌跡跟蹤控制器,并實現對最優局部軌跡的規劃與跟蹤;第3節通過不同的交通場景,對無人車輛軌跡規劃與跟蹤控制器進行驗證分析;第4節分對全文進行總結.

1 基于人工勢場的行車環境建模

行車環境建模主要是對無人車輛在行駛過程中可能遭遇的靜態和動態交通環境進行建模,以量化交通環境對無人車輛構成的風險.在正常情況下,車輛應沿著車道中心線以一個穩定的速度前行,當遭遇本車道前方慢行的障礙車輛時,應執行換道超車動作,或在遭遇他車道的干擾車輛時伺機尋找換道時機.在這個過程中,車輛始終必須保證其既不能同其他的環境車(包括障礙車輛和干擾車輛)發生碰撞,也不能駛出道路邊界線.

假設被控車行駛的路線足夠長,那么在行車過程中經歷的環境主要包括車道線(包括分道線和道路邊界線)、環境車和目標點。因此,可將行車環境對被控車的總勢場U定義為分道線勢場Ulane,道路邊界線勢場Uroad,環境車勢場Ucar和目標點勢場Ugoal等各類勢場的總和.

1.1 車道線勢場

車道線包括分道線和車道邊界線兩類.其中,分道線勢場的作用是確保車輛沿著在車道內行駛,避免不必要的換道.所以在分道線位置勢場取最大值,往兩邊減小.同時,分道線的勢場應該足夠小,以確保被控車在需要換道避障時能跨越.本文采用類高斯函數[8]進行建模.

其中,Alane是分道線勢場系數,ylane,i表示第i條分道線在Y方向上的(位置X為行車方向,Y為垂直方向,且兩者相較于道路最右側車道的中心線上),σlane表示分道線勢場的收斂系數,由車道寬度決定.圖1是道路勢場示意圖.圖1(a)表示一個三車道道路結構的分道線勢場示意圖,設車道寬為3m,則兩條分道線在Y方向的位置分別為y=1.5m和y=4.5m.

車道邊界線的作用是防止車輛駛出道路,是嚴格不可逾越的,因而被控車在靠近車道邊界線時,勢場值應迅速增大,在道路邊界線達到最大值.根據這個特征,采用一個常用于機器人軌跡規劃的勢場函數,道路邊界的具體勢場函數形式下:

圖1 道路勢場示意圖Fig.1 Sketch of road potential

其中,Aroad為道路邊界勢場系數,j∈{1,2},yroad,j為第j條道路邊界線的位置.兩條道路邊界線在Y方向的位置分別為y=?1.5m和y=7.5m,圖1(b)是道路邊界線勢場的示意圖.

因此,車道線的總勢場表示為分道線與道路邊界線勢場的和,如圖1(c)所示.

1.2 環境車勢場

環境車勢場的建立,可保證被控車與其周圍的環境車保持相對安全,且能夠引導被控車從環境車后方換道.對環境車而言,附近區域的危險程度的縱橫向分布并不均勻,因此橫向和縱向的勢場分布也有很大的差異.一般地,在橫向上,被控車可距其1米甚至更近,但在縱向上這個距離卻非常危險.在縱向上,除相對距離因素外,環境車與被控車的相對速度也是影響環境車勢場的一個重要因素.

以車尾中心為原點建立局部坐標系(如圖2所示),對環境車的縱向勢場進行分析.為了更安全地引導后方被控車進行換道,在環境車后方增加一個三角形區域,在其前方增加一個矩形區域,這兩個區域跟車身所在區域一樣,也是被控車不可逾越的區域.車身所在區域,即x∈[0,L],縱向勢場取最大值Acar.

車輛前方,即x∈[L,L+s],這個勢場主要是為了防止相鄰車道的環境車在換至本車道時與被控車過于靠近.距離s與當前車輛的速度相關,速度越快,距離應該相應增大,所以s的計算公式為s=ρ×V+Smin,在這個范圍內的縱向勢場值也取最大值Acar,其中V表示當前環境車的速度,ρ是比例系數,Smin表示設定的最小安全距離.

圖2 環境車的局部坐標系Fig.2 Coordinate system of the environment vehicle

車輛后方的縱向勢場比較特殊,分兩部分考慮.

1)x∈[?S,0],這部分的縱向勢場值也取最大值,作為一個不可靠近的區域,S的計算方式如下:

其中,Vr表示相對速度,?T表示延遲時間.

2)x

其中,Acar,long是縱向勢場系數,K表示到(?S,0)的距離.在計算K之前,對被控車的橫坐標進行一個轉化,x?=x×τ,其中τ∈(0,1],使被控車與環境車的距離比實際更近一點,這樣可以在很安全的距離引導被控車進行換道.τ的值由相對速度決定,相對速度越大,τ值越小.圖3是不同τ值下的縱向勢場值變化曲線.實驗發現,當相對速度小于5m/s時,τ取0.5左右比較合適.

環境車的整體勢場可在其縱向勢場的基礎上向外延伸,并采用一個類高斯函數[14]的形式進行計算.因此,環境車的總勢場計算方式如下:

其中,σcar表示環境車勢場的收斂系數,決定了橫向的影響范圍,根據車道寬來決定.d表示橫向距離,如圖2所示,當計算點處于區域1,2,3時,d值分別取d1,d2,d3.圖4為環境車周圍勢場的分布示意圖,該環境車在坐標(50m,0)點處,與被控車之間的相對速度為3m/s.

圖3 環境車的縱向勢場值隨距離變化圖Fig.3 Longitudinal potential of the environment vehicle with respect to the relative distance

圖4 環境車的勢場示意圖Fig.4 Sketch of the environment vehicle's potential

1.3 目標點勢場

目標點勢場也稱為方向勢場,該勢場的作用是保證被控車朝車道前方行駛,即車輛前方的勢場值要低于后方的勢場值,勢場點離被控車越遠,勢場值就越小,勢場值與該距離成反比,但總是正值.采用的勢場函數如下:

其中,ε是正的勢場常數,κ是較小的勢場系數,目標點勢場可使被控車保持向前行駛的趨勢.

2 基于模型預測控制的軌跡跟蹤控制

行車環境的人工勢場建模為無人車輛規避風險提供了可供選擇的軌跡,在此基礎上,本文通過模型預測控制器中的優化算法對軌跡進行決策,以規劃出最優軌跡,并進行控制輸出,實現無人車輛軌跡規劃與跟蹤的統一建模.

2.1 車輛動力學建模

在設計模型預測控制器之前,首先選擇單軌模型來進行車輛動力學建模,具體如下:

其中,m為車輛質量,u,v,r分別表示縱向速度、橫向速度和橫擺角速度.Iz表示車輛繞z軸的轉動慣量,分別表示車輛所受的縱向力、橫向力、橫擺力矩.X,Y表示車輛在大地坐標系下的位置,?表示車輛的橫擺角.

假設車輛為前輪轉向,并且考慮到車輛的前輪轉角比較小,可得

其中,Fcf,Fcr分別表示前后輪受到的側向力;a,b分別表示前、后軸距.輪胎的縱向力和側向力可分別表示為以輪胎側偏角、滑移率、路面附著系數與垂向載荷為參數的函數.若路面附著系數已知,垂向載荷為

由于前輪轉角較小,根據線性輪胎模型可得

其中,Cf,Cr分別為前后輪側偏剛度,αf,αr分別為前后輪側偏角.側偏角的計算如下:

其中,β為質心側偏角.側偏剛度可由輪胎側向力與側偏角的關系曲線獲得,在側偏角比較小的時候,兩者近似為線性關系.

綜上所述,可得車輛動力學模型如下:

其中,

在該狀態空間系統中,狀態量選取為[X,Y,?,u,v,r]T,控制量選取為[ax,δ]T.

2.2 模型預測控制器設計

一般地,模型預測控制器主要包括預測模型的建立、目標函數的設計和約束條件的確定三部分.其中,預測模型是基礎,主要作用是根據對象的歷史信息和未來的控制輸入序列,預測出系統未來的輸出.為了能夠預測車輛在未來一個時域內的狀態,需要將連續的車輛動力學模型進行離散化,得到離散化后的預測模型.

其中,控制量是車輛的縱向加速度和前輪轉角.為實現無人車輛局部路徑規劃與跟蹤的統一建模,本文在考慮車速跟蹤、控制增量的同時,將行車環境勢場作為一部分增加到模型預測控制器的目標函數中,利用其優化算法實現軌跡規劃.該目標函數的形式如下:

其中,?U(t)表示t時刻下的優化控制輸入,x(t)表示當前時刻t下的狀態量,u(t?1)表示上一時刻的控制量,(t+i|t)表示在當前時刻t之后i步的預測值,Np表示預測時域,Nc表示控制時域,Vdes表示車輛在無障礙下的一個期望速度,Q,R,S分別代表各部分的權重.目標函數主要包括三部分:1)環境勢場,通過建立的車輛動力學模型結合勢場函數計算出預測時域內被控車所受的勢場值,勢場值越大,則行車風險越大,局部路徑的規劃主要取決于這一部分;2)為了保證車輛在沒有干擾的情況下,能夠維持當前的速度行駛;3)控制增量,控制增量最小,可避免車輛運動狀態出現大幅變化.

對于約束條件的設計,首先考慮對控制過程中的控制量和控制增量進行約束.根據現有汽車理論及相關文獻,縱向加速度一般取值為?0.4g～0.2g,前輪轉角一般取值在?25?～25?之間.由于在目標函數中采用了控制增量,因而也須對其進行約束,加速度變化率一般在?2g/s～2g/s之間,前輪轉角速度一般為?9.4?/s～9.4?/s.

同時,考慮到當車輛行駛在附著系數較低的路面時,須確保操縱穩定性.車輛的質心側偏角或輪胎側偏角是衡量車輛操縱穩定性的關鍵之一.文獻[15]分析了質心側偏角對車輛穩定性的影響,給出了判斷穩定性的準則.車輛在極限工況下之所以會失穩,主要是因為輪胎與地面產生的輪胎力達到飽和.輪胎力飽和與輪胎側偏角存在一定關系,因而可根據路面附著條件來約束輪胎側偏角.輪胎側偏角的計算方法如式(13),輪胎側偏角的約束如下:

其中,αmin和αmax分別為輪胎側偏角的下限和上限.根據輪胎模型,輪胎力飽和時輪胎側偏角與路面附著條件有關.一般地,低附著路面條件下的輪胎側偏角極限值應限制在 [?2?,2?].

因此,無人車輛軌跡規劃與跟蹤可統一描述為如下優化問題:

在每個控制周期內完成求解后,得到控制增量序列如下:

將上述控制增量序列的第1個元素,作為實際的控制輸出增量作用于系統.

3 實驗與驗證

3.1 實驗場景設計

無人車輛的行車環境動態多變,為簡化對本文涉及的模型預測控制器進行驗證,選取了3種典型交通場景,包括普通超車、相鄰車道有干擾的超車、跟車.如圖5所示,第1個場景假設超車時相鄰車道空閑,車輛可保持當前車速對前方慢速車輛進行超越;第2個場景假設車輛在超車時相鄰車道有其他環境車輛干擾,此時被控車須降速行駛并等待超車時機;第3個場景假設前方各車道都有慢速車輛,此時被控車只能減速并跟隨前方的慢速車輛.這三個場景都在較好的路面附著條件下完成.

圖5 各場景示意圖Fig.5 Sketch of each scenario

此外,進行車輛的操縱穩定性實驗.即在不同的路面附著條件下進行雙移線實驗.

3.2 實驗結果及分析

通過車輛動力學仿真軟件CarSim與MATLAB/Simulink的聯合仿真進行驗證,模型預測控制器及仿真車輛的參數如表1所示.

表1 控制器參數Table 1 Controller parameters

值得注意的是,對于無人車輛的縱向控制,控制器輸出的是車輛縱向加速度.實際過程中,應將縱向加速度轉化為縱向合力,根據車輛傳動系和制動系的逆模型,計算得到對應的節氣門開度和制動壓力.本文直接將縱向加速度輸出給CarSim軟件,并由該軟件自帶的速度調節器來調節車速.

圖6 各場景關鍵時刻勢場分布圖Fig.6 Potential distributions at key moments in several scenarios

圖7 各場景的被控車速度變化圖Fig.7 Speed variation diagram of the subject vehicle in several scenarios

圖6是各場景關鍵時刻勢場分布圖,圖7是各場景被控車速度變化圖.場景1為普通超車,前方有兩輛慢速行駛的環境車分處兩個車道.在此場景中,被控車在遇到慢速環境車進行換道時,相鄰車道一直存在足夠換道空間.圖6(a)為首次換道時的勢場分布,實心點表示當前被控車的位置,星號為預測軌跡.可見,預測軌跡開始偏向相鄰車道,被控車將要進行換道.圖7(a)是被控車的縱向速度隨時間變化圖,整個過程中的被控車速度始終維持在預定速度25m/s左右,只在兩次換道過程中有略微下降.

圖8 各場景的控制增量以及控制量變化圖Fig.8 Responses of the control increment and the control quantity variation in several scenarios

場景2是在相鄰車道存在干擾的超車.在此場景中,除前方有慢速行駛的環境車外,被控車的側方也有環境車干擾.由于受到側方環境車的影響,被控車在靠近前方慢速車輛時無法立即進行換道,只能先減速跟隨,等待相鄰車道有足夠安全的換道空間.圖6(b)為被控車周邊的勢場分布,從中可知,由于側方環境車2的勢場影響,預測軌跡并未偏向相鄰車道.當被控車減速行駛一段時間后,即相鄰車道有足夠安全空間時,被控車才加速并進行換道.此刻勢場分布與場景1類似,預測軌跡會偏向相鄰車道.圖7(b)是整個過程中被控車的縱向速度響應,被控車的縱向速度在經歷一段時間下降后,會在換道時機出現時加速,直至換道完成便上升至預定的車速.

場景3為普通跟車.在此場景中,本車道和相鄰車道的前方均有慢速行駛的環境車,因而被控車無法實施換道超車,只能降速并保持跟前方慢速行駛的環境車一致.如圖6(c)所示,由于相鄰車道環境車2的勢場影響,被控車在此時的預測軌跡也未偏向相鄰車道.圖7(c)是在該場景中的被控車縱向速度響應,在其行駛一段時間后,速度開始下降,直至與前方環境車1的車速一致,并勻速行駛.

圖8是控制量縱向加速度和前輪轉角的變化情況,左側從上到下依次是場景1、場景2、場景3中被控車控制量的變化情況,右側是控制增量變化情況.由圖8可知,控制量以及控制增量的變化范圍都在約束范圍之內.

在仿真過程中,被控車的其他運動狀態還包括軌跡、側向加速度、橫擺角與橫擺加速度,如圖9所示.圖9(a)是3個場景中的被控車軌跡圖,場景1中的被控車進行了兩次換道;場景2中的被控車進行了一次換道,且換道時間相比場景1的首次換道要晚,這是因為受側方環境車2的影響,一開始沒有足夠的安全換道空間;場景3中的被控車則一直在本車道行駛.

圖9 仿真場景中被控車的軌跡、側向加速度、橫擺角、橫擺角速度的響應圖Fig.9 Responses of the trajectory,lateral acceleration,yaw angle and yaw rate of the controlled vehicle in the simulation scenarios

同樣,被控車在3個場景中的運動變化也反映在側向加速度、橫擺角與橫擺角速度等車輛運動狀態的響應中(如圖9所示),即場景1中被控車的運動狀態會有兩次變化,這兩次變化的趨勢大致對稱,方向正好相反;場景2中被控車的運動狀態出現1次變化,時間較場景1中第1次變化晚;場景3中被控車的側向加速度、橫擺角、橫擺角速度則基本維持在0左右,表明被控車始終未進行換道.

場景4是不同附著系數路面的雙移線實驗.選取高附著路面(附著系數μ=0.9)以及低附著路面(附著系數μ=0.3)分別進行實驗,對比結果如圖10所示.

圖10 場景4的行車軌跡和輪胎側偏角響應圖Fig.10 Responses of the trajectory and the slip angle in Scenario 4

在高附著條件下,輪胎側偏角最大值未超過4?,軌跡比較平順.但是,在低附著路面且未引入輪胎側偏角約束的條件下,車輛的行車軌跡在140米處有明顯抖動,且輪胎側偏角最大值超過了6?;當引入輪胎側偏角約束后,其值始終限定在?2?～2?,且車輛的雙移線行駛軌跡相比無約束情況明顯減少了抖動,有效防止出現輪胎力飽和的情況,提高了車輛行駛的穩定性.

4 結束語

本文提出一種針對無人車輛軌跡規劃與跟蹤控制的統一建模方法,通過人工勢場來描述行車環境,并將行車環境勢場引入到模型預測控制的目標函數中,以實現軌跡規劃與跟蹤的統一建模.由于在進行模型預測控制器設計時,采用了縱橫向耦合的車輛動力學模型,因而可以在無人車輛路徑規劃的過程中實現車輛的縱橫向控制.