Journal of Artificial Intelligence Practice (2024) DOI: 10.23977/jaip.2024.

070115
Clausius Scientific Press, Canada ISSN 2371-8412 Vol. 7 Num. 1

Robot Trajectory Planning and Simulation Based on

Matlab Robotics Toolbox
Liao Shengxi1,2
1
School of Mechanical Engineering, Anhui University of Technology, Ma'anshan, China
2
Agricultural Machinery Management Service Center in Wanzhi District, Wuhu City, China

Keywords: PUMA560 robot; Kinematic simulation; Trajectory planning

Abstract: The trajectory planning of robots refers to the motion design of the pose,
velocity, and acceleration of the end effector (robot operating arm) in spatial motion.
According to the requirements of the robot's task, the end effector moves along the
expected trajectory from the initial state to the endpoint state. This article takes the
PUMA560 robot as the object, uses an improved D-H parameter method to establish a
coordinate system and design parameters, solves the forward and inverse kinematics of
the robot. This article uses the fifth degree polynomial interpolation method to obtain the
curves of robot joint angle, angular velocity, and angular acceleration over time, and uses
Matlab's robot toolbox for trajectory planning simulation to verify the good motion
performance of robot joints and the rationality of parameter design.

1. Preface

Today, robots are gradually being widely used in various fields such as automobiles, electronics,
food, logistics, healthcare, picking, and life services. For robots in motion, the implementation of
the pose motion of the robot's end effector is a key direction of robot research, which requires
trajectory planning and dynamic simulation of robot humans. This article takes the PUMA560 robot
as the object, solves the forward and inverse kinematics of the robot based on the D-H parameter
method, simulates the robot kinematics using the MATLAB Robotics toolbox, and combines
polynomial interpolation method to simulate the joint space trajectory of any two points of the
robot's end effector. This can intuitively display the robot's motion situation, and uses q (position),
qd (angular velocity), qdd (angular acceleration) Drawing function curves further analyzes the
correctness and rationality of robot trajectory planning, providing a universal theoretical analysis
basis for studying other types of robots and offline programming[1].

2. Establishment of Robot Motion Parameters and Coordinate System

2.1. Establishing a Linkage Coordinate System

The establishment of coordinate systems for each rotational joint of the robot and the
corresponding linkage parameters are summarized as follows:
(1) x i -axis: The direction of the common normal lines is consistent with the directions of axes z i

90
and z i 1 , and the direction of axes x i and y i is determined by the right-hand rule;
(2) y i -axis: direction is determined by the right-hand rule based on x i and z i ;
(3) z i -axis: along the axial motion axis of the i+1 joint, pointing away from the direction of the i
joint;
(4) a i =Move the distance from z i to z i 1 along the x i axis;
(5)  i =Around the x i axis, rotate from z i to an angle of z i 1 ;
(6) d i =Move the distance from x i 1 to x i along the z i axis;
(7)  i =Rotate around the z i axis from x i 1 to x i degrees.

2.2. Determine the robot transformation matrix

The transformation matrix of a robotic arm is established by two rotations and two translations,
based on the following rules, after specifying the coordinate system of the connecting rod, to
establish the corresponding relationship between adjacent connecting rods i-1 and i:
(1) Rotate axis x i 1 around axis z i 1 by  i angles, aligning axis x i 1 with axis x i ;
(2) Translate a distance of d i along the z i 1 -axis to make the x i 1 -axis and x i -axis coincide;
(3) Translate a distance of a i along the x i axis, so that the origin of the coordinate system of
connecting rod i -1 coincides with the origin of the coordinate system of connecting rod i ;
(4) Rotate  i degrees around the x i axis to align z i 1 with z i .
The relative position transformation relationship between connecting rod i and connecting rod i -1
can be described by four homogeneous transformations, namely the connecting rod transformation
matrix Ai .

Ai  Rot z, i Trans0,0, d i Transai ,0,0Rot x,  i  (1)

In the formula:
c i  s i 0 0 1 0 0 0
 s c i 0 
0 0 1 0 0 
Rot z, i    i ; Trans0,0, d i    ;
 0 0 1 0 0 0 1 di 
   
 0 0 0 1 0 0 0 1

1 0 0 ai  1 0 0 0
0 1 0 0  0 c  s i 0
Transai ,0,0   ; Rot x,  i    i
.
0 0 1 0 0 s i c i 0
   
0 0 0 1 0 0 0 1
From equation (1), it can be concluded that:
c i  s i c i s i s i ai c i 
 s c i c i  c i s i ai s i 
Ai   i (2)
 0 s i c i di 
 
 0 0 0 1 

91
 The above is the transformation matrix Ai of the connecting rod, which is the transformation
matrix between adjacent connecting rods. When it is required to transform the end connecting rod,
such as connecting rod 6 coordinate system relative to the i -1 connecting rod coordinate system, it
is i1T6 (which is 0T6 with the base system). So there are:
1
T2  A1 A2 ， 2T3  A2 A3 ， 1T3  A1 A2 A3 ，    (3)
i 1
T6  Ai Ai 1    A6 (4)
The transformation formula is:
 c i  s i 0 ai 1 
 s c c i c i 1  s i 1  d i s i 1 
i 1
Ti   i i 1 (5)
 s i s i 1 c i s i 1 c i 1 d i c i 1 
 
 0 0 0 1 
Therefore, for a 6-degree-of-freedom robot, the relationship 0T6 between the end effector and
the base system is:
0
T6  A1 A2    A6 (6)

2.3. PUMA560 robot structure and D-H parameter modeling

The PUMA560 robot belongs to a 6-degree-of-freedom rotational joint type robot. The first 3
joints determine the position of the wrist reference point, the last 3 joints determine the orientation
of the wrist, and the axis of the last 3 joints intersects at a point. This point is selected as the
reference point for the wrist and also as the origin of the linkage coordinate systems {4}, {5}, and
{6}. The axis of joint 1 is vertical, while the axes of joint 2 and joint 3 are horizontal and parallel,
with a distance of a 2 . The axes of joint 1 and joint 2 intersect vertically, while the axes of joint 3
and joint 4 intersect vertically, with a distance of a 3 . The coordinate systems of each connecting rod
are shown in Figure 1[2].

Figure 1: Structure and coordinates of PUMA560 robot

The D-H parameter method is a universal method proposed by Denavit and Hartenberg, which
only requires 4 parameters to express the pose relationship between two coordinate systems. It is a
method used to represent the relationship between robot linkages and joints. This method

92
corresponds the coordinate system to each joint, and expresses the pose transformation relationship
between the coordinate systems by constructing a homogeneous transformation matrix between
them, thereby obtaining a matrix for transforming from the robot base coordinate system to the end
effector hand coordinate system. The corresponding D-H parameters of the connecting rod are
shown in Table 1.
Table 1: PUMA560 Robot D-H Link Parameters (Unit: mm)
connecting
rod variable  i  i 1 ai 1 di Variable range
i
1 1  90  0 0 0  160 ~ 160
2  2  0    90 0 d 2  149.09  225 ~ 45
3  3  90  0 a2  431.8 0  45 ~ 225
4  4  0 
  90 
a3  20.32 d 4  433.07  110 ~ 170
5 5  0  90 0 0  100 ~ 100
6 6  0   90 0 0  266 ~ 266

3. PUMA560 robot forward and inverse kinematics solution

3.1. Forward kinematics solution of robots

Based on the D-H linkage parameters and joint variables mentioned above, the forward
kinematics solution is to solve the pose of the robot's end effector relative to the base system. From
the transformation matrices A1 , A2 , A3 , A4 , A5 , A6 of each adjacent connecting rod, the
transformation matrix 0T6 of the end effector can be obtained, and there is: 0T6  A1 A2    A6 .
Further results can be obtained:
n x ox ax px 
n oy ay p y  0 1 2 3 4 5
0
T6   y  A1 A2 A3 A4 A5 A6 (7)
nz oz az pz 
 
0 0 0 1

93
 n x  c1 c 23 c 4 c5 c6  s 4 s 6   s 23 s5 c6   s1 s 4 c5 c6  c 4 s 6  
n y  s1 c 23 c 4 c5 c6  s 4 s 6   s 23 s5 c6   c1 s 4 c5 c6  c 4 s 6  
n z   s 23 c 4 c5 c6  s 4 s 6   c 23 s5 c6 

o x  c1 c 23  c 4 c5 s 6  s 4 s 6   s 23 s5 s 6   s1 c 4 c6  s 4 c5 s 6 
o y  s1 c 23  c 4 c5 s 6  s 4 s 6   s 23 s5 s 6   c1 c 4 c6  s 4 c5 s 6 

o z   s 23  c 4 c5 s 6  s 4 c6   c 23 s5 s 6 
 (8)
a x  c1 c 23 c 4 s5  s 23 c5   s1 s 4 s5 
a y   s1 c 23 c 4 s5  s 23 c5   c1 s 4 s5 

a z  s 23 c 4 s5  c 23 c5 
p x  c1 a 2 c 2  a3 c 23  d 4 s 23   d 2 s1 

p y  s1 a 2 c 2  a3 c 23  d 4 s 23   d 2 c1 
p z  a3 s 23  a 2 s 2  d 4 c 23 

Note that in all of the above content:
si  sin  i ; ci  cos i ; si  s i  sin i ;
sij  sin i   j   si c j  ci s j ; cij  cos i   j   ci c j  si s j .

3.2. Inverse kinematics solution of robots

The so-called inverse solution in robotics refers to solving the joint variables  1 ,  2 ,  3 ,  4 ,
 5 and  6 of the robot in reverse by knowing the geometric parameters of the robot's connecting
rod and the end effector pose 0T6 , that is, knowing n , o , a and p . The method is to use an
unknown inverse transformation to multiply both sides of equation (7), separate the joint variables,
and solve them accordingly. When solving, it should be noted that the joint angle cannot be
calculated using arccosine arccos, but rather using the two independent variables x and y of the
bivariate arctangent function atan2. When x or y are zero, they have a definite meaning. The
specific solution steps are as follows[3]:
Find  1
A1 1  :
0 1
The left side of equation (7) can be obtained by inverse transformation

T6  0 A1 1 0T6 1A2 2 A3 3 A4 4 A5 5 A6

1 1
(9)

 c1 s1 0 0  n x ox ax px 
 s c1 0 0 n y oy ay p y 
1
T6   1 (10)
 0 0 1 0  n z oz az pz 
  
 0 0 0 1  0 0 0 1
Can be obtained:
 s1 p x  c1 p y  d 2 (11)

By utilizing trigonometric substitution and the bivariate arctangent function atan2( p y , p x ), we

94
obtain:


 1 =a tan 2 p y , p x -a tan 2 d 2 , p x2  p y2  d 22  (12)

Find  3
Two equations can be obtained from equations (10):
c1 p x  s1 p y  a3 c 23  d 4 s 23  a 2 c 2 
 (13)
 p z  a3 s 23  d 4 c 23  a 2 s 2 
The sum of squares of simultaneous equations (11) and equations (13) is:
a3 c3  d 4 s 3  k (14)

p x2  p y2  p z2  a 22  a32  d 22  d 42
In the formula: k  .
2a 2
Similarly, using trigonometric substitution to obtain  3 :


 3 =atan2 a 3 , d 4  -atan2 k , a32  d 42  k 2  (15)

In the formula, the positive and negative signs represent the two possible solutions
corresponding to  3 .
Find  2
To solve  2 , multiply the left side of equation (7) by an inverse transformation of
T3 1， 2， 3  :
0 1

T3 1， 2， 3 0T6  3A4 4 A5 5 A6  3T6

0 1
(16)
By making the elements in the first row, fourth column, and second row, fourth column of the
matrix equation correspond equally on both sides, we can obtain:
c1c 23 p x  s1c 23 p y  s 23 p z  a 2 c3  a3 
 (17)
 c1 s 23 p x  s1 s 23 p y  c 23 p z  a 2 s3  d 4 

By solving equations (17) together, s 23 and c23 can be obtained, and then  23 can be obtained.
Therefore, it can be concluded that:
 a3  a 2 c3  p z  c1 p x  s1 p y a 2 s3  d 4 ,
 23   2   3 =a tan 2 
 d 4  a 2 s3  p z  c1 p x  s1 p y a 2 c3  a3  
(18)

Since  3 has already been calculated in the above, obtaining  2 is:

 2   23   3 (19)

By using this method to calculate  4 ,  5 and  6 in sequence, we can obtain:

 4 =a tan 2 a x s1  a y c1 ,a x c1c23  a y s1c23  a z s23  (20)

95
  5 =atan2 s5 , c5  (21)

 6 =atan2 s6 ,c6  (22)

4. Robot trajectory planning

The so-called trajectory planning is to design the state parameters of the end effector during the
working process based on job requirements, specific environments, and specified motion
characteristics, in order to find a collision free path from the starting state to the target state. To
enable a robot to complete a given task, it is necessary to plan and coordinate its trajectory. When
robots work under continuous trajectory control, trajectory planning in Cartesian space (Cartesian
space) is usually used to obtain the function of the pose of the end effector with respect to time.
This trajectory planning method requires a large amount of computation and may have problems
such as spatial homogeneity points and sudden changes in joint variable curves. When the robot
performs trajectory planning in joint space, that is, point-to-point motion planning, the motion of
each joint of the robot does not need to be linked, only focusing on the pose of the end effector
starting and ending points, without any requirements for the process motion pose. Under this
method, the rotation angle of each joint is the main variable for trajectory planning. Within the
range of joint motion limitations, there will be no problem of exceeding limits and singularities, and
the solution operation is simple. Therefore, this article mainly conducts robot trajectory planning in
joint space.
To plan the motion trajectory in joint space, inverse kinematics solution is required. The angle
values of each joint point are obtained by solving the passing points, and the smooth function with
respect to time is determined by using the angle value as a variable. At the same time, in order to
better analyze the motion characteristics, it is necessary to determine the function of motion
parameters such as joint angular velocity and angular acceleration with respect to time. It should be
noted that there are more than one function that satisfies the above waypoints, so constraint
conditions are required. The pose constraints of the starting and ending points, as well as the
velocity of the starting and ending points, are zero. The trajectory planning solution is obtained
through polynomial interpolation method. In this paper, the path planning of the robot is carried out
using the fifth degree polynomial interpolation method[4].
In joint space, the fifth degree polynomial interpolation method is used for path planning. For the
smoothing function  (t ) of each joint, the fifth degree polynomial has 6 undetermined coefficients.
In order to achieve smooth motion of the joint, 6 constraint conditions are required: position
constraints of the two endpoints, velocity constraints, and acceleration constraints. The position
constraint of endpoints refers to the joint angles corresponding to the starting and ending poses.
Assuming that the initial angle of a joint satisfies:
 (0)   0

 (0)   0 (23)
 
 (0)  a 0
At the termination point, the joint needs to meet the following conditions:

96
  ( f )   f
 
 ( f )   f (24)
 
 ( f )  a f
These 6 constraints can uniquely determine a fifth degree polynomial:
 t   k0  k1t  k 2t 2  k3t 3  k 4t 4  k5t 5 (25)
If the joint angular velocity of the path is (t ) and the joint angular acceleration is a (t ) , then:

 (t )   (t )  k1  2k 2 t  3k 3t 2  4k 4 t 3  5k 5 t 4 (26)

a(t )   (t )  2k 2  6k 3t  12k 4 t 2  20k 5 t 3 (27)
Substituting equations (23) and (24) into equations (25), (26), and (27) respectively, the system
of equations can be obtained as follows:
k 0  0
k  0
 1
k a
 0
 2 2
 20 f  20 0  (8 f  12 0 ) f  (3a 0  a f ) f 2
k  (28)
 3 2f 3

 30 f  30 0  (14 f  16 0 ) f  (3a 0  2a f ) f 2

k 4 
2f 4

k 12 f  12 0  (6 f  6 0 ) f  (a 0  a f ) f 2
 5 
 2f 5
By substituting it into equation (25), a unique equation curve can be obtained.

5. Matlab simulation of robots

5.1. Simulation of robot forward and inverse kinematics

The modeling and simulation of robots are based on the established D-H parameter table, using
the LINK function in the MatlabRobotics toolbox to determine the robot object, and using this robot
function to establish the entire robot object. The calling format of the LINK function is:
L  LINK ([theta D A alpha sigma], CONVENTION )
In the above calling format, theta represents the rotation angle of the joint; D represents the
offset of the connecting rod, which is the distance between the x-axis of adjacent joints; A
represents the length of the connecting rod; Alpha represents the torque angle of the connecting rod;
Sigma represents joint types: 1 is a moving joint, 0 is a rotating joint; The parameter
CONVENTION includes two options: "standard" is the standard D-H parameter representation;
"Modified" is an improved D-H parameter representation.
The program for solving the forward and inverse kinematics of the PUMA560 robot is as
follows:

97
 >> % theta d a alpha offset
L11=Link([0 0 0 0 0 ],'modified');L21=Link([0 0 0 pi/2 0 ],'modified');
L31=Link([0 0.1501 0.4318 0 0 ],'modified');L41=Link([0 0.4318 0.0203 -pi/2 0 ],'modified');
L51=Link([0 0 0 pi/2 0 ],'modified');L61=Link([0 0 0 -pi/2 0 ],'modified');
h=SerialLink([L11 L21 L31 L41 L51 L61],'name','puma560');
>> % Forward kinematics solution
h;fkine(h,[0,pi/2,0,0,pi,0])
ans =
0 0 1 -0.4318
0 1 0 -0.1501
-1 0 0 0.4521
0 0 0 1
The obtained matrix is the homogeneous transformation matrix corresponding to the pose of the
end effector.
The inverse kinematics solution involves finding the corresponding joint variables through a
given homogeneous transformation matrix. For example, assuming that the robotic arm needs to
move to the [0, - pi/4, - pi/4,0, pi/8,0] pose, the homogeneous transformation matrix corresponding
to the end effector pose is:>> q=[0,-pi/4,-pi/4,0,pi/8,0]
q = 0 -0.7854 -0.7854 0 0.3927 0
>> T=fkine(h,q)
T=
0.3827 0 0.9239 0.7371
0 1 0 -0.1501
-0.9239 0 0.3827 -0.3256
0 0 0 1
Now assuming that the homogeneous transformation matrix T mentioned above is known, the
corresponding joint rotation angle can be obtained through the inverse solution function ikine:
>> qi=ikine(h,T)
qi =0.0000 -0.7854 -0.7854 0.0000 0.3927 -0.0000
It is assumed that the robotic arm needs to move to the joint values of [0, - pi/4, - pi/4,0, pi/8,0]
postures.

5.2. Robot trajectory planning and simulation

Due to space constraints, the planning and simulation program has been omitted[5]:
% theta d a alpha offset
L11=Link([0 0 0 0 0 ],'modified');L21=Link([0 0 0 pi/2 0 ],'modified');
L31=Link([0 0.1501 0.4318 0 0 ],'modified');L41=Link([0 0.4318 0.0203 -pi/2 0 ],'modified');
L51=Link([0 0 0 pi/2 0 ],'modified');L61=Link([0 0 0 -pi/2 0 ],'modified');
h=SerialLink([L11 L21 L31 L41 L51 L61],'name','puma560');
...
plot(1+1+t3,q3dd);xlabel ('time (s) ');ylabel ('angular acceleration (rad/s) ');legend ('joint 1 ','
joint 2 ',' joint 3 ',' joint 4 ',' joint 5 ',' joint 6 ')
The obtained 3D model of PUMA560 robot is shown in Figure 2, and the angular displacement,
angular velocity, and angular acceleration curves of the robot are obtained, as shown in Figures 3, 4,
and 5.

98
 Figure 2: 3D model of PUMA560 robot
It is also possible to manually drive the six sliding bars on the left side of the robot as shown in
the right figure of Figure 2 to drive the various joints of the robot, in order to achieve the goal of
driving the end effector of the robot.

Figure 3: Angular displacement variation curve

Figure 4: Angular velocity variation curve

Figure 5: Angular acceleration variation curve

99
6. Conclusion

This article first analyzes the establishment of a six degree of freedom robot kinematic model
based on the D-H parameter method, analyzes the linkage transformation matrix, and then obtains
the relationship between the end effector of the six degree of freedom robot and the base system.
Next, the structure, D-H parameters, and forward and inverse kinematics solutions of the PUMA560
robot were analyzed. Then, Matlab Robotics Toolbox was used to simulate and verify the forward
and inverse kinematics of the PUMA560 robot. Finally, the fifth degree polynomial interpolation
method was used to simulate the joint space trajectory planning of any two points of the robot's end
effector. The specific conclusion is as follows: (1) Sorted out the relevant rules for robot parameter
modeling and established the D-H parameters of the robot. According to the simulation verification
of forward and inverse kinematics, the given homogeneous transformation matrix T is consistent
with the corresponding joint angle values; (2) Using fifth degree polynomials in joint space to
simulate the trajectory planning of PUMA560 robot at any two points, smooth transition curves of
displacement, velocity, and acceleration can be obtained, indicating the smooth motion of the robot
and verifying the rationality of trajectory planning; (3) Through the driving bar in the 3D model of
Matlab Robotics Toolbox, real-time changes in robot joint angles can be visually displayed through
manual operation. Simultaneously using the Matlab Robotics toolbox can improve robot efficiency,
reduce costs, and provide reference for the development of related six degree of freedom robots[6].

References
[1] Peter Corke. Robotics, Vision and Control: Fundamental Algorithms in MATLAB [M]. BeiJing Publishing House of
Electronics Industry. 2016.5.
[2] Cai Zixing. Fundamentals of Robotics [M]. Beijing: Mechanical Industry Press, 2015.3.
[3] Liu Chang, Li Haihong. Course Design Tutorial for Industrial Robotics [M]. Beijing: Mechanical Industry Press,
2022.1.
[4] Wang Na. Simulation study of PUMA560 robotic arm based on MATLAB/Robotics Toolbox [J]. Science and
Technology Vision. 2017. 04. 004.
[5] Li Hui. Robot trajectory optimization and simulation based on MATLAB [M]. Beijing: Beijing University of Posts
and Telecommunications Press, 2019.10.
[6] Ren Jun, Wu Zhenghu, Cao Qiuyu. ER50 robot trajectory planning and simulation based on MATLAB Robotics
toolbox [J]. Mechanical Design and Manufacturing. 2022.08.033.

100