centre for autonomous systems petter Ögrenoptsyst seminar 23/51 formations and obstacle avoidance...

Cen

tre f

or

Auto

nom

ou

s S

yst

em

s

Petter Ögren OptSyst seminar 23/5 1

Formations and Obstacle Avoidance in Mobile Robot Control

Formations and Obstacle Avoidance in Mobile Robot Control

Petter Ögren

Topics from a Doctoral Thesis, 2003

Cen

tre f

or

Auto

nom

ou

s S

yst

em

s


Outline Outline

A brief introduction of all four papers

Overview of how they relate to each other

Details of Paper A

Details of Paper B

Cen

tre f

or

Auto

nom

ou

s S

yst

em

s


All four Papers All four Papers

Obstacle Avoidance

Formations

Paper B

Paper C

Paper A

Paper D

Cen

tre f

or

Auto

nom

ou

s S

yst

em

s


Paper A: A Convergent Dynamic Window Approach to

Obstacle Avoidance


Obstacle Avoidance

Problem formulation: Drive a robot from A to B through a partially unknown environment without collisions.

A

B

Cen

tre f

or

Auto

nom

ou

s S

yst

em

s



Obstacle Avoidance


Obstacle Avoidance

Proposed solution: Merge state-of-the-art heuristics with a provable approach, (using a CLF/MPC framework).

)Optimize pointwise over stabilizing controls

Cen

tre f

or

Auto

nom

ou

s S

yst

em

s


Problem: How do we move the leader to guide a leader-follower formation through obstacle terrain?

Can we use singel vehicle Obstacle Avoidance?

Paper B: Obstacle Avoidance in Formation


Cen

tre f

or

Auto

nom

ou

s S

yst

em

s




Proposed solution: The concept of Configuration Space Obstacles is extended through an Input to State Stability (ISS) argument.

)A map of the leader positions that guarantee followers enough free space. The leader does single vehicle obstacle avoidance using this map.

Cen

tre f

or

Auto

nom

ou

s S

yst

em

s


Paper C: A Control Lyapunov Function Approach to

Multi Agent Coordination



Problem: How to make set-point controlled robots moving along trajectories in a formation ”wait” for eachother?

Cen

tre f

or

Auto

nom

ou

s S

yst

em

s


Idea: Combine Control Lyapunov Functions (CLF) with the Egerstedt&Hu virtual vehicle approach.Under assumptions this will result in:

Bounded formation error (waiting)Approximation of given formation velocity (if no waiting is necessary).Finite completion time (no 1-waiting).





Cen

tre f

or

Auto

nom

ou

s S

yst

em

s


Paper D: Cooperative Control of Mobile Sensor Networks: Adaptive Gradient Climbing in a Distributed Environment


Problem D1: Given a local-spring-damper formation control.

How do we translate, rotate and expand the formation?

Problem D2: Given a field, i.e. temperature or nutrition density in water.

How do we estimate the gradient from noisy distributed measurements?

What formation geometries give good estimates?

Cen

tre f

or

Auto

nom

ou

s S

yst

em

s




Proposed solution D1:

Introduce virtual leaders in the formation and move these.

Let direction of motion be governed by the mission, e.g. gradient climbing.

Let the speed of the motion be influenced by error feedback (from paperC).

Cen

tre f

or

Auto

nom

ou

s S

yst

em

s




Proposed solution D2: Estimate the gradient: Use the least Squares estimate of (a,b) in an affine approximation aTz+b ¼ T(z). Apply Kalman filter over time.Formation geometries: Minimize error due to measurement noise and second order terms. In 1-dimension:

Estimate

True

Noisy

This is generalized to m vehicles in Rn .

Cen

tre f

or

Auto

nom

ou

s S

yst

em

s



Paper A

Paper B

Paper D

Paper C

Details!

Cen

tre f

or

Auto

nom

ou

s S

yst

em

s


Drive a robot from A to B through a partiallyunknown environment without collisions.

A

B

Differential drive robots can be feedback linearized to this.


Obstacle Avoidance


Obstacle Avoidance

Cen

tre f

or

Auto

nom

ou

s S

yst

em

s


Background: The Dynamic Unicycle

Background: The Dynamic Unicycle

Cen

tre f

or

Auto

nom

ou

s S

yst

em

s


Desirable Properties in Obstacle AvoidanceDesirable Properties

in Obstacle Avoidance

No collisions

Convergence to goal position

Efficient, large inputs

‘Real time’

‘Reactive’, (to changes in environment)

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 connected to 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



Paper A

Paper B

Paper D

Paper C

Details!

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, lawn moving etc. Carry large/awkward objectsAdaptive sensing, e.g. surveillance or ocean samplingSatellite 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


Conclusions, paper BConclusions, paper B

Obstacle Avoidance extended to formations by assuming leader-follower structure and ISS.

Future directionsRotations

Expansions

Breaking formation

) ¸ 3 dim NF

Cen

tre f

or

Auto

nom

ou

s S

yst

em

s



Paper A

Paper B

Paper D

Paper C

Details!

Cen

tre f

or

Auto

nom

ou

s S

yst

em

s


End of Presentation.End of Presentation.

Cen

tre f

or

Auto

nom

ou

s S

yst

em

s




P. Ögren, M. Egerstedt* and X. HuRoyal Institute of Technology (KTH), Stockholm

and Georgia Institute of Technology*

IEEE Transactions on Robotics and Automation, Oct 2002



P. Ögren, M. Egerstedt* and X. HuRoyal Institute of Technology (KTH), Stockholm

and Georgia Institute of Technology*

IEEE Transactions on Robotics and Automation, Oct 2002

Cen

tre f

or

Auto

nom

ou

s S

yst

em

s


Problem and Proposed Solution Problem and Proposed Solution

Problem: How to make set-point controlled robots moving along trajectories in a formation ”wait” for eachother?Idea: Combine Control Lyapunov Functions (CLF) with the Egerstedt&Hu virtual vehicle approach.Under assumptions this will result in:

Bounded formation error (waiting)Approx. of given formation velocity (if no waiting is nessesary).Finite completion time (no 1-waiting).

Cen

tre f

or

Auto

nom

ou

s S

yst

em

s


Quantifying Formation KeepingQuantifying Formation Keeping

Will add Lyapunov like assumption satisfied by individual set-point controllers. =>

Think of as parameterized Lyapunov function.

Definition: Formation Function

Cen

tre f

or

Auto

nom

ou

s S

yst

em

s


Examples of Formation FunctionExamples of Formation Function

• Simple linear example !• A CLF for the combined

higher dimensional system:

Note that a,b, are design parameters.

• The approach applies to any parameterized formation scheme with lyapunov stability results.

Cen

tre f

or

Auto

nom

ou

s S

yst

em

s


Main AssumptionMain Assumption

We can find a class K function such that the given set-point controllers satisfy:

This can be done when -dV/dt is lpd, V is lpd and decrescent. It allows us to prove:

Bounded V (error): V(x,s) < VU

Bounded completion time. Keeping formation velocity v0, if V ¿ VU.

Cen

tre f

or

Auto

nom

ou

s S

yst

em

s


Speed along trajectory:

How Do We Update s? Speed along trajectory:

How Do We Update s? Suggestion: s=v0 t

Problems: Bounded ctrl or local ass stability

We want:V to be smallSlowdown if V is largeSpeed v0 if V is small

Suggestion:Let s evolve with feedback from V.

Cen

tre f

or

Auto

nom

ou

s S

yst

em

s


Evolution of sEvolution of s

Choosing to be:

We can prove:

Bounded V (error): V(x,s) < VU

Bounded completion time. Keeping formation velocity v0, if V ¿ VU.

Cen

tre f

or

Auto

nom

ou

s S

yst

em

s


Proof sketch: Formation error Proof sketch: Formation error

Cen

tre f

or

Auto

nom

ou

s S

yst

em

s


Proof sketch: Finite Completion Time Proof sketch: Finite Completion Time

Find lower bound on ds/dt

Cen

tre f

or

Auto

nom

ou

s S

yst

em

s


Example: FormationExample: Formation

Three unicycle robots along trajectory.VU=1, v0=0.1, then v0=0.3 ! 0.27Stochastic measurement error in top robot at 12m mark.

Cen

tre f

or

Auto

nom

ou

s S

yst

em

s


Extending Work by Beard et. al. Extending Work by Beard et. al.

”Satisficing Control for Multi-Agent Formation Maneuvers”, in proc. CDC ’02 It is shown how to find an explicit parameterization of the stabilizing controllers that fulfills the assumption

These controllers are also inverse optimal and have robustness properties to input disturbancesImplementation

Cen

tre f

or

Auto

nom

ou

s S

yst

em

s



Paper A

Paper B

Paper D

Paper C

Details!

Cen

tre f

or

Auto

nom

ou

s S

yst

em

s


Formations with a Mission: Stable Coordination of Vehicle Group Maneuvers

Mathematical Theory of Networks and Systems (MTNS ‘02)

Visit: http://graham.princeton.edu/ for related information

Edward Fiorelli and Naomi Ehrich Leonard [email protected], [email protected]

Mechanical and Aerospace Engineering

Princeton University, USA

Optimization and Systems Theory

Royal Institute of Technology, Sweden

Petter [email protected]

Another extension:Another extension:

Cen

tre f

or

Auto

nom

ou

s S

yst

em

s


•Configuration space of virtual body is for orientation, position and expansion factor:

• Because of artificial potentials, vehicles in formation will translate, rotate, expand and contract with virtual body.

• To ensure stability and convergence, prescribe virtual body dynamics so that its speed is driven by a formation error.

• Define direction of virtual body dynamics to satisfy mission.

• Partial decoupling: Formation guaranteed independent of mission.

• Prove convergence of gradient climbing.

Approach: Use artificial potentials and virtual body with dynamics.

Approach: Use artificial potentials and virtual body with dynamics.

Cen

tre f

or

Auto

nom

ou

s S

yst

em

s


What if dV/dt <= 0 ?What if dV/dt <= 0 ?

If we have semidefinite and stability by La Salle’s principle we choose as:

By a renewed La Salle argument we can still show: V<=VU , s! sf and x! xf.

But not: Completion time and v0.

Cen

tre f

or

Auto

nom

ou

s S

yst

em

s


ConclusionsConclusions

Moving formations by using Control Lyapunov Functions.Theoretical Properties:

V <= VU, error

T < TU, time

v ¼ v0 velocity

Extension used for translation, rotation and expansion in gradient climbing mission

centre for autonomous systems petter Ögrenoptsyst seminar 23/51 formations and obstacle avoidance...

Documents

paper d slide

details of paper b slide

damper formation control

formation geometries

obstacle terrain

leaderfollower formation

multi agent coordination

provable approach