UPJV, Département EEA Master 2 EEAII
Parcours ViRob
Fabio MORBIDI
Laboratoire MIS !
Équipe Perception et Robotique!
E-mail: fabio.morbidi@u-picardie.fr!
Semestre 9, 2015/2016
(en dehors des heures de cours)
Plan du cours
1ère partie: Perception avancée
2ème partie: Robotique mobile
Ch. 1: Introduction Ch. 2: Locomotion
Ch. 3: Décision et contrôle
Ch. 1: Perception pour la robotique
Ch. 2: Modélisation d’incertitudes
Ch. 3: Traitement des mesures
Ch. 3: Décision et contrôle
Commandabilité d’un robot
• Architectures de contrôle
• Contrôle de mouvement
• Dans le chapitre 2, 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 elevée, moins le robot est commandable aisément
Commandabilité d’un robot
• Soit le système non linéaire suivant (affine en l’entrées ):
Objectif: définir plus précisément la notion de commandabilité
˙
x = f (x) +
�
mj=1
g
j(x) u
joù est l’état du système et le vecteur des entrées de commande
u
jx ∈ R
nU : classe des fonctions constantes par morceaux
Σ :
• Pour qu’un robot mobile soit utile (ou utilisable) il faut en premier lieu s’assurer de sa commandabilité
u ∈ U ⊂ R
mCommandabilité d’un robot
Cette solution existe si (c’est-à-dire, les champs vectoriels sont 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
0, u)
u : [0, t] −→ U x(0) = x
0f , g
1, . . . , 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 et finale quelconque u(t)
Σ
Σ
x
0, x
f∈ R
nx(T, 0, x
0, u) = x
fx
0x
ft = 0 u(t) t = T
t ≥ 0
Commandabilité d’un robot
(rappel, par ex., le modèle cinématique du robot de type unicycle et de type tricycle qui, par ailleurs font partie d’une classe de systèmes non linéaires particulaires dits systèmes chaînés), nous avons le
théorème suivant
Si le système est sans terme de dérive ou « driftless » ( ):
Théorème [Chow]
Un système
Σ f (x) = 0
˙ x =
�
mj=1
g
j(x) u
j= G(x) u
x ˙ = G(x) u
est commandable si et seulement si les colonnes de et leur crochets de Lie successifs forment un ensemble de n vecteurs indépendants
G(x)
Commandabilité d’un robot
Exemple [Robot de type tricycle motorisé à l’avant]
θ
ψ
x y
: commande du robot
D
� x
� y
O’
(u
1, u
2)
Modèle cinématique en posture (Ch. 2.3):
˙ q =
˙ x
˙ y θ ˙ ψ ˙
=
cos θ cos ψ 0 sin θ cos ψ 0
sin ψ
D 0
0 1
� u
1u
2�
Nous avons:
g
1=
cos θ cos ψ sin θ cos ψ
sin ψ D
0
, g
2=
0 0 0 1
Commandabilité d’un robot
Exemple [Robot de type tricycle motorisé à l’avant]
En outre:
[g
1, g
2] � ∂ g
2∂ q g
1− ∂ g
1∂ q g
2=
cos θ sin ψ sin θ sin ψ
− cos ψ D 0
[g
1, [g
1, g
2]] =
− sin θ D cos θ
D 0 0
Commandabilité d’un robot
Exemple [Robot de type tricycle motorisé à l’avant]
et
[g
2, [g
1, g
2]] = − [g
1, g
2] =
− cos θ sin ψ
− sin θ sin ψ cos ψ
D 0
On a obtenu 4 vecteurs indépendants,
g
1, g
2, [g
1, g
2], [g
1, [g
1, g
2]]
pour un système de dimension n = 4. Le modèle cinématique du robot de type tricycle est donc commandable.
On peut montrer de même que celui du robot unicycle est aussi
commandable.
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 ajute un intégrateur à chaque canal d’entrée)
est aussi commandable
f (x) � = 0
x ˙ =
�
mj=1
g
j(x) u
j
˙ x =
�
m j=1g
j(x) v
j˙
v
j= u
j, j ∈ { 1, . . . , m }
Toutefois, il y a deux exceptions importantes:
Commandabilité d’un robot
Remarque
2. Pour un système linéaire invariant en temps:
la condition du Théorème de Chow devient:
c’est-à-dire, la condition nécessaire et suffisante pour la commandabilité de Kalman (rappel le Cours d’Automatique), où la matrice :
x ˙ = Ax +
�
mj=1
b
ju
j= A x + B u
C = [B AB A
2B · · · A
n−1B]
est communément appelée matrice de commandabilité
n × nm
rank([B AB A
2B · · · A
n−1B]) = n
Ch. 3: Décision et contrôle
Commandabilité d’un robot
• Architectures de contrôle
• Contrôle de mouvement
Architectures de contrôle
Approche cognitive
1. Approche cognitive: capacité de planification et de prédiction mais
nécessité d’un modèle, peu robust aux changements du monde, complexité calculatoire (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 comportamentale: 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
progressive de connaissances sur le monde d’évolution du robot
Architectures de contrôle
• Le choix d’une approche dépend de l’application (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 Planification
Acquisition des données
sensorielles Modélisation et planification
du monde Action des moteurs et effecteurs
Ch. 3: Décision et contrôle
Commandabilité d’un robot
• Architectures de contrôle
• Contrôle de mouvement
Contrôle de mouvement
Pour plus de simplicité, on consider le modèle cinématique d’un robot unicycle:
˙ x
˙ y θ ˙
=
cos θ 0 sin θ 0
0 1
� v w
�
Pour le robot unicycle, on va étudier deux problèmes fondamentaux:
1. Suivi de trajectoire : Le robot doit suivre asymptotiquement une
trajectoire cartésienne desirée , à partir d’une configuration initiale qui peut appartenir ou non à la trajectoire (x
d(t), y
d(t))
q
0= [x
0, y
0, θ
0]
T2. Régulation de la pose (ou posture) : Le robot doit atteindre asymptotiquement une pose donnée, c’est-à-dire une configuration désirée à partir d’une configuration initiale q
dq
0D'un point de vue pratique le premier problème est le plus important
(un robot se déplace normalement dans un environnement non structuré
Contrôle de mouvement
Suivi de trajectoire
Régulation de la pose (x
d(t), y
d(t))
Initiale
Désirée
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, c’est-à-dire elle doit satisfaire:
(x
d(t), y
d(t))
˙
x
d= v
dcos θ
d˙
y
d= v
dsin θ
dθ ˙
d= ω
dpour quelque choix des entrées v
d, ω
d(xd(t), yd(t))
Suivi de trajectoire
L’orientation du robot le long de la trajectoire désirée peut
être calculée comme: (x
d(t), y
d(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
2d(t) + ˙ y
d2(t)
ω
d(t) = y ¨
d(t) ˙ x
d(t) − x ¨
d(t) ˙ y
d(t)
˙
x
2d(t) + ˙ y
d2(t)
Assumption: La valeur de k (et par consequence le signe de )
a été choisie v
dIl convient de définir l’erreur de suivi comme:
e =
e
1e
2e
3
=
cos θ sin θ 0
− sin θ cos θ 0
0 0 1
x
d− x y
d− y θ
d− θ
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 utilise
les équations précédentes, nous trouvons:
Si on utilise la transformation (invertible):
˙
e
1= v
dcos e
3− v + e
2ω
˙
e
2= v
dsin e
3− e
1ω
˙
e
3= ω
d− ω
v = v
dcos e
3− u
1ω = ω
d− u
2on obtient la dynamique de l’erreur suivante:
˙ e =
0 ω
d0
− ω
d0 0
0 0 0
e +
0 sin e
30
v
d+
1 − e
20 e
10 1
� u
1u
2�
e
p= [x
d− x, y
d− y]
TSuivi de trajectoire
˙ e =
0 ω
d0
− ω
d0 0
0 0 0
e +
0 sin e
30
v
d+
1 − e
20 e
10 1
� u
1u
2�
Remarque: Le premier terme est linéaire et le deuxième et troisième terme sont non linéaires. Le premier et deuxième terme sont variables dans le temps puisque ils dépendent des entrées v
d, ω
dTrois options pour déterminer le contrôleur pour le suivi de trajectoire:
1. Linéarisation du système: on met et on évalue la matrice d’entrée (la matrice dans le troisième terme) sur la trajectoire. On peut ainsi définir le contrôleur:
sin e
3= e
3u
1= − k
1e
1u
2= − k
2e
2− k
3e
3où sont des gains appropriés k
1, k
2, k
3Suivi de trajectoire
où sont fonctions bornées avec dérivées bornées et est constante
2. Contrôle non linéaire: on travaille directement sur le système non linéaire originale et on peut définir le contrôleur:
u
1= − k
1(v
d, ω
d) e
1u
2= − k
2v
dsin e
3e
3e
2− k
3(v
d, ω
d) e
3k
1( · , · ) > 0, k
3( · , · ) > 0 k
2> 0
3. « Input/output linearization » via rétroaction: on définit une
transformation de l’entrée du système pour transformer ce dernier en un système linéaire. Par exemple, on peut choisir:
� v ω
�
=
� cos θ sin θ
− (sin θ)/b (cos θ)/b
� � u
1u
2�
, b � = 0
Suivi de trajectoire: simulations
Trajectoire désirée circulaire: contrôleur basé sur la linéarisation du système
� e
p(t) �
temps Norme de l’erreur de position
Suivi de trajectoire: simulations
Trajectoire désirée en forme de « huit »:
contrôleur non linéaire
� e
p(t) �
temps Norme de l’erreur de position
Suivi de trajectoire: simulations
Trajectoire désirée carrée: contrôleur basé sur l’input/output linearization (b = 0.75, en haut; b = 0.2, en bas)
temps
temps v(t)
ω(t)
v(t)
ω(t)
Regulation de la pose
Pour la regulation de la pose on ne peut pas utiliser directement les contrôleurs développés pour le suivi de trajectoire
Les premiers deux contrôleurs fonctionnent seulement avec des trajectoires persistantes, à savoir | v
d(t) | ≥ v > ¯ 0, ∀ t ≥ 0
Il faut trouver 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[0, 0, 0]
TRégulation de la pose
1 – 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 cartésien , devient q
d= [0, 0]
Te
p= [x
d− x, y
d− y]
Te
p= [ − x, − y]
TOn peut définir le contrôleur:
v = − k
1(x cos θ + y sin θ) ω = k
2(atan2(y, x) − θ + π)
où k
1> 0, k
2> 0.
Projection de l’erreur cartésien sur l’axe sagittal du robot
Différence entre l’orientation du robot et l’orientation du vecteur
e
pe
pAvec ce contrôleur, l’erreur cartésien converge à zero pour toute condition initiale (Démonstration: Théorème de stabilité de Lyapunov)
e
pqd = [0, 0]T
Régulation de la pose
2 – Régulation de la pose: la position et l’orientation finale du robot sont spécifiées
Il convient de définir le problème en coordonnées polaires
ρ = �
x
2+ y
2γ = atan2(y, x) − θ + π δ = γ + θ
Pose désirée
Soyent:
Pose désirée
Régulation de la pose
En cordonnées polaires, le modèle cinématique du robot dévient:
˙
ρ = − v cos γ
˙
γ = sin γ
ρ v − ω δ ˙ = sin γ
ρ v
Ce modèle présente une singularité pour (lorsque le robot est dans l’origine)
ρ = 0
Régulation de la pose
On peut ainsi définir la loi de commande suivante:
avec
v = k
1ρ cos γ
ω = k
2γ + k
1sin γ cos γ
γ (γ + k
3δ)
Le modèle cinématique du robot en coordonnées polaires avec cette loi de commande converge asymptotiquement à [ρ, γ , δ]
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 originales, elle est discontinue à l’origine
• En effet, toute loi de contrôle en 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. Ceci est une consequence d’un théorème plus général valable pour tous les systèmes non holonômes
k
1> 0, k
2> 0, k
3> 0.
Simulations (« manœuvre de parking »)
Régulation cartésienne
Régulation de la pose Condition initiale 1 Condition initiale 2
k
1= 1, k
2= 3
k
1= 1, k
2= 2.5, k
3= 3
Une inversion de
mouvement au maximum
Une inversion de
mouvement au maximum