（6-2）D*算法：D*算法介绍

6.2  D*算法介绍

D*算法（D-star algorithm）是一种路径规划算法，用于在有向图中寻找从起点到目标点的最短路径。D*是基于A*算法的改进，适用于动态环境下的路径规划问题，其中环境可能会随时间变化而改变。D*算法最初由Anthony Stentz于1994年提出，用于机器人路径规划。

6.2.1  D*算法的背景和发展历程

D*算法的背景和发展历程可以追溯到路径规划领域的早期研究和算法发展，以下是对D*算法的主要背景和发展历程的具体说明。

1. 路径规划领域的早期研究：路径规划是人工智能和机器人领域的重要问题之一，早期研究主要集中在静态环境下的路径规划。一些经典算法如 Dijkstra 算法被提出来解决这一问题。
2. A*算法的提出：A*算法是一种启发式搜索算法，由Peter Hart、Nils Nilsson 和Bertram Raphael于1968年提出。它通过估计每个节点到目标节点的代价来导航搜索，以在有向图中找到最短路径。
3. D*算法的诞生：D*算法是在A*算法的基础上发展而来的，旨在解决动态环境下的路径规划问题。Anthony Stentz 在1994年提出了D*算法，作为机器人路径规划的一种新方法。D*算法的主要目标是适应环境的动态变化，能够在环境变化时快速更新路径。
4. 实际应用和改进：D*算法的提出引发了许多实际应用和改进研究。人们开始将D*算法应用于机器人导航、自动驾驶、无人机路径规划等领域。此外，还出现了许多改进版本和变体，如 D* Lite 算法等，以进一步提高算法的效率和性能。
5. 应用领域的拓展：随着技术的进步和对路径规划需求的不断增加，D*算法开始在更多领域得到应用，如游戏开发、智能交通系统、网络路由优化等。

总的来说，D*算法作为一种适应动态环境的路径规划算法，经过多年的发展和实践应用，在人工智能、机器人技术等领域发挥着重要作用，并不断得到改进和拓展。

6.2.2  D*算法的原理和实现步骤

D*算法是一种用于动态环境下路径规划的算法，它可以在环境变化时快速更新路径。

1. D*算法的原理

1. 局部搜索：D*算法从目标节点开始向起始节点进行局部搜索，以找到当前最优路径。
2. 路径更新：算法通过将节点的代价向起始节点进行传播，逐步更新路径的代价。
3. 环境变化处理：如果环境发生变化，如障碍物移动或代价改变，D*算法能够快速根据变化重新规划路径。

2. D*算法的实现步骤

（1）初始化：设定起点和终点，并将终点的代价设为0，其他节点的代价设为无穷大。同时，将所有节点标记为未访问状态。

（2）更新代价：从终点开始，按照一定的启发式规则（如欧几里得距离）向起点搜索路径，并根据当前节点邻居节点的代价更新路径代价。具体来说，算法会更新节点的 $g$ 值，表示从起点到当前节点的实际代价。

（3）路径追溯：沿着更新后的路径，从终点向起点追溯，更新路径上每个节点的邻居节点的代价。这一步骤保证了路径的连续性。

（4）环境变化检测：在每次迭代中，算法会检测环境是否发生变化，如节点代价的改变或者障碍物的移动。

（5）处理环境变化：如果环境发生变化，算法将重新计算受影响的节点的代价，并更新路径。

（6）重复迭代：重复前面的步骤（2）至步骤（5），直到找到从起点到终点的最短路径或者达到预设的迭代次数。

（7）路径提取：最终，根据更新后的路径信息提取出从起点到终点的最短路径。

需要注意的是，D*算法是一种增量式的算法，它允许在动态环境中实时更新路径而无需重新计算整个路径。这使得D算法在实际应用中具有很高的效率和实用性。

6.2.3  D *算法总体流程及与A*算法的对比

同A*算法类似，D*算法通过维护一个优先队列（OpenList）来对场景中的路径节点进行搜索，所不同的是，D*不是由起始点开始搜索，而是以目标点为起始，通过将目标点置于Openlist中来开始搜索，直到机器人当前位置节点由队列中出队为止（当然如果中间某节点状态有动态改变，需要重新寻路，所以才是一个动态寻路算法）。

启发式搜索是利用启发函数来对搜索进行指导，从而实现高效的搜索。增量搜索是对以前的搜索结果信息进行再利用来实现高效搜索，大大减少搜索范围和时间。

D*算法采用反向搜索的目的在于后期需要重新规划路径的时候，能够用到先前搜索到的最短路径信息，减少搜索量。因为以目标向起始点进行搜索得到的最短路径图，是以目标点为中心辐射出的最短路径图，图上目标点到各点之间都是最短路径，为此其在既定路径上遇到问题需要重新路径规划的时候，可以很好的利用原先得到的信息。而以起始点向目标点搜索得到的最短路径图，其是以起始点为中心辐射出的最短路径图，当沿着既定路径前行遇到障碍物之后，需要重新进行路径规划之时，没有办法很好的利用原先搜索得到的信息。

总的来说，D * 算法分为两个阶段，第一个阶段是使用Dijkstra算法或A *算法找到从目标点到起始点的静态可行路径，然后机器人开始从起点向目标点运动。第二个阶段是动态避障搜索阶段，主要用于修正若干个受障碍物影响而导致代价值发生变化的那些节点信息。

（1）初始化：将目标点放入OpenList 中。这一过程与A * 的对比，在A*算法初始化时将起始点放到OpenList 中。

（2）从OpenList 中找到k值最小的节点，并将该节点从OpenList 中移除，并放到closedlist列表中。

在这一过程与A * 的对比：A*算法选择拓展点时是从OpenList 列表中选择f值最小的点拓展（f=g+h），而D * 算法是从OpenList 列表中选择k值最小的点拓展。

在D * 算法中是从目标点向起始点进行搜索，我们将从目标点到当前点的代价记为h （其实从原理上来看与这里的h与A * 中 的g更为相似，都是从搜索开始的点到当前点累计的代价值），在D * 中没有使用从当前点到起始点的估计值，也就是没有使用启发信息，从这一点上来看，D * 与dijkstra算法更为相似。

那么D *中的 k值是什么呢？ D * 是针对动态环境设计的算法，由于环境的改变某点处的h值可能发生改变，而某点处的k值其实记录的就是该点的最小h值，也就是说，对于还未遍历到的点，k=h=inf;对于标识为open或closed的点，k=min { k , h_new}。

注意：为什么不选用最小的h值节点来作为拓展点，而是使用k值呢？

在动态环境下，假如某个节点变成了障碍物，此时其h值会被修改为inf，我们需要将这种变化传递下去，若采用h值作为选取标准，该节点会被置于openList中的最后。也就是说，此时路径规划会从openList中剩余的处于OPEN状态的节点开始，一直扩张至全图都没有不可达节点之后，才会访问该节点。这显然并不合理，因为我们的目的就是要在节点状态动态变化的时候减少搜索的空间，提高搜索效率。 而用最小的h值也就是k值在openList中进行排序，表示这里曾有一条捷径，那么就会优先在这附近进行搜索。

（3）判断当前拓展点的k值是否与该点的h值相同，若k h(y) + c(y ,x),则将当前点x的父节点改为y，并将该点的h值更改为h（x）= h(y) + c(y ,x)，其中c(y ,x)代表从y点到x点的代价值。我们试图用上面的操作来减少该点处的h值，并期望使其回到Lower态。

在这一过程与A * 的对比：A * 算法中不考虑动态障碍物，从而A*算法中并没有与该步骤对应的操作，在D * 算法中每个节点还有Lower态和Raise态两种状态，若h=k，记为Lower态；若h>k,记为Raise态，当该节点处于Raise态时表明有更优的路径。

（4）判断经过第（3）步的操作后，该点的k值是否与h值相等，即是否回到了Lower态，若不等，即k < h，则按照以下准则进行节点的拓展：

①该相邻节点从未被拓展过，即该相邻节点还未进OpenList。

②该相邻节点的父节点是当前点，且h（y）≠ h （x）+ c (x,y), 若该相邻节点y是以上两种情况，则将该相邻节点的父节点设为当前点，并将该相邻节点的h值设为h （x）+ c (x,y)，放到OpenList中。

③若该相邻节点的父节点不是当前点，且h（y）> h （x）+ c (x,y),则将当前点x加入到OpenList，这里为什么不将y的父节点设定为当前点x，并对其h（y）进行更新呢？，往OpenList里面放到为什么不是y，而是x呢？我们可以这样理解，此时的当前点x处于Raise态，即当前x处的代价值不是最优的，所以，我们将x放到OpenList中，等待下一次循环，当前点的代价值变成最优的后，再对该相邻节点进行处理，以此来保证传递下去的代价值是最小的。

④若该相邻节点的父节点不是当前点，且h（x）> h （y）+ c (x,y),且该相邻点y在closedlist中，且h（y）> k,则将y重新放到OpenList中，怎么理解呢？ 我们每次从OpenList取点时，取得是k最小的，既然y已经在closedlist中，说明y比当前点先拓展，即k（y）<= k(x)，而现在h（y）> k（x），说明y受到了障碍物的影响，使得h（y）变大了，因此需要将y放到OpenList进行考察。

（5）进行下一轮拓展，依次循环往复，拓展结束后通过路径回溯找到所需的路径。

（6）动态阶段：机器人按照规划的路径进行移动的过程中，若当前点为x，且探测到要走的下一个节点y存在障碍，或者节点y本来存在障碍物，但是继续行进到x的时候，障碍物被移除。这时就要调用modify_cost(x,y,val)函数，将最新的cost(x,y)以及h(y)的值进行变更，若此时x已经位于closedlist，则需要将其重新放到openlist中，并反复执行process_state()函数用于传播节点h值变更信息和路径搜索直到process_state()函数返回的openList中所有节点的k值中的最小k值(k_{min}) >= h(x) 或者openList没有任何节点时，表示重新路径规划完成，或者无法找到别的路径从x点规划到目标点G。

注意：为何重新进行路径规划的时候，当执行到 (k_{min}) >= h(y) 时停止？

这是因为process_state()函数的功能是用邻居节点来减低自身节点的h值的，当所有处于OPEN状态的节点中的k值的最小值 (k_{min}) 也要大于等于h(y)时，表示不可能再通过process_state()函数的执行来降低h(y)值了，那么自然就没有再搜索的必要，且已经完成了路径的修正。

相比A-star算法，D-star的主要特点就是由目标位置开始向起始位置进行路径搜索，当物体由起始位置向目标位置运行过程中，发现路径中存在新的障碍时，新的障碍不行影响目标位置到新障碍之间的范围内的路径节点，新障碍只会影响机器人所在位置（当前位置）到障碍之间范围的节点的路径。在这时通过将新的障碍周围的节点加入到Openlist中进行处理然后向物体所在位置进行传播，能最小程度的减少计算开销。 路径搜索的过程中跟Dijkstra算法比较像，并没有体现A-star所具有的方向感，在拓展时没有将当前拓展点与起始点之间的代价估计值作为选取拓展点的考量之一，即没有朝着向起点搜索的引导，这种搜索更多的是一种由目标位置向四周发散搜索，直到把起始位置纳入搜索范围为止。

6.2.4  D*算法的常用概念及函数

在下面 的内容中，列出了D*算法的常用概念及函数的信息。

1. G：表示进行路径搜索的目标点。
2. c(x,y)：从节点x移动到节点y的代价。
3. t(x)：节点的状态。每个节点(作者论文中称为state)都有一个状态，其中总共有三种可能NEW,OPEN,CLOSED。NEW表示从未加入到openList中的，也就是从未被遍历查找过的。OPEN表示节点在被查找中，节点在openList中。CLOSED表示从openList中被移出。OPEN和CLOSED状态会相互转化，当节点从openList中移出的时候，状态从OPEN变为CLOSED，当节点再次加入openList，进行 降低节点自身h值或者传播当前节点的h值变更信息给邻居节点，让邻居节点进行h值修改变更 操作时，状态从CLOSED变为OPEN。
4. h(x)：表示地图上的点x到达目标点G的代价。由于D*算法是从目标点开始进行路径规划的，为此，初始化的时候，令h(G) = 0。此后，其代价的变动可能会在两个地方出现，一个是路径搜索的过程中，当节点x的邻居节点被执行搜索过程的时候，如果其能够让h(x)的代价更小，则更新h(x) = h(y) + cost(y,x)。一个是在路径搜索完成后执行路径搜索结果的过程中遇到障碍物之时，通过insert(x,y,val)函数改变障碍物节点y的代价h(y)。
5. k(x)：节点最小的h(x)值。随着节点遍历和搜索过程的进行，节点的h(x)值会不断的变动。
6. b(x) = y：用于记录当前节点x的父节点，这里b(x) = y表示x节点的父节点为y节点。在搜索完成之后，能够根据b(x)的值，回溯追踪到目标节点。
7. openList：存放节点状态为OPEN的节点，且其按照节点的k值从小到大进行排序。
8. process_state()：该函数用于降低openlist表中的某个节点x(state)的h(x)值或者传播节点x的h(x)值变更信息给邻居节点，让邻居节点进行相应变更的同时进行路径搜索查找的。
9. insert(x,val)：该函数用于修改节点x的状态以及h(x)值和k(x)值的。
10. modify_cost(x,y,val)：该函数用于修改节点x和y之间的移动代价cost(x,y)，而且根据节点y的状态t(y)的情况，可能对节点y的h(y)值和状态t(y)进行修改。

注意：本节部分内容参考自：https://blog.csdn.net/qq_44339029/article/details/127490956。

6.2.5  D*算法实战：帮助探险家寻找最短路径

假设你是一位勇敢的探险家，正在探索一片神秘的古老遗迹。你发现了一张古老的地图，但上面的路线已经模糊不清了，而且地图上标记了一些障碍物。现在，请你编写的神奇探索算法，即 D* 算法，来帮助你找到通往古老遗迹的最短路径吧。

1. 起点：(3, 2)。
2. 终点：(23, 16)。
3. 障碍物：{(12, 4), (12, 5), (12, 6), (12, 7), (12, 8), (12, 9), (12, 10), (12, 11), (11, 7), (13, 3), (10, 6), (14, 5), (6, 2)}。

实例6-1：帮助探险家寻找最短路径（codes/6/Dstar/）

1. 实现D*算法

编写实例文件Dstar.h，用于实现D*算法的核心功能，用于在给定地图上找到从起点到目标点的最短路径，并考虑了障碍物的影响。通过类DStar封装了算法的实现，包括路径搜索的各个步骤。具体实现流程如下所示。

（1）实现结构体State，这是一个枚举类型，用于表示节点的状态，包括三种可能的状态：New、Open 和 Closed。在路径搜索的过程中，节点会根据其状态进行不同的处理。具体说明如下所示：

1. New 状态表示节点是新发现的，尚未加入到开放列表中。
2. Open 状态表示节点已经加入到开放列表中，但尚未被处理。
3. Closed 状态表示节点已经被处理过，不再需要再次考虑。

上述状态在算法的执行过程中起到了重要的作用，帮助算法进行节点的管理和处理。

#pragma once
#include#include#include#include
#includenamespace DStar {
    enum State
    {
        New,
        Open,
        Closed
    };

（2）定义结构体 DNode，用于表示地图中的节点，包括节点的坐标、状态（New、Open、Closed）、距离目标的估计距离（H值）、以及路径中的下一个节点等信息。

 struct DNode {
        explicit DNode(int X, int Y, DNode* Next = nullptr) : x(X), y(Y), next(Next) {}
        DNode() = default;
        void setObstacle()
        {
            is_obstacle = true;
        }
        int x = 0;
        int y = 0;
        int H = 0; // distance to target
        int K = 0; //所有时刻最小的H！
        State state = New;
        DNode* next = nullptr;
        bool is_obstacle = false;
    };

（3）实现结构体MyCompare，用于定义自定义的比较函数，它在优先队列中用于比较两个节点的优先级。在这个算法中，优先队列用于存储开放列表中的节点，并根据其优先级（K 值）进行排序。结构体 MyCompare 重载了圆括号运算符，使得它可以像函数一样被调用，用于比较两个节点的优先级。在这里，比较的标准是比较两个节点的 K 值，K 值越小，表示节点的优先级越高。因此，MyCompare 结构体中的 operator() 函数实现了按照 K 值从小到大的顺序对节点进行比较。

 struct MyCompare
    {
        bool operator()(DNode* lhs, DNode* rhs) const {
            return lhs->K > rhs->K;
        }
    };

（4）实现类DStar，这是在本项目中实现 D* 算法的核心部分。其中构造函数接受一个表示地图的二维向量 map，并初始化算法所需的数据结构。在类DStar中包含如下所示的私有成员变量：

1. _cost_low、_cost_high、_cost_max：代表不同操作的代价。
2. openlist：优先队列，用于存储开放列表中的节点，并根据节点的 K 值（最小路径估计）进行排序。
3. Map：表示地图的二维向量。
4. shortestPaths：存储每个节点到终点的最短路径信息。

通过这些功能，类DStar实现了 D* 算法的关键步骤，包括路径搜索、路径更新和处理障碍物等。

 class DStar {
    public:
        DStar(const std::vector> map) : Map(map), shortestPaths(map.size(), std::vector(map.front().size(), nullptr)) {}
        void find_first_path(const std::pair& start, const std::pair& target, const std::vector>& obstacles);
        ~DStar()
        {
            for (int i = 0; i < shortestPaths.size();++i)
            {
                for (int j = 0; j < shortestPaths.front().size();++j)
                {
                    if (shortestPaths[i][j])
                        delete shortestPaths[i][j];
                }
            }
        }
    private:
        int cost(DNode* lhs, DNode* rhs) const;
        void insert(DNode* node, int Cost);
        //核心函数
        int process_state();
        int modify(DNode* node);
        void setObstacles(const std::vector>& obstacles);
    private:
        const static int _cost_low = 10;
        const static int _cost_high = 14;
        const static int _cost_max = 65536;
        std::priority_queue, MyCompare> openlist;
        std::vector> Map;
        std::vector> shortestPaths; //保存每个节点到终点的最短路径信息
    };

（5）DStar::cost(DNode* lhs, DNode* rhs) 是一个私有方法，用于计算两个节点之间的代价。这个方法接受两个节点作为参数，并返回它们之间的代价值。在 D* 算法中，节点之间的代价通常表示为移动到相邻节点所需的成本。这个成本可能因为障碍物的存在而不同。在这个方法中，如果其中一个节点是障碍物，或者两个节点不在同一行或同一列，那么返回的代价将设置为一个很大的值 _cost_max，表示移动是不可行的。具体来说，如果节点 lhs 或节点 rhs 中有一个是障碍物（is_obstacle 为 true），则返回 _cost_max。如果两个节点不在同一行或同一列，则返回 _cost_high，表示斜向移动的成本较高；否则返回 _cost_low，表示直线移动的成本较低。

 int DStar::cost(DNode* lhs, DNode* rhs) const
    {
        if (lhs->is_obstacle || rhs->is_obstacle)
            return _cost_max;
        if (lhs->x != rhs->x && lhs->y != rhs->y)
            return _cost_high;
        else
            return _cost_low;
    }

（6）DStar::insert(DNode* node, int new_H) 是一个私有方法，用于将节点插入到算法中的开放列表 openlist 中，并更新节点的代价信息。该方法接受两个参数：要插入的节点 node 和新的代价值 new_H。节点 node 表示要插入的节点，而 new_H 表示新的代价值。方法DStar::insert(DNode* node, int new_H)的实现流程如下所示。

1. 首先检查节点的状态。如果节点是新节点（New 状态），则将其初始化为开放状态（Open），并设置其代价信息为 new_H。如果节点已经是开放状态，则更新节点的代价信息为 new_H，并确保 openlist 中的节点按照代价值的大小进行排序。
2. 如果节点的状态不是新节点或开放节点，则将其状态设置为开放，并将其插入到 openlist 中。如果节点的状态由关闭状态转变为开放状态，需要更新 shortestPaths 中节点的信息。
3. 最终，确保 openlist 中的节点按照代价值的大小进行排序，并确保节点的状态和代价信息被正确更新。

 void DStar::insert(DNode* node, int new_H)
    {
        new_H = (new_H >= _cost_max) ? _cost_max : new_H;
        if (node->state == New)
        {
            node->H = new_H;
            node->K = new_H;
            node->state = Open;
            shortestPaths[node->x][node->y] = node;
            openlist.push(node);
        }
        else if (node->state == Open) //节点已经在openlist中 那么只需要改变对应openlist位置的节点的H和K即可
        {
            node->H = new_H;
            node->K = std::min(node->K, new_H);
        }
        else
        {
            node->K = std::min(node->H, new_H);
            node->H = new_H;
            node->state = Open;
            openlist.push(node);
        }
    }

（7）DStar::process_state()是 D* 算法中的一个核心方法，用于处理算法中的状态。其主要功能是选择并处理开放列表中的节点，更新其状态和代价信息，并根据需要生成新的节点并将其添加到开放列表中。方法DStar::process_state()的实现流程如下所示。

1. 从开放列表 openlist 中选择具有最小 K 值的节点 cur_min。
2. 将选定的节点 cur_min 从开放列表中移除，并将其状态设置为关闭状态。
3. 获取节点 cur_min 的旧的 K 值 K_old。
4. 如果节点的旧 K 值小于其当前的 H 值，则遍历节点周围的相邻节点，并更新其 H 值和下一个节点指针。
5. 如果节点的旧 K 值等于其当前的 H 值，则根据情况生成新的节点并添加到开放列表中。
6. 如果节点的旧 K 值大于其当前的 H 值，则传递 “raise” 状态，并在需要时生成新的节点并添加到开放列表中。
7. 返回开放列表中具有最小 K 值的节点的新的 K 值。

方法DStar::process_state()的目的是根据当前状态更新开放列表中节点的信息，以便在后续迭代中找到最优路径。

 int DStar::process_state()
    {
        DNode* cur_min = openlist.top();
        if (!cur_min)
            return -1;
        openlist.pop();
        cur_min->state = Closed;
        int K_old = cur_min->K;
        if (K_old < cur_min->H)
        {
            for (int i = cur_min->x - 1; i <= cur_min->x + 1; ++i)
            {
                for (int j = cur_min->y - 1; j <= cur_min->y + 1; ++j)
                {
                    if (i >= 0 && j >= 0 && i < Map.size() && j < Map.front().size() && shortestPaths[i][j])
                    {
                        if (i == cur_min->x && j == cur_min->y)
                            continue;
                        if (shortestPaths[i][j]->H <= K_old && cur_min->H > shortestPaths[i][j]->H + cost(cur_min, shortestPaths[i][j]))
                        {
                            cur_min->H = shortestPaths[i][j]->H + cost(cur_min, shortestPaths[i][j]);
                            cur_min->next = shortestPaths[i][j];
                        }
                    }
                }
            }
        }
        if (K_old == cur_min->H)
        {
            for (int i = cur_min->x - 1; i <= cur_min->x + 1; ++i)
            {
                for (int j = cur_min->y - 1; j <= cur_min->y + 1; ++j)
                {
                    if (i >= 0 && j >= 0 && i < Map.size() && j < Map.front().size())
                    {
                        if (i == cur_min->x && j == cur_min->y)
                            continue;
                        if (!shortestPaths[i][j] && !Map[i][j]) //表示当前节点为New
                        {
                            DNode* neighbor = new DNode(i, j, cur_min);
                            insert(neighbor, cur_min->H + cost(neighbor, cur_min));
                        }
                        else if (shortestPaths[i][j] && !shortestPaths[i][j]->is_obstacle)
                        {
                            if ((shortestPaths[i][j]->next == cur_min && shortestPaths[i][j]->H != cur_min->H + cost(shortestPaths[i][j], cur_min))
                                || (shortestPaths[i][j]->next != cur_min && shortestPaths[i][j]->H > cur_min->H + cost(shortestPaths[i][j], cur_min)))
                            {
                                insert(shortestPaths[i][j], cur_min->H + cost(shortestPaths[i][j], cur_min));
                                shortestPaths[i][j]->next = cur_min;
                            }
                        }
                    }
                }
            }
        }
        else
        {
            //传递raise状态
            for (int i = cur_min->x - 1; i <= cur_min->x + 1; ++i)
            {
                for (int j = cur_min->y - 1; j <= cur_min->y + 1; ++j)
                {
                    if (i == cur_min->x && j == cur_min->y)
                        continue;
                    if (i >= 0 && j >= 0 && i < Map.size() && j < Map.front().size())
                    {
                        if (!shortestPaths[i][j] && !Map[i][j]) //表示当前节点为New
                        {
                            DNode* neighbor = new DNode(i, j, cur_min);
                            insert(neighbor, cur_min->H + cost(neighbor, cur_min));
                        }
                        else if (shortestPaths[i][j] && !shortestPaths[i][j]->is_obstacle)
                        {
                            if (shortestPaths[i][j]->next == cur_min && shortestPaths[i][j]->H != cur_min->H + cost(cur_min, shortestPaths[i][j]))
                            {
                                insert(shortestPaths[i][j], cur_min->H + cost(cur_min, shortestPaths[i][j]));
                            }
                            else
                            {
                                if (shortestPaths[i][j]->next != cur_min)
                                {
                                    int temp = (shortestPaths[i][j]->H + cost(shortestPaths[i][j], cur_min) >= _cost_max) ? _cost_max : shortestPaths[i][j]->H + cost(shortestPaths[i][j], cur_min);
                                    if (cur_min->H < temp)
                                        insert(cur_min, cur_min->H);
                                    else if (shortestPaths[i][j]->next != cur_min && cur_min->H > shortestPaths[i][j]->H + cost(shortestPaths[i][j], cur_min)
                                        && shortestPaths[i][j]->state == Closed && shortestPaths[i][j]->K > K_old)
                                    {
                                        insert(shortestPaths[i][j], shortestPaths[i][j]->H);
                                    }
                                }
                            }
                        }
                    }
                }
            }
        }
        return openlist.top()->K;
    }

（8）方法DStar::setObstacles(const std::vector>& obstacles)用于设置障碍物的位置，并更新相应的节点状态。方法DStar::setObstacles()的具体实现流程如下所示。

1. 接收表示障碍物位置的 obstacles 向量作为参数。
2. 对于每个障碍物的位置 (x, y)，将对应的节点状态设置为障碍物状态，即 is_obstacle 属性设置为 true。
3. 如果节点已经存在于最短路径图中，则将该节点的 H 值设置为最大值 _cost_max，表示障碍物不可通过。
4. 在地图中将障碍物位置标记为 '#'，以便可视化显示。
5. 完成障碍物的设置。

 void DStar::setObstacles(const std::vector>& obstacles)
    {
        if (obstacles.empty())
            return;
        for (const auto& rhs : obstacles)
        {
            if (shortestPaths[rhs.first][rhs.second])
            {
                shortestPaths[rhs.first][rhs.second]->is_obstacle = true;
                shortestPaths[rhs.first][rhs.second]->H = _cost_max;
            }
            Map[rhs.first][rhs.second] = '#';
        }
    }

（9）方法DStar::modify(DNode* node)用于修改节点状态，并重新规划路径以适应新的环境。方法DStar::modify(DNode* node)的具体实现流程如下所示。

1. 接收一个节点 node 作为参数，该节点需要进行修改以适应环境的变化。
2. 获取节点 node 的下一个节点 next。
3. 将下一个节点 next 添加到优先级队列 openlist 中，以重新规划路径。
4. 在循环中执行状态处理 process_state()，直到不存在更好的路径或达到最大迭代次数。
5. 返回修改后的节点的 K 值，用于后续判断是否需要继续修改其他节点。

 int DStar::modify(DNode* node)
    {
        DNode* next = node->next;
        openlist.push(next);
        int k_old;
        do {
            k_old = process_state();
            if (k_old == -1)
                break;
        } while (node->H == _cost_max || k_old < node->H);
        return k_old;
    }

（10）方法DStar::find_first_path()用于寻找起点到目标点的第一条路径，并考虑了障碍物的影响。方法DStar::find_first_path()的具体实现流程如下所示。

1. 接收起点 start、目标点 target 和障碍物列表 obstacles 作为参数。
2. 创建终点 end 并将其加入到优先级队列 openlist 中。
3. 在循环中执行状态处理 process_state()，直到起点节点被标记为 Closed。
4. 根据障碍物列表设置地图的障碍物。
5. 根据路径信息修改地图，并在路径上标记星号。
6. 如果路径无法更新，则输出 "No New Path To Target"；否则，打印更新后的地图。

 void DStar::find_first_path(const std::pair& start, const std::pair& target,
        const std::vector>& obstacles)
    {
        DNode* end = new DNode(target.first, target.second);
        shortestPaths[target.first][target.second] = end;
        openlist.push(end);
        while (true)
        {
            process_state();
            if (shortestPaths[start.first][start.second] && shortestPaths[start.first][start.second]->state == Closed)
                break;
        }
        setObstacles(obstacles);
        DNode* begin = shortestPaths[start.first][start.second];
        int i;
        while (begin)
        {
            Map[begin->x][begin->y] = '*';
            if (begin->next && begin->next->is_obstacle)
            {
                i = modify(begin);
                if (i == -1)
                {
                    break;
                }
            }
            begin = begin->next;
        }
        if (i == -1)
        {
            std::cout << "No New Path To Target" << std::endl;
            return;
        }
        else
        {
            for (const auto& i : Map)
            {
                for (const auto& j : i)
                {
                    if (j > 1)
                        std::cout << char(j) << " ";
                    else
                        std::cout << j << " ";
                }
                std::cout << std::endl;
            }
        }
    }
}

（2）编写实例文件Dstar.cpp，创建了一个 25*25 的地图，其中包含部分障碍物区域。使用 DStar 算法找到起点到终点的第一条路径，并考虑了障碍物的影响。最后，打印输出程序的运行时间（以毫秒为单位）。

#include #include #include "Dstar.h" // 包含 DStar 类的头文件
int main()
{
    // 创建一个 25x25 的地图
    std::vector> map2(25, std::vector(25));
    // 设置部分区域为障碍物
    for (int j = 3; j < 8; ++j)
        map2[6][j] = 1;
    for (int j = 4; j < 7; ++j)
        map2[7][j] = 1;
    // 记录程序开始时间
    clock_t start = clock();
    // 创建 DStar 对象并调用 find_first_path 函数寻找路径
    DStar::DStar d(map2);
    d.find_first_path({ 3, 2 }, { 23, 16 }, { {12, 4}, {12, 5}, {12, 6}, {12, 7}, {12, 8}, {12, 9}, {12, 10}, {12, 11}, {11, 7}, {13, 3}, {10, 6}, {14, 5}, {6, 2} });
    // 记录程序结束时间
    clock_t ends = clock();
    // 输出程序运行时间
    std::cout << "Running Time : " << (double)(ends - start) / CLOCKS_PER_SEC * 1000 << "ms" << std::endl;
    return 0;
}

执行后会输出：

0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 * 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 * 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 * 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 * # 1 1 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 * 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 * 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 * 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 * # 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 * * * # 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 * # # # # # # # # 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 # * * 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 # * 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 * 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 * 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 * 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 * 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 * 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 * 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 * 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 * 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 * * 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
Running Time : 24ms

上面打印输出了一个地图，地图中包含了起点、终点、障碍物以及路径。其中"*" 表示路径，"#" 表示障碍物，"1" 表示未被路径覆盖的可行走区域。

资料参考：

基于C++的D*算法详解
D-star算法简介及相关思考

未完待续