路徑規劃技術概述

Planning實際上是基於路線規劃模組規劃出全域性的路線之後，為車輛尋找一條符合車輛運動學模型原理，並且能夠躲避動態或者靜態障礙點，符合各種交通訊息限制的路點。
一般說來，Planning包括下面三個層次：

• 為車輛尋找一條最優的幾何軌跡。
  
  • 通過增量搜尋或者離散的幾何道路空間。這裡一般用移動機器人在尋路的時候比較常用的RRT演算法和lattice planner(MIT的論文)。
  • 從各種可能的最終狀態中選取最好的動作行為，也就是說列出可行的路徑軌跡，然後選取最優的動作。（Local search）。
• 搜尋最優的策略。
• 搜尋最優的軌跡，優化之前規劃的幾何路徑，使其符合給定的限制，如車輛運動學和動力學的限制。
  

為了直觀的認識路徑規劃相關內容，可以用下面圖示來解釋一下：

圖a：
灰色的點是路點，path planning的作用就是按照路點生成一條几何軌跡，同時要考慮到前車以及周邊車道、障礙物，生成一條可行的幾何路徑。
圖b:
策略規劃的主要作用就是在關鍵結點要做出合理的決策，比如在前車行進速度慢，需要超車；在路口滿足轉彎條件需要左轉或者右轉，或者直行；在路口根據交通燈資訊決定等待還是立即轉彎。
圖c:
圖c就是軌跡規劃的直觀的表達，當轉彎的決策已經做出，需要轉彎，但是如何轉彎，以多大的速度轉彎，方向盤轉角的規劃，左轉和右轉路徑曲率的如何區別的設計，怎樣在右轉的時候車輛不碰到右邊沿，都是軌跡規劃需要優化的細節。

下面就按照這三個方面大概的闡述一些planning的方法。

Incremental search

增量式搜尋演算法就是在之前搜尋結果的基礎上，重新利用以前的搜尋結果來加快搜索速度。
目前比較常見的增量式搜尋演算法是RRT（Rapidly-exploring Random Tree）和Lattice Planners。

RRT

RRT演算法就是移動機器人中比較常見的路徑規劃演算法，RRT演算法構造一個樹的資料結構，通過隨機取樣去構造幾何路徑，直到達到目標點。
基本的演算法過程如下：

1 Initialise a tree (G) starting from point (x0)
2 Repeat
3 Sample a random configuration xrandom from the configuration space (C)
4 Flag x

NEAR the closest point of the initialised tree (G) to xrandom
5 Select the input u which minimises the distance (xrandom , xNEAR)
6 After Dt and the application of u , flag the new configuration xnew
7 Add xnew to G
8 Add the edge between xnew and xNEAR to G
9 Return G

在加入xnew之前要確保此點是沒有障礙物阻礙的，需要一個碰撞檢測演算法。
其中關於RRT的論文有[1]

Lattice planners

在第一個路徑規劃總結中，我們已經講過了，state lattice是一種比較典型的搜尋空間，而lattice planner就是基於state lattice的一種搜尋方法。
這種規劃方法需要結合障礙點的資訊和一些避障的演算法一起，在與規劃的lattice 空間中尋找可行的路徑。

Local search

實時的搜尋整個搜尋空間顯然是缺乏效率的，那麼就可以預先搜尋一個預瞄距離的空間，然後進行選取路徑。
對於on-road的自動駕駛的local search來說，目前比較常見的演算法是搜尋一組含有特性的幾何曲線（一般有迴旋曲線或者樣條曲線），這些曲線之間會有一定的側向偏移。每一個待選的曲線都會用一個特性的cost function去評價這個曲線的優劣，一般的評價方程可以是速度、時間消耗或者是碰撞檢測。

一般規劃的local search的曲線族可以分為兩類：在action space 中帶有側向偏移的曲線族；在state space中帶有側向偏移的曲線族。下圖所示：

左圖是在action space空間規劃的曲線族，叫做roll out trajectory
右圖是在state space規劃的曲線族，叫做partial motion planning 演算法。有關PMP的論文，可以參考PMP論文