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 pltclass LPAStar:def __init__(self, s_start, s_goal, heuristic_type,xI, xG):self.xI, self.xG = xI, xGself.x_range = 51  # size of backgroundself.y_range = 31self.motions = [(-1, 0), (-1, 1), (0, 1), (1, 1),(1, 0), (1, -1), (0, -1), (-1, -1)]  # feasible input setself.obs = self.obs_map()self.s_start, self.s_goal = s_start, s_goalself.heuristic_type = heuristic_typeself.u_set = self.motionsself.obs = self.obsself.x = self.x_rangeself.y = self.y_rangeself.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] = 0self.U[self.s_start] = self.CalculateKey(self.s_start)self.visited = set()self.count = 0self.fig = plt.figure()def obs_map(self):"""Initialize obstacles' positions:return: map of obstacles"""x = 51y = 31obs = 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 obsdef update_obs(self, obs):self.obs = obsdef 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.ydataif 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 += 1if (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]:breakself.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 neighborsself.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_listdef h(self, s):"""Calculate heuristic.:param s: current node (state):return: heuristic function value"""heuristic_type = self.heuristic_type  # heuristic typegoal = self.s_goal  # goal nodeif 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 Trueif 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 Truereturn Falsedef extract_path(self):"""Extract the path based on the PARENT set.:return: The planning path"""path = [self.s_goal]s = self.s_goalfor 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:breakreturn 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 = 0for 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()