

small (250x250 max)
medium (500x500 max)
Large
Extra Large
large ( > 500x500)
Full Resolution


VISIONBASED CONTROL OF MULTIAGENT SYSTEMS By OMAR ARMANDO ADRIÁN ORQUEDA Electronic Engineer Universidad Nacional del Sur Bahía Blanca, Argentina 1996 Doctor on System Control Universidad Nacional del Sur Bahía Blanca, Argentina 2006 Submitted to the Faculty of the Graduate College of the Oklahoma State University in partial ful llment of the requirements for the Degree of DOCTOR OF PHILOSOPHY December, 2006 VISIONBASED CONTROL OF MULTIAGENT SYSTEMS Dissertation Approved: Prof. Martin Hagan Committee Chair Prof. Rafael Fierro Thesis Adviser Prof. Carlos Oliveira Prof. Weihua Sheng Dr. Gordon Emslie Dean of the Graduate College ii To my children Paulina and Tomás. iii Acknowledgements It has been a long journey since I started doing research. During this journey, I have had the pleasure of working with several researchers and learning a lot from all of them. Consequently, the list of acknowledgements is very extensive. Nevertheless, I would like to express my special gratitude to two professors from my home univer sity, Alfredo Desages (in memoriam) and Sylvia Padin. Both strongly in uenced my decision for doing research when still I was an undergraduate student. I would like to thank my advisor Dr. Rafael Fierro for his support and encour agement. In particular, I am grateful for the freedom and exibility I had to look for an approach on formation control in which I was especially interested. I am also extremely thankful for giving me the opportunity of participating in this project at Oklahoma State University. I would like to thank the members of my thesis committee Profs. Hagan (chair), Oliveira, and Sheng, for reading my dissertation and suggesting very constructive comments which help improve the contents of this work. I would like to thank Dr. Tony Zhang for his friendship and helpful discussions on the rst formulation of the output feedback controller presented in Chapter 5. Many thanks to current and past members of Marhes lab, James, Brent, Carlo, Feng, Eric, Ugur, Colby, Kyle, Chris, Lorne, Justin, Kenny, José, my friends in Stillwater and in Argentina. I would like to thank my family, specially, my two children Paulina and Tomás. They encourage me everyday to do my best work. Last but not least, I gratefully acknowledge the sources of my nancial support: This research was supported by the National Science Foundation (NSF) under grants #0311460 and CAREER #0348637, and by the U.S. Army Research O ce under Grant DAAD190310142 (through the University of Oklahoma). iv Preface Creating systems with multiple autonomous vehicles places severe demands on the design of cooperative control schemes and communication strategies. In last years, several approaches have been proposed in the literature. Most of them solve the vehicle coordination problem assuming some kind of communications between team members. Communications are used either to obtain information about neighbors' velocities or relative positions, or to acquire the control law to apply. In the former case, the control computation is decentralized and the later the control is central ized. However, communications make the group sensitive to failure, or restrict the applicability of the controllers to teams of friendly robots. This dissertation is a step forward toward the design of sensorbased decentralized architectures for stabilization of unmanned vehicles moving in formation. The archi tecture consists of two main components: (i) a modelbased vision system, and (ii) a control algorithm. The modelbased vision system can recognize and relatively lo calize neighbor robots using ducial markers on a truncated octagon shape mounted on each vehicle. It is composed of three main components: image acquisition and processing, robot identi cation, and pose estimation. The control algorithm uses vision information to stabilize a group of mobile robots in formation. Two main stabilization problems are addressed: stabilization in leader follower and in twoleaderfollower formations. Several control strategies using rela tive pose between a robot and its designated leader or leaders are presented. These strategies ensure asymptotic coordinated motion using di erent information levels to implement the controllers. The rst strategy is a partial state feedback nonlinear approach that requires full knowledge of leader's velocities and accelerations. The second strategy is a robust partial state feedback nonlinear approach that requires knowledge of the rate of change of the relative position error. Finally, the third strategy is an output feedback approach that uses highgain observers to estimate the derivative of the unmanned vehicles' relative position. In consequence, this last algorithm only requires knowledge of v the leaderfollower relative distance and bearing angle. Both data are computed using measurements from a single camera, eliminating sensitivity to information ow between vehicles. Furthermore, because the leader's exact trajectory is uncertain, this approach can be applied to both the problem of tracking a given trajectory maintaining a desired formation shape or to pursuitevasion problems, where the evader trajectory is assumed unknown. This is a distinctive aspect of this vision based architecture with respect to the current stateoftheart. Lyapunov's stability theorybased analysis and numerical simulations in a realistic tridimensional environment show the stability properties of the control approaches. Finally, we describe our ongoing implementation on virtual and real platforms and show simulation results to verify the validity of the designed methodologies. vi Contents Chapter Page List of Figures xiv I Introduction 1 1 Motivation 2 1.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2 1.2 Formations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6 1.3 Related work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9 1.4 Outline . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11 1.5 Statement of contributions . . . . . . . . . . . . . . . . . . . . . . . . 12 2 Multirobot formations 13 2.1 Mathematical model of the robots . . . . . . . . . . . . . . . . . . . . 13 2.1.1 Leaderfollower polar model . . . . . . . . . . . . . . . . . . . 14 2.2 Graph theory . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16 2.3 Formal de nition of formation . . . . . . . . . . . . . . . . . . . . . . 17 3 Nonlinear estimation 19 3.1 Optimal recursive estimation . . . . . . . . . . . . . . . . . . . . . . 20 3.2 The extended Kalman lter (EKF) . . . . . . . . . . . . . . . . . . . 21 3.2.1 Simulation example . . . . . . . . . . . . . . . . . . . . . . . . 22 3.3 The unscented Kalman lter (UKF) . . . . . . . . . . . . . . . . . . . 24 3.3.1 Simulation example . . . . . . . . . . . . . . . . . . . . . . . . 28 3.4 UKF for parameter estimation . . . . . . . . . . . . . . . . . . . . . . 30 3.4.1 Simulation example . . . . . . . . . . . . . . . . . . . . . . . . 30 3.5 Highgain observers (HGO) . . . . . . . . . . . . . . . . . . . . . . . 32 vii 3.5.1 Simulation example . . . . . . . . . . . . . . . . . . . . . . . . 37 3.6 Higherorder sliding mode (HOSM) observers . . . . . . . . . . . . . . 37 3.6.1 Simulation example . . . . . . . . . . . . . . . . . . . . . . . . 43 II Main Contributions 49 4 Mobile robot vision 50 4.1 System overview . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52 4.2 Marker and ID detection . . . . . . . . . . . . . . . . . . . . . . . . . 57 4.2.1 Image capture . . . . . . . . . . . . . . . . . . . . . . . . . . . 57 4.2.2 Grayscale conversion . . . . . . . . . . . . . . . . . . . . . . . 57 4.2.3 Thresholding: The Otsu method . . . . . . . . . . . . . . . . . 57 4.2.4 Contour extraction and selection . . . . . . . . . . . . . . . . 61 4.2.5 ID recognition . . . . . . . . . . . . . . . . . . . . . . . . . . . 61 4.3 Pose estimation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 64 4.3.1 Camera model . . . . . . . . . . . . . . . . . . . . . . . . . . . 64 4.3.2 OpenGL camera model . . . . . . . . . . . . . . . . . . . . . . 67 4.3.3 Pose estimation algorithms . . . . . . . . . . . . . . . . . . . . 68 4.3.3.1 Pose from orthography and scaling with iteration (POSIT) method . . . . . . . . . . . . . . . . . . . . 68 4.3.3.2 Projection ray attraction (PRA) method . . . . . . . 72 4.3.3.3 Lowe's method . . . . . . . . . . . . . . . . . . . . . 73 4.4 Nonlinear ltering . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 77 4.4.1 Mathematical model of the robots . . . . . . . . . . . . . . . . 77 4.4.2 Nonlinear observability . . . . . . . . . . . . . . . . . . . . . . 78 4.4.3 The dual unscented Kalman lter (DUKF) . . . . . . . . . . . 80 4.5 Simulation results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 81 4.6 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 94 5 Visionbased formation control 99 5.1 Formation mathematical models . . . . . . . . . . . . . . . . . . . . . 100 5.1.1 Leaderfollower mathematical model revisited . . . . . . . . . 100 5.1.1.1 Second order leaderfollower model with d = 0 . . . . 102 5.1.1.2 Second order leaderfollower model with d 6= 0 . . . . 104 5.1.2 Two leaderfollower mathematical model . . . . . . . . . . . . 105 5.1.2.1 Second order twoleaderfollower model with d = 0 . 108 viii 5.1.2.2 Second order twoleaderfollower model with d 6= 0 . 111 5.1.3 Generic model formulation . . . . . . . . . . . . . . . . . . . . 113 5.2 The formation control problem . . . . . . . . . . . . . . . . . . . . . 114 5.2.1 Leaderfollower control problem . . . . . . . . . . . . . . . . . 114 5.2.2 Twoleaderfollower control problem . . . . . . . . . . . . . . . 116 5.2.3 Formation dynamics, communication and sensing models . . . . . . . . . . . . . . . . . . . . . . . . . . 118 5.3 Formation control algorithms . . . . . . . . . . . . . . . . . . . . . . 118 5.3.1 Partial state feedback formation control (PSFB) . . . . . . . . 121 5.3.2 Robust state feedback formation control (RSFB) . . . . . . . . 121 5.3.3 Output feedback formation control algorithm (OFB) . . . . . 122 5.4 Backstepping . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 124 5.5 Simulation results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 126 5.6 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 149 III Conclusions 150 6 Summary and future work 151 Bibliography 152 IV Appendices 167 A Projective geometry 168 B Proofs of lemmas and theorems 171 B.1 Proof of Theorem 5.3.1 . . . . . . . . . . . . . . . . . . . . . . . . . . 171 B.2 Lemma B.2.1 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 172 B.3 Proof of Theorem 5.3.3 . . . . . . . . . . . . . . . . . . . . . . . . . . 174 C De nitions and mathematical background 178 C.1 Preliminaries . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 178 C.2 Controllability and observability . . . . . . . . . . . . . . . . . . . . . 181 C.2.1 Observability . . . . . . . . . . . . . . . . . . . . . . . . . . . 181 C.3 Inputtostate stability . . . . . . . . . . . . . . . . . . . . . . . . . . 182 ix D Perturbations, relative dynamics and degrees 184 D.1 Leaderfollower model with uncertain transformation . . . . . . . . . 184 D.2 Internal dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 185 x List of Figures Figure Page 1.1 Bats, locusts, and ants. . . . . . . . . . . . . . . . . . . . . . . . . . . 3 1.2 Aircraft formations. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4 1.3 Typical formation shapes: (a) line, (b) column, (c) diamond, (d) wedge [4]. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6 1.4 Formation positions: (a) unitcenterreferenced, (b) leaderreferenced, (c) neighborreferenced [4]. . . . . . . . . . . . . . . . . . . . . . . . . 7 2.1 Unicycletype robots. (a) ERSP Scorpion (Evolution Robotics), (b) Marhes TXT platform. . . . . . . . . . . . . . . . . . . . . . . . . . 14 2.2 Formation geometry. . . . . . . . . . . . . . . . . . . . . . . . . . . . 15 3.1 State estimation with the EKF. . . . . . . . . . . . . . . . . . . . . . 24 3.2 State estimation and control with the EKF. . . . . . . . . . . . . . . 25 3.3 State estimation with the UKF. . . . . . . . . . . . . . . . . . . . . . 28 3.4 State estimation and control with the UKF. . . . . . . . . . . . . . . 29 3.5 Parameter estimation with the UKF. . . . . . . . . . . . . . . . . . . 32 3.6 Parameter estimation with the UKF and noisy output. . . . . . . . . 33 3.7 HGO with exact control law. . . . . . . . . . . . . . . . . . . . . . . . 38 3.8 HGO behavior with unsaturated inputs. . . . . . . . . . . . . . . . . 39 3.9 HGO behavior with saturated inputs. . . . . . . . . . . . . . . . . . . 40 3.10 HGO behavior with saturated inputs and noise. . . . . . . . . . . . . 41 3.11 Simulation results with a rst order HOSM. . . . . . . . . . . . . . . 44 3.12 Simulation results with a second order HOSM. . . . . . . . . . . . . . 46 3.13 Simulation results with a second order HOSM and measurement noise. 47 3.14 Closedloop simulation results with a second order HOSM. . . . . . . 48 4.1 Scheme of the TXT platform in MPSLab. . . . . . . . . . . . . . . . 53 4.2 Synthetic camera screen shot. . . . . . . . . . . . . . . . . . . . . . . 53 xi 4.3 Logitech 3000 USB camera (bottomleft), PointGrey's Bumblebee (top), and Unibrain's FireI (bottomright) FireWire IEEE1394 cameras. . 54 4.4 Vision processing ow chart. . . . . . . . . . . . . . . . . . . . . . . . 55 4.5 Camera and robot frames. . . . . . . . . . . . . . . . . . . . . . . . . 56 4.6 Processing sequence of the octagon shape. . . . . . . . . . . . . . . . 58 4.7 Grayscale conversion. (a) Color image, (b) red channel, (c) green channel, (d) blue channel, and (e) mean value image. . . . . . . . . . 59 4.8 Fiducial marker ID computation. . . . . . . . . . . . . . . . . . . . . 61 4.9 Projective transformation. . . . . . . . . . . . . . . . . . . . . . . . . 63 4.10 Pinhole camera model. . . . . . . . . . . . . . . . . . . . . . . . . . . 65 4.11 OpenGL camera frustum [119]. . . . . . . . . . . . . . . . . . . . . . 67 4.12 POSIT projections. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 69 4.13 Dual estimation problem. . . . . . . . . . . . . . . . . . . . . . . . . . 80 4.14 Identi cations: Robot 1 (left) IDs 2, 3, 4, 5; robot 2 (right) IDs 1, 2, 3. 81 4.15 Identi cations: Robot 1 (right) IDs 2, 3, 4; robot 2 (left) IDs 2, 3, 4. . 81 4.16 Identi cations: Robot 1 (right) IDs 2, 3, 4; robot 2 (left) IDs 1, 2. . . 82 4.17 Pure translation using Lowe's method. . . . . . . . . . . . . . . . . . 82 4.18 Pure rotation using Lowe's method. . . . . . . . . . . . . . . . . . . . 82 4.19 Pure translation. Error and number of iterations using PRA method. 83 4.20 Pure translation. Error and number of iterations using PRA method and a priori information. . . . . . . . . . . . . . . . . . . . . . . . . . 84 4.21 Pure rotation. Error and number of iterations using PRA method and a priori information. . . . . . . . . . . . . . . . . . . . . . . . . . . . 84 4.22 Pure translation. Error and number of iterations using Lowe's method. 85 4.23 Pure rotation. Error and number of iterations using Lowe's method. . 85 4.24 Pure translation. Error and number of iterations using POSIT algorithm. 86 4.25 Pure rotation. Error and number of iterations using POSIT algorithm. 86 4.26 Pose estimation with q1 = [1,−0.25, 0]T , q2 = [2, 0.5, 0]T . . . . . . . . 87 4.27 Pose estimation with q1 = [2.5,−0.5, 0]T , q2 = [3, 0.5, 0]T . . . . . . . . 87 4.28 Pose estimation with q1 = [1.5,−0.5, 0]T , q2 = £ 4, 0.5,−¼ 2 ¤T . . . . . . 88 4.29 Fog e ect on pose estimation. . . . . . . . . . . . . . . . . . . . . . . 89 4.30 Computational load with one robot in the eld of view (blue line) and three robots plus another entering at t = 1.25 red line). . . . . . . . . 90 4.31 Relative distance and bearing estimation using a DUKF. . . . . . . . 91 4.32 Velocity estimation using a DUKF. . . . . . . . . . . . . . . . . . . . 92 4.33 Velocity estimation using a DUKF with noise injection. . . . . . . . . 93 xii 4.34 Control, relative distance and bearing estimation using a DUKF. . . . 95 4.35 Control and velocity estimation using a DUKF. . . . . . . . . . . . . 96 4.36 Control, relative distance and bearing estimation using a DUKF with noise injection. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 97 4.37 Control and velocity estimation using a DUKF with noise injection. . 98 5.1 Leaderfollower formation geometry. . . . . . . . . . . . . . . . . . . . 100 5.2 Two leaderfollower formation geometry. . . . . . . . . . . . . . . . . 106 5.3 Leaderfollower dynamic system: (a) complete system, (b) compact form.119 5.4 Leaderfollower (PSFB controller). . . . . . . . . . . . . . . . . . . . . 119 5.5 Leaderfollower (RSFB controller). . . . . . . . . . . . . . . . . . . . 120 5.6 Leaderfollower (OFB controller). . . . . . . . . . . . . . . . . . . . . 120 5.7 Robot trajectories for a straight line path and d = 0. . . . . . . . . . 127 5.8 Velocity comparison between controllers (5.38), (5.40), and (5.52) for a straight line path and d = 0. . . . . . . . . . . . . . . . . . . . . . . 128 5.9 Error comparison between controllers (5.38), (5.40), and (5.52) for a straight line path and d = 0. . . . . . . . . . . . . . . . . . . . . . . . 129 5.10 Control e ort comparison with " = 0.1, 0.001 for a straight line path and d = 0. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 130 5.11 Output error comparison with " = 0.1, 0.001 for a straight line path and d = 0. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 131 5.12 Snake view of a simulation with 3 robots for a straight line path and d = 0. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 132 5.13 Trajectories of the leader robot and two followers for a circular path and d = 0. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 133 5.14 Velocity comparison between controllers (5.38), (5.40), and (5.52) for a circular path and d = 0. . . . . . . . . . . . . . . . . . . . . . . . . 134 5.15 Output error comparison between controllers (5.38), (5.40), and (5.52) for a circular path and d = 0. . . . . . . . . . . . . . . . . . . . . . . 135 5.16 Control e ort comparison with " = 0.1, 0.001 for a circular path and d = 0. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 136 5.17 Output error comparison with " = 0.1, 0.001 for a circular path and d = 0. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 137 5.18 Snake view of a simulation with 3 robots for a circular path and d = 0. 138 5.19 Trajectories of the robots with d = 0. . . . . . . . . . . . . . . . . . . 139 5.20 Velocities of robot 5 using the twoleader algorithm with d = 0. . . . 140 xiii 5.21 Output error of robot 5 using the twoleader algorithm with d = 0. . . 141 5.22 Snake view of a simulation with 7 robots with d = 0. . . . . . . . . . 142 5.23 Speci cations for 7 robots. . . . . . . . . . . . . . . . . . . . . . . . . 142 5.24 Trajectories of the robots for a straight line path and d 6= 0. . . . . . 143 5.25 Error comparison for a straight line path and d 6= 0. . . . . . . . . . . 144 5.26 Snake view of a simulation with 7 robots for a straight line path with d 6= 0. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 145 5.27 Trajectories of the robots for a circular path and d 6= 0. . . . . . . . . 146 5.28 Error comparison for a circular path and d 6= 0. . . . . . . . . . . . . 147 5.29 Snake view of a simulation with 7 robots for a circular path with d 6= 0. 148 A.1 Crossratio invariance. . . . . . . . . . . . . . . . . . . . . . . . . . . 169 A.2 Vanishing points. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 170 xiv Part I Introduction 1 Chapter 1 Motivation 1.1 Introduction In the last decade, cooperative multirobot systems have been increasingly being considered as a mean of performing complex tasks within dynamic environments. These complex and extremely diverse tasks include applications such as automated transportation, spacecraft interferometry, surveillance, mapping, border patrol, search and rescue, disaster relief, unmanned air combat, multitargeting and multiplatform operations, military battle systems, highway and air transportation systems, mobile sensor networks, and wireless surveillance. The main reason for engaging multi robot systems in collective behavior is their expected outperformance over single robot systems in size, cost, exibility, and fault tolerance [43, 90]. The design of multirobot formations was initially inspired by the emergent self organization from social interaction observed in nature. For example, bees, birds, sh, wildebeests, and human beings ock or swarm in particular directions [120], see Figure 1.1. Moreover, it is wellknown that aircraft formations y in a 'V' to decrease drag, save energy, and increase safety, Figure 1.2. As Camazine et al. pointed out [13]: Selforganization is a process in which pattern at the global level of a system emerges solely from numerous interactions among the lowerlevel components of the system. Moreover, the rules specifying interactions among the system's components are executed using only local informa tion, without reference to the global pattern. In short, the pattern is an emergent property of the system, rather than a property imposed on the system by an external ordering in uence ... pattern is a particular, 2 Bats  T.K. Horiuchi  http://www.isr.umd.edu/Labs/CSSL/horiuchilab/lab.html Desert locusts – D. Grünbaum Ants – C. Anderson http://www2.isye.gatech.edu/~carl/ Figure 1.1: Bats, locusts, and ants. 3 http://www.skyflash.com Figure 1.2: Aircraft formations. 4 organized arrangement of object in space or time. Single individuals may use relatively simple behavioral rules to generate structures and patterns at the collective level that are more complex than the components and processes from which they emerge. For instance, in the mid40s Walter, Wiener and Shannon observed complex social behavior in the interaction between simple turtle robots equipped with light and touch sensors [14, 112]. Therefore, to understand selforganization and use it in a productive fashion, two main questions have to be answered: The rst question is related to the problem of how global behaviors can be fostered using local and relatively simple rules. The second question is related to the problem of which sensing capability needs each individual. Regarding to the rst question, studies on ocking and herding mechanisms in dicate that they emerge as a combination of a desire to stay in the group and yet simultaneously keep a minimum separation distance from other members of the ock [96]. In [52], Tu and Toner prove that this combination has to be complemented with motion, by proving that group alignment is not possible in ocks and herds with local perception in the absence of movement. According to recent models from theoretical physics [51, 137], a key factor is the density of animals in the group. If this density increases, a rapid transition occurs from disordered movement of indi viduals to highly aligned collective motion. Two of the most signi cant advances in collective motion and synchronization are the models conceived by Vicsek et al. [137] and the Kuramoto [63, 64, 111]. Vicsek designed a discretetime and stochastic model that analyzes with simple interactions the emergence of di erent behavior pat terns. Kuramoto's model was originally developed for oscillator synchronization, it is a continuoustime and deterministic approach very suitable for analysis of collective behavior emergence without noise. Regarding to the second question, nature also give guidelines on which and how many sensors are necessary to achieve coordinate behavior. For example, bats sense their surroundings using ultrasound. Birds, mammals, and most insects use vision as their main sensor. In Figure 1.2, the pilot can maintain formation by knowing the relative distance, heading, and bearing between airplanes. In summary, it can be stressed that members of ocks of birds or schools of sh do not use explicit communication but local sensing, usually vision, in order to maintain a coherent formation or a coordinated motion, even when they have to move around obstacles or avoid predators. This dissertation nds its main motivation in solving the local control problem using visionbased sensing, showing that a stable local behavior 5 Figure 1.3: Typical formation shapes: (a) line, (b) column, (c) diamond, (d) wedge [4]. can be achieved and maintained without using communications. 1.2 Formations Intuitively, a formation is a group of agents maintaining a determined relationship. The two most important characteristics of a formation are its shape and its position. The most common formation shapes are line, column, diamond, and wedge [4], as shown in Figure 1.3. Common de nitions of formation positions are unitcenter referenced, leader refer enced, and neighbor referenced [4]. In a unitcenterreferenced position, all the robots maintain a relative position to a center point given by the average of the coordi nates px and py of each member of the formation. In a leaderreferenced position the robots maintain a relative position with respect to a leader robot. Finally, in a neighborreferenced position, each robot maintains a position relative to one other predetermined robot. Figure 1.4 depicts these three types of formation position de  nitions. 6 Figure 1.4: Formation positions: (a) unitcenterreferenced, (b) leaderreferenced, (c) neighborreferenced [4]. Types of formations There exist several types of formations. Here we resume some of the more relevant to our work. Flocking The earliest application of arti cial formation behavior starts with the pioneering work of Craig Reynolds [115]. In his work, the author addresses the problem of simulating ocks of birds and schools of sh with a simple egocentric behavioral model. This model consists of collision avoidance, velocity matching, and formation keeping components. Also, three heuristic rules are de ned: • cohesion rule  aim of keeping proximity to nearby ockmates, • separation rule  desire of collision avoidance, • alignment rule  intention of velocity matching with neighbors. These three rules are inherently local and give to each member the possibility of navigating using only its sensing capabilities. From a mathematical point of view, they allow to pose the ocking problem as a decentralized control problem. Moreover, the superposition of these three rules results in all agents moving in a loose (as opposite to rigid) formation, with a common heading while avoiding collisions [128, 129]. 7 The work of Reynolds led to the creation of the rst computer animated ocking. Several lms and video have been produced using ocking, such as Stanley and Stella in: Breaking the Ice (1987), Batman Returns (1992), Cli hanger (1993), The Lion King (1994), From Dusk Till Dawn (1996), The Hunchback of Notre Dame (1996), and Finding Nemo (2003). Behaviorbased formation The main idea of behaviorbased formations is to integrate several goal oriented be haviors with a given arbiter that chooses between them. It is a highly decentralized reactive framework. In [4] for example, the authors de ne two steps for formation maintenance: (i) determination of the robot's position in formation; and (ii) gener ation of motor commands to direct the robot toward the correct location. A global arbiter gives priorities to each behavior depending on environmental conditions. The method performs well in simulations and experiments, but it lacks mathe matical analysis of convergence and robustness. Leaderfollowing formation Leader following can be centralized or decentralized. When a decentralized approach is used, each robot can drive itself keeping a desired distance to some of its neighbors. This scheme typically has one leader which takes care of the assignment of followers' relative positions and of the global mission objective, for instance, obstacle avoidance [33, 34, 125]. The major weakness of singleleader architectures is that they have a single point of failure [73]. Therefore, stability properties of these architectures under changes of leaders is a major concern. Some attempts have been made in this eld. For instance, in [27, 28], the architecture has a high level layer that detects leader failures and rede nes the formation control graph according to some given strategy. Failures include robot malfunctioning, communication errors (in communicationbased formations), and information ow breaches. Virtual structure Virtual structure approaches describe the entire formation as one single rigid body or pattern. In [3], for example, a virtual structure or pattern formation is de ned as the problem of coordination a group of robots to get into and maintain a formation with 8 a certain shape such as a wedge or a line, assuming the existence of a communication channel between the central unit and the individual robots. These approaches are centralized and, consequently, they are very susceptible to operation errors. 1.3 Related work Creating systems with multiple autonomous vehicles working together to achieve a common mission within a changing environment places severe demands on the de sign of decisionmaking supervisors, cooperative control schemes, and communication strategies. Research on multivehicle system coordination has been focused both on centralized and decentralized control strategies. Centralized control strategies have the advantage of being able to reach a global optimum solution for tasks such as path planning and recon guration [8, 12, 108, 143]. However, centralized algorithms be come infeasible when the number of vehicles and constraints increase, preventing their implementation in realtime. On the other hand, decentralized control approaches only require local information and can e ectively achieve multivehicle coordination behaviors as the ones observed in nature [28, 27, 63, 72, 73, 88]. Many approaches for solving multirobot coordination reduce the general control problem to a singleagent control one by assuming that global communication of some coordination information is available. However, a coordination mechanism that does not rely on global communication ensures exibility and mission safety, because ref erence trajectories and mission objectives should not be shared among all agents but to some leaders [20]. Of course, this poses the challenge of designing new formation control paradigms, robust and computationally simple, that can perform well in the presence of uncertainty in leaders' current and future states. Other authors have addressed the coordination problem of multiple unmanned vehicles using optimization techniques [10]. Contributions in this area include work focused on autonomous distributed sensing tasks [25], decentralized optimization based control algorithms [41], optimal motion planning [9], and formation recon g uration planning [143]. More recently, the use of model predictive control (MPC) or recedinghorizon control (RHC) is becoming popular in the multirobot system literature [12, 36, 46, 68, 105]. Generally speaking, MPC algorithms rely on the optimization of a predicted model response with respect to the system input to deter mine the best input changes for a given state. Either hard constraints (that cannot be violated) or soft constraints (that can be violated but with some penalty) can be 9 incorporated into the optimization problem, giving to MPC a potential advantage over passive state feedback control laws. However, there are possible disadvantages to be considered. For instance, in its traditional use for process control, the primary disadvantage of MPC is the need of a good model of the plant, but such model is not always available. In robotics applications, the foremost disadvantage is the com putational cost, negligible for slowmoving systems in the process industry, but very important in realtime applications. In [105], a pathspace iteration (PSI) algorithm is designed to solve the motion coordination problem. In this work, the authors present centralized and decentralized algorithms based on an MPC/PSI formulation to solve nonholonomic multirobot coordination problems in dynamic, unknown en vironments. Chen et al. [20, 18, 19] develop a decentralized control architecture that employs local sensor information and an internal model approach [60, 61] to handle uncertain ties in the leader reference trajectory. The information needed to implement the con troller is relative position and velocity between a robot and its leader. This approach uses system immersion to model the known leader's movements and communications to acquire leader's heading. Leader velocities are piecewise linear functions of time over nite intervals of variable length. Intervehicle collision avoidance is assured by con ning vehicles to speci c sectors during transients. In the last few years, some visionbased motion coordination algorithms have been developed in the literature. In [138], authors design a formation control algorithm based on omnidirectional visual servoing and motion segmentation. Visionbased for mation controllers are described in [26]. The algorithms use inputoutput lineariza tion and require the estimation of leaderfollower relative angle and leader's linear and angular velocities. Two algorithms are described for imagebased leaderfollower formation control, one is based on feedback linearization and the other combines Luenberger observers with a linear controller. In [85], the authors give a su cient condition for observability using a visionbased centralized controller. The control law is based on inputoutput feedback linearization and assumes that the robots have omnidirectional cameras and that the leader can transmit the velocity control to each follower and estimate their states with an extended Kalman lter. Another visionbased formation controller has been recently developed in [88, 89]. This distributed coordination approach is based on nearestneighbor interactions, assuming that robots move with constant linear speed and achieve ocking after a given time. In general, ocking algorithms do not maintain a strict formation shape. Such formation maintenance is critical in applications such as cooperative payload 10 transport [126], cooperative object pushing [50], and distributed sensor deployment when robots move forming certain geometric pattern [142]. In this dissertation, we present di erent approaches for solving a multirobot co ordination problem using monocular vision. The rst part of the dissertation presents an arti cial vision system for pose estimation using ducial marks [103, 104]. The second part presents several visionbased formation control algorithms for leader fol lowing and twoleaders following architectures [106, 102, 101]. The rst two are state feedbackbased controllers that require total or partial knowledge of the state vari ables, respectively. The third is a robust output feedback decentralized controller based only on monocular vision information about the relative motion between a robot and its designated leader or leaders. A highgain observer is used to esti mate the derivatives of the measurements. This last algorithm eliminates the need of intervehicle communications which increases the reliability of the overall system, making this approach suitable for pursuitevasion problems, when the pursued is an unfriendly robot. Finally, we also describe our ongoing implementation on virtual and real platforms. 1.4 Outline The rest of the dissertation is organized as follows. In Chapter 2, we review some de nitions on graph theory and robot formations that are used on the main two chap ters of this dissertation. Chapter 3 resumes four nonlinear estimation algorithms and compare their properties through simple examples. Chapter 4 presents the imple mentation of real and virtual vision systems for robot identi cation and localization. Chapter 5 describes and analyzes visionbased decentralized formation control algo rithms. Finally, we present our concluding remarks and future work in Chapter 6. 11 1.5 Statement of contributions This dissertation is a step forward toward the design of sensorbased decentralized architectures for stabilization of unmanned vehicles moving in formation. The con tributions can be divided into two main parts: • A vision system for robot identi cation and localization. • A set of robust visionbased formation controllers. The vision system, composed of image acquisition and processing, robot identi ca tion, and pose estimation, can recognize and relatively localize neighbor robots using ducial markers. The set of robust visionbased formation controllers allows to maintain relative distance and bearing in leaderfollower and twoleaderfollower formations. These controllers ensure asymptotic coordinated motion coordination using di erent infor mation and sensing levels. To the author's knowledge, the whole architecture is the rst framework that ap plies a realistic sensor system and control algorithms to the formation control problem with guaranteed stability and without communications. 12 Chapter 2 Multirobot formations In this Chapter, some basic concepts relevant to multirobot formations are summa rized. We brie y review the dynamical model of the robot and the polar relationship between a leader and its follower, that is, the basic cell of a formation. We resume some tools for formation characterization using graph theory and the formal de ni tion of a formation. The reader is referred to the bibliography for a more detailed treatment [35, 133]. 2.1 Mathematical model of the robots Let us consider a multirobot system composed of Na agents, as the ones shown in Figure 2.1, modelled as unicyclelike vehicles1 §i moving on the plane, with the dynamic model of the ith robot given by [20] x˙ i (t) = vi (t) cos µi (t) , y˙i (t) = vi (t) sin µi (t) , µ˙i (t) = !i (t) , (2.1) miv˙i (t) = Fi (t) , Ji!˙ i (t) = Ti (t) , where qi (t) = [xi (t) , yi (t) , µi (t)]T 2 SE (2), is the con guration vector with respect to an inertial reference frame, (xi, yi) 2 R2 denotes the position in Cartesian coor dinates and is the intersection of the axis of symmetry with the driving wheels axis, µi 2 S = (−¼, ¼] is the heading angle, ui (t) := [vi (t) , !i (t)]T 2 Ui µ R2, vi (t), 1Note that other mathematical models can be adapted to this framework. 13 Figure 2.1: Unicycletype robots. (a) ERSP Scorpion (Evolution Robotics), (b) Marhes TXT platform. !i (t) are the linear and the angular velocities, respectively, Ui is a compact set of admissible velocities containing the origin, and Fi (t), Ti (t) 2 R are the admissible control signals. Without lost of generality, we assume that all sets Ui are equal, then U = Ui := {(vi, !i) vi · vmax, !i · !max} , and that the masses and inertia moments are equal to one mi = Ji = 1, with i = 1, . . . , Na. As can be seen in Figure 2.1, a monocular camera is mounted on each robot. In an ERSP Scorpion robot, the camera is mounted on the intersection of the axis of symmetry with the driving wheels axis, see Figure 2.1a. In a Marhes TXT platform, the camera is mounted d units ahead on the axis of symmetry, see Figure 2.1b. 2.1.1 Leaderfollower polar model Let the Euclidean distance `ik (t) 2 R¸0 and the angles ®ik (t), µik (t) 2 S = (−¼, ¼] between robots i and k be de ned as `ik (t) = q (xi (t) − xc k (t))2 + (yi (t) − yck (t))2, (2.2) ®ik (t) = ³ik (t) − µi (t) , (2.3) µik (t) = µi (t) − µk (t) , (2.4) 14 where ³ik (t) = atan2 (yi (t) − yck (t) , xi (t) − xc k (t)), and xc k (t) = xk (t) + d cos µk (t), yck (t) = yk (t) + d sin µk (t) are the coordinates of the camera, as shown in Figure 2.2. XI YI Robot k ik b ik a ik i q p q k q Robot i c Z d  Xc m q ik z Figure 2.2: Formation geometry. Then, assuming that both robots are described by the model (2.1), the leader follower system can be described in polar form by the set of equations 2 6666666664 x˙ i (t) y˙i (t) µ˙i (t) `˙ik (t) ®˙ ik (t) µ˙ik (t) 3 7777777775 = 2 6666666664 cos µi 0 sin µi 0 0 1 cos ®ik 0 −sin ®ik `ik −1 0 1 3 7777777775 ui (t)+ 2 6666666664 0 0 0 0 0 0 −cos (®ik + µik) −d sin (®ik + µik) sin(®ik+µik) `ij −d cos(®ik+µik) `ik 0 −1 3 7777777775 uk (t) . (2.5) As mentioned, each agent is equipped with a vision sensor and a pan controller, as shown in Figure 2.2. Then, the measured variables are given by µt (t) = arctan µ −xc (t) zc (t) ¶ , ®ik (t) = µt (t) − µm (t) , µik (t) = µp (t) + µm (t) . 15 2.2 Graph theory Graph theory provides a convenient framework for modeling multivehicle coordina tion problems. Particularly, it is very powerful to generalize individual properties to group properties [133]. For example, graphs have been used to capture the intercon nection topology [37, 97], describe control structure [28, 27], test constraint feasibility [125], characterize information ow [40], and analyze error propagation [133]. Graphs can be undirected to model position constraints [37, 97], or directed to describe in formation ow [40] or leader following interagent control speci cations [34, 127, 132] In this Section, we resume some concepts from graph theory, the reader is referred to the literature for a detailed treatment [35, 96, 133]. A directed graph G is a pair (V, E) of the set of vertices V 2 {1, . . . ,N} and directed edges E 2 V ×V. An edge (i, j) 2 E is an ordered pair of distinct vertices in V that it is said to be incoming with respect to the head j and outgoing with respect to the tail i. The in degree of a vertex is de ned as the number of edges that have this vertex as head. If i, j 2 V and (i, j) 2 E, then i and j are said to be adjacent, or neighbors, and are denoted by i » j. The adjacency matrix A(G) = {aij} of a graph G is a matrix with nonzero elements such that aij 6= 0 , i » j. The set of neighbors of node i is de ned by Ni := {j 2 V\ {i} aij 6= 0} . (2.6) The number of neighbors of each vertex is its valence. A path of length rp from vertex i 2 V to vertex j 2 V is a sequence of rp+1 distinct vertices, v1 = i, . . . , vk, . . . , vrp+1 = j, such that for all k 2 [1, rp], (vk, vk+1) 2 E. A weak path of length rp from vertex i 2 V to vertex j 2 V is a sequence of rp+1 distinct vertices, v1 = i, . . . , vk, . . . , vrp+1 = j, such that for all k 2 [1, rp], (vk, vk+1) 2 E or (vk+1, vk) 2 E. A directed graph G is weakly connected or simply connected if any two vertices can be joined with a weak path. The distance between two vertices i and j in a graph G is the length of the shortest path between both vertices. The diameter of a graph is the maximum distance between two distinct vertices. The incidence matrix B (G¾) of a graph G with orientation ¾, G¾, is a matrix whose rows and columns are indexed by the vertices and edges of G, respectively, such that the (i, j) entry of B (G¾) is equal to 1 if edge (i, j) is incoming to vertex i, −1 if edge (i, j) is out coming from vertex i, and 0 otherwise. The symmetric matrix de ned as L(G) = B (G¾)B (G¾)T 16 is called the Laplacian of G and is independent of the choice of orientation ¾. For a connected graph, L has a single zero eigenvalue and the associated eigenvector is the ndimensional vector of ones, 1n. A cycle is a connected graph where each vertex is incident with one incoming and one outgoing edge. An acyclic graph is a graph with no cycles. Let pi = (pxi , py i ) 2 R2 denote the position of robot i, and ri > 0 denote the interaction range between agent i and the other robots. A spherical neighborhood (or shell) of radius ri around pi is de ned as B (pi, ri) := © q 2 R2 : kq − pik · ri ª . Let us de ne p = col (pi) 2 R2n, where n = V is the number of nodes of graph G, and r = col (ri). We refer to the pair (p, r) as a cluster with con guration p and vector of radii r. A spatial adjacency matrix A(p) = [aij (p)] induced by a cluster is given by aij (p) = ( 1, if pj 2 B (pi, ri) , j 6= i 0, otherwise. The spatial adjacency matrix A(p) de nes a spatially induced graph or net G (p). A node i 2 V with a spherical neighborhood de ne a neighboring graph Ni as Ni (p) := {j 2 V : aij (p) > 0} . (2.7) An ®lattice [96] is a con guration p satisfying the set of constraints kpj − pik = d, 8j 2 Ni (p) . A quasi ®lattice [96] is a con guration p satisfying the set of inequality constraints −± · kpj − pik − d · ±, 8 (i, j) 2 E (p) . 2.3 Formal de nition of formation Now, it is possible to give a formal de nition of formation [133]: De nition 2.3.1 (Formation). A formation is a network of vehicles interconnected via their controller speci cations that dictate the relationship each agent must main tain with respect to its leader or leaders. The interconnections between agents are modeled as edges in an acyclic directed graph, labeled by a given relationship. 17 Using the robot model presented in (2.5), the relationship between two robots could be distance and bearing angle or just distance. In the former case, the in degree of the follower is one. Whereas in the latter case, the indegree of the follower becomes two. De nition 2.3.2 (Formation Control Graph). A formation control graph G = (V, E, S) is a directed acyclic graph consisting of the following: • A nite set V = (v1, . . . , vN) of N vertices and a map assigning to each vertex vk a control system (4.52). • An edge set E ½ V × V encoding leaderfollower relationships between agents. The ordered pair (vi, vk) := eik belongs to E if uk depends on the state of agent i, qi. • A collection S = {sk} of node speci cations de ning control objectives, or set points, for each node k :(vi, vk), (vj , vk) 2 E and for some vi, vj , vk 2 V. Remark 2.3.3. Note that j ´ i when the relationship between robots i and k is given by distance and bearing. Remark 2.3.4. Some examples of control objectives are collision avoidance, or main tenance of communication or sensing links. In the rst case, this objective can be de ned by the minimum distance two robots should keep. The second and third cases can be speci ed by a maximum distance between robots and a given angular range to be achieved. The tails of all incoming edges to a vertex k represent leaders of robot k. Vertices of indegree zero represent formation leaders. Leader agents have no incoming edges; because they regulate their behavior such that the formation may achieve some group objectives, such as navigation avoiding obstacles or tracking reference paths. Therefore, the formation control problem can be divided into two parts: graph as signment and controller design. The rst problem consists in designing the formation control graph G and the speci cation collection S. The second problem consists in maintaining the formation described by the pair G and S in the presence of constrains, obstacles, or limited sensing capabilities. In this dissertation, we assume that G and S are preassigned and mainly focus on the second problem. 18 Chapter 3 Nonlinear estimation Many control algorithms rely on the basic design assumption of exact knowledge of the system model and availability of measurements of every state. For example, the widely used state feedback control techniques [27, 34, 44, 58, 69]. However, in real applications, measurements acquired with one or several sensors are noisy and uncertain and some states are not available. For instance, mobile robots carry sensors such as wheel encoders, inertial units, accelerometers, gyroscopes, and cameras. The data of all these sensors should be smoothened and/or fused to estimate the real values of the system variables using estimation lters [92, 109, 113, 116]. On the other hand, observers can be used to estimate unavailable variables [58, 70, 93, 122]. In particular, output di erentiators play a central role in the estimation of the derivatives of some states. Two of the most wellknown estimation tools available nowadays are the extended Kalman lter (EKF) [23, 86] and the unscented Kalman lter (UKF) [65, 66, 140, 141]. The EKF relies on a linearization using a Taylor series expansion of a nonlinear sensor model. Because the linearization is performed using uncertain states, it can produce the divergence of the lter. The UKF performs the linearization process in the Kalman lter using the un scented transform. The unscented transform is a deterministic sampling approach that propagates through the true nonlinear system a set of chosen sample points to completely describes the true mean and covariance of a Gaussian random variable. The propagated points can capture the posterior mean and covariance accurately up to the 3rd order of a Taylor series expansion. The UKF outperforms the EKF in nonlinear system applications. Both lters, rely on an accurate model description of the system. In the last few years, two state observers have emerged: the highgain observer (HGO) [1, 2, 55, 19 69, 70, 84], and the highorder sliding mode (HOSM) observer [5, 6, 7, 49, 76, 78, 77]. The advantage of both observers is that they do not need an accurate system model, because they are particularly appropriate to deal with unmodeled dynamics and perturbations. The rest of this Chapter is organized as follows. In Section 3.1, we resume basic concepts on recursive estimation. In Section 3.2, we review the extended Kalman algorithm and analyze a simple nonlinear example. In Section 3.3, we describe the unscented Kalman lter and compare it with the EKF. In Section 3.5, we study the performance of highorder observers. In Section 3.6, we survey the highorder sliding model observer and its use as di erentiator. 3.1 Optimal recursive estimation Let the state vector be given by x 2 Rn, and let the system be described by the nonlinear stochastic di erence equation xk+1 = f (xk, uk, vk) , yk = h (xk, nk) , (3.1) where xk is the unobserved state of the system, uk 2 Rm is a known exogenous input, yk 2 Rp is the observed measurement signal, vk and nk are process and measurement noise, respectively. Variables vk and nk are assumed independent, white, and with Gaussian probability distributions p (v) » N (0,Rv) and p (n) » N (0,Rn). The goal is to estimate the state xk using observations yk. The optimal estimate in the minimum meansquared error (MMSE) sense is given by the conditional mean [140, 141] ˆxk = E £ xk ¯¯ Yk 0 ¤ , (3.2) where Yk 0 is the sequence of observations up to time k. In order to solve equation (3.2) it is necessary the knowledge of the a posteriori density p ¡ xk ¯¯ Yk 0 ¢ . The a posteriori density can be computed using the Bayesian approach p ¡ xk ¯¯ Yk 0 ¢ = p ¡ xk ¯¯ Yk−1 0 ¢ p (yk xk ) p ¡ yk ¯¯ Yk−1 0 ¢ , (3.3) where p ¡ xk ¯¯ Yk−1 0 ¢ = Z p (xk xk−1 ) p ¡ xk−1 ¯¯ Yk−1 0 ¢ dxk−1, (3.4) 20 and the normalizing constant p ¡ yk ¯¯ Yk−1 0 ¢ is given by p ¡ yk ¯¯ Yk−1 0 ¢ = Z p ¡ xk ¯¯ Yk−1 0 ¢ p (yk xk ) dxk. (3.5) Equation (3.3) speci es the current state density as a function of the previous den sity and the most recent measurement data. Knowledge of the initial condition p (x0 y0 ) = p(y0x0 )p(x0y0 ) p(y0) determines p ¡ xk ¯¯ Yk 0 ¢ for all k. Statetransition proba bility p (xk xk−1 ) and measurement probability p (yk xk ) are speci ed by the state space model (3.1) and the densities p (vk), p (nk). However, the multidimensional integrations given by (3.4) and (3.5) cannot not be computed in closedform for most systems. As performed in the particle lter approach, the integral can be approx imated by nite sums by applying MonteCarlo sampling techniques. These nite sums converge to the true solution in the limit. Assuming that all densities remain Gaussian, then the Bayesian recursion can be greatly simpli ed because only the conditional mean ˆxk = E £ xk ¯¯ Yk 0 ¤ and covariance Pxk need to be evaluated. This lead to the recursive estimation ˆx− k = E [f (xk−1, uk−1, vk−1)] , (3.6) Kk = PxkykP−1 ˜yk ˜yk , (3.7) ˆy− k = E [h (xk−1, vk−1)] , (3.8) ˆxk = ˆx− k + Kk ¡ yk − ˆy− k ¢ , (3.9) Pxk = P− xk − KkP˜ykKT k , (3.10) where ˆx− k and ˆy− k are optimal predictions xk and yk, respectively, Kk is the optimal gain term, P−x k is the prediction of the covariance of xk and P˜yk is the covariance of ˜yk, with ˜yk = yk − ˆy− k . 3.2 The extended Kalman lter (EKF) The extended Kalman lter (EKF) is an estimation algorithm that uses a predictor corrector mechanism to estimate the current state of a system with its nonlinear model and to correct this estimate using any available sensor measurements. The 21 EKF approximates the optimal nonlinear terms in equations (3.6) to (3.10) as ˆx− k ¼ f (ˆxk−1, uk−1, ¯v) , Kk ¼ ˆP xkyk ˆP −1 ˜yk ˜yk , ˆy− k ¼ h (xk−1, uk−1, vk−1) , where noise random variables with distributions p (vk), p (nk) are approximated by their prior mean ¯v = E [v], ¯n = E [n], respectively. The covariances are computed by linearizing model (3.1) as follows xk+1 ¼ Akxk + Gkuk + Bkvk, yk ¼ Ckxk + Dknk, where Ak = @f (x, uk, ¯v) @x ¯¯¯¯ xk , Gk = @f (xk, u, ¯v) @u ¯¯¯¯ uk , Bk = @f (xk, uk, v) @v ¯¯¯¯ ¯v , Ck = @h (x, ¯n) @x ¯¯¯¯ xk , and Dk = @h (xk, n) @n ¯¯¯¯ ¯n . Algorithm 1 resumes the equations for the EKF. 3.2.1 Simulation example The following model is used through all the examples in the rest of this Chapter. Let the system be given by x˙ 1 = x2, ˙ x2 = x32 + u, (3.11) y = x1. It can be easily proved that the feedback law u = −x32 − x1 − x2, (3.12) stabilizes system (3.11). Figures 3.1 and 3.2 show results of state estimation using the EKF for model (3.11), with additive Gaussian noise on the output p (n) » N (0, 3 × 10−6). The 22 Algorithm 1 Extended Kalman lter 1: Initialization. ˆx0 = E [x0] , Px0 = E h (x0 − ˆx0) (x0 − ˆx0)T i . 2: for k 2 N do 3: Time update: ˆx− k = f (ˆxk−1, uk−1, ¯v) , P− xk = Ak−1Pxk−1ATk −1 + BkRvBTk . 4: Measurement update: Kk = P− xkCTk ¡ CkP− xkCTk + DkRnDTk ¢−1 , ˆxk = ˆx− k + Kk ¡ yk − h ¡ ˆx− k , ¯n ¢¢ , Pxk = (I − KkCk)P− xk , 5: end for where Rv is the process noise covariance, Rn is the measurement noise covariance, and Ak = @f (x, uk, ¯v) @x ¯¯¯¯ ˆxk , Bk = @f ¡ ˆx− k , uk, v ¢ @v ¯¯¯¯¯ ¯v , Ck = @h (x, ¯n) @x ¯¯¯¯ ˆxk , Dk = @h ¡ ˆx− k , n ¢ @n ¯¯¯¯¯ ¯n . 23 0 2 4 6 8 10 0.1 0.08 0.06 0.04 0.02 0 0.02 0.04 0 2 4 6 8 10 0.02 0 0.02 0.04 0.06 0.08 0.1 0.12 0 2 4 6 8 10 0.02 0 0.02 0.04 0.06 0.08 0.1 0.12 0 2 4 6 8 10 0.06 0.05 0.04 0.03 0.02 0.01 0 0.01 real estimated real estimated x1(t) u(t) time [sec] x2(t) y(t) time [sec] time [sec] time [sec] Figure 3.1: State estimation with the EKF. initial state is given by x0 = [x1 (0) , x2 (0)]T = [0.1, 0]T , the lter is initialized with expected initial state ˆx0 = E [x0] = [0, 0]T , and state covariance Px0 = diag (1, 1). Figure 3.1 shows state estimation results for model (3.11) using control law (3.12). Figure 3.2 shows similar results with the control law u = −ˆx32 − ˆx1 − ˆx2. (3.13) Both gures show similar behavior due to the fast convergence of the EKF. 3.3 The unscented Kalman lter (UKF) The unscented Kalman lter (UKF) uses the unscented transform (UT) to capture the mean and covariance estimates with a minimal set of sample points. It provides 3rd order accuracy of the Taylor series expansion of the Gaussian error distribution for any nonlinear system. For nonGaussian inputs, approximations are accurate to at least the 2nd order [65, 66, 140, 141]. 24 0 2 4 6 8 10 0.12 0.1 0.08 0.06 0.04 0.02 0 0.02 0.04 0 2 4 6 8 10 0.02 0 0.02 0.04 0.06 0.08 0.1 0.12 0 2 4 6 8 10 0.02 0 0.02 0.04 0.06 0.08 0.1 0.12 0 2 4 6 8 10 0.06 0.05 0.04 0.03 0.02 0.01 0 0.01 real estimated real estimated x1(t) u(t) time [sec] x2(t) y(t) time [sec] time [sec] time [sec] Figure 3.2: State estimation and control with the EKF. 25 The UT is a deterministic sampling approach that can be summarized as follows [141]: Let x 2 Rn be a ndimensional random variable with mean ¯x and covariance Px. The propagation of the random variable x through a nonlinear function y = g (x) can be approximated by 2n + 1 weighted points Âi 2 Rn, i = 0, . . . , 2n, Â0 = ¯x, Âi = ¯x + ³p (n + ·)Px ´ i , i = 1, . . . , n, Âi = ¯x − ³p (n + ·)Px ´ i , i = n + 1, . . . , 2n, Wm 0 = · n + · , Wc 0 = · n + · + a − ®2 + ¯, Wc i = Wm i = 1 2 (n + ·) , i = 1, . . . , 2n, where · = ®2 (n + ¸)−n is a scaling parameter, ® determines the spread of the sigma points around ¯x and is usually set to a small positive value, ¸ is a scaling parameter of value 0 or 3 − n, ¯ is a parameter used to incorporate any prior knowledge about the distribution of x (¯ = 2 is optimal for Gaussian distributions), ³p (n + ·)Px ´ i is the ith column of the matrix square root, and Wi is a weight associated with the ith point. The sigma points Âi are then propagated through the nonlinear function repre senting the system Yi = g (Âi) , i = 0, . . . , 2n, and the mean and covariance for y are approximated using a weighted sample mean and covariance of the posterior sigma points, ¯y ¼ X2n i=0 Wm i Yi, Py ¼ X2n i=0 (Yi − ¯y) (Yi − ¯y)T . The unscented Kalman lter is a straightforward extension of the UT to recur sive estimation of system (3.1). Algorithm 2 resumes UKF equations for parameter estimation for process and measurement noise purely additive [141]. 26 Algorithm 2 Unscented Kalman lter for state estimation. 1: Initialization. ˆx0 = E [x0] , Px0 = E h (x0 − ˆx0) (x0 − ˆx0)T i . 2: for k 2 N do 3: Compute sigma points: Âk−1 = h ˆxk−1, ˆxk−1 ± ° p Pk−1 i . 4: Time update: Â¤ kk−1 = f (Âk−1, uk−1) , ˆx− k = X2Nx i=0 Wm i Â¤ i,kk−1 , P− k = X2Nx i=0 Wc i ¡ Â¤ i,kk−1 − ˆx− k ¢ ¡ Â¤ i,kk−1 − ˆx− k ¢T + Rv, Âkk−1 = · ˆx− k−1, ˆx− k−1 ± ° q P− k ¸ , Ykk−1 = h ¡ Âkk−1 ¢ , ˆy− k = X2Nx i=0 Wm i Yi,kk−1 . 5: Measurement update: P˜yk ˜yk = X2n i=0 Wc i ¡ Yi,kk−1 − ˆy− k ¢ ¡ Yi,kk−1 − ˆy− k ¢T + Rn, Pxkyk = X2n i=0 Wc i ¡ Xi,kk−1 − ˆx− k ¢ ¡ Yi,kk−1 − ˆy− k ¢T , Kk = PxkykP−1 ˜yk ˜yk , ˆxk = ˆx− k + Kk ¡ yk − ˆy− k ¢ , Pxk = Pxk−1 − KkP˜yk ˜ykKT k , 6: end for where n is the dimension of the original state xk, ° = p n + ¸, Rv is the process noise covariance, and Rn is the measurement noise covariance. 27 0 2 4 6 8 10 0.1 0.08 0.06 0.04 0.02 0 0.02 0.04 0 2 4 6 8 10 0.02 0 0.02 0.04 0.06 0.08 0.1 0.12 0 2 4 6 8 10 0.02 0 0.02 0.04 0.06 0.08 0.1 0.12 0 2 4 6 8 10 0.2 0.15 0.1 0.05 0 0.05 real estimated real estimated x1(t) u(t) time [sec] x2(t) y(t) time [sec] time [sec] time [sec] Figure 3.3: State estimation with the UKF. 3.3.1 Simulation example Figures 3.3 and 3.4 show results of state estimation using the UKF for the model given by equation (3.11), as was presented in Section 3.2.1. Figure 3.3 shows state estimation results for model (3.11) using control law (3.12). As can be seen in gures 3.1 and 3.3, the estimation results are similar except for smoother estimates using the UKF. The smoothing e ect is more clear in the computation of control law (3.13), as can be seen comparing gures 3.2 and 3.4. The smoothing e ect is due to the better approximation that the UKF does of the nonlinear system function. 28 0 2 4 6 8 10 0.15 0.1 0.05 0 0.05 0.1 0 2 4 6 8 10 0.02 0 0.02 0.04 0.06 0.08 0.1 0.12 0 2 4 6 8 10 0.02 0 0.02 0.04 0.06 0.08 0.1 0.12 0 2 4 6 8 10 0.2 0.15 0.1 0.05 0 0.05 x(2) real estimated real estimated x1(t) u(t) time [sec] x2(t) y(t) time [sec] time [sec] time [sec] Figure 3.4: State estimation and control with the UKF. 29 3.4 UKF for parameter estimation An application of the EKF or UKF is learning a nonlinear mapping. This application is used in Chapter 4 to obtain leader's velocities. Let the mapping be given by yk = g (xk,w) , where w 2 Rnw corresponds to a set of unknown parameters. The UKF may be used to estimate the parameters w by writing a new statespace representation wk+1 = wk + vk, yk = g (xk,wk) + nk. (3.14) This model (3.14) solves the optimization problem [141] min w J (w) = min w E ( Xk m=1 [ym − g (xm,w)]T [ym − g (xm,w)] ) . (3.15) Algorithm 3 resumes the equations of the UKF for parameter estimation. In this work, we have chosen the innovation covariance Rv,k in Algorithm 3 as Rv,k = E £ vkvT k ¤ = (¸−1 v − 1)Rw, with ¸v 2 (0, 1], to provide an exponentially decaying weighting term as a forgetting factor to allow adaptation to changes in w. 3.4.1 Simulation example Let the system be given by x˙ 1 = w1x1 + w2x2, ˙ x2 = x32 + u, (3.16) y = 10x1, where w = [1.5, 1.0]T represents the vector of unknown parameters. The control u = −x32 − 5x1 − 3x2 stabilizes system (3.16), with the closed loop matrix given by Acl = " 1.5 1 −5 −3 # , then, the eigenvalues of this system are given by ¸1 (Acl) = −0.5 and ¸2 (Acl) = −1. Figures 3.5 and 3.6 show results of parameter estimation using the UKF for 30 Algorithm 3 Unscented Kalman lter for parameter estimation. 1: Initialization. ˆw0 = E [w] , Pw0 = E h (w − ˆw0) (w − ˆw0)T i . 2: for k 2 N do 3: Time update and sigma points computation: ˆw− k = ˆwk−1, P− wk = Pwk−1 + Rv,k−1, Wkk−1 = h ˆw− k , ˆw− k ± ° q P− wk i , Dkk−1 = g ¡ xk,Wkk−1 ¢ , ˆyk = g ¡ xk, ˆw− k ¢ . 4: Measurement update: P˜yk ˜yk = X2nw i=0 Wc i ¡ Di,kk−1 − ˆyk ¢ ¡ Di,kk−1 − ˆyk ¢T + Rn,k, Pwkyk = X2nw i=0 Wc i ¡ Wi,kk−1 − ˆw− k ¢ ¡ Di,kk−1 − ˆyk ¢T , Kk = PwkykP−1 ˜yk ˜yk , ˆwk = ˆw− k + Kk (yk − ˆyk) , Pwk = P− wk − KkP˜yk ˜ykKT k , 5: end for where nw is the dimension of parameter space, ° = p nw + ¸, Rv is the process noise covariance, and Rn is the measurement noise covariance. 31 0 2 4 6 8 10 0.5 0.4 0.3 0.2 0.1 0 0.1 0.2 0 2 4 6 8 10 0 0.02 0.04 0.06 0.08 0.1 0.12 0.14 0.16 0 2 4 6 8 10 0 0.5 1 1.5 2 0 2 4 6 8 10 0 0.5 1 1.5 2 real estimated real estimated w1(t) u(t) time [sec] w2(t) y(t) time [sec] time [sec] time [sec] Figure 3.5: Parameter estimation with the UKF. model (3.16). The lter is initialized with expected parameter values ˆw0 = E [w0] = [0.1, 0.1]T , and parameter covariance Pw0 = diag (50, 50). Figures 3.5 shows that the parameters converge to true values with a noiseless output signal. Figure 3.6 shows the results of estimating parameters when the output is corrupted by additive Gaussian noise p (n) » N (0, 3 × 10−6). As can be seen, the parameters exhibit polarization, converging to the values w1 (1) = 1.4083 and w2 (1) = 0.9318. This characteristic is typical in system without enough input richness. 3.5 Highgain observers (HGO) In the last years, the combination between highgain observers and globally bounded state feedback control has gained the attention of many researchers [2, 55, 69, 70, 84]. This interest is due to robustness properties exhibited by the observer to uncertainties, unmodeled sensor and actuator dynamics, and to the ability of the observercontroller 32 0 2 4 6 8 10 0.5 0.4 0.3 0.2 0.1 0 0.1 0.2 0 2 4 6 8 10 0.05 0 0.05 0.1 0.15 0.2 0 2 4 6 8 10 0 0.5 1 1.5 2 2.5 3 0 2 4 6 8 10 5 0 5 10 real estimated real estimated w1(t) u(t) time [sec] w2(t) y(t) time [sec] time [sec] time [sec] Figure 3.6: Parameter estimation with the UKF and noisy output. 33 pair to recover the state feedback controller performance when the gain of the observer is high enough. An additional advantage of highgain observers is the possibility of using the separation principle to design controllers for a class of nonlinear systems [2, 62]. The separation principle means that a controller and an observer can be designed in two steps: • Design of a globally bounded state feedback controller based on the state vari able x that stabilizes the system and meets given speci cations. • Development of an output feedback controller by replacing the state x by its estimate ˆx. The combined observercontroller output feedback preserve the main features of the controller with the full state available, in spite of the noncorrectness of highgain observers with any xed nite gain values. One of the main drawbacks of highgain observers is that they extend the band width of the control design to unmodeled fast dynamics. The extended bandwidth does not present a problem for the actuator and sensor dynamics, preventing that their dynamics are su ciently fast relative to the dynamics of the nominal closedloop system [84, 70]. However, it is a problem when the observer estimates derivatives of noisy signals. In the following, we resume an example from [69] to show some properties of the design of output feedback controllers using highgain observers. Let a second order nonlinear system have the form x˙ 1 = x2, x˙ 2 = Á (x, u) , (3.17) y = x1. Let u = ° (x, y) be a locally Lipschitz state feedback control law that stabilizes the origin x = 0. Let the highgain observer be de ned using only measurements of the output y ˙ˆ x1 = ˆx2 + ®1 " (y − ˆx1) , ˙ˆ x2 = Á0 (ˆx, u) + ®1 "2 (y − ˆx1) , (3.18) u = ° (ˆx, y) , 34 where Á0 (x, u) nominal model of the nonlinear function Á (x, u). To satisfy the conditions of the separation principle in [2], the state feedback control and the observer must ful ll the following assumptions [70]: Assumption 3.5.1. ° (0, 0) = 0, ° (x, y) is a twicecontinuously di erentiable function over the domain of interest and a globally bounded function of x, and the origin (x = 0) is an exponentially stable equilibrium point of the closedloop system x˙ i = xi+1, 1 · i · r − 1, x˙ r = Á (x, ° (x, y)) , where r is the relative degree of the system. Assumption 3.5.2. Á0 (0, 0) = 0, Á0 (x, u) is a twicecontinuously di erentiable func tion over the domain of interest and a globally bounded function of x, " is a positive constant, and ®1 to ®r are positive constants chosen such that the roots of sr + ®1sr−1 + . . . + ®r−1s + ®r = 0 have negative real parts. It follows from the separation principle that, for su ciently small ", the output feedback controller stabilizes the origin of the closedloop system and recovers the performance of the state feedback controller in the sense that the trajectories of x under output feedback approach those under state feedback as " tends to zero. Let the estimation error be given by ˜x = " ˜x1 ˜x2 # = " x1 − ˆx1 x2 − ˆx2 # , then the error dynamics are ˙˜ x1 = − h1˜x1 + ˜x2 ˙˜ x2 = − h2˜x1 + ± (x, ˜x) with ± (x, ˜x) = Á (x, ° (ˆx)) − Á0 (ˆx, ° (ˆx)). Let the scaled estimation errors be de ned 35 as ´1 = ˜x1 " , ´2 = ˜x2. Using the scaled estimation errors, we can de ne the singularly perturbed system "´˙1 = −®1´1 + ´2, "´˙2 = −®2´1 + "± (x, x˜) . (3.19) In (3.19), note that small values of " reduce the e ects of the error model ± (·), make ´ much faster than x, and produce peaking phenomenon given by a response of the form 1 " exp µ − ®t " ¶ . This peaking phenomenon can be avoided by saturating the control input u = sat (° (x)) . Finally, the stability analysis is performed over the complete system x˙ 1 = x2, x˙ 2 = Á (x, ° (xˆ)) , "´˙1 = −®1´1 + ´2, "´˙2 = −®2´1 + "± (x, x˜) . Due to the separation principle, the stability analysis can be performed into two parts: • The stability analysis of the slow motion system (x1, x2), with " = 0, using an appropriate Lyapunov function V (x). • The stability analysis of the fast motion system "´˙ = A0´, with A0 = " −®1 1 −®2 0 # , using the Lyapunov function W (´) = ´TP0´, with P0 satisfying P0A0 + AT0 P0 = −I4. 36 3.5.1 Simulation example Let us consider the nonlinear system system (3.11) and the highgain observer ˙ˆ x1 = ˆx2 + 2 " (y − ˆy) , ˙ˆ x2 = 1 "2 (y − ˆx1) , ˆy = ˆx1. Figure 3.7 shows simulation results using control law (3.12) and di erent values of gain ". As can be seen, smaller values of " increase the convergence rate and the overshoot. This e ect is clearly noticed in Figure 3.8. The plot at the top right corner shows that the control input increases signi cantly when " decreases. Despite their short time duration, these large values of the control input may cause closedloop instability. Figure 3.9 shows same results for the case of input saturation u = sat ¡ −ˆx32 − ˆx1 − ˆx2 ¢ , as can be seen, the performance has been improved signi cantly. Figure 3.10 shows results with saturated inputs and noise. Because the HGO increases control bandwidth, the results are notoriously degraded. 3.6 Higherorder sliding mode (HOSM) observers Sliding mode control has been a key approach to solve problems under heavy uncer tainty conditions [70, 121, 135, 136]. To solve these problems, the sliding mode ap proach keeps a properly chosen constraint by means of highfrequency control switch ing. The main features of the sliding mode approach are insensitivity to external and internal disturbances, ultimate accuracy and nitetime transient. Notwithstanding, the sliding has some restrictions: If the sliding mode surface, or constraint, is de ned by an equality s = 0, where s is a system output described by a smooth function, the standard sliding mode may be implemented only if the relative degree of s is 1. Moreover, high frequency control switching leads to the socalled chattering e ect which can be dangerous in real applications. In recent years, a new generation of controllers based on higherorder sliding mode (HOSM) theory has been developed [5, 6, 7, 30, 49, 76, 78]. HOSM generalizes 37 0 2 4 6 8 0.1 0.05 0 0.05 0.1 0 2 4 6 8 0.05 0 0.05 0.1 0.15 0 2 4 6 8 0.05 0 0.05 0.1 0.15 0 0.02 0.04 0.06 0.08 0.1 0 0.05 0.1 0.15 0.2 0 2 4 6 8 2 1 0 1 2 3 4 5 6 0 0.02 0.04 0.06 0.08 0.1 2 1 0 1 2 3 4 5 6 time [sec] u(t) SFB OFB (e = 0.1) OFB (e = 0.05) OFB (e = 0.0085) time [sec] time [sec] time [sec] time [sec] time [sec] y(t) x1(t) Zoom of x1(t) x2(t) Zoom of x2(t) Figure 3.7: HGO with exact control law. 38 0 2 4 6 8 100 50 0 50 0 0.02 0.04 0.06 0.08 0.1 100 50 0 50 0 2 4 6 8 0.8 0.6 0.4 0.2 0 0.2 0 0.02 0.04 0.06 0.08 0.1 0 0.05 0.1 0.15 0.2 0 2 4 6 8 2 1 0 1 2 3 4 5 6 0 0.02 0.04 0.06 0.08 0.1 2 1 0 1 2 3 4 5 6 time [sec] u(t) SFB OFB (e = 0.1) OFB (e = 0.05) OFB (e = 0.0085) time [sec] time [sec] time [sec] time [sec] time [sec] Zoom of u(t) x1(t) Zoom of x1(t) x2(t) Zoom of x2(t) Figure 3.8: HGO behavior with unsaturated inputs. 39 0 2 4 6 8 1 0.5 0 0.5 0 0.02 0.04 0.06 0.08 0.1 1 0.8 0.6 0.4 0.2 0 0 2 4 6 8 0.05 0 0.05 0.1 0.15 0 0.02 0.04 0.06 0.08 0.1 0 0.05 0.1 0.15 0.2 0 2 4 6 8 2 1 0 1 2 3 4 5 6 0 0.02 0.04 0.06 0.08 0.1 2 1 0 1 2 3 4 5 6 time [sec] u(t) SFB OFB (e = 0.1) OFB (e = 0.05) OFB (e = 0.0085) time [sec] time [sec] time [sec] time [sec] time [sec] Zoom of u(t) x1(t) Zoom of x1(t) x2(t) Zoom of x2(t) Figure 3.9: HGO behavior with saturated inputs. 40 0 2 4 6 8 1 0.5 0 0.5 0 2 4 6 8 0.05 0 0.05 0.1 0.15 0 2 4 6 8 0.05 0 0.05 0.1 0.15 0 2 4 6 8 1 0 1 2 3 4 SFB OFB (e = 0.01) time [sec] u(t) y(t) time [sec] time [sec] time [sec] x1(t) x2(t) Figure 3.10: HGO behavior with saturated inputs and noise. 41 the basic sliding mode idea, acting on higher order time derivatives of the system deviation from the sliding surface instead of a ecting the rst deviation derivative, as standard sliding modes do. The rth order sliding mode is determined by the equalities s = s˙ = s¨ = . . . = s(r−1) = 0, where s, s˙, s¨, . . ., s(r−1) are continuous functions. In fact, the sliding mode motions also satisfy the equality s(r) = 0, but as a result of some averaging process. In particular, HOSM theory has been applied to solve the di erentiation problem in [75, 76]. The problem can be stated as follows: Let input signal f (t) be a function de ned on [0,1) consisting of a bounded Lebesguemeasurable noise with unknown features and an unknown base signal f0 (t) with the nth derivative having a known Lipschitz constant L > 0. The problem is to nd realtime robust estimations of f˙0 (t), f¨0 (t), . . ., f(n) 0 (t) which should be exact in the absence of measurement noises. The di erentiator based on HOSM designed in [76] can be resumed as z˙0 = v0, v0 = −¸0 z0 − f (t)n/(n+1) sign (z0 − f (t)) + z1, z˙1 = v1, v1 = −¸1 z1 − v0(n−1)/n sign (z1 − v0) + z2, ... (3.20) z˙n−1 = vn−1, vn−1 = −¸n−1 zn−1 − vn−21/2 sign (zn−1 − vn−2) + zn, z˙n = −¸n sign (zn − vn−1) . The main properties of this di erentiator are summarized by the following two theorems and a lemma. Theorem 3.6.1 ([76]). Given di erentiator (3.20). If the parameters are properly chosen, then the following equalities are true in the absence of input noises after a nite transient process z0 = f0 (t) , zi = f(i) 0 (t) , i = 1, . . . , n. Theorem 3.6.2 ([76]). Let input noise satisfy the inequality f (t) − f0 (t) · ". Then the following inequalities are established in nite time for some positive con 42 stants μi, ºi depending on the parameters of the di erentiator ¯¯ ¯zi − f(i) 0 (t) ¯¯¯ · μi"(n−i+1)/(n+1) , i = 0, . . . , n, ¯¯ ¯vi − f(i+1) 0 (t) ¯¯¯ = ºi"(n−i)/(n+1) , i = 0, . . . , n − 1. Lemma 3.6.3 ([94]). Consider the observer (3.20). If ¯¯ f(n+1) (t) ¯¯ · kn+1, for all t < 1, with kn+1 2 R¸0, Then the observed variables zi, i = 1, . . . , n, cannot diverge in nite time. There are two main di erences between observers (3.18) and (3.20): The rst di erence is the nitetime convergence property of the HOSM observer, in contrast to the asymptotic convergence property of the HGO observer. This property implies that the separation principle is no longer necessary. The second di erence is the boundedness of the control law. The HOSM observer is bounded by design. However, the input of the HGO observer might be saturated to avoid peaking phenomenon. It should be noticed that the only requirements for the implementation of the HOSM di erentiator is the boundedness of some higherorder derivative of its input. 3.6.1 Simulation example Let us consider the nonlinear system system (3.11) and the HOSM (3.20). Figures 3.11 to 3.14 show simulation results with the HOSM. Figure 3.11 shows simulation results using control law (3.12), and the HOSM observer ˙ˆ x1 = v1, v1 = ˆx2 − ¸0 ˆx1 − y1/2 sign (ˆx1 − y) , ˙ˆ x2 = −¸1 sign (ˆx2 − v1) , (3.21) ˆy = ˆx1 with ¸0 = 2.8 and ¸1 = 0.75. There exist a tradeo between convergence and sensitivity to input noise: the larger the parameters, the faster the convergence and the higher the sensitivity to input noise and sampling step. As can be seen in Figure 3.11, the estimated derivative of the output ˆx2 presents a notorious chattering e ect. This e ect is smoothened in its integral value ˆx1. Conse quently, an option to deal with the chattering problem is to computed a higher order 43 0 2 4 6 8 10 0.1 0.08 0.06 0.04 0.02 0 0.02 0.04 0 2 4 6 8 10 0.02 0 0.02 0.04 0.06 0.08 0.1 0 2 4 6 8 10 0.02 0 0.02 0.04 0.06 0.08 0.1 0.12 0 2 4 6 8 10 0.1 0.05 0 0.05 0.1 0.15 0.2 0.25 real estimated real estimated x1(t) u(t) time [sec] x2(t) y(t) time [sec] time [sec] time [sec] Figure 3.11: Simulation results with a rst order HOSM. 44 derivative using observer (3.20), that is ˙ˆ x1 = v1, v1 = −¸0 ˆx1 − y2/3 sign (ˆx1 − y) + ˆx1, ˙ˆ x2 = v2, v2 = −¸1 ˆx2 − v11/2 sign (ˆx2 − v1) + ˆx2, (3.22) ˙ˆ x3 = −¸3 sign (ˆx3 − v2) . Figure 3.12 shows the results of this di erentiator with ¸0 = 7.5, ¸2 = 5.0, and ¸3 = 1.5. Clearly, this version outperforms the previous one. Figure 3.13 shows results of HOSM (3.22) with measurement noise on the out put. As can be seen, the results are similar to the EKF but without using a priori information about the system. Figure 3.14 shows results with the control law (3.13), the results are quite satis factory. 45 0 2 4 6 8 10 0.8 0.6 0.4 0.2 0 0.2 0.4 0.6 0 2 4 6 8 10 0.1 0.08 0.06 0.04 0.02 0 0.02 0.04 0 2 4 6 8 10 0.02 0 0.02 0.04 0.06 0.08 0.1 0.12 0 2 4 6 8 10 0.02 0 0.02 0.04 0.06 0.08 0.1 0.12 0 2 4 6 8 10 0.1 0 0.1 0.2 0.3 0.4 0.5 0.6 real estimated real estimated x1(t) u(t) time [sec] y(t) time [sec] time [sec] real estimated x3(t) time [sec] x2(t) Figure 3.12: Simulation results with a second order HOSM. 46 0 2 4 6 8 10 0.5 0 0.5 0 2 4 6 8 10 0.1 0.08 0.06 0.04 0.02 0 0.02 0.04 0 2 4 6 8 10 0.02 0 0.02 0.04 0.06 0.08 0.1 0.12 0 2 4 6 8 10 0.02 0 0.02 0.04 0.06 0.08 0.1 0.12 0 2 4 6 8 10 0.2 0.1 0 0.1 0.2 0.3 0.4 0.5 real estimated real estimated x1(t) u(t) time [sec] y(t) time [sec] time [sec] time [sec] real estimated x3(t) time [sec] x2(t) Figure 3.13: Simulation results with a second order HOSM and measurement noise. 47 0 2 4 6 8 10 0.7 0.6 0.5 0.4 0.3 0.2 0.1 0 0.1 0 2 4 6 8 10 0.04 0.02 0 0.02 0.04 0.06 0.08 0.1 0.12 0 2 4 6 8 10 0.04 0.02 0 0.02 0.04 0.06 0.08 0.1 0.12 0 2 4 6 8 10 0.2 0.1 0 0.1 0.2 0.3 0.4 0.5 real estimated real estimated x1(t) u(t) time [sec] x2(t) y(t) time [sec] time [sec] time [sec] 0 2 4 6 8 10 0.8 0.6 0.4 0.2 0 0.2 0.4 0.6 real estimated x3(t) time [sec] Figure 3.14: Closedloop simulation results with a second order HOSM. 48 Part II Main Contributions 49 Chapter 4 Mobile robot vision Localization in mobile robot formations has drawn the attention of many researchers during the last few years. Traditionally, the control system design relies on measure ments from dead reckoning sensors. However, these measurements are completely unreliable after few meters of navigation due to encoder's low accuracy and drift. Due to reduced cost and exibility, the current trend is the design of systems using a single or a pair of cameras to determine the relative position of a robot with respect to another robot or object [15, 24, 39, 42, 53, 74, 83, 87]. For example, cameras can recognize and distinguish between objects of similar shapes, types, and colors, a task impossible to perform with range or acoustic sensors. Nonetheless, visionbased control poses new particular challenges. For instance, the controller has to be robust to camera calibration errors, nonlinearities arising from camera models, and intrinsic ambiguities of vision sensors. Localization in mobile robot formations using machine vision is part of what is known as the visual target tracking problem. This tracking can be realized with a xed camera or with a mobile camera. The visual target tracking problem can be divided into two main components: target detection, or target segmentation, and pose (distance and orientation) estimation [53]. There are several approaches for target detection in the literature. For example, in [83], the author presents a method that consists in extracting distinctive features invariant to image scale and rotation. The recognition process is divided in three steps: The rst step is a matching process between individual features and a feature database of known objects using a fast nearestneighbor algorithm. The second step identi es clusters belonging to a single object through the computation of the Hough transform. Finally, the third step veri es consistency of pose parameters using a leastsquares solution. In [117], the method presented in the previous paper is used 50 for global localization using landmarks. An alternative approach that uses feature points is the wellknown and very robust SIFT algorithm [83, 87]. This method is invariant to scale, robust to noise and occlusions, but very computationally intensive. It takes around 3s to compute features with a Pentium IV, making it inadequate for online applications. To cope with this problem, robust target detection is often achieved through the use of arti cial ducial markers. Methods using markers are referred as modelbased pose estimation methods, as classi ed in [57]. In practice, threedimensional positions of features points on the object are measured a priori and stored in a database. Then, the online detection problem consists in detecting and relating image points with the a priori selected features in order to compute the pose of the object. The problem of pose estimation using ducial markers has been mainly addressed by tangible augmented reality system applications [24, 29, 67]. The goal of these methods is to combine virtual 3D representations with the real world for active in teraction. In these systems, markers are distributed around a synthetic environment to help localize and relate the virtual camera position with the real position in the world. In [42], the author presents a visionbased system for controlling multiple robot platforms in real time using planar markers and a topview video camera for online pose estimation. In [16, 17, 38], the authors develop a monocularcamera based visual servo tracking controller for a mobile robot subject to nonholonomic motion constraints. The algorithm de nes a desired trajectory for the vehicle with a sequence of prerecorded images of three target points. In [53], the authors present a visual tracking scheme that detects the target contour using a shape adaptive sum ofsquared di erence algorithm. The target velocity is decomposed into a component caused by the target motion and a component caused by the camera motion. The lat ter component is computed using the image Jacobian, allowing an accurate estimation of the target position in the following image. There exist few applications of machine vision to the formation control problem. In [22, 114], follower robots estimate position and orientation of their leaders using a colortracking algorithm using o theshelf cameras. Each robot is equipped with a color pattern consisting of a central purple rectangle and two lateral green rectangles. The central rectangle provides an estimate of the distance based on measured and real heights. The di erence between the perceived heights of the lateral rectangles provides an estimate of the orientation of the pattern with respect to the observing robot. This algorithm is not robust to noise or pixel vibrations. In [27, 28], an onboard catadioptric camera system is used. The omnidirectional images obtained are used 51 to input data to state observers. In [26, 138, 139], the authors use segmentation from multiple central views to keep track of which pixels correspond to each leader from pictures taken with omnidirectional cameras. Pose estimation of each leader in the image plane of the follower is performed by rank constraint on the central panoramic optical ow across multiple frames. It is wellknow the massive computational power requirements of this technique. In [88, 89], the authors present an algorithm for ocking based on the measurement of timetocollision and optical ow each robot does. In these works there is no stability analysis including the e ects of measurement errors and robust methods for image interpretation. This Chapter presents a visionbased framework for mobile robot detection and tracking using o theshelf cameras mounted on mobile robots. Target detection and pose estimation are performed from single frames using ducial markers as key elements. The method consists in distributing an octagon shaped structure on the back part of each robot. These shapes are easy to extract from the images and posses unique ID codes to facilitate pose estimation. Three pose estimation methods are programmed and compared: the PRA algorithm [98], Lowe's algorithm [82, 15], and a modi ed POSIT algorithm [31, 95]. Finally, a dual Unscented Kalman lter (DUKF) is implemented to smooth measured data and estimate unknown leader's velocities. The rest of the Chapter is organized as follows: Section 4.1 summarizes the visual system and the pose estimation problem. Section 4.2 presents the ID detection ap proach. Section 4.3 describes our pose estimation algorithm. Section 4.4 reviews the leaderfollower model and the dual unscented Kalman lter. Section 4.5 shows sim ulation results for the di erent steps involved in this framework. Finally, we present our concluding remarks and future work in Section 4.6. 4.1 System overview The visual tracking problem is divided in target detection and pose estimation. Target detection is related to image processing, whereas pose estimation is related to vision and nonlinear ltering. The detection process is the most time consuming, but it can be simpli ed by using ducial markers. In our vision system, MRVision for mobile robot vision1, markers are distributed on the back part of each robot on a truncated octagon shaped structure, as can be seen in Figure 4.1. Each face of this shape has a code that not only identi es 1http://orqueda.net/research.aspx 52 the face, but also its position on the robot, as it is explained in the next Section. The ink patter is black and white to reduce lighting and camera sensitivity requirements. Figure 4.1: Scheme of the TXT platform in MPSLab. The vision library consists of synthetic and real cameras and depends on the vision library OpenCV [11] and on MPSLab, a motion planning, simulation, and virtual perception library for simulating systems in 3D environments [99, 100]. Synthetic cameras are used for tuning and testing purposes, see Figure 4.2, and real cameras are used with a PC104 for the onboard image capture and processing system. Currently, Firewire IEEE1394 and USB 2.0 cameras are supported, in particular, Unibrain's FireI, Point Grey's Bumblebee, and Logitech 3000 cameras, see Figure 4.3. Figure 4.2: Synthetic camera screen shot. Vision processing can be divided in the tasks resumed in the ow chart of Fig ure 4.4: 53 Figure 4.3: Logitech 3000 USB camera (bottomleft), PointGrey's Bumblebee (top), and Unibrain's FireI (bottomright) FireWire IEEE1394 cameras. 1. Video capture and transformation: Images from several sources with di erent formats (YUV, RGB, etc.) are transformed into OpenCV's IplImage RGB format. 2. Filtering and thresholding: The IplImage image is converted to grayscale. Then, a thresholding based on a modi ed Otsu method is performed on the grayscale image. 3. Point selection/face labeling: A search and identi cation of each face marker in the binary image is performed. First, all the contours on the binary image are extracted. Second, angular changes in each contour close to 90± are computed to determine the square enclosing each marker. After estimating the four corners of each square, a search for markers is performed. If a valid marker is found, the corresponding four corners are stored to be used for pose estimation. 4. Pose estimation: Using the algorithms presented in Section 4.3, the position of the leader robot with respect to the camera position (xc, yc, zc) and the relative rotation of the leader robot with respect to the camera plane µm are computed, see gures 2.2 and 4.5 for the de nition of the variables. 5. Transformation: The desired values are computed using (xc, yc, zc), µm, and 54 the value of the camera pan µp, as `ij = p x2c + y2 c + z2 c , (4.1) µt = arctan µ −xc zc ¶ , (4.2) ®12 = µt − µm, (4.3) µ12 = µp + µm. (4.4) Labels/points Video capture Filtering/ thresholding Color image Point selection/ face labeling Binary image Pose estimation ( c c c ) m X ,Y ,Z ,q ij ij ij ,a ,b Transformation Figure 4.4: Vision processing ow chart. 55 d Zc XI YI ZI Xc Yc j q i q Zi Xi Yi ij Yj Zj Xj Figure 4.5: Camera and robot frames. 56 4.2 Marker and ID detection The steps involved in marker and ID detection processes are: 1. Image capturing. 2. Grayscale conversion. 3. Thresholding. 4. Contour extraction and selection. 5. ID recognition. Figure 4.6 shows steps 1 to 5 applied to a synthetic image. In the following, we will resume the main characteristics of these steps. 4.2.1 Image capture Image capture is performed using standard drivers. MRVision is programmed in C++ and works on Linux and Windows platforms. It can capture images from synthetic cameras, Firewire IEEE1394 and USB 2.0 compatible cameras. After capture, the image is converted to OpenCV's IplImage format [11]. 4.2.2 Grayscale conversion Grayscale conversion is necessary to reduce the amount of data. This conversion is performed by simply extracting the green channel of the original color image. The use of just one channel is justi ed by the lowlevel of information loss. In Figure 4.7, it can be seen a comparison of di erent methods of grayscale conversion. Figure 4.7a shows the color image, Figure 4.7b, c, and d, show the red, green, blue channel images respectively. As can be seen, the blue channel image loss the most information. Figure 4.7e if the mean value image. This image does not justify the computational burden. 4.2.3 Thresholding: The Otsu method Thresholding, or bilevel thresholding, segments an image into two brightness regions: background and object. That is, for a gray level image I (x, y), with m gray levels 57 Figure 4.6: Processing sequence of the octagon shape. 58 (b) (c) (a) (d) (e) Figure 4.7: Grayscale conversion. (a) Color image, (b) red channel, (c) green channel, (d) blue channel, and (e) mean value image. 59 0, 1, . . . ,m−1, bilevel thresholding is to transform I (x, y) into a binary image Ib (x, y) by a threshold j, 0 · t · m − 1, such that Ib (x, y) = ( 0, if I (x, y) · t, 1, if I (x, y) > t. Traditionally, a xed thresholding value is used. However, in our application, lighting variations, di erent camera properties and environmental conditions make the selection of this value very di cult. For this reason, we use a modi ed version of the method due to Otsu [21, 56, 107]. This method formulates the threshold selection problem as a discriminant analysis. The method divides the gray level image histogram in two groups, A and B, then selects the threshold as the point of maximum variance between both groups. Let ni be the number of pixels with a gray level value i and n = Pm−1 i=0 ni be the total number of pixels in the image I (x, y). Let Pi = ni n be the probability of occurrence of greylevel i. Let !1 (j), M1 (j), !2 (j), M2 (j) be the number of pixels and the average gray level value in group A and group B, respectively. Then !1 (j) = Xj i=0 ni, M1 (j) = Pj i=0 i · ni !1 (j) , !2 (j) = mX−1 i=j+1 ni, M2 (j) = Pm−1 i=j+1 i · ni !2 (j) , Expressing the average gray level value MT of all pixels in image f (x, y) as MT = !1 (j)M1 (j) + !2 (j)M2 (j) !1 (j) + !2 (j) , the variance between the two groups, denoted as ¾2B (j), is ¾2B (j) = !1 (j) [M1 (j) −MT ]2 + !2 (j) [M2 (j) −MT ]2 = !1 (j) !2 (j) [M1 (j) −M2 (j)]2 !1 (j) + !2 (j) . The optimal threshold t = t¤ is the value j for which ¾2B (j) is maximum. The modi cation of Otsu's method presented in this work consists in dividing the image in 9 disjoint and 4 overlapped regions. A threshold value is computed for each region using the Otsu algorithm. Then, the values used are the average threshold 60 values of disjoint and overlapped regions. 4.2.4 Contour extraction and selection Contour extraction from the binary image is performed using the OpenCV imple mentation of Suzuki and Abe's method [123]. This method generates a collection of external contours. Then, just the contours with four strong corners are selected. The corners are chosen by exploring changes in the derivative of a given contour. When this change in the derivative is big enough, the point is classi ed as strong corner point. 4.2.5 ID recognition ID recognition is based on an unique code printed on each face of the octagon shape on the back part of the robot. After having the squares computed with the previous contour extraction and selection method, the next step is to determine which contours are related to valid robot codes. To perform this task, rst a training step creates a database with square markers of m × n cells, and assigns an ID number to each marker. This ID number is computed by a binary string whose position number is 1 if the square is lled and 0 otherwise, as shown in Figure 4.8 for m = 4 and n = 5. 1× 20 + 0 × 21 + 0× 22 +1× 23 + 0 × 24 +1× 25 + 0 × 26 +1× 27 + 0 × 28 +1× 29 +1× 210 +1× 211 +1× 212 + 0 × 213 + 0 × 214 + 0 × 215 + 0 × 216 +1× 217 + 0× 218 +1× 219 = 663209 Figure 4.8: Fiducial marker ID computation. Second, the identi cation step consists in recovering each face's ID from the per spective transformed image. To explain the method developed in this work, we intro duce some basic concepts on Projective Geometry in Appendix A. To compute the ID we use the cross ratio, an invariant to a projective transfor mation [39, 47]. The cross ratio is de ned as 61 Cr (p1, p2; p3, p4) = d13d24 d14d23 , where dij is the Euclidean distance between two points pi = [xi 1, xi 2, xi 3]T and pj = £ xj 1, xj 2, xj 3 ¤T , given by dij = vuut Ã xi 1 xi 3 − xj 1 xj 3 !2 + Ã xi 2 xi 3 − xj 2 xj 3 !2 . Therefore, if qi, i = 1, . . . , 4, denote the projective transformed points pi, i = 1, . . . , 4, then Cr (p1, p2; p3, p4) = Cr (q1, q2; q3, q4) . Using the previous contour analysis, we obtain points qNW, qSW, qNE, and qSE, see Figure 4.9. The point qc is given by the intersection of the segments sNW,SE = (qNW, qSE) and sSE,NE = (qSW, qNE), then qc = (qNW × qSE) × (qSW × qNE) . (4.5) Points v1 and v2 are called vanishing points, they could be at in nity and are given by v1 = (qSE × qSW) × (qNE × qNW) , v2 = (qNE × qSE) × (qNW × qSW) . Transformed points qW, qE, qN, and qS are given by qW =(qNW × qSW) × (v1 × qc) , (4.6) qE =(qNE × qSE) × (v1 × qc) , (4.7) qN =(qNW × qNE) × (v2 × qc) , (4.8) qS =(qSW × qSE) × (v2 × qc) . (4.9) Due to the invariability of the cross ratio, a point p = (®, ¯) belonging to the ducial marker on the original space can be univocally mapped into the point q = (®S, ¯S) on the collineated space, see Figure 4.9. Let the following invariants be 62 qW qE qNW qSW qNE qSE qN qS bW aN bE aS pW pE pNW pSW pNE pSE pN p a S b pc qc p q N qa N qb E qb S qa Figure 4.9: Projective transformation. de ned as ½® = 1 Cr (pNW, ®; pN, pNE) = 1 Cr (qNW, ®; qN, qNE) = 2 ® 1 + ® , (4.10) ½¯ = 1 Cr (pNW, ¯; pW, pSW) = 1 Cr (qNW, ¯; qW, qSW) = 2 ¯ 1 + ¯ , (4.11) where ® and ¯ are de ned with the generation of the ID. Then, the displacement ®N, ®S, ¯W, and ¯E from qN, qS, qW, and qE, respectively, can be computed as ®N = ( dNW,N ½® dNW,NE−dN,NE½® , if ® ¸ 0, dN,NE½® dNW,NE−dNW,N ½® , if ® < 0, (4.12) ®S = ( dSW,S½® dSW,SE−dS,SE½® , if ® ¸ 0, dS,SE½® dSW,SE−dSW,S½® , if ® < 0, (4.13) ¯W = ( dNW,W ½¯ dNW,SW−dW,SW½¯ , if ¯ ¸ 0, dW,SW½¯ dNW,SW−dNW,W ½¯ , if ¯ < 0, (4.14) ¯E = ( dNE,E½¯ dNE,SE−dE,SE½¯ , if ¯ ¸ 0, dE,SE½¯ dNE,SE−dNE,E½¯ , if ¯ < 0. (4.15) Finally, any position on the transformed space q is found as intersection of the seg 63 ments s® = (q®N , q®S ) and s¯ = (q¯w, q¯E) as q = (q®N × q®S ) × (q¯W × q¯E) . (4.16) In summary, the identi cation step transforms the center point of each square region using (4.16) to the collineated space and detect if the square is lled. Then, the ducial marker is obtained using the online recovered ID and the database previously created. To increase the robustness of the method, several modi cations have been per formed: • The vertical size of the square is bigger than the horizontal to take into account lower vertical camera resolutions. • In the classi cation of a region as lled, average or Gaussian lters are applied to avoid false readings due to noise. • The IDs in the database have Hamming distances2 greater or equal than dmin (dmin = 6 in our application). Then, the marker is classi ed with a given ID in the database when its Hamming distance is less than dmin /2. 4.3 Pose estimation Pose estimation refers to the issue of obtaining relative position and orientation be tween two or more mobile robots using a camera. This Section reviews the camera models and resumes the key points of the pose estimation methods used in this work. 4.3.1 Camera model The cameras used in this work are modeled with the wellknow pinhole camera model. This model can be viewed as a box with a hole, or aperture, on one of its sides. This aperture allows light to enter into the box and re ect on the opposite side of the box, where light intensity can be measured, Figure 4.10 shows an schematic representation [47]. Let pi = [xi, yi, zi, 1]T and cpi = [ cxi, cyi, czi, 1]T denote the homogeneous coordi nate vectors of a point pi in the world coordinate and the camera systems, respectively. 2The Hamming distance is de ned as the number of bits which di er between two binary strings. 64 Let the coordinate transformation T between points pi and cpi be given by a rotation R and a translation t, then cpi = T pi. (4.17) where T = [R, t]. The parameters of matrix R and vector t are known as extrinsic parameters. Using the pinhole camera model, the homogeneous image coordinate vector mi = [ui, vi, 1]T of point pi is given by ui = ® cxi czi − ® cot µs cyi czi + u0, vi = − ¯ sin µs cyi czi + v0, (4.18) where µs is the skew angle, ® = kf, ¯ = `f are magni cation parameters, f is the focal length, and k, ` are scale parameters. Note that k = ` = 1, and µs = 90± in an ideal camera. These parameters are known as intrinsic parameters. From (4.18) and (4.17), the projection of point pi on the image is given by mi = 1 czi Mpi, (4.19) where M= K h R t i and K denotes the intrinsic matrix transformation given by K = 2 64 ® ® cot µs u0 0 − ¯ sin µs v0 0 0 1 3 75 . (4.20) f Zc X Y c c V U pi = (xi, yi, zi) mi = (ui, vi) C Figure 4.10: Pinhole camera model. 65 The extrinsic matrix transformation can be represented in various forms. The most common ones are Euler angles, quaternions, and exponential maps [91]. In this work, we have chosen the exponential map representation; because it only requires three parameters to describe a rotation R and other three parameters to describe a translation t. Let ! = [!x, !y, !z]T be the rotation axis unity vector, and µ the rotation angle. Then the rotation matrix can be represented by R() = exp (µ) , (4.21) where is the skewsymmetric matrix = 2 64 0 −!z !y !z 0 −!x −!y !z 0 3 75 . (4.22) Equation (4.21) can be evaluated using Rodrigues' formula [91] R() = exp (µ) = I + sin µ µ + 2 (1 − cos µ) µ2 . (4.23) Therefore, because Euler's theorem says that any orientation R 2 SO (3) is equiv alent to a rotation about a xed axis ! 2 R3 through an angle µ 2 [0, 2¼), a given a rotation matrix can be evaluated by an exponential map. Let R be such rotation matrix R = 2 64 r11 r12 r13 r21 r22 r23 r31 r32 r33 3 75 , then µ = cos−1 µ trace (R) − 1 2 ¶ . If R 6= I, then ! = 1 2 sin µ 2 64 r32 − r23 r13 − r31 r21 − r12 3 75 . If R = I, then µ = 0 and ! can be chosen arbitrarily. 66 4.3.2 OpenGL camera model The main advantage of using a synthetic camera is the possibility of having its in ternal parameters perfectly known. OpenGL performs several transformations before drawing a point on the screen. This Section describes these transformations and obtains a synthetic intrinsic matrix transformation KOGL. Let cpi = [ cxi, cyi, czi, 1]T denote the homogeneous coordinate vector of a point pi in the camera system. Let the projection matrix P be de ned as P = 2 66664 2dn dr−d` 0 dr+d` dr−d` 0 0 2dn dt−db dt+db dt−db 0 0 0 −df+dn df−dn − 2df dn df−dn 0 0 −1 0 3 77775 , where dn, df , d`, dr, dt, and db are the parameters of the OpenGL camera frustum shown in Figure 4.11 [119]. In this work, we use dr = −d` = w 2 , dt = −db = h 2 , where w and h are the image width and height, respectively. ( ) n near d ( ) f far d ( ) t top d ( ) r right d ( ) b bottom d ( ) left d Figure 4.11: OpenGL camera frustum [119]. Applying the projection matrix to the point in eye coordinates, we obtain the point expressed in clip coordinates 2 66664 exi eyi ezi ewi 3 77775 = P 2 66664 cxi cyi czi 1.0 3 77775 , (4.24) 67 Points in clip coordinates are transformed to normalized device coordinates by 2 64 dxi dyi dzi 3 75 = 2 64 1 / ewi 0 0 0 1 / ewi 0 0 0 1 / ewi 3 75 2 64 exi eyi ezi 3 75 . (4.25) Finally, points in normalized device coordinates are transformed to windows or image coordinates using 2 64 ui vi zi 3 75 = 1 2 2 64 w 0 0 0 h 0 0 0 1 3 75 2 64 dxi dyi dzi 3 75 + 1 2 2 64 w h 1 3 75 . (4.26) Using (4.24), (4.25), (4.26) the OpenGL intrinsic matrix transformation KOGL is given by KOGL = 2 664 dnw dr−d` 0 d`w dr−d` 0 0 dnh dt−db dbh dt−db 0 0 0 − df df−dn − df dn df−dn 3 775 . Therefore, by simple comparison with (4.20), we can obtain the intrinsic parameters µ = ¼ 2 , ® = dnw dr − d` , ¯ = − dnh dt − db , u0 = d`w dr − d` , v0 = dbh dt − db . 4.3.3 Pose estimation algorithms This section summarizes and compares three modelbased pose estimation algorithms from single images: 1. Pose from orthography and scaling with iteration (POSIT) algorithm. 2. Projection ray attraction (PRA) method. 3. Lowe's method. 4.3.3.1 Pose from orthography and scaling with iteration (POSIT) method POSIT algorithm assumes that at least four feature points on the object can be de tected and matched in the image. Moreover, as every modelbased pose estimation 68 algorithm, assumes that the geometry of the object is known [31, 32, 95]. The method is an iterated version of POS (pose from orthography and scaling), which approxi mates a perspective projection with a scaled orthographic projection and then nds the transformation matrix of the object by solving a set of linear equations. The algorithm computes point projections on a plane passing through the origin of the object, as shown in Figure 4.12. Let cpi = [ cxi, cyi, czi]T , i = 1, . . . ,N, be points in the camera coordinate frame. Let R denote the rotation matrix, with R = 2 64 r1 r2 r3 3 75 = 2 64 r11 r12 r13 r21 r22 r23 r31 r32 r33 3 75 , (4.27) and t the translation vector t = [tx, ty, tz]T . (4.28) f Zc V U Xc Yc C i p 0 m i m Xo Yo Zo ' i p Figure 4.12: POSIT projections. 69 Let pi, i = 1, . . . ,N, be the points cpi in the object coordinate frame. Then cxi = r1pi + tx, (4.29) cyi = r2pi + ty, (4.30) czi = r3pi + tz. (4.31) The perspective projection of cpi with a simpli ed internal model of the camera, µs = ¼ 2 , ® = ¯ = f in (4.20), is given by ui = f r1pi + tx r3pi + tz , vi = −f r2pi + ty r3pi + tz . These projections can be written ui = i0pi + u0 ²i + 1 , (4.32) vi = j0pi + v0 ²i + 1 , (4.33) with u0 = f tx tz , v0 = −f ty tz , (4.34) i0 = f tz r1, j0 = − f tz r2, (4.35) ²i = 1 tz r3pi, i = 1, . . . ,N. (4.36) Let u 0 i = ui (²i + 1) and v 0 i = vi (²i + 1) be the coordinates of the orthographic pro jection, then u 0 i = f tz cxi, v 0 i = − f tz cyi. From (4.32), (4.33), we have i0pi = ui (²i + 1) − u0, (4.37) j0pi = vi (²i + 1) − v0. (4.38) 70 When the values ²i are known, equations (4.37) and (4.38) form a linear system of equations in which the unknown are the coordinates i0 and j0. If the values given to ²i are inexact, the solution of POS algorithm is just an approximation. However, after computing the unknowns r1 and r2, better approximation of ²i can be computed using (4.36). For each measured image point mi and corresponding object point pi, i = 1, . . . ,N, a set of equations (4.37)(4.38) can be de ned. Let these N equations be written in matrix form, then A " iT0 jT 0 # = V², (4.39) where A = 2 664 pT1 pT1 ... ... pT N pT N 3 775 , and V² = 2 664 u1 (²1 + 1) − u0 v1 (²1 + 1) − v0 ... ... uN (²N + 1) − u0 vN (²N + 1) − v0 3 775 . Therefore, the solution for i0 and j0 is " iT0 jT 0 # = ¡ ATA ¢−1 ATV². (4.40) Algorithm 4 resumes the implemented POSIT algorithm. Algorithm 4 POSIT. 1: Initialize ²i to some positive value. 2: while k²i − ²i−1k > tol do 3: Compute i0 and j0 using (4.40). 4: i0 0 = [i0 (0) , i0 (1) , i0 (2)], j00 = [j0 (0) , j0 (1) , j0 (2)]. 5: tz = f .p ki0 0k kj00 k. 6: r1 = i0 0 ki0 0k, r2 = j00 kj00 k, r3 = r1 × r2. 7: tx = tz f i0 (3), ty = −tz f j0 (3). 8: for i = 1 to N do 9: ²i = 1 tz r3pi. 10: end for 11: end while 71 4.3.3.2 Projection ray attraction (PRA) method Projection ray attraction (PRA) is a method presented in [98] to recover motion in formation from twodimensional projections of a given threedimensional point set. It is divided in two linear estimation parts: depth approximation and pose computa tion. In the depth approximation part, feature points in the threedimensional space are estimated. In the pose computation part, rotation and translation parameters of the object are estimated using singular value decomposition (SVD). Both part are iteratively executed until the result is stable. The main idea is to state pose estimation as a nonlinear optimization problem using a simpli ed camera model: Given a set of image points mi, i = 1, . . . ,N, and threedimensional feature points pi, referred to the object coordinate frame, or cpi referred to the camera coordinate frame, the pose estimation problem can be solved by minimizing the functional min R,t,di F (R, t, di) = XN i=1 kdivi − (Rpi + t)k2 , (4.41) where vi, i = 1, . . . ,N, are normalized threedimensional point representations of the image point mi, vi = Mi kMik , with Mi = 2 64 ui vi f 3 75 = f 2 64 cxi / czi −cyi / czi 1 3 75 , and di is the depth of the object, with cpi = [ cxi, cyi, czi]T = divi. Therefore, the depth approximation part of the algorithm in [98] consists in ap proximate the depth by di = vT i cpi. (4.42) The minimization problem (4.41) is solved by SVD. Let H be de ned as H = 1 N XN i=1 °°° (Mi − μM) ( cpi − μp)T °°° , 72 where μp = 1 N XN i=1 cpi, ¾2 p = 1 N XN i=1 k cpi − μpk2 , μM = 1 N XN i=1 Mi, and ¾2M = 1 N XN i=1 kMi − μMk2 . Let the SVD of H be given by H = UWV T . Then, R = USV T , t = μM − cRμp, where c = 1 ¾2 p trace (WS) . and S is given by S = ( I, if det (H) ¸ 0, diag (1, 1,−1) , if det (H) < 0, when rank (H) = 3, or S = ( I, if det (U) det (V ) = 1, diag (1, 1,−1) , if det (U) det (V ) = −1, when rank (H) 6= 3. See [98] for more details. 4.3.3.3 Lowe's method This method uses a Newtontype optimization algorithm to estimate the 6 parameter of the external matrix: three for the translation vector, and three for the rotation matrix parameterized by an exponential map. The method has the risk of converging to a false local minimum when the initial values of the unknown parameters are far from the real ones. To avoid this problem, the optimization problem is augmented by a stabilization method that incorporates a prior model of parameter uncertainty range and standard deviation estimation of each image [15, 82]. 73 As in the previous approaches, we assume that the transformation between two frames of a point cpi in the camera coordinate frame is a rotation R and a translation t. The estimated image projections of these points are given by " ui vi # = 1 czi Kcpi = 1 r3pi + tz " k11 k12 0 k22 # " r1pi + tx r2pi + ty # , (4.43) where pi and cpi are related by equations (4.17), (4.29) to (4.31). Let the optimization problem be de ned as # = arg min # XN i=1 °°° mi − ˆmi ³ ˆ# ´°°° 2 , (4.44) where # is the parameter vector # = [!x, !y, !z, tx, ty, tz]T , mi, i = 1, . . . ,N, are measured image points, and ˆmi, i = 1, . . . ,N, are estimated image points ˆmi = f ˆr3 cqi + ˆtz " ˆr1 cqi + ˆtx −ˆr2 cqi − ˆty # . Let the error eN (#) be de ned as eN (#) = 2 66666664 u1 − ˆu1 v1 − ˆv1 ... uN − ˆuN vN − ˆvN 3 77777775 2 R2N. (4.45) In general, Newtontype methods compute a vector of corrections ¢# based on the local linearity of (4.44) and error (4.45). Then #¿+1 = #¿ + ¢#, (4.46) where ¿ denotes the iteration index. In particular, in the LevenbergMarquardt algo rithm, the correction ¢# is computed as ¢# = − ¡ JT J + ¸I ¢−1 JT eN, (4.47) 74 where ¸ > 0 is an adaptive parameter, J 2 R2N×6 is the Jacobian matrix, given by J = 2 664 J1 ... JN 3 775 , (4.48) with Ji = " @ui @!x @ui @!y @ui @!z @ui @tx @ui @ty @ui @tz @vi @!x @vi @!y @vi @!z @vi @tx @vi @ty @vi @tz # , i = 1, . . . ,N. To compute the derivatives of the estimated projections with respect to tx, ty, tz, !x, !y, and !z, we need to obtain the derivatives of the rotation matrix R with respect to these parameters. Using (4.23) and (4.22), we have R() = I + c1 (µ) + c2 (µ)2, with c1 (µ) = sin µ µ , c2 (µ) = 1−cos µ µ2 , and 2 = 2 64 − ¡ !2 y + !2 z ¢ !x!y !x!z !x!y −(!2x + !2 z ) !y!z !x!z !y!z − ¡ !2x + !2 y ¢ 3 75 . Then r1 = h 1 0 0 i + c1 (µ)1 + c2 (µ)21 , (4.49) r2 = h 0 1 0 i + c1 (µ)2 + c2 (µ)22 , (4.50) r3 = h 0 0 1 i + c1 (µ)3 + c2 (µ)23 , (4.51) with 1 = h 0, −!z, !y i , 2 = h !z, 0, −!x i , 3 = h −!y, !z, 0 i , 21 = h − ¡ !2 y + !2 z ¢ , !x!y, !x!z, i , 22 = h !x!y, −(!2x + !2 z ) , !y!z i , 23 = h !x!z, !y!z, − ¡ !2x + !2 y ¢ i . 75 Using (4.49) to (4.51), we have @r1 @!x = c2 [0, !y, !z] + c3!x [0, !z, !y] + c4!x £ !2 y!2 z , !x!y, !x!z ¤ , @r1 @!y = c1 [0, 0, 1] + c2 [2!y, !x, 0] + c3!y [0, !z, !y] + c4!y £ !2 y!2 z , !x!y, !x!z ¤ , @r1 @!z = c1 [0, 1, 0] + c2 [2!z, 0, !x] + c3!z [0, !z, !y] + c4!z £ !2 y!2 z , !x!y, !x!z ¤ , @r2 @!x = c1 [0, 0, 1] + c2 [!y, 2!x, 0] + c3!x [!z, 0, !x] + c4!x £ !x!y, !2x !2 z , !y!z ¤ , @r2 @!y = c2 [!x, 0, !z] + c3!y [!z, 0, !x] + c4!y £ !x!y, !2x !2 z , !y!z ¤ , @r2 @!z = c1 [1, 0, 0] + c2 [0, 2!z, !y] + c3!z [!z, 0, !x] + c4!z £ !x!y,−!2x !2 z , !y!z ¤ , @r3 @!x = c1 [0, 1, 0] + c2 [!z, 0, 2!x] + c3!x [!y, !z, 0] + c4!x £ !x!z, !y!z, !2x !2 y ¤ , @r3 @!y = c1 [1, 0, 0] + c2 [0, !z, 2!y] + c3!y [!y, !z, 0] + c4!y £ !x!z, !y!z, !2x !2 y ¤ , @r3 @!z = c2 [!x, !y, 0] + c3!z [−!y, !z, 0] + c4!z £ !x!z, !y!z,−!2x !2 y ¤ , with c3 (µ) = µ cos µ−sin µ µ3 , c4 (µ) = µ sin µ−2(1−cos µ) µ4 . Finally, using (4.43), we have " @ui @!x @vi @!x # = 1 czi Ã" k11 k12 0 k22 # " @r1 @!x @r2 @!x # − " ui vi # @r3 @!x ! pi, " @ui @!y @vi @!y # = 1 czi Ã" k11 k12 0 k22 # " @r1 @!y @r2 @!y # − " ui vi # @r3 @!y ! pi, " @ui @!y @vi @!y # = 1 czi Ã" k11 k12 0 k22 # " @r1 @!z @r2 @!z # − " ui vi # @r3 @!z ! pi, " @ui @tx @vi @tx # = 1 czi " k11 0 # , " @ui @ty @vi @ty # = 1 czi " k12 k22 # , " @ui @tz @vi @tz # = − 1 czi " ui vi # . 76 4.4 Nonlinear ltering In this Section, we resume the leaderfollower mathematical model and an estimation algorithm using a dual unscented Kalman lter. The lter is used to smooth measured variables and to recover leader's velocities. 4.4.1 Mathematical model of the robots In general, leaderfollower controllers require reliable estimates of the leader robot's speed v1 and angular velocity !1 by the follower robot. These quantities can be estimated using an unscented Kalman lter with the appropriate dynamic model and measurements from the camera range `12, bearing ®12, and relative orientation µ12. For this reason, let us consider a leaderfollower system composed of two unicycletype vehicles moving on the plane, with the kinematic model of the ith robot, i = 1, 2, given by q˙i (t) = 2 64 x˙ i (t) y˙i (t) µ˙i (t) 3 75 = 2 64 cos µi (t) 0 sin µi (t) 0 0 1 3 75 " vi (t) !i (t) # . (4.52) where qi (t) = [xi (t) , yi (t) , µi (t)]T 2 SE (2) is the con guration vector, ui (t) = [vi (t) , !i (t)]T 2 Ui µ R2 is the velocity vector, and Ui is a compact set of admissible inputs. Let the Euclidean distance `12 (t) 2 R¸0 and angles ®12 (t), ¯12 (t) 2 (−¼, ¼] between robots 1 (leader) and 2 (follower) be de ned as `12 (t) = q (x1 (t) − xc 2 (t))2 + (y1 (t) − yc2 (t))2, (4.53) ®12 (t) = ³12 (t) − µ1 (t) , (4.54) µ12 (t) = µ1 (t) − µ2 (t) , (4.55) where ³12 (t) = atan2 (y1 − yc2 , x1 − xc 2), and xc 2 (t) = x2+d cos µ2, yc2 (t) = y2+d sin µ2 are the coordinates of the camera, as shown in Figure 2.2. Then, the leaderfollower system can be described in polar form by the set of 77 equations 2 6666666664 x˙ 1 (t) y˙1 (t) µ˙1 (t) `˙12 (t) ®˙ 12 (t) µ˙12 (t) 3 7777777775 = 2 6666666664 cos µ1 0 sin µ1 0 0 1 cos ®12 0 −sin ®12 `12 −1 0 1 3 7777777775 " v1 (t) !1 (t) # + 2 6666666664 0 0 0 0 0 0  cos ¯12 d sin ¯12 sin ¯12 `12 d cos ¯12 `12 0 1 3 7777777775 " v2 (t) !2 (t) # , (4.56) with ¯12 = ®12 + µ12. Equation (4.56) can be written in discrete form as ( xk+1 = f (xk, uk,wk) + vk, yk = h (xk, uk,wk) + nk, (4.57) where xk = [x1 (k) , y1 (k) , µ1 (k) , `12 (k) , ®12 (k) , ¯12 (k)]T , uk = [v2 (k) ,w2 (k)]T , wk = [v1 (k) ,w1 (k)]T , random variables vk and nk are process and measurement noise, respectively, and yk = [`12 (k) , ®12 (k) , ¯12 (k)]T is obtained using measure ments from the vision system (xc, yc, zc), µm, and the value of the camera pan µp. Therefore `12 = p x2c + y2 c + z2 c , µt = arctan µ −xc zc ¶ , ®12 = µt − µm, µ12 = µp + µm. 4.4.2 Nonlinear observability In this Section, we present some observability results for system (4.56). In this par ticular case, the proof of observability is direct, but it will serve the purpose of being introductory for more complex systems analyzed in Chapter 2. The supporting theory for the proofs given in this Section is resumed in Appendix C. Let model (4.56) be written in a ne in control form as § : 8>>< >>: x˙ (t) = X4 i=1 fi (x (t)) ui (t) , y (t) = h (x) , (4.58) 78 with x = [x1, y1, µ1, `12, ®12, ¯12]T 2 M µ R2 × R¸0 × S3, M is an appropriate manifold, y = [`12, ®12, ¯12]T 2 R¸0 × S2, f1 (x) = · cos µ1, sin µ1, 0, cos ®12, − sin ®12 `12 , 0 ¸T , f2 (x) = [0, 0, 1, 0, −1, 1]T , f3 (x) = · 0, 0, 0, −cos ¯12, sin ¯12 `12 , 0 ¸T , f4 (x) = · 0, 0, 0, −d sin ¯12, − d cos ¯12 `12 , −1 ¸T , h (x) = [`12, ®12, µ12]T , u1 (t) = v1 (t) , u2 (t) = !1 (t) , u3 (t) = v2 (t) , u4 (t) = !2 (t) , and we have omitted the dependency with respect to time for brevity. The observation space O is the span over R of ½ 1, `12, ®12, µ12, sin ®12 `12 , sin (®12 + µ12) `12 , d cos (®12 + µ12) `12 , cos ®12, cos (®12 + µ12) , d sin (®12 + µ12) ¾ . Then, the observability codistribution dO(x) is given by dO(x) = span {o1, o2, o3, o4, o5, o6, o7} , with o1 = [0, 0, 0, 1, 0, 0] , o2 = [0, 0, 0, 0, sin ®12, 0] , o3 = [0, 0, 0, 0, sin (®12 + µ12) , sin (®12 + µ12)] , o4 = [0, 0, 0, 0, 1, 0] , o5 = · 0, 0, 0, sin ®12 `2 12 , cos ®12 `12 , 0 ¸ , o6 = · 0, 0, 0, cos (®12 + µ12) `2 12 , sin (®12 + µ12) `12 , sin (®12 + µ12) `12 ¸ , and o7 = [0, 0, 0, 0, 0, 1] . 79 It is clear that x1, y1, and µ1 are unobservable. However, dim (dO(x)) = 3 then, by the observability rank condition theorem, see Appendix C, `12, ®12, µ12 are globally observable. 4.4.3 The dual unscented Kalman lter (DUKF) The unscented Kalman lter (UKF) uses a deterministic sampling approach, the unscented transform (UT), to capture the mean and covariance estimates with a minimal set of sample points. When the sampling points are propagated through the true nonlinear system, the UKF can capture the posterior mean and covariance accurately up to the 3rd order for Taylor series expansion of the Gaussian error distribution with same order of computational complexity as the EKF that can achieve only up to rstorder accuracy. It should also be noted that the of the UKF is the same order as that of EKF [65, 66, 140, 141]. In this Chapter, we use the dual unscented Kalman lter (DUKF) [141] described in Chapter 3. This lter runs two parallel lters for state and parameter estimation. The state estimation lter considers the parameter known for state updating, whereas its parameter estimation counterpart considers states known for parameter updating. In the leaderfollower model (4.57) the states are given by the vector xk and the parameter by vector wk. That is, leader's velocities are considered known for state estimation and then updated by the parameter estimation lter, as it is schematically represented in Figure 4.13. Note that there is no convergence guarantee with the DUKF. 1 1 ( ) vˆ ,wˆ [ ]T f v v 12 12 12 12 12 12 12 1 1 2 2 ˆ ˆ , ˆ , ˆ ˆ ˆ , ˆ , ˆ , , a q w w = = y x x Optimization 12 12 12 ˆ ˆ ˆ q a ( ) [ ]T f v v 12 12 12 12 12 12 12 1 1 2 2 ˆ ˆ , ˆ , ˆ ˆ ˆ , ˆ , ˆ , , a q w w = =   y x x UKF 12 12 12 q a Figure 4.13: Dual estimation problem. 80 4.5 Simulation results Figures 4.144.16 show identi cation results for three robots. Figure 4.14 shows faces detected when the robots are in con gurations q1 = [1.5, .25, 0.0]T and q2 = [2.0,−0.45, 0.0]T relative to the follower con guration. Figure 4.15 and 4.16 show same results with q1 = [2.5,−0.5, 0.0]T , q2 = [3.5, 0.5, 0.0]T and q1 = [2.5,−0.5, 0.0]T , q2 = £ 3.5, 0.5, ¼ 2 ¤T , respectively. The algorithm performs very well, even with small surfaces, changes in illumination and increasing distance. 2 3 4 2 3 5 1 Figure 4.14: Identi cations: Robot 1 (left) IDs 2, 3, 4, 5; robot 2 (right) IDs 1, 2, 3. 2 3 4 2 3 4 Figure 4.15: Identi cations: Robot 1 (right) IDs 2, 3, 4; robot 2 (left) IDs 2, 3, 4. Figures 4.17 and 4.18 show estimated pose for pure translation and pure rotation motions using Lowe's algorithm. 81 1 2 2 3 4 Figure 4.16: Identi cations: Robot 1 (right) IDs 2, 3, 4; robot 2 (left) IDs 1, 2. Figure 4.17: Pure translation using Lowe's method. Figure 4.18: Pure rotation using Lowe's method. 82 Figures 4.19 to 4.25 show simulations using di erent pose estimation methods for pure translation and pure rotation motions. We compared the nal error and the number of iterations to achieve convergence for each algorithm. Figure 4.19 shows the error and number of iteration of PRA algorithm without using information from previous iterations. Figure 4.20 shows same results using previous information as starting con guration. Figure 4.21 shows the error and number of iterations for pure rotations using a priori information. As can be seen, the number of iteration is considerably reduced. 0 5 10 15 0 0.02 0.04 0.06 0.08 0.1 0.12 0.14 0.16 Iteration Error 0 5 10 15 0 2000 4000 6000 8000 10000 12000 14000 16000 18000 Iteration Count Figure 4.19: Pure translation. Error and number of iterations using PRA method. Figures 4.22 and 4.23 show the results for pure translation and rotation motions using Lowe's algorithm. It should be noted that the number of iterations is lower and the nal error is smaller than in the previous case. Figures 4.24 and 4.25 show the results for pure translation and rotation motions using POSIT algorithm with a priori information. The number of iterations is even lower than in the previous case, but the nal error is bigger. A drawback of POSIT algorithm is that, due to the orthographic approximation, it needs the value of czi bigger than  cxi and  cyi to achieve convergence. Figures 4.26 to 4.28 show pose estimation results with Lowe's method initialized with POSIT algorithm. Red skeletons show the estimated positions. The distance and angle are slightly biased with accuracies around 3% and 5%, respectively. As can be seen, the estimated pose is very accurate for practical purposes. Figure 4.29 shows the e ect of noise on the image. As can be seen, the e ect reduces the achievable 83 0 5 10 15 0 0.02 0.04 0.06 0.08 0.1 0.12 0.14 0.16 Iteration Error 0 5 10 15 0 1000 2000 3000 4000 5000 6000 Iteration Count Figure 4.20: Pure translation. Error and number of iterations using PRA method and a priori information. 0 5 10 15 0 1 2 3 4 5 6 7 x 10 3 Iteration Error 0 5 10 15 0 100 200 300 400 500 600 700 800 Iteration Count Figure 4.21: Pure rotation. Error and number of iterations using PRA method and a priori information. 84 0 5 10 15 0 0.05 0.1 0.15 0.2 0.25 0.3 0.35 Iteration Error 0 5 10 15 0 200 400 600 800 1000 1200 Iteration Count Figure 4.22: Pure translation. Error and number of iterations using Lowe's method. 0 5 10 15 0 1 2 3 4 5 6 7 x 10 3 Iteration Error 0 5 10 15 0 5 10 15 20 25 Iteration Count Figure 4.23: Pure rotation. Error and number of iterations using Lowe's method. 85 0 5 10 15 0 0.05 0.1 0.15 0.2 0.25 0.3 0.35 0.4 Iteration Error 0 5 10 15 0 1 2 3 4 5 6 7 8 9 10 Iteration Count Figure 4.24: Pure translation. Error and number of iterations using POSIT algorithm. 0 5 10 15 0 0.005 0.01 0.015 0.02 0.025 0.03 0.035 0.04 0.045 Iteration Error 0 5 10 15 0 0.5 1 1.5 2 2.5 3 Iteration Count Figure 4.25: Pure rotation. Error and number of iterations using POSIT algorithm. 86 maximum distance to 3 m. Figure 4.26: Pose estimation with q1 = [1,−0.25, 0]T , q2 = [2, 0.5, 0]T . Figure 4.27: Pose estimation with q1 = [2.5,−0.5, 0]T , q2 = [3, 0.5, 0]T . Figure 4.30 shows the computation time of the vision system. The blue line shows the time needed to detect a single robot in the eld of view (roughly 10.7 msec). The red line shows the time needed to detect 3 robots plus another robot entering in the eld of view at t = 1.25 sec. Figures 4.31 to 4.37 show real, measured, and estimated relative distance `12 and 87 Figure 4.28: Pose estimation with q1 = [1.5,−0.5, 0]T , q2 = £ 4, 0.5,−¼ 2 ¤T . bearing ®12 using the DUKF. The initial values in the simulation are x0 = [1.5, 0.25, 0, 1.047, 0.233, 0.232]T , Px0 = diag (10, 10, 10) , w0 = [0, 0]T , Pw0 = diag (500, 0.01) for gures 4.31 to 4.33, and Pw0 = diag (10, 0.01) for g ures 4.34 to 4.37. It is assumed that there is a bias in the measured variables given by ¯n = [−0.0702, 0.0248, 0.0236]T , with measurement noise covariance Rn = diag (0.07, 0.03, 0.03). Figures 4.31 and 4.32 show estimation results with the DUKF in open loop. That is, referring to Figure 4.13, the real values of v1 and !1 are sent to the UKF estimator, and real state values x12 are sent to the optimization block. It can be noticed that the optimizer fails to obtain the real leader's velocities due to lack of signal richness, as shown in in Figure 4.32. To improve the richness properties and the convergence, a Gaussian noise signal with covariance Rn0 = diag (0.0035, 0.015, 0.015) is added to the measured output vector for the optimization block. The performance is notoriously increased, as shown in Figure 4.33. Figures 4.34 to 4.37 show the results of the DUKF working in closed loop. It should be noticed the accuracy in the state and velocity estimation. The mean square errors obtained with the estimations are e` = 1.821 × 10−4, e® = 5.730 × 10−5, 88 Distance = 3 m Distance = 3.5 m Distance = 2 m Distance = 2.5 m Distance = 1 m Distance = 1.5 m Figure 4.29: Fog e ect on pose estimation. 89 0 0.5 1 1.5 2 2.5 3 3.5 10 10.5 11 11.5 12 12.5 Elapsed time [msec] time [sec] 1 robot 4 robots Figure 4.30: Computational load with one robot in the eld of view (blue line) and three robots plus another entering at t = 1.25 red line). 90 x 103 0 2 4 6 8 0.2 0 0.2 0.4 0.6 0.8 0 2 4 6 8 5 0 5 10 15 0 2 4 6 8 0 0.2 0.4 0.6 0.8 1 0 2 4 6 8 0.3 0.25 0.2 0.15 0.1 0.05 0 0.05 0.1 0 2 4 6 8 0 0.2 0.4 0.6 0.8 1 0 2 4 6 8 0.01 0 0.01 0.02 0.03 0.04 real measured estimated real measured estimated real measured estimated ( ) [rad] 12 a t ( ) [m] 12 t ( ) [rad] 12 q t ( ) ( ) ˆ ( ) [rad] 12 12 e t q t q t q =  e (t) a (t) aˆ (t) [rad] a =  time [sec] time [sec] time [sec] time [sec] time [sec] time [sec] ( ) ( ) ˆ ( ) [m] 12 12 e t t t =  Figure 4.31: Relative distance and bearing estimation using a DUKF. 91 0 2 4 6 8 1 0.5 0 0.5 1 1.5 2 2.5 3 0 2 4 6 8 1.5 1 0.5 0 0.5 1 0 2 4 6 8 0 1 2 3 4 5 6 7 8 0 2 4 6 8 8 7 6 5 4 3 2 1 0 x 103 x 103 real estimated time [sec] time [sec] time [sec] time [sec] real estimated ( ) ( ) ˆ ( ) [m/ sec] 1 1 e t v t v t v =  e (t) w(t) wˆ (t) [rad / sec] w ( ) [rad / sec] =  1 w t ( ) [m/ sec] 1 v t Figure 4.32: Velocity estimation using a DUKF. 92 0 2 4 6 8 20 15 10 5 0 5 10 0 2 4 6 8 10 5 0 5 10 15 20 0 2 4 6 8 2 1 0 1 2 3 4 5 6 x 103 0 2 4 6 8 6 5 4 3 2 1 0 1 2 x 103 real estimated time [sec] time [sec] time [sec] time [sec] real estimated ( ) ( ) ˆ ( ) [m/ sec] 1 1 e t v t v t v =  e (t) w(t) wˆ (t) [rad / sec]
Click tabs to swap between content that is broken into logical sections.
Rating  
Title  Visionbased Control of Multiagent Systems 
Date  20061201 
Author  Orqueda, Omar Armando Adrian 
Department  Electrical Engineering 
Document Type  
Full Text Type  Open Access 
Abstract  Scope and methodology of study./ Creating systems with multiple autonomous vehicles places severe demands on the design of decisionmaking supervisors, cooperative control schemes, and communication strategies. In last years, several approaches have been developed in the literature. Most of them solve the vehicle coordination problem assuming some kind of communications between team members. However, communications make the group sensitive to failure and restrict the applicability of the controllers to teams of friendly robots. This dissertation deals with the problem of designing decentralized controllers that use just local sensor information to achieve some group goals. Findings and conclusions./ This dissertation presents a decentralized architecture for visionbased stabilization of unmanned vehicles moving in formation. The architecture consists of two main components: (i)� a vision system/, and (ii)�visionbased control algorithms /. The vision system is capable of recognizing and localizing robots. It is a modelbased scheme composed of three main components: image acquisition and processing, robot identification, and pose estimation. Using vision information, we address the problem of stabilizing a groups of mobile robots in leader or two leaderfollower formations. The strategies use relative pose between a robot and its designated leader or leaders to achieve formation objectives. Several leaderfollower formation control algorithms, which ensure asymptotic coordinated motion, are described and compared. Lyapunov's stability theorybased analysis and numerical simulations in a realistic tridimensional environment show the stability properties of the control approaches. 
Note  Dissertation 
Rights  © Oklahoma Agricultural and Mechanical Board of Regents 
Transcript  VISIONBASED CONTROL OF MULTIAGENT SYSTEMS By OMAR ARMANDO ADRIÁN ORQUEDA Electronic Engineer Universidad Nacional del Sur Bahía Blanca, Argentina 1996 Doctor on System Control Universidad Nacional del Sur Bahía Blanca, Argentina 2006 Submitted to the Faculty of the Graduate College of the Oklahoma State University in partial ful llment of the requirements for the Degree of DOCTOR OF PHILOSOPHY December, 2006 VISIONBASED CONTROL OF MULTIAGENT SYSTEMS Dissertation Approved: Prof. Martin Hagan Committee Chair Prof. Rafael Fierro Thesis Adviser Prof. Carlos Oliveira Prof. Weihua Sheng Dr. Gordon Emslie Dean of the Graduate College ii To my children Paulina and Tomás. iii Acknowledgements It has been a long journey since I started doing research. During this journey, I have had the pleasure of working with several researchers and learning a lot from all of them. Consequently, the list of acknowledgements is very extensive. Nevertheless, I would like to express my special gratitude to two professors from my home univer sity, Alfredo Desages (in memoriam) and Sylvia Padin. Both strongly in uenced my decision for doing research when still I was an undergraduate student. I would like to thank my advisor Dr. Rafael Fierro for his support and encour agement. In particular, I am grateful for the freedom and exibility I had to look for an approach on formation control in which I was especially interested. I am also extremely thankful for giving me the opportunity of participating in this project at Oklahoma State University. I would like to thank the members of my thesis committee Profs. Hagan (chair), Oliveira, and Sheng, for reading my dissertation and suggesting very constructive comments which help improve the contents of this work. I would like to thank Dr. Tony Zhang for his friendship and helpful discussions on the rst formulation of the output feedback controller presented in Chapter 5. Many thanks to current and past members of Marhes lab, James, Brent, Carlo, Feng, Eric, Ugur, Colby, Kyle, Chris, Lorne, Justin, Kenny, José, my friends in Stillwater and in Argentina. I would like to thank my family, specially, my two children Paulina and Tomás. They encourage me everyday to do my best work. Last but not least, I gratefully acknowledge the sources of my nancial support: This research was supported by the National Science Foundation (NSF) under grants #0311460 and CAREER #0348637, and by the U.S. Army Research O ce under Grant DAAD190310142 (through the University of Oklahoma). iv Preface Creating systems with multiple autonomous vehicles places severe demands on the design of cooperative control schemes and communication strategies. In last years, several approaches have been proposed in the literature. Most of them solve the vehicle coordination problem assuming some kind of communications between team members. Communications are used either to obtain information about neighbors' velocities or relative positions, or to acquire the control law to apply. In the former case, the control computation is decentralized and the later the control is central ized. However, communications make the group sensitive to failure, or restrict the applicability of the controllers to teams of friendly robots. This dissertation is a step forward toward the design of sensorbased decentralized architectures for stabilization of unmanned vehicles moving in formation. The archi tecture consists of two main components: (i) a modelbased vision system, and (ii) a control algorithm. The modelbased vision system can recognize and relatively lo calize neighbor robots using ducial markers on a truncated octagon shape mounted on each vehicle. It is composed of three main components: image acquisition and processing, robot identi cation, and pose estimation. The control algorithm uses vision information to stabilize a group of mobile robots in formation. Two main stabilization problems are addressed: stabilization in leader follower and in twoleaderfollower formations. Several control strategies using rela tive pose between a robot and its designated leader or leaders are presented. These strategies ensure asymptotic coordinated motion using di erent information levels to implement the controllers. The rst strategy is a partial state feedback nonlinear approach that requires full knowledge of leader's velocities and accelerations. The second strategy is a robust partial state feedback nonlinear approach that requires knowledge of the rate of change of the relative position error. Finally, the third strategy is an output feedback approach that uses highgain observers to estimate the derivative of the unmanned vehicles' relative position. In consequence, this last algorithm only requires knowledge of v the leaderfollower relative distance and bearing angle. Both data are computed using measurements from a single camera, eliminating sensitivity to information ow between vehicles. Furthermore, because the leader's exact trajectory is uncertain, this approach can be applied to both the problem of tracking a given trajectory maintaining a desired formation shape or to pursuitevasion problems, where the evader trajectory is assumed unknown. This is a distinctive aspect of this vision based architecture with respect to the current stateoftheart. Lyapunov's stability theorybased analysis and numerical simulations in a realistic tridimensional environment show the stability properties of the control approaches. Finally, we describe our ongoing implementation on virtual and real platforms and show simulation results to verify the validity of the designed methodologies. vi Contents Chapter Page List of Figures xiv I Introduction 1 1 Motivation 2 1.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2 1.2 Formations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6 1.3 Related work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9 1.4 Outline . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11 1.5 Statement of contributions . . . . . . . . . . . . . . . . . . . . . . . . 12 2 Multirobot formations 13 2.1 Mathematical model of the robots . . . . . . . . . . . . . . . . . . . . 13 2.1.1 Leaderfollower polar model . . . . . . . . . . . . . . . . . . . 14 2.2 Graph theory . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16 2.3 Formal de nition of formation . . . . . . . . . . . . . . . . . . . . . . 17 3 Nonlinear estimation 19 3.1 Optimal recursive estimation . . . . . . . . . . . . . . . . . . . . . . 20 3.2 The extended Kalman lter (EKF) . . . . . . . . . . . . . . . . . . . 21 3.2.1 Simulation example . . . . . . . . . . . . . . . . . . . . . . . . 22 3.3 The unscented Kalman lter (UKF) . . . . . . . . . . . . . . . . . . . 24 3.3.1 Simulation example . . . . . . . . . . . . . . . . . . . . . . . . 28 3.4 UKF for parameter estimation . . . . . . . . . . . . . . . . . . . . . . 30 3.4.1 Simulation example . . . . . . . . . . . . . . . . . . . . . . . . 30 3.5 Highgain observers (HGO) . . . . . . . . . . . . . . . . . . . . . . . 32 vii 3.5.1 Simulation example . . . . . . . . . . . . . . . . . . . . . . . . 37 3.6 Higherorder sliding mode (HOSM) observers . . . . . . . . . . . . . . 37 3.6.1 Simulation example . . . . . . . . . . . . . . . . . . . . . . . . 43 II Main Contributions 49 4 Mobile robot vision 50 4.1 System overview . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52 4.2 Marker and ID detection . . . . . . . . . . . . . . . . . . . . . . . . . 57 4.2.1 Image capture . . . . . . . . . . . . . . . . . . . . . . . . . . . 57 4.2.2 Grayscale conversion . . . . . . . . . . . . . . . . . . . . . . . 57 4.2.3 Thresholding: The Otsu method . . . . . . . . . . . . . . . . . 57 4.2.4 Contour extraction and selection . . . . . . . . . . . . . . . . 61 4.2.5 ID recognition . . . . . . . . . . . . . . . . . . . . . . . . . . . 61 4.3 Pose estimation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 64 4.3.1 Camera model . . . . . . . . . . . . . . . . . . . . . . . . . . . 64 4.3.2 OpenGL camera model . . . . . . . . . . . . . . . . . . . . . . 67 4.3.3 Pose estimation algorithms . . . . . . . . . . . . . . . . . . . . 68 4.3.3.1 Pose from orthography and scaling with iteration (POSIT) method . . . . . . . . . . . . . . . . . . . . 68 4.3.3.2 Projection ray attraction (PRA) method . . . . . . . 72 4.3.3.3 Lowe's method . . . . . . . . . . . . . . . . . . . . . 73 4.4 Nonlinear ltering . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 77 4.4.1 Mathematical model of the robots . . . . . . . . . . . . . . . . 77 4.4.2 Nonlinear observability . . . . . . . . . . . . . . . . . . . . . . 78 4.4.3 The dual unscented Kalman lter (DUKF) . . . . . . . . . . . 80 4.5 Simulation results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 81 4.6 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 94 5 Visionbased formation control 99 5.1 Formation mathematical models . . . . . . . . . . . . . . . . . . . . . 100 5.1.1 Leaderfollower mathematical model revisited . . . . . . . . . 100 5.1.1.1 Second order leaderfollower model with d = 0 . . . . 102 5.1.1.2 Second order leaderfollower model with d 6= 0 . . . . 104 5.1.2 Two leaderfollower mathematical model . . . . . . . . . . . . 105 5.1.2.1 Second order twoleaderfollower model with d = 0 . 108 viii 5.1.2.2 Second order twoleaderfollower model with d 6= 0 . 111 5.1.3 Generic model formulation . . . . . . . . . . . . . . . . . . . . 113 5.2 The formation control problem . . . . . . . . . . . . . . . . . . . . . 114 5.2.1 Leaderfollower control problem . . . . . . . . . . . . . . . . . 114 5.2.2 Twoleaderfollower control problem . . . . . . . . . . . . . . . 116 5.2.3 Formation dynamics, communication and sensing models . . . . . . . . . . . . . . . . . . . . . . . . . . 118 5.3 Formation control algorithms . . . . . . . . . . . . . . . . . . . . . . 118 5.3.1 Partial state feedback formation control (PSFB) . . . . . . . . 121 5.3.2 Robust state feedback formation control (RSFB) . . . . . . . . 121 5.3.3 Output feedback formation control algorithm (OFB) . . . . . 122 5.4 Backstepping . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 124 5.5 Simulation results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 126 5.6 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 149 III Conclusions 150 6 Summary and future work 151 Bibliography 152 IV Appendices 167 A Projective geometry 168 B Proofs of lemmas and theorems 171 B.1 Proof of Theorem 5.3.1 . . . . . . . . . . . . . . . . . . . . . . . . . . 171 B.2 Lemma B.2.1 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 172 B.3 Proof of Theorem 5.3.3 . . . . . . . . . . . . . . . . . . . . . . . . . . 174 C De nitions and mathematical background 178 C.1 Preliminaries . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 178 C.2 Controllability and observability . . . . . . . . . . . . . . . . . . . . . 181 C.2.1 Observability . . . . . . . . . . . . . . . . . . . . . . . . . . . 181 C.3 Inputtostate stability . . . . . . . . . . . . . . . . . . . . . . . . . . 182 ix D Perturbations, relative dynamics and degrees 184 D.1 Leaderfollower model with uncertain transformation . . . . . . . . . 184 D.2 Internal dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 185 x List of Figures Figure Page 1.1 Bats, locusts, and ants. . . . . . . . . . . . . . . . . . . . . . . . . . . 3 1.2 Aircraft formations. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4 1.3 Typical formation shapes: (a) line, (b) column, (c) diamond, (d) wedge [4]. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6 1.4 Formation positions: (a) unitcenterreferenced, (b) leaderreferenced, (c) neighborreferenced [4]. . . . . . . . . . . . . . . . . . . . . . . . . 7 2.1 Unicycletype robots. (a) ERSP Scorpion (Evolution Robotics), (b) Marhes TXT platform. . . . . . . . . . . . . . . . . . . . . . . . . . 14 2.2 Formation geometry. . . . . . . . . . . . . . . . . . . . . . . . . . . . 15 3.1 State estimation with the EKF. . . . . . . . . . . . . . . . . . . . . . 24 3.2 State estimation and control with the EKF. . . . . . . . . . . . . . . 25 3.3 State estimation with the UKF. . . . . . . . . . . . . . . . . . . . . . 28 3.4 State estimation and control with the UKF. . . . . . . . . . . . . . . 29 3.5 Parameter estimation with the UKF. . . . . . . . . . . . . . . . . . . 32 3.6 Parameter estimation with the UKF and noisy output. . . . . . . . . 33 3.7 HGO with exact control law. . . . . . . . . . . . . . . . . . . . . . . . 38 3.8 HGO behavior with unsaturated inputs. . . . . . . . . . . . . . . . . 39 3.9 HGO behavior with saturated inputs. . . . . . . . . . . . . . . . . . . 40 3.10 HGO behavior with saturated inputs and noise. . . . . . . . . . . . . 41 3.11 Simulation results with a rst order HOSM. . . . . . . . . . . . . . . 44 3.12 Simulation results with a second order HOSM. . . . . . . . . . . . . . 46 3.13 Simulation results with a second order HOSM and measurement noise. 47 3.14 Closedloop simulation results with a second order HOSM. . . . . . . 48 4.1 Scheme of the TXT platform in MPSLab. . . . . . . . . . . . . . . . 53 4.2 Synthetic camera screen shot. . . . . . . . . . . . . . . . . . . . . . . 53 xi 4.3 Logitech 3000 USB camera (bottomleft), PointGrey's Bumblebee (top), and Unibrain's FireI (bottomright) FireWire IEEE1394 cameras. . 54 4.4 Vision processing ow chart. . . . . . . . . . . . . . . . . . . . . . . . 55 4.5 Camera and robot frames. . . . . . . . . . . . . . . . . . . . . . . . . 56 4.6 Processing sequence of the octagon shape. . . . . . . . . . . . . . . . 58 4.7 Grayscale conversion. (a) Color image, (b) red channel, (c) green channel, (d) blue channel, and (e) mean value image. . . . . . . . . . 59 4.8 Fiducial marker ID computation. . . . . . . . . . . . . . . . . . . . . 61 4.9 Projective transformation. . . . . . . . . . . . . . . . . . . . . . . . . 63 4.10 Pinhole camera model. . . . . . . . . . . . . . . . . . . . . . . . . . . 65 4.11 OpenGL camera frustum [119]. . . . . . . . . . . . . . . . . . . . . . 67 4.12 POSIT projections. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 69 4.13 Dual estimation problem. . . . . . . . . . . . . . . . . . . . . . . . . . 80 4.14 Identi cations: Robot 1 (left) IDs 2, 3, 4, 5; robot 2 (right) IDs 1, 2, 3. 81 4.15 Identi cations: Robot 1 (right) IDs 2, 3, 4; robot 2 (left) IDs 2, 3, 4. . 81 4.16 Identi cations: Robot 1 (right) IDs 2, 3, 4; robot 2 (left) IDs 1, 2. . . 82 4.17 Pure translation using Lowe's method. . . . . . . . . . . . . . . . . . 82 4.18 Pure rotation using Lowe's method. . . . . . . . . . . . . . . . . . . . 82 4.19 Pure translation. Error and number of iterations using PRA method. 83 4.20 Pure translation. Error and number of iterations using PRA method and a priori information. . . . . . . . . . . . . . . . . . . . . . . . . . 84 4.21 Pure rotation. Error and number of iterations using PRA method and a priori information. . . . . . . . . . . . . . . . . . . . . . . . . . . . 84 4.22 Pure translation. Error and number of iterations using Lowe's method. 85 4.23 Pure rotation. Error and number of iterations using Lowe's method. . 85 4.24 Pure translation. Error and number of iterations using POSIT algorithm. 86 4.25 Pure rotation. Error and number of iterations using POSIT algorithm. 86 4.26 Pose estimation with q1 = [1,−0.25, 0]T , q2 = [2, 0.5, 0]T . . . . . . . . 87 4.27 Pose estimation with q1 = [2.5,−0.5, 0]T , q2 = [3, 0.5, 0]T . . . . . . . . 87 4.28 Pose estimation with q1 = [1.5,−0.5, 0]T , q2 = £ 4, 0.5,−¼ 2 ¤T . . . . . . 88 4.29 Fog e ect on pose estimation. . . . . . . . . . . . . . . . . . . . . . . 89 4.30 Computational load with one robot in the eld of view (blue line) and three robots plus another entering at t = 1.25 red line). . . . . . . . . 90 4.31 Relative distance and bearing estimation using a DUKF. . . . . . . . 91 4.32 Velocity estimation using a DUKF. . . . . . . . . . . . . . . . . . . . 92 4.33 Velocity estimation using a DUKF with noise injection. . . . . . . . . 93 xii 4.34 Control, relative distance and bearing estimation using a DUKF. . . . 95 4.35 Control and velocity estimation using a DUKF. . . . . . . . . . . . . 96 4.36 Control, relative distance and bearing estimation using a DUKF with noise injection. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 97 4.37 Control and velocity estimation using a DUKF with noise injection. . 98 5.1 Leaderfollower formation geometry. . . . . . . . . . . . . . . . . . . . 100 5.2 Two leaderfollower formation geometry. . . . . . . . . . . . . . . . . 106 5.3 Leaderfollower dynamic system: (a) complete system, (b) compact form.119 5.4 Leaderfollower (PSFB controller). . . . . . . . . . . . . . . . . . . . . 119 5.5 Leaderfollower (RSFB controller). . . . . . . . . . . . . . . . . . . . 120 5.6 Leaderfollower (OFB controller). . . . . . . . . . . . . . . . . . . . . 120 5.7 Robot trajectories for a straight line path and d = 0. . . . . . . . . . 127 5.8 Velocity comparison between controllers (5.38), (5.40), and (5.52) for a straight line path and d = 0. . . . . . . . . . . . . . . . . . . . . . . 128 5.9 Error comparison between controllers (5.38), (5.40), and (5.52) for a straight line path and d = 0. . . . . . . . . . . . . . . . . . . . . . . . 129 5.10 Control e ort comparison with " = 0.1, 0.001 for a straight line path and d = 0. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 130 5.11 Output error comparison with " = 0.1, 0.001 for a straight line path and d = 0. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 131 5.12 Snake view of a simulation with 3 robots for a straight line path and d = 0. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 132 5.13 Trajectories of the leader robot and two followers for a circular path and d = 0. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 133 5.14 Velocity comparison between controllers (5.38), (5.40), and (5.52) for a circular path and d = 0. . . . . . . . . . . . . . . . . . . . . . . . . 134 5.15 Output error comparison between controllers (5.38), (5.40), and (5.52) for a circular path and d = 0. . . . . . . . . . . . . . . . . . . . . . . 135 5.16 Control e ort comparison with " = 0.1, 0.001 for a circular path and d = 0. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 136 5.17 Output error comparison with " = 0.1, 0.001 for a circular path and d = 0. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 137 5.18 Snake view of a simulation with 3 robots for a circular path and d = 0. 138 5.19 Trajectories of the robots with d = 0. . . . . . . . . . . . . . . . . . . 139 5.20 Velocities of robot 5 using the twoleader algorithm with d = 0. . . . 140 xiii 5.21 Output error of robot 5 using the twoleader algorithm with d = 0. . . 141 5.22 Snake view of a simulation with 7 robots with d = 0. . . . . . . . . . 142 5.23 Speci cations for 7 robots. . . . . . . . . . . . . . . . . . . . . . . . . 142 5.24 Trajectories of the robots for a straight line path and d 6= 0. . . . . . 143 5.25 Error comparison for a straight line path and d 6= 0. . . . . . . . . . . 144 5.26 Snake view of a simulation with 7 robots for a straight line path with d 6= 0. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 145 5.27 Trajectories of the robots for a circular path and d 6= 0. . . . . . . . . 146 5.28 Error comparison for a circular path and d 6= 0. . . . . . . . . . . . . 147 5.29 Snake view of a simulation with 7 robots for a circular path with d 6= 0. 148 A.1 Crossratio invariance. . . . . . . . . . . . . . . . . . . . . . . . . . . 169 A.2 Vanishing points. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 170 xiv Part I Introduction 1 Chapter 1 Motivation 1.1 Introduction In the last decade, cooperative multirobot systems have been increasingly being considered as a mean of performing complex tasks within dynamic environments. These complex and extremely diverse tasks include applications such as automated transportation, spacecraft interferometry, surveillance, mapping, border patrol, search and rescue, disaster relief, unmanned air combat, multitargeting and multiplatform operations, military battle systems, highway and air transportation systems, mobile sensor networks, and wireless surveillance. The main reason for engaging multi robot systems in collective behavior is their expected outperformance over single robot systems in size, cost, exibility, and fault tolerance [43, 90]. The design of multirobot formations was initially inspired by the emergent self organization from social interaction observed in nature. For example, bees, birds, sh, wildebeests, and human beings ock or swarm in particular directions [120], see Figure 1.1. Moreover, it is wellknown that aircraft formations y in a 'V' to decrease drag, save energy, and increase safety, Figure 1.2. As Camazine et al. pointed out [13]: Selforganization is a process in which pattern at the global level of a system emerges solely from numerous interactions among the lowerlevel components of the system. Moreover, the rules specifying interactions among the system's components are executed using only local informa tion, without reference to the global pattern. In short, the pattern is an emergent property of the system, rather than a property imposed on the system by an external ordering in uence ... pattern is a particular, 2 Bats  T.K. Horiuchi  http://www.isr.umd.edu/Labs/CSSL/horiuchilab/lab.html Desert locusts – D. Grünbaum Ants – C. Anderson http://www2.isye.gatech.edu/~carl/ Figure 1.1: Bats, locusts, and ants. 3 http://www.skyflash.com Figure 1.2: Aircraft formations. 4 organized arrangement of object in space or time. Single individuals may use relatively simple behavioral rules to generate structures and patterns at the collective level that are more complex than the components and processes from which they emerge. For instance, in the mid40s Walter, Wiener and Shannon observed complex social behavior in the interaction between simple turtle robots equipped with light and touch sensors [14, 112]. Therefore, to understand selforganization and use it in a productive fashion, two main questions have to be answered: The rst question is related to the problem of how global behaviors can be fostered using local and relatively simple rules. The second question is related to the problem of which sensing capability needs each individual. Regarding to the rst question, studies on ocking and herding mechanisms in dicate that they emerge as a combination of a desire to stay in the group and yet simultaneously keep a minimum separation distance from other members of the ock [96]. In [52], Tu and Toner prove that this combination has to be complemented with motion, by proving that group alignment is not possible in ocks and herds with local perception in the absence of movement. According to recent models from theoretical physics [51, 137], a key factor is the density of animals in the group. If this density increases, a rapid transition occurs from disordered movement of indi viduals to highly aligned collective motion. Two of the most signi cant advances in collective motion and synchronization are the models conceived by Vicsek et al. [137] and the Kuramoto [63, 64, 111]. Vicsek designed a discretetime and stochastic model that analyzes with simple interactions the emergence of di erent behavior pat terns. Kuramoto's model was originally developed for oscillator synchronization, it is a continuoustime and deterministic approach very suitable for analysis of collective behavior emergence without noise. Regarding to the second question, nature also give guidelines on which and how many sensors are necessary to achieve coordinate behavior. For example, bats sense their surroundings using ultrasound. Birds, mammals, and most insects use vision as their main sensor. In Figure 1.2, the pilot can maintain formation by knowing the relative distance, heading, and bearing between airplanes. In summary, it can be stressed that members of ocks of birds or schools of sh do not use explicit communication but local sensing, usually vision, in order to maintain a coherent formation or a coordinated motion, even when they have to move around obstacles or avoid predators. This dissertation nds its main motivation in solving the local control problem using visionbased sensing, showing that a stable local behavior 5 Figure 1.3: Typical formation shapes: (a) line, (b) column, (c) diamond, (d) wedge [4]. can be achieved and maintained without using communications. 1.2 Formations Intuitively, a formation is a group of agents maintaining a determined relationship. The two most important characteristics of a formation are its shape and its position. The most common formation shapes are line, column, diamond, and wedge [4], as shown in Figure 1.3. Common de nitions of formation positions are unitcenter referenced, leader refer enced, and neighbor referenced [4]. In a unitcenterreferenced position, all the robots maintain a relative position to a center point given by the average of the coordi nates px and py of each member of the formation. In a leaderreferenced position the robots maintain a relative position with respect to a leader robot. Finally, in a neighborreferenced position, each robot maintains a position relative to one other predetermined robot. Figure 1.4 depicts these three types of formation position de  nitions. 6 Figure 1.4: Formation positions: (a) unitcenterreferenced, (b) leaderreferenced, (c) neighborreferenced [4]. Types of formations There exist several types of formations. Here we resume some of the more relevant to our work. Flocking The earliest application of arti cial formation behavior starts with the pioneering work of Craig Reynolds [115]. In his work, the author addresses the problem of simulating ocks of birds and schools of sh with a simple egocentric behavioral model. This model consists of collision avoidance, velocity matching, and formation keeping components. Also, three heuristic rules are de ned: • cohesion rule  aim of keeping proximity to nearby ockmates, • separation rule  desire of collision avoidance, • alignment rule  intention of velocity matching with neighbors. These three rules are inherently local and give to each member the possibility of navigating using only its sensing capabilities. From a mathematical point of view, they allow to pose the ocking problem as a decentralized control problem. Moreover, the superposition of these three rules results in all agents moving in a loose (as opposite to rigid) formation, with a common heading while avoiding collisions [128, 129]. 7 The work of Reynolds led to the creation of the rst computer animated ocking. Several lms and video have been produced using ocking, such as Stanley and Stella in: Breaking the Ice (1987), Batman Returns (1992), Cli hanger (1993), The Lion King (1994), From Dusk Till Dawn (1996), The Hunchback of Notre Dame (1996), and Finding Nemo (2003). Behaviorbased formation The main idea of behaviorbased formations is to integrate several goal oriented be haviors with a given arbiter that chooses between them. It is a highly decentralized reactive framework. In [4] for example, the authors de ne two steps for formation maintenance: (i) determination of the robot's position in formation; and (ii) gener ation of motor commands to direct the robot toward the correct location. A global arbiter gives priorities to each behavior depending on environmental conditions. The method performs well in simulations and experiments, but it lacks mathe matical analysis of convergence and robustness. Leaderfollowing formation Leader following can be centralized or decentralized. When a decentralized approach is used, each robot can drive itself keeping a desired distance to some of its neighbors. This scheme typically has one leader which takes care of the assignment of followers' relative positions and of the global mission objective, for instance, obstacle avoidance [33, 34, 125]. The major weakness of singleleader architectures is that they have a single point of failure [73]. Therefore, stability properties of these architectures under changes of leaders is a major concern. Some attempts have been made in this eld. For instance, in [27, 28], the architecture has a high level layer that detects leader failures and rede nes the formation control graph according to some given strategy. Failures include robot malfunctioning, communication errors (in communicationbased formations), and information ow breaches. Virtual structure Virtual structure approaches describe the entire formation as one single rigid body or pattern. In [3], for example, a virtual structure or pattern formation is de ned as the problem of coordination a group of robots to get into and maintain a formation with 8 a certain shape such as a wedge or a line, assuming the existence of a communication channel between the central unit and the individual robots. These approaches are centralized and, consequently, they are very susceptible to operation errors. 1.3 Related work Creating systems with multiple autonomous vehicles working together to achieve a common mission within a changing environment places severe demands on the de sign of decisionmaking supervisors, cooperative control schemes, and communication strategies. Research on multivehicle system coordination has been focused both on centralized and decentralized control strategies. Centralized control strategies have the advantage of being able to reach a global optimum solution for tasks such as path planning and recon guration [8, 12, 108, 143]. However, centralized algorithms be come infeasible when the number of vehicles and constraints increase, preventing their implementation in realtime. On the other hand, decentralized control approaches only require local information and can e ectively achieve multivehicle coordination behaviors as the ones observed in nature [28, 27, 63, 72, 73, 88]. Many approaches for solving multirobot coordination reduce the general control problem to a singleagent control one by assuming that global communication of some coordination information is available. However, a coordination mechanism that does not rely on global communication ensures exibility and mission safety, because ref erence trajectories and mission objectives should not be shared among all agents but to some leaders [20]. Of course, this poses the challenge of designing new formation control paradigms, robust and computationally simple, that can perform well in the presence of uncertainty in leaders' current and future states. Other authors have addressed the coordination problem of multiple unmanned vehicles using optimization techniques [10]. Contributions in this area include work focused on autonomous distributed sensing tasks [25], decentralized optimization based control algorithms [41], optimal motion planning [9], and formation recon g uration planning [143]. More recently, the use of model predictive control (MPC) or recedinghorizon control (RHC) is becoming popular in the multirobot system literature [12, 36, 46, 68, 105]. Generally speaking, MPC algorithms rely on the optimization of a predicted model response with respect to the system input to deter mine the best input changes for a given state. Either hard constraints (that cannot be violated) or soft constraints (that can be violated but with some penalty) can be 9 incorporated into the optimization problem, giving to MPC a potential advantage over passive state feedback control laws. However, there are possible disadvantages to be considered. For instance, in its traditional use for process control, the primary disadvantage of MPC is the need of a good model of the plant, but such model is not always available. In robotics applications, the foremost disadvantage is the com putational cost, negligible for slowmoving systems in the process industry, but very important in realtime applications. In [105], a pathspace iteration (PSI) algorithm is designed to solve the motion coordination problem. In this work, the authors present centralized and decentralized algorithms based on an MPC/PSI formulation to solve nonholonomic multirobot coordination problems in dynamic, unknown en vironments. Chen et al. [20, 18, 19] develop a decentralized control architecture that employs local sensor information and an internal model approach [60, 61] to handle uncertain ties in the leader reference trajectory. The information needed to implement the con troller is relative position and velocity between a robot and its leader. This approach uses system immersion to model the known leader's movements and communications to acquire leader's heading. Leader velocities are piecewise linear functions of time over nite intervals of variable length. Intervehicle collision avoidance is assured by con ning vehicles to speci c sectors during transients. In the last few years, some visionbased motion coordination algorithms have been developed in the literature. In [138], authors design a formation control algorithm based on omnidirectional visual servoing and motion segmentation. Visionbased for mation controllers are described in [26]. The algorithms use inputoutput lineariza tion and require the estimation of leaderfollower relative angle and leader's linear and angular velocities. Two algorithms are described for imagebased leaderfollower formation control, one is based on feedback linearization and the other combines Luenberger observers with a linear controller. In [85], the authors give a su cient condition for observability using a visionbased centralized controller. The control law is based on inputoutput feedback linearization and assumes that the robots have omnidirectional cameras and that the leader can transmit the velocity control to each follower and estimate their states with an extended Kalman lter. Another visionbased formation controller has been recently developed in [88, 89]. This distributed coordination approach is based on nearestneighbor interactions, assuming that robots move with constant linear speed and achieve ocking after a given time. In general, ocking algorithms do not maintain a strict formation shape. Such formation maintenance is critical in applications such as cooperative payload 10 transport [126], cooperative object pushing [50], and distributed sensor deployment when robots move forming certain geometric pattern [142]. In this dissertation, we present di erent approaches for solving a multirobot co ordination problem using monocular vision. The rst part of the dissertation presents an arti cial vision system for pose estimation using ducial marks [103, 104]. The second part presents several visionbased formation control algorithms for leader fol lowing and twoleaders following architectures [106, 102, 101]. The rst two are state feedbackbased controllers that require total or partial knowledge of the state vari ables, respectively. The third is a robust output feedback decentralized controller based only on monocular vision information about the relative motion between a robot and its designated leader or leaders. A highgain observer is used to esti mate the derivatives of the measurements. This last algorithm eliminates the need of intervehicle communications which increases the reliability of the overall system, making this approach suitable for pursuitevasion problems, when the pursued is an unfriendly robot. Finally, we also describe our ongoing implementation on virtual and real platforms. 1.4 Outline The rest of the dissertation is organized as follows. In Chapter 2, we review some de nitions on graph theory and robot formations that are used on the main two chap ters of this dissertation. Chapter 3 resumes four nonlinear estimation algorithms and compare their properties through simple examples. Chapter 4 presents the imple mentation of real and virtual vision systems for robot identi cation and localization. Chapter 5 describes and analyzes visionbased decentralized formation control algo rithms. Finally, we present our concluding remarks and future work in Chapter 6. 11 1.5 Statement of contributions This dissertation is a step forward toward the design of sensorbased decentralized architectures for stabilization of unmanned vehicles moving in formation. The con tributions can be divided into two main parts: • A vision system for robot identi cation and localization. • A set of robust visionbased formation controllers. The vision system, composed of image acquisition and processing, robot identi ca tion, and pose estimation, can recognize and relatively localize neighbor robots using ducial markers. The set of robust visionbased formation controllers allows to maintain relative distance and bearing in leaderfollower and twoleaderfollower formations. These controllers ensure asymptotic coordinated motion coordination using di erent infor mation and sensing levels. To the author's knowledge, the whole architecture is the rst framework that ap plies a realistic sensor system and control algorithms to the formation control problem with guaranteed stability and without communications. 12 Chapter 2 Multirobot formations In this Chapter, some basic concepts relevant to multirobot formations are summa rized. We brie y review the dynamical model of the robot and the polar relationship between a leader and its follower, that is, the basic cell of a formation. We resume some tools for formation characterization using graph theory and the formal de ni tion of a formation. The reader is referred to the bibliography for a more detailed treatment [35, 133]. 2.1 Mathematical model of the robots Let us consider a multirobot system composed of Na agents, as the ones shown in Figure 2.1, modelled as unicyclelike vehicles1 §i moving on the plane, with the dynamic model of the ith robot given by [20] x˙ i (t) = vi (t) cos µi (t) , y˙i (t) = vi (t) sin µi (t) , µ˙i (t) = !i (t) , (2.1) miv˙i (t) = Fi (t) , Ji!˙ i (t) = Ti (t) , where qi (t) = [xi (t) , yi (t) , µi (t)]T 2 SE (2), is the con guration vector with respect to an inertial reference frame, (xi, yi) 2 R2 denotes the position in Cartesian coor dinates and is the intersection of the axis of symmetry with the driving wheels axis, µi 2 S = (−¼, ¼] is the heading angle, ui (t) := [vi (t) , !i (t)]T 2 Ui µ R2, vi (t), 1Note that other mathematical models can be adapted to this framework. 13 Figure 2.1: Unicycletype robots. (a) ERSP Scorpion (Evolution Robotics), (b) Marhes TXT platform. !i (t) are the linear and the angular velocities, respectively, Ui is a compact set of admissible velocities containing the origin, and Fi (t), Ti (t) 2 R are the admissible control signals. Without lost of generality, we assume that all sets Ui are equal, then U = Ui := {(vi, !i) vi · vmax, !i · !max} , and that the masses and inertia moments are equal to one mi = Ji = 1, with i = 1, . . . , Na. As can be seen in Figure 2.1, a monocular camera is mounted on each robot. In an ERSP Scorpion robot, the camera is mounted on the intersection of the axis of symmetry with the driving wheels axis, see Figure 2.1a. In a Marhes TXT platform, the camera is mounted d units ahead on the axis of symmetry, see Figure 2.1b. 2.1.1 Leaderfollower polar model Let the Euclidean distance `ik (t) 2 R¸0 and the angles ®ik (t), µik (t) 2 S = (−¼, ¼] between robots i and k be de ned as `ik (t) = q (xi (t) − xc k (t))2 + (yi (t) − yck (t))2, (2.2) ®ik (t) = ³ik (t) − µi (t) , (2.3) µik (t) = µi (t) − µk (t) , (2.4) 14 where ³ik (t) = atan2 (yi (t) − yck (t) , xi (t) − xc k (t)), and xc k (t) = xk (t) + d cos µk (t), yck (t) = yk (t) + d sin µk (t) are the coordinates of the camera, as shown in Figure 2.2. XI YI Robot k ik b ik a ik i q p q k q Robot i c Z d  Xc m q ik z Figure 2.2: Formation geometry. Then, assuming that both robots are described by the model (2.1), the leader follower system can be described in polar form by the set of equations 2 6666666664 x˙ i (t) y˙i (t) µ˙i (t) `˙ik (t) ®˙ ik (t) µ˙ik (t) 3 7777777775 = 2 6666666664 cos µi 0 sin µi 0 0 1 cos ®ik 0 −sin ®ik `ik −1 0 1 3 7777777775 ui (t)+ 2 6666666664 0 0 0 0 0 0 −cos (®ik + µik) −d sin (®ik + µik) sin(®ik+µik) `ij −d cos(®ik+µik) `ik 0 −1 3 7777777775 uk (t) . (2.5) As mentioned, each agent is equipped with a vision sensor and a pan controller, as shown in Figure 2.2. Then, the measured variables are given by µt (t) = arctan µ −xc (t) zc (t) ¶ , ®ik (t) = µt (t) − µm (t) , µik (t) = µp (t) + µm (t) . 15 2.2 Graph theory Graph theory provides a convenient framework for modeling multivehicle coordina tion problems. Particularly, it is very powerful to generalize individual properties to group properties [133]. For example, graphs have been used to capture the intercon nection topology [37, 97], describe control structure [28, 27], test constraint feasibility [125], characterize information ow [40], and analyze error propagation [133]. Graphs can be undirected to model position constraints [37, 97], or directed to describe in formation ow [40] or leader following interagent control speci cations [34, 127, 132] In this Section, we resume some concepts from graph theory, the reader is referred to the literature for a detailed treatment [35, 96, 133]. A directed graph G is a pair (V, E) of the set of vertices V 2 {1, . . . ,N} and directed edges E 2 V ×V. An edge (i, j) 2 E is an ordered pair of distinct vertices in V that it is said to be incoming with respect to the head j and outgoing with respect to the tail i. The in degree of a vertex is de ned as the number of edges that have this vertex as head. If i, j 2 V and (i, j) 2 E, then i and j are said to be adjacent, or neighbors, and are denoted by i » j. The adjacency matrix A(G) = {aij} of a graph G is a matrix with nonzero elements such that aij 6= 0 , i » j. The set of neighbors of node i is de ned by Ni := {j 2 V\ {i} aij 6= 0} . (2.6) The number of neighbors of each vertex is its valence. A path of length rp from vertex i 2 V to vertex j 2 V is a sequence of rp+1 distinct vertices, v1 = i, . . . , vk, . . . , vrp+1 = j, such that for all k 2 [1, rp], (vk, vk+1) 2 E. A weak path of length rp from vertex i 2 V to vertex j 2 V is a sequence of rp+1 distinct vertices, v1 = i, . . . , vk, . . . , vrp+1 = j, such that for all k 2 [1, rp], (vk, vk+1) 2 E or (vk+1, vk) 2 E. A directed graph G is weakly connected or simply connected if any two vertices can be joined with a weak path. The distance between two vertices i and j in a graph G is the length of the shortest path between both vertices. The diameter of a graph is the maximum distance between two distinct vertices. The incidence matrix B (G¾) of a graph G with orientation ¾, G¾, is a matrix whose rows and columns are indexed by the vertices and edges of G, respectively, such that the (i, j) entry of B (G¾) is equal to 1 if edge (i, j) is incoming to vertex i, −1 if edge (i, j) is out coming from vertex i, and 0 otherwise. The symmetric matrix de ned as L(G) = B (G¾)B (G¾)T 16 is called the Laplacian of G and is independent of the choice of orientation ¾. For a connected graph, L has a single zero eigenvalue and the associated eigenvector is the ndimensional vector of ones, 1n. A cycle is a connected graph where each vertex is incident with one incoming and one outgoing edge. An acyclic graph is a graph with no cycles. Let pi = (pxi , py i ) 2 R2 denote the position of robot i, and ri > 0 denote the interaction range between agent i and the other robots. A spherical neighborhood (or shell) of radius ri around pi is de ned as B (pi, ri) := © q 2 R2 : kq − pik · ri ª . Let us de ne p = col (pi) 2 R2n, where n = V is the number of nodes of graph G, and r = col (ri). We refer to the pair (p, r) as a cluster with con guration p and vector of radii r. A spatial adjacency matrix A(p) = [aij (p)] induced by a cluster is given by aij (p) = ( 1, if pj 2 B (pi, ri) , j 6= i 0, otherwise. The spatial adjacency matrix A(p) de nes a spatially induced graph or net G (p). A node i 2 V with a spherical neighborhood de ne a neighboring graph Ni as Ni (p) := {j 2 V : aij (p) > 0} . (2.7) An ®lattice [96] is a con guration p satisfying the set of constraints kpj − pik = d, 8j 2 Ni (p) . A quasi ®lattice [96] is a con guration p satisfying the set of inequality constraints −± · kpj − pik − d · ±, 8 (i, j) 2 E (p) . 2.3 Formal de nition of formation Now, it is possible to give a formal de nition of formation [133]: De nition 2.3.1 (Formation). A formation is a network of vehicles interconnected via their controller speci cations that dictate the relationship each agent must main tain with respect to its leader or leaders. The interconnections between agents are modeled as edges in an acyclic directed graph, labeled by a given relationship. 17 Using the robot model presented in (2.5), the relationship between two robots could be distance and bearing angle or just distance. In the former case, the in degree of the follower is one. Whereas in the latter case, the indegree of the follower becomes two. De nition 2.3.2 (Formation Control Graph). A formation control graph G = (V, E, S) is a directed acyclic graph consisting of the following: • A nite set V = (v1, . . . , vN) of N vertices and a map assigning to each vertex vk a control system (4.52). • An edge set E ½ V × V encoding leaderfollower relationships between agents. The ordered pair (vi, vk) := eik belongs to E if uk depends on the state of agent i, qi. • A collection S = {sk} of node speci cations de ning control objectives, or set points, for each node k :(vi, vk), (vj , vk) 2 E and for some vi, vj , vk 2 V. Remark 2.3.3. Note that j ´ i when the relationship between robots i and k is given by distance and bearing. Remark 2.3.4. Some examples of control objectives are collision avoidance, or main tenance of communication or sensing links. In the rst case, this objective can be de ned by the minimum distance two robots should keep. The second and third cases can be speci ed by a maximum distance between robots and a given angular range to be achieved. The tails of all incoming edges to a vertex k represent leaders of robot k. Vertices of indegree zero represent formation leaders. Leader agents have no incoming edges; because they regulate their behavior such that the formation may achieve some group objectives, such as navigation avoiding obstacles or tracking reference paths. Therefore, the formation control problem can be divided into two parts: graph as signment and controller design. The rst problem consists in designing the formation control graph G and the speci cation collection S. The second problem consists in maintaining the formation described by the pair G and S in the presence of constrains, obstacles, or limited sensing capabilities. In this dissertation, we assume that G and S are preassigned and mainly focus on the second problem. 18 Chapter 3 Nonlinear estimation Many control algorithms rely on the basic design assumption of exact knowledge of the system model and availability of measurements of every state. For example, the widely used state feedback control techniques [27, 34, 44, 58, 69]. However, in real applications, measurements acquired with one or several sensors are noisy and uncertain and some states are not available. For instance, mobile robots carry sensors such as wheel encoders, inertial units, accelerometers, gyroscopes, and cameras. The data of all these sensors should be smoothened and/or fused to estimate the real values of the system variables using estimation lters [92, 109, 113, 116]. On the other hand, observers can be used to estimate unavailable variables [58, 70, 93, 122]. In particular, output di erentiators play a central role in the estimation of the derivatives of some states. Two of the most wellknown estimation tools available nowadays are the extended Kalman lter (EKF) [23, 86] and the unscented Kalman lter (UKF) [65, 66, 140, 141]. The EKF relies on a linearization using a Taylor series expansion of a nonlinear sensor model. Because the linearization is performed using uncertain states, it can produce the divergence of the lter. The UKF performs the linearization process in the Kalman lter using the un scented transform. The unscented transform is a deterministic sampling approach that propagates through the true nonlinear system a set of chosen sample points to completely describes the true mean and covariance of a Gaussian random variable. The propagated points can capture the posterior mean and covariance accurately up to the 3rd order of a Taylor series expansion. The UKF outperforms the EKF in nonlinear system applications. Both lters, rely on an accurate model description of the system. In the last few years, two state observers have emerged: the highgain observer (HGO) [1, 2, 55, 19 69, 70, 84], and the highorder sliding mode (HOSM) observer [5, 6, 7, 49, 76, 78, 77]. The advantage of both observers is that they do not need an accurate system model, because they are particularly appropriate to deal with unmodeled dynamics and perturbations. The rest of this Chapter is organized as follows. In Section 3.1, we resume basic concepts on recursive estimation. In Section 3.2, we review the extended Kalman algorithm and analyze a simple nonlinear example. In Section 3.3, we describe the unscented Kalman lter and compare it with the EKF. In Section 3.5, we study the performance of highorder observers. In Section 3.6, we survey the highorder sliding model observer and its use as di erentiator. 3.1 Optimal recursive estimation Let the state vector be given by x 2 Rn, and let the system be described by the nonlinear stochastic di erence equation xk+1 = f (xk, uk, vk) , yk = h (xk, nk) , (3.1) where xk is the unobserved state of the system, uk 2 Rm is a known exogenous input, yk 2 Rp is the observed measurement signal, vk and nk are process and measurement noise, respectively. Variables vk and nk are assumed independent, white, and with Gaussian probability distributions p (v) » N (0,Rv) and p (n) » N (0,Rn). The goal is to estimate the state xk using observations yk. The optimal estimate in the minimum meansquared error (MMSE) sense is given by the conditional mean [140, 141] ˆxk = E £ xk ¯¯ Yk 0 ¤ , (3.2) where Yk 0 is the sequence of observations up to time k. In order to solve equation (3.2) it is necessary the knowledge of the a posteriori density p ¡ xk ¯¯ Yk 0 ¢ . The a posteriori density can be computed using the Bayesian approach p ¡ xk ¯¯ Yk 0 ¢ = p ¡ xk ¯¯ Yk−1 0 ¢ p (yk xk ) p ¡ yk ¯¯ Yk−1 0 ¢ , (3.3) where p ¡ xk ¯¯ Yk−1 0 ¢ = Z p (xk xk−1 ) p ¡ xk−1 ¯¯ Yk−1 0 ¢ dxk−1, (3.4) 20 and the normalizing constant p ¡ yk ¯¯ Yk−1 0 ¢ is given by p ¡ yk ¯¯ Yk−1 0 ¢ = Z p ¡ xk ¯¯ Yk−1 0 ¢ p (yk xk ) dxk. (3.5) Equation (3.3) speci es the current state density as a function of the previous den sity and the most recent measurement data. Knowledge of the initial condition p (x0 y0 ) = p(y0x0 )p(x0y0 ) p(y0) determines p ¡ xk ¯¯ Yk 0 ¢ for all k. Statetransition proba bility p (xk xk−1 ) and measurement probability p (yk xk ) are speci ed by the state space model (3.1) and the densities p (vk), p (nk). However, the multidimensional integrations given by (3.4) and (3.5) cannot not be computed in closedform for most systems. As performed in the particle lter approach, the integral can be approx imated by nite sums by applying MonteCarlo sampling techniques. These nite sums converge to the true solution in the limit. Assuming that all densities remain Gaussian, then the Bayesian recursion can be greatly simpli ed because only the conditional mean ˆxk = E £ xk ¯¯ Yk 0 ¤ and covariance Pxk need to be evaluated. This lead to the recursive estimation ˆx− k = E [f (xk−1, uk−1, vk−1)] , (3.6) Kk = PxkykP−1 ˜yk ˜yk , (3.7) ˆy− k = E [h (xk−1, vk−1)] , (3.8) ˆxk = ˆx− k + Kk ¡ yk − ˆy− k ¢ , (3.9) Pxk = P− xk − KkP˜ykKT k , (3.10) where ˆx− k and ˆy− k are optimal predictions xk and yk, respectively, Kk is the optimal gain term, P−x k is the prediction of the covariance of xk and P˜yk is the covariance of ˜yk, with ˜yk = yk − ˆy− k . 3.2 The extended Kalman lter (EKF) The extended Kalman lter (EKF) is an estimation algorithm that uses a predictor corrector mechanism to estimate the current state of a system with its nonlinear model and to correct this estimate using any available sensor measurements. The 21 EKF approximates the optimal nonlinear terms in equations (3.6) to (3.10) as ˆx− k ¼ f (ˆxk−1, uk−1, ¯v) , Kk ¼ ˆP xkyk ˆP −1 ˜yk ˜yk , ˆy− k ¼ h (xk−1, uk−1, vk−1) , where noise random variables with distributions p (vk), p (nk) are approximated by their prior mean ¯v = E [v], ¯n = E [n], respectively. The covariances are computed by linearizing model (3.1) as follows xk+1 ¼ Akxk + Gkuk + Bkvk, yk ¼ Ckxk + Dknk, where Ak = @f (x, uk, ¯v) @x ¯¯¯¯ xk , Gk = @f (xk, u, ¯v) @u ¯¯¯¯ uk , Bk = @f (xk, uk, v) @v ¯¯¯¯ ¯v , Ck = @h (x, ¯n) @x ¯¯¯¯ xk , and Dk = @h (xk, n) @n ¯¯¯¯ ¯n . Algorithm 1 resumes the equations for the EKF. 3.2.1 Simulation example The following model is used through all the examples in the rest of this Chapter. Let the system be given by x˙ 1 = x2, ˙ x2 = x32 + u, (3.11) y = x1. It can be easily proved that the feedback law u = −x32 − x1 − x2, (3.12) stabilizes system (3.11). Figures 3.1 and 3.2 show results of state estimation using the EKF for model (3.11), with additive Gaussian noise on the output p (n) » N (0, 3 × 10−6). The 22 Algorithm 1 Extended Kalman lter 1: Initialization. ˆx0 = E [x0] , Px0 = E h (x0 − ˆx0) (x0 − ˆx0)T i . 2: for k 2 N do 3: Time update: ˆx− k = f (ˆxk−1, uk−1, ¯v) , P− xk = Ak−1Pxk−1ATk −1 + BkRvBTk . 4: Measurement update: Kk = P− xkCTk ¡ CkP− xkCTk + DkRnDTk ¢−1 , ˆxk = ˆx− k + Kk ¡ yk − h ¡ ˆx− k , ¯n ¢¢ , Pxk = (I − KkCk)P− xk , 5: end for where Rv is the process noise covariance, Rn is the measurement noise covariance, and Ak = @f (x, uk, ¯v) @x ¯¯¯¯ ˆxk , Bk = @f ¡ ˆx− k , uk, v ¢ @v ¯¯¯¯¯ ¯v , Ck = @h (x, ¯n) @x ¯¯¯¯ ˆxk , Dk = @h ¡ ˆx− k , n ¢ @n ¯¯¯¯¯ ¯n . 23 0 2 4 6 8 10 0.1 0.08 0.06 0.04 0.02 0 0.02 0.04 0 2 4 6 8 10 0.02 0 0.02 0.04 0.06 0.08 0.1 0.12 0 2 4 6 8 10 0.02 0 0.02 0.04 0.06 0.08 0.1 0.12 0 2 4 6 8 10 0.06 0.05 0.04 0.03 0.02 0.01 0 0.01 real estimated real estimated x1(t) u(t) time [sec] x2(t) y(t) time [sec] time [sec] time [sec] Figure 3.1: State estimation with the EKF. initial state is given by x0 = [x1 (0) , x2 (0)]T = [0.1, 0]T , the lter is initialized with expected initial state ˆx0 = E [x0] = [0, 0]T , and state covariance Px0 = diag (1, 1). Figure 3.1 shows state estimation results for model (3.11) using control law (3.12). Figure 3.2 shows similar results with the control law u = −ˆx32 − ˆx1 − ˆx2. (3.13) Both gures show similar behavior due to the fast convergence of the EKF. 3.3 The unscented Kalman lter (UKF) The unscented Kalman lter (UKF) uses the unscented transform (UT) to capture the mean and covariance estimates with a minimal set of sample points. It provides 3rd order accuracy of the Taylor series expansion of the Gaussian error distribution for any nonlinear system. For nonGaussian inputs, approximations are accurate to at least the 2nd order [65, 66, 140, 141]. 24 0 2 4 6 8 10 0.12 0.1 0.08 0.06 0.04 0.02 0 0.02 0.04 0 2 4 6 8 10 0.02 0 0.02 0.04 0.06 0.08 0.1 0.12 0 2 4 6 8 10 0.02 0 0.02 0.04 0.06 0.08 0.1 0.12 0 2 4 6 8 10 0.06 0.05 0.04 0.03 0.02 0.01 0 0.01 real estimated real estimated x1(t) u(t) time [sec] x2(t) y(t) time [sec] time [sec] time [sec] Figure 3.2: State estimation and control with the EKF. 25 The UT is a deterministic sampling approach that can be summarized as follows [141]: Let x 2 Rn be a ndimensional random variable with mean ¯x and covariance Px. The propagation of the random variable x through a nonlinear function y = g (x) can be approximated by 2n + 1 weighted points Âi 2 Rn, i = 0, . . . , 2n, Â0 = ¯x, Âi = ¯x + ³p (n + ·)Px ´ i , i = 1, . . . , n, Âi = ¯x − ³p (n + ·)Px ´ i , i = n + 1, . . . , 2n, Wm 0 = · n + · , Wc 0 = · n + · + a − ®2 + ¯, Wc i = Wm i = 1 2 (n + ·) , i = 1, . . . , 2n, where · = ®2 (n + ¸)−n is a scaling parameter, ® determines the spread of the sigma points around ¯x and is usually set to a small positive value, ¸ is a scaling parameter of value 0 or 3 − n, ¯ is a parameter used to incorporate any prior knowledge about the distribution of x (¯ = 2 is optimal for Gaussian distributions), ³p (n + ·)Px ´ i is the ith column of the matrix square root, and Wi is a weight associated with the ith point. The sigma points Âi are then propagated through the nonlinear function repre senting the system Yi = g (Âi) , i = 0, . . . , 2n, and the mean and covariance for y are approximated using a weighted sample mean and covariance of the posterior sigma points, ¯y ¼ X2n i=0 Wm i Yi, Py ¼ X2n i=0 (Yi − ¯y) (Yi − ¯y)T . The unscented Kalman lter is a straightforward extension of the UT to recur sive estimation of system (3.1). Algorithm 2 resumes UKF equations for parameter estimation for process and measurement noise purely additive [141]. 26 Algorithm 2 Unscented Kalman lter for state estimation. 1: Initialization. ˆx0 = E [x0] , Px0 = E h (x0 − ˆx0) (x0 − ˆx0)T i . 2: for k 2 N do 3: Compute sigma points: Âk−1 = h ˆxk−1, ˆxk−1 ± ° p Pk−1 i . 4: Time update: Â¤ kk−1 = f (Âk−1, uk−1) , ˆx− k = X2Nx i=0 Wm i Â¤ i,kk−1 , P− k = X2Nx i=0 Wc i ¡ Â¤ i,kk−1 − ˆx− k ¢ ¡ Â¤ i,kk−1 − ˆx− k ¢T + Rv, Âkk−1 = · ˆx− k−1, ˆx− k−1 ± ° q P− k ¸ , Ykk−1 = h ¡ Âkk−1 ¢ , ˆy− k = X2Nx i=0 Wm i Yi,kk−1 . 5: Measurement update: P˜yk ˜yk = X2n i=0 Wc i ¡ Yi,kk−1 − ˆy− k ¢ ¡ Yi,kk−1 − ˆy− k ¢T + Rn, Pxkyk = X2n i=0 Wc i ¡ Xi,kk−1 − ˆx− k ¢ ¡ Yi,kk−1 − ˆy− k ¢T , Kk = PxkykP−1 ˜yk ˜yk , ˆxk = ˆx− k + Kk ¡ yk − ˆy− k ¢ , Pxk = Pxk−1 − KkP˜yk ˜ykKT k , 6: end for where n is the dimension of the original state xk, ° = p n + ¸, Rv is the process noise covariance, and Rn is the measurement noise covariance. 27 0 2 4 6 8 10 0.1 0.08 0.06 0.04 0.02 0 0.02 0.04 0 2 4 6 8 10 0.02 0 0.02 0.04 0.06 0.08 0.1 0.12 0 2 4 6 8 10 0.02 0 0.02 0.04 0.06 0.08 0.1 0.12 0 2 4 6 8 10 0.2 0.15 0.1 0.05 0 0.05 real estimated real estimated x1(t) u(t) time [sec] x2(t) y(t) time [sec] time [sec] time [sec] Figure 3.3: State estimation with the UKF. 3.3.1 Simulation example Figures 3.3 and 3.4 show results of state estimation using the UKF for the model given by equation (3.11), as was presented in Section 3.2.1. Figure 3.3 shows state estimation results for model (3.11) using control law (3.12). As can be seen in gures 3.1 and 3.3, the estimation results are similar except for smoother estimates using the UKF. The smoothing e ect is more clear in the computation of control law (3.13), as can be seen comparing gures 3.2 and 3.4. The smoothing e ect is due to the better approximation that the UKF does of the nonlinear system function. 28 0 2 4 6 8 10 0.15 0.1 0.05 0 0.05 0.1 0 2 4 6 8 10 0.02 0 0.02 0.04 0.06 0.08 0.1 0.12 0 2 4 6 8 10 0.02 0 0.02 0.04 0.06 0.08 0.1 0.12 0 2 4 6 8 10 0.2 0.15 0.1 0.05 0 0.05 x(2) real estimated real estimated x1(t) u(t) time [sec] x2(t) y(t) time [sec] time [sec] time [sec] Figure 3.4: State estimation and control with the UKF. 29 3.4 UKF for parameter estimation An application of the EKF or UKF is learning a nonlinear mapping. This application is used in Chapter 4 to obtain leader's velocities. Let the mapping be given by yk = g (xk,w) , where w 2 Rnw corresponds to a set of unknown parameters. The UKF may be used to estimate the parameters w by writing a new statespace representation wk+1 = wk + vk, yk = g (xk,wk) + nk. (3.14) This model (3.14) solves the optimization problem [141] min w J (w) = min w E ( Xk m=1 [ym − g (xm,w)]T [ym − g (xm,w)] ) . (3.15) Algorithm 3 resumes the equations of the UKF for parameter estimation. In this work, we have chosen the innovation covariance Rv,k in Algorithm 3 as Rv,k = E £ vkvT k ¤ = (¸−1 v − 1)Rw, with ¸v 2 (0, 1], to provide an exponentially decaying weighting term as a forgetting factor to allow adaptation to changes in w. 3.4.1 Simulation example Let the system be given by x˙ 1 = w1x1 + w2x2, ˙ x2 = x32 + u, (3.16) y = 10x1, where w = [1.5, 1.0]T represents the vector of unknown parameters. The control u = −x32 − 5x1 − 3x2 stabilizes system (3.16), with the closed loop matrix given by Acl = " 1.5 1 −5 −3 # , then, the eigenvalues of this system are given by ¸1 (Acl) = −0.5 and ¸2 (Acl) = −1. Figures 3.5 and 3.6 show results of parameter estimation using the UKF for 30 Algorithm 3 Unscented Kalman lter for parameter estimation. 1: Initialization. ˆw0 = E [w] , Pw0 = E h (w − ˆw0) (w − ˆw0)T i . 2: for k 2 N do 3: Time update and sigma points computation: ˆw− k = ˆwk−1, P− wk = Pwk−1 + Rv,k−1, Wkk−1 = h ˆw− k , ˆw− k ± ° q P− wk i , Dkk−1 = g ¡ xk,Wkk−1 ¢ , ˆyk = g ¡ xk, ˆw− k ¢ . 4: Measurement update: P˜yk ˜yk = X2nw i=0 Wc i ¡ Di,kk−1 − ˆyk ¢ ¡ Di,kk−1 − ˆyk ¢T + Rn,k, Pwkyk = X2nw i=0 Wc i ¡ Wi,kk−1 − ˆw− k ¢ ¡ Di,kk−1 − ˆyk ¢T , Kk = PwkykP−1 ˜yk ˜yk , ˆwk = ˆw− k + Kk (yk − ˆyk) , Pwk = P− wk − KkP˜yk ˜ykKT k , 5: end for where nw is the dimension of parameter space, ° = p nw + ¸, Rv is the process noise covariance, and Rn is the measurement noise covariance. 31 0 2 4 6 8 10 0.5 0.4 0.3 0.2 0.1 0 0.1 0.2 0 2 4 6 8 10 0 0.02 0.04 0.06 0.08 0.1 0.12 0.14 0.16 0 2 4 6 8 10 0 0.5 1 1.5 2 0 2 4 6 8 10 0 0.5 1 1.5 2 real estimated real estimated w1(t) u(t) time [sec] w2(t) y(t) time [sec] time [sec] time [sec] Figure 3.5: Parameter estimation with the UKF. model (3.16). The lter is initialized with expected parameter values ˆw0 = E [w0] = [0.1, 0.1]T , and parameter covariance Pw0 = diag (50, 50). Figures 3.5 shows that the parameters converge to true values with a noiseless output signal. Figure 3.6 shows the results of estimating parameters when the output is corrupted by additive Gaussian noise p (n) » N (0, 3 × 10−6). As can be seen, the parameters exhibit polarization, converging to the values w1 (1) = 1.4083 and w2 (1) = 0.9318. This characteristic is typical in system without enough input richness. 3.5 Highgain observers (HGO) In the last years, the combination between highgain observers and globally bounded state feedback control has gained the attention of many researchers [2, 55, 69, 70, 84]. This interest is due to robustness properties exhibited by the observer to uncertainties, unmodeled sensor and actuator dynamics, and to the ability of the observercontroller 32 0 2 4 6 8 10 0.5 0.4 0.3 0.2 0.1 0 0.1 0.2 0 2 4 6 8 10 0.05 0 0.05 0.1 0.15 0.2 0 2 4 6 8 10 0 0.5 1 1.5 2 2.5 3 0 2 4 6 8 10 5 0 5 10 real estimated real estimated w1(t) u(t) time [sec] w2(t) y(t) time [sec] time [sec] time [sec] Figure 3.6: Parameter estimation with the UKF and noisy output. 33 pair to recover the state feedback controller performance when the gain of the observer is high enough. An additional advantage of highgain observers is the possibility of using the separation principle to design controllers for a class of nonlinear systems [2, 62]. The separation principle means that a controller and an observer can be designed in two steps: • Design of a globally bounded state feedback controller based on the state vari able x that stabilizes the system and meets given speci cations. • Development of an output feedback controller by replacing the state x by its estimate ˆx. The combined observercontroller output feedback preserve the main features of the controller with the full state available, in spite of the noncorrectness of highgain observers with any xed nite gain values. One of the main drawbacks of highgain observers is that they extend the band width of the control design to unmodeled fast dynamics. The extended bandwidth does not present a problem for the actuator and sensor dynamics, preventing that their dynamics are su ciently fast relative to the dynamics of the nominal closedloop system [84, 70]. However, it is a problem when the observer estimates derivatives of noisy signals. In the following, we resume an example from [69] to show some properties of the design of output feedback controllers using highgain observers. Let a second order nonlinear system have the form x˙ 1 = x2, x˙ 2 = Á (x, u) , (3.17) y = x1. Let u = ° (x, y) be a locally Lipschitz state feedback control law that stabilizes the origin x = 0. Let the highgain observer be de ned using only measurements of the output y ˙ˆ x1 = ˆx2 + ®1 " (y − ˆx1) , ˙ˆ x2 = Á0 (ˆx, u) + ®1 "2 (y − ˆx1) , (3.18) u = ° (ˆx, y) , 34 where Á0 (x, u) nominal model of the nonlinear function Á (x, u). To satisfy the conditions of the separation principle in [2], the state feedback control and the observer must ful ll the following assumptions [70]: Assumption 3.5.1. ° (0, 0) = 0, ° (x, y) is a twicecontinuously di erentiable function over the domain of interest and a globally bounded function of x, and the origin (x = 0) is an exponentially stable equilibrium point of the closedloop system x˙ i = xi+1, 1 · i · r − 1, x˙ r = Á (x, ° (x, y)) , where r is the relative degree of the system. Assumption 3.5.2. Á0 (0, 0) = 0, Á0 (x, u) is a twicecontinuously di erentiable func tion over the domain of interest and a globally bounded function of x, " is a positive constant, and ®1 to ®r are positive constants chosen such that the roots of sr + ®1sr−1 + . . . + ®r−1s + ®r = 0 have negative real parts. It follows from the separation principle that, for su ciently small ", the output feedback controller stabilizes the origin of the closedloop system and recovers the performance of the state feedback controller in the sense that the trajectories of x under output feedback approach those under state feedback as " tends to zero. Let the estimation error be given by ˜x = " ˜x1 ˜x2 # = " x1 − ˆx1 x2 − ˆx2 # , then the error dynamics are ˙˜ x1 = − h1˜x1 + ˜x2 ˙˜ x2 = − h2˜x1 + ± (x, ˜x) with ± (x, ˜x) = Á (x, ° (ˆx)) − Á0 (ˆx, ° (ˆx)). Let the scaled estimation errors be de ned 35 as ´1 = ˜x1 " , ´2 = ˜x2. Using the scaled estimation errors, we can de ne the singularly perturbed system "´˙1 = −®1´1 + ´2, "´˙2 = −®2´1 + "± (x, x˜) . (3.19) In (3.19), note that small values of " reduce the e ects of the error model ± (·), make ´ much faster than x, and produce peaking phenomenon given by a response of the form 1 " exp µ − ®t " ¶ . This peaking phenomenon can be avoided by saturating the control input u = sat (° (x)) . Finally, the stability analysis is performed over the complete system x˙ 1 = x2, x˙ 2 = Á (x, ° (xˆ)) , "´˙1 = −®1´1 + ´2, "´˙2 = −®2´1 + "± (x, x˜) . Due to the separation principle, the stability analysis can be performed into two parts: • The stability analysis of the slow motion system (x1, x2), with " = 0, using an appropriate Lyapunov function V (x). • The stability analysis of the fast motion system "´˙ = A0´, with A0 = " −®1 1 −®2 0 # , using the Lyapunov function W (´) = ´TP0´, with P0 satisfying P0A0 + AT0 P0 = −I4. 36 3.5.1 Simulation example Let us consider the nonlinear system system (3.11) and the highgain observer ˙ˆ x1 = ˆx2 + 2 " (y − ˆy) , ˙ˆ x2 = 1 "2 (y − ˆx1) , ˆy = ˆx1. Figure 3.7 shows simulation results using control law (3.12) and di erent values of gain ". As can be seen, smaller values of " increase the convergence rate and the overshoot. This e ect is clearly noticed in Figure 3.8. The plot at the top right corner shows that the control input increases signi cantly when " decreases. Despite their short time duration, these large values of the control input may cause closedloop instability. Figure 3.9 shows same results for the case of input saturation u = sat ¡ −ˆx32 − ˆx1 − ˆx2 ¢ , as can be seen, the performance has been improved signi cantly. Figure 3.10 shows results with saturated inputs and noise. Because the HGO increases control bandwidth, the results are notoriously degraded. 3.6 Higherorder sliding mode (HOSM) observers Sliding mode control has been a key approach to solve problems under heavy uncer tainty conditions [70, 121, 135, 136]. To solve these problems, the sliding mode ap proach keeps a properly chosen constraint by means of highfrequency control switch ing. The main features of the sliding mode approach are insensitivity to external and internal disturbances, ultimate accuracy and nitetime transient. Notwithstanding, the sliding has some restrictions: If the sliding mode surface, or constraint, is de ned by an equality s = 0, where s is a system output described by a smooth function, the standard sliding mode may be implemented only if the relative degree of s is 1. Moreover, high frequency control switching leads to the socalled chattering e ect which can be dangerous in real applications. In recent years, a new generation of controllers based on higherorder sliding mode (HOSM) theory has been developed [5, 6, 7, 30, 49, 76, 78]. HOSM generalizes 37 0 2 4 6 8 0.1 0.05 0 0.05 0.1 0 2 4 6 8 0.05 0 0.05 0.1 0.15 0 2 4 6 8 0.05 0 0.05 0.1 0.15 0 0.02 0.04 0.06 0.08 0.1 0 0.05 0.1 0.15 0.2 0 2 4 6 8 2 1 0 1 2 3 4 5 6 0 0.02 0.04 0.06 0.08 0.1 2 1 0 1 2 3 4 5 6 time [sec] u(t) SFB OFB (e = 0.1) OFB (e = 0.05) OFB (e = 0.0085) time [sec] time [sec] time [sec] time [sec] time [sec] y(t) x1(t) Zoom of x1(t) x2(t) Zoom of x2(t) Figure 3.7: HGO with exact control law. 38 0 2 4 6 8 100 50 0 50 0 0.02 0.04 0.06 0.08 0.1 100 50 0 50 0 2 4 6 8 0.8 0.6 0.4 0.2 0 0.2 0 0.02 0.04 0.06 0.08 0.1 0 0.05 0.1 0.15 0.2 0 2 4 6 8 2 1 0 1 2 3 4 5 6 0 0.02 0.04 0.06 0.08 0.1 2 1 0 1 2 3 4 5 6 time [sec] u(t) SFB OFB (e = 0.1) OFB (e = 0.05) OFB (e = 0.0085) time [sec] time [sec] time [sec] time [sec] time [sec] Zoom of u(t) x1(t) Zoom of x1(t) x2(t) Zoom of x2(t) Figure 3.8: HGO behavior with unsaturated inputs. 39 0 2 4 6 8 1 0.5 0 0.5 0 0.02 0.04 0.06 0.08 0.1 1 0.8 0.6 0.4 0.2 0 0 2 4 6 8 0.05 0 0.05 0.1 0.15 0 0.02 0.04 0.06 0.08 0.1 0 0.05 0.1 0.15 0.2 0 2 4 6 8 2 1 0 1 2 3 4 5 6 0 0.02 0.04 0.06 0.08 0.1 2 1 0 1 2 3 4 5 6 time [sec] u(t) SFB OFB (e = 0.1) OFB (e = 0.05) OFB (e = 0.0085) time [sec] time [sec] time [sec] time [sec] time [sec] Zoom of u(t) x1(t) Zoom of x1(t) x2(t) Zoom of x2(t) Figure 3.9: HGO behavior with saturated inputs. 40 0 2 4 6 8 1 0.5 0 0.5 0 2 4 6 8 0.05 0 0.05 0.1 0.15 0 2 4 6 8 0.05 0 0.05 0.1 0.15 0 2 4 6 8 1 0 1 2 3 4 SFB OFB (e = 0.01) time [sec] u(t) y(t) time [sec] time [sec] time [sec] x1(t) x2(t) Figure 3.10: HGO behavior with saturated inputs and noise. 41 the basic sliding mode idea, acting on higher order time derivatives of the system deviation from the sliding surface instead of a ecting the rst deviation derivative, as standard sliding modes do. The rth order sliding mode is determined by the equalities s = s˙ = s¨ = . . . = s(r−1) = 0, where s, s˙, s¨, . . ., s(r−1) are continuous functions. In fact, the sliding mode motions also satisfy the equality s(r) = 0, but as a result of some averaging process. In particular, HOSM theory has been applied to solve the di erentiation problem in [75, 76]. The problem can be stated as follows: Let input signal f (t) be a function de ned on [0,1) consisting of a bounded Lebesguemeasurable noise with unknown features and an unknown base signal f0 (t) with the nth derivative having a known Lipschitz constant L > 0. The problem is to nd realtime robust estimations of f˙0 (t), f¨0 (t), . . ., f(n) 0 (t) which should be exact in the absence of measurement noises. The di erentiator based on HOSM designed in [76] can be resumed as z˙0 = v0, v0 = −¸0 z0 − f (t)n/(n+1) sign (z0 − f (t)) + z1, z˙1 = v1, v1 = −¸1 z1 − v0(n−1)/n sign (z1 − v0) + z2, ... (3.20) z˙n−1 = vn−1, vn−1 = −¸n−1 zn−1 − vn−21/2 sign (zn−1 − vn−2) + zn, z˙n = −¸n sign (zn − vn−1) . The main properties of this di erentiator are summarized by the following two theorems and a lemma. Theorem 3.6.1 ([76]). Given di erentiator (3.20). If the parameters are properly chosen, then the following equalities are true in the absence of input noises after a nite transient process z0 = f0 (t) , zi = f(i) 0 (t) , i = 1, . . . , n. Theorem 3.6.2 ([76]). Let input noise satisfy the inequality f (t) − f0 (t) · ". Then the following inequalities are established in nite time for some positive con 42 stants μi, ºi depending on the parameters of the di erentiator ¯¯ ¯zi − f(i) 0 (t) ¯¯¯ · μi"(n−i+1)/(n+1) , i = 0, . . . , n, ¯¯ ¯vi − f(i+1) 0 (t) ¯¯¯ = ºi"(n−i)/(n+1) , i = 0, . . . , n − 1. Lemma 3.6.3 ([94]). Consider the observer (3.20). If ¯¯ f(n+1) (t) ¯¯ · kn+1, for all t < 1, with kn+1 2 R¸0, Then the observed variables zi, i = 1, . . . , n, cannot diverge in nite time. There are two main di erences between observers (3.18) and (3.20): The rst di erence is the nitetime convergence property of the HOSM observer, in contrast to the asymptotic convergence property of the HGO observer. This property implies that the separation principle is no longer necessary. The second di erence is the boundedness of the control law. The HOSM observer is bounded by design. However, the input of the HGO observer might be saturated to avoid peaking phenomenon. It should be noticed that the only requirements for the implementation of the HOSM di erentiator is the boundedness of some higherorder derivative of its input. 3.6.1 Simulation example Let us consider the nonlinear system system (3.11) and the HOSM (3.20). Figures 3.11 to 3.14 show simulation results with the HOSM. Figure 3.11 shows simulation results using control law (3.12), and the HOSM observer ˙ˆ x1 = v1, v1 = ˆx2 − ¸0 ˆx1 − y1/2 sign (ˆx1 − y) , ˙ˆ x2 = −¸1 sign (ˆx2 − v1) , (3.21) ˆy = ˆx1 with ¸0 = 2.8 and ¸1 = 0.75. There exist a tradeo between convergence and sensitivity to input noise: the larger the parameters, the faster the convergence and the higher the sensitivity to input noise and sampling step. As can be seen in Figure 3.11, the estimated derivative of the output ˆx2 presents a notorious chattering e ect. This e ect is smoothened in its integral value ˆx1. Conse quently, an option to deal with the chattering problem is to computed a higher order 43 0 2 4 6 8 10 0.1 0.08 0.06 0.04 0.02 0 0.02 0.04 0 2 4 6 8 10 0.02 0 0.02 0.04 0.06 0.08 0.1 0 2 4 6 8 10 0.02 0 0.02 0.04 0.06 0.08 0.1 0.12 0 2 4 6 8 10 0.1 0.05 0 0.05 0.1 0.15 0.2 0.25 real estimated real estimated x1(t) u(t) time [sec] x2(t) y(t) time [sec] time [sec] time [sec] Figure 3.11: Simulation results with a rst order HOSM. 44 derivative using observer (3.20), that is ˙ˆ x1 = v1, v1 = −¸0 ˆx1 − y2/3 sign (ˆx1 − y) + ˆx1, ˙ˆ x2 = v2, v2 = −¸1 ˆx2 − v11/2 sign (ˆx2 − v1) + ˆx2, (3.22) ˙ˆ x3 = −¸3 sign (ˆx3 − v2) . Figure 3.12 shows the results of this di erentiator with ¸0 = 7.5, ¸2 = 5.0, and ¸3 = 1.5. Clearly, this version outperforms the previous one. Figure 3.13 shows results of HOSM (3.22) with measurement noise on the out put. As can be seen, the results are similar to the EKF but without using a priori information about the system. Figure 3.14 shows results with the control law (3.13), the results are quite satis factory. 45 0 2 4 6 8 10 0.8 0.6 0.4 0.2 0 0.2 0.4 0.6 0 2 4 6 8 10 0.1 0.08 0.06 0.04 0.02 0 0.02 0.04 0 2 4 6 8 10 0.02 0 0.02 0.04 0.06 0.08 0.1 0.12 0 2 4 6 8 10 0.02 0 0.02 0.04 0.06 0.08 0.1 0.12 0 2 4 6 8 10 0.1 0 0.1 0.2 0.3 0.4 0.5 0.6 real estimated real estimated x1(t) u(t) time [sec] y(t) time [sec] time [sec] real estimated x3(t) time [sec] x2(t) Figure 3.12: Simulation results with a second order HOSM. 46 0 2 4 6 8 10 0.5 0 0.5 0 2 4 6 8 10 0.1 0.08 0.06 0.04 0.02 0 0.02 0.04 0 2 4 6 8 10 0.02 0 0.02 0.04 0.06 0.08 0.1 0.12 0 2 4 6 8 10 0.02 0 0.02 0.04 0.06 0.08 0.1 0.12 0 2 4 6 8 10 0.2 0.1 0 0.1 0.2 0.3 0.4 0.5 real estimated real estimated x1(t) u(t) time [sec] y(t) time [sec] time [sec] time [sec] real estimated x3(t) time [sec] x2(t) Figure 3.13: Simulation results with a second order HOSM and measurement noise. 47 0 2 4 6 8 10 0.7 0.6 0.5 0.4 0.3 0.2 0.1 0 0.1 0 2 4 6 8 10 0.04 0.02 0 0.02 0.04 0.06 0.08 0.1 0.12 0 2 4 6 8 10 0.04 0.02 0 0.02 0.04 0.06 0.08 0.1 0.12 0 2 4 6 8 10 0.2 0.1 0 0.1 0.2 0.3 0.4 0.5 real estimated real estimated x1(t) u(t) time [sec] x2(t) y(t) time [sec] time [sec] time [sec] 0 2 4 6 8 10 0.8 0.6 0.4 0.2 0 0.2 0.4 0.6 real estimated x3(t) time [sec] Figure 3.14: Closedloop simulation results with a second order HOSM. 48 Part II Main Contributions 49 Chapter 4 Mobile robot vision Localization in mobile robot formations has drawn the attention of many researchers during the last few years. Traditionally, the control system design relies on measure ments from dead reckoning sensors. However, these measurements are completely unreliable after few meters of navigation due to encoder's low accuracy and drift. Due to reduced cost and exibility, the current trend is the design of systems using a single or a pair of cameras to determine the relative position of a robot with respect to another robot or object [15, 24, 39, 42, 53, 74, 83, 87]. For example, cameras can recognize and distinguish between objects of similar shapes, types, and colors, a task impossible to perform with range or acoustic sensors. Nonetheless, visionbased control poses new particular challenges. For instance, the controller has to be robust to camera calibration errors, nonlinearities arising from camera models, and intrinsic ambiguities of vision sensors. Localization in mobile robot formations using machine vision is part of what is known as the visual target tracking problem. This tracking can be realized with a xed camera or with a mobile camera. The visual target tracking problem can be divided into two main components: target detection, or target segmentation, and pose (distance and orientation) estimation [53]. There are several approaches for target detection in the literature. For example, in [83], the author presents a method that consists in extracting distinctive features invariant to image scale and rotation. The recognition process is divided in three steps: The rst step is a matching process between individual features and a feature database of known objects using a fast nearestneighbor algorithm. The second step identi es clusters belonging to a single object through the computation of the Hough transform. Finally, the third step veri es consistency of pose parameters using a leastsquares solution. In [117], the method presented in the previous paper is used 50 for global localization using landmarks. An alternative approach that uses feature points is the wellknown and very robust SIFT algorithm [83, 87]. This method is invariant to scale, robust to noise and occlusions, but very computationally intensive. It takes around 3s to compute features with a Pentium IV, making it inadequate for online applications. To cope with this problem, robust target detection is often achieved through the use of arti cial ducial markers. Methods using markers are referred as modelbased pose estimation methods, as classi ed in [57]. In practice, threedimensional positions of features points on the object are measured a priori and stored in a database. Then, the online detection problem consists in detecting and relating image points with the a priori selected features in order to compute the pose of the object. The problem of pose estimation using ducial markers has been mainly addressed by tangible augmented reality system applications [24, 29, 67]. The goal of these methods is to combine virtual 3D representations with the real world for active in teraction. In these systems, markers are distributed around a synthetic environment to help localize and relate the virtual camera position with the real position in the world. In [42], the author presents a visionbased system for controlling multiple robot platforms in real time using planar markers and a topview video camera for online pose estimation. In [16, 17, 38], the authors develop a monocularcamera based visual servo tracking controller for a mobile robot subject to nonholonomic motion constraints. The algorithm de nes a desired trajectory for the vehicle with a sequence of prerecorded images of three target points. In [53], the authors present a visual tracking scheme that detects the target contour using a shape adaptive sum ofsquared di erence algorithm. The target velocity is decomposed into a component caused by the target motion and a component caused by the camera motion. The lat ter component is computed using the image Jacobian, allowing an accurate estimation of the target position in the following image. There exist few applications of machine vision to the formation control problem. In [22, 114], follower robots estimate position and orientation of their leaders using a colortracking algorithm using o theshelf cameras. Each robot is equipped with a color pattern consisting of a central purple rectangle and two lateral green rectangles. The central rectangle provides an estimate of the distance based on measured and real heights. The di erence between the perceived heights of the lateral rectangles provides an estimate of the orientation of the pattern with respect to the observing robot. This algorithm is not robust to noise or pixel vibrations. In [27, 28], an onboard catadioptric camera system is used. The omnidirectional images obtained are used 51 to input data to state observers. In [26, 138, 139], the authors use segmentation from multiple central views to keep track of which pixels correspond to each leader from pictures taken with omnidirectional cameras. Pose estimation of each leader in the image plane of the follower is performed by rank constraint on the central panoramic optical ow across multiple frames. It is wellknow the massive computational power requirements of this technique. In [88, 89], the authors present an algorithm for ocking based on the measurement of timetocollision and optical ow each robot does. In these works there is no stability analysis including the e ects of measurement errors and robust methods for image interpretation. This Chapter presents a visionbased framework for mobile robot detection and tracking using o theshelf cameras mounted on mobile robots. Target detection and pose estimation are performed from single frames using ducial markers as key elements. The method consists in distributing an octagon shaped structure on the back part of each robot. These shapes are easy to extract from the images and posses unique ID codes to facilitate pose estimation. Three pose estimation methods are programmed and compared: the PRA algorithm [98], Lowe's algorithm [82, 15], and a modi ed POSIT algorithm [31, 95]. Finally, a dual Unscented Kalman lter (DUKF) is implemented to smooth measured data and estimate unknown leader's velocities. The rest of the Chapter is organized as follows: Section 4.1 summarizes the visual system and the pose estimation problem. Section 4.2 presents the ID detection ap proach. Section 4.3 describes our pose estimation algorithm. Section 4.4 reviews the leaderfollower model and the dual unscented Kalman lter. Section 4.5 shows sim ulation results for the di erent steps involved in this framework. Finally, we present our concluding remarks and future work in Section 4.6. 4.1 System overview The visual tracking problem is divided in target detection and pose estimation. Target detection is related to image processing, whereas pose estimation is related to vision and nonlinear ltering. The detection process is the most time consuming, but it can be simpli ed by using ducial markers. In our vision system, MRVision for mobile robot vision1, markers are distributed on the back part of each robot on a truncated octagon shaped structure, as can be seen in Figure 4.1. Each face of this shape has a code that not only identi es 1http://orqueda.net/research.aspx 52 the face, but also its position on the robot, as it is explained in the next Section. The ink patter is black and white to reduce lighting and camera sensitivity requirements. Figure 4.1: Scheme of the TXT platform in MPSLab. The vision library consists of synthetic and real cameras and depends on the vision library OpenCV [11] and on MPSLab, a motion planning, simulation, and virtual perception library for simulating systems in 3D environments [99, 100]. Synthetic cameras are used for tuning and testing purposes, see Figure 4.2, and real cameras are used with a PC104 for the onboard image capture and processing system. Currently, Firewire IEEE1394 and USB 2.0 cameras are supported, in particular, Unibrain's FireI, Point Grey's Bumblebee, and Logitech 3000 cameras, see Figure 4.3. Figure 4.2: Synthetic camera screen shot. Vision processing can be divided in the tasks resumed in the ow chart of Fig ure 4.4: 53 Figure 4.3: Logitech 3000 USB camera (bottomleft), PointGrey's Bumblebee (top), and Unibrain's FireI (bottomright) FireWire IEEE1394 cameras. 1. Video capture and transformation: Images from several sources with di erent formats (YUV, RGB, etc.) are transformed into OpenCV's IplImage RGB format. 2. Filtering and thresholding: The IplImage image is converted to grayscale. Then, a thresholding based on a modi ed Otsu method is performed on the grayscale image. 3. Point selection/face labeling: A search and identi cation of each face marker in the binary image is performed. First, all the contours on the binary image are extracted. Second, angular changes in each contour close to 90± are computed to determine the square enclosing each marker. After estimating the four corners of each square, a search for markers is performed. If a valid marker is found, the corresponding four corners are stored to be used for pose estimation. 4. Pose estimation: Using the algorithms presented in Section 4.3, the position of the leader robot with respect to the camera position (xc, yc, zc) and the relative rotation of the leader robot with respect to the camera plane µm are computed, see gures 2.2 and 4.5 for the de nition of the variables. 5. Transformation: The desired values are computed using (xc, yc, zc), µm, and 54 the value of the camera pan µp, as `ij = p x2c + y2 c + z2 c , (4.1) µt = arctan µ −xc zc ¶ , (4.2) ®12 = µt − µm, (4.3) µ12 = µp + µm. (4.4) Labels/points Video capture Filtering/ thresholding Color image Point selection/ face labeling Binary image Pose estimation ( c c c ) m X ,Y ,Z ,q ij ij ij ,a ,b Transformation Figure 4.4: Vision processing ow chart. 55 d Zc XI YI ZI Xc Yc j q i q Zi Xi Yi ij Yj Zj Xj Figure 4.5: Camera and robot frames. 56 4.2 Marker and ID detection The steps involved in marker and ID detection processes are: 1. Image capturing. 2. Grayscale conversion. 3. Thresholding. 4. Contour extraction and selection. 5. ID recognition. Figure 4.6 shows steps 1 to 5 applied to a synthetic image. In the following, we will resume the main characteristics of these steps. 4.2.1 Image capture Image capture is performed using standard drivers. MRVision is programmed in C++ and works on Linux and Windows platforms. It can capture images from synthetic cameras, Firewire IEEE1394 and USB 2.0 compatible cameras. After capture, the image is converted to OpenCV's IplImage format [11]. 4.2.2 Grayscale conversion Grayscale conversion is necessary to reduce the amount of data. This conversion is performed by simply extracting the green channel of the original color image. The use of just one channel is justi ed by the lowlevel of information loss. In Figure 4.7, it can be seen a comparison of di erent methods of grayscale conversion. Figure 4.7a shows the color image, Figure 4.7b, c, and d, show the red, green, blue channel images respectively. As can be seen, the blue channel image loss the most information. Figure 4.7e if the mean value image. This image does not justify the computational burden. 4.2.3 Thresholding: The Otsu method Thresholding, or bilevel thresholding, segments an image into two brightness regions: background and object. That is, for a gray level image I (x, y), with m gray levels 57 Figure 4.6: Processing sequence of the octagon shape. 58 (b) (c) (a) (d) (e) Figure 4.7: Grayscale conversion. (a) Color image, (b) red channel, (c) green channel, (d) blue channel, and (e) mean value image. 59 0, 1, . . . ,m−1, bilevel thresholding is to transform I (x, y) into a binary image Ib (x, y) by a threshold j, 0 · t · m − 1, such that Ib (x, y) = ( 0, if I (x, y) · t, 1, if I (x, y) > t. Traditionally, a xed thresholding value is used. However, in our application, lighting variations, di erent camera properties and environmental conditions make the selection of this value very di cult. For this reason, we use a modi ed version of the method due to Otsu [21, 56, 107]. This method formulates the threshold selection problem as a discriminant analysis. The method divides the gray level image histogram in two groups, A and B, then selects the threshold as the point of maximum variance between both groups. Let ni be the number of pixels with a gray level value i and n = Pm−1 i=0 ni be the total number of pixels in the image I (x, y). Let Pi = ni n be the probability of occurrence of greylevel i. Let !1 (j), M1 (j), !2 (j), M2 (j) be the number of pixels and the average gray level value in group A and group B, respectively. Then !1 (j) = Xj i=0 ni, M1 (j) = Pj i=0 i · ni !1 (j) , !2 (j) = mX−1 i=j+1 ni, M2 (j) = Pm−1 i=j+1 i · ni !2 (j) , Expressing the average gray level value MT of all pixels in image f (x, y) as MT = !1 (j)M1 (j) + !2 (j)M2 (j) !1 (j) + !2 (j) , the variance between the two groups, denoted as ¾2B (j), is ¾2B (j) = !1 (j) [M1 (j) −MT ]2 + !2 (j) [M2 (j) −MT ]2 = !1 (j) !2 (j) [M1 (j) −M2 (j)]2 !1 (j) + !2 (j) . The optimal threshold t = t¤ is the value j for which ¾2B (j) is maximum. The modi cation of Otsu's method presented in this work consists in dividing the image in 9 disjoint and 4 overlapped regions. A threshold value is computed for each region using the Otsu algorithm. Then, the values used are the average threshold 60 values of disjoint and overlapped regions. 4.2.4 Contour extraction and selection Contour extraction from the binary image is performed using the OpenCV imple mentation of Suzuki and Abe's method [123]. This method generates a collection of external contours. Then, just the contours with four strong corners are selected. The corners are chosen by exploring changes in the derivative of a given contour. When this change in the derivative is big enough, the point is classi ed as strong corner point. 4.2.5 ID recognition ID recognition is based on an unique code printed on each face of the octagon shape on the back part of the robot. After having the squares computed with the previous contour extraction and selection method, the next step is to determine which contours are related to valid robot codes. To perform this task, rst a training step creates a database with square markers of m × n cells, and assigns an ID number to each marker. This ID number is computed by a binary string whose position number is 1 if the square is lled and 0 otherwise, as shown in Figure 4.8 for m = 4 and n = 5. 1× 20 + 0 × 21 + 0× 22 +1× 23 + 0 × 24 +1× 25 + 0 × 26 +1× 27 + 0 × 28 +1× 29 +1× 210 +1× 211 +1× 212 + 0 × 213 + 0 × 214 + 0 × 215 + 0 × 216 +1× 217 + 0× 218 +1× 219 = 663209 Figure 4.8: Fiducial marker ID computation. Second, the identi cation step consists in recovering each face's ID from the per spective transformed image. To explain the method developed in this work, we intro duce some basic concepts on Projective Geometry in Appendix A. To compute the ID we use the cross ratio, an invariant to a projective transfor mation [39, 47]. The cross ratio is de ned as 61 Cr (p1, p2; p3, p4) = d13d24 d14d23 , where dij is the Euclidean distance between two points pi = [xi 1, xi 2, xi 3]T and pj = £ xj 1, xj 2, xj 3 ¤T , given by dij = vuut Ã xi 1 xi 3 − xj 1 xj 3 !2 + Ã xi 2 xi 3 − xj 2 xj 3 !2 . Therefore, if qi, i = 1, . . . , 4, denote the projective transformed points pi, i = 1, . . . , 4, then Cr (p1, p2; p3, p4) = Cr (q1, q2; q3, q4) . Using the previous contour analysis, we obtain points qNW, qSW, qNE, and qSE, see Figure 4.9. The point qc is given by the intersection of the segments sNW,SE = (qNW, qSE) and sSE,NE = (qSW, qNE), then qc = (qNW × qSE) × (qSW × qNE) . (4.5) Points v1 and v2 are called vanishing points, they could be at in nity and are given by v1 = (qSE × qSW) × (qNE × qNW) , v2 = (qNE × qSE) × (qNW × qSW) . Transformed points qW, qE, qN, and qS are given by qW =(qNW × qSW) × (v1 × qc) , (4.6) qE =(qNE × qSE) × (v1 × qc) , (4.7) qN =(qNW × qNE) × (v2 × qc) , (4.8) qS =(qSW × qSE) × (v2 × qc) . (4.9) Due to the invariability of the cross ratio, a point p = (®, ¯) belonging to the ducial marker on the original space can be univocally mapped into the point q = (®S, ¯S) on the collineated space, see Figure 4.9. Let the following invariants be 62 qW qE qNW qSW qNE qSE qN qS bW aN bE aS pW pE pNW pSW pNE pSE pN p a S b pc qc p q N qa N qb E qb S qa Figure 4.9: Projective transformation. de ned as ½® = 1 Cr (pNW, ®; pN, pNE) = 1 Cr (qNW, ®; qN, qNE) = 2 ® 1 + ® , (4.10) ½¯ = 1 Cr (pNW, ¯; pW, pSW) = 1 Cr (qNW, ¯; qW, qSW) = 2 ¯ 1 + ¯ , (4.11) where ® and ¯ are de ned with the generation of the ID. Then, the displacement ®N, ®S, ¯W, and ¯E from qN, qS, qW, and qE, respectively, can be computed as ®N = ( dNW,N ½® dNW,NE−dN,NE½® , if ® ¸ 0, dN,NE½® dNW,NE−dNW,N ½® , if ® < 0, (4.12) ®S = ( dSW,S½® dSW,SE−dS,SE½® , if ® ¸ 0, dS,SE½® dSW,SE−dSW,S½® , if ® < 0, (4.13) ¯W = ( dNW,W ½¯ dNW,SW−dW,SW½¯ , if ¯ ¸ 0, dW,SW½¯ dNW,SW−dNW,W ½¯ , if ¯ < 0, (4.14) ¯E = ( dNE,E½¯ dNE,SE−dE,SE½¯ , if ¯ ¸ 0, dE,SE½¯ dNE,SE−dNE,E½¯ , if ¯ < 0. (4.15) Finally, any position on the transformed space q is found as intersection of the seg 63 ments s® = (q®N , q®S ) and s¯ = (q¯w, q¯E) as q = (q®N × q®S ) × (q¯W × q¯E) . (4.16) In summary, the identi cation step transforms the center point of each square region using (4.16) to the collineated space and detect if the square is lled. Then, the ducial marker is obtained using the online recovered ID and the database previously created. To increase the robustness of the method, several modi cations have been per formed: • The vertical size of the square is bigger than the horizontal to take into account lower vertical camera resolutions. • In the classi cation of a region as lled, average or Gaussian lters are applied to avoid false readings due to noise. • The IDs in the database have Hamming distances2 greater or equal than dmin (dmin = 6 in our application). Then, the marker is classi ed with a given ID in the database when its Hamming distance is less than dmin /2. 4.3 Pose estimation Pose estimation refers to the issue of obtaining relative position and orientation be tween two or more mobile robots using a camera. This Section reviews the camera models and resumes the key points of the pose estimation methods used in this work. 4.3.1 Camera model The cameras used in this work are modeled with the wellknow pinhole camera model. This model can be viewed as a box with a hole, or aperture, on one of its sides. This aperture allows light to enter into the box and re ect on the opposite side of the box, where light intensity can be measured, Figure 4.10 shows an schematic representation [47]. Let pi = [xi, yi, zi, 1]T and cpi = [ cxi, cyi, czi, 1]T denote the homogeneous coordi nate vectors of a point pi in the world coordinate and the camera systems, respectively. 2The Hamming distance is de ned as the number of bits which di er between two binary strings. 64 Let the coordinate transformation T between points pi and cpi be given by a rotation R and a translation t, then cpi = T pi. (4.17) where T = [R, t]. The parameters of matrix R and vector t are known as extrinsic parameters. Using the pinhole camera model, the homogeneous image coordinate vector mi = [ui, vi, 1]T of point pi is given by ui = ® cxi czi − ® cot µs cyi czi + u0, vi = − ¯ sin µs cyi czi + v0, (4.18) where µs is the skew angle, ® = kf, ¯ = `f are magni cation parameters, f is the focal length, and k, ` are scale parameters. Note that k = ` = 1, and µs = 90± in an ideal camera. These parameters are known as intrinsic parameters. From (4.18) and (4.17), the projection of point pi on the image is given by mi = 1 czi Mpi, (4.19) where M= K h R t i and K denotes the intrinsic matrix transformation given by K = 2 64 ® ® cot µs u0 0 − ¯ sin µs v0 0 0 1 3 75 . (4.20) f Zc X Y c c V U pi = (xi, yi, zi) mi = (ui, vi) C Figure 4.10: Pinhole camera model. 65 The extrinsic matrix transformation can be represented in various forms. The most common ones are Euler angles, quaternions, and exponential maps [91]. In this work, we have chosen the exponential map representation; because it only requires three parameters to describe a rotation R and other three parameters to describe a translation t. Let ! = [!x, !y, !z]T be the rotation axis unity vector, and µ the rotation angle. Then the rotation matrix can be represented by R() = exp (µ) , (4.21) where is the skewsymmetric matrix = 2 64 0 −!z !y !z 0 −!x −!y !z 0 3 75 . (4.22) Equation (4.21) can be evaluated using Rodrigues' formula [91] R() = exp (µ) = I + sin µ µ + 2 (1 − cos µ) µ2 . (4.23) Therefore, because Euler's theorem says that any orientation R 2 SO (3) is equiv alent to a rotation about a xed axis ! 2 R3 through an angle µ 2 [0, 2¼), a given a rotation matrix can be evaluated by an exponential map. Let R be such rotation matrix R = 2 64 r11 r12 r13 r21 r22 r23 r31 r32 r33 3 75 , then µ = cos−1 µ trace (R) − 1 2 ¶ . If R 6= I, then ! = 1 2 sin µ 2 64 r32 − r23 r13 − r31 r21 − r12 3 75 . If R = I, then µ = 0 and ! can be chosen arbitrarily. 66 4.3.2 OpenGL camera model The main advantage of using a synthetic camera is the possibility of having its in ternal parameters perfectly known. OpenGL performs several transformations before drawing a point on the screen. This Section describes these transformations and obtains a synthetic intrinsic matrix transformation KOGL. Let cpi = [ cxi, cyi, czi, 1]T denote the homogeneous coordinate vector of a point pi in the camera system. Let the projection matrix P be de ned as P = 2 66664 2dn dr−d` 0 dr+d` dr−d` 0 0 2dn dt−db dt+db dt−db 0 0 0 −df+dn df−dn − 2df dn df−dn 0 0 −1 0 3 77775 , where dn, df , d`, dr, dt, and db are the parameters of the OpenGL camera frustum shown in Figure 4.11 [119]. In this work, we use dr = −d` = w 2 , dt = −db = h 2 , where w and h are the image width and height, respectively. ( ) n near d ( ) f far d ( ) t top d ( ) r right d ( ) b bottom d ( ) left d Figure 4.11: OpenGL camera frustum [119]. Applying the projection matrix to the point in eye coordinates, we obtain the point expressed in clip coordinates 2 66664 exi eyi ezi ewi 3 77775 = P 2 66664 cxi cyi czi 1.0 3 77775 , (4.24) 67 Points in clip coordinates are transformed to normalized device coordinates by 2 64 dxi dyi dzi 3 75 = 2 64 1 / ewi 0 0 0 1 / ewi 0 0 0 1 / ewi 3 75 2 64 exi eyi ezi 3 75 . (4.25) Finally, points in normalized device coordinates are transformed to windows or image coordinates using 2 64 ui vi zi 3 75 = 1 2 2 64 w 0 0 0 h 0 0 0 1 3 75 2 64 dxi dyi dzi 3 75 + 1 2 2 64 w h 1 3 75 . (4.26) Using (4.24), (4.25), (4.26) the OpenGL intrinsic matrix transformation KOGL is given by KOGL = 2 664 dnw dr−d` 0 d`w dr−d` 0 0 dnh dt−db dbh dt−db 0 0 0 − df df−dn − df dn df−dn 3 775 . Therefore, by simple comparison with (4.20), we can obtain the intrinsic parameters µ = ¼ 2 , ® = dnw dr − d` , ¯ = − dnh dt − db , u0 = d`w dr − d` , v0 = dbh dt − db . 4.3.3 Pose estimation algorithms This section summarizes and compares three modelbased pose estimation algorithms from single images: 1. Pose from orthography and scaling with iteration (POSIT) algorithm. 2. Projection ray attraction (PRA) method. 3. Lowe's method. 4.3.3.1 Pose from orthography and scaling with iteration (POSIT) method POSIT algorithm assumes that at least four feature points on the object can be de tected and matched in the image. Moreover, as every modelbased pose estimation 68 algorithm, assumes that the geometry of the object is known [31, 32, 95]. The method is an iterated version of POS (pose from orthography and scaling), which approxi mates a perspective projection with a scaled orthographic projection and then nds the transformation matrix of the object by solving a set of linear equations. The algorithm computes point projections on a plane passing through the origin of the object, as shown in Figure 4.12. Let cpi = [ cxi, cyi, czi]T , i = 1, . . . ,N, be points in the camera coordinate frame. Let R denote the rotation matrix, with R = 2 64 r1 r2 r3 3 75 = 2 64 r11 r12 r13 r21 r22 r23 r31 r32 r33 3 75 , (4.27) and t the translation vector t = [tx, ty, tz]T . (4.28) f Zc V U Xc Yc C i p 0 m i m Xo Yo Zo ' i p Figure 4.12: POSIT projections. 69 Let pi, i = 1, . . . ,N, be the points cpi in the object coordinate frame. Then cxi = r1pi + tx, (4.29) cyi = r2pi + ty, (4.30) czi = r3pi + tz. (4.31) The perspective projection of cpi with a simpli ed internal model of the camera, µs = ¼ 2 , ® = ¯ = f in (4.20), is given by ui = f r1pi + tx r3pi + tz , vi = −f r2pi + ty r3pi + tz . These projections can be written ui = i0pi + u0 ²i + 1 , (4.32) vi = j0pi + v0 ²i + 1 , (4.33) with u0 = f tx tz , v0 = −f ty tz , (4.34) i0 = f tz r1, j0 = − f tz r2, (4.35) ²i = 1 tz r3pi, i = 1, . . . ,N. (4.36) Let u 0 i = ui (²i + 1) and v 0 i = vi (²i + 1) be the coordinates of the orthographic pro jection, then u 0 i = f tz cxi, v 0 i = − f tz cyi. From (4.32), (4.33), we have i0pi = ui (²i + 1) − u0, (4.37) j0pi = vi (²i + 1) − v0. (4.38) 70 When the values ²i are known, equations (4.37) and (4.38) form a linear system of equations in which the unknown are the coordinates i0 and j0. If the values given to ²i are inexact, the solution of POS algorithm is just an approximation. However, after computing the unknowns r1 and r2, better approximation of ²i can be computed using (4.36). For each measured image point mi and corresponding object point pi, i = 1, . . . ,N, a set of equations (4.37)(4.38) can be de ned. Let these N equations be written in matrix form, then A " iT0 jT 0 # = V², (4.39) where A = 2 664 pT1 pT1 ... ... pT N pT N 3 775 , and V² = 2 664 u1 (²1 + 1) − u0 v1 (²1 + 1) − v0 ... ... uN (²N + 1) − u0 vN (²N + 1) − v0 3 775 . Therefore, the solution for i0 and j0 is " iT0 jT 0 # = ¡ ATA ¢−1 ATV². (4.40) Algorithm 4 resumes the implemented POSIT algorithm. Algorithm 4 POSIT. 1: Initialize ²i to some positive value. 2: while k²i − ²i−1k > tol do 3: Compute i0 and j0 using (4.40). 4: i0 0 = [i0 (0) , i0 (1) , i0 (2)], j00 = [j0 (0) , j0 (1) , j0 (2)]. 5: tz = f .p ki0 0k kj00 k. 6: r1 = i0 0 ki0 0k, r2 = j00 kj00 k, r3 = r1 × r2. 7: tx = tz f i0 (3), ty = −tz f j0 (3). 8: for i = 1 to N do 9: ²i = 1 tz r3pi. 10: end for 11: end while 71 4.3.3.2 Projection ray attraction (PRA) method Projection ray attraction (PRA) is a method presented in [98] to recover motion in formation from twodimensional projections of a given threedimensional point set. It is divided in two linear estimation parts: depth approximation and pose computa tion. In the depth approximation part, feature points in the threedimensional space are estimated. In the pose computation part, rotation and translation parameters of the object are estimated using singular value decomposition (SVD). Both part are iteratively executed until the result is stable. The main idea is to state pose estimation as a nonlinear optimization problem using a simpli ed camera model: Given a set of image points mi, i = 1, . . . ,N, and threedimensional feature points pi, referred to the object coordinate frame, or cpi referred to the camera coordinate frame, the pose estimation problem can be solved by minimizing the functional min R,t,di F (R, t, di) = XN i=1 kdivi − (Rpi + t)k2 , (4.41) where vi, i = 1, . . . ,N, are normalized threedimensional point representations of the image point mi, vi = Mi kMik , with Mi = 2 64 ui vi f 3 75 = f 2 64 cxi / czi −cyi / czi 1 3 75 , and di is the depth of the object, with cpi = [ cxi, cyi, czi]T = divi. Therefore, the depth approximation part of the algorithm in [98] consists in ap proximate the depth by di = vT i cpi. (4.42) The minimization problem (4.41) is solved by SVD. Let H be de ned as H = 1 N XN i=1 °°° (Mi − μM) ( cpi − μp)T °°° , 72 where μp = 1 N XN i=1 cpi, ¾2 p = 1 N XN i=1 k cpi − μpk2 , μM = 1 N XN i=1 Mi, and ¾2M = 1 N XN i=1 kMi − μMk2 . Let the SVD of H be given by H = UWV T . Then, R = USV T , t = μM − cRμp, where c = 1 ¾2 p trace (WS) . and S is given by S = ( I, if det (H) ¸ 0, diag (1, 1,−1) , if det (H) < 0, when rank (H) = 3, or S = ( I, if det (U) det (V ) = 1, diag (1, 1,−1) , if det (U) det (V ) = −1, when rank (H) 6= 3. See [98] for more details. 4.3.3.3 Lowe's method This method uses a Newtontype optimization algorithm to estimate the 6 parameter of the external matrix: three for the translation vector, and three for the rotation matrix parameterized by an exponential map. The method has the risk of converging to a false local minimum when the initial values of the unknown parameters are far from the real ones. To avoid this problem, the optimization problem is augmented by a stabilization method that incorporates a prior model of parameter uncertainty range and standard deviation estimation of each image [15, 82]. 73 As in the previous approaches, we assume that the transformation between two frames of a point cpi in the camera coordinate frame is a rotation R and a translation t. The estimated image projections of these points are given by " ui vi # = 1 czi Kcpi = 1 r3pi + tz " k11 k12 0 k22 # " r1pi + tx r2pi + ty # , (4.43) where pi and cpi are related by equations (4.17), (4.29) to (4.31). Let the optimization problem be de ned as # = arg min # XN i=1 °°° mi − ˆmi ³ ˆ# ´°°° 2 , (4.44) where # is the parameter vector # = [!x, !y, !z, tx, ty, tz]T , mi, i = 1, . . . ,N, are measured image points, and ˆmi, i = 1, . . . ,N, are estimated image points ˆmi = f ˆr3 cqi + ˆtz " ˆr1 cqi + ˆtx −ˆr2 cqi − ˆty # . Let the error eN (#) be de ned as eN (#) = 2 66666664 u1 − ˆu1 v1 − ˆv1 ... uN − ˆuN vN − ˆvN 3 77777775 2 R2N. (4.45) In general, Newtontype methods compute a vector of corrections ¢# based on the local linearity of (4.44) and error (4.45). Then #¿+1 = #¿ + ¢#, (4.46) where ¿ denotes the iteration index. In particular, in the LevenbergMarquardt algo rithm, the correction ¢# is computed as ¢# = − ¡ JT J + ¸I ¢−1 JT eN, (4.47) 74 where ¸ > 0 is an adaptive parameter, J 2 R2N×6 is the Jacobian matrix, given by J = 2 664 J1 ... JN 3 775 , (4.48) with Ji = " @ui @!x @ui @!y @ui @!z @ui @tx @ui @ty @ui @tz @vi @!x @vi @!y @vi @!z @vi @tx @vi @ty @vi @tz # , i = 1, . . . ,N. To compute the derivatives of the estimated projections with respect to tx, ty, tz, !x, !y, and !z, we need to obtain the derivatives of the rotation matrix R with respect to these parameters. Using (4.23) and (4.22), we have R() = I + c1 (µ) + c2 (µ)2, with c1 (µ) = sin µ µ , c2 (µ) = 1−cos µ µ2 , and 2 = 2 64 − ¡ !2 y + !2 z ¢ !x!y !x!z !x!y −(!2x + !2 z ) !y!z !x!z !y!z − ¡ !2x + !2 y ¢ 3 75 . Then r1 = h 1 0 0 i + c1 (µ)1 + c2 (µ)21 , (4.49) r2 = h 0 1 0 i + c1 (µ)2 + c2 (µ)22 , (4.50) r3 = h 0 0 1 i + c1 (µ)3 + c2 (µ)23 , (4.51) with 1 = h 0, −!z, !y i , 2 = h !z, 0, −!x i , 3 = h −!y, !z, 0 i , 21 = h − ¡ !2 y + !2 z ¢ , !x!y, !x!z, i , 22 = h !x!y, −(!2x + !2 z ) , !y!z i , 23 = h !x!z, !y!z, − ¡ !2x + !2 y ¢ i . 75 Using (4.49) to (4.51), we have @r1 @!x = c2 [0, !y, !z] + c3!x [0, !z, !y] + c4!x £ !2 y!2 z , !x!y, !x!z ¤ , @r1 @!y = c1 [0, 0, 1] + c2 [2!y, !x, 0] + c3!y [0, !z, !y] + c4!y £ !2 y!2 z , !x!y, !x!z ¤ , @r1 @!z = c1 [0, 1, 0] + c2 [2!z, 0, !x] + c3!z [0, !z, !y] + c4!z £ !2 y!2 z , !x!y, !x!z ¤ , @r2 @!x = c1 [0, 0, 1] + c2 [!y, 2!x, 0] + c3!x [!z, 0, !x] + c4!x £ !x!y, !2x !2 z , !y!z ¤ , @r2 @!y = c2 [!x, 0, !z] + c3!y [!z, 0, !x] + c4!y £ !x!y, !2x !2 z , !y!z ¤ , @r2 @!z = c1 [1, 0, 0] + c2 [0, 2!z, !y] + c3!z [!z, 0, !x] + c4!z £ !x!y,−!2x !2 z , !y!z ¤ , @r3 @!x = c1 [0, 1, 0] + c2 [!z, 0, 2!x] + c3!x [!y, !z, 0] + c4!x £ !x!z, !y!z, !2x !2 y ¤ , @r3 @!y = c1 [1, 0, 0] + c2 [0, !z, 2!y] + c3!y [!y, !z, 0] + c4!y £ !x!z, !y!z, !2x !2 y ¤ , @r3 @!z = c2 [!x, !y, 0] + c3!z [−!y, !z, 0] + c4!z £ !x!z, !y!z,−!2x !2 y ¤ , with c3 (µ) = µ cos µ−sin µ µ3 , c4 (µ) = µ sin µ−2(1−cos µ) µ4 . Finally, using (4.43), we have " @ui @!x @vi @!x # = 1 czi Ã" k11 k12 0 k22 # " @r1 @!x @r2 @!x # − " ui vi # @r3 @!x ! pi, " @ui @!y @vi @!y # = 1 czi Ã" k11 k12 0 k22 # " @r1 @!y @r2 @!y # − " ui vi # @r3 @!y ! pi, " @ui @!y @vi @!y # = 1 czi Ã" k11 k12 0 k22 # " @r1 @!z @r2 @!z # − " ui vi # @r3 @!z ! pi, " @ui @tx @vi @tx # = 1 czi " k11 0 # , " @ui @ty @vi @ty # = 1 czi " k12 k22 # , " @ui @tz @vi @tz # = − 1 czi " ui vi # . 76 4.4 Nonlinear ltering In this Section, we resume the leaderfollower mathematical model and an estimation algorithm using a dual unscented Kalman lter. The lter is used to smooth measured variables and to recover leader's velocities. 4.4.1 Mathematical model of the robots In general, leaderfollower controllers require reliable estimates of the leader robot's speed v1 and angular velocity !1 by the follower robot. These quantities can be estimated using an unscented Kalman lter with the appropriate dynamic model and measurements from the camera range `12, bearing ®12, and relative orientation µ12. For this reason, let us consider a leaderfollower system composed of two unicycletype vehicles moving on the plane, with the kinematic model of the ith robot, i = 1, 2, given by q˙i (t) = 2 64 x˙ i (t) y˙i (t) µ˙i (t) 3 75 = 2 64 cos µi (t) 0 sin µi (t) 0 0 1 3 75 " vi (t) !i (t) # . (4.52) where qi (t) = [xi (t) , yi (t) , µi (t)]T 2 SE (2) is the con guration vector, ui (t) = [vi (t) , !i (t)]T 2 Ui µ R2 is the velocity vector, and Ui is a compact set of admissible inputs. Let the Euclidean distance `12 (t) 2 R¸0 and angles ®12 (t), ¯12 (t) 2 (−¼, ¼] between robots 1 (leader) and 2 (follower) be de ned as `12 (t) = q (x1 (t) − xc 2 (t))2 + (y1 (t) − yc2 (t))2, (4.53) ®12 (t) = ³12 (t) − µ1 (t) , (4.54) µ12 (t) = µ1 (t) − µ2 (t) , (4.55) where ³12 (t) = atan2 (y1 − yc2 , x1 − xc 2), and xc 2 (t) = x2+d cos µ2, yc2 (t) = y2+d sin µ2 are the coordinates of the camera, as shown in Figure 2.2. Then, the leaderfollower system can be described in polar form by the set of 77 equations 2 6666666664 x˙ 1 (t) y˙1 (t) µ˙1 (t) `˙12 (t) ®˙ 12 (t) µ˙12 (t) 3 7777777775 = 2 6666666664 cos µ1 0 sin µ1 0 0 1 cos ®12 0 −sin ®12 `12 −1 0 1 3 7777777775 " v1 (t) !1 (t) # + 2 6666666664 0 0 0 0 0 0  cos ¯12 d sin ¯12 sin ¯12 `12 d cos ¯12 `12 0 1 3 7777777775 " v2 (t) !2 (t) # , (4.56) with ¯12 = ®12 + µ12. Equation (4.56) can be written in discrete form as ( xk+1 = f (xk, uk,wk) + vk, yk = h (xk, uk,wk) + nk, (4.57) where xk = [x1 (k) , y1 (k) , µ1 (k) , `12 (k) , ®12 (k) , ¯12 (k)]T , uk = [v2 (k) ,w2 (k)]T , wk = [v1 (k) ,w1 (k)]T , random variables vk and nk are process and measurement noise, respectively, and yk = [`12 (k) , ®12 (k) , ¯12 (k)]T is obtained using measure ments from the vision system (xc, yc, zc), µm, and the value of the camera pan µp. Therefore `12 = p x2c + y2 c + z2 c , µt = arctan µ −xc zc ¶ , ®12 = µt − µm, µ12 = µp + µm. 4.4.2 Nonlinear observability In this Section, we present some observability results for system (4.56). In this par ticular case, the proof of observability is direct, but it will serve the purpose of being introductory for more complex systems analyzed in Chapter 2. The supporting theory for the proofs given in this Section is resumed in Appendix C. Let model (4.56) be written in a ne in control form as § : 8>>< >>: x˙ (t) = X4 i=1 fi (x (t)) ui (t) , y (t) = h (x) , (4.58) 78 with x = [x1, y1, µ1, `12, ®12, ¯12]T 2 M µ R2 × R¸0 × S3, M is an appropriate manifold, y = [`12, ®12, ¯12]T 2 R¸0 × S2, f1 (x) = · cos µ1, sin µ1, 0, cos ®12, − sin ®12 `12 , 0 ¸T , f2 (x) = [0, 0, 1, 0, −1, 1]T , f3 (x) = · 0, 0, 0, −cos ¯12, sin ¯12 `12 , 0 ¸T , f4 (x) = · 0, 0, 0, −d sin ¯12, − d cos ¯12 `12 , −1 ¸T , h (x) = [`12, ®12, µ12]T , u1 (t) = v1 (t) , u2 (t) = !1 (t) , u3 (t) = v2 (t) , u4 (t) = !2 (t) , and we have omitted the dependency with respect to time for brevity. The observation space O is the span over R of ½ 1, `12, ®12, µ12, sin ®12 `12 , sin (®12 + µ12) `12 , d cos (®12 + µ12) `12 , cos ®12, cos (®12 + µ12) , d sin (®12 + µ12) ¾ . Then, the observability codistribution dO(x) is given by dO(x) = span {o1, o2, o3, o4, o5, o6, o7} , with o1 = [0, 0, 0, 1, 0, 0] , o2 = [0, 0, 0, 0, sin ®12, 0] , o3 = [0, 0, 0, 0, sin (®12 + µ12) , sin (®12 + µ12)] , o4 = [0, 0, 0, 0, 1, 0] , o5 = · 0, 0, 0, sin ®12 `2 12 , cos ®12 `12 , 0 ¸ , o6 = · 0, 0, 0, cos (®12 + µ12) `2 12 , sin (®12 + µ12) `12 , sin (®12 + µ12) `12 ¸ , and o7 = [0, 0, 0, 0, 0, 1] . 79 It is clear that x1, y1, and µ1 are unobservable. However, dim (dO(x)) = 3 then, by the observability rank condition theorem, see Appendix C, `12, ®12, µ12 are globally observable. 4.4.3 The dual unscented Kalman lter (DUKF) The unscented Kalman lter (UKF) uses a deterministic sampling approach, the unscented transform (UT), to capture the mean and covariance estimates with a minimal set of sample points. When the sampling points are propagated through the true nonlinear system, the UKF can capture the posterior mean and covariance accurately up to the 3rd order for Taylor series expansion of the Gaussian error distribution with same order of computational complexity as the EKF that can achieve only up to rstorder accuracy. It should also be noted that the of the UKF is the same order as that of EKF [65, 66, 140, 141]. In this Chapter, we use the dual unscented Kalman lter (DUKF) [141] described in Chapter 3. This lter runs two parallel lters for state and parameter estimation. The state estimation lter considers the parameter known for state updating, whereas its parameter estimation counterpart considers states known for parameter updating. In the leaderfollower model (4.57) the states are given by the vector xk and the parameter by vector wk. That is, leader's velocities are considered known for state estimation and then updated by the parameter estimation lter, as it is schematically represented in Figure 4.13. Note that there is no convergence guarantee with the DUKF. 1 1 ( ) vˆ ,wˆ [ ]T f v v 12 12 12 12 12 12 12 1 1 2 2 ˆ ˆ , ˆ , ˆ ˆ ˆ , ˆ , ˆ , , a q w w = = y x x Optimization 12 12 12 ˆ ˆ ˆ q a ( ) [ ]T f v v 12 12 12 12 12 12 12 1 1 2 2 ˆ ˆ , ˆ , ˆ ˆ ˆ , ˆ , ˆ , , a q w w = =   y x x UKF 12 12 12 q a Figure 4.13: Dual estimation problem. 80 4.5 Simulation results Figures 4.144.16 show identi cation results for three robots. Figure 4.14 shows faces detected when the robots are in con gurations q1 = [1.5, .25, 0.0]T and q2 = [2.0,−0.45, 0.0]T relative to the follower con guration. Figure 4.15 and 4.16 show same results with q1 = [2.5,−0.5, 0.0]T , q2 = [3.5, 0.5, 0.0]T and q1 = [2.5,−0.5, 0.0]T , q2 = £ 3.5, 0.5, ¼ 2 ¤T , respectively. The algorithm performs very well, even with small surfaces, changes in illumination and increasing distance. 2 3 4 2 3 5 1 Figure 4.14: Identi cations: Robot 1 (left) IDs 2, 3, 4, 5; robot 2 (right) IDs 1, 2, 3. 2 3 4 2 3 4 Figure 4.15: Identi cations: Robot 1 (right) IDs 2, 3, 4; robot 2 (left) IDs 2, 3, 4. Figures 4.17 and 4.18 show estimated pose for pure translation and pure rotation motions using Lowe's algorithm. 81 1 2 2 3 4 Figure 4.16: Identi cations: Robot 1 (right) IDs 2, 3, 4; robot 2 (left) IDs 1, 2. Figure 4.17: Pure translation using Lowe's method. Figure 4.18: Pure rotation using Lowe's method. 82 Figures 4.19 to 4.25 show simulations using di erent pose estimation methods for pure translation and pure rotation motions. We compared the nal error and the number of iterations to achieve convergence for each algorithm. Figure 4.19 shows the error and number of iteration of PRA algorithm without using information from previous iterations. Figure 4.20 shows same results using previous information as starting con guration. Figure 4.21 shows the error and number of iterations for pure rotations using a priori information. As can be seen, the number of iteration is considerably reduced. 0 5 10 15 0 0.02 0.04 0.06 0.08 0.1 0.12 0.14 0.16 Iteration Error 0 5 10 15 0 2000 4000 6000 8000 10000 12000 14000 16000 18000 Iteration Count Figure 4.19: Pure translation. Error and number of iterations using PRA method. Figures 4.22 and 4.23 show the results for pure translation and rotation motions using Lowe's algorithm. It should be noted that the number of iterations is lower and the nal error is smaller than in the previous case. Figures 4.24 and 4.25 show the results for pure translation and rotation motions using POSIT algorithm with a priori information. The number of iterations is even lower than in the previous case, but the nal error is bigger. A drawback of POSIT algorithm is that, due to the orthographic approximation, it needs the value of czi bigger than  cxi and  cyi to achieve convergence. Figures 4.26 to 4.28 show pose estimation results with Lowe's method initialized with POSIT algorithm. Red skeletons show the estimated positions. The distance and angle are slightly biased with accuracies around 3% and 5%, respectively. As can be seen, the estimated pose is very accurate for practical purposes. Figure 4.29 shows the e ect of noise on the image. As can be seen, the e ect reduces the achievable 83 0 5 10 15 0 0.02 0.04 0.06 0.08 0.1 0.12 0.14 0.16 Iteration Error 0 5 10 15 0 1000 2000 3000 4000 5000 6000 Iteration Count Figure 4.20: Pure translation. Error and number of iterations using PRA method and a priori information. 0 5 10 15 0 1 2 3 4 5 6 7 x 10 3 Iteration Error 0 5 10 15 0 100 200 300 400 500 600 700 800 Iteration Count Figure 4.21: Pure rotation. Error and number of iterations using PRA method and a priori information. 84 0 5 10 15 0 0.05 0.1 0.15 0.2 0.25 0.3 0.35 Iteration Error 0 5 10 15 0 200 400 600 800 1000 1200 Iteration Count Figure 4.22: Pure translation. Error and number of iterations using Lowe's method. 0 5 10 15 0 1 2 3 4 5 6 7 x 10 3 Iteration Error 0 5 10 15 0 5 10 15 20 25 Iteration Count Figure 4.23: Pure rotation. Error and number of iterations using Lowe's method. 85 0 5 10 15 0 0.05 0.1 0.15 0.2 0.25 0.3 0.35 0.4 Iteration Error 0 5 10 15 0 1 2 3 4 5 6 7 8 9 10 Iteration Count Figure 4.24: Pure translation. Error and number of iterations using POSIT algorithm. 0 5 10 15 0 0.005 0.01 0.015 0.02 0.025 0.03 0.035 0.04 0.045 Iteration Error 0 5 10 15 0 0.5 1 1.5 2 2.5 3 Iteration Count Figure 4.25: Pure rotation. Error and number of iterations using POSIT algorithm. 86 maximum distance to 3 m. Figure 4.26: Pose estimation with q1 = [1,−0.25, 0]T , q2 = [2, 0.5, 0]T . Figure 4.27: Pose estimation with q1 = [2.5,−0.5, 0]T , q2 = [3, 0.5, 0]T . Figure 4.30 shows the computation time of the vision system. The blue line shows the time needed to detect a single robot in the eld of view (roughly 10.7 msec). The red line shows the time needed to detect 3 robots plus another robot entering in the eld of view at t = 1.25 sec. Figures 4.31 to 4.37 show real, measured, and estimated relative distance `12 and 87 Figure 4.28: Pose estimation with q1 = [1.5,−0.5, 0]T , q2 = £ 4, 0.5,−¼ 2 ¤T . bearing ®12 using the DUKF. The initial values in the simulation are x0 = [1.5, 0.25, 0, 1.047, 0.233, 0.232]T , Px0 = diag (10, 10, 10) , w0 = [0, 0]T , Pw0 = diag (500, 0.01) for gures 4.31 to 4.33, and Pw0 = diag (10, 0.01) for g ures 4.34 to 4.37. It is assumed that there is a bias in the measured variables given by ¯n = [−0.0702, 0.0248, 0.0236]T , with measurement noise covariance Rn = diag (0.07, 0.03, 0.03). Figures 4.31 and 4.32 show estimation results with the DUKF in open loop. That is, referring to Figure 4.13, the real values of v1 and !1 are sent to the UKF estimator, and real state values x12 are sent to the optimization block. It can be noticed that the optimizer fails to obtain the real leader's velocities due to lack of signal richness, as shown in in Figure 4.32. To improve the richness properties and the convergence, a Gaussian noise signal with covariance Rn0 = diag (0.0035, 0.015, 0.015) is added to the measured output vector for the optimization block. The performance is notoriously increased, as shown in Figure 4.33. Figures 4.34 to 4.37 show the results of the DUKF working in closed loop. It should be noticed the accuracy in the state and velocity estimation. The mean square errors obtained with the estimations are e` = 1.821 × 10−4, e® = 5.730 × 10−5, 88 Distance = 3 m Distance = 3.5 m Distance = 2 m Distance = 2.5 m Distance = 1 m Distance = 1.5 m Figure 4.29: Fog e ect on pose estimation. 89 0 0.5 1 1.5 2 2.5 3 3.5 10 10.5 11 11.5 12 12.5 Elapsed time [msec] time [sec] 1 robot 4 robots Figure 4.30: Computational load with one robot in the eld of view (blue line) and three robots plus another entering at t = 1.25 red line). 90 x 103 0 2 4 6 8 0.2 0 0.2 0.4 0.6 0.8 0 2 4 6 8 5 0 5 10 15 0 2 4 6 8 0 0.2 0.4 0.6 0.8 1 0 2 4 6 8 0.3 0.25 0.2 0.15 0.1 0.05 0 0.05 0.1 0 2 4 6 8 0 0.2 0.4 0.6 0.8 1 0 2 4 6 8 0.01 0 0.01 0.02 0.03 0.04 real measured estimated real measured estimated real measured estimated ( ) [rad] 12 a t ( ) [m] 12 t ( ) [rad] 12 q t ( ) ( ) ˆ ( ) [rad] 12 12 e t q t q t q =  e (t) a (t) aˆ (t) [rad] a =  time [sec] time [sec] time [sec] time [sec] time [sec] time [sec] ( ) ( ) ˆ ( ) [m] 12 12 e t t t =  Figure 4.31: Relative distance and bearing estimation using a DUKF. 91 0 2 4 6 8 1 0.5 0 0.5 1 1.5 2 2.5 3 0 2 4 6 8 1.5 1 0.5 0 0.5 1 0 2 4 6 8 0 1 2 3 4 5 6 7 8 0 2 4 6 8 8 7 6 5 4 3 2 1 0 x 103 x 103 real estimated time [sec] time [sec] time [sec] time [sec] real estimated ( ) ( ) ˆ ( ) [m/ sec] 1 1 e t v t v t v =  e (t) w(t) wˆ (t) [rad / sec] w ( ) [rad / sec] =  1 w t ( ) [m/ sec] 1 v t Figure 4.32: Velocity estimation using a DUKF. 92 0 2 4 6 8 20 15 10 5 0 5 10 0 2 4 6 8 10 5 0 5 10 15 20 0 2 4 6 8 2 1 0 1 2 3 4 5 6 x 103 0 2 4 6 8 6 5 4 3 2 1 0 1 2 x 103 real estimated time [sec] time [sec] time [sec] time [sec] real estimated ( ) ( ) ˆ ( ) [m/ sec] 1 1 e t v t v t v =  e (t) w(t) wˆ (t) [rad / sec] 



A 

B 

C 

D 

E 

F 

I 

J 

K 

L 

O 

P 

R 

S 

T 

U 

V 

W 


