（8-3-02）自动驾驶中的无地图环境路径探索：D* Lite路径规划系统（2）

8.3.3  实现D* Lite算法

文件d_star_lite.py实现了 D* Lite 算法的关键部分，包括计算路径、更新路径、扫描障碍物、移动并重新扫描等功能。具体来说，文件d_star_lite.py定义了计算 D* Lite 算法中路径的函数，包括计算顶部键、启发式函数、更新顶点、计算最短路径、寻找下一个最短路径、扫描障碍物、移动并重新扫描等功能。这些函数可以实现在动态环境中实时更新路径规划，并根据环境变化进行路径的调整和优化。

（1）函数topKey(queue)用于获取优先队列中最小键值的元素，它首先对队列进行排序，然后返回排序后队列中的第一个元素的前两个值作为键值。如果队列为空，则返回无穷大的键值。

def topKey(queue): queue.sort() # print(queue) if len(queue) > 0: return queue[0][:2] else: # print('empty qu