algorithm - 为路径规划实现 D* Lite - 如何检测边缘成本变化？

Question

我目前尝试实现用于路径规划的 D* Lite 算法(另请参阅 here)以掌握它。我在网上找到了两个实现，都是针对 C/C++ 的，但不知何故不能完全遵循这些想法，因为它们似乎与白皮书中的伪代码的差异超出预期。我特别使用以下两篇论文: Koening,S.;Likhachev,M. - D* Lite, 2002 Koenig & Likkachev, Fast replanning for Navigation in Unknown Terrain, IEEE Transactions on Robotics, Vol. 21, No. 3, June 2005

我尝试实现第一份白皮书(第 5 页，图 4)中 D* Lite 的优化版本，对于“调试”，我使用第二份白皮书(第 6 页，图 1)中显示和解释的示例。 6和图7)。所有工作都在 MatLab 中完成(更容易与他人交换代码)。

到目前为止，我通过运行一次 computeShortestPath() 来运行代码以查找初始最短路径。但现在我被困在伪代码的 {36''} 和 {37''} 行:

{36”} Scan graph for changed edge costs;
{37”} if any edge costs changed

我如何检测这些变化？我不知何故似乎不了解这是如何被检测到的？到目前为止，在我的实现中，我主要使用了 3 个矩阵。 一个与包含所有 rhs 值的网格图大小相同的矩阵。一个大小相同的矩阵，包含类似的所有 g 值。优先级队列的行数可变的矩阵，前两列是优先级键，第三和第四行是 x 和 y 坐标。

比较我的结果，我在第 5 步中第一次运行 computeShortestPath() 时得到相同的结果，如第二份白皮书第 6 页图 6 中所示。将机器人移动一步也不是问题。但是我真的不知道应该如何实现下一步扫描变化的边缘成本。

感谢任何提示、建议和/或帮助!!!

Answer 1

下面是别人给我指出的:

In real-world code, you almost never have to "scan the graph for changes." Your graph only changes when you change it in the code, so you already know exactly when and where it can change!

One common way of implementing this is to have a OnGraphChanged callback in your Graph class, which can be setup to call the OnGraphChanged method in your PathFinder class. Then anywhere the graph changes in your Graph class, make sure the OnGraphChanged callback is called.

我个人通过使用“真实 map ”和“已知 map ”来实现它，并在每次移动后让机器人检查/扫描所有下一个可能的后继者并在真实 map 和已知 map 上进行比较。