
IEEE/CAA Journal of Automatica Sinica
Citation: | K. Shao, K. Huang, S. Zhen, H. Sun, and R. Yu, “A novel approach for trajectory tracking control of an under-actuated quad-rotor UAV,” IEEE/CAA J. Autom. Sinica, vol. 11, no. 9, pp. 2030–2032, Sept. 2024. doi: 10.1109/JAS.2016.7510238 |
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 B{xb,yb,zb} 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 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=(xyz)T and an orientation vector r=(θψφ)T, wherein θ, ψ and φ are the Euler angles corresponding to xb-axis, yb-axis and zb-axis, respectively. Let q= (xyzθψφ)T∈R6 denote the generalized coordinates of the quad-rotor system.
The total lift force ∑4i=1Fi generated by the rotors points at zb-axis, and it can be described in the body-fixed frame B as
FB=(004∑i=1Fi) |
(1) |
where FB is the total lift force vector and Fi (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
R=[cψcφsθsψcφ−cθsφcθsψcφ+sθsφcψsφsθsψsφ+cθcφcθsψsφ−sθcφ−sψsθcψcθcψ] |
(2) |
where s∗=sin(∗), c∗=cos(∗). R is the direction cosine matrix. θ, ψ and φ 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
UI=(U1U2U3)=RFB=(cθsψcφ+sθsφcθsψsφ−sθcφcθcψ)4∑i=1Fi |
(3) |
where UI is the total lift force vector.
Then, by using Newton’s law, the dynamic equations of motion can be written as
m¨x=U1−K1˙x |
(4) |
m¨y=U2−K2˙y |
(5) |
m¨z=U3−mg−K3˙z |
(6) |
Ix¨θ=l(−F1+F2+F3−F4)−K4˙θ |
(7) |
Iy¨ψ=l(F1+F2−F3−F4)−K5˙ψ |
(8) |
Iz¨φ=M1−M2+M3−M4−K6˙φ |
(9) |
where m is the mass of the quad-rotor. Fi (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 xb-axis or yb-axis. Mi = CFi (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. Ki (i = 1, 2,...,6) is the aerodynamic drag coefficient corresponding to the UAV’s velocity of ˙q=(˙x˙y˙z˙θ˙ψ˙φ)T∈R6. Ix, Iy and Iz 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
[mR13mR23mR33IxlIylIzC](¨x¨y¨z¨θ¨ψ¨φ)=−(00mgR33000)+[111111111111−111−111−1−11−11−1](F1F2F3F4) |
(10) |
where R13 = cθsψcφ+sθsφ, R23 = cθsψsφ−sθcφ, and R33 = cθcψ. Equation (10) can be rewritten in the form of
M(q,t)¨q=Q(q,˙q,t)+Bτ(t) |
(11) |
with
M=[mR13mR23mR33IxlIylIzC],Q=−(00mgR33000) |
B=[111111111111−1−111−111−11−11−1],τ=(F1F2F3F4). |
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
n∑i=1Ali(q,t)˙qi+Al(q,t)=0,l=1,2,…,m |
(12) |
where 1≤m≤n with m the number of constraints and n the number of system’s generalized coordinates. Ali(⋅) and Al are both C1 in q and t. The second-order form of (13) can be rewritten in a matrix form of
A(q,t)¨q=b(q,˙q,t) |
(13) |
with A=[Ali]m×n and b=(b1b2⋯bm)T.
Servo control input: Note that the constraint force to be applied to realize the trajectory tracking constraint is τ. Let a=M−1/2Q, ϕ1=AM−1/2, −b=b−ϕ1a=b−AM−1/2Q and ϕ2=M−1/2B. For given ϕ1, ϕ2 and −b, there exists
(ϕ1ϕ2)τ=−b. |
(14) |
For given ϕ1, ϕ2 and −b, (14) is assumed to be consistent for all (q,˙q,t)∈Rn×Rn×R, i.e., (AM−1B)(AM−1B)+−b = −b, which is an existence condition for τ [14].
Then, the control input, i.e., the servo constraint force τ can be given by [15]
τ=(AM−1B)+(b−AM−1Q) |
(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
{x=10sin(t)y=10cos(t)z=3tθ=π4+π8sin(π2t)ψ=π2+π3cos(π2t)φ=π4sin(π3t). |
(16) |
Differentiating the constraint (16) with respect to t twice, (16) can be rewritten as (13) with
A=I(6×6),b={−10sin(t)−10cos(t)0−π332sin(π2t)−π312cos(π2t)−π336sin(π3t) |
where I(6×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 e1=x−xs, e2=y−ys, e3=z−zs, e4=θ−θs, e5=ψ−ψs, e6=φ−φs, where ∗s denotes the corresponding simulated position and orientation. The position and orientation error of the quad-rotor ei (i = 1, 2,...,6) is of the order of 10−3, 10−3, 10−12, 10−4, 10−3 and 10−4 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.
Parameter | Description | Value |
m | Mass of the quad-rotor | 2.0 kg |
l | Arm distance of the quad-rotor | 0.2 m |
Ix | Moment of inertia around x-axis | 1.25 kg⋅m2 |
Iy | Moment of inertia around y-axis | 1.25 kg⋅m2 |
Iz | Moment of inertia around z-axis | 1.25 kg⋅m2 |
C | Force to moment scaling factor | 0.05 |
x0 | y0 | z0 | ˙x0 | ˙y0 | ˙z0 |
0 | 10 | 0 | 10 | 0 | 3 |
θ0 | ψ0 | φ0 | ˙θ0 | ˙ψ0 | ˙φ0 |
π/4 | 5π/6 | 0 | π2/16 | 0 | π2/12 |
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).
[1] |
B. J. Emran and H. Najjaran, “A review of quadrotor: An underactuated mechanical system,” Annu. Rev. Control, vol. 46, pp. 165–180, 2018. doi: 10.1016/j.arcontrol.2018.10.009
|
[2] |
J. Moreno-Valenzuela, R. Pérez-Alcocer, M. Guerrero-Medina, and A. Dzul, “Nonlinear PID-type controller for quadrotor trajectory tracking,” IEEE Trans. Mechatronics, vol. 23, no. 5, pp. 2436–2447, 2018. doi: 10.1109/TMECH.2018.2855161
|
[3] |
C. Trapiello, V. Puig, and B. Morcego, “Position-heading quadrotor control using LPV techniques,” IEEE Contr. Syst. Lett., vol. 13, no. 6, pp. 783–794, 2019.
|
[4] |
H. E. Glida, L. Abdou, A. Chelihi, C. Sentouh, and S.-E.-I. Hasseni, “Optimal model-free backstepping control for a quadrotor helicopter,” Nonlinear Dyn., vol. 100, no. 4, pp. 3449–3468, 2020. doi: 10.1007/s11071-020-05671-x
|
[5] |
S. Lian, W. Meng, K. Shao, J. Zheng, S. Zhu, and H. Li, “Full attitude control of a quadrotor using fast nonsingular terminal sliding mode with angular velocity planning,” IEEE Trans. Ind. Electron., vol. 70, no. 4, pp. 3975–3984, 2023. doi: 10.1109/TIE.2022.3176314
|
[6] |
H. A. Foudeh, P. Luk, and J. Whidborne, “Application of norm optimal iterative learning control to quadrotor unmanned aerial vehicle for monitoring overhead power system,” Energies, vol. 13, no. 12, pp. 3223–3236, 2020. doi: 10.3390/en13123223
|
[7] |
F. E. Udwadia and R. E. Kalaba, “Analytical dynamics: A new approach”, Cambridge, UK: Cambridge University Press, 1996.
|
[8] |
Y.-H. Chen, “Mechanical systems under servo constraints: The Lagrange’s approach,” Mechatronics, vol. 15, no. 3, pp. 317–337, 2005. doi: 10.1016/j.mechatronics.2004.09.003
|
[9] |
Y.-H. Chen, “Equations of motion of mechanical systems under servo constraints: The Maggi approach,” Mechatronics, vol. 18, no. 4, pp. 208–217, 2008. doi: 10.1016/j.mechatronics.2007.12.004
|
[10] |
A. H. Bajodah, D. H. Hodges, and Y.-H. Chen, “Inverse dynamics of servo-constraints based on the generalized inverse,” Nonlinear Dyn., vol. 39, pp. 179–196, 2005. doi: 10.1007/s11071-005-1925-x
|
[11] |
Y.-H. Chen, “Constraint-following servo control design for mechanical systems,” J. Vib. Control, vol. 15, no. 3, pp. 369–389, 2009. doi: 10.1177/1077546307086895
|
[12] |
A. D. Schutte, “Permissible control of general constrained mechanical systems,” J. Franklin Inst., vol. 347, no. 1, pp. 208–227, 2010. doi: 10.1016/j.jfranklin.2009.10.002
|
[13] |
F. E. Udwadia, “A new perspective on the tracking control of nonlinear structural and mechanical systems,” Proc. R. Soc. Lond., vol. 459, pp. 1783–1800, 2003. doi: 10.1098/rspa.2002.1062
|
[14] |
P. B. Koganti and F. E. Udwadia, “Unified approach to modeling and control of rigid multibody systems,” J. Guid. Control Dyn., vol. 39, no. 12, pp. 1–16, 2016.
|
[15] |
R. Yu, Y.-H. Chen, H. Zhao, and H. Sun, “Uniform ultimate boundedness for underactuated mechanical systems as mismatched uncertainty disappeared,” Nonlinear Dyn, vol. 95, pp. 2765–2782, 2019. doi: 10.1007/s11071-018-4721-0
|
[1] | Fazhi Song, Ning Cui, Shuaiqi Chen, Kai Zhang, Yang Liu, Xinkai Chen, Jiubin Tan. Beyond Performance of Learning Control Subject to Uncertainties and Noise: A Frequency-Domain Approach Applied to Wafer Stages[J]. IEEE/CAA Journal of Automatica Sinica, 2025, 12(1): 198-214. doi: 10.1109/JAS.2024.124968 |
[2] | Yufeng Lian, Xingtian Xiao, Jiliang Zhang, Long Jin, Junzhi Yu, Zhongbo Sun. Neural Dynamics for Cooperative Motion Control of Omnidirectional Mobile Manipulators in the Presence of Noises: A Distributed Approach[J]. IEEE/CAA Journal of Automatica Sinica, 2024, 11(7): 1605-1620. doi: 10.1109/JAS.2024.124425 |
[3] | Zheng Wu, Yiyun Zhao, Fanbiao Li, Tao Yang, Yang Shi, Weihua Gui. Asynchronous Learning-Based Output Feedback Sliding Mode Control for Semi-Markov Jump Systems: A Descriptor Approach[J]. IEEE/CAA Journal of Automatica Sinica, 2024, 11(6): 1358-1369. doi: 10.1109/JAS.2024.124416 |
[4] | Luigi D’Alfonso, Francesco Giannini, Giuseppe Franzè, Giuseppe Fedele, Francesco Pupo, Giancarlo Fortino. Autonomous Vehicle Platoons In Urban Road Networks: A Joint Distributed Reinforcement Learning and Model Predictive Control Approach[J]. IEEE/CAA Journal of Automatica Sinica, 2024, 11(1): 141-156. doi: 10.1109/JAS.2023.123705 |
[5] | Jiawen Kang, Junlong Chen, Minrui Xu, Zehui Xiong, Yutao Jiao, Luchao Han, Dusit Niyato, Yongju Tong, Shengli Xie. UAV-Assisted Dynamic Avatar Task Migration for Vehicular Metaverse Services: A Multi-Agent Deep Reinforcement Learning Approach[J]. IEEE/CAA Journal of Automatica Sinica, 2024, 11(2): 430-445. doi: 10.1109/JAS.2023.123993 |
[6] | Yi Yu, Guo-Ping Liu, Yi Huang, Peng Shi. Optimal Cooperative Secondary Control for Islanded DC Microgrids via a Fully Actuated Approach[J]. IEEE/CAA Journal of Automatica Sinica, 2024, 11(2): 405-417. doi: 10.1109/JAS.2023.123942 |
[7] | Yunjun Zheng, Jinchuan Zheng, Ke Shao, Han Zhao, Hao Xie, Hai Wang. Adaptive Trajectory Tracking Control for Nonholonomic Wheeled Mobile Robots: A Barrier Function Sliding Mode Approach[J]. IEEE/CAA Journal of Automatica Sinica, 2024, 11(4): 1007-1021. doi: 10.1109/JAS.2023.124002 |
[8] | Zewei Zheng, Jiazhe Li, Zhiyuan Guan, Zongyu Zuo. Constrained Moving Path Following Control for UAV With Robust Control Barrier Function[J]. IEEE/CAA Journal of Automatica Sinica, 2023, 10(7): 1557-1570. doi: 10.1109/JAS.2023.123573 |
[9] | Longlong Chen, Zongyang Lv, Xiangyu Shen, Yuhu Wu, Xi-Ming Sun. Adaptive Attitude Control for a Coaxial Tilt-Rotor UAV via Immersion and Invariance Methodology[J]. IEEE/CAA Journal of Automatica Sinica, 2022, 9(9): 1710-1713. doi: 10.1109/JAS.2022.105827 |
[10] | Naifeng Wen, Lingling Zhao, Xiaohong Su, Peijun Ma. UAV Online Path Planning Algorithm in a Low Altitude Dangerous Environment[J]. IEEE/CAA Journal of Automatica Sinica, 2015, 2(2): 173-185. |
[11] | Shuochen Liu, Zhiyong Geng, Junyong Sun. Finite-time Attitude Control: A Finite-time Passivity Approach[J]. IEEE/CAA Journal of Automatica Sinica, 2015, 2(1): 102-108. |
[12] | Xiaoli Liu, Shengchao Zhen, Kang Huang, Han Zhao, Ye-Hwa Chen, Ke Shao. A Systematic Approach for Designing Analytical Dynamics and Servo Control of Constrained Mechanical Systems[J]. IEEE/CAA Journal of Automatica Sinica, 2015, 2(4): 382-393. |
[13] | Zhou Fang, Hua Tian, Ping Li. Probabilistic Robust Linear Parameter-varying Control of a Small Helicopter Using Iterative Scenario Approach[J]. IEEE/CAA Journal of Automatica Sinica, 2015, 2(1): 85-93. |
[14] | Bin Xian, Jianchuan Guo, Yao Zhang. Adaptive Backstepping Tracking Control of a 6-DOF Unmanned Helicopter[J]. IEEE/CAA Journal of Automatica Sinica, 2015, 2(1): 19-24. |
[15] | Zixuan Liang, Zhang Ren, Xingyue Shao. Decoupling Trajectory Tracking for Gliding Reentry Vehicles[J]. IEEE/CAA Journal of Automatica Sinica, 2015, 2(1): 115-120. |
[16] | Lu Wang, Jianbo Su. Trajectory Tracking of Vertical Take-off and Landing Unmanned Aerial Vehicles Based on Disturbance Rejection Control[J]. IEEE/CAA Journal of Automatica Sinica, 2015, 2(1): 65-73. |
[17] | Cheng Peng, Yue Bai, Xun Gong, Qingjia Gao, Changjun Zhao, Yantao Tian. Modeling and Robust Backstepping Sliding Mode Control with Adaptive RBFNN for a Novel Coaxial Eight-rotor UAV[J]. IEEE/CAA Journal of Automatica Sinica, 2015, 2(1): 56-64. |
[18] | Huiyang Liu, Long Cheng, Min Tan, Zengguang Hou. Containment Control of General Linear Multi-agent Systems with Multiple Dynamic Leaders: a Fast Sliding Mode Based Approach[J]. IEEE/CAA Journal of Automatica Sinica, 2014, 1(2): 134-140. |
[19] | Tengfei Liu, Zhongping Jiang. Distributed Control of Nonlinear Uncertain Systems: A Cyclic-small-gain Approach[J]. IEEE/CAA Journal of Automatica Sinica, 2014, 1(1): 46-53. |
[20] | Xiaoming Sun, Shuzhi Sam Ge. Adaptive Neural Region Tracking Control of Multi-fully Actuated Ocean Surface Vessels[J]. IEEE/CAA Journal of Automatica Sinica, 2014, 1(1): 77-83. |
Parameter | Description | Value |
m | Mass of the quad-rotor | 2.0 kg |
l | Arm distance of the quad-rotor | 0.2 m |
Ix | Moment of inertia around x-axis | 1.25 kg⋅m2 |
Iy | Moment of inertia around y-axis | 1.25 kg⋅m2 |
Iz | Moment of inertia around z-axis | 1.25 kg⋅m2 |
C | Force to moment scaling factor | 0.05 |
x0 | y0 | z0 | ˙x0 | ˙y0 | ˙z0 |
0 | 10 | 0 | 10 | 0 | 3 |
θ0 | ψ0 | φ0 | ˙θ0 | ˙ψ0 | ˙φ0 |
π/4 | 5π/6 | 0 | π2/16 | 0 | π2/12 |
Parameter | Description | Value |
m | Mass of the quad-rotor | 2.0 kg |
l | Arm distance of the quad-rotor | 0.2 m |
Ix | Moment of inertia around x-axis | 1.25 kg⋅m2 |
Iy | Moment of inertia around y-axis | 1.25 kg⋅m2 |
Iz | Moment of inertia around z-axis | 1.25 kg⋅m2 |
C | Force to moment scaling factor | 0.05 |
x0 | y0 | z0 | ˙x0 | ˙y0 | ˙z0 |
0 | 10 | 0 | 10 | 0 | 3 |
θ0 | ψ0 | φ0 | ˙θ0 | ˙ψ0 | ˙φ0 |
π/4 | 5π/6 | 0 | π2/16 | 0 | π2/12 |