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

Question

目前，我尝试实现路径规划的D* Lite算法(也请参阅这里)来掌握它。我在web上找到了两种实现，都是用于C/C++的，但不知怎么的，我无法完全遵循这些思想，因为它们似乎与白皮书中的伪代码有很大的不同。我特别使用以下两篇文章：Lite，2002年 Koenig & Likkachev，“未知地形下导航的快速重新规划”，IEEE Robotics交易，第21卷，第3号，2005年6月

我尝试从第一个白皮书(p.5，图4)实现D* Lite的优化版本，为了“调试”，我使用了第二个白皮书(p.6、图6和图7)中所示和解释的示例。所有工作都是用MatLab完成的(更容易与他人交换代码)。

到目前为止，我只运行了一次computeShortestPath()代码，就可以找到初始最短路径。但是现在我陷入了伪码的{36‘}和{37’}行：

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

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

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

谢谢您的任何提示、建议和/或帮助！

Answer 1

其他人向我指出以下几点：

在现实世界的代码中，你几乎不需要“扫描图表中的变化”。您的图形只有在代码中更改时才会更改，因此您已经知道它可以在何时何地更改！ 实现这一点的一种常见方法是在您的Graph中有一个OnGraphChanged回调，可以设置它来调用PathFinder类中的OnGraphChanged方法。然后，在图类中的任何地方更改图，确保调用OnGraphChanged回调。

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