未知環境下智能汽車軌跡規劃滾動窗口優化

張 琳，章新杰，郭孔輝，王 超，劉 洋，劉 濤

(1.吉林大學 汽車仿真與控制國家重點實驗室，長春 130022；2.長春孔輝汽車科技股份有限公司, 長春130012；3. 中國第一汽車集團公司 技術中心，長春 130011)

0 引 言

軌跡規劃是實現汽車智能化的關鍵技術之一，其主要任務是在有障礙物的行駛環境中找到一條從初始點到目標點的安全軌跡，并滿足所有約束條件。智能汽車的軌跡規劃問題具有如下特點：①不確定性：行車過程中，車輛只能通過傳感器或車載雷達在有限的探測區域中獲取局部信息，在探測未知環境時，障礙物的突然出現會增加軌跡規劃的難度。②多約束：智能汽車的軌跡規劃不僅要考慮車輛運動存在的幾何約束，還需要考慮非完整性約束以及車輛的動力學約束對軌跡規劃的影響。除此之外，道路邊界、交通規則以及駕駛員的駕駛習慣等也對軌跡規劃提出了更高的要求[1]。

針對多約束條件下智能汽車在未知環境中的軌跡規劃問題，傳統方法是將應用在機器人領域的軌跡規劃算法用于生成車輛軌跡，例如人工勢場法、模擬退火算法等[2-4]，這類方法具有結構簡單、計算量較小的優點，但算法難以處理動態變化的駕駛環境且容易陷入局部最優解，限制了軌跡規劃在智能汽車中的應用。為克服傳統方法的缺點，文獻[5,6]提出了基于學習和人工智能理論的規劃方法，例如神經網絡，遺傳算法等，車輛在行進中能夠不斷融合、分析探測到的信息，逐步建立起全局環境模型并進行一定程度的全局優化，但是這種方法信息冗余較多、計算量大，在實際應用中不能滿足實時性的要求。文獻[7-9]對上述方法進行了改進，如A*, D*等，這類方法通過一個估價函數來估計當前點到終點的距離，并由此決定搜索方向，可以根據車輛在移動中探測到的環境信息快速修正和規劃出最優路徑，具有一定的復雜環境自適應能力，但是沒有考慮車輛非完整約束的限制，影響了智能汽車的軌跡跟蹤效果。文獻[10,11]提出的快速搜索隨機樹(Rapidly exploring random tree, RRT)，采用樹結構解決復雜約束下的軌跡規劃問題，由于搜索速度快且適用于包含運動學和動力學約束的規劃，使得該算法在機器人領域得到了廣泛應用。但是RRT采用隨機采樣策略，導致生成的軌跡重復性不強，算法的隨機性還會導致生成的軌跡出現明顯的拐角或者繞遠，無法被車輛直接執行。

為解決智能汽車在未知環境中生成全局最優軌跡的問題，本文提出了一種基于滾動窗口優化的軌跡規劃方法。該方法在建立車輛模型和環境模型的基礎上，通過6次多項式生成平滑軌跡，利用滾動窗口周期性地刷新窗口內的環境信息，并根據車輛當前狀態對軌跡進行在線優化。最后仿真驗證了本文算法的有效性。

1 問題描述及分析

汽車的工作環境如圖1所示，在二維平面中(x0,y0)、(xf,yf)分別為車輛的起點位置和目標位置；i1,i2,…,in為分布在車輛周圍的靜態障礙物；Rs為車輛行駛過程中的探測半徑；Rs范圍內的環境為探測到的已知環境，Rs范圍外的環境為未知環境。為了簡化計算，采用圓形對行駛車輛及障礙物進行包絡。因此，可以將全局環境未知情況下的軌跡規劃問題定義為：已知車輛起始位姿、目標運動位姿以及探測到的環境信息，根據所給定的地圖規劃出一條滿足各項約束的最優行駛軌跡。

圖1 智能汽車工作環境描述Fig.1 Intelligent vehicle working environment description

1.1 路徑表示

圖2 車輛運動學模型Fig.2 Vehicle kinematic model

為了便于描述，建立了車輛運動學模型，如圖2所示，模型可表示為：

(1)

式中：(x,y)為車輛后軸中點坐標；φ為前輪轉角；θ為車身方向角；l為前后軸距；u1為車速；u2為前輪轉角速度。

由式(1)可進一步獲取狀態量之間的關系表達式：

(2)

結合式(1)(2)，并選取(x,y,θ,φ)為車輛的位姿信息，求解路徑曲線的邊界條件可表示為：

?

(3)

式中：x0、xf分別為慣性坐標系下路徑起始點和目標點的橫坐標值。

考慮車輛在軌跡過程中的平穩性以及參數的求解效率，采用具有連續曲率的多項式軌跡模型，將路徑曲線表示為路徑上任一點的縱坐標y關于橫坐標x的六次多項式函數：

y=a0+a1x+a2x2+a3x3+

a4x4+a5x5+a6x6

(4)

x0≤x≤xf

式中：a=[a0,a1,…,a6]為路徑曲線參數向量。

在每一時刻，若以車輛當前位置為初始位置進行軌跡規劃，聯立式(1)～(4)可解得a0～a5，其表達式為：

[a0a1a2a3a4a5]T=

(Bk)-1(Yk-Aka6)

(5)

(6)

(7)

(8)

式中：xk為車輛當前位置的橫坐標值，初始狀態下xk=x0。

y=[1xx2x3x4x5x6]

(9)

1.2 軌跡規劃的約束條件

汽車軌跡規劃的兩類最重要的約束為路徑可行性約束和安全性約束[12]，可具體分為幾何約束和運動學約束、道路邊界約束以及車輛動力學約束。

1.2.1 幾何約束和運動學約束

為保證車輛安全行駛，在行駛過程中需要與障礙物之間有一定的安全避障距離，以靜態障礙物為研究對象，避障約束示意圖如圖3所示。

圖3 避障約束示意圖Fig.3 Schematic of obstacle constraint

圖3中，分別用半徑為ri、r0的圓包裹行駛車輛和障礙物。(xi,yi)為包裹障礙物圓的圓心坐標；Xa、Xb分別為障礙物左邊界和右邊界橫坐標；(xp,yp)為路徑上任意一點坐標；dis為(xi,yi)到(xp,yp)的距離，若要使車輛避開障礙物，dis需滿足式(10)：

dis≥(ri+r0)?

(yp-yi)2+(xp-xi)2≥(ri+r0)2

(10)

Xa≤xp≤Xb

聯立式(9)(10)可得到利用多項式描述路徑的避障約束數學表達式：

(11)

式中：

h0,i(x)=[g(x)(Bk)-1Yk-yi]2+

(x-xi)2-(ri+r0)2；

h1,i(x)=2[x6-g(x)(Bk)-1Ak]×

[g(x)(Bk)-1Yk+yi]2；

h2(x)=[x6-g(x)(Bk)-1Ak]；

g(x)=[1xx2x3x4x5]。

(12)

(13)

1.2.2 道路邊界約束

在真實的行車環境中，需要對道路邊界進行識別并考慮道路邊界對軌跡規劃的影響。為了減小計算量，假設道路邊界為矩形，將道路邊界也視為一種障礙物，即保證車輛在行駛過程中不觸碰到道路邊界。用直徑為矩形最小邊長的圓對道路邊界進行包絡，并且將道路邊界視為障礙物，因此道路邊界約束被化簡為幾何約束。對道路邊界的處理如圖 4所示。

圖4 道路邊界處理Fig 4 Road boundary processing

1.2.3 車輛動力學約束

智能汽車控制系統是一個受非完整性約束的非線性系統，因此車輛軌跡規劃在滿足避障約束的同時還要滿足動力學約束要求。當車速一定時，行駛軌跡曲率與側向加速度成正比，車輛在實際運動過程中產生的側向加速度受到發動機、輪胎、地面附著系數等多方面限制，同時為了保證行駛過程中駕駛員乘坐的舒適性，需要對最大側向加速度進行約束，側向加速度約束如下所示：

(14)

式中：aymax為最大側向加速度，m/s2。

受車輛轉向系統限制，其前輪轉向角變化范圍有限且變化過程連續進行，由于任意時刻后軸中心點處的線速度始終垂直于后軸，因此轉向角約束可以被轉換成幾何約束來處理，由幾何關系易得：

tanφ=l/R

(15)

式中：R為轉彎半徑；l為前、后軸距。

因此，最大轉向角約束以及轉向角連續約束可以轉化為對轉彎半徑的約束，其數學表達式為：

κmax=1/Rmin=tanφmax/l

(16)

(17)

式中：κ為軌跡曲率；Rmin為最小轉彎半徑；κmax為最大曲率；φmax為最大轉向角。

1.3 目標函數

本文以路徑最短作為性能指標，因此選取距離函數作為目標函數，通過求解目標函數在約束條件中的極值來確定點集的運動方程，最終使迭代路徑點集趨于最優規劃路徑[13]：

(18)

式中：(xk,yk)為車輛當前位置的坐標。

2 基于滾動窗口優化的軌跡規劃算法

當環境信息已知時，離線的全局軌跡規劃可以使智能汽車在有障礙物的工作環境中尋找一條從給定起點到終點的可行路徑，使其在運動過程中能安全、無碰撞地繞過所有障礙物。然而，在許多情況下車輛只能探測到局部環境信息，本文提出了一種的基于滾動窗口優化的軌跡規劃算法。

2.1 滾動窗口優化原理

障礙物未知環境中，移動車輛只能通過傳感器獲取到靜態的局部環境信息，充分利用這些信息便可以實現一個局部環境的軌跡規劃，若干個這樣的局部規劃前后銜接就可以完成一項全局軌跡規劃任務，其原理如圖5所示。

圖5 滾動窗口原理Fig.5 Rolling window principle

圖6 目標點的選取Fig.6 Target point selection

將傳感器可以探測到的區域定義為視覺窗口，車輛當前位置到目標位置的區域定義為規劃窗口，如圖5所示。規劃窗口在每一步長下都向車輛行駛方向滾動，在滾動的每一步，根據探測到的局部信息，用啟發式方法生成優化子目標，在當前滾動窗口內進行局部軌跡規劃，隨著滾動窗口的推進，不斷取得新的環境信息，從而在滾動中實現優化與反饋的結合，故該算法能夠解決全局環境未知時的軌跡規劃問題。另外，規劃窗口目標位置的選取會影響滿足約束條件的算法的求解難度，如圖6所示，P為車輛的當前位置，在道路邊界約束下，f1f2為目標位置的選取范圍，若以直線上任意一點A～E為目標位置，則規劃出不同曲率的路徑。若以B、C為目標位置，將不滿足避障約束；若以A為目標位置，將不滿足側向加速度約束，通過判斷確定PD曲線為滿足多項約束條件的最短路徑。

2.2 約束簡化

智能汽車通過視覺系統檢測到多個障礙物，若將規劃窗口中的所有障礙物不做區分，并作為對軌跡的約束條件，將降低算法的效率。為克服這一缺點，本文引入了度量函數L(di)對障礙物進行分類。分類原則為：優先考慮距離車輛較近的障礙物對軌跡的安全約束，障礙物距離當前車輛越遠，障礙物對軌跡的約束越弱。度量函數表達式如下所示：

(19)

度量函數L(di)表示當前時刻規劃出的軌跡點與障礙物之間的安全距離。由于本文用圓對車輛和障礙物進行包絡，屬于較為保守的安全方案，因此當障礙物距離車輛較近(dmin≤di≤dmax)時，安全距離L(di)為0也能保證車輛與障礙物有一定距離；當車輛與障礙物較遠(di≥dmax)時，安全距離L(di)為負值且隨di線性變化(L(di)=k×di+b)，優點是在減弱較遠障礙物對車輛約束的同時，保證了車輛與該障礙物的安全距離隨著相對距離動態變化；當障礙物距離車輛足夠遠時，安全距離為Lmin且不發生改變，L(di)與di變化關系以及簡化約束原理如圖7所示。

圖7 L(di)與di變化關系Fig.7 Relationship between L(di)and di

2.3 速度規劃

通過上述規劃方法可以得到滿足動力學約束、初始狀態及目標狀態約束的路徑，為了保證車輛安全行駛,需要規劃車輛在該路徑上的行駛速度，即在車輛不失穩的情況下盡快通過該路徑。為保證車輛在任意時刻都可以安全停車，使每個時刻下的終止車速為0，主要分為以下3個步驟：

(1)只考慮前輪不側滑的條件下，計算最大車速約束：

(20)

式中：μ、g分別為靜摩擦因數以及重力加速度；l為軸距；κ為不同路徑點對應的曲率；vmax為最大行駛車速。

(2)計算車輛的在路段下的行駛距離D,即規劃出的路徑點的總長度：

(21)

式中：pk為第k個路徑點(xk,yk)，共有n個路徑點。

(3)計算巡航車速vcoast，假設車輛加速度恒定，則滿足下式：

(22)

式中：adecel為減速段的減速度；aaccel為加速度段的加速度；v0為當前車速；ε>0表示當前車速與最高車速限值vmax的接近程度，vmax-v0≥ε表示當前車速小于且遠離最高車速，可以執行加速-巡航-減速過程；tmin為最短巡航時間；vmax-v0<ε表示當前車速接近最高車速，需要執行減速-巡航-減速過程。規劃出的速度曲線如圖8所示。

圖8 車速曲線Fig.8 Speed profile

設車輛行駛該段路徑的總時間為ttotal,在vmax范圍內對式(22)中巡航車速vcoast進行遍歷，可得到不同vcoast對應的總時間ttotal,選擇ttotal最小值對應的巡航車速，由此確定滿足約束的車速曲線。

2.4 軌跡規劃算法框架

算法流程圖如圖9所示。算法分為主規劃、滾動窗口和尋優3個模塊。主規劃模塊通過對比車輛當前位置與終點位置坐標判斷是否需要啟動軌跡規劃算法。若啟動規劃算法，在檢測到障礙物的情況下根據車輛當前位姿信息以及環境信息進行軌跡尋優，并更新當前步長下的軌跡。滾動窗口模塊有兩個主要功能：一方面在車輛的行駛過程中更新窗口內的環境信息；另一方面，在規劃窗口滾動后，獲取當前車道滿足約束的目標點的位姿信息。如果無法規劃出滿足各項約束的路徑(例如無法避開障礙物)，需要緊急停車以保證車輛安全。

圖9 算法流程圖Fig.9 Algorithm flowchart

3 仿真驗證

通過Simulink-CarSim軟件搭建聯合仿真平臺，在CarSim中設置道路邊界、障礙物、車輛動力學參數及約束條件，利用Sfunction(C語言)編寫軌跡規劃模塊，并搭建最優預瞄駕駛員模型[15]。上述滾動窗口優化的軌跡規劃問題是一個典型的非線性優化問題，目前對該問題的求解通常轉化為非線性規劃問題。本文采用Matlab中非線性規劃求解器對此問題進行求解，模型原理如圖10所示。

圖10 軌跡規劃算法模型搭建Fig.10 Model of trajectory planning

如圖10所示，軌跡規劃模塊在線接收障礙物信息和當前車輛狀態信息，通過滾動窗口優化算法得到最優可行軌跡，并結合設定的預瞄時間計算最優側向加速度和縱向期望車速，駕駛員模型根據期望值決策出方向盤轉角以及踏板開度并輸入給CarSim車輛模型。

在蒙特卡洛賽道中驗證算法的可行性，設置道路寬度為7 m，路面附著系數為0.7，只允許車輛在道路邊界內行駛，故將道路邊界當做障礙物處理，車輛起始狀態和終止狀態分別為(x0,y0,θ0,φ0)=(0，0，0，0)和(xf,yf,θf,φf)=(-90，0，0，0)，車輛主要參數如下所示：軸距為2340 mm；長度為3550 mm;寬度為1495 mm;最小轉彎半徑為4.5 m; 極限前輪轉角為29.79°；最大縱向加速度為7 m/s2; 最高車速為85 km/h。

仿真采用微型車，車輛的對角線半長是1.92 m，包絡車輛的圓半徑設為2 m。用于檢測障礙物傳感器檢測半徑設為20 m，設aymax=4 m/s2時乘坐較為舒適[8]?；跐L動窗口優化的在線軌跡規劃結果如圖11所示。

圖11 蒙特卡洛賽道規劃結果Fig.11 Result of Monte Carlo circuit

如圖11所示，軌跡規劃算法能夠生成滿足車輛運動連續性要求的平滑軌跡，軌跡跟蹤最大側向誤差為0.3 m;全程平均誤差為0.12 m；最大車速跟隨誤差為1.5 m/s;全程平均誤差為0.8 m/s，故在目標工況內軌跡跟蹤效果較好。同時，車輛在與道路邊界保證一定安全距離的情況下選擇較近行駛路徑，例如直角彎中靠近道路內邊界行駛，典型情況如局部放大圖(Ⅰ)所示；U形彎道中,為保證不降低車速的情況下順利通過，則采用“切彎動作”，典型情況如局部放大圖(Ⅱ)所示；在較短距離內通過多個曲率較大彎道時，能夠考慮多個彎道的特點并規劃出可執行軌跡，避免了傳統規劃方法的“振蕩”現象，典型情況如局部放大圖(Ⅲ)和(Ⅳ)所示。

從圖12可以看出：在蒙特卡洛賽道中，前輪轉角隨時間變化較為平緩，在接近100和200 s時，車輛進入急轉彎，軌跡曲率分別為0.08、0.1 m-1，前輪轉角分別出現較大值-16.0428°、-14.8969°，但并未超過極限值29.7938°。

圖12 前輪轉角變化曲線Fig.12 Change curve of front wheel angle

圖13 車速與軌跡曲率關系Fig.13 Relationship between speed and curvature

為保證車輛不發生側滑，規劃出的車速隨軌跡曲率變化關系如圖13所示。當曲率較小時，車速較高，最高可達22 m/s(75 km/h)，典型情況如圖中軌跡長度為1200～1400 m段所示；當曲率由0增加到0.07 m-1時，車速迅速下降的合理范圍為4.7～10.6 m/s(17～38 km/h)，典型情況如圖中軌跡長度為1800～2400 m段所示；若軌跡曲率由較大值頻繁變化，考慮到行車安全，車速先下降到較低值，隨后根據曲率變化逐漸增加，典型情況如圖中軌跡長度為1800～2300 m段所示。為進一步驗證算法的避障功能，恒定車速下要求車輛躲避多個障礙物，試驗結果如圖14所示。

圖14 多障礙物環境中車輛運動軌跡Fig.14 Vehicle trajectory in multi-obstacle environment

結果表明，本文方法在傳感范圍有限的情況下，能夠規劃出曲率連續的避碰軌跡且滿足多個約束條件。滾動窗口的引入有效結合了優化和反饋機制，規劃出的軌跡接近全局最優，保證了全局的收斂性。

4 結束語

參考文獻：

[1] Ziegler J,Bender P,Dang T,et al. Trajectory planning for Bertha—a local, continuous method[C]∥Intelligent Vehicles Symposium Proceedings, Dearborn, MI, USA,2014:450-457.

[2] Turker T,Sahingoz O K,Yilmaz G. 2D path planning for UAVs in radar threatening environment using simulated annealing algorithm[C]∥2015 International Conference on Unmanned Aircraft Systems (ICUAS), Denver,CO,USA, 2015:56-61.

[3] Li G, Yamashita A, Asama H, et al. An efficient improved artificial potential field based regression search method for robot path planning[C]∥2012 IEEE International Conference on Mechatronics and Automation, Chengdu, China, 2012:1227-1232.

[4] Amiryan J,Jamzad M. Adaptive motion planning with artificial potential fields using a prior path[C]∥Rsi International Conference on Robotics and Mechatronics,Tehran,Iran,2015:731-736.

[5] Syed U A, Kunwar F, Iqbal M. Guided autowave pulse coupled neural network(GAPCNN) based real time path planning and an obstacle avoidance scheme for mobile robots[J]. Robotics and autonomous systems,2014,62(4): 474-486.

[6] Aamer N, Ramachandran S. Neural networks based adaptive approach for path planning and obstacle avoidance for autonomous mobile robot (AMR)[J]. International Journal of Research in Computer Applications and Robotics (IJRCAR),2015,3(12):66-79.

[7] Montemerlo M,Becker J,Bhat S,et al. Junior: the stanford entry in the urban challenge[J]. Journal of Field Robotics,2008,25(9):569-597.

[8] Dolgov D,Thrun S,Montemerlo M,et al. Path planning for autonomous vehicles in unknown semi-structured environments[J]. The International Journal of Robotics Research,2010,29(5):485-501.

[9] Yao J, Lin C,Xie X,et al. Path planning for virtual human motion using improved A*star algorithm[C]∥Seventh International Conference on Information Technology: New Generations,Las Vegas,NV,USA,2010:1154-1158.

[10] McNaughton M，Urmson C，Dolan J M，et al. Motion planning for autonomous driving with a conformal spatiotemporal lattice[C]∥2011 IEEE International Conference on Robotics and Automation (ICRA)，Shanghai,China,2011:4889-4895.

[11] Xanthidis M, Rekleitis I M,O′Kane J M. RRT+:fast planning for high-dimensional configuration spaces[J/OL]. [2017-04-02].https:∥www.researchgate.net/publication/312222541_RRT_Fast_Planning_for_High-Dimensional_Configuration_Spaces.

[12] González D,Pérez J,Milanés V,et al. A review of motion planning techniques for automated vehicles[J]. IEEE Transactions on Intelligent Transportation Systems,2016,17(4):1135-1145.

[13] Li Y，Shin B S. Internal Topology Based Flexible Shortest Path Planning Method for Indoor Navigation[M]. Berlin Heidelberg:Springer,2015:171-176.

[14] Yang J,Qu Z,Wang J,et al. Comparison of optimal solutions to real-time path planning for a mobile vehicle[J]. IEEE Transactions on Systems, Man, and Cybernetics—Part A: Systems and Humans,2010,40(4):721-731.

[15] 郭孔輝,馬鳳軍,孔繁森. 人-車-路閉環系統駕駛員模型參數辨識[J]. 汽車工程,2002,24(1):20-24.

Guo Kong-hui, Ma Feng-jun, Kong Fan-sen. Driver model parameter identification of the driver-vehicle-road closed-loop system[J]. Automotive Engineering,2002,24(1):20-24.