A星 混合A星

通俗易懂聊A*算法 - 知乎 (zhihu.com)

伪代码：

输入：网格连通图，起点Node_start，终点Node_end
输出：联通起点和终点的路径

# 初始化
初始化Openlist为空，初始化图中所有的节点，也就是将所有属性置空。
将被障碍物占据的节点的is_in_Closedlist置为1
为初始节点完善相关属性：g = 0；利用曼哈顿距离计算h；parent_idx = null ；is_in_Openlist = 0；is_in_Closedlist = 0
将初始节点加入Openlist，并将初始节点的is_in_Openlist置为1

# 循环
while Openlist ！= 空 ：
    从Openlist中选取一个代价f最小的节点，记为Node_current
    拓展Node_current，将其所有子节点放置在临时集合A中
    for each Node_child in 集合A ：
        if (Node_child 超出地图边界)) or (Node_child的is_in_Closedlist==1):
            continue
        if Node_child的is_in_Openlist==0 :
            计算Node_child的历史代价g，预期代价h，总代价f
            将Node_child的父节点记为 parent_idx = Node_current
            将Node_child放入Openlist，并将Node_child的is_in_Openlist置为1
        else ：
            计算将Node_child的父节点换成Node_current的话，总代价f_alternative
            if f_alternative < Node_child原有的f ：
                将Node_child的父节点更新为 parent_idx = Node_current
                更新Node_child的历史代价g和总代价f
    将Node_current从Openlist中移除。
    将Node_current对应属性改为：is_in_Openlist = 0；is_in_Closedlist = 1
    if 集合A中包含终点Node_end ：
        break

# 输出路径
构建空列表B，将Node_end放入列表B
if 列表B中最后一个节点的父节点不是空 ：
    B.append(将列表B中最后一个节点的父节点)
将列表B逆序打印就是路径

混合A*算法详解 - 知乎 (zhihu.com) 

伪代码：

输入：环境布局，始末位姿
输出：联通起点和终点的路径

# 初始化
初始化Openlist为空，iter=0，ready_flag=false
为初始节点完善相关属性：g = 0；计算h；parent_idx = null ；is_in_Openlist = 0；is_in_Closedlist = 0
将初始节点加入Openlist，并将初始节点的is_in_Openlist置为1

# 循环
while Openlist ！= 空 and ready_flag ！= true：
    iter = iter + 1
    从Openlist中选取一个代价f最小的节点，记为Node_current
    if （iter - 1）可以被 N_rs 整除：
        使用Reeds-Shepp曲线生成当前节点到终点的路径
        if 生成的路径满足碰撞检测：
            ready_flag = true
            定义中止节点Node_end
            设置Node_end的父节点为当前节点
            设置Node_end的轨迹为刚生成的那段轨迹
            break
    拓展Node_current，将其所有子节点放置在临时集合A中
    for each Node_child in 集合A ：
        if (Node_child 超出地图边界)) or (Node_child的is_in_Closedlist==1):
            continue
        if Node_child的is_in_Openlist==0 :
            if 碰撞检测不通过：
                设置Node_child的is_in_Closelist置为1
                continue
            计算Node_child的历史代价g，预期代价h，总代价f，轨迹
            将Node_child的父节点记为 parent_idx = Node_current
            将Node_child放入Openlist，并将Node_child的is_in_Openlist置为1
        else ：
            计算将Node_child的父节点换成Node_current的话，总代价f_alternative
            if f_alternative < Node_child原有的f ：
                将Node_child的父节点更新为 parent_idx = Node_current
                更新Node_child的历史代价g和总代价f
    将Node_current从Openlist中移除。
    将Node_current对应属性改为：is_in_Openlist = 0；is_in_Closedlist = 1
    if 集合A中某节点位姿与终点位姿偏差在某一较小范围内 ：
        ready_flag = true
        将该节点定义为Node_end
        break

if ready_flag ！= true：
    return  #表示没有找到路径
    
# 输出路径
构建空列表B，将Node_end的轨迹放入列表B
设置Node_current为Node_end
while Node_current的父节点不是空 ：
    将Node_current的父节点的轨迹加入集合B
    将Node_current的父节点设置为Node_current
将列表B逆序打印就是路径