PUMA Robot Project: Designing Controllers and Trajectory Planners

This project focuses on designing and implementing independent joint controllers and a trajectory planner for the first three degrees of freedom of a PUMA robot using MATLAB and SIMULINK. The project begins with calculating controller gains (Kp and Kv) for joints 1, 2, and 3, considering effective moment of inertia and damping ratios. These gains are then used to design a feedback controller and implement it in SIMULINK. A joint space trajectory planner is developed to accept user-specified initial and final joint positions and joint speeds. The results, including plots of actual and desired joint angles, velocity, and joint errors versus time, are analyzed to assess the robot's performance. The project also includes the coding of large and small movements using conditional if statements in MATLAB. The discussion highlights the observed errors and provides insights into the robot's behavior. Finally, the conclusion states that the objectives of the experiment were met, despite some minor errors.

Table of Contents
objectives…………………………………………………………………………………………………….. 2
Methodology…………………………………………………………………………………………….. 2
Calculating controller gains(Kp&Kv) for joints 1,2 and 3……………… …………….2
Maximum Velocity and Acceleration (Nominal)………………………….………………3
Finding Large and Small movement………………………………………………..………….3
Finding Large and Small movements (MATLAB)………………………….………………3
Plotting the results…………………………………………………………………………………….4
Designing controllers and trajectory planner………………………………………………5
Discussion………………………………………………………………………………………………….5
Coding the Large and Small movements (MATLAB)……………………………………5
Conclusions……………………………………………………………………………………………….6
Appendix A………………………………………………………………………………………………..6
 MATLAB/SIMULINK
Appendix
B………………………………………………………………………………………………………………12
 Plots/Outputs

Objectives:
The main objective of project was to design and implement robot’s
independent joints controllers as well as its trajectory joint planner. This
project is implemented using Matlabs SIMULINK, thus allowing it to input initial
and final angles of the robots angles. It also inputs maximum joint speed then
performs trajectory plan to be used by PUMA robot.
Methodology
To achieve the objectives of this project, the plan for achieving is by; First,
performing calculations to find controller gains KV and Kp for the three joints, 1,
2 and 3. The controller gain calculated above is then used to design a feedback
controller for the PUMA robot. Secondly, the controller designed above is
implemented using SIMULINK. Thirdly, space trajectory planner is developed
and implemented using Simulink then incorporated into the controller. The
planner will then accept user’s final and initial joint angles and lastly the
velocity of the joints. The results from MATLAB were then used to analyse the
performance of the PUMA robot, from the plots we were able to get the
desired and actual joints angle positions, velocity versus time and finally joints
errors versus time.
Controller Gains Kp and Kv calculations for joints 1, 2 and 3
The first step to achieving the objectives of the project is to calculate the
controllers gain Kv and Kp. The equations below were used to perform the
operations.
Tm= Ra∗Jeff
( Ra∗Feff ) + ( Ka∗Kb ) K= Ka
Ra Feff + ( Kb∗Ka )
Jeff =Jm +n 2 Jl where Jeff is the Effective moment of inertia)
2𝜁𝜔𝑛 = Kv K +1
Tm using the above equations Kv is found as shown below
Kv= Ra Jeff ∗2 ζωn
Ka − Ra feff +Kb Ka
Ka
2 ζωn= KKp
Tm from this equation the equation below is found
1

Kp= 2 RaJeff wn
Ra
Definitions of terms
Variable Definition
Ra Armature Resistance of the motor
Feff Effective Viscous Friction Coefficient
Ka Torque Constant of the motor
Kb Torque Constant of the motor
Jm Armature’s Moment of inertia
n Gear Ratio
𝜔n Natural Frequency
𝜁 Damping Ratio
Calculations for Joint 1
𝐽𝐿11 = 𝐼1𝑦𝑦 + 𝑚1 (𝑥1 2 + 𝑧1 2 )
𝐽𝐿12 = 𝐼2𝑦𝑦 + 𝑚2 [(𝑑2 + 𝑧2 )2 + (𝑎2 + 𝑥1 )2 ]
𝐽𝐿13 = 𝐼3𝑥𝑥 + 𝑚3 [(𝑦3 + 𝑑2 )2 + (𝑧3 + 𝑎2 )2 ]
𝐽𝐿𝑙𝑜𝑎𝑑 = [((𝑎2 + 𝑑4 + 𝑑6 )2 ) + (𝑑2 2 )]
𝐽𝐿1 = 𝐽𝐿11 + 𝐽𝐿12 + 𝐽𝐿13 + 𝐽𝐿𝑙𝑜𝑎𝑑
∴ 𝐾𝑝1 = 13.71957352 - this is the Proportional controller gain for joint 1
∴ 𝐾𝑣1 = 0.55709906 -Velocity controller gain for joint 1
Calculations for Joint 2
2𝐽𝐿22 = 𝐼2𝑧𝑧 + 𝑚2 [(𝑦2 + (𝑎2 + 𝑥2 )2 ]
𝐽𝐿23 = 𝐼3𝑦𝑦 + 𝑚3 [(𝑧3 + 𝑎2 )2 + (𝑥3 + 𝑎3 )2 ]
2𝐽𝐿 𝑙𝑜𝑎𝑑 = [((𝑎2 + 𝑑4 + 𝑑6 )2 ) + (𝑎3 )]
𝐽𝐿2 = 𝐽𝐿22 + 𝐽𝐿23 + 𝐽𝐿𝑙𝑜𝑎𝑑
∴ 𝐾𝑝2 = 4.96983111 - this is the proportional controller gain for Joint 2
∴ 𝐾𝑣2 = 0.03168122 -this is the velocity controller Gain for Joint 2
Calculations for Joint 3
2𝐽𝑙3 = (𝐼3𝑦𝑦 + (𝑚3 )) × (𝑧3 + (𝑎3 + 𝑥3 )2 ) + ((𝑑4 + 𝑑6 )2 + (𝑎3 )2 )
∴ 𝐾𝑝3 = 2.67638911 - this is the proportional gain for joint 3
∴ 𝐾𝑣3 = −0.10168061 – this is the velocity gain for joint 3
Maximum Velocity and Acceleration for the joints
Nominal maximum acceleration and velocity of PUMA robot’s three joints is
required for computation purposes. From the equations below nominal
maximum velocity of any of the three joints can be found
2

V =iRa + Kbθ̇ thus ˙θ=V −iRa
Kb (assuming i=0)
And from the equations below the nominal maximum acceleration is found:
𝑇= 𝐽𝑒𝑓𝑓 * 𝜃 thus, =̈ ̈ T
Jeff
Definition of terms
Variable Definition
V Input Voltage of the motor
i Current flow through the armature
𝜃̇ Angular velocity of the motor
𝜃̈ Angular acceleration of the motor
T Torque
What follows next is the movement calculations attributed to the motor, and
are as shown below.
Calculations for large displacement:
The movement is said to be large if it meet the following conditions.
Δ𝑞𝑖 = 𝑞𝑖𝑓−𝑞𝑖0 > ˙q imax2
q̈imax
From the equation, Δq𝑖 = 0.5 ∗ ˙q imax2
q̈imax ∗2+𝑞̇ 𝑖𝑚𝑎𝑥∗𝑡𝑐. And 𝑡𝑓 = 𝑡𝑐 + 2∗
˙q imax❑
q̈imax
We find that, 𝑡𝑐 = Δqi
q̇imaẋ − 𝑡𝑎 and 𝑡𝑓 = 𝑡𝑐 + 2 ∗ 𝑡𝑎 where 𝑡𝑎 = q̇imax
q̈imax
To find x co-ordinates for large movements at any time t, the equations above
are substituted into equations below.
(t) = 𝑞𝑖e + 0.5 q̈imax t2, t ≤ ta
(t) = 𝑞𝑖e + 0.5 q̈imax ta
2 + q̇imax (t - ta), ta≺ t ≤ ta+tc
(t) = 𝑞𝑖e + 0.5 q̈imax ta
2 + q̇imax (t - ta) - 0.5 q̈imax (t - ta- tc ¿2 , ta+tc ≺ t ≤ tf
Calculations for small displacement:
3

The movement is said to be small if it meets the following conditions
Δ𝑞𝑖 = 𝑞𝑖𝑓 − 𝑞𝑖0 ≤ ˙q imax2
q̈imax
From the equation, Δ𝑞𝑖 = 0.5 ∗ 𝑞̇ 𝑖 ∗ 𝑡𝑓 = 0.25 ∗𝑞̈ 𝑖𝑚𝑎𝑥 ∗ 𝑡𝑓^2
We get, 𝑡𝑓 = 2 ∗ √ Δqi
q̈imax
And given 𝑞̇ 𝑖𝑀 = 𝑞̈ 𝑖𝑚𝑎𝑥 ∗ 0.5 ∗ 𝑡𝑓
To find x co-ordinates for small movements at any time t, the equations above
are substituted into equations below.
(t) = 𝑞𝑖e + 0.5 q̈imax t2 t ≤ 1
2 tf
(t) = 𝑞𝑖e +( 0.5 tf ¿2 + q̇ie (t – 0.5tf ) - 0.5 q̈imax (t - 0.5 - tf ¿2 0.5 tf ≺ t ≤ tf
RESULTS
On running MATLAB commands on command window it returned the following
plots. It is from this plots that the analysis of the PUMA robot were made and
some comparison were made. The plots were
1. Robots actual joint angle in degrees versus time in seconds
2. Robots desired joint angle in degrees versus time in seconds
3. Robots velocity in meters per second versus time in seconds
4. Robots joint error versus time in seconds.
As learned from the previous lecture control square outline was put into
practice whereby the proportional controller gain Kp was fused with velocity
gain Kv. The PUMA robot SIMULINK document was changed to consolidate
both proportional and velocity gains, speed pick-up and to inform the program
on sending back the data to the workspace of MATLAB software
4

Design of Trajectory Planner for the joint
in the design of joint’s trajectory planner, joint space trajectory planner was
outlined in MATLAB. In building it the vars.m file was adjusted first then
computations that recovers the pick-up of the controller were incorporated
into the program so that these properties can be easily re-processed by other
qualities
Coding of small and large movements in MATLAB
For a successful movement of the robot’s joints, conditional if statements were
used. If statement is executed if the condition set is met, and if not, it goes to
the second. The if statement was used in this section to characterize large
movements, the condition after it is executed if the condition is met. The else If
statement was used to determine the position due to small movement, thus all
the equations are put into practice.
Discussion:
From the actual joint position versus time plot it was deduced that the second
joint starts by moving farther from initial position before retreating to the right
path after approximately 2.5 seconds
From the velocity versus time plot it was observed that the second joint had a
negative value of speed for the first 0.25 seconds. The resting position of the
joint(the second) was found to be at 45 degrees which is nearly half of the
predicted resting position, and this is clearly brought out in a plot of actual
joint positions and that of the desired joint position. This is due to errors from
either coding or the SIMULINK.
An error arouse in the positioning of joint 1 and joint 3 as well. The angle
difference between the actual positioning of joint 1 from the desired position
was found to be roughly three degrees. And that of joint 3 is approximately
twelve degrees. This is an indication of errors due to manipulation or in coding.
From velocity versus time graph the speed of joint 1 was observed to have
increased until some point where it stops and returned back to zero. This and
insinuate that the joint didn’t achieve its desired speed before it comes to its
resting position. As for the third joint the velocity shot sharply before it
suddenly falls back. This is an error as well as the velocity ought to have risen
5

in a steady manner or consistently and remain constant, and thereafter fall
back to zero in a steady decelerating speed.
Conclusion:
The design and implementation of control and trajectory planner for the PUMA
robot was implemented with an aid of MATLAB software. Its performance was
analysed using the plots, there were slight errors though and that is normal but
the design was remarkably good. Objectives of the experiment was thus met.
Appendix A
% This function assigns the global values required to run the PUMA
% simulation. It should be called before the simulation can be performed.
% In the function, where brake "i" controls the brake of the 'i'th joint
% of PUMA. When brake is "1", it is on. When brake is "0", it is off.
% In addition, 'thr10' is the initial angular position (in radius) of the
first joint
% of the PUMA. Consequently, 'thr20' and 'thr30' represent the initial
positions
% of the second and third joints of the PUMA.
% 'Mload' is the payload on the PUMA end-effector.
% 'ng1' is the gear ratio of the first joint driving train.
% 'ng2' is the gear ratio of the 2nd joint driving train.
% 'ng3' is the gear ratio of the 3rd joint driving train.
% this program must be run before the simulation.
%
global brake1 brake2 brake3 Mload
global thr10 thr20 thr30 ng1 ng2 ng3
grpno=input('Enter your group number here: ');
fprintf('\n');
brake1=0;
brake2=0;
brake3=0;
Mload=1;
ng1=40+grpno*5;
ng2=107.82;
ng3=53.71;
%variables
feff1=0.01;
feff2=0.005;
feff3=0.003;
OMEGA =31.4159265;
Jm=0.0002;
Ra=1.6;
Ka=0.2531;
Kb=0.2531;
6

%Finding Kv and Kp for all joints 1, 2 and 3
%For more explaining about the calculation method please check Excel file
%"Calculations Excel"
%Joint 3
a3=-0.0203;
x3=0.0204;
d4=0.43307;
d6=0.05625;
m3=6.97;
I3yy=0.3128;
z3=0.1244;
JL3=(I3yy+(m3)*(z3^2+(a3+x3)^2))+((d4+d6)^2+(a3)^2);
Jeff3=Jm+(1/ng3)^2*JL3;
%Joint 2
a2=0.4318;
x2=-0.3289;
I2zz=3.38;
y2=0.005;
JL22=I2zz+m2*(y2^2+(a2+x2)^2);
JL23=I3yy+m3*((z3+a2)^2+(x3+(a3))^2);
JPL2=(a2+d4+d6)^2+(a3)^2;
JL2= JL22+JL23+JPL2;
Jeff2=Jm+(1/ng2)^2*JL2;
%Proportional Gain for joint 2
Kp2=(OMEGA^2*Ra*Jeff2)/Ka;
Tm2=(Ra*Jeff2)/(Ra*feff2+Kb*Ka);
K2=Ka/(Ra*feff2+Kb*Ka);
%Velocity Gain for joint 2
Kv2=(2*OMEGA*Tm2-1)/K2;
%Joint 1
I1yy=0.2;
m1=12.96;
m2=22.34;
z1=0.0389;
d2=0.1491;
x1=0;
I2yy=3.03;
I3xx=0.3148;
z2=0.2038;
y3=0.0137;
JL11=I1yy+m1*((z1)^2+x1^2);
JL12=I2yy+m2*((d2+z2)^2+(a2+x2)^2);
JL13=I3xx+m3*((d2+y3)^2+(a2+z3)^2);
JPL1=((a2+d4+d6)^2+(d2^2));
JL1=JL11+JL12+JL13+JPL1;
7

Jeff1=Jm+(1/(ng1))^2*JL1;
Tm1=(Ra*Jeff1)/(Ra*feff1+Kb*Ka);
K1=(Ka)/(Ra*feff1+Kb*Ka);
%Proportional Gain for joint 1
Kp1=(OMEGA^2*Ra*Jeff1)/(Ka);
%Velocity Gain for joint 1
Kv1=(2*OMEGA*Tm1-1)/K1;
%%Proportional Gain for joint 3
Kp3=(OMEGA^2*Ra*Jeff3)/Ka;
Tm3=(Ra*Jeff3)/(Ra*feff3+Kb*Ka);
K3=(Ka)/(Ra*feff3+Kb*Ka);
%%Proportional Gain for joint 3
Kv3=(2*OMEGA*Tm3-1)/(K3);
%Showing the user each joints Kp and Kv
Joint = [1, 2, 3; Kp1, Kp2, Kp3; Kv1, Kv2, Kv3];
formatS='For Joint %1.0f, Kp is %4.8f and Kv is %4.8f.\n';
fprintf(formatS, Joint);
fprintf('\n');
%Calculating Maximum Velocity (Nominal):
i=0;
V = 40;
th1abs = ((V - i*Ra)/Kb)*ng1;
th2abs = ((V - i*Ra)/Kb)*ng2;
th3abs = ((V - i*Ra)/Kb)*ng3;
%From the given we take Nominal value equals to 25%
th11 = (th1abs*0.25);
th12 = (th2abs*0.25);
th13 = (th3abs*0.25);
%For Nominal maximum acceleration
T=1.2;
th2_1abs = (T/Jeff1);
th2_2abs = (T/Jeff2);
th2_3abs = (T/Jeff3);
th2_J1 = th2_1abs*0.25;
th2_J2 = th2_2abs*0.25;
th2_J3 = th2_3abs*0.25;
%Ask client or user to enter speed of nominal max speed:
PercentageSpeed = input('Enter the joint nominal maximum speed (percentage)
: ');
fprintf('\n');
8

DecimalSpeed = PercentageSpeed/100; %this to change percentage to decimal
%Calculating the actual velocity of the robot according to input:
V1 = th11*DecimalSpeed;
V2 = th12*DecimalSpeed;
V3 = th13*DecimalSpeed;
%Here to enter the Initial and Final joint positions according to the
%client
J1initial=input('Please input Joint 1 Initial (in degrees or radians): ');
J1final=input('Please input Joint 1 Final (in degrees or radians): ');
J2initial=input('Please input Joint 2 Initial (in degrees or radians): ');
J2final=input('Please input Joint 2 Final (in degrees or radians): ');
J3initial=input('Please input Joint 3 Initial (in degrees or radians): ');
J3final=input('Please input Joint 3 Final(in degrees or radians): ');
fprintf('\n');
%Calculating the time
dt=0.0001;
%Calculating the time for Joint 1
deltaq1=abs(J1final-J1initial);
q1tf = gt(deltaq1, (V1^2)/th2_J1); % tf means Truefalse
%For Large movement
if q1tf == 1
ta1 = V1/th2_J1;
tc1 = deltaq1/V1 -ta1;
tf1 = tc1 +2*(ta1);
t11t = (0:dt:ta1)';
t11x = J1initial+0.5*th2_J1*t11t.^2;
t12t = (ta1:dt:ta1+tc1)';
t12x = J1initial + 0.5*th2_J1*ta1.^2 +V1*(t12t - ta1);
t13t = (ta1+tc1:dt:tf1)';
t13x = J1initial + 0.5*th2_J1*ta1.^2 +V1 *(t13t-ta1) -
0.5*th2_J1*(t13t-ta1-tc1).^2;
th1 = [t11t, t11x; t12t, t12x; t13t, t13x];
%For SMALL movement
elseif q1tf == 0
tf1 = 2*sqrt(deltaq1/th2_J1);
qdotm1=th2_J1*0.5*tf1;
t11t = (0:dt:0.5*tf1)';
t11x = J1initial +0.5*th2_J1*t11t.^2;
t12t = (0.5*tf1:dt:tf1)';
t12x = J1initial +0.5*th2_J1*(0.5*tf1).^2 + qdotm1*(t12t-0.5*tf1)-
0.5*th2_J1*(t12t-0.5*tf1).^2;
th1 = [t11t, t11x;t12t, t12x];
end
%Calculating the time for Joint 2
9

Dq2=abs(J2final-J2initial);% D for delta
q2tf = gt(Dq2, (V2^2)/th2_J2);
if q2tf == 1
ta2 = V2/th2_J2;
tc2 = Dq2/V2 -ta2;
tf2 = tc2 +2*(ta2);
t21t = (0:dt:ta2)';
t21x = J2initial+0.5*th2_J2*t21t.^2;
t22t = (ta2:dt:ta2+tc2)';
t22x = J2initial + 0.5*th2_J2*ta2.^2 +V2*(t22t - ta2);
t23t = (ta2+tc2:dt:tf2)';
t23x = J2initial + 0.5*th2_J2*ta2.^2 +V2 *(t23t-ta2) -
0.5*th2_J2*(t23t-ta2-tc2).^2;
th2 = [t21t, t21x; t22t, t22x; t23t, t23x];
elseif q2tf == 0
tf2 = 2*sqrt(Dq2/th2_J2);
qm2=th2_J2*0.5*tf2;
t21t = (0:dt:0.5*tf2)';
t21x = J2initial +0.5*th2_J2*t21t.^2;
t22t = (0.5*tf2:dt:tf2)';
t22x = J2initial +0.5*th2_J2*(0.5*tf2).^2 + qm2*(t22t-0.5*tf2)-
0.5*th2_J2*(t22t-0.5*tf2).^2;
th2 = [t21t, t21x;t22t, t22x];
end
%Calculating the time for Joint 3
Dq3=abs(J3final-J3initial);
q3tf = gt(Dq3, (V3^2)/th2_J3);
if q3tf == 1
ta3 = V3/th2_J3;
tc3 = Dq2/V3 -ta3;
tf3 = tc3 +2*(ta3);
t31t = (0:dt:ta3)';
t31x = J3initial+0.5*th2_J3*t31t.^2;
t32t = (ta3:dt:ta3+tc3)';
t32x = J3initial + 0.5*th2_J3*ta3.^2 +V3*(t32t - ta3);
t33t = (ta3+tc3:dt:tf3)';
t33x = J3initial + 0.5*th2_J3*ta3.^2 +V3 *(t33t-ta3) -
0.5*th2_J3*(t33t-ta3-tc3).^2;
th3 = [t31t, t31x; t32t, t32x; t33t, t33x];
elseif q3tf == 0
tf3 = 2*sqrt(Dq3/th2_J3);
qdotm3=th2_J3*0.5*tf3;
t31t = (0:dt:0.5*tf3)';
10

t31x = J3initial +0.5*th2_J3*t31t.^2;
t32t = (0.5*tf3:dt:tf3)';
t32x = J3initial +0.5*th2_J3*(0.5*tf3).^2 + qdotm3*(t32t-0.5*tf3)-
0.5*th2_J3*(t32t-0.5*tf3).^2;
th3 = [t31t, t31x; t32t, t32x];
end
%input initial values, thr10, thr20 and thr30
thr10=J1initial*pi/180;
thr20=J2initial*pi/180;
thr30=J3initial*pi/180;
sim('puma2017')
%Change them into degrees
jpp1=jp1*180/pi;
jpp2=jp2*180/pi;
jpp3=jp3*180/pi;
%Joint Error
je1 = abs(jpp1-jd1);
je2 = abs(jpp2 -jd2);
je3 = abs(jpp3 - jd3);
%The plots
subplot(2,2,1)
plot(time_out, jpp1, 'mX-', time_out, jpp2, 'k*--', time_out, jpp3,
'y+-.');
title('Positions of Actual Joint')
legend('J1 Position', 'J2 Position', 'J3 Position');
xlabel('Time (in s)');
ylabel ('Joint Angle (in Degrees)');
subplot (2,2,2)
plot(time_out, jd1,'mX-', time_out, jd2, 'k*--', time_out, jd3, 'y+-.');
title('Joint Positions(Desired)');
legend('J1 Position', 'J2 Position', 'J3 Position');
xlabel('Time (in s)');
ylabel('Robot Joint Angle (in Degrees)');
subplot(2,2,3)
plot(time_out,jv1, 'mX-', time_out, jv2,'k*--', time_out, jv3, 'y+-.');
title('Velocity VS Time');
legend('J1 Velocity', 'J2 Velocity', 'J3 Velocity');
xlabel('Time (s)');
ylabel('Velocity (in m/s)');
subplot(2,2,4)
plot(time_out, je1, 'mX-', time_out, je2, 'k*--', time_out, je3, 'y+-.');
title('Joints Error VS Time');
legend('J1 Error', 'J2 Error', 'J3 Error');
11