Upload
nguyen-vo-ngoc-thach
View
121
Download
4
Tags:
Embed Size (px)
DESCRIPTION
Computed Torque Control
Citation preview
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