We have seen the A* algorithm before and know the basic principles of the A* algorithm. However, the flaws of the A* algorithm are also obvious: it is an offline path planning algorithm and can only plan the path once, but if the path is changed later, it will It cannot take effect anymore. To solve this problem, people have developed the D* algorithm. The biggest advantage of the D* algorithm compared to the A* or Dijkstra algorithm is that it is real-time during the movement: if obstacles appear on the originally planned path, a new plan will be made for the current path. , new paths can be found through shorter iterations. But its bigger problem is that it can only handle the situation where obstacles appear on the blank path, but cannot handle the situation where the obstacle originally turned into a blank area. In response to this situation, the LPA* algorithm was born!
Basic principles of LPA* algorithm:
LPA* maintains two estimates of the starting distance of each cell, the g value and the rhs value. The g value corresponds directly to the g value of the A* search. The rhs value is a one-step lookahead based on the g value. The rhs value always satisfies the following relationship (based on the g value):
The starting cell has a rhs value of zero. The rhs value at any other location is the minimum of the neighbor's g value and the cost of moving from the surrounding grid to the target grid. Therefore, the g value of each raster is equal to its rhs value in the initial state. We call this lattice locally consistent. This concept is important because all g values are equal to the corresponding starting distance if all cells are locally consistent.
When the obstacles in the map change, the rhs value of the grid will change. At this time, the g value of the grid and the rhs value will become inconsistent. Rasters whose g values are inconsistent with rhs values are called locally inconsistent rasters. There are also two situations here:
1. The original obstacle grid has the obstacle removed. For this situation, we call it local over-consistency, that is:
g ( s ) > rhs ( s ) g(s) > rhs(s)g(s)>rhs(s)
2. New obstacles are added to the original blank grid. For this situation, we call it local lack of consistency, that is:
g ( s ) < rhs ( s ) g(s) < rhs(s)g(s)<rhs(s)
These two points are relatively easy to understand. For a point that is originally an obstacle, its rhs value and g value are originally infinite. After the obstacle is removed, the rhs value will be updated. According to the above formula, we can know that the value must be a certain value ( Unless the g values of all points around it are infinite). Similarly, originally the g value of a blank grid should be equal to the rhs value, but when it becomes an obstacle, its rhs value will also become infinite, thus satisfying the condition of local inconsistency.
For these two situations, the algorithm will be divided into two different processing methods:
for the locally consistent points, the g value will be updated to the rhs value, and the surrounding point information will be updated at the same time. For points that are locally inconsistent, their g value will be updated to infinity, and then the information of its surrounding points will be updated. For the UpdateVertex function here, the original paper is defined as follows:
Simply put, the algorithm updates each raster in three main steps:
1. Update the rhs value of this grid. Its value is the minimum value of the sum of its g value plus the h value from this grid to the target grid among all the surrounding grids.
2. If this grid belongs to the set U, then remove it from the U set, because each point that needs to be updated is selected from the set U, and the traversed points must be removed first.
3. If the g value of this point is inconsistent with the rhs value, re-insert it into the set U. Note that the U value of the inserted point should be different from the previous value. The calculation of the value of U here follows the following formula:
It can be seen that U is actually a set of two numbers. The first digit brings the one-step predicted value, and the second digit is the smaller of its g value and rhs value.
Simply put, the basic idea of LPA* is to first use the A* algorithm to find a path while maintaining two values of each raster: the g value and the rhs value. Then during the movement, if the obstacles of the map change, its rhs value is updated. At this time, the g value and the rhs value will be inconsistent, and this raster becomes a locally inconsistent raster. The algorithm then uses the ComputeShortestPath function to process these locally inconsistent rasters to make them locally consistent, and in the process a new path is found.
The overall principle flow chart of the entire algorithm is as follows:
example
After talking a lot, let’s look at this problem through a simple example:
Suppose we have the above scene, the light green in the lower left corner is the starting point, the blue grid is the end point, and the red is the obstacle. Initially, use Using the A* algorithm, we plan a path as follows:
At the same time, we obtain the g value and rhs value of each grid as follows:
The upper left corner is the g value and the upper right corner is the rsh value. Note that there are several points where the g value is inf but the rsh value is a certain value. This is because when a certain point is updated, the rsh value of its surrounding points will be updated, but the g value will not be updated until this point is updated. . And A* can be said to be directional, not all points will be traversed, so this problem will occur. But it does not affect the actual use of the algorithm.
Partial consistency processing flow:
Suppose we remove the obstacle at (2, 2). First, the algorithm will update the rhs value of the point. Its value update follows the formula at the top of the article. Therefore its rhs value will become 1.41:
The modified points above will be put into the set U and processed as inconsistent points. The algorithm will then enter the ComputeShortestPath function to update the g values and rhs values of these inconsistencies. The logic of extracting points when updating is to first select the point with the smallest value (min(g(s), rhs(s)) + h(s)) in the set U. Therefore, the first point taken is (2, 2). This point satisfies the local over-consistency condition, so its g value and rhs value are updated. Then the values of all adjacent points around it are updated:
two points are modified in this process. The first modification is the g value of point (2, 2), which is modified to be consistent with the rhs value. At this time, the point returns to a locally consistent point and will be removed from U. At the same time, because the rhs value of the previous point (3, 2) is modified to 2.41, which is smaller than its g value, the point becomes locally inconsistent. Point, join the set U.
Then the algorithm takes the point (3, 2) from U again and executes the ComputeShortestPath function:
in this process, the point (3, 2) is deleted from the set U and the point (3, 3) is added to the set. Then the algorithm takes the point (3, 3) from U again and executes the ComputeShortestPath function:
In this process, point (3, 3) is deleted from the set U and point (3, 4) is added to the set. Then the algorithm takes the point (3, 4) from U again and executes the ComputeShortestPath function:
In this process, point (3, 4) is deleted from the set U and point (3, 5) is added to the set. Then the algorithm takes point (3, 5) from U again and executes the ComputeShortestPath function: in
this process, point (3, 5) is deleted from the set U, and point (3, 6), point (4, 5), point (4, 6) is added to the set. Then the algorithm takes point (4, 5) from U again and executes the ComputeShortestPath function: in
this process, point (4, 5) is deleted from the set U, and point (5, 5), point (5, 6) Join the collection. Then the algorithm takes the point (4, 6) from U again and executes the ComputeShortestPath function: In
this process, the point (4, 6) is deleted from the set U, but the rhs values of the surrounding points have been updated, so this step No new points are added to the set. Then the algorithm takes the point (5, 5) from U again and executes the ComputeShortestPath function:
deletes the point (5, 5) from the set U and adds the point (6, 6) to the set. The algorithm again takes the point (5, 6) from U and executes the ComputeShortestPath function:
deletes the point (5, 6) from the set U. The algorithm again takes the point (6, 6) from U and executes the ComputeShortestPath function. Function:
At this time, these points have restored local consistency again. It is noted that there are still local inconsistent points in this process. For example, (3, 6) has not been updated. But at this time the ComputeShortestPath function is no longer satisfied, so the algorithm jumps out. This point does not need to be updated again.
Regarding the judgment here, the paper defines it this way:
So for point (3, 6), its value should be [9.41, 6.41]. 9.41 is the value of rhs(s)+h(s). The value of the end point is 8.82 which is less than 9.41, so no matter how optimized the point (3, 6) is, it cannot be shorter than the current path, so there is no need to update it.
Finally, we can get the updated path as:
Local lack of consistency processing flow:
After looking at the partially consistent process, let's take a look at the partially inconsistent process. Modify the initial map and assume that the (2, 2) point is blank initially, and the map path is obtained:
Then we add (2, 2) as an obstacle. At this time, the rhs value of the point is inf, then add the point to the set U and execute the ComputeShortestPath function:
At this time, the rhs value of point (3, 2) is modified to 3, the point becomes a locally inconsistent point, and is added to the set U. At the same time, the point (2, 2) is removed from the set U, and the cycle continues: according to the ComputeShortestPath function
algorithm First update the g value of point (3, 2). Since this point is a locally inconsistent point, its g value should be directly updated to inf, and at the same time update the rhs values of points around point (3, 2):
Then update the g value of point (3, 3), and update the rhs values of points around point (3, 3):
Then update the g value of point (3, 2), and update the rhs values of points around point (4, 3): At this time, the point is an over-consistent point, and it will be processed according to the previous over-consistent point: In this way, point (2
, 2) Local consistency is restored with point (3, 2), and then point (3, 4) is updated:
Then process point (3, 3):
Then process point (3, 5):
Then process point (3, 4):
Then process point (4, 5):
Then process point (4, 6):
Then process point (5 , 5):
Then process point (5, 6):
Then process point (6, 6):
Omit part of the iterative process (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), the final result is:
summary
Generally speaking, the idea of the LPA* algorithm is to use two lists to maintain the value of each raster. When the two values are inconsistent, the raster is called locally inconsistent, and the value of each raster is updated according to the ComputeShortestPath function. For the original The situation where the obstacle is eliminated is called a local over-consistent point, which is easier to handle. The original blank point that becomes an obstacle is called a local under-consistent point. The processing situation will be a little more complicated. First, the point will become For local consistent points, it will be processed again according to the processing method of local consistent points, and finally the consistency of the points will be achieved. At the same time, the traversal principle for inconsistent points will be similar to the traversal method of the A* algorithm, which will improve the accuracy of the points. Traversal efficiency.
Code:
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()
In the initial state, the planning result is:
Eliminate an obstacle:
Add a new obstacle:
Reference:
1. " Lifelong Planning A Algorithm (LPA ): Lifelong Planning A* "
2. " LPA* path search algorithm introduction and complete code》
3. " Path Planning Algorithm "