宋春雷，張嘉軒，田曉春，徐建華，吳曉暉，張鈺榮

（1.北京理工大學(xué) 自動(dòng)化學(xué)院，北京 100081；2.北京自動(dòng)化控制設(shè)備研究所，北京 100074）

隨著傳感器和計(jì)算設(shè)備等相關(guān)技術(shù)的發(fā)展與進(jìn)步，自動(dòng)駕駛技術(shù)近些年也得到了飛速發(fā)展。軌跡規(guī)劃是自動(dòng)駕駛技術(shù)中關(guān)鍵的一環(huán)，依據(jù)感知模塊獲取周圍環(huán)境信息，規(guī)劃出一條時(shí)空平滑的、安全的、舒適的行駛軌跡。

目前常見的軌跡規(guī)劃算法主要包含兩類：基于離散優(yōu)化的軌跡規(guī)劃算法[1]和基于機(jī)器學(xué)習(xí)的軌跡規(guī)劃算法[2]?；跈C(jī)器學(xué)習(xí)的軌跡規(guī)劃算法往往具有端到端的特點(diǎn)，即將任務(wù)要求、車輛狀態(tài)、環(huán)境狀態(tài)等作為輸入，根據(jù)訓(xùn)練好的神經(jīng)網(wǎng)絡(luò)等獲得輸入輸出的映射關(guān)系，進(jìn)而直接獲得自動(dòng)駕駛汽車所需的軌跡規(guī)劃結(jié)果。常見的基于學(xué)習(xí)的軌跡規(guī)劃方法如深度學(xué)習(xí)[3]、強(qiáng)化學(xué)習(xí)[4]和深度強(qiáng)化學(xué)習(xí)[5]等，此類方法的缺點(diǎn)是模型可解釋性差，穩(wěn)定性不高以及無(wú)法處理未訓(xùn)練過(guò)的場(chǎng)景。因此基于離散優(yōu)化的軌跡規(guī)劃算法應(yīng)用更廣泛，其采用離散狀態(tài)方程描述被控車輛的運(yùn)動(dòng)學(xué)模型，并將車輛所在的橫向-縱向空間離散，根據(jù)代價(jià)函數(shù)從多條可行路徑中獲得最優(yōu)軌跡[6]。Chu 等人將空間離散為許多的路徑點(diǎn)，根據(jù)安全成本、平滑度和一致性等約束條件選擇出最優(yōu)的路徑點(diǎn)，然后利用三次多項(xiàng)式對(duì)離散點(diǎn)進(jìn)行擬合，從而得到平滑的最優(yōu)路徑[7]。但該方法只考慮了靜態(tài)障礙物的避障任務(wù)，而沒(méi)有考慮動(dòng)態(tài)障礙物對(duì)自動(dòng)駕駛汽車的影響。Wu 等人考慮了行人和其他車輛等動(dòng)態(tài)障礙物，根據(jù)駕駛員的舒適度和最小延時(shí)對(duì)路徑進(jìn)行優(yōu)化，并在仿真中驗(yàn)證了所提算法[8]，但該算法的缺點(diǎn)是在變道時(shí)對(duì)駕駛員的感受缺乏考慮。Martinsen 等人提出了一種混合軌跡規(guī)劃算法，將離散線路圖搜索和凸優(yōu)化相結(jié)合獲得了全局最優(yōu)的軌跡[9]，但由于其將障礙物考慮十分復(fù)雜，對(duì)幾何形狀描述較為精確，因此需要消耗很大的算力，規(guī)劃時(shí)間較長(zhǎng)，實(shí)時(shí)性較差。Jin 等人在傳統(tǒng)的離散優(yōu)化方法基礎(chǔ)上，引入安全檢查的概念，保證了無(wú)人車在極端行駛條件下的安全并且計(jì)算負(fù)擔(dān)降低[10]，但該算法給出的速度曲線并不平滑。王富奎在局部路徑規(guī)劃時(shí)將安全性、舒適性、擬人性等約束條件引入目標(biāo)函數(shù)，并采用圖優(yōu)化的方法進(jìn)行求解[11]，但由于約束條件過(guò)于復(fù)雜導(dǎo)致規(guī)劃前期速度變化劇烈。文獻(xiàn)[12][13]采用采樣和優(yōu)化相結(jié)合的方法，解決了三維空間搜索計(jì)算量大的問(wèn)題，仿真結(jié)果表明所提算法的效率比較高且規(guī)劃時(shí)間是可接受的，但并未研究明顯加減速場(chǎng)景中的速度規(guī)劃問(wèn)題。

針對(duì)離散空間軌跡規(guī)劃問(wèn)題，本文提出一種基于優(yōu)化的規(guī)劃方法。該規(guī)劃方法可以使無(wú)人車得到安全無(wú)碰且曲率變化小的路徑規(guī)劃結(jié)果，以及讓乘客舒適且平滑連續(xù)的速度規(guī)劃結(jié)果。首先，將軌跡規(guī)劃問(wèn)題拆分為路徑規(guī)劃和速度規(guī)劃兩個(gè)模塊，將縱向-橫向空間（S-L空間）和縱向-時(shí)間空間（S-T空間）分別離散化后得到待規(guī)劃的區(qū)域。然后，基于動(dòng)態(tài)規(guī)劃（Dynamic Programming,DP）和二次規(guī)劃（Quadratic Programming,QP）在離散S-L空間中搜索出一條可以靜態(tài)避障的路徑。百度Apollo 提出的EMPlanner 在S-T空間中仍使用一般的動(dòng)態(tài)規(guī)劃，即對(duì)S-T空間中下一列的每個(gè)點(diǎn)均需搜索并計(jì)算，因此計(jì)算時(shí)間較長(zhǎng)[14]。本文在S-T空間中提出一種改進(jìn)的動(dòng)態(tài)規(guī)劃方法，對(duì)速度曲線進(jìn)行初步搜索。該方法根據(jù)道路速度限制和不可倒車約束改進(jìn)動(dòng)態(tài)規(guī)劃算法，并引入啟發(fā)函數(shù)加快對(duì)規(guī)劃終點(diǎn)的搜索速度，減少算法計(jì)算量，提高運(yùn)行效率并且實(shí)現(xiàn)動(dòng)態(tài)避障。最后，在Matlab/Simulink、Prescan 和Carsim 聯(lián)合仿真平臺(tái)對(duì)本文所提算法進(jìn)行驗(yàn)證，通過(guò)設(shè)計(jì)靜態(tài)障礙物和多種動(dòng)態(tài)障礙物驗(yàn)證算法的有效性和優(yōu)越性。

1 基于離散S-L 空間的路徑規(guī)劃

1.1 動(dòng)態(tài)規(guī)劃算法

對(duì)于車道行駛的場(chǎng)景，無(wú)人車完成任務(wù)需要全局路徑信息，而本文的研究重點(diǎn)是用于實(shí)現(xiàn)靜態(tài)避障和動(dòng)態(tài)避障的軌跡規(guī)劃，因此全局路徑信息可直接使用基于傳統(tǒng)算法（如A*、RRΤ、人工勢(shì)場(chǎng)等）規(guī)劃出的結(jié)果。將在全局路徑信息中提取出的參考線作為待規(guī)劃的S-L空間中心線[14]，根據(jù)所需路徑規(guī)劃長(zhǎng)度確定縱向采樣間隔，根據(jù)障礙物的大小確定橫向采樣間隔，從而獲得待規(guī)劃的離散S-L空間。由于無(wú)人車對(duì)靜態(tài)障礙物進(jìn)行避障時(shí)可以選擇從左繞行或從右繞行，因此該問(wèn)題并不是一個(gè)凸優(yōu)化問(wèn)題，存在多種解，非凸問(wèn)題示意圖如圖1 所示。而動(dòng)態(tài)規(guī)劃算法可以給出一個(gè)粗略的解，并且該解把非凸問(wèn)題轉(zhuǎn)化為凸問(wèn)題，進(jìn)而可以利用二次規(guī)劃求解器進(jìn)行求解。

圖1 非凸問(wèn)題示意圖Fig.1 Schematic diagram of non-convex problem

動(dòng)態(tài)規(guī)劃算法是將一個(gè)大問(wèn)題拆解為多個(gè)相同的小問(wèn)題，通過(guò)將每一個(gè)小問(wèn)題求解出最優(yōu)解進(jìn)而得到整體大問(wèn)題的最優(yōu)解，從而降低算法復(fù)雜度，提高算法效率。動(dòng)態(tài)規(guī)劃的示意圖如圖2 所示。

圖2 動(dòng)態(tài)規(guī)劃示意圖Fig.2 Schematic diagram of dynamic programming

無(wú)人車路徑動(dòng)態(tài)規(guī)劃問(wèn)題可以描述為在離散S-L空間中根據(jù)設(shè)定的代價(jià)函數(shù)，從規(guī)劃起始點(diǎn)逐步搜索代價(jià)最低的路徑點(diǎn)直到規(guī)劃終點(diǎn)，再?gòu)暮笙蚯氨闅v得到規(guī)劃的路徑。其中連接各個(gè)路徑點(diǎn)的線并不是圖2中的直線，而是根據(jù)初末狀態(tài)約束求得的最優(yōu)曲線。以連接路徑點(diǎn)(1,1)和(2,2)的曲線為例，初末狀態(tài)的約束如式(1)所示。

其中，g(s)為待定的最優(yōu)曲線；g′(s)和g′(s)分別為g(s)的一階和二階導(dǎo)數(shù)；s和l是離散S-L空間中路徑點(diǎn)的坐標(biāo)。

由式(1)可知，每段路徑的約束條件有6 個(gè)，根據(jù)泛函分析可知最優(yōu)的曲線形式應(yīng)該為五次多項(xiàng)式，因此每個(gè)路徑點(diǎn)之間都通過(guò)五次多項(xiàng)式連接。在確定路徑點(diǎn)之間的連接方式之后，需要根據(jù)代價(jià)函數(shù)計(jì)算每段路徑的代價(jià)，其中代價(jià)函數(shù)如式(2)所示。

其中，costcollison為碰撞代價(jià)；costsmooth為平滑代價(jià)；costref為參考線距離代價(jià)。上述三個(gè)代價(jià)的具體函數(shù)如式(3)-(5)所示。

其中，ω collision為碰撞代價(jià)權(quán)重；ω ref為參考代價(jià)權(quán)重；ωdl、ω ddl、ω dddl分別為路徑各階導(dǎo)數(shù)代價(jià)權(quán)重；mind為無(wú)人車距離最近障礙物的距離；l為路徑距參考線的距離，l′、l′、l′為l的各階導(dǎo)數(shù)。

由式(3)-(5)可知，無(wú)人車距障礙物越近、路徑越不平滑、距離參考線越遠(yuǎn)則代價(jià)越大。對(duì)于每個(gè)路徑點(diǎn)均需計(jì)算其與下一列所有路徑點(diǎn)之間的代價(jià)，并選取其中的最小值，然后記錄該最小值對(duì)應(yīng)的路徑點(diǎn)，重復(fù)上述步驟直到搜索到規(guī)劃終點(diǎn)，最后從終點(diǎn)反向索引即可得到動(dòng)態(tài)規(guī)劃的結(jié)果。

獲得動(dòng)態(tài)規(guī)劃結(jié)果后還需要生成凸空間，即確定無(wú)人車遇到障礙物時(shí)執(zhí)行左繞行還是右繞行。通過(guò)確定無(wú)人車在橫向空間的上下限lmin和lmax與障礙物在縱向空間中的位置lobs之間的關(guān)系確定繞行方式，即執(zhí)行左繞行時(shí)lmin＞lobs，而執(zhí)行右繞行時(shí)lmax＜lobs。由此確定了無(wú)人車?yán)@行障礙物的方式，并獲得了二次規(guī)劃所需的凸空間。

1.2 路徑二次規(guī)劃算法

根據(jù)動(dòng)態(tài)規(guī)劃獲得的結(jié)果和凸空間，可以利用二次規(guī)劃的方法對(duì)粗結(jié)果進(jìn)行平滑和優(yōu)化，進(jìn)而獲得最終的路徑規(guī)劃結(jié)果。二次規(guī)劃算法的特點(diǎn)是可以在凸空間中針對(duì)最小值問(wèn)題求解得到全局最優(yōu)解，因此在約束相較于動(dòng)態(tài)規(guī)劃更嚴(yán)格的情況下，二次規(guī)劃可以使路徑更加平滑、更加貼近全局路徑。帶約束的二次規(guī)劃問(wèn)題如式(6)所示。

其中，x為待求解的狀態(tài)變量，包括待求解路徑的s、l、l′、l′；A和b為碰撞約束所需的矩陣；Aeq和beq為jerk 約束所需的矩陣；xlb和xub為規(guī)劃起點(diǎn)的約束條件；H為參考線約束、路徑的斜率和曲率約束以及路徑末端約束；Τf為中心線以及期望終點(diǎn)的狀態(tài)矩陣。

在獲得上述約束之后，即可通過(guò)二次規(guī)劃求解器對(duì)該問(wèn)題進(jìn)行求解。

2 基于離散S-T 空間的速度規(guī)劃

2.1 改進(jìn)的動(dòng)態(tài)規(guī)劃算法

與路徑動(dòng)態(tài)規(guī)劃算法相似，改進(jìn)的動(dòng)態(tài)規(guī)劃算法也需要在離散空間中進(jìn)行。根據(jù)需要規(guī)劃的縱向距離和常見障礙物的大小確定縱向采樣間隔，根據(jù)規(guī)劃時(shí)間的長(zhǎng)短確定時(shí)間采樣間隔。與路徑動(dòng)態(tài)規(guī)劃不同的是，在S-L空間中進(jìn)行下一步路徑點(diǎn)的選擇時(shí)是任意的，而在S-T空間中下一個(gè)點(diǎn)的選擇需要考慮最大速度限制和不可倒車約束，即無(wú)人車在道路行駛過(guò)程中不得超過(guò)道路限速以及不能在行駛過(guò)程中倒車。根據(jù)這兩點(diǎn)特性，可以對(duì)動(dòng)態(tài)規(guī)劃算法進(jìn)行優(yōu)化，減少動(dòng)態(tài)規(guī)劃的計(jì)算量。改進(jìn)的動(dòng)態(tài)規(guī)劃算法在離散S-T空間中搜索的示意圖如圖3 所示。

圖3 改進(jìn)動(dòng)態(tài)規(guī)劃搜索示意圖Fig.3 Improved dynamic programming search diagram

圖3 表明搜索范圍是紅線和綠線之間，其中綠線代表對(duì)無(wú)人車的不可倒車約束，即搜索綠線以下部分意味著下一時(shí)刻車輛執(zhí)行倒車；紅線代表對(duì)無(wú)人車最大行駛速度的限制，即無(wú)人車的速度（S-T圖中的斜率）不可超過(guò)道路規(guī)定的最大速度。搜索點(diǎn)的數(shù)量由縱向采樣間隔、時(shí)間采樣間隔和道路限速?zèng)Q定，其計(jì)算公式為：

其中，n為搜索點(diǎn)數(shù)量；speedmax為道路限速；ΔT為時(shí)間采樣間隔；ΔS為縱向采樣間隔。

在確定了搜索點(diǎn)數(shù)量后，根據(jù)設(shè)計(jì)的代價(jià)函數(shù)計(jì)算當(dāng)前點(diǎn)到下一列點(diǎn)的代價(jià)最小值，并記錄最小值對(duì)應(yīng)的點(diǎn)，重復(fù)上述步驟直到搜索到規(guī)劃終點(diǎn)。與路徑規(guī)劃不同的是，速度規(guī)劃中各點(diǎn)之間的連接并不需要為五次多項(xiàng)式，只需為直線段即可。因?yàn)闀r(shí)間采樣間隔較小，短時(shí)間內(nèi)可以認(rèn)為無(wú)人車在一個(gè)采樣間隔內(nèi)做勻速運(yùn)動(dòng)，故而采用直線段連接。各點(diǎn)連接的代價(jià)函數(shù)也與路徑規(guī)劃算法不同，速度規(guī)劃中的代價(jià)函數(shù)除了計(jì)算到下一點(diǎn)的代價(jià)，還引入了下一點(diǎn)到搜索終點(diǎn)的啟發(fā)函數(shù)，具體形式如式(8)所示。

其中，costobs為障礙物代價(jià)；costaccel為加速度代價(jià)；costrefv為參考速度代價(jià)；ωH為啟發(fā)函數(shù)的權(quán)重；H(s,t)為節(jié)點(diǎn)到規(guī)劃終點(diǎn)的啟發(fā)函數(shù)。

其中，m為傳感器探測(cè)到的障礙物數(shù)量；d(i)min為無(wú)人車到第i個(gè)障礙物的最小距離；ω obs為障礙物代價(jià)權(quán)重。

當(dāng)無(wú)人車距障礙物最近距離大于1.5 m 時(shí)，障礙物代價(jià)為0；小于等于0.5 m 時(shí)，障礙物代價(jià)為恒值；其余情況障礙物代價(jià)隨距離增大而減小。

加速度代價(jià)為：

其中，a為當(dāng)前無(wú)人車的加速度，其具體范圍與真車實(shí)際參數(shù)保持一致；ω accel為加速度代價(jià)權(quán)重。當(dāng)加速度超過(guò)車輛動(dòng)力學(xué)限制時(shí)，加速度代價(jià)急劇增大。

參考速度代價(jià)為：

其中，speed為無(wú)人車當(dāng)前速度；speedref為參考速度；ωvref為參考速度代價(jià)權(quán)重。無(wú)人車當(dāng)前速度越接近參考速度則代價(jià)越小。

當(dāng)前節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)的啟發(fā)函數(shù)為：

用數(shù)學(xué)知識(shí)解決數(shù)學(xué)問(wèn)題的意義不僅僅在于結(jié)果，更在于過(guò)程中能夠通過(guò)不斷的嘗試和探索體驗(yàn)到解決實(shí)際問(wèn)題的快樂(lè)。小學(xué)生由于年齡較小，比較好動(dòng)，注意力不能長(zhǎng)時(shí)間集中，但是對(duì)于生活中的事物卻有著很高的探索欲望，所以在解決問(wèn)題的時(shí)候應(yīng)讓學(xué)生通過(guò)多動(dòng)手、多嘗試、畫圖、逆向思維、先易后難等策略，使學(xué)生充分體驗(yàn)到“直觀”到“抽象”之間的過(guò)度與過(guò)程，引導(dǎo)學(xué)生進(jìn)行由動(dòng)作思維→形象思維→抽象思維的轉(zhuǎn)變過(guò)程，讓學(xué)生真正掌握題中的關(guān)系，進(jìn)而掌握新的解題策略。

其中，H(s)為縱向啟發(fā)函數(shù)；H(t)為時(shí)間啟發(fā)函數(shù)；scur為當(dāng)前縱向搜索點(diǎn)；s target為目標(biāo)縱向搜索點(diǎn)；s end為縱向搜索終點(diǎn)；tcur為當(dāng)前時(shí)間搜索點(diǎn)；ttarget為目標(biāo)時(shí)間搜索點(diǎn)；tend為時(shí)間搜索終點(diǎn)；ωs為以多個(gè)縱向距離為目標(biāo)點(diǎn)的啟發(fā)函數(shù)權(quán)重；ωt為以多個(gè)時(shí)間為目標(biāo)點(diǎn)的啟發(fā)函數(shù)權(quán)重；m為目標(biāo)點(diǎn)總數(shù)。

根據(jù)所需規(guī)劃的速度不同，可以調(diào)節(jié)sω和ωt的大小，以改進(jìn)不同的目標(biāo)點(diǎn)對(duì)啟發(fā)函數(shù)的影響。當(dāng)ωs較大時(shí)，無(wú)人車規(guī)劃出的速度更大；當(dāng)ωt較大時(shí)，則無(wú)人車規(guī)劃出的速度較為緩慢。

與路徑規(guī)劃相同，在獲得了改進(jìn)的動(dòng)態(tài)規(guī)劃結(jié)果后還需開辟凸空間，即確定無(wú)人車遇到動(dòng)態(tài)障礙物時(shí)是執(zhí)行加速超車還是執(zhí)行減速避讓。在某一固定時(shí)間段內(nèi)，執(zhí)行避讓時(shí)smin≤sobs，而執(zhí)行超車時(shí)smax≥sobs。由此確定了超車和避讓的策略，獲得了可以用于二次規(guī)劃的凸空間。

2.2 速度二次規(guī)劃算法

與路徑二次規(guī)劃算法相似，速度二次規(guī)劃算法的公式表達(dá)與式(6)相同，不同的是式中各約束矩陣的含義。其中x為待求解的縱向距離、速度、加速度；A和b為不可倒車約束矩陣；Aeq和beq為連續(xù)性約束矩陣；x ub和xlb為凸空間約束矩陣；H為參考速度代價(jià)、加速度代價(jià)、jerk 代價(jià)矩陣；f為參考速度矩陣。根據(jù)二次規(guī)劃求解器即可對(duì)速度二次規(guī)劃問(wèn)題求解。

在獲得了路徑二次規(guī)劃結(jié)果和速度二次規(guī)劃結(jié)果后，將所得路徑和速度根據(jù)順序進(jìn)行融合以獲得軌跡規(guī)劃結(jié)果。在完成一個(gè)規(guī)劃周期后，根據(jù)定位信息、上一周期的規(guī)劃結(jié)果以及控制周期，可以將上周期的軌跡和本周期的軌跡進(jìn)行拼接，進(jìn)而發(fā)送給控制模塊實(shí)現(xiàn)無(wú)人車的軌跡控制。路徑規(guī)劃和速度規(guī)劃的迭代流程圖如圖4 所示。

圖4 算法迭代流程圖Fig.4 Algorithm iteration flowchart

3 仿真及結(jié)果分析

3.1 仿真平臺(tái)

本文使用硬件配置為Intel 酷睿i5 12600KF 處理器，內(nèi)存為16 G。軟件使用Windows10 操作系統(tǒng)、Carsim2019.1 仿真平臺(tái)、Matlab2020a、Prescan8.5 以及Visual Studio2019，其中Carsim 是為了獲得無(wú)人車的數(shù)學(xué)模型，Matlab/Simulink 用于編寫具體的規(guī)劃決策程序以及搭建相關(guān)的定位模塊、控制模塊等，Prescan 用于設(shè)置具體的仿真環(huán)境如無(wú)人車、道路和障礙物等，Visual Studio 用于Carsim 和Simulink 聯(lián)合仿真的編譯。

3.2 靜態(tài)避障實(shí)驗(yàn)

本文采用Prescan 作為仿真環(huán)境，通過(guò)引入無(wú)人車模型、理想傳感器、道路模型以及障礙物模型等搭建基本仿真環(huán)境，通過(guò)設(shè)置障礙物的速度建立靜態(tài)和動(dòng)態(tài)避障實(shí)驗(yàn)環(huán)境。基本仿真環(huán)境如圖5 所示，其中無(wú)人車和理想傳感器固連，理想傳感器主要負(fù)責(zé)采集障礙物的速度和航向；綠色陰影部分為傳感器探測(cè)范圍，設(shè)置為半徑60 m 的圓形；紅色直線為根據(jù)全局規(guī)劃算法得到的全局路徑，即車道中心線。

圖5 基本仿真環(huán)境Fig.5 Basic simulation environment

算法中各代價(jià)權(quán)重、最大速度、距障礙物最小距離等參數(shù)的具體設(shè)置如表1 所示。障礙物設(shè)置為多個(gè)1 m×1 m×1 m 的立方體，無(wú)人車的參考速度設(shè)置為15 m/s，并且運(yùn)動(dòng)場(chǎng)景設(shè)置為無(wú)人車在Y 字路口轉(zhuǎn)彎時(shí)遇到多個(gè)障礙物。靜態(tài)避障結(jié)果如圖6 所示。

圖6 靜態(tài)避障實(shí)驗(yàn)結(jié)果Fig.6 Static obstacle avoidance test results

由圖6 可知，當(dāng)無(wú)人車初次探測(cè)到障礙物時(shí)，動(dòng)態(tài)規(guī)劃算法會(huì)規(guī)劃出一個(gè)十分安全但偏離全局路徑很遠(yuǎn)的結(jié)果，并且路徑的曲率較大。二次規(guī)劃的優(yōu)化給無(wú)人車規(guī)劃出一個(gè)比動(dòng)態(tài)規(guī)劃更平滑、更靠近全局路徑的結(jié)果并且也可以實(shí)現(xiàn)避障。當(dāng)完成靜態(tài)避障任務(wù)后，動(dòng)態(tài)規(guī)劃和二次規(guī)劃的結(jié)果重合，表明在無(wú)障礙物時(shí)二者的規(guī)劃結(jié)果一致。從圖6(f)可以看出，當(dāng)無(wú)人車在8 s 左右進(jìn)行右轉(zhuǎn)彎并遇到障礙物時(shí)，有明顯的減速過(guò)程，并且這個(gè)過(guò)程較為平滑；無(wú)人車在16 s后完成避障動(dòng)作，又以平滑的加速度將速度調(diào)整回參考速度15 m/s。當(dāng)無(wú)人車進(jìn)行避障動(dòng)作以及完成避障后進(jìn)行加速動(dòng)作時(shí)，由表2 可知，算法規(guī)劃出的路徑曲率的平均值很小，并且標(biāo)準(zhǔn)差小，說(shuō)明路徑不僅平滑，而且波動(dòng)變化較小，適合無(wú)人車的控制；當(dāng)無(wú)人車回到全局路徑且沒(méi)有障礙物阻擋時(shí)，算法規(guī)劃的路徑與車道中心線完全重合，因此其曲率很小，說(shuō)明路徑規(guī)劃結(jié)果非常平滑。

表2 靜態(tài)避障路徑平滑程度Tab.2 Smoothness of static obstacle avoidance path

3.3 動(dòng)態(tài)避障實(shí)驗(yàn)

動(dòng)態(tài)避障實(shí)驗(yàn)中障礙物的尺寸與靜態(tài)避障實(shí)驗(yàn)相同，不同的是障礙物具有速度，并且在道路兩邊往復(fù)穿梭。各障礙物距起點(diǎn)的距離和自身速度如表3 所示，表中各速度依次對(duì)應(yīng)典型的交通障礙如：行人、非機(jī)動(dòng)車、低速機(jī)動(dòng)車、高速機(jī)動(dòng)車等。動(dòng)態(tài)避障實(shí)驗(yàn)中的算法參數(shù)與靜態(tài)避障實(shí)驗(yàn)中的參數(shù)保持一致。動(dòng)態(tài)避障結(jié)果如圖7 所示。

表3 動(dòng)態(tài)障礙物參數(shù)Tab.3 Dynamic obstacle parameters

圖7 動(dòng)態(tài)避障實(shí)驗(yàn)結(jié)果Fig.7 Dynamic obstacle avoidance test results

由圖7 可知，當(dāng)無(wú)人車遇到第一個(gè)障礙物時(shí)，由于障礙物的速度較慢，無(wú)人車根據(jù)改進(jìn)的動(dòng)態(tài)規(guī)劃算法規(guī)劃出了一個(gè)超車的動(dòng)作，然后二次規(guī)劃使得這個(gè)結(jié)果更加遠(yuǎn)離障礙物，保證了無(wú)人車安全地實(shí)現(xiàn)超車。行駛至任務(wù)中段時(shí)，無(wú)人車再次遇到兩個(gè)障礙物，但根據(jù)障礙物的速度和位置，無(wú)人車判斷該障礙物無(wú)需超車或避讓，繼續(xù)保持原有速度即可安全通行，因此忽略了兩個(gè)障礙物的影響，并且以參考速度安全通過(guò)。當(dāng)無(wú)人車遇到最后一個(gè)障礙物時(shí)，由于障礙物的速度很快，無(wú)人車根據(jù)改進(jìn)的動(dòng)態(tài)規(guī)劃算法規(guī)劃出了一個(gè)減速避讓的動(dòng)作，然后二次規(guī)劃又規(guī)劃出了一個(gè)更低的速度，保證了無(wú)人車安全地執(zhí)行減速避讓動(dòng)作。從圖7(f)可以看出，超車時(shí)無(wú)人車從參考速度15 m/s 開始以較為平穩(wěn)的加速度加速至19.5 m/s，在完成超車動(dòng)作后又平穩(wěn)減速至參考速度。同樣，在執(zhí)行減速避讓動(dòng)作時(shí)速度變化也較為平穩(wěn)。將改進(jìn)DP 算法和傳統(tǒng)的DP 算法相對(duì)比，可得如表4 所示的結(jié)果。

表4 算法對(duì)比結(jié)果Tab.4 Algorithm comparison results

由表4 可知，兩種算法的平均加速度幾乎相當(dāng)，但本文提出的改進(jìn)DP 算法的加速度標(biāo)準(zhǔn)差更小，證明本文算法使無(wú)人車的速度變化更加平穩(wěn)。在算法運(yùn)行時(shí)間方面，考慮進(jìn)行一次動(dòng)態(tài)規(guī)劃所需的計(jì)算時(shí)間，并對(duì)完整的任務(wù)過(guò)程求其平均值得到單次規(guī)劃時(shí)間。與傳統(tǒng)的DP 算法相比，本文改進(jìn)DP 算法的運(yùn)算時(shí)間減少了77.13%，算法效率得到了提高并使得規(guī)劃更加滿足無(wú)人車實(shí)時(shí)性的需求。

4 結(jié)論

本文針對(duì)無(wú)人車軌跡規(guī)劃問(wèn)題，提出了基于優(yōu)化的離散空間軌跡規(guī)劃方法，使得無(wú)人車可以平穩(wěn)、安全、高效地完成任務(wù)。搭建了Matlab/Simulink，Prescan和Carsim 聯(lián)合仿真平臺(tái)，設(shè)計(jì)了靜態(tài)避障實(shí)驗(yàn)和動(dòng)態(tài)避障實(shí)驗(yàn)。仿真實(shí)驗(yàn)結(jié)果表明，本文提出的離散軌跡規(guī)劃方法具有軌跡曲率更小、速度曲線更平滑的優(yōu)點(diǎn)。與傳統(tǒng)的DP 算法相比，本文提出的改進(jìn)DP 算法的加速度標(biāo)準(zhǔn)差更小、單次規(guī)劃時(shí)間更短、算法效率更高，能夠滿足無(wú)人車的實(shí)時(shí)性要求。