Timm Faulwasser Laboratoire d’Automatique, Ecole Polytechnique Fédérale de LausanneInstitut für Angewandte Informatik, Karlsruher Institut für Technologie

Joint work with: Tobias Weber, Janine Matschek, Juan Pablo Zometa, Rolf Findeisen (OvGU MD)Sven Lorenz, Johann Dauer (DLR Braunschweig)

Elgersburg Workshop | 11.02.2016

Pfadverfolgung –Von Normalformen zu prädiktiven Reglern

Stabilization, Trajectory Tracking and Path Following?

Set-point Stabilization

• Reference set-point

• Control error

time invariant problem

2

Tim

m F

aulw

asse

r |

Pfa

dve

rfo

lgu

ng

-V

on

No

rmal

form

en z

u M

PC

2

Stabilization, Trajectory Tracking and Path Following?

Set-point Stabilization

• Reference set-point

• Control error

time invariant problem

Trajectory Tracking

• Reference trajectory

• Tracking error

time varying problemperformance limits?

3

Tim

m F

aulw

asse

r |

Pfa

dve

rfo

lgu

ng

-V

on

No

rmal

form

en z

u M

PC

Qui, L. & Davison, E. Performance limitations of non-minimum phase systems in the servomechanism problem. Automatica, 1993, 29, 337-349 Seron, M.; Braslavsky, J.; Kokotovic, P. V. & Mayne, D. Q. Feedback limitations in nonlinear systems: from Bode integrals tocheap control. IEEE Trans. Automat. Contr., 1999, 44, 829-833 3

Stabilization, Trajectory Tracking and Path Following?

Applications? Controller design?

Trajectory Tracking

• Reference trajectory

• Tracking error

time varying problemperformance limits?

Path Following

• Reference path

• Path-following error

time invariant problemadditional design parameters

4

Tim

m F

aulw

asse

r |

Pfa

dve

rfo

lgu

ng

-V

on

No

rmal

form

en z

u M

PC

4

Path Following

Typical Applications

• Autonomous vehicles

• Machine tools

• Motion problems

• …

Problem definition? Controller design?

Tim

m F

aulw

asse

r |

Pfa

dve

rfo

lgu

ng

-V

on

No

rmal

form

en z

u M

PC

5

Outline

Tim

m F

aulw

asse

r |

Pfa

dve

rfo

lgu

ng

-V

on

No

rmal

form

en z

u M

PC

Model Predictive Path Following Control• Convergence properties• MPFC for a robotic manipulator• Feedforward path following for an autonomous helicopter

Outlook and Summary

Path Following• Set-point stabilization, trajectory tracking and path following• Problem analysis: tailored normal forms• Path followability results• Examples• Feedforward path following and closed-loop path following

6

Output Path-following Problem

Control task

• Convergence to path

• Convergence on path

• Satisfaction of constraints

Questions

• Problem structure Suitable formulation?• Constraints Controller design?

• Dynamic system

• Output path regular 1d curve

Tim

m F

aulw

asse

r |

Pfa

dve

rfo

lgu

ng

-V

on

No

rmal

form

en z

u M

PC

7

Analysis of Path-following Problems

state space output space

• = path manifold

Tim

m F

aulw

asse

r |

Pfa

dve

rfo

lgu

ng

-V

on

No

rmal

form

en z

u M

PC

8

Vector Relative Degree (I)

Tim

m F

aulw

asse

r |

Pfa

dve

rfo

lgu

ng

-V

on

No

rmal

form

en z

u M

PC

[Isidori `95; Nijmeijer & van der Schaft `90] 9

Vector Relative Degree (II)

Tim

m F

aulw

asse

r |

Pfa

dve

rfo

lgu

ng

-V

on

No

rmal

form

en z

u M

PC

[Isidori `95; Nijmeijer & van der Schaft `90] 10

Vector Relative Degree (III)

Tim

m F

aulw

asse

r |

Pfa

dve

rfo

lgu

ng

-V

on

No

rmal

form

en z

u M

PC

[Isidori `95; Nijmeijer & van der Schaft `90] 11

Analysis of Path-following Problems

State space Output space

• = path manifold

Tim

m F

aulw

asse

r |

Pfa

dve

rfo

lgu

ng

-V

on

No

rmal

form

en z

u M

PC

Augmented systemTiming law

12

Idea: map to suitable coordinates

Transverse Normal Form

Augmented system Transverse normal form

Tim

m F

aulw

asse

r |

Pfa

dve

rfo

lgu

ng

-V

on

No

rmal

form

en z

u M

PC

[Nielsen & Maggiore `08; Banaszuk & Hauser `95; F. & Findeisen `16]

13

Idea: map to suitable coordinates

Transverse Normal Form

Augmented system Transverse normal form

Example? Path followability?

Tim

m F

aulw

asse

r |

Pfa

dve

rfo

lgu

ng

-V

on

No

rmal

form

en z

u M

PC

14

Example – Fully Actuated Robot

Tim

m F

aulw

asse

r |

Pfa

dve

rfo

lgu

ng

-V

on

No

rmal

form

en z

u M

PC

Augmented system

15

Example – Fully Actuated Robot

Tim

m F

aulw

asse

r |

Pfa

dve

rfo

lgu

ng

-V

on

No

rmal

form

en z

u M

PC

Augmented system Transverse normal form

16

Path Followability and Transverse Normal Forms

Augmented system Transverse normal form

Necessary conditions? Constraints? Feedback design?

Tim

m F

aulw

asse

r |

Pfa

dve

rfo

lgu

ng

-V

on

No

rmal

form

en z

u M

PC

17

[Faulwasser `13]

Path Followability in the Presence of Constraints

Feedback control?

[Faulwasser `13]

Tim

m F

aulw

asse

r |

Pfa

dve

rfo

lgu

ng

-V

on

No

rmal

form

en z

u M

PC

18

Feedforward Path Following

Tim

m F

aulw

asse

r |

Pfa

dve

rfo

lgu

ng

-V

on

No

rmal

form

en z

u M

PC

19

Closed-loop Path Following

Tim

m F

aulw

asse

r |

Pfa

dve

rfo

lgu

ng

-V

on

No

rmal

form

en z

u M

PC

20

Outline

Tim

m F

aulw

asse

r |

Pfa

dve

rfo

lgu

ng

-V

on

No

rmal

form

en z

u M

PC

Model Predictive Path Following Control• Convergence properties• MPFC for a robotic manipulator• Feedforward path following for an autonomous helicopter

Outlook and Summary

Path Following• Set-point stabilization, trajectory tracking and path following• Problem analysis: tailored normal forms• Path followability results• Examples• Feedforward path following and closed-loop path following

21

Principle of Predictive Control

NMPC for path-following problems?

Tim

m F

aulw

asse

r |

Pfa

dve

rfo

lgu

ng

-V

on

No

rmal

form

en z

u M

PC

22

Model Predictive Control (MPC) = repeated optimal control

Advantages: constraints, MIMO systems, optimization of transients, dist. implementation

1. State observation at

2. Solve

3. Apply for

Principle of Predictive Path Following

Idea• Prediction based on augmented dynamics.• Costs penalize path-following error (and its time derivative).

Stability/convergence? Example?

Optimal Control Problem

23

Tim

m F

aulw

asse

r |

Pfa

dve

rfo

lgu

ng

-V

on

No

rmal

form

en z

u M

PC

23

Application to a Robotic Manipulator

Benchmark Problem• Robot KUKA LBR IV up to 7 joints.• Make the robot write on a blackboard.• Allow interaction between user and robot. Ti

mm

Fau

lwas

ser

| P

fad

verf

olg

un

g -

Vo

n N

orm

alfo

rmen

zu

MP

C

24

Implementation of Predictive Path-Following on KUKA LWR IV

• Dynamics of the robot

– State space representation

– Path parameter dynamics

– Error outputs

• Cost function, terminal penalty

• Prediction horizon , sampling time

• Optimization solved with ACADO Toolkit

Results?26

Tim

m F

aulw

asse

r |

Pfa

dve

rfo

lgu

ng

-V

on

No

rmal

form

en z

u M

PC

26

Application to a Robotic Manipulator

Joint work with: Juan P. Zometa, Janine Matschek, Tobias Weber, Rolf Findeisen (all OvG Magdeburg) [Faulwasser et al. `15]

Tim

m F

aulw

asse

r |

Pfa

dve

rfo

lgu

ng

-V

on

No

rmal

form

en z

u M

PC

27

Application to a Robotic Manipulator

Joint work with: Juan P. Zometa, Janine Matschek, Tobias Weber, Rolf Findeisen (all OvG Magdeburg)

Tim

m F

aulw

asse

r |

Pfa

dve

rfo

lgu

ng

-V

on

No

rmal

form

en z

u M

PC

[Faulwasser et al. `15]

28

Application to a Robotic Manipulator

Tim

m F

aulw

asse

r |

Pfa

dve

rfo

lgu

ng

-V

on

No

rmal

form

en z

u M

PC

29

Application to a Robotic Manipulator

Tim

m F

aulw

asse

r |

Pfa

dve

rfo

lgu

ng

-V

on

No

rmal

form

en z

u M

PC

30

References

Tim

m F

aulw

asse

r |

Pfa

dve

rfo

lgu

ng

-V

on

No

rmal

form

en z

u M

PC

Lyapunov and back-stepping approaches to path following• Aguiar et al. (2005). Path-following for non minimumphase systems removes performanc e limitations. IEEE Transactions on

Automatic Control 50, 234-239.

• Dacic & Kokotovic (2006). Path-following for linear systems with unstable zero dynamics. Automatica 42, 1673-1683.

• Aguiar et al. (2008). Performance limitations in reference tracking and path-following for nonlinear systems. Automatica 44, 598-610.

• Skjetne et al. (2005). Robust output maneuvering for a class of nonlinear systems. Automatica 40, 373-383.

• Do & Pan (2006). Global robust adaptive path followng of underactuated ships. Automatica 42, 1713-1722.

• ...

Differential geometric reformulation• Banaszuk & Hauser (1995). Feedback linearization of transverse dynamics for periodic orbits

Sys. Contr. Lett., 26, 95-105.

• Nielsen & Maggiore (2008). On local transverse feedback linearization. SIAM Journal on Control and Optimization, 47, 2227-2250.

• Faulwasser (2013). Optimization-based Solutions to Constrained Trajectory-tracking and Path-following Problems. Shaker, Aachen, Germany.

• …

Feedforward path following• Shin & McKay (1985). Minimum-time control of robotic manipulators with geometric path constraints

IEEE Trans. Automat. Contr., 30, 531 – 541.

• Verscheure et al. (2009). Time-Optimal Path Tracking for Robots: A Convex Optimization Approach.IEEE Trans. Autom. Contr., 54, 2318-2327.

• Faulwasser, Hagenmeyer & Findeisen (2011). Optimal Exact Path-Following for Constrained Differentially Flat Systems. Proc. of 18th IFAC World Congress, pp. 9875–9880.

• Faulwasser, Hagenmeyer & Findeisen (2014). Constrained Reachability and Trajectory Generation for Flat Systems. Automatica, vol. 50, pp. 1151-1159.

31

ReferencesPredictive control for path following (theory)• Faulwasser & Findeisen (2008). Nonlinear model predictive path-following control. Proc. of Workshop Future Directions of

Nonlinear Model Predictive Control, Pavia, Italy.• Faulwasser, Kern & Findeisen (2009). Model predictive path-following for constrained nonlinear systems. Proc. of 48th IEEE

Conference on Decision and Control, pp. 8642-8647.

• Faulwasser & Findeisen (2016). Nonlinear Model Predictive Control for Constrained Output Path Following. IEEE Trans. Automatic Control.

• Faulwasser (2013). Optimization-based Solutions to Constrained Trajectory-tracking and Path-following Problems. Shaker, Aachen, Germany.

Predictive control for path following (applications)

• Faulwasser et al. (2013). Predictive Path-following Control: Concept and Implementation for an Industrial Robot. Proc. of IEEE Conference on Control Applications (CCA), pp. 128-133.

• Dauer et al. (2013). Optimization-based Feedforward Path Following for Model Reference Adaptive Control of an Unmanned Helicopter. Proc. of AIAA Guidance, Navigation and Control Conference.

• Lam et al. (2013). Model predictive contouring control for biaxial systems. IEEE Trans. Contr. Syst. Techn., 21, 552-559.

• Böck & Kugi (2014). Real-time Nonlinear Model Predictive Path-Following Control of a Laboratory Tower CraneIEEE Trans. Contr. Syst. Techn., 22, 1461-1473.

• Faulwasser et al. (2015). Implementation of Constrained Nonlinear Model Predictive Path-Following Control for an Industrial Robot. Submitted to IEEE Trans. Contr. Syst. Techn.

Tim

m F

aulw

asse

r |

Pfa

dve

rfo

lgu

ng

-V

on

No

rmal

form

en z

u M

PC

32

Summary and Conclusion

Summary

• Path following: combines trajectory generation and tracking

• Typical applications of path following: autonomous vehicles & robots, machine tools, …

• Tailored Byrnes-Isidori Normal Form is the key step for analysis

• MPC can be used to tackle path-following problems

Open questions

• Combination of path following and force control?

• Model uncertainty and repetitive motions?

• Implication of exo-systems with inputs for regulator problems?

• …

Thanks to: • Juan Pablo Zometa, Tobias Weber, Janine Matschek, Rolf Findeisen (OvG University Magdeburg)• Johann Dauer, Sven Lorenz (DLR Braunschweig)

33

Tim

m F

aulw

asse

r |

Pfa

dve

rfo

lgu

ng

-V

on

No

rmal

form

en z

u M

PC

33