作者在读学校Singapore University of Technology and Design

Established under strong support from MIT, Singapore University of Technology and Design (SUTD) plans to do for Singapore what MIT has done for Massachusetts and Silicon Valley, as well as for the world. SUTD is conducting top-tier research and education. In 2017, SUTD was ranked by Clarivate Analytics as the 5th most influential scientific research university in telecommunications in the world. SUTD was also ranked the 6th in electrical engineering research in the world.

代码分享

PythonRobotics是由Atsushi Sakai, Daniel Ingram等人建立的开源代码软件平台，收集了机器人学当下主流算法的python代码（基于python3），为了帮助初学者明白各个算法的基本原理，详细介绍见PythonRobotics: a Python code collection of robotics algorithms

觉得使用方便还请星标和关注笔者的git！

A* 搜索算法代码讲解

A-star是一种图遍历和路径搜索算法，由于其完备性、最优性和最优效率，它经常被用于计算机科学的许多领域。一个主要的实际缺点是它的空间复杂性，因为它将所有生成的节点存储在内存中。因此，在实际的旅行路线系统中，它通常优于可以对图形进行预处理以获得更好性能的算法，以及内存有界方法；然而，在很多情况下，A-star 仍然是最好的解决方案。关于A-star算法的讲解，详细可参考A*算法详解，以下笔者仅简要介绍几个概念：

• 搜索区域：图中的搜索区域被划分为二维数组，数组每个元素对应一个小方格，通常将一个单位的中心点称之为搜索区域节点（Node）
• Open List：路径规划过程中存放待检测的节点的集合
• Closed List: 存放已检测过的格子的集合

• Heuristics Function：启发函数，采用曼哈顿距离，即横纵向走的距离之和，可以看做是一种试探

  函数定义

  首先，我们定义一个AStarPlanner的类，初始化一个简单的地图：

  def __init__(self, ox, oy, reso, rr):
        self.reso = reso
        self.rr = rr
        self.calc_obstacle_map(ox, oy)
        self.motion = self.get_motion_model()

  其中ox,oy是障碍物的x和y坐标，reso是地图的像素，rr是机器人的半径，单位都是米。然后我们定义搜索区域节点（Node）的类

  class Node:
        def __init__(self, x, y, cost, pind):
            self.x = x  # index of grid
            self.y = y  # index of grid
            self.cost = cost
            self.pind = pind
  
        def __str__(self):
            return str(self.x) + "," + str(self.y) + "," + str(
                self.cost) + "," + str(self.pind)

  每个Node都包含坐标x和y，代价cost和pind序号。

  规划函数

  该函数定义在AStarPlanner类中，输入起始点和目标点的坐标(sx,sy)和(gx,gy)，最终输出的结果是路径包含的点的坐标集合rx和ry。首先定义起始点、目标点和open set, closed set

  nstart = self.Node(self.calc_xyindex(sx, self.minx),
                    self.calc_xyindex(sy, self.miny), 0.0, -1)
  ngoal = self.Node(self.calc_xyindex(gx, self.minx),
                    self.calc_xyindex(gy, self.miny), 0.0, -1)
  open_set, closed_set = dict(), dict()
  open_set[self.calc_grid_index(nstart)] = nstart

  然后在while循环中从起始点(sx,sy)开始判断最佳路径，直到找到目标点(gx,gy)后循环终止。while循环中的函数为

  c_id = min(
                open_set,
                key=lambda o: open_set[o].cost + self.calc_heuristic(ngoal,
                                                                     open_set[
                                                                         o]))
            current = open_set[c_id]
  
            # show graph
            if show_animation:  # pragma: no cover
                plt.plot(self.calc_grid_position(current.x, self.minx),
                         self.calc_grid_position(current.y, self.miny), "xc")
                # for stopping simulation with the esc key.
                plt.gcf().canvas.mpl_connect('key_release_event',
                                             lambda event: [exit(
                                                 0) if event.key == 'escape' else None])
                if len(closed_set.keys()) % 10 == 0:
                    plt.pause(0.001)

  通过追踪当前位置current.x和current.y来动态展示路径寻找，终止条件为current与ngoal重合

  if current.x == ngoal.x and current.y == ngoal.y:
                print("Find goal")
                ngoal.pind = current.pind
                ngoal.cost = current.cost
                break
  
            # Remove the item from the open set
            del open_set[c_id]
  
            # Add it to the closed set
            closed_set[c_id] = current

  同时更新closed set，最后一部分是基于运动模型来搜索每个node附近的网格，将cost最小的网格记录在open set中

  for i, _ in enumerate(self.motion):
                node = self.Node(current.x + self.motion[i][0],
                                 current.y + self.motion[i][1],
                                 current.cost + self.motion[i][2], c_id)
                n_id = self.calc_grid_index(node)
  
                # If the node is not safe, do nothing
                if not self.verify_node(node):
                    continue
  
                if n_id in closed_set:
                    continue
  
                if n_id not in open_set:
                    open_set[n_id] = node  # discovered a new node
                else:
                    if open_set[n_id].cost > node.cost:
                        # This path is the best until now. record it
                        open_set[n_id] = node

  最后，通过将目标点和closed set传入calc_final_path函数来产生最后的路径并结束while循环

  rx, ry = self.calc_final_path(ngoal, closed_set)
  def calc_final_path(self, ngoal, closedset):
        # generate final course
        rx, ry = [self.calc_grid_position(ngoal.x, self.minx)], [
            self.calc_grid_position(ngoal.y, self.miny)]
        pind = ngoal.pind
        while pind != -1:
            n = closedset[pind]
            rx.append(self.calc_grid_position(n.x, self.minx))
            ry.append(self.calc_grid_position(n.y, self.miny))
            pind = n.pind
  return rx, ry

  其他函数

  在计算的过程中，需要计算曼哈顿距离、坐标信息以及验证Node的合理性（即在地图范围内）、计算障碍层地图等函数，这些函数都在规划函数后，也比较清晰，笔者在此处略过。

  main函数

  最后的main函数是定义起点和目标点、设置障碍物的位置、调用类以及里面的函数进行规划运算，并且动态展示出来运算结果

  def main():
    print(__file__ + " start!!")
  
    # start and goal position
    sx = 10.0  # [m]
    sy = 10.0  # [m]
    gx = 50.0  # [m]
    gy = 50.0  # [m]
    grid_size = 2.0  # [m]
    robot_radius = 1.0  # [m]
  
    # set obstacle positions
    ox, oy = [], []
    for i in range(-10, 60):
        ox.append(i)
        oy.append(-10.0)
    for i in range(-10, 60):
        ox.append(60.0)
        oy.append(i)
    for i in range(-10, 61):
        ox.append(i)
        oy.append(60.0)
    for i in range(-10, 61):
        ox.append(-10.0)
        oy.append(i)
    for i in range(-10, 40):
        ox.append(20.0)
        oy.append(i)
    for i in range(0, 40):
        ox.append(40.0)
        oy.append(60.0 - i)
  
    if show_animation:  # pragma: no cover
        plt.plot(ox, oy, ".k")
        plt.plot(sx, sy, "og")
        plt.plot(gx, gy, "xb")
        plt.grid(True)
        plt.axis("equal")
  
    a_star = AStarPlanner(ox, oy, grid_size, robot_radius)
    rx, ry = a_star.planning(sx, sy, gx, gy)
  
    if show_animation:  # pragma: no cover
        plt.plot(rx, ry, "-r")
        plt.show()
        plt.pause(0.001)

运行结果

代码的动态运行结果如下图所示



其中绿色的网格是经过验证和搜索的点，最终的红色路线即位最佳路径。大家可以下载PythonRobotics包并运行文件PathPlanning/AStar下的a_star.py来验证。

后记

本文是关于Python Robotics代码中的A*路径规划的详细介绍，大家可以在此基础上扩展延伸。觉得有帮助一定要转发点赞关注哦，谢谢！

笔者本科毕业于上海交通大学，现在是SUTD PhD Candidate，有两年多ROS的使用经验及多机器人编队的科研经验（2018.03-2020.11），现在是总结之前所学，希望能有所帮助。本系列Python Robotics代码详解将结合该著名的机器人学代码包讲解机器人SLAM定位建图及路径规划与导航等相关算法及原理，持续不断更新中。如果大家有相关问题或发现作者漏洞欢迎私戳，同时欢迎关注收藏。
同时欢迎关注博主Git和CSDN：
https://github.com/LiHongbo97
https://blog.csdn.net/qq_33742147?spm=1011.2124.3001.5343