Professional Documents
Culture Documents
Two-Link
Two
Link Robotic Manipulator
f(t)
d 2 x(t )
dx(t )
f (t ) M
Kx(t )
2
dt
dt
Dynamic equation
Mx(t ) x (t ) Kx(t ) f (t )
Transfer function
X (s)
1
2
F ( s ) Ms s K
Dynamic Equations
For simplicity, we assume that all mass exists as a point mass at the distal end of each link.
m1 , m2
l1 , l2
where
c1 : cos(q1 ) ,
The parameters
vi
and
s12 : sin(q1 q2 )
ki
, etc.
etc .
Mq Vq G Fq d
where
M
M 11
M 21
hq2
V
hq1
M 12
, with
M 22
hq1 hq2
, with h m2l1l2 sin(q2 )
0
m2l2 gcos(q1 q2 )
G2
F v q
F 1 1 1
F2 v2 q2
k sgn(q1 )
d d1 1
k
sgn
(
q
)
2
d2 2
d n
f(t)
mx bx kx f
(2)
Step 1
Asume that m, b, k are known
model based control law
m,b,k are obatained by real-time calculation
The system is simplified to mass-only system
Apply a model-based control law to Eq. (2)
f mf s bx kx
(3)
fs
y
will be simplyfied
py
as mass-onlyy second-order system
y
Then, the system
x fs
((4))
fs
.. .
f=mx+bx+kx
bx+kx
10
f s
xd K v e K p e :
xd u
We see that
u K v e K p e
(5)
11
..
fs
xd
.. .
f=mx+bx+kx
Kp
bx+kx
Kv
xd
xd
12
M (q )q V (q, q ) G (q )
(10)
13
M (q )(qd u ) V (q, q ) G (q )
Design
u (t )
(11)
u K v e K p e
(12)
Apply Eq.(12)
Eq (12) to Eq.
Eq (11),
(11) the control law will be
M (q )(qd K v e K p e) V (q, q ) G (q )
(13)
14
e K v e K p e 0
State-space form
d e 0
dt e K p
I e
K v e
c ( s ) | s 2 I K v s K p |
Total system
15
..
qd
M(q)
.
M(q)+V(q,q)+G(q)
q
Ki
1
s
Kp
V(q,q)+G(q)
V(q,q)
G(q)
Kv
qd
qd
(15)
u K v e K p e K i
Total control law
M (q )(qd K v e K p e K i ) V (q, q ) G (q )
(t )
(16)
e(t )
17
0
d
e 0
dt
e K i
I
0
K p
I e
K v e
c ( s ) | s 2 I K v s K p K i |
diagonal control gain
K d , K p , Ki
18
..
qd
M(q)
M(q)+V(q,q)+G(q)
q
Ki
1
s
Kp
V(q,q)+G(q)
Kv
qd
qd
19
M (q )q V (q, q ) G (q )
Tracking error
e(t ) qd (t ) q (t )
PD Computed Torque control law and closed-loop error dynamics (state space form)
M (q )(qd K v e K p e) V (q, q ) G (q )
d e 0
d e K p
dt
I e
K v e
PD-Gravity Control
K v e K p e G (q )
Classical PD Control
K v e K p e
20
e
M (q )(qd K v e K p e K i ) V (q, q ) G (q )
closed-loop error dynamics (state space form)
0
d
e 0
dt
e K i
I
0
K p
I e
K v e
e
K v e K p e K i
21
22
end
d
if tt >= 2;
amas(2)=mas(2)+1;
end
xx0=xx;
db=[0;0];
db=[1;1]*sin(4*pi*tt)*5; % Disturbance
k1=cplt(b,xx,amas,db)*T;
xx=xx0+k1*0.5;
k2=cplt(b,xx,amas,db)*T;
xx=xx0+k2*0.5;
db=[1;1]*sin(4*pi*(tt+T))*5;
k3=cplt(b,xx,amas,db)*T;
xx=xx0+k3;
k4=cplt(b,xx,amas,db)*T;
xx = xx0 + (k1 + 2 * k2 + 2 * k3 + k4) / 6;
XP(:,i)=Xp;
i
XD(:,i)=Xd;
i
d B(:,i)=b;
i b E(:,i)=e;
i
DE(:,i)=de;
i d
[e(1),e(2),norm(e)*10,norm(b),tt]
i=i+1;
tt=tt+T;
24
25
Cplt.m
function k1=cplt(b,xx,amas,db)
GRAV=9.8;
q1=xx(1); q2=xx(2); dq1=xx(3); dq2=xx(4);
c1=cos(q1);c12=cos(q1+q2);
c2=cos(q2);s2=sin(q2);
M=[amas(1)+amas(2)*(2+2*c2),amas(2)*(1+c2);
amas(2)*(1+c2),amas(2)];
V=[-dq2, -dq1-dq2; dq1, 0];
G = [amas(1)*GRAV*c1+amas(2)*GRAV*(c12+c1);amas(2)*GRAV*c12];
torq =b+db;
k1=[dq1;dq2;inv(M)*(torq-V*[dq1;dq2]*amas(2)*s2-G)];
end
26
27
28
p0=P(1:2,1:2);
p1=P(1:2,2+1:2+2);
p11=P(1:2,2+2+1:2+2+2);
l1 = beta2; r1=gamma2;
K1 = -inv(l1) * eye(2)' * p0 ;
K2 = -inv(l1) * eye(2)' * p1 ;
K3 = -inv(l1) * eye(2)' * p11;
%K0 = -inv(l1) * B2' * inv(A' - P*(B2*B2'/l1-B1*B1'/r1)) * P * B1 ;
Kp=Kp+K1;
Kv=Kv+K2;
Ki=K3;
Kp,Kv,Ki,pause
29
Xp = xx(1:2);
dXp= xx(3:4);
q1=xx(1); q2=xx(2); dq1=xx(3); dq2=xx(4);
t1=pi*tt;
s1=sin(t1); c1=cos(t1);
Xd=[1+.2*s1;1-.2*c1];
dXd=[.2*pi*c1;.2*pi*s1];
ddXd=[-.2*pi*pi*s1;.2*pi*pi*c1];
e=Xd-Xp;
de=dXd-dXp;
dei=e*T;
ei=ei+dei;
GRAV=9.8;
c1=cos(q1);c12=cos(q1+q2);
c2=cos(q2);s2=sin(q2);
M=[mas(1)+mas(2)*(2+2*c2),mas(2)*(1+c2);
mas(2)*(1+c2),mas(2)];
V=[-dq2, -dq1-dq2; dq1, 0]*[dq1;dq2]*mas(2)*s2;
G = [mas(1)*GRAV*c1+mas(2)*GRAV*(c12+c1);mas(2)*GRAV*c12];
30
b=M*(ddXd-Kv*de-Kp*e-Ki*ei)+V+G;
if tt >= 2;
amas(2)=mas(2)+1;
end
xx0=xx;
db=[0;0];
% db=[1;1]*sin(4*pi*tt)*5; % Disturbance
k1=cplt(b,xx,amas,db)*T;
xx=xx0+k1*0.5;
k2=cplt(b,xx,amas,db)*T;
xx=xx0+k2*0.5;
% db=[1;1]*sin(4*pi*(tt+T))*5;
k3=cplt(b,xx,amas,db)*T;
xx=xx0+k3;
k4=cplt(b,xx,amas,db)*T;
xx = xx0 + (k1 + 2 * k2 + 2 * k3 + k4) / 6;
end
d
XP(:,i)=Xp;
i
XD(:,i)=Xd;
i
d B(:,i)=b;
i b E(:,i)=e;
i
DE(:,i)=de;
i d
[e(1),e(2),norm(e)*10,norm(b),tt]
i=i+1;
tt=tt+T;
31
figure(1)
plot(t,E(1,:),t,E(2,:),'-.');grid
title('Position errors')
pause
figure(2)
plot(t,DE(1,:),t,DE(2,:),'-.');grid
title('Velocity errors')
pause
figure(3)
plot(t,XP(1,:),t,XD(1,:),'-.');grid
title('Link-1 positions');
pause
figure(4)
plot(t,XP(2,:),t,XD(2,:),'-.');grid
title('Link-2 positions');
pause
figure(5)
plot(t,B(1,:),t,B(2,:),'-.');grid
title('Torques')
il
pause
32
Computed-Torque PD
Robot Control
Co t o Simulation
S u at o with
w t S
Simulink
u
33
Computed-Torque PD
Robot Control
Co t o Simulation
S u at o with
w t S
Simulink
u
clear all;
home;
global m1 m2 L1 L2;
% Mass
Link2
[Kg]
m1 = 1; % at the end-point of gravity of Link1
m2 = 2;
% at the end-point of gravity of
% Length [m]
L1 = 1;
L2 = 1;
% of Link1
% of Link2
Computed-Torque PD
Robot Control
Co t o Simulation
S u at o with
w t S
Simulink
u
35
Computed-Torque PD
Robot Control
Co t o Simulation
S u at o with
w t S
Simulink
u
36
Computed-Torque PD
Robot Control
Co t o Simulation
S u at o with
w t S
Simulink
u
function torq = mvg(fbsig)
global
m1 m2 L1 L2
ddqs= [fbsig(1);fbsig(2)];
x(1)=fbsig(3);
x(2)=fbsig(4);
x(3)=fbsig(5);
x(4)=fbsig(6);
d11 = m1*L1*L1+m2*(L1*L1+L2*L2+2*L1*L2*cos(x(2)));
d12 = m2*L1*L2*cos(x(2))+m2*L2*L2;
( ( ))
;
d21 = d12;
d22 = m2*L2*L2;
D = [d11 d12; d21 d22];
h=m2*L1*L2*sin(x(2));
h=m2
L1 L2 sin(x(2));
v1 = [-h*x(4) -h*x(3)-h*x(4)];
v2 = [h*x(3) 0];
g1=m1*L1*9.8*cos(x(1))+m2*9.8*(L2*cos(x(1)+x(2))+L1*cos(x(1)));
g2=m2*L2*9.8*cos(x(1)+x(2));
2
2*L2*9 8* ( (1) (2))
G = [g1;g2];
torq = D*ddqs+[v1; v2]*[x(3); x(4)]+G;
end
37
Computed-Torque PD
Robot Control
Co t o Simulation
S u at o with
w t S
Simulink
u
function [sys, x0] = tlplnt(t, x, u, flag)
global
if flag == 0
m1 m2 L1 L2 Th1 Th2
mp1=m1; mp2=m2;
sys = [ 4, 0, 2, 2, 0, 0 ];
x0 = zeros(4, 1);
elseif flag == 3
sys = [x(1); x(2)];
elseif flag == 1
d11 = mp1*L1*L1+mp2*(L1*L1+L2*L2+2*L1*L2*cos(x(2)));
d12 = mp2*L1*L2*cos(x(2))+mp2*L2*L2;
d21 = d12;
d22 = mp2*L2*L2;
D = [d11 d12; d21 d22];
h=mp2*L1*L2*sin(x(2));
v1 = [-h*x(4) -h*x(3)-h*x(4)];
v2 = [h*x(3) 0];
V = [v1; v2]*[x(3); x(4)];
g1=mp1*L1*9.8*cos(x(1))+mp2*9.8*(L2*cos(x(1)+x(2))+L1*cos(x(1)));
g2=mp2*L2*9.8*cos(x(1)+x(2));
G = [g1;g2];
f x = inv(D)
f_x
inv(D)*(-V
( V - G);
b_x = inv(D)*u;
sys(1) = x(3);
sys(2) = x(4);
sys(3) = f_x(1) + b_x(1);
sys(4) = f_x(2)
f x(2) + b_x(2);
b x(2);
else
sys = [];
End
38
Computed-Torque PD
Robot Control
Co t o Simulation
S u at o Result
Resu t
39