SUTD
作者在读学校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
本系列已发布文章：
Python Robotics代码详解（一）A* 搜索算法
Python Robotics代码详解（二）Dijkstra 搜索算法
Python Robotics代码详解（三）DFS深度优先与BFS广度优先搜索算法
并且将会持续发布更全面、更深层次的讲解，觉得使用方便还请星标和关注笔者的git！

人工势场法路径规划(PFP)

人工势场法（Potential Field Planning）是常见的局部路径规划算法，详细的原理讲解请参考此篇多机器人编队人工势场法协同避障算法原理及实现。简单地说，PFP是在像素地图已知的前提下，结合引力（目标点施加）与斥力（范围内障碍物施加）计算该地图所有点的势场大小，机器人比较当前位置附近八个点位的势场，选择最优的点作为下一时刻的运动点，直到运动到目标点为止。

PFP算法代码讲解

变量定义

与前文相似，PFP算法所需要的变量包括

# start and goal position
    sx = 0.0  # [m] 起始位置x坐标
    sy = 10.0  # [m] 起始位置y坐标
    gx = 30.0  # [m] 目标点x坐标
    gy = 30.0  # [m] 目标点y坐标
    grid_size = 0.5  # [m] 网格的尺寸
    robot_radius = 5.0  # [m] 机器人尺寸

以及障碍物的位置

# start and goal position
    ox = [15.0, 5.0, 20.0, 25.0]  # 障碍物的x坐标[米]
    oy = [25.0, 15.0, 26.0, 25.0]  # 障碍物的y坐标 [米]

除此之外，PFP需要提前遍历计算整个地图区域下的各个点的势场，因此需要有一个地图的范围，以及引力、斥力的计算系数

    KP = 5.0  # 引力势场增益
    ETA = 100.0  # 斥力势场增益
    AREA_WIDTH = 30.0  # 地图范围 [米]

我们将地图（假设是方形）均匀地网格化，每一个网格代表一个点，同时每个点有一个序列号index变量来记录点的位置，我们需要定义地图内的边界minx, miny和maxx, maxy，以及像素地图两个方向上所有的像素点数xw和yw，用来计算遍历所有点的势场以做出选择，其计算方式如下：

    minx = min(ox) - AREA_WIDTH / 2.0
    miny = min(oy) - AREA_WIDTH / 2.0
    maxx = max(ox) + AREA_WIDTH / 2.0
    maxy = max(oy) + AREA_WIDTH / 2.0
    xw = int(round((maxx - minx) / reso)) #计算x方向的像素点数量
    yw = int(round((maxy - miny) / reso))

除此之外，每次我们需要一个向量pmap来储存计算得到的所有像素点的势场大小

    pmap = [[0.0 for i in range(yw)] for i in range(xw)]

在遍历结束后，机器人会根据pmap判断当前位置的周围八个节点的势场大小，选择势场最小的一个点作为下一时刻的目标点。这个过程需要用到机器人的运动模型motion: 在网状方格地图中，每个节点（边界点除外）周围有八个节点，对应八个方向，motion是固定存储前进方向的数组

def get_motion_model():
    # dx, dy
    motion = [[1, 0],
              [0, 1],
              [-1, 0],
              [0, -1],
              [-1, -1],
              [-1, 1],
              [1, -1],
              [1, 1]]

    return motion

函数定义

首先我们介绍引力与斥力的计算函数

def calc_attractive_potential(x, y, gx, gy):
    return 0.5 * KP * np.hypot(x - gx, y - gy)

def calc_repulsive_potential(x, y, ox, oy, rr):
    # search nearest obstacle
    minid = -1
    dmin = float("inf")
    for i, _ in enumerate(ox): #遍历障碍物的点并判断离x,y最近的障碍物
        d = np.hypot(x - ox[i], y - oy[i])
        if dmin >= d:
            dmin = d
            minid = i

    # 计算这个障碍物与x,y的距离
    dq = np.hypot(x - ox[minid], y - oy[minid])

    if dq <= rr: #判断是否有碰撞风险
        if dq <= 0.1:
            dq = 0.1

        return 0.5 * ETA * (1.0 / dq - 1.0 / rr) ** 2
    else: #没有碰撞风险则斥力为0
        return 0.0

引力函数非常简单，其中np.hypot()函数是计算两个点之间的距离，当距离较大时引力也较大，斥力函数相对复杂，机器人首先从障碍物点集合ox和oy中选择离该位置(x,y)最近的障碍物来计算距离，并判断这个距离dq与机器人半径rr的大小，只有当有碰撞风险时（dp<rr）才会产生一个非常大的斥力，否则这个点的斥力为0。由于当机器人与障碍物越近斥力越大，离目标点(gx,gy)越远引力越大，这样将两个力以标量相加得到的势场大小越小（距离近同时距离障碍物远），机器人以该点为下一个运动点能够，这样能以最快且安全的路径运动到目标点。

在有了这两个函数的情况下，机器人可以遍历并计算地图上所有点的势场大小并返回得到的结果pmap：

def calc_potential_field(gx, gy, ox, oy, reso, rr):
    for ix in range(xw):
        x = ix * reso + minx

        for iy in range(yw):
            y = iy * reso + miny
            ug = calc_attractive_potential(x, y, gx, gy)
            uo = calc_repulsive_potential(x, y, ox, oy, rr)
            uf = ug + uo
            pmap[ix][iy] = uf

    return pmap

以上函数计算是可以提前完成的，当得到pmap后，机器人可以实时比较当前位置周围八个点的势场大小以动态规划最优路径，

def potential_field_planning(sx, sy, gx, gy, ox, oy, reso, rr):

    # 遍历地图上所有点并计算势场
    pmap = calc_potential_field(gx, gy, ox, oy, reso, rr)

    # 计算目标点与起始点的距离，并获取两个点在x和y方向的index
    d = np.hypot(sx - gx, sy - gy)
    ix = round((sx - minx) / reso)
    iy = round((sy - miny) / reso)
    gix = round((gx - minx) / reso)
    giy = round((gy - miny) / reso)
    # 动态展示heatmap
    if show_animation:
        draw_heatmap(pmap)
        # 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])
        plt.plot(ix, iy, "*k")
        plt.plot(gix, giy, "*m")

    rx, ry = [sx], [sy]
    motion = get_motion_model()
    while d >= reso: #只要当前位置不是目标点，就一直循环运行
        minp = float("inf")
        minix, miniy = -1, -1
        for i, _ in enumerate(motion):#比较当前位置周围八个点的势场大小
            inx = int(ix + motion[i][0])
            iny = int(iy + motion[i][1])
            if inx >= len(pmap) or iny >= len(pmap[0]):
                p = float("inf")  # outside area
            else:
                p = pmap[inx][iny]
            if minp > p: #选取势场最小的点
                minp = p
                minix = inx
                miniy = iny
        ix = minix
        iy = miniy
        xp = ix * reso + minx # 计算该最优点的x和y坐标
        yp = iy * reso + miny
        d = np.hypot(gx - xp, gy - yp)
        rx.append(xp) #将该点作为下一个运动点加入到规划路径rx和ry中
        ry.append(yp)

        if show_animation:
            plt.plot(ix, iy, ".r")
            plt.pause(0.01)

    print("Goal!!")

    return rx, ry

笔者已经将函数进行注释讲解，机器人会保持循环规划，直到到达目标点为止

PFP动态运行结果



大家可以下载PythonRobotics包并运行文件PathPlanning/PotentialFieldPlanning下的potential_field_planning.py来验证。

后记

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

笔者本科毕业于上海交通大学，现在是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