动力学约束下的运动规划算法——Hybrid A*算法（附程序实现及详细解释）

   本文主要介绍动力学约束下的运动规划算法中非常经典的Hybrid A*算法，大致分为三部分，第一部分是在传统A * 算法的基础上，对Hybrid A * 算法的原理、流程进行理论介绍。第二部分是详细分析 MotionPlanning运动规划库中Hybrid A * 算法的源码，进一步深入对Hybrid A * 算法的具体细节 进行理解。 第三部分是结合前面第一部分的理论和第二部分的详细源码，对Hybrid A * 算法的流程进行综合的概括总结。

   另外，本文介绍的源码来源于zhm_real/MotionPlanning运动规划库，我进行了简单的修改，并 Hybrid A * 算法涉及到的源码从该运动规划库中独立摘出来，我会将其以绑定资源的形式绑定在博客中，有需要的可自行免费下载。

•    一、Hybrid A*算法 理论介绍

  •    Hybrid A* 算法结合了传统的A*算法和连续状态空间中的运动规划方法。它旨在在具有非完整约束（如车辆的转弯半径）的车辆等机器人中进行高效的路径规划。

    ---

       传统的A * 算法是在栅格地图中进行搜索，可行路径中的点，都是栅格地图的中心点，如下面的第一幅图所示，lattice planner算法是先构建一个用于搜索的lattice图，如下面的第二幅图所示，Hybrid A* 算法结合了A * 算法和lattice planner算法的思想，将栅格地图的路径搜索与lattice图结合起来，在搜索过程中选取不同的控制量预演一段轨迹，且保持在每个栅格中仅保留一个可行的状态，如下面的第三幅图所示

    

       与传统的A * 相比，Hybrid A算法的g(n)和h(n)函数更加复杂；传统的A * 算法在拓展的时候会搜索当前节点周围所有的邻居节点，而Hybrid A算法是在不同的控制输入的驱动下演化出一系列状态点，作为其要拓展的邻居，若当前节点n找出的某个邻居m所在的栅格之前是没有被探索过的，即该栅格中没有记录状态点，则把状态点m记录在该栅格中，若当前节点n找出的某个邻居m所在的栅格中已经记录了状态点，此时需要进行判断，若当前节点n拓展出的状态点m的累计代价更小，则将该栅格中的记录的状态点更改为m，否则不更改。

    

    ---

       如何合理的设置Hybrid A*算法的启发式函数呢？

       选择1是采用二维的欧式距离作为启发式函数，如下面的a图所示；选择2是采用考虑动力学但不考虑障碍物的启发式函数，如下面的b图所示，但在障碍物较多的环境，该启发式函数效率会变得很低，如下面的c图所示，选择3是采用考虑动力学但不考虑障碍物与不考虑动力学但考虑障碍物的一个最短路径相结合的启发式函数，如下面的d图所示

    

       Hybrid A * 算法中作者还提出了一种Analytic Expansions的思想，即在搜索树生长的过程中，在添加了一个新的节点后，会以一定的概率去直接解从该新节点到目标点的理论上的最优可行路径，如果这一段路径恰好满足动力学约束，且不与障碍物相碰，则我们成功找到了一条可行路径（由Hybrid A * 算法搜索生成的从起点到该新的拓展节点的路径+该新的扩展节点到目标点生成的理论最优路径 两段构成），提前结束规划。

       但频繁的使用该方法，在障碍物较多的环境下，往往会降低效率，所以，常常按一定的频率去使用，比如每扩展20个节点，尝试一次。随着新的节点距离目标点越来越近，通过该方式获得可行路径的概率也会增加，所以这个尝试频率，可以随着新的拓展节点距离目标点距离的接近而增大。

    

    

       下图给出了一些Hybrid A*算法的应用示例

    

    

       Hybrid A* 算法的优点在于它在离散状态空间和连续状态空间之间进行了平衡，兼顾了路径的最短性和机器人的动力学特性。然而，这也使得算法比传统的A算法更为复杂，并且需要更多的计算资源来处理连续状态空间中的运动规划。总的来说，Hybrid A 算法是一种强大的路径规划算法，特别适用于需要考虑机器人动力学约束的情况，如自动驾驶车辆、无人机等。

    ---

    

    •    二、Hybrid A*算法python程序解读

      •    zhm_real/MotionPlanning运动规划库中Hybrid A * 算法的主要程序在hybrid_astar.py中，经过上面的介绍我们知道在Hybrid A*算法运行过程中会掉传统的A * 算法以及Reeds sheep算法，所以在hybrid_astar.py中分别调用了astar.py、reeds_shepp.py、以及可视化绘图所需的draw.py。

           涉及到的astar.py和reeds_shepp.py这两个文件中的源码在之前的文章中已经详细介绍过了，链接如下，请大家自行前往查看，所以这里仅放一下这两个文件的源码，方便大家使用，不再作详细介绍

        •    zhm_real/MotionPlanning运动规划库中A*算法源码详细解读（点击可跳转）

          •    reeds_sheep运动规划算法Python源码分析（点击可跳转）

            •    astar.py源码如下：

              • import heapq
                import math
                import numpy as np
                import matplotlib.pyplot as plt
                class Node:
                    def __init__(self, x, y, cost, pind):
                        self.x = x  # x position of node
                        self.y = y  # y position of node
                        self.cost = cost  # g cost of node
                        self.pind = pind  # parent index of node
                class Para:
                    def __init__(self, minx, miny, maxx, maxy, xw, yw, reso, motion):
                        self.minx = minx
                        self.miny = miny
                        self.maxx = maxx
                        self.maxy = maxy
                        self.xw = xw
                        self.yw = yw
                        self.reso = reso  # resolution of grid world
                        self.motion = motion  # motion set
                def astar_planning(sx, sy, gx, gy, ox, oy, reso, rr):
                    """
                    return path of A*.
                    :param sx: starting node x [m]
                    :param sy: starting node y [m]
                    :param gx: goal node x [m]
                    :param gy: goal node y [m]
                    :param ox: obstacles x positions [m]
                    :param oy: obstacles y positions [m]
                    :param reso: xy grid resolution
                    :param rr: robot radius
                    :return: path
                    """
                    n_start = Node(round(sx / reso), round(sy / reso), 0.0, -1)
                    n_goal = Node(round(gx / reso), round(gy / reso), 0.0, -1)
                    ox = [x / reso for x in ox]
                    oy = [y / reso for y in oy]
                    P, obsmap = calc_parameters(ox, oy, rr, reso)
                    open_set, closed_set = dict(), dict()
                    open_set[calc_index(n_start, P)] = n_start
                    q_priority = []
                    heapq.heappush(q_priority,
                                   (fvalue(n_start, n_goal), calc_index(n_start, P)))
                    while True:
                        if not open_set:
                            break
                        _, ind = heapq.heappop(q_priority)
                        n_curr = open_set[ind]
                        closed_set[ind] = n_curr
                        open_set.pop(ind)
                        for i in range(len(P.motion)):
                            node = Node(n_curr.x + P.motion[i][0],
                                        n_curr.y + P.motion[i][1],
                                        n_curr.cost + u_cost(P.motion[i]), ind)
                            if not check_node(node, P, obsmap):
                                continue
                            n_ind = calc_index(node, P)
                            if n_ind not in closed_set:
                                if n_ind in open_set:
                                    if open_set[n_ind].cost > node.cost:
                                        open_set[n_ind].cost = node.cost
                                        open_set[n_ind].pind = ind
                                else:
                                    open_set[n_ind] = node
                                    heapq.heappush(q_priority,
                                                   (fvalue(node, n_goal), calc_index(node, P)))
                    pathx, pathy = extract_path(closed_set, n_start, n_goal, P)
                    return pathx, pathy
                def calc_holonomic_heuristic_with_obstacle(node, ox, oy, reso, rr):
                    n_goal = Node(round(node.x[-1] / reso), round(node.y[-1] / reso), 0.0, -1)
                    ox = [x / reso for x in ox]
                    oy = [y / reso for y in oy]
                    P, obsmap = calc_parameters(ox, oy, reso, rr)
                    open_set, closed_set = dict(), dict()
                    open_set[calc_index(n_goal, P)] = n_goal
                    q_priority = []
                    heapq.heappush(q_priority, (n_goal.cost, calc_index(n_goal, P)))
                    while True:
                        if not open_set:
                            break
                        _, ind = heapq.heappop(q_priority)
                        n_curr = open_set[ind]
                        closed_set[ind] = n_curr
                        open_set.pop(ind)
                        for i in range(len(P.motion)):
                            node = Node(n_curr.x + P.motion[i][0],
                                        n_curr.y + P.motion[i][1],
                                        n_curr.cost + u_cost(P.motion[i]), ind)
                            if not check_node(node, P, obsmap):
                                continue
                            n_ind = calc_index(node, P)
                            if n_ind not in closed_set:
                                if n_ind in open_set:
                                    if open_set[n_ind].cost > node.cost:
                                        open_set[n_ind].cost = node.cost
                                        open_set[n_ind].pind = ind
                                else:
                                    open_set[n_ind] = node
                                    heapq.heappush(q_priority, (node.cost, calc_index(node, P)))
                    hmap = [[np.inf for _ in range(P.yw)] for _ in range(P.xw)]
                    for n in closed_set.values():
                        hmap[n.x - P.minx][n.y - P.miny] = n.cost
                    return hmap
                def check_node(node, P, obsmap):
                    if node.x = P.maxx or \
                            node.y = P.maxy:
                        return False
                    if obsmap[node.x - P.minx][node.y - P.miny]:
                        return False
                    return True
                def u_cost(u):
                    return math.hypot(u[0], u[1])
                def fvalue(node, n_goal):
                    return node.cost + h(node, n_goal)
                def h(node, n_goal):
                    return math.hypot(node.x - n_goal.x, node.y - n_goal.y)
                def calc_index(node, P):
                    return (node.y - P.miny) * P.xw + (node.x - P.minx)
                def calc_parameters(ox, oy, rr, reso):
                    minx, miny = round(min(ox)), round(min(oy))
                    maxx, maxy = round(max(ox)), round(max(oy))
                    xw, yw = maxx - minx, maxy - miny
                    motion = get_motion()
                    P = Para(minx, miny, maxx, maxy, xw, yw, reso, motion)
                    obsmap = calc_obsmap(ox, oy, rr, P)
                    return P, obsmap
                def calc_obsmap(ox, oy, rr, P):
                    obsmap = [[False for _ in range(P.yw)] for _ in range(P.xw)]
                    for x in range(P.xw):
                        xx = x + P.minx
                        for y in range(P.yw):
                            yy = y + P.miny
                            for oxx, oyy in zip(ox, oy):
                                if math.hypot(oxx - xx, oyy - yy) = 0.0:
                        v = M(phi - t)
                        if v >= 0.0:
                            return True, t, u, v
                    return False, 0.0, 0.0, 0.0
                def LSR(x, y, phi):
                    u1, t1 = R(x + math.sin(phi), y - 1.0 - math.cos(phi))
                    u1 = u1 ** 2
                    if u1 >= 4.0:
                        u = math.sqrt(u1 - 4.0)
                        theta = math.atan2(2.0, u)
                        t = M(t1 + theta)
                        v = M(t - phi)
                        if t >= 0.0 and v >= 0.0:
                            return True, t, u, v
                    return False, 0.0, 0.0, 0.0
                def LRL(x, y, phi):
                    u1, t1 = R(x - math.sin(phi), y - 1.0 + math.cos(phi))
                    if u1 = 0.0 and u  0.0 and 0.0 = 2.0:
                        t = theta
                        u = 2.0 - rho
                        v = M(t + 0.5 * PI - phi)
                        if t >= 0.0 and u = 0.0 and u = 0.0:
                                return True, t, u, v
                    return False, 0.0, 0.0, 0.0
                def CCSCC(x, y, phi, paths):
                    flag, t, u, v = LRSLR(x, y, phi)
                    if flag:
                        paths = set_path(paths, [t, -0.5 * PI, u, -0.5 * PI, v], ["WB", "R", "S", "WB", "R"])
                    flag, t, u, v = LRSLR(-x, y, -phi)
                    if flag:
                        paths = set_path(paths, [-t, 0.5 * PI, -u, 0.5 * PI, -v], ["WB", "R", "S", "WB", "R"])
                    flag, t, u, v = LRSLR(x, -y, -phi)
                    if flag:
                        paths = set_path(paths, [t, -0.5 * PI, u, -0.5 * PI, v], ["R", "WB", "S", "R", "WB"])
                    flag, t, u, v = LRSLR(-x, -y, phi)
                    if flag:
                        paths = set_path(paths, [-t, 0.5 * PI, -u, 0.5 * PI, -v], ["R", "WB", "S", "R", "WB"])
                    return paths
                def generate_local_course(L, lengths, mode, maxc, step_size):
                    point_num = int(L / step_size) + len(lengths) + 3
                    px = [0.0 for _ in range(point_num)]
                    py = [0.0 for _ in range(point_num)]
                    pyaw = [0.0 for _ in range(point_num)]
                    directions = [0 for _ in range(point_num)]
                    ind = 1
                    if lengths[0] > 0.0:
                        directions[0] = 1
                    else:
                        directions[0] = -1
                    if lengths[0] > 0.0:
                        d = step_size
                    else:
                        d = -step_size
                    ll = 0.0
                    for m, l, i in zip(mode, lengths, range(len(mode))):
                        if l > 0.0:
                            d = step_size
                        else:
                            d = -step_size
                        ox, oy, oyaw = px[ind], py[ind], pyaw[ind]
                        ind -= 1
                        if i >= 1 and (lengths[i - 1] * lengths[i]) > 0:
                            pd = -d - ll
                        else:
                            pd = d - ll
                        while abs(pd)  0.0:
                        directions[ind] = 1
                    else:
                        directions[ind] = -1
                    return px, py, pyaw, directions
                def generate_path(q0, q1, maxc):
                    dx = q1[0] - q0[0]
                    dy = q1[1] - q0[1]
                    dth = q1[2] - q0[2]
                    c = math.cos(q0[2])
                    s = math.sin(q0[2])
                    x = (c * dx + s * dy) * maxc
                    y = (-s * dx + c * dy) * maxc
                    paths = []
                    paths = SCS(x, y, dth, paths)
                    paths = CSC(x, y, dth, paths)
                    paths = CCC(x, y, dth, paths)
                    paths = CCCC(x, y, dth, paths)
                    paths = CCSC(x, y, dth, paths)
                    paths = CCSCC(x, y, dth, paths)
                    return paths
                # utils
                def pi_2_pi(theta):
                    while theta > PI:
                        theta -= 2.0 * PI
                    while theta