MATLAB Simulation of Collision Free Path Planning &

Trajectory Planning of a 2-link Planar Manipulator

KARNIKA BISWAS1 ANANDA SANKAR KUNDU2

1,2
BENGAL ENGINEERING AND SCIENCE UNIVERSITY, SHIBPUR

Abstract- This paper features the MATLAB simulation of path planning of a 2-link planar
manipulator having static obstacles in the workspace using potential field method. The inverse
kinematics is solved by Jacobian method and a cubic-spline trajectory is constructed between
the start, intermediate and the destination points in joint space. At the intermediate points
continuity in position, velocity and acceleration is maintained.

Keywords: potential field, cubic-spline, inverse kinematics, MATLAB simulation.

This paper is organized as following. Section II

I. INTRODUCTION
presents the path planning strategy. Section III
Motion planning for a manipulator broadly presents the inverse kinematics solution. Section
consists of two tasks, path planning and IV presents the trajectory planning with
trajectory planning. Path planning is the mathematical formulation. Section V presents
generation of a sequence of discrete set points simulation results. Finally Section VI outlines
for the control of manipulator from an initial the main conclusions.
position to a final position in its configuration
II. POTENTIAL FIELD BASED PATH
space. These set points expressed with a time
PLANNING
stamp and generally approximated or
interpolated as a polynomial function to achieve Potential field is one of the well known and
a smooth motion is called trajectory planning. popular methods of finding a collision free path;
Path planning is often an offline event whereas specifically designed for manipulator motion but
trajectory planning is usually implemented as an also considered in the domain of mobile
online tracking task. In this paper the set points robotics. The fast, flexible, easy implementation
generated by path planning without collision of the gradient based algorithm, no dependence
with known static positions of the obstacles are on the occupancy density of the environment
interpolated to generate cubic trajectory and the inherent optimality in its solution makes
functions for each joint variable and simulated it a preferable choice over the graph based and
with MATLAB. Complete description of the cell decomposition algorithms [7]. This method
motion of a manipulator with more than one considers the end-effector as a point under the
joint can be achieved by generating trajectory combined influence of attractive and repulsive
functions for all the joints in a similar way. field or gradient. The goal acts as the source of
Trajectory planning is constrained in terms of an attractive force and the obstacles impart
the manipulator geometry and dynamics. This repulsive forces on the end-effector. The goal is
can be solved in both Cartesian space and joint the single global minima and the obstacles are
space, the latter preferred over the former owing the peaks or the maxima. The gradient-descent
to complex nonlinear dynamics equations and algorithm solves this optimization problem.
non-orthogonality of the joint motions. Addressing issues of simplicity and stability, the
attractive field comprises of a linear conic-well The procedure can be iterated multiple times
potential and a continuously differentiable until the error matrix satisfies certain tolerance
quadratic parabolic-well. The repulsive field is factor.
also a continuously differentiable potential xinitial ,yinitial

function defined to operate within a distance of 2

L2
influence. Only obstacles within this distance xfinal ,yfinal
from the end-effector produce significant Elbow up

repulsive force. The mathematical equations ψ

Elbow down
governing the potential field method are [1]: L1
1 φ

(1)

(2) Fig.1. Schematic representation of planar 2-link

manipulator showing initial & final positions
η (3)
0
The solution for the joint angles 1 and 2 are
calculated with the half angle formula instead of
U(q) is the artificial potential field and F(q) is
the arccos function due to its inherent
the effective force exerted on the end-effector
inaccuracy [8]. The arctan function gives two
where q = (q1, · · · qn)T are the points in
solutions separated by π which result in same
configuration space. ρf (q) be the Euclidean
solution when multiplied by 2. However the
distance between q and qfinal. ζ is a parameter
square-root still generates two solutions which
used to scale the effects of the attractive
are known as Elbow Up and Elbow Down
potential. d = ρf (q) is the boundary. ρ0 to be the
configurations. Depending on the occupancy of
distance of influence of an obstacle η is a scalar
the environment either or any one of the
gain coefficient that determines the influence of
solutions is chosen. The solution of 1 and 2 are
the repulsive field.
given by the mathematical equations given
below:
III. INVERSE KINEMATICS
(8)
The task space and the joint space of a
manipulator are related by the Jacobian matrix. φ- ψ (9)
For a planar non-redundant manipulator, the task (10)
space comprises of two positional coordinates.
Given, the link parameters L1 and L2 and the (11)
initial and final Cartesian coordinates xinitial,
yinitial, xfinal,yfinal, the equations governing the
joint and task space coordinates are: IV. FORMULATION OF 2-CUBIC SPLINE
TRAJECTORY

(4) The desired point-to-point trajectory consists of

a number of nodes including the initial, final and
(5) intermediate or via points described in the joint
space such that, the several segments are
(6) connected at the via points with continuity in
position, velocity and acceleration. If the nodes
(7) are connected by straight line trajectory, the
corresponding joint will come to a stop at each polynomials between two nodes with an
node before proceeding to the next node. A intermediate node is given. The position,
trapezoidal velocity profile is also very popular, velocity and acceleration at the start,
but it suffers from the drawback that the joint intermediate and end nodes are specified as pi, vi,
acceleration undergoes an abrupt change leading ai, pi+1,vi+1,ai+1, pi+2,vi+2,ai+2 respectively. Time
to jerky motion which leads to mechanical wear instants t i indicates t0+i*ts where i is the instant
and tear. To realize a smooth, jerk free motion, a and ts is the sampling time. It has been assumed
higher order polynomial is required. However, that the joint reaches one node to the next in one
larger the order of the polynomial, more sample time.
complex the solution becomes. Alternatively, the
trajectory between two nodes can be considered (12)
as cubic polynomials, splined together satisfying (13)
the boundary conditions of position, velocity and 2*a12 +6*a13*t0 = 2*a22 +6*a23*t0 (14)
acceleration continuity. Cubic polynomials result a11 +2*a12*t1+3*a13*t12 = v1 (15)
in smaller overshoot of angular displacement. a21 +2*a22*t1+3*a23*t12 = v1 (16)
There is an added advantage in using piecewise
cubic polynomials that for n number of nodes a10+a11*t1+a12*t12 +a13*t13 = p1 (18)
only n-2 equations have to be solved [4]. Cubic a20+a21*t2+a22*t22 +a23*t23 = p2 (19)
polynomials are developed for each joint. The
spline is described in terms of the time interval Here, aij are the coefficients of the cubic
between the nodes. It is carefully considered that polynomials. Matrix solution of the 8 equations
at any node the joint angular velocity does not gives unique values for the 8 unknown
exceed the specified constraint. However, coefficients of the cubic polynomial.
acceleration and jerk may appear close to
constrained values. A high acceleration increases
V. SIMULATION RESULTS
the velocity quickly. This approach is especially
suitable for producing time optimized trajectory Figure 3 shows simulation results for trajectory
function. generation with initial position, velocity, final
position, velocity, intermediate position and
p1,v 1, a1, t1 maximum velocity constraint taken as inputs
from the user.

p0, v0, a0, t0

p2, v2, a2, t2
a10+a11t+a12t^2+a13t^3
a20+a21t+a22t^2+a23t^3

Fig. 2. 2-cubic polynomial between 3 nodes showing the

node position, velocity, acceleration and time instant
Fig.3. Blue-position, Red-velocity, Green-

The mathematical equations of the boundary Acceleration, Magenta-jerk plots with respect to.

conditions limiting the generation of two cubic time in x-axis

Fig.4. Start, intermediate and final positions of the
manipulator in unconstrained Cartesian space Fig 5. 3D mesh plot of obstacle positions showing
initial, final coordinates of the end effector and the
Figure 4. refers to the inverse kinematics of the path followed
2-link planar manipulator which takes initial,
final and intermediate positions and inputs,
provided link parameters are known. Table 1.
shows a sample of coordinate errors at
intermediate and final nodes.

xerr1 yerr1 xerr2 yerr2

0.0190 0.0432 0.0107 0.0664

Table 1. Error in position in the intermediate and final

nodes
x 2 3 4 5 6 7 8 9 10
y 2 3 4 5 4 4 4 4 4

Fig 6. Contour plot showing peaks or obstacles,

Table 2. Subset of intermediate nodes generated from global minima or goal and the collision free path of
potential field plot the end-effector

x 30 30 50
y 10 50 10

Table 3. Trial obstacle positions for path planning with

initial and final coordinates (0,0) and (100,100)
respectively

Fig 7. Quiver plot showing velocity profile

Table 2 and 3 show the obstacle positions, initial when obstacles are present in the environment.
and final positions of the end-effector of the
manipulator taken as input from user and the
VII. REFERENCES
discrete intermediate points generated as output
[1] M.W. Spong, S. Hutchinson, M. Vidyasagar, Robot Modeling and
of the path planning strategy. Figure 5 ,6 and 7
Control ,John Wiley & Sons,Inc, NewYork
show the mesh , contour and quiver plots
[2] J.J. Criag, Introduction to Robotics: Mechanics and Control,
showing the obstacles and the collision free path
Prentice-Hall.3rd Edition
and the goal position. The contour plots show
[3] Fu, Gonzalez and Lee, Robotics, Control, Sensing Vision and
the equipotential lines and the quiver plot is the
Intelligence, McGraw Hill
gradient or the velocity vector profile.
[4] C Lin, P.R. Chang and J.Y.S.Luh,”Formulation and Optimization of

Cubic Polynomial Joint Trajectories for Industrial Robots”,IEEE

VI. CONCLUSIONS Transactions on Automatic Control,Vol.AC-28,No.12,Dec 1983

[5] T.C.Manjunath,”Trajectory Planning Design Equations and Control

The potential field method generates a collision of a 4-axes Stationary Robotic Arm”, International Journal of

free path for the end effector and the plots are Computational and Mathematical Sciences,1:1 2007

in tune with expected results. The path [6] B.I.Kazeem,A.I.MAhdi.A.T.Oudah,”Motion Planing for a Robot

planning algorithm also generates the Arm by Using Genetic Algorithm”,Jordan Journal of Mechanical and

intermediate points through which the end- Industrial Engineering,Vol 2, No. 3,Sept.,2008

effector should pass in between the start and [7] R. Siegwart, I.R.Nourbakhsh,”Introduction to Autonomous Mobile

the goal positions. These discrete set points are Robots”,Prentice Hall,2005

utilized in solving the inverse kinematics [8] Brady, J.M., Hollerbach, J.M., Johnson, T.L., Lozano-Perez, T., and

problem solves for the joint angles. This Mason, M.T., eds., Robot Motion: Planning and Control, MIT Press.

converts the Cartesian space problem into a [9] M.Khatib,R.Chatila, “An extended potential field approach for

joint space problem. The plot of the motion of mobile robot sensor-based motions”, Proceedings of Intelligent

the manipulator with given set points are Autonomous Systems IAS-4.

shown and the associated error is tabulated. [10] Jose Mireles Jr. EE Systems and Control Course, Path Planning

However accuracy can be increased by Mobile Robots

iterative means with a low tolerance value.

Finally , the cubic spline trajectory is built with
continuity in position, velocity and acceleration
at the intermediate nodes as shown in Figure 2
and 3 .The extended potential field algorithm[9].
by Khatib and Chatila shall yield better
accuracy in obstacle avoidance. A modified
potential field method can also be used which
avoids the local minima problem by
implementing a random walk yielding results
more generic in nature. In Trajectory Planning
genetic algorithms can be implemented to
address optimization problems of minimum
jerk trajectories for smoother motion. This
experiment can also be extended to redundant
manipulators which shall increase the reach
ability of any point in the configuration space