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

&


P. Ögren (KTH)

N. Leonard (Princeton University)

Cen

tre f

or

Auto

nom

ou

s S

yst

em

s


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


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


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


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


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


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


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


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


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


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


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


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


MPC/CLF frameworkMPC/CLF framework

Primbs general form: Here we write:

Cen

tre f

or

Auto

nom

ou

s S

yst

em

s


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


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


Evaluated MPC TrajectoriesEvaluated MPC Trajectories

Cen

tre f

or

Auto

nom

ou

s S

yst

em

s


Simulation TrajectorySimulation Trajectory

Cen

tre f

or

Auto

nom

ou

s S

yst

em

s


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


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




How do we use singel vehicle Obstacle Avoidance?

Cen

tre f

or

Auto

nom

ou

s S

yst

em

s


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


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


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


ISS ) Uncertainty Region ISS ) Uncertainty Region

Uncertainty Region

Cen

tre f

or

Auto

nom

ou

s S

yst

em

s


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


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


Simulation TrajectoriesSimulation Trajectories

Cen

tre f

or

Auto

nom

ou

s S

yst

em

s


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


Comparison

a convergent dynamic window approach to obstacle avoidance & obstacle avoidance in formation

Documents