•  Contrôle de mouvement

UPJV, Département EEA Master 2 EEAII

Parcours ViRob

Fabio MORBIDI

Laboratoire MIS

Équipe Perception Robotique E-mail: [email protected]

1er Semestre, AU 2017-2018

Mercredi 10h00-12h30, Jeudi 14h00-17h00 Salle TP101

Chapitre 1: Introduction

Plan du cours

Partie II : Robotique Mobile

1. Petit historique

3. Marché mondial et besoins technologiques 2. Systèmes, locomotions, applications

Chapitre 2: Locomotion

1. Effecteurs et actionneurs

3. Robots mobiles à roues 2. Robots mobiles à jambes

4. Robots mobiles aériens

Chapitre 3: Décision et contrôle

1. Commandabilité d’un robot 2. Architectures de contrôle

Ch. 3: Décision et contrôle

•  Architectures de contrôle

•  Contrôle de mouvement

Partie 1

Partie 2

Partie 3

•  Commandabilité d’un robot

•  Dans le chapitre 2.3, on a dit qu’il y a une « relation inverse » entre la manœuvrabilité et la commandabilité d’un robot

•  Plus la manœuvrabilité d’un robot est élevée, moins le robot est commandable aisément

Commandabilité d’un robot

•  Soit le système non linéaire suivant (affine en l’entrée):

Objectif: définir plus rigoureusement la notion de commandabilité

x ˙ = f ( x ) +

m j=1

g

_j

( x ) u

_j

où est l’état du système et le vecteur des entrées de commande

x ∈ R

ⁿ

Σ :

Pour qu’un robot mobile soit utile (ou utilisable), il faut en premier lieu s’assurer de sa commandabilité

u = [u

₁

, . . . , u

_m

]

^T

∈ U ⊂ R

^m

Cette solution existe si (c’est-à-dire, si les champs vectoriels sont des fonctions continûment dérivables)

Soit la solution du système au temps

qui correspond à l’entrée et à la condition initiale

Définition

x (t, 0, x

₀

, u )

u : [0, t] −→ U x (0) = x

₀

f , g

₁

, . . . , g

_m

∈ C

^∞

Le système est commandable, si pour tout choix de , il y a un instant de temps et une entrée tels que:

Σ T ^u : [0, T ] −→ U

Si est le modèle (cinématique) d’un robot, commandabilité signifie qu’il existe toujours une loi de commande amenant le robot d’une configuration initiale à une configuration finale quelconque

u (t) Σ

Σ

x

₀

, x

_f

∈ R

ⁿ

x (T, 0, x

₀

, u ) = x

_f

x

₀

x

_f

t

= 0

u

(t)

t

=

T

t ≥ 0 Commandabilité d’un robot

x

₀

x

_f

(rappel par ex. les modèles cinématiques en posture du robot de type unicycle et tricycle qui, par ailleurs, font partie d’une classe de systèmes non linéaires particulaires dits systèmes chaînés)

Si le système est sans terme de dérive ou « driftless » ( ):

Théorème de Chow Le système

Σ ^f ( x ) = 0

x ˙ =

m j=1

g

_j

( x ) u

j

= G ( x ) u

x ˙ = G ( x ) u

est commandable si et seulement si les colonnes de et leurs crochets de Lie successifs forment un ensemble de

vecteurs linéairement indépendants

G

(

x

)

n Commandabilité d’un robot

nous avons le théorème suivant ( : système sous-actionné): m < n

Exemple (Théorème de Chow)

Si has deux colonnes and , on évaluera:

Degré 1:

G

Degré 2:

Degré 3:

Degré 4:

g

₁

g

₂

g

₁

, g

₂

[ g

₁

, g

₂

]

[ g

₁

, [ g

₁

, g

₂

]], [ g

₂

, [ g

₁

, g

₂

]]

[ g

₁

, [ g

₁

, [ g

₁

, g

₂

]]], [ g

₁

, [ g

₂

, [ g

₁

, g

₂

]]], [ g

₂

, [ g

₁

, [ g

₁

, g

₂

]]], [ g

₂

, [ g

₂

, [ g

₁

, g

₂

]]]

Donc, il ne faut pas calculer les crochets de Lie à tous les niveaux.

En pratique, il y aura un degré fini au-delà duquel les crochets de Lie n'apportent aucune nouvelle information

Mais le crochet de Lie a les propriétés suivantes:

.. .

(anticommutativité)

(identité de Jacobi)

• [ g

_i

, g

_i

] = 0

• [ g

_i

, g

_j

] = −[ g

_j

, g

_i

]

• [ g

_i

, [ g

_j

, g

_k

]] + [ g

_k

, [ g

_i

, g

_j

]] + [ g

_j

, [ g

_k

, g

_i

]] = 0 k

Sophus Lie (1842-1899)

Commandabilité d’un robot

Exemple [Robot de type tricycle motorisé à l’avant]

θ

ψ

x y

: commande du robot

D

x y

O’

Modèle cinématique en posture (Ch. 2.3):

Les colonnes de

sont:

g₁

=

⎡

⎢⎢

⎢⎢

⎢⎣

cos

θ

cos

ψ

sin

θ

cos

ψ

sin

ψ D

⎤

⎥⎥

⎥⎥

⎥⎦, g₂

=

⎡

⎢⎢

⎢⎣

0 0 0

⎤

⎥⎥

⎥⎦

: vecteur de configuration (dans le ch. 2, nous

l’avons appelé )

x ˙ =

⎡

⎢ ⎢

⎢ ⎣ x ˙ y ˙ θ ˙ ψ ˙

⎤

⎥ ⎥

⎥ ⎦ =

⎡

⎢ ⎢

⎢ ⎢

⎢ ⎣

cos θ cos ψ 0 sin θ cos ψ 0

sin ψ

D 0

0 1

⎤

⎥ ⎥

⎥ ⎥

⎥ ⎦

G(x)

u

1

u

2

G

(

x

)

x

= [x, y, θ, ψ]

^T u

= [u

1, u2

]

^T

q

Calcul des crochets de Lie successifs:

[

g₁,

[

g₁, g₂

]] =

⎡

⎢⎢

⎢⎢

⎢⎢

⎢⎣

−

sin

θ D

cos

θ

D

0 0

⎤

⎥⎥

⎥⎥

⎥⎥

⎥⎦

Matrice jacobienne

Commandabilité d’un robot

[

g₁, g₂

]

∂ g₂

∂ x g₁ − ∂ g₁

∂ x g₂

=

>

0

∂ g₂

∂ x g₁ −

⎡

⎢⎢

⎢⎢

⎢⎣

0 0

−

sin

θ

cos

ψ −

cos

θ

sin

ψ

0 0 cos

θ

cos

ψ −

sin

θ

sin

ψ

0 0 0 cos

ψ

D

0 0 0 0

⎤

⎥⎥

⎥⎥

⎥⎦

⎡

⎢⎢

⎢⎣

0 0 0 1

⎤

⎥⎥

⎥⎦

=

⎡

⎢⎢

⎢⎢

⎢⎣

cos

θ

sin

ψ

sin

θ

sin

ψ

−

cos

ψ D

0

⎤

⎥⎥

⎥⎥

⎥⎦

et

[ g

₂

, [ g

₁

, g

₂

]] = −[ g

₁

, g

₂

] =

⎡

⎢ ⎢

⎢ ⎢

⎢ ⎢

⎣

− cos θ sin ψ

− sin θ sin ψ cos ψ

D 0

⎤

⎥ ⎥

⎥ ⎥

⎥ ⎥

⎦

On a obtenu 4 vecteurs linéairement indépendants:

g

₁

, g

₂

, [ g

₁

, g

₂

], [ g

₁

, [ g

₁

, g

₂

]]

pour un système de dimension

. Le modèle cinématique du robot de type tricycle est donc commandable

De même, on peut montrer que le modèle cinématique du robot de type unicycle est commandable

n = 4

Commandabilité d’un robot

Remarque

Pour systèmes avec terme de dérive ( ), la condition du

Théorème de Chow est seulement nécessaire pour la commandabilité

1. Si le système

est commandable, le système avec terme de dérive obtenu par extension dynamique (à savoir, on ajoute un intégrateur à chaque canal d’entrée):

est aussi commandable

f ( x ) = 0

x ˙ =

m j=1

g

_j

( x ) u

j

⎧⎪

⎪⎨

⎪⎪

⎩ x˙ =

m j=1

g_j(x)vj

v˙_j = u_j, j ∈ {1, . . . , m}

Toutefois, il y a deux exceptions importantes:

Σ

Commandabilité d’un robot

=

⇒

⎡

⎢⎢

⎢⎢

⎣ x

˙

v

˙

1

.. .

v

˙

m

⎤

⎥⎥

⎥⎥

⎦

=

⎡

⎢⎢

⎣ m j=1

g_j

(

x

)

vj

0_m×1

⎤

⎥⎥

⎦

+

0_n×m I_m

⎡

⎢⎣ u₁

.. .

um

⎤

⎥⎦

f (·)

G

(·)

2. Pour un système linéaire invariant dans le temps:

la condition du Théorème de Chow devient:

c’est-à-dire, la condition nécessaire et suffisante de Kalman pour la

commandabilité (rappel les Cours d’Automatique), où la matrice

^:

x ˙ = Ax +

m j=1

b

_j

u

j

= A x + B u

est appelée matrice de commandabilité (ou de contrôlabilité)

n × nm Commandabilité d’un robot

rank([ B AB A

²

B · · · A

ⁿ⁻¹

B ]) = n

R = [ B AB A

²

B · · · A

ⁿ⁻¹

B ]

Ch. 3: Décision et contrôle

•  Architectures de contrôle

•  Contrôle de mouvement

Partie 1

Partie 2

Partie 3

•  Commandabilité d’un robot

Approche cognitive

1. Approche cognitive: capacité de planification et de prédiction mais nécessité d’un modèle, peu robuste aux changements dans

l’environnement, complexité computationnelle (pas de temps réel)

Approche réactive

symbole réflexe

vitesse de réponse capacités prédictives

dépendance d’un modèle précis

2. Approche réactive ou comportementale: comportements simples et modulaires, temps réel, mais peu de capacités de prédiction

3. Approche hybride: modulation des comportements réactifs et du raisonnement de haut niveau (la plus utilisée)

Approche constructiviste: basée comportements avec construction

Shakey

+ +

+

_ _ _

Genghis

Architectures de contrôle

Le choix d’une approche dépend de l’application (la mission du robot) et de son environnement d’évolution

Perception Planification Action Hiérarchique

Réactive Perception Action Planification

Basée

comportement

Perception Planification Action

Perception Planification Action

Hybride

Perception

Planification

Action

Acquisition des données

sensorielles Modélisation du monde et planification de chemin

Action des moteurs et des effecteurs

Ch. 3: Décision et contrôle

•  Architectures de contrôle

•  Contrôle de mouvement

Partie 1

Partie 2

Partie 3

•  Commandabilité d’un robot

Contrôle de mouvement

Pour plus de simplicité, on considérera dorénavant le modèle cinématique en posture du robot unicycle:

⎡

⎣ x ˙ y ˙ θ ˙

⎤

⎦ =

⎡

⎢ ⎣

cos θ 0 sin θ 0

0 1

⎤

⎥ ⎦

v w

Pour ce robot, on va étudier deux problèmes fondamentaux:

1. Suivi de trajectoire (ou stabilization de mouvement): Le robot doit suivre asymptotiquement une trajectoire cartésienne désirée

où est le temps, à partir d’une configuration initiale qui se trouve ou pas sur la trajectoire

(x_d(t), y_d(t))

q₀ = [x0, y0, θ0]^T

2. Régulation de pose: Le robot doit atteindre asymptotiquement une pose donnée, c’est-à-dire une configuration désirée à partir d’une

configuration initiale

q

₀

q

_d

•  D'un point de vue pratique le premier problème est plus important

•  Dans les deux cas, on cherche une loi de commande par retour d’état, la commande en boucle ouverte n’offrant que des possibilités limitées dans un environnement incertain contenant des obstacles

t

1. Suivi de trajectoire

2. Régulation de pose

(xd(t), yd(t))

Pose initiale Pose désirée

(erreur de position)

Contrôle de mouvement

e

_p

1. Suivi de trajectoire

Pour pouvoir résoudre le problème, il faut que la trajectoire cartésienne désirée soit admissible pour le modèle cinématique du robot Par conséquent, elle doit satisfaire:

(xd(t), yd(t))

x ˙

_d

= v

_d

cos θ

_d

y ˙

_d

= v

_d

sin θ

_d

θ ˙

_d

= ω

_d

pour certains choix des entrées .

En d’autres termes, on imagine que la trajectoire soit générée par un “robot fictif” de référence (indiqué en bleu dans la figure ci-dessus)

v

d

, ω

d

(xd(t), yd(t))

“robot fictif”

robot

e

_p

1. Suivi de trajectoire

L’orientation du robot le long de la trajectoire désirée peut

être calculée comme: (xd(t), yd(t))

et les entrées de référence peuvent être calculées comme:

θ

d

(t) = atan2( ˙ y

d

(t), x ˙

d

(t)) + kπ, k ∈ {0, 1}

v

d

(t) = ±

x ˙

²_d

(t) + ˙ y

_d²

(t)

ω

d

(t) = y ¨

d

(t) ˙ x

d

(t) − x ¨

d

(t) ˙ y

d

(t) x ˙

²_d

(t) + ˙ y

_d²

(t)

Assumption: La valeur de

(et par conséquent le signe de ) a été fixée

v

d

Il convient de définir l’erreur de suivi de la façon suivante:

e =

⎡

⎣ e

1

e

2

e

₃

⎤

⎦ =

⎡

⎣ cos θ sin θ 0

− sin θ cos θ 0

0 0 1

⎤

⎦

⎡

⎣ x

d

− x y

d

− y θ

_d

− θ

⎤

⎦

k

1. Suivi de trajectoire

Ainsi la partie relative à l’erreur de position, , est exprimée dans un repère aligné avec l’orientation courante

θ

du robot Si on calcule la dérivée par rapport au temps de l’erreur et on fait appel aux équations précédentes, on trouve:

Si on utilise la transformation invertible ( sont des entrées auxiliaires):

e ˙

1

= v

d

cos e

3

− v + e

2

ω e ˙

2

= v

d

sin e

3

− e

1

ω

e ˙

3

= ω

d

− ω

v = v

d

cos e

3

− u

1

ω = ω

_d

− u

2

on obtient la dynamique de l’erreur suivante:

e

˙ =

⎡

⎣

0

ωd

0

−ωd

0 0

0 0 0

⎤

⎦e

+

⎡

⎣

0 sin

e3

0

⎤

⎦v_d

+

⎡

⎣

1

−e2

0

e1

0 1

⎤

⎦ u1

u2

e_p

= [x

d − x, yd − y]^T

e (t)

u₁, u₂

1. Suivi de trajectoire

e

˙ =

⎡

⎣

0

ωd

0

−ωd

0 0

0 0 0

⎤

⎦e

+

⎡

⎣

0 sin

e3

0

⎤

⎦v_d

+

⎡

⎣

1

−e2

0

e1

0 1

⎤

⎦ u1

u2

Remarque: Le premier terme est linéaire et le deuxième et le troisième terme sont non linéaires. Le premier et le deuxième terme sont variables dans le temps (en général) car ils dépendent des entrées

v

d

, ω

d

Trois options pour déterminer la commande pour le suivi de trajectoire:

1. Linéarisation du système: on met et on évalue la matrice d’entrée sur la trajectoire:

sin e

3

= e

3

u1

=

−k1 e1

u2

=

−k2 e2 − k3 e3

On peut alors définir la rétroaction linéaire suivante:

e˙ =

⎡

⎢⎣

0 ωd 0

−ωd 0 vd

0 0 0

⎤

⎥⎦e +

⎡

⎢⎣

1 0 0 0 0 1

⎤

⎥⎦ u1

u2

(système encore variable dans le temps)

où sont fonctions bornées avec dérivées bornées et est une constante

2. Contrôle non linéaire: on travaille directement sur le système non linéaire d’origine et on définit la commande suivante:

u1

=

−k1

(v

d, ωd

)

e1

u2

=

−k2 vd

sin

e3

e3 e2 −k3

(v

d, ωd

)

e3

k₁

(·,

·) >

0, k

₃

(·,

·) >

0

k2 >

0

3. Linéarisation entrée-sortie par rétroaction: on définit une entrée qui transforme le modèle de l’unicycle en un système linéaire.

On peut choisir:

B est un point sur l’axe sagittal du robot à une distance |b^|

du point de contact de la “roue” avec le sol. En particulier, B est devant le point de contact si b > 0 et derrière si b ^{< 0}

(v, ω)

1. Suivi de trajectoire

v

ω

=

cos θ sin θ

−

¹_b

sin θ

¹_b

cos θ

u

₁

u

₂

, b = 0

où

Avec cette transformation de l’entrée, nous pouvons exprimer les équations de l’unicycle sous la forme suivante:

x y

1. Suivi de trajectoire

y ˙

1

= u

1

y ˙

2

= u

2

θ ˙ = 1

b (u

2

cos θ − u

1

sin θ) y

1

= x + b cos θ

y

2

= y + b sin θ

(sorties du système; coordon. du point

B

) Le simple contrôleur linéaire (proportionnel):

avec gains garantit la convergence exponentielle de l’erreur de position à zéro, avec dynamiques découplées dans ses deux composantes

u

1

= ˙ y

1d

+ k

1

(y

1d

− y

1

)

u

₂

= ˙ y

_2d

+ k

₂

(y

_2d

− y

₂

)

k

1

> 0, k

1

> 0

Trajectoire désirée circulaire: commande basée sur la linéarisation du système

e_p(t)

temps Norme de l’erreur de position

x [s]

y

1. Suivi de trajectoire: simulations

[x

c

, y

c

]

^T

= [−3, 3]

^T

m, R = 3 m, ω

d

= 1.3 rad/s

avec

q

₀

= [−1, 0, π/2]

^T

q

₀

SuiviTraj12.m

x

_d

(t) = x

_c

+ R cos(ω

_d

t)

y

_d

(t) = y

_c

+ R sin(ω

_d

t)

Trajectoire désirée en forme de « huit »:

contrôle non linéaire

temps [s]

x y

1. Suivi de trajectoire: simulations

Norme de l’erreur de position

e_p(t)

x

d

(t) = x

c

+ R

1

sin(2ω

d

t) y

d

(t) = y

c

+ R

2

sin(ω

d

t)

[x

c

, y

c

]

^T

= [−3, 3]

^T

m, R

1

= R

2

= 3 m, ω

d

= 1/15 rad/s

avec

q₀

SuiviTraj12.m

Trajectoire désirée carrée (côté 4 m): commande basée sur la linéarisation entrée-sortie (

b

= 0.75 m, en haut;

b

= 0.2 m, en bas)

temps v(t)

ω(t)

v(t)

ω(t) x

y

x y

[s]

[s]

[s]

[s]

temps

[m/s] [rad/s] [m/s] [rad/s]

1. Suivi de trajectoire: simulations

q₀

q₀

SuiviTraj3.m

2. Régulation de pose

Pour la regulation de pose on ne peut pas utiliser directement les contrôleurs développés pour le suivi de trajectoire

Les premiers deux lois de commande fonctionnent seulement avec des trajectoires persistantes, à savoir si |v_d(t)| ≥ v >¯ 0, ∀t ≥ 0

Il faut donc concevoir des solutions ad hoc

Assumption: pour simplifier et sans perte de généralité, on peut choisir comme configuration désirée l’origine, à savoir q_d = [0, 0, 0]^T

Pose désirée

[0, 0, 0]^T

a) Régulation cartésienne: seulement la position cartésienne finale du robot est spécifiée. L’orientation finale du robot est laissée libre.

Puisque , l’erreur de position

devient e_p

= [x

d − x, yd − y]^T

e_p

= [−x,

−y]^T

On peut définir la commande suivante:

v = −k

1

(x cos θ + y sin θ) ω = k

2

(atan2(y, x) − θ + π)

où k1 > 0, k2 > 0.

Projection de l’erreur cartésienne sur l’axe sagittal du robot

: différence entre l’orientation du robot et l’orientation du vecteur

e_p

e_p

Avec ce contrôleur, l’erreur de position converge à zero pour toute condition initiale (Démonstration: théorème de stabilité de Lyapunov)

e

_p

x y

Axe sagittal

γ 2. Régulation de pose

q_d = [0, 0, 0]^T

γ

b) Régulation de pose: la position & l’orientation finale du robot sont spécifiées

Il convient de définir le problème en coordonnées polaires

ρ =

x

²

+ y

²

γ = atan2(y, x) − θ + π δ = γ + θ

Pose désirée

Soient:

e

_p

2. Régulation de pose

= e

_p

Pose désirée

En cordonnées polaires, le modèle cinématique du robot unicycle dévient:

Ce modèle présente une singularité pour (à savoir, lorsque le robot est sur l’origine)

ρ = 0

e

_p

Remarque:

2. Régulation de pose

ρ ˙ = −v cos γ γ ˙ = v sin γ

ρ − ω δ ˙ = v sin γ

ρ

On peut ainsi définir la loi de commande suivante:

avec gains

Le modèle cinématique du robot en coordonnées polaires avec cette loi de commande converge asymptotiquement vers [ρ, γ, δ]^T = [0, 0, 0]^T.

Démonstration: théorème de stabilité de Lyapunov Remarque:

•  Si la loi de commande est réécrite en utilisant les coordonnées cartésiennes originales, elle est discontinue à l’origine

•  En effet, toute loi de commande avec boucle de rétroaction qui régule la pose de l’unicycle doit être nécessairement discontinue par rapport à l’état et/ou variable dans le temps. Ce résultat est la conséquence d’un théorème plus général valable pour tous les systèmes

k1 > 0, k2 > 0, k3 > 0.

v = k

1

ρ cos γ

ω = k

₂

γ + k

₁

sin γ cos γ

γ (γ + k

₃

δ)

2. Régulation de pose

Simulations (manœuvres de parking)

Régulation cartésienne

Régulation de pose

k1

= 1, k

2

= 3

k1

= 1, k

2

= 2.5, k

3

= 3

Une inversion de

mouvement au maximum

Une inversion de

mouvement au maximum x

y

x y

x y

x

q₀ = [−1, 2, −π/2]^T y q₀ = [−1, −2, −π/2]^T

Exercice (Matlab):

1.  Créer un schéma Matlab Simulink qui permet de simuler le comportement du robot unicycle (cf. la figure ci-dessous)

•  Tester le comportement du robot pour différentes entrées

2.  Implémenter en Matlab les lois pour la régulation cartésienne et la régulation de pose du robot unicycle

•  Tester les lois de commande avec des conditions initiales et avec des gains différents, et afficher les trajectoires du robot

•  Reproduire les manœuvres de parking montrées dans les quatre k₁, k₂, k₃

(v, ω)

RegulCart.m RegulPose.m