Dynamique

Basé sur le chapitre 9 de Robotics, Vision and Control de Peter Corke et sur le chapitre 7 de Robotics:
Modelling, Planning and Control de Siciliano, Sciavicco, Villani et Oriolo.

Il utilise la boîte à outils robotique pour MATLAB.

Lien vers la séance théorique sur la Dymanique

Équations de mouvement
La boîte à outils robotique implémente les équations dynamiques à l’aide de la méthode Newton-Euler, qui est efficace
sur le plan informatique. La classe SerialLink possède plusieurs fonctions pour obtenir la dynamique inverse et directe,
ainsi que les différents termes des équations du mouvement.

La dérivation de ces termes se fera également dans ce tutoriel en utilisant la boîte à outils symbolique suivant la méthode
de Lagrange.

Les paramètres dynamiques du robot sont stockés dans chaque lien. Ils comprennent la masse, le centre de masse,
l'inertie, l'inertie du moteur, la friction du moteur et le rapport de transmission.

>> mdl_puma560
>> p560.links(1).dyn
>> p560.links(6).dyn

Dynamique inverse
La fonction rne donne l'ensemble des couples articulaires nécessaires pour atteindre une configuration, une vitesse et
une accélération données. Si une vitesse et une accélération nulles sont spécifiées, les couples obtenus sont ceux
nécessaires pour maintenir le robot contre la gravité.

>> Qn1 = p560.rne(qn,[0 0 0 0 0 0],[0 0 0 0 0 0])

Les couples requis pour suivre une trajectoire souhaitée sont calculés comme suit :

>> q = jtraj(qz, qr,10)

>> Q = p560.rne(q,0*q,0*q)

Q est une matrice 10x6 contenant les couples requis à chaque pas de temps.

Le vecteur gravité utilisé est :

>> p560.gravity

La fonction rne permet également de spécifier un vecteur de gravité différent en le passant en paramètre, ce qui est
utile si par exemple le robot est fixé au plafond. Nous pouvons vérifier que dans un environnement en apesanteur, les
couples requis résultants sont nuls :
 >> Qz = p560.rne(qz,[0 0 0 0 0 0],[0 0 0 0 0 0],[0 0 0])

Enfin, analysons les couples requis à qn lorsque la vitesse de la première articulation est fixée à 1 rad/s (avec une
accélération nulle sur toutes les articulations et sans tenir compte de la gravité) :

>>Qn2 = p560.rne(qn,[1 0 0 0 0 0],[0 0 0 0 0 0],[0 0 0])

Nous pouvons voir qu'outre les couples sur l'articulation 1, des couples sur les articulations 2, 3 et 4 sont également
nécessaires en raison des forces centripèdes et de Coriolis.

Dynamique avant

La dynamique avant est implémentée avec la fonction accel :

qdd = p560.accel(q, qd, Q)

La fonction donne les accélérations articulaires qui résultent de l'application des couples d'actionneur Q au
manipulateur dans l'état ( q , qd ). Il doit être utilisé en conjonction avec l'intégration numérique afin de simuler la
dynamique du manipulateur.

Le modèle Simulink suivant simule l'effondrement du robot puma560 sous l'effet de la gravité lorsqu'un couple nul est
appliqué. Le bloc NF/Puma560 contient la dynamique directe et l'intégration :

>> sl_ztorque

>>> EXERCISE
Change the input torque to Qn1 (in the Zero torque block) and the initial state to qn (in
the NF/Puma560 block) and verify that the computed torque effectively holds the robot at qn
against gravity.

Matrice d'inertie
La matrice d'inertie peut être obtenue par la fonction inertie :
 >> B = p560.inertia(qn)

Observe ceci:

• B est symétrique,
• Les termes diagonaux décrivent l'inertie vue par les articulations, c'est à dire , et donc les
termes et sont plus grands puisqu'ils correspondent aux maillons les plus lourds,
• Les termes hors diagonale représentent les couplages.

Effet de la charge utile

Si une charge utile est ajoutée au niveau de l'effecteur final (par exemple à une distance de 0,1 m dans la direction z),
alors la matrice d'inertie varie :

>> p560.payload(2.5, [0 0 0.1])

>> Bloaded = p560.inertia(qn)

On voit que les composantes diagonales ont augmenté de manière significative, ce qui signifie que l'accélération
maximale autorisée sera réduite (et donc il sera plus difficile de suivre des trajectoires à grande vitesse). Aussi, on voit
que l'inertie du joint 6 n'est pas affectée puisque la masse ajoutée se situe dans l'axe de ce joint.

Codage avec la boîte à outils symbolique Matlab

Considérons le manipulateur planaire à deux liaisons se déplaçant sur le plan xy (avec la gravité dans la direction -y) :
 • Le fichier two_link_manipulator.m comprend la définition des variables symboliques et la modélisation du
manipulateur à deux maillons en utilisant à la fois les expressions symboliques et le modèle à deux maillons par
défaut de Robotics Toolbox qui a été modifié pour inclure l'inertie, les valeurs de frottement et les rapports de
démultiplication.
• Le fichier two_link_inertia.m inclut le calcul de la matrice d'inertie B :

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% INERTIA MATRIX
A01_l = subs(A01i,{ai,di,alphai,qi},{ l1, 0, 0, q1})
A12_l = subs(A01i,{ai,di,alphai,qi},{ l2, 0, 0, q2})
 pl1 = A01_l*origin
pl2 = A01*A12_l*origin
JP_l1 = [cross(z0,pl1(1:3)-p0(1:3)), [0 0 0]']
JP_l2 = [cross(z0,pl2(1:3)-p0(1:3)), cross(z1,pl2(1:3)-p1(1:3))]
Jtheta_l1 = [z0 [0 0 0]']
Jtheta_l2 = [z0 z1]

B1 = m1*JP_l1'*JP_l1;
B2 = m2*JP_l2'*JP_l2;

R1 = A01(1:3,1:3)
vIl11 = subs(Il11,{Ixx1 Iyy1 Ixy1 Ixz1 Iyz1},{0 0 0 0 0})
Il1 = R1*vIl11*R1'

R2 = A01(1:3,1:3)*A12(1:3,1:3)
vIl22 = subs(Il22,{Ixx2 Iyy2 Ixy2 Ixz2 Iyz2},{0 0 0 0 0})
Il2 = R2*vIl22*R2'

B3 = Jtheta_l1'*Il1*Jtheta_l1
B4 = Jtheta_l2'*Il2*Jtheta_l2

B = simplify(B1+B2+B3+B4)

Exécutez les scripts :

>> two_link_manipulator
>> two_link_inertia
>>> EXERCISE
Create a script called verification.m. Use function subs to substitute in the expression of B
the values of the model and check, for configurations qa=[pi/3 pi/3] and qb=[pi/4 pi/4], that
the same is obtained by using the inertia function of the SerialLink class.

Matrice de Coriolis
La matrice de Coriolis contient les termes centripètes (proportionnels à ) et les termes de Coriolis (proportionnels
à ), et peut être obtenue par la fonction coriolis . En considérant le robot en configuration avec vitesse
pour toutes les articulations, alors la matrice de Coriolis est :

>> mdl_puma560
>> qd = 0.5*[1 1 1 1 1 1]
>> C = p560.coriolis(qn,qd)

Observez qu'il s'agit du couplage de la vitesse de l'articulation 2 au couple sur l'articulation 1, c'est-
à-dire que la rotation de l'épaule exerce un couple sur la taille.

Les couples articulaires dus à ces effets centripèdes et de Coriolis sont :

>> C*qd'

Codage avec la boîte à outils symbolique Matlab

Pour le manipulateur planaire à deux liaisons se déplaçant sur le plan xz :

• Le fichier two_link_coriolis.m inclut le calcul de la matrice de Coriolis C :

 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% CORIOLIS MATRIX

%Coriolis and centrifual terms

b11 = B(1,1)
b12 = B(1,2)
b21 = B(2,1)
b22 = B(2,2)
%cijk = simplify(0.5*(diff(bij,qk) + diff(bik,qj)-diff(bjk,qi)))
c111 = simplify(0.5*(diff(b11,q1) + diff(b11,q1)-diff(b11,q1)))
c112 = simplify(0.5*(diff(b11,q2) + diff(b12,q1)-diff(b12,q1)))
c121 = c112
c122 = simplify(0.5*(diff(b12,q2) + diff(b12,q2)-diff(b22,q1)))
c211 = simplify(0.5*(diff(b21,q1) + diff(b21,q1)-diff(b11,q2)))
c212 = simplify(0.5*(diff(b21,q2) + diff(b22,q1)-diff(b12,q2)))
c221 = c212
c222 = simplify(0.5*(diff(b22,q2) + diff(b22,q2)-diff(b22,q2)))

c11 = c111*qdot1+c112*qdot2
c12 = c121*qdot1+c122*qdot2
c21 = c211*qdot1+c212*qdot2
c22 = c221*qdot1+c222*qdot2

C = [c11 c12; c21 c22]

Exécutez le script :

>> two_link_coriolis
>>> EXERCISE
Complete the verification.m script. Substitute in the expression of C the values of the
model and check, for configurations qa=[pi/3 pi/3] and qb=[pi/4 pi/4], that the same is
obtained by using the coriolis function of the SerialLink class. Use the velocity qd=[0.1
0.1].

Terme de gravité
Le terme de gravité donne les couples requis pour compenser la gravité dans une configuration donnée. C'est le terme
dominant dans les équations dynamiques (et il est toujours là !). La fonction gravload calcule ce terme :

>> p560.gravload(qn)

Nous pouvons voir que c'est le même que le couple Qn1 calculé dans la section Dynamique inverse. On peut vérifier
que pour le bras en qs (c'est-à-dire complètement étiré), les composantes requises du terme de gravité sont plus élevées
:

>> p560.gravload(qs)

Codage avec la boîte à outils symbolique Matlab

Pour le manipulateur planaire à deux liaisons se déplaçant sur le plan xz :

• Le fichier two_link_gravity.m inclut le calcul du terme de gravité :

 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% GRAVITATIONAL TERM

%gravitational term
g0 = [0 -g 0]'
g1 = simplify(-m1*g0'*JP_l1(:,1) - m2*g0'*JP_l2(:,1))
g2 = simplify(-m1*g0'*JP_l1(:,2) - m2*g0'*JP_l2(:,2))

G = [g1 g2]'

Exécutez le script :

>> two_link_gravity
>>> EXERCISE
Complete the verification.m script. Substitute in the expression of G the values of the
model and check, for configurations qa=[pi/3 pi/3] and qb=[pi/4 pi/4], that the same is
obtained by using the gravload function of the SerialLink class.

Terme de friction
Comme vu au début du tutoriel, les paramètres dynamiques de chaque maillon incluent le frottement moteur, divisé en
frottement visqueux (paramètre Bm ) et frottement coulombien (paramètres Tc+ et Tc- ) :

La fonction friction donne le vecteur des couples de friction pour le robot se déplaçant à une vitesse qd donnée , à
ajouter au couple de sortie du moteur. Le frottement visqueux du moteur est mis à l'échelle et le frottement du moteur
 Columb par , étant le rapport de démultiplication, c'est-à-dire que la fonction
renvoie .

>> p560.friction([0 1 0 0 0 0])

Codage avec la boîte à outils symbolique Matlab

Pour le manipulateur planaire à deux liaisons se déplaçant sur le plan xz :

• Le fichier two_link_friction.m inclut le calcul du terme de frottement visqueux :

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% FRICTION
% no Coulomb friction is considered for this robot
F = [Bm1 Bm2].*[Gear1^2 Gear2^2]

Exécutez le script :

>> two_link_friction
>>> EXERCISE
Complete the verification.m script. Compute the friction torques using F for the values of
the velocity qd=[0.1 0.1], and check that the same is obtained by using the friction function
of the SerialLink class.

Paramétrage
Une propriété importante du modèle dynamique est la linéarité par rapport aux paramètres dynamiques :

• Le fichier two_link_parameterization.m inclut le paramétrage des équations du mouvement (sans considérer le

frottement) par rapport aux paramètres dynamiques de masse et d'inertie, soit le calcul du régresseur
:

Ce paramétrage permet l'utilisation de techniques d'identification pour estimer les paramètres dynamiques si des
trajectoires de mouvement appropriées sont imposées au manipulateur et des mesures sont effectuées des couples,
configurations, vitesses et accélérations articulaires.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% PARAMETERIZATION
%Equations of motion
q_dot = [qdot1 qdot2]'
q_dotdot = [qdotdot1 qdotdot2]'
tau = B*q_dotdot + C*q_dot + G

%Find the regressor

 param_pi = [m1 Izz1 m2 Izz2]'
c1 = coeffs(tau(1),[m1 Izz1 m2 Izz2])
c2 = coeffs(tau(2),[m2 Izz2])

Y = [c1(4) c1(3) c1(2) c1(1);...

0 0 c2(2) c2(1)]

Exécutez le script :

>> two_link_parameterization

Identification des paramètres dynamiques

Plusieurs trajectoires ont été exécutées en simulation à l'aide de ces fichiers . Les données obtenues sont stockées dans
le fichier traj.mat , qui contient les couples, configurations, vitesses et accélérations.

>>> EXERCISE
Estimate the dynamic parameters of the robot using this data.

Exemples
La solution aux exercices proposés pour la vérification du modèle dynamique du manipulateur planaire à deux maillons
se trouve dans ces fichiers : vérification.m et tutoriel_dynamique.m .

La solution de l'identification des paramètres dynamiques est model_identification.mlx .

Comme autre exemple, le script anthropomorphicArmDynamics.m et la fonction cijk.m sont utilisés pour calculer le
modèle dynamique d'un bras anthropomorphe à liaisons cylindriques réalisé à l'aide de la boîte à outils symbolique, et
pour le vérifier au moyen de la boîte à outils robotique.