Commande cartésienne d`un robot non instrumenté par

Transcription

LASMEA
UMR 6602 du CNRS
Université Blaise Pascal de Clermont-Ferrand
Rapport intermédiaire de Contrat
GIE- INTRA
Commande cartésienne d’un robot non instrumenté
par Asservissement visuel
*******
Participants au projet :
LASMEA
Philippe Martinet (Pr)
Elèves Ingénieurs IFMA
Jean-Baptiste LEVEQUE
[email protected], Tel : 04.73.40.76.53
Nicolas Andreff (MCF)
Julien Clément
Youcef Mezouar (MCF)
Laurent Caekebeke
Date : 8 Mars 2004
Version 1.0
Signature : P. Martinet
Rapport CEA Centaure
Sommaire
I - Introduction : cadre de l’étude de faisabilité
II - Modélisation et environnement d’évaluation
III - Estimation des variables articulaires
IV - Commande cartésienne
V - Positionnement par asservissement visuel
VI - Intégration
VII - Conclusion, perspectives
Annexes
LASMEA UMR 6602 du CNRS
2
I. Introduction : cadre de l’étude de faisabilité
A. Contexte
Le GIE INTRA (CEA, COGEMA, EDF) dispose de robots d’intervention en milieu hostile
nommés Centaure. Conçu il y a vingt ans, ce type de robot est dépourvu de capteurs
articulaires. Pour le piloter, l’utilisateur dispose seulement d’une commande analogique pour
chacune des sept articulations et du retour visuel fourni par deux caméras embarquées.
Sur une proposition du LASMEA, une étude est réalisée. L’objet de cette étude est de vérifier
la faisabilité d’une commande cartésienne du robot par asservissement visuel et de définir les
éléments permettant l’intégration de cette commande au robot.
Ce rapport intermédiaire présente le travail réalisé jusqu’au 30/07/2003, concernant l’étude de
faisabilité.
B. Présentation du robot Centaure
Au cours de l’étude, on doit garder à l’esprit quelques caractéristiques :
Evolution en environnement hostile
Ce robot intervient en milieu hostile. Il occupe un encombrement relativement réduit et
dispose d’éléments lui permettant d’être autonome (batteries, émetteur, récepteur, treuil,
caméras…), doit pouvoir saisir divers éléments.
Conception ancienne
Conçu il y a vingt ans, le Centaure a des caractéristiques particulières :
•
Commande analogique des articulations
Chaque articulation est commandée en courant de manière analogique par un joystick situé
sur le pupitre de commande. Un correcteur électronique tente de compenser le différentiel
présent au niveau des deux dernières articulations. Il en résulte un grand inconfort de
manipulation du robot, l’opérateur devant interpoler lui-même les mouvements articulaires
nécessaire à un mouvement cartésien. On a jusque là recours à des marquages de couleur sur
chaque articulation pour atteindre des positions prédéfinies.
•
Pas de capteurs proprioceptifs, mais deux caméras
Pour retour d’information, l’opérateur dispose seulement de l’image renvoyée par deux
caméras, l’une située sur la base mobile du robot, l’autre à proximité de la pince sur le bras
3
robot. Souvent, une partie plus ou moins importante du robot n’est pas visible.
Le grand inconfort d’utilisation, conjugué à la lenteur du robot, se traduit ainsi : il lui faut
deux heures pour franchir un sas. Sachant que l’autonomie moyenne des batteries est de six
heures, cela ne laisse plus que deux heures d’intervention effective (et durant lesquelles le
robot est tout aussi difficile à manipuler).
C. Cahier des charges de l’étude de faisabilité
Titre de l’étude :Commande cartésienne d'un bras manipulateur mobile
Il s'agit de la commande
- X,Y, Z en position
- R P Y en orientation
d'un bras manipulateur (CENTAURE) monté sur une plate-forme mobile. La caractéristique
de ce système est qu'il est dépourvu de capteurs proprioceptifs (codeurs moteurs).
Nous proposons de mener l'étude en trois phases :
¾ phase 1 : étude de faisabilité de la commande cartésienne
¾ phase 2 : étude de faisabilité de la commande pour la prise automatique d'outils
¾ phase 3 : validation réelle et intégration sur le robot réel
Ce projet s’appuie sur le savoir faire du LASMEA dans les thématiques de l’asservissement
visuel, l’identification et la commande des robots par vision, et la localisation par vision
monoculaire.
Phase 1 :
Etude de faisabilité théorique de la commande cartésienne du bras
manipulateur
Cette phase essentiellement théorique sera de rechercher un principe de commande d'un
robot manipulateur à 6 ddl par vision sans ajout de capteur de position articulaire. Le robot
sera commandé en vitesse articulaire au moyen d'une commande référencée vision (3D, 2D,
ou mixte). La mise en oeuvre de cette commande nécessitera l'observation de l'effecteur afin
de faire la commande en position/orientation, mais aussi l'observation des différents organes
du robot afin d'estimer en ligne la matrice Jacobienne du robot. Cette étude sera validée selon
le protocole suivant en utilisant MATLAB/SIMULINK :
¾ Etude d'un robot simple à 3 ddl style 3R plan (commande en position seulement)
¾ Etude de la commande en position sur un robot 6ddl simple (style cartésien avec
une rotule d'axes concourants)
¾ Etude de la commande en position sur le bras à 6ddl considéré (les modèles
4
géométriques et cinématiques seront fournis)
L'instrumentation du robot réel sera définie au moyen de cette étude de faisabilité.
Eventuellement, plusieurs solutions pourront être produites.
Phase 2 :
Etude de faisabilité théorique de la commande pour une prise automatique
d'outils
Cette étude suivra celle décrite en phase 1. Elle sera également essentiellement théorique et
aura pour objectif de rechercher un principe de commande référencée vision pour la prise
automatique d'outils. La principale action à mener sera de traduire en langage commande
référencée capteur (vision) ce que sera la tâche de prise automatique d'outils : une seule tâche,
enchaînement de plusieurs tâches, combien de degrés de liberté nécessaires, ... Elle consistera
également à définir les algorithmes de vision ainsi que l'instrumentation associés nécessaires à
la réalisation de la tâche. Cette étude sera validée selon le protocole suivant en utilisant
MATLAB/SIMULINK :
¾ Etude de la commande en position sur un robot 6ddl simple (style cartésien avec
une rotule d'axes concourants)
¾ Etude de la commande en position sur le bras à 6ddl considéré (les modèles
géométriques et cinématiques seront fournis)
L'instrumentation du robot réel sera défini au moyen de cette étude de faisabilité.
Eventuellement, plusieurs solutions pourront être produites.
Phase 3 :
Intégration et validation sur le robot réel (pourra être ajustée en fonction
des phases précédentes)
L'objectif de cette troisième action est la réalisation expérimentale des résultats théoriques
exhibés en phases 1 et 2. La phase 3 comportera 3 aspects :
¾ Développement et validation des modèles du robot CENTAURE pour la
commande (MATLAB/Simulink)
¾ Validation des phases 1 et 2 sur robot du LASMEA (à Clermont-Ferrand) ou
IFMA
¾ Validation des phases 1 et 2 sur le robot CENTAURE
Durant cette phase, la mise en oeuvre d'un système de vision et commande temps réel
(prérequis à l'intégration logicielle : commande en vitesse articulaire du robot disponible) sera
faite. Une partie du travail à mener dans cette phase pourra être accomplie en parallèle avec
les phases 1 et 2. Néanmoins, il faut prévoir qu'elle se termine après ces dernières.
Organisation du projet :
Moyens humains disponibles sur ce projet :
¾
¾
3 élèves ingénieurs IFMA
Mcf Nicolas Andreff (Commande référencée capteur, identification géométrique de robot par
5
vision)
¾ Pr Philippe Martinet (Commande référencée capteur, intégration)
¾ Mcf Youcef Mezouar (Commande référencée capteur)
Délivrables et calendrier de réalisation
T0 : début du projet
T0+ 2 mois : Présentation de solutions phase 1 et 2
T0+ 4 mois : Rapport intermédiaire de contrat
T0+ 9 mois : Rapport final de contrat
6
II. Modélisation et environnement d’évaluation
La commande des robots nécessite le calcul de certains modèles mathématiques tels que les modèles de
transformation entre l’espace opérationnel et l’espace articulaire.
La modélisation des robots de façon systématique et automatique exige une méthode adéquate pour la
description de leur morphologie. Plusieurs méthode et notations ont été proposées, la plus répandue est celle de
Denavit-Hartenberg. Cette méthode, bien adaptée pour des structures ouvertes simples, présente des ambiguïtés
lorsqu’elle est adaptée sur des robots ayant des structures fermées ou arborescentes. Khalil et Kleinfinger ont
proposé une notation qui permet la description homogène des architectures ouvertes simples et complexes de
systèmes mécaniques articulés. La modélisation du Centaure a été réalisée avec cette méthode.
A. Description de la structure géométrique des robots
Ce paragraphe introduit les notations utilisées pour décrire la structure géométrique des robots et qui sont par
conséquent à la base de la mise en équation de ces derniers.
i.
Notations et règles générales
La méthode est basée sur les règles et conventions suivantes :
- la variable de l’articulation j est notée qj,
- le corps j est noté Cj
- les corps sont supposés parfaitement rigides. Ils sont connectés par des articulations considérées comme
idéales (pas de jeu mécanique, pas d’élasticité), soit rotoïdes, soit prismatiques
- le repère Rj est lié au corps Cj
- l’axe zj du repère Rj est porté par l’axe de l’articulation j
- les paramètres qui permettent de définir Rj par rapport au repère antécédent sont indicés j
ii.
Description des robots à chaîne ouverte simple
Le système est composé de n+1 corps, notés C0,…,Cn, et de n articulations. Le corps C0 désigne la base du robot
et le corps Cn le corps qui porte l’organe terminal. L’articulation j connecte le corps Cj au corps Cj-1.
Figure 1: robot à chaîne ouverte
Le repère Rj, fixé au corps Cj, est défini de telle sorte que :
- l’axe zj est porté par l’axe de l’articulation j
- l’axe xj est porté par la perpendiculaire commune aux axes zj et zj+1. Si les axes zj et zj+1 sont parallèles
ou colinéaires, le choix de xj n’est pas unique : des considérations de symétrie ou de simplicité
permettent alors un choix rationnel.
Le passage de Rj-1 à Rj s’exprime en fonction des quatre paramètres suivants :
7
-
-
αj : angle entre les axes zj-1 et zj, correspondant à une rotation autour de xj-1
dj : distance entre zj-1 et zj le long de xj-1
θj : angle entre les axes xj-1 et xj, correspondant à une rotation autour de zj
rj : distance entre xj-1 et xj, le long de zj
xk
θk
zk
rk
zk-1
xk-1
αk
d
Figure 2: passage de Rj-1 à Rj
La variable articulaire qj associée à la jième articulation est soit θj, soit rj, selon que cette articulation est de type
rotoïde ou prismatique ce qui se traduit par la relation :
qj= (1-σj) θj + σj rj
avec :
σj=0 si l’articulation j est rotoïde
σj=1 si l’articulation j est prismatique
La matrice de transformation définissant le repère Rj dans le repère Rj-1 est donnée par :
j-1
Tj = Rot(x,αj) Trans(x,dj) Rot(z,θj) Trans(z,rj)
=
Cθj
-Sθj
0
dj
CαjSθj
CαjCθj
-Sαj -rjSαj
SαjSθj
SαjCθj
Cαj
rjCαj
avec la convention Cθj=cos(θj) et Sθj=sin(θj)
Remarques
- pour la définition du repère de référence R0, le choix le plus simple consiste à prendre R0 confondu
avec le repère R1 quand q1=0. Ce choix n’est pas obligatoire, mais il annule les paramètres α1, d1 et
donne q1=1
- cette méthode de description fixe la configuration zéro du robot telle que q=0. on peut cependant
choisir une autre configuration zéro quelconque en procédant au changement de variable suivant :
qc = q – q0
iii.
Le modèle géométrique direct du robot Centaure
8
On s’intéresse ici au bras robot Romain qui est monté sur la base.
Des plans et vues d’ensemble nous ont été fournies par le CEA, ce qui nous a permit de modéliser le bras de la
façon suivante :
z4,z6
q6
X4, X5, X6
z5
q5
z0,z1
q4
q2
z2
D3
RL4
x0,x1,x2
q3
Z3
q1
x3
RL3
Articulation
de la base mobile
Figure 3: modélisation du Centaure
La première liaison (la plus basse sur le schéma) ne faisant pas partie du bras robotisé mais de la base n’est pas
prise en compte dans la modélisation
D’après la modélisation vu précédemment, nous avons les paramètres suivants :
j
σj
αj
dj
θj
rj
1
0
0
0
θ1
0
2
0
π/2
0
θ2
0
3
0
0
D3
θ3
RL3
4
0
-π/2
0
θ4
RL4
5
0
π/2
0
θ5
0
6
0
-π /2
0
θ6
0
9
Figure 4: paramètres de la modélisation
Avec D3 = 300 mm, RL3 = 84.5 mm et RL4 = 350 mm
Ceci nous donne pour les passages entre les différents repères :
Cθ1
Sθ1
0
1
2
3
4
5
T1 =
-Sθ1
0
0
Cθ1 0
0
0
0
1
0
0
0
0
1
Cθ2
-Sθ2
0
0
0
0
-1
0
Sθ2
Cθ2
0
0
Cθ3
-Sθ3
0 D3
Sθ3
Cθ3
0
0
0
0
1
RL3
T2 =
T3 =
T4 =
T5 =
T6 =
Cθ4
-Sθ4
0
0
0
0
1
RL4
-Cθ4
0
0
-Sθ4
Cθ5
-Sθ5
0
0
0
0
-1
0
Sθ5
Cθ5
0
0
Cθ6
-Sθ6
0
0
0
0
1
0
-Cθ6
0
0
-Sθ6
10
avec la convention Cθ1=cos(θ1) et Sθ1=sin(θ1)
Et nous avons 0T6 = 0T1 1T2 2T3 3T4 4T5 5T6
Cela nous donne le modèle géométrique direct (MGD) du Centaure, la position et l’orientation du repère attaché
à l’organe terminal par rapport au repère de base.
L’expression littérale du MGD peut être obtenue à l’aide de Maple.
S’agissant de X= f(q) MGD du robot Centaure, il ne faut pas oublier la définition de X.
Si on prend par exemple
-
X=(XYZ RPY)T, c'est-à-dire la position cartésienne et les angles roulis tangages lacet,
-
Le repère Atelier RA=R0
-
Le repère Outil Routil=R6
Alors on obtient :
X= Px
Y= Py
Z= Pz
R= Atan2(sy,sx) à Pi près (R= Atan2(-sy,-sx)+ Pi)
P= Atan2(-sz, cos(R).sx+sin(R).sy)
Y= Atan2(sin(R).ax-cos(R).ay, -sin(R).nx+cos(R).ny)
où s=(sx,sy,sz)T, n=(nx,ny,nz)T et a=(ax,ay,az)T sont les cosinus directeurs de la matrice 0T6 et P=(Px,Py,Pz)T
représente la matrice de position de 0T6 = 0TOutil.
Dans le cas où sx=sy =0, il y a singularité de représentation.
11
iv.
Mise en place du MGD du robot Centaure sous Matlab
Le programme donnant le modèle géométrique direct du robot a été obtenu en deux étapes :
Une première étape consistait en l’obtention d’un modèle générique pour un robot à six degrés de liberté, ce
programme (mgdDHsym en annexe) utilise en entrée le fichier Centaure (voir annexes) décrivant le robot
(notamment les paramètres de Khalil-Kleinfinger) et les valeurs articulaires. Avec le calcul symbolique, il est
possible de rentrer symboliquement les variables articulaires et d’obtenir en sortie le modèle géométrique direct
sous forme symbolique et totalement paramétré.
Grâce à la première étape, nous obtenons un modèle du robot Centaure utilisant tous les paramètres de KhalilKleinfinger ; la seconde étape consiste à générer un nouveau programme à partir du premier et n’utilisant que les
variables « utiles » de la modélisation, c’est à dire les variables articulaires, afin de réduire les temps de calcul.
Ce modèle est débarrassé des nombreuses variables qui n’évoluent que si l’on change la géométrie du robot. Le
programme ainsi réalisé est présent en annexe : mgdDHnum.
Le programme Matlab réalisant le modèle géométrique direct utilise un autre programme appelé TDH qui
calcule les valeurs des matrices de transfert entre les différents éléments du robot.
Le programme visuDH permet de visualiser grossièrement la position du robot Centaure dans différentes
positions articulaires. Le repère attaché à la base est représenté en magenta avec l’axe z en bleu et le repère final
est représenté en vert toujours avec l’axe z en bleu.
Figure 5: représentation du robot Centaure pour deux configurations articulaires
12
B. Le modèle différentiel direct
Le modèle différentiel direct d’un robot manipulateur décrit les variations élémentaires des coordonnées
opérationnelles en fonction des variables élémentaires des coordonnées articulaires. Il est noté :
dX = J(q) dq
où J(q) désigne la matrice jacobienne de dimension (m × n) du mécanisme, égale à ∂X/∂q et fonction de la
configuration articulaire q.
L’intérêt d’une telle description, fréquemment utilisée en robotique, est multiple :
Elle est à la base du modèle différentiel inverse, permettant de calculer de façon itérative les variables
articulaires q connaissant les coordonnées opérationnelles X. A noter que la solution obtenue est locale et dépend
de la position initiale.
Si, au lieu de calculer les différentielle, on calcule les dérivées par rapport au temps des coordonnées
opérationnelles et des coordonnées articulaires, on obtient le modèle cinématique direct qui exprime les vitesses
opérationnelles en fonction des vitesses articulaires :
& = J (q) q&
Χ
En statique, on utilise le jacobien pour établir la relation liant les efforts exercés sur l’organe terminal aux forces
et couples des actionneurs
Elle facilite le calcul des singularités et de la dimension de l’espace opérationnel accessible du robot.
i.
Le jacobien de base
Le calcul de la matrice jacobienne peut se faire de plusieurs façons. Celle qui semble la plus naturelle consiste à
déduire ses éléments (i,j) de ceux du modèle géométrique direct X = f(q) à partir de la relation suivante :
J ij =
∂f i (q)
∂q j
i =1,…,m ; j = 1,…,n
Cette procédure fait dépendre J(q) des coordonnées opérationnelles X choisies pour décrire la situation
Nous nous intéresserons ici plutôt à une méthode de calcul direct, basée sur le calcul du jacobien entre les
vecteurs de translation et de rotation différentiels (dn,δn) du repère Rn et les différentielles des coordonnées
articulaires dq (l’expression du jacobien est identique si l’on considère le jacobien entre les vecteurs vitesses de
translation et de rotation Vn et ωn du repère Rn et les vitesses articulaires). Le jacobien correspondant à une
représentation quelconque des coordonnées opérationnelles X peut être déduit de ce jacobien, appelé jacobien de
base :
dn
δn
= Jn dq
13
On exprime en général dn et δn soit dans le repère Rn, soit dans le repère R0. la matrice jacobienne
correspondante est notée nJn ou 0Jn respectivement.
ii.
Calcul de la matrice jacobienne de base
Nous nous intéresserons seulement au cas d’une chaîne ouverte simple (c’est le cas du robot manipulateur
Romain).
Considérons la kième articulation d’une chaîne articulée et, selon les conventions choisies, soit ak le vecteur
unitaire porté par l’axe zk de l’articulation. Une variation infinitésimale dqk de la coordonnée articulaire qk
provoque sur le repère terminal Rn du manipulateur des variations infinitésimales de position dk,n et d’orientation
δk,n.
Deux cas se présentent :
L’articulation est prismatique
dk,n=ak dqk
δk,n= 0
figure 6: cas d'une articulation prismatique
l’articulation est rotoïde
dk,n=ak dqk × Lk,n = (ak × Ln,k) dqk
δk,n= ak dqk
14
Figure 7: cas d'une articulation rotoïde
Le terme Lk,n désignant le vecteur reliant les origines des repères Rk et Rn .
En appliquant le théorème de composition des vitesses, on montre que l’on peut additionner les contributions
élémentaires de chaque articulation pour déterminer les vecteurs de translation et de rotation différentielles du
repère terminal par la relation suivante :
dk,n=∑[σk ak + (1-σk) (ak × Ln,k) ]dqk
δk,n= ∑[(1-σk)ak] dqk
En mettant ce système sous forme matricielle et en l’identifiant à la relation :
dn
δn
= Jn dq
on obtient :
dn
σ1a1 + (1-σ1)(a1×L1,n) … σnan + (1-σn)(an×Ln,n)
=
δn
(1-σ1)a1
...
(1-σn)an
dq1
…
…
= Jn dq
dqn
Jn est appelé jacobien vectoriel du robot manipulateur.
Si l’on projette les éléments du jacobien vectoriel Jn dans un repère quelconque Ri, on obtient un jacobien
scalaire iJn du même type que celui défini par la relation :
dn
δn
= Jn dq
15
Cette opération consiste à exprimer les vecteurs de la matrice vectorielle dans le repère Ri considéré.
Après développement, on obtient l’expression générale de la kième colonne njk de nJn . Ses éléments sont fonction
des éléments de la matrice kTn, résultat intermédiaire obtenu lors du calcul du modèle géométrique direct.
k rotoïde
-
k prismatique
sx
Py +
nx
ax
sy
ny
Px
ay
sz
nz
az
nj,k
sz
0
nz
0
az
0
Les éléments s,n,a,P appartiennent à la matrice kTn
Pour trouver une projection dans un autre repère, le repère Ri par exemple, il suffit d’utiliser la relation :
i
An
i
Jn =
0
0
n
Jn
i
An
avec iAn matrice des cosinus directeurs de iTn
iii.
Mise en place du modèle cinématique direct sous Matlab
Le modèle cinématique réalisé sous Matlab a été réalisé en trois grandes étapes :
Réalisation d’un modèle cinématique direct générique obtenu à partir des formules générales développées dans
la section précédente. Ce fichier prend en compte tous les paramètres du robot afin de répercuter les
modifications le plus rapidement possible. Ce modèle demande un temps de calcul relativement important car il
appelle de nombreuses variables contenues dans le fichier de description du robot. L’option de calcul
symbolique de Matlab est nécessaire au fonctionnement de ce programme. En effet, l’intérêt principal de ce
dernier est la génération de la matrice jacobienne 6J6 du robot sous sa forme symbolique, c’est à dire l’obtention
littérale des différents composants de la matrice. Ce programme (JacobtotalsymDH) peut être consulté en annexe
(son expression complexe n’offre que peu d’intérêt).
Génération à partir du premier programme d’un nouveau fichier réalisant le calcul de la jacobienne du robot
Centaure en fonction de son paramétrage. Ce dernier renvoie une réponse numérique ; il réalise l’affectation des
16
valeurs aux variables.
Elaboration d’un programme spécifique pour le robot Centaure afin de réduire le temps de calculs nécessaires.
C’est une version allégée du précédent, seuls les paramètres non nuls interviennent dans le calcul (voir en
annexe, JacobtotalnumDH).
Rappel : selon la définition de X, on a :
⎛Ω
J X (q ) = ⎜⎜ XYZ
⎝ O3
O3 ⎞ ⎛ 0 R6
⎟⋅⎜
Ω RPY ⎟⎠ ⎜⎝ O3
O3 ⎞ 6
⎟⋅ J 6
R6 ⎟⎠
0
Les sous-blocs de la première matrice dépendent totalement de la convention prise pour X.
17
C. Le modèle différentiel inverse
i.
Utilité du modèle différentiel inverse
L’objectif du modèle différentiel inverse est de calculer, à partir d’une configuration q donnée, la différentielle
articulaire dq à commander pour satisfaire une différentielle des coordonnées opérationnelles dX imposée. Cette
définition est analogue à celle du modèle cinématique inverse : ce dernier permet de déterminer les vitesses
articulaires qui assurent au repère terminal une vitesse spécifiée.
Deux classes de méthodes peuvent être mises en œuvre pour obtenir le modèle différentiel inverse.
Celles qui procèdent par dérivation des modèles géométriques inverses. Il s’agit de méthodes analytiques qui ont
pour avantage de donner tous les modèles différentiels inverses associés aux différents modèles géométriques
inverses mais, de part leur principe, elles exigent que le robot manipulateur considéré soit résoluble.
Celles plus générales, qui consistent à inverser le modèle différentiel direct en résolvant un système d’équations
linéaires. La mise en œuvre de ce type de méthodes peut être faite de façon analytique ou numérique :
La solution analytique a pour avantage de diminuer considérablement le nombre d’opérations, mais on doit
traiter séparément tous les cas singuliers
Les méthodes numériques sont plus générales, la plus répandue étant basée sur la notion de pseudo inverse : les
algorithmes traitent de façon unifiée les cas réguliers, singuliers et redondants. Elles nécessitent un temps de
calcul relativement important, ce qui les rend plus adaptées pour la simulation que pour la commande en ligne.
Nous avons ici opté pour une méthode numérique d’inversion du modèle différentiel direct en utilisant la notion
de pseudo inverse. Le modèle cinématique direct, obtenu comme nous avons vu précédemment, renvoie une
matrice de dimension six par six. Celle ci est alors inversée numériquement en utilisant la notion de pseudo
inverse.
ii.
Mise en place du modèle cinématique inverse sous Matlab
La mise en place sous Matlab a consisté en l’écriture d’un programme avec comme entrée une matrice et
utilisant la fonction de pseudo inverse svd (singular value decomposition) de Matlab. Ces programmes sont
présents en annexe sous les nom PINV_SVD et MCD_CENTAURE.
D. Environnement d’évaluation
i.
Présentation des environnements MATLAB/Simulink
Afin de faire du prototypage rapide de loi de commande par retour visuel, ainsi que la validation algorithmique
des principes et méthodes mise en jeu, nous avons choisi l’environnement MATLAB/Simulink.
Dans cette étude de faisabilité, nous retiendrons donc la démarche suivante : validation et évaluation des
18
méthodes et principes en prototypage rapide, et validation expérimentale sur le robot du LASMEA
(MOTOMAN) à 6 degrés de liberté. L’intégration finale est prévue d’être faite au CEA et avec le CEA après
transfert du système de vision commande qui devront être interconnecté (voir la partie intégration).
Matlab est un environnement de programmation permettant l’accès rapide à des bibliothèques (appelée
TOOLBOX : boîte à outils). Simulink permet de définir un problème en flot d’informations et de données dans
un schéma interconnecté : ainsi, lorsque l’on veut faire de la commande en boucle fermée le schéma bloc de
commande est saisie sous simulink. De la même manière, il existe déjà disponible des toolbox spécialisés pour
simulink. Pour plus d’informations sur Matlab/Simulink et les Toolbox se référer à http://www.mathworks.com .
Figure 8: Un exemple de schéma bloc développé sous Simulink
ii.
Un exemple de prototypage rapide pour l’asservissement
visuel : la Visual Servoing Toolbox
Dans la communauté Asservissement Visuel scientifique internationale, la communauté scientifique française est
une des plus actives (Voir exposé de François Chaumette aux JNRR03 disponible sur le site web : http://
http://wwwlasmea.univ-bpclermont.fr/jnrr03/index.html). Ainsi, au niveau européen un groupe d’intérêt, avec
pour thème principal « Visual Servoing », a été créé dans le cadre du réseau d’excellence EURON. Ce groupe
d’intérêt a organisé en 2002 un workshop international dans le cadre de IROS 2002, et une école d’été. Son site
web est le suivant : http://www.robot.uji.es/EURON/visualservoing/ . Dans le cadre de ce groupe d’intérêt, le
développement d’une « Visual Servoing Toolbox » est en cours : la version 1.0 sortira en 2004. L’ensemble des
travaux menés dans cette étude de faisabilité bénéficie des premiers résultats obtenus avec cette toolbox. Un
exemple de schéma d’asservissement visuel 2D est donné par le synoptique simulink suivant :
19
Figure 9 : exemple de schéma d’asservissement visuel 2D
iii.
Environnement d’évaluation retenu
Afin d’évaluer au mieux nos principes et méthodes nous utiliserons l’environnement suivant :
-
une fonction matlab permettant l’évaluation des variables articulaires (voir fichier : estim_theta.m).
Le protoype de la fonction est le suivant : [q_est] = estim_theta(data,fctMGD,fctMCD). Le
paragraphe III donnera les détails sur la méthode employée.
-
Schémas simulink permettant d’implanter des lois de commande particulières
Des modèles de robots différents : Motoman (robot du LASMEA), Centaure (Robot du CEA).
Seules la géométrie et la cinématique des deux robots seront prises en compte.
20
.
III. Estimation des variables articulaires
Afin de résoudre le problème de la commande cartésienne en boucle ouverte ou bien du positionnement du robot
par asservissement visuel le schéma classique nécessite le calcul de l’estimation de J
−1
(q) . Deux approches
principales peuvent être envisagées :
-
Estimation des variables articulaires
•
Estimation en ligne par observation de l’effecteur. Cette approche nécessite une
(ou plusieurs) caméra déportée(s), une mire, les modèles du robot, et un algorithme
de calcul de pose. C’est la méthode que nous avons retenue.
•
Estimation en ligne par observation de l’environnement. Cette variante nécessite
une caméra embarquée, les modèles du robot, et un algorithme de reconstruction
3D (avec ou sans mires dans l’environnement). Sans mires dans l’environnement,
ce problème est un réel problème de recherche en perception. Le fait de rajouter des
mires dans l’environnement de travail a été écarté durant les réunions
d’avancement. De plus, cela nécessite une intervention sur les sites où les robots
doivent être opérationnel.
-
Estimation des éléments de la jacobienne inverse
•
Estimation à l’équilibre (hors ligne). Cette approche nécessite une caméra
embarquée, un algorithme d’estimation paramétrique hors ligne et un algorithme
d’extraction
d’informations
2D
et/ou
3D
(avec
ou
sans
mires
dans
l’environnement). Le calcul s’effectue seulement à l’équilibre (les mesures et
l’estimation sont faites une fois pour toute à l’équilibre). Il est clair que pour la
boucle fermée (boucle d’asservissement visuel) cela peut ne pas poser de problèmes
(proche de l’équilibre), mais pour la commande cartésienne, la propagation des
erreurs risque d’être trop importante pour assurer un bon suivi d’une trajectoire
cartésienne.
•
Estimation en ligne. Cette approche nécessite une caméra embarquée, un
algorithme d’estimation paramétrique en ligne (avec un modèle d’évolution lent) à
partir d’informations 2D et/ou 3D (avec ou sans mires dans l’environnement) (les
mesures et l’estimation sont faites à chaque itération). Cette méthode peut être
intéressante dans la mesure où le modèle d’évolution retenu est fiable (valide) et
alimenter en mesures également pertinente. Une approche multi-capteurs est peutêtre une piste à creuser.
i.
Algorithme d’estimation en ligne des variables articulaires
Comme nous l’avons vu précédemment, il faut pallier à l’absence de capteur articulaire du robot Romain.
Nous allons ainsi utiliser une méthode itérative d’évaluation des variables articulaires utilisant le modèle
cinématique inverse du robot. En effet, le modèle cinématique inverse nous donne :
q& = J −1 (q ) X&
.
21
.
En assimilant q& à ∆q et X& à ∆X, on obtient le modèle variationnel valable dans nos conditions de
fonctionnement (robot relativement lent) :
∆q = J-1(q) ∆X
Il a donc fallu développer un algorithme afin de réaliser l’estimation des q de façon itérative.
La condition d’arrêt ou non de l’algorithme est la proximité de la position courante avec la position désirée (ces
coordonnées sont exprimées en coordonnées cartésiennes dans le repère de base).
Tant que C= ||0Xmesuré-0X(qcourant)|| > ε ; la valeur critère ε est fixé en fonction de la précision voulue
qcourant c’est la variable sur laquelle se fait l’itération, le qcourant initial est connu.
Calcul de 6J6 courant : comme nous l’avons vu précédemment, le calcul de la jacobienne nécessite l’entrée des
variables articulaires pour le calcul, les coordonnées articulaires de la première position sont des paramètres
d’entrée ; au cours des autres itérations, on utilise l’évaluation des variables articulaires obtenue à l’itération
précédente.
Calcul de 0J6 courant : les coordonnées cartésiennes sont exprimées dans le repère de base, il est donc nécessaire
d’exprimer la matrice jacobienne dans le repère de base afin d’assurer l’homogénéité de la démarche. Il est donc
nécessaire de calculer les matrices de passage qui sont fonction de la position cartésienne courante.
Inversion de 0J6 courant : l’inversion est réalisée grâce à l’utilisation de la pseudo inverse (svd) fournie par le
logiciel Matlab. Ceci nous permet de pallier aux problèmes de singularités lors de l’inversion.
Calcul de delta q : en utilisant l’équation ∆q = J-1(q) ∆X, connaissant le ∆X, nous pouvons calculer le ∆q à partir
de l’inversion numérique de la jacobienne calculée au point de coordonnées articulaires courantes.
Evaluation de q : le nouveau q est l’ancien additionné du ∆q calculé.
.
22
.
qi = qinit
Pour une mesure de
J X (q i )
(
)
∆q = −λ ⋅ pinv _ SVD J X (qi ) ⋅ ∆ X
Evaluation de
C
q = qi
oui
C<
ε
non
qi = qi + ∆ q
Figure10 : Algorithme d’estimation des variables articulaires
ii.
Mise en place de l’évaluation des variables articulaires
Le programme Matlab réalisant cette tâche s’appelle estim_theta (présenté en annexe). Il prend en entrée les
modèles du robot, la position cartésienne courante. Il renvoie en sortie les nouvelles variables articulaires. Il met
en œuvre le programme MGI_numerique (voir annexe). La convergence de l’algorithme est rapide et la précision
de l’estimation est réglable au moyen de variables présentes dans les premières lignes du programme :
9
La valeur initiale des variables articulaires (issue de la précédente estimation, sauf au premier coup où il
faut rentrer des données correspondantes à une configuration connue).
9
Nombres d’itération : nous l’avons fixé à 50 itérations. Selon la précision finale souhaitée, ce paramètre
peut être augmenté (meilleure précision), ou diminuer (moins bonne précision).
9
Espilon : la limite de convergence que l’on se fixe pour sortir avant que la limite Selon la précision
finale souhaitée, ce paramètre peut être augmenté (moins bonne précision), ou diminuer (meilleure
précision).
iii.
Quelques résultats de simulation pour l’estimation des
variables articulaires
.
23
.
Des essais ont été menés pour différentes poses du robot CENTAURE tirées de manière aléatoire au niveau
articulaire. Les valeurs initiales des variables articulaires ont été fixées à 0.5, 1, 2 ou 5 degrés près (de
manières aléatoires) afin de rendre réaliste l’algorithme d’estimation.
Les conditions d’évaluation portent sur la précision obtenue entre les variables articulaires réelles et celles
estimées :
- Pour ε = 0.1
convergence et précision en q < 10-2
rd
- Pour ε = 0.01
convergence et précision en q < 10-3
rd
-4
rd
- Pour ε = 0.001
convergence et précision en q < 10
- Pour ε = 0.0001 non convergence et précision en q < 10-3
rd
(convergence en 100 avec q < 10-5 rd)
Dans tous les cas étudiés, la précision a toujours été < à 5.10-3 rd.
.
LASMEA
UMR 6602 du CNRS
.
24
IV. Commande cartésienne référencée capteur
Un des objectifs fixé lors du projet est de permettre la commande cartésienne d’un robot non instrumenté
en mode manuel. Deux modes de fonctionnement peuvent être envisagés :
- La commande a lieu dans le repère de l’effecteur
- La commande a lieu dans le repère d’une caméra déportée (fixe par rapport à la base du robot) fournissant
un retour visuel à l’opérateur
Principe de la commande cartésienne manuelle
La commande manuelle est essentiellement une commande en boucle ouverte dont la consigne (en vitesse
cartésienne) est fournie par l’opérateur. Cette consigne cartésienne doit être convertie en consigne de vitesse
articulaire pour actionner le robot (cf. Figure 11).
-C-
q
6V6
dq*
q
Commande Manuelle
dans repere effecteur (6V6)
Consigne de v itesse articulaire
dq*
0T6
Commande Cartesienne
Repere Effecteur
Robot
v ariables articulaires mesurees
Figure 11 : Principe de la commande manuelle
Pour ce faire, il faut utiliser le modèle cinématique inverse :
⎛V ⎞
q& = J −1 (q )⎜⎜ ⎟⎟
⎝Ω⎠
On remarque que la matrice Jacobienne (J) intervenant dans ce modèle dépend des valeurs articulaires
courantes. Par conséquent, il est nécessaire, pour un robot ne possédant pas de capteurs proprioceptifs, d’estimer
ces valeurs d’une manière ou d’une autre.
Dans notre cas, il s’agit de procéder à cette estimation par l’intermédiaire d’une caméra observant
l’effecteur du robot. Dans une optique de simplification expérimentale, nous placerons une mire sur cet
effecteur. Ceci introduit, en toute rigueur, le besoin de pouvoir estimer la position relative de la mire par rapport
à l’effecteur. Néanmoins, dans un souci pédagogique et sans perte de généralité, les méthodes développées cidessous considèrent les repères associés à ces deux objets comme confondus. Il faudra veiller lors de
l’implantation finale à prendre en compte cette position relative.
Non disponible
-C-
q
6V6
dq*
Commande Manuelle
dans repere effecteur (6V6)
q
Consigne de v itesse articulaire
dq*
0T6
Robot
Repere Effecteur
Estimation des
v ariables articulaires estimees
q_est
image
Camera deportee
image
OT6
Figure 12 : Principe de la commande cartésienne manuelle
avec estimation des variables articulaires par vision
25
.
En assimilant q& à ∆q et X& à ∆X, on obtient le modèle variationnel valable dans nos conditions de
fonctionnement (robot relativement lent) :
∆q = J-1(q) ∆X
Il a donc fallu développer un algorithme afin de réaliser l’estimation des q de façon itérative.
La condition d’arrêt ou non de l’algorithme est la proximité de la position courante avec la position désirée (ces
coordonnées sont exprimées en coordonnées cartésiennes dans le repère de base).
Tant que C= ||0Xmesuré-0X(qcourant)|| > ε ; la valeur critère ε est fixé en fonction de la précision voulue
qcourant c’est la variable sur laquelle se fait l’itération, le qcourant initial est connu.
Calcul de 6J6 courant : comme nous l’avons vu précédemment, le calcul de la jacobienne nécessite l’entrée des
variables articulaires pour le calcul, les coordonnées articulaires de la première position sont des paramètres
d’entrée ; au cours des autres itérations, on utilise l’évaluation des variables articulaires obtenue à l’itération
précédente.
Calcul de 0J6 courant : les coordonnées cartésiennes sont exprimées dans le repère de base, il est donc nécessaire
d’exprimer la matrice jacobienne dans le repère de base afin d’assurer l’homogénéité de la démarche. Il est donc
nécessaire de calculer les matrices de passage qui sont fonction de la position cartésienne courante.
Inversion de 0J6 courant : l’inversion est réalisée grâce à l’utilisation de la pseudo inverse (svd) fournie par le
logiciel Matlab. Ceci nous permet de pallier aux problèmes de singularités lors de l’inversion.
Calcul de delta q : en utilisant l’équation ∆q = J-1(q) ∆X, connaissant le ∆X, nous pouvons calculer le ∆q à partir
de l’inversion numérique de la jacobienne calculée au point de coordonnées articulaires courantes.
Evaluation de q : le nouveau q est l’ancien additionné du ∆q calculé.
.
26
requiert la connaissance de la pose de l’effecteur par rapport à la caméra et que cette information n’est disponible
que grâce au calcul de pose inclus dans l’estimation des valeurs articulaires par vision.
-C-
jVj
iVi
Commande cartesienne manuelle
dans repere camera (cVc)
Commande cartesienne
dans repere ef f ecteur (6V6)
Non disponible
theta
6V6
dq*
jTi
theta
Changement Repere
Camera -> Effecteur
dq*
0T6
Consigne de
v itesse articulaire
Robot
Repere Effecteur
Memorisation
-C- Pose camera->base
theta_init
v ariables articulaires
estimees
theta_est
Camera Deportee
0Tc
Image
uv
uv
0T6
6P
cT6_est
pose camera->ef f ecteur estimee
max_iter (>0)
epsilon (>0)
P
Points Mire
img
Estimation des
epsilon 1e-3
100 max_iter
Figure 14 : Commande cartésienne dans le repère de la caméra déportée
Validation de la commande cartésienne manuelle en simulation
Les deux modes de fonctionnement ont été validés en simulation. Néanmoins, afin de ne pas alourdir
inutilement ce rapport, nous nous concentrerons sur une analyse de la commande cartésienne manuelle dans le
repère de la caméra. En effet, ce mode de fonctionnement étant le plus complexe des deux, sa validation
entraînera celle du second mode.
Il est difficile de tester la méthode dans toutes les configurations du robot, aussi nous sommes-nous
restreints à une seule configuration (non singulière). De plus, il est inutile de tester toutes les commandes
articulaires possibles : nous pouvons nous contenter d’analyser le comportement du système pour des
mouvements particuliers : translation selon un des 3 axes du repère caméra, rotation autour de l’axe optique. En
effet, ces mouvements s’interprètent aisément dans l’image et la qualité de réalisation de ces mouvements
peuvent se « mesurer » visuellement.
1.
2.
3.
-
Les tests ont été conduits de la manière suivante :
Simulation dans le cas idéal où les valeurs des variables articulaires utilisées dans le modèle cinématique
inverse sont les valeurs réelles
Simulation dans le cas quasi-idéal où les valeurs des variables articulaires sont estimées par vision à partir
d’images non bruitées
Simulation dans le cas où les valeurs des variables articulaires sont estimées par vision à partir d’images
bruitées par un bruit uniforme centré (3 niveaux de bruits sont utilisés : ±1 pixel, ±2 pixels, ±5 pixels)
Dans les figures présentées dans la suite, apparaissent en :
rouge, la trajectoire dans le cas idéal ;
bleu, la trajectoire dans le cas quasi-idéal ;
mauve, la trajectoire lorsque le bruit est de ±1 pixel ;
turquoise, la trajectoire lorsque le bruit est de ±2 pixels ;
vert, la trajectoire lorsque le bruit est de ±5 pixels.
Remarque : Ces tests ne prennent volontairement en compte aucune perturbation physique du robot. Par
ailleurs, afin de bien faire ressortir l’effet du bruit dans l’image sur le mouvement réalisé par le robot, nous avons
artificiellement supprimé ce bruit dans le tracé des trajectoires dans l’image.
27
Translation selon l’axe horizontal de l’image
Le premier test présenté ici consiste en une translation du robot Centaure selon l’axe horizontal de la
caméra déportée (c’est-à-dire fixe par rapport à la base du bras manipulateur). La trajectoire dans l’image de
chaque point de la mire qui résulte de ce mouvement spatial doit être un segment horizontal.
Nous avons tracé les trajectoires dans l’image (Figure 15) ainsi que les trajectoires spatiales (Figure 16)
de ce déplacement dans les 5 cas cités ci-dessus.
Figure 15 : Trajectoire des points de la mire dans l’image
pour une translation demandée selon l'axe horizontal de la caméra
(à gauche : image entière ; à droite : zoom sur la partie significative)
Figure 16: Trajectoire spatiale pour une translation demandée le long de l’axe horizontal de la caméra
(en haut : vue 3D ; en bas, à gauche : vue de gauche ; en bas, à droite : vue de dessus)
Pour ce qui est du comportement visuel, on remarque également que malgré les bruits dans l’image, les
trajectoires gardent la même tendance horizontale. De plus, les bruits dans l’image ne sont guère amplifiés lors
de la commande : l’écart à la trajectoire idéale est de l’ordre du niveau de bruit.
28
En ce qui concerne les trajectoires spatiales, on voit apparaître une déviation entre la trajectoire idéale et
la trajectoire quasi-idéale, due probablement à des propagations d’erreurs numériques. Néanmoins, il faut
remarquer que cette déviation est inférieure au millimètre pour une trajectoire d’une trentaine de centimètres
(soit moins de 1%), ce qui ne remet pas en cause la validité de l’algorithme d’estimation des variables
articulaires. On remarque également que malgré des niveaux de bruit élevés dans l’image, les perturbations des
trajectoires spatiales restent très limitées (également de l’ordre du millimètre).
Translation selon l’axe vertical de l’image
Le deuxième test présenté ici consiste en une translation du robot Centaure selon l’axe vertical de la
caméra déportée. La trajectoire dans l’image de chaque point de la mire qui résulte de ce mouvement spatial doit
être un segment vertical. Nous avons également tracé les trajectoires dans l’image (Figure 17) ainsi que les
trajectoires spatiales (Figure 18) de ce déplacement. Nous constatons des effets similaires à ceux observés dans
le test précédent : bon comportement dans l’image et déviation inférieure à 1%.
Figure 17 : Trajectoire des points de la mire dans l'image
pour une translation demandée selon l'axe vertical de la caméra
29
Figure 18 : Trajectoire spatiale pour une translation demandée le long de l’axe vertical de la caméra
Translation selon l’axe optique
Le troisième test présenté ici consiste en une translation du robot Centaure selon l’axe optique de la
caméra déportée. La trajectoire dans l’image de chaque point de la mire qui résulte de ce mouvement spatial doit
être un segment porté par le faisceau des droites convergeant vers le centre optique (point de fuite). Nous avons
également tracé les trajectoires dans l’image (Figure 19) ainsi que les trajectoires spatiales (Figure 20) de ce
déplacement. Nous obtenons également un bon comportement dans l’image et la déviation spatiale est
extrêmement faible (de l’ordre de 0,1%).
pour une translation demandée selon l'axe optique de la caméra
30
Figure 20 : Trajectoire spatiale pour une translation demandée le long de l’axe optique de la caméra
Rotation selon l’axe optique
Le dernier test présenté ici consiste en une translation du robot Centaure autour l’axe optique de la caméra
déportée. La trajectoire dans l’image de chaque point de la mire qui résulte de ce mouvement spatial doit être un
cercle centré sur le centre optique (point fixe). Nous avons également tracé les trajectoires dans l’image (Figure
21) ainsi que les trajectoires spatiales (Figure 22) de ce déplacement. Nous obtenons également un bon
comportement dans l’image et la déviation spatiale est négligeable.
pour une rotation demandée autour l'axe optique de la caméra
31
Figure 22: Trajectoire spatiale pour une rotation demandée autour de l’axe optique de la caméra
Conclusion
Nous avons proposé ici une méthode pour réaliser la commande cartésienne d’un robot non-instrumenté.
Elle se base sur une estimation des variables articulaires par observation de l’effecteur par une caméra déportée.
Cette estimation se fait en calculant par vision la pose de l’effecteur du robot par rapport à la caméra déportée,
puis par la résolution numérique du modèle géométrique inverse du robot. La commande manuelle a été testée
avec succès, hors configuration singulière, en simulation aussi bien sur le robot Motoman que sur le robot
Centaure (pour autant que les modèles utilisés en simulation correspondent effectivement à sa géométrie). Elle
semble donc fonctionner aussi bien lorsque la commande manuelle se fait en donnant une consigne de vitesse
cartésienne (torseur cinématique) dans le repère de l’effecteur que lorsqu’elle se fait dans le repère de la caméra
déportée.
Les limites de la méthode proposée se rencontrent au voisinage des singularités du robot. En effet, dans ce
cas, le calcul générique du modèle géométrique inverse par inversion ne converge pas plus que la commande.
Pour pallier cet inconvénient, il faudrait se tourner vers une méthode plus formelle nécessitant une connaissance
plus conséquente du mécanisme du robot afin d’en déterminer algébriquement toutes les solutions et choisir
parmi celles-ci la plus plausible. Néanmoins, le gain d’un tel investissement est faible pour deux raisons.
D’abord, il nécessite une nouvelle étude pour chaque nouveau robot. Ensuite (et peut-être surtout) il ne
résoudrait au final que le problème de l’estimation des variables articulaires et tomberait malgré cela sur le
problème ardu (et indépendant de l’instrumentation du robot !) de la traversée de singularités en commande
cartésienne.. La solution la plus pragmatique à ce problème dans le cas d’un robot télé-opéré est de s’en remettre
à l’opérateur pour basculer en commande articulaire et sortir le robot de la singularité.
32
V. Positionnement automatique par asservissement visuel
A – Introduction à la commande référencée vision
Depuis les années 70, la commande de robot par vision a suscité l’intérêt de bon nombre de
chercheurs. L’évolution technologique et scientifique cette dernière décade permet
d’envisager de réaliser des boucles de commande pour le positionnement de robots par retour
visuel en temps réel vidéo (soit de l’ordre de 40 à 80ms en classique, jusqu’à 1ms si l’on
utilise des composants spécialisés type ASIC). Pour le lecteur souhaitant approfondir le sujet,
nous lui recommandons la lecture des articles de WEISS du début des années 80, les thèses de
F. Chaumette en 1990, et P. Corke en 1993, un tutorial réalisé par Corke, Hager, et
Hutchinson en 1996, les rapports d’Habilitation à Diriger des Recherches de Chaumette en 98
et Martinet en 99, et enfin le site WEB du groupe d’intérêt en asservissement visuel de
EURON (on trouvera notamment vidéo, cours, articles, et une TOOLBOX Matlab/Simulink
d’une école d’été dédiée à l’asservissement visuel ayant eue lieu en 2002).
Ce qu’il est essentiel de retenir pour le projet courant, c’est :
- qu’il est possible d’asservir un robot à partir d’une caméra
o embarquée sur l’effecteur d’un robot (schéma le plus classique)
o déportée (fixe par rapport aux mouvements du robot)
- qu’il est possible d’utiliser
o des informations directement extraites de l’image appelées informations 2D
(Dans ce cas, on parle d’asservissement visuel 2D (Image Based Visual Servoing))
o des informations 3D reconstruites à partir d’informations 2D et d’un modèle
(Dans ce cas, on parle d’asservissement visuel 3D (Position Based Visual
Servoing))
o des informations à la fois 2D et 3D
(Dans ce cas, on parle d’asservissement visuel 2D/3D (Hybrid Based Visual
Servoing))
Dans la plupart des cas, l’approche « fonction de tâche », introduite par Claude Samson et
Bernard Espiau à la fin des années 80, est retenue pour synthétiser la loi commande. En
quelque lignes et équations nous pouvons la décrire.
Considérons s =(s1, s2, …., sk)T, le vecteur des informations capteurs (2D, 3D, 2D/3D).
On forme une fonction d’erreur appelée e par la relation suivante :
(
e=C⋅ s−s
*
)
où
-
s
informations capteurs obtenues à chaque itération
33
-
s*
informations capteurs obtenues à l’équilibre (en position désirée)
C
est une matrice de combinaison qui permet de prendre en compte plus
d’informations capteurs que nécessaire à la bonne exécution de la tâche.
On souhaite comme comportement, une décroissance exponentielle de la fonction de tâche e,
soit :
e& = −λ ⋅ e
Par ailleurs, on sait que s = s r (t ), t
(
) où r(t) représente la pose spatiale du repère associé
au système capteur.
Par conséquent, on peut écrire :
s& =
δ s dr δ s
⋅ +
δ r dt δt
où le premier terme est le produit entre la variation des signaux capteurs par rapport
aux mouvements du système capteur par le torseur cinématique qui paramètre ce mouvement,
et le second terme est la contribution du mouvement propre de l’objet que l’on observe (à
partir duquel on mesure s). Dans la plupart des cas, la cible ou objet observé est fixe : ce
second terme s’annule. Il vient que :
s& =
δ s dr
⋅ = L ⋅T
δ r dt
avec T : torseur cinématique et L matrice d’interaction associées aux informations
capteurs s
En considérant les conditions de convergence d’un tel système, on peut montrer qu’il suffit de
prendre comme loi de commande
(
T = −λ ⋅ L+ ⋅ s − s
*
)
afin de faire converger le système depuis une information capteur s vers une information
capteur s*.
Cette loi de commande s’applique quelquesoit la forme de s (2D, 3D, 2D/3D). Par contre, la
dimension de s est forcément supérieure à 6 dés lors que l’on souhaite commander les 6
degrés de liberté. Dans l’expression de la commande, L+ représente la pseudo inverse de L
dans le cas ou la dimension de s est supérieure à 6.
Les différentes matrices d’interaction utilisables se retrouve dans la littérature précitée.
34
B – Utilisation de l’asservissement visuel pour le positionnement précis de l’effecteur
L’asservissement visuel peut-être utilisé pour le positionnement précis de l’effecteur à partir
d’une caméra embarquée sur ce même effecteur : en effet, positionner la caméra est
équivalent à positionner l’effecteur (une liaison rigide existant entre les deux). Le schéma
d’asservissement suivant illustre le cas général de l’asservissement visuel 2D:
T = −λ .LTS .( S (t ) − S *)
+
S*
q&
T
-
J-1
Control
+
P
C
ll
S (t )
Joint feedback
f (t )
Features
Visual feedback
Extraction
Sensor of 2D features
Le schéma d’asservissement suivant illustre le cas général de l’asservissement visuel 3D:
T = −λ.LTS .(S (t ) − S *)
+
S*
T
-
Control
+
q&
J-1
P
C
ll
S (t )
Joint feedback
Pose
f (t )
Estimation
Features
Visual feedback
Extraction
Sensor of 3D features
Dans ces deux schémas, le retour articulaire peut être présent ou pas (cf Weiss). Dans le cas
où il n’est pas présent, la commande en vitesse articulaire attaque directement les actionneurs.
Dans ce cas, le moindre bruit présent sur l’entrée de l’étage de puissance se transforme en
35
mouvement parasite sur l’axe articulaire. Le retour visuel est alors indispensable, si on ne veut
pas subir ces effets indésirables de dérive.
De nombreuses études ont déjà démontrée la validité de ces principes (cf Ecole d’été en
Asservissement visuel).
C – Positionnement précis de l’effecteur du Motoman ou Centaure
Dans l’environnement MATLAB/Simulink, ont été développés deux schémas de commande :
9 Le premier permet de commander le robot à partir d’informations extraites de la
caméra embarquée
9 Le second permet de commander le robot à partir d’informations extraites de la
caméra déportée
Les deux algorithmes de commande utilisent des points 3D comme mesures dans la boucle de
retour visuel. L’algorithme de Dementhon est utilisé pour reconstruire ces points 3D à chaque
itération.
C-1 Caméra embarquée
Dans simulink, c’est le fichier IBVS3DpEmbarquee.mdl qui représente le schéma
suivant.
36
C-2 Caméra déportée
Dans simulink, c’est le fichier IBVS3DpDeportee.mdl qui représente le schéma
suivant :
C-3 Résultats obtenus en simulation pour le positionnement de l’effecteur
Dans les deux cas qui suivent, l’estimation des q par la méthode présentée dans la partie III
est utilisée dans le but d’évaluer la Jacobienne inverse du robot.
Caméra embarquée sur Motoman: exemple de convergence à zéro du torseur
cinématique (gauche) et des erreurs sur les signaux capteurs (droite) :
37
Caméra déportée sur Motoman : exemple de convergence à zéro du torseur
cinématique (gauche) et des erreurs sur les signaux capteurs (droite) :
D – Conclusion
La faisabilité de l’asservissement visuel pour le positionnement automatique d’outil est
validée d’un point de vue simulation en intégrant les modèles du robot du LASMEA (le
MOTOMAN) et celui du CEA (le CENTAURE). L’algorithme d’estimation des q a été
intégré dans les schémas simulink.
38
VI. Intégration
La dernière étape du projet (phase 3) consiste en la validation sur le robot CENTAURE des
principes énoncés dans cette étude de faisabilité :
- estimation des q par une caméra déportée
- commande cartésienne XYZ du robot
- positionnement de l’effecteur du robot (quelquesoit la tache à réaliser, une fois
positionné). La tâche est un positionnement par rapport à une mire.
Le travail d’intégration est partagé entre le CEA et le LASMEA. Le LASMEA a préparé les
validations sur son robot en interne : le MOTOMAN. Le CEA instrumente le robot
CENTAURE afin d’accueillir le système de vision et commande du LASMEA, valide les
modèles développés pour le CENTAURE, rend commandable en vitesse le CENTAURE
A – Intégration sur le robot MOTOMAN
A-1 Architecture matérielle et logicielle
L’architecture matérielle mise en place pour les essais sur le robot Motoman est la suivante :
Armoire
De
commande
Vidéo
Vidéo
ana
Num.
PCBN
Commande
Bas niveau
Communication
Ethernet
Par socket
PCV
Vision
Haut niveau
Le PC de commande bas niveau assure la commande en temps réel du robot. Le PC vision fait
l’acquisition des images des caméras déportée et embarquée, réalise l’estimation des variables
articulaire (en vue de l’estimation de la jacobienne du robot), réalise la commande
cartésienne, et assure le positionnement de l’effecteur au moyen d’un retour visuel (cf partie
39
précédente).
Les deux PC sont connectés par une liaison Ethernet avec une communication par Socket. Les
échangent portent sur :
- l’envoi des commandes en vitesses articulaires au PC de bas niveau (PCV Î PCBN)
- l’envoi des mesures articulaires au PC de vision (PCBN Î PCV)
L’architecture logicielle est réalisée autour de la connexion MATLAB/Simulink avec C/C++.
Cet environnement a été mis en place pour cette manipulation et a nécessité beaucoup de
développement. Elle a l’avantage de pouvoir récupérer les schémas de commande réalisés
sous MATLAB/SIMULINK, mais le désavantage de ne pas permettre la gestion du temps, si
elle n’est pas associé à un système temps réel dur. Pour cette étude, compte tenu que les temps
de boucle sont très importants, cet inconvénient est mineur.
A-2 Etapes préalables indispensables
Ces étapes préalables consistent principalement en l’étalonnage de la manipulation complète :
Pour effectuer l’estimation des variables articulaires q, il est nécessaire de calculer à chaque
itération la position de l’effecteur dans le repère de base du robot. Ainsi, pour chaque
position, on peut utiliser la procédure de calcul du MGI numérique présentée dans le rapport.
Ce calcul de position de l’effecteur repose sur :
- la connaissance de la matrice de transformation homogène entre le repère caméra
déportée Rcd et le repère de base du robot R0,
40
- la connaissance de la matrice de transformation homogène entre le repère associé à la
mire Rmire et le repère associé à l’effecteur du robot R6
- la mesure des points de la mire dans l’image
- la reconstruction des points 3D de la mire à partir du modèle de la mire
Pour effectuer le positionnement de l’effecteur par retour visuel, il est nécessaire de connaître
la matrice de transformation homogène entre le repère associé à la caméra embarquée et celui
associé à l’effecteur du robot R6.
Une vingtaine d’images sont prises pour évaluer les différentes matrices de transformations
homogènes.
A-3 Estimation des q
Pour évaluer les performances de l’algorithme d’estimation des q, nous avons utilisés les 20
images prises pour l’étalonnage de la manipulation ainsi que les mesures des positions
articulaires disponibles sur ce robot. A Partir de ces 20 images, et à partir du résultat de
l’étalonnage précédent, nous avons calculé les positions de l’effecteur correspondantes à
chacune des positions articulaires. Nous initialisons l’algorithme avec des positions
articulaires quelconques (environ à ±0.5rd sur chacune des articulations). Le principal intérêt
est de travailler avec des données réelles, mais aussi avec les imperfections amenées par
l’étalonnage de la manipulation. A chaque position de l’effecteur, on calcule les valeurs des
variables articulaires. La figure suivante montre les résultats sur les erreurs (en degrés)
commises durant cette estimation (entre qestimée et qréel_mesurée).
41
Les grands écarts constatés proviennent des problèmes de convergence le l’algorithme
d’estimation des q au voisinage de singularités du robot. Néanmoins, ces grands écarts n’ont
que peu d’influence sur l’estimation de la jacobienne (le cœur de l’algorithme de la
commande cartésienne). Ainsi, afin de valider cette estimation, nous avons comparé terme à
terme les valeurs des éléments des jacobiennes estimées et celles supposées vraies : un écart
de l’ordre de 0.05 à été constaté au maximum (ce qui est rassurant compte tenu des erreurs
potentielles commises dans chaque étape).
Afin de valider théoriquement la boucle de positionnement par retour visuel (également pour
la commande cartésienne en boucle ouverte), il est important de valider si le produit
−1
J est
(q) ⋅ J (q) reste bien définit positif afin de ne pas faire diverger la boucle d’asservissement
visuel. Pour cela nous avons étudié les valeurs propres de la quantité JT.Jest. Les résultats sont
donnés dans la figure suivante (un des pires cas):
Le comportement global est donné par les 3 premières valeurs propres. Sur cette simulation en
particulier, la sixième valeur propre pour 1 position sur 20 intervient. Dans 5 positions sur 20,
la quatrième valeur propre devient négative mais avec une valeur faible (n’empêchant pas le
système de converger).
Cette validation est intéressante et nous montre que l’on peut obtenir une bonne estimation de
42
la jacobienne malgré des estimations articulaires apparemment mauvaises. Au voisinage
d’une singularité, on ne peut pas empêcher ce problème de survenir.
A-4 Commande cartésienne
La commande cartésienne consiste à donner des vitesses de déplacements exprimées dans un
repère donné. Pour le projet, 2 types de repères ont été définis :
- le repère de la caméra déportée
- le repère de l’effecteur
La figure suivante montre l’interface simulink et l’image qui apparaît sur l’écran de
l’opérateur :
Deux schémas Simulink ont été développés en place à cet effet. L’implantation de la
commande n’est pas optimisée en temps pour les raisons indiquées dans la partie A-1. Comme
cette étude est une étude de faisabilité, ce défaut est mineur. Un interface avec curseur est mis
en place pour choisir soit :
- une translation en X dans le repère considéré
- une translation en Y dans le repère considéré
- une translation en Z dans le repère considéré
- une rotation en X dans le repère considéré
- une rotation en Y dans le repère considéré
- une rotation en Z dans le repère considéré
Des vidéos des déplacements ont été faites pour le cas des déplacements exprimés dans le
repère de l’effecteur.
Repère caméra déportée - A partir des valeurs des variables articulaires vraies (mesurées par
les capteurs angulaires du robot) et du modèle géométrique direct du robot, nous avons
reconstitué la trajectoire du robot dans le repère de base. Les trois mouvements de translation
sont validés (TX-,TX+,TX-,TY-,TY-,TZ-) dans le repère de la caméra déportée.
43
A - Trajectoire dans le plan XY
B - Trajectoire dans le plan YZ
C - Trajectoire dans le plan XZ
44
On peut remarquer qu’il existe du couplage entre certains axes. Cela s’explique par
l’étalonnage qui est loin d’être parfait, et les modèles que l’on utilise qui également sont des
modèles théoriques.
Le second point qui est intéressant c’est que comme attendu en faisant par exemple TX+ suivi
de TX- on ne suit pas exactement la même trajectoire. Il est clair que théoriquement il est
impossible de faire en sorte d’avoir une répétabilité de ce type de commande, car le
fonctionnement est en boucle ouverte.
Repère effecteur – Comme précédemment la trajectoire du robot est reconstruite dans le
repère de base. Les trois mouvements de translation sont validés.
A - Trajectoire dans le plan XY
B - Trajectoire dans le plan YZ
45
C - Trajectoire dans le plan XZ
On peut remarquer qu’il existe du couplage entre certains axes. Cela s’explique par
l’étalonnage qui est loin d’être parfait, et les modèles que l’on utilise qui également sont des
modèles théoriques.
A-5 Positionnement de l’effecteur
La dernière phase d’intégration sur le robot Motoman porte sur la commande de
positionnement de l’effecteur. Ce qui nous intéresse bien sur, c’est d’assurer un
positionnement précis de l’effecteur du robot par rapport à un objet particulier. Pour valider
cette phase, nous avons pris le parti de décomposer ce travail en deux étapes :
- validation du positionnement par asservissement visuel utilisant la caméra déportée.
Cette validation est quasi immédiate a réalisée avec les schémas développés
précédemment.
- validation du positionnement par asservissement visuel utilisant une caméra
embarquée. Cette étape est par contre plus délicate, car il faut intégrer deux caméras
dans notre environnement. A cette heure, nous avons rencontré des difficultés liées à
l’outil simulink que nous n’avons pas encore surmontées.
AV avec caméra déportée.
A partir du schéma Simulink réalisée pour la commande cartésienne exprimée dans le repère
de la caméra déportée, il est quasi immédiat de remplacer l’envoi de la commande cartésienne
par celle issue d’un calcul de loi de commande type asservissement visuel de points 3D
directement extraits depuis la caméra déportée.
46
B – Intégration sur le robot CENTAURE
B-1 Architecture matériel et logiciel
Idem Motoman. A faire
B-2 Etapes préalables indispensables
B-3 Estimation des q
A porter et valider
B-4 Commande cartésienne
A porter et valider
B-5 Positionnement de l’effecteur
A porter et valider
47
pour l’intégration de la commande conditionnera les performances : si le temps de
rebouclage est court, alors les imprécisions pourront être corrigées en temps réel et de
ce simple fait la précision de l’asservissement en sera améliorée. Ne s’agissant que
d’une étude pour démontrer l’intérêt des approches, il semble que les idées mises en
avant sont validées.
9 Avancement dans le projet : L’ensemble de ces résultats valident complètement les
phases 1 et 2 du projet et contribuent très largement à l’avancement de la phase 3.
9 Problèmes rencontrés durant la phase d’intégration. Plusieurs problèmes ont
retardés l’obtention de résultats sur le bras Motoman :
o Nouveau robot (modèles non encore validés, architecture matérielle et
logicielle)
o Nouveaux outils avec couplage matlab/simulink avec C/C++ (jamais fait
auparavant) ;
o Robots avec beaucoup de cas de singularités (c’est le cas des 2 robots) ce qui
occasionne l’adressage de quasiment tous les problèmes rencontrés en
robotique,… Il n’a pas à ce jour de problèmes rencontrés remettant en cause la
méthode (sauf les cas classiques rencontrés en robotique où 2 solutions ou plus
se présente pour la résolution du problème inverse (XÎ q ?).
o Nécessité d’une installation fiable : les inconvénients majeurs semblent être la
nécessité d’un étalonnage pas trop mauvais, ce qui n’est pour l’instant pas
quantifiable facilement.
o Caméra FireWire intégrée mais lente (3 im/s), caméra analogique non intégrée
(problème sur les drivers ‘shareware’).
9 Solutions à envisager sur le bras CENTAURE (à rediscuter pour la mise en œuvre) :
o Avoir des modèles validés
o Prendre en compte des couplages d’axes dans le bas niveau
o Installer une caméra sur la base de manière définitive (cf Problème
d’étalonnage)
o Avoir une mire installée à demeure sur l’effecteur (cf pb d’étalonnage)
o Avoir la communication par socket opérationnelle
o Prévoir l’étalonnage de la manipulation
o Faire une démonstration finale
o Pour l’ensemble, il nous parait indispensable que le robot CENTAURE vienne
à Clermont-Ferrand avec une personne du CEA pour 1 à 2 semaines. Un seul
déplacement au CEA est prévu dans la prestation.
48
B – Intégration sur le robot CENTAURE
B-1 Architecture matériel et logiciel
B-2 Etapes préalables indispensables
B-3 Estimation des q
A porter et valider
B-4 Commande cartésienne
A porter et valider
B-5 Positionnement de l’effecteur
A porter et valider
49
VII. Conclusion, perspectives
A l’issue de l’étude, il s’avère que la faisabilité de
• l’estimation des variables articulaires par observation de
l’effecteur muni d’une mire
• la commande cartésienne utilisant l’estimation des variables
articulaires dans la matrice jacobienne
• l’asservissement visuel (utilisant l’estimation des variables
articulaires dans la matrice jacobienne) pour réaliser des tâches
de positionnement (i.e. : pour la prise d’outils, pour l’ouverture
d’un sas)
ont été validées par simulation (cf résultats dans ce rapport
intermédiaire), et en réel sur le bras Motoman au LASMEA. Des
essais en réel ont été montrés au CEA lors de la visite du 5 mars 2004.
Trois questions restées en suspens sont abordées en annexe de ce
rapport :
• utilisation d’une mire cylindrique
• utilisation de plusieurs caméras pour couvrir un espace de travail
plus grand
• validation du principe de positionnement par vision pour
l’ouverture de sas.
et fournissent des éléments de réflexions théoriques quant à leur
réalisation.
L’intégration finale se fera sur le CENTAURE au LASMEA :
• Validation des modèles du robot
• Validation de la communication entre robot et PC vision
• Installation des caméras déportée et embarquée
• Positionnement d’une mire plane sur l’effecteur
• Etalonnage (grossier) robot/Caméra et mire/robot
• Validation de la commande cartésienne
• Validation du positionnement par asservissement visuel avec mire
50
Annexes
Fichiers : TDH.m, mgdDHsym.m, et mgdDHnum.m
Fichiers : MGD_CENTAURE.m
Fichier : visuDH.m
Fichiers : JacobtotalsymDH.m et JacobtotalnumDH.m
Fichiers : PINV_SVD.m et MCD_CENTAURE.m
Eléments de réponse aux questions en suspend.
51
Fichiers : TDH.m, mgdDHsym.m, et mgdDHnum.m
Fichier : TDH.m
Calcul la matrice de transformation homogène associée au paramétrage DH.
function [A] = TDH(p)
A=[
cos(p(3)) , -sin(p(3))
, 0
, p(2)
;
cos(p(1))*sin(p(3)), cos(p(1))*cos(p(3)), -sin(p(1)) , -p(4)*sin(p(1));
sin(p(1))*sin(p(3)), sin(p(1))*cos(p(3)), cos(p(1)) , p(4)*cos(p(1));
0
,
0
, 0 , 1
];
Fichier : mgdDHsym.m
Calcul la matrice de transformation homogène associée au paramétrage DH pour un robot
6ddl
function [A] = mgdDHsym(phi10,phi21,phi32,phi43,phi54,phi65)
format short
%introduction de variables symboliques
%angles phi
phi10 = sym('phi10');
%paramétres alpha
alpha1 = sym('alpha1');
%paramètres d
d1 = sym('d1');
d2 = sym('d2');
d3 = sym('d3');
d4 = sym('d4');
d5 = sym('d5');
d6 = sym('d6');
%paramètres theta
theta01 = sym('theta1');
52
%paramètres r
r1 = sym('r1');
r2 = sym('r2');
r3 = sym('r3');
r4 = sym('r4');
r5 = sym('r5');
r6 = sym('r6');
%def des q symboliques
q1 = phi10+theta01;
q2 = phi21+theta02;
q3 = phi32+theta03;
q4 = phi43+theta04;
q5 = phi54+theta05;
q6 = phi65+theta06;
%def des constantes
%introduction du modéle du robot
robot = centaure
Alpha1 = robot.Para_joint1(1);
D1 = robot.Para_joint1(2);
Theta01 = robot.Para_joint1(3);
R1 = robot.Para_joint1(4);
53
% calcul des matrices de passage symboliques
T01 = TDH(alpha1,d1,q1,r1);
T46 = T45*T56;
simplify(T46);
T36 = T34*T46;
T26 = T23*T36;
T16 = T12*T26;
T06sym = T01*T16;% obtention du modèle symbolique paramètré
%substitution des valeurs aux paramétres
T06=subs(T06sym,{Alpha1,Alpha2,Alpha3,Alpha4,Alpha5,Alpha6,Theta01,Theta02,Theta03
,Theta04,Theta05,Theta06,D1,D2,D3,D4,D5,D6,R1,R2,R3,R4,R5,R6},{alpha1,alpha2,alpha3
,alpha4,alpha5,alpha6,theta01,theta02,theta03,theta04,theta05,theta06,d1,d2,d3,d4,d5,d6,r1,r2
,r3,r4,r5,r6});
%obtention du modele ne dependant que des phi
sx = T06(1,1)
sy = T06(2,1)
sz = T06(3,1)
nx = T06(1,2)
ny = T06(2,2)
nz = T06(3,2)
ax = T06(1,3)
ay = T06(2,3)
az = T06(3,3)
Px = T06(1,4)
Py = T06(2,4)
Pz = T06(3,4)
Fichier : mgdDHnum.m
Calcul la matrice de transformation homogène associée au paramétrage DH pour un robot
6ddl
function [A] = mgdDHnum(q)
phi10=q(1);
phi21=q(2);
phi32=q(3)-phi21;%couplage des liaisons 2 et 3
phi43=q(4);
phi54=q(5);
54
phi65=q(6);
sx
=
cos(phi10)*(cos(phi21)*(cos(phi32)*(cos(phi43)*cos(phi54)*cos(phi65)sin(phi43)*sin(phi65))-sin(phi32)*sin(phi54)*cos(phi65))sin(phi21)*(sin(phi32)*(cos(phi43)*cos(phi54)*cos(phi65)sin(phi43)*sin(phi65))+cos(phi32)*sin(phi54)*cos(phi65)))sin(phi10)*(sin(phi43)*cos(phi54)*cos(phi65)+cos(phi43)*sin(phi65));
sy
=
sin(phi10)*(cos(phi21)*(cos(phi32)*(cos(phi43)*cos(phi54)*cos(phi65)sin(phi43)*sin(phi65))-sin(phi32)*sin(phi54)*cos(phi65))sin(phi21)*(sin(phi32)*(cos(phi43)*cos(phi54)*cos(phi65)sin(phi43)*sin(phi65))+cos(phi32)*sin(phi54)*cos(phi65)))+cos(phi10)*(sin(phi43)*cos(phi5
4)*cos(phi65)+cos(phi43)*sin(phi65));
sz = sin(phi21)*(cos(phi32)*(cos(phi43)*cos(phi54)*cos(phi65)-sin(phi43)*sin(phi65))sin(phi32)*sin(phi54)*cos(phi65))+cos(phi21)*(sin(phi32)*(cos(phi43)*cos(phi54)*cos(phi6
5)-sin(phi43)*sin(phi65))+cos(phi32)*sin(phi54)*cos(phi65));
nx
=
cos(phi10)*(cos(phi21)*(cos(phi32)*(-cos(phi43)*cos(phi54)*sin(phi65)sin(phi43)*cos(phi65))+sin(phi32)*sin(phi54)*sin(phi65))-sin(phi21)*(sin(phi32)*(cos(phi43)*cos(phi54)*sin(phi65)-sin(phi43)*cos(phi65))cos(phi32)*sin(phi54)*sin(phi65)))-sin(phi10)*(sin(phi43)*cos(phi54)*sin(phi65)+cos(phi43)*cos(phi65));
ny
=
sin(phi10)*(cos(phi21)*(cos(phi32)*(-cos(phi43)*cos(phi54)*sin(phi65)sin(phi43)*cos(phi65))+sin(phi32)*sin(phi54)*sin(phi65))-sin(phi21)*(sin(phi32)*(cos(phi43)*cos(phi54)*sin(phi65)-sin(phi43)*cos(phi65))cos(phi32)*sin(phi54)*sin(phi65)))+cos(phi10)*(sin(phi43)*cos(phi54)*sin(phi65)+cos(phi43)*cos(phi65));
nz
=
sin(phi21)*(cos(phi32)*(-cos(phi43)*cos(phi54)*sin(phi65)sin(phi43)*cos(phi65))+sin(phi32)*sin(phi54)*sin(phi65))+cos(phi21)*(sin(phi32)*(cos(phi43)*cos(phi54)*sin(phi65)-sin(phi43)*cos(phi65))-cos(phi32)*sin(phi54)*sin(phi65));
ax = cos(phi10)*(cos(phi21)*(-cos(phi32)*cos(phi43)*sin(phi54)-sin(phi32)*cos(phi54))sin(phi21)*(sin(phi32)*cos(phi43)*sin(phi54)+cos(phi32)*cos(phi54)))+sin(phi10)*sin(phi43)*sin(phi54)
;
ay = sin(phi10)*(cos(phi21)*(-cos(phi32)*cos(phi43)*sin(phi54)-sin(phi32)*cos(phi54))sin(phi21)*(-sin(phi32)*cos(phi43)*sin(phi54)+cos(phi32)*cos(phi54)))cos(phi10)*sin(phi43)*sin(phi54);
az = sin(phi21)*(-cos(phi32)*cos(phi43)*sin(phi54)-sin(phi32)*cos(phi54))+cos(phi21)*(sin(phi32)*cos(phi43)*sin(phi54)+cos(phi32)*cos(phi54));
Px
=
cos(phi10)*(cos(phi21)*(-350*sin(phi32)+300)350*sin(phi21)*cos(phi32))+169/2*sin(phi10);
Py
=
sin(phi10)*(cos(phi21)*(-350*sin(phi32)+300)-350*sin(phi21)*cos(phi32))169/2*cos(phi10);
Pz = sin(phi21)*(-350*sin(phi32)+300)+350*cos(phi21)*cos(phi32);
A = [sx,nx,ax,Px;
sy,ny,ay,Py;
sz,nz,az,Pz;
0 ,0 ,0 ,1 ];
55
Fichier : visuDH.m
Fichier : visuDH.m
Visualisation filaire du robot 6 ddl
function [A] = visuDH(robot,varargin)
% en entree les angles pour la position, sinon, prise des valeurs stockees
% dans la struct du robot (valeurs courantes)
% trace de la representation du robot, repere attache a la base magenta,
% final vert
%def des constantes
alpha1 = robot.Para_joint1(1);
d1 = robot.Para_joint1(2);
theta01 = robot.Para_joint1(3);
r1 = robot.Para_joint1(4);
if nargin < 2
Phi10 = robot.Valeurs_angles_cur(1);
else
56
Phi10=varargin{1};
Phi21=varargin{2};
Phi32=varargin{3};
Phi43=varargin{4};
Phi54=varargin{5};
Phi65=varargin{6};
end
q1 = Phi10+theta01;
q2 = Phi21+theta02;
q3 = Phi32+theta03-Phi21;
q4 = Phi43+theta04;
q5 = Phi54+theta05;
q6 = Phi65+theta06;
T02 = T01*T12;
T021 = T02*[1,0,0,robot.Para_joint3(2);
0,1,0,0;
0,0,1,0;
0,0,0,1];
T022 = T021*[1,0,0,0;
0,1,0,0;
0,0,1,robot.Para_joint3(4);
0,0,0,1];
0,1,0,0;
0,0,0,1];
T03 = T02*T23;
0,1,0,0;
0,0,0,1];
T04 = T03*T34;
T041=T04*[1,0,0,robot.Para_joint5(2);
0,1,0,0;
0,0,0,1];
57
T05 = T04*T45;
T06 = T01*T12*T23*T34*T45*T56;
clear All
axis manual
axis([-400 400 -400 400 -400 400]);
line(30,30,30)
plot3(0,0,0,'b:o')
hold on
plot3([0;50],[0;0],[0;0],'m-')
plot3([0;0],[0;50],[0;0],'m-')
plot3([0;0],[0;0],[0;50],'b-')
plot3([T06(1,4);50*T06(1,1)+T06(1,4)],[T06(2,4);50*T06(2,1)+T06(2,4)],[T06(3,4);50*T06(
3,1)+T06(3,4)],'g-')
plot3([T06(1,4);50*T06(1,2)+T06(1,4)],[T06(2,4);50*T06(2,2)+T06(2,4)],[T06(3,4);50*T06(
3,2)+T06(3,4)],'g-')
plot3([T06(1,4);50*T06(1,3)+T06(1,4)],[T06(2,4);50*T06(2,3)+T06(2,4)],[T06(3,4);50*T06(
3,3)+T06(3,4)],'b-')
plot3([T01(1,4);T02(1,4)],[T01(2,4);T02(2,4)],[T01(3,4);T02(3,4)],'r-')
plot3([T02(1,4);T021(1,4)],[T02(2,4);T021(2,4)],[T02(3,4);T021(3,4)],'r-o')
plot3([T021(1,4);T022(1,4)],[T021(2,4);T022(2,4)],[T021(3,4);T022(3,4)],'r-o')
plot3([T022(1,4);T03(1,4)],[T022(2,4);T03(2,4)],[T022(3,4);T03(3,4)],'r-o')
plot3([T03(1,4);T04(1,4)],[T03(2,4);T04(2,4)],[T03(3,4);T04(3,4)],'r-o')
plot3([T04(1,4);T05(1,4)],[T04(2,4);T05(2,4)],[T04(3,4);T05(3,4)],'r-o')
58
Fichier : MGD_CENTAURE.m
Fichier : MGD_CENTAURE.m
MGD du robot CENTAURE
function [A] = MGD_Centaure(theta)
phi10=theta(1);
phi21=theta(2);
phi32=theta(3);
phi43=theta(4);
phi54=theta(5);
phi65=theta(6);
sx
=
cos(phi10)*(cos(phi21)*(cos(phi32)*(cos(phi43)*cos(phi54)*cos(phi65)sin(phi43)*sin(phi65))-sin(phi32)*sin(phi54)*cos(phi65))sin(phi21)*(sin(phi32)*(cos(phi43)*cos(phi54)*cos(phi65)sin(phi43)*sin(phi65))+cos(phi32)*sin(phi54)*cos(phi65)))sin(phi10)*(sin(phi43)*cos(phi54)*cos(phi65)+cos(phi43)*sin(phi65));
sy
=
sin(phi10)*(cos(phi21)*(cos(phi32)*(cos(phi43)*cos(phi54)*cos(phi65)sin(phi43)*sin(phi65))-sin(phi32)*sin(phi54)*cos(phi65))sin(phi21)*(sin(phi32)*(cos(phi43)*cos(phi54)*cos(phi65)sin(phi43)*sin(phi65))+cos(phi32)*sin(phi54)*cos(phi65)))+cos(phi10)*(sin(phi43)*cos(phi5
4)*cos(phi65)+cos(phi43)*sin(phi65));
sz = sin(phi21)*(cos(phi32)*(cos(phi43)*cos(phi54)*cos(phi65)-sin(phi43)*sin(phi65))sin(phi32)*sin(phi54)*cos(phi65))+cos(phi21)*(sin(phi32)*(cos(phi43)*cos(phi54)*cos(phi6
5)-sin(phi43)*sin(phi65))+cos(phi32)*sin(phi54)*cos(phi65));
nx
=
cos(phi10)*(cos(phi21)*(cos(phi32)*(-cos(phi43)*cos(phi54)*sin(phi65)sin(phi43)*cos(phi65))+sin(phi32)*sin(phi54)*sin(phi65))-sin(phi21)*(sin(phi32)*(cos(phi43)*cos(phi54)*sin(phi65)-sin(phi43)*cos(phi65))cos(phi32)*sin(phi54)*sin(phi65)))-sin(phi10)*(sin(phi43)*cos(phi54)*sin(phi65)+cos(phi43)*cos(phi65));
ny
=
sin(phi10)*(cos(phi21)*(cos(phi32)*(-cos(phi43)*cos(phi54)*sin(phi65)sin(phi43)*cos(phi65))+sin(phi32)*sin(phi54)*sin(phi65))-sin(phi21)*(sin(phi32)*(cos(phi43)*cos(phi54)*sin(phi65)-sin(phi43)*cos(phi65))cos(phi32)*sin(phi54)*sin(phi65)))+cos(phi10)*(sin(phi43)*cos(phi54)*sin(phi65)+cos(phi43)*cos(phi65));
nz
=
sin(phi21)*(cos(phi32)*(-cos(phi43)*cos(phi54)*sin(phi65)sin(phi43)*cos(phi65))+sin(phi32)*sin(phi54)*sin(phi65))+cos(phi21)*(sin(phi32)*(cos(phi43)*cos(phi54)*sin(phi65)-sin(phi43)*cos(phi65))-cos(phi32)*sin(phi54)*sin(phi65));
ax = cos(phi10)*(cos(phi21)*(-cos(phi32)*cos(phi43)*sin(phi54)-sin(phi32)*cos(phi54))sin(phi21)*(sin(phi32)*cos(phi43)*sin(phi54)+cos(phi32)*cos(phi54)))+sin(phi10)*sin(phi43)*sin(phi54)
;
ay = sin(phi10)*(cos(phi21)*(-cos(phi32)*cos(phi43)*sin(phi54)-sin(phi32)*cos(phi54))sin(phi21)*(-sin(phi32)*cos(phi43)*sin(phi54)+cos(phi32)*cos(phi54)))cos(phi10)*sin(phi43)*sin(phi54);
az = sin(phi21)*(-cos(phi32)*cos(phi43)*sin(phi54)-sin(phi32)*cos(phi54))+cos(phi21)*(sin(phi32)*cos(phi43)*sin(phi54)+cos(phi32)*cos(phi54));
59
Px
=
cos(phi10)*(cos(phi21)*(-0.35*sin(phi32)+0.3)0.35*sin(phi21)*cos(phi32))+0.0845*sin(phi10);
Py
=
sin(phi10)*(cos(phi21)*(-0.35*sin(phi32)+0.3)-0.35*sin(phi21)*cos(phi32))0.0845*cos(phi10);
Pz = sin(phi21)*(-0.35*sin(phi32)+0.3)+0.35*cos(phi21)*cos(phi32);
A = [sx,nx,ax,Px;
sy,ny,ay,Py;
sz,nz,az,Pz;
0 ,0 ,0 ,1 ];
60
Fichiers : JacobtotalsymDH.m et JacobtotalnumDH.m
Fichier : JacobtotalsymDH.m
Jacobienne du robot à 6 ddl
%obtention de la jacobienne 66 paramètrée
function [A] = JacobtotalsymDH(Phi10,Phi21,Phi32,Phi43,Phi54,Phi65)
format short
%introduction de variables symboliques
%def des constantes
d1 = sym('d1');
d2 = sym('d2');
d3 = sym('d3');
d4 = sym('d4');
d5 = sym('d5');
d6 = sym('d6');
r1 = sym('r1');
r2 = sym('r2');
r3 = sym('r3');
r4 = sym('r4');
r5 = sym('r5');
r6 = sym('r6');
q1 = phi10+theta01;
q2 = phi21+theta02;
q3 = phi32+theta03;
q4 = phi43+theta04;
61
q5 = phi54+theta05;
q6 = phi65+theta06;
T46 = T45*T56;
simplify(T46);
T36 = T34*T46;
T26 = T23*T36;
T16 = T12*T26;
T06 = T01*T16;
%calcul des colonnes de la jacobienne:
j16 = [-T16(1,1)*T16(2,4)+T16(2,1)*T16(1,4);
-T16(1,2)*T16(2,4)+T16(2,2)*T16(1,4);
-T16(1,3)*T16(2,4)+T16(2,3)*T16(1,4);
T16(3,1);
T16(3,2);
T16(3,3)
];
j26 = [-T26(1,1)*T26(2,4)+T26(2,1)*T26(1,4);
-T26(1,2)*T26(2,4)+T26(2,2)*T26(1,4);
-T26(1,3)*T26(2,4)+T26(2,3)*T26(1,4);
T26(3,1);
T26(3,2);
T26(3,3)
];
j36 = [-T36(1,1)*T36(2,4)+T36(2,1)*T36(1,4);
-T36(1,2)*T36(2,4)+T36(2,2)*T36(1,4);
-T36(1,3)*T36(2,4)+T36(2,3)*T36(1,4);
T36(3,1);
T36(3,2);
T36(3,3)
];
62
j46 = [-T46(1,1)*T46(2,4)+T46(2,1)*T46(1,4);
-T46(1,2)*T46(2,4)+T46(2,2)*T46(1,4);
-T46(1,3)*T46(2,4)+T46(2,3)*T46(1,4);
T46(3,1);
T46(3,2);
T46(3,3)
];
j56 = [-T56(1,1)*T56(2,4)+T56(2,1)*T56(1,4);
-T56(1,2)*T56(2,4)+T56(2,2)*T56(1,4);
-T56(1,3)*T56(2,4)+T56(2,3)*T56(1,4);
T56(3,1);
T56(3,2);
T56(3,3)
]
j66 = [ 0 ;
0;
0;
0;
0;
1
];
Fichier : JacobtotalnumDH.m
Jacobienne du robot à 6 ddl
function [A] = JacobtotalnumDH(varargin)
%def des constantes
robot = Centaure;
63
if nargin < 2
phi10 = robot.Valeurs_angles_cur(1);
else
phi10=varargin{1};
phi21=varargin{2};
phi32=varargin{3}-phi21;
phi43=varargin{4};
phi54=varargin{5};
phi65=varargin{6};
end
%attention lignes de calculs tres longues, ne sont pas affichées, ne pas supprimer les lignes
qui paraissent vides
J611=(cos(phi21+theta2)*(cos(phi32+theta3)*(cos(phi43+theta4)*(cos(phi54+theta5)*cos(phi65+th
eta6)-sin(phi54+theta5)*cos(alpha6)*sin(phi65+theta6))sin(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*cos(phi65+theta6)+cos(alpha5)*cos(phi54
+theta5)*cos(alpha6)*sin(phi65+theta6)-sin(alpha5)*sin(alpha6)*sin(phi65+theta6)))sin(phi32+theta3)*(cos(alpha4)*sin(phi43+theta4)*(cos(phi54+theta5)*cos(phi65+theta6)sin(phi54+theta5)*cos(alpha6)*sin(phi65+theta6))+cos(alpha4)*cos(phi43+theta4)*(cos(alph
a5)*sin(phi54+theta5)*cos(phi65+theta6)+cos(alpha5)*cos(phi54+theta5)*cos(alpha6)*sin(p
hi65+theta6)-sin(alpha5)*sin(alpha6)*sin(phi65+theta6))sin(alpha4)*(sin(alpha5)*sin(phi54+theta5)*cos(phi65+theta6)+sin(alpha5)*cos(phi54+theta5
)*cos(alpha6)*sin(phi65+theta6)+cos(alpha5)*sin(alpha6)*sin(phi65+theta6))))+sin(phi21+th
eta2)*(cos(alpha3)*sin(phi32+theta3)*(cos(phi43+theta4)*(cos(phi54+theta5)*cos(phi65+the
ta6)-sin(phi54+theta5)*cos(alpha6)*sin(phi65+theta6))sin(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*cos(phi65+theta6)+cos(alpha5)*cos(phi54
+theta5)*cos(alpha6)*sin(phi65+theta6)sin(alpha5)*sin(alpha6)*sin(phi65+theta6)))+cos(alpha3)*cos(phi32+theta3)*(cos(alpha4)*si
64
n(phi43+theta4)*(cos(phi54+theta5)*cos(phi65+theta6)sin(phi54+theta5)*cos(alpha6)*sin(phi65+theta6))+cos(alpha4)*cos(phi43+theta4)*(cos(alph
)*cos(alpha6)*sin(phi65+theta6)+cos(alpha5)*sin(alpha6)*sin(phi65+theta6)))sin(alpha3)*(sin(alpha4)*sin(phi43+theta4)*(cos(phi54+theta5)*cos(phi65+theta6)sin(phi54+theta5)*cos(alpha6)*sin(phi65+theta6))+sin(alpha4)*cos(phi43+theta4)*(cos(alpha
5)*sin(phi54+theta5)*cos(phi65+theta6)+cos(alpha5)*cos(phi54+theta5)*cos(alpha6)*sin(phi
65+theta6)sin(alpha5)*sin(alpha6)*sin(phi65+theta6))+cos(alpha4)*(sin(alpha5)*sin(phi54+theta5)*cos(
phi65+theta6)+sin(alpha5)*cos(phi54+theta5)*cos(alpha6)*sin(phi65+theta6)+cos(alpha5)*si
n(alpha6)*sin(phi65+theta6)))))*(cos(alpha2)*sin(phi21+theta2)*(cos(phi32+theta3)*(cos(phi
43+theta4)*(cos(phi54+theta5)*d6+sin(phi54+theta5)*r6*sin(alpha6)+d5)sin(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*d6cos(alpha5)*cos(phi54+theta5)*r6*sin(alpha6)-sin(alpha5)*r6*cos(alpha6)r5*sin(alpha5))+d4)sin(phi32+theta3)*(cos(alpha4)*sin(phi43+theta4)*(cos(phi54+theta5)*d6+sin(phi54+theta5)
*r6*sin(alpha6)+d5)+cos(alpha4)*cos(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*d6cos(alpha5)*cos(phi54+theta5)*r6*sin(alpha6)-sin(alpha5)*r6*cos(alpha6)-r5*sin(alpha5))sin(alpha4)*(sin(alpha5)*sin(phi54+theta5)*d6sin(alpha5)*cos(phi54+theta5)*r6*sin(alpha6)+cos(alpha5)*r6*cos(alpha6)+r5*cos(alpha5))r4*sin(alpha4))+d3)+cos(alpha2)*cos(phi21+theta2)*(cos(alpha3)*sin(phi32+theta3)*(cos(ph
i43+theta4)*(cos(phi54+theta5)*d6+sin(phi54+theta5)*r6*sin(alpha6)+d5)sin(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*d6cos(alpha5)*cos(phi54+theta5)*r6*sin(alpha6)-sin(alpha5)*r6*cos(alpha6)r5*sin(alpha5))+d4)+cos(alpha3)*cos(phi32+theta3)*(cos(alpha4)*sin(phi43+theta4)*(cos(ph
i54+theta5)*d6+sin(phi54+theta5)*r6*sin(alpha6)+d5)+cos(alpha4)*cos(phi43+theta4)*(cos(
alpha5)*sin(phi54+theta5)*d6-cos(alpha5)*cos(phi54+theta5)*r6*sin(alpha6)sin(alpha5)*r6*cos(alpha6)-r5*sin(alpha5))-sin(alpha4)*(sin(alpha5)*sin(phi54+theta5)*d6sin(alpha5)*cos(phi54+theta5)*r6*sin(alpha6)+cos(alpha5)*r6*cos(alpha6)+r5*cos(alpha5))r4*sin(alpha4))sin(alpha3)*(sin(alpha4)*sin(phi43+theta4)*(cos(phi54+theta5)*d6+sin(phi54+theta5)*r6*sin
(alpha6)+d5)+sin(alpha4)*cos(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*d6cos(alpha5)*cos(phi54+theta5)*r6*sin(alpha6)-sin(alpha5)*r6*cos(alpha6)r5*sin(alpha5))+cos(alpha4)*(sin(alpha5)*sin(phi54+theta5)*d6sin(alpha5)*cos(phi54+theta5)*r6*sin(alpha6)+cos(alpha5)*r6*cos(alpha6)+r5*cos(alpha5))+
r4*cos(alpha4))-r3*sin(alpha3))sin(alpha2)*(sin(alpha3)*sin(phi32+theta3)*(cos(phi43+theta4)*(cos(phi54+theta5)*d6+sin(p
hi54+theta5)*r6*sin(alpha6)+d5)-sin(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*d6cos(alpha5)*cos(phi54+theta5)*r6*sin(alpha6)-sin(alpha5)*r6*cos(alpha6)r5*sin(alpha5))+d4)+sin(alpha3)*cos(phi32+theta3)*(cos(alpha4)*sin(phi43+theta4)*(cos(ph
alpha5)*sin(phi54+theta5)*d6-cos(alpha5)*cos(phi54+theta5)*r6*sin(alpha6)sin(alpha5)*r6*cos(alpha6)-r5*sin(alpha5))-sin(alpha4)*(sin(alpha5)*sin(phi54+theta5)*d6sin(alpha5)*cos(phi54+theta5)*r6*sin(alpha6)+cos(alpha5)*r6*cos(alpha6)+r5*cos(alpha5))r4*sin(alpha4))+cos(alpha3)*(sin(alpha4)*sin(phi43+theta4)*(cos(phi54+theta5)*d6+sin(phi
54+theta5)*r6*sin(alpha6)+d5)+sin(alpha4)*cos(phi43+theta4)*(cos(alpha5)*sin(phi54+theta
5)*d6-cos(alpha5)*cos(phi54+theta5)*r6*sin(alpha6)-sin(alpha5)*r6*cos(alpha6)r5*sin(alpha5))+cos(alpha4)*(sin(alpha5)*sin(phi54+theta5)*d6-
65
sin(alpha5)*cos(phi54+theta5)*r6*sin(alpha6)+cos(alpha5)*r6*cos(alpha6)+r5*cos(alpha5))+
r4*cos(alpha4))+r3*cos(alpha3))r2*sin(alpha2))+(cos(alpha2)*sin(phi21+theta2)*(cos(phi32+theta3)*(cos(phi43+theta4)*(cos
(phi54+theta5)*cos(phi65+theta6)-sin(phi54+theta5)*cos(alpha6)*sin(phi65+theta6))sin(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*cos(phi65+theta6)+cos(alpha5)*cos(phi54
)*cos(alpha6)*sin(phi65+theta6)+cos(alpha5)*sin(alpha6)*sin(phi65+theta6))))+cos(alpha2)*
cos(phi21+theta2)*(cos(alpha3)*sin(phi32+theta3)*(cos(phi43+theta4)*(cos(phi54+theta5)*c
os(phi65+theta6)-sin(phi54+theta5)*cos(alpha6)*sin(phi65+theta6))sin(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*cos(phi65+theta6)+cos(alpha5)*cos(phi54
n(alpha6)*sin(phi65+theta6))))sin(alpha2)*(sin(alpha3)*sin(phi32+theta3)*(cos(phi43+theta4)*(cos(phi54+theta5)*cos(phi6
5+theta6)-sin(phi54+theta5)*cos(alpha6)*sin(phi65+theta6))sin(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*cos(phi65+theta6)+cos(alpha5)*cos(phi54
+theta5)*cos(alpha6)*sin(phi65+theta6)sin(alpha5)*sin(alpha6)*sin(phi65+theta6)))+sin(alpha3)*cos(phi32+theta3)*(cos(alpha4)*sin
(phi43+theta4)*(cos(phi54+theta5)*cos(phi65+theta6)sin(phi54+theta5)*cos(alpha6)*sin(phi65+theta6))+cos(alpha4)*cos(phi43+theta4)*(cos(alph
)*cos(alpha6)*sin(phi65+theta6)+cos(alpha5)*sin(alpha6)*sin(phi65+theta6)))+cos(alpha3)*(
sin(alpha4)*sin(phi43+theta4)*(cos(phi54+theta5)*cos(phi65+theta6)sin(phi54+theta5)*cos(alpha6)*sin(phi65+theta6))+sin(alpha4)*cos(phi43+theta4)*(cos(alpha
n(alpha6)*sin(phi65+theta6)))))*(cos(phi21+theta2)*(cos(phi32+theta3)*(cos(phi43+theta4)*
(cos(phi54+theta5)*d6+sin(phi54+theta5)*r6*sin(alpha6)+d5)sin(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*d6-
66
cos(alpha5)*cos(phi54+theta5)*r6*sin(alpha6)-sin(alpha5)*r6*cos(alpha6)r5*sin(alpha5))+d4)sin(phi32+theta3)*(cos(alpha4)*sin(phi43+theta4)*(cos(phi54+theta5)*d6+sin(phi54+theta5)
*r6*sin(alpha6)+d5)+cos(alpha4)*cos(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*d6cos(alpha5)*cos(phi54+theta5)*r6*sin(alpha6)-sin(alpha5)*r6*cos(alpha6)-r5*sin(alpha5))sin(alpha4)*(sin(alpha5)*sin(phi54+theta5)*d6sin(alpha5)*cos(phi54+theta5)*r6*sin(alpha6)+cos(alpha5)*r6*cos(alpha6)+r5*cos(alpha5))r4*sin(alpha4))+d3)sin(phi21+theta2)*(cos(alpha3)*sin(phi32+theta3)*(cos(phi43+theta4)*(cos(phi54+theta5)*d
6+sin(phi54+theta5)*r6*sin(alpha6)+d5)sin(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*d6cos(alpha5)*cos(phi54+theta5)*r6*sin(alpha6)-sin(alpha5)*r6*cos(alpha6)r5*sin(alpha5))+d4)+cos(alpha3)*cos(phi32+theta3)*(cos(alpha4)*sin(phi43+theta4)*(cos(ph
r4*cos(alpha4))-r3*sin(alpha3))+d2);
J612=(-cos(phi21+theta2)*(cos(phi32+theta3)*(cos(phi43+theta4)*(cos(phi54+theta5)*sin(phi65+theta6)-sin(phi54+theta5)*cos(alpha6)*cos(phi65+theta6))sin(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*sin(phi65+theta6)+cos(alpha5)*cos(phi54+theta5)*cos(alpha6
)*cos(phi65+theta6)-sin(alpha5)*sin(alpha6)*cos(phi65+theta6)))sin(phi32+theta3)*(cos(alpha4)*sin(phi43+theta4)*(-cos(phi54+theta5)*sin(phi65+theta6)sin(phi54+theta5)*cos(alpha6)*cos(phi65+theta6))+cos(alpha4)*cos(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*sin(phi65+theta6)+cos(alpha5)*cos(phi54+theta5)*cos(alpha6
)*cos(phi65+theta6)-sin(alpha5)*sin(alpha6)*cos(phi65+theta6))-sin(alpha4)*(sin(alpha5)*sin(phi54+theta5)*sin(phi65+theta6)+sin(alpha5)*cos(phi54+theta5)*cos(alpha6)
*cos(phi65+theta6)+cos(alpha5)*sin(alpha6)*cos(phi65+theta6))))+sin(phi21+theta2)*(cos(al
pha3)*sin(phi32+theta3)*(cos(phi43+theta4)*(-cos(phi54+theta5)*sin(phi65+theta6)sin(phi54+theta5)*cos(alpha6)*cos(phi65+theta6))-sin(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*sin(phi65+theta6)+cos(alpha5)*cos(phi54+theta5)*cos(alpha6
)*cos(phi65+theta6)sin(alpha5)*sin(alpha6)*cos(phi65+theta6)))+cos(alpha3)*cos(phi32+theta3)*(cos(alpha4)*si
n(phi43+theta4)*(-cos(phi54+theta5)*sin(phi65+theta6)sin(phi54+theta5)*cos(alpha6)*cos(phi65+theta6))+cos(alpha4)*cos(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*sin(phi65+theta6)+cos(alpha5)*cos(phi54+theta5)*cos(alpha6
*cos(phi65+theta6)+cos(alpha5)*sin(alpha6)*cos(phi65+theta6)))sin(alpha3)*(sin(alpha4)*sin(phi43+theta4)*(-cos(phi54+theta5)*sin(phi65+theta6)sin(phi54+theta5)*cos(alpha6)*cos(phi65+theta6))+sin(alpha4)*cos(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*sin(phi65+theta6)+cos(alpha5)*cos(phi54+theta5)*cos(alpha6
)*cos(phi65+theta6)-sin(alpha5)*sin(alpha6)*cos(phi65+theta6))+cos(alpha4)*(-
67
sin(alpha5)*sin(phi54+theta5)*sin(phi65+theta6)+sin(alpha5)*cos(phi54+theta5)*cos(alpha6)
*cos(phi65+theta6)+cos(alpha5)*sin(alpha6)*cos(phi65+theta6)))))*(cos(alpha2)*sin(phi21+t
heta2)*(cos(phi32+theta3)*(cos(phi43+theta4)*(cos(phi54+theta5)*d6+sin(phi54+theta5)*r6
*sin(alpha6)+d5)-sin(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*d6cos(alpha5)*cos(phi54+theta5)*r6*sin(alpha6)-sin(alpha5)*r6*cos(alpha6)r5*sin(alpha5))+d4)sin(phi32+theta3)*(cos(alpha4)*sin(phi43+theta4)*(cos(phi54+theta5)*d6+sin(phi54+theta5)
5)*d6-cos(alpha5)*cos(phi54+theta5)*r6*sin(alpha6)-sin(alpha5)*r6*cos(alpha6)r5*sin(alpha5))+cos(alpha4)*(sin(alpha5)*sin(phi54+theta5)*d6sin(alpha5)*cos(phi54+theta5)*r6*sin(alpha6)+cos(alpha5)*r6*cos(alpha6)+r5*cos(alpha5))+
r4*cos(alpha4))+r3*cos(alpha3))r2*sin(alpha2))+(cos(alpha2)*sin(phi21+theta2)*(cos(phi32+theta3)*(cos(phi43+theta4)*(cos(phi54+theta5)*sin(phi65+theta6)-sin(phi54+theta5)*cos(alpha6)*cos(phi65+theta6))sin(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*sin(phi65+theta6)+cos(alpha5)*cos(phi54+theta5)*cos(alpha6
)*cos(phi65+theta6)-sin(alpha5)*sin(alpha6)*cos(phi65+theta6))-sin(alpha4)*(-
68
*cos(phi65+theta6)+cos(alpha5)*sin(alpha6)*cos(phi65+theta6))))+cos(alpha2)*cos(phi21+t
heta2)*(cos(alpha3)*sin(phi32+theta3)*(cos(phi43+theta4)*(cos(phi54+theta5)*sin(phi65+theta6)-sin(phi54+theta5)*cos(alpha6)*cos(phi65+theta6))sin(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*sin(phi65+theta6)+cos(alpha5)*cos(phi54+theta5)*cos(alpha6
)*cos(phi65+theta6)-sin(alpha5)*sin(alpha6)*cos(phi65+theta6))+cos(alpha4)*(sin(alpha5)*sin(phi54+theta5)*sin(phi65+theta6)+sin(alpha5)*cos(phi54+theta5)*cos(alpha6)
*cos(phi65+theta6)+cos(alpha5)*sin(alpha6)*cos(phi65+theta6))))sin(alpha2)*(sin(alpha3)*sin(phi32+theta3)*(cos(phi43+theta4)*(cos(phi54+theta5)*sin(phi65+theta6)-sin(phi54+theta5)*cos(alpha6)*cos(phi65+theta6))sin(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*sin(phi65+theta6)+cos(alpha5)*cos(phi54+theta5)*cos(alpha6
)*cos(phi65+theta6)sin(alpha5)*sin(alpha6)*cos(phi65+theta6)))+sin(alpha3)*cos(phi32+theta3)*(cos(alpha4)*si
*cos(phi65+theta6)+cos(alpha5)*sin(alpha6)*cos(phi65+theta6)))+cos(alpha3)*(sin(alpha4)*
sin(phi43+theta4)*(-cos(phi54+theta5)*sin(phi65+theta6)sin(phi54+theta5)*cos(alpha6)*cos(phi65+theta6))+sin(alpha4)*cos(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*sin(phi65+theta6)+cos(alpha5)*cos(phi54+theta5)*cos(alpha6
*cos(phi65+theta6)+cos(alpha5)*sin(alpha6)*cos(phi65+theta6)))))*(cos(phi21+theta2)*(cos(
phi32+theta3)*(cos(phi43+theta4)*(cos(phi54+theta5)*d6+sin(phi54+theta5)*r6*sin(alpha6)
+d5)-sin(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*d6cos(alpha5)*cos(phi54+theta5)*r6*sin(alpha6)-sin(alpha5)*r6*cos(alpha6)r5*sin(alpha5))+d4)sin(phi32+theta3)*(cos(alpha4)*sin(phi43+theta4)*(cos(phi54+theta5)*d6+sin(phi54+theta5)
6+sin(phi54+theta5)*r6*sin(alpha6)+d5)-
69
sin(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*d6cos(alpha5)*cos(phi54+theta5)*r6*sin(alpha6)-sin(alpha5)*r6*cos(alpha6)r5*sin(alpha5))+d4)+cos(alpha3)*cos(phi32+theta3)*(cos(alpha4)*sin(phi43+theta4)*(cos(ph
J613=(cos(phi21+theta2)*(cos(phi32+theta3)*(cos(phi43+theta4)*sin(phi54+theta5)*sin(alpha6)sin(phi43+theta4)*(-cos(alpha5)*cos(phi54+theta5)*sin(alpha6)-sin(alpha5)*cos(alpha6)))sin(phi32+theta3)*(cos(alpha4)*sin(phi43+theta4)*sin(phi54+theta5)*sin(alpha6)+cos(alpha4
)*cos(phi43+theta4)*(-cos(alpha5)*cos(phi54+theta5)*sin(alpha6)-sin(alpha5)*cos(alpha6))sin(alpha4)*(sin(alpha5)*cos(phi54+theta5)*sin(alpha6)+cos(alpha5)*cos(alpha6))))+sin(phi21+theta2)*(c
os(alpha3)*sin(phi32+theta3)*(cos(phi43+theta4)*sin(phi54+theta5)*sin(alpha6)sin(phi43+theta4)*(-cos(alpha5)*cos(phi54+theta5)*sin(alpha6)sin(alpha5)*cos(alpha6)))+cos(alpha3)*cos(phi32+theta3)*(cos(alpha4)*sin(phi43+theta4)*si
n(phi54+theta5)*sin(alpha6)+cos(alpha4)*cos(phi43+theta4)*(cos(alpha5)*cos(phi54+theta5)*sin(alpha6)-sin(alpha5)*cos(alpha6))-sin(alpha4)*(sin(alpha5)*cos(phi54+theta5)*sin(alpha6)+cos(alpha5)*cos(alpha6)))sin(alpha3)*(sin(alpha4)*sin(phi43+theta4)*sin(phi54+theta5)*sin(alpha6)+sin(alpha4)*cos(p
hi43+theta4)*(-cos(alpha5)*cos(phi54+theta5)*sin(alpha6)sin(alpha5)*cos(alpha6))+cos(alpha4)*(sin(alpha5)*cos(phi54+theta5)*sin(alpha6)+cos(alpha5)*cos(alpha6)))))*(cos(alpha2)*sin(phi
21+theta2)*(cos(phi32+theta3)*(cos(phi43+theta4)*(cos(phi54+theta5)*d6+sin(phi54+theta5
)*r6*sin(alpha6)+d5)-sin(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*d6cos(alpha5)*cos(phi54+theta5)*r6*sin(alpha6)-sin(alpha5)*r6*cos(alpha6)r5*sin(alpha5))+d4)sin(phi32+theta3)*(cos(alpha4)*sin(phi43+theta4)*(cos(phi54+theta5)*d6+sin(phi54+theta5)
alpha5)*sin(phi54+theta5)*d6-cos(alpha5)*cos(phi54+theta5)*r6*sin(alpha6)sin(alpha5)*r6*cos(alpha6)-r5*sin(alpha5))-sin(alpha4)*(sin(alpha5)*sin(phi54+theta5)*d6sin(alpha5)*cos(phi54+theta5)*r6*sin(alpha6)+cos(alpha5)*r6*cos(alpha6)+r5*cos(alpha5))r4*sin(alpha4))-
70
sin(alpha3)*(sin(alpha4)*sin(phi43+theta4)*(cos(phi54+theta5)*d6+sin(phi54+theta5)*r6*sin
r4*cos(alpha4))+r3*cos(alpha3))r2*sin(alpha2))+(cos(alpha2)*sin(phi21+theta2)*(cos(phi32+theta3)*(cos(phi43+theta4)*sin(
phi54+theta5)*sin(alpha6)-sin(phi43+theta4)*(-cos(alpha5)*cos(phi54+theta5)*sin(alpha6)sin(alpha5)*cos(alpha6)))sin(phi32+theta3)*(cos(alpha4)*sin(phi43+theta4)*sin(phi54+theta5)*sin(alpha6)+cos(alpha4
)*cos(phi43+theta4)*(-cos(alpha5)*cos(phi54+theta5)*sin(alpha6)-sin(alpha5)*cos(alpha6))sin(alpha4)*(sin(alpha5)*cos(phi54+theta5)*sin(alpha6)+cos(alpha5)*cos(alpha6))))+cos(alpha2)*cos(phi
21+theta2)*(cos(alpha3)*sin(phi32+theta3)*(cos(phi43+theta4)*sin(phi54+theta5)*sin(alpha
6)-sin(phi43+theta4)*(-cos(alpha5)*cos(phi54+theta5)*sin(alpha6)sin(alpha5)*cos(alpha6)))+cos(alpha3)*cos(phi32+theta3)*(cos(alpha4)*sin(phi43+theta4)*si
hi43+theta4)*(-cos(alpha5)*cos(phi54+theta5)*sin(alpha6)sin(alpha5)*cos(alpha6))+cos(alpha4)*(sin(alpha5)*cos(phi54+theta5)*sin(alpha6)+cos(alpha5)*cos(alpha6))))sin(alpha2)*(sin(alpha3)*sin(phi32+theta3)*(cos(phi43+theta4)*sin(phi54+theta5)*sin(alpha
6)-sin(phi43+theta4)*(-cos(alpha5)*cos(phi54+theta5)*sin(alpha6)sin(alpha5)*cos(alpha6)))+sin(alpha3)*cos(phi32+theta3)*(cos(alpha4)*sin(phi43+theta4)*si
n(phi54+theta5)*sin(alpha6)+cos(alpha4)*cos(phi43+theta4)*(cos(alpha5)*cos(phi54+theta5)*sin(alpha6)-sin(alpha5)*cos(alpha6))-sin(alpha4)*(sin(alpha5)*cos(phi54+theta5)*sin(alpha6)+cos(alpha5)*cos(alpha6)))+cos(alpha3)*(sin(alph
a4)*sin(phi43+theta4)*sin(phi54+theta5)*sin(alpha6)+sin(alpha4)*cos(phi43+theta4)*(cos(alpha5)*cos(phi54+theta5)*sin(alpha6)-sin(alpha5)*cos(alpha6))+cos(alpha4)*(sin(alpha5)*cos(phi54+theta5)*sin(alpha6)+cos(alpha5)*cos(alpha6)))))*(cos(phi21+theta2)*
(cos(phi32+theta3)*(cos(phi43+theta4)*(cos(phi54+theta5)*d6+sin(phi54+theta5)*r6*sin(alp
ha6)+d5)-sin(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*d6cos(alpha5)*cos(phi54+theta5)*r6*sin(alpha6)-sin(alpha5)*r6*cos(alpha6)r5*sin(alpha5))+d4)-
71
sin(phi32+theta3)*(cos(alpha4)*sin(phi43+theta4)*(cos(phi54+theta5)*d6+sin(phi54+theta5)
J614=sin(alpha2)*sin(phi21+theta2)*(cos(phi32+theta3)*(cos(phi43+theta4)*(cos(phi54+thet
a5)*cos(phi65+theta6)-sin(phi54+theta5)*cos(alpha6)*sin(phi65+theta6))sin(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*cos(phi65+theta6)+cos(alpha5)*cos(phi54
)*cos(alpha6)*sin(phi65+theta6)+cos(alpha5)*sin(alpha6)*sin(phi65+theta6))))+sin(alpha2)*
n(alpha6)*sin(phi65+theta6))))+cos(alpha2)*(sin(alpha3)*sin(phi32+theta3)*(cos(phi43+thet
72
a4)*(cos(phi54+theta5)*cos(phi65+theta6)sin(phi54+theta5)*cos(alpha6)*sin(phi65+theta6))sin(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*cos(phi65+theta6)+cos(alpha5)*cos(phi54
n(alpha6)*sin(phi65+theta6))));
J615=sin(alpha2)*sin(phi21+theta2)*(cos(phi32+theta3)*(cos(phi43+theta4)*(cos(phi54+theta5)*sin(phi65+theta6)-sin(phi54+theta5)*cos(alpha6)*cos(phi65+theta6))sin(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*sin(phi65+theta6)+cos(alpha5)*cos(phi54+theta5)*cos(alpha6
*cos(phi65+theta6)+cos(alpha5)*sin(alpha6)*cos(phi65+theta6))))+sin(alpha2)*cos(phi21+th
eta2)*(cos(alpha3)*sin(phi32+theta3)*(cos(phi43+theta4)*(cos(phi54+theta5)*sin(phi65+theta6)-sin(phi54+theta5)*cos(alpha6)*cos(phi65+theta6))sin(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*sin(phi65+theta6)+cos(alpha5)*cos(phi54+theta5)*cos(alpha6
*cos(phi65+theta6)+cos(alpha5)*sin(alpha6)*cos(phi65+theta6))))+cos(alpha2)*(sin(alpha3)
*sin(phi32+theta3)*(cos(phi43+theta4)*(-cos(phi54+theta5)*sin(phi65+theta6)sin(phi54+theta5)*cos(alpha6)*cos(phi65+theta6))-sin(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*sin(phi65+theta6)+cos(alpha5)*cos(phi54+theta5)*cos(alpha6
73
*cos(phi65+theta6)+cos(alpha5)*sin(alpha6)*cos(phi65+theta6))));
J616=sin(alpha2)*sin(phi21+theta2)*(cos(phi32+theta3)*(cos(phi43+theta4)*sin(phi54+theta
5)*sin(alpha6)-sin(phi43+theta4)*(-cos(alpha5)*cos(phi54+theta5)*sin(alpha6)sin(alpha5)*cos(alpha6)))sin(phi32+theta3)*(cos(alpha4)*sin(phi43+theta4)*sin(phi54+theta5)*sin(alpha6)+cos(alpha4
)*cos(phi43+theta4)*(-cos(alpha5)*cos(phi54+theta5)*sin(alpha6)-sin(alpha5)*cos(alpha6))sin(alpha4)*(sin(alpha5)*cos(phi54+theta5)*sin(alpha6)+cos(alpha5)*cos(alpha6))))+sin(alpha2)*cos(phi2
1+theta2)*(cos(alpha3)*sin(phi32+theta3)*(cos(phi43+theta4)*sin(phi54+theta5)*sin(alpha6)
-sin(phi43+theta4)*(-cos(alpha5)*cos(phi54+theta5)*sin(alpha6)sin(alpha5)*cos(alpha6)))+cos(alpha3)*cos(phi32+theta3)*(cos(alpha4)*sin(phi43+theta4)*si
hi43+theta4)*(-cos(alpha5)*cos(phi54+theta5)*sin(alpha6)sin(alpha5)*cos(alpha6))+cos(alpha4)*(sin(alpha5)*cos(phi54+theta5)*sin(alpha6)+cos(alpha5)*cos(alpha6))))+cos(alpha2)*(sin(alp
ha3)*sin(phi32+theta3)*(cos(phi43+theta4)*sin(phi54+theta5)*sin(alpha6)sin(phi43+theta4)*(-cos(alpha5)*cos(phi54+theta5)*sin(alpha6)sin(alpha5)*cos(alpha6)))+sin(alpha3)*cos(phi32+theta3)*(cos(alpha4)*sin(phi43+theta4)*si
a4)*sin(phi43+theta4)*sin(phi54+theta5)*sin(alpha6)+sin(alpha4)*cos(phi43+theta4)*(cos(alpha5)*cos(phi54+theta5)*sin(alpha6)-sin(alpha5)*cos(alpha6))+cos(alpha4)*(sin(alpha5)*cos(phi54+theta5)*sin(alpha6)+cos(alpha5)*cos(alpha6))));
J61=[J611;
J612;
J613;
J614;
J615;
J616
];
74
J62=[(-cos(phi32+theta3)*(cos(phi43+theta4)*(cos(phi54+theta5)*cos(phi65+theta6)sin(phi54+theta5)*cos(alpha6)*sin(phi65+theta6))sin(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*cos(phi65+theta6)+cos(alpha5)*cos(phi54
+theta5)*cos(alpha6)*sin(phi65+theta6)sin(alpha5)*sin(alpha6)*sin(phi65+theta6)))+sin(phi32+theta3)*(cos(alpha4)*sin(phi43+theta
4)*(cos(phi54+theta5)*cos(phi65+theta6)sin(phi54+theta5)*cos(alpha6)*sin(phi65+theta6))+cos(alpha4)*cos(phi43+theta4)*(cos(alph
)*cos(alpha6)*sin(phi65+theta6)+cos(alpha5)*sin(alpha6)*sin(phi65+theta6))))*(cos(alpha3)
*sin(phi32+theta3)*(cos(phi43+theta4)*(cos(phi54+theta5)*d6+sin(phi54+theta5)*r6*sin(alp
ha6)+d5)-sin(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*d6cos(alpha5)*cos(phi54+theta5)*r6*sin(alpha6)-sin(alpha5)*r6*cos(alpha6)r5*sin(alpha5))+d4)+cos(alpha3)*cos(phi32+theta3)*(cos(alpha4)*sin(phi43+theta4)*(cos(ph
r4*cos(alpha4))r3*sin(alpha3))+(cos(alpha3)*sin(phi32+theta3)*(cos(phi43+theta4)*(cos(phi54+theta5)*cos(
phi65+theta6)-sin(phi54+theta5)*cos(alpha6)*sin(phi65+theta6))sin(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*cos(phi65+theta6)+cos(alpha5)*cos(phi54
n(alpha6)*sin(phi65+theta6))))*(cos(phi32+theta3)*(cos(phi43+theta4)*(cos(phi54+theta5)*
d6+sin(phi54+theta5)*r6*sin(alpha6)+d5)sin(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*d6cos(alpha5)*cos(phi54+theta5)*r6*sin(alpha6)-sin(alpha5)*r6*cos(alpha6)r5*sin(alpha5))+d4)sin(phi32+theta3)*(cos(alpha4)*sin(phi43+theta4)*(cos(phi54+theta5)*d6+sin(phi54+theta5)
*r6*sin(alpha6)+d5)+cos(alpha4)*cos(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*d6-
75
cos(alpha5)*cos(phi54+theta5)*r6*sin(alpha6)-sin(alpha5)*r6*cos(alpha6)-r5*sin(alpha5))sin(alpha4)*(sin(alpha5)*sin(phi54+theta5)*d6sin(alpha5)*cos(phi54+theta5)*r6*sin(alpha6)+cos(alpha5)*r6*cos(alpha6)+r5*cos(alpha5))r4*sin(alpha4))+d3);...
(-cos(phi32+theta3)*(cos(phi43+theta4)*(-cos(phi54+theta5)*sin(phi65+theta6)sin(phi54+theta5)*cos(alpha6)*cos(phi65+theta6))-sin(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*sin(phi65+theta6)+cos(alpha5)*cos(phi54+theta5)*cos(alpha6
)*cos(phi65+theta6)sin(alpha5)*sin(alpha6)*cos(phi65+theta6)))+sin(phi32+theta3)*(cos(alpha4)*sin(phi43+thet
a4)*(-cos(phi54+theta5)*sin(phi65+theta6)sin(phi54+theta5)*cos(alpha6)*cos(phi65+theta6))+cos(alpha4)*cos(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*sin(phi65+theta6)+cos(alpha5)*cos(phi54+theta5)*cos(alpha6
*cos(phi65+theta6)+cos(alpha5)*sin(alpha6)*cos(phi65+theta6))))*(cos(alpha3)*sin(phi32+t
heta3)*(cos(phi43+theta4)*(cos(phi54+theta5)*d6+sin(phi54+theta5)*r6*sin(alpha6)+d5)sin(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*d6cos(alpha5)*cos(phi54+theta5)*r6*sin(alpha6)-sin(alpha5)*r6*cos(alpha6)r5*sin(alpha5))+d4)+cos(alpha3)*cos(phi32+theta3)*(cos(alpha4)*sin(phi43+theta4)*(cos(ph
r4*cos(alpha4))-r3*sin(alpha3))+(cos(alpha3)*sin(phi32+theta3)*(cos(phi43+theta4)*(cos(phi54+theta5)*sin(phi65+theta6)-sin(phi54+theta5)*cos(alpha6)*cos(phi65+theta6))sin(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*sin(phi65+theta6)+cos(alpha5)*cos(phi54+theta5)*cos(alpha6
*cos(phi65+theta6)+cos(alpha5)*sin(alpha6)*cos(phi65+theta6))))*(cos(phi32+theta3)*(cos(
phi43+theta4)*(cos(phi54+theta5)*d6+sin(phi54+theta5)*r6*sin(alpha6)+d5)sin(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*d6cos(alpha5)*cos(phi54+theta5)*r6*sin(alpha6)-sin(alpha5)*r6*cos(alpha6)-
76
r5*sin(alpha5))+d4)sin(phi32+theta3)*(cos(alpha4)*sin(phi43+theta4)*(cos(phi54+theta5)*d6+sin(phi54+theta5)
*r6*sin(alpha6)+d5)+cos(alpha4)*cos(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*d6cos(alpha5)*cos(phi54+theta5)*r6*sin(alpha6)-sin(alpha5)*r6*cos(alpha6)-r5*sin(alpha5))sin(alpha4)*(sin(alpha5)*sin(phi54+theta5)*d6sin(alpha5)*cos(phi54+theta5)*r6*sin(alpha6)+cos(alpha5)*r6*cos(alpha6)+r5*cos(alpha5))r4*sin(alpha4))+d3);...
(-cos(phi32+theta3)*(cos(phi43+theta4)*sin(phi54+theta5)*sin(alpha6)sin(phi43+theta4)*(-cos(alpha5)*cos(phi54+theta5)*sin(alpha6)sin(alpha5)*cos(alpha6)))+sin(phi32+theta3)*(cos(alpha4)*sin(phi43+theta4)*sin(phi54+thet
a5)*sin(alpha6)+cos(alpha4)*cos(phi43+theta4)*(cos(alpha5)*cos(phi54+theta5)*sin(alpha6)-sin(alpha5)*cos(alpha6))-sin(alpha4)*(sin(alpha5)*cos(phi54+theta5)*sin(alpha6)+cos(alpha5)*cos(alpha6))))*(cos(alpha3)*sin(phi
32+theta3)*(cos(phi43+theta4)*(cos(phi54+theta5)*d6+sin(phi54+theta5)*r6*sin(alpha6)+d5
)-sin(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*d6cos(alpha5)*cos(phi54+theta5)*r6*sin(alpha6)-sin(alpha5)*r6*cos(alpha6)r5*sin(alpha5))+d4)+cos(alpha3)*cos(phi32+theta3)*(cos(alpha4)*sin(phi43+theta4)*(cos(ph
r4*cos(alpha4))r3*sin(alpha3))+(cos(alpha3)*sin(phi32+theta3)*(cos(phi43+theta4)*sin(phi54+theta5)*sin(a
lpha6)-sin(phi43+theta4)*(-cos(alpha5)*cos(phi54+theta5)*sin(alpha6)sin(alpha5)*cos(alpha6)))+cos(alpha3)*cos(phi32+theta3)*(cos(alpha4)*sin(phi43+theta4)*si
hi43+theta4)*(-cos(alpha5)*cos(phi54+theta5)*sin(alpha6)sin(alpha5)*cos(alpha6))+cos(alpha4)*(sin(alpha5)*cos(phi54+theta5)*sin(alpha6)+cos(alpha5)*cos(alpha6))))*(cos(phi32+theta3)*(
cos(phi43+theta4)*(cos(phi54+theta5)*d6+sin(phi54+theta5)*r6*sin(alpha6)+d5)sin(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*d6cos(alpha5)*cos(phi54+theta5)*r6*sin(alpha6)-sin(alpha5)*r6*cos(alpha6)r5*sin(alpha5))+d4)sin(phi32+theta3)*(cos(alpha4)*sin(phi43+theta4)*(cos(phi54+theta5)*d6+sin(phi54+theta5)
sin(alpha3)*sin(phi32+theta3)*(cos(phi43+theta4)*(cos(phi54+theta5)*cos(phi65+theta6)sin(phi54+theta5)*cos(alpha6)*sin(phi65+theta6))-
77
sin(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*cos(phi65+theta6)+cos(alpha5)*cos(phi54
n(alpha6)*sin(phi65+theta6)));...
sin(alpha3)*sin(phi32+theta3)*(cos(phi43+theta4)*(cos(phi54+theta5)*sin(phi65+theta6)-sin(phi54+theta5)*cos(alpha6)*cos(phi65+theta6))sin(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*sin(phi65+theta6)+cos(alpha5)*cos(phi54+theta5)*cos(alpha6
*cos(phi65+theta6)+cos(alpha5)*sin(alpha6)*cos(phi65+theta6)));...
sin(alpha3)*sin(phi32+theta3)*(cos(phi43+theta4)*sin(phi54+theta5)*sin(alpha6)sin(phi43+theta4)*(-cos(alpha5)*cos(phi54+theta5)*sin(alpha6)sin(alpha5)*cos(alpha6)))+sin(alpha3)*cos(phi32+theta3)*(cos(alpha4)*sin(phi43+theta4)*si
a4)*sin(phi43+theta4)*sin(phi54+theta5)*sin(alpha6)+sin(alpha4)*cos(phi43+theta4)*(cos(alpha5)*cos(phi54+theta5)*sin(alpha6)-sin(alpha5)*cos(alpha6))+cos(alpha4)*(sin(alpha5)*cos(phi54+theta5)*sin(alpha6)+cos(alpha5)*cos(alpha6)))...
];
J63=[(-cos(phi43+theta4)*(cos(phi54+theta5)*cos(phi65+theta6)sin(phi54+theta5)*cos(alpha6)*sin(phi65+theta6))+sin(phi43+theta4)*(cos(alpha5)*sin(phi54
+theta5)*cos(phi65+theta6)+cos(alpha5)*cos(phi54+theta5)*cos(alpha6)*sin(phi65+theta6)sin(alpha5)*sin(alpha6)*sin(phi65+theta6)))*(cos(alpha4)*sin(phi43+theta4)*(cos(phi54+thet
a5)*d6+sin(phi54+theta5)*r6*sin(alpha6)+d5)+cos(alpha4)*cos(phi43+theta4)*(cos(alpha5)*
78
sin(phi54+theta5)*d6-cos(alpha5)*cos(phi54+theta5)*r6*sin(alpha6)sin(alpha5)*r6*cos(alpha6)-r5*sin(alpha5))-sin(alpha4)*(sin(alpha5)*sin(phi54+theta5)*d6sin(alpha5)*cos(phi54+theta5)*r6*sin(alpha6)+cos(alpha5)*r6*cos(alpha6)+r5*cos(alpha5))r4*sin(alpha4))+(cos(alpha4)*sin(phi43+theta4)*(cos(phi54+theta5)*cos(phi65+theta6)sin(phi54+theta5)*cos(alpha6)*sin(phi65+theta6))+cos(alpha4)*cos(phi43+theta4)*(cos(alph
)*cos(alpha6)*sin(phi65+theta6)+cos(alpha5)*sin(alpha6)*sin(phi65+theta6)))*(cos(phi43+th
eta4)*(cos(phi54+theta5)*d6+sin(phi54+theta5)*r6*sin(alpha6)+d5)sin(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*d6cos(alpha5)*cos(phi54+theta5)*r6*sin(alpha6)-sin(alpha5)*r6*cos(alpha6)r5*sin(alpha5))+d4);...
(-cos(phi43+theta4)*(-cos(phi54+theta5)*sin(phi65+theta6)sin(phi54+theta5)*cos(alpha6)*cos(phi65+theta6))+sin(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*sin(phi65+theta6)+cos(alpha5)*cos(phi54+theta5)*cos(alpha6
)*cos(phi65+theta6)sin(alpha5)*sin(alpha6)*cos(phi65+theta6)))*(cos(alpha4)*sin(phi43+theta4)*(cos(phi54+the
ta5)*d6+sin(phi54+theta5)*r6*sin(alpha6)+d5)+cos(alpha4)*cos(phi43+theta4)*(cos(alpha5)
*sin(phi54+theta5)*d6-cos(alpha5)*cos(phi54+theta5)*r6*sin(alpha6)sin(alpha5)*r6*cos(alpha6)-r5*sin(alpha5))-sin(alpha4)*(sin(alpha5)*sin(phi54+theta5)*d6sin(alpha5)*cos(phi54+theta5)*r6*sin(alpha6)+cos(alpha5)*r6*cos(alpha6)+r5*cos(alpha5))r4*sin(alpha4))+(cos(alpha4)*sin(phi43+theta4)*(-cos(phi54+theta5)*sin(phi65+theta6)sin(phi54+theta5)*cos(alpha6)*cos(phi65+theta6))+cos(alpha4)*cos(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*sin(phi65+theta6)+cos(alpha5)*cos(phi54+theta5)*cos(alpha6
*cos(phi65+theta6)+cos(alpha5)*sin(alpha6)*cos(phi65+theta6)))*(cos(phi43+theta4)*(cos(p
hi54+theta5)*d6+sin(phi54+theta5)*r6*sin(alpha6)+d5)sin(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*d6cos(alpha5)*cos(phi54+theta5)*r6*sin(alpha6)-sin(alpha5)*r6*cos(alpha6)r5*sin(alpha5))+d4);...
(-cos(phi43+theta4)*sin(phi54+theta5)*sin(alpha6)+sin(phi43+theta4)*(cos(alpha5)*cos(phi54+theta5)*sin(alpha6)sin(alpha5)*cos(alpha6)))*(cos(alpha4)*sin(phi43+theta4)*(cos(phi54+theta5)*d6+sin(phi54
+theta5)*r6*sin(alpha6)+d5)+cos(alpha4)*cos(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)
*d6-cos(alpha5)*cos(phi54+theta5)*r6*sin(alpha6)-sin(alpha5)*r6*cos(alpha6)r5*sin(alpha5))-sin(alpha4)*(sin(alpha5)*sin(phi54+theta5)*d6sin(alpha5)*cos(phi54+theta5)*r6*sin(alpha6)+cos(alpha5)*r6*cos(alpha6)+r5*cos(alpha5))r4*sin(alpha4))+(cos(alpha4)*sin(phi43+theta4)*sin(phi54+theta5)*sin(alpha6)+cos(alpha4)*
cos(phi43+theta4)*(-cos(alpha5)*cos(phi54+theta5)*sin(alpha6)-sin(alpha5)*cos(alpha6))sin(alpha4)*(sin(alpha5)*cos(phi54+theta5)*sin(alpha6)+cos(alpha5)*cos(alpha6)))*(cos(phi43+theta4)*(c
os(phi54+theta5)*d6+sin(phi54+theta5)*r6*sin(alpha6)+d5)sin(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*d6cos(alpha5)*cos(phi54+theta5)*r6*sin(alpha6)-sin(alpha5)*r6*cos(alpha6)r5*sin(alpha5))+d4);...
79
n(alpha6)*sin(phi65+theta6));...
sin(alpha4)*sin(phi43+theta4)*(-cos(phi54+theta5)*sin(phi65+theta6)sin(phi54+theta5)*cos(alpha6)*cos(phi65+theta6))+sin(alpha4)*cos(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*sin(phi65+theta6)+cos(alpha5)*cos(phi54+theta5)*cos(alpha6
*cos(phi65+theta6)+cos(alpha5)*sin(alpha6)*cos(phi65+theta6));...
sin(alpha4)*sin(phi43+theta4)*sin(phi54+theta5)*sin(alpha6)+sin(alpha4)*cos(phi43+theta4)
*(-cos(alpha5)*cos(phi54+theta5)*sin(alpha6)-sin(alpha5)*cos(alpha6))+cos(alpha4)*(sin(alpha5)*cos(phi54+theta5)*sin(alpha6)+cos(alpha5)*cos(alpha6))...
];
J64=[(cos(phi54+theta5)*cos(phi65+theta6)+sin(phi54+theta5)*cos(alpha6)*sin(phi65+theta6))*(co
s(alpha5)*sin(phi54+theta5)*d6-cos(alpha5)*cos(phi54+theta5)*r6*sin(alpha6)sin(alpha5)*r6*cos(alpha6)r5*sin(alpha5))+(cos(alpha5)*sin(phi54+theta5)*cos(phi65+theta6)+cos(alpha5)*cos(phi54+t
heta5)*cos(alpha6)*sin(phi65+theta6)sin(alpha5)*sin(alpha6)*sin(phi65+theta6))*(cos(phi54+theta5)*d6+sin(phi54+theta5)*r6*sin
(alpha6)+d5);...
(cos(phi54+theta5)*sin(phi65+theta6)+sin(phi54+theta5)*cos(alpha6)*cos(phi65+theta6))*(c
os(alpha5)*sin(phi54+theta5)*d6-cos(alpha5)*cos(phi54+theta5)*r6*sin(alpha6)sin(alpha5)*r6*cos(alpha6)-r5*sin(alpha5))+(cos(alpha5)*sin(phi54+theta5)*sin(phi65+theta6)+cos(alpha5)*cos(phi54+theta5)*cos(alpha6
)*cos(phi65+theta6)sin(alpha5)*sin(alpha6)*cos(phi65+theta6))*(cos(phi54+theta5)*d6+sin(phi54+theta5)*r6*si
n(alpha6)+d5);...
-sin(phi54+theta5)*sin(alpha6)*(cos(alpha5)*sin(phi54+theta5)*d6cos(alpha5)*cos(phi54+theta5)*r6*sin(alpha6)-sin(alpha5)*r6*cos(alpha6)r5*sin(alpha5))+(-cos(alpha5)*cos(phi54+theta5)*sin(alpha6)sin(alpha5)*cos(alpha6))*(cos(phi54+theta5)*d6+sin(phi54+theta5)*r6*sin(alpha6)+d5);...
sin(alpha5)*sin(phi54+theta5)*cos(phi65+theta6)+sin(alpha5)*cos(phi54+theta5)*cos(alpha6
)*sin(phi65+theta6)+cos(alpha5)*sin(alpha6)*sin(phi65+theta6);...
*cos(phi65+theta6)+cos(alpha5)*sin(alpha6)*cos(phi65+theta6);...
-sin(alpha5)*cos(phi54+theta5)*sin(alpha6)+cos(alpha5)*cos(alpha6)...
];
J65=[cos(phi65+theta6)*r6*sin(alpha6)+cos(alpha6)*sin(phi65+theta6)*d6;
-sin(phi65+theta6)*r6*sin(alpha6)+cos(alpha6)*cos(phi65+theta6)*d6;
-sin(alpha6)*d6;
sin(alpha6)*sin(phi65+theta6);
sin(alpha6)*cos(phi65+theta6);
80
cos(alpha6)
];
J66=[ 0 ;
0;
0;
0;
0;
1
];
A= [J61,J62,J63,J64,J65,J66];
81
Fichiers : PINV_SVD.m et MCD_CENTAURE.m
Fichier PINV_SVD.m
Calcul la pseudo inverse par décomposition en valeurs singulières
function [res,inv_cond,dim_noyau,u,si,v]=pinv_svd(mat,tol)
[u,s,v]=svd(mat);
%si=inv(s);
if nargin<2
tol=1e-3;
end
si=s';
nvs=size(s,1);
%disp(['Valeurs singulieres :',num2str(diag(s)')])
inv_cond=s(nvs,nvs)/s(1,1);
si(1,1)=1./s(1,1);
i=2;
while (i<=nvs)
if(s(i,i)/s(i-1,i-1)<tol)
break;
end;
si(i,i)=1./s(i,i);
i=i+1;
end;
for (j=i:nvs)
si(j,j)=0;
end
dim_noyau=nvs-i+1;
%disp('Valeurs singulieres');
%disp(diag(s)');
%disp(diag(si)');
res=v*si*u';
Fichier MCD_CENTAURE.m
Calcul le modèle cinématique directe du CENTAURE
function [A] = MCD_Centaure(theta)
%def des constantes
alpha1 = 0;
alpha2 = pi/2;
alpha3 = 0;
alpha4 = -pi/2;
alpha5 = pi/2;
82
alpha6 = -pi/2;
d1 = 0;
d2 = 0;
d3 = 0.3;
d4 = 0;
d5 = 0;
d6 = 0;
theta1 = 0;
theta2 = 0;
theta3 = 0;
theta4 = 0;
theta5 = 0;
theta6 = 0;
r1 = 0;
r2 = 0;
r3 = 0.0845;
r4 = 0.35;
r5 = 0;
r6 = 0;
phi10=theta(1);
phi21=theta(2);
phi32=theta(3);
phi43=theta(4);
phi54=theta(5);
phi65=theta(6);
%attention lignes de calculs tres longues, ne sont pas affichées, ne pas supprimer les lignes
qui paraissent vides
J611=(cos(phi21+theta2)*(cos(phi32+theta3)*(cos(phi43+theta4)*(cos(phi54+theta5)*cos(phi65+th
eta6)-sin(phi54+theta5)*cos(alpha6)*sin(phi65+theta6))sin(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*cos(phi65+theta6)+cos(alpha5)*cos(phi54
)*cos(alpha6)*sin(phi65+theta6)+cos(alpha5)*sin(alpha6)*sin(phi65+theta6))))+sin(phi21+th
eta2)*(cos(alpha3)*sin(phi32+theta3)*(cos(phi43+theta4)*(cos(phi54+theta5)*cos(phi65+the
ta6)-sin(phi54+theta5)*cos(alpha6)*sin(phi65+theta6))sin(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*cos(phi65+theta6)+cos(alpha5)*cos(phi54
83
n(alpha6)*sin(phi65+theta6)))))*(cos(alpha2)*sin(phi21+theta2)*(cos(phi32+theta3)*(cos(phi
43+theta4)*(cos(phi54+theta5)*d6+sin(phi54+theta5)*r6*sin(alpha6)+d5)sin(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*d6cos(alpha5)*cos(phi54+theta5)*r6*sin(alpha6)-sin(alpha5)*r6*cos(alpha6)r5*sin(alpha5))+d4)sin(phi32+theta3)*(cos(alpha4)*sin(phi43+theta4)*(cos(phi54+theta5)*d6+sin(phi54+theta5)
5)*d6-cos(alpha5)*cos(phi54+theta5)*r6*sin(alpha6)-sin(alpha5)*r6*cos(alpha6)r5*sin(alpha5))+cos(alpha4)*(sin(alpha5)*sin(phi54+theta5)*d6-
84
sin(alpha5)*cos(phi54+theta5)*r6*sin(alpha6)+cos(alpha5)*r6*cos(alpha6)+r5*cos(alpha5))+
r4*cos(alpha4))+r3*cos(alpha3))r2*sin(alpha2))+(cos(alpha2)*sin(phi21+theta2)*(cos(phi32+theta3)*(cos(phi43+theta4)*(cos
(phi54+theta5)*cos(phi65+theta6)-sin(phi54+theta5)*cos(alpha6)*sin(phi65+theta6))sin(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*cos(phi65+theta6)+cos(alpha5)*cos(phi54
)*cos(alpha6)*sin(phi65+theta6)+cos(alpha5)*sin(alpha6)*sin(phi65+theta6))))+cos(alpha2)*
n(alpha6)*sin(phi65+theta6))))sin(alpha2)*(sin(alpha3)*sin(phi32+theta3)*(cos(phi43+theta4)*(cos(phi54+theta5)*cos(phi6
5+theta6)-sin(phi54+theta5)*cos(alpha6)*sin(phi65+theta6))sin(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*cos(phi65+theta6)+cos(alpha5)*cos(phi54
n(alpha6)*sin(phi65+theta6)))))*(cos(phi21+theta2)*(cos(phi32+theta3)*(cos(phi43+theta4)*
(cos(phi54+theta5)*d6+sin(phi54+theta5)*r6*sin(alpha6)+d5)sin(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*d6-
85
cos(alpha5)*cos(phi54+theta5)*r6*sin(alpha6)-sin(alpha5)*r6*cos(alpha6)r5*sin(alpha5))+d4)sin(phi32+theta3)*(cos(alpha4)*sin(phi43+theta4)*(cos(phi54+theta5)*d6+sin(phi54+theta5)
J612=(-cos(phi21+theta2)*(cos(phi32+theta3)*(cos(phi43+theta4)*(cos(phi54+theta5)*sin(phi65+theta6)-sin(phi54+theta5)*cos(alpha6)*cos(phi65+theta6))sin(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*sin(phi65+theta6)+cos(alpha5)*cos(phi54+theta5)*cos(alpha6
*cos(phi65+theta6)+cos(alpha5)*sin(alpha6)*cos(phi65+theta6))))+sin(phi21+theta2)*(cos(al
pha3)*sin(phi32+theta3)*(cos(phi43+theta4)*(-cos(phi54+theta5)*sin(phi65+theta6)sin(phi54+theta5)*cos(alpha6)*cos(phi65+theta6))-sin(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*sin(phi65+theta6)+cos(alpha5)*cos(phi54+theta5)*cos(alpha6
)*cos(phi65+theta6)-sin(alpha5)*sin(alpha6)*cos(phi65+theta6))+cos(alpha4)*(-
86
*cos(phi65+theta6)+cos(alpha5)*sin(alpha6)*cos(phi65+theta6)))))*(cos(alpha2)*sin(phi21+t
heta2)*(cos(phi32+theta3)*(cos(phi43+theta4)*(cos(phi54+theta5)*d6+sin(phi54+theta5)*r6
*sin(alpha6)+d5)-sin(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*d6cos(alpha5)*cos(phi54+theta5)*r6*sin(alpha6)-sin(alpha5)*r6*cos(alpha6)r5*sin(alpha5))+d4)sin(phi32+theta3)*(cos(alpha4)*sin(phi43+theta4)*(cos(phi54+theta5)*d6+sin(phi54+theta5)
r4*cos(alpha4))+r3*cos(alpha3))r2*sin(alpha2))+(cos(alpha2)*sin(phi21+theta2)*(cos(phi32+theta3)*(cos(phi43+theta4)*(cos(phi54+theta5)*sin(phi65+theta6)-sin(phi54+theta5)*cos(alpha6)*cos(phi65+theta6))sin(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*sin(phi65+theta6)+cos(alpha5)*cos(phi54+theta5)*cos(alpha6
)*cos(phi65+theta6)-sin(alpha5)*sin(alpha6)*cos(phi65+theta6))-sin(alpha4)*(-
87
*cos(phi65+theta6)+cos(alpha5)*sin(alpha6)*cos(phi65+theta6))))+cos(alpha2)*cos(phi21+t
heta2)*(cos(alpha3)*sin(phi32+theta3)*(cos(phi43+theta4)*(cos(phi54+theta5)*sin(phi65+theta6)-sin(phi54+theta5)*cos(alpha6)*cos(phi65+theta6))sin(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*sin(phi65+theta6)+cos(alpha5)*cos(phi54+theta5)*cos(alpha6
*cos(phi65+theta6)+cos(alpha5)*sin(alpha6)*cos(phi65+theta6))))sin(alpha2)*(sin(alpha3)*sin(phi32+theta3)*(cos(phi43+theta4)*(cos(phi54+theta5)*sin(phi65+theta6)-sin(phi54+theta5)*cos(alpha6)*cos(phi65+theta6))sin(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*sin(phi65+theta6)+cos(alpha5)*cos(phi54+theta5)*cos(alpha6
*cos(phi65+theta6)+cos(alpha5)*sin(alpha6)*cos(phi65+theta6)))))*(cos(phi21+theta2)*(cos(
phi32+theta3)*(cos(phi43+theta4)*(cos(phi54+theta5)*d6+sin(phi54+theta5)*r6*sin(alpha6)
+d5)-sin(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*d6cos(alpha5)*cos(phi54+theta5)*r6*sin(alpha6)-sin(alpha5)*r6*cos(alpha6)r5*sin(alpha5))+d4)sin(phi32+theta3)*(cos(alpha4)*sin(phi43+theta4)*(cos(phi54+theta5)*d6+sin(phi54+theta5)
6+sin(phi54+theta5)*r6*sin(alpha6)+d5)-
88
sin(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*d6cos(alpha5)*cos(phi54+theta5)*r6*sin(alpha6)-sin(alpha5)*r6*cos(alpha6)r5*sin(alpha5))+d4)+cos(alpha3)*cos(phi32+theta3)*(cos(alpha4)*sin(phi43+theta4)*(cos(ph
J613=(cos(phi21+theta2)*(cos(phi32+theta3)*(cos(phi43+theta4)*sin(phi54+theta5)*sin(alpha6)sin(phi43+theta4)*(-cos(alpha5)*cos(phi54+theta5)*sin(alpha6)-sin(alpha5)*cos(alpha6)))sin(phi32+theta3)*(cos(alpha4)*sin(phi43+theta4)*sin(phi54+theta5)*sin(alpha6)+cos(alpha4
)*cos(phi43+theta4)*(-cos(alpha5)*cos(phi54+theta5)*sin(alpha6)-sin(alpha5)*cos(alpha6))sin(alpha4)*(sin(alpha5)*cos(phi54+theta5)*sin(alpha6)+cos(alpha5)*cos(alpha6))))+sin(phi21+theta2)*(c
os(alpha3)*sin(phi32+theta3)*(cos(phi43+theta4)*sin(phi54+theta5)*sin(alpha6)sin(phi43+theta4)*(-cos(alpha5)*cos(phi54+theta5)*sin(alpha6)sin(alpha5)*cos(alpha6)))+cos(alpha3)*cos(phi32+theta3)*(cos(alpha4)*sin(phi43+theta4)*si
hi43+theta4)*(-cos(alpha5)*cos(phi54+theta5)*sin(alpha6)sin(alpha5)*cos(alpha6))+cos(alpha4)*(sin(alpha5)*cos(phi54+theta5)*sin(alpha6)+cos(alpha5)*cos(alpha6)))))*(cos(alpha2)*sin(phi
21+theta2)*(cos(phi32+theta3)*(cos(phi43+theta4)*(cos(phi54+theta5)*d6+sin(phi54+theta5
)*r6*sin(alpha6)+d5)-sin(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*d6cos(alpha5)*cos(phi54+theta5)*r6*sin(alpha6)-sin(alpha5)*r6*cos(alpha6)r5*sin(alpha5))+d4)sin(phi32+theta3)*(cos(alpha4)*sin(phi43+theta4)*(cos(phi54+theta5)*d6+sin(phi54+theta5)
alpha5)*sin(phi54+theta5)*d6-cos(alpha5)*cos(phi54+theta5)*r6*sin(alpha6)sin(alpha5)*r6*cos(alpha6)-r5*sin(alpha5))-sin(alpha4)*(sin(alpha5)*sin(phi54+theta5)*d6sin(alpha5)*cos(phi54+theta5)*r6*sin(alpha6)+cos(alpha5)*r6*cos(alpha6)+r5*cos(alpha5))r4*sin(alpha4))-
89
sin(alpha3)*(sin(alpha4)*sin(phi43+theta4)*(cos(phi54+theta5)*d6+sin(phi54+theta5)*r6*sin
r4*cos(alpha4))+r3*cos(alpha3))r2*sin(alpha2))+(cos(alpha2)*sin(phi21+theta2)*(cos(phi32+theta3)*(cos(phi43+theta4)*sin(
phi54+theta5)*sin(alpha6)-sin(phi43+theta4)*(-cos(alpha5)*cos(phi54+theta5)*sin(alpha6)sin(alpha5)*cos(alpha6)))sin(phi32+theta3)*(cos(alpha4)*sin(phi43+theta4)*sin(phi54+theta5)*sin(alpha6)+cos(alpha4
)*cos(phi43+theta4)*(-cos(alpha5)*cos(phi54+theta5)*sin(alpha6)-sin(alpha5)*cos(alpha6))sin(alpha4)*(sin(alpha5)*cos(phi54+theta5)*sin(alpha6)+cos(alpha5)*cos(alpha6))))+cos(alpha2)*cos(phi
21+theta2)*(cos(alpha3)*sin(phi32+theta3)*(cos(phi43+theta4)*sin(phi54+theta5)*sin(alpha
6)-sin(phi43+theta4)*(-cos(alpha5)*cos(phi54+theta5)*sin(alpha6)sin(alpha5)*cos(alpha6)))+cos(alpha3)*cos(phi32+theta3)*(cos(alpha4)*sin(phi43+theta4)*si
hi43+theta4)*(-cos(alpha5)*cos(phi54+theta5)*sin(alpha6)sin(alpha5)*cos(alpha6))+cos(alpha4)*(sin(alpha5)*cos(phi54+theta5)*sin(alpha6)+cos(alpha5)*cos(alpha6))))sin(alpha2)*(sin(alpha3)*sin(phi32+theta3)*(cos(phi43+theta4)*sin(phi54+theta5)*sin(alpha
6)-sin(phi43+theta4)*(-cos(alpha5)*cos(phi54+theta5)*sin(alpha6)sin(alpha5)*cos(alpha6)))+sin(alpha3)*cos(phi32+theta3)*(cos(alpha4)*sin(phi43+theta4)*si
a4)*sin(phi43+theta4)*sin(phi54+theta5)*sin(alpha6)+sin(alpha4)*cos(phi43+theta4)*(cos(alpha5)*cos(phi54+theta5)*sin(alpha6)-sin(alpha5)*cos(alpha6))+cos(alpha4)*(sin(alpha5)*cos(phi54+theta5)*sin(alpha6)+cos(alpha5)*cos(alpha6)))))*(cos(phi21+theta2)*
(cos(phi32+theta3)*(cos(phi43+theta4)*(cos(phi54+theta5)*d6+sin(phi54+theta5)*r6*sin(alp
ha6)+d5)-sin(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*d6cos(alpha5)*cos(phi54+theta5)*r6*sin(alpha6)-sin(alpha5)*r6*cos(alpha6)r5*sin(alpha5))+d4)-
90
sin(phi32+theta3)*(cos(alpha4)*sin(phi43+theta4)*(cos(phi54+theta5)*d6+sin(phi54+theta5)
J614=sin(alpha2)*sin(phi21+theta2)*(cos(phi32+theta3)*(cos(phi43+theta4)*(cos(phi54+thet
a5)*cos(phi65+theta6)-sin(phi54+theta5)*cos(alpha6)*sin(phi65+theta6))sin(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*cos(phi65+theta6)+cos(alpha5)*cos(phi54
)*cos(alpha6)*sin(phi65+theta6)+cos(alpha5)*sin(alpha6)*sin(phi65+theta6))))+sin(alpha2)*
n(alpha6)*sin(phi65+theta6))))+cos(alpha2)*(sin(alpha3)*sin(phi32+theta3)*(cos(phi43+thet
91
a4)*(cos(phi54+theta5)*cos(phi65+theta6)sin(phi54+theta5)*cos(alpha6)*sin(phi65+theta6))sin(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*cos(phi65+theta6)+cos(alpha5)*cos(phi54
n(alpha6)*sin(phi65+theta6))));
J615=sin(alpha2)*sin(phi21+theta2)*(cos(phi32+theta3)*(cos(phi43+theta4)*(cos(phi54+theta5)*sin(phi65+theta6)-sin(phi54+theta5)*cos(alpha6)*cos(phi65+theta6))sin(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*sin(phi65+theta6)+cos(alpha5)*cos(phi54+theta5)*cos(alpha6
*cos(phi65+theta6)+cos(alpha5)*sin(alpha6)*cos(phi65+theta6))))+sin(alpha2)*cos(phi21+th
eta2)*(cos(alpha3)*sin(phi32+theta3)*(cos(phi43+theta4)*(cos(phi54+theta5)*sin(phi65+theta6)-sin(phi54+theta5)*cos(alpha6)*cos(phi65+theta6))sin(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*sin(phi65+theta6)+cos(alpha5)*cos(phi54+theta5)*cos(alpha6
*cos(phi65+theta6)+cos(alpha5)*sin(alpha6)*cos(phi65+theta6))))+cos(alpha2)*(sin(alpha3)
*sin(phi32+theta3)*(cos(phi43+theta4)*(-cos(phi54+theta5)*sin(phi65+theta6)sin(phi54+theta5)*cos(alpha6)*cos(phi65+theta6))-sin(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*sin(phi65+theta6)+cos(alpha5)*cos(phi54+theta5)*cos(alpha6
92
*cos(phi65+theta6)+cos(alpha5)*sin(alpha6)*cos(phi65+theta6))));
J616=sin(alpha2)*sin(phi21+theta2)*(cos(phi32+theta3)*(cos(phi43+theta4)*sin(phi54+theta
5)*sin(alpha6)-sin(phi43+theta4)*(-cos(alpha5)*cos(phi54+theta5)*sin(alpha6)sin(alpha5)*cos(alpha6)))sin(phi32+theta3)*(cos(alpha4)*sin(phi43+theta4)*sin(phi54+theta5)*sin(alpha6)+cos(alpha4
)*cos(phi43+theta4)*(-cos(alpha5)*cos(phi54+theta5)*sin(alpha6)-sin(alpha5)*cos(alpha6))sin(alpha4)*(sin(alpha5)*cos(phi54+theta5)*sin(alpha6)+cos(alpha5)*cos(alpha6))))+sin(alpha2)*cos(phi2
1+theta2)*(cos(alpha3)*sin(phi32+theta3)*(cos(phi43+theta4)*sin(phi54+theta5)*sin(alpha6)
-sin(phi43+theta4)*(-cos(alpha5)*cos(phi54+theta5)*sin(alpha6)sin(alpha5)*cos(alpha6)))+cos(alpha3)*cos(phi32+theta3)*(cos(alpha4)*sin(phi43+theta4)*si
hi43+theta4)*(-cos(alpha5)*cos(phi54+theta5)*sin(alpha6)sin(alpha5)*cos(alpha6))+cos(alpha4)*(sin(alpha5)*cos(phi54+theta5)*sin(alpha6)+cos(alpha5)*cos(alpha6))))+cos(alpha2)*(sin(alp
ha3)*sin(phi32+theta3)*(cos(phi43+theta4)*sin(phi54+theta5)*sin(alpha6)sin(phi43+theta4)*(-cos(alpha5)*cos(phi54+theta5)*sin(alpha6)sin(alpha5)*cos(alpha6)))+sin(alpha3)*cos(phi32+theta3)*(cos(alpha4)*sin(phi43+theta4)*si
a4)*sin(phi43+theta4)*sin(phi54+theta5)*sin(alpha6)+sin(alpha4)*cos(phi43+theta4)*(cos(alpha5)*cos(phi54+theta5)*sin(alpha6)-sin(alpha5)*cos(alpha6))+cos(alpha4)*(sin(alpha5)*cos(phi54+theta5)*sin(alpha6)+cos(alpha5)*cos(alpha6))));
J61=[J611;
J612;
J613;
J614;
J615;
J616
];
93
J62=[(-cos(phi32+theta3)*(cos(phi43+theta4)*(cos(phi54+theta5)*cos(phi65+theta6)sin(phi54+theta5)*cos(alpha6)*sin(phi65+theta6))sin(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*cos(phi65+theta6)+cos(alpha5)*cos(phi54
+theta5)*cos(alpha6)*sin(phi65+theta6)sin(alpha5)*sin(alpha6)*sin(phi65+theta6)))+sin(phi32+theta3)*(cos(alpha4)*sin(phi43+theta
4)*(cos(phi54+theta5)*cos(phi65+theta6)sin(phi54+theta5)*cos(alpha6)*sin(phi65+theta6))+cos(alpha4)*cos(phi43+theta4)*(cos(alph
)*cos(alpha6)*sin(phi65+theta6)+cos(alpha5)*sin(alpha6)*sin(phi65+theta6))))*(cos(alpha3)
*sin(phi32+theta3)*(cos(phi43+theta4)*(cos(phi54+theta5)*d6+sin(phi54+theta5)*r6*sin(alp
ha6)+d5)-sin(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*d6cos(alpha5)*cos(phi54+theta5)*r6*sin(alpha6)-sin(alpha5)*r6*cos(alpha6)r5*sin(alpha5))+d4)+cos(alpha3)*cos(phi32+theta3)*(cos(alpha4)*sin(phi43+theta4)*(cos(ph
r4*cos(alpha4))r3*sin(alpha3))+(cos(alpha3)*sin(phi32+theta3)*(cos(phi43+theta4)*(cos(phi54+theta5)*cos(
phi65+theta6)-sin(phi54+theta5)*cos(alpha6)*sin(phi65+theta6))sin(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*cos(phi65+theta6)+cos(alpha5)*cos(phi54
n(alpha6)*sin(phi65+theta6))))*(cos(phi32+theta3)*(cos(phi43+theta4)*(cos(phi54+theta5)*
d6+sin(phi54+theta5)*r6*sin(alpha6)+d5)sin(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*d6cos(alpha5)*cos(phi54+theta5)*r6*sin(alpha6)-sin(alpha5)*r6*cos(alpha6)r5*sin(alpha5))+d4)sin(phi32+theta3)*(cos(alpha4)*sin(phi43+theta4)*(cos(phi54+theta5)*d6+sin(phi54+theta5)
*r6*sin(alpha6)+d5)+cos(alpha4)*cos(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*d6-
94
cos(alpha5)*cos(phi54+theta5)*r6*sin(alpha6)-sin(alpha5)*r6*cos(alpha6)-r5*sin(alpha5))sin(alpha4)*(sin(alpha5)*sin(phi54+theta5)*d6sin(alpha5)*cos(phi54+theta5)*r6*sin(alpha6)+cos(alpha5)*r6*cos(alpha6)+r5*cos(alpha5))r4*sin(alpha4))+d3);...
(-cos(phi32+theta3)*(cos(phi43+theta4)*(-cos(phi54+theta5)*sin(phi65+theta6)sin(phi54+theta5)*cos(alpha6)*cos(phi65+theta6))-sin(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*sin(phi65+theta6)+cos(alpha5)*cos(phi54+theta5)*cos(alpha6
)*cos(phi65+theta6)sin(alpha5)*sin(alpha6)*cos(phi65+theta6)))+sin(phi32+theta3)*(cos(alpha4)*sin(phi43+thet
a4)*(-cos(phi54+theta5)*sin(phi65+theta6)sin(phi54+theta5)*cos(alpha6)*cos(phi65+theta6))+cos(alpha4)*cos(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*sin(phi65+theta6)+cos(alpha5)*cos(phi54+theta5)*cos(alpha6
*cos(phi65+theta6)+cos(alpha5)*sin(alpha6)*cos(phi65+theta6))))*(cos(alpha3)*sin(phi32+t
heta3)*(cos(phi43+theta4)*(cos(phi54+theta5)*d6+sin(phi54+theta5)*r6*sin(alpha6)+d5)sin(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*d6cos(alpha5)*cos(phi54+theta5)*r6*sin(alpha6)-sin(alpha5)*r6*cos(alpha6)r5*sin(alpha5))+d4)+cos(alpha3)*cos(phi32+theta3)*(cos(alpha4)*sin(phi43+theta4)*(cos(ph
r4*cos(alpha4))-r3*sin(alpha3))+(cos(alpha3)*sin(phi32+theta3)*(cos(phi43+theta4)*(cos(phi54+theta5)*sin(phi65+theta6)-sin(phi54+theta5)*cos(alpha6)*cos(phi65+theta6))sin(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*sin(phi65+theta6)+cos(alpha5)*cos(phi54+theta5)*cos(alpha6
*cos(phi65+theta6)+cos(alpha5)*sin(alpha6)*cos(phi65+theta6))))*(cos(phi32+theta3)*(cos(
phi43+theta4)*(cos(phi54+theta5)*d6+sin(phi54+theta5)*r6*sin(alpha6)+d5)sin(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*d6cos(alpha5)*cos(phi54+theta5)*r6*sin(alpha6)-sin(alpha5)*r6*cos(alpha6)-
95
r5*sin(alpha5))+d4)sin(phi32+theta3)*(cos(alpha4)*sin(phi43+theta4)*(cos(phi54+theta5)*d6+sin(phi54+theta5)
(-cos(phi32+theta3)*(cos(phi43+theta4)*sin(phi54+theta5)*sin(alpha6)sin(phi43+theta4)*(-cos(alpha5)*cos(phi54+theta5)*sin(alpha6)sin(alpha5)*cos(alpha6)))+sin(phi32+theta3)*(cos(alpha4)*sin(phi43+theta4)*sin(phi54+thet
a5)*sin(alpha6)+cos(alpha4)*cos(phi43+theta4)*(cos(alpha5)*cos(phi54+theta5)*sin(alpha6)-sin(alpha5)*cos(alpha6))-sin(alpha4)*(sin(alpha5)*cos(phi54+theta5)*sin(alpha6)+cos(alpha5)*cos(alpha6))))*(cos(alpha3)*sin(phi
32+theta3)*(cos(phi43+theta4)*(cos(phi54+theta5)*d6+sin(phi54+theta5)*r6*sin(alpha6)+d5
)-sin(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*d6cos(alpha5)*cos(phi54+theta5)*r6*sin(alpha6)-sin(alpha5)*r6*cos(alpha6)r5*sin(alpha5))+d4)+cos(alpha3)*cos(phi32+theta3)*(cos(alpha4)*sin(phi43+theta4)*(cos(ph
r4*cos(alpha4))r3*sin(alpha3))+(cos(alpha3)*sin(phi32+theta3)*(cos(phi43+theta4)*sin(phi54+theta5)*sin(a
lpha6)-sin(phi43+theta4)*(-cos(alpha5)*cos(phi54+theta5)*sin(alpha6)sin(alpha5)*cos(alpha6)))+cos(alpha3)*cos(phi32+theta3)*(cos(alpha4)*sin(phi43+theta4)*si
hi43+theta4)*(-cos(alpha5)*cos(phi54+theta5)*sin(alpha6)sin(alpha5)*cos(alpha6))+cos(alpha4)*(sin(alpha5)*cos(phi54+theta5)*sin(alpha6)+cos(alpha5)*cos(alpha6))))*(cos(phi32+theta3)*(
cos(phi43+theta4)*(cos(phi54+theta5)*d6+sin(phi54+theta5)*r6*sin(alpha6)+d5)sin(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*d6cos(alpha5)*cos(phi54+theta5)*r6*sin(alpha6)-sin(alpha5)*r6*cos(alpha6)r5*sin(alpha5))+d4)sin(phi32+theta3)*(cos(alpha4)*sin(phi43+theta4)*(cos(phi54+theta5)*d6+sin(phi54+theta5)
sin(alpha3)*sin(phi32+theta3)*(cos(phi43+theta4)*(cos(phi54+theta5)*cos(phi65+theta6)sin(phi54+theta5)*cos(alpha6)*sin(phi65+theta6))-
96
sin(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*cos(phi65+theta6)+cos(alpha5)*cos(phi54
n(alpha6)*sin(phi65+theta6)));...
sin(alpha3)*sin(phi32+theta3)*(cos(phi43+theta4)*(cos(phi54+theta5)*sin(phi65+theta6)-sin(phi54+theta5)*cos(alpha6)*cos(phi65+theta6))sin(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*sin(phi65+theta6)+cos(alpha5)*cos(phi54+theta5)*cos(alpha6
*cos(phi65+theta6)+cos(alpha5)*sin(alpha6)*cos(phi65+theta6)));...
sin(alpha3)*sin(phi32+theta3)*(cos(phi43+theta4)*sin(phi54+theta5)*sin(alpha6)sin(phi43+theta4)*(-cos(alpha5)*cos(phi54+theta5)*sin(alpha6)sin(alpha5)*cos(alpha6)))+sin(alpha3)*cos(phi32+theta3)*(cos(alpha4)*sin(phi43+theta4)*si
a4)*sin(phi43+theta4)*sin(phi54+theta5)*sin(alpha6)+sin(alpha4)*cos(phi43+theta4)*(cos(alpha5)*cos(phi54+theta5)*sin(alpha6)-sin(alpha5)*cos(alpha6))+cos(alpha4)*(sin(alpha5)*cos(phi54+theta5)*sin(alpha6)+cos(alpha5)*cos(alpha6)))...
];
J63=[(-cos(phi43+theta4)*(cos(phi54+theta5)*cos(phi65+theta6)sin(phi54+theta5)*cos(alpha6)*sin(phi65+theta6))+sin(phi43+theta4)*(cos(alpha5)*sin(phi54
+theta5)*cos(phi65+theta6)+cos(alpha5)*cos(phi54+theta5)*cos(alpha6)*sin(phi65+theta6)sin(alpha5)*sin(alpha6)*sin(phi65+theta6)))*(cos(alpha4)*sin(phi43+theta4)*(cos(phi54+thet
a5)*d6+sin(phi54+theta5)*r6*sin(alpha6)+d5)+cos(alpha4)*cos(phi43+theta4)*(cos(alpha5)*
97
sin(phi54+theta5)*d6-cos(alpha5)*cos(phi54+theta5)*r6*sin(alpha6)sin(alpha5)*r6*cos(alpha6)-r5*sin(alpha5))-sin(alpha4)*(sin(alpha5)*sin(phi54+theta5)*d6sin(alpha5)*cos(phi54+theta5)*r6*sin(alpha6)+cos(alpha5)*r6*cos(alpha6)+r5*cos(alpha5))r4*sin(alpha4))+(cos(alpha4)*sin(phi43+theta4)*(cos(phi54+theta5)*cos(phi65+theta6)sin(phi54+theta5)*cos(alpha6)*sin(phi65+theta6))+cos(alpha4)*cos(phi43+theta4)*(cos(alph
)*cos(alpha6)*sin(phi65+theta6)+cos(alpha5)*sin(alpha6)*sin(phi65+theta6)))*(cos(phi43+th
eta4)*(cos(phi54+theta5)*d6+sin(phi54+theta5)*r6*sin(alpha6)+d5)sin(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*d6cos(alpha5)*cos(phi54+theta5)*r6*sin(alpha6)-sin(alpha5)*r6*cos(alpha6)r5*sin(alpha5))+d4);...
(-cos(phi43+theta4)*(-cos(phi54+theta5)*sin(phi65+theta6)sin(phi54+theta5)*cos(alpha6)*cos(phi65+theta6))+sin(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*sin(phi65+theta6)+cos(alpha5)*cos(phi54+theta5)*cos(alpha6
)*cos(phi65+theta6)sin(alpha5)*sin(alpha6)*cos(phi65+theta6)))*(cos(alpha4)*sin(phi43+theta4)*(cos(phi54+the
ta5)*d6+sin(phi54+theta5)*r6*sin(alpha6)+d5)+cos(alpha4)*cos(phi43+theta4)*(cos(alpha5)
*sin(phi54+theta5)*d6-cos(alpha5)*cos(phi54+theta5)*r6*sin(alpha6)sin(alpha5)*r6*cos(alpha6)-r5*sin(alpha5))-sin(alpha4)*(sin(alpha5)*sin(phi54+theta5)*d6sin(alpha5)*cos(phi54+theta5)*r6*sin(alpha6)+cos(alpha5)*r6*cos(alpha6)+r5*cos(alpha5))r4*sin(alpha4))+(cos(alpha4)*sin(phi43+theta4)*(-cos(phi54+theta5)*sin(phi65+theta6)sin(phi54+theta5)*cos(alpha6)*cos(phi65+theta6))+cos(alpha4)*cos(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*sin(phi65+theta6)+cos(alpha5)*cos(phi54+theta5)*cos(alpha6
*cos(phi65+theta6)+cos(alpha5)*sin(alpha6)*cos(phi65+theta6)))*(cos(phi43+theta4)*(cos(p
hi54+theta5)*d6+sin(phi54+theta5)*r6*sin(alpha6)+d5)sin(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*d6cos(alpha5)*cos(phi54+theta5)*r6*sin(alpha6)-sin(alpha5)*r6*cos(alpha6)r5*sin(alpha5))+d4);...
(-cos(phi43+theta4)*sin(phi54+theta5)*sin(alpha6)+sin(phi43+theta4)*(cos(alpha5)*cos(phi54+theta5)*sin(alpha6)sin(alpha5)*cos(alpha6)))*(cos(alpha4)*sin(phi43+theta4)*(cos(phi54+theta5)*d6+sin(phi54
+theta5)*r6*sin(alpha6)+d5)+cos(alpha4)*cos(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)
*d6-cos(alpha5)*cos(phi54+theta5)*r6*sin(alpha6)-sin(alpha5)*r6*cos(alpha6)r5*sin(alpha5))-sin(alpha4)*(sin(alpha5)*sin(phi54+theta5)*d6sin(alpha5)*cos(phi54+theta5)*r6*sin(alpha6)+cos(alpha5)*r6*cos(alpha6)+r5*cos(alpha5))r4*sin(alpha4))+(cos(alpha4)*sin(phi43+theta4)*sin(phi54+theta5)*sin(alpha6)+cos(alpha4)*
cos(phi43+theta4)*(-cos(alpha5)*cos(phi54+theta5)*sin(alpha6)-sin(alpha5)*cos(alpha6))sin(alpha4)*(sin(alpha5)*cos(phi54+theta5)*sin(alpha6)+cos(alpha5)*cos(alpha6)))*(cos(phi43+theta4)*(c
os(phi54+theta5)*d6+sin(phi54+theta5)*r6*sin(alpha6)+d5)sin(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*d6cos(alpha5)*cos(phi54+theta5)*r6*sin(alpha6)-sin(alpha5)*r6*cos(alpha6)r5*sin(alpha5))+d4);...
98
n(alpha6)*sin(phi65+theta6));...
sin(alpha4)*sin(phi43+theta4)*(-cos(phi54+theta5)*sin(phi65+theta6)sin(phi54+theta5)*cos(alpha6)*cos(phi65+theta6))+sin(alpha4)*cos(phi43+theta4)*(cos(alpha5)*sin(phi54+theta5)*sin(phi65+theta6)+cos(alpha5)*cos(phi54+theta5)*cos(alpha6
*cos(phi65+theta6)+cos(alpha5)*sin(alpha6)*cos(phi65+theta6));...
sin(alpha4)*sin(phi43+theta4)*sin(phi54+theta5)*sin(alpha6)+sin(alpha4)*cos(phi43+theta4)
*(-cos(alpha5)*cos(phi54+theta5)*sin(alpha6)-sin(alpha5)*cos(alpha6))+cos(alpha4)*(sin(alpha5)*cos(phi54+theta5)*sin(alpha6)+cos(alpha5)*cos(alpha6))...
];
J64=[(cos(phi54+theta5)*cos(phi65+theta6)+sin(phi54+theta5)*cos(alpha6)*sin(phi65+theta6))*(co
s(alpha5)*sin(phi54+theta5)*d6-cos(alpha5)*cos(phi54+theta5)*r6*sin(alpha6)sin(alpha5)*r6*cos(alpha6)r5*sin(alpha5))+(cos(alpha5)*sin(phi54+theta5)*cos(phi65+theta6)+cos(alpha5)*cos(phi54+t
heta5)*cos(alpha6)*sin(phi65+theta6)sin(alpha5)*sin(alpha6)*sin(phi65+theta6))*(cos(phi54+theta5)*d6+sin(phi54+theta5)*r6*sin
(alpha6)+d5);...
(cos(phi54+theta5)*sin(phi65+theta6)+sin(phi54+theta5)*cos(alpha6)*cos(phi65+theta6))*(c
os(alpha5)*sin(phi54+theta5)*d6-cos(alpha5)*cos(phi54+theta5)*r6*sin(alpha6)sin(alpha5)*r6*cos(alpha6)-r5*sin(alpha5))+(cos(alpha5)*sin(phi54+theta5)*sin(phi65+theta6)+cos(alpha5)*cos(phi54+theta5)*cos(alpha6
)*cos(phi65+theta6)sin(alpha5)*sin(alpha6)*cos(phi65+theta6))*(cos(phi54+theta5)*d6+sin(phi54+theta5)*r6*si
n(alpha6)+d5);...
-sin(phi54+theta5)*sin(alpha6)*(cos(alpha5)*sin(phi54+theta5)*d6cos(alpha5)*cos(phi54+theta5)*r6*sin(alpha6)-sin(alpha5)*r6*cos(alpha6)r5*sin(alpha5))+(-cos(alpha5)*cos(phi54+theta5)*sin(alpha6)sin(alpha5)*cos(alpha6))*(cos(phi54+theta5)*d6+sin(phi54+theta5)*r6*sin(alpha6)+d5);...
sin(alpha5)*sin(phi54+theta5)*cos(phi65+theta6)+sin(alpha5)*cos(phi54+theta5)*cos(alpha6
)*sin(phi65+theta6)+cos(alpha5)*sin(alpha6)*sin(phi65+theta6);...
*cos(phi65+theta6)+cos(alpha5)*sin(alpha6)*cos(phi65+theta6);...
-sin(alpha5)*cos(phi54+theta5)*sin(alpha6)+cos(alpha5)*cos(alpha6)...
];
J65=[cos(phi65+theta6)*r6*sin(alpha6)+cos(alpha6)*sin(phi65+theta6)*d6;
-sin(phi65+theta6)*r6*sin(alpha6)+cos(alpha6)*cos(phi65+theta6)*d6;
-sin(alpha6)*d6;
sin(alpha6)*sin(phi65+theta6);
sin(alpha6)*cos(phi65+theta6);
99
cos(alpha6)
];
J66=[ 0 ;
0;
0;
0;
0;
1
];
A= [J61,J62,J63,J64,J65,J66];
100
Fichiers : estim_theta.m et MGI_numerique.m
Fichier estim_theta.m
Le but est de calculer une estimation des variables articulaires à partir d’une mesure de pose
3D.
%utilisation de la jacobienne inverse pour l'estimation des variables articulaires
function [q_est] = estim_theta(data,fctMGD,fctMCD)
epsilon=data(1);
max_iter=data(2);
pose_effecteur=reshape(data(3:18),4,4);
estimation_initiale=data(19:end);
q_est=MGI_numerique(pose_effecteur,estimation_initiale,fctMGD,fctMCD,epsilon,max_iter,
0);
Fichier MGI_numerique.m
Le but est d’utiliser la jacobienne inverse pour le calcul des q.
%utilisation de la jacobienne inverse pour le calcul des q
% Minimise (selon Gauss-Newton) la fonction de cout
% 1/2 || pose_effecteur - MGD(q) ||^2 + mu/2 || q - q_init ||^2
function q_est =
MGI_numerique(pose_effecteur,q_init,fctMGD,fctMCD,epsilon,max_iter,mu)
i=0;
if nargin<7
mu=0;
end
X_mesure=xyzrpy(pose_effecteur);
q_i=mod(q_init,2*pi);
T0n_i=feval(fctMGD,q_init);
X_i=xyzrpy(T0n_i);
dimq=length(q_init);
Idimq=eye(dimq);
deltaX=X_i-X_mesure;
critere=(abs(deltaX(1))+abs(deltaX(2))+abs(deltaX(3)))*50+abs(deltaX(4))+abs(deltaX(5))+a
bs(deltaX(6));
while (critere>epsilon)
i=i+1;
101
phi=X_i(4);theta=X_i(5);
% calcul de la jacobienne courante
J=feval(fctMCD,q_i);
%calcul des matrices de transfert
cphi=cos(phi);
sphi=sin(phi);
ctheta=cos(theta);
tantheta=tan(theta);
omegaRPY=[cphi*tantheta,sphi*tantheta,1;
-sphi,cphi,0;
cphi/ctheta,sphi/ctheta,0];
omegaXYZ=[1,0,0;
0,1,0;
0,0,1];
R06=T0n_i(1:3,1:3);
vectT06=[ 0 ,-T0n_i(3,4), T0n_i(2,4);
T0n_i(3,4), 0 ,-T0n_i(1,4);
-T0n_i(2,4), T0n_i(1,4), 0 ];
nul=[0,0,0;
0,0,0;
0,0,0];
%pass=[R06,R06*vectT06;
% nul,R06];
Pass06=[R06,nul;
nul,R06];
%calcul de J06 (les X sont exprimes dans le repere 0)
J06 = Pass06*J;
truc= [omegaXYZ,nul;
nul,omegaRPY];
newJ06=truc*J06;
newJ06t=newJ06';
% Le systeme lineaire a resoudre
% SysMat=((1-mu)*newJ06t*newJ06+mu*Idimq);
% Noyau=(In-SysMat);
% SysVect=(newJ06t*deltaX-mu*(Idimq-newJ06t*newJ06)*(q_i-q_init));
% deltaq=0.125*SysMat\SysVect;
[newJ06pinv,inv_cond,dim_noyau,u,si,v]=pinv_svd(newJ06);
lambda=0.125;
deltaq=-lambda*pinv_svd(newJ06)*deltaX-lambda*mu*(Idimq-newJ06t*newJ06)*(q_iq_init);
102
%
%
[Jinv,inv_cond,dim]=pinv_svd(newJ06);
deltaq=0.125*Jinv*deltaX;
%
%
%
if (dim>0)
disp('MGI_numerique: MCD singulier')
end
%q_i=mod(q_i+deltaq,2*pi);
q_i=q_i+deltaq;
T0n_i=feval(fctMGD,q_i);
X_i=xyzrpy(T0n_i);
deltaX=X_i-X_mesure;
critere=(abs(deltaX(1))+abs(deltaX(2))+abs(deltaX(3)))*50+abs(deltaX(4))+abs(deltaX(5))+a
bs(deltaX(6));
if i>max_iter
disp(['Non convergence ! Erreur residuelle : ', num2str(critere)])
break;
end;
end
q_est=mod(q_i,2*pi); %[q_i(1),q_i(2),q_i(3),pi+q_i(4),2*pi-q_i(5),pi+q_i(6)]';
103
Eléments de réponses aux questions restées en suspend
Des solutions de principe sont proposées dans cette annexe. Par contre leur mise en œuvre
(codage, validation in situ, réalisation d’outillage), engagerait des coûts supplémentaires et
n’est pas nécessaire quant à la validation de l’étude de faisabilité :
Commande cartésienne d’un robot non instrumenté
par Asservissement visuel
1. Question 1 : Mire cylindrique sur l’effecteur pour obtenir une visibilité accrue (but
étendre le champ opérationnel du robot)
Supposons que l’on dispose d’une mire cylindrique comme présentée dans la figure
suivante :
Minit
i+1
i-1
i
Exemple de mire cylindrique
Supposons les trois hypothèses suivantes
• Hyp. 1 : un motif permet de calculer une pose
• Hyp. 2 : il y a toujours un motif courant i visible
• Hyp. 3 : Le modèle de la mire cylindrique est connu
alors il peut être envisagé l’algorithme suivant :
•
•
•
•
•
•
On calcul la pose 3D à partir motif i (utilisation du modèle du motif)
On en déduit la pose 3D de la mire cylindrique (utilisation du modèle de la mire)
On en déduit les coordonnées des points 3D des autres motifs (i+1,i-1)
On en déduit les coordonnées des points 2D image des autres motifs (i+1,i-1)
On initialise en permanence 2 algorithmes de suivi des autres motifs (i+1,i-1)
On commute au niveau suivi d’un motif à l’autre sans problème à priori (à
résoudre l’initialisation de l’algorithme – un marqueur supplémentaire sur la mire)
L’algorithme d’estimation des q utilise la pose 3D de la mire cylindrique (qui est
invariante quelquesoit le motif)
104
2. Question 2 : Positionnement de plusieurs caméras pour l’estimation des q (but
étendre le champ opérationnel du robot)
Le problème à résoudre est : assurer la visibilité de la mire effecteur dans un espace de
travail demi-sphérique autour du robot, augmenter la précision des détections pour
améliorer la précision de l’estimation de la pose de l’effecteur dans les tâches effectuées
latéralement au sol (saisir un objet) ou au mur (ouvrir un sas).
• Hyp. 1 : il y a pas d’occultations
• Hyp. 2 : on n’a pas de contraintes de coût
• Hyp. 3 : les caméras utilisées sont calibrées (internes et externes)
alors on peut utiliser 2 caméras Pan Tilt Zoom orientées de chaque coté afin de mettre les
algorithmes d’estimation des q dans les meilleures conditions.
Le Pan et le Tilt sont commandés pour effectuer un centrage de la mire dans l’image. Le
Zoom est commandé selon deux critères : éviter les sorties de champ en fonction des
vitesses du robot, optimiser les détections des amers afin d’adapter au mieux la précision
des détections (compromis à faire)
L’algorithme d’estimation de la pose 3D de la mire cylindrique utilise le modèle des
caméras PTZ.
3. Question 3 : Validation d’un positionnement par asservissement visuel avec volant
(but valider un cas réel d’utilisation : c est un problème de perception
Le principe du positionnement est déjà validé. Reste le problème du suivi et de
l’extraction des primitives nécessaires à la commande.
• Hyp. 1 : le volant est toujours visible
• Hyp. 2 : Le modèle du volant est connu (un cylindre et trois branches
cylindriques à 120°)
alors il peut être envisagé l’algorithme suivant :
• Détection :
o On détecte les 3 branches
o On détecte l’ellipse
• Calcul de pose :
o On calcul la pose 3D à partir des mesures images et du modèle de l’objet
• Suivi de l’objet :
o Un algorithme de suivi permet de simplifier la recherche des
informations visuelles (à résoudre l’initialisation de l’algorithme si
l’environnement est perturbé – un marqueur si l’on souhaite lever
l’ambiguïté de la symétrie de l’objet tous les 120°)
L’algorithme de positionnement par asservissement visuel pourra utiliser la pose 3D du
volant, ou bien des points caractéristiques dans l’image.
105

Commande cartésienne d`un robot non instrumenté par

Transcription

Documents pareils

Pointage et suivi automatiques - Observatoire astronomique de la

formulaire d`abonnement

MT404 Corrigé de l`examen du 13 Septembre 2000 Exercice I Soit a

Corrigé Exercice 1 - Stephane Genouel.

triques, nous pourrons poser MN MP MN ON = Sm c` MP . , d`où d`où

billetteries 2016

Bon de commande cinéma adhérent

Nombres complexes

Prix : 0 euros

Bon de commande cinéma adhérent