Motion Planning Autonomous Mobile Robots - ETH Z Autonomous Systems Lab | Autonomous Mobile Robots...
Transcript of Motion Planning Autonomous Mobile Robots - ETH Z Autonomous Systems Lab | Autonomous Mobile Robots...
ASL
Autonomous Systems Lab
|Autonomous Mobile RobotsMargarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Introduction to Optimization Techniques 1
Motion Planning
Autonomous Mobile Robots
Martin Rufli – IBM Research GmbH
Margarita Chli, Roland Siegwart
ASL
Autonomous Systems Lab
Introduction | the see – think – act cycle
“position“
global map
Cognition
Path Planning
knowledge,
data base
mission
commands
Localization
Map Building
environment model
local mappath
|Autonomous Mobile RobotsMargarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Introduction to Optimization Techniques 2
see-think-actraw data
Sensing Acting
Information
Extraction
Path
Execution
Mo
tio
n C
on
tro
l
Pe
rce
ptio
n
actuator
commands
Real World
Environment
ASL
Autonomous Systems Lab
Introduction | the motion planning problem
|Autonomous Mobile RobotsMargarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Introduction to Optimization Techniques 3
Goal
ASL
Autonomous Systems Lab
|Autonomous Mobile RobotsMargarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Introduction to Optimization Techniques 4
Motion Planning | Introduction to Optimization TechniquesAutonomous Mobile Robots
Martin Rufli – IBM Research GmbH
Margarita Chli, Roland Siegwart
ASL
Autonomous Systems Lab
Introduction | origins and historical developments
� Geometric optimization: Dido‘s problem
|Autonomous Mobile RobotsMargarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Introduction to Optimization Techniques 5
ASL
Autonomous Systems Lab
� Functional optimization: the Brachistochrone problem
Introduction | origins and historical developments
|Autonomous Mobile RobotsMargarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Introduction to Optimization Techniques 6
ASL
Autonomous Systems Lab
� Optimal Control
� Dynamic Programming
Introduction | origins and historical developments
|Autonomous Mobile RobotsMargarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Introduction to Optimization Techniques 7
ASL
Autonomous Systems Lab
1. Motion control
2. Local collision avoidance
3. Global search-based planning
Introduction | hierarchical decomposition
|Autonomous Mobile RobotsMargarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Introduction to Optimization Techniques 8
ASL
Autonomous Systems Lab
Introduction | work-space versus configuration-space
2θ
|Autonomous Mobile RobotsMargarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Introduction to Optimization Techniques 9
Work-spaceConfiguration-spacex
y
Work-space
1θ
2θ
x
y
Configuration-space
1θ
ASL
Autonomous Systems Lab
� Control theory
� D. P. Bertsekas. “Nonlinear Programming (2nd Ed)”. Athena Scientific, Belmont, MA, 1999.
� Motion planning for robotics
� S. M. LaValle. “Planning Algorithms”. Cambridge University Press, Cambridge, UK, 2004.
Introduction | further reading
|Autonomous Mobile RobotsMargarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart
� S. M. LaValle. “Planning Algorithms”. Cambridge University Press, Cambridge, UK, 2004.
Introduction to Optimization Techniques 10
ASL
Autonomous Systems Lab
|Autonomous Mobile RobotsMargarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Collision Avoidance 11
Motion Planning | Collision AvoidanceAutonomous Mobile Robots
Martin Rufli – IBM Research GmbH
Margarita Chli, Roland Siegwart
ASL
Autonomous Systems Lab
� Methods compute actuator commands based on local environment
� They are characterized by
� Being light on computational resources
� Being purely local and thus prone to local optima
� Incorporation of system models
Classic collision avoidance | overview
|Autonomous Mobile RobotsMargarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Collision Avoidance 12
ASL
Autonomous Systems Lab
� Robot is assumed to instantaneously move on circular arcs
� 2D evidence grid is transformed into input-space based on robot
deceleration capabilities / kino-dynamics, leading to
� Static window constrains velocities
� Dynamic window accounts for vehicle dynamics
Dynamic Window Approach (DWA) | working principle
),( ωv
dV
),( ωv
sVoV
|Autonomous Mobile RobotsMargarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart
� Dynamic window accounts for vehicle dynamics
� Selection of -pair within maximizing objective
containing heading, distance to goal and velocity terms
Collision Avoidance 13
dsor VVVV ∩∩=dV
),( ωv
ASL
Autonomous Systems Lab
Dynamic Window Approach (DWA) | working principle
v
|Autonomous Mobile RobotsMargarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Collision Avoidance 14
ω
ASL
Autonomous Systems Lab
Dynamic Window Approach (DWA) | working principle
v
|Autonomous Mobile RobotsMargarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Collision Avoidance 15
ω
ASL
Autonomous Systems Lab
Dynamic Window Approach (DWA) | working principle
v
|Autonomous Mobile RobotsMargarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Collision Avoidance 16
ω
ASL
Autonomous Systems Lab
Dynamic Window Approach (DWA) | working principle
v
|Autonomous Mobile RobotsMargarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Collision Avoidance 17
ω
ASL
Autonomous Systems Lab
� DWA accounts for robot kino-dynamics
� Cost function is prone to local optima
� The method assumes that objects are static
Dynamic Window Approach (DWA) | properties
|Autonomous Mobile RobotsMargarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Collision Avoidance 18
ASL
Autonomous Systems Lab
� The robot is assumed to move on piece-wise linear curves
� The Velocity Obstacle is composed of all robot velocities leading to a collision
with an obstacle before a horizon time
Velocity Obstacles (VO) | working principle
τ
|Autonomous Mobile RobotsMargarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Collision Avoidance 19
ASL
Autonomous Systems Lab
� The robot is assumed to move on piece-wise linear curves
� The Velocity Obstacle is composed of all robot velocities leading to a collision
with an obstacle before a horizon time
Velocity Obstacles (VO) | working principle
τ
|Autonomous Mobile RobotsMargarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Collision Avoidance 20
yv
xv
ASL
Autonomous Systems Lab
� The robot is assumed to move on piece-wise linear curves
� The Velocity Obstacle is composed of all robot velocities leading to a collision
with an obstacle before a horizon time
Velocity Obstacles (VO) | working principle
τ
|Autonomous Mobile RobotsMargarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Collision Avoidance 21
yv
xv
ASL
Autonomous Systems Lab
� The robot is assumed to move on piece-wise linear curves
� The Velocity Obstacle is composed of all robot velocities leading to a collision
with an obstacle before a horizon time
Velocity Obstacles (VO) | working principle
τ
|Autonomous Mobile RobotsMargarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Collision Avoidance 22
yv
xv
ORRRO rrt +<+vp
Uτ
τ
≤≤
−=
t
RORORO
t
r
tDVO
0
,p
ASL
Autonomous Systems Lab
� The robot is assumed to move on piece-wise linear curves
� The Velocity Obstacle is composed of all robot velocities leading to a collision
with an obstacle before a horizon time
Velocity Obstacles (VO) | working principle
τ
|Autonomous Mobile RobotsMargarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Collision Avoidance 23
yv
xv
ASL
Autonomous Systems Lab
� The robot is assumed to move on piece-wise linear curves
� The Velocity Obstacle is composed of all robot velocities leading to a collision
with an obstacle before a horizon time
Velocity Obstacles (VO) | working principle
τ
|Autonomous Mobile RobotsMargarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Collision Avoidance 24
yv
xv
ASL
Autonomous Systems Lab
� VO considers the velocity of other objects
� It is prone to local optima
� It does not model interaction effects
Velocity Obstacles (VO) | properties
|Autonomous Mobile RobotsMargarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Collision Avoidance 25
ASL
Autonomous Systems Lab
Interactive collision avoidance | overview
� Methods compute actuator commands based on local environment
� They are characterized by
� Being light on computational resources
� Being purely local and thus prone to local optima
� Incorporation of system models and higher-order reflection
|Autonomous Mobile RobotsMargarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Collision Avoidance 26
ASL
Autonomous Systems Lab
� The robot is assumed to move on piece-wise linear curves
� Identical to the Velocity Obstacles method, except that collision avoidance is
shared between interacting agents – fairness property
Reciprocal Velocity Obstacles | working principle
|Autonomous Mobile RobotsMargarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Collision Avoidance 27
ASL
Autonomous Systems Lab
� The robot is assumed to move on piece-wise linear curves
� Identical to the Velocity Obstacles method, except that collision avoidance is
shared between interacting agents – fairness property
Reciprocal Velocity Obstacles | working principle
|Autonomous Mobile RobotsMargarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Collision Avoidance 28
ASL
Autonomous Systems Lab
� The robot is assumed to move on piece-wise linear curves
� Identical to the Velocity Obstacles method, except that collision avoidance is
shared between interacting agents – fairness property
Reciprocal Velocity Obstacles | working principle
rp
|Autonomous Mobile RobotsMargarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Collision Avoidance 29
yv
xv
Uτ
τ
≤≤
−=
t
RORORO
t
r
tDVO
0
,p
ASL
Autonomous Systems Lab
� The robot is assumed to move on piece-wise linear curves
� Identical to the Velocity Obstacles method, except that collision avoidance is
shared between interacting agents – fairness property
Reciprocal Velocity Obstacles | working principle
rp
|Autonomous Mobile RobotsMargarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Collision Avoidance 30
yv
xv
Uτ
τ
≤≤
−=
t
RORORO
t
r
tDVO
0
,p
ASL
Autonomous Systems Lab
� The robot is assumed to move on piece-wise linear curves
� Identical to the Velocity Obstacles method, except that collision avoidance is
shared between interacting agents – fairness property
Reciprocal Velocity Obstacles | working principle
rp
|Autonomous Mobile RobotsMargarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Collision Avoidance 31
yv
xv
Uτ
τ
≤≤
−=
t
RORORO
t
r
tDVO
0
,p
ASL
Autonomous Systems Lab
� The robot is assumed to move on piece-wise linear curves
� Identical to the Velocity Obstacles method, except that collision avoidance is
shared between interacting agents – fairness property
Reciprocal Velocity Obstacles | working principle
|Autonomous Mobile RobotsMargarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Collision Avoidance 32
yv
xv
ASL
Autonomous Systems Lab
Reciprocal Velocity Obstacles | working principle
� The robot is assumed to move on piece-wise linear curves
� Identical to the Velocity Obstacles method, except that collision avoidance is
shared between interacting agents – fairness property
|Autonomous Mobile RobotsMargarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Collision Avoidance 33
yv
xv
ASL
Autonomous Systems Lab
Reciprocal Velocity Obstacles | properties
� Cost function is prone to local optima
� Interaction is handled via a fairness property
� The method is restricted to agents with omni-directional actuation
|Autonomous Mobile RobotsMargarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Collision Avoidance 34
ASL
Autonomous Systems Lab
� Integration of more complex motion models into reciprocal collision avoidance
� Integration with global search methods
� M. Rufli, J. Alonso-Mora, and R. Siegwart. “Reciprocal Collision Avoidance with Motion Continuity Constraints”. IEEE Transactions on Robotics, 2013.
Collision Avoidance | further reading
|Autonomous Mobile RobotsMargarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Collision Avoidance 35
ASL
Autonomous Systems Lab
|Autonomous Mobile RobotsMargarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Potential Field Methods 36
Motion Planning | Potential Field MethodsAutonomous Mobile Robots
Martin Rufli – IBM Research GmbH
Margarita Chli, Roland Siegwart
ASL
Autonomous Systems Lab
Potential Field methods | overview
� Methods produce a potential field whose gradient the robot follows
� They are characterized by
� Being global, but at times remaining prone to local optima
� Implicit incorporation of (basic) system models
|Autonomous Mobile RobotsMargarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Potential Field Methods 37
ASL
Autonomous Systems Lab
� The method generates an attractive potential function centered at the goal
and local repulsive potentials around obstacles
Local Potential Fields | working principle
2
goalattatt )(2
1)( qqq −= kU
|Autonomous Mobile RobotsMargarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart
� The robot follows the gradient (force vector) of the overall summed potential
Potential Field Methods 38
≤
−=
otherwise0
)(if1
)(
1
2
1)( lim
2
lim
reprep
ρρρρ
qqq
kU
ASL
Autonomous Systems Lab
Local Potential Fields | working principle
� The method generates an attractive potential function centered at the goal
and local repulsive potentials around obstacles
|Autonomous Mobile RobotsMargarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Potential Field Methods 39
ASL
Autonomous Systems Lab
� Solutions form a control policy
� Solutions may be subject to to local minima due to the localness of
the repulsive potentials
� The formulation does not allow for the incorporation of agent
dynamic constraints
Local Potential Fields | properties
|Autonomous Mobile RobotsMargarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart
dynamic constraints
Potential Field Methods 40
ASL
Autonomous Systems Lab
� Robot follows solution to the Laplace Equation
� Boundary conditions, any mixture of
� Neumann: Equipotential lines lie orthogonal to obstacle boundaries
� Dirichlet: Obstacle boundaries attain constant potential
Harmonic Potential Fields | working principle
02
2
=∂
∂=∆ ∑
iq
UU
|Autonomous Mobile RobotsMargarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Potential Field Methods 41
ASL
Autonomous Systems Lab
� Robot follows solution to the Laplace Equation
� Boundary conditions, any mixture of
� Neumann: Equipotential lines lie orthogonal to obstacle boundaries
� Dirichlet: Obstacle boundaries attain constant potential
Harmonic Potential Fields | working principle
02
2
=∂
∂=∆ ∑
iq
UU
|Autonomous Mobile RobotsMargarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Potential Field Methods 42
Neumann Dirichlet
ASL
Autonomous Systems Lab
Harmonic Potential Fields | numeric solution
02
2
=∂
∂=∆ ∑
iq
UU
|Autonomous Mobile RobotsMargarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Potential Field Methods 43
( )∑=
+ −++=n
i
i
k
i
kkUU
nU
1
1)()(
2
1)( eqeqq δδ
δ
δ )()()(
qeqq
UUU i
i
−+≈∇
ASL
Autonomous Systems Lab
� Solutions form a control policy
� Solutions are free of local optima
� Closed-form solutions exist for simple object shapes only
Harmonic Potential Fields | properties
|Autonomous Mobile RobotsMargarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Potential Field Methods 44
ASL
Autonomous Systems Lab
� Consideration of orientation constraints
� R. A. Grupen, C. I. Connolly, K. X. Souccar, and W. P. Burleson: “Toward a Path Co-processor for Automated Vehicle Control”. In Proceedings of the IEEE Symposium on Intelligent Vehicles, 1995.
� Approximate integration of agent dynamic constraints
Potential Field methods | further reading
|Autonomous Mobile RobotsMargarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart
� Approximate integration of agent dynamic constraints
� A. A. Masoud. Kinodynamic Motion Planning: “A Novel Type of Nonlinear, Passive Damping Forces and Advantages”. IEEE Robotics & Automation Magazine, 17(1):85–99, 2010.
� C. Louste and A. Liegois. Path planning for Non-holonomic Vehicles: “A Potential Viscous Fluid Method”. Robotica, 20:291–298, 2002.
Potential Field Methods 45