机器人路径规划_A*算法

机器人路径规划_A*算法

 

原理             

 

基本思想是：把初始点到达该节点的代价g(n)和从该节点到目标节点的代价h(n)结合起来对节点进行评价：

                 f(n)=g(n)+h(n)

其中：g(n)表示从起始节点到节点n的路径代价

      h(n)表示从节点n到目标节点的最低代价路径的估计带价值

      f(n)就是经过节点n到目标节点的最低代价解的估计代价

采用 A*方法用于移动机器人的路径规划时, 机器人首先按照已知的环境地图规划出一条路径 , 然后沿着这条轨迹运动, 当机器人传感器探测到环境信息和原有的环境信息不一致的时候, 机器人重新规划从当前位置到目标点的路径 .如此循环直至机器人到达目标点或者发现目标点不可达.但如果机器人在动态环境或者未知环境中运动的时候 ,机器人很可能非常频繁地遇到当前探测环境信息和先验环境信息不匹配的情形 , 这就需要进行路径再规划.重新规划算法仍然是个从当前位置到目标点的全局搜索的过程 , 运算量较大.在重新规划期间 ,机器人或者选择停下来等待新的生成路径 ,或者按照错误的路径继续运动.因此, 快速的重新规划算法是非常重要的.A*方法采用栅格表示地图 , 栅格粒度越小 ,障碍物的表示也就越精确, 但是同时算法搜索的范围会按指数增加 .采用改进人工势场的局部路径规划方法对 A*方法进行优化 , 可以有效增大A*方法的栅格粒度, 达到降低 A*方法运算量的目的。

 

 

A*方法是在二维X-Y平面进行路径规划的，相邻两点之间的夹角一定是π/4的整数倍。

优点

是一种静态路网中求解最短路最有效的直接搜索方法。

 

缺点

不仅要考虑搜索节点的多少，同时还要考虑搜索节点被搜索的次数，搜索的复杂度高，计算量大。

A* 和  Hybrid A*的对比

2019.6月补充代码实现

A*  python实现：

 

 

 

 

 

 

# -*- coding: utf-8 -*-
"""
Created on Mon Aug 20 11:07:25 2018

@author: lilyx
"""

# -----------
# User Instructions:
#
# Modify the the search function so that it becomes
# an A* search algorithm as defined in the previous
# lectures.
#
# Your function should return the expanded grid
# which shows, for each element, the count when
# it was expanded or -1 if the element was never expanded.
# 
# If there is no path from init to goal,
# the function should return the string 'fail'
# ----------

grid = [[0, 1, 0, 0, 0, 0],
        [0, 1, 0, 0, 0, 0],
        [0, 1, 0, 0, 0, 0],
        [0, 1, 0, 0, 0, 0],
        [0, 0, 0, 0, 1, 0]]
heuristic_old = [[9, 8, 7, 6, 5, 4],
             [8, 7, 6, 5, 4, 3],
             [7, 6, 5, 4, 3, 2],
             [6, 5, 4, 3, 2, 1],
             [5, 4, 3, 2, 1, 0]]

heuristic = [[0 for col in range(len(grid[0]))] for row in range(len(grid))]

init = [0, 0]
goal = [len(grid)-1, len(grid[0])-1]
cost = 1

delta = [[-1, 0 ], # go up
         [ 0, -1], # go left
         [ 1, 0 ], # go down
         [ 0, 1 ]] # go right

delta_name = ['^', '<', 'v', '>']

def search(grid,init,goal,cost,heuristic):
    # ----------------------------------------
    # modify the code below
    # ----------------------------------------
    closed = [[0 for col in range(len(grid[0]))] for row in range(len(grid))]
    closed[init[0]][init[1]] = 1

    expand = [[-1 for col in range(len(grid[0]))] for row in range(len(grid))]
    action = [[-1 for col in range(len(grid[0]))] for row in range(len(grid))]
    

    x = init[0]
    y = init[1]
    g = 0
    h = heuristic[x][y]
    f = g+h

    open = [[f,g, h,x, y]]

    found = False  # flag that is set when search is complete
    resign = False # flag set if we can't find expand
    count = 0
    
    while not found and not resign:
        if len(open) == 0:
            resign = True
            return "Fail"
        else:
            open.sort()
            open.reverse()
            next = open.pop()
            x = next[3]
            y = next[4]
            g = next[1]
            expand[x][y] = count
            count += 1
            
            if x == goal[0] and y == goal[1]:
                found = True
            else:
                for i in range(len(delta)):
                    x2 = x + delta[i][0]
                    y2 = y + delta[i][1]
                    if x2 >= 0 and x2 < len(grid) and y2 >=0 and y2 < len(grid[0]):
                        if closed[x2][y2] == 0 and grid[x2][y2] == 0:
                            g2 = g + cost
                            h2 = heuristic[x2][y2] #为新的扩展到的点计算启发函数值
                            f2 = g2 + h2
                            open.append([f2,g2,h2, x2, y2])
                            closed[x2][y2] = 1
                            
                            
    for i in range(len(expand)):
        print(expand[i])
         
        
        

    return expand
    
    
  
search(grid,init,goal,cost,heuristic)

结果为：