SLAM从入门到精通（a*搜路算法）

【 声明：版权所有，欢迎转载，请勿用于商业用途。 联系信箱：feixiaoxing @163.com】

        目前机器人常用的搜路算法主要有这么几种，迪杰斯特拉算法、a*算法、贪心算法。每一种算法都有自己的场景和优势，可以灵活选择。但一般来说，客户的场景不算很复杂的话，搜路算法越简单越好，只要能达到最终的目标即可。对于特别复杂的场景，建议也不要通过底层算法的变更来解决业务的问题，这反而是得不偿失的。所以说，这三种算法，如果没有特别原因的话，最好都实现一下，这样方便fae的同学现场部署和实施。搜路算法本身只是一个拓扑算法，它帮助我们分析了目的地本身是否可达，但是机器人能不能过去，这就两说了。

        下面，我们就看下a*算法是怎么实现的。

1、a*算法的核心

        a*算法的核心其实就是F=G+H。其中F是总代价，G是起始点到当前点的代价，H是当前点到目标点的代价。两者加在一起，就是每次选择新插入点的标准。

2、a*算法的流程

a*算法的伪代码流程一般是这样的，

1）将开始点设置为p；

2）p点插入到封闭集当中；

3）搜寻p的所有邻接点，如果邻接点没有在开放集或者封闭集之中，则计算该点的F值，设置该邻接点的parent为p，将临界点插入到开放集当中；

4）判断开放集是否为空，如果为空，则搜路失败，否则继续；

5）从开放集挑出F数值最小的点，作为寻路的下一步起始点；

6）判断该点是否是终点，如果是，结束查找，否则继续；

7）跳转到3继续执行。

3、a*算法的注意事项

        整个a*算法还是不算太复杂的。需要注意的地方只有一处，那就是3）中如果发现邻接点已经在开放集中，那需要重新计算它的G值。一旦发现当前G值更小，则需要同步更新parent、G值和F值。

4、测试代码

        算法的整个过程参考了一本ROS参考书上的python代码。大家可以实际下载下来查看一下效果。代码是用python编写，需要安装matplotlib库。

import matplotlib.pyplot as plt
import math
 
class Node:
    def __init__(self, x, y, parent, cost, index):
        self.x =x
        self.y = y
        self.parent = parent
        self.cost = cost
        self.index = index
 
def calc_path(goaln, closeset):
    rx,ry = [goaln.x], [goaln.y]
    print(closeset[-1])
    parentn = closeset[-1]
        
    while parentn != None:
        rx.append(parentn.x)
        ry.append(parentn.y)
        parentn = parentn.parent
    return rx, ry
 
def astar_plan(sx, sy, gx, gy):
    ox,oy,xwidth,ywidth = map_generation()
    
    plt.figure('Astar algorithm')
    plt.plot(ox, oy, 'ks')
    plt.plot(sx, sy, 'bs')
    plt.plot(gx, gy, 'ro')
    
    motion = motion_model()
    openset, closeset = dict(), dict()
    sidx = sy*xwidth + sx
    gidx = gy*xwidth + gx
    
    starn = Node(sx, sy, None, 0, sidx)
    goaln = Node(gx, gy, None, 0, gidx)
    openset[sidx] = starn
    while 1:
        c_id = min(openset,key=lambda o:openset[o].cost + h_cost(openset[o], goaln))
        curnode = openset[c_id]
        
        if curnode.x == goaln.x and curnode.y == goaln.y:
            print('find goal')
            closeset[-1] = curnode
            break
        else:
            closeset[c_id] = curnode
            plt.plot(curnode.x, curnode.y, 'gx')
            if len(openset.keys())%10 == 0:
                plt.pause(0.01)
            del openset[c_id]
        
        for j in range(len(motion)):
            newnode = Node(curnode.x + motion[j][0],
                            curnode.y + motion[j][1],
                            curnode,
                            curnode.cost + motion[j][2],
                            c_id)
            n_id = index_calc(newnode, xwidth)          
            
            if n_id in closeset:
                continue
                
            if node_verify(newnode, ox, oy):
                continue
                
            if n_id not in openset:
                openset[n_id] = newnode
            else:
                if openset[n_id].cost >= newnode.cost:
                    openset[n_id] = newnode
            
    px,py = calc_path(goaln, closeset)
    return px,py
 
 
def map_generation():
    ox,oy=[],[]
    for i in range(60):
        ox.append(i)
        oy.append(0)
        
    for i in range(60):
        ox.append(i)
        oy.append(60)
        
    for i in range(60):
        ox.append(0)
        oy.append(i)
        
    for i in range(60):
        ox.append(60)
        oy.append(i)
        
    for i in range(25):
        ox.append(i)
        oy.append(20)
        
    for i in range(40):
        ox.append(35)
        oy.append(i)
        
    for i in range(40):
        ox.append(50)
        oy.append(60-i)
    
    minx = min(ox)
    miny = min(oy)
    maxx = max(ox)
    maxy = max(oy)    
    xwidth = maxx-minx
    ywidth = maxy-miny
    return ox,oy,xwidth,ywidth 
 
def motion_model():
    motion= [[1,0,1],
            [1,1,math.sqrt(2)],
            [1,-1,math.sqrt(2)],
            [0,1,1],
            [0,-1,1],
            [-1,1,math.sqrt(2)],
            [-1,0,1],
            [-1,-1,math.sqrt(2)]
    ]
    return motion
 
def h_cost(node, goal):
    w = 1.0
    h = w * (abs(goal.x-node.x) + abs(goal.y-node.y))
    return h
 
def index_calc(node, xwid):
    n_id = node.y * xwid + node.x
    return n_id
    
def node_verify(node, ox, oy):
    if(node.x, node.y) in zip(ox, oy):
        return True
    else:
        return False
        
def main():
    sx,sy=15,15
    gx,gy=55,50
    rx,ry=astar_plan(sx,sy,gx,gy)
    print(rx,ry)
    plt.plot(rx,ry,'r-',linewidth=3)
    plt.show()
    
if __name__ == '__main__':
    main()

        代码中motion_model表示了当前点周围8个点的行驶代价；node_verify则是判断当前点是否在障碍物上；astar_plan是所有算法真正的入口；而map_generation则构建了一个基本的搜寻场景。

5、运行效果图

        运行效果如下所示，供大家参考。直接用python3 astar.py运行即可，