A Novel Approach for Trajectory Tracking Control of an Under-Actuated Quad-Rotor UAV

Dear Editor,

This letter presents a novel Udwadia-Kalaba (U-K) approach for the trajectory tracking control of a quad-rotor unmanned aerial vehicle (UAV). Compared to conventional control approaches, the desired trajectories are treated as trajectory tracking constraints in this approach. Neither making any approximation or linearization of the nonlinear system nor imposing any a priori structure on the nature of the nonlinear controller, this approach provides closed-form nonlinear control. The control inputs satisfying the desired trajectory requirements can be obtained explicitly and in compact closed form by solving U-K equation. The theoretical analysis and MATLAB simulation results verify the validity and efficiency of this approach. The real-time servo constraint force is solved and the quad-rotor UAV satisfies the designed trajectory precisely.

Introduction: Quad-rotor UAVs have broad practical application prospects in both military and public services such as in environments research, resource exploration, national defense, material transport, logistics, search and rescue, space detection, etc., wherein it is dangerous for human beings. In order to accomplish these unmanned autonomous objects, UAVs are often required to follow desired trajectories autonomously. In recent years, various researches have been carried out for developing quad-rotor UAVs’ controllers [1]. Conventional methods such as classical PID and LQR control [2], [3] produce unsatisfactory performance because of the nonlinearity of the system. In order to improve control stability, various nonlinear control methods have been proposed such as backstepping control [4], sliding mode control [5], learning control [6], etc. In most of the above methods, due to the under-actuated feature of the quad-rotor, the control structure is complex and the control design is often a difficult task.

In analytical dynamics, the tracking control problem can be redefined as servo constraint control problem. The U-K theory simplifies this problem in a new way [7]. Using the research results of Udwadia and Kalaba, Chen systematically puts forward the concept of servo constraint control of mechanical systems and achieves the design of constraint force by servo control [8]. Chen [9] studies servo constraint problems on the basis of Maggi equation and indicates that the required constraint force can be obtained by servo control. Bajodah et al. [10] studies some mathematical computation problems in the servo control problem on U-K equation, providing some theoretical basis for the practical application of the equation. Using this equation, Chen [11] also carries on the thorough research on the adaptive robust control of uncertain systems. Schutte [12] studies the control problem of nonlinear mechanical systems with holonomic and nonholonomic constraints on the basis of U-K equation and proposes two types of nonlinear state feedback controllers which are shown to provide exact tracking and stabilization to the constrained system under certain conditions. Udwadia [13] firstly applies the servo constraint control method in the tracking control of nonlinear structural and mechanical systems and makes a preliminary research in this field.

Inspired by [13], this letter proposes a novel control design for the control of a quad-rotor UAV based on U-K approach. By solving U-K equation, the control inputs satisfying the desired trajectory requirements are obtained explicitly and in compact closed form. To verify the tracking performance, MATLAB simulation by ode45 integrator is conducted. The simulation results indicate that the proposed servo force is conveniently solved and the quad-rotor shows an excellent tracking performance.

Main results:

Plant modelling: As is shown in Fig. 1, each rotor is driven by a motor that produces lift force and moment. It is assumed that the body-fixed frame $\mathbf{B}\{{x}_{\mathrm{b}},{y}_{\mathrm{b}},{z}_{\mathrm{b}}\}$ is created at the mass center of the rigid quad-rotor body. The frame B has six degrees of freedom with respect to the earth-fixed frame $\mathbf{I}\{x,y,z\}$ which is assumed as a inertial frame. Therefore, the position and orientation of the quad-rotor can be described as a position vector $p={\left(x\;y\;z\right)}^{{T}}$ and an orientation vector $r={\left(\theta\; \psi\; \varphi \right)}^{{T}}$, wherein $\theta$, $\psi$ and $\varphi$ are the Euler angles corresponding to ${x}_{\mathrm{b}}$-axis, ${y}_{\mathrm{b}}$-axis and ${z}_{\mathrm{b}}$-axis, respectively. Let $q=$ ${\left(x\;y\;z\;\theta\; \psi\; \varphi \right)}^{{T}}\in {{\mathbb{R}}}^{6}$ denote the generalized coordinates of the quad-rotor system.

Figure  1.  Plant model of a quad-rotor.

DownLoad: Full-Size Img PowerPoint

The total lift force ${\sum }_{{i}=1}^{4}{F}_{i}$ generated by the rotors points at ${z}_{\mathrm{b}}$-axis, and it can be described in the body-fixed frame B as

where ${F}_{B}$ is the total lift force vector and ${F}_{i}$ (i = 1, 2, 3, 4) is the lift force produced by the ith rotor.

In order to move the quad-rotor model from earth to the fixed mass center, donate R as the transformation matrix from frame I to frame B, given by

where ${s}_{*}=\mathrm{s}\mathrm{i}\mathrm{n}\left(*\right)$, ${c}_{*}=\mathrm{c}\mathrm{o}\mathrm{s}\left(*\right)$. R is the direction cosine matrix. $\theta$, $\psi$ and $\varphi$ are the roll, pitch and yaw angle, respectively.

Therefore, the corresponding x-axis, y-axis, and z-axis component force vector in frame I can be written as

where ${U}_{I}$ is the total lift force vector.

Then, by using Newton’s law, the dynamic equations of motion can be written as

where m is the mass of the quad-rotor. F_i (i = 1, 2, 3, 4) is the lift force generated by the ith rotor. l is the distance from the center of rotation of the rotor to ${x}_{\mathrm{b}}$-axis or ${y}_{\mathrm{b}}$-axis. M_i = CF_i (i = 1, 2, 3, 4) is the additional moment imposed on the quad-rotor due to rotation of the corresponding rotor, with C the force to moment scaling factor. K_i (i = 1, 2,...,6) is the aerodynamic drag coefficient corresponding to the UAV’s velocity of $\dot{q}={(\dot{x}\;\dot{y}\;\dot{z}\;\dot{\theta }\;\dot{\psi }\;\dot{\varphi })}^{{T}}\in {{\mathbb{R}}}^{6}$. ${I}_{x}$, ${I}_{y}$ and ${I}_{z}$ are the moment of inertia of the quad-rotor around x-axis, y-axis and z-axis, respectively.

Assuming that the drag is very small at low speed, the drag coefficients equal to zero. Combining (3)−(9), the model of the quad-rotor becomes

where R₁₃ = ${c}_{\theta }{s}_{\psi }{c}_{\varphi }+{s}_{\theta }{s}_{\varphi }$, R₂₃ = ${c}_{\theta }{s}_{\psi }{s}_{\varphi }-{s}_{\theta }{c}_{\varphi }$, and R₃₃ = ${c}_{\theta }{c}_{\psi }$. Equation (10) can be rewritten in the form of

with

Trajectory tracking constraints: The problem of constrained motion can be framed as a servo constraint control problem in U-K approach, wherein the desired trajectories are treated as trajectory tracking constraints [7]. The above holonomic or nonholonomic constraint can be described by constraint equation, and the servo constraint force τ is redefined as the control input. Generally, the trajectory tracking constraints can be modeled as

where $1 \le m\le n$ with m the number of constraints and n the number of system’s generalized coordinates. ${A}_{li}(\cdot)$ and ${A}_{l}$ are both ${C}^{1}$ in $q$ and $t$. The second-order form of (13) can be rewritten in a matrix form of

with $A={\left[{A}_{li}\right]}_{m\times n}$ and $b={{(b}_{1}\;{b}_{2}\;\cdots\; {b}_{m})}^{{T}}$.

Servo control input: Note that the constraint force to be applied to realize the trajectory tracking constraint is τ. Let ${a=M}^{-1/2}Q$, ${\phi }_{1}={AM}^{-1/2}$, $\stackrel{-}{b}=b-{\phi }_{1}a=b-{AM}^{-1/2}Q$ and ${\phi }_{2}={M}^{-1/2}B$. For given ${\phi }_{1}$, ${\phi }_{2}$ and $\stackrel{-}{b}$, there exists

For given ${\phi }_{1}$, ${\phi }_{2}$ and $\stackrel{-}{b}$, (14) is assumed to be consistent for all $(q,\dot{q},t)\in {{\mathbb{R}}}^{n}$×${\mathbb{R}}^{n}$×${\mathbb{R}}$, i.e., $({AM}^{-1}B){({AM}^{-1}B)}^{+}\stackrel{-}{b}$ = $\stackrel{-}{b}$, which is an existence condition for $\tau$ [14].

Then, the control input, i.e., the servo constraint force τ can be given by [15]

where the superscript “+” denotes the Moore-Penrose generalized inverse.

Simulations: In the simulation, the UAV is assumed to track a helical trajectory described as

Differentiating the constraint (16) with respect to t twice, (16) can be rewritten as (13) with

where ${I}_{(6\times 6)}$ is a unit matrix.

Parameter values and initial conditions for simulation are listed in Tables 1 and 2. The MATLAB control model is solved by ode45 integrator and the simulation time is 30 s. Fig. 2 shows the numerical errors of position and orientation of the quad-rotor. It is clear that the error between the simulated position and orientation and the desired one is acceptable. Note that ${e}_{1}=x-{x}_{s}$, ${e}_{2}=y-{y}_{s}$, ${e}_{3}=z-{z}_{s}$, ${e}_{4}=\theta -{\theta }_{s}$, ${e}_{5}=\psi -{\psi }_{s}$, ${e}_{6}=\varphi -{\varphi }_{s}$, where ${*}_{s}$ denotes the corresponding simulated position and orientation. The position and orientation error of the quad-rotor ${e}_{i}$ (i = 1, 2,...,6) is of the order of 10⁻³, 10⁻³, 10⁻¹², 10⁻⁴, 10⁻³ and 10⁻⁴ m, respectively, which indicates that the servo constraint force reaches the required trajectory tracking constraints. The real-time control input calculated by (15) is shown in Fig. 3.

Table  1.  Parameter Values for Simulation

Parameter	Description	Value
$m$	Mass of the quad-rotor	2.0 kg
l	Arm distance of the quad-rotor	0.2 m
${I}_{x}$	Moment of inertia around x-axis	1.25 $\mathrm{k}\mathrm{g}\cdot {\mathrm{m}}^{2}$
${I}_{y}$	Moment of inertia around y-axis	1.25 $\mathrm{k}\mathrm{g}\cdot {\mathrm{m}}^{2}$
${I}_{z}$	Moment of inertia around z-axis	1.25 $\mathrm{k}\mathrm{g}\cdot {\mathrm{m}}^{2}$
C	Force to moment scaling factor	0.05

 | Show Table

DownLoad: CSV

Table  2.  Initial Conditions for Simulation

${x}_{0}$	${y}_{0}$	${z}_{0}$	${\dot{x}}_{0}$	${\dot{y}}_{0}$	${\dot{z}}_{0}$
0	10	0	10	0	3
${\theta }_{0}$	${\psi }_{0}$	${\varphi }_{0}$	${\dot{\theta }}_{0}$	${\dot{\psi }}_{0}$	${\dot{\varphi }}_{0}$
$\mathrm{\pi }/4$	$5\mathrm{\pi }/6$	0	${\mathrm{\pi }}^{2}/16$	0	${\mathrm{\pi }}^{2}/12$

 | Show Table

DownLoad: CSV

Figure  2.  Position and orientation errors of the quad-rotor.

DownLoad: Full-Size Img PowerPoint

Figure  3.  Lift force variations of the quad-rotor.

DownLoad: Full-Size Img PowerPoint

Conclusions: Different from conventional approaches, the proposed approach in this letter treats the desired quad-rotor’s trajectory as a constraint called trajectory tracking constraints. The real-time force generated by the four motors can be obtained explicitly and in compact closed form by solving U-K equation. This approach neither makes any assumption or linearization of the nonlinear system nor imposes any a priori structure on the nature of the nonlinear controller. The MATLAB simulation results indicate that the proposed control fulfills the trajectory tracking task of the quad-rotor UAV.

Acknowledgments: This work was supported by the Fundamental Research Funds for Central Universities of China (23GH020222) and the Basic Research Program of Taicang City (TC2023JC06).