Evaluation of Machine Vision Algorithms for Autonomous Aerial Refueling for Unmanned Aerial Vehicles

Evaluation of Machine Vision Algorithms for Autonomous Aerial Refueling for Unmanned Aerial Vehicles by: Mario Luca Fravolini+, Assistant Professor Giampiero Campa*, Assistant Professor Marcello Napolitano*, Professor Antonio Ficola+, Assistant Professor + Department of Electronic and Information Engineering University of Perugia, 06100 Perugia, Italy * Department of Mechanical and Aerospace Engineering West Virginia University, Morgantown, WV 26506/6106 Corresponding author: Giampiero Campa Department of Mechanical and Aerospace Engineering, PO Box 6106 West Virginia University, Morgantown, WV 26506/6106 Tel. (304) 2934111 Ext. 2313 – FAX (304) 2936689 E-mail: giampiero.campa@mail.wvu.edu Submitted to: AIAA Journal of Aerospace Computing, Information and Communication April 2005 1 Evaluation of Machine Vision Algorithms for Autonomous Aerial Refueling for Unmanned Aerial Vehicles M.L. Fravolini1, G. Campa2 M.R. Napolitano3, A.Ficola4 The use of a combined Machine Vision (MV) and GPS-based approach has been recently proposed in simulation efforts as an alternative approach to ‘pure GPS’ for the problem of Autonomous Aerial Refueling (AAR) for Unmanned Aerial Vehicles (UAVs). While MV has appealing capabilities, a few critical issues need to be addressed for the actual implementation of MV for the AAR problem. For this purpose a simulation environment was developed featuring an interaction with a 3D Virtual Reality (VR) interface that generates an image stream of the AAR maneuver. The image flow is processed by the MV algorithm, providing, as output, a vector of the estimates of the relative tanker-UAV distance and attitude. This signal is then used by the UAV feedback control laws for ‘tracking & docking’ to the refueling boom. The MV algorithm specifically provides image processing for the isolation of optical markers, which are located at specific points on the tanker, extraction of the marker center of gravity, marker matching algorithm, and pose estimation algorithm for the final evaluation of the relative distance vector. Within this effort emphasis was placed on the development of an ‘ad-hoc’ feature matching algorithm followed by a comparative analysis of the performance of different matching algorithms. The paper presents a detailed analysis of the results from open loop and closed loop simulation of the different MV algorithms. 1 Research Assistant Professor, Department of Electronic and Information Engineering, University of Perugia, 06100 Perugia, Italy. 2 Research Assistant Professor, Department of Aerospace Engineering, West Virginia University, Morgantown, WV 26506/6106 3 Professor, Department of Aerospace Engineering, West Virginia University, Morgantown, WV 26506/6106 4 Associate Professor, Department of Electronic and Information, University of Perugia, 06100 Perugia, Italy 2 Nomenclature 3DW AAR CCD CG DFG FM GLSDC j k LQR m mL MV N PE Sij STD VRT 3 Dimensional Window Autonomous Aerial Refueling Charged Coupled Device Center of Gravity Digital Frame Grabber Feature Matching Gaussian Least Squares Differential Correction Index of a generic Marker Sampling time Linear Quadratic Regulator Number of MV detected Markers Number of labeled Markers Machine Vision Number of Physical Markers Pose Estimation Matching Support Standard Deviation Virtual Reality Toolbox I. Introduction One of the biggest current limitations of UAVs is their lack of autonomous aerial refueling (AAR) capabilities. To achieve AAR capabilities specific technical challenges need to be overcome. For AAR purposes, a key issue is represented by the need of a high accuracy measurement of the relative ‘TankerUAV’ position and orientation in the final phase of docking and during the refueling. Although sensors based on laser, infrared radar, and GPS technologies are suitable for autonomous docking [1],the use of MV technology has been recently proposed in addition or as an alternative to these technologies [2 3 4]. Furthermore, a MV-based system has been proposed for close proximity operations of aerospace vehicles [5] and for the navigation of UAVs [6]. The estimation of the 3D orientation and position of an object from its images (pose estimation) is a wellknown topic in Computer Vision research. Within this effort, the ‘Pose Estimation Problem’ has been considered for the purpose of evaluating the relative ‘Tanker-UAV’ position and orientation. The control objective is to guide the UAV within a defined 3D Window (3DW) below the tanker where the boom operator can then manually proceed to the docking of the refueling boom with the UAV fuel receptacle followed by the refueling phase. A digital camera installed on the UAV generates a stream of images of the tanker. These images are then processed by a set of MV algorithms providing in real time an estimate of the tanker-UAV relative position and orientation. This problem is well known in the technical literature as the ‘Pose Estimation’ problem. The set-up of the pose estimation is substantially improved if a number of features can be detected from the digital images of the tanker. For the purpose of this study these features are selected to be optical markers, which can be considered to be active sources of lights. It should be emphasized that this assumption does not subtract from the generality of the approach. In fact, different features could be selected - such as passive source of lights or even tanker geometric corners – without any loss of generality. The image processing algorithms have to be computationally simple, reliable and suitable for real-time implementation. Within the pose estimation algorithm it is assumed that between sequential images a set of corresponding points is always found and available. In other words, it is assumed that the position of the detected markers in the object reference frame is known. 3 It is clear that severe problems may arise when this assumption is violated, leading to potentially biased estimation of the relative position parameters [7]. This problem is typically present when the number of corresponding points (that is, the number of detected markers) is small and time varying due to possible physical occlusions and/or loss of visibility during the approach and docking phases. For example, as the UAV approaches the tanker, the presence of the boom itself could lead to a loss of visibility of one or multiple markers. Therefore, within this effort emphasis has been placed in studying the feature correspondence problem interfaced to the pose estimation algorithm within the AAR problem. A detailed AAR simulation environment has been developed by the authors in [8] with the UAV modeled with the parameters of the ICE-101 aircraft [9] and the tanker modeled with the parameter of a KC135 aircraft[10]. This simulation provides detailed modeling for the elastic flexibility of the boom, the wake effects from the tanker on the UAV, and the atmospheric turbulence. The ‘tracking & docking’ control laws have been designed using a conventional LQR-based approach. With respect to the previous effort by the authors, the simulation environment has been augmented with a Virtual Reality interface providing a stream of images. These images are then used by a MV algorithm for the final derivation of the ‘tanker-UAV’ relative distance vector. The reliability of the pose estimation provided by the MV algorithm, based on different feature matching techniques has been analyzed. Extensive open loop and closed loop simulation results of the various MV algorithms have also been analyzed and discussed. II. The MV-based AAR Problem A block diagram of the MV-based AAR problem is shown in Fig.1. The relevant geometric distances and associated reference frames are also shown in the figure. A. Reference frames The study of the AAR problem requires the definition of the following Reference Frames (RFs): • ERF is the earth-fixed reference frame • TRF is the body-fixed tanker reference frame located at the aircraft center of gravity • URF is the body-fixed UAV reference frame located at the aircraft center of gravity • CRF is the body-fixed UAV camera reference frame. The following notations will be used in this paper. ABe represents the vector from point A to point B expressed within the ERF ; the matrix Rte represents the rotation of the TRF with respect to the ERF. To make the docking problem invariant with respect to the nominal heading of the aircraft, an additional fixed frame MRF is defined. This frame is rotated of the nominal heading angle Ψ0 with respect to the ERF. The transformed vectors in the MRF are evaluated using the relationships: RB m = Rem ⋅ RB e where Rem is a fixed rotation matrix from ERF to MRF. B. Problem formulation The objective is to guide the UAV such that its fuel receptacle (point R in Fig.1) tracks the center of a 3dimensional window (3DW) under the tanker (point B). Once the UAV fuel receptacle reaches and remains within this 3DW, the boom operator is assumed to take control of the refueling operations. It should be underlined that point B is fixed in the TRF with the dimensions of the 3DW (δx,δy,δz) being an important design parameter. It is assumed that the tanker and the UAV share a data communication link. The UAV is equipped with a digital camera along with MV algorithms acquiring the images of the tanker and extracting the point-wise features (points Pj ) from the tanker image. For simplicity, the 2-D image plane of the MV is assumed to be coincident with the ‘x-z’ plane of the CRF. 4 C. Receptacle-3DW-center vector As described above the reliability of the AAR is based on the accuracy in the measurement of the distance RBe. This distance is derived from the available measurements of distance and attitude coming from the UAV and the tanker. For this purpose, the following geometric transformation is introduced: RB e = Rue ⋅ (UC u − UR u + Rcu ⋅ CT c ) + Rte ⋅ TB t (1) Since the fuel receptacle and the 3DW-center are located at fixed and known positions with respect to center of gravity of the aircraft, in the above equation the vectors UR u and TB t are exactly known, while the values of rotation matrix Rte and Rue are derived by the attitude sensors of the tanker and the UAV respectively. The camera-tanker distance ( CT c vector) expressed along the CRF is provided by the MV algorithms. The accuracy of the estimation of the CT c vector will be extensively discussed in this paper. TANKER δy δx δz Pj B T 3D window in TRF B E≡ M= U≡ T≡ C≡ R≡ B≡ pi ≡ CT U C R Center Earth-RF Center ψ0-rotated ERF Center UAV-RF Center Tanker-RF Center Camera-RF Receptacle pos. 3DW-center Marker j M UAV E Fig. 1 Block diagram of the AAR problem with relevant reference frames. III. The virtual environment for AAR simulation A. Tanker and UAV dynamics The aircraft models used for AAR simulation purposes have been developed using the conventional modeling approach outlined in [11]. The resulting model is described by a (12 states) state space model. First order dynamic models have been used for the modeling of the actuators using typical values for aircraft of similar size and/or weight. The B747-tanker and UAV features a typical set of autopilot systems designed using a conventional LQR approach. Details on the design of the tracking and docking control scheme are provided in [8]. The boom system has been modelled using a 3-D finite elements model [12]. The atmospheric turbulence acting on the refueling boom and on both aircraft has been modeled using the Dryden wind turbulence model [13]. A ‘light’ turbulence was selected since aerial refueling is typically performed at high altitudes in calm air. The wake effects of the tanker on the UAV are more significant than the atmospheric turbulence and have been modeled through the interpolation 5 from a large amount of experimental data [14][15] as perturbations to the aerodynamic coefficients cD , cL , cm , cY , cl , cn for the UAV aerodynamic forces and moments. B. The 3D visual interface The modeling described above was linked to a Virtual Reality Toolbox® (VRT) interface to provide the 3D graphics associated with the AAR maneuvers. Specifically, the interface allows the positions of the simulated objects, to drive the position and orientation of the corresponding objects in a Virtual World. Within this Virtual World, optical markers, were located on the B747 tanker model at specific locations, as discussed in [8]. The markers were modeled as red spheres with a radius of 10 cm each. The images of the tanker were then acquired and processed by a ‘virtual’ set of MV algorithms, which in real-life are to be hosted in the UAV flight computer. Fig. 2a shows a typical VRT image as seen by the UAV camera, while Fig. 2b shows the selected positions for the markers. Fig. 2 a) VRT image, b) Position of the optical markers. IV. The MV system The UAV-tanker distance vector CT c is estimated through the MV-based algorithms in the block diagram shown in Fig. 3. This scheme, featuring the components described below, will be referred as the “realistic MV model”: • VRT interface This block provides the continuous image stream associated with the (tanker+UAV) system dynamics generated from the simulation. • Digital Frame Grabber (DFG) This block represents the interface between the simulated world and the MV system. Particularly, it emulates the process of the acquisition of the digital image. The DFG is characterized by the pixel resolution of the camera. In this effort a matrix of 1280 × 1280 pixels has been assumed with the image rate set at 20 frames/sec. The output stream produces images in bitmap format. • Image Processing This block performs several important functions. Digital image processing is applied to extract the ‘point features’ from the stream of the images. First, the image is filtered through the application of a feature detection algorithm. Since red markers have been considered in this study, the detection was performed using a simple ‘red color enhancing filter’ with the goal of isolating the “spots” relative to the red markers. Next, additional image manipulation tools are applied, leading, as final result, to a binary black 6 and white image. Finally, after the image of the markers has been isolated from the background, an ‘ad hoc’ procedure, consisting of morphological closures and openings, is applied to isolate and label each single connected spot. A spot is considered significant only if it contains a number of pixels larger than a pre-defined threshold. All the image manipulations previously described were performed using functions of the Matlab Image Toolbox. • CG Extraction This block computes the Center of Gravity (CG) of each marker projection, so that each marker projection can be represented by a single couple of u and v coordinates. • Feature Matching algorithm The previous ‘CG Extraction’ block generates, for each frame, the 2D coordinates [uj, vj] of the CG for each of the markers, which are detected in that particular frame. At this time, these points are not conceptually associated to any specific marker of the tanker. In fact, in the general case of N detected CGs for N markers, there are N! possible associations between CG points and detected markers to be explored. The above problem is a classical ‘Feature Matching’ (FM) problem. It is clear that this problem has major implications on the closed-loop performance of the docking control laws. The FM algorithms will be described with details in section 5. • Pose Estimation algorithm. Finally, after the set of ‘detected markers’ has been matched to the full set or to a subset, in case of loss of visibility, of the physical markers of the tanker, a ‘Pose Estimation’ (PE) algorithm is used to provide estimates of the UAV-tanker relative distance and pose. The PE algorithm will be described with details in section 6. • MV ‘pin hole’ Camera An ‘ideal’ MV model, as opposed to the ‘realistic’ MV model with the components described above, has been also implemented using a geometric projection model (‘pin hole model’) for performance comparison purposes. In this model the coordinates of the markers in TRF are projected without any error on the MV plane. In this “ideal” MV a nominal (error free) matching has also been assumed for all the detected markers. The PE algorithm is the same as for the ‘realistic’ MV model. The ‘ideal’ MV model is shown in Fig. 3. ‘Realistic’ MV Model Matlab Environment VRT interface Digital Frame Grabber C.G. Image Extraction Processing Pose Feature Matching Algorithm Estimation Algorithm Distance estimate from realistic MV model ‘Ideal’ MV Model Tanker UAV simulator ‘ideal’ MV Pose Pin hole Model Estimation Algorithm Distance estimate from ‘Ideal’ MV model Fig. 3 Components of the ‘realistic’ and ‘ideal’ MV models. 7 V. The ‘Image Matching’ problem Following the extraction of the 2D coordinates [uj vj] of the GCs of the detected markers from the image, the next task is to assign to each of them a particular optical marker Pj on the tanker, as shown in Fig. 4. The general approach is to identify a set of detected markers [uj vj] to be matched to a subset of estimated markers positions [uˆ j vˆ j ] through a projection model (“pin-hole” model [2] [16]) that estimates the projection in the camera plane of the nominal position of the markers Pj . Image plane Marker Camera CP(cj ) Pj C f v TP( j ) CT c u x z T Pi TP(ti ) Marker Object (tanker) Fig.4 The MV “pin-hole” model. c According to this model, a marker ‘j’ with coordinates CP( j ) = [ x cp , j , y cp , j , z cp , j ]T in the CRF frame is projected into the image plane whose coordinates [uˆ j , vˆ j ] can be computed using the projection equation: uˆ j  f c  vˆ  = g f , CP( j ) = c x p, j  j ( )  y cp , j   c   z p , j  (2) where f is the camera focal length. The components of the marker Pj expressed in the TRF are fixed and known ‘a priori’ with coordinates defined by the vector TP( j ) t . The following geometric equation relates c t the vector CP( j ) to the vector TP( j ) : CP(cj ) = CT c + Rtc ⋅ TP(tj ) where the vector CT = [ x t , y t , z t ] and the matrix c c c c T Rtc (3) represents the origin and the orientation of the TRF with respect to the CRF respectively. Matrix Rtc is specified by the relative yaw, pitch, and roll angles T Φ c =  ψ ct , θct , ϕct  . The vector CT c and the matrix Rtc are not directly available but are derived from available measurements. Particularly, the following relationships are used: ( ) CT c = Ruc ⋅ Reu ET e − EU e − Ruc ⋅ UC u (4) Rtc = Ruc ⋅ Reu ⋅ Rte (5) 8 In Eq. (4), vectors ETe and EUe are measured through GPS sensors installed on the tanker and UAV respectively, the attitude matrices Reu and Rte are derived using yaw, pitch, and roll angle measurements of the UAV and tanker respectively, while Rcu is constant and known. Therefore, the accuracy of the estimates for [uˆ j , vˆ j ] is strictly related to the accuracy of the measurements involved in Eq. (4) and (5). For the purpose of achieving realistic simulation results, a nominal level of noise has been ‘injected’ in all the measurements involved in Eq. (4) and (5) (see Table 1). A. Formalization of the ‘Points Matching’ problem Initially, the set of points p j ≡ [u j , v j ] from camera measurements are not related to the actual markers on the tanker. The problem can be formalized in terms of matching the set of points P = ( p1 , p2 ,..., pm ) to the set of points Pˆ = ( pˆ1 , pˆ 2 ,..., pˆ n ) where pˆ j ≡ [uˆ j , vˆ j ] . In this effort the set P represents the set of the m ‘to be matched’ detected markers extracted by the camera measurements, while the set Pˆ represents the set of the n (n=9) “nominal” markers estimated through eq. (2). Since the data sets P and Pˆ represents the 2D projection of the markers at the same time instant on the same plane, an high degree of correlation would be expected. In the ideal case corresponding points would be exactly superimposed and the matching process would be trivial; however, in the presence of different sources of system and measurement noise, a matching problem has to be formalized and solved. In general, a matching problem can be formalized as an optimal pairing of points in the set P to those in the set Pˆ such that the sum of the distances between the paired points is minimized. Fig. 5 shows a correct matching between the two data sets in a typical image from the AAR simulator. 1 • → [u j , v j ] ∗ → [uˆ j , vˆ j ] 2 V-axis 3 4 5 7 6 8 9 U-axis Fig.5: Matching between the labeled set of points Pˆ and the unlabelled set P . B. Approaches to the ‘Points Matching’ problem The mapping between feature points is a key issue in several computer vision problems, such as detecting moving objectives in a scene or calculating motion parameters of the camera with respect to a moving object. A detailed technical literature describes a number of robust matching techniques for point sets [17][18]. Usually, the degree of similarity between 2 data sets is defined in terms of a cost function or a 9 distance function derived on general principles as geometric proximity, rigidity, and exclusion [19]. These constraints can be divided in local constraints, which can be evaluated independently for each possible matching, and global constraints, which need the joint analysis of a set o potential matching [20]. The best matching is then evaluated as the result of on optimization process exploring the space of the potential solutions. As later described, a graph can also be derived such that the nodes represent feature points and links represents the distance between features; therefore, the problem can be cast as a classical assignment problem. The assignment problem is a classic example of a network flow problem on a graph. Network flow problems are Linear Programming (LP) problems for which computationally efficient algorithms (that exploit the particular structure of those problems) are routinely used. In [21] a comparison of different methods is shown. Interesting relaxation techniques are shown in [22 23]. Other approaches based on ‘registration’ and ‘tracking’ are shown in [24]. These approaches are often augmented by a feedback from the calculated motion parameters using matching evaluated in the previous frames. In the AAR problem it was observed that the discrepancy between the two matching sets can be reasonably modeled by assuming a simple rigid translational model. Under this assumption the measure of the matching strength between point pi and pˆ j has been defined in terms of their Euclidean distances. The distance Matrix D is defined as the matrix whose entries D(i, j ) are the distances d ij between each couple of points ‘i’ and ‘j’ of the two sets. The performance of a few well-known ‘Point Matching’ algorithms have been evaluated and compared in the context of MV-based AAR. Furthermore, a new ‘ad-hoc’ matching method has been here formulated, where the strength of a candidate match is measured not just in terms of the relative distance between two candidate matching points pi and pˆ j but also in terms of a global matching error between all the matching points that would have been originated by applying the rigid translation associated with the two matching points pi and pˆ j . C. The bipartite graph A weighted bipartite graph – shown in Figure 6 - is a weighted graph whose vertex set can be divided into two disjoint sets. In this study the two sets are represented by P and Pˆ . In the bipartite graph each vertex in P is connected to all the vertex in Pˆ trough edges; the weight d ij associated to the edge ( pi , pˆ j ) is the Euclidean distance between point pi and point pˆ j . The matrix D(i, j ) therefore contains information about all the edges in the graph. A valid matching in this graph is defined as a subset M of η edges ( η = min( n, m ) ) such that each vertex of the graph is incident upon at most one edge of M. Given a subset M, its weight is defined as the sum of all the η weights of M. If an edge ( pi , pˆ j ) belongs to M, then vertex pi is said to be matched to vertex pˆ j and vice versa. A matching with a minimum weight is said to be a minimum weight matching or simply a minimum matching. A specific algorithm, called the Hungarian method, has been introduced for finding a minimum weight matching in a weighted bipartite graph. This algorithm has a complexity of O(n3). A detailed description of this algorithm can be found in [25]. The important aspect of this method is that this algorithm is always guaranteed to find the minimum weight matching. It should be emphasized that, although the Hungarian method always provides a minimum weight matching, it is not guaranteed that the provided matching is indeed correct. 10 p1 p̂1 d1,2 p2 p̂2 • • • pm • • • pˆ n d m, n Pˆ P Fig. 6 Bipartite Graph associated to sets Pˆ and P . D. The greedy method Within this method a greedy algorithm, outlined in [25], is used for solving the points correspondence problem. Given the graph distance matrix D(i, j ) , the set of edges is explored to find the edge ( pi , pˆ j ) with the smallest weight (distance d ij ). The point pi is matched to the point pˆ j ; next, all the edges associated with pi and pˆ j are removed from further consideration. The above step is repeated until every point in P is matched to some point in Pˆ . Although very simple and efficient, this algorithm does not always produce a matching of minimum weight; furthermore, it is possible for the algorithm to get stuck in a local minimum [26]. To improve the robustness of this method and to facilitate the matching, a “center of gravity correction” is preventively applied to the points of set Pˆ , to have the CG of the “translated” Pˆ set coincident with the CG of the data set P . To avoid inconsistent translations, this operation is only applied to the frames where the number of points in P and Pˆ coincide. E. Mixed Hungarian and greedy method This method is essentially the fusion of the two previous methods. Therefore, a point matching ( pi , pˆ j ) is considered valid if and only if the same matching is confirmed by both methods. The points pi for which the two methods do not provide consistent results are considered unreliable and, therefore, discarded by the algorithm. F. The proposed robust matching method A critical issue is the robustness of the matching algorithm in presence of measurement errors affecting the estimation of the position of markers set Pˆ . Therefore, some specific features were introduced with the objective of increasing the robustness of the matching algorithm. Before describing the resulting algorithm, the following definitions are introduced: • Matching Support. It is assumed that the main effect associated with the presence of the (measurement and system) noise is a rigid translation between the two matching sets. Under this assumption, for each candidate match ( pi , pˆ j ) , the vector tij = ( pˆ j − pi ) is assumed as a possible rigid 11 translation vector, due to the presence of the noise, from points in the set P to points in the set Pˆ . After the application of the rigid translation tij to all the points in P , a generic translated point ph + tij is directly matched to the nearest point pˆ kh in Pˆ according to the following “winner takes all” method: ( kh = arg min pˆ k − ( ph + tij ) k ) k = 1,..., n (6) The corresponding matching error is then given by: eh = pˆ kh − ( ph + tij ) k = 1,..., n (7) Repeating this procedure for all the m points in P , it is then possible to define a vector of possible matching K ij = [k1 ,...km ] associated to the translation tij. In the event case that two or more elements in K ij receive the same index, only the association with the smallest matching error is considered valid while the remaining are deleted because their matching is considered unreliable. Therefore, the number of detected and labeled mL markers is in general less or equal to m. The matching support for the association set K ij is then defined as: Sij = 1 mL mL ∑e (8) h h =1 The cost Sij can then be considered a measurement of the robustness for the candidate match K ij , since it takes into account the overall mean matching error between the two data sets. The best matching K ij * is then selected as the one providing the minimum value of the matching support ( Sij* ) evaluated among all the possible translations tij: * Sij = min ( Sij ) i = 1,...n j = 1,...m (9) i, j Additional features could be introduced into the matching algorithm. For example robustness could be increased by estimating marker motion parameters in successive frames and using this information to predict the future position of a labeled marker. VI. The real-time pose estimation problem After the ‘Point Matching’ problem has been solved, the information in the set of points P are then used to derive the rigid transformation that relates the CRF to TRF . This topic is well-known in the technical literature as the ‘Pose Estimation’ (PE) problem [27,28]. Particularly, the objective is to evaluate the mapping correspondences between the 3-D physical points (markers) expressed in the object coordinates (TRF) and their 2-D projections expressed in image ‘u-v’ coordinates (CRF). For an arbitrary number of points, the most extensively used approach for the PE is based on the application of a non-linear leastsquares algorithms reducing itself to the minimization of a non-linear cost function, typically solved iteratively using some variations of the Gauss-Newton method [27,29,30,31]. In particular, the Gaussian Least Squares Differential Correction (GLSDC) algorithm has been implemented in this study [2]. This algorithm has shown to provide desirable robustness, convergence, and accuracy even in presence of quantization noise produced by the CCD matrix. The non-linear 3-D to 2-D correspondence is described in terms of the projection equation already introduced in Eq. (2). The unknowns of the problems are the components of the vectors CT c and Φ c in Eq. (3). For simplicity purposes these unknown have been grouped in a vector X = [ xtc , ytc , ztc ,ψ tc ,θtc ,ϕtc ]T ; this vector defines uniquely the relative distance and attitude of the UAV camera fixed CRF respect to the TRF. 12 A. The Pose Estimation algorithm Prior to describing the algorithm the 3-D to 2-D perspective projection equations relating the unknown t vector X to the known vector TP( j ) are recalled below: t u j   gu ( f , X , TP( j ) )   j = 1,..., mL (10)  = t  v j   g v ( f , X , TP( j ) )  By grouping the equations (10) for all the ‘mL’ labeled markers, the following 1×2mL vector of non-linear relationships is then generated: G =  gu1 , g v1 ,..., gumL , g vmL  T (11) Next, the MV estimation error at sampling time k is calculated as: t ∆G(k ) = Gmeas (k ) − G f , Xˆ (k ), TP( j ) (k ) ( ) (12) ˆ where Gmeas (k ) is the vector of the measured coordinates of the markers on the image plane and X (k ) is the current estimation of vector X (k ) . The updated estimation Xˆ (k + 1) is provided through the application of the GLSDC algorithm: P(k ) = AT (k ) ⋅ W ⋅ A(k ) ∆Xˆ (k ) = P −1 (k ) ⋅ AT (k ) ⋅ W ⋅ ∆G (k ) (13) Xˆ (k + 1) = Xˆ (k ) + ∆Xˆ (k ) where A(k) is the 2mL×6 Jacobian matrix: dG (k ) A(k ) = dX (k ) X = Xˆ ( k ) (15) (14) (16) and W is the 2mL×2mL covariance matrix W = diag (1/ σ u1 ,1/ σ v1 ,...1/ σ vmL ,1/ σ vmL ) of the estimation error. The initial guess Xˆ (k = 0) is computed using the information provided by GPSs and aircraft sensors. B. Modification of the GLSDC The basic algorithm (13-16) is designed to work with a fixed number of ‘mL’ markers. The following set of modifications has been introduced for handling a time varying number of markers. At the beginning of each time step the number of the labeled markers is evaluated; their number is, in general, time varying because of temporary occlusions (with the markers remaining undetected) or simply because the markers do not receive a valid label from the matching algorithm. This implies that (11) has to be modified with the appropriate number of rows; next, the dimensions and the values of the matrices A and W in (13-16) also need to be revised accordingly. It should be emphasized that a minimum of 3 markers are required by the algorithm to generate a compatible solution. If more than 3 markers are available the algorithm provides a ‘Least Square’ solution. The improvements associated with the described modification of the GLSDC algorithm will be evident in the simulations described below. VII. Analysis and results The goal of the study was to analyze the overall performance provided by the different feature labeling schemes in conjunction with the DGLS pose estimation algorithm applied to the AAR problem. A detailed set of simulation studies was performed. During the final docking phase (from the pre-contact to contract position) the UAV and the Tanker are aligned along the same heading angle. It is assumed that the UAV fuel receptacle and the on-board camera are both perfectly aligned with the UAV longitudinal axis. A simple LQR-based approach has been used for the design of the docking control laws [8]. 13 The time histories of the components of the CT c vector during the AAR maneuver are shown in Fig. 7. In the first set of simulations the purpose was to evaluate the accuracy of the estimates of the MV-based sensors during the docking; therefore, in this case the measurements provided by the MV-based sensor are not used by the control laws, to which ‘nominal’ noise-free measurements are provided. In the second set of simulations the MV-based measurements of the CT vector were provided directly to the control laws. For simulation purposes, ‘nominal’ levels of uniformly distributed white noise were artificially added to all the measurements involved in Eqs. (4) and (5). It should be emphasized that the presence of measurement errors is the main reason for the discrepancy in the point matching which has been observed in Fig. 5. Different simulation parameters along with statistical information on the noise acting on sensors, GPS and MV systems are reported in Table 1. Heading angle: Ψ 0 = 0 Known distance vectors: UCu =[-3,0,-0.6] m URu =[-1,2.5,0] m TBt =[-22,0,8] m CTC(0)=[ 125 0 30] m o Table 1: Simulation parameters MV sampling rate: 20 Hz MV pixel size: 10 µm GPS noise-std: 1.5 m CCD size: 1.28 cm GPS sampling rate: 10 Hz V noise-std: 1.5 m/s α,β noise-std:0.0045 rad p,q,r noise-std: 0.015 rad/s ψ,θ,ϕ noise-std: 0.007 rad Transmission delay: 0.05 s 14 Dryden Model at light conditions Wake effects (Bihrle experimental data) c X-Y-Z Components of CT 120 x-axis 100 80 [m] 60 40 z-axis 20 y-axis 0 0 10 20 30 40 50 60 70 80 Time [s] Fig.7 Trajectory of the CT c components during the docking maneuver ( CT c is the distance between the UAV camera and the tanker’s center of gravity ). A. Performance of the MV system Specific criteria for the selection of the ‘best’ location for the markers were not available in the technical literature. Therefore, an empirical selection of the number of markers and their location was introduced. The main criterion was to maximize their visibility from the location of the camera on the UAV. Another criteria was to avoid the installation of the markers at specific locations on the aircraft subjected to structural vibrations (for example, wing tips). An additional criterion was to avoid the installation of the markers close to the wake of the jet engines. Another criterion was to avoid the installation of the markers in any location where their accessibility could be problematic. A final criterion was to select locations identifying multiple planes along the x-axis of the tanker allowing, therefore, adequate levels of resolution for the acquisition of an accurate UAV-Tanker longitudinal distance. At the end of this analysis, a set of 9 markers was selected; their location is shown in Fig.2. This set provided sufficient redundancy and exhibited desirable robustness in the event of temporary/permanent loss of visibility by one or more markers, as described below. The effects of the camera digital resolution on the estimation error and the effects of uncertainties of the markers positions on the tanker have been described in [32]. B. Performance of the Estimation system As stated above, in this first study the MV-based estimates are not provided to the docking control laws. The norm of the measurement error eMV = CT c − CT(cmv ) was evaluated to quantify the error: eMV = ( CT c x − CT(cmv , x ) ) + (CT 2 c y − CT(cmv , y ) ) + (CT 2 c z − CT(cmv , z ) 15 ) 2 (17) where CT c is the ideal signal and CT(cmv ) is the corresponding provided by the MV. The following 5 estimation algorithms for the evaluation the tanker/UAV relative distance vector have been compared: A) “Ideal MV” model, implying a ‘perfect labeling’ algorithm with an ‘ideal MV’ featuring ideal pointwise markers without physical occlusions. The DGLS algorithm is then used for the solution of the ‘pose estimation’ problem. The purpose of this scenario is to provide a benchmark. B) “Realistic MV model B”, including ‘digital frame grabber’ and CG extraction procedures. This scheme features the ‘robust labelling method’, described above, and the DGLS algorithm for the pose estimation’ problem. C) “Realistic MV model C”: as in case B) with the ‘Hungarian’ labelling method in lieu of the ‘robust labelling method’. D) “Realistic MV model D”: as in case B) with the ‘Mixed’ labelling method in lieu of the ‘robust labelling method’. E) “Realistic MV model E”: as in case B) with the ‘Greedy’ labelling method in lieu of the ‘robust labelling method’. C. Analysis of the eMV signal Different aspects have been evaluated and compared. Fig. 8 shows the norm of the measurement error eMV vs. the norm of the distance RB m for the 5 scenarios outlined above. The data are relative to a complete docking maneuver lasting 300 s. As described above, scenario A) provides the benchmark performance. For the more realistic scenarios B, C, D, and E, the value of eMV is the sum of the 3 specific contributions. The first source of error is due to the quantization error associated with the digital camera. This error is more relevant at large distances and it is responsible of a biasing term (function of the distance) in the estimation. This effect becomes negligible at short distances where the estimation is nearly unbiased. The second source of error is due to possible incorrect labeling in the matching algorithm. This in turn implies inconsistent data to the GLSDC algorithm. This ‘labeling error’ is responsible for the large spikes for the estimations in scenarios C, D, and E. Furthermore, in the event that labeling errors occur in a continuous sequence of frames, the problem could lead to closed-loop instability. The third source of error is associated with incorrect CG extraction procedure and it is mainly responsible of the high frequency chattering in the eMV signal. Of particular interest in Fig. 8 is the region (on the right) associated with the condition RB m <5 m. Since the simulation is performed for 300 s., the condition RB m <5 m describes essentially the condition when the UAV is in the 3D refueling window under the tanker. In that particular condition the loss of visibility of multiple markers is possible. This would then lead to deterioration of the performance of the matching algorithm causing the spikes associated with the scenarios C, D, and E. This issue is even clearer from an analysis of Fig. 9 where eMV is shown as function of time (in the [3585] s. window). This specific time interval is particularly relevant because it includes the final phase of the docking when the UAV enters and remains in the 3D refueling window below the tanker. Up to approximately 45 s (that is, when the UAV is still approaching to the tanker) a decrease in the eMV is noted for all the scenarios, with the exception of the ‘ideal’ and ‘perfect’ MV-based algorithms in Scenario A. Starting at approx. 45 s the error has a tendency to converge toward certain bounds. However, the scenarios C, D, and E present a large number of spikes, implying a lack of robustness for the associated labeling algorithms. Only scenario B shows reduced level of error throughout the simulation due to the desirable performance of the matching algorithm. D. Influence of the variable number of the visible and labeled markers 16 As previously described, the loss of visibility of one or more markers by the UAV camera might occur during the docking. If this occurs, the modified GLSDC algorithm has shown capabilities for adapting itself and continuing the estimation with the subset of the labeled markers available at that particular time step. Fig. 10 shows the time histories of the number of the labeled markers, which can be up to the number of visible markers, used by the MV for the 5 scenarios for the same time interval as in Fig. 9. For the ‘ideal’ case of scenario A all the markers in the camera field of view are considered visible and correctly labeled. Starting from the value ‘mL=9’, the reduction of the number of labeled markers is only due to loss of visibility due to physical occlusions (such as interference of the refueling boom). For the scenarios B and C the performance of the labeling algorithms in terms of mL seem to be equivalent. A close cross-analysis of Fig. 9 and 10 reveal that the differences between scenario B and C in terms of mL (Fig.10) are responsible for the spikes in the estimation error in case C (Fig.9). The performance of the labeling algorithms in terms of mL seem to be substantially deteriorated for scenarios D and E; even in this case it can be seen that the ‘drops’ in the value of mL shown in Fig. 10 are associated with the spikes of the error shown in Fig. 9. Norm of the estimation error |eMV |=|CTc -CT(mv)| 3 2 1 a 0 -40 3 -35 -30 -25 -20 -15 -10 -5 0 -35 -30 -25 -20 -15 -10 -5 0 -35 -30 -25 -20 -15 -10 -5 0 -35 -30 -25 -20 -15 -10 -5 0 -35 -30 -25 -20 -15 -10 -5 0 2 1 b [m] 0 -40 3 2 1 c 0 -40 3 2 1 d 0 -40 3 2 1 0 -40 e distance |RBm| Fig. 8 eMV error as function of the distance RB m . 17 Norm of the estimation error |eMV |=|CTc -CTc(mv)| 1 0.5 a 0 35 1 40 45 50 55 60 65 70 75 80 85 40 45 50 55 60 65 70 75 80 85 40 45 50 55 60 65 70 75 80 85 40 45 50 55 60 65 70 75 80 85 40 45 50 55 60 65 70 75 80 85 0.5 b [m] 0 35 1 0.5 c 0 35 1 0.5 d 0 35 1 0.5 0 35 e Time [s] Fig. 9 eMV error as function of time. 18 Number of Labeled Markers mL 10 a 5 35 10 40 45 50 55 60 65 70 75 80 85 b 5 35 10 40 45 50 55 60 65 70 75 80 85 c 5 35 10 40 45 50 55 60 65 70 75 80 85 d 5 35 10 40 45 50 55 60 65 70 75 80 85 e 5 35 40 45 50 55 60 65 70 75 80 85 Time [s] Fig. 10 Number of labeled markers in the interval [35-85] s (used by the DGLS algorithm). E. Quantitative estimation of performance A quantitative analysis was performed separately for two specific phases within the AAR maneuver, that is for the docking maneuver (the first 45 s.) and for the time when the UAV is located within the 3D refueling window below the tanker (between 45 and 300 s.) associated with the condition RB m <5 m. The need to separate the analysis is due to the fact that in these two phases the MV estimation accuracy is clearly different. For each of the 5 scenarios described above the performance have been compared in terms of the statistics of the measurement error eMV . Thus, the mean, the Standard Deviation (SD), the maximum value and the mean number of labeled markers were evaluated. Table 2 shows the results for all the scenarios with scenario B being considered the ‘baseline’ scenario. As stated above, scenario A is just an ideal condition. From the analysis of the results in Table 2, a significant difference is evident between the performance of the ‘baseline’ scenario B and the performance of the scenarios C,D, and E, especially for the phase when the UAV has reached the 3D refueling window. Particularly, the performance degradation is clearer in terms of the standard deviation and maximum value of the error. In terms of physical units, for the scenario B the mean value of eMV is approx. 18 cm with a STD less than 6 cm and with a peak less than 35 cm. 19 Table 2 Overall Estimation performance of the 5 MV system. 3DW DOCKING c c Performance: eMV = CT c − CT(cmv ) [m] Performance: eMV = CT − CT( mv ) [m] mean ( eMV ) std ( eMV ) A B C D E -0.084 (11%) -0.709 (100%) -0.677 (95%) -0.644 (90%) -0.102 (14%) max ( eMV 0.049 (6%) 0.741 (100%) 0.733 (99%) 0.755 (102%) 1.411 (190%) ) 0.200 (8%) 2.479 (100%) 2.621 (105%) 3.466 (139%) 7.788 (314%) Nmark mean ( eMV ) std ( eMV ) 7.974 (107%) 7.466 (100%) 7.466 (100%) 6.067 (81%) 6.728 (90%) 0.003 (2%) 0.181 (100%) 0.168 (92%) 0.176 (97%) 0.191 (106%) max ( eMV 0.010 (20%) 0.051 (100%) 0.397 (794%) 0.084 (168%) 0.889 (1778%) ) 0.032 (9%) 0.341 (100%) 5.090 (1492%) 1.054 (309%) 8.348 (2448%) Nmark 6.00 (106%) 5.68 (100%) 5.70 (100%) 5.03 (89%) 5.30 (93%) F. Closed loop performance In the final simulation the estimates from the MV-based algorithms were provided as feedback to a set of LQR-based docking control laws for final evaluation of the closed-loop performance [8]. Given the unsatisfactory performances of the algorithms in the scenarios C and E, the analysis was limited to the scenarios B and D. Although the MV estimation mechanism is clearly not dependent on the signals employed by the feedback controller, it is nevertheless of particular interest the comparison of the performance achieved when the MV is not in the feedback loop and the controller is driven by ‘nominal’ noise-free measurements (Table 2), and the performance achieved by using the MV-based measurements of the CT vector in the feedback loop. The results are shown in Table 3. An analysis of this table reveals that for scenario B the performance were fairly comparable for both cases. Some limited but significant differences between the two cases exist for scenario D. The origin of these differences is traced back to the spikes in the estimation error signal shown in Figure 7. Another important conclusion from the analysis of the results in Table 3 is that both matching algorithms in the scenarios B and D provide closed-loop robustness to temporary loss of visibility of the markers by the UAV camera. Table 3 Closed-loop performance. DOCKING 3DW c c Performance: eMV = CT − CT( mv ) [m] Performance: eMV = CT c − CT(cmv ) [m] mean ( eMV ) std ( eMV ) B MV in the Loop B no MV in the Loop D MV in the Loop D no MV in the Loop max ( eMV ) Nmark mean ( eMV ) std ( eMV ) max ( eMV ) Nmark -0.6491 (91%) 0.7380 (99%) 2.5558 (103%) 7.5383 (116%) 0.17947 (99%) 0.04973 (96%) 0.33073 (97%) 5.8563 (103%) -0.7090 (100%) 0.7410 (100%) 2.4790 (100%) 7.466 (100%) 0.1810 (100%) 0.0510 (100%) 0.3410 (100%) 5.6880 (100%) -0.6053 (85%) 1.0006 (34%) 7.8360 (318%) 7.8360 (104%) 0.18961 (104%) 0.062856 (123%) 0.39673 (116%) 5.6378 (99%) -0.6440 (90%) 0.7550 (102%) 3.4660 (139%) 6.0670 (81%) 0.1760 (97%) 0.0840 (168%) 1.0540 (309%) 5.0310 (89%) 20 VIII. Conclusions This paper describes the comparison between different feature matching algorithms within a simulation environment specifically built for the study of the MV-based Autonomous Aerial Refueling problem. In this study the attention was mainly focused on the performance, accuracy and robustness of the whole MV scheme when featuring different feature matching techniques. This analysis reveled a very high sensitivity of the pose estimation algorithm on possible errors in the feature matching algorithms. It was also shown that, assuming that the matching algorithms perform nominally, the overall closed-loop performance do not seem to be very sensitive to temporary/complete loss of visibility for some of the physical markers, provided a sufficient redundancy in their number. Furthermore the MV accuracy in the estimation of distance vectors seems to be appropriate given the size of a typical 3D refueling window. Finally, the proposed matching algorithm, which is based on a rigid translation model and on the measure of matching support (derived by the overall mean matching error between the two matching sets) has shown desirable robustness properties from extensive simulations at open-loop and closed-loop conditions. 21 References 1 Korbly, R. and Sensong, L. “Relative attitudes for automatic docking,” AIAA Journal of Guidance Control and Dynamics, Vol. 6, No. 3, 1983, pp. 213-215. 2 Kimmett, J., Valasek, J., and Junkins, J.L., “Autonomous Aerial Refueling Utilizing a Vision Based Navigation System,” Proceedings of the 2002 AIAA GNC Conference, Paper 2002-4469, Monterey, CA, August 2002 3 Fravolini, M.L. Ficola, A., Napolitano M.R., Campa, G., Perhinschi, M.G., “Development of modelling and control tools for aerial refuelling for UAVs”, Proceedings of the 2003 AIAA GNC Conference, Paper 2003-5798, Austin, TX, August 2003 4 Pollini, L., Campa, G., Giulietti, F., Innocenti, M., “Virtual Simulation Set-up for UAVs Aerial Refueling,” Proceedings of the 2003 AIAA Conference on Mmodeling and Simulation Technologies and Exhibits, Paper 2003-5682, Austin, TX, August 2003 5 Philip, N.K., Ananthasayanam, M.R., “Relative Position and Attitude Estimation and Control Schemes for the Final Phase of an Autonomous Docking Mission of Spacecraft,” Acta Astronautica, Vol. 52, 2003, pp. 511-522. 6 Sinopoli, B., Micheli, M., Donato G., Koo T.J., “Vision Based Navigation for an Unmanned Aerial Vehicle”, Proceedings of the IEEE International Conference on Robotics and Automation, Seoul, South Korea, 2001, vol.2 pp. 1757-1764. 7 Pla, F., Marchant, J.A., “Matching Feature Points in Image Sequences through a Region-Based Method,” Computer vision and image understanding, Vol. 66, No. 3, 1997, pp. 271-285. 8 Fravolini, M.L., Ficola, A., Campa, G., Napolitano M.R., Seanor, B., “Modeling and Control Issues for Autonomous Aerial Refueling for UAVs Using a Probe-Drogue Refueling System,” Journal of Aerospace Science Technology, Vol. 8, No. 7, 2004, pp. 611-618. 9 Addington, G.A., Myatt, J.H., “Control-Surface Deflection Effects on the Innovative Control Effectors (ICE 101) Design,”Air Force Report, AFRL-VA-WP-TR-2000-3027, June 2000 10 Air Force Link, online database: http://www.af.mil/factsheets 11 Stevens, B.L., Lewis, F.L., “Aircraft Control and Simulation,” John Wiley & Sons, New York, 1987. 12. Spong, M.W., Vidyasagar, M., “Robot Dynamics and control,” Wiley, New York, 1989. 13 Roskam J. “Airplane Flight Dynamics and Automatic Flight Controls – Part II”, DARC Corporation, Lawrence, KS, 1994. 14 Blake, W., Gingras, D.R., “Comparison of Predicted and Measured Formation Flight Interference Effects”, Proceedings of the AIAA Atmospheric Flight Mechanics Conference, AIAA Paper 20014136, Montreal, Canada, August 2001 15 Gingras, D.R., Player, J.L., Blake, W., “Static and Dynamic Wind Tunnel testing of Air Vehicles in Close Proximity”, Proceedings of the AIAA Atmospheric Flight Mechanics Conference, AIAA Paper 2001-4137, Montreal, Canada, August 2001 16 Hutchinson. S., Hager, G., Corke, P., “A tutorial on visual servo control”, IEEE Transactions on Robotics and Automation, Vol. 12, No. 5, 1996, pp. 651-670. 17 Umeyama, S., “Parameterized point pattern matching and its application to recognition of object families,” IEEE Transactions on Pattern Analysis and Machine Intelligence, Vol.15, No.2, 1993, pp.136-144 22 18 Kim,W., Y, W., Kak, A.C., “3-D object recognition using bipartite matching embedded in discrete relaxation,” IEEE Transactions on Pattern Analysis and Machine Intelligence, Vol. 13 , No. 3, 1991, pp.224-251. 19 Pla, F., Marchant, J.A., “Matching Feature Points in Image Sequences through a Region-Based Method,” Computer vision and image understanding, Vol. 66, No. 3, 1997, pp. 271-285. 20 Ferruz, J., Ollero, A., “Real-Time Feature Matching in Image Sequences for Non-Structured Environments. Applications to Vehicle Guidance,” Journal of Intelligent and Robotic Systems, Vol. 28, No. 1-2, 2000, pp. 85-123. 21 Borrow, H.G., Ambler, A.P., Burstall, R.M., ”Some techniques for recognizing structures in pictures,” Frontiers of pattern recognitions, Academic Press, New York, 1972, pp. 1-29. 22 Rosenfeld, A. Himmel, R.A., Zucker S.W., “Scene labelling by relaxation operators”, IEEE Trans. Syst. Man Cybernetic, SMC-6, 1976, pp.420-433. 23 Strickland R.N., Mao, Z., “Computing correspondence in a sequence of non rigid objectives,” Pattern Recognition, Vol. 25, No. 9, 1992, pp. 901-912. 24 Zhang Z., “On local matching of free form curves,”, Proceedings of British Machine vision Conf. 1992, pp. 345-356. 25 Kumar, S., Sallam, M., Goldgof, D.,“Matching point features under small nonrigid motion,” Pattern Recognition, Vol. 34, 2001, pp. 2353-2365. 26 Zhang, Z., Deriche, R., Faugeras, O., Luong, Q.T., “A robust technique for matching two uncelebrated images through the recovery of the unknown equipolar geometry, Artificial Intelligence Vol. 78, 1995, pp. 87-119. 27 Harlalick, R.M et al., “Pose Estimation from Corresponding Point Data”, IEEE Transactions on Systems, Man, and Cybernetics, Vol. 19, No. 6, 1989, pp. 1426-1446. 28 Wilson, W., “Visual Servo Control of Robots Using Kalman Filter Estimates of Robot Pose Relative to Work-Pieces,Visual Servoing,” K. Hashimoto, ed., World Scientific, 1994, pp. 71-104. 29 Haralick, R.M. and Shapiro, L.G., “Computer and Robot Vision”. Reading, Mass.: AddisonWesley, 1993. 30 Lowe, D.G., “Three-Dimensional Object Recognition from Single Two-Dimensional Image,” Artificial Intelligence, Vol. 31, 1987, pp. 355-395. 31 Lu, C.P., Hager, G.D., Mjolsness, E., “Fast and Globally Convergent Pose Estimation from Video Images,” IEEE Transactions On Pattern Analysis and Machine Intelligence, Vol. 22, No. 6, 2000, pp. 610-622. 32 Campa, G., Fravolini, M.L, Ficola, A., Napolitano, M.R., Seanor B., and Perhinschi, M., “Autonomous Aerial Refueling for UAVs Using a Combined GPS-Machine Vision Guidance”, Proceedings of the AIAA Guidance, Navigation, and Control Conference, AIAA paper 2004-5350, Providence, Rhode Island, August 2004. 23 View publication stats