See

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

Obstacle Avoidance for Kinematically

Redundant Robot

Conference Paper October 2015

DOI: 10.1016/j.ifacol.2015.12.176

CITATIONS READS

0 26

5 authors, including:

Xinyu Wang Junshen Chen

Beijing Institute of Technology Swansea University
9 PUBLICATIONS 16 CITATIONS 5 PUBLICATIONS 0 CITATIONS

SEE PROFILE SEE PROFILE

Hongbin Ma
Beijing Institute of Technology
91 PUBLICATIONS 233 CITATIONS

SEE PROFILE

All content following this page was uploaded by Junshen Chen on 03 May 2016.

The user has requested enhancement of the downloaded file. All in-text references underlined in blue are added to the original document
and are linked to publications on ResearchGate, letting you access and read them immediately.
17th IFAC Symposium on System Identification
17th
17th IFAC
IFAC Symposium
Symposium on
on SystemCenter
Identification
Beijing
17th International
IFAC
Beijing Symposium
International on System
Convention Identification
SystemCenter
Convention Identification
Beijing
October International
19-21, 2015.
Beijing International Convention
Beijing, China
Convention Available online at www.sciencedirect.com
Center
Center
October
October 19-21, 2015. Beijing, China
October 19-21,
19-21, 2015.
2015. Beijing,
Beijing, China
China

ScienceDirect
IFAC-PapersOnLine 48-28 (2015) 490495
Obstacle
Obstacle Avoidance
Avoidance for
for Kinematically
Obstacle Avoidance
ObstacleRedundant for Kinematically
for Kinematically
Avoidance Robot ,,
Kinematically
Redundant Robot ,

Redundant Robot ,
Redundant Robot
Xinyu WANG ,, ,, Chenguang YANG ,
,
Xinyu
Xinyu WANG
WANG ,,
Chenguang
Chenguang YANG
YANG
, ,
Junshen
Xinyu WANG CHEN Hongbin
,,
Chenguang MA , YANGFeng,
LIU
Junshen
Junshen CHEN Hongbin MA , Feng LIU
Junshen CHEN CHEN Hongbin Hongbin MA MA , Feng Feng LIU LIU

School of Automation, Beijing Institute of Technology, Beijing,
School of Automation, Beijing Institute of Technology, Beijing,
School of Automation, Beijing Institute
School of Automation, Beijing China,
China,Institute of Technology, Beijing,
of Technology, Beijing,

State Key Lab of Intelligent
China,
Control and Decision of Complex
China,
State Key Lab of Intelligent Control and Decision of Complex
State Key Lab
System,
State Lab of
Key Beijing of Intelligent
Institute ofControl
Intelligent Technology,
Control and
and Decision
Beijing, of
Decision Complex
China,
of Complex
System,
System, Beijing
Beijing Institute
Institute of
of Technology,
Technology, Beijing,
Beijing, China,
China,
Centre for Robotics and Neural Systems,
System, Beijing Institute of Technology, Beijing, China, Plymouth University,
Centre for Robotics and Neural Systems, Plymouth University,
Centre for Robotics and Neural
Centre for Robotics and Neural Systems,
Plymouth,
Plymouth,
UK,
Systems,
UK,
Plymouth
Plymouth University,
University,

College of Automation
Plymouth,
Science and
Plymouth, UK,
UK, Engineering, South China
College of Automation Science and Engineering, South China
College of
College of Automation
University Science
Science and
of Technology,
Automation Engineering,
Guangzhou,
and Engineering, China,South
South China
China
University
University of
of Technology,
Technology, Guangzhou,
Guangzhou, China,
China,
Beijing Jiaotong University,
University of Technology, Guangzhou, China, Beijing,
Beijing Jiaotong University, Beijing, China,
Beijing Jiaotong University, Beijing, China,
Beijing Jiaotong University, Beijing, China,
Abstract: Manipulators obstacle avoidance is always a challenging topic in robotics. With
Abstract:
Abstract: Manipulators obstacle avoidance is always aa challenging topic in robotics. With
the concernManipulators
Abstract: of the safety obstacle
Manipulators problemsavoidance
obstacle in human
avoidance is
is always challenging
challenging topic
workinga environment,
always in
in robotics.
an improved
topic robotics. With
obstacle
With
the
the concern
concern of
of the
the safety
safety problems
problems in
in human
human working
working environment,
environment, an
an improved
improved obstacle
obstacle
avoidance
the concern method
of the is safety
proposed based oninthe
problems general
human solutionenvironment,
working of the inversean kinematic
improved problem
obstacle of
avoidance
avoidance method
method is
is proposed
proposed based
based on
on the
the general
general solution
solution of the inverse kinematic problem of
a redundant
avoidance manipulator.
method is proposed A new
based obstacle
on the detection
general methodof
solution of the
using
the inverse
Kinectkinematic
inverse R sensor is
kinematic problem
designed
problem of
of
a
a redundant
redundant manipulator.
manipulator. A
A new
new obstacle
obstacle detection
detection method
method using
using Kinect
Kinect 
R sensor is designed
R sensor is designed
to give
atoredundanta robust obstacle
manipulator. information.
A new obstacleBy designing
detection a parallel
method system
using of
Kinect the
R manipulator,
sensor is designedthe
to give
give a robust obstacle information. By designing aa parallel system of the manipulator, the
give aa control
proposed
to
proposed
robust
robust obstacle
control
method is
obstacle
method
information.
is
not only to By
information.
not only to By designing
achieve the goal
designing
achieve the a parallel
goal
of moving
parallel
of moving
system
awayof
system
away
the
the manipulator,
offrom
from
the obstacle, but
manipulator,
the obstacle,
the
the
but
proposed
proposed control
also able control method
to guarantee
method is not
theis robot
not only
move
only to
to achieve
back tothe
achieve the goal
its goal
originalof
of moving away
pose. The
moving away from
stability
from the
the obstacle,
ofobstacle,
the closed- but
but
also
also able
able to
to guarantee
guarantee the
the robot
robot move
move back to its original pose. The stability of the closed-
loop
also
loop
system
able
systemto is established
guarantee
is established theusing
using move back
robotLyapunov
Lyapunov back to
to its
direct
direct its original
method
original
method
to pose.
to
ensureThe
pose.
ensure
the stability
The
the
asymptotic
stability
asymptotic
of
of the
the closed-
stability
closed-
stability
of
of
loopsystem.
the
loop system The
system is established
is established
performance using Lyapunov
of the
using simulation
Lyapunov direct
direct method
results
method to ensure
verifies
to ensure the asymptotic
asymptotic
the effectiveness
the stability
of thestability
proposed of
of
the
the system.
system. The
The performance
performance of
of the
the simulation
simulation results
results verifies
verifies the
the effectiveness
effectiveness of
of the
the proposed
proposed
obstacle
the system.detection
The algorithm and
performance of thesimulation
the obstacle avoidance
results method.
verifies the effectiveness of the proposed
obstacle
obstacle detection algorithm and the obstacle avoidance method.
obstacle detection
detection algorithm
algorithm and and the
the obstacle
obstacle avoidance
avoidance method.method.
2015, IFAC (International Federation of Automatic Control) Hosting by Elsevier Ltd. All rights reserved.
Keywords: Kinematically Redundant, Obstacle Avoidance
Keywords: Kinematically
Keywords: Kinematically Redundant, Obstacle Obstacle Avoidance
Keywords: Kinematically Redundant, Redundant, Obstacle Avoidance Avoidance
1. INTRODUCTION dual neural network is applied for the on-line solution
1. INTRODUCTION
1. INTRODUCTION dual
dual neural
neural network
network is is applied
applied for for the on-line
the control solution
on-line problem,
solution
1. INTRODUCTION of
dualobstacle-avoidance
neural network inverse-kinematic
is applied for the on-line solution
of
of obstacle-avoidance
obstacle-avoidance inverse-kinematic
inverse-kinematic control
control problem,
problem,
1.1 Background andobstacle-avoidance
of then simulated based on the PA10 robot
inverse-kinematic controlmanipulator
problem,
1.1 Background
Background and
and then
then simulated
simulated based
based on the PA10 robot manipulator
1.1
1.1 Background in
and
in
the
thethen presence
simulated
presence of based on
of obstacles.
obstacles.on theForPA10
the
For PA10
the
robot
robot manipulator
the real-time
real-time
computa-
manipulator
computa-
Manipulator, as a crucial part of robotics, has been at- in in the
tion, by presence
Zhang
thebypresence of
and
of obstacles.
Wang
obstacles. For
(2003), the
For thethe real-time
the raw
real-time computa-
point cloud
computa-
Manipulator, as a crucial part of robotics, has been at- tion,
tion, by Zhang
Zhang and
and Wang
Wang (2003),
(2003), the raw
raw point
point cloud
cloud
Manipulator,
tracting
Manipulator,muchas as a crucial
attention
a crucialfrom part
partboth of robotics,
industry has
of robotics, has been
and been
researchat- data
tion, has
by been
Zhang used
and for
Wang obstacle
(2003),
at- data has been used for obstacle detection. The robots detection.
the raw The
point robots
cloud
tracting
tracting much
much attention
attention from
from both
both industry
industry and
and research
research data
data has
environment
has been
beenis used
observed
used for
for obstacle
by Photonic
obstacle detection.
Mixer
detection. The
Device
The robots
cam-
robots
communities,
tracting muchand and has
attention become
from botha hot field due to its poten-
communities, has become
become aa hot industry due and
field due itsresearch
to its poten- environment
environment
providingis
is observed
observed by
by Photonic
Photonic Mixer
Mixer Device
Device cam-
On cam-
communities,
tial
communities, and
wide applications
and has
has in on-line
become a hot field
robot,
hot field to
dangerous
due to its tasks, era,
poten-
poten- environment
era, providing is depth
observed
depth
information
by
informationPhotonic for each
for Mixer
each
pixel.
Device
pixel. On
that
cam-
that
tial
tial wide
wide applications
applications in
in on-line
on-line robot,
robot, dangerous
dangerous tasks,
tasks, era,
basis, providing
a depth
3-dimensional information
environment for each
model pixel.
is On
On that
generated
human-robot
tial
human-robot
interaction
wide applications
interaction in and
and
etc. robot,
on-line
etc.
The obstacle
The dangerous
obstacle tasks, era,
avoidance
avoidance basis, providing
a depth information
3-dimensional environment for each
model pixel.
is that
generated
human-robot
is one of the most interaction
crucialand etc.
functions The of obstacle
robot toavoidance
enable it basis, basis,optimized
and aa 3-dimensional
3-dimensional environment
for path environment
planning and model model
obstacle is avoidance,
is generated
generated
human-robot
is one
one ofof the
the mostinteraction
most crucial and etc.
crucial functions
functions ofThe obstacle
of robot avoidance
robot to enable
enable it it and and optimized for path planning and obstacle avoidance,
is optimized
introduced for
for path
by Leutert et planning
al. (2012).and obstacle
obstacle avoidance,
A 3-dimensional Time-
avoid
is one any
avoid of the
any
obstacle
obstacle
that potentially
most crucial
that potentially of robot to
functions interrupt
interrupt
itsenable
to
its
motion,
motion, it and optimized
introduced by path
Leutert et planning
al. (2012). and
A 3-dimensional avoidance,
Time-
avoid any
especially obstacle
when anythat
human potentially
approachinterrupt its
them accidentally.motion, introduced
Of-Flight
introduced by
(TOF)
by Leutert
camera
Leutert et
et al.
is
al. (2012).
used
(2012). as A
Aa 3-dimensional
proximity
3-dimensional Time-
sensor
Time-to
avoid any
especially obstacle
when any that
human potentially
approach interrupt
them its motion,
accidentally. Of-Flight (TOF) camera is used as a proximity sensor to
especially when any human approach them accidentally. Of-Flight
identify
Of-Flight the(TOF)
position
(TOF) camera
of
camera theis used
obstacle
is obstacle as
used as aneara proximity
near the
proximity elbowsensor
of
sensor to
the
to
especially when
Considerable any human
amount of workapproach
has beenthem carriedaccidentally.
out. A wide identify identify the the position of the
the the elbow
elbow of the
the
Considerable amount of work has been carried out. A wide manipulator
identify the position
by of
Natarajan
position of the obstacle
et al.
obstacle near
(2010).
near the
The
the elbow of
trajectory
of the
Considerable
variety amount
amount of
of approaches work
ofhave
workbeenhas
has been
proposed carriedto out. A
A wide
accomplish manipulator
wide manipulator by
by Natarajan
Natarajan et
et al. (2010). The trajectory
Considerable
variety of of approaches
approaches have been been
proposed carriedto out.
accomplish tracking should
manipulator by be carried out
Natarajan et al.
al. (2010).
(2010). The
in operational Thespace trajectory
while
trajectory
variety
the obstacle
variety of avoidancehave
approaches have been
challenge.
been proposed
Obstacleto
proposed to accomplish
detection
accomplish is tracking
tracking
avoiding
tracking
should
should
an
be
be
obstacle.
should be
carried
carried
For
carried
out
out
this
out
in
in operational
operational
purpose,
in extended
operational
space
space while
while
Jacobian
space while
the
the obstacle
obstacle avoidance
avoidance challenge.
challenge. Obstacle
Obstacle detection
detection is
is avoiding an obstacle. For this purpose, extended Jacobian
one
the of the
obstacle top issues
avoidance in obstacle
challenge. avoidance.
Obstacle Most
detectionof the
is avoiding
method
avoiding an
is
an obstacle.
used. The
obstacle. For this
self-motion
For this purpose,
vector
purpose, extended
is introduced
extended Jacobian
at the
Jacobian
one
one of
of the
the top
top issues
issues in
in obstacle
obstacle avoidance. Most of the method is is used.
used. The
The self-motion
self-motion vector vector is is introduced
introduced at at the
the
obstacle
one of thedetection
top algorithms
issues in obstaclewereavoidance.
given with
avoidance. Most
Most of
a premise
of the
the method
level
method of the
is inverse
used. The kinematics
self-motion solution
vector in
is order
introduced to produce
at the
obstacle detection
obstacle detection algorithms
algorithms were were given
given withwith aa premise
premise level of the inverse kinematics solution in order to produce
of obstacle
obstacle or environment.
detection algorithms Such wereas the
given collision
with a surfaces
premise level of
the obstacle
level of the
the inverse kinematics
avoidance
inverse solution
by Benzaoui
kinematics solution in order
etinal.
order to
(2010).
to produce
Thus,
produce
of obstacle or environment. Such as the collision surfaces
of
of obstacle
obstaclesor environment.
need Such
Such as
to be approximated as the
theby collision
local grid point the
surfaces the obstacle
obstacle avoidance
avoidance by
by Benzaoui et
by Benzaoui et al. (2010).
et al. (2010). Thus,
mainThus,
of obstacle
of obstaclesorneed
obstacles
environment.
need to be
to be approximated
approximated collision
by
by local grid
local
surfaces
grid point robot
point the
robot
avoids obstacles
obstacle
avoids avoidancewithout
obstacles without
influencing
Benzaoui
influencing al. the
(2010).
the main
task.
Thus,
task.
sets
of only,
obstacles Schlemmer
need to and
be Gruebel
approximated (1997); by it is
local well
grid suited
point robot
Also,
robot the avoids
the
avoids obstacles
quasi-desired without
point
obstacles without hasinfluencing
been
influencing used the
theby main
Yoo
main task.
et al.
task.
sets only,
sets only, Schlemmer
Schlemmer and and Gruebel
Gruebel (1997);
(1997); it it is
is well
well suited
suited Also, quasi-desired point has been used by Yoo et al.
for real-time obstacle avoidance path planning. Or the
sets only,
for real-time
for
Schlemmer
real-time obstacle and
obstacle avoidanceGruebel
avoidance path (1997); it
path planning. is well
planning. Or Or the the (2013) to avoid obstacle without utilizing information al.
suited Also,
(2013)
Also, the
theto quasi-desired
avoid obstacle
quasi-desired point
without
point has
has been used
utilizing
been used by Yoo
information
by Yoo et
et of
al.
of
forThis
 real-time
work was obstacle
supported avoidance
in part by path grant
EPSRC planning. Or the (2013)
EP/L026856/1, the jointto
(2013) to avoid
angles.
avoid obstacle
For
obstacle without
each joints,
without theutilizing
null space
utilizing information
of Jacobian
information of
of

 This work was supported in part by EPSRC grant EP/L026856/1, the
the joint
joint angles.
angles. For
For each
each joints,
joints, the
the null
null space
space of
of Jacobian
Jacobian
This
Royal
 work
Society was supported
grant in
IE130681, part by
GuangdongEPSRC grant
ProvincialEP/L026856/1,
Natural Sci- is
theused
joint to avoid
angles. collision
For each without
joints, thechanging
null spacethe task
of space
Jacobian
This
Royal work
Society was supported
grant IE130681, in part by EPSRC
Guangdong Provincial grant EP/L026856/1,
Natural Sci- is
is used
used to to avoid
avoid collision
collision without
without changing
changing the the task
task space
space
Royal
ence
Royal Society
Society grant
Foundation grant IE130681,
of China underGuangdong
IE130681, Guangdong Provincial
Provincial Natural
Grant 2014A030313266, State Key
Natural Sci-
Sci- velocity.
is used to avoid collision without changing the task space
ence
ence Foundation
Foundation
Laboratory
of
of
of Robotics
China
China under
under
andunder
System
Grant
Grant
(HIT)
2014A030313266,
2014A030313266,
SKLRS-2014-MS-05,
State
State Key
Key
and
velocity.
velocity.
ence Foundation
Laboratory of of China
Robotics and SystemGrant 2014A030313266,
(HIT) SKLRS-2014-MS-05, State Key
and velocity.
An improved obstacle avoidance method is introduced in
Laboratory
National of
of Robotics
Natural Scienceand System
System (HIT)
Foundation (NNSF) SKLRS-2014-MS-05,
of China under Grants and An
Laboratory
National
National
61473120
Natural
Natural
and
Robotics
Science
Science
61473038.
and
Foundation
Foundation
(HIT)
(NNSF)
(NNSF)
SKLRS-2014-MS-05,
of
of China
China under
under
and
Grants
Grants An improved
this
An improved obstacle
obstacle
paper. Inobstacle
improved
avoidance
avoidance
most of avoidance
the previous
method
method
studies,
method
is
is introduced
is introduced
the obstacle
introduced
in
in
in
National
61473120
 Natural
and Science Foundation (NNSF) of China under Grants
61473038. this
this paper.
paper. In
In most
most of
of the
the previous
previous studies,
studies, the
the obstacle
obstacle
61473120
 and
Corresponding
61473120 61473038.
authors
and 61473038. are C. Yang and H. Ma. Email: or
this surrounding
paper. In environment
most of the information
previous studies, are commonly
the obstacle
 Corresponding authors are C. Yang and H. Ma. Email: or
or surrounding environment information are commonly
Corresponding
Corresponding authors
{cyang;mathmhb}@ieee.org

{cyang;mathmhb}@ieee.orgauthors are are C.C. YangYang and and H. H. Ma.Ma. Email:
Email: or surrounding
surrounding environment
environment information
information are are commonly
commonly
{cyang;mathmhb}@ieee.org
{cyang;mathmhb}@ieee.org
Copyright IFAC 2015
2405-8963 2015, 490 Hosting by Elsevier Ltd. All rights reserved.
IFAC (International Federation of Automatic Control)
Copyright
Copyright IFAC 2015 490
Peer review
Copyright IFAC
under 2015
IFAC responsibility
2015 490
of International Federation of Automatic
490Control.
10.1016/j.ifacol.2015.12.176
 2015 IFAC SYSID
October 19-21, 2015. Beijing, China Xinyu WANG et al. / IFAC-PapersOnLine 48-28 (2015) 490495 491
treated as known conditions. However, the real world chal-
lenges could be more unpredictable, variable and dynam-
ical. On the other hand, other researchers mainly used
the raw data of the point cloud, Leutert et al. (2012);
Zhang and Wang (2003), but the noise in the point cloud
data may affect the performance of the obstacle avoidance,
and the large amount of point cloud data may result in
heavy computational load. In this paper, a new obstacle
detection method using Kinect R is designed to give a
robust obstacle information. The Hough transform has
been performed to detect the obstacle and transfer it into
a segment in the 3-dimensional Cartesian space.
Furthermore, most of the previous studies only concerned
the avoiding process of the manipulators, i.e., moving
away from the obstacle. The obstacle avoidance process
normally would make the manipulator end up at a new
pose, which may not be the optimal pose of the given
end-effector trajectory and may cause the root become
singular and cannot fulfil the remaining tasks. In order
to enable the robot to restore its original pose as well as
the goal of avoiding the obstacle, a parallel system of the
manipulator is designed in the proposed control method.
Based on the obstacle detection results, the collision points
and the distance between the robot and the obstacle are
calculated to obtain the reasonable avoiding and restoring
velocity, by implementing the specific obstacle avoidance
strategy. A weighting function is also presented based on
a fuzzy inference engine to achieve the smooth switching
between the obstacle avoidance mode and the restoring
mode. The stability of the closed-loop system is proved
using Lyapunov direct method to ensure the asymptotic
stability of the system to deal with the real world tasks.
1.2 Organization of This Paper
The remainder of this paper is organized as follows: Section
2 presents the preliminaries used to achieve the obstacle
avoidance; the restoring method after obstacle avoiding is
proposed in Section 3 and the stability of the closed-loop
system is proved; Section 4 presents an obstacle detection
method using Kinect, R which is the precondition to
achieve the obstacle detection; Section 5 shows the specific
strategy and constitutes of the whole system; Section 6
presents the results of the simulations carried out with
these controllers; finally, we conclude this paper in Section
7 by giving some concluding remarks drawn from these
simulations.
2. PRELIMINARIES
Consider one arm of the robot, denote the end-effector
The kinematics
velocity as x and the joint velocities as .
of the manipulator can be given by (1).
x = J , (1)
where J is the Jacobian matrix.
In order to control the robot by the reference Cartesian
space trajectory, the joint velocities are calculated by the
numerical inverse kinematic method. The manipulator will
become kinematically redundant if the dimension of is
larger than the dimension of x, i.e., the degree of freedom
(DOF) in joint space is larger than that required in the
491
task space. In this case, an infinite number of solutions can
be found and the general solution of the inverse kinematic
problem is given by (2).
= J x + (I J J )z, (2)
where J = J T (J J T )1 is the pseudo-inverse of J and
z is an arbitrary vector.
When the obstacle is found near the manipulator arm,
assume that the collision point on the manipulator arm is
pcr , and the desired velocity of the collision point to avoid
the obstacle is x o , as shown in Fig. 1. The manipulator
need to fulfil (3) and (4) at the same time.
x e = Je (3)
x o = Jo , (4)
where x e is the reference end-effector velocity; Je is the
end-effector Jacobian matrix; Jo is the Jacobian matrix of
pcr .
As the desired end-effector trajectory is always given in
a set of position x in Cartesian space, a closed-loop
controller is designed as (5).
x e = x d + Ke, (5)
where e is the end-effector position error between the
desired end-effector position and the actual position, which
can be obtained by the forward kinematics and K is a
symmetric positive definite matrix.
3. RESTORING METHOD
On the other hand, when the obstacle has been removed,
the manipulator is expected to restore its original state
to eliminate the influence of the obstruction. A parallel
system of the manipulator is designed in the controller
to simulate its pose without the influence of the obstacle
in real time. This parallel system is simply controlled
based on the classic inverse kinematic solution given by
the pseudo-inverse of the Jacobian, and is given by (6).
r = J x e ,
e (6)
where r is the joint velocities of the parallel system. The
real manipulator is expected to restore to the pose same as
the parallel system so that the influence of the obstacle can
be eliminated. To achieve that, the elbow of the manipu-
lator is controlled to move toward the corresponding joint
on the parallel system. The manipulator needs to fulfil (3)
and (7) when the obstacle is absent.
x r = Jr , (7)
where Jr is the Jacobian matrix of the elbow; x r is the de-
sired elbow joint velocity moving toward the corresponding
joint on the parallel system, and is designed as a closed-
loop system as
x r = Kr er , (8)
where Kr is a symmetric positive definite matrix and er
is the position error of the elbow joint between the parallel
system and the real system.
Substituting the general solution (2) into (4) and (7)
respectively, we have (9) and (10).
 
Jo Je x e + Jo I Je Je zo = x o (9)
 
Jr Je x e + Jr I Je Je zr = x r (10)
2015 IFAC SYSID
492
October 19-21, 2015. Beijing, China Xinyu WANG et al. / IFAC-PapersOnLine 48-28 (2015) 490495

In order to obtain the homogeneous solution, zo and zr

are solved from (9) and (10), as shown in (11) and (12),
similar as mentioned by Maciejewski and Klein (1985). 0.5
    
zo = Jo I Je Je x o Jo Je x e
0.4
(11) 0.3
x o
     0.2
zr = Jr I Je Je x r Jr Je x e (12) 0.1
pcr x e
Thus, the homogeneous solution for the cases of with or 0

z
without the obstacle can be obtained by substituting (11) 0.1

and (12) into (2), as (13) and (14). 0.2

0.3

o = Je x e + 0.4

     (13) 0.5

(I Je Je ) Jo I Je Je x o Jo Je x e 0
0.2

r = Je x e +
0.4 1
0.8
0.6
     (14) 0.8 0.4
0.6

(I Je Je ) Jr I Je Je x r Jr Je x e 1 0
0.2

x y

However, when the distance between the obstacle and

the manipulator is around the threshold for avoidance, Fig. 1. The manipulator move away from the obstacle.
the solutions will be switched frequently. This will cause 4.1 Point Cloud Segmentation
undesirable vibration of the manipulators motion. To
avoid abrupt and frequent switch between these two cases, The Kinect R provide two images at 30fps at the same
the weighted sum of (13) and (14) is designed as the time, i.e., the RGB image and the depth image. By
solution for both of the cases, as (15). combining the two images, the 3-D point cloud with RGB
  color can be obtained, as shown in Fig. 2. In the Kinect
= J x e + I J Je [zo + (1 )zr ] ,
e e (15) R
coordinate, the right side is the X  axis; the up side is the
where [0, 1] is the weighting factor and is obtained Y  axis; and the depth side is the Z  axis. Denote the set
based on fuzzy inference engine in Section. 5. of points in the point cloud as P = {pi |i = 1, 2, ..., n}.
The control strategy can be obtained by the close-loop The points on the obstacle, Po P , can be separated
control law (5), as (16) from P by the color of the points. In order to exclude the
 
= J (x d + Ke) + I J Je [zo + (1 )zr ] . (16)
e e
impact of illumination, the segmentation process is done
in the HSV color space. The color of points are converted
To proof that the closed-loop system is asymptotic stable, into HSV space using equation (23).
 
the Lyapunov direct method is utilized. Choose a Lya- GB
punov function as
60
mod 6 , Cmax = R



 
1 BR
V (e) = eT Ke. (17) H = 60 + 2 , Cmax = G
2
The function satisfies that
 

R G


(23)
V (e) > 0 e = 0, V (0) = 0. (18) 60 + 4 , Cmax = B

Differentiating (17) we have
0, Cmax = 0
V = eT K x d eT K x e . (19) S=
, Cmax = 0
Substituting (3) into it, we have Cmax
V = Cmax ,
V = eT K x d eT KJe . (20)
where R, G, B [0, 1] are the RGB channels of a color;
With the control law given by (16),
Cmax = max(R, G, B); = max(R, G, B) min(R, G, B);
V = eT K x d H, S, V are the HSV channels of the color.
   .
eT KJe Je (x d + Ke) + I Je Je [zo + (1 )zr ] The points on the obstacle is then separated by (24). The
(21) segmentation result is shown in Fig. 3.
It can be simplified into Po = {pi |50 < hi < 75, si > 90, vi > 50, pi P } (24)
V = eT KJe J Ke 0
e (22)
4.2 Line Detection in 3-D Space
So the closed-loop system is asymptotic stable.
A 2-D Hough Space is used in the line detection. In the r
4. OBSTACLE DETECTION space, the 2-D line can be detected with high robustness.
However, a line in 3-D space cannot be described using
only r and . The following steps are designed to achieve
The Kinect R sensor is used to achieve the obstacle detec-
the line detection.
tion. A paper stick covered with green paper is used as the
obstacle, in order to separate the stick from the complex Firstly, the projects of the points in Po planes are calcu-
background. lated on X  Y  , Y  Z  and X  Z  . Denote the set of

492
 2015 IFAC SYSID
October 19-21, 2015. Beijing, China Xinyu WANG et al. / IFAC-PapersOnLine 48-28 (2015) 490495 493

x cos XY sin XY rXY

2 sin XY
(27)
z  sin Y Z cos Y Z rY Z y 1
= =
2 cos Y Z 2

4.5

4
L
XY
3.5

Fig. 2. The 3-D point cloud with RGB color. 3 L

YZ L
2.5

Y
2

1.5

0.5 0
LXZ 1
0 2
0 3
1 Z
2 4
3
4 5
X 5

Fig. 4. The projects of line L on X  Y  , Y  Z  , X  Z 

planes.

Fig. 3. The points on the obstacle. 4.3 The Endpoints of the Line

projected points on X  Y  , Y  Z  and X  Z  as PXY , The endpoints of L can be detected by searching the
PY Z and PY Z respectively. nearby points pi Po along L. Indicate the set of points
that uniform and orderly distributed on the detectable
Secondly, the projected points are transformed into r
part of L as QL = {qi |i = 1, 2, ..., m}. The set of points
spaces separately by (25).
on obstacle Qo can be determined by (28).
rXY = xi cos XY + yi sin XY Qo = {qi |N (qi ) > NT H , qi QL } , (28)
rY Z = yj cos Y Z + zj sin Y Z (25) where N (qi ) is the number of points pi Po satisfy
rXZ = xk cos XZ + zk sin XZ , |pi qi | < dT H ; dT H and NT H are the thresholds which
where can be turned in the experiment.
pi (xi , yi ) PXY The qi Qo with maximum and minimum i are the
endpoints of the line. Denote the endpoints of L as
pj (xj , yj ) PY Z ql1 (xl1 , yl1
 
, zl1 ) and ql2 (xl2 , yl2
 
, zl2 ). The obstacle detec-
tion result is shown in Fig. 5, where the red segment is the
pk (xk , yk ) PXZ
detected stick.
The projected lines LXY (rXY , XY ), LY Z (rY Z , Y Z ),
LXZ (rXZ , XZ ) on X  Y  , Y  Z  , X  Z  planes are 5. OBSTACLE AVOIDANCE STRATEGY
detected in the three separate r space, as shown in
Fig. 4. 5.1 Coordinate Transformation
Finally, two of the three projected lines are chosen to
The coordinate transformation is used to map the detected
calculate the original 3-D line L. In order to prevent
obstacle into the robot coordinate system, in order to
that one of the chosen projected lines is too short, the
achieve obstacle avoidance. Assuming that we have three
chosen projected lines are LXY and LXZ (XY < 4 or
non-collinear points s1 , s2 and s3 , the coordinates of the
XY > 4 ) or LY Z ( 4 XY 4 ).
points on the robot coordinate X Y Z and the Kinect R
When XY < 4 or XY > 4 , the equation of L is (26). coordinate X  Y  Z  are (x1 , y1 , z1 ), (x2 , y2 , z2 ),
When 4 XY 4 , the equation of L is (27). (x3 , y3 , z3 ), and (x1 , y1 , z1 ), (x2 , y2 , z2 ), (x3 , y3 , z3 ) respec-
tively. The rotation matrix T from the Kinect R coordi-
y  sin XY cos XY rXY nate to the robot coordinate can be calculated by (29).
2 cos XY
(26)      1
z  sin XZ cos XZ rXZ x 1 x1 x2 x3 x1 x2 x3
= = T = y1 y2 y3 y1 y2 y3 (29)
2 cos XZ 2
z1 z2 z3 z1 z2 z3

493
2015 IFAC SYSID
494
October 19-21, 2015. Beijing, China Xinyu WANG et al. / IFAC-PapersOnLine 48-28 (2015) 490495

5.3 Obstacle Avoidance Velocity Strategy

The obstacle avoidance velocity strategy is then designed

ql2 to enable the manipulator avoid the obstacle smoothly
and quickly. The collision point on the manipulator pcr
should move along the direction away from the obstacle,
and the absolute value of the velocity should be inversely
proportional to the distance of the obstacle. The velocity
ql1
detection is obtained by
pcr pco
vnor = . (33)
pcr pco 
Thus, the obstacle avoidance velocity x o is designed as
ql2 ql2

vmax vnor , d dc

dc
x o = vmax vnor , dc < d < do , (34)
ql1

d

ql1

0 , d do
where d = pcr pco  is the distance between the
Fig. 5. The obstacle detection result. obstacle and the manipulator; vmax is the maximum
obstacle avoidance velocity; do is the distance threshold
The coordinate transformation of the detected endpoints
that the manipulator starts to avoid the obstacle and dc
of the obstacle from the Kinect
R coordinate to the robot
is the minimum acceptable distance and the manipulator
coordinate system can be obtained by (30).
will avoid at the maximum speed.
T T
[xl1 yl1 zl1 ] = T [xl1 yl1
 
zl1 ] +B
(30) The weight is used to switch between the obstacle
T  T
[xl2 yl2 zl2 ] = T [xl2 yl2

zl2 ] +B avoidance mode and the restoring mode, which is obtained
where B is the position coordinate of the KinectR in the by a fuzzy inference engine. The most important factors
robot coordinate system. that should be considered when avoiding the obstacle are
the distance between the obstacle and the manipulator
and the speed at which the obstacle is moving toward the
5.2 Collision Points
manipulator. Thus, the distance d and the speed d are
selected as the inputs of the fuzzy inference engine. The
The collision points, pcr and pco , are the two points either
output surface of the fuzzy rule bases is shown in Fig. 6,
on the robot or on the obstacle, which covers the nearest
where dr is the distance threshold that the manipulator
distance between the robot and the obstacle. The forward
start to restore its original pose.
kinematic of the robot is used to estimate the collision
points. Each link of the robot can be seen as a segment in 3-
D space. The coordinates of the endpoints of the segments,
i.e., the coordinates of the joints, in Cartesian space can
be obtained by (31).
n
Xo = 0A1 1A2 n1An Xn , (31) 0.8

0.7

where X = [x, y, z, 1]T is an augmented vector of Cartesian 0.6

coordinate; and j1Aj is the link homogeneous transform 0.5

matrix, which can be given by (32). 0.4

dr
0.3
j1
Aj = 0.2

1.5
cos j sin j cos j sin j sin j aj cos j 1
0.5
0
0.5
d

sin j cos j cos j cos j sin j aj sin j . (32) d

0 sin j cos j dj
0 0 0 1
The skeleton of the manipulator is used in the estimation
of the collision points to simplify the algorithm. Firstly, the
distance between segment ql1 ql2 and the ith segment
of the robot links [xi , yi , zi ] [xi+1 , yi+1 , zi+1 ] can be
calculated by 3-D geometry, which is denoted by di ; the
nearest points on the robot link and the obstacle are
denoted by pcri and pcoi , respectively. Then, the collision
point pcr and pco and the distance d can be obtained by
1
1.5
do
Fig. 6. Output surface of fuzzy rule bases.
6. SIMULATION EXAMPLES
The simulation is performed under MATLAB. The kine-
matic model of Baxter(R) robot is used as the manipula-
tor, refer to our previous work in Ju et al. (2014); Wang
et al. (2015). The obstacle is detected using Kinect R and

searching the minimum distance among the links on the
manipulator skeleton. Assume the link with the minimum
distance is the ith
min link, then we have d = dimin , pcr =
pcrimin and pco = pcoimin .
the programmed based on Processing 2 and the detection
result is transmitted into MATLAB via UDP protocol.
The desired end-effector trajectory is given by
xd = [xd yd zd ]T = [0.7 0.3 0.2 sin t]T . (35)

494
 2015 IFAC SYSID
October 19-21, 2015. Beijing, China Xinyu WANG et al. / IFAC-PapersOnLine 48-28 (2015) 490495 495

The distance thresholds and the maximum avoiding speed
are given by (36).
do = 0.5, dr = 0.3
(36)
dc = 0.1, vmax = 3.0
The obstacle avoidance simulation result is shown in
Fig. 7, where the black line and the blue points are the
links and the joints of the manipulator, respectively. The
green segmentation is the detected obstacle. The left and
right sub-figures are the poses with and without obstacle
respectively.
The position error of the middle joint between the parallel
system and the real system is shown in Fig. 8. It can be
seen clearly that the error increases since the obstacle
is moving closer and the manipulator begin to avoid
the obstacle. When the obstacle is moving away, the
manipulator start to restore its original pose and after the
obstacle is removed, the error convergence to zero, which
means that the manipulator has eliminated the effect of
the obstacle.
1 1
0.5
z
By designing an artificial parallel system of the manipu-
lator in the obstacle avoidance controller, the proposed
control method enables the robot not only achieve the
goal of moving away from the obstacle, but also enable
the robot restore back its original pose. The stability of
the closed-loop system is proved using Lyapunov direct
method to ensure the asymptotic stability of the system.
The collision points and the distance between the robot
and the obstacle are calculated to obtain the reasonable
avoiding and restoring velocity. A weighting function is
designed based on fuzzy inference engine to achieve the
smooth switching between the obstacle avoidance mode
and the restoring mode. The satisfactory simulation results
using the Baxter robot model and the obstacle detection
system based on Kinect R verifies the effectiveness of the
proposed obstacle detection algorithm and the obstacle
avoidance method.
REFERENCES
Benzaoui, M., Chekireb, H., and Tadjine, M. (2010). Re-
dundant robot manipulator control with obstacle avoid-
ance using extended jacobian method. In Control Au-
tomation (MED), 2010 18th Mediterranean Conference
on, 371376. doi:10.1109/MED.2010.5547696.
q
l2
Ju, Z., Yang, C., and Ma, H. (2014). Kinematics modeling
pcr
0.5
pco and experimental verification of baxter robot. In Control
Conference (CCC), 2014 33rd Chinese, 85188523.

0 0 ql1

0.5
1

0.5
1
Leutert, F., Freier, C., and Schilling, K. (2012). 3d-sensor
0.5
0.5
0

0.5
0.5
0.5
0

0.5
based dynamic path planning and obstacle avoidance
0
0.5

x
1
1 y
0
0.5

x
1
1 y
for industrial manipulators. In Robotics; Proceedings of
ROBOTIK 2012; 7th German Conference on, 16.
Maciejewski, A.A. and Klein, C.A. (1985). Obstacle
1 1
avoidance for kinematically redundant manipulators in
pcr ql2
q
l2 pcr dynamically varying environments. The international
pco

0.5
pco
0.5 journal of robotics research, 4(3), 109117.
ql1
Natarajan, S., Vogt, A., and Kirchner, F. (2010). Dynamic
z

ql1

0
1
0
1
collision avoidance for an anthropomorphic manipulator
0
0.5

0
0.5
using a 3d tof camera. In Robotics (ISR), 2010 41st
International Symposium on and 2010 6th German Con-
0.5 0.5
0.5 0.5
0.5 0.5
0 0
0.5 1 y 0.5 1 y
1 1

x x
ference on Robotics (ROBOTIK), 17.
Schlemmer, M. and Gruebel, G. (1997). A distance
Fig. 7. The Simulation result. function and its gradient for manipulator online obstacle
detection and avoidance. In Advanced Robotics, 1997.
ICAR 97. Proceedings., 8th International Conference
0.2
x on, 427432. doi:10.1109/ICAR.1997.620217.
0.1
y
z Wang, X., Yang, C., and Ma, H. (2015). Automatic ob-
stacle avoidance using redundancy for shared controlled
0
telerobot manipulator. In Cyber Technology in Automa-
er(m)

0.1 tion, Control, and Intelligent Systems (CYBER), 2015

IEEE International Conference on, 13381343.
0.2 Obstacle moving closer
Obstacle moving away Yoo, Y., Oh, K., Choi, Y., and Won, S. (2013). Obstacle
0.3 avoidance for redundant manipulator without informa-
770 780 790
t
800 810 820 tion of the joint angles. In Control Conference (ASCC),
2013 9th Asian, 16. doi:10.1109/ASCC.2013.6606009.
Fig. 8. The error between the reference system and the Zhang, Y. and Wang, J. (2003). Obstacle avoidance of
real system. redundant manipulators using a dual neural network.
In Robotics and Automation, 2003. Proceedings. ICRA
03. IEEE International Conference on, volume 2, 2747
7. CONCLUSIONS 2752 vol.2. doi:10.1109/ROBOT.2003.1242008.

In this paper, an improved obstacle avoidance method

is developed. A new obstacle detection method using
KinectR is designed to obtain the obstacle information.

495
View publication stats