基于D* Lite算法的C++实现

基于D* Lite算法的C++实现

---

一、核心数据结构定义

#include <vector>
#include <queue>
#include <unordered_map>
#include <cmath>

struct Node {
    int x, y;          // 节点坐标
    double g, rhs;     // 实际代价与启发式估计
    double key[2];     // 优先级队列键值 [k1, k2]
    
    Node(int x=0, int y=0) : x(x), y(y), g(INF), rhs(INF) {
        key[0] = key[1] = INF;
    }
    
    bool operator>(const Node& other) const {
        return key[0] > other.key[0] || 
               (key[0] == other.key[0] && key[1] > other.key[1]);
    }
};

using PriorityQueue = std::priority_queue<Node, std::vector<Node>, std::greater<Node>>;

---

二、核心算法实现

1. 初始化函数

void DStarLite::init(int startX, int startY, int goalX, int goalY) {
    // 重置所有节点状态
    for(auto& [pos, node] : nodeMap) {
        node.g = node.rhs = INF;
    }
    
    // 设置终点
    Node goal(goalX, goalY);
    goal.rhs = 0;
    nodeMap[goal] = goal;
    
    // 计算初始键值
    calculateKey(goal);
    openList.push(goal);
}

2. 键值计算

void DStarLite::calculateKey(const Node& node) {
    double minVal = std::min(node.g, node.rhs);
    key[0] = minVal + heuristic(node, startPoint) + km;
    key[1] = minVal;
}

3. 启发式函数（曼哈顿距离）

double DStarLite::heuristic(const Node& a, const Node& b) {
    return std::abs(a.x - b.x) + std::abs(a.y - b.y);
}

4. 节点更新核心逻辑

void DStarLite::updateVertex(Node& u) {
    if(u != goal) {
        double minRhs = INF;
        for(auto& [v, cost] : getSuccessors(u)) {
            minRhs = std::min(minRhs, nodeMap[v].g + cost);
        }
        if(!isEqual(minRhs, u.rhs)) {
            u.rhs = minRhs;
            for(auto& pred : getPredecessors(u)) {
                updateVertex(pred);
            }
        }
    }
    
    if(!isEqual(u.g, u.rhs)) {
        openList.push(u);
    }
}

---

三、动态环境处理

1. 障碍物更新

void DStarLite::updateObstacle(int x, int y, double newCost) {
    Node& node = nodeMap[{x, y}];
    double oldCost = node.g;
    node.g = newCost;
    
    if(!isEqual(oldCost, newCost)) {
        km += heuristic(currentPos, node);
        updateVertex(node);
    }
}

2. 路径重规划

std::vector<Node> DStarLite::replan() {
    while(!openList.empty()) {
        Node u = openList.top();
        openList.pop();
        
        if(u.g > u.rhs) {
            u.g = u.rhs;
            for(auto& pred : getPredecessors(u)) {
                updateVertex(pred);
            }
        } else {
            u.g = INF;
            for(auto& pred : getPredecessors(u)) {
                updateVertex(pred);
            }
            updateVertex(u);
        }
    }
    
    return extractPath();
}

---

四、辅助函数实现

1. 前驱/后继节点获取

std::vector<Node> DStarLite::getSuccessors(const Node& node) {
    std::vector<Node> successors;
    std::vector<std::pair<int, int>> dirs = {{0,1},{1,0},{0,-1},{-1,0}};
    
    for(auto& dir : dirs) {
        int nx = node.x + dir.first;
        int ny = node.y + dir.second;
        if(nodeMap.count({nx, ny})) {
            successors.push_back(nodeMap[{nx, ny}]);
        }
    }
    return successors;
}

2. 路径提取

std::vector<Node> DStarLite::extractPath() {
    std::vector<Node> path;
    Node current = startPoint;
    
    while(current != goal) {
        path.push_back(current);
        auto& successors = getSuccessors(current);
        Node next = *std::min_element(successors.begin(), successors.end(),
            [this](auto& a, auto& b) { return nodeMap[a].g < nodeMap[b].g; });
        current = next;
    }
    path.push_back(goal);
    return path;
}

---

五、测试用例示例

int main() {
    DStarLite planner;
    planner.init(0, 0, 10, 10);
    
    // 设置初始障碍物
    planner.updateObstacle(5, 5, 100);
    
    auto path = planner.replan();
    
    // 动态障碍物更新
    planner.updateObstacle(3, 3, 200);
    auto newPath = planner.replan();
    
    return 0;
}

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

六、关键参数说明

参数	默认值	说明
km	0	累计调整量
INF	1e9	无穷大值
启发式权重	1.0	可调整以平衡速度与精度

---

七、扩展功能实现

1. 多目标点规划

void DStarLite::addGoal(int x, int y) {
    Node newGoal(x, y);
    newGoal.rhs = 0;
    nodeMap[newGoal] = newGoal;
    calculateKey(newGoal);
    openList.push(newGoal);
}

2. 实时避障

void DStarLite::dynamicReplan(const std::vector<Node>& obstacles) {
    for(auto& obs : obstacles) {
        updateObstacle(obs.x, obs.y, 100);
    }
    currentPos = getClosestReachable(currentPos);
    replan();
}

---

八、调试建议

1. 可视化调试：使用SFML或OpenCV绘制路径和障碍物

2. 日志输出：记录每次更新的节点和代价变化

3. 边界检查：添加坐标越界保护

   if(x < 0 || x >= width || y < 0 || y >= height) return;
   

该实现完整覆盖了D* Lite算法的核心逻辑，支持动态障碍物更新和实时路径重规划。