DISSERTATION

FAILURE-TOLERANT PATH PLANNING FOR KINEMATICALLY REDUNDANT

MANIPULATORS
Submitted by
Rodrigo S. Jamisola, Jr.
Department of Electrical and Computer Engineering
In partial fulllment of the requirements
For the Degree of Doctor of Philosophy
Colorado State University
Fort Collins, Colorado
Fall 2006
Copyright by Rodrigo S. Jamisola, Jr. 2006
All Rights Reserved
COLORADO STATE UNIVERSITY
July 31, 2006
WE HEREBY RECOMMEND THAT THE DISSERTATION PREPARED UNDER
OUR SUPERVISION BY RODRIGO S. JAMISOLA, JR. ENTITLED FAILURE-
TOLERANT PATH PLANNING FOR KINEMATICALLYREDUNDANT MA-
NIPULATORS BE ACCEPTED AS FULFILLING IN PART REQUIREMENTS FOR
THE DEGREE OF DOCTOR OF PHILOSOPHY.
Committee on Graduate Work
Prof. Rodney G. Roberts
Prof. Edwin K. P. Chong
Prof. Wade O. Troxell
Prof. Iuliana Oprea
Prof. Anthony A. Maciejewski
Adviser
Prof. Anthony A. Maciejewski
Department Head
ii
ABSTRACT OF DISSERTATION
FAILURE-TOLERANT PATH PLANNING FOR KINEMATICALLY REDUNDANT
MANIPULATORS
This works addresses the issue of nding locations in a robots workspace that are
guaranteed reachable in the presence of joint failures. This guarantee is useful for manip-
ulators performing tasks in remote or hazardous environments that preclude direct human
intervention, and for tasks where a robot failure could possibly pose a signicant danger.
The rst part of this work considers start and goal workspace locations that are guaran-
teed reachable in the presence of joint failures and obstacles in the workspace. The method
of nding a solution is to nd a set of possible obstacle-free paths that guarantees the ex-
istence of a solution. It is shown that this set makes up a simply-connected, obstacle-free
surface with no internal local minimum or maximum in the manipulators conguration
space. A necessary condition and a sucient condition are presented that state the require-
ments to possibly nd a solution.
The second part of this work considers the problem of identifying a region in the
workspace where failure tolerance can be achieved. This is performed by nding a suit-
able set of articial joint limits prior to a failure such that the failure-tolerant region exists.
Conditions are presented that characterize the end-eector locations in this region. Based
on these conditions, a method is presented that identies its the boundaries. Once a failure-
tolerant workspace region is identied, robot tasks can be specied within this region.
Rodrigo S. Jamisola, Jr.
Department of Electrical and Computer Engineering
Colorado State University
Fort Collins, Colorado 80523
Fall 2006
iii
To my wife, who is a strong pillar and uncomplaining helper,
to our beautiful children, Maanyag and Dagohoy,
and to God the Almighty, who unceasingly whispers His ideas.
iv
TABLE OF CONTENTS
SIGNATURE . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . ii
ABSTRACT OF DISSERTATION . . . . . . . . . . . . . . . . . . . . . . . . . iii
DEDICATION . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . iv
LIST OF TABLES . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . vii
LIST OF FIGURES . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . viii
LIST OF SYMBOLS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . xiii
I INTRODUCTION . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1
1.1 Problem Statements . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2
1.2 Summary of Previous Work . . . . . . . . . . . . . . . . . . . . . . . . . . 2
1.2.1 Robot Safety and Reliability . . . . . . . . . . . . . . . . . . . . . . 3
1.2.2 Tasks Requiring Tolerance to Failure by Robots . . . . . . . . . . . 6
1.2.3 Failure Detection and Identication . . . . . . . . . . . . . . . . . . 7
1.2.4 Failure Tolerance through Layered Control Architecture . . . . . . 8
1.2.5 Kinematic Failure Tolerance . . . . . . . . . . . . . . . . . . . . . . 9
1.2.6 Local Kinematic Failure Tolerance . . . . . . . . . . . . . . . . . . 10
1.2.7 Identifying Kinematic Failure-Tolerant Workspace . . . . . . . . . . 11
1.2.8 Kinematic Failure-Tolerant Trajectory Planning . . . . . . . . . . . 13
1.3 Organization of this Work . . . . . . . . . . . . . . . . . . . . . . . . . . . 17
II KINEMATICS OF REDUNDANT MANIPULATORS . . . . . . . . . 19
2.1 The Robot Workspace and Conguration Space . . . . . . . . . . . . . . . 19
2.2 The D-H Convention . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21
2.3 The Robot Jacobian . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23
III A SOLUTION TO THE FAILURE-TOLERANT PATH PLANNING
PROBLEM . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26
3.1 Overview of the Proposed Approach . . . . . . . . . . . . . . . . . . . . . . 26
3.2 Conditions for the Existence of a Solution . . . . . . . . . . . . . . . . . . 29
3.3 Global Failure Tolerance . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33
v
IV ALGORITHM AND PSEUDO CODE IMPLEMENTATION . . . . . 38
4.1 Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38
4.2 Pseudo Code Implementation . . . . . . . . . . . . . . . . . . . . . . . . . 38
4.2.1 Step 1: Computing M
s
and M
g
. . . . . . . . . . . . . . . . . . . 39
4.2.2 Step 2: Identifying
s
and
g
. . . . . . . . . . . . . . . . . . . . . 46
4.2.3 Step 3: Testing the Necessary Condition . . . . . . . . . . . . . . . 49
4.2.4 Step 4. Testing the Sucient Condition . . . . . . . . . . . . . . . 55
4.3 Generating Monotonic Curves . . . . . . . . . . . . . . . . . . . . . . . . . 62
V SIMULATION RESULTS . . . . . . . . . . . . . . . . . . . . . . . . . . . . 65
5.1 Illustrative Examples on a 3-DOF Planar Robot . . . . . . . . . . . . . . . 65
5.2 Statistical Examples on a 3-DOF Planar Robot . . . . . . . . . . . . . . . 68
5.2.1 Computation of Self-Motion Manifolds . . . . . . . . . . . . . . . . 71
5.2.2 Identifying Obstacle-Free Portions of M
s
and M
g
. . . . . . . . . 72
5.2.3 Checking the Necessary Condition . . . . . . . . . . . . . . . . . . . 73
5.2.4 Computing a Failure Surface . . . . . . . . . . . . . . . . . . . . . . 75
5.2.5 Comparison to Previous Approaches . . . . . . . . . . . . . . . . . 77
5.3 Seven DOF Redundant Robot Example . . . . . . . . . . . . . . . . . . . . 78
VI IDENTIFYING THE FAILURE-TOLERANT WORKSPACE BOUND-
ARIES OF A KINEMATICALLY REDUNDANT MANIPULATOR . 83
6.1 The Imposition of Articial Joint Limits . . . . . . . . . . . . . . . . . . . 83
6.2 Problem Formulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 85
6.3 Identifying the Pre-failure Workspace Boundaries . . . . . . . . . . . . . . 88
6.4 Conditions for Failure-Tolerant Workspace Locations . . . . . . . . . . . . 91
6.5 Identifying the Failure-Tolerant Workspace Boundaries . . . . . . . . . . . 92
6.6 Examples . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 99
VII SUMMARY AND CONCLUSIONS . . . . . . . . . . . . . . . . . . . . . 104
vi
LIST OF TABLES
1 The set of start congurations shown in Fig. 19 with the corresponding coor-
dinates of the failure planes that intersect the continuous obstacle-free portion
of the goal self-motion manifold,
g
. . . . . . . . . . . . . . . . . . . . . . . 66
2 The set of feasible obstacle-free start congurations,
s
, shown in the top
tow of Fig. 22 with the corresponding failure planes H
i
(
s
) intersecting a
continuous obstacle-free portion of the goal self-motion manifold,
g
, shown
in units of degrees. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 70
3 Computation of self-motion manifolds . . . . . . . . . . . . . . . . . . . . . 72
4 Computation to check the necessary condition . . . . . . . . . . . . . . . . . 74
5 Computations when S is Found . . . . . . . . . . . . . . . . . . . . . . . . . 77
6 The set of solutions for the examples shown in Fig. 29 with their corre-
sponding feasible obstacle-free start conguration,
s
. The failure hyperplane
H
i
(
s
) intersects
g
which is parameterized in terms of v [0, 1]. . . . . . . 80
vii
LIST OF FIGURES
1 A simple redundant manipulator: 3-DOF planar robot. . . . . . . . . . . . . 21
2 The gure shows a start and a goal self-motion manifold in the C-space of a
planar 3-DOF robot. All the failure planes corresponding to an obstacle-free
start conguration,
s
, intersect a continuous obstacle-free portion of the
goal self-motion manifold,
g
. The failure cube contains
s
and
g
. The
failure surface corresponding to
s
, shown as a web-like network of paths, is
identied by connecting
s
to points on
g
via monotonic paths within the
failure cube. Each node along
g
denes an intersection with a face of the
failure cube. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28
3 The Mitsubhishi PA-10 seven degrees of freedom robot. . . . . . . . . . . . 36
4 The Robotics Research Corporation K-1207i seven degrees of freedom robot.
(With permission from Keith A. Kowalski, Robotics Research Corporation,
http://www.robotics-research.com.) . . . . . . . . . . . . . . . . . . . . . . 37
5 Pseudo code for computing self-motion manifold(s). . . . . . . . . . . . . . 39
6 Pseudo code for calculating congurations on a self-motion manifold. . . . . 40
7 Pseudo code to compute congurations on the self-motion manifold that are
of a predened distance from each other. . . . . . . . . . . . . . . . . . . . . 41
8 Pseudo code to converge to a conguration on the self-motion manifold. . . 43
9 Pseudo code to identify the obstacle-free congurations of the given self-
motion manifold. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 47
10 Pseudo code to check a conguration on the self-motion manifold for collision
against all the workspace obstacles. . . . . . . . . . . . . . . . . . . . . . . . 47
11 Pseudo code to check for the intersection of a failure hyperplane H
i
(
s
) with
a continuous obstacle-free portion of the goal self-motion manifold,
g
, for
i = 1, . . . , n. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49
12 Pseudo code to check for an intersection of
g
with a failure hyperplane
H
i
(
s
) that was not previously intersected. . . . . . . . . . . . . . . . . . . 53
13 Pseudo code to check for the existence of a failure surface. . . . . . . . . . . 55
14 Pseudo code for connecting linear paths from two input congurations. . . . 57
15 Pseudo code for connecting monotonic quadratic paths from two input con-
gurations. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 58
16 Pseudo code for searching the monotonic quadratic coecients using bisec-
tion method. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 60
17 A graphical representation of a set of feasible values of a and b coecients for
a monotonic quadratic polynomial where
2
>
1
and its derivative is greater
than or equal to zero. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62
viii
18 Motion of joint 2 of a 3-DOF planar robot moves the whole robot struc-
ture away from the most critical obstacle (colored black). The monotonic
quadratic or cubic parameterization of a single joint component, while the
rest of the joints has linear parameterization, will try to nd dierent com-
binations on the rate of motion for each robot joint that will allow such a
phenomenon to happen. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 64
19 A 3-DOF planar robot with three distinct sets of obstacles in the environ-
ment. For each scenario, the robot conguration shown is the feasible start
conguration
s
for which a failure surface S is found. Scenarios (a) and
(b) have x
s
at [200, 0]
T
and x
g
at [100, 0]
T
. Scenario (c) has x
s
at [250, 0]
T
and x
g
at [100, 0]
T
. The robot has equal link lengths of 100 units and each
obstacle has a diameter of 40 units. In all cases the robot can reach the
desired position x
g
from the shown start conguration
s
despite any single
joint failure. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 66
20 A projection of the self-motion manifolds, drawn in broken lines, for x
g
=
[100, 0]
T
(outermost oblong), x
s
= [200, 0]
T
(middle oblong), and x
s
=
[250, 0]
T
(innermost oblong). The start congurations,
s
s, that corresponds
to each scenario in Fig. 19 and their corresponding failure planes intersections
with the continuous obstacle-free portions of the goal self-motion manifolds,

g
s are shown. The surface bounded by solid lines represent the failure
surface, S, for each
s
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 67
21 The workspace of a 3-DOF planar manipulator used for the simulation ex-
periments (all three link lengths are 100 units long). The one thousand start
locations, x
s
, are randomly generated within the range [100, 200] units along
the x-axis (shown as a thick bold line). The one thousand goal workspace
locations, x
g
, (shown as dots) are randomly generated to be within the range
[0, 200] units away from the range of start locations (but inside the reachable
workspace). The center of the workspace is marked with a bold cross, the
workspace boundary with a solid line, and the boundary of the goal locations
with a dashed line. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 69
22 Several examples selected from the eleven thousand scenarios where the
failure-tolerant path planning algorithm was applied to a 3-DOF planar ma-
nipulator. The subgures denoted (a) through (e) correspond to workspaces
containing from twenty to twelve obstacles, respectively. In each case, the
top scenario represents one in which the algorithm successfully identied a
failure surface, S, whereas the bottom scenario represents a case where such
a surface could not be found. In all cases the start conguration shown for
the manipulator is one that satises the necessary condition for a surface to
exist. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 69
ix
23 Projections of the self-motion manifolds on the
2

3
-plane for the top row
scenarios of Fig. 22(a)-(c). The failure surface, S, is shown as a web-like
network of paths. The feasible
s
is shown as a cross on the self-motion
manifold while the
g
is the curve on the self-motion manifold between the
points labeled 0 and 1. For the example shown in Fig. 22(c), the goal
manifold consists of two disjoint manifolds in the C-space because x
g
is less
than one link length away from the robot base. . . . . . . . . . . . . . . . . 70
24 Percent of the self-motion manifolds that are obstacle free. . . . . . . . . . . 72
25 Time needed to determine the obstacle-free portion of the start self-motion
manifold M
s
and the goal self-motion manifold M
g
as a function of the
number of workspace obstacles. . . . . . . . . . . . . . . . . . . . . . . . . . 73
26 Average length of a
g
that satises the necessary condition and that also
satises the sucient condition as a function of the number of obstacles in
the workspace. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 75
27 Percent of total cases where the necessary condition is satised and percent
of total cases where the sucient condition is satised, i.e., where a failure
surface S is found. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 76
28 The subgures from (a) through (e) have the same start and goal workspace
locations where x
s
= [8.8, 188]
T
and x
g
= [42.5, 76.8]
T
. The shaded
regions represent the end-eector locations corresponding to dierent sets of
monotonic curves that will take the end-eector from x
s
to x
g
. . . . . . . . 78
29 The Mitsubishi PA-10 workspace with 10 randomly generated spherical ob-
stacles of diameter 0.254 m (10 in) for the corresponding x
s
and x
g
shown.
The corresponding solutions are shown in Table 6. The rst three vector
components are the desired positions in units of meters. The last three vec-
tor components are the desired orientations in units of degrees expressed in
terms of Euler angles using System I as discussed in [1]. . . . . . . . . . . . 79
30 The failure surface for example (a), shown as a web of paths in the congu-
ration space, with projections from joint axes 1 and 3, 2 and 7, and 5 and 6.
The projections are shown in the same scale with units of radians. The bold
curves represent portions of M
g
, while the less thick curves represent por-
tions of M
s
. The axes shown are translated from the origin to the feasible

s
. Its corresponding
g
, which is parameterized in terms of v [0, 1], is the
curve between the points labeled 0 and 1. . . . . . . . . . . . . . . . . . 81
31 Snapshots of the Mitsubishi PA-10 robot amongst obstacles while going
through a portion of
g
for example (c). . . . . . . . . . . . . . . . . . . . . 82
32 Two congurations of a planar 3R robot with equal link lengths of L me-
ters. The congurations shown are from an innite family of congura-
tions resulting in the end-eector position [L, 0]
T
. The rst conguration,
= [60

, 60

, 120

]
T
, is fault tolerant but the second conguration, =
[0

, 0

, 180

]
T
, is fault intolerant to a locked-joint failure in the third joint as
this will restrict the end eector to remain on the circle shown regardless of
the values of the remaining two healthy joints. . . . . . . . . . . . . . . . . 86
x
33 An example of a self-motion manifold on the verge of violating Condition 2
at an endpoint of A
i
= [a
i
, a
i
]. Arbitrarily near this self-motion manifold are
self-motion manifolds that rise above the hyperplane q
i
= a
i
, indicating that
the corresponding end-eector location may not be within the fault tolerant
workspace. Self-motion manifolds that dip below the same hyperplane can
also be found arbitrarily near the self-motion manifold shown in the gure.
Hence, the corresponding end-eector location may be a boundary point of
the fault tolerant workspace. Since the self-motion manifold is tangent to the
hyperplane, it follows that n
Ji
= 0 when the self-motion manifold touches
the hyperplane given by q
i
= a
i
. . . . . . . . . . . . . . . . . . . . . . . . . . 95
34 Another example of a self-motion manifold on the verge of violating Condi-
tion 2 at an endpoint of A
i
= [a
i
, a
i
]. The corresponding end-eector location
may be a boundary point of the fault tolerant workspace. In this case, there
will be similar self-motion manifolds arbitrarily close by that fail Condition 2
as they exit the conguration space C
B
R
n
below the hyperplane given by

i
= a
i
. On the other hand, there will also be other self-motion manifolds
arbitrarily close by that exit in such a way that Condition 2 is satised. . . 96
35 An example of a self-motion manifold on the verge of violating Condition
2 at an interior point of A
i
. The self-motion manifold is tangent to the
j-th physical joint limit constraint so that n
Jj
= 0. The corresponding end-
eector location may be a boundary point of the fault tolerant workspace
as there are nearby self-motion manifolds that appear within C
B
satisfying
Condition 2, but there are also self-motion manifolds arbitrarily close by
which come out of C
B
R
n
, violating Condition 2 in the interior of A
i
=
[a
i
, a
i
]. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 97
36 An example of a portion of the conguration space containing a self-motion
manifold that is co-regular. The intersecting lines represent a set of joint
values resulting in the same end-eector location. The conguration at the
intersection is a kinematic singularity. Such congurations are associated
with a fundamental change in the topology of the conguration space and
may indicate a boundary point corresponding to a violation of Condition 2
at an interior point of A
i
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 98
37 Shown in the gure are the workspaces corresponding to an optimal A
WF
of
a 3-DOF planar robot with equal link lengths. The optimal W
F
is shown
as the black region which can be derived by the intersection of W
1
, W
2
,
W
3
, and W
O
. It has an area of 3.56 sq. units for 1 unit link lengths with
symmetric articial joint limits of [18, 111, 111]
T
degrees. . . . . . . . . . . . 100
38 The gure shows the plots of A
WF
at the slices of the optimal articial joint
limits of [18, 111, 111]
T
degrees for the 3-DOF planar robot with equal link
lengths. The contour lines are at 0.313 sq. units apart. The white portion
shows the vicinity of the optimal A
WF
. . . . . . . . . . . . . . . . . . . . . 101
xi
39 The workspaces corresponding to the optimal A
WF
= 0.5939 m
2
for a kine-
matically designed 3-DOF planar robot are shown in the gure. The op-
timal W
F
is shown as the black region which can be derived by the in-
tersection of W
1
, W
2
, W
3
, and W
O
. The corresponding link lengths
are L = [0.462, 0.276, 0.462]
T
m
2
with symmetric articial joint limits of
[11, 128, 128] degrees. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 102
40 The gure shows slices at the optimal failure-tolerant workspace area for a
3-DOF planar robot that is kinematically designed to maximize A
WF
. The
contour lines are at 0.05 m
2
apart. The white portion shows the vicinity
of the optimal A
WF
. In the three subgures where the articial joint limits
are varied, the link lengths are held constant at the optimal value. For the
subgure where link lengths are varied, the articial joint limits are held
constant at the optimal values. . . . . . . . . . . . . . . . . . . . . . . . . . 102
41 Optimized A
WF
= 0.3380 m
2
for the PA-10 that is used as a 3-DOF planar
robot is shown. The link lengths are [0.45, 0.5, 0.45]
T
m
2
, the articial joint
limits are [32, 92, 92]
T
degrees, and the physical limits are at [90, 135, 160]
T
degrees. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 103
42 Slices at the optimal articial limits [32, 92, 92]
T
degrees of the PA-10 as a
3-DOF planar robot are shown. The ridge in the topology is due to the
switching of the potential boundaries B
T
s of W
2
and W
3
depending on
which one becomes a true boundary of W
F
. Contours are 0.04 m
2
apart. . 103
xii
LIST OF SYMBOLS
Note: The symbols are listed according to the order in which they appear in this work.
DOF degree-of-freedom
n number of DOFs of a robot
m number of DOFs of a robots workspace
DOR degree-of-redundancy
r DOR of a robot
x end-eector workspace position and/or orientation R
m
p end-eector workspace position vector R
3
in six-dimensional space
end-eector workspace orientation vector R
3
in six-dimensional space
r an arbitrary axis of rotation
an arbitrary rotation
e
i
an Euler parameter where i [0, 3]
e vector expressed as [e
1
, e
2
, e
3
]
T
q a robot conguration R
n
with i-th component q
i
R orthonormal rotation matrix R
33
T homogeneous transformation matrix of end-eector with respect to base
n, o, a orientation vectors R
3
i1
A
i
homogeneous transformation matrix between adjacent links R
44
x end-eector workspace velocity R
m
q joint velocity R
n
J(q) robot Jacobian R
mn
SVD Singular Value Decomposition
U unitary matrix R
mm
whose columns are the left singular vectors u
1
to u
m
diagonal matrix R
mn
whose diagonal elements are the singular values
1
to
k
where k = min(m, n)
V unitary matrix R
nn
whose columns are the right singular vectors v
1
to v
n
xiii
N
J
null space of the Jacobian, N(J), for r > 1 manipulators R
nr
when non-
singular with elements [ n
J(m+1)
, . . . , n
Jn
]
T
n
J
null space vector for r = 1 manipulators R
n
when non-singular with i-th
component n
Ji
G(q) redundancy optimization criterion
a specied constant
x
s
start workspace position and/or orientation
x
g
goal workspace position and/or orientation
M a self-motion manifold R
r
M
s
a start self-motion manifold corresponding to x
s
M
g
a goal self-motion manifold corresponding to x
g
n
m
number of self-motion manifolds

s
an obstacle-free start conguration with i-th component
si

g
a continuous obstacle-free portion of M
g
with i-th component
gi
H
i
(
s
) a failure hyperplane associated with
s

g
(v) segment of
g
that satised the necessary condition where v [0, 1]
H
0
j
(
s
) failure hyperplane associated with
s
that intersects
g
(0)
H
1
k
(
s
) failure hyperplane associated with
s
that intersects
g
(1)
V a failure hypercube
S a failure surface
S(u, v) parameterized S where u, v [0, 1]
a robot conguration with i-th component
i
(t) a curve lying on S(u, v) where t [0, 1]

0
a point on S(u, v)

1
a point on S(u, v) and lying on
g
(v)
z a vector expressed as [v, S
i
(u, v)]
T
w a vector expressed as [u, v]
T
G Jacobian derived from g/w where g(w) = z
xiv

i
maximum excursion of joint i in the self-motion manifold

min
worst-case global failure tolerance measure

ave
global failure tolerance measure averages over all joints

ratio
ratio of the least and highest failure tolerance measure

prod
product of all joints failure tolerance measure
x
d
desired x

(i)
a conguration at index i of the discretized M with j-th component
(i)
j
n
(i)
J
unit null vector at index of discretization i
n
mp
number of discretized congurations on M

o
original input conguration

p
previously computed conguration
n
p
J
unit null vector associated with
p
n
d
, o
d
, a
d
unit vectors associated with the desired orientation
p
d
desired position vector
n
a
, o
a
, a
a
unit vectors associated with the actual orientation
p
a
actual position vector
x
a
actual workspace position and/or orientation
x
p
dierence between desired and actual workspace positions
x
o
dierence between desired and actual workspace orientations
x dierence between desired and actual workspace positions and/or
orientations
incremental step angle
K
p
diagonal matrix with elements corresponding to the workspace
proportional gains
damping factor
p
(i)
l
workspace position of the tip of link i
x
(i)
workspace position and/or orientation of the tip of link i
p
(j)
obs
workspace position of the j-th obstacle
xv
d a specied distance
r
obs
radius of an obstacle
p number of obstacles
i
H
indices i of H
i
(
s
) intersecting
g
where i [1, n] with i-th component i
Hi
n
g
number of congurations on the discretized
g

g
[a, b] discretized
g
starting from discretization index a to b

(k)
g
a conguration on
g
at the k-th discretization index
k
H
stored values of k when
g
is intersected by a failure hyperplane with i-th
component k
Hi

min
minimum components of a bounding box with i-th component
mini

max
maximum components of a bounding box with i-th component
maxi
k
first
value of index k when a failure hyperplane intersects
g
[a, b] at a
n
fp
number of failure hyperplanes intersecting
g
[a, b]

Hj
value of
(k)
g
when
g
[a, b] is intersected by a failure hyperplane such that
the index j indicates the order of intersection

g
[0, 1]
g
[a, b] that satised the necessary condition and is parameterized in terms
of v [0, 1]
i
stop
number of failure hyperplane intersections encountered along
g
[0, 1] as
parameter v goes from 1 to some value just before the failure surface
identication exits

g
a conguration on
g
with i-th component
gi
i
H1
index j of H
0
j
(
s
)
i
Hn
index k of H
1
k
(
s
)
n
g[0,1]
the number parameterized points on
g
[0, 1]
i
quad
index of the joint component to be parameterized quadratically
a
k
quadratic coecient at k-th parameterization of
g
[0, 1] with i-th component
a
ki
b
k
linear coecient at k-th parameterization of
g
[0, 1] with i-th component b
ki
c
k
constant coecient at k-th parameterization of
g
[0, 1]
xvi
n
deg
degree of resolution of the bisection search
n
c
a specied counter
C
B
conguration space dened by the robots physical joint limits
B
i
range of physical limits [b
i
,

b
i
]
b
i
lower physical limit

b
i
upper physical limit
C
A
conguration space dened by the robots articial joint limits
A
i
range of articial limits [a
i
, a
i
]
a
i
lower articial limit
a
i
upper articial limit
F failure index set {1, 2, . . . , n}
W
0
pre-failure workspace f (C
A
)
i
C reduced conguration space as a result of a locked i-th joint
W
i
post-failure workspace
W
F
failure-tolerant workspace
P
i
[S] projection of a set S R onto the i-th axis
A
W
F
failure-tolerant workspace area
xvii
CHAPTER I
INTRODUCTION
Robots are one of the most complex devices man has ever made. Their complexity
comes as a result of a design geared toward a purpose to assist, and in some cases replace,
man in his tasks. As technologies advanced, robots are endowed with more and more
capabilities that enables them to perform tasks which are too dicult or almost impossible
for man to do himself. Robots give man access to distant planets, to unexplored depths
of the ocean, to a physical journey inside man himself, and to dangerous environments
of volcanic cauldrons and nuclear reactor chambers. Robots alleviate man of repetitive
labor and of the burden associated with tasks that require high mechanical precision. Such
usefulness make robots virtually indispensable to man, and more is expected from these
subservient intelligent machines to serve mans insatiable needs.
However the more complex a system is, the more likely it is to fail. Such is a universally
accepted logic of failure. And such is what robots have to face. Indeed robot failures are
not uncommon and there have been several accidents concerning robots that cost human
lives. It is therefore helpful if robots would have the intelligence to know what to do when
failure occurs instead of going uncontrolled and causing damage to lives and properties.
Advanced failure detection and instantaneous application of fail-safe brakes when a failure
is detected are some measures that address robot safety. However in some cases, robots
may be required to complete a desired task despite the occurrence of robot failures. These
include tasks that are performed in remote environments where robot failures can cause
costly unnecessary delays. And tasks that are performed in hazardous environments where
a robot failure can also pose a signicant danger.
Kinematic failure tolerance is a recent area of study which addresses ways of optimizing
current robot congurations of kinematically redundant manipulators in anticipation of joint
failures. A kinematically redundant manipulator is a robotic arm whose number of joints
1
is more than the dimension of its workspace. A robots workspace is the world space that
the given robot operates. It has coordinates of position and/or orientation. In full space its
dimension is six: three for position and three for orientation. On the other hand, a robots
joint space or conguration space (C-space) is where a robots posture or conguration is
expressed. It has coordinates in terms of each of the robots joints and has a dimension
equal to the number of joints. Thus the C-space dimension of a kinematically redundant
manipulator is higher the dimension of its workspace.
In this work, kinematic failure tolerance is extended to cases where obstacles are present
in the workspace. The problem being addressed in this work is stated as follows.
1.1 Problem Statements
Two kinematic failure tolerance problems are being address in this work:
Given a start and a goal position and/or orientation in a redundant robots workspace,
determine a set of paths that would guarantee reachability of the goal, despite any
single locked-joint failure that may occur at anytime and despite the presence of
obstacles in the workspace.
Given a redundant manipulator, identify a region in its workspace such that the
workspace locations within this region is guaranteed reachable before and after a joint
failure.
1.2 Summary of Previous Work
Robots, in general, include all intelligent machines that are capable of doing work. The
word robot originated from the Czech word robota, meaning work and was introduced
into the English language by playwright Karel Capek in his 1921 satirical drama, R.U.R.
(Rossums Universal Robots). The Robot Institute of America gave a more precise denition
of an industrial robot and is quoted here: A robot is a reprogrammable multi-functional
manipulator designed to move materials, parts, tools, or specialized devices, through vari-
able programmed motions for the performance of a variety of tasks.
2
1.2.1 Robot Safety and Reliability
Robot safety and reliability is the wider area of study that encompass issues concerning
robot failures. The assessment of robot safety and reliability are discussed by Tosunoglu
and Monteverde [2], and Leuschen et. al. [3]. Discussions on their analysis are presented by
McCulloch [4], Schneider et. al. [5], Dhillon [6], Khodabandehloo [7], Dhillon and Fashandi
[8], and Carreras and Walker [9]. Robot designs primarily for safety and reliability include
works by Ng and Tan [10], and Trevelyan [11].
1.2.1.1 Assessment
Tosunoglu and Monterverde [2] reviewed failure-tolerant design issues from a kinematic
and structural viewpoint based on four levels: joint level (single and dual actuators), link
level (serial and parallel modules), sub-system level (non-redundant and redundant manip-
ulators), and system level (multiple cooperating manipulators). A mathematical measure
for failure tolerance capacity of robotic manipulators was developed and demonstrated as a
tool for design selection of reliable robots. The authors claimed that a hybrid manipulator
system that contained both serial and parallel structures or a redundant serial arm with
strategically placed dual actuators are the most promising and practical designs for robot
failure tolerance.
Reliability of hydraulic robots for hazardous environments was investigated by Leuschen
et. al. [3] using analytic redundancy. Under consideration was a mobile worksystem which
consisted of a wheeled platform containing a hydraulic power supply powered by an electric
tether, four independently steerable wheels, and a heavy-duty crane/manipulator. It was
designed for nuclear reactor decontamination and dismantlement. The hydraulic wheel
actuator subsystem was determined to be a vital component of the mobile platform through
reliability analysis. The authors claimed that through analytic redundancy, they were able
to exploit information of sensor values and system model used to derive test of consistency
of sensor data which were later used in monitoring and diagnostics of the robotic system.
3
1.2.1.2 Analysis
Safety requirements for robotic systems in Department of Energy nuclear facilities
were analyzed by McCulloch [4]. Robotics systems that handle nuclear materials were
assessed based on their design and operation by considering the potential for both personnel
injury and payload damage resulting from robot operations, failures, and accidents. The
author stated that the level of analysis and documentation is to be commensurate with
the magnitude of the hazards involved and the complexity of the system and its safety
features. The general requirement for the nuclear safety analysis was presented and specic
approaches for applying those requirements to the systems under present considerations
were described.
Schneider et. al. [5] have developed and tested a reliability performance index for mod-
ular robotic systems. Using a probabilistic representation of manipulator kinematics and
a reliability block diagram model of the manipulator system, the reliability performance
index representing the probability that no hardware or software failure occurred and the
manipulator achieved the specied end-eector position and/or orientation was developed.
The authors noted that this performance index showed promise in reducing the design space
for the selection of modular congurations but needed to be incorporated into an on-line
computer-aided design system.
Dhillon [6] wrote a whole book discussing robot safety and reliability. This book was
the result of ltering through almost all literature, which include reports on many advances
and other relevant results, on robot safety and reliability. The author claimed that there
are no other published book which covered current developments in this area. Some topics
discussed in the book include robot standardization and safety standards, robot accident
analysis and prevention, robot safeguard methods, reliability analysis of a robot system
with human error, and robot reliability measures.
A case study on the analysis of robot systems using fault and event trees was presented by
Khodabandehloo [7]. The paper demonstrated an approach of using established techniques
of failure mode and eect analysis, fault tree analysis, and event tree analysis in the eld
of robotics. The author stated that this has increased understanding of the safety and
4
reliability issues while highlighting the design improvements that may be achieved through
innovation facilitated by the systematic applications of the analysis techniques. It was
claimed that the analysis provided some very useful information by giving a framework
within which knowledge of equipment design can be claried and decisions can be made on
appropriate safety measures.
Safety and reliability techniques in robotics were discussed by Dhillon and Fashandi [8].
The authors highlighted two techniques for robot safety analysis: fault tree analysis and
failure mode and eects analysis. The most suitable analytical methods in performing
reliability analysis of robotic systems were enumerated by the authors as follows: failure
mode and eects analysis, fault tree analysis, block diagram, combinational models, Markov
and Non-Markov models, and Monte Carlo simulation techniques.
Carreras and Walker [9] described a technique based on interval methods for estimating
reliability using fault trees. The approach encoded inherent uncertainty in the input data
by modeling these data in terms of intervals. The authors stated that the interval method
appreciably improves the accuracy of reliability estimates over existing approaches to the
problem of uncertain input data and avoided the problem of loss of uncertainty inherent in
some approaches when applied to noncoherent systems.
1.2.1.3 Robot Designs
Safety enhancements for medical robots were discussed by Ng and Tan [10]. The authors
discussed measure to increase the level of safety in the event of a failure, including ways of
failure prevention and tolerance at the software level, and has pointed out the diculties in
assuring a safe system using software failure prevention. Hardware measures were reckoned
to be the most direct and reliable route to achieve a safer system than that obtainable
by software monitoring. It was stated that supplementing hardware design with software
design for safety was considered to be the ideal solution.
Trevelyan [11] advocated simplifying robotics to make them more reliable in order to
reduce performance uncertainties and hence costs that would pave the way to building
complex systems with less eort and more certainty. The author noted that real advances
5
can only be made with systems built from reliable component which work time after time.
By addressing issues of simplicity, reliability, and robustness in developing robot components
the author has stated that some insights were gained into some problems which are thought
to be major impediments to the wider applications of robots.
1.2.2 Tasks Requiring Tolerance to Failure by Robots
Tolerance to failures enhances safety and reliability for robots carrying out tasks in
remote or hazardous environments. Space explorations are one example of such tasks as
discussed by Wu et. al. [12], and by Visentin and Didot [13]. Other examples are underwater
explorations as presented by Babcock and Zinchuk [14] and nuclear waste disposal as dis-
cussed by Colbaugh and Jamshidi [15], McCulloch [4], and Leuschen et. al. [3]. Kinematic
failure tolerance allows robots to immediately complete the task at hand without unneces-
sary delays due to robot repair and also avoid the potentially signicant danger associated
with a robot failure during task execution amongst hazardous materials. It guarantees com-
pletion of time critical tasks where a robot cannot aord to stop in the process of doing
these tasks.
1.2.2.1 Space Explorations
Wu et. al. [12] presented an analysis and experiment of failure-tolerant joints for a space
shuttle remote manipulator system. It investigated the feasibility of space-based failure-
tolerant robot joint design with a dual-motor, single-output dierential-based mechanical
drive system. Simulations were performed for various joint failure conditions and a scaled-
down dierential test bed was subsequently designed and built to validate the analytical
results.
Visentin and Didot [13] discussed a test for space robotics on the Japanese Engineer-
ing Test Satellite VII which was designed to test key rendezvous, docking, and robotics
technologies in space equipped with a robotic manipulator and a small sub-satellite. Two
experiments were performed in space: interactive autonomy robotic control mode (broken
6
down into autonomous robot manipulations and robot-user interactions) and computer-
vision-assisted robotic control. Final evaluations of the data were still in progress at the
time of the authors writing.
1.2.2.2 Underwater Explorations
Babcock and Zinchuk [14] discussed failure-tolerant design optimization and its appli-
cation to underwater vehicle navigation. The authors presented sensitivity analysis that
is used to provide systematic guidance in selecting the design modications used in the
optimization process, optimization process stopping criteria, and robustness measures for
the resulting design.
1.2.2.3 Nuclear Waste Disposal
Robot manipulator control for hazardous waste-handling applications were discussed
by Colbaugh and Jamshidi [15]. A proposed controller was presented that was claimed
to have a capability of accurate end-eector impedance control and eective redundancy
utilization, did not require knowledge of the complex robot dynamic model or parameter
values for the robot or the environment, and was implemented without calculation of the
robot inverse kinematic transformation. An example was given using a computer simulation
for a four-joint redundant robot under adaptive impedance control.
1.2.3 Failure Detection and Identication
A crucial component to robots tolerance to failure is its ability to detect and identify
the occurrence of failures. This has been the focus of several studies in the past decade
which included failure detection and identication by Visinsky et. al. [16], and Schneider
and Frank [17].
The work of Visinsky et. al. [16] presented a method of detecting robot failures by
generating dynamic, model-based threshold that proved capable of dynamically responding
to changes in the robot system so as to maintain a threshold that dierentiates between real
failures in the system versus false alarms due to modeling errors. The authors claimed that
7
their proposed model-based threshold are implementable in real-time, less complex, and
produces adequate, if not completely optimal, thresholds. These advantages were combined
with the fact that their proposed method focused only on local history, thus, the decrease in
complexity enabled the authors to generate faster algorithm for producing failure-tolerant
threshold.
A fuzzy logic-based threshold adaptation for detecting failures in robots was investi-
gated by Scheider and Frank [17]. The authors noted that analytical methods can only
be used for structured disturbances, thus, there was a need to use heuristic knowledge for
residual evaluation of unstructured disturbances. Using heuristic knowledge, an adaptive
fuzzy logic-based threshold has been presented where the degree of adaptation depended on
characteristic process conditions extracted from observation and experience. The authors
claimed that using the heuristic approach, a performance improvement for failure detection
for processes with unstructured disturbances can be achieved.
1.2.4 Failure Tolerance through Layered Control Architecture
Some studies are geared towards the derivation a holistic approach to toleration of
robot failures through layered control architecture. These are discussed by Ting et. al. [18],
Tso et. al. [19], and Visinsky et. al. [20].
Ting et. al. [18] provided guidelines for the design of control algorithms towards the
full development of failure-tolerant robotic manipulator systems. They investigated internal
shock phenomena due to the failure of joint actuation, and a recovery algorithm for both
serial and parallel mechanism. A control algorithm was studied that consisted of a model
reference algorithm and computed torque method in the feed-forward process, and a sim-
ple proportional, integral, and derivative controller in the feedback process. The recovery
algorithm was shown not only to cases where some joints have fully failed but also to cases
where some joints experienced partial failure.
Tso et. al. [19] described a strategy that integrated system level hardware/software
failure tolerance with task level handling of uncertainties and unexpected events in robotic
8
control. A distributed recovery block was used for system level failure tolerance that pro-
tected against application software, system software, hardware, and network failures. Fail-
ure tolerance provisions were implemented using rule-based and model-based reasoning to
monitor, diagnose, and recover from unexpected events.
Another work by Visinsky et. al. [20] discussed a layered control framework which was
divided into servo, interface, and supervisor layers. The servo layer was the continuous
robot system and normal controller. The interface layer monitored the servo layer for
failures which provided some sensor failure tolerance without direction from the supervisor
layer. In the event that the interface layer runs out of alternatives, the supervisor layer
would search for the remaining failure tolerance options and initiate appropriate action.
These layers provided dierent levels of failure detection and tolerance capabilities.
1.2.5 Kinematic Failure Tolerance
Kinematic failure tolerance assumes that a given kinematically redundant robot has
the ability to identify joint failures and is focused towards planning the robot motions in
anticipation of these failure. In the event of joint failures, this preplanning strategy enables
the robot the ability to gracefully accommodate such failures.
Pioneering work on kinematic failure tolerance by Maciejewski [21] analyzed the degra-
dation of dexterity due to a joint failure. The optimal failure-tolerant conguration was
dened as a conguration where each joint of the manipulator contributed equally to the
end-eector motion, and equivalently, where each joint of the manipulator contributed
equally to the null space of the Jacobian transformation. (A robot conguration is the
robot posture derived from the actual values of the joint angles.) The minimum singular
value, derived from the singular value decomposition (SVD), of the failed manipulator Ja-
cobian matrix was used as a worst-case measure of a robots tolerance to a joint failure.
(A manipulator Jacobian matrix is a matrix of rst-order partial derivatives of the position
and/or orientation vector with respect to the joint angles vector.)
9
1.2.6 Local Kinematic Failure Tolerance
The nature of joint failures that have been studied in kinematic failure tolerance include
locked-joint failures by Roberts and Maciejewski [22], Lewis and Maciejewski [23], and
Groom et. al. [24]. The other type of joint failure that has been considered is the free-
swinging joint failures which was rst investigated by English and Maciejewski [25]. A
real-time implementation of kinematic failure tolerance was demonstrated by Groom et.
al. [24].
Roberts and Maciejewski [22] discussed a local measure of failure tolerance for kinemat-
ically redundant manipulators. A given conguration was dened to be a failure intolerant
conguration with respect to a particular joint if the reduced Jacobian matrix due to the
failure of this joint is singular. Two local measures of failure tolerance based on manip-
ulability index as a measure of dexterity: the reduced manipulability index dened as a
measure of manipulability following a locked-joint failure, and the relative manipulability
index dened as a measure of the relative loss of manipulability due to a joint failure. A
method was developed that used these measures to determine congurations that are locally
optimal. The proposed method was applied to single and multiple joint failures.
In [24], Groom et. al. showed real-time failure-tolerant control for single locked-joint
failures. Their work showed that by doing power method approximation to compute for
estimates of the worst-case failure-tolerant measure and its gradient, the robot performance
is indistinguishable from that of brute-force implementation with the advantage that the
former can be implemented in real-time. The manipulator chosen for the real-time im-
plementation was a commercially available spatial redundant manipulator, the Robotics
Research Corporation K-1207i.
The issue of tolerating a free-swinging joint failure was rst investigated by English and
Maciejewski [25] whose focus was on how to best congure a slow-moving manipulator before
a failure. Three cost functions were dened which quantitatively reected the susceptibility
of a manipulator to a joint failure: the torque-based function measured failure likelihood and
force-domain eects, the acceleration-based function measured immediate failure dynamics,
and the swing-angle-based function measured susceptibility to secondary damage after a
10
failure. Failure tolerance was achieved by minimizing these functions.
1.2.7 Identifying Kinematic Failure-Tolerant Workspace
A local kinematic failure tolerance measure was formally dened by Lewis and Ma-
ciejewski [26] as the minimum over all the minimum singular values of the failed manipulator
Jacobian matrix. Later, the same authors pioneered the determination of a failure-tolerant
workspace in [23]. Considerable details about their work will be discussed here since a
similar approach was used in this work as part to the solutions discussed.
The authors derived a method for determining necessary constraints that ensure failure
tolerance with respect to a given critical task. This method was based on bounding boxes
enclosing the self-motion manifolds whose intersection denes articial joint limits that can
guarantee reachable task points after a joint failure. (A self-motion manifold is dened as a
set of congurations in the manipulators conguration space that expresses the same ma-
nipulator end-eector position and/or orientation in the workspace.) Their work addressed
a more global issue that could guarantee that a complete desired trajectory would remain
feasible after a joint failure. The paper discussed techniques for determining constraints
on each joints motion which would guarantee the failure tolerance of the entire task. The
determined constraints are upper and lower bounds on the range of motion for each joint
and thus would only aect the motion of the manipulator when it is operating near these
software joint limits. Redundancy is still utilized to maximize the local dexterity after a
joint failure by maximizing the local kinematic failure tolerance measure. It was stated that
for a three-joint planar robot with equal link lengths, exactly one link length away from
the base is the single-joint failure-tolerant workspace for this manipulator. A critical task
that lies solely on this circle can be completed regardless of any single-joint failure. It was
further stated that in order to guarantee a manipulator of returning to a critical workspace
position and/or orientation, the motion range for each of the joints must be constrained
to lie within the range spanned by the self-motion manifold associated with that position
and/or orientation. The minimum and maximum joint values of a particular joint super-
scribed an n-dimensional bounding box aligned with the joint axes around the self-motion
11
manifold, where n is the number of joints of the manipulator. These joint restrictions were
simple software joint limits and can be removed after a joint failure has occurred. Thus
the problem of maintaining failure tolerance of a given critical position and/or orientation
was reduced to that of maintaining joint limits specied by the bounding box of the self-
motion manifold for that position and/or orientation. The paper introduced the following
procedure to guarantee failure tolerance of a redundant manipulator with respect to critical
tasks.
1. The workspace is analyzed to nd regions having large self-motion manifolds.
2. Critical tasks are placed in these regions of the workspace.
3. The bounding boxes for the self-motion surfaces associated with each critical position
and/or orientation are determined.
4. The intersection of the bounding boxes is calculated to determine the required con-
straints.
5. Each critical workspace point is checked to determine if the manipulator is capable of
positioning and/or orienting its end-eector at the desired position and/or orientation
while maintaining the constraints imposed by the intersection of all bounding boxes.
6. Null space control through kinematic failure tolerance measure optimization together
with combination of joint limit avoidance are used to ensure that all specied task
points are reachable and that there is a maximum dexterity in the vicinity of the
failure.
The authors also developed a technique for estimating the bounds of a two-dimensional
self-motion manifold surface which provides an estimate of the bounding box containing
the self-motion manifold surface. A simple and eective method for estimating its bounds
was presented by iteratively tracing out a linearly increasing spiral on the self-motion man-
ifold surface. An example was given using a PUMA 560 manipulator performing a three-
dimensional Cartesian positioning task. The manipulator had nominally two degrees of
12
redundancy since the sixth joint does not change the Cartesian position which is true if the
tool point is collinear with the sixth joint axis.
1.2.8 Kinematic Failure-Tolerant Trajectory Planning
Although the presence of obstacles in the environment greatly aects a kinematic failure
tolerance algorithm, this issue has not been given much attention in the past. The work of
Ralph and Pai [27] considered failure tolerance by trajectory planning but did not consider
obstacles in the workspace. The authors introduced a method of planning failure-tolerant
trajectories based on what they called a least constraint framework. A performance measure
they called longevity quantied a robots recovery ability.
A previous work that investigated a very similar problem that this work is trying to
address is the work of Paredis and Khosla [28]. Their work presented a trajectory plan-
ning algorithm that guaranteed failure tolerance while satisfying joint limits and obstacles
requirements. Their work will also be discussed in more detail here.
The algorithm that the authors presented consisted of two parts: determining acceptable
postures for failure-tolerant trajectories, and constructing a connectivity graph representing
the sequence of acceptable postures that determined a failure-tolerant path. It was assumed
that the manipulator has failure detection and identication capabilities that monitor the
proper functioning of each joint of the kinematically redundant manipulator. As soon as a
failure is identied, the redundant manipulator would apply the corresponding brakes and
would adapt the joint trajectory of the new manipulator. Task would be continued with-
out interruption. The basic idea was to achieve failure tolerance by avoiding unfavorable
joint congurations. The problem was formally dened as: given a manipulator dened
by its geometry, joint limits, and redundancy resolution algorithm, together with the task
description in Cartesian path and geometry of obstacles, nd a failure-tolerant trajectory
in joint space. The redundancy resolution algorithm was dened as the joint velocities that
are needed to drive the joints of a redundant manipulator in order to assume a posture as
dened by a desired gradient. A manipulator conguration was considered tolerant to a
single joint failure if for every possible joint failure and for every discretized point along
13
the desired Cartesian trajectory, there existed an alternate trajectory that maps into the
desired trajectory and does not violate joint limits and obstacles requirements. It was noted
that alternate trajectories were innite and explicit storage of all alternate trajectories was
impossible. Further assumptions were enumerated as follows. Alternate trajectories were
stored implicitly in a redundancy resolution algorithm that computed an alternative trajec-
tory at run time once a failure has taken place. It was stated that the redundancy resolution
algorithm unambiguously determined the alternative trajectory. Only redundancy resolu-
tion algorithms that determine the next joint vector based on the current vector, and not
on past joint vectors are considered. The redundancy resolution algorithm was a given in
the problem, i.e., part of the manipulator denition. For a given joint failure, there ex-
isted a unique alternative trajectory. The choice of redundancy resolution algorithm fully
determined alternate trajectories and it also inuenced the solution of the trajectory plan-
ning problem. The primary requirement was to follow the Cartesian path, with joint limits
and obstacles as secondary requirements which can include any other kinematic require-
ments that only depended on the current position. The following denitions were given.
A preimage was dened as a function of a Cartesian point that was composed of all an-
gles in the conguration space that resulted in the same Cartesian point by solving the
forward kinematics equation. The preimage consisted of manifolds of dimension equal to
the degree redundancy of the manipulator. A degree of redundancy of a manipulator is
the number of its joints minus the dimension of the workspace. A k-reduced derivative was
dened as the manipulator that resulted when k number of the joints were immobilized.
It was noted that the resulting robot with failed joint(s) can complete the task depending
on the posture at the instance of failure. The resulting manipulator could not complete
the task when the required path passed outside the workspace of the resulting k-reduced
derivative manipulator, or when redundancy resolution algorithm get stuck at a singularity,
or when the alternate trajectory violated joint limits or cause collision with obstacles. A
given posture was considered to be failure tolerant to a joints failure if all the postures
along the corresponding alternate trajectory are also tolerant to a failure of the same joint.
The posture that is tolerant to a failure was a subset of the preimage of the Cartesian point
14
at the instance of failure. A given posture was considered to be an acceptable posture for
a failure-tolerant trajectory if is it tolerant to failures of each of the joint. Their proposed
algorithm consisted of two parts.
1. Determine a set of acceptable postures at discretized point along the Cartesian path.
2. Create a connectivity graph for the acceptable postures and search this graph to
determine a failure-tolerant trajectory.
Details of Part 1 of their overall algorithm were enumerated as the following.
1. Discretize the given path for k = 1, . . . , last.
2. For k = last:
(a) Compute the preimage of the last point.
(b) Compute the acceptable posture for the last point. This is composed of all
the components of the preimage which are checked against the joint limits and
obstacles requirements.
3. Iterate from k = (last 1) to 1:
(a) Compute the preimage at k.
(b) For j = 1 to n: (for each joint of the given manipulator)
i. For every posture in the preimage do: (one joint is locked)
A. Compute alternative trajectory (posture) using a redundancy resolution
algorithm.
B. If the computed posture meets secondary requirements and that the
alternative trajectory (posture) is an element of the k+1 failure-tolerant
posture, then the computed posture is a failure-tolerant posture at k for
this particular joint failure.
ii. Compute the set of acceptable postures (posture that are have found alterna-
tive trajectories (postures) that are elements of the failure-tolerant postures
in k + 1 and have found alternative trajectories (postures) for all js).
15
Acceptable postures were stored. A failure-tolerant trajectory which was a sequence of
acceptable postures was determined, one acceptable posture for each instant along the
trajectory. It was stated that there should exist a continuous trajectory of acceptable
postures connecting each adjacent posture along the trajectory, moreover, the sequence
should preferably vary smoothly and stay away from the boundaries of the acceptable
postures. The set of acceptable postures can be consisted of several disjoint regions of
acceptable postures. Two consecutive regions (in terms of k) were connected if there existed
a continuous trajectory of acceptable postures between these regions. The connectivity
graph representing connections between regions was very simple due to the limited number
of disjoint regions in each group of acceptable postures and due to the limited number of
connections between regions at k and k + 1. It was possible that there existed no failure-
tolerant sequence of connected regions. Part 2 of the overall algorithm are discussed in the
following:
1. For k = last to 1 do:
(a) Group acceptable postures in disjoint regions.
(b) For each region in k, determine the connections with the regions for k + 1.
(c) store connectivity graph
2. Search for connectivity graph for failure-tolerant sequence in the regions.
3. Select a failure tolerant trajectory.
The authors commented that the computational complexity of their algorithm was mainly
determined by the nested loop of the Part 1 of the overall algorithm which was stated as
Pe
r
O(n
2
). (1)
The symbol P represented the number of points used to discretize the required Cartesian
path (equal to k), e represented a function of the number of postures use to approximate
the preimage at k, and r was the degree of redundancy of the given manipulator. An
example was given using a three-link planar manipulator with unit length each. The desired
16
workspace trajectory resembled a sliced pie with corners are at approximately (1.8, 0.5),
(1.8, 0.5), (1.4, 0.4), (1.4, 0.4). A single workspace obstacle was centered at (0.7,0) and has
a radius of 0.2 units. Another example was given using a four-joint spatial manipulator.
The task was to follow a circular path on a table while avoiding collisions with the table
even after one of the joints has failed and was immobilized. There were no other obstacles
in the workspace.
The method proposed in this work is similar to [28] in that it allows a manipulator to
avoid obstacles in the presence of joint failures, however, it diers in that the task denition
is specied as a desired start and goal location in the workspace, as is typical of pick-and-
place tasks. This is signicant because constraining the end-eector to a specic path will
greatly reduce the likelihood that an obstacle-free, failure-tolerant trajectory exists.
The method being proposed focused on searching for a set of solutions in the C-space
that contains an obstacle-free start conguration and a connected set of obstacle-free goal
congurations. It will be shown that this set of solutions is composed of a simply-connected,
obstacle-free surface with no internal local minimum or maximum in the C-space that
contains a start conguration and a connected set of goal congurations. The existence of
this surface guarantees that the given robot can successfully reach a goal workspace position
and/or orientation from an obstacle-free start conguration despite any single locked-joint
failure at any time and despite the presence of workspace obstacles. Examples will be shown
in applying the proposed method to a three-joint planar robot and a seven-joint fully spatial
robot. For the three-joint planar robot, more than 11,000 experiments were performed with
randomly specied start, goal, and workspace locations. The number of obstacles ranges
from zero to twenty obstacles. For the seven-joint fully spatial robot, ten obstacles were
randomly placed in the workspace in three dierent sets of start and goal locations and
obstacles positions.
1.3 Organization of this Work
This work is organized as follows. Chapter 2 discusses the background on the kinematics
of redundant manipulators. The conditions that can guarantee the existence of a solution to
17
failure-tolerant path planning is presented in Chapter 3. This chapter also describes a global
failure tolerance measure. Chapter 4 presents the algorithm of the failure-tolerant path
planning method discussed in this work. It also discusses a pseudo code implementation.
Simulation results are presented in Chapter 5 which were performed on a three degrees of
freedom planar robot and a seven degrees of freedom Mitsubishi PA-10 robot. This included
11,000 experiments on a three degree of freedom planar robot that tested the feasibility
of implementing the proposed algorithm presented in this work. The identication of the
boundaries of a redundant robots failure-tolerant workspace is discussed in Chapter 6. And
lastly, Chapter 7 gives the summary of the method presented and the conclusion derived
from this work.
18
CHAPTER II
KINEMATICS OF REDUNDANT MANIPULATORS
Kinematics background for redundant manipulators will be discussed in this chapter.
Important terms that are used in this work will also be dened for reference and for clarity.
In this work, the word robot would mean a mechanical manipulator arm consisting of
actuated joints and is controlled by a computer. It also be referred to as a manipulator. A
degree of freedom (DOF) is dened as the number of independent axes that a given robot
is free to move. The DOFs of a robot is equal to the number of independently actuated
joints which could either be revolute or prismatic.
2.1 The Robot Workspace and Conguration Space
A workspace is the world space where a given robot operates to perform its required
task through the execution of a desired motion and/or the application of a specied end-
eector force. A robots end-eector is the general term for a gripper or tool attached to the
end of the robots last link. Typically, a robots workspace uses Cartesian space coordinate
axes and its dimension is also referred to as the DOFs of a robots workspace. On the other
hand, a conguration space (C-space) is the robot space whose coordinate axes are the
robot joints such that a robot posture or conguration can be expressed as a single point.
The C-space dimension is equal to the DOFs of the given robot.
Let n denote the DOFs of a robot, and m denote the DOFs of a robots workspace. A
workspace position and/or orientation, denoted x R
m
, can be dened as
x
_

_
p

_ (2)
where p denotes a workspace position vector and denotes a workspace orientation vector.
19
In six-dimensional space, the position vector components are dened as
p
_

_
p
x
p
y
p
z
_

_
(3)
where p
x
, p
y
, and p
z
represent the components of position in the x, y, and z axes, respec-
tively. In expressing orientations in three-dimensional space, several methods are used and
some of them results in ambiguous orientations. This happens when a single orientation can
be represented by several dierent sets of rotational angles, allowing a phenomenon known
as gimbal lock. A gimbal lock is caused by the alignment of two of the three axes together
so that one of the rotation references is cancelled. An unambiguous method of expressing
orientation is the use of a rotation matrix in R
33
where four parameters known as Euler
parameters, also known as quaternions, are used to derive it. The four Euler parameters,
e
0
, e
1
, e
2
, and e
3
that describe a rotation about an arbitrary axis r are expressed as,
e
0
= cos
_

2
_
(4)
e =
_

_
e
1
e
2
e
3
_

_
=r sin
_

2
_
. (5)
The elements of a rotation matrix becomes
R =
_

_
e
2
0
+ e
2
1
e
2
2
e
2
3
2(e
1
e
2
+ e
0
e
3
) 2(e
1
e
3
e
0
e
2
)
2(e
1
e
2
e
0
e
3
) e
2
0
e
2
1
+ e
2
2
e
2
3
2(e
1
e
3
+ e
0
e
1
)
2(e
1
e
3
+ e
0
e
2
) 2(e
2
e
3
e
0
e
1
) e
2
0
e
2
1
e
2
2
+ e
2
3
_

_
. (6)
A robot conguration in the C-space, denoted q R
n
, is dened as
q
_

_
q
1
q
2
.
.
.
q
n
_

_
(7)
20
Figure 1: A simple redundant manipulator: 3-DOF planar robot.
where q
1
, q
2
, . . ., q
n
represent displacement of the joints. For a rotational or revolute joint,
the corresponding q
i
is replaced by
i
angular displacement. For a prismatic joint, it is
replaced by d
i
linear displacement.
A forward kinematics equation expresses the relationship between the workspace posi-
tion and/or orientation and the C-space conguration which is dened as
x f(q). (8)
A robot or manipulator is said to be redundant when there are more joints in the robot
than the dimension of its workspace, that is, when n > m. A simple example of a redundant
manipulator is a 3-DOF planar robot as shown in Fig. 1.
A redundant robot has a degree of redundancy (DOR), denoted by r > 0, which is
the dierence between the number of joints of the robot and the dimension of the robots
workspace, i.e.,
r = n m. (9)
2.2 The D-H Convention
To express the relative motion of the robot links with respect to one another, each
robot joint is attached with a coordinate frame. A homogeneous transformation matrix,
denoted T, transforms the end-eector frame with respect to the base frame and is dened
21
as,
T
_

_
R p
0
T
1
_

_ =
_

_
n o a p
0 0 0 1
_

_ (10)
where R is an orthonormal rotation matrix composed of n, o, and a unit rotation vectors,
and p is a position vector.
In 1955, Denavit and Hartenberg (D-H) [?] developed a systematic method of establish-
ing a coordinate system attached to each link of an articulated chain. This introduces a
set of parameters for describing the kinematics of a robot. For each robot link i, four D-H
parameters are dened: link length, a
i
; joint length, d
i
; twist angle,
i
; and joint angle,
i
.
The index i refers to a joint or link index where, normally, i = 0, . . . , n and link 0 refers
to the base of the robot. Using these parameters, a homogeneous transformation matrix,
i1
A
i
R
44
, describing the relationship between two adjacent links of a kinematic chain
is represented as a product of four basic transformations
i1
A
i
= Translate
z,d
i
Rotate
z,
i
Translate
x,a
i
Rotate
x,
i
=
_

_
cos
i
cos
i
sin
i
sin
i
sin
i
a
i
cos
i
sin
i
cos
i
cos
i
sin
i
cos
i
a
i
sin
i
0 sin
i
cos
i
d
i
0 0 0 1
_

_
(11)
If a joint is rotational, its corresponding
i
is the variable. Else if a joint is prismatic, its
corresponding d
i
is the variable.
The homogeneous transformation matrix that expresses the robot end-eector frame
with respect to the robot base frame can be computed by successively postmultiplying
adjacent link transformations,
T =
0
A
n
=
0
A
1
1
A
2
. . .
n1
A
n
, (12)
which is the matrix equivalent of the workspace position and/or orientation vector, x.
22
2.3 The Robot Jacobian
A linear relationship between the C-space and the workspace of a given robot can be
expressed in terms of the robot Jacobian
x J(q) q (13)
where x R
m
denotes an end-eector workspace velocity, q R
n
denotes a joint velocity,
and J(q) R
mn
denotes the robot Jacobian. The matrix J(q) is composed of the partial
derivatives of the end-eector position and/or orientation vector, x, with respect to the
joint angle vector, q, i.e.,
J(q)
x
q
=
_

_
x
1
/q
1
x
1
/q
n
.
.
.
.
.
.
x
m
/q
1
x
m
/q
n
_

_
. (14)
This can also be expressed in terms of moving coordinate frame where each of the column
of the Jacobian as shown by Whitney [?] can be expressed as,
j
i
(q) =
_

_
_

_
0
z
i1

i1
p
n
0
z
i1
_

_ if joint i is rotational
_

_
0
z
i1
0
_

_ if joint i is prismatic
(15)
where
0
z
i1
is the vector a
i1
from the rotation matrix and
i1
p
n
is the position vector.
The terms are expressed with respect to frames reected by their indices.
For non-redundant robots where n = m, at non-singular congurations there exist a
limited number of discrete solutions, qs, that correspond to a given end-eector position
and/or orientation, x, by solving the inverse of the forward kinematics equation,
q = f
1
(x). (16)
A singular conguration occurs when the solution to (16) does not exist. This happens
when a robot loses a DOF in the workspace. Examples are a fully extended robot that
23
has reached its workspace boundary. Another example is when the axes of the robot joints
aligned that results in a robot losing a DOF in the workspace. For redundant robots where
n > m, a given end-eector position and/or orientation, x, generally corresponds to an
innite number of congurations in the C-space.
The singular value decomposition of a given matrix A C
mn
is a factorization
A = UV
T
(17)
where
U C
mm
is unitary,
V C
nn
is unitary, and
R
mn
is diagonal.
The symbol C means that the value could either be real or complex. The diagonal entries

i
of , also known as singular values, are non-negative and in a non-increasing order, that
is,
1

2

k
0 where k = min(m, n). The columns u
1
, . . . , u
m
C
m
of U are
called the left singular vectors of A. The columns v
1
, . . . , v
n
C
n
of V are called the right
singular vectors of A. Thus, the singular value decomposition factors a Jacobian matrix of
the form
J() = UV
T
. (18)
The null space component of the Jacobian, N(J), represented by N
J
R
nr
can be
generically expressed as
N
J
=
_
n
J(m+1)
. . . n
Jn
_
(19)
which forms an orthonormal basis for the null space, N(J). When the SVD of the manip-
ulator Jacobian is computed, at non-singular congurations the null space matrix, N
J
, can
be derived by taking the j = (m+1), . . . , n right singular vectors. For r = 1, N
J
is a vector
and can be expressed as n
J
R
n
. For a 3-DOF planar robot the null vector, n
J
, can be
derived by computing the cross product of the two rows of the Jacobian matrix. At the
velocity level, the inverse of the Jacobian matrix of a non-redundant robot in a non-singular
conguration solves the joint velocity necessary to realizing the desired end-eector velocity,
q = J
1
(q) x, (20)
24
which is known as the resolve motion rate control proposed by Whitney [?]. On the other
hand, the solution for the joint rates of a redundant robot in a non-singular conguration
that satisfy the desired end-eector velocities can be represented by the projection operator
formulation proposed by Liegeois [29] which is expressed as
q = J
+
x + (I J
+
J)z (21)
where J
+
is the pseudo-inverse of the robot Jacobian matrix, (IJ
+
J) is the projection onto
the null space, and z is a vector in the q-space with some desirable properties. The rst term
represents the control for the specied robot end-eector velocity while the second term is
frequently used to optimize some criterion G(q), i.e., a function of a robot conguration q,
by choosing z to be a function of its gradient, z = G(q) where alpha is some constant.
25
CHAPTER III
A SOLUTION TO THE FAILURE-TOLERANT PATH
PLANNING PROBLEM
Kinematic failure-tolerant path planning is a motion planning strategy that gives re-
dundant robots the ability to gracefully accommodate joint failures. This is based on kine-
matic failure tolerance which deals with optimizing congurations for a redundant robot in
anticipation of joint failures. This work focuses on single locked-joint failures which results
into an uninterrupted reduction of the robots C-space into its corresponding subspaces. It
is assumed that the given redundant robot has the ability to identify a joint failure and to
immediately lock the failed joint.
3.1 Overview of the Proposed Approach
Consider a kinematically redundant manipulator, operating in a failure-prone environ-
ment, that must complete a pick-and-place type task in a workspace lled with obstacles.
The goal of our work is to generate a family of joint trajectories, each of which would
allow the manipulator to avoid obstacles and tolerate any single locked-joint failure as its
end-eector travels from the given start workspace location to the desired goal location. To
achieve this goal, two fundamental issues need to be resolved. First, the manipulator must
be restricted to a region of the conguration space that is obstacle free. Second, while in
this region, one must guarantee that the goal workspace location is reachable even after
any joint failure. Our approach is to generate a surface in the conguration space that is
obstacle free and guarantees that the goal remains reachable. The characterization of the
properties of such a surface is a key contribution of this work.
The requirement of failure tolerance dictates the boundaries of where the surface must
be contained so that the goal remains reachable. This is equivalent to determining the range
of the joint values for being at the goal workspace location. Thus the rst step is to make
26
sure that the starting joint conguration falls within this range. This is the same type of
test used in [28] and has also been applied to determine a manipulators failure-tolerant
workspace [23].
Staying within these boundaries is not sucient for guaranteeing that the workspace
goal is reachable. However, it is possible to guarantee reachability if the surface has certain
properties, namely that it contains no internal local minimum or maximum in terms of
the individual joint variables. If a surface with these properties is also obstacle free, then
operating on this surface guarantees failure tolerance. Thus the crux of our approach is to
identify such a surface.
The remainder of this section will dene the terms that are used in this work. Let n
denote the number of degrees of freedom (DOFs) of a robot and let m denote the number
of DOFs of a robots workspace. For a kinematically redundant robot (n > m), the degree
of redundancy is r = n m. Its given end-eector position and/or orientation, denoted x,
generally corresponds to an innite number of congurations in the C-space.
Denition 1 (Self-Motion Manifold). The set of congurations in C-space that result
in the same end-eector workspace location x is called the pre-image of x. The pre-image
when the Jacobian cannot be singular can be written as a union of disjoint connected sets
f
1
(x) =
nm
_
i=1
M
i
(22)
where M
i
is the i-th r-dimensional self-motion manifold in the inverse kinematic pre-image
such that M
i
M
j
= when i = j, and n
m
is the number of self-motion manifolds [30].
When the pre-image of x contains both singular and non-singular congurations, it is consist
of manifolds that meet at a singular conguration and is equivalent to a bouquet of manifolds
[31].
A start self-motion manifold, denoted M
s
, corresponds to a start position and/or ori-
entation, denoted x
s
, while a goal self-motion manifold, denoted M
g
, corresponds to a goal
position and/or orientation, denoted x
g
. Fig. 2 shows a pair of single dimensional start
and goal self-motion manifolds for a robot with r = 1. The dark portions of the self-motion
27

g
start self-
motion
manifold
goal self-motion
manifold
failure planes
failure plane
failure
cube

s1

s3

s2
Figure 2: The gure shows a start and a goal self-motion manifold in the C-space of a planar
3-DOF robot. All the failure planes corresponding to an obstacle-free start conguration,

s
, intersect a continuous obstacle-free portion of the goal self-motion manifold,
g
. The
failure cube contains
s
and
g
. The failure surface corresponding to
s
, shown as a web-
like network of paths, is identied by connecting
s
to points on
g
via monotonic paths
within the failure cube. Each node along
g
denes an intersection with a face of the failure
cube.
manifolds denote congurations of the robot that are outside the joint limits or are in con-
tact with obstacles. A continuous, obstacle-free portion of the goal self-motion manifold
is denoted
g
, while an obstacle-free start conguration is denoted
s
. A point on
g
is
denoted
g
.
When a robot suers from a locked-joint failure, its feasible motion in the C-space is re-
duced. In particular, a single locked-joint failure restricts the reachable robot congurations
in the C-space into an (n 1)-dimensional hyperplane called a failure hyperplane.
Denition 2 (Failure Hyperplane). A failure hyperplane is a hyperplane in the C-
space where a joint component remains constant and is equal to a joint component of the
28
corresponding start conguration. A failure hyperplane can be expressed as
H
i
(
s
) = { |
i
=
si
} (23)
where H
i
(
s
) is the failure hyperplane associated with
s
where i [1, n].
We will call H
i
(
s
) the failure hyperplane associated with
s
. Fig. 2 shows
s
with its
corresponding failure planes H
1
(
s
), H
2
(
s
), and H
3
(
s
). The failure hyperplane that in-
tersects
g
(0) is denoted H
0
j
(
s
), and the failure hyperplane that intersects
g
(1) is denoted
H
1
k
(
s
).
Given a desired workspace goal x
g
, a necessary condition will be developed to determine
if that goal can be reached from
s
in spite of any locked-joint failure. This condition will
identify a portion of the
g
curve, parameterized here by the function
g
(v) : [0, 1] R
n
.
Denition 3 (Failure Hypercuboid). The extremal points of
g
(v) dene the bounds
of a hypervolume, V, in C-space called a failure hypercuboid. This failure hypercuboid has
the form
V = { | min
0v1

gi
(v)
i
max
0v1

gi
(v), i = 1, . . . , n}. (24)
This determines a failure-tolerant workspace corresponding to
s
and
g
(v). In this
case
s
is called a feasible start conguration. The failure-tolerant workspace discussed in
this work is a subset of the one discussed in [23], which is a function of x
s
and x
g
.
Because there are obstacles in the workspace, there is a need to identify a suitable region
in the failure hypercuboid that would allow the robot to reach the goal while avoiding
obstacles. Furthermore, this region must have the property that the robot can still reach
its goal in spite of a single locked-joint failure. This will be accomplished by choosing a
surface connecting a suitable
s
to a
g
. This surface is chosen so that the manipulator
will not encounter obstacles and can remain on the surface even if a joint is locked. The
conditions for the existence of such a surface, called a failure surface, S, will be derived in
the following section.
3.2 Conditions for the Existence of a Solution
In the previous section, an approach is described to guarantee failure tolerance in the
29
presence of obstacles using a surface. In this section conditions will be derived concerning
the existence of such a surface. The rst condition is a necessary condition to determine
the suitability of a start conguration,
s
, based on the ability of the manipulator to reach
the
g
curve with any single joint locked. While
s
and
g
are obstacle free, this condition
does not take into account the possibility of encountering obstacles on the way, and is thus
only a necessary condition. We will see later that this tends to be a strong indicator of the
likelihood that such a surface exists. Fortunately it turns out that this is a relatively fast
calculation that can easily eliminate cases where no surface exists. The second condition is
used to determine the existence of the solution once a feasible
s
is identied. The proof
of this result will motivate an algorithm for calculating a suitable surface. Such a surface
cannot have a local minimum or maximum in any of its
i
components.
The necessary condition is given in terms of the failure hyperplanes associated with

s
. Given x
s
and x
g
, the manipulator can reach
g
when joint i is locked at
s
provided
that the corresponding failure hyperplane of
s
intersects
g
, that is, H
i
(
s
)
g
= .
Physically this means that the resulting failure-induced C-space due to a locked joint i
failure at
s
contains at least a point of
g
. This indicates the possibility of reaching x
g
despite a locked-joint failure at
s
. Thus, to guarantee that the robot will reach
g
from
s
for any locked-joint failure, it is necessary that each of the n failure hyperplanes, H
i
(
s
),
intersects the
g
curve. This motivates the denition of a feasible start conguration
s
when all of its corresponding failure hyperplanes intersect
g
. This proposition is formally
stated as a necessary condition.
Proposition 1 (Necessary Condition). A necessary condition for a given obstacle-free
start conguration,
s
, to be a feasible start conguration is that all of the corresponding
failure hyperplanes of
s
intersect a continuous, obstacle-free portion of the goal self-motion
manifold,
g
, that is,
H
i
(
s
)
g
= , for all i = 1, . . . , n. (25)
After a feasible
s
has been identied, its corresponding
g
(v) denes the bounds of a
30
failure hypercuboid, V. If there were no obstacles in the workspace, then the identication
of V is sucient to guarantee that a given robot can successfully reach x
g
from
s
for any
single locked-joint failure at any time by keeping its congurations within V as the given
robot approaches x
g
. When obstacles are present in the workspace, some regions of V may
no longer be available. The problem now becomes that of searching for a solution set within
V that can guarantee successful completion. This will be accomplished by generating an
obstacle-free surface connecting
s
to the
g
curve in such a way that the manipulator
can continue to move along the surface to
g
in spite of any single locked-joint failure.
When a joint is locked the manipulator is constrained to be on a corresponding hyperplane.
Therefore, the intersection of that hyperplane with the surface needs to connect with
g
.
The surface must satisfy a monotonicity property in the following sense. Consider locking
joint i. As the value at which joint i is locked varies, this determines a contour-like plot
in which
i
is analogous to the height. The dierent contours represent the manipulators
motion along the surface with its i-th joint locked at dierent xed values. In order for
a manipulator to be able to reach its goal for any locked-joint failure, no closed contours
can exist. Therefore the surface cannot have any local internal minimum or maximum with
respect to any
i
. This monotonicity condition is captured by the following theorem.
Theorem 1 (Sucient Condition). Let S be an obstacle-free, two-dimensional, simply-
connected surface in R
n
. Suppose S = {S(u, v) | u, v [0, 1]} is the image of a continuous
function S : [0, 1] [0, 1] R
n
that is continuously dierentiable in its interior and that
S(u, v) has the following properties:
(i) S(0, v) =
s
for all v [0, 1].
(ii) S(1, v) =
g
(v) for all v [0, 1] where
g
(v) is a smooth connected curve.
(iii) For a xed v [0, 1] and a xed integer i = 1, 2, . . . , n, the i-th component S
i
(u, v) of
S(u, v) is either a strictly monotonic function of u such that
S
u
= 0 for 0 < u < 1 or,
in the case when
gi
(v) =
si
, a constant with respect to u.
Furthermore, assume that any point on S satises
31
(iv) min
0v1

gi
(v)
i
max
0v1

gi
(v) for i = 1, 2, . . . , n.
Then given any point
0
on S and any integer i = 1, 2, . . . , n, there is a curve : [0, 1] R
n
lying entirely on S that connects
0
to a point
1
on the curve
g
(v) such that
i
(t) =
0i
for all t [0, 1].
Proof. The result will be proven by constructing a suitable curve. Fix i {1, 2, . . . , n}.
Given
0
S, there is a pair (u
0
, v
0
) such that S(u
0
, v
0
) =
0
. By assumption (iv),

gi
(v
1
) =
0i
for some v
1
[0, 1] where we assume v
1
is the closest such value to v
0
. We
need to show that there is a curve : [0, 1] R
n
such that (0) =
0
, (1) = S(1, v
1
), and

i
(t) =
0i
for all t [0, 1].
The problem is trivial if
0
is on the
g
curve. Assuming that it is not, we consider
three cases. First, suppose that
0
=
s
. Then
gi
(v
1
) =
si
and by assumption (iii) the
curve (t) = S(t, v
1
) is a suitable curve. Next, consider the case when
0
=
s
but
0i
=
si
.
Then u
0
= 0 but S
i
(0, v
0
) = S
i
(u
0
, v
0
) so that S
i
(u, v
0
) is not strictly monotonic in u. But,
by assumption (iii), this implies that S
i
(u, v
0
) =
si
for 0 u 1. This gives a suitable
curve (t) = S(u
0
(1 t) + t, v
0
).
Lastly, consider the case when
0i
=
si
. The goal is to determine a trajectory w(t) =
[u(t) v(t)]
T
, 0 t 1, going fromw
0
= [u
0
v
0
]
T
to w
1
= [1 v
1
]
T
while keeping S
i
(u(t), v(t)) =

0i
constant. Since the domain is compact, this can be done by applying an inverse velocity
kinematics type algorithm to the system
z =
_

_
z
1
z
2
_

_
= g(w) =
_

_
v
S
i
(u, v)
_

_
(26)
to nd a suitable trajectory (u(t), v(t)) provided of course that the Jacobian of g,
G =
g
w
=
_

_
0 1
S
i
u
S
i
v
_

_
, (27)
does not become singular along the way. However, by assumption (iii), the determinant of
G, which is equal to S
i
/u, is zero only when S
i
(u, v) =
si
. This never occurs along the
trajectory as S
i
(u(t), v(t)) =
si
is xed. We can therefore generate a trajectory (u(t), v(t))
32
such that
z(t) =
_

_
v
0
(1 t) + v
1
t

0i
_

_, 0 t 1. (28)
To nish o the proof, we need to show that u(t) stays on the interval [0, 1] throughout
the trajectory and ends at 1. We do this by rst showing that u(t) cannot be 1 or 0 for
0 t < 1 and then using an argument concerning the monotonicity of S
i
with respect to u
to show that u(1) must be 1. The case u(t) = 0 for some 0 t < 1 violates the assumption
that
0i
=
si
while the case u(t) = 1 for some 0 t < 1 violates the assumption that v
1
is
the closest v-value to v
0
such that
gi
=
0i
. Since S
i
(u(t), v(t)) =
0i
on the trajectory, we
have that S
i
(u(1), v
1
) =
0i
=
gi
(v
1
) = S
i
(1, v
1
) so that by assumption (iii), u(1) = 1. We
thus have a suitable curve (t) = S(u(t), v(t)).
Thus, the monotonicity of paths from
s
to
g
(v) guarantees that after a single locked-
joint failure, the given robot will not get stuck in the vicinity of an internal local minimum
or maximum and will reach a point on
g
(v). By specifying only the start and goal locations
for pick and place tasks in the presence of obstacles, the solution being presented provides
some exibility in choosing the obstacle-free, failure-tolerant path towards the goal based
on some desired optimization criteria prior to a failure. After a failure, there would exist
a nite number of feasible paths. When there is more than one feasible path the shortest
path towards the goal, for example, could be chosen.
Theorem 1 motivates the method for identifying a failure surface in the algorithm by
generating monotonic paths from
s
to
g
(v). Note that this theorem is only a sucient
condition and can only guarantee task completion when the failure surface exists. When
the failure surface identication is unsuccessful, a solution may still be present.
3.3 Global Failure Tolerance
A local failure tolerance measure was rst proposed by Lewis and Maciejewski [26]
as the minimum over all the minimum singular values of the failed manipulator Jacobian
matrix. This measure is useful in optimizing the manipulator conguration towards the
most failure-tolerant conguration with respect to the current workspace position and/or
33
orientation.
In this work, a global kinematic failure tolerance measure is presented which is derived
from the self-motion manifolds of a manipulator. Based on this measure, an estimate can
be deduced as to how tolerant a given manipulator would be to any joint failure through-
out its workspace. Because of its global nature, an extensive computation is required to
search throughout the entire self-motion manifolds of an arbitrary manipulator to be able
to derive its value. However, for the case where a given robot is globally failure intolerant
to a particular robot failure, this measure is very useful and does not require extensive
computation.
The null space of a robot with r = 1 is represented by a null vector, n
J
, of the manip-
ulator Jacobian whose i-th component is denoted n
Ji
. It has been shown by Roberts and
Maciejewski [22] that, for r = 1, a conguration is intolerant to a joint i failure if the null
vector component n
Ji
= 0. If this is true for all congurations in the robots C-space, then
it is globally intolerant to an i-th joint failure.
When a joint component i has zero null vector component, n
Ji
= 0, always, it means that
this specic joint has no contribution in the manipulators null space. This joints component
in the self-motion manifold is constant. Thus, the larger the null vector component of
joint i is, the bigger is its excursion in the self-motion manifold. A joints tolerance to
failure, therefore, is related to its range of values in the self-motion manifold. The larger
the change in a joints value, the greater tolerance there will be to a failure in that joint.
A joint whose value is constant throughout the self-motion manifold is a globally failure
intolerant joint and is a critical manipulator joint.
A global kinematic failure tolerance measure can be derived by using the range of
each joints excursion in the self-motion manifold. The joint with the minimum range of
excursion determines the worst-case global failure tolerance measure

min
= min
1in
(|
i
|) (29)
where
i
is the maximum excursion of joint i as it spans the self-motion manifold. (This
is the same as the bounding box discussed in [23].) Other possible global failure tolerance
34
measures include the square root of the sum of squares of each joints maximum excursion
in the self-motion manifold

ave
=

_
n

i=1

2
i
(30)
or the ratio of the joint with the least maximum excursion with that of the joint with the
largest maximum excursion in the self-motion manifold

ratio
=
min
1in
(|
i
|)
max
1in
(|
i
|)
(31)
or the product of each joints maximum excursion in the self-motion manifold

prod
=
n

i=1
|
i
|. (32)
Normally, this range can vary from zero to some nite non-negative value. It would be
computationally expensive to nd the exact value of for a given robot because one would
need to evaluate the whole space of the self-motion manifold. However, if
min
= 0, then

i
= 0 for some i and the robot is globally intolerant to a failure of joint i which is a critical
manipulator joint. A good redundant manipulator design that considers failure tolerance is
a design with a large global failure tolerance measure . Such a design would allow a larger
range of values in the manipulators self-motion manifold and increases the likelihood of
satisfying the necessary condition in Section 3.2.
There is a rare exception that a robot can still be failure tolerant to a joint i failure
when
min
= 0 due to
i
= 0. This happens when the projections of the start and goal
self-motion manifolds along the critical manipulator joint i axis coincide with each other.
This means that for the robot to move from x
s
to x
g
, joint i needs to be at this xed angle
where the manifolds projections coincide, thus, a locked-joint failure at joint i would still
make the robot tolerant to a joint i failure.
Two redundant robots has been tested with the proposed global kinematic failure toler-
ance measure discussed here: Mitsubishi PA-10 robot shown in Fig. 3 and Robotics Research
Corporation K-1207i robot shown in Fig. 4. Both robots claim to be anthropomorphic, that
is, their design resembles that of the human arm.
35
Figure 3: The Mitsubhishi PA-10 seven degrees of freedom robot.
36
Figure 4: The Robotics Research Corporation K-1207i seven degrees of freedom
robot. (With permission from Keith A. Kowalski, Robotics Research Corporation,
http://www.robotics-research.com.)
The Mitsubishi PA-10 robot is not fully a kinematically failure-tolerant robot because
the null space component of joint four is zero, i.e.,
n
J4
= 0, (33)
throughout the entire C-space. Thus, the PA-10 has a worst-case global failure tolerance
measure
min
= 0 due to joint four and is intolerant to a joint four failure. The reason
for this is because the three axes of its shoulder joints meet at a common point as do the
three axes of its wrist joints. And joint four is the only joint that can alter the relative
distance between these two common points. Thus to maintain a given workspace location,
all the other joints can move in the null space except joint four. Although the Robotics
Research Corporation K-1207i robot is not completely intolerant to its joint four failure, its
global kinematic failure tolerance measure,
min
, due to its joint four is barely signicant.
The slight gain is due to a small oset of its wrist axes with respect to the shoulder axes.
Thus, because of their anthropomorphic designs, these two robots inherited the frailty of
the human arm, that is, its intolerance to a broken elbow.
37
CHAPTER IV
ALGORITHM AND PSEUDO CODE IMPLEMENTATION
In this chapter, the algorithm for failure-tolerant path planning is presented and the
pseudo codes for implementing each step of the algorithm are discussed.
4.1 Algorithm
Based on the conditions discussed from the previous chapter, an algorithm consisting
of four steps are enumerated in the following:
1. Determine the start self-motion manifold, M
s
, and goal self-motion manifold, M
g
,
corresponding to the given start, x
s
, and goal, x
g
, workspace position and/or orien-
tation.
2. Identify an obstacle-free start conguration,
s
, and an obstacle-free portion of the
goal self-motion manifold,
g
.
3. Check for intersections of the failure hyperplane H
i
(
s
) with
g
for i = 1, . . . , n.
(Note that this step uses the necessary condition in Section 3.2).
4. Check for the existence of a failure surface, S. This is done by generating monotonic
paths from the feasible
s
to points on
g
(v) and checking for intersections with
obstacles. (This step utilizes the sucient condition in Section 3.2).
4.2 Pseudo Code Implementation
The pseudo code implementation for the proposed algorithm are discussed below. This
implementation focused on single dimensional self-motion manifolds, that is r = 1. For
robots with higher DOR, the same algorithm can still be used by rst identifying single
dimensional curves from the higher dimensional manifolds.
38
4.2.1 Step 1: Computing M
s
and M
g
Given x
s
and x
g
the corresponding M
s
and M
g
are computed. The dimension of
a self-motion manifold is dened by the manipulators degree of redundancy. The pseudo
code for the main function to compute a single dimensional self-motion manifold(s) for a
desired workspace position and/or orientation, x
d
, is given in Fig. 5.
Figure 5: Pseudo code for computing self-motion manifold(s).
ComputeSelfMotionManifold(x
d
)
x
d
, desired workspace position and/or orientation
n
m
, total number of disjoint self-motion manifolds for the given x
d
, a robot conguration
(1) for i to n
m
For every disjoint manifold, nd an
(2) = InitializeManifold(i, x
d
) initial conguration from a lookup
(3) CalculateManifoldPoints(, x
d
) table, and compute the succeeding
congurations in the manifold.
A workspace position and/or orientation can correspond to more than one disjoint self-
motion manifolds depending on its location in the workspace. For a three-link planar
manipulator with equal link lengths, there are two disjoint manifolds that corresponds to
a workspace location that is closer than one link length from its base. At a workspace
location that is farther than one link length from the manipulators base, there is only
a single corresponding manifold. A lookup table provides the initial point or congu-
ration for each manifold that corresponds to a given workspace position and/or orienta-
tion. Once an initial manifold conguration is found, the rest of the congurations on
the manifold can be computed. The pseudo code of the function for computing each
conguration on a manifold after nding an initial conguration is shown in Fig. 6.
39
Figure 6: Pseudo code for calculating congurations on a self-motion manifold.
CalculateManifoldPoints(, x
d
)
, input conguration
x
d
, desired workspace position and/or orientation

(i)
, robot conguration at index i with j-th component
(i)
j
n
(i)
J
, unit null vector of Jacobian at
(i)
(1) i = 1,
(1)
= , compute n
(1)
J
From an initial conguration and null
(2) do vector, compute all subsequent cong-
(3) i = i + 1 urations on the manifold until a joint
(4) [
(i)
, n
(i)
J
] = ConvergeManifoldPoint(
(i1)
, n
(i1)
J
, x
d
)
(5) while (
(i)
j

(1)
j
+ 2) and (
(i)

(1)
)
component of the new conguration is
2 away from its initial value or until
the new conguration is back to the
initial conguration.
The function CalculateManifoldPoints(, x
d
) computes congurations that are a spec-
ied distance apart on the self-motion manifold after an initial manifold conguration has
been found. Throughout this function, a corresponding null vector is determined for each
conguration on the manifold. Each null vector is tangent to the manifold at the correspond-
ing manifold conguration and provides a mechanism in determining the next conguration
to compute on the manifold. Several methods can be used in solving for the null vector
of a given redundant manipulator. For a 3-DOF planar manipulator, the null vector can
be calculated by computing the cross product of the two rows of the manipulator Jacobian
matrix. For higher DOF manipulators, the null vector is extracted from the SVD of the
Jacobian which is computed for solving the damped least squares solution. (This will be
discussed later).
The computation for a new conguration on the manifold continues until one of the
40
joint components of the latest computed conguration is 2 away from its initial value
or until it has completed a loop and is back to the initial conguration. The function
ConvergeManifoldPoint(, n
J
, x
d
) as shown in Fig. 7 ensures that the distance between
the computed congurations on the manifold is of the predened value.
Figure 7: Pseudo code to compute congurations on the self-motion manifold that are of a
predened distance from each other.
ConvergeManifoldPoint(, n
J
, x
d
)
, input conguration; also stores the computed conguration
n
J
, input unit null vector; also stores the computed unit null vector
x
d
, desired workspace position and/or orientation

o
, original input conguration

p
, previously computed conguration
n
p
J
, unit null vector associated with
p
, a variable distance to move along a null vector
STEP, a desired predened distance between two adjacent congurations on
the self-motion manifold
41
(1)
p
=
o
= , n
p
J
= n
J
From the initial conguration and
(2) = STEP null vector, nd the next congura-
(3) do tion on the manifold that is a STEP
(4) =
p
+ n
p
J
away from the initial conguration
(5) = ConvergeJointAngle(, x
d
) via the following. Move along the
(6) compute n
J
direction of the previous null vector
(7) if ( n
T
J
n
p
J
0) and then converge to a manifold con-
(8) n
p
J
= n
J
guration. Choose its corresponding
(9) else null vector to be within 90

from
(10) n
p
J
= n
J
the previous null vector. Adjust the
(11) = STEP
o
current step size such that the subse-
(12)
p
= quent congurations would converge
(13) while (|| ) to within STEP from the input con-
(14) return , n
p
J
guration.
The function ConvergeManifoldPoint(, n
J
, x
d
), computes a conguration along the
direction of the previous null vector, then converges to a conguration on the manifold. The
function continues to perform this operation until the new computed conguration is at a
predened distance from the initial conguration. To ensure that each new conguration
is consistently computed in the direction of the undetermined portion of the manifold, the
direction of its corresponding null vector is chosen to be is along the direction of the previous
null vector. This, of course, uses the assumption that the new conguration is close enough
from the previous conguration such that no sharp turns occur between congurations.
At each iteration, the variable step to move along the null vector is adjusted in order to
progress closer to a new conguration that is of the predened distance from the original
conguration.
Because the null vector is tangent to the manifold, the new conguration derived by
moving along the null vector is not on the manifold, and must be corrected to return to the
manifold. The function ConvergeJointAngle(, x
d
) converges to a conguration on the
42
manifold from an input conguration. The pseudo code of this function is shown in Fig. 8.
Figure 8: Pseudo code to converge to a conguration on the self-motion manifold.
ConvergeJointAngle(, x
d
)
, input conguration; also stores the computed robot conguration
n
d
, o
d
, a
d
, unit vectors associated with the desired orientation
p
d
, desired position vector
x
d
, desired workspace position and/or orientation which is a function of
n
d
, o
d
, a
d
, and p
d
n
a
, o
a
, a
a
, unit vectors associated with the actual orientation
p
a
, actual position vector
x
a
, actual workspace position and/or orientation which is a function of
n
a
, o
a
, a
a
, and p
a
J(), manipulator Jacobian matrix
I
m
, identity matrix R
mm
x
p
, dierence between desired and actual workspace positions
x
o
, dierence between desired and actual workspace orientations
x, dierence between desired and actual workspace positions and/or
orientations
, incremental step angle
K
p
, diagonal matrix with elements corresponding to the workspace
proportional gains
, damping factor
43
(1) do A damped least squares solution
(2) x
a
= f() is computed to nd the incremen-
(3) update J() tal step conguration of the robot
(4) x
p
= p
d
p
a
corresponding to the end-eector
(5) x
o
=
1
2
[( n
a
n
d
) + (o
a
o
d
) + (a
a
a
d
)]
(6) x = [x
T
p
, x
T
o
]
T
position and/or orientation error.
(7) = J
T
[JJ
T
+
2
I
m
]
1
K
p
x The computed conguration has
(8) = + converged to the manifold when
(9) while ( ) the computed error is less than .
(10) return
The function ConvergeJointAngles(, x
d
) iteratively solves the inverse kinematics prob-
lem in order to converge to a conguration on the manifold for a given input conguration.
For every iteration, this function solves a damped least squares solution to produce an in-
cremental step conguration, , derived from the incremental error between the desired
and actual positions and/or orientations. The position error, x
p
, is simply the dierence
in the desired and actual positions. The computation for the dierence in orientation, x
o
,
requires the cross products of the vector components of the actual and desired rotation
matrices as given by Luh et al. [?],
x
o
=
1
2
[( n
a
n
d
) + (o
a
o
d
) + (a
a
a
d
)]. (34)
The new computed conguration is considered to have converged to a conguration on the
manifold if the incremental step in conguration, , is less than a small number, .
For the 3-DOF planar robot the damped least squares solution can be computed,
= J
T
[JJ
T
+
2
I
m
]
1
K
p
x, (35)
such that the value of the damping factor, , is set according to the desired optimization
criteria and to the robots proximity to singular congurations which is determined by the
value of the the minimum singular value of the Jacobian,
m
. The null vector, minimum
44
singular value, and damped least squares solution, which are required by the algorithm, are
computed using closed-form equations. The null vector is derived from the cross product of
the two rows of the Jacobian. Its minimum singular value can derived by computing
det(JJ
T

I
m
) = 0 (36)
where

is the eigenvalue of JJ
T
. (Note that the symbol is used as the damping factor).
This is possible because the Jacobian has only two rows. Letting the eigenvalues

1
and

2
as the roots of (36), the minimum singular value of the Jacobian is
2
=
_
min(|

1
|, |

2
|).
The damped least squares solution is computed via Cramers Rule.
For higher DOFs manipulators with a single degree of redundancy, a numerical method
for computing the values of the minimum singular value, the null vector, and the damped
least squares solution is needed. The method used in this work is by computing the SVD
of the Jacobian which provides all the necessary information needed to compute the above
mentioned three values. However, the computation cost is in a multiple of 4mn
2
+22m
3
for a
matrix of size m > n using Chans algorithm [?] which is higher compared to other numerical
methods. This computation can be greatly improved by using elementary row operations,
like Gaussian elimination with backsubstitution, which has a faster computation time that
is in a multiple of
1
2
m
2
n +
1
3
m
3
as stated by Press et al. [?]. Implementation of damped
least squares solutions with minimum singular value estimates is shown by Maciejewski and
Klein [?].
By computing the SVD of the Jacobian, the minimum singular value,
m
, and the
null vector which is the n-th right singular vector, v
n
, can be directly extracted from the
decomposition. The damped least squares solution can be computed as,
=
_
m

i=1

2
i
+
2
v
i
u
T
i
_
k
p
x (37)
where m is the number of rows of the Jacobian. The value of the damping factor, , is set
according to a desired optimization criterion in regions closer to singularities.
When a given robot is in the region of singularity, the null vector is computed by getting
the projection of the previous null vector n
p
J
onto the current (nr
n
)-dimensional null space,
45
where r
n
is the rank of the Jacobian. Thus the current non-zero null vector becomes,
n
J
=

nrn
i=1
( n
p
J
n
Ji
) n
Ji

nrn
i=1
( n
p
J
n
Ji
) n
Ji

(38)
where n
Ji
is the i-th orthogonal basis in the (n r
n
)-dimensional null space.
4.2.2 Step 2: Identifying
s
and
g
After computing for the start and goal self-motion manifolds, Step 2 identies the
obstacle-free portions of the manifolds: the set of obstacle-free start congurations,
s
s,
and the set of continuous obstacle-free portions of the goal self-motion manifold,
g
s. This
step is purely collision detection for each conguration on the self-motion manifolds.
One of the widely known collision-detection algorithm is the Lin-Canny algorithm [?].
The method uses collision and contact determination between geometric models described
by linear or curved boundaries undergoing rigid motion. It maintains the pair of closest
features (vertices, edges, or faces) between two convex polyhedra moving through space
and utilizes convexity to establish some local applicability criteria for verifying the closest
features. Near constant query time is achieved in practice by exploiting the fact that the
current closest features are probably near the previous closest features. Once the closest
features are known, the distance between two polyhedra is easily found. A collision is
declared when this distance falls below some small epsilon.
A modication to the Lin-Canny algorithm, the Voronoi-Clip, or V-Clip, algorithm [?]
tracks closest pairs of features similar to the Lin-Canny but is based on impulse of rigid
body systems. The author claims that V-Clip is not as complex as Lin-Canny to code, is
more robust, the implementation has no numerical tolerances, and does not exhibit cycling
problems. The algorithm also handles penetrating polyhedra, and can be used to detect
collisions between non-convex polyhedra described as hierarchies of convex pieces.
In this work, the collision-detection algorithm is carried out by modeling manipulator
links as cylinders with semi-spherical ends. For a given conguration, the distance between
each robot link and the workspace obstacles is analytically checked. The pseudo code for
the main function to identify the obstacle-free congurations of a given self-motion manifold
is given in Fig. 9.
46
Figure 9: Pseudo code to identify the obstacle-free congurations of the given self-motion
manifold.
ObstacleFreeTheta(M)
M, input self-motion manifold
n
mp
, number of discretized congurations on the input self-motion manifold

(k)
, a conguration on the input self-motion manifold at k-th dicretization
index where k [1, n
mp
]
free, ag for collision detection
(1) for k = 1 to n
mp
Obstacle collision for every conguration
(2) free=CheckCollision(
(k)
) on the self-motion manifold is checked.
(3) if free== 0 A conguration in collision with an obs-
(4)
(k)
.free= 0 tacle is agged to be not obstacle free.
Congurations on the self-motion manifold that are in collision with obstacles are agged
to dierentiate them from the obstacle-free congurations. For a given conguration, the
pseudo code for detecting collision of a given robot with all the obstacles in the workspace
is shown in Fig. 10.
Figure 10: Pseudo code to check a conguration on the self-motion manifold for collision
against all the workspace obstacles.
CheckFreeSpace()
Assumption: In checking collisions with obstacles, robot links are treated as
cylinders with semi-spherical ends.
, input conguration
p
(i)
l
, workspace position of a reference frame at the tip of link i such that p
(0)
l
is
the workspace position of a reference frame at the robot base
x
(i)
, workspace position and/or orientation of a reference frame at the tip of
link i computed from forward kinematics
47
p
(j)
obs
, position of the j-th obstacle in the workspace
d, shortest distance from an obstacle to a robot link
u, parameter of a robot link parameterized in terms of u [0, 1]
r
obs
, radius of an obstacle
p, number of workspace obstacles
n, number of DOFs of the manipulator
free, ag for detecting collision
(1) p
(0)
l
= 0, free = 1, j = 0 The shortest distance between an
(2) do obstacle and a robot link in 3-D
(3) j = j + 1, i = 0 space is computed. Collision is
(4) do detected if the shortest distance
(5) i = i + 1 of an obstacle to a link is less
(6) x
(i)
= f() than the obstacle radius and the
(7) d =
(p
(i)
l
p
(i1)
l
)(p
(j)
obs
p
(i1)
l
)
p
(i)
l
p
(i1)
l

intersection occurs within the
(8) if (d < r
obs
) interval u [0, 1]. If the intersec-
(9) u =
(p
(j)
obs
p
(i1)
l
)
T
(p
(i)
l
p
(i1)
l
)
p
(i)
l
p
(i1)
l

2
tion occurs outside this interval,
(10) free= (u < 0 or u > 1) the link end-points are checked
(11) if (free== 1) for collision. Collision detection
(12) free = (p
(j)
obs
p
(i1)
l
> r
obs
and p
(j)
obs
p
(i)
l
> r
obs
)
(13) while (i < n) and (free== 1) exits immediately once a collision
(14) while (j < p) and (free== 1) is detected.
(15) return free
The function CheckFreeSpace() analytically checks collision by computing the short-
est distance between each robot link and the workspace obstacles in a general 3-dimensional
space [?]. The robot links are modeled as cylinders with semi-spherical ends and the ob-
stacles are spherical. In this work, analytically nding the shortest distance of an obstacle
to a robot link is similar to nding the shortest distance of a point x
0
to a line segment
48
(x
1
x
2
). A parallelogram with base (x
2
x
1
) and side (x
0
x
1
) has an altitude which is
the shortest distance of x
0
to the base (x
2
x
1
), that is,
d =
area of parallelogram
length of parallelogram base
=
(x
2
x
1
) (x
0
x
1
)
x
2
x
1

. (39)
The link parameter, u, expresses the normalized scalar projection of the side (x
0
x
1
) along
the base (x
2
x
1
), that is,
u =
scalar projection along parallelogram base
length of parallelogram base
=
(x
0
x
1
)
T
(x
2
x
1
)
x
2
x
1

2
. (40)
Collision is detected if the shortest distance, d, is smaller than an obstacle radius and the
intersection occurs within the link parameter u [0, 1]. Collision is also determined when
the intersection occurs outside the parameterized link but the distance between an obstacle
and an end-point of a link is less than the obstacle radius.
4.2.3 Step 3: Testing the Necessary Condition
After identifying the obstacle-free portions of the start and goal self-motion manifolds,
the next step is to take an obstacle-free start conguration,
s
, pair it with a continuous
obstacle-free portion of the goal self-motion manifold,
g
, and check for intersections with
the failure hyperplanes H
i
(
s
) with
g
for i = 1, . . . , n. The main function to check
for the necessary condition is shown in Fig. 11. This function checks for the sucient
condition immediately after the necessary condition is satised. Thus, the check for the
sucient condition occurs inside the necessary condition check but it will be discussed later
in Step 4.
Figure 11: Pseudo code to check for the intersection of a failure hyperplane H
i
(
s
) with
a continuous obstacle-free portion of the goal self-motion manifold,
g
, for i = 1, . . . , n.
CheckNecessaryCondition(
s
,
g
)

s
, an input obstacle-free start conguration

g
, an input continuous obstacle-free portion of the goal self-motion manifold
i
H
, stored indices i of H
i
(
s
) intersecting
g
where i = 1, . . . , n with i-th com-
49
ponent i
Hi
n
g
, number of congurations on the discretized
g

g
[1, n
g
], discretized
g
with discretization starting from 1 to n
g
k, index of discretization which goes from 1 to n
g

(k)
g
, a conguration on
g
[1, n
g
] at the k-th discretization index
k
H
, stored values of k when
g
[1, n
g
] is intersected by a failure hyperplane with i-th
component k
Hi

min
, stored minimum components of a bounding box with i-th component
mini

max
, stored maximum components of a bounding box with i-th component
maxi
check
nc
, ag for checking the necessary condition
check
sc
, ag for checking the sucient condition
flag, ag for checking the occurrence of failure hyperplane intersections
loop, ag to loop back the discretization index of the manifold
k
first
, value of index k when a failure hyperplane rst intersects
g
[1, n
g
]
as index k goes from 1 to n
g
n
fp
, number of failure hyperplanes intersecting
g
[1, n
g
]

Hj
, the value of
(k)
g
when
g
[1, n
g
] is intersected by a failure hyperplane
such that the index j indicates the order of intersection

H
, a matrix whose columns are
H1
, . . . ,
Hn
fp

g
[k
first
, k], a portion of
g
[1, n
g
] starting from k
first
to the current k-th
discretization index

gi
[k
first
, k], i-th component of
g
[k
first
, k]

g
[0, 1], the
g
[k
first
, k] that satised the necessary condition and is para-
meterized in terms of v [0, 1]
i
stop
, number of failure hyperplane intersections encountered along
g
[0, 1] as
parameter v goes from 1 to some value just before the failure surface
identication exits
n, number of DOFs of the manipulator
50
(1) i
H
= k
H
= 0, check
nc
=check
sc
=loop= k = n
fp
= 0
(2)
min
=
max
=
(1)
g
For each conguration on
g
[1, n
g
],
(3) do check its intersection with a failure
(4) k = k + 1 hyperplane. An intersection is deter-
(5) (i
H
, n
fp
,
min
,
max
, flag) =CheckBoundingBox(i
H
, n
fp
,
min
,
max
,
s
,
(k)
g
)
(6) if (flag== 1) mined if a component
si
of the in-
(7) k
Hn
fp
= k put start conguration lies within
(8) if (n
fp
== 1) bounding box of
g
[1, k] where k is
(9) k
first
= k the current discretization index.
(10)
Hn
fp
=
(k)
g
When all the failure hyperplanes
(11)
H
= [
H1

Hn
fp
] are intersected, the existence of a
(12) if (n
fp
== n) failure surface is checked. If no
(13) check
nc
= 1 failure surface is found, the corre-
(14)
g
[0, 1] =
g
[k
first
, k] sponding indices are reset such
(15) (check
sc
, i
stop
) =CheckFailureSurface(i
H1
, i
Hn
,
H
,
s
,
g
[0, 1])
(16) if (check
sc
== 0) that the portion of
g
[0, 1] where
(17) n
fp
= n
fp
i
stop
collision was detected is looped
(18) if (loop== 0) back at the end of n
g
. The re-
(19) loop= 1 maining portion of
g
[0, 1] which
(20) n
g
= n
g
+ k
Histop
was not connected to
s
will be re-
(21) if (n
fp
> 0) used in the next necessary condi-
(22) k
first
= k
H(1+istop)
tion test.
(23) for i = 1 to n
fp
(24) i
Hi
= i
H(i+istop)
(25)
Hi
=
H(i+istop)
(26) for i = 1 to n
(27)
mini
= min
gi
[k
first
, k]
(28)
maxi
= max
gi
[k
first
, k]
51
(29) for i = (1 + n
fp
) to n
(30) i
Hi
= 0
(31) else if (k < n
g
)
(32) i
H
= k
H
= 0, n
fp
= 0
(33)
min
=
max
=
(k+1)
g
(34) while (k < n
g
) and (check
sc
== 0)
(35) return check
nc
+ check
sc
The input
g
is discretized in terms of k = 1, . . . , n
g
and denoted
g
[1, n
g
], where n
g
is the total discretized points of
g
. A vector i
H
stores the index i of the failure hyperplane
H
i
(
s
) that intersects
g
[1, n
g
] where i = 1, . . . , n. An intersection is determined if a
component
si
lies within the bounding box of
g
[1, k] where k is the current discretization
index. The intersection point is stored as
Hj
where the index j indicates the number of
intersections found. The columns of
H
are updated every time an intersection is found.
The vector k
H
stores the index k when a failure hyperplane intersects
g
[1, n
g
]. The
portion of
g
[1, n
g
] from k
first
to the current k index, denoted
g
[k
first
, k], that satises
the necessary condition is parameterized from 0 to 1 and denoted
g
[0, 1]. Failure surface
identication follows immediately after the satisfaction of the necessary condition.
If collision is detected when connecting
s
to parameterized points on
g
[0, 1], the
portion of
g
[k
first
, k] where the unsuccessful failure surface identication was performed
is looped back at the end of n
g
. The remaining portion of
g
[k
first
, k] which was not
connected to
s
will be reused in the next necessary condition test. The index k
first
is
reset to the failure hyperplane intersection k
Hi
after the intersection index i
stop
, that is
k
first
= k
H(1+istop)
. If i
stop
= n and k < n
g
then all the indices restart at index k + 1.
Fig. 12 shows the pseudo code for checking the intersection of
g
with a failure hyperplane
that was not previously intersected.
52
Figure 12: Pseudo code to check for an intersection of
g
with a failure hyperplane H
i
(
s
)
that was not previously intersected.
CheckBoundingBox(i
H
, n
fp
,
min
,
max
,
s
,
g
)
i
H
, stored indices i of H
i
(
s
) intersecting
g
where i = 1, . . . , n with i-th compo-
nent i
Hi
n
fp
, number of failure hyperplanes H
i
(
s
) intersecting
g

s
, an input start conguration with i-th component
si

g
, an input goal conguration with i-th component
gi

min
, stored minimum components of a bounding box with i-th component
mini

max
, stored maximum components of a bounding box with i-th component
maxi
n, number of DOFs of the manipulator
flag, ag for checking previously intersected failure hyperplanes
total, total failure hyperplane intersections found
53
(1) for i = 1 to n Adjust the bounding box bound-
(2) if (
gi
<
mini
) aries with respect to the input goal
(3)
mini
=
gi
conguration. Next check if any
(4) if (
gi
>
maxi
) failure hyperplane that was not
(5)
maxi
=
gi
previously intersected now inter-
(6) i = 0, total= 0 sects
g
. An intersection of a fail-
(7) do ure hyperplane with
g
is deter-
(8) i = i + 1 mined when
si
lies within the mi-
(9) flag= 1 nimum and maximum boundaries
(10) for j = 1 to n
fp
of the corresponding bounding box.
(11) flag = (i
Hj
= i) It is possible that an input
g
is in-
(12) if (flag== 1) and (
si

mini
) and (
si

maxi
)
(13) n
fp
= n
fp
+ 1 tersected by more than one failure
(14) i
Hn
fp
= i hyperplanes.
(15) total = total + 1
(16) while (i < n)
(17) if (total> 0)
(18) flag = 1
(19) return i
H
, n
fp
,
min
,
max
,flag
The function CheckBoundingBox(i
H
, n
fp
,
min
,
max
,
s
,
g
) adjusts the boundary val-
ues of the current bounding box with respect to the input goal conguration
g
. An
intersection with a failure hyperplane that was not previously intersected is determined if
a component of the start conguration lies within the current bounding box of
g
, that is,

mini

si

maxi
. (41)
It is possible that an input
g
is intersected by more than one failure hyperplanes. When
at least one intersection is found, the failure hyperplane indices are returned together with
the incremented number of failure hyperplane intersections, the adjusted bounding box
boundaries, and the corresponding ag.
54
4.2.4 Step 4. Testing the Sucient Condition
When a {
s
,
g
} pair satises the necessary condition, the existence of a failure surface,
S, is tested on
s
and its corresponding parameterized
g
[0, 1]. This step is a function of
monotonic path generation and collision-detection algorithms. The pseudo code for the
main function to check for the existence of a failure surface is shown in Fig. 13.
Figure 13: Pseudo code to check for the existence of a failure surface.
CheckFailureSurface(i
H1
, i
Hn
,
H
,
s
,
g
[0, 1])

s
, input obstacle-free start conguration

g
[0, 1], parameterized
g
(v) where v [0, 1]
i
H1
, stored index j of H
0
j
(
s
)
i
Hn
, stored index k of H
1
k
(
s
)
n
g[0,1]
, the number parameterized points on
g
[0, 1]
n
fp
, number of failure hyperplanes H
i
(
s
) intersecting
g
[0, 1]
k, index of parameterization of
g
[0, 1] which goes from 1 to n
g[0,1]

(k)
g
, a conguration on
g
[0, 1] at its k-th parameterization

Hj
, the value of
(k)
g
at the intersection of
g
[1, 0] with a failure hyperplane
such that the index j indicates the order of intersection

H
, a matrix whose columns are
H1
, . . . ,
Hn
fp
i
stop
, number of failure hyperplane intersections encountered along
g
[0, 1] as
parameter v goes from 1 to a value just before the failure surface
identication exits
k
first
, value of index k when the linear path connection rst failed to connect
i
quad
, index of the joint component to be parameterized quadratically
check
ln
, ag for collision detection in linear path generation
check
qd
, ag for collision detection in quadratic path generation
n, number of DOFs of the manipulator
55
(1) i
stop
= k
first
= k = 0,check
ln
=check
qd
= i
quad
= 1
(2) do At parameter k, a straight line
(3) k = k + 1 path is rst attempted to connect
(4) do
s
to
(k)
g
. If collision is detected,
(5) if (
(k)
g
==
Hj
) and (j > i
stop
) monotonic quadratic connection is
(6) i
stop
= i
stop
+ 1 attempted next. Throughout the
(7) while (j < n) entire failure surface, only a single
(8) if (check
qd
== 1) joint component is parameterized
(9) check
ln=
LinearPath(k,
s
,
(k)
g
) quadratically. If the monotonic
(10) if (check
ln
== 0) quadratic path connections for a
(11) check
qd
== 0 particular joint component is un-
(12) if (i
quad
= i
H1
) and (i
quad
= i
Hn
)
(13) check
qd
=QuadraticPath(k, i
quad
,
s
,
(k)
g
)
(14) if (k
first
== 0) successful, the next joint compo-
(15) k
first
= k 1 nent is parameterized quadratically
(16) if (check
qd
== 0) starting from the k index when the
(17) i
quad
= i
quad
+ 1 linear path connection was rst un-
(18) k = k
first
successful.
(19) while (k < n
g[0,1]
) and (i
quad
n)
(20) return check
qd
, i
stop
In the failure surface identication linear and quadratic parameterizations are used. At
the k-th parameterization where k goes from 1 to n
g[0,1]
, a linear path connection from

s
to
(k)
g
is attempted rst. If collision is detected, a monotonic quadratic path is at-
tempted next. In the monotonic quadratic parameterization, only a single joint component
is consistently parameterized throughout the entire failure surface. The rest of the joint
components are linearly parameterized. Consistently parameterizing a particular joint com-
ponent throughout the entire failure surface ensures that the path connections from
s
to

(k)
g
for k [1, n
g[0,1]
] are devoid of internal local minimum or maximum.
56
If no obstacle-free monotonic quadratic path is found using a particular joint component,
the next joint component is used in the monotonic quadratic parameterization starting
from the rst k index where the linear path connection failed. The i
stop
value represents
the number of failure hyperplane intersections encountered along
g
[0, 1] until the failure
surface identication exits. It is incremented when a point
(k)
g
is equal to the failure
hyperplane intersection at index j and the intersection index is greater than the number
of intersections encountered. If the monotonic quadratic path attempts for the allowable
joint components were unsuccessful, the failure surface identication is stopped and the
corresponding ags are returned. Fig. 14 shows the pseudo code for connecting linear paths
between two congurations.
Figure 14: Pseudo code for connecting linear paths from two input congurations.
LinearPath(k,
1
,
2
)
k, k-th index of the parameterized
g
[0, 1]

1
, a starting input conguration

2
, an ending input conguration
b
k
, a global linear coecient at k-th parameterization of
g
[0, 1]
c
k
, a global constant coecient at k-th parameterization of
g
[0, 1]
u, a linear path parameter such that u [0, 1]
, conguration computed from the linear parameterization
free, ag for collision detection
57
(1) b
k
=
2

1
The coecients of the linear path
(2) c
k
=
1
parameterization are dened with
(3) free= 1 respect to the two input congu-
(4) for u = 0 to 1 rations. Based on these coecients
(5) = b
k
u +c
k
a linear parameterization is comput-
(6) free =CheckFreeSpace() ed and the corresponding congu-
(7) return free ration at each parameterization is
checked against obstacles.
The function LinearPath(k,
1
,
2
) connects a straight-line path from a starting input
conguration,
1
, to an ending input conguration,
2
. Computation for the coecients of
the linear parameterization is based on these two input congurations. Each parameterized
point computed from the linear coecients is checked against collision. The function to
check for collision is the same function used in Step 2. Fig. 15 shows the pseudo code for
generating monotonic quadratic paths.
Figure 15: Pseudo code for connecting monotonic quadratic paths from two input cong-
urations.
QuadraticPath(k, i
quad
,
1
,
2
)
k, k-th index of the parameterized
g
[0, 1]
i
quad
, index of the joint component to parameterize quadratically

1
, a starting input conguration

2
, an ending input conguration

1i
quad
, the i
quad
joint component of
1

2i
quad
, the i
quad
joint component of
2
a
k
, a global quadratic coecient at k-th parameterization of
g
[0, 1] with i-th
component a
ki
b
k
, a global linear coecient at k-th parameterization of
g
[0, 1] with i-th
component b
ki
58
c
k
, a global constant coecient at k-th parameterization of
g
[0, 1]
n
deg
, degree of resolution of the bisection search
u, a quadratic path parameter such that u [0, 1]
, conguration computed from the quadratic parameterization
free, ag for collision detection
RES, a predened resolution value
(1) n
deg
= 0 Only a single joint component corres-
(2) a
k
= 0 ponding to i
quad
is parameterized
(3) b
k
=
2

1
quadratically. A bisection method is
(4) c
k
=
1
used to search for the monotonic
(5) do quadratic coecients up to a prede-
(6) n
deg
= BisectionPick(n
deg
, k, i
quad
,
1i
quad
,
2i
quad
)
(7) free = 1 ned resolution while the rest of the
(8) for u = 0 to 1 joint components remain linear in pa-
(9) = a
k
u
2
+b
k
u +c
k
rameter. This function exits imme-
(10) free = CheckFreeSpace() diately when an obstacle-free path is
(11) while (n
deg
< RES) and (free== 0) found.
(12) return free
The function QuadraticPath(k, i
quad
,
1
,
2
) connects a parameterized monotonic quadratic
path for the i
quad
joint component from
1i
quad
to
2i
quad
. The rest of the joint components
have linear parameterization. The monotonic quadratic coecients for the i
quad
joint com-
ponent are searched through bisection method. Collision detection is checked at every
parameterized conguration of the monotonic path generated from the selected coecients.
When an obstacle-free monotonic path is determined, this function is exited and the corre-
sponding ag is returned. Otherwise, every monotonic quadratic path generated from the
bisection search is tested for collision until the predened maximum resolution is reached.
(Derivation of monotonic curves are discussed in the next section.) The pseudo code for
59
performing the bisection search for the monotonic quadratic path is shown in Fig. 16.
Figure 16: Pseudo code for searching the monotonic quadratic coecients using bisection
method.
BisectionPick(n
deg
, k, i,
1
,
2
)
n
deg
, degree of resolution of the bisection search
k, k-th index of the parameterized
g
[0, 1]
i, index of the joint component where a quadratic coecient is searched

1
, a joint component of a start conguration

2
, a joint component of a goal conguration
d, range of values to be searched for a possible quadratic coecient
n
c
, counter used to pick odd-indexed increments for the quadratic coecient
a
ki
, i-th component of the global quadratic coecient, a
k
b
ki
, i-th component of the global linear coecient, b
k
stage, ag to switch between stages of bisection search
60
(1) static stage, n
c
In searching for a value of the quadratic
(2) d = 2(
2

1
) coecient, a
ki
, via bisection method, the
(3) if (n
deg
== 0) acceptable range of possible values is
(4) stage = 0 computed as d = 2(
2

1
). The val-
(5) switch stage ues at the end-points of this range are
(6) case 0 : used as the rst two values of a
ki
. The
(7) n
c
= 0 range is then divided into two and the
(8) a
ki
= (
2

1
) value at the division is used. To fur-
(9) stage = 1 ther search for values, the current reso-
(10) break lution at which values are selected from
(11) case 1 : the range of feasible values is further
(12) a
ki
= (
2

1
) +
(2nc+1)d
2
n
deg
divided into two and the odd-indexed
(13) n
c
= n
c
+ 1 divisions are used. (The even-indexed
(14) if (2n
c
+ 1 > 2
n
deg
) divisions have been previously used.)
(15) n
deg
= n
deg
+ 1 At each chosen value of a
ki
, the corre-
(16) n
c
= 0 sponding b
ki
is computed.
(17) break
(18) b
ki
=
2

1
a
ki
(19) return n
deg
The function BisectionPick(n
deg
, k, i,
1
,
2
) searches for a possible value of a
ki
in a
single-dimensional space which is a line segment whose length is given by d = 2(
2

1
).
The rst two values of a
ki
are the end-points of the line segment given as (
2

1
) and
(
2

1
). The line segment is then bisected and the value at the bisection point is used
next. To search for further values of a
ki
, each division of the line segment is further bisected
and the odd-indexed bisection points are used. The even-indexed bisection points of the
newly performed bisection process have been previously used. The odd-indexed bisection
points in the line segment are searched until the new odd-indexed bisection point exceeds
the acceptable range. In this case, a new bisection process is performed. At each value of
61
(0,
2

1
)
range of feasible
coefficients
(
2

1
, 0) (
2

1
, 0)
a+b=
2

1
a
2a+b=0
b
(0, 2
2
2
1
)
Figure 17: A graphical representation of a set of feasible values of a and b coecients for
a monotonic quadratic polynomial where
2
>
1
and its derivative is greater than or equal
to zero.
a
ki
the corresponding b
ki
is computed.
A graphical representation of the feasible range of coecient values where d = 2(
2

1
)
such that a [d/2, d/2] and b [0, d]is shown in Fig. 17. The graph of the coecients
shown is for a monotonic quadratic curve where
2
>
1
and its derivative is greater than
or equal to zero such that following set of constraints hold: a + b =
2

1
, b 0, and
2a + b 0.
4.3 Generating Monotonic Curves
Choosing the acceptable values for the coecients of the monotonic polynomials are
discussed in the following. A polynomial p(t) is monotonic on a region of interest provided
that its derivative, p

(t), does not change sign in that region. In particular, a polynomial

p(t) is monotonic on [0, 1] provided that the zeros of p

(t) do not occur in the open interval

(0, 1). A linear parametric equation p(t) = at + b is monotonic throughout because its
derivative p

(t) = b is constant.
62
Given a component of a start conguration
si
and a component of a goal conguration

gi
, it is easy to see that any quadratic polynomial p(t) satisfying the constraints p(0) =
si
and p(1) =
gi
has the form
p(t) =
si
(1 t) +
gi
t + t(1 t) (42)
where the parameter can be any real number. Now a polynomial is monotonic on the
closed interval [0, 1] if and only if its derivative does not change sign on the open interval
(0, 1). Since the derivative of a quadratic polynomial represents the equation of a line, it
follows that one only needs to check the endpoints of the interval [0, 1] to determine the set
of s that makes (42) monotonic on [0, 1]. Therefore, for (42) to be monotonic on [0, 1],
p

(0) =
gi

si
+ and p

(1) =
gi

si
should not have opposite signs. This is clearly
true if and only if || |
gi

si
|.
For completeness, monotonic cubic polynomials will be discussed. There are two cases
characterizing when p(t) is a cubic polynomial that is monotonic on [0, 1] with end conditions
p(0) =
si
and p(1) =
gi
.
Case 1. The roots of p

(t) are complex or occur as a double root. This occurs if and only
if p(t) is monotonic everywhere. In this case p

(t) = K(t
2
+ 2at + a
2
+ b
2
) where a is any
real number and b 0. One can parameterize a and b by a = tan
1
for /2 <
1
< /2
and b = tan
2
for 0
2
< /2. Integrating p

(t) and taking into account the endpoints

results in
p(t) = (
gi

si
)
t
3
+ 3at
2
+ 3(a
2
+ b
2
)t
1 + 3a + 3(a
2
+ b
2
)
+
si
. (43)
Case 2. The roots of p

(t) are real and do not occur in (0, 1). Then p

(t) = K(t t
1
)(t t
2
)
where t
i
(0, 1), i = 1, 2. There are three subcases: (2a) t
1
, t
2
(, 0]; (2b) t
1
(, 0]
and t
2
[1, ); and (2c) t
1
, t
2
[1, ). These cases can be parameterized in a manner
similar to that used in Case 1. Integrating p

(t) and satisfying the endpoint conditions gives

p(t) = (
gi

si
)
2t
3
3(t
1
+ t
2
)t
2
+ 6t
1
t
2
t
2 3(t
1
+ t
2
) + 6t
1
t
2
+
si
. (44)
Any joint component can be parameterized as quadratic or cubic as long as it is consis-
tent throughout a single failure surface. If the joint component correspond to the xed joint
63
Figure 18: Motion of joint 2 of a 3-DOF planar robot moves the whole robot structure
away from the most critical obstacle (colored black). The monotonic quadratic or cubic
parameterization of a single joint component, while the rest of the joints has linear param-
eterization, will try to nd dierent combinations on the rate of motion for each robot joint
that will allow such a phenomenon to happen.
component associated with either H
0
j
(
s
) or H
1
k
(
s
) the joint component remains constant
because
si
=
gi
(v) = S
i
(u, v) for u [0, 1]. The single joint component where quadratic or
cubic parameterization is performed can be visually imagined as the joint that will move at
a monotonic rate while the rest of the robot joints will move at a constant rate. By trying a
dierent joint to parameterize at a time, this would provide dierent combinations of rates
of motion for each robot joint that will move the most critical robot link (the link that is
closest to the most critical obstacle) and the rest of the robot structure away from the most
critical obstacle. This is shown in Fig. 18 where the most critical obstacle is colored black.
Consistently parameterizing one joint component in terms of quadratic or cubic param-
eterization while the rest of the joint components are linearly parameterized ensures that
the resulting failure surface generated is devoid of internal local minimum or maximum.
64
CHAPTER V
SIMULATION RESULTS
The proposed method of guaranteeing failure tolerance for redundant manipulators
operating amongst obstacles were implemented using a 3-DOF planar robot and a 7-DOF
Mitsubishi PA-10 robot. The results are presented in this chapter. The 3-DOF planar
robot simulation was performed with two sets of scenarios where a scenario is consists of
the given x
s
, x
g
, and the specied locations of the obstacles. The rst set of scenarios are
purposely chosen such that the robot will have diculty in nding a solution. The second
set of scenarios that is composed of 11,000 experiments, are randomly generated to test
the feasibility of implementing the proposed solution being presented. The PA-10 examples
present three randomly chosen scenarios.
5.1 Illustrative Examples on a 3-DOF Planar Robot
The 3-DOF planar robot that is used as an example has equal link lengths of 100
units and is required to move from x
s
to x
g
in the workspace as shown in Fig. 19. Each
disk obstacle has a diameter of 40 units. In Figs. 19(a) and 19(b), the start position is
x
s
= [200, 0]
T
while in Fig. 19(c) the start position is x
s
= [250, 0]
T
. In all cases the goal
position is x
g
= [100, 0]
T
. Note that the desired positions were chosen along x = [x
1
, x
2
]
T
such that x
2
= 0. By choosing a dierent value of x
2
, as long as the chosen position is at
the same distance away from the manipulator base, the shape of the self-motion manifold(s)
would the same in the C-space except for shifted values in the
1
component.
The obstacles were selected in such a way that the resulting scenario would be dicult to
nd a failure surface, S, from a feasible start conguration,
s
. For each scenario in Fig. 19,
the robot conguration shown is the feasible start conguration,
s
, that has a corresponding
failure surface, S. In this rst set of examples, only straight-line paths are used to connect
the obstacle-free start conguration,
s
, to points on the continuous obstacle-free portion
65
Figure 19: A 3-DOF planar robot with three distinct sets of obstacles in the environment.
For each scenario, the robot conguration shown is the feasible start conguration
s
for
which a failure surface S is found. Scenarios (a) and (b) have x
s
at [200, 0]
T
and x
g
at
[100, 0]
T
. Scenario (c) has x
s
at [250, 0]
T
and x
g
at [100, 0]
T
. The robot has equal link
lengths of 100 units and each obstacle has a diameter of 40 units. In all cases the robot can
reach the desired position x
g
from the shown start conguration
s
despite any single joint
failure.
Table 1: The set of start congurations shown in Fig. 19 with the corresponding coordinates
of the failure planes that intersect the continuous obstacle-free portion of the goal self-motion
manifold,
g
.
Fig. Start Conguration Failure Plane Hi(s) Intersection with
g
19 s1 s2 s3 H11 H12 H13 H21 H22 H23 H31 H32 H33
(a) -73.3 114.0 -23.0 -73.3 179.4 -105.9 -113.4 114.0 65.8 -154.0 178.6 -23.0
(b) 66.7 -119.1 45.1 66.7 -179.4 112.8 118.7 -119.1 -60.2 133.3 -179.2 45.1
(c) 23.5 -73.0 70.9 23.5 -178.6 155.3 -0.3 -73.0 179.4 108.9 -179.4 70.9
of the goal self-motion manifold,
g
, in order to identify a failure surface, S. The statistical
study on the planar 3-DOF robot and the example on the Mitsubishi PA-10 robot both use
straight line paths and quadratic paths in the failure surface identication.
Table 1 shows the values of the start conguration
s
= [
s1
,
s2
,
s3
]
T
correspond-
ing to each scenario in Fig. 19. The planes H
1
(
s
), H
2
(
s
), and H
3
(
s
) are the cor-
responding failure planes corresponding to the obstacle-free start conguration,
s
, that
intersect the continuous obstacle-free portion of the goal self-motion manifold,
g
, at

Hi
= [
Hi1
,
Hi2
,
Hi3
]
T
where i = 1, 2, 3.
Obstacles in scenario Fig. 19(a) were chosen in a more random manner such that the
robot squeezed its way through to nd a feasible start conguration
s
where a corre-
sponding S exists. Obstacles in scenario Fig. 19(b) were chosen in such a way that they
66
Figure 20: A projection of the self-motion manifolds, drawn in broken lines, for x
g
=
[100, 0]
T
(outermost oblong), x
s
= [200, 0]
T
(middle oblong), and x
s
= [250, 0]
T
(innermost
oblong). The start congurations,
s
s, that corresponds to each scenario in Fig. 19 and
their corresponding failure planes intersections with the continuous obstacle-free portions of
the goal self-motion manifolds,
g
s are shown. The surface bounded by solid lines represent
the failure surface, S, for each
s
.
will have a more linear arrangement such that the robot is in a walled or pipe-like
environment. Obstacles in scenario Fig. 19(c) were chosen randomly as in Fig. 19(a), how-
ever, the start conguration is further away from the robots base. Notice that the dicult
obstacles in Fig. 19(c) are wider apart from each other compared to the dicult obstacles
in Fig. 19(a). The reason is because in Fig. 19(c), the robot links are more stretched out
at the start position, x
s
, and if a joint failure occurs at the start conguration, the robot
would need a bigger obstacle-free sweep area for it to reach the goal position, x
g
.
Fig. 20 shows a projection of the C-space self-motion manifolds, drawn in broken lines,
corresponding to workspace positions in Fig. 19. The goal position x
g
= [100, 0]
T
corre-
sponds to the outermost oblong self-motion manifold, x
s
= [200, 0]
T
corresponds to the
center oblong self-motion manifold, and x
s
= [250, 0]
T
corresponds to the innermost ob-
long self-motion manifold. Also shown are the start conguration,
s
, for the each scenario
in Fig. 19 and its corresponding failure planes intersections,
Hi
s, with the continuous
obstacle-free portion of the goal self-motion manifold,
g
. Notice that the straight lines
67
connecting a
s
to a
Hi
are along the failure planes corresponding to
s
. The failure
planes are orthogonal to each other but do not appear orthogonal in the gure because the
projection is taken at an angle. The failure surface S for each
s
is the surface bounded by
the solid lines. Indeed, as shown in Fig. 20, a failure surface S exists that would guarantee
successfully reaching the goal despite obstacles in the environment and despite any single
joint failure at any time.
5.2 Statistical Examples on a 3-DOF Planar Robot
In this second set of experiments, the same 3-DOF planar robot with equal link lengths
of 100 units is used to explore the feasibility of the proposed approach. The same circular
disk obstacles with the same diameter of 40 units were used but the number of obstacles
was varied from zero to twenty in two-obstacle increments.
For each number of obstacles, experiments were performed for 1,000 randomly generated
scenarios. In each scenario, the locations for the corresponding number of obstacles are
randomly selected from a uniform distribution throughout the entire robot workspace. The
start workspace location, x
s
, is randomly selected from a uniform distribution of [100, 200]
along the x-axis, that is, at a distance that corresponds to between one and two-link lengths
away from the robot base. The goal workspace location, x
g
, is randomly selected to be
within a range of [0, 200] units from the range of start workspace locations (while restricting
the goal to be within the reachable workspace of the manipulator). Fig. 21 presents an
example of the start and goal locations generated for one thousand scenarios.
Ten examples from the set of 11,000 scenarios are presented in Fig. 22, where the number
of obstacles is varied from twenty to twelve in two-obstacle decrements. The top row of
this gure illustrates cases where the failure-tolerant path planning algorithm was able to
identify a failure surface, S, with the bottom row showing cases where it could not. The
necessary condition for a surface to exist was satised in all cases, with the manipulator
conguration illustrating one such feasible starting conguration,
s
.
Table 2 shows the values of the start conguration
s
= [
s1
,
s2
,
s3
]
T
corresponding
to the top row scenarios in Fig. 22. The failure plane H
i
(
s
) intersects the continuous
68
Figure 21: The workspace of a 3-DOF planar manipulator used for the simulation exper-
iments (all three link lengths are 100 units long). The one thousand start locations, x
s
,
are randomly generated within the range [100, 200] units along the x-axis (shown as a thick
bold line). The one thousand goal workspace locations, x
g
, (shown as dots) are randomly
generated to be within the range [0, 200] units away from the range of start locations (but
inside the reachable workspace). The center of the workspace is marked with a bold cross,
the workspace boundary with a solid line, and the boundary of the goal locations with a
dashed line.
(a) (b) (d) (c) (e)
x
x
g
s
x
x
g
s
x
x
g
s x
x
g
s
x
x
g
s
x
x
g
s
x
x
g
s
x
x
g
s
x
x
g
s
x
x
g
s
Figure 22: Several examples selected from the eleven thousand scenarios where the failure-
tolerant path planning algorithm was applied to a 3-DOF planar manipulator. The sub-
gures denoted (a) through (e) correspond to workspaces containing from twenty to twelve
obstacles, respectively. In each case, the top scenario represents one in which the algorithm
successfully identied a failure surface, S, whereas the bottom scenario represents a case
where such a surface could not be found. In all cases the start conguration shown for the
manipulator is one that satises the necessary condition for a surface to exist.
69
Table 2: The set of feasible obstacle-free start congurations,
s
, shown in the top tow of
Fig. 22 with the corresponding failure planes H
i
(
s
) intersecting a continuous obstacle-free
portion of the goal self-motion manifold,
g
, shown in units of degrees.
Top Start Conguration Failure Plane Hi(s) Intersecting
g
Row s1 s2 s3 H11 H12 H13 H21 H22 H23 H31 H32 H33
(a) -7.3 123.2 -166.5 -6.3 173.8 -128.9 39.3 123.1 -174.2 32.1 155.5 -168.3
(b) 38.0 -126.9 111.5 36.3 -155.6 163.0 26.6 -126.8 171.4 -43.6 62.4 110.3
(c) -64.7 61.8 75.9 -65.8 60.9 140.7 -71.6 63.6 136.0 102.2 164.4 77.1
(d) 4.5 -90.0 152.3 4.0 66.1 104.1 73.6 -88.3 170.2 47.5 8.5 150.5
(e) -71.6 75.3 58.5 -69.9 160.5 -2.7 -7.2 76.9 96.0 -40.8 114.9 57.1

s
0
1
0
0
1
1
(a)
(b)
(c)

3
Figure 23: Projections of the self-motion manifolds on the
2

3
-plane for the top row
scenarios of Fig. 22(a)-(c). The failure surface, S, is shown as a web-like network of paths.
The feasible
s
is shown as a cross on the self-motion manifold while the
g
is the curve on
the self-motion manifold between the points labeled 0 and 1. For the example shown
in Fig. 22(c), the goal manifold consists of two disjoint manifolds in the C-space because x
g
is less than one link length away from the robot base.
70
obstacle-free portion of the goal self-motion manifold,
g
, at
Hi
= [
Hi1
,
Hi2
,
Hi3
]
T
where
i = 1, 2, 3.
Fig. 23 shows projections of the self-motion manifolds in C-space for the top row sce-
narios of Fig. 22(a)-(c) where a failure surface exists. The cross on a self-motion manifold
denotes the feasible start conguration,
s
, for the corresponding scenario. The
g
that
satised the necessary condition is parameterized in terms of u [0, 1] and shown as the
curve on the self-motion manifold between the points labeled 0 and 1. The web-like
network of paths represents the failure surface for each of the corresponding scenarios. In
the following subsections, data gathered from these eleven thousand simulation experiments
will be presented according to the order in which these steps are performed in the algorithm
described in Sec. 4.1. In all cases the algorithm was executed on a computer with dual Intel
Xeon processors running at 2.4 GHz.
5.2.1 Computation of Self-Motion Manifolds
The rst step in the failure-tolerant path planning algorithm is to compute the self-
motion manifold(s) for both the start and goal workspace locations. These are computed by
identifying a single conguration on each disjoint manifold and then stepping along the man-
ifold (in two degree increments) by integrating the null vector of the manipulator Jacobian
matrix (which corresponds to the tangent of the self-motion manifold). The average lengths
for the sum of all manifolds corresponding to a workspace location are given in Table 3. The
length for the start manifolds are larger than those for the goal manifold because the start
workspace locations were intentionally restricted to a region of the workspace where these
manifolds are larger, and thus, the locations are more failure tolerant [23]. (A distance of
one link length from the base is optimally failure tolerant, i.e., all joints span their entire
range of motion, while the location on the workspace boundary is most failure intolerant,
i.e., its self-motion manifold consists of a single point.) The average computation time over
all manifolds was 41.9 ms.
71
Table 3: Computation of self-motion manifolds
Ave. Std. Dev.
Length of Ms (deg) 1043.95 4.97
Length of Mg (deg) 883.71 8.52
Time to compute Ms, Mg (ms) 41.90 5.88
5.2.2 Identifying Obstacle-Free Portions of M
s
and M
g
The next step is to determine the obstacle-free portions of the manifolds. This iden-
ties candidate feasible obstacle-free start congurations,
s
, and continuous obstacle-free
portions of the goal self-motion manifold,
g
, that can possibly satisfy the necessary con-
dition.
Fig. 24 shows the percentage of the start self-motion manifold M
s
and goal self-motion
manifold M
g
that are obstacle free as a function of the number of obstacles in the workspace.
As expected, the percentage of the obstacle-free self-motion manifold decreases as the num-
ber of workspace obstacles increases. Fig. 25 shows the time, in milliseconds, required to
determine the obstacle-free portion of the self-motion manifolds. As expected, the time
required to determine which portions of a manifold are obstacle free is a linear function of
the number of obstacles.
Percent of Manifolds that are Obstacle Free
0.0
20.0
40.0
60.0
80.0
100.0
0 2 4 6 8 10 12 14 16 18 20
No. of Obstacles
P
e
r
c
e
n
t
Figure 24: Percent of the self-motion manifolds that are obstacle free.
72
Time to Compute Obstacle-Free Manifolds
0.0
50.0
100.0
150.0
200.0
250.0
300.0
2 4 6 8 10 12 14 16 18 20
No. of Obstacles
M
i
l
l
i
s
e
c
o
n
d
s
Figure 25: Time needed to determine the obstacle-free portion of the start self-motion man-
ifold M
s
and the goal self-motion manifold M
g
as a function of the number of workspace
obstacles.
In many cases, one could avoid most of the computation time associated with checking
the entire manifold for collisions with obstacles by performing the necessary condition check
rst, and then verifying that the start conguration and corresponding portion of the goal
self-motion manifold are collision free. (This check is the same as determining if the goal
location, x
g
, is in the failure-tolerant workspace of the start location, x
s
, as rst considered
in [23].)
5.2.3 Checking the Necessary Condition
Table 4 presents the computational data associated with checking the necessary con-
dition. As the number of obstacles in the workspace increases, the percentage of cases that
satisfy the necessary condition decreases, reaching a minimum of 25% for the case with
twenty obstacles. For those cases where a
s
satised the necessary condition, we further
processed the start manifold to see what percentage of the start manifold would be able to
satisfy the necessary condition. (This is not required by the algorithm and the time required
to perform this computation was not included in the overall execution time data presented.)
73
Table 4: Computation to check the necessary condition
No. of Time N.C. Time N.C. % Cases % Ms
Obs. Sat. (ms) Not Sat. (ms) N.C. Sat. N.C. Sat.
0 0.73 4.39 69.3 25.4
2 0.84 4.59 64.8 20.5
4 1.13 5.03 57.9 16.5
6 1.13 4.86 52.2 15.4
8 1.05 4.57 48.9 12.4
10 1.33 5.13 45.8 10.0
12 1.48 5.35 38.2 7.6
14 1.50 5.43 36.7 6.3
16 1.84 6.15 30.8 5.9
18 1.90 5.56 27.6 4.1
20 2.92 5.53 24.8 3.7
Table 4 shows that this is also a monotonically decreasing function of the number of obsta-
cles in the workspace. Thus, it becomes increasingly more time consuming to identify a
s
that satises the necessary condition as the number of obstacles in the workspace increases.
This is illustrated by the fact that the computation time is a monotonically increasing func-
tion of the number of obstacles. This is true despite the fact that there is less and less of the
start manifold that is obstacle free (see Fig. 24) because an increasingly smaller percentage
of the obstacle-free manifold is able to satisfy the necessary condition. In contrast, the
time to compute that the necessary condition is not satised is relatively independent of
the number of obstacles. This at rst appears anomalous because one would expect this to
be a monotonically decreasing function due to a smaller percentage of the start manifold
needing to be checked (because less of it is obstacle free). However, this is oset by the fact
that larger and larger manifolds are now failing the necessary condition, thus keeping the
computation time relatively constant.
Fig. 26 shows the average length of the
g
from a (
s
,
g
) pair that satises the necessary
condition. This generally decreases as the number of obstacles increases due to the fact that
it is more dicult to have large
g
because they are required to be obstacle free. The size
of a
g
that satises the necessary condition is correlated to the distance between the start
and goal workspace locations so that, as expected, start and goal locations must be closer
together as more and more obstacles are added to the workspace.
74
Length of
g
0
50
100
150
200
250
300
0 2 4 6 8 10 12 14 16 18 20
No. of Obstacles
D
e
g
r
e
e
s
N.C. Satisfied
S.C. Satisfied
Figure 26: Average length of a
g
that satises the necessary condition and that also
satises the sucient condition as a function of the number of obstacles in the workspace.
5.2.4 Computing a Failure Surface
The nal step in the algorithm is to check the sucient condition by attempting to
compute a failure surface that guarantees the existence of a solution to the failure-tolerant
path planning problem. Once a feasible
s
is found from the previous step, the search
for a failure surface begins. A failure surface is identied by generating monotonic paths
that connect a feasible
s
to points on its corresponding
g
that satises the necessary
condition. (The
g
curve is discretized at a resolution of two degrees.) For each point on

g
the algorithm rst attempts to use a straight-line path. If this path is not obstacle free,
then it attempts to nd a monotonic quadratic path that is obstacle free up to a desired
resolution of in (42). If no such path can be found then the algorithm discards this
(
s
,
g
) pair and uses the next (
s
,
g
) pair that satises the necessary condition. If all
such pairs are exhausted without completing a failure surface then the algorithm terminates
with a message that it was unsuccessful.
It is interesting to note that once the necessary condition is satised, it is highly likely
that a failure surface will be found, i.e., this occurs 84% of the time. In addition, this
percentage is relatively independent of the number of obstacles that are in the workspace
75
Overall Percentage
0
10
20
30
40
50
60
70
80
0 2 4 6 8 10 12 14 16 18 20
No. of Obstacles
P
e
r
c
e
n
t
N.C. Satisfied
S.C. Satisfied
Figure 27: Percent of total cases where the necessary condition is satised and percent of
total cases where the sucient condition is satised, i.e., where a failure surface S is found.
as illustrated in Fig. 27 (except, of course, for the case of no obstacles). This is fortuitous,
because the construction of failure surfaces is by far the most time consuming portion of the
algorithm. The average time for computing a failure surface as a function of the number of
obstacles is given in the second column of Table 5. The overall average time was 1.5 s with
the maximum time to compute a failure surface over all scenarios was 78.0 s. Similarly, it
takes on average 1.8 s to exhaust all possible candidates in cases where a surface cannot
be found, with a maximum time of 66.7 s. Thus the time to evaluate surfaces is seldom
wasted, with the majority of the cases where no solution exists being identied in a matter
of milliseconds by the necessary condition test.
Because monotonic path generation represents the most time consuming portion of the
algorithm, an additional analysis was performed to determine the number and computa-
tional cost of linear paths versus quadratic paths. The data from this analysis is presented
in Table 5 where L denotes that only linear paths were tried by the algorithm, L+Q
denotes that both linear and quadratic paths were tried, and (L+Q)* represents the sub-
set of L+Q scenarios that correspond to cases where the L algorithm was also able to
compute a failure surface. It is interesting to note that nearly all (99.6%) of the paths in
failure surfaces are linear. Furthermore, the number of additional failure surfaces that are
76
Table 5: Computations when S is Found
No. of Overall Time (s) No. of Path Evaluations Di.
Obs. L+Q (L+Q)* L L+Q (L+Q)* L
0 0.04 0.04 0.04 246 246 246 0
2 0.35 0.34 0.41 658 651 755 5
4 0.78 0.75 0.75 818 793 800 4
6 1.16 1.10 1.07 844 793 752 4
8 1.49 1.42 1.32 818 778 718 2
10 1.77 1.61 1.35 775 698 569 5
12 2.19 2.21 1.78 815 823 630 8
14 2.28 2.26 1.62 718 709 481 2
16 2.68 2.49 1.90 743 682 496 5
18 2.15 2.16 1.72 503 505 379 4
20 1.89 1.87 1.40 381 376 257 1
identied when trying quadratic paths is minimal, i.e., always less than 1% (see the last
column in Table 5). Given that the use of quadratic paths results in higher execution times
(this is true for all cases except for four or fewer obstacles) and a minimal improvement
in the number of failure surfaces being identied, the benet of implementing higher-order
monotonic paths is questionable.
5.2.5 Comparison to Previous Approaches
It is interesting to compare the results of our approach with that in [28]. Both ap-
proaches are similar in that the set of monotonic curves generated by the algorithm pre-
sented here plays the same role as the connectivity graph in [28]. However, any workspace
trajectory generated by a path through the connectivity graph will result in the same end-
eector trajectory because this is required by the problem denition in [28]. While this is
appropriate for tasks that must have the end-eector follow a prescribed trajectory, it limits
the number of possible choices for failure-tolerant trajectories if the robot is only required
to perform a pick-and-place type operation. The method proposed here will produce an
innite number of possible obstacle-free, failure-tolerant paths from the start to the goal
1
because it does not constrain the end-eector trajectory.
Fig. 28 illustrates the broad range of end-eector trajectories that are failure tolerant
1
The specic trajectory that the robot follows can then be selected based on some secondary criterion.
77
(a) (b) (d) (c) (e)
x
s
x x
s
x
x
g
Figure 28: The subgures from (a) through (e) have the same start and goal workspace
locations where x
s
= [8.8, 188]
T
and x
g
= [42.5, 76.8]
T
. The shaded regions represent
the end-eector locations corresponding to dierent sets of monotonic curves that will take
the end-eector from x
s
to x
g
.
if the task is only constrained to a desired start and goal location rather than a complete
trajectory. Each of the subgures (a) through (e) show the mapping of a set of failure-
tolerant, monotonic curves in the conguration space to the workspace for the same start
and goal location. Clearly, relaxing the constraint on the end-eector trajectory makes it
much more likely that a collision-free, failure-tolerant path will exist.
5.3 Seven DOF Redundant Robot Example
Although, as mentioned earlier, the Mitsubishi PA-10 is not a fully kinematically
failure-tolerant robot because of its of joint four, it is still chosen as a platform for im-
plementation because it is the most common commercially available redundant robot. The
PA-10 has a worst-case global failure tolerance measure
min
= 0 due to joint four and is
thus intolerant to a failure in joint four. Physically this is due to the fact that joint four is
the only joint that can alter the distance between the wrist and the shoulder. Therefore,
for this example it will be assumed that there will be no failure in joint four and consider
planning a failure tolerant path for the remaining six joints. In addition, we do not consider
joint limits or self collisions. Finally, we assume that the end-eector is oset from the wrist
by 0.3 m.
Ten spherical obstacles with a diameter of 0.254 m (10 in) are randomly placed in
the PA-10 workspace with a given set of x
s
and x
g
. The equivalent M
s
and M
g
are
then computed and the corresponding
s
and
g
are respectively determined. A candidate
(
s
,
g
) pair that satises the necessary condition is identied when the failure hyperplane
78
z
Figure 29: The Mitsubishi PA-10 workspace with 10 randomly generated spherical obstacles
of diameter 0.254 m (10 in) for the corresponding x
s
and x
g
shown. The corresponding
solutions are shown in Table 6. The rst three vector components are the desired positions
in units of meters. The last three vector components are the desired orientations in units
of degrees expressed in terms of Euler angles using System I as discussed in [1].
79
Table 6: The set of solutions for the examples shown in Fig. 29 with their corresponding
feasible obstacle-free start conguration,
s
. The failure hyperplane H
i
(
s
) intersects
g
which is parameterized in terms of v [0, 1].
Fig. Feasible Obstacle-Free Start Conguration, s Hi(s) Intersecting
g
at v [0, 1]
29 s1 s2 s3 s4 s5 s6 s7 H1 H2 H3 H4 H5 H6 H7
(a) -11.5 75.1 43.8 -49.2 46.8 -117.9 27.4 1.0 0.6 0.2 n.a. 0.4 0.8 0.0
(b) -6.4 -76.6 50.7 52.1 33.0 110.1 28.7 0.0 0.8 0.2 n.a. 0.4 0.6 1.0
(c) -0.5 17.5 235.7 -87.6 15.1 -128.1 191.1 0.6 0.8 1.0 n.a. 0.4 0.2 0.0
H
i
(
s
) intersects
g
for i = 1, . . . , n. Using this (
s
,
g
) pair, the algorithm searches for
the existence of a failure surface, S. This is done by connecting
s
to points on
g
. For
each point on
g
, the algorithm rst attempts to use a linear path. If the linear path is
not obstacle free, it tries to use a monotonic quadratic path using the possible values of
in (42) until an obstacle-free monotonic quadratic path is found. When all the possible
values of are tried and no such path can be found, the algorithm discards this (
s
,
g
)
pair and uses the next (
s
,
g
) pair that satises the necessary condition. If all such pairs
are exhausted without completing a failure surface, S, then the algorithm terminates with
a message that is was unsuccessful. If the path connections from
s
to all points on
g
parameterized between [0,1] are obstacle free, a failure surface, S, is successfully identied.
Three examples are shown in Fig. 29 where the corresponding failure surfaces, S, are
successfully found. The rst example shows the PA-10 with a start and goal end-eector
orientations facing downward with the corresponding desired positions. The second example
shows a start and goal end-eector orientations facing forward, while the third example
shows a start end-eector orientation facing downward and an arbitrary nal end-eector
orientation for the corresponding desired positions. The set of solutions are shown in Table 6
with the corresponding feasible
s
. The failure hyperplane H
i
(
s
) intersects
g
at the
shown values of
g
parameterized on the interval [0, 1]. Recall that the failure hyperplane
corresponding to joint four, H
4
(
s
), does not intersect
g
which thus makes joint four a
critical manipulator joint. Fig. 30 shows the corresponding failure surface, S, for example
(a). Fig. 31 shows snapshots of the PA-10 amongst obstacles while going through a portion
of
g
in example (c).
80

g
/2
/2
/2

7
/4
/2

5
1 /2
0
1
0
0
1
Figure 30: The failure surface for example (a), shown as a web of paths in the conguration
space, with projections from joint axes 1 and 3, 2 and 7, and 5 and 6. The projections are
shown in the same scale with units of radians. The bold curves represent portions of M
g
,
while the less thick curves represent portions of M
s
. The axes shown are translated from
the origin to the feasible
s
. Its corresponding
g
, which is parameterized in terms of
v [0, 1], is the curve between the points labeled 0 and 1.
81
Figure 31: Snapshots of the Mitsubishi PA-10 robot amongst obstacles while going through
a portion of
g
for example (c).
82
CHAPTER VI
IDENTIFYING THE FAILURE-TOLERANT
WORKSPACE BOUNDARIES OF A KINEMATICALLY
REDUNDANT MANIPULATOR
The failure-tolerant path planning strategy that was discussed in the rst part of this
work took into consideration discrete start and goal workspace locations, such that the goal
location was guaranteed reachable for locked-joint failures in the presence of obstacles. In
this chapter a method will be presented where a region in the workspace of a given redun-
dant robot is identied, such that every location within this region is guaranteed reachable
before and after any locked-joint failures. The existence of such a region, called a failure-
tolerant workspace, will be guaranteed by imposing a suitable set of articial joint limits
prior to a failure. Conditions are presented that characterize end-eector locations in this
region. Based on these conditions, a method is presented that identies the boundaries of
the failure-tolerant workspace. Optimized failure-tolerant workspaces for a three degree-of-
freedom planar robot are presented.
6.1 The Imposition of Articial Joint Limits
While kinematic redundancy allows for the possibility of fault tolerance to locked-joint
failures, if a joint is locked at a particularly inconvenient joint value, the manipulators
ability to accomplish its task can be severely compromised. An extreme example of this
is the planar 3R manipulator with unit length links. If either the second or third joint
is locked at 180 degrees, the end eector is constrained to lie on the unit circle. Another
example illustrating the severity of locked-joint failures is a 7R anthropomorphic arm. Since
the elbow joint is the only joint that changes the distance between the shoulder and the
83
wrist center, the robot is intolerant to locked-joint failures of the elbow.
One approach to guaranteeing failure tolerance is to add enough kinematic redundancy
to compensate for a locked-joint failure. It is shown in [32] that when no joint limits are
specied, at least two DORs are necessary and sucient to have a physically meaningful
failure-tolerant workspace. The authors further showed that a robot with a single DOR can
perform the required failure-tolerant task only when the path and redundancy resolution
algorithm are given.
A more practical problem is to improve the fault tolerance of an existing robot geometry,
and nding ways of identifying its failure-tolerant workspace is a more practical approach.
By judiciously choosing suitable joint constraints, one can signicantly increase the ma-
nipulators failure-tolerant workspace even if the manipulator only has a single degree of
redundancy. This approach was rst proposed in [23] where workspace locations can be
guaranteed to be within the failure-tolerant workspace by using the bounding boxes of the
self-motion manifolds corresponding to the workspace locations. The intersection of the
bounding boxes serves as articial joint limits, that will guarantee reachability between
specic task points after a joint failure.
The approach that we introduce here is also based on the idea of using articial joint
limits. However, rather than guaranteeing reachability between specied workspace points,
this work focuses on nding the boundaries of a failure-tolerant workspace. Articial joint
limits provide the necessary constraints prior to a failure to avoid those congurations where
a failure can have a detrimental eect. A proper choice of these limits can result in a larger
failure-tolerant workspace. Thus, the crux of our approach is that making a compromise in
the pre-failure workspace by introducing joint limits can help insure a suitable post-failure
workspace. It is assumed that once a joint failure occurs, that joint is immediately locked.
In this case, the articial joint limits are released and the remaining healthy joints are
allowed to freely move within their physical joint limits.
In the next section, the fault tolerant workspace problem is mathematically formulated.
In Section 3, the problem of identifying the pre-failure workspace is described. The bound-
aries of this workspace are identied by kinematic singularities and joint-limit singularities.
84
New techniques for identifying workspace boundaries due to multiple joint limits are de-
scribed. In Section 4, conditions for the fault tolerant workspace are given. These conditions
are used in Section 5 to identify potential boundaries for the fault tolerant workspace. The
technique is illustrated with an example in Section 6, and conclusions appear in Section 7.
6.2 Problem Formulation
Let the kinematic function mapping the joint space C R
n
to the workspace W R
m
be denoted by f : C W. In this work, we will assume that the conguration space C has
the form C
B
= B
1
B
n
. If joint i has no physical joint limits, B
i
= R, and if joint
i does have physical joint limits, B
i
= [b
i
,

b
i
] where b
i
<

b
i
. Initially we introduce articial
limits for each joint so that the i-th joint q
i
A
i
= [a
i
, a
i
]. If it can be safely assumed that
joint i will not fail then we can set A
i
= B
i
. The set C
A
= A
1
A
n
denotes the region
of the conguration space corresponding to the articial joint limits. The joint space prior
to a failure is then simply C
A
. Once a locked-joint failure occurs, the articial joint limits
are released and the robot is constrained to operate on a failure-induced hyperplane. This
of course has a signicant impact on the resulting reachable workspace. There are generally
end-eector locations that were reachable prior to the failure that are no longer reachable
after a failure. There may also be areas of the workspace that were formerly unreachable
but that, in spite of the locked joint, become reachable after releasing the articial joint
limits of the non-failed joints. The fault tolerant workspace is dened as the part of the
workspace that is reachable prior to and after any single locked-joint failure where the joint
failure can occur at any conguration in C
A
.
To illustrate the signicance of the fault tolerant workspace, consider a planar 3R manip-
ulator with equal length links. Without articial or physical joint limits, the fault tolerant
workspace is quite small. This can be clearly seen in Fig. 32, where two congurations cor-
responding to the same end-eector location on the unit circle are shown. While the rst
conguration is relatively fault tolerant to a locked-joint failure, the second conguration
is fault intolerant to joint 3 being locked, in which case the end eector is constrained to
85
remain on the dashed unit circle shown in the gure. In fact, it was shown in [23] that this
unit circle is the only region of the workspace that is guaranteed to be reachable following
any possible locked-joint failure. However, it will be shown in Section 6 that the fault toler-
ant workspace can be signicantly increased by simply enforcing articial joint limits that
can be released after a failure.
x
y

2
=-60

1
=60

3
=-120
L
L
L
L
L

1,

2
=0
2 3 1
2
1
3

3
=180
Figure 32: Two congurations of a planar 3R robot with equal link lengths of L meters.
The congurations shown are from an innite family of congurations resulting in the end-
eector position [L, 0]
T
. The rst conguration, = [60

, 60

, 120

]
T
, is fault tolerant
but the second conguration, = [0

, 0

, 180

]
T
, is fault intolerant to a locked-joint failure
in the third joint as this will restrict the end eector to remain on the circle shown regardless
of the values of the remaining two healthy joints.
In some cases, only certain joints are prone to failures. Let failure index set F
{1, 2, . . . , n} denote the joint labels of the failure-prone joints. We will assume that those
joints that are not contained in F will remain healthy throughout the robots mission. Our
goal then is to determine the fault tolerant workspace corresponding to at most one locked-
joint failure where any joint i F can fail. Mathematically, this problem can be formulated
in the following way. Prior to a joint failure, the robots operating conguration space is
C
A
and the pre-failure workspace is given by
W
0
= f (C
A
) = {x = f (q) | q C
A
}. (45)
If the i-th joint is locked at q
i
=
i
and the remaining articial joint limits are released, the
86
resulting reduced conguration space is given by
i
C(
i
) = {q C
B
| q
i
=
i
}. (46)
Geometrically, one can consider
i
C(
i
) to be the intersection of the hyperplane given by
q
i
=
i
with the feasible conguration space C
B
. Because the articial joint limits were
enforced prior to the failure, we have that a
i

i
a
i
. It is assumed that the failure can
occur anywhere in this interval and the joint is locked at that conguration. Hence, the
guaranteed workspace following a locked-joint failure of joint i subject to the articial joint
limits is
W
i
=

a
i

i
a
i
f (
i
C(
i
)). (47)
The fault tolerant workspace is then the intersection of the pre-failure workspace W
0
and
the various post-failure fault tolerant workspaces W
i
, i F:
W
F
=

iF{0}
W
i
. (48)
Our goal is to determine W
F
.
Unfortunately, nding W
F
directly is generally impossible, so the approach taken here
will be to identify necessary conditions for its boundaries. Recall that a boundary point of a
subset S of R
m
is a point x R
n
such that every open neighborhood of x contains at least
one point in S and at least one point not in S. Although the boundary of a general point
set can be quite complicated, the boundary sets considered in this work are simple and cor-
respond to simple curves or hyper-surfaces, depending on the dimension of the workspace.
These boundaries are determined by rst identifying candidate boundary sets. These can-
didate boundary sets correspond to a limitation of the manipulators ability to move its end
eector arbitrarily. In particular, the candidate workspace boundaries prior to a failure are
related to the concepts of kinematic singularities and joint-limit singularities. In the next
section, we will describe how one can identify these candidate boundaries. Conditions are
then introduced in Section 4 and are subsequently applied in Section 5 to identify the fault
tolerant workspace boundaries.
87
6.3 Identifying the Pre-failure Workspace Boundaries
Before developing criteria for identifying the fault tolerant workspace boundary, we
discuss the problem of identifying the workspace boundary of a healthy robot without any
failures. These boundaries are located by identifying two types of singularities: kinematic
singularities and joint-limit singularities.
The local motion of the end eector is characterized by the Jacobian equation
v = J q (49)
where v denotes the end-eector velocity, q denotes the joint velocity, and J denotes the
manipulator Jacobian. If the manipulator Jacobian has full rank and the joints are un-
constrained, then the end eector can move locally in any direction. In this case, the end
eector is at an interior point of the workspace. If the manipulator Jacobian does not have
full rank, i.e., if the robot is in a kinematic singularity, then the end eector does not locally
have full motion control. This occurs for example at a reach singularity. Note that not all
kinematic singularities correspond to workspace boundaries. However, since the workspace
boundaries of a manipulator without joint limits can only occur at kinematic singularities,
kinematic singularities can be used to identify candidate workspace boundaries.
When the manipulator has joint limits, the columns of the manipulator Jacobian corre-
sponding to the constrained joints can only contribute in one direction (a positive amount
if the joint is in a lower limit and a negative amount if the joint is in an upper limit). In
some cases, this may cause the loss of full end-eector motion even if the manipulator is
not in a kinematic singularity. The joint conguration in this case is said to be a joint-limit
singularity (the term semi-singularity is also used [33]). A workspace boundary implies that
the manipulator is either in a kinematic singularity or a joint-limit singularity. This obser-
vation can be used to nd the workspace boundaries of the manipulator, but once again, it
is important to note that the end-eector location corresponding to a kinematic singularity
or a joint-limit singularity is not necessarily a boundary point.
Kinematic singularities are relatively easy to nd; they are simply those congurations
where the manipulator Jacobian does not have full rank. For a kinematically redundant
88
manipulator these congurations are characterized by det(JJ
T
) = 0. This is further sim-
plied for the case of a single degree of redundancy by the fact that det(JJ
T
) = n
J

2
2
=
n
2
J1
+ + n
2
Jn
where n
J
is the canonical null vector of J, i.e., the null vector obtained
by taking the equivalent of the cross product of the rows of J. Hence, the kinematic sin-
gularities are those congurations for which each element n
Ji
of n
J
is zero. This is easily
illustrated for a planar 3R manipulator, which has as its canonical null vector
n
J
=
_

_
l
2
l
3
sin
3
l
2
l
3
sin
3
l
1
l
3
sin(
2
+
3
)
l
1
l
2
sin
2
+ l
1
l
3
sin(
2
+
3
)
_

_
(50)
where l
i
denotes the length of the i-th link and
i
denotes the angle of the i-th joint. The
kinematic singularities are precisely those congurations where each component of (50)
is zero. From the rst component of (50) it is clear that
3
must be a multiple of and,
since the sum, l
1
l
3
sin(
2
+
3
), of the rst two components of (50) is zero, it follows that

2
+
3
is also a multiple of . Hence the kinematic singularities of the general planar 3R is
given by (
2
,
3
) = (k, l) where k and l are integers. Physically, the links are completely
extended or folded back or a combination of the two.
Identifying joint-limit singularities is a more challenging problem, particularly when
more than one joint is at its limit. As with kinematic singularities, joint-limit singularities
are characterized by a local loss of full end-eector motion. In this case, full motion control
is lost because one or more joints are at their limits. To more clearly see the reason for
the problem, we note that pseudoinverse control q = J
+
v is typically insucient for full
end-eector control when one or more joints are at their limit. Indeed, if the end-eector
velocity v is feasible under pseudoinverse control where at least one constrained joint has a
non-zero joint velocity moving that joint away from its limit, then the end-eector velocity
v would not be feasible as it would require the constrained joint to move past its limit.
In such cases, one must rely on the null space to add enough joint velocity to meet the
required joint velocity constraints while still achieving the desired end-eector motion.
It can be shown that if a manipulator has a single degree of redundancy and is not in
a kinematic singularity, then the family of joint velocities that result in the end-eector
89
velocity v is given by
q = J
+
v + n
J
(51)
where is an arbitrary scalar. In order for (51) to be feasible given the joint-limit conditions,
the components corresponding to the actively constrained joints must have the appropriate
sign, i.e., if joint i is at its upper (lower) limit, the joint velocity q
i
must be non-positive
(non-negative). If the i-th component of n
J
is nonzero, then by choosing an appropriate
value for will allow (51) to be feasible. On the other hand, if the i-th component of n
J
is
zero, then no amount of the null vector can be added in (51) to adjust the sign of the q
i
and
consequently, certain end-eector velocities will not be possible. If two or more joint are at
their limits, then the relative directions of the columns of the manipulator Jacobian must be
examined. For example, if joints i and j (i = j) are at their upper limits, then the null space
term can compensate for the joint limits provided that n
Ji
and n
Jj
are nonzero and of the
same sign. Otherwise, certain end-eector velocities cannot be achieved. However, if joint i
is at its upper limit and joint j is at its lower limit, arbitrary end-eector velocities can be
achieved if and only if n
Ji
and n
Jj
are nonzero and of the opposite sign. The generalization
to more joints being at their limits is obvious. Similar results hold for robots with higher
degrees of redundancy, e.g., if two joints are at their upper limits, there must be a vector
of the null space of J for which the corresponding components are nonzero and of the same
sign. Of course, this is easier to determine when there is only a single degree of redundancy.
More details on the multiple degree-of-redundancy case can be found in [22].
There are a signicant number of cases to check when identifying potential joint-limit
singularities when multiple joints are at their limits. Consider an n degree-of-freedom ma-
nipulator where each joint has an upper and lower limit. One rst needs to check for the
kinematic singularities of the robot. One can then check the cases when precisely one joint
is at one of its limits. There are n joints and for each joint we must consider its upper
and lower limit. There are then 2n such cases to consider for having a single joint at its
limit. Next, one can check the cases when there are precisely two joints at a limit. In this
case there are
_
n
2
_
combinations of two joints to consider of which there are four subcases to
consider based on which combination of upper/lower joint limits are evaluated. This results
90
in
_
n
2
_
2
2
cases. More generally, there are
_
n
k
_
2
k
cases to consider when evaluating scenarios
with precisely k joints at their limits. Adding all of these cases together, we have a total
of 3
n
cases to check. This quantity also follows from the fact that there are three cases for
each individual joint: the joint is at its upper limit, lower limit, or in between these limits.
Hence, there are 3
7
= 2187 cases to consider for a fully spatial 7R manipulator if each joint
has an upper and lower limit. Each of these cases would need to be considered when iden-
tifying kinematic and joint-limit singularities. In the case of a 7R anthropomorphic arm,
congurations for which the elbow joint is at its limit are clearly joint-limit singularities
due to the critical importance of the elbow in moving the position of the wrist center.
6.4 Conditions for Failure-Tolerant Workspace Locations
Whether or not a given end-eector location is in the fault tolerant workspace is com-
pletely determined by its pre-image, i.e., the family of congurations corresponding to that
workspace location. The pre-image of a workspace location x W is the set
f
1
(x) = {q C
B
| f (q) = x}. (52)
The reachability of the end-eector location x after a locked-joint failure has occurred is
determined by whether or not the joint value of the locked joint falls within that individual
joint range of the set f
1
(x). In particular, the end-eector location x is still reachable
when joint i is locked at q
i
=
i
if and only if
i
is contained in the projection of f
1
(x)
onto the i-th axis. The projection of a set S R
n
is given by
P
i
[S] = {s
i
| s =
_
s
1
s
2
s
n
_
T
S}. (53)
Note that P
i
[S] is a subset of R that can be thought of as the projection of S onto the i-th
axis. Thus the end-eector location x can be reached after a locked-joint failure q
i
=
i
if
and only if
i
P
i
[f
1
(x)].
91
We can now formally state the characterizing conditions for the fault tolerant work space
W
F
. A workspace region is failure tolerant to a single failure in joint i F for a given C
A
and a given C
B
if and only if the following two conditions hold:
Condition 1. Reachability prior to a failure: For any x W
F
,
C
A
f
1
(x) = . (54)
Condition 2. Reachability after a failure: For any x W
F
,
A
i
P
i
[f
1
(x)] for i F. (55)
Condition 1 is simple enough; it merely says that any x W
F
should be reachable prior
to a failure, i.e., there is at least one conguration in the pre-image of x that is contained
in the pre-failure conguration space C
A
. Eqn. (54) is equivalent to x W
0
. Condition 2
is somewhat more complicated. In order for the manipulator to be capable of reaching a
workspace conguration x following a locked-joint failure in joint i that can occur at any
angle within its specied articial limits, the pre-image of x must have at least one con-
guration whose i-th component is equal to that joint value. Condition 2 insures that this
is true for all joint values within the individual articial joint limits for each failure-prone
joint. If Conditions 1 and 2 hold, the end-eector location x is failure tolerant to a locked-
joint failure of any joint in the set F. Eqn. (55) is equivalent to x W
i
for i F.
6.5 Identifying the Failure-Tolerant Workspace Boundaries
In Section 2, we formulated the problem of identifying the fault tolerant workspace in
terms of the intersection of the pre-failure workspace with the intersection of a continuous
family of images of hypersurfaces
i
C(
i
) as
i
is varied over A
i
, i F. Although mathe-
matically correct, this approach is not a feasible method for identifying the fault tolerant
workspace. In Section 4, characterizing conditions based on the pre-images of workspace
locations were given. Since closed form expressions for pre-images are dicult if not im-
possible to obtain, this approach is also not a feasible method for determining the fault
92
tolerant workspace. Instead, we will use these conditions to identify candidate boundaries
of the fault tolerant workspace.
Based on the two conditions introduced in Section 4, we can develop necessary conditions
for identifying the boundaries of the fault tolerant work space W
F
. Condition 1 relates to
the workspace prior to a failure, and the techniques for nding its workspace boundaries
were already developed in Section 6.3. Condition 2 can be used to identify additional
potential boundaries related to a locked-joint failure. Once all of the potential boundaries
have been identied, one can readily test these candidate boundaries to determine the real
boundaries of the fault tolerant workspace.
When a given location x is within the failure tolerant workspace, its pre-image satises
Condition 2 for i F; otherwise, Condition 2 will not be satised. Our goal is to identify
those workspace locations that form the boundary between where Condition 2 is satised
and where it is violated, i.e., a potential boundary of the fault tolerant workspace. This can
occur in two ways: (case I) the projection of the preimage for this workspace location fails
to contain an endpoint of A
i
or (case II) the projection of its preimage becomes disjoint
within A
i
. We will systematically identify the complete set of these potential boundaries and
introduce a procedure to extract the true boundary. While the formulation of Condition 2
is based on the concept of a pre-image, when identifying boundaries, it is more convenient
to work with self-motion manifolds, as stated in Eqn. (22).
When identifying the workspace boundaries, we will at times examine the minimum
and maximum values of the dierent joint variables
i
on the self-motion manifolds. For
manipulators with a single degree of redundancy, the self-motion manifolds are smooth one-
dimensional curves whose tangent vector is given by the null vector n
J
. Consequently, at
minimum and maximum values of
i
along the self-motion curve, we have n
Ji
= 0. For
manipulators with higher degrees-of-redundancy, the tangent space of the self-motion is
given by the null space of the manipulator Jacobian.
We will now consider how to evaluate the potential boundaries characterized by case I
and II. Case I, has two subcases (a) and (b) that are illustrated in Figs. 33 and 34 respec-
tively. Fig. 33 illustrates the subcase I(a) when the violation occurs inside C
B
. In this case,
93
the self-motion manifold is tangent to the hyperplane corresponding to the articial joint
limit for joint i. When this occurs, n
Ji
= 0. Thus, setting q
i
to an articial joint limit and
solving for the remaining joints subject to the constraint n
Ji
(q) = 0 gives candidate joint
congurations that may correspond to boundary points of the fault tolerant workspace. One
can see from Fig. 33 that, in some sense, this can be viewed as an optimization problem in
which one looks for extremal values of q
i
along a self-motion manifold and those self-motion
manifolds where these extremal values happen to occur on a hyperplane corresponding to
an articial joint limit are identied as possibly corresponding to a fault tolerant workspace
boundary.
In case I(b), illustrated in Fig. 34, the violation of Condition 2 occurs because the self-
motion manifold exits C
B
R
n
before the projection of the self-motion onto the i-th axis
can cover A
i
so that the self-motion manifold cannot be reached if joint i is locked at the
endpoint of A
i
subject to the physical requirements that the manipulator must operate
within C
B
. This case is characterized by the failure prone joint i being at an endpoint
of A
i
and some other joint j = i being at an endpoint of B
j
, where once again B
j
is the
set of joint values that are within the physical limits of joint j. In terms of a constrained
optimization problem, Fig. 34 depicts a self-motion manifold for which the i-th component
is maximized subject to the constraint q C
B
where at least one of the constraints (in this
case, the constraint on joint j) is active.
Case II also contains two subcases, (a) and (b) that are illustrated in Figs. 35 and 36,
respectively, where Condition 2 fails within the interior of A
i
. Fig.35 illustrates the case
where the self-motion manifold exits C
B
at a point where the manifolds projection is in the
interior of A
i
. Consequently, there are self-motion manifolds arbitrarily near the self-motion
manifold in Fig. 35 that come outside of C
B
so that their projections onto the i-th axis are
disconnected inside A
i
.
Case II(b) is illustrated in Fig. 36. In this last case Condition 2 is violated in the
interior of A
i
where the failure occurs in the interior of C
B
rather than at its boundary.
This corresponds to a sudden change in the topological nature of the self-motion manifolds.
In this case, the changes in topology are identied by co-regular surfaces [30] and the
94
candidate boundaries are given by the images of the kinematic singularities of the robot.
Now that the set of candidate boundaries have been determined, our goal is to extract
the true boundaries. This is performed by rst determining the intersections between po-
tential boundaries. A section of a potential boundary is dened by such intersections. A
potential boundary that does not intersect another potential boundary is by itself a sec-
tion of the same potential boundary. Each section of a potential boundary is then checked
against Condition 2. If a section is a true boundary, each point on this section will satisfy
Condition 2. Thus one would only need to check Condition 2 on a single point for each
section.
From all the sections of the potential boundaries that satisfy Condition 2, the true
boundaries can be identied. By correctly choosing two neighborhood points for each
section, a true boundary satises Condition 2 only on one neighborhood point.
C
B

j

k

i
a
i

a
i

Figure 33: An example of a self-motion manifold on the verge of violating Condition 2
at an endpoint of A
i
= [a
i
, a
i
]. Arbitrarily near this self-motion manifold are self-motion
manifolds that rise above the hyperplane q
i
= a
i
, indicating that the corresponding end-
eector location may not be within the fault tolerant workspace. Self-motion manifolds that
dip below the same hyperplane can also be found arbitrarily near the self-motion manifold
shown in the gure. Hence, the corresponding end-eector location may be a boundary
point of the fault tolerant workspace. Since the self-motion manifold is tangent to the
hyperplane, it follows that n
Ji
= 0 when the self-motion manifold touches the hyperplane
given by q
i
= a
i
.
95
C
B

j

k

i
a
i

a
i

Figure 34: Another example of a self-motion manifold on the verge of violating Condition 2
at an endpoint of A
i
= [a
i
, a
i
]. The corresponding end-eector location may be a boundary
point of the fault tolerant workspace. In this case, there will be similar self-motion manifolds
arbitrarily close by that fail Condition 2 as they exit the conguration space C
B
R
n
below
the hyperplane given by
i
= a
i
. On the other hand, there will also be other self-motion
manifolds arbitrarily close by that exit in such a way that Condition 2 is satised.
96
C
B

j

k

i
a
i

a
i

Figure 35: An example of a self-motion manifold on the verge of violating Condition 2 at
an interior point of A
i
. The self-motion manifold is tangent to the j-th physical joint limit
constraint so that n
Jj
= 0. The corresponding end-eector location may be a boundary
point of the fault tolerant workspace as there are nearby self-motion manifolds that appear
within C
B
satisfying Condition 2, but there are also self-motion manifolds arbitrarily close
by which come out of C
B
R
n
, violating Condition 2 in the interior of A
i
= [a
i
, a
i
].
97

2
-
1
8
0
-180
180
1
8
0
0
0
Figure 36: An example of a portion of the conguration space containing a self-motion
manifold that is co-regular. The intersecting lines represent a set of joint values resulting
in the same end-eector location. The conguration at the intersection is a kinematic
singularity. Such congurations are associated with a fundamental change in the topology
of the conguration space and may indicate a boundary point corresponding to a violation
of Condition 2 at an interior point of A
i
.
98
We summarize the approach in the following algorithm.
Algorithm for Identifying the Fault Tolerant Workspace Boundary
Step 1. Determine boundaries for W
0
using the methods described in Section 3.
Step 2. For i F, nd congurations in q C
B
satisfying at least one of the
following conditions and determine their images:
(a) n
Ji
(q) = 0 where q
i
= a
i
or a
i
.
(b) q
i
= a
i
or a
i
with q
j
= b
j
or

b
j
for some j = i.
(c) n
Jj
(q) = 0 for a
i
q
i
a
i
and q
j
= b
j
or

b
j
for some j = i.
Step 3. (Co-regular Surface Condition) Check for det(JJ
T
) = 0 and determine their
images.
Step 4. From the candidate boundaries determined in Steps 1-3, identify the true
boundary.
6.6 Examples
The algorithm presented above is implemented to nd the corresponding failure-tolerant
workspace area, A
WF
, of a 3-DOF planar robot. Three sets of examples are shown. The
rst example shows a case where all the robot links are equal and the articial limits are
specied, but there are no physical limits. In the second example, the robot link lengths are
allowed to vary, such that both articial limits and link lengths are the input variables, but
no physical limits are specied. In the third example, the Mitsubishi PA-10 robot is used as
a 3-DOF planar by activating only joints two, four, and six. In this example, physical limits
are considered and the link lengths are xed and unequal, and only the articial limits are
allowed to vary. In all cases, optimization of A
WF
is performed and all the joint limits are
symmetric. The symmetry of joint limits is true for most robots including the PA-10.
Let B
T
denote the tangent potential boundary along the q
i
-axis, as described in Step 2(a)
of the algorithm. The edge potential boundary in Step 2(b) is denoted B
E
. And the tangent
99
potential boundary along q
j
-axis in Step 2(c) is denoted B

T
. The co-regular potential
boundary in Step 3 is denoted B
C
.
Fig. 37 shows the optimization results for a 3-DOF planar with equal link lengths. This
result is a 13% improvement in A
WF
from the one shown in [23]. The corresponding A
WF
=
3.56 sq. units for 1 unit link lengths with symmetric articial joint limits of [18, 111, 111]
T
degrees. For W
0
, only B
E
s are checked because the rest of the potential boundaries
corresponding to it are captured in Steps 2 and 3. The boundaries of W
1
are determined
by B
T
s and the boundaries of both W
2
and W
3
are determined by both B
T
and B
C
. The
resulting W
F
is shown as the black region that is the result of the intersections of W
1
,
W
2
, W
3
, and W
O
.
Figure 37: Shown in the gure are the workspaces corresponding to an optimal A
WF
of a
3-DOF planar robot with equal link lengths. The optimal W
F
is shown as the black region
which can be derived by the intersection of W
1
, W
2
, W
3
, and W
O
. It has an area of
3.56 sq. units for 1 unit link lengths with symmetric articial joint limits of [18, 111, 111]
T
degrees.
Fig. 38 shows the slices at the optimal value of A
WF
that is shown in Fig. 37. The
contour lines are at 0.313 sq. units apart. The ridge for each subgure corresponds to the
switching of the tangent potential boundaries B
T
s of W
2
and W
3
, depending on which
potential boundary becomes a true boundary.
100
Figure 38: The gure shows the plots of A
WF
at the slices of the optimal articial joint
limits of [18, 111, 111]
T
degrees for the 3-DOF planar robot with equal link lengths. The
contour lines are at 0.313 sq. units apart. The white portion shows the vicinity of the
optimal A
WF
.
The kinematic design of a 3-DOF planar robot with respect to an optimal A
WF
is
shown in Fig. 39. Due to the unequal link lengths, a whole is created in the failure-tolerant
workspace. It is bounded by two tangent potential boundaries, B
T
s, and two co-regular
potential boundaries, B
C
s. Intuitively, one may think that the optimal value of A
WF
compared to the equal link case is lesser because of this. However, in the resulting optimal
W
F
, the hole did not have any contribution when all its corresponding workspaces are
intersected. In addition, the location of the co-regular potential boundary B
C
of W
2
and
W
3
is closer to the origin compared to the equal link length case. The resulting A
WF
has a 4.2% increase from the optimal equal link length case shown in Fig. 37. In this
case, the total link lengths is held constant at 1.2 m. The optimal link lengths are at
[0.462, 0.276, 0.462]
T
m with articial joint limits at [11, 128, 128]
T
degrees. The optimal
A
WF
= 0.5939 m
2
.
Slices at the optimal A
WF
in Fig. 39 is shown in Fig. 40. In all the three subgures
where the articial joint limits are varied, the link lengths are held at the optimal value. In
the subgure where the link lengths are varied, the articial joint limits are held constant.
In all the subgures, the sharpest ridge is due to the switching of the tangent potential
boundaries, B
T
s, of W
2
and W
3
, depending on which potential boundary becomes a true
boundary. The less sharper ridge is due to the hole in the workspace created by the internal
singularity.
Optimization of A
WF
for the PA-10 that is used as a 3-DOF planar robot is shown in
101
Figure 39: The workspaces corresponding to the optimal A
WF
= 0.5939 m
2
for a kinemat-
ically designed 3-DOF planar robot are shown in the gure. The optimal W
F
is shown as
the black region which can be derived by the intersection of W
1
, W
2
, W
3
, and W
O
. The
corresponding link lengths are L = [0.462, 0.276, 0.462]
T
m
2
with symmetric articial joint
limits of [11, 128, 128] degrees.
Figure 40: The gure shows slices at the optimal failure-tolerant workspace area for a 3-
DOF planar robot that is kinematically designed to maximize A
WF
. The contour lines are
at 0.05 m
2
apart. The white portion shows the vicinity of the optimal A
WF
. In the three
subgures where the articial joint limits are varied, the link lengths are held constant at
the optimal value. For the subgure where link lengths are varied, the articial joint limits
are held constant at the optimal values.
102
Fig. 41. The link lengths are [0.45, 0.5, 0.45] m
2
, the articial joint limits are [32, 92, 92]
T
de-
grees, and the physical limits are at [90, 135, 160]
T
degrees. The optimal A
WF
= 0.3380 m
2
.
Compared to the previous examples, the tangent potential boundary due to the physical
limits, B

T
, and the edge potential boundaries of the physical limits, B
E
s, now inuenced
the resulting boundaries of W
F
. The slices at the optimal articial joint limits are shown
in Fig. 42. As in the previous examples, the ridge in the topology is due to the switching
of the potential boundaries B
T
s of W
2
and W
3
from becoming a true boundary of W
F
.
Figure 41: Optimized A
WF
= 0.3380 m
2
for the PA-10 that is used as a 3-DOF planar
robot is shown. The link lengths are [0.45, 0.5, 0.45]
T
m
2
, the articial joint limits are
[32, 92, 92]
T
degrees, and the physical limits are at [90, 135, 160]
T
degrees.
Figure 42: Slices at the optimal articial limits [32, 92, 92]
T
degrees of the PA-10 as a
3-DOF planar robot are shown. The ridge in the topology is due to the switching of the
potential boundaries B
T
s of W
2
and W
3
depending on which one becomes a true boundary
of W
F
. Contours are 0.04 m
2
apart.
103
CHAPTER VII
SUMMARY AND CONCLUSIONS
This work considered the failure-tolerant operation of kinematically redundant manipu-
lators. The rst problem addressed the issue of guaranteeing reachability for a goal location
from a given start location, in the presence of locked-joint failures and obstacles in the envi-
ronment. The proposed method of solution involved a search for a set of obstacle-free paths
such that failure-tolerance is guaranteed. Conditions were formulated that guarantee the
existence of a solution. An algorithm was presented that searches for a simply-connected,
obstacle-free surface with no internal local minimum or maximum in the conguration
space, called a failure surface, whose existence guarantees a solution. The second problem
addressed the issue of nding the boundaries of a failure-tolerant workspace region for a
given redundant robot. Conditions for reachability of locations within the failure-tolerant
workspace region before and after a joint failure are described. The characterization of the
potential failure-tolerant workspace boundaries, based on the properties of the self-motion
manifolds, are presented. From these potential boundaries, the true boundaries of the
failure-tolerant workspace are identied.
104
REFERENCES
[1] K. S. Fu, R. C. Gonzales, and C. S. G. Lee, Robotics: Control, Sensing, Vision, and
Intelligence. U.S.A.: McGraw-Hill, Inc., 1987.
[2] S. Tosunoglu and V. Monteverde, Kinematic and structural design assessment of faul-
tolerant manipulators, Intelligent Automation and Soft Computing, vol. 4, no. 3, pp.
261268, 1998.
[3] M. L. Leuschen, I. D. Walker, and J. R. Cavallaro, Investigation of reliability of
hydraulic robots for hazardous environment using analytic redundancy, in Proceedings
Annual Reliability and Maintainability Symposium, Washington, D.C., Jan. 18-21 1999,
pp. 122128.
[4] W. H. McCulloch, Safety analysis requirements for robotic systems in DOE nuclear
facilities, in Proc. 2nd Specialty Conf. Robot. Challenging Environ., Albuquerque, NM,
June 1-6 1996, pp. 235240.
[5] D. L. Schneider, D. Tesar, and J. W. Barnes, Development & testing of a reliability
performance index for modular robotic systems, in Proceedings Annual Reliability and
Maintainability Symposium, Anaheim, CA, Jan. 24-27 1994, pp. 263271.
[6] B. S. Dhillon, Robot Reliability and Safety. New York: Springer-Verlag, 1991.
[7] K. Khodabandehloo, Analysis of robot systems using fault and event trees: Case
studies, Reliability Engineering and System Safety, vol. 53, no. 3, pp. 247264, Sept.
1996.
[8] B. S. Dhillon and A. R. M. Fashandi, Safety and reliability assessment techniques in
robotics, Robotica, vol. 15, no. 6, pp. 701708, Nov.-Dec. 1997.
105
[9] C. Carreras and I. D. Walker, Interval methods for fault-tree analysis in robotics,
IEEE Transactions on Robotics and Automation, vol. 50, no. 1, pp. 311, March 2001.
[10] W. S. Ng and C. K. Tan, On safety enhancements for medical robots, Reliability
Engineering and System Safety, vol. 54, no. 1, pp. 3535, Oct. 1996.
[11] J. Trevelyan, Simplifying robotics - a challenge for research, Robotcis and Au-
tonomous Systems, vol. 21, no. 3, pp. 207220, Sept. 1997.
[12] E. C. Wu, J. C. Hwang, and J. T. Chladek, Fault-tolerant joint development for the
space shuttle remote manipulator system: Analysis and experiment, IEEE Transac-
tions on Robotics and Automation, vol. 9, no. 5, pp. 675684, Oct. 1993.
[13] G. Visentin and F. Didot, Testing space robotics on the Japanese ETS-VII satellite,
ESA Bulletin-European Space Agency, pp. 6165, Sept. 1999.
[14] P. S. Babcock and J. J. Zinchuk, Fault-tolerant design optimization: Application to
an autonomous underwater vehicle navigation system, in Proc. 1990 Symp. Autonom.
Underwater Vehicle Technol., Washington, D.C., June 5-6 1990, pp. 3443.
[15] R. Colbaugh and M. Jamshidi, Robot manipulator control for hazardous waste-
handling applications, Journal of Robotic Systems, vol. 9, no. 2, pp. 215250, 1992.
[16] M. L. Visinsky, J. R. Cavallaro, and I. D. Walker, Robot fault detection and fault
tolerance: A survey, Reliability Engineering and System Safety, vol. 46, pp. 139158,
1994.
[17] H. Schneider and P. M. Frank, Fuzzy logic based threshold adaption for fault detection
in robots, in Proc. 3rd IEEE Conf. Contr. Applicat., Glasgow, UK, Aug. 24-26 1994,
pp. 11271132.
[18] Y. Ting, S. Tosunoglu, and D. Tesar, A control structure for fault-tolerant operation
of robotic manipulators, in Proceedings IEEE International Conference on Robotics
and Automation, Atlanta, GA, May 2-6 1993, pp. 684690.
106
[19] K. S. Tso, M. Hecht, and N. I. Marzwell, Fault-tolerant robotic system for critical
applications, in Proceedings IEEE International Conference on Robotics and Automa-
tion, Atlanta, GA, May 2-6 1993, pp. 691696.
[20] M. L. Visinsky, J. R. Cavallaro, and I. D. Walker, A dynamic fault tolerance framework
for remote robots, IEEE Transactions on Robotics and Automation, vol. 11, no. 4, pp.
477490, Aug. 1995.
[21] A. A. Maciejewski, Fault tolerant properties of kinematically redundant manipula-
tors, in Proceedings IEEE International Conference on Robotics and Automation,
Cincinnati, OH, May 13-18 1990, pp. 638642.
[22] R. G. Roberts and A. A. Maciejewski, A local measure of fault tolerance for kine-
matically redundant manipulators, IEEE Transactions on Robotics and Automation,
vol. 12, no. 4, pp. 543552, Aug. 1996.
[23] C. L. Lewis and A. A. Maciejewski, Fault tolerant operation of kinematically re-
dundant manipulators for locked joint failures, IEEE Transactions on Robotics and
Automation, vol. 13, no. 4, pp. 622629, Aug. 1997.
[24] K. N. Groom, A. A. Maciejewski, and V. Balakrishnan, Real-time failure-tolerant
control of kinematically redundant manipulators, IEEE Transactions on Robotics and
Automation, vol. 15, no. 6, pp. 11091116, Dec. 1999.
[25] J. D. English and A. A. Maciejewski, Fault tolerance for kinematically redundant ma-
nipulators: Anticipating free-swinging joint failures, IEEE Transactions on Robotics
and Automation, vol. 14, no. 4, pp. 566575, Aug. 1998.
[26] C. L. Lewis and A. A. Maciejewski, Dexterity optimization of kinematically redundant
manipulators in the presence of joint failures, Computers and Electrical Engineering,
vol. 20, no. 3, pp. 273288, 1994.
107
[27] S. K. Ralph and D. K. Pai, Computing fault tolerant motions for a robot manipulator,
in Proceedings IEEE International Conference on Robotics and Automation, Detroit,
MI, May 10-15 1999, pp. 486493.
[28] C. J. J. Paredis and P. K. Khosla, Fault tolerant task execution through global tra-
jectory planning, Reliability Engineering and System Safety, vol. 53, pp. 225235,
1996.
[29] A. Liegeois, Automatic supervisory control of the conguration and behavior of multi-
body mechanisms, IEEE Transactions on Systems, Man, and Cybernetics, vol. 7,
no. 12, pp. 868871, Dec. 1977.
[30] J. W. Burdick, On the inverse kinematics of redundant manipulators: Characteriza-
tion of the self-motion manifolds, in Proceedings IEEE International Conference on
Robotics and Automation, Scottsdale, AZ, May 14-19 1989, pp. 264270.
[31] , Kinematic analysis and design of redundant robot manipulators, Ph.D. disser-
tation, Stanford University, Stanford, California, Mar. 1988.
[32] C. J. J. Paredis and P. K. Khosla, Designing fault-tolerant manipulators: How many
degrees of freedom, International Journal of Robotics Research, vol. 15, no. 6, pp.
611628, Dec. 1996.
[33] C. Luck, Self-motion representation and global path planning optimization for redun-
dant manipulators through topology-based discretization, Journal of Intelligent and
Robotic Systems, vol. 19, no. 1, pp. 2338, May 1997.
108