Full-Pose Trajectory Tracking of Overactuated Multi-Rotor Aerial Vehicles With Limited Actuation Abilities

This letter presents a novel optimization-based full-pose trajectory tracking method to control overactuated multi-rotor aerial vehicles with limited actuation abilities. The proposed method allocates feasible control inputs to track a reference trajectory, while ensuring the tracking of the reference position, and while tracking the closest feasible attitude. The optimization simultaneously searches for a feasible trajectory and corresponding feasible control inputs from the infinite possible solutions, while ensuring smooth control inputs. The proposed real-time algorithm is tested in extensive simulation on multiple platforms with fixed and actuated propellers. The simulation experiments show the ability of the proposed approach to exploit the complex set of feasible forces and moments of overactuated platforms while allocating smooth feasible control inputs.


I. INTRODUCTION
U NMANNED Aerial Vehicles (UAV)s gained popularity in the last two decades due to their versatility, and wide range of applications spanning aerial-physical inspection, photography and others. With the expansion of applications, many new UAV designs emerged aiming at expanding the work space beyond what was possible with the classical quadrotor. Such new designs changed the number and configuration of propellers to construct fully actuated and overactuated platforms [1].
Overactuated platforms are UAVs with redundant control inputs, such that the number of control inputs is larger than the dimensionality of the 6D force-moment work space. Such platforms provide redundancy in the case of propeller failures and ensure full actuation when some of their actuation limits have been reached. For example, [2] prove that for a platform with fixed propellers and a uni-directional thrust constraint, at least seven propellers are required to achieve omnidirectional thrust (i.e., full actuation in all directions). Many other overactuated platforms are presented in the literature, such as [3], [4], [5], etc... However, most overactuated platforms operate away from actuation limits and thus do not fully exploit the full range of the platforms' control inputs. In spite of the advantages of overactuated platforms, the larger control set provides many challenges.
One of the challenges of interest in this letter is the allocation of feasible control inputs to track a reference trajectory, in the presence of an infinite number of such possible controls and while satisfying actuation limits. In many designs, the minimum energy solution (i.e., moore-penrose pseudo inverse allocation [6]) provides feasible controls to hover around some of the operating points, and could break actuation limits when diverging from those points. For example, the minimum energy soltuion for the dual-tilt quadrotor shown in [7] provides a feasible solution when the desired force is parallel to the platform's vertical axis, and violates the actuation constraints in other operating points. On the other hand, the authors in [8] show that for the fixed propeller overactuated heptarotor they designed, the minimum energy solution is guaranteed to break at least one of the actuation constraints. To avoid actuation constraints, the authors in [9] suggest a null space-based approach to find feasible control inputs to achieve a desired force and moment. to the currently applied controls. The optimization is relaxed by adding an error term, assumed to be in the null space of the allocation matrix. Similar to [9], the authors in [10] suggest to allocate incremental control inputs added to the currently applied controls; however, their proposed overactuated platform had only thrust constraints, and thus it was possible to operate away from actuation limits.
While overactuated and fully actuated platforms have the ability to track independent position and attitude [11] away from actuation limits, the full 6D trajectory tracking have to be relaxed near actuation limits to ensure the satisfaction of the corresponding constraints. Correspondingly, a trajectory could be modified online to ensure that the allocated forces and moments are inside the platform's set of feasible forces and moments. Most solutions in the literature follow the reference position while compromising the reference attitude. This approach is similar to the classical approach applied to classical quadrotors. In classical quadrotors, the set of feasible forces is simply a line along the body's vertical axis [1]. As such, classical controllers point the platform's vertical axis in the direction of the desired force [12]. As the set of feasible forces and moments is higher dimensional for overactuated and fully actuated platforms, multiple letters from the literature estimate the corresponding set to find a feasible solution. The authors in [13] define a maximum roll and pitch angle the platform could reach, and allocate forces and moments within the defined constraints, implicitly estimated as a cone. Note that multiple letters from the literature follow a similar approach. On the other hand, multiple letters in the literature present solutions after estimating the set of feasible forces while applying zero moments, referred to as the force set. For example, the authors in [14] generate 6D trajectory primitives, and test the feasibility of each primitive offline. While their method is promising, it requires the definition of geometrical limits of the platform's thrust and angular velocity. As such, their method can ensure the feasibility of the trajectory, but it does not fully exploit the set of feasible control. On the other hand, the authors in [15] estimate the platform's force set as a cylinder, and limit the maximum rotation of the platform such that the applied force is withing the defined bounds. Similarly, the authors in [16] estimate the force set as a cone, and allocate angular velocities to track the closest attitude to the reference, while tracking the desired position and satisfying the cone constraints. These letters provide a promising solution for trajectory tracking of overactuated and fully actuated platforms. However, all these solutions estimate the set of feasible forces of the platform, parameterized by a single parameter (maximum angle of the cone, radius of the cylinder, etc..). As shown in [17], the estimation of this parameter could either be conservative, and thus do not fully exploit the platform's actuation limits, or could be liberal and risk allocating unfeasible forces and moments. In addition, the complexity of the set of feasible forces and moments grows with the number of added control inputs [1]. Note also that in the above solutions, the force set is estimated while applying zero-moment, and the variable parameter is set conservatively such that the allocated controls are always feasible.
In recent years, optimization-based control allocation became very popular. Multiple letters from the literature showed that the optimization of the platform's trajectory over a time horizon could be a solution to ensure the feasibility of the trajectory [18], [19]. Such methods have also been used in the literature for the control allocation of multi-input systems, such as humanoids and aerial manipulators [20].
While there has been many control allocation strategies in the literature [21], in this letter we propose a novel optimizationbased trajectory tracking algorithm for overactuated and fully actuated platforms with limited actuation abilities. The proposed optimization framework searches online for statics based feasible control inputs to track a reference 6D trajectory directly in the set of feasible control inputs, without the need to model the set of feasible forces and moments. In the absence of such control inputs, it searches for feasible control inputs such that the reference position is tracked, while tracking a feasible attitude that is the closest to the reference. While searching for a feasible trajectory, the optimization algorithm enforces a smooth change in control inputs to accommodate for the platform's dynamics.
The performance of the proposed real-time algorithm is tested in an exhaustive realistic simulation. We demonstrate in these simulations the ability of the proposed approach to exploit the complex topology of the set of feasible forces and moments for overactuated platforms while satisfying actuation limits. These simulations also demonstrate the generalization of the proposed algorithm to different platforms, with fixed and actuated propellers.
The letter is organized as follows. Section II models a generic UAV and Section III defines the generic UAV control and feasible trajectory problem. Section IV describes the optimizationbased 6D trajectory tracking algorithm. Section V analyzes the proposed method experimentally, and finally, Section VI concludes the letter.

II. MODELING
Let us first denote by F W as the world frame with axes {x W , y W , z W } and a body fixed frame F B with axes {x B , y B , z B }. The origin of the body frame O B is assumed to coincide with the Center of Mass (CoM) of the platform. Let us denote by p B , v B ∈ R 3 as respectively the position of O B in F W , and its velocity with respect to (w.r.t.) F W , and by R B ∈ SO (3) and ω B ∈ R 3 as the orientation of F B in F W and its angular velocity w.r.t. F B . Furthermore, let us denote by N ∈ N >0 as the number of propellers, and let us assume for the sake of this letter, the generic case where each propeller can be tilted independently in S 2 (i.e., each propeller is actuated by 2 independent servo motors). Note that propellers tilting in S 1 or fixed are special cases of the above [22]. Let us denote by F p i as the propeller fixed frame, with its origin O p i coinciding with the fixed attachment point of the i th propeller in body frame. F p i is defined such that the propeller's thrust is always generated about its z−axis z p i . Finally, let us denote by R pi ∈ R i ⊂ SO(3) as a rotation that brings a vector in F p i to F B , where R i is the set of feasible propeller orientations. Depending on its orientation, each propeller can produce forces is the propeller's rotational speed, and c f its lift coefficient assumed constant. We denote by [u,ū] as the minimum and maximum thrust that can be generated by a propeller, and corresponding to the range of ω i .
Let us denote by u ∈ U ⊂ R 3 N as the platform's control input, where U is denoted as the set of feasible control inputs. It is easy to see that U can be deduced from the propellers' feasible orientations R i , and from the propellers' rotational speed range [ω,ω].
To understand the effect of the propellers' thrust on the platform's motion, let us write the platform's Newton-Euler equations as follows: >0 are the platform's mass and inertia matrix, g is the gravitational constant, f , m ∈ R 3 are propeller induced forces and moments applied on the platform's CoM in F B , and G is a matrix that rotates f from F B to F W . The platform's forces and moments for a platform with propellers tilting in S 2 can be written in the following format: where S(·) is the skew-symmetric operator, r = c τ /c f is the ratio between the drag coefficient and the lift coefficient, and I is the identity matrix. The matrix F , referred to as the full allocation matrix, is constant throughout the operation of the platform [22]. The full allocation matrix can be written as Finally, let us define the platform's force set at a specific moment as follows:

III. PLATFORM CONTROL
In this section we discuss the control structure behind the Fullpose trajectory tracking algorithm. The complete architecture is illustrated in Fig. 1.
A full-pose reference trajectory is denoted by q r (t) = (p r (t), R r (t)) : (3), where p r (t) ∈ R 3 is the reference position trajectory and R r (t) ∈ SO(3) is the reference attitude trajectory. It is assumed that q r (t) is a smooth polynomial trajectory up to the 4 th order (i.e., continuous jerk), similar to the one presented in [23]. Let us denote by f r (t) as the force required to follow a desired position p r (t), and by m r (t) as the moment required to follow a desired orientation R r (t).
For simplicity, let us assume that any direction of m ∈ R 3 is admissible as long as the corresponding control input does not reach saturation. Following this assumption, a feasible trajectory is illustrated in Fig. 1(a), and can be defined as follows: as a desired feasible trajectory as follows: where m d is the feasible desired moment required to follow the feasible rotation R T d (t) and dist is any function that represents the distance between the two rotations, such that the function is minimal when the two rotations are equivalent. Given the above definition of a feasible trajectory, the objective of this letter is to find a feasible trajectory q d (t) that is the closest to the reference trajectory as stated in (4). This problem can be stated as follows: Problem 1: Given a reference trajectory q r (t) : [t 0 , t f ], find a feasible trajectory: where it was assumed that f W d (t) = f W r (t).

A. Feedback Controller
First, let us state the position and attitude controllers: where K p , K v , K R , K w are control gains, e p , e v , e R and e w are respectively the error terms in position, velocity, orientation, and angular velocity, and FF is the feedforward attitude tracking term. While the existence of the feedforward term enhances the trajectory tracking of the platform, the platform can still converge to the desired trajectory in the absence of such term [15]. The implementation of the feedforward term requires the computation of ω d andω d , in addition to the computed R d . While it is straightforward to compute the feedforward term for the fixed trajectory q r (t), it might be impractical to compute it for the varying feasible trajectory q d (t). As such, the feedforward term will be ignored in the optimization formulation of this letter. Following the computation of the desired force and moment, a tentative control input can be computed as follows: where F † ∈ R 3 N ×6 is the moore-penrose pseudo-inverse of the allocation matrix F ,ū c is the minimum energy solution, B ∈ R 3 N ×max(0,3N −6) its null space basis, and x ∈ R max(0,3N −6) is an arbitrary variable, such that ∀x ∈ R max(0,3N −6) F y = 0. In the case where 3N − 6 > 0, even if f d ∈ F(m d ), it is possible that ∃u c ∈ U whileū c / ∈ U. In this case, the allocated control input in the null space of the allocation matrix allows changing the control input such that u c ∈ U without changing the applied forces and moments. Then for a feasible force and moment, the control input in the null space can be found as follows: where Q = B T B. Following (10) we can find a feasible control input for any feasible force and moment.

IV. FEASIBLE TRAJECTORY OPTIMIZATION
To find a feasible trajectory q d (t) for an overactuated platform, one has to solve the minimization from (4), while allocating optimal control inputs for the desired forces and moments following (10).
If there exists a control input that allows the platform to follow q r (t), then q d (t) = q r (t). In the case where q r (t) is not feasible, our optimization will advantage following the reference position while following a feasible desired orientation closest to the reference (similar to [15] Then the problem would resolve in finding a platform orientation such . Obviously, the solution depends on the shape of the platform's force set. For example, in the case of a classical quadrotor, the force set is a line [1], and the trivial solution will always be to orient such line in the direction of the desired force, such Multiple approaches in the literature fit a predefined geometrical shape into the force set (such as a cone [16] or a cylinder [15]). As shown in [17], such assumptions can either be conservative, and thus do not fully exploit the feasible control set, or elaborate and risk allocating infeasible control inputs. In this letter, we propose to find a feasible solution by searching directly in U . As such, the allocated controls can take advantage of the entire U while ensuring the feasibility of any allocated control input.
Let us write the optimization function J at each time step as follows (in what follows we will drop the time dependence for clarity): Let us assume for simplicity that the constraint u c ∈ U can be expressed as a group of linear and quadratic constraints. Despite this assumption, the optimization from (11) is still non convex due to the optimization of the rotation matrix. Instead, let us define the force direction in body frame as d = R T f W / f W , and let us note by d B , d r and d d as the corresponding force direction in current body frame R B , in reference body frame R r and in desired body frame R d . As such, the minimization between R r and R d can be expressed as a minimization between d r and d d . To be able to minimize the distance between the two vectors, let us define a tangent space shown in Fig. 2, tangent to d B . The basis of the tangent space, referred to as C ∈ R 3×2 can be computed as follows: where c 2 = d B × e 3   us also denote by v r and v d as the deviations corresponding to d r and d d .
Since d is defined as a unit vector, then following the angleaxis representation, d can be written as follows: where θ = Cv . Let us assume θ to represent a small change in the platform's orientation, and rewrite d as follows: Note that v represents a vector change in the platform's orientation. To find the corresponding angular change, let us denote by R e as the error rotation matrix such that R e = R T B R d . The angular error about F B can be written e R = R ∧ e , where [ ] ∧ is the inverse skew symmetric operator. The error rotation matrix can be written in the Rodrigues' form as follows: where I n is an n × n identity matrix, and [ ] × is the skew- Following the small angle approximation made earlier, we can simplify (15) as follows: Let us update (17) for a computed v d as follows: where e z is the error between F B and F B r w.r.t. z B computed similar to [24]. Note that since e z is computed directly from the error between F B and F B r , the corresponding expected rotation is accommodated for in the computation of d B and d r , where F B r is the reference body frame. Also, note that the desired angular velocity ω d was not included in the error computation due to the difficulty of calculating its desired value for an online changing trajectory.
Following the above derivations, the optimization problem from (11) can be written as follows: (20) where α, β and γ are the weighting terms of the multi-objective optimization problem andv is the maximum allowed norm for v. The goal of the regularization term u c − u c (t − 1) 2 in the objective function is to avoid large changes in control inputs between one control step and another. In addition, the constraint imposed on the norm of v serves two objectives. First, it should be noted that the tangent space defined by (14) is only valid locally. As v increases, norm of d diverges from unity. As such, while optimizing the value of v, (19) ensures the validity of the assumptions made to write (14). In addition, if v r <v, the constraint from (19) ensures that the total rotation angle θ = Cv is chosen s.t. θ ≤ θ r . Gradients of the objective function and possible constraint functions are derived in the appendix of the technical report found in the following link [Technical Report].
It should be noted that the presented optimization algorithm is solved online at each time instance, and the ensuing optimal control input is commanded to the corresponding motors to actuate the platform. a) Note on convexity: The problem in (18)- (20) is the dual of the following optimization problem: First, it is assumed that a solution exists for the above problem, i.e., there exists a direction d at which we can apply the desired force, and there exists a corresponding moment that rotates the platform from the current direction d R to the desired direction d. The solution for the corresponding problem when a solution to J dual does not exist is left as future work. Let us write (23) as u c = Ad + Bx +û.
It is easy to prove that A T A and B T B are positive semidefinite. We omit the corresponding proof in this letter due to space limitations. In the case where U represents linear (quadratic) inequality constraints, J dual is a quadratic programming with linear (quadratic) constraints solved on the tangent space of the Riemannian manifold d = 1. Since both types of quadratic  programming optimizations are convex [25], and the optimization over the tangent space of a Riemannian manifold conserves convexity [26], J dual is convex and correspondingly J is convex.

A. Experimental Setup
We validate the proposed approach in a realistic simulation environment implemented in MATLAB/Simulink. The environment simulates the platform shown in Fig. 3 with the corresponding actuator dynamics. The platform is a modification of the dual-tilt quadrotor platform from [7] with a higher thrust to weight ratio such thatū/m B g = 1.05. With the current configuration, each propeller can generate thrust in a semi-sphere to avoid propellers' collision with the platform's arms. As such, the platform's constraints can be written as follows: where we assumed u = 0. The force set of the platform while applying zero moment is shown in Fig. 4. It is easy to see that, it is difficult to fit a cylinder or a cone into the force set (similar to [15], [16]) without either reducing the reachable force set, or breaking the actuation constraints. Similarly, while F(0) is similar in shape to an ellipsoid, it can be observed that the lateral diagonal force (directions such that f = [± f √ 2/2, ± f √ 2/2, 0] T ) is larger than those along the x B and y B axes. As such, an algorithm that takes full advantage of the feasible control set could exploit such non-symmetries in the force set, which could be otherwise assumed unreachable when modeling the force set as a standard shape. Note that the corresponding force set changes depending on the applied moment.
Finally, we will demonstrate the generalizability of the proposed method to Fully Actuated (FA) platforms, where we will exhibit the trajectory tracking performance of the proposed algorithm when used on the Fully Actuated platform from [15]. The FA platform can apply forces and moments in R 6 with bounded lateral forces. As such, the FA platform has limited hovering orientations near the horizontal hovering direction.

B. Algorithm Performance
We first study the performance of the proposed algorithm while applying the following trajectory: where Ω represents the euler angles of the platform, and where p r is represented in [m] and Ω is represented in degrees. This trajectory is feasible for the dual-tilt quadrotor. To demonstrate the performance of the proposed approach with unfeasible trajectories, we will analyze the performance of the platform after a fault, such thatū f ault =ū/3. This induced fault introduces unfeasible patches in the positive and negative x B and y B directions. Fig. 5 shows the performance of the dual-tilt quadrotor in both cases while following the desired trajectory, where θ, φ, ψ correspond to the platform's orientation in Euler angle representation. While the desired position is tracked with a transient error, in both cases the platform converges to the reference position correctly. However, while the fully functional platform is able to track correctly the desired orientation, the faulty platform compromises the reference orientation to be able to track the reference position. As we can see from Fig. 4, the dual-tilt quadrotor can achieve hover at any orientation with all its propellers functional. On the other hand, while the faulty platform could generate forces to hover at the desired vertical orientation, it can only do so while applying zero-moment. As such, the optimization algorithm was not able to find a feasible control input that rotates the platform to hover at the desired orientation. In addition, as the force set is non-symmetrical, with larger force authority near the diagonals of the force set, while the desired trajectory required only a change in pitch, the optimizer increased the roll angle to be able to correspondingly increase the pitch angle without jeopardizing the desired force.    6 shows the propeller commands of the faulty platform while following the trajectory from (27). From this figure, we can see that in spite of the trajectory being very close to the actuation limits, the commanded control inputs are smooth, due to the regularization term in the optimization function.
Finally, note that the proposed algorithm runs in MAT-LAB/simulink at 750 [Hz].

C. Circular Trajectory
One of the advantages of the Overactuated (OA) dual-tilt quadrotor is its ability to track a reference position while tracking an independent reference orientation. This advantage allows the platform, for example, to record a video of a desired object without jeopardizing its position. To highlight this ability, in this section we demonstrate the trajectory tracking performance of the platform while following a circular trajectory around a cylinder, while changing its altitude (for example to avoid a possible obstacle) and while continuously observing a desired line along the cylinder. To demonstrate the generalization of the proposed algorithm to different platforms, the same trajectory is also tracked by the FA platform discussed in Section V-A. Fig. 7 shows the corresponding circular trajectory tracking performance of the OA and FA platforms. In this figure, we refer to the Field of View (FoV) as the center of the platform's field of view projected on the observed cylinder, and calculated as the projection of the platform's x B onto the observed cylinder. From this figure we can first see that the proposed approach allows the FA platform to track the desired trajectory, while advantaging tracking of the desired position over the desired FoV. As such, once the desired pitch angle is large enough, the platform tracks the closest feasible pitch angle, while allocating feasible control inputs to track the desired position. On the other hand, we can see that the OA platform is able to track the desired trajectory, while simultaneously tracking the desired FoV. While following the desired trajectory, the OA platform was able to track the desired position with a maximum error of 0.0426 [m], while the FA platform tracked the desired trajectory with a maximum error of 0.0352 [m]. On the other hand, the OA platform was able to track the desired FoV with a maximum error of 0.0632 [m], while the FA platform had a significantly larger FoV tracking error of 0.6294 [m].
Note that in the case of the OA platform, and since the trajectory was not modified by the optimizer, it is possible to compute the FF term and to accommodate for the reference angular speed, calculated from the reference trajectory. Such modification can improve the tracking performance such that the maximum position tracking error is reduced to 0.0167 [m], and the FoV tracking error is reduced to 0.0201 [m]. However, in Fig. 7 we show the tracking results of both platforms without the computation of the desired angular velocity and acceleration for consistency.

VI. CONCLUSION
In this letter we proposed a novel optimization-based approach for the full-pose trajectory tracking of overactuated platforms with limited actuation abilities. The proposed approach runs in real-time, and allocates online feasible control inputs while modifying the reference attitude to satisfy actuation constraints. The proposed method was tested in a realistic simulation.
The simulative experiments, conducted on an overactuated and a fully actuated platform, show the ability of the proposed algorithm to allocate smooth control inputs trackable by the platform's dynamics. When the reference trajectory is feasible, the optimization method allocated control inputs to follow accurately the reference trajectory. On the other hand, when the reference trajectory is not feasible, the proposed method ensured accurate tracking of the reference position, while fully exploring the set of feasible forces to find the closest attitude to the reference one.
In the future, our team is working on manufacturing the platform shown in Fig. 3 to test the proposed algorithm to fly the manufactured drone.
In addition, we hope to expand the proposed approach in multiple directions: 1) to include the actuator dynamics into the optimization algorithm, and thus accommodate for the relatively slow dynamics of the servomotors; 2) to calculate online the desired angular velocities, and thus reduce the transient attitude tracking error; 3) to include vision requirements in the optimization loop, and thus track a desired field of view while modifying both the position and attitude parts of the reference trajectory.