D* Lite路径规划中find_if性能瓶颈的替代优化方案咨询

我在C++中实现D* Lite路径规划器时，维护了一个单元格的优先队列（U），每个单元格的排序Key由两个成本值计算得到。目前删除队列中单元格的操作占路径规划总耗时的80%，是核心性能瓶颈，现寻求高效替代方案。

当前代码基础 #

类型与队列定义 #

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

元素添加逻辑 #

U.push_back({ k, id });
push_heap(U.begin(), U.end(), KeyCompare());

性能瓶颈：删除操作 #

当前在updateVertex中通过遍历查找并删除元素：

auto it = find_if(U.begin(), U.end(), [=](auto p) { return p.second == id; });
U.erase(it);

完整相关函数代码 #

void DstarPlanner::insertHeap(unsigned int id, HeapKey k) {
    U.push_back({ k, id });
    push_heap(U.begin(), U.end(), KeyCompare());
    in_U[id]++;
}

void DstarPlanner::updateVertex(unsigned int id) {
    Cell* u = graph.getCell(id);

    if (u->id != id_goal) {
        Cost mincost = infinity;
        for (auto s : u->neighbors) {
            mincost = min(mincost, graph.getEdgeCost(u->id, s->id) + s->g);
        }
        u->rhs = mincost;
    }
    if (in_U[id]) {
        auto it = find_if(U.begin(), U.end(), [=](auto p) { return p.second == id; });
        U.erase(it);
        in_U[id]--;
    } 
    if (u->g != u->rhs) {
        insertHeap(id, u->calculateKey());
    }
}

vector<int> DstarPlanner::ComputeShortestPath() {
    vector<int> bestPath;
    vector<int> emptyPath;
    Cell* n = graph.getCell(id_start);    

    while (U.front().first < n->calculateKey() || n->rhs != n->g) {
        auto uid = U.front().second;
        Cell* u = graph.getCell(uid);
        auto kold = U.front().first;
        pop_heap(U.begin(), U.end(), KeyCompare());
        U.pop_back();
        in_U[u->id]--;

        if (kold < u->calculateKey()) {
            insertHeap(u->id, u->calculateKey());
        } else if (u->g > u->rhs) {
            u->g = u->rhs; 
            for (auto s : u->neighbors) {
                if (!occupied(s->id)) {
                    updateVertex(s->id);
                }
            }
        } else {
            u->g = infinity;
            for (auto s : u->neighbors) {
                if (!occupied(s->id)) {
                    updateVertex(s->id);
                }
            }
            updateVertex(u->id);
        }
    }

    bestPath=constructPath();
    return bestPath;
}

优化方案 #

1. 延迟删除（最优推荐，适配D* Lite特性） #

D* Lite算法天然支持懒删除策略，无需主动删除堆中旧条目，而是在弹出堆元素时验证有效性：

• 移除updateVertex中查找并删除堆元素的逻辑，需要更新时直接插入新条目到堆中，允许堆存在同一单元格的多个条目。
• 从堆顶弹出元素后，先检查该条目是否有效：若单元格的g == rhs，或者条目的Key与当前单元格计算的最新Key不一致，则判定为过时无效，直接跳过处理。

修改后的updateVertex：

void DstarPlanner::updateVertex(unsigned int id) {
    Cell* u = graph.getCell(id);

    if (u->id != id_goal) {
        Cost mincost = infinity;
        for (auto s : u->neighbors) {
            mincost = min(mincost, graph.getEdgeCost(u->id, s->id) + s->g);
        }
        u->rhs = mincost;
    }
    // 移除原查找删除逻辑
    if (u->g != u->rhs) {
        insertHeap(id, u->calculateKey());
    }
}

修改后的堆顶处理逻辑片段：

auto uid = U.front().second;
Cell* u = graph.getCell(uid);
auto kold = U.front().first;
pop_heap(U.begin(), U.end(), KeyCompare());
U.pop_back();
in_U[u->id]--;

// 新增无效条目判断
if (u->g == u->rhs) {
    continue;
}

if (kold < u->calculateKey()) {
    insertHeap(u->id, u->calculateKey());
} else if (u->g > u->rhs) {
    // 原有逻辑保持不变
    u->g = u->rhs; 
    for (auto s : u->neighbors) {
        if (!occupied(s->id)) {
            updateVertex(s->id);
        }
    }
} else {
    // 原有逻辑保持不变
    u->g = infinity;
    for (auto s : u->neighbors) {
        if (!occupied(s->id)) {
            updateVertex(s->id);
        }
    }
    updateVertex(u->id);
}

2. 自定义带索引的优先队列 #

若不想用延迟删除，可维护哈希表记录每个单元格在堆中的位置：

• 使用unordered_map<unsigned int, size_t>记录单元格id对应的堆下标，但需注意vector扩容会导致下标失效，可改用std::list作为堆底层容器，但list的堆操作效率略低于vector，需权衡性能与实现复杂度。

3. 第三方优先队列库 #

使用支持高效删除的优先队列实现，比如Boost库的boost::heap::fibonacci_heap，配合哈希表可实现O(1)查找与O(log n)删除，但会引入第三方依赖，需根据项目情况选择。

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