TP Génération de trajectoires

ENIB
2006–2007
5A — Mécatronique
Robotique 2
TP Génération de trajectoires
Rendre 1 compte rendu par groupe de 2 étudiants maximum avant le mercredi 20 décembre 2006
à 18h (des points de pénalité seront retirés après cette date).
Dans ce TP on souhaite générer des trajectoires pour le robot ABB. Les trajectoires seront générées dans Scilab sous forme discrète, c’est-à-dire un ensemble de point dans l’espace articulaire.
Cet ensemble de point sera ensuite traduit en programme RAPID (langage de programmation
robot).
Ce programme RAPID sera chargé dans le logiciel de simulation et de développement RobotStudio pour y être testé et analysé.
Au final, les trajectoires correctes pourront être envoyées sur le vrai robot.
Scilab
Génération
de
Trajectoires
Ensemble de
coord. articulaires
par lesquelles le
robot doit passer
RobotStudio
Génération
d’un
programme
RAPID
Programme
RAPID
(fichier txt)
Test et
simulation
des
trajectoires
Robot ABB
Mouvements
réels
Transfert sur
la baie de
commande
F IG . 1 – Vue d’ensemble du TP
On définit par M = (xM , yM , zM ) le point terminal du robot (voir figure 2). Dans ce TP nous nous
intéressons seulement au mouvement de ce point sans tenir compte ni de l’orientation ni des contacts
éventuels d’autres points du robot avec des obstacles.
O UTILS FOURNIS :
Deux fonctions Scilab sont fournies :
– X=direct(q) : donne le modèle géométrique direct du robot,
où q représente les coordonnées articulaires du robot en radians (dim(q) = 6)
où X représente les coordonnées cartésiennes du point M (dim(X) = 3)
– J=jacobienne(q) : donne la matrice jacobienne du robot au point q telle que Ẋ = J q̇,
où q représente les coordonnées articulaires du robot en radians (dim(q) = 6)
où X représente les coordonnées cartésiennes du point M (dim(X) = 3). La matrice J a été
obtenue à partir de la fonction précédente J = ∂ direct(q)
.
∂q
M = (xM , yM , zM )T
z
y
x
F IG . 2 – Robot ABB
Pour pouvoir utiliser ces deux commandes vous devez ajouter les deux lignes suivantes à votre
programme Scilab et décompresser l’archive GenerationModeles.zip1 dans votre répertoire de travail :
exec GenerationModeles/loader.sce;
exec GenerationModeles/loader-func.sce;
PARTIE I : G ÉNÉRATION D ’ UN MOUVEMENT EN LIGNE DROITE DANS L’ ESPACE OPÉRATIONNEL
Dans cette partie on souhaite générer un déplacement du point M en ligne droite dans l’espace de
travail, sans avoir la connaissance du modèle géométrique inverse du robot.
Soit le point de départ défini dans l’espace articulaire par


18°
 27° 


 -2° 


qD = 

0°


 0° 
0°
et le point d’arrivé souhaité défini dans l’espace de travail par


600 mm
XA =  -300 mm 
760 mm
(1)
(2)
1 – A l’aide des fonctions Scilab proposées, calculer les coordonnées opérationnelle du point de
départ (soit X0 ces cordonnées).
2 – Admettons que le point terminal du robot soit à la position Xk connue. A partir de Xk et de
1
l’archive peut être téléchargée à l’adresse http://www.enib.fr/~bourgeot/Cours.php
2
XA calculer le vecteur suivant lequel le robot devrait se déplacer pour rejoindre le point d’arrivé XA .
Soit Ẋ ⋆ ce vecteur vitesse de déplacement désiré.
Les vitesses articulaires et opérationnelles sont liées par la relation suivante :
Ẋ = J q̇
(3)
Pour obtenir la vitesse articulaire désirée q̇ ⋆ nécessaire pour rejoindre XA , il faut donc inverser la relation (3). Or la matrice J n’est pas carré (dim X = 3 et dim q = 6), il existe possiblement une infinité
de solution, le robot est dit redondant. Une solution possible est donnée par la pseudo-inverse2 :
q̇ =
J † Ẋ
avec J † = J T (JJ T )−1
(4)
(5)
3 – A partir de la relation (4) déterminer la vitesse articulaire que le robot devrait avoir pour
aller vers XA , soit q̇ ⋆ cette vitesse. La relation 3 n’est valable qu’à un instant t donné. La vitesse
désirée n’est donc valable que sur un intervalle de temps δt. Si la vitesse q̇ ⋆ est appliquée pendant ∆t,
quelle est la nouvelle position articulaire du robot qk+1 (en fonction de ∆t, qk et q̇ ⋆ ). Avec le modèle
géométrique directe on peut en déduire Xk+1 .
4 – En partant du point q0 = qD , et en répétant les étapes des questions 1 à 3 jusqu’à obtenir
||Xk+1 −XA || < ǫ, on peut générer de proche en proche un ensemble de point dans l’espace articulaire
(les points qk ) qui forme la trajectoire désirée. De plus si on se trouve loin des singularités du robot et
si ∆t est choisi assez petit, alors la trajectoire correspondante dans l’espace de travail est rectiligne.
5 – Programmer un script Scilab qui permette de générer une telle trajectoire. Ce script doit vous
fournir (au minimum, un tableau de point correspondant à tout les qk ).
6 – En fonction de la valeur de ∆t choisie, le tableau obtenu à la question 5 peut avoir une taille
très importante. Ré-échantillonné votre trajectoire pour n’avoir qu’une vingtaine de points.
7 – Visualisé vos trajectoires dans l’espace de travail (et dans l’espace articulaire). A l’aide des
fonctions scilab suivantes : clf();param3d1(X,Y,Z,45,45,"x@y@z"); où X, Y et Z sont
des vecteurs contenants les coordonnées (x, y, z) des points à afficher.
On souhaite maintenant pouvoir utiliser cette trajectoire sur le robot. Pour cela on va générer un
programme (dans le langage propre au robot) qui va le déplacer au point q0 , puis q1 , · · · jusqu’à qn .
Un programme RAPID faisant cela est de la forme (les symboles \\ indiquent que l’on se trouve
sur la même ligne) :
2
Pour information, c’est la solution qui minimise ||q̇||
3
%%%
VERSION:1
LANGUAGE:ENGLISH
%%%
MODULE GeneTraj
VAR num i:=1;
PROC main()
Gene_Trajectoire;
ENDPROC
PROC Gene_Trajectoire()
CONST jointtarget POC:=[[0,0,0,0,0,0],
\\[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST jointtarget Position1:=[[18.00,27.00,-2.00,0.00,0.00,0.00],
\\[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST jointtarget Position2:=[[q1,q2,q3,q4,q5,q6],
\\[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST jointtarget Position3:=[[q1,q2,q3,q4,q5,q6],
\\[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
.
.
.
CONST jointtarget Position20:=[[q1,q2,q3,q4,q5,q6],
\\[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
SingArea\Wrist;
ConfL\Off;
ConfJ\Off;
MoveAbsJ POC,v1000,fine,tool0;
MoveAbsJ Position1,v1000,fine\Inpos:=inpos100,tool0;
WaitTime 2;
MoveAbsJ Position2,v100,fine\Inpos:=inpos100,tool0;
MoveAbsJ Position3,v100,fine\Inpos:=inpos100,tool0;
MoveAbsJ Position4,v100,fine\Inpos:=inpos100,tool0;
.
.
.
MoveAbsJ Position19,v100,fine\Inpos:=inpos100,tool0;
MoveAbsJ Position20,v100,fine\Inpos:=inpos100,tool0;
WaitTime 2;
MoveAbsJ POC,v1000,fine,tool0;
ENDPROC
ENDMODULE
4
où q1, q2, q3, q4, q5 et q6 représentent les coordonnées articulaires de chaque point de passage
exprimé en degrés.
8 – A l’aide des fonctions Scilab mopen et mfprintf générer automatiquement un programme
RAPID sous forme de fichier texte (le fichier générer devra avoir l’extension .prg).
9 – Importer ce programme dans RobotStudio ;
– ouvrir RobotStudio ABB Automation/Robot Studio/RobotStudio
– charger le robot IRB140_M2000 Fichier/Importer/Bibliothèque
– démarrer le système de commande en faisant un clic droit sur composant/Irb140_M2000
– charger votre programme Fichier/Importer/Programme
et exécuter ce programme (bouton play). Vérifier la conformité de la trajectoire avec la trajectoire désirée. La trace du mouvement peut être obtenue en cochant la case Trace dans le menu
Simulation/Moniteur. Vous pouvez utiliser les outils de RobotStudio pour tracer la ligne droite
de référence (
Créer/Composants 2D/Ligne). Si la trajectoire n’est pas rectiligne, expliquer pourquoi, comment remédier à cela ?
Remarque :
Dans le cadre d’une application sur le robot industrielle IRB140, cette méthode ne présente que peu
d’intérêt puisque un mouvement rectiligne dans l’espace opérationnel peut directement être obtenu
grâce à la commande RAPID "MOVEL". Néanmoins cette méthode reste applicable pour contrôler
n’importe quel autre robot dont vous ne connaîtriez pas le modèle géométrique inverse.
PARTIE II : E VITEMENT D ’ OBSTACLE
On souhaite maintenant modifier la méthode utilisée dans la partie 1, pour générer des trajectoires
qui évitent des obstacles.
Soit un obstacle sphérique3 de rayon r = 60 cm et d’origine Xobs = (600, −50, 630)T .
Dans l’algorithme précédent on souhaite modifier la façon de calculer la vitesse désirée q̇ ⋆ pour
pouvoir éviter des collisions. On propose de calculer q̇ ⋆ avec la relation suivante :
q̇ ⋆ = K1 J † (XA − X) +K2 J † Ψ(X, Xobs )
|
|
{z
}
{z
}
Terme 1
(6)
Terme 2
où K1 et K2 sont deux gains, et Ψ(X, Xobs ) une fonction de pénalisation à déterminer.
Le terme 1 permet de générer une composante de la vitesse en direction de la position souhaitée
tel qu’étudié dans la 1ère partie du TP. Le terme 2 permet de générer une une composante de la vitesse
qui tend à éloigner le point M de l’obstacle.
3
Si l’obstacle n’est pas sphérique, nous pouvons toujours considérer la plus petite sphère englobant l’obstacle.
5
Si le robot est loin de l’obstacle, le terme 1 doit être prépondérant. Par contre si le point M
s’approche dangereusement de l’obstacle le terme 2 doit être prépondérant.
10 – Proposer une fonction Ψ(X, Xobs ), modifier le script Scilab pour obtenir une trajectoire
d’évitement.
A IDE :
– Ψ(X, Xobs ) doit fournir une vitesse d’éloignement par rapport à l’obstacle inversement proportionnel à la distance qui sépare le robot de l’obstacle (ou mieux encore, inversement proportionnel au carrée de la distance).
– Ψ(X, Xobs ) doit être dirigée suivant un vecteur qui permet de s’éloigner de l’obstacle (on peut
prendre la direction normale à la surface de l’obstacle).
11 – Visualiser cette trajectoire dans Scilab, une sphère peut-être affichée en utilisant les commandes suivantes :
deff("[x,y,z]=sph(alp,tet)",["x=r*cos(alp).*cos(tet)+orig(1)*ones(tet)";
"y=r*cos(alp).*sin(tet)+orig(2)*ones(tet)";
"z=r*sin(alp)+orig(3)*ones(tet)"]);
r=rayon_obs; orig=x_obst;
[xx,yy,zz]=eval3dp(sph,linspace(-%pi/2,%pi/2,40),linspace(0,%pi*2,20));
plot3d(xx,yy,zz,,,,[0,0,0],);
12 – Etudier l’effet des gains K1 et K2 , du nombre de point définissant la trajectoire ainsi que du
∆t utilisé.
13 – Dans RobotStudio, créer un obstacle sphérique
Charger votre programme et tester le.
Créer/Composants 3D/Sphère.
14 – Executer le programme RAPID sur le robot réel.
15 – [Bonus] Même question avec deux obstacles.
–
–
–
–
–
–
–
–
–
Q UELQUES FONCTIONS S CILAB :
X’ : Calcul la transposée de X
help(nom_de_fonction) : Renvoie l’aide associé à une fonction scilab
size(X) : Renvoie la taille d’une matrice ou d’un vecteur
size(X,1) : Renvoie le nombre de ligne d’une matrice ou d’un vecteur colonne
size(X,2) : Renvoie le nombre de colonne d’une matrice ou d’un vecteur ligne
X(:,i) : Renvoie la ieme colonne de la matrice X
X(i,:) : Renvoie la ieme ligne de la matrice X
floor(x) : Renvoie la partie entière du réel x.
Pour générer un tableau :
– tab=[] : initialise le tableau tab.
– tab=[tab X] : ajoute la colonne X à la fin du tableau (si X est un vecteur colonne)
– tab=[tab; X] : ajoute la ligne X à la fin du tableau (si X est un vecteur ligne)
6