International Journal of Science, Engineering and Technology Research (IJSETR)

Volume 6, Issue 11, November 2017, ISSN: 2278 -7798

Point to Point Trajectory Control of 3R Planar

Robot Arm
Moe Thu Zar, Wai Phyo Ei

 pose. The control objective is to make the 3R robot

Abstract— The functions of 3R Planar Robot Arm are manipulator traces desired trajectory. There are two
described in this journal. It includes three main parts: first is approaches to generating such trajectories known
calculation of inverse kinematics of robot arm, second is respectively as joint-space and Cartesian motion. In this
sending joint angle values to ARDUINO by serial research, Cartesian space or operational space trajectory is
communication and another is ARDUINO drive the arm. In
used. PID tuning software method (eg. MATLAB) are used
which, three motors are used to move the arm. When the
desired input (x, y and ϕ) are entered, inverse kinematic in the
in this research.
MATLAB program in PC is calculated joint angle values and
then send it to the ARDUINO. This research used the MATLAB 2) SYSTEM BLOCK DIAGRAM OF PLANAR ROBOT ARM
software to calculate the inverse kinematic of robot arm and The complete system block diagram shown in Figure 1
also uses it as a graphical user interface (GUI) for controlling consists of many parts like, personal computer with serial
the movement of this robot. If the ARDUINO get the joint angle
values, the motor drives the arm. communication adapter, ARDUINO, servo, potentiometer
and robot arm. Firstly, when the program is run, inverse
Index Terms— Interface, MATLAB, Forward Kinematics, kinematics in MATLAB is calculating the joint angle value
Inverse Kinematics, Trajectory. and then sending data to ARDUINO that drives the robot
arm. The main function of the ARDUINO is sending joint
1) INTRODUCTION angle values to the servo motors, feeding the joint angle
values from encoders of the servo motors and send back to
Robotics education has been based on mobile robotics
the ARDUINO.
and manipulator-based robotics. Manipulator-based robotics
education requires a large start up investment. This thesis is
concerned with the fundamentals of robotics, including
kinematics, computer interfacing, and control concepts as Position
applied to industrial robot manipulators. The mathematical Computer + ARDUINO
Actuator
Robot Arm
modelling of spatial linkages is quite involved. It is useful to (servo)
_
start with planar robots because the kinematics of planar PID
MATLAB
mechanisms is generally much simpler to analyze. A link
parameter link length and link twist defines the relative
Reference Position
location of the two axes in space. A link parameter is link Sensor
length and link twist. The link offset is the distance between (potentiometer)
two consecutive links along the axis of the joint. The joint
angle is the rotation of one link with respect to the next about
the joint axis. These four parameters together are called
Denavit-Hartenberg (D-H) parameters. The end effectors
position can be controlled using either the joint angle or the Figure.1. Overall Block Diagram of the System.
link offset.
The forward kinematics problem is concerned with the 3) ROBOTIC ANALYSIS
relationship between the individual joints of the robot
manipulator and the position and orientation of the tool or The analysis of a robot consists of determining D-H
end-effector. The inverse problem of finding the joint parameters, calculating robot kinematics and dynamics, and
variables in terms of the end effector position and orientation controlling the robot via a control scheme. The control
is the problem of inverse kinematics and it is in general more scheme may be PID, Fuzzy, NN or Visual Control.
difficult than the forward kinematics problem. Deriving both i. Determining D-H Parameters
forward and inverse kinematics is an important step in robot
D-H parameters table is a notation developed by Denavit
modelling based on Denavit -Hartenberg (D-H)
and Hartenberg, is intended for the allocation of orthogonal
representation [1].
coordinates for a pair of adjacent links in an open kinematic
One of the most common requirements in robotics is to
system. It is used in robotics, where a robot can be modelled
move the end-effector smoothly from one pose to another
as a number of related solids (segments) where the D-H
Manuscript received Oct, 2017. parameters are used to define the relationship between the
Moe Thu Zar, Mechatronic Engineering Department, Mandalay two adjacent segments. The first step in determining the D-H
Technological University, Mandalay, Myanmar, +959786060301. parameters is locating links and determines the type of
Wai Phyo Ei, Mechatronic Engineering Department, Mandalay
Technological University, Mandalay, Myanmar.
movement (rotation or translation) for each link.

1435
All Rights Reserved © 2017 IJSETR
 International Journal of Science, Engineering and Technology Research (IJSETR)
Volume 6, Issue 11, November 2017, ISSN: 2278 -7798

Using D-H parameters defined in the previous steps in

c1  s1 0 0
Table l, robot model was created in MATLAB software using
s c1 0 0
T1=  1 
the Robotic Toolbox. Robot model in addition to previously 0
(3)
determined D-H parameters contains physical parameters
0 0 1 0
which is using in the calculation of the dynamics movement.
 0 0 0 1 
TABLE 1
c2 s
2
0 l 
1
D-H PARAMETERS  
T2=  2
1 s c 0 0
link α a d θ 2 (4)
0 0 1 0
1 0 10.5cm 0 0<θ1<180  
0 0 0 1
2 0 9.8cm 0 0<θ2<120
c3  s3 0 l2 
0<θ3<150 s 0 0
3 0 1cm 0
c3
2
T3=  3  (5)
y1 y2 y3 y4 0 0 1 0
 0 0 0 1 

1 0 0 l3 
L1 L2 L3
x4
0 1 0 0
x1 x2 x3 3
Ttool=   (6)
0 0 1 0
θ1
θ2 θ3 Tool 0 0 0 1 
Wrist
Elbow c123  s123 0 l1c1  l2c12 
z1 Shoulder
z3 z4 s c123 0 l1s1  l2 s12 
T3=  123 
z2 0
(7)
Figure 2. Coordinate Frame Assignment of Robot Arm
 0 0 1 0 
 0 0 0 1 
Since the robot has three revolute joints, the joint
coordinate is Finally, to obtain 0Ttool, multiply 0T3 and 3Ttool then get,
qi= [θ1 θ2 θ3] c123  s123 0 l1c1  l2c12  l3c123 
ii. Direct Geometric Model
s c123 0 l1s1  l2 s12  l3s123 
0
Ttool=  123  (8)
The direct geometric model (DGM) represents the  0 0 1 0 
relations calculating the operational coordinates, giving the  0 0 0 1 
location of the end-effector, in terms of the joint coordinates.
After establishing D-H coordinate system for each link as where, c1  cos(1 )
shown in Table 1, a homogeneous transformation matrix can
easily be developed considering frame {i-1} and frame {i} s1  sin(1 )
transformation. So, the link transformation matrix between c12  cos(1   2 )
coordinate frames {i-1} and {i} has the following form [2];
i-1
Ti = Rot(z,θi).Trans(z,di).Trans(x,ai).Rot(x,αi) (1) s12  sin(1  2 )
cos i  cos i sin i sin i sin i ai cos i  c123  cos(1   2  3 )
 sin  cos  cos   sin  cos  a sin  
i
Ti+1=  i i i i i i i (2) s123  sin(1   2  3 )
 0 sin i cos i di 
Another generalized symbolic transformation matrix,
 0 1 
0 0
 nx ox a x px 
Where the four quantities, αi, ai, di, and θi are the n y o y a y py 
parameters of link i and joint i. T=   (9)
The various parameters in previous equation are given the  nz oz a z pz 
following names:  0 0 0 1 
ai (Length) is the distance from zi to zi+1, measured
along zi; where, nx  c123
αi (Twist), is the angle between z i and z i+1 measured n y  s123
about xi;
di (Offset), is the distance from xi to xi+1 measured nz  0
along zi; and
θi (Angle), is the angle between xi and xi+1 measured ox   s123
about zi. o y  c123

oz  0

1436
All Rights Reserved © 2017 IJSETR
 International Journal of Science, Engineering and Technology Research (IJSETR)
Volume 6, Issue 11, November 2017, ISSN: 2278 -7798

ax  0 s2   1  c2
2
(16)
ay  0
  a tan 2( s , c ) (17)
2 2 2
az  1
By expanding cos(1   2 ) and sin(1   2 ) of equation
p x  l1c1  l2c12  l3c123
(13) and (14), and rearranging them as:
p y  l1s1  l2 s12  l3 s123 wx  (a1  a2 c2 ) c1  a2 s1 s 2 (18)
pz  0 w y  ( a1  a2 c 2 ) s1  a2 c1 s 2 (19)

iii. Inverse Geometric Model (a1  a2 cos(2 )) w y  a2 s2 wx

s1  (20)
Manipulator solution strategies into two broad classes: 2 2
wx  wy
closed-form solutions and numerical solutions. Because of
their iterative nature, numerical solutions generally are much (a1  a2 cos( 2 )) wx  a2 s2 w y
slower than the corresponding closed-form solution. For c1  (21)
2 2
most uses, are not interested in the numerical approach to wx  wy
solution of kinematics [3]. Within the class of closed-form 1  a tan 2( s1, c1) (22)
solutions, distinguish two methods of obtaining the solution:
algebraic and geometric. Inverse kinematic need to find the 3    1   2 (23)
joint angle , and ,corresponding end effector’s
position and orientation. For a planar motion, the position and 4) ROBOT HARDWARE AND SOFTWARE
orientation of the end effector can be specified by the origin
of the frame 4, i.e (px, py) and the orientation of the frame To achieve a control of a robot arm by using personal
attached to the end effector with respect to X axis, i.e. angle computer, it must make the connection between the robot and
ϕ. PC. This connection is called interface connection and it is
done by using a ARDUINO. ARDUINO is designed to
interface to and interact with electrical/electronic devices,
Φ=θ1+θ2+θ3 sensors and actuators, and high-tech gadgets to automate
Y systems. The complete control process can be divided into
two categories, hardware and software.
θ3 A. Hardware Environment
a3
The hardware environment for this thesis consists of a
ARDUINO, a PC, and a data link between the PC and robot
arm. The ARDUINO is a device that interfaces to sensors and
robot actuators and performs embedded computing. The PC
a2
θ2 is used to control the robot by GUI; it is used to write the user
defined embedded program which is to be run on the
ARDUINO. In this thesis, use a serial communication
a1 link(wire) between the ARDUINO and PC.

θ1
X

Figure 3. 3-DOF (3R) Planar Robot Arm

For the simulation of the inverse kinematics for 3R planar

robot arm, these equations can be simplified as follows:
p x  a1 c1  a2 c12  a3 c123 (10)
p y  a1 s1  a2 s12  a3 s123 (11)
  1   2  3 (12)
The coordinate of wrist is wx and w y :
wx  p x  a3 cos( )  a1 c1  a2 c12 (13)
w y  p y  a3 sin( )  a1 s1  a2 s12 (14) Figure.4. Hardware Environment
Squaring the two sides of equation (13) and (14), i. Arduino
2 2 2 2
wx  wy  a1  a2  2a1a2 cos(2 ) The Arduino Mega 2560 is a microcontroller board based
on the ATmega2560 (datasheet). It has 54 digital input/output
2 2 2 2
wx  w y  a1  a2 pins (of which 14 can be used as PWM outputs), 16 analog
cos( 2 )  (15) inputs, 4 UARTs (hardware serial ports), a 16 MHz crystal
2a1a2 oscillator, a USB connection, a power jack, an ICSP header,

1437
All Rights Reserved © 2017 IJSETR
 International Journal of Science, Engineering and Technology Research (IJSETR)
Volume 6, Issue 11, November 2017, ISSN: 2278 -7798

and a reset button. It contains everything needed to support obtained by specifying the discrete time joint- displacements
the microcontroller; simply connect it to a computer with a at a constant time rate. The velocity and acceleration of the
USB cable or power it with a AC-to-DC adapter or battery to points can be calculated from the numerical approximation of
get started. The Mega is compatible with most shields the time derivatives. Several methods were used to compress
designed for the Arduino Duemilanove or Diecimila. the describing data of the trajectories as cubic, quintic and
LSPB trajectory.
ii. Personal Computer Cubic Trajectory: Cubic polynomial or third order
As previously mentioned, the PC is used to write Basic polynomial approximation describes the path parametrically
programs that the ARDUINO executes and to display sensory as a function of time with the position and velocity
data processed by the ARDUINO and control the robot using constraints at initial time t = zero and final time tf. It gives
MATLAB GUI. Any PC that supports MATLAB can be continuous positions and velocities at the start and finish
used. points times but discontinuities in the acceleration. The
derivative of acceleration is called the jerk. Higher order
iii. Servo Motor polynomials are required to guarantee the smoothness of the
Servo control in general can be broken into two joint accelerations.
fundamental classes of problems. The first class deals with Quintic Trajectory: A discontinuity in acceleration leads to
command tracking. It addresses the question of how well an impulsive jerk, which may excite vibration modes in the
does the actual motion follow what is being commanded. The manipulator and reduce tracking accuracy. For this reason,
typical commands in rotary motion control are position, one may wish to specify constraints on the acceleration as
velocity, acceleration and torque. The second general class of well as on the position and velocity. In this case, we have six
servo control addresses the disturbance rejection constraints (one each for initial and final configurations,
characteristics of the system. Disturbances can be anything initial and final velocities, and initial and final accelerations).
from torque disturbances on the motor shaft to incorrect Therefore, a fifth order polynomial is required.
motor parameter estimations used in the feedforward control. for distance:
2 3 4 5
q (t )  a0  a1t  a2t  a3t  a4t  a5t  q
for velocity:
.
q (t )  a  2a t  3a t 2  4a t 3  5a t 4  v
1 2 3 4 5
for acceleration:
..
q (t )  2a  6a t  12a t 2  20a t 3  
2 3 4 5

Figure.5. Servo Motor (MG995_Tower-Pro)

B. Software Environment
Software environment can be divided into two parts: the
ARDUINO program and MATLAB Program. In ARDUINO
program, write a code to make the interfacing between PC
and arm. The MATLAB program consists of the Serial
Communication code and the graphical user interface (GUI).

5) OPERATION OF THE CONTROL SYSTEM

MATLAB and ARDUINO program is applied in this
research.
When the inverse kinematic of the robot arm is calculated,
Figure 6. Quintic Polynomial Trajectory with Zero-Velocity Boundary
MATLAB sends joint angle value as serial data to the robot. Conditions [t0=0, tf=50 second, q0=0, qf=1, v=0 and α=0]
By entering each corresponding input, the operator can
control simultaneously the robot movement. While
performing these actions, the operator can check whether the
command is correct or not with the help of LCD display.
All the motors are driven by the joint angle values received
from the MATLAB in PC. ARDUINO is also used as the
main microcontroller to interface with other devices by
receiving the data. The robot is moved to the desired position
with the help of three servo motors and another one servo
motor is used to move the ball-pen up and down to write
some numbers.

6) ALGORITHM OF TRAJECTORY CONTROL SYSTEM

Straight-line motions are most common in the industrial
Figure 7. Quintic Polynomial Trajectory with Velocity Boundary Conditions
applications; however, movement on a line is mostly [t0=0, tf=50 second, q0=0, qf=1, v0=0.5, vf=0 and α=0]

1438
All Rights Reserved © 2017 IJSETR
 International Journal of Science, Engineering and Technology Research (IJSETR)
Volume 6, Issue 11, November 2017, ISSN: 2278 -7798

LSPB trajectory: Another way to generate suitable joint At home position all angles are zero, put θ1 = 0, θ2 = 0, θ3
space trajectories is by so-called Linear Segments with = 0.
Parabolic Blends or (LSPB) for short. This type of trajectory So the transformation matrix reduced to:
is appropriate when a constant velocity is desired along a
portion of the path. The LSPB trajectory is such that the
velocity is initially “ramped up” to its desired value and then
“ramped down” when it approaches the goal position. To
achieve this, we specify the desired trajectory in three parts.
The first part from time t0 to time tb is a quadratic polynomial.
This results in a linear “ramp” velocity. At time tb, called the
blend time, the trajectory switches to a linear function. This
corresponds to a constant velocity. Finally, at time tf − t b the
trajectory switches once again, this time to a quadratic
polynomial so that the velocity is linear [4][5].
 a 2 0  t  tb Figure 10. Visualization of robot in all zero joint angles in 3D and 2D
 q0  t
2 At this position all joint angles are 60 ,̊ put θ1=60 ,̊ θ2=60 ̊
 and θ3=60 .̊
 q f  q0  Vt f tb  t  t f  tb
q (t )    Vt
 2
2
 at f a 2
q f   at f t  t t f  tb  t  t f
 2 2

(a)

Figure 8. LSPB with Zero-Velocity Boundary Conditions [t0=0, tf=50 second,

q0=0, qf=1, v=0 and α=0]

(b)

Figure.11. Visualization of robot in all 60 ̊ joint angles [ (a) 3D and (b) 2D

view]

7) RESULTS AND DISCUSSIONS

The user enters the input position into the left three edit
boxes for moving the base motor, first joint motor and second
joint motor. MATLAB program is calculated the joint angle
values by using inverse kinematic equation of the robot arm
and then send to the microcontroller. Microcontroller
receives these signals and then the motors rotate according to
Figure 9. LSPB with Velocity Boundary Conditions [t0=0, tf=50 second,
q0=0, qf=1, v0[maximum]=0.035, v0[minimum]=0.025, vf=0, and α=0] the instructed direction. MATLAB simulation result of the
robot arm is shown in Figure 12.

1439
All Rights Reserved © 2017 IJSETR
 International Journal of Science, Engineering and Technology Research (IJSETR)
Volume 6, Issue 11, November 2017, ISSN: 2278 -7798

Figure.15. Testing Photo of Robot Arm Control System from Home Position
to Goal Positions (x=16 cm, y=10 cm and x =10 cm, y=10 cm with φ=45 )̊

Figure.12. Simulation Result of Serial Interfacing for Robot Arm in

The trajectory traced by the end effector with respect to
MATLAB Form original points. It is seen that at every point some error is
occurring. This may be due to inaccuracies in the model
From home position, (x=30.3cm, y=0 cm and ϕ=0) to goal points is shown in setting including the motor clearance as well as due to the
Figure 13, Figure 14 and Figure 15. integer values of joint angles given to the ARDUINO
program. The precision of robot arm is 2 cm.
Since MATLAB is slow in the execution time, recommend
using a high-level computer programming language to
perform the software part.

CONCLUSIONS
This journal represents a writing robot using PC based
point to point trajectory control and it is designed for only as
a sample. The Graphical User Interface (GUI) of the software
package was developed for testing motional characteristics of
the robot arm. A physical interface between the robot arm and
the GUI was designed and built. A comparison between
kinematics solutions of the virtual arm and the robot's arm
physical motional behaviours have been accomplished. The
results are displayed in a graphical format and the motion of
all joints and end effector can be observed. Many future
developments can be carried on this robot arm like other
Figure.13. Testing Photo of Robot Arm Control System for Home Position
(x=30.3cm, y=0 cm and ϕ=0) types for robots, these developments include path-planning,
dynamics modelling, force control and computer vision.

ACKNOWLEDGEMENT
The author is deeply gratitude to Dr. Sint Soe, Rector of
Mandalay Technological University (MTU) for his kind
permission to submit this research. The author would like to
thank to her supervisor, Daw Wai Phyo Ei, Lecturer of
Mechatronic Engineering Department, Mandalay
Technological University, for her valuable supervision
guidance and comments for preparation of the research. After
that, the author would like to thank Dr. Sint Soe, Associate
Professor and thanks to all her teachers from Mechatronic
Department, Mandalay Technological University.

REFERENCES
[1] MAHMOUD A. ABUALSEBAH, “Trajectory Tracking Control of
3-Dof Robot Manipulator Using Tsk Fuzzy Controller”, AUGUST
2013.
Figure.14. Testing Photo of Robot Arm Control System from Home Position [2] J J. DENAVIT, R.S. HARTENBERG, “A Kinematics Notion
to Goal Position (x=12 cm, y=10 cm and ϕ=90 )̊ For Lower-Pair Mechanisms Based On Atrices”, ASME JOURNAL
OF APPLIED MECHANICS, VOL. 22, PP. 215–221, 1955.
[3] PAUL R.C.P., “Robot Manipulators: Mathematics, Programming
and Control”, MIT PRESS, CAMBRIDGE, 1981.

1440
All Rights Reserved © 2017 IJSETR
 International Journal of Science, Engineering and Technology Research (IJSETR)
Volume 6, Issue 11, November 2017, ISSN: 2278 -7798

[4] MOHAMMED REYAD ABUQASSEM, “Simulation and Interfacing

of 5 Dof Educational Robot Arm”, DEGREE OF MASTER OF
SCIENCE IN ELECTRICAL ENGINEERING, JUNE 2010
[5] PETER CORKE, “Robotics, Vision and Control”.

1441
All Rights Reserved © 2017 IJSETR