−(2ωien+ωenn)×vin the acceleration andRbn(ωien+ωenn)in the angular velocity, moreover we used the notations
¯
a=a˜b−ba−a˜noise (5.48)
ω¯ =ω˜b−bω−ω˜noise=: P¯ Q¯ R¯
(5.49) Introducing statex= (pT,vT,qT,bTa,bTω)T, inputu= (a˜T,ω˜T)T and outputy= (pT,vT)T, the continuous time nonlinear state equations will have the usual form
˙
x= f(x,u,nx) (5.50)
y=h(x,ny) =
I3 0 0 0 0 0 I3 0 0 0
=Cx+ny (5.51)
Assuming constant ¯ω between two sampling instants and neglecting the noise, we are able to integrate the differential equation ofqby using exp(ΩT¯ )which can easily be determined in our case:
Ω¯int =
0 −PT¯ /2 −QT¯ /2 −RT¯ /2 PT¯ /2 0 RT¯ /2 −QT¯ /2 QT¯ /2 −RT¯ /2 0 PT¯ /2 RT¯ /2 QT¯ /2 −PT¯ /2 0
,ϑ = 1
2 q
(PT¯ )2+ (QT¯ )2+ (RT¯ )2 (5.52) eΩ¯int =I4cos(ϑ) +sin(ϑ)Ω¯int (5.53) Here we used left side rule for integration however other methods (trapezoidal rule etc.) can also be applied to obtain ¯Ωandϑ, especially during post-processing for identification purposes. As a consequence, using Euler method except for q, the discrete time state equations can be determined.
Discrete time state equations in Flat Earth navigation pk+1=pk+T vk
vk+1=vk+T n
Rnb(q)a¯+
0 0 1 T
g0 o qk+1=
I4cos(ϑ) +sin(ϑ)Ω¯int
qk (5.54)
ba,k+1=ba,k+Twba,k bω,k+1=bω,k+Twb
ω,k
5.6 Summary
In this chapter an improved EKF method was presented based on MEMS sensors containing IMU (3D acceleration and 3D angular velocity sensors), 3D magnetometer sensors and GPS sensors according to the WGS-84 standard. The goal was the fusion of these sensors to tolerate the large difference between IMU and GPS frequencies and estimate the system states for the different type of control algorithms in large distance and short distance
navigation of vehicles.
Thesis Group 4: I have developed an EKF based technique for sensor fusion that can well tolerate the different measurement frequencies and can be applied for any type of vehicles both in long distance and short distance missions, large and small height, and height dependent gravity acceleration. Three EKFs participate in the state estimation. The efficiency of the state estimation was demonstrated for a fixed wing UAV based on real flight data.
The results were published in [S5].
Thesis 4.1:I have elaborated the kinetic differential equations in which the orientation can be described by quaternion (converted to Euler angles for the user), the gravity can be described by the Roger model (or the Schwartz-Wei model for large height), and the GPS position and velocity equations take into consideration that the acceleration sensor (acc) measures the real acc plus the negative gravity acc, the IMU has unknown biases and the geomertical and magnetic North directions differ. EKF1 solves the quaternion estimation, EKF2 improves the orientation if GPS is present, and EKF3 determines the remaining state variables including the biases in an external loop if GPS measurements are available.
Thesis 4.2:I developed a set of output mappings and another set for output measurements to formulate the INS navigation conceptions and distributed them among the EKFs. Because TGPS>>TIMU thus EKF3 would be inaccurate, hence a subdivision and LTV approach was introduced to improve the state (S) and noise (Q) covariance matrices. It was experimentally demonstrated by using real flight data that the elaborated state estimation method can well satisfy the practical concepts formulated in the output mappings and measurements.
Chapter 6 Conclusions
The autonomous unmanned aerial (UAV) and the car-like ground (UGV) vehicles are nonlinear dynamic systems. Their control integrates the knowledge of various fields, and their research is in the center of the international trends. The main topic is the control of various vehicles (mobile robot agents) based on common principles including modeling, identification, path design, movement analysis and navigation using GPS/IMU sensor-fusion, fast communication for control and system optimization. The main goal of the research was to develop modern control design methods for the embedded control of quadrotor helicopters, propeller driven fixed wing airplanes and car-like robots taking into consideration the underactuated character of the nonlinear systems, i.e. the degree of freedom (DOF) is less than the number of actuators. For such systems not every path can be realized and the goal is to decrease the errors.
For quadrotor UGVs I applied a complex and an approximating nonlinear dynamic model for control purposes (dynamic model, simplified dynamic model and rotor dynamics) and I developed model based integral backstepping (IBSC) control algorithms satisfying nonlinear Lyapunov stability. The control is state dependent and consists of hierarchical position, orientation (attitude) and rotor control components. Path design methods were presented satisfying continuous acceleration or continuous jerk. I generalized the IBSC method for adaptive model parameter and disturbance force identification.
For fixed wing UAVs I applied a complex nonlinear dynamic model and I have elabo-rated four type control algorithms for the autonomous (unmanned) control, three for the attitude (orientation) control and a common one for position control. The path design can always be performed in position and Euler angles of the vehicle’s body frame, and the orientation (attitude) can be converted to unit quaternion (if necessary). Outside the singularity of Euler angles, I developed a model based IBSC attitude control satisfying nonlinear Lyapunov stability. For the general case of highly maneuvering (acrobatic) UAVs, I developed two quaternion based nonlinear attitude control methods, one control law without the use of the quaternion logarithm and a second using log(q). Both methods satisfy Lyapunov stability of the closed loop. The position control algorithm assures small errors in height, velocity and lateral motion during maneuvers. The path design method can select maneuvers from a palette and put them together smoothly at their boundaries.
For car-like UGVs I elaborated and analyzed a modified chain form model that can take into consideration the sliding effects in kinematic control design. The goal is to follow a desired reference path, the errors are the vehicle lateral error, the orientation (heading) error and a third error component which depends immediately on the kinematic control. The
high-level kinematic controller can be made robust using LMI technique. For the low level I developed three types of controllers: i) nominal control saving the steering angle of the high-level kinematic control as reference signal for the low level, ii) differential geometry based nonlinear I/O linearization (DGA) method, iii) flatness-based control for the low level. I have proved that both front wheel and rear wheel driven car-like robots satisfy the differentially flat properties. I have demonstrated by simulation that the high-level kinematic control-based approach can have lateral errors in the order of 1m in high-speed maneuvers that may cause problems for UGVs without visual information during lane changing or overtaking.
For state estimation I have developed an EKF based technique for sensor fusion that can well tolerate the different measurement frequencies and can be applied for any type of vehicles both in long distance and short distance missions, large and small height, and height dependent gravity acceleration. Three EKFs participate in the state estimation. The efficiency of the state estimation was demonstrated for a fixed wing UAV based on real flight data.
Bibliography
[1] S. Boubadallah and R. Siegwart, “Backstepping and sliding-mode techniques applied to an indoor micro quadrotor,” inProceedings of the IEEE International Conference on Robotics and Automation ICRA, Barcelona, Spain, April 2005, pp. 2247–2252.
[2] A. Das, K. Subbarao, and F. Lewis, “Dynamic inversion of quadrotor with zero-dynamics stabilization,” inProceedings of the IEEE Conference on Control Applica-tions CCA, San Antonio, USA, September 2008, pp. 1189–1194.
[3] T. Madani and A. Benallegue, “Control of a quadrotor mini-helicopter via full state backstepping technique,” in Proceedings of the IEEE Conference on Decision and Control CDC, San Diego, USA, December 2006, pp. 1515–1520.
[4] M. Cook,Flight Dynamics Principles. London: 2nd edition. Elsevier, 2007.
[5] B. Stevens and F. Lewis,Aircraft Control and Simulation. New York: Wiley, 1992.
[6] O. Harkegard,Flight Control Design Using Backstepping. Linköping, Sweden: Ph.D.
Thesis. Linköpings Universitet, 2001.
[7] I. K. Peddle,Acceleration Based Manoeuvre Flight Control System for Unmanned Aerial Vehicles. Matieland, South Africa: Ph.D. Thesis. Stellenbosch University, 2008.
[8] D. Blaauw,Flight Control System for a Variable Stability Blended-Wing-Body Un-manned Aerial Vehicle. Matieland, South Africa: M.Sc. Thesis. Stellenbosch Univer-sity, 2009.
[9] R. Rajamani,Vehicle dynamics and control. Springer, New York, 2006.
[10] L. talvala, K. Kritayakirana, and J. Gerdes, “Pushing the limits: From linekeeping to autonomous racing.”Annu. Rev. Control, vol. 35, no. 1, pp. 137–148, 2011.
[11] G. Max and B. Lantos, “Time optimal control of four-in–wheel-motors driven electric cars.”Periodica Polytechnica Electrical Engineering and Computer Science, vol. 58, no. 4, pp. 149–159, 2014.
[12] A. D. Luca, G. Oriolo, and C. Samson,Feedback control of a nonholonomic car-like robot. J.P. Laumond, Ed. ser. Lecture Notes in Control and Information Sciences, Springer Verlag, New York, 1998.
114
[13] A. Arogeti and N. Berman, “Path following of autonomous vehicles in the presence of sliding effects.” IEEE Transaction on Vehicular Technology, vol. 61, no. 4, pp.
1481–1492, 2012.
[14] C. Scherer and S. Weiland,Lecture Notes DISC Course on Linear Matrix Inequalities in Control, ver. 2.0, pp. 50–57. N/A, 1999.
[15] B. Lantos,Theory and Design of Control Systems II. (in Hungarian). Budapest:
Akademiai Kiado, 2003.
[16] R. van der Merwe, E. Wan, and S. Julier, “Sigma-point kalman filters for nonlinear estimation and sensor fusion. applications to integrated navigation,” inCollection of Technical Papers. AIAA Guidance, Navigation and Control Conference, Providence, USA, August 2004, pp. 1735–1764.
[17] S. Bonnabel, P. Martin, and P. Rouchon, “Symmetry-preserving observers,”IEEE Trans. on Automatic Control, vol. 53, no. 11, pp. 2514–2526, 2008.
[18] B. Lantos and L. Marton, Nonlinear control of vehicles and robots. Springer, London, 2011.
[19] O. Rodrigues, “Des lois géometriques qui regissent les déplacements d’ un systéme solide dans l’espace, et de la variation des coordonnées provenant de ces déplacement considérées indépendent des causes qui peuvent les produire (geometric laws which govern the displacements of a solid system in space: and the variation of the coordinates coming from these displacements considered independently of the causes which can produce them),”Journal de Mathématiques Pures et Appliquées, vol. 21, no. 4, pp.
375–385, November 1840.
[20] L. Kis, G. Regula, and B. Lantos, “Design and hardware-in-the-loop test of the embedded control system of an indoor quadrotor helicopter,” inProceedings of the Workshop on Intelligent Solutions in Embedded Systems WISES, Regensburg, Germany, July 2008, pp. 35–44.
[21] Y. Tan, J. Chang, H. Tan, and J. Hu, “Integral backstepping control and experimental implementation for motion system,” inProceedings of the 2000 IEEE International Conference on Control Applications, Anchorage, USA, September 2000, pp. 367–372.
[22] A. Sooumelidis, P. Gaspar, B. Lantos, and G. Regula, “Control of an experimental mini quad-rotor UAV,” inProceedings of the Mediterranean Conference on Control and Automation MED, Ajaccio, France, June 2008, pp. 1252–1257.
[23] M. Lungu, “Stabilization and control of a UAV flight attitude angles using the back-stepping method,”International Journal of Aerospace and Mechanical Engineering, vol. 6, no. 1, pp. 53–60, 2012.
[24] V. Klein and E. Morelli,Aircraft System Identification. Theory and Practice. Reston, Virginia: American Institute of Aeronautics and Astronautics, Inc., 2006.
[25] M. Rauw,FDC 1.4 – A Simulink Toolbox for Flight Dynamics and Control Analysis.
Draft Version 7. FDC User Manual. Haarlem, The Netherlands: Haarlem, 2005.
116 BIBLIOGRAPHY [26] R. Tjee and J. Mulder,Stability and Control Derivatives of the De Havilland
DHC-2 Beaver Aircraft. Delft, The Netherlands: Report LR-556, Delft University of Technology, Faculty of Aerospace Engineering, 1988.
[27] E. Oland, “Quaternion-based control of fixed-wing UAVs using logarithmic mapping,”
inProceedings of the 9th Interrnational Conference on Mechanical and Aerospace Engineering ICMAE, Budapest, Hungary, July 2018, pp. 11–17.
[28] H. Pacejka,Tire and vehicle dynamics. SAE International, 2005.
[29] M. Fliess, J. Levine, P. Martin, and P. Rouchon, “Lie–Bäcklund approach to equiv-alence and flatness of nonlinear systems,”IEEE Transaction on Automatic Control, vol. 44, no. 5, pp. 922–937, May 1999.
[30] L. Menhour, B. d’Andrea Novel, M. Fliess, and H. Mounier, “Coupled nonlinear vehicle control: Flatness-based setting with algebraic estimation techniques.”Control Engineering Practice, vol. 22, pp. 135–146, 2014.
[31] E. Diekema and T. Koornwinder, “Differentiation by integration using orthogonal polynomials, a survey,” Journal of Approximation Theory, vol. 164, pp. 637–667, 2012.
[32] G. Max and B. Lantos, “A comparison of advanced non-linear state estimation techniques for ground vehicles,”Hungarian Journal of Industry and Chemistry, vol. 42, no. 2, pp. 57–64, 2014.
[33] J. Cheng, D. Chen, R. Landry, L. Zhao, and D. Guan, “An adaptive unscented kalman filtering algorithm for mems/gps integrated navigation systems.”Hindawi Publishing Corporation, Journal of Applied Mathematics, Article ID 451939, pp. 1–8, 2014.
[34] J. Kim, B. Kiss, and D. Lee, “An adaptive unscented kalman filtering approach using selective scaling,” inIEEE Conference SMC2016, Budapest, Hungary, October 2016, pp. 1–6, Conference CD.
[35] L. Lukács,Advanced design methods for unmanned aerial vehicle control. PhD The-sis. Budapest University of Technology and Economics, Dept. Control Engineering and Informatics, 2017.
[36] L. Kis and B. Lantos, “Development of state estimation system with INS, Magne-tometer and Carrier Phase GPS for Vehicle Navigation,”Gyroscopy and Navigation, vol. 5, no. 3, pp. 153–161, 2014.
[37] B. Lantos,Efficient state estimation methods for orientation initialization and com-plete state estimation besed on sensor fusion. Task 2/4, Research Report. Gy˝or, Hungary: Széchenyi University, TAMOP-4.2.2.A-11/1/KONV-2012-0012, 2013.
[38] J. Farrell and M. Barth,The Global Positioning System & Inertial Navigation. New York: McGraw-Hill, 1999.
Publications
[S1] Béla Lantos, Zsofia Bodo. “High Level Kinematic and Low Level Nonlinear Dynamic Control of Unmanned Ground Vehicles”, Acta Polytechnica Hungarica, Vol.16 : 1 pp. 97-117. , 21 p., 2019
[S2] Zsofia Bodo, Béla Lantos. “Integrating Backstepping Control of Outdoor Quadrotor UAVs”, Periodica Polytechnica-Electrical Engineering and Computer Science, Vol.63 : 2 pp. 122-132. , 11 p., 2019
[S3] Zsofia Bodo, Bela Lantos. “Modeling and Control of Quadrotor UAVs”, IEEE International Symposium on Intelligent Systems and Informatics (SISY 2018), Subotica, Serbia, September 2018.
[S4] Zsofia Bodo, Bela Lantos. “Error Caused by Kinematic Control in the Dynamic Be-havior of Unmanned Ground Vehicles”, IEEE International Symposium on Applied Computational Intelligence and Informatics(SACI 2018), Timisoara, Romania, May 2018.
[S5] Zsofia Bodo, Bela Lantos. “State Estimation for UAVs Using Sensor Fusion”, IEEE International Symposium on Intelligent Systems and Informatics(SISY 2017), Subotica, Serbia, September 2017.
[S6] Zsofia Bodo, Bela Lantos. “Control of maneuvering fixed wing UAVs” IEEE International Symposium on Applied Computational Intelligence and Informatics (SACI 2020), Timisoara, Romania, May, 2020.
[S7] Zsofia Bodo, Bela Lantos. “Modeling and control of fixed wing UAVs” IEEE International Symposium on Applied Computational Intelligence and Informatics (SACI 2019), Timisoara, Romania, May, 2019.
[S8] Zsofia Bodo, Bela Lantos. “Nonlinear control of maneuvering fixed wing UAVs using quaternion logarithm”International Symposium on Measurement and Control in Robotics(ISMCR 2020), Piscataway (NJ), United States of America, October, 2020.
117
Appendix A
Derivation of the Generalized Kinematic Control
A.1 The original chain form of car-like robots
The original chain form of car-like robots uses the notationsθ for the orientation andφ for the steering angle, respectively. These notations differ to Fig. 4.1 but used here because the popularity of these notations in the literature. The kinematic model without slipping is as follows:
˙
x=cos(θ)v=: ¯v⇒v=v/¯ cos(θ)⇒x˙=v¯
˙
y=sin(θ)v=tan(θ)v¯ θ˙= tan(φ)
L v= tan(φ) Lcos(θv¯ φ˙=w
Using the geometric path definitionyd:= f(x)and the component errors e1:= f(x)−y
e2:= f0(x)−tan(θ)) e3:= f00(x)− tan(φ)
Lcos3(θ) the error derivatives satisfy the so called chain form:
˙
e1= f0(x)x˙−y˙= [f0(x)−tan(θ)]v¯=e2v¯
˙
e2= f00(x)x˙− 1 cos2(θ)
θ˙= [f00(x)− tan(φ)
Lcos3(θ)]v¯=e3v¯
˙
e3= f000(x)x˙− 1
cos2(φ)φ˙ 1
Lcos3(θ)−tan(φ)
L (−3) 1
cos3(θ)(−sin(θ))θ˙
= [f000(x)−3tan2(φ)tan(θ)
L2cos4(θ) ]v¯− 1
Lcos2(φ)cos3(θ)w=:u
i
For the chain form the controlucan be developed and from itwcan be determined:
w:={[f000(x)−3tan2(φ)tan(θ)
L2cos4(θ) ]v¯−u}Lcos2(φ)cos3(θ)
wherewis the derivative of the steering angle that can be integrated to obtain the steering angle. Notice that the above technique of de Luca and coworkers [12] cannot manage the slipping effects.
A Derivation of the Generalized Kinematic Control iii