基于MPC的智能車輛路徑規劃與跟蹤控制

張麗霞,田 碩,潘福全,嚴濤峰,李寶剛

(青島理工大學 機械與汽車工程學院,山東 青島 266520)

0 引言

智能車輛是聚集了先進感知系統、規劃決策系統、跟蹤控制系統、執行系統、多輔助駕駛系統的1個綜合體[1]。目前,對路徑規劃與跟蹤控制已經進行了很多研究,但是在面對復雜工況和曲率較大的路況時,跟蹤控制的效果仍然不理想。

研究路徑規劃算法,實現智能汽車在不同環境下的精準避障,可為車輛提供一條安全、可靠的避障路徑[2-3]。文獻[4]在結合智能汽車動力學約束條件的基礎上,提出一種基于蟻群算法優化的最佳路徑設計,經過仿真驗證,該算法可以提高全局搜索能力,減少節點數量,有效地降低汽車行駛能耗。文獻[5]提出一種基于模糊控制的避障路徑規劃器,通過超聲波測距傳感器測出左、右、前三方的距離,將其作為模糊控制的輸入,以車輛轉向角作為輸出,建立模糊控制規則。文獻[6]在人工勢場基礎上建立目標點引力函數,并引入障礙物約束和車輛動力學約束,建立改進的人工勢場模型。文獻[7]在人工勢場模型中引入道路邊界斥力勢場來進行轉向避障路徑的規劃,并用橢圓形勢場代替了原來的斥力勢場。通過仿真試驗,證明了采用該方法可獲得安全的局部避障路徑。文獻[3]針對傳統人工勢場(artificial potential field,APF)算法易陷入局部極值的問題,引入Bug算法。通過搭建智能車實驗平臺證明了改進的APF-Bug混合算法可以有效提高智能車在行駛過程中的安全性。

路徑跟蹤控制主要通過控制車輛的前輪轉角和油門/剎車踏板,使車輛的行駛路徑與規劃路徑間的橫向偏差與橫擺角偏差盡可能小[8]。文獻[9]以期望航向和實際航向的偏差作為反饋,通過設計魯棒比例積分微分(proportional-integral-derivative,PID)控制器計算控制量。經過驗證,所提出的路徑跟蹤橫向控制策略跟蹤效果較好。文獻[10]建立14自由度車輛模型,提出了一種以8自由度車輛模型作為預測模型的模型預測控制(model predictive control,MPC)方法。從MPC控制器中生成最優車輪轉向角序列,并以曲率連續變化的曲線作為參考軌跡。文獻[11]建立了線性時變路徑跟蹤預測模型,并在目標函數中引入向量松弛因子,將路徑跟蹤問題轉化為帶軟約束的二次規劃問題。文獻[12]提出一種基于滑膜變結構的控制方法,分別引入積分滑膜項和模糊自適應項,用于車輛轉向時的車速和轉向角速度的控制。

綜合國內外研究發現,針對避障路徑規劃,多數使用單個規劃算法,得到的規劃效果不是很理想,如:蟻群算法的搜索效率不高,易受到局部最優解的影響[13];模糊控制在復雜的環境下,規則庫難以建立[14];人工勢場法具有效率高、適應性好等優點,但也存在局部最優等缺陷[15]。針對路徑跟蹤控制,PID控制雖然易于實現,但其抗干擾能力和魯棒性不強,存在對擾動敏感、誤差計算不合理等問題[16]。MPC控制方法雖然能夠補償模型誤差,控制較為精確,但MPC的權重系數為固定值,無法滿足某些工況對其精度和穩定性的要求[17]。綜上,本文采用基于MPC和改進的APF算法,既解決了傳統APF的缺陷,又在考慮車輛動力學特性下,有效地進行動態避障路徑規劃。同時根據車輛橫向偏差和參考路徑曲率,引入模糊控制對MPC橫向路徑跟蹤控制器權重系數進行優化,提高了跟蹤精度及穩定性。

1 車輛動力學模型

建立三自由度車輛動力學模型,如圖1所示。圖1中,坐標系X-O-Y為大地坐標系,坐標系x-o-y為車輛坐標系,x軸沿車輛縱軸向前為正,y軸垂直于車輛縱軸指向駕駛員左側為正。

根據圖1可得車輛動力學方程:

(1)

其中:m為整車質量,kg;vx為車輛縱向速度,m/s;vy為車輛橫向速度,m/s;φ為車輛橫擺角速度,rad/s;Fxf、Fxr為車輛坐標系下x軸方向前、后輪胎受到的力,N;Flf、Flr為前、后輪胎受到的縱向力,N;δ為前輪轉角,rad;Fyf、Fyr為車輛坐標系下y軸方向前、后輪胎受到的力,N;Fcf、Fcr為前、后輪胎受到的側向力,N;Iz為車輛繞z軸的轉動慣量,kg·m2;a為前軸到質心處的距離,m;b為后軸到質心處的距離,m。

輪胎的縱向力和側向力可用線性函數近似表示為:

(2)

其中:cli為輪胎縱向剛度,N/m;cci為輪胎側偏剛度,N/rad;si為輪胎滑移率;αi為輪胎側偏角,rad。

通過對以上各式進行簡化和推導,可得到基于小角度和線性輪胎模型假設條件下的車輛非線性系統動力學模型:

(3)

2 基于MPC和改進APF的避障路徑動態規劃器設計

針對傳統APF中存在的目標不可達、局部最優等缺陷,本文改進障礙物斥力勢場,添加道路邊界約束、速度斥力約束、車道引力約束等多個約束條件,建立改進APF;同時考慮到車輛動力學特性,結合MPC,設計了基于MPC和改進APF的避障路徑動態規劃器。

在路徑規劃過程中,將車輛簡化為質點。車輛行駛環境狀況中的目標點位置、障礙物位置和車輛自身位置均由感知層確定。車輛的位置為q(x,y)T,障礙物的位置為qob(xob,yob)T,目標點的位置為qg(xg,yg)T。

引力勢場函數可表示為:

(4)

其中:κ為引力勢場尺度因子;d(qg,q)為車輛與目標點間的歐幾里德距離,方向為車輛指向目標點,m。

在路徑規劃過程中,不僅考慮躲避障礙物,還要考慮車輛駛出道路邊界影響安全的因素。故建立道路邊界斥力勢場為:

(5)

其中:ηedge1、ηedge2為道路邊界斥力勢場增益系數;yl、yr分別為左右車道中心線的橫向位置,m;L為道路寬度,m。

在傳統人工勢場法中,障礙物處于其對車輛產生作用的最大距離區域d0內時,行駛車輛才受到斥力作用,但障礙物形狀多樣,固定的參數無法滿足實際行駛的場景,所以改進為橢圓形斥力勢場。因此本文建立改進的障礙物斥力勢場函數為:

(6)

在車輛的整個行駛環境中,不僅有靜態障礙物,還存在動態障礙物,在建立障礙物斥力勢場時,不僅要考慮被控車輛距障礙物的距離,還應考慮障礙物速度影響。圖2為被控車輛與障礙物的速度方向示意圖。

圖2 速度方向示意圖

速度斥力勢場函數可表達為:

(7)

其中:ηvel為速度斥力勢場尺度因子。

車輛在行駛和轉向避障時,主要沿著車道中心線行駛。本文以車道中心線為引力點建立車道引力勢場模型,以前方車道中心線上各點為引力點,構建車道引力勢場函數,其表達式為:

Ulane=kroad(|y-y0|2+|x-xd|),

(8)

其中:kroad為車道引力勢場增益系數;y0為車輛起始橫向位置坐標,m;xd為車輛轉向區域的最大縱向距離,m。

綜合上述分析,車輛在合力勢場作用下的勢能為:

Uall=Urep+Uroad+Uvel+Ulane+Uatt。

(9)

將改進APF建立的勢場模型函數引入到MPC的目標函數和約束中,建立基于MPC和改進APF的避障路徑動態規劃器。

首先將式(3)轉換為狀態空間方程:

(10)

設計的目標函數如下:

(11)

其中:λφ為輸出量權重矩陣;λU為環境勢場權重矩陣;λu為控制增量權重矩陣。

各約束條件的取值參考文獻[18]?；贛PC和改進APF的路徑規劃問題可轉化為以下帶約束問題:

(12)

通過對式(16)進行求解,可得到當前時刻的控制增量,進而確定當前時刻的控制量,如此循環迭代,得到預測時域內的最優路徑點序列。

3 基于模糊MPC的車輛橫向路徑跟蹤控制器設計

3.1 MPC控制器設計

根據文獻[19],建立如下目標函數:

(13)

結合目標函數與約束條件,路徑跟蹤控制問題轉化為求解如下帶約束的二次規劃問題:

(14)

最后通過對路徑跟蹤控制器的目標函數取最小,在每個控制周期內,可得到控制時域內一系列的控制增量序列,并將這一序列的第1個元素作為實際的控制輸入增量作用到系統上。如此循環迭代,實現車輛對期望軌跡的跟蹤。

3.2 模糊控制器設計

將橫向偏差和參考路徑曲率作為模糊控制的輸入量,權重系數qφ和Ru的修正量Δqφ和ΔRu作為模糊控制的輸出量,建立模糊規則。同時對于輸入量選擇三角形隸屬度函數,輸出量選擇高斯隸屬度函數,進而對MPC橫向路徑跟蹤控制器的權重系數進行優化。

首先,需對模糊控制輸入量進行歸一化處理,有:

(15)

(16)

其中:eY,max、eY,min為最大、最小橫向路徑偏差,m;kmax、kmin為最大、最小參考路徑曲率。

定義權重系數的調整算式為:

qφ=qφ0×100Δqφ;

(17)

Ru=Ru0×10ΔRu,

(18)

其中:qφ、Ru為模糊MPC控制器當前階段計算出的權重系數;qφ0、Ru0為MPC控制器的權重系數,Δqφ、ΔRu為權重系數當前階段的修正量。

表1 Δqφ模糊控制規則表

表2 ΔRu模糊控制規則表

經過模糊推理得出的結果不能直接作用于被控車輛,需經過反模糊化輸出精確值,從而對權重系數進行調節?？紤]到控制穩定性、抗干擾性和靈敏性的要求,本文采用面積重心法進行解模糊處理[20]。

為驗證模糊MPC路徑跟蹤控制器的控制性能,選擇權重系數為常量的MPC路徑跟蹤控制器和模糊MPC路徑跟蹤控制器進行仿真對比。選擇雙移線工況作為參考路徑,車速設定為60 km/h。不同附著系數的路面會提供輪胎不同的側向力,直接影響車輛的控制性能,因此本文分別選擇μ=0.85 的干燥路面和μ=0.40的濕滑路面2種不同工況進行仿真實驗,MPC路徑跟蹤控制器參數設置如表3所示。

表3 MPC路徑跟蹤控制器參數

測試工況一:車速60 km/h,路面附著系數0.85。

圖3為路面附著系數0.85,60 km/h車速下2種控制器的跟蹤結果對比。由圖3a可以看出:2種控制器控制的車輛都能夠較為精確地跟蹤參考路徑,但車輛在曲率較大的路段時,MPC路徑跟蹤控制器的最大橫向偏差約為0.350 m,而模糊MPC路徑跟蹤控制器的最大橫向偏差約為0.283 m,最大橫向偏差減少19.14%,這是由于曲率較大的路段難以跟蹤,模糊MPC路徑跟蹤控制器中的權重系數qφ和Ru變小,減小在該路段的橫向偏差,使得車輛能夠跟蹤參考路徑。由圖3b可知:在 40 m的縱向范圍內MPC控制器的橫擺角軌跡跟蹤最大偏差為 0.031 rad,模糊MPC控制器的最大偏差為 0.009 rad,因此模糊MPC控制器的軌跡跟蹤更準確。從圖3c和圖3d可以看出:模糊MPC路徑跟蹤控制器的最大橫擺角速度和前輪轉角雖然比MPC路徑跟蹤控制器的最大值略有增加,穩定性有所降低,但保證了車輛的行駛安全性;同時整個軌跡跟蹤過程中變化平緩,而且都處于約束范圍內,說明此時車輛的穩定性良好。

(a) 車輛行駛路徑

測試工況二:車速60 km/h,路面附著系數0.40。

圖4為路面附著系數0.40,60 km/h車速下兩種控制器的跟蹤結果對比。由圖4a和圖4b可以看出:由于輪胎與地面摩擦力的下降,MPC路徑跟蹤控制器和模糊MPC路徑跟蹤控制器的軌跡跟隨都存在一定的滯后性,在50 m的縱向范圍內MPC控制器的橫向偏差為0.651 m,模糊MPC控制器的橫向偏差為0.1 m,相比較減少了0.551 m。同時在40 m范圍內MPC控制器的橫擺角軌跡跟蹤最大偏差為 0.021 rad,模糊MPC控制器的最大偏差為 0.005 rad,由此模糊MPC控制器軌跡跟蹤精度更高。從圖4c與圖4d可以看出:模糊MPC路徑跟蹤控制器的橫擺角速度與前輪轉角變化范圍都因地面附著系數的減小而變大,但是沒有發生突變,曲線變化始終平順光滑,從始至終一直處于約束范圍之內,車輛行駛穩定性良好。

(a) 車輛行駛路徑

4 避障路徑動態規劃與跟蹤控制試驗驗證

將設計的路徑規劃器和路徑跟蹤控制器進行聯合,搭建避障路徑規劃與跟蹤控制聯合仿真模型,并進行動態障礙物不同速度下的避障路徑規劃與跟蹤試驗。

搭建的避障路徑規劃與跟蹤控制聯合仿真模型如圖5所示。

圖5 避障路徑規劃及跟蹤控制聯合仿真模型

基于MPC和改進APF的避障路徑動態規劃器參數如表4所示。

表4 基于MPC和改進APF的避障路徑動態規劃器參數

第1種工況距智能車輛約90 m處設有10 km/h的動態障礙物;第2種速度工況距智能車輛約130 m處設有20 km/h的動態障礙物;第3種工況距智能車輛約200 m處設有40 km/h的動態障礙物,沿著車道中心線行駛。動態障礙物不同速度下的避障路徑規劃及跟蹤行駛曲線見圖6,車輛行駛路徑與規劃路徑的橫向偏差見圖7。智能車輛先沿著車道中心線行駛,檢測到動態障礙物后,路徑規劃器規劃出向左換道的路徑,控制智能車輛開始向左側換道避障,此時3種速度工況下的動態障礙物位置分別為圖中紅色、黑色、綠色矩形框位置;避障換道后,系統規劃出沿著車道中心線,控制車輛沿著該車道中心線繼續行駛,而動態障礙物繼續沿著原車道車輛中心線繼續行駛。

圖6 動態障礙物不同速度下的避障路徑規劃及跟蹤行駛曲線

圖7 車輛行駛路徑與規劃路徑的橫向偏差

圖8為智能車輛與動態障礙物的相對距離與偏離車道中心線、橫擺角的關系曲線圖。從圖8a和圖8b可以看出:隨著動態障礙物速度的增加,智能車輛與障礙物間的相對速度越來越小,此時系統控制車輛跟蹤所規劃路徑的橫擺角也在減小。

(a) 偏離車道

圖9為動態障礙物不同速度下的路徑跟蹤結果圖。從圖9可以看出:障礙物速度為10 km/h時,車輛前輪轉角、橫擺角速度、質心側偏角、側向加速度的最大值分別為0.026 1 rad、0.071 7 rad/s、0.002 4 rad、0.518 m/s2;障礙物速度為20 km/h時,車輛前輪轉角、橫擺角速度、質心側偏角、側向加速度的最大值分別為0.023 8 rad、0.069 1 rad/s、0.023 9 rad、0.473 m/s2;障礙物速度為40 km/h時,車輛前輪轉角、橫擺角速度、質心側偏角、側向加速度的最大值分別為0.022 9 rad、0.062 7 rad/s、0.001 9 rad、0.404 m/s2,各參數曲線變化平穩,沒有出現較大波動,其值均處于合理范圍內,并且隨著障礙物速度的增加,車輛的前輪轉角、橫擺角速度、質心側偏角、側向加速度的值在減小。因為障礙物速度的增加,智能車輛與障礙物間的相對速度在減小,進而智能車輛受到的勢場作用減弱,致使系統規劃的路徑更為平滑,系統能夠平穩、精確地控制車輛進行路徑跟蹤。

(a) 前輪轉角

5 結束語

本文考慮智能車輛的路徑規劃,將改進APF建立的勢場模型函數引入到MPC的目標函數和約束中,設計了基于MPC和改進APF的避障路徑動態規劃器。經過仿真驗證分析,避障路徑規劃器在跟蹤規劃路徑過程中最大橫向偏差約為0.170 m,證明改進的APF和MPC能夠在動態障礙物環境下合理規劃出安全的避障路徑。同時隨著障礙物速度的增加,車輛的前輪轉角、橫擺角速度、質心側偏角、側向加速度的值在減小,因此基于MPC和改進APF的避障路徑動態規劃器規劃的路徑更為平滑,系統能夠平穩、精確地控制車輛進行路徑跟蹤。

本文針對智能車輛的跟蹤控制,建立了基于模糊MPC的車輛橫向路徑跟蹤控制器,與傳統MPC相比,模糊MPC路徑跟蹤控制方法可使控制輸出平滑,更加滿足車輛的穩定性,同時模糊MPC控制器在干燥路面時最大橫向偏差減少19.14%,在濕滑路面下橫向偏差減少0.55 m,因此在不同路面下模糊MPC能更有效協調路徑跟蹤準確性和橫向穩定。