UFR des Sciences, Département EEA. M2 EEAII Parcours ViRob. Fabio MORBIDI

Transcription

1 UFR des Sciences, Département EEA M2 EEAII Parcours ViRob Fabio MORBIDI Laboratoire MIS! Équipe Perception et Robotique! Semestre 9, 2014/2015

2 Plan du cours 1ère partie: Perception avancée Ch. 1: Perception pour la robotique Ch. 2: Modélisation d incertitudes Ch. 3: Traitement des mesures 2ème partie: Robotique mobile Ch. 1: Introduction Ch. 2: Locomotion Ch. 3: Décision et contrôle 2

3 Ch. 3: Décision et contrôle Commandabilité d un robot Architectures de contrôle Contrôle de mouvement 3

4 Commandabilité d un robot 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 Pour qu un robot mobile soit utile (ou utilisable) il faut en premier lieu s assurer de sa commandabilité Objectif: définir plus précisément la notion de commandabilité Soit le système non linéaire suivant (affine en l entrées ): Σ : ẋ = f(x) + m g j (x) u j j=1 u U R m où x R n est l état du système et le vecteur des entrées de commande U : classe des fonctions constantes par morceaux u j 4

5 Commandabilité d un robot x(t, 0, x 0, u) Soit la solution du système au temps qui correspond à l entrée u :[0,t] U et à la condition initiale x(0) = x 0 Cette solution existe si f, g 1,...,g m C (c est-à-dire, les champs vectoriels sont fonctions continûment dérivables) Définition Σ Le système est commandable, si pour tout choix de, il y a un instant de temps et une entrée u :[0,T] U tels que: Σ T Si est le modèle (cinématique) d un robot, commandabilité signifie qu il existe toujours une loi de commande u(t) amenant le robot d une configuration initiale et finale quelconque Σ x(t, 0, x 0, u) =x f t 0 x 0, x f R n x 0 t =0 u(t) x f t = T 5

6 Commandabilité d un robot Si le système Σ est sans terme de dérive ou «driftless» ( f(x) =0): (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 Théorème [Chow] ẋ = m j=1 g j (x) u j = G(x) u Un système ẋ = G(x) u est commandable, si et seulement si les colonnes de G(x) et leur crochets de Lie successifs forment un ensemble de n vecteurs indépendants 6

7 Commandabilité d un robot Exemple [Robot de type tricycle motorisé à l avant] y y Nous avons: O x D g 1 = ψ θ cos θ cos ψ sin θ cos ψ sin ψ D 0 x Modèle cinématique en posture (Ch. 2.3): q =, g 2 = ẋ ẏ θ ψ = (u 1,u 2 ) cos θ cos ψ 0 sin θ cos ψ 0 sin ψ D : commande du robot u1 u 2 7

8 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 = [g 1, [g 1, g 2 ]] = sin θ D cos θ D 0 cos θ sin ψ sin θ sin ψ cos ψ D 0 0 8

9 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. 9

10 Commandabilité d un robot Remarque f(x) = 0 Pour systèmes avec terme de dérive ( ), la condition du Théorème de Chow est seulement nécessaire pour la commandabilité Toutefois, il y a deux exceptions importantes: 1. Si le système ẋ = m g j (x) u j j=1 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) m ẋ = g j (x) v j est aussi commandable j=1 v j = u j, j {1,...,m} 10

11 Commandabilité d un robot Remarque 2. Pour un système linéaire invariant en temps: m ẋ = Ax + b j u j = Ax + Bu j=1 la condition du Théorème de Chow devient: ranq([b AB A 2 B A n 1 B]) = n c est-à-dire, la condition nécessaire et suffisante pour la commandabilité de Kalman (rappel le cours d automatique), où la matrice : C =[B AB A 2 B A n 1 B] est communément appelée matrice de commandabilité n nm 11

13 Architectures de contrôle Approche cognitive Approche réactive symbole vitesse de réponse capacités prédictives réflexe dépendance d un modèle précis 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) 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 13

14 Architectures de contrôle Le choix d une approche dépend de l application (mission du robot) et de son environnement d évolution Hiérarchique Acquisition des données sensorielles Modélisation et planification du monde Action des moteurs et effecteurs Perception Planification Action Réactive Perception Planification Action Basée comportement Perception Planification Action Perception Planification Action Hybride Perception Planification Action 14

15 Générateurs de plans (planification) Un plan est une structure de données composée de: Un ensemble d étapes plan Un ensemble de contraintes ordonnées (ordre d exécution des actions) Un ensemble de contraintes variables Un ensemble de liens de causalité (entre étapes) Critères d évaluation: Complétude: garantie de trouver une solution (si elle existe) Complexité temporelle: temps de résolution Complexité spatiale: mémoire nécessaire pour trouver une solution Optimalité: la solution trouvée est-elle optimale? 15

17 Contrôle de mouvement Pour plus de simplicité, on consider le modèle cinématique d un robot unicycle: ẋ ẏ θ = cos θ 0 sin θ 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 (x d (t), y d (t)), à partir d une configuration initiale q 0 =[x 0,y 0, θ 0 ] T qui peut appartenir ou non à la trajectoire 2. Régulation de la pose (ou de la posture) : Le robot doit atteindre asymptotiquement une pose donnée, c est-à-dire une configuration désirée q d à partir d une configuration initiale q 0 D'un point de vue pratique le premier problème est le plus important (un robot se deplace normalement dans un environnement non structuré contenant des obstacles) 17

18 Contrôle de mouvement (x d (t), y d (t)) Suivi de trajectoire Régulation de la pose Désirée Initiale 18

19 Suivi de trajectoire Pour pouvoir résoudre le problème, il faut que la trajectoire cartésienne désirée (x d (t), y d (t)) soit admissible pour le modèle cinématique du robot, c est-à-dire elle doit satisfaire: (x d (t), y d (t)) ẋ d = v d cos θ d ẏ d = v d sin θ d θ d = ω d pour quelque choix des entrées v d, ω d 19

20 Suivi de trajectoire L orientation du robot le long de la trajectoire désirée être calculée comme: (x d (t), y d (t)) peut θ d (t) = atan2(ẏ d (t), ẋ d (t)) + kπ, k {0, 1} et les entrées de référence peuvent être calculées comme: v d (t) =± ẋ 2 d (t)+ẏ2 d (t) ω d (t) =ÿd(t)ẋ d (t) ẍ d (t)ẏ d (t) ẋ 2 d (t)+ẏ2 d (t) Assumption: La valeur de k (et par consequence le signe de v d ) a été choisie Il convient de définir l erreur de suivi comme: e = e 1 e 2 e 3 = cos θ sin θ 0 sin θ cos θ x d x y d y θ d θ 20

21 Suivi de trajectoire Ainsi, la partie relative à l erreur de position, e p =[x d x, y d y] T, 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: ė 1 = v d cos e 3 v + e 2 ω ė 2 = v d sin e 3 e 1 ω ė 3 = ω d ω Si on utilise la transformation (invertible): v = v d cos e 3 u 1 ω = ω d u 2 on obtient la dynamique de l erreur suivante: ė = 0 ω d 0 ω d e + 0 sin e 3 0 v d + 1 e 2 0 e u1 u 2 21

22 Suivi de trajectoire ė = 0 ω d 0 ω d e + 0 sin e 3 0 v d + 1 e 2 0 e u1 u 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, ω d Trois 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: u 1 = k 1 e 1 sin e 3 = e 3 u 2 = k 2 e 2 k 3 e 3 où k 1,k 2,k 3 sont des gains appropriés 22

23 Suivi de trajectoire 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: où bornées et u 1 = k 1 (v d, ω d ) e 1 k 1 (, ) > 0, k 3 (, ) > 0 k 2 > 0 u 2 = k 2 v d sin e 3 e 3 e 2 k 3 (v d, ω d ) e 3 sont fonctions bornées avec dérivées est constante 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 θ)/b sin θ (cos θ)/b u1 u 2, b = 0 23

24 Suivi de trajectoire: simulations e p (t) Norme de l erreur de position temps Trajectoire désirée circulaire: contrôleur basé sur la linéarisation du système 24

25 Suivi de trajectoire: simulations e p (t) Norme de l erreur de position temps Trajectoire désirée en forme de «huit»: contrôleur non linéaire 25

26 Suivi de trajectoire: simulations v(t) temps ω(t) Trajectoire désirée carrée: contrôleur basé sur l input/output linearization (b = 0.75, en haut; b = 0.2, en bas) v(t) temps ω(t) 26

27 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] T 27

28 Ré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 q d =[0, 0] T e p =[ x, y] T e p =[x d x, y d y] T Puisque, l erreur cartésien, devient On peut définir le contrôleur: Projection de l erreur cartésien sur l axe sagittal du robot e p q d =[0, 0] T v = k 1 (x cos θ + y sin θ) ω = k 2 (atan2(y, x) θ + π) e p où k 1 > 0, k 2 > 0. Différence entre l orientation du robot et l orientation du vecteur e p Avec ce contrôleur, l erreur cartésien converge à zero pour toute condition initiale (Démonstration: Théorème de stabilité de Lyapunov) 28

29 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 Soyent: ρ = x 2 + y 2 γ = atan2(y, x) θ + π δ = γ + θ Pose désirée 29

30 Régulation de la pose En cordonnées polaires, le modèle cinématique du robot dévient: ρ = v cos γ γ = sin γ ρ δ = sin γ ρ v ω v Ce modèle présente une singularité pour (lorsque le robot est dans l origine) ρ =0 Pose désirée 30

31 Régulation de la pose On peut ainsi définir la loi de commande suivante: avec v = k 1 ρ cos γ sin γ cos γ ω = k 2 γ + k 1 γ k 1 > 0, k 2 > 0, k 3 > 0. (γ + 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 (Théorème de Brockett) 31

32 Simulations («manœuvre de parking») Condition initiale 1 Condition initiale 2 q 0 Régulation cartésienne k 1 =1,k 2 =3 q 0 Une inversion de mouvement au maximum q 0 Régulation de la pose k 1 =1,k 2 =2.5, k 3 =3 Une inversion de mouvement au maximum q 0 32