D*算法实现路径规划

前言

D*（D-Star）算法是一种用于动态环境下的路径规划算法，它可以在环境变化时快速重新规划路径。D*算法的核心思想是利用局部更新策略来重新计算路径，而不是每次都从头开始计算。

逐步实现D*算法的流程： 

一. 初始化

• 创建一个图表示环境，其中节点表示位置，边表示位置间的移动以及相应的成本。
• 初始化一个空的开放列表（open list）和关闭列表（closed list）。

二. 构建初始路径

• 使用A*算法从起点到终点计算一条初始路径。

三. 监控环境变化

• 观察环境中的变化，如障碍物的添加或移除。

四. 局部更新

• 当环境发生变化时，识别受影响的节点集合。这通常包括变化点附近的节点。
• 将受影响的节点添加到开放列表中。

五. D*算法主循环

• 从开放列表中取出最近的节点。
• 遍历该节点的所有邻居，计算通过当前节点到达邻居的代价。
• 如果通过当前节点的代价更低，更新邻居的代价，并将其加入开放列表。

六. 路径重新规划

• 重复步骤5，直到开放列表为空，表示所有受影响的节点都已更新。

八. 路径检索

• 从终点开始，沿着代价最低的路径回溯到起点，构建新的路径。

九.测试代码

#include <iostream>
#include <vector>
#include <queue>
#include <map>
#include <algorithm>
#include <cmath>

using namespace std;

// 定义图中的节点
struct Node {
    int x, y;
    double g; // 从起点到该点的代价
    double h; // 该点到终点的启发式代价
    double f() const { return g + h; } // 总代价
};

// 定义图中的边
struct Edge {
    Node* to;
    double cost;
};

// 优先队列比较函数，用于D*算法中的open列表
struct Compare {
    bool operator()(Node* a, Node* b) {
        return a->f() > b->f();
    }
};

// D*算法类
class DStar {
private:
    map<int, Node> nodes; // 存储所有节点
    vector<Edge> edges; // 存储所有边
    priority_queue<Node*, vector<Node*>, Compare> openList; // open列表
    map<Node*, double> tentativeGValues; // 临时g值

    // 启发式函数，例如使用曼哈顿距离
    double heuristic(const Node& a, const Node& b) {
        return abs(a.x - b.x) + abs(a.y - b.y);
    }

    // 检查节点是否在open列表中
    bool isInOpenList(Node* node) {
        for (auto it = openList.begin(); it != openList.end(); ++it) {
            if (*it == node) return true;
        }
        return false;
    }

public:
    // 添加节点
    void addNode(int id, int x, int y) {
        nodes[id] = Node{x, y};
    }

    // 添加边
    void addEdge(int fromId, int toId, double cost) {
        edges.push_back(Edge{&nodes[toId], cost});
    }

    // D*算法主函数
    void computePath(int startId, int goalId) {
        Node* start = &nodes[startId];
        Node* goal = &nodes[goalId];

        // 初始化
        start->g = 0;
        tentativeGValues[start] = 0;
        openList.push(start);

        while (!openList.empty()) {
            Node* current = openList.top();
            openList.pop();

            if (current == goal) {
                // 达到终点，路径计算完成
                break;
            }

            // 对当前节点的所有邻居进行更新
            for (auto& edge : edges) {
                if (edge.to == current) {
                    double tentativeGValue = tentativeGValues[current] + edge.cost;
                    if (tentativeGValue < tentativeGValues[edge.to] || 
                        !isInOpenList(edge.to)) {
                        tentativeGValues[edge.to] = tentativeGValue;
                        openList.push(edge.to);
                    }
                }
            }
        }

        // 打印路径（这里仅展示如何访问open列表）
        cout << "Open List contains: ";
        for (Node* node : openList) {
            cout << "(" << node->x << ", " << node->y << ") ";
        }
        cout << endl;
    }
};

int main() {
    DStar dstar;

    // 添加节点
    dstar.addNode(0, 0, 0);
    dstar.addNode(1, 1, 0);
    dstar.addNode(2, 0, 1);
    dstar.addNode(3, 1, 1);

    // 添加边
    dstar.addEdge(0, 1, 1.0);
    dstar.addEdge(1, 2, 1.0);
    dstar.addEdge(2, 3, 1.0);
    dstar.addEdge(0, 2, 1.4); // 斜向移动的代价
    dstar.addEdge(1, 3, 1.4);

    // 计算从节点0到节点3的路径
    dstar.computePath(0, 3);

    return 0;
}