未知環(huán)境下智能汽車軌跡規(guī)劃滾動(dòng)窗口優(yōu)化

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

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

0 引 言

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

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

為解決智能汽車在未知環(huán)境中生成全局最優(yōu)軌跡的問題，本文提出了一種基于滾動(dòng)窗口優(yōu)化的軌跡規(guī)劃方法。該方法在建立車輛模型和環(huán)境模型的基礎(chǔ)上，通過6次多項(xiàng)式生成平滑軌跡，利用滾動(dòng)窗口周期性地刷新窗口內(nèi)的環(huán)境信息，并根據(jù)車輛當(dāng)前狀態(tài)對(duì)軌跡進(jìn)行在線優(yōu)化。最后仿真驗(yàn)證了本文算法的有效性。

1 問題描述及分析

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

圖1 智能汽車工作環(huán)境描述Fig.1 Intelligent vehicle working environment description

1.1 路徑表示

圖2 車輛運(yùn)動(dòng)學(xué)模型Fig.2 Vehicle kinematic model

為了便于描述，建立了車輛運(yùn)動(dòng)學(xué)模型，如圖2所示，模型可表示為：

(1)

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

由式(1)可進(jìn)一步獲取狀態(tài)量之間的關(guān)系表達(dá)式：

(2)

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

?

(3)

式中：x0、xf分別為慣性坐標(biāo)系下路徑起始點(diǎn)和目標(biāo)點(diǎn)的橫坐標(biāo)值。

考慮車輛在軌跡過程中的平穩(wěn)性以及參數(shù)的求解效率，采用具有連續(xù)曲率的多項(xiàng)式軌跡模型，將路徑曲線表示為路徑上任一點(diǎn)的縱坐標(biāo)y關(guān)于橫坐標(biāo)x的六次多項(xiàng)式函數(shù)：

y=a0+a1x+a2x2+a3x3+

a4x4+a5x5+a6x6

(4)

x0≤x≤xf

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

在每一時(shí)刻，若以車輛當(dāng)前位置為初始位置進(jìn)行軌跡規(guī)劃，聯(lián)立式(1)～(4)可解得a0～a5，其表達(dá)式為：

[a0a1a2a3a4a5]T=

(Bk)-1(Yk-Aka6)

(5)

(6)

(7)

(8)

式中：xk為車輛當(dāng)前位置的橫坐標(biāo)值，初始狀態(tài)下xk=x0。

y=[1xx2x3x4x5x6]

(9)

1.2 軌跡規(guī)劃的約束條件

汽車軌跡規(guī)劃的兩類最重要的約束為路徑可行性約束和安全性約束[12]，可具體分為幾何約束和運(yùn)動(dòng)學(xué)約束、道路邊界約束以及車輛動(dòng)力學(xué)約束。

1.2.1 幾何約束和運(yùn)動(dòng)學(xué)約束

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

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

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

dis≥(ri+r0)?

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

(10)

Xa≤xp≤Xb

聯(lián)立式(9)(10)可得到利用多項(xiàng)式描述路徑的避障約束數(shù)學(xué)表達(dá)式：

(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 道路邊界約束

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

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

1.2.3 車輛動(dòng)力學(xué)約束

智能汽車控制系統(tǒng)是一個(gè)受非完整性約束的非線性系統(tǒng)，因此車輛軌跡規(guī)劃在滿足避障約束的同時(shí)還要滿足動(dòng)力學(xué)約束要求。當(dāng)車速一定時(shí)，行駛軌跡曲率與側(cè)向加速度成正比，車輛在實(shí)際運(yùn)動(dòng)過程中產(chǎn)生的側(cè)向加速度受到發(fā)動(dòng)機(jī)、輪胎、地面附著系數(shù)等多方面限制，同時(shí)為了保證行駛過程中駕駛員乘坐的舒適性，需要對(duì)最大側(cè)向加速度進(jìn)行約束，側(cè)向加速度約束如下所示：

(14)

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

受車輛轉(zhuǎn)向系統(tǒng)限制，其前輪轉(zhuǎn)向角變化范圍有限且變化過程連續(xù)進(jìn)行，由于任意時(shí)刻后軸中心點(diǎn)處的線速度始終垂直于后軸，因此轉(zhuǎn)向角約束可以被轉(zhuǎn)換成幾何約束來處理，由幾何關(guān)系易得：

tanφ=l/R

(15)

式中：R為轉(zhuǎn)彎半徑；l為前、后軸距。

因此，最大轉(zhuǎn)向角約束以及轉(zhuǎn)向角連續(xù)約束可以轉(zhuǎn)化為對(duì)轉(zhuǎn)彎半徑的約束，其數(shù)學(xué)表達(dá)式為：

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

(16)

(17)

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

1.3 目標(biāo)函數(shù)

本文以路徑最短作為性能指標(biāo)，因此選取距離函數(shù)作為目標(biāo)函數(shù)，通過求解目標(biāo)函數(shù)在約束條件中的極值來確定點(diǎn)集的運(yùn)動(dòng)方程，最終使迭代路徑點(diǎn)集趨于最優(yōu)規(guī)劃路徑[13]：

(18)

式中：(xk,yk)為車輛當(dāng)前位置的坐標(biāo)。

2 基于滾動(dòng)窗口優(yōu)化的軌跡規(guī)劃算法

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

2.1 滾動(dòng)窗口優(yōu)化原理

障礙物未知環(huán)境中，移動(dòng)車輛只能通過傳感器獲取到靜態(tài)的局部環(huán)境信息，充分利用這些信息便可以實(shí)現(xiàn)一個(gè)局部環(huán)境的軌跡規(guī)劃，若干個(gè)這樣的局部規(guī)劃前后銜接就可以完成一項(xiàng)全局軌跡規(guī)劃任務(wù)，其原理如圖5所示。

圖5 滾動(dòng)窗口原理Fig.5 Rolling window principle

圖6 目標(biāo)點(diǎn)的選取Fig.6 Target point selection

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

2.2 約束簡化

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

(19)

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

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

2.3 速度規(guī)劃

通過上述規(guī)劃方法可以得到滿足動(dòng)力學(xué)約束、初始狀態(tài)及目標(biāo)狀態(tài)約束的路徑，為了保證車輛安全行駛,需要規(guī)劃車輛在該路徑上的行駛速度，即在車輛不失穩(wěn)的情況下盡快通過該路徑。為保證車輛在任意時(shí)刻都可以安全停車，使每個(gè)時(shí)刻下的終止車速為0，主要分為以下3個(gè)步驟：

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

(20)

式中：μ、g分別為靜摩擦因數(shù)以及重力加速度；l為軸距；κ為不同路徑點(diǎn)對(duì)應(yīng)的曲率；vmax為最大行駛車速。

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

(21)

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

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

(22)

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

圖8 車速曲線Fig.8 Speed profile

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

2.4 軌跡規(guī)劃算法框架

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

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

3 仿真驗(yàn)證

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

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

如圖10所示，軌跡規(guī)劃模塊在線接收障礙物信息和當(dāng)前車輛狀態(tài)信息，通過滾動(dòng)窗口優(yōu)化算法得到最優(yōu)可行軌跡，并結(jié)合設(shè)定的預(yù)瞄時(shí)間計(jì)算最優(yōu)側(cè)向加速度和縱向期望車速，駕駛員模型根據(jù)期望值決策出方向盤轉(zhuǎn)角以及踏板開度并輸入給CarSim車輛模型。

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

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

圖11 蒙特卡洛賽道規(guī)劃結(jié)果Fig.11 Result of Monte Carlo circuit

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

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

圖12 前輪轉(zhuǎn)角變化曲線Fig.12 Change curve of front wheel angle

圖13 車速與軌跡曲率關(guān)系Fig.13 Relationship between speed and curvature

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

圖14 多障礙物環(huán)境中車輛運(yùn)動(dòng)軌跡Fig.14 Vehicle trajectory in multi-obstacle environment

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

4 結(jié)束語

參考文獻(xiàn)：

[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] 郭孔輝,馬鳳軍,孔繁森. 人-車-路閉環(huán)系統(tǒng)駕駛員模型參數(shù)辨識(shí)[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.