Trinocular Ground System to Control UAVs



Similar documents
Visual Servoing using Fuzzy Controllers on an Unmanned Aerial Vehicle

A Pan-Tilt Camera Fuzzy Vision Controller on an Unmanned Aerial Vehicle

Tracking of Small Unmanned Aerial Vehicles

CHAPTER 1 INTRODUCTION

Onboard electronics of UAVs

Geometric Camera Parameters

UAV Pose Estimation using POSIT Algorithm

An Active Head Tracking System for Distance Education and Videoconferencing Applications

ZMART Technical Report The International Aerial Robotics Competition 2014

3D Vision An enabling Technology for Advanced Driver Assistance and Autonomous Offroad Driving

Current Challenges in UAS Research Intelligent Navigation and Sense & Avoid

Force/position control of a robotic system for transcranial magnetic stimulation


Control of a quadrotor UAV (slides prepared by M. Cognetti)

Automatic Labeling of Lane Markings for Autonomous Vehicles

Aerospace Information Technology Topics for Internships and Bachelor s and Master s Theses

The Design and Implementation of a Quadrotor Flight Controller Using the QUEST Algorithm

Analecta Vol. 8, No. 2 ISSN

Robust and accurate global vision system for real time tracking of multiple mobile robots

2. Dynamics, Control and Trajectory Following

An Energy-Based Vehicle Tracking System using Principal Component Analysis and Unsupervised ART Network

A PHOTOGRAMMETRIC APPRAOCH FOR AUTOMATIC TRAFFIC ASSESSMENT USING CONVENTIONAL CCTV CAMERA

PHOTOGRAMMETRIC TECHNIQUES FOR MEASUREMENTS IN WOODWORKING INDUSTRY

SIX DEGREE-OF-FREEDOM MODELING OF AN UNINHABITED AERIAL VEHICLE. A thesis presented to. the faculty of

A Reliability Point and Kalman Filter-based Vehicle Tracking Technique

How To Fuse A Point Cloud With A Laser And Image Data From A Pointcloud

Research Methodology Part III: Thesis Proposal. Dr. Tarek A. Tutunji Mechatronics Engineering Department Philadelphia University - Jordan

Landing on a Moving Target using an Autonomous Helicopter

3D Scanner using Line Laser. 1. Introduction. 2. Theory

Path Tracking for a Miniature Robot

An inertial haptic interface for robotic applications

3D Vision An enabling Technology for Advanced Driver Assistance and Autonomous Offroad Driving

Design Specifications of an UAV for Environmental Monitoring, Safety, Video Surveillance, and Urban Security

Mean-Shift Tracking with Random Sampling

Hardware In The Loop Simulator in UAV Rapid Development Life Cycle

VEHICLE TRACKING USING ACOUSTIC AND VIDEO SENSORS

THE problem of visual servoing guiding a robot using

Intelligent Submersible Manipulator-Robot, Design, Modeling, Simulation and Motion Optimization for Maritime Robotic Research

INSTRUCTOR WORKBOOK Quanser Robotics Package for Education for MATLAB /Simulink Users

Colour Image Segmentation Technique for Screen Printing

CE801: Intelligent Systems and Robotics Lecture 3: Actuators and Localisation. Prof. Dr. Hani Hagras

Terrain Traversability Analysis using Organized Point Cloud, Superpixel Surface Normals-based segmentation and PCA-based Classification

THE CONTROL OF A ROBOT END-EFFECTOR USING PHOTOGRAMMETRY

Dynamic composition of tracking primitives for interactive vision-guided navigation

Autonomous Advertising Mobile Robot for Exhibitions, Developed at BMF

A Prototype For Eye-Gaze Corrected

Vision-Based Blind Spot Detection Using Optical Flow

A Genetic Algorithm-Evolved 3D Point Cloud Descriptor

Introduction.

Video stabilization for high resolution images reconstruction

Mouse Control using a Web Camera based on Colour Detection

Eigenständige Erkundung komplexer Umgebungen mit einem Hubschrauber UAV und dem Sampling basierten Missionsplaner MiPlEx

Robot Perception Continued

PRODUCT DATASHEET. J1939 Vehicle Inertia Monitor. Advanced Vehicle Inertial Measurement and Vibration Monitoring Device. fleet-genius.

An Approach for Utility Pole Recognition in Real Conditions

2. igps Web Service Overview

Interactive Segmentation, Tracking, and Kinematic Modeling of Unknown 3D Articulated Objects

Solution Guide III-C. 3D Vision. Building Vision for Business. MVTec Software GmbH

High-accuracy ultrasound target localization for hand-eye calibration between optical tracking systems and three-dimensional ultrasound

Static Environment Recognition Using Omni-camera from a Moving Vehicle

Automated Pose Determination for Unrestrained, Non-anesthetized Small Animal Micro-SPECT and Micro-CT Imaging

CONTRIBUTIONS TO THE AUTOMATIC CONTROL OF AERIAL VEHICLES

EFFICIENT VEHICLE TRACKING AND CLASSIFICATION FOR AN AUTOMATED TRAFFIC SURVEILLANCE SYSTEM

Robert Collins CSE598G. More on Mean-shift. R.Collins, CSE, PSU CSE598G Spring 2006

Enhancing the SNR of the Fiber Optic Rotation Sensor using the LMS Algorithm

3D Vehicle Extraction and Tracking from Multiple Viewpoints for Traffic Monitoring by using Probability Fusion Map

V-PITS : VIDEO BASED PHONOMICROSURGERY INSTRUMENT TRACKING SYSTEM. Ketan Surender

RS platforms. Fabio Dell Acqua - Gruppo di Telerilevamento

A System for Capturing High Resolution Images

Basler. Area Scan Cameras

How To Control Gimbal

How To Use Trackeye

PID, LQR and LQR-PID on a Quadcopter Platform

Real-Time Automated Simulation Generation Based on CAD Modeling and Motion Capture

7 MEGAPIXEL 180 DEGREE IP VIDEO CAMERA

From Pixel to Info-Cloud News at Leica Geosystems JACIE Denver, 31 March 2011 Ruedi Wagner Hexagon Geosystems, Geospatial Solutions Division.

Tracking and Recognition in Sports Videos

Sensory-motor control scheme based on Kohonen Maps and AVITE model

Classifying Manipulation Primitives from Visual Data

Introduction to Robotics Analysis, Systems, Applications

Time Domain and Frequency Domain Techniques For Multi Shaker Time Waveform Replication

MP2128 3X MicroPilot's. Triple Redundant UAV Autopilot

Mechanical Design of a 6-DOF Aerial Manipulator for assembling bar structures using UAVs

Product Characteristics Page 2. Management & Administration Page 2. Real-Time Detections & Alerts Page 4. Video Search Page 6

Automotive Applications of 3D Laser Scanning Introduction

Segmentation of building models from dense 3D point-clouds

Real Time Target Tracking with Pan Tilt Zoom Camera

Removing Moving Objects from Point Cloud Scenes

Cloud tracking with optical flow for short-term solar forecasting

Digital Image Increase

A Method for Controlling Mouse Movement using a Real- Time Camera

K.Prasanna NIT Rourkela,India Summer Internship NUS

Mobile Robot FastSLAM with Xbox Kinect

Basic Principles of Inertial Navigation. Seminar on inertial navigation systems Tampere University of Technology

TRIMBLE ATS TOTAL STATION ADVANCED TRACKING SYSTEMS FOR HIGH-PRECISION CONSTRUCTION APPLICATIONS

Transcription:

Trinocular Ground System to Control UAVs Carol Martínez, Pascual Campoy, Iván Mondragón, and Miguel A Olivares-Méndez Computer Vision Group Universidad Politécnica de Madrid Jose Gutierrez Abascal 2, 28006 Madrid, Spain carolvivianamartinez@upmes wwwdisamupmes/colibri Abstract In this paper we introduce a real-time trinocular system to control rotary wing Unmanned Aerial Vehicles based on the 3D information extracted by cameras located on the ground The algorithm is based on key features onboard the UAV to estimate the vehicle s position and orientation The algorithm is validated against onboard sensors and known 3D positions, showing that the proposed camera configuration robustly estimates the helicopter s position with an adequate resolution, improving the position estimation, especially the height estimation The obtained results show that the proposed algorithm is suitable to complement or replace the GPS-based position estimation in situations where GPS information is unavailable or where its information is inaccurate, allowing the vehicle to develop tasks at low heights, such as autonomous landing, take-off, and positioning, using the extracted 3D information as a visual feedback to the flight controller I INTRODUCTION The use of computer vision algorithms designed to control UAVs has been an attractive and interesting research area in the academic and industrial field, being a special case of study the use of autonomous helicopters due to their grate maneuver capabilities that allow them to fly at low speed velocities (lateral and longitudinal flights), hover, perform vertical take-off and landing, and maneuver in reduced spaces The vision systems implemented in UAVs cover areas such as object detection and object tracking [], pose estimation [2], navigation [3], obstacle detection [4], and autonomous landing [5], among others, in which vision is always used as a rich source of information for the mission s success The goal of this research is to provide UAVs with an additional vision-based source of information, extracted by ground cameras, in order to allow UAVs to develop visually guided tasks, such as landing, inspection or ground-air cooperation missions, especially in situations when the GPS information is not available, when the GPS-based position estimation is not accurate enough for the task to develop, or when the payload restrictions do not allow the incorporation of additional sensors The vision based pose estimation problem has been extensively studied, such as in [6], [7], [8], and [9], where the information of the vision system is integrated with the onboard sensors to control the UAV In another approach, an onboard camera, in conjunction with a ground moire pattern, is used to estimate the vehicle s six degrees of freedom for landing purposes [0] Ground-based cameras approaches for solving the pose estimation problem have also been proposed In [], a ground camera is used to control a dirigible based on the information of artificial landmarks In that work, the 3D position and orientation information are recovered based on the knowledge of the geometrical properties of the landmarks A more recent work deals with the pose estimation problem of a quadrotor vehicle using simultaneously both ground and onboard cameras [2] In the latter work, color landmarks are used to recover the vehicle s pose In this paper, we introduce a vision-based control system for UAVs by presenting the estimation of the position and orientation (Yaw angle) of a rotary wing Unmanned Aerial Vehicle and its integration in the UAV control loop All of this, has the aim of developing a visionbased platform for autonomous take-off and landing, and high precision positioning tasks The implemented system is a redundant system composed of three cameras located on the ground in charge of recovering the information of key features onboard the helicopter in order to obtain a robust 3D position estimation by using a trinocular or binocular estimation (depending on the number of cameras that see a specific key feature), and also to increase the vehicle s workspace In these first steps towards an external vision-based control, color landmarks onboard the UAV have been used as key features The helicopter detection and tracking is based on such landmarks, and is achieved by using the CamShift (Continuously Adaptive Mean SHIFT) algorithm [3] 3D reconstruction techniques are also used for the estimation of the helicopter s pose [4] The proposed algorithm has been tested in a variety of situations (indoors and outdoors), and the results of these tests are shown in this paper, whose outline is as follows: Section II presents the system architecture Section III describes the reference systems Section IV deals with the vision-based control system for UAVs Finally, in Sections V and VI experiment results, conclusions and future works are presented II SYSTEM ARCHITECTURE The adequate intervention of several components from a hardware and software perspective is required for a mission s success, especially when a vision system is used in a UAV

control loop The following section presents a description of the different components of the proposed system Fig Helicopter tesbed Colibri III, during a flight Color landmarks onboard the UAV are used as key features for the position estimation algorithm A Helicopter Testbed The Computer Vision Group at the UPM (Universidad Politcnica de Madrid) counts with three fully operational UAV systems to develop indoors and outdoors applications Such systems form part of the COLIBRI Project, whose purpose is to develop algorithms for vision based control of UAVs [5] The experiments were carried out with the Colibri III system (see Fig ) It is a Rotomotion SR20 helicopter with an electric motor of 300 W, 8A It is equipped with an xscale-based flight computer, a GPS, an IMU (Inertial Measurement Unit), an onboard pan and tilt camera, and an onboard Nano Itx computer for image processing algorithms The flight computer runs Linux OS and uses an 802g wireless Ethernet protocol for sending and receiving information to/from the ground station B Trinocular System The trinocular system, as shown in Fig 2, is composed of three FireWire cameras with 34mm lenses ( Field Of View: H FOV 87 V FOV 7 ); each camera captures images of 320 240 size at 30fps (frames per second) These cameras are strategically located on an aluminum platform in order to allow a convergent optical axis configuration in two of the three cameras, and to take advantage of the cameras field of view The cameras position and orientation are fixed, and their intrinsic and extrinsic parameters are known due to a calibration process The cameras are connected to a vision computer, which is a 22 GHz laptop running Linux OS C Communication System We use a client-server architecture implemented in our systems (Colibri I, Colibri II, and Colibri III) This architecture is based on TCP/UDP messages and is used to exchange information between the helicopter, the ground station and the ground vision system The exchange is achieved through a high level layer defined by a communication API (Application Programming Interface), which routes the messages to the specific process depending on the message type The messages can be sent from the ground station or from the Fig 2 Trinocular system It is composed by three FireWire cameras that capture images of 320 240 size at 30fps The rotation matrices and translation vectors that define the relation of the cameras with the World Coordinate System are calculated from a calibration process vision system, to the flight controller and can also be sent in the other direction (from the flight controller to the ground station and the vision system) Those messages include velocity control, position control, and helicopter and GPS state messages, among others Fig 3 shows an example of messages that can be transmitted through the switching layer Fig 3 Switching layer TCP/UDP messages are used to exchange data between the flight controller, the ground station, and the vision system III COORDINATE SYSTEMS Different coordinate systems are used to map the extracted visual information from R 2 to R 3, and then to convert this information into commands to the helicopter This section provides a description of the coordinate systems and their corresponding transformations to achieve vision-based tasks We have different coordinate systems; the Image Coordinate System (X i ), that includes the Lateral (X f ) and Central Coordinate Systems (X u ) in the image plane, the Camera Coordinate System (X c ), the Helicopter Coordinate System (X h ), and an additional one: the World Coordinate System (X w ), used as the principal reference system to control the vehicle (see Fig 4) Image and Camera Coordinate Systems

Fig 4 Local and global coordinate systems The mapping from the image plane to the Camera Coordinate System requires the calculation of the intrinsic camera parameters (c x,c y, f ) Transformation from the Camera Coordinate System to World Coordinate System is performed through a rigid transformation, and requires the extrinsic parameters of the trinocular system The World and the Helicopter Coordinate Systems are related through a rigid transformation, which defines the helicopter s position and orientation, and allows to send the adequate commands to the flight controller The relation between the Camera Coordinate System and the Image Coordinate System is taken from the pinhole camera model It states that any point referenced in the Camera Coordinate System x c is projected onto the image plane in the point x f by intersecting the ray that links the 3D point x c with the center of projection and the image plane This mapping is described in (), where x c and x f are represented in homogenous coordinates nx f ny f n = f x 0 c x 0 0 f y c y 0 0 0 0 x f = K k [I 0]x c The matrix K k contains the intrinsic camera parameters of the k th camera, such as the coordinates of the center of projection (c x,c y ) in pixel units, and the focal length ( f x, f y ), where f x = f m x and f y = f m y represent the focal length in terms of pixel dimensions, being m x and m y the number of pixels per unit distance The above-mentioned camera model assumes that the world point, the image point, and the optical center are collinear; however, in a real camera lens there are some effects (lens distortions) that have to be compensated in order to have a complete model This compensation can be achieved by the calculation of the distortion coefficients through a calibration process [6], in which the intrinsic camera parameters, as well as the radial and tangential distortion coefficients, are calculated Camera and World Coordinate Systems Considering that the cameras are fixed, these systems are related by a rigid transformation that allows to define the pose of the k th camera in a World Coordinate Frame As presented in (2), this transformation is defined by a rotation matrix R k and a translation vector t k that link the two coordinate systems and represent the extrinsic camera parameters x c y c z c () Such parameters are calculated through a calibration process of the trinocular system [ R k t x c = k ] 0 T x w (2) World and Helicopter Coordinate Systems The Helicopter Reference System, as described in Fig 4, has its origin at the center of mass of the vehicle and its correspondent axes: X h, aligned with the helicopter s longitudinal axis; Y h, transversal to the helicopter; and Z h, pointing down Considering that the estimation of the helicopter s pose with respect to the World Coordinate System is based on the distribution of the landmarks around the Helicopter Coordinate System, and that the information extracted from the vision system will be used as reference to the flight controller, a relation between those coordinate systems has to be found In Fig 4, it is possible to observe that this relation depends on a translation vector that defines the helicopter s position (t), and on a rotation matrix R that defines the orientation of the helicopter (pitch, roll and yaw angles) For practical purposes, pitch and roll angles are considered = 0 (helicopter flying at low velocities (< 4m/s), and only the yaw angle (θ) is taken into account in order to send the adequate commands to the helicopter Therefore, the relation of the World and the Helicopter Coordinate Systems can be expressed as follows: x w y w z w = cos(θ) sin(θ) 0 t x sin(θ) cos(θ) 0 t y 0 0 t z 0 0 0 x h y h z h (3) Where (t x, t y, t z ) will represent the position of the helicopter (x wuav,y wuav,z wuav ) with respect to the World Coordinate System, and θ the helicopter s orientation IV VISION-BASED CONTROL A Feature extraction and tracking A color-based algorithm is applied to extract four different color landmarks (i), which are located onboard the UAV as shown in Fig The algorithm that is implemented for color segmentation is based on probability distributions As a consequence, a model probability distribution of the color of each landmark and a probability distribution of those colors in each frame have to be determined Converting from RGB to HSV color spaces, and using the histogram of the Hue channel, the color probability distributions are created Then, the relation between the probability distributions (the models ones with the probability distribution of each color in each frame) is found using the backprojection algorithm proposed by Swain and Ballard in [7] This algorithm computes the likelihood that each image point belongs to the model This relation is found by calculating a ratio histogram Rh, as defined in (4) [ Mh Rh k k ] i ( j) = min i ( j) Ih k, (4) i ( j)

Where Mh k i ( j), Ihk i ( j), and Rhk i ( j) correspond respectively to the values of the j th bin of the model histogram, the current image histogram, and the ratio histogram created for the i th color in the k th camera Once Rh k i is found, it is then backprojected onto the image, replacing the values of the image by the values of Rh k i that the former values index The resulting image is a gray-scale image, where pixels values represent the probability that the pixel belongs to the object of interest Once each backprojected image has been extracted, a threshold is applied in order to eliminate outliers The new location of the object in each frame is found by using the backprojected image previously extracted and the Continuously Adaptive Mean Shi f t (CamShi f t) algorithm [3] to track the landmarks This tracker is based on the Mean Shi f t algorithm, originally introduced in [8] This algorithm operates on probability distributions [9] and uses the gradient of the distributions in order to find the nearest dominant mode (peak) However, taking into account that this distribution can change over time, the CamShi f t algorithm allows the Mean Shi ft algorithm to adapt dynamically to those changes based on the zeroth moment information of the search window [3] The following steps summarize the CamShi f t algorithm [20]: ) Choose the initial location and size of the search window 2) Run Mean Shi ft algorithm (one or many iterations) to find the densest region Once it is found, store the zeroth moment and its central location 3) Center the search window at the mean location found in Step 2 and set the search window size equal to a function of the zeroth moment found in Step 2 4) Repeat Steps 2 to 3 until converge occurs (when the mean location moves less than a preset threshold) For each frame in each camera, the CamShi ft algorithm finds the centroid of the landmark ( x i k,ȳk i ) within the search window by calculating the zeroth (m k i 00 ) and first moments in the X and Y axes (m k i 0, m k i 0 ), as presented in (5) m k i 00 = Σ x Σ y I k i (x,y) m k i 0 = Σ x Σ y xii k(x,y), mk i 0 = Σ x Σ y yii k(x,y) (5) x i k = mk i 0 ; ȳ k m k i = mk i0 i m k 00 i 00 Where Ii k (x,y) represents the intensity value (within the search window) of the backprojected image found for landmark i in the k th camera The centroid found is then used as feature for the 3D reconstruction stage B 3D Reconstruction The 3D reconstruction of each landmark requires a previous calibration of the system in order to find the intrinsic parameters (K k ) of the cameras and the extrinsic parameters (R k and t k ) of the trinocular system (Fig 2) This calibration is done using the CalTech camera calibration toolbox [2] Once the previously mentioned parameters are calculated for each camera, the different transformations mentioned in Section III have to be applied from the image plane to the World Coordinate System in order to recover the 3D position of the landmarks Reorganizing (), we have that: x fi c k x = f k x x ci z ci, y fi c k y = f k y y ci z ci (6) If we consider that x ui = x fi c k x, y ui = y fi c k y, and if we take into account the color information of the landmarks and the trinocular extrinsic parameters of the trinocular system, equation (2) can be applied to relate a 3D point (the 3D position of landmark i) with its projection in each camera using the following equation: x k u i = f k rk x w i +r k 2 y w i +r k 3 z w i +t k x r k 3 x w i +r k 32 y w i +r k 33 z w i +t k z y k u i = f k rk 2 x w i +r k 22 y w i +r k 23 z w i +t k y r k 3 x w i +r k 32 y w i +r k 33 z w i +t k z In the aforementioned equations, xu k i and y k u i represent the coordinates of landmark i expressed in the Central Camera Coordinate System of the k th camera, whereas, r k and t k are the components of the rotation matrix R K and the translation vector t k that represent the extrinsic parameters of the trinocular system Therefore, for each landmark in each camera we will obtain two equations such as those presented in (7) Applying a restriction regarding the minimum number of cameras that see a specific landmark (in order to triangulate the landmark position), if at least two cameras are seeing the same landmark, it is possible to create a system of equations of the form Ac = b, where: (xc i r3 r ) (x c i r32 r 2 ) (x c i r33 r 3 ) (y c i r3 r 2 ) (y c i r32 r 22 ) (y c i r33 r 23 ) A = (8) (xc k i r3 k rk ) (xk c i r32 k rk 2 ) (xk c i r33 k rk 3 ) (y k c i r3 k rk 2 ) (yk c i r32 k rk 22 ) (yk c i r33 k rk 23 ) c = b = x w i y wi z wi (t x x k c i t z ) (t y y k c i t z ) (t k x x k c i t k z ) (t k y y k c i t k z ) (7) (9) (0) The previous system can be solved using the pseudoinverse of the matrix A, as presented in (), where the obtained vector c represents the position (x wi,y wi,z wi ) of the i th landmark c = A + b = (A T A) A T b ()

C Pose estimation In this case, the estimation of the helicopter s pose will be related exclusively with the estimation of the 3D position of the helicopter (x wuav ) and the orientation of the helicopter both expressed with respect to the World Coordinate System The helicopter s orientation is defined only with respect to the Z h axis (θ), while the angles, with respect to the other axes, are considered = 0 Therefore, taking into account the 3D positions of the landmarks extracted in the previous stage, and taking into account the landmarks distribution around the Helicopter Coordinate System, it is possible to formulate equation (2) for each landmark, considering only the yaw angle θ (as explained in section III): x wi y wi z wi = cos(θ) sin(θ) 0 t x sin(θ) cos(θ) 0 t y 0 0 t z 0 0 0 x hi y hi z hi (2) Reorganizing (2) for all the landmarks that have been detected, and considering that cθ = cos(θ), sθ = sin(θ), x wuav = t x, y wuav = t y, z wuav = t z, equation (2) can be rewritten in the form: Ac = b (3) x h y h 0 0 y h x h 0 0 0 0 0 0 x hi y hi 0 0 y hi x hi 0 0 0 0 0 0 cθ sθ x wuav y wuav z wuav = x w y w z w z h x wi y wi z wi z hi Where our unknown parameters cθ,sθ,x wuav,y wuav,z wuav correspond to the values that define the orientation (yaw angle) and the position of the helicopter with respect to the World Coordinate System The previous system of equations can be solved as in (), with the restriction that the reconstruction of at least two landmarks is required to find both x wuav and the yaw angle (θ) However, this restriction can vary depending on the task to develop: in the case of autonomous landing, if we assume that the helicopter is in the adequate landing position and the trinocular system will actuate only on the helicopter s height, the control task can be achieved with only one detected landmark D Vision system and flight control system integration The integration of the visual system into the UAV control system is presented in Fig 5 It follows the architecture of a dynamic look and move system [22], which is a hierarchical control architecture composed of a fast internal control loop -flight control system- (±20Hz), which is in charge of the helicopter s stability, and an external loop (the vision system), which operates at low frequencies and gives references to the helicopter s control loop The helicopter s control loop is composed of: a State Estimator, which fuses the information from different sensors (GPS, Magnetometers, IMU -Inertial Measurement Unit-) using a Kalman f ilter to determine the position and orientation of the vehicle, and the Flight Controller which is composed of different control loops based on PID controllers (for attitude control, velocity control, position control) that allow different modes of operation, as presented in [23], [5], and [24] Fig 5 Vision-based control In this scheme, a fast internal control loop (flight control system) is in charge of the helicopter s stability, whereas the external loop (vision system) operates at low frequency and uses the visual information as a feedback to the helicopter s flight controller Considering that the vision system will determine the position of the UAV in the World Coordinate System, the adequate references will be given in terms of 3D positions, so that the position x wsetpoint and the position information given by the trinocular system x wuav, both defined in the World Coordinate System, will be compared to generate references to the position controller, as shown in Fig 5 These references are first transformed into commands to the helicopter x hsetpoint by taking into account the helicopter s orientation θ (see Fig 5), and then those references are sent to the position controller in order to move the helicopter to the desired position V EXPERIMENTS AND RESULTS Several experiments were developed with the aim of validating the proposed algorithm regarding feature extraction, tracking and position estimation The program was developed in C + +, and the OpenCV libraries were used for image processing The UAV system and the trinocular platform described in Section II were used for the tests Static tests The first set of experiments consisted in positioning the helicopter in different known 3D coordinates with respect to the World Coordinate System Real values of the UAV positions are obtained measuring them The Root Mean Square Error (RMSE) of the helicopter s position is calculated by comparing the algorithm results with the known position Fig 6 presents the RMSE for different situations As expected, the error in the Y w axis increases as the helicopter gets farther away from the trinocular platform This is so because at those positions the parallax angle within the rays becomes small Errors in Z w and X w axes are satisfactory and constant within a small range, compared with those obtained

in the Y w direction This happens because changes of X w and Y w can be better perceived in the image plane (a) Fig 6 Test results MSE obtained by comparing the pose estimation results of the trinocular system with the known 3D positions at different depths The errors found in these experiments give an idea of the precision of the system, being 5cm for the X w axis, 5cm for the Z w axis, while for the Y w axis, the precision depends on the depth If a landing task has to be achieved, the adequate depth for this task is 3m, whereupon a precision of 0cm will be the one obtained for the Y w axis The precisions that were obtained satisfy the requirements of the tasks we expect to accomplish (landing and positioning) Flight tests The algorithm has been tested during different flights (the video sequences can be found in [25]) An interesting sequence to analyze corresponds to a landing task which has been performed in manual-mode (flown by the pilot) In Fig 7 (figures a,b, and c), the normalized signals of the helicopter s state estimator and the vision system are compared regarding the UAV s position estimation It is important to notice that, in spite of being a manual-mode flight (having thus strong movements), the algorithm follows the landmarks in the different frames and the reconstructed values are consistent with the real movements experimented by the helicopter Analyzing the different signals, the vision system signals (red lines) are consistent with the position of the helicopter shown in the image sequences (the helicopter moves to the left and right, forward and backward, up and down of Camera 2) However, the UAV values (green lines) present different variations at the end of the landing phase that do not correspond with the helicopter s real movements, especially when the helicopter has landed (from frame 6750) Therefore, it is demonstrated that the vision system is not only capable of sensing the different movements adequately, but also that it is capable of sensing small variations in position (±50cm), improving the position estimation in the different axes Another analysis has been done regarding the yaw (θ) values that are estimated when the helicopter has been rotated in different directions In Fig 8, it is possible to see the similar behavior of the signals, obtaining an RMSE of 43 considering the measure given by the helicopter s state estimator as ground truth Algorithm performance Table I presents the execution time for the different stages of the algorithm The image processing stage (as expected) (b) (c) Fig 7 Algorithm results vs helicopter s state estimator The graphics correspond to a manual-mode landing task The vision system s estimation is compared with the UAV s state estimator values and saved images, demonstrating that the vision system not only senses the different helicopter s movements adequately, but also that it is capable of sensing small variations in position (±50cm) Fig 8 Algorithm results vs helicopter s state estimator IIThe signal of the Yaw angle of the helicopter state estimator (green line) is compared with the raw data of the vision-based estimation (red lines) takes the largest CPU time The average time for solving the pose estimation process is 567ms/ f rame, allowing a frequency of 763 f rames/sec, which proves that the algorithm can be used for real-time applications VI CONCLUSIONS AND FUTURE WORK In this paper, we have presented and validated a realtime vision system that estimates the UAV s position and

TABLE I COMPUTATIONAL COST ALGORITHM STAGE TIME (ms/frame) PERCENTAGE Capture 3455 6% Image Processing 59 95% Triangulation 0 0% Other tasks 3 23% Total 567 00% orientation based on information extracted by three cameras located on the ground The algorithm has been tested in indoor and outdoor environments, the latter corresponding to real flight tests The experimental results show that the estimation is consistent regarding 3D real data with approximate errors of ±5cm in X w and Z w axes, and of ±0cm in the Y w axis (at 3m depth), which satisfies the requirements of the tasks we expect to accomplish (landing, takeoff and navigation) The comparison of the information provided by the vision system with that of the UAV s state estimator shows that the vision system improves the position estimation, especially the helicopter s height estimation, whose current accuracy based on GPS is ±05m or lower when the vehicle approaches the ground Additionally, it was possible to see that small variations in position are better perceived with the vision system, which makes this system suitable for maneuvers at low heights and close to structures where precise movements are required Our future work will focus on the implementation of a Kalman Filter, in order to smooth the estimation values It will also aim to close the UAV s control loop with the extracted information in order to perform positioning and landing tasks Additionally, we will study other techniques for feature extraction and matching employing other key points All these new characteristics will be useful for the improvement of the UAV s pose estimation, which is crucial to allow UAVs to operate at low heights and to perform landing, take-off, and other ground-air cooperation tasks VII ACKNOWLEDGMENTS The work reported in this paper is the product of several research stages at the Computer Vision Group of the Universidad Politécnica de Madrid The authors would like to thank Jorge León for supporting the flight trials and the IA Institute of the CSIC for collaborating in the flights consecution We would also like to thank the Universidad Politécnica de Madrid, the Consejería de Educación de la Comunidad de Madrid and the Fondo Social Europeo (FSE) for the Authors PhD Scholarships This work has been sponsored by the Spanish Science and Technology Ministry under grant CICYT DPI 2007-6656 REFERENCES [] Luis Mejias, Srikanth Saripalli, Gauvav Sukhatme, and Pascual Campoy Detection and tracking of external features in a urban environment using an autonomous helicopter In Proceedings of IEEE International Conference on Robotics and Automation, pages 3983 3988, May 2005 [2] Omead Amidi, Takeo Kanade, and K Fujita A visual odometer for autonomous helicopter flight In Proceedings of the Fifth International Conference on Intelligent Autonomous Systems (IAS-5), June 998 [3] S Hrabar, GS Sukhatme, P Corke, K Usher, and J Roberts Combined optic-flow and stereo-based navigation of urban canyons for a uav Intelligent Robots and Systems, 2005 (IROS 2005) 2005 IEEE/RSJ International Conference on, pages 3309 336, Aug 2005 [4] TG McGee, R Sengupta, and K Hedrick Obstacle detection for small autonomous aircraft using sky segmentation Robotics and Automation, 2005 ICRA 2005 Proceedings of the 2005 IEEE International Conference on, pages 4679 4684, April 2005 [5] S Saripalli, JF Montgomery, and GS Sukhatme Visually guided landing of an unmanned aerial vehicle Robotics and Automation, IEEE Transactions on, 9(3):37 380, June 2003 [6] Cui Xu, Liankui Qiu, Ming Liu, Bin Kong, and Yunjian Ge Stereo vision based relative pose and motion estimation for unmanned helicopter landing Information Acquisition, 2006 IEEE International Conference on, pages 3 36, Aug 2006 [7] D Hubbard, B Morse, C Theodore, M Tischler, and T McLain Performance evaluation of vision-based navigation and landing on a rotorcraft unmanned aerial vehicle Applications of Computer Vision, 2007 WACV 07 IEEE Workshop on, pages 5 5, Feb 2007 [8] Peter Corke An inertial and visual sensing system for a small autonomous helicopter Journal of Robotic Systems, 2(2):43 5, 2004 [9] Alison Proctor Allen Wu, Eric Johnson A vision-aided inertial navigation for flight control Journal of Aerospace Computing, Information, and Communication, 2(9), 2005 [0] Glenn P Tournier, Mario Valenti, Jonathan P How, and Eric Feron Estimation and control of a quadrotor vehicle using monocular vision and moir patterns In In AIAA Guidance, Navigation and Control Conference, pages 2006 67 AIAA, 2006 [] Mario Fernando Montenegro Campos and Lúcio de Souza Coelho Autonomous dirigible navigation using visual tracking and pose estimation In ICRA, pages 2584 2589, 999 [2] Erdinç Altuğ, James P Ostrowski, and Camillo J Taylor Control of a quadrotor helicopter using dual camera visual feedback Int J Rob Res, 24(5):329 34, 2005 [3] Gary R Bradski Computer vision face tracking for use in a perceptual user interface Intel Technology Journal, (Q2), 998 [4] E Trucco y A Verri Introductory Techniques for 3-D Computer Vision Prentice Hall, 998 [5] Pascual Campoy, Juan F Correa, Ivan Mondragón, Carol Martínez, Miguel Olivares, Luis Mejías, and Jorge Artieda Computer vision onboard uavs for civilian tasks J Intell Robotics Syst, 54(-3):05 35, 2009 [6] Z Zhang A flexible new technique for camera calibration IEEE Transactions on pattern analysis and machine intelligence, 22():330 334, 2000 [7] Michael J Swain and Dana H Ballard Color indexing Int J Comput Vision, 7(): 32, November 99 [8] K Fukunaga and L Hostetler The estimation of the gradient of a density function, with applications in pattern recognition Information Theory, IEEE Transactions on, 2():32 40, Jan 975 [9] D Comaniciu and P Meer Robust analysis of feature spaces: color image segmentation Computer Vision and Pattern Recognition, IEEE Computer Society Conference on, 0:750, 997 [20] Intel Corporation (200) Open source computer vision library reference manual [2] Jean-Yves Bouguet Camera calibration toolbox for matlab, 2006 [22] Seth Hutchinson, Greg Hager, and Peter Corke A tutorial on visual servo control IEEE Transactions on Robotics and Automation, 2:65 670, 996 [23] Luis Mejias, Srikanth Saripalli, Pascual Campoy, and Gaurav Sukhatme Visual servoing approach for tracking features in urban areas using an autonomous helicopter In Proceedings of IEEE International Conference on Robotics and Automation, pages 2503 2508, Orlando, Florida, May 2006 [24] Luis Mejias Control visual de un vehiculo aéreo autonomo usando detección y seguimiento de características en espacios exteriores PhD thesis, Escuela Técnica Superior de Ingenieros Industriales Universidad Politécnica de Madrid, Spain, December 2006 [25] Computer Vision Group Universidad Politécnica de Madrid COL- IBRI project homepage http://wwwdisamupmes/colibri, 2009