有了全局路徑參考信息，有了局部環(huán)境信息了，有了行為決策模塊輸入的決策信息，下一步自然而然的就要進(jìn)行運(yùn)動(dòng)規(guī)劃，從而生成一條局部的更加具體的行駛軌跡，并且這條軌跡要滿(mǎn)足安全性和舒適性要求。

考慮到車(chē)輛是一個(gè)具有巨大慣性的鐵疙瘩且沒(méi)有瞬間移動(dòng)的功能，如果僅考慮瞬時(shí)狀態(tài)的行駛軌跡，不規(guī)劃出未來(lái)一段時(shí)間有前瞻性的行駛軌跡，那么很容易造成一段時(shí)間后無(wú)解。因此，運(yùn)動(dòng)規(guī)劃生成的軌跡是一種由二維空間和一維時(shí)間組成的三維空間中的曲線(xiàn)，是一種偏實(shí)時(shí)的路徑規(guī)劃。

運(yùn)動(dòng)規(guī)劃的第一步往往采用隨機(jī)采樣算法，即走一步看一步，不斷更新行駛軌跡。代表算法是基于采樣的方法：PRM、RRT、Lattice。這類(lèi)算法通過(guò)隨機(jī)采樣的方式在地圖上生成子節(jié)點(diǎn)，并與父節(jié)點(diǎn)相連，若連線(xiàn)與障礙物無(wú)碰撞風(fēng)險(xiǎn)，則擴(kuò)展該子節(jié)點(diǎn)。重復(fù)上述步驟，不斷擴(kuò)展樣本點(diǎn)，直到生成一條連接起點(diǎn)到終點(diǎn)的路徑。

PRM

概率路標(biāo)法 (Probabilistic Road Maps, PRM），是一種經(jīng)典的采樣方法，由Lydia E.等人在1996年提出。PRM主要包含三個(gè)階段，一是采樣階段，二是碰撞檢測(cè)階段，三是搜索階段。

圖25為已知起點(diǎn)A和終點(diǎn)B的地圖空間，黑色空間代表障礙物，白色空間代表可通行區(qū)域。在采樣階段中，PRM首先在地圖空間進(jìn)行均勻的隨即采樣，也就是對(duì)地圖進(jìn)行稀疏采樣，目的是將大地圖簡(jiǎn)化為較少的采樣點(diǎn)。

在碰撞檢測(cè)階段，剔除落在障礙物上的采樣點(diǎn)，并將剩下的點(diǎn)與其一定距離范圍內(nèi)的點(diǎn)相連，同時(shí)刪除穿越障礙物的連線(xiàn)，從而構(gòu)成一張無(wú)向圖。

在搜索階段，利用全局路徑規(guī)劃算法章節(jié)介紹的搜索算法（Dijkstra、A*等）在無(wú)向圖中進(jìn)行搜索，從而找出一條起點(diǎn)A到終點(diǎn)B之間的可行路徑。

圖27 PRM工作原理示意圖（來(lái)源：https://mp.weixin.qq.com/s/WGOUf7g0C4Od4X9rnCfqxA）

算法步驟可以總結(jié)為：

（1）構(gòu)造無(wú)向圖G =（V，E），其中V代表隨機(jī)采樣的點(diǎn)集，E代表兩采樣點(diǎn)之間所有可能的無(wú)碰撞路徑，G初始狀態(tài)為空。

（2）隨機(jī)撒點(diǎn)，并選取一個(gè)無(wú)碰撞的點(diǎn)c(i)加入到V中。

（3）定義距離r，如果c(i)與V中某些點(diǎn)的距離小于r，則將V中這些點(diǎn)定義為c(i)的鄰域點(diǎn)。

（4）將c(i)與其鄰域點(diǎn)相連，生成連線(xiàn)t，并檢測(cè)連線(xiàn)t是否與障礙物發(fā)生碰撞，如果無(wú)碰撞，則將t加入E中。

（5）重復(fù)步驟2-4，直到所有采樣點(diǎn)（滿(mǎn)足采樣數(shù)量要求）均已完成上述步驟。

（5）采用圖搜索算法對(duì)無(wú)向圖G進(jìn)行搜索，如果能找到起始點(diǎn)A到終點(diǎn)B的路線(xiàn)，說(shuō)明存在可行的行駛軌跡。

PRM算法相比基于搜索的算法，簡(jiǎn)化了環(huán)境、提高了效率。但是在有狹窄通道場(chǎng)景中，很難采樣出可行路徑，效率會(huì)大幅降低。

RRT

快速探索隨機(jī)樹(shù)（Rapidly Exploring Random Trees，RRT），是Steven M. LaValle和James J. Kuffner Jr.在1998年提出的一種基于隨機(jī)生長(zhǎng)樹(shù)思想實(shí)現(xiàn)對(duì)非凸高維空間快速搜索的算法。與PRM相同的是兩者都是基于隨機(jī)采樣的算法，不同的是PRM最終生成的是一個(gè)無(wú)向圖，而RRT生成的是一個(gè)隨機(jī)樹(shù)。RRT的最顯著特征就是具備空間探索的能力，即從一點(diǎn)向外探索拓展的特征。

RRT分單樹(shù)和雙樹(shù)兩種類(lèi)型，單樹(shù)RRT將規(guī)起點(diǎn)作為隨機(jī)樹(shù)的根節(jié)點(diǎn)，通過(guò)隨機(jī)采樣、碰撞檢測(cè)的方式為隨機(jī)樹(shù)增加葉子節(jié)點(diǎn)，最終生成一顆隨機(jī)樹(shù)。而雙樹(shù)RRT則擁有兩顆隨機(jī)樹(shù)，分別以起點(diǎn)和終點(diǎn)為根節(jié)點(diǎn)，以同樣的方式進(jìn)行向外的探索，直到兩顆隨機(jī)樹(shù)相遇，從而達(dá)到提高規(guī)劃效率的目的。

下面以圖28所示的地圖空間為例介紹單樹(shù)RRT算法的實(shí)現(xiàn)過(guò)程。在此地圖空間中，我們只知道起點(diǎn)A和終點(diǎn)B以及障礙物的位置（黑色的框）。

圖28 RRT算法舉例的地圖空間

對(duì)于單樹(shù)RRT算法，我們將起點(diǎn)A設(shè)置為隨機(jī)樹(shù)的根，并生成一個(gè)隨機(jī)采樣點(diǎn)，如圖27所示，隨機(jī)采樣點(diǎn)有下面這幾種情況。

（1）隨機(jī)采樣點(diǎn)1落在自由區(qū)域中，但是根節(jié)點(diǎn)A和隨機(jī)采樣點(diǎn)1之間的連線(xiàn)存在障礙物，無(wú)法通過(guò)碰撞檢測(cè)，采樣點(diǎn)1會(huì)被舍棄，重新再生成隨機(jī)采樣點(diǎn)。

（2）隨機(jī)采樣點(diǎn)2落在障礙物的位置，采樣點(diǎn)2也會(huì)被舍棄，重新再生成隨機(jī)采樣點(diǎn)。

（3）隨機(jī)采樣點(diǎn)3落在自由區(qū)域，且與根節(jié)點(diǎn)A之間的連線(xiàn)不存在障礙物，但是超過(guò)根節(jié)點(diǎn)的步長(zhǎng)限制。但此時(shí)這個(gè)節(jié)點(diǎn)不會(huì)被簡(jiǎn)單的舍棄點(diǎn)，而是會(huì)沿著根節(jié)點(diǎn)和隨機(jī)采樣點(diǎn)3的連線(xiàn)，找出符合步長(zhǎng)限制的中間點(diǎn)，將這個(gè)中間點(diǎn)作為新的采樣點(diǎn)，也就是圖29中的4。

圖29 不同隨機(jī)采樣點(diǎn)舉例

接著我們繼續(xù)生成新的隨機(jī)采樣點(diǎn)，如果新的隨機(jī)采樣點(diǎn)位于自由區(qū)域，那么我們就可以遍歷隨機(jī)樹(shù)中已有的全部節(jié)點(diǎn)，找出距離新的隨機(jī)采樣點(diǎn)最近的節(jié)點(diǎn)，同時(shí)求出兩者之間的距離，如果滿(mǎn)足步長(zhǎng)限制的話(huà)，我們將接著對(duì)這兩個(gè)節(jié)點(diǎn)進(jìn)行碰撞檢測(cè)，如果不滿(mǎn)足步長(zhǎng)限制的話(huà)，我們需要沿著新的隨機(jī)采樣點(diǎn)和最近的節(jié)點(diǎn)的連線(xiàn)方向，找出一個(gè)符合步長(zhǎng)限制的中間點(diǎn)，用來(lái)替代新的隨機(jī)采樣點(diǎn)。最后如果新的隨機(jī)采樣點(diǎn)和最近的節(jié)點(diǎn)通過(guò)了碰撞檢測(cè)，就意味著二者之間存在邊，我們便可以將新的隨機(jī)采樣點(diǎn)添加進(jìn)隨機(jī)樹(shù)中，并將最近的點(diǎn)設(shè)置為新的隨機(jī)采樣點(diǎn)的父節(jié)點(diǎn)。

重復(fù)上述過(guò)程，直到新的隨機(jī)采樣點(diǎn)在終點(diǎn)的步長(zhǎng)限制范圍內(nèi)，且滿(mǎn)足碰撞檢測(cè)。則將新的隨機(jī)采樣點(diǎn)設(shè)為終點(diǎn)B的父節(jié)點(diǎn)，并將終點(diǎn)加入隨機(jī)樹(shù)，從而完成迭代，生成如圖30所示的完整隨機(jī)樹(shù)。

圖30隨機(jī)樹(shù)結(jié)算結(jié)果示例

相比PRM，RRT無(wú)需搜索步驟、效率更高。通過(guò)增量式擴(kuò)展的方式，找到路徑后就立即結(jié)束，搜索終點(diǎn)的目的性更強(qiáng)。但是RRT作為一種純粹的隨機(jī)搜索算法，對(duì)環(huán)境類(lèi)型不敏感，當(dāng)?shù)貓D空間中存在狹窄通道時(shí)，因被采樣的概率低，導(dǎo)致算法的收斂速度慢，效率會(huì)大幅下降，有時(shí)候甚至難以在有狹窄通道的環(huán)境找到路徑。

圖31展示了 RRT應(yīng)對(duì)存在狹窄通道地圖空間時(shí)的兩種表現(xiàn)，一種是RRT很快就找到了出路，一種是一直被困在障礙物里面。



圖31 RRT面對(duì)狹窄通道時(shí)的表現(xiàn)

圍繞如何更好的“進(jìn)行隨機(jī)采樣”、“定義最近的點(diǎn)”以及“進(jìn)行樹(shù)的擴(kuò)展”等方面，誕生了多種改進(jìn)型的算法，包括雙樹(shù)RRT-Connect（雙樹(shù)）、lazy-RRT, RRT-Extend等。

PRM和RRT都是一個(gè)概率完備但非最優(yōu)的路徑規(guī)劃算法，也就是只要起點(diǎn)和終點(diǎn)之間存在有效的路徑，那么只要規(guī)劃的時(shí)間足夠長(zhǎng)，采樣點(diǎn)足夠多，必然可以找到有效的路徑。但是這個(gè)解無(wú)法保證是最優(yōu)的。

采用PRM和RRT等隨機(jī)采樣算法生成的行駛軌跡，大多是一條條線(xiàn)段，線(xiàn)段之間的曲率也不不連續(xù)，這樣的行駛軌跡是不能保證舒適性的，所以還需要進(jìn)一步進(jìn)行曲線(xiàn)平滑、角度平滑處理。代表算法是基于曲線(xiàn)插值的方法：RS曲線(xiàn)、Dubins曲線(xiàn)、多項(xiàng)式曲線(xiàn)、貝塞爾曲線(xiàn)和樣條曲線(xiàn)等。

所有基于曲線(xiàn)插值方法要解決的問(wèn)題就是：在圖32上的若干點(diǎn)中，求出一條光滑曲線(xiàn)盡可能逼近所有點(diǎn)。下文以多項(xiàng)式曲線(xiàn)和貝塞爾曲線(xiàn)為例，介紹曲線(xiàn)插值算法的示例。

圖32 曲線(xiàn)插值方法要解決的問(wèn)題描述

多項(xiàng)式曲線(xiàn)

找到一條曲線(xiàn)擬合所有的點(diǎn)，最容易想到的方法就是多項(xiàng)式曲線(xiàn)。常用的有三階多項(xiàng)式曲線(xiàn)、五階多項(xiàng)式曲線(xiàn)和七階多項(xiàng)式曲線(xiàn)。理論上只要多項(xiàng)式的階數(shù)足夠高，就可以擬合各種曲線(xiàn)。但從滿(mǎn)足需求和工程實(shí)現(xiàn)的角度，階數(shù)越低越好。

車(chē)輛在運(yùn)動(dòng)規(guī)劃中，舒適度是一個(gè)非常重要的指標(biāo)，在物理中衡量舒適性的物理量為躍度(Jerk)，它是加速度的導(dǎo)數(shù)。Jerk的絕對(duì)值越小意味著加速度的變化越平緩，加速度的變化越平緩意味著越舒適。而五次多項(xiàng)式曲線(xiàn)則被證明是在運(yùn)動(dòng)規(guī)劃中可以使Jerk比較小的多項(xiàng)式曲線(xiàn)。

以圖30所示換道場(chǎng)景為例，已知Frenet坐標(biāo)系下?lián)Q道起點(diǎn)和終點(diǎn)的六個(gè)參數(shù)s0、v0、a0、st、vt、at，采用橫縱向解耦分別進(jìn)行運(yùn)動(dòng)規(guī)劃的方法，可得橫向位置x(t)和縱向位置y(t)關(guān)于時(shí)間t的五次多項(xiàng)式表達(dá)式。

五次多項(xiàng)式中存在六個(gè)未知量，將起點(diǎn)和終點(diǎn)已知的六個(gè)參數(shù)代入便可這個(gè)六個(gè)未知量。然后根據(jù)時(shí)間t進(jìn)行合并即可得到橫縱向聯(lián)合控制的曲線(xiàn)，即最終運(yùn)動(dòng)規(guī)劃的曲線(xiàn)。

貝塞爾曲線(xiàn)

對(duì)于比較少的點(diǎn)來(lái)說(shuō)，采用多項(xiàng)式曲線(xiàn)非常合理。但是當(dāng)點(diǎn)比較多時(shí)，為了逼近所有點(diǎn)，就不得不增加多項(xiàng)式的次數(shù)，而由此帶來(lái)的負(fù)面影響就是曲線(xiàn)震蕩。退一步講，即使震蕩能夠被消除，獲得的曲線(xiàn)由于存在非常多的起伏，也不夠光順。而貝塞爾曲線(xiàn)的出現(xiàn)，正好解決了上述問(wèn)題。

1959年，法國(guó)數(shù)學(xué)家保爾·德·卡斯特里使用獨(dú)家配方求出貝塞爾曲線(xiàn)。1962年，法國(guó)雷諾汽車(chē)公司工程師皮埃爾·貝塞爾將自己在汽車(chē)造型設(shè)計(jì)的一些心得歸納總結(jié)，并廣泛發(fā)表。貝塞爾在造型設(shè)計(jì)的心得可簡(jiǎn)單總結(jié)為：先用折線(xiàn)段勾畫(huà)出汽車(chē)的外形大致輪廓，再用光滑的參數(shù)曲線(xiàn)去逼近這個(gè)折線(xiàn)多邊形。

繪制貝塞爾曲線(xiàn)之前，我們需要知道起點(diǎn)和終點(diǎn)的參數(shù)，然后再提供任意數(shù)量的控制點(diǎn)的參數(shù)。如果控制點(diǎn)的數(shù)量為0，則為一階貝塞爾曲線(xiàn)，如果控制點(diǎn)的數(shù)量為1，則為二階貝塞爾曲線(xiàn)，如果控制點(diǎn)的數(shù)量為2，則為三階貝塞爾曲線(xiàn)，依次類(lèi)推。不論是起點(diǎn)、終點(diǎn)還是控制點(diǎn)，它們均代表坐標(biāo)系下的一個(gè)向量。

下面我們以經(jīng)典的二階貝塞爾曲線(xiàn)為例，介紹其繪制方法。如圖33所示，P0和P2為已知的參數(shù)的起點(diǎn)和終點(diǎn)，P1為已知參數(shù)的控制點(diǎn)。首先我們按照起點(diǎn)、控制點(diǎn)、終點(diǎn)的順序依次連接，生成兩條直線(xiàn)。



圖33 二階貝塞爾曲線(xiàn)示例

接著我們以每條直線(xiàn)的起點(diǎn)開(kāi)始，向各自的終點(diǎn)按比例t取點(diǎn)，如圖中的A和B。隨后我們將A和B相連得到一條直線(xiàn)，也按相同的比例t取點(diǎn)，便可得到C點(diǎn)，這也是二階貝塞爾曲線(xiàn)在比例為t時(shí)會(huì)經(jīng)過(guò)的點(diǎn)。比列t滿(mǎn)足如下的公式。

當(dāng)我們比例t一點(diǎn)點(diǎn)變大（從0到1），就得到起點(diǎn)到終點(diǎn)的所有貝塞爾點(diǎn)，所有點(diǎn)相連便繪制出完整的二階貝塞爾曲線(xiàn)C(t)，用公式表達(dá)為。

由二階貝塞爾曲線(xiàn)拓展到N階貝塞爾曲線(xiàn)，可得數(shù)學(xué)表達(dá)式如下。

審核編輯：劉清