See discussions, stats, and author profiles for this publication at: https://www.researchgate.

net/publication/271852686

Obstacle Avoidance and Dynamic Control of a Kinematically Redundant 3-DOF

Planar Manipulator

Technical Report · March 2015

DOI: 10.13140/2.1.3085.9048

CITATIONS READS

0 423

1 author:

Mehari K. Tesfay
ACTuate
10 PUBLICATIONS 104 CITATIONS

SEE PROFILE

All content following this page was uploaded by Mehari K. Tesfay on 06 February 2015.

The user has requested enhancement of the downloaded file.

 Robotics and Sensor Data Fusion • • • February 6, 2015• • • Project

Obstacle Avoidance and Dynamic Control of a Kinematically

Redundant 3-DOF Planar Manipulator

Tesfay Mehari Kebede,1

Masters Degree in Mechatronics Engineering

Universita Degli Di Trento - Trento
mehari58@gmail.com

Abstract. This report deals with kinematic and dynamic control algorithms for on-line obstacle
avoidance and trajectory tracking which allow a kinematically redundant manipulator to move in
an unstructured environment without colliding with obstacles.The dynamic control law guarantees
the tracking of a given end effector trajectory and also provides for the control of the redundant joint
velocity.The extra degrees of freedom of a redundant manipulator are exhibited as joint velocities that
do not contribute to the velocity of the end effector. The vast majority of work to date concerned with
obstacle avoidance for manipulators has dealt with task descriptions in the form of pick-and-place
movements. The added flexibility in motion control for manipulators possessing redundant degrees of
freedom permits the consideration of obstacle avoidance in the context of a specified end-effector trajec-
tory as the task description. The approach presented here is to determine the required joint angle rates
for the manipulator under the constraints of multiple goals, the primary goal described by the specified
end-effector trajectory and secondary goals describing the obstacle avoidance criteria. The decomposi-
tion of the solution into a particular and a homogeneous component effectively illustrates the priority
of the multiple goals that is exact end-effector control with redundant degrees of freedom maximizing
the distance to obstacles.

Keywords: Direct Kinematics, Inverse Kinematics, Obstacle Avoidance, Model Control, Ja-
cobian

1 Introduction

Kinematically redundant manipulators can be defined as manipulators that possess

more than the required degrees of freedom to arbitrarily position and orient their end-
effectors in space. These extra, so-called redundant degrees of freedom result in greater
dexterity and flexibility in the specification of motion for the manipulator [1]. This re-
port presents a formulation that allows the use of these redundant degrees of freedom
so that a manipulator can avoid obstacles in the workplace while completing a specified
task. Obstacles are defined as any portion of an object with which contact is undesirable.

One of the goals of the robotic research is to provide control algorithms which allow
robots to move in an environment with obstacles. The natural strategy to avoid obstacles
would be to move the manipulator away from the obstacle into the configuration where
the manipulator is not in the contact with the obstacle. Without changing the motion of
the end-effector, the reconfiguration of the manipulator into a collision-free configura-
tion can be done only if the manipulator has redundant degrees-of-freedom (DOF). The
flexibility depends on the degree-of-redundancy, i.e. on the number of redundant DOF.
A high degree-of-redundancy is important especially when the manipulator is working
in an environment with many potential collisions with obstacles. For obstacle avoidance

1
 Robotics and Sensor Data Fusion • • • February 6, 2015• • • Project

several local strategies have been developed. The local strategies treat the obstacle avoid-
ance as a control problem [4].

Fig. 1: Three-DOF Planar Manipulator

Like most of the local strategies that solve the obstacle avoidance problem at the kine-
matic level [4], the approach presented in this report is to assign each point on the body
of the manipulator, which is close to the obstacle, a motion component in a direction
away from the obstacle. The motion avoidance strategy is achieved by maximizing the
distance between the critical point and the obstacle.

Clearly, obstacle avoidance is essential for satisfactory task completion. Current manip-
ulator applications typically involve removal of potential obstacles from the manipula-
tor’s workspace and the use of fixed motion commands. While preventing manipulator
collisions, this method is unduly restrictive because of its inability to deal with unpre-
dictable or dynamic environments. The satisfaction of the obstacle avoidance criteria for
such environments would greatly increase the autonomy of the manipulator and result
in a wider range of applications that could benefit from the advantages of automation
[1].

2 Direct Kinematics

The mechanical structure of a manipulator is characterized by a number of degrees of

freedom (DOFs) which uniquely determine its posture.Each DOF is typically associated

2
 Robotics and Sensor Data Fusion • • • February 6, 2015• • • Project

with a joint articulation and constitutes ajoint variable.The aim of direct kinematics is to
compute the pose of the end-effector  asa function of the joint variables.
q1 " #
  pe
Defining the joint variables as: q = 
q2  and end-effector position vector by xe = φ

e
q3
, where qi = θi . Accounting for the dependence of position and orientation from the
joint variables, the direct kinematics equation can be written in a form:

xe = K (q)

, for our
 case the direct kinematic equation can be written as: 
px a1 cosθ1 + a2 cos(θ1 + θ2 ) + a3 cos(θ1 + θ2 + θ3 )
   
xe =  p
  y
 = K (q) =  a1 sinθ1 + a2 sin(θ1 + θ2 ) + a3 sin(θ1 + θ2 + θ3 ) 
 
φe θ1 + θ2 + θ3
Since in this case we are dealing with a redundant manipulator where we want to con-
strain only the x and y positions of the end effector, the third row of the kinematic
equations is not important. Therefore:
" # " #
px a1 cosθ1 + a2 cos(θ1 + θ2 ) + a3 cos(θ1 + θ2 + θ3 )
xe = = K (q) =
py a1 sinθ1 + a2 sin(θ1 + θ2 ) + a3 sin(θ1 + θ2 + θ3 )
And the velocity of the end effector as a function of joint velocities is given by:
" #
ṗ x
ẋe = ve = = J (q)q̇
ṗy
, Which represents the differential kinematics equation of the manipulator and J (q) is a
2 × 3 Jacobian Matrix.

3 Inverse Kinematics

The inverse kinematics problem consists of the determination of the joint variables cor-
responding to a given end-effector position and orientation. The solution to this problem
is of fundamental importance in order to transform the motion specifications, assigned
to the end-effector in the operational space, into the corresponding joint space motions
that allow execution of the desired motion.

The inverse kinematics problem, unlike the direct one, is much more complex for the
following reasons[2]:
• The equations to solve are in general nonlinear, and thus it is not always possible to
find a closed form solution
• Multiple solutionsmay exist.
• Infinite solutions may exist, e.g., in the case of a kinematically redundant manipula-
tor.
• There might be no admissible solutions, in view of the manipulator kinematic struc-
ture.

3
 Robotics and Sensor Data Fusion • • • February 6, 2015• • • Project

Considering a full rank and square Jacobian matrix, the joint velocities can be given as:

q̇ = J −1 ve

In our case,however, the manipulator is redundant and the Jacobian is not square matrix
and it can not be inverted. This problem can be solved by the introduction of a Pseudo-
inverse matrix of the Jacobian.

q̇ = J T ( J J T )−1 ve = J + ve

4 Dynamic Model and Control

The problem of controlling a manipulator can be formulated as that to determine the

time history of the generalized forces (forces or torques) to be developed by the joint
actuators, so as to guarantee execution of the commanded task while satisfying given
transient and steady-state requirements. The task may regard either the execution of
specified motions for a manipulator operating in free space, or the execution of speci-
fied motions and contact forces for a manipulator whose end-effector is constrained by
the environment[2].
Without any concern to the specific type of mechanical manipulator, it is worth remark-

Fig. 2: Overall System Design

ing that task specification (end-effector motion and forces) is usually carried out in the
operational space, whereas control actions (joint actuator generalized forces) are per-
formed in the joint space. This fact naturally leads to considering two kinds of general
control schemes, namely, a joint space control scheme and an operational space control
scheme[2].

The dynamic control law guarantees the tracking of a given end effector trajectory and
also provides for the control of the redundant joint velocity.The extra degrees of freedom
of a redundant manipulator are exhibited as joint velocities that do not contribute to the
velocity of the end effector. The joint space control problem is actually articulated in two
subproblems.

4
 Robotics and Sensor Data Fusion • • • February 6, 2015• • • Project

First, manipulator inverse kinematics is solved to transform the motion requirements xd

from the operational space into the corresponding motion qd in the joint space. Then,
a joint space control scheme is designed that allows the actual motion q to track the
reference inputs. Having qd as a constant equilibrium posture assigned for the system
as the vector of desired joint variables, It is desired to find the structure of the controller
which ensures global asymptotic stability of the above posture.

Fig. 3: The Designed Dynamic Control System

Dynamic model of a non-elastic system given by the manipulator and drives is described
by a generic motion equation:

B(q)q̈ + C (q, q̇)q̇ + F q̇ + g (q) = u (1)

,where B(q), C (q, q̇), F and g (q) represent the mass matrix, Coriolis and Centrifugal
Matrix, Frictional Forces and Gravitational forces respectively. The whole idea behind
the control strategy lies on finding a control vector u a function of the system state, which
is capable of realizing an input/output relationship of linear type; in other words, it is
desired to perform not an approximate linearization but an exact linearization of system
dynamics obtained by means of a nonlinear state feedback. Considering the control u as
a function of manipulator state:

Assuming a new unidentified input vector

q̈ = v

, The nonlinear control law above is termed inverse dynamics control since it is based
on the computation of manipulator inverse dynamics. The system under control[1] is
linear and decoupled with respect to the new input v. In other words, the component
vi influences, with a double integrator relationship, only the joint variable qi , indepen-

5
 Robotics and Sensor Data Fusion • • • February 6, 2015• • • Project

dently of the motion of the other joints[2].

Let’s choose the v to assume to form:

v = q̈d + Kd (q̇d − q̇) + K p (qd − q)

q̈ = q̈d + Kd (q̇d − q̇) + K p (qd − q)
q̈d − q̈ + Kd (q̇d − q̇) + K p (qd − q) = 0
ë + Kd ė + K p e = 0

, since matrices K p and Kd are assumed to be diagonal matrices which control the speed
of convergence, the error asymptotically converges to zero to satisfy the equation. There-
fore the control input finally takes the form:

u = B(q)(q̈d + Kd (q̇d − q̇) + K p (qd − q)) + C (q, q̇)q̇ + g (q)

It can be seen that as part of the control scheme in fig[3], a nonlinear compensation
action of gravitational terms with a linear proportional-derivative (PD) action has been
employed.

4
theta1
3 theta2
Radian−Simulated Joint Angles

theta3
2

−1

−2

−3
0 1 2 3 4 5 6 7 8 9 10
Time

4
theta1
3 theta2
Radian− Actual Joint Angles

theta3
2

−1

−2

−3
0 1 2 3 4 5 6 7 8 9 10
Time

Fig. 4: Simulated and Actual Joint Angles

5 Obstacle Avoidance Algorithm

The redundant nature of the manipulator gives us an advantage to exploit it to achieve

another secondary task other than the primary purpose of trajectory tracking. By creat-

6
 Robotics and Sensor Data Fusion • • • February 6, 2015• • • Project

ing an internal joint motion it’s possible to dodge obstacles that exist in the manipulator
environment with out changing the end-effector position. The inverse kinematic formula
can be written in a new form that enables us to implement this advantage[3].

q̇d = J + ve + ( I − J + J ) q̇0
| {z }
NullProjector

, Where q̇0 is a vector in q̇ space that can be arbitrarily selected since it affect the end-
effector position as it’s in the kernel of the transformation Jacobian J from q̇ to ve .

Formulation

Considering a perpendicular distance from the obstacle point to a generic point on the
extended link length, which means the shortest possible distance between obstacle point
and the link vector and defining the following unit vectors along each link and and from
obstacle point to each point of perpendicularity on the extended link vector, we can for-
mulate a way that leads us towards a solution for our problem.
   
t̂1 â1 " #
ox
t̂ =  t̂2  , â =  â2 , and the obstacle point position vector is O = and
   
oy
t̂3 â3
finally we have the position vectors d1 , d2 and d3 at the points of perpendicularity. From
âi ⊥ t̂i and from the definitions of the unit vectors along the links, we get the following
relations.
" # " # " #
cosθ1 cos(θ1 + θ2 ) cos(θ1 + θ2 + θ3 )
â1 = , â2 = , and â3 =
sinθ1 sin(θ1 + θ2 ) sin(θ1 + θ2 + θ3 )

From the perpendicularity condition t̂iT · âi = 0, the explicit forms for the t̂i versors are
drawn. Considering analysis on the position vectors di at the point of perpendicularity
of each link:

d1 = â1 α1 , again d1 = t̂1 β 1 + O. This leads us to an expression formulated as:

" #
h i α1
â1 −t̂1 =O (2)
β1
Considering the whole link i length as a vector of magnitude ai , we can define them as
follows:

a1 = a1 â1
a2 = a2 â2
a2 = a3 â3

Therefore for d2 , we formulate the following relation

d2 = a2 + â2 α2 = O + t̂2 β 2

7
 Robotics and Sensor Data Fusion • • • February 6, 2015• • • Project

This gives us another important matrix equation:

" #
h i α2
â2 −t̂2 = O − a1 (3)
β2

And for the third link:

d3 = a1 + a2 + â3 α3 = O + t̂3 β 3

, which leads to another matrix formulation:

" #
h i α3
â3 −t̂3 = O − a1 − a2 (4)
β3
Collecting equations [2, 3 and 4], we form the final relation as follows:
 
α1
 
  α   
2
â1 0 0 −t̂1 0 0    O
 α  
 0 â2 0 0 −t̂2 0   3  =  O − a1  (5)
 
 β1 
0 0 â3 0 0 − â3 β 
 O − a1 − a2
 2
β3

, Where αi is a scalar distance from the origin of link i to the point of perpendicularity on
the extended link i and β i is the perpendicular distance from link i to the obstacle point.

The objective is to find the β parameters as function of the joint angles and link lengths.
Therefore:

β i = β i ( θ1 , θ2 , θ3 ) = β i ( q )

If the perpendicularity point lies on the link, 0 ≤ αi ≤ ai , and the corresponding β i ithe
minimum distance between the link and the obstacle point. Otherwise;we can consider
two scenarios where αi > ai and αi < 0 since the calculated β i are not the shortest
distances to the obstacle point. Therefore:
Case 1: αi > ai , q
β i,shortest = (αi − ai )2 + β2i

Case 2: αi < 0 q
β i,shortest = α2i + β2i

Having found the shortest distances from each of the links to the obstacle point, then
the shortest distance β∗ (q) among the three is selected for the current configuration.

How do we select q̇0 ? One of the techniques that can be employed is to choose q̇0 in the
direction that maximizes the gradient of the minimum distance between the obstacle
point and the critical point on the link. Applying the gradient method formula:

8
 Robotics and Sensor Data Fusion • • • February 6, 2015• • • Project

∂B∗ (q) T
q̇0 = K0 ( ) (6)
∂q
, where
B∗ (q) = min||O − di ||

The effects of the

Obstacle Avoidance
0.8
Obstacle Avoidance

0.8 0.7

0.7 0.6

0.6
0.5

y−position, py
0.5
y−position, py

0.4
0.4

0.3 0.3

0.2
0.2
0.1
0.1
0

−0.1 0
−0.5 −0.4 −0.3 −0.2 −0.1 0 0.1 0.2 0.3 0.4 −0.8 −0.6 −0.4 −0.2 0 0.2 0.4 0.6
x−position, px x−position, px

(a) Before Obstacle Avoidance. (b) After Obstacle Avoidance.

Fig. 5: Manipulator movement before and after obstacle avoidance

6 Conclusion

The time history of the joint positions and torques and of the tip position errors for the
trajectory tracking task show that the control scheme used performs well. The redun-
dancy resolution at velocity level has been exploited to achieve extra goals. The primary
task is determined by the end-effector trajectories and for the obstacle avoidance the
internal motion of the manipulator joints is used. The objective is to assign each point
on the body of the manipulator, which is close to the obstacle, a velocity component in a
direction that is away from the obstacle and it is achieved by choosing q̇0 in the direction
that maximizes the gradient of the minimum distance between the obstacle point and
the critical point on the link. As it can be observed from the figures, the algorithm gives
satisfactory results. However, The real-time demands of obstacle avoidance in practical
applications, particularly in dynamic environments, demands a computationally effi-
cient algorithm.

References

1. Anthony A Maciejewski and Charles A Klein. Obstacle avoidance for kinematically redundant manip-
ulators in dynamically varying environments. The international journal of robotics research, 4(3):109–117,
1985.
2. Lorenzo Sciavicco and Luigi Villani. Robotics: modelling, planning and control. Springer, 2009.
3. Bruno Siciliano. Kinematic control of redundant robot manipulators: A tutorial. Journal of Intelligent and
Robotic Systems, 3(3):201–212, 1990.
4. Leon Zlajpah and Bojan Nemec. Kinematic control algorithms for on-line obstacle avoidance for redun-
dant manipulators. In Intelligent Robots and Systems, 2002. IEEE/RSJ International Conference on, volume 2,
pages 1898–1903. IEEE, 2002.

View publication stats