Path Planning Using Concatenated Analytically-Defined Trajectories for Quadrotor UAVs ^†

Abstract

This paper presents a semi-analytical trajectory planning method for quadrotor UAVs. These trajectories are analytically defined, are constant in speed and sub-optimal with respect to a weighted quadratic cost function of the translational and angular velocities. A technique for concatenating the trajectories into multi-segment paths is demonstrated. These paths are smooth to the first derivative of the translational position and pass through defined waypoints. A method for detecting potential collisions by discretizing the path into a coarse mesh before using a numerical optimiser to determine the point of the path closest to the obstacle is presented. This hybrid method reduces the computation time when compared to discretizing the trajectory into a fine mesh and calculating the minimum distance. A tracking controller is defined and used to show that the paths are dynamically feasible and the typical magnitudes of the controller inputs required to fly them.

1. Introduction

Unmanned aerial system (UAS) applications often require the vehicle to fly in areas with many obstacles. With this desire to fly in restricted space, such as urban environments, there is increasing need for better path and trajectory planning. Typical UAS civilian applications, such as data collection, environmental monitoring and security, require autonomous navigation to avoid collisions without the assistance of a pilot.

There are a variety of different UAS types, including fixed-wing planes, helicopters and multi-rotors. For agile navigation in small areas with tight space constraints or a dense field of obstacles, quadrotors are most suitable. Their ability to hover in a stationary position is also beneficial when awaiting instructions or collecting data using on-board sensors. In contrast, fixed-wing vehicles can efficiently cover greater distances and have faster maximum speeds. However, they cannot hover (barring experimental vehicles, such as [1]), which limits their use in certain scenarios. Additionally, their dynamic constraints restrict their manoeuvrability, and they must maintain a minimum airspeed to produce sufficient lift. Finally, helicopters offer similar performance and agility to multi-rotors, but their mechanical complexity is not justified at the scale of standard UAS. This paper will focus on trajectory generation for multi-rotors (specifically, quadrotors), because they have the least dynamic constraints; however, future work will investigate applying an extension of the methods to other vehicles.

The differential flatness of quadrotor dynamics can be exploited to allow for easier planning using the decoupled translational axes [2]. A yaw angle also needs to be defined; in the literature, this is normally fixed at zero. One technique for trajectory planning is to use an analytical function to describe the position over time. The shape of the function is altered by varying parameters. After defining an error function for the final state, a numerical minimization is performed to calculate the parameters that satisfy the boundary conditions. The derivatives of the state can be found by differentiating the analytical function. Functions that have already been used for quadrotor trajectory planning include splines [3], polynomials [4] and Bézier curves [5]. In this paper, we use the maximum principle of optimal control to define curves using standard functions that are sub-optimal with respect to a weighted quadratic cost function. The term sub-optimal is used as the analytical curves are the projection of optimal curves on $SE\left(3\right)$ onto $R^3$, and the rotational component of the optimal motion is not used in the motion planning.

A typical method of defining obstacles is to use p-norms [6] and is applicable in both two- and three-dimensional planning. This method can also be extended to polygonal shapes [7]. However, for the purpose of demonstrating the obstacle detection algorithm, the obstacles in this paper are defined as spheres. Obstacle avoidance using Dubins curves and sampling-based planning is considered in [8]. Methods for avoiding other moving vehicles and obstacles are given in [9,10].

This paper extends our conference paper [11] to develop concatenation of trajectories to form multi-segment paths and the obstacle detection algorithm. The format of this papers is as follows. The quadrotor dynamics and kinematics are presented in Section 2, with the analytical function defining the curves given in Section 3.1. The method for repositioning the generated curves is described in Section 3.2, and forming a single path from curves patched together is demonstrated in Section 5.2. An obstacle detection algorithm is presented in Section 4 and evaluated in Section 5.3. A tracking controller is defined (Section 5.1) and used to demonstrate the feasibility of the path in Section 5.2. Finally, the findings are summarised and future work discussed in Section 6.

2. Quadrotor Dynamics and Kinematics

A standard quadrotor has four motors and propellers arranged in a square formation. It has an inertial translational position, velocity and acceleration $x={[x_1,x_2,x_3]}^T$, $\dot{x}={[{\dot{x}}_1,{\dot{x}}_2,{\dot{x}}_3]}^T$ and $\ddot{x}={[{\ddot{x}}_1,{\ddot{x}}_2,{\ddot{x}}_3]}^T$, respectively. The angular velocities about the body axes are $\Omega ={[{\Omega }_1,{\Omega }_2,{\Omega }_3]}^T$, and the body frame velocities are $v={[v_1,v_2,v_3]}^T$. The forces generated by the propellers are $f={[f_1,f_2,f_3,f_4]}^T$, and the moments induced in the body-fixed frame are denoted as $M={[M_1,M_2,M_3]}^T$. The total thrust is defined as $F={\displaystyle \sum _{i=1}^4{f}_i}$. The mass of the quadrotor is denoted by m and the inertia matrix by $J\in R^{3\times 3}$. The body-frame thrust vector is $e_3={[0,0,1]}^T$. In matrix form, the total thrust and moments are:

$$\left[\begin{array}{c}F\\ M_1\\ M_2\\ M_3\end{array}\right]=\left[\begin{array}{cccc}1& 1& 1& 1\\ 0& -d& 0& d\\ d& 0& -d& 0\\ -c_{\tau }f& c_{\tau }f& -c_{\tau }f& c_{\tau }f\end{array}\right]\left[\begin{array}{c}{f}_1\\ f_2\\ f_3\\ f_4\end{array}\right]$$

(1)

where d is the distance between the centre of the quadrotor and $c_{\tau }f$ is a constant that relates thrust to induced yaw. In this paper, the quadrotor is treated as a rigid body with a constant gravitational acceleration $g_a$. To simplify the procedure of the motion planning and controller design, typical aerodynamic disturbances on the moments and forces, such as drag, ground effect and rotor dynamics, are neglected. The simulated quadrotor dynamics follow the derivations in [12,13,14]. The equations of motion are as described in [15]:

$$\dot{x}=Rv$$

(2)

$$m\ddot{x}=m{g}_a{e}_3-FR{e}_3$$

(3)

$$\dot{R}=R\widehat{\Omega }$$

(4)

$$J\dot{\Omega }+\Omega \times J\Omega =M$$

(5)

with $R\in SO\left(3\right)$, where:

$$\mathrm{SO}\left(3\right)\stackrel{\Delta }{=}\{R\in R^{3\times 3}:\begin{array}{ccc}{R}^{T}R=I& \mathrm{and}& \det \left(R\right)=1\}\end{array}$$

(6)

3. Trajectory Planning

The first subsection defines the curves and the basis behind them. The second demonstrates the process to reposition the curves from starting at the origin to a specified translational position and orientation in the inertial frame.

3.1. Sub-Riemannian Curves for Quadrotor Trajectory Planning

The kinematics, Equations (2) and (4), can be expressed equivalently on the Euclidean group of motions $SE\left(3\right)$:

$$\frac{dg}{dt}=g(\sum _{n=1}^3{B}_i{v}_i+\sum _{n=1}^3{A}_i{\Omega }_i)$$

(7)

where $A_1$, $A_2$, $A_3$, $B_1$, $B_2$ and $B_3$ are the basis elements of the Lie algebra of the Lie group $SE\left(3\right)$ [16,17,18,19] with $g\in SE\left(3\right)$:

$$g=\left(\begin{array}{cc}1& \begin{array}{ccc}0& 0& 0\end{array}\\ x& R\end{array}\right)$$

(8)

Body velocities for the planned motion were specified as $v_1=v_2={\Omega }_3=0$ to simplify the process of deriving analytical expressions for the reference curves. It is convenient to choose a single translational body velocity, so that the translational speed in the inertial frame is equal to the magnitude of that body velocity. The translational speed will also be constant, since after applying the maximum principle, it is shown that the translational body velocity is time invariant. Additionally, having a single non-zero body velocity facilitates the joining of trajectory segments using the method described in Section 3.2. Future work will consider the inclusion of the other body velocities. With the given restrictions on the body velocities, Equation (7) can be reduced to:

$$\frac{dg}{dt}=g(v_3{B}_3+A_1{\Omega }_1+A_2{\Omega }_2)$$

(9)

We choose to define a set of curves for motion planning that minimizes the cost function:

$$J=\frac{1}{2}{\int }_0^T{v}_3^2+c({\Omega }_1^2+{\Omega }_2^2)dt$$

(10)

subject to the nonholonomic constraint Equation (9) and given boundary conditions $g\left(0\right)=g_0$ and $g\left(T\right)=g_T$, where c is a constant weight and $v_i$ and ${\Omega }_i$ are measurable and bounded functions. A curve that satisfies the constraint Equation (9) and minimizes the cost function subject to the boundary conditions defines a sub-Riemannian curve on $SE\left(3\right)$ [19].

Sub-Riemannian curves are useful for trajectory planning, because they are smooth and globally defined on $SE\left(3\right)$, unlike Euler angles or quaternions that use local co-ordinates. They also naturally satisfy the constraint Equation (6). In this particular case, they can also be analytically defined, which reduces the motion planning problem to one of parameter optimisation. The full derivation for the curves is presented in Appendix. The curves are defined using standard functions; knowledge and understanding of their derivation are not required for their application. They are obtained by a projection of a particular sub-Riemannian curve $g_p\in SE\left(3\right)$ onto $x_p\in R^3$, where $x_p={[x_{p1},x_{p2},x_{p3}]}^T$ and:

$$\begin{array}{c}{x}_{p1}=\frac{c_2\nu }{\gamma }(\left(1-cos\gamma t\right)sin\beta +c_{1}cos\beta \left(sin\gamma t-\gamma t\right))\hfill \\ x_{p2}=\frac{c_2\nu }{\gamma }(\left(1-cos\gamma t\right)cos\beta -c_{1}sin\beta \left(sin\gamma t-\gamma t\right))\hfill \\ x_{p3}=\frac{c_2^2\nu }{\gamma }sin\gamma t+c_1^2\nu t\hfill \end{array}$$

(11)

where:

$$\begin{array}{c}s=-\sqrt{{\lambda }_1^2+{\lambda }_2^2}\hfill \\ {\lambda }_3=\frac{{\lambda }_4{\lambda }_1}{{\lambda }_2}\hfill \\ r=-\sqrt{{\lambda }_3^2+{\lambda }_4^2}\hfill \\ \beta =atan2(-{\lambda }_2,{\lambda }_1)\hfill \\ c_1=\frac{\nu }{\sqrt{r^2+{\nu }^2}}\hfill \\ c_2=\frac{r}{\sqrt{r^2+{\nu }^2}}\hfill \\ \gamma =\frac{s\sqrt{r^2+{\nu }^2}}{rc}\hfill \end{array}$$

(12)

The quadrotor speed ν and weighting c are chosen by the user before the optimisation. A desired final position $x_d={[x_{d1},x_{d2},x_{d3}]}^T$ is also specified by the user. A numerical optimisation using MATLAB’s $fmincon$ that implements a sequential quadratic programming method with an active-set region was used to find the optimisation vector $\Xi =[{\lambda }_1,{\lambda }_2,{\lambda }_4,T]$ that minimizes the error in the final position:

$$C_{x_f}=\sqrt{{(x_d\left(T\right)-x_p\left(T\right))}^2}$$

(13)

where T is the final time and constrained such that $T>0$. The translational velocity ${\dot{x}}_p$ can be defined analytically by taking the first derivative of Equation (11):

$$\begin{array}{c}{\dot{x}}_{p1}=c_2\nu (c_{1}cos\beta \left(cos\gamma t-1\right)+sin\beta sin\gamma t)\hfill \\ {\dot{x}}_{p2}=c_2\nu (c_{1}sin\beta \left(1-cos\gamma t\right)+cos\beta sin\gamma t)\hfill \\ {\dot{x}}_{p3}=\nu (c_1^2+c_2^{2}cos\gamma t)\hfill \end{array}$$

(14)

Likewise, the second derivative of Equation (11) gives the translational acceleration:

$$\begin{array}{c}{\ddot{x}}_{p1}=c_2\nu (\gamma cos\left(t\gamma \right)sin\left(\beta \right)-c_1\gamma cos\left(\beta \right)sin\left(t\gamma \right))\hfill \\ {\ddot{x}}_{p2}=c_2\nu (\gamma cos\left(t\gamma \right)cos\left(\beta \right)+c_1\gamma sin\left(\beta \right)sin\left(t\gamma \right))\hfill \\ {\ddot{x}}_{p3}=-{c_2}^2\nu \gamma sin\left(t\gamma \right)\hfill \end{array}$$

(15)

The magnitude of the acceleration is:

$$\parallel \mathbf{a}\parallel =\sqrt{{{\ddot{x}}_{p1}}^2+{{\ddot{x}}_{p2}}^2+{{\ddot{x}}_{p3}}^2}$$

(16)

It can be shown that $\parallel \mathbf{a}\parallel$ is constant by substituting Equation (15) into Equation (16) and simplifying:

$$\parallel \mathbf{a}\parallel =abs\left(c_2\nu \gamma \right)$$

(17)

The magnitude of the velocity is constant and simply ν, as determined by the user before calculating the optimisation vector.

3.2. Trajectory Repositioning and Reorientation

For the particular curves described in Section 3.1, the initial position and rotation are, respectively:

$$\begin{array}{c}{x}_p\left(0\right)={[0,0,0]}^T\hfill \\ R\left(0\right)=I\hfill \end{array}$$

(18)

where I is the the identity matrix. The curves can be placed anywhere in the inertial frame $x_i\left(t\right)$ using:

$${\left[1x_i\left(t\right)\right]}^T=g_i\left(0\right).{\left[1x_p\left(t\right)\right]}^T$$

(19)

where $g_i\left(0\right)$ is the initial state of the path. It should be noted that $g_i\left(t\right)$ includes a description of the rotation (R) of the curve over time. However, this is not used as part of the trajectory tracking, because it is purely kinematic and does not account for the dynamics (such as gravitational acceleration). Similarly, for an initial and desired state in the inertial frame, the final state $g_p\left(T\right)$ for the particular curves is given by:

$$g_p\left(T\right)=g_i{\left(0\right)}^{-1}.g_i\left(T\right)$$

(20)

Since the final rotation is not used when generating the curves using the numerical optimiser, the translation position $x_p\left(T\right)$ can be retrieved from:

$${\left[1x_p\left(T\right)\right]}^T=g_p\left(T\right).{\left[1000\right]}^T$$

(21)

The paths generated using this procedure of joining trajectories are smooth to the second derivative; there are no discontinuities in position or velocity. However, they are not smooth in the third derivative, because the trajectories have different curvatures, so it is not possible to match acceleration.

4. Obstacle Detection

A simple method for checking the validity of a path in the obstacle space is to discretize it into nodes separated by a small time step. Each node lies on the path and can be checked for collisions. The choice of time step is a trade-off between computational time and the completeness of the check. This method can be extended to a hybrid method using a large time step and then performs a numerical minimization to find the smallest collision distance d along the path. The collision distance for an obstacle is defined as the Euclidean distance between the edge of the object and the quadrotor:

$$d\left(t\right)=\parallel x_{obs}-x_i\left(t\right)\parallel -r_{ob}$$

(22)

where $x_{obs}\in R^3$ is the centre of the sphere, $x_i$ is the planned translation position of the quadrotor Equation (19) using the curves described in Section 3.1 and $r_{ob}$ is the radius of the sphere. For practical purposes, some additional margin should be given, because the actual size of the quadrotor is not accounted for by Equation (22).

The paths are composed of trajectories patched together using the method explained in Section 3.2. To find the location on a path where an object is closest, the optimisation vector for the relevant segment must be loaded. The concatenated nature of the path is not an issue, because there are no discontinuities in position when transitioning between trajectories. Time was constrained, such that:

$$0\le t\le T$$

(23)

and the function to minimize is:

$$d_{min}=min\left(d\left(t\right)\right)$$

(24)

where $d_{min}$ is the minimum collision distance along a path for a specified object. If $d_{min}\ge 0$, then the quadrotor will not collide with the obstacle, providing that the tracking controller guides the vehicle along the planned trajectory.

The simple local optimiser ($fmincon$) used in this paper has the potential to get caught in local minima. A global optimiser may be more suitable (such as a genetic algorithm [20]), but the trade-off between computational expense and accuracy needs to be investigated in more detail. However, in this paper, we demonstrate the implementation of the general analytical method by using the local optimiser and by providing a suitable initial guess. The hybrid method used in this paper combines discretization with a large time step and then uses a numerical minimizer to determine the precise location and magnitude of the collision distance. If any of the points are $d\left(t\right)<0$, then the algorithm terminates and the path is reported as invalid. The process of the hybrid algorithm is represented pictorially in Figure 1. An evaluation of the collision detection algorithm is performed in Section 5.3 and compared to discretizing the trajectory with a small time step.

Figure 1. Obstacle detection algorithm.

Figure 1. Obstacle detection algorithm.

5. Simulations

In the first of the following subsections, the tracking controller is described that is used to simulate the path generated in Section 5.2. Finally, Section 5.3 tests the hybrid obstacle detection algorithm and compares it to discretizing with a coarse time step.

5.1. Tracking Controller

The tracking controller was used in this paper to demonstrate the feasibility of the trajectories and was first presented in [15]. In addition to the favourable tracking performance, this controller was chosen because the trajectories are generated on the $SE\left(3\right)$ group. The tracking errors for position, velocity and angular velocity are defined as:

$$e_x=x-x_p$$

(25)

$$e_v=\dot{x}-{\dot{x}}_p$$

(26)

$$e_{\Omega }=\Omega -R^T{R}_d{\Omega }_d$$

(27)

where ${\Omega }_d=R_d^T\dot{R_d}$, and the desired rotation is defined as:

$$R_d=[{\overrightarrow{b}}_{1_d};{\overrightarrow{b}}_{3_d}\times {\overrightarrow{b}}_{1_d};{\overrightarrow{b}}_{3_d}]$$

(28)

and:

$${\overrightarrow{b}}_{3_d}=R_d{e}_3=-\frac{-k_x{e}_x-k_v{e}_v-m{g}_a{e}_3+m{\ddot{x}}_p}{∥-k_x{e}_x-k_v{e}_v-m{g}_a{e}_3+m{\ddot{x}}_p∥}$$

(29)

where ${\overrightarrow{b}}_{3_d}$ is chosen to minimize the attitude tracking error in the term $FR{e}_3$ from Equation (3) and is the tracking controller in the translational direction. The attitude error is chosen as:

$$e_R=\frac{1}{2}{(R_d^{T}R-R^T{R}_d)}^{\vee }$$

(30)

where the $veema{p}^{\vee }:so\left(3\right)\to R^3$. From a given desired trajectory $x_i\left(t\right)$ and heading vector ${\overrightarrow{b}}_{1_d}$, the control inputs can be determined:

$$\begin{array}{c}F=-(-k_x{e}_x-k_v{e}_v-m{g}_a{e}_3+m{\ddot{x}}_d)·R{e}_3\\ M=-k_R{e}_R-k_{\Omega }{e}_{\Omega }+\Omega \times J\Omega \end{array}$$

(31)

where the tracking controller gains $k_x$, $k_v$, $k_R$ and $k_{\Omega }$ must be defined and greater than zero.

5.2. Waypoints

This section gives an example of generating a path through a set of predetermined waypoints and following it using the tracking controller. The waypoints in this paper were arbitrarily chosen to demonstrate the trajectory generation algorithm. In practice, waypoints could be used to give the quadrotor-specific locations to take measurements or generated by a sampling-based planning algorithm, such as rapidly-exploring random trees.

The physical parameters used to represent a realistic quadrotor were taken from the UAV developed in [21]: $m=4.2$ kg, $J=diag[0.0820,0.0845,0.1377]$ kg m${}^2$, $d=0.315$ m and $c_{\tau f}=8.004\times {10}^{-3}$. For the tracking controller, the gains were those used in [15]: $k_x=16$ m, $k_v=5.6$ m, $k_R=8.81$ and $k_{\Omega }=2.54$. The desired heading angle was fixed as ${\overrightarrow{b}}_{1_d}\left(t\right)=[1,0,0]$. Likewise, the magnitude of the velocity was set as $\nu =1$ and the weight $c=200$. The initial conditions were set to:

$$\begin{array}{c}x\left(0\right)=[0,0,0]\\ \dot{x}\left(0\right)=[0,0,0]\\ \Omega \left(0\right)=[0,0,0]\\ R\left(0\right)=I\end{array}$$

(32)

A 30% control margin [22] was used, so for a mass of $4.2$ kg, the maximum thrust that can be provided by each motor is 13.4 N. This constraint was applied to the simulation.

The path consists of five waypoints specified in Table 1. The 3D path generated by the trajectory planner is shown in Figure 2 with the start of a segment and endpoint denoted by an asterisk. Figure 3 plots the inertial frame velocity components and the magnitude of the velocity, which is constant and equal to one. The total path length is $42.8$ m and a total flight time of $42.8$ s. The optimisation vector parameters and solution time for each segment are given in Table 2.

Table 1. Waypoints.

Waypoint	${\mathit{x}}_{\mathbf{1}}$	${\mathit{x}}_{\mathbf{2}}$	${\mathit{x}}_{\mathbf{3}}$
1	3	4	5
2	−2	7	3
3	−2	0	6
4	3	−4	6
5	2	0	0

Table 1. Waypoints.

Waypoint	${\mathit{x}}_{\mathbf{1}}$	${\mathit{x}}_{\mathbf{2}}$	${\mathit{x}}_{\mathbf{3}}$
1	3	4	5
2	−2	7	3
3	−2	0	6
4	3	−4	6
5	2	0	0

Figure 2. Concatenated trajectory passing the prescribed waypoints.

Figure 2. Concatenated trajectory passing the prescribed waypoints.

Figure 3. Quadrotor velocity during the concantenated trajectory.

Figure 3. Quadrotor velocity during the concantenated trajectory.

Table 2. Solution details.

Segment	${\mathit{\lambda }}_{\mathbf{1}}$	${\mathit{\lambda }}_{\mathbf{2}}$	${\mathit{\lambda }}_{\mathbf{4}}$	T	Solution Time (ms)
1	−14.136	14.281	2.773	7.858	526
2	−28.675	−15.967	−9.2926	9.656	183
3	−11.621	−10.550	3.025	8.161	108
4	−15.955	10.789	2.482	6.898	194
5	−23.425	13.467	4.527	10.224	130

Table 2. Solution details.

Segment	${\mathit{\lambda }}_{\mathbf{1}}$	${\mathit{\lambda }}_{\mathbf{2}}$	${\mathit{\lambda }}_{\mathbf{4}}$	T	Solution Time (ms)
1	−14.136	14.281	2.773	7.858	526
2	−28.675	−15.967	−9.2926	9.656	183
3	−11.621	−10.550	3.025	8.161	108
4	−15.955	10.789	2.482	6.898	194
5	−23.425	13.467	4.527	10.224	130

The tracking controller’s ability to follow the curve and demonstrate the dynamic feasibility of the path is shown in Figure 4. The required thrust, F, is plotted against time in Figure 4b. The initial spike at 0 s is the quadrotor accelerating from rest to 1 m/s, and the additional fluctuations are caused by changing from one segment to another. This is because the paths are only smooth to the first derivative (that is, acceleration is not continuous between them). However, Figure 4a shows that the position errors oscillate around 0 m, but always remain less than $0.04$ m, so the acceleration discontinuity between the segments is handled well by the tracking controller. Similarly, Figure 4c shows that the moments over time fluctuate, but none of the components have a magnitude larger than $0.5$ Nm.

Figure 4. Tracking controller performance. (a) Position error $e_x$; (b) control thrust (N); (c) control moment (N m).

Figure 4. Tracking controller performance. (a) Position error $e_x$; (b) control thrust (N); (c) control moment (N m).

5.3. Obstacle Collision Detection

The path from Section 5.2 was used to test the obstacle detection algorithm on a standard 3.4-GHz desktop computer. Five spheres were placed in the vicinity of the curve, and the points on the path closest to each obstacle were calculated using two methods.

The first method discretized the trajectory into 1000 nodes and took 463 ms to find the closest position on the trajectory for each of the five obstacles. The second hybrid method uses a coarse discretization and then a numerical minimizer (as described in Section 4) to find the collision distance. The solution time for the hybrid method was 309 ms and is shown in Figure 5. The circles on the path mark the points where an obstacle was closest. The percentage difference between the two methods is 40%, so reduced computing time is possible, especially in cases were there are many obstacles. Additionally, since MATLAB is an interpreted language, it is slower than compiled code. If the algorithms were rewritten in a different language, such as C, and compiled, then the computation time would be reduced. However, since we are interested in comparing the methods, MATLAB was chosen for its ease of use.

Figure 5. Obstacle detection.

Figure 5. Obstacle detection.

6. Conclusions

This paper presents a method for concatenating separate trajectories to form smooth, analytically-defined paths for a quadrotor along a set of waypoints. Since the functions are defined using analytical expressions, only a numerical minimizer is needed to calculate the optimisation vector that controls the shape of the curve. A controller was used to ensure that the generated trajectory is trackable and dynamically feasible. Additionally, it provided information about the thrust and moments required to fly a typical quadrotor along the trajectory. An obstacle detection algorithm is presented that is a hybrid method of discretizing the path into nodes and using a numerical minimizer to find the minimum collision distance to an obstacle. Compared to just discretizing the path, this method is faster and able to, more precisely, locate the correct collision distance.

Further work will look at using the analytically-defined curves for sample-based planning methods, allowing for a full path planning method that does not rely on the input of waypoints to navigate an area. This paper focuses on trajectory planning for quadrotor unmanned aerial systems, but the techniques used could also be applied to other types of vehicles.