Safe trajectory tracking is an essential requirement for autonomous aerial vehicles, where an aerial vehicle is required to track a reference trajectory and avoid unexpected obstacles. To achieve high-accuracy tracking and avoid obstacles, the typical approach utilizes a high-level planner to generate a safe trajectory, and a low-level controller running at a much higher frequency than the planner to track the planned trajectory.
In practice, for high-speed quadrotor tracking in a clutter scenario with dense obstacles and limited sensing range, it is challenging for a planner to replan a safe trajectory in real-time. On the other hand, it is difficult to obtain an accurate model for the low-level tracking controller due to wind disturbances, which may cause the safety-critical quadrotor system to deviate from the planned trajectory and even collide with obstacles. These challenges show an urgent need for designing a safe tracking controller that ensures both obstacle avoidance and accurate trajectory tracking for a quadrotor with a limited sensing range and under uncertain wind disturbances.
In recent years, for safe trajectory tracking, the cascaded architecture [1, 2] using control barrier functions (CBFs) to ensure safety has shown great potential for addressing these challenges with practical feasibility. CBFs  have been widely used to enforce safety constraints, and has proved to be an effective strategy to avoid obstacles for safety-critical systems. In , a learning-based CBF is presented to constrain a quadrotor within a static ellipsoid safe region under wind disturbances. In , a cascaded quadratic program (QP) safety-critical controller incorporating CBFs and control Lyapunov functions (CLFs) is proposed. In this work, the quadrotor with a limited sensing range can asymptotically track a reference trajectory by constructing stability constraints (represented by CLFs), and simultaneously avoid obstacles by constructing safety constraints (represented by CBFs). Although this controller can handle time-varying constraints, a precise model of the system is required to enforce the constraints.
Alternatively, Model Predictive Control (MPC)  is an effective finite-horizon optimal control method to handle constraints naturally in a safe tracking control problem. For instance,  proposes a Nonlinear Model Predictive Control (NMPC) for safe trajectory tracking with obstacle avoidance capacity under constant disturbances. Considering varying disturbances, an adaptive MPC is proposed in , which combines a robust MPC with a feedback adaptive controller for safe and high-accuracy tracking. Although these methods can efficiently deal with obstacles when tracking a trajectory, the considered obstacles are assumed to be static.
Considering the limitations of current approaches, a safe tracking controller is desirable to be developed for a quadrotor subject to a limited sensing range to ensure accurate trajectory tracking and enforce safety constraints under wind disturbances. Particularly, it should drive the quadrotor back to the predefined reference trajectory after safely avoiding unexpected static and dynamic obstacles.
In this paper, drawing inspiration from the cascaded architecture , we propose a learning-based safety-preserving cascaded QP control (SPQC) approach composed of a position-level controller and an attitude-level controller, as illustrated in Fig. 1. We utilize the Gaussian Process (GP) to estimate a high confidence interval accounting for the model uncertainties caused by wind disturbances. Then, a local motion planning (LMP) method is designed based on the to generate a reference orientation for the attitude controller. In each QP controller, a stability constraint based on CLF is formulated to develop a nominal Lyapunov-based cascaded tracking controller for accurate tracking. Considering safety, the Iterative Regional Inflation by Semidefinite programming (IRIS) algorithm  is adopted to construct a time-varying obstacle-free safe region and then use this safe region to design two CBFs to enforce safety constraints both on the position level and the attitude level for the nominal cascaded tracking controller. Finally, the safety constraints are enforced to minimally modify the Lyapunov-based tracking controller to formulate the SPQC controller.
The of this paper are presented as follows:
A learning-based SPQC controller consists of a position-level and an attitude-level controller is presented for quadrotors to safely track the reference trajectory with obstacle avoidance capacity under wind disturbances. An LMP algorithm is designed for the attitude-level controller for accurate tracking under wind disturbances.
Two CBFs are designed to enforce safety constraints on the position and attitude level for the quadrotor to minimally modify the controls generated by the nominal tracking controller via constrained QPs.
The proposed SPQC controller is validated through numerical simulations on a quadrotor in a cluttered environment under varying wind disturbances.
This paper is organized as follows: Section II presents the preliminaries used in this paper. The problem statement is introduced in Section III. Section IV illustrates the proposed SPQC controller. Section V shows simulation results to validate and clarify our SPQC controller. Finally, conclusions are drawn in Section VI.
Ii-a Control Barrier Function
Consider a control affine system
where , denote the state and the control input of the system, respectively. The function and are Lipschitz continuous.
The of the system can be defined by
where is a continuously differentiable function related to state constraints.
Definition 2. The set is called , if for every , for all .
To ensure forward invariance of , e.g. quadrotors stay in the collision-free safety set at all times, we consider the following definition.
Definition 3 (Definition 5 of ). For the dynamical system (1), given a set defined by (2) for a continuously differentiable function , the function is called a defined on the set with , if there exists an extended class function ( and strictly increasing) such that
where represents the Lie derivatives. To be more specific:
Ii-B Dynamics of Quadrotor
The motion of the quadrotor can be described by six degrees of freedom: The translational positionin the inertial coordinate system and attitude represented by Euler angles (roll , pitch , and yaw ). The vehicle attitude is defined by the rotation matrix  from the vehicle coordinate system to the inertial coordinate system .
Considering the unknown wind force caused by wind disturbances , we adopt the following model to describe the nonlinear quadrotor dynamics,
where , is the gravity acceleration. and denote the mass and inertia of the quadrotor, respectively. , , and stand for , , and , respectively. The control inputs of the quadrotor are denoted by the body rotational rates and the thrust force along the body axis in the vehicle coordinate system. is unknown wind force acting on the quadrotor.
In this paper, we consider the following state-space nonlinear control affine quadrotor system with partially unknown dynamics, i.e.,
where and denote the state and control input, respectively. and compose a priori model representing our knowledge of the actual system, and is the model error representing the effects of wind disturbances acting on the quadrotor.
Ii-C Gaussian Processes
We use the Gaussian process (GP) to learn the evolving model error caused by wind disturbances . A GP is a nonparametric regression method that can estimate complex functions and their uncertain distribution . Given observations
, the mean and variance ofat the query state can be given by
is the observed vector subject to a zero mean Gaussian noise. is the covariance matrix with entries, where , , and is the kernel function. , and
is the identity matrix.
With the system model error learned by GPs, a reliable high confidence interval on the uncertain dynamics can be obtained by designing the constant .
For instance, and confidence are achieved at and , respectively.
Iii Problem Statement
The safe trajectory tracking problem considered herein can be described as follows. Let the quadrotor with a limited sensing range have an initial state (consisting of initial position , velocity , attitude , , ), and the reference yaw . The quadrotor should track a given reference trajectory in a varying wind field while avoiding unexpected static and dynamic obstacles. If it deviates from the reference trajectory due to a sudden wind disturbance or after avoiding obstacles, it should rapidly converge to .
In this section, we describe the proposed learning-based safety-preserving cascaded QP control (SPQC) for the system (6) that exploits the disturbance model to provide accurate tracking with safety and input constraints.
We follow a cascaded approach described in . The cascaded controller consists of two QP controllers: a position-level QP controller and an attitude-level QP controller, as illustrated in Fig. 1. The position-level QP controller generates the desired thrust , and the attitude-level QP controller utilizes this thrust together with the high confidence uncertainty interval estimated via GPs to compute the desired body rotational rates . Besides, a corresponding safety constraint is designed to minimally modify each nominal QP controller to keep the quadrotor within the obstacle-free region and avoid unexpected obstacles when the quadrotor is tracking the reference trajectory.
Iv-a Nominal Lyapunov-based Cascaded QP Controller
A nominal Lyapunov-based cascaded QP controller is proposed for trajectory tracking based on CLFs and GPs without the consideration of obstacle avoidance. We first construct a position-level QP controller to track the position of the quadrotor. Then, an attitude-level QP controller with a local motion planning strategy is designed to adjust the attitude of the quadrotor.
Iv-A1 Position-level QP controller
Given a reference trajectory , we can construct the following CLF:
where , , is the current position of the quadrotor. The values of and are chosen specifically to make the quadratic.
Considering wind disturbances, GPs are employed to estimate the model uncertainties in terms of the predicted mean and variance of the model error through (7) and (8). As shown by our previous work , stability constraints can be constructed based on the CLF to ensure stable high tracking performance for the uncertain dynamical system (6). Hence, based on our previous work , we can construct a position-level QP controller to compute a nominal thrust for tracking as follows:
where is the upper bound of thrust, and is a positive constant. , , and is a slack variable to ensure there is no conflict among the stability constraints and control constraints in (11). and denote the Lie derivatives of with respect to and , respectively.
Remark 1. Note that the optimization (11) is not sensitive to the parameter as long as it is large enough (e.g. ), such that stability constraints violation is heavily penalized.
Iv-A2 Local motion planning (LMP)
During trajectory tracking, a quadrotor may deviate from the reference trajectory due to a sudden wind disturbance or unexpected obstacles. In this study, we proposed an LMP algorithm to obtain a reference attitude for the quadrotor to drive it back to its reference trajectory under wind disturbances. The LMP method is described from the view of discrete control with a control period of .
Let and denote the position of a quadrotor and the target waypoint of reference trajectory at time , respectively. We use GPs to estimate the mean wind disturbances at position level, and at velocity level based on (7). Hence, we can get the next nominal position and velocity of the quadrotor based on the nominal thrust generated by the position-level QP controller (11).
To generate the desired orientation of the quadrotor for tracking, we can adjust the orientation of thrust at time to obtain the desired thrust direction at position level as illustrated in Fig. 2. The desired direction of the thrust at the velocity level at can be obtained in a similar way to that of the desired thrust direction at the position level, and we do not show it in Fig. 2 due to lack of space.
The desired direction of the thrust at position level at time can be denoted as follows:
The desired direction of the thrust at velocity level at time can be denoted as follows:
where is a weight coefficient.
Hence, we can construct a CLF based on the desired attitude as follows:
where , , , and the values of and are chosen specifically to make the quadratic.
Iv-A3 Orientation-level QP controller
A procedure similar to design the position-level QP controller (11) can be used for the formulation of the attitude-level QP controller as follows:
where is the upper bound of body rotational rates, is positive definite, is a positive constant. is a slack variable for stability constraints, .
Remark 2. Note that the optimization (16) is not sensitive to the parameter as long as it is large enough (e.g. ), such that stability constraints violation is heavily penalized.
Iv-B Safety Barrier Scheme with a Limited Sensing Range
In practice, for quadrotor tracking in cluttered environments, the reference trajectory becomes unsafe when there exist unexpected obstacles in or near the trajectory. The quadrotor should sacrifice its tracking performance and adjust its attitude to avoid unexpected obstacles in real-time as shown in Fig. 3.
Iv-B1 Safety obstacle-free region
An ellipsoid safe region can be obtained in real-time for the quadrotor with a limited sensing range based on the IRIS algorithm . The IRIS algorithm is efficient in quickly computing the largest ellipsoidal regions of obstacle-free space through semidefinite programming. For a quadrotor tracking task, when there exist obstacles within its sensing region, a maximum ellipsoid in mathematical form can be constructed by utilizing the distance from the quadrotor to the obstacles in each direction.
Remark 3. Note that the quadrotor has a limited sensing range , such that a point on an obstacle is detected only when , where is the current position of the quadrotor in the inertial coordinate system .
We represent the inscribed ellipsoid as an image of the unit ball:
where , denotes a unit ball, denotes the mapping matrix, and denotes the offset vector. The matrix and can be obtained for specific polyhedrons (e.g. obstacles) based on the IRIS algorithm. Then, we can get the following equation via equivalent transformation:
Finally, we can construct a ZCBF at the position level by taking the inscribed ellipsoid as the safe region of the quadrotor:
where is the current position of quadrotor.
Remark 4. Note that shows that the quadrotor stays within the safety ellipsoid region .
We can construct another ZCBF to drive the direction of the quadrotor thrust toward the center of the obstacle-free ellipsoid to avoid obstacles as illustrated in Fig. 3:
where , .
Iv-B2 Safety-preserving cascaded QP control (SPQC)
A minimally modified SPQC controller can be designed to avoid obstacles based on these two ZCBFs and the estimated high-confidence interval (9) accounting for wind disturbances. The SPQC is designed to modify the nominal tracking controller in a way of minimally modified. GPs are employed to estimate the wind disturbances in terms of the predicted mean and variance of the model error through (7) and (8).
As shown by our previous work , safety constraints can be enforced on the uncertain system (6) based on the ZCBF to keep the quadrotor’s position in the safe region . Hence, the position-level safety constraints can be enforced as follows:
where is the thrust generated by the nominal position-level controller (11), , and is a slack variable.
To avoid obstacles, a safe attitude-level QP based on the ZCBF is designed to construct the attitude-level safety constraints as follows:
where is the body rotational rates generated by the nominal attitude-level controller (16), , and is a slack variable.
Remark 5. Note that the solution and to the QP in (21) and (22) are always feasible because the slack variables and can ensure no conflict among the safety and control input constraints. Furthermore, the weights and are set large values (e.g. , ) to penalize safety violation, and hence, the optimizations in (21) and (22) are not sensitive to the and parameters.
We built a simulator with Python 3.6 to numerically validate the performance of the proposed SPQC controller. The python library CVXOPT  is utilized to solve the QP problem. The simulation time is set to be 20 , and the control frequency is 50 . The quadrotor model is a crazyflie 2.0 with a two-meter sensing range. The mass of the quadrotor , the maximum thrust , and the body rotational rate .
V-a Experimental setup
|Uncertainty||Wind Disturbance||NMPC ||SPQC-N||SPQC|
In the experiments, we assess two aspects of the proposed SPQC controller: (i) trajectory tracking performance under different wind disturbances, and (ii) trajectory tracking with obstacle avoidance under varying wind disturbances. The reference trajectory is given as a spiral curve and . The initial state of the quadrotor is , , , , , , , , .
We use GPs to learn the uncertainty vector (6
). Each GP uses the same mixture of linear and radial basis function (RBF) kernels to capture the model uncertainties that result from wind disturbances. Each GP uses the pastobservations collected at 50 . To generate high confidence intervals to estimate wind disturbances, we use , where , as high confidence intervals (9).
V-B Trajectory Tracking under Different Wind Disturbances
As shown in Fig. 4(b), we define zones A, B and C with different wind disturbances. In zone A, the wind disturbance is constant: . In zone B, a varying wind field , where and is added to . In zone C, gusts is added to on the system from to to simulate a sudden wind gust.
It can be seen from Fig. 4(a), the position error converges to zero in each zone. The sudden gust pushes the quadrotor away from the trajectory when the quadrotor travels in zone C, and the SPQC controller enables the quadrotor back to the trajectory after the gust. Fig. 4(a) shows that the actual wind disturbance lies within the uncertainty interval estimated via GPs when the quadrotor is in zone A and zone B. Besides, the actual wind disturbance lies within the uncertainty interval after the sudden wind blow in zone C.
We compare the SPQC controller with the NMPC  and an SPQC-N controller, where the SPQC-N is an ablation version of SPQC without estimated model uncertainties. The Root Mean Square Error (RMSE) of position tracking for each controller is shown in Table. I. In each zone, the SPQC method achieves a smaller tracking error than the NMPC and SPQC-N controller.
V-C Trajectory Tracking with Obstacles
In the second experiment, the quadrotor is commanded to track the same dynamic time-varying trajectory under a varying wind disturbance. The wind disturbance . The reference trajectory is surrendered by several static obstacles. Two dynamic obstacles fly to the quadrotor along the reference trajectory with a speed of when the quadrotor is tracking the .
Figure 5(d) shows the tracking error between the quadrotor position and the reference trajectory . When there are no obstacles along the trajectory, the tracking error converges to zero. Furthermore, at about 2.5, 7, 14, and 16 seconds in the simulation time, the tracking errors rise to high values which indicates that the quadrotor has changed its trajectory to avoid collision with the detected obstacles. Fig. 5 shows that the SPQC controller stably tracks the reference trajectory when the trajectory is safe, while the controller relaxes tracking the reference when avoiding obstacles. This simulation results indicate that the SPQC controller can mediate the trade-off between safety and tracking performance.
This paper presents a novel learning-based SPQC scheme that achieves high-accuracy tracking performance while guaranteeing safety for the quadrotor with a limited sensing range and under wind disturbances. The proposed SPQC scheme consists of a nominal tracking controller and the safety constraints based on ZCBFs. In this control scheme, the GPs are exploited to online estimate the uncertainties of the wind disturbances, and an LMP algorithm is designed to generate the desired tracking attitude. By minimally modifying the nominal tracking controls, the quadrotor can avoid unexpected obstacles while staying within the safety region represented by ZCBFs. The proposed SPQC algorithm was shown to improve tracking performance compared with an NMPC  method under different disturbances. Numerical simulation results show that the SPQC controller was capable of performing the trajectory tracking task with obstacle avoidance capacity under varying wind disturbances.
-  M. Khan, M. Zafar, and A. Chatterjee, “Barrier functions in cascaded controller: Safe quadrotor control,” in 2020 American Control Conference (ACC). IEEE, 2020, pp. 1737–1742.
-  G. Wu and K. Sreenath, “Safety-critical control of a 3d quadrotor with range-limited sensing,” in Dynamic Systems and Control Conference, vol. 50695. American Society of Mechanical Engineers, 2016, p. V001T05A006.
-  A. D. Ames, X. Xu, J. W. Grizzle, and P. Tabuada, “Control barrier function based quadratic programs for safety critical systems,” IEEE Transactions on Automatic Control, vol. 62, no. 8, pp. 3861–3876, 2016.
-  L. Wang, E. Theodorou, and M. Egerstedt, “Safe learning of quadrotor dynamics using barrier certificates,” 2018 IEEE International Conference on Robotics and Automation (ICRA), pp. 2460–2465, 2018.
-  D. Q. Mayne, J. B. Rawlings, C. V. Rao, and P. O. M. Scokaert, “Constrained model predictive control: Stability and optimality,” Automatica, vol. 36, pp. 789–814, 2000.
-  J. C. N. Pereira, V. J. S. Leite, and G. V. Raffo, “Nonlinear model predictive control on se(3) for quadrotor trajectory tracking and obstacle avoidance,” 2019 19th International Conference on Advanced Robotics (ICAR), pp. 155–160, 2019.
-  K. Pereida and A. P. Schoellig, “All models are wrong: Robust adaptive model predictive control for safe and high accuracy trajectory tracking in the presence of model errors.”
-  R. Deits and R. Tedrake, “Computing large convex regions of obstacle-free space through semidefinite programming,” in WAFR, 2014.
C. E. Rasmussen and C. K. I. Williams, “Gaussian processes for machine learning,” inAdaptive computation and machine learning, 2005.
-  X. Xu, P. Tabuada, J. W. Grizzle, and A. D. Ames, “Robustness of control barrier functions for safety critical control,” in ADHS, 2015.
-  L. Qian and H. H. T. Liu, “Path-following control of a quadrotor uav with a cable-suspended payload under wind disturbances,” IEEE Transactions on Industrial Electronics, vol. 67, pp. 2021–2029, 2020.
-  L. Zheng, J. Pan, R. Yang, H. Cheng, and H. Hu, “Learning-based safety-stability-driven control for safety-critical systems under model uncertainties,” 2020.
-  M. S. Andersen, J. Dahl, and L. Vandenberghe, “Cvxopt: A python package for convex optimization, version 1.1. 6,” Available at cvxopt. org, vol. 54, 2013.
-  M. I. Owis, S. El-Bouhy, and A. El-Badawy, “Quadrotor trajectory tracking control using non-linear model predictive control with ros implementation,” 2019 7th International Conference on Control, Mechatronics and Automation (ICCMA), pp. 243–247, 2019.