C++实现D星 Lite算法

D* Lite算法核心概念

D* Lite是一种增量式的路径规划算法，适用于动态环境，能够高效地重新规划路径，而无需每次都从头开始计算。下表汇总了其实现中的关键数据结构与核心函数：

组件类型	名称	说明
关键数据结构	优先队列 (U)	存储待处理的节点，按键值(key)排序。键值通常包含 [k1, k2]，其中 k1 = min(g(s), rhs(s)) + h(s_start, s) + km，k2 = min(g(s), rhs(s))。
	g值	记录节点 s 到目标点的历史最短代价估计。
	rhs值	基于邻居节点 g 值计算的最小代价，满足 rhs(s) = min_{s'∈Succ(s)} (c(s, s') + g(s'))。若 g(s) == rhs(s)，则称节点 s 是一致的。
核心函数	CalculateKey(s)	计算节点 s 的键值，用于确定在优先队列中的优先级。
	Initialize()	初始化算法，设置 g 和 rhs 的初始值（通常为无穷大），并将目标节点的 rhs 设为0后加入队列。
	UpdateVertex(u)	当节点 u 的 rhs 值或其邻居信息发生变化时，更新其 rhs 值及其在队列中的状态。
	ComputeShortestPath()	核心计算过程，持续扩展节点直到起始节点达到一致且队列顶部的键值不小于起始节点的键值。

在D* Lite中，前驱（Pred）和后继（Succ） 指的是图的邻接关系。Succ(u) 是所有从 u 出发有边直接到达的节点（即 u 的后继），而 Pred(u) 是所有有边直接指向 u 的节点（即 u 的前驱）。由于算法是从目标点向起始点反向搜索 的，所以在代码中我们通常操作的是节点的前驱集合 Pred。

算法实现步骤与C++代码框架

以下是一个简化的D* Lite算法C++实现框架，基于网格地图环境。实际的实现会更复杂，但这能帮你理清主线逻辑。

#include <vector>
#include <queue>
#include <unordered_map>
#include <limits>

// 定义节点类型，例如用网格坐标表示
struct Node {
    int x, y;
    // 重载运算符以便用于比较或作为哈希键值
    bool operator==(const Node& other) const {
        return x == other.x && y == other.y;
    }
};

// 为Node特化std::hash
namespace std {
    template<> struct hash<Node> {
        size_t operator()(const Node& n) const {
            return hash<int>()(n.x) ^ (hash<int>()(n.y) << 1);
        }
    };
}

class DStarLite {
private:
    std::unordered_map<Node, double> g_; // g值表
    std::unordered_map<Node, double> rhs_; // rhs值表
    // 优先队列：元素为 (键值k1, 键值k2, 节点)
    using QueueElement = std::tuple<double, double, Node>;
    std::priority_queue<QueueElement, std::vector<QueueElement>, std::greater<QueueElement>> U_;
    double km_; // 用于键值计算的偏移量
    Node s_start_, s_goal_, s_last_;
    // ... 其他成员变量，如地图信息 ...

    // 计算启发式函数h(s, s')
    double heuristic(const Node& a, const Node& b) {
        // 例如，使用曼哈顿距离或欧几里得距离
        return std::abs(a.x - b.x) + std::abs(a.y - b.y);
    }

    // 计算键值
    std::pair<double, double> CalculateKey(const Node& s) {
        double g_val = g_.count(s) ? g_[s] : std::numeric_limits<double>::infinity();
        double rhs_val = rhs_.count(s) ? rhs_[s] : std::numeric_limits<double>::infinity();
        double k1 = std::min(g_val, rhs_val) + heuristic(s_start_, s) + km_;
        double k2 = std::min(g_val, rhs_val);
        return {k1, k2};
    }

    // 初始化算法
    void Initialize() {
        U_ = std::priority_queue<QueueElement, std::vector<QueueElement>, std::greater<QueueElement>>();
        km_ = 0.0;
        // 初始化所有节点的g和rhs为无穷大
        g_.clear();
        rhs_.clear();
        // 设置目标节点的rhs为0
        rhs_[s_goal_] = 0.0;
        U_.push({CalculateKey(s_goal_), s_goal_});
    }

    // 更新顶点u
    void UpdateVertex(const Node& u) {
        if (u == s_goal_) {
            // 目标节点特殊处理，通常其rhs保持为0
            return;
        }
        // 获取u的所有前驱节点 Pred(u) （在反向搜索中，这些是原图中u的后继）
        std::vector<Node> predecessors = GetPredecessors(u); // 需要你根据图的结构实现此函数
        // 计算新的rhs值：取所有 (c(u, s') + g(s')) 的最小值，其中s'属于Pred(u)
        double min_rhs = std::numeric_limits<double>::infinity();
        for (const Node& pred : predecessors) {
            double cost = GetCost(u, pred); // 获取边(u, pred)的成本，需要实现。注意方向：在反向搜索中，我们关心从u到pred的成本（原图中是pred到u的成本）。
            double g_pred = g_.count(pred) ? g_[pred] : std::numeric_limits<double>::infinity();
            if (cost + g_pred < min_rhs) {
                min_rhs = cost + g_pred;
            }
        }
        rhs_[u] = min_rhs;

        // 如果u在队列中，先移除
        // （在实际实现中，可能需要一个标记或更复杂的队列管理来高效判断和移除）
        // 这里简化为先尝试移除（如果存在），然后再判断是否需要插入
        // 更高效的实现可能需要维护一个单独的队列元素存在性标记

        // 如果g(u) != rhs(u)，则将u以其CalculateKey插入或更新到队列U中
        double g_u = g_.count(u) ? g_[u] : std::numeric_limits<double>::infinity();
        if (g_u != rhs_[u]) {
            // 这里需要实现U_.update或先删除再插入的逻辑。简单起见，可以先删除再插入，但效率较低。
            // 假设我们的队列不支持直接update，我们这里先简单推入新键值，在ComputeShortestPath中处理重复节点。
            auto key = CalculateKey(u);
            U_.push({key.first, key.second, u});
        } else {
            // 如果一致，且u在队列中，则应移除。这里简化处理，依赖ComputeShortestPath中处理无效节点。
        }
    }

    // 计算最短路径
    void ComputeShortestPath() {
        while (!U_.empty()) {
            auto [k1_old, k2_old, u] = U_.top();
            U_.pop();
            auto [k1_new, k2_new] = CalculateKey(u);
            // 如果旧的键值小于新的键值，说明节点需要重新以新键值加入队列
            if (k1_old < k1_new || (k1_old == k1_new && k2_old < k2_new)) {
                U_.push({k1_new, k2_new, u});
            }
            // 如果节点u是过一致的 (g(u) > rhs(u))
            else if ((g_.count(u) ? g_[u] : INFINITY) > (rhs_.count(u) ? rhs_[u] : INFINITY)) {
                g_[u] = rhs_[u]; // 使节点一致
                // 更新u的所有前驱节点（在反向搜索中，这些是原图中u的后继）
                std::vector<Node> predecessors = GetPredecessors(u);
                for (const Node& pred : predecessors) {
                    UpdateVertex(pred);
                }
            }
            // 如果节点u是欠一致的 (g(u) < rhs(u))
            else {
                g_[u] = std::numeric_limits<double>::infinity(); // 将g(u)设为无穷大，使其变为过一致或未定义
                // 需要更新u本身及其所有前驱节点
                UpdateVertex(u); // 更新自身
                std::vector<Node> predecessors = GetPredecessors(u);
                for (const Node& pred : predecessors) {
                    UpdateVertex(pred);
                }
            }
            // 循环终止条件：起始节点一致且队列顶部的键值不小于起始节点的键值
            double g_start = g_.count(s_start_) ? g_[s_start_] : std::numeric_limits<double>::infinity();
            double rhs_start = rhs_.count(s_start_) ? rhs_[s_start_] : std::numeric_limits<double>::infinity();
            if (g_start == rhs_start && (U_.empty() || CalculateKey(U_.top().s) >= CalculateKey(s_start_))) {
                break;
            }
        }
    }

public:
    // 主循环
    void Main() {
        s_last_ = s_start_;
        Initialize();
        ComputeShortestPath();
        while (s_start_ != s_goal_) {
            // 如果g(s_start)是无穷大，说明无路径
            if (g_.count(s_start_) == 0 || g_[s_start_] == std::numeric_limits<double>::infinity()) {
                // 处理无路径情况
                break;
            }
            // 选择下一个移动点：argmin_{s' in Succ(s_start)} (c(s_start, s') + g(s'))
            std::vector<Node> successors = GetSuccessors(s_start_); // 获取s_start在原图中的后继节点
            double min_cost = std::numeric_limits<double>::infinity();
            Node next_node = s_start_;
            for (const Node& succ : successors) {
                double cost = GetCost(s_start_, succ); // 获取边(s_start, succ)的成本
                double g_succ = g_.count(succ) ? g_[succ] : std::numeric_limits<double>::infinity();
                if (cost + g_succ < min_cost) {
                    min_cost = cost + g_succ;
                    next_node = succ;
                }
            }
            s_start_ = next_node; // 移动到下一个节点
            // 移动后，扫描地图变化（在实际应用中，这里需要你根据传感器信息更新边成本）
            // 例如：如果检测到边(u, v)的成本发生变化
            // km_ = km_ + heuristic(s_last_, s_start_);
            // s_last_ = s_start_;
            // for each changed edge (u, v): 
            //   Update the edge cost c(u, v)
            //   UpdateVertex(u)
            // ComputeShortestPath(); // 重新规划
        }
    }

    // 需要你实现的辅助函数：
    std::vector<Node> GetPredecessors(const Node& u); // 在反向搜索中，获取节点u的前驱（即原图结构中的后继）
    std::vector<Node> GetSuccessors(const Node& u);   // 获取节点u在原图结构中的后继
    double GetCost(const Node& from, const Node& to); // 获取两个相邻节点之间的移动成本
};

实现注意事项与常见问题

1. 图的表示与邻居获取 ：上述代码中的 GetPredecessors、GetSuccessors 和 GetCost 函数需要你根据具体的图结构（如网格地图）来实现。在网格中，一个节点的邻居通常是其上下左右（或包括对角线）的相邻格子。
2. 优先队列的高效管理 ：标准库的 priority_queue 不支持直接更新元素的值。一个常见的优化是使用"惰性删除"策略：当从队列顶部弹出节点时，检查其键值是否最新（即其 g 或 rhs 值自该元素入队后是否未改变），如果已过时则直接忽略。你也可以考虑使用支持 decrease-key 操作的堆结构。
3. 处理动态变化 ：当检测到边成本变化时（例如，在 Main 函数的注释部分），需要更新受影响节点的 rhs 值并调用 UpdateVertex。关键在于只更新成本实际发生变化的边所关联的节点。在机器人路径规划中，通常机器人只感知局部环境的变化。
4. 避免循环 ：不正确的 UpdateVertex 逻辑或键值计算可能导致算法在两个节点间无限循环。确保在节点变为一致（g(u) == rhs(u)）时将其从队列中移除（或在键值计算中体现其一致性），并正确更新受影响的邻居。
5. 启发式函数的选择 ：启发式函数 h(s_start, s) 应满足可容性（admissible，即不高估真实成本）以保证最优性。在网格中，曼哈顿距离或欧几里得距离是常见选择。

参考代码 D*lite算法的C++实现 www.3dddown.com/csa/60495.html

实现 D* Lite 算法确实有一定挑战性，建议从简单的静态环境开始，逐步增加动态障碍物功能，并善加调试。