commande optimale (critère quadratique) et filtrage de kalman

Des équations similaires parce qu’on minimise un critère quadratique

Justification fondée sur le principe de Bellman et une écriturerécursive de la solution du problème de commande optimale

commande optimale : un système est dans un état initial connu ;comment l’amener à un état final donné en un nombre fini d’étapeset en minimisant un critère quadratique

y(t)

mémoire(retard)

z-1A

Bu(t)

Commande(entrée)

x(t+1) C+ +

x(t)état

sortie(mesure)

x(t+1)=A.x(t)+B.u(t)

D

Joël Le Roux, EPU, Université de Nice ; leroux@polytech.unice.fr

N

nmmm

Tmmm

Tmnn

Tn uRuxQxxPx )(

Le critère qu’on cherche à minimiser

il fait intervenir les états et les commandes ; les matrices Q et R sont connueset définies positives

nnTnnn

Tnnn

Tnnn

Tn uRuxQxxPxxPx 111

nnTnnn

Tn

N

nmmm

Tmmm

Tmnn

Tn uRuxQxuRuxQxxPx

1

)(

formalisation du critère 1

notez l’écritu

re!

0

)(nm

mmTmmm

Tmnn

Tn uRuxQxxPx

Le critère qu’on cherche à minimiser

il fait intervenir les états et les commandes ; les matrices Q et R sont connueset définies positives

nnTnnn

Tnnn

Tnnn

Tn uRuxQxxPxxPx 111

nnn uBxAx ..1

nnTnnn

Tnnnn

Tnnnn

Tn uRuxQxuBxAPuBxAxPx )..()..( 1

formalisation du critère 2

0

)(nm

mmTmmm

Tmnn

Tn uRuxQxxPx

Le critère qu’on cherche à minimiser

il fait intervenir les états et les commandes ; les matrices Q et R sont connueset définies positives

nnTnnn

Tnnn

Tnnn

Tn uRuxQxxPxxPx 111

nnn uBxAx ..1

nnTnnn

Tnnnn

Tnnnn

Tn uRuxQxuBxAPuBxAxPx )..()..( 1

nnTnnn

TTnnn

TTnnn

TTn

nnTnnn

TTnnn

Tn

uRuuBPBuxAPBuuBPAx

xQxxAPAxxPx

.).(..

..

111

1

formalisation du critère 3

nnnnTT

nnnTT

nnnTT

n

nnTnnn

TTnnn

Tn

uRuBPBuxAPBuuBPAx

xQxxAPAxxPx

).().(..

..

111

1

)..().(..

.. 1

nnT

nnnTT

n

nnTnnn

TTnnn

Tn

xauxauxx

xQxxAPAxxPx

qu’on cherche à mettre sous la forme

dans le cas scalaire2

22

222

.2.

.4.....

cu

c

bxx

c

bxaucuxbxa

nTT

nnTT

nnTT

nnTT

nnTT

n

nnTnnn

TTnnn

Tn

auauxauauxxxxx

xQxxAPAxxPx

....

.. 1

formalisation du critère 4

nnnTT

nnnTT

nnnTT

n

nnTnnn

TTnnn

Tn

uRBPBuxAPBuuBPAx

xQxxAPAxxPx

)(...

..

111

1

)..().(..

.. 1

nnT

nnnTT

n

nnTnnn

TTnnn

Tn

xauxauxx

xQxxAPAxxPx

qu’on cherche à mettre sous la forme

pour que les expressions soient identiques, il faut que

BPBR nTT ... 1BPA n

TT ... 1

nTT

nnTT

nnTT

nnTT

nnTT

n

nnTnnn

TTnnn

Tn

auauxauauxxxxx

xQxxAPAxxPx

....

.. 1

formalisation du critère 5

BPBR nTT ... 1 BPA n

TT ... 1

2/11

2/1 )...()..(. BPBRBPBR n

TTn

TT

Factorisation en racines carrées (triangulaires)

APB nTT .. 1

2/11 )..( BPBR n

T

)..().(..

.. 1

nnT

nnnTT

n

nnTnnn

TTnnn

Tn

xauxauxx

xQxxAPAxxPx

xAPBBPBRuxAPBu

xu

nT

nT

nTT

....)..(...

.

11

1

11

1

le critère est minimum quand le dernier terme est nul

minimisation du critère 1

)..().(..

.. 1

nnT

nnnTT

n

nnTnnn

TTnnn

Tn

xauxauxx

xQxxAPAxxPx

nT

nnTT

nnnTn xQAPAxxPx )..( 1

evaluation du critère quand le minimum est atteint

minimisation du critère 2

)..().(..

.. 1

nnT

nnnTT

n

nnTnnn

TTnnn

Tn

xauxauxx

xQxxAPAxxPx

nT

nnTT

nnnTn xQAPAxxPx )..( 1

nnTT

nT

nnTT

nnnTn xAPBBPAQAPAxxPx ).....( 1

111

evaluation du critère quand le minimum est atteint

APB nTT .. 1

minimisation du critère 3

)..().(..

.. 1

nnT

nnnTT

n

nnTnnn

TTnnn

Tn

xauxauxx

xQxxAPAxxPx

nT

nnTT

nnnTn xQAPAxxPx )..( 1

nnTT

nT

nnTT

nnnTn xAPBBPAQAPAxxPx ).....( 1

111

nnT

nT

nT

nnTT

nnnTn xAPBBPBRBPAQAPAxxPx )..)...(..( 1

1111

evaluation du critère quand le minimum est atteint

BPBR nTT ... 1

APBBPBRBPAQAPAP nT

nT

nT

nnT

n ..)...(.. 11

111

minimisation du critère 4

APBBPBRBPAQAPAP nT

nT

nT

nnT

n ..)...(.. 11

111

xAPBBPBRu nT

nT ....)..( 1

11

expression finale des calculs de commande optimale

1)()./1().().()./1()(

TcTTPTcrTcTTPTk Tv

)/1().().()/1()1( TTPTcTkTTPTP T

wT rTATPTATTP )().().()/1(

Equations correspondantes du filtre de Kalman

Cas élémentaire

10

1 A

1

0B

0

0NX

minimiser2

1

0

2N

N

mm XuE

pénalisation si l’état final n’est pas

v

pX 0

état initial état final voulu

0

0NX

Illustration par une simulation (mathcad)

p 1- état initial v 5X

0

p

v

- nombre d'étapesN 1000

- matrices du système 0.001

A1

0

1

B0

1

calcul récursif de la matrice P

traduit la pénalisation si l’état final n’est pas

0

0NX

- pénalisation de la commande uR 0.001

- fonction de calcul de la matrice P antecedant P( ) AT P A AT P B R BT P B 1 BT P A

- initialisation de la matrice PPN

1

0

0

1

1

- calcul récursif (rétrograde) des P m N 1 0

Pm

antecedant Pm 1

calcul itératif de la commande

- fonction de calcul de la commande optimale

commande P X( ) R BT P B 1BT P A X

XetU P P

X X

Un

commande Pn 1 X

n

Xn 1 A X

n B U

n

n 0 N 1for

UN

0

pile X U( )return

- calcul récursif (temps croissant) de lacommande et de l'état

evolution de la matrice P

P_00 P_01 P_10 P_11

tabP

0 1 2 3

0

1

2

3

4

5

6

7

8

9

10

11

12

13

14

15

1.2·10 -5 6.006·10 -6 6.006·10 -6 4.006·10 -6

1.204·10 -5 6.018·10 -6 6.018·10 -6 4.01·10 -6

1.207·10 -5 6.03·10 -6 6.03·10 -6 4.014·10 -6

1.211·10 -5 6.042·10 -6 6.042·10 -6 4.018·10 -6

1.214·10 -5 6.054·10 -6 6.054·10 -6 4.022·10 -6

1.218·10 -5 6.066·10 -6 6.066·10 -6 4.026·10 -6

1.222·10 -5 6.079·10 -6 6.079·10 -6 4.03·10 -6

1.226·10 -5 6.091·10 -6 6.091·10 -6 4.034·10 -6

1.229·10 -5 6.103·10 -6 6.103·10 -6 4.038·10 -6

1.233·10 -5 6.116·10 -6 6.116·10 -6 4.042·10 -6

1.237·10 -5 6.128·10 -6 6.128·10 -6 4.046·10 -6

1.24·10 -5 6.14·10 -6 6.14·10 -6 4.051·10 -6

1.244·10 -5 6.153·10 -6 6.153·10 -6 4.055·10 -6

1.248·10 -5 6.165·10 -6 6.165·10 -6 4.059·10 -6

1.252·10 -5 6.178·10 -6 6.178·10 -6 4.063·10 -6

1.256·10 -5 6.19·10 -6 6.19·10 -6 4.067·10 -6

evolution de la matrice P

P_00 P_01 P_10 P_11

tabP

0 1 2 3

985

986

987

988

989

990

991

992

993

994

995

996

997

998

999

1000

0.781 6.25·10 -3 6.25·10 -3 1.167·10 -4

0.815 6.11·10 -3 6.11·10 -3 1.173·10 -4

0.846 5.922·10 -3 5.922·10 -3 1.184·10 -4

0.875 5.687·10 -3 5.687·10 -3 1.203·10 -4

0.901 5.406·10 -3 5.406·10 -3 1.233·10 -4

0.924 5.081·10 -3 5.081·10 -3 1.279·10 -4

0.943 4.717·10 -3 4.717·10 -3 1.347·10 -4

0.96 4.319·10 -3 4.319·10 -3 1.444·10 -4

0.973 3.891·10 -3 3.891·10 -3 1.584·10 -4

0.983 3.44·10 -3 3.44·10 -3 1.787·10 -4

0.99 2.971·10 -3 2.971·10 -3 2.089·10 -4

0.995 2.488·10 -3 2.488·10 -3 2.562·10 -4

0.998 1.996·10 -3 1.996·10 -3 3.372·10 -4

1 1.499·10 -3 1.499·10 -3 5.02·10 -4

1 1·10 -3 1·10 -3 1·10 -3

1 0 0 1

- position vitesse commande valeur du critère

tabX

0 1 2 3

985

986

987

988

989

990

991

992

993

994

995

996

997

998

999

1000

1.938·10 -3 -0.236 0.015 3.7·10 -6

1.702·10 -3 -0.22 0.015 3.462·10 -6

1.482·10 -3 -0.205 0.015 3.224·10 -6

1.278·10 -3 -0.189 0.015 2.984·10 -6

1.089·10 -3 -0.174 0.016 2.743·10 -6

9.149·10 -4 -0.158 0.016 2.5·10 -6

7.568·10 -4 -0.142 0.016 2.256·10 -6

6.144·10 -4 -0.127 0.016 2.011·10 -6

4.876·10 -4 -0.111 0.016 1.765·10 -6

3.765·10 -4 -0.095 0.016 1.517·10 -6

2.812·10 -4 -0.08 0.016 1.268·10 -6

2.016·10 -4 -0.064 0.016 1.017·10 -6

1.379·10 -4 -0.048 0.016 7.654·10 -7

9·10 -5 -0.032 0.016 5.123·10 -7

5.804·10 -5 -0.016 0.016 2.578·10 -7

4.203·10 -5 -1.599·10 -5 0 2.022·10 -9

- position vitesse commande valeur du critère

tabX

0 1 2 3

0

1

2

3

4

5

6

7

8

9

10

11

12

13

14

15

1 5 4.203·10 -5 1.722·10 -4

1.005 4.974 -0.026 1.715·10 -4

1.01 4.948 -0.026 1.709·10 -4

1.015 4.922 -0.026 1.702·10 -4

1.02 4.896 -0.026 1.695·10 -4

1.025 4.87 -0.026 1.689·10 -4

1.03 4.845 -0.026 1.682·10 -4

1.034 4.819 -0.026 1.675·10 -4

1.039 4.793 -0.026 1.669·10 -4

1.044 4.768 -0.026 1.662·10 -4

1.049 4.742 -0.026 1.655·10 -4

1.054 4.716 -0.026 1.649·10 -4

1.058 4.691 -0.026 1.642·10 -4

1.063 4.665 -0.025 1.636·10 -4

1.068 4.64 -0.025 1.629·10 -4

1.072 4.615 -0.025 1.623·10 -4

0 1 24

2

0

2

4

6

- vitesse

- vitesse + commande

- position

évolution de la position et de la vitesse en fonction de la commande

0 100 200 300 400 500 600 700 800 900 10004

2

0

2

4

6

.

iteration

évolution en fonction du temps