Real-Time Pose Estimation and Obstacle Avoidance for Multi-segment Continuum Manipulator in Dynamic Environments

Abstract

In this paper, we present a novel pose estimation and obstacle avoidance approach for tendon-driven multi-segment continuum manipulators moving in dynamic environments. A novel multi-stage implementation of an Extended Kalman Filter is used to estimate the pose of every point along the manipulator's body using only the position information of each segment tip. Combined with a potential field, the overall algorithm will guide the manipulator tip to a desired target location and, at the same time, keep the manipulator body safe from collisions with obstacles. The results show that the approach works well in a real-time simulation environment that contains moving obstacles in the vicinity of the manipulator.

Full text

King’s Research Portal
Document Version
Peer reviewed version
Link to publication record in King's Research Portal
Citation for published version (APA):
Ataka, A., Qi, P., Shiva, A., Shafti, A., Würdemann, H., Liu, H., & Althoefer, K. (2016). Real-Time Pose
Estimation and Obstacle Avoidance for Multi-Segment Continuum Manipulator in Dynamic Environments. In
2016 IEEE/RSJ International Conference on Intelligent Robots and Systems. (pp. 2827-2832)
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: 28. Oct. 2016

Real-Time Pose Estimation and Obstacle Avoidance for Multi-segment
Continuum Manipulator in Dynamic Environments*
Ahmad Ataka1, Peng Qi2, Ali Shiva1, Ali Shafti1, Helge Wurdemann3, Hongbin Liu1, and Kaspar Althoefer4
Abstract— In this paper, we present a novel pose estimation
and obstacle avoidance approach for tendon-driven multi-
segment continuum manipulators moving in dynamic environ-
ments. A novel multi-stage implementation of an Extended
Kalman Filter is used to estimate the pose of every point along
the manipulator’s body using only the position information of
each segment tip. Combined with a potential field, the overall
algorithm will guide the manipulator tip to a desired target
location and, at the same time, keep the manipulator body
safe from collisions with obstacles. The results show that the
approach works well in a real-time simulation environment that
contains moving obstacles in the vicinity of the manipulator.
I. INTRODUCTION
The field of continuum robots has become very popular
in recent years. Various continuum robot structures have
been reported on in the last decade, including tendon-
driven actuation [1], pneumatically actuated designs [2]
and stiffness-controllable manipulators [3]. Kinematic and
dynamic modelling, as well as the control of continuum
manipulator have also been explored as reported in [4].
Very much inspired by nature, for example using the
octopus arm [5] as role models, continuum manipulators
possess higher flexibility and maneuverability in compari-
son with standard rigid-link manipulators. Exploiting those
features, continuum arms can now be applied in areas in
which they were previously prohibited from, such as surgery.
However, this beneficial characteristic leads to a complexity
in estimating their pose and modelling their highly flexible
structure accurately. This in turn leads to new challenges
when designing navigation strategies. Combining a model-
based pose estimator and an obstacle avoidance algorithm
*Research is partially supported by the STIFF-FLOP project grant from
the European Commission Seventh Framework Programme under grant
agreement 287728, the Four By Three grant from the European Frame-
work Programme for Research and Innovation Horizon 2020 under grant
agreement no 637095, and the Indonesia Endowment Fund for Education,
Ministry of Finance Republic of Indonesia.
1A. Ataka, A. Shiva, A. Shafti, and H. Liu are with The Centre
for Robotics Research (CoRe), Department of Informatics, Kings College
London, London WC2R 2LS, United Kingdom. Corresponding author e-
mail: ahmad.ataka@kcl.ac.uk
2P. Qi is with the Department of Biomedical Engineering, Faculty of
Engineering, National University of Singapore, Singapore 117583. (e-mail:
peng.qi@nus.edu.sg)
3H. Wurdemann is with the Department of Mechanical Engineering,
University College London, Torrington Place, London WC1E 7JE, United
Kingdom. (e-mail: h.wurdemann@ucl.ac.uk)
4K. Althoefer is with the School of Engineering and Material Science,
Queen Mary University of London, Mile End Road, London E1 4NS, United
Kingdom. (e-mail: k.althoefer@qmul.ac.uk)
Corresponding author e-mail: ahmad.ataka@kcl.ac.uk
(a) (b)
Fig. 1. (a) A tendon-driven single-segment arms with NDI Aurora electro-
magnetic based tracker for tip pose measurement. (b) Illustration of the
proposed multi-stages pose estimation for three-segments manipulator. The
solid line specifies active segments.
for continuum manipulators is a suitable approach to achieve
collision-free motion, especially in dynamic environments.
Pose estimation strategies have been implemented for
various types of continuum manipulators, such as snake-
like robots [6], [7], concentric tube [8], steerable catheter
[9], as well as multi-section tendon-driven manipulators [10].
Most of the previous works make use of the Kalman-Filter
[6], [8]. Several researchers rely on visual sensing [8], [9]
which can be unreliable for dynamic environments. On the
other hand, inertial sensors used, for example, by [7] suffer
from an accumulated integration error. An interesting method
as presented in [6] uses electro-magnetic trackers providing
position and orientation information of a point along the body
of manipulators. However, none of the works tries to use
estimation results to improve obstacle avoidance.
In the obstacle avoidance side, real-time adaptive mo-
tion planning was implemented for multi-segment contin-
uum manipulator in dynamic environments [11]. However,
it assumes a planar workspace, limiting its applicability.
Another study suggested an inverse-Jacobian-based planning
which provides solutions in actuator space [12]. However,
it is specifically designed for a manipulator moving inside
a tubular environment. A classical potential field, like the
one employed in [13], albeit suffers from local minima in
complex environments, is reactive and thus suitable to be
used for real-time applications in dynamic environments.
In this paper, we propose an approach based on real-time
pose estimation and obstacle avoidance for tendon-driven
multi-segment continuum manipulators, as an extension to
the one shown in Figure 1a [1], moving in dynamic environ-
ments. A novel multi-stage implementation of a non-linear
observer based on an Extended Kalman Filter is proposed
to estimate the pose of every point along the manipulator’s
body with the tip’s position serves as the only information

provided besides the kinematic model. To the best of our
knowledge, this is the first work combining pose estima-
tion and obstacle avoidance for collision-free navigation of
tendon-driven multi-segment continuum manipulators, as an
extension to our previous work [14]. The overall strategy will
guide the manipulator’s tip to a desired target location and
keep the entire manipulator body avoid obstacles.
II. CONTINUUM MANIPULATOR MODELLING
A. Forward Kinematics and Jacobian Derivation
In this paper, each segment of the manipulator is assumed
to be a circle with a constant radius of curvature. Therefore,
each segment can be parameterized by configuration space
variables ki=κiφisiT, which stand for the curva-
ture (κi), rotational deflection angle (φi), and arc length
(si) of segment-i. The homogeneous transformation matrix
describing the tip pose of segment-iwith respect to its base,
i
i−1T(ki)∈SE(3), depends on kias presented in [4]. The
pose of the end effector with respect to the world frame for
Nsegments manipulator is described as follows
N
0T(k) =
N
∏
i=1
i
i−1T(ki)(1)
where k=k1k2... kNT. The base of the bottom
segment is fixed.
For tendon-driven actuation, the actuator space variables
of segment-iare written as qi=li1li2li3Twhere the
length of tendon- jin segment-iis given by L+li j for
a segment with normal length L. The complete actuator
space variables for Nsegments manipulator can then be
expressed as q=q1q2... qNT. The configuration
space variables kcan be expressed as function of the tendon
length L+li j and cross-section radius das presented in [4].
A point along the segment of manipulator is specified
by a scalar ξi∈[0,1]from the base (ξi=0) to the tip
(ξi=1) of segment-i. The set of scalars of all segments
are then constructed into a scalar coefficient vector [15],
ξ=ξ1ξ2... ξNT. The value of the vector is defined
as ξ={ξr=1 : ∀r<i,ξi,ξv=0 : ∀v>i}.
So, the complete forward kinematic is given by
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. A Jacobian, defined as J(q,ξ) =
∂p(q,ξ)
∂q∈R3×3N, is used to map velocity in task space to
configuration space as follows
˙
p(q,ξ) = J(q,ξ)˙
q⇔˙
q=J(q,ξ)+˙
p(q,ξ),(3)
where the (+) operation stands for the pseudo-inverse matrix
operation defined as J+=JT(JJT)−1.
B. State-Space Representation
The kinematic model of the continuum manipulator can
be rewritten in the form of the state space representation.
Assume that we will only analyze n≤Nsegments. For the
sake of simplicity, we call these nsegments active segments.
As we will show in the next section, the number of active
segments can be adjusted over time. Using our model, the
tendon’s actuator space q∈R3ncorresponds to the state
variable xwhile the input uis represented by the DC motor’s
rotational speed, in which the tendon is connected, as an
actuator. Hence, the input value is equivalent to the tendon
length’s rate of change ˙
q∈R3n. The state variables and input
of the system can then be written as
x=q=q1q2... qnT,(4)
u=˙
q=˙
q1˙
q2... ˙
qnT,(5)
where qi=li1li2li3T. The state equation is defined as
xk+1=f(xk,uk) = xk+∆tuk,(6)
where ∆tstands for the time sampling.
A 3-DOF electro-magnetic-based tracker is assumed to
be embedded in the tip of each segment. This sensor will
give the tip’s position of every segment with respect to the
manipulator base at a rate whose value is assumed to be
equal to the sampling frequency 1
∆tso that there is always
updated data available at every sampling step. Hence, the
output value y∈R3nis represented by the tip position of
each active segment,
yk=g(xk) = p(xk,ξ=χ1)... p(xk,ξ=χn)T,(7)
where χi=ξ1=1... ξi=1ξi+1=0... ξn=0T
and p(xk,ξ)is derived using (2).
III. POSE ESTIMATION AND OBSTACLE
AVOIDANCE
A. Multi-Stages Pose Estimation
A non-linear observer, which does not rely on the informa-
tion of the state’s initial value, is needed to estimate the pose
of a point along the manipulator’s body. The well-known
Extended Kalman Filter (EKF) [16] is used to estimate the
state value. Given the state equation and output equation as
expressed in (6)-(7), we can estimate the next tendon length
value ˆ
xk+1|k+1based on the current value of estimate ˆ
xk|k,
input uk, and the measurement data yk. For nnumber of
active segments, the EKF can be expressed as follows
ˆ
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,
(8)
where Pk|k,Qk,Rk∈R3n×3nrepresent the estimation co-
variance, process noise variance and measurement noise
variance matrix respectively, Kk∈Rrepresents the Kalman

gain, while the output ykis computed from position sensor
measurement data, such as an electro-magnetic tracker.
The matrix Ak∈R3n×3nand Ck∈R3n×3nare a local
linearization of (6) and (7), defined as Ak=∂f(xk,uk)
∂xkand
Ck=∂g(xk)
∂xk. Using our state space equation model, Aturns
out to be an identity matrix while Cturns out to be a matrix
consisting of the Jacobian of each segment tip, as follows
Ak=I∈R3n×3n,(9)
Ck=J(xk,ξ=χ1)... J(xk,ξ=χn)T.(10)
The tip position estimation ˆ
yk|kis derived by applying Eq.
(7) to the state estimation ˆ
xk|kas follows,
ˆ
yk|k=g(ˆ
xk|k).(11)
However, as will be shown in Section IV, implementing
a standard EKF for all segments simultaneously from the
beginning (i.e., that the number of active segments chosen
to be equal to the total number of segments (n=N) at all
times), it is possible for the state estimate to arrive at a
physically impossible state value, such as a negative tendon
length. This is due to the fact that there can be more than
one mathematically possible state value for a given segment
tip position. To overcome this problem, a novel multi-stage
implementation of a non-linear observer is proposed.
For a three segment manipulator, the estimation starts from
the bottom segment only, followed by the bottom and middle
segments jointly, and finally all three segments as one are
considered, as illustrated in Figure 1b. This approach is
motivated by the fact that the large estimation error of the
base segment will disturb the performance of estimation in
the more distal segments. By doing such a stage-by-stage
estimation, a segment will be taken into consideration for the
estimation process (i.e. by incrementing the number of active
segments n) only after the tip position estimation error of
the previous segment(s) is lower than a predefined threshold
value δ. The proposed algorithm is formally defined in
Algorithm 1.
It is important to be noted that the size of the following
variables ˆ
xk|k,Pk|k,Qk,Rk,uk,yk,ˆ
yk|k,Ak,Ckdepend on the
number of active segments n. In line 3 of Algorithm 1, we
initialize a component of state vector qn∈R3. This vector
is then added as a new component of ˆ
xk|kin line 4. The
function Initialize() in line 5 will initialize matrix Pk|k,Qk,
Rk∈R3n×3nevery time the active segments is incremented.
The routine GetInputSignal() and GetSensorData() in line 7
and 8 will return vector uk∈R3nand yk∈R3nretrieved from
the input signal and sensor data respectively. In line 13, the
error vector eis derived by subtracting the latest component
of ˆ
yk|kand yk, i.e. estands for the tip’s position estimation
error of the last active segment.
B. The Complete Obstacle Avoidance Algotihm
A modified version of a reactive potential field method
based on [13] is used. The field is considered as the velocity
of the manipulator ˙
pdefined as the negative gradient of a
potential function U, i.e. ˙
p=−∇pU(p).
Algorithm 1 Multi-Stages Pose Estimation
1: n←1, k←0
2: loop
3: qn←InitState()
4: ˆ
xk|k←Add(qn)
5: (Pk|k,Qk,Rk)←Initialize(n)
6: loop
7: uk←GetInputSignal(n)
8: yk←GetSensorData(n)
9: Ak←Equation (9)
10: Ck←Equation (10)
11: (ˆ
xk+1|k+1,Pk+1|k+1)←EKF(ˆ
xk|k,Pk|k,uk,yk,Ak,
Ck)
12: ˆ
yk|k←Equation (11)
13: e←DetermineError(ˆ
yk|k,yk)
14: k←k+1
15: if norm(e)<δand n<Nthen
16: break
17: end if
18: end loop
19: n←n+1
20: end loop
Fig. 2. The block diagram of the proposed algorithm.
The attractive potential field to attract the manipulator’s
tip to a desired position pdcan be expressed as
˙
ppd=−c(p−pd).(12)
where cis an attractor stiffness. The repulsive potential
produced by an obstacle Ocan be expressed as follows
˙
pO=(η(1
ρ−1
ρ0)1
ρ2
∂ ρ
∂pif ρ<ρ0
0 if ρ≥ρ0
.(13)
where ρ=p(p−pO)T(p−pO)is the closest distance be-
tween a point in the manipulator’s body to an obstacle O,
ηis positive repulsion constant, and ρ0is the range of the
potential influence.
To make the whole body of the manipulator safe from
collision, we choose a number of point subjected to poten-
tials (PSP) along the body of the manipulator. The pose of
a PSP in segment-iis expressed as p(q,ξ=λi)where λi=
ξ1=1... ξi∈[0,1]ξi+1=0... ξN=0T. The re-
pulsive potential will be applied only to the PSP which is
closest to the obstacles.

0 2 4
0.03
0.04
0.05
0.06
t (s)
l11 (m)
0 2 4
0.04
0.06
0.08
t (s)
l12 (m)
024
0.03
0.04
0.05
0.06
t (s)
l13 (m)
0 2 4
0
0.02
0.04
0.06
t (s)
l21 (m)
0 2 4
0
0.02
0.04
0.06
t (s)
l22 (m)
024
0
0.02
0.04
0.06
t (s)
l23 (m)
0 2 4
−0.2
0
0.2
t (s)
l31 (m)
0 2 4
−0.2
0
0.2
t (s)
l32 (m)
024
−0.1
0
0.1
t (s)
l33 (m)
(a)
0 2 4
0
0.05
0.1
t (s)
x1 (m)
0 2 4
0
0.1
0.2
t (s)
y1 (m)
024
0.12
0.14
0.16
0.18
t (s)
z1 (m)
0 2 4
0.05
0.1
0.15
0.2
t (s)
x2 (m)
0 2 4
0.05
0.1
0.15
0.2
t (s)
y2 (m)
024
0.2
0.3
0.4
t (s)
z2 (m)
0 2 4
0
0.1
0.2
t (s)
x3 (m)
0 2 4
0
0.2
0.4
t (s)
y3 (m)
024
0
0.2
0.4
t (s)
z3 (m)
(b)
Fig. 3. The pose estimation using an multi-stages EKF for the zero-input scenario shows (a) comparison between the true state (red line) and the estimated
state (blue line) and (b) comparison between the true pose with added Gaussian noise (red line) and the estimated pose (blue line).
0 0.5
0.058
0.06
0.062
0.064
t (s)
l11 (m)
0 0.5
0.052
0.054
0.056
t (s)
l12 (m)
0 0.5
0.044
0.046
0.048
t (s)
l13 (m)
0 0.5
0.04
0.06
0.08
t (s)
l21 (m)
0 0.5
0.04
0.06
0.08
t (s)
l22 (m)
0 0.5
0.04
0.06
0.08
t (s)
l23 (m)
0 0.5
−0.5
0
0.5
t (s)
l31 (m)
0 0.5
−0.5
0
0.5
t (s)
l32 (m)
0 0.5
−0.5
0
0.5
t (s)
l33 (m)
(a)
0 0.5
−0.02
0
0.02
0.04
t (s)
x1 (m)
0 0.5
−0.2
−0.1
0
t (s)
y1 (m)
0 0.5
0
0.2
0.4
t (s)
z1 (m)
0 0.5
0.02
0.04
0.06
0.08
t (s)
x2 (m)
0 0.5
−0.16
−0.14
−0.12
−0.1
t (s)
y2 (m)
0 0.5
0.3
0.32
0.34
t (s)
z2 (m)
0 0.5
−0.02
0
0.02
t (s)
x3 (m)
0 0.5
−0.12
−0.1
−0.08
−0.06
t (s)
y3 (m)
0 0.5
0.32
0.34
0.36
0.38
0.4
t (s)
z3 (m)
(b)
Fig. 4. The pose estimation using a standard EKF for the zero-input scenario shows (a) comparison between the true state (red line) and the estimated
state (blue line) and (b) comparison between the true pose with added Gaussian noise (red line) and the estimated pose (blue line).
Finally, we transform each spatial velocity, applied at the
end-effector and the PSP, to the corresponding actuator space
velocity as an input for the kinematic model and the EKF,
uk. The total velocity in the actuator space is given by
uk=˙
q=J+
e˙
ppd+J+
a˙
pO(14)
where Jeand Jadenote the Jacobian of the tip and the
Jacobian of the PSP-a, i.e. the closest PSP to the obstacles.
The estimation and obstacle avoidance are combined to-
gether as shown in Figure 2. The pose of the end-effector
p(xk,ξ=χN)and the PSPs p(xk,ξ=λi)needed by the
obstacle avoidance stage are all retrieved from the estimation
stage. The estimated poses are determined by doing forward
kinematics for the estimated state produced by EKF, i.e.
ˆ
pk(ξ) = p(ˆ
xk|k,ξ). The pose information is used by the
obstacle avoidance stage to produce input signal ukas given
in (14) and then feed this to the estimation stages as written
in line 7 of Algorithm 1.
It is important to be noted that in the early phase, the
estimated state and pose are not yet to be close to the
real value. Thus, it becomes necessary to deactivate the
obstacle avoidance stage at the beginning of the estimation
process and activate it only when the sum of the tip position
estimation errors for all segments, ε, is less than a predefined
threshold value Ω. This is done by multiplying the value
of the attractor stiffness cand repulsion constant ηwith a
weight scalar wwhose value is given as follows
w=(0 if ε≥Ω
1 if ε<Ω,(15)
where ε=N
∑
i=1
norm(ˆ
pk(ξ=χi)−p(xk,ξ=χi)).
IV. RESULTS AND DISCUSSION
We implemented our proposed algorithm in ROS (Robot
Operating System), a real-time simulation environment, run-
ning at a rate of 1
∆t=40 Hz for a three-segment manipulator
model. Table I shows the properties of the manipulator
and the algorithm. We have 5 number of PSPs distributed

TABLE I
PROP ERT IE S OF MA NI PU LATO R AN D TH E PROP OS ED ALGORITHM
Param Value Param Value
d0.0134 m δ5×10−3m
L0.12 m c2
N3η2×10−7
Qk10−10I∈R3n×3nρ00.07 m
Rk2.5×10−5I∈R3n×3nΩ0.06 m
∆t2.5×10−2s
uniformly along the backbone of each segment from the tip
to the point close to the base. A spherical obstacle with a
radius of 1 cm is assumed to move at a constant speed.
The measurement data, i.e. the tip pose of each segment
yk, is retrieved from an ideal kinematic model with added
Gaussian noise to simulate the electromagnetic position
sensor. The noise has a zero mean and a standard deviation
of σ=5×10−3, i.e. the variance will be Rk=σ2I∈R3n×3n.
The kinematic model has the true state xkupdated at every
iteration, which is unknown, so that the obstacle avoidance
stage which relies on the estimated states ˆ
xk|kfrom the EKF.
It is important to be noted that the state value in this paper,
li j, refers to the deviation from a normal length L, so that
the tendon length is given by L+lij .
A. State and Pose Estimation
In the first pose estimation simulation, the input to the
manipulator is assumed to be zero (u=0∈R3N) at all
times. Starting from a random state value, we compare the
performance of the proposed multi-stage EKF and standard
EKF as shown in Figure 3 and 4 respectively. The resulting
segment length estimate is then used to produce tip pose
estimate via forward kinematics using (11).
We can see that the proposed multi-stage estimation is
able to estimate the states value (Figure 3a) as well as the
tip positions of all segments (Figure 3b) and, at the same
time, suppress the noise in the tip’s position measurement.
Although capable of estimating the tip position (Figure 4b),
the standard EKF fails to estimate the states value correctly
(Figure 4a). As we can observe, some of the state values,
for the third segment in this case, will produce a negative
tendon length, which is physically not possible. Hence, even
though the tip position estimation error converges to zero, we
are not able to correctly estimate the position of every point
along the segment of the manipulator except the tip using this
approach. Thus, the proposed multi-stage pose estimation has
better performance.
In the second simulation, we force the manipulator end
effector to follow a circular path. The input signal ukis
derived from the inverse Jacobian equation in (3). The results
(see Figures 5a and 5b) show that the proposed multi-stage
EKF can cope with the variation in the input signal and tip
pose measurement data.
B. Obstacle Avoidance
In this part, the pose estimator is combined with the ob-
stacle avoidance approach. A dynamically moving spherical
obstacle, drawn as a black sphere, moves close to the body
of manipulator while the target position, a small red dot, is
assumed to be constant.
In the first simulation, the obstacle moves at a height such
that it is close to the first segment, as shown in Figure
6a. In the second simulation, the obstacle moves close to
the middle segment, as shown in Figure 6b. Both figures
show how the obstacle avoidance approach makes use of
the pose estimation result to avoid collisions between the
body of the manipulator and the moving obstacle while also
continuously attracting the end effector to the desired target
position. It is noted that the algorithm will avoid not only the
tip of each segment, where the position sensors are attached,
from collision, but also the whole body of the manipulator.
This feature can not be achieved without combining the pose
estimation and obstacle avoidance and thus, it becomes the
main contribution of the proposed algorithm.
V. CONCLUSIONS AND FUTURE WORKS
In this paper, we propose a real-time pose estimation
and obstacle avoidance approach for a tendon-driven multi-
segment continuum manipulator operating in dynamic envi-
ronments. To the best of our knowledge, this is the first time
pose estimation and obstacle avoidance have been combined
to achieve collision-free motion for a multi-segment contin-
uum manipulator. The proposed multi-stage implementation
of the EKF outperforms the standard EKF and correctly
estimates the state values. The overall algorithm works well
in a real-time simulation environment.
In the future, this algorithm can be applied to real ma-
nipulators. Future work will also explore the application
of our approach to a more advanced dynamic model of
the continuum manipulator and a possible improvement on
the motion planning side, by combining the local reactive
obstacle avoidance with a global motion planning strategy
when operating in more complex environments.
REFERENCES
[1] P. Qi, C. Qiu, H. Liu, J. Dai, L. Seneviratne, and K. Althoefer,
“A novel continuum-style robot with multilayer compliant modules,”
in Intelligent Robots and Systems (IROS 2014), 2014 IEEE/RSJ
International Conference on, Sept. 2014, pp. 3175–3180.
[2] T. Mahl, A. Hildebrandt, and O. Sawodny, “A Variable Curvature
Continuum Kinematics for Kinematic Control of the Bionic Handling
Assistant,” Robotics, IEEE Transactions on, vol. 30, no. 4, pp. 935–
949, Aug. 2014.
[3] F. Maghooa, A. Stilli, Y. Noh, K. Althoefer, and H. Wurdemann,
“Tendon and pressure actuation for a bio-inspired manipulator based
on an antagonistic principle,” in Robotics and Automation (ICRA),
2015 IEEE International Conference on, May 2015, pp. 2556–2561.
[4] R. J. Webster, III and B. A. Jones, “Design and Kinematic Modeling
of Constant Curvature Continuum Robots: A Review,” Int. J. Rob.
Res., vol. 29, no. 13, pp. 1661–1683, Nov. 2010. [Online]. Available:
http://dx.doi.org/10.1177/0278364910368147
[5] W. McMahan, V. Chitrakaran, M. Csencsits, D. Dawson, I. Walker,
B. Jones, M. Pritts, D. Dienno, M. Grissom, and C. Rahn, “Field trials
and testing of the OctArm continuum manipulator,” in Robotics and
Automation, 2006. ICRA 2006. Proceedings 2006 IEEE International
Conference on, May 2006, pp. 2336–2341.
[6] R. Srivatsan, M. Travers, and H. Choset, “Using Lie algebra for shape
estimation of medical snake robots,” in Intelligent Robots and Systems
(IROS 2014), 2014 IEEE/RSJ International Conference on, Sept. 2014,
pp. 3483–3488.

0 5
−0.02
0
0.02
t (s)
l11 (m)
0 5
−0.01
0
0.01
0.02
t (s)
l12 (m)
0 5
0
0.01
0.02
0.03
t (s)
l13 (m)
0 5
0
0.02
0.04
t (s)
l21 (m)
0 5
−5
0
5x 10−3
t (s)
l22 (m)
0 5
0
0.01
0.02
0.03
t (s)
l23 (m)
0 5
−0.01
0
0.01
0.02
t (s)
l31 (m)
0 5
0
0.005
0.01
0.015
t (s)
l32 (m)
0 5
−0.01
0
0.01
0.02
t (s)
l33 (m)
(a)
0 2 4 6
−0.1
−0.05
0
t (s)
x1 (m)
0246
−0.1
0
0.1
t (s)
y1 (m)
0246
0
0.2
0.4
t (s)
z1 (m)
0 2 4 6
−0.1
0
0.1
t (s)
x2 (m)
0246
0
0.5
1
t (s)
y2 (m)
0246
0
0.2
0.4
t (s)
z2 (m)
0 2 4 6
−0.2
0
0.2
t (s)
x3 (m)
0246
−0.5
0
0.5
t (s)
y3 (m)
0246
0
0.5
1
t (s)
z3 (m)
(b)
Fig. 5. The pose estimation using an multi-stage EKF for the circular-end-effector-path scenario shows (a) comparison between the true state (red line)
and the estimated state (blue line), (b) comparison between the true pose with added Gaussian noise (red line) and the estimated pose (blue line)
(a)
(b)
Fig. 6. A three-segments continuum manipulator with a static target (small red dot) when obstacle (black sphere) moves close to (a) the bottom segment
and (b) middle segment. The order of movement is from left to right picture.
[7] Z. Zhang, J. Shang, C. Seneci, and G.-Z. Yang, “Snake robot shape
sensing using micro-inertial sensors,” in Intelligent Robots and Systems
(IROS), 2013 IEEE/RSJ International Conference on, Nov. 2013, pp.
831–836.
[8] E. Lobaton, J. Fu, L. Torres, and R. Alterovitz, “Continuous shape
estimation of continuum robots using X-ray images,” in Robotics and
Automation (ICRA), 2013 IEEE International Conference on, May
2013, pp. 725–732.
[9] J. Borgstadt, M. Zinn, and N. Ferrier, “Multi-modal localization
algorithm for catheter interventions,” in Robotics and Automation
(ICRA), 2015 IEEE International Conference on, May 2015, pp. 5350–
5357.
[10] W. Rone and P. Ben-Tzvi, “Multi-segment continuum robot shape
estimation using passive cable displacement,” in Robotic and Sensors
Environments (ROSE), 2013 IEEE International Symposium on, Oct.
2013, pp. 37–42.
[11] J. Xiao and R. Vatcha, “Real-time adaptive motion planning for a
continuum manipulator,” in Intelligent Robots and Systems (IROS),
2010 IEEE/RSJ International Conference on, Oct. 2010, pp. 5919–
5926.
[12] G. Chen, M. T. Pham, and T. Redarce, “Sensor-based
guidance control of a continuum robot for a semi-
autonomous colonoscopy,” Robotics and Autonomous Systems,
vol. 57, no. 67, pp. 712 – 722, 2009. [Online]. Available:
http://www.sciencedirect.com/science/article/pii/S0921889008002042
[13] A. Ataka, P. Qi, H. Liu, and K. Althoefer, “Real-Time Planner for
Multi-Segment Continuum Manipulator in Dynamic Environments,” in
Robotics and Automation (ICRA), 2016 IEEE International Conference
on, May 2016, pp. 4080–4085.
[14] A. Ataka, P. Qi, A. Shiva, A. Shafti, H. Wurdemann, P. Dasgupta, and
K. Althoefer, “Towards safer obstacle avoidance for continuum-style
manipulator in dynamic environments,” in 2016 6th IEEE International
Conference on Biomedical Robotics and Biomechatronics (BioRob),
June 2016, pp. 600–605.
[15] I. Godage, D. Branson, E. Guglielmino, and D. Caldwell, “Path
planning for multisection continuum arms,” in Mechatronics and
Automation (ICMA), 2012 International Conference on, Aug. 2012,
pp. 1208–1213.
[16] A. Jazwinski, Stochastic Processes and Filtering Theory, ser. Mathe-
matics in Science and Engineering. Elsevier Science, 1970. [Online].
Available: https://books.google.co.id/books?id=nGlSNvKyY2MC