Efficient Motion Primitives-Based Trajectory Planning for UAVs in the Presence of Obstacles

Abstract

The achievement of full autonomy in Unmanned Aerial Vehicles (UAVs) is significantly dependent on effective motion planning. Specifically, it is crucial to plan collision-free trajectories for smooth transitions from initial to final configurations. However, finding a solution executable by the actual system adds complexity: the planned motion must be dynamically feasible. This involves meeting rigorous criteria, including vehicle dynamics, input constraints, and state constraints. This work addresses optimal kinodynamic motion planning for UAVs in the presence of obstacles by employing a hybrid technique instead of conventional search-based or direct trajectory optimization approaches. This technique involves precomputing a library of motion primitives by solving several Two-Point-Boundary-Value Problems (TPBVP) offline. This library is then repeatedly used online within a graph-search framework. Moreover, to make the method computationally tractable, continuity between consecutive motion primitives is enforced only on a subset of the state variables. This approach is compared with a state-of-the-art quadrotor-tailored search-based approach, which generates motion primitives online through control input discretization and forward propagation of the dynamic equations of a simplified model. The effectiveness of both methods is assessed through simulations and real-world experiments, demonstrating their ability to generate resolution-complete, resolution-optimal, collision-free, and dynamically feasible trajectories. Finally, a comparative analysis highlights the advantages, disadvantages, and optimal usage scenarios for each approach.

1. Introduction

Unmanned Aerial Vehicles (UAVs) have achieved significant popularity in recent years, enabling autonomous flight without the need for human pilots. The versatility of UAVs is evident in their autonomous navigation and task execution, including a wide range of applications in both civilian and military domains. These applications span surveillance, reconnaissance, package delivery, and exploration [1]. However, as the roles of UAVs evolve, navigating complex environments with agility becomes crucial. While path-planning algorithms ensure collision-free paths, their execution by the actual system may not be feasible. This is especially evident in agile vehicles, where additional limitations from dynamics or nonholonomic constraints must be considered. Kinodynamic motion planning addresses this issue, considering vehicle dynamics, input constraints, and state constraints.

Frazzoli et al. [2,3] provided some of the pioneering work on real-time kinodynamic motion planning. The proposed technique involves mapping the dynamics of the vehicle onto a finite-dimensional space, restricting the vehicle’s potential states to two categories: either a trim state or a maneuver state. Since then, the field has seen significant progress, yielding numerous trajectory-generation algorithms. Many researchers have adopted a two-step approach, separating geometric and temporal planning. Initially, an optimal collision-free geometric path is generated by linking a series of waypoints by straight lines. Subsequently, the path is smoothed and parameterized in time to ensure compliance with the vehicle dynamics. This smoothing step can be performed in various ways. For instance, one approach involves selecting the optimal sequence of trims and maneuvers from a precomputed library of motion primitives [4]. Alternatively, a sequence of polynomial segments can be jointly optimized to connect waypoints into a smooth trajectory from start to goal [5]. These two-step strategies significantly simplify the planning problem. However, the initial geometric path does not consider the dynamics of the vehicle, restricting the resulting trajectory to a specific homology class. This limitation may exclude the possibility of achieving a globally optimal (or even feasible) trajectory. Moreover, the final smoothed trajectory is not guaranteed to be obstacle-free.

Several studies have adopted a direct trajectory-generation approach that integrates the vehicle dynamics from the beginning, bypassing the two-step process. Many of these works leverage the inherent differential flatness property of quadrotor dynamics to represent trajectories using time-parameterized polynomials [6,7,8,9]. This transforms the trajectory-generation problem into the task of determining polynomial coefficients that meet specific constraints. While this approach has been extensively studied and applied in the context of quadrotors [6,7,8,9,10], its applicability is limited to systems with similar dynamics. Extending this methodology to more complex systems such as Vertical Take-Off and Landing (VTOL) and fixed-wing aircraft presents significant challenges.

Other stream of research use methods based on direct trajectory optimization, such as [11,12,13,14], to design trajectories that account for the full system dynamics. However, converting the planning problem into a non-convex optimization problem and using local optimization techniques can fail to find a collision-free trajectory in cluttered environments. Additionally, most trajectory optimization methods are prone to becoming stuck in local minima. Consequently, there is growing interest in constructing global solutions for motion planning problems. Early attempts to address this challenge formulated the problem as a single optimization problem using a mixed-integer optimization approach [15,16]. However, this method is tractable only for a limited number of obstacles. More recent approaches, such as those exploiting convex decomposition of the environment [17], allow for efficient computation but struggle with nonlinear dynamics, as this would result in a mixed-integer nonlinear problem, which is computationally intractable.

Motion primitives offer a promising alternative by combining the strengths of optimal control, which can handle arbitrary dynamics, with the effectiveness of graph-based algorithms that perform global searches in non-convex configuration spaces. This paper investigates motion primitives-based methods for addressing quadrotor kinodynamic motion planning, focusing on two main approaches. One approach [8] generates motion primitives online through control input discretization and forward propagation of the dynamic equations of a simplified model, serving as a benchmark for comparison. Additionally, the approach presented in [18] is revised and customized for quadrotor kinodynamic motion planning. Specifically, the differential flatness property of quadrotors is exploited to construct a library of precomputed trajectories during an offline phase. This library is then used online to concatenate the motion primitives using a search-based framework to obtain a complete trajectory. Moreover, to make the method computationally tractable, continuity between consecutive motion primitives is enforced only on a subset of the state variables (position, velocity, and acceleration). The methods are compared through both numerical simulations and real-world experiments to highlight the strengths and weaknesses of each approach.

The remainder of this paper is organized as follows: Section 2 provides an overview of the quadrotor model and its differential flatness property. Section 3 formulates the kinodynamic motion planning problem. Section 4 introduces the methodology presented in [18], aimed at generating an offline library of motion primitives tailored for systems with arbitrary dynamics. This method is then adapted to suit the quadrotor system within a search-based framework. Additionally, continuity between consecutive motion primitives is enforced on a subset of state variables to enhance computational tractability. Section 5 introduces the state-of-the-art approach proposed in [8], serving as a benchmark for comparison. Here, motion primitives for quadrotors are generated online through input discretization and forward propagation, employing a simplified model of vehicle dynamics. Section 6 presents the numerical results. Section 7 details the experimental outcomes. Finally, Section 8 offers a comparative analysis of the two methods, while Section 9 draws conclusions and delineates potential directions for future research.

Notation: $\mathbb{R}({\mathbb{R}}_{>0},{\mathbb{R}}_{\ge 0})$ denotes the set of (positive, nonnegative) real numbers, ${\mathbb{R}}^n$ denotes the n-dimensional real coordinate space and ${\mathbb{R}}^{m\times n}$ the set of $m\times n$ real matrices. Given $x\in {\mathbb{R}}^n$, $y\in {\mathbb{R}}^m$, denote $(x,y):={[x^{\top },y^{\top }]}^{\top }$.

The i-th vector of the canonical basis in ${\mathbb{R}}^n$ is denoted $e_i$ and the identity matrix in ${\mathbb{R}}^{n\times n}$ is $I_n:=[e_1\cdots e_i\cdots e_n]$. Given vectors $x,y\in {\mathbb{R}}^n$, the standard inner product is defined as $\langle x,y\rangle :=x^{\top }y$. The Euclidean norm of a vector $x\in {\mathbb{R}}^n$ is $\parallel x\parallel :=\sqrt{\langle x,x\rangle }$.

The n-dimensional unit sphere is denoted as ${\mathbb{S}}^n:=\{x\in {\mathbb{R}}^{n+1}:\parallel x\parallel =1\}$. The set $\mathrm{SO}\left(3\right):=\{R\in {\mathbb{R}}^{3\times 3}:R^{\top }R=I_3,det\left(R\right)=1\}$ is the 3D Special Orthogonal group. The map $S(·):{\mathbb{R}}^3\to \mathfrak{so}\left(3\right):=\{W\in {\mathbb{R}}^{3\times 3}:W=-W^{\top }\}$ is defined such that given $x,y\in {\mathbb{R}}^3$ one has $S\left(x\right)y=x\times y$.

2. Quadrotor Model

The quadrotor is modeled as a rigid body with six degrees of freedom (DOFs). The differential equations governing the flight are [19]

$$\begin{array}{cc}\hfill & \dot{p}=v\hfill \\ \hfill & m\dot{v}=-mg{e}_3+t_{c}R{e}_3\hfill \\ \hfill & \dot{R}=RS\left(\omega \right)\hfill \\ \hfill & J\dot{\omega }=-S\left(\omega \right)J\omega +{\tau }_c.\hfill \end{array}$$

(1)

In this formulation, $p=(x,y,z)\in {\mathbb{R}}^3$ and $v=(\dot{x},\dot{y},\dot{z})\in {\mathbb{R}}^3$ represent the position and velocity of the quadrotor in the inertial frame, respectively. $R\in \mathrm{SO}\left(3\right)$ is the rotation matrix describing the orientation of the body frame relative to the inertial frame, and $\omega \in {\mathbb{R}}^3$ denotes the angular velocity vector in the body frame. $m\in {\mathbb{R}}_{>0}$ is the quadrotor mass while $J=J^{\top }\in {\mathbb{R}}^{3\times 3}$ is its inertia matrix with respect to the center of mass. $t_c\in {\mathbb{R}}_{>0}$ denotes the thrust in the body frame and ${\tau }_c\in {\mathbb{R}}^3$ is a vector of three body torques. Finally, $g=9.81$ ms⁻² represents the gravity acceleration and $e_3=(0,0,1)$.

2.1. Differential Flatness

Equation (1) is inherently nonlinear and complex to manage. Nevertheless, the differential flatness property inherent to quadrotor systems can be leveraged to significantly simplify the motion planning problem [5] and the tracking problem [20]. As demonstrated in [21], the quadrotor is a flat system characterized by the following flat outputs:

$$\sigma =(p,\psi )\in {\mathbb{R}}^4,$$

(2)

where $\sigma$ is the vector containing the flat outputs. Specifically, these outputs consist of the coordinates of the center of mass, denoted as p, and the yaw angle $\psi$.

Differential flatness implies that all the state variables $x=(p,v,R,\omega )\in {\mathbb{R}}^{12}$ and control inputs $u=(t_c,{\tau }_c)\in {\mathbb{R}}^4$ can be written as algebraic functions of the flat outputs and a finite number of their derivatives. As a consequence, any trajectory in the state of the flat output is flyable as long as the flat outputs and their derivatives satisfy specific smoothness conditions. In particular, the position p must be at least four times differentiable, while the yaw angle $\psi$ must be at least twice differentiable (see Appendix A for further details).

3. Kinodynamic Motion Planning: Problem Statement

Kinodynamic motion planning aims to find the optimal trajectory for a vehicle navigating from an initial state $x_i$ to a final state $x_f$ while considering both its kinematics and dynamics.

The kinodynamic motion planning problem can be formulated as an optimization problem, with the aim of finding the input functions $u\left(t\right)$, the corresponding state trajectories $x\left(t\right)$, and the total duration T that minimize the cost function J, all while adhering to specified constraints. The formulation is as follows:

$$\begin{array}{}\mathrm{(3a)}& \underset{x\left(t\right),u\left(t\right),T}{\min }J={\int }_0^T{\parallel u\left(t\right)\parallel }^{2}dt+\rho T\mathrm{(3b)}& \mathrm{s}.\mathrm{t}.\dot{x}=f(x\left(t\right),u\left(t\right)),\forall t\in [0,T]\mathrm{(3c)}& x\left(t\right)\in \mathcal{X},\forall t\in [0,T]\hfill \mathrm{(3d)}& u\left(t\right)\in \mathcal{U},\forall t\in [0,T]\hfill \mathrm{(3e)}& x\left(0\right)=x_i,\hfill \mathrm{(3f)}& x\left(T\right)=x_f.\hfill \end{array}$$

Equation (3a) defines the cost function, which aims to strike a balance between minimizing the total time T and minimizing the control effort required for the trajectory. This balance is governed by the weight parameter $\rho$. Equation (3b) enforces the system’s dynamics outlined in Equation (1). Equation (3c) describes the state constraints, which consist of obstacle avoidance (non-convex) and limits on velocity and acceleration. Equation (3d) represents the control input constraints. The initial and final states in Equation (3e) and Equation (3f) correspond to the boundary conditions.

Explicitly solving Problem (3) is hard due to states and input constraints, as well as the non-convex nature of obstacle avoidance. An effective strategy to alleviate the computational load associated with motion planning involves transforming the problem into a graph-search problem. This method involves choosing a finite set of candidate solutions known as motion primitives. Motion primitives represent a collection of precomputed motions tailored for specific dynamic systems. These precalculated solutions address sub-problems, which can be concatenated to form a complete trajectory that effectively resolves the motion planning problem.

A motion primitive can be generated through various methodologies, including solving a Two-Point-Boundary-Value Problem (TPBVP) (see Section 4) or forward integration of the state equations (see Section 5). Additionally, motion primitives can be generated offline and stored in a database when dealing with complex systems (see Section 4) or generated online when a simplified model accurately represents the dynamics of the system (see Section 5).

4. Search-Based Kinodynamic Planning with Motion Primitives Library for Differentially Flat Quadrotors

This section builds upon the work presented in [18], which addresses motion planning for systems with arbitrary dynamics through a two-phase approach: offline and online. During the offline phase, the state space is discretized to obtain a suitable number of initial and final condition pairs. Motion primitives are then computed as solutions to a TPBVP, using these initial and final conditions as boundary conditions. Finally, these motion primitives are stored in a library. This process shifts the computational demands of trajectory generation to the offline phase, significantly reducing the computational load during online motion planning. In the online phase, the task of the planner is to select suitable motion primitives from the library to generate a complete trajectory. This two-phase approach enables efficient online motion planning for various systems, including complete quadrotor models as well as more complex systems like fixed-wing and VTOL aircraft.

In this section, this methodology is modified by tailoring it specifically to the quadrotor system, leveraging its inherent differential flatness property. Moreover, to make the method computationally tractable, continuity between consecutive motion primitives is enforced only on a subset of the state variables. Finally, the original approach is modified by utilizing a search-based planner rather than a sampling-based one.

4.1. Motion Primitives Library

Given an initial and a final condition, a motion primitive is the solution to the following TPBVP:

$$\begin{array}{cc}\hfill & \underset{x\left(t\right),u\left(t\right),\tau }{\min }J={\int }_0^{\tau }{\parallel u\left(t\right)\parallel }^{2}dt+\rho \tau \hfill \\ \hfill \mathrm{s}.\mathrm{t}.& \dot{x}=f(x\left(t\right),u\left(t\right)),\forall t\in [0,\tau ]\hfill \\ \hfill & x\left(t\right)\in \mathcal{X},\forall t\in [0,\tau ]\hfill \\ \hfill & u\left(t\right)\in \mathcal{U},\forall t\in [0,\tau ]\hfill \\ \hfill & x\left(0\right)=x_i,\hfill \\ \hfill & x\left(\tau \right)=x_f.\hfill \end{array}$$

(4)

Unlike the TPBVP presented in Problem (3), this formulation focuses solely on generating a short-duration motion primitive rather than solving the overall motion planning problem. Consequently, the total trajectory duration T is replaced with the primitive duration $\tau$, and the final state $x_f$ is no longer the goal; instead, it is determined by the desired state space discretization.

In traditional TPBVPs, enforcing system dynamics as constraints can be complex and computationally demanding. However, for flat quadrotor systems, since all state variables x and control inputs u can be expressed in terms of flat outputs $\sigma =(p,\psi )$ and their derivatives, Problem (4) can be reformulated in the space of flat outputs where the system dynamics in Equation (1) are inherently satisfied (see Appendix A for further details). Moreover, when the goal is to generate a smooth trajectory, the yaw angle $\psi$ is typically negligible, and therefore it is omitted here.

For flat quadrotors, the position along each axis can be represented as a k-th order polynomial

$$p\left(t\right)=c_k{t}^k+\cdots +c_{1}t+c_0\in {\mathbb{R}}^3.$$

(5)

The corresponding velocity $v\left(t\right):=\dot{p}\left(t\right)$, acceleration $a\left(t\right):=\ddot{p}\left(t\right)$ and jerk $j\left(t\right):=\stackrel{⃛}{p}\left(t\right)$ can be obtained by taking the derivatives of Equation (5). Then, Problem (4) can be reformulated as follows:

$$\begin{array}{cc}\hfill & \underset{\tau ,c_i,i=1:k}{\mathrm{minimize}}J={\int }_0^{\tau }{\parallel p^{\left(q\right)}\left(t\right)\parallel }^{2}dt+\rho \tau \hfill \\ \hfill \mathrm{s}.\mathrm{t}.& \parallel \dot{p}\left(t\right)\parallel \le v_{max},\forall t\in [0,\tau ]\hfill \\ \hfill & \parallel \ddot{p}\left(t\right)\parallel \le a_{max},\forall t\in [0,\tau ]\hfill \\ \hfill & \parallel \stackrel{⃛}{p}\left(t\right)\parallel \le j_{max},\forall t\in [0,\tau ]\hfill \\ \hfill & ⋮\hfill \\ \hfill & \parallel p^{\left(q\right)}\left(t\right)\parallel \le p_{max}^{\left(q\right)},\forall t\in [0,\tau ]\hfill \\ \hfill & p\left(0\right)=p_i,p\left(\tau \right)=p_f\hfill \\ \hfill & \dot{p}\left(0\right)=v_i,\dot{p}\left(\tau \right)=v_f,\hfill \\ \hfill & \ddot{p}\left(0\right)=a_i,\ddot{p}\left(\tau \right)=a_f.\hfill \end{array}$$

(6)

where $p^{\left(q\right)}\left(t\right)$ is the qth derivative of the position. Additional constraints, such as initial and final conditions on jerk and snap, can also be considered.

The motion primitives database is generated through an offline process where Problem (6) is solved for a suitable number of initial and final condition pairs obtained via discretization of the state space. Subsequently, the resulting optimal trajectories and optimal costs are stored in a Look-Up Table (LUT). Note that thanks to the differential flatness property, the library can be efficiently stored, as each primitive is defined by a set of coefficients $c_i$ and a duration $\tau$.

Obstacle avoidance is not addressed at this stage of motion library generation. Once the library is computed, the planner will concatenate the motion primitives online to construct a complete trajectory while considering obstacles. To concatenate the primitives, the initial conditions of a candidate primitive must coincide with the final conditions of the previous one. Ideally, one would grid the entire state space and construct the motion primitives accordingly. However, this would result in a prohibitively large library. Therefore, in practice, these final and initial conditions are enforced only on a subset of the state variables. This ensures the continuity of the planned trajectory for this specific subset. The remaining state variables will be continuous along each motion primitive and will experience discontinuity only at the points where primitives are concatenated.

It is worth noting that in cases where a vehicle model is not available, trajectories can be obtained directly from experimental flight data [2]. Therefore, it can be safely assumed that primitives are inherently compatible with vehicle dynamics.

Figure 1 shows a subset of the motion primitives database for a quadrotor originating from the steady state $x_i=y_i=v_{xi}=v_{yi}=0$.

Invariance Properties

The motion primitive library can grow to include many primitives, requiring a considerable amount of memory for database storage. However, several invariance properties, extensively explored in previous works [3,22], can be leveraged to minimize this requirement.

Dynamic systems, characterized by translation invariance, demonstrate consistent behavior under coordinate translations. This means that all trajectories can be rigidly translated to start from any arbitrary point. This property allows for the maintenance of a small database while covering the entire vehicle operating space. In practice, for the two-dimensional case, the initial position can be set to $({\widehat{x}}_i,{\widehat{y}}_i)=(0,0)$ during the database construction phase. Subsequently, the motion primitives can be easily translated to match any other initial position $(x_i,y_i)$, as shown in Figure 2.

In particular, a first translation is performed on the pair of initial and final states $(v_i,v_f)$ (Figure 2a). This translation aims to relocate the initial state $v_i$ to the origin of the library (Figure 2b), resulting in a new pair of states $({\widehat{v}}_i,{\widehat{v}}_f)$. Subsequently, a database query is executed to find the trajectory connecting these two states. Finally, an inverse translation is applied to restore the trajectory connecting the original pair of boundary states $(v_i,v_f)$ (Figure 2c).

Additionally, specific systems, like quadrotors, possess additional invariance properties, being invariant to horizontal plane translations and rotations about the vertical axis. Observing Figure 1 it becomes evident that the motion primitives for the quadrotor are symmetric with respect to both the x- and y-axes. As a result, only trajectories within the first quadrant (depicted by black curves) need to be stored in the database. Trajectories in other quadrants (depicted by pink curves) can be generated through mirroring.

4.2. Library-Based Kinodynamic Motion Planning

The motion primitives library establishes a finite lattice discretization within the state space, enabling the construction of a graph representation $\mathcal{G}(\mathcal{V},\mathcal{E})$. Here, $\mathcal{V}$ represents the set of states, and $\mathcal{E}$ represents the set of edges. The states in $\mathcal{V}$ are the initial and final conditions obtained through the state space discretization. The edges in $\mathcal{E}$ are defined by selecting a subset of the motion primitives library, ensuring continuity of the desired state variables with those of the previous motion primitive. Through this graph representation, the motion planning problem can be rewritten as a graph-search problem. This search problem can be solved using either search-based [8,9], or sampling-based [2,3,18,23] algorithms, which differ mainly in the optimality of the solution and computational time.

Sampling-based methods avoid the explicit construction of the configuration space by randomly or systematically sampling the space to build a graph representation of the environment. Search-based methods, on the other hand, employ traditional search algorithms to navigate a predefined representation of the configuration space. The primary distinction between these two methods lies in their approach to motion planning. Search-based methods aim to find optimal solutions, prioritizing optimality, while sampling-based methods focus on finding feasible solutions, emphasizing practicality and efficiency. Therefore, sampling-based algorithms are fast and particularly useful for complex and high-dimensional spaces. However, sampling-based methods employ a randomized approach, which could represent an obstacle when fast online replanning is required. Specifically, paths generated by sampling-based methods during successive replanning phases may exhibit significant variations, making them less suitable for applications where path consistency is crucial [9].

Based on these considerations, the A* algorithm [24] is employed to efficiently solve this graph-search problem. A* employs the GetSuccessorsLibrary procedure described in Algorithm 1 to explore the state space and build the graph. When provided with the current node under expansion v, this procedure is employed to query the database of motion primitives. This process involves a sequence of continuity checks and translations. Specifically, line 5 performs the continuity check, selecting primitives whose initial state $x_i$ matches the current node v. Line 6 executes the primitive translation, as illustrated in Figure 2b,c. In line 7, a collision check is performed on the translated primitive $p{r}^{\prime }$. If it passes the check, its final state $x_f^{\prime }$ is added to the successors list along with its associated cost.

Algorithm 1 Given the current node under expansion v and the motion primitive library $\mathcal{L}$, including initial states $x_i$, final states $x_f$, connecting primitives $pr$, and their associated effort costs c, find the set of successors Succ(v) and their cost SuccCost(v)

1: function GetSuccessorsLibrary(v, $\mathcal{L}$) 2: Succ(v) $\leftarrow \varnothing$; 3: SuccCost(v) $\leftarrow \varnothing$; 4: for all $(x_i,x_f)\in \mathcal{L}$ do 5:    if  $isEqual(x_i,v)$  then 6:       $p{r}^{\prime }\leftarrow$TranslatePrimitive($v,pr$); 7:       if isCollisionFree($p{r}^{\prime }$) then 8:    $x_f^{\prime }\leftarrow p{r}^{\prime }\left(\tau \right)$; 9:    Succ(v) ← Succ(v) $\cup \{x_f^{\prime }\}$; 10:     SuccCost(v) ← SuccCost(v) $\cup \{c+\rho \tau \}$; 11:      end if 12:    end if 12: end for 14: return Succ(v), SuccCost(v); 15: end function

5. Search-Based Kinodynamic Planning with Online Motion Primitives

This section describes the state-of-the-art approach outlined in [8], serving as a benchmark for comparison with the method presented in Section 4. This method introduces a search-based planning method for computing obstacle-free and dynamically feasible trajectories for quadrotors navigating obstacle-cluttered environments. In this approach, motion primitives are generated online through input discretization and forward propagation of the dynamic equations of a simplified model, leveraging the differential flatness property of quadrotors. Specifically, each motion primitive is created by applying a constant control input to an initial state for a predefined duration. These primitives induce a finite lattice discretization of the state space, enabling a graph representation. As a result, the motion planning problem can be transformed into a graph-search problem, which can then be solved using search-based algorithms.

5.1. Motion Primitives Generation

As previously stated, by leveraging the differential flatness property of quadrotors, the position along each axis can be expressed as a k-th order polynomial (Equation (5)). These polynomial trajectories can be derived from a linear time-invariant system

$$\begin{array}{cc}\hfill \dot{x}& =Ax+Bu\hfill \\ \hfill \dot{x}& =\left[\begin{array}{ccccc}0& I_3& 0& \dots & 0\\ 0& 0& I_3& \dots & 0\\ ⋮& \ddots & \ddots & \ddots & ⋮\\ 0& \dots & \dots & 0& I_3\\ 0& \dots & \dots & 0& 0\end{array}\right]x+\left[\begin{array}{c}0\\ 0\\ ⋮\\ 0\\ I_3\end{array}\right]u,\hfill \end{array}$$

(7)

where the state $x\left(t\right)$ consists of position $p\left(t\right)$ and its $(k-1)$ derivatives (velocity v, acceleration a, jerk j, snap s, etc.) in a three-dimensional space $x\left(t\right)=(p\left(t\right),\dot{p}\left(t\right),\ddot{p}\left(t\right),\dots ,p^{(k-1)}\left(t\right))$. Upon examination of Equation (7), it becomes evident that the quadrotor dynamics exhibit decoupling along each axis. Consequently, one can approach each axis as a double, triple, or k integrator system [8,10].

A motion primitive is generated independently for each of the three spatial axes by applying a constant control input $u_l\in {\mathcal{U}}_{\mathcal{L}}$ to an initial state $x_i=(p_i,v_i,a_i,\dots )$ for a duration $\tau$. The resulting trajectory takes the form outlined in Equation (5). Equivalently, the trajectory resulting from the linear time-invariant system described in Equation (7) is represented as follows:

$$x\left(t\right)=e^{At}{x}_i+\left[{\int }_0^t{e}^{A(t-\beta )}Bd\beta \right]u_l=F\left(t\right)x_i+G\left(t\right)u_l.$$

Following the procedure proposed in [8], a discretization ${\mathcal{U}}_{\mathcal{L}}=\{u_1,\dots ,u_L\}$ of the admissible control input set $\mathcal{U}=[-u_{max},u_{max}]$ is considered. Each control $u_l\in {\mathbb{R}}^3$ defines a short-duration trajectory, denoted motion primitive. To create this discretization, a discrete number of samples $N_s$ is selected along each axis within the permissible control input range $[0,u_{max}]$. This choice of samples results in a discretization step $du=\frac{u_{max}}{N_s}$, which, in turn, generates a total of ${(2N_s+1)}^3$ motion primitives [25]. Figure 3 shows an example of motion primitives generated through this approach.

The generated motion primitives are feasible with respect to control input constraints (e.g., acceleration), as they are derived from inputs within the admissible control input set. However, it is important to note that constraints regarding other state variables (e.g., velocity) and obstacle avoidance are not taken into account during the motion primitive generation phase. Each primitive must be evaluated against these constraints to ensure a trajectory that is both dynamically feasible and collision-free. This feasibility check occurs online, which is inherently time-consuming and significantly slows down real-time trajectory computation.

5.2. Graph Construction

Motion primitives create a finite lattice discretization within the state space, enabling the construction of a graph representation $\mathcal{G}(\mathcal{V},\mathcal{E})$, as illustrated in Figure 4. Here, $\mathcal{V}$ represents the set of states and $\mathcal{E}$ represents the set of edges. The states in $\mathcal{V}$, depicted as blue dots in Figure 4, are generated by iteratively applying each control input $u_l\in {\mathcal{U}}_L$ for a duration $\tau$ to each initial state. This process results in ${(2N_s+1)}^3$ distinct motion primitives. Among these, the motion primitives that are both collision-free and dynamically feasible, shown as black curves in Figure 4, form the set of edges $\mathcal{E}$. Motion primitives that collide with obstacles or are dynamically unfeasible (e.g., exceed velocity limits), depicted as red curves in Figure 4, are excluded from the graph representation.

5.3. Search-Based Kinodynamic Motion Planning

By leveraging the graph representation, the motion planning problem can be reformulated as a graph-search problem, representing the discrete version of Problem (3). Based on [25], this can be done by treating the control as piecewise constant over intervals of duration $\tau$: $u\left(t\right)={\sum }_{n=0}^{N-1}{u}_n$. Given an initial state $x_i$, a goal region ${\mathcal{X}}_{goal}$, and a finite set of motion primitives, each with a duration $\tau >0$, the objective is to determine the optimal sequence of control inputs $u_n,n=0,...,N-1$ such that

$$\begin{array}{cc}\hfill & \underset{u_n,x_n,N}{\min }\left(\sum _{n=0}^{N-1}{\parallel u_n\parallel }^2+\rho N\right)\tau \hfill \\ \hfill \mathrm{s}.\mathrm{t}.& x_n\left(t\right)=F\left(t\right)x_{i,n}+G\left(t\right)u_n,\forall n=0,\dots ,N-1,t\in [0,\tau ]\hfill \\ \hfill & x_n\left(t\right)\in \mathcal{X},\forall n=0,\dots ,N-1,t\in [0,\tau ]\hfill \\ \hfill & u_n\in {\mathcal{U}}_L,\forall n=0,\dots ,N-1\hfill \\ \hfill & x_{i,n+1}=x_n\left(\tau \right),\forall n=0,\dots ,N-1\hfill \\ \hfill & x_{i,0}=x_i,\hfill \\ \hfill & x_{i,N}\in {\mathcal{X}}_{goal}.\hfill \end{array}$$

(8)

As mentioned in Section 2.1, the position p is required to be four times differentiable, and the yaw angle $\psi$ must be twice differentiable to fully define the state (see Appendix A for further details). Under these conditions, Equation (1) is automatically satisfied. However, lower degrees of differentiability are often utilized for computational efficiency.

The A* algorithm [24] is used to solve Problem (8). A* employs the GetSuccessors procedure outlined in Algorithm 2 to explore the state space and construct the graph. In essence, for each control action within the admissible control input set $u_l\in {\mathcal{U}}_L$, a motion primitive is generated. This involves applying the selected control action $u_l$ to the current node v for a duration $\tau$. Subsequently, the trajectory’s dynamic feasibility and obstacle avoidance are evaluated. If both criteria are met, the successor node $v_f$ is added to the successors list along with the cost of the primitive.

Algorithm 2 Given the current node v and the discretized control set ${\mathcal{U}}_L$, find the set of successors of v Succ(v) and their associated cost SuccCost(v)

1: function GetSuccessors(v, ${\mathcal{U}}_L$,$\tau$) 2: Succ(v) $\leftarrow \varnothing$; 3: SuccCost(v) $\leftarrow \varnothing$; 4: for all $u_l\in {\mathcal{U}}_L$ do 5:    pr ←GeneratePrimitive(v,$u_l$,τ); 6:    if isDynamicallyFeasible(pr) and isCollisionFree(pr) then 7:       $v_f\leftarrow$ pr($\tau$); 8:       Succ(v) ← Succ(v) $\cup \left\{v_f\right\}$; 9:       SuccCost(v) ← SuccCost(v) $\cup \left\{\right(\parallel u_l{\parallel }^2+\rho \left)\tau \right\}$; 10:   end if 11: end for 12: return Succ(v), SuccCost(v); 13: end function

6. Numerical Simulations

This section presents the numerical simulations performed to demonstrate the effectiveness of the two methods. Specifically, a quadrotor is tasked with navigating from an initial state located at $(x_i,y_i)=(2,1)$ m to a final state positioned at $(x_f,y_f)=(42,12)$ m within a 2D 45 m × 15 m virtual environment. At both the initial and final states, velocity and eventually accelerations are set to zero.

Motion primitives library: To guarantee the automatic satisfaction of the system dynamics described in Equation (1), the position p must be at least four times differentiable. Therefore, a fifth-order polynomial representation of the 2D position is considered. This ensures continuity in terms of position, velocity, acceleration, jerk, snap, and the derivative of snap along each primitive. However, to prevent the library from becoming prohibitively large, the initial and final conditions for database generation are enforced only on position, velocity, and acceleration. As a result, the planned trajectory will be continuous in position, velocity, and acceleration. The remaining state variables will be continuous along each motion primitive and will experience discontinuity at the concatenation points.

The motion primitives library is generated by solving a particular instance of Problem (6) for every combination of initial and final states, employing MATLAB’s R2024a fmincon function. Specifically, a trade-off between the integral of the jerk squared and primitive duration is minimized. Constraints are imposed on total acceleration and total jerk, set, respectively, at $a_{max}=3\sqrt{2}$ ms⁻² and $j_{max}=15\sqrt{2}$ ms⁻³. Note that by leveraging the differential flatness property, the imposition of constraints on acceleration and jerk effectively restricts the commanded thrust and commanded angular rates to ensure adherence to the actuation constraints. This ensures that the motion primitives remain feasible and executable within the physical constraints of the platform. Additional information can be found in Appendix A.

Online motion primitives: To achieve a dynamically feasible trajectory, a fourth-order polynomial representation of position should be employed, with the snap serving as control input. However, planning in higher-dimensional spaces requires more time. Additionally, discretizing the snap is not intuitive due to its lack of direct physical meaning. Furthermore, an online feasibility check, as described in Section 5.1, must be conducted for velocity, acceleration, and jerk. This process is computationally intensive when performed online, making it inefficient. In practice, it is more effective to sample over acceleration and employ second-order polynomials for position, as done in [8]. This approach restricts the feasibility check to velocity alone, significantly reducing the online computational load. Additionally, discretizing acceleration is straightforward due to the well-understood impact of acceleration on the behavior of the system. Therefore, in this example, second-order polynomials are used to represent position. The 2D quadrotor state is defined as $x\left(t\right)=(x,y,v_x,v_y)$ and acceleration $a=(a_x,a_y)$ is used as control input. Total velocity $\parallel v\parallel =\sqrt{v_x^2+v_y^2}$ is constrained to be lower than $1.5\sqrt{2}$ ms⁻¹. Maximum acceleration along each axis is set to 3 ms⁻².

6.1. Parameters Selection and State Space Discretization

This section explains how the discretization of the state space, as well as input discretization ${\mathcal{U}}_{\mathcal{L}}$ and the primitive duration $\tau$, influence planning performance. Furthermore, it elaborates on their selection process tailored to the specific application at hand.

State space discretization for library computation: The discretization of the state space significantly affects planning performance, including computation time and solution optimality. The grid resolution determines the density of the graph and the computation time. A fine-resolution grid results in a denser graph, allowing for a more complete search and smoother trajectories. However, a denser graph requires more exploration to reach the goal, thus increasing computation time due to the higher number of nearby nodes to be checked. Conversely, a coarse resolution grid limits the set of candidate solutions, which can potentially result in search failures. Specifically, if the state space discretization is not fine enough, the resulting problem could become infeasible. However, for computational reasons, a coarse grid is preferable as it reduces the number of neighbor connections to be evaluated, therefore speeding up the search process.

Determining the optimal state space discretization depends on the specific application. For this specific application, the following state space discretization has been found suitable. The database is assembled starting from an initial position at $(x_i,y_i)=(0,0)$. It is based on a uniform square grid, where the final state position coordinates $(x_f,y_f)\in [-4,0)\cup (0,4]\times [-4,0)\cup (0,4]$ and each square cell spans one meter. These values have been selected based on the simulation environment. Given that the environment is large and filled with obstacles, it is beneficial to include both long (e.g., 4 m) and short (e.g., 1 m) motion primitives in the library. This approach allows the system to use shorter motion primitives for navigating through obstacle-dense regions to avoid collisions and longer primitives for traversing obstacle-free areas, ensuring smoother motion. Initial and final velocities ($v_{xi},v_{yi},v_{xf},v_{yf}$) and initial and final accelerations ($a_{xi},a_{yi},a_{xf},a_{yf}$) are each selected from a set of three values to prevent the library from becoming prohibitively large. Specifically, initial and final velocities are selected from $\{-1.5,0,1.5\}$ ms⁻¹, and initial and final accelerations are chosen from $\{-3,0,3\}$ ms⁻².

Control input discretization and motion primitive duration for online motion primitives computation: The motion primitives duration $\tau$ and the control input discretization ${\mathcal{U}}_{\mathcal{L}}$ have a significant effect on the planning performance, including computation time and optimality of the solution. The primitives duration $\tau$ determines the density of the graph and computation time. A small $\tau$ results in a denser graph, requiring more exploration to reach the goal, therefore increasing computation time. Conversely, large $\tau$ reduces the space of candidate solutions, potentially leading to search failures. Specifically, if the discretization of the control input ${\mathcal{U}}_{\mathcal{L}}$ is not fine enough, the resulting problem could become infeasible (i.e., the set of candidate solutions could be empty). Moreover, since the goal is to optimize the duration of the overall trajectory, the duration of the primitives $\tau$ imposes a lower bound on the optimality of the solution. As the duration of the motion primitives decreases, so does the cost of the optimal trajectory. However, for computational reasons, a longer duration is preferable. The control space discretization ${\mathcal{U}}_{\mathcal{L}}$ also influences the density of the graph and computation time: finer discretization yields a slower but more complete search and smoother trajectories, while coarser discretization produces the opposite effect. Moreover, finer control space discretization ${\mathcal{U}}_{\mathcal{L}}$ significantly increases computation time, making the approach impractical in practice.

The optimal combination of control input discretization and primitive duration depends on the specific application. For this specific application, the following values have proven to be suitable. To ensure a balance between search completeness and computational efficiency, the set of control inputs is discretized into nine options within the allowable range of $(u_x,u_y)=(a_x,a_y)\in [-3,3]$ ms⁻² and the primitive duration is set at $\tau =0.5$ s.

6.2. Simulations Results

This section presents the results of the numerical simulations.

Planning with motion primitives library: Figure 5 shows the optimal collision-free trajectory guiding the vehicle from the initial state (green dot) to the goal state (red dot). Figure 6, Figure 7 and Figure 8 depict the velocity, acceleration, and actuation profiles. These figures demonstrate the efficacy of this approach in generating collision-free and dynamically feasible trajectories. Observing the velocity profile in Figure 6, it is evident that rather than experiencing a gradual and continuous increase in velocity, sudden changes in acceleration occur. This issue arises from constraining velocity and acceleration at each node to match one of the values in the database precisely. To obtain a smoother trajectory, a finer state space discretization is required.

Planning with online motion primitives: Figure 9 shows the optimal collision-free trajectory guiding the vehicle from the start state (green dot) to the target state (red dot). Figure 10 and Figure 11 show the velocity profile and the actuation (acceleration) profile for the optimal trajectory, demonstrating adherence to velocity constraints.

7. Experimental Results

This section presents the experimental tests performed to demonstrate the effectiveness of the two methods on a real quadrotor.

7.1. Experimental Set-Up

Flight tests take place within the indoor Flying Arena for Rotorcraft Technologies (FlyART) at Politecnico di Milano, equipped with a Motion Capture system (Mo-Cap) comprising 12 cameras (Figure 12). The UAV employed for these tests is an ANT-X fixed-pitch quadrotor [26] (Figure 12). The Mo-Cap system detects markers mounted to the UAV and collects measurements, which are then transmitted to a ground control station. These measurements, along with the trajectory to be followed defined by specific waypoints, are forwarded to the drone at frequencies of 100 Hz and 20 Hz, respectively.

Note that the maximum velocity $v_{max}$ and acceleration $a_{max}$ constraints generally come from the dynamics of the system in question, such as the thrust-to-weight ratio. The platform utilized in the experimental tests has a thrust-to-weight ratio of approximately $2.5$. However, it is often necessary to impose constraints on maximum velocity and acceleration due to spatial limitations and safety concerns. As a result, lower values are selected as constraints for the experimental tests.

The control system employed follows a set-up similar to the PX4 autopilot, employing a cascaded controller structure. Specifically, it features a double cascade arrangement using P/PID controllers for position and attitude control. Moreover, this set-up involves modifications to the PX4 autopilot, incorporating position, velocity, and acceleration feedforwards. For more details, please refer to [27,28].

The motion planning computations are executed in MATLAB on a personal computer equipped with an Intel Core i7 processor running at 2.30 GHz and 16 GB RAM. Future work will involve converting all components of the online phase to C++ for execution on an embedded processor within the quadrotor.

The UAV operates in a 10 m × 4 m environment featuring two 0.4 m × 1.5 m obstacles. The starting position is set at $p_i=(-3.5,0.5,1)$ m, and the goal region is a square with a side length of 0.5 m, centered at $p_f=(3,-0.5,1)$ m. Velocity and acceleration are zero at both the initial and final positions.

7.2. Experiment: Planning with Motion Primitives Library

A fifth-order polynomial representation of the 2D position is employed for this experiment. The database of motion primitives is computed by solving a particular instance of Problem (6) for every combination of boundary conditions. The database is assembled starting from an initial position at $(x_i,y_i)=(0,0)$. It is based on a uniform square grid, where the final state position coordinates $(x_f,y_f)\in [-2,0)\cup (0,2]\times [-2,0)\cup (0,2]$ and each square cell spans half a meter. Initial and final velocities are selected from $\{-1.5,0,1.5\}$ ms⁻¹, and initial and final accelerations are chosen from $\{-4.5,0,4.5\}$ ms⁻². Total velocity, acceleration, and jerk limits are set at $v_{max}=1.5\sqrt{2}$ ms⁻¹, $a_{max}=4.5\sqrt{2}$ ms⁻² and $j_{max}=15\sqrt{2}$ ms⁻³. In this experiment, a constraint on the maximum speed is added due to spatial limitations and safety concerns. Refer to Section 6.1 for insights on selecting the state space discretization.

Figure 13 shows the obstacle-free planned trajectory. Figure 14 and Figure 15 depict the position and velocity profiles with their references, demonstrating precise tracking. References represent the desired positions and velocities of the planned trajectory. Specifically, they are the target values for each motion primitive that composes the planned trajectory. These target values are fed to the control system to ensure tracking and execution of the planned path.

7.3. Experiment: Planning with Online Motion Primitives

In this experiment, second-order polynomials are used to represent position. The 2D quadrotor state is defined as $x\left(t\right)=(x,y,v_x,v_y)$ and acceleration $a=(a_x,a_y)$ is used as control input. The set of control inputs is discretized with nine options within the allowable range of $(u_x,u_y)=(a_x,a_y)\in [-3,3]$ ms⁻² and the duration of the primitives is set to a fixed value of $\tau =0.5$ s. Total velocity $\parallel v\parallel =\sqrt{v_x^2+v_y^2}$ is constrained to be lower than $1.5\sqrt{2}$ ms⁻¹. Refer to Section 6.1 for insights on selecting the input discretization and primitive duration.

Figure 16 depicts the planned obstacle-free trajectory. Figure 17 and Figure 18 show the tracked position and velocity profiles alongside their corresponding references, demonstrating good tracking.

8. Discussion

This section delves into a comparative analysis of the two approaches, each offering distinct advantages. The selection of the appropriate method depends on the specific characteristics of the system at hand and its performance requirements. For a concise overview, the key features of both methods are summarized in

Table 1. Key features of the two methods.

Planning with Online Motion Primitives	Planning with Motion Primitives Library
suitable for simple systems	suitable for complex systems
fixed-duration primitives	optimized-duration primitives
online feasibility check	offline feasibility check
online collision check	online collision check
meticulous control input and duration tuning	meticulous grid discretization

and discussed in the following sections.

8.1. Adaptability to Complex Systems

The planning with online motion primitives method is advantageous when a simplified representation of the system accurately captures its dynamics. This approach is particularly useful in scenarios where moderate performance and tracking are acceptable, making a simplified model adequate to satisfy these requirements. However, its applicability to more complex systems is limited. On the other hand, the planning with motion primitives library method is the preferred choice when a simplified representation of the system fails to meet performance requirements, necessitating a more complex model. In fact, this method extends its applicability beyond differentially flat quadrotor systems to encompass a wide range of complex systems, including VTOL and fixed-wing aircraft. Its versatility is attributed to its capacity to shift the computational demands of trajectory generation to an offline phase, therefore ensuring precise solutions without compromising efficiency. Furthermore, this method is advantageous in scenarios requiring aggressive and agile maneuvers, where precise tracking and the full utilization of the system’s capabilities are essential.

8.2. Motion Primitives Duration

In the planning with online motion primitives method, all the motion primitives have the same duration, denoted as $\tau$, which is usually set sufficiently large for computational reasons. However, this hinders the overall objective of achieving a minimum-time trajectory. In contrast, the planning with motion primitives library method employs minimum-time primitives generated by solving a TPBVP, aligning more closely with the objective of achieving a minimum-time trajectory.

8.3. Motion Primitives Feasibility Check

The planning with online motion primitives method generates motion primitives through control input discretization and forward propagation of the dynamic equations of a simplified model. Consequently, the resulting primitives are feasible with respect to control inputs (e.g., acceleration) but do not inherently incorporate velocity constraints, necessitating a subsequent feasibility check. This additional step involves further computations and significantly slows down the online trajectory computation. In contrast, the planning with motion primitives library method directly incorporates velocity, acceleration, and other constraints into its library, eliminating the need for a subsequent feasibility check and accelerating online trajectory computation.

8.4. Further Considerations

In a known environment, geometric considerations on the environment itself (e.g., environment dimensions, characteristics distances between obstacles) easily translate into grid selection. On the contrary, the tuning process for the control input set and the duration of primitives $\tau$ is more challenging, especially for complex systems.

9. Conclusions

This work addresses trajectory planning for UAVs in the presence of obstacles by introducing a library of motion primitives tailored for quadrotors. The versatility of this approach lies in its ability to shift the computational demands of trajectory generation to the offline phase of database construction. To further enhance computational tractability, continuity between consecutive motion primitives is enforced only on a subset of state variables (position, velocity, and acceleration). This method has proven effective in maintaining computational efficiency while ensuring accurate trajectory planning and proves promising for applications to more complex systems, including VTOL and fixed-wing aircraft.

The proposed method is compared with a state-of-the-art quadrotor-tailored search-based planner, which generates motion primitives online by discretizing control inputs and propagating the dynamic equations of a simplified model forward. However, this state-of-the-art approach is computationally efficient only for simple systems, such as single or double integrators. Conversely, the proposed library-based method enables efficient motion planning for arbitrary systems.

Notably, both methodologies demonstrate the ability to generate resolution-complete, collision-free, resolution-optimal, and dynamically feasible trajectories, as demonstrated by numerical and experimental results. Furthermore, a comparison of the two approaches highlights their respective strengths and weaknesses.

Future research will focus on addressing motion planning in unknown and dynamic environments. This involves vision-based dynamic replanning using real-time data to navigate and avoid newly detected obstacles. Furthermore, upcoming studies will concentrate on developing motion primitive libraries for the identified model of the quadrotor. Additionally, efforts will be directed toward mitigating the discontinuity of the state variables at the nodes. Although enforcing continuity during the library construction phase presents memory challenges, a potential solution involves generating a trajectory that guarantees only position and velocity continuity at the nodes. This trajectory will serve as a high-quality initial guess for an online trajectory-generation algorithm, with continuity in other states achieved in a receding horizon fashion.