Overview of path planning
1. Background introduction
Path planning refers to the process of finding the best path from a starting point to an end point in a given environment. It has a wide range of applications in real life, including driverless driving, logistics distribution, robot navigation and other fields. With the development of artificial intelligence and computer technology, path planning technology is constantly being improved and applied.
Common algorithms in path planning can be divided into two categories: search-based planning and sampling-based planning.
Search-based planning includes
- Breadth-First Searching (BFS)、
- Depth-First Searching (DFS)、
- Best-First Searching、
- Dijkstra's、
- A*、
- Bidirectional A*、
- Anytime Repairing A*、
- Learning Real-time A* (LRTA*)、
- Real-time Adaptive A* (RTAA*)、
- Lifelong Planning A* (LPA*)、
- Dynamic A* (D*)、
- D* Lite and
- Anytime D*
and other algorithms. These algorithms find the shortest or optimal path by searching the graph structure, among which A* is one of the most commonly used and classic algorithms.
Comparison of advantages and disadvantages
- Advantages of BFS (Breadth-First Searching)
: can find the shortest path; suitable for unweighted graphs.
Disadvantages: high time complexity; high space complexity.
- Advantages of DFS (Depth-First Searching)
: low space complexity.
Disadvantages: It may fall into an infinite loop; it may not be possible to find the shortest path.
- Advantages of Best-First Searching
: fast; can process heuristic information.
Disadvantage: May fall into local optimal solution.
- Dijkstra’s
advantages: can find the shortest path; suitable for weighted graphs.
Disadvantages: high time complexity; cannot handle negative weight edges.
- A*
advantages: fast; can process heuristic information; can find the shortest path.
Disadvantage: May fall into local optimal solution.
- Bidirectional A*
Advantages: fast speed; can find the shortest path.
Disadvantages: Two search trees need to be stored; problems may arise, such as the search space being too large or the search tree growing too slowly.
- Anytime Repairing A*
Advantages: The search can be stopped at any time and the best path can be returned; heuristic information can be processed.
Disadvantage: May fall into local optimal solution.
- LRTA* (Learning Real-time A*)
advantages: can handle dynamic environments; can handle heuristic information.
Disadvantages: Requires real-time calculations, which may cause performance issues.
- RTAA* (Real-time Adaptive A*)
advantages: can handle dynamic environments; can handle heuristic information.
Disadvantages: Requires real-time calculations, which may cause performance issues.
- LPA* (Lifelong Planning A*)
advantages: can search in different time periods; can process heuristic information.
Disadvantages: Need to store a large number of search trees.
class Node:
def __init__(self, n):
self.x = n[0]
self.y = n[1]
self.parent = None
class RrtStarSmart:
def __init__(self, x_start, x_goal, step_len,
goal_sample_rate, search_radius, iter_max):
self.x_start = Node(x_start)
self.x_goal = Node(x_goal)
self.step_len = step_len
self.goal_sample_rate = goal_sample_rate
self.search_radius = search_radius
self.iter_max = iter_max
self.env = env.Env()
self.plotting = plotting.Plotting(x_start, x_goal)
self.utils = utils.Utils()
self.fig, self.ax = plt.subplots()
self.delta = self.utils.delta
self.x_range = self.env.x_range
self.y_range = self.env.y_range
self.obs_circle = self.env.obs_circle
self.obs_rectangle = self.env.obs_rectangle
self.obs_boundary = self.env.obs_boundary
self.V = [self.x_start]
self.beacons = []
self.beacons_radius = 2
self.direct_cost_old = np.inf
self.obs_vertex = self.utils.get_obs_vertex()
self.path = None
def planning(self):
n = 0
b = 2
InitPathFlag = False
self.ReformObsVertex()
for k in range(self.iter_max):
if k % 200 == 0:
print(k)
if (k - n) % b == 0 and len(self.beacons) > 0:
x_rand = self.Sample(self.beacons)
else:
x_rand = self.Sample()
x_nearest = self.Nearest(self.V, x_rand)
x_new = self.Steer(x_nearest, x_rand)
if x_new and not self.utils.is_collision(x_nearest, x_new):
X_near = self.Near(self.V, x_new)
self.V.append(x_new)
if X_near:
# choose parent
cost_list = [self.Cost(x_near) + self.Line(x_near, x_new) for x_near in X_near]
x_new.parent = X_near[int(np.argmin(cost_list))]
# rewire
c_min = self.Cost(x_new)
for x_near in X_near:
c_near = self.Cost(x_near)
c_new = c_min + self.Line(x_new, x_near)
if c_new < c_near:
x_near.parent = x_new
if not InitPathFlag and self.InitialPathFound(x_new):
InitPathFlag = True
n = k
if InitPathFlag:
self.PathOptimization(x_new)
if k % 5 == 0:
self.animation()
self.path = self.ExtractPath()
self.animation()
plt.plot([x for x, _ in self.path], [y for _, y in self.path], '-r')
plt.pause(0.01)
plt.show()
def PathOptimization(self, node):
direct_cost_new = 0.0
node_end = self.x_goal
while node.parent:
node_parent = node.parent
if not self.utils.is_collision(node_parent, node_end):
node_end.parent = node_parent
else:
direct_cost_new += self.Line(node, node_end)
node_end = node
node = node_parent
if direct_cost_new < self.direct_cost_old:
self.direct_cost_old = direct_cost_new
self.UpdateBeacons()
def UpdateBeacons(self):
node = self.x_goal
beacons = []
while node.parent:
near_vertex = [v for v in self.obs_vertex
if (node.x - v[0]) ** 2 + (node.y - v[1]) ** 2 < 9]
if len(near_vertex) > 0:
for v in near_vertex:
beacons.append(v)
node = node.parent
self.beacons = beacons
def ReformObsVertex(self):
obs_vertex = []
for obs in self.obs_vertex:
for vertex in obs:
obs_vertex.append(vertex)
self.obs_vertex = obs_vertex
def Steer(self, x_start, x_goal):
dist, theta = self.get_distance_and_angle(x_start, x_goal)
dist = min(self.step_len, dist)
node_new = Node((x_start.x + dist * math.cos(theta),
x_start.y + dist * math.sin(theta)))
node_new.parent = x_start
return node_new
def Near(self, nodelist, node):
n = len(self.V) + 1
r = 50 * math.sqrt((math.log(n) / n))
dist_table = [(nd.x - node.x) ** 2 + (nd.y - node.y) ** 2 for nd in nodelist]
X_near = [nodelist[ind] for ind in range(len(dist_table)) if dist_table[ind] <= r ** 2 and
not self.utils.is_collision(node, nodelist[ind])]
return X_near
def Sample(self, goal=None):
if goal is None:
delta = self.utils.delta
goal_sample_rate = self.goal_sample_rate
if np.random.random() > goal_sample_rate:
return Node((np.random.uniform(self.x_range[0] + delta, self.x_range[1] - delta),
np.random.uniform(self.y_range[0] + delta, self.y_range[1] - delta)))
return self.x_goal
else:
R = self.beacons_radius
r = random.uniform(0, R)
theta = random.uniform(0, 2 * math.pi)
ind = random.randint(0, len(goal) - 1)
return Node((goal[ind][0] + r * math.cos(theta),
goal[ind][1] + r * math.sin(theta)))
def SampleFreeSpace(self):
delta = self.delta
if np.random.random() > self.goal_sample_rate:
return Node((np.random.uniform(self.x_range[0] + delta, self.x_range[1] - delta),
np.random.uniform(self.y_range[0] + delta, self.y_range[1] - delta)))
return self.x_goal
- D* (Dynamic A*)
advantages: can handle dynamic environments; can handle heuristic information.
Disadvantages: Need to store a large number of search trees.
- Advantages of D* Lite
: it can handle dynamic environments; it can handle heuristic information; it has low space complexity.
Disadvantage: May fall into local optimal solution.
- Anytime D*
advantages: can stop the search at any time and return the best path; can handle dynamic environments; can handle heuristic information.
Disadvantage: May fall into local optimal solution.
Sampling-based planning uses random sampling to generate paths.
One of the most common algorithms is
-
RRT、
-
RRT-Connect、
-
Extended-RRT、
-
Dynamic-RRT、
-
RRT*、
-
Informed RRT*、
-
RRT* Smart、
-
Anytime RRT*、
-
Closed-Loop RRT*、
-
Spline-RRT*、
-
Fast Marching Trees (FMT*) 和
-
Batch Informed Trees (BIT*)
and other algorithms. These algorithms are suitable for path planning in complex environments, such as robot navigation, driverless driving and logistics distribution.
Advantages and Disadvantages
- RRT (Rapidly-Exploring Random Trees)
advantages: suitable for high-dimensional space; can effectively handle complex environments; fast operation speed.
Disadvantages: There is no guarantee that the optimal solution will be found; the generated path may not be the shortest path.
- Advantages of RRT-Connect
: It can ensure that feasible paths can be found; it is suitable for multi-robot path planning problems.
Disadvantages: Path quality may be poor; convergence may be slow.
- Advantages of Extended-RRT
: It can handle non-holonomic dynamic systems; it is suitable for multi-robot collaborative planning.
Disadvantages: Path quality may be poor; operation speed is slower.
- Advantages of Dynamic-RRT
: It can handle path planning problems in dynamic environments; it is suitable for mobile robots, drones and other fields.
Disadvantages: Slow operation speed; path quality may be poor.
- RRT* (Rapidly-Exploring Random Trees Star)
Advantages: It can find the optimal path; the path quality is high.
Disadvantages: Slow operation speed; may require a large amount of storage space.
- Informed RRT*
Advantages: Combined with heuristic information, it can speed up the search process; it can find approximately optimal solutions.
Disadvantages: Slow operation speed; path quality may be poor.
- RRT* Smart
advantages: improve search efficiency through intelligent sampling strategy; able to find the optimal path.
Disadvantages: Slow operation speed; path quality may be poor.
- Anytime RRT*
Advantages: The search can be stopped at any time and the current best path can be returned; the approximate optimal solution can be found.
Disadvantages: Path quality may be poor; real-time calculations are required.
- Closed-Loop RRT*
Advantages: Able to handle non-holonomic dynamic systems and constraints; higher path quality.
Disadvantages: Slow operation speed; may require a large amount of storage space.
# --------Visualization specialized for dynamic RRT
def visualization(self):
if self.ind % 100 == 0 or self.done:
V = np.array(self.V)
Path = np.array(self.Path)
start = self.env.start
goal = self.env.goal
# edges = []
# for i in self.Parent:
# edges.append([i, self.Parent[i]])
edges = np.array([list(i) for i in self.Edge])
ax = plt.subplot(111, projection='3d')
# ax.view_init(elev=0.+ 0.03*initparams.ind/(2*np.pi), azim=90 + 0.03*initparams.ind/(2*np.pi))
# ax.view_init(elev=0., azim=90.)
ax.view_init(elev=90., azim=0.)
ax.clear()
# drawing objects
draw_Spheres(ax, self.env.balls)
draw_block_list(ax, self.env.blocks)
if self.env.OBB is not None:
draw_obb(ax, self.env.OBB)
draw_block_list(ax, np.array([self.env.boundary]), alpha=0)
draw_line(ax, edges, visibility=0.75, color='g')
draw_line(ax, Path, color='r')
# if len(V) > 0:
# ax.scatter3D(V[:, 0], V[:, 1], V[:, 2], s=2, color='g', )
ax.plot(start[0:1], start[1:2], start[2:], 'go', markersize=7, markeredgecolor='k')
ax.plot(goal[0:1], goal[1:2], goal[2:], 'ro', markersize=7, markeredgecolor='k')
# adjust the aspect ratio
set_axes_equal(ax)
make_transparent(ax)
# plt.xlabel('s')
# plt.ylabel('y')
ax.set_axis_off()
plt.pause(0.0001)
if __name__ == '__main__':
rrt = dynamic_rrt_3D()
rrt.Main()
- Spline-RRT*
Advantages: Improve path quality through spline interpolation; able to find smooth paths.
Disadvantages: Slow operation speed; may require a large amount of storage space.
- Advantages of Fast Marching Trees (FMT*)
: fast operation speed; able to find the shortest path.
Disadvantages: The path quality may be poor; the effect may not be ideal in high-dimensional space.
- Advantages of Batch Informed Trees (BIT*)
: improve search efficiency through batch sampling; can find the optimal path.
Disadvantages: Slow operation speed; may require a large amount of storage space.
2. Common path planning algorithms
2.1 Dijkstra’s algorithm
Dijkstra's algorithm is an algorithm for finding the shortest path in a graph. It can be applied to directed or undirected graphs. This algorithm finds the final shortest path by continuously updating the shortest path from the starting point to each vertex. The time complexity of Dijkstra's algorithm is O(V^2), where V is the number of vertices, but the time complexity can be optimized by implementing a minimum heap through a priority queue.
2.2 A* algorithm
Algorithm A is a heuristic search algorithm that combines the advantages of Dijkstra's algorithm and greedy best-first search algorithm. Algorithm A dynamically adjusts the search direction by estimating the cost from the current node to the target node to find the best path faster. The A* algorithm performs well in many practical applications and has high efficiency and accuracy.
2.3 RRT algorithm
The RRT (Rapidly-exploring Random Tree) algorithm is a path planning algorithm suitable for high-dimensional spaces. It searches for paths through random sampling and continuous expansion of the tree structure. The RRT algorithm is suitable for environments with complex spatial structures and is widely used in robot navigation and motion planning.
3. Application of path planning in driverless driving
Unmanned driving technology is one of the hot spots in the field of artificial intelligence today, and path planning plays a crucial role in it. Self-driving vehicles need to obtain surrounding environment information through sensors and use path planning algorithms to determine the vehicle's driving route to ensure safe and efficient arrival at the destination. Due to the complexity of the road traffic environment, path planning algorithms need to take into account real-time traffic conditions, obstacle avoidance, traffic rules and other factors, so the requirements for path planning algorithms are also more stringent.
4. Application of path planning in logistics distribution
With the rapid development of the e-commerce industry, logistics and distribution have become an area of great concern. The application of path planning in logistics distribution can not only improve distribution efficiency, but also reduce costs. Through reasonable route planning, delivery vehicles can cover more delivery points in the shortest time, thereby improving delivery efficiency. At the same time, the path planning algorithm also needs to take into account factors such as the timeliness of the distribution point and traffic congestion to provide the optimal distribution plan.
5. Challenges and future development of path planning
With the continuous development of artificial intelligence and computer technology, the field of path planning is also facing some challenges. For example, in complex urban environments, path planning needs to take into account multiple factors such as sidewalks, traffic lights, pedestrians and vehicles, etc., which places higher requirements on the accuracy and real-time performance of the algorithm. In the future, path planning technology may combine more sensor data and deep learning technology to improve the efficiency and accuracy of path planning.
Conclusion
As one of the important applications in the field of artificial intelligence, path planning is of great significance for realizing intelligent transportation systems and logistics distribution. With the continuous advancement of technology, path planning algorithms will play a role in more fields, bringing convenience and safety to people's lives.