LPA*算法图文详解

之前我们看过了A* 算法，知道了A* 算法的基本原理，但是A* 算法的缺陷也很明显：它是离线的路径规划算法，只能一次规划出路径，但是后面路径被改变的话就无法生效了。针对这个问题，人们研究出了D* 算法。D* 算法相对于A* 也好还是Dijkstra算法也好它最大的优点就是再于它在运动过程中是实时的：在原先规划的路径上如果出现障碍物的话，会对当前路径进行新的规划，通过较短的迭代即可找到新的路径。但是它比较大的问题在于它只能处理空白路径上出现障碍物的情况，而不能处理本来是障碍物但是变成空白区域的情况。而针对这种情况，LPA* 算法诞生啦！

LPA* 算法基本原理：

LPA* 保持每个单元的起始距离的两个估计，即g值和rhs值。g值直接对应于A*搜索的g值。rhs值是基于g值的一步前瞻值。rhs值是始终满足以下关系（基于g值）：
在这里插入图片描述

起始单元格的rhs值为零。任何其他位置的rhs值是邻居的g值和从周围格子移动到目标格子的成本的最小值。因此，在初始状态下每个栅格的g值等于其rhs值。我们称这种格子为局部一致的。这个概念很重要，因为所有g值都等于相应的起始距离，如果所有单元都是局部一致的。

当地图中障碍物发生变化的时候，该格子的rhs值会发生改变。此时格子的g值跟rhs值会变的不一致。对于g值跟rhs值不一致的栅格，称为局部不一致栅格。这里同样分为两种情况：

1、原来是障碍物的栅格移除了障碍物，对于这种情况，我们称之为局部过一致，即：
$g (s) > r h s (s)$

2、原来空白的栅格添加了新的障碍物，对于这种情况，我们称之为局部欠一致，即：
$g (s) < r h s (s)$

对于这两点，还是比较好理解的。对于原来是障碍物的点，它的rhs值以及g值本来都是无穷大，在将障碍物移除后，会更新rhs值，则根据上面的公式我们可以知道该值一定是一个确定的值（除非它周围所有点的g值都是无穷）。而同样的，本来一个空白的格子其g值应该等与rhs值，但是当其变为障碍物时，它的rhs值也会变成无穷，也就满足了局部欠一致的条件。

对于这两个情况，算法会分为两种不同的处理方式：
在这里插入图片描述
对于局部过一致的点位，会将其g值更新为rhs值，并同时更新其周围点位信息。对于局部欠一致的点位，会将其g值更新为无穷大，然后更新其周围点的信息。对于这里的UpdateVertex函数，原论文是这么定义的：
在这里插入图片描述
简单的来说，算法对于每个栅格的更新主要分为三步：

1、更新这个格子的rhs值，它的值为其周围所有栅格中，其g值加上这个栅格到目标栅格的h值之和的最小值。

2、如果这个栅格属于集合U，则将其移除U集合，因为每次需要更新的点位都是从集合U中选取的，对于遍历过的点位自然要先移除。

3、如果这个点位的g值跟rhs值不一致，则重新将其插入集合U。注意插入后的点的U的值跟之前的值应该是不一样的。这里关于U的值的计算遵循下述公式：

在这里插入图片描述
可以看到U其实是个两个数的集合，第一位数带了一步预测值，第二位数为其g值与rhs值中较小的那一个。

简单的来说，LPA* 的基本思想就是，首先利用A* 算法找到一条路径，同时维护每个栅格的两个值：g值与rhs值。然后在运动过程中，如果地图的障碍物发生变化，则更新其rhs值。此时g值与rhs值会不一致，这种栅格成为局部不一致栅格。然后算法通过ComputeShortestPath函数对这些局部不一致栅格进行处理，使其变得局部一致，在这个过程中也就找到了一条新的路径。

整个算法的总体原理流程图如下：
在这里插入图片描述

例子

讲了一堆，云里雾里，还是通过一个简单的例子来看一下这个问题：
在这里插入图片描述假设我们有如上场景，左下角浅绿色为起点，蓝色格子为终点，红色为障碍物，初始时，采用A* 算法我们规划出一条路径如下：
同时我们得到每个栅格的g值与rhs值如下：

其中左上角为g值右上角为rsh值。注意这里有几个点的g值为inf但是rsh值是确定值，这里是因为在更新某个点的时候会更新其周围点的rsh值，但是g值则需要更新这个点的时候才会更新。而A* 可以说是带有方向性的，不是所有点都会被遍历，因此会出现这个问题。但是不影响算法实际的使用。

局部过一致处理流程：

假设我们将（2，2）处的障碍物移除。首先算法会更新该点的rhs值。其值更新遵循文章最上面的公式。因此其rhs值会变为1.41：
在这里插入图片描述

对于上面被修改过的点，会被放入集合U作为不一致点进行处理。然后算法会进入ComputeShortestPath函数，更新这些不一致点的g值与rhs值。更新时取出点的逻辑为先取处集合U中（min(g(s), rhs(s)) + h(s))值最小的点。因此首先被取处的是（2，2）,该点满足局部过一致条件，因此更新其g值等与rhs值。然后再更新其周围所有相邻点的值：
在这里插入图片描述在这个过程中有两个点被修改了，首先修改的是（2，2）点的g值，其被修改为与rhs值一致。此时该点位回归为局部一致点位，会将其从U中剔除，同时由于之前的点位（3，2）的rhs值被修改为2.41小于其g值，所以该点变为局部不一致点位，加入集合U。

然后算法再次从U中取处点位（3，2），执行ComputeShortestPath函数：
在这里插入图片描述这个过程点位（3，2）从集合U中删除，并将点位（3，3）加入集合。然后算法再次从U中取处点位（3，3），执行ComputeShortestPath函数：

这个过程点位（3，3）从集合U中删除，并将点位（3，4）加入集合。然后算法再次从U中取处点位（3，4），执行ComputeShortestPath函数：

在这里插入图片描述这个过程点位（3，4）从集合U中删除，并将点位（3，5）加入集合。然后算法再次从U中取处点位（3，5），执行ComputeShortestPath函数：
这个过程点位（3，5）从集合U中删除，并将点位（3，6）、点位（4，5）、点位（4，6）加入集合。然后算法再次从U中取处点位（4，5），执行ComputeShortestPath函数：
在这里插入图片描述
这个过程点位（4，5）从集合U中删除，并将点位（5，5）、点位（5，6）加入集合。然后算法再次从U中取处点位（4，6），执行ComputeShortestPath函数：
这个过程点位（4，6）从集合U中删除，但是周围点的rhs值都被更新过了，所以这一步没有新的点位加入集合。然后算法再次从U中取处点位（5，5），执行ComputeShortestPath函数：
在这里插入图片描述
将点位（5，5）从集合U中删除，将点位（6，6）加入集合。算法再次从U中取处点位（5，6），执行ComputeShortestPath函数：
将点位（5，6）从集合U中删除，算法再次从U中取处点位（6，6），执行ComputeShortestPath函数：
在这里插入图片描述此时这些点都再次恢复了局部一致性，注意到其实在这个过程中还是存在局部不一致点位的，例如（3，6）一直没有更新。但是此时ComputeShortestPath函数已经不满足了，所以算法跳出了。该点也就不需要再次更新了。

关于这里的判断，论文中是这么定义的：
在这里插入图片描述
所以对于点（3，6）而言，它的值应该为[9.41，6.41]。9.41为rhs(s)+h(s)的值。而终点的值为8.82小于9.41，所以（3，6）这个点怎么优化都不可能比现在的路径更短，自然不需要再更新了。

最后，我们可以得到更新后的路径为：
在这里插入图片描述

局部欠一致处理流程：

看完局部过一致流程，接下来我们再看一下局部欠一致处理流程。修改初始地图，假设初始时（2，2）点位为空白，得到地图路径为：

在这里插入图片描述
然后我们将（2，2）添加为障碍物，此时该点的rhs值为inf，然后将点位添加到集合U中，执行ComputeShortestPath函数：

在这里插入图片描述此时点（3，2）的rhs值被修改为3，该点变为局部不一致点位，加入集合U，同时从集合U中剔除点位（2，2）,继续循环：
根据ComputeShortestPath函数算法首先更新点（3，2）的g值，由于这个点是局部欠一致点位，所以其g值应该直接更新为inf，同时更新点（3，2）周围点的rhs值：

在这里插入图片描述然后更新点（3，3）的g值，同时更新点（3，3）周围点的rhs值：

在这里插入图片描述然后更新点（3，2）的g值，同时更新点（4，3）周围点的rhs值：此时该点为过一致点，按照前面过一致点进行处理：
这样子，点（2，2）与点（3，2）都恢复了局部一致性，然后更新点（3，4）：

在这里插入图片描述然后处理点（3，3）：

然后处理点（3，5）：

然后处理点（3，4）：
然后处理点（4，5）：
然后处理点（4，6）：
然后处理点（5，5）：
然后处理点（5，6）：
然后处理点（6，6）：

在这里插入图片描述
中间省略一部分迭代过程（1，5）》（5，1）》（5，2）》（3，5）》（5，3）》（4，5）》(5，4）》（6，2）》（6，3）》（4，6）》（6，4）》（5，5）》（5，6）》（6，5）》（6，6），得到最终结果为：

小结

总体来说LPA* 算法的思路就是使用两个列表维护每个栅格的值，当两个值不一致时，称该栅格为局部不一致，则根据ComputeShortestPath函数更新每个栅格的值，对于原来是障碍物后面消除的情况称为局部过一致点，处理起来会比较容易，对于原来是空白点位后面变成障碍物的称为局部欠一致点，处理情况会略微复杂一点，首先会将点变为局部过一致点，再按照局部过一致点的处理方式再处理一遍，最后达到点位的一致性，同时对于不一致点的遍历原则会类似于A* 算法的遍历方式，这样会提高点位的遍历效率。

代码实现：

import os
import sys
import math
import matplotlib.pyplot as plt

class LPAStar:
    def __init__(self, s_start, s_goal, heuristic_type,xI, xG):
        self.xI, self.xG = xI, xG
        self.x_range = 51  # size of background
        self.y_range = 31
        self.motions = [(-1, 0), (-1, 1), (0, 1), (1, 1),
                        (1, 0), (1, -1), (0, -1), (-1, -1)]  # feasible input set
        self.obs = self.obs_map()
        self.s_start, self.s_goal = s_start, s_goal
        self.heuristic_type = heuristic_type
        self.u_set = self.motions
        self.obs = self.obs
        self.x = self.x_range
        self.y = self.y_range

        self.g, self.rhs, self.U = {}, {}, {}

        for i in range(self.x_range):
            for j in range(self.y_range):
                self.rhs[(i, j)] = float("inf")
                self.g[(i, j)] = float("inf")

        self.rhs[self.s_start] = 0
        self.U[self.s_start] = self.CalculateKey(self.s_start)
        self.visited = set()
        self.count = 0

        self.fig = plt.figure()

    def obs_map(self):
        """
        Initialize obstacles' positions
        :return: map of obstacles
        """

        x = 51
        y = 31
        obs = set()

        for i in range(x):
            obs.add((i, 0))
        for i in range(x):
            obs.add((i, y - 1))

        for i in range(y):
            obs.add((0, i))
        for i in range(y):
            obs.add((x - 1, i))

        for i in range(10, 21):
            obs.add((i, 15))
        for i in range(15):
            obs.add((20, i))

        for i in range(15, 30):
            obs.add((30, i))
        for i in range(16):
            obs.add((40, i))

        return obs
    def update_obs(self, obs):
        self.obs = obs


    def plot_grid(self, name):
        obs_x = [x[0] for x in self.obs]
        obs_y = [x[1] for x in self.obs]

        plt.plot(self.xI[0], self.xI[1], "bs")
        plt.plot(self.xG[0], self.xG[1], "gs")
        plt.plot(obs_x, obs_y, "sk")
        plt.title(name)
        plt.axis("equal")
    def plot_path(self, path, cl='r', flag=False):
        path_x = [path[i][0] for i in range(len(path))]
        path_y = [path[i][1] for i in range(len(path))]

        if not flag:
            plt.plot(path_x, path_y, linewidth='3', color='r')
        else:
            plt.plot(path_x, path_y, linewidth='3', color=cl)

        plt.plot(self.xI[0], self.xI[1], "bs")
        plt.plot(self.xG[0], self.xG[1], "gs")

        plt.pause(0.01)


    def run(self):
        self.plot_grid("Lifelong Planning A*")

        self.ComputeShortestPath()
        self.plot_path(self.extract_path())
        self.fig.canvas.mpl_connect('button_press_event', self.on_press)

        plt.show()

    def on_press(self, event):
        x, y = event.xdata, event.ydata
        if x < 0 or x > self.x - 1 or y < 0 or y > self.y - 1:
            print("Please choose right area!")
        else:
            x, y = int(x), int(y)
            print("Change position: s =", x, ",", "y =", y)

            self.visited = set()
            self.count += 1

            if (x, y) not in self.obs:
                self.obs.add((x, y))
                self.UpdateVertex((x, y))
            else:
                self.obs.remove((x, y))
                self.UpdateVertex((x, y))

            self.update_obs(self.obs)

            #for s_n in self.get_neighbor((x, y)):
            #    self.UpdateVertex(s_n)

            self.ComputeShortestPath()
            
            plt.cla()
            self.plot_grid("Lifelong Planning A*")
            self.plot_visited(self.visited)
            self.plot_path(self.extract_path())
            self.fig.canvas.draw_idle()

    def ComputeShortestPath(self):
        while True:
            s, v = self.TopKey()
            if v >= self.CalculateKey(self.s_goal) and \
                    self.rhs[self.s_goal] == self.g[self.s_goal]:
                break

            self.U.pop(s)
            self.visited.add(s)
            
            if self.g[s] > self.rhs[s]:

                # Condition: over-consistent (eg: deleted obstacles)
                # So, rhs[s] decreased -- > rhs[s] < g[s]
                self.g[s] = self.rhs[s]
            else:

                # Condition: # under-consistent (eg: added obstacles)
                # So, rhs[s] increased --> rhs[s] > g[s]
                self.g[s] = float("inf")
                self.UpdateVertex(s)

            for s_n in self.get_neighbor(s):
                self.UpdateVertex(s_n)

    def UpdateVertex(self, s):
        """
        update the status and the current cost to come of state s.
        :param s: state s
        """

        if s != self.s_start:

            # Condition: cost of parent of s changed
            # Since we do not record the children of a state, we need to enumerate its neighbors
            self.rhs[s] = min(self.g[s_n] + self.cost(s_n, s)
                              for s_n in self.get_neighbor(s))

        if s in self.U:
            self.U.pop(s)

        if self.g[s] != self.rhs[s]:

            # Condition: current cost to come is different to that of last time
            # state s should be added into OPEN set (set U)
            self.U[s] = self.CalculateKey(s)
            #print(self.U[s])

    def TopKey(self):
        """
        :return: return the min key and its value.
        """

        s = min(self.U, key=self.U.get)

        return s, self.U[s]

    def CalculateKey(self, s):

        return [min(self.g[s], self.rhs[s]) + self.h(s),
                min(self.g[s], self.rhs[s])]

    def get_neighbor(self, s):
        """
        find neighbors of state s that not in obstacles.
        :param s: state
        :return: neighbors
        """

        s_list = set()

        for u in self.u_set:
            s_next = tuple([s[i] + u[i] for i in range(2)])
            if s_next not in self.obs:
                s_list.add(s_next)

        return s_list

    def h(self, s):
        """
        Calculate heuristic.
        :param s: current node (state)
        :return: heuristic function value
        """

        heuristic_type = self.heuristic_type  # heuristic type
        goal = self.s_goal  # goal node

        if heuristic_type == "manhattan":
            return abs(goal[0] - s[0]) + abs(goal[1] - s[1])
        else:
            return math.hypot(goal[0] - s[0], goal[1] - s[1])

    def cost(self, s_start, s_goal):
        """
        Calculate Cost for this motion
        :param s_start: starting node
        :param s_goal: end node
        :return:  Cost for this motion
        :note: Cost function could be more complicate!
        """

        if self.is_collision(s_start, s_goal):
            return float("inf")

        return math.hypot(s_goal[0] - s_start[0], s_goal[1] - s_start[1])

    def is_collision(self, s_start, s_end):
        if s_start in self.obs or s_end in self.obs:
            return True

        if s_start[0] != s_end[0] and s_start[1] != s_end[1]:
            if s_end[0] - s_start[0] == s_start[1] - s_end[1]:
                s1 = (min(s_start[0], s_end[0]), min(s_start[1], s_end[1]))
                s2 = (max(s_start[0], s_end[0]), max(s_start[1], s_end[1]))
            else:
                s1 = (min(s_start[0], s_end[0]), max(s_start[1], s_end[1]))
                s2 = (max(s_start[0], s_end[0]), min(s_start[1], s_end[1]))

            if s1 in self.obs or s2 in self.obs:
                return True

        return False

    def extract_path(self):
        """
        Extract the path based on the PARENT set.
        :return: The planning path
        """

        path = [self.s_goal]
        s = self.s_goal

        for k in range(100):
            g_list = {}
            for x in self.get_neighbor(s):
                if not self.is_collision(s, x):
                    g_list[x] = self.g[x]
            s = min(g_list, key=g_list.get)
            path.append(s)
            if s == self.s_start:
                break

        return list(reversed(path))

    def plot_path(self, path):
        px = [x[0] for x in path]
        py = [x[1] for x in path]
        plt.plot(px, py, linewidth=2)
        plt.plot(self.s_start[0], self.s_start[1], "bs")
        plt.plot(self.s_goal[0], self.s_goal[1], "gs")

    def plot_visited(self, visited):
        color = ['gainsboro', 'lightgray', 'silver', 'darkgray',
                 'bisque', 'navajowhite', 'moccasin', 'wheat',
                 'powderblue', 'skyblue', 'lightskyblue', 'cornflowerblue']

        if self.count >= len(color) - 1:
            self.count = 0

        for x in visited:
            plt.plot(x[0], x[1], marker='s', color=color[self.count])


def main():
    x_start = (5, 5)
    x_goal = (45, 25)

    lpastar = LPAStar(x_start, x_goal, "Euclidean",x_start,x_goal)
    lpastar.run()


if __name__ == '__main__':
    main()