A Convergent Dynamic Window Approach to Obstacle Avoidance & Obstacle Avoidance in Formation
-
Upload
alec-nieves -
Category
Documents
-
view
148 -
download
0
description
Transcript of A Convergent Dynamic Window Approach to Obstacle Avoidance & Obstacle Avoidance in Formation
Cen
tre f
or
Auto
nom
ou
s S
yst
em
s
Petter Ögren FOI presentation 1
A Convergent Dynamic Window Approach to Obstacle Avoidance
&
Obstacle Avoidance in Formation
A Convergent Dynamic Window Approach to Obstacle Avoidance
&
Obstacle Avoidance in Formation
P. Ögren (KTH)
N. Leonard (Princeton University)
Cen
tre f
or
Auto
nom
ou
s S
yst
em
s
Petter Ögren FOI presentation 2
Problem FormulationProblem Formulation
Drive a robot from A to B through a partiallyunknown environment without collisions.
A
B
Differential drive robots can be feedback linearized to this.
Cen
tre f
or
Auto
nom
ou
s S
yst
em
s
Petter Ögren FOI presentation 3
Background: The Dynamic Unicycle (or a Tank?)
Background: The Dynamic Unicycle (or a Tank?)
Cen
tre f
or
Auto
nom
ou
s S
yst
em
s
Petter Ögren FOI presentation 4
Desirable PropertiesDesirable Properties
No collisions
Convergence to goal position
Efficient, large inputs
‘Real time’
‘Reactive’, to changes
Cen
tre f
or
Auto
nom
ou
s S
yst
em
s
Petter Ögren FOI presentation 5
Background: Two main Obstacle Avoidance approaches
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
Combine the two?
Deliberative/Sense-Plan-Act• Trajectory planning/tracking• Navigation function
(Koditschek ’92).• Provable features.• Changes are a problem
Cen
tre f
or
Auto
nom
ou
s S
yst
em
s
Petter Ögren FOI presentation 6
Background: The Navigation Function (NF) tool
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
Cen
tre f
or
Auto
nom
ou
s S
yst
em
s
Petter Ögren FOI presentation 7
Basic IdeaBasic Idea
Control LyapunovFunction (CLF)
DWA, Fox et. al. and Brock et al
Model PredictiveControl (MPC)
MPC/CLF Framework, Primbs ’99
Convergent DWA
Exact Navigation,using Art. Pot. Fcn.
Koditscheck ’92
• ‘Real time’
• Efficient, large inputs
• ‘Reactive’, to changes
• Convergence proof.
• No collisions
Cen
tre f
or
Auto
nom
ou
s S
yst
em
s
Petter Ögren FOI presentation 8
Background: Model Predictive Control (MPC)
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 seconds.
How is this used in the Dynamic Window Approach?
Cen
tre f
or
Auto
nom
ou
s S
yst
em
s
Petter Ögren FOI presentation 9
Global Dynamic Window Approach (Brock and Khatib ‘99)
Global Dynamic Window Approach (Brock and Khatib ‘99)
Vx
Vy
Dynamic Window
Control Options
ObstaclesVmax
Current Velocity
Velocity Space
Robot
Cirular arc pseudo-trajectories
Cen
tre f
or
Auto
nom
ou
s S
yst
em
s
Petter Ögren FOI presentation 10
Global Dynamic Window Approach (continued)
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
Cen
tre f
or
Auto
nom
ou
s S
yst
em
s
Petter Ögren FOI presentation 11
Background: Control Lyapunov Function (CLF)
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
Cen
tre f
or
Auto
nom
ou
s S
yst
em
s
Petter Ögren FOI presentation 12
Exact Robot Navigation using Artificial Potential Functions, (Rimon and Koditscheck ‘92)
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
DrawbacksHard to (re)calculate.Inefficient
Idea: Use C0 Control Lyapunov Function.
Cen
tre f
or
Auto
nom
ou
s S
yst
em
s
Petter Ögren FOI presentation 13
Our Navigation Function (NF)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.
The discretization
Cen
tre f
or
Auto
nom
ou
s S
yst
em
s
Petter Ögren FOI presentation 14
MPC/CLF frameworkMPC/CLF framework
Primbs general form: Here we write:
Cen
tre f
or
Auto
nom
ou
s S
yst
em
s
Petter Ögren FOI presentation 15
The resulting scheme: Lyapunov Function and Control
The resulting scheme: Lyapunov Function and Control
Lyapunov function candidate:
gives the following set of controls, incl.
Compare: Acceleration of down hill skier.
Cen
tre f
or
Auto
nom
ou
s S
yst
em
s
Petter Ögren FOI presentation 16
Safety and DiscretizationSafety 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
Cen
tre f
or
Auto
nom
ou
s S
yst
em
s
Petter Ögren FOI presentation 17
Evaluated MPC TrajectoriesEvaluated MPC Trajectories
Cen
tre f
or
Auto
nom
ou
s S
yst
em
s
Petter Ögren FOI presentation 18
Simulation TrajectorySimulation Trajectory
Cen
tre f
or
Auto
nom
ou
s S
yst
em
s
Petter Ögren FOI presentation 19
Single Vehicle Conclusions
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?
Cen
tre f
or
Auto
nom
ou
s S
yst
em
s
Petter Ögren FOI presentation 20
Why Multi Agent Robotics?Why Multi Agent Robotics?
Applications:
Search and Rescue missions
Carry large/awkward objects
Adaptive sensing
Satellite imaging in formation
Motivations:
Flexibility
Robustness
Performance
Price
Cen
tre f
or
Auto
nom
ou
s S
yst
em
s
Petter Ögren FOI presentation 21
Obstacle Avoidance in Formation
Obstacle Avoidance in Formation
How do we use singel vehicle Obstacle Avoidance?
Cen
tre f
or
Auto
nom
ou
s S
yst
em
s
Petter Ögren FOI presentation 22
Desirable propertiesDesirable properties
No collisionsConvergence to goal positionEfficient, large inputs‘Real time’‘Reactive’, to changes
&Distributed/Local information
Cen
tre f
or
Auto
nom
ou
s S
yst
em
s
Petter Ögren FOI presentation 23
A Leader-Follower Structure A Leader-Follower Structure
Two Cases:No explicit information exchange ) leader acceleration, u1, is a disturbance
Feedforward of u1) time delays and calibration errors are disturbances
Information flow
Leader
How big deviations will the disturbances cause?
Cen
tre f
or
Auto
nom
ou
s S
yst
em
s
Petter Ögren FOI presentation 24
Background: Input to State Stability (ISS)
Background: Input to State Stability (ISS)
We will use the ISS to calculate ”Uncertainty Regions”
Cen
tre f
or
Auto
nom
ou
s S
yst
em
s
Petter Ögren FOI presentation 25
ISS ) Uncertainty Region ISS ) Uncertainty Region
Uncertainty Region
Cen
tre f
or
Auto
nom
ou
s S
yst
em
s
Petter Ögren FOI presentation 26
Formation Leader Obstacles, an extension of
Configuration Space Obstacles
Formation Leader Obstacles, an extension of
Configuration Space Obstacles
”Free” leader pos.
”Occupied” leader pos.
How do we calculate a map of ”free” leader positions?
Obstacle
Cen
tre f
or
Auto
nom
ou
s S
yst
em
s
Petter Ögren FOI presentation 27
Formation Leader MapFormation 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.
Cen
tre f
or
Auto
nom
ou
s S
yst
em
s
Petter Ögren FOI presentation 28
Simulation TrajectoriesSimulation Trajectories
Cen
tre f
or
Auto
nom
ou
s S
yst
em
s
Petter Ögren FOI presentation 29
Final ConclusionsFinal Conclusions
Obstacle Avoidance extended to formations by assuming leader-follower structure and ISS.
Future directionsRotations
Expansions
Braking formation
) ¸ 3 dim NF
Cen
tre f
or
Auto
nom
ou
s S
yst
em
s
Petter Ögren FOI presentation 30
Comparison