Localización a largo plazo de vehículos aéreos no tripulados basada en la percepción tridimensional del entorno

DEPARTAMENTO DE INGENIER´ IA DE SISTEMAS Y AUTOM´ ATICA ESCUELA SUPERIOR DE INGENIER´ IA UNIVERSIDAD DE SEVILLA Long-Term Localization of Unmanned Aerial Vehicles based on 3D Environment Perception por Francisco Javier P´erez Grau Ingeniero de Telecomunicaci´on PROPUESTA DE TESIS DOCTORAL PARA LA OBTENCI ´ ON DEL T´ ITULO DE DOCTOR POR LA UNIVERSIDAD DE SEVILLA SEVILLA, 2017 Directores Dr.-Ing. Fernando Caballero Ben´ıtez, Profesor Titular Dr.-Ing. An´ıbal Ollero Baturone, Catedr´atico ii UNIVERSIDAD DE SEVILLA Memoria para optar al grado de Doctor por la Universidad de Sevilla Autor: Francisco Javier P´erez Grau T´ıtulo: Long-Term Localization of Unmanned Aerial Vehicles based on 3D Environment Perception Departamento: Departamento de Ingenier´ıa de Sistemas y Autom´atica V◦B◦Director: Fernando Caballero Ben´ıtez V◦B◦Director: An´ıbal Ollero Baturone El autor: Francisco Javier P´erez Grau iii iv A problem well put is half solved. (John Dewey) v vi Acknowledgements First of all, I would like to sincerely thank my supervisors Fernando Caballero and An´ıbal Ollero for all the support, time, patience and teaching that I received from them. I appreciate a lot the amusing moments while developing, testing or writing with Fernando, as well as the effort and inspiring contributions from An´ıbal towards this dissertation. I must extend my gratitude to Antidio Viguria, since this work would not have been possible without his guidance and constant advice. He provided me with all the necessary resources to achieve my goals, and has always endeavored to ensure the maximum overlap between this dissertation and the different research projects in which I have participated over the past years. During my time as a PhD candidate, I had the opportunity to do a research visit at the Australian Centre for Field Robotics (ACFR) in Sydney. I want to thank Ali Haydar G¨oktoˇgan for his friendly hosting during my stay, aligning his research interests with those of this dissertation. I am happy to have shared those months with Will Reid at The Mars Lab, and most notably with Dan Wilson who embraced me as a brother so far away from home. Research activities like the ones carried out towards this dissertation would not have been possible without the great team of people at the Center for Advanced Aerospace Technologies (CATEC). I would like to thank my colleagues for their kindness, help and encouragement. The combination of excellent professional people in a friendly environment has led to many happy moments. In particular, I am grateful to ´ Angel Petrus and Ricardo Ragel for spending countless hours testing the aerial platform. I am also happy to have shared the indoor testbed with several fellows, vii especially Miguel ´ Angel Trujillo, who gave highly appreciated constructive feedback to this work in order to further improve it, apart from his bright ideas regarding “strings engineering” (´ Angel too!). And last, but certainly not least, I would like to thank my parents for all their love and support. They have enabled me to address the subsequent challenges I have faced throughout my life. Also, my closest friends who have been there whenever needed, in good times and bad. And most of all Mar´ıa, my loving, encouraging and patient life partner, whose faithful support during the final stages of this work, suffering my uncountable working hours at nights or holidays, is so appreciated. Thank you. Seville, May 2017. viii Abstract Unmanned Aerial Vehicles (UAVs) are currently used in countless civil and commercial applications, and the trend is rising. Outdoor obstacle-free operation based on Global Positioning System (GPS) can be generally assumed thanks to the availability of mature commercial products. However, some applications require their use in confined spaces or indoors, where GPS signals are not available. In order to allow for the safe introduction of autonomous aerial robots in GPS-denied areas, there is still a need for reliability in several key technologies to procure a robust operation, such as localization, obstacle avoidance and planning. Existing approaches for autonomous navigation in GPS-denied areas are not robust enough when it comes to aerial robots, or fail in long-term operation. This dissertation handles the localization problem, proposing a methodology suitable for aerial robots moving in a Three Dimensional (3D) environment using a combination of measurements from a variety of on-board sensors. We have focused on fusing three types of sensor data: images and 3D point clouds acquired from stereo or structured light cameras, inertial information from an on-board Inertial Measurement Unit (IMU), and distance measurements to several Ultra Wide-Band (UWB) radio beacons installed in the environment. The overall approach makes use of a 3D map of the environment, for which a mapping method that exploits the synergies between point clouds and radio-based sensing is also presented, in order to be able to use the whole methodology in any given scenario. The main contributions of this dissertation focus on a thoughtful combination of technologies in order to achieve robust, reliable and computationally efficient longterm localization of UAVs in indoor environments. This work has been validated ix xvi Contents 3 Robust Visual Odometry for UAVs 35 3.1 Registration................................ 36 3.1.1 Depth-only Registration . . . . . . . . . . . . . . . . . . . . . 36 3.1.2 Color-depth Registration . . . . . . . . . . . . . . . . . . . . . 45 3.2 Visual-Inertial Odometry . . . . . . . . . . . . . . . . . . . . . . . . . 53 3.2.1 Feature Detection . . . . . . . . . . . . . . . . . . . . . . . . . 55 3.2.2 Feature Description . . . . . . . . . . . . . . . . . . . . . . . . 55 3.2.3 FrameMatching.......................... 56 3.2.4 Attitude Correction . . . . . . . . . . . . . . . . . . . . . . . . 61 3.2.5 Key-Framing ........................... 61 3.2.6 Sparse Bundle Adjustment . . . . . . . . . . . . . . . . . . . . 62 3.2.7 Ground Plane Estimation . . . . . . . . . . . . . . . . . . . . 64 3.2.8 Experimental Results . . . . . . . . . . . . . . . . . . . . . . . 65 3.3 Conclusions ................................ 71 4 Multi-Modal Sensor Fusion for Long-Term Localization 73 4.1 StateEstimation ............................. 74 4.2 Gaussian Filters for State Estimation . . . . . . . . . . . . . . . . . . 76 4.2.1 Prediction............................. 80 4.2.2 Update............................... 80 4.2.3 Outlier Rejection . . . . . . . . . . . . . . . . . . . . . . . . . 81 4.2.4 Experimental Results . . . . . . . . . . . . . . . . . . . . . . . 81 4.3 Particle Filters for State Estimation . . . . . . . . . . . . . . . . . . . 85 4.3.1 Initialization............................ 87 4.3.2 Prediction............................. 89 4.3.3 Update............................... 90 4.3.4 Re-sampling............................ 93 4.3.5 Pose Computation . . . . . . . . . . . . . . . . . . . . . . . . 93 4.3.6 Experimental Results . . . . . . . . . . . . . . . . . . . . . . . 94 4.4 Conclusions ................................ 105 Contents xvii 5 Multi-Modal Mapping 107 5.1 Range-only localization and mapping (step 1) . . . . . . . . . . . . . 108 5.1.1 Problem Definition . . . . . . . . . . . . . . . . . . . . . . . . 109 5.1.2 Optimization ........................... 111 5.1.3 Initialization............................ 113 5.1.4 Weighting ............................. 113 5.2 3D Mapping and Pose Refinement (step 2) . . . . . . . . . . . . . . . 114 5.3 Experimental Results . . . . . . . . . . . . . . . . . . . . . . . . . . . 115 5.3.1 Mapping Experiment . . . . . . . . . . . . . . . . . . . . . . . 117 5.3.2 Localization Experiment . . . . . . . . . . . . . . . . . . . . . 119 5.4 Conclusions ................................ 122 6 System Architecture and Framework 123 6.1 UAVPlatform............................... 127 6.2 Controlledtests.............................. 129 6.3 EuRoCBenchmarking .......................... 130 6.4 EuRoCFree-Style............................. 133 6.5 EuRoCShowcase ............................. 137 6.5.1 Autonomous Delivery System . . . . . . . . . . . . . . . . . . 140 6.5.2 Missing Item Detection . . . . . . . . . . . . . . . . . . . . . . 141 6.6 Conclusions ................................ 143 7 Discussion and Conclusions 145 7.1 Conclusions of this Dissertation . . . . . . . . . . . . . . . . . . . . . 145 7.2 LessonsLearned.............................. 146 7.3 FutureWork................................ 147 References 149 xviii Contents List of Figures 1.1 GPS-based aerial survey in a refinery. . . . . . . . . . . . . . . . . . . 4 1.2 Vicon-based indoor testbed at CATEC. . . . . . . . . . . . . . . . . . 7 1.3 3D localization and mapping using a hand-held RGB-D camera and RTAB-Map................................. 11 1.4 6D localization for humanoid robots based on AMCL. . . . . . . . . . 14 1.5 Thesisoutline................................ 19 2.1 Sample color image (left) and its associated depth image (right). . . . 23 2.2 Stereo vision principle. . . . . . . . . . . . . . . . . . . . . . . . . . . 25 2.3 Skybotix’s VI-Sensor Schneith (2014). . . . . . . . . . . . . . . . . . . 26 2.4 RGB-D camera (ASUS’s Xtion PRO LIVE) Asus (2017). . . . . . . . 29 2.5 Orbbec’s Astra Orbbec (2017). . . . . . . . . . . . . . . . . . . . . . . 30 2.6 Nanotron’s swarm ER Nanotron (2017). . . . . . . . . . . . . . . . . 33 3.1 Scaled scenario of a landing site for testing depth-only registration. . 40 3.2 UAV used for testing depth-only registration. . . . . . . . . . . . . . 41 3.3 UAV flying over the asteroid scaled model and sample 3D data. . . . 41 3.4 Pose estimation for the descent trajectory compared to ground-truth data..................................... 43 3.5 Pose estimation for the hovering trajectory compared to ground-truth data. (Left) Position plots. (Right) Orientation plots. . . . . . . . . . 44 3.6 Schematic overview of the RGB-D registration pipeline. . . . . . . . . 47 3.7 Pixel comparisons to determine the existence of a FAST key-point. . 48 xix xx List of Figures 3.8 MAMMOTH rover with an RGB-D sensor on top. . . . . . . . . . . . 50 3.9 Map generated after driving the MAMMOTH rover over a section of theMarsYard. .............................. 50 3.10 Sequence of images from the trial. . . . . . . . . . . . . . . . . . . . . 51 3.11 Position and orientation plots of the MAMMOTH rover from the trial. 52 3.12 Schematic overview of our visual-inertial odometry pipeline. . . . . . 54 3.13 Features detected from the original image (top) on a sample image before (bottom left) and after (bottom right) applying the bucketing technique. ................................. 56 3.14 Feature matching between left and right images from the stereo pair. 57 3.15 Bundle adjustment projection example. . . . . . . . . . . . . . . . . . 62 3.16 CATEC testbed with a mockup scenario. . . . . . . . . . . . . . . . . 65 3.17 The UAV with the RGB-D sensor at the front. . . . . . . . . . . . . . 66 3.18 Ground-truth UAV trajectory in XY during the experiment. . . . . . 66 3.19 Estimated UAV position and orientation. . . . . . . . . . . . . . . . . 67 3.20 Localization errors in UAV estimation. . . . . . . . . . . . . . . . . . 69 3.21 Estimated UAV position and orientation using other approaches. . . . 70 4.1 Schematic overview of the EKF approach. . . . . . . . . . . . . . . . 79 4.2 UAV with the VI-sensor at the front (left) and ground-truth trajectory intheXYplane(right). ......................... 82 4.3 Estimated UAV localization (ground-truth in red, visual odometry in green, proposed approach in blue). . . . . . . . . . . . . . . . . . . . 83 4.4 Localization errors in position and associated RMS error. . . . . . . . 84 4.5 Automatic initialization of particles. From left to right and top to bottom, time evolution of particles after automatic initialization. Black arrows represent the particles. . . . . . . . . . . . . . . . . . . . . . . 88 4.6 CATEC indoor testbed (top) and 3D map with approximate radio beacons locations(bottom) used for field experiments. . . . . . . . . . 95 4.7 UAV with an RGB-D sensor at the front. . . . . . . . . . . . . . . . . 96 4.8 Another RGB-D sensor with passive markers for precise 3D map building. 96 List of Figures xxi 4.9 Ground-truth trajectory followed by the aerial robot during the experiment..................................... 97 4.10 Localization results (position and yaw) showing the ground-truth, visual odometry and the proposed approach. . . . . . . . . . . . . . . . . . . 98 4.11 Position and orientation errors with respect to ground-truth through time..................................... 99 4.12 Estimated UAV position and orientation using other approaches based onRGB-Dsensors. ............................ 100 4.13 Localization errors for different values of α................ 102 4.14 Localization errors for different number of particles used. . . . . . . . 103 4.15 3D map of the area with different resolutions: 0.2m (left) and 0.4m (right).................................... 104 4.16 Localization errors for different OcTree resolutions of the 3D map. . . 105 5.1 Multiple hypotheses for the localization of three UWB beacons. . . . 112 5.2 The UAV with RGB-D sensors at the front (left) and the rear (right) side, and a UWB sensor on top. . . . . . . . . . . . . . . . . . . . . . 116 5.3 The indoor testbed at CATEC with a mock-up scenario for 3D map building................................... 116 5.4 Results of the first step: Range-only localization and mapping. . . . . 117 5.5 Results of the second step: 3D Mapping and Pose Refinement. . . . . 118 5.6 Top view of the ground-truth map (left) and reconstructed map (right).119 5.7 Estimated UAV position and yaw angle for the long-term flight. . . . 120 5.8 3D visualization of sensor data, along with the particles (red cloud). . 121 5.9 Errors in the estimated UAV position and yaw angle using the groundtruth map and the reconstructed map. . . . . . . . . . . . . . . . . . 122 6.1 Schematic overview of the proposed architecture. . . . . . . . . . . . 124 6.2 The two main sensors tested on-board the UAV: VI-Sensor (left) and Astra(right). ............................... 128 6.3 A sample of 3D printed mount for the main vision-based sensor on-board theUAV. ................................. 129 xxii List of Figures 6.4 One of the testing scenarios in CATEC’s indoor testbed. . . . . . . . 130 6.5 ETH’s flying arena used in the Benchmarking and Free-Style rounds. 131 6.6 The UAV performing obstacle detection and avoidance of a human worker (left) and another smaller UAV (right). . . . . . . . . . . . . . 134 6.7 Obstacle detection and trajectory replanning results in the mannequin (top) and multi-UAV (bottom) experiments. . . . . . . . . . . . . . . 134 6.8 Estimated UAV position and orientation in the mannequin (top) and multi-UAV (bottom) experiments. . . . . . . . . . . . . . . . . . . . . 136 6.9 Airbus D&S manufacturing plant (top) and replicated environment at CATEC(bottom). ............................ 138 6.10 RGB-D sensors on-board the UAV and signaling lights. . . . . . . . . 138 6.11 UWB beacons installed in the indoor testbed. . . . . . . . . . . . . . 139 6.12 Small cargo bay on the UAV (left) and hopper for autonomous delivery (right).................................... 140 6.13 Estimated UAV position and orientation in the autonomous delivery experiment. ................................ 141 6.14 Estimated UAV position and orientation in the missing item detection experiment. ................................ 142 Chapter 1 Introduction 1.1 Motivation 1.1.1 The Localization Problem Imagine you are at home pleasantly watching a movie on a Saturday night, and suddenly the power goes out. Your cell phone is not at hand so you need to go check your fuses in complete darkness. Even though you know exactly how to reach that place in your house, and you think that the steps you make are taking you there properly, you will more likely bump into nearby obstacles such as tables or doors. Most of us rely on our eyes to help us find our way. By using the information from our eyes and other senses, we get an idea of where we are in the world, and thus can correct the inherent imperfections of our movements. Mobile robots suffer from the same issues when traversing the world. If they only move around without looking at where their movements are taking them, imperfections in their moving mechanisms will get them lost. Just like humans, sensing the environment can help them detect these imperfections and get a better idea of where they are. Besides mobility, autonomy is another key feature that enables robots to operate effectively in complex environments and perform tasks without explicit human intervention. Sensing the environment allows an autonomous robot to decide the execution of specific actions according to the data that is being gathered. But before doing 1 2Introduction so, the robot needs to be capable of self-localizing in its environment. Localization is one of the basic pillars of autonomous mobile robotics. The localization problem is essential for building a mobile robotic system, since accurate pose estimation is required for even the most basic tasks, even holding the position. It is commonly referred to as “the most fundamental problem to providing a mobile robot with autonomous capabilities” Cox (1991). Once this is achieved, the rest of technologies such as navigation and guidance can be implemented in order to determine where the goals are and how to reach them. 1.1.2 Aerial Robots: Definitions and Categories Nowadays, the aerial vehicles popularly known as “drones” are not unfamiliar to any of us. Though often associated with military activity, there is also keen interest in many civilian applications. Apparently, this is the last example of military technology transferred and made available for civilian use, such as the Internet or Global Positioning System (GPS). These platforms have brought a revolution in almost every field, largely due to the decreasing cost of technology and the fact that they have distinct functional advantages with respect to manned aviation. Industry experts indicate that this technology has been the most dynamic growth sector of the aerospace industry in the past decade Cavoukian (2012). They are often present in the news on television, newspapers, radio or social networks, used by domestic law enforcement, the private sector or amateur enthusiasts. Factors like the ability of rapidly exploring large and/or inaccessible areas, the reduction of material costs as well as personnel costs, the process automation and the reduction of working times make them suitable for many uses in industrial, governmental and academic fields. They are already being used in a variety of applications, and many more areas will benefit by their use Jenkins and Vasigh (2013). Among these areas, we can find wildfire mapping Merino et al. (2012), agricultural monitoring Saari et al. (2011), disaster management Maza et al. (2011), power line surveys Wang et al. (2010), law enforcement Puri (2005), telecommunication Zhan et al. (2011), weather monitoring Revercomb et al. (1996), aerial imaging/mapping Nex and Remondino (2014), television news coverage 1.1 Motivation 3 CNN (2016), sporting events Pedersen and Cooke (2006), movie-making Lin and Yang (2014), environmental monitoring Acevedo et al. (2013), oil and gas exploration Hausamann et al. (2005), etc. Common acronyms often used are UAV, UAS, RPA or RPAS; however, they do not mean the same. “Drone” is the popular denomination in the media, and started out as a military term; nevertheless, it is not commonly applied by specialized personnel in research activities. For ease of clarity, we will briefly discuss the meanings of the main acronyms in order to properly introduce the specific category used throughout this thesis. According to the International Civil Aviation Authority (ICAO) Cary (2011), the main terminology is explained as follows: • UAV (Unmanned Aerial Vehicle): an aircraft which is intended to operate with no pilot on board. • UAS (Unmanned Aircraft System): it is the UAV and its associated elements which are operated with no pilot on board, i.e. communication link, ground control station, etc. • RPA (Remotely Piloted Aircraft): an aircraft where the flying pilot is not on board (it is a subcategory of UAV). • RPAS (Remotely Piloted Aircraft System): it is the RPA and its associated remote pilot station, the required command and control links and any other system elements as may be required, at any point during flight operation. To summarize, the UAV (or UAS) is any aircraft (or system) in which the pilot is not physically on board the platform. In the case of RPA (or RPAS), express reference is made to the existence of a pilot who remotely operates the aircraft; whereas the definition of UAV (or UAS) leaves the option of carrying out the flight, or parts of it, as a fully autonomous operation. Hence all RPAS are UAS, but not all UAS are RPAS. That said, since this work focuses on the autonomous operation of aerial vehicles, we will refer to them as UAV or UAS, depending on the reference to the aerial platform or the complete system. In the scope of this dissertation, we will also refer to UAVs as aerial robots. 10 Introduction Many of the algorithms developed for stereo-vision odometry and SLAM can be applied to 3D cameras based on pattern projection, i.e. Red,Green,Blue-Depth (RGB-D) cameras Endres et al. (2012); Kerl et al. (2013). These sensors have recently become a very popular option due to their low weight, low cost and the amount of information provided; apart from RGB images they directly provide depth images of the scene in front of the sensor, saving the burden of 3D reconstruction computation (as in stereo-vision systems). Besides, they exhibit another important advantage with respect to classic stereo-based approaches: depth estimation does not depend on the presence of distinct visual features in the scene in order to estimate the depth. The availability of RGB-D cameras has made dense 3D point clouds available, which were previously only accessible using much more expensive sensors like Time of Flight cameras or scanning 3D laser rangefinders. There are several state-of-the-art open-source algorithms that provide localization estimations based on RGB-D cameras, such as RGBD-SLAM Endres et al. (2012), City College of New York (CCNY) RGB-D Dryanovski et al. (2013) or Real-Time Appearance-Based Mapping (RTAB-Map) Labbe and Michaud (2014) (see Figure 1.3). Nevertheless, they are rarely employed for on-line computation during the UAV flight, since these sensors typically generate voluminous data that typically cannot be processed in its entirety in real time (e.g. the Microsoft Kinect sensor produces over 9 million 3D points per second). The fact that the cited SLAM algorithms are continuously building their maps on-line greatly increases their computational requirements, which limit their usability and robustness given the usual on-board restrictions in terms of payload capacity and computational resources when working with UAVs. Point cloud based odometry and localization is another significant research area. Although computationally expensive, recent advances in kd -trees Nuchter et al. (2007) and Approximate Nearest Neighbours (ANN) Marden and Guivant (2012) enable faster computation times in the presence of medium and dense point clouds. The problem of aligning a pair of point clouds is commonly known as registration. Its output is usually a transformation matrix representing the rotation and translation that would have to be applied on one of the clouds in order to be perfectly aligned with the other. In order to do this, several techniques use a landmark-based approach, 1.2 Localization of UAS in GPS-denied Areas 11 Figure 1.3: 3D localization and mapping using a hand-held RGB-D camera and RTAB-Map. geometrically computing an estimate of the transformation based on the recognition of distinct features in the environment, occurring naturally or artificially placed Rusu et al. (2009). The factors contributing to the successful performance and integrity of these methods is the reliable acquisition and extraction of features from sensor data, and the ability to efficiently recognize and associate such features. This can be challenging depending on the spatial resolution and noise present in the depth sensor measurement, apart from the computational cost of analyzing dense 3D data. Another family of approaches is based on data correlation, attempting to utilize whatever sensor data are available to compute the transformation. This eliminates the need to decide what constitutes a feature, and uses a maximum likelihood alignment to find the best fit between two sets of data points. Iterative Closest Point (ICP) is a method capable of providing a computationally efficient pose estimations in complex, unstructured environments. ICP has the advantages of locally solving the problem of matching and localization, being generic and potentially used for real-time applications Pomerleau et al. (2013). 12 Introduction In general, vision-based odometry and localization systems for aerial robots are not reliable enough in the long term due not only to cumulative drift, but also external factors such as poor illumination, lack of texture, occlusions or moving objects. All these have a significant impact on the robustness and reliability of most state-of- the-art algorithms. In any case, the aforementioned approaches demonstrate fairly good results in the short term; however, they could quickly diverge depending on the environment. Autonomous systems intended to operate over long periods of time usually perform some sort of loop closing in order to recognize revisited places. This allows reducing the localization uncertainty at the cost of adding computational complexity S¨underhauf and Protzel (2011); Lowry et al. (2016). However, the problem of loop closing in order to distinguish revisited places is usually framed as a classification task, rather than a robot localization task Angeli et al. (2008). Reliable place recognition can be challenging in large-scale environments or sites with repetitive structures that might exhibit similar scenes in different areas. This is a very delicate issue since a single wrong loop closure can result in a devastating failure of the localization system, which could lead to an undesired flight termination. 1.2.2 Map-based approaches The other large family of localization techniques relies on previously built maps of the environment. Some UAS applications are usually carried out in known environments, e.g. logistic services or post-disaster assessments Ezequiel et al. (2014). In these cases, the aerial robot is required to carry out specific trajectories or reach certain places. This could be also a requirement for path planning in order to specify goals in a predefined coordinate system. These approaches use features such as the lines or planes that describe walls in hallways or offices to build a map of the environment. Sensor data can be matched with such features in order to determine the UAV location. Probably the most common map representations are occupancy grids, which were first introduced several decades ago Moravec and Elfes (1985). These grids are a probabilistic approach to represent 1.2 Localization of UAS in GPS-denied Areas 13 the environment in discrete cells that indicate their probability of being occupied by an obstacle. A great advantage is that they do not rely on specific predefined features, and are able to represent unknown areas. However, an important drawback of this approach is its large memory requirement, but recent developments such as OctoMap Hornung et al. (2013) provide efficient data structures particularly suited for robotics applications in the constrained equipment usually found on-board UAVs Nieuwenhuisen et al. (2014); Droeschel et al. (2016). One of the approaches based on a predefined model of the environment is commonly known as teach-replay Chen and Birchfield (2006); Royer et al. (2007), which is accomplished in two stages: first the robot is manually piloted along the desired path as in a teaching phase, and an accurate 3D map of the environment is built, along with the robot motion from this learning path; afterwards, this map is used to locate the robot when it repeatedly visits the same path. However, this approach is somewhat simplistic and limited, since it only enables a robot to follow a predetermined trajectory. Monte Carlo Localization (MCL) is another approach that makes use of a known map of the environment, and is one of the most popular algorithms used for robot navigation in indoor environments Thrun et al. (2001). It is a probabilistic localization algorithm that makes use of a particle filter to estimate the pose of the robot within the map, based on sensor measurements. Important benefits include the possibility of accommodating arbitrary sensor characteristics, motion dynamics and noise distributions. In order to obtain a reliable localization result, a certain number of particles will be needed. The larger the environment is, the more particles are needed. Actually, each particle can be seen as a pseudo-robot, which perceives the environment using a probabilistic measurement model. At each iteration, the virtual measurement takes large computational costs if there are hundreds of particles. For that reason, there is a variant of MCL called Adaptive MCL (AMCL) Fox (2001). The term “adaptive” comes from the fact that the number of particles is adjusted dynamically: if there is high uncertainty about the robot pose, the number of particles increases; if the pose is well known, such number decreases. Experimental approaches of either MCL or AMCL exhibit some limitations when it comes to aerial robots. Due to the aforementioned 14 Introduction Figure 1.4: 6D localization for humanoid robots based on AMCL. computational requirements, most of the existing approaches are meant for wheeled robots moving in a 2D environment, requiring a 2D laser scanner for map building and localization. Other authors presented an extension of this approach for 6D localization based on 2D laser scanner Hornung et al. (2010), but it is meant for 2D motion of humanoid robots in a 3D environment (see Figure 1.4), which makes it not suitable for UAVs. 1.3 Contributions The aforementioned approaches are promising in that they can all provide solutions to the localization problem, but important drawbacks are present using each approach alone. Infrastructure-based positioning systems achieve reliable performances, but they can be expensive and might require laborious setup and calibration processes. Vision-based approaches based on natural landmarks demonstrate good results relying only on on-board equipment, but are not robust enough when applied to UAVs and especially for long-term operation (i.e. long flight time). Map-based methods are robust solutions for long-term localization, but often demand a high computational cost and they need to previously build an accurate representation of the environment. 1.3 Contributions 15 The main contribution of this work focuses on the combination of technologies in order to achieve long-term autonomous operation of UAVs in indoor environments, taking advantage of their respective benefits to overcome their main limitations. This is accomplished by fusing data from different sensors in order to improve the performance of the overall system. In particular, a visual odometry algorithm based on stereo or RGB-D cameras and a localization algorithm based on UWB sensor beacons have been merged into an enhanced MCL algorithm which relies on a previously built multi-modal map that includes 3D occupancy data and the location of the UWB beacons. Specific contributions in each field are listed below, along with relevant related publications. • Research and development of a robust visual odometry approach suitable for 3D sensors, which provides a reliable short-term pose estimation. Validation of the approaches using both aerial and ground robots. – F.J. Perez, J. Gil, G. Binet and A. Viguria, “Validation of 3D Environment Perception for Landing on Small Bodies using UAV Platforms”, 13th Symposium on Advanced Space Technologies in Robotics and Automation (ASTRA 2015), ESA/ESTEC, Noordwijk, The Netherlands, 2015. Link. – W. Reid, F. J. Perez-Grau, A. H. G¨oktoˇgan and S. Sukkarieh, “Actively articulated suspension for a wheel-on-leg rover operating on a Martian analog surface”, 2016 IEEE International Conference on Robotics and Automation (ICRA), Stockholm, Sweden, 2016, pp. 5596-5602. doi: 10.1109 / ICRA.2016.7487777 – F. J. Perez-Grau, R. Ragel, F. Caballero, A. Viguria and A. Ollero, “Semi- Autonomous Teleoperation of UAVs in Search and Rescue Scenarios”, 2017 International Conference on Unmanned Aircraft Systems (ICUAS), Miami, FL, USA, 2017. Accepted for publication. • Research and development of multi-modal sensor fusion methods that combine the aforementioned visual odometry with other sources of 3D measurements, namely radio-range sensing and point clouds, for long-term localization. The 16 Introduction noise and outliers from radio measurements are filtered thanks to the odometry estimations, while the odometry drift is bounded thanks to radio-based measurements and point cloud matching. – F. J. Perez-Grau, F. R. Fabresse, F. Caballero, A. Viguria and A. Ollero, “Long-term aerial robot localization based on visual odometry and radiobased ranging”, 2016 International Conference on Unmanned Aircraft Systems (ICUAS), Arlington, VA, USA, 2016, pp. 608-614. doi: 10.1109 / ICUAS.2016.7502653 – F. J. Perez-Grau, F. Caballero, A. Viguria and A. Ollero, “Multi-Sensor 3D Monte Carlo Localization (MCL) for Long-Term Aerial Robot Navigation”, International Journal of Advanced Robotics Systems (IJARS). Accepted for publication. • Research and development of a multi-modal map building algorithm that exploits the synergies between radio-based distance estimations and point clouds from 3D imaging sensors, which requires a minimum setup. – F. J. Perez-Grau, F. Caballero, L. Merino and A. Viguria, “Multi-Modal Mapping and Localization of Unmanned Aerial Robots based on Ultra- Wideband and RGB-D sensing”, 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). Under review. • Development of a modular and extensible software architecture for safe and reliable autonomous navigation of aerial robots in GPS-denied environments, validated during extensive field testing throughout different experiment campaigns and demonstrations in the context of national and European Union (EU) funded research projects. – F. J. Perez-Grau, R. Ragel, F. Caballero, A. Viguria and A. Ollero, “An Architecture for Robust UAV Navigation in GPS-denied Areas”, Journal of Field Robotics (JFR), Special Issue on High Speed Vision-Based Autonomous UAVs. Accepted for publication. 1.4 Thesis Framework 17 • Highly efficient implementation of all the algorithms in order to make them suitable for real-time on-line localization in the usually constrained equipment that can be mounted on-board a UAV. – Video showing on-line localization for the autonomous operation of a UAV using the proposed architecture and algorithms. EuRoC Challenge 3 – Team GRVC-CATEC – Stage IIb (Showcase). It is important to point out the strong experimental focus of this dissertation, whose main contributions have been focused not only on advancing in the state-of- the-art, but also on implementing and validating different approaches in real-world setups. Proof of the potential impact of this work regarding its current technology development and future transfer to the industry is its recent recognition within the 1st EU Drone Awards, organized by the European Young Innovators Forum at the European Parliament in January 2017. A special innovative prize in the category “Best Drone-based Solution” was awarded to the application of indoor localization of UAVs to logistic operations in aircraft manufacturing plants EYIF (2017), where the system developed within this dissertation is used. 1.4 Thesis Framework The research leading to these results has received support from the Spanish Centro para el Desarrollo Tecnol´ogico Industrial (CDTI) INNPRONTA 2011-2014 program within Perigeo project, and the European Community’s Seventh Framework Program (FP7) project EuRoC (FP7-ICT-608849) in the period 2014-2017. This dissertation has been extensively validated within the context of the European Robotics Challenges (EuRoC) 1 , an EU FP7 project whose main motivation is to bring innovative technologies from research labs to industrial end-users. In order to do that, a series of challenges in a public competition format were presented, and one of them (Challenge 3) is related to the demonstration of high-level semi-autonomous operation of a UAV for an inspection task. The goal is to enable unskilled workers 1http://www.euroc-project.eu 18 Introduction to perform complex inspection missions with the aid of UAVs. This is difficult to achieve as the complexity of UAVs requires expert piloting skills. In this context, a framework has been developed in order to perform localization and state estimation of the UAV without external positioning systems such as GPS or motion capturing systems, as well as autonomous local obstacle avoidance, local path planning, following of structures or homing of the aerial robot. The author is part of the challenger team GRVC-CATEC, which is currently competing among top European research institutions, and has successfully demonstrated accurate localization of the UAV platform within the project. 1.5 Thesis Outline The different chapters of this document describe the evolution of the developed system along the years of work towards this dissertation. There was an incremental strategy based on the accomplishment of intermediate development objectives, all of them aiming at the overall goal of achieving long-term localization of UAVs in GPS-denied areas, keeping in mind safety and robustness as flagship features for the successful widespread use of autonomous aerial robots. Following this incremental approach, the chapters are structured as follows: • Chapter 2 discusses several sensing modalities that can be used throughout the different developments. • Chapter 3 details the incremental works towards a vision-based odometry algorithm using images, 3D point clouds and inertial measurements. • Chapter 4 presents the evolution of our approach to combine visual odometry with radio-based measurements from UWB beacons and a 3D map of the environment. • Chapter 5 describes a 3D map building method in order to be able to use the previously described localization system in any environment. 1.5 Thesis Outline 19 • Chapter 6 introduces an overview of the whole framework in which this work has been tested and validated for autonomous navigation of UAVs in EuRoC project. •Chapter 7 includes conclusions, lessons learned and future lines of work. Figure 1.5 shows a pictorial representation of the thesis outline, presenting how chapters are related with each other. Chapter 1 Introduction Chapter 2 3D Perception for Localization Chapter 3 Robust Visual Odometry for UAVs Chapter 4 Multi-Modal Sensor Fusion for Long-Term Localization Chapter 5 Multi-Modal Mapping Chapter 6 System Architecture and Framework Chapter 7 Discussion and Conclusions Experimental Results 6DoF Pose 3D MapOdometry Sensors On-line localization Figure 1.5: Thesis outline. 26 3D Perception for Localization Figure 2.3: Skybotix’s VI-Sensor Schneith (2014). the cameras). Given the restrictions in camera sizes that can be mounted on-board a small UAV, applicable baselines can handle a relatively limited depth range. The stereo-vision sensor used in some stages of this work is the Visual-Inertial (VI-) Sensor, shown in Figure 2.3, which provides fully time-synchronized and factory calibrated IMU- and stereo-camera data streams. In particular, stereo-vision has been used as the main odometry source in some of the experiments included in Chapters 3 and 6. 2.1.2 Active Imaging LIDAR Probably the most common sensors based on active optical methods for obtaining depth information are LIght Detection And Ranging (LIDAR) instruments. They are based on the well-known time of flight principle, i.e. the distance to an object is measured from the time it takes a light pulse to travel from the emitter until it returns reflected from the object. Hence an active light source is required, often in the IR frequency range. Since all electromagnetic energy travels at the speed of light c , in free space, the relationship between the distance Z and the round-trip travel time t is given by Z=c·t 2(2.2) LIDAR systems provide high precision measurements at high data rates; however, their high cost and relatively high weight, or the non-simultaneous points acquisition 2.1 Optical Sensing 27 (points are grabbed one after another, not simultaneously), limit their widespread use in some applications involving UAV platforms. For these reasons, this dissertation has not focused on this type of active imaging technique. ToF cameras Light pulses are not the only method to measure time of flight. By using a modulated signal, distances can be calculated by estimating the phase difference between the emitted and the reflected signal. The distance Zis then obtained as follows: Z=c 2·fm·φ 2π(2.3) where c is again the speed of light, fm is the modulation frequency of the emitted signal and φis the estimated phase shift between the emitted and received signal. Due to the periodicity of the modulation signal, Equation 2.3 is only valid for distances smaller than c 2·fm . The modulation frequency fm of the emitted signal determines the “ambiguity-free” distance range of the sensor. The last decade has seen an increasing trend in the development of 3D cameras. Time of Flight (ToF) cameras are a relatively new type of sensor that delivers 3D imaging at a high frame rate, simultaneously providing intensity data and range information for every pixel. Near-IR modulated light waves are emitted by several Light-Emitting Diodes (LEDs) and reflected by the objects on the scene back to the imager. Conventional imaging sensors consist of multiple photo-diodes arranged in a matrix. Normally, these diodes provide a gray-scale or color image of the scene. In contrast to normal cameras, a Photon Mixing Device (PMD) sensor additionally acquires a distance value for each pixel simultaneously to the intensity (gray) value. Despite this remarkable progress, it is still made with standard Complementary Metal- Oxide-Semiconductor (CMOS) technology. Therefore, the pixels in these cameras are often called “smart pixels” Xu et al. (1998). Compared to other technologies that obtain 3D information, ToF cameras allow the acquisition of depth images without any scanning mechanism and from just one point of view. They register dense depth along with intensity images at a high frame 28 3D Perception for Localization rate. They are small, low-weight and compact, since no mobile parts are needed. They have low costs compared to LIDAR systems, and a lower power consumption with respect to classical laser scanners. Another important characteristic of these devices is their ability to operate in environments where other systems would be unable to do so. For example in untextured scenarios, where the operation of stereo-vision systems would be very difficult due to the lack of representative features on the images to solve the corresponding problem. Important disadvantages of these systems are their high sensitivity to noise, especially in outdoor scenarios due to interference with direct sunlight, and their low-depth measurement accuracy. This is due mainly to the way in which ToF cameras grab data, which consists in the scene being bombarded with near-IR light, capturing a whole surface included into the emitted light cone in one single shot. This differs from laser rangefinders, which acquire points sequentially with very high accuracy. These devices also provide very low resolution (no more than a few thousands of tens of pixels) compared to current standard cameras. Nevertheless, when it comes to mounting a sensor on-board a UAV platform, additional considerations and requirements must be taken into account. Especially important are the weight, size and power consumption of the sensor, since the payload capacity of small UAVs is very limited. Due to these reasons, the preferred sensing modality for obtaining dense 3D depth images is based on another type of active imaging technique known as structured light. Structured light Sensors based on structured light simplify the solution of the correspondence problem introduced in stereo-vision techniques. They replace the second camera in the stereo setup by a light source which projects a known pattern of light on the scene. If the scene is simply a planar surface, the pattern acquired by the first camera will be similar to the projected pattern. When the surface is non-planar, the geometric shape of the surface distorts the projected pattern. The shape of this surface can then be obtained based on the information from the distortion of the projected pattern. Still, some correspondences between both patterns must be solved. Different projection 2.1 Optical Sensing 29 Figure 2.4: RGB-D camera (ASUS’s Xtion PRO LIVE) Asus (2017). patterns have been proposed such as binary codes or color-coded stripes Geng (2011). The most usual pattern is the projection of a grid, in which an easier correspondence problem has to be solved. In this case, we only need to identify, for each point of the captured pattern, the corresponding point of the projected pattern. Apart from ToF cameras, a new type of sensors commonly referred to as RGB-D cameras can provide both visual texture information and per-pixel depth information simultaneously. They have two cameras: the first is usually a conventional webcam that records color video, and the second is an IR camera that records a non-visible structured light pattern generated by the IR projector (see Figure 2.4). Microsoft’s Kinect was probably the first affordable RGB-D camera widely used in robotics research Boudjit et al. (2008). The per-pixel depth-sensing technology that is used in consumer RGB-D cameras was developed and patented by PrimeSense Garcia and Zalevsky (2008). The depth acquisition technology is named Light Coding; it has an IR light source to project a complex pattern of dots into the scene. This pattern is perceived by an IR camera and the distance to each dot is computed by triangulation to build a 3D model of the scene. The color information of this model is gathered by an RGB camera. Regardless of texture and illumination condition, an RGB-D camera can directly obtain 3D information, unlike stereo-vision due to the lack of representative features in the images. Above all, they are small, low-weight and compact, since no mobile 30 3D Perception for Localization Figure 2.5: Orbbec’s Astra Orbbec (2017). parts are involved. Besides, they have much lower cost and lower power consumption when compared to LIDAR systems and ToF cameras. One important limitation of RGB-D cameras is that they can only operate reliably indoors, since the projected pattern is overwhelmed by exterior lighting conditions. Another drawback is the measurement noise when compared to LIDAR sensors, and their limited working range (up to 10m). Nevertheless, RGB-D sensors have seen widespread adoption in robotics research due to their ability to generate reliable 3D data at a fast frame rate at low cost Han et al. (2013). Regarding on-board requirements for the UAV platform, subsequent models of RGB-D cameras from other vendors exhibit great advantages with respect to Microsoft Kinect: they are significantly smaller, easier to integrate and do not require an external power supply. These aspects make it much more portable and suitable for small aerial vehicles. Examples of these other cameras are ASUS’s Xtion PRO LIVE (shown in Figure 2.4), and more recently Orbbec’s Astra (see Figure 2.5) which exhibits a longer depth range (up to 10m instead of 4m). These two cameras have been used in the experiments of this dissertation, the Xtion PRO LIVE in Chapter 3 and the Astra in Chapters 4, 5 and 6. Table 2.1 summarizes the main specifications of the previously discussed RGB-D cameras. 2.2 Non-optical Sensing 31 Table 2.1: RGB-D cameras comparison Specifications Kinect Xtion PRO LIVE Astra Size (mm) 305 x 63 x 76 180 x 35 x 50 165 x 30 x 40 Weight (g) 1320 540 300 Range (m) 0.8 - 4 0.8 - 3.5 0.6 - 8 Depth Image Size 640 x 480 640 x 480 640 x 480 RGB Image Size 640 x 480 640 x 480 640 x 480 Frames per second 30 30 30 Field of View (◦) 57 x 43 58 x 45 60 x 49.5 Power External USB USB 2.2 Non-optical Sensing As stated earlier in this chapter, non-optical sensing refers to the use of pulses or waves not included in the visible or the IR spectrum for obtaining 3D measurements. This section focuses on microwaves, in particular on UWB, which is the technology that has been used in this work. UWB is a high data rate, low power short-range wireless technology that is generating a lot of interest in the research community and the industry, as a highspeed alternative to existing wireless technologies such as IEEE 802.11 WLAN, Home Radio Frequency (RF) and HiperLANs Lad (2004). Even though UWB has been around for more than 40 years, a substantial change occurred in 2002, when the Federal Communication Commission (FCC) issued a report allowing the commercial and unlicensed deployment of UWB with a given spectral mask for both indoor and outdoor applications in United States. This wide frequency allocation initiated a lot of research activities from both industry and academia, focusing in recent years on consumer electronics and wireless communications. UWB transmits binary data, using low energy and extremely short duration impulses or bursts (in the order of picoseconds) over a wide spectrum of frequencies. It delivers data over 15 to 100 meters and does not require a dedicated radio frequency, 32 3D Perception for Localization so is also known as carrier-free, impulse or base-band radio. UWB systems use carrierfree, meaning that data is not modulated on a continuous waveform with a specific carrier frequency, as in narrowband and wideband technologies. UWB technology has the following significant characteristics Lad (2004): •High data rate : it can handle more bandwidth-intensive applications like streaming video, than either 802.11 or Bluetooth, reaching data rates of roughly 100 Megabits per second (Mbps), with speeds up to 500 Mbps. The maximum speed for 802.11a is 54 Mbps, while for Bluetooth it is about 1 Mbps. •Low power consumption: it constantly transmits short impulses, instead of transmitting modulated waves like most narrowband systems do, and hence does not need conversion between frequencies, local oscillators, mixers, and other filters. •Interference immunity : due to low power and high frequency transmission, UWB’s aggregate interference is vaguely detected by narrowband receivers. This makes it suitable for coexistence with narrowband radio systems operating in the same spectrum without causing undue interference. •High security : since UWB systems’ noise is very low, they are inherently covert and extremely difficult for unintended users to detect. •Low complexity, low cost : the most attractive of UWB’s advantages are its low system complexity and cost. Traditional carrier based technologies modulate and demodulate complex analog carrier waveforms. Due to the absence of carrier in UWB, the transceiver structure can be very simple. Besides, recent advances in silicon process and switching speeds make UWB systems also low-cost. •Resistance to jamming : UWB spectrum covers a huge range of frequencies. That is why its signals are relatively resistant to jamming, because it is very difficult to jam every frequency in the UWB spectrum at the same time. Therefore, there is a wide frequency range available even in the case that some frequencies are jammed. 2.2 Non-optical Sensing 33 Figure 2.6: Nanotron’s swarm ER Nanotron (2017). •Scalability : UWB systems are very flexible because their common architecture is software re-definable, so that it can dynamically trade-off high-data throughput for range Chong et al. (2006). UWB signaling is especially suitable for position and ranging applications due to its low energy, high bandwidth and fine temporal resolution properties. It allows accuracies of a few centimeters in ranging, as well as low-power and low-cost implementation of communication systems Gezici et al. (2005). The process involves exchange of signals between nodes, and measurement of parameters to estimate distances. Distance measurements between two radio-based sensors are usually based on the energy (signal strength) or travel times (time of arrival) of signals between the devices. To determine the distance from energy measurements, the characteristics of the channel must be known; therefore this technique is very sensitive to the estimation of those parameters. In that sense, the measurements of travel times is preferred. If the two sensors have a common clock, the sensor receiving the signal can determine the time of arrival of the incoming signal that is time-stamped by the emitter sensor. In the absence of a common clock between the sensors, round-trip times can be measured in one of the sensors to estimate the distance between the two. The UWB sensors used in this work are manufactured by Nanotron Technologies within their swarm product family, which provide accurate and relatively low-cost location capabilities. These UWB sensors provide distance measurements with an accuracy of 10 cm. A sample board is shown in Figure 2.6. 34 3D Perception for Localization 2.3 Conclusions This chapter summarizes different 3D sensing modalities that are considered relevant for the purpose of localization of aerial robots in GPS-denied areas. They have been classified according to how they obtain depth information, whether it is a simple point-to-point distance estimation or a full depth image. Different sensors have been discussed, highlighting their main advantages and limitations regarding their applicability to small UAVs. Taking into account these factors and on-board requirements, the most interesting selected devices to be tested are stereo cameras, RGB-D cameras and UWB sensors. Chapter 3 Robust Visual Odometry for UAVs The first step of the work carried out towards this dissertation was obtaining localization estimations through the analysis of data provided by a 3D imaging sensor. During the past decades, different techniques have been proposed in order to tackle this estimation problem, generally by processing sensor data acquired at subsequent time instants. Vision-based odometry addresses the problem of estimating the motion of a robot by only using information from vision sensors Scaramuzza and Fraundorfer (2011). One of the most popular techniques is known as registration, which is the process by which two data sets are brought into alignment. In particular, when dealing with 3D imaging sensors, it involves aligning 3D point clouds. Both passive and active optical sensors, such as stereo cameras or RGB-D cameras, can provide dense 3D point clouds at a high frequency. Then, subsequent point clouds can be matched in order to deduce the transformation between them and, consequently, the 6 Degrees-of-Freedom (DoF) motion of the sensor. Apart from that, many state-of-the-art approaches are based on extracting interest points from RGB images, and matching them with those extracted in previous frames. By using the 3D information associated with such points, the transformation between frames can be estimated, and hence the motion of the robot Fraundorfer and Scaramuzza (2012). In the presented approach, registration was first implemented, using only 3D point clouds to estimate the robot localization. Then, this processing pipeline was optimized 35 42 Robust Visual Odometry for UAVs Ground-truth localization data for comparison were acquired by the motion capture system of CATEC’s indoor testbed. In both experiments, the UAV automatically followed a predefined list of waypoints using the motion capture system to close the control loop, and then the point clouds were processed off-line in order to obtain the localization estimations shown in the figures. Only the initial ground-truth pose of the UAV was used in order to properly initialize the registration algorithm. Figure 3.4 shows the results of the descent trajectory in position ( x, y, z ) and orientation (roll, pitch, yaw), including both the ground truth values (gtX, gtY, gtZ, gtROLL, gtPITCH and gtYAW) and the registration algorithm outputs (estX, estY, estZ, estROLL, estPITCH and estYAW). Errors in position are relatively small ( < 10cm), as well as in orientation ( < 0 . 2 ◦ ) during the whole trajectory. In the case of descent trajectories, initial errors in the point cloud matching process tend to converge. The overall map that the algorithm is continuously building usually represents the same surface of the small body. As the UAV gets closer to the asteroid model, the point clouds describe this surface with more detail, leading to better results in the matching process. Even though the results are already promising, there is still significant room for improvement. The overall map that the algorithm continuously builds is initialized at the beginning of the trajectory, when the aerial robot is at the furthest distance from the scaled asteroid surface. At such distance (around 3m), possible UAV vibrations and 3D sensor noise greatly affect the initial point cloud comparisons, thus making the matching process much more challenging than when the UAV is closer to objects in general. Figure 3.5 shows the results of the hovering trajectory also in position and orientation. Errors in position are similar than those in the case of the descent trajectory. In this case, the UAV flight height was approximately 1m above the scaled asteroid, which allowed the 3D sensor to provide depth measurements without much noise. However, at certain moments, the surface region acquired by the sensor appears mainly flat; hence the point cloud matching can be ambiguous since there are not enough depth details that could help the matching process. 3.1 Registration 43 Figure 3.4: Pose estimation for the descent trajectory compared to ground-truth data. Table 3.1 summarizes the root-mean-square (RMS) errors of pose estimations for both trajectories, in position and orientation. 44 Robust Visual Odometry for UAVs Figure 3.5: Pose estimation for the hovering trajectory compared to ground-truth data. (Left) Position plots. (Right) Orientation plots. The results show the good performance of the proposed algorithm, but a number of shortcomings prevent this setup from constituting a generic solution for long-term UAV localization: 3.1 Registration 45 Table 3.1: RMS localization errors x(m) y(m) z(m) roll (◦)pitch (◦)yaw (◦) Descent trajectory 0.024 0.042 0.063 0.007 0.002 0.111 Hovering trajectory 0.027 0.023 0.009 0.004 0.02 0.003 • The distance that the UAV traveled in the experiments was relatively short (<5m). • The velocity at which the UAV performed the experiments was low (0 . 05m/s) in order to recreate scaled-down space exploration trajectories. • Sensor data was underused. Apart from 3D point clouds, the RGB-D sensor provided color images that were not used in order to faithfully emulate the Flash LIDAR sensor under test. Besides, orientation data from the on-board IMU were not used either. Nevertheless, these experiments helped to start the development of the software framework for the rest of technologies towards this dissertation, as well as gaining field experience in the setup of indoor flight testing. 3.1.2 Color-depth Registration 3D imaging sensors usually provide not only a point cloud of the scene, but also their associated color images. It seems reasonable to make use of such images, since adding RGB information to the software processing pipeline can improve the reliability of the registration process. In this way, we can take full advantage of all the information that the sensor provides (either a stereo or RGB-D camera), both visual and depth data. Over the last decades, different ICP variants have been introduced Rusinkiewicz and Levoy (2001) proposing improvements in any or more of the stages of the algorithm, namely: •Selecting some set of points in one or both point clouds. 46 Robust Visual Odometry for UAVs •Matching the points from one cloud to samples in the other point clouds. •Weighing the corresponding pairs appropriately. • Rejecting certain pairs based on looking at each pair individually or considering the entire set of pairs. •Assigning an error metric based on the point pairs. •Solving the optimization problem. In order to reduce the computational cost of our processing pipeline, we have focused on optimizing the first stage of the ICP algorithm, i.e. selecting interest points on both the reading and the reference point clouds. At each frame, a set of visual features is extracted from the color image and mapped to their 3D locations using the associated point cloud. The set of 3D points is then matched across consecutive frames to estimate sensor pose increments since the last processed frame. Figure 3.6 shows an overview of the method. The interest points are extracted using the Features from the Accelerated Segment Test (FAST) algorithm Rosten and Drummond (2006), which is a computationally efficient method for corner detection. According to this algorithm, a pixel is defined as a key-point if in a circle surrounding the pixel, N or more contiguous pixels are all significantly brighter than, or all significantly darker than the center pixel, as depicted in Figure 3.7. The extracted key-points are pixels which contain local information that ideally makes them repeatable across consecutive frames. Depending on the environment, the scene in front of the sensor could be feature-less, or there might be strong-detailed regions where all the key-points are detected. Both cases pose problems in the subsequent stages of the registration method and may lead to poor transformation estimations. In order to overcome such issues, a bucketing technique has been adopted. The image is divided into several regions, and a fixed number of key-points is required to be extracted from each one. Our approach is based on subdividing the image into six buckets (two columns and three rows), hence providing a relatively homogeneous distribution of key-points in the image. 3.1 Registration 47 Figure 3.6: Schematic overview of the RGB-D registration pipeline. The proposed approach is not purely based on visual feature matching though, in the sense that it does not try to find the same key-points across consecutive RGB images. Instead, the set of 2D key-points based on FAST features is enhanced with depth information to turn it into a set of 3D key-points. This is performed by directly using their corresponding depth values from the associated point cloud (in the case of stereo cameras, from the disparity map computed from the pair of images). Then, the strategy is to align consecutive sets of 3D key-points, assuming that the selected FAST features are repetitive enough to be found in consecutive frames. 3D imaging sensors provide depth information for most of the image pixels, but not all of them. This is a common situation in both stereo and RGB-D cameras. 48 Robust Visual Odometry for UAVs Figure 3.7: Pixel comparisons to determine the existence of a FAST key-point. Depending on the scene and the physical properties of the materials in the environment, a 2D key-point might not have a corresponding depth value. Moreover, a 2D key-point might correspond to a very far location, thus the noise of the depth measurement can be significant. In these cases, our approach rejects the 2D key-points from the feature set. The fact that we might be throwing away possibly “good” key-points does not significantly impact on the performance of the algorithm, since the size of the key-points sets is relatively large (several hundreds of points). Subsequent frames are analyzed in order to obtain sets of 3D key-points. Once a set of 3D key-points has been filtered, an alignment process is carried out to find the best fit between two sets. As in depth-only registration, the compared sets do not correspond with consecutive frames in order to mitigate the drift effect commonly present in odometry approaches. The sets to align are the current frame and the overall map that is continuously being built using the aligned 3D points. The overall goal is still to apply a transformation to one set to bring it as close as possible to the other, and ICP is used again to find such transformation. In this case, due to the sparse distribution of the 3D key-points, the point-to-point error metric is used in the iterative process: E(T) = N X i=1 kTui−zik(3.1) 3.1 Registration 49 where N is the size of the current set of 3D key-points ui , zi is the corresponding point in the map point cloud, and T is the transformation matrix composed of a rotation and translation. ICP’s computational load is greatly decreased with respect to our previous approach by reducing the size of the point clouds when only using the 3D key-points from both data sets. Other main drawbacks of ICP include its inability to deal with noise and outliers in the point clouds, or the absence of enough overlap between the clouds. Both cases are covered in the proposed approach, since the 3D key-point extraction process focuses on removing noise from distant points and possible outliers. Additionally, the high frame rate of 3D imaging sensors facilitates high overlap between the current point cloud and the overall map that is being built. The sensor pose update is finally transformed to the robot body frame in order to obtain a localization pose update. Experimental Results Color-depth registration results were obtained in the context of a research stay performed at the Australian Centre for Field Robotics (ACFR) at the University of Sydney. RGB images and 3D point clouds are acquired from an RGB-D sensor facing slightly down on-board the Mars Analog Multi-Mode Traverse Hybrid (MAMMOTH) rover Reid et al. (2014), shown in Figure 3.8. Even though the robotic platform is not a UAV, the accuracy of the localization algorithm was successfully validated through an active articulation strategy for the rover, which relied on the correct on-board on-line robot localization and a sparse map building in order to estimate each wheel contact point in the terrain. The rover can then articulate its limb joints in order to actively conform to the terrain while traversing rough areas. Ground-truth localization data for direct comparison were available only for orientation thanks to an on-board IMU; nevertheless, the following results describe the successful overall performance of the approach in both position and orientation. An example terrain point cloud generated from consecutive frames is shown in Figure 3.9, for which accurate localization is first needed in order to perform proper registration of the 3D point clouds. 50 Robust Visual Odometry for UAVs Figure 3.8: MAMMOTH rover with an RGB-D sensor on top. Figure 3.9: Map generated after driving the MAMMOTH rover over a section of the Mars Yard. To validate the actively articulated suspension technique based on the RGB-D sensor based localization described before, the MAMMOTH rover is driven across a step obstacle and instructed to maintain a constant orientation and linear velocity while driving forward (along the x -axis relative to the world frame). The terrain that the MAMMOTH rover traverses is at the Sydney Powerhouse Museum’s Mars Yard, 3.1 Registration 51 Figure 3.10: Sequence of images from the trial. a 7x17 m space designed to resemble a section of the Columbia Hills region visited by the “Spirit” Mars Exploration Rover from the National Aeronautics and Space Administration (NASA). The MAMMOTH rover moves over a bed of rocks that surrounds a 0 . 15m tall cinder block, shown in Figure 3.10. In all trials, the rover initially drives over a 4m section of relatively flat terrain so as to allow each of its wheels to be within the mapped terrain region. Once all wheels are within this region, the active articulation controller is activated by an operator. The desired rates to be kept for each trial are zero translational velocity along the y and z axes, and zero rotational velocity for roll, pitch and yaw angles. In the trial shown in the plots, the commanded speed of the rover is vx = 0 . 05 m/s . The world frame is defined at the base of the rover at the beginning of its traverse. Initially, the x and y positions of the world frame are coincident with body frame’s x and y positions. The z position of the world frame is at the average initial height of each of the wheel contact frames relative to the body frame. 58 Robust Visual Odometry for UAVs i.e. the estimation of a camera pose (6 DoF: rotation and translation) with respect to a coordinate frame in which 3D points and their 2D projections in the camera are provided, given the camera calibration parameters. The 3D points are the coordinates of the key-points from the previous frame, while the 2D projections are their matched correspondences in the current frame. Hence the resulting pose provides an estimation of the camera motion between both frames. Given a set of n 3D points in a world reference frame and their corresponding 2D image projections, as well as the calibrated intrinsic camera parameters, the pose of the camera with respect to the world frame is calculated as follows. The perspective model for the camera is: s pc=K[R|t]pw(3.2) where pw = [ x y z 1] T is the homogeneous world point, pc = [ u v 1] T is the corresponding homogeneous image point, K is the matrix of intrinsic camera parameters (which are fx and fy for the scaled focal lengths, γ for the skew and ( u0, v0 ) is the principal point), s is a scale factor for the image point, and R and t are the desired 3D rotation and 3D translation of the camera (extrinsic parameters) that are to be estimated. This leads to the following equation for the model: s    u v 1     =    fxγ u0 0fyv0 0 0 1        r11 r12 r13 t1 r21 r22 r23 t2 r31 r32 r33 t3           x y z 1       (3.3) The assumption made in most solutions is that the camera is already calibrated. For each solution to P n P, the chosen point correspondences cannot be coplanar. In addition, P n P can have multiple solutions, and choosing a particular solution would require post-processing of the solution set. Furthermore, using more point correspondences can reduce the impact of noisy data when solving the problem. A commonly used solution to the P n P problem exists for n = 3, which is called Perspective-Three-Point (P3P) Gao et al. (2003). However, with only 3 correspondences, P3P yields many solutions, so a fourth correspondence is used in practice 3.2 Visual-Inertial Odometry 59 to remove ambiguity. Let P be the center of projection for the camera, A , B and C the 3D world points with corresponding image points u , v and w . Let X = |PA| , Y = |PB| , Z = |PC| , α = ∠BPC , β = ∠APC , γ = ∠APB , p = 2 cosα , q = 2 cosβ , r = 2 cosγ , a0 = |AB| , b0 = |BC| and c0 = |AC| . This forms triangles PBC , PAC and PAB from which we obtain the equation system for P3P: Y2+Z2−Y Zp −b02= 0 (3.4) Z2+X2−XZq −c02= 0 (3.5) X2+Y2−XY p −a02= 0 (3.6) It is common to normalize the image points before solving P3P. Solving the P3P system results in four possible solutions for R and T . The aforementioned fourth world point D and its corresponding image point z are then used to find the best solution among the four. As previously mentioned, using more point correspondences helps to reduce the impact of noisy data, which will generally be our case when using 3D imaging sensors on-board UAVs. In particular, the Efficient P n P (EP n P) algorithm Lepetit et al. (2008) has been used in our approach, which provides an efficient implementation for solving the P n P problem for n≥ 3. This method is based on the notion that each of the n points (which are called reference points) can be expressed as a weighted sum of four virtual control points. Thus, the coordinates of these control points become the unknowns of the problem. It is from these control points that the final pose of the camera is solved. As an overview of the process, first note that each of the n reference points in the world frame, pw i , and their corresponding image points, pc i , are weighted sums of the four controls points, cw j and cc j ( j = 1 ... 4) respectively, and the weights are normalized 60 Robust Visual Odometry for UAVs per reference point as shown below. All points are expressed in homogeneous form. pw i= 4 X j=1 αijcw j(3.7) pc i= 4 X j=1 αijcc j(3.8) 4 X j=1 αij = 1 (3.9) From this, the derivation of the image reference points becomes sipc i=K 4 X j=1 αijcc i(3.10) The homogeneous image control point has the form cc j = hxc jyc jzc jiT . Rearranging the image reference point equation yields the following two linear equations for each reference point: 4 X j=1 αijfxxc j+αij(u0−ui)zc j= 0 (3.11) 4 X j=1 αijfyyc j+αij(v0−vi)zc j= 0 (3.12) Using these two equations for each of the n reference points, the system Mx = 0 can be formed where x = hccT 1ccT 2ccT 3ccT 4iT . The solution for the control points exists in the null space of Mand is expressed as x= N X i=1 βivi(3.13) where N is the number of null singular values in M and each vi is the corresponding right singular vector of M . N can range from 1 to 4. After calculating the initial 3.2 Visual-Inertial Odometry 61 coefficients βi , the Gauss-Newton algorithm is used to refine them. The R and T matrices that minimize the reprojection error of the world reference points, pw i , and their corresponding actual image points pc i, are then calculated. This solution has O ( n ) complexity and works in the general case of P n P for both planar and non-planar control points. 3.2.4 Attitude Correction The P n P problem estimates a 6 DoF motion between frames, including translation and rotation. Since the UAV counts with an on-board IMU, our odometry system is able to read its data and integrate the roll and pitch angles with the purely visual estimation before and after the P n P algorithm. They are first introduced in the algorithm to provide an initial estimation for the rotation that took place between the frames under comparison. Afterwards, they are used again to compensate for the final angles that the algorithm outputs. 3.2.5 Key-Framing In order to partially mitigate the effect of cumulative errors usually found in odometry approaches, a key-framing approach has been adopted. Thus, new key-frames are produced when the feature tracking flow exceeds a given threshold. This is a measure of how much the image shifted since the last creation of a key-frame, by comparing the pixel positions of the key-points. At that moment, the current image key-points descriptors, their 3D estimated position and the robot position/orientation are stored. Subsequent frames will compute the transformation with respect to the last key-frame (and its pose) instead of the immediately preceding image, so that errors are only accumulated with the introduction of new key-frames, instead of with each frame. 62 Robust Visual Odometry for UAVs Figure 3.15: Bundle adjustment projection example. 3.2.6 Sparse Bundle Adjustment In general, Bundle Adjustment (BA) is the problem of refining a visual reconstruction, and is almost invariably used as the last step of every feature-based multiple-view reconstruction vision algorithm to obtain jointly optimal 3D structure and viewing parameter (camera pose and/or calibration) estimates. It is optimal because the parameter estimates are found by minimizing a cost function that quantifies the model fitting error, and it is jointly optimal because the solution is simultaneously optimal with respect to both structure and camera variations Triggs et al. (1999). Its name refers to the bundles of light rays originating from each 3D feature and converging on each camera’s optical center, which are adjusted optimally with respect to both the structure and viewing parameters. Figure 3.15 shows an example using three views. BA boils down to minimizing the reprojection error between the image locations of observed and predicted image points, which is expressed as the sum of squares of a large number of nonlinear, real-valued functions. Thus, the minimization is achieved using nonlinear least-squares algorithms. Of these, Levenberg–Marquardt (LM) has proven to be one of the most successful due to its ease of implementation and its use 3.2 Visual-Inertial Odometry 63 of an effective damping strategy that lends it the ability to converge quickly from a wide range of initial guesses. BA amounts to jointly refining a set of initial camera and structure parameter estimates for finding the set of parameters that most accurately predict the locations of the observed points in the set of available images. More formally, assume that N 3D points Xi are seen in M views Pj , and let xij be the projection of the i -th point on image j . Let vij denote the binary variables that equal 1 if point i is visible in image j , and 0 otherwise. Assume also that each camera Pj is parameterized by a vector aj , and each 3D point Xi by a vector bi . Bundle adjustment minimizes the total reprojection error with respect to all 3D point and camera parameters, specifically min aj,bi N X i=1 M X j=1 vij d(Q(aj,bi),xij)2(3.14) where Q ( aj,bi ) is the predicted projection of point i on image j , and d ( x,y ) denotes the Euclidean distance between the image points represented by vectors x and y . Clearly, BA is by definition tolerant to missing image projections and minimizes a physically meaningful criterion. However, due to the large number of unknowns contributing to the minimized reprojection error, a general purpose implementation of the LM algorithm (such as MINPACK’s lmder Mor´e et al. (1980)) incurs high computational costs when applied to the minimization problem defined in the context of BA. Fortunately, the lack of interaction among parameters for different 3D points and cameras results in the underlying normal equations exhibiting a sparse block structure. Our approach makes use of an efficient solution called Sparse Bundle Adjustment (SBA) Lourakis and Argyros (2009), which exploits this sparseness by employing a tailored sparse variant of the LM algorithm that leads to considerable computational gains. SBA is generic in the sense that it grants the user full control over the definition of the parameters describing cameras and 3D structure. Therefore, it can support virtually any manifestation or parameterization of the multiple view reconstruction problem, such as arbitrary projective cameras, partially or fully intrinsically calibrated 64 Robust Visual Odometry for UAVs cameras, exterior orientation (i.e. pose) estimation from fixed 3D points, refinement of intrinsic parameters, etc. In our case, let us consider a series of key-frames, each of them with their associated camera poses and key-points with 3D coordinates and 2D image projections. The idea is to improve the computation of the camera motion between key-frames according to the sets of observations. SBA is able to optimize not only the motion estimations, but also reduce the errors that were introduced into the 3D coordinates of the key-points. Our approach only considers the last K produced key-frames ( K =10 in the experiments), and applies two subsequent methods in order to allow on-line computation of the pose refinement: • It only considers the key-points that are seen and matched across at least K/ 2 consecutive key-frames. • It applies a bucketing technique to the surviving key-points, keeping the best 3 key-points from 28 image buckets (four rows and seven columns). 3.2.7 Ground Plane Estimation Another feature that was considered in this work is the estimation of the ground plane, given the particularities of the sensor arrangement and the testing environment. The 3D imaging sensor is mounted slightly pointing downwards, hence the point cloud can easily be used to estimate the flight height of the UAV, as an altimeter does, assuming that enough ground scene is within the field of view of the camera. The point cloud is first downsampled in order to keep the computational efficiency of the whole pipeline. A simple plane segmentation has been performed, i.e. find all the points within the point cloud that support a plane model. The detected planes are filtered by their normal vector, since we are looking for horizontal planes up to a maximum normal angle (10 ◦ in our approach). The estimated height of the UAV is an input to the pose correction regarding Z , which is averaged with the purely visual height using a configurable weight factor (0.5 in our experiments). 3.2 Visual-Inertial Odometry 65 Figure 3.16: CATEC testbed with a mockup scenario. The final camera pose estimation is transformed from the sensor frame to the aerial robot body frame, and used to calculate the new relative position and orientation estimations with respect to the initial pose. 3.2.8 Experimental Results In order to validate our approach using the odometry approach described in this section, a UAV has performed a flight in CATEC’s indoor testbed within a scenario recreating an industrial facility, as shown in Figure 3.16. The visual odometry has been used as the localization estimation input in order to close the control loop, and the UAV was teleoperated through a joystick to command small increments in position ( x , y or z ) and orientation (only yaw ). The main objective of the experiment was to demonstrate the suitability and accuracy of the approach in real-time. This section provides experimental results from the estimated localization compared with ground-truth data obtained from the testbed tracking system. The UAV used to demonstrate our approach is shown in Figure 3.17. The main sensor on-board the platform is the RGB-D camera Orbbec Astra, which is facing forward and slightly tilted down (25 ◦ ). The trajectory followed by the UAV according to ground-truth data is shown in Figure 3.18. 66 Robust Visual Odometry for UAVs Figure 3.17: The UAV with the RGB-D sensor at the front. x (m) -4 -3.5 -3 -2.5 -2 -1.5 -1 y (m) -1.5 -1 -0.5 0 0.5 1 1.5 2 2.5 Figure 3.18: Ground-truth UAV trajectory in XY during the experiment. The estimated UAV localization provided by the visual odometry algorithm during the experiment can be seen in Figure 3.19. The estimated position and yaw angle 3.2 Visual-Inertial Odometry 67 time (s) 0 20 40 60 80 100 120 140 160 x (m) -2 0 2Ground Truth Odometry time (s) 0 20 40 60 80 100 120 140 160 y (m) -2 0 2 time (s) 0 20 40 60 80 100 120 140 160 z (m) 0 1 2 time (s) 0 20 40 60 80 100 120 140 160 yaw (rad) -1 0 1 Figure 3.19: Estimated UAV position and orientation. during the experiment are shown: the red dashed line corresponds to the UAV groundtruth captured by the motion capture system, while the green dotted line is the estimated UAV position and orientation using the proposed visual odometry. Roll and pitch angles were acquired directly from the on-board IMU and hence are not shown in the plots, since they are directly integrated into the algorithm. As expected, the odometry exhibits some drift, though it provides a smooth estimation which tremendously helps the control algorithms achieve a stable performance. Figure 3.19 also shows how the odometry is consistent throughout the whole flight trajectory, taking into account the relatively adverse conditions of the flight within the indoor testbed for the algorithm, since there are almost no visual features in the floor or the mockup scenario, and sometimes the nearest objects are more than five 74 Multi-Modal Sensor Fusion for Long-Term Localization does not provide enough information to constitute a full localization system, since the data provided does not include bearing information. By fusing both sensing modalities, the reliable short-term position estimation based on odometry is combined with distances to fixed UWB sensors to correct its cumulative drift. Both benefit from each other when integrated in order to obtain fast and error-bounded localization estimations. Furthermore, the high efficiency of the implemented algorithms makes them suitable for real-time localization in the usually constrained equipment that can be mounted on-board an aerial vehicle. This particular combination is the main strength of the approach, in which two different sensor fusion strategies have been developed and tested using UAVs; they are explained in detail later in this chapter. First, an overview of how the aerial robot state estimation is handled from a probabilistic point of view is presented. 4.1 State Estimation Generally speaking, the interaction of a robot and its environment can be modeled as a dynamic system, in which the robot can manipulate its environment by choosing controls, and in which it can perceive its environment through sensor measurements. In probabilistic robotics, the dynamics of the robot and its environment are characterized in the form of two probabilistic laws: • the state transition distribution, which characterizes how state changes over time, normally as the effect of a robot control (e.g. a movement), and • the measurement distribution, which characterizes how measurements are governed by states. Both laws are probabilistic, accounting for the inherent uncertainty in state evolution and sensing. The estimation of the state of a mobile robot is usually achieved by the Bayes filter Thrun et al. (2005). The localization problem consists in finding the robot state at time t , xt , given the last measurement zt and a prior state xt−1 . The Markov 4.1 State Estimation 75 assumption holds, i.e. a state is a complete summary of the past. Hence the Bayes filter is recursive, since xtis calculated from xt−1. The Bayes filter algorithm possesses two essential steps. It first calculates a belief over the state xt based on the prior belief over state xt−1 and a control input ut . This is usually called the prediction step. The second step is called the update, in which the algorithm multiplies the belief xt by the probability that the measurement zt may have been observed. The result is normalized since the resulting product is generally not a probability. In order to compute xt recursively, the algorithm requires an initial belief at time t = 0. If one knows the value of x0 with certainty, it should be initialized with a point mass distribution that centers all probability mass on the correct value, assigning zero probability anywhere else. If one is entirely ignorant about the initial value x0 , it may be initialized using a uniform distribution over the domain of x0. Bayesian filters are implemented in several different ways. Each of them relies on different assumptions regarding the state transition, measurement probabilities and the initial belief. In general, exact techniques for state calculation are not available and hence the state has to be approximated. Finding a suitable approximation is usually a challenging problem. When choosing an implementation, several properties have to be considered. • Computational efficiency: some approximations allow computing state beliefs in time polynomial in the dimension of the state space, for example linear Gaussian approximations. Particle-based techniques have an any-time characteristic, enabling them to trade-off accuracy with computational efficiency. • Accuracy of the approximation: some techniques can approximate a wider range of distributions more tightly than others. For example, linear Gaussian approximations are limited to unimodal distributions. Particle representations can approximate a wider range of distributions, but the number of particles needed to attain the desired accuracy can be large. • Ease of implementation: it depends on a variety of factors, such as the form of the measurement probability and the state transition probability. Particle 76 Multi-Modal Sensor Fusion for Long-Term Localization representations often yield surprisingly simple implementations for complex non-linear systems, which is one of the reasons for their recent popularity. The next two sections describe the two techniques that have been used in the context of this dissertation, which represent examples of probably the two most popular families of recursive state estimation techniques, both derived from the Bayes filter: Gaussian filters and particle filters. 4.2 Gaussian Filters for State Estimation Gaussian techniques share the basic idea that beliefs of the robot state are represented by multi-variate normal distributions. The density over the variable x is characterized by two sets of parameters: • The mean µ , which is a vector that possesses the same dimensionality as the state x. • The covariance Σ, which is a quadratic matrix that is symmetric and positivesemidefinite. Its dimension is the dimensionality of the state x squared (i.e. the number of elements depends quadratically on the number of elements in the state vector). Representing the belief by a Gaussian distribution has important implications. Most importantly, Gaussians are unimodal, that is, they possess a single maximum. This is characteristic of many tracking problems in robotics, in which the belief of the state is focused around the true state with a small margin of uncertainty. However, they are a poor match for many global estimation problems in which many distinct hypotheses exist. Probably the best studied Gaussian technique for implementing Bayes filters is the Kalman filter (KF) Kalman et al. (1960), which has been widely used for filtering and prediction in linear systems. At time t , the belief is represented by the mean µt and the covariance Σ t . Apart from the Markov assumption, the following three properties must hold: 4.2 Gaussian Filters for State Estimation 77 • The state transition probability p ( xt|ut,xt−1 ) must be a linear function in its arguments with added Gaussian noise: xt=Atxt−1+Btut+t(4.1) • The measurement probability p ( zt|xt ) must also be linear in its arguments with added Gaussian noise: zt=Ctxt+δt(4.2) •The initial belief in t= 0 must be normal distributed. The assumptions of linear state transitions and linear measurements with added Gaussian noise are rarely fulfilled in practice. For example, a robot that moves with constant translational and rotational velocity typically moves on a circular trajectory, which cannot be described by linear state transitions. This observation, along with the assumption of unimodal beliefs, renders plain Kalman filters inapplicable to all but the most trivial robotic problems. The extended Kalman filter (EKF) overcomes one of these assumptions: linearity. Here the assumption is that the state transition and the measurement probabilities are governed by non-linear functions: xt=g(xt−1, ut) + t(4.3) zt=h(xt) + δt(4.4) A Gaussian projected through the non-linear state transition function g is typically non-Gaussian. Linearization approximates g by a linear function that is tangent to g at the mean of the Gaussian. By projecting the Gaussian through this linear approximation, the result is also a Gaussian. The same applies to the multiplication of Gaussians when the measurement function h is involved. Tangents are linear, making the filter applicable. There exist many techniques for linearizing non-linear functions. EKFs utilize Taylor expansion, which involves calculating the first derivative of the 78 Multi-Modal Sensor Fusion for Long-Term Localization target function, and evaluating it at a specific point. The result of this operation is a matrix known as the Jacobian. The EKF is one of the most popular tools for state estimation in robotics Mao et al. (2007); Purvis et al. (2008); Rullan-Lara et al. (2011). Its strength lies in its simplicity and computational efficiency. The EKF owes its computational efficiency to the fact that it represents the belief by a multi-variate Gaussian distribution. A Gaussian is a unimodal distribution, which can be thought of as a single guess, annotated with an uncertainty ellipse. In many practical problems, Gaussians are robust estimators. An important limitation of the EKF arises from the fact that it approximates the state transition and measurement functions using linear Taylor expansions. In virtually all robotic problems, these functions are non-linear. The accuracy of this approximation depends on two main factors: the degree of non-linearity of the functions that are being approximated, and the degree of uncertainty which is directly related to the width of the probability distribution (the larger the uncertainty, the higher the error introduced by the linearization). In our incremental approach for achieving robust long-term localization of UAVs, an EKF has been first used to fuse the information provided by the visual odometry algorithm and UWB-based distance measurements. Although localization based on visual odometry will eventually diverge due to cumulative errors, it can be used as a short-time predictor of the UAV motion (prediction). The UWB sensor readings can then constrain the odometry drift (update). Thus, using the odometry as motion prior helps filtering UWB noise and detecting/removing measurement outliers. At the same time, the UWB-based update helps to remove the cumulative errors in the odometry, building an accurate and stable localization system both together. Figure 4.1 shows an overview of the method. The UWB sensor on-board the UAV computes the distance to all the sensors at a lower operating frequency than that of the visual odometry system, and integrates the information into an EKF described below. As the robot moves, the UWB sensor periodically sends out a query, and any other sensor within range responds by sending a reply. Since each sensor transmits a unique ID number, distance readings are automatically associated with the appropriate tags, so the data association problem is 4.2 Gaussian Filters for State Estimation 79 Figure 4.1: Schematic overview of the EKF approach. trivially solved. Another key advantage is that the robot can estimate the distance to each responding sensor even when they are not within line of sight. This is very useful in situations where visual-based methods usually fail, such as poorly illuminated scenes or highly dynamic environments. The range measurements and odometry priors are all integrated into a simple EKF that allows estimating the robot’s position and velocity with the following state vector: x="p v#=             x y z vx vy vz             (4.5) where p and v refer to the robot position and velocity respectively. Orientation data is not considered in the EKF since, as stated before, the UWB-based measurements used in the update step do not provide bearing information. Hence, visual odometry is the only source of information for estimating the aerial robot orientation. As a reminder, roll and pitch angles are integrated directly from the on-board IMU, while yaw is estimated purely from visual information. 80 Multi-Modal Sensor Fusion for Long-Term Localization 4.2.1 Prediction The visual odometry system provides the estimated velocity of the robot periodically, vo = [ vox, voy, voz ] t . This information is used in the prediction stage of the EKF to constrain the robot localization according to the following expression: pt=pt−1+ ∆t·vt−1(4.6) vt=vo (4.7) Eventually, the odometry system might not be able to provide an estimation, for instance, due to the lack of texture in the scene. In such a case, the state is predicted based on random walk. 4.2.2 Update Given the distance di measured from the robot to a beacon with known position bi , the following constraint can be applied to the robot position: di=||pt−bi|| (4.8) This constraint can be easily integrated into the update stage of the EKF, following the equation: zt=h(x(t)) (4.9) where zt=diand h(x(t)) = ||pt−bi||. This filter update is applied to all the UWB measurements received within the last period of time according to the filter operating frequency (50ms in our experiments). If more than one measurement to a specific beacon are available, the distance is averaged. Note that the filter does not need distances to several beacons in order to perform a filter update; the distance to a single sensor is enough. This is possible thanks to the state prior provided by the prediction stage, which is expected to be accurate and stable at short-term. This is a great advantage with respect to state-of-the-art 4.2 Gaussian Filters for State Estimation 81 UWB-based localization systems, which usually wait until the distances to three or more sensors are measured. 4.2.3 Outlier Rejection Distance measurements based on radio sensors are subject to frequent outliers when performed indoors. These outliers are caused by multi-path and signal reflection in walls and structures, and introduce significant errors in the localization. The odometry prior (EKF prediction) provides a short-term accurate and reliable estimation of the robot’s position, so the EKF measurement residual yt = zt−h ( xt ) can be used to decide whether a range measurement is an outlier or not. Thus, if the residual of a range measurement is above a threshold rth , the measurement is determined as an outlier with high probability and the filter will reject the data. This threshold has been set to rth = 2min our experiments. 4.2.4 Experimental Results In order to validate the aforementioned approach, which makes use of visual odometry and UWB-based measurements, an aerial robot has performed a relatively long flight (around 7 minutes). The flight has taken place in an indoor controlled scenario where several UWB devices (six sensors) have been installed. The main objective of the experiment is to demonstrate the suitability of the approach during regular operations. The experiments took place at the indoor testbed of CATEC, whose motion capture system provided ground-truth localization data. Prior to the discussion of the experimental results, the setup describing all the elements involved in the experiment is presented: • Six UWB sensors used as beacons, installed across the indoor testbed at different locations, homogeneously distributed also at different heights. These locations are previously known by the system. 82 Multi-Modal Sensor Fusion for Long-Term Localization Figure 4.2: UAV with the VI-sensor at the front (left) and ground-truth trajectory in the XY plane (right). • An aerial robot with the following on-board sensors: a VI-sensor (stereo camera), an IMU and a UWB sensor in order to compute point-to-point distances (see Figure 4.2). The aerial robot was manually piloted through the testbed area at different heights. The ground-truth trajectory followed by the vehicle is also presented in Figure 4.2. This is a 75 meters long trajectory in which the robot was piloted within the indoor testbed, flying at different altitudes. The estimated UAV position during the experiment can be seen in Figure 4.3, which includes the estimated fused localization (blue solid line) together with the raw visual odometry output (green dotted line), both compared to the ground-truth position (red dashed line). As expected, the odometry system slowly diverges through time, while the proposed method integrating radio-based measurements follows the ground-truth with small deviations. Only position results are analyzed since radio-based sensing does not provide orientation measurements. It can be also seen in Figure 4.3 how the odometry is consistent most of the time, taking into account the relatively adverse conditions of the flight within the indoor testbed: low texture (there are almost no visual features in the floor) and high distance to 3D scenes (sometimes up to ten meters far ahead). 4.2 Gaussian Filters for State Estimation 83 Figure 4.3: Estimated UAV localization (ground-truth in red, visual odometry in green, proposed approach in blue). Figure 4.4 shows the estimation error of the proposed approach in X, Y and Z separately. The RMS error for each localization component is also presented. It can be seen how the RMS error is around 0.25m in Y and Z, and 0.4m for X. As it was previously observed, the flight conditions are far from ideal regarding the visual odometry approach. The scenario floor exhibits a very homogeneous scene, and the introduced objects to recreate a 3D environment are sometimes very far from the aerial vehicle (up to ten meters). Hence, disparity computation results in higher errors that lead to possible wrong matches across frames. Nevertheless, the results presented in this section show that these issues, when combined with radio-based measurements, can be overcome resulting in a more robust solution, suitable for long-term operation of autonomous vehicles. 90 Multi-Modal Sensor Fusion for Long-Term Localization 4.3.3 Update In this phase, we use a measurement model to incorporate information from the sensors. We have defined thresholds in position and orientation such that if the visual odometry estimation exceeds any of those, a filter update is performed using the last 3D point cloud received from the stereo/RGB-D camera, and all the distance measurements to the UWB beacons received since the last filter update. Then, each particle x[i] t evaluates its relative importance by checking how likely it would receive such sensor readings at its current pose, hence computing a new weight value w[i] t . Then, particles can later be re-sampled considering these weights, thus obtaining a new estimate of the current state given the last measurement zt. In this work, zt corresponds to the point clouds from the 3D imaging sensor and the distance measurements to the UWB sensors. The update is performed through the use of a 3D map of the environment in the form of an OctoMap Hornung et al. (2013), augmented with the position of the fixed locations of UWB beacons. Given the distinct nature of the two technologies involved, we calculate separate weights for each sensing modality, i.e. w[i],map t and w[i],uwb t . A simple weighted average is later used to obtain the final weight of each particle: w[i] t=α∗w[i],map t+ (1 −α)∗w[i],uwb t(4.18) where α is chosen depending on the particularities of the indoor environment where the UAV is going to operate. If the map used in the filter does not contain the full environment, or its accuracy is not enough to trust the map matching, α should be lower than 0 . 5. Whereas if there are few UWB sensors deployed in the scenario, or their location is not accurate, αshould be higher. Computation of w[i],map t The acquired 3D point cloud is transformed to each particle’s pose in order to find correspondences between such cloud and what the map should look like from that particle’s pose. Since this is very expensive computationally, we first compute a 3D probability grid as in Hornung et al. (2010), in which each position stores a value of 4.3 Particle Filters for State Estimation 91 how likely it is that such position falls within an occupied point of the map, instead of storing binary information about occupancy as in the provided map. Each 3D position pi of the grid is then filled with probability values according to a specific Gaussian distribution centered in the closest occupied point in the map from pi , mapi , and whose variance σ2depends on the sensor noise used in the approach. grid(pi) = 1 √2πσ2e−||pi−mapi||2/2σ2(4.19) Such probability grid only needs to be computed once, is not required to be updated for a given environment, and relieves from performing numerous distance computations between each cloud point for each particle and its closest occupied point in the map. Besides, each point cloud is first transformed according to the current roll and pitch provided by the on-board IMU. This transformation is done just once per update, reducing the computational requirements as well. Then, for every point of the transformed cloud, we access its corresponding value in the 3D probability grid. Such value would be an indicator of how likely is that point to be part of the map. By doing this with every point of the cloud and adding all the probability values, we obtain a figure of how well that particle fits the true location of the aerial robot according to the map. Finally, the weight wi of each particle pi is computed. Assuming that the point cloud is composed by M 3D points cj , the weight is computed by adding all the associated probability grid values as follows: wmap i=1 M M X j=1 grid(pi(cj)) (4.20) where pi ( cj ) stands for the transformation of the point to the particle’s state, and grid(pi(cj)) is the evaluation of the probability grid in such transformed position. Computation of w[i],uwb t On the other hand, distance measurements between UWB sensors are used to compute another weight value for each particle according to how well their state fits to the 92 Multi-Modal Sensor Fusion for Long-Term Localization distribution of fixed radio beacons. Since the radio beacons do not provide bearing information, we first define new states without ψ (yaw angle), i.e. x0[i] t = [ x, y, z ] T , and UWB beacon states using their estimated positions, bj = [ xj, yj, zj ] T . Given a measured distance dj from the UWB sensor on-board the UAV to the j -th beacon, the following constraint can be applied to each particle state: dj=||x0[i] t−bj|| (4.21) This can be easily applied to the particle weight calculation according to the actual Euclidean distance rij between the state x0[i] t of the i -th particle and bj , and how close this is to dj . The product is used to aggregate the values from the measurements of different beacons, since they are independent probabilistic processes. The weight of the i-th particle associated to UWB sensing is calculated as follows: w[i],uwb t= B Y j=1 1 σ√2πe−(dj−rij )2/2σ2(4.22) where B is the number of beacons in the scenario. The distance measurements of the sensors used in our approach have a standard deviation σ of roughly 0 . 1m, after filtering potential outliers. A great advantage of this approach is that the distance to a single sensor is enough to update the weights of the particles, it does not need to wait until distances to three or more sensors are obtained by the on-board radio-tag, which usually happens in other state-of-the-art range-based localization systems. Weight combination Before combining the weights of both sensing approaches, all the weights must first be normalized within their categories in order to verify Equation 4.12, hence representing a valid probability distribution. This also prevents combining weights of distinct nature which usually are in different orders of magnitude. Equation 4.18 is used to obtain the final weight of each particle, and the new weights are normalized again in order to represent a valid probability distribution. 4.3 Particle Filters for State Estimation 93 We have also evaluated the option of drawing new particles into the filter with each new radio-based measurement, taking into account the lack of bearing information. Thus, with each distance measurement, a set of particles would be drawn in a sphere surrounding the on-board sensor position into the map. Most of these particles will be later on destroyed in the next re-sampling after update thanks to the weight contribution of the point cloud matching with the 3D map. However, this approach was discarded because it requires a large number of particles for a proper representation of the range hypotheses, which hinders the computational efficiency. 4.3.4 Re-sampling The next step of the method in order to complete one iteration of the algorithm involves the generation of a new set of particles from the current one, by choosing each particle x[i] t according to its weight w[i] t . Hence, the new poses of the particles will be more likely to be accurate. The algorithm employed for re-sampling is the low variance sampler Thrun et al. (2005). In this case, a single random number is used as a starting point to obtain Nparticles according to their relative weights. 4.3.5 Pose Computation The particle filter method is an iterative process that involves moving, sensing and re-sampling multiple localization hypotheses. While the filter produces a set of localization hypotheses, the navigation needs to decide which hypothesis is the correct one. The updated state vector for the aerial robot is then calculated as the weighted sum of all the particles, since when the filter converges to a single hypothesis, that is a good estimation of the true position of the robot. In this case, all particles are found to be clustered around a specific region. Due to the mixed sensing approach, the outliers commonly present in indoor UWB measurements are rejected thanks to the particle weighting. If an outlier in the distance measurement from a radio-based sensor is received, following Equation 4.22, this would result in very low values for w[i],uwb t ; in this case the re-sampling would only depend on the associated weights calculated from the point cloud matching, and 94 Multi-Modal Sensor Fusion for Long-Term Localization the relative scoring among the set of particles would remain unaffected. On the other hand, if the map matching between using the 3D point clouds is not accurate, its associated weight would not influence the overall particle’s weight, which will only depend on the contribution from the UWB side. 4.3.6 Experimental Results An experimental setup has been conceived to validate the presented approach. The field test took place at an indoor testbed of CATEC; part of it can be seen in Figure 4.6. The aerial robot in Figure 4.7 performed a flight based on the testbed’s motion capture system for localization, and navigated using joystick commands that sent nearby position and orientation waypoints in the robot coordinate frame. The results presented in this section were obtained off-line. In order to validate the long-term character of our approach, the flight took roughly 9 minutes so the visual odometry could drift enough to verify that the two sensing measurements used in the particle filter help preventing localization error growth. The complete experimental setup is composed of the following elements: • An aerial robot with the following on-board sensors: an RGB-D sensor (Orbbec’s Astra), an IMU and a UWB sensor (see Figure 4.7). • A 3D map of the working area (see Figure 4.6) obtained using an RGB-D sensor (ASUS’s Xtion PRO LIVE) and motion capture data. •Three UWB beacons installed across the indoor testbed at known positions. The 3D map of the area was acquired using a different RGB-D sensor from the one on-board the UAV, whose housing was augmented with several passive markers (see Figure 4.8) in order to get its pose with high accuracy from the testbed’s motion capture system. Such sensor was manually carried around the testbed while connected to a laptop, and the acquired point clouds were projected using the corresponding sensor pose and merged into an accurate OcTree. This map can be seen in Figure 4.6. 4.3 Particle Filters for State Estimation 95 Figure 4.6: CATEC indoor testbed (top) and 3D map with approximate radio beacons locations(bottom) used for field experiments. A small infrastructure of UWB beacons (three sensors) has been installed in the indoor scenario where the flight has taken place. The radio-based sensors have been installed at three different locations homogeneously distributed within the indoor testbed. Their true locations were acquired using the motion capture system and 96 Multi-Modal Sensor Fusion for Long-Term Localization Figure 4.7: UAV with an RGB-D sensor at the front. Figure 4.8: Another RGB-D sensor with passive markers for precise 3D map building. provided to our filter. There is an additional UWB sensor installed on the aerial robot in order to compute point-to-point distances. The ground-truth trajectory followed by the vehicle is presented in Figure 4.9. This is a roughly 200 meters long trajectory in which the robot performed a trajectory for a whole battery duration. The main objective of the experiment is to demonstrate the suitability of the approach in real-time during regular operations. The estimated UAV position and yaw during the experiment can be seen in Figure 4.10, both from the raw visual odometry and the particle filter. Roll and pitch angles are not included in the plots because, as previously stated, they are observable from the on-board IMU and are directly 4.3 Particle Filters for State Estimation 97 x (m) -4 -3 -2 -1 0 1 2 3 4 5 6 y (m) -4 -3 -2 -1 0 1 2 3 4 x (m) -2 -1 0 1 2 3 4 5 z (m) -1 0 1 2 3 4 5 Figure 4.9: Ground-truth trajectory followed by the aerial robot during the experiment. integrated into our localization approach. The IMU used in our approach provides accurate and filtered estimations of such angles. These results (Figures 4.10 and 4.11) were obtained after configuring the particle filter with the parameters shown in Table 4.1. Table 4.1: MCL parameters Parameter Value Number of particles 500 α(Eq. 4.18) 0.5 OcTree resolution 0.1m RGB-D sensor σ0.05m UWB sensor σ0.1m Update threshold (pos) 0.1m Update threshold (rot) 0.1rad The plot also shows the ground-truth data provided by the testbed’s motion capture system. It can be seen how the visual odometry slowly accumulates errors over time, while the estimations from our complete approach closely follow the ground-truth during all the experiment. It is also worth to mention that this estimation does not drift with time and the errors are approximately bounded. 98 Multi-Modal Sensor Fusion for Long-Term Localization time (s) 0 50 100 150 200 250 300 350 400 450 500 x (m) -2 0 2 4 6 8 Ground Truth Odometry MCL time (s) 0 50 100 150 200 250 300 350 400 450 500 y (m) -5 0 5 time (s) 0 50 100 150 200 250 300 350 400 450 500 z (m) 0 2 4 time (s) 0 50 100 150 200 250 300 350 400 450 500 yaw (rad) -4 -2 0 2 4 Figure 4.10: Localization results (position and yaw) showing the ground-truth, visual odometry and the proposed approach. Figure 4.11 shows the error of the estimation during the trajectory execution. The error is computed as the Euclidean distance between the estimation and the groundtruth at every time step. In the figure, left plots correspond to the odometry approach, while right plots show results from the complete proposed approach including the filter. Besides, the plot depicts the computed RMS errors for each axis, which are also shown in Table 4.2. It can be seen how the RMS errors are approximately 0 . 4m in x and y , while in z is less than 0 . 2m thanks to the ground estimation module. RMS error in yaw angle is around 10 degrees. Errors in x and y are higher due to the larger distance traveled 4.3 Particle Filters for State Estimation 99 time (s) 0 100 200 300 400 500 x error (m) 0 5 Odometry Errors time (s) 0 100 200 300 400 500 y error (m) 0 5 time (s) 0 100 200 300 400 500 z error (m) 0 0.5 time (s) 0 100 200 300 400 500 yaw error (rad) 0 1 2 time (s) 0 100 200 300 400 500 x error (m) 0 0.5 1 1.5 MCL Errors time (s) 0 100 200 300 400 500 y error (m) 0 0.5 1 1.5 time (s) 0 100 200 300 400 500 z error (m) 0 0.5 time (s) 0 100 200 300 400 500 yaw error (rad) 0 0.5 1 Figure 4.11: Position and orientation errors with respect to ground-truth through time. through these axes. Nevertheless, the global RMS error in position is 0 . 32m which can be considered acceptable for robot navigation. Table 4.2: RMS localization errors x(m) y(m) z(m) yaw (rad) Odometry 2.47 2.53 0.19 0.88 MCL 0.36 0.39 0.17 0.18 In order to quantitatively assess the performance of our localization approach, we have tested the data from the validation experiment against other popular approaches 106 Multi-Modal Sensor Fusion for Long-Term Localization from our complete approach closely follow the ground-truth during all the experiment. It is also worth to mention that this estimation does not drift with time and the errors are approximately bounded. Even though good accuracy through the whole flight duration were achieved, the localization approach relied on a 3D map that was built using a motion capture system. In order to be able to extend the proposed system to a custom indoor scenario for autonomous UAV operation, we must complement this methodology with mapping capabilities relying also only on 3D imaging sensors and UWB sensing. This is explained in detail in Chapter 5. Chapter 5 Multi-Modal Mapping Prior to robot localization, a 3D map of the environment needs to be built in order to make use of the particle filter proposed in Chapter 4. Available sensor data to achieve this are the 3D point clouds from stereo or RGB-D cameras and the distance measurements between UWB sensors (one of them is on-board the UAV and the rest are fixed in the environment). Neither the UAV trajectory nor the position of the UWB sensors in the environment are known a priori. The objectives of this approach are to map the environment based on the 3D point clouds and to accurately localize the set of fixed UWB sensors, or beacons. Approaching these two tasks separately may seem logical. Firstly, the scene could be mapped based exclusively on the point clouds while at the same time the aerial robot trajectory is calculated. This is usually performed through the use of a SLAM approach based on 3D point clouds such as Endres et al. (2012); Labbe and Michaud (2014). Later, the trajectory information could be used to accurately locate the UWB beacons into the map, according to the corresponding distance measurements from each UAV pose to each beacon. However, this method does not take advantage of UWB sensing for mapping, neither localization. The method proposed in this chapter follows an integrated approach in which we take advantage of both sensing modalities. The position of the UWB beacons are firstly approximated, and later on refined together with the 3D map of the environment. There are two main steps in our approach. The first step considers data from the 107 108 Multi-Modal Mapping UWB sensors to compute a globally consistent 3D trajectory of the UAV, and then to automatically perform loop closing, which is the detection of a previously visited location by some means. Both the initially computed trajectory and the loop closures will be used in a second step to optimize the 3D position of the UAV, and also the positions of the UWB beacons. With this information, a 3D map can be built according to the estimated UAV trajectory and projecting the 3D point cloud at each location. Further details about the implementation of both steps are explained in the following sections. 5.1 Range-only localization and mapping (step 1) The objective of the first step of the algorithm is to compute an initial guess of the positions of the UWB beacons based on the measured distances to the sensor on-board the UAV. Then we can use this information to obtain a globally consistent trajectory of the aerial robot. This is generally referred to as a Range-Only Simultaneous Localization and Mapping (RO-SLAM) Kantor and Singh (2002); Newman and Leonard (2003). In general, RO-SLAM aims to localize a mobile system and at the same time map the position of a set of range sensors. In contrast with other SLAM approaches based on cameras or LIDAR, RO-SLAM has the advantage of not requiring direct line of sight between each pair of sensors when radio-based range sensing is used. Besides, the problem of data association is trivially solved; this could pose a significant obstacle for the algorithm, since range sensors typically provide distance measurements to some object without identifying such object. In the case of radio-based sensors, they usually transmit their unique ID along with the range information, as it is the case of the UWB sensors used in the proposed approach. Most of the approaches for RO-SLAM in the literature are based on time filtering and probabilistic frameworks such as EKF-SLAM, Unscented Kalman Filter (UKF)- SLAM, FastSLAM, and others. Comparisons of these frameworks can be found in Kurt-Yavuz and Yavuz (2012) and Li et al. (2012). Kurt-Yavuz and Yavuz (2012) present how the unscented FastSLAM exhibits better performance over other classical methods based on EKF or UKF. However, FastSLAM solutions do not preserve the 5.1 Range-only localization and mapping (step 1) 109 correlation between different beacons of the map in those applications in which it might exist. Thus, for example, in Blanco et al. (2008); Wang et al. (2009); Yang (2012), a FastSLAM solution is proposed using a particle filter for robot localization and another one for each beacon (i.e. for each radio emitter). In Yang (2012), an optimization on the beacons’ particle filter is proposed by reducing the number of particles, hence decreasing the computational burden of the filter. On the other hand, Wang et al. (2009) optimize the problem using an adaptive re-sampling method which dynamically reduces the number of particles required for each beacon. A different beacon-based SLAM algorithm is considered in Hai et al. (2010), where the authors use a particle filter to initialize the EKF filter for each new beacon of the FastSLAM. The main drawback of the previous approaches lies in the delayed initialization of the beacons into the filters, which significantly reduces the optimization of the robot localization until the position solution of the range sensors has converged. This problem is solved in Boots and Gordon (2013), where a batch solution is proposed to estimate the position of the mobile robot and the beacons by using a singular value decomposition (SVD) of the observation matrix. A batch processing solution is also presented in Kehagias et al. (2006) based on optimization. However, these methods assume measurements from all the radio beacons at every robot position; otherwise, the measurement must be interpolated. This is a hard constraint in realistic implementations where the visibility of all the range sensors cannot be guaranteed at all times. This section proposes a new optimization approach for the RO-SLAM problem that follows the batch processing ideas but generalizes to a more common situation in which the range measurements from the range sensors might arrive at the robot independently, or even a specific robot pose does not have associated range measurements. 5.1.1 Problem Definition The UAV trajectory is represented as a series of N robot poses X = {x1,x2, ..., xN} , where each pose is defined as xi = [ xi, yi, zi, ψi ] T , being ψ the yaw angle of the aerial robot. There is also a set of M UWB beacons B = {b1,b2, ..., bM} , each one located at 110 Multi-Modal Mapping a position defined by bj = [ xbj, ybj, zbj ] T . The objective of the RO-SLAM optimization problem is to compute the best UAV trajectory X and locations of UWB beacons B that minimizes the errors with respect to the observations of the UWB measurements from each UAV pose. Note how the robot roll and pitch angles are not included into the robot pose definition. We assume these angles are available and accurate enough in an UAV through the use of its on-board IMU. They are fully observable and their values are usually accurate in aerial robots because they are the most basic control variables (together with the rotation rates) for the system stability. While it is true that roll and pitch estimation based on accelerometer and gyroscope integrations might be biased under constant accelerations (e.g. loitering in fixed-wing UAVs), these scenarios are very difficult to achieve indoors and hence are not considered in this approach. The UWB-based range observations are the distances between the UAV position and each beacon position. Thus, the observations at pose xi are modeled as the set of measurements Di = {di1, di2, ..., diM } with arbitrary length from 0 (no measurements) to M (measurements to all UWB sensors), where dij is the Euclidean distance between pose xiand beacon position bj: dij =q(xi−xbj)2+ (yi−ybj)2+ (zi−zbj)2(5.1) Thus, the resulting aerial robot trajectory and UWB beacon positions will be that which minimizes the following expression: arg min {X,B}"N X i=1 M X j=1 cij (kxi−bjk−dij)#(5.2) where cij is a variable that takes value 1 if there is a measurement from pose xi to UWB beacon bj, and 0 otherwise. However, due to the nature of the problem addressed, there exists the possibility that a number of poses do not have associated measurements (the UAV is out of range of all UWB beacons) and, more frequently, that the number of range measurements in a pose is below four (minimum number to compute the robot position in 3D). 5.1 Range-only localization and mapping (step 1) 111 These limitations are overcome by including odometric constraints into Equation 5.2, obtaining the final expression to minimize: arg min {X,B}"N X i=1 Ei+ M X j=1 cij(kxi−bjk−dij )!# (5.3) where Ei stands for the error between pose xi and xi−1 according to odometry information. This function transforms the pose xi−1 according to the estimated robot odometry and computes the error with respect to xi in each pose dimension. We make use of the visual-inertial odometry algorithm presented in Chapter 3. 5.1.2 Optimization Solving Equation 5.3 is straight-forward if good guesses about the aerial robot poses and the positions of UWB beacons are available. However, RO-SLAM does not necessarily have prior information about the position of the beacons. An option would be to initialize B to random positions and let the optimization process estimate them, but the algorithm will most likely get stuck in a local minimum. In order to overcome this problem, this approach proposes two actions. First, the zb parameter of every UWB beacon in B could be estimated by just measuring their distances to the floor. This is an easy process that can be implemented accurately. This assumption allows reducing the number of unknown parameters of each UWB sensor position to two ( xb and yb ), but still, more information is needed in order to initialize each UWB beacon position for the optimization method. The second action consists in re-parameterizing the beacon position so that we can feed several estimation hypotheses into the optimizer, as proposed in Fabresse et al. (2013). Thus, when the robot is at pose xi and receives the first range measurement dij to sensor j , the beacon bj would be in a circumference around the current robot pose and at altitude zbj . The proposed approach samples this circumference with multiple position hypotheses at different angles, allowing the optimizer to choose the best one. Figure 5.1 shows an example including a UAV 3D trajectory seen from an orthogonal view for easy visualization (triangles are robot poses and circles are UWB 112 Multi-Modal Mapping Figure 5.1: Multiple hypotheses for the localization of three UWB beacons. beacon position hypotheses). Thus, assuming H different position hypotheses along the circumference, the UWB beacon position will be parameterized as follows: bj= [bj1,bj2, ..., bjHj] (5.4) where each position hypothesis is represented as bjk = [ xbjk , ybjk , zbj ] T . Note how zbj is the same for all hypotheses because it is already known. With this parameterization in mind, Equation 5.3 can be reformulated as follows: arg min {X,B}  N X i=1  Ei+ M X j=1 1 Hj Hj X k=1 cij (kxi−bjkk−dij)  (5.5) Note how the contribution of a single UWB sensor j is scaled by 1 /Hj for every pose xi , so that we do not double-count the information provided by a single range measurement. 5.1 Range-only localization and mapping (step 1) 113 5.1.3 Initialization In summary, the parameters of the optimization process are the position of the robot in each pose xi , which is initialized according to the odometry values, and the different position hypotheses for every UWB beacon. When a range measurement dij is received from the j -th beacon for the first time, the current robot position estimation xi is used to initialize the Hj position hypotheses according to the following equations: xbjk =xi+dij ·cos(2π(k−1)/Hj) (5.6) ybjk =yi+dij ·sin(2π(k−1)/Hj) (5.7) zbjk =bzj(5.8) The value of Hj is initialized to Hj = 10 ·dij , so that the number of hypotheses adapts to the sensor distance. Thus, the outcome of this first step is a globally consistent trajectory and an initial position estimation for the UWB beacons. If the robot motion was enough to let the optimizer disambiguate the horizontal flip ambiguity Fabresse et al. (2016), then the algorithm converges to a single solution and all the hypotheses will be localized in a specific position. 5.1.4 Weighting It is important to take into account that the different sources of information in Equation 5.5 have different levels of accuracy. It is very usual to make use of the constraint’s associated information matrix to fine tune the minimization process. In this way, very different sources of information such as visual odometry and UWB sensors can be easily considered. However, practical experience shows that the UWB measurements tend to incorporate outliers frequently due to multi-path effects or signal attenuation. This is the reason behind weighting the different components into Equation 5.5 differently. We establish a weighting factor assuming that the odometry is reliable in the short term, 114 Multi-Modal Mapping while UWB might provide outliers depending on the environment and no matter the time. Thus, in the proposed implementation, UWB constraints are multiplied by a factor ranging from 0 . 5 to 0 . 1 depending on the quality of the information, while odometry will be always weighted by a factor of 1 . 0. That is, we trust odometry constraints more than UWB at short-term. 5.2 3D Mapping and Pose Refinement (step 2) The results of the first step are a coherent UAV trajectory and initial position estimations of the UWB beacons. The second step in our methodology involves the automatic detection of loop closures, which is usually based on different approaches such as visual place recognition or scan matching. In this case, a massive scan matching process is carried out among all the poses that fall within a given search radius, based on the 3D point clouds acquired by the on-board 3D imaging sensor (a stereo or RGB-D camera). This loop-closure detection will add new constraints to the robot poses Grisetti et al. (2010). However, this approach goes one step forward and takes into account a new type of constraint apart from the usual transform between poses, so that we can also optimize the alignment between the 3D point clouds directly into the optimizer. Given the sensor point cloud pci at pose xi , and the point cloud pcj at pose xj , the scan matching process establishes the transform that best aligns both point clouds (using ICP as in Chapter 3). In the literature, this transform is commonly used as a constraint between both poses, and its associated information matrix allows tuning the importance of such constraint into the non-linear optimization process. Instead, we propose to include the alignment error into the optimization process. To this end, each point cloud is transformed to the global reference frame according to its associated pose ( pcg i ) and then the alignment error between point clouds is computed as the averaged Euclidean distance between their individual 3D points. Equation 5.3 is used instead of Equation 5.5, since now a single hypotheses is available for each UWB position. This equation can be enlarged with this new constraint as 5.3 Experimental Results 115 follows: arg min {X,B}"N X i=1 Ei+ M X j=1 cij(kxi−bjk−dij ) + Pi X l=1 D(pcg i,pcg l)!# (5.9) where Pi is the number of loop closures in which pose i is involved, and the function D ( pcg i,pcg l ) computes the average Euclidean distance between the given point clouds in the global frame. Obtaining D ( pcg i,pcg l ) could have a significant computational cost if the point clouds are large, because we need to calculate the closest 3D point of each cloud into the other, hence slowing down the optimization process. However, assuming that the poses to be optimized are not far from the final estimates thanks to the RO-SLAM step, these associations between the point clouds can be pre-computed. In this way, the function D ( pcg i,pcg l ) only needs to compute the average distance between 3D points because the associations are already known. 5.3 Experimental Results An experimental setup has been conceived in order to test the proposed approach. A UAV has been equipped with two RGB-D sensors (Orbbec’s Astra), one in the front and another one in the rear side of the robot. Both are tilted down slightly (25 ◦ ). A UWB sensor has been also installed on top of the aerial robot, and a set of three UWB beacons have been placed in the scenario. The UWB distance measurements have a standard deviation of approximately 20cm, but they are subject to further distortions due to reflexions or sigma attenuation. The UAV used for testing is depicted in Figure 5.2. The experiments have been carried out once again at CATEC’s indoor testbed, in which a scenario recreating an aircraft manufacturing plant has been installed as shown in Figure 5.3. As in previous experiments, accurate ground-truth localization data for the UAV were acquired, as well as the positions of the three UWB beacons installed in the environment, using the testbed’s motion capture system. 122 Multi-Modal Mapping time (s) 0 50 100 150 200 250 300 350 400 x error (m) 0 0.5 1 Errors using ground-truth map time (s) 0 50 100 150 200 250 300 350 400 y error (m) 0 0.5 1 time (s) 0 50 100 150 200 250 300 350 400 z error (m) 0 0.5 time (s) 0 50 100 150 200 250 300 350 400 yaw error (rad) 0 0.5 time (s) 0 50 100 150 200 250 300 350 400 x error (m) 0 0.5 1 Errors using our built map time (s) 0 50 100 150 200 250 300 350 400 y error (m) 0 0.5 1 time (s) 0 50 100 150 200 250 300 350 400 z error (m) 0 0.5 time (s) 0 50 100 150 200 250 300 350 400 yaw error (rad) 0 0.5 Figure 5.9: Errors in the estimated UAV position and yaw angle using the ground-truth map and the reconstructed map. 5.4 Conclusions The experimental results show how a multi-sensor suite can be used to perform 3D mapping and long-term localization, integrating sensor readings from UWB beacons and RGB-D cameras to build a robust approach. The mapping algorithm exploits the synergies between UWB and RGB-D to build an accurate 3D map of the environment and to localize the UWB sensors into such map. The localization approach successfully integrates both sensor types to overcome the limitations of each sensor modality by its own. This mapping approach is oriented to be used with the particle filter presented in Chapter 4, hence providing a full methodology for applying this safe, robust and long-term localization approach to any custom scenario in which small UAVs need to be deployed. Chapter 6 System Architecture and Framework In order to simplify the accomplishment of all the tasks related to the autonomous navigation of UAVs, a framework for combining and executing heterogeneous software modules has been developed. There are several open-source projects that target complete software architectures Lim et al. (2012), such as ArduPilot 1 or PX4 2 . However, these approaches usually lack flexibility to perform and support high-level functionalities, which are often demanded by users that wish to develop innovative approaches towards UAV autonomy. In recent years, several initiatives have arisen from some research groups. The Autonomous Systems Lab from Eidgen¨ossische Technische Hochschule (ETH) Zurich developed asctec mav framework 3 , however it is dependent on the UAV manufacturer. The Paparazzi project 4 from ´ Ecole Nationale de l’Aviation Civile (ENAC) is a complete solution that also includes specific hardware, the Paparazzi autopilot. Another example is hector quadrotor 5 from Technische Universit¨at Darmstadt, which is heavily focused on simulation environments and has limited applicability in real systems performing 1http://ardupilot.org 2http://px4.io 3http://wiki.ros.org/asctec mav framework 4http://wiki.paparazziuav.org/wiki/Main Page 5http://wiki.ros.org/hector quadrotor 123 124 System Architecture and Framework Figure 6.1: Schematic overview of the proposed architecture. experimental flights. Even though these approaches have produced major advances in this line of research, there are still important challenges related to the achieved level of autonomy and flexibility of the system to be able to adapt it for different aerial robots or applications. Figure 6.1 shows the software architecture that has been built and refined throughout the different works towards this dissertation. The system is decomposed into several modules which are in charge of different tasks. One of the first things to consider for an autonomous robot is the estimation of the current state of the platform in a specific coordinate system. For this, sensor data is first processed and used to obtain a localization estimation with respect to its environment. This is commonly carried out by an odometry calculation in Sensor-based Odometry and a localization estimation in Robot Localization to produce a reliable 6 DoF pose (position and orientation). Sensor data is commonly used in both modules in order to reduce uncertainties and improve the robustness of the estimations, especially for long-term operation of the 125 aerial robot. These two modules have been the specific target of the work described in this dissertation. Once reliable robot localization is available, subsequent modules can consider tackling more complex tasks, such as where to go or how to reach a specific location. The generation of safe trajectories is another essential capability for autonomous navigation, especially in cluttered areas where several obstacles or even other robots can be found. Realistic operating environments for aerial robots may exhibit fixed or mobile elements and simple or complex obstacles, all of them sharing the same area. Therefore, the aerial robot must continuously conduct a local recalculation of the trajectory in real-time in order to avoid collisions with different types of elements as the on-board sensors gain information about its surroundings. Additionally, the UAV might also need to adopt different collision avoidance strategies depending on the nature of the obstacles; the robot should avoid persons differently than static objects for instance. Sensor data is also acquired by the Trajectory Planner for reactive collision avoidance, along with the localization output in order to compute safe trajectories in the aerial robot environment. The Trajectory Tracker ensures the UAV reaches the desired waypoints at the correct times, and thus sends the Controller the references that it needs in order to calculate the necessary actions for the UAV. Special focus has been given to safety in the operation of the aerial robot, hence the key components in our approach are the perception of the environment for both localization and motion planning. This will allow the UAV to select an appropriate behavior according to the awareness of the current situation. The core idea behind the architecture is the combination of simple concepts and components to build a reliable system. Each module runs as an independent process on-board the UAV. The different modules can implement a wide variety of algorithms and techniques for performing their task, according to the equipment of the aerial robot. It is important to point out that working with UAVs involves additional constraints in terms of computation and payload capacity which must be taken into account for the development of applicable systems. That said, the different algorithms can be developed independently and switch or replace them within any of the modules to solve the task that is intended to perform. This does not affect the rest of the 126 System Architecture and Framework architecture provided that the new algorithm complies with the applicable input and output interfaces of the specific module. Besides, the architecture is focused on the software components, remaining independent from the hardware used underneath. The core algorithms of the system do not depend on the specific manufacturer of the UAV platform or the specific sensors that are used, and hence it is possible to replace them with a different model or even with an upgraded version of the device, implying only minor changes to the system. Apart from that, it is possible to complement the system with additional sensors or enhanced processing capabilities without altering the overall scheme. Moreover, this architecture is able to accommodate applications involving a single or multiple UAVs, as well as highly autonomous operations or teleoperated missions. Our approach uses as middleware the Robot Operating System (ROS) Quigley et al. (2009), which was developed by Willow Garage and Stanford University as part of the STanford Artificial Intelligence Robot (STAIR) project as a free and open-source robotic middleware for the large-scale development of complex robotic systems. ROS acts as a meta-operating system for robots as it provides hardware abstraction, low-level device control, inter-processes message-passing and package management. It also provides tools and libraries for obtaining, building, writing, and running code across multiple computers. Currently, ROS is widely used in research activities and is becoming a de facto standard for robotic applications. One of the main advantages of ROS is that it allows manipulating sensor data as a labeled abstract data stream, called topic, without having to deal with hardware drivers. The interfaces between our modules are based on standard ROS data types for transformations, pose estimations or trajectory commands. This greatly improves the flexibility of the architecture to allow testing custom high-level algorithms for autonomous operation, while at the same time enables the use of UAVs from different vendors and a wide variety of sensors for which supported drivers are already available. We have made extensive use of the tools available in ROS to help to debug and detect issues in both hardware and software. This includes simulations run in Gazebo, logging and playing back data from different experiments or visualization and inspection tools. This is especially useful when an 6.1 UAV Platform 127 innovative approach or algorithm is under development, and there is a potential risk of crashing the platform. Thanks to the use of these tools, it is possible to test the algorithms and modules off-line using real flight data and debug internal states of the UAV or present relevant data in a graphical interface. As stated in Chapter 1, the main context in which the proposed architecture has been developed and tested is EuRoC, in which high-level semi-autonomous operation of a UAV for inspection tasks has been demonstrated. The aforementioned framework has been designed and implemented in order to perform UAV localization without external positioning systems, among other tasks. Our team GRVC-CATEC successfully completed Stage I of the project (Qualifying Simulation Contest), being among the top 15 challenger teams. In order to gain access to Stage II of EuRoC project, an end-user had to be selected in order to actually demonstrate the system capabilities, and in our case, the end-user is Airbus Defence&Space (D&S), a world leader in aircraft manufacturing, interested in the automation of logistic processes in their manufacturing plants. The evaluation of the proposals was carried out by the Challenge Advisory Board (ETH Zurich among other institutions) with the help of external reviewers and renowned independent experts, and it was based on novelty of the application, level of difficulty of the use case, potential market for it, strength of the team, further to the rank in the simulation contest. Our team was granted access to Stage II with the second highest score, and we are currently competing with other four European challenger teams to gain access to the final Stage III. The proposed architecture has been developed in the context of this project, and the UAV has been extensively tested in order to succeed in the different rounds of experiments. 6.1 UAV Platform The UAV used in EuRoC’s Stage II is a research prototype from Ascending Technologies called AscTec Neo.It is a hexacopter with 9” propellers which can lift up to 2Kg, but the maximum nominal flight time of 20 minutes (without payload) will be reduced accordingly. This is a platform developed and optimized within the framework of different European research projects. 128 System Architecture and Framework Figure 6.2: The two main sensors tested on-board the UAV: VI-Sensor (left) and Astra (right). The default main sensor on-board the platform is Skybotix’s VI-Sensor, a stereo camera with a calibrated and synchronized IMU. Nevertheless, the original mount has been modified in order to accommodate different sensors. Our architecture has been validated not only with the VI-Sensor, but also using an RGB-D camera as the main sensor, in particular Orbbec’s Astra, as it can be seen in Figure 6.2. The sensor is mounted facing forward in the direction of the flight in order to maximize the information gathered regarding possible nearby obstacles. The orientation of the sensor is customizable as we have manufactured different mounts using a 3D printer. Figure 6.3 shows one of the tested mounts. The selected mount will depend on the experiments scenario, the flight height or the type of obstacles that the UAV might encounter in its environment. Depending on the main visual sensor mounted on-board the UAV, the software modules involved with sensor data were adapted accordingly in order to work with the type of data that each sensor delivers. The on-board embedded computer (Intel Next Unit of Computing (NUC) with Core i7) is in charge of acquiring sensor data, estimating the aerial robot current state, generating the appropriate trajectories according to a given navigation goal and running the autopilot for controlling the UAV. Given all these requirements, special care has been put over all the running modules in order to cut down the computational needs when possible. 6.2 Controlled tests 129 Figure 6.3: A sample of 3D printed mount for the main vision-based sensor on-board the UAV. It is important to point out that all the results shown in this chapter correspond to different flights in which the control loop is closed using the localization estimations based on on-board sensors, which are computed on-line, unlike previous chapters where data were processed off-line. 6.2 Controlled tests To succeed in achieving robust autonomous navigation, the aerial robot must demonstrate both reliability and good performance. Extensive field testing has taken place at the indoor testbed of CATEC, shown in Figure 6.4, whose motion capture system tremendously helped us to benchmark the developed algorithms against a very precise ground-truth for robot localization, motion planning or obstacle avoidance with safety distance assurance. The goal of the controlled tests performed at this indoor testbed was to monitor our development progress and measure flying skills of the aerial robot. We also used the tests to evaluate the effects of changes in hardware and software on the UAV. The flying skills which we have systematically tested to assess our progress were the following: •Ability of the UAV to autonomously take-off and perform stable hovering. •Ability of the UAV to accurately follow a pre-planned obstacle-free trajectory. 130 System Architecture and Framework Figure 6.4: One of the testing scenarios in CATEC’s indoor testbed. • Ability of the UAV to generate an obstacle-free trajectory in order to safely reach a specific waypoint. • Ability of the UAV to dynamically modify a generated trajectory towards a waypoint to avoid perceived obstacles along the way. As it is apparent in Figure 6.4, the testbed structure allowed us to secure the UAV from the top via a safety rope to prevent an undesired flight termination. This has been very useful during the development process, as testing new features in the algorithms may sometimes result in unexpected behaviors. 6.3 EuRoC Benchmarking The first part of Stage II consisted of performing a series of tasks defined by the Challenge Host (ETH Zurich), which were the same for all the teams in the same flying arena with the same UAV platform. This section briefly describes our experience 6.3 EuRoC Benchmarking 131 Figure 6.5: ETH’s flying arena used in the Benchmarking and Free-Style rounds. and results obtained from the experiments carried out during the summer of 2016 regarding the Benchmarking round, that took place in ETH Zurich (see Figure 6.5). The UAV running the proposed architecture has been extensively tested in order to fulfill the proposed tasks, which were meant to guide our development towards high-level autonomous tasks. Our system relied on a Sensor-based Odometry based on the visual-inertial odometry algorithm described in Chapter 3, gathering sensor data from the on-board VI-Sensor stereo camera. The Robot Localization module used in this experiment round was based on Stereo Parallel Tracking and Mapping (S-PTAM) Pire et al. (2015), a stereo SLAM system which we adapted in order to make it work within our architecture and using our odometry estimations along with the acquired stereo images. A series of Benchmarking experiments were proposed, which consisted of several tasks that the aerial robot should perform, and metrics to evaluate them. •Task 1 - System Setup : this task simply benchmarks the time that teams need to set up the platform out of the transportation box until stable hovering at 1 meter of altitude above the take-off location. •Task 2 - State Estimation : the purpose of this task is to assess the drift of the visual localization system and state estimator. Collision-free random