Motion Planning Autonomous Mobile Robots - ETH Z Autonomous Systems Lab | Autonomous Mobile Robots Margarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland

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


Introduction | the see – think – act cycle

“position“

global map

Cognition

Path Planning

knowledge,

data base

mission

commands

Localization

Map Building

environment model

local mappath


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


Introduction | the motion planning problem


Goal

ASL



Motion Planning | Introduction to Optimization TechniquesAutonomous Mobile Robots



ASL


Introduction | origins and historical developments

� Geometric optimization: Dido‘s problem


ASL


� Functional optimization: the Brachistochrone problem



ASL


� Optimal Control

� Dynamic Programming



ASL


1. Motion control

2. Local collision avoidance

3. Global search-based planning

Introduction | hierarchical decomposition


ASL


Introduction | work-space versus configuration-space

2θ


Work-spaceConfiguration-spacex

y

Work-space

1θ

2θ

x

y

Configuration-space

1θ

ASL


� 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 Mobile RobotsMargarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Collision Avoidance 11

Motion Planning | Collision AvoidanceAutonomous Mobile Robots



ASL


� 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


ASL


� 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


� 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



v


ω

ASL



v


ω

ASL



v


ω

ASL



v


ω

ASL


� 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


ASL


� 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

τ


ASL






τ


yv

xv

ASL






τ


yv

xv

ASL






τ


yv

xv

ORRRO rrt +<+vp

Uτ

τ

≤≤

−=

t

RORORO

t

r

tDVO

0

,p

ASL






τ


yv

xv

ASL






τ


yv

xv

ASL


� VO considers the velocity of other objects

� It is prone to local optima

� It does not model interaction effects

Velocity Obstacles (VO) | properties


ASL


Interactive collision avoidance | overview

� Methods compute actuator commands based on local environment


� Being light on computational resources

� Being purely local and thus prone to local optima

� Incorporation of system models and higher-order reflection


ASL



� Identical to the Velocity Obstacles method, except that collision avoidance is

shared between interacting agents – fairness property

Reciprocal Velocity Obstacles | working principle


ASL







ASL






rp


yv

xv

Uτ

τ

≤≤

−=

t

RORORO

t

r

tDVO

0

,p

ASL






rp


yv

xv

Uτ

τ

≤≤

−=

t

RORORO

t

r

tDVO

0

,p

ASL






rp


yv

xv

Uτ

τ

≤≤

−=

t

RORORO

t

r

tDVO

0

,p

ASL







yv

xv

ASL







yv

xv

ASL


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


ASL


� 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


ASL


|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



ASL


Potential Field methods | overview

� Methods produce a potential field whose gradient the robot follows


� Being global, but at times remaining prone to local optima

� Implicit incorporation of (basic) system models


ASL


� 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


� 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


Local Potential Fields | working principle

� The method generates an attractive potential function centered at the goal

and local repulsive potentials around obstacles


ASL


� 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


dynamic constraints


ASL


� 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


ASL


� 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


Neumann Dirichlet

ASL


Harmonic Potential Fields | numeric solution

02

2

=∂

∂=∆ ∑

iq

UU


( )∑=

+ −++=n

i

i

k

i

kkUU

nU

1

1)()(

2

1)( eqeqq δδ

δ

δ )()()(

qeqq

UUU i

i

−+≈∇

ASL


� Solutions form a control policy

� Solutions are free of local optima

� Closed-form solutions exist for simple object shapes only

Harmonic Potential Fields | properties


ASL


� 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


� 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.


Documents

Motion Planning Autonomous Mobile Robots - ETH Z Autonomous Systems Lab | Autonomous Mobile Robots Margarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland