End Term Simulation Project: Control of Robot Manipulator

Chinmay Garanayak, Entry No: 2016EEA2465

I. Classical PD (Pure feedback, no feed forward)

Here the classical proportional and derivative control law is implemented.
Mathematical expression of control Torque: 𝐓 = −𝐊𝐩 ∗ 𝐞 − 𝐊𝐝 ∗ 𝐞𝐝𝐨𝐭
e= error between measured angular position and desired trajectory.
edot = error between measured angular velocity and derivative of desired trajectory.
Values used for simulation: Kp=500, Kd=100

Simulation results:

0.5
e1
0.4 e2

0.3
Tracking error in angular posiion

0.2

0.1

-0.1

-0.2

-0.3

-0.4

-0.5
0 1 2 3 4 5 6 7 8 9 10
Time in sec

Fig.1.Plot of errors in angular positions

250
T1
T2
200

150
Torque as control input

100

50

-50

-100
0 1 2 3 4 5 6 7 8 9 10
Time in sec

Fig.2.Plot of control torque

 II. Exact Model Knowledge controller(Feedforward + PD
Feedback)

Let us assume system dynamics is known as well as all states are measurable.
Expression for Control Torque:
𝐓 = 𝐅 ∗ 𝐪𝐝𝐨𝐭 − 𝛂 ∗ 𝐌 ∗ 𝐞𝐝𝐨𝐭 + 𝐕 ∗ 𝐪𝐝𝐨𝐭 − 𝐕 ∗ 𝐫 − 𝐊 ∗ 𝐫 + 𝐌 ∗ 𝐪𝐝𝐝𝐝

Where: 𝑒 = 𝑞 − 𝑞𝑑
𝑟 = 𝑒 + 𝛼 ∗ 𝑒𝑑𝑜𝑡
𝑑𝑞 𝑑𝑞𝑑
𝑒𝑑𝑜𝑡 = −
𝑑𝑡 𝑑𝑡
dq
𝑞𝑑𝑜𝑡 = dt
𝑑2 𝑞𝑑
𝑞𝑑𝑑𝑑 = 𝑑𝑡 2

Values used for simulation: K=500, α=10

Simulation results:
0.5
e1
0.4 e2

0.3
Tracking error in angular position

0.2

0.1

-0.1

-0.2

-0.3

-0.4

-0.5
0 1 2 3 4 5 6 7 8 9 10
Time in sec

Fig.3.Plot of error in angular positions

2000 T1
T2

1500

1000
Torque as control input

500

-500

-1000

-1500

-2000
0 0.2 0.4 0.6 0.8 1 1.2
Time in sec

Fig.4.Plot of Control torque

 III. Adaptive Control
Assumptions here is all states are measurable and as system is linearly parametrizable.
We estimate the parameters to use in control law.
The regressor matrix is:
𝑦(1,1) 𝑦(1,2) 𝑦(1,3) 𝑦(1,4) 𝑦(1,5)
𝑦(2,1) 𝑦(2,2) 𝑦(2,3) 𝑦(2,4) 𝑦(2,5)
𝑌=[ ]

𝑦(1,1) = 𝑞𝑑1𝑑𝑑 + 𝛼 ∗ 𝑒𝑑𝑜𝑡1

𝑦(1,2) = 𝑞𝑑2𝑑𝑑 − 𝑠2 ∗ (𝑞1𝑑 + 𝑞2𝑑) ∗ 𝑞2𝑑 + 𝛼 ∗ 𝑒𝑑𝑜𝑡2
𝑦(1,3) = 𝑐2 ∗ 𝑞𝑑2𝑑𝑑 − 𝑠2 ∗ 𝑞2𝑑 ∗ 𝑞1𝑑 − 𝑠2 ∗ 𝑞2𝑑 ∗ 𝑟1 + 𝛼 ∗ 𝑐2 ∗ 𝑒𝑑𝑜𝑡2 − 𝑠2 ∗ (𝑞1𝑑 + 𝑞2𝑑) ∗ 𝑟2
+ 2 ∗ 𝑐2 ∗ 𝑞𝑑1𝑑𝑑 + 2 ∗ 𝛼 ∗ 𝑐2 ∗ 𝑒𝑑𝑜𝑡1
𝑦(1,4) = 𝑞1𝑑
𝑦(1,5) = 0
𝑦(2,1) = 0
𝑦(2,2) = qd1dd + qd2dd + alpha ∗ edot(1) + 𝛼 ∗ edot2
𝑦(2,3) = 𝑐2 ∗ 𝑞𝑑1𝑑𝑑 + 𝑠2 ∗ 𝑞1𝑑 ∗ 𝑞1𝑑 + 𝑠2 ∗ 𝑞1𝑑 ∗ 𝑟1 + 𝛼 ∗ 𝑐2 ∗ 𝑒𝑑𝑜𝑡1
𝑦(2,4) = 0
𝑦(2,5) = 𝑞2𝑑

Where qd1dd and qd2dd are the components of second derivative of qd, q1d and q2d are
components measured angular velocity, edot1 and edot2 are components of edot and r1 and
r2 are components of r and 𝑠2 = sin(𝑞2) , 𝑐2 = cos(𝑞2)
The parameter matrix is: θ = [p1 p2 p3 f1d f2d]′

The control law is: Torque= 𝒀 ∗ 𝜭𝒄𝒂𝒑 + 𝑲 ∗ 𝒓

𝒅𝜭𝒄𝒂𝒑
The update law is given by: = 𝒈𝒂𝒎𝒎𝒂 ∗ 𝒀′ ∗ 𝒓
𝒅𝒕

The gains chosen for simulation: K=100, 𝑔𝑎𝑚𝑚𝑎 = 10 , 𝛼 = 10

Simulation results:

e1
e2
0.2

0
Angular position tracking error

-0.2

-0.4

-0.6

-0.8

-1
0 2 4 6 8 10 12
Time in secs

Fig.5.error in angular position

 30
p1 error
p2 error
20 p3 error
fd1 error
fd2 error
Parameter estimation errors 10

-10

-20

-30

-40
0 10 20 30 40 50 60 70 80 90 100
Time in sec

Fig.6.Parameter estimation errors in Adaptive control

T1
4000
T2
3000

2000

1000
Torque as control input

-1000

-2000

-3000

-4000

-5000

-6000
0 0.2 0.4 0.6 0.8 1 1.2
Time in sec

Fig.7.Control Torque in Adaptive control

IV(a). DCAL-based Output Feedback Design

Semi-global result without saturating the feedback terms
Assume only positions q1(t) and q2(t) are available for measurement.
The filter tracking error is defined as:
𝑟 = 𝑒𝑑𝑜𝑡 + 𝑒𝑓 +e
𝑒𝑓 = −𝑘 ∗ 𝑒 + 𝑝
𝑝𝑑𝑜𝑡 = −(𝑘 + 1) ∗ 𝑝 + (𝑘 2 + 1) ∗ 𝑒
The regressor matrix:
𝑦(1,1) 𝑦(1,2) 𝑦(1,3) 𝑦(1,4) 𝑦(1,5)
𝑦(2,1) 𝑦(2,2) 𝑦(2,3) 𝑦(2,4) 𝑦(2,5)
𝑌𝑑 = [ ]

𝑦(1,1) = 𝑞𝑑1𝑑𝑑
𝑦(1,2) = 𝑞𝑑2𝑑𝑑
𝑦(1,3) = 𝑐𝑑2 ∗ 𝑞𝑑2𝑑𝑑 + 2 ∗ 𝑐𝑑2 ∗ 𝑞𝑑1𝑑𝑑 − 𝑠𝑑2 ∗ 𝑞𝑑1𝑑 ∗ 𝑞𝑑2𝑑 − 𝑠𝑑2 ∗ (𝑞𝑑1𝑑 + 𝑞𝑑2𝑑) ∗ 𝑞𝑑2𝑑
𝑦(1,4) = 𝑞𝑑1𝑑
𝑦(1,5) = 0
 𝑦(2,1) = 0
𝑦(2,2) = qd1dd + qd2dd
𝑦(2,3) = 𝑐𝑑2𝑑 ∗ 𝑞𝑑1𝑑𝑑 + 𝑠𝑑2 ∗ 𝑞𝑑1𝑑 ∗ 𝑞𝑑1𝑑
𝑦(2,4) = 0
𝑦(2,5) = 𝑞𝑑2𝑑
Here cd2,sd2,cd2d,sd2d,qd1d,qd2d,qd1dd,qd2dd are cos(𝑞𝑑2 ),sin(𝑞𝑑2 ),derivative of
cos(𝑞𝑑2 ), derivative of sin(𝑞𝑑2 ),derivative of 𝑞𝑑1 and 𝑞𝑑2 and double derivative of 𝑞𝑑1 and 𝑞𝑑2
respectively.

d𝑌 ′
T1dot = dt𝑑 ∗ e
T2dot = 𝑌𝑑 ′ ∗ e
𝑇3𝑑𝑜𝑡 = 𝑌𝑑 ∗ 𝑒𝑓

The update law for parameter estimation is given by:

𝜭𝒄𝒂𝒑 =𝜭𝒄𝒂𝒑 𝒛𝒆𝒓𝒐 + 𝒈𝒂𝒎𝒎𝒂 ∗ (𝒀′𝒅 ∗ (𝒆 − 𝒆𝒛𝒆𝒓𝒐) − 𝑻𝟏 + 𝑻𝟐 + 𝑻𝟑)
The Control torque is given by: 𝐓 = 𝒀𝒅 ∗ 𝜭𝒄𝒂𝒑 + 𝐞 − 𝐤 ∗ 𝒆𝒇
The gains chosen for simulation: 𝑘 = 5000, 𝑔𝑎𝑚𝑚𝑎 = 5

Simulation results:

e1
0.6 e2

0.4
Angular position tracking error

0.2

-0.2

-0.4
0 1 2 3 4 5 6 7 8
Time in secs

Fig.8.angular position error in DCAL design without saturating feedback

20
p1 error
15 p2 error
p3 error
fd1 error
10 fd2 error
Parameter estimation errors

-5

-10

-15

-20

-25
0 5 10 15 20 25 30 35 40 45 50
Time in sec

Fig.9.Parameter estimation errors in DCAL without saturating feedback

 7
x 10

T1
T2
0

-0.5
Torque as control input

-1

-1.5

-2

-2.5

0 0.01 0.02 0.03 0.04 0.05 0.06 0.07 0.08 0.09 0.1
Time in sec

Fig.10.Control Torque in DCAL based design without saturating feedback

IV(b). DCAL-based Output Feedback Design

Global result with saturated feedback

Assume only positions q1(t) and q2(t) are available for measurement.
The filter tracking error is defined as: 𝑟 = 𝑒𝑑𝑜𝑡 + tanh(𝑒𝑓 ) + tanh(𝑒)
𝑝𝑖 𝑑𝑜𝑡 = −(1 − (𝑝𝑖 − 𝑘 ∗ 𝑒𝑖 )2 ) ∗ (𝑝𝑖 − 𝑘 ∗ 𝑒𝑖 − tanh(𝑒𝑖 )) − 𝑘 ∗ (tanh(𝑒𝑖 ) + 𝑝𝑖 − 𝑘 ∗ 𝑒𝑖 )
𝑦 =𝑝−𝑘∗𝑒
𝑟𝑜𝑤𝑑𝑜𝑡 = −𝑌𝑑 𝑑𝑜𝑡 ′ ∗ 𝑒 + 𝑌𝑑′ ∗ (𝑇𝑎𝑛ℎ(𝑒) + 𝑦)
𝑻𝒉𝒆𝒕𝒂𝒄𝒂𝒑 = 𝒈𝒂𝒎𝒂 ∗ 𝒀′𝒅 ∗ 𝒆 + 𝒈𝒂𝒎𝒂 ∗ 𝒓𝒐𝒘
𝟐
𝑻𝒐𝒓𝒒𝒖𝒆 = 𝒀𝒅 ∗ 𝑻𝒉𝒆𝒕𝒂𝒄𝒂𝒑 + 𝑻𝒂𝒏𝒉(𝒆) − 𝒌 (𝐜𝐨𝐬𝐡(𝒆𝒇 )) ∗ 𝑻𝒂𝒏𝒉(𝒆)

Simulation results:

1.5
e1
e2

1
Angularposition tracking error

0.5

-0.5

-1
0 50 100 150 200 250
Time in sec

Fig.11. Angular position tracking error in OFD with saturated feedback

 25
p1 error
p2 error
20
p3 error
fd1 error
fd2 error
Parametre estimation errors 15

10

-5

-10

-15
0 50 100 150 200 250
Time in sec

Fig.12.Parameter estimation errors in OFD with saturated feedback

50
T1
40 T2

30

20
Torque as control input

10

-10

-20

-30

-40

0 20 40 60 80 100 120 140

Time in sec

Fig.13.Control Torque in OFD based design with saturated feedback

V. Discussion/Observation
In the classical PD controller the control objective is achieved but the error is varying
sinusoidally about zero with very low amplitude.
In exact-model based design the error transient dies faster and error variation from zero is
very less w.r.t PD controller performance.
In adaptive control design the error transient dynamics is underdamped type where as in the
previous two cases it is almost over damped type and error variation from zero is very less
w.r.t classical PD controller.
The starting control torque in exact-model based controller is much higher than in classical
PD controller. The starting control torque in adaptive control is higher than the previous two
design.
In adaptive control the parameters are converging to zero after some time.
In DCAL semi-global result case the error is converging to zero with much faster rate but
control input is too high i.e. it is higher than all the previous designs and in this case the
parameter estimation errors are oscillating but are bounded.
In DCAL global result with saturated feedback design the error is converging to zero but at
starting there is a reasonable amount of transient. The parameter estimation errors also are
converging to zero with some transient at starting. In this case the starting control torque is
lesser than all the previous case and it is good for practical implementation because it will not
saturate the actuator if implemented. In steady state it is varying sinusoidally.
 VI. APPENDIX:
Code for Classical PD (Pure feedback, no feed forward):
function Solve=ControlRM1(t,x)
p1=3.473;p2=0.196;p3=0.242;fd1=5.3;fd2=1.1;kp1=500;kp2=500;kd1=100;kd2=100;
q1=x(1);
q2=x(2);
q1dot=x(3);
q2dot=x(4);
c2=cos(q2);
s2=sin(q2);
M=[p1+2*p3*c2 p2+p3*c2;
p2+p3*c2 p2];
V=[-p3*s2*q2dot -p3*s2*(q1dot+q2dot);
p3*s2*q1dot 0];
F=[fd1 0;
0 fd2];
q1d=2*sin(t);
q2d=cos(t);
q1dd=2*cos(t);
q2dd=-sin(t);
q=[q1;q2];
qdot=[q1dot;q2dot];
qd=[q1d;q2d];
qdotd=[q1dd;q2dd];
e=q-qd;
edot=qdot-qdotd;
Kp=[kp1 0;
0 kp2];
Kd=[kd1 0;
0 kd2];
T=-(Kp*e+Kd*edot);
Func=(M^-1)*(T-F*qdot-V*qdot);
Solve=[x(3);x(4);Func(1);Func(2)];
End

function [t,x]=ControlRM1_plot(x1i,x2i,x3i,x4i,n)
f=[x1i,x2i,x3i,x4i];
g=[0,n];
[t,x]=ode45(@ControlRM1,g,f);
figure(1)
plot(t,x(:,1),'r');
hold on
plot(t,x(:,2),'g');
legend('q1','q2');
figure(2)
plot(t,x(:,3),'r');
hold on
plot(t,x(:,4),'g');
legend('q1dot','q2dot');
kp1=500;
kd1=100;
kp2=500;
kd2=100;
q1d=2*sin(t);
q2d=cos(t);
q1dd=2*cos(t);
q2dd=-sin(t);
q1dot=x(:,3);
q2dot=x(:,4);
q1=x(:,1);
q2=x(:,2);
e1=q1-q1d;
e2=q2-q2d;
edot1=q1dot-q1dd;
edot2=q2dot-q2dd;
T1=-(kp1*e1+kd1*edot1);
T2=-(kp2*e2+kd2*edot2);
figure(3)
plot(t,T1,'r');
hold on
plot(t,T2,'g');
legend('T1','T2');
figure(4)
plot(t,e1,'r');
hold on
plot(t,e2,'g');
legend('e1','e2');
end

Code for Exact Model Knowledge controller(Feedforward + PD Feedback)

function Solve=ControlRM2(t,x)
p1=3.473;p2=0.196;p3=0.242;fd1=5.3;fd2=1.1;k1=500;k2=500;alpha=10;
q1=x(1);
q2=x(2);
q1dot=x(3);
q2dot=x(4);
c2=cos(q2);
s2=sin(q2);
M=[p1+2*p3*c2 p2+p3*c2;
p2+p3*c2 p2];
V=[-p3*s2*q2dot -p3*s2*(q1dot+q2dot);
p3*s2*q1dot 0];
F=[fd1 0;
0 fd2];
q1d=2*sin(t);
q2d=cos(t);
q1dd=2*cos(t);
q2dd=-sin(t);
q1ddd=-2*cos(t);
q2ddd=-cos(t);
q=[q1;q2];
qdot=[q1dot;q2dot];
qd=[q1d;q2d];
qdotd=[q1dd;q2dd];
qddd=[q1ddd;q2ddd];
e=q-qd;
edot=qdot-qdotd;
K=[k1 0;
0 k2];
r=edot+alpha.*e;
T=F*qdot-alpha.*M*edot+V*qdot-V*r-K*r+M*qddd;
plot(t,T(1),'r')
hold on ;
plot(t,T(2),'g')
hold on;
Func=(M^-1)*(-alpha.*M*edot-V*r-K*r+M*qddd);
Solve=[x(3);x(4);Func(1);Func(2)]end
function [t,x]=ControlRM2_plot(x1i,x2i,x3i,x4i,n)
f=[x1i,x2i,x3i,x4i];
g=[0,n];
[t,x]=ode45(@ControlRM2,g,f);
figure(1)
plot(t,x(:,1),'r');
hold on
plot(t,x(:,2),'g');
legend('q1','q2');
figure(2)
plot(t,x(:,3),'r');
hold on
plot(t,x(:,4),'g');
legend('q1dot','q2dot');
q1d=2*sin(t);
q2d=cos(t);
q1=x(:,1);
q2=x(:,2);
e1=q1-q1d;
e2=q2-q2d;
figure(3)
plot(t,e1,'r');
hold on
plot(t,e2,'g');
legend('e1','e2');
end

Code for Adaptive Control

Simulink model:

System:
function qdd = fcn(torque,q1,q2,q1d,q2d)
p1=3.473;p2=0.196;p3=0.242;fd1=5.3;fd2=1.1;c2=cos(q2);s2=sin(q2);
M=[p1+2*p3*c2 p2+p3*c2;p2+p3*c2 p2];
V=[-p3*s2*q2d -p3*s2*(q1d+q2d);p3*s2*q1d 0];
F=[fd1 0;0 fd2];
qd=[q1d;q2d];
qdd=M^-1*(-V*qd-F*qd+torque);
end
Controller:
function [torque,thetacap_dot,e] = fcn(t,thetacap,q1,q2,q1d,q2d)
alpha=10;
K=100;
gama=10*eye(5);
s2=sin(q2);
c2=cos(q2);
qd1=2*sin(t);
qd2=cos(t);
qd=[qd1;qd2];
q=[q1;q2];
e=qd-q;
qd1d=2*cos(t);
qd2d=-sin(t);
qdd=[qd1d;qd2d];
qdot=[q1d;q2d];
edot=qdd-qdot;
r=edot+alpha.*e;
qd1dd=-2*sin(t);
qd2dd=-cos(t);
Y=[qd1dd+alpha*edot(1) qd2dd-s2*(q1d+q2d)*q2d+alpha*edot(2) c2*qd2dd-
s2*q2d*q1d-s2*q2d*r(1)+alpha*c2*edot(2)-
s2*(q1d+q2d)*r(2)+2*c2*qd1dd+2*alpha*c2*edot(1) q1d 0;
0 qd1dd+qd2dd+alpha*edot(1)+alpha*edot(2)
c2*qd1dd+s2*q1d^2+s2*q1d*r(1)+alpha*c2*edot(1) 0 q2d];

torque=Y*thetacap+K.*r;
thetacap_dot=gama*Y'*r;
end

Code for DCAL-based Output Feedback Design

Semi-global result without saturating the feedback terms
Simulink model:

Controller:
function [torque,t1dot,t2dot,t3dot,pdot,thetacap,e] =
fcn(t,q1,q2,t1,t2,t3,p)
K=5000;
gama=5*eye(5);
thetacapzero=[1;1;1;1;1];
ezero=[-1;0];
qd1=2*sin(t);
 qd2=cos(t);
sd2=sin(qd2);
cd2=cos(qd2);
qd=[qd1;qd2];
q=[q1;q2];
e=qd-q;
qd1d=2*cos(t);
qd2d=-sin(t);
qdd=[qd1d;qd2d];
pdot=-(K+1).*p+(K^2+1).*e;
ef=-K.*e+ p;
qd1dd=-2*sin(t);
qd2dd=-cos(t);
qd1ddd=-2*cos(t);
qd2ddd=sin(t);
sd2d=cd2*qd2d;
cd2d=-sd2*qd2d;
Yd=[qd1dd qd2dd cd2*qd2dd+2*cd2*qd1dd-sd2*qd1d*qd2d-sd2*(qd1d+qd2d)*qd2d
qd1d 0;
0 qd1dd+qd2dd cd2*qd1dd+sd2*qd1d^2 0 qd2d];
Yddot=[qd1ddd qd2ddd cd2d*qd2dd+cd2*qd2ddd+2*cd2d*qd1dd+2*cd2*qd1ddd-
sd2d*qd1d*qd2d-sd2*qd1dd*qd2d-sd2*qd1d*qd2dd-sd2d*(qd1d+qd2d)*qd2d-
sd2*(qd1dd+qd2dd)*qd2d-sd2*(qd1dd+qd2dd)*qd2dd qd1dd 0;
0 qd1ddd+qd2ddd cd2d*qd1dd+cd2*qd1ddd+sd2d*qd1d^2+sd2*2*qd1dd 0
qd2dd];
t1dot=Yddot'*e;
t2dot=Yd'*e;
t3dot=Yd'*ef;
thetacap=thetacapzero+gama*(Yd'*(e-ezero)-t1+t2+t3);
torque=Yd*thetacap+e-K.*ef;
end

Code for DCAL-based Output Feedback Design Global result with saturated feedback
Simulink model:

Controller:
function [torque,pdot,rowdot,thetacap,e] = fcn(t,q1,q2,p,row)
K=.1;
gama=5*eye(5);
qd1=2*sin(t);
qd2=cos(t);
sd2=sin(qd2);
cd2=cos(qd2);
qd=[qd1;qd2];
 q=[q1;q2];
e=qd-q;
qd1d=2*cos(t);
qd2d=-sin(t);
qdd=[qd1d;qd2d];
qd1dd=-2*sin(t);
qd2dd=-cos(t);
qd1ddd=-2*cos(t);
qd2ddd=sin(t);
sd2d=cd2*qd2d;
cd2d=-sd2*qd2d;
Yd=[qd1dd qd2dd cd2*qd2dd+2*cd2*qd1dd-sd2*qd1d*qd2d-sd2*(qd1d+qd2d)*qd2d
qd1d 0;
0 qd1dd+qd2dd cd2*qd1dd+sd2*qd1d^2 0 qd2d];
Yddot=[qd1ddd qd2ddd cd2d*qd2dd+cd2*qd2ddd+2*cd2d*qd1dd+2*cd2*qd1ddd-
sd2d*qd1d*qd2d-sd2*qd1dd*qd2d-sd2*qd1d*qd2dd-sd2d*(qd1d+qd2d)*qd2d-
sd2*(qd1dd+qd2dd)*qd2d-sd2*(qd1dd+qd2dd)*qd2dd qd1dd 0;
0 qd1ddd+qd2ddd cd2d*qd1dd+cd2*qd1ddd+sd2d*qd1d^2+sd2*2*qd1dd 0
qd2dd];
p1dot=-(1-((p(1)-K*e(1)))^2)*(p(1)-K*e(1)-tanh(e(1)))-K*(tanh(e(1))+p(1)-
K*e(1));
p2dot=-(1-((p(2)-K*e(2)))^2)*(p(2)-K*e(2)-tanh(e(2)))-K*(tanh(e(2))+p(2)-
K*e(2));
pdot=[p1dot;p2dot];
y=p-K.*e;
thetacap=gama*Yd'*e+gama*row;
rowdot=-Yddot'*e+Yd'*([tanh(e(1));tanh(e(2))]+y);
cosh2ef=[1/(1-y(1)^2) 0;
0 1/(1-y(2)^2)];
torque=Yd*thetacap+[tanh(e(1));tanh(e(2))]-
K.*cosh2ef*[tanh(e(1));tanh(e(2))];
end