D*Lite Unity项目主要源码

点此查看D*Lite算法详解

点此下载Unity完整项目

下面是算法主要源码：

• 在这个项目我事实上做了一些变通，Km和H的计算方式没有完全依据算法，另外去掉了一些感觉用不上的过程
• 还有队列的储存方式为了贪方便只是用了个单向链表，查找时还是很慢的，如果改变储存方式其实能有进一步提升（并不，半年后我实现了堆的优先队列版本，至少在110*100的地图下，链表略占优）

链表和堆实现优先队列的对比

算法需要用到的操作有几个：

• 不根据Key直接访问结点（Update函数）
• 不根据Key直接删除结点（Remove函数）
• 入队（插入并保持有序性）
• 出队（取出并删除最小值）

每次操作的时间复杂度如下：
其中n为存储的元素个数

储存方式	直接访问	直接删除	入队	出队
有序链表	O(n)	O(n)	O(n)	O(1)
小顶堆	O(n)	O(n)	O(logn)	O(logn)
(可删除)小顶堆	O(n)	O(logn)	O(logn)	O(logn)

1. 有序链表前三者都是从头开始遍历，而出队直接取出首结点，然后头结点指向下个结点即可

2. 小顶堆访问最小元素的复杂度是O(1)，但是因为删除最小元素和插入任意元素要保证堆序，需要做O(logn)的调整，因此入队出队都是O(logn)；而堆中其余元素的整体有序性并不好，不根据键值访问只能是遍历，因此是O(n)

3. 鉴于小顶堆删除任意结点的时间复杂度高，又引入了可删除堆，原理是利用一个辅助堆记录删除的元素，当两堆顶元素相等时，同时Pop出舍弃，详见：
   https://www.cnblogs.com/fusiwei/p/11432510.html

   可以做到单纯删除(Pop)的操作时间复杂度为O(1)，但是记录时和Pop出后，需要维护堆内其它元素顺序，要做O(logn)的维护操作，因此直接删除总体时间复杂度为：

   入辅助堆+比较并在删除堆与队列堆同时Pop出删除结点+删除后两堆维护元素次序，即：
   O(logn1) + O(1) + (O(logn1)+O(logn2)) = O(logn)

   但是这个做法的弊端是会暂存删除的结点，使用的内存空间相对多亿点。

4. 更新结点(Update函数)可以视为删除结点再马上入队新结点，因此可以用两个O(logn)的操作避免直接访问的O(n)操作

因此最终两个队列的有效操作时间复杂度如下：
其中n为存储的元素个数

储存方式	直接删除	入队	出队
有序链表	O(n)	O(n)	O(1)
(可删除)小顶堆	O(logn)	O(logn)	O(logn)

乍眼一看怎么都是下面的比较快，然而事实上在110*100地图的测试环境下，我写的两个版本中，小顶堆并无太大优势，大多数情况下链表更胜一筹。
根据测试，小顶堆中直接删除仍占大量时间

不论实现上的问题，从理论上可能是两个方面引起的时间频度的问题：

1. 堆操作的时间常数比链表要大。 即同样执行一步循环时，链表更快。因为链表只需无脑访问Next，而堆往往要进行额外操作。特别对于删除函数：链表仍只需检测+无脑Next，找到了就直接删除，而可删除堆的删除共有3步组成，撇开O(1)操作不管，仍有3个O(logn)的操作；而且堆的每步操作都普遍比链表耗时长。时间复杂度只是描述耗时增长的速度，而每步循环都有自己本身的耗时，实际耗时应是：
    
   单步循环时间×期望循环次数n，其中时间复杂度只衡量了期望循环次数，而忽略了前面的时间常量
    
   举个例子，假设某O(n)操作单次循环需要运行1句（时间常数为1），而某O(logn)操作的单次循环需要运行x(>1)句（时间常数为x），我们虽然能保证n→∞时，n＞xlogn，x∈N+，但我们能保证n很小时n＞xlogn恒成立吗？
    
   大规模问题的主导影响是时间复杂度，而对于小规模问题时间常数的影响不可忽视，显然在这里110*100还是受到了时间常数的影响了。
2. 衡量链表、堆中的问题规模n并不一致。我们希望以实际入队元素个数来考察耗时问题，然而现实是：链表中元素个数n=算法意义上的入队结点数，而可删除堆中元素个数n＞入队结点数。
    
   听起来好像不可能，但可删除堆中确实是这样的：可删除堆并不是即时删除，而是继续存在堆中，并入队待删除堆，当两堆顶元素相等时，才实际移除并舍弃堆中元素。这里有个问题，就是会使堆中暂存许多待删除的无效结点。这导致了入队和出队时，仍不可避免地需要遍历这些无效结点，事实上是抬高了入队、出队操作的耗时。
   入队出队O(logn)中的n不再代表入队结点数，而是入队结点+待删除结点数，但是我们希望的是以入队元素个数角度来衡量同一问题。因此为了让问题有可比性，统一用m代表入队结点（寻路算法意义上的入队，不包括队中待删除结点），然后在相同问题规模m下计算出两者的复杂度，至于如何确定n与m的关系，由于无效结点的加入依赖于算法，而算法比较复杂，这里只通过统计得出：
    
   比如在如下情景：Remove+Enqueue函数代替了该算法的Update函数后，Remove需要频繁执行，导致大量待删除结点囤积在队列中。根据测试在地形不同时会分别导致堆中有大概3/10~7/10无效结点囤积。
   取最坏情况：堆中7/10为无效结点，即(n-m)/n = 7/10，解得m:n=3:10，也就是有效入队结点m只占所有结点n的30%。
   由：堆中元素个数为n，则入队出队时间复杂度为O(logn)
   代入得：对m:n=3:10时，用m表示时间复杂度为O(log10m/3)(由于问题规模小，常数不忽略)
   同理，由于链表中不存在无效结点，n=m，得出m规模下时间复杂度依然为O(m)
    
   由此可见，对于同一问题规模m，堆优先队列的时间常数又双叒叕被抬升了，这意味着当问题规模小时，链表更可能优于堆实现的优先队列。

总而言之这大概是执行语句时间常数影响时间频度的问题。因为问题规模不够大，而堆队列的基本语句又比链表复杂，导致执行一次循环体的时间仍造成较大影响。

但是毕竟时间复杂度摆在这，在地图更大的时候，优先队列应该会比链表有更好的表现吧

Key类和U队列

using System.Collections;
using System.Collections.Generic;
using System.Diagnostics;

/// <summary>
/// 用于算法中的Key
/// </summary>
public class Key
{
	static int idgiver;
	public double Key1;
	public double Key2;
	public int id;
	public Key() { }
	public Key(double k1, double k2)
	{
		Key1 = k1;
		Key2 = k2;
		id = idgiver++;
	}

	/// <summary>
	/// 绝对的偏序关系，内部维护确保不可能有两个Key相等
	/// 当Key1,Key2均相等时，会由分配的id决定大小（这样设计是因为优先队列必须比出顺序）
	/// </summary>
	/// <param name="a"></param>
	/// <param name="b"></param>
	/// <returns></returns>
	public static bool operator <(Key a, Key b)
	{
		if (a.Key1 < b.Key1)
			return true;
		else if (a.Key1 > b.Key1)
			return false;
		else if (a.Key2 < b.Key2)
			return true;
		else if (a.Key2 > b.Key2)
			return false;
		else if (a.id < b.id)
			return true;
		else
			return false;
	}

	/// <summary>
	/// 绝对的偏序关系，内部维护确保不可能有两个Key相等
	/// 当Key1,Key2均相等时，会由分配的id决定大小（这样设计是因为优先队列必须比出顺序）
	/// </summary>
	/// <param name="a"></param>
	/// <param name="b"></param>
	/// <returns></returns>
	public static bool operator >(Key a, Key b)
	{
		if (a.Key1 > b.Key1)
			return true;
		else if (a.Key1 < b.Key1)
			return false;
		else if (a.Key2 > b.Key2)
			return true;
		else if (a.Key2 < b.Key2)
			return false;
		else if (a.id > b.id)
			return true;
		else
			return false;
	}

	/// <summary>
    /// 用于比较Key值的小于关系（非小于等于）
	/// 当key左 == key右的时候不相等，<=只是记号
	/// </summary>
	/// <param name="a"></param>
	/// <param name="b"></param>
	/// <returns></returns>
	public static bool operator <=(Key a, Key b)
	{
		if (a.Key1 < b.Key1)
			return true;
		else if (a.Key1 > b.Key1)
			return false;
		else if (a.Key2 < b.Key2)
			return true;
		else
			return false;
	}

	/// <summary>
	/// 用于比较Key值的大于关系（非大于等于）
	/// 当key左 == key右的时候不相等，>=只是记号
	/// </summary>
	/// <param name="a"></param>
	/// <param name="b"></param>
	/// <returns></returns>
	public static bool operator >=(Key a, Key b)
	{
		if (a.Key1 > b.Key1)
			return true;
		else if (a.Key1 < b.Key1)
			return false;
		else if (a.Key2 > b.Key2)
			return true;
		else
			return false;

	}
	public static bool operator ==(Key a, Key b)
	{
		if ((a as object) != null)
			return a.Equals(b);

		else

			return (b as object) == null;
	}
	public static bool operator !=(Key a, Key b)
	{
		if ((a as object) != null)
			return !a.Equals(b);

		else

			return (b as object) != null;
	}
	public override bool Equals(object obj)
	{
		if (obj == null)
		{
			return false;
		}
		if ((obj.GetType().Equals(this.GetType())) == false)
		{
			return false;
		}
		Key b = (Key)obj;

		return Key1 == b.Key1 && Key2 == b.Key2 && id == b.id;
	}
	public override int GetHashCode()
	{
		return Key1.GetHashCode() ^ Key2.GetHashCode() ^ id.GetHashCode();
	}
}

/// <summary>
/// 队列元素
/// </summary>
public class QueueElement
{
	public Key key;
	public Node node;

	public QueueElement(Key k, Node n)
	{
		key = k;
		node = n;
	}

	public static bool operator <(QueueElement x, QueueElement y)
	{
		if (x.key < y.key)
			return true;
		else
			return false;
	}

	public static bool operator >(QueueElement x, QueueElement y)
	{
		if (x.key > y.key)
			return true;
		else
			return false;
	}
}

public class PriorityQueue
{	///========================
	///为了提高赋值效率，把部分的struct变量赋值改为ref赋值
	///========================

	public List<QueueElement> heap;//为减少几次运算，a[0]舍去，数据从a[1]开始记录
	DeleteQueue deleteQueue;
	Stack<int> TraversalIndex = new Stack<int>();
	int id;
	int count;

	public int Count { get => count; }


	public PriorityQueue(int queueId, Key key, Node node)
	{
		heap = new List<QueueElement>
		{
			null,
		};
		deleteQueue = new DeleteQueue();
		id = queueId;
		count = 0;
		Enqueue(key, node);
	}

	public PriorityQueue(PriorityQueue priorityQueue)
	{
		heap = new List<QueueElement>(priorityQueue.heap);
		count = priorityQueue.count;
		id = priorityQueue.id;
	}

	/// <summary>
	/// 在队列中加入指定键值对
	/// 为提升效率，应在外部额外记录结点的插入情况
	/// </summary>
	/// <param name="key"></param>
	/// <param name="node"></param>
	public void Enqueue(Key key, Node node)
	{
		node.insertKey[id] = key;//记录插入时的key对象

		count++;//先增加元素个数
		if (count + 1 > heap.Count)//如果满了
			heap.Add(new QueueElement(key, node));
		else//否则直接加
			heap[count] = new QueueElement(key, node);

		//然后调整新结点在堆中的位置
		AdjustInsert(count);
	}

	/// <summary>
	/// 取出队列中最小的键值对，当队列为空时返回默认值(<0,0>,null)
	/// 为提升效率，应在外部额外记录结点的插入情况
	/// </summary>
	/// <returns>(Key, Node)值元组</returns>
	public (Key, Node) Dequeue()
	{
		//新增。若与删除堆的对顶相同，同时Pop出（舍弃）
		while (count > 0 && deleteQueue.Count > 0 && deleteQueue.Top.key == heap[1].key && deleteQueue.Top.node == heap[1].node)
		{
			heap[1] = heap[count];
			count--;
			AdjustHeap(1);

			deleteQueue.Dequeue();
		}
		//原有
		if (count > 0)
		{
			QueueElement min = heap[1];
			heap[1] = heap[count];
			count--;
			min.node.insertKey[id] = null;//记录移除
			AdjustHeap(1);
			return (min.key, min.node);
		}
		else
			return (null, null);
	}

	/// <summary>
	/// 移除指定结点，如不在队中则不做修改，但会耗费大量时间！
	/// 所以最好请在移除前判断是否在队中！
	/// 所以最好请在移除前判断是否在队中！
    /// 另外，该函数是O(n)操作，已弃用，改用O1Remve
	/// </summary>
	/// <param name="node"></param>
	public void Remove(Node node)
	{

        //========================= 先序遍历，提前折回版（如果Key不符合，则子树也不符合）========================

        //if (node.inserted[id] == null)
        //    return;

        //Node checkingNode;
        //TraversalIndex.Clear(); //创建并初始化堆栈S

        //int i = 1;
        //while (true)
        //{
        //    while (i <= count)        //一直向左并将沿途节点访问（打印）后压入堆栈 
        //    {
        //        checkingNode = heap[i];//取出访问
        //        if (checkingNode == node)//找到结点，则删除并返回
        //        {
        //            //不是最后一个元素则需要调整
        //            if (i != count)
        //            {
        //                if (heap[i].inserted[id] < heap[count].inserted[id])//最后一个元素＞删除元素。代替后可能破坏子结点次序，视为根结点向下调整
        //                {
        //                    heap[i] = heap[count];
        //                    AdjustHeap(i);
        //                }
        //                else//否则最后一个元素≤删除元素。代替后可能破坏父结点次序，视为叶子结点向上调整
        //                {
        //                    heap[i] = heap[count];
        //                    AdjustInsert(i);
        //                }
        //            }
        //            count--;//减少长度

        //            //清除key
        //            checkingNode.inserted[id] = null;

        //            return;
        //        }
        //        else if (checkingNode.inserted[id] > node.inserted[id]) //当前结点>请求结点，则不可能在以该结点为根的子树出现
        //        {
        //            break;
        //        }
        //        else //当前结点<请求结点，继续监测子树
        //        {
        //            TraversalIndex.Push(i);
        //            i <<= 1;//左孩子
        //        }
        //    }
        //    if (TraversalIndex.Count > 0)
        //    {
        //        i = (TraversalIndex.Pop() << 1) + 1;//右孩子
        //    }
        //}


        //=================== 完全层次遍历版 ================
        遍历表
        //for (int i = 1; i <= count; i++)
        //{

        //    //如果找到元素，则删除将用最后一个元素替代删除元素，并调整堆
        //    if (heap[i] == node)
        //    {
        //        stop.Start();
        //        //不是最后一个元素则需要调整
        //        if (i != count)
        //        {
        //            if (heap[i].insertKeys[id] < heap[count].insertKeys[id])//最后一个元素＞删除元素。代替后可能破坏子结点次序，视为根结点向下调整
        //            {
        //                heap[i] = heap[count];
        //                AdjustHeap(i);
        //            }
        //            else//否则最后一个元素≤删除元素。代替后可能破坏父结点次序，视为叶子结点向上调整
        //            {
        //                heap[i] = heap[count];
        //                AdjustInsert(i);
        //            }
        //        }
        //        count--;//减少长度
        //        stop.Stop();
        //    }

        //}
    }

	/// <summary>
	/// 根据Node现在所记录的Key对象记录这次删除
	/// 以标记代替删除，时间复杂度O(1)，但大幅增加了空间使用
	/// </summary>
	/// <param name="node"></param>
	public void O1Remove(Node node)
    {
		//若未插入（key空）或已标记移除，返回
		if (node.insertKey[id] == null)
			return;

        deleteQueue.Enqueue(node.insertKey[id], node);
		node.insertKey[id] = null;//记录移除
	}

	/// <summary>
    /// 更新一个结点
    /// O(n)操作，已弃用，改为O1Remove+Enqueue
    /// </summary>
    /// <param name="key"></param>
    /// <param name="node"></param>
	public void Update(Key key, Node node)
    {
		if (node.insertKey[id] == key)
			return;

        TraversalIndex.Clear(); //创建并初始化堆栈S
        int i = 1;

        while (true)
        {
			while (i <= count)//一直向左并将沿途节点访问后压入堆栈 
            {
				if (heap[i].key == node.insertKey[id] && heap[i].node == node)//找到结点（并且不是待删除的），则更新key并返回
                {
					heap[i].node.insertKey[id] = key;
					if (heap[i].key < key)//旧Key<新Key。代替后可能破坏子结点次序，视为根结点向下调整
					{
						heap[i].key = key;
						AdjustHeap(i);
					}
					else//否则旧Key≥新Key。代替后可能破坏父结点次序，视为叶子结点向上调整
					{
						heap[i].key = key;
						AdjustInsert(i);
					}
					return;
                }
                else if (heap[i].key > node.insertKey[id]) //当前结点>请求结点，则不可能在以该结点为根的子树出现
                {
                    break;
                }
                else //当前结点<请求结点，继续监测子树
                {
                    TraversalIndex.Push(i);
                    i <<= 1;//左孩子
                }
            }
            if (TraversalIndex.Count > 0)
            {
                i = (TraversalIndex.Pop() << 1) + 1;//右孩子
            }
        }
    }

	/// <summary>
	/// 自上而下调整一个结点在堆中的顺序
	/// 适用于（相对的）根结点
	/// </summary>
	/// <param name="index"></param>
	public void AdjustHeap(int index)
	{
		QueueElement temp = heap[index];
		for (int child = index << 1; child <= count; index = child, child <<= 1)//index结点的左子结点开始
		{
			if (child + 1 <= count && heap[child] > heap[child + 1])//如果左子结点大于右子结点，child指向右子结点
				child++;

			if (heap[child] < temp)//如果子节点小于temp节点，子结点上升
			{
				heap[index] = heap[child];
			}
			else
			{
				break;
			}
		}
		heap[index] = temp;//将temp值放到最终的位置
	}


	/// <summary>
	/// 自下而上调整一个结点在堆中的顺序
	/// 适用于（相对的）叶子结点
	/// </summary>
	/// <param name="index"></param>
	public void AdjustInsert(int index)
    {
		QueueElement temp = heap[index];
		for (int parent = index >> 1; parent >= 1; index = parent, parent >>= 1)
        {
			if (heap[parent] > temp)//父元素比该结点大，父结点下沉
            {
				heap[index] = heap[parent];
			}
			else//否则父结点≤该结点，在index位置插入该结点
            {
				break;
			}
		}
		heap[index] = temp;//将temp值放到最终的位置
	}
}

/// <summary>
/// 存储待删除数据，以实现以O(1)实现优先队列的任意结点删除(删除后的维护其实是O(logn))
/// 只辅助优先队列删除，不对Node造成任何实际更改
/// </summary>
public class DeleteQueue
{
	List<QueueElement> heap;//为减少几次运算，a[0]舍去，数据从a[1]开始记录

	int count;

	public int Count { get => count; }

	public QueueElement Top { get => count > 0 ? heap[1] : null; }

	public DeleteQueue()
	{
		heap = new List<QueueElement>
		{
			null,
		};
		count = 0;
	}

	public void Enqueue(Key key, Node node)
	{
		count++;//先增加元素个数
		if (count + 1 > heap.Count)//如果满了
			heap.Add(new QueueElement(key, node));
		else//否则直接加
			heap[count] = new QueueElement(key, node);

		//然后调整新结点在堆中的位置
		AdjustInsert(count);
	}

	/// <summary>
	/// 取出队列中最小的键值对，当队列为空时返回默认值(<0,0>,null)
	/// 为提升效率，应在外部额外记录结点的插入情况
	/// </summary>
	/// <returns>(Key, Node)值元组</returns>
	public void Dequeue()
	{
		if (count > 0)
		{
			heap[1] = heap[count];
			count--;
			AdjustHeap(1);
		}
	}


	/// <summary>
	/// 自上而下调整一个结点在堆中的顺序
	/// 适用于（相对的）根结点
	/// </summary>
	/// <param name="index"></param>
	public void AdjustHeap(int index)
	{
		QueueElement temp = heap[index];
		for (int child = index << 1; child <= count; index = child, child <<= 1)//index结点的左子结点开始
		{
			if (child + 1 <= count && heap[child] > heap[child + 1])//如果左子结点大于右子结点，child指向右子结点
				child++;

			if (heap[child] < temp)//如果子节点小于temp节点，子结点上升
			{
				heap[index] = heap[child];
			}
			else
			{
				break;
			}
		}
		heap[index] = temp;//将temp值放到最终的位置
	}


	/// <summary>
	/// 自下而上调整一个结点在堆中的顺序
	/// 适用于（相对的）叶子结点
	/// </summary>
	/// <param name="index"></param>
	public void AdjustInsert(int index)
	{
		QueueElement temp = heap[index];
		for (int parent = index >> 1; parent >= 1; index = parent, parent >>= 1)
		{
			if (heap[parent] > temp)//父元素比该结点大，父结点下沉
			{
				heap[index] = heap[parent];
			}
			else//否则父结点≤该结点，在index位置插入该结点
			{
				break;
			}
		}
		heap[index] = temp;//将temp值放到最终的位置
	}
}

Node（结点）类

using System.Collections;
using System.Collections.Generic;
using System.Diagnostics;
using UnityEngine;

public class Node
{
	//====================变量====================
	//记录地图块信息
	public int x;
	public int y;

	//寻路地图到实际地图的映射关系，非Unity项目可直接删除
	public Vector3 Position { get { return new Vector3(x * 2 + 1, y * 2 + 1, 0); } }

	//记录算法主要数值
	public bool enabled = true;
	public HashSet<Node> neighbour = new HashSet<Node>();//相当于predecessors和successors的合并，由于不需要重复，所以使用不可重的hashset代替list

	//由于是多个D*进程，所以会有多组算法变量，用List存储
	//static List
	public static List<double> km = new List<double>();
	//List 一个结点储存多组寻路信息（以供多个单位）
	public List<Key> insertKey = new List<Key>();
	public List<bool> inRemoveQueue = new List<bool>();
	public List<double> g = new List<double>();
	public List<double> rhs = new List<double>();
	public List<double> h = new List<double>();

	//===================更新结点信息===================
	public void AddNodeInfo()
	{
		insertKey.Add(null);
		//inRemoveQueue.Add(false);
		g.Add(double.PositiveInfinity);
		rhs.Add(double.PositiveInfinity);
		h.Add(0);
	}

	public void RenewNodeInfo(int i)//留下
	{
		insertKey[i] = null;
		//inRemoveQueue.Add(false);
		g[i] = double.PositiveInfinity;
		rhs[i] = double.PositiveInfinity;
		h[i] = 0;
	}

	//====================构造函数====================
	public Node() {}
	public Node(int inputX, int inputY)
	{
		x = inputX;
		y = inputY;
	}
	//====================算法函数====================
	public Key CalculateKey(int i)
	{
		return new Key((g[i] < rhs[i] ? g[i] : rhs[i]) + h[i] + km[i], g[i] < rhs[i] ? g[i] : rhs[i]);
	}

	public void UpdateVertex(int i, PriorityQueue U)
	{
		//代替下面
        if (insertKey[i] != null)
        {
            U.O1Remove(this);
        }

		if (g[i] != rhs[i])
        {
			U.Enqueue(CalculateKey(i), this);
        }


        //较慢，已弃用
        //if (g[i] != rhs[i])
        //{
        //    if (insertKey[i] == null)
        //        U.Enqueue(CalculateKey(i), this);//不在队中，入队
        //    else
        //        U.Update(CalculateKey(i), this);//在队中，更新Key值
        //}
        //else if (insertKey[i] != null)
        //    U.O1Remove(this);//在队中，移除

    }
	//====================重载====================
	public static bool operator !=(Node a, Node b)
	{
		if ((a as object) != null)
			return !a.Equals(b);
		else
			return (b as object) != null;
	}

	public static bool operator ==(Node a, Node b)
	{
		if ((a as object) != null)
			return a.Equals(b);
		else
			return (b as object) == null;
	}

	public override bool Equals(object obj)
	{
		if (obj == null)
		{
			return false;
		}
		Node nodeObj = obj as Node;
		if ((object)nodeObj == null)
		{
			return false;
		}
		else if (x == nodeObj.x && y == nodeObj.y)
		{
			return true;
		}
		else
			return false;
	}
	public override int GetHashCode()
	{
		return x.GetHashCode() ^ y.GetHashCode();
	}
}

DStarLite类

using System.Collections;
using System.Collections.Generic;
using System.Diagnostics;
using UnityEngine;

public class DStarLite
{
    int i;
    ///=================给自己看的提示=================
    ///该算法后来修复了穿墙角，共有两个部分共同组成
    ///第一部分在Cost函数，第二部分在GetFinalPath函数
    ///CalculateH必须要在结点更新前调用
    ///障碍物出现与消失需要外部手动调用该函数
    ///====================End提示====================
    //====================委托====================
    //将地图要素改变时，通过委托通知所有寻路实例
    public delegate void MapChanges();
    private static MapChanges reCalculateAllH;
    private static MapChanges reCalculateAllPath;
    public static MapChanges ReCalculateAllPath { get => reCalculateAllPath; private set => reCalculateAllPath = value; }

    public static MapChanges ReCalculateAllH { get => reCalculateAllH; private set => reCalculateAllH = value; }

    public delegate void NodeChanges(Node node);
    private static NodeChanges onNodeDisabled;
    private static NodeChanges onNodeEnabled;
    public static NodeChanges OnNodeDisabled { get => onNodeDisabled; private set => onNodeDisabled = value; }
    public static NodeChanges OnNodeEnabled { get => onNodeEnabled; private set => onNodeEnabled = value; }

    //====================变量====================
    //管理实例编号
    private static int IdGiver = 0;
    private static Queue<int> IdPool = new Queue<int>();

    int id;
    //储存全局的结点
    public static Node[,] Nodes;
    public static Node GlobalStart { get; private set; }
    public static Node GlobalGoal { get; private set; }

    //储存个体的结点
    public Node lastStart;
    public Node sStart;
    public Node sGoal;
    //public KeyQueue U;
    public PriorityQueue U;

    //寻路状态：初次？再次？，默认为初次
    private bool FirstCalculte = true;

    //最终路径
    public Queue<Node> finalPath = new Queue<Node>();

    /// <summary>
    /// 示意寻路是否已达预定终点，为真时表示没有终点或未达终点
    /// </summary>
    public bool NotAchieveGoal
    {
        get
        {
            if (sStart != null && sGoal != null && sStart != sGoal)
                return true;
            else
                return false;
        }
    }

    //====================交互函数====================
    /// <summary>
    /// 构造函数会配合Node类一起，分配一个编号，并开辟储存g,h,rhs数值的空间
    /// </summary>
    public DStarLite()
    {
        if (IdPool.Count == 0)
        {
            //新增ID
            //在Node中增加相关信息
            Node.km.Add(0);

            foreach (Node node in Nodes)
            {
                node.AddNodeInfo();
            }
            id = IdGiver++;
        }
        else
        {
            //重用ID
            id = IdPool.Dequeue();
            foreach (Node node in Nodes)
            {
                node.RenewNodeInfo(id);
            }
        }
    }

    /// <summary>
    /// 对整个算法系统的初始化
    /// 一般在新关卡开始时需要重新初始化变量
    /// </summary>
    public static void GlobalInitialize(Node globalStart, Node globalGoal)
    {
        //设置起点、终点
        GlobalStart = globalStart;
        GlobalGoal = globalGoal;

        //重置分配id
        IdPool.Clear();
        IdGiver = 0;

        //清空委托
        ClearMapChanges(ref reCalculateAllPath);
        ClearMapChanges(ref reCalculateAllH);
        ClearNodeChanges(ref onNodeDisabled);
        ClearNodeChanges(ref onNodeEnabled);

        void ClearMapChanges(ref MapChanges mapChangeDelegate)
        {

            if (mapChangeDelegate == null)
                return;
            else
                MonoBehaviour.print(mapChangeDelegate);
            System.Delegate[] dlg = mapChangeDelegate.GetInvocationList();
            for (int i = 0; i < dlg.Length; i++)
            {
                mapChangeDelegate -= dlg[i] as MapChanges;
            }
            MonoBehaviour.print(mapChangeDelegate);
        }
        void ClearNodeChanges(ref NodeChanges mapChangeDelegate)
        {
            if (mapChangeDelegate == null)
                return;
            System.Delegate[] dlg = mapChangeDelegate.GetInvocationList();
            for (int i = 0; i < dlg.Length; i++)
            {
                mapChangeDelegate -= dlg[i] as NodeChanges;
            }
        }

    }

    /// <summary>
    /// 表示已经到达finalPath的当前点，该函数将返回路径中该点并从finalPath移除，此时要更新部分数据
    /// </summary>
    public Node HaveGetToNextNode()
    {
        if (finalPath != null && finalPath.Count != 0)//获取下一个节点
        {
            Node to = finalPath.Dequeue();
            Node.km[id] += System.Math.Abs((sStart.h[id] - to.h[id]));
            sStart = to;


            //如果获取后寻路结束
            if (sStart == sGoal)
            {
                //==========回收ID==========
                IdPool.Enqueue(id);
                //==========寻路结束，从寻路事件中移除==========
                ReCalculateAllPath -= ComputePath;
                ReCalculateAllH -= CalculateAllH;
                OnNodeDisabled -= NodeDisabled;
                OnNodeEnabled -= NodeEnabled;

                MonoBehaviour.print("到终点");
            }
            return to;
        }
        else//获取失败
        {
            return null;
        }
    }

    //====================算法函数====================
    public void ComputePath()
    {
        //==========================ForDebugStart==========================
        //=====计时器
        Stopwatch readdTime = new Stopwatch();
        Stopwatch overConsistanceTime = new Stopwatch();
        Stopwatch underConsistanceTime = new Stopwatch();
        Stopwatch totalTime = new Stopwatch();
        Stopwatch dequeueTime = new Stopwatch();
        totalTime.Start();

        //=====DequeueLoopCount 记录循环中队列Dequeue了多少次
        int DequeueLoopCount = 0;
        int readdToDeque = 0;
        int overConsistance = 0;
        int underConsistance = 0;
        string timtText;
        //===========================ForDebugEnd===========================

        //对自身的初始化
        if (FirstCalculte)
        {
            sStart = GlobalStart;
            sGoal = GlobalGoal;

            CalculateAllH();

            #region 原算法的Initialize()
            sGoal.rhs[id] = 0;
            Node.km[id] = 0;

            U = new PriorityQueue(id,sGoal.CalculateKey(id) , sGoal);
            #endregion

            //==========加入代理==========
            ReCalculateAllPath += ComputePath;
            ReCalculateAllH += CalculateAllH;
            OnNodeDisabled += NodeDisabled;
            OnNodeEnabled += NodeEnabled;

            FirstCalculte = false;
        }
        //else
        //{
        //    Node.km[id] += lastStart.h[id];
        //}

        //lastStart = sStart;

        Key tempNewKey;
        Key topKey;
        Node Utop;

        double tempRhs;
        double gOld;

        //进行节点队列处理
        while (true)
        {
            //先取一个值，使用元组析构赋值
            dequeueTime.Start();
            (topKey, Utop) = U.Dequeue();
            dequeueTime.Stop();
            
            //空，溜
            if (Utop == null) break;

            //不满足条件，溜
            if (!(topKey <= sStart.CalculateKey(id) || sStart.rhs[id] > sStart.g[id])) break;

            DequeueLoopCount++;

            //更新后Key值
            tempNewKey = Utop.CalculateKey(id);

            if (topKey <= tempNewKey)
            {
                readdTime.Start();
                readdToDeque++;

                U.Enqueue(tempNewKey, Utop);
            }
            //局部过一致
            else if (Utop.g[id] > Utop.rhs[id])
            {
                overConsistanceTime.Start();
                overConsistance++;

                //设置为局部一致
                Utop.g[id] = Utop.rhs[id];
                //对九宫格内(除自己)的节点传递
                foreach (Node predecessor in Utop.neighbour)
                {
                    if (predecessor != sGoal)
                    {
                        //如果这个更新后的Utop节点是一个更好的successor，则把更低的Rhs赋值给自己(Rhs决定路径，即从结果而言这步是改变最佳路径)
                        tempRhs = Cost(predecessor, Utop) + Utop.g[id];
                        if (predecessor.rhs[id] > tempRhs)
                        {
                            predecessor.rhs[id] = tempRhs;
                        }
                    }
                    predecessor.UpdateVertex(id, U);
                }
            }
            //局部欠一致（这里意味着g<rhs(因为进入U的都g!=rhs，且没有执行上一个if语句)，则意味着出现了障碍）
            else
            {
                underConsistanceTime.Start();
                underConsistance++;

                gOld = Utop.g[id];
                Utop.g[id] = double.PositiveInfinity;
                //对九宫格内(除自己)的节点传递
                foreach (Node predecessor in Utop.neighbour)
                {
                    if (predecessor != sGoal)
                    {
                        //如果rhs == Cost + gOld成立，意味着花费和gOld都没变
                        //这个rhs == Cost + gOld意味着在之前一种情况中，Utop → predecessor是一条最优回溯路径（即最终路径的一部分），而且障碍物出现后，Cost没有改变（障碍物没有直接阻挡这条路径）
                        //总体意思是：现在由于Utop节点被影响导致需要重计算g值。Utop单个节点虽然还是通的，但由于障碍物出现有可能导致这整条路径不再是最优路径了（甚至可能整体上不通），所以我们需要给这个点重新选择最优的路径（即重新选择successor）
                        if (predecessor.rhs[id] == Cost(predecessor, Utop) + gOld)
                        {
                            //重新寻找最优的successor
                            predecessor.rhs[id] = double.PositiveInfinity;

                            foreach (Node successor in predecessor.neighbour)
                            {
                                //==========计算稍微优化？==========
                                //根据随机频率测试，该判断大概可以有66%的概率避免if语句内的无效运算
                                if (successor.g[id] != double.PositiveInfinity)
                                {
                                    tempRhs = Cost(successor, predecessor) + successor.g[id];

                                    if (predecessor.rhs[id] > tempRhs)
                                    {
                                        predecessor.rhs[id] = tempRhs;
                                    }
                                }
                            }
                        }
                    }
                    predecessor.UpdateVertex(id, U);
                }
                Utop.UpdateVertex(id, U);
            }
            readdTime.Stop();
            overConsistanceTime.Stop();
            underConsistanceTime.Stop();
        }
        SetFinalPath();
        totalTime.Stop();
        //==========================ForDebugStart==========================
        timtText =
            "总队列检测次数：" + DequeueLoopCount + "\n" +
            "重新加入队列次数：" + readdToDeque + "\n" + "耗时(ms)：" + readdTime.Elapsed.TotalMilliseconds + "\n" +
            "局部过一致次数：" + overConsistance + "\n" + "耗时(ms)：" + overConsistanceTime.Elapsed.TotalMilliseconds + "\n" +
            "局部欠一致次数：" + underConsistance + "\n" + "耗时(ms)：" + underConsistanceTime.Elapsed.TotalMilliseconds + "\n" +
            "总耗时(ms)：" + totalTime.Elapsed.TotalMilliseconds;
        PrintUsingTime.ChangeTimeText(timtText);

        MonoBehaviour.print("dequeueTime: " + dequeueTime.ElapsedMilliseconds);
        //===========================ForDebugEnd===========================
    }

    /// <summary>
    /// （计算h的函数，由于这个案例中km不用△h来计算，所以只有出现障碍物才需要再次计算h）
    ///  在任意的结点被更新(包括障碍物出现、消失的结算)前，如果h过时，则需要调用该函数
    /// </summary>
    void CalculateAllH()
    {
        foreach (Node tempNode in Nodes)
        {
            tempNode.h[id] = (Mathf.Abs(tempNode.x - sStart.x) + Mathf.Abs(tempNode.y - sStart.y)) * 10;//用近似距离作为启发值
        }
    }

    /// <summary>
    /// 将寻路结果设置到finalPath变量
    /// </summary>
    void SetFinalPath()
    {
        finalPath.Clear();

        //如果起点就是终点
        if (sStart == sGoal)
            return;

        //初始化
        Node tempStartNode = sStart;
        Node tempBestSuccessor = null;
        double tempG;

        //===========debug
        i = 0;
        do
        {
            i++;
            if (i > 100000)
            {
                MonoBehaviour.print("SetFinalPath DeadLoop");
                break;
            }

            tempG = double.PositiveInfinity;
            //寻找一个最好的successor
            foreach (Node successor in tempStartNode.neighbour)
            {
                //贪婪选路+穿墙角修复

                if (tempG > successor.g[id])
                {
                    //穿墙角修复②：这里修补半穿墙角的情况，如：
                    //□■
                    //□□
                    int DeltaX = successor.x - tempStartNode.x;
                    int DeltaY = successor.y - tempStartNode.y;
                    if (Mathf.Abs(DeltaX) + Mathf.Abs(DeltaY) == 2)//判断斜角移动
                    {
                        //而且如果有障碍
                        if (!Nodes[tempStartNode.x + DeltaX, tempStartNode.y].enabled || !Nodes[tempStartNode.x, tempStartNode.y + DeltaY].enabled)
                        {
                            continue;//直接跳过
                        }
                    }
                    tempG = successor.g[id];
                    tempBestSuccessor = successor;
                }
            }
            //如果到处碰壁
            if (tempG == double.PositiveInfinity)
            {
                //返回空路径
                finalPath.Clear();
                return;
            }
            //否则添加最好successor至路径
            else
            {
                finalPath.Enqueue(tempBestSuccessor);
                tempStartNode = tempBestSuccessor;
            }
        }
        while (tempStartNode != sGoal);
    }

    /// <summary>
    /// 两个相邻节点之间的花费，包含穿墙角修复的一部分代码
    /// </summary>
    /// <param name="from">初始节点</param>
    /// <param name="to">目标节点</param>
    /// <returns>花费</returns>
    double Cost(Node from, Node to)
    {
        if (!from.enabled || !to.enabled)
        {
            return double.PositiveInfinity;
        }
        else
        {
            int DeltaX = to.x - from.x;
            int DeltaY = to.y - from.y;
            switch (Mathf.Abs(DeltaX) + Mathf.Abs(DeltaY))
            {
                //用值模拟，为了和△h统一，所以没用√2，事实上用√2反而会出错
                case 0: return 0;
                case 1: return 10;
                case 2:
                    //穿墙角修复①：这里修补半穿墙角的情况，如：
                    //□■
                    //■□
                    if (Nodes[from.x + DeltaX, from.y].enabled && Nodes[from.x, from.y + DeltaY].enabled)
                    {
                        return 20;
                    }
                    else
                    {
                        return double.PositiveInfinity;
                    }
                default: return 0;
            }
        }
    }

    /// <summary>
    /// 当节点需要被失效时调用，需要手动调用寻路函数
    /// </summary>
    /// <param name="disabled">失效的节点</param>
    void NodeDisabled(Node disabled)
    {
        //需要在更新结点前重新计算H
        //ReCalculateAllH(); 重要！：但是由于同一时间更改多个结点只需调用一次该函数，故需在外部手动调用


        double cOld;
        double tempRhs;
        Node tempOldNode = new Node //注意这里将浅拷贝disabled，由于只是给cost函数调用，所以只拷贝关键的变量
        {
            x = disabled.x,
            y = disabled.y,
            //enabled = true
        };

        //对自己
        disabled.rhs[id] = double.PositiveInfinity;

        //对九宫格内所有节点
        foreach (Node predecessor in disabled.neighbour)
        {
            cOld = Cost(predecessor, tempOldNode);
            //如果predecessor.rhs == cOld + disabled.g，则意味着predecessor→disabled曾是最好路径
            //所以现在要给这个predecessor重新找最好路径
            if (predecessor.rhs[id] == cOld + disabled.g[id])
            {
                if (predecessor == sGoal)
                {
                    //跳过终点的检测
                    continue;
                }

                //需要重新给予rhs值，所以先假设rhs最大，再取更小值
                predecessor.rhs[id] = double.PositiveInfinity;

                //对该predecessor的所有successor
                foreach (Node successor in predecessor.neighbour)
                {
                    tempRhs = Cost(predecessor, successor) + successor.g[id];
                    if (predecessor.rhs[id] > tempRhs)
                    {
                        predecessor.rhs[id] = tempRhs;
                    }
                }
            }
            predecessor.UpdateVertex(id, U);
        }
        disabled.UpdateVertex(id, U);


    }

    /// <summary>
    /// 当节点需要重新启用时调用，需要手动调用寻路函数
    /// </summary>
    /// <param name="enabled">生效的节点</param>
    void NodeEnabled(Node enabled)
    {
        //需要在更新结点前重新计算H
        //ReCalculateAllH(); 重要！：但是由于同一时间更改多个结点只需调用一次该函数，故需在外部手动调用


        double tempCost;
        double tempRhs;
        //检测九宫格内所有节点
        foreach (Node predecessor in enabled.neighbour)
        {
            tempCost = Cost(predecessor, enabled);
            //对自己更新（注意：由于终点的设定是不能被disable或enable，所以这里对自己必然!=sGoal，就没有对 == sGoal的检测）
            tempRhs = tempCost + predecessor.g[id];
            if (enabled.rhs[id] > tempRhs)
            {
                enabled.rhs[id] = tempRhs;
            }

            //对predecessor更新
            if (predecessor == sGoal)
            {
                continue;
            }
            //enabled.g一定还是无穷，这一步没有意义
            //tempRhs = tempCost + enabled.g[id];
            //if (predecessor.rhs[id] > tempRhs)
            //{
            //    predecessor.rhs[id] = tempRhs;
            //}
            //predecessor.UpdateVertex(id, U);
        }
        enabled.UpdateVertex(id, U);
    }
}

补充说明

每新建一个DstarLite对象即可生成一个寻路实例，其它交互函数见DstarLite类。编译时没有Unity库可以直接把错误的行注释掉，不影响算法。

使用示范

这个脚本需要Unity实现，但是可以参考实现思路。重点注意ReCalculateAllH函数的调用位置。

using System.Collections;
using System.Collections.Generic;
using System.Diagnostics;
using UnityEngine;
using UnityEngine.Tilemaps;

public class DStarLiteDebugger : MonoBehaviour
{
    [SerializeField]
    GameObject UI;

    //=========预设值=============
    int X = 110, Y = 100;
    float blockPercentage = 0.2f;

    //=========寻路实例============
    Node sStart, sGoal;
    DStarLite pathFinder;
    Queue<Node> path;
    Queue<Node> lastPath;

    bool start;

    //==========Tilemap===========
    [SerializeField]
    Tile blockTile;
    [SerializeField]
    Tile roadTile;
    [SerializeField]
    Tile startTile;
    [SerializeField]
    Tile goalTile;
    [SerializeField]
    Tile pathTile;

    [SerializeField]
    Tilemap tilemap;
    void Start()
    {
        lastPath = new Queue<Node>();
        //创建地图
        DStarDebugMap();
    }
    void DStarDebugMap()
    {
        Node tempNode;
        DStarLite.Nodes = new Node[X, Y];

        //============创建Nodes并处理邻近结点====================
        for (int x = 0; x < X; x++)
        {
            for (int y = 0; y < Y; y++)
            {
                tempNode = DStarLite.Nodes[x, y] = new Node(x, y);
                //随机障碍
                if (Random.value < blockPercentage)
                {
                    tempNode.enabled = false;
                    tilemap.SetTile(new Vector3Int(x, y, 0), blockTile);
                }
                else
                {
                    tilemap.SetTile(new Vector3Int(x, y, 0), roadTile);
                }
            }
        }
        for (int x = 0; x < X; x++)
        {
            for (int y = 0; y < Y; y++)
            {
                for (int i = x - 1; i <= x + 1; i++)
                {
                    for (int j = y - 1; j <= y + 1; j++)
                    {
                        //确保在范围内
                        if ((i != x || j != y) && i >= 0 && j >= 0 && i < X && j < Y)
                        {
                            DStarLite.Nodes[x, y].neighbour.Add(DStarLite.Nodes[i, j]);
                        }
                    }
                }
            }
        }

    }
    void Update()
    {
        //退出
        Esc();

        //右键中途取消设置障碍
        RighClickCancelBlock();

        //左键设置开始结束位置（第一次起点，第二次终点）
        LeftClickSetStartAndEnd();

        //中键设置障碍
        MiddleClickSetBlock();

        //显示路径
        VisiblePath();

        //空格键开始第一次寻路
        PressSpaceStart();
    }

    void PressSpaceStart()
    {
        if (Input.GetKeyDown(KeyCode.Space))
        {
            if (!start)
            {
                if (sStart != null && sGoal != null)
                {
                    DStarLite.GlobalInitialize(sStart, sGoal);

                    pathFinder = new DStarLite();
                    pathFinder.ComputePath();
                    path = pathFinder.finalPath;

                    StartCoroutine(Dequeue());

                    start = true;
                }

            }
            else
            {
                //重新开始
                UnityEngine.SceneManagement.SceneManager.LoadScene(0);
                StopAllCoroutines();
                sStart = null;
                sGoal = null;

                start = false;
            }
        }
    }
    IEnumerator Dequeue()
    {
        do
        {
            yield return new WaitForSeconds(1.0f);

            if (path.Count != 0)
                pathFinder.HaveGetToNextNode();
        }
        while (true);
    }

    void VisiblePath()
    {
        //先撤回上次更改
        if (lastPath != null)
        {
            foreach (Node node in lastPath)
            {
                if (node.enabled == true)
                    tilemap.SetTile(new Vector3Int(node.x, node.y, 0), roadTile);
                else
                    tilemap.SetTile(new Vector3Int(node.x, node.y, 0), blockTile);
            }
        }

        //再重新修改路径颜色
        if (path != null)
        {
            lastPath.Clear();
            bool head = true;
            foreach (Node node in path)
            {
                if (head)
                {
                    tilemap.SetTile(new Vector3Int(node.x, node.y, 0), startTile);
                    head = false;
                }
                else
                    tilemap.SetTile(new Vector3Int(node.x, node.y, 0), pathTile);

                lastPath.Enqueue(node);
            }
        }
    }

    void RighClickCancelBlock()
    {
        if (Input.GetMouseButton(1))
        {

            Node tempNode = GetNodeFromMouse();
            if (tempNode != null)
            {
                if (!tempNode.enabled)
                {
                    //更新D*结点前先更新全局H值
                    DStarLite.ReCalculateAllH?.Invoke();
                    tempNode.enabled = true;
                    //设置真假后需要调用结点更变委托
                    DStarLite.OnNodeEnabled?.Invoke(tempNode);
                    DStarLite.ReCalculateAllPath?.Invoke();

                    tilemap.SetTile(new Vector3Int(tempNode.x, tempNode.y, 0), roadTile);
                }
            }
        }
    }

    void LeftClickSetStartAndEnd()
    {
        //左键设置开始结束位置
        if (Input.GetMouseButtonDown(0))
        {
            if (sStart == null)
            {
                sStart = GetNodeFromMouse();
                if (sStart != null)
                {
                    tilemap.SetTile(new Vector3Int(sStart.x, sStart.y, 0), startTile);
                }
            }
            else if (sGoal == null)
            {
                sGoal = GetNodeFromMouse();
                if (sGoal != null)
                {
                    //DStarLite全局初始化
                    tilemap.SetTile(new Vector3Int(sGoal.x, sGoal.y, 0), goalTile);
                }
            }
        }
    }

    void MiddleClickSetBlock()
    {
        if (Input.GetMouseButton(2))
        {
            Node tempNode = GetNodeFromMouse();
            if (tempNode != null)
            {
                if (tempNode.enabled)
                {
                    //更新D*结点前先更新全局H值
                    DStarLite.ReCalculateAllH?.Invoke();
                    tempNode.enabled = false;
                    //设置真假后需要调用结点更变委托
                    DStarLite.OnNodeDisabled?.Invoke(tempNode);
                    DStarLite.ReCalculateAllPath?.Invoke();

                    tilemap.SetTile(new Vector3Int(tempNode.x, tempNode.y, 0), blockTile);
                }
            }
        }
    }

    void Esc()
    {
        if (Input.GetKeyDown(KeyCode.Escape))
        {
            Application.Quit();
        }
    }

    Node GetNodeFromMouse()
    {
        Vector3 tempVector = Camera.main.ScreenToWorldPoint(Input.mousePosition);
        int x = (int)tempVector.x;
        int y = (int)tempVector.y;
        if (x >= 0 && x < X && y >= 0 && y < Y)
            return DStarLite.Nodes[x, y];
        else
            return null;
    }
}