Loading [MathJax]/jax/output/CommonHTML/config.js
前往小程序,Get更优阅读体验!
立即前往
首页
学习
活动
专区
圈层
工具
发布
首页
学习
活动
专区
圈层
工具
MCP广场
社区首页 >专栏 >移动机器人路径规划:人工势场法[通俗易懂]

移动机器人路径规划:人工势场法[通俗易懂]

作者头像
全栈程序员站长
发布于 2022-07-02 08:54:14
发布于 2022-07-02 08:54:14
1.7K02
代码可运行
举报
运行总次数:2
代码可运行

大家好,又见面了,我是你们的朋友全栈君。

人工势场法是一种原理比较简单的移动机器人路径规划算法,它将目标点位置视做势能最低点,将地图中的障碍物视为势能高点,计算整个已知地图的势场图,然后理想情况下,机器人就像一个滚落的小球,自动避开各个障碍物滚向目标点。

具体地,目标点的势能公式为:

其中写道,为防止距离目标点较远时的速度过快,可以采用分段函数的形式描述,后文会进行展示。 而障碍物的势能表示为:

即在障碍物周围某个范围内具有高势能,范围外视障碍物的影响为0。 最终将目标点和障碍物的势能相加,获得整张势能地图:

以下是参考链接中的源代码,比较容易懂:

代码语言:javascript
代码运行次数:0
运行
AI代码解释
复制
""" Potential Field based path planner author: Atsushi Sakai (@Atsushi_twi) Ref: https://www.cs.cmu.edu/~motionplanning/lecture/Chap4-Potential-Field_howie.pdf """

from collections import deque
import numpy as np
import matplotlib.pyplot as plt

# Parameters
KP = 5.0  # attractive potential gain
ETA = 100.0  # repulsive potential gain
AREA_WIDTH = 30.0  # potential area width [m]
# the number of previous positions used to check oscillations
OSCILLATIONS_DETECTION_LENGTH = 3

show_animation = True


def calc_potential_field(gx, gy, ox, oy, reso, rr, sx, sy):
    """ 计算势场图 gx,gy: 目标坐标 ox,oy: 障碍物坐标列表 reso: 势场图分辨率 rr: 机器人半径 sx,sy: 起点坐标 """
    # 确定势场图坐标范围:
    minx = min(min(ox), sx, gx) - AREA_WIDTH / 2.0
    miny = min(min(oy), sy, gy) - AREA_WIDTH / 2.0
    maxx = max(max(ox), sx, gx) + AREA_WIDTH / 2.0
    maxy = max(max(oy), sy, gy) + AREA_WIDTH / 2.0
    # 根据范围和分辨率确定格数:
    xw = int(round((maxx - minx) / reso))
    yw = int(round((maxy - miny) / reso))

    # calc each potential
    pmap = [[0.0 for i in range(yw)] for i in range(xw)]

    for ix in range(xw):
        x = ix * reso + minx   # 根据索引和分辨率确定x坐标

        for iy in range(yw):
            y = iy * reso + miny  # 根据索引和分辨率确定x坐标
            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, minx, miny


def calc_attractive_potential(x, y, gx, gy):
    """ 计算引力势能:1/2*KP*d """
    return 0.5 * KP * np.hypot(x - gx, y - gy)


def calc_repulsive_potential(x, y, ox, oy, rr):
    """ 计算斥力势能: 如果与最近障碍物的距离dq在机器人膨胀半径rr之内:1/2*ETA*(1/dq-1/rr)**2 否则:0.0 """
    # search nearest obstacle
    minid = -1
    dmin = float("inf")
    for i, _ in enumerate(ox):
        d = np.hypot(x - ox[i], y - oy[i])
        if dmin >= d:
            dmin = d
            minid = i

    # calc repulsive potential
    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:
        return 0.0


def get_motion_model():
    # dx, dy
    # 九宫格中 8个可能的运动方向
    motion = [[1, 0],
              [0, 1],
              [-1, 0],
              [0, -1],
              [-1, -1],
              [-1, 1],
              [1, -1],
              [1, 1]]

    return motion


def oscillations_detection(previous_ids, ix, iy):
    """ 振荡检测:避免“反复横跳” """
    previous_ids.append((ix, iy))

    if (len(previous_ids) > OSCILLATIONS_DETECTION_LENGTH):
        previous_ids.popleft()

    # check if contains any duplicates by copying into a set
    previous_ids_set = set()
    for index in previous_ids:
        if index in previous_ids_set:
            return True
        else:
            previous_ids_set.add(index)
    return False


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

    # calc potential field
    pmap, minx, miny = calc_potential_field(gx, gy, ox, oy, reso, rr, sx, sy)

    # search path
    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)

    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()
    previous_ids = deque()

    while d >= reso:
        minp = float("inf")
        minix, miniy = -1, -1
        # 寻找8个运动方向中势场最小的方向
        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]) or inx < 0 or iny < 0:
                p = float("inf")  # outside area
                print("outside potential!")
            else:
                p = pmap[inx][iny]
            if minp > p:
                minp = p
                minix = inx
                miniy = iny
        ix = minix
        iy = miniy
        xp = ix * reso + minx
        yp = iy * reso + miny
        d = np.hypot(gx - xp, gy - yp)
        rx.append(xp)
        ry.append(yp)
        # 振荡检测,以避免陷入局部最小值:
        if (oscillations_detection(previous_ids, ix, iy)):
            print("Oscillation detected at ({},{})!".format(ix, iy))
            break

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

    print("Goal!!")

    return rx, ry


def draw_heatmap(data):
    data = np.array(data).T
    plt.pcolor(data, vmax=100.0, cmap=plt.cm.Blues)


def main():
    print("potential_field_planning start")

    sx = 0.0  # start x position [m]
    sy = 10.0  # start y positon [m]
    gx = 30.0  # goal x position [m]
    gy = 30.0  # goal y position [m]
    grid_size = 0.5  # potential grid size [m]
    robot_radius = 5.0  # robot radius [m]
    # 以下障碍物坐标是我进行修改后的,用来展示人工势场法的困于局部最优的情况:
    ox = [15.0, 5.0, 20.0, 25.0, 12.0, 15.0, 19.0, 28.0, 27.0, 23.0, 30.0, 32.0]  # obstacle x position list [m]
    oy = [25.0, 15.0, 26.0, 25.0, 12.0, 20.0, 29.0, 28.0, 26.0, 25.0, 28.0, 27.0]  # obstacle y position list [m]

    if show_animation:
        plt.grid(True)
        plt.axis("equal")

    # path generation
    _, _ = potential_field_planning(
        sx, sy, gx, gy, ox, oy, grid_size, robot_radius)

    if show_animation:
        plt.show()


if __name__ == '__main__':
    print(__file__ + " start!!")
    main()
    print(__file__ + " Done!!")

人工势场法的一项主要缺点就是可能会落入局部最优解,下图是源代码运行后的结果:

下面是在我添加了一些障碍物后,人工势场法困于局部最优解的情况:虽然还没有到达目标点,但势场决定了路径无法再前进。

需要注意的是,源代码在计算目标点势场的时候,使用的是某x,y位置距离目标点的距离的一次项,并未如课件中所示使用二次项,也是为了使势场变化没有那么快。下面是按照课件中所说,使用距离的二次项运行的结果,我们可以看到,为运行正常,KP需要调得很低:

代码语言:javascript
代码运行次数:0
运行
AI代码解释
复制
KP = 0.1
def calc_attractive_potential(x, y, gx, gy):
    """ 计算引力势能:1/2*KP*d^2 """
    return 0.5 * KP * np.hypot(x - gx, y - gy)**2

正常运行:

困在局部最优点:

可以从势场图中看到,引力变化较上一个例子快得多。

最后,我们将程序修改成上面课件截图中所示的分段函数:

代码语言:javascript
代码运行次数:0
运行
AI代码解释
复制
KP = 0.25
dgoal = 10
def calc_attractive_potential(x, y, gx, gy):
    """ 计算引力:如课件截图 """
    dg = np.hypot(x - gx, y - gy)
    if dg<=dgoal:
        U = 0.5 * KP * np.hypot(x - gx, y - gy)**2
    else:
        U = dgoal*KP*np.hypot(x - gx, y - gy) - 0.5*KP*dgoal
    return U

正常运行:

困于局部最优:

可以看到引力势场分段的效果。

发布者:全栈程序员栈长,转载请注明出处:https://javaforall.cn/148611.html原文链接:https://javaforall.cn

本文参与 腾讯云自媒体同步曝光计划,分享自作者个人站点/博客。
如有侵权请联系 cloudcommunity@tencent.com 删除

本文分享自 作者个人站点/博客 前往查看

如有侵权,请联系 cloudcommunity@tencent.com 删除。

本文参与 腾讯云自媒体同步曝光计划  ,欢迎热爱写作的你一起参与!

评论
登录后参与评论
暂无评论
推荐阅读
编辑精选文章
换一批
ROS1云课→20迷宫不惑之A*大法(一种虽古老但实用全局路径规划算法)
www.gamedev.net/reference/articles/article2003.asp 
zhangrelay
2022/09/28
2500
ROS1云课→20迷宫不惑之A*大法(一种虽古老但实用全局路径规划算法)
基于matlab的机械臂仿真_移动机器人matlab运动学仿真
目的 本文手把手教你在 Mathematica 科学计算软件中搭建机器人的仿真环境,具体包括以下内容:    1 导入机械臂的三维模型    2 正\逆运动学仿真    3 碰撞检测    4 轨迹规划    5 正\逆动力学仿真    6 运动控制 文中的所有代码和模型文件都在此处:https://github.com/robinvista/Mathematica 。使用的软件版本是 Mathematica 11.1,较早的版本可能缺少某些函数,所以最好使用最新版。交流网站是www.robotattractor.com。进入正文之前不妨先看几个例子:
全栈程序员站长
2022/11/01
5.2K0
基于matlab的机械臂仿真_移动机器人matlab运动学仿真
移动端页面适配方案(viewport)[通俗易懂]
优点是可以使用px布局,不用额外进行rem或者vw等等单位的换算了 缺点是如果是无滚动条的页面在某些设备上(例如平板这种宽高3比4的,折叠屏8比7的)由于宽高比不同有些区域会被挤到视口之外从而导致一些体验上的问题,不过demo2也给出了解决方案;
全栈程序员站长
2022/09/13
6550
【数据分析报告】携程客户分析与流失预测
携程作为中国领先的综合性旅行服务公司,每天向超过2.5亿会员提供全方位的旅行服务,因此每天都会产生海量的用户行为数据,这些数据蕴含着丰富的信息资源。另外,客户是企业的重要资源,也是企业的无形资产,客户的流失,也就意味着资产的流失,因此客户流失率是考量业务成绩的一个非常关键的指标。
全栈程序员站长
2022/11/02
7.4K0
【数据分析报告】携程客户分析与流失预测
base64编码图片 生成图片,返回地址[通俗易懂]
发布者:全栈程序员栈长,转载请注明出处:https://javaforall.cn/158546.html原文链接:https://javaforall.cn
全栈程序员站长
2022/09/14
2K0
图片和Base64编码相互转换[通俗易懂]
发布者:全栈程序员栈长,转载请注明出处:https://javaforall.cn/135035.html原文链接:https://javaforall.cn
全栈程序员站长
2022/09/07
1.1K0
FIST! FIST! FIST! Its all in the wrist: Remote Exec[通俗易懂]
发布者:全栈程序员栈长,转载请注明出处:https://javaforall.cn/167020.html原文链接:https://javaforall.cn
全栈程序员站长
2022/09/20
1.5K0
获取base64编码格式的图片大小[通俗易懂]
发布者:全栈程序员栈长,转载请注明出处:https://javaforall.cn/151123.html原文链接:https://javaforall.cn
全栈程序员站长
2022/09/06
2.2K0
在HbuilderX中实现微信小程序下蓝牙连接打印机完整实战案例
商家打印小票,小票包含顾客消费的商品明细信息以及末尾附上二维码,二维码供顾客扫码开票。 
跟着飞哥学编程
2022/11/30
2.7K0
在HbuilderX中实现微信小程序下蓝牙连接打印机完整实战案例
vue pc端打印二维码[通俗易懂]
以上新建一个index.js文件夹,在main.js里引入。在任何一个vue文件里调用 this.$dayin(img)
全栈程序员站长
2022/09/06
3960
文末彩蛋 | 这个 Request URL 长得好不一样
很明显这不是加密的数据,这是一张图片 base64 后的结果,第一次写爬虫朋友遇到这样的请求,可能需要琢磨一下这是什么东西。
全栈程序员站长
2022/11/04
3400
文末彩蛋 | 这个 Request URL 长得好不一样
echarts+vue_vue安装echarts
版权声明:本文内容由互联网用户自发贡献,该文观点仅代表作者本人。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如发现本站有涉嫌侵权/违法违规的内容, 请发送邮件至 举报,一经查实,本站将立刻删除。
全栈程序员站长
2022/11/08
4.1K0
使用javascript实现对于chineseocr的API调用「建议收藏」
发布者:全栈程序员栈长,转载请注明出处:https://javaforall.cn/125721.html原文链接:https://javaforall.cn
全栈程序员站长
2022/07/22
8890
使用javascript实现对于chineseocr的API调用「建议收藏」
Improvise_a_Jazz_Solo_with_an_LSTM_Network_v3a-2
Welcome to your final programming assignment of this week! In this notebook, you will implement a model that uses an LSTM to generate music. You will even be able to listen to your own music at the end of the assignment.
列夫托尔斯昊
2020/08/25
1.6K0
Improvise_a_Jazz_Solo_with_an_LSTM_Network_v3a-2
web添加图片的代码_html保存图片到本地
转载于:https://www.cnblogs.com/larryzeal/p/5991182.html
全栈程序员站长
2022/11/02
3.1K0
web添加图片的代码_html保存图片到本地
数据库的增删改查加遍历
发布者:全栈程序员栈长,转载请注明出处:https://javaforall.cn/160891.html原文链接:https://javaforall.cn
全栈程序员站长
2022/09/13
8750
Data URI scheme「建议收藏」
data URI scheme 允许我们使用内联(inline-code)的方式在网页中包含数据,目的是将一些小的数据,直接嵌入到网页中,从而不用再从外部文件载入。常用于将图片嵌入网页。
全栈程序员站长
2022/11/02
5960
3dreshaper_3d曲面屏幕是什么意思
版权声明:本文内容由互联网用户自发贡献,该文观点仅代表作者本人。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如发现本站有涉嫌侵权/违法违规的内容, 请发送邮件至 举报,一经查实,本站将立刻删除。
全栈程序员站长
2022/11/04
4800
3dreshaper_3d曲面屏幕是什么意思
用java实现图片切换_电视背景集成墙面
下载地址:Download OpenCV from SourceForge.net
全栈程序员站长
2022/09/30
5970
用java实现图片切换_电视背景集成墙面
应急响应记录之水坑挂马事件分析
在攻防演练中红队时而会在获取到目标系统的webshell权限之后会通过篡改前端网页的JS或HTML文件内容并插入恶意代码来挂载水坑扩大战果,具体的效果主要是使其在用户首次访问网站时就出现弹窗诱导用户下载恶意文件并进行安装,也有不少更加彻底直接通过篡改文件来挂载Flash水坑文件来诱导用户下载恶意文件,本篇文章主要是通过介绍近期在应急响应阶段中的几个水坑挂载木马的安全事件从侧面介绍关于水坑挂载的排查思路以及方式,同时对水坑挂载的恶意文件内容进行简要分析
Al1ex
2024/12/23
1230
应急响应记录之水坑挂马事件分析
推荐阅读
相关推荐
ROS1云课→20迷宫不惑之A*大法(一种虽古老但实用全局路径规划算法)
更多 >
领券
问题归档专栏文章快讯文章归档关键词归档开发者手册归档开发者手册 Section 归档
本文部分代码块支持一键运行,欢迎体验
本文部分代码块支持一键运行,欢迎体验