基于改進人工勢場法的路徑規劃決策一體化算法研究

袁朝春 翁爍豐 何友國 SHEN Jie 陳 龍 王 桐

(1.江蘇大學汽車工程研究院， 鎮江 212013； 2.密歇根大學迪爾本分校， 迪爾本 MI 48128)

0 引言

路徑規劃決策算法是無人駕駛汽車的重要理論依據之一[1-3]。車輛行駛環境復雜而多變，這對無人駕駛汽車的路徑規劃決策提出了很高的實時性要求。目前常用的路徑規劃算法包括人工勢場法、網格法、可視圖空間法等。人工勢場法采用虛擬的斥力與引力建立梯度力場，根據梯度力場矢量方向對車輛路徑進行規劃，該算法規劃的曲線平滑，計算量較小，但易陷入局部最優點[4]；網格法通過將當前環境劃分為系列網格，并通過該網格是否存在障礙物判斷網格的可通行性，以此規劃出一條可通行路線，網格法需要大量的計算量，限制了算法的實時性[5]；可視圖空間法將環境中障礙物抽象為多邊形，并繪制起點與終點之間的多條連線，并在此基礎上篩選最優路徑，該方法靈活性較差，在復雜變化環境下表現不佳[6]。

目前研究較多的路徑跟蹤算法有基于駕駛員預瞄模型的路徑跟蹤算法和基于模型預測控制算法的路徑跟蹤算法。駕駛員預瞄模型根據車輛未來的預估位置與目標位置的差距進行補償決策控制，主要分為單點與多點預瞄模型，能夠獲得較好的跟蹤控制效果[7-8]；模型預測控制算法通過設置不同的評價函數來確定路徑跟蹤過程中各參數量對車輛的重要程度，最終獲得理想的控制結果[9]。然而，現階段的路徑跟蹤算法環節需要較大的數據量與計算量，導致無人駕駛汽車的路徑規劃與決策控制環節存在著時滯，會引起檢測信息與真實行駛環境的偏差。

本文基于車輛行駛過程中的環境信息與運動學信息對行駛碰撞風險等級進行評估，并且基于車輛動力學因素及環境交通參與者狀態建立改進人工勢場模型，將無人駕駛汽車的路徑規劃與控制決策相結合建立控制與決策一體化算法，以期為無人駕駛汽車在復雜環境下的實時控制提供幫助。

1 車輛行駛環境安全性判定

車輛行駛環境動態復雜而多變。為了保證行駛過程中的安全性，本文基于車輛行駛運動學特性與車輛行駛環境特征，建立車輛橫、縱向安全模型，以綜合反映車輛行駛過程中的安全性。

1.1 縱向安全模型

為了反映車輛在行駛過程中的縱向安全特性，本文建立了一種基于道路、車輛特征的分級安全距離模型，以體現車輛與環境交通參與者的不同級別的碰撞風險，為車輛縱向速度的控制決策提供合理依據。

(1)直行工況

行駛過程中，車輛的實際制動能力受車輛性能參數、輪胎-路面摩擦因數和實際駕駛環境等的影響，假設車輛制動系統可以發揮道路的最大制動效能，則最大制動減速度主要受輪胎-路面摩擦因數和實際駕駛環境的影響。車輛行駛過程中，縱向方向上需保持的距離如圖1所示。

圖1 車輛縱向安全距離Fig.1 Longitudinal safety distance model

結合車輛運動學特征并綜合道路環境、車輛因素影響，利用車輛最大制動減速度、自車車速、前車車速、前車減速度等信息建立縱向分級安全距離模型[10-12]，為

(1)

(2)

式中Dbr1、Dbr2——1、2級安全距離閾值，m

vx——車輛當前行駛縱向速度，m/s

ar-max——車輛最大制動減速度，m/s2

vlf——當前車道前方交通參與者速度，m/s

alf——當前車道前方交通參與者加速度，m/s2

Tbr——自車制動響應時間，s

(2)彎道工況

如圖2所示，彎道行駛過程中，車輛的實際行駛路徑的距離與傳感器檢測的直線距離有所偏差。

圖2 彎道行駛工況示意圖Fig.2 Vehicle driving on a bend

傳感器的測量結果Sobstacle與車輛的預期行駛軌跡存在偏差，無法準確地描述自車行駛到前車位置過程中實際行駛的距離。為了保證安全判斷的準確性，本文按照圖3所示的方法對交通參與者與自車之間的距離作了修正計算。

圖3 車輛在彎道行駛工況中的修正距離Fig.3 Corrected distance on a bend

自車與前方交通參與者之間的修正距離為

(3)

式中S′obstacle——修正后自車與前方交通參與者距離，m

α——車輛行駛軌跡扇形的圓心角，(°)

δ——前輪轉角，(°)

l——前后軸距之和，m

R——車輛轉向半徑，m

根據前方交通參與者修正距離與分級安全距離模型分級閾值的關系，得到車輛目前行駛環境縱向安全級別為

(4)

1.2 橫向換道安全模型

車輛行駛過程中，不僅需要合理控制縱向速度以保證車輛不會與前方交通參與者發生碰撞，還需要在當前車道存在擁堵或靜止障礙物時控制車輛執行換道操作。為了保證換道過程中的安全性，建立了車輛橫向換道安全模型[13-15]。

換道過程中，為了保證車輛不會與前方車輛發生碰撞事故，結合無人駕駛汽車與前向環境交通參與者的運動學信息，建立前向換道防碰撞模型為

(5)

式中fjud——換道過程前方交通參與者碰撞風險判別參數

Tchange——換道所需時間，s

vf——鄰車道前方交通參與者車速，m/s

af——鄰車道前方交通參與者加速度，m/s2

Sremain——預留安全車距，m

Ssf——車輛與鄰車道前方障礙物縱向距離，m

同時，為了保證車輛不干涉相鄰車道后方車輛的正常行駛，結合無人駕駛汽車與后向環境交通參與者的運動學信息，建立了后向換道防追尾模型

(6)

式中bjud——換道過程后方交通參與者碰撞風險判別參數

vb——鄰車道后方交通參與者車速，m/s

ab——鄰車道后方交通參與者加速度，m/s2

當滿足條件：fjud>0和bjud>0時，車輛安全換道狀態參數Csafe為1，不滿足時，Csafe為0。

2 車輛7自由度動力學模型

為了更好地分析車輛動力學主要特征，作如下假設：直接將前輪轉角作為方向盤輸入，不考慮轉向系統影響；不考慮車輛垂直運動；假設車輛為剛性系統，忽略懸架系統；忽略側向風對車輛產生的影響。根據車輛特性，建立圖4所示的車輛7自由度動力學模型[16-18]。

圖4 7自由度車輛動力學受力示意圖Fig.4 Forces of seven degrees of freedom model

(1)動力學方程

7自由度車輛動力學模型包括2個平動(縱向、側向運動)和5個轉動(橫擺運動、4個車輪轉動)，動力學方程為

(7)

式中vy——橫向車速，m/s

r——橫擺角速度，rad/s

Fxi、Fyi、Fzi——輪胎縱向力、側向力、垂向力，i分別對應左前輪fl、右前輪fr、左后輪rl、右后輪rr，N

m——整車質量，kg

a、b——前后軸到質心的距離，m

Iz——整車繞Z軸的轉動慣量，kg·m2

tw1——前軸輪距，m

tw2——后軸輪距，m

Fw——車輛行駛過程中空氣阻力

(2)約束方程建立

采用魔術公式建立輪胎模型，考慮到車輛在行駛過程中由于加速或減速引起各車輪的垂直載荷偏移，建立各輪胎垂向載荷力求解公式為

(8)

式中hg——整車質心高度，m

各輪胎側偏角計算公式為

(9)

各車輪輪心在車輪坐標系下縱向速度為

(10)

式中vt_fl、vt_fr、vt_rl、vt_rr——輪胎坐標系下的輪胎縱向速度，m/s

(3)Simulink模型建立

如圖5所示，使用Simulink搭建7自由度整車動力學模型。

圖5 整車動力學模型Simulink框圖Fig.5 Simulink block diagram of vehicle dynamics model

3 路徑規劃決策一體化模型

局部路徑規劃算法的核心是在全局路徑規劃算法的基礎上尋找2個節點之間的行駛最優路徑[19]。現有算法需要先根據環境情況規劃一條行駛路線，通過控制器控制車輛在此路徑上進行跟蹤。然而，智能車輛行駛過程中，環境信息與車輛自身信息處于不斷的更新狀態，傳統路徑規劃決策算法在規劃層與決策層的脫離可能引起無人車控制的不穩定性。

在傳統路徑規劃與路徑跟蹤的基礎上，利用多類約束下的虛擬斥力與引力模型體現智能車在交通法規、交通事故風險、車輛動力學等方面所受的約束。用路徑規劃決策一體化模型代替傳統路徑規劃算法與路徑跟蹤控制器，為車輛提供合理的局部路徑的同時提供當前所需的執行機構的最優控制值，為復雜工況的智能車輛主動避撞規劃提供更加合理高效的方法。算法流程如圖6所示。

圖6 改進人工勢場算法流程示意圖Fig.6 Flow chart of improved artificial potential field algorithm

3.1 人工引力建模

人工虛擬引力代表智能車輛行駛過程中對其具有吸引作用的環境因素，主要有全局規劃虛擬引力與車輛換道虛擬引力，分別用于提供智能車輛行駛過程中克服環境阻力正常行駛所需的牽引力及換道操作對車輛的牽引作用。

3.1.1全局規劃虛擬引力建模

全局規劃路線虛擬引力用于提供車輛正常行駛所需的牽引力，其大小與當前行駛道路的最高限速、車輛在該路面行駛的滾動阻力系數以及車輛自身質量有關。該設計虛擬引力是為保證智能車輛在無交通參與者或交通參與者碰撞威脅級別為0級的道路上能夠在該虛擬引力的作用下持續加速，并在車輛速度與當前道路限速相同為止之后達到受力平衡狀態并保持勻速行駛，直到前方出現交通參與者或道路限速發生變化為止。而在前方道路有其他交通參與者時，車輛將根據該交通參與者對自車的威脅級別進行處理，當威脅級別處于1級時，全局規劃引力將僅提供保持車輛在當前速度下正常行駛的力，當威脅級別處于0級時，全局規劃虛擬引力為0，釋放車輛油門。

在無坡道的道路上，該虛擬引力表示為

(11)

式中μroll——車輛滾動阻力系數

A——車輛的等效迎風面積,m2

Cw——車輛的風阻系數

vlimit——車輛行駛最高速度,m/s

Kattract-plan——全局規劃虛擬引力系數

3.1.2智能車輛換道引力建模

智能車輛行駛過程中，當遇到存在前方交通參與者或其他阻礙車輛正常行駛的狀態時，車輛需要進行換道行駛。該引力為智能車輛的換道提供決策與執行依據。

車輛換道引力受當前行駛路面的滑動摩擦因數、車輛行駛速度、車輛已完成的換道距離、車輛質量等因素決定。車輛換道引力計算式為

(12)

式中Schange——車輛已完成的橫向位移距離，m

Slane1-2——車輛與所在車道及換行車道的中線距離,m

μslide——路面滑動摩擦因數

3.2 人工斥力建模

虛擬斥力代表車輛行駛過程中對車輛行駛具有排斥作用的環境因素，用于引導車輛規避交通參與者(車輛、行人、道路兩側灌木等)與車輛自身特性約束(動力學特性、運動學特性)。主要分為：前向交通參與者斥力與前方傳感器檢測盲區虛擬斥力。

3.2.1前向交通參與者斥力建模

前向交通參與者斥力用于表征前方交通參與者對于車輛產生的影響，能充分反映前方交通參與者對于自車的威脅，可以保證車輛在發生碰撞之前完成制動或減速。

該斥力與車輛當前行駛速度、車輛與前方交通參與者理論安全距離、當前路面附著力系數、整車質量、前方交通參與者運動速度有關。以建立的交通參與者篩選算法與車輛安全距離模型作為依據，對車輛運動學特性進行合理分析，建立前方交通參與者斥力模型為

(13)

式中Kr-obstacle——前向斥力系數

μroad——當前路面最大附著力系數

Srem——行駛過程中車輛與前方交通參與者預留安全距離，m

3.2.2前方傳感器檢測盲區斥力建模

傳感器檢測盲區斥力用于反映車輛在前方存在傳感器檢測盲區時的制動減速度特性。該模型根據傳感器檢測的盲區邊界，在該邊界的基礎上控制車輛進行制動減速，以保證車輛在該力的作用下能夠避免盲區內突然出現交通參與者引起制動距離不足而導致的交通事故。

該虛擬斥力主要由前方傳感器盲區邊界與自車距離、自車行駛速度、當前道路附著力系數確定，計算式為

(14)

式中Kr-blind——前方傳感器檢測盲區斥力系數

Sblind——盲區邊界與車輛當前距離，m

4 Carsim/Simulink聯合仿真分析

為了驗證提出模型的有效性與優越性，使用Carsim/Simulink建立了多個仿真工況，對算法有效性與時滯性進行了分析。

4.1 算法有效性驗證

仿真設車輛迎風面積為1.6 m2，風阻系數為0.3，質量為1 100 kg，車輛滾動系數為0.014，道路附著力系數設為0.85，方向盤與前輪轉角傳動系數為20。

4.1.1全局規劃虛擬引力模型驗證

為了驗證該模型的有效性，設置了不同的道路限速工況。工況1，車輛以初速度40 km/h在直線車道上行駛，將道路速度分別限制為60、80 km/h。車輛速度與加速度變化曲線如圖7所示。

圖7 車輛速度與加速度變化曲線(工況1)Fig.7 Diagram of vehicle speed and acceleration (condition 1)

由圖7可以看出，在全局規劃虛擬引力的作用之下，車輛在無交通參與者道路上行駛時將保持一個逐漸減少的加速度進行持續加速，并在車輛速度達到當前限速值之后保持勻速直線行駛。

工況2，車輛在直線車道上行駛，道路前1 km的限速為60 km/h,后續道路限速為80 km/h。車輛的速度與加速度變化曲線如圖8所示。

由圖8可以看出，在全局規劃虛擬引力的作用下，前半段道路上，車輛將持續加速行駛，向60 km/h的速度持續逼近，并在道路限速改變之后改變加速度，控制車速向80 km/h的速度逼近。

工況3，車輛在直線車道上行駛，道路前1 km的限速為80 km/h,后續道路限速為60 km/h。車輛的速度與加速度變化曲線如圖9所示。

由圖9可以看出，在全局規劃虛擬引力的作用下，前半段道路上，車輛將持續加速行駛，向80 km/h的速度持續逼近。道路限速改變之后，車輛的全局規劃虛擬引力小于車輛當前所受的行駛阻力的，車輛將減速，車速向60 km/h的速度逼近。

通過上述3種工況的仿真，可以看出，全局規劃虛擬引力可以引導車輛速度向當前路面的限速條件不斷逼近，直到車輛行駛速度達到當前道路允許的最大行駛速度。

圖8 車輛速度與加速度變化曲線(工況2)Fig.8 Diagram of vehicle speed and acceleration (condition 2)

圖9 車輛速度與加速度變化曲線(工況3)Fig.9 Diagram of vehicle speed and acceleration (condition 3)

4.1.2換道引力模型驗證

為了驗證該模型的有效性，設置了3種行駛工況，車輛分別以60、80、100 km/h的速度行駛，在換道引力與車輛行駛引力的綜合作用下進行換道操作，方向盤轉角變化、車輛側向加速度變化、車輛位置變化、車輛質心側偏角變化如圖10所示。

圖10 方向盤轉角、車輛側向加速度、車輛位置與車輛質心側偏角變化曲線Fig.10 Diagram of steering angle, lateral acceleration, vehicle position and side slip angle

如圖10所示，不同行駛速度下，車輛的方向盤轉角峰值隨車速的增加而減小；不同車速下，車輛側向加速度的峰值均可控制在1.5 m/s2以內，可保證良好的乘坐感受；車輛位置變化為平滑曲線，轉向過渡自然；車輛質心側偏角未超過0.2°，保證了良好的車輛穩定性。

4.1.3前向交通參與者斥力模型驗證

為了驗證該模型的有效性，針對不同的前方交通參與者場景與自車速度進行了仿真分析。設定前方交通參與者行駛速度為20 km/h，車輛與前方交通參與者初始距離為70 m，車輛以60、80 km/h的速度沿道路行駛，車輛速度、車輛與前方交通參與者距離變化曲線如圖11所示。

圖11 車輛速度及車輛與前方交通參與者距離變化曲線Fig.11 Diagram of vehicle speed and distance from obstacle ahead

如圖11所示，在車輛與前方交通參與者的距離小于安全距離模型所規定的閾值之后，此時，車輛前向交通參與者斥力大于道路行駛引力，車輛在合力的作用下減速，在車輛與前方交通參與者的距離大于安全距離模型所規定的閾值之后，車輛又會在道路行駛引力的作用下加速，最終，車輛與前方交通參與者的距離穩定在安全距離模型所允許的值附近。綜上，車輛前向交通參與者斥力可以保證行駛中的前向碰撞安全性。

4.1.4傳感器盲區斥力模型驗證

為了驗證該模型的有效性，設定了一種傳感器盲區突然出現交通參與者的工況，該工況為：車輛以60 km/h的速度在道路上行駛，前方100 m處車輛右前方有一傳感器盲區，傳感器盲區于5.5 s時突然出現交通參與者，分別在未考慮傳感器盲區斥力與考慮傳感器盲區斥力的基礎上分別進行仿真分析，車輛速度與加速度曲線如圖12所示。

由圖12可以看出，考慮盲區邊界條件的情況下，車輛會較早地對盲區風險做出預判而提前減速，在交通參與者出現后，車輛由于預判減速的存在，留有足夠的制動距離，從而規避碰撞事故的發生，而未考慮盲區的情況下，車輛在發現前方交通參與者之后才開始制動減速，制動距離過短，易導致碰撞事故的發生。

圖12 車輛與盲區障礙物距離、速度、加速度變化曲線Fig.12 Diagram of vehicle distance from blind area, vehicle speed and vehicle acceleration

4.2 算法時滯分析

仿真實驗所使用的運算平臺CPU處理器為Intel G4560，RAM為4 GB。Matlab版本為R2014a，Carsim版本為8.1。

選用基于MPC的軌跡規劃算法與基于MPC的路徑跟蹤算法作為參照對象，建立圖13所示的聯合仿真模型[20]，規劃量輸出頻率為50 Hz，設定仿真時間為5 s，使用基于Matlab中profile函數對算法進行了運行時間分析，重復進行10次，不計Carsim動力學模型耗時，得到的結果如表1所示。綜合多次實驗結果可知，該算法單次運行所需時間為0.06 s。

搭建圖14所示的基于改進人工勢場法的路徑規劃決策一體化算法模型，使用profile函數對其進行時滯分析。控制量輸出頻率為1 000 Hz，運行時間為5 s，重復10次的程序運行耗時數據如表2所示，由表2可知，單次規劃決策平均運行時間為5.5×10-5s。

由表1、2可知，本文提出的路徑決策規劃一體化算法在時滯方面表現更優。

圖13 基于MPC的軌跡規劃算法與基于MPC的路徑跟蹤算法模型Fig.13 Model of MPC-based trajectory planning algorithm and MPC-based path tracking

s

圖14 基于改進人工勢場法的路徑規劃決策一體化算法模型Fig.14 Integrated algorithm model of path planning and decision-making based on improved artificial potential field method

s

5 結論

(1)針對車輛行駛環境特征與自身運動學特征對智能車行駛過程中橫縱向風險進行了分析，綜合反映了智能車行駛過程中的環境安全性。

(2)針對智能車輛傳感器盲區作了盲區斥力分析，避免傳感器盲區帶來的車輛碰撞風險。

(3)提出了路徑決策規劃一體化算法的概念，根據車輛與環境信息建立了改進人工勢場模型，利用模型在規劃車輛行駛路徑的同時給出車輛控制量，減小了規劃決策過程中時滯的影響。