路径规划之Field D*算法

提出背景：在以往的路径规划算法中，如A*, D*, D* Lite等，机器人的运动角度被限制为π/4的倍数。因此我们也可以在相关的算法论文的实验部分看到一些我们觉得这样运动会造成成本变大的路径。基于此，field d*算法将取消运动角度为π/4的倍数这一限制，并且告诉机器人移动多大的角度。

思路分析：在网格地图的路径规划算法中，从某个结点（例如s）到目标结点的代价为：

其中nbrs(s)是s的所有相邻节点的集合；c(s, s’)是从s到s'的代价，g(s’)是节点s’到目标结点的代价。

之前的算法都是假设从节点s到相邻节点的唯一可能的移动只能是直线轨迹（只能是从一格移动到另一格）。而Filed D*考虑放宽这个假设，允许从节点s到其网格单元边界上的任何一点的直线轨迹（如图1蓝线所示）。那么相似的，我们只需要知道边s1s2上的sb的值，就可以通过最小化c(s, sb)+g(sb)来求得s点的代价。其中，c(s, sb) 的计算方式是s和sb之间的距离乘以到达s所在单元的代价。这里对s1,s2,,,s8的理解并不是一个网格的8个点，而是说他们分别代表了一个网格，把他们看成每个网格的质点即可。

图1 图2 图3

如图2所示，设s到s1的代价为b，s到s2的代价为c。再假设机器人按照图3的轨迹运动（可能的运动方式不止这一种，接下来会介绍）。那么 c(s,sy)= $bx+c\sqrt{(1-x)^{^{2}}+y^{^{2}}}$ .然后我们再通过线性插值法，令 $g(s_{_{y}})=yg(s_{2})+(1-y)g(s_{1})$ .虽然这个假设并不完美，毕竟线性插值法是一种假设，但是实际应用中证明了这是行之有效的方法，因此这里姑且这样。至此，我们s点的路径代价可以计为：

现在我们接着上文讨论field d*算法有几种可能的运行路径。由于线性插值法，x和y中至少有一个为0或者1。如果能明确这一点，那么有几种运行路径就显而易见了。我们可以做一个简单的证明

首先令f=g(s1)-g(s2),把它看作是右边(right edge)即线段s1s2的代价大小。基于此，我们研究的问题等价于寻找一条从s到s2的最短路径。因为如果f<0，那么最短路径永远是从s到s1的那条底部水平线段，这相当于让f＜0以便让机器人走过整个右边（举个例子，我们想从s1沿着右边运动到s2，所以问题就变成了寻找s到s1的最短路径）。当然这一部分只是从s到s2的路径成本，对于结点s的整个成本而言，还应该加上g(s2)。

现在，我们做出一个与上一段中红字相反的假设：x和y中没有一个为0或者1，机器人的运动路径如图4(a)的蓝线所示，它是最短路径，小于s到s1再到s2的路径成本。那么sx到sy的成本也就小于sx到s1再到sy，用公式表示为

图4

注意：这里切不可简单的理解为三角形的两边之和大于第三边，因为我们讨论的成本相当于图论中的边的权重，而不是边的长度。在图4(b)中作△(sx,s1,sy)的相似三角形，边的关系也同样满足。至此，观察图4(c),不难发现新的蓝色路径成本低于图4(a)的成本。得证。

因此，机器人的运行路径只有图(ii)(iii)(iv)所示的三种情况，分别对应{x=1,y=0},{x≠0,1 y=1},{x=0, y≠0,1}.那么这三种路径中哪个成本最低呢？这取决于c与b的相对大小以及f的值。

图 5

情形①：f＜0，则g(s)=min(c,b)+g(s1).

情形②: f=b.那么图(iii)和(iv)的成本是一样的。我们可以通过求y(数值上等于1-x)的值得到最低成本路径。令k=f=b,则g(s)= $c\sqrt{1+y^{2}}+k(1-y)+g(s_{2})$ ,对y求导，令其为0，可求得取最小值时的y值为 $y=\sqrt{k^{2}/(c^{2}-k^{2})}$ .

情形③：f＜b. 此时采用图5(iv)所示的路径，令k=f，用②中的式子计算成本

情形④： f>b. 此时采用图5(iii)所示的路径，令k=b,y=1-x，用②中的式子计算成本

P.S.:对于情形②中两个成本相等的说法，这里可以给出证明：

路径规划之Field D*算法相关推荐

路径规划之RRT类算法简述
关注同名微信公众号"混沌无形",有趣好文! 原文链接:机器人空间采样算法研究现状简述(包含原文PDF百度云下载链接) 空间采样算法按照采样空间不同,可分为:状态空间采样和运动空间采 ...
路径规划之DWA类算法简述
关注同名微信公众号"混沌无形",有趣好文! 原文链接:机器人空间采样算法研究现状简述(包含原文PDF百度云下载链接) 空间采样算法按照采样空间不同,可分为:状态空间采样和运动空间采 ...
路径规划之空间采样算法研究现状简述
关注同名微信公众号"混沌无形",有趣好文! 原文链接:机器人空间采样算法研究现状简述(包含原文PDF百度云下载链接) 摘要:运动规划是移动机器人自主导航系统中的重要模块之一,相关算 ...
机器人路径规划_人工蜂群算法
机器人路径规划_人工蜂群算法原理 ABC(Artificial BeesColony)算法最先由Basturk等人提出并应用于函数优化问题,蜂群采蜜行为与函数优化问题对应关系如表1所示.由表1可知, ...
【Matlab路径规划】蚁群算法机器人大规模栅格地图最短路径规划【含源码 1860期】
一.代码运行视频(哔哩哔哩) [Matlab路径规划]蚁群算法机器人大规模栅格地图最短路径规划[含源码 1860期] 二.蚁群算法及栅格地图简介随着机器人技术在诸多领域的应用, 如机器人协作焊接.灾 ...
路径规划之PRM类算法
关注同名微信公众号"混沌无形",有趣好文! 原文链接:机器人空间采样算法研究现状简述(包含原文PDF百度云下载链接) 空间采样算法按照采样空间不同,可分为:状态空间采样和运动空间采 ...
路径规划与避障算法(七)---DWA算法流程之三---碰撞检测评价函数
版权声明:本文为博主原创文章,原创不易, 转载请联系博主. 本篇博客主要介绍DWA算法所采用的评价函数中障碍物相关的评价函数评价函数: 轨迹主要依据以下三条准则进行评分,综合评分后选取分数最小的路径 ...
【路径规划】(1) Dijkstra 算法求解最短路，附python完整代码
好久不见,我又回来了,这段时间把路径规划的一系列算法整理一下,感兴趣的点个关注.今天介绍一下机器人路径规划算法中最基础的 Dijkstra 算法,文末有 python 完整代码,那我们开始吧. 1. ...
【Matlab路径规划】蚁群算法求解机器人栅格地图最短路径规划问题【含源码 1580期】
一.代码运行视频(哔哩哔哩) [Matlab路径规划]蚁群算法求解机器人栅格地图最短路径规划问题[含源码 1580期] 二.matlab版本及参考文献 1 matlab版本 2014a 2 参考文献 ...

路径规划之Field D*算法

路径规划之Field D*算法相关推荐

最新文章

热门文章