D* Lite路径规划中make_heap插入性能瓶颈的优化方案问询

我在C++中实现了D* Lite路径规划器，但遇到障碍物时重规划耗时远超预期。在2560×2560地图测试中，起点出现障碍物后的重规划耗时约49.5秒，其中42.8秒消耗在插入节点到堆的insertHeap函数中，该函数实现如下：

void DstarPlanner::insertHeap(unsigned int id, HeapKey k) {
  U.push_back({ k, id });
  make_heap(U.begin(), U.end(), KeyCompare());
}

相关键值及变量定义：

using Cost = float;
using Edge = pair<unsigned int, unsigned int>;
using HeapKey = pair<Cost, Cost>;
using KeyCompare = std::greater<std::pair<HeapKey, unsigned int>>;
vector<pair<HeapKey, unsigned int>> U;

我的旧版A*规划器采用multiset实现开放列表：

struct cells {
  int currentCell;
  float fCost;
};

multiset<cells> OPL;

小地图起点遇障时A速度优于当前D Lite，但障碍物消失时D* Lite更快。还看到网上有D* Lite实现用ds_pq定义开放列表：

ds_pq openList;

其中ds_pq定义为：

typedef std::priority_queue<state, std::vector<state>, std::greater<state>> dq_pq;

希望找到无需重构整个程序的优化方案，达到D* Lite应有的高效性能。

核心瓶颈修复：替换低效堆维护方式 #

• 问题根源：当前insertHeap每次插入节点后调用make_heap，会对整个堆进行O(n)时间复杂度的重构，大地图节点数量极多时，直接导致了42.8秒的耗时。
• 优化方案：改用std::priority_queue替代手动维护vector堆：
  1. 替换容器定义：

     std::priority_queue<pair<HeapKey, unsigned int>, vector<pair<HeapKey, unsigned int>>, KeyCompare> U;
     

  2. 修改insertHeap函数：

     void DstarPlanner::insertHeap(unsigned int id, HeapKey k) {
       U.push({k, id});
     }
     

  priority_queue内部会自动在每次push时以O(log n)的时间复杂度调整堆结构，比O(n)的make_heap效率提升几个数量级，完全适配D* Lite对开放列表的操作需求。

备选优化：复用A*的multiset实现 #

如果不想切换到priority_queue，可以直接复用你A中的multiset作为D Lite的开放列表：

1. 定义适配D* Lite键值的结构体：

   struct HeapNode {
     HeapKey key;
     unsigned int id;
     bool operator<(const HeapNode& other) const {
       return KeyCompare()(std::make_pair(key, id), std::make_pair(other.key, other.id));
     }
   };
   

2. 替换原容器：

   multiset<HeapNode> U;
   

3. 修改insertHeap：

   void DstarPlanner::insertHeap(unsigned int id, HeapKey k) {
     U.insert({k, id});
   }
   

multiset的插入操作是O(log n)，虽然比priority_queue常数稍大，但你已熟悉其用法，无需额外学习成本，且能解决当前性能问题。

额外细节优化 #

• 处理重复节点：D* Lite中同一节点可能多次进入开放列表，使用priority_queue时无需主动删除旧节点，只需在弹出节点时检查其键值是否为当前有效键值，无效则直接跳过；使用multiset时可先查找并删除旧节点再插入新节点，减少冗余计算。
• 内存预分配：如果使用priority_queue或vector作为底层容器，提前调用reserve预分配足够空间（根据地图大小预估节点数量），避免频繁内存分配带来的开销。

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