本文經(jīng)自動(dòng)駕駛之心公眾號(hào)授權(quán)轉(zhuǎn)載，轉(zhuǎn)載請(qǐng)聯(lián)系出處。

1 決策控制與運(yùn)動(dòng)規(guī)劃概述

目前決策控制方法可以分為三類：sequential planning、behavior-aware planning、和end-to-end planning。

• sequential planning：最傳統(tǒng)的方法，感知、決策與控制三個(gè)部分層次較為清晰；
• behavior-aware planning：相比第一種亮點(diǎn)在于引入人機(jī)共駕、車路協(xié)同以及車輛對(duì)外部動(dòng)態(tài)環(huán)境的風(fēng)險(xiǎn)預(yù)估；
• end-to-end planning：DL、DRL技術(shù)，借助大量的數(shù)據(jù)訓(xùn)練，獲得從圖像等感知信息到方向盤(pán)轉(zhuǎn)角等車輛控制輸入的關(guān)系，屬于時(shí)下最熱門(mén)的方法之一。

本文將對(duì)sequential planning進(jìn)行介紹，按照整個(gè)決策控制順序講述自動(dòng)駕駛汽車的感知控制過(guò)程，最后會(huì)簡(jiǎn)要總結(jié)一下前文所提到的待解決的問(wèn)題。

Control architecture for automated vehicles

2 路徑規(guī)劃概述

sequential planning 的過(guò)程簡(jiǎn)要概括為路徑規(guī)劃->決策過(guò)程->車輛控制，本文講述的路徑規(guī)劃屬于第一步與第三步。

https://mediatum.ub.tum.de/doc/1523134/74110395763.pdf

在無(wú)人車的運(yùn)動(dòng)軌跡生成問(wèn)題上，有直接軌跡生成法與路徑-速度分解法兩種，相比第一種，路徑-速度難度更小，因此更加常用。

2.1 路徑規(guī)劃的類型

路徑規(guī)劃可分為四大類：以PRM、RRT為代表的基于采樣的算法、以為A* 、D* 代表的基于搜索的算法、以β樣條曲線為代表的基于插值擬合的軌跡生成算法，和以MPC為代表的用于局部路徑規(guī)劃的最優(yōu)控制算法。本小節(jié)將按照上述順序逐一講解：

A Review of Motion Planning Techniquesfor Automated Vehicles

2.2 路徑規(guī)劃算法優(yōu)缺點(diǎn)

3 路徑規(guī)劃方法

3.1 基于采樣的算法

3.1.1 基本算法PRM與RRT

(1) PRM

PRM算法（Probabilistic Road Map）。PRM主要包含了兩個(gè)步驟，一是學(xué)習(xí)階段，二是查詢階段。

第一步，學(xué)習(xí)階段：對(duì)狀態(tài)空間內(nèi)的安全區(qū)域均勻隨機(jī)采樣ｎ個(gè)點(diǎn)，并刪除采樣落在障礙物上的點(diǎn)，接著對(duì)相鄰的點(diǎn)進(jìn)行連接并做碰撞檢測(cè)，剔除不是collision-free的連線，最終得到一個(gè)連通圖。

第二步，查詢階段：對(duì)于給定的一對(duì)初始和目標(biāo)狀態(tài)，利用上一步構(gòu)建好的采樣節(jié)點(diǎn)及連續(xù)，運(yùn)用圖搜索的方法（Dijkstra或者A*）來(lái)找出一條可行路徑。

完成PRM構(gòu)建后，就可以用于解決不同初始、目標(biāo)狀態(tài)的運(yùn)動(dòng)規(guī)劃問(wèn)題，但是這個(gè)特性對(duì)于無(wú)人車運(yùn)動(dòng)規(guī)劃而言是不必要的。另外PRM要求對(duì)狀態(tài)之間作精確連接，這對(duì)于存在復(fù)雜微分約束的運(yùn)動(dòng)規(guī)劃問(wèn)題是十分困難的。

(2) RRT

RRT (Rapidly-exploring Random Tree)算法。RRT其實(shí)代表了一系列基于隨機(jī)生長(zhǎng)樹(shù)思想的算法，是目前機(jī)器人領(lǐng)域運(yùn)用最廣泛、優(yōu)化變種最多的一類算法.

① 樹(shù)初始化：初始化樹(shù)的結(jié)點(diǎn)集和邊集，結(jié)點(diǎn)集只包含初始狀態(tài)，邊集為空；

② 樹(shù)的生長(zhǎng)：對(duì)狀態(tài)空間隨機(jī)采樣，當(dāng)采樣點(diǎn)落在狀態(tài)空間安全區(qū)域時(shí)，選擇當(dāng)前樹(shù)中離采樣點(diǎn)最近的結(jié)點(diǎn)，將其向采樣點(diǎn)擴(kuò)展連接；若生成的軌跡不與障礙物發(fā)生碰撞，則將該軌跡加入樹(shù)的邊集，該軌跡的終點(diǎn)加入到樹(shù)的結(jié)點(diǎn)集

③ 重復(fù)步驟②，直至擴(kuò)展到目標(biāo)狀態(tài)集中，相比PRM的無(wú)向圖而言，RRT構(gòu)建的是初始狀態(tài)作為根結(jié)點(diǎn)、目標(biāo)狀態(tài)作為葉結(jié)點(diǎn)的樹(shù)結(jié)構(gòu)，對(duì)于不同的初始和目標(biāo)狀態(tài)，需要構(gòu)建不同的樹(shù)。

RRT不要求狀態(tài)之間的精確連接，更適合解決像無(wú)人車運(yùn)動(dòng)規(guī)劃這樣的運(yùn)動(dòng)動(dòng)力學(xué)問(wèn)題。

3.1.2 采樣法的問(wèn)題與解決辦法

求解效率與是否最優(yōu)解。PRM與RRT擁有概率完備性的原因在于其幾乎會(huì)遍歷構(gòu)型空間中所有位置。

(1) 求解效率

在提升求解效率方面，優(yōu)化RRT的核心思想在于引導(dǎo)樹(shù)向空曠區(qū)域，即盡量遠(yuǎn)離障礙物，避免對(duì)于障礙物處的節(jié)點(diǎn)的重復(fù)檢查，以此提升效率。主要解決方法：

① 均勻采樣

標(biāo)準(zhǔn)RRT算法對(duì)狀態(tài)空間均勻隨機(jī)采樣，當(dāng)前樹(shù)中結(jié)點(diǎn)獲得擴(kuò)展的概率與其Voronoi區(qū)域面積成正比，所以樹(shù)會(huì)向著狀態(tài)空間的空曠區(qū)域生長(zhǎng)，均勻充滿狀態(tài)空間的自由區(qū)域。

RRT-connect算法同時(shí)構(gòu)建兩棵分別起始于初始狀態(tài)和目標(biāo)狀態(tài)的樹(shù)，當(dāng)兩棵樹(shù)生長(zhǎng)到一起時(shí)則找到可行解。Go-biaing在隨機(jī)采樣序列中以一定比例插入目標(biāo)狀態(tài)，引導(dǎo)樹(shù)向目標(biāo)狀態(tài)擴(kuò)展，加快求解速度，提高求解質(zhì)量。

heuristic RRT使用啟發(fā)式函數(shù)增加擴(kuò)展代價(jià)低的結(jié)點(diǎn)被采樣的概率，計(jì)算樹(shù)中每個(gè)結(jié)點(diǎn)的代價(jià)，但是在復(fù)雜環(huán)境中，代價(jià)函數(shù)的定義困難的，為解決這一問(wèn)題，f-biased采樣方法先將狀態(tài)空間離散化為網(wǎng)格，再使用Dijkstra算法計(jì)算每個(gè)網(wǎng)格上的代價(jià)，這個(gè)網(wǎng)格所在區(qū)域的點(diǎn)的代價(jià)值都等于該值，以此構(gòu)建啟發(fā)式函數(shù)。

② 優(yōu)化距離度量

距離用來(lái)度量?jī)蓚€(gè)構(gòu)形之間路徑的代價(jià)，輔助生成啟發(fā)式代價(jià)函數(shù)，引導(dǎo)樹(shù)的走向。但在考慮障礙物的情況下，距離計(jì)算的難度高，運(yùn)動(dòng)規(guī)劃中距離的定義采用類似歐氏距離的定義。RG-RRT （rechability guided RRT）可以消除不準(zhǔn)確的距離對(duì)RRT探索能力的影響，它需要計(jì)算樹(shù)中結(jié)點(diǎn)的能達(dá)集，當(dāng)采樣點(diǎn)到結(jié)點(diǎn)的距離大于到該結(jié)點(diǎn)能達(dá)集的距離時(shí)，該節(jié)點(diǎn)才有可能被選中進(jìn)行擴(kuò)展。

③ 降低碰撞檢查次數(shù)

碰撞檢查采樣法的效率瓶頸之一，通常的做法是對(duì)路徑等距離離散化，再對(duì)每個(gè)點(diǎn)處的構(gòu)形作碰撞檢查。resolution complete RRT通過(guò)降低靠近障礙物的結(jié)點(diǎn)獲得擴(kuò)展的概率，它對(duì)輸入空間離散化，對(duì)于某個(gè)結(jié)點(diǎn)輸入只使用一次；若某個(gè)輸入對(duì)應(yīng)的軌跡與障礙物碰撞，則對(duì)該節(jié)點(diǎn)加上一個(gè)懲罰值，該懲罰值越高，該節(jié)點(diǎn)獲得擴(kuò)展的概率越小。dynamic domain RRT與adaptive dynamic domain RRT限制采樣區(qū)域在當(dāng)前樹(shù)所在的局部空間，以防止靠近障礙物的結(jié)點(diǎn)反復(fù)擴(kuò)展失敗，提高算法效率。

④ 提升實(shí)時(shí)性

Anytime RRT先快速構(gòu)建一個(gè) RRT，獲得一個(gè)可行解并記錄其代價(jià)，之后繼續(xù)采樣，但僅將有利于降低可行解代價(jià)的結(jié)點(diǎn)插入樹(shù)中，從而逐漸獲得較優(yōu)的可行解。Replanning將整個(gè)規(guī)劃任務(wù)分解為若干等時(shí)間的子任務(wù)序列，在執(zhí)行當(dāng)前任務(wù)的同時(shí)規(guī)劃下一個(gè)任務(wù)。

(2) 在解決最優(yōu)性問(wèn)題上主要有以下方法：

RGG算法（random geometric graph）：根據(jù)隨機(jī)幾何圖理論對(duì)標(biāo)準(zhǔn)PRM和RRT 改進(jìn)的具有漸近最優(yōu)性質(zhì)的PRM、RRG和RRT 算法，在狀態(tài)空間中隨機(jī)采樣ｎ個(gè)點(diǎn)，并將距離小于r(n)的點(diǎn)連起來(lái)，就構(gòu)成了RGG。

RRT* 算法：在RRG基礎(chǔ)上引入“重新連接”步驟，檢查新插入結(jié)點(diǎn)作為其臨近點(diǎn)的父結(jié)點(diǎn)是否會(huì)使其臨近點(diǎn)的代價(jià)降低，若降低，則去掉臨近點(diǎn)原來(lái)的父子關(guān)系，將當(dāng)前插入點(diǎn)作為其父結(jié)點(diǎn)，這就是RRT*算法。

LBT-RRT算法：大量的結(jié)點(diǎn)連接和局部調(diào)整使得PRM和RRT的效率十分低下。LBT-RRT算法將RRG和RRT* 算法結(jié)合起來(lái)，在獲得漸進(jìn)最優(yōu)性的前提下，獲取更高的效率。

3.2 基于搜索的算法

基本思想是將狀態(tài)空間通過(guò)確定的方式離散成一個(gè)圖，然后利用各種啟發(fā)式搜索算法搜索可行解甚至是最優(yōu)解，該種類別算法比較成熟。

基于搜索的算法的基礎(chǔ)是狀態(tài)格子，狀態(tài)格子是狀態(tài)空間離散化，由狀態(tài)結(jié)點(diǎn)和從該結(jié)點(diǎn)出發(fā)到達(dá)相鄰結(jié)點(diǎn)的運(yùn)動(dòng)基元組成，一個(gè)狀態(tài)結(jié)點(diǎn)可以通過(guò)其運(yùn)動(dòng)基元變換到另一個(gè)狀態(tài)結(jié)點(diǎn)。狀態(tài)格子就將原來(lái)連續(xù)的狀態(tài)空間轉(zhuǎn)化為一個(gè)搜索圖，運(yùn)動(dòng)規(guī)劃問(wèn)題就變成了在圖中搜索出一系列將初始狀態(tài)變換到目標(biāo)狀態(tài)運(yùn)動(dòng)基元，構(gòu)建起狀態(tài)格子后就可以使用圖搜索算法來(lái)搜索最優(yōu)軌跡。

3.2.1 基礎(chǔ)算法Dijkstra、A*的構(gòu)建

Dijkstra算法遍歷整個(gè)構(gòu)型空間，找出每?jī)蓚€(gè)格子之間的距離，最后選擇出發(fā)點(diǎn)到目標(biāo)點(diǎn)的最短路徑，其廣度優(yōu)先的性質(zhì)導(dǎo)致效率很低，在該算法的基礎(chǔ)上加入啟發(fā)式函數(shù)，即所搜索結(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)的距離，并以此為基礎(chǔ)再次進(jìn)行搜索可避免全局搜索帶來(lái)的效率低下，這即為A*算法，如下圖所示，紅色為搜索區(qū)域。

圖6：A*與Dijkstra算法效果對(duì)比圖

3.2.2 搜索法的問(wèn)題與建議

與基于采樣的算法相同，這種類別的算法也需要做效率與最優(yōu)性的優(yōu)化。

在提升效率上面，A* 本身屬于靜態(tài)規(guī)劃的算法，針對(duì)A* 算法的延申有weighted A* 通過(guò)增加啟發(fā)式函數(shù)的權(quán)重進(jìn)一步引導(dǎo)搜索方向向這目標(biāo)節(jié)點(diǎn)進(jìn)行，搜索速度很快，但是容易陷入局部極小值，無(wú)法保證全局最優(yōu)解。

對(duì)于運(yùn)動(dòng)的車輛來(lái)說(shuō)，使用A* 的衍生算法D（dynamic A）可大幅度提升效率。同樣以動(dòng)態(tài)規(guī)劃為基礎(chǔ)的還有LPA，該算法可以處理狀態(tài)格子的運(yùn)動(dòng)基元的代價(jià)是時(shí)變的情況，當(dāng)環(huán)境發(fā)生變化時(shí)可以通過(guò)對(duì)較少數(shù)目節(jié)點(diǎn)的重新搜索規(guī)劃出新的最優(yōu)路徑。在LPA 的基礎(chǔ)上開(kāi)發(fā)出D*-Lite可以獲得與D*同樣的結(jié)果，但是效率更高。

在進(jìn)行最優(yōu)化解的探尋時(shí)，ARA* 是在Weighted A* 基礎(chǔ)上發(fā)展出的具有Anytime性質(zhì)的搜索算法，它通過(guò)多次調(diào)用Weighted A* 算法且每次調(diào)用就縮小啟發(fā)式函數(shù)的權(quán)重，這樣算法可以快速求出可行解，通過(guò)引入集合INCONS使得每次循環(huán)可以繼續(xù)使用上一次循環(huán)的信息，對(duì)路徑做出優(yōu)化，逐漸逼近最優(yōu)解。

在兼顧算法效率與最優(yōu)性的問(wèn)題上，Sandin aine等提出了MHA* 算法，引入多個(gè)啟發(fā)式函數(shù)，保證其中有一個(gè)啟發(fā)式函數(shù)在單獨(dú)使用時(shí)可以找到最優(yōu)解，從而通過(guò)協(xié)調(diào)不同啟發(fā)式函數(shù)生成的路徑代價(jià)，可以兼顧算法的效率和最優(yōu)性。DMHA在MHA的基礎(chǔ)上在線實(shí)時(shí)生成合適的啟發(fā)式函數(shù)，從而避免局部最小值問(wèn)題。

3.3 基于插值擬合的算法

基于插值擬合的算法可被定義為：根據(jù)已知的一系列用于描述道路圖的點(diǎn)集，通過(guò)使用數(shù)據(jù)插值與曲線擬合的方式創(chuàng)造出智能車將行駛的路徑，該路徑可提供較好的連續(xù)性、較高的可導(dǎo)性。具體的方法如下：

Dubins曲線和Reeds and Sheep(RS)曲線是連接構(gòu)形空間中任意兩點(diǎn)的最短路徑，分別對(duì)應(yīng)無(wú)倒車和有倒車的情況。它們都是由最大曲率圓弧和直線組成的，在圓弧和直線連接處存在曲率不連續(xù)，實(shí)際車輛按照這樣曲線行駛時(shí)必須在曲率不連續(xù)處停車調(diào)整方向輪才能繼續(xù)行駛。

多項(xiàng)式插值曲線是最常用的一種方法，它可以通過(guò)滿足結(jié)點(diǎn)的要求來(lái)設(shè)定多項(xiàng)式系數(shù)，并且獲得較好的連續(xù)可導(dǎo)性，四階多項(xiàng)式常用于縱向約束控制，五階多項(xiàng)式常用于橫向約束控制，三階多項(xiàng)式也被用于超車軌跡中。

樣條曲線具有封閉的表達(dá)式，容易保證曲率連續(xù)性。β樣條曲線可以實(shí)現(xiàn)曲率連續(xù)性，三次Bezier曲線可以保證曲率的連續(xù)性和有界性，并且計(jì)算量相對(duì)較小。η^3曲線[43]是一種七次樣條曲線，它有著很好的性質(zhì)：曲率連續(xù)性和曲率導(dǎo)數(shù)的連續(xù)性，這對(duì)于高速行駛車輛是很有意義的。

3.4 基于最優(yōu)控制的算法

將基于最優(yōu)控制的算法歸在路徑規(guī)劃中，主要是因?yàn)槠渲械腗PC可以進(jìn)行局部的路徑規(guī)劃以進(jìn)行避障，除此之外，MPC主要的作用是進(jìn)行軌跡跟蹤，其所考慮的問(wèn)題除了必要的動(dòng)力學(xué)、運(yùn)動(dòng)學(xué)約束以外，未來(lái)還應(yīng)考慮舒適性、感知信息的不確定性、車間通信的不確定性，并且在局部軌跡規(guī)劃時(shí)還可以將駕駛員納入控制閉環(huán)。對(duì)于以上所提到的不確定性問(wèn)題與將駕駛員納入控制閉環(huán)將在第四節(jié)討論。關(guān)于MPC的學(xué)習(xí)，主要從優(yōu)化理論與工程實(shí)踐兩個(gè)方面入手。對(duì)于前者，推薦Dimitri P. Bertsekas的Convex Optimization Algorithms，James B. Rawlings的Model Predictive Control：Theory, Computation, and Design。中文領(lǐng)域劉浩洋老師的最優(yōu)化書(shū)個(gè)人覺(jué)得相對(duì)清晰易懂。對(duì)于后者，首先龔建偉老師的那本無(wú)人駕駛MPC書(shū)強(qiáng)推了，老版書(shū)里的demo有問(wèn)題，不過(guò)都在新版里解決了。

MPC所使用的預(yù)測(cè)模型有很多種：諸如卷積神經(jīng)網(wǎng)絡(luò)、模糊控制、狀態(tài)空間等等，其中用的最多的為狀態(tài)空間法。MPC可簡(jiǎn)要表述為：滿足必要的動(dòng)力學(xué)、運(yùn)動(dòng)學(xué)等等約束的情況下，通過(guò)數(shù)值手段求解模型的最優(yōu)解，該最優(yōu)解即為狀態(tài)方程的控制量，如方向盤(pán)轉(zhuǎn)角等等，并將控制量作用在車模上以獲得要求的狀態(tài)量，如速度、加速度、坐標(biāo)等等。

通過(guò)上述描述可知，MPC的關(guān)鍵在于模型的建立與模型的求解，如何等效簡(jiǎn)化模型的建立以及提升求解的效率是重中之重。在不同的控制輸入下車輛會(huì)走不同的軌跡，每一條軌跡都與之對(duì)應(yīng)一個(gè)目標(biāo)函數(shù)值，無(wú)人駕駛車輛會(huì)通過(guò)求解算法找出最小目標(biāo)函數(shù)值對(duì)應(yīng)的控制量，并將其作用在車上，如下圖所示：

為了降低建模難度，也有使用人工勢(shì)能場(chǎng)模型進(jìn)行建模，人工勢(shì)能場(chǎng)的基本思想類似于電場(chǎng)，道路上的障礙物類似于電場(chǎng)中與場(chǎng)源相異電荷極性的電荷。障礙物（動(dòng)態(tài)、靜態(tài)）處的勢(shì)能更高，無(wú)人車將向低勢(shì)能位置前進(jìn)。

4 開(kāi)源項(xiàng)目

推薦一個(gè)開(kāi)源項(xiàng)目CppRobotics：

• Path Planning
• Dijkstra
• A Star
• RRT
• Dynamic Window Approach
• Model Predictive Trajectory Generator
• Cubic Spline Planner
• State Lattice Planner
• Frenet Frame Trajectory

5 學(xué)習(xí)方法

入門(mén)新領(lǐng)域的學(xué)習(xí)脈絡(luò)是：工程、理論以及視野三駕馬車齊頭并進(jìn)，以路徑規(guī)劃為例：

5.1 工程

指的是了解各路徑規(guī)劃算法內(nèi)容，一邊從廣度上了解各算法內(nèi)容，一邊從深度上深入學(xué)習(xí)各算法細(xì)節(jié)。關(guān)于路徑規(guī)劃領(lǐng)域的算法，當(dāng)前沒(méi)見(jiàn)全面的教程，但是龔建偉的NMPC運(yùn)動(dòng)規(guī)劃可以參考。

5.2 理論

指的是了解支撐這些算法運(yùn)算數(shù)學(xué)原理以及這些算法產(chǎn)生的原因（數(shù)學(xué)視角）。

• 構(gòu)建目標(biāo)函數(shù)與約束條件同時(shí)求極值來(lái)得到最優(yōu)控制量（路徑），屬于最優(yōu)化理論；
• 在求解最優(yōu)控制量時(shí)大家常見(jiàn)的牛頓法、最速下降法等等這些數(shù)值求解方法，本質(zhì)來(lái)自于數(shù)值求解代數(shù)等式方程，屬于數(shù)值分析；
• 求解過(guò)程中所見(jiàn)到的導(dǎo)數(shù)雅可比矩陣、判斷條件中的向量范數(shù)等等，本質(zhì)就是把一維數(shù)值求解放到了高維，屬于矩陣?yán)碚?/strong>。

5.3 視野

指的是了解路徑規(guī)劃在科研以及企業(yè)的主要應(yīng)用，手段分別為科研文獻(xiàn)以及成果報(bào)告等等。

6 小結(jié)

本文介紹了當(dāng)前路徑規(guī)劃的梗概，了解目前路徑規(guī)劃有那些方法。內(nèi)容很繁雜，很難在沒(méi)有實(shí)際應(yīng)用導(dǎo)向的情況下下短期全部學(xué)通，只能在需要的時(shí)候再重點(diǎn)學(xué)習(xí)。