Dynamic Equations of Motion for a Two-Link Robotic ManipulatorTwo Link Robotic Manipulator

Mechanical Systems - Translational motion

x(t)

K

f(t)

K

MMass - kinetic energyStiffness - potential energy Damper Friction Damper , Friction

Equations of Mechanical Systems

Differential equation

2

2

( ) ( ) ( )( ) d x t dx t Kx tdt

tdt

f M

Dynamic equation

( ) ( ) ( ) ( )Mx t x t Kx t f t ( ) ( ) ( ) ( )Mx t x t Kx t f t Transfer function

( ) 1X2

( ) 1( )

X sF s Ms s K

3

Two-link manipulator with point masses at distal end of links

A simple two-rigid-link robot manipulator shown in A simple two rigid link robot manipulator shown in

Figure, and modelled as a set of nonlinear coupled

differential equations to use as a vehicle for

fi i ll th l ith d l d i thi b kconfirming all the algorithms developed in this book.

Although it is a simple two-link manipulator,

the dynamic equations of the manipulator are

multi-variable, highly non-linear, and contain

frictions, disturbances, unknown and/or time-varying , , / y g

Paremeters.

The model illustrates all of the necessary features,

itho t being er complicated for testing thewithout being very complicated, for testing the

control algorithms developed in this book.

We use these equations in simulations throughout the

book.

4

Dynamic Equations

For simplicity, we assume that all mass exists as a point mass at the distal end of each link.

masses of each link lengths of each link

The equations of motion for this manipulator are

21,m m 21,l l

2 22 21 2 2 1 2 2 1 2 2 1 2 1 2 1 1

22 1 2 2 2 2 1 2 2 1 2 2 2 12 1 2 1 1

( ) (2 ) ( )2 ( )

( )

m l q q m l l c q q m m l qm l l s q m l l s q q m l qs m m l gs

v q k sgn q

1 1 1 1

2 22 2 2 1 2 2 1 2 2 1 2 1 2 2 1 2 2 12

( )

( )

v q k sgn q

m l q q m l l c q m l l s q m l gs

where etc

2 2 2 2( )v q k sgn q

: cos( )c q : sin( )s q q where , , etc..

The parameters and are viscous and Coulomb friction coefficients, respectively.

1 1: cos( )c q 12 1 2: sin( )s q q

iv ik

5

Form of Dynamic Equations

The equation can be written in the form of

dMq Vq G Fq where

2 2 211 1 1 2 1 2 1 2 2

11 12 212 2 1 2 2 2 2

[ 2 ( )], with ( )

M m l m l l l l cos qM M

M M m l l cos q m l

12 2 1 2 2 2 2

21 22 222 2 2

, with ( )M M m l l cos q m lM M

M m l

2 1 22 1 2 2, with ( )

hq hq hqV h m l l sin q

2 1 2 2

1

, with ( )0

V h m l l sin qhq

1 1 1 1 2 2 1 2 1 1( ) [ ( ) ( )]( )

G m l gcos q m g l cos q q l cos qG

G m l gcos q q

2 2 2 1 2( )G m l gcos q q

1 1 1

2 2 2

F v qF

F v q

actuator torque vectorn position, velocity,ac, c, elerationnq q q 2 2 2q

1 1 1

2 2 2

( )( )

dd

d

k sgn qk sgn q

position, velocity,ac, c, elerationq q qmas( s mat ix) rn nM q centrifugal and Coriolisforcevect r( , ) onV q q

gravity torque( ector) vnG q 2 2 2( )d k sgn q

6

gravity torque ( ector) vG q joint friction torque( ) vectornF q

uncertainty torque vectornd

Computed Torque Controlof Robot Manipulatorsof Robot Manipulators

Mechanical Systems - Translational motion

x(t)

K

f(t)

K

MMass - kinetic energyStiffness - potential energy Damper Friction Damper , Friction

open-loop dynamic equation

mx bx kx f (2)

8

Computed Torque Control Design Step 1

Two-Step Design

Step 1

Asume that m, b, k are known

model based control law

Step 1

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)

s bf f x kxm (3)s

9

Structure of Contpued Torque Control

where is the new-input of the system as shown in the figure.

Then, the system will be simplyfied as mass-only second-order systemsfy p y y y

sx f (4)sf ( )

m f=mx+bx+kx

x

x.

ffs .. .

bx+kx.

10

Step 2 of Computed Torque Control

Step 2

Step 1: Simplify the system

Step 2: Apply classical control law. Ex. PID, Pole-placement

Apply servo contorl law to (4)For example,

(5):s d v p dx K e e x uf K

We see that is the PD(position-defivative) feedback loop.v pu K e K e

11

Structure of Computed Torque Control

Plant: Eq.(2)

Model-based Control law: Eq.(3)

Servo-contorl law: Eq. (5)

= Computed Torque Control System

xfxd.. fs

m f=mx+bx+kx

x.

fd s .. .

_xd

xd.

bx+kxKp Kv

.

d _

12

Computed torque control of manipulators

Dynamic equations of robot manipulatorDynamic equations of robot manipulator

( ,( ) ) ( )M q q G qq V q (10)

13

PD feedback loop Computed Torque Control

Model-based control law of Eq.(10)

( )( ) ( , ) ( )dM q q u V q q G q

Design with PD(propotional-plus-derivative) feedback loop( )u t

(11)

Apply Eq (12) to Eq (11) the control law will be

v pu K e K e (12)

Apply Eq.(12) to Eq. (11), the control law will be

( )( ) ( , ) ( )d v pM q q K e K e V q q G q (13)

14

PD feedback loop Computed Torque Contorl

Apply the control law (13) to the plant (10) will result the system closed-loop error dynamic as

State-space form

0v pe K e K e

0

p v

Ie edK Ke edt

Close-loop characteristic polynomial

2( | |) I K s Ks s ( | |)c v pI K s Ks s

Total system

15

PD feedback loop computed torque control

Total system

q..

M(q) M(q)+V(q,q)+G(q).

q

q.

V(q,q)+G(q)K

Ki

K1

qd

_qd

qd.

V(q,q) G(q)Kp Kv1s

_

[Figure] Computed-torque controller(a) Task space PD controller(b) Joint space PID controllerp

16

PID feedback loop computed torque control

PD computed-torque control is usful if system parameters are known, no disturbance, --- which are impossibleexist steady state errorexist steady-state error

-> feedforwaard loop + integrator = PID computed-torque controller

-> Eq. (12) will become

e (15)

v p ie K e Ku K

Total control law

p

( )( ) ( , ) ( )d v p iM q q K e K e K V q q G q (16)

= intgral of tracking error( )t ( )e t

17

PID feedback loop Computed torque control

state-space form of closed loop error dynamics

0 00 0

Id e I edt

e K K K e

i p ve K K K e

Closed -loop characteristic polynomial

2 |( ) |c v p iIs K Kss K

diagonal control gain , ,d p iK K K

18

PID feedback loop computed torque control

Apply the control law to the plant, the the total system block diagram will be

M(q) M(q)+V(q,q)+G(q).

q

q.

qd..

qd.

V(q,q)+G(q)Kp

Ki

Kv1s

_qd _

[Figure] Computed-torque controllerJoint space PID controller

19

Computed-Torque Robot controller Characteristics

Robot dynamics

Tracking error

( ,( ) ) ( )M q q G qq V q

(( )) ) (te q q tt Tracking error (( )) ) (d te q q tt

PD Computed Torque control law and closed-loop error dynamics (state space form)

( )( ) ( , ) ( )d v pM q q K e K e V q q G q

0 Ie edK Kd

p vK Ke edt

PD-Gravity Control

( )v pK e K e G q

Classical PD Control

v pK e K e

20

Computed-Torque Robot controller Characteristics

PID Computed Torque control law

( )( ) ( , ) ( )d v p iM q q K e K e K V q q G q e

closed-loop error dynamics (state space form)

0 00 0

Id e I edt

Classical PID Control

i p ve K K K e

Classical PID Control

e v p iK e K e K p

21

Computed-Torque PID Robot Control MATLAB SimulationS u at o

function [XP,XD,E,DE,B]=ctorq2(T1,T)

% Computed torque control% CTORQ(simulation_time, sampling_time)

mas=[2;2];amas=mas;ma1=mas;l1=1;l2=1;GRAV=9.8;

t=linspace(0,T1,T1/T);nu=length(t);

% PID GainsKv=-[22.7 0;0 41.2]; Kp=-[14.6 0;0 14.8];Ki=-[100 0;0 100];

% initial statexx=[1;0.8;.2*pi;0];e=[0;0]; ei=[0;0];

tt=0;tt1=0;i=1;

22

Computed-Torque PID Robot Control MATLAB SimulationS u at o

for i=1:nuXp = 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;

c1=cos(q1);c12=cos(q1+q2);ic2=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 1 G 1 2 G 12 1 2 G 12G = [mas(1)*GRAV*c1+mas(2)*GRAV*(c12+c1);mas(2)*GRAV*c12];

23

Computed-Torque PID Robot Control MATLAB SimulationS u at o

b=M*(ddXd-Kv*de-Kp*e-Ki*ei)+V+G;

if tt >= 2;amas(2)=mas(2)+1;

endxx0=xx;db=[0;0];

% db=[1;1]*sin(4*pi*tt)*5; % Disturbancek1=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;i i d i b i i dXP(:,i)=Xp; XD(:,i)=Xd; B(:,i)=b; E(:,i)=e; DE(:,i)=de;

[e(1),e(2),norm(e)*10,norm(b),tt]i=i+1;

tt=tt+T;dend

24

Computed-Torque PID Robot Control MATLAB SimulationS u at o

figure(1)plot(t,E(1,:),t,E(2,:),'-.');gridtitle('Position errors')pausefigure(2)plot(t,DE(1,:),t,DE(2,:),'-.');gridtitle('Velocity errors')pausefigure(3)plot(t,XP(1,:),t,XD(1,:),'-.');gridtitle('Link-1 positions');pausefigure(4)plot(t,XP(2,:),t,XD(2,:),'-.');gridtitle('Link-2 positions');pausefigure(5)plot(t,B(1,:),t,B(2,:),'-.');gridi ltitle('Torques')pause

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

Computed-Torque Robot Control PID via MATLAB Simulation

HS u at o

function [XP,XD,E,DE,B]=ctorq_hi(T1,T,beta, gamma)

% Computed torque control - H_infinity

% CTORQ(simulation_time, sampling_time, beta, gamma)

mas=[2;2];amas=mas;ma1=mas;l1=1;l2=1;GRAV=9.8;

t=linspace(0,T1,T1/T);p ( , , / );

nu=length(t);

tt=0;tt1=0;i=1;

% initial state

xx=[1;0.8;.2*pi;0];

e=[0;0]; ei=[0;0];

27

Computed-Torque Robot Control PID via MATLAB Simulation

HS u at o

% PID Gain find using H_infinity theory

% 1. make stable system with some gainsKv=[-4 0;0 -4]; Kp=[-5 0;0 -5];

% 2. get K using H_infty methodbeta2=beta*beta;gamma2=gamma*gamma;A = [eye(2)*0, eye(2), eye(2)*0;Kp, Kv, eye(2)*0;eye(2),eye(2)*0,eye(2)*0];B1= [eye(2)*0;eye(2);eye(2)*0];B2= [eye(2)*0;eye(2);eye(2)*0];C1 = [eye(2)*0, eye(2)*0, eye(2)];C = [1 1 1 1];Q=eye(6);%[k,P]=lqr(A, B1, eye(4), beta2); %for H_2 %Q=C'*C;

l f bP=lqgf(A, B2*B2'/beta2 - B1*B1'/gamma2, Q);if min(eig(P)) < 1e-10;error('r too small, stop'),endP,pause

28

Computed-Torque Robot Control PID via MATLAB Simulation

HS u at o

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

Computed-Torque Robot Control PID via MATLAB Simulation

HS u at o

for i=1:nuXp = 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

Computed-Torque Robot Control PID via MATLAB Simulation

HS u at o

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; % Disturbancek1=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;

i i d i b i i dXP(:,i)=Xp; XD(:,i)=Xd; B(:,i)=b; E(:,i)=e; DE(:,i)=de;[e(1),e(2),norm(e)*10,norm(b),tt]i=i+1;tt=tt+T;

dend

31

Computed-Torque Robot Control PID via MATLAB Simulation

HS u at o

figure(1)plot(t,E(1,:),t,E(2,:),'-.');gridtitle('Position errors')pausefigure(2)plot(t,DE(1,:),t,DE(2,:),'-.');gridtitle('Velocity errors')pausefigure(3)plot(t,XP(1,:),t,XD(1,:),'-.');gridtitle('Link-1 positions');pausefigure(4)plot(t,XP(2,:),t,XD(2,:),'-.');gridtitle('Link-2 positions');pausefigure(5)plot(t,B(1,:),t,B(2,:),'-.');gridi ltitle('Torques')pause

32

Computed-Torque PD Robot Control Simulation with SimulinkRobot Co t o S u at o w t S u

33

Computed-Torque PD Robot Control Simulation with SimulinkRobot Co t o S u at o w t S u

clear all;home;home;

global m1 m2 L1 L2;

% Mass [Kg]% Mass [Kg]m1 = 1; % at the end-point of gravity of Link1m2 = 2; % at the end-point of gravity of

Link2

% Length [m]L1 = 1; % of Link1 L2 = 1; % of Link2

% Threshold of Friction Function [Nm]Th1 = 5.50; % of Motor1Th2 = 0.90; % of Motor2

disp('Initialization is done');

34

Computed-Torque PD Robot Control Simulation with SimulinkRobot Co t o S u at o w t S u

35

Computed-Torque PD Robot Control Simulation with SimulinkRobot Co t o S u at o w t S u

36

Computed-Torque PD Robot Control Simulation with SimulinkRobot Co t o S u at o w t S u

function torq = mvg(fbsig)

global m1 m2 L1 L2global 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)));2 2*L2*9 8* ( (1) (2))g2=m2*L2*9.8*cos(x(1)+x(2));

G = [g1;g2];

torq = D*ddqs+[v1; v2]*[x(3); x(4)]+G;

end

37

Computed-Torque PD Robot Control Simulation with SimulinkRobot Co t o S u at o w t S u

function [sys, x0] = tlplnt(t, x, u, flag)

global m1 m2 L1 L2 Th1 Th2mp1=m1; mp2=m2;

if flag == 0sys = [ 4, 0, 2, 2, 0, 0 ];x0 = zeros(4, 1);

elseif flag == 3sys = [x(1); x(2)];sys [x(1); x(2)];

elseif flag == 1d11 = 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;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)*(-V - G);f_x inv(D) ( 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) + b x(2);sys(4) = f_x(2) + b_x(2);

else sys = [];

End 38

Computed-Torque PD Robot Control Simulation ResultRobot Co t o S u at o Resu t

39