LPA*算法图文详解

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

LPA* 算法基本原理:

LPA* 保持每个单元的起始距离的两个估计,即g值和rhs值。g值直接对应于A*搜索的g值。rhs值是基于g值的一步前瞻值。rhs值是始终满足以下关系(基于g值):
LPA*算法图文详解_第1张图片

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

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

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

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

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

对于这两个情况,算法会分为两种不同的处理方式:
LPA*算法图文详解_第2张图片
对于局部过一致的点位,会将其g值更新为rhs值,并同时更新其周围点位信息。对于局部欠一致的点位,会将其g值更新为无穷大,然后更新其周围点的信息。对于这里的UpdateVertex函数,原论文是这么定义的:
LPA*算法图文详解_第3张图片
简单的来说,算法对于每个栅格的更新主要分为三步:

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函数对这些局部不一致栅格进行处理,使其变得局部一致,在这个过程中也就找到了一条新的路径。

整个算法的总体原理流程图如下:
LPA*算法图文详解_第4张图片

例子

讲了一堆,云里雾里,还是通过一个简单的例子来看一下这个问题:
LPA*算法图文详解_第5张图片假设我们有如上场景,左下角浅绿色为起点,蓝色格子为终点,红色为障碍物,初始时,采用A* 算法我们规划出一条路径如下:
LPA*算法图文详解_第6张图片同时我们得到每个栅格的g值与rhs值如下:
LPA*算法图文详解_第7张图片

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

局部过一致处理流程:

假设我们将(2,2)处的障碍物移除。首先算法会更新该点的rhs值。其值更新遵循文章最上面的公式。因此其rhs值会变为1.41:
LPA*算法图文详解_第8张图片

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

然后算法再次从U中取处点位(3,2),执行ComputeShortestPath函数:
LPA*算法图文详解_第10张图片这个过程点位(3,2)从集合U中删除,并将点位(3,3)加入集合。然后算法再次从U中取处点位(3,3),执行ComputeShortestPath函数:
LPA*算法图文详解_第11张图片

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

LPA*算法图文详解_第12张图片这个过程点位(3,4)从集合U中删除,并将点位(3,5)加入集合。然后算法再次从U中取处点位(3,5),执行ComputeShortestPath函数:
LPA*算法图文详解_第13张图片这个过程点位(3,5)从集合U中删除,并将点位(3,6)、点位(4,5)、点位(4,6)加入集合。然后算法再次从U中取处点位(4,5),执行ComputeShortestPath函数:
LPA*算法图文详解_第14张图片
这个过程点位(4,5)从集合U中删除,并将点位(5,5)、点位(5,6)加入集合。然后算法再次从U中取处点位(4,6),执行ComputeShortestPath函数:
LPA*算法图文详解_第15张图片这个过程点位(4,6)从集合U中删除,但是周围点的rhs值都被更新过了,所以这一步没有新的点位加入集合。然后算法再次从U中取处点位(5,5),执行ComputeShortestPath函数:
LPA*算法图文详解_第16张图片
将点位(5,5)从集合U中删除,将点位(6,6)加入集合。算法再次从U中取处点位(5,6),执行ComputeShortestPath函数:
LPA*算法图文详解_第17张图片将点位(5,6)从集合U中删除,算法再次从U中取处点位(6,6),执行ComputeShortestPath函数:
LPA*算法图文详解_第18张图片此时这些点都再次恢复了局部一致性,注意到其实在这个过程中还是存在局部不一致点位的,例如(3,6)一直没有更新。但是此时ComputeShortestPath函数已经不满足了,所以算法跳出了。该点也就不需要再次更新了。

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

最后,我们可以得到更新后的路径为:
LPA*算法图文详解_第19张图片

局部欠一致处理流程:

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

LPA*算法图文详解_第20张图片
然后我们将(2,2)添加为障碍物,此时该点的rhs值为inf,然后将点位添加到集合U中,执行ComputeShortestPath函数:

LPA*算法图文详解_第21张图片此时点(3,2)的rhs值被修改为3,该点变为局部不一致点位,加入集合U,同时从集合U中剔除点位(2,2),继续循环:
LPA*算法图文详解_第22张图片根据ComputeShortestPath函数算法首先更新点(3,2)的g值,由于这个点是局部欠一致点位,所以其g值应该直接更新为inf,同时更新点(3,2)周围点的rhs值:

LPA*算法图文详解_第23张图片然后更新点(3,3)的g值,同时更新点(3,3)周围点的rhs值:

LPA*算法图文详解_第24张图片然后更新点(3,2)的g值,同时更新点(4,3)周围点的rhs值:此时该点为过一致点,按照前面过一致点进行处理:
LPA*算法图文详解_第25张图片这样子,点(2,2)与点(3,2)都恢复了局部一致性,然后更新点(3,4):

LPA*算法图文详解_第26张图片然后处理点(3,3):
LPA*算法图文详解_第27张图片
然后处理点(3,5):
LPA*算法图文详解_第28张图片
然后处理点(3,4):
LPA*算法图文详解_第29张图片然后处理点(4,5):
LPA*算法图文详解_第30张图片然后处理点(4,6):
LPA*算法图文详解_第31张图片然后处理点(5,5):
LPA*算法图文详解_第32张图片然后处理点(5,6):
LPA*算法图文详解_第33张图片然后处理点(6,6):

LPA*算法图文详解_第34张图片
中间省略一部分迭代过程(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*算法图文详解_第35张图片

小结

总体来说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()

初始状态下规划结果为:
LPA*算法图文详解_第36张图片
消除某个障碍物:
LPA*算法图文详解_第37张图片
添加一个新的障碍物:
LPA*算法图文详解_第38张图片
参考:
1、《终身规划A算法(LPA):Lifelong Planning A*》
2、《LPA* 路径搜索算法介绍及完整代码》
3、《路径规划算法》

你可能感兴趣的:(路径规划算法,算法)