关于D* Lite算法能否批量处理新增障碍物的技术问询

非优化版UpdateVertex函数正确性验证 #

非优化版D* Lite的UpdateVertex核心逻辑中，三个分支并非全部都会执行：

• 第一个分支（判断u != goal）是计算rhs值的前置步骤，仅当u不是终点时执行；
• 后续两个分支是互斥的：
  • 若g[u] > rhs[u]，说明u的代价值需更新为更优的rhs值，将u加入OPEN队列；
  • 若g[u] < rhs[u]，说明u的当前g值已失效，设为无穷大后加入队列，并递归更新所有邻居节点；
  • 若g[u] == rhs[u]，则后续两个分支都不会执行。

你提到单次调用会执行全部三个if，大概率是代码逻辑错误：比如把第二个分支的else if写成了独立的if，导致在g[u] > rhs[u]成立后，还会进入g[u] < rhs[u]的判断（这在数值正常时不可能发生）。先检查分支语法，确保后两个判断是互斥的else if结构，同时排查g/rhs值是否存在异常（比如溢出、NaN）。

批量新增障碍物的路径规划失败问题 #

从以下几个方向排查：

• 障碍物cost更新不完整：批量新增障碍物时，需将所有对应网格的通行cost设为无穷大，若遗漏部分网格或更新顺序错误，会导致rhs/g值不一致，算法无法正确计算路径；
• 受影响节点未全部触发更新：大量障碍物会影响多个节点的代价，需确保所有受影响的节点都被调用UpdateVertex，若仅触发局部节点更新，会导致全局代价未同步；
• 数值精度/溢出问题：若用int类型存储cost，大量障碍物可能导致累计代价超出int范围，改用double或float类型，并检查是否存在负数、溢出等异常值；
• 算法终止条件错误：检查是否存在提前终止规划的逻辑，比如误判OPEN队列为空，或错误判断当前节点为起点，导致未完成全局代价更新就返回路径。

ROS TurtleBot部署注意事项 #

• 地图格式适配：将D* Lite的网格地图与ROS的nav_msgs/OccupancyGrid栅格地图做转换，注意坐标系差异（ROS以左下为原点，部分算法实现以左上为原点）；
• 实时性优化：非优化版D* Lite在大规模地图上效率较低，调通后建议切换到论文中的优化版（带启发式的优先级队列实现），满足TurtleBot的实时运动控制需求；
• ROS节点通信封装：将D* Lite封装为ROS节点，订阅地图更新、起点/终点话题，发布nav_msgs/Path路径话题，若替代move_base的路径规划器，需适配其接口规范；
• 障碍物更新触发逻辑：在ROS中，需在costmap批量更新完成后再触发D* Lite的重新规划，避免频繁触发导致计算混乱。

内容的提问来源于stack exchange，提问作者OverDemon