网站建设a云世家网络,秦皇岛短视频优化,建设电子商务网站的基本步骤,企业网站优化应该怎么做目录0 专栏介绍1 什么是D*算法#xff1f;2 D*算法核心概念一览3 D*算法流程图4 步步图解#xff1a;算法实例5 算法仿真与实现5.1 ROS C实现5.2 Python实现0 专栏介绍
#x1f525;附C/Python/Matlab全套代码#x1f525;课程设计、毕业设计、创新竞赛必备#xff01;详…
目录0 专栏介绍1 什么是D*算法2 D*算法核心概念一览3 D*算法流程图4 步步图解算法实例5 算法仿真与实现5.1 ROS C实现5.2 Python实现0 专栏介绍
附C/Python/Matlab全套代码课程设计、毕业设计、创新竞赛必备详细介绍全局规划(图搜索、采样法、智能算法等)局部规划(DWA、APF等)曲线优化(贝塞尔曲线、B样条曲线等)。
详情图解自动驾驶中的运动规划(Motion Planning)附几十种规划算法 1 什么是D*算法
动态A*(Dynamic A*, D*)算法是一种增量式路径规划算法与A*算法相比它最大的优势是可以同时兼容静态环境和存在未知动态变化的场景曾被美国用于火星探测器寻路。如果还不了解A*算法可以看路径规划 | 图解A*、Dijkstra、GBFS算法的异同(附C/Python/Matlab仿真) 这里有一个概念——增量式什么是增量式呢与启发式搜索利用启发函数指导高效搜索不同增量式搜索对曾经的规划信息进行综合再利用以减少搜索范围和时间。
D*算法实现增量式规划采用从目标点向起始点的反向搜索策略当既定路径被障碍阻塞时障碍后各个节点到目标点的最短路径未被影响只需在障碍附近重规划并更新起点到障碍处各节点的信息当路径绕过障碍后即可重新利用曾经规划的到达目标位置的最短路径。
所以动态障碍越靠近起点能重复利用的信息越多规划效率越高。当动态障碍靠近终点时仍要更新整张地图节点信息相当于应用反向A*算法进行重规划。
2 D*算法核心概念一览
D*算法的核心概念如下所示 c(x,y)c\left( x,y \right)c(x,y)从节点xxx移动到节点yyy的代价若xxx、yyy间存在障碍则c(x,y)infc\left( x,y \right) \mathrm{inf}c(x,y)inf t(x)t\left( x \right)t(x)节点xxx的表状态。有 NEW——未被搜索过的点OPEN——正在搜索的待考察节点CLOSED——被搜索过的节点。 为了适应动态环境D*算法的OPEN与CLOSED状态允许相互转化当节点扩展后从开节点表中移除时状态从OPEN变为CLOSED当节点传播动态障碍信息给邻域时节点将再次加入开节点表状态从CLOSED变为OPEN h(x)h\left( x \right)h(x)从节点xxx到目标点的代价 k(x)k\left( x \right)k(x)节点xxx的历史最小h(x)h\left( x \right)h(x)值即 k(x){h(x),t(x)NEWmin{k(x),h(x)},otherwisek\left( x \right) \begin{cases} h\left( x \right) \,\, , t\left( x \right) \mathrm{NEW}\\ \min \left\{ k\left( x \right) ,h\left( x \right) \right\} , \mathrm{otherwise}\\\end{cases}k(x){h(x),t(x)NEWmin{k(x),h(x)},otherwise 设置k(x)k\left( x \right)k(x)的目的在于加快障碍处局部路径重规划的效率。当某个路径点变为障碍时其h(x)infh\left( x \right) \mathrm{inf}h(x)inf而k(x)k\left( x \right)k(x)不变。若从开节点表中首选h(x)h\left( x \right)h(x)最小的点则将最后考察障碍附近的点而若从开节点表中首选k(x)k\left( x \right)k(x)最小的点则将率先考察障碍附近的点符合实际情况 节点xxx的元状态有 RAISED——此时k(x)h(x)k\left( x \right) h\left( x \right)k(x)h(x)说明节点xxx受到障碍影响LOWER——此时k(x)h(x)k\left( x \right) h\left( x \right)k(x)h(x)说明节点xxx未受障碍影响 process_state()\mathrm{process}\_\mathrm{state}\left( \right)process_state()核心函数静态环境下可以更新各节点到达目标点的最短路径及节点连接关系动态环境下可以传播受障碍影响后节点h(x)h\left( x \right)h(x)的变更信息到邻域 insert(x,val)\mathrm{insert}\left( x,val \right)insert(x,val)将节点xxx插入开节点表并令其h(x)valh\left( x \right) valh(x)val更新k(x)k\left( x \right)k(x) modify_cost(x)\mathrm{modify}\_\cos\mathrm{t}\left( x \right)modify_cost(x)通过insert(x,h(x)c(x,x.parent))\mathrm{insert}\left( x,h\left( x \right) c\left( x,x.\mathrm{parent} \right) \right)insert(x,h(x)c(x,x.parent))将处于CLOSED状态的节点xxx转换为OPEN状态
3 D*算法流程图
D*算法主函数流程如下所示 其中核心的process_state()\mathrm{process}\_\mathrm{state}\left( \right)process_state()算法流程如下 在将动态信息传播到邻域进行修正的过程中分为两种情况讨论
节点xxx处于LOWER态。考虑到障碍的出现只可能让路径不变或更曲折即让节点h(x)⩾k(x)h\left( x \right) \geqslant k\left( x \right)h(x)⩾k(x)所以对处在LOWER态的节点不存在更优路径此时只需将节点 的最优信息更新到邻域即可节点xxx处于RAISED态。与LOWER态节点不同此时节点xxx处可能存在更优路径即可能存在y∈neighbor(x)y\in \mathrm{neighbor}\left( x \right)y∈neighbor(x)使h(x)h(y)c(x,y)h\left( x \right) h\left( y \right) c\left( x,y \right)h(x)h(y)c(x,y)。对于y.parentxy.\mathrm{parent}xy.parentx的情形RAISED态与LOWER态更新情况相同以保持联结节点信息的同步。对于y.parent≠xy.\mathrm{parent}\ne xy.parentx的情形若h(y)h(x)c(x,y)h\left( y \right) h\left( x \right) c\left( x,y \right)h(y)h(x)c(x,y)则重新将节点xxx加入开节点表进行考察因为xxx可能可以用更优的代价值来更新邻域若h(x)h(y)c(x,y)h\left( x \right) h\left( y \right) c\left( x,y \right)h(x)h(y)c(x,y)考虑到h(y)⩽koldh\left( y \right) \leqslant k_{\mathrm{old}}h(y)⩽kold的情形在S2处已考虑因此只需包含h(y)koldh\left( y \right) k_{\mathrm{old}}h(y)kold更进一步由于节点yyy处在CLOSED状态所以通常h(y)k(y)⩽koldh\left( y \right) k\left( y \right) \leqslant k_{\mathrm{old}}h(y)k(y)⩽kold但此处h(y)koldh\left( y \right) k_{\mathrm{old}}h(y)kold说明节点yyy也受到了障碍影响导致价值升高体现了将yyy重新纳入开节点表的必要性。
4 步步图解算法实例
以下面的栅格地图为例红色栅格表示起点蓝色栅格表示终点黄色栅格表示开节点表中的节点绿色栅格表示闭节点表中的栅格 D*算法的静态规划阶段如下图所示 D*算法动态路径修正阶段如下所示 5 算法仿真与实现
5.1 ROS C实现
double DStar::processState()
{if (open_list_.empty())return -1;double k_old open_list_.begin()-first;DNodePtr x open_list_.begin()-second;open_list_.erase(open_list_.begin());x-t CLOSED;expand_.push_back(*x);std::vectorDNodePtr neigbours;this-getNeighbours(x, neigbours);// RAISE state, try to reduce k value by neibhboursif (k_old x-g_){for (DNodePtr y : neigbours){if (y-t ! NEW y-g_ k_old x-g_ y-g_ this-getCost(y, x)){x-pid_ y-id_;x-g_ y-g_ this-getCost(y, x);}}}// LOWER state, cost reductionsif (k_old x-g_){for (DNodePtr y : neigbours){if (y-t NEW || ((y-pid_ x-id_) (y-g_ ! x-g_ this-getCost(x, y))) ||((y-pid_ ! x-id_) (y-g_ x-g_ this-getCost(x, y)))){y-pid_ x-id_;this-insert(y, x-g_ this-getCost(x, y));}}}else{// RAISE statefor (DNodePtr y : neigbours){if (y-t NEW || ((y-pid_ x-id_) (y-g_ ! x-g_ this-getCost(x, y)))){y-pid_ x-id_;this-insert(y, x-g_ this-getCost(x, y));}else if (y-pid_ ! x-id_ (y-g_ x-g_ this-getCost(x, y))){this-insert(x, x-g_);}else if (y-pid_ ! x-id_ (x-g_ y-g_ this-getCost(y, x)) y-t CLOSED (y-g_ k_old)){this-insert(y, y-g_);}}}return open_list_.begin()-first;
}5.2 Python实现
def processState(self) - float:# get node in OPEN set with min k valuenode self.min_stateself.EXPAND.append(node)if node is None:return -1# record the min k value of this iterationk_old self.min_k# move node from OPEN set to CLOSED setself.delete(node) # k_min h[x] -- x: RAISE state (try to reduce k value by neighbor)if k_old node.h:for node_n in self.getNeighbor(node):if node_n.h k_old and node.h node_n.h self.cost(node, node_n):# update h_value and choose parentnode.parent node_n.currentnode.h node_n.h self.cost(node, node_n)# k_min h[x] -- x: LOWER state (cost reductions)if k_old node.h:for node_n in self.getNeighbor(node):if node_n.t NEW or \(node_n.parent node.current and node_n.h ! node.h self.cost(node, node_n)) or \(node_n.parent ! node.current and node_n.h node.h self.cost(node, node_n)):# Condition:# 1) t[node_n] NEW: not visited# 2) node_ns parent: cost reduction# 3) node_n find a better parentnode_n.parent node.currentself.insert(node_n, node.h self.cost(node, node_n))else:for node_n in self.getNeighbor(node):if node_n.t NEW or \(node_n.parent node.current and node_n.h ! node.h self.cost(node, node_n)):node_n.parent node.currentself.insert(node_n, node.h self.cost(node, node_n))else:if node_n.parent ! node.current and \node_n.h node.h self.cost(node, node_n):self.insert(node, node.h)else:if node_n.parent ! node.current and \node.h node_n.h self.cost(node, node_n) and \node_n.t CLOSED and \node_n.h k_old:self.insert(node_n, node_n.h)return self.min_k完整工程代码请联系下方博主名片获取 更多精彩专栏
《ROS从入门到精通》《Pytorch深度学习实战》《机器学习强基计划》《运动规划实战精讲》… 源码获取 · 技术交流 · 抱团学习 · 咨询分享 请联系
