Content uploaded by Ahmad Ataka Awwalur Rizqi
Author content
All content in this area was uploaded by Ahmad Ataka Awwalur Rizqi on Sep 30, 2016
Content may be subject to copyright.
King’s Research Portal
Document Version
Peer reviewed version
Link to publication record in King's Research Portal
Citation for published version (APA):
Rizqi, A. A. A., Shafti, S. A., Shiva, A., Würdemann, H. A., & Althoefer, K. A. (2016). Real-Time Obstacle
Avoidance for Continuum Manipulator: Towards Safer Application in Human Environments. In A. Shafti, & A.
Orlandini (Eds.), The 26th International Conference on Automated Planning and Scheduling: Proceedings of the
1st Workshop on Planning, Scheduling and Dependability in Safe Human-Robot Interactions. (pp. 17)
Citing this paper
Please note that where the full-text provided on King's Research Portal is the Author Accepted Manuscript or Post-Print version this may
differ from the final Published version. If citing, it is advised that you check and use the publisher's definitive version for pagination,
volume/issue, and date of publication details. And where the final published version is provided on the Research Portal, if citing you are
again advised to check the publisher's website for any subsequent corrections.
General rights
Copyright and moral rights for the publications made accessible in the Research Portal are retained by the authors and/or other copyright
owners and it is a condition of accessing publications that users recognize and abide by the legal requirements associated with these rights.
•Users may download and print one copy of any publication from the Research Portal for the purpose of private study or research.
•You may not further distribute the material or use it for any profit-making activity or commercial gain
•You may freely distribute the URL identifying the publication in the Research Portal
Take down policy
If you believe that this document breaches copyright please contact librarypure@kcl.ac.uk providing details, and we will remove access to
the work immediately and investigate your claim.
Download date: 30. Sep. 2016
Real-Time Obstacle Avoidance for Continuum Manipulator: Towards Safer
Application in Human Environments
Ahmad Ataka, Ali Shafti, Ali Shiva, Helge Wurdemann, and Kaspar Althoefer
Centre for Robotics Research, King’s College London
Strand, London, WC2R 2LS
England, United Kingdom
Abstract
The rigid-link manipulators have been extensively em-
ployed in industrial setting nowadays. However, prob-
lem arises when such manipulators are assigned to work
alongside human or employed in cluttered, complex,
and tight environment. Hence, continuum manipula-
tor, with its inherent compliant and flexibility, is prefer-
able in this aspect. In this paper, we present our re-
cent work presenting a real-time pose estimation and
obstacle avoidance for continuum manipulator in dy-
namic environments. The approach is shown to work
in a model of both single-segment and multi-segment
continuum manipulator and also in a real tendon-driven
single-segment continuum manipulator in dynamic en-
vironment, and thus, suitable to be used in human envi-
ronments.
Introduction
Nowadays, the field of rigid-link manipulators is a well-
established discipline. Its ability to have precise position
control and trajectory generation for doing various manip-
ulations and tasks has made it popular in industrial setting.
The rigid-link manipulators are now used daily in various in-
dustrial environment and manufacturing plants worldwide.
However, the current generation of rigid-link manipulators
are mainly employed automatically in task with limited hu-
man intervention due to the safety reason. This constraints
the applicability of such manipulators to tasks which require
human-robot collaboration.
Several researchers proposed solutions for safer human-
robot interaction, such as the utilization of flexible joint (Oz-
goli and Taghirad 2006) and variable stiffness actuator (Kim
and Song 2010), an elastic strip approach which exploits re-
dundancy of the manipulators (Khatib et al. 1999), (Brock
and Khatib 2002), and others which include safety criteria
to robots motion planning and obstacle avoidance (Haddadin
et al. 2013). These, however, do not eliminate the fact that
most industrial manipulators still need an open space and
well-defined environment in order to execute the task.
One alternative solution is by employing a continuum-
style manipulators, mainly used in medical applications
(Burgner-Kahrs, Rucker, and Choset 2015). Mostly inspired
Copyright c
2015, Association for the Advancement of Artificial
Intelligence (www.aaai.org). All rights reserved.
Figure 1: A tendon-driven single-segment arms used as a
model in this paper. To measure the tip’s pose and the obsta-
cle’s pose, the NDI Aurora tracker can be adopted.
by nature, such as octopus arm (McMahan et al. 2006) or
snake (Hirose 1993), continuum manipulator has an ability
to bend at any point along its body. Thus, it is very suit-
able to be used in complex and tight environments where
the rigid-link counterparts would be unable to manoeuvre.
Its inherent compliance will also make it safer and more
adaptable when interact with sensitive environment, includ-
ing human. Continuum manipulators, for example mounted
on mobile platforms, will have more flexibility than their
rigid-link counterparts when employed in industrial setting
with human presence.
However, the advantage of backbone flexibility possessed
by continuum manipulators comes with the consequence of
difficulty in modelling their structure. This will in turn com-
plicates the motion planning and trajectory generation. The
model-based pose estimator is needed to correctly estimate
the pose of manipulators body such that the real-time obsta-
cle avoidance can be employed to safely avoid collision with
human or dynamic environments.
In this paper, we present a real-time obstacle avoidance
for continuum manipulators in dynamic environments. The
obstacle avoidance is equipped with non-linear observer to
estimate the pose of any point along the body of manipu-
Figure 2: An illustration of three segments continuum ma-
nipulator with mobile base.
lator to make sure that the whole body of the manipulator
can avoid collision. The algorithm is tested for a model
of tendon-driven single-segment continuum manipulator as
shown in Figure 1 and has been verified experimentally as
presented in (Ataka et al. 2016). The obstacle avoidance is
also implemented to a model of multi-segment continuum
manipulators with mobile platform (Ataka et al. 2016). The
overall algorithm is shown to work well in avoiding moving
obstacle, and thus, makes it suitable to be used in human
environments.
Continuum Manipulator
Kinematic Model
Here, we present a model of a tendon-driven continuum
manipulator. The manipulator is moved by modifying the
length of three uniformly distributed tendons along the sur-
face in each segment The model is based on constant-
curvature assumption, i.e. each segment is assumed to have
a constant radius of curvature at a given time. Each seg-
ment can then be parameterized by configuration space vari-
ables kiconsisting of curvature κi, deflection angle φi, and
segment length si. The forward kinematics relation map-
ping these variables to task space position of the segments
tip is presented in (Webster and Jones 2010). For a contin-
uum manipulator with mobile platform, as shown in Figure
2, the homogeneous transformation of the end-effector with
respect to the world frame is expressed as
N
0T(k) = TB
N
∏
i=1
i
i−1T(ki)(1)
Where Nspecifies the segment number, k=
[k1k2... kN]Tdenotes the vector consisting of con-
figuration space variables of all segments, and TB∈SE(3)
denotes the homogeneous transformation matrix of the
frame attached to the base.
Moreover, the mapping from the configuration space vari-
ables kto the actuator space q, in this case specifies tendons
length, is also well defined in (Webster and Jones 2010).
Adding the mobile platform pose q0to the actuator space
q, we have q= [q0q1... qN]Twhere each component
qiconsists of the tendon length of each segment, written as
qi= [li1li2li3]T.
In order to specify any point along the body of the manip-
ulator, we use a scalar ξi∈[0,1]for each segment, ranging
from the base (ξi=0) to the tip (ξi=1). The list of scalars
ξifor all segments are then combined to be a vector ξwhose
value is govern by ξ={ξr=1 : ∀r<i,ξi,ξr=0 : ∀r>i}.
In short, we can write the forward kinematics of the con-
tinuum manipulators as
N
0T(q,ξ) = R(q,ξ)p(q,ξ)
01×31(2)
where R(q,ξ)∈SO(3)stands for the rotation matrix and
p(q,ξ)∈R3stands for the position vector of the point
along the body of manipulator. The Jacobian for our kine-
matic model, defined as J(q,ξ) = ∂p(q,ξ)
∂q∈R3×(3N+6), is
expressed as follows
˙
p(q,ξ) = J(q,ξ)˙
q⇔˙
q=J(q,ξ)−1˙
p(q,ξ).(3)
State-Space Representation
The kinematic model can also be expressed in terms of state
space representation, i.e. the state equation and output equa-
tion. Here, we present the state space analysis for a static
single segment only. We use the tendon length q∈R3as
our state, x∈X. Hence, the input to our system, u∈U, is
given by the actuator lengths rate of change, ˙
q∈R3, which
is governed by DC motors connected to the tendon. The
measurement value yk∈Ycomes from a position sensor em-
bedded in the tip of manipulator. The NDI Aurora tracker,
like the one shown in Figure 1, can be used for this pur-
pose. Hence, the output equation matches the component
of the matrix given by the forward kinematics relation in 2,
p(q,ξ)for ξ=1 (tip). Here, X,U, and Ydenote the state
space, input space, and output space respectively.
The state space and output equation in discrete form, for
a sampling time of ∆t, can then be expressed as
xk+1=f(xk,uk) = xk+∆tuk.(4)
y=g(xk) = p(q,ξ=1),(5)
where the function f:X×U→Xis used to map the current
state and input to the next state while g:X→Yis used to
map the current state to the output.
However, using only this information is not enough to es-
timate the pose of the whole body of the manipulator, due to
the unknown initial state value. This is where the non-linear
observer is needed to estimate the state and, in turn, use the
estimation state to estimate the pose of any point along the
body of manipulators.
Pose Estimation and Obstacle Avoidance
Pose Estimation
Based on the presented state space model, we employ a
well-known Extended Kalman Filter (EKF) approach to our
Figure 3: The proposed pose estimator and obstacle avoidance algorithm. An ideal kinematics model, added with Gaussian
noise, is used to replace the continuum manipulator and the pose sensor during the simulation.
model. The EKF can be formulated as
ˆ
xk+1|k=f(ˆ
xk|k,uk),
Pk+1|k=AkPk|kAT
k+Qk,
Kk=Pk+1|kCT
k(CkPk+1|kCT
k+Rk)−1,
ˆ
xk+1|k+1=ˆ
xk+1|k+Kk(yk−g(ˆ
xk+1|k)),
Pk+1|k+1= (I−KkCk)Pk+1|k.
(6)
ˆ
xk+1|k+1,ˆ
xk|k,uk, and ykrepresent the next state estima-
tion, the current state estimation, the input signal, and the
measurement data respectively. The matrix Qk∈R3×3and
Rk∈R3×3are the process noise variance and measurement
noise variance respectively.
The matrix Akand Ckare defined as A=∂f(xk,uk)
∂xk,B=
∂f(xk,uk)
∂ukand C=∂g(xk)
∂xkrespectively and can be written as
Ak=∂f(xk,uk)
∂xk
=I∈R3×3,(7)
Ck=∂g(xk)
∂xk
=∂p(qk,ξ=1)
∂qk
=J(qk,ξ=1).(8)
The estimation state can then be used to estimate the pose
of any point along the body of manipulator using a forward
kinematics relation in (2) by modifying the scalar ξfrom 0
(base) to 1 (tip). This information is used in the obstacle
avoidance stage.
Modified Potential Field
The reactive potential field presented in (Khatib 1985) is
modified here. The idea is to use the potential function Uto
attract the tip of manipulator to a desired target position and
repel its body from colliding with environment. A standard
potential field, which usually produces a task space force
F=−∇U, is modified such that it is suitable to be used
in continuum manipulators kinematic model. Rather than
force, the task space velocity ˙
pis produced.
The attractive potential field is given by
˙
ppd=−c(p−pd).(9)
where pdand crepresent desired position and a positive con-
stant gain respectively. The repulsive field is expressed as
˙
pO=(η(1
ρ−1
ρ0)1
ρ2
∂ ρ
∂pif ρ<ρ0
0 if ρ≥ρ0
.(10)
where ρ=p(p−pO)T(p−pO)denote the closest distance
from an obstacle to the manipulator’s body, ηis positive
constant, and ρ0indicates the limit distance of the potential
influence.
In order to achieve safer obstacle avoidance, the repulsive
potential needs to be applied not only at the tip but also at
points along the body of manipulator. Therefore, we define
point subjected to potential (PSP) as a point in the backbone
of manipulator in which the repulsive potential is possible to
be applied. The PSP to be chosen is the closest one to the
obstacles or environments. The position of this PSP as well
as the tip are all estimated by the pose estimation stage at
every iteration as follows
ˆ
pk(ξ) = p(ˆ
xk|k,ξ).(11)
Finally, each attractive and repulsive velocity in task
space, using their corresponding working points, are
mapped to the actuator space via inverse Jacobian relation.
The combined velocity in actuator space is then fed as an
input to our manipulator, uk, as follows
uk=˙
q=J−1
e˙
ppd+J−1
a˙
pO,(12)
where Jeand Jaindicate the Jacobian of the tip and the cho-
sen PSP respectively and ˙
pObrepresents repulsive potential
produced by the closest obstacle.
The overall pose estimation and obstacle avoidance algo-
rithm is combined as shown in Figure 3.
Mechanical Constraint Avoidance
Other problem that needs to be addressed for this kind of
manipulator is the inherent mechanical limitation which can
disturb the movement of the manipulator in avoiding obsta-
cle. In a tendon-driven manipulator, the tendons length li j
can only be in the region of (1−ζ)L<li j <(1+ζ)Lwhere
−0.05 00.05
0
0.5
0
0.5
1
1.5
2
2.5
Length (m)
Distance (m)
Potential Function
Figure 4: Illustration of the proposed potential function de-
signed to satisfy mechanical constraint as function of the
tendon’s length and end-effector-to-target Euclidean dis-
tance.
ζand Lrepresent extension ratio and normal length respec-
tively.
We propose an attractive potential in actuator space to at-
tract the length towards normal length L, and thus, makes the
tendons avoid mechanical constraint. The potential function
is described as
Ulim(q) =
N
∑
i=1
3
∑
j=1
σ(li j −L
ζL)2,(13)
where σis positive constant. The attractive velocity field is
as follows
˙
qlim (q) = −2σ1
ζ2L2(l−L),(14)
where l= [l11 ... l13 l21 ... lN3]Tand L=L13N×1.
Moreover, we weight this potential field by a weight func-
tion wwhich will reduce the contribution of the mechanical
constraint field as the end effector approaches the target po-
sition, as follows
w(x) = (1−e−µk(x−xd)k),(15)
where µis positive constant.
Figure 4 shows an illustration of this potential as a func-
tion of tendons length as well as the distance between end-
effector and the target. The final proposed mechanical con-
straint avoidance in actuator space is given by
˙
qnew(q,x) = 06×1
w(x)˙
qlim (q)(16)
so that the total potential is given by
˙
q(q) = Je(q)+˙
xxd(x) +
m
∑
a=1
n
∑
b=1
Ja(q)+˙
xOb(x) + ˙
qnew(q,x)
(17)
Results and Discussion
Here, we present the implementation of our approach both
in the simulation and the experiment. The combined pose
estimation and obstacle avoidance for single segment case
is tested on a model of continuum manipulator running in a
real-time simulation environment. The obstacle avoidance
Figure 5: A single segments continuum manipulator’s move-
ment with a static target position (small red dot) when ob-
stacle (black sphere) moves close to the tip. The order of
movement is as follows: upper left picture, upper right pic-
ture, lower left picture, and finally lower right picture.
algorithm without the pose estimation is also validated in
an experiment as presented in our publication (Ataka et al.
2016). Moreover, we also applied the obstacle avoidance
and mechanical constraint avoidance to a model of multi-
segment continuum manipulator as presented in the same
publication. A moving obstacle, assumed to be a 5-mm-
radius sphere, moves at a constant speed in the surround-
ing of the manipulator to simulate the part of human body’s
movement.
Single Segment Case
In this subsection, we will first present the simulation re-
sults of a combined pose estimation and obstacle avoidance.
For the pose estimation, to simulate the sensor, an ideal
kinematic model with added Gaussian noise is used. This
perfect kinematic model has a true state xkupdated at ev-
ery iteration based on the model kinematics. However, the
value is assumed to be unaccessible for the obstacle avoid-
ance, which will only capitalize the estimated states ˆ
xkfrom
the EKF. The noise has zero-mean and the standard devi-
ation of σ=10−4. The variance matrix is then given by
R=σ2I∈R3×3. The black sphere represents the obstacle
while the rod dot represents the tip’s target, assumed to be
(a) (b)
Figure 6: A single segments continuum manipulator’s movement with a static target position (small red dot) when obstacle
(black sphere) moves close to the middle point of the manipulator’s body (a) in x-axis direction and (b) y-axis direction. The
order of movement for each subfigure is as follows: upper left picture, upper right picture, lower left picture, and finally lower
right picture.
fixed.
We show several scenarios, such as an obstacle moves
close to the segment’s tip, as depicted in Figure 5 and moves
close to the middle part of the backbone, as shown in Figure
6a (for the obstacle’s movement in x-direction) and Figure
6b (for the obstacle’s movement in y-direction). We can con-
clude that the proposed algorithm works well to improve the
safety of the robot’s body from collision. The main con-
tribution of the algorithm, however, is demonstrated when
the obstacle move at a lower height, such as in Figure 6.
Our proposed algorithm enables not only the tip but also the
body of manipulator to avoid obstacle.
The obstacle avoidance algorithm, without the pose es-
timation, has been implemented in an ortho-planar spring
tendon-driven continuum manipulator as shown in Figure 1
as presented in (Ataka et al. 2016). The Maxon DC Mo-
tors are used as actuators. An electromagnetic-based Aurora
sensor coil is embedded in the tip of the manipulator to track
the position of the tip. The obstacle is represented by a sec-
ond sensor coil hung in the air by a thread. The obstacle is
assumed to be a sphere with radius of 0.01 m whose centre
is specified by the second sensor coil’s location. The target
is assumed to move in a straight line.
We can see from Figure 7, from left to right, both the
3D view (Figure 7a) and the top view (Figure 7b) of the
tip’s movement, as well as the comparison between the tip-
obstacle distance and the target-obstacle distance (Figure
7c). Without pose estimation, the PSP is assumed to be lo-
cated only at the tip, hence, only the tip will be safe from
collision.
Three Segments Case
Here, we present the performance of our proposed mechan-
ical constraint avoidance in a model of three-segments con-
tinuum manipulator. We can see how our proposed algo-
rithm improves the performance of continuum manipulator
in tracking the moving target. Without our algorithm, once
the tendons length reach their limit, there appears an im-
mediate reduction in the manipulator’s maneuverability such
that the tip is unable to reach the moving target, as shown in
Figure 8a. While, using our algorithm, it is shown in Figure
8b that the manipulator’s tip is able to track the moving tar-
0.05
0.1 −0.06−0.04−0.0200.02
0.08
0.1
0.12
y (m)
x (m)
z (m)
(a) (b)
0 20 40 60 80
0.025
0.03
0.035
Time (s)
Distance (m)
(c)
Figure 7: The manipulator’s tip (red line) tracks the desired trajectory (blue line) and avoids static obstacle (black spheres)
nearby in (a) 3D view and (b) top view. (c) The graph shows the closest distance from the obstacle to the estimated manipulator’s
body (red line) and to the target (blue line) respectively.
(a)
(b)
Figure 8: The movement comparison between the continuum manipulator (a) without the proposed algorithm and (b) with the
proposed algorithm.
get smoothly. The complete obstacle avoidance simulation
results can be seen in (Ataka et al. 2016).
Conclusions and Future Works
In this paper, we present our works on real-time obstacle
avoidance, based on modified potential field, for continuum
manipulators in dynamic environments. The obstacle avoid-
ance can be equipped with a pose estimator, based on an Ex-
tended Kalman Filter, to make the whole body of manipula-
tor safer from collision. The novel potential field in actuator
space is also proposed in order to avoid the inherent mechan-
ical constraint of the manipulators. The combined obstacle
avoidance and pose estimator is shown to perform well in
a simulation for the model of tendon-driven single-segment
continuum manipulator. The proposed potential field is also
verified in experiment. The extension of the obstacle avoid-
ance for multi-segment case as well as the mechanical con-
straint avoidance is implemented successfully in a model of
three-segments continuum manipulator.
The proposed algorithm has a promising capability to be
implemented in a real environment consisting human. For
the future works, the whole algorithm can be fully imple-
mented for the real multi-segment continuum manipulators
equipped with electromagnetic-based position sensors. The
experiment to test the whole algorithm in a continuum ma-
nipulator with mobile platform will also be investigated in
the future.
Acknowledgement
The work described in this paper is partially supported by
the STIFF-FLOP project grant from the European Commu-
nities Seventh Framework Programme under grant agree-
ment 287728, the Four By Three grant from the European
Framework Programme for Research and Innovation Hori-
zon 2020 under grant agreement no 637095, and the Indone-
sia Endowment Fund for Education, Ministry of Finance Re-
public of Indonesia.
References
Ataka, A.; Qi, P.; Liu, H.; and Althoefer, K. 2016. Ac-
cepted. Real-Time Planner for Multi-segment continuum
manipulator in dynamic environments. Proceedings of 2016
IEEE International Conference on Robotics and Automation
(ICRA).
Brock, O., and Khatib, O. 2002. Elastic strips: A framework
for motion generation in human environments. The Interna-
tional Journal of Robotics Research 21(12):1031–1052.
Burgner-Kahrs, J.; Rucker, D. C.; and Choset, H. 2015.
Continuum Robots for Medical Applications: A Survey.
IEEE Transactions on Robotics 31(6):1261–1280.
Haddadin, S.; Parusel, S.; Belder, R.; and Albu-Schffer, A.
2013. Computer Safety, Reliability, and Security: 32nd
International Conference, SAFECOMP 2013, Toulouse,
France, September 24-27, 2013. Proceedings. Berlin, Hei-
delberg: Springer Berlin Heidelberg. 202–215.
Hirose, S. 1993. Biologically inspired robots: snake-like
locomotors and manipulators. Oxford science publications.
Oxford University Press.
Khatib, O.; Yokoi, K.; Brock, O.; Chang, K.; and Casal,
A. 1999. Robots in human environments. In Robot Motion
and Control, 1999. RoMoCo ’99. Proceedings of the First
Workshop on, 213–221.
Khatib, O. 1985. Real-time obstacle avoidance for ma-
nipulators and mobile robots. In Robotics and Automation.
Proceedings. 1985 IEEE International Conference on, vol-
ume 2, 500–505.
Kim, B. S., and Song, J. B. 2010. Hybrid dual actuator
unit: A design of a variable stiffness actuator based on an
adjustable moment arm mechanism. In Robotics and Au-
tomation (ICRA), 2010 IEEE International Conference on,
1655–1660.
McMahan, W.; Chitrakaran, V.; Csencsits, M.; Dawson, D.;
Walker, I.; Jones, B.; Pritts, M.; Dienno, D.; Grissom, M.;
and Rahn, C. 2006. Field trials and testing of the OctArm
continuum manipulator. In Robotics and Automation, 2006.
ICRA 2006. Proceedings 2006 IEEE International Confer-
ence on, 2336–2341.
Ozgoli, S., and Taghirad, H. 2006. A survey on the control
of flexible joint robots. Asian Journal of Control 8(4):332–
344.
Webster, III, R. J., and Jones, B. A. 2010. Design and Kine-
matic Modeling of Constant Curvature Continuum Robots:
A Review. Int. J. Rob. Res. 29(13):1661–1683.
