ROS中使用A星算法进行路径规划

前言

由于本人这学期修了一门《智能工程》的课程，课程的大作业要求大家自主编写机器人导航程序，包括：路径规划、轨迹规划、控制器、机器人建模等相关程序，因此特在此写下一篇博客，记录开发过程。

这是第一篇博客，用于记录如何将自己编写的A星算法用于ROS中。

python编写A星算法

描述

先声明：在使用Python编写A*算法的过程中，深深的体会到了C++的链表、指针的便利性，但奈何已经1年多没使用C++写过程序了，所以还是用了Python来完成的。我编写的A星算法的时间复杂度和空间复杂度肯定都很高，希望大家看了轻喷。

由于要用在ROS的map中寻找路径，所以我把A星算法写成了一个类，只需要传进去三个参数，一个是二维的List（ROS中规定：可走区域的数值为0，障碍物数值为100，未知领域数值为-1），一个是起始点的坐标，一个是终点的坐标。

最后程序会返回一个List，里面的内容是从终点到起点的最短路径，用每一个点的坐标表示。

比如下面是一张地图：

	0	1	2	3	4	5	6	7
0	0	0	0	0	0	0	0	0
1	0	0	0	100	0	0	0	0
2	0	0	0	100	0	0	0	0
3	0	0	0	100	0	0	0	0
4	0	0	0	100	0	0	0	0
5	0	0	0	0	0	0	0	0
6	0	0	0	0	0	0	0	0

规定起点为[2,2]，终点为[2,4]，则返回的内容为：[(2,4),(1,4),(0,3),(1,2),(2,2)]

具体实现

对于地图中每一个像素点，如果这个像素点可到达，那就计算当前的G代价（self.cost_g），H代价（self.cost_h），F代价（self.cost_f）

这个类，相当于C++中的struct

class map_node():
    def __init__(self):
    	# x、y代表该像素点在地图中的横纵坐标
        self.x = 0
        self.y = 0
        # F = G + H
        self.cost_f = 0
        self.cost_g = 0
        self.cost_h = 0
        # 父节点的横纵坐标
        self.parent = [0,0]

主要类：

class find_path():

类初始化函数：

• extend_map函数是将地图扩展一圈，加一圈障碍物（像素值为1）

• 声明一个state_map用于保存map中每一个像素点的状态：

  • 2代表已经在open表中
  • 3代表已经在close表中
  • 0代表还没有处理过

• 起点和终点自然横纵坐标都+=1

• 初始化一些变量 openlist、closelist

    def __init__(self, map, start, goal):
        # map是一个二维地图， start是起点坐标[]，goal是终点坐标[]
        self.map = self.extend_map(map)
        # 2代表在open表中 3代表在close表中
        self.state_map = np.zeros([len(map) + 2, len(map[0]) + 2])
        # print self.map

        self.start = start
        self.start[0] += 1
        self.start[1] += 1
        self.goal = goal
        self.goal[0] += 1
        self.goal[1] += 1
        self.open_list = []
        self.cloase_list = []
        self.path = []
        self.if_reach = False

扩展地图边界的函数

      def extend_map(self, map):
        new_row = np.ones(len(map[0]))
        new_col = np.ones(len(map) + 2)
        x = np.insert(map, 0, new_row, axis=0)
        x = np.insert(x, len(map) + 1, new_row, axis=0)
        x = np.insert(x, 0 , new_col, axis=1)
        x = np.insert(x, len(map[0]) + 1 , new_col, axis=1)
        return x

主要的寻找路径的函数

• 如果设置的起始点和终点不可达，print 提示，然后直接退出
• append_around_open函数是把该点周围的8个点加到open表中
• 把起始点加到close表中
• 进入循环，利用find_min_cost_f每次寻找open表中最小的cost_f，把它周围的8个点加到open表中，然后把这个节点加到close表，直到找到了到终点的路径，然后利用append_path函数回溯父节点直到回溯到起点。

    def start_find(self):
        #第一次操作，把起点的周围的点指向起点，起点和周围的点加到open list,
        # print "-----start point-----",self.start
        if self.map[self.start[0]][self.start[1]] != 0:
            print "\033[0;31m[E] : Please set the valid start point\033[0m"
            print "value = ", self.map[self.start[0]][self.start[1]]
            return "None"
        if self.map