Keywords: planning

i Contenu de la Partie I Cette partie contient les preliminaires sur les différents outils utilisés dans cette vi thèse ainsi qu'un état de l'art sur les thèmes abordés.

Le Chapitre 2 apporte une introduction sur les quadrirotors et les principales techniques de contrôle, de planification et d'estimation d'état. La notion de platitude differentielle est également présentée, qui constitue une propriété fondamentale dans la planification de trajectoire. Enfin nous mettons en avant les difficultés rencontrées dans le contrôle basé vision.

Le Chapitre 3 apporte les techniques standard pour résoudre une problème de contrôle optimal.

Le Chapitre 4 présente la Commande Prédictive et des résultats préliminaires liés à des cas d'applications simples sur la génération de trajectoire avec un quadrirotor sont donnés.

Le Chapitre 5 apporte un large état de l'art sur les principales thématiques abordées dans cette thèse, à savoir, la planification en ligne, le contrôle basé vision et la génération de trajectoires optimales.

Contenu de la Partie II

.

Le Chapitre 6 présente notre contribution sur la planification basée vision en temps-minimum en présence de contraintes de visibilité.

Le Chapitre 7 introduit nos strategies de planification optimale pour répondre à l'évitement d'occultations et de collisions tout en suivant une cible mobile. Une formulation "assouplie" est proposée afin de permettre une re-planification en ligne.

Le Chapitre 8 présente notre algorithm de recherche dans un graphe que nous avons développé dans le but de trouver des trajectoires robustes en temps-minimum en présence de mesures visuelles intermittentes. Contenu de la Partie III Le Chapitre 9 apporte les conclusions de cette thèse et résume les principales contributions données. De plus, quelques enjeux ouverts sont listés et nous discutons des directions futures qui mériteraient d'être étudiées. vii L'Annexe A donne la transformation complète de la platitude différentielle et de son inverse pour le quadrirotor. L'Annexe B fournit des informations complémentaires sur les courbes B-spline et de plus amples détails sur les algorithmes de manipulation utilisés dans cette thèse. L'Annexe C inclut une introduction aux techniques numériques pour l'évaluation des dérivées et particulièrement la différenciation utilisant la partie imaginaire.

De la même façon que la recherche en robotique mobile à favorisé les robots à roues pour l'étude fondamentale du mouvement, parmi tous les robots aériens les quadrirotors sont considérés comme les plateformes les plus flexibles et versatiles pour la recherche en robotique aérienne ces 15 dernières années. Ces systèmes sont capables de réaliser des mouvements agiles et du vol stable dans l'espace tridimensionnel, ce qui offre les capacités idéales pour de nombreuses applications incluant en autres : la surveillance, la recherche et de sauvetage, la reconnaissance, le transport et l'inspection d'environnements complexes. Cependant les quadrirotors sont sujet à beaucoup plus d'incertitude que les robots mobiles terrestres (e.g., les incertitudes de modélisation, d'actionnement et de détection) et sont plus nettement plus sensibles aux perturbations extérieurs (e.g., les rafales de vent, l'interaction physique avec l'environnement et les autres systèmes). Ces défis ont en réalité encourager l'investigation sur des problèmes plus complexes liés à la planification et à la localisation en espace tridimensionnel, au contrôle, et à la détection. Il est bien connus que les quadrirotors possèdent une efficacité de vol bien plus limitée que les systèmes à voilure fixe. Désormais ces plateformes à coût modéré fournissent une autonomie de vol et une charge utile satisfaisante à de nombreuses applications en intérieur et en extérieur. De plus, les quadrirotors sont de plus en plus sérieusement considérés pour des applications commerciales (e.g., la livraison de colis, la publicité, les prises de vue aériennes), des applications de secours (e.g., livraison de premiers secours, lutte contre les incendies, l'analyse de zones de désastre). De plus, les quadrirotors sont le plus souvent équipés de capteurs de vision ce qui les propulsent au premier plan de l'inspection et la détection en milieu complexes et encombrés. Dotés de grandes capacités en terme d'agilité, les quadrirotors ont également fortement motivé la recherche en robotique aérienne à exploiter la dynamique de tels systèmes. La difficulté majeure réside dans le fait que ces systèmes sont sousactionnés (il n'y a que quatres moteurs pour contrôler les six degrés de liberté). Le iii

Objectifs et contributions de la thèse

La vision représente un des plus importants signaux en robotique. Une unique caméra monoculaire peut fournir des riches informations visuelles à une fréquence raisonnable pouvant être utilisées pour le contrôle, l'estimation d'état ou la navigation dans des environnements inconnus par exemple. Il est cependant nécessaire de respecter des contraintes visuelles spécifiques telles que la visibilité de mesures images et les occultations durant le mouvement afin de garder certaines cibles visuelles dans le champ de vision. Les quadrirotors sont dotés de capacités de mouvement très réactives du fait de leur structure compacte et de la configuration des moteurs. De plus, la vision par une caméra embarquée (fixe) va subir des rotations dues au sous-actionnement du système. Dans cette thèse nous voulons bénéficier de l'agi- Les contraintes visuelles sont donc critiques et difficiles dans un tel contexte. Dans cette thèse nous exploitons l'optimisation numérique pour générer des trajectoires faisables satisfaisant un certain nombre de contraintes d'état, d'entrées et visuelles nonlinéaires. A l'aide la platitude différentielle et de la paramétrisation par des B-splines nous proposons une stratégie de re-planification performante inspirée de la commande prédictive (MPC) pour générer des trajectoires lisses et réactives. Enfin, nous présentons un algorithme de planification en temps minimum qui supporte des pertes de visibilité intermittentes afin de naviguer dans des environnements encombrés plus vastes. Cette contribution porte l'incertitude de l'estimation d'état au niveau de la planification pour produire des trajectoires robustes et sûres. Les développements théoriques discutés dans cette thèse et corroborés par des simulations et expériences en utilisant un quadrirotor. Les résultats reportés montrent l'efficacité des techniques proposées.

Pour résumer, les travaux présentés dans cette thèse abordent les défis suivants : 1) l'exploitation du potentiel des quadrirotors en terme d'agilité pour réaliser des maneuvres agiles ; 2) l'incorporation de contraintes visuelles dans la planification afin de maintenir la visibilité de cibles visuelles ; 3) la re-planification efficace de trajectoires réactives sous de mulitples contraintes nonlinéaires ; 4) l'incorporation de l'incertitude de l'estimation d'état à l'étape de planification. v En réponses à ces objectifs nous proposons les méthodes listées ci-dessous qui ont méné à plusieurs publications dans des conférences internationales.

• La planification en-ligne de trajectoires en temps-minimum sous contraintes de visibilité.

Penin, Bryan and Spica, Riccardo and Giordano, Paolo Robuffo and Chaumette, François. "Vision-Based Minimum-Time Trajectory Generation for a Quadrotor UAV" in IEEE/RSJ Int. Conf. on Intelligent Robots and Systems, 2017.

• Une stratégie de planification pour le suivi réactif d'une cible visuelle tout en évitant les occultations et les collisions.

Penin, Bryan and Giordano, Paolo Robuffo and Chaumette, François, "Visionbased reactive planning for aggressive target tracking while avoiding collisions and occlusions" IEEE Robotics and Automation Letters, 2018.

• Un nouvel algorithme générant des trajectoires en temps-minimum en présence de mesures visuelles intermittentes pour un unicycle et un quadrirotor.

Penin, Bryan and Giordano, Paolo Robuffo and Chaumette, François, "Minimum-Time Trajectory Planning Under Intermittent Measurements" IEEE Robotics and Automation Letters, 2019.

Structure de la thèse

Cette thèse est divisée en trois parties principales. la première contient une introduction aux éléments liés au contrôle et à la planification pour les quadrirotors. Ensuite, les principaux outils utilisés dans nos travaux sont présentés suivis par un état de l'art dans les sujets abordés. La seconde partie met en lumière nos contributions dans les algorithmes de planification basés vision. Enfin la troisième partie contient les conclusions de la thèse et les futures axes de recherche possibles. Enfin, nous apportons des informations complémentaires sur quelques sections techniques.

Introduction

Quadrotors in robotics

In the same way research in robotic vehicle mobility favoured wheeled robots to derive fundamental results, among all Unmanned Aerial Vehicles (UAVs) quadrotors have been considered as the most flexible and versatile platforms worldwide for undertaking aerial research over the last 15 years. These vehicles are capable of agile motion and stable hovering in 3D space that offer ideal capabilities for many different applications including but not limited to: surveillance, search-and-rescue, reconnaissance, transport and inspection in complex environments. However, quadrotors are subject to much more uncertainty than ground vehicles (e.g., modelling, actuation and sensing uncertainty) and are more sensitive to external disturbances (e.g., wind gust, physical interaction with the environment or other robots). These challenges have actually enhanced the investigation on more complex research problems related to three-dimensional planning, control, localization and sensing. Although quadrotors are known to suffer from a much lower flight efficiency than fixed-wing aircraft, these low-cost platforms provide now sufficient flight endurance and payload for a number of indoor and outdoor applications and are now more and more seriously considered for commercial applications (e.g., package delivery, advertising, aerial photography) or emergency assistance (e.g., first-aid kit delivery Fig. 1.1a, Endowed with a special actuation configuration that allows extremely high motion capabilities, quadrotors are inherently prone to high speed and agile flights. Since several years, the research community has been developing new control and planning methods in the field of three-dimensional dynamic motion for systems with fast control loops such as quadrotors. Today, quadrotors have reached a very satisfying level of autonomy and reliability for fundamental research applications. Yet, the active research in aerial robotics is pushing the limits of planning, control and sensing to address more complex and agile tasks.

Moreover, computation improvements have also motivated the revision of already existing control and planning techniques (especially optimization-based methods) to the concept of aerial robots that can plan their motion online and quickly respond to changes in dynamic environments. Starting from this idea, many researchers have focused their effort on apprehending flight characteristics and proper-1.1. Quadrotors in robotics ties of motion itself (optimality, representation and especially the notion of smoothness 1 ) and their implications in control and planning for completing complex and reactive tasks such as aggressive grasping [START_REF] Thomas | Aggressive flight with quadrotors for perching on inclined surfaces[END_REF] or interception manoeuvres [START_REF] Hehn | Real-time trajectory generation for interception maneuvers with quadrocopters[END_REF]. Several testbeds such as [START_REF] Michael | The grasp multiple microuav testbed[END_REF][START_REF] Chamseddine | Flatnessbased trajectory planning for a quadrotor unmanned aerial vehicle test-bed considering actuator and system constraints[END_REF][START_REF] Ritz | Quadrocopter performance benchmarking using optimal control[END_REF] have originated from these fundamental studies to demonstrate the feasibility of new motions that are close to the actuation limits (and singularities) such as flips [START_REF] Lupashin | A simple learning strategy for high-speed quadrocopter multi-flips[END_REF], aerobatics [START_REF] Brescianini | Quadrocopter pole acrobatics[END_REF], swing manoeuvres [START_REF] Tang | Aggressive flight with suspended payloads using vision-based control[END_REF] or juggling [START_REF] Ritz | Cooperative quadrocopter ball throwing and catching[END_REF]. Originally composed of four propellers (see Fig. 1.2), quadrotors have been subject to numerous advanced mechanical design extensions depending on the aerial task to be performed. One can acknowledge overactuated variable-pitch quadrotors [START_REF] Cutler | Analysis and control of a variable-pitch quadrotor for agile flight[END_REF][START_REF] Ryll | A novel overactuated quadrotor unmanned aerial vehicle: Modeling, control, and experimental validation[END_REF] or aerial systems with tilting rotors (see e.g., [START_REF] Kendoul | Modeling and control of a small autonomous aircraft having two tilting rotors[END_REF]). Finally, there exists systems that can change their configuration: for a hexarotor in [START_REF] Ryll | Modeling and control of fast-hex: a fullyactuated by synchronized-tilting hexarotor[END_REF] or for a complex multi-body UAV in [START_REF] Zhao | Design, modeling, and control of an aerial robot dragon: A dual-rotor-embedded multilink robot with the ability of multi-degree-of-freedom aerial transformation[END_REF] (Fig. 1.3a). In a similar spirit [START_REF] Delamare | Toward aerial physical locomotion: The contact-fly-contact problem[END_REF] augments a quadrotor with a hooking system to enhance its motion capabilities and [START_REF] Zhang | Spidermav: Perching and stabilizing micro aerial vehicles with bio-inspired tensile anchoring systems[END_REF] with anchoring modules to extend its flight autonomy for instance Fig. 1.3b.

These platforms are mostly designed to facilitate physical interaction and navigation in cluttered environment. Yet, complex actuation leads to larger uncertainties and the complex control algorithms involved make them not mature enough for undertaking aggressive motions in 3D we are interested in.

In the course of this thesis, several fundamental works have arisen from the research community to demonstrate that quadrotors are capable of performing complex tasks while exploiting their full potential especially in terms of agility. Along these works, state estimation and sensing algorithms have been improved to cope with high speed motions. The sensory channel has not changed much but has surely improved due to continuing progress in technology. Quadrotors can now be equipped with complex vision sensors such as a lidar or a kinect. For instance [START_REF] Mohta | Experiments in fast, autonomous, gpsdenied quadrotor flight[END_REF] equipped a quadrotor with a nodding Hokuyo lidar, a second lidar serving as an altimeter and a high-resolution stereo camera to perform autonomous flights at impressive speeds up to 18m/s. Among all vision sensors, cameras are still the most preferred 1. Introduction (a) The multilink DRAGON system can change its shape for passing in narrow holes or carrying objects, from [START_REF] Zhao | Design, modeling, and control of an aerial robot dragon: A dual-rotor-embedded multilink robot with the ability of multi-degree-of-freedom aerial transformation[END_REF].

(b) The SpiderMAV uses an air-compressed module to shoot hooks attached with wires to stabilize itself, from [START_REF] Zhang | Spidermav: Perching and stabilizing micro aerial vehicles with bio-inspired tensile anchoring systems[END_REF]. ones and possess a long history in robotic control. New kind of cameras are even developed especially for these applications such as the event-based cameras [START_REF] Kueng | Low-latency visual odometry using event-based feature tracks[END_REF].

Since quadrotors have fewer independent control actuators than degrees of freedom (four motors for controlling six degrees of freedom) they belong to the large class of underactuated mechanical systems. Controlling such systems is challenging to the nonlinear control community especially in terms of stability and robustness. Developing controllers for these systems is clearly motivated by the mechanical gain procured by their simple mechanical structure. A extensive study of the control of underactuated systems can be found in e.g., [START_REF] Fantoni | Non-linear control for underactuated mechanical systems[END_REF].

Even though nonlinear controllers have been developed for quadrotors, stable and robust control is still challenging when the system has to undertake aggressive manoeuvres. This is due to the fact that the robot attitude is not negligible and aerodynamics become significant and are difficult to model and to incorporate in control. In this context, proofs of convergence and stability are much more laborious to establish. Nowadays, optimization-based planning methods appear to be more and more flexible and adapted for computing trajectories at the edge of the system motion capabilities for satisfying multiple (and possibly conflicting) goals while being subject to numerous (and possibly nonlinear) constraints.

Pioneered by [START_REF] Mellinger | Minimum snap trajectory generation and control for quadrotors[END_REF] (Fig. 1.4a) in 2011 very recent works show how optimization techniques are able to produce aggressive flight modes based on the generation of feasible and smooth trajectories [START_REF] Falanga | Aggressive quadrotor flight through narrow gaps with onboard sensing and computing using active vision[END_REF] demonstrated that quadrotors can even undertake agile motions in a complete autonomous way using vision as principal feedback Fig. 1.4b. All the papers cited in this section were published in 2016 at the earliest and shown in Fig. 1. [START_REF]Minimum-time trajectory generation under intermittent measurements[END_REF] and Fig. 1.5. This shows the current interest that 1.2. Challenges has sparked in the field of agile manoeuvres.

(a) Smooth and fast flights between hoops, from [START_REF] Mellinger | Minimum snap trajectory generation and control for quadrotors[END_REF].

(b) Autonomous agile passage through a window using visual feedback, from [START_REF] Falanga | Aggressive quadrotor flight through narrow gaps with onboard sensing and computing using active vision[END_REF]. [START_REF] Thomas | Aggressive flight with quadrotors for perching on inclined surfaces[END_REF]) and agile swing manoeuvres with a suspended load (right figure from [START_REF] Foehn | Fast trajectory optimization for agile quadrotor maneuvers with a cable-suspended payload[END_REF])

Challenges

Quadrotors are under-actuated systems since they have four control inputs to control their six degrees of freedom. Such a simple configuration is desirable for performing agile motions but at the cost of shifting the difficulty to the control and the planning schemes. Literature flourishes with contributions on these topics. Most of the works consider applications requiring near-hovering flights and low speed motions that allow the use of much simpler control schemes. As the state-of-the-art in Chapt. 5 will show, recent works have been dedicated to pushing the quadrotors flight limits in order to perform agile manoeuvres in more complex scenarios. Now, although sensors are growing in accuracy and processors are becoming more powerful, reliable estimation of the robot state is still challenging knowing it is crucial in this context. To the latter purpose but also for designing control schemes, the use of cameras has been very popular in robotics since they can provide rich information by the observation of some visual features present in the scene. However, when attached to a quadrotor the underactuation may severely affect the visual perception since the camera will undergo possibly large rotations. For these reasons, aggressive control of quadrotors should account for the capacity of the visual feedback to provide reliable information.

Figure 1.6 represents a quadrotor with a down-facing camera that needs to move in the right direction while using the red dot on the ground as visual feedback for a visual-based control scheme (such as visual servoing [START_REF] Chaumette | Visual servo control. I. Basic approaches[END_REF]). Since the commanded velocities are defined in the image plane, in order to move in the desired direction, the robot must necessarily rotate clockwise so as to correctly orient the thrust force generated by the propellers. While doing so, the field of view of the camera will also move and the robot might lose visibility of the target. Guaranteeing visibility of the visual features is of paramount importance since loosing visual tracking leads to an increasingly poor state estimation (that would just be driven by the odometry, i.e., the onboard IMU) and, thus, possibly, to a controller/task failure. Furthermore, when performing agile flights close to the physical limits of a quadrotor, motors might saturate, which leads to an inability to control the four independent degrees of freedom. A proper choice of the dynamic constraints and considerations on some motion properties such as smoothness are the common ingredients for planning feasible trajectories that can be accurately tracked by the real system.

Failure of the task can also be caused by the magnitude of modelling errors and disturbances that are boosted at high speeds. Robustness is of paramount importance in that case and can be achieved by efficiently adapting the trajectory on-line according to changes of the environments or to the tracker response. A 1.3. Thesis contributions second reasoning (among others) is to model or identify the unknown variables and aspects in order to be directly incorporated in control. These techniques will help absorbing the uncertainties acting inside or on the system. This thesis tackles the following challenges:

• The exploitation of the potential of a quadrotor in terms of agility to perform agile manoeuvres.

• The incorporation of a collection of vision-based constraints in planning for maintaining visibility.

• The efficient re-planning of reactive trajectories subject to multiple constraints.

• The incorporation of the state estimation uncertainty at the planning stage.

Thesis contributions

More precisely, this thesis focuses on developing real-time trajectory generation algorithms for undertaking aggressive motions while satisfying a collection of complex constraints with a particular care for visual perception. We rely on already existing trajectory controllers running at a high frequency for accurately tracking the optimal trajectories. Nevertheless, the design of such trajectories incorporates motion aspects that are beneficial for the tracker performance. These strategies use a Receding Horizon Control (RHC) approach for modifying online the reference trajectory in order to account for noise, disturbances and any other non-modelled effect. We are mostly interested in visual perception, therefore the presented planning strategies targets visual constraints for maintaining visibility and avoiding occlusions by obstacles present in the environment. Indeed, quadrotors can estimate their state by collecting visual measurements from targets that have to remain visible during motion. The presented planning methods rely on efficiently solving nonlinear optimal control programs and are applied to the tracking of a moving target and navigation. This thesis also presents a contribution in uncertainty-aware planning under intermittent measurements collected from vision. The goal is to relax the visibility constraints that can be very restrictive for navigating in large environments. The contributions are listed below:

• An on-line re-planning algorithm for generating minimum-time trajectories under visibility constraints.

• A reactive re-planning strategy for aggressive target tracking while avoiding occlusions and collisions.

Introduction

• A novel graph-search planner for finding robust minimum-time trajectories in the presence of intermittent visual measurements for a unicycle and a quadrotor.

Our work led to the following contributions:

Thesis structure

This thesis is divided into three main parts. The first part (Part I) contains an introduction to the ingredients for control and planning for quadrotors. Then, the main tools used in our works are presented along with a state of the art in the related topics. The second part (Part II) highlights our contributions about optimizationbased planning algorithms. The results illustrated in this part correspond to the following publications of the candidate [START_REF] Penin | Vision-based minimum-time trajectory generation for a quadrotor uav[END_REF][START_REF] Penin | Vision-based reactive planning for aggressive target tracking while avoiding collisions and occlusions[END_REF] and [START_REF]Minimum-time trajectory generation under intermittent measurements[END_REF]. The third part (Part III) contains the thesis conclusions and future directions. Finally, we report complementary information corroborating a few technical sections.

Outline of Part I This part contains preliminaries on the different tools and techniques involved in this thesis and a state-of-the-art on the related topics.

Chapter 2 gives an introduction to the quadrotor system and an overview of classic control, planning and state estimation techniques. It also introduces the notion of differential flatness which is a fundamental property in trajectory planning. Finally, we bring to light the issues related to vision-based control.

Chapter 3 provides standard techniques for solving an optimal control problem.

Thesis structure

Chapter 4 introduces Model Predictive Control and preliminary results of simple applications to trajectory generation with a quadrotor are presented.

Chapter 5 gives an extensive state of the art for the topics of online planning, vision-based control and optimal aggressive trajectory generation.

Outline of Part II This part contains the author contributions

Chapter 6 provides our contributions on minimum-time vision-based planning in the presence of visibility constraints.

Chapter 7 presents our planning frameworks for addressing collisions and occlusions avoidance while tracking a moving target. A relaxed formulation is given for allowing reactive re-planning.

Chapter 8 introduces the uncertainty-aware graph-search algorithm that we developed for finding robust minimum-time trajectories in presence of intermittent visual measurements.

Outline of Part III Chapter 9 reports the conclusions of the thesis and the main contributions brought to the state of the art are summarized. Moreover, some open issues are listed and we discuss future directions which would be worth exploring.

Part I

Preliminaries and related works

Chapter 2

Planning and control of a quadrotor UAV

Introduction

Our focus in this chapter is on the modelling of the quadrotor dynamics and on the role of controllers and trajectory generation.

Because of the nonlinear dynamic behaviour, the control and guidance of quadrotors remain subjects of active research, especially in applications covering searchand-rescue, surveillance, inspection, etc. For these applications, high stability, high precision hovering ability, high bandwidth, and high manoeuvrability are important.

Quadrotors have been widely adopted as experimental platforms for research in flying robotics. Reasons for the popularity of these vehicles include the ability to hover, mechanical simplicity and robustness, and their exceptional manoeuvrability due to typically high thrust-to-weight ratios explained by the relatively large offcenter mounting of the propellers. Such a configuration offers very low rotational inertia, thus allowing large translational accelerations and extraordinarily fast rotational accelerations (when free of other body parts or payloads). These motion capabilities authorize complex and agile manoeuvres that have been demonstrated in [START_REF] Mellinger | Trajectory generation and control for precise aggressive maneuvers with quadrotors[END_REF][START_REF] Ferrin | Differential flatness based control of a rotorcraft for aggressive maneuvers[END_REF][START_REF] Landry | Aggressive quadrotor flight through cluttered environments using mixed integer programming[END_REF][START_REF] Falanga | Aggressive quadrotor flight through narrow gaps with onboard sensing and computing using active vision[END_REF] for instance. Moreover, having four propellers of small diameter reduces the damage in case of collision with an obstacle due to their low kinetic energy. This makes it safer to navigate in narrow and cluttered environments. Some works even studied recovery flight modes for quadrotors in case of complete loss of one to three propellers [START_REF] Mueller | Stability and control of a quadrocopter despite the complete loss of one, two, or three propellers[END_REF].

Planning and control of a quadrotor UAV

Quadrotor model

Now let us derive the general equations of motion for the quadrotor. With reference to Fig. 2.1, let us define a world frame W " te 1 , e 2 , e 3 u (being e i the i-th column of the identity matrix) and a body frame B " tx B , y B , z B u with fixed origin O B attached to the center of mass (COM) and axis z B parallel to the propeller rotational axes. The configuration manifold is the special Euclidean group SE(3), which is the semi-direct product of R 3 and the special orthogonal group SO(3)

" � R P R 3ˆ3 | R T R " I, det R " 1 (
. Let us also assume, without loss of generality, that the robot COM corresponds to the barycentre of the propellers. The robot state is

χ " ¨rB W R B v B B ω BW ‹ ‹ ‹ ‹ ' P SEp3q ˆR6 (2.1)
where r B P R 3 is the position of the robot COM in W, W R B P SO(3) is the rotation matrix from W to B, v B the COM linear velocity expressed in W and B ω BW the angular velocity expressed in B.

In some cases, for more clarity, we will also use the roll, pitch and yaw (RPY) angles to represent the orientation of the robot. The rotation matrix corresponding to a given RPY configuration is given by:

W R B pφ, θ, ψq " R z pψqR y pθqR x pφq " ¨cψ c θ c ψ s θ s φ ´sψ c φ c ψ s θ c φ `sψ s φ s ψ c θ s ψ s θ s φ `cψ c φ s ψ s θ c φ ´cψ s φ ´sθ c θ s φ c θ c φ ‹ ' (2.2a) (2.2b) 2.2. Quadrotor model
It is also immediate to verify that the inverse transformation is given by: θ " Arctan2 ˆ´r 31 ˘br 2

32

`r2 33 φ " Arctan2 pr 32 , r 33 q ψ " Arctan2 pr 21 , r 11 q

(2.3a)

(2.3b) (2.3c)
where r ij indicates the component on the i-th row and j-th column of W R B . The transformation has a singularity of representation for cospθq " 0.

As known, the derivative of the rotation matrix is given by:

W 9 R B " W R B B Ω BW (2.4)
where B Ω BW is the skew-symmetric matrix built with the components of B ω BW . More specifically, assuming that

B ω BW " ¨ωx ω y ω z ‹ ' (2.5)
we have

B Ω BW " ¨0 ´ωz ω y ω z 0 ´ωy ´ωy ω x 0 ‹ ' (2.6)
The map that relates B ω BW to the corresponding skew-symmetric matrix B Ω BW is often called hat-map. Its inverse typically takes the name of vee-map. The angular velocity of the robot is also related to the vector of roll, pitch and yaw angles derivatives, indeed

B ω BW " R x pφq T ¨9 φ 0 0 ‹ '`Rxpφq T R y pθq T ¨0 9 θ 0 ‹ '`Rxpφq T R y pθq T R z pψq T ¨0 0 9 ψ ‹ ' (2.7) then B ω BW " Tpθ, φq ¨9 φ 9 θ 9 ψ ‹ ' (2.
‹ ' B ω BW (2.11)
As it is well known, each of the four propellers produces a force of modulus f i along z B and a torque of modulus τ i about z B . Both can be modelled in first approximation as proportional to the square of the motor rotational speed ω i

f i " kω i 2 τ i " bω i 2 (2.12a) (2.12b)
where k and b are the thrust and drag factors respectively. They are both positive and their value depends on the shape of the propellers.

We also introduce the following input transformation:

¨u1 u 2 u 3 u 4 ‹ ‹ ‹ ‹ ' " ¨k k k k 0 kl 0 ´kl ´kl 0 kl 0 b ´b b ´b ‹ ‹ ‹ ‹ ' ¨ω1 2 ω 2 2 ω 3 2 ω 4 2 ‹ ‹ ‹ ‹ ' " Aũ (2.13)
where l is the distance between the rotor axes of rotation and the geometric center of the quadrotor. The matrix A has always maximum rank, then the transformation is also invertible. The transformed input vector comprises the total thrust force u 1 along z B and the torques u 2 , u 3 , u 4 around x B , y B and z B respectively.

Having said that, the forces acting on the system are the gravity force directed along z B " e 3 and the total thrust force generated by the propellers and directed along z B . We also assume that the robot center of mass is coincident with its geometric center, where the total thrust is applied. With these assumptions, the translational dynamics of the system is given by the following Newton's equation:

m: r B " ´mge 3 `u1 z B (2.14)
where m is the robot mass and g P R 3 the (constant) gravity acceleration in the world frame. The angular acceleration is governed by the Euler's equation. Since the gravity force is applied to the robot center of mass, the only torque acting on the system is the one generated by the propellers, hence

J B 9 ω BW `Bω BW ˆJ B ω BW " ¨u2 u 3 u 4 ‹ ' (2.15)
2.3. General control and trajectory generation techniques for a quadrotor where J P R 3ˆ3 is the inertia tensor. If the robot is assumed to have a perfect cylindrical symmetry with respect to the axis z B , the inertia matrix is also diagonal and two of its eigenvalues, namely J xx and J yy , are equal. This makes it possible, if desired, to neglect the gyroscopic term B ω BW ˆJ B ω BW without introducing large modelling errors (see for example [START_REF] Mellinger | Trajectory generation and control for precise aggressive maneuvers with quadrotors[END_REF]).

Finally, we define the quadrotor dynamics with simplified notation as

9 r " v 9 v " g ´f m z B 9 R " Rrωs 9 ω " J ´1prJ ωs ˆω `τ q (2.16a) (2.16b) (2.16c) (2.16d)
where r¨s ˆthe usual skew-symmetric operator, R " W R B and pf, τ q P R 4 are the total thrust and torques applied by the propellers, which can be expressed as a set of control inputs u in terms of the individual propeller thrusts u " pf 1 , f 2 , f 3 , f 4 q P R 4 with the linear expression (2.13).

Most of agile control methods have proved that high performance flights can be performed with the present quadrotor model [START_REF] Mellinger | Trajectory generation and control for precise aggressive maneuvers with quadrotors[END_REF][START_REF] Richter | Polynomial trajectory planning for aggressive quadrotor flight in dense indoor environments[END_REF][START_REF] Ferrin | Differential flatness based control of a rotorcraft for aggressive maneuvers[END_REF]. Nowadays, efforts seem to be less and less allocated to the development of more accurate dynamical modelling (the above equations of motion are approximate, see e.g., [START_REF] Bouabdallah | Full control of a quadrotor[END_REF] for a more precise modelling). It seems these issues have reached an adequate level of maturity and do not need further major improvements, at least concerning standard applications. Incorporating the motor dynamics would add a fifth order to the dynamics without significantly improving the performance. Modelling simplifications are even more and more considered (especially on the inputs constraints) in order to face with more complex and higher-level tasks applications [START_REF] Hehn | Real-time trajectory generation for interception maneuvers with quadrocopters[END_REF][START_REF] Mueller | A computationally efficient motion primitive for quadrocopter trajectory generation[END_REF][START_REF] Mueller | A model predictive controller for quadrocopter state interception[END_REF]. Since one seeks to exploit the quadrotor's agility, it would be reasonable to consider aerodynamic effects, which become consistent when small aerial vehicles reach high velocities. However, these effects are rather complex to model and to incorporate into the control. For these reasons we choose to neglect any aerodynamic effect, entrusting the control action for their compensation. A philosophy largely exploited in control in robotics (more details are given in the following section).

General control and trajectory generation techniques for a quadrotor

Although quadrotors have a low mechanical complexity their control is still challenging. The major difficulty lies in the system underactuation, i.e., the coupling 2. Planning and control of a quadrotor UAV between the translational and rotational motions (2.16b). Since the number of independent inputs is less than its degrees of freedom some trajectories are not reachable making it difficult to find feasible trajectories and then design reasonable tracking control laws.

Quadrotor control

The most common nonlinear control techniques used to control quadrotor are backstepping [START_REF] Mokhtari | Feedback linearization and linear observer for a quadrotor unmanned aerial vehicle[END_REF], integral backstepping [START_REF] Jasim | Integral backstepping controller for quadrotor path tracking[END_REF], sliding-mode control [START_REF] Perozzi | On sliding mode control design for uav using realistic aerodynamic coefficients[END_REF], feedback linearization [START_REF] Al-Hiddabi | Quadrotor control using feedback linearization with dynamic extension[END_REF] and combination of these methods [START_REF] Jia | Integral backstepping sliding mode control for quadrotor helicopter under external uncertain disturbances[END_REF]. Because of the highly nonlinear dynamics most of the works in the area use controllers that are derived from linearisation of the model around hover conditions [START_REF] How | Real-time indoor autonomous vehicle test environment[END_REF]. Stability can be guaranteed for reasonably small roll and pitch angles [START_REF] Hoffmann | Quadrotor helicopter trajectory tracking control[END_REF]. These simplifications lead to neglecting the underactuation and alleviate the equations of motion to derive stability and convergence proofs.

Besides these common control schemes, several other control methods from the optimal control theory have been proposed in the literature for quadrotor control such as Linear Quadratic Regulator control (LQR) [START_REF] Reyes-Valeria | Lqr control for a quadrotor using unit quaternions: Modeling and simulation[END_REF], Model Predictive Control (MPC) [START_REF] Ganga | Mpc controller for trajectory tracking control of quadcopter[END_REF].

At a lower level, a common architecture for underactuated control consists in a two-loop design [START_REF] Voos | Nonlinear control of a quadrotor micro-uav using feedbacklinearization[END_REF][START_REF] Bouabdallah | Backstepping and sliding-mode techniques applied to an indoor micro quadrotor[END_REF], where the outer loop is the position control and the inner loop provides attitude (roll, pitch and yaw) control, as illustrated in Fig. 2.2. The outer loop typically implements a PD control law on position and velocity with feed-forward terms to compensate for gravity and accelerations from a reference trajectory. A desired acceleration is computed and mapped to the desired collective thrust and a desired attitude of the simplified quadrotor model. The inner loop controls the attitude together with the altitude on the assumption that the attitude dynamics of a quadrotor are much faster than its position and velocity dynamics. Such a control strategy provides almost asymptotical stability. In practice a two cascaded control loop is often used instead of a single one for controlling the attitude, e.g., in [START_REF] Kumar | Path following for quadrotors[END_REF][START_REF] Brescianini | Nonlinear quadrocopter attitude control[END_REF]. The reason is that it is more practical to separate the onboard processing on two independent units. One handles the state estimation and highlevel control on the body rates while the second processing unit runs a low-level controller on the attitude.

Nevertheless, there are no stability and convergence guarantees when the attitude of the vehicle deviates substantially from hover conditions. These properties can be stated in the design of only a few controllers used for tracking these trajectories. [START_REF] Sreenath | Dynamics, control and planning for cooperative manipulation of payloads suspended by cables from multiple quadrotor robots[END_REF] designs a nonlinear geometric controller in SO(3) which is almost global exponential stability for the load attitude tracking, and almost global exponential attractivity of the load position tracking. In [START_REF] Mellinger | Minimum snap trajectory generation and control for quadrotors[END_REF] the system is underactuated; however, it is possible to design controllers that guarantee convergence from almost any point on SE [START_REF] Penin | Vision-based reactive planning for aggressive target tracking while avoiding collisions and occlusions[END_REF]. An other appropriate controller for aggressive manoeuvres is the one proposed by [START_REF] Lee | Geometric tracking control of a quadrotor uav on se (3)[END_REF] that developed a nonlinear tracking controller on SE(3) and shown to have exponentially stable dynamics and almost globally exponential attractiveness of the complete dynamics (under some conditions and precise knowledge of the inertial parameters). This controller has been used for the simulations and the experiments presented in this thesis.

Control techniques can offer reasonably fast motions [START_REF] Ferrin | Differential flatness based control of a rotorcraft for aggressive maneuvers[END_REF][START_REF] Foehn | Onboard State Dependent LQR for Agile Quadrotors[END_REF] but generally lead to severe sub-optimalities when the system is subject to multiple nonlinear constraints. In this case, it appears to be more attractive to produce trajectories that fulfil a set of boundary conditions and dynamic constraints, specific properties, and optimal criterion and then fed to an accurate trajectory tracker. Some work in this area has addressed complex and agile tasks such as aerobatic manoeuvres [START_REF] Mellinger | Trajectory generation and control for precise aggressive maneuvers with quadrotors[END_REF][START_REF] Mellinger | Minimum snap trajectory generation and control for quadrotors[END_REF] and ball catching [START_REF] Mueller | A computationally efficient motion primitive for quadrocopter trajectory generation[END_REF]. Similar problems have been addressed using MPC [START_REF] Kim | Nonlinear model predictive tracking control for rotorcraft-based unmanned aerial vehicles[END_REF][START_REF] Yu | Comparison of nonlinear control design techniques on a model of the caltech ducted fan[END_REF]. With these approaches, guarantees of convergence are only available when the linearised model is fully controllable [START_REF] Yu | Comparison of nonlinear control design techniques on a model of the caltech ducted fan[END_REF] or if a control Lyapunov function can be synthesized [START_REF] Jadbabaie | On the stability of receding horizon control with a general terminal cost[END_REF]. As such it appears to be difficult to directly apply such techniques to the trajectory generation of a quadrotor. Learning algorithms have been successful in learning models [START_REF] Abbeel | Apprenticeship learning and reinforcement learning with application to robotic control[END_REF], agile motions [START_REF] Lupashin | A simple learning strategy for high-speed quadrocopter multi-flips[END_REF] or stabilization policies [START_REF] Hwangbo | Control of a quadrotor with reinforcement learning[END_REF] using data from simulated and real world. Although very promising, these approaches do not appear to lend themselves (yet) to more general motion planning or trajectory generation, such as in environments with obstacles for instance.

Trajectory generation for a quadrotor

Once trajectory tracker algorithms are designed, the problem shifts to the higher level of task definition often assimilated into trajectory planning which is devoted to generate the reference inputs for the trajectory tracker.

Planning and control of a quadrotor UAV

In applications seeking agile manoeuvres, it is necessary to develop flight plans that leverage the dynamics of the system instead of simply viewing the dynamics as a constraint on the system. It is necessary to relax small angle assumptions and allow for significant excursions from the hover state. A recent focus has been the planning and following of trajectories that exploit the dynamical capabilities of these vehicles. Results include algorithms that plan trajectories from classes of motion primitives [START_REF] Hehn | Real-time trajectory generation for interception maneuvers with quadrocopters[END_REF][START_REF] Mueller | A computationally efficient motion primitive for quadrocopter trajectory generation[END_REF], while others solve an optimal control problem for approximate or full vehicle dynamics (e.g. for minimum snap [START_REF] Mellinger | Minimum snap trajectory generation and control for quadrotors[END_REF] or minimum time [START_REF] Hehn | Performance benchmarking of quadrotor systems using time-optimal control[END_REF]).

Robust trajectory tracking is crucial especially for high speeds and accelerations. In [START_REF] Faessler | Differential flatness of quadrotor dynamics subject to rotor drag for accurate tracking of high-speed trajectories[END_REF] the authors chose to consider a more accurate dynamical model in the controller by incorporating the motor dynamics and aerodynamic drag effects. Indeed, identifying the model parameters and external disturbances will improve the controller performance. Similarly [START_REF] Svacha | Improving quadrotor trajectory tracking by compensating for aerodynamic effects[END_REF][START_REF] Kai | Nonlinear feedback control of quadrotors exploiting first-order drag effects[END_REF] developed a controller compensating for aerodynamic effects and especially the drag. [START_REF] Svacha | Improving quadrotor trajectory tracking by compensating for aerodynamic effects[END_REF] was able to exhibit lower position errors even at flight speeds up to 18m/s in [START_REF] Mohta | Experiments in fast, autonomous, gpsdenied quadrotor flight[END_REF].

It is also important to account for the tracking precision and energy consumption when designing aggressive and complex trajectories. Otherwise the tasks may not be effectively completed due to instabilities or motor saturations.

A popular approach in robotics and first applied to manipulators is to produce smooth motions; i.e. trajectories with good continuity features; in particular, continuous velocity, accelerations and jerks in the interests of avoiding mechanical resonance (e.g., for manipulators) and reducing stresses to the actuators and to the mechanical structure. Smooth trajectories will help performing aggressive and fast trajectories for a quadrotor while aiding the controller action. Finally, smoothness is desirable for maintaining the quality of onboard sensor measurements. Since vision is part of the planning scheme, smooth motions may help reducing motion blur in the image plane to facilitate visual tracking.

Smoothness

In this section we discuss the smoothness of motion in robotic applications. First of all, a movement is perceived to be smooth, when it happens in a continual fashion without any interruptions. It is closely related to effort minimization which is a major objective, especially in manufacturing for cost and ecological reasons, but it is also desirable for robots carrying limited energy source (e.g., robots for spatial and submarine exploration). Among others, these conclusions motivated the use of smooth trajectories to connect two states and was applied to robotics [START_REF] Leahy | Compensation of unmodeled puma manipulator dynamics[END_REF][START_REF] Piazzi | Global minimum-jerk trajectory planning of robot manipulators[END_REF] a few years later with robotic arms to name a few.

The resolution problem of optimal trajectory satisfying a smooth performance 20 2.3. General control and trajectory generation techniques for a quadrotor index is considered as an optimal control problem. So one of the keys in trajectory generation is the selection of an appropriate cost function.

As often research takes inspiration from direct observations from the nature. In [START_REF] Flash | The coordination of arm movements: an experimentally confirmed mathematical model[END_REF] the authors observed that for reaching trajectories human appears to minimize the integral of the square of the norm of the jerk which is the time derivative of acceleration, hence, the third time derivative of position.

... r " d 3 r dt 3 (2.17)
For a particular trajectory xptq that starts at t 0 and ends at time t f , one can measure smoothness by calculating the jerk cost:

ż t f t"t 0 } ... xptq} 2 dt P R (2.18)
The derivative to minimize has motivated a large number of research especially in the neuroscience domain. These studies reveal some observations:

' the minimum jerk criterion does not produce acceleration jumps at the start and end points, while the minimum acceleration criterion does.

' it is related to the control effort minimization ' the jerk can be minimized independently on each axis [START_REF] Flash | The coordination of arm movements: an experimentally confirmed mathematical model[END_REF] Later [START_REF] Richardson | Comparing smooth arm movements with the two-thirds power law and the related segmented-control hypothesis[END_REF] pushed the study to higher derivatives (snap: the fourth derivative of position, crackle: the fifth derivative of position, pop: the sixth derivative of position). They found that as the order of the derivative increased, the solution to the functional xptq approached a step function. It is indeed legitimate to ask ourselves: What derivative to minimize ? ' Acceleration is the simplest, but most naive to define as the goal, since it will imply the less possible thrust, thus constraining excessively the aggressiveness of the trajectory. Smooth trajectories are desirable, but with some aggressiveness to explore the time optimal possible trajectory ' Jerk is a better representative of the aggressiveness of the true system inputs [START_REF] Mueller | A computationally efficient motion primitive for quadrocopter trajectory generation[END_REF] and, like the acceleration, has a direct link with thrust. Moreover [START_REF] Hehn | Real-time trajectory generation for interception maneuvers with quadrocopters[END_REF] affirms that maintaining constraints on the acceleration and jerk leads to a continuous thrust during the manoeuvre, which is then supported by [START_REF] Bipin | Autonomous navigation of generic quadrocopter with minimum time trajectory planning and control[END_REF] when affirming that constraints on jerk are necessary for a smooth trajectory.

' Snap trajectories have also been proven effective to generate quadrotor trajectories [START_REF] Richter | Polynomial trajectory planning for quadrotor flight[END_REF], due to the linkage in the motors commands and body rate derivatives.

Planning and control of a quadrotor UAV

Trajectories that quadrotors can follow quickly and accurately should be at least continuous up to the third derivative of position (or C 3 ). This is because, for quadrotors, discontinuities in lateral acceleration require instantaneous changes in attitude and discontinuities in lateral jerk require instantaneous changes in angular velocity.

In the past 10 years this approach has been extended to quadrotors. Jerk is minimized in [START_REF] Yu | Minimum jerk trajectory generation of a quadrotor based on the differential flatness[END_REF][START_REF] Hehn | Real-time trajectory generation for interception maneuvers with quadrocopters[END_REF][START_REF] Hehn | Quadrocopter trajectory generation and control[END_REF] where feasible trajectories are generated based on the decoupling of the rotational degrees of freedom. Analytic solutions for minimum jerk trajectories between collision-free points have been formulated in [START_REF] Kyriakopoulos | Minimum jerk path generation[END_REF] using the Pontryagin's minimum principle (see e.g., [START_REF] Bertsekas | Dynamic programming and optimal control[END_REF]). In [START_REF] Mellinger | Minimum snap trajectory generation and control for quadrotors[END_REF] the authors separated the optimal problem into four independent optimization problems and minimize the integral of the squared snap and the yaw acceleration since the inputs u 2 and u 3 are function of the fourth derivative of the positions and u 4 is function of the yaw angle second derivative.

[70] chose to optimize over the integral of the squared norm of the acceleration instead of snap, minimizing the energy that the considered helicopter needs. Compared to snap, acceleration directly translates into permanent additional thrust that all the motors have to provide, while snap just causes particular motors to spin up/down quickly. In [START_REF] Yu | Minimum jerk trajectory generation of a quadrotor based on the differential flatness[END_REF] the jerk is minimized for a quadrotor and is more generally a choice for robot manipulators [START_REF] Richardson | Comparing smooth arm movements with the two-thirds power law and the related segmented-control hypothesis[END_REF].

Minimizing the snap is present in many optimization frameworks in the literature (pioneered by [START_REF] Mellinger | Minimum snap trajectory generation and control for quadrotors[END_REF], see also [START_REF] Gao | Quadrotor trajectory generation in dynamic environments using semi-definite relaxation on nonconvex qcqp[END_REF][START_REF] Bry | Aggressive flight of fixed-wing and quadrotor aircraft in dense indoor environments[END_REF]). Indeed, an even more important goal than finding minimum-time trajectories might be trajectory smoothness, especially for quadrotors. It plays a role in producing safer trajectories which will facilitate the trajectory controller action [START_REF] Constantinescu | Smooth and time-optimal trajectory planning for industrial manipulators along specified paths[END_REF]. Although the first quantity acts on the optimal inputs, it mainly produces smooth trajectories for a quadrotor (by helping to reduce the angular acceleration) while using the actuation capability since the solution yields a trajectory with a larger peak speed relative to average speed (see [START_REF] Richardson | Comparing smooth arm movements with the two-thirds power law and the related segmented-control hypothesis[END_REF] for more details).

Finally, in [START_REF] Richter | Polynomial trajectory planning for quadrotor flight[END_REF][START_REF] Mellinger | Minimum snap trajectory generation and control for quadrotors[END_REF] a optimal trajectory is computed to pass though a number of position keyframes in a continuous way instead of following straight lines which have an infinite curvature at the keyframes that would force the quadrotor to stop at each connection.

To summarize, there seems to be no good general answer on which method is to be favoured between acceleration, jerk or snap and this might depend on the application. The next section introduces the property of differential flatness. We show that the inputs are a function of fourth-order derivative of position. For this reason and as it is done in [START_REF] Mellinger | Minimum snap trajectory generation and control for quadrotors[END_REF] we choose to minimize the snap.

State estimation

State estimation

The challenge is now to get a reliable knowledge of the robot's state since the performance of the controller depends on the quality of the state estimate. Many robotic applications rely on external centralized localization systems such as Vicon or global positioning system (GPS) or full SLAM systems [START_REF] Howard | Experiments with a large heterogeneous mobile robot team: Exploration, mapping, deployment and detection[END_REF][START_REF] Yan | A survey and analysis of multirobot coordination[END_REF]. However, GPS signals are only available outdoor and are not sufficiently reliable and precise enough for some specific tasks (e.g., involving interaction with the environment or navigation among obstacles). One may often need accurate knowledge of the state and a solution is to exploit local observations of the environment.

To obtain a reliable response from a quadrotor for experimental purposes some preliminary tasks are required: the calibration of the Inertial Measurement Unit (IMU), an identification of offsets, biases, motor curves, etc. Once all of this is completed (see e.g., [START_REF] Spica | An Open-Source Hardware/Software Architecture for Quadrotor UAVs[END_REF] for further details on the procedure) state estimation algorithms are implemented. Their role is to provide a reliable estimate of the system state based on the outputs of a channel of proprioceptive sensors (e.g., accelerometer, gyroscope,...) and exteroceptive sensors (e.g., altimeter, cameras, lidar,...).

State estimation algorithms are generally based on multi-sensor fusion to combine the sensory measurements and properties of different sensors (e.g., acquisition rate, robustness to some noise, weight,...). In aerial robotics the attitude and angular velocity are the most important as they are primary variable in attitude control of the vehicle. A very popular choice is to combine an IMU composed of an accelerometer and a gyroscope with a camera to merge high rate acceleration and angular velocity measurements (from the IMU) with lower rate visual cues. These visual measurements can be used for, e.g., position, orientation and velocity estimation from the environment or for visual odometry [START_REF] Scaramuzza | Visual odometry [tutorial[END_REF]. Sensor-fusion algorithms have been successfully applied in many works to provide full autonomy of the robotic platforms, e.g., [START_REF] Weiss | Monocular-slam-based navigation for autonomous micro helicopters in gps-denied environments[END_REF][START_REF] Forster | Svo: Fast semi-direct monocular visual odometry[END_REF]. Fully autonomous high-speed navigation has only been achieved in the last few years (e.g., [START_REF] Mohta | Experiments in fast, autonomous, gpsdenied quadrotor flight[END_REF][START_REF] Shen | Vision-based state estimation and trajectory control towards high-speed flight with a quadrotor[END_REF]). These recent developments were largely supported by the improvements of sensors in terms of measurement accuracy, compactness and acquisition rate. Most of the approaches use Extended Kalman filters (EKF) for its robustness and simplicity. However, no guarantees of convergence and stability are given. These questions are addressed in [START_REF] Hua | Attitude, linear velocity and depth estimation of a camera observing a planar target using continuous homography and inertial data[END_REF] by the design of an observer endowed with exponential stability and convergence guarantees. The observer fuses optical flow with inertial measurements to estimate the attitude, the linear velocity and the depth of a camera observing a planar target.

2. Planning and control of a quadrotor UAV

Differential flatness

Planning trajectories in high dimensional space is challenging especially with an underactuated system. In this section we show how control and planning problems can be simplified without any additional approximations by using the fact that the quadrotor dynamics are flat. This property makes the trajectories design easier and guarantees the trajectories are feasible, i.e., trajectories that satisfy the equations of motion.

Definition and properties

Differential flatness was primarily introduced by Fliess [START_REF] Fliess | Flatness and defect of nonlinear systems: introductory theory and examples[END_REF] in a differential algebraic context aimed at nonlinear system [START_REF] Fliess | Flatness and defect of nonlinear systems: introductory theory and examples[END_REF]. Then Martin, Murray, Rouchon, Lévine and Van Nieuwstadt [START_REF] Martin | Flat systems, equivalence and trajectory generation[END_REF][START_REF] Van Nieuwstadt | Real-time trajectory generation for differentially flat systems[END_REF] made further study about this theory and its implications in trajectory generation. They discovered the existence of a set of flat outputs with nonlinear dynamic characteristics that allow exact linearisation of particular nonlinear systems. In a nutshell, for a differentially flat system, all states and inputs can be expressed as algebraic functions of a set of outputs and their derivatives. More specifically, a nonlinear system:

9 x " f px, uq, x P R n , u P R m (2.19)
is termed flat if we can find outputs σ P R m of the form γ " ξpx, u, 9 u, . . . , u r q ξ : R n ˆpR m q r`1 Ñ R m (2.20) such that

x " φ x pγ, 9 γ, . . . , γ l q φ : pR m q r Ñ R n u " φ u pγ, 9 γ, . . . , γ l q ψ : pR

m q r`1 Ñ R m (2.21a) (2.21b)
where ξ, φ x and φ u are smooth functions and γ is called the flat outputs. This means that the new system's description is given by the m algebraic variables γ i , i " 1, . . . , m. So for a differentially flat system, if given a desired trajectory γ d , we can obtain all expected states, inputs and outputs:

x d " φ x pγ d , 9 γ d , . . . , γ l d q u d " φ u pγ d , 9 γ d , . . . , γ l d q (2.22a) (2.22b)
Note that ξ is bijective.

In [START_REF] Murray | Differential flatness of mechanical control systems: A catalog of prototype systems[END_REF] the authors presented a catalogue of flat systems in 1995 including nonholonomic mobile robots, the Planar Vertical Take-Off and Landing (PVTOL) aircraft, the inverted pendulum and the ducted fan. They also provided insights on 2.5. Differential flatness determining if a system is differentially flat by considering its mechanical structure. Since then, new flat systems have emerged, for instance, the ballbot robot under small angles assumptions [START_REF] Shomin | Differentially flat trajectory generation for a dynamically stable mobile robot[END_REF] and especially the quadrotor in [START_REF] Mellinger | Minimum snap trajectory generation and control for quadrotors[END_REF] and later revised and extended with the consideration of rotor drag effects in [START_REF] Faessler | Differential flatness of quadrotor dynamics subject to rotor drag for accurate tracking of high-speed trajectories[END_REF]. Several "protocentric aerial manipulators" (systems where the first joint of the manipulator coincides with the quadrotor center of mass) were proven to be flat [START_REF] Yüksel | Differential flatness and control of protocentric aerial manipulators with any number of arms and mixed rigid-/elastic-joints[END_REF] as well as a quadrotor tethered by cables/bars [START_REF] Tognon | Dynamics, control, and estimation for aerial robots tethered by cables or bars[END_REF]. It is known from [START_REF] Mellinger | Minimum snap trajectory generation and control for quadrotors[END_REF] that the quadrotor dynamics (2.16) are flat with flat outputs γ " pr, ψq T P R 4 [START_REF] Mellinger | Minimum snap trajectory generation and control for quadrotors[END_REF], where ψ is the yaw angle from the usual roll/pitch/yaw decomposition of the rotation matrix R. Indeed, under the assumption f ą 0, one can find an invertible algebraic mapping of the form:

χ " φ χ pr, v, 9 v, : v, ψ, 9 ψq pf, 9 f, : f, τ q " φ u p 9 v, : v, ... v, ψ, 9 ψ, : ψq (2.23a) (2.23b)
We report the complete proof of the flat transformation and its inverse transformation for the quadrotor dynamics in Appendix A. For simplicity of notation, we indicate with σ " pr, v, 9 v, : v, ... v, ψ, 9 ψ, : ψq the vector of all quantities appearing on the right side of (2.23), and with σ χ " pr, v, 9 v, : v, ψ, 9 ψq only those involved in (2.23a).

The implications of flatness for all these systems is that the trajectory generation problem can be reduced to simple algebra, in theory, and computationally attractive algorithms in practice. For instance, in the case of the quadrotor the state space of dimension 12 can be reduced to a 4-dimensional space in which the integration of equation (2.19) (often costly and numerically challenging step) is not necessary. Traditional approaches to trajectory generation, such as optimal control, cannot be easily applied in many cases (see [START_REF] Murray | Differential flatness of mechanical control systems: A catalog of prototype systems[END_REF] for examples). Since the flat output functions are completely free, the only constraints that must be satisfied are the initial and final conditions on the endpoints, their tangents, and higher order derivatives. Any other constraints on the system, such as bounds on the inputs, can be transformed into the at output space and (typically) become limits on the curvature or higher order derivative properties of the curve. Moreover, any curve that satisfies the boundary conditions in the flat output space is a trajectory of the original system.

Referring to Fig. 2.3, the problem of finding curves that take the system from xp0q, up0q to xpT q, upT q is reduced to finding any sufficiently smooth curve that satisfies γ k p0q and γ k pT q up to some finite number l. There is no need to solve a two-point boundary value problem (BVP) if the system is differentially flat. Once all the boundary conditions and trajectory constraints are mapped into the flat output space, (optimal) trajectories can be planned in the flat output space and then lifted back to the original state and input space with (2.22). The idea is that this methodology alleviates adjoining the system dynamics in the optimal control problem formulation. Consequently, the number of variables in the optimal control problem is reduced to expedite real-time computation. Therefore, by converting the input constraints on the quadrotor to constraints on the curvature and higher derivatives of the position and the yaw angle, it is possible to design efficient techniques for the generation of feasible trajectories.

Existence

Differentially flat systems encompass all linear, controllable systems and many nonlinear systems as well. Although there is no general methods to judge whether the system 9

x " f px, uq, x P R n , u P R m is differentially flat and it is difficulty to find the flat outputs for most known differentially flat systems, some researchers still found and proved the existence of flat outputs of some systems (like the one cited in the previous section). While Fliess [START_REF] Fliess | Flatness and defect of nonlinear systems: introductory theory and examples[END_REF][START_REF] Fliess | A lie-backlund approach to equivalence and flatness of nonlinear systems[END_REF] and Charlet [START_REF] Charlet | On dynamic feedback linearization[END_REF] provided necessary conditions and sufficient conditions separately for a class of systems, Chetverikov is the first to show necessary and sufficient conditions [START_REF] Chetverikov | New flatness conditions for control systems[END_REF]. Yet, one frequently has to resort to trial and error to construct the flat outputs. Flat outputs of a system are not unique [START_REF] Murray | Differential flatness of mechanical control systems: A catalog of prototype systems[END_REF], it is therefore preferable to select the flat outputs leading to simple computations for the mappings φ x and φ u . Their choice can also be motivated by the design of the control laws or the planning formulation into a reduced or more relevant space. In [START_REF] Thomas | Toward Autonomous Avian-Inspired Grasping for Micro Aerial Vechicles[END_REF] the flat outputs are chosen as a set of image features to simplify the planning in the image space for a grasping task performed by a quadrotor.

Differential flatness

In addition, a particular parametrization may also depend on the complexity of deriving the constraints from the outputs. However, it is generally recommended to use a parametrization that eliminates all equality constraints. Indeed, equality constraints are the most difficult to handle in nonlinear programming.

Differential flatness in control and trajectory planning

At this point, differential flatness plays a strictly practical role and quickly gained popularity for deriving control schemes and solving various optimal control problems as the state of the art in Chapt. 5 will show. Indeed, a transformation of the system into a linear equivalent description is obtained and then it is straightforward to design a controller based on linear control theory, e.g., [START_REF] Formentin | Flatness-based control of a quadrotor helicopter via feedforward linearization[END_REF] with disturbance rejection and [START_REF] Ferrin | Differential flatness based control of a rotorcraft for aggressive maneuvers[END_REF] with the use of a LQR for controlling a hexacopter Fig. 2.4. Furthermore, classic polynomial control laws can be applied on the flat outputs and their derivatives and compared with the actual flat state measurements. Such a method has been efficiently validated in [START_REF] Mokhtari | Feedback linearization and linear observer for a quadrotor unmanned aerial vehicle[END_REF]. Such control laws can be adopted for tracking any trajectory y d ptq " pγ d ptq, 9 γ d ptq, . . . , γ plq d ptqq directly in the space of the Brunovsky states by considering the new control inputs ω " γ pl`1q . Then, the real world inputs u are obtained via dynamic feedback and applied to the real system. This strategy was adopted in e.g., [START_REF] Rao | Visual Servoing of a UGV from a UAV using Differential Flatness[END_REF] to design a visual-based controller for a UGV and [START_REF] Tognon | Observer-based control of position and tension for an aerial robot tethered to a moving platform[END_REF] for controlling a tethered aerial robot. Asymptotical convergence can be achieved with an appropriate choice of the gains that can be determined by pole-placement techniques to ensure good tracking and some robustness to model uncertainties as well. However, in general, after a given order l ď l one may loose the capacity to observe the higher-order derivatives of the flat outputs. Typically, one resort to designing observers as also shown in [START_REF] Tognon | Observer-based control of position and tension for an aerial robot tethered to a moving platform[END_REF][START_REF] Mokhtari | Feedback linearization and linear observer for a quadrotor unmanned aerial vehicle[END_REF].

A similar strategy is found in optimization-based control: the optimal control outputs are computed in the flat space which are then lifted back to the space of 2. Planning and control of a quadrotor UAV the real control inputs, see e.g., [START_REF] Allibert | Predictive Control for Constrained Image-Based Visual Servoing[END_REF].

As already discussed in Sect. 2.3 from a control perspective, most early research on quadrotor dynamics focused on near-hover operation. Now, in the context of fast motion, differential flatness has been considered as a strong system property that can be used for generating dynamically feasible trajectories for underactuated robotic systems leading to significant progress both in trajectory generation and control of quadrotor systems in the recent years (e.g., [START_REF] Mellinger | Minimum snap trajectory generation and control for quadrotors[END_REF][START_REF] De Doná | A flatness-based iterative method for reference trajectory generation in constrained nmpc[END_REF][START_REF] Sheckells | Optimal Visual Servoing for Differentially Flat Underactuated Systems[END_REF][START_REF] Ferrin | Differential flatness based control of a rotorcraft for aggressive maneuvers[END_REF] to name a few).

In [START_REF] De Luca | Trajectory planning and control for planar robots with passive last joint[END_REF] a comparison is proposed between differential flatness and dynamic feedback linearisation in motion planning. Indeed, the two properties are equivalent in the sense that any feedback linearisable system is also differentially flat and vice versa as it is demonstrated in [START_REF] Fliess | A lie-backlund approach to equivalence and flatness of nonlinear systems[END_REF]. Moreover, the feedback linearising outputs and the flat outputs of a system coincide.

Vision-based control

To be reactive to changes in the environment, the use of on-board cameras has become a fundamental necessity. Indeed, moderately invasive and low power, they come with numerous computer vision algorithms for tracking objects, mapping, detecting obstacles but also for giving various measurements to non-linear observers (e.g., through active vision, state estimation).

For these reasons, incorporating visual cues in the loop has become a popular approach in robotic control for many years for designing robust and reactive control laws to complete positioning [START_REF] Guenard | A Practical Visual Servo Control for an Unmanned Aerial Vehicle[END_REF], grasping tasks [START_REF] Thomas | Toward Autonomous Avian-Inspired Grasping for Micro Aerial Vechicles[END_REF] or navigation among obstacles [START_REF] Odelga | Obstacle detection, tracking and avoidance for a teleoperated uav[END_REF] to name a few. However, vision is not without its challenges. Computation is intense and can result in low sample rate. Vision-based control techniques may be deceived by ambiguities between certain camera motions and scale since there exists a problem with scale recovery when using a single camera. Finally, they can suffer from delays between the image frame capture, transmission and processing. However, processors continue to improve and many vision-based autonomous applications are flourishing [START_REF] Falanga | Aggressive quadrotor flight through narrow gaps with onboard sensing and computing using active vision[END_REF][START_REF] Lee | Autonomous landing of a vtol uav on a moving platform using image-based visual servoing[END_REF][START_REF] Thomas | Autonomous flight for detection, localization, and tracking of moving targets with a small quadrotor[END_REF][START_REF] Seo | Aerial grasping of cylindrical object using visual servoing based on stochastic model predictive control[END_REF].

Issues related to vision-based control

A historical technique is visual servoing [START_REF] Chaumette | Visual servo control. I. Basic approaches[END_REF]. Visual servoing is the fusion of results from many elemental areas including high-speed image processing, kinematics, dynamics, control theory, and real-time computing. This popular control scheme uses visual cues for directly controlling the robot's motion and referred as Image-Based Visual Servoing (IBVS). This strategy was originally developed in the context of industrial robots (see e.g., [START_REF] Kragic | Survey on visual servoing for manipulation[END_REF] for a survey on the topic), which are usually equipped 2.7. Issues related to vision-based control with low-level high-gain control loops that allow neglecting the dynamics of the platform and, e.g., controlling it at the velocity level. Unfortunately, such simplification cannot be extended to quadrotors away from near-hovering conditions. Since quadrotors show non-negligible dynamics the visual control problem becomes significantly more complex. This is due to the inherent under-actuation that generates rotations of the camera which may conflict with the main servoing task (as detailed in the Introduction). Intuitively, in order to overcome this problem, the robot can either limit its rotational motion (thus reducing the acceleration and increasing the time needed to reach the desired position) or compensate the rotation by also moving upwards for increasing the size of the scene projected within the camera field of view.

Standard visual servoing approaches for underactuated systems, such as quadrotors, oftentimes do not explicitly ensure that the relevant image features stay in the camera's field of view, especially while the system is performing agile manoeuvres.

Preserving visibility is a substantial concern when vision is in the loop since losing track of features may lead to a failure of the task. The original formulation of visual servoing does not prevent critical configurations related to:

• field of view limits, i.e., the target may become invisible as the camera moves;

• occlusions, i.e., the image features may be occluded due to obstacles or body parts;

• singularities, i.e., specific features configurations can lead to an ill-conditioning of the interaction matrix (e.g., the cylinder singularity for three points);

• local minima, i.e., the control law may lead to a convergence to unexpected configurations. Local minima may appear with the use of redundant measurements that is the usual approach to avoid singularities;

Finally, one can complete the above list with issues related to aggressive motion planning:

• physical constraints such as feasibility constraints;

• camera underactuation, i.e., when a camera is attached to an underactuated robot such as a quadrotor the image features are more subject to being repelled from the camera center as the quadrotor perform translational motions.

• motion blur, i.e., the aggressiveness of the camera motion (especially with high angular accelerations) may lead to poor quality of the image frames that can jeopardize the vision algorithms.

Planning and control of a quadrotor UAV

• collision with obstacles or self-collision;

Note that two mechanical solutions could of course help reducing the effect of underactuation: 1) a pan-tilt camera could be mounted instead of a fixed camera [START_REF] Cretual | Application of motion-based visual servoing to target tracking[END_REF]. Apart from the additional payload and extra consumption, sensing is still limited and one cannot guarantee that all visual features will remain visible. 2) fully actuated quadrotors with tilting propellers [START_REF] Ryll | A novel overactuated quadrotor unmanned aerial vehicle: Modeling, control, and experimental validation[END_REF][START_REF] Cutler | Analysis and control of a variable-pitch quadrotor for agile flight[END_REF] have been recently developed to gain full access to the 6 DOFs (see, e.g. [START_REF] Mahony | A port-Hamiltonian approach to image-based visual servo control for dynamic systems[END_REF][START_REF] Zergeroglu | Visionbased nonlinear tracking controllers with uncertain robot-camera parameters[END_REF] for visual controllers). Therefore, a fixed camera would be less subject to rotation motions but still, if aggressive motions are performed rotations are possible.

In this thesis we provide optimization-based solutions ensuring visibility of visual features. Moreover, in Sect. [START_REF] Michael | The grasp multiple microuav testbed[END_REF]

Introduction to optimization

In many occasions, the trajectory generation problem cannot be solved analytically. An exception is formed by linear systems. For general systems we can only solve the generation problem by repeatedly integrating the system equations and trying to minimize some errors between the computed trajectory and the desired trajectory. The resolution of more and more complex problems may not be possible by analytic resolution techniques. Moreover, we believe they may be too difficult and 3. Optimization and numerical resolution not generic enough. In this section we introduce optimization and especially direct optimization which we believe represents a more appropriate approach for the resolution of complex problems. The idea is to compute a finite sequence of optimal controls and states over a time horizon as a numerical approximation of the system dynamics. Optimization is a well-understood field and is able to exhibit valid inputs and trajectories adapted to changes in the task, the environment and system dynamics.

Minimum-time trajectory generation problem

A very specific trajectory generation problem in robotics is the generation of timeoptimal trajectories between two states. The problem of connecting a given initial state χ 0 at time t and a chosen final state χ f at a time t `T can be formulated as that of constructing a feasible trajectory

χptq ˚: rt, t `T s Ñ X (3.1)
for the state of the quadrotor where X denotes the state space, and T defines the trajectory duration. Now, an infinite number of trajectories can connect these two states. Since one seeks the generation of agile manoeuvres, we are particularly interested in minimizing the completion time T . Using an optimal formulation we define Problem 1 where Jpχp.q, up.qq P R defines the cost function or objective function that we want to minimize. Given the dynamic model (3.2d) at a generic time t, we seek for a solution to the following optimization problem.

Problem 1 Find χpsq, upsq, T, s P rt, t `T s, such that:

min χpsq,upsq,T Jpχpsq, upsqq " T s.t. χptq " χ t , χpt `T q " χ ˚, 9
χ " hpχ, uq, upsq P U , @s P rt, t `T s

(3.2a) (3.2b) (3.2c) (3.2d) (3.2e)
where χ t is the current robot state, χ ˚is the desired one, and (3.2d) was introduced to represent (2.16) in a compact form.

Note that Problem 1 is quite general. In particular, it does not impose any constraint on the initial and final states which, e.g., do not have to be hovering states (i.e. with R " I, v " 0, and ω " 0).

Pontryagin's minimum principle

We also want to find a feasible trajectory for the quadrotor. This is encoded with (3.2d) that imposes the trajectory to respect the nonlinear dynamics equations (2.16).

The robot is finally subject to (nonlinear) inputs constraints (3.2e) due to the physical limits of its actuators. These constraints act on the minimum and maximum values of the system inputs. Vector u contains then the propellers individual thrust (u " pf 1 , f 2 , f 3 , f 4 q P R 4 ). Note that, since the propellers can not change their direction of rotation during the motion, we have to assume f i ą 0.

The set of admissible control inputs is the box U " rf m , f M s 4 with 0 ă f m ă f M where f m and f M are determined by the physical characteristics of the motor, the available power, propeller, etc. One can prove that Problem 1 is solvable [START_REF] Mellinger | Minimum snap trajectory generation and control for quadrotors[END_REF][START_REF] Spica | Aerial Grasping of a Moving Target with a Quadrotor UAV[END_REF], i.e., there always exist a parameter T such that (3.2e) are satisfied (with a reasonable choice of f m and f M ).

Pontryagin's minimum principle

The problem of generating optimal trajectories for a quadrotor UAV has been addressed with analytic resolution [START_REF] Hehn | Quadrocopter trajectory generation and control[END_REF][START_REF] Hehn | Performance benchmarking of quadrotor systems using time-optimal control[END_REF] using Pontryagin's minimum principle. This optimal control theory principle has been formulated by Lev Semenovich Pontryagin and his students in 1956 and defines a necessary, but not sufficient, condition for optimality of a system trajectory. The problem is a generalization of the Euler-Lagrange equations that also includes problems with constraints on the control inputs and applies to a large class of control problems.

Let us assume that we want to find a trajectory for the state and the input

χptq ˚: rt, t `T s Ñ X uptq ˚: rt, t `T s Ñ U (3.3a) (3.3b) that minimizes the cost function J " Φpχpt `T q ˚q `ż t`T t F pχptq ˚, uptq ˚q dt (3.4) subject to 9 χ " f pχptq, uptqq, χptq ˚" χ 0 , χpt `T q ˚" χ f (3.5)
Neglecting the time dependencies, we define the Hamiltonian of the system as Hpχ, u, pq " F pχ, uq `pT f pχ, uq

where p is also called the costate vector and plays a similar role to the Lagrange multipliers.

Optimization and numerical resolution

The Pontryagin's principle states that if uptq ˚is an optimal trajectory for the input and χptq ˚is the corresponding optimal trajectory for the state, then the following conditions hold

9 χptq ˚" f pχptq ˚, uptq ˚q 9 χptq ˚" χ 0 9 χpt `T q ˚" χ f 9 pptq " ´∇χ H pχptq ˚, uptq ˚, pptqq (3.7a) (3.7b) (3.7c) (3.7d)
and for all t P rt, t `T s

9 uptq ˚" argmin uPU Hpχptq ˚, u, pptqq (3.8)
Moreover if the total time t f is not fixed by the problem, the following condition also holds true

Hpχptq ˚, uptq ˚, pptqq " 0 (3.9)
For a detailed explanation of the Pontryagin's principle, refer to e.g. [START_REF] Bertsekas | Dynamic programming and optimal control[END_REF]. It has hence been shown that the solution to the minimum-time problem is generally a bang-bang control policy [START_REF] Hermes | On the nonlinear control problem with control appearing linearly[END_REF], that is, a control policy in which the control signal switches between two or several extreme values. Pontryagin's minimum principle is exploited in [START_REF] Hehn | Real-time trajectory generation for interception maneuvers with quadrocopters[END_REF][START_REF] Hehn | Quadrocopter trajectory generation and control[END_REF][START_REF] Van Loock | Time-optimal quadrotor flight[END_REF] and [START_REF] Ritz | Quadrocopter performance benchmarking using optimal control[END_REF] to generate minimum-time trajectories for a quadrotor. For instance [START_REF] Hehn | Real-time trajectory generation for interception maneuvers with quadrocopters[END_REF] generates minimum-time interception trajectories for aggressively catching a ball in mid-air: bang-singular trajectories where the goal is to reach a given position at a given time, while minimizing the time required to stop after the intercept. These methods are computationally fast, with solution times on the order of microseconds which is compatible for closed-loop control.

However, these strategies are not able to account for geometric constraints and are independent of the yaw angle in order to decouple the quadrotor axes. In this thesis, we consider more complex constraints such as visibility constraints that are not compatible to Pontryagin's minimum principle as far as we know.

In the next section we show how an optimal control problem (OCP) can be turned into a nonlinear program (NLP) that is suited for numerical resolution.

Numerical solutions of optimal control problems using nonlinear programming

Nonlinear optimization describes the class of optimization problems when the objective or constraint functions are not linear and not known to be convex as well. These problems are considered as much more complex and difficult to solve and there is 3.4. Numerical solutions of optimal control problems using nonlinear programming no effective method of solving them. However, there exist different approaches to their resolution that involve some compromises.

The problem of finding a local minimizer x P R n for a nonlinear function F pxq subject to a set of nonlinear constraints c ě 0, where cpxq P R n , is a nonlinear constrained optimization problem. All the problems of interest to be solved in this thesis can be generalized into the form

Problem 2 min x F pxq s.t. cpxq ě 0 (3.10a) (3.10b)
Optimization problems of the form of Problem 2 can be a very difficult problem to solve. Algorithms to solve this problem may take many iterations and function evaluations. Moreover, global optimization of Problem 2 is a difficult problem and an open area of research. In this thesis, we will concentrate on using the well understood numerical techniques that will find local minimum.

Nearly all techniques for nonlinear programming are iterative, producing a sequence of subproblems related in some way to the original problem. Newton methods have rapid local convergence rates, but fail to converge from all starting points. Gradient descent methods converge from nearly any starting point but have poor local convergence properties. Line-search methods are one means of ensuring global convergence while attempting to maintain fast local convergence. Line-search methods limit the size of the step taken from the current point to the next iterate. Such methods generate a sequence of iterates of the form

x k`1 " x k `αp (3.11)
where p is the search direction obtained from the subproblem, and α is a positive scalar step-length that has to be chosen carefully. However, determining a minimizer along p is an iterative process and frequently time consuming. Typically, x is determined by a finite process that ensures a reduction in F pxq. See [START_REF] Fletcher | Practical methods of optimization[END_REF] for an overview of line-search methods.

Two very different approaches may be considered to solve Problem 2, the indirect one and the direct one.

Indirect and direct methods for nonlinear programming

Most early numerical methods of solution to constrained optimal trajectory generation problems relied on either indirect or direct methods of solution. The indirect method relies on finding a solution to the Pontryagin's maximum principle presented

Optimization and numerical resolution earlier. Indirect methods turn the problem into an integration problem consisting of ordinary differential equation (ODE) or differential-algebraic equation (DAE).

The resulting problem is a differential equation which is unfortunately often too complex to be integrated as is. When it is possible, this approach provides a complete (and often comparatively cheap) solution to the problem. However, this type of approach is usually applied on a specific system and/or task so the differential equation can be simplified enough to be integrated.

The direct method obtains solutions by direct minimization of the objective function, subject to the constraints of the optimal control problem. In the direct approach, the optimal control problem is transformed into a NLP. In a first approach, this can be done with the so-called direct single-and particularly-multiple shooting methods. The key strategy is to divide the time vector, state and control trajectories into a finite grid. Therefore, the direct approach directly solves a discretized approximation of the nominal problem using numerical optimization techniques. This allows turning an optimization problem of an infinite dimension (the search space is infinite dimensional) to a finite one in order to be efficiently solved by selecting outputs from a finite dimensional space. This results in finding a numerical solution to a two-point boundary value problem (BVP), if no closed form solution can be found. Examples along this line can be found in [START_REF] Pesch | Real-time computation of feedback controls for constrained optimal control problems. part 1: Neighbouring extremals[END_REF][START_REF]Real-time computation of feedback controls for constrained optimal control problems. part 2: A correction method based on multiple shooting[END_REF]. These methods normally cannot meet the performance requirement for on-line calculation, especially when the system manoeuvring time is short. In addition, the optimal trajectories represented by the discrete collocation points are not continuous or smooth curves.

Is is known from e.g., [START_REF] Rao | A survey of numerical methods for optimal control[END_REF] that direct methods are generally less precise but more robust to the initial solution guess than indirect methods. However, it appears that the computational requirements of direct methods are at least that of indirect methods. The collocation method of [START_REF] Hargraves | Direct trajectory optimization using nonlinear programming and collocation[END_REF] and adjoint method [START_REF] Polak | Optimization: algorithms and consistent approximations[END_REF] take part of the most relevant transcription methods to the trajectory generation problem.

Nonlinear solvers

Now that we showed how OCPs are discretized to obtain a structured NLP in nonconvex form one has to select a nonlinear solver. Sequential Quadratic Programming (SQP) and Interior Point Methods (IPM) are popular gradient-based classes of methods considered to be effective and reliable for locally solving (3.10b). These methods are guided by the first-and second-order derivatives, i.e., the gradients and the Hessian matrix. Interior point refers to the fact that the slack variables are required to remain strictly positive throughout the optimization (more can be found in [START_REF] Wright | The interior-point revolution in optimization: history, recent developments, and lasting consequences[END_REF]). SQP ( [START_REF] Gill | Practical optimization[END_REF][START_REF] Lawrence | Nonlinear equality constraints in feasible sequential quadratic programming[END_REF]) is the technique we will use to solve the nonlinear 3.4. Numerical solutions of optimal control problems using nonlinear programming programming problems presented in this thesis. The fundamental approach SQP is to solve a NLP by solving a sequence of quadratic programs (QP) that are easy to solve. More precisely, at each iteration of a SQP, one solves a QP subproblem that models Problem 2 locally at the current iterate. The solution to the QP is used as a search direction by a line-search algorithm to determine the next iterate.

SQP is known for its rapid convergence (a few SQP iterations) when iterating from an initial point (or initial guess) that is close to a (local) minimum but may show erratic behaviour when the initial point is far. Moreover, SQP is not a feasible method; that is, neither the initial guess nor any of the subsequent iterations need to be feasible (a feasible point that satisfies the constraints). This is a major advantage since finding a feasible point when there are nonlinear constraints may be nearly as hard as solving the NLP itself. However, converging to a minimum would generally require more iterations than starting from a feasible point. SQP solvers differ in the way the Hessian is approximated, the line-search is done, the QP subproblems are solved or the constraints are relaxed. SQP has been shown a powerful tool and because of its superlinear convergence rate and its ability to deal with nonlinear constraints. It is currently considered as one of the most powerful algorithm to solve numerous formulations of NLP.

Note that the gradients have to be supplied and their accuracy is crucial for local convergence. In principle the gradients can almost always be computed using very little additional computational effort. In practice, and especially with highly nonlinear programs analytic formulation of the Jacobians can be very complex, subject to errors and finally hard to code. We will see in Appendix C that there exist efficient numerical methods for accurately evaluating these functions.

The Hessian is even more complex and approximation methods exist such as the Broyden-Fletcher-Goldfarb-Shanno method (BFGS) [START_REF] Broyden | The convergence of a class of double-rank minimization algorithms 1. general considerations[END_REF]. This method builds an approximation of the Hessian based on successive gradient evaluations that are stored over for a given horizon. This method has proven to have a good performance even for non-smooth optimizations. Thus, the Hessian matrix will help accelerating local optimization.

Both SQP and IPM methods iterate from an initial guess for the optimization variables. This initial guess is therefore critical and has huge impact of the objective value of the local solution obtained. Using local optimization methods, one often has to resort to experimenting with the algorithm choice, parameters and initial guess. The methodology is not rigorously defined.

In the next section we show how Problem 1 can be turned into a NLP using differential flatness and parametrization with B-spline curves. The described method is the one we use in this thesis.

Optimization and numerical resolution

Differential flatness and B-spline curves for nonlinear programming

Referring to Sect. 2.5.1, we show how an optimal control problem can be reposed to allow direct optimization to occur within the output space (of the flat outputs) as opposed to the control space. First of all, we map the problem to the space of the flat outputs and their derivatives that we indicate with σ " pr, v, 9 v, : v, ... v, ψ, 9 ψ, : ψq. Thanks to differential flatness, one can move the planning problem from the input space to the flat output space (i.e., the problem becomes a static problem): any sufficiently smooth trajectory of the flat outputs is, in fact, guaranteed to be an admissible trajectory for the original system dynamics. This property is extremely interesting for our purposes because it allows avoiding to deal with the non-linear differential equality constraint (3.2d), which would require the numerical integration of the system dynamics during the numerical optimization phase. A prediction of the state at any time in rt 0 , t f s can, instead, be computed algebraically from the planned flat-output trajectory. For these reasons differential flatness has been widely used for trajectory planning in the past [START_REF] Milam | Real-time Optimal Trajectory Generation for Constrained Dynamical Systems[END_REF][START_REF] Mellinger | Minimum snap trajectory generation and control for quadrotors[END_REF][START_REF] Spica | Aerial Grasping of a Moving Target with a Quadrotor UAV[END_REF].

Mapping Problem 2 into the flat space gives the equivalent following problem Problem 3

min σp.q Lpσq s.t. gpσq ě 0 (3.12a) (3.12b)
Now, in solving Problem 3, we face two challenges: (i) instead of a finite set of variables, the optimization variable is a function σp.q and (ii) the constraints must be enforced at all time instances. Therefore, the problem is infinite dimensional with an infinite number of constraints. To cope with the infinite dimensionality σp.q is usually approximated with fixed parametric curves defined by a finite set of variables, a technique known as parametrization.

Parametrization of the flat outputs

There are many curves defined by a finite number of variables that can be used to approximate the outputs σp.q (Fourier series, Legendre polynomials, Laguerre polynomials, Chebyshev polynomials, Taylor series, etc.). Now, a requirement is to accurately represent a basis of a trajectory with a reasonable number of decision variables that will constitute the degrees of freedom of the solver. A second important requirement of the curve is the ability to set a level of continuity C k , without adding additional constraints. Specifying the level of continuity is necessary, since 3.5. Differential flatness and B-spline curves for nonlinear programming the states and inputs are a function of the outputs and their derivatives. A high degree single polynomial would be necessary to satisfy complex constraints but solving for the coefficients of high degree curves can be an inefficient and ill-conditioned operation. Finally, when a high number of basis functions is desired in order to satisfy multiple conditions still leaving some room for optimization, polynomial functions are not a good choice. Indeed to increase the number of parameters in a polynomial we need to increase its degree. Local support is also a desirable property of the basis functions. Local support means that the curves only influence the curve locally to the current point of interest which is also favourable for numerically stable computer implementation.

A solution that meets the main requirements are Bézier polynomials or B-splines [START_REF] Boor | A practical guide to splines[END_REF]. An exhaustive introduction on B-spline is given in Appendix B including the many interesting properties and manipulation algorithms these curves possess. These functions are obtained as a composition of a certain number of polynomials, each of whom is defined in a limited sub-domain of the overall function domain. The advantage of this solution is that we can increase the number of curve coefficients by increasing the number of polynomial components, while maintaining a low degree of the single polynomials. In particular a spline is said to be of degree p if it is composed by polynomials of degree p. A B-spline curve is constructed from Bézier curves joined together with a prescribed level of continuity between them. The points at which the curves are joined are called the breakpoints and are constructed so that they join with some level of continuity. The breakpoints are a strictly increasing sequence of real numbers. A non-decreasing sequence of real numbers containing K `1 breakpoints U " pu 0 , ..., u K q is called the knot vector. A breakpoint may appear multiple times in the interior of a knot vector and be referred as a breakpoint of multiplicity m. A recurrence relation is used to define the B-spline basis functions B i,j of the B-spline curves:

spuq " n ÿ i"1 B i,p puqP i n ě k ´1 (3.13)
where P i are the control points and the B i,p are piecewise polynomial functions of degree p (and order k " p `1) forming a basis for the vector space of all piecewise polynomial functions of the desired degree and continuity. Given the knot vector U and the degree p, the B-spline basis functions are defined by:

B i,0 puq " # 1 if u i ď u ď u i`1 0 otherwise B i,p puq " u´u i u i`p ´ui B i,p´1 puq `ui`p`1 ´u u i`p`1 ´ui`1 B i`1,p´1 puq (3.14)
Now, in the case of the quadrotor, given a vector of control points P " pr 1 , . . . , r nr , ψ 1 , . . . , ψ n ψ q P R 3nr`n ψ , and two (fixed) normalized knot vectors U p P r0, 1s Kp , U ψ P r0, 1s K ψ , the

Optimization and numerical resolution

flat output trajectories can be represented as:

$ ' ' ' ' & ' ' ' ' % rpsq " nr ÿ i"1 B i,kr ˆs ´t T ˙ri ψpsq " n ψ ÿ i"1 B i,k ψ ˆs ´t T ˙ψi , @s P rt, t `T s, (3.15)
where B i,k is the i-th B-spline basis function of order k, which can be computed recursively as described in [START_REF] Fliess | Flatness and defect of nonlinear systems: introductory theory and examples[END_REF].

Given (2.23), in order to ensure state continuity and input boundedness, one has to guarantee Lipschitz continuity of : v and 9 ψ (and continuity of lower order derivatives). This condition can be met by using open-uniform distributions of K " n `k knots (i.e. u i " 0, for i " 1, . . . , k, u i " 1, for i " n, . . . , K, and u k , . . . , u n equally spaced in r0, 1s) and by taking k " k r " 4 for r and k " k ψ " 2 for ψ.

Problem 1 can, finally, be restated as a NLP as follows.

Problem 4 Find P , T , such that:

min P ,T T s.t. @ P , B dr kr ptq, B d ψ k ψ ptq D " σ χt , @ P , B dr kr pt `T q, B d ψ k ψ pt `T q D " σ χ ů ´P , B dr kr psq, B d ψ k ψ psq ¯P U , @s P rt, t `T s (3.16a) (3.16b) (3.16c) (3.16d)
where B d k psq P R n is d-th order derivative B-spline basis of order k evaluated at s P rt, t `T s. The above formulation is adopted in our works to numerically solve optimal control problems with different costs and constraints. Although, the system nonlinear dynamics equality constraints (2.16) become transparent due to the flatness transformation, their nonlinearities are in fact transferred to the other constraints, here, the real inputs constraints (3.16d). Indeed, the inputs obtained with u " φ u pr, 9 r, : r, r p3q , r p4q , ψ, 9 ψ, : ψq are more complex to evaluate than in their original formulation (2.13).

As already said, to guarantee the continuity of the state, the position must be continuous up to the third order of derivation while the yaw angle must be continuous up to the first order. To keep the degree of the spline as low as possible we use two different splines: one for the position vector and an other (scalar) one for the yaw angle. The parameter s can be directly equal to the time and we will choose the knot vector so that all the internal nodes have multiplicity 1. For a 4-th order clamped B-spline with n control points, the knot vector is

U " ˆ0, 0, 0, 0, 0, 1 pn ´kq , . . . , n ´k ´1 pn ´kq , 1, 1, 1, 1, 1 ˙(3.17)
The number n of control points obviously depends on the number of conditions that we want to impose to the spline and on the redundancy we want to keep for further optimization. For each of the two connecting trajectories we must satisfy boundary conditions determined by the initial and final states. Also in this case the continuity of the state is guaranteed by the continuity of the position up to the third order of derivation and of the yaw angle up to the first order of derivation. This results in a total amount of 8 conditions on the position spline and 4 conditions on the yaw spline. Therefore, in order to satisfy these conditions, we need at least eight control points for the position (n r " 8) and four control points for the yaw (n ψ " 4). If we choose these values we end up with two square linear systems in the control points that can be conveniently written in a matrix form

A r B P r B " B r B , A ψ p ψ " b ψ (3.18a) (3.18b)
where the system variables are

P r B " ¨pT r B ,1 p T r B ,2 . . . p T r B ,8 ‹ ‹ ‹ ‹ ' , p ψ " ¨pψ,1 p ψ,2 . . . p ψ,4 ‹ ‹ ‹ ‹ ' (3.19)
The coefficients matrices A r B and A ψ contain the values of the B-spline basis functions and their derivatives at the initial and final times:

A r B " ¨B1,1 pt 0 q B 2,1 pt 0 q . . . B 8,1 pt 0 q B p1q 1,2 pt 0 q B p1q 2,2 pt 0 q . . . B p1q 8,2 pt 0 q B p2q 1,3 pt 0 q B p2q 2,3 pt 0 q . . . B p2q 8,3 pt 0 q B p3q 1,4 pt 0 q B p3q 2,4 pt 0 q . . . B p3q 8,4 pt 0 q B 1,1 pt f q B 2,1 pt f q . . . B 8,1 pt f q B p1q 1,2 pt f q B p1q 2,2 pt f q . . . B p1q 8,2 pt f q B p2q 1,3 pt f q B p2q 2,3 pt f q . . . B p2q 8,3 pt f q B p3q 1,4 pt f q B p3q 2,4 pt f q . . . B p3q 8,4 pt f q ‹ ‹ ‹ ‹ ‹ ‹ ‹ ‹ ‹ ‹ ‹ ‹ ‹ ‹ ' (3.20) and A ψ " ¨B1,1 pt 0 q B 2,1 pt 0 q B 3,1 pt 0 q B 4,1 pt 0 q B p1q 1,2 pt 0 q B p1q 2,2 pt 0 q B p1q 3,2 pt 0 q B p1q 4,2 pt 0 q B 1,1 pt f q B 2,1 pt f q B 3,1 pt f q B 4,1 pt f q B p1q 1,2 pt f q B p1q 2,2 pt f q B p1q 3,2 pt f q B p1q 4,2 pt f q ‹ ‹ ‹ ‹ ' (3.21)

Optimization and numerical resolution

Finally the known terms are determined by transforming the boundary conditions χ 0 and χ f into the equivalent conditions on the flat outputs and their derivatives

B r B " ¨rB T 0 v B T 0 : r B T 0 9 a B T 0 r B T f v B T f : r B T f 9 a B T f ‹ ‹ ‹ ‹ ‹ ‹ ‹ ‹ ‹ ‹ ‹ ‹ ‹ ‹ ' , b ψ " ¨ψ0 9 ψ 0 ψ f 9 ψ f ‹ ‹ ‹ ‹ ' (3.22)
The system has a unique solution, provided that t 0 ‰ t f and that the knots are properly chosen.

Any coefficient vector that satisfies the linear constraints (3.18) automatically satisfies the initial and final state constraints. For simplifications one can fix these conditions to reduce the number of decision variables. Regarding the B-spline knot vector, this means that the initial and final elements are fixed.

At this point, any general-purpose optimization strategy can be used to find a numerical solution to Problem 4. Unfortunately, due to the non trivial non-linearity of (3.16d), Problem 4 cannot be proven to be convex. The optimization will thus, in general, return a local minimum. Figure 3.1 shows a solution trajectory and the inputs profiles obtained from the resolution of Problem 4 with an initial hovering state at position r " p0, 0, 1q and a final hovering state r " p2.5, 2.5, 1q. Note that satisfaction of the inputs constraints cannot be guaranteed (although it is the case in this example). The reason is that the inputs are discretized and the inputs can be very sharp (see Fig. 3.1c). The discretization time-step might be chosen in response to the sharpness of the inputs.

To sum up, there are three steps to trajectory generation based on the differential flatness theory; i) The first is to choose flat outputs, so the system can be mapped to a lower dimensional output space. Meanwhile, the cost function, the boundary and constraints can also be mapped to the output space; ii) The second is to choose a suitable basis function to parametrize flat outputs; iii) After parametrizing the selected outputs, we need to solve a set of coefficients.

In this thesis, we use nonlinear programming to solve for the coefficients of the B-splines to minimize the cost function subject to bound conditions and trajectory constraints in flat output space. Then, we obtain the flat output trajectories satisfying the constraints expressed by the computed coefficients.

Note that by suitably parametrizing trajectories with basis functions in the flat space and by considering linear inequalities in the flat space to model constraints on 3.6. Summary states and inputs u it is possible to turn this optimization problem into a quadratic program that can be solved in real-time for planning. This simplification will be discussed further in Sect. 4.5 and applied in Chapt. 8.

Summary

In this chapter, we presented a brief overview of the classical numerical methods for solving constrained optimal control problems. To sum up, constrained optimal control problems do not contain a closed form solution, approximation techniques can be employed for a numerical solution. The advantage of indirect methods is that very accurate solutions can be obtained. The main disadvantage of indirect methods is their lack of robustness to a poor initial guess. In this thesis we mainly focus in the parametrization of trajectories with B-splines whose control points constitute the decision variables of the nonlinear programs we will define. Several properties of such curves will be exploited in the design of the planning problems. Now that we presented a way of solving nonlinear problems we focus on the Chapter 4

Model predictive control: toward trajectory re-planning

Introduction and context

One can observe that the research community in aerial robotics is exploring more and more complex objectives such as tracking a fast moving target, avoiding dynamic obstacles, passing through several way-points and so on. More details are given in the state of the art in Chapt. 5. Designing a controller to perform these tasks would be challenging and could possibly lead to severe sub-optimality. With optimization-based controllers, it is easy to include different (and possibly conflicting) objectives and constraints. Now, due to increasing performance of computers, nonlinear programming tends to be more and more tractable and able to substitute popular controllers. Having robots planning their own trajectories becomes more 4. Model predictive control: toward trajectory re-planning and more practicable. However, in order to be efficient the important goal of optimal trajectory generation is to construct, in real time, a solution that optimizes the system objective while satisfying system dynamics, as well as state and actuation constraints.

Moreover, as briefly discussed in Sect. 2.2, since the system's model is imperfect, model and parametric uncertainty (e.g., inertial parameters) may lead to substantial deviation from the reference trajectory. Classic solutions include adaptive control [START_REF] Antonelli | Adaptive trajectory tracking for quadrotor mavs in presence of parameter uncertainties and external disturbances[END_REF] for estimating the nominal parameters online. However, trajectories that "excite" enough the estimation may be hard to find and may even conflict with the main task especially if we are interested in minimum-time control. A second approach is to design robust control laws typically implementing feed-forward terms for instance [START_REF] Faessler | Differential flatness of quadrotor dynamics subject to rotor drag for accurate tracking of high-speed trajectories[END_REF]. Finally, another strategy consists in directly designing specific trajectories that are robust to these modelling errors or nominal parameters uncertainty [START_REF] Giordano | Trajectory generation for minimum closed-loop state sensitivity[END_REF]. MPC tackles these issues in a slightly different approach: the reference trajectory and/or command inputs are adapted in real-time via optimization techniques as a feedback to cope with disturbances and modelling errors. MPC acts more as a high-level controller in this approach. Note that MPC techniques can incorporate uncertainty in the control process and are referred as Robust MPC and Stochastic MPC.

Several randomized trajectory generation techniques (such as RRT ˚and A ˚), originally applied to the mission level (see e.g., [START_REF] Lavalle | Randomized kinodynamic planning[END_REF][START_REF] Frazzoli | Real-time motion planning for agile autonomous vehicles[END_REF][START_REF] Hsu | Randomized kinodynamic motion planning with moving obstacles[END_REF]), have been recently reported in complex scenarios [START_REF] Liu | Search-based Motion Planning for Aggressive Flight in SE (3)[END_REF] and real-time applications for dynamic systems [START_REF] Pivtoraiko | Incremental micro-uav motion replanning for exploring unknown environments[END_REF][START_REF] Liu | Search-based Motion Planning for Aggressive Flight in SE (3)[END_REF]. We will introduce and discuss more about search-based methods further in this thesis since they also play a decisive role in complex motion planning nowadays.

In this section, we shed light on reactive replanning methods and especially Model Predictive Control (MPC) also known as Receding Horizon Control (RHC). By reactive we mean able to generate solutions on-line fast enough to respond efficiently to sudden changes in the environment (obstacles, target, ...) or new situations.

Principle

In Model Predictive Control, an open-loop trajectory is found by solving a finitehorizon constrained optimal control problem starting from the current state. The optimal controls of this trajectory are then applied to the system for a certain fraction of the horizon length, after which the process is repeated (see Fig. 4.1). Note that the essence of MPC is to optimize over the predictions of a process behaviour. Therefore, the process model is essential.

Principle

MPC is a family of algorithms which give the possibility to:

• explicitly include in the problem formulation constraints on state, input, output variables, and logic relations;

• consider hundreds of control and controlled variables;

• transform the control problem into an optimization one, where different, and sometimes conflicting, goals can be stated;

• use very detailed physical (nonlinear, DAE, ...) models with continuous and integer variables.

• close an optimal control loop Then the first value of the optimal control sequence is applied to the system (circled in red). At time k `1 the optimization is repeated with the same prediction horizon.

To implement the receding horizon control strategy, a constrained (and often nonlinear) optimization control problem must be solved on-line. Due to the complexity of solving a nonlinear programming problem in real-time, the computational delay cannot be ignored. This is particularly important in aerial and aerospace applications, where the dynamics is high and the internal control loops are very short. Originally applied in the process control industry where dynamics are relatively slow, the application of receding horizon control to aerial vehicles has been proposed and analysed by several researchers [START_REF] Raemaekers | Design of a model predictive controller to control uavs[END_REF][START_REF] Mueller | A model predictive controller for quadrocopter state interception[END_REF]. Most popular applications include system stabilization, evasive manoeuvres, obstacle avoidance and target tracking.

The receding horizon strategy offers many benefits in this environment, such as the inherent ability to deal with constraints in the state and control. Examples of 4. Model predictive control: toward trajectory re-planning such constraints commonly encountered include dynamic terrain obstacles, dynamic or pop-up threats, saturations on the actuators, impair of the capacity of a vehicle. However, a few requirements are needed, we must guarantee the convergence of the algorithm at each computation, and guarantee the fastness of the convergence. Indeed, the faster the algorithm is, the less the previous solution is out-dated so the more the algorithm will be able to improve it instead of just adapt it to the new situation.

Moreover, these approaches serve little practical purpose until stable and efficient computational techniques are developed to provide real-time solutions to the underlying constrained nonlinear optimal control problems. Closed-loop stability has been well defined in [START_REF] Mayne | Constrained model predictive control: Stability and optimality[END_REF] but gets much more difficult to prove when the problem is nonlinear.

In this thesis we take inspiration from the replanning scheme of receding horizon control for iteratively solving nonlinear programs. This general idea has been widely employed in research with several planning strategies in order to face the heavy computation loads and to meet real-time [START_REF] Hehn | Quadrocopter trajectory generation and control[END_REF][START_REF] Mueller | A computationally efficient motion primitive for quadrocopter trajectory generation[END_REF][START_REF] Mueller | A model predictive controller for quadrocopter state interception[END_REF].

Receding horizon formulation: the linear case

Let us define a general linear optimal control problem by considering the following system xpk `1q " Axpkq `Bupkq ypkq " Cxpkq (4.1a) (4.1b) where x P R n is the state vector of dimension n, u P R m is the input vector of dimension m and y P R p is the output vector of dimension p. At time k we want to compute the sequence of future control variables up.q " rupkq, upk `1q, ...upk `N ´1qs T (4.1c) minimizing the objective Jpxp.q, up.q, kq "

N ´1 ÿ i"0 p}xpk `iq} 2 Q `}upk `iq} 2 R q `}xpk `N q} 2 S (4.2)
where Q " Q T ě 0, R " R T ě 0, S " S T ě 0 play the role of weighting matrices and N denotes the prediction horizon. Finally, }xpk `N q} 2 S is the terminal cost. The optimal solution to this problem is given by the state-feedback control law u 0 pk `iq " ´Kpiqxpk `iq, i " 0, 1, ..., N ´1 (4.3) 4.3. Receding horizon formulation: the linear case where u 0 p.q is the sequence of optimal inputs and the gain Kpiq is given by the expression

Kpiq " pR `BT P pi `1qBq ´1B T P pi `1qA (4.4)

and P piq is the solution of the difference Riccati equation P piq " Q `AT P pi `1qA ´AT P pi `1qBpR `BT P pi `1qBq ´1B T P pi `1qA (4.5) with terminal condition P pN q " S (4.6)

The weighting matrix S plays a role in closed loop stability, a typical choice is a quadratic Lyapunov function especially for the generalization to nonlinear and constrained systems, see [START_REF] Murray | Optimization-based control[END_REF] for instance. Finally, recalling the Lagrange equation

xpk `iq " A i xpkq `i´1 ÿ j"0 A i´j´1 Bupk `jq, i ą 0 (4.7)
and defining

Xpkq "

¨xpk `1q xpk `2q . . . xpk `N ´1q xpk `N q ‹ ‹ ‹ ‹ ‹ ‹ ‹ ' , A " ¨A A 2 . . . A N ´1 A N ‹ ‹ ‹ ‹ ‹ ‹ ‹ ' , U pkq " ¨upkq upk `1q . . . upk `N ´2q upk `N ´1q ‹ ‹ ‹ ‹ ‹ ‹ ‹ ' (4.8) B " ¨B 0 0 ¨¨¨0 0 AB B 0 ¨¨¨0 0 ¨¨¨¨¨¨¨¨¨¨¨¨¨¨¨¨¨Ä N ´2B A N ´3B A N ´4B ¨¨¨B 0 A N ´1B A N ´2B A N ´3B ¨¨¨AB B ‹ ‹ ‹ ‹ ‹ ‹ ' (4.9)
then the future state variables are given by Xpkq " Axpkq `BU pkq (4.10)

The optimal cost can be found as Jpxpkq, up.q, kq " X T pkqdiagpQqXpkq `U T pkqdiagpRqU pkq (4.11) where the term x T pkqdiagpQqxpkq can be ignored since it does not depend on U pkq.

Model predictive control: toward trajectory re-planning

Linear constraints on the state, inputs and outputs can easily be handled with this framework. Let us define the following constraints the problem is subject to xpk `1q " Axpkq `Bupkq ū ď upk `iq ď ū i " 0, 1, ...., N ´1

x ď xpk `iq ď x i " 0, 1, ...., N ´1

ȳ ď ypk `iq ď ȳ i " 0, 1, ...., N ´1 (4.12a) (4.12b) (4.12c) (4.12d)
This problem (quadratic cost function, linear constraints) can be easily solved by means of a QP method with very reasonable computational time (which obviously depends on the problem size).

However, for constrained (and obviously nonlinear) systems the control law is implicitly defined, i.e., its value can be numerically computed through the solution of the optimization problem, but its analytic expression is unknown. However, the principle is the same, only the first element (in general) of the input sequence of the open-loop solution is applied to the system which defines a time-invariant control law for the closed-loop of the form u 0 pkq " χ RH pkq. For the linear case we have

u 0 pkq " ´Kp0qxpkq (4.13)
with Kp0q " pR `BT P p1qBq ´1B T P p1qA (

obtained by iterating the Riccati equation backwards from

P pN q " S (4.15)
Now, one can address the case of nonlinear systems which constitutes a much wider class than linear systems. Considering the applications, one can resort to local linearization of the system dynamics and constraints which is a classical technique in chemical industry. Note that this technique was successfully applied to the control of robot manipulators [START_REF] Homsi | A Hierarchical Approach to Minimum Time Control of Industrial Robots[END_REF] to achieve fast motions.

When applied to nonlinear systems, the algorithm may demand tremendous computational power, and can exhibit poor convergent stability if not implemented properly. For instance, if one chooses a large prediction horizon the solver may not be able to compute a solution fast enough. If too short, the resulting solution may lead to instabilities and even to a failure of the task since the problem may not have enough degrees of freedom to converge to a solution. These difficulties have largely prevented its application to stability critical nonlinear systems with fast dynamics. Increasingly powerful and affordable computing facilities combined with better understanding of receding horizon control's stability properties have revived interests in this area. See e.g., [START_REF] Mayne | Constrained model predictive control: Stability and optimality[END_REF][START_REF] Findeisen | An introduction to nonlinear model predictive control[END_REF] for a good review of recent work in this field.

An application of MPC to quadrotor control

In this section we present some simulation and experimental results to illustrate the role of a MPC scheme applied to trajectory tracking with a quadrotor in presence of obstacles 1 .

Since MPC relies on a model equations we choose to use the Brunosvky equivalent linear form (A.32) in the flat space. Without loss of generality we do not plan over the yaw angle that we assume it is kept constant. Let us define a classic objective function Jprp.q, up.qq "

N ´1 ÿ i"0 p}r i ´ri } 2 Q `}u i } 2 R q `}r N ´rN } 2 S (4.16)
where r i " px i , y i , z i q T and u " px p4q i , y p4q i , z p4q i q T are the predicted trajectory and inputs respectively, r i denotes the i-th reference point. We choose the Gerono lemniscate as the desired reference path defined at time t by xptq " r cospαtq sinpαtq,

yptq " r cospαtq, zptq " z 0 (4.17a) (4.17b) (4.17c)
Therefore, r i is defined as a moving point on the reference path and we penalize the deviation of the quadrotor from this reference point over the prediction horizon N . In order to evaluate the MPC response to obstacle avoidance, we add two planes along the X and Y-axis located in x wall 1 and y wall 2 and a spherical obstacle of radius r obs at position r obs that overlap with the reference path (see Fig. 4.2). We model the quadrotor as a sphere of radius l centered in r. The geometric constraints are defined as }r ´robs } ě r obs `l,

x ď x wall 1 `l, y ď y wall 2 `l (4.18a) (4.18b) (4.18c)
As already discussed, thanks to differential flatness any smooth trajectory in the space of flat outputs can be followed provided that derivatives are correctly bounded. Nevertheless, this powerful property can be difficult to apprehend and implement for real systems involving highly nonlinear dynamics. The following section details alternatives to the definition of the dynamic constraints for the quadrotor. The quadrotor has to avoid collisions with a sphere and two walls while keeping a minimal distance with the reference path.

A relaxed formulation based on differential flatness

In some cases, changing the planning space to the flat space may not grant obvious physical meaning and makes the equations, and particularly the expression of the real system inputs much more complex than their original formulation. Now, a very debated question is how to define these constraints on the dynamics. Basically, it is inherent to questions of formulation complexity, conservatism and planning strategy. Namely, it would be better of course to consider the real physical limitations of the motors as constraints to guarantee that the trajectories are feasible by the real system but considering the application it may be more attractive to use less complex constraints (linear if possible) on different level of the dynamics. [START_REF] Boeuf | Planning agile motions for quadrotors in constrained environments[END_REF] addresses the generation of smooth trajectories in the kinodynamic state space with inequality constraints on the absolute value of the derivatives of the flat outputs defined as follows

|v| ă v max | 9 v| ă a max |: v| ă j max | ... v| ă s max (4.19a) (4.19b) (4.19c) (4.19d) (4.19e)
Some works have focused on estimating the feasible set in flat output space by polytopic approximations (e.g., [START_REF] Faiz | Differentially flat systems with inequality constraints: An approach to real-time feasible trajectory generation[END_REF]), however this set is generally a non-convex function of nonlinear inequalities and is a hard optimization problem unto itself.

In this preliminary work we consider the abovementioned constraints (4.19) to achieve the desired replanning rate and closed-loop stability. Indeed, using constraints directly on the motors thrust with constraint (3.2e) were found too complex for the solver.

An application of MPC to quadrotor control

Similar choices were made in [START_REF] Hehn | Real-time trajectory generation for interception maneuvers with quadrocopters[END_REF] using constraints on the acceleration and the jerk. Constraints on the total thrust are evaluated afterwards and the problem is rescaled until they are satisfied. The conservative nature of the jerk bounds means that only a fraction of the allowable body rates is typically used. If these exceed limitations, it was shown that a feasible trajectory can always be found by reducing the allowable jerk values.

Results and delay compensation

The MPC scheme was carried out using the ACADO toolkit [START_REF] Houska | ACADO Toolkit -An Open Source Framework for Automatic Control and Dynamic Optimization[END_REF] which implements a multiple-shooting algorithm. ACADO solves multiple shooting problems thanks to a SQP algorithm, together with state-of-the-art techniques to condense, relax, integrate and differentiate the problem. The quadrotor dynamics were simulated using V-Rep2 at 150Hz. The generated trajectories of the flat outputs were sent to TeleKyb [START_REF] Grabe | The TeleKyb Framework for a Modular and Extendible ROS-based Quadrotor Control[END_REF] at a rate of 30Hz which then computed the actual control inputs using the geometric controller developed in [START_REF] Lee | Geometric tracking control of a quadrotor UAV on SE(3)[END_REF].

A major issue in the implementation of receding horizon control is handling the computational delay associated with the real-time optimization. We present here a simple method for designing an initial guess and take delay into consideration for the replanning. We select a section of the optimal states sequence which is sent to the controller. Since the control loop runs faster than the solver, we choose to interpolate cubic splines between the optimal states to smooth the controller action. Finally, to compensate for the delay (assumed constant at 1/30ms) we predict the initial state for the next OCP by projecting the previous solution in the future (i.e., 30ms ahead of the current time).

We opted for the following bounds: v max " 1.5m{s, a max " 4m.s ´2, j max " 15m.s ´3, s max " 100m.s ´4. The robot is able to plan trajectories that avoid the obstacles with a prediction horizon N " 50. The considered optimal control problem is solved within around 30 ms. The robot profiles during simulation are shown in Fig. 4.3.

To illustrate the reactivity of MPC we conducted a second simulation where a human operator is sending velocity commands (up to 1.5m/s) to the quadrotor via a joystick. We encode collision avoidance constraints so that safe and highspeed navigation among obstacles is handled by the MPC action, see Fig. 4.4. We impose the following bounds: v max " 1.5m{s, a max " 10m.s ´2, j max " 30m.s ´3, s max " 150m.s ´4.

Summary

The results presented in this chapter demonstrate the potential of real-time receding horizon control for constrained systems with fast dynamics. Real-time RHC control represents a revolutionary alternative to the traditional linear or nonlinear controller design with many benefits.

First, in most cases, a global system model and objective function are easier to obtain than a traditional linear or nonlinear controller that works globally. For a complex nonlinear system, classical controller design techniques may show weak stability proofs and may not exhibit flexible actions to the different possible situations and environment changes. In comparison, given an accurate nonlinear model and adequately defined objective function, real-time RHC could provide a global optimal control that is elegant and flexible. For example, RHC can be easily reconfigured by changing the model or any parameter (see [START_REF] Geisert | Trajectory generation for quadrotor based systems using numerical optimal control[END_REF] for an illustration of RHC strategies applied to complex systems).

Second, real-time RHC can provide optimal control solutions, even for systems with complex constraints such as actuator saturation, operational limits, terrain avoidance, etc. In contrast, it is extremely difficult to design a classic controller for constrained systems.

Third, with accurate modelling and precise objective definition, system performance could be far more superior than classic linear or nonlinear controller, particularly for very aggressive manoeuvring that pushes the constraint boundaries.

Fourth, in many cases, real-time RHC eliminates the necessity of both inner loops and outer loops that is common in classic tracking and stability control design. Instead, trajectory generation and robust control are performed in a single integrated design with potentially better performance and higher bandwidth. In this chapter we used ACADO which is specifically designed for implementing MPC problems. Yet, handling more complex and nonlinear constraints on the motors thrust did not lead to satisfactory results, especially for real-time control. In our thesis we opted for a different on-the-shelf nonlinear solver to generate feasible and reactive trajectories in the presence of multiple nonlinear constraints for accomplishing several vision-based tasks. Now that we have presented the main ingredients and concepts used in this thesis, we introduce the most relevant contributions identified in the literature that are related to our work.

Chapter 5

Aggressive trajectory generation and vision-based planning for a quadrotor: related works

Optimization-based methods

Many dynamic manoeuvres have been performed in the recent years using optimization methods. They include fast translations [START_REF] Mellinger | Trajectory generation and control for precise aggressive maneuvers with quadrotors[END_REF], ball catching [START_REF] Mueller | A computationally efficient motion primitive for quadrocopter trajectory generation[END_REF] and flights through narrow gaps [START_REF] Falanga | Aggressive quadrotor flight through narrow gaps with onboard sensing and computing using active vision[END_REF] for instance. In [START_REF] Falanga | Aggressive quadrotor flight through narrow gaps with onboard sensing and computing using active vision[END_REF] a quadrotor flies through a window using vision as feedback in a complete autonomous way using only onboard sensing and computing. High angular rates were achieved in real experiments. The aggressiveness of motions is mainly limited by the quality of the visual feedback (especially due to motion blur).

Due to the growing computation power of computers it becomes more and more practicable to generate trajectories online. Yet, to meet this challenging demand, several works have been developing mathematical tools to efficiently generate feasible trajectories close to the actuation limits by relying on more or less conservative 57 5. Aggressive trajectory generation and vision-based planning for a quadrotor: related works approaches. We can refer to the following leading strategies listed below. All of these methods rely on a particular optimal criterion. ' Using a class of lightweight motion primitives: Several approaches merge optimal trajectories and reactive re-planning by generating computationally lightweight motion primitives as an implicit feedback control law. These simple curves (polynomials, splines, lines) constitute more or less rich trajectories that are easy to manipulate and evaluate at a lower level. [START_REF] Hehn | Real-time trajectory generation for interception maneuvers with quadrocopters[END_REF][START_REF] Mueller | A computationally efficient motion primitive for quadrocopter trajectory generation[END_REF] use the Pontryagin's minimum principle to generate candidate time-optimal trajectories between two states that are sent to the controller after checking that the constraints are satisfied. In [START_REF] Mueller | A computationally efficient motion primitive for quadrocopter trajectory generation[END_REF] a two-dimensional quadrotor model is considered, the axes are decoupled and conservative feasibility tests are developed on the total thrust and the angular rates to validate the generated trajectories that are shown in Fig. 5.1. Nevertheless, position constraints are not considered.

To make such an algorithm successful, the classic paradigm in control schemes is addressed, namely the trade-off between trajectory quality (i.e., in terms of feasibility, optimality, constraints satisfaction, ...) and planning rate (which needs to be high for such an agile system).

Figure 5.1 -Sampled motion primitives for a catching manoeuvre. The presented algorithm is able to generate about 6700 feasible motion primitives per second, from [START_REF] Mueller | A computationally efficient motion primitive for quadrocopter trajectory generation[END_REF].

' using graph-search methods: the approach relies on exploring the state space with probabilistic methods such as RRT, RRT* or A* algorithms. A path is built as a succession of straight paths [START_REF] Kuffner | Rrt-connect: An efficient approach to singlequery path planning[END_REF] or curves [START_REF] Lavalle | Rapidly-exploring random trees: A new tool for path planning[END_REF] or motion primitives (as shown in Fig. 5.2) forming a set of vertices connected by edges with a certain level of continuity [START_REF] Liu | Search-based motion planning for quadrotors using linear quadratic minimum time control[END_REF][START_REF] Pivtoraiko | Incremental micro-uav motion replanning for exploring unknown environments[END_REF]. Generally, various (and complex) constraints can be considered. Constraints can be checked at each extended vertex using simple tests. If constraints are satisfied the vertex is added to the graph.

' using Mixed-Integer Programming: this method involves problems in which some of the variables are integers. The algorithm is usually employed for 5.1. Optimization-based methods finding collision-free paths passing through a set of keyframes by minimizing some criterion. This planning method is capable of handling a large set of constraints but usually only enforces collision avoidance with obstacles or body parts and plans a single trajectory with integer constraints (see e.g., [START_REF] Landry | Aggressive quadrotor flight through cluttered environments using mixed integer programming[END_REF] Fig. 5.3 or [START_REF] Deits | Efficient mixed-integer planning for uavs in cluttered environments[END_REF] for a single quadrotor, [START_REF] Mellinger | Mixed-integer quadratic program trajectory generation for heterogeneous quadrotor teams[END_REF] for multiple agents and [START_REF] Tang | Mixed integer quadratic program trajectory generation for a quadrotor with a cable-suspended payload[END_REF] for multi-body system). The environment is often partitioned into convex sub-regions in the configuration space, constraints are linear and differential flatness is used for tractability reasons but the solver generally takes seconds to hundreds of seconds to determine a proper solution. ' using direct optimization: this method (that we presented in Chapt. 3) is capable of dealing with generic problems with various constraints. Nonlinear problems can be solved numerically using SQP for instance to generate online optimal solutions if properly posed. Re-planning strategies have been demonstrated by successively solving OCP, such as MPC [START_REF] Mueller | A model predictive controller for quadrocopter state interception[END_REF].

' using a combination of the aforementioned techniques: in many occasions, a collision-free path is built and a second step involves an optimization program that computes a path of higher resolution taking the system dynamics into 59 5. Aggressive trajectory generation and vision-based planning for a quadrotor: related works account. Examples along this line are numerous, e.g., [START_REF] Deits | Efficient mixed-integer planning for uavs in cluttered environments[END_REF] combines mixedinteger programming and direct optimization, [START_REF] Richter | Polynomial trajectory planning for aggressive quadrotor flight in dense indoor environments[END_REF] combines graph-search and direct optimization to perform high-speed flights for a quadrotor, and [START_REF] Liu | Planning dynamically feasible trajectories for quadrotors using safe flight corridors in 3-d complex environments[END_REF] combines a variant of the three methods.

Graph-search approaches

These approaches that we briefly introduced earlier in Sect. 5.1 are widely exploited in the literature. Also very generic and computationally more and more attractive, they have been successfully applied to solve many motion planning problems for UAVs. Searching algorithms such as RRT* or A* are known for suffering from the curse of dimensionality-the ability to properly scale to high-dimensional space. Nowadays, several techniques have been proposed to overcome these limitations. Many authors separate the problem into two steps: an optimal path is found through graph-search without considering dynamic constraints. Then, an optimal dynamic trajectory is generated by optimizing over a collection of waypoints. This second step plays the role of enforcing dynamic feasibility and constraints by adapting the trajectory speed [START_REF] Hoffmann | Quadrotor helicopter trajectory tracking control[END_REF], jerk [START_REF] Hehn | Quadrocopter trajectory generation and control[END_REF] or time [START_REF] Mellinger | Minimum snap trajectory generation and control for quadrotors[END_REF] such that input constraints are not violated. This process is generally done recursively or using a scaling algorithm [START_REF] Mueller | A computationally efficient motion primitive for quadrocopter trajectory generation[END_REF][START_REF] Bouffard | A hybrid randomized/nonlinear programming technique for small aerial vehicle trajectory planning in 3d[END_REF] until dynamic constraints become active. These approaches are usually sufficiently fast to provide a feedback loop by re-planning the reference trajectory at every controller update. In [START_REF] Bry | Aggressive flight of fixed-wing and quadrotor aircraft in dense indoor environments[END_REF] the authors combine RRT* with polynomial trajectory generation to compute dynamically feasible trajectories for a quadrotor using a two-step approach (see Fig. 5.4). A variant of RRT for flat systems is detailed in [START_REF] Bascetta | Flat-rrt*: A sampling-based optimal trajectory planner for differentially flat vehicles with constrained dynamics[END_REF] to produce smooth dynamically feasible motion plans in real-time and for online navigation in dynamic environments with a quadrotor in [START_REF] Pivtoraiko | Incremental micro-uav motion replanning for exploring unknown environments[END_REF]. Both works exploit differential flatness to build a look-up table of pre-computed feasible motion primitives.

However, several classes of problems cannot be treated using this approach. For instance the robot orientation cannot be properly considered at the geometric stage in general. The sole counter example is [START_REF] Liu | Search-based motion planning for quadrotors using linear quadratic minimum time control[END_REF] where constraints on the quadrotor attitude are considered at the planning stage (see Fig. 5.5).

Finally, such techniques become more and more adapted to real-time planning and successfully applied to navigation in unknown environments, see e.g., [START_REF] Allen | A real-time framework for kinodynamic planning with application to quadrotor obstacle avoidance[END_REF][START_REF] Liu | Planning dynamically feasible trajectories for quadrotors using safe flight corridors in 3-d complex environments[END_REF][START_REF] Florence | Integrated perception and control at high speed: Evaluating collision avoidance maneuvers without maps[END_REF]. Figure 5.5 -A minimum-jerk trajectory is found for ensuring safe navigation among obstacles. The collision avoidance relies on the robot occupancy modelled as an ellipsoid (in pink) which is more accurate than a spherical model (in red), from [START_REF] Liu | Search-based Motion Planning for Aggressive Flight in SE (3)[END_REF]).

The minimum-time problem

The minimum-time problem

Naturally, the generation of aggressive trajectories often resorts to the minimization of time. Having the flying time T as a decision variable is very complicated. It is a free parameter (i.e., it is not directly subject to any constraints) but it strongly acts on the dynamic constraints and the shape of the trajectory (see Fig. 5.6).

Aggressive trajectory generation and vision-based planning for a quadrotor: related works

If trajectories are parametrized with polynomials, T appears in each derivative as a nonlinear decision variable resulting in re-evaluation of the basis at each solver iteration. Moreover, as T converges to zero numerical stability can be compromised. [START_REF] Van Den Broeck | Model predictive control for time-optimal point-to-point motion control[END_REF] overcame with this numerical issue by minimizing instead the settling time N P N, namely the number of discrete-sampling intervals required to reach the goal. This method greatly alleviates the numerical resolution and is able to suppress high frequency chattering due to measurement noise especially occurring in the vicinity of the origin. In [START_REF] Homsi | A Hierarchical Approach to Minimum Time Control of Industrial Robots[END_REF] a similar penalization is considered but in a hierarchical optimization framework to approach minimum-time trajectories for robot manipulators.

In a similar philosophy, many works rely on scaling approaches [START_REF] Hehn | Quadrocopter trajectory generation and control[END_REF] to perform motion close to the actuation limits. Bang-bang strategies [START_REF] Hehn | Performance benchmarking of quadrotor systems using time-optimal control[END_REF] are designed for quadrotors. [START_REF] Boeuf | Planning agile motions for quadrotors in constrained environments[END_REF] relies on a closed-form solution to compute arcs in the kinodynamic state space that tend to minimize the flying time. Here, the intuition is to minimize the time spent to reach the full speed during a flying phase resulting in bang-similar-bang. In the end it is shown that this implies to maximize time spent at maximum snap during jerk variations and to minimize the durations of snap variations.

[155] reformulates the minimum-time problem by expressing the quadrotor dynamics in a new set of "transverse" coordinates with respect to the reference path. However real-time could not be achieved.

In our works (e.g., [START_REF] Penin | Vision-based minimum-time trajectory generation for a quadrotor uav[END_REF][START_REF]Minimum-time trajectory generation under intermittent measurements[END_REF]), due to the underactuation a quadrotor equipped with a camera may have to increase its height in order to enlarge its field of view for converging faster towards a visual target while keeping it in the image plane. In this context, rescaling strategies might not be able to reproduce such a behaviour and will lead to severe sub-optimal solutions since it only acts on the single temporal parameter. The same observations can be made when addressing collision avoidance.

Vision-based control for the underactuated quadrotor

As already discussed, most vision-based approaches assume first-order or fullyactuated systems. Classic methods cannot be directly applied to quadrotors due to their complex dynamics and inherent underactuation that conflicts with the main servoing task.

In the next section we present how the issues related to vision with underactuated systems are addressed in the literature.

Visibility constraints and occlusion avoidance

A common approach in IBVS is to decouple the rotational kinematics of the vehicle from the image features. The image feature error is projected in a "rotationcompensated" camera frame or "virtual plane" (see Fig. 5.7) which is horizontal and has the same position and yaw angle of the real camera's image plane. Thus, by re-projecting the image points using attitude measurements the camera rotation is decoupled from the translation motion. This virtual plane also facilitates the estimation of depth of image points. This strategy has been applied in several works, e.g., in [START_REF] Thomas | Toward Autonomous Avian-Inspired Grasping for Micro Aerial Vechicles[END_REF][START_REF] Jabbari | Dynamic IBVS Control of an Underactuated UAV[END_REF][START_REF] Jabbari | An Adaptative Scheme for IBVS of an Underactuated UAV[END_REF][START_REF] Thomas | Toward autonomous avian-inspired grasping for micro aerial vehicles[END_REF][START_REF] Lee | Autonomous landing of a vtol uav on a moving platform using image-based visual servoing[END_REF] for the design of globally stable dynamic IBVS schemes. Although these works develop controllers that guarantee the image error in the rotation-compensated frame will converge to zero, the quadrotor underactuation is not explicitly taken into account by the control design. Therefore, it is still possible for the image features to completely leave the camera field of view if the system has significant rotation, resulting in tracking failure for high speed manoeuvres.

[159] presents several IBVS control techniques which decouple the image space from the task space by using spherical image moments as features [START_REF] Chaumette | Image moments: a general and useful set of features for visual servoing[END_REF]. Since the image error becomes a function of position only, large rotations could still occur, making the system vulnerable to failure as previously described.

Although, dynamic visual servoing schemes have been developed for second order or under-actuated systems (e.g., [START_REF] Bourquardez | Image-Based Visual Servo Control of the Translation Kinematics of a Quadrotor Aerial Vehicle[END_REF], [START_REF] Jabbari | An Adaptative Scheme for IBVS of an Underactuated UAV[END_REF], [START_REF] Fink | Nonlinear dynamic imagebased visual servoing of a quadrotor[END_REF] or [START_REF] Mebarki | Nonlinear visual control of unmanned aerial vehicles in gps-denied environments[END_REF] for quadrotors), the underlying assumptions fail for high-speed manoeuvres and in any case, do not take into account possible loss of visibility or occlusions. Yet, the effort was allocated to proving stability of the closed-loop dynamics and providing robustness analysis.

In [START_REF] Hamel | Visual servoing of an under-actuated dynamic rigid-body system: an image-based approach[END_REF]164] a dynamic IBVS controller based on a backstepping method is proposed using first-order spherical image moments as visual features. Both papers provide interesting passivity properties and rigorous proofs of closed-loop stability, but the proposed interaction matrices remain ill-conditioned as the image feature is insensitive to change in altitude. Hence, performance suffers from a low rate of convergence in altitude. Following these works [START_REF] Serra | Landing on a moving target using image-based visual servo control[END_REF] later eliminates the need of height estimation and the use of an external sensor for measuring the translational velocity. Guarantees of convergence are given for landing on a moving target. However, features are assumed to remain visible at all times and such a controller may not be applied to higher-speed translational motions.

[166] introduced a controller that takes into account the quadrotor underactuation and uses a virtual spring force to prevent the robot from rotating too much. However, a small change in roll or pitch may cause a large change in the proposed interaction matrix. This clearly reduces the quadrotor reactivity and, in any case, 5. Aggressive trajectory generation and vision-based planning for a quadrotor: related works does not strictly guarantee the satisfaction of visibility constraints. Some authors focused on feature estimation for recovering the visual-based task rather than avoiding occlusions [START_REF] Fleurmond | Handling visual features losses during a coordinated vision-based task with a dual-arm robotic system[END_REF] or visibility losses [START_REF] Falanga | Vision-based autonomous quadrotor landing on a moving platform[END_REF]. In [START_REF] Teuliere | Chasing a moving target from a flying uav[END_REF] the authors propose a vision-based algorithm to autonomously track a moving object. The tracking algorithm is robust to occlusions but does not avoid them and assumes the target stays in the field of view.

Potential fields are classically used for designing control laws for repelling visual cues from the projected obstacles in the image [START_REF] Cowan | Visual servoing via navigation functions[END_REF], [START_REF] Mezouar | Path planning for robust image-based control[END_REF] but this technique may strongly conflict with the nominal servoing task and increase the chance of falling into local minima. Gradient Projection Methods (GPM) use the system redundancy to mitigate the completion of two tasks [START_REF] Marchand | Dynamic sensor planning in visual servoing[END_REF], [START_REF] Mansard | A new redundancy formalism for avoidance in visual servoing[END_REF]. The secondary task gradient is projected on the null-space of the main task and uses the remaining redundancy to complete the avoidance task [START_REF] Marchand | Dynamic sensor planning in visual servoing[END_REF]. However, if all DOF are used one cannot apply this approach. Obviously, the redundancy formalism does not appear reasonable when dealing with underactuated robots such as quadrotors. Using the same redundancy formalism spirit a 6-DOF robot is controlled in [START_REF] Mansard | A new redundancy formalism for avoidance in visual servoing[END_REF] while simultaneously avoiding occlusions and joint limits. A relaxed control law is proposed which uses all DOF to simply prevent the main task error from increasing while performing a secondary task.

Another approach is to use activation functions in the control law to enable smooth transitions between safe and forbidden regions in the image plane [START_REF] Folio | A controller to avoid both occlusions and obstacles during a vision-based navigation task in a cluttered environment[END_REF]. The control acts on features that are out of some confidence area in order to release some degrees of freedom to manage others tasks. However, this technique raises stability issues.

Vision-based control for the underactuated quadrotor

After an examination of the relevant literature we can conclude that visionbased control laws for underactuated systems, such as quadrotors, oftentimes do not explicitly ensure that the relevant image features stay in the field of view of the camera and hardly deal with occlusions. In any case, they can be applied to perform agile manoeuvres. Note that visual servoing controllers for fully actuated second-order systems have been proposed in the literature (see, e.g. [START_REF] Mahony | A port-Hamiltonian approach to image-based visual servo control for dynamic systems[END_REF][START_REF] Zergeroglu | Visionbased nonlinear tracking controllers with uncertain robot-camera parameters[END_REF].

In the context of vision-based optimization, using visual features as flat outputs has been considered. [START_REF] Allibert | A flat model predictive controller for trajectory tracking in image based visual servoing[END_REF] extended their peer work [START_REF] Rao | Visual servoing of a ugv from a uav using differential flatness[END_REF] by encapsulating an image-based flatness formulation inside a MPC scheme by using the target image coordinates of a mobile robot as flat outputs. Now, visibility constraints appear in the flat space and thus are more simple to satisfy. However, this work only considers a fixed overhead camera. Further improvements were made more recently in [START_REF] Thomas | Toward autonomous avian-inspired grasping for micro aerial vehicles[END_REF] by finding flat outputs in the image plane considering a fixed camera attached to a 2D quadrotor to perform visual-based agile grasping in the XZ plane Fig. 5.8. The authors presented a trajectory generation method which guarantees dynamic feasibility and enables incorporating visual constraints as linear constraints. However, the existence of differential flatness is only possible with some model conservatism/approximations. Indeed, the mapping was done in a virtual image plane which is it not affected by the pitch angle. Therefore, the visibility constraints are not specified in the real image plane and may be too restraining for large rotations.

Figure 5.8 -Aggressive catching manoeuvres at 3m/s in the sagittal plane using a monocular camera. The catching strategy is inspired from the natural behaviour of the bald eagle snatching its prey, from [START_REF] Thomas | Toward autonomous avian-inspired grasping for micro aerial vehicles[END_REF].

The generation of motion primitives candidates presented in [START_REF] Mueller | A computationally efficient motion primitive for quadrocopter trajectory generation[END_REF] has been applied to autonomous landing on a visual target in [START_REF] Falanga | Vision-based autonomous quadrotor landing on a moving platform[END_REF]. The target visibility is not guaranteed but a Kalman filter is used to estimate the target position in case of partial visibility losses. Nevertheless, this technique might not be suitable to complex 3D motions when the trajectory is shaped by visibility constraints for instance. Indeed, in many occasions the quadrotor may have to accelerate upwards in order to compensate for the camera rotation that inherently repels the image features from the image plane center. An other relevant degree of freedom used for 5. Aggressive trajectory generation and vision-based planning for a quadrotor: related works keeping visibility of point features is to also combine rotations along z B to exploit the "shape" of the field of view (if one considers a square field of view). This is something that we observed in our works [START_REF] Penin | Vision-based minimum-time trajectory generation for a quadrotor uav[END_REF][START_REF] Penin | Vision-based reactive planning for aggressive target tracking while avoiding collisions and occlusions[END_REF] but rarely seen in the literature.

Recently, self-collision and simple occlusion avoidance tasks for a humanoid were incorporated in a quadratic optimization problem in [START_REF] Agravante | Visual servoing in an optimization framework for the whole-body control of humanoid robots[END_REF]. Visibility of the feature is handled by using an avoidance task as in [START_REF] Folio | A controller to avoid both occlusions and obstacles during a vision-based navigation task in a cluttered environment[END_REF]. A more precise occlusion avoidance formulation is proposed in [START_REF] Nicolis | Occlusion-free visual servoing for the shared autonomy teleoperation of dual-arm robots[END_REF] but uses a larger set of visual constraints. In [START_REF] Kazemi | Randomized kinodynamic planning for robust visual servoing[END_REF] the authors explored a randomized kinodynamic hybrid path planning approach applied to a manipulator for finding a feasible path. It satisfies a great deal of constraints both in the image and in the joint space but takes a few minutes to return a solution (see Fig. 5.9). Figure 5.9 -A visual servoing task is performed with a manipulator while avoiding occlusions and loss of the visibility features, from [START_REF] Kazemi | Randomized kinodynamic planning for robust visual servoing[END_REF].

More and more works incorporate perception objectives within an optimization program to keep some visual features in the field of view of the camera in order to improve the quality of the vision-based state estimation [START_REF] Falanga | PAMPC: Perception-aware model predictive control for quadrotors[END_REF] (see Fig. 5.10a) or for keeping a visual feedback of a moving target [START_REF] Thomas | Autonomous flight for detection, localization, and tracking of moving targets with a small quadrotor[END_REF] as shown in Fig. 5.10b.

Finally, MPC has taken down many of these issues through numerical optimization. [START_REF] Sheckells | Optimal Visual Servoing for Differentially Flat Underactuated Systems[END_REF] implemented a dense hybrid optimal visual servoing scheme to steer the underactued quadrotor towards a goal pose encoded as a desired goal set of image features. It uses a Perspective-n-Point (PnP) algorithm to estimate the goal state then an optimal trajectory minimizes the reprojection error of the features along the trajectory and the deviation of the path from the goal state. Although a large set of image features are considered there is no guaranty that enough features remain in the field of view since it is not encoded as a hard constraint. [START_REF] Ding | Real-time model predictive control for keeping a quadrotor visible on the camera field-of-view of a ground robot[END_REF] uses barrier functions in a MPC framework to keep a quadrotor in the field of view of a mobile platform with a upward-looking camera in the presence of external disturbances. In [START_REF] Gao | Quadrotor trajectory generation in dynamic environments using semi-definite relaxation on nonconvex qcqp[END_REF] smooth collision-free trajectories are generated for multiple quadrotors by predicting the agents motion using pose observations. A stochastic MPC was implemented in [START_REF] Seo | Aerial grasping of cylindrical object using visual servoing based on stochastic model predictive control[END_REF] for autonomous aerial grasping. The MPC action 5.5. Perception and uncertainty-aware planning (a) A quadrotor is equipped with a front-looking camera and is asked to fly at 3m/s around a region of interest while keeping it visible in the field of view of its camera, from [START_REF] Falanga | PAMPC: Perception-aware model predictive control for quadrotors[END_REF].

(b) A quadrotor tracking a moving target with a downward-looking camera. The quadrotor is able to increase its height in order to compensate for the camera rotation as the quadrotor moves forward, from [START_REF] Thomas | Autonomous flight for detection, localization, and tracking of moving targets with a small quadrotor[END_REF]. is able to respect visibility constraints but the achieved trajectories are close to near-hovering. Field of view and inputs constraints were considered in a fully autonomous aggressive target tracking receding horizon framework relying on onboard sensors with a downward camera attached to the quadrotor used for estimating the target position [START_REF] Thomas | Autonomous flight for detection, localization, and tracking of moving targets with a small quadrotor[END_REF] which constitutes one of the most relevant works. The real robot velocity hits 5 m/s and was able to compensate for the camera rotation by accelerating upwards (see Fig. 5.10b). The authors algorithm generates smooth trajectories by minimizing the relative velocity error and the jerk first and then penalizing the relative position error after some proximity threshold is reached. We believe such a strategy contributes in improving motion stability but may however abate the motion aggressiveness.

In the aforementioned works, only a few consider hard visibility constraints for a fully actuated robot for aggressive motion, i.e., [START_REF] Thomas | Autonomous flight for detection, localization, and tracking of moving targets with a small quadrotor[END_REF]. A second relevant work considers tracking a moving target with a quadrotor while avoiding obstacles in an unknown environment by generating on-line smooth and dynamically feasible trajectories provided that the target stays in the field of view [START_REF] Chen | Tracking a moving target in cluttered environments using a quadrotor[END_REF].

Perception and uncertainty-aware planning

Since vision plays a major role in state estimation, many works have merged visual objectives with stochastic problems.

In [START_REF] Costante | Perception-aware path planning[END_REF] the authors resort to an RRT* algorithm to find optimal and online paths that minimize the pose uncertainty by driving a quadrotor equipped with a downward-looking camera toward regions of rich texture (see Fig. 5.11). The approach relies on photometric information of the ground in the context of visual 5. Aggressive trajectory generation and vision-based planning for a quadrotor: related works odometry. This work is one of the first to incorporate perception goals in path planning with a quadrotor. However, the planning takes place in a 2.5D set-up (motion in the horizontal plane and at a given fixed height) and is not designed for generating dynamic motions. With the presented method the planner is able to find paths with minimal uncertainty (the blue ellipsoids are much smaller), from [START_REF] Costante | Perception-aware path planning[END_REF].

A comment we can make on trajectory smoothness (already mentioned in the Introduction of this thesis) is that smoothness may also play a decisive role in vision-based trajectory since a jerky camera motion with high angular acceleration especially will make the projection of a 3D point onto the image suffer from motion blur, making it very complicated, if not impossible, to extract meaningful information. This issue has been recently raised in [START_REF] Falanga | PAMPC: Perception-aware model predictive control for quadrotors[END_REF] in the context of robust visual perception with a fast moving autonomous quadrotor. The authors adopt a MPC framework to optimize over perception objectives for providing robust and reliable visual feedback during motion. The authors choose to maximize the visibility of a collection of points of interest by penalizing the deviation of their projections from the image center and the velocity of their projections in the image plane (see Fig. 5.10a). Here, a forward-looking camera is attached. The quadrotor is able to exploit the height of the room to compensate for the pitch while moving to manage the visibility of points of interest. Moreover, the planning naturally mostly acts on the system's heading since rotating around z B directly affects the visibility and at a lower energy cost than for accelerating upwards (since the total thrust is also penalized).

Recently, the Robotics & Perception Group at ETH developed a new dynamic vision sensor or event-based camera [START_REF] Kueng | Low-latency visual odometry using event-based feature tracks[END_REF] which is way less sensitive to motion blur and change of illumination and has a lower-latency compared to classic CCD cam-5.6. Summary eras. A second solution exploited in [START_REF] Mohta | Experiments in fast, autonomous, gpsdenied quadrotor flight[END_REF] is to control the exposure time of cameras to limit motion blur. Thus, motion blur issues can be managed both by hardware and control solutions.

In [START_REF] Pepy | Reliable robust path planning with application to mobile robots[END_REF] the authors consider a nonholonomic robot that has to reach a goal area of a given size delimiting the admissible position uncertainty (Fig. 5.12). The authors implemented a RRT variant where uncertain states are modelled as boxes.

Figure 5.12 -A robust path is found for a unicycle. Notice that the problem may not be feasible if the final constraint is too tight or if the level of uncertainties is too large (upper pictures), from [START_REF] Pepy | Reliable robust path planning with application to mobile robots[END_REF].

Other recent works have considered underactuated robots and sensor limitations in the context of active exploration [START_REF] Charrow | Information-theoretic mapping using cauchy-schwarz quadratic mutual information[END_REF][START_REF] Heng | Efficient visual exploration and coverage with a micro aerial vehicle in unknown environments[END_REF]. However, in these works the robot dynamics are simplified and the input constraints (i.e. the propeller speed) are not strictly imposed. An active sensing strategy considering the full quadrotor dynamics was proposed in [START_REF] Achtelik | Motionand uncertainty-aware path planning for micro aerial vehicles[END_REF], but without considering strict input constraints. Moreover, these works focus on environment coverage and a correct robot localization and none of them attempts to maintain visibility with respect to a specific set of features, which could, instead, be useful for target tracking applications.

Summary

In contrast to the presented works, our contributions merge vision-based and motor thrust constraints for the full dynamics of the quadrotor within fast and efficient 69 5. Aggressive trajectory generation and vision-based planning for a quadrotor: related works receding horizon frameworks that are capable of generating smooth and feasible optimal trajectories at the camera rate (30Hz) even though the problem is highly nonlinear. The re-planning strategies have been tested in various simulation tests and also with a real quadrotor by relying on an external motion capture Vicon system.

We have seen that a few works are tackling the issue of minimizing the uncertainty along a specific trajectory. However, depending on the environment topology and the visual features present in the scene, such a trajectory may return a solution that takes large detours before reaching the goal. Indeed, assuming an optimal solution exists providing continuous visual sensing (e.g., the system passes by every visual landmarks present on the scene), the resulting trajectory would be sub-optimal in terms of completion time and energy. We address this issue in our work presented in Chapt. 8.

Introduction

In this section we derive several optimal frameworks to perform reactive tracking of a moving target while ensuring visibility constraints. We demonstrate that the defined optimal problems are suited for a re-planning strategy inspired from MPC. To do so, we present our hot-start algorithm and the different techniques used for aiding the SQP solver converge to a local minimum within the given time allocation.

Reactive target tracking: a minimum-time optimal problem

Referring again to Fig. 6.1 let us assume the robot to be equipped with an on-board camera whose pose w.r.t. B is known from a preliminary calibration. Without loss of generality we assume that the camera is down-facing with optical center in O B

Aggressive vision-based trajectory generation

β i " R T pr i ´rq z B T R T pr i ´rq " » - - x i y i 1 fi ffi fl P P 2 , i " 1, . . . , N (6.1)
where r i P R 3 is the known position of the features in the inertial frame and P 2 is the space of 3-D homogeneous vectors. We assume that the number of points and their configuration is such that the complete pose pr, Rq of the robot can be reconstructed using visual information only. In particular, we consider N " 4 points on the ground plane since this is sufficient for our 3D case (one could also consider more complex features such as image moments [START_REF] Chaumette | Image moments: a general and useful set of features for visual servoing[END_REF]).

We also want to consider the field of view constraints so the object does not leave the image. The simplest way to solve this problem is to prescribe a maximum attitude angle (e.g., arccospe 3 ¨Re 3 q ď β max ). A trajectory could then be planned simultaneously using the maximum attitude constraint and the reduced field of view to constrain the relative positions. However, this approach is more conservative than desired, especially when aggressive maneuvers are necessary. Certainly, we do not want to restrict the maximum attitude. Instead, we directly incorporate the field of view as constraints in the optimization by defining the (square) image domain as

Ω " tβ P P 2 s.t. maxpβ T x B , β T y B q ď tanpαqu (6.2)
where α is the camera field of view: the measurement (6.1) is available iff β i P Ω. Later in Sect. 6.6 the visibility constraints will be defined on the unit sphere considering the spherical projection of the image features.

The sensory equipment is completed by an inertial measurement unit (IMU) providing a measure of the robot angular velocities ω and specific force R T p 9 v ǵq at a much higher frequency than the camera frame rate. We assume that a state estimator, such as the ones described in [START_REF] Weiss | Monocular Vision for Long-term Micro Aerial Vehicle State Estimation: A Compendium[END_REF][START_REF] Spica | An Open-Source Hardware/Software Architecture for Quadrotor UAVs[END_REF], uses the visual and inertial 6.2. Reactive target tracking: a minimum-time optimal problem measurements to provide an estimation of the current robot state at the IMU rate. Note, however, that between two image frames, the robot pose estimation can only be updated by dead-reckoning of the IMU data. Due to noise and IMU biases, this "inter-frame" estimation is expected to be of much lower accuracy than the one obtained after visual measurements.

Problem definition

Thanks to the flatness property it is possible to move the trajectory planning problem from the control space to the output space. Since the flat transformation is invertible, as it has been shown in Appendix A, we can transform the conditions on the initial and final states in equivalent conditions on the flat outputs and their derivatives that we indicate with As shown in Sect. 3.2 Problem 5 does not impose any constraint on the initial and final states, however, if both χ t and χ ˚are hovering states, β i ptq P Ω, β i pt`T q P Ω, @i " 1, . . . N , and the hovering input u " pmg{4, mg{4, mg{4, mg{4q P U is not an isolated point in U , then a solution to Problem 5 always exists. Indeed, in this case, it is always possible to find a sufficiently large T such that the robot moves in near-hovering conditions, along a feasible and almost straight trajectory from the initial pose to the desired one [START_REF] Mellinger | Minimum snap trajectory generation and control for quadrotors[END_REF][START_REF] Spica | Aerial Grasping of a Moving Target with a Quadrotor UAV[END_REF]. Now, due to the absence of rotational motions, the linear trajectory in 3-D space is also mapped to linear trajectories of 6. Aggressive vision-based trajectory generation the image features from β i ptq to β i pt `T q. Thanks to the convexity of the image domain Ω, this guarantees that the feature visibility will be maintained.

σ i " pr i , v i , 9 v i , : v i , ψ i , 9 ψ i q σ f " pr f , v f , 9 v f , : v f , ψ f , 9 ψ f q (6.
Problem 5 contains non-linear algebraic and differential constraints and, to the best of our knowledge, does not admit an explicit analytic solution. As it is often the case in these situations, we then attempt to find a sub-optimal solution using a numeric resolution strategy as discussed in the next section.

Numerical resolution

In its original form, Problem 5 is not suited for a direct numerical resolution. First of all, the system dynamic equation (6.4d) represents a non-linear differential equality constraint, which is particularly hard to deal with in a numerical resolution scheme. In addition to this, the search space of the problem (the control input time law uptq) is infinite dimensional. As explained in Sect. 3.5 in order to overcome these problems, we exploit differential flatness for eliminating constraint (6.4d) and we use B-spline parametrization to obtain a finite representation of the search space. As mentioned in the above section, the robot can move from any state to any other, provided that the limits on the propeller rotational speeds are not too strict. As a consequence, whatever are the initial and final states there always exist a feasible trajectories. Nevertheless with the introduction of the B-spline parametrization, we have reduced the search space so that it may not contain these feasible transfer trajectories. Moreover, even when our search space contains some feasible solutions, 6.4. Recursive online control it may not contain the optimal one in the sense that we might still find a better solution if we enlarged the search space.

Problem 5 can be restated as a NLP as follows.

Problem 6 find P , T , such that:

min P ,T T s.t. σ χ ptq " σ χt , σ χ pt `T q " σ χ ˚,
β i psq P Ω, @s P rt, t `T s, i " 1, . . . , N, upsq P U , @s P rt, t `T s, (6.5a) (6.5b) (6.5c) (6.5d) (6.5e) where σ χt " φ ´1 χ pχ t q and σ χ ˚" φ ´1 χ pχ ˚q.

At this point, any general-purpose optimization strategy can be used to find a numerical solution to Problem 6. Unfortunately, due to the non trivial non-linearity of (6.5d-6.5e), Problem 6 cannot be proven to be convex. The optimization will thus, in general, return a local minimum.

Recursive online control

Once Problem 6 is solved, the resulting flat output trajectory could be used in (2.23) for computing the control inputs u to be fed to the system. In practice, however, different sources of disturbance (e.g. noise, miscalibrations, neglected dynamics, and so on) will make the robot to quickly diverge from the planned trajectory when using such an open-loop control strategy. In order to cope with these uncertainties and disturbances, we then incorporate a feedback action in the considered optimization schemes.

The reasons are multiple: i) since the B-spline order is minimal the snap is piecewise continuous and the inputs can be very sharp (see Fig. 6.3). If one sends such values to the controller we can observe a deviation from the original trajectory. Now, if one increases the B-spline order we can see that the system model integration on smoother inputs result in a more accurate resulting trajectory. ii) having an extra feedback action from the controller provides more robustness to uncertainties and a higher stability.

As already introduced in Sect. 4.2 we take inspiration from Model Predictive Control [START_REF] Michalska | Robust receding horizon control of constrained nonlinear systems[END_REF] to perform an on-line re-planning of an optimal trajectory by solving Problem 6 each time a new visual measurement is available. By doing so, we expect to improve the system performance while, more importantly, ensuring the satisfaction of the visibility constraint (6.5d). Finally, instead of feeding the optimal inputs sequence directly to the system we send the optimal trajectory to the trajectory controller [START_REF] Lee | Geometric tracking control of a quadrotor uav on se (3)[END_REF] as in [START_REF] Lee | Geometric tracking control of a quadrotor UAV on SE(3)[END_REF][START_REF] Mellinger | Minimum snap trajectory generation and control for quadrotors[END_REF] at the solver rate.

On the one hand, this allow to reject, to some extent, the disturbances acting on the system. On the other hand, however, the optimality of the resulting trajectory can be compromised and, more importantly, the visibility constraints (6.5d) can be violated.

Trajectory re-planning strategy

A major issue in the implementation of receding horizon control is handling the computational delay associated with the real-time optimization. We present here our method for designing an initial guess and take delay into consideration for the re-planning.

The re-planning strategy is best explained by a visual example, shown in Fig. 6.4.

Let us assume that, in a previous planning step, at time t " t k´1 , the resolution of Problem 6 generated a trajectory σ k´1 , represented in red in the figure. The system is now at time t " t k and a new visual measurement becomes available to be used in the innovation step1 of the state observer to produce an estimation of the current system state χt . This estimation will, in general, be different from the expected system state Φ χ pσ k´1 ptqq due to the non-idealities mentioned above. A new optimal trajectory should, hence, be planned by solving Problem 6 and using the current state estimate to compute the initial condition σ χt .

Unfortunately, the resolution of Problem 9 requires a non-negligible time to complete. This time will, in general, vary, depending on the quality of the initial Figure 6.4 -Single instance of the re-planning process. The red line represents the trajectory computed in a previous planning iteration. The robot is following this trajectory when, at time t k , a visual measurement and a new state estimation become available (green dot). The red trajectory is split and clamped to this measurement, resulting in the green line. The first part of this latter is immediately used as reference for the controller. The second part (the dashed green line) is fed as initial guess to the solver of Problem 6, and also used to predict the state in which the system will be at time t k `δt p , when the optimization will be over. Finally, the blue line is the new optimal trajectory resulting from the numerical resolution of Problem 6. The process is repeated again at t k`1 , when a new measurement is available.

guess for the optimization variables, on the number of necessary iterations and on the available computational resources. Here, for simplicity, we assume that the processing will be concluded after, at most, a constant maximum duration δt p , possibly by introducing a watchdog timer and accepting an intermediate sub-optimal solution. For computing the system control inputs while the optimization is running, we simply "adapt" the previous trajectory to the new initial conditions by using a fast procedure that does not involve the resolution of Problem 6. First of all, we split the trajectory σ k´1 at time t k , as described in Sect. 6.4.2, to extract only its second part σ k´1 (the dashed red curve in Fig. 6.4). Then, we look for a new trajectory σ ḱ (represented in green in Fig. 6.4) that is "as close as possible" to σ k´1 , but starts from Φ ´1 χ p χt q. Details about this step are provided in Sect. 6.4.3. Note that this "temporary" trajectory σ ḱ is sub-optimal and its calculation does not take into account any of the actuation and visibility constraints (6.5d-6.5e), which, as a consequence, could be violated. However, we accept this risk in order to be able to provide an immediate update of the reference trajectory to the new state estimation while a better solution is being computed by appropriately resolving Problem 9 as follows.

Aggressive vision-based trajectory generation

During the optimization process, the system will, most probably, move away from the current state χ t . As a consequence, if χptq were used as initial condition in (6.4b) (or, equivalently, (6.5b)), the newly planned trajectory would not start from the actual state of the robot at time t `δt p . We mitigate this problem by using the trajectory σ ḱ also to predict (by a simple B-spline evaluation) the value of the flat outputs corresponding to the state χtopt in which the system will be when the optimization will be over. This value is used as initial condition in Problem 6.

Finally, since we use recursive optimization methods to find a solution to Problem 6, we also need to provide an initial guess for the optimal trajectory. This initial guess is computed by splitting the trajectory σ ḱ at time t `δt p (green dashed line in Fig. 6.4) as described in Sect. 6.4.2 and taking the second part (green dashed line in Fig. 6.4) of the trajectory.

The optimization can finally run and a new optimal trajectory (the blue one in Fig. 6.4) will be generated. Such trajectory will be used to control the system starting from time t `δt p until a new measurement becomes available at time t " t k`1 . At the arrival of a new measurement the above procedure is repeated. This strategy allows to re-plan online an optimal trajectory each time a new visual measurement is available. Each one of the generated trajectories could be used directly in (2.23b) to calculate the robot inputs. As already mentioned, however, an alternative possibility is, instead, to use them as references for a fast trajectory tracker. This second possibility is appealing because it allows to fully exploit the sensing capabilities of the robot: between two visual measurements, in fact, an estimation of the quadrotor state can be obtained, at a much higher frequency, by using the IMU for dead reckoning. A fast trajectory tracker can, thus, use this information to reduce the effect of non-idealities between two planning steps.

Note that, as the quadrotor approaches the desired state, the planning distance and time horizon tend to zero, potentially introducing numerical issues in the resolution of Problem 6. To overcome this problem, when the system is close to the desired goal, we deactivate the re-planning and directly feed the trajectory tracker with the desired state χ ˚.

B-spline splitting

An advantage of using B-spline trajectories for motion planning is that there exist lightweight and easy algorithms to perform different manipulations on their shape. One such manipulation, that we perform multiple times in the recursive algorithm described in Sect. 6.4.1, is the splitting. Details about how to split a B-spline curve at a point and how to calculate the knots and control points of the resulting parts can be found in many sources, such as [START_REF] Patrikalakis | Shape Interrogation for Computer Aided Design and Manufacturing[END_REF].

Recursive online control

An undesirable effect of the splitting operation is that it also modifies the knot sequence and possibly (depending on the position of the split) even eliminates some knots. In order to maintain a constant number of uniformly distributed knots (and thus a constant number of control points acting "evenly" on the whole spline length), after the split, we perform a sequence of knot insertion and knot removal operations (see [START_REF] Patrikalakis | Shape Interrogation for Computer Aided Design and Manufacturing[END_REF]) meant to redistribute the knots of the new trajectory evenly. De Boor's algorithm is a generalization of de Casteljau's algorithm. It provides a fast and numerically stable way for finding a point on a B-spline curve given a u in the domain. The core of the algorithm lies in the knot multiplicity rule: if a knot u is inserted m times to a B-spline/NURBS curve, the last generated new control point is the point on the curve that corresponds to u. Meaning that we only need to insert u enough number of times so that u becomes a knot of multiplicity m. If u is already a knot of multiplicity s, then inserting it m ´s times would be sufficient. Indeed, after inserting u m times, the triangular computation scheme yields one point. Because the given B-spline/NURBS curve must pass by this new point, it is the point on the curve corresponding to u. Note that this argument holds even if u is inserted as an existing knot. The depicted procedure is more formalized in Appendix B.2.1. This technique is also applied to evaluate the spline at u. We simply need to insert u m times and the last point is ppuq Since the given B-spline curve is subdivided at its knots, each curve segment has no internal knots. Moreover, the subdivision process makes the internal knots to have multiplicity m `1, and the curve segment is "clamped" at the first and last control points of each curve segment.

In the process of subdividing a B-spline curve, a large number of control points will be introduced. Therefore, manipulating a B-spline curve is easier than manipulating its component Bézier curves. Moreover, the B-spline curve of degree p is C p´m continuous at a knot point, where m is the multiplicity of the corresponding knot. When we manipulate a B-spline curve by moving control points, this continuity is always maintained. However, if a B-spline curve is subdivided into a sequence of Bézier curves, maintaining the continuity at the joining control points would be a challenging task. Consequently, handling a B-spline curve is much easier than handling a sequence of Bézier curves.

Adapting previous trajectories to new initial conditions

In this section we describe how to efficiently "adapt" a previously computed B-spline trajectory (e.g. the trajectory σ k´1 represented by a red dashed line in Fig. 6.4) to a new estimation of the current robot state (green dot in Fig. 6.4). To perform this operation we exploit two important properties of B-splines:

Aggressive vision-based trajectory generation

• The local support property stands that the shape of the curve in a knot span ps k , s k`1 q is only determined by a subset of k of the B-spline control points.

• The convex hull property guarantees, instead, that in each knot span, the spline curve is locally contained in the convex hull of the same subset of control points. In practice this allows to conclude that changing the first control points (those determining the initial state of the system) will not affect the shape of the spline towards its end (in particular the final system state will not change) and that two splines with similar control points (according to some norm) are also geometrically close to each other.

Given a spline σ k´1 , with control points P , the control points P ´of the new spline σ ḱ can then be computed by solving the following linear quadratic optimization.

Problem 7 Find a vector of control points P ´such that

min P ´1 2 nr ÿ j"1 › › ›r j ´rj › › › 2 `1 2 n ψ ÿ j"1 › › ›ψ j ´ψj › › › 2 s.t. σ χ ptq " σ χt , (6.6) (6.7)
Note that Problem 7 does not take into account the actuation and visibility constraints in (6.5d-7.9h). While we cannot formally guarantee that these constraints will not be violated, we want to stress that the resulting trajectory is only used for a short amount of time, namely the time needed for the numerical resolution of Problem 6. Introducing a saturation of the control commands one still guarantees the satisfaction of (7.9h) at the cost of introducing a deviation of the robot from its nominal trajectory. Finally, by introducing some security margins in the definition of Ω, one could also reduce the probability of losing feature track in practice.

Simulation setup and results

In this section we report the results obtained by using our planning method in a physically realistic simulation environment. In this thesis we used the on-the-shelf optimization library NLOPT [START_REF] Johnson | The nlopt nonlinear-optimization package[END_REF] that we present below.

The NLOPT algorithm

NLOPT is a free/open-source library for nonlinear optimization and implements a number of optimization algorithms routines in including: It also provides stopping routines to stop iterating once some termination criterion is satisfied, e.g., maximal 6.5. Simulation setup and results number of iterations or function-value f tol, step tolerance xtol and especially the maximal running time (which is not the case of ACADO) to control the minimal variations in local searches and to stop when sufficient precision is reached. This feature is desirable for our re-planning strategy.

More precisely, we use the SQP C++ routine implemented in NLOPT as the SLSQP (Sequential Least-Squares Quadratic Programming) algorithm from [START_REF] Kraft | A software package for sequential quadratic programming[END_REF]. It optimizes successive second-order (quadratic/least-squares) approximations of the objective function, with first-order (affine) approximations of the constraints. The approximations of the objective function are done via the Broyden-Fletcher-Goldfarb-Shanno method (BFGS) to build an approximation of the Hessian matrix.

Simulation results

The quadrotor dynamics were simulated using V-Rep 2 with a time step of 6 ms. The planning strategy described in Sects. 6.3 and 6.4 was implemented in C++ and the SQP method of NLOPT was used to numerically resolve Problem 6. The generated trajectories were sent to TeleKyb which then computed the actual control inputs using controller [START_REF] Spica | An Open-Source Hardware/Software Architecture for Quadrotor UAVs[END_REF][START_REF] Lee | Geometric tracking control of a quadrotor UAV on SE(3)[END_REF].

We simulated visual measurements at a rate of 15Hz for four targets positioned in p˘0.2, ˘0.1q. In our implementation, each planning operation (resolution of Problem 6) takes about 30ms during which the system uses an adaptation of a previously planned trajectory, obtained by resolving Problem 7. Thus, a new trajectory is sent to the controller at the rate of 30Hz. The simulated camera had a field of view of 90 degrees (α " π{4) and each propeller could generate thrusts between 0.1 N and 7 N. For realism purposes, we introduced a Gaussian noise into the state measurements (up to 2% absolute error) and into the motors thrust sent by the controller (up to 5% absolute error). We also purposely used different inertial parameters for the re-planning algorithm and for the actually simulated quadrotor in V-Rep in order to introduce presence of (typical) modelling errors between planned trajectory and actual execution. In particular, we used the following values: mass Inertia matrix (diagonal) Planning 1.0 (0.01562 0.01562 0.03125) Simulation 1.08 (0.016 0.0145 0.027) Table 1. Inertial parameters used for the re-planning and in V-Rep Figure 6.5 shows some snapshots of the simulation. The robot started from an initial hovering state at r " p´1.1, 1.1, 2q and ψ " 1.6 rad and was required

Aggressive vision-based trajectory generation

to reach another hovering state with r ˚" p0, 0, 0.6q and ψ ˚" 0. The solid red line in Fig. 6.5 shows the resulting quadrotor trajectory in space while the blue line represents the currently planned trajectory. Figure 6.6 shows the predicted evolution (given the currently planned trajectories) of the four points in the image plane at equally spaced time instants. The actual evolution of the four image point coordinates is shown in Fig. 6.7 whereas Fig. 6.8 shows the thrust generated by each propeller. The dashed lines in Figs. 6.6 to 6.8 represent the constraints.

The robot was able to accomplish the task in a total time of approximately 2.3 s over which the trajectory planning algorithm was triggered 34 times.

During motion, the quadrotor reached a translational speed up to 1.0 m{s along the X axis, and rotations up to 20 deg as illustrated in Fig. 6.9. From Fig. 6.7 one can see that the features moved very close to the limits of the field of view. Finally Fig. 6.8 shows that also the motor thrusts hit the actuation limits. These results clearly show that the performed trajectory was rather aggressive and that the actuation and sensing capabilities of the robot were exploited. Therefore, we showed that in the presence of modelling uncertainties and noise, the feedback introduced by updating the reference trajectory was able to reject some of these disturbances while satisfying the several constraints. We encourage the reader to watch the video3 attached to the concerned contribution [START_REF] Penin | Vision-based minimum-time trajectory generation for a quadrotor uav[END_REF]: there, we show how an "open-loop" execution of the initially planned trajectory quickly fails to meet the visibility constraints because of the (purposely introduced) actuation noise and model uncertainties. On the other hand, as discussed, the online re-planning allows gaining a sufficient level of robustness against these non-idealities. As it is common, a high planning rate is privileged against optimality to some extent.

The rest of this chapter presents our second contribution [START_REF] Penin | Vision-based reactive planning for aggressive target tracking while avoiding collisions and occlusions[END_REF] that addresses more complex vision-based tasks.

Vision-based target tracking

In this section, we first address the case of tracking a moving 3D target with a front-looking camera and ensuring final visibility of the target at the (unique) image center. We assume that the relative pose of the target can be estimated using vision only as done in [START_REF] Thomas | Autonomous flight for detection, localization, and tracking of moving targets with a small quadrotor[END_REF] with some preliminary knowledge of the target model (e.g., the target radius in case of a sphere). We are also interested in allowing the quadrotor undertaking aggressive manoeuvres for reaching the target in near minimum-time conditions. The final goal is to reach a hovering state such that the target appears at the image center while keeping a safety distance from the target (see Fig. 6.10).

Vision-based target tracking

Figure 6.10 -The quadrotor has to follow the target in red while keeping a safety distance represented by the light blue sphere. The terminal constraint in the image space is represented by the green circle. The blue circle represents the field of view inside which the feature trajectory must lie. Here, the optimal trajectory in blue steers the quadrotor towards a final position (white dot) and the resulting image feature trajectory is the one in white

Multi-objective cost function

For achieving the aforementioned behaviour we adopt multi-objective programming and minimize the following weighted cost function at running time t with weights

w i ą 0 P R. J " w 1 ż t`T t } ... vptq} 2 dt `w2 ż t`T t › › › : ψptq › › › 2 dt `w3 ż t`T t }dptq ´Rs } 2 dt `w4 ż t`T t }vptq} 2 dt (6.8)
The latter is divided into four parts: the first and the second terms minimize the norm of the snap and the norm of the yaw acceleration respectively. They are used for encouraging smoothness while still exploiting the quadrotor aggressiveness [START_REF] Mellinger | Minimum snap trajectory generation and control for quadrotors[END_REF]. The third term minimizes the error between the Euclidean distance dptq of the target to the camera and a value R s defined as a safety distance: the radius square of the sphere centred on the target at position q (see Fig. 6.10). Naturally, to achieve more aggressive trajectories, one can enforce this cost. The fourth term penalizes the path length. It is an approximation of the sum of the path segments ř k"N ´1 k"0 }rpk `1q ´rpkq} 2 which appears to be equivalent to penalizing the linear velocity over the time horizon. Indeed, for a circular symmetric target the quadrotor may fly around the target indefinitely without changing the image feature position, an effect arising from the residual available degrees of freedom and that one can prevent by adding this fourth term.

In the previous section (Sect. 6.2) we minimized the completion time T to generate aggressive trajectories [START_REF] Penin | Vision-based minimum-time trajectory generation for a quadrotor uav[END_REF] . Here, we are dealing with more complex constraints and we will show that we can still exploit the quadrotor's agility. Moreover, we avoid the penalization of such a complex parameter and the re-evaluation of the 6. Aggressive vision-based trajectory generation B-spline bases at each solver iteration. The completion time T is then a fixed parameter and should be chosen as a rough upper bound of the time required to reach the final pose vicinity. If T is too small, the trajectories might be infeasible. If too high, the system may be less reactive.

Finally, by suitably weighing these costs we are able to trigger the expected behaviour and prevent the objectives from conflicting with each other.

Visibility constraints

In this section we propose an alternative to (6.5d) for the formulation of constraints on the image features. Let us define the spherical projection of a target point in 3D w.r.t. the frame B as the bearing vector β " R T pr ´rq }R T pr ´rq} " m }m} P S (6.9)

where r P R 3 is the position of the feature in the world frame and S is the surface of the unit sphere and m " pu, v, 1q is the image measurement from which β is computed.

Defining e c as the camera optical axis in the frame B, namely, x B (or y B ) for a front-looking camera and ´zB for a down-looking camera, the visibility constraint is written as β T ptqe c ě cospα{2q, @s P rt, t `T s (6.10

)
where α is the angle of view of the camera and β is given by (6.1). Fig. 6.11 shows that (6.10) is equivalent as constraining the feature bearing angle β but whose numerical evaluation is more complex. In contrast to the previous formulation (6.5d) a single constraint is defined using (6.10). However, the field of view of the camera is modelled as a cone (see Fig. 6.10) which is a less realistic representation than a pyramid-shaped field of view. Now, instead of imposing equality constraints on the final position in order to guarantee visual convergence of the target to the camera center, we rather define a terminal constraint such that the feature bearing angle has to belong to a (smaller) angular area at the camera centre (see Fig. 6.10) with

β T ptqe c ě cospγ{2q, @s P rt `Tv , t `T s, 0 ď T v ď T (6.11)
where γ is the angle defining the circular region of convergence in the image. The time T v is a parameter that defines at what time the feature shall enter the vicinity region. It can be tuned to affect the convergence rate towards this region. Namely, a value closer to 0 will demand a longer activation of the constraint. T v will also vary depending on the camera orientation due to the quadrotor dynamics. With the above definitions, one can encapsulate visibility constraints and visual convergence for any camera orientation. The approach considered in [START_REF] Sheckells | Optimal Visual Servoing for Differentially Flat Underactuated Systems[END_REF] penalizes the motion aggressiveness since it minimizes the deviation of the image features from the center of the camera in the image plane. Therefore it does not fully exploit the image space while, in our case, the target is free to move away from the camera center in order to allow large rotations of the camera and therefore large accelerations of the quadrotor.

Following the same strategy as in Sect. 6.2, we exploit differential flatness and parametrize the flat outputs with B-splines with control points P . We define the following problem with the cost function (6.8) Problem 8 Find P such that: min P J s.t. σ χ ptq " σ χt , σ piq pt `T q " σ χ ˚, i " 1, . . . , 3, upsq P U , @s P rt, t `T s, β T ptqe c ě cospα{2q, @s P rt, t `T s, β T ptqe c ě cospγ{2q, @s P rt `Tv , t `T s (6.12a) (6.12b) (6.12c) (6.12d) (6.12e) (6.12f)

Simulation and experimental results

The video4 attached to this work [START_REF] Penin | Vision-based reactive planning for aggressive target tracking while avoiding collisions and occlusions[END_REF] shows the reactive target tracking considering the scenario depicted in Fig. 6.10 to validate the proposed replanning strategy with the visibility constraints detailed in Sect. 6.6.2. Figure 6.12 shows successive snapshots of the simulated environment. We show in Fig. 6.13 that the computed inputs and images features trajectories are maintained within their allowed domains. We exploit again the replanning strategy described in Sect. 6.4.1 and the SQP optimization routine from NLOPT to compute the optimal solutions.

Chapter 7 On collisions and occlusions avoidance

In this chapter we address vision-based navigation in the presence of obstacles for target tracking applications. Since we consider the visibility constraint on the tracked target it is crucial to also avoid the occlusions generated by the obstacles themselves with the target. To do so, we design a new constraint formulation in the image space by drawing the analogy with volumetric constraints used for collision avoidance. In order to improve the convergence towards a local minimum we use complex-step differentiation (CS) to efficiently approximate the gradients of the nonlinear terms in the cost function and of the nonlinear constraints.

Contributions

In Sect. 6.2 we presented preliminaries for online minimum-time trajectory replanning under field of view constraints. In this chapter we show that reactive manoeuvres can still be achieved without minimizing time and in the presence of more complex constraints. Besides, we improve the accuracy and the numerical stability of the gradients evaluation by using complex-step differentiation which aids the SQP convergence. Our method differs from most relevant works (e.g., [START_REF] Nicolis | Occlusion-free visual servoing for the shared autonomy teleoperation of dual-arm robots[END_REF][START_REF] Kazemi | Randomized kinodynamic planning for robust visual servoing[END_REF][START_REF] Agravante | Visual servoing in an optimization framework for the whole-body control of humanoid robots[END_REF]) under three main aspects:

' efficient and reactive online re-planning strategy considering an underactuated robot ' soft occlusion avoidance formulation in the image space ' visual constraints independent of the camera/UAV configuration 7. On collisions and occlusions avoidance

Constraints formulation

In this section, we describe the vision-based optimization scheme for avoiding collisions with static spherical obstacles of radius R occ and the occlusions they might generate with the tracked target, see Fig. 7.1. Collision-free trajectories with N static spherical obstacles of inflated radius R col ą R occ at position r obs are generated using volumetric constraints such as

}r ´robsi } 2 ą R col 2
i , i " 1, . . . , N (7.1)

Figure 7.1 -An optimal trajectory for a quadrotor equipped with a camera is generated for reaching a minimum distance with a target (blue sphere) while avoiding collisions with spherical obstacles (inflated dark spheres) and occlusions of the target from the obstacles (red spheres).

The occlusion constraint can be modelled analogously to (7.1), but in the image plane, as follows

}β ´βobsi } 2 ą a 2 1 , i " 1, . . . , N (7.2)
where β and β obs are the spherical projections (see (6.9)) of the target and the obstacle center respectively, and a 1 is the length of the semi-minor axis of the projected ellipse of the spherical obstacle of radius R occ in the image plane, see Fig. 7.2. Assuming knowledge of the size of an obstacle in 3D with coordinates

r obs " pX o , Y o , Z o q in the image plane one has [193] a 2 1 " R 2 occ 4pZ 2 o ´R2 occ q (7.3)
With this constraint one seeks to prevent the target from colliding the projected ellipses of the obstacles (see Fig. 7.1) in the image space by keeping a minimum distance a 1 that grows as the depth Z o of the obstacle in the camera frame decreases. As the quadrotor moves towards the target the occlusion constraints from the obstacles passing behind the camera are of course discarded.

However, when dealing with occlusion avoidance with a quadrotor there exist configurations where strict avoidance is not feasible. Indeed, when the target goes exactly below an obstacle (for a down-looking camera, see Fig. 7.3) the quadrotor may not have sufficient actuation capability or sufficient space for avoiding any occlusions. These situations may occur for any camera orientation. Therefore, in order to avoid such critical situations and always provide a feasible solution, we introduce a slack variable λ within the occlusion constraint to authorize partial occlusion if necessary. We set a 2 1 " pR occ ´λq 2 4pZ 2 o ´pR occ ´λq 2 q , s.t 0 ď λ ď R occ (7.4) The λ term plays the role of relaxing a hard constraint when the solver encounters not feasible situations. At the most critical configurations when the target is below an obstacle (Fig. 7.3), the collision constraint can be reduced or even cancelled by having λ reach the value R occ which is the actual radius of the obstacle (lower than R col ). With equation (7.4) we observed very reactive responses from the system in case of occlusions. This may be due to the fact that small changes of λ induce a strong action on the occlusion constraint (7.2). Besides, by imposing a straightforward upper bound (R occ ) for λ, its action will take effect only in case of violations of constraint (7.2).

It would make sense of course to use the ellipse semi-major axis defined as:

a 2 2 " R 2 occ pX 2 o `Y 2 o `Z2 o ´R2 occ q 4pR 2 occ ´Z2 o q 2 (7.5)
This formulation would give more conservative occlusion avoidance constraints. However, in practice one has

X 2 o `Y 2 o ! Z 2
o especially if one considers the limited camera field of view. This is why we consider the semi-minor axis (7.3) which also has the advantage of being less complex. The blue trajectory is then a relaxed solution where λ hits its limit value R occ (obstacle radius). The orange trajectory represents the case of a less relaxed occlusion constraint where λ eventually reaches a smaller value and is zero when there are no occlusions (green trajectory)

Of course, the occlusion constraint and the slack variable λ introduce conservatism to some extent. However, the main objective of λ is to improve stability and continuity of the solution in case of critical configurations.

Optimization problem definition

Finally, the current optimization problem related to the scenario shown in Fig. 7.1 can be stated as the following static NLP considering the cost function (6.8) Problem 9 Find P , λ such that: min P ,λ J `w5 }λ} 2 s.t. σ χ ptq " σ χt , σ piq pt `T q " σ χ ˚, i " 1, . . . , 3, upsq P U , @s P rt, t `T s, β T ptqe c ě cospα{2q, @s P rt, t `T s, β T ptqe c ě cospγ{2q, @s P rt `Tv , t `T s }rpsq ´robs } 2 ą R 2 col , @s P rt, t `T s, }βpsq ´βobs } 2 ą a 2 1 , @s P rt, t `T s,

0 ď λ ď R occ (7.6a) (7.6b) (7.6c) (7.6d) (7.6e) (7.6f) (7.6g) (7.6h) (7.6i)
where σ χt " φ ´1 χ pχ t q and σ χ ˚" φ ´1 χ pχ ˚q. For a final hovering state χ ˚one has of course σ piq pt `T q " 0, i " 1, . . . , 3. We choose not to explicitly constrain the final 7.4. A reactive re-planning framework with a down-looking camera Cartesian position which is considered as a free parameter to be determined by the optimization algorithm. We seek feasible trajectories with constraint (7.6d) but, due to unknown target motion, the quadrotor might not have sufficient actuation to always satisfy the visibility constraint (7.6e).

The costs and constraints gradients in the flat space are computed analytically. However, we think it is more efficient to estimate (numerically) the gradients of the visual and inputs constraints instead of deriving their complex and heavy analytic formulation. To do so, we use complex-step differentiation [START_REF] Squire | Using complex variables to estimate derivatives of real functions[END_REF]. It can be shown that the first-order derivative of a function f P R can be approximated as

Bf Bx " �pf px `ihqq h `Oph 2 q, h P R (7.7)
where i is the complex number such that i 2 " ´1 and �pzq denotes the imaginary part of a complex number z. This technique is attractive to determine first derivatives since it only requires a single evaluation of the function and avoids the problem of subtractive cancellation of classic finite approximation (round-off errors). Therefore, it is known to have superior accuracy (close the analytic accuracy) and numerical stability as analysed in [START_REF] Abreu | On the generalization of the complex step method[END_REF]. It is also less intrusive in terms of program transformation than automatic differentiation which can sometimes require large and deep source overloads. On the other hand, complex differentiation requires some mathematical adaptations to be used with complex values (the square root or the absolute value functions for instance) which increases the computational cost. The choice between these two methods then hinges on a trade-off between ease of implementation and execution efficiency and is further discussed in [START_REF] Martins | The connection between the complexstep derivative approximation and algorithmic differentiation[END_REF]. More details and comparison results are given in Appendix C.

A reactive re-planning framework with a down-looking camera

In this section, we express the re-planning strategy for tracking a mobile target on the ground (as shown in Fig. 7.3). We choose to consider a down-looking camera with a (more realistic) pyramid-shaped field of view as in our work [START_REF] Penin | Vision-based minimum-time trajectory generation for a quadrotor uav[END_REF] in order to also show that the planning strategy is able to exploit the corners of the field of view. The perspective projection of the target is again considered

β " R T pr ´rq z B T R T pr ´rq " » - - u v 1 fi ffi fl P P 2 (7.8)
where P 2 is the space of 3-D homogeneous vectors.

On collisions and occlusions avoidance

In this case, we adopt the penalization of the position error between the quadrotor and the target in the XY plane denoted as e xy P R 2 . Moreover, we let the final height z T free but bounded for limiting the motion along e 3 . This lighter formulation will basically compute trajectories for steering the quadrotor to a final hovering position such that the target appears at the camera center.

We define the following optimal problem Problem 10 Find P , λ such that:

min P ,λ J 2 " w 1 ż T t } ... vptq} 2 dt `w2 ż T t } : ψptq} 2 dt `w3 ż T t }e xy ptq} 2 dt `w4 z T `w5 }λ} 2 s.t. σ χ ptq " σ χt ,
σ piq pt `T q " 0, i " 1, . . . , 3, r xy pt `T q " r xy , ψpt `T q " ψ ˚, (arbitrary value) βpsq P Ω, @s P rt, t `T s, upsq P U , @s P rt, t `T s, }rpsq ´robs } 2 ą R 2 col , @s P rt, t `T s, }βpsq ´βobs } 2 ą a 2 1 , @s P rt, t `T s, It should be noted that the gradient of the occlusion avoidance constraint (7.9g) gives three possible descent directions for the decision variables for satisfying this constraint. More precisely, the SQP algorithm can: i) increase the distance between βpsq and β obs , ii) reduce the quantity a 1 by increasing the distance Z o , iii) relax the constraint by increasing λ. In order to restrain the second direction that steers the solution towards the local minima of infinite height (a 1 Ñ 0) we put an upper limit on the quadrotor final height z T with an additional constraint (7.9l). We also minimize z T (linear in P ) to prevent the camera from staying at the maximum height Z max . We also consider a minimum height Z min for preventing the quadrotor from flying below the obstacles since our main focus is to show its ability to avoid occlusions. This is indeed a behaviour the quadrotor may exhibit. This formulation allows efficient re-planning at the rate of 30Hz. Simulation results are given in the next sections.

0 ď λ ď R occ , Z min ď z T ď Z max (7.9a) (7.9b) (7.9c) (7.9d) (7.9e) (7.9f) (7.9g) (7.9h) (7.9i)
We also give more insight on the different system behaviours triggered by different tuning of the weights in the cost function (7.9b).

Simulation results

The presented approach was validated in a physically realistic environment. The inertial parameters of the quadrotor were slightly biased on purpose to introduce model uncertainties. The quadrotor dynamics were simulated using V-Rep at 150Hz. We exploit again the re-planning strategy described in Sect. 6.4.1 and the SQP optimization routine from NLOPT to compute the optimal solutions. The generated trajectories of the flat outputs were sent to TeleKyb which then computed the actual control inputs using controller [START_REF] Lee | Geometric tracking control of a quadrotor UAV on SE(3)[END_REF].

The tracked target shown in Fig. 7.3 is manually commanded in velocity saturated at 1.0 m/s. We considered three spherical obstacles of radius R occ " 0.15m and of inflated radius R col " 0.4m (see Fig. 7.6). The video1 attached to this work [START_REF] Penin | Vision-based reactive planning for aggressive target tracking while avoiding collisions and occlusions[END_REF] shows the pertinence of having occlusion constraints in vision-based navigation since obstacles may occlude the target in many occasions if not considered at the planning stage. Finally, we show the efficiency of our re-planning framework in avoiding immediate and sudden occlusions. When critical situations are encountered the slack variable is able to keep a stable flight by allowing very brief occlusions.

Table 7.1 shows some performance indexes from the on-line resolution of Problem 10 for a flight duration of about 90 seconds. One has respectively: the number of computed trajectories, the duration T max for the stopping criterion, the planning horizon T , the range of SQP iterations and the mean iteration, the step and constraint tolerance and finally the percentage of total optimization instances that failed to return a solution within T max . When it occurs the previous solution is returned. loops T max T iter range SQP iter tol ratio 2734 33ms 3.5s r6, 63s 31 10 ´4 6% Table 7.1 -Settings and performance of the solver for a 90 seconds flight

In the proposed formulations, the weights in the cost function are free parameters and need to be tuned empirically in order to generate the expected behaviour. Since the optimal problem is complex and highly nonlinear, any set of weights can generate 7. On collisions and occlusions avoidance a different response. However, from our experience the tuning triggers consistent responses and does not necessitate a particularly fine analysis. Generally, a higher w 1 will produce smoother trajectories and less aggressive motion. A higher w 3 will increase the convergence rate towards the target and a lower w 5 will tend to produce more relaxed occlusion constraints. Table 7.2 shows the numerical parameters used for the simulation. Moreover, we have used five different sets of weights (Table 7.3) an discuss their consistent effect on the generated trajectories shown in Fig. 7.6.

T

w 1 w 2 w 3 w 4 w 5 3.5s 1e ´5 5e ´3 1e 1 5 5e 3 Table 7.2 -Considered parameters for the simulation Figure 7.4 -Evolution of λ for the simulation. The peaks reveal the presence of sudden occlusions. The flat section (from around instance 1100 to instance 1700) respresents a flight period when the target is far from the obstacles. Therefore, λ is very close to 0. Here λ did not hit the upper limit R occ " 0.15m. The hot-start method presented in our work [START_REF] Penin | Vision-based minimum-time trajectory generation for a quadrotor uav[END_REF] definitely helps the solver as showed in Table 7.4. Using the exact same conditions we compared the performance of the solver with:

' our hot-start algorithm (M1) in Sect. 6.4.1 7.2) with a higher w 3 . One can observe a faster convergence towards the image center (with an overshoot). The blue one has a higher w 1 resulting in a smoother trajectory than the orange one. The green one is even smoother and is more compliant to occlusions (less jerky) since w 5 is lower. The yellow trajectory is very sharp (low w 1 ) because the occlusion constraints are more respected (high w 5 ) and the final height is less penalized (low w 4 ). The pink solution gives a different and smoother path (high w 1 ) that benefits less from the constraints relaxation (since w 5 is high).

Simulation results

Trajectory

w 1 w 2 w 3 w 4 w 5 orange 1e ´5 5e ´3 5e 1 5 5e 3 blue 1e ´4 5e ´3 1e 1 1e ´1 5e 3 green 1e ´3 5e ´3 1e 1 1e 1 1e 3 yellow 1e ´5 5e ´3 1e 1 1 5e 4 pink 1e ´3 5e ´3 1e 1 5 1e 4 Table 7.
' using the previous solution (M2) Methods M2 and M3 clearly fail to meet the solver performance achieved using method M1. Moreover, M3 even quickly led to a failure of the task. The ratio can 7. On collisions and occlusions avoidance quickly escalate. Indeed if the solver fails in returning a solution within 1/30 ms, its last (infeasible) iterate will still be used as initial guess to the next solver instance, probably generating an increasing depreciation of the initial guess.

'

Summary

In this work, we adapted our previous approach [START_REF] Penin | Vision-based minimum-time trajectory generation for a quadrotor uav[END_REF] on vision-based optimal trajectory generation to a wider context by considering reactive target tracking and both occlusions and collisions avoidance for either a front or a down looking camera (or any other camera/UAV configuration).

The quadrotor trajectories are mainly driven by vision while seeking aggressive but smooth trajectories that respect actuation and sensor limits for any camera orientations. Then, starting from a good initial guess the solver is able to return an optimal solution about 94% of the time within 1/30ms allowing an online replanning strategy capable of absorbing noise, disturbances and any non-modelled effect for long duration flights. The same strategy was applied during an experiment using a real quadrotor for the case of a front-looking camera.

In contrast to the literature, we proposed a method that explicitly handles visibility constraints and occlusion avoidance within a fast online re-planning strategy. Besised, the occlusion constraint is expressed as a single constraint per object in contrast to [START_REF] Nicolis | Occlusion-free visual servoing for the shared autonomy teleoperation of dual-arm robots[END_REF]. Finally, we coped with the issue that the discontinuity of occlusion constraints can generate by introducing a minimal relaxation in Sect. 7.2.

The optimization problem in the current contribution differs in several points from the minimum-time Problem 6, especially in terms of costs and constraints. Although the re-planning strategy is the same, we emphasize the general online re-planning efficiency with different optimal problems. Moreover, the complex-step differentiation method plays a non negligible role in the framework. Indeed, optimality and stability of the re-planning framework has been improved by accelerating the gradient evaluation and improving its accuracy and numerical stability compared to our previous work [START_REF] Penin | Vision-based minimum-time trajectory generation for a quadrotor uav[END_REF].

To be successful, our path-planning approach requires a complete knowledge of the environment and robot model. These requirements can be limiting in many real applications. The need for such exact knowledge could be relaxed by accounting for modelling and calibration uncertainties at planning stage. Finally, future work includes validating the method with real (and maybe dynamic) obstacles and the use of vision only for estimating the target relative position. In this case the relaxation term will play an even more decisive role.

8. Toward visual constraints relaxation: planning under intermittent measurements has been derived to address the above problem. Another class of works exploits local optimal control policies assuming a linear quadratic Gaussian (LQG) control strategy. However, these approaches suffer from the "curse of dimensionality", in particular POMDPs are notorious for their computational complexity that may prohibit their application for navigation in complex or uncertain environments in high dimensional state spaces. In [START_REF] Rafieisakhaei | Belief space planning simplified: Trajectory-optimized lqg (t-lqg)[END_REF] a more scalable LQG variant is proposed and applied to environments with discontinuous sensing regions. An approximate solution to POMDPs is given in [START_REF] Bry | Rapidly-exploring random belief trees for motion planning under uncertainty[END_REF] but with the use of considerable pre-processing. To deal with more complex objectives, deterministic planners such as RRT* and A* have become very popular since they benefit from asymptotic optimality and can explore the whole configuration space efficiently. In [START_REF] Gonzalez | Planning with uncertainty in position an optimal and efficient planner[END_REF] a graph-search based on A* is proposed by discretizing the environment into cells for finding a safe route for a unicycle vehicle. Active visual perception with a quadrotor has been addressed in [START_REF] Costante | Perception-aware path planning[END_REF] for determining the path with minimal state uncertainty considering photometric information, and in [START_REF] Davis | C-opt: Coverage-aware trajectory optimization under uncertainty[END_REF] for maximizing visual coverage of a scene in presence of obstacles, localization and sensing uncertainty. Recently, [START_REF] Van Den | Motion planning under uncertainty using differential dynamic programming in belief space[END_REF] proposed an approximate POMDP control policy based on an initial guess trajectory returned by a RRT planner in a discretized environment. Figure 8.1 -Simulation environment for our framework. An optimal collision-free trajectory for the unicycle case connects the initial state (green dot) and a final state (yellow dot) in presence of obstacles (blue boxes). The pose uncertainty is represented by the blue ellipsoids whose size is reduced as soon as a landmark (red bars) is close enough to the robot and enters the field of view of the simulated camera attached to the robot. We assume the landmarks are not occluded by the obstacles. The propagated edges of the two graphs are rendered as the red curves.

In this work, we aim at planning a trajectory from an initial to a final state in presence of obstacles and input constraints for non-trivial robotic systems (like a quadrotor). We assume that the state is not available (especially the position) but on-board sensors (including a camera) are used to reconstruct the state with some estimation algorithm fusing position measurements in the world frame recon-8.1. Introduction structed from vision. Note that these measurements can be intermittent because of limited field of view, maximum range and so on. We want that the path guarantees some desired level of uncertainty in the reconstructed state despite the fact that measurements are not always available. More precisely, the goal state has to be reached with a bounded position uncertainty to guarantee some confidence level on the robot's location. Therefore, the system has to collect sufficient information from visual landmarks sparsely placed in the environment to satisfy this final constraint (see Fig. 8.1 for the unicycle case). Basically, the shape of the trajectory will vary depending on the level of uncertainty, i.e., process and measurement noises (e.g., see [START_REF] Lambert | Safe path planning in an uncertain-configuration space[END_REF] with a unicycle) but also on the given initial and final states which is, to the best of our knowledge, not the case in the literature.

Literature in perception-aware planning has generally focused on maximizing observability [START_REF] Costante | Perception-aware path planning[END_REF][START_REF] Zhang | Perception-aware receding horizon navigation for mavs[END_REF][START_REF] Candido | Minimum uncertainty robot navigation using information-guided pomdp planning[END_REF] (or minimizing the state uncertainty) based on some criteria e.g., the trace or the smallest eigenvalue of the covariance matrix. This strategy definitely helps in finding a path that tries to collect as much as information as possible for preventing the state uncertainty to increase too much. However, the path itself may be severely suboptimal in terms of length and duration (e.g., [START_REF] Hausman | Observability-aware trajectory optimization for self-calibration with application to uavs[END_REF] for optimal self-calibration of UAVs). Indeed, the path length is generally not constrained and can be excessively long, especially if the robot needs to pass by all the regions/beacons with richest information. Figure 8.2 -A minimum snap (red) passing through waypoints and a trajectory (green) yielding well-observable states. This trajectory is much longer and much more complex, from [START_REF] Hausman | Observability-aware trajectory optimization for self-calibration with application to uavs[END_REF].

In this work, we propose a minimum-time planning algorithm for dynamic systems returning feasible and robust trajectories that do not guarantee minimal state uncertainty along the trajectory but a bounded state uncertainty with given bounds at the goal, which we consider is a more practical application.

8. Toward visual constraints relaxation: planning under intermittent measurements

Contributions

This work focuses on finding robust paths for a robotic system by taking into account the state uncertainty and the probability of collision. We are interested in dealing with intermittent exteroceptive measurements (e.g., collected from vision). We assume these cues provide reliable measurements that will update a state estimation algorithm wherever they are available. The planner has to manage two tasks: reaching the goal in a minimum time and collecting sufficient measurements to reach the goal state with a given confidence level. We present a robust perceptionaware bi-directional A* planner for differentially flat systems such as the unicycle and the quadrotor UAV and use a derivative-free Kalman filter to approximate the belief dynamics in the flat space. We also propose an efficient way of ensuring continuity and feasibility between the graphs by exploiting the convex-hull property of B-spline curves.

In a previous chapter (Sect. 7.6) we considered hard visibility constraints that may become too restrictive for minimum-time planning. In this chapter, we propose to relax these constraints by allowing intermittent visual cues losses to perform faster trajectories in larger and more complex environments. We implement a bidirectional A* algorithm that grows two graphs, one from the initial state and one from the final state (see Fig. 8.4). A solution trajectory is built by connecting the two graphs. This work blends the following features within graph-search algorithm: (i) incorporation of model and sensor uncertainty in collision avoidance and perception, (ii) generation of minimum-time and feasible trajectories for flat dynamic systems, (iii) incorporation of discontinuous visual measurements that are function of the robot's attitude, (iv) efficient graph connection using the convex hull property of B-spline curves.

Our work is mostly based on the recent work of [START_REF] Liu | Search-based motion planning for quadrotors using linear quadratic minimum time control[END_REF] and [START_REF] Liu | Search-based Motion Planning for Aggressive Flight in SE (3)[END_REF] that propose an efficient A* planner in the flat space of a quadrotor which is applied to aggressive and precise collision avoidance that is function of the robot attitude in cluttered environments. In contrast to [START_REF] Liu | Search-based Motion Planning for Aggressive Flight in SE (3)[END_REF] we include perception constraints and state uncertainty and directly minimize the time. To the best of our knowledge, this is the first time minimum-time trajectories are generated in a graph-search planner while accounting for uncertainty in the visual perception which is affected by the system's attitude. We apply this method first to a unicycle for illustrating the approach, and then also to the case of a quadrotor for demonstrating the feasibility on a much more complex system while performing aggressive motions.

The rest of this chapter is organized as follows. Sect. 8.3 introduces differential flatness for the unicycle. Sect. 8.4 presents the uncertainty-aware planner formulated as a graph-search problem. How the graph is built is described in Sect. 8.5. The 8.3. Preliminaries graphs rewiring is detailed in Sect. 8.6. In Sect. 8.7 our method is extended to the quadrotor case. Simulation and experimental results are given in Sect. 8.8 for both robotic systems. Finally we draw some conclusions and future directions in Sect. 8.9.

Preliminaries

Differential flatness for the unicycle

As already said in Sect. 2.5 differentially flat systems are systems whose states and inputs can be expressed as algebraic functions of flat outputs derivatives up to some suitable order [START_REF] Fliess | Flatness and defect of nonlinear systems: Introductory theory and examples[END_REF]. Differential flatness is often used for planning purposes since it reduces the problem size. We will exploit differential flatness for the unicycle in this section and for the quadrotor in Sect. 8.7. As usual, the kinematic model of a unicycle is

$ ' & ' % 9 x " v cospθq 9 y " v sinpθq 9 θ " ω (8.1a) (8.1b) (8.1c)
where v and ω are the forward and angular velocities inputs of the robot respectively while px, yq are the coordinates of the center of the rear axle and θ is the robot orientation in the world frame. It can be shown that the unicycle is flat with flat outputs [START_REF] Tang | Differential flatness-based kinematic and dynamic control of a differentially driven wheeled mobile robot[END_REF]:

η " px, yq (8.2)
Namely, the system can be fully linearised and described by a double integrator with state s " pη, 9 ηq P R 4 :

x " u 1 , :

y " u 2 (8.3)
where u 1 and u 2 are the new control inputs. The real system inputs and angular position can be obtained from the flat outputs and their derivatives as follows v " a 9

x 2 `9 y 2 , ω " 9

x: y ´9 y :

x 9 x 2 `9 y 2 , θ " Arctan2 ˆ9 x 9 y ˙(8.4)
8. Toward visual constraints relaxation: planning under intermittent measurements

We will see in the next section why it is interesting to propagate the above dynamic model (8.3) instead of (8.1a) to extend the two A* graphs.

Problem formulation

We aim at solving an optimal control problem connecting an initial state s 0 and a final state s T in a minimum time T . Let X f ree " r v, vsˆr ω, ωs denote the admissible input space. Let us define the following optimal control problem.

Problem 11 Find the input up.q and time T such that:

min up.q,T T s.t. sp0q " s 0 , spT q " s T ,
maxteigpP η pT qqu ď λ, pvpτ q, ωpτ qq P X f ree @τ P r0, T s

where eigpP η pT qq P R 2 contains the eigenvalues of the position covariance matrix at the goal state s T . The desired bound on the position uncertainty is defined by λ ą 0. For nonlinear systems such as a unicycle (and a quadrotor), the Extended Kalman Filter (EKF) is often used for approximating the belief dynamics. The EKF is based on a linearization of the system dynamics which results in cumulative errors due to the local linearization assumption. In this work, we stick to the flat space in order to perform a derivative-free Kalman filter without the need for derivatives and Jacobians calculations. Moreover, the state estimation accuracy of a derivativefree Kalman filter can be improved w.r.t. a standard EKF, especially for nonlinear systems [START_REF] Rigatos | Derivative-free kalman filtering for autonomous navigation of unmanned ground vehicles[END_REF]. Finally, we impose input constraints with (8.5e). Considering the linear equivalent system one defines the process model. When a landmark is visible we have

9 s " As `Bu `ζ, y " Cs `ν (8.6a) (8.6b)
where ζ P R 4 is the process noise and ν P R 2 is the measurement noise. Assuming the velocity is estimated through filtering of position measurements, the above matrices are given by

A " ¨0 0 1 0 0 0 0 1 0 0 0 0 0 0 0 0 ‹ ‹ ‹ ‹ ' , B " ¨0 0 0 0 1 0 0 1 ‹ ‹ ‹ ‹ ' , C " ˜1 0 0 0 0 1 0 0 ¸(8.7)

Problem formulation

In the next sections we show how Problem 11 can be transformed from an infinite dimensional optimal control problem to a finite dimensional graph-search problem. We choose to extend two graphs to increase the chance and the rate of convergence to a solution, especially in complex and cluttered environments. Moreover, it generally propagates fewer vertices than with a single graph [START_REF] Qureshi | Intelligent bidirectional rapidly-exploring random trees for optimal motion planning in complex cluttered environments[END_REF].

Motion primitives

As in [START_REF] Liu | Search-based motion planning for quadrotors using linear quadratic minimum time control[END_REF] we use polynomials to parametrize the flat state components and generate motion primitives to explore the flat space in a discrete way. More precisely, by applying a number of sampled constant acceleration inputs (see (8.3)) along each axis u k P U M :" r´u max , . . . , u max s 2 for a duration τ ą 0 one can iteratively build a graph GpV, Eq rooted in state s 0 , where V is the set of discrete states denoted as vertices s in the graph representation that are connected with a motion primitive referred as an edge in the set E (e.g., see Fig. 8.1). A motion primitive represents the state sptq starting at a state s 0 on t P r0, τ s with a curve defined as sptq " M pu m , s 0 , tq :"

« u m t 2 2 `9 η 0 t `η0 u m t `9 η 0 ff (8.8)
These trajectories reflect the system dynamics thanks to differential flatness and provide the minimum acceleration between the states s 0 and spτ q [START_REF] Mueller | A computationally efficient motion primitive for quadrocopter trajectory generation[END_REF]. The free flat space will be explored with a propagation of these motion primitives further detailed in Sect. 8.5.1. Naturally, changing the input bounds and duration τ will affect the free space coverage.

Problem 11 can be reformulated as Problem 12 in the graph representation where we seek the trajectory connecting the initial and goal states with the optimal control sequence u k and the minimal number N ˚of motion primitives.

Problem 12 Find the sequence u k and N such that:

min u k ,N N s.t. s 0 " s init , s N " s goal , maxteigpP η
N qu ď λ, zpu k , s i , tq P X f ree @i P v0, N w where P η N is the covariance matrix on the position at the goal vertex, function z : pu k , s i , tq Þ Ñ rvptq, ωptqs T computes the system inputs on discretized states sampled between sptq and spt `τ q along a primitive curve. The resulting trajectory will 8. Toward visual constraints relaxation: planning under intermittent measurements have a total time N ˚τ . Finally, collisions are avoided by considering the robot's shape as representative of the position uncertainty ellipse (or ellipsoid in 3D) whose estimation is detailed in the next section. Motion primitives that violate the collision and inputs constraints are not added to the graph.

The advantage of graph-search planners in contrast to optimization-based methods (and especially gradient-based) is that complex constraints are not directly part of the optimization problem but are checked at each vertex expansion. Moreover, optimization-based methods may not be adapted to problems involving discontinuous constraints gradients as for (8.9d) that is the solution of a stochastic process with intermittent Kalman updates and possibly large periods without any sensing information. Evaluating such a gradient for gradient-descent solvers would be challenging and computationally intense since it is also completely re-evaluated at each iteration. In the next section we show how state uncertainty is included in visual perception and in collision avoidance to guarantee perception of visual measurements and safe navigation to a given level of confidence.

State estimation uncertainty

Let σ be the major axis of the uncertainty ellipse P η at a given state. Then, for a 99% confidence level one has σ 99% " ? 9.21 ? λ where λ is the largest eigenvalue of P η . This confidence ellipse defines the region that contains 99% of all samples that can be drawn from the Gaussian distribution. We take a circle (a sphere in 3D) with radius σ 99% as representative of the robot occupancy. It will vary with the pose uncertainty and will be incorporated in the planner for ensuring robust collision-free paths. Now, let us include the position uncertainty in the visual measurements. A visual landmark at known position η L " px L , y L q is visible when its bearing angle φ is smaller than the field of view angle 2α and lies in a given range r c from the camera (see Fig. where P η and P 9 η denote the position covariance matrix and the linear velocity covariance matrix respectively. They are evaluated with the Kalman filter along each 8.5. Building the graph valid discretized motion primitive. Applying the same process for the range condition (with lower and upper bounds rc and rc ), one can ensure a (theoretical) 99% confidence on the perception if the following upper bound conditions are satisfied at a given state |φ| `?9.21∆φ ă α rc ă r c `?9.21∆r c ă rc (8.12a) (8.12b) The update step of the Kalman filter is therefore applied with the simulated measurements whenever conditions (8.12) are met along the propagated motion primitives.

Building the graph

In this section we show how to exploit some vertices to efficiently explore the free space with the design of a heuristic function in order to build the graph. Traditionally, distance-based heuristics are used but they are not very relevant for dynamic or nonholonomic systems that cannot change their velocity, acceleration or orientation instantaneously. That is why a heuristic function more appropriate for second-or higher-order systems has been proposed in [START_REF] Liu | Search-based motion planning for quadrotors using linear quadratic minimum time control[END_REF], by taking dynamics and smoothness into account. As well know, A* algorithms rely on two functions: the heuristic function hps, s goal q that encodes an (optimistic) approximation of the cost-to-go from vertex s to the goal s goal and the function gpsq which represents the cost of vertex s. Without a heuristic, A* is equivalent to a Dijkstra search, but encoding some theoretic information into the heuristic function can greatly reduce the number of expansions in favouring exploration toward promising directions/areas. We use the heuristic function proposed in [START_REF] Liu | Search-based motion planning for quadrotors using linear quadratic minimum time control[END_REF] that originates from the resolution of Pontryagin's minimum principle and invite the reader to refer to the latter paper for more information. This function now encodes the "effort" required to connect two states given the considered control input (velocity, acceleration or jerk) and is used to select vertices leading to the exploration toward regions with minimal energy in order to encourage smooth trajectories. The cost of an edge itself is gpsq " τ because we want to minimize the time. We propose a bi-directional A* algorithm that builds two graphs G 1 and G 2 . G 1 starts at the initial state s 0 and G 2 starts at the final state s T . Both graphs will be propagated and connected to return full trajectories from s 0 to s T . The planner is detailed in the following section.

Uncertainty-aware bi-directional A* algorithm

Designing an efficient space exploration is tedious when complex tasks are involved and should not rely on a too strong a priori. Namely, in our case, the search should 8. Toward visual constraints relaxation: planning under intermittent measurements not be biased towards the goal since it may not allow sufficient collection of visual information from the landmarks to satisfy (8.9d). When multiple goals are present one may bias the search towards these goals, here, the landmarks (see e.g., the perception heuristic of [START_REF] Ichter | Robust motion planning via perception-aware multiobjective search on gpus[END_REF]). However, ensuring convergence to the final goal is not straightforward, especially for dynamic systems with perception goals that are function of the attitude (which is not the case in [START_REF] Ichter | Robust motion planning via perception-aware multiobjective search on gpus[END_REF]). In the end, we choose not to rely on any exploration a priori to be able to deal with any environment and landmark configurations (provided a solution exists). We rely instead on randombased exploration by smoothly propagating vertices toward states sampled randomly in the free space. Algorithm 1 runs for a given number of iterations I and is detailed below. Note that the uncertainty is only propagated on graph G 1 with the Kalman filter since graph G 2 is grown backwards (i.e., from the final goal s goal towards the initial state s init ). With reference to Fig. 8.5, the algorithm procedures are detailed below:

Sample: returns an independent and uniformly distributed random sample vertex s rand in the free space.

NearVertices: given a sample vertex s rand , a graph G " pV, Eq and a ball region B r of a given radius ρ, the set of near vertices is defined as N earps, G, ρq " ts P V : dps, s rand q ď ρu where d is the Euclidean distance and ρ " γplogpKq{Kq 1{q is the radius for expansion with K is the number of vertices and q is the space dimension. The ball radius helps capturing vertices when the graph is hollow and shrinks with the number of vertices to reduce the computation time. We use a constant radius γ c for finding connections candidates (see procedure ConnectG).

GetSortedList: given a list of vertices V and a goal s goal , this function returns a list L s of the sorted vertices s P V in increasing heuristic cost hps, s goal q.

ChooseBestParent: the vertex with lowest h cost from a list of vertices is chosen for expansion. We seek to find the parent vertex that will expand vertices towards the given goal with the lowest energy (highest smoothness).

BestVertices: when no near vertices are found in B r , this function finds the vertex s in graph G 1 with lowest cost hps, s T q and analogously for G 2 with hps, s 0 q.

ExtendVertex : propagates the valid motion primitives from a given parent vertex. This function includes the belief state propagation with the Kalman filter and collision and feasibility tests.

InsertVertices: valid vertices/edges are added to the graph and marked as children from their parent vertex.

InsertVertex : this function inserts a single vertex/edge pair.

ConnectG: this procedure is triggered whenever vertices from both graphs are found in the procedure NearVertices within a second ball region of constant radius 8.5. Building the graph Algorithm 1 Bi-A* 1: gps 1 best q Ð 8, gps 2 best q Ð 8 2: for i Ð 1 to I do 3:

s rand Ð Samplepq 4: pX 1 near , X 2 near q Ð N earV erticesps rand , G 1 , G 2 , ρq 5: pX 1 c , X 2 c q Ð N earV erticesps rand , G 1 , G 2 , γ c q 6: if X 1 c ‰ and X 2 c ‰ then 7: ps 1 new , s 2 new q Ð ConnectGpX 1 c , X 2 c q 8: pP η N , collq Ð BackP ropps 2 new , P s 1 new , G 2 q 9:
if maxteigpP η N qu ď λ and !coll 10:

if gps 1 new q `gps 2 new q ă gps 1 best q `gps 2 best q 11:

s 1 best " s 1 new , s 2 best " s 2 new 12:
end if

13:
if X 1 near ‰ then 14:

L s Ð GetSortedListpX 1 near q 15: s 1 Ð ChooseBestP arentpL s , s T q 16: L c 1 Ð ExtendV ertexps 1 q 17: G 1 Ð InsertV erticespL c 1 q 18:
end if

19: if X 2 near ‰ then 20: L s Ð GetSortedListpX 2 near q 21: s 2 Ð ChooseBestP arentpL s , s 0 q 22: L c 2 Ð ExtendV ertexps 2 q 23: G 2 Ð InsertV erticespL c 2 q 24:
end if

25:

if X 1 near " and X 2 near " then 26:

ps 1 , s 2 q Ð BestV erticespG 1 , G 2 q 27:
L c 1 Ð ExtendV ertexps 1 q 28:

G 1 Ð InsertV erticespL c 1 q 29: L c 2 Ð ExtendV ertexps 2 q 30: G 2 Ð InsertV erticespL c 2 q 31:
end if 32: end forreturn s 1 best , s 2 best 8. Toward visual constraints relaxation: planning under intermittent measurements γ c centered on s rand . Indeed, we seek pairs of vertices in a vicinity region to perform connection tests (see Algorithm 2) using the function solveQP presented in Sect. 8.6. Note that γ c can for instance be chosen as the "spatial resolution" of motion primitives or larger to find more connection candidates.

BackProp: given a vertex s 0 with a covariance matrix P 0 from graph G 1 , once a connection is found we back-propagate the state uncertainty through G 2 by considering the sampled states between s 0 and the goal s goal (see Fig. 8.5).

Algorithm 1 aims at finding the most direct trajectory towards the goal, especially in case of low process noise and tries to mimic a couple of nice properties of classic graph-search planners, namely: i) expansion towards unexplored regions; ii) probabilistic completeness due to a uniform random walk ; iii) asymptotic optimality. ). This helps finding a direct path that may be the optimal solution in case of low process noise. G 2 extends fewer vertices in this environment because of the obstacles that are slightly cutting down its expansion. One can see that the free space is explored in a reasonable uniform way.

Algorithm 2 performs connection trials on the vertices in X 1 c , X 2 c if their heuristic cost is lower than a given value h. This value can be chosen off-line to skip if hps 1 , s 2 q ă h then 5:

success Ð solveQP ps 1 , s 2 q 6:

if success " true then end for 11: end for 12: return 0 Figure 8.5 -Inside a ball of radius ρ centered at s rand (black dot), picture 1 shows how the vertex with lowest h cost is chosen for expansion (in cyan). The black arrows represent the vertex velocity vector. If vertices from graph G 1 (in green) and from graph G 2 (in yellow) are found inside a ball region of fixed radius γ c , connections trials are performed except for connections with a high h cost (orange lines). Note that we consider the opposite velocity (and acceleration) vectors for vertices coming from graph G 2 . If a candidate connection if found (blue line) the uncertainty is propagated along G 2 starting from s 0 (picture 3). If no collisions are found between the obstacles (blue box) and the uncertain robot's occupancy (turquoise circles) and if the final constraint (8.9d) is satisfied on P η N , a solution trajectory is reconstructed from the initial vertex s 0 (green dot) to the goal vertex s T (yellow dot) and its total cost is evaluated.

8. Toward visual constraints relaxation: planning under intermittent measurements connections that may require "too much" energy (see Fig. 8.6). Usually, graph-Figure 8.6 -Computation of the heuristic cost hps 1 , s 2 q between multiple vertices s 1 and a goal vertex s 2 at the origin. The arrows represent the vertices linear velocity. As we can see the vertex with the lowest cost is the one whose velocity vector is the most "aligned" with the goal velocity vector. Vertices that are closer to the goal have a higher cost.

search planners for dynamic systems involve two steps. First, an optimal path is found ignoring the system dynamics. Then a refining step is performed by optimizing over a selection of state keyframes along the path. The resulting trajectory is smoothed and more adapted to dynamic systems (see e.g., [START_REF] Liu | Search-based motion planning for quadrotors using linear quadratic minimum time control[END_REF], [START_REF] Richter | Polynomial trajectory planning for aggressive quadrotor flight in dense indoor environments[END_REF]). However, the shape of this trajectory may strongly differ from the original path (e.g., in [START_REF] Richter | Polynomial trajectory planning for aggressive quadrotor flight in dense indoor environments[END_REF]). In our context, visual perception cannot be guaranteed with such a technique and it does not take into account the uncertainty in collision avoidance. A key role of the bi-directional planner used in this work is that if a connection is made, the initial and final states are exactly connected, which is generally not the case in the graph-search planners literature. For instance, [START_REF] Liu | Search-based motion planning for quadrotors using linear quadratic minimum time control[END_REF] stops the search when a vertex becomes close enough to the goal state s T , a condition that may not be met if not properly tuned. In this work we aim instead at finding the optimal trajectory that will be directly tracked by the real system without additional refining steps.

Next section details how the connection between the two graphs is performed in an optimal and efficient way.

Connecting the graphs

Connecting the two graphs is a critical step. One has to ensure state continuity between two candidate vertices. This problem is known as the Boundary Value Problem (BVP). Moreover, one wants to ensure feasibility as well and connections have to be evaluated quickly since the process may be called many times. We propose an optimal formulation to the BVP that can be solved as a single convex quadratic program. We exploit the convex-hull property of B-splines in order to 8.6. Connecting the graphs impose constraints directly on the curve control points to alleviate the solver.

Solving the constrained BVP

The original problem we want to solve is the following We penalize the input norm to obtain a smooth connection trajectory. Now, we parameterize the flat state s as B-splines to turn the infinite dimensional problem to a finite one with a limited number of coefficients that can be solved numerically.

A trajectory s is parameterized as

sptq " i"n ÿ i"0 B i,p pτ qP , @τ P r0, T s (8.14)
where B i is a polynomial basis of degree p (of order k " p `1) and P P R n`1 represents the set of coefficients. Now, Problem 13 involves nonlinear constraints (8.13d) and (8.13e) and would require a nonlinear solver. We choose to simplify this problem by replacing these constraints with bounds on the linear velocity 9 η and acceleration : η with r 9 ηpτ q, : ηpτ qs T P X f ree @τ P r0, T s (8.15) with X f ree :" r´v, vs 2 ˆr´ā, ās 2 the hypercube space of the admissible velocities and accelerations.

A linear quadratic program based on B-splines

The reason we use B-splines is for their convex hull property that will allow us to write linear inequality constraints directly function of the B-spline control points.

A similar approach has been used in [START_REF] Van Loock | B-spline parameterized optimal motion trajectories for robotic systems with guaranteed constraint satisfaction[END_REF] for manipulators.

These techniques are attractive and mostly adopted because of the convex hull property of the B-spline parametrization, which states that a spline is always contained in the convex hull of its B-spline coefficients [START_REF] Boor | A practical guide to splines[END_REF]. This way, spline constraints can be relaxed to constraints on the B-spline coefficients and we are ensured 8. Toward visual constraints relaxation: planning under intermittent measurements that the B-spline curve will satisfy the same constraints. Replacing semi-infinite sets of constraints by finite, yet conservative sets is called a B-spline relaxation. B-spline relaxations can only be applied on splines, meaning that all constraints must be written as derivatives, antiderivatives, or polynomials of splines. Therefore, a nonlinear change of variables needs to be adopted to transform all constraints into bounds on spline functions [START_REF] Mercy | Spline-based motion planning for autonomous guided vehicles in a dynamic environment[END_REF] The major advantage is that B-spline relaxations avoid time gridding of the constraints, while they guarantee constraint satisfaction at all times. The disadvantage is that B-spline relaxations also introduce some conservatism. This conservatism can be reduced by choosing a higher dimensional basis, at the cost of introducing extra constraints [START_REF] Van Loock | B-spline parameterized optimal motion trajectories for robotic systems with guaranteed constraint satisfaction[END_REF]. This conservatism stems from the distance between the control polygon and the spline itself. Knot insertion is generally the preferred technique, since, in this way, the conservatism reduces quadratically with the number of constraints, while this decrease is only linear when using degree elevation [START_REF] Prautzsch | Bézier and B-spline techniques[END_REF]. In addition, knot insertion allows refining the control polygon only locally, whereas degree elevation always affects the entire control polygon. Since using a higher dimensional basis translates into more constraints, it is necessary to make a trade-off between conservatism and computational complexity (number of constraints).

For nonlinear systems, one can resort to convex approximations of the feasible set [START_REF] Louembet | Motion planning for flat systems using positive b-splines: An lmi approach[END_REF][START_REF] Faiz | Differentially flat systems with inequality constraints: An approach to real-time feasible trajectory generation[END_REF]. Inevitably, this method introduces conservatism in the problem. Moreover, some feasible sets do not admit such a polytopic approximation, e.g. obstacle avoidance constraints.

Constraints (8.15) can be mapped in the space of the control points. Let us differentiate the B-spline of degree p defined on the clamped knot vector of size n `k `1 such that u i ď u i`1 , i " 0, . . . , n ´k U " p0, . . . , 0 loomoon p`1

, u p`1 , . . . , u k´p´1 , 1, . . . , 1 loomoon p`1 q (8.16)

The first derivative can be expressed as a function of the control points P with

s 1 puq " p i"n´1 ÿ i"0 B i`1,p´1 ptq P i`1 ´P i T pu i`p`1 ´ui`1 q (8.17)
Let us define the vector of new coefficients

Q i " p P i`1 ´P i T pu i`p`1 ´ui`1 q
, @i P v0, n ´1w (8.18)

The new knot vector is obtained by dropping the first and last knots from U , i.e., U where R i are the control points of the second derivative. One has

R i " pp ´1q Q i`1 ´Qi T pu i`p`1 ´ui`2 q
, @i P v0, n ´2w (8.21)

Now we can express Q and R as functions of P with

Q " A Q P , R " A R Q " A R A Q P (8.22)
where matrix A Q P R nˆpn`1q and A R P R pn´1qˆn . Now, one can easily set semiinfinite bounds on the derivatives coefficients Q and R that are linear in P . All the constraints can be rewritten as functions of the control points P . This strategy avoids discretizing the flat outputs and the constraints that would may lead to a great number of constraints. We want to solve the following problem on each axis Problem 14 Find P such that:

min P P T pB T r B r qP s.t. @ P , B i p p0q
D " η i 0 , @i P v0, r ´1w @ P , B i p pT q D " η i T , @i P v0, r ´1w ´Q ď Q i ď Q, @i P v0, n ´1w ´R ď R i ď R @i P v0, n ´2w where B d p psq P R n is d-th order derivative B-spline basis of degree p evaluated at s P rt, t `T s. In this case B r P R Hˆn represents the r-th order derivative basis discretized in H samples between t and t `T . Finally, as we minimize the acceleration we choose r " 2.

The problem can be written in the compact form of a quadratic program with Problem 15 Find P such that: where H P R nˆn denotes a positive (semi-)definite Hessian matrix, f P R n is the gradient vector (null here), A P R mˆn is the constraint matrix and A eq P R rˆn is the equality constraint matrix. The upper and lower bounds on the constraints are defined by the vectors b P R m and b eq P R r and bounds on the decision variables can be set with l b P R n and u b P R n but will be null in our case. Here we will not impose bounds on P , i.e, (8.24d) is not considered. Problem 14 will be solved using qpOASES [START_REF] Ferreau | qpOASES: A parametric active-set algorithm for quadratic programming[END_REF] that implements an online active set strategy. Note that the connection time T is fixed and we found that choosing T " τ generates a reasonable amount of successful connections.

Extension to the quadrotor UAV

As seen in Sect. 2.5, thanks to differential flatness the quadrotor's dynamics can be decoupled into four linear subsystems of the form x p4q " ν 1 , y p4q " ν 2 , z p4q " ν 3 , ψ p2q " ν 4 (8.25) where ν " pν 1 , ν 2 , ν 3 , ν 4 q defines the new control inputs in the flat space. For the sake of simplicity, we do not plan over the yaw angle ψ that is assumed to be constant at zero. Moreover, we consider the quadrotor as three triple-integrators controlled in jerk along axes X,Y and Z, and we consider linear constraints on the first and second derivatives. With the above simplifications we seek to alleviate the planner whose complexity grows exponentially with the state dimension. The derivative-free Kalman filter in the flat space drastically reduces the computation load for the state estimation compared to an EKF with the real nonlinear quadrotor dynamics. We aim at solving Problem 12 with the same planning framework for the new state vector s " px, y, z, 9

x, 9 y, 9 z, :

x, : y, : zq and input u " px p3q , y p3q , z p3q q with X f ree :" r´v, vs 3 ˆr´ā, ās 3 ˆr´j, js 3 where j denotes the jerk bound. We consider that the quadrotor is equipped with a fixed downward-looking camera capable of providing reliable position measurements when fixed landmarks on the ground enter the limited field of view. Again, the uncertainty on the state s is incorporated in the planning. With reference to Fig. 8.7, to check that a landmark at position r L " px L , y L , z L q is visible along an edge, we impose conditions on angles β 1 and β 2 on both planes X-Z and Y-Z given by β i " arccos ˆpr L e i q.l i }pr L e i q.l i } ˙, i " 1, 2 where P : η is the acceleration covariance matrix. We consider a landmark is guaranteed to be visible when

β i `?9.21∆β ă 2α, i " 1, 2 (8.29)
Problem 14 is now extended with constraints on the jerk that is minimized (i.e., r " 3). The latter constraints are written as ´S ď S i ď S @i P v0, n ´3w (8.30)

where the vector S P R n´3 is given by

S i " pp ´2q R i`1 ´Ri T pu i`p`1 ´ui`3 q , @i P v0, n ´3w (8.31) with S " A S R " A S A R A Q P (8.32)
where matrix A S P R pn´2qˆpn´1q .

Simulation and experimental results

In this section we show some results for different scenarios for both the unicycle and the quadrotor. Problem 14 is solved within 0.5 ms for the unicycle after 30 solver iterations on average and within 5 ms for the quadrotor with around 90 iterations.

Toward visual constraints relaxation: planning under intermittent measurements

We were able to find solutions under 150 ms for the unicycle and after a 5 to 10 seconds in general for the quadrotor. In Fig. 8.8 two solution trajectories are shown for the unicycle. We can see that the robot finds a way to collect visual cues from the landmarks to reach the goal with a bounded uncertainty with different landmark and obstacles configurations for given initial and final states. With the given degree we can see that the curves (e.g., the jerks) are not penalized by conservatism. Finally, Fig. 8.13 shows the tracking performance for a simulated quadrotor in V-Rep1 using controller [START_REF] Lee | Geometric tracking control of a quadrotor UAV on SE(3)[END_REF]. We can see that the third-integrator model approximation is reasonable. Figure 8.9 -An optimal trajectory for the 3D quadrotor with τ " 0.35s and u max " 10m.s ´3 in a 12x8x5m operating region. The initial and final states are chosen such that no landmark is visible (red blobs on the ground) so the quadrotor starts with some uncertainty and is able to reach the goal with a bounded uncertainty by observing the landmarks during its motion. Note that we do not consider occlusions due to the obstacles and the motion primitives are not shown. We show in Fig. 8.11 an example of a full optimal trajectory which is "patched" by the optimal three-dimensional B-spline curve connecting the two trajectories rooted in the initial and the final state. The full position, velocity and acceleration profiles are shown in Fig. 8.12. For the experiment illustrated in this section we used a MK-Quadro equipped with a front-looking camera with a field of view of 45 ˝. Note that we validated the AprilTags tracking off-board on the image sequences recorded during the flights using ViSP [START_REF] Marchand | Visp for visual servoing: a generic software platform with a wide class of robot control skills[END_REF]. The setup includes an on-board ODROID-XU4 Linux computer running ROS and the TeleKyb framework for controlling the quadrotor. An optimal trajectory computed off-line using a jerk input u max " 4m.s ´3 is tracked by the system (see Fig. The related video2 shows 6 different solution trajectories (see Fig. 8.15) tracked by the quadrotor. Apart from environment differences, several solutions may be found with very different characteristics and shape. This is explained by the random-based search of the algorithm and by the non-convex nature of the optimal program driving the solutions to local minima. The fifth trajectory Fig. 8.15c shows an interesting behaviour, the quadrotor flies above the two obstacles by increasing its height. This way, the field of view is extended and sufficient visual measurements can be collected. Thus, the presented algorithm is able to generate this behaviour that we depicted in the previous work [START_REF] Penin | Vision-based minimum-time trajectory generation for a quadrotor uav[END_REF]. Finally, we chose very low Figure 8.14 -Snapshots of the quadrotor during the experiment. The quadrotor is tracked with a Vicon system and follows an optimal trajectory (in green) along which landmarks (the AprilTags in orange) are visible on some portions. The lower right figure shows an Apriltag tracked using ViSP when the quadrotor is at the configuration circled in red. The evolution of the uncertainty is shown below after running the Kalman filter on the recorded data and using the AprilTags detection. The landmark at the goal is not taken into account in the planning and is used to check that the quadrotor arrives at the goal state within the expected confidence region.

values for the process and the measurement noise. The computed trajectory (the sixth one) actually resembles to a minimum-time trajectory in the sense that the quadrotor flies directly toward the goal state at a constant height without "passing" by the visual landmarks. One can conclude that the proposed algorithm is able to generate consistent solutions regarding the noise magnitude.

Summary and future directions

In this chapter we proposed to incorporate perception constraints in a graph-search planner for planning minimum-time and feasible trajectories for flat dynamic systems. We believe that the optimization frameworks presented in the previous works which rely on gradient-descent methods may not be adapted to the considered goals in this section. They provided limited state space exploration and were not able to generalize to the large environments considered in this work. The optimization framework allows exact connection between a given initial and final states while en- suring collision avoidance and bounded final uncertainty at the goal by accounting for the state uncertainty at the planning stage. We considered visual measurements that are a function of the attitude and proposed an efficient optimal graph rewiring by exploiting the convex-hull property of B-splines. Of course, other exteroceptive sensors could be considered such as laser range finders or sonar arrays along with more accurate measurement noise models. One could as well incorporate uncertainties related to wheels skid and odometry bias in the unicycle case. The planner success rate depends on the motion primitives parameters, the ball regions radius and the maximal number of iterations. It could be possible to re-plan optimal trajectories during motion and even consider dynamic obstacles for the unicycle case. We assumed the position of the landmarks is known but it would be possible to incorporate their position uncertainty in the planner. Finally, we believe the triple-integrator approximation of the quadrotor dynamics could become closer to a fourth-integrator model by having an additional noise in the current model. This would give a more adequate representation of the real quadrotor dynamics which would result in a more feasible trajectory.

Part III

Conclusion and future directions

Chapter 9

Conclusion and future directions

In this last chapter, we wish to review the main theoretical and experimental results achieved in the thesis and point out some issues that are still left open. Regarding the latter, we also intend to indicate possible directions to follow for further investigation and research.

Summary and contributions

The goal of this thesis was mainly to explore the generation of reactive trajectories for a quadrotor subject to visibility constraints and inputs constraints. The considered system consists in a regular quadrotor equipped with a fixed camera (either down-or front-looking). Since we focused on exploiting the potential of a quadrotor in terms of agility to perform aggressive motions, the effect of underactuation could not be ignored, especially because visual perception is very sensitive to the inherent rotation motions. Moreover, we assumed visual feedback played crucial roles either for state estimation or for tracking a moving target. Thus, a collection of visual constraints were formulated in this thesis such as visibility constraints (Sect. 6.7) followed by occlusion avoidance constraints (Sect. 7.6) encapsulated in an optimization framework.

In Sect. 6.7 we proposed re-planning strategies inspired from Model Predictive Control to generate minimum-time and feasible trajectories while keeping a set of visual features in the field of view of the camera. We proposed a hot-start algorithm for building initial guess trajectories by exploiting properties of B-splines curves. We were able to efficiently re-plan optimal trajectories at a rate of 30Hz. This preliminary work led to the resolution of more complex problems in Sect. 7.6 for tackling collisions and occlusions avoidance in real-time. We proposed a multiobjective nonlinear program, first for tracking a free moving target in 3D space with a front-looking camera then for avoiding occlusions generated by spherical obsta-

Conclusion and future directions

cles in the environment considering a down-looking camera. We dealt with sudden occlusions and critical camera/target configurations that would lead to a failure of the solver in principle by adding a slack variable to the nonlinear program that acts as a damper to relax the occlusions avoidance constraints. Such a parameter helped improving the continuity of the solution and therefore the stability of motion. We showed that tuning the optimization programs do not require fine analysis (although it may be interesting to show a sensitivity analysis) and that we are able to manage conflicting goals to exhibit the desired behaviours. Finally, we succeeded in computing optimal trajectories in real-time by improving the evaluation of the gradients necessary for the SQP solver with the use of complex-step differentiation. With this method we are able to obtain a lightweight evaluation of the complex gradients with a near analytic precision. We showed that the hot-start algorithm definitely contributed in the success of the re-planning framework.

In Chapt. 8 we addressed the problem of planning under intermittent visual measurements provided by visual landmarks scattered in the environment. The objective was to relax the vision-based constraints developed in the previous contributions which may highly restrict the robot motions to limited operating spaces. We proposed a graph-search algorithm that takes state and measurement uncertainties into account to find robust and collision-free trajectories that satisfy a confidence level at the goal state. In contrast to several works we searched for robust minimum-time paths ensuring some confidence level instead of paths with minimal uncertainty. This strategy may make more sense depending on the configuration of the obstacles and the landmarks in the environment. Indeed, if the system and the sensors are well known, the uncertainty is low, thus one may navigate almost directly to the goal and arrive with a limited state uncertainty. On the other hand, if the system is subject to large uncertainties in a complex environment, one also seeks the most direct path connecting the goal state by collecting a sufficient (and ideally a minimal) amount of visual measurements. The proposed algorithm grows two graphs based on a A* variant which are connected smoothly to build a full feasible trajectory for the considered flat dynamic systems. The motivation was to improve the rate of convergence and to plan a trajectory that connects the initial and final states exactly, which is not often the case in the literature. To the best of our knowledge, this is the first time minimum-time trajectories are generated in a graph-search planner while accounting for uncertainty in the visual perception which is affected by the robot attitude. We demonstrated the utility of the algorithm by considering a unicycle and a 3D quadrotor.

In this thesis we extensively used properties of the differential flatness both for trajectory planning and state estimation. We used a derivative-free Kalman filter for the latter in Chapt. 8. This technique contributed in improving the fastness of the algorithm although is it not suited for real-time planning (at least for the quadrotor case). Differential flatness played a significant role in simplifying trajectory generation to meet real-time planning and for the propagation of feasible motion primitives in Chapt. 8.

Properties of the B-spline curves were also largely exploited in the presented contributions. In Sect. 6.7 we took advantage of the properties of compact form and smoothness along with powerful manipulation algorithms to instantly build initial guess trajectories. Then, such a parametrization allowed the use of a reasonable number of control points to satisfy the considered constraints. Finally, in Chapt. 8 we exploited the convex hull property to develop a quadratic program for smoothly connecting the two graphs with B-spline curves while satisfying linear inequality constraints.

Open issues and future perspectives

However, even if our approaches presented several promising results it also highlighted some limitations both theoretical and practical.

One limitation of our work is that we relied on an external motion-capture Vicon system providing accurate state estimates at a high frequency. In our works, we assumed that navigation relies on a state estimation algorithm which is updated with the visual measurements extracted from computer vision. The design of the image processing algorithms was beyond the scope of this thesis. Although, it would be relatively easy to implement them using ViSP.

In order to fully prove the presented works, we believe experimental results should be conducted by relying on onboard sensors (a single camera and an inertial measurement unit) and computers for detecting, localizing, and tracking moving objects. As pointed out in Chapt. 8, motion blur may impair computer vision algorithms. One possible solution could be to reduce the system aggressiveness or to directly incorporate motion blur as an additional noise affecting the perception. Another approach could be the minimization of motion blur at the planning state (e.g., by minimizing the features velocity in the image plane or by penalizing the angular rates of the quadrotor). As explained in the experimental sections, we equipped our robot with an ODROID-XU4. However, we think that the new, and more powerful, NVIDIA JETSON TX2 module could be a more reasonable solution for a complete onboard implementation of the vision, planning and state estimation modules.

Although [START_REF] Thomas | Toward autonomous avian-inspired grasping for micro aerial vehicles[END_REF] warned that finding images features as flat outputs might be impossible if one considers the full dynamics of the 3D quadrotor we tried to de-

Conclusion and future directions

rive a differential flat mapping between the state space and the image space but the equations become very complex (so would be the constraints on the inputs) if one considers the rotation matrix. Furthermore, it would require knowledge of high-order derivatives that are not directly measurable. Maybe partial differential flatness (as in e.g., [START_REF] Ramasamy | Dynamically feasible motion planning through partial differential flatness[END_REF]) might bring a weaker but elegant mapping of the dynamics in the image plane. Now, further improvements and extensions could be considered in Sect. 7.6. We only dealt with spherical obstacles, but more complex shapes could be considered. One possibility could be to find the (minimal) enclosing sphere (i.e., the smallest sphere containing the object). For elongated objects such as bars, one could cover the object main axis with a finite number of spheres as it is done sometimes for collision avoidance with robot manipulators (a technique known as "sphere swept models" see e.g., [START_REF] Sugiura | Real-time collision avoidance with whole body motion control for humanoid robots[END_REF]). However, dealing with dynamic obstacles seems quite challenging. To do so, vision would play a major role in estimating the object's position. Thus, as always, there is a trade-off between conservatism/accuracy and ease of computation.

Finally, more accurate noise models could replace the current ones in Chapt. 8 without adding an excessive burden to the computing. Thus, conservatism could be reduced and more precise trajectories could be found.

Final thoughts

Aerial navigation has received considerable attention over the last 15 years. Very powerful and efficient forms of optimal planning methods have emerged to tackle more complex scenarios and environments involving obstacles or additional moving parts that traditional controllers were not able to address. In 2011, [START_REF] Mellinger | Minimum snap trajectory generation and control for quadrotors[END_REF] demonstrated the ability of optimization techniques to design feasible and high-speed flight plans for quadrotors for passing through obstacles with substantial pitch and roll angles. The presented strategy inspired many works to study aggressive motions, feasibility and re-planning strategies. Six years later, [START_REF] Falanga | Aggressive quadrotor flight through narrow gaps with onboard sensing and computing using active vision[END_REF] was able to reproduce such scenarios in a fully autonomous fashion by re-planning optimal trajectories using vision as principal feedback. This work merges planning and vision and proved that active vision is definitely a key to the future of aerial navigation especially associated with agile manoeuvres. Moreover, since deep-learning has handily surpassed every existing computer vision techniques for tracking, detecting and localizing, it has been very seriously considered in robot control and planning. Now, deep-learning constitutes the state-of-the-art approach across computer vision, audio, and natural language processing and is largely adopted and studied by the research community. Therefore deep-learning is gaining in maturity exponentially and is even about to 9.3. Final thoughts replace the action of many existing complex controllers. The reason is not especially because of the recent theoretical improvements made in the field by mainly because of the impressive fastness of today's computers. Indeed, Moore's law stating that the number of transistors in an integrated circuit doubles about every two years is coming to an end (mainly because of heat exchange and electronic disturbances due to proximity of circuits). However, the power of computers keeps growing due to parallelization of processors and the use of graphics processing units (GPU). Now, robots are even able to efficiently develop robust and complex flight plans by learning from their "mistakes" (even from crashes [START_REF] Gandhi | Learning to fly by crashing[END_REF]). Besides, learning can be accelerated by using simulation data. If properly done, it can save a considerable amount of time and of course avoid tedious experimental setups and replacements of spare parts. However, such techniques are expensive since they require a considerable computational power which is not always affordable. Although the strength of deeplearning is its ability to generalize and adapt to new situations, a few aspects remain unclear. We are not yet able to prove its robustness, stability and convergence, central criteria that might refrain its adoption in industry especially at the control level. Moreover, huge data sets are needed and may be difficult to obtain depending on the targeted task. Besides, it is known that slightly altering the input data (e.g., changing a few pixels in the input image) of a well -trained neural network can lead to absurd outputs. Interpretability in deep learning referring to understanding why a system makes a certain decision is a hot topic and an open problem.

For these reasons, more interpretable techniques such as model-based and analytic control techniques which rely on a long history of research developments will still play a major role in the future. Note that the hidden process of patterns generation in deep-learning has already been modelled to replicate some of its properties to some extent. For instance, a visual servoing task is proposed in [START_REF] Crombez | Photometric gaussian mixtures based visual servoing[END_REF] that eliminates the need for detecting and tracking image features by using photometric Gaussian mixtures. This strategy subsamples the images and extracts photometric data at increasing levels of precision until convergence. This technique mimics deep-learning and is able to drastically improve the convergence domain of a classical visual servoing task and to reduce the computation expense. Finally, new computers and sensors are being developed and will give the possibility to address even more complex scenarios. Even-based cameras seem to constitute promising elements of the future sensory channel of aerial robots and any system involving fast camera motions.

To conclude, this thesis tackled and revealed challenging problems that are currently addressed actively by the research community. A key aspect to the success of future autonomous and complex navigation seems to lie in coupling of trajectory planning and the considered tasks especially including vision-based perception objectives. Algorithms requiring extensive computation loads can now be easily deployed on on-board computers to generate safe and robust motions on-line for reactively responding to changes in the environments. ω BW " mx B `ny B `oz B (A. As already said, the strength of differential flatness is to transform the system such that the equations of motion for the flat output variables become trivial. Using the flat output and its derivatives, the system of (2.16) can be written in the Brunovsky linear canonical form:

d 4 x dt " v 1 d 4 y dt " v 2 d 4 z dt " v 3 d 2 ψ dt " v 4 (A.32a) (A.32b) (A.32c) (A.32d)
One can define a new system 9 zptq " Azptq `Bvptq with new control inputs v with v " pv 1 , v 2 , v 3 , v 4 q T . With ' convex hull : because the basis functions are positive and sum up to one, a spline is always contained in the convex hull of its control polygon which is the convex hull of the spline control points. This polygon corresponds to the piecewise linear interpolation of the spline coefficients.

A " » - - - - - A 1 0 4ˆ4 0 4ˆ4 0 4ˆ2 0 4ˆ4 A 1 0 4ˆ4 0 4ˆ2 0 4ˆ4 0 4ˆ4 A 1 0 4ˆ2 0 2ˆ4 0 2ˆ4 0 2ˆ4 A 2
' the B-spline is of class C 8 in the interior of every knot span and it is of class C p´m in a knot of multiplicity m;

' the number of knots K `1 is related to the number of control points n and to the order of the curve k by K " n `k

' the derivative of a B-spline is also a B-spline of lower degree. Indeed s piq ptq "

n´1 ÿ i"1 B i,p ptqP u 0 ď t ď u K (B.2)
and it is possible to efficiently compute the r-th order derivative of the basis functions in terms of the basis functions of degree p ´i defined on the same knot vector U where the coefficients a r,l are defined in a recursive way a 0,0 " 1 a r,0 " a r´1,0 u i`p´r`1 ´ui a r,i " a r´1,l ´ar´1,l´1 u i`p´r`l`1 ´ui`l , f or l " 1, ..., r ´1 a r,r " ´ar´1,l´1 u i`p`1 ´ui`1 Thanks to all these properties, B-splines have been widely used in different applications such as computer graphics, data interpolation and trajectory planning, e.g., [START_REF] Sheckells | Optimal Visual Servoing for Differentially Flat Underactuated Systems[END_REF], [START_REF] Suryawan | On splines and polynomial tools for constrained motion planning[END_REF]. For an exhaustive description of the B-splines and their properties see [START_REF] Biagiotti | Trajectory planning for automatic machines and robots[END_REF]. If the initial and final knots have multiplicity k then the B-spline curve is clamped -the first and last control points coincide with the endpoints of the curve, i.e. spu 0 q " P 0 and spu K q " P n .

B.2 Manipulation algorithms

B-spline curves benefit from very powerful algorithms such as evaluation, knot insertion, knot removal and subdivision. A brief description of these algorithms are given in this section.

B. Parametrization using B-splines curves without modifying the shape of the original curve. This algorithm is the core of the hot-start algorithm presented in Sect. 6.4.2.

First, the function findspan finds the knot span ru k , u k`1 q containing u (see [START_REF] Piegl | The NURBS book[END_REF] p.80 for details on the algorithm). From the convex hull property (see the previous section), spuq lies in the convex hull defined by the control points P k " pP k´p , P k´p`1 , . . . , P k q. Now, we show the procedure for subdividing a B-spline curve in two at the point u. Thus, the output of the algorithm consists in two B-spline curves, one has the left curve defined by the pair tP L , U L u and the right curve defined by the pair tP R , U R u (see Fig. B.2). The algorithm is referred as the De Boor algorithm and necessitates operations only on the subset P k of the control points P . The algorithm performs the insertion of the knot u p times. The shape of the curve is unchanged but the two curve halfs become independent. Indeed, curves of degree p corresponding to a knot vector with a knot u of multiplicity p have their local support contained in either r0, us or ru, 1s. In Fig. B.3 we show how the control points of the two curves are determined using the De Boor algorithm. In the end one has P L " pP 0 , P 1,1 , P 2,2 , . . . , P p,p q with knot vector U L " pr0, uq , u lo omo on p q and P R " pP p,p , P p´1,p , . . . , P 1,p , P p q with knot vector U R " p u lo omo on p , pu, 1sq. For our hot-start algorithm, we are interested in the right section since it corresponds to the future trajectory that will be adapted and used as initial guess for the next solver instance. Once the original curve (i.e., the previous solution) is split, we need to add the potential missing knots until the length of U L matches the length of U . Indeed,the length of U L varies with the position of u in the knot span. Such an operation is done with the knot insertion algorithm detailed in [START_REF] Piegl | The NURBS book[END_REF] p.161. To do so, control points are also added. Then, U L is rescaled between r0, 1s to finally match the original uniform knot vector U . In the end, we have a new vector of control points P L of same length as P defining the new B-spline curve with the pair ! P L , U ) of degree p. Hence, for an analytic function, differentiation is equivalent to evaluating terms of a Taylor series.

C.2.1 Finite difference method

One method that is very commonly used is finite differencing. Although it is not known for being particularly accurate or computationally efficient, the biggest advantage of this method lies in the fact that it is extremely easy to implement.

Taking a " x`∆x in equation (C.2) and reordering we can obtain an expression for the first order derivative similar to (C.1):

f 1 pxq " f px `∆xq ´f pxq ∆x `Op∆xq (C.3)
An expression like (C.3) is called a Finite Difference (FD) approximation, in this case the first-order forward approximation for the first derivative, where the differential step is taken in the positive direction. The symbol O expresses the error related to truncating the Taylor series in the second order derivative. Fourth-order accuracy can be achieved, of course at the price of increasing computational cost. Whatever the order of accuracy, all FD approximations involve a truncation error depending on the step size ∆x. FD approximations are still the most classic, simple and intuitive approaches to approximate derivatives of a function, and are widely used in numerical schemes. However they suffer from numerical issues related to the "step-size dilemma", that is, the desire to choose a small step size to minimize truncation error while avoiding the use of a step so small that errors due to subtractive cancellation become dominant [START_REF] Gill | Practical optimization[END_REF].

Working with arbitrarily small steps ∆x is not feasible on a computer. FD schemes, as the name suggests, involve some difference operator in the numerator, and this difference itself is an intrinsic problem. For a given step size ∆x, and particularly for small steps, the differences of the values of our function at successive evaluation points may become small, leading to a loss of significant digits as one approaches machine precision, and eventually a value zero for the numerator and the derivative when the computer fails to recognize the difference between the two numbers. This problem is known as subtractive cancellation or term cancellation. Since in numerical simulations we often have little hints on the actual shape of the functions involved, subtractive cancellation is not straightforward to control, which forces us into a conservative choice of step size at the expense of larger truncation errors. On the other hand, large values of ∆x may create instabilities and affect the data quality. In the next section we introduce a less intuitive technique when ∆x is a complex number that provides better numerical accuracy and stability than FD.

C.2.2 Complex-step differentiation

Most naturally, derivatives of real functions are evaluated using real numbers, but the less intuitive idea of using an imaginary number in real functions differentiation has been shown capable of overcoming the term cancellation inherent to the ordinary FD method, as well as reducing the associated approximation error. The use of complex variables in numerical differentiation was introduced by [START_REF] Lyness | Numerical algorithms based on the theory of complex variable[END_REF], describing a method for computing the derivatives of any analytic function. After falling into oblivion for 20 years, this theory reappeared in the scientific literature when Squire and Trapp [START_REF] Squire | Using complex variables to estimate derivatives of real functions[END_REF] formally presented the Complex Step Method (CS) to obtain a very simple expression for estimating the first and second derivatives of a real function using a purely imaginary number i (i 2 " ´1). This estimate is suitable for use in modern numerical computing and has been shown to be very accurate, extremely robust and surprisingly easy to implement, while retaining a reasonable computational cost. Further research on the subject has been carried out by [START_REF] Martins | An automated method for sensitivity analysis using complex variables[END_REF] for sensitivity analysis.

The CS method can be very easily derived from the Taylor series expansion of f px `i∆xq, i.e., f px `i∆xq " f pxq `i∆xf 1 pxq `pi∆xq 2 2! f 2 pxq `pi∆xq 3 3! f 3 pxq `. . . Taking the imaginary part on both sides and reordering we obtain the CS expression for the first derivative found by [START_REF] Squire | Using complex variables to estimate derivatives of real functions[END_REF] f 1 pxq " �pf px `i∆xqq ∆x `Op∆x 2 q (C.5) Note that, �pf pxqq " 0 because x is set to be a real number. Compared to (C.1) this solution is not a function of differences, which ultimately provides better accuracy than a standard finite difference. The second order term in the Taylor series expansion of f px `i∆xq appears with a factor of i 2 , meaning that it is a real quantity.

Compared with (C.3) the truncation error is now of order ∆x 2 , thus smaller. An expression for the second order derivative can be found by taking the real part of (C.4) and reordering, Generalizations to high order derivatives made by [START_REF] Lai | Extensions of the first and second complex-step derivative approximations[END_REF] and [START_REF] Bagley | On fourier differentiation-a numerical tool for implicit functions[END_REF] were done by converting the Taylor series into a Fourier series (Taylor expansion of f px `∆xe iθ q, i.e., f px `∆xe iθ q " f pxq `∆xe iθ f 1 pxq `∆x 2 2! e 2iθ f 2 pxq `∆x 3 3! e 3iθ f 3 pxq `. . . In the expression (C.9) the imaginary step does not vanish with even powers of the Taylor series which allows to compute high order derivatives by combining different ∆x steps values and using the real or imaginary part without the limitations of the ordinary CS method. The main limitation of this formulation is that the real and imaginary steps are set to be orthogonal (e iθ " cos θ `i sin θ) depending on a parameter θ. In other words we cannot choose the relation between real and imaginary step sizes which brings many advantages as discussed below.

f
For first derivatives the complex-step approach does not suffer subtraction cancellation errors as in standard numerical finite-difference approaches. Therefore, since an arbitrarily small step-size can be chosen, the complex-step method can achieve near analytical accuracy. However, implementation of the complex-step approach for second derivatives does suffer from round-off errors. Therefore, an arbitrarily small step-size cannot be chosen. Moreover, one of the limitations of the CS method is that only the first-order derivative is accessible using the imaginary part of the function, while second derivatives are proportional to i 2 and have to be evaluated by taking the real part of the function.

The advantages of the complex-step approximation approach over a standard finite difference include: 1) the Jacobian approximation is not subject to subtractive cancellations inherent in roundoff errors, 2) it can be used on discontinuous functions, and 3) it is easy to implement in a black-box manner, thereby making it applicable to general nonlinear functions.

C.2.3 Automatic differentiation

In terms of implementation, the continuous approach can only be derived by hand, while the discrete approach to differentiation can be implemented automatically if the program that solves the discretized governing equations is provided. This method is known as algorithmic differentiation, computational differentiation or automatic differentiation. It is a well-known method based on the systematic application of the chain rule of differentiation to computer programs [START_REF] Rall | An introduction to automatic differentiation[END_REF]. This approach is as accurate as other analytic methods, and it is considerably easier to implement.

C.2.4 Implementations

The implementation of any of the derivative calculation methods, for practical purposes, should be as automated as possible. Changing the source code manually is not only an extremely tedious task, but is also likely to result in the introduction of coding errors in the program. There are two main possibilities for implementing algorithmic differentiation: by source code transformation or by using derived data types and operator overloading. To implement algorithmic differentiation by source transformation, the whole code must be processed with a parser and all the derivative calculations are introduced as extra lines of code. The resulting extended code is greatly enlarged and it becomes practically unreadable. This fact constitutes an implementation disadvantage as it becomes impractical to debug this new extended code.

However, for CS several mathematical functions need to be rewritten in their complex form before implementing the gradient evaluation. In the next section we provide a few functions that we needed in our optimal control problems.

C.2.5 Table of complex functions

For CS we need to keep both the real and the complex part of every functions involved in the constraints evaluation. Therefore, some functions need to be used in their complex form, see table C.1.

Of course the computation is increased and from practical aspects, the code length is inevitably larger after implementing the aforementioned transformation but only the parts involved in the constraints evaluation are concerned, which is not the case of AD that requires a full overload of the code. In the next section, we compare the performance of FD versus CS using Matlab and show how using CS affects the solver performance. x 2 ´y2 `2ixy z 1 ¨z2 z 1 ¨z2 " px 1 x 2 `y1 y 2 q `ipx 1 y 2 ´y1 x 2 q

Table C.1 -Complex formulation of a few classic functions with z " x `iy

C.3 Comparison results

Considering the collision avoidance constraint of a single obstacle

.2 shows that the relative error between the analytic gradient of (C.10) and its approximate with FD and CS is of the order of Matlab floating-point relative accuracy (2.2204e-16). Whereas the error with FD is larger to a factor of 1e10. Moreover, the precision of CS seemed not to be affected by different step values, which is not the case for FD.

Table C.2 shows the average time taken to approximate the nonlinear constraints gradient by FD and CS. Interestingly, CS seems to be about twice as fast as FD. This result shows the advantage of having a single evaluation of the considered function with CS instead of two for FD. Now, from the above result, we can assume CS provides results very close to the analytic form. In Title : Contributions to optimal and reactive vision-based trajectory generation for a quadrotor UAV Keywords : visibility constraints, nonlinear optimization, agile aerial navigation, quadrotor UAV Abstract : Vision constitutes one of the most important cues in robotics. A single monocular camera can provide rich visual information at a reasonable rate that can be used as a feedback for control, state estimation of mobile robots or safe navigation in unknown environments for instance. However, it is necessary to satisfy particular visual constraints on the image such as visibility and occlusion constraints during motion to keep some visual targets visible. Quadrotors are endowed with very reactive motion capabilities due to their compact structure and motor configuration. Moreover, vision from a (fixed) on-board camera will suffer from rotation motions due to the system underactuation. In this thesis, we want to benefit from the system aggressiveness to perform several vision-based navigation tasks. We assume state estimation relies solely on sensor fusion of an onboard inertial measurement unit (IMU) and a monocular camera that provides reliable pose estimates. Therefore, visual constraints are challenging and critical in this context. In this thesis we exploit numerical optimization to design feasible trajectories satisfying several state, input and visual nonlinear constraints. With the help of differential flatness and B-spline parametrization we will propose an efficient replanning strategy inspired form Model Predictive Control to generate smooth and agile trajectories. Finally, we propose a minimum-time planning algorithm that handles intermittent visibility losses in order to navigate in larger cluttered environments. This contribution brings state estimation uncertainty at the planning stage to produce robust and safe trajectories. All the theoretical developments discussed in this thesis are corroborated by simulations and experiments run by using a quadrotor UAV. The reported results show the effectiveness of proposed techniques.

  (a) Un quadrirotor pouvant rapidement livrer un kit de premier secours. (b) Un quadrirotor utilisé par les ONGs Anglaises durant le tremblement de terre d'Avril 2015. (c) Le quadrirotor US-1 développé par Impossible Aerospace pour des missions de lutte contre les incendies.

Figure 1 -

 1 Figure 1 -Exemple d'applications pratiques réalisables par des quadrirotors

Figure 2 -

 2 Figure 2 -Un quadrirotor traversant des anneaux à haute vitesse nécessitant de fortes variations angulaires, de [1].

Contents

  Objectifs et contributions de la thèse . . . . . . . . . . . . . . . . . . . iv Structure de la thèse . . . . . . . . . . . . . . . . . . . . . . . . . . . . vi

  charge, Impossible Aerospace Fig.1.1c now opens a door to new exciting aerial applications. (a) Quadrotors could be deployed to rapidly deliver first aid kits. (b) A quadrotor used by British NGOs during the Nepal earthquake in April 2015. (c) Prelaunch units of the new quadrotor US-1 developed by Impossible Aerospace have been sold to firefighters, police departments, and search and rescue teams across the United States. The system could carry thermal cameras or multispectral sensors for search-and-rescue applications.

Figure 1 . 1 -

 11 Figure 1.1 -Example of practical applications completed by a quadrotor UAV

Figure 1 .

 1 Figure 1.2 -A classic quadrotor platform (from MikroKopter 2 ) used for our experiments.

Figure 1 . 3 -

 13 Figure 1.3 -Complex augmented aerial systems capable of extending their flight capabilities

Figure 1 . 4 -

 14 Figure 1.4 -Aggressive trajectories performed at the GRASP Lab from the University of Pennsylvania (left) and at the Robotics & Perception Group -UZH ETH Zurich (right).

Figure 1 . 5 -

 15 Figure 1.5 -Aggressive perching manoeuvres (left figure from [5]) and agile swing manoeuvres with a suspended load (right figure from [25])

Figure 1 . 6 -

 16 Figure 1.6 -Effect of underactuation for visual control of a quadrotor: the red target is repelled from the field of view as the quadrotor moves towards it.

Figure 2 . 1 -

 21 Figure 2.1 -Quadrotor model

Figure 2 . 2 -

 22 Figure 2.2 -Classic two loop controller. A task provides the desired position p d and yaw angle ψ d . The position controller generates the required pitch and roll angles to the attitude controller which applies the computed input thrust and torques to the system.

Figure 2 . 3 -

 23 Figure 2.3 -Trajectories in the state space can be mapped in the static flat space (i.e., without dynamics) of lower dimension. Any smooth enough curve in the flat space will be feasible in the state space.

Figure 2 . 4 -

 24 Figure 2.4 -System architecture for LQR control of a hexacopter. A function ( f ´1) converts the inputs u to the actual commands ν fed to the real system (commanded in acceleration with w) from [28].

  (a) Acceleration profiles that are typical in bang-bang control. (b) Trajectory (blue line) and total thrust direction (red arrows). The green line represents the convex hull of the spline control points (green dots).(c) Motors thrust within bounds fm " 0.1N and fM " 5N .

Figure 3 . 1 -

 31 Figure 3.1 -Results from the resolution of the minimum-time Problem 4 using the SQP method of the fmincon function in Matlab. The minimum-time trajectory has a duration of T ˚" 2.02s and is found after 13 SQP iterations

Contents

3. 1

 1 Introduction to optimization . . . . . . . . . . . . . . . . . . . . 31 3.2 Minimum-time trajectory generation problem . . . . . . . . . . 32 3.3 Pontryagin's minimum principle . . . . . . . . . . . . . . . . . . 33 3.4 Numerical solutions of optimal control problems using nonlinear programming . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34 3.4.1 Indirect and direct methods for nonlinear programming 35 3.4.2 Nonlinear solvers . . . . . . . . . . . . . . . . . . . . . . 36 3.5 Differential flatness and B-spline curves for nonlinear programming 38 3.5.1 Parametrization of the flat outputs . . . . . . . . . . . . 38 3.6 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43

Figure 4 . 1 -

 41 Figure 4.1 -At time k the future sequence of control variables is computed based on the prediction of the future states.Then the first value of the optimal control sequence is applied to the system (circled in red). At time k `1 the optimization is repeated with the same prediction horizon.

Figure 4 . 2 -

 42 Figure 4.2 -Simulated environment. The blue line represents the lemniscate path to track.The quadrotor has to avoid collisions with a sphere and two walls while keeping a minimal distance with the reference path.

Figure 4 . 3 -

 43 Figure 4.3 -The upper left figure shows the robot path (in red) and the reference path (in green). The obstacles are represented with the black volumes. The other figures show the derivatives of the flat outputs x, y, z.

Figure 4 . 4 -

 44 Figure 4.4 -The upper left figure shows the robot path in a cluttered environment. The robot is able to avoid the static obstacles even though the operator is sending high velocity commands with the joystick. The other figures show that the dynamic constraints are respected thanks to the MPC action. Note that the quadrotor reaches linear velocities up to 1.5m/s

Contents 4 . 1

 41 Introduction and context . . . . . . . . . . . . . . . . . . . . . . 45 4.2 Principle . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46 4.3 Receding horizon formulation: the linear case . . . . . . . . . . . 48 4.4 An application of MPC to quadrotor control . . . . . . . . . . . 51 4.4.1 A relaxed formulation based on differential flatness . . . 52 4.4.2 Results and delay compensation . . . . . . . . . . . . . . 53 4.5 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 55

Figure 5 . 2 -

 52 Figure 5.2 -Example of motion primitives from an initial state for an acceleration-controller system (left) and a jerk-controller system (right). The black arrow indicates the correpsonding control inputs, from [145].

Figure 5 . 3 -

 53 Figure 5.3 -Agile path tracking with a micro quadrotor in a heavily cluttered environment of strings and poles. The algorithm takes about 10 minutes to return a solution, from [29].

Figure 5 . 4 -

 54 Figure 5.4 -A path is built from a straight-line RRT* (middle figure) and then refined to obtain a minimum-snap trajectory that is feasible for the real system (right figure). The approach is much faster than a RRT* with a polynomial steer function (left figure), from [73]).

Figure 5 . 6 -

 56 Figure 5.6 -Shape of a trajectory passing through waypoints with different flying times, from [73].

Figure 5 . 7 -

 57 Figure 5.7 -Virtual image plane always parallel to the ground, from [157].

Figure 5 . 10 -

 510 Figure 5.10 -Examples of optimal navigation merging aggressive motion and perception objectives.

Figure 5 .

 5 Figure 5.11 -The two figures on the left show paths obtained without taking the state uncertainty into account (represented by the orange ellipsoids).With the presented method the planner is able to find paths with minimal uncertainty (the blue ellipsoids are much smaller), from[START_REF] Costante | Perception-aware path planning[END_REF].

Figure 6 . 1 -

 61 Figure 6.1 -Quadrotor model

Figure 6 . 2 -

 62 Figure 6.2 -Consider two pairs of dots on the ground horizontal plane (XY view in the upper right corner). It would be possible to cope with the field of view constraints by planning a near hovering trajectory (e.g. path in red), but in this work we aim at finding a trajectory similar to the path in yellow which is much more dynamic (and with a shorter completion time).

Figure 6 . 3 -

 63 Figure 6.3 -Example of (bounded) motors thrust profiles considering a parametrization of the flat outputs with a 4-th order B-spline basis.

Figure 6 . 5 -

 65 Figure 6.5 -Successive snapshots taken from V-Rep at different time instants. The straight line represents the vertical axis of the camera, the blue line is the planned trajectory and the red line is the actual system trajectory. The camera view is shown in the upper right corner.

Figure 6 . 6 -

 66 Figure 6.6 -Image feature trajectories planned at different planning steps. Four dashed segments represent the boundaries of the image domain. The image features are initially in the upper right corner.

Figure 6 . 7 -

 67 Figure 6.7 -Actual image feature coordinates measured during the re-planning. The horizontal dashed lines represent limits of the image domain.

Figure 6 . 8 -

 68 Figure 6.8 -Motor thrusts evolution for the four propellers with horizontal dashed lines representing the actuation domain U " r0.1, 7s.

Figure 6 . 9 -

 69 Figure 6.9 -Linear velocity (upper figure) and pitch and roll angles (bottom figure) during motion.

Figure 6 . 11 -

 611 Figure 6.11 -The visual constraints (6.10) is equivalent as ensuring that the angular position β of an image point P is lower than the field of view angle α{2

Figure 6 .

 6 Figure 6.12 -Successive snapshots taken from V-Rep at different time instants. The blue line is the planned trajectory and the red line is the actual system trajectory. The camera view is shown in the upper right corner where the red circle represents the field of view limits and the blue circle represents the vicinity constraint (6.11).

  (a) Optimal motor thruts profiles constrained between 0.1N and 5N. (b) Images features trajectories in the image plane with a field of view of 90 ˝.

Figure 6 .

 6 Figure 6.13 -A representative set of the computed inputs Fig. 6.13a and image features Fig. 6.13b trajectories from the resolution of Problem 8.

Figure 7 . 2 -

 72 Figure 7.2 -The projection of a sphere on the image plane of a camera at position r is an ellipse of semi-minor axis a 1 .

Figure 7 . 3 -

 73 Figure7.3 -In case of strong and sudden occlusions when the target is below an obstacle, the red trajectory cannot be a viable solution since it may violate actuation and/or spatial constraints. We instead allow minimal constraint violations to keep the solver efficient. The blue trajectory is then a relaxed solution where λ hits its limit value R occ (obstacle radius). The orange trajectory represents the case of a less relaxed occlusion constraint where λ eventually reaches a smaller value and is zero when there are no occlusions (green trajectory)

  where r xy is the target position in the XY plane at time t and the (square) image domain Ω " tβ P P 2 s.t. maxp|β T x B |, |β T y B |q ď tanpαqu.

Figure 7 . 5 -

 75 Figure 7.5 -Evolution of z T for the simulation. The final height was bounded between 1.9m and 3.5m. Again, z T reaches its minimum value when no collisions and occlusions occur.

3 - 6 Figure 7 . 6 -

 3676 Figure 7.6 -Different solution trajectories returned with different sets of weights (in Table 7.3) in our simulation environment. The camera view is shown in the upper right corner with the image trajectories and a view from above the target is shown below. The orange trajectory uses the chosen values (in Table7.2) with a higher w 3 . One can observe a faster convergence towards the image center (with an overshoot). The blue one has a higher w 1 resulting in a smoother trajectory than the orange one. The green one is even smoother and is more compliant to occlusions (less jerky) since w 5 is lower. The yellow trajectory is very sharp (low w 1 ) because the occlusion constraints are more respected (high w 5 ) and the final height is less penalized (low w 4 ). The pink solution gives a different and smoother path (high w 1 ) that benefits less from the constraints relaxation (since w 5 is high).

Figure 8 . 3 -

 83 Figure 8.3 -Unicyle and landmark notation.

  uncertainty ∆φ P R related to the bearing angle φ can be obtained as a function of the state uncertainty and is given by ∆φ "

Figure 8 . 4 -

 84 Figure 8.4 -Planner performance in exploring a 2D environment. When the graphs are hollow and no near vertices are found, both graphs propagate vertices toward each other (Fig. 8.4a). This helps finding a direct path that may be the optimal solution in case of low process noise. G 2 extends fewer vertices in this environment because of the obstacles that are slightly cutting down its expansion. One can see that the free space is explored in a reasonable uniform way.

114 8. 5 . 2 c 1 :c do 3 :

 1145213 Building the graph Algorithm 2 ConnectG Input: X 1 c , X success " false 2: for s 1 in X 1 for s 2 in X 2 c do 4:

Problem 13 Find}: ηpτ q} 2 ( 8

 1328 dτ s.t. sp0q " s 0 , spT q " s T , v ď vpτ q ď v, @τ P r0, T s |ωpτ q| ď ω @τ P r0, T s

min P 1 2 P 8 .

 128 T HP `f T P s.t. AP ď b, A eq P " b eq l b ď P ď u b Toward visual constraints relaxation: planning under intermittent measurements

Figure 8 . 7 -

 87 Figure 8.7 -The quadrotor in the vertical planes X-Z and Y-Z with ψ " 0. Having uncertainties on the state affects the perception. To evaluate if a landmark (red blob) is visible under the state uncertainty we check that condition (8.28) is satisfied on both planes. These conditions allow an exact and fast evaluation of the visibility and only rely on the flat state.

120 8. 8 .

 1208 Simulation and experimental results

Figure 8 . 8 -

 88 Figure 8.8 -Example trajectories for the unicycle with τ " 0.3s and u max " 3m.s ´2 in a 12x8m operating region. The figure on the left shows an optimal trajectory that moves past the landmarks to sufficiently reduce the uncertainty. On the right figure different initial and final states are chosen.

Figure 8 .

 8 Figure 8.9 shows an optimal trajectory for the 3D quadrotor and Fig. 8.10 shows the constrained derivatives along the connection considering a B-spline of order 4.With the given degree we can see that the curves (e.g., the jerks) are not penalized by conservatism. Finally, Fig.8.13 shows the tracking performance for a simulated quadrotor in V-Rep 1 using controller[START_REF] Lee | Geometric tracking control of a quadrotor UAV on SE(3)[END_REF]. We can see that the third-integrator model approximation is reasonable.

Figure 8 .

 8 Figure 8.10 -Velocity, acceleration and jerk constraints along the connection trajectory with bounds v " 2m.s ´1, ā " 4m.s ´2 and j " 10m.s ´3. The small squares represent the B-spline control points.

Figure 8 . 11 -

 811 Figure 8.11 -Full trajectory found by the planner. The red arrows represent the total thrust direction and the green circles represent the B-spline coefficients used for connecting the two trajectories.

8 .

 8 8.14) in presence of two obstacles (blue boxes) and four landmarks. 123 Toward visual constraints relaxation: planning under intermittent measurements (a) Full position profiles of Fig. 8.11. (b) Full velocity profiles of Fig. 8.11. (c) Full acceleration profiles of Fig. 8.11.

Figure 8 . 12 -Figure 8 . 13 -

 812813 Figure 8.12 -Connection of the position, velocity and acceleration profiles

8 .

 8 Toward visual constraints relaxation: planning under intermittent measurements (a) An optimal trajectory starting with a negligible uncertainty and connecting the goal with a limited uncertainty.(b) An optimal trajectory starting with a significant uncertainty. A path is found to reduce the uncertainty by collecting visual measurements from three targets.(c) An optimal trajectory exploiting the height of the room to increase the field of view of the camera.

Figure 8 . 15 -

 815 Figure 8.15 -Three examples of the six solution trajectories found with our method (all starting from the green dot).

  l B i`1,k´r puq (B.3)

Figure B. 1

 1 Figure B.1 shows basis functions of a B-spline curve. If the initial and final knots have multiplicity k then the B-spline curve is clamped -the first and last control points coincide with the endpoints of the curve, i.e. spu 0 q " P 0 and spu K q " P n .

Figure B. 2 -

 2 Figure B.2 -Running the De Boor algorithm at the parametric point u. At the final knot insertion, the last control point P 3,3 " u resulting in two independent left and right curves.

Figure B. 3 -

 3 Figure B.3 -Data flow diagram for the De Boor algorithm. The enveloppe contains the control points of the two independent curves.

´}r ´robs } 2 1 Figure C. 1 -

 211 Figure C.1 -Analytic evaluation of the collision avoidance constraints gradient.

  Fig. C.3 we compare the absolute error between FD and CS

Figure C. 2 - 157 C. 3 .

 21573 Figure C.2 -Relative error with CS and h " 1e ´10.

•

  Penin, Bryan and Spica, Riccardo and Giordano, Paolo Robuffo and Chaumette, François. "Vision-Based Minimum-Time Trajectory Generation for a Quadrotor UAV" in IEEE/RSJ Int. Conf. on Intelligent Robots and Systems, 2017. • Penin, Bryan and Giordano, Paolo Robuffo and Chaumette, François, "Visionbased reactive planning for aggressive target tracking while avoiding collisions and occlusions" IEEE Robotics and Automation Letters, 2018.

• Penin, Bryan and Giordano, Paolo Robuffo and Chaumette, François, "Minimum-Time Trajectory Planning Under Intermittent Measurements" IEEE Robotics and Automation Letters, 2019. Submitted to RAL/ICRA'19.

  .[START_REF] Hehn | Real-time trajectory generation for interception maneuvers with quadrocopters[END_REF] and Chapt. 8 we present solutions for relaxing the visibility constraints in complex environments.

	Chapter 3
	Optimization and numerical
	resolution
	Contents
	2.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13
	2.2 Quadrotor model . . . . . . . . . . . . . . . . . . . . . . . . . . 14
	2.3 General control and trajectory generation techniques for a quadro-
	tor . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17
	2.3.1 Quadrotor control . . . . . . . . . . . . . . . . . . . . . 18
	2.3.2 Trajectory generation for a quadrotor . . . . . . . . . . 19
	2.3.3 Smoothness . . . . . . . . . . . . . . . . . . . . . . . . . 20
	2.4 State estimation . . . . . . . . . . . . . . . . . . . . . . . . . . . 23
	2.5 Differential flatness . . . . . . . . . . . . . . . . . . . . . . . . . 24
	2.5.1 Definition and properties . . . . . . . . . . . . . . . . . . 24
	2.5.2 Existence . . . . . . . . . . . . . . . . . . . . . . . . . . 26
	2.5.3 Differential flatness in control and trajectory planning . 27
	2.6 Vision-based control . . . . . . . . . . . . . . . . . . . . . . . . . 28
	2.7 Issues related to vision-based control . . . . . . . . . . . . . . . 28

Table 7 .

 7 using the initial solution (a straight line from the initial to the final flat state) 4 -Convergence comparison between three different initial guess strategies. Failure ratio represents the percentage of total solutions that fail to converge within the current rate (1/30 ms).

	(M3)
	method mean number of SQP iterations failure ratio
	M1	25	2.8%
	M2	33	20%
	M3	43	84%

  With g as the (constant) gravity acceleration in the world frame one has t i " pae i `gq{}ae i `g}, i " 1, 2. The bearing uncertainty ∆β P R can be computed as

							.26)
	where l i are given by
	l 1 "	˜cos α ´sin α sin α cos α	¸t1 , l 2 "	˜cos α sin α ´sin α cos α	¸t2	(8.27)
		∆β "	ˆBβ Bη	Bβ B : η	¸ˆBβ 0 0 P : ˙T ˜Pη η Bη	B : η Bβ	˙(8.28)

  py C ˆzB q " ´ωBW T xB " ´}x B }ω x xT B 9 x B " x T C px B ˆzB 9 ψq ´yT C px B ˆhq " y T C z B ω x ´xT y B ω z 9 ψ ´xT C z B ω y 9 ψ `yT C z B pn ´ωx ω z qs ´ωx ω y z B pn ´ωx ω z qs ´ωx ω y (A.31)Finally, from Sect. 2.2 we compute the remaining inputs pu 2 , u 3 , u 4 q

	Since
			y T C h " ´ωBW	C y B	9 ψ	(A.27a) (A.27b)
	we have
	o "	1 }x B }	rx T C 9 x B 9 ψ `xT C x B : ψ `xT
					(A.28)
	Moreover from (2.4) we obtain
					x T B 9 x B " 0 y T B 9 x B " ω z z T B 9 x B " ´ωy	(A.29a) (A.29b) (A.29c)
	then
					x T C 9 x B " x T C py B ω z ´zB ω y q	(A.30)
	and we conclude that
	o "	1 }x B }	r2px T C 9 x B ω z	´xT
					24)
	It is easy to verify that
					m " ´lT y B n " l T x B	(A.25a) (A.25b)
	The third component o is found by taking the derivative of (A.19)
	o " ´x T 1 }x B } B 9 x B py T C x B 9 ψ 2 `xT C 9 x B 9 ψ `xT C x B : ψ ´xT C z B ω y 9 ψ `yT C hω z C x B 9 ψ `yT C z B qq }x B } 3 px T " 1 }x B } px T C 9 x B 9 ψ `xT	`yT C z B nq	(A.26a) (A.26b) (A.26c)

C x B : ψ ´xT C z B ω y 9 ψ `yT C hω z `yT C z B n ´xT B 9 x B ω z q T C C z B ω y q 9 ψ `xT C x B : ψ `yT C

  Using the Taylor series expansion of f px ´i∆xq, it can be verified that Equation (C.5) and (C.6) are the most basic equations that can be found using (C.4). The numerical advantages of the CS method are noticeable: Equation (C.5) actually shows a single term in the numerator rather than a difference, and hereby circumvents the instability related to term cancellation inherent to all classic, real valued FD approximations besides being more accurate. Equation (C.6) and (C.8) allows to compute an approximation to the second derivative in a single step that cannot be achieved by any FD approximation.

			Repf px `i∆xqq " �pf px ´i∆xqq	(C.7)
	Therefore, (C.6) can be written as
	f	2 pxq "	2pf pxq ´�pf px ´i∆xqqq ∆x 2	`Op∆x 2 q	(C.8)
	2 pxq "	2pf pxq ´�pf px `i∆xqqq ∆x 2	`Op∆x 2 q	(C.6)

The exact meaning of this term will be clear later

Optimization and numerical resolution on-line generation of optimal trajectories.

This work is the fruit of a common work conducted with Gerardo Rodriguez who completed his Master Thesis at Inria Rennes

http://www.coppeliarobotics.com/

Note that we trigger the planning at camera rate and not at the estimation rate. This is motivated by computational limitations and by the fact that, as already mentioned, the interframe estimation obtained by dead reckoning is expected to have a much lower accuracy.

https://www.youtube.com/watch?v=mZrS2wutZCI

https://www.youtube.com/watch?v=mvvF1I72HM8

https://www.youtube.com/watch?v=mvvF1I72HM8

http://www.coppeliarobotics.com/

https://youtu.be/a3akkwzyEfw

Appendix A gives the complete flat transformation and its inverse for the quadrotor.

Appendix B provides complementary information on the B-spline curves and details on the relevant manipulation algorithms used in this thesis.

Appendix C includes an introduction to numerical techniques for evaluating derivatives and especially complex-step differentiation. Practical results are given for comparing finite difference with complex-step difference.

We also conducted the same re-planning strategy with a real quadrotor. We used a MK-Quadro equipped with a front-looking camera with a field of view of 45 ˝. The setup included an on-board ODROID-XU4 Linux computer running ROS and the TeleKyb framework for interfacing the replanning algorithm which ran on a standard desktop PC (Quadcore Intel i7 CPU@2.6 GHz). A Vicon motion capture system was employed for giving state measurements of the quadrotor and position measurements of the tracked target. We used AprilTags as a generic target attached on top of another MK-Quadro controlled remotely (Fig. 6.14). The video shows the general behaviour of the system.

Introduction

The role of navigation in robotics is to find a path moving a robot from its current state to a goal state. Practical experiments on path following show us that paths cannot always be followed, because nothing is as perfect in reality as assumed during the planning (e.g., dead reckoning is not perfect in the real world). So there is a strong need to take into account the uncertainties during the planning phase. Indeed, model, sensors and environment uncertainties are inherent to many robotic applications and may lead to a failure of the task or impair the possibility to accurately follow a path if disregarded at the planning stage. For these reasons uncertainty-aware planning, also called belief-space planning, has received considerable attention in recent years. The concept of robust path planning can be tracked back to the mid 1990s. A class of control techniques that operate over the belief space, known as partially-observable Markov decision processes (POMDPs) [START_REF] Thrun | Probabilistic robotics[END_REF] Appendix A

The proof of differential flatness for the quadrotor

In this appendix we give the differential flatness transformation and its inverse for the quadrotor.

A. and also the total thrust

Given the yaw angle ψ " σ 4 we can define the vector:

and from (2.2) it is easy to verity that:

Provided that cospϕq ą 0, we are the able to compute x B as

The last axis of the frame B is simply given by

A. The proof of differential flatness for the quadrotor and the rotation matrix describing the full 3D orientation of the robot is

Now we take the first derivative of (2.14)

Projecting the equation along z B we obtain

We can now substitute 9 u 1 and u 1 back in (A.9) getting

We assumed in (2.5) that ω BW has components ω x , ω y and ω z in the body frame, i.e.

and hence

The third component ω z is found by considering that from (2.4)

and

Then, since y T B x B " 0, we can conclude that

Once the values of ω x , ω y and ω z are known we are able to compute ω BW as:

To calculate the angular acceleration B 9 ω BW we operate in the same way. By deriving (A.9) with respect to time we obtain:

Projecting this equation along z B we have:

A.1.1 Inverse flat transformation

Now, we deal with the inverse problem of the one studied in Appendix A.1: given the state of the robot in terms of r B , 9 r B , W R B and B ω BW and possibly the input vector u, we want to compute the value of the flat outputs and their derivatives. The position vector and its derivative are simply contained in the state and can immediately be extracted from it. Using the equations reported at the beginning of Sect. 2.2 we can compute the roll, pitch and yaw angles and their derivatives from the state components W R B and B ω BW . The linear acceleration is given by (2.14)

If the thrust is fixed then : r B is univoquelly defined, otherwise any value satisfying the equation pI ´zB z T B q: r B " ´gpI ´zB z

The minimum norm solution is

If 9 u 1 is fixed, then we must add to 9 a B a component along the z B axis such that (A.10) is satisfied, i.e.,

Assuming that the torque inputs u 2 , u 3 and u 4 are known we can compute the angular acceleration in the body frame from Sect. 2.2

and using (A.31) we compute : ψ as

where n and o are the last two components of B 9 ω BW . We also compute the component of δ orthogonal to z B by solving the system

Parametrization using B-splines

An overview of B-splines, from which much of the following is derived, can be found in De Boor [START_REF] Boor | A practical guide to splines[END_REF].

B.1 B-spline curve properties

In this section we provide a complete list of B-spline properties. Several of them are exploited in this thesis to derive algorithms to serve planning and optimization purposes.

' B i,p puq is a piecewise polynomial of degree p; ' B i,p puq has a minimum local support, i.e. it is equal to zero outside the interval ru i , u i`p`1 s.;

' Non negativity: the basis functions are positive;

' Geometry invariance: the B-spline basis function defines a partition of the unity, i.e.

which assures the B-spline is invariant under affine transformations (translation, rotation or scaling) of its control points;

' Local support: the function B i,k is zero outside ru i , u i`k s. This means that the change of a control point P i only modifies the spline in the interval ru i , u i`k`1 s ' the B-spline can be scaled or translated in time by scaling or translating the knot vector. The derivatives will scale or translate accordingly, in particular if Û " λU then ûpiq ptq " u piq ptq λ i ; ' knot insertion algorithm: A knot can be inserted into a B-spline without changing the geometry of the curve. The new curve is identical to the old one, with a new basis.

The algorithm is known as Boehm's algorithm. Inserting knots is generally used for refining the curve at a specific region and corresponds to an order elevation. As a result, the modified curve will get closer to its convex hull. If a knot is inserted at u as many times as the order of the original B-spline, the curve and the convex hull will coincide at u.

' knot removal algorithm: Knots can be removed for data reduction and curve approximation.

' subdivision algorithm: A B-spline curve can be subdivided into multiple Bsplines without changing the shape of the original curve. A subdivision at a specific parameter u can be performed either by applying the de Boor algorithm at u or by inserting a knot p times at u, where p is the B-spline curve degree. Such a process uses the De Boor algorithm and is detailed in the next section.

B.2.1 The curve subdivision algorithm

This section details the procedure for subdividing a B-spline curve of degree p with control points P " pP 0 , P 1 , . . . , P n q related to the knot vector U in two B-spline Appendix C

Gradient evaluation

C.1 On derivatives evaluation

Since we rely on gradient descent optimization algorithm, the quality of the gradient evaluation is central for driving the optimal solution towards a local minimum. Moreover, we are concerned with its accuracy and computational burden.

Differentiation results are well-known for certain classes of functions (quadratic functions for instance), but can be tricky for others. Although, analytic differentiation can be computed for complex constraints or terms in the cost function, their formulation generally inflates and may be tedious and subject to errors. Moreover, when one needs to code them it may take a huge amount of space and memory. Therefore, numerical solutions may become more attractive. In this section, we discuss and compare the most popular numerical differentiation techniques: finite difference, automatic differentiation, complex-step differentiation. Here, the main concerns are the accuracy, the numerical stability and the computation load.

C.2 Gradient approximation techniques

The technique of differentiation was introduced independently by Isaac Newton (1642´1727) and Gottfried Leibniz (1646´1716). Formally, the slope of the tangent line at a point x is the limit of the ratio of the change in the function to the change in the independent variable, as that change approaches 0, i.e.: The quotient in (C.1) is referred to as the Newton quotient or the difference quotient. Another way of expressing the derivative of a function derives from its expansion in a Taylor series, introduced by Brook Taylor in 1715. The Taylor series expresses any analytic real or complex function at a real or complex number a by an infinite