Trajectory Tracking For Quadrotors: An Optimization-Based Planning Followed By Controlling Approach

We present an optimization-based reference trajectory tracking method for quadrotor robots for slow-speed maneuvers. The proposed method uses planning followed by the controlling paradigm. The basic concept of the proposed method is an analogy to Linear Quadratic Gaussian (LQG) in which Nonlinear Model Predictive Control (NMPC) is employed for predicting optimal control policy in each iteration. Multiple-shooting (MS) is suggested over Direct-collocation (DC) for imposing constraints when modelling the NMPC. Incremental Euclidean Distance Transformation Map (EDTM) is constructed for obtaining the closest free distances relative to the predicted trajectory; these distances are considered obstacle constraints. The reference trajectory is generated, ensuring dynamic feasibility. The objec-tive is to minimize the error between the quadrotor’s current pose and the desired reference trajectory pose in each iteration. Finally, we evaluated the proposed method with two other approaches and showed that our proposal is better than those two in terms of reaching the goal without any collision. Additionally, we published a new dataset, which can be used for evaluating the performance of trajectory tracking algorithms.


Introduction
In recent years, quadrotor related research, particularly motion planning, widens the range for venturing out computationally expensive techniques in various disciplines due to the high growth rate of light-weight embedded devices.Yet path following and trajectory tracking are still open research fields due to their complexity.Path and trajectory are referred to as two different concepts: path is not represented by any temporal law (i.e., time, speed) and trajectory is defined by a time parameterized path.Thus, the reference path is described with a time-free parameterization in the path following.Subsequently, collisionfree, geometrically feasible path generation is referred to as the path planning.Trajectory tracking involves time parameterization with space references.In addition to geometrical feasibility, dynamic and kinematic constraints are considered in trajectory tracking.In a nutshell, trajectory tracking involves finding optimal control policy to maneuver the quadrotor, ensuring dynamic and obstacle constraints.
Model Predictive Control (MPC) is one of the robust methods for determining an optimal control policy imposing constraints seamlessly.In each iteration, MPC solves an optimal control problem for a given prediction horizon.Consequently, the first portion of the optimal policy is applied to the system.Since the Figure 1: A field experiment result shows how the proposed trajectory tracker operates in real conditions.In this scenario, the reference trajectory (p ref ) passes through an obstacles zone; the trajectory tracker is trying to avoid the obstacles zone while keep minimizing the error between current pose and desired reference trajectory pose.The complete experiment is available at (TrajectoryTracker, ) optimal policy calculation procedure has to compute in each step, MPC is computationally expensive.Hence, we proposed planning followed by a controlling strategy (see Fig. 1) to overcome computational demands with short prediction horizon in which a simplified motion model (4-DoF) is proposed at planning stage while keeping the real 12-DoF quadtotor model at controlling stage.However, the inability to use the real motion model and prediction horizon truncation reduce the trajectory tracker stability.The terminal constraints set (Morari and Lee, 1999) has to be introduced to preserve stability.However, adding a terminal constraints set increases computational demand at each step.Thus, we proposed a LQG (Stein and Athans, 1987) based approach to reduce the disturbance and increase system stability.
Our Contributions: 1. Proposing a simplified motion model, which consists of only four states, i.e., position ∈ R 3 and yaw, without considering pitch and roll changes in the planning stage since this research focuses on slow speed maneuvers.On the contrary, such motion model will lead to system instability.LQG-based planning followed by the controlling approach is proposed to overcome those drawbacks.Furthermore, NMPC is designed based on two different parametrization techniques: MS and DC.Hence, the comparative study was carried out for those two parametrization techniques 2. Constructing obstacles constraints directly from EDTM for identifying closest obstacles poses along the NMPC prediction horizon 3. Releasing a new dataset, consisting of a hundred trajectories across cluttered environments; this dataset can be used for evaluating the performance of reference trajectory tracking approaches 2 Related Works

Map Building and Motion Model Selection
The proposed trajectory tracker can avoid obstacles while tracking the reference trajectory.Considering that a fast and accurate real-time map building technique is required.Within the literature, among the contributions towards real-time incremental map building, OctoMap (Hornung et al., 2013) is one of the popular choices among the researchers over the last decade.Moreover, Voxblox (Oleynikova et al., 2016b) which is based on Truncated Signed Distance Fields (TSDFs) can be used for map building as well while keeping the map precision at high level.In this work, we use OctoMap for map building, but no restriction to use Voxblox instead.
In general, the quadrotor dynamics is described by 12-DOF.However, planning followed by controlling approaches does not require to define an actual motion model for planing since the controller consists of a fully-fledged motion model of the quadrotor.In (van den Berg, 2016), the motion model of the system is expressed as 12-DOF.Other than the exact model, a 6-DOF motion model is proposed for governing a quadrotor in a distributed setup (Trawny et al., 2010).Later, it was reduced to 4-DOF motion model (Wanasinghe et al., 2015).

Trajectory Generation
Trajectory generation can be carried out in different ways, including path planning followed by smoothing and trajectory planning.The authors of (Kulathunga et al., 2020) proposed a technique for path planning using Rapidly-Exploring-Random-Trees(RRT*) (Karaman and Frazzoli, 2011) and further smoothing the path by B-spline for a quadrotor.In (Yang and Sukkarieh, 2010), continuous curvature-based path-smoothing technique is proposed for fixed-wind aerial vehicles.(Zhou et al., 2019b) proposed a spline-based optimization in which kinodynamic A* is used to find a feasible collision-free path for a quadrotor.In (Zhou et al., 2019a), researchers first generated topological paths and then utilized a guided gradient optimization-based technique for trajectory generation.The Minimum-snap (Mellinger and Kumar, 2011) one of the successful techniques was proposed for trajectory generation in which differential flatness (Van Nieuwstadt and Murray, 1998) property was used to navigate the quadrotor.(Richter et al., 2016) proposed an optimization-based trajectory planning approach in which a set of polynomials is used to define the trajectory.

Trajectory Tracking
Trajectory generation and trajectory tracking are interconnected.MPC especially, NMPC (Kamel et al., 2017) is a promising technique in which trajectory generation and tracking are combined and solved as one problem.Nonlinear and Linear, i.e., Linear Quadratic Regulator (LQR) (Reyes-Valeria et al., 2013) approaches are the main approaches to controlling a system.With the recent development of deep reinforcement learning, learning-based approaches (Fan et al., 2020), could be added as the third approach.Recently, NMPC has been employed in more and more applications due to its capabilities, e.g., dealing with multiple input and output, handling equality and inequality constraints, which could be either convex or non-convex.The authors of (Islam et al., 2019) formulated the quadrotor dynamics based on quaternion orientation.Afterwards, they proposed an MPC based trajectory tracker in which optimal control policy is generated by minimizing the quaternion error.In contrast to the this approach, Linear MPC trajectory tracker is proposed (Chipofya et al., 2015) in which a system is designed as an orthonormal function, e.g., Laguerre.
MPC can be formed in different ways, e.g., Stochastic MPC and Tube MPC.In (Alessandretti et al., 2013), trajectory tracker was formulated as a Tube MPC in which a terminal set was introduced to ensure the asymptotic convergence.The sparsity of the problem structure does matter for the fast convergence of MPC.Unlike above mentioned approached, MS-based approach was proposed by (Gros et al., 2012) to maneuver the quadrotor in a cluttered environment.They claim that MS helps to improve the sparsity of the optimal control problem (OCP).However, this technique was validated only in a simulated environment due to on-line computational demands.(Li et al., 2020) proposed to obtain an optimal control policy using State-dependant Distance Metric (SDDM).They modelled the system dynamics as a linear time-invariant.Furthermore, a reference governor (Garone and Nicotra, 2015) was introduced to maintain safety and stability since linear motion model was considered.
In the work of (Ji et al., 2020), they proposed intermediate controller, i.e., Corridor-based Model Predictive Contouring Control (CMPCC) in between local planner and the main controller.CMPCC was based on SDDM in which flight corridors ware set as hard constraints.Thus, CMPCC can capture local changes in real-time; this yields how the system can address unmeasured disturbances.Once the reference trajectory was provided, trajectory tracking error was defined by the distance between the current pose of a quadrotor and the moving reference position.Initially, a set of hyperplanes was constructed within the prediction horizon as a polyhedron.Since MPC does calculate control inputs, i.e., velocities, flight corridor was generated intersecting velocity's orthogonal projection with corresponding polyhedral.Further, those projections are considered as the safe constrains set, which are considered as hard constraints when solving the MPC.

Methodology
The proposed trajectory tracker was designed based on planning followed by controlling.There are several ways to control the quadrotor, including position (control x,y, and z position), velocity (control velocity on x,y, z direction and yaw rate), and attitude control (control the orientation with respect to an inertial frame).We decided to use velocity control in which velocity and yaw rate are required to control the robot.Moreover, velocity control helps bypass generated high jerk, which tends to excite mechanical resonances, causing vibrations.Hence, accurate control policy, i.e., velocity and yaw rate, must be generated at the planning state in each iteration of NMPC.Yet, reducing the computation time of NMPC is also necessary for undergoing smooth trajectory tracking.Hence, having a simplified motion model rather than a real motion model is more appropriate at the planning stage.Since this work focuses on reference trajectory tracking for slow speed maneuvers, it is assumed that aggressive maneuvers are not necessary for such a task at the controlling stage.If we consider a 12-DoF motion model, NMPC becomes diverged for a long prediction horizon and computationally expensive in the planning stage.Thus, a 4-DOF motion model was considered instead of incorporating the actual quadrotor model whose dynamics is described by 12-DOF.A similar simplified motion model was proposed in (Mehrez et al., 2017) for distributed MPC setup.On the contrary, the simplified motion model leads to an unstable control policy generation.Generating accurate control policies and estimating the quadrotor pose are required for smooth trajectory tracking while reducing the instability at the planning stage.As a result, we designed an LQG-based approach (Fig. 2).The following sections explain each component of the proposed trajectory tracker.

Simplified Motion Model
The system states and control inputs are described by where f c (•) : R nx × R nu → R nx and n x = n u = 4.The proposed tracking problem is formulated as a discrete dynamical model.Forward Euler discretization, x k+1 = f d (x k , u k ) is introduced for a given sampling period in second, δ ∈ R > 0, we set δ = 0.05s in this experiment. where

Close-in Obstacles Identification
Close-in obstacle pose identification relative to the quadrotor is crucial for tracking a trajectory safely.In this work, OctoMap was utilized to build the EDTM of the environment incrementally.

Reference Trajectory Generation
Tracking a reference trajectory while avoiding obstacles is quite a challenging task due to several reasons.For example, the robot could be trapped in a local minimum or have difficulty finding the appropriate return position back to the reference trajectory after avoiding the obstacles.Nonetheless, MPC-based trajectory tracking handles the these problems adequately.In this work, the third-order (d=3) uniform B-Spline was used to generate the reference trajectories.In general, d th order B-spline can be defined for a given knot sequence, i.e., p knot = {t 0 , t 1 , ., y i , z i > in R 3 where i = 0, ..., n p .For a given time index, i.e., k corresponding position, c ref (k) can be fully determined using DeBoorCox formula as given in Algorithm 1, which was initially proposed by de Boor (de Boor, 1971): k, otherwise 3: while true do 5: 9: for i ← 0 to d do 10: for r ← 1 to d do 12: for i ← d to r do 13:

Tracking Problem Synthesis
The proposed trajectory tracker was initially defined as an OCP problem and then transformed into an NLP problem.A problem is always solved better in nonlinear manner as opposed to linearizing motion model at every time instance since the motion model is nonlinear.The above OCP problem can transform into NLP in various ways including, MS and DC.We implemented both MS and DC methods.The intuition of MS is to split system integration into small time intervals incorporating state constraints into the decision variables.NMPC can handle the constraints, which do not need to be convex.Hence, we selected NMPC as a closed-loop controller which handles a constrained non-linear system implicitly to solve the optimal control policy for a fixed prediction horizon N e ∈ N. The sections below explain the formulation of the proposed trajectory tracker with MS and DC.

With multiple-shooting
As detailed in reference trajectory generation subsection, c ref (t) and ċref (t) give only position and velocity in the R 3 .Yaw and yaw rate were not incorporated for tracking because keeping the reference yaw does not have any meaningful impact when avoiding obstacles due to changed heading angle.Thus, we relaxed yaw and yaw rate from the tracking problem.x k and u k are the state vector and control inputs (2).For simplicity, we used the same notation x k and u k when formulating NMPC, but NMPC skips the 4th entry in both state and control inputs, i.e., yaw and yaw rate.MS can be described as: where w = [u k , . . ., u k+Ne−1 , x k , . . ., x k+Ne ] denotes the decision variables set to be minimized.Q ∈ R nx×nx and R ∈ R nu×nu are symmetric.Furthermore, Q and R should be positive definite and positive semi-definite, respectively.In the experiment, both Q and R were set as identity matrices.p upper and p lower depict the upper and lower bound of x k , respectively.These bounds are enforced based on the map constraints where quadrotor is allowed to fly.u lower and u upper define the minimum and maximum linear and angular velocities allowed for the quarotor maneuver.g 1 (w) depicts the constraints that system dynamics imposes as follows: g 2 (w) describes the constraints imposed by obstacles, which is reconstructed in each iteration due to changes in the number of obstacles relative to the quadrotor pose.Subsequently, reconstructing g 2 (w) is useful to integrate the dynamic environment changes into the trajectory tracker as described in the equation below: where xk is the initial position and N o is the number of obstacles and dis(x o j , x k+h ) can be calculated as follows: − where d o is the safe zone distance between a quadrotor and close-in obstacles.

With direct-collocation
In MS, we traded nonlinearity with a sparsity structure to reduce the nonlinearity.DC adds more degrees of freedom.Hence, DC exploits the sparsity even more.On the contrary, computation power is increased dramatically, though DC gives higher accuracy than MS.The first step is to define the collocation points with respect to a chosen polynomial when formulating DC.In this experiment, we chose Lagrangian 3rd order (N d ) polynomial to select collocation points.However, other polynomials such as B-spline or Bézier can be utilized as well.We fixed the time interval (δ) when defining g 1 in MS.Nonetheless, DC has more freedom to determine how points are distributed between two consecutive time interval.
To formulate DC, we kept the same discretization as in the MS, i.e., u k+i , u k+i+1 , for i ∈ [t k , t k+1 ], i = 0, .., N d − 1.Let η be the coefficient for 3rd order (N d ) Legendre points, η = [0, 0.112, 0.500, 0.888].Subsequently, each consecutive time interval (t k+i and t k+i+1 ) was divided into small sub-intervals as follows: where h k = t k+1 − t k and corresponding state vector at t k,j is denoted by x k,j .In each control interval, Langrangian polynomial is defined as State trajectory approximation (x k ) and intermediate derivatives, i.e., ▽x k,j , can be derived by using Langrangian functions (8 , where Lr (η j ) depicts the derivative of L r (η j ).Plugging ▽x k,j into (2), a set of collocation equations is constructed at each collocation point as follows: where k = 0, ..., N e − 1 and j = 0, ..., N d .As we formulated NMPC in multiple-shooting subsection, for DC, only g 1 (w) is changed, i.e., (9), and the rest will remain the same.
NMPC output is post-processed to improve the smoothness using a KF smoother before sending to the controller as shown in Fig. 2. KF smoother is needed to reduce the noise of NMPC, which occurs when number of obstacles constraints are increased.Sub-optimal KF (Van Der Merwe and Wan, 2001) was utilized over optimal KF to have better numerical stability.When defining the relationship between the current state and the next state of KF, the same motion model is utilized In KF smoother, B = 0 and ûk+1 = Aû k where A is an identity matrix in R nx×nx and B is given by , ωz k > denote the expected values after applying KF and KF smoother, respectively.As explained in multiple-shooting subsection, yaw rate, i.e., α z k , was excluded from the tracking problem.Hence, yaw rate is estimated as follows: Estimated control and pose, ûk and xk , respectively, and actual control and pose (see Fig. 2), >, respectively, are the inputs for the PD regulator that is formulated as follows for a given time index k: where constant terms (k p = 0.8, k d = 0.27, k a = 2.5, and k v = 0.5) were estimated empirically.

Experiment Procedure
We conducted several types of experiments in the simulated and real-world environments to perform qualitative and quantitative analyses of the proposed trajectory tracker.Firstly, trajectory tracker accuracy is analysed in an obstacle-free zone to show reference trajectory tracking accuracy.Afterwards, we compared the proposed trajectory tracker and two other approaches.The second part of experiment lays out the qualitative analysis of the proposed trajectory tracker.The proposed trajectory tracker was implemented in C++11 with -O2 (code-level optimization).We used a computer with 2.70 GHz CPU and 8GB ram for simulated experiments whereas, Jetson AGX Xavier was utilized as the on-board computer on the quadrotor as shown in Fig. 4.
Figure 4: The hardware setup of the quadrotor.TFMini Plus is for estimating the altitude whereas Velodyne LiDAR-16 is for reasoning the environment Checking the trajectory tracking accuracy is required for an obstacle-free zone since this is the initial proposal.Fig. 5 shows the reference trajectory employed for this experiment, and the complete test is available at (TrajectoryTracker, ).Fig. 6b clearly shows that the quadrotor almost follows the reference trajectory Figure 5: An example reference trajectory (p ref ) that was generated as proposed in the reference trajectory generation subsection.p act is the actual trajectory along which the quadrotor flew using the proposed trajectory tracker adhering to the imposed constraints, e.g., max velocity (0.4m/s) (Fig. 6a).
The next experiment is devoted to comparing the proposed trajectory tracker with two other approaches: (Oleynikova et al., 2016a) and (Usenko et al., 2017), which were targeted on the slow-speed maneuvers same as the proposed approach.We used the same validation technique, which (Oleynikova et al., 2016a) utilized for checking the behaviour in different environments.We generated 10 Poisson forests (Karaman and Frazzoli, 2012) with 10x10x10 of densities in between 0.2 trees/m and 0.8 trees/m.Since (Usenko et al., 2017) also used the same validation test, we compared the proposed trajectory tracker with theirs as given in Table .1; an example scenario is shown in Fig. 7.We kept all the other configuration parameters constant only the maximum velocity (u k ≤ V max ) was varied.When V max is higher, coverage of prediction horizon is higher, i.e., may take a higher number of obstacles constraints, which is directly affected on the Mean Computation Time (MCT = total execution time total N M P C iterations ) of NMPC solver.As seen in Table 1, the proposed trajectory tracker could solve the trajectory almost all the given test cases which imply having high success fraction (SF = success test cases total test cases ).The obstacle cost was incorporated as constraints rather than minimizing as part of the cost; that was the key to obtaining high SF.In the other two approaches, obstacle cost was incorporated as a minimizing term of the objective.The objective minimization does not imply that obstacle constraints are met every time.Thus, there is a high possibility to crash the quadrotor.That is why the other two approaches are having less SF despite low MCT.The computational demand of NMPC is considerably higher, which yields higher solving time (MCT).Higher MCT is due to problem formulation, i.e., as a constraint optimization problem.Since our objective is lowspeed maneuvers, approximately 0.1s of MCT is sufficient.Mean Norm Path Length (MNPL) was calculated by ratio to the total distance of the projected path to straight line distance between start and goal pose.Table .1 shows the dataset (Karaman and Frazzoli, 2012) we used for comparison.This dataset was not complicated enough for the proposed trajectory tracker.Additionally, this dataset was not created targeting for trajectory tracking.Thus, we proposed a new dataset that can be used for validating the performance of trajectory tracking algorithms in cluttered environments.The proposed dataset consists of 100 different reference trajectories as described in reference trajectory generation subsection.Each trajectory is provided with a set of intermediate waypoints (see Fig. 8).The number of waypoints is used to construct a reference trajectory is varying between four to sixteen.For more information about the dataset, refer to (Trajectory-Tracker, ).Table .2 shows the evaluation of the proposed trajectory tracker against the proposed dataset.In this evaluation, MNPL was defined as the ratio of the reference trajectory path length to the corresponding proposed trajectory.SF and MCT are the same as before.
Table 2:  ACADO (Ferreau et al., 2012) was used for solving the NMPC.Table 2 displays the comparative result of two parameterization techniques (DC and MS).Even DC has a higher SF, MCT of DC is almost two-wise than MS.Thus, for real-time application, this behaviour is not acceptable.With that, we can conclude that MS is the optimal choice.When it comes to computational power, MS is better than DC, whereas the accuracy of DC somewhat higher than MS.So, we decided to use MS as the default technique though the proposed solution supports both methods; the one to be used can be configured.

Conclusion
We proposed an optimization-based trajectory tracking approach for slow-speed maneuvers and conducted qualitative and quantitative performance analysis.All in all, the proposed trajectory tracker can track a given reference trajectory while avoiding obstacles for low-speed maneuvers in which changes in roll and pitch were not considered while the only change in yaw is considered.With such consideration, the simplified motion model-based was proposed, which helped to reduce the computational demands when calculating control policy.Moreover, the problem was formulated as a constraints optimization problem that ensures safety.However, computational demand is yet to be further reduced to guarantee a fast reaction time to handle the dynamic obstacles, which improves safety maneuvering in cluttered environments.Forthcoming works will concentrate on further reducing the computational demands.

Figure 2 :
Figure 2: The block schema of the proposed trajectory tracker.Trajectory tracker and controller work as two separate closed-loop systems, provided that smoothed control command (û k ) is sent to the controller.Quadrotor has its onboard low level controller, namely DJI A3 flight controller (DJIA3, ), which acts as a velocity controller that accepts velocity and yaw rate as input to control the quadrotor.PD regulator is employed for minimizing the error between trajectory tracker and actual controls (u act k ) from DJI A3. x ref k A 16-channel LiDAR was used for reasoning the environment.EDTM was used to estimate the obstacle-free distance and the corresponding closest pose (close-in obstacle pose) from a specified pose.NMPC predicts a sequence of steps ahead of the forecasted trajectory.Thus, close-in obstacle poses are calculated with respect to each position along the NMPC forecasted trajectory.The number of steps depends on the horizon length of NMPC.Fig.3shows an instance view of close-in obstacle detection with respect to quadrotor pose.Each red dotted line depicts the distance between quadrotor and an obstacle position.Initially, close-in obstacles are estimated relative to the initial position of the quadrotor.After that, it uses the previous prediction horizon steps to estimate the close-in obstacle poses.This will result in generating a set of obstacle constraints to guarantee a collision-free trajectory after discarding the same obstacle poses.Tracking problem synthesis subsection describes how obstacle constraints are incorporated into trajectory tracker.

Figure 3 :
Figure 3: Illustration of close-in obstacle detection.Close-in obstacle pose estimation in respect of current prediction horizon (h act ) within the region of interest using Euclidean Distance Transformation Map (EDTM)

)
Here c ref (k) does not exactly equal to p ref i .c ref (k) is continuous and it may or may not pass through control points, e.g., p ref i due to B-spline interpolation.We estimate reference velocity by taking the first derivative of p ref and evaluating B-spline derivative using ċref (k) = DeBoorCox(k, ṗref ) since the derivative of B-spline is also a B-spline.More information about the way uniform B-spline trajectory are generated(Zhou et al., 2019b).Algorithm 1 Reference position c ref (k) or velocity ċref (k) estimation for a given time k.p can be either p ref (position) or ṗref (velocity), and k is the desired time index 1: procedure DeBoorCox(k, p) 2: x ref k = c ref (k) and u ref k = ċref (k) are the reference state vector and control inputs (3).

Figure 6 :
Figure 6: The proposed trajectory tracker was evaluated using a reference trajectory (i.e., Fig. 5) in a place where no presence of close-in obstacles (a) The quadrotor flying at the altitude of about 10m (b) The quadrotor flying at low altitude

Table 1 :
Comparison of trajectory planning approaches S: the order of trajectory segment size with respect to minimizing parameters, i.e., jerk, vel, C: control points to be taken for the optimization, N e : NMPC prediction horizon, δ = 0.05s : forward Euler discretization step size