As the price-performance ratio of embedded processors increases and the size of microchips de- ... 2.3.3 Derivation of Predictor Corrector methods . ..... As the contact state changes from free motion to sliding or rolling, the functional form.
SIMULATION AND CONTROL OF HYBRID SYSTEMS WITH APPLICATIONS TO MOBILE ROBOTICS Joel M. Esposito A DISSERTATION in Mechanical Engineering and Applied Mechanics
Presented to the Faculties of the University of Pennsylvania in Partial Fulfillment of the Requirements for the Degree of Doctor of Philosophy.
2002
Professor Vijay Kumar Supervisor of Dissertation
Professor G.K. Ananthasuresh Graduate Group Chairperson
This thesis is dedicated: to the memory of my parents, John and Mary, who started a young boy down this path but never got to see him reach the destination; to all my friends and family, everywhere, who ensured I never traveled alone; and to my wife, Meredith, without whom I would be lost, for making it all worthwhile.
ii
ABSTRACT As the price-performance ratio of embedded processors increases and the size of microchips decreases, more sophisticated paradigms for software based control have evolved. This trend is particularly evident in mobile robotics because these systems typically have significant onboard computing power, complex mission requirements and the need to process data coming from many types of sensors in real time. Such systems are modeled as Hierarchical Hybrid Systems (HHS). In the first part of the dissertation three novel numerical techniques for simulating HHS are introduced. The first of these addresses the problem of detecting and locating the occurrence of discrete transitions (i.e., control mode or model switches) during the course of a simulation, especially switches which occur near a model singularity - an area in which all competing event detections methods fail. The remaining two simulation techniques seek to circumvent one of the major causes of computational bottle necks in large-scale simulations - the requirement that all differential equations in the system be simulated using a single global choice of step size. In one case differential equations with right hand sides that are partially coupled, without consideration of the mode switches, are considered. The third algorithm is concerned with simulating so-called multi-agent hybrid systems. These systems have decoupled sets of differential equations yet the inequalities which signal mode switches are coupled. Here the challenge is properly detecting transitions when the numerical integration of the various differential equations is performed asynchronously using many local time clocks. In the second part of the dissertation a framework for reactive motion planning and control of mobile robots is presented. It is our approach that reactive control and motion planning schemes for mobile robots are best modeled as Hybrid Systems. We assume a suite of low level controllers have been designed. The problem is how to compose these controllers, either through designing a switching sequence or synthesizing entirely new control modes, in such a way that the aggregate behavior of the system achieves all of the objectives simultaneously, or reports failure if it cannot. The numerical algorithm represents a rigorous approach to modular control law composition.
iii
Contents
1
Introduction
1
1.1
Simulating hierarchical hybrid systems . . . . . . . . . . . . . . . . . . . . . . .
7
1.1.1
Discrete event detection . . . . . . . . . . . . . . . . . . . . . . . . . .
7
1.1.2
Multi-rate simulation . . . . . . . . . . . . . . . . . . . . . . . . . . . .
10
1.1.3
Multi-agent systems . . . . . . . . . . . . . . . . . . . . . . . . . . . .
12
1.2
Reactive control design and robot motion planning . . . . . . . . . . . . . . . .
14
1.3
Approach . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
15
1.3.1
Simulation Techniques . . . . . . . . . . . . . . . . . . . . . . . . . . .
15
1.3.2
Reactive control and robot motion planning . . . . . . . . . . . . . . . .
17
Organization of the dissertation . . . . . . . . . . . . . . . . . . . . . . . . . . .
18
1.4 2
Background
22
2.1
Hybrid Systems . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
23
2.2
Simulating Hybrid Systems . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
26
2.3
Numerical Integration Methods . . . . . . . . . . . . . . . . . . . . . . . . . . .
27
2.3.1
Notation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
28
2.3.2
Definitions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
29
2.3.3
Derivation of Predictor Corrector methods . . . . . . . . . . . . . . . . .
29
Robot motion planning review . . . . . . . . . . . . . . . . . . . . . . . . . . .
34
2.4
iv
3
Notation and basic problem . . . . . . . . . . . . . . . . . . . . . . . .
34
2.4.2
Definitions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
35
Related Work
40
3.1
Review of Hybrid System Simulation . . . . . . . . . . . . . . . . . . . . . . .
40
3.1.1
Event detection . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
40
3.1.2
Multirate integration methods . . . . . . . . . . . . . . . . . . . . . . .
44
3.1.3
Multi-agent system simulation . . . . . . . . . . . . . . . . . . . . . . .
45
3.1.4
A control systems approach to simulation . . . . . . . . . . . . . . . . .
48
Literature review for robot motion planning . . . . . . . . . . . . . . . . . . . .
49
3.2.1
Explicit approaches . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
49
3.2.2
Reactive approaches . . . . . . . . . . . . . . . . . . . . . . . . . . . .
50
Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
56
3.2
3.3 4
2.4.1
Event Detection
58
4.1
Problem Statement . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
58
4.2
Events near model singularities . . . . . . . . . . . . . . . . . . . . . . . . . . .
60
4.3
A Control Theoretic Approach . . . . . . . . . . . . . . . . . . . . . . . . . . .
61
4.4
Step size selection technique for general guards . . . . . . . . . . . . . . . . . .
63
4.5
Step selection for special classes of guard functions . . . . . . . . . . . . . . . .
64
4.5.1
Linear guards . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
64
4.5.2
Polynomial guards . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
65
4.5.3
Time dependent guards . . . . . . . . . . . . . . . . . . . . . . . . . . .
65
4.5.4
Approximate linearization for arbitrary nonlinear guards . . . . . . . . .
66
4.6
Computation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
66
4.7
Boolean combinations of inequalities . . . . . . . . . . . . . . . . . . . . . . . .
68
4.8
Computation of step sizes . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
68
v
4.9
5
6
Termination tolerance and selecting
. . . . . . . . . . . . . . . . . . . . . . .
69
4.10 The Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
74
4.11 Examples and experiments . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
76
4.12 Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
79
4.13 Computational Cost . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
80
4.14 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
83
Multi-agent Hybrid System Simulation
86
5.1
Problem Statement and Discussion . . . . . . . . . . . . . . . . . . . . . . . . .
86
5.2
Multi-agent event detection . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
88
5.3
The algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
92
5.4
Examples . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
95
5.5
Efficiency analysis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
96
5.5.1
The traditional algorithm . . . . . . . . . . . . . . . . . . . . . . . . . .
96
5.5.2
Average complexity analysis . . . . . . . . . . . . . . . . . . . . . . . .
98
5.6
Numerical experiments . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 103
5.7
Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 105
5.8
Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 106
Multi-rate Integration Techniques
115
6.1
Problem Statement and Discussion . . . . . . . . . . . . . . . . . . . . . . . . . 115
6.2
The algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 118
6.3
Analysis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 119 6.3.1
Integration error analysis . . . . . . . . . . . . . . . . . . . . . . . . . . 119
6.3.2
Stability . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 122
6.4
Efficiency . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 123
6.5
Implementation and experiments . . . . . . . . . . . . . . . . . . . . . . . . . . 126 vi
6.6 7
Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 127
A hybrid system framework for reactive motion planning
133
7.1
Problem statement . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 134
7.2
Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 135
7.3
Approach . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 136
7.4
Parallel control law composition . . . . . . . . . . . . . . . . . . . . . . . . . . 137
7.5
7.4.1
Stabilizing directions . . . . . . . . . . . . . . . . . . . . . . . . . . . . 138
7.4.2
Usable directions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 140
7.4.3
Feasible directions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 141
Control architecture . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 144 7.5.1
Deliberative Objective Mode . . . . . . . . . . . . . . . . . . . . . . . . 145
7.5.2
Superposition Mode . . . . . . . . . . . . . . . . . . . . . . . . . . . . 146
7.5.3
Parallel Mode . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 147
7.5.4
Sliding mode . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 149
7.6
Computational method . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 150
7.7
Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 151
7.8
Applications . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 152
7.9
Fully actuated systems . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 154
7.10 Underactuated systems . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 154 7.10.1 Direct approach . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 155 7.10.2 Indirect approach . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 156 7.11 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 158 8
Conclusion
160
8.1
Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 160
8.2
Contributions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 162 vii
8.3
Future work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 163
viii
List of Figures 1.1
Multirate technique with . . . . . . . . . . . . . . . . . . . . . . .
1.2
Examples of Multi-agent hybrid systems: Multi-robot teams, multi-body dynamics, automated highway systems. . . . . . . . . . . . . . . . . . . . . . . . . . .
1.3
11
12
More examples of multi-agent hybrid systems: Air Traffic Management, biological cellular networks. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
13
2.1
A graphical representation of a generic hybrid automata. . . . . . . . . . . . . .
24
2.2
A simple physical example of a hybrid system: the bouncing ball. . . . . . . . .
25
2.3
The basic hybrid system simulation algorithm. . . . . . . . . . . . . . . . . . . .
26
2.4
An example of the basic motion planning problem and one of its many solutions.
35
2.5
An example of a navigation function. The black lines represent level sets of the scalar function. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
3.1
37
Situations which can be problematic for many event detection algorithms. Multiple zero crossings arise from osculatory solution trajectories or “thin” guard sets (left and center). Almost all algorithms fail when the event occurs near a region where the right hand side of the differential equation is ill-defined (i.e. a model
3.2
singularity) (right). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
42
A taxonomy of motion planning techniques. . . . . . . . . . . . . . . . . . . . .
51
ix
4.1
The guard dynamics as a function of time (left). After performing Input/Output Linearization, the guard dynamics as a function of the step number have been reparametrized to behave like a stable linear system (right). . . . . . . . . . . . .
4.2
A graphical illustration of the
selection for unilateral events. The state is guar-
anteed not to cross the event surface. 4.3
A graphical illustration of the
63
. . . . . . . . . . . . . . . . . . . . . . .
71
selection in the bilateral case. The state is guar-
anteed to cross the event surface. . . . . . . . . . . . . . . . . . . . . . . . . . .
73
4.4
A graphical illustration of the
73
4.5
The overall simulation algorithm introduced in this chapter. The pseudo-code for
selection in the accuracy critical case. . . . . . .
the step size computation (depicted in the box to the far left entitled Compute New Based on Event) is given in Section 4.10. . . . . . . . . . . . . . . . . . . . . 4.6
The traditional simulation algorithm. The event dynamics are not used to select a the step size for the next iteration. . . . . . . . . . . . . . . . . . . . . . . . . .
4.7
75
Two examples: (1) an autonomous robot navigating a corridor; (2) a planar two link manipulator with workspace limitations. . . . . . . . . . . . . . . . . . . .
4.8
74
76
Simulations of the mobile robot in example 1: (a) standard simulation technique fails to detect the collision; (b) our method slows down as it approaches the event surface. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
4.9
77
Simulations of the two link manipulator from example 2: (a) root bracketing methods cannot be used since the vector field is ill-defined out side the workspace; (b) our method approaches the surface asymptotically without every requiring a function evaluation outside the workspace. . . . . . . . . . . . . . . . . . . . . . . .
5.1
78
The overall simulation algorithm introduced in this chapter. A more detailed description for the entire algorithm as well as the for the step size computation (de picted in the box to titled “Compute New via step-select” is given in Section 5.3. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 109 x
5.2
Two automated vehicles driving in spiral trajectories. . . . . . . . . . . . . . . . 110
5.3
The trajectory of the two robot vehicles in the plane is plotted (left). The separation distance, , is small enough that they collide. The value of the guard as a function of step number is shown (right). The guard value converges to zero exponentially. 110
5.4
Step sizes used in the first example are shown here(left). As described,
and
are selected independently away from the constraint but are brought into synchronization when an event is impending. The value of (right), which is a measure of how out of synchronization the two local time clocks are rapidly decreases in magnitude as the event is approached. . . . . . . . . . . . . . . . . . . . . . . . 110 5.5
The trajectory of the two robot vehicles in the plane for the second example (left).
is selected such that the two robots come very close without colliding. The
value of the guard as a function of the step number is shown(right). The constraint comes close to zero as the robots barely miss each other. . . . . . . . . . . . . . 111 5.6
Step sizes used in the second example are shown(left). Again,
and
are
selected to synchronize the simulation when it seems an event may occur. Once it is apparent that this is not the case, the simulation returns to asynchrony. The magnitude of (right), therefore decreases during this period but soon increases again. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 111 5.7
A histogram depicting the normalized frequency of selected step sizes for the Single-agent algorithm, for several values of
(the number of agents). As the
number of agents becomes large. . . . . . . . . . . . . . . . . . . . . . . . . . . 112 5.8
The expected value of , for the Traditional Algorithm as a function of the number
of agents. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 112 5.9
The -agent “bumper car” example. Events occur when an agent collides with one of the walls or another agent. . . . . . . . . . . . . . . . . . . . . . . . . . . 113 xi
5.10 Sample experimental results on the complexity of each algorithm as a function of the number of agents. Squares represent SA data and crosses are MA data. The data supports the claim that MA scales quadratically while SA scales exponentially. 113 5.11 Experimental results: computational cost as a function of the number of agents, for various integration tolerances. Dash lines represent analytical predictions; while the solid line is the geometric mean of the experimental data.. . . . . . . . . . . . 114 6.1
Multirate technique with . . . . . . . . . . . . . . . . . . . . . . . 117
6.2
Inferring the value of the state at off-mesh points represents a challenge for multirate methods. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 118
6.3
A flow chart of the traditional Predictor-Corrector numerical integration algorithm. 129
6.4
A flow chart of the multirate Predictor-Corrector numerical integration algorithm. 130
6.5
Efficiency gain as a function of the cost of evaluating the right hand side. . . . . . 131
6.6
Simulation results for the hierarchically controlled cart. The coordinates are plotted at each level. The multirate simulation algorithm requires only 39 % of the computation time used by the Matlab built in integration scheme ode45. . . 131
6.7
Illustration of the frequency with which the different levels in the hierarchy are integrated. The step size used for the first level is approximately 0.6, while the step size for the lowest level is 0.015. The step size is selected automatically to maintain the estimated errors at a desired level while seeking maximum efficiency. 132
6.8
Experimental results for the mobile robot example problem on the efficiency gain of the multirate technique as a function of the desired integration tolerance. . . . 132
7.1
An illustration of the vectors !#" , and $ % '&)(* $+ !,"-&)( in .0/ . Any input vector in the same half plane as also decreases "1324 . . . . . . . . . . . . . . . 139
7.2
In the absence of constraints, the set of feasible directions is the half plane containing !#" . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 142 xii
7.3
The addition of a constraint, 56 , with no time dependence further constrains the set of directions to the intersection of the half spaces containing !#"
7.4
and !756 . . 142
A graphical representation of the resulting Hybrid Automata. The connectivity of the modes can be understood from this diagram. . . . . . . . . . . . . . . . . . . 145
7.5
(L) The Robot (on the right) proceeds to the goal (on the left). (C) The robot tries to steer to the right around the moving convoy but is blocked by a static obstacle. Finally (R) the robot loops around to pass the convoy safely on the left. . . . . . 152
7.6
(L) The robots assume formation. (C) Traveling in formation. (R) Breaking off to pursue individual objectives. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 153
7.7
(L) The robots nominal trajectory would cause it to bump into the unmodeled obstacle (dark circle). (R) The dynamic planning methodology enables the robot to steer around the unexpected obstacle on-the-fly.
xiii
. . . . . . . . . . . . . . . . 157
Chapter 1
Introduction In many modern applications it seems that the traditional control paradigm of developing a single mathematical model of the plant and a single control law to achieve the task at hand is, at worst, intractable and, at best, impractical. As control engineers seek to control increasingly complex systems with the goal of automating increasingly complex tasks, new paradigms for control have evolved. Current trends seem to indicate that the popularity of these techniques and frequency of use among control engineers is ever increasing. It is important to note that their use is facilitated by the widespread proliferation of the microprocessor for embedded control. This ubiquity of processing power has enabled the development of sophisticated logic based control techniques which would have not been practical for real time applications 20 years ago. Two emerging ideas appear to be central to managing control complexity: modularity and hierarchy. In most cases a single continuous control law, of the type found in many control engineering text books, is insufficient for the task at hand. Inevitably several control laws must be designed and composed in some meaningful fashion to achieve their desired effect. Once simple individual control laws are designed for various subtasks (control modules or modes) the system designer must determine how to put these pieces together to complete the high level task. Modularity is concerned with addressing how to put these individual modes together in a meaningful way and 1
determining what properties the aggregate system will inherit from its constituent control laws. Once issues of modularity are resolved the problem of designing a control system to achieve a complex task is reduced to subdividing the task into a series of subgoals, designing a set of atomic control laws to achieve these subgoals and finally composing all of the pieces into a composite system.
Hierarchy is another way of managing complexity. Whereas the complexity discussed above is a product of the task to be completed; in other cases complexity emerges from the plant model itself. The plant may be complex enough that directly designing control laws is prohibitive. Instead designers may use various levels of abstracted models for control law design. The motivation for this is that often, for example, the full blown dynamic model may be of too high a dimension to practically solve optimal control or trajectory planning problems directly. Instead simplified models are used for high level and long range planning. The control synthesis technique called backstepping is essentially a hierarchical scheme for designing stabilizing controllers. Naturally there are trade-offs between optimality and computational complexity that one must consider.
Systems which are modeled and controlled using the concepts of hierarchy and modularity are collectively termed Hierarchical Hybrid Systems. This paradigm for modeling and control is finding application is several diverse areas of robotics. Mathematically, Hybrid Systems take the form of a set of differential equations whose right hand sides are smooth functions of the continuous states and possibly discontinuous functions of the discrete state 2 . The dynamics of the discrete state are governed by as separate set of equations but are functions of continuous state . The notion of hierarchy arises in the dependency structure of the equations. The states of hierarchical systems can be natural grouped into subsystems. The dynamics of the state groupings lower in the hierarchy depend on the dynamics of the higher level subsystems. The simplest 2
example of a Hierarchical Hybrid System would be 8
9 326
8 :; 8
/
with #MN.-OIP , %QMR.-OS ,
9326:W %432=>'?'?'? . ] ½ Definition 2.3 Execution An execution À of a hybrid automata with being a hybrid time trajectory, 274#¶¡ and Á#¢
is a collection À3Bc26L
satisfying:
q Initial conditions: 323'Ã@3'ÃÄM
£4¤mw§C ; q Continuous evolution: for all i with
x¾B³ , and 2 are continuous over $ c B³ & and for all ] ] ] CÄM}$ c ³ @CÄM£4¤m:32Cc and Å KCcFH9F32C@C ; and ] ] ÅcÆ ]
q Discrete evolution: for all w , GÇ32B3 ³ @c2 ³ 3 ÈÉM ]D ] .ËbG3B³ . ]
25
¥
K3 ³ ÉMʦ¹bG> , and 3 pÉM ]D ]
Initial Conditions
Continuous Update: Integrate through time step h
Discrete Update: Update q Apply R
YES
Event Detection
Event Location
NO
Figure 2.3: The basic hybrid system simulation algorithm.
2.2
Simulating Hybrid Systems
Obviously the goal of hybrid system simulators is to produce sufficiently accurate approximations of executions using the least computational effort possible. The basic, high level simulation algorithm is illustrated in Figure 2.3. Referring to the definition of an execution, the first required component, the initial conditions 323'Ã@K3'à , are supplied by the user of the simulation software. The simulation will begin by approximating the continuous evolution of the state, KCc described by the flow 9F323'Ã'@C . The state evolution described by the differential equation will be approximated by a numerical integration technique, so instead of the simulator producing a continuous map Br¯¢
(i.e. – an analytical solution to the differential equation), the simula-
tor returns a sequence of approximations of the continuous state at a discrete set of points along the Hybrid Time Trajectory »=3'Ã@KC@È@KCÌo@'?'?'?C ~
½
for 'Ã-{C d
N> , for Í# '?'?'?c .
In [16] it was shown that the proper way to simulate any hybrid system during the continuous evolution is to numerically integrate all of the differential equations with 2 held constant. The time at which the final state estimate C ~
in the sequence is generated should be as close as possible
to – the first time at which 5:Cz . At this point the numerical integration is stopped. Precisely finding and locating > is called the event detection problem. Often this is done 26
in two phases. The first phase occurs at every integration step and consists of determining if Î M$ C C & such that 5:3 0Ïz . This is called the event detection phase. If such a time ] ] d d D exists, the event location phase is activated in which a more precise value for is computed. ] All transitions are then taken, the discrete component of the state is updated to 23>p and any reset mappings are applied. The guards are checked once again, and any additional transitions are taken which have been enabled after the resets. Once no further discrete evolution can occur, the the integration may be restarted using 9F323 @L as the flow, with 323 @K3 as the new set of initial conditions. This technique is referred to as discontinuity locking and is widely accepted as the standard simulation methodology. The important point is that 2 is held constant during an continuous evolution. This is a nontrivial point since, given C will generate the next state approximation C states KCc with C
~ J
rCËÏC ~
~
~ J
@ for example, many numerical integrators
by evaluating 9F326L at several intermediate
. Even if some of these states satisfy M¦ , 2 and hence the
map 9 should not be allowed to change mid-step. The rationale behind this is that most numerical integration techniques are based upon Taylor series approximations, and so are only valid when the flows are continuous (and therefore the state trajectories are smooth). Therefore, a series approximation should not be constructed over an interval containing a discontinuity. While simulating the discrete component of the evolution is relatively straight forward, simulating the continuous portion through numerical integration is not and will therefore be reviewed in the next section.
2.3
Numerical Integration Methods
In this section the basic principles of numerical integration methods are reviewed and a particular class of methods, Predictor Corrector methods, are derived. Much of the material is adapted from [46] and [8]. 27
2.3.1
Notation
Given the a system of ordinary differential equations, 8 rH9FC@
(2.1)
with state M}.~ and a map 9 .~Ð.Ê.~ and an initial condition K3zÁ¬Ð à , the exact or analytical solution of such a system is referred to in this work as Ñ C . However, while solving a differential equation in the analytical sense entails finding a continuous function, in the case of a numerical solution one must generate a discrete set, »= à '?'?'?c d
'?'?'? , of state values which ½
approximate K Ñ Cc at the set of discrete times, »=CgÃC@''?'?'?C ' ?'?'? , often called mesh points. It is ½ d customary to denote the approximate solution at the discrete time C value of the time derivative may be written as 9
H9F
d
as
¼KC , and then the d d
. It is also convention to define the time C YC . Occasionally we will Ê step (the spacing between successive approximations) as d d d J d
d
use superscripts to denote various components (or groups of components, called subsystems or agents) of the state ]FM
.-O where ¤
x¾ . ]
Generating the discrete set of points which constitutes a numerical simulation, is generally accomplished by defining a difference equation
d D
Òr
d D
d
'?'?'?o
and, beginning with the initial data à '?'?'?o ^
d J ^
p9
d D
p9 d
'?'?'?p9
d J ^
d D
, iterating the difference formula. A basic taxonomy
of numerical integration methods can be outlined as follows. If the formula Ò
(2.2)
is dependent on
it is considered implicit; otherwise it is explicit. If Ó Hz in the definition of Ò , the method is referred to as a single step method; otherwise it is a multistep method. Finally if ng¸%wc§Ô , ] d D
it is a fixed step size method, otherwise it is a variable step size method. Modern numerical integration methods can have any combination of these attributes. Occasionally one is interested in not only generating approximations to Ñ C at the mesh points, but also over some continuous interval. In such situations an interpolant, denoted as KÕ Cc , is generated. Typically the interpolant is only a locally valid approximation of K Ñ Cc . 28
2.3.2
Definitions
Definition 2.4 Local Truncation Error The local truncation error of a particular numerical integration method is defined as G
d D
ÏÑ
d D
ÒQÈÑ
d D
p9FÑ @ 4Ñ ' ?'?'?Ñ @p9FÑ @'?'?'?op9FÈÑ d d J ^ d D d d J ^ d D
(2.3)
the residual when the difference operator for that method is applied to the exact solution of the differential equation. If the expression for G
d D
is proportional to
ÁÖ
where × is some constant integer, one refers to the
method as being of accurate (or consistent) to order × . Typically the accuracy of the method is order Ó
where Ó
2.3.3
Derivation of Predictor Corrector methods
is the number of data points used in Eq.( 2.2).
One specific family of numerical integration methods are called Linear Multi-step Methods. The most general form of a Ó -step linear multistep method (LMSM) is Ø^ nÈÙLÃLÚ
n'
n
d J D
d
Ø^ nÈÙLÃ%Û
n9
n d J D
(2.4)
where n and n are the coefficients of the method. Particular LMSM’s differ in how and Ú Û Ú Û are selected, however they are always some polynomial function in the step sizes. Multi-step methods, as opposed to say Runge-Kutta methods, are a natural choice for simulating hybrid systems because the expressions for
and can be used as to generate natural interpolation Û Ú formulae which approximate the solution between mesh points. These natural interpolants are then exploited for event detection.
The Adams Methods: The Adams family is by far the most popular class of these methods. All the following derivations are done for a scalar differential equation; however the extension to systems of equations is entirely straightforward. Given the differential equation 8 rH9FC@
29
(2.5)
integrating both sides through an interval $ C
C & gives d
d J
Æ´Ý
KC E C b Ð (*)
4. is
8A + L [ [[ Ð
or
?
T: terminate
Ë
F: relabel agents values of
1Ð
b
>m
]]]
m,
in order of descending
; goto 1
P 5m mP 5 m mP Ð m mP Ð m 2 2lZ3Æ 2 2lZ3Æ ]]]1jm1 #Ð]] ] m°m ] ])] s 1em A
= step_select(
Ë
for i=1:N
Ë
8 b = b5;
end
;/ ¾ 8
® IA2 = °I ¬ [ ®.- A
;
= Roots of Eq.5.17;
_ 8 if
stÁÛ¿t £ / m,0r : t @ 8 m_ Þ : t @ w _ 8 Ê 1 ¹Ð 8 z ;
else
;
1 ¹5 8 ( : t @ ; 1 ¹Ð 8 /@ 2 Ð G 1 ¹5 ;
end
1 Ð 8 dj1jem?( : 13Åm?( jd 1 ¹Ð m1 %Ð h @ h ; w 94
5.4
Examples
In this section the effectiveness of the algorithm presented in the previous section via a set of examples. The general scenario for these example is illustrated in Figure 5.2, which depicts two
Ë
Ë
Ë Ü8 > m J
mobile robots driving in the plane. The dynamics for agent
The
1
Ë
øË and
kä Ë 8 àä Ë 8 æ 8 ä
are
1 Ë êFëÅì : æ Ë @ 1 Ë ì ( : æ @
(5.19) (5.20)
ø
Ë Ë ] are state feedback laws (i.e., functions of k , à
and
æ
Ë
(5.21) ) and are selected in such a
way as to steer the vehicles in spiral trajectories. However Robot 1 travels much faster than Robot 2. The guard function is chosen to detect a collision between the two robots
A 8 =
ý
5 L L 5 L L : k = k @ G: à = à @ G _ ?
(5.22)
where R is the sum of the two robot vehicle’s radii. This example was simulated using the algorithm presented earlier for two scenarios – each with a different value for the separation distance (marked
2
in Figure 5.2).
In the first example (see Fig. 5.3 and 5.4) the value of the separation distance
2
is small enough
that the robots eventually collide. The path of the robots in the plane is shown in Fig. 5.3(left).
¢
Figure 5.3(right) is a plot of the value of the guard as a function of the step number . The guard value peaks first around step 70, when Robot 1 completes a half of a rotation; finally at step number 107 the event detection criterion becomes active and the step sizes are selected in such a way that
A$
exponentially. Further insight is gained by examining Fig. 5.4(left) which shows
the step sizes used for each agent as a function of time. Note how until
b «43
]ðö
15
and
1L
vary independently
when the event detection mode is active; whereafter they both begin adjusting the
step sizes so as to synchronize the two local clocks. This is further illustrated in Fig. 5.4(right)
which plots the history of the synchronization function . Its value rapidly approaches zero, at which point
b 5 8b L .
95
In the second case, (see Figure 5.5 and 5.6) the value of the separation distance
2
is increase
enough that the robots do not collide, but they do come very close to one another. The trajectories of the two robots are shown in Fig. 5.5(left). There is a near miss halfway through Robot 1’s second rotation. The history of the constraint in Fig. 5.5(right) shows that the value of
Aq: k @
comes
> . During this period, the value of , Figure 5.6(left), ï decreases in preparation for a possible event. Figure 5.6(right) shows how the step sizes were very close to zero around step number
decreased to slow down the simulation. However once the two robots pass each other and it becomes apparent that no collision will occur, the step sizes quickly increase and the two local clocks are allowed to fall out of synchronization again. Note that in these simulations,
; 8
]ðï
so that the “slow down” and synchronization, as
the state approached the guard surface, could be more easily illustrated. Using the method in Section 4.9,
;
can be selected in such a way that the “convergence” to the event surface is rapid.
Typically terminating in 1-3 steps.
5.5
Efficiency analysis
Obviously it is important to use an algorithm that is reliable, the primary motivation for using asynchronous simulation is to improve the efficiency of the simulator. In this section the predicted performance of the asynchronous algorithm is modeled and compared to that of the more traditional asynchronous algorithm.
5.5.1
The traditional algorithm
For the sake of comparison, the traditional synchronized algorithm is reviewed here. The traditional algorithm is makes no distinction between single agent systems and multi-agent system. In both cases it treats the entire state vector as if it belongs to a single system by using a single global step size; we refer to this as the Single Agent Algorithm or SA. The notation follows the same convention as in the Multi-agent Algorithm MA. 96
SA: Single agent (or Synchronous) Algorithm
0. Given values for 1. Compute
1¹
1jm13m13% ,
and
°;
let
¢8 >
via methods in Chpt.4
2. Select final step size
3.
1 8 : 31 jm? : 1 ¹ m13%lm13 @Ç@ for i = 1:N Ë integrate k using h Ë estimate error Þ end
4. 5.
Þ 8 : Þ 5 m mÞ Ð @ ° is Þ ? ]]] T: accept result compute F: reduce
¢ ' 6. is
¢ GÂ> ; b2/ZR5 ( )
1
13%
for next iter.
and goto 3
or
A 8 ?
T: terminate F: goto 1
Since the purpose of this discussion is to compare the efficiency of the asynchronous integration scheme to traditional synchronized simulation techniques, and not to compare different integration methods or event detection techniques, we compare the performance of the synchronized algorithm in Chapter 4 to that of the asynchronous algorithm described in this chapter. Both use precisely the same integration method (Adams Predictor-Corrector), the same tolerances and parameters, and the same event detection technique (reviewed earlier) so far as possible. Note that this analysis framework is in no way restrictive, because even if MA was compared to other 97
methods in the literature, no fundamental changes to this analysis are required, aside from recomputing the relative overhead constant (to be explained later). This is because the principle advantage of the Multi-agent Algorithm is that it permits independent step size selection, while no other algorithm in the literature does. The difference between the performance of SA and MA arises two primary ways. In SA a single ideal step size from the point of view of event detection
1¹
is computed using, for example,
Eq.(4.21) while in MA the event dynamics in Eq.(5.16) and (5.15) are used to compute
distinct
step sizes, one for each agent. Clearly, selecting the step sizes in the multi-agent case requires more computation due to the fact that the synchronization requirements must be dealt with. A single iteration of SA is also slightly more efficient since one need not keep track of
distinct
time clocks, step sizes and synchronization events ( ). Despite the fact that a single iteration of SA is more efficient, it will in general require more iterations to reach the final time single global time step forces
1
()
. This is because, satisfying error and event criteria using a
to be computed as the minimum value of all the agent’s time steps.
This restriction is explicit in computing the ideal step size for truncation error considerations
17#
based on the worst case error across all agents. Another situation in which the traditional technique produces overly conservative choices of
1
is when the model contains some guards which are only
a function of a subset of all the agent’s state’s. If such an event is about to be triggered, a small value of
1¹
is typically required. In the Multi-agent algorithm only the agents whose states the
guard is a function of must use the smaller time steps. In contrast, the traditional algorithm would require that all agents use
1 ¹ , including agents whose states do not appear in the expression for A .
These issues surface in the analysis that follows.
5.5.2
Average complexity analysis
Throughout this section the subscripts
576
and
896
to the Single Agent or Multi-agent algorithms. 98
are used to distinguish quantities pertaining
The computational effort for any simulation algorithm is simply the product of the number of steps taken and the number of computations per step. In the case of the Single Agent Algorithm
where
:?;
¬: ;y«=:?; [ : ¬A@ [ @ m
(5.23)
is the number of simulation time steps the Single Agent Algorithm executes to reach
the final time
D ( , A¬ @
is the cost of simulating a single agent through one step (via the Predictor
Corrector Method), and
is the total number of agents to be simulated. Since the algorithm is
synchronous, the number of steps used to simulate each agent is identical. On the other hand, the computational effort for the Multi-agent Algorithm is
¬BC;*« Ë ÒÐ > < BC; : @ [ : ¬A@ G,DFE*@ 5 ]
(5.24)
Note that in contrast to the Traditional Algorithm the number of simulation steps vary for each agent
8 > m
]]]
m!
Ç G J ] ö ð] ï
(5.35)
Recall that the relative overhead is the overhead as a percentage of the cost of integrating through a single step
¬A@ .
Naturally
¬A@
is highly problem dependent but it is generally greater than 10
floating point operations. Problems with more complicated expressions for higher values of
¬ @.
P k : @
Prediction and discussion: Using Eq.(5.30) on can then predict the value of tion of the number of agents, for different values of
¬A@ .
typically have
8 ¹ WH
as a func-
A closer look at Eq.(5.30), in light of
the derivations presented in this section, reveals that the numerator which is the average cost of the multi-agent algorithm contains a term which scales linearly with the number of agents (the overhead associated with synchronizing on-the-fly
ª
DFE
);
itself as a multiplicative factor; and
two terms which are constant with respect to the number of agents: the expected number of iterations of the multi-agent algorithm
d BC; h
and the problem dependent cost of integration 102
¬A@ .
This implies that the cost of the multi-agent algorithm is quadratic in the number of agents as one might expect.
ª
In contrast, the denominator, which represents the average cost of the traditional single agent algorithm, contains the term
5.6
d
(5.36)
were randomly assigned functions of time.
Ë
The “agent-wall” collision guards are
where
áââ
k
and are the
Ë
Ñ Ñ[Z A Ë 8=Y Ë = k Z A Ë 8k Ë = Y Z A ËÆ 8à = Y Æ Ë Z A 8=Y = à
± ± ± ±
coordinates of the left and right walls, and
Ë
(5.37) (5.38) (5.39) (5.40)
Y ÆZ
and
Ë
Y Z
and are
coordinates of the top and bottom walls. The “agent-agent” collision event is guarded by
where
_
L L L 8 Ê ¯m A É 8 : k = k É @ GÂ: à = à É @ = _ ± mN\Q] ^ ô is the radius of one of the cars. The number of differential equations is
number of guards is
ô
G [ : =?>/@ I J . 103
(5.41)
ò
and the
The algorithms SA and MA were coded in Matlab so as to be as similar as possible for the sake of fair comparison. Parameters such as the minimum and maximum step size, and integration tolerance were the same in both cases. The actual numerical integration algorithms were also identical. Experiments were conducted to determine how the speedup factor
8 ¹ !H
varies with the number
agents. Recall that it was predicted that as the number of agents increases, the Single Agent algorithm becomes increasingly likely to select smaller step sizes, while the Multi-agent Algorithm is not biased toward smaller step sizes. Data was collected for systems with 2 to 20 agents. For a given number of Agents and tolerance, 10 randomly generated scenarios were simulated for a fixed duration using both SA and MA. The number of floating point operations used by each algorithm for a given scenario was measured. The raw data, using an integration error tolerance of
>
ZO_
is
shown in Figure 5.10. Note for most of the 190 test scenarios, with the exception of 2 of the two agent scenarios, MA outperformed SA. In the case when
8 J , SA averaged double the amount
of computations required by MA. The figure supports the claim that MA is generally more efficient than SA and seems to agree with the predicted quadratic scaling of MA vs. the exponential scaling of SA. This echos anecdotal reports of computational bottle necks in large scale simulations by SHIFT users. A second factor whose impact on performance was studied is the user specified integration error tolerance. The performance improvement associated with MA would be expected to decline at tighter error tolerances. This is anticipated because tighter error tolerances have the effect of biasing the step size selection mechanism toward smaller step sizes. Each of the 190 scenarios mentioned above was simulated with three different integration errors tolerances, and
>
°H8 > OZ _ , > Za`
ZOb , for a total of 570 experiments. The number of floating point operations used by each
algorithm for a given scenario was measured. Based on this data the speedup ratio for each scenario was calculated.
8 ¹ WH
for a given number
of agents and a given tolerance was computed as the geometric mean of the speedup ratios of the 104
10 random scenarios. The geometric mean as a function of the number of agents is plotted in Fig 5.11. Each curve displays the results for a different value of the integration tolerance. Tighter integration tolerances have the effect of biasing the algorithms toward selecting smaller step sizes. The dashed lines in the figure show the predicted speedup.
5.7
Discussion
Although the probabilistic modeling did predict the overall trend in the number of agents vs. cost quite well, it is obvious that the model tended to overstate the performance improvement seen in practice. This is no doubt due the fact that the actual step size “distribution” is of course nonuniform. This observation warrants a discussion of why a uniform distribution was selected in the first place since it is a major factor impacting the predictive power of the modeling. Again, the distribution is entirely problem dependent, so from the point of view of predicting the performance it is important to choose a distribution that does not particularly favor one algorithm over the other. Distributions which are non-uniform and heavily weighted around a mean will certainly bias the prediction depending on how the mean varies across all agents. If all the agents have the same distribution, with the same mean, this implies that the agents are all varying at nearly the same time scale, in which case little advantage is to be gained from an asynchronous simulation. On the other hand, if all agents have the same general distribution but their means to do not coincide, the asynchronous simulation will be highly favored since this implies that some agents are consistently “slow” and others consistently “fast”. In light of this a uniform distribution was selected since it is does not necessarily favor either of these two extremes, representing a somewhat neutral assumption. 105
5.8
Conclusion
A major cause of performance loss when simulating large scale systems of ODEs can be traced back to the requirement that a single global step size is traditionally used despite the fact that the various individual differential equations in the system may each warrant a very different choice of step size. The only proper choice of a global time step among each individual equation’s candidate time step is, unfortunately, the minimum of these candidates. As a result, the simulation often must proceed unnecessarily slowly using the smallest possible step size. The more individual differential equations in the system, higher the probability of more frequent and severe performance drain. In situations in which the coupling between the right hand sides of the ODEs is strong this cannot be avoided. Many classes of Hybrid Systems however, contain a large number of decoupled or partially decoupled ODEs. Majority of the interactions within the system may arise in the discrete states. One situation, the case of decoupled ODEs with highly coupled guards – referred to here are Multi-agent Hybrid Systems – appears often enough in practice to warrant special techniques. Examples of such system include groups of automated highway vehicles, unmanned aerial vehicles, mobile robots, etc. – systems which are nominally physically decoupled, however, through sensing, software or actual physical collisions, interactions may arise which precipitate mode changes within the system. In such cases, despite the fact that the ODEs are decoupled, all current simulators use a single global step size since it greatly simplifies the event detection problem. In this chapter a technique is introduced for simulating multi-agent hybrid systems in an asynchronous fashion, that is without resorting to a single global steps size. In this asynchronous simulation algorithm each agent has its own local time clock. During parts of the execution in which no discrete transitions occur, the individual time clocks are allowed to advance at different rates as dictated by the agent’s dynamics, allowing the maximum utilization of the available computational resources. This complicates the event detection problem because it is no longer 106
sufficient to simply check the sign of the guard function since the individual states are being reported at different points in time. The primary challenge here is selecting for the
integration step sizes
agents in such a way that the individual time clocks will synchronize on-the-fly when
some relevant subset of the states approach a guard surface. The step sizes should be selected so that the simulation terminates exactly on the guard surface with the value of the local time clocks identical. As in Chapter 4, a Input/Output Linearizing step size controller is derived to accomplish this. In this case the problem is a multi-input multi-output control system, for which the inputs are the
step sizes and the output functions are the guard function, along with
=O>
synchronization
functions, which measure discrepancies between the local time clocks.
We show that while a single iteration of the new algorithm is somewhat more expensive due to the overhead associated with adaptive synchronization, not using a global time step requires fewer total iterations. The extent to which one algorithm out performs the other is somewhat problem dependent both in terms of the amount of computation required to complete a single step and the number of steps needed. In order to predict the efficiency of the algorithm we construct a probabilistic model of the step size selection scheme, assuming the ideal step size for each agent is a random variable. Under the assumption that the distribution is uniform, it is shown that average complexity of the multi-agent algorithm is approximately quadratic in the number of agents being simulated. In contrast, it is demonstrated through experiments that, as the number of agents increases, the average computation time for the traditional algorithm seems to exponentially approach an upper bound. The total time is bounded in the sense that the user specification of a minimum allowable step size bounds the total number of iterations. If there is no minimum allowable time step, the total simulation time would approach infinity. This analysis confirms that the new simulation algorithm is a drastic improvement over the traditional algorithms for systems with many agents.
Experiments were conducted in which both the traditional single agent algorithm and the new 107
multi-agent algorithm were used to simulate a problem many times with a large number of randomly generated sets of initial conditions. Two variables were manipulated: the total number of agents and the integration error tolerance. It was shown that the predicted relationship between the number of agents and the computational cost savings was valid and that requiring finer simulation accuracy serves to amplify the potential savings associated with the new algorithm.
108
Initial Conditions
Initialize Compute new h via step_select
Predictor Reduce h Corrector
Estimate Error
Error acceptable?
Relabel Agents 1,...N in decending order of t1,...tN
INTEGRATOR
Store Results
Event? NO
Discrete update YES
Figure 5.1: The overall simulation algorithm introduced in this chapter. A more detailed description for the entire algorithm as well as the for the step size computation (depicted in the box to titled “Compute New via step-select” is given in Section 5.3.
1
109
D
o
o
Car #1
Car #2
Figure 5.2: Two automated vehicles driving in spiral trajectories.
20
0
15 −5
10
Region of Exponential Convergence
START − CAR 2 −10
Guard
Y − axis
5 START − CAR 1
0
−5
−15
−10
COLLISION −20
−15
−20 −20
−15
−10
−5
0
5
10
15
−25
20
50
100
X − axis
150
200
250
Step Number
Figure 5.3: The trajectory of the two robot vehicles in the plane is plotted (left). The separation distance, , is small enough that they collide. The value of the guard as a function of step number is shown (right). The guard value converges to zero exponentially.
2
0.35
0.01
0.3
0
h2
−0.01
0.25
Synchronization
Tau
Step Sizes
−0.02 0.2 −0.03
0.15 −0.04 0.1
h1
−0.05
0.05 −0.06
0
1
2
3
4
5
6
7
8
9
10
−0.07 100
11
Time
110
120
130
140
150
160
170
180
190
200
Step Number
15
1L
Figure 5.4: Step sizes used in the first example are shown here(left). As described, and are selected independently away from the constraint but are brought into synchronization when an event is impending. The value of (right), which is a measure of how out of synchronization the two local time clocks are rapidly decreases in magnitude as the event is approached.
110
0
20
15
−5 10
START: CAR 1
−10
START: CAR 2
Near Miss
Guard
Y−Axis
5
0
−15
−5
−20 −10
NO COLLISION
−25
−15
−20 −20
−15
−10
−5
0
5
10
15
−30
20
20
40
60
80
100
120
140
160
180
200
220
Step Number
X−Axis
2
Figure 5.5: The trajectory of the two robot vehicles in the plane for the second example (left). is selected such that the two robots come very close without colliding. The value of the guard as a function of the step number is shown(right). The constraint comes close to zero as the robots barely miss each other.
0.5 0
0.45 −0.05
0.4
−0.1
0.3 −0.15
Tau
Step Sizes
h2
h1
0.35
0.25
−0.2
0.2
0.15 −0.25
Near Miss
0.1
0
−0.3
Near Miss
0.05
4
6
8
10
12
14
16
18
90
Time
100
110
120
130
140
150
160
170
180
190
200
Step Number
15
1L
Figure 5.6: Step sizes used in the second example are shown(left). Again, and are selected to synchronize the simulation when it seems an event may occur. Once it is apparent that this is not the case, the simulation returns to asynchrony. The magnitude of (right), therefore decreases during this period but soon increases again.
111
0.1
0.09
0.08
Normalized Frequency
0.07
0.06
10 Agents
0.05
0.04
0.03
5 Agents 0.02
1 Agent 0.01
0 0
0.1
0.2
0.3
0.4
0.5
0.6
0.7
0.8
0.9
1
Step size h
Figure 5.7: A histogram depicting the normalized frequency of selected step sizes for the Singleagent algorithm, for several values of (the number of agents). As the number of agents becomes large.
0.5
Expected Value of h
0.4
0.3
0.2
0.1
0
0
10
20
30
40
50
60
70
80
90
100
Number Of Agents
1
Figure 5.8: The expected value of , for the Traditional Algorithm as a function of the number of agents.
112
Figure 5.9: The -agent “bumper car” example. Events occur when an agent collides with one of the walls or another agent.
7
x 10 7
Floating point operations
6
5
4
3
2
1
0
2
4
6
8
10
12
14
16
18
20
Number of Agents
Figure 5.10: Sample experimental results on the complexity of each algorithm as a function of the number of agents. Squares represent SA data and crosses are MA data. The data supports the claim that MA scales quadratically while SA scales exponentially.
113
1
0.9
−5
10
0.8
0.7
−4
10
0.5
S
avg
0.6
0.4
10−3
0.3
0.2
0.1
0
2
4
6
8
10
12
14
16
18
20
number of agents
Figure 5.11: Experimental results: computational cost as a function of the number of agents, for various integration tolerances. Dash lines represent analytical predictions; while the solid line is the geometric mean of the experimental data..
114
Chapter 6
Multi-rate Integration Techniques In this chapter an algorithm for multi-rate integration is introduced which is well suited to simulating Hierarchical Hybrid Systems. The ultimate goal is to improve simulation efficiency through the use of an algorithm which is capable of exploiting this structure. A observed feature of such systems is that the dynamics of the various subsystems evolve at different time scales. As emphasized in Chapter 5, anytime a system naturally possesses different time scales, there is potential to improve simulation efficiency by permitting the different system components to be integrated with different step sizes, as opposed to a single global step size. Whereas the challenge in developing an asynchronous simulation in Chapter 5 was to account for the coupling of the discrete dynamics, in this chapter we consider systems with decoupled discrete dynamics but coupled continuous dynamics. Parts of this work first appeared in [29].
6.1
Problem Statement and Discussion
Hierarchical hybrid systems are simply a special class of Hybrid systems that have a special structure as well as a convenient representation. Nested or hierarchical control systems are examples 115
of Hierarchical Hybrid Systems. Mathematically these systems have the following structure:
P 5 5 m k 5 (6.1) : @ . Ë .. Ë Ë Ë k ä 8 P]]] : m k 5 m m k @ ]]] .. . k ä Ð 8 P]]Ð ] : Ð m k 5 m m k Ð @ Ë Ë ]]] P W where k the guards Ë £ _ , £c , and d _ [[[ _ _ . It Ë is assumed for each subsystem only depend on that subsystem’s continuous state, A : k @ ± . The lower kä 5 8
triangular dependence of the continuous dynamics on the continuous states stems from the fact that we assume information (typically in the form of reference signals) propagates down the hierarchy. This also reflects, for example, the read – write structure of the CHARON hybrid system modeling language ??. Another interesting feature of such system is that, often each subsystem evolves on a different time scale. That is to say,
kÐ
, in some sense, changes “faster” than
k Ð RZ 5 . For example
any large linear system with a significant disparity in the magnitude of its eigenvalues will exhibit multi-rate evolution. In a robotics example, the top level could be some type of path planner which computes via points at 1 Hz; while the lowest level might be a motor control loop which runs at 10kHz. While the concept of “Hz” refers to how frequently a discrete time control loop is closed, often the frequency of the controller is dictated by the time constants of the dynamics. As always, the tradeoff between accuracy and efficiency must be managed online by selecting the largest step size consistent with the accuracy requirements. Traditionally, at each time step,
1 5m
1 , for each of the subsystems would be computed based ]]] smallest of these time steps 1÷8Â( dj1 5 m 1 eh , typically on the estimated truncation error and the ]]] 1a , would be selected as the global time step to be used to integrate each of the subsystems. appropriate simulation step sizes,
As discussed in the previous chapter, given the disparity of the time scales involved, it would 116
seem wasteful to simulate each level in the hierarchy using the smallest overall candidate time step. Instead it would be most efficient to use the appropriate candidate time step to simulate each subsystem. However, using different step sizes for each subsystem results in asynchronous time meshes for the various subsystems. In Chapter 5, this complicated the event detection problem. Here, since the event dynamics are decoupled, no special treatment of the topic is needed. However, in contrast to the previous chapter, the right hand sides of the differential equations are coupled. The coupling of the continuous dynamics complicates the integration process.
X1
T
X2
h1
T
X3
h2 = 1/2 h1
T
h3 = 1/2 h2 = 1/4 h1
Figure 6.1: Multirate technique with
t 5 8tlL¶8 > I J .
b
8 computing the derivative of k L (a necessary step in numerical PL 5 L 5 integration) requires evaluating ]ðï k m k , and thus having the value of k at b8 : @ . However if k 5 , evolving significantly slower, is being integrated with a time step of 1$8 > then its]ðï state is only computed at b £ s m > m J m w . Thus one needs an acceptable way of estimating slow variables ]] the value of k 5 at ba8 , knowing only k 5 and k 5 . This at off-mesh points, in this ]case :@ :>/@ ð ] ï situation is depicted in Figure 6.2. For example at time
117
x1 x1(0) x 1(1)
x1(0.5) ??? t t=0
t=0.5
t=1
Figure 6.2: Inferring the value of the state at off-mesh points represents a challenge for multirate methods.
6.2
The algorithm
Assumptions:
While multirate methods can and have been extended to systems of equations
Ë
Ë
Ë
with structures other than lower triangular form, they are best suited to equations such as Eq.(6.1). It is assumed that: (1) if
]
, in some sense,
k
evolves slower than
k ZR5 ; (2) the P
’s are
sufficiently continuous and satisfy all the criteria to ensure the existence and uniqueness of the solution to the differential equation; and (3) the guards describing when the discrete transitions for each subsystem occur are decoupled.
Traditional Predictor-Corrector Algorithm
As a point of comparison, the reader is referred
to the description of the traditional Predictor-Corrector Algorithm discussed in Chapter 2. For convenient comparison, a flow chart of the traditional algorithm is given in Figure 6.3. Note that in the “Error Estimation” step the error for each subsystem is estimated, and maximum error across all of the components is used as the estimate. This means that when a step is rejected because a particular subsystem’s error is outside of the nominal range, the results for all the subsystems are discarded and the new step size is computed based upon this maximum error. Likewise, when the error associated with a step is smaller than the ideal value and the step size is increased for the next iteration, the size of the increase is constrained by the subsystem with the maximum error. 118
Multi-rate Algorithm: In this section a multirate version of an
r ÆS
order predictor-corrector
(PC) pair of the Adams family is introduced for the first time. Multirate versions of other numerical integration schemes, such as BDF or extrapolation methods, have appeared elsewhere in the literature. The multirate Adams method differs from the tradition version of the Predictor-Corrector algorithm (described above and in Figure 6.3) in that the systems is integrated asynchronously one
Ë
Ë
component at a time. For each component a different time step may be used, in much that same
Ë
way as was done in Chapter 5. Here
1
S
is the step size for the for the Æ equation; and
1 1É
for
Ë ] . The Ë Ë integer ratio, t of step sizes for adjacent levels in the hierarchy is given by the relation 1 8t 1 465 for »8 > m mfe =?> , as seen in Figure 6.1. ]]] for our Multirate Predictor Corrector simulator is shown in FigThe conceptual algorithm ure 6.4.
Ë
The steps of selecting a subsystem to integrate, evaluating interpolants for constructing an interpolant for
k
k 5m
Ë
]]]
k ZR5 , and
are not present in the traditional version of the algorithm. How
these steps are completed determines the underlying accuracy and stability of the algorithm. This is discussed in the following section.
6.3
Analysis
In this section we examine how the choice of interpolants to approximate the slow variables and the choice of the order in which to integrate the subsystems impacts the accuracy and stability (respectively) of the simulation algorithm.
6.3.1
Integration error analysis
In this section we analyze the accuracy of the method compared to traditional simulation techniques. Such an analysis is important (1) to determine the expected performance of the method, (2) to provide a method to control and estimate truncation errors (and hence select an appropriate step size at runtime), and (3) to provide a basis for selecting acceptable interpolating functions. As 119
in most numerical analysis, we will perform the error analysis locally in Section 6.3.1 by assuming that these required past values of the
gOh ’s and ijh ’s used in the PC method are exact, then look
at the errors introduced after one step. A general error expression is derived without assuming a specific form of the interpolation function. This expression is then used to select an appropriate interpolant. The ultimate goal is to devise a multi-rate algorithm whose accuracy is the same order of magnitude as a comparable traditional (non-multirate) algorithm. The local error of the multirate simulator,
k#lXm
is defined as the difference between the value
g hporqps t , and the analytical solution, gUu h orqvst , to the differential equation after a single step from qpsxw*y to qvs assuming that there was no u error induced in the previous step (i.e. g orqvsxw*y.t9z g orqpsxw*y.t ) k h{T| orqps t}z g h orqvs~t g u h orqps t. (6.2) n
of the state of a particular subsystem generated by simulation,
If we let
g hÆ |f.
denote the value of the state approximation had the traditional (non-multirate)
Predictor-Corrector integrator been used, then (6.3) k h{T| orqvs~t}z4 g h orqps~t g hÆ |f. orqvs~tp
g hÆ |f. orqvs~t g u h orqpstp
} u It is apparent that gUh |p. orqpst gOh orqvs~t is by definition, the local truncation error of the traditional Æ
integrator; while
g hforqvs~tO g hÆ |fW orqpst
is the discrepancy in the values produced by the traditional vs.
multirate integrators. The traditional integration error has a well known expression,
k hÆ |fW orqps~t} y9 o hs t {
(6.4)
y is a known constant, ahs is the size of the integration interval used for subsystem n at step , and is the order of the Predictor. Furthermore, it will be shown later that g hforqvs~t g h |fW?orqpst Æ
where
depends on the errors in interpolating the slow variables. So the error can be expressed as the sum of errors associated with the traditional PC method
k Æ |fW
and an additional term which accounts
for the fact that interpolated values are used to accommodate coupling
k {T| z k Æ |f. k# 120
k# , (6.5)
Since
k Æ |fW
is known, an expression for
k z g hforqpst g hÆ |fW orqvs~t
must be derived. Note that
in the flow charts in Figure 6.4 and 6.3, the two algorithms produce identical results up to the predictor step. This is because this step is only based on information at
qpsxw*y , and there is no
need to interpolate any quantities, therefore in either case the output of the Predictor step will be referred to as
g h |f orqpst .
Terms contributing to
k
are introduced only during the corrector step
since it is that step which relies on interpolating the slow variables. Let
g
indicate an interpolated
quantity. After one corrector iteration using the traditional method, we get
{ *w y w * y y hs?O i h o g Æ |fW orqpst.x g hÆ |fW ro qvs~t. g h |pv orqpstft . i s#h w ¢ ¡ ¢y £ g hÆ |p. orqpst9z g hs , y
(6.6)
and for the multirate method one must interpolate the slow variables, so
{ *w y y * w y hs?O i h o g orqpst.x g h orqvs~t. g h |fv orqvs~tft ¢ i s#h w ¢ ¡ Wy £ g h orqpstTz g hs ¤ y Then k# z gUh ro qps t gOh |fW orqps~t can be expressed as Æ
(6.7)
k z a h O ¥¦ i h¨§ g Æy |fW orqvs~t.# g hÆ |pw*.y orqps t. g h |f orqps~tv© ijh § g y orqvs~t.# gU h w*y orqps~t. gOh |fv orqvs~tv©«ª Assuming ¬ g orqvs~t g |fW orqps~t® is small we can expand the second term about g f| W orqps~t . We then Æ Æ
have, to first order
&° ± h *w y i h z h O¥$¯ g ¬ g Æ p| . orqpst k y³² ²
g orqvs~t®Q,´µo ¬g orqpst g Æ f| W orqvst®·¶#t¹¸&» º
g u orqps t . This term is effectively a measure of how sensitive our method is to errors in estimating ijh due to inexact interpolation of slow variables. Note that this term is multiplied by h which is typically small. Since the error contribution from k should not dominate k |fW , we select g to be ƽ¼ order Æ polynomials. Thus the interpolation errors vary with the ƽ¼ power of the interpolation interval, . Recall that the larger step sizes of higher layers in the hierarchy can be expressed in terms where the partial derivative is evaluated about
121
of smaller step sizes as
ah w*y z¿¾h w*y h
and in general
z4À hÁ *w y ¾ Á h . Using these relations, a
general error expression is
{ ¡ y ° ± h w*y i h Ê w*y Á·Ë { { O¥$¯ g ,ÈaÉ Á ¾ ,Ì¢Íθ» º xk l  ÃÅy Ä h Æ $ (6.9) yG² ÃÄ y vÇM|p.  ¶ h Æ ² Ï Thus the dominant error term is k |fW – the same as that of the traditional Adams PC method. Æ Note that as the step size of the slower equations approach that of the fastest equations (i.e.
¾ ÁÐ Ì , ÑjÒ zÓÌ n Ì ), the k
term vanishes and the error expression reduces to that of the
traditional technique. In addition to providing an error estimate, Eq.(6.9) gives a rule of thumb for selecting appropriate ratios of the various step sizes. In particular since we would like to ensure the errors due to the interpolation of the same order of magnitude as those of the integration technique, we should select
{ h w*y i h Ê w*y Á Ë O ¥ g ÔÉ É Á ¾ ÕÌ Ë¿Ö Ì h y¹² y ²
(6.10)
In practice this may be hard to achieve exactly since we may not always precompute
××#Ú¢ØÛ Ù ; but
generally there is a limit to how large the ratio between the largest and smallest step size should be for a given choice of step size. It also tells us that any arbitrary choice of interpolants is not acceptable. Had linear interpolants, for example, been used the than
k
term would be
´µo { ¡ y t ; and hence, the dominant contributer to the overall error.
6.3.2
´µo ¶ t
rather
Stability
The decision of which equation to integrate at a given time is made according to the slowestfirst criterion which was shown in [23] to have superior performance versus the so-called fastest first methods . The slowest first criterion selects to advance the equation which has been simulated through the least amount of time so far. Since the step sizes are all integer multiples of one another, often there will be several equations that fulfill the criterion. In the case of such a tie we integrate the one with the smallest index (the slowest equation) first. Given the nature of the coupling and 122
the type of information needed to compute the Corrector, allowing the slow variables to lead the fast variables ensures that the slow variables are always interpolated rather than extrapolated. By guaranteeing that the slow variables are always interpolated (by virtue of the slowest first method), the approximation error is always well bounded; whereas, had they been extrapolated, it would be much more difficult to ensure the approximation error is small [23]. While the stability of the method is more difficult to analyze since traditional techniques for determining the stability of numerical operators fail for multirate methods, the scheme presented here shares the same stability properties as the traditional integration method. This is because the nature of the coupling in conjunction with the slowest first rule implies that the integration error is always bounded, and therefore accumulates in a stable fashion. [23]. In addition, as a result of proper interpolant selection (discussed in the previous section) the interpolation error is an order of magnitude smaller that the traditional error contribution – meaning that the multi-rate method shares the traditional method’s stability properties. The stability of traditional
½Æ ¼
order Adams PC pairs has been studied exhaustively in the
literature, see for example [8]. The stability characteristics of Adams methods are known to be excellent, though they are not absolutely stable (i.e. not stable for arbitrarily large step sizes) and hence not suitable for stiff problems.
6.4
Efficiency
While stability and accuracy are necessary conditions for using a numerical technique, they are not sufficient. Recall that the motivation for introducing multirate methods in robotics was to increase the efficiency of the simulation. Despite the fact that larger step sizes can be used, the multirate simulation techniques is not always more efficient than traditional techniques since there is some overhead associated with constructing and evaluating the interpolation polynomials. The primary virtue of taking large steps is that total the number integration steps, and hence the total number of times the right hand side of the differential equation needs to be evaluated is minimized. A 123
similar trade-off was explored for the algorithm in Chapter 5 using a probabilistic model. Not surprisingly, the amount of computation that is saved using a multirate method will depend of how expensive it is to evaluate the right hand side as compared with the cost of constructing and evaluating the interpolating polynomial. This becomes apparent in the following analysis. Although one could elect to use a probabilistic model to predict performance, in this case we elect not to because there are simply too many parameters to assume. The assumption that each of the subsystems evolves over distinctly different time scales implies that each system’s model would have a nonuniform distribution, each with a different mean. Since it is impossible to select means that would be representative of every problem we instead derive a model for the efficiency under some conservative assumptions, merely to illustrate how the overall efficiency depends on various factors and then test the efficiency experimentally on an example problem. For the purpose of illustration we make a number of simplifying assumptions here. We assume the step size is fixed with respect to time, that the right hand sides of each of the subsystems (
i y i Ü ), are equally expensive to evaluate, and that the difference in step sizes in the various
levels of the hierarchy in the modular simulation is constant (namely the step size at any given level is twice the size of the step size being used one level below and half that of one level above). In the traditional simulation, all components are integrated using the maximum acceptable step
size for the fastest component, . We assume that
Ý
is the total number of steps of size
needed
to complete the simulation. Referring to Figure 6.3, and ignoring minor overhead and the irregularities introduced in the start up process, the most expensive steps required to simulate one component through one step
(associated computational costs denoted in parenthesis) are:
Þ
predict (
Þ
evaluate the right hand side (
Þ
correct (
Þ
reevaluate right hand side (
ß ß
);
àGáNâ
);
); and
àGáãâ
). 124
If there are
ä
subsystems the total computations to complete the simulation are
ä Ý # å o Gà áãâ ß Ft.
(6.11)
For the multirate simulator, assume that the fastest component is integrated at the same rate as
in the global simulator, , and that at each higher level the step size is double that of the previous.
Ì
ä
For convenience we label the fastest component as level- and the slowest as level- . Component
n
is then simulated with step size of
the simulation is
Ýêé å æ h w*yè .
åçæ h w*yè
n
, and the number of steps taken at level to complete
Referring to Figure 6.4, at each step at level
n
for the multirate simulator all the steps of
traditional method listed above are computed in addition to
Þ
evaluating the interpolation polynomials of the (n-i) slower variables (
Þ
generating the interpolation coefficients (
êì
ßFë
); and
).
So the total computation is
Ü Ì Ý å æ h w*yè o å o àGáNâ ß Ftíîo ä n t ßFë ,êìFt (6.12) y h We are interested in computing the speed up ratio, â , which reflects how much faster (or
slower) the multirate simulation is vs. the global simulation. It is computed by dividing the computational cost required to simulate a given system with the multirate technique by the cost of the traditional simulation. A small value of
â
implies a great efficiency gain is possible with
multirate simulation. Dividing Eq.(6.12) by Eq.(6.11), and simplifying gives
ï ¶ o àGáãâ Ôì ß Ft ðïÅñ o àGáãâ ßFë ß êt â z=ï y L with
y w*y yÜw¶ å å ã + ÷ ä ¶ f ò ïy z ï¶ ô z óf¶ õ ïÅñöz åä ¶ ä
each of the coefficients is strictly a function of the number of subsystems sizes across the hierarchy (in this case assumed to be 125
ä
(6.13)
and the ratio of step
å Ü *w y t ; While the terms they multiply are
specific to both the implementation and the particular set of ODE being integrated (
àGáNâ
). The
first terms reflects the gain in speed based simply on the fact that less steps are taken with the modular simulation algorithm. This gain increases (
ïy
decreases)
drastically as the number of subsystems increases. The final two terms reflect the fact that interpolants are used for off-mesh point. In these terms
ï¶
and
ïÅñ
are multiplied by the ratios of
the cost of generating or evaluating the polynomials versus performing the integration. So
â
is
largely dependent on how expensive it is to compute the right hand side of the subsystems.
6.5
Implementation and experiments
ø ½¼
The algorithm has been implemented in Matlab, using a Æ
order PC pair. It supports automatic
step size selection, both across time and across levels of the hierarchy, to monitor and control truncation error to a user defined tolerance as the simulation proceeds. The user may specify groups of variables, called subsystems, that should be computed at the same rate. In our implementation with
ø ½¼
a Æ
order Adams method:
Ôìù^÷ ú
flops (floating point operations);
ß ùzûÌ~Ì ; ßFë zîü
flops.
Figure 6.5 shows the predicted efficiency of the method calculated with Eq.(6.13), as a function of the complexity of evaluating the right hand sides of ode’s (
àGáãâ
ý ÷
Note that the point at which the multirate simulation becomes faster (when (15-25 flops per
àGáNâ
ø
) for , , and level hierarchies.
â Ö Ì ), is low enough
) that many robotic systems will fall in that category. For example, systems
controlled using feedback linearization require inverting a matrix ( require computing inverse kinematics ( body contact models (
àGáãâ 4÷ ú ), systems which
àGáãâ å ú~ú ), or systems which require computing rigid
àGáãâ å ú~ú~ú ). Observe that the benefits of multirate simulation become
more pronounced as the number of subsystems increases. It also should be mentioned that, while
å
the ratio of step sizes of neighboring levels in the hierarchy used to compute Figure 6.5 is , the efficiency gain increases as this ratio increases. The hierarchical control of a differential drive cart-like robot was used as a test example, Figure 6.6, shows the results of simulating this system with the tool presented in this paper. The 126
o g fþt
coordinates of the various subsystem are plotted. Input-output linearization techniques
were used to design standard controllers for the system. The desired path (level-1) is an arc and the controller successfully calculates wheel velocities at level-2 and wheel torques at level-3 to track this path. In addition we also added a fourth level of in the hierarchy which models a second dynamic cart attempting to follow the first cart at a prespecified distance. In this particular case we compared the performance of our (nonoptimized) code vs. that of a popular built in Matlab ode solver(ode45). Both solvers were executed with the same error tolerances. The multirate code required only 27 % of the number of floating point operations used by the built-in solver (about 4 times faster). Figure 6.7 shows the frequency with which the multirate algorithm integrates the various levels in the hierarchy. As expected the first level in the hierarchy
y 4úQ ÿ ) while the lowest level is integrated with a much smaller step size to capture the fast changing dynamics ( =úQ[úÌ ø ). These step sizes are selected is integrated with a large steps size (
dynamically to maintain a desired level of the estimated integration error while maximizing the efficiency of the simulation. Recall that in the traditional simulation all components would have been integrated with a global step size corresponding to the fastest component (
¤4úQ[úÌ ø ).
In
general the speed up factor is a function of several parameters, especially the desired integration accuracy. Figure 6.8 depicts this relationship. In this particular example the efficiency gain is more dramatic than the conservative estimate indicates in Figure 6.5. This is because, while the analysis was performed assuming the ratio of step sizes used in neighboring layers in the hierarchy was two, this particular example permitted ratios as high as 8. Notice that the benefits of multirate simulation increase as the error tolerances become tighter.
6.6
Conclusions
In this chapter we have introduced multirate predictor corrector techniques as a tool for simulating robotic systems which inherently posses multiple time scales. The method is shown to have accuracy on the same order as that of the traditional predictor-corrector pair and the same stability 127
properties, for the class of systems discussed here. The efficiency analysis indicates that a significant reduction (a conservative analysis indicates 30-80 % for some representative systems) of computation time is possible, when the underlying system to be integrated is sufficiently complex. Indeed even in the relatively simple example of a mobile robot, solved in Section 6.5, 73 % fewer computations were used. Other robotic systems could potentially benefit from this type of simulation. For example, in compliant contact models for rigid body simulation the compliant contact point represents the fast dynamics while the gross motion is comparatively slow. Hierarchically abstracted systems such as the one presented in [36] bear an obvious connection. This may be particularly useful in underwater undulatory locomotion systems, as indicated in [49], whose most detailed dynamic models may even require fluid flow solvers making them very expensive to evaluate.
128
Initial Conditions
Initialize
Predictor
Eval. RHS
Reduce h
Corrector Estimate Error Compute new h based on error Error acceptable? NO YES Eval. RHS
Store Results
Figure 6.3: A flow chart of the traditional Predictor-Corrector numerical integration algorithm.
129
Initial Conditions
Initialize
Select slowest Subsystem: x_{i}
Predictor Eval. Interpolants for x1, ...x_{i-1} Eval. RHS
Reduce h_{i}
Corrector Estimate Error Compute new h_{i} based on error Error acceptable? NO YES Eval. RHS Construct Interp. for x_{i}
Store Results
Figure 6.4: A flow chart of the multirate Predictor-Corrector numerical integration algorithm.
130
1.2
1
Efficiency
0.8
3−Level
0.6
4−Level 5−Level
0.4
0.2
0
50
100
150
200
250
300
350
400
450
500
FLOPS for RHS
Figure 6.5: Efficiency gain as a function of the cost of evaluating the right hand side.
0.4
Follower robot
Nonholonomic kinematic cart
0.2
0
Y
Full dynamic model −0.2
Desired trajectory (unconstrained system) −0.4
−0.6
−0.5
0
0.5
1
X
o g fþat
Figure 6.6: Simulation results for the hierarchically controlled cart. The coordinates are plotted at each level. The multirate simulation algorithm requires only 39 % of the computation time used by the Matlab built in integration scheme ode45.
131
Level in hierarchy
4
3
2
1
0.2
0.4
0.6
0.8
1
1.2
1.4
1.6
1.8
2
Time
Figure 6.7: Illustration of the frequency with which the different levels in the hierarchy are integrated. The step size used for the first level is approximately 0.6, while the step size for the lowest level is 0.015. The step size is selected automatically to maintain the estimated errors at a desired level while seeking maximum efficiency.
0.36
0.34
Efficeincy Ratio
0.32
0.3
0.28
0.26
0.24
0.22 −8 10
−7
10
−6
−5
10 10 Relative accuracy
−4
10
−3
10
Figure 6.8: Experimental results for the mobile robot example problem on the efficiency gain of the multirate technique as a function of the desired integration tolerance.
132
Chapter 7
A hybrid system framework for reactive motion planning This chapter departs from the simulation work described in earlier chapters. It describes a framework for reactive motion planning for mobile robots in dynamic uncertain environments. In particular, it is concerned with high level control tasks which are posed as achieving several simpler control objectives in “parallel”, when one of the tasks at hand can be posed as a stabilization task (deliberative, “go-to-goal” type behaviors) and the remaining can be posed as reactive constraints – descriptions of what not to do. Such task descriptions often arise in robotics. For example a manipulation task might be phrased as: “Go to the goal configuration while avoiding obstacles; traveling in formation; and maintaining contact with the object”. The framework here is focused on developing reactive solutions to such tasks, rather than global ones, since the dynamics of these constraints are often dynamic and unpredictable. It is assumed that a basic set of simple controllers exists to accomplish each objective in isolation. The task of composing these simple controllers into a high level system whose collective dynamics satisfy the high level objective is addressed as a hybrid control system design problem. 133
The design is accomplished through a combination of sequential composition (i.e., mode switching) and parallel composition of the basic control laws. The topic of parallel composition requires delicate treatment so as to ensure the aggregate system behaves as expected.
7.1
Problem statement
Given a fully actuated, holonomic, kinematic robot
whose configuration is described by
.
(in later sections we will effect of relaxing this assumption), then, assuming the configuration space
is locally diffeomorphic to
à
, the dynamics of
are
z
, where
à
, the
space of all possible actuating inputs. We call the configuration velocity. Let
´ h
, for
n z Ì#
, be the set of configuration space obstacles. We assume these
obstacles are known by the robot a priori. The free space
z h y ´ h
represents the set
of all configurations which the robot can physically assume. Note that these obstacles are static, therefore their set descriptions are not functions of time. Other static constraints on the robot’s configuration, which may not represent physical objects, can also be modeled in this way. The deliberative objective of reaching the goal without hitting obstacles is not time dependent, therefore we refer to it as the static objective. It can be phrased as finding a function
orq!t Ð
as
q Ð
and
such that
orq!t Ñ q ¬ úQ ® . We refer to this stabilization objective as
. Note
that this is the basic motion planning problem. Let
ì , z Ìx
be a collection of sets in
which the robot should not enter. Unlike
the deliberative objective these objectives may change with time and are therefore referred to as dynamics or reactive constraints. These sets can be described as
o fq!t"!,ú . We assume the robot
has no knowledge of the time-dependence of these constraints, but is equipped with sensors which enable it to measure for
z Ì#
and either
be referred to as
or
.
×× Æ
online. Let the objective of satisfying each
$ such o orq!t.fqft#!=ú , 'Ñ z¿Ì( , Ñ q L¬ úQ ® . Simply
The reactive motion planning problem can then be stated as follows. Find an input that
orq!t Ð %
as
q Ð&
and
orqft
and
o fq!t#!^ú
134
put the goal is to design a controller to solve the basic motion planning problem while complying with the dynamic constraints
y # l
. An input which achieves this is referred to as a nominal
solution to the reactive motion planning problem. In some situations it may turn out that some of these objectives are mutually exclusive in which case no nominal solution exists. In such a case some objectives must be discarded. To facilitate this we assume that the robot has been assigned some priority to the various objectives. This priority enables one to order the objectives
7.2
) y x ~l
from most critical to least critical.
Discussion
In all cases considered in this work, the static objective will be a “go-to-goal” task. Typically the dynamic constraints could represent moving obstacles such as humans or other robots, but could also represent a wide variety of other criteria. Other examples would include maintaining proximity to teammates so as to stay within the range of a wireless communication system, maintaining line of sight with a target, traveling in formation, or maintaining force or form closure with an object. A special case of the constraints would be when
× +Æ * z=ú . This represents static constraints ×
which were not accounted for in advance. The approach presented here is an attempt to bridge the gap between traditional algorithmically complete path planning, which have performance guarantees but lack robustness, and reactive or “behavioral” approaches, which are robust but lack performance guarantees. Note that the dynamic problem is different from optimal control approaches to motion planning because the dynamics of the constraints are completely unpredictable and must be sensed on-line. However, a differential game approach would yield a solution to the problem. Unfortunately such approaches tend to be intractable for all but the most simple constraints. Instead, we chose to solve this dynamic motion planning problem using a reactive hybrid control system, meaning that we assume the robot has already computed a nominal motion plan for the static scene and seeks to locally modify that motion in an on-line fashion, in response to unpredictable changes 135
in the environment. The motivation for this approach is: (1) global re-planning is expensive and therefore one would like to rely on reactive solutions whenever possible without sacrificing completeness; and (2) since the robot has no prior knowledge of the time-dependence of the constraints it would be impossible to account for them up front.
7.3
Approach
With regard to the static problem we choose to base our methodology on Navigation functions [78] because they represent an algorithmically complete closed loop solution to the planning problem. Typically, the robot’s input
is selected such that
z -, )
, which causes the robot to reach
the goal and halt while avoiding obstacles (and hence represents what we call a solution to the static planning problem). We assume that navigation function constructed by taking into account
´ h . The navigation function ) o t is defined such that by using the controller /.+z 0, ) o t. (7.1) the system z1 /. , will satisfy orqft Ð % as q Ð2 and orq!t ,Ñ q . an initial map of
With regard to the rective objectives, each reactive objective can be satisfied in isolation by selecting
Since
such that
ÔÖ ú
(7.2)
q (7.3) ² ² ² ² the controller 3* zopÎ y ¶ q t ¬ ® (7.4) ² ² ² the initial condition satisfies the forces the system z4 to satisfy o orq!t.fqft657úQ Ñ q ² , provided +* constraint o ú?t"!,ú and the constants y 5,ú and z Ì é o87 × 7 ¶ t . ¶ ×9
z
In light of these two observations, we have a controller which satisfies the static objectives, as well as a set of controller which satisfies each of the reactive objectives in isolation. The problem 136
remains how to achieve more that one objective simultaneously using either one of the two control modes mentioned above in sequence or some new mode which represents a parallel composition of these two basic modes. The key observation which we exploit to solve the above problem is that
) o t , which solve the static problem, are actually Lyapunov functions. The traditional selection of such that z 0, ) is not the only input capable of rendering ) !7ú ; there is in fact a uncountable set of such inputs. We exploit this freedom in choosing to satisfy
Navigation functions,
the additional dynamic constraints whenever required.
7.4
Parallel control law composition
Before describing our overall hybrid controller we address the issue of creating more sophisticated control laws through the composition of simpler ones. It has been observed in Section 3.2 that there are two ways of composing feedback laws. One approach is sequential composition. Mode switching would be considered sequential composition since feedback laws are applied in a mutually exclusive fashion in some particular sequence, implicitly determined by both the switching logic and the continuous dynamics. On the other hand the concept of parallel composition is much more nebulous. The word parallel implies that in someway two control laws are used concurrently to produce an entirely new control law. Naive approaches would include simply taking the weighted sum of the two constituent vector fields or perhaps some suitable product of the two inputs. However it can be easily shown that if the weights are not chosen with care, the summation of two inputs can easily produce an input which satisfies neither objective. It is important to note that our discussion addresses local composition. We do not consider a global control law redesign. In this chapter we present what is, in our opinion, the only existing principled approach to local parallel composition. First we derive the set of possible control inputs, based on
/.
which can
accomplish the “go-to-goal” behavior. Borrowing terminology from the numerical optimization literature, we call this the set of stabilizing directions. We then describe the set of control inputs, 137
based on
3* , which satisfies the constraints – the set of usable directions.
Ultimately it is the
inputs which lie in the intersection of these two sets, the set of feasible directions, which will satisfy all the task objectives. The only meaningful, and correct, parallel composition of multiple constituent control inputs lies in the set of feasible directions. The set of feasible directions is essentially the set of all vector fields for which decrease both
7.4.1
and
.
Stabilizing directions
As mentioned earlier we assume that a Navigation function, the static problem (i.e., a function that steers the robot to ically, the robot’s input
)
)
is selected such that
z: /.
)
has been constructed which solves while avoiding
where
;.>zd-, )
´ y #.´Îl
). Typ-
. Due to the nature of
, this causes the robot to reach the goal and halt while avoiding obstacles. Navigation Functions
z< ;. , because ) o t is positive definite by construction and, by definition of the control policy, the value of ) is always decreasing along
can be though of as Lyapunov functions for the system
system trajectories
) z=, ) . z 0, ) , ) Ö úQ
(7.5)
It is apparent however that this control policy is not unique – any control policy which renders
) z -, ) Ö ú
also solves the planning problem. This fact is observed in [50] and in [28]; the
set of all input vectors which decrease some cost-to-go function is termed the “cone of progress”. However in both of these contexts the fact is used passively to address sensor uncertainty. Here however we wish to actually construct a parameterized set of directions which solve the static planning problem. We define this set as
â z ?> , ) Ö úQ@ $ £ Proposition – If
Aà Ü
(7.6)
, define mutually perpendicular vector fields
Ì ä ùÌ , which are also everywhere perpendicular to /.ço t . vectors has been normalized and is of unit length. See Figure 7.1. 138
¬ ;. o t®ChB
, where
n z
Further assume each of these
EDEDED DEDEDE DEEDED DEDEDE EDEDED DEDEDE EDEDED DEDEDE EDEDED DEDEDE EDEEDD DEEDDE EDED DEDE
Perp(V)1
Perp(V)2
EDEDED DEDEDE DEEDED DEDEDE EDEDED DEDEDE EDEDED DEDEDE EDEDED DEDEDE EDEEDD DEEDDE EDED DEDE
EDEDED DEEDED
V=const
EDEDED EDEDED EDEDED EDEEDD EDED
-Grad(V)
/.>zd0, ) , and ¬ ;.® B z ¬ -, ) ® B ) o t.
Figure 7.1: An illustration of the vectors also decreases vector in the same half plane as
;.
in
à ñ . Any input
â
Then the set, , of control laws
Ü w*y â z zop̹ F h ¶ t Hy G ¶ ;.A h y
Ü w*y Ü w*y F ¬ ;.® B > F ¶ !Ì@ I £ h h y h h y h also solves the static planning problem; and the function for , parameterized by F all the directions in â is denoted by ;J . K The set of vector fields space as
0, )
LJUo t
(7.7) which generate
represents all the vectors which lie in the same half-tangent
. To prove the proposition we show the robot: (1) reaches the goal; and, (2) does
not hit any obstacles. First we prove that
Ð % :
) o t serves as a common Lyapunov function for the equation zM ;JUo t regardless of the values of F since h Ü w*y Ü w*y H y G F ) z , ) opÔo h ¶ ÕÌ#t ¶ ;.A F h ¬ /.® hB t y h y Ü w*hy yHG z ÔopÌö F h ¶ t ¶ , ) o t , ) o t Ö ú h y * w y Ü provided N y F ¶ !Ì . Note that the new control law is free of local minima since the ) zîú iff h h z by definition of the navigation function. K To show that the robot does no «â contacts the obstacles: Proof 2: If the obstacles are defined by a closed surface let ä o Ôt be the unit normal pointing Proof 1: Observe that
toward the interior of the free space. Then proving that the robot does not hit the obstacles is 139
equivalent to proving
äI O ú
z LJUo t
1 Ü w*y yHG
z ofopÌö F h ¶ t ¶ /.A h y using
Ü w*y F h y h ¬ ;.® hB t ä
(7.8)
;.
Recall that navigation functions are uniformly maximal on the boundary of the free space, so
;. ä o tQ5,ú and .B ä o t}z=ú . Therefore Eq.(7.8) becomes Ü w*y yHG op̹ F h ¶ t ¶ oR ;. ä t O Qú h y K Thus all controllers in the set J solve the static planning problem. w*y Finally we add that since the set of stabilizing inputs â z ;JUo t%> N y F ¶ !cÌ £ results in h h a set of closed loop systems which share ) o t as a common Lyapunov function, it can be shown is parallel to
ä o Ôt
for all
P
, so
(see [66]) that a system whose right hand side switches between these inputs is also stabilizing, regardless of the nature of the switching sequence since every direction in the set decreases This implies that we are free to choose the values of
F
)
.
online, in a possibly discontinuous or time
varying fashion, without affecting the overall stability of the system (with respect to
% ) or the
completeness of the solution.
7.4.2
Usable directions
Once a reactive constraint becomes active,
êÖ ú . The time derivative of
O ú , our goal is to select an input which makes
is conveniently expressed as the sum of two quantities:
(7.10) ² ² q ² ² the first term represents the robot’s own influence on and is assumed to be known; the second represents the dynamic nature of and must be either sensed online or some assumptions must
o orq!t.fqftz
be placed on its value. We assume the robot has an expression for
and is equipped with sensor enabling it to
measure its value online. In this work we only consider what are referred to in the optimal control 140
literature as first order constraints, that is constraints for which
S ú ÑT ; although the extension × +* z= ×9
for higher order constraints is straightforward. Therefore the Usable set of directions,
U
is described by
Ö C úQ@ $ £ (7.11) ² ² q#V ² ² One can construct an explicit parameterized description of in much the same way as was done for â . 7z ? >
7.4.3
Feasible directions
Finally, tangent directions which are consistent with both the stabilization criteria and the constraints are termed the set of feasible inputs.
W z X> o âZY Ôt £ that is,
W
represents all choices of
(7.12)
such that
W z ?> ) !ÕúQ z C q !,úQ@ [ £ ² ² ² ² ² ²
Geometric insight behind the problem can be gained from recognizing that the defines a cone,
ï
]\ ¼
inequality
(or the complement of a cone) with its apex at the origin in the tangent space
of the body fixed frame; while the set of vectors
â z ;JT>@N Üh w*y y F h ¶ Ö Ì £
à ¶. z ` It should be said that if â_^1 z â_^ ï y ^ ^ ï 4
defines a half space.
Figures 7.2-7.3 illustrate this in
there is no input that can simultane-
ously solve both the static planning problem and satisfy the reactive constraints. If the cone is not empty, an infinite number of solutions exist. We defer the discussion of how to actually compute vectors
W
to Section 7.6, however for now several observations can be made. For the sake of
clarity we illustrate these observations for the case of a single constraint with no time dependence (i.e.,
× × \ õ zîú )
141
-Grad(V)
Feasible Half Plane
V=const
Figure 7.2: In the absence of constraints, the set of feasible directions is the half plane containing .
-, )
-Grad(V)
Feasible Space Grad(G1)
V=const
y
Figure 7.3: The addition of a constraint, , with no time dependence further constrains the set of and . directions to the intersection of the half spaces containing
If lie in
W
/. õ O ú
computing a vector in
W
0, )
, y
is trivial. Any convex combination of
;.
and
will
.
W z zop̹batH ;.cad > ú Ö a Ö Ì £ õ If
õ
a zùú , for example, «z= /.
and the system will go the goal quickly; if
(7.14)
a zcÌ , «z
and the
system decreases the constraint more rapidly.
/. 3* Ö ú , a solution does exist, but it does not lie in the convex hull. W In such a case one is free to use some optimization criteria to select which member of is most If on the other hand
142
e_fhg
desirable. For example
ikjml ¬&i o @ t 0, ) ® 7 n7-!A ;oqpsr
(7.15) (7.16)
Note that Eq.(7.15) restricts ’s magnitude to ensure that the objective function is bounded (perhaps dictated by
t
). In this case determining a vector in
W
can be phrased as a quadratic or
linear program (depending on the norm used to bound the magnitude of ).
. ;3* Ö ú
y
> . ;3*u>Uzv> . > > ; õ > ). W . and y . By selecting values of In this situation, is actually a hyperplane normal to both / in this plane, both ) and y remain constant, as the system “slides” on this hyper-surface. There is an apparent similarity between this controller which forces the system to evolve on a w Ì A special case of
is when
.
and
are anti-parallel (
dimensional manifold and true sliding mode control; however the two should not be confused. In sliding mode control the goal is to stabilize the system to this surface; where as in our case, stabilization is not the goal per se – the system may or may not return to this surface if perturbed depending on the overall state of the controller. This surface is important because it coincides with the level surfaces of
and
)
, and vectors tangent to this surface represent the only viable
motion directions if neither constraint is to be increased.. This special case is worth mentioning for several reasons. 1. The numerical optimization method discussed later is not guaranteed to produce a vector which lies exactly on this plane, therefore post-processing is required. 2. In practice it is impossible to force the system to “slide” on this surface. A chattering phenomena will occur and it is possible that the constraints may be violated by a small margin during the chattering. 3. Even in the ideal case where the sliding controller is perfectly implemented, both
)
and
are merely remaining constant rather than decreasing. Therefore a system which stays in a sliding more indefinitely is only marginally stable with respect to 143
)
and
.
Finally, it should be noted that, in this case, if only one objective was considered, one could merely use the given control laws
W
;.
or
. If two objectives are considered simultaneously, then
is never empty. The sliding solution represents the limiting case. Therefore if the number of
å
objectives is less than or equal to a solution always exists. Finally if three or more objectives are considered it is possible that no solution exists. However if
× 3\ * 5 ú ×
the existence of a solution in
general depends on the nature of the bounds on the input vectors. The optimization method can be used to determine this.
7.5
Control architecture
The goal is to design a hybrid automata which will solve the reactive motion planning problem. In this section we outline all of the system’s modes, including the control laws used in each, the guards which enable transitions between them as well as their connectivity. The basic system modes consist of the Static Objective Mode which is active when no constraints are active; a Superposition Mode when one or more constraints are active however a suitable input can be computed as the convex hull of
/.
and the
’s; a Parallel Mode in which W
does not lie in
the convex hull of the vectors given by the basic controllers; and finally a Sliding Mode in which at least two of the objectives being considered are anti-parallel and hence a special solution is required. First we must determine a reactive condition which determines which
’s should be taken
into account at any given time. Since the reactive constraints are unpredictable in nature we do not always take them into account. As an objective measure of when to react to changes in the environment we introduce a quantity (
o orqft.fq!tTzîú
)
x q
which is an estimate of the time to constraint activation
x q z o o orqfort.qft.fq!fq!t t
(7.17)
This quantity also encodes information about the kinematics of the robot. Small positive values of
x q
imply that a constraint activation is impending; while negative values (moving away from the 144
zûú ) or large positive values are not a cause for concern. Thus if ú Ö x q CÖzyf , is added to the list of active constraints, where yf is some predetermine constant termed the look ahead time. Of course there are no hard and fast rules for selecting yf , however the top speed
level surface
of the robot and its maximum sensor range help determine “natural” values. Throughout the ensuing discussion of the constituent control modes, the reader is referred to Figure 7.4 which depicts the overall architecture of the control system.
add/rem. Constr. No Constr!
mode: stablize Inv : g j < 0, ∀j
mode: superposition Inv : (uv • u g j ) > 0, ∀j ∈ P
Flow :q = uv
(uv • u g j ) > 0
Flow :q} = (1 − α )uv + αu g j
No Constr!
(uv • u g j ) > 0
No Constr!
mode: slide Inv : ∃j ∈ P, | uv • u g j |=| uv | ⋅ | u g j |
~
uv || u g j
Flow :q = u slide
add/rem. Constr.
(uv • u g j ) < 0
(uv • u g j ) < 0
uv || u g j
infeasible! rem. constr.
mode: parallel Inv : (uv • u g j ) < 0, ∀j ∈ P
|
Flow :q = U para (uv ,u g1 , { , u g P )
(uv • u g j ) < 0 add/rem. Constr.
infeasible! rem. constr.
Figure 7.4: A graphical representation of the resulting Hybrid Automata. The connectivity of the modes can be understood from this diagram.
7.5.1
Deliberative Objective Mode
In this mode none of the reactive objectives are considered active and the system simply implements the deliberative or static control law
/. , in which the robot attempts to travel to the goal
configuration, using its internal map, without colliding with known obstacles. Specifically, the 145
mode could be described as
nH ï.¾ k q k â sq çq k z Ì W Ò
z1 ;.?X ;. z × ×9æ 9 è ä Z o ú Ox q t ão x q O yf t. Ñ' z Ìx
(7.18)
5 ú?t. â ´#
â ] _ ß > o ú Ö x q dt ão x q GÖyf tdãoR /. ,
â ´#
ß ç¾ k Z ß > o ú Ö x q tão x q FÖyf tãoR ;. ÔÖ ú?t. â ´#
~â9ÒMnHçk6k ß > o ú Ö x q t ão x q FÖyf tão> ;. ~> z> ;.> > &> t The system remains in this mode so long as no constraints become active. Once a constraint becomes active it may transition to either the Superposition Mode if Mode if
oR ;. ÔÖ ú?t ; or the Sliding Mode if o> ;. > z> ;.> > &> t .
7.5.2
Superposition Mode
In this mode, some set of constraints, combination of
.
and
ß
ß
, are active however
oR /. 5cú?t ; the Parallel
oR /. 5Õú?t. Ñ ß
so any convex
will achieve all the objectives.
nH  k q kxâ sq çq k6 W Ò
z å
z1 / zop̹N jh a tH . Aa ä Z o ú Ö x q tdão x q GÖMyf t. Ñ' ß
u ³o ú OMx q Á t ão x q Á O Åy Á .t ÑjÒ ß ¹oR ;. t O ú Ñ Z ß 146
(7.19)
â ];
â ´ o ú Ox q dt $o x q O fy .t Ñ Z ß â L
ß ç¾ k Z ß > oR /. ÔÖ ú?t â L
âÒrn çk6¡k Z ß > o> ;. >~z> ;.> > >&t â ]L
â ¢Ò£ ß u > o ú Ö x q Á tão x à W! ß o ú Ox q tdNo x à |f{ ¥ . ?ß > >&t ß ?¾h
ß ç¾ ¢QÒ? ß u > o ú Ö x q Á tão x q ÁÖyÅÁ t. à W! ß o ú Ox q tão x q O yf t à |f{ ¥ . ß4¤ ß äd
â?
Ò W Òª z^q¾m k à Ü ¥@«?¥ Á ?ß ;. Us>~z> ;.> > Us>
âÒrn k
â ´ o ú Ox q dt No x q O fy .t Ñ' Z ß âÒrn çk
â ] oR /. O ú?t. Ñ' ß âÒrn çk
ß ç¾ ß oR ;. GÖ ú?t.8¹o> ;. >! > ;.'> > &> t. Ñ' _ ß âÒrn çk
âÒrn çk°QÒ£ ß u > o ú Ö x q Á tdNo x q ÁíÖcyÅÁ t. à .! ß4¤ ß Ò k Z ß > o ú Ox q t§ão x q O y t à |p{ ¥ . ?ß y > é o ¶y ¶¶ t . As is the case for Navigation functions,
various coordinate transformations are used to create a field which steers the system around stationary obstacles. Using this methodology, potential fields can be built for nonholonomic systems in static environments which share many of the completeness properties of Navigation Functions. This idea is developed further in [89] where an associated controller is introduced that, in the
¾
case of a kinnematic cart-like system, stabilizes so that the nonholonomic system is able to track the gradient direction. This controller is developed through a backstepping approach, by looking at how a holonomic system would move under the influence of the dipolar field and then deriving a controller for the nonholonomic system which would mimic the trajectory of the holonomic system. This approach is approximate in that, because the underlying controller will have a transient period and may not have a zero steady state error, the nonholonomic system will not mimic the underactuated system precisely at all times. In theory it is always a possible to mimic the ideal trajectory of the holonomic system by having high enough gains and a large enough lookahead 156
time but it is difficult to actually select these values since they are problem dependent. During those intervals where there is a tracking error it is possible that some of the constraints could be violated. Never-the-less, this approach may prove to be a acceptable solution in practice depending on the application. 1.2
1.2
Start
Start
1
1
0.8
0.8
Unmodeled Obstacle
Unmodeled Obstacle
0.6
0.6
0.4
0.4
0.2
0.2
0
0
−0.2
−0.2
−0.4
−0.4
−0.6
−2
−0.6
−1.8
−1.6
−1.4
−1.2
−1
−0.8
−0.6
−0.4
−0.2
0
−2
−1.8
−1.6
−1.4
−1.2
−1
−0.8
−0.6
−0.4
−0.2
0
Figure 7.7: (L) The robots nominal trajectory would cause it to bump into the unmodeled obstacle (dark circle). (R) The dynamic planning methodology enables the robot to steer around the unexpected obstacle on-the-fly.
This idea was tested in simulation, the results of which are shown in Figure 7.7. In the scenario depicted there, a unicycle type robot with the kinematic model
¿ÀÀ
ÂÄÃ ÀÀÁ g ÃÃÃ þ Å z ¾
¿ÀÀ ³± ² ÂÄÃÃ ÀÀÁZÆÈÇ]É ¾ ÃÃ É ¾ Å ú
¿ÀÀ
ÂÄÃ ÀÀÁ ú ÃÃÃ ú ÅÊ Ì
(7.38)
has an initial map of the environment which does not contain any obstacles. It uses the dipolar potential field and controller from [89] as a static solution. The left frame of the figure shows that the nominal trajectory would have steered the robot on a collision course with an obstacle that is unmodeled initially but detected at run time. The right panel shows the trajectory of the robot using the methodology outlined in this paper, once the robot detects the obstacle it is able to steer around. The concentric circles in the figure are level sets of the potential field. One can see that at all points along the trajectory, the potential function is decreasing, thereby ensuring stability. 157
7.11
Conclusion
In this chapter, the problem of motion planning in environments with both known static obstacles and unpredictable dynamic constraints was considered. It was assumed that a set of basic control laws was given and a reactive approach to the dynamic motion planning problem was adopted. A hybrid control system was presented as a solution. When none of the active constraints are considered active, the system resides in a mode which simply guides the system according to the basic “go-to-goal” control law. Once constraints become active the system transitions to new modes which represent provably correct parallel compositions of the given set of basic control laws. The control directions give by parallel composition of two controllers are guaranteed to decrease both objectives simultaneously. Depending on certain geometric conditions (guards) on the objectives, various methods for computing vectors in this set are given. Each computation method represents a different mode. In the most general case synthesizing the new input directions requires a numerical optimization algorithm. Because the underlying optimization problem is always linear or quadratic it can be solved relatively inexpensively in a finite and predictable number of steps, when a solution exists. As long as a solution to the optimization problem exists, the overall correctness of the hybrid controller can be shown. If at any point in time the set of control directions given by the parallel composition criteria are empty, a flag is raised by the solution algorithm indicating that the underlying objective functions are unreconcilable and the completeness of the solution cannot be guaranteed. The effectiveness of this control approach was demonstrated on two dynamic planning examples. In the first example the robot was required to avoid other static and moving obstacles. The second involved traveling in formation with other robots, in the absence of explicit communication among them. In the first part of the chapter it is assumed that the robot’s dynamics take the form
zv
.
Since the algorithm essentially consists of finding a velocity vector at every point along a trajectory which will decrease multiple objective functions, some discussion of the case where the system’s 158
dynamics are more restrictive was required. In any case where the system’s dynamics are full state linearizable the original control solution is applicable. In cases where that is not possible such as under-actuated and nonholonomic systems, the dynamics (and hence the velocity constraints) can be handled directly in the optimization algorithm as additional constraints; or an indirect approach can be adopted. The indirect approach consists of using an potential function suitable for nonholonomic systems along with a back stepping approach to control, in which the desired directions are computed and a controller is designed to force the nonholonomic system to track these velocities. Regardless of which solution is used, there are no longer any guarantees on the completeness of the solution when the system is nonholonomic.
159
Chapter 8
Conclusion 8.1
Summary
As the price-performance ratio of embedded processors increases and the size of chips decreases, more sophisticated paradigms for software based control have evolved. This trend is particularly evident in mobile robotics because these systems typically have significant onboard computing power, complex mission requirements and the need to process data coming from many types of sensors in real time. Accordingly, two increasingly popular design methodologies have emerged: modularity and hierarchy. Modularity involves designing many basic control laws (modes), possibly along with several plant models, and switching between them as needed to achieve more complicated control objectives. Hierarchy is used as a way to overcome model complexity. A series of increasingly detailed plant models are designed and different models are used for different phase of the planning and control process, with the goal being to maximize the trade-off between optimality and complexity. Systems designed using both of these concepts are referred to as Hierarchical Hybrid Systems. As mentioned earlier, this dissertation is concerned with two primary topics unified under the theme of complexity management in control system design and simulation. In the first part of the dissertation three novel numerical techniques for simulating Hierarchical 160
Hybrid Systems are introduced. The first of these techniques addresses the problem of detecting and locating discrete transitions (i.e., mode or model switches) which occur during the course of a simulation, especially switches which occur near a model singularity - an area in which all competing event detections methods fail. The remaining two simulation techniques introduced here seek to circumvent one of the major causes of computational bottle necks in large-scale simulations – the requirement that all differential equations in the system be simulated using a single global choice of step size. In one case differential equations with right hand sides that are partially coupled, without consideration of the mode switches, are considered. A multi-rate PredictorCorrector method is presented which can significantly reduce the computation time required for certain simulations. The third algorithm is concerned with simulating so-called multi-agent hybrid systems. These systems have decoupled sets of differential equations yet the inequalities which signal the mode switches are coupled. Here the challenge is properly detecting transitions when the numerical integration of the various differential equations is performed asynchronously using many local time clocks.
In the second part of the dissertation a framework for reactive motion planning and control of mobile robots is presented. It is our approach that reactive control and motion planning schemes for mobile robots are best modeled as Hybrid Systems. We assume a suite of low level controllers have been designed to accomplish basic tasks such as “Go-to-goal” or “Avoid-obstacle”. The problem is how to compose these controllers, either through designing a switching sequence or by synthesizing entirely new control modes, in such a way that the aggregate behavior of the system achieves all of the objectives simultaneously, or reports failure if it cannot. The numerical algorithm controller synthesis represents a rigorous approach to modular control law composition. 161
8.2
Contributions
Regarding the work presented here on simulation, it represents the first effort to introduce a comprehensive set of tools specifically developed to accommodate the special properties of Hierarchical Hybrid Systems, such as the presence of multiple time scales and mode switching. Specific contributions are listed below.
Þ
The introduction of the only known event detection method of capable of localizing discrete transitions occurring at or near model singularities encountered during the course of simulating a hybrid system. All existing methods fail because they attempt to interpolate through the singular region. The method is guaranteed to locate all events, including even numbers of events in a given integration step and events expressed as the boolean conjunction of inequalities, modulo the accuracy of the integration method itself. The computational cost of the method is only slightly more (at most
ËkÌ
more) than that of the best known event
Þ
detectors. This seems to be a reasonable price to pay for the marked increase in robustness. The only known general purpose algorithm for simulating multi-agent hybrid systems in an asynchronous fashion is introduced. The systems we are concerned with, such as teams of multiple mobile robots or automated highway vehicles, have decoupled continuous dynamics and coupled discrete dynamics. Asynchronous simulation permits the use of different step sizes for different agents, dramatically increasing the simulation’s efficiency, especially when the number of agents is large. Despite this asynchronicity, the algorithm meets the challenge of being able to correctly detect events when the discrete dynamics of the agents are coupled. It was shown that for certain representative problems the computation time increases proportional to the square of the total number of agents, in contrast to traditional simulation methods which scale exponentially with the number of agents, representing a
Þ
marked improvement over traditional algorithms. The first multi-rate Predictor-Corrector numerical integration scheme is introduced. It is 162
well suited to simulating hierarchical systems evolving on different time scales. Analysis of it accuracy and stability are shown to be comparable to that of the traditional method while it is significantly more efficient. The efficiency improvement over traditional algorithms increases when the cost of evaluating the continuous dynamics is large. From the point of view of motion planning, the contributions of this thesis are as follows.
Þ
The reactive motion planning problem is modeled in a hybrid system control synthesis framework which is capable of accommodating multiple unpredictable dynamic objective functions (e.g., “avoid-obstacle”, “follow-leader”) along with a primary deliberative (i.e.,“goto-goal”) objective. Using Lyapunov stability, conditions are derived under which a hybrid control system is guaranteed to meet all of the specified objectives.
Þ
A computational algorithm, based on the Method of Feasible Directions, which is capable of synthesizing such inputs numerically is presented. This algorithm essentially determines which low level controller the system should use at a given time, or, if needed, the algorithm synthesizes a new control mode which represents a rigorous approach to parallel control law composition. If it is impossible for the system to meet all the specifications simultaneously the algorithm reports failure rather than producing erroneous results.
8.3
Future work
As it becomes increasingly clear that progress toward general purpose automated symbolic analysis of hybrid systems is ultimately limited by results on the undecidability of such systems, numerical methods are playing an increasingly important role in automated analysis and verification of hybrid systems. Hopefully, the simulation methods here will provide the building blocks for the next generation of automated analysis tools for hybrid systems. One specific area which appears to have much potential is in exploring the possibility of implementing the asynchronous multi-agent simulation algorithm presented in Chapter 5 in a distributed 163
computing environment. Simulating each agent on a different processor could further reduce computation times for very large scale systems such as those envisioned by automated highway and automated air traffic control experts. One important capability would perhaps be the ability to dynamically partition the entire set of agents into groups of agents whose dynamics influence one another. This entire group could then be simulated on the same processor while other groups are simulated on other machines. This dynamic partitioning would reduce unnecessary overhead in communicating intermediate results between processors. The hybrid control framework for reactive planning seems to have much potential for designing controllers in cooperative multi-robot (i.e., multi-agent) tasks. Distributed manipulation is one example. Here the static planning problem is for several robots to orient a part in the plane. The reactive objective is to keep the object in either force or form closure while doing so. A major challenge is in designing each agent’s control strategy so that it is robust with respect to the failure of other agents.
164
Bibliography [1] Real-time motion planning using ratserizing computer graohics hardware, SIGGRAPH, Dallas, 1990. [2] A. G. A. Deshpande and L. Semenzato. Shift programming language and runtime system for dynamic networks of hybrid automata. California PATH, 1995. [3] A.Isidori. Nonlinear Control Systems. Springer, London, 1995. [4] J. Albus. The NIST real-time control system (RCS) an approach to intelligent systems research. Special Issue of the Journal of Experimental and Theoretical Artificial Intelligence, 9:157–174, 1997. [5] J. Andrus. Numerical solution of systems of ordinary differential equations separated into subsystems. SIAM Journal of unmerical analysis, 16(4):605–611, 1979. [6] I. M. A.R.C de Almedia and T.J.Ypma. Distributed-multirate methods for large weakly coupled differential systems. Applied Mathematics and Computation, 31:18–39, 1989. [7] R. Arkin. Behavior Based Robotics. The MIT Press, Cambridge, MA, 1998. [8] U. Ascher and L.Petzold.
Computer methods for ordinary differential equations and
differential-algebraic equations. Society for Industrial and Applied Mathematics, Philadelphia, PA, 1998. 165
[9] V. Bahl and A. Linninger. Modeling of continuous–discrete processes. In M. D. Benedetto and A. Sangiovanni-Vincentelli, editors, Hybrid Systems : Computation and Control, volume 2034 of Lecture Notes in Computer Science, pages 387–402. Springer Verlag, 2001. [10] L. D. Berkovitz. Variational methods in problems of control and programming. Journal of Mathematical Analysis and Applications, 3:145–169, 1961. [11] M. Branicky. Studies in Hybrid Systems: Modeling, Analysis and Control. PhD thesis, MIT, Cambridge, MA, 1995. [12] R. Brooks. A robust layered control system for a mobile robot. IEEE Journal of Robotics and Automation, RA - 2(1):14–23, March 1986. [13] Bryson and Ho. Applied Optimal Control. LOOK UP THE CORRECT INFO, London, 1971. [14] J. F. Canny. The complexity of robot motion planning. MIT Press, Cambridge, MA, 1988. [15] M. Carver. Efficient integration over discontinuities in ordinary differential equation simulations. Mathematics and Computers in Simulation, XX:190–196, 1978. [16] F. Cellier. Combined discrete/ continuous system simulation by use of digital computers: techniques and tools. PhD thesis, ETH Zurich, Zurich, Switzerland, 1979. [17] C.Engstler and C.Lubich. Multirate extrapolation methods for differential equations with different time scales. Computing, 58:173–185, 1996. [18] C.Engstler and C.Lubich. MUR8: a multirate extension of the eight-order Dormand-Prince method. Applied Numerical Mathematics, 58:173–185, 1996. [19] A. Chutinam and B. Krogh. Verification of polyhedral-invariant hybrid automata using polygonal flow pipe approximations. In F. Vaandrager and J. H. van Schuppen, editors, 166
Hybrid Systems : Computation and Control, volume 1569 of Lecture Notes in Computer Science. Springer Verlag, 1999. [20] C.I.Connolly and R.A.Grupen. On the application of harmonic functions to robotics. Journal of robotic systems, 10(7):931–946, October 1993. [21] C.O’Dunlaing, M.Sharir, and C.K.Yap. Retraction: A new appraoch to motion planning. In 15th ACM symposium on the theory of computing, pages 207–220, Boston, 1983. [22] C.W.Gear and D.R.Wells. Multirate linear multistep methods. BIT, 24:484–502, 1984. [23] C.W.Gear and O.Osterby. Solving ordinary differential equations with discontinuities. ACM transactions on mathematical software, 10(1):23–44, 1984. [24] T. Dang and O. Maler. Reachability analysis via face lifting. In T. Henzinger and S. Sastry, editors, Hybrid Systems : Computation and Control, volume 1386 of Lecture Notes in Computer Science, pages 96–109. Springer Verlag, Berlin, 1998. [25] E.Hofer. A partially implicit method for large stiff systems of odes with only a few equations introducing small time constants. SIAM Journal on Numerical Analysis, 13(5):645–663, 1976. [26] E.Klavins and D.E.Koditschek. A formalism for the composition of concurrent robot behaviors. In IEEE Conference on Robotics and Automation, 2000. [27] E.Klavins, D.E.Koditschek, and R.W.Ghrist. Toward the regulation and composition of cyclic behaviors.
In Fourth International workshop on teh algorithmic foundations of
robotics, Hanover, NH, March 2000. [28] M. Erdmann. Understanding action and sensing by designing action-based sensors. International Journal of Robotics Research, 14(5):483–509, 1995. 167
[29] J. Esposito and V. Kumar. Efficient dynamic simulation of robotic systems with hierarchy. In IEEE International Conference on Robotics and Automation, pages 2818–2823, May 2001. [30] J. Esposito and V. Kumar. On the performance of a multi-agent hybrid system simulation algorithm. submitted to IEEE Conference on Decision and Control, December 2002. [31] J. Esposito, G. Papas, and V. Kumar. Multi-agent hybrid system simulation. IEEE Conference on Decision and Control, December 2001. [32] J. M. Esposito, V. Kumar, and G. J. Pappas. Accurate event detection for simulating hybrid systems. In M. D. Benedetto and A. Sangiovanni-Vincentelli, editors, Hybrid Systems : Computation and Control, volume 2034 of Lecture Notes in Computer Science, pages 204– 217. Springer Verlag, 2001. [33] E.Tunstel. Coordination of distributed fuzzy behaviors in mobile robot control. In IEEE International conference on systems, man and cybernetics, pages 4009–4014, Vancouver, BC, October 1995. [34] E.W.Large, H.I.Christensen, and R. Bajcsy. Scaling the dynamic approach to path planning and control:competition among behavioral constraints. International journal of robotic research, 18:37–58, January 1997. [35] C. Gear and O.Osterby. Solving ordinary differential equations with discontinuities. Technical Report UIUCDCS-R-81-1064, Dept. of Comput. Sci., University of Illinois, 1981. [36] G.Pappas, G.Lafferriere, and S.Sastry. Hierarchically consistant control systems. IEEE transactions on automatic control, 45(6):1144–1160, 2000. [37] G.Schoner and M.Dose. A dynamics systems approach to task level systems integration used to plan and control autonomous vehicle operation. Robotics and Autonomous Systems, 10:253–267, October 1992. 168
[38] G.S.Joglekar and G.V.Reklaitis. A simulator for batch and semi-continuous processes. Computers in chemical engineering, 8(6):315–327, 1984. [39] K. Gustafsson. Control-theoretic techniques for stepsize selection in explicit runge-kutta methods. ACM transactions on mathematical software, 17(4):533–554, 1991. [40] K. Gustafsson. Control-theoretic techniques for stepsize selection in implicit runge-kutta methods. ACM transactions on mathematical software, 20(4):496–517, 1994. [41] D. B. H. Elmqvist and M. Otter. Dymola – user’s manual. Dynasim AB Research Park Ideon, Lund Switzerland, 1996. [42] J. Hay and A. Griffin. Simulation of discontinuous dynamical systems. In Proceedings of the 9th IMACS conference on simulation of systems, pages 79–87, 1979. [43] J.C.Gerdes and V. Kumar. An impact model for mechanical backlash for control system analysis. American Control Conference, Seatle, Washington, June 21–23, 1995. [44] J.Kosecka, H.Christensesn, and R.Bajcsy. Experiments in behavior composition. Robotics and autonomous systems, 19:287–298, 1997. [45] J.Lygeros, C.J.Tomlin, and S.Sastry. Multiobjective hybrid control synthisis. In Proceedings of hybrid and realtime systems, volume 1201 of Lecture Notes in Computer Science. Springer-Verlag, Grenoble, March 1997. [46] S. C. K.Benan and L. Petzold. Numerical solutions of initial value problems. North Holland, London, 1989. [47] K.Gokbayrak and C.Cassandras. Hybrid controlers for hierarchically decomposed systems. Hybrid Systems Computation and Control: Third international workshop, 3:117–129, 2000. [48] O. Khatib. Real-time obstacle avoidance for manipulators and mobile robots. International Journal of Robotics Research, 5(1):90–98, 1986. 169
[49] K.McIsaac and J.Ostrowski. Steering algorithms for dynamic robotic locomotion systems. Workshop for the Algorithmic Foundations of Robotics, page ???, 2000. [50] D. E. Koditschek. The geometry of a robot programming language. Workshop on the Algorithmic Foundations of Robotics, 3:263–268, 1994. [51] U. Kotta. Inversion Methods for Discrete-time Nonlinear Control Systems Synthesis Problems. Lecture Notes in Control and Information Science 205. Springer, London, 1995. [52] S. Kowaleski, M.Fritz, H. Graf, J.Preubig, S.Simon, O.Stursberg, and H.Treseler. A case study in tool-aided analysis of discretely controled continuous systems: the two tanks problem. In Hybrid Systems V, Lecture Notes in Computer Science. Springer Verlag, 1998. [53] G. Lafferriere, G. Pappas, and S. Yovine. Symbolic reachability computations for families of linear vector fields. Journal of symbolic computation, 32(3):231–253, 2001. [54] F. Lamiraux and J. Laumaond. Smooth motion planning for car-like vehicles. IEEE transactions on robotics and automation, 17(4):498–502, 1991. [55] S. Lavalle and S. Hutchinson. Path selection and coordination of multiple mobile robots via nash equilibria. Proceedings of 1994 International Conference on Robotics and Automation, pages 1847–1852, 1994. [56] L.F.Shampine, I.Gladwell, and R.W.Brankin. Reliable solution of special event location problems for ODEs. ACM transactions on Mathematical Software, 17(1):11–25, March 1991. [57] L.-J. Lin. Scaling-up reinforcement learning for robot control. In 10th international conference on machine learning, 1993. [58] J. Lygeros and G. Pappas. A tutorial on hybrid systems: Modeling, analysis and control. 14th IEEE International Symposium on Intelligent Control/Intelligent Systems and Semiotics, September 1999. 170
[59] N. McClamroch. A singular perturbation approach to modeling and control of manipulators constrained by a stiff envrionment. Proceedings of the
ÍkÎhϳÐ
Conference on Decision and
Control, pages 2407–2411, Dec. 1989. [60] M.Engerstedt. Behavior based robotics using hybrid automata. Hybrid Systems Computation and Control: Third international workshop, 3:103–115, 2000. [61] M.Gunther and P.Rentrop. Multirate row methods and latency of electrical circuits. Applied Numerical Mathematics, 13:83–102, 1993. [62] M.Humphrys. Action selection methods using reinforcement learning. PhD thesis, Computer Laboratory, University of Cambridge, 1997. [63] B. Mirtich. Timewarp rigid body simulation. In SIGGRAPH, 2000. [64] I. Mitchell and C. Tomlin. Level set methods for computation in hybrid systems. In N. Lynch and B. H. Krogh, editors, Hybrid Systems : Computation and Control, volume 1790 of Lecture Notes in Computer Science, pages 310–323. Springer Verlag, 2000. [65] R. Murray and S. Sastry. Nonholonomic motion planning: Steering using sinusoids. IEEE Transactions on Automatic Control, 38(5):700–716, 1993. [66] K. Narendra and J.Balakrishnan. A common lyapunov function for stable LTI systems with commuting A matricies. IEEE Transactions on Automatic Control, 39:2469–2471, 1994. [67] N. Nilsson. A mobile automaton: An application of artificial intellegencetechniques. In 1st international joint conference on artificial intellegence, pages 509–520, Washington, D.C., 1969. [68] C. Pantelides. The consistent initialization of differential algebraic systems. SIAM journal of scientific and statistical computing, 9(1):213–231, 1988. 171
[69] P. Pirjanian. Multiple objective behavior based control. to appear in Journal of Robotics and Autonomous systems, 1999. [70] P.Mosterman. An overview of hybrid simulation phenomena and their support by simulation packages. In F. Vaandrager and J. H. van Schuppen, editors, Hybrid Systems : Computation and Control, volume 1569 of Lecture Notes in Computer Science, pages 163–177. Springer Verlag, 1999. [71] A. Prestin and M.Berzine. Algorithms for teh location of discontinuities in dynamic simulation problems. Computers in chemical engineering, 15(10):701–713, 1991. [72] A. Quaid and A. Rizzi. Robust and efficient motion planning for a planar robot using hybrid control. In IEEE International Conference on Robotics and Automation 2000, volume 4, pages 4021 – 4026, April 2000. [73] R.Alur, A.Das, J.Esposito, R.Fierro, Y.Hur, G.Grudic, V. Kumar, I.Lee, J.P.Ostrowski, G.J.Pappas, J.Southall, J.Spletzer, and C.Taylor. A framework and architecture for multirobot coordination. In Experimental robotics VII, volume 271 of Lecture Notes in Computer and Information Science. Springer, 2001. [74] R.Alur, R. Grosse, Y.Hur, V. Kumar, and I. Lee. Modular specification of hybrid systems in charon. Hybrid Systems Computation and Control: Third international workshop, 3:6–19, 2000. [75] R.Burridge, A.Rizzi, and D.E.Koditschek. Sequential composition of dynamically dexterous robot behaviors. International Journal of Robotics Research, 18(6):534–555, 1999. [76] R.C.Rice. Split Runge-Kutta methods for simultaneous equations. Journal of Res. National Bureau of Standards, 64B:151–170, 1960. [77] R.Grupen, M.Huber, J.Coelho, and K.Souccar. A basis for distributed control of maipulation tasks. IEEE Journal on expert systems, 10(2):9–14, 1995. 172
[78] E. Rimon and D. E. Koditschek. Exact robot navigation using artificial potential functions. IEEE Transactions on Robotics and Automation, 8(5):501–518, 1992. [79] A. Rizzi. Hybrid control as a method for robot motion programming. In IEEE Int’l. Conf. on Robotics and Automation, volume 1, pages 832 – 837, May 1998. [80] D. R.Jefferson. Virtual time. ACM Transactions on Programming Languages and Systems, 7(3):404–425, July 1985. [81] K. Ruohonen. Event detection for ODEs and nonrecuirsive heirarchies. In Results and trends in theoretical computer science, volume 812 of Lecture Notes in Computer Science, pages 358–371. Springer Verlag, 1994. [82] R.Volpe and P.Khosla. Artificial potentials with eliptical isopotential contours for obstacle avoidance. In 26th IEEE conference on decision and control, pages 180–185, Los Angeles, 1987. [83] J. Sand and S. Skelboe. Stability of backward euler multirate methods and convergence of waveform relaxation. BIT, 32:350–366, 1992. [84] J. Schwartz and M.Sharir. On the ’Piano Movers’ problem: II General techniques for computing topological properties of real algebraic manifolds. Advances in applied mathematics, 4:298–351, 1983. [85] S. Skelboe. Methods for parallel integration of stiff systems of odes. BIT, 32:689–701, 1992. [86] P. Song, M. Yashima, and V. Kumar. Dynamic simulation for grasping and whole arm manipulation. IEEE International Conference on Robotics and Automation, pages 1082–1087, May 2-7, 1993. [87] A. Stuart and A. Humphries. Dynamical Systems and Numerical Analysis. Monographs on Applied and Computational Mathematics. Cambridge press, 1998. 173
[88] H. G. Tanner and K. J. Kyriakopoulos. Nonholonomic motion planning for mobile manipulators. Proceedings of the 2000 IEEE International Conference on Robotics and Automation, pages 1233–1238, 2000. [89] H. G. Tanner, S. Loizou, and K. J. Kyriakopoulos. Nonholonomic motion planning for mobile manipulators. Proceedings of the 2001 IEEE/RSJ International Conference on Intelligent Robots and Systems, 2001. [90] T.J.Koo, G.J.Pappas, and S.Sastry. Mode switching sysnthisis for reachibility specifications. In Hybrid Systems: Computation and Control, volume 2034 of Lecture Notes in Computer Science. Springer, March 2001. [91] T.J.Koo, G.J.Pappas, and S.Sastry. Multi-modal control of systems with constraints. In 40th IEEE conference on decision and control, pages 2075–2080, Orlando, FL, December 2001. [92] C. Tomlin, J. Lygeros, and S. Sastry. A game theoretic approach to controller design for hybrid systems. Proceedings of the IEEE, 88(7), 2000. [93] T.Park and P.Barton. State event location in differential-algebraic models. ACM transactions on modeling and computer simulation, 6(2):137–165, 1996. [94] G. Vanderplaats. Numerical Optimization Techniques for Engineering Design. McGraw– Hill, 1984. [95] D. Wells. Multirate linear multistep methods for the solution of systems of ordinary differential equations. PhD thesis, Univ. of Illinois at Urbana-Champaign, IL, 1982. [96] W.S.Howard and V.Kumar. A minimum principle for the dynamic analysis of systems with frictional contacts. IEEE International Conference on Robotics and Automation, Atlanta, pages 437–442, May 2-7, 1993. 174
[97] J. X.Lui, T.J.Koo, B.Sinopoli, S.Sastry, and E.A.Lee. A hierarchical hybrid system model and its simulation. Proceedings of the
ÑkÎhϳÐ
Conference on Decision and Control, pages
2407–2411, 1999. ˇ [98] M. Zefran. Continuous methods for motion planning. PhD thesis, U. of Pennsylvania, Philadelphia, PA, 1996.
175
