考慮車(chē)輛運(yùn)動(dòng)約束的最優(yōu)避障軌跡規(guī)劃算法*

楊 彬，宋學(xué)偉，高振海

（吉林大學(xué)，汽車(chē)仿真與控制國(guó)家重點(diǎn)實(shí)驗(yàn)室，長(zhǎng)春 130022）

前言

對(duì)于無(wú)人駕駛車(chē)輛而言，在工作空間中尋找出一條從初始狀態(tài)到目標(biāo)狀態(tài)的無(wú)碰撞軌跡是車(chē)輛自主行進(jìn)的重要環(huán)節(jié)［1］。近年來(lái)，國(guó)內(nèi)外專(zhuān)家學(xué)者針對(duì)軌跡規(guī)劃問(wèn)題進(jìn)行了廣泛研究［2-3］。然而，受到車(chē)輛狀態(tài)、環(huán)境感知、定位技術(shù)的限制，很難找到一種計(jì)算效率高、實(shí)時(shí)性好、滿(mǎn)足汽車(chē)運(yùn)動(dòng)約束的軌跡規(guī)劃方法對(duì)車(chē)輛的可通行路徑進(jìn)行規(guī)劃和預(yù)測(cè)。

在各類(lèi)軌跡規(guī)劃算法中，圖解法［4-5］通常能夠較好地解決復(fù)雜工作空間中的避障軌跡規(guī)劃問(wèn)題，但其生成的軌跡通常不滿(mǎn)足車(chē)輛的運(yùn)動(dòng)約束，且計(jì)算效率受占位格密度和搜索空間維度的制約，很難用于車(chē)輛行駛過(guò)程的在線軌跡規(guī)劃。采樣法［6-7］在復(fù)雜的搜索空間中進(jìn)行大量采樣，能夠快速尋找出工作空間中的可通行路徑。然而，由于采樣點(diǎn)不可能覆蓋全部規(guī)劃空間，采樣法尋找最優(yōu)或次優(yōu)路徑的能力較差；隨機(jī)性強(qiáng)，相同搜索空間中可能會(huì)得到完全不同的結(jié)果。插值法［8-9］能夠在可行性、舒適性、車(chē)輛動(dòng)力學(xué)的約束下對(duì)路徑進(jìn)行描述，但容易忽視軌跡規(guī)劃過(guò)程中交通環(huán)境變化的問(wèn)題，應(yīng)對(duì)動(dòng)態(tài)的障礙物干擾的能力較差，常用于軌跡的平滑和優(yōu)化環(huán)節(jié)中。

狀態(tài)格（State Lattice）是一種基于圖和采樣的路徑規(guī)劃方法，在無(wú)人駕駛車(chē)輛的避障規(guī)劃問(wèn)題上得到了大量應(yīng)用，并在DARPA 城市挑戰(zhàn)賽中展現(xiàn)出較好的效果［10］。文獻(xiàn)［11］中通過(guò)離線表格生成初始化軌跡，并在線根據(jù)行為規(guī)劃層輸出的目標(biāo)位置對(duì)離線軌跡進(jìn)行優(yōu)化，但具有大分辨率和大分支因子的狀態(tài)格集合會(huì)導(dǎo)致計(jì)算效率低下。文獻(xiàn)［12］中引入了時(shí)空搜索圖的概念，避免了動(dòng)態(tài)環(huán)境下搜索空間引入時(shí)間維度后導(dǎo)致的狀態(tài)格數(shù)量爆炸的情況。文獻(xiàn)［13］中將搜索空間映射到低維度的變形空間中，并利用啟發(fā)式搜索的方式尋找可通行路徑，雖然對(duì)狀態(tài)格進(jìn)行了精簡(jiǎn)，但啟發(fā)式搜索的計(jì)算復(fù)雜度仍然較高，并不能從根本上降低狀態(tài)格算法的計(jì)算復(fù)雜度。文獻(xiàn)［14］中將多分辨率狀態(tài)格填充搜索空間，使?fàn)顟B(tài)格的分辨率完備，提升規(guī)劃效果的最優(yōu)性，并有效提升規(guī)劃速度。該方法在復(fù)雜全局地圖中的軌跡搜索問(wèn)題中有較好的表現(xiàn)，但在局部動(dòng)態(tài)軌跡規(guī)劃問(wèn)題上略顯復(fù)雜。文獻(xiàn)［15］中提出了基于衍生狀態(tài)格的避障軌跡規(guī)劃方法，考慮障礙物狀態(tài)和狀態(tài)格成本，但衍生狀態(tài)格的選擇較為單一，很難接近最優(yōu)解，同時(shí)在規(guī)劃過(guò)程中只考慮了道路曲率及軌跡長(zhǎng)度，未能很好地考慮車(chē)輛的運(yùn)動(dòng)狀態(tài)。

綜合考慮多種軌跡規(guī)劃方法的優(yōu)缺點(diǎn)，本文結(jié)合圖解法、采樣法和插值法提出了一種考慮車(chē)輛運(yùn)動(dòng)約束的避障軌跡規(guī)劃方法。在車(chē)輛的局部環(huán)境地圖中衍生出車(chē)輛可安全通行的終端狀態(tài)區(qū)域。并在衍生狀態(tài)區(qū)域中對(duì)超越障礙物的期望位置進(jìn)行采樣，將復(fù)雜道路環(huán)境中的避障軌跡搜索問(wèn)題轉(zhuǎn)換為自車(chē)與衍生狀態(tài)區(qū)域內(nèi)離散終端狀態(tài)點(diǎn)之間的軌跡規(guī)劃和尋優(yōu)問(wèn)題。車(chē)輛當(dāng)前位置與終端狀態(tài)點(diǎn)集之間通過(guò)基于車(chē)輛動(dòng)力學(xué)模型的Bézier 曲線規(guī)劃器進(jìn)行軌跡擬合，保證了局部軌跡是滿(mǎn)足車(chē)輛運(yùn)動(dòng)特性的。針對(duì)車(chē)輛在軌跡跟隨過(guò)程的操縱穩(wěn)定性和行駛平順性設(shè)計(jì)軌跡評(píng)價(jià)函數(shù)，并最終在避障軌跡簇中篩選出局部最優(yōu)軌跡。通過(guò)與State Lattice 算法和MPC 算法進(jìn)行對(duì)比，驗(yàn)證了該方法的操縱穩(wěn)定性和平順性。

1 車(chē)輛運(yùn)動(dòng)約束下的避障軌跡規(guī)劃算法

狀態(tài)格方法具有圖解法和采樣法的優(yōu)勢(shì)，能夠應(yīng)對(duì)動(dòng)態(tài)環(huán)境的變化，同時(shí)在計(jì)算負(fù)載上遠(yuǎn)小于圖解法。然而，狀態(tài)格方法一般在車(chē)輛前進(jìn)方向的一段結(jié)構(gòu)化搜索空間中進(jìn)行大范圍、大間隔的均勻采樣，導(dǎo)致規(guī)劃得到路徑很難得到最優(yōu)或次優(yōu)解，同時(shí)軌跡簇中存在許多冗余的無(wú)效解，產(chǎn)生大量不必要的運(yùn)算，嚴(yán)重影響著算法的計(jì)算效率。同時(shí)，避障軌跡多以多項(xiàng)式或圓弧段進(jìn)行擬合，很難根據(jù)搜索空間的環(huán)境變化和車(chē)輛運(yùn)動(dòng)變化對(duì)軌跡段進(jìn)行局部調(diào)整。本文針對(duì)上述問(wèn)題提出了如下改進(jìn)和優(yōu)化方案。

（1）車(chē)輛實(shí)際行駛過(guò)程中，最優(yōu)終端狀態(tài)點(diǎn)（如圖1 中紅色點(diǎn)所示）通常位于障礙物的側(cè)方區(qū)域。因此調(diào)整狀態(tài)點(diǎn)的搜索空間至理想終端狀態(tài)點(diǎn)附近（State Lattice 采樣方式如圖1 所示，本文采樣方式如圖2 所示），將搜索空間壓縮至障礙物位置和運(yùn)動(dòng)狀態(tài)推算出的衍生狀態(tài)區(qū)域（圖2 中黑色框所示）。在初始狀態(tài)點(diǎn)與終端狀態(tài)點(diǎn)集之間以車(chē)輛動(dòng)力學(xué)上可行的曲線進(jìn)行擬合，減少軌跡的冗余計(jì)算，并使結(jié)果更加靠近最優(yōu)解或次優(yōu)解。

（2）相比于圓弧或多項(xiàng)式的曲線擬合方法，Bézier 曲線可通過(guò)調(diào)整軌跡控制點(diǎn)實(shí)現(xiàn)曲線的快速局部修改。依靠車(chē)輛起始狀態(tài)點(diǎn)位置及終端狀態(tài)點(diǎn)位置進(jìn)行軌跡控制點(diǎn)的初步選擇，后根據(jù)搜索空間的環(huán)境變化和車(chē)輛的運(yùn)動(dòng)特性進(jìn)行軌跡局部適應(yīng)性的調(diào)整及軌跡篩選，可大幅提升避障軌跡的靈活性和環(huán)境適應(yīng)性。同時(shí)，Bézier 曲線規(guī)劃器在進(jìn)行曲線擬合時(shí)，只需提供起始點(diǎn)的位置和航向，通過(guò)改變中間控制點(diǎn)位置即可進(jìn)行避障安全性、車(chē)輛運(yùn)動(dòng)約束等調(diào)整，間接減小了狀態(tài)格的分支數(shù)量，降低了計(jì)算復(fù)雜度。

圖1 狀態(tài)格采樣示意圖

圖2 本文算法采樣示意圖

本文的技術(shù)路線如圖3 所示。以車(chē)輛開(kāi)始執(zhí)行避障操作的位姿作為初始狀態(tài)點(diǎn)，車(chē)輛完成障礙物超越的期望位姿作為終端狀態(tài)點(diǎn)。局部避障軌跡在初始狀態(tài)點(diǎn)與終端狀態(tài)點(diǎn)之間通過(guò)基于車(chē)輛運(yùn)動(dòng)模型的曲線規(guī)劃器擬合后得到。在車(chē)輛的局部環(huán)境地圖中，以障礙物的位姿為出發(fā)點(diǎn)衍生出車(chē)輛可安全通行的終端狀態(tài)區(qū)域。在該區(qū)域內(nèi)進(jìn)行終端狀態(tài)點(diǎn)的采樣并組成離散終端狀態(tài)點(diǎn)集。狀態(tài)點(diǎn)集中的每一個(gè)終端狀態(tài)點(diǎn)通過(guò)Bézier 曲線規(guī)劃器的軌跡擬合和參數(shù)修正后得到一條對(duì)應(yīng)的滿(mǎn)足車(chē)輛運(yùn)動(dòng)約束的避障軌跡。最終組成一組車(chē)輛可安全平穩(wěn)通行的避障軌跡簇?？紤]車(chē)輛在跟隨避障軌跡時(shí)的操縱穩(wěn)定性和行駛平順性，對(duì)軌跡簇中的軌跡進(jìn)行客觀評(píng)價(jià)，選擇一條符合條件的最優(yōu)軌跡作為最終的局部最優(yōu)避障軌跡。

圖3 本文算法技術(shù)路線

2 避障軌跡規(guī)劃器設(shè)計(jì)

避障軌跡在保證能夠安全通行的前提下，不僅需要考慮車(chē)輛的運(yùn)動(dòng)特性以保證軌跡可循，而且需要滿(mǎn)足一定的曲率約束和連續(xù)性條件，使車(chē)輛在循跡過(guò)程保持較好的操縱穩(wěn)定性和行駛平順性。

2.1 車(chē)輛動(dòng)力學(xué)模型

在車(chē)輛路徑規(guī)劃算法中考慮車(chē)輛運(yùn)動(dòng)約束，可以保證規(guī)劃出的路徑是車(chē)輛可實(shí)際跟隨的，使車(chē)輛在自動(dòng)駕駛的過(guò)程中能夠更精準(zhǔn)地跟隨規(guī)劃的路徑。同時(shí)，在軌跡規(guī)劃器中應(yīng)用車(chē)輛動(dòng)力學(xué)模型，對(duì)局部軌跡進(jìn)行篩選，可控制無(wú)人駕駛車(chē)輛維持較小的側(cè)向加速度和橫擺角速度的變化幅度。

車(chē)輛在地面運(yùn)動(dòng)的動(dòng)力學(xué)過(guò)程非常復(fù)雜，而無(wú)人駕駛車(chē)輛的規(guī)劃層中引入過(guò)于復(fù)雜的模型是不必要的。因此在運(yùn)動(dòng)學(xué)假設(shè)下引入簡(jiǎn)化的車(chē)輛單軌模型［16］，如圖4 所示。其中，［X，Y］為車(chē)輛在大地坐標(biāo)系下的坐標(biāo)；v為車(chē)輛質(zhì)心的中心速度，φ為車(chē)輛的航向角；φ?為車(chē)輛的橫擺角速度。在速度控制上，本文考慮恒定的縱向速度，因此縱向加速度為0。

圖4 車(chē)輛動(dòng)力學(xué)模型

假設(shè)車(chē)輛完全按照規(guī)劃軌跡段行駛，根據(jù)阿克曼轉(zhuǎn)向規(guī)律，可將車(chē)輛轉(zhuǎn)向控制量與軌跡段參數(shù)進(jìn)行聯(lián)系。

在車(chē)輛穩(wěn)態(tài)轉(zhuǎn)向和小角度輪胎側(cè)偏角的假設(shè)下，對(duì)車(chē)輛的運(yùn)動(dòng)方程進(jìn)行推導(dǎo)，由車(chē)輪轉(zhuǎn)向引起的橫擺角速度和側(cè)向加速度如式（2）和式（3）所示。

通過(guò)式（2）和式（3）可由軌跡參數(shù)進(jìn)行航跡點(diǎn)上的側(cè)向加速度和橫擺角速度的估算，用以在軌跡參數(shù)化的過(guò)程中進(jìn)行軌跡的車(chē)輛運(yùn)動(dòng)約束和評(píng)價(jià)。

2.2 離散終端狀態(tài)點(diǎn)集

避障軌跡可通過(guò)初始狀態(tài)點(diǎn)和終端狀態(tài)點(diǎn)的參數(shù)化擬合后得到。初始狀態(tài)點(diǎn)位于車(chē)輛開(kāi)始執(zhí)行避障操作處，終端狀態(tài)點(diǎn)則位于障礙物與行駛環(huán)境邊界之間，方向與路徑點(diǎn)處的道路曲率相關(guān)。障礙物與行駛環(huán)境邊界之間的可通行區(qū)域稱(chēng)為衍生狀態(tài)區(qū)域，在衍生狀態(tài)區(qū)域中完成采樣的多個(gè)可行的終端狀態(tài)點(diǎn)組成離散終端狀態(tài)點(diǎn)集。衍生狀態(tài)區(qū)域?qū)⒔K端狀態(tài)點(diǎn)的搜索空間壓縮為與障礙物推演位置和車(chē)輛可行駛區(qū)域相關(guān)的局部區(qū)域中，如圖5 所示。由于衍生狀態(tài)區(qū)域的生成考慮了自車(chē)與障礙物和道路邊界的安全距離，幾乎可以保證自車(chē)行駛至衍生狀態(tài)區(qū)域內(nèi)的終端狀態(tài)點(diǎn)的過(guò)程中不會(huì)與障礙物發(fā)生碰撞，因此避障軌跡在初始狀態(tài)點(diǎn)與離散終端狀態(tài)點(diǎn)集之間進(jìn)行擬合。

圖5 大地坐標(biāo)系下衍生狀態(tài)區(qū)域示意圖

終端狀態(tài)點(diǎn)在衍生狀態(tài)區(qū)域中進(jìn)行均勻采樣。衍生狀態(tài)區(qū)域位于障礙物矩形塊和可行駛區(qū)域邊界之間，其邊界與障礙物的側(cè)向間距（D′obs）、與可行駛區(qū)域邊界的側(cè)向間距（D′lane）均須大于車(chē)輛行駛的臨界安全車(chē)距［17］。大地坐標(biāo)系下衍生狀態(tài)區(qū)域示意圖如圖5 所示。為簡(jiǎn)化軌跡搜索和求解的難度，將大地坐標(biāo)系下衍生狀態(tài)區(qū)域、障礙物與車(chē)輛的相對(duì)位置轉(zhuǎn)換至Frenét框架下：

式中[Dlane，Dobs，L]為Frenét框架下的相對(duì)位置關(guān)系。Frenét 框架下，衍生狀態(tài)區(qū)域的縱向位置位于障礙物運(yùn)動(dòng)方向的側(cè)向，受交通規(guī)則的限制，在障礙物左側(cè)可以得到至多1個(gè)衍生狀態(tài)區(qū)域，如圖6所示。若側(cè)向間距不滿(mǎn)足側(cè)向安全距離的約束，則排除掉該衍生狀態(tài)區(qū)域，認(rèn)為當(dāng)前環(huán)境無(wú)法滿(mǎn)足避障需求，此時(shí)不會(huì)進(jìn)行避障軌跡的規(guī)劃。

本文的研究重點(diǎn)在于避障軌跡的生成，在此假設(shè)障礙物的運(yùn)動(dòng)狀態(tài)可預(yù)測(cè)，且t時(shí)間后的障礙物位置[xobs_t，yobs_t]和航向φt可知。假設(shè)車(chē)輛的行駛區(qū)域?yàn)檐?chē)道寬度為d的雙車(chē)道結(jié)構(gòu)化道路，在檢測(cè)到前方路徑上存在障礙物后，開(kāi)始進(jìn)行障礙物規(guī)避行為，且駛過(guò)L距離后完成軌跡偏移并到達(dá)衍生狀態(tài)區(qū)域。衍生狀態(tài)區(qū)域的位置會(huì)隨著障礙物的移動(dòng)而發(fā)生變化，但相對(duì)于障礙物的航向保持平行，與車(chē)輛前進(jìn)方向保持一致，并滿(mǎn)足安全車(chē)距的約束。

圖6 Frenét框架下衍生狀態(tài)區(qū)域示意圖

在Frenét 框架下，衍生狀態(tài)區(qū)域中心的縱向位置ystate由障礙物完成軌跡偏移時(shí)的行駛距離L確定，范圍在［0，Ldom］之間；側(cè)向衍生狀態(tài)區(qū)域由障礙物完成軌跡偏移的側(cè)向位置xobst與側(cè)向安全車(chē)距確定。得到衍生狀態(tài)區(qū)域的坐標(biāo)范圍：

在衍生狀態(tài)區(qū)域內(nèi)，假設(shè)車(chē)輛的瞬時(shí)轉(zhuǎn)向半徑與道路曲率半徑相同，終端狀態(tài)點(diǎn)的航向可由該位置的道路切線方向近似計(jì)算得到。假設(shè)道路軌跡以函數(shù)Y=f(X)表示，可得到終端狀態(tài)點(diǎn)的航向?yàn)?/p>

通過(guò)起始避障位置和終端狀態(tài)點(diǎn)擬合出的軌跡不僅應(yīng)滿(mǎn)足車(chē)輛的約束，還應(yīng)有較小的側(cè)向加速度、側(cè)向急動(dòng)度、橫擺角速度以保證車(chē)輛在轉(zhuǎn)向過(guò)程中保持良好的操縱穩(wěn)定性和行駛平順性。定義成本函數(shù)如下：

式中：κ*max為標(biāo)準(zhǔn)歸一化的路徑段的最大曲率；為標(biāo)準(zhǔn)歸一化的最大側(cè)向急動(dòng)度；為標(biāo)準(zhǔn)歸一化的最大橫擺角速度；ωκ、ωJ和ωφ為權(quán)重系數(shù)。成本函數(shù)越小則表征路徑段更滿(mǎn)足車(chē)輛的運(yùn)動(dòng)性質(zhì)，車(chē)輛能夠更為平穩(wěn)快速地對(duì)路徑段進(jìn)行跟蹤。

在軌跡規(guī)劃起點(diǎn)和終端狀態(tài)點(diǎn)之間的軌跡擬合的過(guò)程中，為保證路徑段對(duì)于車(chē)輛運(yùn)動(dòng)特性而言是可實(shí)現(xiàn)的，需要對(duì)路徑段的曲率和橫擺角速度進(jìn)行嚴(yán)格約束，因此在成本函數(shù)中曲率和橫擺角速度具有更大的權(quán)重。在軌跡簇的篩選過(guò)程中，可能需要對(duì)駕駛員需求進(jìn)行車(chē)輛的操控穩(wěn)定性和行駛平順性的個(gè)性化調(diào)節(jié)，因此在這段過(guò)程中，成本函數(shù)中側(cè)向急動(dòng)度和橫擺角速度具有更大的權(quán)重。

2.3 軌跡參數(shù)化

以通過(guò)自車(chē)的當(dāng)前位姿和衍生狀態(tài)區(qū)域內(nèi)的位姿，利用Bézier 曲線可規(guī)劃出無(wú)窮多條行駛軌跡，需要在其中唯一確定一條滿(mǎn)足車(chē)輛運(yùn)動(dòng)約束且綜合評(píng)價(jià)最佳的曲線。

對(duì)于實(shí)際的無(wú)人駕駛車(chē)輛系統(tǒng)來(lái)說(shuō)，軌跡規(guī)劃需要保證其規(guī)劃出來(lái)的軌跡滿(mǎn)足運(yùn)動(dòng)學(xué)約束和執(zhí)行機(jī)構(gòu)約束。以多項(xiàng)式進(jìn)行軌跡規(guī)劃時(shí)，曲線擬合規(guī)律不好把握，較難進(jìn)行車(chē)輛運(yùn)動(dòng)狀態(tài)的約束，在部分情況下多項(xiàng)式擬合無(wú)法得到解析解。通過(guò)Bézier 曲線進(jìn)行擬合則能夠較好地解決上述問(wèn)題，不僅在擬合區(qū)間上所有的連續(xù)函數(shù)都可以用多項(xiàng)式來(lái)逼近，且一致收斂；同時(shí)，Bézier 曲線在擬合過(guò)程中，可以通過(guò)很少的控制點(diǎn)，去生成復(fù)雜的平滑曲線。

Bézier 曲線的計(jì)算成本隨階數(shù)的增加而增加，為提升避障軌跡的計(jì)算效率，選用可生成連續(xù)曲率軌跡的最小階數(shù)曲線：3 階Bézier 曲線［18］進(jìn)行起始點(diǎn)至終點(diǎn)之間的軌跡擬合。由平面上4 個(gè)控制點(diǎn)P0（x0，y0）、P1（x1，y1）、P2（x2，y2）、P3（x3，y3）可唯一確定1 條3 階Bézier 曲線。Bézier 曲線的參數(shù)化表達(dá)式為

式中：0 ≤t≤1；Bi，3(t)為Bernstein基函數(shù)。

曲線上任意點(diǎn)的切矢量和2階導(dǎo)矢量為［19］

由曲率公式可得到軌跡端點(diǎn)的曲率為

為使避障軌跡與參考軌跡能夠平順連接，僅保證軌跡之間的連續(xù)性是不夠的，還需要盡量保證擬合軌跡段與直線路段之間的連接處保持切矢量的一致和曲率的連續(xù)。直線路段上，任意位置的曲線切矢量和曲率均為0。同樣的，在擬合軌跡段與直線路段之間的連接處，擬合曲線的起點(diǎn)和終端處的切矢量和曲率需趨近于0：

擬合軌跡段需要滿(mǎn)足其與直線路段的連接處的切矢量為0。由式（4）～式（6）可得到式（11）所示的控制點(diǎn)間關(guān)系。當(dāng)y0=y1且y2=y3，即控制點(diǎn)y1、y2分別在y0、y3所在的直線路段方向上時(shí)，能夠保證規(guī)劃路徑段在起點(diǎn)、終點(diǎn)處的切矢量連續(xù)。

在滿(mǎn)足切矢量連續(xù)的前提下，將式（4）～式（7）帶入式（8）中可得到：

可以看出，3階Bézier 曲線規(guī)劃器擬合的軌跡段不能保證起點(diǎn)和終點(diǎn)處曲率為0。相應(yīng)的，在車(chē)輛循跡過(guò)程中，3階Bézier 曲線在起始端和終端將會(huì)產(chǎn)生不連續(xù)的側(cè)向加速度和急動(dòng)度變化，即曲線起始端和終端處的車(chē)輛側(cè)向加速度和急動(dòng)度不為0。且|和|越大，起點(diǎn)和終點(diǎn)的曲率越趨近于0。由于控制點(diǎn)之間存在約束，在確定了控制點(diǎn)P0和P3的位置后，通過(guò)對(duì)e1、e2進(jìn)行合理的賦值，即可唯一確定一條擬合軌跡。

2.3.1 曲率連續(xù)約束

在進(jìn)行避障軌跡規(guī)劃時(shí)，希望整條路徑上的航跡點(diǎn)在避免碰撞的同時(shí)，都能夠平穩(wěn)滿(mǎn)足曲率連續(xù)性和最大曲率約束，使曲線在端點(diǎn)處與車(chē)輛的航向一致保證曲率矢量的連續(xù)。

為保證曲率矢量連續(xù)，需要保持控制點(diǎn)P1、P2分別選擇在車(chē)輛當(dāng)前位置和規(guī)劃終點(diǎn)位置的航向上。3 階Bézier 曲線控制點(diǎn)及控制參數(shù)如圖7 所示，其中P0為初始狀態(tài)點(diǎn)位置，P3為終端狀態(tài)點(diǎn)位置，為保證規(guī)劃的3 階Bézier 曲線在原始路徑的連接處保持1階幾何連續(xù)，選取P1、P2時(shí)需要保證為車(chē)輛的航向?yàn)樵撐恢锰幍能壽E切線方向。e1、e2分別為P1、P2在縱向、側(cè)向距P0、P3的比例系數(shù)，即可通過(guò)確定e1、e2的大小和P0、P3的位置得到P1、P2的位置：

圖7 Bézier曲線控制點(diǎn)及相關(guān)參數(shù)示意圖

通過(guò)改變e1、e2可直接對(duì)曲線進(jìn)行局部調(diào)整和控制。以固定e2值時(shí)e1的變化對(duì)曲線的影響為例進(jìn)行說(shuō)明，如圖8 所示。隨著e1的逐漸變大，軌跡段初始區(qū)域處的曲率將產(chǎn)生更加平滑的過(guò)渡效果，而軌跡終端區(qū)域則未受到明顯影響。因此在狀態(tài)格生成的過(guò)程中，不需要在起始控制點(diǎn)和終端控制點(diǎn)之間額外增加更多的分支。通過(guò)對(duì)e1、e2的控制和調(diào)整對(duì)傳統(tǒng)狀態(tài)格的分支進(jìn)行代替，以減小算法的計(jì)算成本。

圖8 通過(guò)改變控制點(diǎn)位置可實(shí)現(xiàn)曲線的局部調(diào)整

2.3.2 車(chē)輛運(yùn)動(dòng)學(xué)約束

受車(chē)輛轉(zhuǎn)向半徑的約束，須保證擬合的曲線上任意點(diǎn)曲率不小于車(chē)輛當(dāng)前速度下的最小轉(zhuǎn)向半徑，在Bézier曲線上的任意點(diǎn)須滿(mǎn)足：

2.3.3 車(chē)輛動(dòng)力學(xué)約束

為防止車(chē)輛在轉(zhuǎn)向過(guò)程中發(fā)生側(cè)滑或側(cè)傾，提升避讓路徑的舒適性，須選擇適當(dāng)?shù)膮?shù)對(duì)側(cè)向加速度、橫擺角速度和側(cè)向急動(dòng)度進(jìn)行約束［20］。

其中，車(chē)輛側(cè)向加速度和橫擺角速度由車(chē)輛動(dòng)力學(xué)模型估算得到，而側(cè)向急動(dòng)度通過(guò)軌跡段上航跡點(diǎn)之間的側(cè)向加速度變化率計(jì)算獲得。

3 仿真試驗(yàn)與結(jié)果分析

本節(jié)中的結(jié)果是在對(duì)比仿真試驗(yàn)中獲得的。在高速公路駕駛和城市駕駛兩種試驗(yàn)場(chǎng)景下（如圖9所示），通過(guò)比較本文提出方法與State Lattice 算法和MPC 算法的避障效果，對(duì)避障算法規(guī)劃出的軌跡進(jìn)行對(duì)比評(píng)價(jià)［21］。

圖9 仿真場(chǎng)景

測(cè)試場(chǎng)景1 為雙車(chē)道高速公路場(chǎng)景，測(cè)試車(chē)輛以80 km/h的速度行駛并接近單一障礙物，測(cè)試車(chē)輛須及時(shí)進(jìn)行障礙物規(guī)避動(dòng)作；測(cè)試場(chǎng)景2 為雙車(chē)道城市駕駛場(chǎng)景，測(cè)試車(chē)輛以50 km/h 的速度行駛，在測(cè)試車(chē)輛的當(dāng)前車(chē)道和相鄰車(chē)道上均有障礙物阻礙車(chē)輛的行駛，要求測(cè)試車(chē)輛能夠同時(shí)避讓多個(gè)障礙物。仿真結(jié)果如圖10～圖15 所示，分別顯示了不同算法在行進(jìn)距離上的側(cè)向位移、側(cè)向加速度和側(cè)向急動(dòng)度的變化規(guī)律。

圖10～圖12 為測(cè)試場(chǎng)景1 中不同規(guī)劃路徑方法的仿真結(jié)果對(duì)比，包括3 種路徑規(guī)劃方法生成的避障軌跡、軌跡段上側(cè)向加速度和側(cè)向急動(dòng)度的變化規(guī)律。單一障礙物下，3 種規(guī)劃方法生成的軌跡雖較為接近，但也存在可辨別的差異。MPC 算法在完成避障后有返回原行駛路線的趨勢(shì)，State Lattice 算法與本文算法類(lèi)似，在完成避障后保持航向在當(dāng)前車(chē)道方向上。本文算法較另兩種方法更靠近障礙物，但避障過(guò)程更為平緩。3 種方法在避障過(guò)程中產(chǎn)生的側(cè)向沖擊則清晰可辨，本文中提出的避障軌跡規(guī)劃器生成的路徑較其他方法具有更低的側(cè)向加速度，側(cè)向急動(dòng)度也控制在更為穩(wěn)定且更小的水平。

在側(cè)向急動(dòng)度的表現(xiàn)上，State Lattice 算法不僅會(huì)在避障起始和終止位置出現(xiàn)突變，而且具有較大的側(cè)向急動(dòng)度峰值；MPC 算法在避障開(kāi)始時(shí)刻會(huì)出現(xiàn)急動(dòng)度小幅的震蕩，但隨后避障過(guò)程的表現(xiàn)良好，急動(dòng)度變化平滑且保持在更小的閾值內(nèi)；相比而言，本文算法具有更好側(cè)向急動(dòng)度變化趨勢(shì)，避障過(guò)程的側(cè)向沖擊變化平穩(wěn)，且峰值僅在避障軌跡中段出現(xiàn)，在其余避障軌跡段均維持在較小的變化范圍內(nèi)。

如前文所述，由于3 階Bézier 曲線為2 階幾何連續(xù)，至多能保證曲線段間的曲率連續(xù)性，無(wú)法處理曲線3 階導(dǎo)數(shù)連續(xù)的情況。而在軌跡段上航跡點(diǎn)處側(cè)向加速度和急動(dòng)度的估算中，分別存在曲線3 階導(dǎo)數(shù)和4 階導(dǎo)數(shù)的計(jì)算環(huán)節(jié)。因此在軌跡段的起始端和終端無(wú)法保證側(cè)向加速度和側(cè)向急動(dòng)度連續(xù)變化。雖然本文算法生成的路徑在起始點(diǎn)處的側(cè)向加速度和急動(dòng)度不為0，但初始值較小。其在變化規(guī)律上更為平滑和穩(wěn)定，且始終保持在更小的側(cè)向加速度范圍內(nèi)，以更小的側(cè)向沖擊實(shí)現(xiàn)低側(cè)向加速度，整體表現(xiàn)更為穩(wěn)定和平滑。

圖10 場(chǎng)景1（高速公路駕駛）規(guī)劃路徑仿真對(duì)比

圖11 場(chǎng)景1側(cè)向加速度對(duì)比結(jié)果

圖12 場(chǎng)景1側(cè)向急動(dòng)度對(duì)比結(jié)果

圖13～圖15中針對(duì)測(cè)試場(chǎng)景2，進(jìn)行了不同規(guī)劃路徑方法的仿真結(jié)果對(duì)比。3 種方法之間存在明顯的差異，本文避障軌跡規(guī)劃器生成的軌跡在應(yīng)對(duì)多障礙物的情況下，同樣有更好的表現(xiàn)。State Lattice算法受曲率段之間的道路對(duì)齊約束，規(guī)避多障礙物時(shí)的軌跡路段之間過(guò)渡生澀，路段中的側(cè)向沖擊變化劇烈，存在較大的側(cè)向沖擊峰值。MPC 算法則有更好的表現(xiàn)，軌跡更為平順，側(cè)向沖擊變化平穩(wěn)。然而MPC 算法同樣會(huì)在避障行為的開(kāi)始階段出現(xiàn)急動(dòng)度的震蕩。本文算法生成的避障軌跡過(guò)渡平滑，在側(cè)向沖擊的表現(xiàn)上也有更好的表現(xiàn)，側(cè)向加速度、急動(dòng)度均保持在較小的水平上，且變化平穩(wěn)。同時(shí)，本文算法在完成多障礙的規(guī)避后，能夠較好地返回初始軌跡線，更適合具有全局參考軌跡的無(wú)人駕駛車(chē)輛局部避障軌跡規(guī)劃問(wèn)題。

在仿真過(guò)程中，雖然早兩段避障曲線的連接處不可避免地出現(xiàn)了側(cè)向急動(dòng)度突變的現(xiàn)象，但急動(dòng)度在兩段避障軌跡段的連接處數(shù)據(jù)跳動(dòng)極小，不足0.7 m/s3，對(duì)車(chē)輛的操穩(wěn)性產(chǎn)生的影響十分有限，因此不需要更高階次的Bézier 曲線規(guī)劃器進(jìn)行軌跡擬合。同時(shí)，急動(dòng)度在避障起始和終點(diǎn)均接近0，避障路徑與初始路徑的連接性能有較好的保證。本文算法不僅具備處理復(fù)雜場(chǎng)景的能力，同時(shí)能夠保證規(guī)劃方案有良好的尋優(yōu)能力。

圖13 場(chǎng)景2（城市駕駛）規(guī)劃路徑仿真對(duì)比

圖14 場(chǎng)景2側(cè)向加速度對(duì)比結(jié)果

圖15 場(chǎng)景2側(cè)向急動(dòng)度對(duì)比結(jié)果

4 結(jié)論

本文中提出的衍生狀態(tài)區(qū)域的規(guī)劃路徑的方法考慮了車(chē)輛的運(yùn)動(dòng)特性，將避障問(wèn)題轉(zhuǎn)化為根據(jù)設(shè)定規(guī)則的有限解尋優(yōu)問(wèn)題，并在2 種測(cè)試場(chǎng)景下與State Lattice 及MPC 路徑規(guī)劃器的避障規(guī)劃效果進(jìn)行對(duì)比，驗(yàn)證了本文算法的有效性。本文提出的算法具有如下特點(diǎn)。

（1）由于Bézier 曲線的規(guī)劃方法直觀且曲線生成的過(guò)程不受坐標(biāo)位置、曲線控制點(diǎn)數(shù)目等條件的約束，離散終端狀態(tài)點(diǎn)集方法在多種場(chǎng)景中計(jì)算有效且可預(yù)測(cè)。本文算法能夠處理多種場(chǎng)景下的避障規(guī)劃問(wèn)題，同時(shí)生成的軌跡更滿(mǎn)足車(chē)輛的運(yùn)動(dòng)特性需求，曲率變化更平滑，側(cè)向沖擊更小。

（2）對(duì)本文方法規(guī)劃的路徑進(jìn)行了基于車(chē)輛動(dòng)力學(xué)模型的局部尋優(yōu)操作，提升了車(chē)輛的操縱穩(wěn)定性。仿真結(jié)果對(duì)比也表明，車(chē)輛按照規(guī)劃路徑行駛的過(guò)程中，其側(cè)向加速度、側(cè)向急動(dòng)度、橫擺角速度等均符合車(chē)輛行駛穩(wěn)定性要求，本文提出的方法是有效且可行的。