決策規(guī)劃系列：運(yùn)動規(guī)劃常用算法

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

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

運(yùn)動規(guī)劃的第一步往往采用隨機(jī)采樣算法，即走一步看一步，不斷更新行駛軌跡。代表算法是基于采樣的方法：PRM、RRT、Lattice。這類算法通過隨機(jī)采樣的方式在地圖上生成子節(jié)點(diǎn)，并與父節(jié)點(diǎn)相連，若連線與障礙物無碰撞風(fēng)險，則擴(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主要包含三個階段，一是采樣階段，二是碰撞檢測階段，三是搜索階段。

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

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

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

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

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

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

（2）隨機(jī)撒點(diǎn)，并選取一個無碰撞的點(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)相連，生成連線t，并檢測連線t是否與障礙物發(fā)生碰撞，如果無碰撞，則將t加入E中。

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

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

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

RRT

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

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

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

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

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

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

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

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

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

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

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

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

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

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



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

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

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

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

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

圖32 曲線插值方法要解決的問題描述

多項(xiàng)式曲線

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

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

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

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

貝塞爾曲線

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

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

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

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



圖33 二階貝塞爾曲線示例

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

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

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

審核編輯：劉清