A Constraint-Based Approach for Computing Fault Tolerant Robot Programs by Scott K. Ralph M.Sc. (Computer Science) University of British Columbia 1991 B.Sc. (Honours, Computer Science) Memorial University 1989 A THESIS SUBMITTED IN PARTIAL FULFILLMENT OF THE REQUIREMENTS FOR THE DEGREE OF Doctor of Philosophy in THE FACULTY OF GRADUATE STUDIES (Department of Computer Science) we accept this thesis as conforming to the required standard . The University of British Columbia June 1999 © Scott K. Ralph, 1999 In presenting this thesis in partial fulfilment of the requirements for an advanced degree at the University of British Columbia, I agree that the Library shall make it freely available for reference and study. I further agree that permission for extensive copying of this thesis for scholarly purposes may be granted by the head of my department or by his or her representatives. It is understood that copying or publication of this thesis for financial gain shall not be allowed without my written permission. Department of Computer Science The University of British Columbia Vancouver, Canada Date: July 7, 1999 Abstract We develop a new framework, based on the Least Constraint, for pro-gramming robots to perform a task using a fault tolerant trajectory. We take a specification of the task, expressed as a set of constraints on the robot's con-figuration over time, and produce a fault tolerant trajectory. The methodology encourages fault tolerant behavior at two levels: first at the task-design phase by encouraging the designer to omit extraneous constraints which reduce the poten-tial for fault tolerant operation, and secondly, at the trajectory generation phase by avoiding critical configurations. The critical configurations are identified via a measure of fault tolerance which is global in nature. The dual optimization of fault tolerance at both the high-level design phase as well as the low-level recovery-motion generation phase allows more of the inherent fault tolerance of the robot to be exploited. We believe that combining these two processes into a single formalism is unique and beneficial. The methodology is unique in its ability to deal with robots which are not kinematically redundant with respect to arbitrary task, but which are sufficiently redundant with respect to the particular task constraints to allow the task to be described as a set of "loose" constraints over time. The constraint-based approach allows us to model faults as additional con-straints to the specification, thereby allowing an efficient means of computing the effect a fault will have on the ability to complete the task, using the reduced config-uration space of the robot. Faults not previously considered, such as the inclusion of additional obstacles, as well as dynamic information arising from sensors, can also be included using this formalism. An efficient algorithm for constructing a recovery motion for a fault has been developed. A specific example of a seldom considered fault, the collision of the robot with an unknown obstacle, is presented. We show that in addition to detecting the event, we are also able to recover the collision geometry. This information can then be used in a more intelligent recovery motion selection. We have developed a new global fault tolerance measure called longevity. The fault tolerance measure examines a set of faults which may occur at a given configuration, and based on the optimal recovery motions for the given fault, ranks the configuration in its ability to satisfy the future task requirements. Using this fault tolerance measure, a trajectory which maximizes the worst-case failure mode of the robot is computed. A number of experiments show the applicability of the method to a num-ber of domains. We analyze the resulting trajectories with respect to their ability to sustain a fault, and we compare them to more traditional methods for accom-plishing the same task. We demonstrate that trajectories obtained using the least constraint specification and the fault tolerance measure are able to achieve a much larger degree of fault tolerance than naive methods for the same task. The fault tolerant trajectories make optimal use of the 1-fault tolerant configuration space, and maximize the worst-case utility of the trajectory given a fault. ni Contents Abstract ii Contents iv List of Tables ix List of Figures x Acknowledgements xiii Glossary of Terms xiv 1 Introduction 1 1.1 Fault Tolerance: Terms and Definitions 3 1.2 An Architecture for Fault Tolerant Robotic Systems 5 1.3 Fault Tolerance Scenario 8 1.4 Notational Conventions 10 1.5 Examples of Tolerable Faults 10 1.5.1 3-R Planar Manipulator 10 1.5.2 Tolerable Faults for Non-redundant manipulators 16 1.6 Overview of the Methodology 18 IV 1.7 Outline of the thesis 22 1.8 Thesis Contributions 24 2 Fault Tolerant Design 26 2.1 Background 27 2.1.1 Kinematics of the Task 28 2.1.2 Kinematic Effects of a Fault 30 2.1.3 Adding Redundancy 34 2.1.4 Defining the Task 36 2.1.5 Example of an Explicit Task Description 38 2.1.6 Velocity Profile Specification 39 2.1.7 Example of an Implicit Task Description 40 2.1.8 Least Constraint Robot Programming 41 2.2 Design of Fault Tolerant Robots 43 2.2.1 Planar Case 44 2.3 Fault Tolerant Task Design 46 2.3.1 Ideal Properties of a Specification 47 2.3.2 Valid Trajectories, Verification 51 2.3.3 Constructing the Specification 51 2.3.4 Linking Functions 52 2.3.5 Driving Constraints 53 2.3.6 Examples of LC Specifications 54 2.3.7 Limitations 57 2.3.8 Ranking Trajectories 58 2.3.9 Optimal Utility Paths 60 2.4 Decomposition of the Valid Space 62 2.4.1 Uniform Decomposition 64 2.4.2 Graph of Time-augmented Configuration Space 65 2.4.3 Utility of a Discrete Path 66 3 Fault Tolerant Trajectory Planning 70 3.1 Background 73 3.1.1 Local Measures of Fault Tolerance 74 3.1.2 Global Methods 77 3.1.3 Planning Under Uncertainty 86 3.2 Reactive Path Planning 89 3.2.1 Representing Faults 90 3.2.2 Recovery Motions for a Fault 93 3.2.3 R O D ' S in a Discrete Configuration Space . . 94 3.2.4 Additional Obstacles as Faults 97 3.2.5 Computing Optimal Recovery Motions 98 3.2.6 Computing Recovery Motions for Multiple Source Vertices . 100 3.3 Contingency Planning 102 3.3.1 A Global Fault Tolerance Measure 105 3.3.2 Longevity: A Global Measure of Fault Tolerance 106 3.3.3 Computing Longevity 110 3.3.4 The Sorted-Minimum Path Ranking 110 3.3.5 Interpretation of Sorted-Minimum Performance Metric . . . 112 3.3.6 Computing the Fault Tolerant Path 113 3.3.7 Complexity Analysis of the Sorted-Minimum Path Algorithm 115 VI 4 Reactive Elements 118 4.1 Previous Work in FDI 119 4.2 Analytical Redundancy: Parity Space Methods 121 4.3 Detecting and Localizing a Manipulator Collision 123 4.3.1 Motivation 123 4.3.2 Introduction 124 4.3.3 Contact Forces 127 4.3.4 Contact Localization 131 4.3.5 Admissibility Constraints 132 4.3.6 Feature Identification 134 4.3.7 Results 134 4.3.8 Extensions 136 4.3.9 Conclusions 139 5 Trajectory Planning Experiments 140 5.1 Fault Tolerant Locomotion . . : 141 5.1.1 The 4-Beast . 142 5.1.2 Rolling Gait 144 5.1.3 4-Beast Design 144 5.1.4 Specification of the Tumble Step 146 5.1.5 Decomposition of JFCT 151 5.1.6 Computing the Measure of Fault Tolerance . 151 5.1.7 Generating the Paths 153 5.1.8 Evaluating Path Performance 154 5.2 Fault Tolerant Manipulation 158 vi i 5.2.1 Defining the Task 160 5.2.2 Decomposition of the Configuration Space 161 6 Conclusions and Future Work 169 6.1 Future Work 171 Bibliography 172 Appendix A Decomposition of TCT 179 Appendix B Computing the Optimal Recovery Motion for a Fault 183 B.l Computing the Recovery Motion for a Single Source Vertex . . . . 184 B.2 Computing Recovery Motions for Multiple Source Vertices 185 Appendix C Algorithms for Computing the Most Fault Tolerant Tra-jectory 187 C.l Algorithm for Sorted-Minimum Path Comparison Operator 187 C.2 Computing Sorted Minimum Paths 189 C.3 Proof of Correctness of Sorted-Minimum Path Algorithm 192 vni List of Tables 3.1 Symbols used in the complexity analysis and their meaning 115 4.1 Classification error rate with varying relative errors in r^ 136 4.2 Contact Parameters 138 5.1 Trajectory lengths for 4-beast experiment 155 5.2 Denavit-Hartenberg parameters of Puma 560 manipulator 160 5.3 Summary of the RODs used in computing the recovery motions for the Puma 560 163 IX List of Figures 1.1 And-gate with a fault 5 1.2 Generic architecture for a fault tolerant robotic system . 6 1.3 Positioning task performed by a 3-R planar manipulator 11 1.4 Example of 3R planar manipulator executing a positioning task with a fault 12 1.5 Point of failure of a 3R planar manipulator 12 1.6 Family of joint angles for an end-effector position and a joint-2 failure constraint 13 1.7 Family of joint angles for an end-effector position and a joint-3 failure constraint 13 1.8 Trajectory for 3R planar manipulator, and a recovery motion after a joint-2 failure 14 1.9 Trajectory for 3R planar manipulator, and a recovery motion after a joint-3 failure 15 1.10 An example of a non-redundant robot performing a task described as a set of constraints 17 1.11 Components of the proposed framework 22 2.1 Planar 3-R manipulator with unit-lengths showing fault tolerant configuration space 32 2.2 Self-motion manifold for planar 3-R manipulator (see Fig. 2.1). . . . 33 x 2.3 A 3-R manipulator, and a 6 DOF manipulator which is first-order fault tolerant 36 2.4 Example of an implicit method of defining a task using collision-space obstacle representation 41 2.5 Static and time-dependent "driving" constraints, used to produce a motion 54 2.6 Polygonal obstacle constraint in LC 55 2.7 LC specification for a place-on-table task 56 2.8 Decomposition of the time-augmented configuration space into non-overlapping convex cells 63 2.9 Utility of a cell 67 3.1 Trajectories of [PK95] sharing a common recovery motion 84 3.2 Canadian Traveler Problem [PY89] 88 3.3 Example of modeling a failed actuator as an additional constraint. 92 3.4 Effects of a fault constraint when using a discretized configurations space 96 3.5 Immobilized actuator constraint in discrete graph 97 3.6 Computing recovery motions in a discrete topology 102 3.7 Relationship between perf(ui) and fault tolerance measure L{vi). . . 109 4.1 Residuals of a system, formed using a system model 120 4.2 Contact forces of a planar manipulator 129 4.3 Cumulative distribution of localization errors for varying relative error in r^ 137 5.1 The 4- and 8-beasts: the first two platonic beasts 142 5.2 Image of the prototype 4-beast 143 5.3 Simulation of a canonical tumble-step of the 4-beast 145 XI 5.4 Simulator for the 4-Beast 146 5.5 Canonical tumble of 4-beast up a 20 deg. incline 147 5.6 Starting configuration for a tumble-step 148 5.7 Two views of the decomposed configuration space of 4-beast 151 5.8 Fault tolerant trajectories of the 4-beast 154 5.9 Evaluation of the fault tolerance of the path generated with the Lavg- 156 5.10 Evaluation of the fault tolerance of the path generated with the Lavg-157 5.11 Initial and final configurations of pick-and-place task using a Puma 560 robot manipulator 158 5.12 Two similar configurations with very dissimilar tolerance to faults. . 159 5.13 Distribution of util(ufc) and L^orsti'^k), taken as a percentage of the 470,400 valid cells. 163 5.14 Comparison of fault tolerant and joint-interpolated motion for Puma 560 example 165 5.15 Endpoints for optimal recovery motions for Puma 560 example. . . 166 5.16 Longevity and utility vs. path length for optimal and straight-line joint-interpolated motion; 167 A.l Computing whether a constraint surface forms part of the TCT boundary within a given cell 182 C.l Edge Relaxation 191 C.2 Proof of correctness of Algm. C.2 195 Xll Acknowledgements I would like to give special thanks to my advisor Dinesh Pai for his help and guidance over the past several years. It has been a pleasure to work with someone so talanted, and who is also such a good mentor. I would also like to thank my committee, Peter Lawrence, Jim Little, Alan Mackworth, and Alan Wagner for their many comments which helped to make this a better thesis. Special thanks to Jim for his always fun discussions! I also owe a lot to my many friends at U.B.C., for their encouragement and companionship: Bill, David, Roger, Rob Walker, Andreas, Anne. I am sure that I am forgetting several! Special thanks to Valerie and Rob Scharein for encourage-ment when I needed it the most. My parents Judith and Earle have been very loving and supportive through-out my graduate studies - it is to them that this thesis is dedicated. SCOTT K . RALPH The University of British Columbia June 1999 Xll l Glossary of Terms Qjj : C -^ E The fault constraint function associated with the predicate Wj C C E" The configuration space of the robot. C = C X E"*" The time-augmented configuration space. Cell{vi) The interior of cell labeled Vi in decomposition of the configuration space. d The number of sub-divisions of each dimension of C. E = {cij} The set of edges of the decomposed configuration space. TCT C C The feasible configuration-time space. G The constraint specification of the task. gi = {hi < 0), A constraint predicate used in the specification of the 9r,j = {hi,j < 0) task (J^CT). , 7 The maximum trajectory length in the graph of vertices. hi, hi J : C —)• E The constraint functions associated with gi and gij respectively. J e E"^" The manipulator Jaccobian. XIV V e £'"><" The failed manipulator Jaccobian (freezing i'^ actuator). /C : C —>^ W Forward kinematic relation. L{vi,u)) Longevity of a vertex Vi given a fault described by u. Lw{vi) Worst-case longevity value over all faults uji G fl. A measure of fault tolerance. m The dimension of the workspace of the robot, n The dimension of the configuration space of the robot. Number of vertices in the decomposed configuration space. The set of faults considered. A point in the workspace of the robot. Array storing the optimal recovery motion for a vertex. Points in the configuration space of the robot. Points in the time-augmented configuration space. The subset of C reachable from q. Predicate. True when 9{t) satisfies the specification over the time interval [0, tmax]-0{t),x{t) G C Trajectories of the robot through the configuration space. util(^, F) The utility of 9{t), subject to the restriction F. V = {vi} The set of vertices of the decomposed configuration space. iOi = (oji < 0) A constraint predicate. W C E"" The workspace of the robot. A^„ Q p' ,x '€ W n[] q , q ' e C q,q'eC R{q,F) sat(6',F,tmax) XV Chapter 1 Introduction Issues pertaining to fault tolerance in robotic systems are seldom addressed at present, most likely because the vast majority of robots are used in manufactur-ing applications. In such applications the environment can be carefully engineered so that unexpected interactions with the environment are kept to a minimum. The growing number of robots deployed in hazardous and/or unstructured envi-ronments, medical applications, and other safety-critical applications will place an increasing importance on the development of fault tolerant robotic systems. In mission critical applications, such as space exploration, the benefit of autonomous detection and compensation of errors is obvious. In teleoperation applications hu-man operators, who now perform the bulk of error detection and recovery, may not be able to detect errors with sufficient speed and accuracy to ensure the safety of the robot and its surroundings. Additionally, inevitable transmission delays may amplify the effects of errors making the autonomous detection of errors crucial. In hazardous waste removal tasks, such as nuclear cleanup operation [BT84], human intervention may not be possible, making failures very costly. Fault tolerance in industrial robotics may increase the lifespan of a robot, or reduce the frequency of human intervention, significantly reducing the operating costs. Lastly, in cases where a robot is unable to tolerate a fault and still complete the task, often it will still be useful to detect the error, and take actions which minimize the severity of the damage or risk. For the above reasons it is increasingly attractive to deploy robot systems which are able to detect errors and isolate the fault causing the error. Where possible, the robot can then utilize the remaining functionality to compensate for the fault and complete the task. These considerations motivate research in the areas of fault detection and identification [Fra90, VWC94], design [PAK94], and path planning for redundant manipulators with redundancy resolution [LM94b, PK96]. Fault tolerant path planning which examines the topological properties of the configuration space has been applied to locomotion tasks [RP97], and 3 DOF positioning tasks performed by a redundant manipulator [RP99]. Error detection and recovery has also been studied within the scope of manipulation tasks [Don89], producing paths which either succeed, or noticeably fail. Much of the work in fault tolerant robotics uses the terms "fault", "error" and "failure" as synonyms which can cause unnecessary confusion. To avoid this confusion we will clearly define the terms "fault", "error", "failure", and "error re-covery" in Section 1.1 in a consistent way as proposed in [LA81]. Next we will place the task of fault tolerant robot programming in context, and introduce a simple system architecture, similar to that proposed by [GV89, Vis94], which separates the subproblems of control, fault detection and identification, error recovery, and path planning into separate modules. 1.1 Fault Tolerance: Terms and Definitions Lee and Anderson [LA81] define a system as a set of components, each of which may be recursively defined in terms of smaller components. At the lowest level are atomic components; their structure is not broken down into sub-components. A fault is any defect occurring in an atomic component causing it to behave incon-sistently with its specification. We should note that the system also includes the system specification, defined below, and that faults may exist within the specifica-tion itself. Examples of a fault include a broken wire, a coding bug, or an unexpected interaction with the environment (such as the collision of a manipulator with an obstacle not modeled by the program). The system evolves through a set of internal states as a result of the changes to the state of its components. The external state of a system is an abstraction of the internal state, and is an ordered set of the external state of its components. We rnay not directly observe the internal state of the system. The system specification is an authoritative description of the set of valid internal states of the system. Each state of the system should be classified as being valid, meaning it corresponds to the system specification, or erroneous (or invalid), indicating that it does not meet the system specification. If the specification classifies every possible state of the system, then the specification is said to be complete; this is a highly desirable quality of the system specification. Validity of the system is an attribute of the system's state, and not of the trajectory. A failure is a defined as a deviation of the behavior of the system from its specification, and is an event from which there is no recovery. An erroneous state is a valid state of the system, which could, but need not, result in a system failure through a series of valid transitions. The part of the erroneous state which differs from a valid state is called an error. The error is the only direct manifestation of the fault that we can directly observe when attempting to detect a fault. The causal relationship between a fault, an error, and a failure is given below: fault —>• error —>• failure physical universe information universe external universe (abstraction of physical) Fault detection and identification (FDI), is the process of examining the error and abductively determining which fault(s) produced it. Once the fault has been identified, the inconsistent or erroneous internal state may need to be changed so that it is once again consistent with the specification. This process is called error recovery. To illustrate the relationship between a fault, an error, and a failure, consider an and-gate such as shown in Fig. 1.1. Under normal operation the result should give c = aAb. If during fabrication the input a was short-circuited to ground, then this would be considered a fault. The manifestation of the fault, a being stuck at zero, is an error. If the input is a = 1,6 = 1 then an incorrect value would be computed at c leading to an failure. However, errors do not have to lead to failures as the input a = 1,6 = 0 will still compute the correct value. We will look more carefully at the process of fault detection and identifica-a h Figure 1.1: And gate. Normal computation will result in c = a A 6. tion for typical robot faults in Chapter 4. For faults such as sensor or actuator faults which leave an actuator inoperable, the error recovery involves computing an alternative trajectory, or recovery motion which continues to satisfy the task. The challenge of generating a fault tolerant robot program is the construc-tion of a system which, when a fault occurs, is still able to meet the demands of the specification and continue to satisfy the system requirements. 1.2 An Architecture for Fault Tolerant Robotic Systems Fig 1.2 describes a simple system architecture in which the tasks of fault detection and identification (FDI) are separated from the higher level tasks of path planning and error recovery. We will assume that there is a low-level controller which takes input from the trajectory generator and the sensors and produces commanded torques to the actuators. The FDI subsystem, which is responsible for monitoring the sensors and actuators, compares the sensor readings to their expected values. Upon detecting an anomaly, the FDI system is responsible for identifying the fault which is most likely to have given rise to the error, and sending the fault information to the trajectory generation subsystem. In order to successfully tolerate faults we require that the FDI system detect errors quickly and accurately identify the fault. We also require the trajectory generation to produce effective recovery actions which maximize the use of the remaining functionality of the robot to complete the task. Increasing real-time demands Path Planning/ Robot Design Trajectory Generator (Error Recovery) 1 Discrete/Symbolic Domain Controller FDI Continuous Domain Sensors & Actuators Figure 1.2: The architecture of the system is divided into five parts: the sensors and actuators; the controller which sends commanded motions to the actuators and receives sensor data; the FDI subsystem which monitors the sensors and actuators, detects errors, and identifies which fault was responsible; the trajectory generation which is also responsible for choosing appropriate recovery actions in the event of a fault; and the path planning/robot design encompassing issues of the design of the robot and the completion of the task as a whole. Notice that the trajectory generator can receive events from two sources: the controller, or the FDI system. Whether we decide to classify a given event as a fault, or as an event handled by the controller is dependent on the relative complexity of the controller compared to the trajectory generator. For example, if we wish to implement a guarded-move operation [McK91], we may choose to perform the contact monitoring by the controller, in which case the contact event is passed from the controller to the trajectory generator, or we may use a simple controller and perform the monitoring by the FDI system which will send the 6 contact-fault signal to the trajectory generator. A fault could include any unexpected or un-modeled external interaction with the system. Typically faults will require intervention by the error-recovery mechanism, provided an effective recovery action exists for the fault. We will not consider minor external perturbations as faults, such as small positioning errors, as these are typically dealt with by the low-level controller (s). For a large set of faults, such as those involving sensors or actuators, the only error recovery possible is to render the affected actuator immobile. This is reasonable since the failed actuator is likely to move unpredictably, and the failure of a sensor associated with an actuator yields the actuator uncontrollable. Next we must re-plan and execute a new trajectory called the recovery motion, which is an alternative trajectory to accomplish the task. It is the job of the error-recovery system to plan for and initiate the recovery motion. This joint-immobilization fault scenario is common, and is described in Section 1.3. Using this fault tolerance assumption, the generation of a recovery motion is a kinematic problem involving a new mechanism with one fewer degree of freedom, obtained by fixing one of joint angles of the original robot. Since we are interested in the higher-level problems of contingency planning, we will focus on faults involving the immobilization of an actuator. The task of the FDI subsystem is to recover as much information about the fault as possible so that it can be used by the error recovery system. For faults resulting in the immobilization of an actuator the identification of the actuator involved in the fault is sufficient. In Chapter 4 we describe an interesting fault, namely the collision of a robot manipulator by an un-modeled obstacle. This 7 fault is interesting because the dynamics of the manipulator itself allows the entire manipulator to act as a virtual position sensor for the point of contact. The extraction of the contact geometry is potentially useful for the generation of the recovery motion since it provides some additional obstacle constraint information. Some faults are transient, making error recovery relatively straightforward, however since we are interested in higher level contingency planning, we will focus our attention on persistent faults, specifically those which require the immobiliza-tion of an entire actuator. 1.3 Fault Tolerance Scenario A robot is said to be 1-fault tolerant if it can successfully complete the task in the event of a joint failure under the following scenario [PK94]: An FDI algorithm monitors the proper functioning of each actua-tor/sensor of a robotic system. Upon detecting a fault, an intelligent controller immobilizes the actuator by activating its brake, and auto-matically adapts the joint trajectories to reflect the new robot structure. The task is then continued without interruption. To successfully tolerate a fault we must have some degree of redundancy in our sensors and actuators. Much of the previous work in fault tolerant robotics has focused on kinematically redundant robots executing motions which have been explicitly prescribed. For example we may specify a set of via-points in the robot's workspace [PAK94, PK95], or a velocity profile of the robot [LM94a]. Provided we 8 stay within the kinematically redundant workspace of the robot, we can sustain a fault while continuing to follow the commanded motion. We feel that this view of fault tolerance is too restrictive since it only reflects the ability of the robot to follow a specific motion and does not consider the intrinsic constraints of the task. In many instances there may exist a family of trajectories which all satisfy the task. For example, when moving a manipulator to a particular position, it may be sufficient to constrain the endpoint of the trajectory, and avoid obstacles. By giving the controller the ability to choose intelligently the most fault tolerant trajectory from a set of trajectories, all of which satisfy the task constraints, we may expect a larger degree of fault tolerance of the resulting system. By considering only the constraints of a given instance of a task we are able to exploit the full potential fault tolerance of the robot. In particular, we are interested in developing methods for producing fault tolerant behavior for robots which are not necessarily fault tolerant for arbitrary tasks, but are sufficiently redundant with respect to a particular task. Specifically we consider tasks which can be naturally expressed as a set of loose constraints on the configuration over time. As we shall see, the use of an intelligent task specification is vital if we are to exploit the full potential of the inherent fault tolerance of the robot with respect to the task. In Chapter 2 we will introduce a language based on least constraint which defines a task by a set of constraints on the robot's configuration over time. The following illustrates an example of a task which is easily expressed as a set constraints. 1.4 Notational Conventions We will use the convention that bold-face variable names denote vectors, as in q to denote a vector of joint angles, and subscripts denoted elements of a vector, or column of a matrix, e.g., q = {qi,q2,- •', Qn)^ • We will differentiate two vector quantities by superscripts, e.g. q'. 1.5 Examples of Tolerable Faults We will give two short examples of faults which can be tolerated by a robotic sys-tem. In the first example the manipulator is kinematically redundant. The second example is of a task which is naturally expressed as a set of constraints on the robot, and while not kinematically redundant, there is still sufficient redundancy of the robot with respect to the task constraints to permit a valid trajectory to the desired goal. 1.5.1 3-R Planar Manipulator The first example we will look at is that of a kinematically redundant manipulator. As long as the end-effector positions are within the kinematically fault tolerant region of the workspace, alternative trajectories can be chosen to reach a given point in the workspace. Consider a 3-R planar manipulator as depicted in Fig. 1.3 in which our task is to move from an initial point in the workspace p^ to a final position p" .^ We 10 assume an initial configuration of q^ = (151.2°,-88.6°, 114.9°)^ y /I p ' = (1.1,0) Figure 1.3: Positioning task performed by a 3-R planar manipulator. From q-^ we find that the configuration q^ which places the end effector at p^, and is closest in joint space to the initial configuration q^ is j2 = (40.6°,-171.4°, 136.9°)^ The configurations q'^ and q^ as depicted in Fig. 1.4 give the initial and final configurations of the 3-R manipulator. We will assume that the manipulator performs a joint interpolated motion in which the joint velocities remain constant throughout the motion. Suppose that the motion takes one time unit to be completed, and that during the execution, a fault occurs at time t = 0.5, or at the configuration q^ = (95.9°, -130.0°, 125.9°)^ as shown in Fig. 1.5. Since the manipulator is 11 kinematically redundant, there is a family of joint angles which position the end effector at the same position. Depending on the kinematic structure of the manip-ulator, the position of the goal, and the configuration at which the fault occurred, it may be possible to accomplish the positioning goal. Figure 1.4: Initial and goal positions configurations of the manipulator. Figure 1.5: Point of failure. Upon detecting the fault the controller must re-plan a new trajectory of the manipulator since the failed joint is no longer movable. The new motion is called the recovery motion for the fault. We can consider the manipulator after the fault as being a new manipulator with one less actuator and one fixed joint angle. The new path planning problem is the same as the original with one fewer degrees of freedom. In our example, upon freezing the joint, the manipulator is no longer kinematically redundant, and there are at most two distinct configurations which accomplish the goal end eff"ector position. Figure 1.6 shows the family of joint angle solutions for the goal position p^. The failure in joint 2 constrains the set of configurations to lie on the configuration space plane q^ — —130.0°. Since there is a point in this plane which achieves p" ,^ we can construct a recovery motion from the point of failure to the goal, as shown in Fig. 1.8. 12 Family of joint angle solutions for the point (1.1.0.0) q3 (deg. p2 joint solutions -q2 = -130deg constraint • •50 0 50 q2 (deg.) q1 (deg.) Figure 1.6: The family of solutions for end-effector position (1.1,0.0). The planar fault constraint 92 = —130° intersects the family of solutions at two points, allowing an alternative trajectory to the goal. Family of joint angle solutions for ttie point (1.1,0.0) • p2 joint solutions • q3 = 125.9deg constraint • q2 (deg.) Figure 1.7: The family of solutions for end-effector position (1.1,0.0). The planar constraint qz = 125.9° intersects the family of solutions at two points, allowing an alternative trajectory to the goal. 13 200 150 100 50 -50 -100 q1 no fault -»-q3 no fault - i -q1 recovery -«-q3 recovery -t-0.2 0.4 0.6 time 0.8 Figure 1.8: Original trajectory and corresponding error recovery motion for a fail-ure in joint 2. 14 200 150' 100 50 0 -50 -100 -150 .or\ri Joint 3 Fault q1 no fault -«— q2 no fault -t— q1 recovery - « - -' • -______^ q2 recovery - H — " ~~~~~~~~'* ' t——^ .^ ^^ ' 1 1 1 1 0.2 0.4 0.6 time 0.8 Figure 1.9: Original trajectory and corresponding error recovery motion for a fail-ure in joint 3. 15 Similarly there is a corresponding failure constraint for the actuator q^ as depicted in Fig. 1.7, for which there is a corresponding recovery motion shown in Fig. 1.9 1.5.2 Tolerable Faults for Non-redundant manipulators The above example illustrates the ability of a redundant manipulator to sustain a failed actuator by choosing an alternative trajectory to the goal position. A second instance in which the robot is not kinematically redundant but is still able to sustain a failed actuator is given in the following example. There are many tasks which can be described best using a set of constraints on the robot's configurations over time. Such tasks can permit a large range of valid trajectories which achieve the task. This large solution space can be exploited to obtain a fault tolerant trajectory which satisfies the constraints. Consider the following task of a robot walking in two dimensions as depicted in Fig. 1.10. The task goal is to translate the body to the left to allow the transfer foot to make contact with the next foothold. We will assume that only workspace and static stability constraints are present. Stability is specified by requiring the center of mass to satisfy inequality constraints gi and g2 shown in the figure. Horizontal motion is achieved by a moving constraint g4{t). If a fault is introduced in the present configuration rendering the distal right joint immovable, the body position is constrained to lie on a circular arc (shown as a dotted arc). By not prescribing a specific trajectory, the robot is still able to reach the goal using the reduced 16 workspace. stability constraints 93 \\\\\\\\W Figure 1.10: An example of a non-redundant robot performing a task described as a set of constraints. The locomotion task described above is an example of a task which is natu-rally expressed as set of constraints on the robot's configuration over time. Instead of prescribing a particular trajectory of the robot over time, we describe the task using a set of 4 constraints. In the event of a fault the new configuration space of the robot is reduced to a sub-manifold of the original configuration space. To ascer-tain whether the robot is still capable of completing the task we need only consider the satisfiability of the task constraints in the reduced configuration space. As we shall see later, representing the addition of a fault as the inclusion of an additional task constraint allows a particularly elegant means of computing the capabilities of a robot under varying fault hypotheses. This illustrates the utility of expressing a task explicitly as a set of con-straints on the robot's configuration over time. In addition to providing a natural means of expressing the task, we also ensure that there is a large family of trajec-17 tories from which we may choose. This correspondingly increases the likelihood of being able to sustain a fault, and still achieve the goal of translating the robot to the goal. This method of specifying a task using a set of constraints is particularly useful when defining the behavior for an autonomous robot where one may wish to specify a number of safety constraints which must be satisfied. This permits us to compose a set of constraints, some of which are used to drive the trajectory to the goal, and others which ensure a minimal set of safety requirements. This thesis develops a methodology for taking a given instance of a robot and task specification, and producing a robot program which maximizes the fault tolerance of the robotic system. What follows is an overview of the overall method-ology. 1.6 Overview of t he Methodology Constructing a fault tolerant program can be divided into three parts, each differ-ing in the degree of reactivity. These parts are: fault tolerant design, contingency planning, and fault reactive components. The design components, as well as the contingency planning, are computationally intensive, and must be performed of-fline. Fault tolerant design (FTD) and contingency planning are similar in that they both involve examining global properties of the kinematics of the task and the effects faults have on the resulting capabilities of the robot. The reactive com-ponents reflect the need of the robot to quickly respond to detected errors and commence recovery actions in order to minimize the effects of the faults, and thus 18 increase the likelihood of completion of the task. Fault tolerant design, the least reactive, is performed entirely off-line, and involves the design of the robot and task. The design of the robot consists of the geometric properties of the robot: the type of joints and the geometry of the links. In the design problem we are looking for a set of design parameters which maximize the fault tolerance of the resulting robot. The design of the robot determines the forward kinematics, and therefore the set of reachable configurations. When a fault is encountered the set of reachable configurations is changed. To be fault tolerant the robot must be able to reach the points along a valid trajectory, even when a fault has occurred. For this reason the design of the robot places the largest restrictions on the fault tolerant potential of the robot. The second design element, the construction of the task, involves defining the set of valid trajectories which satisfy the designers intentions. The task speci-fication involves encoding constraints on the task so that the resulting trajectories meet the design requirements. Given the design, we may wish to verify that the design specification is correct, that is that all the resulting trajectories of the robot satisfy the design requirements, as well as verifying that the design is fault tolerant with respect to a set of potential faults. Given the specification, the path planning subsystem generates a valid path through the configuration space. A fault tolerant path will utilize the extra de-grees of freedom of the robot with respect to the task to maintain a configuration which is fault tolerant. By choosing a design which encodes relatively few task constraints, and by choosing the constraints from salient features of the task, we correspondingly increase the amount of redundancy of the robot with respect to 19 the task. This increases the set of valid trajectories of the robot, giving us larger freedom when choosing a recovery motion for a fault. Through careful design of the robot, as well as intelligent planning of the task, we are able to construct a robot program which has a large potential for fault tolerant operation. Put another way, the design parameters effect the planning and execution of the robot program, and therefore determine the inherent potential for fault tolerant operation. The second part of the problem, contingency planning, takes the specifi-cation of the robot and the task, and reasons about future faults, and their effects on the robot's ability to complete the task. The end product of the contingency planning is a contingency plan which minimizes the detrimental effects of faults on the robot, and thus maximizes the probability of successful completion of the task. The contingency plan is a path with a set of recovery motions. The recov-ery motions may be explicitly stored, or given algorithmically as a function of the configuration. The design problems, as well as the contingency planning problems must characterize global properties of the configuration space, such as connectedness, to characterize the performance of a given design or contingency plan. We should avoid robot designs which force the use of, and trajectories which contain, configu-rations which become disconnected to the task goal by the introduction of a fault. For these configurations there is no recovery action which completes the task. The last part, fault reactive components, are those aspects which are performed at the time of the fault, and are the most reactive. This includes the 20 fault detection and identification, and the use of effective recovery motions. The main emphases of this work are in the components of fault tolerant task specification, the computation of optimal recovery motions, and global contingency planning of the task. The planning and design problems are significantly harder to solve since they must consider the global aspects of the task, but need be performed only once for a each combination of robot and task. The contingency planning makes use of a performance measure which characterizes the risk associated with a configuration by measuring the effectiveness of recovery motions for a suite of potential faults. Fig. 1.11 gives a description of the components of the methodology that we will employ. The task specification as well as the faults considered are modeled as algebraic constraints on the robot's configuration over time. The configuration space is decomposed into a discrete set of regions so that paths through the configu-ration space can be computed in an efficient manner. From the task specification a measure of utility is constructed. Topological properties of the configuration space under various fault scenarios are computed which are then used for the construc-tion of optimal recovery motions. The effectiveness of the recovery motions is then used to construct a performance measure which ranks the fault tolerant potential of the configuration. Using the performance metric, paths are constructed which maximize the use of fault tolerant regions of the configuration space. 21 Faults Task Specification Model ing of task and faults as constraints Decomposition of Configuration Space Topological Properties Utility Measures Performance Measures Recovery Motions Fault Tolerant Paths Measures over the configuration space Resul t ing path and associated recovery mot ions Figure 1.11: Components of the proposed framework. 1.7 Outline of the thesis We organize the thesis into four chapters: fault tolerant design, fault tolerant trajectory planning, fault detection and identification, and a set of experiments with analyses. Chapter 2 considers the design aspects of constructing a fault tolerant robot, as well as methods for defining a robotic task which allows a greater degree of the fault tolerant capabilities of the robot to be exploited. The method we propose for defining the task, called least constraint, uses a set of constraints on the robot's configuration over time. The constraint-based approach allows us to model a fault as the inclusion of an additional constraint on the configuration space, thus naturally incorporating the kinematic constraints of a fault. Chapter 3 develops methods for producing fault tolerant trajectories for a robot executing a given task. This involves determining the risk of a given fault. 22 computing contingency plans which reduce the overall risk during the execution of the task, and the computation of recovery motions which allow the robot to compensate for a given fault and still achieve the goal. Chapter 4 examines the process of detecting the occurrence of a fault, identi-fying the cause of the fault, and executing an appropriate recovery action. We also investigated fault identification of a collision event, a type of fault not commonly considered, and show how appropriate modeling of the event can allow extraction of collision geometry, which is useful in planning a recovery motion. The theory of this type of fault identification is developed, and some simulation results given. Chapter 5 describes a set of experiments in which a task is described as a set of constraints, and a fault tolerant trajectory is generated using the fault tolerance measure. The first example concerns the generation of a fault tolerant gait for a 4-legged robot. This also demonstrates how easy it is to program a large degree of freedom robot using the LC approach. The second example concerns a Puma 560 manipulator performing a pick-and-place task, and involves 5 actuated degrees of freedom. The trajectories for both examples are analyzed with respect to fault tolerance. Chapter 6 summarizes the main results of the thesis, and discusses future directions for research in fault tolerant robotics. 23 1.8 Thesis Contributions We have developed a new framework for the programming of robots to perform a task in a fault tolerant manner. The methodology encourages fault tolerant behavior at two levels: at the task-design phase by encouraging the designer to omit extraneous constraints which reduce the potential for fault tolerant operation, and at the trajectory generation phase by avoiding critical configurations. We have developed a global measure for fault tolerance which considers the optimal recovery motions over a set of faults, and ranks the configuration in terms of its ability to continue to satisfy the task requirements. Since we do not con-strain the recovery motions, as is common with methods which use a redundancy resolution algorithm to compute the recovery motions, the fault tolerance measure is more likely to reflect the true fault tolerant potential of a configuration. The modeling of the task by constraints also gives rise to an elegant and efficient method modeling faults as additional constraints. This permits one to quickly ascertain the effect a fault will have on the available recovery motions. An efficient algorithm for constructing a recovery motion for a fault has been developed. Using constraints to represent faults allows us to consider new faults not previously considered, such as collisions with an unmodelled object. In addition, dynamic knowledge, such as the discovery of an additional obstacle, can also be easily incorporated using our method. The LC framework used as a method to specify a robot's behavior was first presented in [Pai91]. What is novel here is the use of LC to model faults as additional constraints, the use of this efficient representation to compute the 24 optimal recovery motions, and the construction of a global fault tolerance measure which reflects the effectiveness of the optimal recovery motions to complete the task. The methodology is unique in its ability to deal with robots which are not kinematically redundant with respect to arbitrary task, but which are sufficiently redundant so as to allow the task to be described as ja set of "loose" constraints over time. Prior to this work, little consideration was given to maximizing the fault tolerance of robots executing such tasks. It is believed this type of scenario is common among robots currently deployed, hence there is a great potential for these methods to be applied to a large number of present day tasks, with little additional overhead. In addition, the methods presented are also applicable to, traditional kinematically redundant manipulators. We have developed an efficient algorithm for taking the constraint-based task description, and producing a fault tolerant trajectory. A number of experi-ments have been performed showing the applicability of the method to a number of domains. The resulting trajectories have been analyzed with respect to their abil-ity to sustain a fault, and compared to more traditional methods for accomplishing the same task. Using the LC method a considerable amount of fault tolerance was achieved. The optimization of the fault tolerance at both the design and trajectory generation phase allowed exploitation of more of the inherent fault tolerance of the robot. The computed trajectories make optimal use of the 1-fault tolerant configuration space, and maximize the worst-case utility of the trajectory given a fault. 25 Chapter 2 Fault Tolerant Design This chapter considers design components which affect the construction of a fault tolerant robot program. We will look at both the design of the physical robot, as well as the design of the task itself. The product of these two design decisions in tandem determine the overall fault tolerant potential of the robot. The section pertaining to robot design will be a review of previous work, allowing us to focus on the relatively novel aspects of task design. The problem of task design is similar to that of robot programming in that it requires the designer to take a set of design constraints, and "compile" them into a set of motion/action primitives which accomplish the goal, while satisfying the task constraints at each point along the trajectory. The task can be defined in terms of explicit methods, in which one directly specified the motion, or implicit methods which embody a higher-level approach. The design of the task itself has largely been neglected in previous work in 26 fault tolerant robotics. Typically it has been assumed that the motion is defined explicitly as a particular trajectory to be followed, requiring the manipulator to be kinematically redundant along the entire trajectory. We use the term "task design" to emphasize the fact that we are inter-ested in formalizing the process of constructing a robot program which is fault tolerant. In addition it focuses attention on the design parameters which are to be optimized to obtain a robot program that is robust to faults. The design of the task determines the points in the workspace which are usable, which in turn constrains the configurations of the robot. The risk associated with each configu-ration depends on properties of the kinematics of the corresponding reduced order derivatives constructed at the point in configuration space. By attempting to design the task using constraints obtained from salient features of the task constraints, we hope that additional fault tolerant capabilities can be achieved. Since we require that the robot be fault tolerant with respect to task constraints, and not fault tolerant over the entire workspace, the methodology is applicable to non-kinematically redundant manipulators. 2.1 B ackgr ound Determining the fault tolerant capabilities of a robot executing a task involves examining the kinematic mapping of the robot's joint angles to points in the workspace which satisfy the task constraints. What follows is a brief overview of the kinematic analysis of a robot performing a task as it pertains to the fault tolerant capabilities of the robot. 27 2.1.1 Kinematics of the Task Assume that we have a robot with n actuators, with C C E", the configuration space, giving the set of all possible configurations of the robot. We will denote by q = (91) • • •) 9n) £ C a particular configuration of the robot. We will assume that the workspace of the robot is W C E'". The relationship of joint angles q to points in the workspace x is given by the forward kinematic mapping: X = X:(q), (2.1) where x e W C E"* is a generalized position vector giving both position/orientation of the end-effector, W is the workspace of the robot, and q G C C E" is a vector of joint angles of the robot. As pointed out in [Bur89]: Roughly speaking, the forward kinematic map 'rips' the configuration space manifold apart into pieces; distorts each piece; and combines the distorted pieces to form W (page 265) Given a position in the workspace, the set of joint angles which accomplish this position is given by the inverse of the kinematic map: q = /C-^(x). (2.2) which is generally not unique. For a non-redundant manipulator there may be a discrete finite set of joint angle solutions. For a kinematically redundant manip-ulator the inverse-kinematics mapping produces an infinite family of solutions for each point in the workspace. The family of solutions to Eq. 2.2 can be understood by looking at the differential motion of the manipulator. The linear approximation 28 dx dx _ _ _ dx dqi dq2 dqn (2.4) to the relationship of joint angle rates q G K", and the end-effector velocity x G E"* is given by: J q = X, (2.3) where the Jacobian, J 6 R"*^", is defined as: The solution for all joint rates q which satisfy Eq. 2.3 is q = J+x + ( / - J + J ) i , (2.5) where the superscript "+" indicates the pseudo-inverse, and z is an arbitrary joint velocity vector [Alb72, KH83]. The term (/ — J^ J) is the projection onto the null-space of the Jacobian. The family of solutions of Eq. 2.1 forms a (n — m)-dimensional hyper-surface in n-dimensions called the self-motion manifold [KH83]. Trajectories along these hyper-surfaces do not affect the end-effector po-sition/orientation, and so are in the null-space of the manipulator Jacobian. The null space of the manipulator's Jacobian, the set of vectors satisfying x = 0 in Eq. 2.3, gives velocities tangent to the self-motion surface. The interplay of the two design problems is evident from Eq. 2.1. The specification of the task determines the trajectory that the robot will ultimately follow, thus determining which points, x, of the workspace are to be used. The design of the robot involves choosing design parameters, including the number of actuators, n, as well as the link lengths and other geometric properties, all of which affect the kinematic mapping relation, /C. 29 2.1.2 Kinematic Effects of a Fault A fault need not remove a usable actuator from the system. For example, we may consider the collision of the robot by an unmodeled obstacle as a fault (as discussed in Chapter 4). However, the most commonly anticipated fault will involve immobilization of an actuator. Kinematically we may consider a robot with a fault that results in k actuators becoming immobilized as equivalent to its k-th reduced order derivative, which is the robot with {n — k) effective degrees of freedom, with link-geometry which is consistent with immobilization of the actuators [PAK94]. Suppose that we have a fault that has resulted in a new lower-dimensional configuration space CROD- Determining whether the fault will still permit success-ful completion of the task involves determining whether we can find a family of joint angles q(t) € CROD which satisfies the task requirements. While it should be clear that a task description which admits the largest family of trajectories is more likely to be tolerant of the fault, let us assume that we can identify a number of points in the workspace which are critical to the task. Characterizing a task using a set of critical points is a method found in [PAK94, LM94b]. Determining whether a critical point x*^ G W is reachable by a reduced order derivative involves computing the preimage of x'^ under the relation JC, denoted p, and determining whether a trajectory can be constructed in CROD from the point of failure, to a point in p. Clearly if p fl CROD = 0 then there can exist no such trajectory. In addition, p fl CROD may consist of a number of connected components which are not reachable from all failure positions. 30 We can better understand the preimage, p, of a point by looking at families q along the self-motion manifold at the critical point. For a critical point of a task, the global fault tolerance of a point is related to the characteristics of the self-motion manifold at that point since it defines the range of joint angle values that correspond to a fixed point in the workspace of the manipulator [LM94b]. The bounds on the joint angle values along the self-motion manifold determine the set of configurations for which there exists a configuration to the point in the work space. Consider the kinematically redundant 3-R planar manipulator illustrated in Fig. 2.1. The self-motion manifold of configurations of the manipulator corre-sponds to one-dimensional curves embedded in the three-dimensional configuration space of the manipulator, T^. Projecting T^ onto the g293-plane gives us the plot in Fig. 2.2 in which each point in the plane corresponds to the family of configu-rations whose distance from the origin of the workspace is a constant. The curves represent the self-motion manifolds of various configurations of the manipulator, and represent the families of vectors q whose end-effector positions are at a fixed distance from the origin. Consider the 4 points x \x^ ,x^, and x^ in the workspace in Fig. 2.1 with corresponding configurations p^,p^,p^ and p"* of Fig. 2.2. Point x^ corresponds to a point on the reach singularity in which the manipulator is fully extended. The self-motion manifold for this configuration is a single point, hence there is no other configuration which corresponds to the same workspace point x^. Therefore any position along the reach singularity surface is inherently non-fault-tolerant. Point x^ corresponds to a point whose distance from the origin is slightly 31 Non-Fault Tolerant Curve Fault Tolerant Curve Figure 2.1: Planar 3-R manipulator with unit link lengths (adapted from [LM94b]). The manipulator is 1-fault tolerant in the region bounded by the two circular arcs. larger than one link-length. At this configuration the self-motion manifold is very large, spanning almost the entire range of 52 and ^3. Consequently any configura-tion which is slightly farther than one link-length away from the origin is inherently fault tolerant since there is such a large family of kinematic solutions. An indication of the fault tolerance of a given configuration is the bounding-box of the self-motion manifold of the position. The bounding boxes for the points x^ and x"* are denoted by the dotted lines in Fig. 2.2. We can see that the bounding-box of x^ is much smaller than x^, hence there is a larger family of configurations which is able to satisfy the positioning of the manipulator at x^ as compared to x l We can see that for points in the work space that are closer than one link-length from the origin, such as x^, there are two distinct self-motion curves that are disconnected, resulting in two non-overlapping bounding boxes. For points in 32 Null-Curves of 3R planar manipulator Figure 2.2: Self-motion manifold for the 3-R manipulator (see Fig. 2.1). The self-motion manifold is a one-dimensional curve in E ,^ which is projected onto the g293-plane. Curves labeled p^,p^,p^ and p'' correspond to the workspace points of x^,x^,x^, and x^ respectively of Fig. 2.1. 33 this region of the workspace not all configurations are reachable by remaining in one component of the self-motion manifold. For a fault resulting in the immobilization of the joint, we may guarantee that the manipulator is able to reach a given point in the future by constraining the range of motion of each of the n actuators. Determining the degree of fault tolerance of a configuration in the above example is greatly simplified since it purely a function of the kinematics of the manipulator. The fact that all points in the configuration space are feasible also greatly simplifies the analysis. If we have further constraints imposed by the task, for example due to the addition of an obstacle, the analysis becomes much more complicated. We will introduce a method for specifying the set of trajectories which satisfy the task constraints using a set of constraints in Section 2.3. Since the introduc-tion of a fault can also be described as the addition of further constraints to the task description, we have a simple mechanism for determining the fault tolerant capabilities of reduced order derivatives. We will show how faults can be modeled as additional constraints in Chapter 3, and show how to compute recovery motions given a fault. 2.1.3 Adding Redundancy Designing a fault tolerant robot involves the addition of mechanisms which, when a fault occurs which leaves a mechanism immovable, can be used to take over the re-sponsibilities of the failed mechanism, and thereby continue to perform the desired 34 task. Sreevijayan et al. describe a subsumptive architecture involving redundancy at four levels [STT94]: 1. Dual Actuators : extra actuators per joint. 2. Parallel Structures : extra joints per DOF. 3. Redundant Manipulators : extra DOF's per arm. 4. Multiple Arms : extra arms per system. The choice for how many additional actuators to use, and the choice for the kine-matic design parameters is dependent on the demands placed on the robot by the task, as well as financial and other constraints. If feasible we may replicate each of the actuators with a parallel actuator whose axis is aligned with the original. This is illustrated in Fig. 2.3 where (a) represents the non-redundant 3-R manipulator, and (b) gives the 6 DOF manipu-lator obtained by adding a parallel actuator to each of the 3 actuators in (a). The manipulator in (b) is called l-fault-tolerant since it is able to sustain any one fault and still achieve any positioning task in the original workspace [PAK94]. While the manipulator of Fig. 2.3(b) has the benefit of being fault tolerant throughout the entire workspace, it does so at the cost of doubling the number of actuators. Alternatively we may choose to use fewer additional actuators, and obtain a robot which is fault tolerant for a subset of the original workspace. When a fault is introduced it alters the set of accessible configurations of the robot. Determining the abilities of a robot when a fault has left one or more 35 (a) Figure 2.3: Depicted in (a) is a 3-R manipulator, and (b) a 6 DOF manipulator which is first-order fault tolerant. Each joint has a parallel joint which, in the event of a fault, can be used to position the arm. The example is modified from [PAK94] and is illustrative of designs found in [WDHC91]. actuators immovable requires that we look at properties of the kinematic mapping of joint angles to positions in the workspace. 2.1.4 Defining the Task The process of designing a task involves a compilation of the task constraints into a robot program which achieves the task. Programming a robot is a very complex problem, so it is a natural goal to, subdivide it into a set of simpler sub-tasks; one example is proposed in [McK91, p. 485]: Task level The definition of the task within the framework of the designer's con-ceptual model of the production process. Action level A sequence of actions completing the task such as "insert part", "slide end-effector", "place object". 36 Robot level Sequence of "robot machine code" further decomposing the action. For example, "pick up part A" may translate into "open manipulator", "more gripper to grasping position", "grasp part", "raise gripper to position B". Joint level At this level control systems for position, velocity and force directly determined the joint parameters. Previous work in fault tolerant robotics assumes that we have the task de-composed to the robot level. The goal of fault tolerant trajectory generation is to produce a joint level description of the task which is able to tolerate faults during the execution of the program. The methodology that we present, as we shall show in the subsequent sections, spans the four levels above. By utilizing the flexibility obtained through careful definition of the task, we hope the resulting trajectories are able to express more of the inherent fault tolerance of the robot with respect to the task. Task constraints may include avoiding obstacles, ensuring that joint angle limits and joint angle velocity constraints are satisfied, as well as satisfying con-straints that are particular to the specific task. Pai has broadly classified approaches to robot programming into two types: explicit, and implicit approaches [Pai91]. Explicit approaches are those approaches in which the designer explicitly defines the motion of the robot. Typically the mo-tion is specified via a set of primitives provided by a robot programming language (such as [TSM83, Una83]). The advantage of this approach is that since the user is directly specifying the trajectory, fine motion control is possible. The price for this performance is that the user is often forced to make arbitrary decisions in order 37 to execute the motion. While these decisions may be arbitrary with respect to ob-taining a trajectory which satisfies the designer's intentions, they may have large implications to the overall fault tolerance of the resulting program. For example, if the designer chooses to force the motion of the robot through a point which is inherently fault intolerant, such as a reach singularity, the fragility of the resulting robot program is inevitable. Implicit approaches differ in that they require the user to specify the high-level goals of the task, omitting low-level details. Examples of these types of approaches include motion planning [Lat91, LP82], as well as optimal trajectory planning [BDG85, SH85]. These approaches are powerful and provide good results as long as the method is well suited for the task. For example, there is little point in using an optimal trajectory planning method which minimizes the joint angle velocities if the joint angle velocities are not important to the problem. If the optimality criteria for trajectory generation do not reflect the fault tolerance of configurations along the path, then it is very likely that the resulting paths will not be very tolerant to faults during the execution. We shall give an example of explicit and implicit task specification, and then suggest an alternative which is a compromise, sharing features of both. 2.1.5 Example of an Explicit Task Description One method for specifying the task at the action level is by specifying a set of points in the workspace called knot points [McK91]. Typically the trajectory is constructed using cubic splines through the knot points. The ability to sample the 38 spline at fine intervals allows one to perform smooth motion of the end-effector. A second benefit of using a cubic spline to specify the trajectory is that the positions and the velocities of the end-effector are guaranteed to be continuous. One problem of the approach is that it is difficult to ensure that the tra-jectory does not pass through a singularity, causing the joint velocities to exceed their limits. To overcome this we may define the set of knots in joint space, and perform a smooth interpolation between points in joint angle space. Specifying the task in joint space has its own problems, however. Obstacle constraints, which are easier to specify in the workspace of the robot, may be difficult for the designer to visualize, making the construction of the task error-prone. Furthermore, due to the highly nonlinear nature of the forward kinematics, end effector motion arising from a joint interpolated motion may yield unexpected results. 2.1.6 Velocity Profile Specification Another method of specifying the task is by specifying a velocity profile, x(t), of the robot's end-effector. This has the advantage that the velocity of the end-effector is defined for all points along the trajectory, and is not the by-product of the interpolation, as is the case with knot-point specification. Velocity profiles, however, have the same difficulties with respect to singularities and obstacles as described above. 39 2.1.7 Example of an Implicit Task Description Examples of implicit methods are described in [Lat91]. As an illustrative example consider the problem depicted in Fig. 2.4 of moving a polygonal robot, denoted by the triangle, in a room with obstacles which are also polygonal. To simplify the problem we will ignore rotations of the robot, and will constrain the robot to the region inside the room walls. A useful technique is to choose some reference point on the robot, and to compute the portions of the configuration space which correspond to some point of the robot colliding with some obstacle (either the square obstacle or the walls). Each obstacle results in a collision-space obstacle with edges arising from features of the robot or the obstacle. Once the computation of the configuration space obstacles has been performed, the problem reduces to computing a path of a point in the configuration space among a set of polygonal configuration space obstacles. This example illustrates the power of implicit techniques in allowing the designer to concisely define the task at a high level; the designer need only specify the geometry of the robot and the obstacles and specify the initial and final con-figurations. However the method does not provide the designer any fine control of the particular trajectory that is chosen. Since there is little control over the specific trajectory which is chosen there is no ability to fine tune the trajectory. For example, there is no way to indicate that we would like to exploit regions of the configuration space which are more fault tolerant. As we have seen there are benefits and drawbacks when using both implicit and and explicit methods. We will now discuss an alternative approach called 40 (a) Work space analysis. (b) Configurations space analysis. Final Configuration /\ • -Obstacle Initial Configuration y \ Wall A Reference Point Configu Space Obstac I iration >-es • ' ' \ ^ Collision-free Path ?.' Figure 2.4: Example of an implicit method for describing a task. The initial and final configurations are given by the triangles. A path is constructed by computing a path in the configuration space which avoids the configuration space obstacles. Least Constraint which shares properties of both; tasks can be described in a high level while still permitting fine control over the specific trajectory when needed. 2.1.8 Least Constraint Robot Programming In an effort to decrease the complexity of specifying the control of large degree of freedom mechanical systems, Pai introduced a method called Least Constraint [Pai91]. LC allows the designer to express the task using a set of constraints on the configuration of the robot. The constraints describe the motion using large time-varying sets of non-zero measure to represent "goal" regions, and requiring the robot to be inside this set throughout the trajectory. As long as the task is specified correctly we should not care which point in the region is chosen. The 41 regions are defined using a conjunction of inequality constraints. For systems with many degrees of freedom it may not be convenient or natural to express constraints in a single space. To alleviate this, a number of domain systems, {Vr.ie / } , which relate to each other with linking functions lij :Vi ^Vj, {i,j) eLcI X I, which are assumed to satisfy the consistency condition that the corresponding mapping diagrains commute. We assume that there is a basic domain VQ. The choice for the domains is left open to the designer, and can be an arbitrary manifold, however in practice they would likely be copies of R". Using the domain systems allows the designer to specify a constraints in any domain which is convenient, and using the linking functions to lift the constraints from one domain to the next. The motion specification in LC consists of a system time-varying inequal-ity constraints Pa = fa{x{t),t) < 0, where /^ : r>i x R ^ R, a e A, (2.6) where x{t) is a time-dependent trajectory in I>j. Once the trajectory is specified using the constraint functions, the control actions are computed using constraint satisfaction. The motion is obtained by lifting each of the constraints fa from their respective domain di into the domain VQ, to produce a trajectory x{t) G VQ which satisfies the constraints at all times t. For efficiency, Pai implemented the constraint satisfaction using a fast relax-ation method, and utilized automatic differentiation [Pai91]. The control strategy 42 relies on the property of the constraint specification that, given a configuration which satisfies the constraints at a particular time, computing a configuration which satisfies the constraints for a time shortly later can be done very efficiently. There is a close similarity between constraint satisfaction and obstacle avoid-ance. We can view constraint surfaces as forming obstacles in a related space, and the satisfaction of the constraint corresponding the point being in the exterior of all "constraint objects". It is not surprising therefore, that obstacle constraints are easily expressed using LC. An alternative approach to LC, similar in that it is a constraint-based ap-proach is found in [ZM95]. Using a formalism called constraint nets, a problem can be specified, and in many instances the controller synthesized, while also sat-isfying constraints on safety, reachability, or persistence. 2.2 Design of Fault Tolerant Robots Paredis and Khosla [PAK94] examined the task of constructing fault tolerant planar manipulators. In contrast to [Mac90] which attributed fault tolerance to a specific posture, fault tolerance as defined in [PAK94] relates to the manipulator as a whole. A manipulator is considered A;-fault tolerant if and only if every (n — A;)-degree of freedom reduced order derivative can still accomplish the task. Since each of the k actuators may be frozen at any angle, showing that a manipulator is A;-fault tolerant requires that we prove that there does not exist a set of /c-joint angles for which the k-th reduced order derivative is unable to accomplish the task. The task was described by a set of points in the workspace which are critical to the task. 43 Paredis and Khosla described analytically the necessary and sufficient con-ditions for fault tolerant planar manipulators, and showed how these conditions could be used to design a 5 DOF planar manipulator which is 1-fault tolerant. For spatial manipulators, the geometric complexity of the constraints makes the analytical design of a fault tolerant manipulator infeasible. Instead a numerical approach is described which makes use of a penalty function to produce the set of Denavit-Hartenberg parameters which satisfy the task requirements. We will limit our discussion to the planar case. 2.2.1 Planar Case A planar manipulator, denoted by At = (/i, • • • ,ln), is described by the set of link lengths li. The task is defined as a set of positions/orientations in the workspace, W = {{xj,yj,(l)j)}. The fault tolerant workspace, denoted FTWS of a /c-faillt tolerant n DOF manipulator is the set of points in the workspace that are reachable by all possible {n—k) reduced order derivatives. The manipulator is A;-fault tolerant for the task W so long as W C FTWS. To find a set of link lengths k which yields a 1-fault tolerant manipulator, we use induction on the number of links, and split the manipulator into two parts: the last link /„, and the first (n — 1) links which comprise a new manipulator M.* = (/i, • • • ,/„_i). For M to be 1-fault tolerant M* must be able to reach the points in the new workspace W* in any orientation when all actuators are operative, and in at least one orientation when any one of the (n — 1) actuators 44 are frozen at an arbitrary angle. The new workspace W* is defined as W* = { {Xj - In cos (j)j, Vj - In Siu (j)j) G E^ | (^^.^ y.^ ^.^ g ^ | (2.7) This problem is easier to solve due to the radial symmetry of the manipu-lator. The manipulator M. is first order fault tolerant if and only if W* C FTWS* = Ux, y) em^ R' < ^x^ + y^ <R^\ (2.8) where R'^ and R^ are the closest and farthest radii reachable by A1*. Analytical expressions for Re and Rf can be found by considering all of the reduced order derivatives M.i, Mi{ql) = {h,---,li-2,Ci{q{),k+i,---,Q (2.9) where Ci{q{) is the new length between joints {i — 1) and {i + 1), \h-i-h\<C,{ci{)<{k^i+k). The value q{ is the angle at which the actuator is frozen. When maximizing the reach of the planar manipulator, the worst angle for a reduced order derivative is q- ~ TT, for which Ci = \li-i — li\. This gives a reach of R{ = L - {li_i + k) + \k_i - k\, (2.10) where n - l L=Y.k, (2.11) is the total length of the manipulator Ai*. If we set lo = In = L, then this expression is correct for i = 1 and i = n. The final expression for the maximum reach radius is thus: R^ = mmR{. (2.12) 45 To compute R^ we use the fact that the minimum distance between two endpoints of a chain of rigid links is max jo, length of the longest link — ^ lengths of other Unks} . (2.13) Depending on whether £j is the largest link we set q( = 0 or q{ = ir for maximum radius R^ - max{0,2(/i_i + k) - L, 21^^ -L + {k^i + k) - l^ .^i + li\}. (2.14) If we set /o = /„ = 0 this expression is correct for i = 1 and i = n, giving R" =maxRf. (2.15) Using the constraints of R^ and R'^ above, Paredis and Khosla were able to prove that 5 DOF were sufficient for 1-fault tolerance, in the plane. A fault tolerant workspace without holes {i.e., R'^ = 0) is obtained by setting the first four link lengths equal, A4 = (/, I, I, I, I5), where 5^ can be chosen freely. 2.3 Fault Tolerant Task Design Before we describe our method of defining a task we will give a list of properties which an ideal motion specification language would have. Some of these properties have already been mentioned in Sections 2.1.8 and 2.1.5 when we contrasted im-plicit and explicit methods for describing a motion. The properties are derived from different facets of the motion specification problem; some properties are desired of any motion specification; others arise from the specific demands of producing a fault tolerant path. The properties reflect the following three ways in which the specification will be used: 46 • To verify that the specification is correct. This involves determining whether a vahd trajectory exists which meets the design goals. • To construct paths. • To efficiently assess the effect of faults at a particular configuration and the overall fault tolerance of a given path. 2.3.1 Ideal Properties of a Specification PI Completeness. The specification should be complete and unambiguous with every possible state uniquely classified as a valid or invalid state of the system. P2 High Level Description The talk specification should permit the goals of the task to be expressed at a high level. The constructs of the language, where possible, should reflect the demands of the task, and not describe a means of satisfying the demands. P3 Fine-level control The specification language should allow the designer to have a fine-level con-trol over the resulting motion. When a task requires fine-motion control tailored by the designer, the constructs of the specification language should permit this. P2 and P3 reflect the respective properties of implicit and explicit methods of robot motion programming, as mentioned in Sections 2.1.7 and 2.1.5. These two properties are often antagonistic; however LC, being an intermediate approach and sharing properties of both explicit and implicit methods, is a compromise on these two approaches. P4 Use of salient features of the task. Where possible the task should be defined using only the salient features of the task's domain, and should not contain constraints which are artifacts of the specification itself. To this end, the designer should not have to make arbitrary decisions simply to make a well-formed specification (as is often the case in "explicit methods" described in Section 2.1.5). 47 An example of a specification which introduces additional constraints as an artifact of the specification language itself is the following. Suppose that we specify a task using joint-interpolated motion commands through a series of knot-points in the configuration space, through which the end-effector is to pass. The joint-interpolated motion will produce a motion with constant joint velocity rates. This additional constraint will likely not refiect any real constraint on the task, and will likely produce a motion which passes through regions which are less fault tolerant than necessary. P5 Explicit use of tolerances. When the designer has specific knowledge about the task tolerances the speci-fication should incorporate this directly. Knowing the tolerances allows one to design the task such that a larger family of "valid" trajectories are included in the specification. This larger set of trajectories can be used in choosing a trajectory which is more fault tolerant, thus allowing more of the inherent fault tolerance of the robot to be used. P6 Ease of modification. Incremental changes during the design process should be handled with ease. The designer should be able to easily alter task parameters, such as the posi-tion and shape of obstacles, without having to adjust the entire description. This requires that the effects of common task parameters should have localized effects to the task description. P7 Fast Verification. Given a specification of the task, the process of verifying that the specification is consistent, and permits a valid trajectory accomplishing the goal, should be computationally inexpensive. Additionally, when the specification is inconsis-tent, the verification process should indicate which portions of the specification should be changed. P7 and P6 reflect the fact that the process of constructing a specification is 48 an iterative process, often requiring many verification/modification cycles. To aid in this process, the specification should allow a parameterization of the task using measures which are meaningful to the designer. P4 and P 5 aid in this process. P8 Decoupling of specification and implementation. The specification should not concern itself with the means of producing the trajectory through the configuration space. P9 Easy inclusion of dynamic information. One source of dynamic information is the use of sensing by the robot, which must be characterized by the specification. Another source of dynamic infor-mation is the introduction of a fault, which may be considered an environ-mental interaction for which there is no direct sensing. A specification for a fault tolerant system should have an easy mechanism for describing unex-pected interactions, a means of computing the effects of a fault on the system, and a means for describing effective recovery actions. P9 is crucial for planning fault tolerant tasks. When a fault occurs we are left with a new robot, the reduced order derivative, which requires a re-planning of the motion to complete the task. LC provides a natural means of reasoning about the effects of a fault on the overall task since we can model faults as additional constraints which are imposed at execution time. We will take an approach that is similar to the LC specification of [Pai91], in which motions are described by assertions of configuration- and state-dependent constraints on the robot which must be maintained throughout the entire trajec-tory. Assume that the configuration space of the robot is denoted by C C E", and that a particular configuration is denoted by q, q = {qi,---,qnf. (2.16) 49 A trajectory is simply a mapping 9{t), 0 :R+ ^C. (2.17) from time, E"*" (t = 0 denoting the start of the task), to configurations. We will not impose any restrictions on the trajectories considered, however we will insist that they be continuous, 9 G T, r - { / : E + ^ C , / G C ° , } . (2.18) Since the specification deals with time- and configuration-dependent con-straints, it is useful to define an abstract space C = C X E+, (2.19) the product of the configuration space and time. We will call C the time-augmented configuration space. The end product of the specification, which we will describe later, is the construction of the feasible set J^CT C C. (2.20) TCT, the feasible configuration-time space, forms a complete spec-ification because it uniquely classifies all trajectories as valid or invalid. TCT gives, at each instant of time, all configurations which meet the specification re-quirements. The interpretation of the temporal dimension of points in the space C may be literal, or it may simply denote a single parameter which characterizes the progress towards the goal. When explicit time constraints are not present, it is still useful to use a single time-like parameter to characterize the progress towards the 50 goal. Constraints on C can easily express time^ordering and deadline constraints. Essentially we can think of transforming the path planning problem in C with time constraints to an equivalent path planning.problem in (C x E"*") with "obstacles" representing the time constraints. We show later that the explicit inclusion of time in the task specification provides us with a natural measure of utility of a configuration. 2.3.2 Valid Trajectories, Verification Given a task described by J-^CT, all trajectories, 9, are classified as valid or invalid as follows. Definition 2.1 (Trajectory/Configuration Validity) Given the set ^FCT, a trajectory, 6, is called valid if, and only if V t > 0 , {e{t),t) eTCT. (2.21) Likewise a configuration q' is valid at time t, > 0 iff (q',tj) G TCT- • The process of determining whether the trajectory satisfies the specification involves a verification step, namely Eq. 2.21. 2.3.3 Constructing the Specification Constructing a robot program using LC involves defining a set of configuration- and time-dependent constraints, and from the composition of these constraints we form 51 TCT• Care must be taken when specifying the constraints since TCT is not simply defining a single trajectory, rather it is defining the entire family of acceptable trajectories. If constructed properly one should not care which configuration q'' is used so long as (q°,^o) ^ TCT. The specification is composed of a set of constraint functions of the form: hi^j : C -> E, (2.22) and the corresponding set of predicates gi,o •• {hij < 0). (2.23) The task specification is simply the set of constraints G = {gij}, denoting the Disjunctive Normal Form [GN87]: G '^ V A 9i.^ (2-24) 1=1 j=i TCT = {qeC\G{q)}. (2.25) where A^v gives the total number of OR-terms, the z-th term composed of A^Ai AND-terms. 2.3.4 Linking Functions The constraint functions of Eq. 2.22 are examples of constraints in the basic domain, VQ, of an LC specification [Pai91]. For our problem, the basic domain is Vo = C. The set of valid trajectories will be determined using constraints in the basic domain, denoted by T>Q in [Pai91], or C. 52 We will assume that each constraint h is supplied by the designer directly, or by specifying an alternative constraint in an alternative domain Pfc. Each domain V^ is related to the basic domain by the composition of the linking functions Po ^ • • • -^ ^fc- (2.26) From this we can construct the equivalent constraint function, h, in the domain Vo, such that h = h'' olkiO---oloj. (2.27) 2.3.5 Driving Constraints Using algebraic inequalities to define the task allows us to easily express static constraints, such as joint angle limits or configuration space obstacles, as well dynamic constraints which drive the robot through the valid configuration space towards the goal. We will refer to the time-dependent constraints which are used to push the robot towards the goal as driving constraints. To illustrate the use of time-dependent constraints in producing a motion, consider the simple example in Fig. 2.5, in which the goal is to produce a trajec-tory to the goal position located at the intersection point of the two constraints. The driving constraint reduces the set of acceptable configurations over time until convergence at the goal position. 53 / i d ( q , i ) = 0 Figure 2.5: The use of static and time-dependent driving constraints in producing a motion. Notice that there is no explicit representation of the goal; the goal is im-plicitly defined using the driving constraints of the system. The goal state(s) can be thought of as the set of configurations (possibly empty) which satisfy the specification at some time tmax- The design process involves constructing a set of constraints which ensure that the valid configurations converge to the goal states by some upper time-bound m^ax-2.3.6 Examples of LC Specifications The following are some examples of simple task constraints that are common to robot tasks, and illustrate the ease with which task constraints, such as obstacle avoidance and end-effector placement, are expressed using LC. In the first example we wish to assert that the robot is not to collide with an obstacle. Example 2.1 (Obstacle Avoidance) Consider a navigation task in the plane with C = K ,^ with a triangular configu-54 ration space obstacle, as depicted in Fig. 2.6. ( C 1 , C 2 ) (gi>?2) ( 0 1 , 0 2 ) (bubi) Figure 2.6: The configuration space of a robot is E^ with q = (91,92) denoting the robot. The triangular configuration space obstacle is given by the vertices (oi, 02), (61, 62) and (ci,C2). The non-intersection constraint can be expressed us-ing three algebraic inequalities, each denoting that q lie in one of the half-planes constructed by the edges of the triangle. Constraining q not to lie in the interior of the triangle is described using three constraints. hn = (92-^2)(ci-61) - (91-6i)(c2-62), h2i = (92-02)(&i-a i ) - (91-a i ) (62-02) , hi = (92-C2)(a i -c i ) - (91-ci)(a2- t ;2) . (2.28) (2.29) (2.30) Each constraint corresponds to configurations which occur on a half-plane con-structed with one of the three edges. The configurations which are safe from collision are described by (/in V /i2i V /131). By reversing the sense of each of the constraints, and replacing disjunction over the three constraints with conjunction, we enforce the configuration to be in the interior. This is useful for specifying support-regions for static stability, such as found in legged locomotion tasks [RP97]. 55 A second illustration of LC is the task of moving the end-eff'ector to a given position in the workspace. This is achieved using driving constraints which ensure that the distance of the end-effector to the goal point decreases over time. Example 2.2. Placing an object on a table Consider the pick-and-place task of putting the object held by the gripper onto a Figure 2.7: LC task specification for the placement of an object onto a surface (modified from [Pai91]). specified place on the table (as depicted in Fig. 2.7). The task is specified with constraints on positions of the gripper in the task space of the manipulator. The constraints / i and /2 ensure that the manipulator stays within the bounds of the table, /4 ensures that the manipulator does not make contact with the table. The end position of the object is the center of a cone that is given by the constraints /s and /e. The motion of the manipulator is achieved with the driving constraint /a which forces the height of the manipulator to decrease over time. The cone constraint ensures that position of the manipulator converges to the end position. 56 These examples illustrate the incremental nature of defining a task in LC. Each constraint is an assertion of some property that the designer would like to impose on the resulting trajectories. Furthermore these assertions can be added during the design process as the verification process brings new factors to light. 2.3.7 Limitations The LC specification is limited to first-order predicates over C, and does not allow the use of the existential operator. Hence many properties that require second order logic cannot be expressed. Second order logic is useful for expressing temporally indeterminate properties such as "p(q) will occur in the future" or "p2(q) will be true some time after pi(q) is true. For example, we cannot express predicates such as: V(q,i) finished-a{q^,t) =^ 3t'> t, such that startb{q,t'). This makes it impossible to express deadlock conditions directly in the spec-ification. If we want to verify that a property is true of the specification, it must be verified using other techniques. Another shortcoming of the LC specification is that it does not allow one to express constraints which are functions of the joint velocities q. If the constraints on the joint velocities are complex functions of q, then we may make the configuration space include the joint velocities, i.e., C = q x q [SPA99]. Often, however, it may suffice to simply place bounds on the magnitude of the joint velocities. In these circumstances we may omit the joint velocity constraints from the specification and ensure that the velocity constraints are satisfied when constructing paths. 57 Later we will show how to decompose the configuration space into a set of non-overlapping convex regions. Letting a vertex represent each region we con-struct a graph, G = {E,V). Using this graph we can denote sets of trajectories by listing the vertices of the graph through which the trajectory passes. Using the graph of the feasible configuration space, velocity constraints can be expressed with appropriate constraints on the edges of the discrete graph. 2.3.8 Ranking Trajectories The problem of taking the task specification, expressed in LC, and producing a trajectory satisfying the constraints is an example of a traditional trajectory planning problem, and is described in [Pai91]. Pai used an iterative method which took the current configuration and a set of constraints, and using a fast relaxation method produced a nearby configuration which also satisfied the constraints. The resulting set of solutions comprised the trajectory of the robot. In this way LC was being used as a "reactive" method for producing the trajectory. We are interested in producing more than a valid trajectory through the configuration space; we seek a trajectory which is fault tolerant. Using a fault tolerance measure which assesses the ability of the robot to complete the task given a fault, we will show how to produce a fault tolerant trajectory which also satisfies the task constraints. The LC specification only requires that each point along the trajectory sat-isfies the constraint predicates. The goal-motion is implicitly described as the trajectory obtained by satisfying the constraints over time. For a finite task it is 58 sufficient to satisfy the task for some time interval [0,trnax], where it is assumed that convergence to the goal configuration is guaranteed by appropriate selection of the constraint functions. For infinite tasks we must satisfy the constraint functions for all i > 0. From the use of time, either literally, or as an parameterization of progress towards the goal, it should be clear that the length of time for which a trajectory satisfies the constraints is a meaningful measure of the utility. Given the implicit definition of the goal of an LC specification, we will introduce definitions for satisfiability and utility which allows us to construct an objective function. Computing the best trajectory is therefore a classical optimal control problem [Kir70] in which we seek a trajectory (not necessarily unique) which obtains a maximum utility. The verification process confirms that the task constraints are satisfied over some time interval, which we shall denote by the predicate sat() as follows: Definition 2.2 (Satisfiability of a trajectory) We say that a trajectory 9 satisfies a feasibility set F, for a time ^max, denoted sat(^, F, ^max), if it remains entirely inside the set F up to a time imax^ sat(^,F,t„ax) ^ VtG[0,i„ax], {e{t),t)eF. (2.31) D We define satisfiability in terms of an abstract feasibility set F , enabling us to alter F dynamically. Thus F captures the task constraints as well as any additional constraints resulting from additional sensing or faults. 59 For a robot with no additional fault constraints, we say that a trajectory 0 satisfies the task for a time imax iff sat(^, J^CT, tmax)-Since the only requirement is constraint satisfaction, the only meaningful measure of utility is the length of time for which the constraints are satisfied: Definition 2.3 (Utility of a trajectory) Given a feasibility set F , and a trajectory 9 : W^ ^ C,9 E T, the utility of the trajectory with respect to the feasibility, denoted util(^, F) , is: util(^,F) = sup{i |sat(^,F,t)}. (2.32) D The utility of a trajectory ^, given a robot with no additional fault con-straints, is util(^,J'^Cr). (2.33) 2.3.9 Optimal Utility Paths Given the definition of utility, we seek a trajectory T which maximizes this measure. This is an example of an optimal control problem where we seek the optimal trajectory Topt such that: 9opt{F) = argmaxutil(^,F). (2.34) We can see that the maximum utility obtainable is a function of the spec-ification G, and the topology of the (reduced) feasibility set F. Specifically it is 60 related to the utilities of the valid configurations which can be reached by a given configuration. For a robot with no additional fault constraints, the optimal utility path is given by eopti^CT). (2.35) Definition 2.4 (Reachability set) Given a set F C TCT representing a (potentially restricted) feasibility set, and a pair q — (q'^jto) G TCT^ the set of points in TCT which are reachable from q at time to is given by i?(9, F): R{q,F) = {{q\ti)eF, U>to\39eT, (^(io) = q°) A (^(t,) = q^) A {yte[to,h], {9{t),t)eF)}. (2.36) R{q, F) is called the reachability set of q in F. • Again, for a robot with no additional fault constraints, the reachability set is given as R{q,TCT), (2.37) which gives the all accessible configurations for a robot with no additional fault constraints. 61 2.4 Decomposit ion of t he Valid Space Configuration space path planning can be roughly broken into two types of tech-niques: exact and approximate methods (see [Lat91] for survey). The difference in the two methods is in the way in which the valid configuration space is represented. Exact methods do not approximate the valid configuration space and are thus more accurate, however this is at the cost of increasing the computational complexity of planning a path through the configuration space. An important class of approxi-mate methods decompose the valid space into a set of disjoint regions called cells, The advantage of using approximate decomposition methods is that it allows us to represent the valid configuration space using a discrete graph of vertices. This greatly simplifies the problem of computing a path through the configuration space. We will perform a similar approximate cell decomposition on the time-augmented configuration space, C. Each cell has a unique label Vi, V = {vt}. The interior of the cell is denoted Cell{vi) C C. We assume that the decomposition includes the entire time-augmented configuration space, that is, C = ]JCell{vi), i and that they are non-overlapping, yi^j, Cell{v,)nCell{vj) = ^. Each cell is classified as valid, if Cell{vi) C TCT, invalid if Cell{vi) n TCT = 0, and mixed otherwise. Figure 2.8 illustrates a time-augmented config-uration space that is decomposed into a set of regular n-dimensional rectangular regions. 62 Valid , Cells t 1 S ^ • >^ ^^ J-• C Invalid Cells Figure 2.8: Decomposition of the time-augmented configuration space into 20 rect-angular cells. The decomposition yields 4 valid cells, 2 invalid cells, and 14 mixed cells. If required we may further refine the decomposition by further sub-dividing mixed cells into smaller cells, some of which may be valid, invalid, or mixed. The classification of the cells as valid, invalid or mixed involves determin-ing whether a surface of TCT intersects the boundary of a cell. The boundaries of J-CT are formed by taking portions of the constraint surfaces of the form: hi[q) 0. (2.38) To determine whether the boundary of TCT intersects a given cell we may first identify which of the constraint surfaces hi = 0 intersect a given cell. For each constraint function which intersects the cell, we must then determine if any part of the surface is used in forming the surface of TCT. Determining solutions for h{q) = 0 is a constrained optimization problem. Details of the decomposition process can be found in appendix A. 63 2.4.1 Uniform Decomposition To simplify the decomposition process, as well as to ease the determination of topological properties of the decomposed space, we decompose C by regularly subdividing each of the n-dimensions into equally spaced intervals. Thus each of the cells is a (n + l)-dimensional rectangular region, the first n coordinates of which define the point in C, and the (n + l)-dimension representing time. Let Ni,- • • ,Nn G Z+ denote the number of subdivisions of each of the n dimensions of C. We can then associate with each cell Vk an index, X{vk) which gives the position of the cell in the integer lattice: X{vk) = ( a ; t , - - - , x y , 4 G Z , l < 4 < i V , - . (2.39) Furthermore, we will denote each j-th interval of the subdivision of the i-dimension of C by the closed interval [0^,6^]. Thus for each cell Cell{v,) = [al b',] x[alb',]x... [a^ b^] x [tl,, 4 J , (2.40) where [^in^^ax] represents some closed time interval associated with the cell k. There are two drawbacks to using a uniform decomposition. First, since we only make use of cells whose interior is entirely contained inside the valid region, there may be large regions of TCT which are inaccessible. By further decomposing mixed cells into smaller cells one could obtain a better approximation to TCT. The second problem with using a uniform decomposition of TCT is that it leads to a number of cells that is 0(2"). To remedy these problems would require a more intelligent, hierarchical decomposition of the valid configuration space (see [Lat91] for examples), however this would also require a large number of modifications to the methods presented here, and is therefore outside the scope of this thesis. 64 2.4.2 Graph of Time-augmented Configuration Space Once decomposed we can represent the time-augmented configuration space as a graph G = {V,E), y ^^fwilvi i sa Validcell}, (2.41) E = {e,j} . where V is the set of vertices representing valid cells, and E is the set of edges which connect the vertices. The adjacency relationships contained within E are dependent both on the structure of !FCT and its decomposition, as well as the dynamic constraints on the robot. We will assume that an edge tij is in E if and only if there exists a corre-sponding valid trajectory between the two cells: ejj ^ E iff 3^ e T, and ti,tj,tk: € M"^ , ti < tk < tj, such that {e{U),U)eCell{vi), {9{tj),tj)eCell{vj), (2.42) {t e [ti,tk)) =^ {e{t),t) e Cellivi), and {tE [tk,tj])^{e{t),t)eCell{vj). and further, that this trajectory 9 satisfies all static and dynamic constraints de-sired by the designer which are not explicitly covered by the LC specification. Eq. 2.43 ensures that there exists a path which starts in Cell{vi), ends in CeU{vj), and does not pass through any other cell before passing to Vj. Given the graph of the time-augmented configuration space, Q = {V, E), we 65 can represent paths embedded in C by an ordered list, p, of vertices vi as P: P = {Pi,P2,---,Pk},Pi&V. The path P is valid, if and only if n-l ensuring that only valid edges are used. The path P is a valid initial path, if it is valid and (q,0)eCe/ /(pi) , which ensures that the path starts at time t = 0. The valid discrete path represents an equivalence class of trajectories, T{P), specifically: r(P) = I ^ e r Vt e 3ii, ^2, • • •, tfc e M, ti<t2--- <tk, so that ( A (Vt G {ti-i,t), {9{t),t) G Cellipi))) A (2.43) \yte[o,h){e{t),t) eCeilipiYj A (yt>h, {9{t),t)eCell{pk)\Y The times ti,- • • ,tk in Eq. 2.43 are transition times at which the trajectory passes from one cell to the next. 2.4.3 Utility of a Discrete Path Since a discrete path represents an equivalence class of trajectories we seek a mea-sure of utility which is conservative. Additionally it should be computationally inexpensive to compute, hence we would like to avoid a minimization over T{P). 66 First we will propose a measure of utility for a vertex v^ assuming that it is the terminus of a path. Definition 2.5 (Utility of a cell) Given a cell, Vk, a conservative estimate for configurations (q, t) € Cell{vk) is util(t'fc) = min supjti > t(j\{q,ti) G TCT}. {(l,to)eCell(vk) (2.44) D The utility of a cell Vk is depicted in Fig. 2.9. If the cell's boundary does not correspond to the boundary of !FCT, then computing uti\(vk) corresponds to finding the configuration q" which is "closest" to the TCT boundary. If part of the boundary of TCT corresponds to the cell's boundary, then the minimization of Eq. 2.44 may correspond to a configuration lying in the interior of the cell. util(ufc) A '/ Cell Vk tu) Boundary oiTCT Minimum utility point ^ = Figure 2.9: The utility of a vertex Vk-67 Given the utility of a vertex, we may define the utility of the path in terms of the utility of the last vertex. Definition 2.6 (Utility of a discrete path) Given a path P = {pi, • • • ,Pk}, Pi € V, we define the utility of P as: util(P) = util(pfc). (2.45) D The utility measure of the discrete path is conservative, in that util(P) < min util(^,J^CT). (2.46) Since the utility of a path is a function only of the last vertex, it is dependent only on the cell's boundaries and the structure of TCT. Computing Eq. 2.45 for each of the cells Vk allows us to know that utility of any possible path P. We can find the optimal utility path from a vertex v^ by looking at all vertices that it is connected to. In Eq. 2.36 we used F C J-CT to denote a restricted subset of TCT- In an analogous way, we will define the reachability set of a vertex, given a subset F<ZV. Definition 2.7 (Reachability set of vertex Vk) The reachability set of a vertex Vk is the subset set of vertices, which are reachable from Vk using only vertices in F , and edges in E^ R{vk,F) = {vj\3 avaMpathP = {pi,---,pm}, Pz € F } . (2.47) 68 F represents a restricted set of vertices, similar to F in Eq. 2.31. • For a robot with no additional fault constraints, the set of reachability set of a vertex Vk is R{vk,V). (2.48) 69 Chapter 3 Fault Tolerant Trajectory Planning Once the robot has been designed, and the task specified, the next step is to construct a trajectory which satisfies the task requirements. To the extent that the design elements of the robot and the task specification permit, we should also choose a trajectory which avoids the use of configurations which, if a fault were to occur, would leave the robot unable to complete the task. To clarify these aims, we will consider two types of path planning problems that must be solved when computing a globally fault tolerant trajectory. While the two path planning problems are similar in their goals, they diff'er in how they consider faults. Specifically they diff'er in the temporal nature of the faults considered. The first type, called reactive path planning, deals with faults that have just occurred or have occurred in the recent past. Paths constructed during reactive path planning are the recovery motions which are used to compensate for the fault 70 and thereby attempt to preempt a failure. Reactive path planning deals with the original task description with a relatively few number of additional constraints imposed by a fault. The second type, what we term contingency planning, deals with con-structing a navigational strategy, the goal of which is to minimize the effects of potential faults in the future, and thereby maximize the likelihood of completing the task. Contingency planning is much harder than reactive path planning since it must construct a strategy using incomplete, or uncertain knowledge. Addition-ally, contingency planning in the domain of fault tolerant path planning must, if it is to be effective, reason about the recovery motions for each fault. This means that at least approximate solutions to the reactive path planning are needed for contingency planning. These solutions can be computed offline. The construction of the fault tolerant trajectories has the following four aspects: Problem 1 LC specification -^ valid trajectory Problem 2 LC specification + fault constraint ^^ recovery motion Problem 3 LC specification + recovery motions -^ fault tolerance measures Problem 4 LC-specification + fault tolerance measures ' ^ fault tolerant path The first problem is that of producing a valid trajectory, and is described in [Pai91]. The second problem involves computing a recovery motion for a fault by expressing the fault as an additional constraint to the specification, and is an 71 example of reactive path planning. This can be computed on demand once the fault has been detected and identified, or may be computed ahead of time. The third problem concerns computing a measure of fault tolerance of a configuration given the recovery motions for an enumerated set of faults. Lastly, in problem 4, given the measures of fault tolerance we compute a fault tolerant trajectory. These two aspects comprise the contingency planning portion of our problem. There are two types of motion planning problems in the robotics literature: path planning, which deals with the construction of a collision-free path through the configuration space; and trajectory planning which constructs a trajectory 9{t) (see [Lat91] for survey). In general the trajectory planning problem is more diffi-cult since it must also satisfy the velocity constraints of the robot. Our goal is to produce a fault tolerant trajectory, so we will be solving a trajectory planning prob-lem. However, due to our representation of the problem using the time-augmented C = C X E"*" there is the potential for confusion. By decomposing C into disjoint regions, we will identify families of trajec-tories, 9{t), by the ordered list of regions of C that the trajectory passes. Using this representation the problem of constructing trajectories is reduced to a path-planning problem in a discrete graph. Before describing the two types of path planning, we will introduce some previous work on the construction of fault tolerant trajectories. 72 3.1 Background To the best knowledge of the author, the most closely related and relevant bodies of work dealing with the construction of fault tolerant trajectories for a robot are Lewis and Maciejewski's work [LM94b, LM94a] and Paredis and Khosla's work of [PK95]. There are two key properties, shared by these two bodies of work, which are in contrast to the methods employed in this thesis. First, both assume a kinematically redundant manipulator that is executing a task defined as an explicit path in the workspace of the robot such as x(t) E R^. From this the differential motion m = ^q, (3.1) is computed. Describing the task as in Eq. 3.1 has the advantage that, unlike the the kinematic function, /C(q), the Jacobian of the robot with the failed actuator is obtained easily from the original value of the Jacobian, J. For a frozen z-th actuator, the Jacobian for the failed manipulator is denoted by V, where V 9x 9x r\ dx dx dqi dqi-i dqi+i dqn obtained by zeroing the z-th column of J. (3.2) The second feature common to both [PK95] and [LM94b] is the use of a redundancy resolution algorithm to compute the joint angle trajectory q(t) for the fault recovery. A redundancy resolution algorithm resolves an under-determined system of joint angles or velocities defined by Eq. 2.1 or Eq. 3.1 to a particular solution q or q. This means that the fault recovery mechanism for both approaches is completely described by the redundancy resolution algorithm. 73 Both [PK95] and [LM94b] consider only redundancy resolution algorithms which can be described as selecting a joint velocity q using the free parameter z in q = J+±+{I-J-^J)z, (3.3) which we have already described in Section 2.1.1. For review of redundancy res-olution algorithms of the form of Eq. 3.3 please refer to [Nen89]. Typically z is chosen so as to optimize some objective function, such as maximizing the distance to joint angle limits [Lie97]. Since the choice for q is dependent only on the current state, q, we need not consider the previous states of the manipulator when computing a recovery motion. While [PK95] and [LM94b, LM94a] share the use of a redundancy resolution algorithm to construct recovery motions for a fault, they differ in the method for choosing the nominal trajectory q(t) which is followed when no fault is present. Lewis and Maciejewski define a local measure of the fault tolerance of a configu-ration when choosing the trajectory. We describe this measure in Section 3.1.1. Paredis and Khosla use methods that examine global properties of the kinematic mapping, which are described in Section 3.1.2. 3.1.1 Local Measures of Fault Tolerance The approach used in [LM94a] is to construct a local measure of the fault tolerance of a configuration. This measure is then maximized by using a redundancy reso-lution algorithm which performs a null-space maximization of this measure. That is, a direction z is chosen which maximizes this measure. The (/ — J"*" J) term in 74 Eq. 3.3 projects this direction into the null space of the manipulator Jacobian, and hence has no effect on the position of the end-effector in the workspace. The fault tolerance measure, kfm{q), is based on the amount of dexterity remaining after a fault has occurred. The measure is maximized when, despite a single actuator failure, the manipulator is still able to perform motions in arbitrary directions. Computing kfm{q) requires the singular value decomposition (SVD) of the Jacobian. Given the Jacobian J = E"*^", we can decompose the matrix in the standard way so that J - UEV, (3.4) where U E E"*^ "* and V e E"^" are orthogonal matrices, and S is a diagonal matrix whose elements, ai, are typically given in descending order [Str88]. The SVD decomposition has an intuitive physical interpretation: the singu-lar values, (Tj, give the size of possible motions for unit norm joint angle rates; the column vectors of V give the normalized of joint angle rates that give a motion of ai, in the direction Ui of the workspace [LM94a]. A number of kinematic dexterity measures have been proposed which are based on the singular values of the Jacobian. For example, the ratio of the largest to smallest singular values, also known as the condition number, has been used to develop isotropic redundant manipulators [KM91, Ang92]. A measure, called manipulability, the product of the singular values is proportional to the volume of the velocity ellipsoid [Yos85]. The smallest singular value of the Jacobian, denoted by km, is the dexterity measure used by [LM94a, LM94b], and gives the worst-case scaling of joint angle 75 velocities to end-effector velocities: km = am{J)- (3.5) This can be interpreted as the ease with which the manipulator can be moved in the least suitable direction. From the definition of dexterity of Eq. 3.5, the measure of kinematic fault tolerance, kfm, is constructed. Assuming that a fault in the i-th actuator results in the locking of the actuator at the position at which the fault occurred, the resulting Jacobian of the faulty manipulator is: kfm{(i) = mincr^lV , (3.6) the minimum over all faults, i, of the smallest singular value of the failed-Jacobian. There are several difficulties with the use of kfm. as a sole guide in the selection of a fault tolerant trajectory. Due to the local nature of the measure, it will likely not capture the subtle nuances of the kinematic mapping on a global scale, and therefore can not guarantee global fault tolerance. Nonetheless, there are applications where kfm is still of great use, specifically in cases where the desired trajectory x(i) is not known a priori. In these cases global fault tolerance is unachievable, and local kinematic fault tolerance measures are the only hope for selecting fault tolerant trajectories. Practical use of the approach would also necessitate the addition of joint angle and obstacle avoidance into the scheme, which are not considered. Another detractor of the method is that it requires the computation of the gradient of kfm.{q), forcing one to compute the complete SVD of V. The 76 computational complexity of SVD, is approximately [GL89], 4m^n + 8mn^ + 9r^^ (3.7) which is relatively expensive for an online algorithm. With the capabilities of current microprocessors, however, this is not a lirriiting property of the method, except in situations where one is forced to use antiquated hardware. Lastly, kfm does not consider information about the specific desired trajec-tory x(t), but rather it assumes that end-effector movements in all directions will be required after joint failure. Consider a situation in which, at some point along the trajectory, the remaining portion of the trajectory requires the end effector to move in directions comprising a small subspace of the original velocity space of the manipulator. In such, a case we do not want the measure of fault tolerance to disproportionately discredit a configuration's inability to perform motions not needed to complete the task. As an example, if x is confined to points in the xy-plane, then we would like to ignore the z-component of * J when computing kfm. Task-specific performance measures are an appropriate alternative, such as those found in [vdDP94]. Acknowledging the shortcomings of the local kinematic fault tolerance mea-sure, Lewis and Maciejewski proposed a global method [LM94b], which, along with Paredis and Khosla's work in [PK95] is described in Section 3.1.2. 3.1.2 Global Methods As discussed in Section 2.1.2, the global fault tolerance associated with a par-ticular configuration is related to properties of the self-motion manifold at that 77 point. Methods for characterizing the self-motion manifold can be found in [Bur89, LM94b]. Lewis and Maciejewski [LM94b] used a number of points in the workspace, called "critical points," and determined constraints on the fault tolerant exe-cution of the task in terms of the self-motion manifolds at each critical point. Since each of the critical points must be reachable when an actuator failure occurs, the self-motion manifold gives the range of joint angle values which can reach the critical point. For each critical point a "bounding box" of the preimage manifold is com-puted. An example of these bounding boxes corresponding to the 3-R planar manipulator, depicted in Fig. 2.1 (page 32), can be seen in Fig. 2.2. (page 33). Extremum points, such as x^ occurring at the reach singularity, have a family of joint angle solutions consisting of a single point, p^, so the preimage manifold, and hence the bounding box, is a single point occurring at the origin of Fig. 2.1. This indicates that a critical point of x^ is extremely fault intolerant, as a fault leaving any joint frozen at a non-zero position would leave the critical point unreachable. For this reason one should choose to tailor the robot/task so that critical points similar to x-*^ do not occur. A critical point such as x"^ on the other hand has a preimage manifold which spans almost the entire configuration space (the bounding box is omitted for p'^), and hence it minimally constrains the family of fault tolerant trajectories which accomplish the task. The point x^ is therefore extremely fault tolerant. The bounding boxes for the preimage manifolds for points x^ and x"* are 78 illustrated by dotted-lines in Fig. 2.2 (page 33). The preimage manifold of x^ consists of two disconnected regions, differing in the sense of the q2 joint angle. To construct a fault tolerant trajectory we must ensure that the robot stays inside the fault tolerant configuration space of the robot. The subspace of configu-rations which are fault tolerant with respect to the task was determined in [LM94b] by computing the bounding boxes for each of the critical-points, and then taking the set intersection of the bounding boxes. Global fault tolerance is achieved by using a redundancy resolution algorithm which ensures that the trajectory remains in the intersection of the bounding boxes. Enforcing that the trajectory remain in the intersection region by use of null-space optimization of Eq. 3.3 is identical to the problem addressed in [Lie97]. One problem with this approach is that the bounding-box constraints them-selves do not ensure that a path through the fault-tolerant workspace will be found by the redundancy resolution algorithm. In some instances the redundancy-resolution algorithm may get stuck at local minima. In other situations the topol-ogy of the configuration space may be such that a continuous path does not exist. As pointed out in [PK95], the bounding box information is not adequate for cap-turing global topological properties of connectedness. Therefore there may be instances where a path through the fault tolerant configuration space cannot be found. Another problem with the approach, mentioned in [LM94b], is the compu-tation expense of computing the envelope of the preimage manifold. The method is not feasible for higher-dimensional problems as this computation is too expen-sive. For one dimensional self-motion manifolds they make elegant use of a lin-79 early increasing spiral to estimate the bounds of a two dimensional surface in an n-dimensional space. Efficient methods for characterizing higher-dimensional preimage manifolds remains an open problem. Paredis and Khosla [PK95] propose a method which is similar to [LM94b], in that it also pre-computes the preimage manifolds to ensure global fault tolerance. Like [LM94b] they also make use of a redundancy resolution algorithm of the form of Eq. 3.3, and it is the responsibility of the redundancy resolution algorithm to execute the recovery motion upon discovery of a failed actuator. We devote the rest of Section 3.1.2 to the description of the algorithm presented in [PK95]. In keeping with their nomenclature, let 9{t) e T" represent a trajectory in the joint space, where T" is a n-dimensional torus (only revolute joints were considered). A path p{t) G M'" defines the task as an explicit path through the workspace of the robot. If a fault occurs, at a time denoted hy t*, a recovery motion is computed using the redundancy resolution algorithm, assumed to be of the form of Jacobian-based algorithms of Eq. 3.3. This alternative trajectory, dependent on the partic-ular joint effected, j , as well as the time of failure, t*, is written as e{t,j,f). A trajectory 9{t) is defined to be 1-fault tolerant with respect to the task p{t), if for every joint j e {!,• • •, n}, and at each instant t*, there exists a 9{t,j, t*) for which: 1. 9{t,j,t*) maps onto p{t) under /C. 80 2. e{t*) = 9{t*,j,t*). 3. 9j{tJ,t*) = 9j{t*),yt>t\ 4. 6{t,j,t*) does not violate any secondary task requirements . Item 1 ensures that the alternative trajectory 9{t,j,t*) is able to complete the task; items 2 and 3 ensure that the alternative trajectory is consistent with the actuator failure, and the final item ensures that any further task constraints which are not specified by p{t), such as joint angle or obstacle constraints, are also satisfied. If a posture (configuration), 9, satisfies the secondary requirements, we write 9 E S. Let r{p) be the preimage of the point p E W^ defined as^ r(p) = {9eT''\)C{9)=p}, (3.8) which will be a set of r—dimensional manifolds, an exception being when p is a critical value where it will not be a manifold but a bouquet of tori [PK95]. The value r gives the degree of redundancy of the manipulator. From the preimage, they define the tolerability of a posture as: Definition 3.1 (Posture Tolerant to a Failure of Joint j [PK95]) A posture 9 ET{p{t*)) is tolerant to a failure of DOF j ifi^the alternative trajectory 9{t,j, t*) as determined by the redundancy resolution algorithm, satisfies all of the task requirements. The set of postures which are tolerant of a failed j-th joint are ^The symbol S was used in [PK95]. We use F to avoid confusion with the diagonal matrix of SVD of Eq. 3.4. 81 given by the set ^i c r(p(r)). n We highlight a portion of Defn. 3.1 to draw attention to a subtle, yet impor-tant feature of how fault tolerance is defined. Specifically this is the dependence of the definition on the particular redundancy resolution algorithm. This is not an oversight on the part of the authors; they clearly state that the method takes as input a redundancy resolution algorithm. However, when one is confronted with a fault which is classified as fault intolerant, one can never be sure if it is an artifact of the underlying kinematics of the problem, or if it stems from the inability of the redundancy resolution algorithm to make use of the entire 1-fault tolerant config-uration space. For example for certain p(i*) the redundancy resolution algorithm may get stuck at a singularity, the alternative trajectory Q{t,j, t*) may violate joint angle or collision constraints, or 9{t,j,t*) may pass outside the workspace. This shows the potential tradeoff of using a redundancy resolution algo-rithm. On the one hand we get a concise and compact algorithmic description of the recovery motions for all potential faults, however, it comes at the price of potentially constraining the family of trajectories which can be used to complete the task. Given the set of postures J-^', we can find the set of acceptable postures, denoted A*\ by taking the set intersection over all joints j , A'' = fl-^f- (3-9) 82 Two properties are noted in [PK95] which allows them to compute the set of acceptable postures efficiently. Similar properties of recovery motions were in-dependently identified by the author and used in the algorithms presented later in this chapter. The properties are similar to those used when solving optimal control problems using dynamic programming techniques. Property 1: The acceptability of a posture d{t*) is dependent only on the future course of the trajectory p{t), and is independent of the history of the trajectory, p{t) for t<t*. Since at the last instant in time iiast, the path is completed, all faults are tolerable, and hence the acceptable postures is the entire preimage of the last task-point, piast-^*la. t = j r j a s t ^ . . . ^ j-^last ^ r{pi^,), ( 3 .10 ) This forms the initialization step of the algorithm. From ^*iast we work backwards in time to compute the fault tolerant path. Property 2: Given that the redundancy resolution algorithm determines the veloc-ity 9 based only on the value of j and p{t), two alternate trajectories, 9'{t,j,ti) and 9''{t,j,t2), is > ^2, if they intersect at a common point, will follow identical trajectories thereafter. This situation is depicted in Fig. 3.1. A corollary to property 2 is that a posture 9{ti) is fault tolerant with respect to a failed joint j if and only if the corresponding alternative trajectories are also 83 0 n Oi{t,j,ti) = t2 Figure 3.1: Two trajectories, 9^{t) and 9'^{t) for which the alternative trajectories 9i{t,j,ti) a.nd92{t,j,t2), arising from different faults, are coincident for times t > i2 (adapted from [PK95]). tolerant of a failed j-th joint: 9{h) e T'j' <^ 9{t\j,h) e JF**, Vr > ti, and ^(^i) G S, (3.11) where <S denotes that it satisfies all the secondary task requirements. Using the above two properties, an algorithm was developed for computing the sets of acceptable postures A*'' for discrete time steps tk- The next part of the algorithm is to take the sets ^** and form a smooth trajectory through the acceptable sets. Ideally they should avoid close proximity to the boundaries of the acceptable sets. To simplify the search of a trajectory 9{t) through the acceptable sets, each set A^'', is taken as the union of disjoint regions 7V''' with A^" = y nl' and nl' n nf, Vi ^^ j. (3.12) i Searching for a continuous trajectory 9{t) involves finding a sequence of regions 84 Ti^'i' which are connected by a continuous path. A connectivity graph is created which describes the regions 7^ -* and 'R!'-^ for which there is a trajectory connecting them. The structure of this graph is simple due to the small number of disjoint regions ??.•*' in each set A^*'. Topological information is required at three parts of the implementation: when computing the fault tolerant sets T^, when intersecting these sets to form the acceptable postures A!''', and during the determination of the disjoint regions 7^ **^ . Locally the preimage manifold is diffeomorphic to E'", allowing it to be ap-proximated by a r-dimensional hyper-plane. The preimage, r(p), is formed using a simplicial approximation with r-dimensional simplices, for example line segments when r = 1, triangular patches when r = 2, etc.. Let P denote the total number of path points pk = p{k/S.t)., and S represent the total number of postures 9 E T"' used in approximating T{p). Increasing S improves the accuracy, however 5 = 0(2'-). The total complexity of the algorithm can be expressed as 0 ( F 2 V ) . (3.13) The exponential growth due to r means that it is practical for only r = 1 or r = 2, however these degrees of redundancy are sufficient for a large number of problems. To illustrate the adequacy of the algorithm, a simulation of a 4 DOF robot executing a planar task of tracing out a circle was performed. By simulating the 85 introduction of various faults during the execution of the task they were able to demonstrate the fault tolerance of the chosen trajectory and redundancy resolution algorithm. 3.1.3 Planning Under Uncertainty In addition to the previous work related to fault tolerant path planning, there exists a body of related work which deals with the general problem of path planning under uncertainty. Computing a navigation strategy in a (partially) unknown environment is similar in nature to planning a trajectory in anticipation of faults, however the source of this uncertainty is quite different. Two sources of uncertainty may confront us when planning a path in an uncertain environment. First we may have incomplete knowledge of the terrain, requiring a method for gradually acquiring this knowledge over time, and building a map [KL88]. Secondly, there may be uncertainty resulting from unknown events, such as a fault, or quantities that are only known probabilistically, such as a bridge which may or may not be blocked from use. Of these two types of uncertainty, the second of these two problems is most applicable to the problem of computing a fault tolerant trajectory for a robot. Dean et al. used utility theory in their development of a navigation system [DBC"'"90]. Emphasis is placed on the coordination of task-achieving activities and map-building activities. Computing optimal plans for navigation in large and uncertain environments was investigated in [LG87]. The uncertainty in the environment was encoded using 86 uncertain grids in which each grid element is assigned 0 or 1 if the traversability is known, or a random variable if unknown. Regions are then labeled as "passable," "impassable," or "choke," the last meaning it is dependent on one or more random variables. Linden and Glickman propose a navigation algorithm which is similar to A* (see [GN87]). Papadimitriou and Yannakakis introduced a problem called the Canadian Traveler Problem (CTP), variants of which have relevance to many problems of contingency planning including navigation and network routing [PY89]. In CTP we are given a graph, such as that depicted in Fig. 3.2, representing an uncertain map, the edges of which are partitioned into two sets: a set of edges which are fixed, and a set of edges which may disappear probabilistically. Each edge is assigned a weight which is interpreted as the cost incurred by using the edge. The goal is to determine a contingency plan which achieves minimal total cost. The difficulty in computing a good contingency plan is that the knowledge of whether or not a probabilistic edge is present is known only when we are at the vertex that is incident to the edge. Bar-Noy and Schieber [BNS91] introduced a variant of CTP, the k-Canadian Travelers Problem, in which the number of blocked edges is bounded above by k. Using a recursive algorithm they were able to produce a travel plan that guar-antees the smallest worst-case travel cost. Another variant that they look at is the Stochastic Recoverable CTP in which each of the edges that become blocked will be reopened in a certain time. The similarity of the CTP and its variants to the computation of fault tolerant paths is apparent if we let nodes in the graph represent configurations 87 Vl Edge ei,2 ei,3 62,4 62,5 63,5 63,6 64,7 65,7 66,7 Type Fixed Uncertain Uncertain Fixed Fixed Fixed Fixed Fixed Uncertain Cost 3 9 2 8 3 6 5 4 1 Probability -0.7 0.8 -----0.5 Figure 3.2: The Canadian Traveler Problem distinguishes between edges which are fixed (always traversable), and those whose presence is known only probabilistically (uncertain edges). of the robot. Edges encode the topological information about the configuration space when no fault occurs. Constructing a fault tolerant path is similar to CTP in that we must reason about how potential faults will remove edges. Rather than minimizing the cost of the path, we try instead to minimize the use of "critical" vertices, which are susceptible to failure given removal of edges due to a fault. Runping Qi investigated the problem of path planning under uncertainty using decision graphs in his PhD thesis [Qi94]. The framework proposed involved using "U-graphs," or uncertain graphs, which are distance graphs in which edge weights are not a constant, but are a random variable. Solutions to minimal cost U-graph problems correspond to optimal navigational plans. For a good overview of the problem of navigation under uncertainty please see [Qi94]. One commonality of [PY89, BNS91, Qi94] is the treatment of the presence or absence of an edge as an independent random variable. This is in contrast to the planning of fault tolerant trajectories since a single fault may remove more than 88 one edge from the graph. Thus the process of determining the usability of an edge is not an independent event. The approach that we will use is similar to the CTP, except we include/exclude vertices of the graph rather than edges. By treating the fault as a constraint, we exclude all vertices that are not consistent with the fault constraint. 3.2 Reactive Path Planning So far we have not concerned ourselves with reactive path planning since it has been assumed to be part of the redundancy resolution algorithm. As mentioned in Section 3.1.2, the use of a redundancy resolution algorithm has the benefit of providing a compact, algorithmic representation of the recovery motions. However, the author believes that constraining potential recovery motions to those that are realizable from a particular redundancy resolution algorithm unnecessarily limits the family of recovery motions, and hence has the potential for limiting the degree of fault tolerance of the robot executing the task. Using an LC approach we can represent the introduction of a fault as an additional constraint, and can thus treat the problem of computing a recovery motion as a path planning problem in a smaller dimensional space. The appeal of this approach is that it allows us to use a similar method to produce the trajectory, q(t), as well as the recovery motions given a fault. 89 3.2.1 Representing Faults As described in Section 2.1.2, the introduction of a fault which immobilizes an ac-tuator can be thought of as determining a new robot, the reduced order derivative, which has one fewer actuated degrees of freedom. The new path planning problem can be solved in one of two ways. First we can consider the recovery motion as a path planning problem using the lower-dimensional configuration space CROD with the original task constraints. Secondly, we can compute the path using the original configuration space C, and adding additional constraints to the specification to ensure the robot remains in the reduced configuration space sub-manifold. From a theoretical point of view these two ways of phrasing the reactive path planning problem are equivalent, however there are a number of benefits to using the second approach. The most important benefit is that there are a number of types of faults which can be easily modeled as the inclusion of additional constraints to the specification that can not be easily captured using the former approach. For example, an unexpected collision of the robot by a heretofore unknown obstacle can be dealt with by adding an inequality constraint to keep the robot away from the new obstacle. This type of situation can not be easily expressed using the original task description and a reduced'configuration space. Secondly, modeling faults as additional constraints on the task provides an efficient means of computing the recovery motion for a large suite of faults. By considering a large number of fault scenarios at a given configuration, as well as the resulting recovery motions, we may accurately measure global fault tolerance of a configuration. Since the measures of satisfiability and utility are still applicable 90 to the additionally constrained task description, the same methods can be used to compute the nominal trajectory as well as any recovery motion. We assume that we are given a set of faults Q — {fi} enumerating all possible faults we wish to consider. In a similar way to the task specification, we associate with each fault fi a constraint function ai-.C^R, (3.14) and an associated predicate, iUi = {ai < 0), (3.15) which describes the fault constraint. The reduced feasible configuration space, denoted TCT\^, is defined as ^CT\^ = {qeJ'CTHq)}. (3.16) We say that a configuration q is feasible, given a fault uj iiq E TCT\^. Effectively TCT\^ determines the set of valid trajectories given a fault. The reduced feasibility set acts as a new specification for the generation of a valid recovery motion. By way of contrast, we could say that the fault is tolerable by the robot if there exists a path in CROD satisfying the task constraints, as described in [PAK94], or we could simply require that there exists a path in the reduced feasible configuration space J^CT\^. The advantage to the latter is that a much larger class of faults can be modeled, specifically those that do not result in a frozen actuator. The following example illustrates the modeling of a fault as an additional constraint and the resulting reduced feasibility set TCT\^. 91 Example 3.1 (Example of a failed actuator constraint for C C E^) Suppose we are given a task in which TCT corresponds to the interior of the conical region of C = K^ x E+, as illustrated in Fig. 3.3. A fault occurring at time t = 0, involving actuator "^2, while at position gfaii, gives rise to a failure constraint described by UJ •• q2 = 9fail- (3.17) 9 = ( q " . < 0 ) Figure 3.3: A conical feasible configuration space J-CT, and corresponding failure constraint involving actuator 92- The reduced feasible configuration space, TCT\^ is obtained by taking the intersection of the failure constraint surface and jFCT. Taking the intersection of TCT and the fault-constraint plane, we get a triangular-shaped reduced feasible configuration space of TCT\^, depicted on the right of Fig. 3.3. TCT\^ determines the feasible trajectories which can be used as recovery motions for the fault. Given a point q = (q°, to), denoting a configuration at some time after the fault occurred, we see that the recovery motion obtaining the largest utility would be one which ended at point q', which satisfies the specification until a time ti. • We constructed the example so that the fault occurred at time t = 0 to simplify the example. The full expression for a frozen-actuator fault constraint is 92 given by UJfa = (wo => {OJI A LO2)) , (3.18) where Q!o(q,t) = (ifaii - ^ ) , (3.19) Q!i(q,t) = ( g j - f e i i ) , and (3.20) a 2 ( q , 0 = (9faii-gj)- (3.21) which has three parameters, j , the actuator involved, Qfaii, the position of the joint at the time of failure, and ifaii, the time of failure. The predicate ujfa is formed from three predicates, CUQ which implements the "switching" effect, making the constraint active only after the time ifaii; and UJI,LO2 which implement the equality constraint via two inequality constraints. Recall that the implication expression "a =^ b can be re-written as "^a V b." The subset of C which is consistent with the frozen actuator constraint is {qeC\u{q)} = { ( g , t ) G C | ( g i = feii)V(t<tfaii)}. (3.22) 3.2.2 Recovery Motions for a Fault Using the reduced feasibility set TCT\^ as the parameter F , the definitions for satisfiability, utility, and reachability, which were presented in Chapter 2 are easily adapted for their application to robots with additional fault constraints. Definition 3.2 (Tolerating a fault) We say that a trajectory 6 successfully tolerates a fault, uj for a time tjnax iff sat{e,TCTi^,t^^^). (3.23) D 93 The utility of a trajectory 0 given a fault is ut i l (^,J-Cr |J . (3.24) Using the measure of utility we can define the optimal recovery motion as follows: Definition 3.3 (Optimum Recovery Motion for a Fault) Given a fault w, occurring at a time i/, the optimal recovery motion is defined to be OoTm{uj,tf) = arg max util (6',:FCTi ) . (3.25) {9eT\vtelo,tf], co{e{t))} ^ ' This is the maximum utility trajectory which is consistent with the fault constraint. The particular trajectory Q is not necessarily unique. • Like optimal utility trajectories, the optimal recovery motion for a fault is dependent only on the specification, and the topology of the reduced feasibil-ity set TCT\^. The topological properties are described by the set of reachable configurations R{q,TCTiJ, q = {q',to). (3.26) 3.2.3 R O D ' S in a Discrete Configuration Space When using a discretized configuration space to represent the valid trajectories of the specification (as described in Section 2.4), each fault will classify each of the vertices as consistent or inconsistent with the fault constraint. 94 In Section 2.4 we classified a cell as valid if its interior was entirely contained inside the valid region, that is Vk is valid ^ Cell{vk) C TCT. (3.27) Similarly we will classify a fault, u, as being consistent with a fault iff Cell{vk) C J^CT\^, (3.28) otherwise we will classify the cell as inconsistent. We will denote by ROD(a;) C V the set of vertices that are consistent with the fault uj. ROD{uj) = {vkeV\CeU{vk)CJ^Cri^]. (3.29) The set of vertices which are consistent with a fault, and reachable from a given vertex Vk is written as R{vk,ROD{uj)). (3.30) The properties of consistency and reachability as determined by the presence of a fault is depicted in Fig. 3.4 below. Only cells whose interiors completely lie inside the reduced feasibility region TCT\^ are considered consistent with respect to the fault a;, so Vi e ROD{UJ),VJ G ROD(w), and Vk ^ ROD(w). Using a discretized configuration space requires us to make corresponding changes to the fault constraints that are used. For faults in which the actuator is immobilized, the reduced feasible configuration space is a lower dimensional sub-manifold, so clearly we cannot require that all points in a given cell Cell{vk) be consistent with this type of fault constraint. Since a vertex Vk represents a family of trajectories over a given range of joint angle values, we would like to avoid having 95 Figure 3.4: Given a graph Q = {V, E) representing the feasible region of TCT, each fault partitions the set of vertices into those that are consistent with the fault constraints, denoted by ROD(a;), and those that are inconsistent. to know the particular configuration in Cell{vk) at which the fault occurred, and treat all faults of a given actuator while in a given cell the same. Consider an immobilized j - th joint occurring while in a configuration con-tained in cell{vk). Assuming a uniform rectangular decomposition of the feasible configuration space, the ranges of the joint angle qj will be given by the interval [a),h]\ Constraining the j - th joint to remain in this interval after some time tfaii is achieved by the following constraint predicate: u Vk,3 (it > tfai.) =» {{qj > 4 ) A (g,- < b';))) , (3.31) which is true at times t < tfan, and for all configurations for which qj lie in the interval of joint angles spanned by Cell{vk). This is illustrated in Fig. 3.5. 96 Inconsistent Regions Figure 3.5: The reduced feasible configuration, TCT]^-, space resulting from a frozen actuator using the discrete frozen actuator constraints of 3.31, projected into the Qji-plane. 3.2.4 Additional Obstacles as Faults To further illustrate the flexibility of the approach to modeling a broad range of faults, consider the case where, during the execution of a task, we discover that there is an additional obstacle in the workspace. If we know the geometry of the obstacle, or are able to approximate it conservatively by some bounding polygon, then we can treat the additional obstacle as a fault, and include the additional obstacle constraints at run-time to produce a trajectory which avoids the new obstacle. • For example, if the additional obstacle is approximated by a bounding tri-angle, as shown in Fig. 2.6, then we can use the three constraint functions /in, /121 and hzi from Eq. 2.28, Eq. 2.29, and Eq. 2.30 to form the fault constraint Qjobs = min(/iii,/i2i,^3i) 97 (3.32) Since the methods of [LM94a, LM94b, PK95] only deal with faults that result in a frozen actuator, they provide no method for dealing with unexpected interactions such as a new obstacle. Modeling external interactions as constraints allows the recovery-motion generation to deal with a broad range of unexpected interactions. This allows us to separate the fault tolerant planning into two parts: the nominal path to accomplish the goal, and various exception handling routines to deal with unexpected interactions. 3.2.5 Computing Optimal Recovery Motions We defined the optimal recovery motion in the continuous case in Eq. 3.25, how-ever finding the optimal recovery motion requires finding the particular trajectory which maximizes the utility. Solving Eq. 3.25 in the general case is at least as hard as robot motion planning, which using exact methods is NP-hard [Can88]. Therefore computing the constrained optimization as phrased in Eq. 3.25 is too computationally expensive to be used to generate the recovery motion at the time of a fault. Instead we will use the conservative utility estimates for vertices in the discrete graph of cells, and solve for the optimal path through the restricted set of vertices ROD(a;). We will define the optimal recovery motion as the shortest path whose endpoint has the largest utility. Definition 3.4 (Optimal Discrete Recovery Motion) Assume a situation in which during the execution of a task, the robot 98 is in some configuration q-^ at time t{^i\, when a fault occurs, with (q- ,^tfaii) € Cell{vk),Vk e ROD(a;). Let Vj be the largest utility vertex reachable by Vk, Vj = arg max util(?;i). (3.33) vieR{vk,ROB{w)) We define the optimal recovery motion as the shortest valid path P={Vk,P2,---,P7n,Vj}. (3.34) Let Prm{fJki<^) denote this shortest-length maximum-utility recovery motion. In the event that Vj is not unique in Eq. 3.33, choose Vj so as to minimize the length ofp inEq. 3.34. D To compute the recovery motion as defined above, we use a breadth-first search algorithm to find the shortest path. If the degree of each vertex is bounded above by 5, and we let NF = I R O D (O;) I denote the number of vertices in the ROD, then the computational complexity of computing the recovery motion for the vertex v^ is 0{SNF). (3.35) The breadth-first search algorithm for computing the recovery motion is given in Appendix B.L The recovery motions are stored using the array 7r[i], 99 which gives the vertex adjacent to Vi which is used in the recovery motion. Thus the recovery path for vertex Vi is given by: {ui, v^ii], v^[^[{i], •••,Vk}, where 7t[k] = 0. The end of the recovery motion is denoted by 7r[k] — 0. 3.2.6 Computing Recovery Motions for Multiple Source Vertices So far we have only considered the case of planning a recovery motion for a single fault. We will now describe an algorithm that, given a reduced feasibility set F, will compute the recovery motions for all vertices in F simultaneously. This has the advantage that, in general, neighboring vertices will have similar recovery motions. Treating the recovery motions as an optimal control problem, we can use a dynamic programming approach, and save computation by using the recovery motions for neighboring vertices as partial solutions to other recovery motions (see [BDG71] for an overview of the use of dynamic programming techniques for the purposes of optimal control). There are two factors which motivate the simultaneous computation of re-covery motions for multiple sources: • If sufficient resources are available we would like to pre-compute the set of recovery motions for a set of faults, thus enabling their immediate use in the event of a fault. • Computing the recovery motions for a set of possible faults can be used as 100 a measure of risk of utilizing a given vertex in the nominal trajectory. We will describe a measure of risk which uses the results of the recovery motions over a set of faults in Section 3.3.2. An important feature of the recovery motions as defined by Eq. 3.34 is that, for faults that immobilize an actuator, as described by the constraint function Eq. 3.18, the set of reachable vertices given a fault, R{vk,ROD{uj)), is not de-pendent on the time at which the fault occurred. In other words, as long as the vertex Vk is consistent with the joint-immobilization constraint, once we are at the vertex Vk, the set of vertices that we can now reach is not dependent on when the joint-angle constraint became active. In this sense the recovery motion generation is stateless, and need only consider the present configuration Vk-Therefore, for the purposes of computing the set of reachable vertices from a given vertex, we can use the simplified constraint predicate LU «(-'=) = (^a'^ - q. < o) A [qj - b'^ < O) . (3.36) In addition, using a regular decomposition of the configuration space allows us to compute F^^""^ = ROD(a;^ ('^ '=)) (3.37) for a relatively small number of intervals. The algorithm for computing the recovery motions for all vertices Vk consis-tent with the fault constraint is given in Appendix B.2, and is an example of an edge-relaxation algorithm similar to Dijkstra's edge-relaxation algorithm for deter-101 mining the shortest paths [CLR90]. The difference lies in that our partial ordering of paths considers both the utility and length of the path. The computational complexity of computing the recovery motions for all vertices in ROD (a;) simultaneously is 0{\E\ + NF\og^Np), (3.38) which is the same as "Modified Dijkstra's Algorithm" for the set of shortest paths to a single destination [CLR90]. To illustrate how 7r[A;] stores the optimum utility recovery motions, consider the example given in-Fig. 3.6. Vl V2 V3 ^5_ V4 V6 i- 4-Vg Vertex Vi • Vl V2 vz V4 V5 V6 vr Vs V9 util(vi) 0.9 0.4 0.2 0.6 0.1 0.3 0.8 0.5 0.7 7r[i] 0 1 2 1 4 5 4 5 8 Figure 3.6: Given a set of vertices F = {vi,- • •, VQ} consistent with some fault to, the edges of the recovery motions are shown by the large arrows. 3.3 Contingency Planning The contingency planning problem that we will look at is the task of selecting a nominal trajectory which satisfies the task constraints, such that when a fault occurs, a recovery motion is likely to exist which will allow the completion of the 102 task. This is an example of contingency planning since we do not know ahead of time where or when a fault may occur, rather we must construct a trajectory which avoids configurations which are overly susceptible to faults. In contrast to the CTP problem in which edges have a cost which is to be minimized, we will associate with each vertex a performance measure which quantifies the fault tolerance of the vertex. The performance measure is to be maximized along the trajectory. The performance measure, described in Section 3.3.1, is analogous to the kinematic fault tolerance measure kfmQ of [LM94a, LM94b], and is at a maximum when the configuration has sufficient residual ability after the fault to complete the task. The performance measure integrates information from all possible failure modes at the vertex, to give a single scalar value given by perf(?;i). We can therefore break the path planning problem into two parts: the com-putation of the performance measures for varying configurations Vi and fault sce-narios (J, and next determining the path which maximizes the performance measure along the path. In computing the discrete paths we will assume there is some maximum time m^ax which, by judicious choice of LC constraint functions, the robot is guaranteed to complete the task. For tasks which are specified to be performed ad infinitum, some means of computing the trajectories by repeated concatenation of finite time interval trajectories is needed. In such a case tmax can be considered a planning horizon. We will only concern ourselves with finite tasks for the present. 103 The set of "source" vertices, denoted Vgrc is s^rc = {vieV\3{q,0)eCell{vi)}. (3.39) Similarly the set of "destination" vertices, which is determined by tmax is Kist = {Wilutil(Wi) > t „ , a x } . (3.40) Since util(wj) is a conservative estimate, we seek a path -Pgoai = {Pi,---,Pm}, with piEVsrc, and Pm ^Vdst, (3.41) which is a valid path, and whose performance measure perf() is maximized along the path. We will let V = {Pgoal} (3.42) be the set of all discrete paths Pgoai which complete the task as defined above. Choosing the particular path from V which is most fault tolerant is done by ranking paths using the Sorted-Minimum path ranking. This ranking scheme considers the fault tolerance measure computed at each vertex along the path. The highest-ranked path is taken as the most fault tolerant path to the goal. What follows in the remainder of this chapter is a description of the fault tolerance measure and the sorted-minimum path ranking, which, when combined, identifies the best fault tolerant trajectory to the goal. We will show that the trajectory produced is optimal with respect to the worst-case failure mode of the robot executing the task. 104 3.3.1 A Global Fault Tolerance Measure A measure of fault tolerance, if it is to be meaningful, should characterize the ability of a robot, using its remaining functional capacity after a fault, to complete the task. The performance measure of a configuration evaluates this ability over all applicable failure modes of the robot, for example, by considering immobilizing each of the n actuators, and combines them by taking the worst-case or average-case behaviors, to produce a single performance metric. There are two properties we would like in a ideal fault tolerance performance measure: PI Global The measure should reflect global topological properties of the configuration space, and how altering the topological properties via a fault affects the ability to continue to satisfy the task requirements. P2 Ease of Interpretation In addition to quantifying the fault tolerance at a configuration, the value of the measure should be in units which are relevant to the task. If the fault tolerance measure has a natural interpretation there is the possibility of using it to guide the designer when constructing the task. In contrast the kinematic fault tolerance measure of kfm{) is a local mea-sure, and therefore does not reflect the task as a whole. The measure kfm{) gives the smallest singular value of the failed-Jacobian, which gives the worst scaling of joint angle velocities to end effector velocities. This relates to the dexterity of the 105 configuration, but the value has no interpretation with respect to the task and its completion. In [PK95], sets of postures, J\-*, that were fault tolerant with respect to a joint j were computed. These sets of postures did reflect the global nature of the configuration space and the task. The diflflculty with this approach is that it restricted the types of recovery motions considered; a posture was fault tolerant if the redundancy resolution algorithm could find a sufficient recovery motion. Also, the methods classify a configuration as fault tolerant or fault intolerant, and therefore does not give any additional information as to how close the configuration is to being able to sustain a fault and complete the goal. 3.3.2 Longevity: A Global Measure of Fault Tolerance The appeal of local performance measures such as kfm{) is they require limited computational resources for their computation; typically they require only knowl-edge about differential behavior of the robot, such as the failed-Jacobian V for example. However, given that we have a utility measure util() which ranks a tra-jectory's ability to satisfy the task requirements, and given an efficient algorithm for computing the recovery motions of a large set of configurations/faults (described in Appendix B.2), a fault tolerance performance measure which considers both the effects of, and recovery motions for, a fault scenario is possible. The fault tolerance measure, called longevity, ranks the configuration, Vk, as to its ability to tolerate a fault, to. Definition 3.5. (Longevity Fault Tolerance Measure) 106 Given a fault, described by the predicate to, and a configuration and time (qo, to) G Cell{vk)i the longevity of the vertex Vk is defined to be the utility of the optimal recovery motion for the fault computed at Vk\ L{vk,uj) =< . (3.43) -oo Vk ^ ROD(a;) We assign the value of — oo to any configuration Vk which is not consistent with the fault constraint. Let ft denote the set of all possible faults we wish to consider, and fl{vk) denote all faults consistent with the vertex v^: 0 = {oui}, Q{vk) = {co^ e ^\vk e ROD{uJ^)}. (3.44) The worst-case longevity is defined as Lmin = min L{vk,uj), (3.45) and gives the utility of the optimal recovery motion for the worst-case failure mode of the robot while in a configuration Vk- , • The units of the longevity fault tolerance measure the same as util(), namely time. This reflects the philosophy of LC: we should not care about the particular configurations chosen along a trajectory, but only that the constraints are satisfied over time. Since we only care how long the constraints can be satisfied, like utility, the fault tolerance measure gives the length of time that the constraints can be satisfied given a fault. Defining the fault tolerance measure in this way ensures that we do not impose any further semantic constraints on how we interpret an LC program. 107 If we limit the faults to immobilized actuator faults, then Q{vk) = K'='^|; = l , - - - ,n} (3.46) where LO'"'"^ corresponds to the constraint given by the function a'"'"^ of Eq. 3.31. The benefit of the longevity measure is that it allows for a very natural interpretation. If •^minV^fcj ^ ' 'max) then configurations in Cell{vk) are fault tolerant with respect to all faults fl. Vertices Vk for which are able to tolerate a fault involving the jf-th actuator. If then the vertex is not fault tolerant, but does guarantee to satisfy the task con-straints until at least time ty^,. If we think of an adversary who is able to introduce a single fault during the execution of a task, and whose goal is to minimize the utility of our trajectory, then L{vk) gives a lower bound on the utility that the adversary is able to attain. Since it is reasonable to assume that any fault process that is likely to encounter will not have knowledge of our trajectory ahead of time, L{vk) is clearly a conservative means of selecting a configuration. The longevity performance gives an indication of the safety associated with a configuration, Vk, however it does not reflect the main objective of completing 108 the task. For example, a vertex Vk with util(tifc) < L{vk) = t„ is 1-fault tolerant, however it is very far from reaching the goal. We will define the performance measure, perf(ujt), so as to combine the two objectives of maintaining safety and accomplishing the goal: perf('yfc) = < L{vk) if L(vfc) < ^max + Util(Ufc) if L{Vk) > (3.47) For vertices Vk G Vdst which correspond to attaining the goal, perf(ufc) > 2imax- The interpretation of the performance perf(ufc) is given in Fig 3.7. perf(vfe) 2U Task completion 1-fault tolerant "Longevity": L{vk) 1-fault intolerant Figure 3.7: The relationship between the performance measure perf(ufc) and the fault tolerance measure, longevity, L{vk). Consider a path p = {pi,P2-iP3,P4}-Vertices with perf('yfc) < tmax) corresponding to pi and p2j are 1-fault intolerant. Vertices with tmin < perf(wfc) < 2tmax, such as p^ are 1-fault tolerant. Vertices with perf(ufc) > 2tma.xi such as p4, correspond to completion of the task. 109 3.3.3 Computing Longevity If the number of faults considered at each vertex is a coristant NQ = \Q,{vk)\, then we can represent the set of recovery motions as a two dimensional array of edges Trim, j = l,2,---,Nn, which gives the edge of the recovery motion from Vk for the fault Wj E Q{vk). If we consider only joint immobilization faults, then we can compute the set of recovery motions for the fault Uj for all vertices Vk simultaneously using the algorithm described in Appendix B.2. Computing the value of L{vk,u}j) involves traversing the linked-list stored at 7r[j][A;], and returning the utility of terminal vertex: J utilK) if 7rb][A;]^0 L{vk,ujj) = < . (3.48) L \VTr[j][k],ujj] otherwise Once the performance measures perf(^ ;fc) have been computed for each ver-tex, what remains is the construction of a path which maximizes this measure. This is done by finding the path which is ranked highest according to the Sorted-Minimum Path ranking, defined next. 3.3.4 The Sorted-Minimum Path Ranking There are several objective functions available to rank a given path, each producing trajectories with differing characteristics. We could use a conservative objective function m minperffwj), 110 however, since it considers the performance at only one point where it is at a minimum, it can not distinguish between two paths which share a common min-imum performance value. For example, if the vertex at the initial point of the trajectory happens to have the smallest performance value, then all paths from the initial vertex will be ranked the same. Alternatively we could take the mean of the performance along the path •1 m — ^perf(ui) , however this has the disadvantage of promoting paths which are circuitous, since the mean can be increased by loitering in areas of high performance, perhaps not even attaining the goal. In many instances, due to the topology of the valid configuration space, we may be forced to use vertices for which the performance is very poor. The use of these regions, if inevitable, should not unduly influence the ranking of a path. A compromise is the Sorted-Minimum ranking, which was suggested by Pai and Reissell [PR95] as a metric for choosing a path over rough terrain. The sorted-minimum path metric takes the performance values at each vertex in the path, and sorts the values. Two paths are ranked by taking the sorted performances and comparing them lexicographic manner as follows: Definition 3.6 (Sorted-Minimum Path Metric) Given two valid paths P= {Pi,---,Pm}, Pi^V, p' = {p'i,---,p'j}, Pi^V, let z and z' be the sequence of perf(pj) and perf(p^), sorted in increasing order so 111 that z = {zj}, Zi = perf(pj), ^' = {^a, ^: = perf(p;), (3.49) -2^1 < -2^2 < • • • < ^m, a n d 2i < 22 — • • • — -2^ j- (3 .50 ) The partial ordering of paths, written as p > p', indicating that the path p is preferred over p', is computed by comparing the sequences of sorted performance values in a lexicographic manner. We write p > p' if A - i \ 3j < min(m,n), / \ Zj = z• A Zj > z'y (3.51) Additionally, if either z or z' are prefixes of the other, then the shorter path is preferred. Two paths are equivalent, p = p' iff (m = n) A (Vi = 1, • • •, n, Zi = z[). The optimally fault tolerant path, not necessarily unique, is the path Pft with Vy e V, {p' ^ Pft) ^ (pft > p'] . (3.52) 3.3.5 Interpretation of Sorted-Minimum Performance Met-ric When a fault tolerant path is not possible, for example when all possible trajecto-ries are forced to pass through a "critical" point which is inherently fault intolerant, 112 the path generation scheme should still produce paths which maximize the achiev-able fault tolerance. This is achieved with the Sorted-Minimum ranking since any critical portion of the task, corresponding to the unavoidable risk, will be repre-sented as common entries of the sequence of sorted performance values. Thus the path pft will correspond to the shortest fault tolerant path, if one exists. However, if a fault tolerant path does not exist, pft will maximize the realizable fault toler-ance along the path. The fact that pft is well defined and meaningful for tasks in which there does not exist a 1-fault tolerant path is a desirable characteristic of the path generation mechanism. In cases where a 1-fault tolerant path does not exist, we can interpret pjt as the "closest approximation." In contrast, postures in [PK95] are described by the binary property of inclusion in the set of fault tolerant postures, J^j*. Paths are generated by finding a connected path through the fault tolerant regions. If such a path does not exist there exists no method for generating a path which is "most fault tolerant." This inability to deal with critical configurations is also exhibited by paths generated using null-space optimization of kfrnQ, as found in [LM94a]. Assuming any path to the goal must involve the critical region, null-space optimization of kfm{) may fail to find a path through the critical region. 3.3.6 Computing the Fault Tolerant Path Given the partial ordering of paths, >, the algorithm for computing the opti-mal sorted-minimum performance path is easily described as an edge-relaxation algorithm. The algorithm is similar to the "Modified Dijkstra's algorithm" for computing minimum-cost paths from a single source [CLR90, p. 575-531]. 113 At each point in the algorithm each vertex stores a currently-best-known path to a destination vertex. Using a priority-queue, we consider paths in decreas-ing >-order, and check to see if the path constructed by adding the edge e^j to the beginning of the path would improve the current best path from Vi. The qualifier "modified" in "Modified Dijkstra's Algorithm" refers to the use of a heap in implementing the priority-queue. For sparse graphs the complexity of the Modified Dijkstra's Algorithm is 0{{V + E)\og^E), an improvement from O(V^) of the original Dijkstra's Algorithm [CLR90]. Before we describe the algorithm we will look at implementation of the Sorted-Minimum path comparison, >. Efficient implementation of > is crucial since it is the most computationally expensive operation of computing the paths. Like the algorithms for computing the recovery motions, we will store the paths using an array denoted by Tv[i]. In the worst-case, computing the boolean predicate p > p' for two paths P= {Vi, Pnli], Pn[7r\i]], • • • , P m } , a n d p' = {Vj, p^[j], P,r[7r[j]], ' ' ' , Pk}, involves traversing the linked-list of vertices for both paths, sorting the arrays of performance measures, and incrementally examining the sorted arrays until a unique performance value is found. If we let 7 denote the maximum path length, then the cost of this comparison is C(7log2 7), since it involves the sorting of 7 real numbers. We can greatly improve the efficiency 114 of the path-comparison operator by storing the minimum performance measure of each path in a separate array. If the minimum performance value of two paths is unique, then we can determine p > p' in constant time. Otherwise we must perform the expensive traversal, sorting and comparison operation. The algorithm for computing > is given in Appendix C.l. The algorithm for computing the Sorted-Minimum performance paths, which makes use of the path comparison operator > described above, is given in Ap-pendix C.2. The proof of correctness is very similar to that of Dijkstra's algorithm [CLR90], and can be found in Appendix C.3. 3.3.7 Complexity Analysis of the Sorted-Minimum Path Algorithm In computing the overall time complexity of the algorithm, we must first consider the total number of calls to the path comparison operator >. Multiplying this complexity by the number of real-valued comparisons of perf(wi) gives the total complexity of the algorithm. In the analysis we will use the following quantities: Symbol N, = \V\ 7 Meaning The number of vertices in V. An upper-bound on the length of any Sorted-Minimum performance path to a vertex in Vdst-Table 3.1: Symbols used in the complexity analysis and their meaning. Given that the maximum size of the priority queue is Ny, each addition, 115 removal, and heap re-ordering operation (denoted by AddToPriorityQueue((5,fj), RemoveMaxPCl(Q), and ReHeapif y(Q, j)) will take at most 0(log2 Ny) >-operations. Each call to the procedure Rela.x{i,j) of Line 6 will have one call to > and one call to one of AddToPriorityQueue((5,t'j,7r[]) or ReHeapify((3, j ) , for a total of calls to >. Since the While-loop of line 2 is repeated a total of Ny times, the total number of calls to > is C>(iV„(l + log2iV„)). (3.53) Given a regular-decomposition, in which Ny = d"-^ this reduces to C>(d"(l + nlog2d)) = 0 (nd" log2d). (3.54) Determining the computational complexity of the path-comparison operator >, as described in Appendix C.l, is difficult since it depends on the distribution of the performance measure perf(t;j), as well as the structure of the graph. In cases where the minimum performance measures for both paths are unique, the comparison can be performed in 0(1) time. If two paths have the same minimum performance measure, then the number of comparisons is determined by the sorting of the two lists of performance measures, and is 0(7log2 7), corresponding to the worst-case behavior of the algorithm. The upper-bound on the maximum path length, 7, is dependent on the topological properties of the configuration space, the particular decomposition of the feasible space J-CT-, the dimensionality of the configuration space, n, as well 116 as the task specification. Using a regular decomposition, it is reasonable to assume that 7 = 0{nd) (3.55) indicating that it grows linearly with both the dimensionality of configuration space, as well as the number of sub-intervals each joint angle of the configuration space is divided into. This is the same as saying that the path lengths are at worst a constant factor more than the Manhattan distance between the two vertices which are farthest apart. The worst-case time complexity of Algm. C.l is therefore given as O {nd\og2{nd)). (3.56) The total time complexity of Algm. C.2 is therefore 0(n2rf"+Mog2(d)log2(dn)) > 0{dn^N^). (3.57) This indicates that the computational complexity is at least quadratic in n, the dimension of the configuration space, and linear in the total number of vertices N^. However, Ny = O {d^), and is therefore exponential in n, the number of actuated degrees of freedom. This motivates the use of a more intelligent decomposition of the configuration space, such as a hierarchical scheme, to avoid this exponential growth. 117 Chapter 4 Reactive Elements This chapter considers the reactive elements of executing a robot task in a fault tolerant manner. These include the real-time monitoring of the sensors and actua-tors to detect when an erroneous event has occurred, as well as the identification of the fault that was likely the cause of the error. Much of the research involving fault tolerance in robots has concerned the problem of path planning, and have assumed the prior existence of a fault detection and identification (FDI) subsystem which performs these functions [RP97, RP99, LM94b, LM94a, PK95]. We will give a brief overview of previous work in the area of fault-detection and identification, and introduce a novel method for diagnosing collision faults where a manipulator comes in contact with an unknown obstacle. While a collision of the robot with an obstacle may appear similar to an actuator failure, in so far as the actuator abruptly stops, the constraints that an additional obstacle places on the completion of the task are often much less 118 limiting than an actuator failure. Determining the geometry of the obstacle allows us to include the obstacle via additional constraints on the task. The difficulty in recovering the geometry of the obstacle is that we have no way of directly sensing the object other than the robot sensor histories since, presumably if we had sophisticated sensors for the detection of the obstacle, such as a vision system, then the collision could have been avoided in the first place. We will introduce a method which, given a model of the dynamics of the manipulator and the histories of the sensors, the collision geometry of the obstacle can be recovered. 4.1 Previous Work in FDI Error detection schemes can be divided into two types: those that use structural redundancy, and those that use analytical redundancy. Structural redundancy makes use of redundant sensors in which each sensor reports its reading, and an arbitration takes place to form a consensus. Sensors which are outliers from this consensus reading indicate a potential fault in the sensor. An example of structural redundancy, often employed in control systems, is the use of Triple Modular Redundancy [HSL78, Wen78], in which a set of three sensors vote to produce a consensus. Fault detection in this scheme is simple: any sensor which does not agree with the majority is likely a faulty sensor. Control systems may exploit structural redundancy to reduce the effects of noise on the system [Ste91]. In general, the use of replicated hardware will be bounded by cost and weight. For this reason we will focus on analytical redundancy techniques. Analytical redundancy, a more complicated method of detecting a fault, 119 u(t) (system inputs) Physical. System System Model X(t) (system state) Residual Generation (predicted state) \^ Residual (s) Figure 4.1: A set of residuals of a system. The state of the system, x{t), evolves over time due to the current state, and the inputs u{t). With the system model, a predicted state x{t) is constructed, which is combined with x{t) to produce a residual whose magnitude indicates the degree of departure of the system from its expected state. relies on a system model to produce an independent estimate on the system state. This independent estimate can then be used to validate the proper operation of the sensors and actuators. Common to many error detection schemes is the use of a residual, which indicates the degree of departure of the system state from that estimated using the system model. This is depicted in Fig. 4.1. Using the residuals to detect that an anomaly has occurred, we must now identify the fault which is responsible for the observed system behavior. The pro-cess of fault identification is similar to the task of diagnosis in AI [FL87]. Provided we can characterize a set of faults in which we are interested, we may use the model of the system, as well as observations of its behavior to diagnose the fault. Many errors have associated parameters which, provided they can be re-120 covered, may aid in the error recovery. An specific example of this, discussed in Section 4.3, is the determination of new obstacle's location from the collision event. For this we need to extract features from the residuals, and estimate the state of the system as well as the environment. This has been explored by many researchers, and is well summarized in [Ise93]. 4.2 Analytical Redundancy: Parity Space Meth-ods Due to the increased weight and cost of replicating sensors, much of the work in FDI in robotics has focused on analytical redundancy techniques [LR91, Cla78, VWC94]. Of particular interest is the use of parity space methods [CW84]. Chow and Willski [CW84] have developed a methodology for fault detection in discrete linear systems that is based on the parity space of the system. Given a discrete time linear system with inputs u and outputs y, X{t + 1) = AX{t) + Bu (4.1) y{t) = CX{t), where X G K^ is the state vector, u G M^ a vector of inputs, y{t) G M^, and A G R^""^, B G R^""^, and C G R^""^ are constants which depend on the linearization of the system. Chow and Willsky [CW84] define P = {w|u^Z = 0 } , (4.2) 121 where Z = C CA CA' (4.3) as the order-s parity space of the system; Z is the s-step observable subspace. If we let ^ = {(j)i} he a, set of linearly independent parity vectors which span this space (not necessarily unique), each 0j gives an linear combination of observations y{k—i) which correspond to a unique fault direction. Since the input u(A;) is non-zero, we must compensate for the applied input as in [CW84] to give the parity-vectors as: p[k) — $ < y{k - s) yik) ,-H u{k — s) u{k) 0 (4.4) CB 0 H = CAB CB 0 (4.5) CA'-^B • CAB CB 0 Parity techniques have a number of nice qualities. The number of tests is optimal in the sense that each vector corresponds to a unique fault direction. It is possible, in theory, to construct $ so as to have distinct columns. In this case each parity vector will correspond to a unique fault hypothesis. Also, since parity methods are able to exploit both direct and temporal redundancy of the system, they can be applied to the detection of actuator and sensor failures. 122 The main problem with FDI techniques using analytical redundancy is that they suffer from the practical limitation that the system model on which the model is based is never known exactly [Fra90], hence the actual outputs will never match the modeled outputs, and therefore the residuals will always be non-zero. To ensure that there is not a constant false-alarm due to the non-zero residual, the residuals are compared against a threshold which must be tuned. This may significantly reduce the sensitivity of the error detection. 4.3 Detecting and Localizing a Manipulator Col-lision This section deals with the detection and localization collision event involving a robot manipulator and an un-modeled obstacle. The detection scheme combines information about the observed disturbance torques to detect collisions and to infer the position of the collision in the environment. This work was first presented in [RP95]. 4.3.1 Motivation Typical robotic tasks often require some collision free-motions, and there has been a considerable work in methods for collision avoidance [Bro83, Can93, LatQl]. While we may construct a path which successfully avoids collisions with known obstacles, the problem of unexpected collisions always exists as long as there is uncertainty in our sensing, control, or our modeling of the environment. This is particularly true 123 for mobile robotics where the errors in position may increase as the robot moves in the environment [Mal91]. When a collision event occurs, the error recovery mechanism must first per-form the necessary emergency actions, such as the application of the braking sys-tem, to limit the damage to the robot and the obstacle. Next a new trajectory must be constructed which satisfies the obstacle constraints imposed by the new obsta-cle. To facilitate this, some knowledge of the obstacle's position in the workspace is crucial. We propose a method for collision identification and localization using ob-served disturbance torques at the joints. The disturbance torques provide a great deal of information about the interaction of the manipulator with the environment, with little or no additional sensing. 4.3.2 Introduction The method we propose models interactions between surfaces of the manipulator and points in the environment as a set oi features which are configuration indepen-dent. These features have associated parameters which provide a basis for the set of all generalized forces generated by the given feature. Combining these features, with knowledge of the disturbance torques and the manipulator configuration yields a system which is sufficient for identification and localization of collisions of the manipulator with the environment. Localization of the collision involves solving for feature parameters which "best" fit the observed disturbance torques. We demonstrate how additional constraints on the system yield a over-constrained system, and argue that the least-squares solution provides a means 124 of determining feature parameters which is robust with respect to noise. We also give a measure based on the least-square projection which provides a useful measure for comparing the merits of competing collision hypotheses. We will assume below that the disturbance torque r^ can be estimated with some uncertainty. This could be done by joint torque measurements if we have a model of the actuator dynamics, or measuring measuring joint states and using a disturbance observer [TM089]. In general we are given a n-link manipulator whose equations of motion are described by: Td = M{e)9 + V{9,e) + G{9)-Ti^p,^ (4.6) where M is the mass matrix, V denotes velocity-dependent terms such as the centrifugal and coriolis terms and viscous friction, G denotes position-dependent terms {e.g. gravity), Tjnput represents the input to the system, and TJ. represents a disturbance torque. Given measurements or estimations of 9, 9, and 9 we may observe the disturbance torque r^. There have been advances in path planning which deal with uncertain con-trol and sensing [LMT87], as well as path planning which is guaranteed to succeed or noticeably fail [Don87]. Much less attention has been given to the task of collision detection and localization as a source of information for recovery from un-expected errors. We propose a means by which we may combine knowledge of the sensor and actuator histories, with a model of the dynamics to infer the geometry of contact with the obstacle. The use of contact information is prevalent in grasping (e.g., [Sal83]), mobile robotics (e.g., [Mal91]), and industrial robotics (e.g., [TM089]). [Sal83] uses force 125 information from strain gauges to infer interactions with the end effectors and the object. Here the objects position is relatively well known, and it is the position and orientation of the various contacts that are recovered. [Mal91] uses contact information to reduce the uncertainty of the robots position and orientation. The contact serves as a reference point for the robot. [TM089] uses a model of the dynamics of a serial manipulator and infers collision when a disturbance of sufficient magnitude occurs. The overall goal of our approach is similar to the collision detection presented in [TM089], but attempts to extract more information from the limited torque sensing. Like [TM089] we may estimate the disturbance torques by observing the system dynamics, or we may use direct measurements of the forces and moments as in [Sal83] if this sensor information is available. Sensing issues aside, the problem we wish to address is, to a large extent, the inverse problem of [Sal83]. The grasping problem of [Sal83] involves precise knowledge of the object, both position and orientation, with unknown contact geometry. The goal is to. infer the contact geometry from measurements of the applied forces and torques at the contacts. With the collision localization problem we use a model of the contact, with measured interaction forces, and infer the unknown position of the object. We propose a means by which not only the presence of a collision, but also the position of the collision on the manipulator can be inferred. [TM089] assumes that once a collision has taken place the robot is able to return to a "safe position". This is not simple in practice since the same positional errors in the robot that may have lead to the collision may make it impossible to move to the safe position. By 126 recovering the collision geometry we may make a more intelligent choice for error recovery. We begin in Section 4.3.3 with a description of contact forces on a serial manipulator in terms of features of the links, and their associated parameters. These features are combined with the configuration-dependent terms to produce a contact Jacobian which will fully describe the set of joint forces observable by the manipulator. The task of localizing the collision from a set of disturbance torques is presented in section 4.3.4. Geometric constraints on the position, as well as cone-constraints due to friction are given to further constrain the system. A means of qualitatively determining which feature took part in the collision, as well as metric for comparing competing contact hypotheses is then developed. Results of a simulation of a planar 3 DOF manipulator is presented in Section 4.3.7, followed by a discussion of possible extensions to the formulation in Section 4.3.8. We conclude with a summary of the results in Section 4.3.9. 4.3.3 Contact Forces Determining contact position involves finding a position and force which is con-sistent with the observed disturbance torques. Since there will be errors in our disturbance torque measurements, the contact information should correspond to the interpretation which "best" describes the measurements. To sufficiently con-strain the system we may have to impose additional constraints on the number and the type contacts which are modeled. To model the interaction of the manipulator with an object, we will con-127 sider a set of features which describe the set of generalized forces which can be transmitted to the manipulator. These features may be generate by point, line, or soft-finger contacts and may include frictional forces (see, for instance, [MS85]). For example, consider the simplified example of a three DOF manipulator with parallel joints in Fig. 4.2, with triangular shaped links. The manipulator is effectively planar, but we shall treat it as a spatial manipulator for consistency. Suppose there is frictionless contact between face i of link j and a point in the environment. Then the contact wrench (i.e., force and torque) on in link-_7's frame of reference is f \ ( n 'Wi e Ail \ Ui X rji (4.7) Here Aji is the magnitude of the contact force, rji G K^ is the unit vector normal to face i, Ui G M^ is a unit vector tangent to face i in the plane perpendicular to the joint axes (since this is eflFectively a planar problem), and Ai2 parameterizes the location of the contact on face i. Thus associated with each feature is a vector Aj whose elements parameterize the set of possible generalized forces the feature may produce. A,: V A,; i2 (4.8) It is important to note that the A^ are subject to further admissibility constraints; e.g., Aji is required to be non-negative since it represents the inward contact force, and Aj2 is subject to constraints from the geometry of the face i. We will return to this issue in Section 4.3.4. 128 Figure 4.2: A three DOF planar manipulator with triangular faces (taken from [RP95]). The set of all possible contact forces can be expressed in a single configura-tion independent matrix where n is the number of degrees of freedom of the manipulator, and n/ is the number of contact features. For the example suppose the potential contacts between points in the envi-ronment and faces of the links can be described by six features shown in Figure 4.2, one for each face i whose normals are given by 77^ . F is given as 129 F^ 7?i0 772 0 0 0 0 0 0 0 0 0 O z i O 22 0 0 0 0 0 0 0 0 0 0 0 0 773 0 774 0 0 0 0 0 0 0 0 0 0 23 0 24 0 0 0 0 0 0 0 0 0 0 0 0 775 0 776 0 0 0 0 0 0 0 0 0 0 25 0 26 ^ . ^ yAey 2j = Z^ j X 77J. (4.9) (4.10) (4.11) The propagation of forces on one link to another is represented by matrix (^(q) e lR6"x6n^ '/^ (q) )^ (^... ^ W21: \ WnE J 2iT ZxT I w 'A 0 )R P-,)R 0 j j J \R = (l)FX 10 / ^0^ ••• -2<t> (4.12) (4.13) (4.14) 130 where *WJE is the total wrench from all features (i, i + I,- • •, n), and J(/) is the adjoint transform [MLS94], which transforms a twist in reference frame j into an equivalent wrench in frame i. ^^R is the rotation matrix from i to j , and pjj is a vector from the origin of frame j to frame i. The matrix 0 is sometimes called the Composite Rigid Body transformation [Jai91]. We may then express observed torques at each of the joints as: T, - 5^(q),/.(q)FA (4.15) C{q) ^52(q) 0 (4.16) 0 "s„(q) where *Sj is the unit twist of the i-th joint. The contact Jacobian, C, gives the basis of all disturbance torques arising from the features / j . Therefore all information related the configuration and geometry of the arm is in the contact Jacobian C and the actual contact that occurs is parameterized by A. 4.3.4 Contact Localization The contact Jacobian, C, is a (nxup) matrix, where Up is the number of parameters in A. In order to solve for r^ using Eq. 4.15, we will have to make some assumptions on A. Generically, the initial contact between the manipulator and the environment will occur at a single feature of the manipulator. Since this is the most important case for detecting and localizing collisions, we will focus on this case here. In the example above, the single contact assumption gives us 6 possible solutions. 131 each corresponding to an over-determined 3 x 2 system of equations. Each contact hypothesis corresponds to taking a different subset of the columns of C. We will denote the reduced system obtained by taking the columns of C corresponding to feature z as Cj. We may then solve for K = CrVrf, (4.17) or if Ci is over-determined, then we may take the least squares solution Ai = (C^C)" ' c^ rd . (4.18) Once we have determined a value for A^ , the corresponding position on the link can be determined. For our example, the position of the link is given by P(A.) = ^ e [ 0 , L ] . (4.19) To determine which contact hypothesis best explains the observations, the contact state will be further analyzed as follows. In Section 4.3.5 we will check that the state of hypothesized contact is admissible given geometric and physical constraints. After this stage, it may be the case that more than one admissible hypothesis satisfies the constraints; in Section 4.3.6, we show how to construct a metric which compares the merit of the competing hypotheses to select an optimal hypothesis. 4.3.5 Admissibility Constraints The constraints on A depend on the parameterization of the features. We will give the A constraints for the example problem. The constraints for problems in 132 3D, or with the addition of friction will be marginally more complicated. Typical constraints include: Ci'. Non-negative normal force. The contact forces can only be "outward" relative to the surface of the link. C2: Geometric Constraints. The contact position must be on the link's surface. C3: Frictional Constraints. For our example the constraints are: Ci : Aji > 0. (4.20) C2 : 0 < £> = ^ < L. (4.21) where D is the position of the contact measured relative to link-z's frame of refer-ence. The constraints can be treated as a filter to eliminate hypotheses after the computation of the parameters Aj. However the constraints are typically linear inequalities, AiXi < 0; (e.g., Eq. 4.20 and Eq. 4.21). In this case the feasibility problem, QA, = Td (4.22) AiXi < 0 (4.23) can be solved simultaneously using linear programming. 133 4.3.6 Feature Identification In instances where there exists more than one admissible single-contact hypothesis which satisfies the constraints, we must use some means of determining which is most likely. For over-constrained problems, such as our example, a natural choice for ranking our solutions is the residual: proj, = | | ( / - C, {CfQ) Cf) T,\\ (4.24) the length of the projection of r^ orthogonal to the column space of Cj. This is the sum of squared differences of the predicted and observed disturbance torques. 4.3.7 Results To investigate the effectiveness of Eq. 4.24 as a feature classifier, a series of simula-tions involving the three DOF triangular-shaped manipulator were performed. A constant reaction force of IN was used in generating the feature torques, with unit link lengths (L=l). The feature, /,, was chosen randomly, as well as the position on the link. The joint angles g2 and q^ were chosen randomly from [0, 27r]. The ideal disturbance torques r^ were computed, to which varying noise was added. The rela-tive magnitude of the noise was held constant at various levels (0.01, 0.02, • • •, 0.30). The direction of the error in R^ was uniformly distributed. In this way the error was uniformly distributed amongst the individual disturbance torques. We measure success at classification in two ways: feature identification is measured by the percentage of contact features that are correctly classified; for each correctly identified feature, we measure the accuracy oi feature localization. 134 Table 4.1 shows the effect of noise on the error rate of the feature identifi-cation. The error rates of the classification scheme utilizing the constraints Ci in conjunction with projj are very small; features were misclassified in less than 2% of the tests for relative errors in r^ up to 30%. This indicates that proj^ is very effective in the identification of the feature involved in the collision, even when the disturbance torques contain a large relative error. It should be noted that the error rates do not include contacts with features / i and /2 on link 1 {i.e., only features /s, • • •, /e)- All simulated contacts on features / i and /2 are wrongly classified as /a and f^ respectively. This is due to the fact that a small error associated with the torque at 92 will always produce an explanation of a collision on link 2 very close to the proximal end of the link. Since the system is under-constrained for C\ and C2, any solution using the disturbance torques of Q2,- • • iQn refiect only the noise. Features /a and f^ are chosen rather than /s , /e because we are looking for the smallest A satisfying Eq. 4.15. In practice, this can be easily dealt with; for example small disturbance torques at the distal joints of the manipulator can be set to zero or solutions with positions very close to the proximal end of the link can be rejected. Since we have a large degree of confidence in the feature identification, we now turn our attention to the localization of the contact. The same method of constructing random collision examples was performed with the same noise mod-els, and an estimate of each collision location was computed for each example. Only samples in which the correct feature was identified were considered. Addi-tionally, only contacts involving /3,--- , /6 were considered since position cannot be recovered for collisions on the first link as it is under-constrained. 135 Relative Error ll^d-^dealll \\rA\ 0.01 0.02 0.05 0.10 0.15 0.20 0.25 0.30 Percentage Mis-classification 0.02 0.04 0.11 0.39 0.70 0.98 1.39 1.71 Table 4.1: Classification error rate with varying relative errors in r^. Fig. 4.3 shows the eflfects of noise on the confidence level of our scheme in localizing the collision to various tolerances. Consider the task of resolving the collision to within 5% of its true value. We can see that our confidence level is very high, 98.6% for 1% relative error, 91.7% for 2% relative error, and 72.3% for 5% relative error. 4.3.8 Extensions The methodology can be extended to include three-dimensional links, frictional forces, and links with curved surfaces. We briefly describe these extensions here. Three dimensional links and frictional forces can be modeled by extending the number of parameters for each feature. Curved link geometries are more difficult because of the non-linearities introduced. For example, addition of friction to our two-dimensional problem adds an additional parameter, the component of reaction force tangent to the surface, as 136 D. E 100 90 h 80 70 60 50 40 30 20 10 0 1 Samples successfully localized vs. localization tolerance " 1 " " " . ^ " . - - • o .-•' o .-' o 0.05 0.1 0.15 Localization tolerance (fraction of link-length) 0.01 — 0.02 —• 0.05 - -0.10 0.15 - -0.20 —• 0.25 ----0.30 o 0.2 Figure 4.3: Cumulative distribution of localization errors for varying relative error in Td. 137 well as an additional frictional constraint. Thus ^Wi = Aji ["•1 ["I + Aj2 f " 1 \i^iXr]i j + Ai3 l"'] [") (4.25) (4.26) where [i is the coefficient of friction. Since there are three parameters, we will only determine the collision position for collisions with link 3 or higher. Table 4.2 gives the number of parameters needed for various types of contact [MS85]: Contact Point contact without friction Point contact with friction Soft contact 2-D 2 3 3 3-D 3 5 6 Table 4.2: Contact Parameters. Thus for 3-dimensional frictional contacts we have 6 parameters. In general this will require that the contact occur on link-i, z > 6, if we are to determine exactly the position of the contact. In some cases, this may restrict our ability to recover contact geometry. However, the increased number of constraints may sufficiently restrict the set of feasible contacts to still be of use for contacts on prior links. For some restricted applications we may have the required information to recover contact geometry exactly. For example, knowledge of the shape of the payload of a 6 or greater DOF manipulator will allow collisions of the payload with the environment to be recovered. This might be useful for teleoperation tasks for example. The example we have been discussing has involved links whose surfaces are 138 easy to parameterize. In some applications, links will have curved surfaces, leading to a feature matrix, F = F{X), which is non-linear. This requires the solution of non-linear system of equations for Aj. An alternative approach is to approximate the surface of the link by a series of polyhedral faces. This approximation can be hierarchical and successively refined, i.e., if a solution is feasible at given level of approximation, the face can be decomposed into smaller faces, and the process is repeated. Thus at each step we may eliminate a large fraction of the remaining surface of feasible contacts. Our assumption is that while there may be localization inaccuracies due to the errors in approximating the normals of the surface with polygons, the positional information will be sufficient to constrain the position to a polygonal region. 4.3.9 Conclusions We have described a method by which un-modeled manipulator collisions can be identified, and the position of the contact can be localized. The method is based purely on the observed disturbance torques, and a set of features given by the geometry of the manipulator. The formulation provides an easy means of testing collision hypotheses, as well as a method for ranking competing hypotheses. We also describe extensions that are currently being investigated. Simulations of the method on a planar manipulator indicate that the method is robust with respect to noise for both collision feature identification, as well as feature localization. 139 Chapter 5 Trajectory Planning Experiments In this chapter we will describe a set of experiments which demonstrate the appli-cation of our methodology to practical problems. To show the applicability of the method to a variety of domains, we have chosen two very dissimilar domains: a locomotion task involving a 4-legged robot [RP97], and a pick-and-place task with a Puma 650 manipulator [RP99]. The locomotion task is an excellent candidate for the method since it shows the ease with which static stability and reachability con-straints can be expressed in LC. In addition, since there are 12 actuated degrees of freedom, LC provides a natural means of programming the robot. While there are a large number of actuated degrees of freedom, since we consider only translations of the body, and due to the positional constraints, the configuration space of the robot is C = K .^ The Puma 560 example deals with a larger number of degrees of freedom; we include it to show the method's ability to deal with higher dimensional problems. In addition, the simplicity of the task facilitates the interpretation of trajectories and their degree of fault tolerance. 140 5.1 Fault Tolerant Locomotion The work of fault tolerant locomotion was presented in [RP97], and concerns a 4-legged, spherically symmetric robot, called the 4-Beast, the first of a family of robots called Platonic Beasts, developed at UBC. The interested reader is encouraged to consult [PBR95a, PBR95b, PBR94] for a detailed description of the robot and prior work. There were two main reasons for constructing the 4-beast. First we hoped that the novel configuration of the robot would give new insights on general loco-motion, as well as allow the investigation of new gaits. Secondly, we were interested in the potential increase in fault tolerance that the spherically symmetric construc-tion would allow. Since it is widely believed that legged robots are well suited for rough or unknown terrain, it is hoped that an increased ability to circumvent or recover from tipping or falling would increase the utility of legged robots in these situations. People have been interested in legged robots from the early 1960s, as well as an increased interest in mobile robotics in general (for surveys see [CW90, Rai84, Rai86, SW89]). Inspired from biological perspectives on locomotion, the arrange-ment of the legs is typically modeled after insects and mammals [Fer93, Bro89]. In addition, knowledge obtained by observing animals is incorporated into the locomotion algorithms. As a byproduct of wanting to reduce the total energy ex-penditure, legged robots are designed to have a relatively small range of preferred orientations (there is evidence for this in nature as well). As a result they are sus-ceptible to toppling. Due to the unique spherical symmetry of the 4-beast, there is 141 no preferred orientation, and hence it is hoped the robot will be much more robust when operating in rough terrain. 5.1.1 The 4-Beast The 4-Beast was the first member of a family of robots called "platonic beasts", which are spherically symmetric, high degree of freedom robots with multi-purpose limbs. These robots are constructed by attaching a kinematic chain, i.e., a limb, at each vertex of a spherically symmetric polyhedron. The polyhedron can be one of the five Platonic solids — hence the name of the family; however, robots based on other spherically symmetric polyhedra such as the Archimedian polyhedra are included in this family as well. A sketch of the first two members of this family, the 4-beast generated by the tetrahedron, and the 8-beast generated by the cube, are given in Fig. 5.1, Figure 5.1: Examples of platonic beasts. The figure sketches the 4-beast, with RRR limbs placed at the vertices of a tetrahedron and an 8-beast with limbs at the vertices of a cube (adapted from [PBR94]). A prototype of the 4-beast is depicted in Fig. 5.2. The prototype was con-structed using an octahedron and taking the center of every other face as the vertex of a virtual tetrahedron. The octahedron allows each face to serve as either a mount 142 for the limb, or as a mount for the embedded micro-controller and electronics. Figure 5.2: Prototype of 4-beast. The robot has 4 limbs, each with 3 revolute joints, for a total of 12 actuated degrees of freedom. A key benefit of the spherical symmetry of limb placement is robustness with respect to toppling. This is particularly important for locomotion on rough terrain where it is difficult to measure terrain orientation, friction and integrity. On such terrain, it is not possible to guarantee toppling avoidance. Even if there is no physical damage to the robot after toppling, most legged robots may not be able to recover since the limb placement is specialized for operation in a small range of body orientations and the robot can land on its "back". The platonic beast design, on the other hand, has no direction specialized as the "up" direction, as can be seen from the rolling gait. A statically stable foot placement is available in all orientations of the body in three dimensions, allowing the robot to recover from a topple. We are not aware of any other robot with this ability. In addition to the symmetry, each of the legs are identical to one another 143 allowing the substitution of one leg for another in the event of a leg actuator failure. Each leg is controlled separately by a dedicated Motorola M68332 micro-controller, increasing the ease with which a leg may be substituted for another. This advantage is particularly relevant for beasts with larger number of legs such as the 8-beast, where, if a limb were to fail, the body could be rotated to a configuration where the defective limb was not needed for locomotion. 5.1.2 Rolling Gait Provided the relative lengths of the limbs as compared to the size of the body allows the beast to place all four limbs on the ground, the beast will be capable of the crawl or statically stable creeping gaits [MF68]. However, the novel construction of the robot gives rise to a new gait, termed a Rolling gait [PBR94]. The rolling gait is composed of a set of isomorphic steps called tumbles. A canonical tumble-step is depicted in graphic simulation in Fig. 5.3. 5.1.3 4-Beast Design Due to the modular design of the links it is possible to configure the legs in a variety of ways, as well as changing the link lengths. Before the gait could be constructed, it was first necessary to compute the design parameters which permitted the best gait given the requirements of static stability and maximum torque limits. To this end, a simulator was developed which allows the user to specify the mass and size of the body, and the leg geometry [PBR94]. The user can compose a candidate tumble trajectory, and verify that the torque and static stability constraints are 144 Figure 5.3: A simulation of a canonical tumble-step. We start in the initial config-uration given by (a), and rotate the body counter-clockwise. The top leg replaces the rightmost leg as a a support leg, bringing the rightmost leg to rest at (h) at the top. (adapted from [PBR94]). 145 not violated. An image of the 4-beast simulation is given in Fig. 5.4. Figure 5.4: A simulator, running on a SGI workstation, of the 4-Beast. The design verification involves ensuring static stability and maximum torque requirements are satisfied throughout the trajectory. 5.1.4 Specification of the Tumble Step The task of generating a canonical tumble-step has been investigated in [PBR95b], and is illustrated in Fig. 5.5, however the trajectory was not chosen according to any fault tolerance criteria. To compute a fault tolerant tumble-step for the 4-beast, consider an idealized 4-beast, depicted in its initial configuration in Fig. 5.6. For the purposes of the specification, we will label the feet as L,R,T and B meaning the "left", "right", "top" and "back" feet respectively, as indicated in Fig. 5.6. We will let the edges of the tetrahedron, as well as the link lengths for each leg, be of unit length. This means that the workspace of each leg is a sphere of radius 2 units centered about 146 Figure 5.5: Canonical tumble with 4-beast prototype up a 20 degree slope (taken from [PBR95b]). the vertex to which the leg is attached. Furthermore we will assume that the foot positions for the left, right and back foot are given by p \ p ^ and p^ respectively, as depicted in 5.6, with The position of the transfer foot is p' = *(o .^ .o) ' . The parameter 0 determines the size of the support triangle. The foot placement was chosen to lie on an equilateral triangle since this maximized the size of the feasible configuration space, as well as simplifying the kinematics. Also, using a tumble with equilateral triangles allows one to construct a path with a series of isomorphic steps, where each step is generated from a canonical tumble by a relabeling of the limbs, and a fixed rotation about the body's center of mass. The goal of the tumble-step is to move the body in the (—y)-direction, allowing the robot to place the top leg at position p"*. Each step consists of a translation of the body, followed by a rotation at the end of the step to reorient the body. If we are free to position and orient the body with three feet placed on the 147 p . V Back Support Polygon Front Support Polygon Transfer foot Figure 5.6: Starting configuration for a tumble-step for an idealized 4-beast. ground, then the configuration space of the robot is X S0{3) X T 12 for a total of 18 degrees of freedom. The constraint that the feet must remain fixed at positions p^, p^ and p^ provides us 9 constraints leaving 9 remaining degrees of freedom. To simplify the visualization of the resulting trajectories, we will consider only translations of the body, and can therefore take C = E^ with denoting the position of the center of mass of the robot's body. Fixing the feet positions means that the joint angles for each of the support legs can be determined directly from the body position, with at most 4 distinct solutions for each leg. Visualizing the trajectory of the robot in this reduced configuration space is made much easier. 148 To ensure that we remain statically stable, we must ensure that the center of mass of the robot, when projected into the a;y-plane, lies in one of the two support triangles, Ap^pV, or Ap^pV-We can write the static stability constraint corresponding to the support triangle Ap-'^p^p^ as the conjunction of three constraints: 3 where /ii,i(q,i) = q x ( p ^ —P^)-k, (5.2) hiM^t) = q x ( p 3 - p 2 ) . k , hiM,t) = q x ( p ^ - p ^ ) - k , and k denotes the unit vector in the positive ^-direction. Similarly for the support triangle Ap-'^p'*p^, the static stability constraint is 3 1=1 where /i2,i(q, t) = q x (p^ - p-^ ) • k, (5.3) ^2,2(q,t) = q x ( p 2 - p 4 ) . k , h2,3{q,t) = q x ( p i - p 2 ) . k . In addition to the stability requirement, we must also ensure that the robot is able to reach each of the feet position. For each foot position p ' the reachability constraint is given by gp'{q,t) = ( V < 0 ) ' (5-4) 149 where hpi p ' - q - ( < / . - ! ) 2 - 1 2x/3 \ 271 / - 2 , (5.5) Next we must define a driving constraint which forces the trajectory of the robot in the (—y)-direction. This is given by the constraint function 9d = {hd < 0), hd,{q,t) = -yo + y + t, yo = 0.2165. (5.6) This ensures that the robot moves from its initial position of y = yo at a rate of at least one unit length per unit time. The choice for the value yo was made to correspond to the decomposition of the feasible configuration space (discussed in Section 5.1.5). Lastly, we must also ensure that the robot does not collide with the ground, which is accomplished with the following height constraint: 9h hh{q,t) = {hh < 0), 1 2^6 z. (5.7) Using the above constraint predicates, the specification is: G = [gdAQh^ (5.8) {{gpip2p3 A gp, A 5fp2 A ppg) V {gpip4p2 A gp, Agp.AgpJ)). This ensures that the robot is in one of the two support triangles, can reach each of the three foot positions corresponding to the support triangle, and continues to move in the (—y)-direction. 150 5.1.5 Decomposi t ion of ^ C T Next we performed a uniform decomposition of the valid space, defined by Eq. 5.9, in which each of the three dimensions was subdivided into 8 intervals. This yielded 56 valid cells, depicted in Fig. 5.7. 0.50 X Figure 5.7: Two views of the 56 valid cells of the configuration space for the 4-beast. 5.1.6 Computing the Measure of Fault Tolerance When computing the kinematic effects of a fault, we must consider the kinematic mapping of/Cf23 and /Cf42, where /C, : T ^ ^ C . (5.9) which maps a set of 9 joint angles, corresponding to the three supporting legs, into a robot position (x, y, z) € C. For configurations q which make use of support 151 triangle Ap-^p^p^ we use JC^i^, and for configurations using Ap-^p^p^ we use /Cf42. The inverse kinematic mapping is not unique, but gives at most four leg solutions for each body configuration. In computing the fault tolerance measure for each of the 56 cells, we con-sidered 12 faults, each corresponding to the immobilization of a single actuator of the robot. To compute the fault tolerance measure, longevity, we took the center point of each cell, and found the trajectory corresponding to the optimal recovery motion. The optimal recovery motion is a trajectory from the center of the cell to the configuration in the reduced order derivative with the largest utility. This trajectory exists in the configuration space of the reduced order derivative, a two dimensional sub-manifold of C. This trajectory is the optimal recovery motion for the fault. A gradient descent method was used to find this recovery motion [PTTF92]. The total number of constrained optimization problems solved was 56 X 12 = 672. Given the values of L{vi,ujj), z = l , - -- ,56, j = l , --- ,12, we can compute the average- and worst-case fault tolerance measures as: L^Yg{vk) = 7^ (Zl-^(^^'O) ' and (5.10) 12 LwoTst{vk) = mnL{vk,i). (5.11) Given that the units of L{vk,i) are time, we can compute the equivalent y-position, y{vk), using Eq. 5.6, setting h^ — Q and solving for y: Ve^vgivk) = Vo- -^ avg(wfc), and (5.12) 152 yvjOTstyVk) — yo ~ Lworst\Vk)- (5.13) This allows us to interpret ya,yg{vk) as the closest position to the goal attainable, given a fault, averaged over the 12 failure modes of configuration Vk- The closest position to the goal attainable, given the worst possible fault, while in configuration Vk is given by ^worst(t'A;)-5.1.7 Generating the Paths Using the fault tolerance performance measures of Lavg(t'A:) and L^iorstivk), a tra-jectory was constructed using the Sorted-Minimum path ranking. Let x^^s and j^ worst (jenote the paths constructed using Z/avgC^ jt) and -Lworst(wfc) respectively. The initial point was taken as / \ V (5.14) / 0 0.2165 0.9375 No final position was specified, and was left as a free parameter for the path optimization. As a benchmark for performance we will compare the trajectories to a straight-line motion. The straight line motion in parametric form is: / . \ / (5.15) 0 x'(t) = 0.2165 +t ^ 0.9375 This path is the shortest straight-line path passing through the centers of starting cell to the smallest attainable y-value. / V 0 - 1 -0.325 \ 153 5.1.8 Evaluating Path Performance The results of the fault tolerant paths generated using Lavgl^ fc) and I/worst (^ Jt) are depicted with the straight-line motion in Fig. 5.8, as well as the a^vg and yworst depicted in Fig. 5.9 and Fig. 5.10 respectively. z 1 -0.9 0.8 0.7 0.6 0.5 0.4 0.1 -0.05 • X <^cr 0 ^ 0.05 start 0.2 Figure 5.8: Fault tolerant trajectories of the 4-beast as computed using the Sorted-Minimum path ranking and the fault tolerance measures I/avg(^ fc) and I/worst ("^ fc), as well as a straight-line motion for the same task. The straight-line motion, denoted x^, acts as a benchmark to gauge the fault tolerance of the resulting trajectories. There are two interesting features of the fault tolerant paths, given in Fig. 5.8, which are worth noting. First, the fault tolerant paths are consider-ably longer than the straight-line motion. The lengths of the trajectories in C are summarized in table 5.1. This indicates that the fault tolerant path was forced to move significantly 154 Path Straight-line -'^avg -^ worst Length 0.607 1.58 1.60 Table 5.1: Trajectory lengths for 4-beast experiment. away from the goal in order to ensure tolerance to faults along the path. The second feature is the departure of both the average- and worst-case fault tolerant paths from the straight-line motion. Both of these departures occurred at approximately the mid-point of the straight-line motion. This indicates that the region of the configuration space near the point of divergence has a relatively low measure of fault tolerance. We can verify that the measure of fault tolerance in this region was small by examining the values of a^vg and ^worst in this region. To evaluate the degree of fault tolerance of each of the three paths, 20 samples were taken along the trajectory such that the arc length between samples was equal. For each sample yavg and yworst was computed, as well as the y-positions for the straight line motion. The results are depicted in Fig. 5.9 and Fig. 5.10. Comparing the average-case fault behaviors of yavg(x^^ )^ against the corre-sponding straight-line motion yavg(x^) we see that the longevity path consistently performs much better. If we were to define "success" as reaching a y value of say, —0.35, we would see that over half of the longevity path would be 1-fault tolerant on average. Comparing the relative improvements of the x^^^ and j^ .worst ^ ^g compared to the straight-line motion, we see that the worst-case fault tolerant path has a larger relative improvement. The larger relative improvement is likely due in part to the 155 S3i yavg vs. arc length T 0.4 0.6 Scaled arc length Figure 5.9: Evaluating the fault tolerance of the trajectories. The closest point to the goal, given a fault, averaged over all 12 possible failure modes of a configuration. yavg(x^^ )^ corresponds to points taken along the fault tolerant path, while yavg(x^) correspond to equivalent configurations taken along the straight-line path. 156 ;35 0.05 <iif yworst vs. arc length 0 \ N -0.05 --0.1 --0.15 -0.2 h -0.25 -0.3 -0.35 -0.4 >> >> VwOTSt y^^i^tl -<s-<^ «>. ^x>—o—^-^^-o-<t _L I I 0 0.2 0.4 0.6 Scaled arc length 0.8 Figure 5.10: Evaluating the fault tolerance of the trajectories. The closest point to the goal, given the worst-case fault over all 12 failure modes of a configu-ration. yworst(x™°'^ '^ *) corresponds to points taken along the fault tolerant path, while yworst(x )^ correspond to equivalent configurations taken along the straight-line path. 157 fact that the minimum path performance criteria was used which is better suited for computing worst-case paths than for average-case paths. 5.2 Fault Tolerant Manipulation Computing a fault tolerant trajectory for a robot manipulator, first presented in [RP99], concerned a pick-and-place task performed on a Puma 560 manipulator. An example of such a task is given in Fig. 5.11, in which we are given an initial and final position in the workspace. We will assume that the positioning task ignores the orientation of the end-effector, hence W C M .^ Since the manipulator has 5 actuated degrees of freedom which effect the position of the end-effector (the 6**^ actuator performs a roll along the z-axis of the tool), the robot is kinematically redundant, with r = 5 — 3 = 2, orders of redundancy. ID Figure 5.11: Initial and final configurations for a pick-and-place task using a Puma 560 robot, (a) denotes the initial, and (b) the final configuration. The goal position is given by the small cube in the workspace of the manipulator. Despite the fact that there are two degrees of redundancy, the fault tolerance of configurations of the robot vary greatly. This illustrated in Fig. 5.12. The goal 158 position is given by the small cube, (a) and (b) are the same distance from the goal in joint space, but have very different fault tolerant capabilities. Taking the worst-case fault for (a) and (b), resulting in a frozen actuator, we compute the recovery motions which minimize the distances to the goal. The endpoints for the recovery motions of (a) and (b) are given by (c) and (d) respectively. We see that the recovery motion of (a) is able to get much closer to the the goal position as compared to (b). E ID C=±) (b) DD (d) Figure 5.12: Two competing configurations, (a) and (b) are two configurations at the same distance in joint space from the goal. Freezing the most critical-actuator for each we compute the optimal recovery motion to the goal (shown as the small cube). The endpoints for the recovery motions for (a) and (b) are given by (c) and (d) respectively. 159 5.2.1 Defining the Task The Denavit-Hartenberg parameters, for the Puma 560, simplified according to [McK91, p. 218-219], are given in table 5.2. Link 1 2 3 4 5 6 Angle dn ei 02 03 04 0, Oe Displacement dn 660.4 149.5 0 432 0 56.5 Length ''n 0 432 0 0 0 0 Twist ^n +90° 0° -90° +90° -90° 0° Range (°) -160,160 -225,45 -45,225 -110,170 -100,100 -266, 266 Table 5.2: Denavit-Hartenberg parameters of Puma 560 manipulator. The forward kinematics relation is given as where p^(q) = Cil-CisdedS^ - S23{deC5 + d^) + I2C2C2] +5i {deSiSs + (^2), Pj/(q) - Si[-C2sd(iC4S5-S23{deC5 + di) + l2C2] + [^65455 + ^2] , Pz(q) = -S23deC4S5 + C23{deC5 + d4)+l2S2 + di, Ci = cos{qi), Si = sm{qi), Cij = cos{qi + qj), Sij = sm{qi + qj). (5.16) (5.17) (5.18) (5.19) (5.20) We will take the goal manipulator position as x^. To simplify the specifica-tion, as well as permitting a simple interpretation of the results, we will define the proximity of the end-effector as p r o x ( q ) = dj^^ (d^ax - I l ^puma(q ) " X^ (5.21) 160 where dmax is the farthest distance of any point in the workspace of the manipulator from the goal position x^. The range of prox(q) is the unit interval, and is at a maximum when the end effector is at the goal. We then define the task as a simple relation on the proximity: G = (^proxA^ji) (5.22) ^prox = (/iprox < 0), /iprox(q,i) = t - p rox (q ) , (5.23) 9JL = 91 e [-160°, 160°] A 92 e [-225°, 45°] A (5.24) 93 G [-45°, 225°] A 94 e [-110°, 170°] A 95 G [-100°, 100°]. The constraint gji ensures that the joint limits are enforced. Each interval constraint of the form Qi G [a, h] can be specified using two inequality constraints. The specification is constructed so that motion is completed in one time unit. The construction of the task allows us to interpret the safety measure L(q, w) in terms of the proximity prox(q'^) of the endpoint q'^ of the recovery motion. 5.2.2 Decomposition of the Configuration Space Since the only time-dependent constraint is Eq. 5.23, which is a simple linear function of t, we can omit the time-dimension of C and let C = C, 161 reducing the dimension of C from 6 to 5 dimensions. Taking the goal position x^ = (55, —430,1472), and taking d = 20, resulted in a total of 470,400 cells within the joint angle limits, each 18° on a side. Each cell is connected to all valid cells that share a face, thus each vertex has a minimum degree of 5, and a maximum degree of 10. The accessible portion of TCT is [-144°, 144°] X [-216°, 36°] x [-36°, 216°] x [-108°, 162°] x [-90°, 90°] (5.25) ^ J- ' ^ V ' ^ V ' ^ J- ' ^ J- ' 16 cells 14 cells 14 cells 15 cells 10 cells Trajectories were constructed using the center points of each of the cells through which the path passed. The initial configuration was taken to be q° = (171°, -171°, 27°, 153°, -27°)^, corresponding to a manipulator position of (101.6,155.7, 216.0)-^ measured in mm from the center of the base. The initial configuration is depicted in Fig. 5.11(a). The distribution of the utility values, util(wfc), and the fault tolerance mea-sure, L{vk) is given in Fig. 5.13. The recovery motions for each cell were computed for each of the 5 possible actuator faults at each cell. Each recovery motion was constructed by taking 69 different slices through the discretized configuration space to form the RODs, as summarized in Table 5.3. The number of cells in each ROD varied from 29,400 to 47,040 cells, depending on the failed actuator. The vertices of the ROD corre-sponded to F / = [v,\x] = k], k = l,---,d. (5.26) A fault tolerant trajectory was computed using the worst-case fault toler-162 35 Distribution of util(T;fc) and L^oTst{vk), over 470,400 cells 25 h 20 15 10 5 0 ----_ -1 1 1 (/J O ^ 1 1 util(vfc) -^worst('yfc) 1 1 r- -1 ----^ -0.2 0.4 ^ ^ 0.6 ^ ^ 0.8 Value of util(wfc) or Lworst(^ 'fc) Figure 5.13: Distribution of util(wA:) and L„oTst{i^k), taken as a percentage of the 470,400 valid cells. Failed Actuator 1 2 3 4 5 Total # RODs considered 16 14 14 15 10 69 # cells in each ROD 29,400 33,600 33,600 31,360 47,040 Table 5.3: A summary of the number of actuator failures considered, and the size of each ROD. 163 ance measure Lworst from the initial configuration to the goal position x^, passing through a total of 20 vertices. Let Pft represent this fault tolerant trajectory. For the sake of comparison, a joint-interpolated motion was also constructed from the initial configuration q*^ to the goal. The joint-interpolated motion cor-responded to the smallest total displacement in configuration space. Let Pji be the list of vertices corresponding to this joint interpolated motion. Like the fault tolerant path Pft the joint interpolated motion passed through 20 vertices. The trajectories Pft and Pji are given in Fig. 5.14. To evaluate the fault tolerance of the two paths, the recovery motions for the worst-case fault for each vertex in both PpT and Pjj was computed. Fig. 5.15 gives the endpoint of the optimal recovery motion for each of the worst-case fault scenarios. We can see that the fault-tolerant path is able to guarantee a significantly closer proximity to the goal for much of the trajectory. This is especially true for the vertices 10-19 of the trajectories, where the worst-case faults result in recovery motions that are still quite close to the goal. We can better evaluate the performance of the fault tolerant trajectory by examining the utility and longevity along the two paths. We can interpret the utility of the trajectory as the proximity to the goal position, and the longevity as the proximity of the recovery motion for the worst-case fault. Fig. 5.16 gives these values taken at each vertex of PpT and Pjj. We can see that the joint interpolated motion has a much closer proximity, especially through steps 1-14. We can compute the actual distance knowing d^ax = 1826mm, so d(q) = (1 — prox(q))(imax- The proximity value (utility) at step 14 164 Vertex# 1 2 3 4 5 6 7 (D Q (D ID ID (D C FT JI Vertex# 8 10 11 12 13 14 FT JI I (D (D (D (D Vertex# 15 FT 16 17 18 19 20 JI ii Figure 5.14: Trajectories of fault tolerant path, Pft, generated with L^orst measure, and joint interpolated motion Pji. 165 Vertex# 1 2 FT Critical Act. 1 f JI Critical Act. 1 Vertex# 8 9 10 11 12 13 14 FT Critical Act. 2 JI Critical Act. f I' f (f Vertex# 15 16 17 18 19 20 FT Critical Act. JI Critical Act. 1 1 Figure 5.15: Endpoint configurations for optimal recovery motions for the worst-case faults for both the fault tolerant path (FT) and the joint interpolated path (JI). 166 Fault tolerance measure Lworst and utility, for Pjt and Pjj 0.95 -0.85 0.75 -10 12 14 16 Path Length Figure 5.16: Longevity and utility vs. path length for optimal and straight-line joint-interpolated motion. 167 is only 0.365 (1160mm) for PpT while Pjj has a proximity of 0.653 (634mm). While the proximity values of the joint-interpolated path are almost monotonically increasing, the values for PpT remain almost constant at 0.725 (502mm) for steps 1-11. Examining the longevity values for both trajectories we see that the fault tolerant path is able to make considerable gains over the interpolated motion. The mean longevity value for Ppr path is 0.907, and for joint-interpolated motion it is 0.849 for a difference of 0.058 (106mm). This means that given a single fault occurring along the trajectory, the use of the fault tolerant path will, on average, result in a recovery motion which is 106mm closer to the goal. The maximum difference in the longevity values occurred at step 11 where the FT path had a longevity value which exceeded the JI path by 0.114. A significant feature of the longevity plots is that the longevity values remain at the optimal value of unity from step 14 to the end of the motion for PpT- The joint-interpolated trajectory on the other hand does not reach a longevity value of unity until step 19. This means that, even though the proximity at step 14 is only 0.365 at this point, it is guaranteed to reach the goal under any 1-fault scenario. From the plots of utility and longevity it is clear that the fault tolerant path is optimizing the longevity measure by choosing configurations which are not closer to the goal, but rather are safer. 168 Chapter 6 Conclusions and Future Work We have described a comprehensive framework for programming robots to perform a task in a fault tolerant manner. The methodology encourages fault tolerant behavior at two levels: at the task-design phase by encouraging the designer to omit extraneous constraints which reduce the potential for fault tolerant operation, and at the trajectory generation phase by avoiding critical configurations. The method is unique in its ability to deal with robots which are not kinematically redundant with respect to arbitrary task, but which are sufficiently redundant so as to allow the task to be described as a set of "loose" constraints over time. Since LC allows us to model faults as additional constraints to the speci-fication, we can efficiently compute the effect a fault will have on the ability to complete the task, using the reduced configuration space of the robot. Faults not previously considered, such as the inclusion of additional obstacles, as well as dynamic information arising from sensors, can also be included using this formal-ism. An efficient algorithm for constructing a recovery motion for a fault has been 169 developed. We have developed a global measure of fault tolerance which can be used to identify configurations which are tolerable to faults. The fault tolerance measure examines a set of faults which may occur at a given configuration, and based on the optimal recovery motions for the given fault, ranks the configuration in its ability to continue to satisfy the task requirements. We have developed an algorithm which, given the fault tolerance measure evaluated at discrete points of the configuration space, produces a trajectory which maximizes the utility of the worst-case failure mode of the robot. The effectiveness of the methodology has been demonstrated in two experiments, as well as showing the applicability of the method to a number of domains. The resulting trajectories were analyzed with respect to their ability to sustain a fault, and we compared them to more traditional methods for accomplishing the same task. We have demonstrated that trajectories obtained using the LC method were able to achieve a much larger degree of fault tolerance than naive methods for the same task. The results of the experiments have shown that the fault tolerant paths are often less direct, making use of configurations that are safe and not necessarily close to the goal. In this sense they are trading oflf trajectory length for safety. A specific example of a seldom considered fault, the collision of the robot by an unknown obstacle, has been developed. We have shown that in addition to detecting the event, we are also able to recover the collision geometry. This information can then be used in a more intelligent choice for a recovery motion. 170 6.1 Future Work Thus far we have only considered fault scenarios involving a single actuator. The generality of modeling faults as additional task constraints would easily permit us to model two or more faults using the same formalism. For example, when computing combinations of two faults, to^ and tu ,^ the valid portion of the reduced order derivative is simply J^ Cr^ i^ 2 = {qeTCT\uj'{q)Auj\q)}. (6.1) While more computationally intensive, computing trajectories which are 2-fault tolerant is possible, and would be an interesting avenue for future work. Since we can easily model the inclusion of obstacles, the methods could be easily adapted for computing trajectories in which an obstacle of known geometry, but unknown position, is introduced into the configuration space of the robot. In this sense we are able to model sensor uncertainty as a "fault" insofar as the construction of the trajectory is concerned. We have focused on the sorted-minimum path metric when producing the fault tolerant trajectories. This metric is conservative in that it considers the worst-case failure mode of the trajectory. Exploring alternative path metrics, such as the mean fault tolerance measure along the trajectory, would be a practical extension of the methods proposed. 171 Bibliography [Alb72] Arthur Albert. Regression and the Moore-Penrose Pseudoinverse. Aca-demic Press Inc., New York, 1972. [Ang92] J. Angeles. The design of isotropic manipulator architectures in the presence of redundancies. In International Journal of Robotics Re-search, volume 11, pages 196-201, 1992. [BDG71] R. Boudarel, J. Delmas, and P. Guichet. Dynamic Programming and its Application to Optimal Control. Academic Press, New York, 1971. [BDG85] J. Bobrow, S. Dubowsky, and J. Gibson. Time-optimal control of robot manipulators. International Journal of Robotics Research, 4(3), 1985. [BNS91] A. Bar-Noy and B. Schieber. The Canadian traveler problem. In Proc. 2nd Annual ACM-SIAM Sym. on Discrete Algorithms, pages 261-270, 1991. [Bro83] R. A. Brooks. Solving the find-path problem by good representation of free space. IEEE Trans. Systems, Man, and Cybernetics, SMC-13(3):190-197, 1983. [Bro89] R. A. Brooks. Robots that walk: Emergent behaviors from a care-fully evolved network. In International Conference on Robotics and Automation, pages 692-694, 1989. [BT84] C. Bonivento and A. Tonielli. A detection estimation multifilter ap-proach with nuclear application. In Proceedings of the 9th World Congress of IFAC, pages 1771-1776, Budapest, Hungary, 1984. [Bur89] J. W. Burdick. On the inverse kinematics of redundant manipulators. In International Conference on Robotics and Automation, pages 264-270, Scottsdale, AZ., May 14-18 1989. [Can88] John F. Canny. The Complexity of Robot Motion Planning. MIT Press, Cambridge, MA., 1988. 172 [Gan93] J. F. Ganny. The Complexity of Robot Motion Planning. MIT Press, Cambridge, MA., 1993. [Gla78] R. N. Clark. Instrument fault detection. IEEE Transactions on Aerospace and Electronic Systems, 14(3), 1978. [GLR90] Thomas H. Gormen, Charles E. Leiserson, and Ronald L. Rivest. In-troduction to Algorithms (McGraw-Hill edition). McGraw-Hill, 1990. [GW84] Edward Y. Chow and Alan S Willsky. Analytical redundancy and the design of robust failure detection systems. IEEE Transactions on Automatic Control, AG-29(7):603-614, July 1984. [GW90] I. J. Cox and G. T. Wilfong, editors. Autonomous Robot Vehicles. Springer Verlag, 1990. [DBC+90] T. Dean, K. Basye, R. Chekaluk, S. Hyun, M. Lejiter, and M. Ran-dazza. Coping with uncertainty in control systems for navigation and exploration. In AAAI-90, pages 1010-1015, 1990. [Don87] B. R. Donald. Error Detection and Recovery for Robot Planning with Uncertainty. PhD thesis, MIT Department of Electrical Engineering and Computer Science, Cambridge, MA., 1987. [Don89] Bruce R. Donald. Error detection and recovery in robotics. Springer-Verlag, Berlin, 1989. [Fer93] Cynthia Ferrell. Robust agent control of an autonomous robot with many sensors and actuators. Master's thesis, MIT, 1993. [FL87] Pamela K. Fink and John C. Lusth. Expert systems and diagnostic expertise in the mechanical and electricaldomains. IEEE Transactions on Systems, man and Cybernetics, SMG-17(3):340—349, May/June 1987. [Fra90] Paul M. Frank. Fault diagnosis in dynamic systems using analytical and knowledge-based redundancy - a survey and some new results. Automatica, 26(3):459-474, 1990. [GL89] G. H. Golub and C. F. Van Loan. Matrix Computations. The John Hopkins University Press, Baltimore, second edition, 1989. [GN87] Michael R. Genesereth and Nils J. Nilsson. Logical foundations of artificial intelligence. Morgan Kaufmann, Los Altos, GA., 1987. [GV89] Aleks GoUii and Pravin Varaiya. Hybrid dynamical systems. In IEEE 29th Conference on Decision and Control, pages 2708-2712, Dec. 1989. 173 [HSL78] A. L. Hopkins, T. B. Smith, and J. H. Lala. Ftmp - a highly reliable fault-tolerant multiprocessor for aircraft. Proceedings of the IEEE, 66(10):1221-1240, Oct. 1978. [Ise93] Rolf Isermann. Fault diagnosis of machines via parameter estimation and knowledge processing - tutorial paper. Automatica, 29(4):815-835, Jul 1993. [Jai91] A. Jain. Unified formulation of dynamics for serial rigid multibody systems. Journal of Guidance, Control, and Dynamics, 14(3):531-542, 1991. [KH83] C. A. Klein and C. H. Huang. Review of pseudoinverse control for use with kinematically redundant manipulators. IEEE Transactions on Systems Man and Cybernetics, SMC-13(2):245-250, March/April 1983. [Kir70] Donald E. Kirk. Optimal Control Theory: An Introduction. Electrical Engineering Series. Prentice-Hall, 1970. [KL88] B. J. Kuipers and Tod S. Levitt. Navigation and mapping in large-scale space. AI Magazine, 9(2):25-43, 1988. [KM91] C. A. Klein and T. A. Miklos. Spatial robotic isotropy. In IJRR, volume 10, 1991. [LA81] P. A. Lee and T. Anderson. Fault tolerance, principles and practice. Prentice Hall, Englewood Cliffs, NJ., second revised edition, 1981. [Lat91] Jean-Claude Latombe. Robot Motion Planning. Kluwer Academic Publishers, Boston, MA., 1991. [LG87] T. A. Linden and J. Glicksman. Contingency planning for an au-tonomous land vehicle. In International Joint Conference on Artificial Intelligence, pages 1047-1054, Milan, Italy, 1987. [Lie97] Liegois. Automatic supervisory control of the configuration and be-havior of multibody mechanisms. IEEE Transactions on Systems Man and Cybernetics, SMC-7(12):868-871, Dec. 1997. [LM94a] Christopher L. Lewis and Anthony A. Maciejewski. Dexterity opti-mization of kinematically redundant manipulators in the presence of failures. Computers and Electrical Engineering, 20(3):273-288, 1994. 174 [LM94b] Christopher L. Lewis and Anthony A. Maciejewski. An example of failure tolerant operation of a kinematically redundant manipulator. In International Conference on Robotics and Automation, pages 1380-1387, 1994. [LMT87] T. Lozano-Perez, M. T. Mason, and R. H. Taylor. Automatic synthe-sis of fine-motion strategies for robots. Internal Journal of Robotics Research, 3(l):3-24, 1987. [LP82] T. Lozano-Perez. Robot Motion: Planning and Control, chapter 6. MIT Press, 1982. [LR91] Rogelio Luck and Asok Ray. Failure detection and isolation of ultra-sonic ranging sensors for robotic applications. IEEE Transactions on Systems, Man, and Cybernetics, 21(l):221-227, 1991. [Mac90] A. A. Maciejewski. Fault tolerant properties of kinematically redun-dant manipulators. In International Conference on Robotics and Au-tomation, pages 638-642, 1990. [Mal91] Raashid Malik. Location by collision. In Proceedings of the IEEE In-ternational Conference on Systems, Man and Cybernetics, V. 2, pages 877-882, 1991. [McK91] Phillip John McKerrow. Introduction to Robotics. Electronic Systems Engineering Series. Addison-Wesley Publishing Co., Reading, MA., 1991. [MF68] R. B. McGhee and A. A. Frank. On the stability properties of quadruped creeping gaits. Mathematical Biosciences, 3:331-351, 1968. [MLS94] Richard M. Murray, Zexiang Li, and S. Shankar Sastry. A Mathemat-ical Introduction to Robot Manipulation. CRC Press, Boka Raton, FL, 1994. [MS85] Matthew T. Mason and J. Kenneth Salisbury, Jr. Robot Hands and the Mechanics of Manipulation. MIT Press, Cambridge, MA., 1985. [Nen89] D. N. Nenchev. Redundancy resolution through local optimization: A review. Journal of Robotic Systems, 6(6):769-798, 1989. [Pai91] Dinesh K. Pai. Least constraint: A framework for the control of complex mechanical systems. In Proceedings of the American Control Conference, pages 1615-1621. American Automatic Control Council, IEEE, 1991. 175 [PAK94] C. J. J. Paredis, W. K. Frederick Au, and P. K. Khosla. Kinematic design of fault tolerant manipulators. Computers and Electrical Engi-neering, 20(3), 1994. [PBR94] Dinesh K. Pai, Roderick A. Barman, and Scott K. Ralph. Platonic beasts: A new family of multilimbed robots. In International Confer-ence on Robotics and Automation, volume 2, pages 1019-1025, 1994. [PBR95a] Dinesh K. Pai, Roderick Barman, and Scott K. Ralph. Design and programming of symmetric platonic beast robots. In O. Khatib and J. K. Salisbury, editors. Experimental Robotics IV. Springer-Verlag, 1995. Presented at the Fourth International Symposium on Experi-mental Robotics, June 30-July2, 1995. [PBR95b] Dinesh K. Pai, Roderick A. Barman, and Scott K. Ralph. Pla-tonic beasts: Spherically symmetric multilimbed robots. Autonomous Robots, 3(2):191-202, 1995. [PK94] C. J. J. Paredis and P. K. Khosla. Mapping tasks into fault tolerant manipulators. In 1994 IEEE International Conference on Robotics and Automation, volume 1, pages 696-703, 1994. [PK95] Christiaan J. J. Paredis and Pradeep K. Khosla. Global trajectory planning for fault tolerant manipulators. In 1995 lEEE/RSJ Interna-tional Conference on Intelligent Robotis and Systems, volume 2, pages 428-434, 1995. [PK96] Christiaan J. J. Paredis and Pradeep K. Khosla. Fault tolerant task execution through global trajectory planning. Reliability Engineering and System Safety, 53(2):225-236, 1996. [PR95] Dinesh K. Pai and L. M. Reissell. Multiresolution rough terrain motion planning. In IEEE International Conference on Intelligent Robots and Systems (IROS), volume 2, Pittsburgh, PA., 1995. [PTTF92] William H. Press, Saul A. Teukolsky, William T. Tetterling, and Brian Flannery. Numerical Recipes in C: The Art of Scientific Computing. Cambridge University Press, second edition, 1992. [PY89] Christos H. Papadimitriou and Mihalis Yannakakis. Shortest paths without a map (extended abstract). In Proceedings of the 16th ICALP, Lecture Notes in Computer Science, No. 372, pages 610-620. Springer Verlag, July 1989. 176 [Qi94] Runping Qi. Decision Graphs: Algorithms and Applications to Influ-ence Diagram Evaluation and High-Level Path Planning Under Uncer-tainty. PhD thesis, University of British Columbia, 1994. [Rai84] M. H. Raibert, editor. International Journal on Robotics Research. MIT Press, 1984. Special issue on robot locomotion. [Raise] M. H. Raibert. Legged Robots that Ballance. MIT Press, 1986. [RP95] Scott K. Ralph and Dinesh K. Pai. Detection and localization of un-modeled manipulator collisions. In IEEE International Conference on Intelligent Robots and Systems (IROS), volume 2, pages 504-509,1995. [RP97] Scott K. Ralph and Dinesh K. Pai. Fault tolerant locomotion for walk-ing robots. In 1997 IEEE International Symposium on Computational Intelligence in Robotics and Automation, Monterey, CA, pages 130-137, Monterey, CA, July 10-11 1997. [RP99] Scott K. Ralph and Dinesh K. Pai. Computing fault tolerant motions for a robot manipulator. In International Conference on Robotics and Automation, pages 111-111, 1999. [Sal83] J. Kenneth Salisbury, Jr. Interpretation of contact geometries from force measurments. In Michael Brady and Richard Paul, editors. Robotics Research: the First International Symposium. MIT Press, Cambridge, MA., 1983. [SH85] G. Sahar and J. Hollerbach. Planning minimum-time trajectories for robot arms. In International Conference on Robotics and Automation, 1985. [SPA99] Raymond J. Spiteri, Dinesh K. Pai, and Uri Ascher. Programming and control of robots by means of differential algebraic inequalities. IEEE Transactions on Robotics and Automation, 1999. To appear. [Ste91] R. F. Stengel. Intelligent fault tolerant control. IEEE Control Systems Magazine, 11 (4): 14-23, June 1991. [Str88] Gilbert Strang. Linear Algebra and its Applications. Harcourt Brace Jovanovich, Orlando, FL., third edition, 1988. [STT94] D. Sreevijayan, Sabri Tosunoglu, and Delbert Tesar. Architectures for fault-tolerant mechanical systems. In Proceedings of Mediterranean Electrotechnical Conference (MALECON) '94, pages 1029-1033, 1994. [SW89] Shin-Min Song and Kenneth J. Waldron. Machines that Walk: The Adaptive Suspension Vehicle. MIT Press, 1989. 177 [TM089] Shinji Takakura, Toshiyuki Murakami, and Kouhei Ohnishi. An ap-proach to collision detection and recovery motion in industrial robot. In Proceedings of the 1989 IEEE lECON, pages 421-426, 1989. [TSM83] R. H. Taylor, P. D. Summers, and J. M. Meyer. Ami: A manufactur-ing language. International Journal of Robotics Research, 1(3):19-41, 1983. [Una83] Unamation INC. User's Guide to Val, 1983. [vdDP94] Kees van den Doel and Dinesh K. Pai. Constructing performance mea-sures for robot manipulators. In Proceedings of the 1994 International Conference on Robotics and Automation, pages 1601-1607, 1994. [Vis94] Monica L. Visinsky. Dynamic Fault Detection and Intelligent Fault Tolerance for Robotics. PhD thesis. Rice University, 1994. [VWC94] M. L. Visinsky, I. D. Walker, and J. R. Cavallaro. New dynamic model-based fault detection thresholds for robot manipulators. In In-ternational Conference on Robotics and Automation, pages 1388-1395. IEEE, 1994. [WDHC91] Eugene Wu, Myron Difler, James Hwang, and J. Chladek. A fault tol-erant joint drive system for the space shuttle remote manipulator sys-tem. In International Conference on Robotics and Automation, pages 2504-2509, April 1991. [Wen78] J. H. Wensley. Sift: Design and analysis of a fault-tolerant computer for aircraft control. Proceedings of the IEEE, 66(10):1240-1255, Oct. 1978. [Yos85] T. Yoshikawa. Manipulability of robotic mechanisms. In International Journal of Robotics Research, volume 4, pages 3-9, 1985. [ZM95] Ying Zhang and Alan K. Mackworth. Hybrid Systems II, chapter Syn-thesis of Hybrid Constraint-Based Controllers, pages 552-567. Num-ber 999 in Lecture Notes in Computer Science. Springer-Verlag, Berlin, 1995. 178 Appendix A Decomposition of TCT Computing the set of vertices V = {vi \Cell{vi) C TCT} , involves determining, for each cell, v^, whether there exists a point (q^t,)GCe^/K), (q\t,)^;^cr. We may first determine which cells are not in V by testing a number of points (q',ti) in each cell. If any point (q',ii) ^ TCT, then Vi is classified as invalid. For a rectangular decomposition we may test some or all of the corners of the rectangeloid of the cell's boundary. At this point we may do one of two things. First, we may classify as valid all cells which are not shown to be invalid using the above test, and rely on a separate verification phase where we ensure that recovery motions are feasible. Secondly, we may more accurately classify the cells by checking to see which of the cells the 179 boundary of TCT intersects. This process is described next. The boundary of TCT is a surface that is formed by combining portions of constraint function surfaces. These surfaces are of the form K = 0. (A.l) Determining whether the constraint surface of Eq. A.l passes through a given cell Vk requires solving a constrained optimization to find the root of hi in the cell's interior. While more sophisticated methods exist (see [PTTF92] for a survey), we utilized a simple gradient ascent method to find the root. If the constraint surface intersects the boundary of the cell we may still not infer that the cell Vk is invalid since the constraint surface need not correspond to a portion of the TCT boundary. Let H{k) denote the set of constraint functions for which intersect Cell{vkys boundary, H{k) = {hij eG\3q e CeU{vk), such that hij{q) = 0} . (A.2) Any cell v^ for which H{k) = 0 is obviously a valid cell. To determine which of the cells are valid that have one or more constraint functions intersect them requires that we look closer at the constraint surface. Suppose that g* = (q',ti) is a root of the constraint function hi, hi{q') = 0. (A.3) We can test to see if the point g* corresponds to a TCT boundary by com-180 puting the direction of the surface normal 57(9') G l""*"^ , V^{q') = ^K{q'), (A.4) and finding a nearby point q^ q^ = q' + cfiM). (A.6) for some small e > 0. If q^ ^ J^CT then q^ is a boundary point, and the cell is classified as invalid. If (p G TCT then h may still form part of the valid-space boundary, but not at the point q^. If hi does form part of the boundary in Cell{vk), then there must exist a point q' which lies at the intersection of the hi = 0 surface, and another constraint surface hj = 0, hj G H{k). This is depicted in Fig. A.l. To find q' we must follow the hi = 0 surface to find the point where hj = 0. At q' we may again test to see if it is a boundary point using a test similar to Eq. A.6. 181 hj = 0 hi = 0 Tcr boundary Figure A.l: Computing whether a constraint surface forms part of the TCT bound-ary within a given cell. 182 Appendix B Computing the Optimal Recovery Motion for a Fault The following two algorithms are used to compute the optimal recovery motions for a fault. Section B.l describes the algorithm for computing the recovery motion for a single fault, with a single-source - the vertex in which the fault occurred. Given a restricted set of vertices corresponding to a fault scenario, the recovery motions can be computed for a set of source vertices simultaneously using the algorithm given in Section B.2. 183 B.l Computing the Recovery Motion for a Single Source Vertex The following algorithm finds the optimal recovery motion using a breadth-first search [CLR90]. During the execution we will construct an array 7r[i], which gives the vertex adjacent to Vi which is used in the recovery motion. Thus the recovery path for vertex Vi is given by: [vi,v^[il,v^[^li]],---,Vk] , where 7r[/c] == 0. The end of the recovery motion is denoted by 7r[A;] = 0. Algorithm B. l . Computing a recovery motion, Prm{vi,u>) for a vertex Vk-RecoveryMotion(Set F of Vertex , Int A''^ , Vertex Vk) /* Given a set of vertices F = ROD(a;), with Np = \F\, * compute the recovery motion for the fault u from vertex Vk V Var visited : Array [1 • • • Np] of Boolean TT : Array [1 • • • Np] of Vertex ; Q : FIFO Queue; 1. For i = 1 to Np Do /* initialization */ Let visited[i] = False ; 2. Let Ej - {cij € E \vi e F and Vj e F]; 3. Let Tx[k] = 0; Let visited[A;] = True ; 4. AddToFiFoQueue(g,Wfc); 5. While (-.EmptyQueue(Q)) Do 6. Let Vi = removeFromQueue(Q) 7. For Each e^j e Ep Do 184 8. If ^visited [j] Then 9. Let visited[j] = True ; Let 7r[j] = i; addToFiloQueue(t;j, Q) /* Find the largest utility of those visited /* 10. Let Vm be largest utility vertex Vj with visited[j] = True ; /* Reverse the linked list {vm, u^rH, • • •, ^ fc} */ 11. Return ReverseLinkedList(m, TT); D The While loop of line 5 builds up a linked-list of vertices giving the path back to the source vertex Vk- Upon termination of the While loop we find the largest utility vertex, Vm, and reverse the path back to Vk to get the optimum utility recovery motion from vertex v^. We assume that the procedure to reverse the linked list in line 11 modifies the array 7r[]. B.2 Computing Recovery Motions for Multiple Source Vertices Algorithm B.2. Computing Multiple-Source Recovery Motions for a fault u AllRecoveryMotion(Set F of Vertex , Int Np) /* Given a fault LO for which F = ROD(a;), and |F | = Np, compute * the recovery motions for all Vi E F simultaneously. Store the * path in an array n[i]. V Var srt : Array [1 • • • Np] of Vertex ; path Jen : Array [1 • • • Np] of Int ; pathjutil : Array [1 • • • Np] of Real ; 185 n : Array [1 • • • Np] of Vertex ; 1. Sort Vi G Fin decreasing util(wi) order and store in srtW; 2. /* Initialize recovery-paths to be null */ For z = 1 to NF D O Let pathjutil[i] = util(t;i); Let pathJen[i] = 0; Let 7r[z] = 0 /* Null-path */ 3. For A; = 1 to A^ ^ Do /* Over vertices */ 4. Let i — srt[k]; /* Vi is the largest utility not yet considered */ 5. For Each j such that Vj e F and ej^i e E Do /* Over edges Cj^i */ 6. If {path-util[i] > path-uUl[j]) or {{pathjutil[i] = path-util[j]) and {pathJen[j] > {pathJen[i] + 1))) Then 7. Let 7r[j] = z; /* Using edge e^ j^ is better */ Let pathJen[j] = pathJen[i] + 1; Let pathjutil[j] — path-util[i]; 8. Return 7r[]; D The algorithm proceeds as follows. First the vertices are sorted in descending order taking 0(A^irlog2 A^F) steps (line 1). Line 2 initializes the paths to be the empty path. At all times during the execution of the the For loop in line 3, 7r[] stores the best known utility paths of all edges considered thus far. Line 4 then considers each vertex Vi and each corresponding edge Cj^i in decreasing order of utility. 186 Appendix C Algorithms for Computing the Most Fault Tolerant Trajectory The following is a description of the algorithm for computing the sorted-minimum fault tolerant paths. The path comparison operator > is described in Section C.l. Fault tolerant paths are constructed using the algorithm in Section C.2. We give a proof of correctness of the algorithm in Section C.3. C.l Algorithm for Sorted-Minimum Path Com-parison Operator 187 We will assume that during the computation of the optimal paths, the array min[z] maintains the minimum performance measure perf(wj) which occurs in the currently best-known path from Vi, stored in n[i]. Algorithm C.l. Sorted-Minimum Path Comparison Operator > Var / * Global Variables */ Array mm[l • • • MAX-VERT] of Real ; /* min perf(x;i) */ path from vi Array 7r[l • • -MAXJVERT] of Vertex ; / * the current best-known paths */ Boolean path_comparison(Int i, Int j) /* Given two paths path p - {vi, z;^ [i], u [^^ [i]], • • •} and p' = {vj,v„[j], v^i^y]], • • •}, Var Array perfJistl[l • --MAX-PLEN] of Real ; /* perfQ along p */ Array perfJist2[l • ••MAX.PLEN] of Real ; /* perfQ along p' */ 1. If (min[i] < min[j]) Then Return False ; /* Distinct minimums */ 2. If (min[z] > min[j]) Then Return True ; /* Distinct minimums */ /* We have identical minimum performance measures along p and p', 3. Extract path p = {vi, v^^ii], f7r[7r[i]]}, placing perf{vj) in perfJistl[l, • • •, ni], let Ui be the path length. 4. Extract path p' = {wj, t'Tr^ ], W7r[7r[;]]}, placing perf(ui) in perfJist2[l, • • •, 7^ 2], let 712 be the path length. 5. Sort(perfJistl,7Zi); Sort(perfJist2, 722); 188 6. For k = 1 to min(ni,n2) Do If (perfJistl[A;] ^ perfJist2[A;]) Then Return (perfJistl[A;] > perfJist2[A;]); 7. Return {rii < 77.2);' C.2 Computing Sorted Minimum Paths Algorithm C.2. Optimal Sorted-Miniumum Path SortedMinimumPaths(V, T r^c, Vdst, E) /* Given a set of vertices, V, and edges, E, from graph of the * valid space TCT, and a measure of the performance, perf{vi) at * each vertex, compute the optimal Sorted-Minimum paths for each * source vertex Vg G Vgrc, to a destination vertex v^ 6 Vdst- */ /* Local Variables */ Var PriorityQueue Q; /* P.Q. sorted by > on paths of 7r[]. */ / * Initialize all of the null-paths to from each destination vertex */ 1. For Each w^ e F Do If Wj G ^dst Then Let 7r[i] = 0; AddToPriorityQueue((5, Wi, 7r[]); Else Let 7r[z] = _L / * Undefined path */ 2. Let 5 = 0 2. While (-EmptyPQ(Q)) Do 189 /* Get largest > path from Q */ 3. Let Vi = RemoveMaxPQ(Q); 4. Let S = S U {vi}; /* Path from vi is known optimal */ 5. For Each Vj such that Cj^i e E Do 6. Relax(?,j); Procedure Relax(Int i, Int j) r update the best known path from Vj. */ 7. Let p be the path {vj, w,r[j], • • •, 'Wfc} 8. Let p' be the path {wj, w,, U;r[i], • • •, ^^ m} 9. \i p'>p Then Let 7r[j] = i\ Let min[j] = min(perf(?;j),min[i]); / * Update path min. */ 10. If 7r[j] = ± Then 11. AddToPriorityQueue(Q,'yj,7r[]) / * New path - add */ Else 12. ReHeapify((5, j ) ; / * -/Vew path from 7r[j] may * disturb ordering */ We denote the undefined path by setting 7r[i] = ± , and assume that any path is ranked higher (>) than any undefined path. The edge relaxation procedure takes a known optimal path from Vi, and updates the currently best known paths by considering the addition of the edge BJ^I. At line 6 of the algorithm, we check 190 to see if the currently best known path from Vj can be improved by using the path through the vertex Vi, obtained by prefixing the edge Cj^i to the path from Vi. This is illustrated in Fig. C.l. Currently best known path from V Considered Optimal path Edge e,-, f'^ O"^ ^ ^ Figure C.l: Edge Relaxation on edge Cj^i The re-ordering operation given by "ReHeapify((5, i ) " of line 12 ensures that the heap maintains the property that each element of the heap is at least as large as each of its siblings. The re-ordering proceeds by swapping vertices in the heap with its parent, until the one storing the path Vj is at its proper location. Maintaining the heap structure is crucial for the efficient and correct execution of the algorithm. 191 c .3 Proof of Correctness of Sorted-Minimum Path Algorithm Before proving the correctness of the algorithm, we must prove some properties of optimal Sorted-Minimum paths. We will use the predicate optimal{p), to denote that a path p is optimal. Lemma C. l . Given a path and a suffix path r then: P = {Pi,P2,---,Pn}, Pi^V, r = {P2r--,Pn} optimal{p) =^ optimal{r). Proof: It suffices to show that -^optimal{r) =^ -^optimal{p). -'optimal{r) =^ 3a path s — {p2, • • •, Pn} ¥" P-, with s > r. 192 Since the addition of a common vertex pi to two paths cannot alter the Sorted-Minimum path ordering, we can construct a path t which i ^ {Pl,P2,---,Pn} > {Pl,P2,---,Pn} =P V ' ^ V ' s r => -'optimal{p). a Lemma C.2. Given a path p = {pi,P2, • • • ,Pn},Pi ^ V, then for all paths p' which are suffixes oip, VI < J < ( n — 1) optimal(p) => optimal(p'). Proof: Trivially by induction on the j using Lemma C.l. • Lemma C.2 shows that our Sorted-Minimum paths will exhibit the "principle of optimality" which is common for multistage decision problems of optimal control [BDG71]. Theorem C . l . Algorithm C.2 produces the set of optimal paths from each vertex in V to a vertex in Vjst-Proof: The proof of correctness of the algorithm is performed by proving that at all points of the execution of the algorithm, the paths stored in 7r[i] for all Vi E S are optimal. 193 By induction on 5, since a new vertex Vi is added at each iteration of the while loop, upon termination the optimal paths for all vertices in V are stored in 7r[]. The proof is similar in flavor to the proof of correctness of Dijkstra's algorithm given in [CLR90]. Base Case: 5 = 0. Trivially, the set of paths from vertices in S are optimal. Inductive Step Assume that the paths generated by the algorithm for all vertices Vi E S are optimal. We must demonstrate that the execution of lines 3-6 of the While loop will result in an optimal sorted-minimum path for vertex Vi, and thus optimal paths for {S U {vi}) are generated. The algorithm proceeds by finding a new edge ej^i crossing the boundary of S {i.e. Vj 0 S and Vi G S). as depicted in Fig. C.2. To prove that the edge chosen by the algorithm results in a new optimal path, we will assume that the path constructed by adding the edge Cj^i is not optimal, and show that this leads to a contradiction. Assume that the path produced by Algm. C.2 is given by p, and the optimal path is given by p' where p = {vi,Vj,VTr[j],v.^[^[j]],---,Vn}, a n d P' = {Vj',---,Vk,Vi,---Vm}-194 We will assume that / 0 p >p, (C.l) and show that this leads to a contradiction. Figure C.2: Proof of correctness of Algm. C.2. The optimal paths for vertices in S have been computed, to which we add the vertex Vi. We know that Vji ^ S since if it were it would have been selected as the next highest sorted-minimum path in the priority queue. Since the vertices in Vdst are the first to be included into S, we know that v^ € 5. Therefore there must exist an edge ek,i which crosses the boundary of 5 with Vk ^ S and vi E S. By the properties of > it is obvious that {Vk,Vi,---,Vm} > {Vj'l,--- ,Vk,Vi,--- ,Vm}, (C.2) Since the priority queue selects a vertex not in S with greatest Sorted-Minimum performance, we know that p= {Vi,Vj,---,Vn} > {Vk,Vi,---,Vm} > P- (C.3) 195 Since Eq. C.3 contradicts our assumption of Eq. C.l, p > p' and p is optimal. Since the algorithm produces the optimal path for Vi, the paths for (5 U {vi}) are optimal. At the termination it is apparent that S = V, and therefore the set of optimal Sorted-Minimum paths will be stored in 7r[]. n 196
- Library Home /
- Search Collections /
- Open Collections /
- Browse Collections /
- UBC Theses and Dissertations /
- A constraint-based approach for computing fault tolerant...
Open Collections
UBC Theses and Dissertations
Featured Collection
UBC Theses and Dissertations
A constraint-based approach for computing fault tolerant robot programs Ralph, Scott 1999
pdf
Page Metadata
Item Metadata
Title | A constraint-based approach for computing fault tolerant robot programs |
Creator |
Ralph, Scott |
Date Issued | 1999 |
Description | We develop a new framework, based on the Least Constraint, for programming robots to perform a task using a fault tolerant trajectory. We take a specification of the task, expressed as a set of constraints on the robot's configuration over time, and produce a fault tolerant trajectory. The methodology encourages fault tolerant behavior at two levels: first at the task-design phase by encouraging the designer to omit extraneous constraints which reduce the potential for fault tolerant operation, and secondly, at the trajectory generation phase by avoiding critical configurations. The critical configurations are identified via a measure of fault tolerance which is global in nature. The dual optimization of fault tolerance at both the high-level design phase as well as the low-level recoverymotion generation phase allows more of the inherent fault tolerance of the robot to be exploited. We believe that combining these two processes into a single formalism is unique and beneficial. The methodology is unique in its ability to deal with robots which are not kinematically redundant with respect to arbitrary task, but which are sufficiently redundant with respect to the particular task constraints to allow the task to be described as a set of "loose" constraints over time. The constraint-based approach allows us to model faults as additional constraints to the specification, thereby allowing an efficient means of computing the effect a fault will have on the ability to complete the task, using the reduced configuration space of the robot. Faults not previously considered, such as the inclusion of additional obstacles, as well as dynamic information arising from sensors, can also be included using this formalism. An efficient algorithm for constructing a recovery motion for a fault has been developed. A specific example of a seldom considered fault, the collision of the robot with an unknown obstacle, is presented. We show that in addition to detecting the event, we are also able to recover the collision geometry. This information can then be used in a more intelligent recovery motion selection. We have developed a new global fault tolerance measure called longevity. The fault tolerance measure examines a set of faults which may occur at a given configuration, and based on the optimal recovery motions for the given fault, ranks the configuration in its ability to satisfy the future task requirements. Using this fault tolerance measure, a trajectory which maximizes the worst-case failure mode of the robot is computed. A number of experiments show the applicability of the method to a number of domains. We analyze the resulting trajectories with respect to their ability to sustain a fault, and we compare them to more traditional methods for accomplishing the same task. We demonstrate that trajectories obtained using the least constraint specification and the fault tolerance measure are able to achieve a much larger degree of fault tolerance than naive methods for the same task. The fault tolerant trajectories make optimal use of the 1-fault tolerant configuration space, and maximize the worst-case utility of the trajectory given a fault. |
Extent | 10153695 bytes |
Genre |
Thesis/Dissertation |
Type |
Text |
FileFormat | application/pdf |
Language | eng |
Date Available | 2009-08-07 |
Provider | Vancouver : University of British Columbia Library |
Rights | For non-commercial purposes only, such as research, private study and education. Additional conditions apply, see Terms of Use https://open.library.ubc.ca/terms_of_use. |
DOI | 10.14288/1.0051674 |
URI | http://hdl.handle.net/2429/11987 |
Degree |
Doctor of Philosophy - PhD |
Program |
Computer Science |
Affiliation |
Science, Faculty of Computer Science, Department of |
Degree Grantor | University of British Columbia |
GraduationDate | 1999-11 |
Campus |
UBCV |
Scholarly Level | Graduate |
AggregatedSourceRepository | DSpace |
Download
- Media
- 831-ubc_1999-464083.pdf [ 9.68MB ]
- Metadata
- JSON: 831-1.0051674.json
- JSON-LD: 831-1.0051674-ld.json
- RDF/XML (Pretty): 831-1.0051674-rdf.xml
- RDF/JSON: 831-1.0051674-rdf.json
- Turtle: 831-1.0051674-turtle.txt
- N-Triples: 831-1.0051674-rdf-ntriples.txt
- Original Record: 831-1.0051674-source.json
- Full Text
- 831-1.0051674-fulltext.txt
- Citation
- 831-1.0051674.ris
Full Text
Cite
Citation Scheme:
Usage Statistics
Share
Embed
Customize your widget with the following options, then copy and paste the code below into the HTML
of your page to embed this item in your website.
<div id="ubcOpenCollectionsWidgetDisplay">
<script id="ubcOpenCollectionsWidget"
src="{[{embed.src}]}"
data-item="{[{embed.item}]}"
data-collection="{[{embed.collection}]}"
data-metadata="{[{embed.showMetadata}]}"
data-width="{[{embed.width}]}"
async >
</script>
</div>
Our image viewer uses the IIIF 2.0 standard.
To load this item in other compatible viewers, use this url:
https://iiif.library.ubc.ca/presentation/dsp.831.1-0051674/manifest