Formations and Obstacle Avoidance in Mobile Robot ... - DiVA Portal

Formations and Obstacle Avoidance in Mobile Robot Control

¨ Petter Ogren

Doctoral Thesis Stockholm, 2003

OPTIMIZATION AND SYSTEMS THEORY DEPARTMENT OF MATHEMATICS ROYAL INSTITUTE OF TECHNOLOGY STOCKHOLM, SWEDEN

Akademisk avhandling som med tillst˚ and av Kungliga Tekniska H¨ ogskolan framl¨ agges till offentlig granskning f¨ or avl¨ aggande av teknologie doktorsexamen fredagen den 6:e juni 2003 kl 10.00 i Kollegiesalen, Administrationsbyggnaden, Kungliga Tekniska H¨ ogskolan, Valhallav¨ agen 79. ¨ c Copyright
2003 by Petter Ogren ISBN 91-7283-521-4 TRITA-MAT-03-OS-06 ISSN 1401-2294 ISRN KTH/OPT SYST/DA 03/06-SE Universitetsservice US AB, Stockholm, 2003

Till Sture, Christina och Maria

v

Abstract This thesis consists of four independent papers concerning the control of mobile robots in the context of obstacle avoidance and formation keeping. The first paper describes a new theoretically verifiable approach to obstacle avoidance. It merges the ideas of two previous methods, with complementary properties, by using a combined control Lyapunov function (CLF) and model predictive control (MPC) framework. The second paper investigates the problem of moving a fixed formation of vehicles through a partially known environment with obstacles. Using an input to state (ISS) formulation the concept of configuration space obstacles is generalized to leader follower formations. This generalization then makes it possible to convert the problem into a standard single vehicle obstacle avoidance problem, such as the one considered in the first paper. The properties of goal convergence and safety thus carries over to the formation obstacle avoidance case. In the third paper, coordination along trajectories of a nonhomogenuos set of vehicles is considered. By using a control Lyapunov function approach, properties such as bounded formation error and finite completion time is shown. Finally, the fourth paper applies a generalized version of the control in the third paper to translate, rotate and expand a formation. It is furthermore shown how a partial decoupling of formation keeping and formation mission can be achieved. The approach is then applied to a scenario of underwater vehicles climbing gradients in search for specific thermal/biological regions of interest. The sensor data fusion problem for different formation configurations is investigated and an optimal formation geometry is proposed. Keywords: Mobile Robots, Robot Control, Obstacle Avoidance, Multirobot System, Formation Control, Navigation Function, Lyapunov Function, Model Predictive Control, Receding Horizon Control, Gradient Climbing, Gradient Estimation. Mathematics Subject Classification (2000): 70E60, 68T40, 93D15, 34D20.

Acknowledgment1 During my time as a graduate student I have had the privilege to meet and interact with many intelligent and inspiring people. This journey would have been much harder and a lot less fun without them. First of all I would like to express my gratitude to my advisor, Professor Xiaoming Hu. Somehow, he has always found time to discuss whatever was on my mind, from mathematical details to higher level research and career choices. I would also like to express my gratitude to Professor Naomi Leonard. My two visits to Princeton proved to be most rewarding both professionally and personally. I furthermore would like to thank Professor Anders Lindquist, for making the division of Optimization and Systems Theory such a great place to work and Professor Henrik Christensen, for giving me the opportunity to do robotics research. The faculty members and graduate students at the division all contributed in making the lunch and coffee break discussions animated and entertaining. You are too many to mention here, but know that I wanted to. Troy, Pradeep, Eddie, Luc and Eliane of Princeton, thanks for the hospitality and many warm, witty conversations. Magnus Egerstedt, my former colleague. Thank you for showing me, by example, that nothing is impossible. Anders Blomqvist, my colleague and room mate during the last couple of years. Thank you for being my fellow Muppet-show-balcony-guys-style heckler. To Concepts, Incentive structures and Apple farms! Sture, Christina and Maria, the older I get the more I realize and appreciate what you have given me. Finally, thank you Viktoria, for giving me something to look forward to at the end of each and every day.

¨ Petter Ogren Stockholm, April 2003

1 The research described in this thesis was financially supported by the Swedish Foundation for Strategic Research through its Center for Autonomous Systems (CAS) at KTH

Contents

1 Introduction 1. Robotics, Uncertainty and Feedback . . . . . . . . . . . . 2. Problems Considered and Motivating Applications . . . . 2.1 Navigation and Obstacle Avoidance . . . . . . . . 2.2 Formations and Multi-Agent Robotics . . . . . . . 3. Background Material on Robotics and Control . . . . . . 3.1 Robots and Mathematical Models . . . . . . . . . 3.2 Lyapunov Theory of Stability . . . . . . . . . . . . 3.3 Brockett’s Theorem and Nonholonomic Constraints 3.4 Feedback linearization of the Unicycle . . . . . . . 3.5 Reacting vs. Planning . . . . . . . . . . . . . . . . 4. Examples of Current Research . . . . . . . . . . . . . . . 4.1 Navigation and Obstacle Avoidance . . . . . . . . 4.2 Formations and Multi-Agent Robotics . . . . . . . 5. Summary of the Appended Papers . . . . . . . . . . . . . 5.1 Division of work between authors . . . . . . . . . . References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . A

A Convergent Dynamic Window Approach to Obstacle Avoidance 1. Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . 2. Previous Work used in this Paper . . . . . . . . . . . . . . 2.1 The Dynamic Window approach and its extension 2.2 Exact Robot Navigation using Artificial Potential Fields . . . . . . . . . . . . . . . . . . . . . . . . . 2.3 Control Lyapunov Functions and Model Predictive Control . . . . . . . . . . . . . . . . . . . . . . . . 3. A Proven Convergent Dynamic Window Approach . . . . 3.1 Navigation Function . . . . . . . . . . . . . . . . . 3.2 General Control Scheme . . . . . . . . . . . . . . . 3.3 Discretized Control Scheme . . . . . . . . . . . . . 3.4 Example of Convergence Failure of Previous Approach . . . . . . . . . . . . . . . . . . . . . . . . . 4. Proof of Convergence and Safety . . . . . . . . . . . . . . 5. Simulation Example . . . . . . . . . . . . . . . . . . . . .

1 4 5 5 8 8 13 15 17 20 25 25 29 33 36 36

42 43 43 44 45 46 47 50 51 55 55 57

x

Contents

6. Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . B Obstacle Avoidance in Formation 1. Introduction . . . . . . . . . . . . 2. Formation Control . . . . . . . . 3. Obstacle Avoidance . . . . . . . 4. Simulation Example . . . . . . . 5. Conclusions and Future Research References . . . . . . . . . . . . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

C A Control Lyapunov Function Approach to Multi-Agent Coordination 1. Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . 2. Formation Functions . . . . . . . . . . . . . . . . . . . . . 3. Coordinated Control . . . . . . . . . . . . . . . . . . . . . 4. Theoretical Properties . . . . . . . . . . . . . . . . . . . . 5. Simulation Examples . . . . . . . . . . . . . . . . . . . . . 6. Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . 7. Recent Work . . . . . . . . . . . . . . . . . . . . . . . . . References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

59 59

64 65 73 74 76 76

82 83 86 87 90 92 92 92

D Cooperative Control of Mobile Sensor Networks: Adaptive Gradient Climbing in a Distributed Environment 1. Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . 98 2. Artificial Potentials, Virtual Bodies and Symmetry . . . . 102 3. Formation Motion: Translation, Rotation, and Expansion 105 3.1 Translation and Rotation . . . . . . . . . . . . . . 105 3.2 Expansion and Contraction . . . . . . . . . . . . . 106 3.3 Retained Symmetries . . . . . . . . . . . . . . . . . 106 3.4 Sensor-Driven Tasks and Mission Trajectories . . . 107 4. Speed of Traversal and Formation Stabilization . . . . . . 108 4.1 Convergence and Boundedness . . . . . . . . . . . 108 4.2 Example: Two Vehicle Rotation . . . . . . . . . . 110 5. Gradient Climbing in a Distributed Environment . . . . . 112 5.1 Least Squares and Optimal Distances . . . . . . . 113 5.2 Kalman Filter . . . . . . . . . . . . . . . . . . . . . 121 5.3 Convergence . . . . . . . . . . . . . . . . . . . . . 122 6. Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . 125

Contents

xi

References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 125

Introduction This thesis consists of an introduction and four separate appended papers. The purpose of the introduction is to give a glimpse of mobile robotics in general and the fields of navigation and coordination in particular. The first two sections of the introduction should be accessible to anyone. The remaining parts are intended for someone familiar with basic mathematical concepts, but not necessarily with robotics and control.

1.

Robotics, Uncertainty and Feedback

Hasta la Vista, Baby. - T2, 1991. The quote above (the 7th most popular movie quote, according to [FCH+ 00]) illustrates one of the many futuristic visions presented by the entertainment industry the last decades. As these visions are still very far from being realized it might be interesting to see what visions are held in the robotics research community itself. In the words of the scientist Jean-Claude Latombe: “One of the ultimate goals in Robotics is to create autonomous robots. Such robots will accept high-level descriptions of tasks and will execute them without further human intervention. The input descriptions will specify what the user wants done rather than how to do it. The robots will be any kind of versatile mechanical device equipped with actuators and sensors under the control of a computing system.” [Lat91]. Looking back at the development of robotics it can be said to have started with the first visions of cybernetics1 by MIT Mathematician Norbert Wiener in the late 1940’s [Wie48]. It would however take some time before something close to the goals of Latombe (above) appears. The early robots were primary used for manufacturing, i.e., welding, painting, and so-called pick and place operations. They were used in environments where very few unexpected events occurred and where exact repeatability of actions was the main measure of excellence. 1 Cybernetics - the theoretical study of communication and control processes in biological, mechanical, and electronic systems, especially the comparison of these processes in biological and artificial systems. Source: The American Heritage Dictionary of the English Language, Fourth Edition, Houghton Mifflin Company.

2

Introduction

The biggest obstacle in the design of robots for other areas of application, such as domestic environments, is uncertainty. In an area where humans move about, positions of things are bound to change all the time. Furthermore, the variety of obstacles and objects the robot will encounter is very large. In such circumstances, both sensing and control become much more complex; in the words of Latombe [Lat91]: “When knowledge at planning time is too incomplete, it may become necessary to interweave planning and execution in order to collect appropriate information through sensing.” A more concise description of “interweaving planning and execution” in this context is feedback control. It turns out that in many application, considerable performance improvements can be attained by continuously measuring relevant properties of a system, and feeding this information back into the control of the system. The scope of the theory is summarized in the following words: “The field of control provides the principles and methods used to design engineering systems that maintain desirable performance by automatically adapting to changes in the environment.” in [M˚ AB03, p. i]. To understand what feedback and control theory are, we look at a very common application: the cruise control in a car. A cruise control has to deal with uncertainty in terms of the car itself, e.g. changing mass due to number of passengers, and in terms of external conditions, e.g. up- or down hill road and different wind conditions, and still keep the car traveling at the prescribed speed. Reference Signal + −

Control Signal

Controller

Measurements

Process

Measurements

Figure 1: A feedback loop. To understand how the cruise control works we look at a typical feedback loop, depicted in Figure 1. The process corresponds to the car, the measurements of the process could be the speed and acceleration, the reference signal is the prescribed velocity, e.g. 60mph, the controller is the cruise control unit itself and the control signal is the throttle or gas pedal position. By adjusting the control (throttle) we try to make the measured output (speed and acceleration) behave as desired (e.g. stay at

1. Robotics, Uncertainty and Feedback

3

60mph). What a standard cruise controller, of so-called PID-type does is that it compares the measured output, the speed, to the desired value and if it is too low the throttle is increased (P, proportional control). If the speed is increasing fast, the throttle is decreased (D, differentiating control) and if it has been consistently too low over a time period the throttle is increased (I, integrating control). All these three effects (P, I, and D) are combined into one resulting position of the gas pedal. This is repeated at short time intervals all the time, thus allowing the over-all system to adopt to changes in the environment, e.g. a sudden strong head wind.

Figure 2: A fly-ball governor. Courtesy of Arts et Metiers Museum, Paris, France One of the earliest examples of feedback control mechanisms is the fly-ball governor depicted in Figure 2. It was invented in the 1780’s and used as an important part of the steam engine. In one clever mechanical design it combines the elements of measuring, computing and delivering a control signal. The process in this case is a steam engine. To understand how the fly-ball governor works we note that the engine turns the vertical axis (measurement). The centrifugal forces rising from the speed of the engine makes the two masses move up, against gravity, and out from the axis. This in turn moves a small lever, A, that is connected to the throttle of the engine. Therefore, the faster the engine goes, the less steam pressure it will get, which reduces the rotation. The machine is thus forced towards an equilibrium speed by the controller. This equilibrium can be adjusted by changing the geometry of the fly-ball construction,

4

Introduction

e.g. lengthening the rod P . The arrival of the digital computer made the separation into measurement, computation and delivery of control signal much easier, allowing more complex control strategies to be implemented. Nowadays feedback control can be found in applications ranging from high performance military jets to DVD players, and, of course, in robotics -the topic of this thesis. As robotics evolves to deal with increased levels of uncertainty the control problems change from e.g. how much throttle to apply to what route to chose, or, more general, what subtasks to decompose a mission into. To address some of these questions new methodologies in control have to be devised, a need identified by the Panel on future directions in control, dynamics and systems, [M˚ AB03, p. 90]: “ The role of logic and decision making in control systems is becoming an increasingly large portion of modern control systems. ... This decision making includes not only traditional logical branching based on system conditions, but higher levels of abstract reasoning using high level languages. These problems have traditionally been in the domain of the artificial intelligence (AI) community, but the increasing role of dynamics, robustness, and interconnection in many applications points to a clear need for participation by the control community as well ... To tackle these problems, the Panel recommends that government agencies and the control community Substantially increase research in control at higher levels of decision making, moving toward enterprise level systems. ” As the Panel suggests, this will be an area of intensified research in the years to come. Robotics is an interesting field, not only because of its mind-boggling potential but also due to it being inherently cross-disciplinary. To actually build a robot it takes some mechanical and electrical engineering. Then you need some computer science and/or control theory to design the software to run it. In the background there are the mathematical theories underlying all three disciplines above, and on top of everything you can always draw inspiration from biology, psychology, and philosophy. In this thesis we will look closer at some parts of the mathematical tools, control theory and, to some extent, computer science and AI of robotics.

2.

Problems Considered and Motivating Applications

The research problems considered in this thesis stem from two areas of robotics. Both are in the subfield called mobile robotics, thus excluding

2. Problems Considered and Motivating Applications

5

robot arms: the most common robot type in industry. The first research area is navigation and obstacle avoidance which basically deals with the question of getting from A to B in a safe and efficient manner. The second research area is multi-robot coordination. This is a somewhat broader area where the common theme is that of trying to achieve a collective goal using a group of robots, e.g. stay in a prescribed formation. 2.1.

Navigation and Obstacle Avoidance

The problem of programming a mobile robot to move from one place to another is of course as old as the first mobile robot. This is, however, not as easy as one might think. Questions like “What path should be chosen to get to the goal location?” and “How fast and how close to the obstacles can the robot go without compromising safety?” need to be considered. This is done in Paper A of the thesis, where is is assumed that we are given a high-quality map of the immediate surroundings of the robot. It must be noted however, that map building and localization contains a whole research field in itself. In [Jen01], the area of simultaneous localization and mapping (SLAM) is investigated. Since GPS is not an option for indoor applications, this problem is quite hard, as can be seen by comparing a 15th century explorers map with the satellite images available today. SLAM is however not the topic of this thesis and we assume that someone has taken care of this problem for us. One can also imagine the navigation problem being complicated by a set of moving obstacles, such as humans, pets, and other robots. The control method proposed in paper A will probably perform well in such settings, due to the fast reactive nature of one of its components, but the provable properties of safety and goal attainment does not extend to the moving obstacle case. 2.2.

Formations and Multi-Agent Robotics

The mob has many heads but no brain. -English Proverb The thought of cooperating robots has received an increasing amount of attention in recent years. Besides the philosophical interest in cooperating machines the main reason is to try to take advantage of “strengths in numbers”, i.e., that there are properties like • Redundancy/Robustness

6

Introduction

• Flexibility • Price reduction • Efficiency • Feasibility to be gained. Having several robots doing something often means that you have the flexibility of dividing the robots into groups working at different locations. Having many also implies robustness, since losing one robot leaves the others intact to finish the mission. The mobile robots of today are typically produced in small numbers; however, if there is a big increase in multi-agent applications, there might be a price reduction due to mass production benefits. Efficiency can be gained in terms of e.g. fuel consumption in formation flight. Finally, some missions are impossible to carry out with only one robot; these include deep space inferometry, a satellite imaging application, and the surveillance of large areas or buildings. Following Arkin [Ark98], we divide the coordination problems into the following fields • Foraging2 /Consuming, where randomly placed objects in the environment are to be found and either carried somewhere or operated on in place. This includes collecting rocks on e.g. Mars. • Grazing, where an area should be swept by sensors or actuators. This includes lawn mowing (e.g. the Husqvarna Solarmower) and vacuuming (e.g. the Electrolux Trilobite). Special cases of area sweeping include so-called search-and-rescue and pursuit-evasion scenarios, [VSK+ 03]. In these situations the looked for item, e.g. a missing person or an enemy vehicle, is moving. • Formation keeping, where the robots are to form some geometric pattern and maintain it while moving about in the world. This problem is studied in e.g. [YBK01]. Applications include formation flight for fuel efficiency and coordinated motion for collaborative lifting of large objects. In this area biological influences are very common and efforts are being made both to understand animal flocking/schooling and copy their effective strategies. A satellite application called deep space inferometry requires a large and exact 2 foraging

- searching for food

2. Problems Considered and Motivating Applications

7

sensor spacing which would be impossible to achieve with a single satellite. • Traffic control, where a number of vehicles share a common resource, highways or airspace, while trying to achieve their individual goals. In automated highway projects [Bis01], the problem scales have ranged from keeping inter-vehicle spacing (formation keeping) via lane changes to choosing routes that minimize the over all effect of traffic jams. Air traffic control investigations, [TPS98], are motivated by the increasing congestion around major airports. The hope is to improve efficiency without compromising the vital safety. The properties of flexibility and robustness is of course very attractive with the armed forces as is shown in the following quote from [M˚ AB03, p. 52] “The U.S. military is considering the use of multiple vehicles operating in a coordinated fashion for surveillance, logistical support, and combat, to offload the burden of dirty, dangerous, and dull missions from humans.” Problems facing a multi-agent team operating in a limited space include blockage and collisions. More generally, a highly distributed system might generate competition rather than cooperation. Attempts to exploit such inter-robot competition in a market economic framework has been investigated in e.g. [DS02]. Finally there is always a cost of communication, in terms of additional hardware, increased computational load, and energy consumption. We conclude with noting that the possible drawbacks of a multi-agent approach is hinted at in the old saying “Too many cooks spoil the broth”. The multi-agent problems considered in this thesis are the following. In paperB the problem is to move a given formation through a partially known environment, containing a priori unknown obstacles, towards some goal point while maintaining formation. This can be part of a grazing type search and rescue mission or just a convenient way to move a group of robots with e.g. different sensors. PaperC describes a high level coordination scheme allowing very different types of robots to move in a formation respecting a user defined limit on how exact the formation geometry is to be kept. PaperD builds upon PaperC to allow formation motions including rotations and expansions of a set of interchangeable vehicles controlled in a decentralized way. In paperD, it is also investigated how a group of e.g. underwater vehicles could combine their temperature readings in order to find and reach so-called up-wellings and other temperature zones of high interest to biologists.

8

3.

Introduction

Background Material on Robotics and Control

This section contains material to facilitate the reading of the appended papers; it should be accessible to anyone with a science or engineering background. The first section reviews the models that are the starting point of most mathematical excursions into the realm of mobile robotics. It also contains some pictures of robots from the Center for Autonomous Systems (CAS) laboratory at KTH. The second section reviews the mathematical tool of Lyapunov theory, lying at the core of nonlinear system analysis and being used in all four papers. The following sections make a note on the topic of nonholonomic constraints and display the use of feedback linearization on a particular robot model. Finally, the last section tries to compare and contrast the two main control paradigms in robotics: The ‘Deliberative/Planning’ and the ‘Reactive’ approaches. 3.1.

Robots and Mathematical Models

The book of the universe is written in the language of mathematics. - Galileo Models have limitations, stupidity does not. - Unknown Mathematical models lie at the foundation of every quantitative science, from physics to finance and from electronics to chemistry. They allow us to make an abstraction of a real world system and use this abstraction to understand the system. The benefits of a model stems from two factors: how well the model predicts the real world and how simple the model is. The simpler, or less complicated, the better, and the more accurate, the better. Mathematics has played a fundamental role in the development of control theory. In fact, “Control theorists and engineers have made rigorous use of and contributions to mathematics, motivated by the need to develop provably correct techniques for design of feedback systems”[M˚ AB03, p. 3]. Of course, properties can be proven only in the context of a mathematical model; given a set of assumptions some specific set of conclusions can be drawn. The applicability of these conclusions to the real world is then depending on the accuracy of the mathematical model and the validity of the assumptions. We will now go through some of the most common mathematical models in mobile robotics. For a more thorough presentation we refer

3. Background Material on Robotics and Control

9

to [CdWSB96]. The simplest possible model describes a point-like robot moving around in the plane. x˙ 1 x˙ 2

= = or x˙ =

u1 , u2 ,

(3.1)

u,

where x, u ∈ R2 , x = (x1 , x2 )T , u = (u1 , u2 )T . This model was used in e.g. [CMKB02]. This type of model is called kinematic or first order since there is a maximum of one integration from input to state. In many instances it is natural to limit the velocity with an input bound, ||u|| ≤ umax . The main drawback of the model is that it allows instant velocity changes, this problem is especially significant if the vehicle is heavy, relative to its motor power. The obvious fix of the “instant velocity changes” problem is to make the model obey Newton’s law, F = ma. This corresponds to x˙ 1 x˙ 2 v˙ 1 v˙ 2

= = = = or x ¨ =

v1 , v2 , u1 /m, u2 /m,

(3.2)

u/m,

where m is the mass, x = (x1 , x2 )T ∈ R2 is the position, u = (u1 , u2 )T ∈ R2 is the input (force) and (v1 , v2 )T ∈ R2 is the velocity. Models using Newton’s law are called dynamic or second order. The one above is also referred to as a double integrator in the plane, since the mapping from input to position is integrating the input twice. Here it is also natural to bound the input force ||u|| ≤ umax . Sometimes, for notational convenience, the mass is incorporated in a new input u
= u/m yielding x ¨ = u
with an input bound ||u
|| ≤ u
max . This model was implicitly used for the XR4000 robot (depicted in Figure 3) in [BK99]. It is further more used in [OSM02], paperA, paperB and paperD. This robot type is called omni directional i.e. it is free to move equally well in all directions from standing still. The XR4000 can furthermore actually rotate freely independent of its translational motion. This property is achieved through a clever castor mounting of the tree independently actuated wheels.

10

Introduction

Figure 3: The Nomadic Technologies XR4000 with a Puma 560 manipulator arm mounted on top. A mechanically much simpler, and therefore more common, wheel geometry is seen in Figure 4: two large independently actuated fixed wheels and one small free moving castor wheel to keep the balance. The configuration is called differential drive or unicycle and is schematically depicted in Figure 5. The simplest possible (kinematic) model for the unicycle, taken from [CdWSB96], is given by z˙1 z˙2 θ˙

= v cos θ, = v sin θ, = ω,

(3.3)

where z = (z1 , z2 )T is the center point on the wheel axis, see Figure 5, θ ∈ R is the orientation and the inputs v, ω is the translational and angular velocities respectively. This model describes many indoor robots such as the Scout above as well as outdoor caterpillar-type and skid-to-turn vehicles, e.g. the ATRV in Figure 6. The errors are however much bigger for the outdoor vehicles

3. Background Material on Robotics and Control

11

Figure 4: Nomadic Technologies Scout. since the center of rotation depends on the uncertain wheel-to-ground friction. This model was used in [EH01, DOK98]. It can also be used as a very coarse model of an airplane if you add bounds on ω as well as a lower positive bound on the velocity v. This renders a speed dependent minimal turning radius and is used to model unmanned aerial vehicles (UAVs) in [BMGA02]. As before, the kinematic model above does not obey Newton’s law. Adding two more states to remedy this flaw yields the dynamic unicycle: z˙1 z˙2 θ˙ v˙ ω˙

= = = = =

v cos θ, v sin θ, ω, F/m, τ /J.

(3.4)

where again z = (z1 , z2 )T is the position, θ ∈ R is the orientation, v, ω are the translational and angular velocities, respectively, and the inputs are force over mass, F/m, and torque over moment of inertia, τ /J. The next natural step is the model of a car-like vehicle. These systems are much harder to work with and although a lot of attention has been

12

Introduction

Fixed wheels

Castor wheel

L (x ,x ) 1

(z ,z ) 1

2

2

Figure 5: The unicycle model. given to e.g. the parallel parking problem, car-like vehicles are typically not considered in the subfields treated in this thesis, nor are they carried by most mobile robot hardware vendors. However, the abundance of reasonably priced radio controlled cars and importance of car-type vehicles in our society still motivates their inclusion in this section. Looking at the car-type vehicle in Figure 7 we see that the back axis configuration resembles a unicycle and thus the (z1 , z2 )T equations are similar. The rotation control is however different. To understand the equations we assume the vehicle moves while applying a constant steering angle φ. This makes the point (z1 , z2 )T trace out a circle with radius p, which in turn gives the angular velocity as θ˙ = vp = Lv Lp = Lv tan φ. This gives the equations z˙1 z˙2 θ˙

= v cos θ, = v sin θ, φ = v tan L .

(3.5)

Typically there is a hardware bound on the steering angle φ ∈ [−b, b]. The fact that v appears in all three rows excludes the possibility to turn on the spot as the unicycle. Newtonian dynamics can of course be incorporated in this model as well, but as it is not used in any of the papers we will not dwell on that topic here. In the sections below we will first look at the general question of whether or not a system is stable. We will then review the implications of a surprising and important result of Brockett, followed by a display of how some of the difficulties with models (3.3),(3.4) can be addressed using feedback linearization. Finally we will compare and contrast the two major design paradigms in robot control.

3. Background Material on Robotics and Control

13

Figure 6: The iRobot Corporation ATRV.

L

1111 0000 0000 1111

111 000 000 111 000 111 000 111 000 111 000 111 000 111 000 111 000 111 000 111

(z1 ,z2 )

θ

φ

1111 0000 0000 1111

p

Figure 7: A car-type of vehicle, the darker rectangles are the wheels.

3.2.

Lyapunov Theory of Stability

Lyapunov theory is a very elegant and powerful tool used in stability analysis of nonlinear systems. The meaning of stability in this context is formally defined below, but intuitively it means that small perturbations will not give rise to big changes. The ideas below were first presented in 1892 by the Russian mathematician Aleksandr Lyapunov and they build upon a very natural observation: if the energy of a system decreases constantly, then the system will end up in a state of locally minimal

14

Introduction

energy. We follow Sastry, [Sas99], closely, but for the sake of clarity and conciseness we sacrifice some generality, e.g. time dependence. Throughout this section we consider a system on nonlinear differential equations. (3.6) x˙ = f (x), x(t0 ) = x0 , where x, x0 ∈ Rn and f (·) : Rn → Rn . Some condition to guarantee existence and uniqueness of solutions is further needed, e.g., f locally satisfying the Lipschitz condition: ||f (x1 ) − f (x2 )|| ≤ l||x1 − x2 ||, where l ∈ R+ is a fixed scalar . Points of interest in stability analysis are the so-called stationary or equilibrium points. Definition 3.1. (Equilibrium Point) x∗ is said to be an Equilibrium point of (3.6) if f (x∗ ) = 0. We will now go on to investigate if a given equilibrium point is stable or not. Since it is always possible to move a point of interest to the origin by a coordinate transformation, we will henceforth assume x∗ = 0 for notational convenience. Definition 3.2. (Stable Equilibrium) The equilibrium point x∗ = 0 is said to be a stable point of (3.6) if, for all  > 0, there exists a δ() such that ||x0 || < δ() ⇒ ||x(t)|| < , ∀t ≥ t0 , where x(t) is the solution of (3.6). Definition 3.3. (Asymptotic Stability) The equilibrium point x∗ = 0 is said to be an asymptotically stable point of (3.6) if (a) It is stable. (b) It is attractive, i.e. there exists a δ such that ||x0 || < δ ⇒

lim ||x(t)|| = 0,

t→∞

where x(t) is the solution of (3.6). Note that (b) above does not necessarily imply (a). Before stating the main stability result of Lyapunov theory we need one more definition.

3. Background Material on Robotics and Control

15

Definition 3.4. (Locally Positive Definite Function) A continuous function V (x) : Rn → R+ is called a locally positive definite function if, for some h > 0 and α(·), V (0) = 0, and V (x) ≥ α(||x||), ∀x : ||x|| ≤ h where α(·) : R+ → R+ is continuous, strictly increasing and α(0) = 0. Theorem 3.1. (Basic Lyapunov Theorem) Suppose we are given a system (3.6), an equilibrium point x∗ = 0 and a locally positive definite function V (x). (a) If −V˙ ≥ 0 (locally), then x∗ is stable. (b) If −V˙ is locally positive definite, then x∗ is asymptotically stable. In both cases above we call the function V (·) a Lyapunov function. As indicated above, this is a extremely compact exposition of the very rich set of Lyapunov results in control theory. We note that there are many extensions, among which the most straightforward include time dependent systems, and non-local results. For these we refer to the textbook used here, [Sas99]. 3.3.

Brockett’s Theorem and Nonholonomic Constraints

In a seminal paper by Brockett, [Bro83], it is implied that so-called nonholonomic systems can not be stabilized by a “well behaved”, i.e. differentiable and non time varying, state feedback. This result is important for two reasons: firstly, many control problems can be restated as stability problems, e.g. making a system assymptotically stable around some desired point of operation, and secondly, many robot models, including (3.3),(3.4), and (3.5), are in fact nonholonomic. Intuitively a nonholonomic constraint restricts a vehicles motion locally but not globally. Consider a car; it can not move sideways instantly, but it can get to a position to its side through a series of backing and forward moving trajectories, as in a parallel parking operation. To make this notion formal we follow [MLS94] in the definitions below. Suppose q ∈ Rn is the configuration variable, i.e., positions and angles, of some system. Definition 3.5. (Holonomic Constraint) A constraint that restricts motion of system to a smooth hyper surface in the configuration space is called a Holonomic constraint.

16

Introduction

Example 3.1. (Motion on a circle) The algebraic equality h(q) = q12 + q22 − C = 0 is always fulfilled for the system     q˙1 q2 q˙2  = −q1  , q˙3 u similar to an augmented spring mass system. Note that C depends on the initial state of the system. Definition 3.6. (Pfaffian Constraint) Given a system with configuration variables q, we call a constraint of the following type Pfaffian: A(q)q˙ = 0, where A(q) ∈ Rm×n and q ∈ Rn . Remark 3.1. Note that all algebraic constraints h(q) = 0 can be written as an equivalent Pfaffian constraint h
(q)q˙ by a differentiation with respect to time:   q˙1 h(q) = q12 + q22 − C = 0 ⇒ 2(q1 , q2 , 0) q˙2  = 0. q˙3 The opposite is however not true, a fact that motivates the definition below. Definition 3.7. (Nonholonomic Constraint) A Pfaffian constraint that is not equivalent to a holonomic constraint is called a Nonholonomic constraint. Remark 3.2. The definitions above can also be stated in terms of socalled one-forms or covector fields, [Isi96], being exact or not. Example 3.2. (The Kinematic Unicycle) z˙1 z˙2 θ˙

= v cos θ, = v sin θ, = ω,

3. Background Material on Robotics and Control

17

implies the nonholonomic constraint 

− sin θ

cos θ

  z˙1 0 z˙2  = 0, θ˙ 

i.e., no sideways motion of a point on the wheel axis. Note that the constraint above is indeed not equivalent to a holonomic one, i.e., the motion is not restricted to a hyper surface in the configuration space. In fact, there is a feasible trajectory between any two configurations. Paying attention to these kinds of questions is important when choosing robot type for an application. Brockett’s theorem is further more not contradicted by the feedback linearization below. The price we pay for free motion of an off-axis point is that we totally lose control of orientation, (θ˙ being the internal dynamics). This is acceptable in cases where the mission critical sensor or actuator, e.g. a robot arm, is mounted on the off-axis position in question and is itself free to rotate. Vehicle orientation is however crucial using other types of actuators, such as those on e.g. a forklift. Note also that the result only excludes differentiable static state feedback. However, there are time varying or discontinuous feedback laws that can stabilize such systems.

3.4.

Feedback linearization of the Unicycle

In this section we will see how the models (3.3) and (3.4) can in fact be made to look like (3.1) and (3.2). This can be achieved if we ignore orientation and only focus on the position of an off-axis point on the robot. The trick is then to chose a part of the control so that it cancels the complex nonlinear dynamics. Following [LYB03] and looking back at Figure 5 we write the position of x as

x1 = z1 + L cos θ x2 = z2 + L sin θ.

18

Introduction

Differentiating this gives ˙ sin θ x˙ 1 = z˙1 − θL = v cos θ − ωL sin θ x˙ = v sin θ + ωL cos θ    2  cos θ − sin θ v x˙ 1 = . x˙ 2 sin θ cos θ Lω Letting

 R(θ) =

cos θ sin θ

− sin θ cos θ

(3.7)



be the rotation matrix, which is invertible for all θ, R(θ)−1 = R(−θ), we choose     v u1 = R(θ)−1 . Lω u2 

This makes

x˙ 1 x˙ 2



 = R(θ)

v Lω

 =

  u1 , u2

i.e., the same as (3.1). Differentiating (3.7) once more we get 

x ¨1 x ¨2





   v v˙ + R(θ) , Lω Lω˙     v F/m
+ R(θ) , = R (θ)ω Lω Lτ /J   F/m − Lω 2 . = R(θ) Lτ /J + vω

˙ = R(θ)

Therefore, choosing 

F/m Lτ /J

gives



x ¨1 x ¨2



 =



   Lω 2 −1 u1 + R(θ) , −vω u2

 = R(θ)

(3.8)

F/m − Lω 2 Lτ /J + vω



 =

 u1 , u2

(3.9)

which is the same as (3.2) with the mass incorporated in the control signal.

3. Background Material on Robotics and Control

19

The reason that this could be done is that the chosen output gives a constant relative degree of 2, [Sas99], (hence the name double integrator). For completeness we state the complete coordinate change ψ giving the new variables below     z1 + L cos θ x1  z2 + L sin θ  x2      x˙ 1  = ψ(z1 , z2 , θ, v, ω) = v cos θ − Lω sin θ .     v sin θ + Lω cos θ x˙ 2  θ θ This coordinate change gives the stable internal dynamics, 1 1 θ˙ = − x˙ 1 sin(θ) + x˙ 2 cos(θ). 2L 2L The interpretation is that θ is constant when the new outputs are identically zero, i.e., (x1 , x2 , x˙ 1 , x˙ 2 ) = 0 gives θ˙ = 0. An important issue not to be forgotten is how the actuator bounds transfer when applying feedback linearization. Assuming that each independently actuated wheel applies a force F1 , F2 against the ground we get the force and torque as      1 1 F1 F 2 2 , = F2 J − 2l 2l where l is the length of the wheel axis. The feedback linearizing controls are (from above)     u1 F/m − Lω 2 , = R(θ) u2 Lτ /J + vω which rewritten as a function of F1 , F2 gives     1    1 u1 F1 −Lω 2 2m 2m . = R(θ) + 2L u2 F2 vω − 2L lJ lJ Now assuming Fi ∈ [−b, b] we get     b b , −Lω 2 + m ] u1 [−Lω 2 − m R(−θ) ∈ , 4Lb u2 [vω − 4Lb lJ , vω + lJ ] i.e., a rectangle centered at (−Lω 2 , vω) in vehicle fixed coordinates. This concludes our exposition of background control theory material. Below we go on to look at some higher level system integration issues, concerning AI and robot control paradigms.

20 3.5.

Introduction

Reacting vs. Planning

Everybody’s got plans... until they get hit. - Mike Tyson Everything should be made as simple as possible, but not simpler. - Albert Einstein There seems to be a movement away from mathematical models in general and world models in particular in parts of the robotics community. In this section we will try to investigate the rationale behind this trend, and contrast it to the strong belief in world models and theory held in the control community. Understanding the two sides is important not only because it allows us to appreciate the different approaches to e.g. obstacle avoidance found in the literature. But also since the gap between control and AI will be closing, when more complex high level problems are addressed in the future, as discussed in Section 1. The new paradigm is called either Reactive, Behavoir-based, or Nouvelle AI and the old one is denoted Planning, Deliberative, Hierarchical or Classical AI. The approaches are contrasted in Table 1.1 and discussed in detail below. Table 1.1: The two robot control approaches, [Ark98, p. 20]. Planning World model dependent Slower response High level AI Variable latency

Reactive World model free Real-time response Low level AI Simple computation

In a paper by MIT AI lab scientist Rodney Brooks it is claimed that “the symbol system hypothesis upon which classical AI is based is fundamentally flawed,...” [Bro90]. The hypothesis mentioned states that “intelligence operates on a system of symbols.” [Bro90]. The idea is that perception and motor interfaces are a set of symbols and that intelligent reasoning about them is independent of their actual meaning. It is assumed that once such reasoning is done, by a machine or a human, it is easy to relate or connect the symbols back to the real world, referred to as grounding the symbols. Brooks argues that such grounding is instead an essential part of the intelligence problem and that the division into reasoning and grounding is highly counterproductive: “The effect of

3. Background Material on Robotics and Control

21

the symbol system hypothesis has been to encourage vision researchers to quest after the goal of a general purpose vision system which delivers complete descriptions of the world in symbolic form. Only recently has there been a movement towards active vision, which is much more task dependent, or task driven.” The alternative Brooks advocates is called Nouvelle AI, based on the physical grounding hypothesis, “ This hypothesis states that to build a system that is intelligent it is necessary to have its representations grounded in the physical world ... the key observation is that the world is its own best model. It is always exactly up to date. It always contains every detail there is to be known. The trick is to sense it appropriately and often enough.” Having made this statement Brooks presents the subsumption architecture, implementing the ideas of the physical grounding hypothesis. An approach to obstacle avoidance building on these ideas is presented in Section 4.1. Brooks’ strong faith in the new approach is displayed in the following statement: “Like the advocates of the symbol system hypothesis, we believe that in principle we have uncovered the fundamental foundation of intelligence.” In a recent book by Georgia Tech researcher R. Arkin [Ark98] these ideas are expanded upon and the new reactive approaches are described in detail. Arkin summarizes the idea as follows. “Simply put, reactive control is a technique for tightly coupling perception and action, typically in the context of motor behaviors, to produce timely robotic response in dynamic and unstructured worlds.” He goes on to describe the fundamentals of the proposed control architecture as, [Ark98, p. 26]: 1. An individual Behavior: a stimulus/response pair for a given environmental setting that is modulated by attention and determined by intention. 2. Attention: prioritizes tasks and focuses sensory resources and is determined by the current environmental context. 3. Intention: determines which set of behaviors should be active based on the robotic agent’s internal goals and objectives. 4. Emergent Behavior: the global behavior of the robot or organism as a consequence of the interaction of the active individual behaviors. A typical reactive architecture is seen in Figure 8. The behaviors work in paralell, each describing its own complete sensor-to-actuator mapping. This fact satisfies the physical grounding hypothesis of Brooks mentioned

22

Introduction

Avoid Obstacle

Move−to Goal SENSING

Arbitration

ACTIONS

Noise

More Behaviors

Figure 8: A Typical reactive architecture, from [Ark98, p. 112-114]. above, and also makes it very easy to add or remove behavoirs. Attention governs sensor resources, e.g. where a camera is pointing. Intention deals with the setup of the arbitration, see below. Note the absence of a common world model, sensory data is fed directly to the behavoir modules for computation. The desired actuator commands of the behaviors are then merged by an arbitrator. There are mainly four suggested ways of arbitration: 1. Suppression: A strict hierarchy of behaviors is defined, where the highest active one makes the call. For example, the avoid-obstacle can override the go-to-goal behavior if there is an obstacle close by. This is the arbitration mechanism suggested by Brooks in the subsumption architecture mentioned above. 2. Selection: A variable hierarchy where both the agent’s goals (intention) and sensory information governs what behavior gets control. 3. Voting: All behavoirs are allowed to vote (intention governs the number of votes) on their preferred action. The choice with the most votes is then executed. 4. Vector summation: All behavoir outputs are in the form of a vector, e.g. either desired velocity or acceleration. These vectors are then summed, and in some implementations normalized, to get the

3. Background Material on Robotics and Control

23

desired motor command. This approach is used in the example of Section 4.1. The reactive architecture is to be compared with the previously dominant planning or deliberative methods with prominent features including, [Ark98, p. 21]: 1. Hierarchical in structure, similar to military command. 2. Control and Communication flow up and down (not on same level) in fixed pattern. 3. Subgoals provided to lower (subordinate) levels from higher ones. 4. Heavy reliance on symbolic world model. A typical hierarchical control structure is seen in Figure 9. Strategic Global Planning

Tactical Intermediate Planning WORLD MODEL Short−Term Local Planning

Actuator Control

ACTIONS

SENSING

Figure 9: A Hierarchical Planner, from [Ark98, p. 210]. Brooks states that the advantage of nouvelle AI can be seen from evaluating robot performance [Bro90]: “... as is shown by the relatively weaker performance of symbol based mobile robots as opposed to physically grounded robots.” The conclusion is however not so clear judging from the outcomes of robotics and AI competitions such as the Robo Cup, a robotic soccer tournament held every year, [KANM98]. In

24

Introduction

˚B03, p. 53], the leader of the Cornell University Robo Cup team Raf[MA faello D’Andrea describes a classical hierarchical planning design consistent with the descriptions of Arkin: “...the system was decomposed into estimation and prediction, real time trajectory generation and control, and high level strategy.” The Cornell team seemed to outperform their opponents as reported in [M˚ AB03, p. 52]: “Cornell was the winner of the F180 League in both 1999, the first year it entered the competition, and 2000. The team’s success can be directly attributed to the adoption of a systems engineering approach to the problem, and by emphasizing system dynamics and control.” The core of the disagreement seems to be the use of world models [Ark98, p. 211]: “A clear cut distinction can be seen in the hierarchical planner’s heavy reliance on world models (either a priori or dynamically acquired) as compared to the avoidance in most reactive behavior-based system of world representations entirely.” It is noted that in hierarchical planners “perception is the establishment and maintenance of correspondence between the internal world model and the external real world. Consequently action results from reasoning over the world model. Perception thus is not tied directly to action. ”[Ark98, p. 22] But a world model is crucial in modeling the interactions of the robot with its environment. Since a model is the starting point for all application of mathematical analysis, nouvelle AI is left with heuristics and experimental trial and error as its only design tools. This stands in strong contrast to the beliefs held in the control community: “A core strength of control has been its respect for and effective use of theory, as well as contributions to mathematics driven by control problems. Rigor is a trademark of the community and one that has been key to many of its successes. ”[M˚ AB03, p. 92] The hierarchical way of using perception to update a world model and using the world model for determining action described above is precisely how the observer and Kalman filter designs found in a multitude of control applications work. It thus seems the control community is using the planning approach. Arkin however notes that the assumptions underlying nouvelle AI is quite strong, [Ark98]: 1. The environment lacks temporal consistency and stability. 2. The robot’s immediate sensing is adequate for the task at hand. 3. It is difficult to localize a robot relative to a world model. 4. Symbolic representational world knowledge is of little or no value.

25

4. Examples of Current Research

He suggests that the best solution might be a combination of the two: “The interface between deliberation and reactivity is poorly understood and serves as the focus of research...” Perhaps the denunciation of world models in the reactive approaches should be seen more as a reaction against classical AI than against world models as used in the control community. In either case it is likely that future research will exhibit more approaches with elements from both of the two schools.

4.

Examples of Current Research

In this section we will look at examples of current research in the subfields of obstacle avoidance and formation control. These examples are picked to illustrate the reactive and planning approaches described in the previous section. An overview of the categories, papers of this thesis and picked examples is shown in Table 1.2. Table 1.2: Overview of examples and thesis papers. Reactive Obstacle Avoidance (both) Formation keeping

[BA98]

(both)

Planning

PaperA

[RK92]

PaperB [DOK98] PaperD [OSM02]

PaperC [YBK01]

In Obstacle Avoidance the division between the Reactive and Planning approaches is very clear; it is, however, harder in Formation keeping. The division in Table 1.2 is done in such a way that the more computationally heavy and centralized schemes are put in the planning category. The more local and distributed, but still world model dependent, methods are put in the middle category; leaving the purely reactive ones in the left-most column. We now turn to look at the examples and their relation to the papers in some detail. 4.1.

Navigation and Obstacle Avoidance

The problem of navigation and obstacle avoidance deals with making a robot move from one position to another as efficiently as possible, while not bumping into things on the way. As can be seen in [Lat91], the problem is well-studied, but the need for further improvements still remains

26

Introduction

˚B03, p. 54]: “Finally we note the need to develop robots that can op[MA erate in highly unstructured environments. This will require considerable advances in visual processing and understanding, complex reasoning and learning, and dynamic motion planning and control.” We will give two quite different examples from the literature on the subject. One is from the behavior-based tradition of Brooks with a focus on short computation times and few world assumptions, the other one relies on advanced mathematics in combination with precise knowledge of the obstacles positions to prove safety and attainment of the goal position. 4.1.1.

Reactive Obstacle Avoidance

The approach presented in “Behavior-based formation control for multi robot teams” [BA98] is a formation extension of classical reactive obstacle avoidance. Therefore, the material of this paper serves as an example of both single and multi vehicle reactive obstacle avoidance. For the single vehicle case, three behaviors are used: • avoid-static-obstacle • move-to-goal • noise Each of these output a desired velocity vector vi ∈ R2 and the final input to the robot is a normalized sum, u=

Σi vi . ||Σi vi ||

Since the robot input is desired velocities the implicit robot model is the single integrator, (3.1). The details of the behavior computations follow. 1. avoid-static-obstacle: A behavior used for collision avoidance. It repels the robot from static obstacles. The sphere of influence and gain are tuning parameters. The magnitude is calculated as follows (depicted in Figure 10),   ∞, G S−d , ||vi || =  S−R 0,

if d < R if R ≤ d ≤ S if S < d,

where d is distance from robot to center of obstacle, R is radius of obstacle, S is adjustable sphere of influence and G is adjustable

27

4. Examples of Current Research

||v i||

inf G

d R

S

Figure 10: The magnitude of avoid-static-obstacle as a function of distance, d. gain. Finally the direction of vi is away from the center of the obstacle. 2. move-to-goal: A vector of adjustable magnitude (gain) pointing towards the goal location. 3. noise: A vector of adjustable magnitude that changes randomly at adjustable time intervals. Claimed to “serve as a form of reactive ‘grease’, dealing with some of the problems endemic to purely reactive navigational methods such as local maxima, minima, and cyclic behavoir”, [BA98, p. 929]. Using just the avoid-static-obstacle and move-to-goal we get the vector field depicted in Figure 11. As can be seen, all initial positions except {(x, y) : x ≤ −3, y = 0} end up in the goal position. The exceptions end up on the x-axis close to (−5.5, 0) where the two behaviors cancel each other. This is an unstable equilibrium, i.e., any small perturbation away from the line will allow the robot to escape towards the goal. This is exactly the purpose of the noise behavior. If the obstacle was e.g. ∪-shaped however, the equilibrium would have been stable and adding noise would have accomplished nothing. So although impressive results can be achieved with this simple structure, the basic problems of non-convex obstacles, such as walls in any reasonable indoor environment, remains. Arkin, [Ark98], report tests with so-called avoid-past behaviors, but they are far from acceptable solutions. Therefore his suggestions about combining reactivity and planning [Ark98, p. 234], seems to be the inevitable choice.

28

Introduction

5 4 3 2 1 0 −1 −2 −3 −4 −5

−6

−4

−2

0

2

4

6

Figure 11: The resulting vector field from an obstacle on the left and the goal point on the right. The parameters are set to S = 4, G = 2, R = 1 for avoid-static-obstacle and G = 1 for move-to-goal 4.1.2.

Verifiable Obstacle Avoidance

An example of a more mathematical approach towards the problem of obstacle avoidance can be found in [Rim91],[RK92]. In those papers Rimon and Koditschek propose the construction of a special potential function, a navigation function, whose most important property is the absence of undesired local minima. The construction is quite technical, using diffeomophisms to map between different obstacle shapes. They make the following “naive” assumptions: 1. The obstacles are fixed. 2. There is given perfect information concerning the geometry of the robot and the obstacles. 3. The robot has ideal bounded torque actuators and perfect sensors for each degree of freedom. Under these assumptions they formally define the Navigation Problem: The robot, presented with a fixed destination must approach it and halt there, while avoiding the obstacles. To solve the problem above they

4. Examples of Current Research

29

propose the control τ (p, p) ˙ = −∇φ(p) + d(p, p), ˙ where τ is the control input (force), p is the robot configuration e.g. position, φ(p) is the navigation function and d(p, p) ˙ is an arbitrary dissi˙ ≤ 0. The construction of φ renders the pative vector field, i.e. p˙T d(p, p) control bounded and furthermore guarantees the absence of collisions. A general robot model is used, which in the planar two dimensional case corresponds to model (3.2). It has been shown by Koditschek “that the closed loop robot system ‘inherits’ the qualitative behavior of φ’s gradient trajectories, (i.e. solutions to p˙ = −∇φ(p))”,[Rim91]. Note how the assumptions above are almost the opposite of the reactive robot control assumptions in Section 3.5. This fact again underlines the difference in starting point of the two approaches. The work of Brock and Khatib in [BK99] is an attempt to combine the merits of a reactive approach with the global properties of a navigation function. PaperA continues this line of thought, but in a more formal framework allowing proofs of goal attainment and safety while preserving the reactive qualities of [BK99]. 4.2.

Formations and Multi-Agent Robotics

In this sections we will review examples of the main formation keeping approaches and see how they relate to paperB, paperC, and paperD. According to Beard, [LYB03], three main frameworks have emerged to address the multi-agent problems. These are the Behavior-based, Virtual Structure and the Leader Following or Graph Approaches. The behavior-based (reactive) framework is highly decentralized and all robots are interchangeable. Typically a “keep-formation” behavior is added to “go-to-goal” and “avoid-obstacles” and the outputs of the behaviors are merged in some fashion. The drawback is the difficulty of analyzing such approaches mathematically and get a clear picture of when and why they might fail. Leader following and graph approaches are also decentralized but the robots can tell each other apart and typically tries to keep some desired distance to some (decided by the operator) of their neighbors. These schemes typically have one leader who takes care of the mission, and this robot constitutes a weakness, in terms of being a single point of failure. To address this problems you can imagine designing a method to reassign the leadership and information flow in case of a failure.

30

Introduction

Virtual structure approaches describe the whole formation as one single rigid body. As this body moves around it traces out trajectories for all individual robots to track. The problem here is that the motion of the rigid body needs to be calculated centrally and broadcast to all robots regularly. Thus it is the least decentralized approach of the three. Below we give examples of all three categories. 4.2.1.

Behavior Based Formations

For an example of a standard behavior based approach we look at [BA98]. The presented approach consists of five behaviors: maintain-formation, avoid-static-obstacle, avoid-robot, move-to-goal, and noise. The basic structure of summation and normalization is reported above in Section 4.1.1. The behaviors are 1. avoid-static-obstacle: (see above) 2. avoid-robot: A special case of avoid-static-obstacle. The same formulas are used but with different parameters. The moving robots position are used as obstacle center. 3. move-to-goal: (see above) 4. noise: (see above) 5. maintain-formation: A vector pointing towards a desired robot position which have been calculated using information about other robots positions. This position is a fixed distance from either a designated leader, a designated neighbor, or a “unit-center” defined as the average position of all robots. The magnitude is depicted in Figure 12 and is calculated according to   0, G d−R , ||vi || =  S−R G,

if d < R if R ≤ d ≤ S if S < d,

where d is the distance from robot to desired position, R is adjustable radius of “Dead zone” (no motion), S is adjustable radius of “Controlled zone” (linear growing desired velocity) and G is adjustable gain.

31

4. Examples of Current Research

||v i||

G

d R

S

Figure 12: The magnitude of maintain-formation as a function of distance. The reported results show that the method seems to perform well in simulations and experiments. The performance is further evaluated numerically over a set of simulation runs and it is concluded that the “unitcenter” approach is better than the “leader” one. It is, however, clear that the addition of noise will only remedy stationary points of saddle type and not significant local minima caused by e.g. non-convex obstacles, of which there are none in the paper. The formation keeping scheme presented in paperD is an unusually mathematically stringent example of behavior based formation keeping. It uses one single behavior generating a force on the vehicle from each neighbor within a certain radius; the force pulls or pushes, depending on the distance, the vehicle towards keeping some defined ideal inter-robot distance. The model used is the double integrator (3.2). For details we refer to paperD but note that the absence of a complex arbitration mechanism is what makes the approach theoretically verifiable. The addition of virtual vehicles controlled by error feedback makes paperD something of a behavior-based/virtual-structure hybrid. 4.2.2.

Leader following

A classical leader follower approach can be found in [DOK98]. There the kinematic unicycle model is used and the position of an off axis point is controlled, in a way closely related to the feedback linearization described in Section 3.4. The information flow in the formation is modeled as a directed graph where each robot (node) maintains a prescribed distances and/or orientation (of edges) to a subset of the others. It is noted how one graph topology can correspond to several different formations by varying

32

Introduction

the edge distances. Obstacle avoidance is provided by controlling the leader and then interchanging some formation constraints with obstacle distance constraints. A more recent graph approach is suggested in [OSM02]. There it is shown how to stabilize a formation of bounded control double integrators, i.e., model (3.2). The information flow is symmetric, i.e., if A can sense/see B then B can sense/see A. The fact that the applied control forces are also symmetric enables the construction of an artificial potential function with a unique global minimum. Local collision free properties are also shown and it is discussed under what circumstances a graph with given edge distances uniquely determines a formation. PaperB assumes the use of a control strategy taken from a general leader-follower class. It is then investigated how bounds on leader follower distances can be calculated and how these can be used when performing formation obstacle avoidance. The scheme can be seen as a generalization of the configuration space obstacle concept. 4.2.3.

Virtual Structure

This approach to formation keeping is the most centralized. The whole formation is seen as a virtual body translating and rotating in the plane. Each vehicle has its own (possibly changing) relative position in the body and this position traces out a trajectory for each robot to track. In the paper [YBK01] this approach is used to achieve a stable formation using double integrator robot models (3.2). The proposed scheme furthermore includes “group feedback”, i.e., if one robot slows down the others wait. The central idea is to use a “coordination variable” ξ = (xc , yc , θc , xd , yd ), where xc , yc , θc is the center and orientation of the virtual body and xd , yd the relative positions of each robot. The goal configuration is denoted ξ d and then the virtual body is moved according to ξ˙i =

1 Ktanh(ξi − ξid ), KF φ(q) + 1

where q is the robot positions and φ(q) ≥ 0, is some error term, e.g. φ(q) = Σ||qi − qid ||. The robot control is then chosen as a feed forward PD control q¨i = ui = q¨id − kpi (zi − zid ) − kdi (z˙i − z˙id ). The problem formulation and ideas of [YBK01] is similar to those of paperC. The coordination variable is used in a similar way as the s

33

5. Summary of the Appended Papers

G Supervisor

y

z

F Formation Control

z1

zN Coordination Variable ξ

u1

K1

KN

Local Control

Local Control

y1

uN

R1

RN

Robot #1

Robot #N

yN

Figure 13: Proposed Architecture variable to achieve “group feedback” in both cases. In PaperC however, the feedback is used in such a way that not only formation convergence (stability) to the final configuration but also a bounded error during the transfer is guaranteed. The slowdown, i.e., waiting, is limited in terms of an upper bound on completion time. These advantages is due to strong but general Lyapunov assumptions instead of focusing on one particular robot model. The Lyapunov assumptions are valid for the double integrator as well as locally for all asymptotically stabilizable systems. For an alternative method of coordination along preplanned trajectories see [KXS00]. PaperD describes an extension of paperC and applies this to a decentralized verifiable behavior-based control structure as described above. It furthermore investigates the application of climbing e.g. temperature gradients by using measurements at each vehicle. The problem of evaluating and choosing formation geometry for this application is studied and an optimization approach is proposed.

5.

Summary of the Appended Papers

The four papers constituting the thesis are A: A Convergent Dynamic Window Approach to Obstacle Avoidance coauthored with Naomi E. Leonard. The dynamic window approach (DWA) is a well known navigation

34

Introduction

scheme developed by Fox et. al. [FBT97] and extended by Brock and Khatib [BK99]. It is safe by construction and has been shown to perform very efficiently in experimental setups. However, one can construct examples where the proposed scheme fails to attain the goal configuration. What has been lacking is a theoretical treatment of the algorithm’s convergence properties. Here we present such a treatment by merging the ideas of the DWA with the convergent but less performance oriented scheme suggested by Rimon and Koditschek [RK92]. Viewing the DWA as a Model Predictive Control (MPC) method and using the Control Lyapunov Function (CLF) framework of [RK92] we draw inspiration from a MPC/CLF framework put forth by Primbs [PND99] to propose a version of the DWA that is tractable and convergent. Paper A corresponds to the following publications: ¨ A1: P. Ogren and N. Leonard: A Provable Convergent Dynamic Window Approach to Obstacle Avoidance, IFAC World Congress , Barcelona, Spain, July 2002. ¨ A2: P. Ogren and N. Leonard: A Tractable Convergent Dynamic Window Approach to Obstacle Avoidance, IEEE/RS International Conference on Intelligent Robots and Systems. Lausanne, Switzerland, October 2002. ¨ A3: P. Ogren and N. Leonard: A Convergent Dynamic Window Approach to Obstacle Avoidance, Submitted to IEEE Transactions on Robotics and Automation. B: Obstacle Avoidance in Formation coauthored with Naomi E. Leonard. In this paper, we present an approach to obstacle avoidance for a group of unmanned vehicles moving in formation. The goal of the group is to move through a partially unknown environment with obstacles and reach a destination while maintaining the formation. We address this problem for a class of dynamic unicycle robots. Using Input-to-State Stability we combine a general class of formation-keeping control schemes with a new dynamic window approach to obstacle avoidance in order to guarantee safety and stability of the formation as well as convergence to the goal position. An important part of the proposed approach can be seen as a formation extension of the configuration space obstacles concept. We illustrate the method with a challenging example.

5. Summary of the Appended Papers

35

Paper B corresponds to the following publications: ¨ B1: P. Ogren and N. Leonard: Obstacle Avoidance in Formation, IEEE International Conference on Robotics and Automation, Taiwan, May 2003. ¨ B2: P. Ogren and N. Leonard: Obstacle Avoidance in Formation using a new Convergent Dynamic Window Approach, Reglerm¨ ote 2002, Link¨ oping, May 2003. C: A Control Lyapunov Function Approach to Multi-Agent Coordination coauthored with Magnus Egerstedt and Xiaoming Hu. In this paper, the problem of multi-agent coordination along parameterized trajectories is studied. This problem is addressed for a class of robots for which control Lyapunov functions can be found. The main result is a suite of theorems about formation maintenance, task completion time, and formation velocity. The approach is also useful for single-Lyapunov function systems, where an upper bound on the set-point tracking error is to be maintained. An example is provided that illustrates the soundness of the method. Paper C corresponds to the following publications: ¨ C1: P. Ogren, M. Egerstedt and X. Hu: A Control Lyapunov Function Approach to Multi-Agent Coordination, IEEE Transactions on Robotics and Automation, October 2002, vol 18, pages 847-851. ¨ C2: P. Ogren, M. Egerstedt and X. Hu: A Control Lyapunov Function Approach to Multi-Agent Coordination. Proceedings of the IEEE Conference on Decision and Control, Orlando, Florida, December 2001. D: Cooperative Control of Mobile Sensor Networks: Adaptive Gradient Climbing in a Distributed Environment coauthored with Eddie Fiorelli and Naomi E. Leonard. We present a stable control strategy for groups of vehicles to move and re-configure cooperatively in response to a sensed, distributed environment. Each vehicle in the group serves as a mobile sensor and the vehicle network as a mobile and re-configurable sensor array. Our control strategy decouples, in part, the cooperative management of the network formation, the network maneuvers, and

36

Introduction

the network estimation of the environment. The underlying coordination framework uses virtual bodies and artificial potentials. We design the virtual body dynamics for two complementary purposes: we specify the direction of its motion to satisfy the network’s environment-driven mission, and we regulate its speed to ensure network stability and convergence properties. We focus on gradient climbing missions in which the mobile sensor network seeks out local maxima or minima in the environmental field. The network can adapt its configuration in response to the sensed environment in order to optimize its gradient climb. Paper D corresponds to the following publications: ¨ D1: P. Ogren, E. Fiorelli and N. Leonard: Formations with a Mission: Stable Coordination of Vehicle Group Maneuvers. 15th Int. Symposium on Mathematical Theory of Networks and Systems, South Bend, Indiana, Aug. 2002. ¨ D2: P. Ogren, E. Fiorelli and N. Leonard: Cooperative Control of Mobile Sensor Networks: Adaptive Gradient Climbing in a Distributed Environment To be submitted to the IEEE Transactions on Automatic Control. 5.1.

Division of work between authors

In paperA, paperB, and paperC the basic ideas and all definitions, lemmas, theorems and proofs were suggested by the first author. The coauthors took the role of advisors, pointing out unclear parts of arguments, helping out with introduction and references, and suggesting remedies to awkward ways of presentation. In paperD, however, sections 2 and 4.2 are completely due to the coauthours Eddie Fiorelli and Naomi Leonard.

References [Ark98]

R.C. Arkin. Behavior-Based Robotics. MIT Press, Cambridge, MA, 1998.

[BA98]

T. Balch and R. Arkin. Behavior-based formation control for multirobot teams. IEEE Transactions on Robotics and Automation, pages 926–939, Dec 1998.

[Bis01]

Richard Bishop. Whatever happened to automated highway systems (ahs)? Traffic Technology International, August 2001.

References

[BK99]

37 O. Brock and O. Khatib. High-speed navigation using the global dynamic window approach. In IEEE International Conference on Robotics and Automation, 1999.

[BMGA02] W. Beard, T. McLain, M. Goodrich, and E. Anderson. Coordinated target assignment and intercept for unmanned air vehicles. IEEE Transactions on Robotics and Automation, pages 911–922, December 2002. [Bro83]

R. Brockett. Differential Geometric Control Theory, chapter Asymptotic stability and feedback stabilization, pages 181–191. Birkhauser, 1983.

[Bro90]

Rodney Brooks. Elephants don’t play chess. Robotics and Autonomous Systems (6), pages 3–15, 1990.

[CdWSB96] C. Canudas de Wit, B. Siciliano, and G. Bastin. Theory of Robot Control. Springer-Verlag, New York, 1996. [CMKB02]

J. Cortes, S. Martinez, T. Karatas, and F. Bullo. Coverage control for mobile sensing networks. Submitted to IEEE Transactions on Robotics and Automation, 2002.

[DOK98]

J. Desai, J. Ostrowski, and V. Kumar. Control of formations for multiple robots. In Proc. IEEE International Conference on Robotics and Automation, May 1998.

[DS02]

M.B. Dias and A. Stentz. Opportunistic optimization for marketbased multirobot control. IEEE International Conference on Intelligent Robots and Systems, September 2002.

[EH01]

M. Egerstedt and Xiaoming Hu. Formation constrained multiagent control. IEEE Transactions on Robotics and Automation, pages 947–951, 2001.

[FBT97]

D. Fox, W. Burgard, and S. Thrun. The dynamic window approach to collision avoidance. IEEE Robotics and Automation Magazine, pages 23–33, March 1997.

[FCH+ 00]

Fischoff, Cardenas, Hernandez, Wyatt, Young, and Gordon. Popular movie quotes: Reflections of a people and a culture. Annual Convention of the American Psychological Association, Washington, D.C., August 2000.

[Isi96]

Alberto Isidori. Nonlinear Control Systems. Springer Verlag, 1996.

[Jen01]

Patric Jensfelt. Approaches to Mobile Robot Localizatoin in Indoor Environments. PhD thesis, KTH, Sweden, ISBN 91-7283-135-9, 2001.

[KANM98] H. Kitano, M. Asada, I. Noda, and H. Matsubara. Robocup: robot world cup. IEEE Robotics and Automation Magazine, pages 30– 36, Sep 1998.