运动规划入门 | 5. 白话人工势场法,从原理到Matlab实现

2133
94
2020年8月13日 10时47分

上一篇,我们讲完了RRT,这回我们来讲运动规划入门系列的最后一个算法——人工势场法。回想当时我第一次听说人工势场法的时候,不明觉厉,感觉这似乎是一个十分高大上的规划算法,但是当我真正了解以后才发现,其实人工势场法的思路反而是这个系列五个算法中最好理解,最符合直觉的。在这篇博文中,我将为小伙伴们揭开人工势场法神秘的面纱。

1. 人工势场的原理详解

物理学的势(potential),也称做“位”,是一种能量概念。在保守场里,把一个单位质点(如重力场中的单位质量,静电场中的单位正电荷)从场中的某一点A移到参考点,场力所作的功是一个定值。也就是说,在保守场中,单位质点在A点与参考点的势能之差是一定的,人们把这个势能差定义为保守场中A点的“势”。

高中知识会学到电场和磁场,一个处于电场中的电荷会受到电场力的作用,从而做功。而人工势场的思路于其十分的相似,我们人工地在障碍物地图的基础上添加一个特殊的场,这个场会引领机器人从起点运动到终点。

而人工势场法的重点就落在了以下两个问题上:

  1. 如何构建这样的一个人工势场?
  2. 如何利用人工势场进行运动规划?

    1.1 引力势场(Attractive Potential Field)

    人工势场这个特殊的势场并不是一个单一的场,其实它是由两个场叠加组合而成的,一个是引力场,一个是斥力场。

    顾名思义引力势场是具有吸引的性质,会将机器人从起点处朝着终点处吸引,所以引力场的存在使得机器人获得了运动的大方向。

    在实际的工程中,其实有很多种方法可以构建这样的一个引力势场,最简单而且也是最常用的方式就是直接对地图自由空间的每个点都相对终点计算出欧氏距离的平方,并乘上一个缩放系数ε

    运动规划入门 | 5. 白话人工势场法,从原理到Matlab实现插图

    fa(x, y)便是所谓的引力势场函数,它会构建一个离终点越远引力越大的特殊势场,在Matlab中可以非常直观地看到这个引力势场。

    运动规划入门 | 5. 白话人工势场法,从原理到Matlab实现插图(1)

    1.2 斥力势场(Repulsive Potential Field)

    当然仅仅只有引力势场是不够的,我们还需要让机器人懂得避开地图中的障碍物,这时斥力势场便有用武之地了,斥力势场的会构建一个距离障碍物越近,斥力越大的特殊势场。

    这个过程其实非常好理解,引力势场负责吸引机器人从起点朝着终点运动,斥力势场负责规避地图中的障碍。

    通常我们会用下面这个函数来构建斥力势场:

    运动规划入门 | 5. 白话人工势场法,从原理到Matlab实现插图(2)

    其中ρ(x, y)是一个特殊的函数,它会计算出离当前点(x,y)最近的障碍物的距离,而d0是一个距离阈值,当当前点到最近障碍物的距离大于d0时不产生斥力。η同样是一个简单的缩放系数。

    在Matlab中可以直观地看到这个斥力势场的样子,离障碍物越近的点所具备的斥力值越大,离障碍物越远斥力值越小,甚至为0。

    运动规划入门 | 5. 白话人工势场法,从原理到Matlab实现插图(3)

    我们生成了引力势场和斥力势场这两个特殊势场之后,只要将两个势场进行简单的叠加组合之后就得到了最终的人工势场。

    在Matlab中可以直观地看到这个叠加的结果如下,其实相信看到这里小伙伴们应该有点感觉了,当我们每个人工势场都是根据特定的终点而特定生成的,在地图不变的情况下,改变终点的位置便会改变人工势场。终点的位置总是人工势场中势能最低的地方,想象一下,现实中存在一个长得和人工势场一样的曲面,现在我们拿着一个小球放在曲面的高处,那么在重力的作用下,小球自然而然地就会顺着曲面滚到最低点。如果你成功地想象出这个画面,那么恭喜你,你已经领悟了人工势场的核心思想了。

    运动规划入门 | 5. 白话人工势场法,从原理到Matlab实现插图(4)

    1.3 梯度下降

    相信只要是学过高数的小伙伴一定知道梯度的概念,如果你忘记了,那么赶紧回去复习。梯度下降法在寻找最优点的问题上被十分广泛地运用,但其实这也是一个听名字很高大上,说穿了很直观的方法。

    我还是和各路前辈一样使用山坡为例,来说说明梯度下降法的核心思想。假设你一觉醒来被空投到一个未知的山坡上,现在要求你尽全力下山,也就是说要你找到最低的点。那么这时候你该怎么办呢?非常简单,第一步:环顾你的四周找到一个坡度最陡峭的地方;第二步:朝着最陡峭的方向迈出一步;就这样重复执行第一步、第二步,直到你到达一个位置,在这个位置的四周都比它高,也就是说这是一个你能找到的最低的点。

    在数学中,梯度的方向就是函数位于当前某一点(x0,y0时,增长速度最快的方向。那么反过来,将函数添加上负号,再求梯度,便可以找到函数减少最快的方向。

    所以这便回答了方才提出的“如何利用人工势场进行运动规划”这一问题,我们采用梯度下降法配合人工势场进行运动规划。

    2. 人工势场法的Matlab实现

    2.1 引力势场函数

    首先我们需要生成一个和地图同样大小的meshgrid

    运动规划入门 | 5. 白话人工势场法,从原理到Matlab实现插图(5)

    然后引力势场函数的实现相对比较简单,就是计算meshgrid中每个点相对于终点的欧氏距离的平方,乘以一个缩放系数。对,就是这么简单。

    运动规划入门 | 5. 白话人工势场法,从原理到Matlab实现插图(6)

    2.2 斥力势场函数

    斥力势场函数在实现中比较麻烦的点在于ρ(x,y)函数,但是各位不要担心,万能的Matlab已经有现成的函数可以实现这个功能,这个函数就是bwdist()

    运动规划入门 | 5. 白话人工势场法,从原理到Matlab实现插图(7)

    bwdist()的描述中我们可以得知,这个函数其实是一个图像处理函数,他会计算出一张二值图片中所有像素点相对其最近的非零像素点的距离。这就正合我们意了,我们的障碍物地图恰好就是一张二值图片,0表示自由空间,1表示障碍物。那么这个函数就会计算出地图中每个点相对最近障碍物的距离值,当然如果当前点恰好是个障碍物,那么最近的障碍物就是它自己,所以得到的距离结果就是0。

    如下图的代码所示,influence就是表达式中的d0即距离阈值,这里将bwdist()的结果除以100再加1,首先是除以100是为了将距离值缩放到一个合理的范围,其次加1是为了规避前面说过障碍物结果为0的情况。

    运动规划入门 | 5. 白话人工势场法,从原理到Matlab实现插图(8)

    最终将两个生成的势场简单地相加就可以得到最终的人工势场了。

    运动规划入门 | 5. 白话人工势场法,从原理到Matlab实现插图(9)

    2.3 梯度规划器

    Matlab将梯度显示出来以后我们可以看到如下图所示,图中各个小箭头就是代表当位置的梯度大小和方向,其中远离障碍物的位置的梯度明显指向黄色的终点,越靠近终点处的梯度值越小;靠近障碍物的地方的梯度明显呈现排斥的趋势,而且越靠近障碍物排斥力越大。

    运动规划入门 | 5. 白话人工势场法,从原理到Matlab实现插图(10)

    梯度规划器核心代码如下所示,可以看到在每轮迭代中都会沿着当前梯度的方向前进单位距离,直到到达终点,即梯度下降收敛。

    运动规划入门 | 5. 白话人工势场法,从原理到Matlab实现插图(11)

    2.4 最终运行效果

    最终的运行结果如下,可以看到最终规划出的是一条沿着梯度方向走的光滑路径

    运动规划入门 | 5. 白话人工势场法,从原理到Matlab实现插图(12)

    为了更加直观地看到效果,我另外用一个小球代表机器人制作了下面这个动画,这回是不是就特别好懂了。这就是为什么我在开头的时候说人工势场是这个系列五个算法中最好理解,最符合直觉的。

    运动规划入门 | 5. 白话人工势场法,从原理到Matlab实现插图(13)

    3. 人工势场的不足

    人工势场法其实也不是完美无缺的规划算法,它也是有缺点的,比如在某些情况下会陷入局部最优解,而导致规划失败,比如当我们设置起点和终点如下。

    运动规划入门 | 5. 白话人工势场法,从原理到Matlab实现插图(14)

    那么这种情况是会陷入局部最优解的,具体情况如下图所示,我们可以看到路径在梯度下降的过程中卡在了死胡同里,最终导致规划失败。

    运动规划入门 | 5. 白话人工势场法,从原理到Matlab实现插图(15)

    运动规划入门 | 5. 白话人工势场法,从原理到Matlab实现插图(16)

    再比如,我们将起点和终点设置如下,让我们来看看会怎么样?

    运动规划入门 | 5. 白话人工势场法,从原理到Matlab实现插图(17)

    从下图的结果可以看出,这会导致另一种情况:终点的位置距离障碍物太近,规划路径难以接近终点的位置,只能停在终点附近,最终规划失败。

    运动规划入门 | 5. 白话人工势场法,从原理到Matlab实现插图(18)

尾声和我的暂别:

从4月份到现在,当初答应你们的5个运动规划入门算法已经悉数奉上,那么本系列就可以暂告一段落了。希望笔者这粗糙的讲解可以帮到你们些许。

由于笔者精力实在是有限,只能每月一更,这个更新速度确实是挺慢的,所以非常感谢各位这几个月以来的包容和支持。同样也是笔者个人的精力原因,我需要暂时停更一段时间,2020年恐怕是不会再更新了,当然你们的评论我还是会尽力答复的。

那么各位,来年春天再见!

发表评论

后才能评论

评论列表(94条)

  • 这个天才比较蠢 2020年11月25日 下午4:33

    老师您好,您的讲解非常生动形象,希望能得到五种运动规划的源码,邮箱:892986354@qq.com 非常感谢!

  • q4m7r_5959 2020年11月15日 下午10:01

    老师,求您的源码,谢谢了。souopoeoro@hotmail.com

  • 龘焱 2020年11月9日 下午7:10

    老师你好,运动规划文章写的很详细,给了我很大的帮助,希望能得到它们的源码,谢谢老师,邮箱:1252153741@qq.com

  • g84hc_1889 2020年11月3日 下午8:04

    老师您好,您的这些运动规划介绍让我受益匪浅,希望能得到它们的源码,邮箱:2979898455@qq.com 非常感谢!

    • tloinny 回复 g84hc_1889 2020年11月5日 下午11:11

      邮件已发,注意查收,祝您生活愉快!

  • t9n27_1522 2020年11月2日 下午10:34

    太棒了,浅显易懂,代码也好看懂。尤其是那个动图,绝了,理解起来太简单了。太感谢楼主了。
    也想麻烦楼主分享一下 运动规划的5个源码。 邮箱:1065523337@qq.com 万分感谢

    • tloinny 回复 t9n27_1522 2020年11月5日 下午11:10

      邮件已发,注意查收,祝您生活愉快!

  • ucav9_2579 2020年10月29日 下午3:44

    老师您好,可以发一下运动规划系列5个源码学习一下吗?邮箱是: 1173219926@qq.com 万分感谢!

    • tloinny 回复 ucav9_2579 2020年11月5日 下午11:07

      邮件已发,注意查收,祝您生活愉快!

  • 璟此一生 2020年10月29日 上午10:15

    老师您好,您什么时候可以出一个JPS跳点搜索算法的matlab实现呢,想学习一下。

  • xdx50_1271 2020年10月26日 下午8:13

    老师你好,运动规划文章写的很详细,给了我很大的帮助,希望能得到它们的源码,谢谢老师,邮箱:duansutao@126.com

    • tloinny 回复 xdx50_1271 2020年10月27日 上午10:12

      邮件已发,注意查收,祝您生活愉快!

  • 6oxaw_1996 2020年10月15日 上午9:57

    老师你好,您的这些运动规划文章给了我很大的帮助,希望能得到它们的源码,谢谢老师,邮箱:121071318@qq.com

    • tloinny 回复 6oxaw_1996 2020年10月16日 下午2:06

      邮件已发,注意查收,祝您生活愉快!

  • 7djip_6698 2020年10月13日 下午5:23

    老师 能发一下源码吗? 邮箱:875193130@qq.com 感谢!

  • 一个人 2020年10月8日 下午9:46

    对于小白来说,您的教程浅显易懂,真的受益匪浅,非常感谢您提供的教程,不知可否分享一下运动规划的5个程序源代码,如果可以的话,就真的太感谢您了!
    邮箱:
    xhma1998@163.com

    • tloinny 回复 一个人 2020年10月9日 上午9:16

      邮件已发,注意查收,祝您生活愉快!

    • t9n27_1522 回复 一个人 2020年11月4日 上午11:28

      你好,同为小白,想请楼主分享代码,但是可能楼主最近比较忙, 不知道您可以 将楼主发给你的代码 转发给我呢? 谢谢啦

    • t9n27_1522 回复 t9n27_1522 2020年11月4日 上午11:28

      我的邮箱 1065523337@qq.com

    • t9n27_1522 回复 一个人 2020年11月4日 上午11:29

      你好,同为小白,想请楼主分享代码,但是可能楼主最近比较忙, 不知道您可以 将楼主发给你的代码 转发给我呢? 谢谢啦! 我的邮箱 1065523337@qq.com

  • JYY 2020年10月7日 下午10:47

    非常感谢您提供的教程,可否分享一下运动规划的5个程序源代码?非常感谢!
    邮箱:
    1620082624@qq.com

    • tloinny 回复 JYY 2020年10月9日 上午9:16

      邮件已发,注意查收,祝您生活愉快!

  • 树对 2020年10月7日 下午8:06

    老师好,运动规划系列代码方便发一下吗,邮箱1419411504@qq.com,感谢!

  • pw0na_9524 2020年10月5日 上午11:11

    老师您好!可以发一下代码吗,邮箱是1071477073@qq.com,谢谢!

  • zhangss 2020年10月3日 下午6:56

    老师您好,您能分享一下规划入门的五个程序源代码吗?非常感谢!邮箱:zhangss0817@outlook.com

    • tloinny 回复 zhangss 2020年10月4日 上午11:55

      邮件已发,注意查收,祝您生活愉快!

  • 黏土 2020年9月30日 上午10:21

    楼主,可以发一份路径规划入门系列1-5的完整运行代码?
    邮箱是1289833979@qq.com
    好人一生平安!

  • 命运独奏曲 2020年9月24日 上午10:06

    老师您好,请问可以分享一下路径规划入门五个Matlab程序吗?邮箱:625836410@qq.com,非常感谢老师!

  • 我是谁 2020年9月23日 上午10:02

    老师您好,您能分享一下规划入门的五个程序源代码吗?非常感谢!邮箱:1972925189@qq.com

    • tloinny 回复 我是谁 2020年9月25日 上午9:03

      邮件已发,注意查收,祝您生活愉快!

    • 黏土 回复 我是谁 2020年9月30日 上午10:22

      同学可以分享一下代码吗?十分感谢!
      邮箱是:1289833979@qq.com

  • 青花瓷 2020年9月22日 下午4:07

    老师,能发一下五个算法的源码吗?十分感谢!邮箱1786797220@qq.com

    • tloinny 回复 青花瓷 2020年9月25日 上午9:02

      邮件已发,注意查收,祝您生活愉快!

  • MCOMEBACK 2020年9月22日 下午3:04

    老师您好,请问可以分享一下RRT和人工势场法算法的源代码吗?非常感谢!邮箱704461808@qq.com

    • tloinny 回复 MCOMEBACK 2020年9月25日 上午9:01

      邮件已发,注意查收,祝您生活愉快!

  • gbeik_2186 2020年9月21日 下午9:02

    老师您好,请问可以分享一下您介绍的这五个算法的源代码吗?想学习一下。非常感谢!邮箱845749962@qq.com

  • k5see_7647 2020年9月21日 下午3:36

    老师您好,请问可以分享一下您介绍的这五个算法的源代码吗?非常感谢!邮箱2428304940@qq.com

  • C ’ 2020年9月21日 上午2:27

    老师您好,可以分享一下这五个算法的源代码吗?非常感谢您!邮箱373439110@qq.com

    • tloinny 回复 C ’ 2020年9月21日 上午9:16

      邮件已发,注意查收,祝您生活愉快

  • l5j93_0119 2020年9月20日 下午10:06

    老师,可以分享一下五个算法的源代码吗?非常感谢!邮箱574668446@qq.com

  • 弥满甜蜜 2020年9月18日 下午9:05

    老师,麻烦分享一下五个算法的源代码,谢谢,邮箱523986162@qq.comqq.com

  • HappenSamaQAQ 2020年9月17日 下午5:26

    老师,能分享一下五个算法的源码吗?十分感谢!邮箱2362459918@qq.com

  • p6klk_3778 2020年9月16日 下午9:24

    我应该在这里问,老师,能发一下五个算法的源码吗?非常感谢
    matthew_sjtu@163.com

    • tloinny 回复 p6klk_3778 2020年9月16日 下午11:21

      邮件已发,注意查收,祝您生活愉快!

  • zncq5_5191 2020年9月13日 上午1:13

    老师,能发一下五个算法的源码吗?十分感谢!邮箱1416700422@qq.com

    • tloinny 回复 zncq5_5191 2020年9月13日 上午10:21

      邮件已发,注意查收,祝您生活愉快!

  • 48dj4_4379 2020年9月10日 下午5:38

    老师能将运动规划系列5个源码分享学习一下吗?万分感谢 邮箱是:876734147@qq.com

  • untamed 2020年9月6日 下午1:14

    非常崇拜,可以分享学习一下5个源码吗? 非常感谢!! 邮箱是 374397003@qq.com

    • tloinny 回复 untamed 2020年9月6日 下午6:41

      邮件已发,注意查收,祝您生活愉快!

  • 丢了薛定谔的猫 2020年9月2日 下午10:03

    老师您好,请问您可以讲整个运动规划系列的源码分享一下吗???非常感谢。 邮箱是979588364@qq.com

  • 时光香茶 2020年8月26日 下午11:10

    老师能将运动规划系列5个源码分享学习一下吗?万分感谢 邮箱是:xiarisuixing@163.com

  • xq453_0317 2020年8月26日 下午4:02

    写的太好了,简单易懂,可以分享一下源码吗?想学习一下,感谢!邮箱:819032030@qq.com

  • 璟此一生 2020年8月25日 下午4:08

    老师您好,可以发一下运动规划系列5个源码学习一下吗?邮箱是: 929415785@qq.com 万分感谢!

  • May丶 2020年8月25日 下午2:28

    老师您好,可以分享一下源码吗?想学习一下,感谢!邮箱:923047247@qq.com

    • tloinny 回复 May丶 2020年8月25日 下午11:04

      邮件已发,注意查收,祝您生活愉快!

  • Jason 2020年8月20日 下午7:11

    老师您好,可以分享一下源码吗?想学习一下,感谢!邮箱:374672587@qq.com

    • tloinny 回复 Jason 2020年8月20日 下午8:38

      邮件已发,注意查收,祝您生活愉快!

  • 罗伯特祥 2020年8月20日 上午10:01

    大佬,求系列代码学习一下,感谢分享,邮箱robert.h.x.s@foxmail.com

  • The & shy 2020年8月18日 下午11:35

    老师,您好,最近再看关于导航,路径规划方面得东西,拜读了您的文章,获益匪浅,希望能学习一下您的源码方便更深入得理解,感谢! 邮箱:1477856814@qq.com

  • bot_2333 2020年8月18日 下午4:38

    老师您好,可以发一下人工势场源码学习一下吗?邮箱是1872808056@qq.com
    谢谢老师~

  • dtvee_6515 2020年8月18日 上午8:38

    厉害,期待更多算法更新;
    690064483@qq.com
    麻烦提供源码学习

  • 无花果 2020年8月17日 上午9:48

    老师您好,可以发一下人工势场的源码学习一下吗?邮箱是: 1518161708@qq.com 万分感谢。

    • tloinny 回复 无花果 2020年8月18日 上午9:29

      邮件已发,注意查收,祝您生活愉快!

  • Wayne 2020年8月14日 下午6:51

    您好,可以发一下人工势场源码学习一下吗?感谢!邮箱:1242238432@qq.com

    • tloinny 回复 Wayne 2020年8月14日 下午9:57

      邮件已发,注意查收,祝您生活愉快!

  • 97lwc_4987 2020年8月14日 上午11:30

    老师您好,可以将Dijkstra、A*、PRM、RRT、人工势场法这五种算法的源代码分享一下吗,感谢您!邮箱是1140436248@qq.com

  • u71ll_8507 2020年8月14日 上午2:37

    导师给我定的研究方向就是路径规划方向的,但我现在还没入门,跪求大佬分享一下有关运动规划的源码!非常感谢!邮箱是1783779664@qq.com

  • 53riv_3929 2020年8月13日 下午8:04

    同求Dijkstra、A*、PRM、RRT和人工势场法的源代码,多谢了!!!邮箱是2583409209@qq.com

  • x4q9d_2991 2020年8月13日 下午4:16

    您好,如果方便的话希望可以分享下一下Dijkstra、A*、PRM、RRT和人工势场法的源代码,非常感谢