LPA* 路径搜索算法介绍及完整代码

1.简介

LPA*算法（全称Lifelong Planning A*算法）是由Koenig和Likhachev在2004年提出的，是一种参考人工智能的增量搜索进行改进的A*算法。它被用来处理动态环境下从给定起始点到给定目标点的最短路径问题.

Koenig和Likhachev原论文

A*算法在此不再赘述, 请参考另一篇文章A* 路径搜索算法介绍及完整代码

2. 算法描述

网上有不少博客详细的介绍了论文中的符号和基本算法逻辑, 如:

LPA* 路径搜索算法介绍

终身规划A*算法（LPA*）：Lifelong Planning A*

在此就不重复介绍论文中的内容, 只强调几个可能困惑的点:

1) 父节点和子节点

子节点 (successors): 节点s的后续节点集合，代表s可以到达的点.

父节点 (predecessors)：节点s的前代节点，代表可以到达s的点.

在论文举例中, 由于是无向图, 所以都指向相邻节点.

2) 论文举例中的h (heuristic) 计算

论文中没有明确列出h值的计算, 从例子中可以看出移动时可以走斜线, 所以相邻点为八个方向:

[(0, 1), (0, -1), (1, 0), (-1, 0), (1, 1), (1, -1), (-1, 1), (-1, -1)]

h值的计算应该取x, y 差值的最大值

h = max(abs(a[0]-b[0]), abs(a[1]-b[1]))

3. 伪代码

4. Python代码实现

import numpy as np
import matplotlib.pyplot as plt
from collections import OrderedDict


class PriorityQueue:
    def __init__(self):
        self.queue = OrderedDict()

    def insert(self, vertex, key):
        self.queue[vertex] = key
        self.queue = OrderedDict(sorted(self.queue.items(), key=lambda kv: (kv[1][0], kv[1][1])))

    def popvertex(self):
        first_item = self.queue.popitem(last=False)
        return first_item[0]

    def topkey(self):
        if self.queue: