Transcript Obstacle Avoidance in Formation

Centre for Autonomous Systems
Formations and Obstacle Avoidance
in Mobile Robot Control
Petter Ögren
Topics from a Doctoral Thesis, 2003
Petter Ögren
OptSyst seminar 23/5
1
Centre for Autonomous Systems
Outline
A brief introduction of all four papers
Overview of how they relate to each other
Details of Paper A
Details of Paper B
Petter Ögren
OptSyst seminar 23/5
2
Centre for Autonomous Systems
All four Papers
Obstacle
Avoidance
Paper A
Paper B
Paper D
Formations
Paper C
Petter Ögren
OptSyst seminar 23/5
3
Centre for Autonomous Systems
Paper A: A Convergent
Dynamic Window Approach to
Obstacle Avoidance
Problem formulation: Drive a robot from A to B
through a partially unknown environment
without collisions.
B
A
Petter Ögren
OptSyst seminar 23/5
4
Centre for Autonomous Systems
Paper A: A Convergent
Dynamic Window Approach to
Obstacle Avoidance
Proposed solution: Merge state-of-the-art
heuristics with a provable approach, (using a
CLF/MPC framework).
)
Optimize pointwise over stabilizing controls
Petter Ögren
OptSyst seminar 23/5
5
Centre for Autonomous Systems
Paper B: Obstacle Avoidance in
Formation
Problem: How do we move the leader to guide a
leader-follower formation through obstacle terrain?
Can we use singel vehicle Obstacle Avoidance?
Petter Ögren
OptSyst seminar 23/5
6
Centre for Autonomous Systems
Paper B: Obstacle Avoidance in
Formation
Proposed solution: The concept of Configuration
Space Obstacles is extended through an Input to
State Stability (ISS) argument.
)A map of the leader positions that guarantee
followers enough free space. The leader does single
vehicle obstacle avoidance using this map.
Petter Ögren
OptSyst seminar 23/5
7
Centre for Autonomous Systems
Paper C: A Control Lyapunov
Function Approach to
Multi Agent Coordination
Problem: How to make set-point controlled
robots moving along trajectories in a formation
”wait” for eachother?
Petter Ögren
OptSyst seminar 23/5
8
Centre for Autonomous Systems
Paper C: A Control Lyapunov
Function Approach to
Multi Agent Coordination
Idea: Combine Control Lyapunov Functions (CLF) with
the Egerstedt&Hu virtual vehicle approach.
Under assumptions this will result in:
Bounded formation error (waiting)
Approximation of given formation velocity (if no
waiting is necessary).
Finite completion time (no 1-waiting).
Petter Ögren
OptSyst seminar 23/5
9
Paper D: Cooperative Control of Mobile
Centre for Autonomous Systems
Sensor Networks: Adaptive Gradient
Climbing in a Distributed Environment
Problem D1: Given a local-spring-damper formation
control.
How do we translate, rotate and expand the formation?
Problem D2: Given a field, i.e. temperature or nutrition
density in water.
How do we estimate the gradient from noisy distributed
measurements?
What formation geometries give good estimates?
Petter Ögren
OptSyst seminar 23/5
10
Paper D: Cooperative Control of Mobile
Centre for Autonomous Systems
Sensor Networks: Adaptive Gradient
Climbing in a Distributed Environment
Proposed solution D1:
Introduce virtual leaders in the formation and
move these.
Let direction of motion be governed by the
mission, e.g. gradient climbing.
Let the speed of the motion be influenced by
error feedback (from paperC).
Petter Ögren
OptSyst seminar 23/5
11
Centre for Autonomous Systems
Paper D: Cooperative Control of Mobile
Sensor Networks: Adaptive Gradient
Climbing in a Distributed Environment
Proposed solution D2:
Estimate the gradient: Use the least Squares
estimate of (a,b) in an affine approximation
aTz+b ¼ T(z). Apply Kalman filter over time.
Formation geometries: Minimize error due to
measurement noise and second order terms. In
1-dimension:
Noisy
True
Estimate
This is generalized to m
vehicles in Rn .
Petter Ögren
OptSyst seminar 23/5
12
Centre for Autonomous Systems
All four Papers
Paper A
Details!
Paper B
Paper D
Paper C
Petter Ögren
OptSyst seminar 23/5
13
Centre for Autonomous Systems
Paper A: A Convergent
Dynamic Window Approach to
Obstacle Avoidance
Drive a robot from A to B through a partially
unknown environment without collisions.
B
Differential drive
robots can be
feedback
linearized to
this.
A
Petter Ögren
OptSyst seminar 23/5
14
Centre for Autonomous Systems
Background: The Dynamic
Unicycle
q
Petter Ögren
OptSyst seminar 23/5
15
Centre for Autonomous Systems
Desirable Properties
in Obstacle Avoidance
No collisions
Convergence to goal position
Efficient, large inputs
‘Real time’
‘Reactive’, (to changes in environment)
Petter Ögren
OptSyst seminar 23/5
16
Centre for Autonomous Systems
Background: Two main Obstacle
Avoidance approaches
Reactive/Behavior Based
Biologically motivated
Fast, local rules.
‘The world is the map’
No proofs.
Changing environment
not a problem
Deliberative/Sense-Plan-Act
• Trajectory planning/tracking
• Navigation function
(Koditschek ’92).
• Provable features.
• Changes are a problem
Combine the two?
Petter Ögren
OptSyst seminar 23/5
17
Centre for Autonomous Systems
Background: The Navigation
Function (NF) tool
One local/global
min at goal.
Gradient gives
direction to
goal.
Solves ‘maze’
problems.
Obstacles and NF level curves
Goal
Petter Ögren
OptSyst seminar 23/5
18
Centre for Autonomous Systems
Basic Idea
Exact Navigation,
DWA, Fox et. al.
using Art. Pot. Fcn.
Koditscheck ’92
and Brock et al
Model Predictive
Control (MPC)
•
•
•
‘Real time’
Efficient, large inputs
‘Reactive’, to changes
Control Lyapunov
Function (CLF)
MPC/CLF Framework,
Primbs ’99
•
•
Convergence proof.
No collisions
Convergent DWA
Petter Ögren
OptSyst seminar 23/5
19
Centre for Autonomous Systems
Background: Model
Predictive Control (MPC)
Idea: Given a good model,
we can simulate the result
of different control choices
(over time T) and apply the
best.
Feedback: repeat simulation
every t<T seconds.
How is this connected to the Dynamic Window Approach?
Petter Ögren
OptSyst seminar 23/5
20
Centre for Autonomous Systems
Global Dynamic Window
Approach (Brock and Khatib ‘99)
Robot
Cirular arc pseudo-trajectories
Velocity Space
Vy
Current Velocity
Dynamic Window
Vx
Control Options
Obstacles
Petter Ögren
Vmax
OptSyst seminar 23/5
21
Centre for Autonomous Systems
Global Dynamic Window
Approach (continued)
Check arcs for collision free length.
Chose control by optimization of the
heuristic utility function:
Speeds up to 1m/s indoors with XR 4000
robot (Good!).
No proofs. (Counter example!)
Idea:
See as Model Predictive Control (MPC)
Use navigation function as CLF
Petter Ögren
OptSyst seminar 23/5
22
Centre for Autonomous Systems
Background: Control
Lyapunov Function (CLF)
Idea: If the energy of
a system decreases
all the time, it will
eventually “stop”.
A CLF, V, is an
“energy-like” function
such that
V
x
Petter Ögren
OptSyst seminar 23/5
23
Centre for Autonomous Systems
Exact Robot Navigation using
Artificial Potential Functions,
(Rimon and Koditscheck ‘92)
C1 Navigation Function NF(p) constructed.
NF(p)=NFmax at obstacles of Sphere and Star
worlds.
Control:
Features:
Lyapunov function:
=> No collisions.
Bounded Control.
Convergence Proof
Drawbacks
Hard to (re)calculate.
Inefficient
Idea: Use C0 Control Lyapunov Function.
Petter Ögren
OptSyst seminar 23/5
24
Centre for Autonomous Systems
Our Navigation Function (NF)
One local/global
min at goal.
Calculate shortest
path in
discretization.
Make continuous
surface by careful
interpolation using
triangles.
Provable
properties.
Petter Ögren
The discretization
OptSyst seminar 23/5
25
Centre for Autonomous Systems
MPC/CLF framework
Primbs general form:
Petter Ögren
Here we write:
OptSyst seminar 23/5
26
Centre for Autonomous Systems
The resulting scheme:
Lyapunov Function and Control
Lyapunov function candidate:
gives the following set of controls, incl.
Compare: Acceleration of down hill skier.
Petter Ögren
OptSyst seminar 23/5
27
Centre for Autonomous Systems
Safety and Discretization
The CLF gives stability, what about safety?
In MPC, consider controls stop without collision.
Plan to first accelerate:
then brake:
Apply first part and replan.
Compare: Being able to stop in visible part of road ) safety
Petter Ögren
OptSyst seminar 23/5
28
Centre for Autonomous Systems
Evaluated MPC Trajectories
Petter Ögren
OptSyst seminar 23/5
29
Centre for Autonomous Systems
Simulation Trajectory
Petter Ögren
OptSyst seminar 23/5
30
Centre for Autonomous Systems
Single Vehicle
Conclusions
Properties:
No collisions (stop safely option)
Convergence to goal position (CLF)
Efficient (MPC).
Reactive (MPC).
Real time (?), small discretized control set, formalizing
earlier approach.
Can this scheme be extended to the multi vehicle case?
Petter Ögren
OptSyst seminar 23/5
31
Centre for Autonomous Systems
All four Papers
Paper A
Details!
Paper B
Paper D
Paper C
Petter Ögren
OptSyst seminar 23/5
32
Centre for Autonomous Systems
Why Multi Agent Robotics?
Applications:
Search and Rescue
missions, lawn
moving etc.
Carry large/awkward
objects
Adaptive sensing,
e.g. surveillance or
ocean sampling
Satellite imaging in
formation
Petter Ögren
Motivations:
Flexibility
Robustness
Performance
Price
OptSyst seminar 23/5
33
Centre for Autonomous Systems
Paper B: Obstacle Avoidance
in Formation
How do we use singel vehicle Obstacle Avoidance?
Petter Ögren
OptSyst seminar 23/5
34
Centre for Autonomous Systems
Desirable properties
No collisions
Convergence to goal position
Efficient, large inputs
‘Real time’
‘Reactive’, to changes
&
Distributed/Local information
Petter Ögren
OptSyst seminar 23/5
35
Centre for Autonomous Systems
A Leader-Follower Structure
Leader
Information flow
Two Cases:
No explicit
information
exchange ) leader
acceleration, u1, is
a disturbance
Feedforward of u1)
time delays and
calibration errors
are disturbances
How big deviations will the disturbances cause?
Petter Ögren
OptSyst seminar 23/5
36
Centre for Autonomous Systems
Background: Input to State
Stability (ISS)
We will use the ISS to calculate ”Uncertainty Regions”
Petter Ögren
OptSyst seminar 23/5
37
Centre for Autonomous Systems
ISS ) Uncertainty Region
Uncertainty Region
Petter Ögren
OptSyst seminar 23/5
38
Formation Leader Obstacles,
Centre for Autonomous Systems
an extension of
Configuration Space Obstacles
”Occupied” leader pos.
Obstacle
”Free” leader pos.
How do we calculate a map of ”free” leader positions?
Petter Ögren
OptSyst seminar 23/5
39
Centre for Autonomous Systems
Formation Leader Map
Unc. Region and Obstacles
Formation Obstacles
• Computable by conv2 (matlab).
• Leader does obstacle avoidance in new map.
• Followers do formation keeping under disturbance.
Petter Ögren
OptSyst seminar 23/5
40
Centre for Autonomous Systems
Simulation Trajectories
Petter Ögren
OptSyst seminar 23/5
41
Centre for Autonomous Systems
Conclusions, paper B
Obstacle Avoidance extended to
formations by assuming leaderfollower structure and ISS.
Future directions
Rotations
Expansions
Breaking formation
) ¸ 3 dim NF
Petter Ögren
OptSyst seminar 23/5
42
Centre for Autonomous Systems
All four Papers
Paper A
Details!
Paper B
Paper D
Paper C
Petter Ögren
OptSyst seminar 23/5
43
Centre for Autonomous Systems
End of Presentation.
Petter Ögren
OptSyst seminar 23/5
44
Centre for Autonomous Systems
Paper C:
A Control Lyapunov Function Approach to
Multi Agent Coordination
P. Ögren, M. Egerstedt* and X. Hu
Royal Institute of Technology (KTH), Stockholm
and Georgia Institute of Technology*
IEEE Transactions on Robotics and Automation, Oct 2002
Petter Ögren
OptSyst seminar 23/5
45
Centre for Autonomous Systems
Problem and Proposed Solution
Problem: How to make set-point controlled
robots moving along trajectories in a formation
”wait” for eachother?
Idea: Combine Control Lyapunov Functions
(CLF) with the Egerstedt&Hu virtual vehicle
approach.
Under assumptions this will result in:
Bounded formation error (waiting)
Approx. of given formation velocity (if no waiting is
nessesary).
Finite completion time (no 1-waiting).
Petter Ögren
OptSyst seminar 23/5
46
Centre for Autonomous Systems
Quantifying Formation Keeping
Definition: Formation Function
Will add Lyapunov like assumption satisfied by
individual set-point controllers. =>
Think of as parameterized Lyapunov function.
Petter Ögren
OptSyst seminar 23/5
47
Centre for Autonomous Systems
Examples of Formation Function
• Simple linear example !
• A CLF for the combined
higher dimensional
system:
Note that a,b, are design
parameters.
• The approach applies to
any parameterized
formation scheme with
lyapunov stability results.
Petter Ögren
OptSyst seminar 23/5
48
Centre for Autonomous Systems
Main Assumption
We can find a class K function s such that the
given set-point controllers satisfy:
This can be done when -dV/dt is lpd, V is lpd
and decrescent. It allows us to prove:
Bounded V (error): V(x,s) < VU
Bounded completion time.
Keeping formation velocity v0, if V ¿ VU.
Petter Ögren
OptSyst seminar 23/5
49
Speed along trajectory:
Centre for Autonomous Systems
How Do We Update s?
Suggestion: s=v0 t
Problems: Bounded ctrl
or local ass stability
We want:
V to be small
Slowdown if V is large
Speed v0 if V is small
Suggestion:
Let s evolve with
feedback from V.
Petter Ögren
OptSyst seminar 23/5
50
Centre for Autonomous Systems
Evolution of s
Choosing
to be:
We can prove:
Bounded V (error): V(x,s) < VU
Bounded completion time.
Keeping formation velocity v0, if V ¿ VU.
Petter Ögren
OptSyst seminar 23/5
51
Centre for Autonomous Systems
Proof sketch: Formation error
Petter Ögren
OptSyst seminar 23/5
52
Centre for Autonomous Systems
Proof sketch: Finite Completion Time
Find lower bound on ds/dt
Petter Ögren
OptSyst seminar 23/5
53
Centre for Autonomous Systems
Example: Formation
Three unicycle
robots along
trajectory.
VU=1, v0=0.1, then
v0=0.3 ! 0.27
Stochastic
measurement error
in top robot at
12m mark.
Petter Ögren
OptSyst seminar 23/5
54
Centre for Autonomous Systems
Extending Work by Beard et. al.
”Satisficing Control for Multi-Agent Formation
Maneuvers”, in proc. CDC ’02
It is shown how to find an explicit
parameterization of the stabilizing controllers
that fulfills the assumption
These controllers are also inverse optimal and
have robustness properties to input
disturbances
Implementation
Petter Ögren
OptSyst seminar 23/5
55
Centre for Autonomous Systems
All four Papers
Paper A
Details!
Paper B
Paper D
Paper C
Petter Ögren
OptSyst seminar 23/5
56
Centre for Autonomous Systems
Another extension:
Formations with a Mission: Stable
Coordination of Vehicle Group Maneuvers
Petter Ogren
Edward Fiorelli and Naomi Ehrich Leonard
[email protected]
[email protected], [email protected]
Optimization and Systems Theory
Mechanical and Aerospace Engineering
Royal Institute of Technology, Sweden
Princeton University, USA
Mathematical Theory of Networks and Systems (MTNS ‘02)
Visit: http://graham.princeton.edu/ for related information
Petter Ögren
OptSyst seminar 23/5
57
Centre for Autonomous Systems
Approach: Use artificial potentials and
virtual body with dynamics.
•Configuration space of virtual body is
for orientation, position and expansion factor:
• Because of artificial potentials, vehicles in formation will
translate, rotate, expand and contract with virtual body.
• To ensure stability and convergence, prescribe virtual body
dynamics so that its speed is driven by a formation error.
• Define direction of virtual body dynamics to satisfy mission.
• Partial decoupling: Formation guaranteed independent of mission.
• Prove convergence of gradient climbing.
Petter Ögren
OptSyst seminar 23/5
58
Centre for Autonomous Systems
What if dV/dt <= 0 ?
If we have semidefinite
and stability
by La Salle’s principle we choose
as:
By a renewed La Salle argument we can
still show: V<=VU , s! sf and x! xf.
But not: Completion time and v0.
Petter Ögren
OptSyst seminar 23/5
59
Centre for Autonomous Systems
Conclusions
Moving formations by using Control
Lyapunov Functions.
Theoretical Properties:
V <= VU, error
T < TU, time
v ¼ v0 velocity
Extension used for translation, rotation
and expansion in gradient climbing
mission
Petter Ögren
OptSyst seminar 23/5
60