An Overview and Comparison of Traditional Motion Planning Based on Rapidly Exploring Random Trees

Abstract

Motion planning is a fundamental problem in robotics that involves determining feasible or optimal paths within finite time. While complete motion planning algorithms are guaranteed to converge to a path solution in finite time, they are proven to be computationally inefficient, making them unsuitable for most practical problems. Resolution-complete algorithms, on the other hand, ensure completeness only if the resolution parameter is sufficiently fine, but they suffer severely from the curse of dimensionality. In contrast, sampling-based algorithms, such as Rapidly Exploring Random Trees (RRT) and its variants, have gained the increasing attention of researchers due to their computational efficiency and effectiveness, particularly in high-dimensional problems. This review paper introduces RRT-based algorithms and provides an overview of their key methodological aspects.

1. Introduction

Motion planning is a well-known and fundamental problem in robotics that involves finding a (optimal) path for a robot from its initial position toward goal regions while avoiding collisions with any obstacles in its environment. While not the only fundamental problem of robotics, motion planning has garnered significant attention from researchers due to widespread applications across various fields, including robotics [1], computer animation [2], medical procedures [3], and manufacturing [4,5].

Many motion planning algorithms have been proposed, including complete, resolution complete, heuristic, potential field, and sampling-based algorithms. Each method offers distinct advantages and limitations in terms of space complexity, time complexity, and path optimization.

Complete algorithms primarily focus on geometric methods [6,7,8,9] that are guaranteed to find a path solution in finite time. However, both finding the shortest path and finding a feasible path under velocity constraints are proven to be NP-hard [10].

Resolution-complete algorithms include cell decomposition methods [11] and graph search methods [12,13,14]. They can guarantee completeness and optimality if fine resolution parameters are provided. However, their computation time and memory space grow exponentially with increasing dimensionality.

Heuristic algorithms consist of neural networks (NNs) and meta-heuristic optimization algorithms. On the one hand, NNs, with their advantages such as nonlinear mapping capabilities and learning ability, are employed in various aspects of navigation, including processing sensory data [15], obstacle avoidance, and path planning [16,17,18,19]. Additionally, end-to-end learning approaches for driving have gained significant attention in recent years, such as Deepdriving [20] and ChauffeurNet [21]. However, their disadvantages include slow training speeds, the need for large datasets, and a lack of traceability. On the other hand, meta-heuristic optimization algorithms offer high success rates and the ability to manage complex constraints. Notable examples include Brain Storm Optimization [22], Genetic Algorithm [23], Particle Swarm Optimization [24], and Ant Colony Optimization (ACO) [25]. However, they require considerable time to reach optimal solutions, especially for large-scale problems.

The Artificial Potential Field (APF) [26] is a well-known method usually used to handle unknown and dynamic environments [27]. However, APF suffers from the problem of local minima. To address this issue, APF is combined with various heuristics to achieve path optimality and safety, such as APF with random sampling [28], APF with ACO [29], and APF with GA [30].

Sampling-based algorithms have emerged as particularly prominent in recent years for two key reasons: (i) They avoid the explicit construction of obstacle configurations (which can be challenging in high-dimensional spaces). (ii) They allow the incorporation of differential constraints to generate dynamically feasible paths. Among sampling-based methods, Rapidly Exploring Random Tree (RRT) [31] and its asymptotically optimal variant (RRT* [32]) are widely recognized as foundational algorithms.

Despite the success of RRT*-based algorithms, there are still some major limitations [33]: (i) Their slow convergence rate to optimal solutions; (ii) Their significantly large memory requirements due to numerous iterations utilized to calculate the optimal path. Various improvements have been proposed to address these challenges. Some approaches focus on enhanced sampling strategies to replace pure uniform sampling, such as Informed-RRT* [34], RRT*-Smart [35], and CE-RRT* [36]. Other methods employ two directional trees with greedy connect heuristics to enhance the convergence rates of RRT and RRT*, including RRT-Connect [37], B-RRT* [38], and IB-RRT* [33].

Recent research has increasingly focused on extending sampling-based algorithms to handle dynamic systems. While RRT and RRT* excel at high-level path planning, they lack a low-level control synthesis. Several approaches attempt to bridge the gap by designing controllers to steer the system between two generated vertices, such as Kinodynamic-RRT* [39] and LQR-RRT* [40]. For environments with dynamical obstacles, CBF-RRT [41] incorporates control barrier functions (CBFs) into RRT to achieve dynamical obstacle avoidance. Similarly, CBF-RRT* [42] combines CBFs for safety with control Lyapunov functions (CLFs) to generate safe motion trajectories between vertices of RRT*.

This paper is structured as follows: Section 2 introduces fundamental components of RRT-based motion planning. Various RRT-based algorithms are introduced in Section 3. Section 4 introduces RRT-based algorithms for dynamic systems. Section 5 presents experimental results and analysis, and the conclusion is in Section 6.

2. Background

In this section, we introduce key local motion planning components commonly integrated into RRT-based algorithms. We focus on three fundamental approaches: linear quadratic regulators (LQRs), which optimize control inputs to minimize cost functions, control barrier functions (CBFs), which ensure safety constraints and obstacle avoidance, and control Lyapunov functions (CLFs), which guarantee stability and convergence properties.

2.1. System Dynamics

Consider robot dynamics modeled as affine control dynamics,

$$\dot{\mathbf{x}}=f_{\mathrm{cl}}(\mathbf{x},\mathbf{u}):=f\left(\mathbf{x}\right)+g\left(\mathbf{x}\right)\mathbf{u},$$

(1)

with $f:{\mathbb{R}}^n\to {\mathbb{R}}^n$ and $g:{\mathbb{R}}^n\to {\mathbb{R}}^{n\times m}$ locally Lipschitz, the state $\mathbf{x}\in \mathcal{X}\subset {\mathbb{R}}^n$, and the control input $\mathbf{u}\in \mathcal{U}\subset {\mathbb{R}}^m$. The initial state is denoted as ${\mathbf{x}}_{\mathrm{init}}$ and the goal region is denoted as ${\mathcal{X}}_{\mathrm{goal}}\subset \mathcal{X}$. ${\mathcal{O}}_i\subset \mathcal{X}$ represents the i-th obstacle region, where $i=1,\dots ,n$. The obstacle-free set is defined as ${\mathcal{X}}_{\mathrm{safe}}:=\mathcal{X}-{\bigcup }_{i=1}^n{\mathcal{O}}_i$.

2.2. Linear Quadratic Regulator

An LQR is a widely used optimal control strategy that minimizes a quadratic cost function. Consider a linear time-invariant system

$$\dot{\mathbf{x}}=\mathbf{Ax}+\mathbf{Bu},$$

which is a special case of the general affine system (1) where $f\left(\mathbf{x}\right)=\mathbf{Ax},g\left(\mathbf{x}\right)\equiv \mathbf{B}$. The LQR problem is to find control inputs $\mathbf{u}\left(t\right)$ that minimize the following quadratic cost criterion:

$$J_{\mathrm{LQR}}:={\int }_0^{\infty }\mathbf{x}{\left(t\right)}^{\top }\mathbf{Qx}\left(t\right)+\mathbf{u}{\left(t\right)}^{\top }\mathbf{Ru}\left(t\right)dt,$$

where $\mathbf{Q}$ and $\mathbf{R}$ are positive-definite weighting matrices.

According to Theorem 10.1 in [43], if there exists a symmetric solution $\mathbf{P}$ to the following algebraic Riccati equation:

$${\mathbf{A}}^{\top }\mathbf{P}+\mathbf{PA}+\mathbf{Q}-{\mathbf{PBR}}^{-1}{\mathbf{B}}^{\top }\mathbf{P}=\mathbf{0},$$

(2)

for which $\mathbf{A}-{\mathbf{BR}}^{-1}{\mathbf{B}}^{\top }\mathbf{P}=\mathbf{0}$ is a stability matrix, then the optimal feedback control law is given by

$$\mathbf{u}=-\mathbf{Kx}, \mathrm{where} \mathbf{K}:={\mathbf{R}}^{-1}{\mathbf{B}}^{\top }\mathbf{P},$$

with the optimal cost equal to $\mathbf{x}{\left(0\right)}^{\top }\mathbf{P}\mathbf{x}\left(0\right).$ This feedback law also stabilizes the closed-loop system.

In addition, LQR can be extended to handle nonlinear system dynamics through local linearization. More specifically, a local linearization can be made around an equilibrium point $({\mathbf{x}}^{\mathrm{eq}},{\mathbf{u}}^{\mathrm{eq}})$ using first-order Taylor expansion [43]:

$$\dot{\delta \mathbf{x}}=\widehat{\mathbf{A}}\delta \mathbf{x}+\widehat{\mathbf{B}}\delta \mathbf{u},$$

defined by the following Jacobian matrices:

$$\widehat{\mathbf{A}}:={\displaystyle \frac{\partial f_{\mathrm{cl}}({\mathbf{x}}^{\mathrm{eq}},{\mathbf{u}}^{\mathrm{eq}})}{\partial \mathbf{x}}},\widehat{\mathbf{B}}:={\displaystyle \frac{\partial f_{\mathrm{cl}}({\mathbf{x}}^{\mathrm{eq}},{\mathbf{u}}^{\mathrm{eq}})}{\partial \mathbf{u}}},$$

where $\delta \mathbf{x}:=\mathbf{x}-{\mathbf{x}}^{\mathrm{eq}},\delta \mathbf{u}:=\mathbf{u}-{\mathbf{u}}^{\mathrm{eq}}$, $\dot{\delta \mathbf{x}}:=\dot{\mathbf{x}}-\dot{{\mathbf{x}}^{\mathrm{eq}}}$. Therefore, this local linearization allows the application of LQR techniques to nonlinear systems in the neighborhood of the equilibrium point.

2.3. Control Barrier Functions

CBFs are employed to generate control inputs that steer the system (1) towards an exploratory point while ensuring obstacle avoidance. Let the safe set $\mathcal{C}$ be defined as

$$\mathcal{C}=\{\mathbf{x}\in \mathcal{D}:h(\mathbf{x})\ge 0\},$$

for a continuously differentiable function $h:\mathcal{D}\subset {\mathbb{R}}^n\to \mathbb{R}.$ The boundary and interior of $\mathcal{C}$ are given by

$$\partial \mathcal{C}=\{\mathbf{x}\in \mathcal{D}:h(\mathbf{x})=0\}, Int\left(\mathcal{C}\right)=\{\mathbf{x}\in \mathcal{D}:h(\mathbf{x})>0\}.$$

The set $\mathcal{C}$ is called forward invariant for the system (1) if $\mathbf{x}\left(0\right)\in \mathcal{C}$ implies $\mathbf{x}\left(t\right)\in \mathcal{C}, \forall t>0$. Intuitively, set invariance ensures that any trajectory starting within the invariant set will never leave it, thereby guaranteeing safety.

First, consider a model without control inputs, $\dot{\mathbf{x}}=f_{\mathrm{cl}}\left(\mathbf{x}\right)$. According to Nagumo’s Theorem [44], the necessary and sufficient conditions for set invariance are

$$\dot{h}\left(\mathbf{x}\right)\ge 0\forall \mathbf{x}\in \partial \mathcal{C}.$$

Alternatively, another necessary and sufficient condition [45] is the existence of an extended class $\mathcal{K}$ function $\alpha$ such that

$$\dot{h}\left(\mathbf{x}\right)\ge -\alpha \left(h\left(\mathbf{x}\right)\right), \forall \mathbf{x}\in \mathcal{D}.$$

(3)

The function h, which defines the forward invariant set, is called a zeroing barrier function.

Next, consider the system dynamics (1). The derivative of h becomes

$$\dot{h}\left(\mathbf{x}\right)={\mathcal{L}}_{f}h\left(\mathbf{x}\right)+{\mathcal{L}}_{g}h\left(\mathbf{x}\right)\mathbf{u}.$$

Combining the above equation with Equation (3), for any point $\mathbf{x}\in \mathcal{D}$, the set of all controllers rendering the set $\mathcal{C}$ forward invariant is defined as

$${\mathcal{K}}_{\mathrm{zcbf}}\left(\mathbf{x}\right):=\left\{\mathbf{u}\in \mathcal{U}:{\mathcal{L}}_{f}h\left(\mathbf{x}\right)+{\mathcal{L}}_{g}h\left(\mathbf{x}\right)\mathbf{u}\ge -\alpha \left(h\left(\mathbf{x}\right)\right)\right\}.$$

The function h is called a zeroing control barrier function (ZCBF) if ${\mathcal{K}}_{\mathrm{zcbf}}\left(\mathbf{x}\right)\ne \varnothing ,\forall \mathbf{x}\in \mathcal{D}$.

Finally, given a ZCBF h, safety can be ensured by modifying an existing controller in a minimal way. More specifically, given a feedback controller $\mathbf{u}=k\left(\mathbf{x}\right)$ for the control system (1), it may happen that $k\left(\mathbf{x}\right)\notin K_{\mathrm{zcbf}}\left(\mathbf{x}\right)$ for some $\mathbf{x}\in \mathcal{D}$. To guarantee safety, one can determine the minimum perturbation to $\mathbf{u}$ by solving the following quadratic program (CBF-QP):

$$\begin{array}{cc}\hfill u\left(\mathbf{x}\right)=\underset{\mathbf{u}\in {\mathbb{R}}^m}{argmin}& {\displaystyle \frac{1}{2}}{\parallel \mathbf{u}-k\left(\mathbf{x}\right)\parallel }^2\hfill \\ \hfill s.t.& {\mathcal{L}}_{f}h\left(\mathbf{x}\right)+{\mathcal{L}}_{g}h\left(\mathbf{x}\right)\mathbf{u}\ge -\alpha \left(h\left(\mathbf{x}\right)\right).\hfill \end{array}$$

(4)

2.4. Control Lyapunov Functions

CLFs can be employed to generate control inputs that drive a system to a desired equilibrium point $(\overline{\mathbf{x}},\overline{\mathbf{u}})$ that is, $f_{\mathrm{cl}}(\overline{\mathbf{x}},\overline{\mathbf{u}})=\mathbf{0}.$ Without loss of generality, the equilibrium point is typically assumed at the origin $(\overline{\mathbf{x}}=\mathbf{0},\overline{\mathbf{u}}=\mathbf{0}).$ Additionally, it is typically assumed that the equilibrium point is stable and there exists an asymptotically stable neighborhood around it. More specifically, the equilibrium point is stable [46] if $\forall ϵ>0\exists \delta >0,$

$$\parallel \mathbf{x}\left(0\right)\parallel <\delta \Rightarrow \parallel \mathbf{x}\left(t\right)\parallel <ϵ,\forall t\ge 0.$$

Intuitively, the stability implies that a trajectory starting in a $\delta$ neighborhood of the origin will never leave the $ϵ$ neighborhood. Moreover, a neighborhood $\mathcal{S}$ of the stable equilibrium point is called the region of asymptotic stability [46] if

$$\parallel \mathbf{x}\left(0\right)\parallel \in \mathcal{S}\Rightarrow \underset{t\to \infty }{lim}\mathbf{x}\left(t\right)=\mathbf{0}.$$

Intuitively, the asymptotic stability implies that any point starting within $\mathcal{S}$ converges to the stable equilibrium point as t tends to infinity.

First, consider a model without control inputs, $\dot{\mathbf{x}}=f_{\mathrm{cl}}\left(\mathbf{x}\right)$. According to Theorem 2.26 in [44], a neighborhood $\mathcal{S}$ of origin is the region of asymptotic stability if there exists a locally Lipschitz-positive definite function $\mathsf{\Psi }$ such that

$$\mathcal{S}\subset \mathcal{N}[\mathsf{\Psi },v]:=\{\mathbf{x}:\mathsf{\Psi }(\mathbf{x})\le v\} \mathrm{for} \mathrm{some} v>0,$$

and for all $\mathbf{x}\in \mathcal{N}[\mathsf{\Psi },v]$,

$$D^{+}\mathsf{\Psi }\left(\mathbf{x}\right)\le -\varphi (\parallel \mathbf{x}\left(t\right)\parallel ) \mathrm{for} \mathrm{some} \mathcal{K} \mathrm{function} \varphi ,$$

(5)

where $D^{+}$ is a Dini derivative. And this function $\mathsf{\Psi }$ is called the Lyapunov function inside $\mathcal{S}$.

Next, consider the system dynamics (1). The Dini derivative of $\mathsf{\Psi }$ is given by

$$D^{+}\mathsf{\Psi }\left(\mathbf{x}\right)={\mathcal{L}}_f\mathsf{\Psi }\left(\mathbf{x}\right)+{\mathcal{L}}_g\mathsf{\Psi }\left(\mathbf{x}\right)\mathbf{u}.$$

Combining the above equation and Equation (5), the set of all stabilizing controllers for every point $\mathbf{x}\in \mathcal{S}$ is defined by

$${\mathcal{K}}_{\mathrm{clf}}\left(\mathbf{x}\right):=\left\{\mathbf{u}\in \mathcal{U}:{\mathcal{L}}_f\mathsf{\Psi }\left(\mathbf{x}\right)+{\mathcal{L}}_g\mathsf{\Psi }\left(\mathbf{x}\right)\mathbf{u}\le -\varphi (\parallel \mathbf{x}\left(t\right)\parallel )\right\}.$$

The function $\mathsf{\Psi }$ is called the control Lyapunov function (CLF) if ${\mathcal{K}}_{\mathrm{clf}}\left(\mathbf{x}\right)\ne \varnothing , \forall \mathbf{x}\in \mathcal{S}$.

Finally, given a CLF $\mathsf{\Psi }$ and ZCBF h, one can steer the state $\mathbf{x}$ to another state ${\mathbf{x}}^{\prime }$ while guaranteeing safety by solving a sequence of QPs. More specifically, suppose $({\mathbf{x}}^{\prime },\overline{\mathbf{u}})$ is the equilibrium point of the system. Define new variables $\mathbf{z}\left(t\right):=\mathbf{x}\left(t\right)-{\mathbf{x}}^{\prime }$ and $\mathbf{v}\left(t\right):=\mathbf{u}\left(t\right)-\overline{\mathbf{u}}$ through a translation to establish a new system whose equilibrium point is the origin. The initial state is set to $\mathbf{z}\left(0\right)=\mathbf{x}-{\mathbf{x}}^{\prime }$. By restricting the control inputs $\mathbf{v}\left(t\right)\in {\mathcal{K}}_{\mathrm{clf}}$ for all $t>0$, one can guarantee $\mathbf{z}\left(t\right)$ converges to the origin. This constraint can then be incorporated into the CBF-QP, resulting in a new problem termed the CLF-CBF-QP:

$$\begin{array}{cc}\hfill v\left(\mathbf{z}\right)=\underset{\mathbf{v}\in {\mathbb{R}}^m}{argmin}& {\displaystyle \frac{1}{2}}{\parallel \mathbf{v}-k\left(\mathbf{z}\right)\parallel }^2\hfill \\ \hfill s.t.& {\mathcal{L}}_{f}h\left(\mathbf{z}\right)+{\mathcal{L}}_{g}h\left(\mathbf{z}\right)\mathbf{v}\ge -\alpha \left(h\left(\mathbf{z}\right)\right)\hfill \\ & {\mathcal{L}}_f\mathsf{\Psi }\left(\mathbf{z}\right)+{\mathcal{L}}_g\mathsf{\Psi }\left(\mathbf{z}\right)\mathbf{v}\le -\varphi (\parallel \mathbf{z}\left(t\right)\parallel ).\hfill \end{array}$$

(6)

3. The RRT-Based Motion Planning

In this section, several incremental RRT-based motion planning algorithms are introduced.

3.1. The RRT Algorithm and Its Variations

The Rapidly Exploring Random Tree (RRT) is introduced as an efficient data structure and sampling scheme to explore efficiently nonconvex high-dimensional spaces by growing the search tree toward goal areas. The major components of the RRT are as follows:

• Sample: The procedure provides independent identically distributed samples from ${\mathcal{X}}_{free}$.
• Steer: Given two states $\mathbf{x},{\mathbf{x}}^{\prime }\in \mathcal{X}$, the Steer procedure returns a state $\mathbf{z}\in {\mathcal{X}}_{free}$ that lies between $\mathbf{x}$ and ${\mathbf{x}}^{\prime }$ while maintaining $\parallel \mathbf{z}-\mathbf{x}\parallel \le \eta$, for a prespecified constant $\eta >0$. It is typically defined by

  $$Steer(\mathbf{x},{\mathbf{x}}^{\prime })=\underset{\mathbf{z}\in {\mathcal{X}}_{free},\parallel \mathbf{z}-\mathbf{x}\parallel \le \eta }{argmin}\parallel \mathbf{z}-{\mathbf{x}}^{\prime }\parallel .$$
• Nearest node: Given a tree $\mathcal{T}=(\mathcal{V},\mathcal{E})$ with a set of nodes $\mathcal{V}$ and a set of edges $\mathcal{E}$, the $Nearest(\mathcal{V},\mathbf{x})$ procedure provides a node in $\mathcal{V}$ that is closest to $\mathbf{x}$, i.e.,

  $$Nearest(\mathcal{V},\mathbf{x})=\underset{{\mathbf{x}}^{\prime }\in \mathcal{V}}{argmin}\parallel \mathbf{x}-{\mathbf{x}}^{\prime }\parallel .$$
• Collision test: Given two points $\mathbf{x},{\mathbf{x}}^{\prime }\in {\mathcal{X}}_{free}$, the procedure $ObstacleFree(\mathbf{x},{\mathbf{x}}^{\prime })$ returns True if and only if the line segment between $\mathbf{x}$ and ${\mathbf{x}}^{\prime }$ lies entirely within ${\mathcal{X}}_{free}$, i.e.,

  $$ObstacleFree(\mathbf{x},{\mathbf{x}}^{\prime })=\left\{\begin{array}{cc}\mathrm{True},\hfill & \mathrm{if}[\mathbf{x},{\mathbf{x}}^{\prime }]\subset {\mathcal{X}}_{free},\hfill \\ \mathrm{False},\hfill & \mathrm{otherwise}.\hfill \end{array}\right.$$

The body of RRT algorithms is presented in Algorithm 1. The algorithm proceeds as follows. First, a state is sampled from the configuration space, denoted as ${\mathbf{x}}_{samp}$. Next, the Extend procedure grows the tree $\mathcal{T}$ towards ${\mathbf{x}}_{samp}$, as outlined in Algorithm 2. More specifically, the Nearest procedure selects the closest neighbor node ${\mathbf{x}}_{nearest}\in \mathcal{T}$ to ${\mathbf{x}}_{samp}$. The Steer procedure steers the state from ${\mathbf{x}}_{nearest}$ to ${\mathbf{x}}_{samp}$, resulting in a new state ${\mathbf{x}}_{new}$. Finally, the state ${\mathbf{x}}_{new}$ and the edge $({\mathbf{x}}_{nearest},{\mathbf{x}}_{new})$ are added to $\mathcal{T}$ if the ObstacleFree procedure returns True.

Additionally, RRT-Connect, a variation of RRT, employs a two-RRT planner to achieve rapid convergence to a solution. More specifically, it constructs two trees, ${\mathcal{T}}_1$ and ${\mathcal{T}}_2$, starting from ${\mathbf{x}}_{\mathrm{init}}$ and ${\mathbf{x}}_{\mathrm{goal}}$, respectively. In each iteration, both trees are extended towards a random state ${\mathbf{x}}_{samp}$ simultaneously. The algorithm maintains both trees at all times until they connect and a solution is found.

Last but not least, these algorithms ensure probabilistic completeness, i.e., as the number of iterations approaches infinity, the probability of finding a solution (if one exists) approaches one (see Theorem 3 in [47]).

Algorithm 1: Body of RRT and RRT* algorithms

Algorithm 2: ExtendRRT

3.2. The RRT* Algorithm and Its Variations

Although RRT ensures probabilistic completeness, it does not guarantee optimality, i.e., it converges to a suboptimal solution with probability one (see Theorem 10 in [32]). To address this limitation, RRT* is introduced as an asymptotically optimal variant that asymptotically optimizes the existing tree to achieve asymptotic optimality properties, i.e., almost-sure convergence to an optimal solution. Several key components are added in RRT*:

• Near nodes: Given a tree $\mathcal{T}=(\mathcal{V},\mathcal{E})$ with a set of nodes $\mathcal{V}$ and a set of edges $\mathcal{E}$, the $Near(\mathcal{V},\mathbf{x})$ procedure utilizes a pre-defined distance $\parallel ·\parallel$ to identify a set of near nodes ${\mathcal{X}}_{near}$ in $\mathcal{T}$ that are close to $\mathbf{x}$. This is typically defined as the set of all nodes within a closed ball of radius $r_n$ centered at $\mathbf{x}$, i.e.,

  $${\mathcal{X}}_{near}:=\left\{{\mathbf{x}}^{\prime }\in \mathcal{V}:\parallel \mathbf{x}-{\mathbf{x}}^{\prime }\parallel \le r_n\right\},$$

  where the radius is defined by

  $$r_n:=\gamma {\left({\displaystyle \frac{logn}{n}}\right)}^{1/d},$$

  (7)

   n is the number of vertices in the tree, d is the dimension of the space, and $\gamma$ is a constant.
• Choose Parent: The procedure tries to find collision-free paths between a selected node $\mathbf{x}$ and all its near nodes, and selects the near node that yields the lowest cost of $\mathbf{x}$ as the parent of $\mathbf{x}$.
• Rewire: The procedure attempts to connect a selected node $\mathbf{x}$ with each node in its neighborhood ${\mathcal{X}}_{near}$. If the trajectory from $\mathbf{x}$ to the near node ${\mathbf{x}}_{near}$ results in a lower cost for ${\mathbf{x}}_{near}$, then $\mathbf{x}$ becomes the new parent of ${\mathbf{x}}_{\mathrm{near}}$.

It is worth noting that the Rewire procedure guarantees the asymptotic optimality properties of RRT*.

The body of the RRT* algorithm is outlined in Algorithm 1, while the Extend procedure for the RRT* is detailed in Algorithm 3. The Extend procedure operates as follows. First, the Nearest procedure selects the closest neighbor node ${\mathbf{x}}_{nearest}\in \mathcal{T}$ of ${\mathbf{x}}_{samp}$. Second, the Steer procedure steers the state from ${\mathbf{x}}_{nearest}$ to ${\mathbf{x}}_{samp}$, resulting in a new state ${\mathbf{x}}_{new}$. Third, the ChooseParent procedure identifies a potential parent node for ${\mathbf{x}}_{new}$. If a suitable parent node ${\mathbf{x}}_{parent}$ is found, then ${\mathbf{x}}_{new}$ and the edge $({\mathbf{x}}_{parent},{\mathbf{x}}_{new})$ are added to the tree $\mathcal{T}$. Fourth, the Rewire procedure attempts to connect ${\mathbf{x}}_{new}$ with each node in its neighborhood ${\mathcal{X}}_{near}$. If the trajectory from ${\mathbf{x}}_{new}$ to the near node ${\mathbf{x}}_{near}$ results in a lower cost for ${\mathbf{x}}_{near}$, then ${\mathbf{x}}_{new}$ becomes the new parent of ${\mathbf{x}}_{near}$.

Although RRT* ensures asymptotic optimality, its convergence rate is relatively slow. More specifically, let $f\left(\mathbf{x}\right)$ be the cost of an optimal path from ${\mathbf{x}}_{\mathrm{init}}$ to ${\mathbf{x}}_{\mathrm{goal}}$ constrained to pass through $\mathbf{x}$. The set of states that could improve the current solution is defined by

$${\mathcal{X}}_f:=\{\mathbf{x}\in \mathcal{X}:f\left(\mathbf{x}\right)<c_{\mathrm{best}}\},$$

where $c_{\mathrm{best}}$ is the cost of the current solution. In RRT*, the probability of sampling states from ${\mathcal{X}}_f$ becomes arbitrarily small for large subsets (see Remark 1 in [34]), leading to a slow convergence rate. Rapid convergence can be expected only if the probability of sampling states from ${\mathcal{X}}_f$ increases.

Algorithm 3: ExtendRRT*

Several methods aim to enhance the probability of sampling states from ${\mathcal{X}}_f$ through sample biasing. For instance, Informed-RRT* employs the heuristic $\widehat{f}\left(\mathbf{x}\right):=\parallel {\mathbf{x}}_{\mathrm{init}}{-\mathbf{x}\parallel }_2+{\parallel \mathbf{x}-{\mathbf{x}}_{\mathrm{goal}}\parallel }_2$ to estimate the unknown $f\left(\mathbf{x}\right)$. The set ${\mathcal{X}}_f$ is then estimated by

$${\mathcal{X}}_{\widehat{f}}:=\{\mathbf{x}\in \mathcal{X}:\parallel {\mathbf{x}}_{\mathrm{init}}{-\mathbf{x}\parallel }_2+\parallel \mathbf{x}-{\mathbf{x}}_{\mathrm{goal}}{\parallel }_2\le c_{\mathrm{best}}\}.$$

The Sample procedure of Informed-RRT* is modified to generate uniformly distributed samples from the hyperellipsoid ${\mathcal{X}}_{\widehat{f}}$.

RRT*-Smart also implements biased sampling to accelerate the convergence rate. It tends to sample new states as close as possible to obstacles, inspired by visibility graph techniques, which optimize the existing path at turns.

Another approach is adaptive sampling, utilized by CE-RRT*, which applies the cross-entropy method to estimate the probability $\mathbb{P}\left({\mathcal{X}}_f\right)$. This estimator is then used to sample trajectories, resulting in a faster convergence rate.

Similar to RRT-Connect, there is also a bidirectional version of RRT* known as B-RRT*. B-RRT* employs a slight variation of the greedy RRT-Connect heuristic for connecting two trees. However, using two directional trees with a greedy connection heuristic does not guarantee asymptotic optimality.

4. The RRT-Based Motion Planning with Dynamics

It is challenging to apply RRT* to problems with dynamics due to the need for two domain-specific extension heuristics [40]: a distance metric and a steering procedure.

On the one hand, while the Euclidean distance is widely used in high-level path planning, it may not accurately reflect the dynamics of particular domains, especially in cases involving complex or underactuated dynamics. This can lead to poor exploration and slow convergence to optimal solutions.

On the other hand, a prevalent steering procedure in high-level path planning involves generating straight paths between vertices. However, steering safe trajectories between vertices in RRT* becomes challenging when considering low-level control synthesis.

In recent years, significant efforts have been made to adapt RRT-based algorithms to dynamic domains. This section will look at these methods that address the said challenges by leveraging either LQRs or CBFs.

4.1. The LQR-RRT* Algorithm

LQR-RRT* [48] is an extension of RRT* that integrates LQR into the Steer, Near, and Nearest procedures for optimal control synthesis. More specifically, given an equilibrium point $\mathbf{x}$, $\cos \mathrm{t}(\mathbf{v},\mathbf{x})$ is defined as the LQR cost from any state $\mathbf{v}$ to $\mathbf{x}$ under the LQR policy. The cost function over the neighborhood of $\mathbf{x}$ can be expressed as

$${\cos \mathrm{t}}_{\mathbf{x}}\left(\mathbf{v}\right)={(\mathbf{v}-\mathbf{x})}^{\top }\mathbf{P}(\mathbf{v}-\mathbf{x}),$$

where $\mathbf{P}$ is a solution to Equation (2). Then, the three components of RRT* are modified as follows:

• LQRNearest: The $\mathrm{LQRNearest}(\mathcal{V},\mathbf{x})$ procedure identifies a node in $\mathcal{V}$ that is closest to $\mathbf{x}$ according to the cost function, i.e.,

  $${\mathbf{x}}_{\mathrm{nearest}}=\underset{\mathbf{v}\in \mathcal{V}}{argmin}{\cos \mathrm{t}}_{\mathbf{x}}\left(\mathbf{v}\right).$$
• Near nodes: the $\mathrm{LQRNear}(\mathcal{V},\mathbf{x})$ procedure employs the cost function to find a set of near nodes ${\mathcal{X}}_{near}$ in $\mathcal{T}$ that are close to $\mathbf{x}$, i.e.,

  $${\mathcal{X}}_{\mathrm{near}}:=\{\mathbf{v}\in \mathcal{V}:{\cos \mathrm{t}}_{\mathbf{x}}\left(\mathbf{v}\right)\le r_n\},$$

  where $r_n$ is consistent with Equation (7).
• Steer: Given two states, $\mathbf{x},{\mathbf{x}}^{\prime }$, the LQR controller generates a sequence of optimal controls $\left\{{\mathbf{u}}_i\right\}$ that steer a state trajectory from $\mathbf{x}$ to ${\mathbf{x}}^{\prime }$.

4.2. The CBF-RRT Algorithm and Its Variations

CBF-RRT is an extension of RRT that incorporates CBFs into the steering function to ensure safety, eliminating the need for the Nearest or ObstacleFree procedures of RRT. The Steer procedure is modified as follows:

• Safe steer: Given a state ${\mathbf{x}}_0$, a time horizon $t_h$, and a control reference $k\left(\mathbf{x}\right)$, the $\mathrm{SafeSteer}({\mathbf{x}}_0,t_h,k)$ procedure steers the state ${\mathbf{x}}_0$ to an exploratory new state ${\mathbf{x}}_{\mathrm{new}}$. Concretely, it generates a sequence of control inputs $\mathbf{u}\left(t\right)$ by solving a sequence of CBF-QPs (Equation (4)) that steers a collision-free trajectory to ${\mathbf{x}}_{\mathrm{new}}$ at time $t_0+t_h$.

The overall algorithm of CBF-RRT is outlined in Algorithm 4.

Similar to CBF-RRT, CBF-RRT* incorporates CBFs into the RRT* algorithm to ensure safety. In addition to safety, RRT* requires the exact local motion planning because it needs to steer ${\mathbf{x}}_{\mathrm{near}}$ to ${\mathbf{x}}_{\mathrm{new}}$ in the ChooseParent procedure and steer ${\mathbf{x}}_{\mathrm{new}}$ to ${\mathbf{x}}_{\mathrm{near}}$ in the Rewire procedure. Consequently, CBF-RRT* implements the exact motion planning by solving a sequence of CLF-CBF-QPs (Equation (6)). The state trajectory from ${\mathbf{x}}_{\mathrm{new}}$ to ${\mathbf{x}}_{\mathrm{near}}$ can be also utilized to evaluate the cost between the two states, offering greater precision than the LQR cost-to-go function used in LQR-RRT*.

Algorithm 4: CBF-RRT

LQR-CBF-RRT* [48], an incremental version of CBF-RRT*, not only incorporates CBFs into the steering function for safety but also employs LQR for optimal control synthesis. Additionally, it proposes truncating the state trajectory that violates CBF constraints to reduce the burden of solving the QPs, leading to a new Steer procedure:

• LQR-CBF-Steer: Given two states $\mathbf{x},{\mathbf{x}}^{\prime }$, the LQR controller generates a sequence of optimal controls $\left\{{\mathbf{u}}_i\right\}$ that steer a state trajectory from $\mathbf{x}$ to ${\mathbf{x}}^{\prime }$. The CBF constraints are checked to ensure that the generated trajectory is collision-free. If none of the CBF constraints are violated, then ${\mathbf{x}}^{\prime }$ is added to the tree. Otherwise, the trajectory is truncated so that the end state ${\mathbf{x}}_{new}$ is safe, and the end state ${\mathbf{x}}_{new}$ is then added to the tree.

5. Experimental Results

In this section, we conduct an experimental study to compare the performance of several baseline methods.

5.1. Path Planning

We evaluated RRT, RRT*, RRT-Connect, Informed-RRT*, and RRT*-Smart on two examples for planning the motions of rigid objects in 2D. The implementations of RRT, RRT*, RRT-Connect, RRT*-Smart, and Informed-RRT* were based on the reference code from the package https://github.com/zhm-real/PathPlanning (accessed on 10 December 2024). The hyperparameters for these methods were set as follows: $\mathrm{step}\_\mathrm{len}=1, \mathrm{goal}\_\mathrm{sample}\_\mathrm{rate}=0.1$, and $\mathrm{search}\_\mathrm{radius}=12$ (applied to RRT*-based methods only).

Figure 1 illustrates the evolution of baseline methods across both examples. The comparison between RRT and RRT* demonstrates that RRT* can asymptotically optimize the existing path but its convergence rate is notably slow. The comparison between RRT and RRT-Connect demonstrates that the greedy behavior of RRT-Connect improves both the convergence rate and final solution quality of RRT. With the direct sampling of an ellipsoidal subset, Informed-RRT* improves the convergence rate and final solution quality compared to RRT*. Additionally, with intelligent sampling and path optimization, RRT*-Smart produces significantly shorter paths than RRT*.

Figure 2 presents the convergence pattern of the costs associated with baseline methods on these two examples. RRT*-Smart demonstrates the fastest convergence rate and the highest final solution quality among all baseline methods. This highlights the effectiveness of the intelligent sampling and path optimization techniques introduced by RRT*-Smart. Notably, RRT-Connect also performs exceptionally well, as its greedy connect heuristic significantly outperforms both RRT* and Informed-RRT*. Additionally, Informed-RRT* shows improvements in both convergence rate and final solution quality compared to RRT*.

5.2. Double Integrator Model

Following [48], we considered a double integrator model with linear dynamics,

$$\ddot{x_1}=u_1, \ddot{x_3}=u_2$$

with the state $\mathbf{x}=[x_1,x_2,x_3,x_4]$ where $[x_1,x_3]$ denotes the position and $[x_2,x_4]$ denotes the velocity. The control input $\mathbf{u}=[u_1,u_2]$ consists of acceleration.

We present an experimental performance comparison between LQR-RRT* and LQR-CBF-RRT*. The LQR-RRT* and LQR-CBF-RRT* used reference implementations from the package https://github.com/mingyucai/LQR_CBF_rrtStar (accessed on 10 December 2024). The hyperparameters for these methods were set as follows: $\mathrm{step}\_\mathrm{len}=10, \mathrm{goal}\_\mathrm{sample}\_\mathrm{rate}=0.1,$ and $\mathrm{search}\_\mathrm{radius}=20$.

Figure 3 shows the evolution of these two methods on the double integrator model. It can be observed that, given the same number of iterations, LQR-CBF-RRT* achieves a higher-quality final solution compared to LQR-RRT*. However, as is shown in Figure 4, LQR-RRT* achieves a faster convergence rate with respect to running time than LQR-CBF-RRT*. This difference may be attributed to the increased time complexity introduced by using CBFs in LQR-CBF-RRT*.

6. Conclusions

In this paper, we have summarized the current state of RRT-based algorithms and conducted an experimental study to compare the performance of several baseline methods. Our results confirm a good performance of the greedy connection heuristic introduced by RRT-Connect, the intelligent sampling and path optimization introduced by RRT*-Smart, and the effectiveness of LQR-RRT*. However, the field still has much to explore. There are undoubtedly many new challenges, discoveries, and insights awaiting further investigation in the future.