This is the html version of the file https://www.researchgate.net/profile/Amirhossein-H-Memar/publication/282010403_Motion_Planning_for_a_Multi-Segment_Continuum_Robot/links/5601598d08aec948c4faa8e9/Motion-Planning-for-a-Multi-Segment-Continuum-Robot.pdf.
Google automatically generates html versions of documents as we crawl the web.
Motion Planning for a Multi-Segment Continuum Robot
  Location via proxy:   [ UP ]  
[Report a bug]   [Manage cookies]                
Page 1
Proceedings of the International Conference on Mechanical Engineering and Mechatronics
Ottawa, Ontario, Canada, 15-17 August 2012
Paper No. XXX (The number assigned by the OpenConf System)
XXX-1
Motion Planning for a Multi-Segment Continuum Robot
Amirhossein Haji agha Memar1, Mehdi Keshmiri2, Keivan Torabi3
1,3 Department of Mechanical Engineering, University of Kashan, Kashan, Iran
2, Department of Mechanical Engineering, Isfahan University of Technology, Isfahan, Iran
ah.hajiaghamemar@me.iut.ac.ir; mehdik@cc.iut.ac.ir; kvntrb@kashanu.ac.ir
Abstract – Motion planning and redundancy resolution of a multi-segment continuum robot is studied. The
direct and inverse kinematics model of the robot is derived assuming a constant radius of curvature in each flexible
segment of the robot. Due to the primary and secondary backbone structure of the segment, each segment is
modelled by two configuration variables and three joint space variables. This introduces both kinematics and
actuation redundancy in the system. Kinematic redundancy of the robot is resolved through a closed loop Jacobian
pseudo inverse technique, completed with the damped least square technique for singularity robustness provision.
Furthermore, an improved Jacobian based motion planning is introduced which optimizes a predefined cost function
corresponding a given secondary task. The methods are implemented both numerically and experimentally and
accuracy and errors of the results are analyzed.
Keywords: Continuum Robot, Motion Planning, Redundancy Resolution, Jacobian Pseudo Inverse Method,
Damped Least Square Method
1. Introduction
Continuum or continuous backbone robots have been recently received a good attention by
researchers. Continuum robots are inspired by invertebrate structures which are found in the nature such
as biological tongues, trunks, and tentacles (Trivedi et al., 2008). Unlike the traditional rigid-link robot
manipulators, continuum robots provide their motion by deformation of flexible parts. Continuum robots
offer some distinctive features compared to traditional rigid link robots such as inherent compliance,
reduced weight, fault tolerance, and whole arm manipulation capability. These robots can search inside
confined spaces much easier than traditional rigid link robots. They exhibit improved performance in the
areas of obstacle avoidance, compliant operation, and navigation in highly unstructured and narrow
environments (Godage et al., 2011). Continuum robot applications in the areas of nuclear (OC Robotics,
Web-1), medical surgical procedures (Haiyan et al.,2011; Reiter et al., 2011), and rescue tasks
(Tsukagoshi et al., 2001) have become increasingly popular nowadays. Just like an elephant which picks
up a log with its trunk, a continuum robot can picks up any object by wrapping itself around it.
Unlike traditional rigid-link robots, continuum robots are more complex to model, design, and
construct due to their lack of rigidity. In recent years, significant progress has been made in the modeling
of the continuum robots. General kinematics models of these robots have been developed by (Chirikjian
and Burdick, 1994; Gravagne and Walker, 2000; Jones and Walker, 2006), and researches in dynamic
model ing of the robot s have been developed by (Mochiyama and Suzuki, 2003; Tatlicioglu et al., 2009).
In the research related to the continuum robots, path planning of the robot is a missing part and so
far has not been elaborated by the researchers. This paper studies this issue for an n-segment continuum
robot and the approaches are implemented numerically and practically on a two-segment robot. In the
following sections, initially the kinematics modeling of a multi-segment continuum robot is presented.
Then two different approaches for motion planning are introduced to provide singularity robustness and
redundancy resolution for the robot. In the final part of the paper the experiment results gathered from
practical implementation of the approaches on a two-segment continuum robot (Fig. 1) are presented.
Page 2
XXX-2
2. Kinematics Modeling
2. 1. Robot Structure
This paper presents motion planning of a continuum robot with non-extendable backbone which is
initially introduced by Simaan et al. (2004a, b). The flexible segments of the robot can be stacked serially
to form a robot with more DoFs. As Fig. (2) shows each segment consists of one passive primary
backbone with fixed length and three lateral active secondary backbones with adjustable lengths. These
secondary backbones are equidistant from each other as well as the primary backbone. The robot
actuators could force the secondary backbones move in push-pull modes, and cause the whole robot to
move. In addition to backbones, in each segments there are some rigid disks used in the robot structure
called the base disk, the spacer disks and the end disk consecutively, where the end disk of each segment
would be the base disk of the next segment. The primary backbone is attached to the base disk and the
end disk, while the secondary backbones are only attached to the end disk and can slide freely through the
base disk and the spacer disks.
Fig. 1. The experimental setup
consists of two continuum segments
Fig. 2. Structure of a segment of a continuum robot and its kinematics
nomenclature (Xu K., 2009)
2. 2. Kinematics
The actual bending of the segments assumes a shape that minimizes its potential energy. This shape
function is the solution of a nonlinear partial differential equation (Li and Rhan, 2002). A general closed-
form solution has not been obtained yet. Therefore, for to simplify kinematics modeling of the robot it is
assumed that each robot segment bends with a constant radius of curvature.
As shown in Fig. (2) the coordinate systems attached to the base disk (BCS) and the end disk (ECS)
of t-th segment are defined as { } {
}
t
tb
tb
tb
ˆ ˆ ˆ
b
x ,y ,z
=
and { } {
}
t
te
te
te
ˆ ˆ ˆ
e
x ,y ,z
=
Respectively. The
configuration of each segment is defined by the bending angle ( tL
θ ) and the angle of bending plane
containing the primary backbone ( t
δ ). The configuration space for an n-segment continuum robot is
defined as
T
T
T
T
(n)
1
2
n
...
ψ = ψ ψ
ψ
, where
T
t
tL
t
ψ = θ δ  is the configuration space of t-th robot segment.
The actuation length vector in the joint space for an n-segment continuum robot is therefore defined
as
[
]T
(n)
1
2
n
q
q q ...q
=
, where its t-th component is defined by
[
]T
t
t1
t2
t3
q
q q q
=
and ti
ti
t
q
L
L
=
− .
t
ti
L andL are the length of the primary backbone and the i-th secondary backbones of the t-th segment,
correspondingly.
Page 3
XXX-3
The tip position of each segment with respect to the base disk coordinate system is obtained by
integrating along the tangent of the primary backbone. Therefore, the simplified relations between robot
configuration space and task space are given by:
t
tL
bt
t
tL
t
tL
tL
tL
cos (1 sin )
L
p
sin (sin
1)
2
cos
δ −
θ
=
δ
θ −
π −θ
θ
(1)
when tL
θ approaches 2
π , Eq. (1) has an expression of
[
]T
bt
tL
t
p
00L
=
. The rotation matrix of ECS with
respect to BCS for the t-th segment can be obtained by a successive rotation sequence as follows:
bt
et
tb
t
tc
tL
te
t
ˆ
ˆ
ˆ
R
Rot(z , )Rot(y , 2
)Rot(z , )
=
−δ
π − θ
δ
(2)
where
ˆ
Rot(n, )γ is the rotation matrix of rotating γ about the n . Using elements of this rotation matrix,
the corresponding Euler angles could be calculated.
According to the definition of robot coordinate systems, the BCS of the t-th segment will be the
same as EDS of the (t-1)-th segment. Hence the tip pose of t-th segment can be derived as:
bt
b1
b1
tL
tL
bt
p
p
T
1
=
×
(3)
b1
b1
b1
bt
(t 1)L
bt
R
p
T
0
1
−
= 
(4)
b1
b1
bt
et
bt
et
R
R
R
=
×
(5)
The curvature radius offset of i-th secondary backbone from the primary backbone is ti
ti
r cos
∆ =
δ ,
obtained by projecting the secondary backbone on the segment bending plane, where r is the radius of
pitch circle containing the secondary backbone positions in the robot disks, and ti
t
2 (i 1) 3
δ = δ + π −
.
Using the definition of the length of the primary backbone, t
L , and the length of the i-th secondary
backbone as ti
L , the kinematics relation between the robot configuration space and joint space variables
of the t-th segment is derived as:
ti
ti
tL
q
(
2)
=∆ θ −π
(6)
Inverse of Eq. (6) for a given ti
q is as follows:
tL
ti
ti
2 q
θ = π +
∆
(7)
t
t2
t1
t1
atan2(q
q cos(2 3), q sin(2 3))
δ =
−
π
−
π
(8)
Because of the actuation coupling effect between adjacent segments in a multi-segment robot, the
actuation length of t-th segment ( (t)
q ) must be formulated as:
t 1
(t)
t
i
i 1
q
q
q
−
=
= + ∑
(9)
Page 4
XXX-4
2. 3. Instantaneous kinematics
The Instantaneous kinematics between the joint space variables to the robot task space variables
can be defined by the Jacobian matrices as the followings:
q
q J ψ
=
ψ
(10)
x
x J ψ
=
ψ
(11)
The Jacobian matrix related the configuration space velocity vector ( t
ψ ) to the joint space velocity
vector ( t
q ) is obtained by taking time derivative of Eq. (6) with respect to t
ψ . For an n-segment
continuum robot q
J ψ can be formulated as Eq. (12) considering the actuation coupling effect:
1 1
2
2
n
n
q
q
q
J
0
0
I 0
0
0
J
0
I I 0
q A
where A
I
0
0
0
I
I I
0
0 J
ψ
ψ
ψ
= ×
=
(12)
where
3 3
I R ×
∈
is an identity matrix.
3. Motion Planning
For continuum robots consisting of two or more segments, there is a set of kinem atic redundancy of
DoFs with respect to a pure positioning of end disk task, since each segment has two DoFs. This
redundancy provides the robot with an increased level of dexterity that maybe used to execute secondary
tasks like avoiding singularities, joint limits, and task space obstacle or in general to optimize desired
objective functions. Normally in the redundant robots, especially in the continuum robots, there is no
closed-form solution for the inverse kinematics problem and redundancy resolution is performed using
differential form of the robot kinematics.
3. 1. Redundancy Resolution
The Robot configuration space (ψ ) is used to perform redundancy resolution. For multi-segment
robots the general solution of Eq. (11) is defined by Jacobian pseudo inverse method as:
(
)
†
†
x
x
x
0
J x I J J
ψ
ψ
ψ
ψ =
+ −
ψ
(13)
this equation is in fact a least square solution to the end disk task constraint that minimizes
x
x J ψ
−
ψ .
Matrix (
)
†
x
x
I J Jψ
ψ
−
is an orthogonal projection matrix in the null space of x
J ψ and 0
ψ is an arbitrary
velocity vector in the robot configuration space. It should be noted that the second part of this solution is a
null space velocity vector. Various values of 0
ψ result in different configuration velocities that all provide
the same end disk task velocity. Therefore, 0
ψ can be used to execute given secondary tasks.
The desired configuration position profile is obtained by time integration of Eq. (13). In the
computer simulation and implementation of the robot motion, the configuration space velocity will be
available at specified time instants, because the integration is performed numerically in discrete time
steps. Regardless of numerical integration method, the unavoidable small error occurring in each
numerical integration step, leads to a long term deviation from the exact position profile. Another source
of error accumulation is the possible uncertainty in the initial position of the robot configuration which
causes an initial as well as steady state drift from the desired position profile. Normally a solution to
Page 5
XXX-5
overcome these problems is introduced based on the use of a feedback correction algorithm which named
closed-loop inverse kinematics (Chiacchio et al., 1991). In this method the robot configuration space
velocity at the current position is given by:
(
) (
)
†
†
x
P
x
x
0
J x K x g( )
I J J
ψ
ψ
ψ
ψ =
+
− ψ
+ −
ψ
(14)
where P
K is a positive definite gain matrix and g( )ψ is the forward kinematics function.
3. 3. Redundancy Resolution via Optimization
The second part of Eq. (13) can be used to execute secondary task while a desired path is being
tracked by the robot end disk. In this equation 0
ψ should be chose in a way that to minimize or maximize
an index function or a criteria performance as the secondary task. This can be performed through a local
optimization. A simple form of the local optimization is the pseudo inverse solution gained from 0
0
ψ =
which provides the configuration space velocity with the minimum norm among those which realize the
task constraint.
Another technique is to use the general solution (Eq. 13) by choosing the arbitrary configuration
space velocity ( 0
ψ ) in the direction of the anti-gradient of a scalar configuration-dependent performance
criteria like H( )ψ which must be minimized (if 0
ψ is chosen in the direction of performance criteria
gradient, H( )
ψ reaches its maximum value). Therefore 0
ψ is defined as:
0
H
k
H( )
ψ =− ∇ ψ
(15)
where H
k is a scalar step size and H
∇
is the gradient of H( )ψ with respect to ψ at the current robot
configuration. This technique has been identified as the projected gradient method in the literature.
Substituting Eq. (15) in Eq. (13) results in the following expression:
(
)
†
†
x
H
x
x
J x k I J J
H( )
ψ
ψ
ψ
ψ =
−
−
∇ ψ
(16)
Equation (16) gives a trade-off between local minimization of the performance criteria and the
satisfaction of motion constraint (Eq. 11) by a minimum-norm configuration space velocity. The choice of
the step size H
k is again the critical issue for obtaining a good performance in the redundancy resolution
of the robot. A small value of H
k may slow down the minimization of H( )
ψ , while a large value may
even lead to an increase of H( )ψ , since the gradient of H( )ψ is computed locally.
Controlling the maximum bending of the robot segments can be introduced as an application of this
redundancy resolution. Assuming a bending angle limitation in each segment of the continuum robot,
kinematic redundancy and the above mentioned optimization can be performed such that to keep the tL
θ
components of robot configuration space away from this limitation. To ensure this purpose the following
performance criteria is introduced:
2
n
tL
tL,mid
t 1
tL,max
tL,min
1
H( )
2 =
θ −θ
ψ =
θ
−θ
∑
(17)
where tL,min
tL,max
,
θ
θ
is the allowed range for bending angle and tL,mid
θ
at its midpoint.
Another interesting application of this type of redundancy resolution is obstacle avoidance, which
can be enforced by minimizing an obstacle artificial potential field cost function. In this method a scalar
function as the potential field intensity is defined regarding the obstacle position and shape. The value of
Page 6
XXX-6
this function increases by approaching the obstacle surface. A summation of the field intensities at certain
points on the robot configuration represents the performance criteria. For example to avoid a spherical
obstacle, H( )ψ is given by:
m
O
i 1
O
i
1
P
C (P) 1
=
=
−
∑
(18)
( ) (
) (
) (
)
2
2
2
2
O
i
C
i
C
i
C
i
C
C (P) 1R
x x
y y
z z
=
−
+
−
+
−
(19)
where m is the number of points on the robot configuration, [
]T
i
i
i
x ,y ,z
identifies the Cartesian position
of the i-th point, [
]T
C
C
C
x ,y ,z
identifies the position of the obstacle central point, and C
R is the radius of
the sphere.
3. 2. Singularity Robustness
A robot configuration is singular when the task Jacobian ( x
J ψ ) becomes rank deficient. In a
singular configuration it is impossible to generate end disk movement in certain directions. This
singularity can be better understood using singular value decomposition of the Jacobian matrix and its
pseudo inverse. They can be written as:
M
T
T
x
i i i
i 1
J
U V
u v
ψ
=
= ∑
=
σ
∑
(20)
R
†
†
T
T
x
i i
i 1
i
1
J
V
U
v u
ψ
=
= ∑
=
σ
∑
(21)
where U is the M M
×
orthonormal matrix of the output singular vectors i
u , V is the N N
×
orthonormal matrix of the input singular vectors i
v , ∑ is the M N
×
matrix whose a M M
×
diagonal
submatrix containing the singular values i
σ of the matrix x
J ψ , and R denote rank of this matrix. In a
redundant robot with N M
>
, when a singularity occurs, the smallest singular value ( M
σ ) tends to zero.
Therefore, task velocities along M
u become unfeasible for the robot end disk.
This singularity analysis is performed for our laboratory one and two-segment continuum robots
through the calculation of the smallest singular value for different robot configurations. It is seen that for
single or multi-segments robots singularity occurs only at the robot straight configuration. To avoid this
singularity and introduce singularity robustness when the continuum robot cross this straight
configuration, a damped least square technique is used in the inverse kinematics solution as follows:
(
) 1
T
T
2
x
x
x
J J J
I x
−
ψ
ψ
ψ
ψ=
+λ
(22)
where λ is the damping factor. This technique provides consideration of accuracy and feasibility together
as choosing the configuration space velocity required to match the given task space velocity. Therefore,
adequate damping factor selection is the major issue in this method. A greater value of λ results in lower
tracking accuracy but higher robustness to the singular configuration. In our simulation and experimental
analysis the following damping factor is used:
Page 7
XXX-7
( )
M
2
2
2
M
max
0
when
1
otherwise
σ ≥ε

λ =
− σ ε λ

(23)
where ε defines the size of the singular region and max
λ
is the maximum value of the damping factor
which is used only at the singularity. When the robot configuration approaches its singularity, Eq. (22) is
used instead of Eq. (14) in relations.
4. Implementation
The closed-loop inverse kinematics technique along with singularity avoidance through the damped
least square approach is implemented numerically as well as experimentally in a two-segment continuum
robot. The experiment platform is shown in Fig. (1) and its closed loop block diagram structure is shown
in Fig. (3).
The experiment platform is a two-segment continuum robot with concentric super-elastic tubes as
backbones. Each secondary backbone is actuated in a push-pull mode by a clamping gripper. It is attached
to the corresponding backbone and is driven by motorized ball screws. Position (length) control of the
actuating backbones is performed through a set of servomotors with shaft encoders and 0.002 mm
resolution. The robot actuation unit has been designed such that the secondary backbones do not pass
through the cone channels. Furthermore, a stereo vision system with a rate of 75 Hz has been used to
recognize the spatial position of the robot end disk. Physical parameters of robot segments are the same as
values shown in Table. (1). These values are used in the numerical simulation as well.
Two different paths in the task space have been introduced to be tracked by the end disk of the
robot for this part of the paper. In the first one a straight path from the initial robot straight configuration
with no bending to point [
]T
d
d
R ,0,Z
is tracked by the end disk and the second one is a circular path with
radius d
R on the plane perpendicular to tb
z containing the above point is introduced to the end disk to be
tracked. The magnitude of the physical parameters and coefficients of the robot used in these motion
plannings test are shown in Table. 1.
The particular solution obtained by setting 0
0
ψ = in Eq. (13) provides the least-squares solution of
Eq. (11) with minimum norm solution which known as the pseudo inverse solution. The least-squares
property quantifies the accuracy of the end disk path tracking, while the minimum norm property may be
relevant for the feasibility of the configuration space velocities.
Fig. 3. Block diagram of the continuum robot control loop
Table. 1. The value of robot segments physical parameters and motion planning coefficients
Parameter
Value
Parameter
Value
Parameter
Value
Parameter
Value
1
L (mm)
93
r(mm)
4.8
max
λ
0.25
d
R (mm)
30
2
L (mm)
69
P
K
3 3
0.8I ×
ε
0.01
d
Z (mm)
140
Simulation results for the straight line path tracking is shown in Fig. (4) while the experimental
results for the second part, circular path tracking, are shown in Fig. (5). The initial errors in the straight
line path tracking are due to passage of the singular configuration and less accuracy of the redundancy
Page 8
XXX-8
resolution technique introduced by the damped least square method. Figure (5) compares the robot end
disk motion obtained from stereo vision system with the desired path. Similar or even a better
performance is obtained in the first test, where the end disk tracks a straight line. The experiments were
performed based on the encoder data and the introduced forward kinematics model of the multi-segment
robot. Part of the errors in the experiment set up can be justified through this feature of the experiment
setup.
Fig. 4. The simulation result of tracking straight path by
the robot end disk
Fig. 5. The implementation result of tracking circular
path by the robot end disk
5. Conclusion
Motion planning for a multi-segment continuum robot has been studied in this paper. Kinematic
modeling and derivation of the Jacobian matrices has been done by assumption of constant radius of
curvature along robot backbones. An offline closed-loop Jacobian pseudo inverse technique has been used
to perform redundancy resolution and motion planning. It has been realized that the robot fully straight
configuration is a singular configuration. Since passing through this configuration is inevitable, the
damped least square technique is included in the redundancy resolution procedure to provide singularity
robustness to the robot while passing through the singular configuration. Furthermore, the method is
improved for some secondary tasks via cost function minimization while satisfying the end disk motion
constraint. The methods have been implemented both in a numerical simulation and a two-segment
experimental setup. Results showed the good performance of motion planning techniques in generating
the joint space trajectories. Error of end disk path tracking can be justified by uncertainties of the
kinematics model and errors of the measurement system. A closed-loop control strategy will be
introduced in future research to overcome these errors.
References
Chiacchio P., Chiaverini S., Sciavicco L., Siciliano B. (1991). Closed-loop inverse kinematics schemes
for constrained redundant manipulators with task space augmentation and task priority strategy. Int.
J. Robot, 10, 410–425.
Chirikjian G., Burdick J. (1994). A modal approach to hyper-redundant manipulator kinematics. IEEE
Trans. Robot. Autom, 10, 343–354.
Godage I. S., Guglielmino E., Branson D. T., Medrano-Cerda G. A., Caldwell D. G. (2011). Novel Modal
Approach for Kinematics of Multisection Continuum Arms “Proc. of IEEE/RSJ Int. Conf. on
Intelligent Robots and Systems (IROS),” San Francisco, CA, USA, Sept. 25-30, pp. 1093-1098.
Gravagne I., Walker I. (2000). Kinematic transformations for remotely actuated planar continuum robots
“Proc. of Int. IEEE Conf. Robot. Autom.,” San Francisco, CA, USA, Apr. 24-28, pp. 19–26.
Page 9
XXX-9
Haiyan H., Li W., Li J., Li M., Sun L. (2011). Two-dimension guidance control and simulation of a
colonoscopic robot “Proc. of Int. Conf. Strategic Technology (IFOST),” Harbin, Heilongjiang, China,
Aug. 22-24, pp. 323 – 327.
Jones B., Walker I. (2006). Kinematics of multisection continuum robots. IEEE Trans. Robot., 22, 43–57.
Li C., Rhan C. (2002). Design of Continuous Backbone, Cable-Driven Robots. ASME J. of Mechanical
Design, 124, 265-271.
Mochiyama H., Suzuki T. (2003). Kinematics and dynamics of a cable-like hyper-flexible manipulator
“Proc. of IEEE Int. Conf. Robot. Autom.,” Taipei, Taiwan, Sept. 14-19, pp. 3672–3677.
Reiter A., Goldman R. E., Bajo A., Iliopoulos K., Simaan N., Allen P. K. (2011). A learning algorithm for
visual pose estimation of continuum robots “Proc. of IEEE/RSJ Int. Conf. on Intelligent Robots and
Systems (IROS),” San Francisco, CA, USA, Sept. 25-30, pp. 2390–2396.
Simaan N., Taylor R., Flint P. (2004). High Dexterity Snakelike Robotic Slaves for Minimally Invasive
Telesurgery of the Upper Airway “Proc. of Int. Conf. Medical Image Computing and
ComputerAssisted Intervention (MICCAI),” St. Malo, France, Sept. 26-30, pp. 17-24.
Simaan N., Taylor R., Flint P. (2004). A Dexterous System for Laryngeal Surgery Multi Backbone
Bending Snakelike Slaves for Teleoperated Dexterous Surgical Tool Manipulation “Proc. of IEEE
Int. Conf. Robotics and Automation (ICRA),” New Orleans, USA, 26 April-1 May, pp. 351-357.
Tatlicioglu E., Walker I., Dawson D. (2009). Dynamic modeling for planar extensible continuum robot
manipulators. Int. Jour. Robot. Autom., 24(1), 1087–1099.
Trivedi D., Rahn C. D., Kierb W. M., Walker I. D. (2008). Soft robotics: Biological inspiration, state of
the art, and future research. Applied Bionics and Biomechanics, 5(3), 99-117.
Tsukagoshi H., Kitagawa A., Segawa M. (2001). Active Hose: an artificial elephant's nose with
maneuverability for rescue operation “Proc. Of IEEE Int. Robotics and Automation (ICRA),” Seoul,
Korea, May 21-26, pp. 2454-2459.
Xu K. (2009). Design, Modeling and Analysis of Continuum Robots as Surgical Assistants with Intrinsic
Sensory Capabilities. Thesis (PhD), Columbia University.
Web sites:
Web-1: http://www.ocrobotics.com/