Field Ｄ*路径规划算法
Field Ｄ*路径规划算法
- 1.栅格法路径规划存在的问题
- 2.Filed D*算法主要思想解析
- 3.Filed D*算法伪代码
- 4.算法优化
- 5.算法总结
- 参考文献
- 搜索算法其他文章
紧接着上一篇D* Lite路径规划算法，这一篇介绍D* Lite算法的改进版Field Ｄ*。
Filed D_star算法是D_star Lite算法的一种改进版本，该算法针对基于栅格的路径规划算法通常以栅格端点或中心点作为路径的节点，限制了路径方向变化只能为π/4的倍数，会导致机器人不必要的运动转向，影响执行效率。而Dave Ferguson提出的Filed D*算法，通过对栅格进行线性插值使路径点不局限于端点，规划方向的变化不再受限于π/4的倍数，生成较为平滑的路径。
1.栅格法路径规划存在的问题
在规划2D网格时，通常从每个网格单元的中心进行规划，并且只允许转换到相邻网格单元的中心。这会将限制路径方向变化只能为π/4的倍数，从而导致路径为次优的，并且在实践中难以实现。如下图1所示，在实际的路径规划中，规划的路径只能走s点的8个方向（s1,s2,s3,s4,s5,s6,s7,s8），只能为π/4的倍数。
举个例子，机器人在如下图2没有障碍物的环境中，需要从起始点s运动到目标点g。显然，最优化的路径是起始点s到目标点g的直线。但是机器人的朝向角度为π/8，不是π/4的倍数，因而不能直接走s、g间的直线，只能按照e1e_1e1，e2e_2e2走折线段。根据图中的计算可知，走e1e_1e1，e2e_2e2的路线约多走8%的路程。
有一种方式可以缓解这种困难，例如在机器人起点s，寻找路径时，找到从s到p的直线路径无碰撞的最远点p，然后用这条直线路径替换原来的p路径（如图3）。但是，也不是总是有效的。在这里，基于网格的路径从s到g(顶部，黑色)不能平滑，因为有四个障碍单元(阴影)。最佳路径显示为虚线(蓝色)。
2.Filed D*算法主要思想解析
Filed D*算法的关键是在给定相邻节点路径代价的前提下，提出一种计算每个网格节点路径代价的新方法。在基于网格的路径规划中，一个节点的代价为（从节点到目标的路径代价）：
其中，nbrs(s)是s的所有相邻节点的集合；c(s, s’)是遍历s和s’之间的边的代价，g(s’)是节点s’的路径代价。而在此之前都是假设从节点s到相邻节点的唯一可能的移动只能是直线轨迹（只能是从一格移动到另一格）。而Filed D*考虑放宽这个假设，允许从节点s到其网格单元边界上的任何一点的直线轨迹（如下图4所示）。如果知道每一个点sbs_bsb在边界的值,那么仅仅可以通过最小化c(s,sb)+g(sb)c(s, s_b)+ g(s_b)c(s,sb)+g(sb)计算节点s的最优值，c(s,sb)c(s, s_b)c(s,sb)通过s和sb之间的距离乘以到达s所在单元的代价。但是，这样的点有无数个，因此不可能为每个点计算g(sb)g(s_b)g(sb)。
但是，用线性插值法对每个边界点提供g(sb)的近似是可能的。首先将图4中的节点视为连续代价的样本点，由节点s开始的一条优化路径必须通过连接两个连续相邻的边缘,例如s1s2⃗\vec{s_1s_2}s1s2。因此，设置通过这些边的路径的最小代价为s的路径代价，而这些边被认为是一次一条。为了通过s1s2⃗\vec{s_1s_2}s1s2计算节点s的路径，需要使用节点s1s_1s1和s2s_2s2的路径代价以及图5（a）中所示中间网格的转移代价c和底部网格的转移代价b。
假设位于s1s_1s1和s2s_2s2之间边界上的任意点sy的路径代价是g(s1)g(s_1)g(s1)和g(s2)g(s_2)g(s2)的线性组合（如图6）:
其中，其中y是s1s_1s1到sys_ysy的距离(假设单位单元格)。这个假设并不完美，sys_ysy的路径代价可能不是g(s1)g(s_1)g(s1)和g(s2)g(s_2)g(s2)的线性组合，甚至也不是这些路径代价的函数。然而，这种线性近似在实际应用中效果良好，可以构造节点s路径代价的封闭形式解。根据这个近似，s在s1s_1s1、s2s_2s2、网格代价c和b下的路径代价可以计算为
这个公式可以通过上图7中的（a）图理解。公式中的第1项为s→sxs_xsx的代价，第2项为sxs_xsx→sys_ysy的代价，第3、4项为y处的代价。其中，x∈[0,1]x\in[0, 1]x∈[0,1]，y∈[0,1]y\in[0, 1]y∈[0,1]。当x=y=1时，路径为底部的路径，但是代价却是中间网格的代价。
令(x⃗,y⃗)(\vec{x},\vec{y})(x,y)是一对解决上述最小化x和y的值。由于线性插值，这些值中至少有一个是0或1。直观地说，如果部分地穿过中心单元格要比绕边界而行花费更少，那么完全地穿过单元格的开销最小。因此，如果有任何代价最低的路径穿过中心网格，它将尽可能大，迫使x = 0或y = 1。如果没有路径穿过中心网格，然后y= 0。（详细证明见论文）
经过论证可知，路径要么沿着整个底部边缘s1s_1s1(图5(ii))；或者将移动距离x底部边缘，然后直接沿着直线路径s2(图5(3))；或将s到sy点直线路径右侧边缘(图5 (iv))。哪一条路代价低，取决于c，b的相对大小，路径s1s_1s1和s2s_2s2代价差异：f=g(s1)−g(s2)f = g(s_1)−g(s_2)f=g(s1)−g(s2)。具体来说，如果f < 0，那么从s到s1s_1s1的最优路径为min(c,b)+g(s1)min(c, b)+g(s_1)min(c,b)+g(s1)(图5(ii))。如果f = b，那么使用底部边缘一部分路径的代价（图5(iii)）将等于不使用底部边缘路径的成本（图5(iv)）。可以解出后者的路径y⃗\vec{y}y(等于1−x⃗1-\vec{x}1−x前路径),最小成本路径如下。首先，令k = f = b，s的路径代价为（图8示例说明）
上式对y求导，令为0，可得取最小值时的y值：
无论使用底边还是右边，最终得到的计算和路径代价都是相同的。所以重要的是哪条边代价更低。如果f < b，然后使用上面的右边缘，并如上计算路径的代价(当k = f)；如果b < f，使用底部边缘，并用k = b和y⃗=1−x⃗\vec{y}=1-\vec{x}y=1−x替代到上面的方程。对于任意两个相邻的sas_asa和sbs_bsb，计算s最小的路径代价算法如图9所示。
3.Filed D*算法伪代码
一旦对图中给定的节点进行了基于插值的路径代价计算，然后就可以将其插入许多路径规划算法中以生成平滑路径。图10给出了Field D_star公式，这是一种包含这些插值路径代价的增量重新规划算法。这个版本的Field D_star是基于D_star Lite算法的(不同之处用红色高亮显示)。与最初基于图形的D* Lite版本不同，{20 - 22}行将Field D_star裁剪为网格。同时，由于路径相交的不仅仅是节点，h(sstart,s)h(s_{start}, s)h(sstart,s)必须足够小，当添加到从s发出的任何边的代价时，它仍然不大于从sstarts_{start}sstart到s的最小代价路径。
一旦计算出从初始状态到目标的路径的代价，就可以从初始位置开始扩展路径，并迭代计算要移动到下一个位置的单元边界点。由于插值技术，计算网格单元内任意一点的路径代价是可能的，而不仅仅是角落，这对于提取路径和在执行不完美的情况下返回轨道(通常对于真正的机器人是如此)都是有用的。Field D*应用的两个例子如下图11：
4.算法优化
为了减少计算量，通过在算法中降低被弹出状态的相邻点信息更新(第{09 - 14}行)时需要的计算量，只考虑那些被弹出状态的新值实际影响的状态以及这些状态是如何受到影响的。解决方案是引入状态函数bptr(s)、cknbr(s, s’)和ccknbr(s, s’)，和对算法做出两处修改。最终的算法为：
5.算法总结
Filed D_star算法是基于D_star Lite算法进行改进的，与D_star Lite算法最大的不同在于路径搜索的过程中，搜索方向不再局限于只能为π/4的倍数，而可以是任意的方向，可以生成更为平滑的路径，同时尽可能是最优路径，而不是次优路径。但是，该算法同样存在一定的问题：由于sys_ysy的路径代价不一定符合g(s1)g(s_1)g(s1)与g(s2)g(s_2)g(s2)的线性组合，线性估计的误差随路径点代价的更新累积，导致插值点位置估计发生偏差，使其前后路径连线不一定共线。
参考文献
[1]马丽莎, 周文晖, 龚小谨, et al. 基于运动约束的泛化Field D~路径规划[J]. 浙江大学学报(工学版), 2012, 46(08): 1546-1552.
[2]Ferguson D, Stentz A. The Field D Algorithm for Improved Path Planning and Replanning in Uniform and Non-Uniform Cost Environments[C], 2005.
搜索算法其他文章
Dstar Lite路径规划算法
终身规划Astar算法（LPA*）：Lifelong Planning A*
D*路径搜索算法原理解析及Python实现
Field Ｄ*路径规划算法相关推荐
- D* Lite路径规划算法
D* Lite路径规划算法 1.D* Lite算法简述 2.D* Lite算法伪代码 3.D*Lite算法一个简单的例子 3.1 地图无变化时 3.2地图变化时 4.算法总结 参考资料 搜索算法其他文 ...
- 人工势场法路径规划算法（APF）
本文主要对人工势场法路径规划算法进行介绍,主要涉及人工势场法的简介.引力和斥力模型及其推导过程.人工势场法的缺陷及改进思路.人工势场法的Python与MATLAB开源源码等方面 一.人工势 ...
- 自动驾驶路径规划——基于概率采样的路径规划算法（RRT、RRT*）
目录 1. RRT算法背景 1.1 RRT算法核心思想 1.2 RRT算法优缺点 2. 经典RRT算法 2.1 RRT算法流程 2.2 RRT伪代码 3. 基于目标概率采样 4. RRT*算法 4.1 ...
- 中国物流供应链“零的突破”！阿里路径规划算法入围运筹学“奥斯卡”
鱼羊 发自 凹非寺 量子位 报道 | 公众号 QbitAI 最新消息,中国物流力量,刚在全球运筹和管理科学界的最高荣誉中,实现零的突破! 1月15日,国际运筹学与管理科学学会(INFORMS)公布了2 ...
- 机器人学习--路径规划算法
参考网址:https://mp.weixin.qq.com/s?__biz=MzI5MDUyMDIxNA==&mid=2247520667&idx=3&sn=47ee67915 ...
- 多边形之间相交求交点的算法_路径规划算法总结
本文来自知乎网友@搬砖的旺财,地平线机器人算法工程师.作者根据自己本科和硕士阶段的学习经历,整理归纳了所接触过的规划算法. 1.自主机器人近距离操作运动规划体系 在研究自主运动规划问题之前,首先需建立 ...
- 基于SSD的自动路径规划算法
目录 1.场景需求 2.路径规划算法简介 2.1 .PRM算法简介 2.2.RRT算法简介 3.基于SSD的自动路径规划算法简介 4.基于SSD的自动路径规划算法详解 4.1.利用外置摄像头获取图像或 ...
- 路径规划算法_自动驾驶汽车路径规划算法浅析
自动驾驶汽车的路径规划算法最早源于机器人的路径规划研究,但是就工况而言却比机器人的路径规划复杂得多,自动驾驶车辆需要考虑车速.道路的附着情况.车辆最小转弯半径.外界天气环境等因素. 本文将为大家介绍四 ...
- 多机器人路径规划的代码_知荐 | 地平线机器人算法工程师总结六大路径规划算法...
来源 | 知乎 知圈 | 进"高精度地图社群",请加微信15221054164,备注地图 目录 1 自主机器人近距离操作运动规划体系········1.1 单个自主机器人的规划体系 ...
最新文章
- select weui 动态加载数据_weui中的picker使用js进行动态绑定数据问题
- 算法-----------数组------------只出现一次的数字
- 【控制】《多智能体系统一致性协同演化控制理论与技术》纪良浩老师-第14章-带通信和输入时延的异构竞争多智能体系统分组一致性
- Linux下samba服务速度很慢的问题
- RTT设备与驱动之PIN设备
- 原 layer父子页面交互
- SAP Spartacus B2B 页面 Popover Component 的条件显示逻辑
- 大数据、AI“武装”企业服务：风控、检索、安全
- ESFramework介绍之（28）―― Udp组件
- HDU 1234- 开门人和关门人
- 25 逆波兰表达式、波兰表达式
- 灌篮高手总决赛下载地址，都是pdf文件，黑白的，很清晰
- 视频技术系列 - 谈谈毫米波
- 《预训练周刊》第52期：屏蔽视觉预训练、目标导向对话
- python怎么隐藏输入法_打开和关闭输入法
- 最近在B站重新看学习视频小结
- 通过数据了解用户——数据在新品设计中的应用
- insmod等模块命令
- 【强化学习】Actor-Critic（演员-评论家）算法详解
- MonoRail例子（入门篇）