We have previously learned the basic principles of the Dijkstra and A* algorithms. For both algorithms, we can find a path from the starting point to the end point if there is a solution. However, both algorithms themselves are based on static maps, that is to say: when the robot finds the path and starts moving along the starting point to the end point, the default map will not change, but in fact most of the time we Knowing that the map is dynamic, if there is a sudden obstacle blocking the original path at any time, then the original path will be invalid at this time. A simple and crude way is of course to re-execute Dijkstra or A* search from the current point, but this will be very inefficient. For example, the obstacle only blocks the current position a little bit, so it is obviously not appropriate to perform a global search. , so is there any way to quickly and effectively find a suitable path? The answer is: D*
basic logic
The emergence of the D* algorithm itself is to solve dynamic path planning. Its basic idea is as follows:
1、初始时，使用Dijkstra算法寻找到一条从起点到达终点的路径。注意一点的是在传统的Dijkstra算法中，只维护了一个代价值f[s]，但是D* 中对于每个点维护了两个不同的代价值k[s]与h[s]。此外，注意D*的算法是从终点往起点搜索的，这点比较重要要注意一下，因为涉及到后面沿着parent的搜索。
2、机器人沿着找到的路径前进，假设在某个位置突然出现了一个障碍物。
3、算法将该点处的h值更新为无穷大（不可达），同时调用process_state处理状态，直到算法找到新的路径或者找完了发现没有路径跳出
Let's look down one by one, starting with the first item, the initial search. This basic principle is basically consistent with Dijkstra. The difference is that in order to maintain an accurate cost value of reaching the target point, D* seems to search from the end point to the starting point. In addition, regarding the definitions of k[s] and h[s], the original paper defines them as follows:
For each state X on the OPEN list (i.e., r ( X ) = OPEN ) , the key function,k(G, X ) , is defined to be equal to the minimum of h(G, X )
before modification and all values assumed by h(G, X ) since X was placed on the OPEN list.
That is, for each point, there are two values k[s] and h[s], and k[s] is equal to the minimum cost value of the point during the search process, that is, the smallest one encountered in h[s]. What's the use of this? We’ll talk about it later, so let’s continue reading.
After finding a path, the robot starts to move along this path until an obstacle is triggered, at which time the h value of that point will be modified. The modification rules are defined in the paper as follows:
a function insert is used. This function is not explained in detail in the paper. After reading some other people's articles, the unified logic is handled like this:
def insert(self, s, h_new):
"""
insert node into OPEN set.
:param s: node
:param h_new: new or better cost to come value
"""
if self.t[s] == 'NEW':
self.k[s] = h_new
elif self.t[s] == 'OPEN':
self.k[s] = min(self.k[s], h_new)
elif self.t[s] == 'CLOSED':
self.k[s] = min(self.h[s], h_new)
self.h[s] = h_new
self.t[s] = 'OPEN'
self.OPEN.add(s)
For a point on the path that is an obstacle, its h(x) is inf, which is infinite, so the algorithm here only changes the h value of the point without changing its k value. At the same time, this point is added to the open list, and then the algorithm The process_state function will be called for processing. Process_state is defined in the original paper:
This is the essence of the D* algorithm. When the algorithm enters this function, it first obtains the point with the smallest k value in the OPEN list and deletes the point from the OPEN list:
kold = GET - KMIN( ) ; DELETE(X)
The calculation of h(x) is consistent with the definition in the Dijkstra algorithm or the A* algorithm, and represents the cost value from the current point to the target point. When a point is identified as an obstacle, its h value will become inf, so the h value of the point must be greater than the K value. This state is defined in the paper as the RAISE state, that is, k (G, X)<h(G,X), and if k(G,X)=h(G,X), it is in LOWER state. The paper mentions:
If X is a RAISE state, its path cost may not be optimal.
D* uses the RAISE state on the OPEN list to propagate information about path cost increases, and the LOWER state to propagate information about path cost decreases. That is, if X is in RAISE state, its path cost may not be optimal. This is also easier to understand. Originally, k(s)=h(s) when planning was completed for the first time, but h(s) increases after encountering obstacles, that is, the cost of going to the target point at this point increases because of the obstacles. , then the path to this point at this time is not optimal in most cases, and will cause a collision.
At the same time, we will also start to search for a new path at and near the point based on this state. The search method is to cyclically update the information of each point and its nearby points in the open list according to the above process_state until it is satisfied:
if k_min >= self.h[s]:
Here k_min refers to the smallest k value in the open list, and s is the point where the obstacle was encountered before. Its value changed to inf during the execution of modify, but its value will be changed later in process_state. , and some points around it are also changed. The meaning here is that the algorithm starts a directional search from the point where it encounters the obstacle, and its search direction is biased towards the target point, because relatively speaking, the k of these points The smaller the value, the easier it is to search, thus avoiding some meaningless searches.
Example
Let’s look at this problem through a simple example:
Assume that there is currently a map like the one above, with the starting point coordinates being (5, 5) and the end point being (45, 25). At the beginning of the algorithm, the Dijkstra algorithm was used to find an optimal path. This The points passed by the path are:
[(5, 5), (6, 6), (7, 7), (8, 8), (9, 9), (9, 10), (9, 11), (9, 12), (9, 13), (9, 14), (9, 15), (9, 16), (10, 16), (11, 16), (12, 16), (13, 16), (14, 16), (15, 16), (16, 16), (17, 16), (18, 16), (19, 16), (20, 16), (21, 16), (22, 15), (23, 14), (24, 14), (25, 14), (26, 14), (27, 14), (28, 14), (29, 14), (30, 14), (31, 14), (32, 15), (33, 16), (34, 17), (35, 18), (36, 19), (37, 20), (38, 21), (39, 22), (40, 23), (41, 24), (42, 25), (43, 25), (44, 25), (45, 25)]
Then we insert an obstacle at the position (9, 10). At this time, the robot starts from the starting point and moves towards the end point. When the robot reaches (9, 9), it will find that the next point is an obstacle: the
upper right corner of the figure represents the current k value. At the beginning, the k value and the h value are the same, so the h value is not listed here. The direction of the arrow Represents the parent direction of the point.
At this time, the algorithm will call the modify function to modify the h value of (9, 9). Because the next point is an obstacle, the h value of (9, 9) will become inf.
The algorithm then calls process_state to handle this point. First, the algorithm will enter the first IF statement:
find a value from the vicinity of the current point that can make the current point smaller. Of course, since this point is currently a point on the optimal path, the k values of the surrounding points should be Bigger than that. So the if statement here will not be entered currently. The algorithm traverses the surrounding 8 points but does not change the parameters of the points. Then the algorithm enters the second IF statement:
We know earlier that the k value does not change when modifying the point information, so we will enter the ELSE part here and traverse the points around the current point again, but the judgment conditions here are different. . It will be judged based on two conditions: whether the parent of the point is (9, 9) and whether its value will be inconsistent with the previous one. For example, for point (8, 8), its current parent is (9, 9), so it will enter the second IF statement, but for (8, 9), its parent is not (9, 9), so it Will enter the second else.
These judgments will change the status of the eight neighboring points around the current point. Initially, the status of these points is:
(8,10) h: 47.798989873223334 parent: (9, 11) k: 47.798989873223334
(9,10) h: 47.38477631085024 parent: (9, 11) k: 47.38477631085024
(10,10) h: 47.798989873223334 parent: (9, 11) k: 47.798989873223334
(8,9) h: 48.798989873223334 parent: (9, 10) k: 48.798989873223334
(9,9) h: 48.38477631085024 parent: (9, 10) k: 48.38477631085024
(10,9) h: 48.798989873223334 parent: (9, 10) k: 48.798989873223334
(8,8) h: 49.798989873223334 parent: (9, 9) k: 49.798989873223334
(9,8) h: 49.38477631085024 parent: (9, 9) k: 49.38477631085024
(10,8) h: 49.798989873223334 parent: (9, 9) k: 49.798989873223334
After one traversal it becomes:
(8,10) h: 47.798989873223334 parent: (9, 11) k: 47.798989873223334
(9,10) h: 47.38477631085024 parent: (9, 11) k: 47.38477631085024
(10,10) h: 47.798989873223334 parent: (9, 11) k: 47.798989873223334
(8,9) h: 48.798989873223334 parent: (9, 10) k: 48.798989873223334
(9,9) h: inf parent: (9, 10) k: 48.38477631085024
(10,9) h: 48.798989873223334 parent: (9, 10) k: 48.798989873223334
(8,8) h: inf parent: (9, 9) k: 49.798989873223334
(9,8) h: inf parent: (9, 9) k: 49.38477631085024
(10,8) h: inf parent: (9, 9) k: 49.798989873223334
The light blue in the figure represents the points that have been changed during the current traversal. You can see that their h values have temporarily changed to inf.
At the same time, some of the points just traversed were inserted into the OPEN list, such as (8,9), and (8,10), (9,10), (10,10) will not be inserted into the OPEN list because they do not meet the conditions. , so now the smallest k value becomes (8,9).
Similarly, traverse the points around (8, 9), find and modify its value. At this time, due to changes in the h value of some points, their parent lines will change. For example, the parent line of (9, 9) will From (9, 10) to (8, 9), because (9, 10) is an obstacle, the cost value between them is infinite. Obviously, the cost value of going to (8, 9) will be much smaller, so (9, The next step of 9) was changed to (8,9).
Then continue to loop, the algorithm will traverse in sequence: (9, 9), (8, 9):
(10, 9）：
(9, 8)：
(8, 9)：
(8, 10)：
Wait for a series of sequences, and the final modified result is:
（9，9）的父系变成（8，9）
（8，9）的父系变为（8，10）
（8，10）的父系变为（8，11）
The new path is:
The parent line of (8,11) is (9,12), which bypasses the obstacle. At this point, a new path is planned:
Attached code:
import os
import sys
import math
import matplotlib.pyplot as plt
class DStar:
def __init__(self, s_start, s_goal, xI, xG):
self.s_start, self.s_goal = s_start, s_goal
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.xI, self.xG = xI, xG
self.u_set = self.motions
self.obs = self.obs
self.x = self.x_range
self.y = self.y_range
self.fig = plt.figure()
self.flag = True
self.OPEN = set()
self.t = dict()
self.PARENT = dict()
self.h = dict()
self.k = dict()
self.path = []
self.visited = set()
self.count = 0
def init(self):
for i in range(self.x_range):
for j in range(self.y_range):
self.t[(i, j)] = 'NEW'
self.k[(i, j)] = 0.0
self.h[(i, j)] = float("inf")
self.PARENT[(i, j)] = None
self.h[self.s_goal] = 0.0
def update_obs(self, obs):
self.obs = obs
def animation(self, path, visited, name):
self.plot_grid(name)
self.plot_visited(visited)
self.plot_path(path)
plt.show()
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_visited(self, visited, cl='gray'):
if self.xI in visited:
visited.remove(self.xI)
if self.xG in visited:
visited.remove(self.xG)
count = 0
for x in visited:
count += 1
plt.plot(x[0], x[1], color=cl, marker='o')
plt.gcf().canvas.mpl_connect('key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
if count < len(visited) / 3:
length = 20
elif count < len(visited) * 2 / 3:
length = 30
else:
length = 40
#
# length = 15
if count % length == 0:
plt.pause(0.001)
plt.pause(0.01)
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 obs_map(self):
"""
Initialize obstacles' positions
:return: map of obstacles
"""
x = self.x_range
y = self.y_range
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 run(self, s_start, s_end):
self.init()
self.insert(s_end, 0)
while True:
self.process_state()
if self.t[s_start] == 'CLOSED':
break
self.path = self.extract_path(s_start, s_end)
print(self.path)
self.plot_grid("Dynamic A* (D*)")
self.plot_path(self.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)
if (x, y) not in self.obs:
print("Add obstacle at: s =", x, ",", "y =", y)
self.obs.add((x, y))
self.update_obs(self.obs)
s = self.s_start
self.visited = set()
self.count += 1
self.flag = True
self.numb = 0
while s != self.s_goal:
#print("s is in:",s)
self.numb += 1
if self.numb > 1000:
print("no route")
break
if self.is_collision(s, self.PARENT[s]):
if self.flag == True:
self.flag = False
self.s_start = s
self.modify(s)
continue
s = self.PARENT[s]
self.path = self.extract_path(self.s_start, self.s_goal)
print(self.path)
plt.cla()
self.plot_grid("Dynamic A* (D*)")
self.plot_visited(self.visited)
self.plot_path(self.path)
self.fig.canvas.draw_idle()
def extract_path(self, s_start, s_end):
path = [s_start]
s = s_start
while True:
s = self.PARENT[s]
path.append(s)
if s == s_end:
return path
def process_state(self):
s = self.min_state() # get node in OPEN set with min k value
self.visited.add(s)
if s is None:
return -1 # OPEN set is empty
k_old = self.get_k_min() # record the min k value of this iteration (min path cost)
self.delete(s) # move state s from OPEN set to CLOSED set
self.t[s] = 'CLOSED'
# k_min < h[s] --> s: RAISE state (increased cost)
if k_old < self.h[s]:
for s_n in self.get_neighbor(s):
if self.h[s_n] <= k_old and \
self.h[s] > self.h[s_n] + self.cost(s_n, s):
# update h_value and choose parent
self.PARENT[s] = s_n
self.h[s] = self.h[s_n] + self.cost(s_n, s)
# s: k_min >= h[s] -- > s: LOWER state (cost reductions)
if k_old == self.h[s]:
for s_n in self.get_neighbor(s):
if self.t[s_n] == 'NEW' or \
(self.PARENT[s_n] == s and self.h[s_n] != self.h[s] + self.cost(s, s_n)) or \
(self.PARENT[s_n] != s and self.h[s_n] > self.h[s] + self.cost(s, s_n)):
# Condition:
# 1) t[s_n] == 'NEW': not visited
# 2) s_n's parent: cost reduction
# 3) s_n find a better parent
self.PARENT[s_n] = s
self.insert(s_n, self.h[s] + self.cost(s, s_n))
else:
for s_n in self.get_neighbor(s):
if self.t[s_n] == 'NEW' or \
(self.PARENT[s_n] == s and self.h[s_n] != self.h[s] + self.cost(s, s_n)):
# Condition:
# 1) t[s_n] == 'NEW': not visited
# 2) s_n's parent: cost reduction
self.PARENT[s_n] = s
self.insert(s_n, self.h[s] + self.cost(s, s_n))
else:
if self.PARENT[s_n] != s and \
self.h[s_n] > self.h[s] + self.cost(s, s_n):
# Condition: LOWER happened in OPEN set (s), s should be explored again
self.k[s] = self.h[s]
self.insert(s, self.h[s])
else:
if self.PARENT[s_n] != s and \
self.h[s] > self.h[s_n] + self.cost(s_n, s) and \
self.t[s_n] == 'CLOSED' and \
self.h[s_n] > k_old:
# Condition: LOWER happened in CLOSED set (s_n), s_n should be explored again
self.insert(s_n, self.h[s_n])
return self.get_k_min()
def min_state(self):
"""
choose the node with the minimum k value in OPEN set.
:return: state
"""
if not self.OPEN:
return None
return min(self.OPEN, key=lambda x: self.k[x])
def get_k_min(self):
"""
calc the min k value for nodes in OPEN set.
:return: k value
"""
if not self.OPEN:
return -1
return min([self.k[x] for x in self.OPEN])
def insert(self, s, h_new):
"""
insert node into OPEN set.
:param s: node
:param h_new: new or better cost to come value
"""
if self.t[s] == 'NEW':
self.k[s] = h_new
elif self.t[s] == 'OPEN':
self.k[s] = min(self.k[s], h_new)
elif self.t[s] == 'CLOSED':
self.k[s] = min(self.h[s], h_new)
self.h[s] = h_new
self.t[s] = 'OPEN'
self.OPEN.add(s)
def delete(self, s):
"""
delete: move state s from OPEN set to CLOSED set.
:param s: state should be deleted
"""
if self.t[s] == 'OPEN':
self.t[s] = 'CLOSED'
self.OPEN.remove(s)
def modify(self, s):
"""
start processing from state s.
:param s: is a node whose status is RAISE or LOWER.
"""
self.modify_cost(s)
while True:
if not self.OPEN:
print("no route")
break
k_min = self.process_state()
if k_min >= self.h[s]:
break
def modify_cost(self, s):
# if node in CLOSED set, put it into OPEN set.
# Since cost may be changed between s - s.parent, calc cost(s, s.p) again
if self.t[s] == 'CLOSED':
self.insert(s, self.h[self.PARENT[s]] + 100*self.cost(s, self.PARENT[s]))
def get_neighbor(self, s):
nei_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:
nei_list.add(s_next)
return nei_list
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:
return True
if 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 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():
s_start = (5, 5)
s_goal = (45, 25)
dstar = DStar(s_start, s_goal,s_start,s_goal)
dstar.run(s_start, s_goal)
if __name__ == '__main__':
main()
Notice:
1. The source code of the above code originally came from the code sharing of the article "Path Planning Algorithm ", but a problem was discovered during the use: the original paper mentioned that planning starts from the location of encountering obstacles. However, every time an obstacle is encountered in this article, the extraction path will still be extracted from the original starting point, so I found such a problem in the code in this article: when I continuously add obstacles near the same point When extracting the path from the initial starting point to the end point, there will be a problem:
In fact, its real path should be like this:
The reason for the previous problem is that it was not modified enough when traversing the points around the obstacle. The parent of the far point causes the path starting from the initial point to not be completely modified. I'm not sure whether it's an algorithm flaw in the D* algorithm itself or whether I still have some problems understanding the article about it.
2. Although the D* algorithm will re-plan the obstacle points on the path, it is unable to plan the path after the existing obstacles are eliminated. That is, if you put an obstacle on the path and the robot detours, and then you remove the obstacle, the robot will still detour next time and will not resume the original path. This is also a shortcoming of the D* algorithm.
But overall D* is indeed a very excellent algorithm.
Reference:
1. " Path Planning Algorithm "
2. " D* Path Search Algorithm Principle Analysis and Python Implementation "
3. " D* Algorithm Super Detailed Explanation "
4. " D* Algorithm Principle and Program Detailed Explanation (Python) "
5. " D *Algorithm (Dynamic A Star)》