Guidance, navigation and control of multirotors

UNIVERSITAT POLIT` ECNICA DE CATALUNYA Programa de Doctorat: AUTOM` ATICA, ROB` OTICA I VISI´ O Tesi Doctoral GUIDANCE, NAVIGATION AND CONTROL OF MULTIROTORS Bartomeu Rub´ı Perell´o Directors de la tesi: Bernardo Morcego Seix i Ramon P´erez Magran´e Octubre de 2020 To my parents and to my significant other i Abstract This thesis presents contributions to the Guidance, Navigation and Control (GNC) systems for multirotor vehicles by applying and developing diverse control techniques and machine learning theory with innovative results. The aim of the thesis is to obtain a GNC system able to make the vehicle follow predefined paths while avoiding obstacles in the vehicle’s route. The system must be adaptable to different paths, situations and missions, reducing the tuning effort and parametrisation of the proposed approaches. The multirotor platform, formed by the Asctec Hummingbird quadrotor vehicle, is studied and described in detail. A complete mathematical model is obtained and a freely available and open simulation platform is built. Furthermore, an autopilot controller is designed and implemented in the real platform. The control part is focused on the path following problem. That is, following a predefined path in space without any time constraint. Diverse control-oriented and geometrical algorithms are studied, implemented and compared. Then, the geometrical algorithms are improved by obtaining adaptive approaches that do not need any parameter tuning. The adaptive geometrical approaches are developed by means of Neural Networks. To end up, a deep reinforcement learning approach is developed to solve the path following problem. This approach implements the Deep Deterministic Policy Gradient algorithm. The resulting approach is trained in a realistic multirotor simulator and tested in real experiments with success. The proposed approach is able to accurately follow a path while adapting the vehicle’s velocity depending on the path’s shape. In the navigation part, an obstacle detection system based on the use of a LIDAR sensor is implemented. A model of the sensor is derived and included in the simulator. Moreover, an approach for treating the sensor data to eliminate the possible ground detections is developed. The guidance part is focused on the reactive path planning problem. That is, a path planning algorithm that is able to re-plan the trajectory online if an unexpected event, such as detecting an obstacle in the vehicle’s route, occurs. A deep reinforcement learning approach for the reactive obstacle avoidance problem is developed. This approach implements the Deep Deterministic Policy Gradient algorithm. The developed deep reinforcement learning agent is trained and tested in the realistic simulation platform. This agent is combined with the path following agent and the rest of the elements developed in the thesis obtaining a GNC system that is able to follow different types of paths while avoiding obstacle in the vehicle’s route. iii Resumen Esta tesis doctoral presenta varias contribuciones relacionas con los sistemas de Guiado, Navegaci´on y Control (GNC) para veh´ıculos multirotor, aplicando y desarrollando diversas t´ecnicas de control y de machine learning con resultados innovadores. El objetivo principal de la tesis es obtener un sistema de GNC capaz de dirigir el veh´ıculo para que siga una trayectoria predefinida mientras evita los obst´aculos que puedan aparecer en el recorrido del veh´ıculo. El sistema debe ser adaptable a diferentes trayectorias, situaciones y misiones, reduciendo el esfuerzo realizado en el ajuste y la parametrizaci´on de los m´etodos propuestos. La plataforma experimental, formada por el cuadric´optero Asctec Hummingbird, se estudia y describe en detalle. Se obtiene un modelo matem´atico completo de la plataforma y se desarrolla una herramienta de simulaci´on, la cual es de c´odigo libre. Adem´as, se dise˜na un controlador autopilot, el cual es implementado en la plataforma real. La parte de control est´a enfocada en el problema de path following. En este problema, el veh´ıculo debe seguir una trayectoria predefinida en el espacio tridimensional sin ninguna restricci´on temporal. Se estudian, implementan y comparan varios algoritmos de control y geom´etricos de path following. Luego, se mejoran los algoritmos geom´etricos usando redes neuronales para convertirlos en algoritmos adaptativos. Para finalizar, se desarrolla un m´etodo de path following basado en t´ecnicas de aprendizaje por refuerzo profundo (deep reinforcement learning). Este m´etodo implementa el algoritmo Deep Deterministic Policy Gradient. El agente inteligente resultante es entrenado en un simulador realista de multirotores y validado en la plataforma experimental real con ´exito. Los resultados muestran que el agente es capaz de seguir de forma precisa la trayectoria de referencia adaptando la velocidad del veh´ıculo seg´un la curvatura del recorrido. En la parte de navegaci´on se implementa un sistema de detecci´on de obst´aculos basado en el uso de un sensor LIDAR. Se deriva un modelo del sensor y este se incluye en el simulador. Adem´as, se desarrolla un m´etodo para tratar las medidas del sensor para eliminar las posibles detecciones del suelo. En cuanto a la parte de guiado, est´a focalizada en el problema de reactive path planning. Es decir, un algoritmo de planificaci´on de trayectoria que es capaz de re-planear el recorrido del veh´ıculo al instante si ocurre alg´un evento inesperado, como lo es la detecci´on de un obst´aculo en el recorrido del veh´ıculo. Se desarrolla un m´etodo basado en aprendizaje por refuerzo profundo para la evasi´on de obst´aculos. Este implementa el algoritmo Deep Deterministic Policy Gradient. El agente de aprendizaje por refuerzo se entrena y valida en un simulador de multirotors realista. v vi Este agente se combina con el agente de path following y el resto de elementos desarrollados en la tesis para obtener un sistema GNC capaz de seguir diferentes tipos de trayectorias evadiendo los obst´aculos que est´en en el recorrido del veh´ıculo. Resum Aquesta tesi doctoral presenta diverses contribucions relaciones amb els sistemes de Guiatge, Navegaci´o i Control (GNC) per a vehicles multirrotor, aplicant i desenvolupant diverses t`ecniques de control i de machine learning amb resultats innovadors. L’objectiu principal de la tesi ´es obtenir un sistema de GNC capa¸c de dirigir el vehicle perqu`e segueixi una traject`oria predefinida mentre evita els obstacles que puguin apar`eixer en el recorregut del vehicle. El sistema ha de ser adaptable a diferents traject`ories, situacions i missions, reduint l’esfor¸c realitzat en l’ajust i la parametritzaci´o dels m`etodes proposats. La plataforma experimental, formada pel cuadric`opter Asctec Hummingbird, s’estudia i es descriu en detall. S’obt´e un model matem`atic complet de la plataforma i es desenvolupa una eina de simulaci´o, la qual ´es de codi lliure. A m´es, es dissenya un controlador autopilot i s’implementa en la plataforma real. La part de control est`a enfocada al problema de path following. En aquest problema, el vehicle ha de seguir una traject`oria predefinida en l’espai sense cap tipus de restricci´o temporal. S’estudien, s’implementen i es comparen diversos algoritmes de control i geom`etrics de path following. Despr´es, es milloren els algoritmes geom`etrics usant xarxes neuronals per convertirlos en algoritmes adaptatius. Per finalitzar, es desenvolupa un m`etode de path following basat en t`ecniques d’aprenentatge per refor¸c profund (deep Reinforcement learning). Aquest m`etode implementa l’algoritme Deep Deterministic Policy Gradient. L’agent intel · ligent resultant ´es entrenat en un simulador realista de multirotors i validat en la plataforma experimental real amb `exit. Els resultats mostren que l’agent ´es capa¸c de seguir de forma precisa la traject`oria de refer`encia adaptant la velocitat del vehicle segons la curvatura del recorregut. A la part de navegaci´o, s’implementa un sistema de detecci´o d’obstacles basat en l’´us d’un sensor LIDAR. Es deriva un model del sensor i aquest s’inclou en el simulador. A m´es, es desenvolupa un m`etode per tractar les mesures del sensor per eliminar les possibles deteccions del terra. Pel que fa a la part de guiatge, aquesta est`a focalitzada en el problema de reactive path planning. ´ Es a dir, un algoritme de planificaci´o de traject`oria que ´es capa¸c de re-planejar el recorregut del vehicle a l’instant si algun esdeveniment inesperat ocorre, com ho ´es la detecci´o d’un obstacle en el recorregut del vehicle. Es desenvolupa un m`etode basat en aprenentatge per refor¸c profund per l’evasi´o d’obstacles. Aquest m`etode implementa l’algoritme Deep Deterministic Policy Gradient. L’agent d’aprenentatge per refor¸c s’entrena i valida en un simulador de multirotors realista. Aquest agent es combina amb l’agent de path following i la resta d’elements desenvolupats en vii List of Figures 2.1 Guidance, Navigation and Control structure. .............. 8 2.2 Separated Guidance and Control structure. ............... 10 2.3 Integrated Guidance and Control structure. ............... 10 3.1 Asctec Hummingbird Quadrotor with the Odroid XU4Q on-board PC (center bottom of the UAV). ...................... 30 3.2 Scheme of the elements of the experimental platform (based on an image from wiki.asctec.de). ......................... 30 3.3 Odroid-XU4 computer (original from hardkernel.com). . . . . . . . . 32 3.4 Outdoors experimental platform. ..................... 33 3.5 States conventions and frames of references of the quadrotor model. 34 3.6 Normalized motor speed in a hover experiment. ............ 38 3.7 SGC structure for path following algorithms. .............. 39 3.8 Wind disturbance generated with a random walk. ........... 41 3.9 NLGL: Obtaining the VTP for a straight line path. . . . . . . . . . . 42 3.10 Carrot-Chasing: Obtaining the VTP for a straight line path. . . . . 43 3.11 Benchmark simulink blocks. ........................ 44 3.12 User interface. ................................. 44 3.13 Results performing a 10 degrees step during 0.5 seconds of pitch: Experimental (red) and simulation (blue). ................ 46 3.14 Results performing a 30 degrees step of yaw: Experimental (red) and simulation (blue). ............................... 46 3.15 Trajectory on the xy plane of NLGL algorithm: Benchmark simulation with wind (blue) and experimental (red) (Eight-shape path reference (black), Vref = 1m /s). ...................... 46 3.16 Scheme with nodes and topics programmed in the RotorS environment. 49 4.1 General derivative estimation diagram .................. 57 4.2 Backstepping control structure. ...................... 58 4.3 Feedback Linearization control structure. ................ 64 4.4 3D trajectory for one lap of the helix in steady state regime: 3D- NLGL algorithm. ............................... 71 4.5 Simulation results varying the initial xposition from 0.5 to 6 meters: Backstepping (red), Feedback Linearisation (blue), 3D-NLGL (purple) and 3D Carrot-Chasing (green). ................ 73 xv xvi LIST OF FIGURES 4.6 3D trajectory comparing the four algorithms for one lap of the helix. Initial position: x=0.5, y =0, z =3. .................... 74 4.7 3D trajectory evolution comparing the four algorithms for one lap of the helix. Initial position: x=6, y =0, z =3. ............... 74 4.8 Simulation results varying the initial yaw angle from 0 to 180 degrees: Backstepping (red), Feedback Linearisation (blue), 3D-NLGL (purple) and 3D Carrot-Chasing (green). ................ 75 4.9 3D trajectory evolution comparing the four algorithms for one lap of the helix. Initial position: x=3, y =0, z =3.Initial yaw = 0○.. . . . 76 4.10 3D trajectory evolution comparing the four algorithms for one lap of the helix. Initial position: x=3, y =0, z =3.Initial yaw = 180○.. . . 76 4.11 Path distance error against wind disturbances. ............. 77 4.12 Realistic wind disturbances. ........................ 79 4.13 3D trajectory evolution comparing Backstepping and 3D-Carrot-Chasing for one lap of the Lemniscate under realistic flight conditions (2 m /s). 80 4.14 Simulation results varying the velocity reference from 1 to 4 m /sunder realistic flight conditions: Backstepping (red) and 3D Carrot-Chasing (green). ..................................... 81 5.1 MAE of don a full lap of a circular path varying L. Constant speed: 1m /s. Different radius: 1.5m, 3m and 20m. ................ 86 5.2 Lopt in function of the path radius and vehicle’s velocity. ....... 87 5.3 MAE of dwith Lopt in function of the path radius and vehicle’s velocity. 88 5.4 Neural Network surface and its training points (NLGL). ....... 89 5.5 Simulation scenario to obtain the optimal anticipation distance (da ∗). 90 5.6 Optimal anticipation distance in function of the vehicle’s velocity and radius of the curve. ........................... 91 5.7 Optimal anticipation distance can not be used to obtain the path radius (input of the NN). .......................... 91 5.8 Anticipation distance window to obtain the path radius (input of the NN). ...................................... 92 5.9 Optimal L(blue), stability condition limit for L(purple) and stability limit in simulation (red) in function of the path radius (u= 1m /s). ...................................... 94 5.10 Trajectory on the xy plane of NLGL (blue) and Adaptive NLGL with velocity reduction (red) (Lemniscate path, Vref = 1m /s). . . . . . . . 95 5.11 Evolution of Lwith the Adaptive NLGL (green) and Adaptive NLGL with velocity reduction (red) (Lemniscate path, Vref = 1m /s). . . . . 96 5.12 Trajectory of NLGL (blue) and Adaptive NLGL with velocity reduction (red) (Spiral path, Vref = 2m /s). ................... 97 5.13 Evolution of Lwith the Adaptive NLGL (green) and Adaptive NLGL with velocity reduction (red) (Spiral path, Vref = 2m /s). ....... 98 5.14 δopt in function of the path radius and vehicle’s velocity. ....... 99 5.15 MAE of dwith δopt in function of the path radius and vehicle’s velocity. 99 LIST OF FIGURES xvii 5.16 Neural Network surface and its training points (Carrot-Chasing). . . 100 5.17 Trajectory on the xy plane of Adaptive NLGL (blue) and Adaptive CC (red) in realistic flight conditions (Lemniscate path, Vref = 1m /s).101 5.18 Trajectory on the xy plane of Adaptive NLGL (blue) and Adaptive CC (red) in realistic flight conditions (Spiral path, Vref = 2m /s). . . 102 6.1 Reinforcement learning structure adapted to the SGC structure. . . 104 6.2 Actor-Critic agent structure. ........................ 105 6.3 Elements of the transition tuple. ...................... 106 6.4 GUI of RotorS simulator with Hummingbird model. . . . . . . . . . . 107 6.5 Scheme of the training environment. ................... 108 6.6 States of the agent are with respect of the tangential frame {T}.. . 110 6.7 Actor NN structure: 2 feed-forward hidden layers. . . . . . . . . . . . 111 6.8 Critic NN structure: 2 feed-forward hidden layers. . . . . . . . . . . . 111 6.9 States of the second approach; angle error with respect forward tangential frame {T2}............................... 112 6.10 Average distance error and accumulated reward on each episode during training phase of 1st approach agent (2 states). . . . . . . . . . . 115 6.11 Average distance error and accumulated reward on each episode during training phase of 2nd approach agent (3 states). . . . . . . . . . . 116 6.12 Average distance error and accumulated reward on each episode during training phase with non-ideal initial conditions of 2nd approach agent; gray dashed lines are real values and black lines are a 20- episodes moving average. .......................... 117 6.13 Average distance error, average velocity and accumulated reward on each episode during training phase of 3rd approach agent; gray dashed lines are real values and black lines are a 50-episodes moving average. .................................... 118 6.14 Trajectories on xy of lemniscate path, simulation with sensor models: Agent 1 in green (dotted line), Agent 2 in blue (dash-dotted line) and Agent 3 (dashed line) in red. ..................... 120 6.15 Actions of Agent 3 following a lemniscate path, simulations with sensor models: references computed by agent (angle and velocity) in red and real values in blue (dashed line). ................. 121 6.16 Trajectories on xy of spiral path, simulations with sensor models: Agent 1 in green (dotted line), Agent 2 in blue (dash-dotted line) and Agent 3 (dashed line) in red. ..................... 122 6.17 Actions of Agent 3 following a spiral path, simulations with sensor models: references computed by agent (angle and velocity) in red and real values in blue (dashed line). ................... 122 6.18 Trajectories on xy of lemniscate path, experimental results: Agent 1 in green (dotted line), Agent 2 in blue (dash-dotted line) and Agent 3(dashed line) in red. ............................ 125 xviii LIST OF FIGURES 6.19 Actions of Agent 3 following a lemniscate path, experimental results: references computed by agent (angle and velocity) in red and real values in blue (dashed line). ........................ 125 6.20 Trajectories on xy of spiral path, experimental results: DDPG v1 in green (dotted line), DDPG v2 in blue (dash-dotted line) and Agent 3(dashed line) in red. ............................ 126 6.21 Actions of Agent 3 following a spiral path, experimental results: references computed by agent (angle and velocity) in red and real values in blue (dashed line). ............................ 127 7.1 Examples of LIDAR sensors for UAVs (images obtained from the company’s website of each sensor). .................... 133 7.2 Leddar VU8 (original image from leddartech.com). . . . . . . . . . . . 135 7.3 Leddar VU8 detects obstacles in 8 segments (original image from leddartech.com). ............................... 136 7.4 Leddar VU8 data represented in RVIZ. .................. 136 7.5 Leddar VU8 data represented in RVIZ. .................. 137 7.6 Capture of Gazebo including the Hummingbird quadrotor with the LIDAR sensor and an obstacle. ...................... 138 7.7 Distance from LIDAR to ground depending on pitch angle. . . . . . 139 7.8 LIDAR ground detections generated by pitch angle: xy-body plane. 139 7.9 Distance from LIDAR to ground depending on roll angle. . . . . . . . 140 7.10 LIDAR ground detections generated by pitch and roll angles: xybody plane. .................................. 141 7.11 Trigonometrical problem to obtain ground detections (dG,i). . . . . . 141 8.1 Reinforcement learning structure employed in this work. . . . . . . . 145 8.2 Action of the agent modifies the path offseet doff .. . . . . . . . . . . 147 8.3 Vehicle crashes if only instantaneous information of LIDAR is used. 149 8.4 Integral LIDAR state, nL, while performing an avoidance manoeuvre (kc=1, kd=0.1 and nL,max =6. ...................... 150 8.5 Obstacle surrounded by the prohibited zone and the safety zone. . . 152 8.6 Comparing two forms of calculating the LIDAR-based reward term: inverse function and sigmoidal function. ................. 155 8.7 Circular distance threshold or elliptical threshold (dT,r). . . . . . . . 155 8.8 Flowchart of the two scripts that implement the PF and Reactive OA approach. ................................. 157 8.9 Average distance error, average velocity, probability of collision and accumulated reward on each episode during training phase of the OA agent with the standard reward; gray dashed lines are real values and black lines are a 100-episodes moving average. . . . . . . . . . . . . . 160 LIST OF FIGURES xix 8.10 Average distance error, average velocity, probability of collision and accumulated reward on each episode during training phase of the OA agent with the LIDAR-based reward; gray dashed lines are real values and black lines are a 100-episodes moving average. . . . . . . . 161 8.11 Trajectory on the xy plane of the agent with the standard reward (red dashed line) and the agent including the LIDAR-based reward (dash-dotted green line) while following a lemniscate path with an obstacle (blue circular object). ....................... 163 8.12 Simulation results following the lemniscate path with obstacles centred on the path. ............................... 165 8.13 Simulation results following the lemniscate path with obstacles not centred on the path. ............................. 167 8.14 Trajectory on the xy plane of a simulation where the agent avoids the obstacle by the wrong side. ...................... 168 8.15 Trajectory on the xy plane of a simulation that ended in a collision. 168 8.16 Simulation results following the spiral path with different obstacle positions. .................................... 170 List of Tables 2.1 Comparison of the PF techniques. ............................. 18 3.1 Parameters of the X-BL-52S motor. ............................ 31 3.2 Parameters of the model. .................................. 38 3.3 Constants of the PID controllers of the autopilot. ................... 39 3.4 Results for one lap of the Lemniscate with NLGL: Experimental and Simulation. 47 4.1 Control parameters for each algorithm. .......................... 69 4.2 Results for one lap in steady state regime. ........................ 70 4.3 Results for 100 seconds in time-varying wind conditions. ............... 78 4.4 Results for one lap under realistic flight conditions (2 m /s). .............. 79 4.5 Qualitative comparison of the path following algorithms. ............... 82 5.1 Results for one lap of the lemniscate path (Vref =1m /s). ............... 96 5.2 Results for one lap on the spiral path (Vref =2m /s). .................. 97 5.3 Results for one lap on the lemniscate path in realistic flight conditions (Vref =1m /s).101 5.4 Results for one lap on the spiral path in realistic flight conditions (Vref =2m /s). . 102 6.1 Parameters of the DDPG agent. .............................. 111 6.2 Results for one lap of the lemniscate path, simulations with ground truth measurements. ........................................... 119 6.3 Results for one lap on the lemniscate path, simulations with sensor models. . . . 120 6.4 Results for one lap of the spiral path, simulations with ground truth measurements.121 6.5 Results for one lap on the spiral path, simulations with sensor models. . . . . . . 121 6.6 Experimental results for one lap on the lemniscate path. . . . . . . . . . . . . . . . 124 6.7 Experimental results for one lap on the spiral path. .................. 126 7.1 Comparison of the most common LIDAR sensors for UAVs. . . . . . . . . . . . . . 134 7.2 Characteristics of the Leddar VU8 Medium FOV. ................... 135 8.1 Modified noise parameters of the OA agent. ....................... 156 8.2 Behaviour of some of the trained and tested agents. .................. 162 8.3 Simulation results for one lap on the lemniscate path with no obstacles. . . . . . . 163 8.4 Simulation results for one lap on the lemniscate path with obstacle centred on the path. .............................................. 164 8.5 Simulation results for one lap on the lemniscate path with obstacles not centred on the path. .......................................... 166 xxi xxii LIST OF TABLES 8.6 Simulation results for one lap on the spiral path with no obstacles. . . . . . . . . . 168 8.7 Simulation results for one lap on the spiral path with different obstacle positions. 169 Acronyms ADRC Active Disturbance Rejection Control APF Artificial Potential Field BN Batch Normalisation BS Backstepping CC Carrot-Chasing CCN Convolutional Neural Networks DAPF Dynamic Artificial Potential Field DDPG Deep Deterministic Policy Gradient DF Differential Flatness DQN Deep Q-Network DPG Deterministic Policy Gradient DRL Deep Reinforcement Learning EKF Extended Kalman Filter ESC Electronic Speed Controller FL Feedback Linearisation FOV Field-Of-View GNC Guidance, Navigation and Control GPS Global Positioning System HOSM High-Order Sliding Mode HSA Heuristic Search Algorithm IGC Integrated Guidance and Control IMU Inertial Measurement Unit xxiii Chapter 1 Introduction 1.1 Motivation In recent years, the growing interest to develop fully autonomous aerial vehicles, known as Unmanned Aerial Vehicles (UAVs), has seen a civil market demand increase compared to its military applications. The increasing number of tasks and applications that UAVs can perform is well known. The interest in automating these applications drives a continuous progress both in the artificial intelligence and control areas. Out of all UAVs, multirotors stand out for their good manoeuvrability and mechanical simplicity. Throughout the education of the thesis’ author, focused on electronic engineering and automation, the research on this type of vehicles has become one of his main interests. This platform permits to study, develop and implement approaches on diverse areas such as control, machine learning, artificial vision or state estimation, which are aligned with the author’s background. While studying physics, automatic control or artificial intelligence subjects, the UAV has always been a baseline system to study and apply the author’s acquired knowledge. His final master’s project was focused on the modelling and control of a coaxial helicopter UAV. Furthermore, the radio controlled aerial vehicles have for long been one of the major author’s passions, having expertise on piloting multirotors and small helicopters, as well as a wide knowledge on their mechanical and electronic parts. The Research Center for Supervision, Safety and Automatic Control (RS2AC), the research group were this thesis was carried on, has a large expertise on the research on UAVs and other type of autonomous vehicles. Furthermore, the group has different indoor and outdoor UAV experimental platforms that become a great opportunity for implementing and testing the work developed in this thesis. Moreover, the group is involved in projects in which the research focus is put on security and control of autonomous vehicles. The motivation of this thesis emerges from both the interest of the group on the UAV research and its practical implementation as well as the author’s interests on this type of vehicles and in the research and implementation of novel control and machine learning algorithms. 1 2CHAPTER 1. INTRODUCTION 1.2 Thesis objectives The principal objective of this PhD thesis is to study, develop and apply innovative control techniques and machine learning algorithms to implement a Guidance, Navigation and Control system for a multirotor vehicle. This system must be able to make the vehicle to follow predefined paths while avoiding obstacles in the vehicle’s route. The aim of the thesis is to obtain a highly autonomous system that can be adapted to different paths, situations and missions, reducing the tuning effort and parametrisation of the proposed approaches. Secondary objectives involved in the generation of the Guidance, Navigation and Control system are: ●Control Structure: Define a structure of the Guidance, Navigation and Control system that includes the required modules and their information flows for addressing the challenges derived from the defined problem. ●Path Following: Study the path following problem of a multirotor vehicle. Implement and compare state-of-the-art path following algorithms for multirotor vehicles and/or adapt algorithms applied to other vehicles. Improve the algorithms or develop an approach that will be proposed as solution. The proposed approach must be straightforwardly adaptable to different reference paths without requiring any additional tuning of the algorithm parameters. Moreover, it must dynamically adapt the velocity of the vehicle to the shape of the path, anticipating the curves to come. ●Path Planning & Obstacle Avoidance: Study the problem of planning a route online to follow a predefined path while avoiding possible obstacles and propose a reactive path planning approach. The proposed approach must integrate the information of the state estimator, the perception systems and the given reference path to generate the reference route. Also, it must be capable of replanning the route online when an unexpected event, such as the detection of an obstacle, occurs. This approach must be integrated with the path following algorithm. ●Obstacle Detection: Implement a perception system capable to detect obstacles in the vehicle’s route. This system must be installed in the real platform and the perceived information must be treated and adapted to send it to the reactive path planning algorithm. ●Real implementation: Implement and test in the real experimental platform the Guidance, Navigation and Control System that integrates the work and contributions undertaken on each area. This includes the setup of the experimental platform, the implementation of inner controllers and the treatment of the sensor measurements to estimate the required states. 1.3 Thesis Outline This thesis is organised as follows: 1.3. THESIS OUTLINE 3 ●Chapter 2: Guidance, Navigation and Control System This chapter proposes the control structure for the Guidance, Navigation and Control system that is developed in this thesis. The elements of this structure and their communication flow are described in detail. Next, a thorough literature review of the control, navigation and control fields for multirotor vehicles is presented. This literature review is focused on the problems that are studied in this thesis. These are the path following problem, the obstacle detection problem and the reactive path planning problem. ●Chapter 3: Multirotor Environment This chapter is focused on the multirotor platform. The Asctec Hummingbird quadrotor and the rest of the elements that form the experimental platform are carefully described. Next, a complete and realistic mathematical model of the real multirotor platform is derived. An autopilot controller is designed and implemented in the actual platform. Path-Flyer, a freely available and open simulation benchmark for testing path following algorithms on a quadrotor vehicle, is developed in this chapter. Experimental tests are compared with the simulation results of Path-Flyer to prove its validity. To end up, the RotorS/Gazebo platform is introduced and the modifications made to this platform to emulate the real experimental one are explained in detail. ●Chapter 4: Control-oriented and Geometric Path Following In this chapter, four of the most relevant path folowing algorithms reviewed in Chap. (2) are implemented with the quadrotor model described in Chap. (3). These are, Backstepping and Feedback Linearisation control-oriented algorithms and Nonlinear Guidance Law and Carrot-Chasing geometric algorithms. The geometric algorithms are adapted to the three-dimensional space. A complete comparison and discussion of these four path following algorithms is derived from the simulation results. Finally, a comparison including qualitative and quantitative indicators is provided. ●Chapter 5: Adaptive Geometric Path Following The geometrical algorithms implemented in Chap. (4) (Nonlinear Guidance Law and Carrot-Chasing) are simple, effective and have only one tunning parameter. However, their control parameter depends on various factors, such as the velocity of the vehicle, the shape of the reference path and the dynamics of the vehicle. This chapter analyses the effect of the control parameter of these algorithms on their performance. Next, an adaptive version of both algorithms based on the use of Neural Networks is proposed. The proposed approaches include a velocity reduction term. Stability proofs are also given. Simulation results show that the proposed approaches improve the performance of the standard geometric algorithms. Furthermore, they have no parameters to tune. ●Chapter 6: Path Following with Deep Reinforcement Learning This chapter proposes a solution for the path following problem of a quadrotor vehicle based on deep reinforcement learning theory. Three different approaches implementing the Deep Deterministic Policy Gradient algorithm are presented. Each approach emerges as an improved version of the preceding one. The first approach uses only instantaneous 4CHAPTER 1. INTRODUCTION information of the path for solving the problem. The second approach includes a structure that allows the agent to anticipate to the curves. The third agent is capable to compute the optimal velocity according to the path’s shape. A training framework that combines the tensorflow-python environment with Gazebo- ROS using the RotorS simulator is built. The three agents are tested in RotorS and experimentally with the Asctec Hummingbird quadrotor. Experimental results prove the validity of the agents, which are able to achieve a generalized solution for the path following problem. ●Chapter 7: Obstacle Detection In this chapter, a study of obstacle detection solutions based on different sensors, such as cameras, LIDARs, RADARs or ultrasound sensors, is carried out. From the reviewed solutions, a LIDAR-based system is chosen to be implemented in the experimental platform. A model of the LIDAR sensor is developed in the RotorS environment. Then, an algorithm for processing the sensor measures to eliminate the possible ground detections is derived. ●Chapter 8: Obstacle Avoidance with Deep Reinforcement Learning A deep reinforcement learning approach for solving the obstacle avoidance problem is proposed in this chapter. This approach implements the Deep Deterministic Policy Gradient algorithm. It uses the LIDAR processed information to detect obstacles around the vehicle. If an obstacle is detected in the vehicle’s route, the agent modifies the reference path to avoid it. The developed agent communicates with the path following agent by sending it the modified reference path. A detailed description of the process of defining the state vector, the reward function and the action of the agent is given. Different solutions are obtained and compared. The agents are trained and tested in the RotorS/gazebo platform. Simulations results prove the validity of the proposed approach. ●Chapter 9: Conclusions This chapter summarizes the contributions of the thesis, gives conclusions and describes the next steps. Part I Background 5 Chapter 2 Guidance, Navigation and Control System The main topic of this PhD thesis is the multirotor Guidance, Navigation and Control (GNC) systems. Hence, before presenting the methodologies, strategies and contributions related to these systems, it is important to understand what is a Guidance, Navigation and Control system and which are its main components. Moreover, it is necessary to elaborate an exhaustive literature review on this area. These concerns are covered in this chapter. 2.1 Control Structure One of the most important issues when designing or implementing a Guidance, Navigation and Control system is to define a proper control structure. The control structure defines the main elements of the system, and how they are connected with each other. This structure will depend on the type of vehicle that we are using and also in the problem that is going to be solved. The structure of the Guidance, Navigation and Control system proposed here is presented in Fig. 2.1. This is a general and typical structure for a GNC system [90][88]. However, it includes various blocks that are specific for the problem considered in this PhD thesis, as is the case of the Obstacle Detection block. The introduction of this structure will be helpful to understand the rest of the literature review presented in this chapter. The main element of the structure of Fig. 2.1 is the Unmanned Aerial System (UAS). The UAS is composed of the Unmanned Aerial Vehicle (UAV), the actuators of the vehicle and the set of sensors. The description of the UAV platform employed in this thesis (i.e. the multirotor vehicle, sensors, actuators, on-board PCs, software, etc.) as well as the mathematical model of the system is found in Chap. (3). The Control block is responsible for the stabilization of the vehicle (Autopilot) and for making the vehicle follow a desired given trajectory (Path Following). These two elements receive the 7 8CHAPTER 2. GUIDANCE, NAVIGATION AND CONTROL SYSTEM UAV ActuatorsSensors Autopilot Path Following State Estimator Target Detection Obstacle Detection Trajectory Generation Path Planning Obstacle Avoidance GUIDANCE NAVIGATION CONTROL PERCEPTION UAS Map Database Figure 2.1: Guidance, Navigation and Control structure. state information from the state estimation block and they end up controlling the UAV by sending commands to the actuators of the vehicle. It is important to mention that the autopilot is not always necessary, since some path following approaches deal with the stabilization of the vehicle too. The Navigation block includes tasks that process the information from the sensors. Those tasks depend on the specific navigation problem being solved. It can be divided in two parts; the one dedicated to obtain the state of the vehicle (State Estimator) and the perception block that acquires information of the environment. In the perception block we include a task dedicated to the localization or detection the target (Target Detection) and a task dedicated to detect any obstacle in the vehicle’s route (Obstacle Detection). The Guidance block is the responsible of determining the route that the vehicle has to complete to follow a path, to follow a target or to get to a desired position. That is, to generate the reference trajectory that receives the Control block. In this particular case, the Path Planning task takes into account the location of the path and the state of the vehicle, both received from 2.2. CONTROL 9 the Navigation block, to design the best route to follow the path. Furthermore, it includes an Obstacle Avoidance task, that re-plans a new route when an obstacle in the vehicle’s trajectory is detected. The Map Database gives a priori information of the path and environment, such as path point coordinates or localization of obstacles, to the path planner. The Trajectory Generation task is an optional block that is sometimes needed to adapt the reference trajectory generated by the path planner to the control block requirements. That is, to modify the trajectory, such as decreasing the vehicle’s velocity or smoothing the path curves [82][35], to fulfil the control or vehicle constrains. This block is also responsible of generating additional trajectory parameters, if they are required by the trajectory controller. The blocks marked in black in Fig. 2.1 denote the elements that are studied in the PhD thesis. They include tasks where contributions are produced (i.e. Path Following and Path Planning & Obstacle Avoidance) and other tasks where state-of-the-art solutions are implemented (i.e. Autopilot and Obstacle Detection). The UAS is also included in this list since it was necessary to perform the modelling, identification, simulation and experimental set-up of the system. The literature of the three main blocks of the GNC system is reviewed in detail in the rest of this chapter. 2.2 Control The control block includes the stabilization of the system and the trajectory control. The autopilot is in charge of stabilizing the attitude of the multirotor, the fastest and most influential dynamics. The stabilization control problem for the particular case of a quadrotor has been solved using different techniques such as Backstepping,Feedback Linearisation,Sliding Mode Control,PID, optimal control, robust control, learning-based control, etc [13][131][101]. Since the stabilization control has already been widely studied, it is not studied in this thesis. However, a simple and effective solution for the autopilot is implemented in Section 3.3. The trajectory control problem, defined as making a vehicle follow a pre-established path in space, can be solved mainly by two different approaches: using a trajectory tracking controller or with a path following controller. For the trajectory tracking problem a reference specified in time is tracked, where the references of the path are given by a temporal evolution of each space coordinate. Whereas path following (PF) handles the problem of following a path with no preassigned timing information, thus any time dependence of the problem is removed. In [2] the authors demonstrate that following a geometric path is less demanding than tracking a timed reference signal. They argue that, although it is possible to perfectly track any reference with minimum phase stable systems, the tracking error increases in non-linear systems with presence of unstable zero dynamics as the signal frequencies approach those of the unstable zeros. PF controllers offer a number of advantages over trajectory tracking controllers, not only these are easier to design [84] but also result in smoother convergence to the path and less demand on the control effort [27], a smaller transient error and a stronger robustness [171] and the control signals are less likely to be saturated [39]. 16 CHAPTER 2. GUIDANCE, NAVIGATION AND CONTROL SYSTEM 2.2.7 Learning-based Learning-based algorithms constitute an emerging field that, due to the significant progress made in recent years, has become a wise solution for different types of problems. In particular, it is being introduced in the trajectory control problem on different types of vehicles, including UAVs. This field includes supervised and unsupervised techniques, reinforcement learning algorithms, data-driven approaches and other deep machine learning methods. In [169] a MPC controller is used as a supervisor to train a neural network control policy to solve the path following and obstacle avoidance problem on a quadrotor vehicle. With this algorithm the computational efficiency problem of the MPC technique is solved while maintaining a similar performance, as proved by experimental results. An Iterative Learning Control approach is proposed in [197] to solve the PF problem on a quadrotor. The control scheme is composed by a PD controller and an feed-forward controller. This approach focuses on repetitive flight to learn from experience. Simulation tests and off-line experimental results are presented to prove the effectiveness of the controller. In [195] a data-driven control approach is proposed for the adaptive path following control of a fixed-wing vehicle. The reliability of the approach is demonstrated through simulation results and flight tests. Other learning-based approaches have been used to solve the path following problem on different types of vehicles, such as vessels [62][114][160] or airships [128]. Various learning-based approaches are found in the literature solving the trajectory tracking problem for a quadrotor vehicle [178][38]. Some of these approaches consider wind disturbances in its design, as in [104] where an adaptive trajectory tracking control based on a reinforcement learning algorithm is presented. 2.2.8 Optimal Control The Optimal Control theory aims to operate a dynamic system at a minimum cost. That is, to follow a path with a minimum error and control effort. The most well-known control techniques to solve this problem are the Linear Quadratic Regulator (LQR) and the Linear Quadratic Gaussian (LQG). In [179] a general solution for the PF problem is presented. The proposed approach is developed as a fixed end-time optimal control problem and it relies on a geometric formulation based on the notion of differential flatness. The resulting controller is applied to a quadrotor simulation system with the mission of performing aggressive manoeuvres and it is demonstrated that the proposed problem formulation is solved efficiently. A UAV guidance law using an adaptive LQR formulation is addressed in [95]. The LQR is optimized using a genetic algorithm for tighter control of UAV errors in high disturbances. Simulations for straight line and loiter paths under various wind conditions prove the effectiveness of the approach. Experimental results applying optimal control theory to solve the trajectory tracking problem on a quadrotor vehicle can be found in [69]. In this paper, the authors propose a solution based on 2.2. CONTROL 17 the definition of a path-dependent error space to express the dynamic model of the vehicle. The controller is designed using LQR state space feedback and adopts the D-methodology integrated with the anti-windup technique in order to achieve zero static error for the integral states and avoid actuator saturation. 2.2.9 Sliding Mode Control Sliding Mode Control (SMC) is a non-linear control method that attains the control objectives by constraining the system dynamics to a pre-defined surface by means of a discontinuous control law. This control technique is considered to be effective and robust [48]. However, it can present implementation issues due to the chattering effect. The bibliographic search revealed no SMC application solving the path following problem for a quadrotor vehicle. However, it has been applied to solve the PF problem on other UAVs, such as fixed-wing vehicles. In [159] a lateral guidance law for cross-track control based on the SMC technique is developed. This guidance law includes a feed-forward component related to the rate of change of the desired heading angle, which permits to improve the performance and achieve accurate tracking while following curved paths. The proposed guidance scheme is evaluated based on experimental flight results. According to the authors, it presents a good performance in the presence of wind and parametric uncertainty. In [9] the controller presented in [159] is improved by implementing a Partially-IGC strategy that includes a sliding mode control on the inner and the outer loops using a non-linear sliding surface based on Second Order Sliding Mode (SOSM ) control theory. Experimental tests compare the conventional SGC approach and the proposed Partially-IGC approach to show that the second one presents a faster convergence of the cross-track error toward zero. In [190] the Second Order Sliding structure is used to develop a PF application. The proposed controller provides smooth bank and turn coupled motions. To estimate the uncertain sliding surfaces a High-Order Sliding Mode (HOSM ) differentiator is applied. The sliding surface structure is based on the Pure Pursuit algorithm through a set of intermediate control variables and also introducing a virtual target point in the path. This approach eliminates time-consuming and intensive computation. According to the authors, simulations show that it provides an excellent performance even under wind turbulence conditions. Regarding the trajectory tracking problem, numerous SMC quadrotor implementations are found [196][20]. Some of these approaches are able to deal with wind disturbances [49][182]. 2.2.10 Comparison Refer to Table 2.1 for a comparison of the reviewed PF algorithms. The characteristics of these algorithms are evaluated only in the context of the path following problem applied to UAVs and are based on the reviewed literature. The columns refer respectively to: the control structure (i.e. Integrated Guidance and Control or Separated Guidance and control); the type of results (experimental or simulation); the application to quadrotors; good experimental results against 18 CHAPTER 2. GUIDANCE, NAVIGATION AND CONTROL SYSTEM external disturbances; including a Fault Tolerant Control (FTC) strategy; and implementing an adaptive approach. Table 2.1: Comparison of the PF techniques. Structure Results Quadrotor Wind dist. FTC Adaptive Backstepping IGC Experimental Yes Yes No No Lyapunov-based SGC and IGC Experimental Yes Yes No Yes Feedback Linearization SGC and IGC Simulation Yes No Yes No Geometric SGC Experimental Yes No No No Model Predictive Control SGC Experimental Yes Yes No Yes Vector Field SGC Experimental Yes Yes No Yes Learning-based SGC Experimental Yes Yes No Yes Optimal Control SGC and IGC Simulation No Yes No Yes Sliding Mode Control SGC and IGC Experimental No Yes No No 2.3 Navigation Navigation can be defined as the process of data acquisition, data analysis and extraction of information about the vehicle’s state and its surrounding environment with the objective of accomplishing assigned missions successfully and safely [88]. This information is obtained by means of the sensors of the Unmanned Aerial System. The most common sensors for UAS are the accelerometers, gyroscopes, magnetometers, pressure sensors, GPS, cameras and LIDARs. The raw measurements provided by these sensors are used to perform the state estimation and for the perception algorithms. In this thesis the navigation is mainly focused on the detection of an obstacle. There exist different forms of performing this task in addition to the pure obstacle avoidance approaches, as for instance with a Simultaneous Mapping And Planning method. The literature of these perception algorithms are reviewed in this section. Furthermore, state estimation methodologies and other important perception tasks are reviewed. 2.3.1 State Estimation The state estimation concerns mainly the processing of raw sensor measurements to estimate variables that are related to the vehicle’s state, such as attitude, position and velocity [88]. The state estimation algorithms usually fuse information from many sensors to estimate this state. The most common approach for fusing sensor data to estimate the state is the extended Kalman filter (EKF) [155][72]. That is, using one filter with all the state variables that need to be estimated. Sometimes, the state estimator is composed by two cascaded EKFs: one for attitude 2.3. NAVIGATION 19 estimation using Inertial Measurement Unit (IMU) raw data, and the other for position and velocity estimation using GPS raw measurements and translational accelerations. Another widely used method that provides good results with the estimation of the attitude on UAVs is the complementary filter [111][110][52]. This method is only applicable to the measurements of accelerometers, gyroscopes and magnetometers of an IMU sensor. The measurements of these sensors are combined to obtain an estimation of the orientation of the vehicle represented in quaternions. The GPS systems depend on the access to the signals from satellites. Since satellites are not always available, as in the case of indoor and urban environments, other approaches to solve the state estimation problem are needed. The ranging systems, like infrared [23] or ultrasonic [162] sensors, are used to aid an state estimator for the stabilization of a vehicle relative to the walls in an indoor environment. As the outdoor UAS generally fly in relatively large open spaces, there is not sufficient environmental structure for the relative position estimation using this ranging systems. In outdoor environments where the GPS signal are not available, the vision-based state estimators become a great solution. In the on-ground vision, cameras are placed externally to the vehicle and are used to track it and to estimate its attitude and/or position. For example, [74] and [123] use the VICON system in their research on flight control and for cooperative path following. Visual odometry [12] consists on an incremental method that analyses a sequence of images to estimate changes in position and/or orientation over time. In the target relative navigation [78] [107] the position of the vehicle relative to a specific detected target is estimated. The terrain relative navigation [41] estimates the vehicle’s position, and sometimes the velocity, by comparing terrain measurements from on-board cameras with a terrain map. 2.3.2 Perception Perception is the ability to use measurements from sensors to build an internal model of the environment and to generate events of situations perceived in the environment [88]. The recognition process involves comparing what is observed with the UAS a priori knowledge [80]. The information is usually obtained by vision systems (i.e. with cameras) and by LIDAR sensors. Typically, the information obtained by the perception algorithms is not directly used as measurements in the flight controller, but as inputs to higher-level guidance systems. The perception block can serve to several functions such as target detection, obstacle detection or mapping. These functions are covered in next subsections, and a literature review on both vision-based and LIDAR-based approaches is given. Target Detection Most of these perception algorithms are based on vision systems. These algorithms are similar to the target relative navigation mentioned in the state estimation section. The main difference is that here the information is used for guidance and the trajectory control relies on another 20 CHAPTER 2. GUIDANCE, NAVIGATION AND CONTROL SYSTEM state estimator algorithm, while in the target relative navigation the visual estimates are used to control the vehicle. One of the applications of target detection is the automatic landing. For example, in [156] a vision algorithm is used to detect and recognize from a monocular camera a helipad for landing a helicopter. In [71] a similar approach is presented to land a UAV on a stationary target using vision and GPS. The horizontal target approach is also a typical application of the target detection. That is, to approximate to a frontal target and to hover at some distance from it. A vision-based velocity controller for frontal target tracking is presented in [117]. The vision algorithm detects and tracks building windows. It detects the target by using segmentation and square finding. The tracking is made with template-matching and a Kalman filter. Another application is the mobile ground target detection and tracking. In [180] a pursuit-evasion game with a helicopter and UGVs is implemented by means of a vision-based approach. The implemented vision system can actively track coloured objects. An indoor quadrotor application for cooperative vision-based tracking of ground vehicles is presented in [22]. An optimization technique and a Kalman filter are used by the vision tracking algorithm. Obstacle Detection This subsection introduces perception approaches for obstacle detection that do not perform a mapping of the environment. The approaches of obstacle detection that include maps are presented in the next subsection. Both vision-based and LIDAR-based systems are commonly used to detect obstacles. Computer vision can be applied to solve this problem by using optical flow, stereo vision systems or monocular cameras. The optical flow estimates the motion of the elements of an image. The stereo vision systems are commonly used to obtain the depth information. Different techniques can be used to detect the obstacle with monocular vision. These techniques include estimating the relative size or clarity of the obstacle, using a texture gradient, by means of interposition or by motion parallax [17]. Also, the obstacle can be detected from the known characteristics of an object (e.g. color or shape). A single camera obstacle detection algorithm based on the estimation of an optical flow probability distribution is presented in [98]. The resulting algorithm provides distance to the obstacles surrounding the UAV. This methodology was tested experimentally. In [79] the obstacle detection for the navigation of a UAV through urban canyons is solved by the use of an optic flow (from a pair of sideways-looking fish-eye cameras) and a stereo vision. The environment is represented with a 3D point cloud map, and obstacles are detected by using a distance threshold. A multi-obstacle detection algorithm based on stereo vision is presented in [189]. First, a depth map of the scene is obtained, and then, another algorithm is used to find out five dangerous objects and give them bounding boxes. The results show that the algorithm can detect at most five obstacles in 15m. In [15] the obstacle global position is estimated by tracking some coloured 2.3. NAVIGATION 21 flags on the obstacle and using a GPS and IMU for knowing the global position of the vehicle. Other interesting application with monocular vision systems that estimate the size expansion [6] or the relative direction of an obstacle [108], or the relative distance to an obstacle [152] can be found in the literature. Using LIDAR sensors for obstacle detection and avoidance is an interesting solution because these approaches are less computationally expensive than vision-based approaches, providing a fast reactive system that can prevent collisions in the last minute. In [119] the authors propose a system that uses immediate LIDAR measurements to detect the ground and frontal obstacles and computes a reactive action based on the current context. A collision avoidance approach for an hexacopter UAV with a mounted LIDAR is presented in [132]. A Kalman filter is used to estimate the position, velocity, and acceleration of the obstacle by using the data of the LIDAR as the associated measurement. The estimation of the obstacle state is used to predict the future trajectory of the moving obstacle. In [170] a simple obstacle detection and avoidance approach for a quadrotor UAV is developed by using the information of a rotating LIDAR sensor. The aim is to support the pilot during the manual flights avoiding possible collisions. The approach is assessed by experimental results. A 2D-LIDAR based obstacle detection method for a UAV is implemented in [198]. In this approach a velocity estimation method is used to estimate the position of the LIDAR as it scans each point and then corrects the twisted point cloud. The effectiveness of the methodology is proved by simulation and experimental results. RADAR sensors are also used to detect obstacles in UAV systems [96]. However, this sensors are very heavy, which limits their application to small multirotor vehicles. Applications using acoustic sensors (ultrasounds) can also be found in the literature [138]. The problem of this sensors is that they have a short operational range, which restricts their use to only indoor usage and low-speed objects. Other multi-sensor applications [138][54] use information from several sources to detect an obstacle. Mapping Mapping the environment consists of building some internal representation of the scene [88]. Mapping-based approaches allow the use of more sophisticated path planning and obstacle avoidance algorithms. The mapping perception systems can be classified in three categories: the simultaneous location and mapping (SLAM), the simultaneous mapping and planning (SMAP) and the safe landing area detection (SLAD). Simultaneous Localization And Mapping: Consists on building a map of an unknown environment and localizing the vehicle on the map at the same time. This map is usually represented by a set of features or a point cloud. Generally, SLAM approaches are not applied to the detection and avoidance of obstacles. In [16] the implementation of visual SLAM techniques to outdoors images taken from UAVs is presented and tested experimentally. An optic flow-based vision system for autonomous localization and scene mapping for a small and micro-UAVs is presented in [89]. 22 CHAPTER 2. GUIDANCE, NAVIGATION AND CONTROL SYSTEM SLAM LIDAR-based approaches can be also found in the literature. They have been generally developed for indoor navigation of small vehicles [61] [19]. However, in some cases LIDAR has also been used on bigger UAVs for outdoor mapping, such as [85], where a 3D terrain mapping from LIDAR on-board a helicopter is demonstrated. Simultaneous Mapping And Planning: The maps built with this approaches are typically used for obstacle avoidance and path planning. SMAP focuses more in efficiency and robustness rather than accuracy. State estimates are generally available from the GPS-IMU. Some interesting work on applying vision-based SMAP for UAVs can be found in the literature. Such as in [14] where SMAP is performed using a stereo vision system, or in [25] where another perception system with stereo cameras combining depth image information with image segmentation is presented. However, the most successful approaches on SMAP for UAS have been done using LIDARs. [161] proposes an obstacle detection and avoidance scheme based on building local obstacle maps and designing object-free trajectories using MPC. In [176] a LIDAR system is used to develop a effective 3D outdoor navigation system for an autonomous UAV. Safe autonomous flight with 3D obstacle avoidance capability is demonstrated in [157]. This method combines global planning and reactive obstacle avoidance. [66] developed a navigation approach for a quadrotor to explore and map unknown indoor environments. Safe Landing Area Detection: These algorithms are needed when UAVs are commanded to land on unknown terrains to accomplish their mission or to achieve an emergency landing. [116] presents a stereo vision-based system for a UAV that combines terrain mapping with SLAD. In [175] a stereo range map of the terrain is created and then a SLAD algorithm finds all safe landing regions by applying a set of landing point constraints (slope, roughness, distance to obstacles) to the map. Typically, LIDAR sensors are used in situations with complex terrains or poor light conditions. In [154], two different SLAD algorithms are compared; one using a monocular vision and the other a hemispherical LIDAR. Although the algorithms reported similar results, the LIDAR- based algorithm, unlike the vision-based, was able to run on-board the UAV due to its lower computational cost. Moreover, the LIDAR-based approach proved its effectiveness in night flights. In [185], another SLAD algorithm was applied to the 3D point cloud obtained from a hemispherical 3D LIDAR to determine the safe landing regions. [158] investigated the effects of factors such as smoke and dust on a LIDAR-based SLAD approach. 2.4 Guidance Guidance is the part of the system that is in charge of carrying out the planning and decisionmaking functions to achieve assigned missions or goals [88]. It takes inputs from the navigation system and uses mission information to generate reference trajectories for the control system. This block allows to replace the cognitive process of a human pilot or operator. Guidance can 2.4. GUIDANCE 23 include different tasks such as path planning, mission planning, decision-making or trajectory generation. This thesis focuses on the path planning and obstacle avoidance task. Thus, only path planning literature is reviewed. Path planning is defined as the process of using accumulated navigation data and a priori information to allow the UAS to find the best and safest way to accomplish a specific mission [88]. In most of the cases, the path planning is also responsible for the obstacle avoidance task [31][34]. In this section the most relevant and practical path planning techniques for UAS are presented. Each technique is employed for different situations and problems. For example, reactive path planing is commonly applied to perform real-time obstacle avoidance to avoid last instant collisions. 2.4.1 Road Maps A road map is generally represented as a connectivity graph. The nodes correspond to positions of the vehicle and the edges represent obstacle-free paths between these positions. In these algorithms the connectivity graph is first constructed and then the best path according to a defined criteria is searched [163]. The most usual path planning methods using Road maps are briefly described hereafter: Visibility Graph: In this method obstacles are approximated by polygons and the edges of these polygons are connected by straight line segments. It is a complete method but it only works in two dimensions. In [73] visibility graphs are applied for path generation on a quadrotor system. Voronoi Diagrams: The road map is formed by a set of Voronoi edges. These edges are equidistant from all the obstacles in the region. Voronoi paths are, by definition, as far as possible from the obstacles. In [75] the authors developed an obstacle field route planner that is based on Voronoi road maps. Probabilistic Road Maps (PRM): It takes random samples from a set of discretized positions from the vehicle’s space and it connects them with obstacle-free segments. This method is adequate for large spaces, however, due to its slow searching rate it is inefficient for dynamic obstacle avoidance. A path planner based on the PRM method for a UAV is presented in [76]. Rapidly Exploring Random Trees (RRT): This approach is a variant of the PRM method. Instead of taking random samples, the planner starts at the initial vehicle’s position and randomly expands a tree. That is, nodes are added successively to the tree, connected via edges, until a termination condition is reached [163]. This method improves the efficiency of PRM since it is able to rapidly search in large spaces. Furthermore, it is appropriate for unknown environments, as demonstrated in [187], where the authors compare the performance of the PRM and RTT planners with a helicopter UAS. 24 CHAPTER 2. GUIDANCE, NAVIGATION AND CONTROL SYSTEM 2.4.2 Potential Fields In this method the vehicle is considered under the influence of force fields generated by the goals (attractive forces) and obstacles (repulsive forces) in the space. It has low computational cost and is easy to implement. The principal limitation is that a local minima can appear depending on the obstacle shape and size [163]. In [157], a variant of the Potential Field based path planning algorithm implemented on a UAV is presented. The system relies only on LIDAR-based sensing and perception. 2.4.3 Heuristic Search Algorithms As they name denote they are based on heuristic rules. These rules are used for guessing which path moves the vehicle closer to the defined objective. Heuristic Search Algorithms (HSA) are able to provide reasonably good performance with low computational cost. A* and D* algorithms are the most common HSA. These algorithms are based on a tree-search with nodes that represent the possible solutions (i.e. positions of the vehicle). A* algorithm evaluates the goodness of each node by estimating the distance to the goal. D* is an incremental version of the A* algorithm that is able to re-plan the path in real time when changes in the environment occur. The term incremental refers to the ability of reusing previous search effort in subsequent search iterations. NASA researchers [176][185] have developed two 3D path planners for a multirotor combining heuristic planning concepts and the A* search algorithm. 2.4.4 Optimization Methods In these methods the path planning problem is considered as a numerical optimization problem. Constraints such as obstacles or vehicle’s kinematic and dynamic limitations are represented with mathematical relationships [60]. The main advantages are that, theoretically, they produce optimal solutions and they consider the limitations of the vehicle. Nevertheless, they are computationally expensive. The most investigated optimization methods for UAS are Mixed Integer Linear Programming (MILP), Receding Horizon Control (RHC) and Motion Primitive (MP). MILP and RHC have been successfully applied for path planning of an indoor quadrotor [42][120]. 2.4.5 Planning Under Uncertainties When finding the best path for a specific mission, uncertainties such as position, environment knowledge or limited precision in tracking commands can become a serious problem [60]. To deal with these uncertainties, the most common approach is to consider the worst case by introducing a conservative safety margin. However, there are some works that consider uncertainties directly in the planning algorithm, such as in [67], where this problem was addressed for autonomous 2.4. GUIDANCE 25 indoor exploration using a quadrotor vehicle. NASA researchers [59] developed a 3D path planning algorithm based on risk minimization that allows the UAS to operate with reliability and safety under uncertainties. 2.4.6 Reactive Path Planning Most of the path planners previously presented are based on a global representation of the environment and they are generally computationally expensive. Instead, reactive planning or reactive obstacle avoidance algorithms run very fast and are useful for preventing collisions in the last instant. The term reactive planning refers in general to a class of algorithms that use only local knowledge of the obstacle field to plan the trajectory [60]. There exist different methodologies to implement a reactive path planning approach such as with potential fields, by implementing optimization methods, machine learning approaches or other methods based on geometric relations. Note that reactive path planning includes techniques that are also used in global path planning (i.e. potential fields or optimization methods). However, in the reactive approaches these methods only use local information of the environment. Some of these techniques and other methodologies implemented on multirotors and other UAVs are reviewed next. In [45] a reactive obstacle avoidance approach based on the potential fluid flow theory is implemented to a fixed-wing UAV. The algorithm computes the instantaneous local potential velocity vector, which is used as a command of the inner controller. Manoeuvring constraints of the UAV are included in the control design. Obstacles are approximated by bounding rectangles. The efficacy of the proposed approach is demonstrated through numerical simulations. Another UAV approach based in potential fields is presented in [46]. The proposed solution is based on the dynamic artificial potential field (DAPF) algorithm, which generates real-time reactive collision-free paths according to the threat level of moving obstacles. The effectiveness of the proposed path planning method is assessed through simulations results. A 3D reactive motion planner based on optimal control theory for a fixed wing UAV in a dynamic workspace is presented in [21]. A virtual space representation is used to formulate the problem and generate the locally optimal trajectories in real time. These trajectories are defined by the vehicle’s speed, the flight path angle (pitch) and the heading angle. The dynamic and kinematic constraints are taken into account. The effectiveness of the proposed methods are shown by simulation results. [188] combines a reactive collision avoidance algorithm with global path planning techniques for UAVs operating in unknown environments. The implemented global path planning techniques are the Probabilistic Roadmaps and the Rapidly-Exploring Random Trees. Additionally, the system includes a reactive controller based on Optimal Reciprocal Collision Avoidance (ORCA) for achieving a fast sense-and-avoid behaviour. The proposed system is evaluated in simulation and experimentally. The experimental platform is formed by a quadrotor vehicle equipped with a structured-light depth sensor that is used to obtain information about the environment in form of occupancy grid map. In [7] the OCRA algorithm is used to develop a decentralized method for reactive avoidance with multiple aerial vehicles in a 32 CHAPTER 3. MULTIROTOR ENVIRONMENT Asctec AutoPilot Board The Autopilot Board has two processors (Fig. 3.2): the low-level processor and the high-level processor. The low-level processor receives signals from the sensors (except the GPS) and it is in charge of sending commands to the motor controllers. This unit is provided with a sensor fusion software that treats the measures received by sensors and also gives an estimation of the attitude angles. Furthermore, the low-level processor has an autopilot controller (attitude, altitude and position controllers). This processor is not accessible for the user. However, the user can control some of its functionalities through the high-level processor. The high-level processor is connected to the GPS sensor. Also, it is connected to the low-level processor by using a SPI protocol. This processor is user-programmable through the Asctec software development kit (SKD). Nevertheless, in the experimental platform presented in this thesis, this processor is only used as a bridge to communicate the low-level processor with the the Odroid XU4, a more powerful computer that was included to the platform. Odroid-XU4 The Odroid-XU4 (Fig. 3.3) is a small-size computer (83x58x20mm, weight: 60g) that, despite its low cost, provides very good performance. This odroid has 2GB of RAM and the Samsung Exynos5422 Cortex-A15 and Cortex-A7 CPUs. This computer was included on-board the quadrotor to enhance its capabilities, as well as to permit a better monitoring of the plant. The computer runs with linux Ubuntu 16.04LTS and it is equipped with Robot Operating System (ROS). ROS is a standard framework in robotics and aerial vehicles, supported by a large community. One of the main advantages of ROS is that permits the user to build complex programs without having a deep knowledge on the hardware system. The asctec mav framework ROS package allows the communications between the Asctec’s high-level processor and the Odroid computer. That is, it works like a driver to receive and send information to the Asctec AutoPilot Board. To permit this communication, the high-level processor needs to be flashed with a specific firmware (asctec hl firmware). This package provides the information about the sensors, actuators, battery and other information that helps in the monitoring of the plant. Figure 3.3: Odroid-XU4 computer (original from hardkernel.com). 3.2. MATHEMATICAL MODEL 33 3.1.4 Ground Station The ground station of this experimental platform is equipped by a laptop PC and a R/C transmitter (Futaba FF7). The laptop runs with linux Ubuntu with the i7-8550U intel processor and 16GB RAM. The PC is also equipped with the ROS framework and it connects to the ROS core in Odroid via a wifi communication. That is, ROS nodes can be launched from this PC and topics with information of the quadrotor can be monitored as well. The R/C transmitter can be used to pilot the aircraft manually. Typically, it is used in the take off and landing manoeuvrings or in case of emergency. Fig. 3.4 shows an image of the experimental platform. As it can be observed, it is a an outdoors platform. The picture includes the vehicle platform detailed in this section and the ground station R/C transmitter with the pilot prepared to perform manual manoeuvrings in eventual case of emergency. Figure 3.4: Outdoors experimental platform. 3.2 Mathematical Model In this section, the mathematical model of the AscTec Hummingbird quadrotor is presented. The coordinate systems as well as the axes labels and rotational conventions defined in this model are shown in Fig. 3.5. Two coordinate systems can be found: The body frame of reference {B}, which is attached to the body of the quadrotor, and the world reference frame {W}, considered inertial. Details of this model are given in next subsections. 3.2.1 Dynamic Equations The quadrotor dynamic model is based on the standard Newton-Euler nonlinear model described in detail in [112][24]. Additionally, the equations presented here include other effects such as gyroscopic moments, drag forces and wind disturbance forces. The dynamic model has twelve 34 CHAPTER 3. MULTIROTOR ENVIRONMENT Figure 3.5: States conventions and frames of references of the quadrotor model. states that are the world position (x,yand z), the Euler angles (φ-roll,θ-pitch and ψ-yaw), the body velocities (u,vand w) and the body angular velocities (p,qand r). Eqs. (3.1)-(3.4) define the state equations of the dynamic model of the quadrotor. ˙ p=Rv=[˙x˙y˙z]T (3.1) ˙ Φ=Hω=[˙ φ˙ θ˙ ψ]T (3.2) ˙ v=1 m(F−Fd−Fw)+g B−ω×v=[˙u˙v˙w]T (3.3) ˙ ω=(J)−1[τ−ω×Jω]=[˙p˙q˙r]T (3.4) The position state vector (x,yand z) is calculated in Eq. (3.1). The term Ris a rotation matrix to transform from the body to the world frame using the rotational sequence zyx. Eq. (3.5) shows how the construction of this matrix is made and used to transform the motion of the vehicle from the body frame to the inertial frame of reference. The resultant rotational matrix Ris shown in Eq. (3.6). v W= ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ cos(φ)sin(φ)0 −sin(φ)cos(φ)0 0 0 1 ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ cos(θ)0−sin(θ) 0 1 0 sin(θ)0 cos(θ) ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ 1 0 0 0 cos(ψ)sin(ψ) 0−sin(ψ)cos(ψ) ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ v B(3.5) 3.2. MATHEMATICAL MODEL 35 R= ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ cos(ψ)cos(θ)cos(ψ)sin(θ)sin(ψ)−sin(ψ)cos(φ)cos(ψ)sin(θ)cos(φ)+sin(ψ)sin(φ) sin(ψ)cos(θ)sin(ψ)sin(θ)sin(ψ)+cos(ψ)cos(φ)sin(ψ)sin(θ)cos(φ)−cos(ψ)sin(φ) −sin(ψ)cos(θ)sin(φ)cos(θ) ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ (3.6) Eq. (3.2) is the Euler kinematic equation that determines the rate of change of the Euler angles (φ-roll,θ-pitch and ψ-yaw) in the world frame. The basic idea of the Euler angles is to represent the orientation by decomposition the rotation in three consecutive simpler rotations about known axes. Matrix Hrelates the angular velocity of the vehicle in the body frame with the Euler angle’s rate of change. This rotation matrix (Eq. 3.8) is obtained by using sequential rotation matrices with the roll-pitch-yaw sequence, as shown in Eq. (3.7) where angular velocity vector is obtained from the Euler angles. ω= ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ ˙ φ 0 0 ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ + ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ 1 0 0 0 cos(φ)sin(φ) 0−sin(φ)cos(φ) ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ ⎛ ⎜ ⎜ ⎜ ⎜ ⎜ ⎝ ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ 0 ˙ θ 0 ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ + ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ cos(θ)0−sin(θ) 0 1 0 sin(θ)0 cos(θ) ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ 0 0 ˙ ψ ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ ⎞ ⎟ ⎟ ⎟ ⎟ ⎟ ⎠ =H−1 ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ ˙ φ ˙ θ ˙ ψ ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ (3.7) H= ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ 1 tan(θ)sin(φ)tan(θ)cos(φ) 0 cos(φ)−sin(φ) 0sin(φ)/cos(θ)cos(φ)/cos(θ) ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ (3.8) Eq. (3.3) is the linear velocity state (u,vand w) equation. They are the linear velocities that correspond to the x,yand zaxis of {B}, respectively. Frepresents the thrust forces. Fdand Fw are the drag and wind disturbance forces, expressed in the body axis. These forces are explained in Section 3.2.3. The term g Wrepresents the gravity acceleration constant expressed in the body axis. Eq. (3.4) defines the angular velocity state update. p,qand rare the rate change of roll, pitch and yaw, respectively, represented in the body frame. Eq. (3.9) shows the thrust force generated by the ith motor. ωmi is the rotational velocity of motor i,cTis the thrust coefficient, ρis the air mass density and Ais the area of the rotor. kTis the thrust constant (1 /2cTρA), which relates the square rotational velocity of the motors in rpm with the generated thrust force. The thurst force generated by the four motors of the quadrotor is shown in Eq. (3.10). This force is always parallel to the zbody axis. Eq. (3.11) defines the generated aerodynamic, gyroscopic and thrust moments on each body axis. The four motor rotational velocities are considered the inputs of the dynamic model. 36 CHAPTER 3. MULTIROTOR ENVIRONMENT Ti=1 2cTρA(ωi)2=kT(ωi)2(3.9) F=[0 0 kT(ω2 m1+ω2 m2+ω2 m3+ω2 m4)](3.10) τ= ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ dmkTω2 m2−dmkTω2 m4+Jmq(π/30)(−ωm1+ωm2−ωm3+ωm4) −dmkTω2 m1+dmkTω2 m3+Jmp(π/30)(ωm1−ωm2+ωm3−ωm4) −kQω2 m1+kQω2 m2−kQω2 m3+kQω2 m4 ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ (3.11) The parameters of the model are defined in Table 3.2 and introduced in Section 3.2.4, where the parameter identification process is described. 3.2.2 Motor Model and Control Mixing The inputs of the dynamic model are the velocities of the motors (ωmi). However, in the real AscTec Hummingbird platform, the inputs are four digital signals limited from 0 to 200 that are associated to thrust (uz) and to roll (uφ), pitch (uθ) and yaw (uψ) rates. These inputs are related to the square of the rotational velocity reference of the motors, ωn,ri, by Eq. (3.12). ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ ωn,r1 2 ωn,r2 2 ωn,r3 2 ωn,r4 2 ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ = ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ 1 0 −1−1 1 1 0 1 1 0 1 −1 1−1 0 1 ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ uz uφ−100 uθ−100 uψ−100 ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ (3.12) The motor’s rotational velocity references (ωn,ri) obtained from Eq. (3.12) are normalised from 0 to 200. Thus, they are still digital signals. According to the Asctec’s manual, the velocity reference of the motors in rpm (ωr1) can be obtained from the linear expression of Eq. (3.13), being 8600 rpm the maximum velocity and 1075 rpm the minimum. The process of calculating these motor velocity references from the quadrotor digital inputs is performed by a software block known as the control mixing. ωri =37.625ωn,ri +1075 rpm (3.13) The motors of the quadrotor are controlled to follow the references computed by the control mixing block. The dynamics of the motors are modelled by a first order differential equation. Real velocities of the motors, ωmi, are obtained by Eq. (3.14). ωmi =ωri τms+1(3.14) 3.2. MATHEMATICAL MODEL 37 3.2.3 Other Effects In addition to the basic dynamic behaviour, other effects are included in the model. For instance, the gyroscopic effects (τgyr), apparent in the moment vector (Eq. 3.11) with the terms shown in Eq. (3.15). τgyr,x =Jmq(π/30)(−ωm1+ωm2−ωm3+ωm4) τgyr,y =Jmp(π/30)(ωm1−ωm2+ωm3−ωm4)(3.15) The drag and wind disturbance forces are obtained by a simple aerodynamic equation (Eq. 3.16) which calculates the drag experienced by the quadrotor due to movement through the air. This force is calculated from the flow velocity relative to the vehicle, where ρis the air mass density, S is vehicle’s surface on the xz plane, cDis the drag coefficient and vwis the wind velocity vector. Fd+Fw=1 2ρScD(v−vw)2(3.16) 3.2.4 Parameter Identification The parameters of the model (Table 3.2) are obtained from the Hummingbird vehicle. The mass of the vehicle and distances are directly measured. Motor parameters are acquired from the datasheet of the motor (see Section 3.1.2). The thurst constant (kT) is computed from the data obtained with different experimental tests in Hover (i.e. experiments around the equilibrium point). Knowing the mass of the vehicle and the average rotational velocity of the motors during these experiments, kTcan be obtained as shown in Eq. (3.17). Fig. 3.6 shows the normalized speed of each motor during a hover experiment of 1 minute. In this experiment the average normalized speed is 98.2094, which is equivalent to 4760.3rpm (Eq. 3.13), and applying Eq. (3.17) we obtain a kTof 7.5543 ⋅10−8. Taking into account more Hover experiments in different conditions permits to obtain a more accurate value of kT=7.1103 ⋅10−8. On the other hand, torque constant (kQ) is obtained from [87], where aerodynamic parameters of Hummimgbird quadrotor are estimated through wind tunnel tests. kT=mg ω2 m1+ω2 m2+ω2 m3+ω2 m4 (3.17) The mass moment inertia matrix has been calculated applying the Huygens–Steiner theorem assuming body of the quadrotor as solid cylinders, the arms as cylindrical rods, the ESCs as flat plates and the motors as solid cylinders. Masses and dimensions of each component are measured directly on the vehicle. The obtained mass moment inertia matrix is shown in Eq. (3.18). 38 CHAPTER 3. MULTIROTOR ENVIRONMENT 0 10 20 30 40 50 60 0 100 200 0 10 20 30 40 50 60 0 100 200 0 10 20 30 40 50 60 0 100 200 0 10 20 30 40 50 60 0 100 200 Figure 3.6: Normalized motor speed in a hover experiment. J=10−3 ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ 3.4313 0 0 0 3.4313 0 0 0 6.002 ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ kg m2(3.18) Table 3.2: Parameters of the model. Symbol Description Value mMass of the quadrotor. 0.698 kg JMass moment of inertia matrix. Eq. (3.18) dmDistance from a motor to the c.o.g. 0.171 m JmInertia of each motor’s rotating components. 1.302 ⋅10−6kg m2 τmMotor time constant 5.634 ⋅10−3s kTThrust constant. 7.1103 ⋅10−8 kQTorque constant. 1.0088 ⋅10−9 3.3 Autopilot This thesis is not focused in the study of the stabilization problem. However, it is necessary to implement an autopilot controller if separated guidance and control PF strategies are going to 3.3. AUTOPILOT 39 be studied. An autopilot based on a set of Proportional-Integral-Derivative (PID) controllers is developed. Fig. 3.7 shows the control structure of the autopilot with the path following controller. The autopilot is formed by a velocity controller and an altitude & attitude controller. The controllers have been tuned by pole-placement technique and re-adjusted experimentally in the real platform. Table 3.3 presents the final parameters of these PIDs. Altitude controller includes a feed-forward gravity offset term, calculated in Eq. (3.19), that is added to the PID output. This autopilot with the parameters of Table 3.3 is used in the simulation and experimental results presented in this thesis. Path Following Algorithm Quadrotor Altitude & Attitude Controller Velocity Controller Autopilot Figure 3.7: SGC structure for path following algorithms. goffset =√mg/4kT−1075 37.625 (3.19) Table 3.3: Constants of the PID controllers of the autopilot. Kp Ki Kd u0.32 - 0.1 v0.32 - 0.1 φ4 2.2 2.4 θ4 2.2 2.4 ψ8 1 7 z4 2.2 6.6 This autopilot is programmed as a ROS package. Four ROS nodes are implemented: attitude controller at 100Hz, velocity controller at 50Hz, altitude controller at 20Hz and path following controller at 5Hz. PID controllers of the autopilot are discretized using the Tusting approximation. This package has a modular structure in the sense that each of the control blocks can be easily activated/deactivated or substituted by another controller. Also, new controllers can be incorporated to the structure straightforwardly. 40 CHAPTER 3. MULTIROTOR ENVIRONMENT 3.4 Path-Flyer: A Benchmark of Quadrotor Path Following Algorithms Before implementing and testing a control approach experimentally, its performance and functionality should be studied from numerical simulations. Moreover, nowadays the solutions proposed for problems as the obstacle or collision avoidance, given the impact and costs of an experimental failure, are only validated in simulation. For these reasons, it becomes extremely important to have a complete, realistic and validated simulation platform of the multirotor and its environment. There exist various real-time simulators implemented in ROS, as for instance the RotorS Gazebo UAV Simulator [53]. ROS is a programming framework for robots and autonomous vehicles, but is not focused on simulation tasks. Therefore, programming in ROS can become tedious for those users that only require a simulator. Apart from ROS-based platforms, there are few simulation models described in the literature but, most of them are not available on-line, they are little user-friendly, little customizable and very general purpose, in the sense of not being focused to any particular control problem. Nevertheless, there can still be found some interesting simulation platforms dedicated to different problems such, for instance, visual tracking [126] or path planning [174]. However, to the best of my knowledge, there is no simulation platform aimed to the path following problem, which is one of the main research topic of this thesis. A simulation benchmark, named Path-Flyer [146], is developed with the double objective of procuring a platform to perform simulations presented in this thesis and provide to the community a user-friendly quadrotor simulation tool. Path-Flyer, is a simulation benchmark for testing path following algorithms on a quadrotor vehicle. This benchmark allows to compare multiple path following algorithms in the same scenarios with flexibility. The model of the vehicle is complete and realistic. An identification process has been carried out to obtain its parameters. Also, the model has been validated experimentally. Path-Flyer is modular and programmable, meaning that new algorithms, blocks and/or simulation scenarios can be easily incorporated. For all these reasons, I consider this platform to be of interest for research as well as for education. What is more, it has been successfully used as a support tool for lab sessions in various subjects at the Automatic Control Department in UPC. Path-Flyer accomplishes the next key features: 1. The mathematical model of the quadrotor and its environment is complete, realistic and experimentally validated. 2. The benchmark is designed to allow the incorporation of new path following algorithms and reference paths with ease. 3. The benchmark facilitates to change simulation scenarios and it helps the user to explore and analyse relevant information of simulation results. Path-Flyer is built upon the Quad-Sim platform, a MatLab simulation tool developed in Drexdel University for simulation and control of quadrotors [65]. To this end, the objective is to improve 3.4. PATH-FLYER: A BENCHMARK OF QUADROTOR PATH FOLLOWING 41 and modify this tool to have a realistic mathematical and dynamic model of the quadrotor and its environment, to implement a proper structure to solve the path following problem and include various PF algorithms, and to incorporate new functionalities suited for this benchmark. 3.4.1 Quadrotor Model The mathematical model implemented in this platform is exactly the same as the one described in Section 3.2. That is, the same dynamic and motor equations, as well as the same control mixing and parameters are used. The magnitude and direction of the wind velocity vector, vw apparent in Eq. (3.16), has been modelled as a random walk by adding a constant value to the integral of a band-limited white noise. An example of the resultant wind disturbance signal is shown in Fig. 3.8. This random walk is fully customizable. Furthermore, the user can decide whether to perform simulations with wind disturbance or without. User can also program their own wind profile. 0 5 10 15 20 25 30 35 40 1 2 3 4 5 0 5 10 15 20 25 30 35 40 0.4 0.6 0.8 1 1.2 Figure 3.8: Wind disturbance generated with a random walk. On the other hand, a band-limited white noise signal is added to each of the twelve measured states. Those signals have been adjusted to resemble the noise of the sensors of the real quadrotor platform. Moreover, Path-Flyer allows the user to customize each noise signal and to enable each one separately. 3.4.2 Implemented Algorithms Four path following algorithms are implemented in this benchmark: Non-Linear Guidance Law (NLGL), Carrot-Chasing and an adaptive version of both algorithms. These are geometric 48 CHAPTER 3. MULTIROTOR ENVIRONMENT interaction of the R/C transmitter present in the real platform is also included in the RotorS environment. A scheme of the structure of the nodes and topics programmed in the RotorS environment is presented in Fig. 3.16. This scheme shows each of the nodes implemented in this simulation environment represented as a single block. The set of topics generated by each of these nodes is also included inside every block. The RotorS platform is included in the scheme as well. The RotorS block represents the UAS of the control structure in Fig. 2.1. RotorS is formed by several nodes, however, in this scheme it is represented as a unique block. Furthermore, RotorS generates a large number of topics but only the topics that are used in this thesis are included. This topics are, essentially, the topic of the LIDAR sensor, the topics of the other modelled sensors, the topics that provide the ground truth measurements and the topic that receives the commands of the quadrotor’s motors speeds. The LIDAR is the sensor added in the platform to detect obstacles in the vehicle’s route. It is used to perform the perception task. A realistic model of the LIDAR sensor that is installed on the real quadrotor is employed to generate the LIDAR topic. More information about the selection, installation and modelling of this sensor is found in Section 7. The translate topics node communicates the RotorS platform with the guidance and control nodes. That is, this node generates the topics with the same structure, name and format of those in the real platform. The topics that this node publishes as well as the set of topics of RotorS that it is subscribed to are denoted in light green in the scheme of Fig. 3.16. Note that two groups of topics of RotorS are highlighted with this color. These correspond to the set of topics that provide the measurements of the modelled sensors and the set of topics that provide the ground truth measurements. The scheme of Fig. 3.16 presents the structure when ground truth measurements are used. Nevertheless, it is important to mention that, when sensor measurements are used, another node is required. This node computes the position of the vehicle in the world frame (xand ycoordinates) from the latitude and longitude measurements received by the gps sensor. The emulate rcdata node is used to emulate the data of a R/C transmitter. The topics generated by both the translate topics and emulate rcdata nodes are sent to the rest of the nodes that form the GNC structure; translate topics provides the measurements and emulate rcdata is used to manage the mission control. That is, mission control in the sense that the R/C data is used to define when to perform trajectory control, hover control or manual control (controlled manually by a pilot), just as in the real experiments. The R/C data can be modified by the user by using different ROS services. 3.5. ROTORS 49 /hummingbird/Lidar RotorS /hummingbird/command/motor_speed /hummingbird/ground_truth/imu /hummingbird/ground_truth/odometry /hummingbird/ground_truth/pose_with_covariance /hummingbird/motor_speed translate_topics /fcu/imu /fcu/imu_custom /fcu/gps_pose /fcu/gps_custom emulate_rcdata /fcu/rcdata UAS attitude_controller /fcu/control altitude_controller /altitude_ctrl/uz velocity_controller /vel_ctrl/attitude_cmd OA_node /ddpg_avoidance/status_and_state PF_node /ddpg_ctrl/vel_psi_cmd /ddpg_ctrl/z_cmd CONTROL GUIDANCE AUTOPILOT PERCEPTION 1 1 1 2 2 2 1212 12 /hummingbird/gps_sensor/ground_speed /hummingbird/gps_sensor/gps /hummingbird/imu /hummingbird/odometry_sensor1/odometry Figure 3.16: Scheme with nodes and topics programmed in the RotorS environment. 50 CHAPTER 3. MULTIROTOR ENVIRONMENT The rest of the nodes form the Guidance, Navigation and Control structure (Fig. 2.1). The control block is formed by the PF algorithm (PF node) and the autopilot. The path following block sends commands of velocities, yaw angle and altitude to the velocity controller and altitude controller of the autopilot. These two controllers provide the angle commands and the altitude action (uz) to the attitude controller, respectively. Then, the attitude controller generates the digital inputs of the Asctec Hummingbird vehicle. These inputs are treated in the translate topics node, which computes the commands of the motor velocities that are sent to the RotorS block. The guidance block is formed by the obstacle avoidance node (OA node). This node sends a topic to the path following block that contains the path following state, used for knowing the path to follow, and the status, that tells whether to perform path following control or hover control. More information about the design, training, implementation and results of the developed deep reinforcement learning approaches for path following and for obstacle avoidance is found in Chaps. (6) and (8), respectively. Part II Control 51 Chapter 4 Control-oriented and Geometric Path Following The path following problem consists on following a trajectory defined in space without any temporal constraint. Path following can be solved by implementing a separated guidance and control structure (SGC) or with a integrated guidance and control structure (IGC). The SGC control is divided in two elements: the path following and the autopilot. The autopilot is the inner controller that is in charge of tracking the commands generated by the path following controller. In this thesis, the autopilot is solved by a set of PID-based controllers (Section 3.3). The main contributions of this thesis in the control part are made in the path following problem. More information about the control structure and the path following problem are found in Chap. (2) where an exhaustive literature review of UAV path following algorithms is presented. In this chapter, four path following algorithms have been chosen from the literature review made in Section 2.2 to be implemented and compared. They were selected for their popularity and performance. These algorithms are described in detail, harmonizing the nomenclature of different approaches, and then, they are applied to the realistic quadrotor simulation model. A comprehensive comparison of the PF algorithms on the same scenario in equal conditions is provided. This chapter is divided in two sections; first, the implementation of the algorithms, and then, the comparison with simulation results. 4.1 Implementation of Path Following Algorithms on a Quadrotor In this section, four path following algorithms are implemented on the quadrotor vehicle. First, Backstepping and Feedback Linearization, which are the two most referenced algorithms applied to quadrotors, as reported in Section 2.2. Next, Non-Linear Guidance Law (NLGL) and Carrot- Chasing geometric algorithms. 53 54 CHAPTER 4. CONTROL-ORIENTED AND GEOMETRIC PATH FOLLOWING In the reviewed literature, no application to quadrotors has been reported of the NLGL and Carrot-Chasing algorithms. However, they are commonly used on fixed-wing vehicles [172][133], as well as on Unmanned Ground Vehicles (UGV) [122][130], and sometimes on other types of unmanned vehicles. They are simple and they typically provide feasible solutions with good path following performance. These geometric algorithms are very similar. They differ in how the VTP is calculated. In this chapter such simple and fruitful algorithms are implemented to evaluate their performance. Developing my own implementation of these four algorithms will permit to evaluate and compare them under the same conditions. This is shown in the Section 4.2, where the simulation results are presented. These algorithms are designed and tuned only considering the dynamic equations of the body (Eqs. 3.1-3.4). Thus, the equations of the motors, control mixing and effects such as gyroscopic effects or wind disturbance have been omitted to perform the calculations to obtain the control laws. However, to validate the algorithms properly, they are taken into account in the simulation model. The inputs of this model are the total thrust (T) and the torques applied on each rotational axis (τφ, τθ, τψ). Eq. (4.1) presents the relation between the rotation speed of the motors (ωm1−4) and these inputs. ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ T τφ τθ τψ ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ = ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ kTkTkTkT 0d kT0−d kT −d kT0d kT0 −kQkQ−kQkQ ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ ω2 m1 ω2 m2 ω2 m3 ω2 m4 ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ (4.1) 4.1.1 Backstepping The Backstepping controller developed in this chapter is an adaptation of the one used in [27]. Note that the UAV model used in [27] is slightly different from the one defined in Section 3.2. The frames of reference considered in each model are different and [27] uses a rotation matrix to represent the attitude. In this algorithm, S()represents the skew symmetric matrix that verifies S(x)y=x×y. And p′ dis the partial derivative of pdwith respect to γ,∂pd ∂γ . It is important to recall that γis the scalar parameter of the virtual arc that parametrizes the path (Definition 2.2.1). To solve the path following problem, four backstepping error vectors are defined, each one of three components. The first error vector, Eq. (4.2), is the position error. The second error vector, Eq. (4.3), includes a term of position error and a term of velocity error in the world frame. The sigmoidal function σ(⋅), Eq. (4.4), limits the growth of e2when there are large position errors, where pmax is the allowed limit. This guarantees that the actuation does not grow unbounded. e1=p−pd(γ)(4.2) 4.1. IMPLEMENTATION OF PF ALGORITHMS ON A QUADROTOR 55 e2=σ(e1)+1 k1 ˙ e1(4.3) σ(x)=pmax x 1+∥x∥(4.4) The convergence of e1and e2to zero is assured by defining two control Lyapunov functions, whose derivatives are negative definite if the thrust force, T, follows Td, Eq. (4.5), with the direction r3d, Eq. (4.6), where u3=[001]T . Td=m∥k2 1k2e2+k1˙ σ(e1)+gu3−¨ pd∥(4.5) r3d=k2 1k2e2+k1˙ σ(e1)+gu3−¨ pd ∥k2 1k2e2+k1˙ σ(e1)+gu3−¨ pd∥(4.6) The control law expression for the thrust force is calculated as shown in Eq. (4.7). r3is the third column of R, which represents the direction of the zbody axis of the vehicle. Thus, if r3is equal to the desired thrust direction r3d, the thrust force will be the same as the desired thrust force Td. For this reason, the third error vector is defined as the error of the thrust direction, as stated in Eq. (4.8). T=rT 3dr3Td(4.7) e3=r3−r3d(4.8) A third control Lyapunov function, which includes the first and second Lyapunov functions as well as a term for the third backstepping error, is defined. With the aim of making the first three errors tend to zero, the expression of the fourth error vector, Eq. (4.9), is defined in such a way that if this error becomes zero, the time derivative of the third Lyapunov function remains negative definite. In Eq. (4.9)Rdrepresents the desired orientation matrix and ωdstands for the desired angular speed of the vehicle. e4=−k3S(u3)2RTr3d+S(u3)(ω−RTRdωd) −Td mk1S(u3)2RTe2 (4.9) Finally, a fourth Lyapunov function is defined for the fourth error. To assure the stability of the system with this Lyapunov function, the expression of the angular acceleration of the vehicle is calculated as stated in Eq. (4.10). In this expression ˙ ωcstands for the calculated angular acceleration and ˙ω3c(t)is an arbitrary function that defines the dynamics of the yaw 56 CHAPTER 4. CONTROL-ORIENTED AND GEOMETRIC PATH FOLLOWING angle. Then, the control law of the torque can be calculated from this expression by means of Eq. (4.11). ˙ ωc=−S(u3)(−k4e4−RTr3d +k3S(u3)2(˙ RTr3d+RT˙ r3d) +1 mk1S(u3)2(˙ TdRTe2+Td˙ RTe2+TdRT˙ e2)) +˙ RTRdωd+RTRd˙ ωd+[0 0 ˙ω3c(t)]T (4.10) τ=J˙ ωc+S(ω)Jω(4.11) It can be noticed that constants k1−4appear in the calculated control laws. These constants define the dynamics of each backstepping error. More details about the derivation of these control laws and about their stability proofs are given in [27]. Notice that defining the error vectors this way sets only the third column of Rd. This is evident by checking the definitions of error e3(in Eq. 4.8) or function ˙ω3c(t), that appears in the expression of the calculated angular velocity (Eq. 4.10). Thus, the direction of the xand yaxis can be assigned arbitrarily, as long as the orthogonality of the frame of reference is satisfied. That is to say, a degree of freedom appears for the evolution of yaw. Exploiting this degree of freedom, the desired rotation matrix Rdhas been defined as the one that makes the velocity vector of the vehicle tangent to the path, as shown in Eq. (4.12). To assure this control specification, a PD-like controller is designed as a control law for function ˙ω3c, as shown in Eq. (4.13), where r1 is the first column of the rotation matrix Rand r2dis the second column of the desired rotation matrix Rd. Rd=[−S(r3d)2p′ d ∥S(r3d)2p′ d∥ S(r3d)p′ d ∥S(r3d)p′ d∥r3d](4.12) ˙ω3c=−l2(ω3−ω3d+l1rT 2dr1)+˙ω3d−l1d dt (rT 2dr1)(4.13) Parameter γis present in the definition of e1, Eq. (4.2), and implicit in the rest of the controller expressions. This parameter defines the desired trajectory position, velocity and acceleration at a certain time instant. Therefore, the controller algorithm needs to have this parameter properly scheduled to work correctly. This is known as the timing law. The timing law stated in this algorithm is given by the second time derivative of γ, defined in Eq. (4.14). Matrix W TRT(γ) represents the rotation matrix from a frame {T}tangent to the path to the world frame, obtained as shown in Eq. (4.15), where ξ(γ)is a function that changes sign in the inflection points of the path. This timing law has been defined to make the time derivative of γconverge to its desired value ˙γd, and also to preserve the stability of the system as long as the condition stated in 4.1. IMPLEMENTATION OF PF ALGORITHMS ON A QUADROTOR 57 Eq. (4.16) is satisfied. That condition assures the ascending behaviour of the path. ˙γdis a control specification that can be related to the path’s shape. Examples of the definition of this variable are given in Section 4.2. ¨γ=−kγσ(˙γ−˙γd)+uT 1 W TRT(k2 1k2e2+k1˙ σ(e1)−˙γp′′ d)(4.14) W TRT(γ)=[p′ d(γ) ∥p′ d(γ)∥ξ(γ)S(p′ d(γ))2p′′ d(γ) ∥S(p′ d(γ))2p′′ d(γ)∥ξ(γ)S(p′ d(γ))p′′ d(γ) ∥S(p′ d(γ))p′′ d(γ)∥](4.15) ∣p′ d(γ)Tu3∣>α(4.16) Some of the mathematical expressions stated for the Backstepping algorithm require derivatives that have to be estimated in order to avoid their arduous analytical calculations. In this chapter, a first order filter was used to obtain a time derivative estimation of a known variable. The scheme of this filter is shown in Fig. 4.1, where xis the known variable, ˆxis the filtered variable, ˙ ˆxis the time derivative estimation of xand x0is its initial value. Figure 4.1: General derivative estimation diagram The filter was used to estimate the time derivative of the thrust ( ˙ Td) and the three components of the direction vector of this force ( ˙ r3d). With ˙ r3dit is possible to calculate ˙ Rdas shown in Eq. (4.17). The angular speed vector ωdcan be obtained from its skew symmetric matrix, calculated as shown in Eq. (4.18). The time derivative of the desired angular speed ˙ ωdcan be estimated using an estimation filter for each component of vector ωd. ˙ Rd=∂Rd ∂r3d ˙ r3d+∂Rd ∂p′ d p′′ d˙γ(4.17) S(ωd)=RT d˙ Rd(4.18) The control structure of the BS controller is shown in Fig. 4.2. It is an IGC structure, as it is implemented without any inner loop controller. The second derivative of γ, calculated by the timing law, is integrated twice to obtain ˙γand γ. The derivative estimation filters have been omitted to clarify the scheme. 64 CHAPTER 4. CONTROL-ORIENTED AND GEOMETRIC PATH FOLLOWING Figure 4.3: Feedback Linearization control structure. The implementation of the Feedback Linearisation controller is stated in Algorithm 2. This algorithm requires the full state of the extended system (xe), the radius of the circumference (A), the desired velocity of the vehicle (Vref ) and the feedback gains of the linear controller (Kij). It returns the four inputs of the quadrotor. The algorithm starts by an axis transformation of the velocities to the world frame. Later, the outputs of the system are obtained by means of Eq. (4.32). Note that the arctan is implemented using the atan2 function. Next, the references of the linear states (ηref 11 ,ηref 12 and ηref 21 ) are calculated. These define the experiment references and are detailed in theSection 4.2. Note that it is ensured that ηref 21 , the yaw reference, satisfies the condition ∣ηref 21 −ψ∣⩽π, since it is expected that the vehicle moves the smallest angle towards its reference yaw angle. Final steps are the calculation of the linear states (ξ,η), αand β(Eqs. 4.34,4.36 and 4.37), to obtain the virtual inputs (Eq. 4.39), and to calculate the inputs used to control the system (Eq. 4.38). It is important to note that the input of the total thrust of the quadrotor is obtained by double integrating the u1input of the extended system. 4.1.3 Three-dimensional NLGL NLGL, as mentioned in Section 2.2.4, is a geometric PF algorithm based on the Virtual Target Point (VTP) concept. The VTP is a target reference point placed on the desired path, which is updated periodically. The NLGL algorithm obtains the VTP as the point on the path at a predefined distance Lfrom the vehicle (Fig. 3.9), where Lis a scalar constant parameter that affects the performance of the NLGL controller. Once the VTP is obtained, the algorithm calculates the needed yaw angle to face the vehicle towards the VTP on the path. Since the vehicle is required to move at a constant predefined speed, it will end up moving to the path. From the algorithm description it can be seen that it is designed to work only in the twodimensional space given by the xy plane. For this reason, as the two previous algorithms operate in three dimensions and the objective of this chapter is to compare the algorithms in the same conditions, NLGL algorithm presented in [172] has been adapted to operate in 3D as well. To this end, the 3D-NLGL algorithm has been stated in Algorithm 3. The Algorithm 3requires the current position pof the vehicle and its yaw angle (ψ), as well as the desired velocity of the vehicle (Vref ) and the scalar parameter of the guidance law (L). 4.1. IMPLEMENTATION OF PF ALGORITHMS ON A QUADROTOR 65 Algorithm 2 Feedback Linearization Require: vecxe∶={x, y, z, u, v, w, p, q, r, φ, θ, ψ, ζ1, ζ2}, A, Vref , Kij Transform velocities axis: 1: [WuWvWw]T=R[u v w]T Calculate outputs: 2: h1=x2+y2−A2 3: h2=Z−fz(xe) 4: h3=Aatan2(y, x) 5: h4=ψ Calculate linear state references: 6: ηref 11 =0(K31 =0) 7: ηref 12 =Vref 8: ηref 21 =h3/A+π/2 →ensure that ∣ηref 21 −ψ∣⩽π Calculate linear states (ξ,η), αand β 9: {ξ1i,ξ2i,η1i,η2k}∶=Ψ(xe) 10: αi=Lm fhi 11: βi=LgLm−1 fhi →i∈{1,2,3,4}, k ∈{1,2} Calculate virtual inputs 12: {v1, v2, v3, v4}from Eq. (4.39) Calculate inputs of the extended system 13: ui=β−1 i[vi−αi] →i∈{1,2,3,4} Obtain real inputs 14: ˙ ζ2=u1,˙ ζ1=ζ2, T =ζ1, 15: τφ=u2, τθ=u3, τψ=u4 return T, τφ, τθ, τψ It returns the commands for the velocities in the xand yaxis, the altitude and the yaw angle of the quadrotor. In this algorithm, γprev represents the value of γdmin saved from the previous execution of the algorithm, where γdmin is the point on the path, given by the value of γ, that is at a minimum distance to the location of the vehicle. The value of γprev is initialized to 0 in the first execution of this algorithm. As seen in the algorithm, the first step after the initialization of the variables is to calculate the point on the path that is at a minimum distance from the vehicle, given by γdmin , and the value of this distance. Then, if the distance to γdmin is larger than L, the VTP is defined as γdmin , making the vehicle move directly towards the path. When the vehicle is closer to the path, the algorithm will find the first point on the path, starting from γdmin , that is at an Ldistance from the vehicle, and will set it as the VTP. The fact that it starts to search the VTP from the point 66 CHAPTER 4. CONTROL-ORIENTED AND GEOMETRIC PATH FOLLOWING Algorithm 3 Three-dimensional NLGL Require: p∶(x, y, z), ψ, L, Vref , pd(γ)∶=[xd(γ), yd(γ), zd(γ)]T, γf Initialize (First execution): 1: γprev =0 Calculate dmin, γdmin : 2: dmin ∶=min γ(∥p−pd(γ)∥)∣γ∈[γprev;γf] 3: γdmin ∶=argmin γ(∥p−pd(γ)∥)∣γ∈[γprev;γf] Calculate γV T P : 4: if dmin >Lthen 5: γV T P ∶=γdmin 6: else 7: γV T P ∶=argmin γ(∣∥p−pd(γ)∥−L∣)∣γ∈[γdmin ;γf] Calculate commands: 8: ψcmd ∶=atan2(yd(γV T P )−y, xd(γV T P )−x) →ensure that ∣ψcmd −ψ∣⩽π 9: zcmd ∶=zd(γdmin ) 10: vcmd ∶=0 11: ucmd ∶=Vref dV T P,xy L return ψcmd, zcmd, vcmd, ucmd at a minimum distance (γdmin ) going forwards, ensures that the vehicle moves always forward on the path. The next step of this algorithm is to calculate the commands, which are the outputs of the controller. This is important to ensure the correct adaptation of the algorithm from 2D to 3D. First of all, similarly to the 2D-NLGL algorithm, the yaw command angle is calculated as the angle that would face the vehicle towards the VTP, projected in the xy plane of the vehicle. Then, as in the Feedback Linearisation controller, it is ensured that ψcmd satisfies the condition ∣ψcmd −ψ∣⩽π. The altitude command zcmd is obtained as the desired Zin the point of minimum distance γdmin . As in the two-dimensional algorithm, the velocity command vcmd in the ybody coordinate is set to zero, as no movement is required on this coordinate. In the 2D-NLGL algorithm the velocity command ucmd in the xbody coordinate has a constant value (Vref ). Considering the vertical distance to the VTP, in this 3D algorithm the value of ucmd is set proportional to dV T P,xy, that is the projection on the xy plane of the distance Lto the VTP. Line 11 of the algorithm calculates this command. It can be seen that when the VTP is on the xy plane ucmd =Vref . The outputs of this controller are the ucmd and vcmd velocity command, the yaw command (ψcmd) and the altitude command (zcmd). That is, this algorithm uses a SGC structure (Fig. 3.7) for solving the path following problem. Thus, the autopilot of Section 3.3 is implemented. It is important to mention that, with an SGC structure, the path following controller’s behaviour 4.1. IMPLEMENTATION OF PF ALGORITHMS ON A QUADROTOR 67 Algorithm 4 Three-dimensional Carrot-Chasing Require: p∶(x, y, z), ψ, δ, Vref , pd(γ)∶=[xd(γ), yd(γ), zd(γ)]T, γf Initialize (First execution): 1: γprev ∶=0 Calculate dmin, γdmin : 2: dmin ∶=min γ(∥p−pd(γ)∥)∣γ∈[γprev;γf] 3: γdmin ∶=argmin γ(∥p−pd(γ)∥)∣γ∈[γprev;γf] Calculate γV T P : 4: γV T P ∶=argmin γ(∣(∫γ γdmin ∥p ′ d(γ)∥dγ)−L∣)∣γ∈[γdmin ;γf] Calculate commands: 5: ψcmd ∶=atan2(yd(γV T P )−y, xd(γV T P )−x) →ensure that ∣ψcmd −ψ∣⩽π 6: zcmd ∶=zd(γdmin ) 7: vcmd ∶=0 8: ucmd ∶=Vref dV T P,xy dV T P return ψcmd, zcmd, vcmd, ucmd will always be affected by the performance of the inner-loop controller. Other techniques, such as LQR, could be applied to design the autopilot controller with different results. However, the PID controllers, which do not require the full state estimation, have been chosen by their simplicity which is aligned with the simplicity of the geometric algorithms. This allows to make a comparison of the control algorithms (Backstepping and Feedback linearisation) with the simplest PF algorithms. 4.1.4 Three-dimensional Carrot-Chasing Carrot-Chasing is also a geometric algorithm based on the VTP concept. In the CC approach, the VTP is called carrot and is obtained as the point at constant length δalong the path from the point at a minimum distance to the vehicle (Fig. 3.10). That is, δis computed along the path arc length, and therefore it is not equivalent to the Euclidean distance computed in the NLGL algorithm. The vehicle, which is required to move at a constant speed, is faced to the VTP by changing its yaw angle, and thus it is said to chase the carrot. Again, it can be noted that the algorithm is designed to operate in the two-dimensional space. Thus, in this chapter it has been modified and adapted to work in the three-dimensional space. The resulting developed 3D approach is found in Algorithm 4. This algorithm is very similar to the 3D-NLGL algorithm, since the structure and the operations to calculate the commands are identical. However, in this case the VTP is obtained as the point on the path that is at aδlength from the γdmin point, where this length is computed along the path arc. Another difference between CC and NLGL algorithms is that in CC the VTP is always calculated the same way, regardless of the distance from the UAV to the path. 68 CHAPTER 4. CONTROL-ORIENTED AND GEOMETRIC PATH FOLLOWING The control structure used to apply the 3D-CC is the same as the 3D-NLGL, represented in the Fig. 3.7. Thus, the same autopilot has been used. 4.2 Algorithm Comparison Based on Simulation Results In this section a set of simulation results comparing the four algorithms described previously is presented. The simulations have been performed on the simulation model presented in Chap. (3). Thus, the dynamics of the motors, the control mixing and aerodynamic effects, such as gyroscopic effects and drag forces, are included. Noise on the measurements and wind disturbances are not included unless otherwise specified. The desired path is a helix defined by pd(γ)= ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ Acos(γ) Asin(γ) γ+3 ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ (4.40) where Ais the radius of the helix, which is 3 meters. Note that with this definition the path starts at 3 meters of altitude. Additionally, the vehicle is required to travel at a constant velocity Vref on the path and with a yaw angle tangent to the path. To meet the velocity specification with the Backstepping controller, the desired evolution of γ has been set as defined in Eq. (4.41). The yaw requirement is assured by defining the rotational matrix as in Eq. (4.12) and including the PD-like controller for the angular acceleration on the zaxis as defined in Eq. (4.13). ˙γd=Vref A(4.41) Assuming that the vehicle’s orientation is tangent to the path, γcan be obtained subtracting π/2to the yaw command (ψcmd). Thus, function fz(x)(Eq. 4.32 of Feedback Linearisation algorithm) becomes fz(x)=γ+3=ψcmd −π 2+3 (4.42) In the FL controller the desired velocity requirement is met by making the reference of the first derivative of h3(ηref 12 ) equal to Vref . The feedback gain K31, which regulates the position of the vehicle along the path, is set to zero. Hence, h3is only in charge of controlling the velocity of the vehicle along the path. On the other hand, to follow the yaw specification, the reference for h4is calculated as the path tangent angle on the closest point of the path to the vehicle, as seen on line 8 of the Algorithm 2. For the 3D-NLGL and 3D-CC algorithms, the velocity and yaw specifications are accomplished by construction. The control parameters for each of the four algorithms have been tuned as those that achieve the least mean absolute error (MAE) in terms of distance to the path error. This MAE has 4.2. ALGORITHM COMPARISON BASED ON SIMULATION RESULTS 69 been evaluated in the simulation platform. These parameters are presented in Table 4.1. For the geometric algorithms, as they only have one parameter to tune, a parameter sweep was performed in order to find the value which procures the minimum distance MAE for the defined path. It is important to remark that those parameters depend on the velocity of the vehicle and the path shape. Regarding the FL algorithm, the constants that control each of the four linear subsystems (xy-position, altitude, path velocity and yaw) were tuned separately minimizing the error of each variable. It was done by means of an iterative redesign through the pole placement technique, assuring stability on each iteration. Once the constants of each subsystem were obtained, it was verified that the complete system still behaved with minimum path following error and that it was stable. In the BS algorithm, the stability of the controlled system is very sensitive to changes in the control parameters (k1−4). As they were difficult to tune, they were empirically set to make the system stable. The rest of the parameters of this algorithm were obtained by means of a parameter sweep search. Table 4.1: Control parameters for each algorithm. Algorithm Parameters Backstepping k1=2, k2=1, k3=50, k4=2, kγ=50, l1=10, l2=2, pmax =1, kf=10 Feedback k11 =−18.75, k12 =−40, k13 =−32.25, k14 =−10, Linearization k21 =−1 250, k22 =−1 650, k23 =−435, k24 =−36, k31 =0, k32 =−104, k33 =−124, k34 =−21, k41 =−29, k42 =−10 3D-NLGL L=0.59 3D Carrotδ=0.59 Chasing Three types of results are reported in this section. First, on the steady state regime, when the vehicle has converged to the path. Next, on the transient regime, changing the initial state conditions. Last, with time-varying wind disturbances. 4.2.1 Steady State Regime This section shows results about the steady state regime of the vehicle’s response with each controller. Steady state refers to the controller’s error being constant. The performance of the four algorithms for one full lap of the helix is compared in Table 4.2. The columns relate to: the time to travel one lap, the mean of the distance to the path (d), the mean absolute yaw error, the mean velocity of the quadrotor and the computational effort of the algorithm. The path distance is the minimum distance between the path and the vehicle, the mean absolute yaw error is calculated from the error between the vehicle’s yaw angle and the path tangent angle and the computational effort (Compeff ) is a dimensionless parameter that represents the normalized computation time of each algorithm. 70 CHAPTER 4. CONTROL-ORIENTED AND GEOMETRIC PATH FOLLOWING The computational effort was calculated as follows: A discrete time simulation with a time step of 1ms was performed for each of the four algorithms and the execution time of each block of the simulink model was monitored. The total runtime dedicated to execute the calculations of the control (tcontrol) part were divided by the total runtime needed for the calculations of the dynamics of the system (tmodel) for each algorithm (Eq. 4.43). Assuming that the time dedicated to compute the dynamics of the quadrotor is similar on each execution, the obtained quotient is considered a representative value to evaluate the computational effort. Finally, result was normalized, setting to 1 the lowest one which corresponds to the NLGL algorithm. The implementation of the algorithms was not optimized in terms of computation time. Nevertheless, none of them includes especially intensive calculations (such as solving optimization problems or matrix operations). Thus, they are subject to slight changes on the computation effort term. Compeff =tcontrol tmodel (4.43) Table 4.2: Results for one lap in steady state regime. time (s)d(m) ∣eψ∣(deg) ∥v∥(m /s)Compeff Backstepping 44.6306 0.0016 1.6094 0.4451 57.3895 Feedback Linearization 40.7400 0.0078 3.0522 0.4874 1.4877 3D-NLGL 40.3800 0.0198 2.6953 0.4897 1 3D-Carrot-Chasing 40.3800 0.0194 2.6949 0.4897 1.0147 4.2. ALGORITHM COMPARISON BASED ON SIMULATION RESULTS 71 Figure 4.4: 3D trajectory for one lap of the helix in steady state regime: 3D-NLGL algorithm. The three-dimensional trajectory for one lap of the helix in steady state regime using the 3D- NLGL algorithm is shown in Fig. 4.4. The performance of this algorithm and the 3D-CC algorithm is poor compared to the control-oriented algorithms where the vehicle stabilized closer to the path. Nevertheless, it can still be considered that both algorithms present an accurate behaviour as evidenced by Fig. 4.4. 4.2.2 Transient Regime In these simulations the initial state conditions are modified to observe the transient behaviour of the algorithms. In particular, the effects of changing the initial x-coordinate of the vehicle and the effects of varying its initial yaw are analyzed. In the simulations where the initial x-coordinate position changes, the quadrotor starts on the position given by x={0.5k∣k=1..12},y=0 and z=3. The initial yaw angle is 90 degrees in the case of the BS and FL controllers whereas in the geometric algorithms the vehicle faces the VTP. This orientations result in a minimum initial yaw error. The initial vehicle’s linear velocities, angular velocities, accelerations, roll and pitch angles are zero. Fig. 4.5 shows four performance indicator graphs, for each algorithm and for each initial position. First, the time the quadrotor takes to converge to the path. Convergence condition is defined as the vehicle remaining at path distance less than 10 times the stable state regime error. Second plot shows the accumulated distance to the path during this convergence period. Note that the periods are different for each algorithm, since their stabilization time are different. The third 72 CHAPTER 4. CONTROL-ORIENTED AND GEOMETRIC PATH FOLLOWING graph, shows a parameter for the control effort, i.e. the mean of the absolute value of the control action derivative. Note that the control action of motors (umi) is given in percentage. Finally, the convergence position on the path given by γis represented, where the magnitude of γis given in number of radius along the path (A). The mathematical definition of these indicators is stated in Eqs. (4.44)-(4.47), where tconv is the stabilization time, dis the distance to the path, dss is the distance error on the steady state regime, dint is the integral of the path distance on the convergence time, ceff is the control effort term, umi is the control action of the ith motor and γconv is the convergence position given by the virtual arc parameter. tconv ∶d<10dss ∀t>tconv (4.44) dint =∫tconv tinit d(t)dt (4.45) ceff =(∣dum1 dt ∣+∣dum2 dt ∣+∣dum3 dt ∣+∣dum4 dt ∣) 4(4.46) γconv =γ(tconv)(4.47) Fig. 4.6 and Fig. 4.7 show the three-dimensional trajectory when the initial x-coordinate is 0.5m and 6m, respectively, comparing the response of the four algorithms for one lap. In both figures, circles indicate the convergence point of each algorithm. A solid line represents the convergence trail, and a dotted line where the vehicle has converged. The simulation results changing the initial yaw angle are shown in Fig. 4.8. In these simulations the vehicle starts at the initial point of the path (x=3, y =0, z =3) and the yaw orientation varies from 0 to 180 degrees in steps of 22.5○in each simulation. The rest of the initial state conditions are identical to the previous simulations. Furthermore, the plots represented in Fig. 4.8 are equivalent to the ones found in Fig. 4.5. Since the convergence criterion only takes into account the distance to the path, in some cases it is considered that the quadrotor has converged to the path while there is still significant yaw error. Once more, the three-dimensional trajectory plots comparing the behaviour of each algorithm are presented in figures 4.9 and 4.10, for the extreme cases of the initial yaw angle. The convergence point, represented with a circle, divide the convergence trail (solid line) and the rest of the trajectory (dotted line). 4.2. ALGORITHM COMPARISON BASED ON SIMULATION RESULTS 73 Figure 4.5: Simulation results varying the initial xposition from 0.5 to 6 meters: Backstepping (red), Feedback Linearisation (blue), 3D-NLGL (purple) and 3D Carrot-Chasing (green). 80 CHAPTER 4. CONTROL-ORIENTED AND GEOMETRIC PATH FOLLOWING Figure 4.13: 3D trajectory evolution comparing Backstepping and 3D-Carrot-Chasing for one lap of the Lemniscate under realistic flight conditions (2 m /s). steps. Each point on the graphs corresponds to a simulation of a full lap of the Lemniscate as in Fig. 4.13. 4.2.5 Discussion From the steady state regime simulation results, in Table 4.2, it can be observed that the Backstepping algorithm is the one that achieves the best performance in terms of distance to the path, which is usually the most important parameter. BS presents a little more than 1.5mm of error, compared to the 8mm of the Feedback Linearisation algorithm or the almost 2cm of the geometric algorithms. However, the BS algorithm presents a very large computational effort in comparison to the other algorithms, which obtain similar values of this indicator. Another drawback of the BS algorithm is that it reduces the cruise velocity in order to achieve this great precision. This is observed in the higher time that it takes to accomplish one lap of the helix. Regarding the yaw error, again the Backstesping algorithm has the best response with a mean yaw error of 1.6○. The geometric algorithms have a larger yaw error than BS because these algorithms are not designed to keep the vehicle tangent to the path but to face it to the VTP. FL algorithm shows a yaw error even larger than the geometric algorithms, although it is designed to be tangent to the path. Regarding the transient regime results, in the simulations where the initial x-coordinate changes, the 3D-NLGL algorithm presents the worst transient behaviour. It has wide oscillations along the path that increase its convergence time. The 3D Carrot-Chasing algorithm also presents an oscillating performance. However, the oscillations are smaller, as can be noticed from the 3D 4.2. ALGORITHM COMPARISON BASED ON SIMULATION RESULTS 81 Figure 4.14: Simulation results varying the velocity reference from 1 to 4 m /sunder realistic flight conditions: Backstepping (red) and 3D Carrot-Chasing (green). plots of Fig. 4.6 and Fig. 4.7. This performance distinction between the two geometric algorithms is due to the way they approach to the path. The 3D-NLGL algorithm moves directly to the minimum distance point on the path (γdmin ), while the 3D-CC moves always to a δdistance from this point. This slight difference becomes significant on the final performance. From our experience, these oscillations on the geometric algorithms, and thus the convergence time, can be reduced by increasing the geometric control parameters (Land δ). However, this results in an increment of the path distance error too. The modified initial x-coordinate simulations also reflect that the control-oriented algorithms (BS and FL) obtain very similar stabilization times. Neverthelles, FL results in higher path distance errors because it converges to a further point on the path, as evidenced by the convergence path position plot (Fig. 4.5). The control-oriented algorithms, especially the FL algorithm, make a higher control effort to remain on the path (Fig. 4.5) than the geometric algorithms. Control effort results at x=3 should not be considered as some algorithms converged with as few as 2 or 3 time steps. Analysing the results in which the initial yaw orientation is varied, it can be seen again that the control-oriented algorithms obtain better performance than the geometric ones. 3D-NLGL and 3D-CC perform similarly in terms of convergence time and path position. That is because, 82 CHAPTER 4. CONTROL-ORIENTED AND GEOMETRIC PATH FOLLOWING when the vehicle is close to the path, the distinct behaviour between these two algorithms on the approach to the path takes no remarkable effect. Also, it is important to note that the control-oriented algorithms make a significantly larger control effort to correct the yaw angle than the geometric algorithms. That is due to the aggressive yaw control that the BS and FL algorithms produce. When the quadrotor deals with the effect of time-varying wind disturbances, represented in Fig. 4.11 and Table 4.3,FL algorithm performs worst. BS is the algorithm that handles best the external forces in terms of path distance error. Note that, apparently, the FL does not have sufficient strength to cope with wind, evidenced by its low average speed and the short distance along the path (γ=2.72) that it is able to cover, compared with the other algorithms. The yaw error of geometric algorithms is larger than the one of BS, while the path distance error is similar. From the results of the simulations with realistic flight conditions (Fig. 4.14), it can be seen that with a velocity reference of 1 m /sthe path distance error of the BS and 3D-CC algorithms is quite similar, but when the velocity reference increases, the gap between the algorithms increases and 3D-CC starts behaving worse. Regarding the yaw error, it behaves similarly in both algorithms, although it is always larger in 3D-CC. It is also observed that the average velocity of the vehicle is almost identical for both algorithms, having always nearly the same relative error. However, it is possible to notice that the time spent by the BS algorithm is slightly shorter due to its smaller path distance error. Table 4.5: Qualitative comparison of the path following algorithms. BS FL NLGL CC Path Distance Control 1 2 4 4 Yaw Control 1 4 2 2 Velocity Control 4 3 1 1 Convergence to the Path 1 2 4 3 Computational Effort 4 3 1 1 Wind Disturbance 1 4 2 2 Control Effort 3 4 2 1 Design & Tuning Effort 3411 Path Adaptability 3411 State Information Requirements 3 3 1 1 Model-Based 3 3 1 1 Domain of Attraction 1 4 2 2 Table 4.5 presents a comparison between the four algorithms summarizing the simulation result 4.2. ALGORITHM COMPARISON BASED ON SIMULATION RESULTS 83 analysis and other qualitative indicators. This table sorts from best (1) to worst (4) the four algorithms on each qualitative aspect. Regarding the design & tuning effort, the geometric algorithms (3D-NLGL and 3D-CC ) are easier to implement and they require only one parameter to tune. In contrast, the implementation of the control-oriented algorithms (BS and FL) is tedious since both present several complex derivatives and they have more parameters to tune. Furthermore, in the FL algorithm it is necessary to define a mathematical function (h3) that calculates the minimum distance point on the path (γdmin ) given the position of the vehicle. This function depends exclusively on the geometry of the path and no solution is guaranteed to exist for every path. The path adaptability defines the capability of each algorithm to adapt to different types of paths. The requirement of finding h3makes FL the worst for adapting to new path shapes. The BS algorithm is rated third since the defined path (pd(γ)) needs to be continuous and derivable. The geometric algorithms are again the best rated because they can be adapted to any path by changing only their specific control parameter. The control-oriented algorithms require the full state information, while the geometric ones (along with the PIDs of the autopilot) only require the position, attitude and the velocities on the xand yaxis. Furthermore, the design of the control-oriented algorithms is model-based, while it is model-free in the geometric algorithms. Because of this, they can be applied to different kinds of vehicles. Both qualitative aspects are reflected in Table 4.5. To end up, the domain of attraction of each algorithm is evaluated. The behaviour of the algorithms with the vehicle away from the path and in different initial conditions is analysed. Backstepping is global since it is based on the non-linear model and it includes a saturation on the position error term that results also in a saturation on the control actions. The BS algorithm is rated first on this characteristic since when the vehicle is far from the path and independently of the initial condition it is always able to converge to the path. Feedback Linearisation should be a global algorithm as well, since it is based on a full-state non-linear dynamic inversion. Nevertheless, the simulation results show that it is not really global, since it becomes unstable in specific initial conditions. That is, opposite to the BS case, the approach velocity of the FL algorithm grows unbounded as the distance to the path increases. Moreover, it gets unstable when the vehicle is in position x=0, y =0 since the third output (h3) is indeterminate. This is the reason why the transient experiments start on x=0.5mand not on x=0. For these reasons, the FL algorithm is rated worst in the domain of attraction aspect. Regarding the geometric algorithms, the NLGL has a domain of attraction defined by the distance L. When the vehicle is further away a special procedure must be performed, which may not assure stability. The CC is considered global, as the convergence to the path is assured for any position of the vehicle. Note that the analysis of the domain of attraction for the geometric algorithms depends exclusively on the position states, since the rest of states (velocity, orientation and angular velocity) are controlled by the autopilot. Chapter 5 Adaptive Geometric Path Following In Chap. (2) several control-oriented, geometric and learning-based algorithms for UAV path following are reviewed and compared. The most prominent and popular are implemented in a realistic quadrotor model in Chap. (4). Conclusions reveal that, in spite of its slightly worse performance in comparison with the control-oriented algorithms, geometrical algorithms are easier to implement, require less state information and result in a lower computational and control effort. Therefore, they become a wise solution for the path following problem. Geometric guidance algorithms were originally described in the missile guidance literature, but most of them were adapted to other type of vehicles, such as UGVs [122][70] or UAVs. Examples of geometric algorithms that are applied to UAVs are: NonLinear Guidance Law (NLGL) [172], Trajectory Shaping [137], Vector-field based [127] and Pure Pursuit [57]. Some of those geometric guidance algorithms have very few (1 or 2) parameters to tune. These parameters define the performance and the stability of the controller [172][70]. The choice of the proper values of those parameters depends on factors such as the velocity of the vehicle or the reference path shape, and needs to be done manually each time the experiments conditions vary. In [134] the authors adjust these parameters for different path following algorithms by means of an optimization procedure based on Genetic Algorithms. However, this optimization is performed off-line and needs to be redone when path conditions vary. The present chapter is focused on the study of the parameter selection for the NonLinear Guidance Law and Carrot-Chasing algorithms, and presents an adaptive approach based on the use of neural networks. That is, the parameters of the geometric algorithm are automatically generated (on-line) depending on the path shape and the vehicle’s velocity. Stability proofs of the proposed approach are given. The performance of the adaptive algorithms are assessed by numerical simulations. All the results and graphs reported in this chapter were obtained with the Path-Flyer quadrotor simulator, which implements a Separated, Guidance and Control structure (Fig. 3.7) along with 85 86 CHAPTER 5. ADAPTIVE GEOMETRIC PATH FOLLOWING the standard nonlinear dynamic equations. It also includes the dynamics of the motors and other aerodynamic effects, such as drag forces. In this simulator the autopilot (Section 3.3) is implemented by a set of PID controllers. More information of the Path-Flyer platform in Section 3.4. 5.1 Adaptive NLGL The aim of this section is to develop an adaptive strategy for the NonLinear Guidance Law (NLGL) applied to a quadrotor vehicle to solve the path following problem. The algorithm corresponds to the three-dimensional version of the NLGL developed in Section 4.1.3. 5.1.1 Performance of NLGL Depending on the Parameter Selection This subsection analyses the performance of NLGL as a function of parameter L. The mean absolute error (MAE) in terms of distance to the path, noted dhereafter, is used to evaluate the performance of the algorithm. The dynamics of the autopilot and the vehicle are not modified throughout the chapter. That is, it is not analyzed how having different inner dynamics affects on the optimal selection of L. Path distance MAE as a function of parameter L The simulation scenario is a circular path of radius Rand a constant velocity reference of 1 m /s. The vehicle starts in a point on the path at hover condition (i.e. zero linear and angular velocities) and it is requested to perform a full lap of the path. The average distance of the vehicle to the path (d) is measured in each simulation. Figure 5.1: MAE of don a full lap of a circular path varying L. Constant speed: 1m /s. Different radius: 1.5m, 3m and 20m. 5.1. ADAPTIVE NLGL 87 Fig. 5.1 shows the results for three different path radius (1.5m, 3m and 20m) when parameter Lis varied in each simulation by steps of 0.01m. Each point on the graph corresponds to a full lap simulation. The initial value of Lon each of the three cases, denoted with a square on the plot, is the first one that does not make the system unstable. That is, smaller values of Lmake the vehicle’s trajectory become unstable. Note that for each radius, there always exists a value of Lthat achieves the minimum error. In this section, this value is represented by Lopt, which stands for optimal L. Optimal Lvalue in function of vehicle’s velocity and path radius The results showing how Lopt changes with the vehicle’s reference velocity (Vref ) and the path radius (R) are presented in Fig. 5.2. The reference paths are circumferences again. Each point on the graph shows the value of Lopt for a given velocity reference and path radius. Lopt was obtained with an exhaustive discrete search (step of 0.01m). The behaviour of Lopt in function of Vref and Rcan be divided in three zones, represented by A,Band Cin Fig. 5.2. Zone C shows constant Lopt with regard to the path radius. The speed of the vehicle here is slow enough to assume the path as a straight line. Thus, the value of Lopt corresponds to the value of the optimal Lfor a straight path. Zone Bcorresponds to the regular behaviour of Lopt for a circular path. Finally, zone Ais the one where the vehicle moves too fast for the given path radius and it is not able to follow the path correctly. There is no value of Lthat makes the vehicle converge to the path and follow it, due to the kinematic constrains of the plant. AB C Figure 5.2: Lopt in function of the path radius and vehicle’s velocity. The average of the path distance error (d) achieved with Lopt when the vehicle is requested to perform a full lap starting from hover conditions is reported in Fig. 5.3. To make it clearer, a surface was chosen to represent this error, however, this plot is obtained with the same values 88 CHAPTER 5. ADAPTIVE GEOMETRIC PATH FOLLOWING Figure 5.3: MAE of dwith Lopt in function of the path radius and vehicle’s velocity. of Rand Vref of Fig. 5.2. From this plot, it is clear that the vehicle is unable to follow the path in Zone A, as evidenced by the very high path distance error exhibit on this zone. 5.1.2 Adaptive Parameter Selection As seen in the previous section, the selection of parameter Ldepends on the velocity of the vehicle and the radius of the path. To develop an adaptive approach for the NonLinear Guidance Law it is necessary to define a function (or algorithm) that computes the value of Lopt given the vehicle’s velocity and path radius. The output of this function has to be as similar as possible to the set of points obtained in Fig. 5.2. In this chapter a neural network (NN) is employed to fit a surface to the given set of points. This problem can also be solved using other types of approximations (polynomial, wavelets, etc.), however, since the shape of the surface to be fitted is similar to a sigmoidal function the NN seems to be the best option. The details of this NN are explained below. Neural network The network architecture consists in three hidden layers full connected in feedforward sequence. The inputs of the NN are the radius of the path and the velocity of the vehicle. The output is parameter Lopt. The hidden layers have 3, 6 and 3 neurons, respectively. The activation function of the neurons is sigmoidal, chosen mainly because of its resemblance with the surface required to fit. This architecture was found as the one that fits better the surface minimizing the overfitting problem. The training algorithm used is the Levenberg-Marquardt method with a regularization 5.1. ADAPTIVE NLGL 89 term. The regularization term is included to reduce the average generalization error, in other words, to avoid the overfitting problem. Fig. 5.4 shows the surface function obtained by the neural network with the training points denoted in red. These points correspond to the points of Zone Band Zone Cof Fig. 5.2. That is, points of Zone Awere removed, since this zone presents inaccurate performance for any value of L. Note that the real velocity (u) was used as input to the NN, instead of the reference velocity plotted in Fig. 5.2. That is because, despite these two velocities are quite similar in the analysed simulation scenarios, they can differ significantly in other circumstances, e.g., when the vehicle is required to move on the zdirection, due to a bad performance of the inner velocity controller or because of external disturbances. Figure 5.4: Neural Network surface and its training points (NLGL). Obtaining the path radius With the developed NN it is possible to obtain the optimal value of L, given the current x-body velocity of the vehicle and the radius of the path. The radius of a continuous parametrized path (pd, Definition 2.2.1), can be calculated with the inverse of the path curvature (k), given by k(γ)=1 R(γ)=∥dpd(γ) dγ ×d2pd(γ) dγ ∥ ∥dpd(γ) dγ ∥3(5.1) The value of γused to calculate k(or equivalently, R) must be chosen carefully. One could be tempted to use γdmin (i.e. γat minimum distance to the vehicle), but that is not a sensible choice. It would be equivalent, when driving a car, to steer it when you are already on the 96 CHAPTER 5. ADAPTIVE GEOMETRIC PATH FOLLOWING Table 5.1 shows MAE of the path distance, the total time and the average velocity obtained by three variants of the NLGL algorithm performing one lap on the lemniscate path. These variants are: the original NLGL, the proposed Adaptive NLGL and the Adaptive NLGL with the velocity reduction term. As shown in the results, the Adaptive NLGL with the velocity reduction is the one that presents the smallest distance error, but a slightly lower average velocity. It is important to highlight that the Adaptive NLGL presents a better performance than the regular NLGL. Furthermore, the Adaptive NLGL, as opposed to the standard NLGL, does not need any tuning of its parameters when the reference path is changed, which becomes the main advantage of this approach. Table 5.1: Results for one lap of the lemniscate path (Vref =1m /s). d(m)time (s) ∥v∥(m /s) NLGL 0.1282 32.8375 0.9339 Adaptive NLGL 0.1054 32.9414 0.9338 Adaptive NLGL + Vel. red. 0.0947 33.4749 0.9146 Fig. 5.11 shows the evolution in time of parameter Lcomputed by the Adaptive NLGL (green) and Adaptive NLGL with velocity reduction (red) when performing a full lap of the reference path. The velocity reduction term reduces the velocity in the curves which, at the same time, makes the algorithm reduce parameter L. Figure 5.11: Evolution of Lwith the Adaptive NLGL (green) and Adaptive NLGL with velocity reduction (red) (Lemniscate path, Vref = 1m /s). Spiral path The spiral path is defined by Eq. (5.9). 5.1. ADAPTIVE NLGL 97 pd(γ)= ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ γ/2cos(γ) γ/2sin(γ) γ+3 ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ (5.9) Fig. 5.12 shows the evolution in space of the regular NLGL (blue) and Adaptive NLGL with velocity reduction (red) following the spiral path with a velocity reference of 2m /s. Again, the L of the original NLGL was tunned to minimize path distance error for the given path and velocity. Figure 5.12: Trajectory of NLGL (blue) and Adaptive NLGL with velocity reduction (red) (Spiral path, Vref = 2m /s). A comparison of the obtained results with the three variants of the NLGL algorithm when following the spiral path is found in Table 5.2. This table shows the mean absolute error, the total time and the average velocity exhibited by the three variants when traveling from 0 rad to 6πrad of the spiral path. Again, the proposed Adaptive NLGL with the velocity reduction term provides the best performance and the simple Adaptive NLGL shows a better performance than the NLGL. Table 5.2: Results for one lap on the spiral path (Vref =2m /s). d(m)time (s) ∥v∥(m /s) NLGL 0.2205 53.2537 1.7899 Adaptive NLGL 0.1787 52.2592 1.7866 Adaptive NLGL + Vel. red. 0.0730 55.3999 1.6563 98 CHAPTER 5. ADAPTIVE GEOMETRIC PATH FOLLOWING The evolution of parameter Lcomputed by the Adaptive NLGL (green) and by the Adaptive NLGL with velocity reduction term (red) is shown in Fig. 5.13. The effect of the velocity reduction is mainly observed in the first 10 seconds, when the path radius is smaller. 0 10 20 30 40 50 60 time (s) 0 0.5 1 1.5 2 2.5 3 3.5 L(m) Figure 5.13: Evolution of Lwith the Adaptive NLGL (green) and Adaptive NLGL with velocity reduction (red) (Spiral path, Vref = 2m /s). 5.2 Adaptive Carrot-Chasing This section presents an adaptive version of the Carrot-Chasing algorithm that follows the same structure and methodology of the Adaptive NLGL approach introduced in the previous section. This approach is based on the three-dimensional version of this algorithm developed in Section 4.1.4. It is necessary to recall that this algorithm, just as the NLGL, only has one parameter to tune, that is the δdistance. 5.2.1 Adaptive Parameter Selection The process of obtaining the Adaptive CC approach is similar to the one described in Section 5.1. First, the optimal parameter of the Carrot-Chasing algorithm, δopt, in function of the radius of the path and the vehicle’s velocity is obtained by performing a set of simulations with a circumference path. The value of δopt in function of Vref and Ris presented in Fig. 5.14. This set of points is divided in three zones as in Fig. 5.14. These correspond to the Zone A, where the vehicle cannot follow the path correctly due to the kinematic constrains, the Zone B, where parameter δvaries in function of the path radius, and the Zone C, where the path radius is sufficiently large for a given high velocity so the algorithm behaves as if it were following a 5.2. ADAPTIVE CARROT-CHASING 99 straight line path. Fig. 5.15 represents the average distance error obtained with the optimal values of parameter δ. AB C Figure 5.14: δopt in function of the path radius and vehicle’s velocity. Figure 5.15: MAE of dwith δopt in function of the path radius and vehicle’s velocity. The values of δopt are approximated by a neural network of 3 feed-forward hidden layers of 3, 7 and 3 neurons, respectively. The inputs of the net are the velocity of the vehicle and the path radius. As the NN of the Adaptive NLGL approach, it uses a sigmoidal function as activation function, and it is trained with the Levenberg-Marquardt method with a regularization term. 100 CHAPTER 5. ADAPTIVE GEOMETRIC PATH FOLLOWING Fig. 5.16 shows the output surface of the NN with the training points highlighted in red. These points correspond to the points of zones Band Cof Fig. 5.14. Figure 5.16: Neural Network surface and its training points (Carrot-Chasing). Once again, an anticipation distance is used to evaluate the radius that is fed to the neural network. The optimal anticipation distance is obtained by performing several simulations with the path of Fig. 5.5 as described in Section 5.1.2. This set of optimal anticipation distances is approximated by a linear function of the path radius and the vehicle’s velocity (Eq. 5.10). This formula is used to generate an anticipation distance window with the maximum and minimum possible radius. The Algorithm 5is used to obtain the most restrictive radius of the path inside this anticipation distance window, which then is fed to the NN. da ∗=1.8099u−0.1535R+0.2034 (5.10) A velocity reduction term is included to avoid the algorithm to enter in the Zone Aof Fig. 5.14. Eq. (5.11) shows the maximum permitted velocity of the vehicle in function of the path radius. This function was obtained with the MatLab curve fitting tool approximating the edge between zones Aand Bof Fig. 5.14. Vpathmax =5.529 arctan(0.2779R−0.4759)+2.017 (5.11) 5.2. ADAPTIVE CARROT-CHASING 101 5.2.2 Results The results presented in this section compare the Adaptive CC algorithm with the Adaptive NLGL approach in realistic flight conditions simulations. Noise on the measurements are included. This noise is adjusted so that it emulates the observed in the real sensors. Furthermore, a realistic wind profile is generated. Similarly to the wind profile of Fig. 4.12, the magnitude of the wind disturbance is generated as a random walk of 10m /sin average and the direction is also a random walk with an average of π/4rad (i.e. +x,+ydirection). Again, results are obtained with the Path-Flyer benchmark. First, both adaptive approaches are tested with the lemniscate path of Eq. (5.8) at a velocity of 1m /s. The trajectories obtained by the proposed adaptive approaches following the stated path are shown in Fig. 5.17. Both approaches include the velocity reduction term. The results of these simulations are summarized in Table 5.3, where the cross-track error, the total time and the average velocity are evaluated. The two algorithms are able to follow the path correctly, presenting a similar performance. Nevertheless, the Adaptive NLGL is capable of achieving a slightly lower average distance error. Figure 5.17: Trajectory on the xy plane of Adaptive NLGL (blue) and Adaptive CC (red) in realistic flight conditions (Lemniscate path, Vref = 1m /s). Table 5.3: Results for one lap on the lemniscate path in realistic flight conditions (Vref = 1m /s). d(m)time (s) ∥v∥(m /s) Adaptive NLGL 0.1607 32.7287 0.9388 Adaptive CC 0.1738 31.3084 0.9557 Next, the adaptive algorithms are tested with a spiral path of constant height, Eq. (5.12), at a velocity reference of 2m /s. The trajectory performed by the adaptive approaches while following the spiral path is presented in Fig. 5.18. Table 5.4 compares the path distance error, the time to travel the path and the average velocity of the vehicle of the two algorithms. Again, the Adaptive NLGL approach achieves better performance than the Adaptive CC in terms of path following 102 CHAPTER 5. ADAPTIVE GEOMETRIC PATH FOLLOWING error. In this path, both algorithms present larger cross-track errors than in the lemniscate, especially in the first part of the path that has a very sharp curve. Nevertheless, in this part of the path both algorithms reduce their reference velocity to procure a better performance. pd(γ)= ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ 1.25γcos(γ) 1.25γsin(γ) 3 ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ (5.12) Figure 5.18: Trajectory on the xy plane of Adaptive NLGL (blue) and Adaptive CC (red) in realistic flight conditions (Spiral path, Vref = 2m /s). Table 5.4: Results for one lap on the spiral path in realistic flight conditions (Vref =2m /s). d(m)time (s) ∥v∥(m /s) Adaptive NLGL 0.3449 51.0562 1.8954 Adaptive CC 0.3753 51.1005 1.8946 More results comparing both algorithms and their standard versions can be straightforwardly obtained with the freely available and open Path-Flyer platform [142]. Chapter 6 Path Following with Deep Reinforcement Learning An adaptive version of the NonLinear Guidance Law (NLGL) was developed in Chap. (5). In that work neural networks were used to approximate the relation between the optimal control parameters of NLGL, the vehicle’s velocity and the path’s shape. Results showed that it outperforms the standard NLGL in different experimental conditions without requiring any retuning of its parameters. The main drawback of this approach is that it is an ad-hoc solution since it relies on approximations made from the collected data of the mathematical model. Therefore, if the vehicle’s model or the dynamics of the attitude controller change, it is necessary to carry out again a large number of simulations, extract the information of interest and adjust the data, which may become tedious. The limitations on the adaptability of the approach to different experimental conditions as well as the portability to different multirotor vehicles motivated the subsequent work. It was important, nonetheless, to preserve the control structure and the advantages of the geometrical algorithms. The emerging deep reinforcement learning theory appeared as a promising option to accomplish those objectives. In recent years, a significant progress has been made in the fields of reinforcement learning (RL) and deep learning. Thus, now RL is no longer constrained to discrete and small environments. Deep Q-Network (DQN) [125] and Deep Deterministic Policy Gradient (DDPG) [103] are two of the most popular deep RL algorithms. In DQN the inputs of the agent are images, while DDPG is especially designed for continuous state-action spaces. Both algorithms have been used to solve diverse computer science and engineering problems [29][165][194][177][100]. DDPG has been also implemented on a quadrotor vehicle to solve the landing problem [139] with successful results. Other quadrotor applications of deep reinforcement learning can also be found in the literature [86][92][97][124][135]. In this chapter three different approaches implementing the DDPG algorithm to solve the path following problem in a quadrotor are presented. The approaches implement the same structure and concept of the geometrical algorithms. That is, they use a separated control and guidance structure with an autopilot tracking the attitude and velocity commands. Each approach 103 104 CHAPTER 6. PATH FOLLOWING WITH DEEP REINFORCEMENT LEARNING emerges as an improved version of the preceding one. The first approach uses only instantaneous information of the path for solving the problem. The second approach includes a structure that allows the agent to anticipate to the curves. The third agent is capable to compute the optimal velocity according to the path’s shape. The agents are implemented in the tensorflow-python framework an trained in Gazebo-ROS using the RotorS simulator, a realistic multirotor simulator (Section 3.5). The agents are trained to deal with noisy sensor measurements and to perform well when the vehicle is far from the reference path. The resulting agents are implemented and validated in the Asctec Hummingbird experimental platform. 6.1 Problem Statement The aim of this chapter is to develop a deep reinforcement learning agent capable of solving the path following problem for a quadrotor vehicle. The agent must be capable of learning online from real experimental tests and must simplify the training process of the work presented in Chap. (5). This approach will follow the control structure of geometric algorithms, a Separated Guidance and Control (SGC) structure (Fig. 3.7). The agent must be able to work with continuous state-action spaces and also be portable to other multirotor vehicles. Moreover, this agent must compute the proper velocity of the vehicle which, according to the defined reward, best adapts to the shape of the given path. This agent will be implemented with the Deep Deterministic Policy Gradient algorithm. It will be trained in a simulated environment and tested experimentally. Fig. 6.1 shows the typical reinforcement learning structure adapted to the control structure of the geometric algorithms. In this structure the agent is the path following algorithm and the environment includes the autopilot controller, the path reference and the quadrotor’s environment. The reinforcement learning agent receives the current state and reward and computes the action that is sent to the environment. Path Following Algorithm Quadrotor Altitude & Attitude Controller Velocity Controller Autopilot Environment Agent Action State Reward Figure 6.1: Reinforcement learning structure adapted to the SGC structure. 6.2. DEEP DETERMINISTIC POLICY GRADIENT 105 6.2 Deep Deterministic Policy Gradient The deep reinforcement learning algorithm implemented in this work is the Deep Deterministic Policy Gradient. This algorithm is an improvement of the standard Deterministic Policy Gradient [164] algorithm including new concepts of deep learning theory. One of its major advantages is that it is able to provide good performance in large and continuous state-action space environments, which motivated its selection. Deep Deterministic Policy Gradient [103] is an actor-critic RL algorithm. It is off-policy since the policy that is being improved is different from the policy that is used to generate the action to compute the loss function. And it is model-free because it makes no effort to learn the dynamics of the environment. Instead, it estimates directly the optimal policy and value function. Fig. 6.2 shows a common structure of an actor-critic agent, where the policy (actor) is represented independently from the value function (critic). According to the learned policy function (µ(s)), the actor computes the optimal action depending on a state of the environment. The critic estimates the value function (Q(s, a)) given the state and the action. The value function gives us information of the expected cumulated future reward for this state-action pair. The critic is also in charge of calculating the temporal-difference error (TD) (i.e. the loss function) that is used on the learning process for both the critic and the actor. In deep reinforcement learning the policy function and the value function, actor and critic, are approximated by neural networks. Value Function Policy Environment Critic Actor Action State Reward TD error Figure 6.2: Actor-Critic agent structure. DDPG uses two characteristic elements of Deep-Q-Network [125]; the replay buffer and the target networks, which are used to stabilize the learning of the Q-function. A replay buffer is a finite sized memory that stores the transition tuple at each step. Fig. 6.3 shows the main elements of the transition tuple. This tuple is formed by the current state (si), the action (ai), the obtained reward (ri), the next state (si+1) and a boolean variable that indicates if the next state is terminal or not (ti). A terminal state is understood as a state where the experiment ends. At each timestep the critic and the actor are trained from a minibatch obtained by sampling random tuples of the replay buffer. This way of training reduces time correlation between learning samples and facilitates convergence in the learning process. 112 CHAPTER 6. PATH FOLLOWING WITH DEEP REINFORCEMENT LEARNING Figure 6.9: States of the second approach; angle error with respect forward tangential frame {T2}. included (ψT2). This state is an angle error between the vehicle’s yaw angle and the path’s tangential angle. However, in this case the angle error is not computed from the point pct but in a point that is forward on the path, as represented in Fig. 6.6. This new state gives information about future orientation of the path with respect to the vehicle and makes it possible for the agent to anticipate the curves to come, improving substantially its path following performance (see Section 6.6). The state vector of this approach is presented in Eq. (6.11), where T2subscript indicates that the state is computed from the tangential frame on a point that is forward on the path. s={yT, ψT, ψT2}(6.11) The distance at which the second tangential frame, {T2}, is placed on the path is named anticipation distance and it is represented by da. To obtain the best possible performance of the agent, it is necessary to choose a proper anticipation distance. From different tests, it was proven that dadepends on the velocity of the vehicle on the path. That is, with higher velocities it is necessary to have a larger anticipation distance. For instance, the optimal anticipation distance (according to the obtained PF performance) at a velocity of 1 m /sis 0.6m. 6.4.3 Third Approach: Adaptive Velocity The main drawback of the previous approaches is that, with the defined structure, the agent can only learn to solve the problem at one specific velocity. That is, if during the training process the velocity of the vehicle is changed every episode, convergence cannot be achieved. In other words, the policy depends on the vehicle’s velocity. This subsection presents an improvement that permits the agent to work at different velocities and also makes it capable of computing at each step the velocity of the vehicle that best adapts to the shape of the path according to the defined reward. 6.4. DDPG FOR PATH FOLLOWING 113 In order to have an agent that is resilient to different velocities and path’s shapes, the first step is to include the velocity of the vehicle (∥v∥) as a state of the agent. Nevertheless, this modification is not sufficient to accomplish our goal. In DDPG it is necessary to define a state vector that fulfils the deterministic property. This means that, knowing the current state vector and action, the next state can be estimated. Therefore, since the velocity of the vehicle is an exogenous variable of the system (defined by the user) it is not possible to predict its value, and thus, it is not a deterministic state. To make it deterministic, the action vector must act on the velocity state. In this third approach, in addition to the yaw correction action defined in Eq. (6.6), a new action that computes a velocity correction (ucorr,k) over the current velocity of the vehicle is included. Eq. (6.12) shows how the velocity command on the xaxis is produced from this action (including exploration noise of Eq. 6.7, only used during the training phase). Again, with the aim of avoiding fast changes on the velocity and to assure the stability of the system, a correction action has been used rather than a velocity action or an incremental action of the command. ucmd,k =(ucorr,k +nk j/λ+1)∆t+uk(6.12) Introducing this new state (∥v∥) and new action (ucorr,k) to the agent may seem to be enough to solve the problem. However, as mentioned in Section 6.4.2, notice that the path’s position where the future angle error state (ψT2) is computed depends on the velocity of the vehicle. Therefore, having only this state computed with a fixed anticipation distance (da) does not provide enough information to solve the path following problem at different velocities. To deal with this problem, two solutions were considered: Adding more future angle error states at different anticipation distances or modifying at each step the anticipation distance at which the angle error is computed in function of the vehicle’s velocity. Including several future angle states at different distances resulted disadvantageous for two reasons: first, having more states makes the training process much slower; second, since at a given velocity only the information of 1 or 2 future angle states is exploited, the remaining states become irrelevant. Having many states that do not provide significant information to solve the problem leads the agent to lose effectiveness. For this reason, in this approach the mentioned issue is solved by having only one future angle state (ψT2), which is computed with an anticipation distance adapted according to the vehicle’s velocity. Several tests at different velocities were performed in order to find the relation between the velocity of the vehicle and the optimal anticipation distance (da,opt). Optimal in the sense of being the distance that provides more information, and thus, results in a higher performance of the agent. The results obtained from these tests were approximated by the linear piecewise function shown in Eq. (6.13). This function computes the optimal anticipation distance as a function of the current velocity of the vehicle. 114 CHAPTER 6. PATH FOLLOWING WITH DEEP REINFORCEMENT LEARNING da,opt =⎧ ⎪ ⎪ ⎪ ⎨ ⎪ ⎪ ⎪ ⎩ 0.6∥v∥+0.1 if ∥v∥<1 ∥v∥−0.3 else (6.13) Summarizing, in this approach the velocity of the vehicle is added as part of the state vector and the future angle state is computed with an adaptive anticipation distance (da,opt). A velocity correction is included in the action vector. Eqs. 6.14 and 6.15 present the state and action vectors, respectively. The agent computes the velocity command on the xaxis in such a way that it adapts to the path’s shape. Velocity on the yaxis is still fixed to 0. The reward function of the first approach (Eq. 6.10) is also maintained. Weights of the reward (k1and k2) acquire a significant role in this approach, since they define the priority of the trade-off between having small path distance error or travelling at high velocities. All parameters of Table 6.1 are preserved except for the ratio of the exploration-exploitation transition (λ), which is set to 1000. This is because the training process of this approach is slower. s={yT, ψT, ψT2(da,opt),∥v∥} (6.14) a={ψcorr,k, ucorr,k}(6.15) The ingredients that make the agent capable of following a trajectory in space with adaptive velocity have been defined. Nevertheless, it is of utmost importance to design a rich training environment that allows the agent to converge to an efficient and robust solution. Details of this training process are given in Section 6.5. 6.5 Training Process The training process of the agents has been performed in the training environment detailed in Section 6.3.1. This training environment is integrated in a linux Xubuntu virtual machine with a dedication of 8GB RAM and four 1.80GHz processors (i7-8550U CPU). The training process is performed in real time. 6.5.1 Training of 1st and 2nd approaches The first and second approaches followed the same structure in the training phase. That is, the vehicle is required to follow a half lemniscate (8-shaped) path at a constant velocity of 1 m /sin the xbody axis (ucmd). This path is defined in Eq. (6.16), where Ais the radius of one of the circumferences of the path, fixed to 4m, and γis the virtual arc, which ranges from 0 to 2πrad. The path is discretized with a precision of 0.01m between each path point. 6.5. TRAINING PROCESS 115 xd(γ)=2Acos(γ) yd(γ)=Asin(2γ)(6.16) Both agents were trained following the specified path in ideal conditions, meaning that the system uses ground truth measurements and the vehicle starts each episode at the initial position of the path with the yaw angle oriented tangentially to it. As denoted in Table 6.1, each episode has 300 steps of 0.1 seconds. The learning evolution of the first and the second approaches are shown in Figs. 6.10 and 6.11, respectively. These figures show, for each episode, the average path distance error (∣d∣) and the accumulated reward (∑r) in all the steps of the episode. As the agents keep training the average error decreases and the accumulated reward grows until training converges. It is important to mention that, as the training process is stochastic, even if the same parameters and structure are maintained, the performance of the trained agents can vary. The agents presented in this chapter are the ones that achieve the best performance, in terms of path distance error, among a set of different trained agents that were obtained. In this particular case, the 1st approach converged around the 120th episode while the 2nd approach did it approximately at episode 90. Figure 6.10: Average distance error and accumulated reward on each episode during training phase of 1st approach agent (2 states). The resulting agents were tested in the RotorS simulation platform (see Section 6.6.1). They proved to perform well with ground truth measurements. However, if a model of the sensors is added, the agents present some difficulties to follow the path properly. Particularly, when the vehicle moves far from the path (due to drift or jumps on sensor measurements) and needs to converge back, the vehicle can start loitering around the path without being able to converge to it. 116 CHAPTER 6. PATH FOLLOWING WITH DEEP REINFORCEMENT LEARNING Figure 6.11: Average distance error and accumulated reward on each episode during training phase of 2nd approach agent (3 states). The solution to the mentioned problem could be to train the agent with the model with sensors. However, to capture the dynamics of the system with noisy measurements becomes challenging for the agent and, sometimes, training does not converge in these conditions. Alternatively, this issue is tackled by retraining the agents to learn the policy when the vehicle is far from the path. To do so, the agents are first trained as explained before, and then, they are retrained following the same path but starting at random positions and orientations different from the initial point of the path. In this way, the agents learn how to behave out of the path. Thus, if the vehicle occasionally moves out of the path because of the noisy sensor measurements, the agent will be able to drive the vehicle back to the path. Both agents (1st and 2nd approaches) were trained 100 more episodes following the specified lemniscate path (Eq. 6.16) with random initial conditions. That is, in each episode of this training phase the starting position of the vehicle is set at a distance of −2mto 2mfrom the initial position of the path, and the initial orientation is incremented an angle between −π/2to π/2radians from the initial path tangential angle. The initial position and angle are selected randomly with a uniform probability distribution in the defined intervals. Fig. 6.12 shows the learning evolution of the 2nd approach with the 100 new training episodes. Since the initial position and orientation change randomly in each episode, the obtained average distance error and accumulated reward also vary arbitrarily. For this reason, to show better the progression of this learning phase, a 20-values moving average is presented in both plots. That is, episode values are represented with gray dashed lines, while the moving average is represented with solid black lines in Fig. 6.12. The learning results show how this training phase permits the agent to learn to perform better in diverse initial conditions. This acquired knowledge will notably improve the performance in real experiments, as revealed in Section 6.6. 6.5. TRAINING PROCESS 117 Figure 6.12: Average distance error and accumulated reward on each episode during training phase with non-ideal initial conditions of 2nd approach agent; gray dashed lines are real values and black lines are a 20-episodes moving average. 6.5.2 Training of 3rd approach The 3rd DDPG approach developed in this work requires training in a richer environment than the previous versions. That is because the agent needs to train with different curves in order to learn the optimal vehicle’s velocity and the yaw angle’s policy according to the path radius. In the training process of this agent the vehicle will be required to follow an asymmetrical half lemniscate path. This is an 8-shaped path where each circle has a different radius. This path is defined in Eq. (6.17), where A1and A2are the radius of each circumference of the path, respectively. The value of this radius is changed every episode, taking a random value between 0.5mand 10mwith a uniform probability distribution. Again, the virtual arc parameter (γ) ranges from 0 to π/2rads, and the path is discretized with a precision of 0.01m. xd(γ)=⎧ ⎪ ⎪ ⎪ ⎨ ⎪ ⎪ ⎪ ⎩ 2A1cos(γ)if 0 ≤γ≤π/4 2A2cos(γ)if π/4<γ≤π/2 yd(γ)=⎧ ⎪ ⎪ ⎪ ⎨ ⎪ ⎪ ⎪ ⎩ A1sin(2γ)if 0 ≤γ≤π/4 A2sin(2γ)if π/4<γ≤π/2 (6.17) The first training attempts of the agent with the stated environment resulted to be quite unfruitful. Concretely, after hundreds of episodes, the agent just learned that the best way of maximizing the reward (reward function in Section 6.4) was to keep the vehicle static. The reason for this strange behaviour can be explained as follows: since turning around arbitrarily is not penalized when, due to the lack of exploration the policy is not defined yet, whenever the agent starts moving the vehicle forward, as it is rotating, it ends up moving in the opposite 118 CHAPTER 6. PATH FOLLOWING WITH DEEP REINFORCEMENT LEARNING direction of the path, receiving a penalty for that policy; therefore the best action is to keep ucmd =0. A simple but effective solution for such issue is proposed in this work. It consists on forcing the vehicle to move constantly by establishing a minimum velocity of 0.1m /s. Even if this condition initially produces negative rewards, it ends up promoting the agent to learn the policy of the yaw action. At the same time, as soon as the velocity vector of the vehicle starts to be parallel to the path, the agent can start learning that higher velocities lead to greater rewards. Hence, a successful learning process is achieved. The training results of this agent are shown in Fig. 6.13. This figure shows the average distance error (∣d∣), the average velocity on the xaxis (u) and the accumulated reward (∑r) on each episode. A 50-episodes moving average is applied to episode values to help the interpretation of each of the three plots. Again, gray dashed lines represent episode values while black lines show the moving average. Figure 6.13: Average distance error, average velocity and accumulated reward on each episode during training phase of 3rd approach agent; gray dashed lines are real values and black lines are a 50-episodes moving average. It may seem that training converged around episode 400. However, even the average error or reward appear to be constant, evaluating the trained agents with simulation tests showed that they kept learning and improving their performance until around episode 1000. The reason for that is because training more episodes permits to learn the policy on unusual states. 6.6. RESULTS 119 Such long and complex training process allows the agent to learn the policy out of the path. Thus, unlike the 1st and 2nd approaches, this approach does not need any additional training with diverse initial conditions to improve its performance on experimental results. 6.6 Results This section presents the results obtained with the three trained agents while following a path in different conditions. The agents were tested in simulation and experimentally with the Asctec Hummingbird platform. 6.6.1 Simulation The simulations presented in this section were performed in the same framework where the agents were trained. That is, the RotorS simulator integrated in the ROS-Gazebo platform. First, the three approaches were tested following a lemniscate path (Eq. 6.16), the same path used in the training phase. Again, the radius of the path is A=4m. However, this time the vehicle was required to follow a full lemniscate, with the virtual arc parameter, γ, ranging from 0 to 4πrads. The vehicle started at the initial point on the path with the yaw angle oriented tangentially to it. Table 6.2 shows the results obtained while following this path with ground truth measurements. That is, in the same conditions used for training. This table shows the average cross-track error (d), the average velocity (∥v∥) and the total time taken to perform a full lap of the path by each agent. Also, to evaluate the 3rd approach agent in the same conditions of the two other agents, another simulation was made with this agent limiting its maximum velocity to 1 m /s. Note that 1st approach is denoted as Agent 1 in the table, 2nd approach is Agent 2 and so on. This nomenclature is maintained hereafter in this section. Table 6.2: Results for one lap of the lemniscate path, simulations with ground truth measurements. d(m)time (s) ∥v∥(m /s) Agent 1 0.1041 67.10 0.8707 Agent 2 0.0398 54.79 0.8780 Agent 3 0.0671 39.81 1.2276 Agent 3 (vmax =1)0.0669 56.10 0.8696 The results performing a full lap of the lemniscate path while using the sensor measurements instead of ground truth values, are shown in Table 6.3. Same parameters and agents of Table 6.2 are evaluated. The trajectory on the xy plane followed by these agents is shown in Fig. 6.14. 120 CHAPTER 6. PATH FOLLOWING WITH DEEP REINFORCEMENT LEARNING Table 6.3: Results for one lap on the lemniscate path, simulations with sensor models. d(m)time (s) ∥v∥(m /s) Agent 1 0.1123 54.79 0.9476 Agent 2 0.0895 51.70 0.9484 Agent 3 0.0968 40.00 1.2338 Agent 3 (vmax =1)0.0816 54.41 0.9111 Figure 6.14: Trajectories on xy of lemniscate path, simulation with sensor models: Agent 1 in green (dotted line), Agent 2 in blue (dash-dotted line) and Agent 3 (dashed line) in red. Fig. 6.15 shows the references of yaw angle (ψcmd) and velocity in the xaxis (ucmd) computed by the Agent 3 and the values of the angle ψand the velocity uin the same simulation. As observed in the simulation results following the lemniscate path, the Agent 2 appears to be the one that obtains the best results in terms of cross-track error. However, it is important to recall that this agent was only trained to perform well at the particular velocity of 1 m /s. On the other hand, Agent 3 achieved a similar performance while reducing considerably the time taken to perform a full lap of the lemniscate. That is, this agent computes the optimal velocity at each part of the path, which allows the vehicle to accelerate in the straight lines, arriving at a maximum velocity of 1.82 m /s. Thus, it was able to increase the average velocity while maintaining almost the same error. To analyse the performance of the agents while following a different path from the one that was used to train them, a new path was defined. This new path is a spiral, stated in Eq. (6.18). This time, parameter Adetermined the rate at which the radius of the spiral grows and takes a value of 1.25. The virtual arc (γ) ranges from 0 to 2π. Table 6.4 shows the simulation results obtained while following the spiral path with ground truth measures, while Table 6.5 presents the results of the agents following the same path with sensors measurements. 6.6. RESULTS 121 Figure 6.15: Actions of Agent 3 following a lemniscate path, simulations with sensor models: references computed by agent (angle and velocity) in red and real values in blue (dashed line). xd=−Aγ cos(γ) yd=Aγ sin(γ)(6.18) Table 6.4: Results for one lap of the spiral path, simulations with ground truth measurements. d(m)time (s) ∥v∥(m /s) Agent 1 0.2907 34.30 0.8860 Agent 2 0.1840 32.43 0.8872 Agent 3 0.1418 23.52 1.2119 Agent 3 (vmax =1)0.0759 32.10 0.8530 Table 6.5: Results for one lap on the spiral path, simulations with sensor models. d(m)time (s) ∥v∥(m /s) Agent 1 0.3035 32.86 0.9448 Agent 2 0.2540 31.18 0.9366 Agent 3 0.1677 22.62 1.2262 Agent 3 (vmax =1)0.0987 30.59 0.8830