【动手学运动规划】 4.6 Hybrid A*算法-EW帮帮网

勇敢不是不感到恐惧，而是在恐惧面前仍然前行。

—《狮子王》

🏰代码及环境配置：请参考环境配置和代码运行!

4.6.1 概述

外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传

Hybrid A* 是 2004 年 DARPA 城市挑战中的冠军车队使用的全局导航算法。相比与传统的 A* 算法，Hybrid A* 将由基于栅格地图的四邻域、八邻域的节点拓展方式，更换为符合车辆动力学（Kinodynamic）的运动轨迹，见下图：

由于车辆运动学模型的限制，车辆无法安全按照A所生成的路径行走，而Hybrid A算法同时考虑空间连通性和车辆朝向，将二维平面空间和角度同时进行二维离散化，其充分考虑了车辆初始朝向和终点朝向，并且每一次节点扩展都符合车辆的运动学约束，因此生成的路径一定是车辆可以跟随的。

4.6.2 算法详解

4.6.2.1 算法流程

Hybrid A的算法伪代码如下，其与A算法的流程基于一致，主要的不同在于：

$g (n)$ 和 $h (n)$ 的设计
节点的扩展
节点的剪枝

下面将详细介绍这几个关键点。

Algorithm Hybrid A star(G, start, goal):
    let **open_list** be a priority_queue
    g[**start**] = 0
    f[**start**] = g[**start**] + h[**start**]
    **open_list.**push(**start,** f[**start**])
    while **open_list** is not empty do
        v = **open_list**.pop()
        mark v as visted
        if v is the **goal**:
		        return v
        for **all unvisited neighbours** next of v in Graph G do 
		        next_g_cost = g[v] + cost(v, next)
		        next_h_cost = h[next]
		        if next is not in **open_list**:
			        g[next] = next_g_cost
			        f[next] = next_g_cost + next_h_cost
			        **open_list**.push(next, f[next])
			        next.parent = v
			      else:
				      if g[next] > next_g_cost:
					      g[next] = next_g_cost
				        f[next] = next_g_cost + next_h_cost
					      **open_list**.**update_priority**(next, f[next])
					      next.parent = v

4.6.2.2 Hybrid A*搜索树的扩展

满足车辆运动学约束

搜索树扩展过程需要基于车辆运动模型，不同类型的车辆运动模型有差异，详细的车辆模型介绍可参考https://www.helloxiaobai.cn/article/bmp/1-1。

自行车模型的运动学约束如下，其中$(x,y,\theta )$是车辆的姿态；$distance$是车辆在当前行驶方向上前进的距离；$\delta$ 是前轮转角；$L$为车辆轴距。

$$
\begin{align*}                x_{i+ 1} =x_{i}+distance\times cos(\theta )   \\                 y_{i+ 1} =y_{i}+distance\times cos(\theta )     \\                \theta _{i+1}= \theta _{i} + distance\times tan(\delta )/L\end{align*}
$$

车辆控制空间离散化

车辆的控制输入主要有两个：方向盘转角（Steer Angle）和运动方向（Direction）。
- 方向盘转角：从最小转角到最大转角之间按照一定间隔采样。
- 运动方向：向前运动和向后运动。
对运动空间进行扩展探索

对运动空间进行扩展探索的过程就是以车辆的控制参数(Steering Angle和Direction)为输入，从车辆的当前姿态开始，不断采样生成增量扩展的搜索树的过程。

在生成搜索树的过程中，有两个细节：

1）对采样扩展的结果进行碰撞检测，并剔除不满足碰撞检测的扩展。碰撞检测的过程不仅考虑障碍物的位置和形状，还需要考虑车辆自身的位置和形状；此处不再详细讲解，可以参考：

1.3 碰撞检测算法:AABB, SAT
1.4 碰撞检测算法:GJK

2）最大程度的保证采样扩展的起点和终点不在同一个网格中。可以将采样扩展的长度设置为比对角线长度大一点；

4.6.2.3 搜索代价预估(Heuristics)

Cost 在 A* 中被用于指引方向和加速搜索。在 Hybrid A* 中，依然会使用 Heuristic Cost 来指引搜索方向，但除此之外，还增加了很多其他的 Cost 用于控制路径的质量。

Hybird A*算法依赖如下两种Heuristics：Non Holonomic Without Obstacles和Obstacles Without Holonomic。

Non Holonomic Without Obstacles

Non Holonomic Without Obstacles只考虑车辆运动的非完整特性，而不考虑障碍物对车辆运动的限制，即认为车辆在完全没有障碍物的开放空间上运动。

$He u r i s t i cs C os t = M a x (n o nh o l o n o mi c w i t h o u t o b s t a c l escos t, 2 D E u c l i d e an d i s t an ce)$

之所以使用Non Holonomic Without Obstacles Cost和2D Euclidean distance的原因在于，它可以对靠近目标附近的错误Heading搜索进行大量有效的剪枝。

Non Holonomic Without Obstacles Cost的计算过程中，对车辆的运动方向变化、车辆转向角度变化、车辆方向盘转角大小等行为施加一定的惩罚，保证车辆按照预期的行为进行运动。
Obstacles Without Holonomic

Obstacles Without Holonomic只考虑环境中的障碍物，不考虑车辆的运动约束。这种情况的处理就非常常见了，先基于已知环境和已知障碍物构建网格地图，再采用动态规划算法(Dynamic Programming)计算每个网格到达目的地所在网格的Cost(Cost一般使用欧式距离就够了)。

使用该Heuristic的好处是，可以提前发现所有的U型障碍物和Dead Ends，从而引导车辆尽早避开这些区域。

下图为采用不同的启发项得到的的效果图：

- (a)：使用2-D欧式距离，共扩展了20515个节点
- (b)：使用non-holonomic-without-obstacles，共扩展了1465个节点
- (c)：使用non-holonomic-without-obstacles，在dead-ends处扩展了很多无用的节点，一共68730个节点
- (d)：结合non-holonomic-without-obstacles和holonomic-with-obstacles heuristic，共扩展了10588个节点，效果最好。

4.6.2.4 解析扩展（Analytic Expansions）

之前我们提到，Heuristic 的计算可能不考虑地图障碍物的，如果在这种情况下生成出路径恰好没有碰撞风险，那我们就可以直接使用了，这样就大大减少了节点拓展的计算量。但如果要直接使用这段路径，那么就要求 Heuristic 的计算方式符合车辆的运动学约束，而 Dubins Path 和 Reeds Shepp Path 就恰好符合车辆的运动学约束。这两种曲线可以参考以下链接：

2.5 Dubins曲线

2.6 Reeds Shepp曲线

出于计算效率的考虑，不宜对每个节点都应用Dubins或者Reed-Shepp扩展（尤其是在远离目标的位置，因为大多数这样的路径可能会穿过障碍物）。在我们的实现中，我们采用了一个简单的选择规则，即每N个节点中选择一个进行Dubins或者Reed-Shepp扩展，其中N的值随着到目标的成本启发项（cost-to-goal heuristic）的减小而减小（这意味着在接近目标时，会更频繁地进行解析扩展）。

在下图中展示了带有Reed-Shepp扩展的搜索树。通过节点的短增量扩展生成的搜索树部分以黄绿色范围显示，而Reed-Shepp扩展则以一条通向目标的紫色线条表示。我们发现，这种对搜索树的解析扩展在准确性和规划时间方面都带来了显著的益处。

4.6.3 Hybrid A*路径的后处理

通过Hybrid A*算法得到的路径虽然确保了实际可行驶性，但往往还需要进一步的处理和优化，以满足更高级别的驾驶要求，比如更短的行驶时间、更低的能耗以及更平滑的行驶轨迹。这种优化分为两个步骤：

1. 应用非线性优化算法对路径的长度和平滑性进行优化

非线性优化算法是一类用于解决非线性问题中参数最优选择的数学方法。在路径规划中，这类算法可以用来调整路径上的点，使得路径在满足所有约束条件（如避障、最大转弯角度等）的前提下，达到某种最优目标，比如路径长度最短或路径的平滑性最高。

路径长度优化：通过调整路径上的点，使得整个路径的总长度尽可能短，从而减少行驶时间或能耗。
路径平滑性优化：平滑性通常指的是路径上相邻点之间的变化率或曲率是否连续且平缓。通过优化，可以减少急转弯或不必要的转向动作，使车辆或机器人能够以更稳定的姿态行驶，提高乘坐舒适性和安全性。

2. 对优化后的路径进行非参数化插值

非参数化插值是一种数据拟合方法，它不依赖于固定的参数化形式（如多项式或三角函数）来描述数据点之间的关系，而是直接根据数据点本身的特性来生成插值曲线或曲面。在路径规划中，非参数化插值通常用于在优化后的路径点之间生成连续的、平滑的曲线。

插值的目的：由于优化算法可能只调整了路径上的关键点，而这些点之间的路径仍然是未知的或不够平滑的。通过非参数化插值，可以在这些关键点之间填充更多的点，以生成一条连续且平滑的路径，便于车辆或机器人的实际行驶。
插值的优势：非参数化插值可以灵活地适应各种形状的路径，无需预先假设路径的数学形式，因此能够生成更加贴合实际行驶需求的路径。

4.6.4 优缺点

优点
- 应用广泛，不仅适用于简单的二维网格环境，还能扩展到三维空间以及具有复杂障碍物的环境，多用于泊车场景。
- 满足运动学约束，能够规划出更加平滑、可执行的路径。
缺点
- 计算复杂度较高
- 参数调整困难

4.6.c Hybrid A*代码解析

本节提供了Hybrid A*算法的代码测试：

python3 tests/search_based_planning/hybrid_a_star_test.py

4.6.c.1 构图的代码实现

本节构建的图比较简单，主要包含map border（board_x和board_y）和obstacles（ox和oy），读者也可根据需求修改构图方式。

def construct_env_info():
    ox = []
    oy = []
    border_x = []
    border_y = []

    # road border.
    for i in range(0, 60, 1):
        border_x.append(i)
        border_y.append(0.0)
    for i in range(0, 60, 1):
        border_x.append(60.0)
        border_y.append(i)
    for i in range(0, 61, 1):
        border_x.append(i)
        border_y.append(60.0)
    for i in range(0, 61, 1):
        border_x.append(0.0)
        border_y.append(i)

    # obstacle 1.
    for i in range(40, 55, 1):
        for j in range(5, 15, 1):
            ox.append(i)
            oy.append(j)

    # obstacle 2.
    for i in range(40):
        for j in range(20, 25, 1):
            ox.append(j)
            oy.append(i)

    # obstacle 3.
    for i in range(30):
        for j in range(40, 45, 1):
            ox.append(j)
            oy.append(60.0 - i)
    return border_x, border_y, ox, oy

4.6.c.2 Hybrid A*的代码实现

在Hybrid A*算法中，基于环境信息，起始点位置，目标点位置以及x, y ,yaw 的分辨率搜到最优路径：

start:起始点信息：x，y，yaw

goal:目标点信息：x，y，yaw

ox:障碍物和地图边界的x坐标序列

oy:障碍物和地图边界的x坐标序列

xy_resolution: x和y的分辨率，用来encoding坐标值

yaw_resolution:yaw的分辨率，用来encoding横摆角

最终返回一条路径：path: 包含了一系列的x, y和yaw信息

🌞Note: 在进行碰撞检测时会使用kdtree来加速

def hybrid_a_star_planning(start, goal, ox, oy, xy_resolution, yaw_resolution):
    start[2], goal[2] = rs.pi_2_pi(start[2]), rs.pi_2_pi(goal[2])
    tox, toy = ox[:], oy[:]
    obstacle_kd_tree = cKDTree(np.vstack((tox, toy)).T)
    config = Config(tox, toy, xy_resolution, yaw_resolution)
    start_node = Node(
        round(start[0] / xy_resolution),
        round(start[1] / xy_resolution),
        round(start[2] / yaw_resolution),
        True,
        [start[0]],
        [start[1]],
        [start[2]],
        [True],
        cost=0,
    )
    goal_node = Node(
        round(goal[0] / xy_resolution),
        round(goal[1] / xy_resolution),
        round(goal[2] / yaw_resolution),
        True,
        [goal[0]],
        [goal[1]],
        [goal[2]],
        [True],
    )

    openList, closedList = {}, {}
    h_dp = calc_distance_heuristic(
        goal_node.x_list[-1], goal_node.y_list[-1], ox, oy, xy_resolution, BUBBLE_R
    )

    pq = []
    openList[calc_index(start_node, config)] = start_node
    heapq.heappush(
        pq, (calc_cost(start_node, h_dp, config), calc_index(start_node, config))
    )
    final_path = None

    while True:
        if not openList:
            print("Error: Cannot find path, No open set")
            return [], [], []

        cost, c_id = heapq.heappop(pq)
        if c_id in openList:
            current = openList.pop(c_id)
            closedList[c_id] = current
        else:
            continue

        if show_animation:  # pragma: no cover
            plt.plot(current.x_list[-1], current.y_list[-1], "xc")
            plt.savefig(gif_creator.get_image_path())
            # for stopping simulation with the esc key.
            plt.gcf().canvas.mpl_connect(
                "key_release_event",
                lambda event: [exit(0) if event.key == "escape" else None],
            )
            if len(closedList.keys()) % 10 == 0:
                plt.pause(0.001)

        is_updated, final_path = update_node_with_analytic_expansion(
            current, goal_node, config, ox, oy, obstacle_kd_tree
        )

        if is_updated:
            print("path found")
            break

        for neighbor in get_neighbors(current, config, ox, oy, obstacle_kd_tree):
            neighbor_index = calc_index(neighbor, config)
            if neighbor_index in closedList:
                continue
            if (
                neighbor_index not in openList
                or openList[neighbor_index].cost > neighbor.cost
            ):
                heapq.heappush(pq, (calc_cost(neighbor, h_dp, config), neighbor_index))
                openList[neighbor_index] = neighbor

    path = get_final_path(closedList, final_path)
    return path

另外hybrid a star算法中，启发式函数时使用动态规划计算所有点到终点的距离，此时我们是考虑了障碍物的影响的。

def calc_distance_heuristic(gx, gy, ox, oy, resolution, rr):
    goal_node = SimpleNode(round(gx / resolution), round(gy / resolution), 0.0, -1)
    ox = [iox / resolution for iox in ox]
    oy = [ioy / resolution for ioy in oy]
    obstacle_map, min_x, min_y, max_x, max_y, x_w, y_w = calc_obstacle_map(
        ox, oy, resolution, rr
    )
    motion = get_motion_model()
    open_set, closed_set = dict(), dict()
    open_set[calculate_index(goal_node, x_w, min_x, min_y)] = goal_node
    priority_queue = [(0, calculate_index(goal_node, x_w, min_x, min_y))]
    while True:
        if not priority_queue:
            break
        cost, c_id = heapq.heappop(priority_queue)
        if c_id in open_set:
            current = open_set[c_id]
            closed_set[c_id] = current
            open_set.pop(c_id)
        else:
            continue
            
        # expand search grid based on motion model
        for i, _ in enumerate(motion):
            node = SimpleNode(
                current.x + motion[i][0],
                current.y + motion[i][1],
                current.cost + motion[i][2],
                c_id,
            )
            n_id = calculate_index(node, x_w, min_x, min_y)
            if n_id in closed_set:
                continue
            if not verify_node(node, obstacle_map, min_x, min_y, max_x, max_y):
                continue
            if n_id not in open_set:
                open_set[n_id] = node  # Discover a new node
                heapq.heappush(
                    priority_queue,
                    (node.cost, calculate_index(node, x_w, min_x, min_y)),
                )
            else:
                if open_set[n_id].cost >= node.cost:
                    # This path is the best until now. record it!
                    open_set[n_id] = node
                    heapq.heappush(
                        priority_queue,
                        (node.cost, calculate_index(node, x_w, min_x, min_y)),
                    )

    return closed_set

在进行一个节点的analytic expansion时我们使用的时Reeds Shepp 曲线：

def update_node_with_analytic_expansion(current, goal, c, ox, oy, kd_tree):
    path = analytic_expansion(current, goal, ox, oy, kd_tree)
    if path:
        f_x = path.x[1:]
        f_y = path.y[1:]
        f_yaw = path.yaw[1:]
        f_cost = current.cost + calc_rs_path_cost(path)
        f_parent_index = calc_index(current, c)
        fd = []
        for d in path.directions[1:]:
            fd.append(d >= 0
        f_steer = 0.0
        f_path = Node(
            current.x_index,
            current.y_index,
            current.yaw_index,
            current.direction,
            f_x,
            f_y,
            f_yaw,
            fd,
            cost=f_cost,
            parent_index=f_parent_index,
            steer=f_steer,
        )
        return True, f_path
    return False, None

4.6.c.3 Hybrid A*的代码测试

在main 函数中设置起始点，目标点，grid的分辨率和yaw的分辨率，在创建grid map之后，并运行Hybrid A算法，即可找到最优路径。如下图所示，红色路径即为最终Hybrid A搜出来的最优路径。

def main():
    # Set Initial parameters
    start = [10.0, 10.0, np.deg2rad(90.0)]
    goal = [50.0, 50.0, np.deg2rad(-90.0)]

    # construct environment info.
    border_x, border_y, ox, oy = construct_env_info()

    raw_ox = ox
    raw_oy = oy
    ox.extend(border_x)
    oy.extend(border_y)
    path = hybrid_a_star_planning(
        start, goal, ox, oy, XY_GRID_RESOLUTION, YAW_GRID_RESOLUTION
    )

    x = path.x_list
    y = path.y_list
    yaw = path.yaw_list

    if show_animation:
        for i, (i_x, i_y, i_yaw) in enumerate(zip(x, y, yaw), start=1):
            if i % 5 == 0:
                plt.cla()
                plt.plot(border_x, border_y, ".k", markersize=10)
                plt.plot(ox, oy, ".k")
                plt.plot(start[0], start[1], ".r", markersize=20)
                plt.plot(goal[0], goal[1], ".b", markersize=20)
                plt.plot(x, y, "-r", label="Hybrid A* path")
                plt.grid(True)
                plt.axis("equal")
                plot_car(i_x, i_y, i_yaw)
                plt.pause(0.0001)
                plt.savefig(gif_creator.get_image_path())
    gif_creator.create_gif()
    plt.show()

外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传

4.6.5 参考

Paper | Dmitri Dolgov, et. al. Practical Search Techniques in Path Planning for Autonomous Driving

【动手学运动规划】 4.6 Hybrid A*算法