ENSEM 2016/2017

I. BUT DE L’ETUDE
L’objectif de cette manipulation consiste élaborer des lois de commandes par PID et par
découplage non linéaire pour un robot plan à trois axes permettant :
- La stabilisation du robot dans l’espace articulaire autour de tout point de fonctionnement.
- La poursuite d’une trajectoire servant de référence pour l’organe terminal : commande dans
l’espace opérationnel.
II. DESCRIPTION
Soit le bras du robot rigide a trois degrés de liberté suivant :

Figure 1: robot plan

On notera :

𝑋 = [𝑃𝑥 𝑃𝑦 𝜃]𝑇 :Vecteur des coordonnées opérationnelles.

𝑋 = [𝑃𝑥 𝑃𝑦 𝜃]𝑇 : Vecteur des vitesses opérationnelles.

𝑞 = [𝜃1 𝜃2 𝜃3]𝑇 : Vecteur des coordonnées articulaires.

𝑞 = [𝜃1 𝜃2 𝜃3]𝑇 : Vecteur des vitesses articulaires.

𝑢 = [𝑢1 𝑢2 𝑢3]𝑇 : Vecteur des commandes délivrées par les moteurs électriques
L1, L2, L3 : longueurs des segments
m1, m2, m3 : masses suspendues aux extrémités des segments.

III. Etudes théorique :

1. Modélisationgéométrique et dynamique
1.1. Modelégéométrique direct (MGD) du robot :𝑿 = 𝒇(𝒒)

1
ENSEM 2016/2017

𝜎𝑖 𝛼𝑗 𝑑𝑗 𝜃𝑗 𝑟𝑗
1 0 0 0 𝜃1 0
2 0 0 L1 𝜃2 0
3 0 0 L2 𝜃3 0

𝑗 −1
 Matrice élémentaire 𝑇𝑗 𝑎𝑣𝑒𝑐 𝑗 = 1,2,3, 𝑚3 voir
cos 𝜃1 − sin 𝜃1 0 0 cos 𝜃2 − sin 𝜃2 0 𝐿1
0 sin 𝜃1 cos 𝜃1 0 0 1 sin 𝜃2 cos 𝜃2 0 0
𝑇1 = ; 𝑇2 =
0 0 1 0 0 0 1 0
0 0 0 1 0 0 0 1
cos 𝜃3 − sin 𝜃3 0 𝐿2 1 0 0 𝐿3
2 sin 𝜃3 cos 𝜃3 0 0 3 0 1 0 0
𝑇3 = ; 𝑇𝑚3 =
0 0 1 0 0 0 1 0
0 0 0 1 0 0 0 1
𝑈𝑜 = 0𝑇𝑚3 = 0𝑇1 × 1𝑇2 × 2𝑇3 × 3𝑇𝑚3 Avec
𝒔𝒙 𝒏𝒙 𝒂𝒙 𝑷𝒙
𝒔𝒚 𝒏𝒚 𝒂𝒚 𝑷𝒚
𝑼𝟎 =
𝒔𝒛 𝒏𝒛 𝒂𝒛 𝑷𝒛
𝟎 𝟎 𝟎 𝟏
On obtient la matrice en multipliant le terme de la droite vers la gauche:

𝑈3 = 3𝑇𝑚3 ;

𝑈2 = 2𝑇3 × 𝑈3;

𝑈1 = 1𝑇2 × 𝑈2

𝑈0 = 0𝑇1 × 𝑈1Une fois cette matrice déterminée il suffit de faire par identification.
 On pourra directement déterminer cette matrice sur le plan du robot (voir figure) :
On aura alors
cos 𝜃1 + 𝜃2 + 𝜃3 0 0 𝑃𝑥
0 cos 𝜃1 + 𝜃2 + 𝜃3 0 𝑃𝑦
𝑈0 = 3𝑇𝑚3 = Avec :
0 0 cos 𝜃1 + 𝜃2 + 𝜃3 0
0 0 0 1
𝑷𝒙 𝑳𝟏 𝐜𝐨𝐬 𝜽𝟏 + 𝑳𝟐 𝐜𝐨𝐬 𝜽𝟏 + 𝜽𝟐 + 𝑳𝟑 𝐜𝐨𝐬 𝜽𝟏 + 𝜽𝟐 + 𝜽𝟑
𝑷𝒚 = 𝑳𝟏 𝐬𝐢𝐧 𝜽𝟏 + 𝑳𝟐 𝐬𝐢𝐧 𝜽𝟏 + 𝜽𝟐 + 𝑳𝟑 𝐬𝐢𝐧 𝜽𝟏 + 𝜽𝟐 + 𝜽𝟑
𝜽 𝜽𝟏 + 𝜽𝟐 + 𝜽𝟑

Ainsi avec les coordonnées articulaires 𝑞 = [𝜃1 𝜃2 𝜃3]𝑇 ; on pourra determiner les
coordonnées operationnelles 𝑋 = [𝑃𝑥 𝑃𝑦 𝜃]𝑇

1.2. Le modèlegéométrique Inverse : 𝒒 = 𝒇−𝟏 𝒙

2
 ENSEM 2016/2017

Ce modèle consiste à trouver les coordonnées articulaires pour des coordonnées

opérationnellesimposés ; donc la matrice U0 est donnée.
 Application de la méthode de Khalil86 :
Sois le MGD :

𝑃𝑥 𝐿1 cos 𝜃1 + 𝐿2 cos 𝜃1 + 𝜃2 + 𝐿3 cos 𝜃1 + 𝜃2 + 𝜃3

𝑃𝑦 = 𝐿1 sin 𝜃1 + 𝐿2 sin 𝜃1 + 𝜃2 + 𝐿3 sin 𝜃1 + 𝜃2 + 𝜃3
𝜃 𝜃1 + 𝜃2 + 𝜃3 = 𝜃

Elle correspond à l’équation de khalil86 de type 8 pour la détermination de 𝜃1 , 𝜃2 , 𝜃3 :

𝐿1 cos 𝜃1 + 𝐿2 cos 𝜃1 + 𝜃2 = 𝑃𝑥 − 𝐿3 cos 𝜃 𝑋 cos 𝜃𝑖 + 𝑌 cos 𝜃𝑖 + 𝜃𝑗 = 𝑍1

=
𝐿1 sin 𝜃1 + 𝐿2 sin 𝜃1 + 𝜃2 = 𝑃𝑦 − 𝐿3 sin 𝜃 𝑋 sin 𝜃𝑖 + 𝑌 sin 𝜃𝑖 + 𝜃𝑗 = 𝑍2

Avec :𝑋 = 𝐿1; 𝑌 = 𝐿2; 𝜃𝑖 = 𝜃1 ; 𝜃𝑗 = 𝜃2 ; 𝑍1 = 𝑃𝑥 − 𝐿3 cos 𝜃; 𝑍2 = 𝑃𝑦 − 𝐿3 sin 𝜃

d’autre part : 𝐵1 = 𝑋 + 𝑌 cos 𝜃𝑗 ; 𝐵2 = 𝑌 sin 𝜃𝑗

𝑍12 + 𝑍22 − 𝑋 2 − 𝑌² 𝐵1 × 𝑍1 + 𝐵2 × 𝑍2
𝜃𝑗 = 𝜃2 = cos −1 𝑒𝑡 𝜃𝑖 = 𝜃1 = cos−1 𝑒𝑡 𝜃3
2𝑋𝑌 𝐵12 + 𝐵2²
= 𝜃 − 𝜃2 − 𝜃1
On fera la simulation de ces modelés sur Matlab.

2. Modélisation dynamique
2.1. Matrice de l’équation de mouvement

Sois l’équation du mouvement : 𝑔𝑎𝑚𝑚𝑎(𝑞)𝑞 = 𝑔𝑎𝑚𝑚𝑎1(𝑞, 𝑞) + 𝑢

Gamma(q) : matrice d’inertie symétriquedéfinie positive.
Gamma1 (q,𝑞 ) est une vecteur qyu regroupe les non linéarité du système.
1 2 1 2 1
On a d’aprèsLaGrange : 𝐿 = 𝑇 − 𝑈 𝑎𝑣𝑒𝑐 𝑇 = 2 𝑚1. 𝑣1 + 2 𝑚2. 𝑣2 + 2 𝑚3. 𝑣3 ² et
𝑈 = 𝑚1. 𝑔. 𝐿1. sin 𝜃1 + 𝑚2. 𝑔. 𝐿2. sin 𝜃2 + 𝑚3. 𝑔. 𝐿3. sin 𝜃3
Apres calcul des vitesses on aura :
1 1 1
𝑇 = 𝑚123. 𝐿12 . 𝜃1 ² + 𝑚23. 𝐿22 . 𝜃12 ² + 𝑚3. 𝐿32 . 𝜃123 ²
2 2 2
+ 𝑚23. 𝐿1. 𝐿2. 𝜃1 . 𝜃12 . cos 𝜃2 + 𝑚3. 𝐿1. 𝐿3. 𝜃1 . 𝜃123 cos 𝜃23 + 𝑚3 . 𝐿2. 𝐿3. 𝜃12 . 𝜃123 . cos 𝜃3
𝑎11 𝑎12 𝑎13
1
On a:𝑇 = 2 × 𝑞 . 𝑔𝑎𝑚𝑚𝑎 𝑞 . 𝑞 𝑎𝑣𝑒𝑐𝑔𝑎𝑚𝑚𝑎 𝑞 = 𝑎21
2𝑇 𝑎22 𝑎23
𝑎31 𝑎32 𝑎33

𝑎11 𝑎12 𝑎13 𝜃1

1
T devient : 𝑇 = 2 × 𝜃1 ² 𝜃2 ² 𝜃3 ² . 𝑎21 𝑎22 𝑎23 . 𝜃2 par identification on
𝑎31 𝑎32 𝑎33 𝜃
3
déterminera la matrice d’inertie gamma (voir les coefficients de la matrice dans la programmation Matlab).

𝑔𝑎𝑚𝑚𝑎1 𝑞, 𝑞 = [𝑏1 𝑏2 𝑏3]𝑇

3
 ENSEM 2016/2017

L’équation du mouvement sous forme matricielle devient :

𝒂𝟏𝟏 𝒂𝟏𝟐 𝒂𝟏𝟑 𝒖𝟏
𝒂𝟐𝟏 𝒂𝟐𝟐 𝒂𝟐𝟑 . 𝜽𝟏 𝑻
𝜽𝟐 𝜽𝟑 = 𝒃𝟏 𝒃𝟐 𝒃𝟑 + 𝒖𝟐
𝒂𝟑𝟏 𝒂𝟑𝟐 𝒂𝟑𝟑 𝒖𝟐
On définira par la suite le paramètre U qui n’est rien d’autre que la commande linéaire PID
2.2. Représentation d’état du système compte tenu de 𝜽𝟏 𝜽𝟐 𝜽𝟑 :
𝑥1 𝑞
Sois 𝑥 = 𝑥2 = 𝑞 = [𝜃1 𝜃2 𝜃3 𝜃1 𝜃2 𝜃3 𝜃1 𝜃2 𝜃3 ]𝑇 = 𝑥1 𝑥2 𝑥3 𝑥4 𝑥6 𝑥7 𝑥8 𝑥9 𝑒𝑠𝑡 𝑙𝑒 𝑣𝑒𝑐𝑡𝑒𝑢𝑟 𝑑′𝑒𝑡𝑎𝑡
𝑥3 𝑞

𝒙𝟐 𝟎𝟑.𝟑
𝑥 = 𝑓 𝑥 + 𝑔 𝑥 .𝑢 𝒙= −𝟏 + .𝒖
Qui devient : 𝒈𝒂𝒎𝒎𝒂 𝒙𝟏 × 𝒈𝒂𝒎𝒎𝒂𝟏(𝒙𝟏, 𝒙𝟐) 𝒈𝒂𝒎𝒎𝒂−𝟏 (𝒙𝟏)
𝑦 = 𝑐𝑥
𝒚 = 𝒙𝟏

2.3. Représentation d’état du système compte tenu de 𝑷𝒙 𝑷𝒚 𝜽 :

𝑋
Sois 𝜉 = = [𝑃𝑥 𝑃𝑦 𝜃 𝑃𝑥𝑃𝑦 𝜃 ]𝑇 = 𝜉1 𝜉2 𝜉3 𝜉4 𝜉6 𝑒𝑠𝑡 𝑙𝑒 𝑣𝑒𝑐𝑡𝑒𝑢𝑟 𝑑′𝑒𝑡𝑎𝑡
𝑋
𝜉 = 𝑓 𝜉 + 𝑔 𝜉 .𝑢
Qui
𝑦 = 𝑐𝜉
𝝃𝟐 𝟎𝟑.𝟑
𝝃= + .𝒖
devient : 𝒈𝒂𝒎𝒎𝒂 −𝟏
𝝃𝟏 × 𝒈𝒂𝒎𝒎𝒂𝟏(𝝃𝟏, 𝝃𝟐) 𝒈𝒂𝒎𝒎𝒂−𝟏 (𝝃𝟏)
𝒚 = 𝝃𝟏

3. Commande en linéaire de type PID

Sois la figure suivante illustrant le schéma d’une commande par PID d’un robot
manipulateur :

3.1. Loi de commande du robot plan :

4
ENSEM 2016/2017

𝑡
𝑢 = 𝐾𝑝 𝑞𝑐 − 𝑞 + 𝐾𝑑 𝑞𝑐 − 𝑞 + 𝐾𝐼 𝑞𝑐 − 𝑞 𝑑𝑡
𝑡0

Avec :
[𝑞𝑐 𝑞𝑐 ] =
𝑥1𝑐 𝑥2𝑐 𝑥3𝑐 𝑥4𝑐 𝑥5𝑐 𝑥6𝑐 𝑞𝑢𝑖 𝑠𝑜𝑛𝑡 𝑙𝑒𝑠 𝑐𝑜𝑛𝑠𝑖𝑔𝑛𝑒𝑠 𝑑𝑒𝑠 𝑝𝑜𝑠𝑖𝑡𝑖𝑜𝑛𝑠 𝑒𝑡 𝑣𝑖𝑡𝑒𝑠𝑠𝑒 𝑑𝑎𝑛𝑠 𝑙 ′ 𝑒𝑠𝑝𝑎𝑐𝑒 𝑎𝑟𝑡𝑖𝑐𝑢𝑙𝑎𝑖𝑟𝑒
𝑡
On a:𝑢𝑗 = 𝐾𝑝𝑗 𝑞𝑐 − 𝑞 + 𝐾𝑑𝑗 𝑞𝑐 − 𝑞 + 𝐾𝐼𝑗 𝑡0
𝑞𝑐 − 𝑞 𝑑𝑡 𝑎𝑣𝑒𝑐𝑗 = 1,2,3

Pour le calcul des gainsproportionnelsKpj, dérivés Kdj et intégrauxKIj, il s’agit de considérer le

modèle de l’articulation jrégit par le systèmelinéaire du second ordre à coefficient constant :
𝒖𝒋 = 𝒂𝒋 𝒒𝒋 𝒂𝒗𝒆𝒄 𝒂𝒋 = 𝑨𝒋𝒋𝒎𝒂𝒙 𝒍𝒂 𝒗𝒂𝒍𝒆𝒖𝒓 𝒎𝒂𝒙𝒊𝒎𝒂𝒍 𝒅𝒆 𝒍𝒂 𝒎𝒂𝒕𝒓𝒊𝒄𝒆 𝒅′ 𝒊𝒏𝒆𝒓𝒕𝒊𝒆𝒅𝒖 𝒓𝒐𝒃𝒐𝒕

𝑲𝒑𝒋 = 𝟑. 𝒂𝒋 . 𝒘𝟐𝒋
On en déduit alors : 𝒌𝒅𝒋 = 𝟑. 𝒂𝒋 . 𝒘𝒋
𝑲𝑰𝒋 = 𝒂𝒋 . 𝒘𝟐𝒋

(Voir simulation pour plus de détaille)

IV. SIMULATION/ MANIPULATION

1- Détermination du domaine de travail dans le plan (O, X, Y) sachant que :
𝜃1 𝑣𝑎𝑟𝑖𝑒 𝑒𝑛𝑡𝑟𝑒 0 𝜋 ; 𝜃2 𝑒𝑡𝜃3 𝑣𝑎𝑟𝑖𝑒𝑛𝑡 𝑒𝑛𝑡𝑟𝑒 [0 2𝜋]
Les longueurs de bras : L1=0.75 ; L2=0.25 ; L3=0.15 ;
2- Limitation du domaine de travail du robot dans un rectangle choisit.
3- Soit l’équation suivant : 𝑃𝑦 = 𝑎 ∗ sin 𝑤 ∗ 𝑃𝑥 + 𝑏 choisir des paramètresa, b et w
pour mettre la trajectoire de référence dans le domaine de travail.
a=0.15 ; b=0.65 et w=8
Programme des instructions 1, 2,3
Clearall; close all; clc
L1=0.75; L2=0.25; L3=0.15; i=1;
%determination du domaine de travail du robot
for teta1=0:pi/20:pi;
for teta2=0:pi/20:2*pi;
for teta3=0:pi/20:2*pi;
PX(i)= L1*cos(teta1)+L2*cos(teta1+teta2)+L3*cos(teta1+teta2+teta3);
PY(i)= L1*sin(teta1)+L2*sin(teta1+teta2)+L3*sin(teta1+teta2+teta3);
teta= teta1+teta2+teta3;
i=i+1;
end
end
end
plot(PX,PY,'b',0,0,'*r');
holdon

% limitation de l'espace de travail dans un rectangle

plot([-0.5 0.5 0.5 -0.5 -0.5],[0.5 0.5 0.8 0.8 0.5], 'r');

5
ENSEM 2016/2017

%trajectoire de l'organe termine dans le rectangle predefinit

PXR=-0.5:0.01:0.5; PYR=0.15*sin(8*PXR)+0.65;
plot(PXR,PYR,'g'),grid;
holdon
for PXR=-0.5:0.01:0.5; PYR=0.15*sin(8*PXR)+0.65;
tetaR=pi/2;
Z1=PXR-L3*cos(tetaR); Z2=PYR-L3*sin(tetaR);
X=L1; Y=L2;
teta2(i)=acos((Z1^2+Z2^2-X^2-Y^2)/(2*X*Y));
B1=X+Y*cos(teta2(i)); B2=Y*sin(teta2(i));
teta1(i)=acos((B1*Z1+B2*Z2)/(B1^2+B2^2));
teta3(i)=tetaR-teta1(i)-teta2(i);
i=i+1;
end
fori=1:length(teta1);
X11=L1*cos(teta1(i)); Y11=L1*sin(teta1(i));
X12=X11 + L2*cos(teta1(i)+teta2(i));
Y12=Y11 + L2*sin(teta1(i)+teta2(i));
X13=X12 + L3*cos(teta1(i)+teta2(i)+teta3(i));
Y13=Y12 + L3*sin(teta1(i)+teta2(i)+teta3(i));

plot([0 X11 X12 X13],[0 Y11 Y12 Y13],'k'); holdon

pause(0.2);

plot([-0.5 0.5 0.5 -0.5 -0.5],[0.5 0.5 0.8 0.8 0.5], 'r');
PXR=-0.5:0.01:0.5;
PYR=0.15*sin(8*PXR)+0.65;
holdon
plot(PXR,PYR,'g'),grid;

end

Figure 2: trajectoire définie

4- Simulation du comportement du robot en commande linéaire PID

1 𝐾𝐼𝑗
𝐹𝑗 (𝑝) = 𝑎 Et 𝐶𝑗 𝑝 = 𝐾𝑝𝑗 + + 𝐾𝑑𝑗 𝑝 𝑎𝑣𝑒𝑐 𝐾𝑝𝑗 = 3. 𝑎𝑗 . 𝑤𝑗2 ; 𝐾𝐼𝑗 = 𝑎𝑗 . 𝑤𝑗2 𝑒𝑡 𝑘𝑑𝑗 = 3. 𝑎𝑗 . 𝑤𝑗
𝑗 ∗𝑝² 𝑝

Voirprogramme :
clearall; close all; clc;
L1=0.75; L2=0.50; L3=0.25; m1=7.5; m2=5; m3=2.5; w1=8; w2=12; w3=15;
a11=(m1+m2+m3)*L1^2+(m2+m3)*L2^2+m3*L3^2+2*(m2+m3)*L1*L2+2*m3*L1*L3+2*m3*L2*L3;
a22=(m2+m3)*L2^2+m3*L3^2+2*m3*L2*L3;
a33=m3*L3^2;

% premiere articulation

6
ENSEM 2016/2017

Kp1=3*a11*w1^2; Kd1=3*a11*w1; %deuxieme articulation

Ki1=a11*w1^3; Kp2=3*a22*w2^2; Kd2=3*a22*w2;
num1=[Kd1 Kp1 Ki1]; Ki2=a22*w2^3;
den1=[a11 Kd1 Kp1 Ki1]; num2=[Kd2 Kp2 Ki2];
t=0:0.01:2; den2=[a22 Kd2 Kp2 Ki2];
y=step(num1,den1,t); t=0:0.01:2;
plot(t,y);grid; y=step(num2,den2,t);
holdon plot(t,y,'r');grid;
holdon
%troisieme articulation
Kp3=3*a33*w3^2; Kd3=3*a33*w3; y=1.05;
Ki3=a33*w3^3; t=0:0.01:2;
num3=[Kd3 Kp3 Ki3]; plot(t,y,'b');
den3=[a33 Kd3 Kp3 Ki3]; holdon
t=0:0.01:2; y=0.95;
y=step(num3,den3,t); t=0:0.01:2;
plot(t,y,'g');grid; plot(t,y,'b');
holdon

Figure 3: réponse indiciel : articulation 1 en bleu, articulation 2 en rouge et articulation 3 en vert

Articulation 1 Articulation 2 Articulation 3

Temps de réponse Tr 0.34 0.23 0.18
Temps de montée Tm 0.08 0.05 0.03
Temps du premier pic Tpic 0.16 0.1 0.08
dépassement 20%

Interprétation :
- si on donne lamême pulsation w aux articulations, on aura le mêmetemps de
réponse.
- La troisième articulation possède un temps de réponse plus rapide
- On remarque que la variation du paramètrean’a pas d’influence sur le système, car le
dynamique du système et la pulsation est fixer par l’opérateur.

5- Application en simulation de la commande PID décrite dans la partie théorique :

7
ENSEM 2016/2017

Pour se faire on utilisera la commande ode45 qui se fait par l’exécution de deux programmes
 Fonction principale :
clearall; close all;clc

t0=0;tf=2;
x0=[pi/2,pi/4,pi/8,0,0,0,0,0,0];

[t,x]=ode45('test2',t0,tf,x0)
figure(1)
plot(t,x(:,1));grid; xlabel('temps'); ylabel('gain')
figure(2)
plot(t,x(:,2));grid; xlabel('temps'); ylabel('gain')
figure(3)
plot(t,x(:,3));grid; xlabel('temps'); ylabel('gain')
figure(4)
plot(t,x(:,4));grid; xlabel('temps'); ylabel('gain')
figure(5)
plot(t,x(:,5));grid; xlabel('temps'); ylabel('gain')
figure(6)
plot(t,x(:,6));grid; xlabel('temps'); ylabel('gain')

 Fonction secondaire
FunctionXd=test2(t,x);
x1=x(1); x10=pi/2; x1c=pi/3; x8=x(8);
x2=x(2); x20=pi/4; x2c=pi/4; x9=x(9);
x3=x(3); x30=pi/8; x3c=pi/2;
x4=x(4); x40=0; x4c=0; L1=0.75; L2=0.50; L3=0.25 ;m1=7.5; m2=5; m3=2.5;
x5=x(5); x50=0; x5c=0;
x6=x(6); x60=0; x6c=0; w1=8; w2=12; w3=15; g=10;
x7=x(7);

% paramètres de la matrice d’inertie max gamma

a11=(m1+m2+m3)*L1^2+(m2+m3)*L2^2+m3*L3^2+2*(m2+m3)*L1*L2+2*m3*L1*L3+2*m3*L2*L3;
a12=(m2+m3)*L2^2+m3*L3^2+(m3+m2)*L1*L2+m3*L1*L3+2*m3*L2*L3;
a13=m3*L3^2+m3*L1*L2+m3*L3*L2;
a22=(m2+m3)*L2^2+m3*L3^2+2*m3*L2*L3;
a23=m3*L3^2+m3*L2*L3;
a33=m3*L3^2;

% parametre de la matrice des non linearités gamma1

b1=(m2+m3)*L1*L2*((x4+x5)+x4)*x5+m3*L1*L3*((x4+x5+x6)+x4)*(x5+x6)+m3*L2*L3*((x4+x5+x6)+(x4+x5)
)*x6-(m1+m2+m3)*g*L1-(m2+m3)*g*L2-m3*g*L3;
b2=(m2+m3)*L1*L2*x4*x5+m3*L1*L3*x4*(x5+x5)+m3*L2*L3*((x4+x5+x6)+(x4+x5))*x6-
(m2+m3)*L1*L2*x4*(x4+x5)-m3*L1*L3*x4*(x4+x5+x6)-(m2+m3)*g*L3-m3*g*L3;
b3=m3*L1*L3*x4*(x5+x6)+m3*L2*L3*(x4+x5)*x6-m3*g*L3;

% matrice d inertie max

gamma=[a11 a12 a13;a12 a22 a23;a13 a23 % matrice des non lineairté
a33]; gamma1=[b1;b2;b3];

% expression de f et g
% notons que q=[x1 x2 x3 x4 x5 x6]
f=[x4;x5;x6;inv(gamma)*gamma1];
g=[zeros(3); inv(gamma)];

%articulation 1 %articulation 2 %articulation 3

Kp1=3*a11*w1^2; Kp2=3*a22*w2^2; Kp3=3*a33*w3^2;
Kd1=3*a11*w1; Kd2=3*a22*w2; Kd3=3*a33*w3;
Ki1=a11*w1^3; Ki2=a22*w2^3; Ki3=a33*w3^3;

u11=Kp1*(x1c-x1); u12=Kd1*(x4c-x4); u13=x7;

8
ENSEM 2016/2017

u21=Kp2*(x2c-x2); u33=x9; u2=u21+u22+u23;

u22=Kd2*(x5c-x5); u31=Kp3*(x3c-x3); u3=u31+u32+u33;
u23=x8; u32=Kd3*(x6c-x6); u1=u11+u12+u13;

% expression de la loi de commande (matrice u)

u=[u1;u2;u3];

Xd=[f+g*u;Ki1*(x1c-x1);Ki2*(x2c-x2);Ki3*(x3c-x3)];

 Résultat et interpretation

Les figures sont de 1 à 6 de la gauche vers la droite et du haut vers le bas.

CONCLUSION
Ce TP a pour but la visualisation de toutes les configurations du robot sur l’écran graphique
de la poursuite de trajectoire de l’organe terminal d’un robot par la loi de la commande
élaborée dans l‘espaceopérationnel.
En effet on observe le domaine total de travail du robot prédéfini, le robot suivra la
trajectoire de référence 𝑃𝑦 = 𝑎 ∗ sin 𝑤 ∗ 𝑃𝑥 + 𝑏