算法解析：自動(dòng)駕駛實(shí)時(shí)路徑規(guī)劃

駕駛哥

2021年10月13日 09:46

作者 | Pirate Jack

來源 | Vehicle

導(dǎo)讀：本節(jié)主要介紹在自動(dòng)道路駕駛領(lǐng)域現(xiàn)有研究中使用的規(guī)劃技術(shù)。給定一條由路線規(guī)劃（導(dǎo)航）提供的路線，在道路上行駛的運(yùn)動(dòng)規(guī)劃（以下簡稱規(guī)劃）主要是在考慮車輛運(yùn)動(dòng)模型、車輛應(yīng)遵循的航路點(diǎn)和交通環(huán)境的約束條件下，包括靜態(tài)障礙物和動(dòng)態(tài)障礙物，尋找車輛行駛的最佳路徑。

算法解析：自動(dòng)駕駛實(shí)時(shí)路徑規(guī)劃的圖1

簡介

規(guī)劃可以分為增量方法，即通過重復(fù)使用以前搜索的信息來尋找狀態(tài)轉(zhuǎn)換的最佳順序（從一開始就沒有完全指定），以及試圖為車輛找到最佳單狀態(tài)轉(zhuǎn)換的本地方法。全局或局部路徑也與車輛執(zhí)行的決策或操縱有很強(qiáng)的相關(guān)性，因此也將討論操縱規(guī)劃。如下圖所示，路徑搜索在從路線規(guī)劃器中選擇路線后啟動(dòng)，并作為搜索最佳操縱的輸入（即使車輛具有最正確和安全行為的操縱）。然而，最終路徑可能會(huì)根據(jù)最佳操縱而改變，如這兩個(gè)模塊之間的反饋回路所示。一旦路徑最終確定，就生成了最終的軌跡規(guī)劃。

因此，自動(dòng)駕駛路徑規(guī)劃分為三個(gè)層次：

（1）找到車輛要遵循的最佳幾何路徑

a 通過增量采樣或離散幾何結(jié)構(gòu)（即增量搜索）找到最佳的動(dòng)作序列。

b 從多個(gè)最終狀態(tài)中找到最佳操作（即局部搜索）。

（2）找到最佳的動(dòng)作執(zhí)行。

（3）根據(jù)給定的約束條件，找到最佳軌跡，以完成幾何曲線的優(yōu)化。

例如，當(dāng)車輛在道路上時(shí)，它遵循從路線導(dǎo)航規(guī)劃獲取的路徑點(diǎn)序列，然后構(gòu)造車輛的幾何路徑（圖3a）。這些航路點(diǎn)必須是無障礙物的，因?yàn)檐囕v需要與其他車輛相互作用，以便在道路上協(xié)同移動(dòng)。根據(jù)已導(dǎo)出的幾何路徑和與其他車輛的相互作用，自動(dòng)車輛必須決定其下一個(gè)“高水平”動(dòng)作（圖3b）；即是否應(yīng)超越領(lǐng)先車輛及時(shí)到達(dá)下一個(gè)航路點(diǎn)？正如所暗示的，這些高層決策取決于路徑，因?yàn)檐囕v需要參考航路點(diǎn)來決定其最佳行動(dòng)。如果航路點(diǎn)和正確的操縱已經(jīng)完成，那么軌跡規(guī)劃描述了尋找連接所確定航路點(diǎn)的最佳方式的程序（圖3c）。

算法解析：自動(dòng)駕駛實(shí)時(shí)路徑規(guī)劃的圖4

增量搜索

在搜索最佳路徑時(shí)，增量搜索是指搜索配置或狀態(tài)沒有事先完全指定的技術(shù)。這樣的算法還會(huì)重復(fù)使用以前搜索的信息來提高搜索速度。主要使用兩種技術(shù)：

（1）快速探索隨機(jī)樹Rapidly exploring random trees（RRTs）

（2）格子規(guī)劃器Lattice Planners（LP）

討論如下：

Rapidly exploring random trees快速探索隨機(jī)樹 (RRTs)

RRTs構(gòu)造了一個(gè)樹型數(shù)據(jù)結(jié)構(gòu)，通過在每次迭代中添加新的配置（頂點(diǎn)），這些配置（頂點(diǎn)）從配置空間隨機(jī)采樣，直到達(dá)到目標(biāo)配置為止。RRT也可以推廣到狀態(tài)空間中，在狀態(tài)空間中，樹是從采樣狀態(tài)而不是配置中生長出來的（LaValle，1998）。

表2給出了構(gòu)造RRT的基本算法（LaValle，1998）。

在表2中，G是樹拓?fù)鋱D；C是配置空間；xrandom是從配置空間中隨機(jī)抽樣的配置；xNEAR是從距離度量中最接近xrandom的頂點(diǎn)；u是一個(gè)選定的輸入，使xrandom和xNEAR之間的距離最小，以確保新配置包含在Xfree中；xnew是新配置，如果在固定的時(shí)間間隔Dt內(nèi)應(yīng)用u，則獲得新配置，并且xnew是根據(jù)狀態(tài)或配置轉(zhuǎn)換公式計(jì)算的。需要一個(gè)碰撞檢測算法來確保xnew是無障礙的。在步驟7中，新配置作為頂點(diǎn)添加到樹中，最后，在步驟8中，在隨機(jī)采樣配置和xNEAR之間創(chuàng)建一條邊。這些步驟如圖4所示。

RRT在概率上完全保證運(yùn)動(dòng)學(xué)可行性，可以很容易地實(shí)時(shí)實(shí)現(xiàn)，并處理一般動(dòng)力學(xué)模型。RRT規(guī)劃師的這些優(yōu)勢正是它們被用于許多自動(dòng)駕駛案例的原因。RRT的主要優(yōu)點(diǎn)是對(duì)自由空間的快速探索；然而，它們的主要缺點(diǎn)在于它們創(chuàng)建的不規(guī)則路徑，以及樹擴(kuò)展對(duì)最近鄰度量的強(qiáng)烈依賴。RRTs的其他局限性還包括對(duì)每個(gè)擴(kuò)展節(jié)點(diǎn)進(jìn)行碰撞檢查的必要性，這可能會(huì)導(dǎo)致計(jì)算復(fù)雜性。此外，最優(yōu)性保證常常被忽視，而傾向于快速探索自由空間。

Macek等人（2006）使用RRT算法的簡單版本和B樣條曲線來生成平滑路徑。隨機(jī)參數(shù)包括加速度、加速度持續(xù)時(shí)間、采樣時(shí)間和方位角。碰撞檢查是通過假設(shè)車輛和障礙物的圓形表示來執(zhí)行的。該算法在800m*800m模擬環(huán)境中進(jìn)行了測試，該模擬環(huán)境中有靜止和移動(dòng)的障礙物，類似于真實(shí)的交通環(huán)境，并且使用了配備有激光雷達(dá)、單目攝像機(jī)、慣性測量單元（IMU）和GPS傳感器的自動(dòng)車輛。雖然可以有效地找到路徑，但車輛僅限于較低的最大速度和加速度，而且在一次實(shí)驗(yàn)中，由于障礙物跟蹤錯(cuò)誤，車輛不得不減速以徹底搜索最佳路徑。

Kuwata等人。（2009）使用閉環(huán)控制器來搖擺配置的采樣并模擬朝向它們的動(dòng)態(tài)軌跡，而不是直接從配置空間采樣（見圖5）。而經(jīng)典的方法，如RRT算法所示，在Kuwata等人采用的方法中，從配置空間隨機(jī)抽樣（上面的第3步）。從閉環(huán)控制器的輸入空間中選擇樣本（即閉環(huán)RRT，稱為CL-RRT）。此外，加入到樹上的每一條邊都是一條動(dòng)態(tài)可行的軌跡，通過正向仿真計(jì)算出每個(gè)頂點(diǎn)的可穿越性代價(jià)，評(píng)估可能軌跡的可行性。

該算法結(jié)合了有效的技術(shù)，如只在最佳航路點(diǎn)序列中選擇狀態(tài)和配置，以及車輛狀態(tài)的重新傳播，以降低構(gòu)造狀態(tài)空間的計(jì)算復(fù)雜度。搜索過程中存在大量的啟發(fā)式算法，這可能會(huì)增加執(zhí)行時(shí)間或收斂時(shí)間。在DARPA城市挑戰(zhàn)賽實(shí)施期間，發(fā)生了兩起車禍?zhǔn)录‵letcher等人，2008年）。Aoude等人提出了一種改進(jìn)CL-RRT的方法。（2010b）其中結(jié)合了使用博弈論方面的每個(gè)可能軌跡的威脅評(píng)估。所開發(fā)的算法（稱為RRT-Reach）搜索整棵樹中的威脅，假設(shè)每個(gè)策略都從障礙物中獲得了完美的知識(shí)，并且僅用兩輛車進(jìn)行了仿真測試。在障礙處理方面，CL-RRT的另一個(gè)改進(jìn)是由Aoude等人實(shí)施的。其中，高斯過程的最優(yōu)性估計(jì)需要進(jìn)一步的研究，但需要在實(shí)際時(shí)間范圍內(nèi)進(jìn)行偏差估計(jì)。

Karaman和Frazzoli（2011）開發(fā)了RRT*算法，以保證所提出路徑的最優(yōu)性。在RRT*中，所有可行連接都是根據(jù)其去代價(jià)來評(píng)估的，并且只在最小代價(jià)路徑上添加頂點(diǎn)到樹中。由RRT*找到的解決方案更有可能是最佳方案。Jeon等人（2013）采用RRT*算法，并通過半車動(dòng)態(tài)模型計(jì)算出路徑，使用該動(dòng)態(tài)模型可在U形轉(zhuǎn)彎處產(chǎn)生動(dòng)態(tài)可行的軌跡，并在無障礙的環(huán)路環(huán)境中實(shí)現(xiàn)。雖然使用動(dòng)態(tài)模型會(huì)導(dǎo)致更多的計(jì)算工作用于樹邊緣的可行性檢查，但與RRT*的組合會(huì)導(dǎo)致在給定控制輸入間隔內(nèi)（即由路線規(guī)劃師提供）最優(yōu)的解決方案，但不一定是全局最優(yōu)的。

Garrote等人（2014）也使用RRT*，但使用兩個(gè)額外的例程對(duì)其進(jìn)行修改（一個(gè)用于碰撞檢查，另一個(gè)用于展開）。在碰撞檢查方面，對(duì)于每一個(gè)探測到的節(jié)點(diǎn)，都要測量碰撞檢查的時(shí)間，以限定規(guī)劃所需的時(shí)間。懲罰例程負(fù)責(zé)檢查從每個(gè)新節(jié)點(diǎn)生長樹是否可行或不可行。利用這兩個(gè)額外的特征增強(qiáng)RRT*算法的思想是根據(jù)動(dòng)態(tài)障礙物的運(yùn)動(dòng)來擴(kuò)展樹。在Garrote等的工作（2014），ego車輛模擬為矩形，障礙物為圓形。該算法僅在靜態(tài)障礙物和動(dòng)態(tài)障礙物的模擬環(huán)境中進(jìn)行了測試，沒有對(duì)仿真性能進(jìn)行統(tǒng)計(jì)測試。

Reyes Castro等人（2013）給出了更先進(jìn)的技術(shù)。車輛和路網(wǎng)的動(dòng)力學(xué)系統(tǒng)被形成Kripke結(jié)構(gòu)（Kripke，1963），并使用非確定性有限自動(dòng)機(jī)來捕捉交通規(guī)則。這兩種技術(shù)的組合用于增加RRT*（稱為最小違規(guī)RRT*），但道路網(wǎng)絡(luò)上沒有障礙物。

Lattice Planner格子規(guī)劃器

狀態(tài)格構(gòu)造了一個(gè)離散的搜索空間，使得相關(guān)的狀態(tài)連續(xù)，以確定的方式獲取目標(biāo)狀態(tài)，并滿足車輛的微分約束。由于啟用了預(yù)計(jì)算（Howard，2009），這也降低了計(jì)算成本（Howard，2009），除非晶格彎曲以遵循車道或道路的形狀（Madas等人，2013年）。它們通常非常適合非完整和高度受限的環(huán)境（Likhachev和Ferguson，2009），例如道路環(huán)境。

Lattice Planner的解析完成。這意味著控制空間可以根據(jù)每一個(gè)分辨率的變化自動(dòng)調(diào)整，并且可以對(duì)空間進(jìn)行一致的探索。Lattice Planner還保證了最佳性和平滑性，因?yàn)樗鼈儾粫?huì)引入與反向指針相關(guān)的不連續(xù)性（Pivtoraiko等人，2009年）。生成的計(jì)劃基本上接近車輛的實(shí)際運(yùn)動(dòng)，但由于無法快速考慮備選目標(biāo)狀態(tài)，因此無法有效地執(zhí)行規(guī)避操作（Werling等人，2011年）。此外，航向角的離散化可能是有問題的，并且可能導(dǎo)致定向樣本之間的振蕩（如圖6所示），窮舉采樣可能導(dǎo)致不必要的計(jì)算復(fù)雜性。

Likhachev和Ferguson（2009）在車輛和目標(biāo)附近使用高分辨率的多分辨率狀態(tài)晶格，而在其他地方使用低分辨率。每種狀態(tài)都包括位置（x，y），方向（h）和速度，與常規(guī)晶格相比，搜索速度增加了3倍。不同的分辨率使車輛能夠構(gòu)建長期計(jì)劃，而不會(huì)通過窮舉采樣增加規(guī)劃的計(jì)算復(fù)雜性。該多分辨率網(wǎng)格采用確定性算法（稱為Anytime Dynamic）進(jìn)行搜索，該算法適用于規(guī)劃空間的有限性和不完全感知性以及有限的計(jì)算時(shí)間。在DARPA城市挑戰(zhàn)賽期間，使用一輛真實(shí)世界的自動(dòng)車輛進(jìn)行了實(shí)驗(yàn)，以15 mph的最大速度行駛，但發(fā)現(xiàn)主要缺點(diǎn)是曲率不連續(xù)。

為了解決曲率問題，Rufli和Siegwart（2010）通過在4D配置空間（包括2D位置、航向和轉(zhuǎn)向角）上定義一個(gè)2D輸入集U（航向和轉(zhuǎn)向角被離散化），構(gòu)造了一個(gè)輸入和狀態(tài)格。網(wǎng)格跨越24個(gè)方向，這些節(jié)點(diǎn)通過三次多項(xiàng)式曲線連接起來，并且具有自我變形的能力，以適應(yīng)給定參考路徑的彎曲和狹窄道路結(jié)構(gòu)。

全局固定狀態(tài)格需要平坦的地形和固定的機(jī)動(dòng)性，因此限制了車輛的移動(dòng)，因?yàn)榕c格點(diǎn)集軌跡的微小差別可以顯著增加搜索時(shí)間和計(jì)算量。

這一缺陷促使了時(shí)空狀態(tài)格的發(fā)展，如Ziegler和Stiller（2009）在圖7中所示，其中狀態(tài)空間和時(shí)間在與非時(shí)間晶格類似地離散之前結(jié)合在單個(gè)流形中。在他們的工作中，五次多項(xiàng)式被用于連接頂點(diǎn)，并且顯示了一個(gè)用于晶格匹配道路形狀的變形范例。時(shí)間的增加增加了維數(shù)（二維位置、二維速度、二維加速度和時(shí)間由每個(gè)狀態(tài)描述）。該算法能實(shí)時(shí)有效地對(duì)狀態(tài)空間進(jìn)行采樣，并能更靈活地分配邊緣權(quán)值。缺點(diǎn)是時(shí)間和速度集合上的固定值，以及五次多項(xiàng)式與車輛運(yùn)動(dòng)方程之間的不平衡。

在McNaughton等人（2011）將曲率添加到每個(gè)狀態(tài)（初始狀態(tài)包含2D位置、航向和曲率），并且路徑（車輛和采樣端點(diǎn)之間）用三次多項(xiàng)式緩和曲線連接。然后將時(shí)間和速度范圍分配給每個(gè)頂點(diǎn)，以實(shí)現(xiàn)時(shí)空搜索；但是需要恒定的加速度，使得車輛很難遵循這樣的加速度曲線。時(shí)間和速度范圍的分配減少了搜索空間，但引入了次優(yōu)性，并且需要在評(píng)估每個(gè)軌跡后計(jì)算精確值。為了找到最佳路徑，動(dòng)態(tài)規(guī)劃算法在格子中窮盡搜索，同時(shí)考慮障礙物的存在、行程時(shí)間和期望行為。

為了改進(jìn)McNaughton等人的方法，Xu等人。（2012）使用四次曲率多項(xiàng)式來提供規(guī)劃周期之間的連續(xù)曲率變化率；不僅從采樣端點(diǎn)建立連接，而且從當(dāng)前車輛姿態(tài)進(jìn)行連接。另一個(gè)區(qū)別是，速度曲線是反向生成的，在評(píng)估替代路徑時(shí)，也考慮了舒適性、效率和能耗。

Gu等人也使用了時(shí)空晶格。（2013）在一個(gè)2級(jí)規(guī)劃方法中，首先生成一個(gè)最佳無碰撞參考路徑，然后對(duì)狀態(tài)空間進(jìn)行采樣，以便根據(jù)參考路徑找到最佳路徑。構(gòu)建參考路徑是為了處理詳盡的采樣；從而導(dǎo)致更集中的搜索和更人性化的駕駛風(fēng)格。

RRTs與Lattice Planner比較

RRT和Lattice Planner的比較如表3所示：

算法解析：自動(dòng)駕駛實(shí)時(shí)路徑規(guī)劃的圖10

綜上所述，RRT和Lattice Planner都使用數(shù)據(jù)結(jié)構(gòu)（分別是樹和格）對(duì)狀態(tài)空間進(jìn)行采樣，試圖以一種快速、安全的方式對(duì)其進(jìn)行探索。在這兩種情況下都可以快速探索，并向規(guī)劃模塊提供一系列可能的路徑，供車輛遵循。然而，據(jù)稱規(guī)劃范圍相對(duì)較大，并且，對(duì)于道路行駛的動(dòng)態(tài)特性，當(dāng)障礙物或障礙物突然出現(xiàn)時(shí)，需要重新規(guī)劃例行程序來補(bǔ)充這些增量搜索方法。最后，為了提高安全性，應(yīng)該使用額外的碰撞預(yù)測模塊，而不是算法的內(nèi)置碰撞檢查功能。

算法解析：自動(dòng)駕駛實(shí)時(shí)路徑規(guī)劃的圖11

局部搜索

實(shí)時(shí)搜索整個(gè)圖并不總是有效的；因此，一些方法使用有限的視界，無論是在時(shí)間還是空間上。

在局部搜索級(jí)別，用于道路自動(dòng)駕駛的最流行的技術(shù)可能是搜索空間包含某個(gè)幾何曲線（例如回旋曲線或樣條曲線）以及該曲線的幾個(gè)橫向位移。然后通過一個(gè)代價(jià)函數(shù)對(duì)每個(gè)候選路徑進(jìn)行評(píng)估，考慮到距離和時(shí)間成本、加速和碰撞檢查。

橫向位移產(chǎn)生的路徑一般可分為兩類：

（i）車輛作用空間的橫向移動(dòng)

（ii）車輛狀態(tài)空間中的橫向位移

下圖展示出了相對(duì)比較：

算法解析：自動(dòng)駕駛實(shí)時(shí)路徑規(guī)劃的圖12

在復(fù)雜的動(dòng)態(tài)環(huán)境中，橫向移動(dòng)的軌跡可能無法很好地執(zhí)行（Gu和Dolan，2012）。因?yàn)樗皇浅繕?biāo)搜索整個(gè)狀態(tài)空間，這樣會(huì)在無限時(shí)間范圍內(nèi)需要大量的計(jì)算能力（用于構(gòu)建狀態(tài)空間）。Benenson等人提出的部分運(yùn)動(dòng)規(guī)劃（Partial Motion Planning-PMP）可以使用。PMP使用短時(shí)間范圍與RRT相結(jié)合，并利用不可避免碰撞狀態(tài)（ICS）的概念（即無法避免碰撞的車輛狀態(tài)定義）進(jìn)行安全檢查。不可避免的碰撞狀態(tài)保證不會(huì)發(fā)生碰撞，但需要對(duì)車輛周圍環(huán)境有充分的了解（Althoff等人，2011年），而PMP方法通常接近最佳狀態(tài)保證（Kushleyev和khachev，2009年）。如下圖所示，如果找到不可避免碰撞狀態(tài)（ICS），則搜索替代路徑，并且在每個(gè)時(shí)刻以RRT方式展開步驟節(jié)點(diǎn)。

算法解析：自動(dòng)駕駛實(shí)時(shí)路徑規(guī)劃的圖13

算法解析：自動(dòng)駕駛實(shí)時(shí)路徑規(guī)劃的圖14

總之，橫向移動(dòng)的軌跡和PMP方法都旨在找到適合車輛行駛的最佳下一個(gè)動(dòng)作（就路徑作為一個(gè)動(dòng)作而言）。迂回軌跡在動(dòng)作或狀態(tài)空間中采樣最終條件，而PMP方法使用RRT在有限的時(shí)間范圍內(nèi)進(jìn)行規(guī)劃。對(duì)于車道和道路邊界以及其他交通參與者的運(yùn)動(dòng)而言，對(duì)狀態(tài)空間進(jìn)行采樣的技術(shù)可能會(huì)導(dǎo)致平滑和安全的路徑。這與PMP和對(duì)動(dòng)作空間進(jìn)行采樣的技術(shù)相矛盾，這可能導(dǎo)致適應(yīng)大量障礙物處理的路徑不可行。

參考文獻(xiàn)：

1. Real-time motion planning methods for autonomous on-road driving: State-of-the-art and future research directions -Christos Katrakazas a , Mohammed Quddus a,? , Wen-Hua Chen b,1 , Lipika Deka a

2. Partial Motion Planning Framework for Reactive Planning Within Dynamic Environments

3. Multi-layer occupancy grid mapping for autonomous vehicles navigation

4. Robust Calculation of Ego-Vehicle Corridors

5. Multi-layer occupancy grid mapping for autonomous vehicles navigation

登錄后免費(fèi)查看全文

立即登錄