rrt運動規劃算法流程圖_自動駕駛運動規劃-Hybird A*算法

下面的視訊展示了DARPA Urban Challenge(DARPA 2007)中Stanford Racing Team的無人車Junior使用的運動規劃(Motion Planning)算法Hybird A*在增量建構的迷宮場景、阻斷的道路場景和停車場狹窄停車位場景的實際表現。

無人車Hybird A*算法表現https://www.zhihu.com/video/1242878670462967808

在迷宮場景中，可以看到随着車輛的運動，周圍在不斷的做增量建構,這也就意味着，迷宮中的障礙物是通過車端的傳感器實時感覺結果得到的。車輛隻能看到它周圍的環境，随着車輛的持續運動，周圍的環境被增量式的建構出來。車輛根據增量建構的場景，實時的調整自身的運動規劃政策。

rrt運動規劃算法流程圖_自動駕駛運動規劃-Hybird A*算法

視訊中黃色的小短線是Hybird A*搜尋樹，可以看到該算法在不同位置、不同轉向角度的情況下都可以實時的為車輛規劃出可行的運動路徑。

在道路阻斷導緻車輛無法繼續前行的場景下，Hybird A*算法可以規劃出掉頭曲線，進而避開阻塞的道路，從其它道路繼續前進。

rrt運動規劃算法流程圖_自動駕駛運動規劃-Hybird A*算法

最後是一個在停車場進入狹窄停車位的場景，可以看到Hybird A*算法可以規劃出複雜的運動路線，使得車輛先前進，再後退，再一次性的進入到狹窄的空車位中。

rrt運動規劃算法流程圖_自動駕駛運動規劃-Hybird A*算法

既然是A*算法，Hybird A*算法具有A*算法的基本特征，即通過目前狀态到目标狀态的代價(Cost)預估，引導車輛更快的收斂到目标狀态。

1、搜尋空間離散化

傳統的開放空間(Open Space)中的A*路徑搜尋的算法，一般将空間劃分為小網格，使用網格中心作為A*路徑規劃的節點，在這些節點中尋求一條規避障礙物的路徑。求解的路徑隻保證連通性，不保證車輛實際可行。

rrt運動規劃算法流程圖_自動駕駛運動規劃-Hybird A*算法

Hybird A*算法同時考慮空間連通性和車輛朝向，将二維平面空間和角度同時進行二維離散化。論文《Practical Search Techniques in Path Planning for Autonomous Driving》中設定的二維網格大小為1m x 1m，角度分辨率為

rrt運動規劃算法流程圖_自動駕駛運動規劃-Hybird A*算法

。在(X,Y,

rrt運動規劃算法流程圖_自動駕駛運動規劃-Hybird A*算法

)三個次元上進行搜尋樹(Search Tree)擴充時，Hybird A*将車輛的運動學限制引入其中，路徑節點可以是二維小網格内的任意一點，保證了搜尋出的路徑一定是車輛實際可以行駛的。

**2、Hybird A*搜尋樹擴充**

2.1 滿足車輛運動學限制

搜尋樹擴充過程需要基于車輛運動模型，不同類型的車輛運動模型有差異，這裡以以前提到的Simple Car Model為例。

半杯茶的小酒杯：自動駕駛中的車輛運動學模型zhuanlan.zhihu.com

rrt運動規劃算法流程圖_自動駕駛運動規劃-Hybird A*算法

Simple Car Model的車輛運動學限制的實作如下，其中(x,y, yaw)是車輛的目前姿态；distance是車輛在目前行駛方向上前進的距離；steer是方向盤與車輛行駛方向的夾角；函數傳回的是滿足車輛運動學限制的下一個姿态點。

def move(x, y, yaw, distance, steer, L=WB):
    x += distance * cos(yaw)
    y += distance * sin(yaw)
    yaw += pi_2_pi(distance * tan(steer) / L)  # distance/2

    return x, y, yaw

2.2 車輛控制空間離散化

車輛的控制輸入主要有兩個：方向盤轉角(Steering Angle)和運動方向(direction)。将方向盤轉角從最小轉角(Min Steering Angle)到最大轉角(Max Steering Angle)按照一定間隔進行采樣；車輛的運動方向隻有兩個：向前運動和向後運動。

for steer in np.concatenate((np.linspace(-MAX_STEER, MAX_STEER, N_STEER),[0.0])):
    for d in [1, -1]:
        yield [steer, d]

2.3 對運動空間進行擴充探索

對運動空間進行擴充探索的過程就是以車輛的控制參數(Steering Angle和Direction)為輸入，從車輛的目前姿态為輸入，不斷采樣生成增量擴充的搜尋樹的過程。

在生成搜尋樹的過程中，有兩個細節：

1）對采樣擴充的結果進行碰撞檢測，并剔除不滿足碰撞檢測的擴充。碰撞檢測的過程不僅考慮障礙物的位置和形狀，還需要考慮車輛自身的位置和形狀；

2）最大程度的保證采樣擴充的起點和終點不在同一個網格中。可以将采樣擴充的長度設定為比對角線長度大一點；

rrt運動規劃算法流程圖_自動駕駛運動規劃-Hybird A*算法

采樣擴充的示例代碼如下：

...
x, y, yaw = current.xlist[-1], current.ylist[-1], current.yawlist[-1]

arc_l = XY_GRID_RESOLUTION * 1.5
xlist, ylist, yawlist = [], [], []
for dist in np.arange(0, arc_l, MOTION_RESOLUTION):
    x, y, yaw = move(x, y, yaw, MOTION_RESOLUTION * direction, steer)
    xlist.append(x)
    ylist.append(y)
    yawlist.append(yaw)

if not check_car_collision(xlist, ylist, yawlist, ox, oy, kdtree):
    return None
...

3 搜尋代價預估(Heuristics)

Hybird A*算法依賴如下兩種Heuristics：Non Holonomic Without Obstacles和Obstacles Without Holonomic。

3.1 Non Holonomic Without Obstacles

Non Holonomic Without Obstacles隻考慮車輛運動的非完整限制特性，而不考慮障礙物對車輛運動的限制，即認為車輛在完全沒有障礙物的開放空間上運動。

Heuristics Cost = Max(non holonomic without obstacles cost, 2D Euclidean distance)

之是以使用Non Holonomic Without Obstacles Cost和2D Euclidean distance的原因在于，它可以對靠近目标附近的錯誤Heading搜尋進行大量有效的剪枝。

Non Holonomic Without Obstacles Cost的計算過程中，對車輛的運動方向變化、車輛轉向角度變化、車輛方向盤轉角大小等行為施加一定的懲罰，保證車輛按照預期的行為進行運動。

rrt運動規劃算法流程圖_自動駕駛運動規劃-Hybird A*算法

3.2 Obstacles Without Holonomic

Obstacles Without Holonomic隻考慮環境中的障礙物，不考慮車輛的運動限制。這種情況的處理就非常常見了，先基于已知環境和已知障礙物建構網格地圖，再采用動态規劃算法(Dynamic Programming)計算每個網格到達目的地所在網格的Cost(Cost一般使用歐式距離就夠了)。

使用該Heuristic的好處是，可以提前發現所有的U型障礙物和Dead Ends，進而引導車輛盡早避開這些區域。

rrt運動規劃算法流程圖_自動駕駛運動規劃-Hybird A*算法

4 Analytic Expansions

前面提到的Hybird A*算法中對運動空間(X, Y,

rrt運動規劃算法流程圖_自動駕駛運動規劃-Hybird A*算法

)和車輛控制參數(Steering Angle)進行了離散化處理，這就決定了它永遠不可能精确的到達連續變化的目标姿态。

為了解決這一問題，論文《Practical Search Techniques in Path Planning for Autonomous Driving》中提出使用基于Reed Shepp模型的Analytic Expansions，即選出一些節點，使用Reed Shepp曲線計算從該節點到目标姿态的路徑，如果該路徑在已知的環境中不與任何障礙物發生碰撞，則将其作為可選的行駛路徑。

半杯茶的小酒杯：自動駕駛運動規劃-Reeds Shepp曲線zhuanlan.zhihu.com

rrt運動規劃算法流程圖_自動駕駛運動規劃-Hybird A*算法

paths = rs.calc_paths(sx, sy, syaw, gx, gy, gyaw,
                        max_curvature, step_size=MOTION_RESOLUTION)

if not paths:
    return None

best_path, best = None, None

for path in paths:
    if check_car_collision(path.x, path.y, path.yaw, ox, oy, kdtree):
        cost = calc_rs_path_cost(path)
        if not best or best > cost:
            best = cost
            best_path = path

return best_path

到目前為止，我們通過Hybird A*算法得到了一條實際可行駛的運動路徑，但這樣的路徑往往需要進一步的優化才能得到更好的預期駕駛行為。這種優化分為兩個步驟：

1）應用非線性優化算法(non-linear optimization)對路徑的長度(length)和平滑性(smoothness)進行優化；

2）對優化後的路徑進行非參數化的插值(non-parametric interpolation)。

如何對規劃出的路徑進行繼續優化下周繼續研究！

參考材料

1、Explaining the Hybrid A Star pathfinding algorithm for selfdriving cars.(https://blog.habrador.com/2015/11/explaining-hybrid-star-pathfinding.html)

2、Udacity A* in Action-Artificial Intelligence for Robotics(https://www.youtube.com/watch?v=qXZt-B7iUyw)

3、Practical Search Techniques in Path Planning for Autonomous Driving(https://ai.stanford.edu/~ddolgov/papers/dolgov_gpp_stair08.pdf)

4、文中代碼出處(https://github.com/gyq18/PythonRobotics/blob/9a9ea3b3d7cc2f5e4cb10b384610964044f17583/PathPlanning/HybridAStar/hybrid_a_star.py)

注：本文首發于微信公衆号【半杯茶的小酒杯】，轉載請注明出處，謝謝！

rrt運動規劃算法流程圖_自動駕駛運動規劃-Hybird A*算法

1、搜尋空間離散化

2、Hybird A*搜尋樹擴充

2.1 滿足車輛運動學限制

2.2 車輛控制空間離散化

2.3 對運動空間進行擴充探索

3 搜尋代價預估(Heuristics)

3.1 Non Holonomic Without Obstacles

3.2 Obstacles Without Holonomic

4 Analytic Expansions

參考材料

推薦閱讀：

**2、Hybird A*搜尋樹擴充**