1. DARPA ITO/MARS Program
Control and Coordination of Multiple Autonomous Robots
Vijay Kumar
GRASP Laboratory
University of Pennsylvania

2. Motivation We are interested in coordinated control of robots
manipulation
vision-based control
Large number of modes
Scalability
Individual modes (behaviors) are
well understood, but the interaction between them is not.
Software design: modes - bottom up, protocols - top down

3. MARS Analysis Learning Algorithms CHARON to Java Translator
CHARON Code
(High level language)
Analysis
Learning
Algorithms
CHARON to Java Translator
Java
Libraries
Drivers
Java Code
Control Code Generator
Simulator Code Generator
Human Interface

4. Outline of the Talk Language and software architecture
CHARON
agents and modes
examples
Reactive control algorithms
mode switching
hierarchical composition of reactive control algorithms
results
From reactive to deliberative schemes
Simulation
Reinforcement learning
learn mode switching and composition rules
Future work

5. Participants Rajeev Alur Aveek Das Joel Esposito Rafael Fierro
Radu Grosu
Greg Grudic
Yerang Hur
Vijay Kumar
Insup Lee
Ben Southall
John Spletzer
Camillo Taylor
Lyle Ungar

6. Architectural Hierarchy in CHARON
Agent
Agent1
Agent2
sensor
sensor
processor
processor
actuator
actuator
Input Port
Output Port
Each agent can be represented as a parallel composition of sub-agents

7. Behavioral Hierarchy in CHARON
Modes
main
awayTarget
atTarget
sensing
control
Entry Port
Exit Port
Each agent consists of modes or behaviors
Modes can in turn consist of sub modes

8. CHARON Individual components described as agents
Composition, instantiation, and hiding
Individual behaviors described as modes
Encapsulation, instantiation, and Scoping
Support for concurrency
Shared variables as well as message passing
Support for discrete and continuous behavior
Well-defined formal semantics
Composition of submodes is called encapsulation.
Sequential composition.
Follow the wall and obstacle avoidance can be sequentially implemented

9. Reactive Behaviors based on Vision
Avoid
Obstacle
Collision
Recovery
Pursue
Motion Controller
Range
Mapper
Robot Position
Estimator
Target
Detector
Edge
Detector
Collision
Color Blob
Finder
Frame Grabber
Actuators

10. Robot Agent robotController = rmapper || cdetector || explore
rmapper = rangeMapper() [rangeMap = rM];
cdetector = collisionDetector() [collisionDetected = cD];
explore = obstacleAvoider() [collisionDetected,
rangeMap = cD, rM];
agent explore(){
mode top = exploreTopMode() }
agent rangeMapper(){
mode top = rangeMapperTopMode()
agent collisionDetector(){
mode top = collisionDetectorTopMode()

11. Collision Recovery mode collisionRecoveryMode(real recoveryDuration,
real c) {
entry enPt;
exit exPt;
readWrite diff analog real x;
readWrite diff analog real phi;
readWrite diff analog real recoveryTimer;
diffEqn dRecovery { d(x) = -c; d(phi) = 0;
d(recoveryTimer) = 1.0 }
inv invRecovery { 0.0 <= recoveryTimer &&
recoveryTimer <= recoveryDuration }
} // end of mode collisionRecoveryMode

12. Obstacle Avoidance mode obAvoidanceMode(){ entry enPt; exit exPt;
read discrete bool collisionDetected;
read RangeMap rangeMap;
readWrite diff analog real x;
readWrite diff analog real phi;
diffEqn dObAvoidance {d(x) = computeSpeed(rangeMap);
d(phi) = computeAngle(rangeMap)}
inv invObAvoidance {collisionDetected = false}
initTrans from obAvoidanceMode to obAvoidanceMode
when true
do{x = 0.0; phi = 0.0} }

13. Explore mode exploreTopMode() { entry enPt;
read discrete bool collisionDetected;
read RangeMap rangeMap;
private diff analog real recoveryTimer;
mode obAvoidance = obAvoidanceMode()
mode collisionRecovery =
collisionRecoveryMode(recoveryDuration, c)
initTrans from obstacleAvoiderTopMod to obAvoidance
when true do {recoveryDuration = 10.0;
c = 1.0} // initialization
trans OaToCr from obAvoidance.exPt
to collisionRecovery.enPt
when (collisionDetected == true) do {}
trans CrToOa from collisionRecovery.exPt to obAvoidance.enPt
when (recoveryTimer == recoveryDuration)
do {recoveryTimer = 0.0} // reset the timer }

14. . . Explore Explore collisionRecovery obAvoidance rangeMap
collisionDetected .
phi = -k2
x=k1r .
recoveryTimer = 1
phi = 0
x=-c
collisionRecovery
dRecovery
obAvoidance
dObAvoidance
collision
timeOut

15. Vision Based Control with Mobile Robots
Parallel composition of software agents
obstacle avoidance
wall following
Mode switching
Multiple levels of abstraction of data from sensor

16. Explore: Wall Following with Obstacles

17. Explore, Search, and Pursue

18. local diff analog timer
Multiagent Control
pos = target
local diff analog timer
awTarget
dPlan
iAway
atTarget
dStop
iAt
arrive
pos
r2Est1
r2Est2
r1Est1
r1Est2
Robot1
dTimer
pos.x = v * cos(phi)
pos.y = v * sin(phi) .
timer/updateFreq = 0
timer = 1 .
moving
dSteer
aOmega
iFreq
sense
sensing
dStop
iConst
arrive
move
omega = k * (theta – phi)

19. Multiagent Control

20. Modular Simulation Goal Modes are simulated using local information
Simulation is efficient and accurate
Integration of modes at different time scales
Integration of agents at different time scales
Modes are simulated using local information
Submodes are regarded as black-boxes
Submodes are simulated independently of other ones
Agents are simulated using local information
Agents are regarded as black-boxes
Agents are simulated independently of other ones

21. Time Round of a Mode (Agent)
1. Get integration time d and invariants from
the supermode (or the scheduler). x .
d, xInv y .
2. While (time t = 0; t <= d) do:
dt,
yInv
- Simplify all invariants. z .
- Predict integration step dt based on d and the
invariants.
- Execute time round of the active submode and get
state s and time elapsed e.
e, sz
- Integrate for time e and get new state s.
t+e, sy
- Return s and t+e if invariants were violated.
- Increment t = t+e.
3. Return s and d

22. Modular Simulation - Global execution
t+dt
time
Agents A1 A2 A3 t e d
1. Pick up the agents with minimum and second
minimum reached time.
2. Compute the time round interval d for
the minimum agent, i.e. A2, such that its
absolute time may exceed with at most dt
the time reached by the second minimum
agent, i.e. A1.
3. The time round may end before the time
interval d was consumed if the invariants
of A2 were violated. Then, an actual time
increment would be e.
4. The agent A2 executes an update round to
synchronize the discrete variables with the
analog ones.
5. The state of A2 get visible to other agents.

23. Modular Multi-rate Simulationx1 x2 x3
time
Use a different time step for each component to exploit multiple time scales, to increasing efficiency.
ratio of largest to smallest step size
coupling
step size
constant
“Slowest-first” order of integration
Coupling is accommodated by using interpolants for slow variables
Tight error bound: O( hm+1 )

24. Simulation and Analysis of Hierarchical Systems with Mode Switching
Modular simulation
Automatic detection of events
mode switching
transitions, guards
Synthesis of controllers
include models of uncertainty
Sensor fusion
include models of noise

25. Traditional Model of Hierarchy
NASREM Architecture
[Albus, 80]
Implementations
Demo III
NASA robotic systems

26. Event detection Given:
dynamics
output
Given:
g(x)
x(t)
We re-parameterize time by controlling the integration step size:
Event !
output dynamics
input
Using feedback linearization we select our “speed” (step-size) along the integral curves to converge to the event surface

27. . . Hysteresis -a a+2 1 -1 -(a+2) a strMinus Env inc Hyst up dec
y = 2u
x1 < a
x2 = -1 .
strMinus dY
iStrM
aStrM .
Hyst
Env u
x1 = u
inc
dX1
s2u up dY
iUp
aUp
dec
inc -a
a+2
u2p 1
dec
dX1
strPlus dY
iStrP
aStrP -1
-(a+2) a

28. Global versus Modular Simulation-1 1
Hysteresis example
2 levels of hierarchy
global state is two dimensional
Significant potential for more complex systems

29. Modular Simulation Error

30. Current Implementation Status
CHARON Specification
Work to date
CHARON semantics
Parser for CHARON
Internal representation
Type checker
Current work
Modular simulation scheme
Internal representation generator
CHARON Parser
Type
Checker
Syntax Tree
Internal Representation Generator
Internal Representation
Control Code
Generator
Simulator
Generator
Model Checker

31. Reactive to Deliberative Schemes
Reactive scheme is a composition of
go to target
collision avoidance
Deliberative scheme
preplanned path around the nominal model
Reactive schemes
robust
easy to implement
may be limited in terms of being able to accomplish complex tasks
may not compare favorably to recursive implementations of deliberative controllers
Nominal
Model
Obstacle

32. Toward a composition of reactive and deliberative decision making
u1 - vector field specified by a reactive planner
u2 - vector field specified by a deliberative planner
If u1 Î U, u2 Î U, then
au1 + (1- a) u2 Î U

33. Composition of reactive and deliberative planners
Framework for decision making
U is the set of available control policies
Y is the uncertainty set
uncertainty in the environment model
uncertainty in dynamics
uncertainty in localization
Best decision under the worst uncertainty

34. Results
Worst Case Outcome
Better than Worst-Case Outcomes
Minimization
weighting prior information and current information
resolving the discrepancy between prior and current plans
Closure property of “basis behaviors” ensures robustness
Requires a priori calculation of roadmap

35. Detailed Analysis Cost Function  cross-section Min-Max Max-Min
x cross-section
Cost Function
Global saddle point does not exist due to non-smooth solution

36. Best under Worst Case Uncertainty
More Results
Open Loop Recursive
Best under Worst Case Uncertainty
Open Loop

37. Deliberative and Reactive Behaviors in a Dynamic Setting
Obstacle dynamics are known, exact inputs are not
obstacle
robot
target

38. Paradigm for Learning
Sensory
information
Information
state
Situation
partitions
Modes
Action
space
Hierarchical structure allows learning at several levels
Lowest level
parameter estimation within each mode
algorithms for extracting the information state (features, position and velocity, high level descriptors)
Intermediate level
select best mode for a situation
determine the best partitioning of states for a given information state
Advanced level
transfer knowledge (programs, behaviors) between robots and human
Learning at any level forces changes at others

39. Reinforcement Learning and Robotics
Successful RL
(Kaelbling, et al 96)
Low dimensional discrete state space
100,000’s training runs necessary
Stochastic search required
Robotics Systems
Large, continuous state space
A large number of training runs (e.g, 100, 000) may not be practical
Stochastic search not desirable
The fundamental question we must ask is can RL learning be applied to robotics give that previous successful implementations of RL seem directly incompatible with robotics.

40. Boundary Localized Reinforcement Learning
Our approach to robot control
Noisy state space
Deterministic modes
Our approach to Reinforcement Learning
Search only mode boundaries
Ignore most of the state space
Minimize stochastic search
RL using no stochastic search during learning
Our goal is to apply reinforcement learning to high dimensional robotics problems which have deterministic mode switching controllers (i.e. the estimate of the current state deterministically defines the mode/action taken).
In order to apply RL to such deterministic and high dimensional problems (such problems have NOT previously been successfully addressed in the RL framework), we will reformulate the RL into a mode boundary localized search with minimal stochastic search and no stochastic search at all. This greatly reduces the solution search space, thus making high dimensional reinforcement learning feasible.

41. Mode Switching Controllers
Mode of operation (action ai executed)
Parameterization of boundary
Mode Boundaries
Our definition of a mode switching control system.
Modes include:
Move away from obstacle. Follow wall. Follow leader.
Follow hallway. Go towards goal.
Note that theta defines the parameterization of the boundaries
between modes in state (information) space.
State Space

42. Reinforcement Learning for Mode Switching Controllers
Initial Guess
(prior knowledge) R
Reinforcement Feedback
Our framework for applying RL to mode switching controllers:
1. Use prior knowledge to define a parameterization of mode boundaries.
2. Use reinforcement feedback from the robots environment to to modify these boundaries to obtain locally optimal performance.
“Optimal” parameterization

43. Reinforcement Learning
Markov Decision Process
Policy
Reinforcement Feedback (environment): rt
Goal: modify policy to maximize performance
Policy Gradient Formulation
Summary of our Reinforcement Learning formulation as a Markov Decision Process (i.e action taken is only a function of the current state).
Note: Discounted reward performance function shown here.
1. A policy is a probabilistic distribution of actions taken in the current state (given the parameterization theta).
2. The environment supplies a reinforcement feedback.
3. The robot’s goal is the maximize reward (i.e. maximize positive reinforcement feedback) under some reward specification (one example is discounted reward but can also have others - I.e. average reward, etc.).
5. We use the policy gradient formulation of RL where we update the theta parameters using by differentiating the reward function and performing gradient ascent.

44. Why Policy Gradient RL?
Computation linear in the number of parameters q
avoids blow-up from discretization as with other RL methods
Generalization in state space is implicitly defined by the parametric representation
generalization is important for high dimensional problems
We use the policy gradient formulation because its computational cost is linear with the number of parameters used to specify the policy.
Other RL methods grow exponentially with the number of dimensions in the state/information space.

45. Key Result #1
Any q parameterized probabilistic policy can be transformed into a approximately deterministic policy parameterized by q
Deterministic everywhere except near mode boundaries.
There are 3 theoretical results.
The first one states that we can transform any probabilistic policy into an approximately deterministic policy (I.e. deterministic everywhere except arbitrarily near mode boundaries).

46. Key Result #2
Convergence to a locally optimal mode switching policies is obtained by searching near mode boundaries
All other regions of the state space can be ignored
This significantly reduces the search space
The second theoretical results says that locally optimal mode switching policies are obtained by using stochastic search near mode boundaries.
This means that we don’t need to do stochastic search in other regions of the state/information space .
The result is that our search space is significantly smaller (as compared with typical probabilistic RL).

47. Stochastic Search Localized to Mode Boundaries
Regions
A diagrammatic representation of the second theoretical result.
State Space

48. Key Result #3
Reinforcement learning can be applied to locally optimizing deterministic mode switching policies without using stochastic search if
robot takes small steps
value of executing actions (Q) is smooth w.r.t. state
These conditions are met almost everywhere in typical robot applications
The final theoretical result is that stochastic search is NOT necessary at all if the reward function is smooth in theta!
This result is significant because the basic premise of all RL (up to now) it cannot be done without stochastic search.
This result eliminates the need for any stochastic search - which is good for robotics applications where randomly chosen action can result in unsafe or expensive outcomes.

49. Deterministic Search at Mode Boundaries
Regions
A diagrammatic representation of the second theoretical result.
State Space

50. Simulation State Space: Robot Position
Boundary Definitions: Gaussian centers and widths
2 parameters per Gaussian for each dimension.
20 parameters.
2 types of modes: Toward a goal, away from an obstacle.
Reward: +1 for reaching a goal, -1 for hitting an obstacle.

51. Convergence Results
Learning Algorithm used: Action Transition Policy Gradient
Policy gradient updates occur only at action transitions
Orders of magnitude faster convergence than other PG algorithms
algorithm can also be used on entirely stochastic policies
convergence guaranteed under appropriate smoothness conditions
A comparison of number of passes through the environment (or iterations) required for the 3 algorithms implemented:
1) Traditional Probabilistic RL: stochastic search in the entire state space.
2) Stochastic Boundary Localized RL (BLRL): stochastic search limited to areas near mode boundaries.
3) Deterministic BLRL: no stochastic search.
Note:
A) The traditional probabilistic RL is the slowest to converge.
B) Stochastic BLRL is one order magnitude faster.
C) Deterministic BLRL is the fasted to converge.
D) All 3 algorithms converge to similar solutions - therefore there is no performance loss entailed in using Deterministic BLRL (as predicted by theory).

52. Convergence in Higher Dimensions
Convergence Results
2D Projection
4500
Stochastic Policy
4000
3500
Initial
3000
Number of trials
2500
Boundary Localized
2000
1500
The robot's learn not to run into walls or obstacles while minimizing
distance traveled to goal (it takes about 5 to 10 runs to learn this).
They start with an initial guess of how close they can approach an
obstacle without hitting it, and this distance is incrementally modified
to trade off between path length and collisions - the learning stops when
the robot never collides with obstacles while taking the shortest path.
The current demo is only learning one parameter, but now it is a simple
matter to add more parameters (starting with the entire range map) and
modes (i.e. the sensor and actuator models are now working properly).
Learned
Deterministic
1000
500 20 40 60 80
100
120
140
Dimension

53. Current work: More Sophisticated Simulation
Realistic models of sensors and actuators
models of noise
Control code used for actual robots also used in simulator
allows modeling of realistic mode switching
modes tested in simulation are directly transferred to real robots
learning can be done in simulation
transferred to real robots for further refinement

54. Status Framework for learning
parameter adaptation using reinforcement reward - low level
learning rules for selecting and switching between behaviors - intermediate level
Preliminary numerical experiments are reassuring
supported by theory
Current work
more realistic simulation
Future work
combine experiments with simulation

55. Summary of Progress Software tools Implementation on robot hardware
On schedule
Implementation on robot hardware
Mixed
Simulation
New results on modular simulation
Control algorithms
Reactive to deliberative algorithms
New
Learning algorithms
Ahead of schedule