Transcript Obstacle Avoidance in Formation

Centre for Autonomous Systems
A Convergent Dynamic Window
Approach to Obstacle Avoidance
&
Obstacle Avoidance in Formation
P. Ögren (KTH)
N. Leonard (Princeton University)
Petter Ögren
FOI presentation
1
Centre for Autonomous Systems
Problem Formulation
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
FOI presentation
2
Centre for Autonomous Systems
Background: The Dynamic
Unicycle (or a Tank?)
q
Petter Ögren
FOI presentation
3
Centre for Autonomous Systems
Desirable Properties
No collisions
Convergence to goal position
Efficient, large inputs
‘Real time’
‘Reactive’, to changes
Petter Ögren
FOI presentation
4
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
FOI presentation
5
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
FOI presentation
6
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
FOI presentation
7
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 used in the Dynamic Window Approach?
Petter Ögren
FOI presentation
8
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
FOI presentation
9
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
FOI presentation
10
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
FOI presentation
11
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
FOI presentation
12
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
FOI presentation
13
Centre for Autonomous Systems
MPC/CLF framework
Primbs general form:
Petter Ögren
Here we write:
FOI presentation
14
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
FOI presentation
15
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
FOI presentation
16
Centre for Autonomous Systems
Evaluated MPC Trajectories
Petter Ögren
FOI presentation
17
Centre for Autonomous Systems
Simulation Trajectory
Petter Ögren
FOI presentation
18
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
FOI presentation
19
Centre for Autonomous Systems
Why Multi Agent Robotics?
Applications:
Search and Rescue
missions
Carry large/awkward
objects
Adaptive sensing
Satellite imaging in
formation
Petter Ögren
Motivations:
Flexibility
Robustness
Performance
Price
FOI presentation
20
Centre for Autonomous Systems
Obstacle Avoidance in
Formation
How do we use singel vehicle Obstacle Avoidance?
Petter Ögren
FOI presentation
21
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
FOI presentation
22
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
FOI presentation
23
Centre for Autonomous Systems
Background: Input to State
Stability (ISS)
We will use the ISS to calculate ”Uncertainty Regions”
Petter Ögren
FOI presentation
24
Centre for Autonomous Systems
ISS ) Uncertainty Region
Uncertainty Region
Petter Ögren
FOI presentation
25
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
FOI presentation
26
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
FOI presentation
27
Centre for Autonomous Systems
Simulation Trajectories
Petter Ögren
FOI presentation
28
Centre for Autonomous Systems
Final Conclusions
Obstacle Avoidance extended to
formations by assuming leaderfollower structure and ISS.
Future directions
Rotations
Expansions
Braking formation
) ¸ 3 dim NF
Petter Ögren
FOI presentation
29
Centre for Autonomous Systems
Comparison
Petter Ögren
FOI presentation
30