第212篇 D* Lite算法——动态环境中的增量路径规划

前两篇讲了A和它的变体提到DLite是动态环境中的增量搜索算法。今天展开讲D* Lite的具体细节——它是怎么做到环境变了不用从头搜的。这个算法理解起来有一定难度但面试考的概率很高。D* Lite是2002年由Koenig和Likhachev两人提出的。讲真这是移动机器人导航领域用得最多的全局规划算法之一。ROS的navigation2里就有D* Lite的实现很多商业AMR也用它做全局规划。面试时如果你能说清楚D* Lite的工作原理基本说明你对规划算法有真正的理解。一、为什么需要增量搜索想象一个场景机器人在仓库里导航地图大部分是已知的但偶尔会有临时放的货物箱。用标准A*每次检测到新障碍物丢掉整棵搜索树从头搜索。即使只多了一个小箱子也要重新搜索几千个节点。用D* Lite只更新箱子附近的节点其他节点的搜索结果保持不变。增量更新可能只需要修改十几个节点。差距有多大之前做AMR项目时测过100x100的网格新增一个障碍物A重新搜索需要3msDLite增量更新只需要0.1ms。快了30倍。在实时性要求高的场景比如机器人速度很快这个差距是决定性的。二、D* Lite的核心概念D* Lite有两个关键概念g(s)节点s到起点的当前估计距离。这就是A*里的g值。rhs(s)节点s的一步lookahead距离。定义rhs(s) min(cost(s, s) g(s)) for all successors s通俗地说rhs(s)是从s走一步到某个邻居再从邻居走到起点的最短距离。一致性当g(s) rhs(s)时节点是一致的consistent。一致的节点不需要更新。当g(s) ! rhs(s)时节点是不一致的需要重新计算。一致性有两种情况g(s) rhs(s)节点被高估了需要降低g值g(s) rhs(s)节点被低估了通常因为障碍物出现需要升高g值环境变化时只有变化点附近的节点会变得不一致。D* Lite只更新这些不一致的节点其他节点保持不变。这就是增量搜索的核心。三、D* Lite的算法流程D* Lite从终点反向搜索到起点。为什么反向因为机器人移动时起点在不断变化机器人当前位置终点不变。反向搜索只需要初始化一次之后机器人每移动一步只需要更新起点。class DStarLite: def __init__(self, grid, start, goal): self.grid grid self.start start self.goal goal self.g {} # g值 self.rhs {} # rhs值 self.U PriorityQueue() # 优先队列 # 初始化所有节点ginf, rhsinf for s in all_states(grid): self.g[s] float(inf) self.rhs[s] float(inf) # 终点初始化 self.rhs[goal] 0 self.g[goal] 0 self.U.put(goal, self.key(goal)) def key(self, s): # 优先队列的排序键 return [min(self.g[s], self.rhs[s]), min(self.g[s], self.rhs[s]) heuristic(s, self.start)] def compute_shortest_path(self): while self.U: u self.U.get() if self.key(u) self.key(self.start): # 更新不一致的节点 if self.g[u] ! self.rhs[u]: self.g[u] self.rhs[u] for s in predecessors(u): self.update_vertex(s) else: break def update_vertex(self, s): self.rhs[s] min(cost(s, s_next) self.g[s_next] for s_next in successors(s)) if self.g[s] ! self.rhs[s]: self.U.put(s, self.key(s))四、环境变化后的处理当检测到地图变化比如某个格子的障碍物状态变了D* Lite的处理流程更新变化格子的边代价更新变化格子的rhs值如果rhs变了把变化格子加入优先队列调用compute_shortest_path()——只处理优先队列中的不一致节点def map_changed(self, changed_cells): for s in changed_cells: # 更新受影响的边 self.update_edge_costs(s) # 更新rhs self.update_vertex(s) # 更新邻居 for s_next in neighbors(s): self.update_vertex(s_next) # 增量搜索 self.compute_shortest_path()关键点只有变化格子及其邻居需要更新。其他节点的g值和rhs值保持不变。这就是增量搜索的威力——环境变化只影响局部不影响全局。就像你在高速公路上开车前方有一个事故局部变化你只需要绕一下事故点就行不用重新规划从家到公司的整条路线。五、D* Lite vs D*D是DLite的前身1994年Stentz提出。D* Lite改进了D*的几个问题代码更简洁——D的实现非常复杂DLite简化了很多理论分析更清晰——g值和rhs值的设计让正确性证明更直观实际效率更高——减少了不必要的节点更新避免了D*中的重复计算工程上基本都用D* Lite而不是原版D。面试时提到DLite就够了不需要了解D*的具体实现细节。六、面试实战QDLite为什么从终点反向搜索* A因为机器人移动时起点在变当前位置终点不变。反向搜索只需要初始化一次。机器人移动后只需要更新起点位置不用重新搜索。如果正向搜索每次机器人移动都要更新所有节点的g值。QDLite和A的最优性一样吗** A一样。D* Lite保证找到最短路径和A*一样。增量更新不改变最优性——它只是避免了重复计算已经正确的部分。QDLite的缺点是什么* A一是实现复杂度比A高不少——g值、rhs值、优先队列的key函数都比较复杂。二是环境变化很大时比如一半地图都变了增量更新可能比重新搜索还慢——因为大部分节点都变得不一致了。三是在高维空间中效果不明显——DLite主要用在2D或2.5D的移动机器人导航。QDLite在实际项目中怎么用的* A之前做仓储AGV时全局地图用D* Lite。机器人每移动一步检查传感器有没有检测到新障碍物。如果有更新地图然后调用D* Lite的增量更新。大部分情况下增量更新只需要0.1ms比重新搜索快很多。只有在地图变化特别大比如搬了一批新货架时才重新搜索。小结D* Lite的核心用g值和rhs值维护节点的一致性。环境变化后只更新不一致的节点避免从头搜索。从终点反向搜索机器人移动时只需更新起点。增量更新在环境变化不大时比重新搜索快几十倍。D* Lite是移动机器人导航的标配算法。如果你做移动机器人方向D* Lite是必须掌握的。面试时能讲清楚g值和rhs值的关系、为什么反向搜索、增量更新的原理和适用场景基本就过关了。下一篇讲PRM概率路线图——预计算方案的适用场景这是采样规划系列的开篇。如果这篇文章对你有帮助欢迎点赞、在看、转发三连。 你的支持是我持续更新的最大动力。「机器人软件开发面试·从入门到精通」连载系列上一篇第211篇 A算法变体——Weighted A/ARA*/D*的工程应用下一篇预告第213篇 PRM概率路线图——预计算方案的适用场景有任何问题欢迎评论区留言我会尽量回复。