AN APPROACH TO MOTION PLANNING AND MOTION CONTROL OF TWO ROBOTS IN A COMMON WORKSPACE by Bum Hee Lee A dissertation submitted in partial fulfillment of the requirements for the degree of Doctor of Philosophy (Computer, Information, and Control Engineering) in The University of Michigan 1985 Doctoral Committee: Assistant Professor C. S. George Lee, Chairman Professor Elmer G. Gilbert Professor Richard A. Volz Professor Kensall D. Wise Associate Professor Anthony C. Woo

ABSTRACT AN APPROACH TO MOTION PLANNING AND MOTION CONTROL OF TWO ROBOTS IN A COMMON WORKSPACE by Bum Hee Lee Chairman: C. S. George Lee This thesis focuses on motion planning and motion control for two robots in a common workspace, which involves the investigation and development of an off-line discrete-time trajectory planning scheme of straight line paths in Cartesian space; off-line collision-free path planning algorithms for two robots; and an on-line resolved motion adaptive perturbation control method. A discrete-time trajectory planning scheme for a straight line path has been developed to determine the trajectory set points in Cartesian coordinates which satisfy both smoothness and torque constraints. An iterative forward and backward search algorithm coupled with a modified forward search algorithm is developed to determine the joint values at each servo interval on the given straight line path. Relaxation points are needed in planning the trajectory to meet the boundary conditions at the final location of the straight line path. The resultant straight line trajectory is composed of three segments: an acceleration, a deceleration, and a relaxation portion. Using a sphere model for the wrist of a manipulator, planning collision-free paths of two time-scheduled robots is considered. For planning a collision-free path 1

of a moving robot in the presence of a stationary robot, several performance measures are considered, from which two performance measures are selected and used for planning a collision-free path. For planning collision-free paths of two moving robots, various collision situations are identified. Notions of a collision map and time scheduling are introduced, which are used to develop two procedures for planning collision-free paths of the two moving robots. An adaptive control in Cartesian space, which adopts the idea of resolved motion rate and acceleration control, has been developed to track the collision-free trajectory faithfully over a wide range of manipulator motion and payloads. The control scheme is based on the linearized perturbation equations along a desired hand trajectory. The control system is characterized by feedforward and feedback components which can be computed separately and simultaneously. A feasibility study of implementing the adaptive control for a six-joint PUMA manipulator is explored using present-day low-cost microprocessors. Finally, the proposed straight line trajectory planning scheme is simulated on a VAX-11/780 computer to evaluate its performance. 2

To My Parents, Wife, and Children: Yong Jei and Yong Sun 1ii

ACKNOWLEDGMENTS It is a pleasure to express my appreciation to a number of people for their help during my graduate study. I am most grateful to my advisor, Professor C. S. George Lee, for his constant guidance and encouragement without which this dissertation would not have been completed; His kindness, patience and invaluable ideas are so helpful that it has been a delight to work with him throughout my entire graduate study. My appreciation goes to the members of my Dissertation Committee, Professor Gilbert, Professor Volz, Professor Wise, and Professor Woo for the time and efforts they have spent in reading, commenting on, and enhancing the quality of this dissertation. Especially, Professor Gilbert and Professor Volz have patiently reviewed and improved many revisions of this work. Finally, to my parents and wife, I wish to express my thanks for their endless love and encouragement during my graduate study. During the last two years of this work, it was partially supported by the National Science Foundation Grant ECS-8106954 and the Robot Systems Division in the Center for Research in Integrated Manufacturing (CRIM) under the AFOSR Grant F49620-82-C-0089.

TABLE OF CONTENTS DEDICATION..........................................................ii ACKNOWLEDGEMENTS................................................................................ iii LIST OF FIGURES.................... vii LIST OF TABLES...........-...........I.....0......................... xi LIST OF APPENDICES........................................................................... xii CHAPTER 1. INTRODUCTION 1.1 Introduction..........1.................. 1 1.2 The Problem and the Approach.............................................. 1.3 Literature Survey................... 7 1.4 Contribution and Organization of the Thesis.14 2. PROBLEM FORMULATION 2.1 Introduction.............................................................................. 19 2.2 Hierarchical Robot Control Structure.................................. 19 2.3 A Scheme for Straight Line Trajectory Planning.................... 23 2.4 Geometric Modeling of the Wrist of a Robot.......................... 30 2.5 Various Collision Situations for Two Robot System............... 33 2.8 A Cartesian Space Control.................................................. 36 2.7 Summary................................................................................. 40 3. PLANNING OF STRAIGHT LINE MANIPULATOR TRAJECTORY 3.1 Introduction............................................................................... 41 3.2 Formulation for Local Speed Optimization............................. 41 3.3 Search Algorithm............................................... 54 3.3.1 Method to Find Initial Estimated Length Ratio......... 56 3.3.2 The Ratio of Initial Estimated Path Length.............. 58 3.3.3 Forward Search Algorithm....................... 60 iv

3.4 Determination of Break point and Relaxation Points............ 82 3.4.1 The Break Point and The Modified Search Algorithm................................................. 62 3.4.2 Existence of Straight Line Trajectory.......................... 65 3.4.3 Relaxation Points...................................*** 68 3.5 Summary.......................................... 75 4. PLANNING OF COLLISION-FREE MANIPULATOR PATH 4.1 Introduction............................................................................. 78 4.2 Collision-Free Path for the Case of a Stationary Robot......... 77 4.2.1 Overviews and Notations............. 77 4.2.2 Planning of Collision-free Path by Measure (1)........................................ 80 4.2.3 Planning of Collision-free Path by Measure (2)........................................ 88 4.2.4 Comparisons and Discussions.................................. 91 4.3 Time Scheduling and Collision Map..............................9......... 91 4.3.1 Case 1 and Case 2 Collision Situations...................... 92 4.3.2 Time Scheduling of Straight Line Trajectory.............. 93 4.3.3 Collision Map............................ 97 4.4 Collision-Free Path for Two Moving Robots.................. 107 4.4.1 Collision-Free Path Planning for Case (1).................................... 107 4.4.2 Collision-Free Path Planning for Case (2).......................................... 118 4.5 Summary........................................ 129 5. CONTROL OF MANIPULATOR IN CARTESIAN SPACE 5.1 Introduction................................................ 130 5.2 Kinematics of the Manipulator Hand..................................... 131 5.3 Equations of Motion of the Manipulator................................. 133 5.4 Resolved Motion Adaptive Control.................................... 135 5.4.1 Perturbation Equations of Motion.............................. 137 5.4.2 Parameter Identification and Perturbation Control.......................... 140 or

5.5 Computational Complexity of Resolved Motion Adaptive Control........................................ 148 5.6 Summary.................................................................................. 152 6. COMPUTER SIMULATION 8.1 Straight Line Trajectory Planning........................................... 153 8.1.1 Simulation and Results........................................ 153 8.1.2 Sampling Effects on Trajectory Planning.................... 167 8.2 Existence of Straight Line Trajectory...................................... 175 6.3 Trajectory Planning of Connected Straight Line Segments.......................................................................... 188 8.4 Time Scheduling of a Straight Line Trajectory....................... 196 6.5 Summary.................................................................................. 211 7. DISCUSSION 7.1 Summary and Contribution.................................... 212 7.2 Future Research......................... 214 APPENDICES................. 216 BIBLIOGRAPHY............................................................................... 228 vi

LIST OF FIGURES Figure 1.1 Configurations of Robots........................................ 2 2.1 Hierarchical Control Structure of a Robot.................. 21 2.2 Two Cooperative Robot System..................................................... 22 2.3 Joint Interpolated Trajectory........................................ 25 2.4 Cylindrical and Truncated Cone Model................................................. 31 2.5 Sphere Model..................... 32 2.6 Two Colliding Robots......................................... 34 2.7 Hand Coordinate System....................................... 38 3.1 Straight Line Path................................................ 55 3.2 Cartesian Trajectory Planner.................................................................. 74 4.1 Stationary Robot Avoidance............................................. 78 4.2 Tangential Cut Planes................... 83 4.3 Rotation of a Vector................................... 87 4.4 Geometric Analysis for Deviation Error................................................ 90 4.5 Initial Path and Trajectory Planning.................................. 95 4.6 A Collision Map.................................................... 98 4.7 Two Robot Path............................. 100 Vii

4.8 Boundary Points for Collision Region................................................... 102 4.9 Collision Map....................................................... 104 4.10 Several Types of Collision Region....................................................... 108 4.11 A Collision Map with Potential Collisions............................................. 108 4.12 Collision-free Trajectory Curves....................................................... 112 4.13 Collision Map after Time Scheduling..................................................... 115 4.14 Collision Avoidance by Path Modification............................................. 120 4.15 Initial Collision Map............................................................................... 122 4.18 Collision Map with Path Modification................................. 128 5.1 Hand Coordinate System........................................................................ 132 5.2 Resolved Motion Adaptive Control Block Diagram............................... 147 6.1 Joint 1 Trajectory of the Straight Line.......................................... 180 8.2 Joint 2 Trajectory of the Straight Line.................................................. 161 6.3 Joint 3 Trajectory of the Straight Line...................................... 162 8.4 Joint 4 Trajectory of the Straight Line.................................................. 163 6.5 Joint 5 Trajectory of the Straight Line................................... 184 68. Joint 6 Trajectory of the Straight Line........................................ 185 8.7 Traveled Length versus Time Curve....................................... 168 8.8 Sampling Effects on Joint 1 Trajectory................................................. 19 6.9 Sampling Effects on Joint 2 Trajectory......................................... 170 viii

6.10 Sampling Effects on Joint 3 Trajectory....................................... 171 6.11 Sampling Effects on Joint 4 Trajectory..172 6.12 Sampling Effects on Joint 5 Trajectory.. 173 8.13 Sampling Effects on Joint 6 Trajectory.............174 6.14 Straight Line Path for the Procedure ESLT....................................... 177 8.15 Traveled Length versus Servo Time Curve...................................... 179 6.16 Joint 1 Trajectory from the Procedure ESLT........................................ 180 6.17 Joint 2 Trajectory from the Procedure ESLT........................................ 181 8.18 Joint 3 Trajectory from the Procedure ESLT........................................ 182 6.19 Joint 4 Trajectory from the Procedure ESLT....................................... 183 8.20 Joint 5 Trajectory from the Procedure ESLT........................................ 184 6.21 Joint 8 Trajectory from the Procedure ESLT...................................... 185 8.22 Simulated Connected Straight Line Segments....................................... 188 6.23 Joint 1 Trajectory of Connected Straight Line Segments............... 190 8.24 Joint 2 Trajectory of Connected Straight Line Segments................ 191 6.25 Joint 3 Trajectory of Connected Straight Line Segments............... 192 8.26 Joint 4 Trajectory of Connected Straight Line Segments.............. 193 6.27 Joint 5 Trajectory of Connected Straight Line Segments............ 194 6.28 Joint 6 Trajectory of Connected Straight Line Segments............ 195 8.29 Collision Map with Possible Collision.................................................. 200 ix

6.30 Time Scheduled Collision Map............................................................... 201 8.31 Simulated Straight Line Segment........................................................... 202 6.32 Time Scheduled Joint 1 Trajectory........................................................ 204 6.33 Time Scheduled Joint 2 Trajectory...205 8.34 Time Scheduled Joint 3 Trajectory........................................................ 206 8.35 Time Scheduled Joint 4 Trajectory................................... 207 6.36 Time Scheduled Joint 5 Trajectory........................................................ 208 6.37 Time Scheduled Joint 6 Trajectory........................................................ 209 6.38 Comparison of Final Arrival Times............................ 210

LIST OF TABLES Table 5.1 Parallel Computations of the Proposed Adaptive Controller................ 150 5.2 Implementation of Resolved Motion Adaptive Control................. 151 8.1 Data Used in Computer Simulation........................................ 156 8.2 Physical Constraints for Computer Simulation..................................... 157 6.3 Comparisons of Several Break Points...................................... 158 8.4 Effects of Various Break Points on the Final Search Point.............. 159 8.5 Break Point Information from the Procedure ESLT............................. 178 8.8 Connected Straight Line Segment Location Information....................... 189 6.7 Time Scheduled Trajectory Location Information................................. 203 xi

LIST OF APPENDICES Appendix A. A PUMA Robot Arm Configuration........................................ 217 B. Homogeneous Transformation Matrix of a PUMA Robot............ 218 C. Kinematic Equations for Euler Angles.......................................... 220 D. Inverse Kinematic Equations for a PUMA Robot......................... 221 E. Direction Number of a Line.......................................................... 224

CHAPTER 1 INTRODUCTION 1.1. Introduction An industrial robot is a general purpose manipulator which consists of several rigid bodies (or links) connected in series by revolute or prismatic joints (see Figure 1.1 [24]). One end of the chain is attached to a supporting base while the other end is free and attached with a tool to manipulate objects or perform assembly tasks. Mechanically a robot is composed of an arm (or main frame) and a wrist subassembly plus a tool. It is designed to reach a workpiece located within its work volume. The work volume is the sphere of influence such that the arm can deliver the wrist subassembly unit to any point within the sphere. The arm subassembly typically consists of three degrees of freedom movement. The combination of the movements of the arm subassembly will place or position the wrist unit at the workpiece. The wrist subassembly unit usually consists of three rotary motions. The combination of these motions will orient the tool according to the configuration of the object to ease pickup. Hence for a six-joint robot, the arm subassembly is the positioning mechanism, while the wrist subassembly is the orientation mechanism. 1

CARTESIAN OR x-y-z CYLINDRICAL SPHERICAL REVOLUTE Figure 1.1 Configurations of Robots

3 Presently there are many commercially available industrial robots which are widely used in simple material-handling, spot/arc welding, and loading and unloading numerically-controlled machines. These robots exhibit their characteristics in motion and geometry. Mechanically they fall into one of the four basic motiondefining categories according to the motion of the arm subassembly (see Figure 1.1): * Cartesian robot (three linear axes) * Cylindrical robot (two linear and one rotary axes) * Spherical robot (one linear and two rotary axes) * Revolute or Articulated robot (three rotary axes) Most automated manufacturing tasks are done by special purpose machines which are designed to perform their prespecified functions in a manufacturing process. The inflexibility of these machines makes the computer-controlled manipulators more attractive and cost effective in various manufacturing and assembly tasks. These include cost savings, reliability, tolerance of working environments unacceptable to humans, and an adaptability to both structured and unstructured environments through simple reprogramming. The end results are improved productivity, efficiency, and flexibility in industrial manufacturing and automation. Due to the recent development of sophisticated sensors, advanced programming languages, and efficient visual recognition methods, the application domain of robots is being expanded to more sophisticated tasks and in particular assembly tasks [4,55,60]. Undoubtably, more robots will be put to use in assembly tasks in manufacturing cells which, as described by Whitney [81], are small size manufacturing systems in which there are sequences of coordinated operations among various manufacturing devices. These operations require the robot arms to move their end

effectors from one location in the Cartesian space to another in coordinated motion without any collisions with other objects. Current industrial practice employs simple time-space coordination which does not allow more than one robot working in a common workspace. Such coordination and control result in underutilizatioa of robot arm capabilities. Any improvements in the performance of controlling two robot arms in a common workspace will require further investigation of sophisticated straight line trajectory planning, collision avoidance strategies, and Cartesian space control strategies. This initiates and motivates the research efforts that constitute this thesis. 1.2. The Problem and The Approach Undoubtably, the use of two robots in a common workspace will increase productivity. In this system configuration, given the locations of workpieces, the robot arms have to sense the workpieces, manipulate them and transfer the assembled workpieces to designated locations. However, with two robots working in a common workspace, problems are created that were not anticipated in a single stand-alone robot system. While one robot tries to assist the other robot, it may become an "obstacle" to the other robot. Thus, before each robot executes its motion command, it is important for the motion planning system to detect the possibility of potential collisions between the wrists of the robots.1 If the wrist of a robot is found to be on the path of the other robot, then paths must be replanned or the traveling time on the paths must be modified to avoid the potential collisions. Thus, the main task of this e aume in this system configuration that there are no collisions between the rst thre links of the robots.

thesis is to develop motion planning and motion control algorithms and methodolo gies so that two robots 'can work safely in a common workspace. This objective leads naturally to the investigation of the following three separate but coherent areas: (1) Off-line planning of straight line manipulator trajectory (2) Off-line planning of wrist collision-free path, and (3) On-line Cartesian space control for tracking the wrist collision-free straight line trajectory That is, we are concerned with the formalism of describing the desired manipulator motion as a sequence of set points in the Cartesian space (position and orientation of the manipulator hand) through which the manipulator hand must pass in straight line motion, must avoid any possible wrist collisions, and must be controlled with high accuracy. It has been well recognized that optimum control of a manipulator is very difficult due to its inherent nonlinearity and complexity in the equations of motion, in addition to the nonlinear trigonometric function relationship between the joint coordinates and the Cartesian coordinates [24,30,38,461. Thus, optimum control of a manipulator can be conveniently decomposed into an off-line trajectory planning (motion planning) followed by an on-line path tracking control scheme (motion control) [22]. This decomposition reduces the real time computational burden of the optimal controller and transfers the computational burden to the off-line trajectory planning phase. This thesis rests on a similar decomposition concept of manipulator control.

The motion planning phase focuses on the investigation of a straight line trajectory planning scheme in Cartesian space followed by wrist collision detection along the trajectory, and replanning the path or time-scheduling of the original trajectory if necessary. Several advantages exist for designing a straight line trajectory for controlling the manipulator: (1) Visibility concept: If one of the manipulators cannot "see" its destination position, then obstacle(s) exists on the path.2 (2) Motion correctness: It is easier for the user to specify straight line Cartesian motion and the correctness of motion can be verified from the motion of the manipulator hand. (3) Shortest path: The straight line path is the shortest path between two assembly locations even if the required traveling time may not be minimum. (4) Time-varying obstacles: If two robots are not moving in straight line motion, it may be extremely difficult to handle the time-varying environments for mutual wrist collision avoidance. (5) Path Information: After detecting that potential collisions exist on the path, we can utilize the path information of one robot to decide the path or trajectory of the other robot for collision-free motion. Since the location of each robot on the straight line path must be available at all servo instant to detect the existence of potential collisions and to obtain a collision-free path in the two moving robot environment, a servo time based trajectory planner and a collision-free path generator will be the immediate problems to 9The reverse is not true. That is, if the manipulator can see the destination position, it still does not garantee tke path to be colluiion free.

7 be solved. An accurate Cartesian space control scheme must be used to follow the collision-free trajectories faithfully. Each of these areas will be covered in detail in Chapters 3, 4, and 5, respectively. 1.3. Literature Survey In this section, a brief review of previous work on multirobot control systems, trajectory planning, collision avoidance, and adaptive control strategies is presented. There are only a few research institutions working on multirobot control problems. Conventional techniques involve careful off-line trajectory planning such that the robot arms do not collide with known obstacles in fixed locations. This technique involves "interpolation" of intermediate Cartesian knot points that the manipulator hand must travel by a class of polynominal functions. Others are involved in the development of efficient sensory interactive hierarchical control system for the "factory of the future" [2,4,45,54,56,681. At the National Bureau of Standards, a hierarchical control system with sensory feedback at various levels/stages coupled with the CMAC ( Cerebellar Model Articulation Control ) concept [21 has been proposed for manufacturing systems. The focus is mainly on the global control of "loosely-coupled" robots in a manufacturing environment. The two robot system in a common workspace is a "tightlycoupled" system which requires much more intensive computations and coordinations. Although, significant theoretical results about the multiple robot control problems.were not obtained, it was found that, for controlling multiple robots, an exchange of information among various robots in real time is necessary to achieve collision avoidance.

8 Saridis [51] proposed an intelligent control concept for controlling multiple robots in an "optimized" manner using both artificial intelligence and control discipline ideas. This presents a desirable research direction for multiple robot systems. An interesting modular system approach [541 with computer communication network between a supervisory computer and functional module computers is being investigated by researchers at SRI. Their approach is based on sensory feedback signals to actuate control strategies. They employed very simple algorithms to calculate hand positions to avoid potential collisions and emphasized vision feedback signal processing in global coordinate system for two robot arms. Recently, Freund [16,171 exploited table look-up and hierarchical decisions to manage collision avoidance for two robot arms in a common workspace. His approach is based on the hierarchical kinematic decisions for positioning the manipulator links to avoid potential collisions. This technique focuses more on gross motion control of two robot arms than the fine motion control of two robot arms. Although, the above approaches solved the problems in the multiple robot system with various degrees of success, they do not address the problems of the two robot system adequately for the following reasons: (1) most of the approaches are based on kinematic information, (2) most of the obstacles are fixed in location, (3) no collision avoidance is considered for the wrists ( last three links of the robot ) of the two robots. In view of this, the approach employed in this thesis is based on the off-line planning of a straight line path and trajectory with an on-line path tracking control strategy. This involves planning of the straight line trajectory in Cartesian space, appropriate geometric modeling of the wrists which will be used in detecting the potential wrist collisions along the path, replanning the path or time.

9 scheduling of the original trajectories if collisions exist, and finally controlling the robots to track the desired collision-free trajectories faithfully. We shall now review the previous work in the above areas, namely, trajectory planning, obstacle avoidance of robot hand, and Cartesian space control strategies. Trajectory planning involves interpolation and/or approximation of the desired Cartesian path by a class of polynomial functions of degree n or less. For the past several years, there has been a surge of efforts in the planning of manipulator trajectories which concern more sophisticated and efficient movements of the manipulator hand (or tool) along a straight line path or a jointinterpolated smooth trajectory which approximates the given path closely [8,33,34,37,42,48,47]. In general, trajectory planning in the jointvariable space is simpler than that in the Cartesian space because of problems caused by degeneracies and dynamic constraints [8]. The degenerate case happens when a set point on the manipulator hand trajectory in the Cartesian space results in a singular Jacobian matrix for the mappings between joint space and Cartesian space. The dynamic constraints are composed of maximum allowable velocity and acceleration of each joint and torque constraints depending on instantaneous joint position and velocity. In addition to the dynamic constraints, each joint trajectory must maintain a smooth transition which yields velocity and acceleration continuity from one set point to the next set point [8,46]. The trajectory planning of a manipulator path made up of straight line segments was discussed by Paul [481, using a homogeneous transformation matrix to represent target positions for the manipulator hand to traverse. Movement between two consecutive target positions is accomplished by two sequential operations: a

10 translation and a rotation to align the approach vector of the manipulator hand and a final rotation about the tool axis to align the gripper orientation. A quadratic polynomial interpolation routine in the joint-variable space is then used to guarantee smooth transition between two connected path segments. Taylor [581 improved Paul's technique by using quaternions to represent the rotational operation. He also developed a bounded deviation joint control scheme which involved selecting more intermediate interpolation points when the joint polynomial approximation deviates too much from the desired straight line path. In order to achieve a real-time trajectory planning objective, both approaches neglect the physical manipulator dynamics. Other existing trajectory planning schemes satisfy the continuity and the torque constraints simultaneously. To assure the torque constraint in the trajectory planning stage, it is assumed that the maximum allowable torque is known and independent of position and velocity. For example, instead of using a varying torque constraint, Lin et al. [341 use velocity, acceleration, and jerks bounds which are assumed constant for each joint. They select several knot points on the desired Cartesian path, solve the inverse kinematics and obtain set points in joint space, and find cubic splines for the joint variables which guarantee continuity conditions and a fit through the set points in the joint-variable space. Then, by relaxing the normalized time to real time, the dynamic constraints on velocity, acceleration, and jerk are included. However, due to the joint interpolation function, the exact location of the manipulator hand at every servo instant is not exactly on the desired path, but on a 3jerk is defined as the rate of change of acceleration.

11 related smooth function. This results in inaccurate straight line trajectory planning. Recently, Bobrow, et al. [7] and Shin, et al. 153] utilized a parameterized joint interpolation function to find the optimal trajectory on a given geometric path. Here we follow a similar approach but focus on very accurate implementation of a straight line trajectory which allows the detection of potential wrist collisions along the trajectory. This constitutes a part of the thesis and will be discussed in detail in Chapter 3. Most of the existing off-line path planning schemes which concern obstacle avoidance concern the problem of avoiding fixed and stationary obstacles in a workspace. Ahuja, et al. [11 discussed two methods for detecting intersections of three dimensional objects that are based on the projection checking and the octree traversing of the stationary obstacles. Chien, et al.[11] adopted the concepts of state space and developed rotation mapping graph to describe the relationship between the positions and the corresponding collision-free orientations of a robot among obstacles. To simplify the collision avoidance problem, Udupa [59] used a polyhedra model for the obstacles and minimum bounding cylinders in detecting the interference and avoiding collisions among three dimensional stationary objects. He discretized the three-dimensional space into sectoroids and pascs which were designated free if not occupied by obstacles and robots. A collision-free path is then obtained as lists of free pascs joined together. Lozano-Perez 135,36] and Brooks 19,10] extended the polyhedra models and obtained linear intersection functions for collision avoidance. Their schemes require special planning for the orientation problem.

12 Luh et al. [39-41] used the concept of pseudo-obstacles by expanding the real obstacles' edges and faces and letting the robot be viewed as a point location in space. They formulated the forbidden region for the Stanford arm as the union set of polygonal obstacles and pseudo-obstacles. An algorithm is developed to determine the shortest collision-free path for stationary obstacles givena a sequence of edges to be traversed. Pennington et al. [49] employed a geometric modeling approach to model a PUMA robot as a sphere model, where the collision is detected based on the swept volume of the robot. However, the collision detection was found quite involved in its mathematical computation. Petrov[501 proposed a learning algorithm to avoid the obstacles by considering the obstacles in joint space and in Cartesian space simultaneously, and showed a hybrid computer implementation of the algorithm for the collision avoidance. Recently, Gilbert et al. [119] described a procedure where a collision-free optimal path for a robot is obtained by solving an optimal control problem where the distances between the potentially colliding parts of the robot and the obstacles impose a state space constraint. The state constraints and actuator constraints are handled numerically by a penalty function approach. All the above collision avoidance schemes emphasize a single robot with a fixed environment of stationary obstacles and achieved various degrees of success. In this research, our concerns focus mainly on the wrist collision avoidance of the two moving robots in a common workspace. Given the wrist collision-free path and trajectory, the purpose of manipulator control is to control the manipulator hand to track the desired trajectory as closely

13 as possible under various payload conditions. Most of the existing control schemes [5,13,21-23,25,28,44,57,651 control the arm at the joint level and emphasize nonlinear compensations of the interaction forces among the various joints. For most applications, resolved motion control, which commands the manipulator hand to move in a desired Cartesian direction in a coordinated position and rate control, may be more appropriate. The resolved motion control algorithms [18,43,62-641 control the arm at the hand level with or without external sensory feedback information. Whitney developed a resolved motion rate control 1621 in which motions of various joint motors are combined and resolved into separately controllable hand motions along the world coordinate axes. His method indicates that several joint motors must run simultaneously at different time-varying rates in order to achieve desired coordinated hand motion along the world coordinate axes. The method enables the user to specify the direction and speed along any arbitrarily oriented path. This motion control greatly simplifies the specification of the sequence of motion for completing a task because users are usually more adapted to the Cartesian coordinate system than to the joint coordinate system. Luh et al. [431 extended the concept of resolved motion rate control to include acceleration control. It presents an alternative position control which deals directly with the position and orientation of the manipulator hand, and it assumes that the desired accelerations of a preplanned hand motion are specified by the user. The paper by Gilbert et al. [18] can be thought of as a generalization of this ideas. The resolved motion control algorithms discussed may be inadequate for accurate path tracking control purposes because they neglect the changes of the load in

14 a task cycle. They may result in reduced servo response speed and damping, and limiting the precision and speed of the end-effector. As a result, the manipulator hand may deviate too much from the specified collision-free path, causing unexpected wrist collisions. A significant performance gain in this can be considered by adaptive control techniques in Cartesian space [28]. Recently, various adaptive control algorithms [3,13,21,23,25] have been proposed. Dubowsky et al. [131 proposed a model referenced adaptive control which uses a linear second-order time invariant differential equation as the referenced model for each degree of freedom of the robot arm. The manipulator is controlled by adjusting the position and velocity feedback gains to follow the model. A steepest descent method is used to update the feedback gains. Koivo et al. [23] proposed an adaptive self-tuning controller using an autoregressive model to fit the input-output data from the manipulator. Both control algorithms assume that the interaction forces among the joints are negligible. Lee et al. [25] proposed an adaptive perturbation control with feedforward compensation for robot manipulators in the joint-variable space. A part of this research is devoted to extend the adaptive perturbation control to the Cartesian space so that the manipulator hand can be controlled to follow the desired collision-free trajectory faithfully in the Cartesian space. 1.4. Contribution and Organization of the Thesis This thesis focuses on motion planning and motion control for two robots so that they can work safely in a common workspace. It involves the investigation of an off-line straight line trajectory planning scheme in Cartesian space, off-line collision-free path planning algorithms for two robots, and an on-line adaptive

15 perturbation control method in Cartesian space. Research results from these three areas constitute the major contribution of the thesis. They are now described briefly. A discrete time trajectory planning scheme for a given straight line path has been developed to determine the trajectory set points which satisfy both smoothness and torque constraints. A real time servo interval instead of a normalized time interval is used for planning all the joint trajectories. The planning of the straight line trajectory is performed in the Cartesian space and requires maximization of the speed between two consecutive Cartesian set points on the straight line path. An iterative forward and backward search algorithm coupled with a modified forward search algorithm is developed to determine the joint values at each servo interval such that they are within the bounds imposed by the smoothness and torque constraints and satisfy the straight line requirement with high accuracy. The resultant straight line trajectory is composed of three segments: an acceleration portion, a deceleration portion, and a relaxation portion. The control set points on the acceleration and deceleration portions of the trajectory satisfy the straight line requirement while those on the relaxation portion do not. These sets of joint position, velocity and acceleration can be used as reference inputs to the manipulator controller for the joint-variable space control [25,261 or can be used to determine the corresponding Cartesian position, velocity, and acceleration for the proposed Cartesian space adaptive control [28]. After obtaining the Cartesian trajectory set points from the trajectory planner, the potential wrist collisions between two robots can be detected easily by using a sphere model to represent the manipulator wrist. The detection of the potential

16 wrist collisions can be done by checking the distance between the center of the two spheres with respect to a global coordinate system. A collision-free path of a moving robot in the presence of a stationary robot is found from connected straight line segments selected subject to a performance measure. Various collision situations for two moving robots are identified. Notions of collision map, which provides the location and corresponding servo time information of the robots simultaneously, and time scheduling are introduced and utilized. Collision-free paths of two moving robots are obtained by considering the paths and trajectories of the two robots. When the path modification is not allowed for collision avoidance, rescheduling the traveling time on the existing path is performed to realize collisionfree motion planning. When the path modification is allowed for collision avoidance, a collision-free path is obtained subject to a performance measure. An adaptive control in Cartesian space, which adopts the ideas of resolved motion rate control and resolved motion acceleration control, is proposed to control the manipulator hand to track the collision-free trajectory as closely as possible. Equations of motion of the manipulator hand are developed in Cartesian coordinates from which an adaptive control scheme is derived. The adaptive control scheme is based on the linearized perturbation equations along a desired hand trajectory. The controlled system is characterized by feedforward and feedback components which can be computed separately and simultaneously. The feedforward component computes the nominal torques from the Newton-Euler equations of motion using the joint trajectory information from the trajectory planner. The feedback component, consisting of a recursive least square identification scheme and an optimal adaptive self-tuning controller for the linearized system, computes the

17 perturbation torques which reduce the manipulator hand position and velocity errors along the nominal hand trajectory. A feasibility study of implementing the adaptive control for a six-joint PUMA manipulator is explored using present-day low-cost microprocessors. This thesis is organized in seven chapters. The problem of motion planning and motion control of two robots in a common worlkspace is formulated in three separate but coherent subproblems: trajectory planning, wrist modeling and collision-free path planning, and adaptive control in the Cartesian space. The hierarchical robot arm control structure is briefly discssed in Chapter 2 together with a discussion and problem formulation for each of the three areas of the proposed research. In Chapter 3, the trajectory planning scheme in discrete time is presented for a straight line path. The objective of the trajectory planner is to obtain the control set points which are exactly on the specified straight line path and within the smoothness and torque constraints of the manipulator. In Chapter 4, planning of a collision-free path is discussed for the two robot system. A collision-free path is found for a moving robot in the presence of a stationary robot, which consists of connected straight line segments selected subject to a performance measure. Various collision situations are identified and discussed for the case of two moving robots. Notions of collision map and time scheduling are introduced and two procedures are developed to obtain collision-free paths for the two moving robots. In Chapter 5, an adaptive perturbation control is developed in Cartesian space to control the manipulator to track a collision-free trajectory faithfully. Equations

18 of motion of the manipulator hand are developed in Cartesian coordinates. A least square identification scheme together with an optimal adaptive self-tuning control is discussed. Computational complexity of the proposed adaptive perturbation control scheme is tabulated for future implementation. In Chapter 8, computer simulation results of the proposed straight line trajectory planning scheme are presented. Sampling effects and the existence of a straight line trajectory are investigated. Also, the time scheduling of a straight line trajectory is performed and utilized in the collision map to obtain a collision-free trajectory for the case of two moving robots. In Chapter 7, conclusions and summaries are presented. Extensions with future research issues are also discussed.

CHAPTER 2 PROBLEM FORMULATION 2.1. Introduction This chapter addresses problem formulation for motion planning and motion control of a two robot system. The motion planning and motion control are investigated by focusing on three separate but coherent areas: planning of a straight line trajectory in Cartesian space, planning of a collision-free path of two robots, and manipulator control in Cartesian space. The functional operation and characteristics of the hierarchical robot control structure are discussed in Section 2.2. Planning of a straight line trajectory in Cartesian space is discussed in Section 2.3. Several geometric models for the manipulator wrist used in planning a collision-free path are discussed with their advantages and disadvantages in Section 2.4. In Section 2.5, various collision situations are identified and discussed to obtain collision-free paths of the two robots. A Cartesian space control method is proposed in Section 2.6. Finally, summaries are presented in Section 2.7. 2.2. Hierarchical Robot Control Structure In this section, the overall control structure of the robot system is discussed within the context of this thesis. In general, a task is performed in three phases of 19

20 execution: task planning, motion planning, and motion control of the manipulator. [55] By the nature of the robot system, the overall control structure can be arranged hierarchically into three levels as shown in Figure 2.1. The highest system level performs task planning. The task planning system is composed of a human operator and a task planner. The human operator interacts with the task planner and initiates the task specification for the robot system to accomplish. The task planner decomposes the given task into sequences of operations for the robot to implement. The intermediate system level consists of path planning and trajectory planning. The path planning system receives information about the locations of the various workpieces from the task planning system and generates the Cartesian path to be followed by the manipulator to accomplish the specified task. Then the trajectory planning system generates the time dependent control set points. The lowest system level implements motion control. The motion control system receives control set points from the trajectory planning system and sensory feedback information to generate error actuating signals to servo the joint motors so that the manipulator will track the trajectory as closely as possible. One way to control the two robots is to supervise and coordinate the motion of each robot by a supervisory computer [31]. Figure 2.2 shows the hierarchical structure of a two robot system with a supervisory computer. To control the two robots without collision in a common workspace, sharing and exchanging of sensory feedback information between the two robots must be performed in real time. A method of sharing and exchanging of sensory information is to design a real time

Task Planning Motion Planning Motion Control Robot System Figure 2.1 Hierarchical Control Structure of a Robot

Supervisory Computer Robot 1 Robot 2 Figure 2.2 Two Cooperative Robot System

23 interactive controller for each robot using visual, force and touch sensory information as feedback signals. However, for a present-day robot system, many practical difficulties occur in designing such a sensor-based interactive controller. As an approach to realizing appropriate interaction between the two robots, we plan the path and trajectory for both robots interactively. Due to the geometry of the wrist of a robot, potential wrist collisions along the pre-planned path may occur. Thus, a suitable geometric model of the wrist of the robot is needed. Furthermore, algorithms must be developed to replan the paths or trajectories if the potential wrist collisions occur along the paths. 2.3. A Scheme for Straight Line Trajectory Planning Trajectory planning is part of motion planning, which converts the desired path information into-a sequence of time-based intermediate configurations of the manipulator starting at the initial location and ending at the final location. Two currently available approaches to trajectory planning are briefly described. A scheme using discrete time approximation is proposed for straight line trajectory planning later. The first approach requires the user to explicitly specify a set of constraints on position, velocity, and acceleration of the manipulator's joint coordinates at selected locations (called knot points) along the trajectory. Then a parameterized trajectory is selected from a certain class of functions (usually the class of piecewise polynomial functions of degree n or less, for some n, in the time interval [to, tl ]) that "interpolates" and satisfies the constraints at the interpolation points. The second approach requires the user to explicitly specify the path that the manipulator hand has to traverse, such as a straight line path in Cartesian coordinates; and requires

24 the trajectory planner to determine a desired trajectory that closely "approximates" the desired path in Cartesian coordinates. These two approaches result in simple trajectories which are efficient, smooth, and accurate with a fast computation time (near real time). For the purpose of controlling the manipulator, the joint trajectory function must be evaluated at each servo instant of time. In obtaining the control set points from the joint interpolation function, the resultant control set points are assumed to be within the torque constraints of the manipulator. However, due to the approximation of the specified path by the joint interpolation function, the control set points on the joint interpolation function do not satisfy exactly the specified path in the Cartesian space. For example, a desired straight line path with several knot points A, B, C, ~ ~ ~ is shown in Figure 2.3. If we obtain an appropriate joint interpolation function through these knot points and evaluate the function for the control set points, the resultant control set points will be the points a l, a2, as,. in the joint-variable space, which corresponds to the points A1, A2, A3,... in the Cartesian space, respectively. It is obvious that these points A 1, A 2, A 3,... may not be on the desired straight line path. We now describe the relationships which exist between joint variables and Cartesian variables. These are necessary for us to describe our scheme of trajectory planning. The location of the manipulator hand with respect to a global coordinate system ( xO, yo, so ) can be realized by establishing an orthonormal coordinate frame at the manipulator hand which can be represented by a 4X4 hand coordinate transformation matrix [24,481,

25 Cartesian space AI A2 C. A joint-variable space 0q. al time Figure 2.3 Joint Interpolated Trajectory

25 H(t) [R(t) p(t) P] [n(t) s(t) a(t) p(t)] 0 I 0 0 0 1 n,(t) as(t) as(t) p,(t) (2.1) n,(t) *y(t) ay(t) py(t) n.(t) s,(t) az(t) p,(t) 0 0 o 1 where p(t) is the position vector of the manipulator hand; and n(t), s(t), a(t) are the unit normal vectors along the axes of the coordinate frame fixed to the manipulator hand. The dependence of the elements of the hand transformation matrix on joint variables is shown in the Appendix B. Instead of using the rotation submatrix [n( t), s( t), a(t)] to describe the orientation, Euler angles ( yaw (a(t )), pitch (f(t )), and roll ((t)) ) can be used to describe the rotation of the manipulator hand. The relationship between [n(t t) tt)anda(t), #(t), (t)]and (t) t), t)]is: n,(t) o,(t) a,(t)J [C -S 7 S,6] [1 0 0 R(t)- ny(t) oy(t) ay(t) = S 7 C'7 -0 1 0 0 C a -S at n,(t) *,(t) a,(t) 0 0 1 - 9 0a c. Lo s a C a (2.2) C r C S -S Ca+ C '7S S a S $Sa + C 'S g C a S $C C C a+ S 7S S a -C 7S a+ S 7S BC a. -S C S a C RC a where sin a = S a,cos a- C a, sin,-S B cos 3- C B, sin- =S,cos G —C C. In fact, [ a(t), f(t), (t) J are the functions of joint variables as indicated in Appendices B and C. Define the position p(t), orientation A(t), linear velocity v(t ), and angular velocity c(t) vectors of the manipulator hand with respect to the global coordinate frame, respectively, as:

27 p(t)'-(Ps(t), py(t), pz (t))T; (t)-'(a(t),(t), t))T; ~~A~~~~~~ A (2.3) (t ) =- (,,(t),,vy(t),, (t ) ) t; n(t ) = ( W.(t0), y(t), WZt) ) 7. where the superscript T denotes the transpose operation. When we denote the joint position vector as q(t), the relationship between ( p(t), (4(t)) and q(t) can be represented as: ( pT(t), *T(t) )T - N(q(t)) (2.4a) or q(t) N-( p(t), b(t) ) (2.4b) where N(') is the function describing the kinematics routine and N-'1() is the function describing the inverse kinematics routine. The linear and angular velocity of the manipulator hand can be obtained from the lower joint velocities based on the moving coordinate frame concept [381, [nfl(t )] - [J(q)]q(t )= [I(q) J(q ), J6(q)] q(t) (2.5) where qi (t) = P, (t) if joint i is rotary, and qi (t) = di (t) if joint i is prismatic; q(t) = (ql(t), ~., q6(t ))T is the joint velocity vector of the robot; and J(q) is a 6X6 matrix whose ith column vector Ji (q) can be found from s; if joint is rotational Ji (q)- (2.6) [s.-]; if joint i is translational where X indicates vector cross product, Pi- is the position of the origin of the (i-l)"' coordinate frame with respect to the reference frame, s, l is the unit vector

28 along the axis of motion of joint i, and p is the position of the manipulator hand with respect to the reference coordinate frame. The linear and angular acceleration of the manipulator hand can be obtained by taking the time derivative of the velocity vector in Eq. (2.5), f[(t)] [J(q q)]q(t) + [J(q)]q(t) (2.7) where q(t) = (ql(t), ~ ~, q(t ))T is the joint acceleration vector of the robot. Now, the equation of a straight line path is described. An initial location ( p(t0), (to)) and a final location ( p(tf ), $(tf )) are given to the manipulator. The manipulator hand is required to move from the initial location to the desired final location along the straight line specified by these two end points. The straight line equation that passes through these two points can be described as: p(t )= p(t0) + X(t) ( p(t1 )- p(to) ) (2.8a) where 0 < X(t ) < 1. It is worth noting that t1 is not a fixed final traveling time but something we will find. Also, since 4P(t0) and $(tf ) are specified, it is desirable that the manipulator hand reaches its final orientation angles when p(t) approaches p(tf ). This can be easily done by interpolating the Euler angles between t(to) and ( (tf ) linearly along the given straight line: p(t) = *(t0) + ( t (te )- *(t) ) (2.8b) where X(t) is the same as in Eq. (2.8a). As previously discussed, currently available trajectory planning schemes are based on underlying joint interpolation function, which must be evaluated at every servo time instant for an input to the manipulator control system. For the control

29 system which we will consider, it is required that [ p(t), *(t), v(t), r), v(t), f(t) be known at every servo time instant. Since the detection of potential wrist collisions must be performed at each servo instant, it is desirable to have the control servo points ( or set points ) precisely on the straight line path given by Eqs. (2.8a) and (2.8b). To do this, a discrete time approach will be used. In addition to the straight line requirement, the discretized control set points in the joint-variable space must be within certain limits to maintain and guarantee the smoothness of the trajectory. Also, due to the physical limitations of the joint motors, the required joint torques to move the robot from the current set point to the next set point must be within the torque constraint of the joint motors. Thus, the resultant control set points must satisfy the straight line requirement and must be within the smoothness and torque constraints of the robot so that these set points can be tracked by the manipulator hand faithfully. A real time servo interval instead of a normalized time interval will be used in obtaining the control set points. Let us denote T as the servo time interval. Then we consider p(kT) when k is integer. Similarly, we consider *(kT) and X(kT). Ideally, we want to obtain the minimum time trajectory of the robot for the given path. There are many difficulties in obtaining the minimum time trajectory. Thus, instead we maximize the local traveling speed, which is identical to maximize X(kT) - X(kT-T). More precisely, the trajectory planning problem is formulated as follows: maz;imize I I X(kT)-X(kT-T) I I (2.9) subject to the conditions that every p(kT) must be on the straight line and the

30 corresponding joint values from (p(kT), 4(kT)) must be within the smoothness and torque constraints. A detailed derivation of the straight line trajectory planning scheme is presented in Chapter 3. 2.4. Geometric Modeling of the Wrist of a Robot The detection of potential wrist collisions cannot be handled easily due to their complex geometry. A simplified and appropriate geometric model of the wrist is necessary to detect any potential collisions along the preplanned trajectory [491. We shall concentrate on investigating a suitable wrist model for a PUMA robot. Figure 2.4 shows a cylindrical model and a truncated cone model for the wrist which is composed of the upper three links of the robot. For the detection of potential wrist collisions between two robots, all geometric models should be viewed with respect to the global coordinate frame (xo,yo,so). The mathematical expression of these models in the global coordinate frame become time dependent and very complicated, thus causing the detection of potential wrist collisions to be computationally intensive. A sphere model, shown in Figure 2.5, is proposed in this thesis. The sphere contains links 4,5, and 8, including the workpiece grasped by the manipulator hand. The origin of the sphere, which coincides with the origin of the hand coordinate frame, is uniquely determined from displacement of the joints in the arm. The radius of the sphere is determined depending on the workpiece and the wrist geometry. The main advantage of this model is that the sphere is rotationally invariant. This reduces the complexity of the detection of potential wrist collisions to calculating the distance between the origins of the two spheres.

robot robot robot 2 robot 2 Figure 2.4 Cylindrical and Truncated Cone Model

32 robot I robot 2 Figure 2.5 Sphere Model

33 In modeling the wrist geometry as a sphere, a large unnecessary space is included into the sphere so that computational efficiency in detecting the potential collisions is achieved at the expense of the modeling accuracy. The use of sphere model in obtaining a collision-free path is described in Chapter 4. 2.5. Various Collision Situations for Two Robot System In this section, an investigation of various collision situations is presented. The terminology of "potential collision" is confined to the wrist collision between the two robots. The sphere model is used to model the wrists of each manipulator. Two major classes of the potential collision are identified: (1) potential collision between a stationary robot and a moving robot; (2) potential collision between two moving robots. When two robots are working together in a common workspace, one robot may stop temporarily at one position, which corresponds to the class (1). On the other hand, two robots may work together in a common workspace, changing position and orientation simultaneously, which corresponds to the class (2). Planning a collision-free path for class (1) is investigated in an assumed collision situation, where the sphere of a stationary robot wrist with radius r (we label this robot as Robot 1) collides with the sphere of a moving robot wrist with radius r2 (we label this robot as Robot 2) as shown in Figure 2~6. It is assumed that potential collisions occur between the two robots during the time interval from t = mT to t = nT,'where T is the servo sampling period for both robots. Various performance measures can be considered for obtaining a collision-free path in class (1). A collision-free path in this class will be found in Section 4.2. Planning ofr a collision-free path for class (2) is investigated by considering the paths and trajectories of two robots simultaneously. It is important to note that

Robot 2 -mT...a, Figue 2. rT Robot 1 ' - nT Figure 21. Two Colliding Robots

36 even if the original robot paths intersect, a potential collision may not occur because of the differences in the traveling time of the two robots along each path. Thus, the planning of a collision-free path in class (2) is viewed differently from that in class (1). Assume that a straight line path and its trajectory information are given for robot 2 with the initial time to and the final time t/. Also, assume that a potential collision is detected and robot 2 has the lower priority such that it must change its path or trajectory information to avoid collision with robot 1. Here, we classify the collision situation into 4 cases on the assumption that the straight line trajectories of two robots are planned as discussed in Section 2.3. Case 1: The final arrival time t1 of robot 2 can be relaxed but its original path cannot be changed because the modification of path may induce another collision with other obstacles. Case 2: The final arrival time t! of robot 2 can be relaxed and its original path can be changed. Case 3: The final arrival time tI of robot 2 cannot be relaxed but its original path can be changed. Case 4: The final arrival time tL of robot 2 and its original path cannot be relaxed. In case 1, since only the final arrival time tf of robot 2 can be changed, a method to obtain a different trajectory, which can eliminate the potential collision with robot 1, must be developed for the same path. We call this time scheduling of a straight line trajectory. In case 2, except for the initial and the final locations, the original path and trajectory information can be changed freely in order to avoid

38 potential collisions. There are a number of ways to obtain a collision-free path of robot 2. A collision-free path of robot 2 in this case will be found in Section 4.4. In case 3, the final arrival time t/ is fixed while the original path can be changed. Due to the fixed final arrival time tl, there is no guarantee that there exists a collision-free path of robot 2 satisfying the final arrival time t. In case 4, the original path and trajectory cannot be changed. That is, the collision avoidance must be realized by changing the path and/or trajectory of the other moving robot. Only cases 1 and 2 are considered in this thesis; cases 3 and 4 are more difficult. Detailed derivations in planning a collision-free path are presented in Chapter 4. 2.8. A Cartesian Space Control Most of the existing control schemes control the robot arm at the joint level and emphasize nonlinear compensation of the interaction forces among the various joints. One of the main reasons for performing control in the Cartesian space is that, in the joint-variable space control, a small joint error may cause a fairly large error in the Cartesian position of the manipulator hand. The error may be crucial in achieving the collision-free motion control for the two robots. In general, it is easier to specify the desired motion of the robot in terms of a manipulator hand trajectory in Cartesian coordinates. However, the robot control system requires the reference inputs specified in joint coordinates. The mathematiL cal relationship between these two coordinate systems is important in deriving a resolved motion control scheme. Hence, it is necessary to examine the kinematic relationships of position, velocity and acceleration between the joint and the Cartesian coordinate systems.

The location of the manipulator hand with respect to the global coordinate system can be realized by establishing an orthonormal hand coordinate frame at the gripping point of the manipulator hand as shown in Figure 2.7. The problem of finding the location of the manipulator hand is reduced to finding the position and orientation of the hand coordinate frame with respect to the global coordinate frame. This can be conveniently achieved by the 4X4 hand transformation matrix as in Eq. (2.1). Using the same definition of linear and angular position, and velocity as in Eq. (2.3), the linear velocity of the manipulator hand with respect to the global coordinate frame is equal to the time derivative of the position of the manipulator hand, d(t)-= p(t) =p(t) (2.10) dt Since the inverse of a direction cosine matrix is equivalent to its transpose, the instantaneous angular velocity of the hand coordinate frame about the axes of the global coordinate frame can be obtained from Eq. (2.2) as [15], 0 - W W] R d d = -d R Rr R dtdRT r 0 -W dt dt w ws 0 (2.11) O -Sf +- -SdCa-c- C S a-7 o C C a -S S r C a + c 7 - C 7 C ha + S Ad O From Eq. (2.11), the relation between the (w,(t), w (t), (t))r and ( c(t ), d(t), (t)) r can be found by equating the non-zero elements in the matrices,

~~~~~~~~~~~~~~r Zo P6 X. Figure 2.7 Hand Coordinate System

w~(' )'Cg-s r -IC~ a(t) vy(t) S '1 C B C '7 0 9.) (2.12) wO(t) -S9 O 1 5(t) Then, its inverse relation can be found easily, a(t) r c 1 ws(t)] It) = secof - C y C C O 0 WY(t). (2.13) it) C ~sl sO SfS C OJ(t) Using our vector notation, this becomes *(t )= [M (4)] n(1t) (2.14) Based on the moving coordinate frame concept [38], the linear and angular velocities of the manipulator hand can be obtained from the velocities of the lower joints, as in Eq. (2.5). If J is invertible at q(t ), then the joint velocity q(t) can be computed from the manipulator hand velocity using Eq. (2.5), q(t ) -— = l [[ (t-(q ) ] (2.15) Given the desired linear and angular velocities of the manipulator hand, Eq. (2.15) computes the corresponding joint velocity and indicates the rates at which the joint motors must be maintained in order to achieve a steady hand motion along the desired Cartesian direction. Using Eqs. (2.7) and (2.15), we have,

40 Ln(t) [S2(t!] = [ J(q' q) [ (q) Lfi(t + [$(q) I q(t) Then the joint acceleration q(t) can be computed from the manipulator hand velocity and its derivative as, q(t) (q) [()] (q) (q )J-(q) (t (2.17) The Eqs. (2.4b), (2.15), and (2.17) constitute the governing equations which transform the Cartesian coordinates to the joint coordinates. Our objective is to utilize the resolved motion control concept to develop an adaptive control in the Cartesian space. Hence, it is necessary to develop an equations of motion of the manipulator in Cartesian coordinates. The above kinematic relations between the joint and Cartesian coordinates will be used to derive the equations of motion of the manipulator in Cartesian coordinates. Detailed derivation of the resolved motion adaptive control scheme is presented in Chapter 5. 2.7. Summary The overall control structure of the robot system is arranged in three hierarchical levels: task planning, motion planning, and motion control. The servo-timebased trajectory planning scheme is proposed and the problems associated with the trajectory planning scheme are also discussed. Using a sphere model for the wrist of a robot, the detection of potential wrist collision is found to be computationally efficient at the expense of modeling accuracy. Various collision situations are identified and classified for obtaining the collision-free paths of two robots. Finally, an adaptive control in the Cartesian space which extends the resolved motion control concept has been suggested.

CHAPTER 3 PLANNING OF STRAIGHT LINE MIANIPULATOR TRAJECTORY 3.1. Introduction In this chapter, we develop a discrete time trajectory planning scheme to obtain the straight line trajectory set points which are applicable to avoiding collisions of the wrists between two moving robots, or between one stationary and one moving robot. Using the kinematic equations described in Section 2.3, the local speed optimization of the robot hand between two consecutive Cartesian set points is formulated along with a discussion of the various constraints in Section 3.2. Section 3.3 describes a binary search algorithm to find the servo control set points subject to the various constraints. The binary search technique generates servo control set points which result in an acceleration portion and a deceleration portion of the given straight line. Break point and relaxation points which are needed to construct the overall trajectories are discussed in Section 3.4. In Section 3.5, the trajectory planning of connected straight line segments is presented. Finally, summaries are given in Section 3.8. 3.2. Formulation for Local Speed Optimization In this section, the optimization of local traveling speed along the given straight line is described subject to the straight line requirement, and smoothness 41

42 and torque constraints. It is based on the discretized joint velocity, acceleration, and jerk approximations. Straight Line Constraint. In the straight line trajectory planning, the initial location [ p(to), 4(to)] and the final location [ p(tlt ), *(tf ) are given for the manipulator, where p(t) and ~(t) specify the Cartesian position and the Euler angles of the manipulator hand at time t, respectively. The manipulator hand is required to move from an initial location (position and orientation) to a desired final location in the Cartesian space along the straight line specified by these two end points. Using P(t) = ( P (t), p,(t, p. (t )T, the straight line equation that passes through these two points is described by Ps (t) = X(t) ( P, (tt) - Ps (to)) + Ps (to) p, (t) = (t)( p, (t/,) - pg (to) ) + p, (to) (3.1) pz (t) = X(t) ( P, (tt ) - P. (to) ) + P, (to) where 0 < X(t) < 1, and to and tft are the initial and final time respectively. The time t/t is not fixed, but will be determined for the robot by the overall trajectory planning algorithm. Thus we use the subscript ft to denote the final time to be tried or determined. Also, since <((to) and 4c(t/t) are specified, it is desirable that the manipulator hand reaches the final orientation angle when p(t) approaches p(tt ). Using *(t) = ( a(t ), f(t ), (t) )T, this can be achieved by interpolating the Euler angles between *((to) and ((t1t) linearly using the ratio of the traveled length to the total length of the given straight line as described in the following equation,

43 a(t) = X(t) ( a(t/t) - a(to) ) + c(to) B(t ) (t ) ( (t/,) - A(t) ) + A(to) (3.2) t ) X(t ) o ( ttf) - (to) ) + Ito) The position p(t) and the Euler angles 4(t) can be augmented into a X 1 vector and described by: [t] =N(q(t)) = (Nl(q(t)), N6(q(t ))) (3.3) where N(.) is a OX 1 nonlinear vector function depending on the manipulator configuration. Currently available trajectory planning schemes are based on underlying joint interpolation functions. The joint interpolation function must be evaluated at every servo time instant and the joint values must be stored in the computer memory as an input to the manipulator control system. For our control scheme, it is required that [ p(t), $0(t), v(t), nfl(t), (t), (t) ] be known at every instant of sampling period for the Cartesian space control 1281 and that these control set points be precisely on a straight line. The algorithm which we present uses a discrete time approach to find these data directly at every sampling instant. To initiate the discretized trajectory analysis, let us denote the sampling period for the servo control of the robot as T (usually 3 ma < T < 20 ma ) and q(k) to represent angular displacement q(kT): q(kT) = 4(k); k 0,1, -. (3.4) Then we will approximate the velocity, the acceleration, and the jerk, respectively, at time kT by:

44 q(kT) — T ((k) - (k-1)) = (k (3.5) T q(kT) - 2 (4(k) - 2 q(k -1) + q(k-2)) = q(k) (3.8) w(kT) - L3((k) - 3(k-1) + 3 (,(k -2)- ((k-3)) = W(k) (3.7) T3 where w(kT) denotes the jerk at time t = kT. For simplicity and brevity, we shall abuse notation and drop the super bar from the rest of the equations. That is, q(k ) -q(k) (3.8a) q(k) = (k) (3.8b) q(k) )-(k) (3.&) w(k ) — (k) (3.8d) Finally, we let to - koT, where ko is assumed to be zero, and tlt = kfl T. Smoothness Constraint. Since we will obtain the control set points not from a joint interpolation polynomial function but from the discrete time approximation, all discretized control set points in the joint variable space must be within certain limits to maintain and guarantee the smoothness of the trajectory. The smoothness constraint on the joint trajectory set points can be stipulated in three possible ways by a velocity bound (VB), an acceleration bound (AB), and a jerk bound (JB). They are given respectively as: qi (k) I < c,; E,>0 and = 1,..,6 (3.9) I i (k)'} < e,; e > 0 and i 1,. e, (3.10) woi (k) I < /i; ci,> 0 and i —1,. -,6 (3.11) where e?", e?, and eiJ are the i'th element of -dimensional bound vectors for the

45 manipulator. The velocity bound (VB) and acceleration bound (AB) constrain the joint actuators from exceeding the maximum limits of the velocity and acceleration. The jerk bound (JB) reduces wear of joint actuators, reduces excitation of vibrations, and constrains the smoothness. Hence, we impose VB, AB, and JB on the entire trajectory from one trajectory set point to another. Combining Eqs. (3.10) and (3.11), we have: q (k ) < q (k ) < q, (k ); i = 1, 2,, 8 (3.12) where qi,,,, (k) ( max -f, q (k -1) - f/J T (3.13) qi,(k) =- - mi { c, qi(k-1) +,- T } (3.14) Similarly, we have the following equation from Eqs. (3.9) and (3.12) for the joint velocity constraint. qi,,i,s(k) ( q (k) qia(k); i = 1, 2,,6 (3.15) where in j(kl)= max ( -fr T * qs,(k) + q,(k-1) } (3.16) qma.s (k) = min { T q, (k) + q, (k-l) }(3.17) Then, we can obtain the joint position constraint as: imin (k) <5 q(k) qi,a (k); i = 1, 2,, 6 (3.18) where

48 i;,, (k ) T q~,n(k) + q, ( ) + i(k -1) (3.19) qi, (k )-= T ~ qi,n (k) + q (k-1) Torque Constraint. Due to the physical limitations of the joint motors, the robot is limited on how far it can move from current set point to the next set point on the straight line path. The required joint torques to move the robot are described by its dynamic equations of motion and depend on the instantaneous joint position, velocity, acceleration, and the load that it is carrying. In general, the dynamical behavior of a robot can be described by the Lagrange-Euler equations of motion as [24,30,461.(kT) )- k ) = [D(q(k )) q(k) + h(q(k ), q(k )) + c(q(k )) (3.20) where Ak) is an 6X1 applied torque vector for joint motors, c(q(k)) is an 6X1 gravitational force vector, h(q(k), q(k)) is an 6X1 Coriolis and centrifugal force vector, and [D(q(k))] is an 6X6 acceleration-related matrix. The approximate equality results from the discrete-time approximation of q, q and q. Hereafter we omit the super bar from ik ). If q(k), q(k) and q(k) are given, the required piecewise joint torques can be computed by treating the equations of motion as an inverse dynamics problem. In a simplified notation, r(k) - [Dk ]q(k)+ hk + Ck (3.21) where Dk = D(q(k)), hk = h(q(k), q(k)) and Ck = c(q(k )). At k0 = 0, we assume: q(0)=0; q(0)=0; w(O) O (3.22) Let us further assume that the torques generated from Eq. (3.21) are constrained by

47 limits that are dependent on the joint position ( due to the manipulator actuator geometry ) and on the joint velocity ( due to the back electromotive force terms or other actuator effects ) as, ri, mi,, (q(k), q(k)) ri (k) ri,ms (q(k), q(k)); i = 1, ',6(3.23) or in a simplified notation as, ri.i (k) ~ ri (k ) < ri.. (k); -= 1, * *,6 (3.24) Since the joint torque is represented by Eq. (3.21), the Eq. (3.24) can be written as, ri.,(k ) < Di, q(k ) < r+s (k) (3.25) where Di,k represents the ilA row of the matrix Dk, and r,.(k) and riK+(k) are written as: ri, (k) = -ri,mi (k) - hi,k - Ci,k (3.26) ri+a (k) - ri,,ms (k)- hi, - Ci,k (3.27) where hik and c, i,k are the itk element of the vectors hk and ck, respectively. Therefore, the joint values at each servo time instant must satisfy the straight line requirement given by Eqs. (3.1)-(3.2), the smoothness constraint given by Eq. (3.18), and the torque constraint given by Eq. (3.25). Formulation for Optimization. The Eqs. (3.1)-(3.2) for the straight line requirement, Eq. (3.18) for the smoothness constraints, and Eq. (3.25) for the torque constraints will be used as constraints for finding the set points on the straight line path in an optimization process. We define 5(k) )- p(kT) = ( p, (kT), p,(kT), p,(kT) )T (3.28)

48 Again we drop the super bar from P(k) and $(k) for notational consistency. Let us assume that at the time t =((k-1)T, p(k -1) and O(k-1) are given and that they are within the physical bounds from the straight line requirements, the smoothness and torque constraints at t = (k-1)T. Then, we would like to find the next set point, [ p(k), 4(k )1, such that it is again within the smoothness and torque constraints and must lie on the specified straight line path exactly. It is desirable to obtain the minimum time trajectory of the straight line path. However, there are many arising difficulties in obtaining the minimum time trajectory of the straight line path due to the discrete time approximation. Here, we would like to minimize the local traveling time during one servo time period, which does not yield the minimum time trajectory for the given straight line path. We maximize the Cartesian distance between two consecutive set points l{ p(k)-p(k-1) I I (3.30) subject to the smoothness constraint, torque constraint, and the straight line requirements of p(k). We introduce X(k) which satisfies the following equation, k (kP) (kf)- P(k o) p+ (k 0) (331) L(kf )[(ko)] + k)] (3.31)(k) where 0 < X(k) < 1. From Eq. (3.3), LA$)(k = [vN(q(k ))]1 q(k) (3.32) where Ap(k) -p(k ) p(k-1), A6(k) - (k)- A(k-1), Aq(k) q(k)- q(k -1), and the elements of [vN(q(k ))] are found to be [vN(q(k ))j; ij = 1, 2, q(k,)) (3.33) qiq(k); = 1,2, 6

49 Utilizing Eq. (3.31) at time t = (k -1)T and t = kT and- their difference yields, ~p~k A(kp(kft - p(ko)] (3.34) where AX(k) = X(k) - X(k-1). Combining Eqs. (3.32) and (3.34), we have: 4 rp(k/, ) - p(k 0) (3.35) [v N(q(k ))l' L~q(k ) - Xk)L(l _*k(.5 If [vN(q(k )) is non-singular at time t = kT, then rp(kf, ) - p(ko) 1 Aq(k) = AX(k )[vN(q(k )fl-t 14~(k/t) - b(k0)] (3.36) We can rewrite Eq. (3.38) as: Aq(k) ~ A),(k )Q(k) (3.37) where Q(k = [vN(q(k ))]-I rp( t, ) - p(to0) [P(' t/ ) - p N to)] (3.38)!Q (k), o, Q6(k)]T Physically, Q(k) is the vector which relates the angular displacement of each joint with AhX(k) of a given straight line. Since the servo time interval T is very small, let us assume that, for the joint position at.,t = kT, q(k) = q(k -) + aq(k) (3.39) Then using Eq. (3.37), we have, q(k) q (k-1) + AX(k )Q(k) (3.40) Combining Eqs. (3.18), (3.19) and (3.40), we have:

50 T ~ q,,;,(k) T qim(k3-41) Qi (k) -Q (k) Another constraint on AX(k) is included from the torque constraint given by Eq. (3.25). Combining Eqs. (3.25) and (3.40), we have: AX,,,imi(k) < AX(k) AXi,m,(k); i - 1,,6 (3.42) where = T2r,(k ) + Di, q(k -1) - Di,, q(k -2) k Di, Q(k) (3.43),2,(k) (k) + Di,k q(k -1) - Dk q(k -2) Aeximan (k) and,il,. (k) are the maximum and minimum constraints for AX(k) from the i t joint torque constraint.. Since the maximum AX(k) results in the maximum local speed at t = kT, it is desirable to formulate the problem as: maximize AX(k); k-1, 2... subject to the smoothness constraint T qm(k ) < AX(k) <T qs(k) (3.44) Qi (k) Q,(k) and the torque constraint Axi,mi, (k) < Ax(k) < Ai,ms (k) (3.45) where i = 1,2, ~ ~,8. If we rewrite equation (3.44) as A-(k) < AX(k) _ AX+(k ) (3.46) where

51 t T q~, (k) T qi (k ) (3.47) T qimin (k) Q (k) for i 1, ~, 6, then the problem can be formulated as follows: maximize AX(k); -1, 2, subject to AXimin (k) AS(k) AS, mu (k) and ALX-(k ) AX(k) < AX +(k) wherei -1, e,6. Since the constraints on AX(k ) may not be consistent, the existence of the maximum of AX(k) is not always guaranteed. The existence of the straight line trajectory is discussed in Section 3.4. In fact, A,/X(k), AX,-(k), AX,,,,,, (k) and AXi,ms (k) are obtained and derived by approximations. Hence, it is very difficult to satisfy inequalities (3.45) and (3.46) exactly. Computational aspects of (3.45) and (3.46) are considered in the following discussion. Computational Aspecta The computations of AX,+(k) and AX-(k) require the computations of [vN(q(k))] and its inverse, if it exists. Also, the computations of ALXi,min and AX,,.. require the actual torque constraints in Eq. (3.23) which are dependent on q(k) and q(k). The elements of the nonlinear vector function N(q(k)) = (N1(q(k)),...,N8(q(k)))T for the PUMA robot (Appendix C) are as follows:

62 Nl(q(k)) = CL[d6(C23C4Ss + S23Cs) + S23d4 + a3C23 (3.48) + a2C2]1- Sx(d6S4Ss + d2) N2(q(k )) = S1[d6(C23C4S5 + S23C5) + S23d4 + a3C23 + a3C2] + C2(d6S4Ss + d2) N3(q(k )) = d(C23Cs - S23C4Ss) + C23d4 - a3S23 - a2S2 (3.50) (k q~t))=~tan~' ~,(t] ". (k a, (k )sin(tan n-(k) ) + a.(k kos(tan-l Y(k) N.(q(k)) = ta ns (k) n-, (k) -o(kkcos~tanl;n(k)) (3.51) N4( q(k ))in — - f - +- (k) -,n(k) where ns(k) = C1[C23(CCC8 - S4S6) - S23S5C61 (3 -S2(S4C5CB + C C6) n1(k) = S1[C23(C4C6C6- S4S() - S23S5C6] + C2(S4CSC6 + C4C6) n, (k) = -S[C23(C4C5C6 - S4S6)- 23S5C (3.58) as (k) = C2(C23C4S + S23CS) - S2S4S5 a%(k) - S1(C23C4S5 + S23Cs) + C254S- (3.57)

53 a,(k) =C1[-C23(C4C5C6 + SS) + 23SS6 (3.58) (3.58) -S1(-SCSS6 + C4 C6) and ay(k) = S1[-C23(C4C5S5 + S4,C) + 2356] (359 + C2(-S4C5S6 + C4C6) where Cj = cosq, Si = singq, Cj, - cos(qi + qj ) and Si = sin( q + qj ) The computations of [VN(q(k ))li, i, j = 1,,6, require intensive mathematical operations especially for the orientation part of N(q(k )), that is, N,(q(k )),Ns(q(k)), and N6(q(k )). In addition, the computation of arc tangent values may generate errors due to the table look-up method or the power series expansion in actual computations. The inverse of [vN(q(k ))] will also require a.moderate computational effort. In addition to the mathematical complexity, all the computation involved is based on a first order approximation, thus it is very difficult to obtain the exact constraint on AX(k). The inequalities (3.46) from the torque constraints are also based on an approximation. Due to the computational difficulties and the errors from the first order approximation, we resort to a search algorithm to find the maximum of AX(k ). Let p(k) be the reachable hand position of the robot from p(k-l) and ~(k) be the corresponding Euler angles at time t = kT. If the inverse kinematics solution of [p(k), $D(k)] is within the smoothness and torque constraints, then finding the maximum of AX(k) corresponds to finding the maximum of I I p(k) - p(k -1). We define lk as: A I= I I p(k)-p(k-l) I I = AX(k)- 1 p(kl )-p(k0) [ [ (3.60) In the next section, we utilize a search algorithm to get the approximated maximum

54 value of AX(k ). 3.3. Search Algorithm The method to get the approximated maximum value of AX(k) involves three stages of development and utilizes a binary search technique to determine the largest value of path length ratio AX(k) at time t = kT within all the physical constraints. Let us use a "hat" notation as f to denote the estimated value of the corresponding function f. Then, three stages of development are as follows. First, using the directional information of the straight line, X(1) is determined. Recall that X(1) = AX(1). The initial estimate, AX1(1), is then used in an iterative binary search algorithm to obtain the largest value of A\(1). Here, we use the superscript 1 to denote the initial estimated value of the AX(1). Second, using AX(k-l) and the projection of the estimate of linear velocity 't(k) on to the straight line, the initial estimate of AVX(k), for k = 2, 3,., can be obtained. Third, the initial estimate zAX1(k) is used in the same iterative binary search algorithm to find the corresponding largest value of AX(k ) for k = 2, 3,. From Figure 3.1, AB indicates the straight line path that the robot has to travel. At point A, p(k0),,(k ), v(k0),,(k0) are specified and satisfy the all constraints. We would like to find the control set points p(k), $(k), v(k), fl(k), v(k), f[(k) on the straight line together with the corresponding q(k), q(k) and q(k) for k = 1,. such that the robot hand will traverse the straight line. Passing the (p(ko), 4(ko)) values into the inverse kinematics solution routine, the corresponding joint angles, q(ko) = NS1(p(ko), (ko)) are obtained. We assume, without loss of generality,

*rq ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~~q C4~~~~~~~~~~~~~~~~~~~~4 \ ~~~CU C C \ -8 Ad '5; ~' O O O oo so~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~P 4.E I~~~~~~~~~~~~~~~~~I o 0 o~~~~~~~~~~ l~~~~~~~a ' I^ \~ ~~ 'S *f O H

58 that v(ko) fl(ko) -- v(ko ) = f(ko) = 0 which lead to q(ko) = q(ko)- O. We further assume that w(ko)- 0. With these assumptions, a method to obtain AX(k) is discussed in the next section. 3.3.1. Method to Find Initial Estimated Length Ratio X(1) can be estimated by evaluating the directional information of the given straight line. From the joint position constraint at time t = T (Eq. (3.18)), qi (1) must be bounded by qi,mis (1) and qi,,, (1). Depending on the straight line equation, the linear velocity of the robot hand, v(1), must be constrained. Let us denote the ij't element of the Jacobian matrix which is defined in Eq. (2.6) as Jii. From Eq. (2.5), vs (1) = J11(1)q1(1) +... + J16(1)q6(1) v(1) = J21(1)q(1) + - -. + J2(1)q6(1) v,(1)- J3(1)q1(1) +. + J36(1)q6(1) (3.61) wz(1) J41(1)ql(l)+. + J46(1)q6(1) %W(1)= J51(1)Q1(1) +.. + Js(1)q6(1) W3(1) J61(1)ql(1) + ( + J1()q6(1) Since the robot motion is expected to be very slow at the first servo time instant, we assume that Jii(1) Jij(0). If v,(1) > 0 from the given straight line, then qj (1) can be selected positively whenever Jl1 (1) > 0, otherwise it can be selected negatively. That is, if v, (1) > 0, then q,(1) q= jqi,m(l) if J1,(O) >; = 1, 2,., (3.62) Similarly, if vu (1) < 0, then

57.(1)= { sas(1) if Jl(O) <0 (3. qj(1- qim.",(1) if iJ(o) > o; j = 1, 2,, 8 (3.83) From the chosen values of I (1), we can obtain js (1) from the kinematic equations. Let us use a notation X, (1) to represent the estimated value of X(1) from the x directional requirement. Using Eq. (3.1), we can determine X,(I) from the directional constraint along the x-axis of the reference frame, APSPP, (1)- p,(ko) X" (1) p (k) - p(ko) (3.64a) where if p, (k,) = ps(ko), then there is no constraint on X,(1). Similarly, along the y axis and s axis of the reference frame, we have, respectively, - y j, (1) p5(k0) Xy () PJ (kft)- _y(kO) (3.64b) P, (1)- p,(ko) PS (kf) - P (ko) (3.4C) where if py(k/t) py (k0), then there is no constraint on Xy(1), and if pz (kf) = pz (k ), then there is no constraint on X, (1). Similarly, the signs of,w, w1 and w3 can be determined from the given Euler angle information and Eq. (2.12). That is, if ws (1) > 0, then qjq,,m (1) if J4j (0) > 0 q(1) q (1) if J4(0) < 0; j = 1, 2,, (3.65) and if ws (1) < O0, then qj,m.a(1) if J4 (0) < 0 3. qj (1) = qQjmi,(1) if J4ji() > O j=l, 2, 8 (3.6) Thus we have another set of constraints on X(1) from the Euler angle information.

58 That is, we have X.(1), *X(1) and \X(1) as in Eqs. (3.64a)-(3.84c). Then we can choose X(1) as: X(1)-= min { Xs(1),,(1), IX(1), Ya(1), O (i), l ()1) } (3.67) Recall that AX(1) = X(1) by the definition of X(k). Physically the choice of the value of X(1) as in Eq. (3.87) means that the robot motion at t -= T is constrained critically by the corresponding position or Euler angle information. 3.3.2. The Ratio of Initial Estimated Path Length Once X(1) is found from Eq. (3.87), it can be used in Eqs. (3.1)x(3.2) to find p(1) and 4(1). It should be noted that the [ $(l), $(1)J is on the given straight line exactly. However, the inverse kinematics solution from p(1) and i(1) may not be within the smoothness and torque constraints due to the fact that the value Ax1(1) is determined by one directional constraint in Eq. (3.87). In order to find the largest value of AX(1) within the smoothness and torque constraints, the AX1(1) is used in the steps FW4 to FW8 in the iterative binary forward search algorithm (Algorithm FW) in the next section. Once AX(1) is found, we need to determine the initial estimate of AX1(k), for k = 2, 3,... on the straight line and the corresponding largest value of AX(k) from the same binary search algorithm. Since we utilize the binary search algorithm to find AX(k), it is reasonable to have an initial estimated value of AX1(k) satisfying the smoothness and torque constraints to guarantee the existence of the solution. The torque constraint can be verified in the iterative search algorithm by checking whether the computed torques satisfy the torque constraints ( Eq. (3.25) ) or not. For the smoothness constraint, the best initial estimate of nj (k ), which corresponds to AX1(k), is chosen to be the

59 mid-point of the allowable position bounds as: qi(k)=f (qj,,()+ ( k)+qs(k)); i =1, 2,..., 6 (3.68) Using eq (k ) and qi (k -1), the initial estimate of joint velocity is found as: q i(kt T (q, i (k q(k -l)); =i 1,2,... (3.69) The initial estimate of linear velocity,,(k), can be obtained by using Eq. (2.5) as: (k ) Ju= (q(k )) q(k) (3.70) where J. (q(k )) is the upper 3X6 submatrix of the Jacobian matrix in Eq. (2.6). To obtain the estimated value of AXl(k), let,p(k) be the projection of %(k) onto the given straight line specified by the initial position p(ko) and the final position p(kf t). Then A(k) =. ( p(k/) - p(k0) v(k) (3.71) VPI I p(kf,)-p(ko) II Let us denote the total length of the given straight line path as ltot41. Then, the projection of the linear velocity v(k ) onto the given straight line approximately AX,(k) ~/,o,,,i corresponds to the value of at t = klT because the linear velocity is T just a component of the velocity in the straight line direction. That is, Tp (k) uT h Ao(kl) taoxm (3.72) Thus, we use the following approximation at t - kT. ail~~s '(k)T (.7A I L I P

80 which leads to the initial estimate of AX1(k) as, i ( p(kt g)- p(ko) 'I I (k)ot A,\X(k ) T.; k = 2, 3, *- (3.74) The estimate AX1(k) will be used to obtain the value of AX(k) from which the Cartesian position and the Euler angles can be obtained and converted into the control set point at t =-*1T on the straight line path. However, we have to mention that even if it is rational to take the initial estimate as in Eq. (3.74), the inverse kinematics solution from AX1(k) may not always satisfy the smoothness and torque constraints. 3.3.3. Forward Search Algorithm After finding the initial estimate of AX(k ), our objective is to find the largest value of AX(k) on the straight line subject to the smoothness and torque constraints. This is solved by an iterative forward binary search algorithm which is used to determine all the control set points for the "acceleration" portion of the given straight line. The algorithm FW is outlined below. Algorithm FW: Given the initial estimates of AX'(k ), this algorithm performs an iterative forward search of the largest value of AX(k) on the given straight line. FW1. [Initialize and loop] For each k = 1, 2, ~ ~ ~, perform steps FW2 to FW8. FW2. [Compute the desired set of constraint limits] Compute the constraint limits for each joint: (1) Smoothness constraint as in Eq. (3.18): qi,i,n (k), qi.,, (k) (2) Torque constraint as in Eq. (3.25): ri-,(k ), r[+,(k) FW3. [Initial estimate] Find AX1(k) (Eq. (3.B7) and (3.74)) and set m = 0. Also, set LX0 I(k) = A ILX(k) and Xhigh (k) P2 AX1(k), where p1 and P2 are designated constants. (when k =m 1, AX u(1) O )

81 FW4. [Check the global stopping criterion for Xm (k) J Update the value of m and find X' (k) = X(k-1) + AXm (k). If X" (k) < 1 - 1,. ( where 61 > 0 is a designated constant ) then go to step FW5, otherwise stop. (Trajectory planning has been completed.) FW5. [Find Cartesian solution] Using X" (k), p(k) and $(k) are found as follows: P(k ) X"(k) ( p(k, ) - p(k0) ) + p(ko) *(k )- X'(k) *( $(kl,) - (ko) )+ 4(k0) FW6. [Find thy inverse kinematics solution] Obtain the inverse kinematic solution of p(k ), t(k) ]. That is, find 4(k) N-'( p(k ), D(k) ) FW7. [Check estimate's constraints and adjust AX' (k)] If q(k) and q(k) satisfy the bounds computed in FW2, then set AX"'' (k) = AX"' (k ) and AX' + -(k ) = (k+.h (k) and go to step FW8, otherwise set AXO (k) = AX"(k) and A +l"(k ) _AS' (k) -+ AS ( k) 2 and go to step FW4. FW8. [Check stopping criterion for AXSm (k) If I AXm"+(k) - AXm(k) < 62 AXl(k) ( where 62 > 0 is a designated constant ) then AX(k) = AXm (k); k = k +1; and go to step FW2, otherwise go to step FW4. In step FW3, AX"' (k) and A\gAi' (k) constitute the low and high bound for the bisection process. The Pi and p2 must be chosen for AX"" (k) and AXI"' (k) such that the former satisfies the constraint limits in step FW2 while the latter does not. Since the low bound AX~'' (k) is obtained from the smoothness constraints only, there is no guarantee that it will satisfy the torque constraints. Then, the failure of the search algorithm will result. In step FW8, the stopping criterion 62 will constrain the number of looping in the FW algorithm. That is, if 62 - 0.0001,

62 then the bisection process between AX1(k) and AX"O'f(k) will continue until m - 14. Simulation results with the failure of the search algorithm will be shown in Chapter 6. 3.4. Determination of Break Point and Relaxation Points 3.4.1. The Break Point and The Modified Search Algorithm The forward search algorithm (Algorithm FW) generates the trajectory set points which constitute the acceleration portion of the straight line. Thus, a point must be determined on the straight line so that the deceleration portion starts from the point to meet the boundary conditions at the final location. A break point is defined as a point on the given straight line at which the acceleration portion of the straight line ends and the deceleration portion of the straight line starts. ( Later, we will expand the definition of the break point to include a transition point from a deceleration portion to an acceleration portion also. ) Based on the initial position/orientation and linear/angular velocity information, [ p(ko), 4(ko), v(ko), f(ko) ], the forward search algorithm generates the largest values of AX(1), AX(2),, AX(), which in turn generate the corresponding trajectory set points, [p(l), 4(l)l, [p(2), 4(2)1, [ ), p ( )I, X. -, on the straight line and at the same time the inverse kinematics solution of these locations satisfy the smoothness and torque constraints. Similarly, based on the final position/orientation and linear/angular velocity information, [ p(kr ), 4t(klf ), v(.k1), n(kf) J, the backward search algorithm (Algorithm BW), which is identical to the algorithm FW but starts from the final location, generates the largest values of

83 AXb(1), AX6(2), - ', AX6(r), -- ' which corresponds to the trajectory set points, [Pb (1), Ib (1)1, [pb (2), b (2)1, *, [Pb (r )),, (r )I, ', satisfying the smoothness and torque constraints also. We use the subscript b in AXb (k), Pb (k) and P6 (k), where k = 1, 2, ~ ~ ~, to clarify that they are obtained from the backward search algorithm. In generating the backward trajectory set points, we really do not know the time kit, but we can re-index the time from zero. In our estimation of the break point, we must then translate the backward trajectory set points in time so as to obtain a good fit of the forward and backward trajectories. The forward and backward binary search algorithms provide us the information about the position of a break point. The backward search algorithm generates the set points which do not constitute the control set points in the correct traveling direction of the robot. Also, the jerk bound may be violated at the break point if the backward search points are used from the break point. Thus, a modified forward search algorithm (Algorithm MFW) is used to find the set points from the break point to the final location. This break point can be determined from the following procedure BR: Procedure BR: Given the forward trajectory set points [ p(1), *(1)], [ p(2), $(2) ],. p(s ), $( ) ], ' -. and the backward trajectory set points [pb(l),4b(1) 1, [pb(2), 4b(2), ' [p (b(r), b6(r)],..., this procedure determines a suitable break point on the straight line where decelera. tion motion starts in order to meet the boundary conditions of the final location. BR1. [Determine a possible break point] Determine a set point p(s ) on the straight line from the forward and backward search points such that

64 I I P(a)-Pb() | I + kd ~ I I P( +1)-p(?) Pb(r)- p(r-1) T T is minimum over a and r. The number kd is a weighting factor that measures the slope error between the forward and backward motions. Also, set the loop counter c = 0. BR2. [Apply the MFW algorithm from the set point found in BR1.] Update the loop counter c. Treating the set point p( ) as an initial position, apply the modified forward search algorithm for the deceleration motion and check whether the algorithm MFW is stopped by the global stopping criterion 61. BR3. [Adjust the location of the break point.] If the algorithm MFW is stopped by the global stopping constant bl, then decrease a by 1 and go to step BR2, otherwise go to step BR4. BR4. [Determine the best break point.] If the loop counter c > 2, then p(a +1) is the best break point, and compute the final joint position, velocity, and acceleration. otherwise reset the loop counter c to 0, increase a by 1, and go to step BR2. Step BR1 indicates that the approximated location of the break point is initially selected at a point where each two consecutive set points from the forward and backward search algorithm are very close and the linear velocities between them are very close to each other. The remaining steps are used to correct the approximated location of the break point. Thus we obtain the earliest break point which yields the smallest error at the final search point. The modified forward binary search algorithm is identical to the algorithm FW except the step FW7 which is modified to accommodate the deceleration motion. The step FW7 is modified in the algorithm MFW as follows:

85 Algorithm MFW: Given the initial estimate of AXl(k), this algorithm performs a modified iterative forward search of the largest value of AX(k) on the given straight line from the break point to the vicinity of the final position. Steps MFW1 through MFWO and MFW8 are identical to the steps FW1 through FW6 and FW8 in the Algorithm FW, respectively, except that step MFW3 do not need Eq. (3.87), and Pi and P2 are different from those of the algorithm FW. MFW7. [Check estimate's constraints and adjust AX" (k)] If the joint inverse kinematics solution satisfies the bounds computed in MFW2, then set AXAiA(k) = ____(k) and A_"_+_(k_) _(k) 2_(_, and go to step MFW8, otherwise set A'" (k)- AX" (k) and i'+tl)(k ) = A'" (k) ) '2 ( ), and go to step MFW4. In algorithm MFW, the Pi and P2 must be chosen such that the former generates the inverse kinematics solution which does not satisfies the constraint limits in MFW2, while the latter generates the inverse kinematics solution which satisfies the constraint limits in MFW2. The forward search algorithm (Algorithm FW) gives us all the control set points on the straight line from the initial position to the break point. This portion of the trajectory constitutes the "acceleration" portion of the given straight line. Treating the chosen break point as an initial position and taking the same final position as before, the remaining control set points of the trajectory are determined using the modified forward binary search algorithm (Algorithm MFW). This portion of the trajectory constitutes the "deceleration" portion of the given straight line. 3.4.2. Existence of Straight Line Trajectory For the given straight line path, there is no general rule to guarantee the existence of the straight line trajectory with a acceleration portion followed by a

68 deceleration portion. As discussed in [201, the straight line trajectory between two specified end points may not exist if the average speed of the robot is beyond a certain limit. In fact, if the constraints on AX(k) from Eqs. (3.18) and (3.24) are not consistent ( i.e. qi;,,,, (k) > qi,, (k) or ri,,,, (k) > ri,, (k ) ), then the set point on the straight line path, which is based on the smoothness and torque constraints, does not exist at time t = kT. In other words, any joint value satisfying the Eqs. (3.18) and (3.24) cannot accommodate the hand position and orientation on the straight line path at time t = k T. Then, the algorithm FW or algorithm BW may stop before arriving at the final and initial locations, respectively. The failure of the algorithm FW or BW can happen for the following cases: (1) when the initial estimate AX'(k) generates the inverse kinematics solution which does not satisfy the smoothness and torque constraints; (2) when the algorithm FW or BW is not completed' by the global stopping criterion 61. Since either case will generate a large number in the loop counter m, the failure of the algorithm can be detected if the loop counter exceeds a prespecified number of the bisection process. Then a different planning must be used for a given straight line path. Here a procedure (Procedure ESLT) is presented to check the existence of straight line trajectory and to obtain a different trajectory for a given straight line. Procedure ESLT: Given a straight line path, this procedure addresses the existence of the straight line trajectory and proposes an alternative trajectory planning scheme. ESLT1. [Check whether the FW algorithm is completed or not.] Identify the failure of the algorithm FW. (When the loop counter m exceeds a specified number, apply this procedure to a given straight line path.)

67 If FW search points are completed and stopped by 61, then go to step ESLT2. otherwise go to step ESLT3. ESLT2. [Find a break point and apply the MFW algorithm.] Apply the procedure BR and the algorithm MFW, and stop. (Straight line trajectory planning is completed except for the relaxation portion.) ESLT3. [Apply the algorithm MFW from the final search point.] Apply the algorithm MFW from the final search point of the algorithm FW to the desired final point. If the algorithm MFW is completed and stopped by 61, then apply the procedure BR. ( from step BR2 to BR4 ) (Straight line trajectory planning is completed except for the relaxation portion. But it may not be a near-minimum time trajectory.) otherwise go to step ESLT4. ESLT4 [From the final search point of the MFW algorithm, start to apply the FW algorithm.] Take the final search point from the algorithm MFW as the initial point and go to step ESLT1. ( Take the initial estimated AX(k) of the final search point as the initial estimated AX(k) of the forthcoming algorithm FW in step ESLT1. ) The procedure ESLT generates trajectory set points with a combination of several acceleration and deceleration portions for a given straight line path. As indicated in the procedure ESLT, the resultant trajectory may require a longer traveling time than that of the trajectory with one break point for the given straight line path. The reason is that the procedure is performed several times by the algorithm FW, which is followed by the algorithm MFW from the final search point of the algorithm FW, or vice versa. The application of the procedure ESLT will be shown in Section 8.2.

8 3.4.3. Relaxation Points After finding and utilizing the break point in the MFW algorithm, the remaining control set points are obtained from the break point to the final point. We use a subscript g to indicate the information at the final search point. Then, the final search point, [ p(k ), $(k,) J from the algorithm MFW does not reach exactly the desired final position and orientation, [ p(k, ), $(k, )] because of the discrete time approximation, the iterative search algorithm and the user-designed stopping constant 61. Also, errors exist in the velocity and acceleration between the final search point and the desired final point. If this occurs, then intermediate set points called relaxation points, which satisfy the torque and the smoothness constraints, must be inserted between these two points. The velocity of the robot in the joint-variable or Cartesian space should be very small at the final search point because of the way in which the break point is computed. If I qi(kg)-qi(kft) I < T e/; i -, -, (3.75) where qi(kg) and qi(kl,) are the joint acceleration values at the final search point and the desired final location, respectively, then the time required to meet the acceleration conditions at the final location will be T with position and velocity errors. However, if the above inequalities are not satisfied, then the time required for the robot to meet any of the final conditions must be relaxed to satisfy the smoothness and torque constraints. Two approaches are discussed in the following: Discrete Time Approach. Since the final search point from the algorithm MFW is within the torque constraint, and the velocity at the final search point is close to zero, we consider the jerk bound only for the relaxation points. It is obvious that

89 one would like to obtain the smallest value of t that is greater than T while generating the set points satisfying the smoothness constraint. If qi(kt ) qi (kg) + wi, where wi is the acceleration difference between the final search point and the desired final point for joint i, then the required time t for the relaxation process is: t=max, 1 T, i = 1,.,n =m T (3.78) where r z 1 is the least integer greater than or equal to z and e/ is the jerk bound for joint i. Once the required time t is obtained, the relaxation points between p(kg ) and p(klt) can be generated to satisfy the smoothness constraint. The average change of acceleration in time t of joint i can be obtained as:. _lj tji. qi ( t )- = mnax ( r; T,;=1,**In } (3.77) and the average acceleration for each sampling period T is: t T -- * T X= max| r ti 1, = I n (3.78) For convenience, we denote the traveling time from the initial location to the break point as FT and from the break point to the final search point as f T. Restarting the time index from the final search point, the acceleration of joint i at t = jT is: q,(j) = q, (kg) + I ~ 3.7 wm ax a i Iod I E;1, 2,* dn (3t79) where j- 1,, m. During the relaxation period, qi(j) can be used to deter

70 mine qi (j) and qi (j) and the corresponding values of p(i), 4(j), v(j), i(j), v(i), and n(j). However, we will obtain an erroneous position/orientation, and linear and angular velocity/acceleration at the vicinity of the desired final location because of the relaxation process. From the above analysis, the total time required to travel the straight line with the specified end points is: Total traveling time (kit ~ T) =FT + f T + t ( F + f + m ) T (3.80) This total traveling time, which will be denoted as kf hereafter, is obtained based on all the physical constraints along the straight line with one break point. If more break points are used in planning the straight line path, the average speed of the manipulator in one servo time period will become smaller, which directly requires more servo time intervals to traverse the given straight line. While the discrete time approach generates the error between the actual final point and the desired final point, the joint interpolated trajectory planning scheme, which is now described, can eliminate this error by interpolating the final search point and the desired final point. Cubic Spline Function Approach. We discuss a joint interpolated trajectory planning scheme by using cubic spline functions. The trajectory planning scheme using cubic spline functions subject to the maximum allowable joint velocity, acceleration and jerk can be found in the literature [341. Some of the findings in [34] are presented here briefly. Since the cubic spline function fit is a joint interpolation technique, the continuous time index t instead of the discrete time index k will be used. Let t1 < t2 < t3 <... < t-_2 < tn-l < t. be an ordered time sequence. We set

71 tl - kg T and tm, - kit T, respectively. For the present development, the argument of q is the actual time t, that is, q(k, T) is q(k, ) in our notation. Then, from the final search point information and the desired final conditions, we have: q(tl) =q(k T); q(4t) = q(k T); q(t) -- q(k T) (3.81) and q(t,) q(kft T); q(t,)= q(kt T); q(t.) q(klt T) (3.82) In addition, joint positions q(t3), q(t4),...,q(t.-2) must be specified for the joint trajectory to pass through. Thus, we can assign the joint position values as: q(t ) = j- ( q(k ) - q(k, ) ) + q(k ) (3.83) where j = 3, *,-n-2. But q(t2) and q(t,_l) must be free as extra knots for obtaining the cubic polynomial functions. Let Ci,;(t) be a cubic polynomial function defined on the time interval [ti, tj+l] for joint i. The problem of trajectory interpolation is to spline C,i(t) for j = 1, 2, - *, n-l, together such that the required joint position, velocity, and acceleration are satisfied, and the joint position, velocity, and acceleration trajectories are continuous on the entire interval [tl, t, ]. Because Ci, (t) is cubic, the second time derivative C,i(t ) can be expressed as C2i(t) — 'j+- ci 2i(ti) + h - C2,i(t +l) (3.84) where i =1,2, ',~, j 1, 2, ',n-1, and hi =t+l - tj. By intergrating Eq. (3.84) twice and utilizing the joint position values, Ci,(t,) can be derived. Since the cubic polynomial functions must be constrained by the maximum velocity, acceleration, and jerk limits for each joint, we have:

72 I C Ij(t) I e I CO(t) I <I, (3.85) I C,3,(t) I< ( where C?3(t) denotes the third time derivative of the cubic polynomial function. We denote the total traveling time as ht, where hit = 1+ * + hA. By developing an optimizing algorithm, the total traveling time Ah for the relaxation portion has been optimized under the constraints of maximum velocity, acceleration, and jerk limits. Due to the cubic spline function fit, there are no errors in joint position, velocity and acceleration between the actual final point and the desired final point. However, we must calculate the coefficients of the cubic spline functions for the relaxation portion of the straight line trajectory. The resultant trajectory set points, obtained by evaluating the cubic spline functions, yield joint values, which satisfy the smoothness constraints but are not on the desired straight line path. Also, the total number of servo time intervals for the relaxation portion can be determined from dividing the total time At by the servo time interval T. Usually, more servo time intervals than m will be required at the expense of fitting the boundary conditions at the final location. A more detailed derivation can be found with related simulation results in the reference [341. After getting q(k), p(k), and $(k) from the search algorithm followed by the relaxation process, they can be used to determine v(k) and f((k) using the kinematic equation in Eq. (2.5). In summary, the whole procedure to obtain the straight line trajectory can be stated as follows:

73 Overall Trajectory Planning Procedure Step 1: Given a straight line path specified by two end points, apply the algorithms FW and BW. If the algorithms FW and BW are completed successfully, then go to step 2, otherwise, go to step 3. Step 2: Apply the procedure BR to obtain the servo control points of the acceleration portion and the deceleration portion, and go to step 4. Step 3: Apply the procedure ESLT to the given straight line and go to step 4. Step 4: Obtain the relaxation points from the discrete time approach or the cubic spline function approach. (Trajectory planning has been completed.) A block digram showing the generation of Cartesian control set points of a given straight line is shown in Figure 3.2.

IForward Forwar'~d Acceleration Set Point$ L~~~rX' Slt~~~et-olut set polatts (1) Sc~~~Serch Algorithma Deceleration Ut Pointer p(k 0) 0(kI I Modified Forward Relaxationlo Straight 4rlt ~~Pl S(PoD~ r~co Line 9arch se oit Algorithmo Ax, 0) Sea~rrch cai'(o of (V, ~ Algorithm fltk) V~~~rr rr k(); Figure 3.2 Cartesian Trajectory Planner

75 3.5. Summary This chapter presents a scheme in straight line trajectory planning subject to the various constraints. The scheme is based on the discrete time trajectory analysis. A real time servo interval instead of a normalized time interval is used for planning the manipulator trajectory. Because of split constraints in joint and Cartesian coordinate systems, the set points are determined by maximizing the path length ratio between two consecutive Cartesian control set points subject to the smoothness and torque constraints and the straight line requirement. An iterative forward search algorithm is developed to determine the control set points from the initial position to the break point on the straight line. A modified forward search algorithm is then developed to determine the control set points from the break point to the vicinity of the final point. The final search point is found to be very close to the desired final point. However, relaxation points, which do not satisfy the straight line requirement, may be necessary if these two points exhibit a large acceleration difference. Thus, the straight line trajectory is composed of three segments: the acceleration portion, the deceleration portion, and the relaxation portion. The existence of the straight line trajectory is also discussed. Computationally, the proposed trajectory planning scheme provides a simple method of determining the control set points, which is different from conventional methods that require the determination of the joint interpolation function followed by the evaluation process. Simulation results for planning a straight line trajectory are presented in Chapter 8. Finally, Chapter 5 describes how the resultant trajectory set points are used for the Cartesian space control.

CHAPTER 4 PLANNING OF COLLISION-FREE MANIPULATOR PATH 4.1. Introduction In this chapter, we discuss a method for obtaining collision-free paths of two robots in a common workspace. The planning of collision-free paths of two robots is considered in two classes: one robot is stationary and the other robot is moving; and two robots are moving simultaneously. The original pre-planned paths of two robots are composed of straight line paths. It is assumed that no collisions occur at the initial and final locations of the straight line paths between the robots. It is also assumed that the straight line trajectories of two robots are planned by the trajectory planner discussed in Chapter 3. Notations for geometric analysis are presented and several performance measures are discussed in Section 4.2. Using a sphere model to represent the wrists of each robot, a collision-free path for a moving robot in the presence of a stationary robot is found which consists of connected straight line segments. For the class of two moving robots, various collision situations are identified and discussed in Section 4.3, where notions of collision map and time scheduling are introduced. Two procedures are developed to obtain collision-free paths and/or trajectories in Section 4.4. Finally, summaries are presented in Section 4.5. 78

77 4.2. Collision-Free Path for the Case of a Stationary Robot 4.2.1. Overviews and Notations In this section, the collision situation between a stationary robot and a moving robot is investigated using a geometric analysis technique. The analysis assumes that a straight line trajectory is known for the moving robot and that the wrist of each robot is modeled by a single sphere. Several performance measures are considered and necessary notations are presented. In Figure 4.1, a moving robot ( say robot 2 ) having a wrist of radius r2 collides with the wrist of radius r of a stationary robot ( say robot 1 ) during the time interval from t = mT to t - nT. That is, no collision occurs before t = mT and after t = nT. The original straight line path WZ of robot 2 must be replanned to avoid wrist collision with robot 1. The curve from point X to point Y is a curve, which robot 2 must follow for the collision avoidance with minimum deviation from the original path WZ. We call this a collision-free curve. Let us denote Q as the origin of the sphere of robot 1 with respect to a fixed global coordinate frame. Then, the distance from Q to the origin of the sphere of robot 2 must be at least rl + r2 for the wrist collision avoidance. The locus of the distance of r 1 + r2 can be realized as a curve ( or an arc ) of the "pseudo-sphere" ( or colliding sphere ) surface of radius r1 + r2. Depending on the performance measure selected, this curve will be approximated by finding and connecting via points, which can be determined subject to the allowable deviation error from the collision-free curve. Straight line segments will be used to approximate the curve. Several performance measures can be considered for measuring the deviation of proposed paths from the path WX, XY ( along the

t < T robot '2 robot 1 \ 4 1 dr2 ~ r000 YO Xo rLixed global coordinate frame Figure 4.1 Stationary Robot Avoidance

79 collision-free curve ), and YZ. They are: (1) Minimum Deviation from the Collision-Free Curve Minimization of the maximum deviation error from the collision-free curve XY while constraining the path to lie on the portion of original path between WX and YZ (2) Minimum Deviation from the Original Path: Minimization of the maximum deviation error from the path composed of the straight line segments WX, YZ and the curve segment XY (3) Minimum Spanned Area: Minimization of the area enclosed by a collision-free path and the original path (4) Least Squared Distance Error: Least squared distance error calculated at each servo interval between the original path and a collision-free path (5) Minimum EnerZr: Minimization of energy for traveling along a collision-free path (6) Minimum Time: Minimization of traveling time for a collision-free path Among the various possible measures, the first two measures ( we call them Measure (1) and Measure (2) ), are utilized to obtain a collision-free path in later sections. The application of some other measures can be found in the literature [8,19,34,381. In order to carry out the geometric analysis for collision-free path planning, necessary notations of a plane, a line, and the direction number of a straight line are presented in the following. A plane in a 3 dimensional space is described as: alz + bly + clz - d= (4.1)

80 In general, a line is generated by two intersecting planes in the 3 dimensional space. Since a plane is represented by Eq. (4.1), a line can be represented as: iaz + bly + c1:z dl (402) 422 + b2y + c2z = d2 The direction number of the line described in Eq. (4.2) can be written as ( see Appendix E ): [ blc2- lb2, e1a2- a1c2, a1b2- bla2 ] (4.3) The direction number of the line will be used in the next section to analyze the geometry of the collision situation. A point Q is represented with its coordinates as Q 1( zQ 1, yQ 1 z ), throughout this chapter, where the subscript Q 1 denotes the point Q 1. The distance d between the points Q1 and Q2 is obtained from the norm of the vector subtraction and represented as: d = 10Q O1Q - oQ2 I (4.4) where the point 0 is the origin of the global coordinate frame. 4.2.2. Planning of Collision-Free Path by Measure (1) In this section, the notations in Section 4.2.1 are utilized to plan a collision-free path for a moving robot in the presence of a stationary robot. From the measure (1), via points around the curve are selected so that a path that passes through these via points will approximate closely the collision-free curve XY in Figure 4.1. Tangential cut planes around the pseudo sphere are derived to determine these via We use the notation Q i(z2 1!q Ye Zq i) to represent a point Q 1 in the Cartesian space expressed with respect to the global coordinate system with the coordinate (zq 1, 3VQ,' Zq i).

81 points. As a result of connecting these via points, the collision-free curve XY is approximated by connected straight line segments. In Figure 4.2, a collision situation is shown, where the point M is a point on the original path of the moving robot ( robot 2 ) at time t = mT, the point N is a point on the original path of the moving robot at time t - nT, the point Q is the origin of the wrist model of the stationary robot and the point O is the origin of the global coordinate frame. We assume that the time t =- mT is the time at which a potential collision would fist occur and the time t = nT is the time at which a potential collision would last occur. Since the potential collision would occur at time between t = (m -1)T and t = mT, it is desirable to find a potential collision starting location. Similarly, a potential collision ending location must be found. Let us denote the path of robot 2 as: h1z + h2Y + h3z = h4 k1z + k2y + k3z- k4 and the equation of the pseudo-sphere with radius rl + r2 discussed in Section 4.2.1 as: (z - Q)2 + (y - yq)2 + (z - zQ)2 = (r, + 2)2 (4.6) where Q (zq, yqt z ) is the origin of the stationary robot 1. Then, the actual collision starting point A and the actual collision ending point B can be obtained by solving Eqs. (4.5) and (4.6) simultaneously. The point A can be interpreted as the point at which robot 2-must deviate from its original path for collision avoidance and the point B as the point after which robot 2 must continue to move on the original path. We obtain the tangential plane to the pseudo-sphere at the point A (ZA, YA, ZA ), called TPA, which is represented as:

82 (2-ZA XZA-ZQ ) + (y-yA XyA-YQ ) + (z-zA XZA -z: ) =- (4.7) Similarly, the tangential plane to the pseudo-sphere at the point B(ZB, YB ZB), called TPB, is obtained as: (2-2B X2Ba-zQ ) + ( -yB XYB-yQ ) + (z -ZB XB -:Q ) 0 (4.8) The intersection line between these two tangential planes is obtained by Eqs. (4.7) and (4.8). That is, the intersection line is described by: (Z-ZA XZA -ZQ ) + (Y-VA XYA -Q ) + (Z-ZA XZA -ZQ ) = 0 ( -Z XB -ZQ) + (Y-~Y Xys-q ) + ( -ZB XZB -Z) = o Let us use C to denote the point from A to the intersection line and C2 to denote the point from B to the intersection line, which are all in the perpendicular direction to the intersection line. That is, if we use the notation SS' to indicate the intersection line, then AC1 is perpendicular to the line SS' and BC2 is also perpendicular to the line SS'. It can be shown that the points C1 and C2 are the same points if two planes are the tangential planes of a sphere. By definition, the vector QA is perpendicular to the tangential plane TPA and the vector QB is perpendicular to the tangential plane TPB. Since the intersection line SS' is on the planes TPA and TPB, it is also perpendicular to the vectors QA and QB. This results from the fact that the line SS' is in the same direction as the normal vector to the plane which is uniquely determined from the point Q, and vectors QA and QB. We denote this plane as TPQ. Then any lines from the points A and B, which are perpendicular to the line SS' and meet the line SS' exist on the plane TPS. Therefore, the points C1 and C2 will coincide at a point C, which is the intersection point between the line SS' and the plane TPq. From

S I,~~I PI~~I.0~~~~~~~ 04iu.2 aetluPn.0op ~ ~ i CC t moo c1C %ho~ ~ ~ ~~~h TPo Figure 4.2 Tangential Cut Planes

84 now on, we will utilize the tangential lines instead of the tangential planes due to the fact that all the via points are on the plane TPO. Since the direction number [as, pis, s of the line SS' is obtained from Eqs. (4.3) and (4.9) as: as - (VQ-YA )(ZQ-ZB ) - (ZQ -ZA XYQ B ) =s - (zq-zA XQ-ZB )- (ZQ-ZA XZQ-ZB ) (4.10) 's = (-Z -zA )(Q -YB) - (YQ -A X Q-28 ) the plane TPQ is described as: aS(, -,2 ) +/ S(Y - vQ) + 7s(Z Z -,) 0 (4.11) The point C can be determined by solving the line SS' equation (Eq. (4.9)) and the plane TPQ equation (Eq. (4.11)) together. However, since the point C is on the plane TPQ, a simpler method will be utilized to find the point. In fact, the collision-free curve, which is the intersected curve of the pseudosphere and the plane TPQ, can be obtained by solving Eqs. (4.9) and (4.11) simultaneously. If the path of robot 2 described by Eq. (4.5) becomes closer to the origin of the pseudo-sphere, then the deviation of the via point C from the collision-free curve will increase. Then the approximation of the path ( A to B 'via C ) to the collision-free curve ( on the pseudo-sphere ) will be difficult to justify. In the ultimate case, if the path of robot 2 passes through the origin of the sphere of robot 1, then the two tangential planes TPA and TPB will not intersect each other. Thus, additional via points will be required to avoid these situations and to get a better approximation. Two via points can be obtained by inserting an intermediate tangential plane at a point Pi as in Figure 4.2.

85 The determination of via points requires finding tangent points on the collision-free curve, where tangential planes or tangential lines to the pseudo-sphere can be found. Since one intermediate tangential line will generate two via points, two intermediate tangential lines will generate three via points, and so on, the selection of c intermediate via points (the number of the via points is determined from the allowable deviation error of the path) to approximate the collision-free curve will require c - 1 intermediate tangential lines. The points on the collision-free curve, where the tangential lines to the curve will be found, are denoted as Pi, where i = 1, 2,..., c - 1 as in Figure 4.3. The points P. can be obtained by rotating the point A around the normal vector of the plane TPQ by an angle of i, where the C angle d is the angle between QA and QB and i = 1, 2,..., c - 1. Notice that the points Pi when i = 0O and i = c correspond to the points A and B respectively. Let us denote the normal unit vector of the plane TPQ as ONq. The vector ONQ -= ((n, ny, n )T is obtained as: IQA XQB ON< i IQA X QB (4.12) where X denotes the vector crow product. Then, the tangent points can be found as [7,42]: -_ RN- _R OPji e C QA + OQ (4.13) = (Icos(- )+ ONQ ON(1 - cos(- )) + RN;i-C(-) QA + OQ (Icaa(;Ce C where I is a 3X3 identity matrix, i - 1, 2,..., c - 1, 0 is the origin of the global coordinate frame and

88 -nz ny RN n, 0 -n 1 (4.14) -n ns O To find the via points, we furst find the tangential lines at Pi to the collisionfree curve, and then determine the intersection points. Since every via point is on the same plane TP, they can be obtained by adding two consecutive QPi vectors and scaling appropriately. A via point Vi is obtained by adding two vectors QP, and QPi,1 and taking the magnitude as: II Qv II (rl+ r2) sec(-)2 (4.15) where i = 1, 2,..., c - 1. That is, the vector QV, is represented as: qPi + QPi-I &Qv = ( g ) (rl + r2) sec() (4.16) II QP +IQPll I 2c where i = 1, 2,..., - 1. Therefore, the vector O Vi is: OVi =- QVi + OQ (4.17) and the deviation error of the via point V; from the collision-free curve is: d = (r, + r2) (aec (- ) - 1) (4.18) If the maximum allowable deviation error is d1, then we have: (r, + r2) -(e ('C) - 1) < d, (4.19) Thus, the required number of via points c for the maximum allowable error d1 is obtained as:

Z90 or~~~~~~~~~~~~g~~z Figure 4.3 Rotation Of VIt~ Y Figlure 4.3 Rotatinofao rVetor

88 1 c >. 1 2 l(, + r2 (4.20) rl + r2 + dl From Eq. (4.20), it is found that if the maximum allowable deviation error de approaches to 0, then the required number of via points, c, will approach infinity, which results in the collision-free path the same as the collision-free curve on the pseudo sphere. The decision of how many via points must be used to approximate the pseudo-sphere curve is up to the allowable deviation error and the path accuracy for the collision avoidance purpose, and the user has the freedom to decide it. 4.2.3. Planning of Collision-Free Path by Measure (2) From the Measure (2) in Section 4.2.1, a collision-free path is derived and obtained around the original path and the collision-free curve by approximating the path and the curve within the allowable deviation error. Given the allowable deviation error from the curve, the number of necessary via points are calculated to derive the collision-free path. In Figure 4.4, assume that d2 is the allowable deviation error of the collisionfree path from the collision-free curve and c is the required number of via points corresponding to the deviation error d2. Also, point C is the intersection point between the tangential plane to the pseudo-sphere at point P1 and the original robot 2 path. Similarly, point D is the intersection point between the tangential plane to the pseudo-sphere at point P, +1 and the original robot 2 path. The angle between QA and QB is denoted by 0 as before. Since the angle between QA and QP1 is denoted by 6 and the radius of the pseudo-sphere is rl + r2 we have: d= (ri + r2) * (1 - cos6) (4.21)

89 which leads to = cos1( r + r2 - d2) (4.22) rl + r2 Once the maximum allowable deviation error, d2, is given, the angle 6 can be obtained and used to find the points P1 and P,+1 by using Eq. (4.13) with replacement of the angle of the trigonometric function by 6 and g - 6, respectively. Thus tangential planes at points PI and P,+I can be derived and used to find the points C and D. The angle between QP1 and QP,+1 is denoted by qi and can be obtained as: q' = - 2 ~ 6 (4.23) Recall that the deviation error from c intermediate via points is obtained from Eq. (4.18). Since the maximum deviation error must be equal to or less than d2, we have: (r + r2) sec(( )- 1) d2 (4.24) Combining Eqs. (4.22)-(4.24), we have: c > cs + 1 2 Cof-1( l + r2 (4.25) r, + r2 + d2 From Eq. (4.25), it is found that if d2 approaches to 0, then, the number of required via points c will approach infinity, which results in the collision-free path the same as the collision-free curve on the pseudo-sphere.

Pt IP"IOI - P I~~~~~~~~~~~~~~~~~~~~~~ -OQ Q Figure 4.4 Geometric Analysis For Deviation Error

91 4.2.4. Comparisons and Discussions Methods for obtaining collision-free paths for a stationary robot and a moving robot have been discussed by approximating the collision-free curve with connected straight line segments. Depending on the selected measure, the number of required via points are derived in Eqs. (4.20) and (4.25) for the given allowable deviation errors. For a specified maximum deviation error, it is found that the Measure (2) requires fewer via points for the approximated collision-free path. Ultimately, if the path of robot 2 passes through the origin of the pseudo-sphere, then the difference of the required number of via points will be the maximum for the specified allowable deviation error. Another aspect is the total required traveling time of the collision-free path. Since the Measure (2) requires fewer via points for the specified allowable deviation error than the Measure (1) does, the total traveling time for the path from the Measure (2) is likely to be shorter than that from the Measure (1). Since the robot must stop and go at each of the intersection points of c connected straight line segments, the traveling time in both Measures will be longer when a more accurate approximation to the collision-free curve is obtained. 4.3. Time Scheduling and Collision Map In this section, we discuss notions of time scheduling and collision map for finding the collision-free trajectory for two moving robots. The time scheduling is a procedure to modify and reschedule the traveling speed along the pre-planned trajectory for the purpose of collision avoidance. In a broader sense, time scheduling also implies time delaying the motion of one of the robots. Thus, the robot motion may slow down or be delayed as a result of the time scheduling of the trajectory.

02 The collision map is a two dimensional figure, in which we can incorporate both the path and trajectory information of two moving robots simultaneously. The time scheduling procedure is used with the collision map in obtaining collisionfree trajectories for two moving robots. 4.3.1. Case I and Case 2 Collision Situations As described in Section 2.5, methods in obtaining the collision-free path may vary depending on the various collision situations when two robots are moving on the straight line paths simultaneously. Since trajectory information from Chapter 3 is utilized, the discrete time index is used here instead of the continuous time index. For example, we use kl and ko instead of t1 and to to denote the final arrival time and the initial starting time of the robot, respectively. Only cases 1 and 2 as discussed in Section 2.5 are considered in this section. In case 1, the final arrival time kf of robot 2 can be relaxed but the original straight line path cannot be altered for the collision avoidance purpose. Thus, we can only change the speed or delay the motion of robot 2 along the original path to avoid the collision. Since the change of the robot speed can only be accomplished by modifying the trajectory information, the time scheduling procedure will be applied to obtain the collision-free trajectory of robot 2. The trajectory set points from the trajectory planner are obtained based on the physical constraints of the robot, thus the time scheduled trajectory is obtained with the smoothness and torque constraints taken into consideration. Details about the time scheduling for case 1 collision situation are discussed in Section 4.4.1. In case 2, we discuss a collision-free path of robot 2 in the following categories: (1) when time scheduling is applied without any path modification; (2) when only

93 path modification is applied without considering the trajectory information of two robots; (3) when path modification and time scheduling are applied simultaneously. In category (1), a collision-free path can be found exactly in the same way as in case 1. If a solution from category (1) is not adequate because a fairly large time delay is required and speed reduction is not appropriate for the collision avoidance (we will show this later), then we can consider a solution in categories (2) and (3). In category (2), a collision-free path can be found through a geometric analysis. To reduce the unnecessary path deviation from category (2), path modification and time scheduling can be applied simultaneously. In category (3), we apply path modification and time scheduling at the same time. There exists an infinite number of collision-free paths and trajectories in this category. A method is presented for planning a collision-free path with path modification and time scheduling of the trajectory subject to the performance measures selected. Detailed derivation will be presented in Section 4.4.2. In the next section, the general concepts of time scheduling of a straight line trajectory are described. These concepts are then used to obtain the collision-free trajectory for cases 1 and 2. 4.3.2. Time Scheduling of Straight Line Trajectory In Figure 4.5, assuming that a straight line path whose trajectory set points are obtained from the trajectory planner discussed in Chapter 3, A is the initial location, C is the final location, ko is the initial time and kf is the final time of the robot. We further assume that the given path can be planned with a break point. By applying the algorithms FW and MFW together with the procedure BR to the given straight line path, we obtain the trajectory set points along the path with an

94 acceleration portion and a deceleration portion. We denote this break point B at time kB on the path. Then, during the time interval [ko, kB ], the manipulator hand is in the accelerating motion and during the time interval [kB, k/ ], the manipulator hand is in the decelerating motion. Since the set points in the relaxation portion of the trajectory are not on the given straight line path, we shall plan the trajectory without the relaxation portion of the trajectory. From the procedure BR, the break point B can be selected uniquely such that the position and Euler angle errors between the final search point and the desired final point are the smallest. In order to reduce the speed of the robot or increase the traveling time along the path, additional break points are used at appropriate locations on the path. Thus, referring to Figure 4.5, if the robot starts the decelerating motion from a point D, which is located in the acceleration portion of the straight line AC, then the decelerating motion will end up at some point E after applying the algorithm MFW with modified stopping criterion and a different starting point D, which results in longer traveling time of the robot from point A to point E than that from the original trajectory information. As a result, the robot will be in the accelerating motion on the portion AD and in the decelerating motion on the portion DE. From point E in Figure 4.5, we can determine another break point F for an acceleration portion EF and a deceleration portion FC. That is, if we use 3 break points ( points D, E, and F ) for the trajectory planning of the straight line path, then the location of the rust break point D will determine the whole time scheduling with the locations of the break points E and F.

A D B E F C V Figure 4.5 Initial Path and Trajectory Planning Figure 4.5 Initial Path and Trajectory Planning

98 If the point E is taken as a break point, then the segment AE in Figure 4.5 can be regarded as a single segment and the trajectory planning of the segment corresponds to the time scheduling of the original trajectory. Thus, an immediate objective of time scheduling is to find segments such that the whole trajectories of the segments can avoid the potential collisions. Also, the final portion of the straight line must be in the decelerating motion to reduce the final position and Euler angle errors. Thus, the total number of break points on a straight line path must be odd. The trajectory planning of a straight line path with one break point results in near-minimum time for traversing the path as discussed in Chapter 3. Thus, if we have more segments for a path, then the total traveling time will be greater than that with a break point because the average traveling speed of the manipulator hand will be much slower than before. However, we must also note that the number of all the possible combinations of the acceleration portion and the decelera. tion portion may be very large. In the limiting case, we can apply the algorithm FW and the algorithm MFW to every two trajectory set points. Hence, the time scheduling of a straight line trajectory is a procedure in achieving the required "time delay" or "speed reduction" of the robot motion for the collision avoidance purpose. In the next section, we shall present a collision map which incorporates the location and the corresponding servo time information of the robot into a two dimensional figure. This will be used in obtaining the collision-free path or collisionfree time scheduling of the robot in Section 4.4.

97 4.3.3. Collision Map Since the collision-free paths of two moving robots are related not only to the path, but to the trajectory information, a notion of a collision map is introduced in this section. It is useful in obtaining the collision-free paths or the collision-free trajectories of the two moving robots. The collision map is obtained from the path and trajectory information of two moving robots. From the trajectory planner in Chapter 3, the traveled length X(k ) Ito,I, and the corresponding servo time instant t - kT of robot 2 can be obtained along a straight line path. Using this information, a curve, which relates the traveled length with corresponding servo time instant of robot 2, can be drawn. See Figure 4.8, where the vertical axis is the traveled length and the horizontal axis is the servo time. Two moving robots have the potential for colliding with each other under the original trajectory information, if there is a range of collision lengths where the path of robot 2 is within the colliding range of a point on the path of robot 1. The union of these collision lengths at the collection of servo time instants determining the points on the path of robot 1 can be drawn as a connected region. See Figure 4.6. More precisely, the collision length at time k corresponds to all points on the path for robot 2 that are within r I + r2 of the point that lies on the path of robot 1 at time k. If the traveled length versus servo time curve touches or crosses the region, it indicates that a potential collision of the wrists of robot 1 and robot 2 exists. Now, we present a method to obtain the collision region in more detail. Assume that two robots are moving in a straight line fashion as shown in Figure 4.7, where robot 1 has to move from point A ] to point B1 and robot 2 has to move

98 _g~~ |I~ / a~ curve for robot 2 p = 0 k servo time Figure 4.8 A Collision Map

99 from point A2 to point B2. Note that paths of A 1B1 and A 2B2 are generally not on a common plane. Since robot 2 moves on the straight line path A2B2, a potential collision between the robots may occur if the sphere of the the wrist of robot 1 intersects the sphere of the wrist of robot 2 during their motions. The distance between two trai jectory set points on the robot 1 and the robot 2 paths must be greater than r 1 + r 2 for the collision avoidance. The portion that must be avoided by robot 2 at time k can be determined from the intersection of the sphere of radius of rlI + r2, centered at the point on the path of robot 1 at time k, and the original robot 2 path. The intersected length, however, must be computed for all the servo time instants of potential collisions. 'With reference to Figure 4.7, point pl(k) is a point on the robot 1 path at time k. The equation of the robot 2 path is denoted as: P2 = p2(k0) + X ( P2(k/ )- P2(k0) ). (4.26) Since the potential collisions between the robots occur at time k when the distance between point pl(k) on the robot 1 path and the robot 2 path in Eq. (4.26) is less than or equal to r I + r 2i we solve the following equation: (r1 + r2)2= I I pl(k)-p2 1 12 (4.27) Using Eq. (4.26) for P2, we have: (rl + 2)2 - { pl(k)- P2(k0)- X (P2(k )- p(k0)) }r (4.28) (4.28) { pl(k) - p2(ko) - X ~ (p2(k )- p2(k)) } More explicitly,

B2 Al I Di II e, I %.I e. ~w ootPt

101 (rl + r2)2 = [ pl(k)- p2(k0) 12 _ 2X (pl(k) - p2k0))r(p1(k )- p2(ko)) (4.29) + X2 I I (p(kf )- p2(k0)) I 12 Eq. (4.29) is a second order equation for X, which can be solved easily. There are three possible solution cases: (1) Real roots of X do not exist, (2) Two real roots XI(k) and X2(k) exist ( Xl(k)> X#(k). ), and (3) Only one real double root Xl(k) exists. When no real root exists, we know that there is no collision between robot 1 and robot 2 at time k. When two real roots exist, we know that the collision lengths range from i/ ~ X1(k) to I ~ X2(k ). When only one real double root exists, it marks the beginning or ending times for the collision region. Since the Cartesian trajectory planner discussed in Chapter 3 yields the trajectory information for robot 1 based on the servo time interval, the collision region in the collision map can be obtained by calculating the intersected lengths of the path A2B2 for all the servo time instants of potential collisions. It is worth noting that the same analysis applies if the path of robot 1 is a series of straight line segments. Several boundary points are interpreted in the collision region in Figure 4.8. The points A', B', al1 and b1' in Figure 4.8 correspond to the points A, B, al and bI in the collision map respectively. For example, we can interpret that the portion albl of the collision region is the intersected length or the intersected portion of the robot 2 path, which must be avoided by robot 2 at time t = ki T. The collision region is drawn in Figure 4.9 as a shaded region. The curve of traveled length versus servo time for robot 2 cannot enter the region if collision is to be avoided. Since it is difficult to describe the geometric shape of the collision region exactly and analytically, a bounding box or a "window" which encloses the whole collision region is established to "approximate" the collision region as shown

C. a I t I S, g;I I Sb II I I ~ i I I=~~~~~~~~~~~~~- S I I mmmmmmm~~~~~~~~~~ ~ mm m mm 'U i~~~~~~~~~~~ Ime I m 01 0~~~~~~~~~~~~~ 2 II I m~~~~~~~~~~~~~~~~~~~~~~~C

103 in Figure 4.9. From now on, we will denote the potential collision starting and ending time as k, and k6, respectively, and the initial and final time as k0 and k, respectively. Also, we denote the traveled length from point A2 to the collision starting point and to the collision ending point as l, and 4,, respectively, and the total traveling length of the path of robot 2 as If. Various collision maps can be identified depending on the collision situations. Figure 4.10 shows the collision maps which could happen in various collision situations. In Figure 4.10-(a), the collision occurs for most of the path of robot 2 for a long time interval and in Figure 4.10-(b), the collision occurs for most of the path of robot 2 for a short time interval. In Figure 4.10-(c), the collision occurs for a small portion of the robot 2 path for a long time interval and in Figure 4.10-(d), the collision occurs for a small portion of the path of robot 2 for a short time interval. If l, = 0, then this corresponds to a special case of Figures from 4.10-(a) to (d). In this case, a different solution to avoid the collision region will be discussed in the next section. In fact, the traveled length versus servo time curve and the collision region are obtained from the path and trajectory information of two robots, which result from the application of the iterative binary search algorithms in Section 3.3. Thus, the shape of the collision region varies depending on the collision situation. We assumed only a simple collision situation in this section to illustrate the collision map. Multiple collision regions may occur in some collision situations. It is anticipated that a bounding box enclosing the whole scattered collision region may result in rough approximation. However, the discussions presented here can be applied to the case of multiple collision regions also. In the next section, we will utilize the col

a~~~~~~~~~~~~~~~~~~ a~~~~~~~~~~~~ I *~ I 50i ft.1 'O~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ ac~~~~~~~~~~~~~~~~~~~~~~~~~

105 lision map to analyze and design the collision-free trajectories for two moving robots.

Is -. C, ( I ~~~~~~~~~~~~~II r~ ~~ —t7~L --- —L-. I.., o k( kgk k o kv kg kf k (C( Fisure 4.10 Seveml Typw of Co R I r~~~~~~~~~~~~~~~~~~~~~ 5 ---- -- I I~~~~~~~~~~~~~~~~~~~~~~~ I Ok (c) kt k, ko k9 k, kc (d) Ftgure 4.10 Sevra~l Iypm of Collision Region

107 4.4. Collision-Free Path for Two Moving Robots Using the collision map and the time scheduling concept, this section presents a method to realize a collision-free motion planning of two moving robots. The collision situations of case 1 and case 2 are discussed separately. 4.4.1. Collision-Free Path Planning for Case 1 In case 1, one robot ( robot 2 ) cannot change its original path but can change its arrival time kf for collision avoidance with the other moving robot ( robot 1). The collision map is utilized in obtaining the collision-free time scheduling of the straight line trajectory. From the collision map of a collision situation as shown in Figure 4.9, the final arrival time kf of robot 2 can be altered for the purpose of collision avoidance with robot 1. Since the potential collision starts at time k, from the collision map, the time for robot 2 to travel the length l, must be modified so that robot 2 reaches the location of length 1, at a different time. Thus, to find the collision-free time scheduling of the trajectory, we need to find a traveled length versus servo time curve of robot 2 which does not cross or touch the collision region in the collision map. In Figure 4.11, it is assumed that the traveled length versus servo time curve OM crosses the collision box, which indicates a potential collision. Since the objective is to find a curve like OM' which avoids the collision region, it is necessary to make robot 2 slow down or delay its motion. The arising problems in time scheduling of the original trajectory are then identified as follows: * Is either a time delay or speed reduction of the robot 2 motion necessary

m m m - mm - mm m m m mm m m m 'I 1 — ----- ojI OAIV ~I I ~ I IM I I g I I 0 I I -- I Figure 4.11 A Collision Map with Potential Collisions

109 for avoiding potential collisions? ~ How much must the robot 2 motion be delayed and where must the time delay must be incurred? * How much must the speed of robot 2 be reduced and where should it occur? * What are the effects of the time delay and speed reduction on the final arrival time? These problems can be addressed easily by using the collision map from the collision situation. We assume that a collision map with potential collision between two robots is given, in which the traveled length versus servo time curve of robot 2 crosses the collision region. Depending on the task scheduling requirements and the environment of collision situation, various approaches are discussed in obtaining the traveled length versus servo time curve of robot 2 which can avoid the collision region. In Figure 4.12, it is assumed that the collision region starts at the location of length 1, at time k, and ends at the location of length 1, at time k,, respectively. Also, we denote the final arrival time of robot 2 as kl from the original trajectory information. Figure 4.12-(a) shows the traveled length versus servo time curve OM1 which can avoid the collision region by purely delaying the starting time at the initial location. Figure 4.12-(b) shows the traveled length versus servo time curve OM2 which can avoid the collision region by delaying the starting time at the initial location and reducing the speed of the robot 2 motion. Figure 4.12-(c) shows the traveled length versus servo time curve OM3 which can avoid the collision region by delaying the robot 2 motion at the location of length 1, and reducing the speed of

110 the robot 2 motion. Figure 4.12-(d) shows the traveled length versus servo time curves OM4 and OMS which can avoid the collision region by reducing the speed of the robot 2 motion. In the above discussion, we assumed that the collision region is such that 1, 7 0. If l, O0, then the only possible solution to avoid the collision region is to delay the robot 2 motibn so that robot 2 starts to move after any potential collisions are cleared out on its path. In Figure 4.12-(a), the curve OM1 is a curve which avoids the collision region and can be obtained by purely delaying the starting time at the initial location. The time, when robot 2 arrives at the location of length I, from the original trajectory information, is denoted as k 1. Then, the required time delay at the initial location will be k. - k1. The arrival time klI of robot 2 for the curve OM1 will be kf + ke - k 1. However, if robot 2 has any constraints to leave the initial location as scheduled, then this curve OM1 cannot be used for collision avoidance with robot I and a different method, which utilizes speed reduction of the robot 2 motion, will be required. In Figure 4.12-(b), the curve OM2 is a curve which avoids the collision region and can be obtained by slowing down the robot 2 motion and delaying the starting time at the initial location. The speed reduction of robot 2 is performed on the path of length I, from the initial location. The time, when robot 2 arrives at the location of length 1, after speed reduction, is denoted as k2. If we allow the robot to accelerate and decelerate again, the corresponding traveled length versus servo time curve for the segment of length Il - I, may touch or cross the collision region again. In this case, the time delay strategy can be realized at the initial location to

111 avoid the collision region. The required time delay at the initial location will be kg - k2. The arrival time k1 of robot 2 for the curve OM2 will be k! + k, - k2, where k! is the arrival time of robot 2 for the curve OM2' only after speed reduction. If k, < k2, then the speed reduction on the path of length I, is sufficient to avoid the collision region. In Figure 4.12-(c), the curve OM3 is a curve which avoids the collision region and can be obtained by slowing down the robot 2 motion and stopping the movement temporarily at the location of length 4, to cause the required time delay. The time, when robot 2 arrives at the location of length I, after speed reduction, is denoted as k3. Then, the required time delay at the location of length I, will be k - k3. The arrival time k3 of robot 2 for the curve OMS will be k, + k, - k3, where k! is the arrival time of robot 2 for the curve OM3' only after speed reduction. Again, if ke < k3, then the time delay at the location of length I, is not necessary, which corresponds to the case that pure speed reduction is sufficient to avoid the collision region. In Figure 4.12-(d), the curve OM4 is a curve which avoids the collision region and can be obtained by slowing down the robot 2 motion adequately. There are a number of ways to reduce the robot 2 speed so that the curve OM4 can avoid the collision region. Since the curve must avoid the corner point X, speed reduction will be applied to the robot 2 motion on the path of length 1, as an approach to the solution. The curve from the speed reduction of the robot 2 motion can be obtained by applying the time scheduling concept discussed in Section 4.3.2. Since it is assumed that the traveled length versnus servo time curve OM exists for the given straight line path, the speed. reduction can be realized only on the acceleration pore

OII k# k k ) 0 ke k2 k1 b k,!1 _ _ _ I ~ 6 I I ollrs r olwus 0 ~~k, 'kg kf kfk,3 k 0k kg' k14 k/ Figure 4.12 coUisiou..Freo Trajectory C~urvem

113 tion of the Straight line trajectory of robot 2 by selecting an appropriate break point. This corresponds to the explanation that point D is selected on the acceleration portion AB in Figure 4.5 to realize the speed reduction. If one is only interested in pure speed reduction, the speed reduction will be performed initially on the segments of length l, and If - I,. The traveled length versus servo time curve from planning thetwo segments may still touch or cross the collision region. Then, further speed reduction must be applied to the segment of length I,. This can be accomplished by dividing this segment of length 1, into two, three, or four, etc. segments so that each segment can be planned with a break point until the combined traveled length versus servo time curve does not touch or cross the collision region. As an example, the curve OMs is drawn from the comI i1 bined trajectory information for three segments of length 2, -, and I -I1 in 22 Figure 4.12-(d). Recall that if we use more break points in the trajectory planning of a path, the total traveling time will be much longer. Thus the final arrival time for the curve OMs will be much longer than that of the curve OM. The effects on the final arrival time due to the time scheduling (i.e. time delay and/or speed reduction) of a trajectory are considered in more detail. Hereafter, discussions are based on the assumption that the servo time interval is very small and the effects from discretization are negligible. Later, the results will be verified from a computer simulation. We consider the collision situations shown in Figure 4.13, where the curve OMo is a traveled length versus servo time curve from the original trajectory information of robot 2 and the curve OM is a time scheduled traveled length versus servo time curve with a final arrival time k1. The curve OM can be obtained by

114 reducing the robot 2 speed on the path of length 4, from the initial location. The time k1 is the time when the original curve OM reaches the location of length 4, The time k2 is the time when the curve OM reaches the location of length 4J after speed reduction. If the robot is allowed to delay its starting time by ke - k1, then the original curve OMo is shifted to avoid the collision region in the collision map, which corresponds to the curve OM1 in Figure 4.13. By the speed reduction of the robot 2 motion, a curve OM can be obtained also. Here, it is possible that k2 > k,, or k2 < ke to occur in Figure 4.13. When k2 > k,, the speed reduction on the path of length 4, will be sufficient to obtain a curve for avoiding the collision region. When k2 < ke, speed reduction is not sufficient for collision avoidance and pure time delay at an appropriate location can be used for collision avoidance, which corresponds to the curve OM2' in Figure 4.12-(b). In Figure 4.12-(b), the curve OM2 shows a time delay at the initial location with speed reduction, while in Figure 4.12-(c), the curve OM3 shows a time delay at an intermediate location with speed reduction to avoid the collision region. If we assume that time delay is not allowed for the robot 2 motion, then k2 must be equal to or greater than k, for avoiding the collision region. Also, for the speed reduction on the acceleration portion, we have kl < k2. That is, the average speed of the robot for the curve OM is equal to or smaller than that for the curve OMo on the path of length I,, which results in the fact that the traveling time corresponding to the curve OM is longer than that to the curve OMo. Thus the following inequality from speed reduction must be satisfied for the collision avoidance with robot 1.

/ O~~~~~~o~O a a/ I I I a a/ I I~~ ~ I I~~~~ I a I a I a r 1~~~~~~~~ I I r~~~~~~~~~a a 0L k k1 k1 k2 k1 (k k - Fbgure 4.13 Co~tllo MA~p ~aftr Time Slcheduling(

118 k1 5 k, < k2 (4.30) The curve OMo is obtained from the trajectory information constituting an acceleration portion and a deceleration portion of the straight line trajectory. It is notable that the traveling time k - is the lower bound for all those motions going from the location of length 4, to that of length If, where the velocity at 1, is less than the velocity of OM at 4l. Thus, considering the traveling time on the path of length If - I, when the robot follows the curve OM (where the velocity at k2 is equal to zero.), we have: k, - k, < kF -k2 (4.31) or kf > k/ + k2- k, (4.32) where time k1 is the final arrival time of curve OM. When the speed reduction is performed on the segment of length 1, for avoiding the collision box, we want to show that the final arrival time from the speed reduction is longer than that from the time delay of the robot 2 motion. Since k. - k, < k2 - k, we have: k! > k/ + k2- k - k/ + k, - kl (4.33) Therefore, kf > k! +k, -k1 (4.34) The time k, - k1 is the required time delay for the curve to avoid the collision region ( the curve OM1 in Figure 4.13). The above equation indicates that the final arrival time from the speed reduction on the path of length 4, is greater than that from the time delay. However, if speed reduction occurs at an arbitrary location of

117 the straight line path, the final arrival time after time delay may not always give rise to shorter traveling time than that after speed reduction of the robot motion. These aspects will be discussed with a simulation in Section 6.4. A procedure is outlined below for the time scheduling of a straight line trajectory. From the discussion of Section 4.3.2, a segmentation process for the path of collision starting length, is described for speed reduction instead of specifying the locations of the break points. The procedure utilize a user-designated integer 64 for specifying the maximum number of the segments on the path of length I,. If a potential collision still occurs even after the path is segmented by 64, then time delay of the robot 2 motion will be used for the purpose of collision avoidance. Procedure TS ( Time Scheduling ): This procedure addresses the iterative steps in obtaining the time scheduling of the straight line trajectory for the collision avoidance of two moving robots. TS1. [Initialize the index i.] Set i = 1. TS2. [Construct a collision map from the collision situation.] Obtain a collision map from the path and trajectory information of robots 1 and 2. TS3. [Obtain the traveled length versus servo time curve.] Identify the collision starting length I. Then apply the algorithm FW, the procedure BR, and the algorithm MFW to the segments of length l, and If - l, and draw the combined traveled length versu servo time curve from the resultant trajectory information in the collision map of step TS2. TS4. [Check the collision map and divide the segment of length 4, if necessary.] If the curve in step TS3 still touches the collision box, then increase i by 1, and divide the segment of length I, by i, and go to step TS5. otherwise, stop. ( Planning of collision-free path by time scheduling of the trajectory is successful.) TS5. [Obtain the combined traveled length versus servo time curve.] Apply the search algorithm FW, the procedure BR, and the algorithm MFW to each of the segments, draw the combined traveled length versus servo time curve in the collision map, and go to step TS8.

118 TS6. [Check the stopping variable.] If i > 6,, where 64 is a designated integer that specifies the maximum allowable number of segments on the path of length 1,, then time delay of the robot 2 motion will be used for the collision avoidance purpose. otherwise, go to step TS4. 4.4.2. Collision-Free Path Planning for Case 2 A collision-free path of robot 2 is considered in the following categories: (1) when time scheduling is applied without any path modification, (2) when only path modification is applied without considering the trajectory information of two robots, and (3) when path modification and time scheduling are applied simultaneously. Since the arrival time can be changed in case 2, the time scheduling on the original robot 2 path, which is similar to the solution in case 1, is considered first. As indicated in Eq. (4.34), if speed reduction is performed on the path of length 1,, time delay yields the smaller final arrival time than speed reduction for avoiding a potential collision. Thus, time delay is considered in this section for the time scheduling of a trajectory. If we denote the final arrival time of robot 2 as kf for the original trajectory, then the total traveling time after a time delay will be: kTS = kl + Akf (4.35) where Ak/ is a required time delay for avoiding a potential collision. If Akf is very small compared to kf, then time delay of the original trajectory provides us a good solution for the purpose of collision avoidance. When Akf is fairly large and I, = 0 (See Figure 4.15), we can consider a solution in categories (2) and (3). If a path deviates from the robot 1 path for at least a distance of r + r2, then the path can be a collision-free path and constitute a solu-.

119 tion in category (2). It is notable that the total traveling time of this path can be smaller, equal to, or larger than kTS in Eq. (4.35). The deviation of this path can be reduced by considering a solution in category (3), where path modification and time delay are used at the same time. If path modification and time delay are applied simultaneously for planning a collision-free path, there exists an infinite number of collision-free paths of the two robots. Since two robots are working simultaneously in a common workspace, and their movements are coordinated in a time sequence, there is likely to be a constraint on the time delay of the robot 2 motion. Also, it is desirable to plan a collision-free path with the smallest deviation from the original path. Thus, planning of a collision-free path is considered with respect to an allowable time delay at the initial location and the minimum deviation. First, we find a collision-free path which deviates from the robot 1 path for at least a distance of rl + r2 through a geometric analysis. The path is then guaranteed for the collision avoidance with robot 1. A collision situation is shown in Figure 4.14, where robot 1 moves from A 1 to B, while robot 2 moves from A 2 to B2. We assume that a potential collision exists between the two moving robots with a corresponding collision map. See Figure 4.15 for the collision map, where P and Q are the corner points of the collision region at which the potential collision is assumed to start at time k, and end at time k,, respectively. The length A2B2 is the total length of the path from A2 to B2. The following analysis indicates how to choose a point C2 such that the path from A2 to B2 via C2 deviates from the robot 1 path for at least a distance of r I + r 2. With reference to Figure 4.14, the distance between the robot 1 path and the robot 2 path and corresponding nearest points on each path will be found. The

120 o ~~C3 C. Ut. N~~~U b - s~~~~ So, ]~~~~~~~e CF X~~~ \ ~ C d \ \ X~~~~~~~~'

121 straight line path A 1Bt is a line which passes through a point A 1(zA 1, A 1, ZA 1) with the direction number of (ZB - ZA y1, i - IA, zB - zA:). Thus, the general form of an arbitrary point GA 1B1 on the path A B1 can be represented by a vector notation as: OGA 1L = { \1(Z- - 1A 1) + ZA, X 1(YB1- YA 1) + YA 1 X1(ZB - ZA 1) + ZA 1 ) (4.36) where X1 is a parameter for representing the straight line path A 1B 1. Similarly, the general form of an arbitrary point GA 22 on the path A 2B2 can be represented by a vector notation as: OGA 2B2 { X2(B2 - 2A2) + ZA2, X\2(B2- YA 2) + YA 2 X2(ZB2 - ZA2) + ZA2 } (4.37) where X2 is a parameter for representing the straight line path A 2B2. Let us denote the distance between GA 1B1 and GA 2B2 as DS1. Then the square of the distance DS1 can be obtained as: DSI2 = |I O - OGA2B2 12 (4.38) To find the nearest two points on each path, the partial derivatives of DS2 with respect to X1 and X2 are obtained. Since the partial derivatives are zeros at the nearest two points of the two straight line paths, we have: ODS2 dDS2 --- ~;,-'0 (4.39) Solving Eq. (4.39) simultaneously, we can find X1 and X2, which correspond to the nearest two points between the two straight line paths. We denote these two points on the robot 1 and robot 2 path as Kl(ZKl, yK1, ZK1) and K2(K2, YK2, ZK2), respectively, as shown in Figure 4.14. It is notable that points A 2, B2, K1, and K2 are on the same plane which can be constructed by A 2B2 and K1K2.

In O, kgr I.~Ii'lCliro l 0k kik k! Figure 4.15 Initil Collision Map

123 It is notable that the swept volume of a sphere in the direction of a straight line forms a cylinder capped with two semi-spheres at both ends. Thus, for considering the distance of r1 + r2 between the two robots, we view the collision situation between the cylinder surface of radius rl + r2 in the direction of A Bl and the robot 2 path. There'are two intersection points on the cylinder: surface, we call them A and B, at which the cylinder surface is perforated by the robot 2 path. To maintain the distance of r I + r2 between the robots, we consider the intersecting curve between the cylinder surface and the plane which is constructed by A 2B2 and K1K2. We call the intersecting curve on the cylinder surface a collision-free locus. The collision-free locus connecting A and B forms a part of an ellipse, which is centered at K1 (See Figure 4.14). Since K1 and K2 are the nearest two points between the two straight line paths, it is easy to show that axes of the ellipse are in the directions of A 2B2 and K1K2. Thus, the maximum deviation of the collision-free locus from K1 occurs in the direction of K1K2. In order to obtain straight line segments which approximate the collision-free locus, we construct lines 11 and 12 which lead separately from A2 and B2 and are tangent to the ellipse. Consider two points Q1 and Q2 in the direction of K1K2. We denote their deviations from K1 as d' and d2, respectively. First, we obtain the location of QI and Q2 in terms of d1 and d2, respectively. Then, we find constraints on d' and d2 to guarantee that the distances between two straight line paths A 1K1 and A2Q1, and B1K1 and B2Q2 are equal to r l + r 2 The location of Q1(zQ1, /ly, ZQ1) is obtained in terms of d' as: -_ -_ OK2- OK1 0Q1 -= OK1 +, A, I.. - (4.4o I

124 Since the value of d, specifies the location of Q1, we can represent the distance between two straight line paths A 1K1 and A2Q1 in terms of the deviation d1 [6]. If we denote this distance as DS2, then: DS2 = 1f (d1) (4.41) where f 1 'relates the distance between the paths of A IK1 and A2Q1 with the deviation dl. To maintain the distance between the paths A 1K and A 2Q l for at least rl + r 2, we must have: DS2 f l(dl) r l + r2 (4.42) Here, we will consider the equality only to avoid an unnecessary large deviation from K1. Then, we can obtain a constraint on the deviation di as: d= - f l (rl + r2) (4.43) Similarly, for the paths of B K1 and B2 Q2, we have a constraint on the deviation d2 as: d2 - f ` (r1 + T2) (4.44) where f 2 relates the distance between the paths of B1K1 and B2Q2 with the deviation d2. The location of Q1 can be obtained from Eq. (4.40) by using d' of Eq. (4.43). Also, the location of Q2 can be obtained from Eq. (4.40) by substituting d2 of Eq. (4.44) for d1. Using the locations of Q1 and Q2, the two straight lines iI and 12 in the directions of A2Ql and B2Q2 (See Figure 4.14.) can be identified easily. It is notable that 11 and 12 are on the same plane which is constructed by A 2B2 and K1K2. Now, we consider the intersection point of these two straight lines ZI and 12.

125 Apparently, the path from A2 to B2 via the intersection point, which is denoted as C2, is a collision-free path in category (2). To find the deviation of this path from the original robot 2 path, the nearest point on A 2B2 from C2, which we call C2, is found from a vector projection and addition as: B2C2 ( OA 2- OB2 ) OC2 OB2 + ( OA2 - OB2 ) (4.45) I OA2- oB2 1 12 Then the actual deviation of the collision-free path from the original robot 2 path is found as: dC = OC2- OC2 11 (4.46) Note that no time delay of the robot 2 motion is required for the path from A2 to B2 via C2 for the purpose of collision avoidance, while Ahk in Eq. (4.35) is required for the original robot 2 path. Of course the time of travel from A 2 to B2 via C2 will exceed K. We: now consider a collision-free path by both path modification and time delay. A path, which deviates from C2 for a distance of Ad, is considered. A point C2 can be found for the deviation of Ad from C2 as: OC2 - OC2 OC2 OC2 + - 0 Ad (4.47) 1I 1oc2- o21 I1 The point C21 specifies the lengths of A2Cn2 and C 2 as and I, respectively, with the corresponding collision map as shown in Figure 4.16, where the traveled length versus servo time curve OM' is assumed to cross the collision region for the path A 2- > C2 - B2. The required time delay for avoiding the collision region can be found from the trajectory information. The traveled length versus

128 servo time curve OM2 is a curve, which is obtained from OM' with the required time delay for the path A2 - C' - B2. As mentioned earlier, the two robots are moving simultaneously in a timecoordinated sequence. Other objects may need to move close to the initial location of robot 2. Thus, there is likely to be a constraint on the time delay at the initial location for avoiding the potential collision. If the allowable time delay at the initial location is denoted as Ak61,, we want to obtain a collision-free path which does not violate this constraint. It is notable that the path from A2 to B2 via C2 does not need any time delay for the purpose of collision avoidance. Thus, this path always meets the constraint on the time delay at the expense of the deviation d Ct If Ak/ in Eq. (4.35) is smaller than or equal to Akeao, then the solution in category (1) will be enough for the purpose of collision avoidance. However, if Akl is bigger than Akalo,, then time delay on the original robot 2 path cannot be used for the purpose of collision avoidance. Recall that speed reduction on the original robot 2 path is not appropriate since we are considering the case that I, = 0 (See Figure 4.15). In this aspect, we want to find a collision-free path by both path modification and time delay of the robot 2 motion. Note, however, that although the time delay we determine will be less than Ak,lAu,, the total traveling time may be larger than kTs. If the required time delay for a path between A 2B2 and the path from A 2 to B2 via C1 exceeds Ak.r6, then we can increase the path deviation from the original robot 2 path. Otherwise, we can decrease the path deviation from the original robot 2 path. A bisection method between C2 and C2 is suggested to increase or to decrease the path deviation depending on whether the required time delay exceeds

127 the allowable time delay Akk,o., or not. Using the bisection method, we develop an iterative procedure for obtaining a collision-free path of robot 2 subject to the performance measure (2) (Refer Section 4.2.1) and a constraint on the time delay(Ak,,). Note that, since the path from A2 to B2 via C2 does not need any time delay for the purpose of collision avoidance, the existence of a collision-free path from the bisection process is always guaranteed. A procedure is outlined next for planning a collision-free path in category (3) subject to the performance measure (2) and a constraint on the time delay of the robot 2 motion. Procedure PM ( Path Modification ): This procedure addresses the iterative steps for obtaining a collision-free path in category (3) subject to the performance measure (2) and a constraint on the time delay of the robot motion. The 65 in step PM7 specifies the maximum iteration number of the bisection process for stopping the procedure. This procedure is referred to the collision situation shown in Figure 4.14 and the collision map in Figure 4.15. PM1. [Initialize the index i.] Initialize i = 1. PM2. [Compute the necessary information.] Obtain C2 (Eqs. (4.38)-(4.44)), C2 (Eq. (4.45)), and dd" (Eq. (4.46)), set Ad(1) -, and obtain C 2 corresponding 2 to Ad (1) (Eq. (4.47)). PM3. [Draw the collision map.] Obtain the collision map corresponding to the robot 2 path from A 2 to B2 via C~. If a potential collision exists, then go to step PM4. otherwise go to step PM8. PM4. [Compute the required time delay.] Using the collision map and trajectory information, obtain the required time delay for the path from A2 to B2 via C for the purpose of collision avoidance. PM5. [Increase the deviation from the original path.] If the required time delay on the path from A2 to C~ exceeds the constraint on time delay, then compute A d(i+1) such that Ad(i+1) = Ad(i) + d, find C~'.~,8, fidC+

128 'I~ I ~12~~. i-t........ '..' ~~~~~~~~~~~~~~~~~~~~~~~~I II I I k, k, AI' I A I 0, ke k1/ k1 k Figure 4.16 Collison Map2with Figure 4.18 Co~llsion Map with Path Modification

129 corresponding to Ad(i+1) (Eq. (4.47)), and go to step PM3 with the updated value of i. otherwise, go to step PM8. PM8. [Decrease the deviation from the original path.] Compute Ad(i +1) such d act that Ad(i+l)=Ad(i) 2, find C'+1 corresponding to Ad(i+l) (Eq. (4.47)), and go to step PM7 with the updated value of i. PM7. [Check the stopping variable.] If i > 65, where 65 is a userdesignated integer that specifies the global stopping of the bisection process, then stop. ( The path from A2 to B2 via Ci is a collision-free path subject to the performance measure (2) and the constraint on time delay with 65 for stopping the bisection process. ) otherwise, go to step PM3. 4.5. Summary Collision-free path planning problem of two robots is investigated systematically by using a sphere model for the wrist of the robot, straight line path planning, and the notions of collision map and time scheduling. Using geometric analysis, connected straight line segments have been obtained subject to the performance measure to obtain a collision-free path for a moving robot in the presence of a stationary robot. Discussions of the relationship between the number of via points and the allowable deviation error of the collision-free path are also presented. Several collision situations are identified for obtaining collision-free paths of two moving robots. Depending on the collision situation, the time scheduling of the original trajectory and/or path modification have been used to obtain the collision-free paths. The resultant collision-free trajectory will be used in an adaptive perturbation control in Cartesian space, which will be discussed in the next chapter.

CHAPTER 5 CONTROL OF MANIPULATOR IN CARTESIAN SPACE 5.1. Introduction The purpose of manipulator control is to maintain a prescribed motion for the manipulator along a desired time-based trajectory by applying corrective compensation torques to the actuators to adjust for any deviations of the manipulator from the trajectory. In this Chapter, we present a Cartesian space adaptive perturbation control strategy [281 which controls the manipulator hand to follow a collision-free straight line trajectory as closely as possible under various loading conditions. In the Cartesian space control, we want small position and velocity errors along the desired hand trajectory. Thus, the control feedback should be performed at the hand level. Also, the changes of the load in a task cycle should be taken into account so that the accuracy of the controller is not significantly effected by them. This suggests an adaptive control approach. We will utilize the Cartesian trajectory planner developed in Chapter 3, and concentrate on developing a Cartesian space control method which is based on resolved motion and adaptation. The necessary kinematic relationships between the Cartesian coordinates and joint coordinates are presented again in Section 5.2 to develop equations of motion of the manipulator hand. In Section 5.3, the equations 130

131 of motion of the m-Anipulator are derived for formulating the adaptive control scheme. The resolved motion adaptive control, which is based on the perturbation theory and parameter identification scheme, is presented in Section 5.4. In Section 5.5, the computationat c6mplexity of the resolved motion adaptive control scheme is discussed. Finally, summaries and discussions are presented in Section 5.6 56.2. Kinematics of the Manipulator Hand The kinematic relationships of position, velocity and acceleration between the joint space and the Cartesian space are required to obtain the equations of motion of the manipulator in the Cartesian space. The location of the manipulator hand with respect to a fixed reference coordinate system can be realized by establishing an orthonormal hand coordinate frame at the gripping point of the hand. Figure 2.7 is shown again in Figure 5.1. Let us define the position, orientation, linear velocity, and angular velocity vectors of the manipulator hand with respect to the reference frame respectively as in Eq. (2.3), p(t)-( p (t), P,(t )) P (t)); (t)-(a(t), B(t), 7(t)) (t) v (,(t), Vy (t),,(t) ) T; (t ) (, (t) y(t,): WZ(t ) )T where the superscript "T" denotes transpose operation on vectors and matrices. The time derivative of the position of the hand with respect to the reference frame is equal to the linear velocity of the hand, as derived in Eq. (2.10), p(t) dp(t) v(t) (5.2) From Eq. (2.13), the relationship between the (a(t),(t), y(t)) T and

(0!~~~~~~~~~~~~~~~~~~~~m k~~~~~~~~~~~~~~~~~~~~~~~~~~* 4~~~~~~~~ z, P6 Figure 5.1 Hand Coordinate System

133 a(t) C S o 1 ris (t) (t ) 1 B- S5C 7 s 0 w,(t (t3) it). C 7 S Ay Sjq S iS C [w3(t) or ()(t) [M (4)] n(t) (5.4) The linear and angular acceleration of the manipulator hand can be found from the Eq. (2.16) as, O( V(t) fl(t J],q)= [ J ^J(q) ] (q(t) ] (55) The Eqs.(5.2)-(5.5) are the equations required to obtain hand information from joint information. 5.3. Equations of Motion of the Manipulator To formulate the resolved motion control method, we obtain the manipulator equations of motion by using the formulas developed in Section 5.2. The dynamics of a six-joint manipulator can be derived either by LagrangeEuler [5,24,46] or Newton-Euler formulations [24,38]. The resulting equations of motion are highly nonlinear and consist of inertia loading, coupling reaction forces (Coriolis and centrifugal) among the various joints, and gravity loading effects. In general, the Lagrange-Euler equations of motion of a six-joint manipulator, excluding the actuator dynamics, gear friction and backlash, can be expressed in vector matrix notation as, I)(q)q(t)+h(q,q)+c(q)= T(t) (5.6)

134 where r(t) is a X applied torque vector for joint actuators, q is the angular positions, q is the angular velocities, q (t) is a OX 1 joint acceleration vector, c(q) is a OX 1 gravitational force vector, h(q, q)is a 8 X1 Coriolis and centrifugal force vector, and D(q) is a OXO acceleration-related matrix. Since D(q) is always nonsingular, q(t1) can be obtained from (5.8) and substituted into (5.5) to obtain the accelerations of the manipulator hand, (r)1() 1 [J( q) [J-'(q)] +[J(t)- h(q, )- e(q) (5.7) where q and q can be obtained from Eq. (2.4b) and Eq. (2.15), respectively. For convenience, let us partition J(q), r-(q), and D-l(q) into 3X3 submatrices and h(q, q), c(q), and t(t) into 3X 1 submatrices, Jl1(q) I 314q) A 11(q) I K12(q) 3(q)] -q] [K(q] I J2i(q) I J2(q) J K21(q) I K24(q)j (5.8) rEll(q) I E1q) h( q, q) [D-1(q)] E(q)A = - ' i [(q)] ][ - ) - E21(q) I E2( )) h2(q,q) c(q) j r:(2) D -)(q)] I - I, Fq; [h(t)],] - (5.10) Combining Eqs. (5.2), (5.4) and (5.7), and using Eqs. (5.8-5.10), we can obtain the equations of motion of the manipulator in Cartesian coordinates:

135 ')''' o1 ~ I 0o p( |(O 0 0 1 0 I M () |() |(y) 0 0 I J(q,. q)Kq)(q) + Jiq,. )K(q) jt J(q, )K.q) + J1iq, q)q) '(1) 0 ). l, J (q, q)KL(q) + Jq,.K (q) I JI.(q, q)K +,Jq, q)KJq)J (s.11) 0 I 0 | [ - O | q. -~,(q) +,|p J-(q)Eu(q) + JLq)EU(q) I J,4q)Eiq) + JLiq)EIq [ - Iq,. - ciq) + rfg) JU(q)Eu(q) + J=(q)E=(q) I JU(q)EL4q) + Jdq)Eq) where I3 is a 3X3 identity matrix. The Eq. (5.11) shows that the equations of motion of the manipulator in Cartesian coordinates are extremely complex and shows the nonlinearly coupled dynamics of the manipulator. Design of an appropriate control law based on the Eq. (5.11) may be very difficult. Instead a linearized perturbation model about the nominal hand trajectory may provide a better approach. 5.4. Resolved Motion Adaptive Control Defining the state vector for the manipulator hand as: X(t)- (Z1(t), Z2(t),.,Z12(t))r = (p,(t), p,(t), p,(t )(t t), (t ), )? A), Us(t), v,(t), tv(t), (, W(t ) (t),,(t)) (5.12) _( TP (t),. r(t),v T(t),n T(t)) r and the input torque vector as: u(t) - (,(t), ~, u(t)) r t (r,(t),.., r6(t) ) r (5.13) the Eq. (5.11) can be expressed in state space representation as,

138 x(t ) f(x(t), u(t) ) (5.14) where x(t)E R2", u(t)E R", f(') is defined and continuously differentiable on R 2, X R5 except at certain critical points of R 2" X R, and n is the number of degree of freedom of the manipulator. The critical points may come from the particular configuration of the manipulator and the singularity of the Euler angle formulation of the hand orientation. From Eq. (3.3), we have: ( pT(t), *T(t) )T = ( 21,., )T = N(q(t)) (5.15) The Eq. (5.14) can be expressed explicitly as, 21(t) = f ( X(t),u(t) ) = 7(t) 2(t) = f ( (t ). (t ) ) = Z(t8):3(t) = f( (x(t), u(t) ) = 2s(t);3(t )-f 3( 0t ), u(t ) ) 24t ) z4(t) = f ( x(t), u(t ) ) sec:s(,,o CZ6 +:Z, SZ6) (5.16) 5(t ) = f ( x(t ), u(t ) ) - sec zs( 10 Czs S26 - 211 Cz5 CZ6) 6 s(t ) = f ( x(t), U(t) ) sec Z5( o SZ 5 CZ6 + Z 11 SZ5 SZ A + Z12 CZS ) zi+6(t) = /i+6( x(t),u(t)) i+6(q, q)x(t) + bi+s (q) X(qq) + bi+6(q)u(t) where i =1, ' *,6 and g+6(q, q) is the ( i+6 )t' row of the matrix, o o i Is o o o I o M (l) 0 0 Jll(q, q)Kl(q) + J2(q, q)K21(q),ll(q, q)K2(q) + J,2(q, q)i22(q) 0 0 JI 21(q, q)K11(q) + J22(q, q)K21(q) J21(q, q)K12(q) + J22(q, q)K2(q) b, ~+(q) is the ( i +6 )' row of the matrix:

137 o I o. I ':,': J11(q)E11(q) + Jl(q)E21(q) I Jl1(q)E2(q) + J1(q)22(q) J21(q)El(q) + J22(q)E21(q) I. Jl(q)E12(q) + J22(q22(q) Moreover, h(q, q) - cl(q) X(q q), h2(q, q) - c2(q) q = NI-(z1,, 6), and from Eq. (2.15) = J-'(N('(z,, (,:))T. (,7, * * *) The variables z1, ',:6 must be restricted to a set on which the inverse functions N-' and J-1 are defined and Eq. (5.16) is defined. 5.4.1. Perturbation Equations of Motion Since the Eq. (5.16) describes the complete robot arm dynamics, the desired joint torques for each hand trajectory set point ( pd (t) d (t),vd (), (t), Id (t) ),;d d (t) ) can be computed in open-loop fashion quite accurately as follows: (i) The hand trajectory set points are resolved into a set of values of desired joint positions, velocities and accelerations using Eqs. (2.4b), (2.15), (2.17), and inverse kinematics equations at each sampling instant ( In fact, these joint values are obtained from the trajectory planner developed in Chapter 3. ), (ii) the desired joint torques along the hand trajectory are computed from the Newton-Euler equations of motion [38] using the computed sets of values

I38 of joint positions, velocities and accelerations. These computed torques constitute the nominal torque values. Using the Taylor series expansion on the Eq. (5.16) about the nominal hand trajectory and assuming the higher order terms are negligible, the associated linearized perturbation model for this control system can be obtained, &x(t ) A(t ) x(t) + B(t) u(t.) + e(t) (5.17) where A(t )-V f. and B(t )V. f j are the system parameters and are equivalent to the Jacobians of ft( x(t), u(t) )evaluated at the nominal states, x, (t), and inputs, u, (t), respectively, e(t) is the error generated from the higher order terms of the Tayler series expansion and the inaccurate perturbation model, 6x(t) x(t)- x, (t), and 6u(t) u(t)- u(t). A(t) and B(t ) are functionally defined by: lA,(t ) I A12(t) o 0 A(t)= [ IA -j B(t)= - -I (5.18) A21(t ) A=(t ) B21(t) ] B=(t) where the submatrices are found explicitly as, 0000 0 0 100 0 0 0 0000 0 0 010 0 0 0 0000 0 0 001 0 0 0 Alz(t) -= l~ t4 a AI ) a 4 af,4 afo o9f4 afs a a0f 8: a1 8:1 ~0000~ 810 o 6afOa afs a8f afA o ' o, / d |0 0 0 az 8z 6 25 |

139 817 a817 817 817 817 8/a7 8z1 az2 8a. 8:7 8:8 a:12 a8/,s a./, A21(t) I; A2(t) - a 12 1 a!12 f 12 12 8s1 8:Z a8z7 asZ12 (5.20) af, a17 aJ 7 /17 af 7 a/7 a,1 au 2 au3 a84 a5. a856 B21(t )-"...; Bt ).. a,12 8a12 a12 a 12 a12 a 12 aUs aU2 aUs: aU, aus aU6 (5.21) Relative to the closed-loop dynamics, the matrices A(t) and B(t) are slowly time-varying and depend on the instantaneous manipulator hand position and velocity. The design of a feedback control law for the perturbation equations requires that the system parameters of the Eq. (5.17) be known at all times. Thus parameter identification techniques must be used to identify the unknown elements in A(t) and B(t). As a result of this formulation, the control problem is reduced to determining 6u(t) which drives bx(t) to zero at all times along the nominal hand trajectory. In other words, the position/orientation and velocity errors of the hand along the preplanned hand trajectory are minimized. The overall controlled system is

140 characterized by feedforward and feedback components. The feedforward component computes the nominal joint torques u,(t) from the Newton-Euler equations of motion and the feedback component computes the perturbation torques bu(t). The main advantages of this formulation are twofold. Firstly it reduces a nonlinear control problem to a linear control problem about a nominal hand trajectory, and secondly the computations of the nominal and perturbation torques can be performed separately and simultaneously. 5.4.2. Parameter Identification and Perturbation Control Hereafter, we denote n as the degree of freedom of the robot. For implementation on digital computer, the Eq. (5.17) needs to be discretized to obtain the following discrete-time linear equations appropriate for parameter identification: 6x( (k +1)T )F( kT ) bx( kT ) + G( kT ) 6u( T ) + E( kT ); 0, 1, (5.22) where T is the sampling period, u( kT )E R" is a piecewise constant control input vector of u(t) over the time interval between any two consecutive sampling instants for kT < t < (k + 1) T, and bx( kT )E R2' is a perturbed state vector which is given by: kT [x(kT )- (kT, to ) to) + f (kT, t B (t )u (t )+ e( t )) dt to (5.23) where O( kT, to) is the state-transition matrix of the system (5.17). F( kT ), G( kT ), and E( kT ) are respectively 2n X2n, 2n Xn, and 2n X1 matrices and are given by: F( kT ) e( ((k + 1)T, kT ) (5.24)

141 (k +1)T G(kT)= f e((k +)T,t)B(t)dt (5.25) ~T and (k +I)r E(kT )- f e( ) dt (5.26) OT With this model, a maximum of an2 parameters need to be identified. Without confusion, the sampling period T will be dropped from the above equations for clarity. Specifically, we abuse notations and let bx(kT) = x(k), bu(kT) = u(k), and E(kT) = E(k). Then we obtain the following equation from Eq. (5.22). x(k+1) -F() (k) + G(k) u(k) + E(k); k =0,1, - (5.27) In order to simplify the identification algorithm for real-time applications, a recursive least square parameter identification scheme is used. In the parameter identification scheme, we make the following assumptions: 1) The parameters of the system are slowly time-varying but the variation speed is slower than the adaptation speed, 2) Measurement noise is negligible, and 3) The state variables x(k) of Eq. (5.27) are measurable. The first assumption was justified from the simulation in the joint variable space control [251 and the same findings were reported in a recent publication [231. The second assumption was made to simplify the identification scheme and it needs to be verified experimentally. The third assumption was based on the fact that the state variable x(k ) can be computed from the measured values of q(t ) and q(t ) and the desired hand position and velocity from the hand trajectory. Defining and putting the in row of the unknown system parameters in the Eq. (5.27) at the k ei instant of time in a 3n +1-imensional vector as,

142 i (k) -= (f,(k),, f,,(k), gl(k ), g,,(k), e,(k) )T (5.28) where p - 2n, and the outputs and inputs at the k instant of time in a vector as, (k) ( zl(k), -, z,(k), ul(k), ~ o, u(k), 1)T (5.29) and the state vector at the k tA instant of time as, c(k) ( ~ (k), ~~, * (k))r (5.30) Then, the Eq. (5.27) can be rewritten as follow,;,(k +1) = T(k)O,(k); i - 1, 2, - 2n. (5.31) where zi (k +1) is the Oih state variable at the (k +l)th sampling instant. Based on the input-output relation in Eq. (5.31), a recursive least squares parameter identification algorithm can be found [14,52]: j (k +1) --,(k) + P(k )O(k) [T (k )P(k )(k ) + pl-' [i (k +1) _T (k ) (k )] (5.32) P(k +1) = [P(k) - P(k )#(k) [ T(k)P(k )#(k) + p1-' T (k)P(k)J for i 1, 2, ~ ~ ~,2n, where 0 < p < 1 and "hat" is used to indicate the estimate of the parameters O (k) and P(k) is a (3n +1)X(3n +1) symmetric positive definite matrix. P(k)=p[ *(k) *(k)]-' and I(k)=[[(1), (2), 2)...,$(k)] is the measurement matrix up to the k th sampling instant. The P(k) matrix has the similar effect as the error covariance matrix in stochastic identification with zero mean modeling error. The above recursive equations indicate that the estimate of the parameters Oi(k+1) at the k+1ia sampling period is equal to the previous estimate i;(k) corrected by the term proportional to [ z, (k +1) - g "(k )U, (k ) J. The r(k )9, (k) is

143 the prediction of the valle zx (k +1) based on the estimate of the parameters OU (k) and the measurement vector (k ). The components of the vector P(k )q(k )[rT(k)P(k )(k) + p]- are weighting factors which indicate how the corrections and the previous estimate should be weighted to obtain the new estimate O (k +1). The parameter p is a weighting factor which is commonly used for tracking slowly time-varying parameters by exponentially forgetting the "aged" measurements. If p << 1, a large weighting factor is placed on the more recent sampled data by rapidly weighing out previous samples. If p x 1, accuracy in tracking the the time-varying parameters will be lost due to the truncation of measured data sequence. We can compromise between fast adaptation capabilities and loss of accuracy in parameter identification by adjusting the weighting factor p. In most applications for tracking slowly time-varying parameters, p is usually chosen to be 0.90 < p < 1.0. Finally the above identification scheme can be started by choosing the initial values of P(O) to be P(O) = - I3n+, (5.33) where a is a large positive scalar. The initial estimate of the unknown parameters F(k) and G(k ) can be approximated by the following equations: F(O) 1I2g + [elf(xn(O)u(O))]T e+ f(x()u())] 2 (5.34a) C7x ON 2

144 G(O) [r a(.n(o),uu(o)) T + X(X (~)U(~o)) (( ) (5.34b) + [.(x(O).u(o))] [. (xU(O),ul(O))] - where T is the sampling period. With the determination of F(k) and G(k) from the parameter identification scheme, proper control laws can be designed to obtain the required correction torques to reduce the position/orientation and velocity errors of the manipulator hand along a nominal hand trajectory. This can be done by finding an optimal control, u'(k), which minimizes the performance index, J(k), while satisfying the constraints of the Eq. (5.27): J(k) = [ xT(k +l)x(k+l) + uT(k)Ru(k)] (5.35) where Q is a 2n X2n semi-positive definite weighting matrix and R is an n X n positive definite weighting matrix. We can use the diagonal matrix for Q which can be compromised between the tracking accuracy and the allowable magnitude of perturbed inputs. Also, we can constrain the perturbed torque by increasing the elements of the weighting matrix R. The one-stage performance index in the Eq. (5.35) indicates that the objective of the optimal control is to drive the position/orientation and velocity errors of the manipulator hand to zero along the nominal hand trajectory in a coordinated position and rate control per interval step, while at the same time, a cost is attached to the use of control efforts. The optimal control solution which minimizes the functional in the Eq. (5.35) subject to the constraints of the Eq. (5.27) is well-known and is found to be [52]:

145 u'(k) -R+G (k)Q (k G (k)QF(k)x(k) (5.33) where F(k) and G(k) are the system parameters obtained from the identification algorithm at the k tA sampling instant. From the simulation of the adaptive perturbation control in the joint variable space [251, given the F(O) and G(O) matrices in the Eqs. (5.34a) and (5.34b), it was found that the performance of the adaptive controller is quite sensitive to the initial values in the P(O) matrix and less sensitive to the Q and R weighting matrices. This result is expected as the P(k) matrix has the same effect as the error covariance matrix in stochastic identification with, zero mean modeling error, if the weighting factor p is chosen appropriately. In general, the convergence of the parameter identification is not guaranteed and hence the performance of the adaptive controller depends on the proper selection of the P(O) matrix. The initial large values of the P(O) matrix usually give better convergence. The values of the Q matrix did not affect the performance of the controller very much, however, a small value of the R matrix did give a better tracking result. These results agree with the findings in [23]. The above identification and control algorithms in the Eqs. (5.32) and (5.36) do not require complex computations. In the Eq. (5.32), [T (k)P(k )(k) + p] gives a scalar value which simplifies its inversion. Although the weighting factor p can be adjusted for each iOb state variable as desired, this requires excessive computations in the P(k+l) matrix. For real-time robot arm control, such adjustment is not desirable. P(k+l) is computed only once at each sampling time using the same weighting factor p. Moreover, since P(k) is a symmetric positive definite matrix, only the upper diagonal matrix of P(k ) needs to be computed.

146 The combined identification and control algorithm can be computed in O(n 3) time as shown in the next section, where the computational requirements of the identification and control algorithms for a n -joint manipulator is tabulated in Table 5.1. The proposed resolved motion adaptive control block diagram is shown in Figure 5.2. It is notable that the feedback state x(t) is calculated from the measurements of q(t ) and q(t ) in the control block diagram. The kinematics routine and J(q) are stored in the computer, thus the control method can be viewed as a control method in the joint variable-space. An alternative way of doing control in the jointvariable space has been discussed in [25J. It eliminates the computational complexity of the kinematics routine and J(q). The disadvantage of doing this is that the error is measured in the joint-variable space and small error in the joint variables may give fairly large error in Cartesian space. By suitable choice of Q and R, it may be possible to correct this disadvantage. However, the choice of the weighting matrices will require more investigation.

Robot Lilnk Pa rameters Hand q q Newton-Euler U*1(A) tbKi) Trajectory Equations Mniputator Routine Planning ol Motion One-step Recursive F(O), Q.R Optimal Least Square 0(0), J(1) Conrlroller Identification P(O) Scheme I ))I) V(I) PW1 p IF(A) Ca (k) 00) 4 11 1t ) d d(gW) (1) Figur 5.2 Resolved Motion Adaptive Control Blo) ck Diavr) Figure 5.2 Resolved Motion Adaptive Control Block Diagram

148 5.5. Computationsl Complexity of Resolved Motion Adaptive Control The overall resolved motion adaptive control system is characterized by feedforward and feedback components which can be computed separately and simultaneously. Assuming that multi-processors are available for parallel computations, a feasibility study of implementing the adaptive controller using present day low-cost microprocessors can be conducted by looking at the computational requirements in terms of required mathematical multiplication and addition operations. The feedforward component which computes the nominal joint torques along a desired hand trajectory can be computed directly from the trajectory planner information because the Cartesian trajectory planner in Chapter 3 provides the trajectory information both in the joint and the Cartesian space simultaneously. The nominal joint torques can be computed along the desired hand trajectory using the Newton-Euler equations of motion [24]. It requires a total of 678 multiplications and 597 additions for a PUMA manipulator. The feedback control component which computes the perturbation torques can be conveniently computed serially in three separate stages. The first stage computes the actual manipulator hand position and velocity from the measured values of joint position and velocity. The second stage involves the computations of the least square identification scheme (Eq. (5.32)) for the linearized perturbation system. The third stage involves the computation of the optimal adaptive self-tuning controller (Eq. (5.38)) for the linearized system. It requires about 3348 multiplications and 3118 additions for the PUMA manipulator. Since the feedforward and feedback components can be computed in parallel, the proposed adaptive controller requires a total of 3348 multiplications and 3118

149 additions in each sampling period for the PUMA manipulator. Computational requirements in terms of multiplication and addition operations for the feedforward and feedback components for a n -joint manipulator are tabulated in Table 5.1. Based on the specification sheet of an INTEL 8087, a Motorola MC68000 microprocessor, and a PDP 11/45 computer, and assuming that for each integer multiply and addition operation, it requires two memory fetches, the required time to compute the proposed adaptive controller is tabulated in Table 5.2. Both INTEL and Motorola microprocessors are not fast enough for closing the servo loop. The PDP 11/45 can compute the controller in about 17.8 mscc which translates to approximately a sampling frequency of 58 Hz. However, the PDP 11/45 is a uniprocessor machine and parallel computation assumption is not valid. But it does give us an indication of the required processing speed of the microprocessors. In the above computations, we neglect the time to compute the trigonometric function values, the time required to sample the joint position and velocity and the time required to perform rescaling of the joint values. However, we anticipate that faster microprocessors will be just around the corner that will be able to compute the proposed resolved motion adaptive controller in 10 macc within a year or two.

150 Adaptive Number of Number of Controllcrr Multiplications Additions stage 1 Compute -r 11 7n - 24 103n - 21 1 ~ (678) (597) Total Feedforward Computations 11 7n - 24 103n - 21 (. 78) (597) Compute ( pTr,r)r (48) (22) stage 1 Compute (vTr)r| n2 + 27n - 21 n2 + 18n - 15 (177) (129) Compute Hand Errors (x(k) - x,,(k)) 0 2n (0) (12) stage 2 and Identification Scheme 33n2 + On + 2 34 }n2 - 1 3n (1244) (1233) stage 3 Compute Adaptive Controller 8n3 + 4n2 + n + 1 8n3 - n (1879). (1722) Total Feedback Computations 8n3 + 38n2 + 37n + 430 8n3 + 35 - + 17 4n + 7 (3348) (3118) Total Mathematical Operations 8n3 + 38n2 + 37n + 30 8n3 + 35 yin2 + 17 Yn + 7 (3348) (3118) (Number inside the parenthesis indicates computations for n=8) Table 6.1 Parallel Computations of the Proposed Adaptive Controller

integer integer Memory Required Computing System Multiplications Additions Fetch Computing Time INTEL 8087 19p/s 17 us 9 / s 233.01 ms (4.29Hz) (5 MHz Clock) MC68000 5.6 jz s 0.96 pt s 0.32 /u s 25.88 ms (38.64 Hz) (12.5 MHz Clock) PDP-11/45 3.3 4 s 300 ns 450 ns 17.80 ms(56.17 Hz) Table 5.2 Implementation of Resolved Motion Adaptive Control

152 5.8. Summary A resolved motion adaptive control based on the perturbation theory has been presented. The adaptive control system is characterized by feedforward and feedback components. The feedforward component computes the nominal joint torques u, (t ) from the Newton-Euler equations of motion using the joint information from the trajectory planner, and the feedback component consisting of recursive least square identification and control algorithms for the linearized system computes the perturbation torques 6u(t). The analysis presented is based on the ideal system study because it neglected such nonlinear effects as gear friction and backlash. The physical implementation of the proposed adaptive control may require further investigation on the effects of gear friction, backlash, control device dynamics, and flexible link structure to the controller. Also, since the computations of the nominal and perturbation torques can be performed in parallel, the computations of the resolved motion adaptive control for a six-joint robot arm may be implemented in low-cost microprocessors. Tables 5.1 and 5.2 show the feasibility study of implementing the resolved motion adaptive control scheme.

CHAPTER 8 COMPUTER SIMULATION 8.1. Straight Line Trajectory Planning B.1.1. Simulation and Results A "C" language program was written on a VAX-11/780 computer to evaluate the performance of the trajectory planning scheme discussed in Chapter 3. The trajectory planner reads in the given initial and final locations of a straight line path with the stopping variables 61 and 62, and outputs the control set points in joint variable coordinates. The control set points consist of joint position, velocity and acceleration along the given straight line path. The corresponding Cartesian control set points can be obtained also directly from the trajectory planner. The servo control period in this simulation was assumed to be 10 maec. Numerical values which are tabulated in Table 6.1 were inputed into the trajectory planner. The torque constraints which depend upon the instantaneous joint position and velocity were not available from the manufacturer's specification sheet for the PUMA robot. Thus we implemented only the smoothness constraint into the simulation package. The jerk, acceleration and velocity bounds are tabulated in Table 8.2. The acceleration and velocity bounds are from the reference [34J and the jerk bound are the somewhat arbitrary set of values shown in Table (.2. 153

154 Recall that the Pi and P2 must be chosen for AXS\ w(k) and Ax~ig (k) such that the former satisfies the constraint limits in step FW2 while the latter does not. In fact, the choice of PI and P2 is closely related to the existence of straight line trajectory for a given path as discussed in Section 3.4.2. The Pi and P2 were selected to be 1 and 2, respectively, as a choice in the algorithm FW. Also, the PI and P2 were selected to be 0 and 1, respectively, as a choice in the algorithm MFW. The number of forward and backward search points was found to be 121. From the procedure BR, the best break point, which yields the smallest position and Euler angle errors among all possible break points, occurred at the 84'A control set point. Depending on the choice of the break point, the Cartesian and angular position, velocity and acceleration errors between the final search point and the final desired point vary appreciately. Table 8.3 indicates the final position and Euler angle errors from several possible break points for the given straight line path. The linear velocity and acceleration of the robot are also shown with the required number of relaxation points when the discrete time relaxation process is applied. It clearly indicates that there is a trade-off between the final position and Euler angle errors and the total traveling time for the given path. From the simulation, the acceleration difference between the final search point [ p(k, ), 4(k) J and the final desired point [p(kl ), *(k1f) J for the possible break points were found to exceed the jerk bounds of the joint motors. Thus, relaxation points are inserted between these two points. The resultant Cartesian position error due to the relaxation process was found to be less than 1 mm while the maximum Euler angle error was found to be less than 0.1 degree.

155 Table 6.4 shows more details about the effects of various break points on the final search point. Depending on the location of a break point, the Cartesian position, Euler angles, joint velocity, and joint acceleration values at the final search point are shown. From Table 6.4, if the trajectory planner starts the deceleration portion from the 841A point of the trajectory set points, it produces the smallest position and Euler angle errors at the final search point among the all possible break points. Hence, the 84a point from the procedure BR is the best break point for this trajectory planning. In the output data of the simulation, all the control set points are within the smoothness constraints. Figures 6.1 - 6.8 show the joint position, velocity, and acceleration for the given straight line trajectory with relaxation points included, where markers are used for every 50 macc. For the figures of joint trajectories from the simulation, the integer parts of the time and joint angles are spaced from their fractional parts on the axis label. Most of joint trajectories are found to be constrained by the limits fr6m the jerk bound and the given straight line requirement. Figure 8.4 shows that the acceleration of joint 4 reaches the maximum allowable limit for the acceleration and deceleration portion of the straight line trajectory. Although the resultant joint acceleration trajectory shows rapid changes near the break point, the acceleration set points are still within the allowable jerk bound for the smoothness of the joint trajectory. Using Eqs. (3.1) and (3.80), the traveled length at each servo time interval can be computed. Then the traveled length versus servo time curve can be plotted for this straight line trajectory as shown in Figure 8.7. This curve will be used in the collision map to determine the collision-free path and/or trajectory in Section 8.4.

186 Initial Cartesian Point (m) (-0. 140000, 0.560000, 0.390000) Initial Orientation (deg.) (0.00000, 90.00000. 90.00000) Initial Joint Vel..& Acc. = qi = O, i = 1,2, - -,n ( deg/ sec and deg/ sec2 ) FInal Cartesian Point (m) (0.000000, 0.440000, 0.480000) Fmal Orientation (deg.) (30.00000, 60.00000, 60.00000) Final Joint Vel. & Acc. C- = qi = O, i = 1,2, - -,n ( deg/ sec and deg/ sec2 ) Sampling Period 10 msec Stopping Variables 61 = 0.000001. 6 = 0.0001 Number of Forward Search Points 121 Number of Backward Search Points 121 Possible Break Point at the 85th sampling point Actual Break Pbint at the 84th sampling point Table 6.1 Data Used in Computer Simulation

Joint 1 Joint 2 Joint 3 Joint 4 Joint 6 Joint 6 700 500 2100 4000 2100 8100 (degl secs) 45 40 75 70 go 80 (deg/ see2) 100 95 100 150 130 110 (degl sec) Table 6.2 Physical Constraints for Computer Simulation

Break Final Position Errors Vel. and Ace. En-ors No. of No. of Total Enror (After Robatoe (Before Helaxation) (Before Relaxation)L..Relaxation No. of Points Point Position Orientation Velocity Acceleration Set Points Points Set Points Position orienato (MMn) (deg) (rmM/sec) (Umm/Sec 2) (aMM) (dog) I NOTto REACHABLE 83 t 84 REACHABLE 0.041 0.006 11.2 303 187 4 171 0.2740.4 86 REACHABLE 0.066 0.008 67.3 308 154 3 lb? 1.1900.0 88 REACHABLE 0.789 0.11lb 8b6. 302 147 3 160 1.683.2 87 REACHABLE 0.399 0.068 100.6 303 144 3 14? 2.20?.7 88444444 to 121 REACHABLE INCRLASING DECREASING DECREASING INCREASING UKRASN Table 0.3 Comparisons of Several Break Points.

Choice ol Vinal LBreak Search Kp or Jonul I p, or JoinL 2 p5 or Joint 3 a or Jotul 4 p or Joint b * or Joint 6 Point Pont p(kL),,(kL) -0.000028 0.440024 0.4700t2 29.0994048 0.00W9b1 60.00bOb 84 q(k) -04 -.b969468 O.Ob4JUO -2.b:377b7 -0.863823 1.227129 |q(lk) 2U8.618493 16.4b2218U -1.bOUbt4:3 869.97b2U88 17.821UU888 -I33.71bb21 p(k6).*(k*) -0.000037 0.440032 0.479976 290.192112 80.007891 60.007809 fib q(k) -b.:3lb263 -3:.O'J03434 0.274248 - 12.943bb0 -3.2b1000 8.2bO0477 q(kl) 27.:Ut3890b 1I b.8b60980 -0.320186 8|9.9 06 i986 183.:73209 -3i.b29t6 |p(k).*(ks) -.0.000b6U 0.440482 0.479i864 29.68146t1 b 60.1 b:384 60. 1 I b384 886 q(k) -7.908676 -4.b27168 0.386b901 -19.3:273J8tl 4.8704 9.28b:0 I q(ka) 2b.74981'3 b.82887 1.00496b 89.908986 18.919624 -21. b92790 01 P(ka).*(ka) -0.000272 0.4402:33 0.4791l2b 29.9417b0 80.ObtJ2bO 60.obtl2b0 87 q(k -9.2972 -b.:19864 0.4622:4 22.690571 -b.722332 10.921216 q(it) 24.72b62Ub 14.980249 1.9.1011 70.00t9439 0.397737 -26.86b719 p(kl), *(ks) -0.000731 0.440627 0.470630 20.t4:12b 1 60. b6748 60. I b6748| 88 q(kL) -11.088272 -6.33J'87b7 0.b22947 -27.094877 -6.849314 12.9 1700 q(kg) 23.08I60:39. 14.471463 3.4687138 86t.872836b 19.739247 -23.7b6183 Units: p is in meters, 4 is in degrees, q is in dg/lsec, and q is in deg/sec20 Table 0.4 Effects of Various BreakL Points on the Final Soearch Point

1]60 Ill9& 92 o 0o0 ),o 601 i I 1 16, to TIf'I( msec ) JOINT I POSITIO ( SRVO Tl[E 10 nsec ) 0 1000 u a# IA -J -32 03 -, a000 '20o 0 I @ 33 1;0l TI'lE( nsec ) JOINT I CCELOCITY C SERVO T IM 10 msec ) FA JOlrsT t ACC~L ~Rnl'ON C SERUO TIME 10 nsec c Figure 8.1 Joint 1 Trajectory of the Straight Line

161 o Sp 0 Li -3. 000o 2as 1t t 6 1 10 TIME( msec ) JOINT 2 POSITIOn ( SERUO TIME 10 mse~c a tOO ' 1 #A Lai 0 000 24qa G40 Z o26 1 37 I;10 TIME( msec ) JOINT 2 VELOCITY ( SERVO TIME 10 msec 16 52 aa -20 13 0 040 3420 1 826 110 TIME( msec) JOINT 2 ACCELERATIOr ( SERVO TIME 10 msec a' a' Li -J 00036 '01? 3031 6~ ~ ~ ~~~~~~~1(me O ~ ~ ~ ~~ON CEEiT~i sROTM 0me Fiue82Jit2Taetoyo h tagtLn

162,P 101 O 000 1W0 104 3" 1 710 TIMt( nsec ) JOIMT 3 POSITION ( SERUO TItME 10 nsec ) \ 1A - a 1 3 01o TIME( osec ) JOrINT 3 VELOCITY ( SERVO T[ME 10 msc Z6 63 uI 3 360 -19 89 a coo "a 3 o z w 3360 TItE( msec ) JOINT 3 nCCELERATIOlt < SERVO TrINE 10 msec Figure 8.3 Joint 3 Trajectory of the Straight Line

163 l O000 0 060 (d0q I YI I 171 T llrI( se: ) JOINT 4 POSITION C SCRuO TIME 10 msec ) I oo e.4 uJ 0 ooo 2 1 1 3 110 TIME( sec ) JOINT C VELOCITY ( SERVO TIME 10 msec u 4 Li -19 9I TIME( msec JOINT 4 ACCELERATIOri ( SERVO TIME 10 msec Figure 8.4 Joint 4 Trajectory of the Straight LinLe

164 & a000 sea 1 *I I iO 1 10 TIME( msec ) JOINT 5 POSlTIOn ( SERUO TIME 10 mc ) 193 cE TIM oe ec ) JOINT 5 VELOCITrY ( SERVO rTIME 10 sec ) 19 00 e0 eO t *2 t m19 1 tto TflE( nsec) JOXIT 5 CCELERAT1O C SERVO TIME 10 msec Figure 8.5 Joint 5 Trajectory of the Straight Line

165 41 - 29, O OIJO ),~ll 6llI00i l1 3I I 7110 TIIE( nsec ) JO1NT 6 POsti5Or1 ( SERJO t tME 10 msec) Li o 7 a 000 o~0 o o it Oe I I 7to TIME( msec ) JOINT 6 UCLOCITY ( SERVO ME 10 nsec ) * 3771 - - -33 7a *,., Tlrtl( msec ) JOINT 6 aCCELERT ION ( SERUO r]IEC 1O msec ) Figure ~.5 Joint 5 Trajectory of the Straight Line

east 1t368 a# J W 0683 oo 000 o 000 '4150 8300 1 45 1 660 T-IME( msec ) Figure 6.7 Traveled Length versus Time Curve

187 6.1.2. Sampling Effects on Trajectory Planning The effects of discrete time approximation in Eqs. (3.5)(3.7) are investigated in this section. Different servo time intervals are used for trajectory planning of the same straight line path as in Section B.1.1. The smoothness constraints in Table 8.2 are used. It is expected that a large servo time interval may induce erroneous results due to the discretized approximation of the joint velocity, acceleration and jerk. In Section 3.4.2, the existence of a straight line trajectory for a given path requires that the inverse kinematic solution from pi - Al(k ) be bounded by the smoothness constraints and that from 2 - AX1(k) not be bounded by the smoothness constraints in the algorithm FW. The use of a large servo time interval may initially result in large approximated error in AX1(k), which in turn induces a failure in satisfying the requirements for the existence of straight line trajectory from the search algorithms. Thus, the existence of the solution from the search algorithms is questionable if a large servo time interval is used to approximate the joint velocity, acceleration, and jerk. In this simulation, the same values of p, and 92 as in Section 8.1.1 were used for the algorithms FW and MFW, respectively. Figures 8.8 - 8.13 show the joint position, velocity, and acceleration trajectory for the given straight line path with a servo time period, 20 matc, where markers are used on the trajectory for every 80 mncc. Again, most of joint trajectories are found to be constrained by the limits from the jerk bound and the given straight line requirement. Figure 6.11 shows that the acceleration of joint 4 reaches the maximum allowable limit for the acceleration and deceleration portion of the straight line trajectory.

188 For the same straight line path as simulated in Section 8.1.1, the following effects from using a different sampling period ( 20 maec ) are found: (1) the total traveling time is reduced to 1.62 sec from 1.71 sec of Section 6.1.1. (2) the break point exists at 42A sampling instant. (3) the Cartesian distance error at the final search point is found to be 0.242 mm with the maximum Euler angle error of 0.038 degrees ( before relaxation ). (4) the Cartesian distance error at the final search point is found to be 1.749 mm with the maximum Euler angle error of 0.258 degrees ( after relaxation ). (5) the required number of relaxation points is found to be 3. These can be compared with the contents in Table 8.3. By increasing the servo time interval to 30 msc, it was found that the discrete time approximation results in a failure of applying the search algorithms. As a remedy of this case, the procedure ESLT in Section 3.4.2 can be applied.

169 m 26 0 000 Wee 24 2 1 29i 1 a2 TI M( msec) JOINT I POSITION ( SCRUO TIME 20 sec ) l 000 U 4z LI cr -15 3,, o 00 o %660,20. 29s, 620 TIME( msec ) JOINT I UVELOCITY SERVO tItlC 20,sec 3?' 34 * m u 4, 41 'A — i, 39 a oeo uqo 6agso t,20 o 296 g o TIME( mse~ ) JOINT I ACCE~LERATION ( SERUO TIME 20 msec) Figure ~.8 Sampling Effects on Joint 1 Trajectory

170 s 4 4, i ii i ii i ~ 9L I4 eco s*@ WG 0711 * se I TI1E( 5e ) JOINT 2 POSITrlOt SCRUO TIMC 20 msec ) A 4, * *oo s t \ 4, UI pI D _?-,.77... TI IE msec) JOINT 2 UCLOCIRT ( SERSUO rtlE 20 msec 4, U' \ UI O-1 N -19 30 0 00 o 2?qO 46O 97;0 1 239 s 4aO T IME( msec > JO tIT 2 aCCELERaT I 0rN < SCRUO r I~ 20 msec Figure 6.9 Sampling Effects on Joint 2 Trajectory

171 La. 71W o o20 G*0 972 2 s1a0 TIME( msec) JOINT 3 POSITYOrN ( SERUO TIME 20 se ) 25 9 4, 0 o ago o 00 972 1 29 1 020 TIrE( nsec JOINT 3 UCLOCITY ~ SECRJO TIMe 20 0ec ) A I 75o 4, Figure 8.10 Sampling Effects on Joint 3 Trajectory

172 w -'I.1 \ opo~~~~~~~~~~e o, A~sao TIMC( asec ) JOINT 4 POSITlON ( SERUO TIME 20 sec ) * 600 U o,ao 11, -,;.-20 TIME( msec) JOIHT A UCLOCITY ( SERUO TIME 20 se~ ) 0 o o0o. Z4O,.o 972o 1 29 %, e TIMEC mseC ) JO1INT ECC~L~OCrTY O ( SERUO TI 20 0 sec ) '4 9 * 0 J4O0 7012 2 v~ ~ ~ ~~~~~TMC"e w~ ~~~~ON 4~CLR~INCSROTM 0"e Fiue61 a pigEfct nJit4Taetr

173 o tool 716 TlM( nsec ) JOIt T 5 PtLOCTtq ~ SEItO rT1E 20 nsec ) ~ -0 a I '3 -2309- I M -I O 0 1 23 6 ' TIME( msec ) JOINT 5 CCCELEROCTION ( SERUO tE 20 sec Figure 6.12 Sampling Effects on Joint 5 Trajectory

174 3,3, 000 se a* J o TIME( se: ) JOINT 6 POSltrON ( SERVO TIME 20O msc I I" u C 3 S 000 f TIME( se ) JOINt 6 VELOClrIY C SERVO r jmf 20 mse) U L, U CL -31 '.. O 000 fl 0240 9L7o 1 094 I a20 TIME'( msec) JOlINT 6 aCCELE~RATOi1 ( SERUO rTnE 20 msec ) Figure 6.13 S&mpling Effects on Joint 6 Trajectory

175 6.2. Existence of Straight Line Trajectory This section describes the simulation results on the existence of the straight line trajectory. The procedure ESLT discussed in Section 3.4.2 is applied to a: different path from Section 6.1, which must be planned by the procedure ESLT. The path is shown schematically in Figure 8.14. The points A and F are the initial and the final point, respectively. The application of the algorithms FW and MFW to the path is found to be a failure. The initial estimate pl - AX1(k) did not satisfy the smoothness constraints during the execution of the algorithm FW. Thus we apply the procedure ESLT to the path. The point B is the final search point of the algorithm FW. From point B, the algorithm MFW is applied with point B as an initial location and F as a final target location. The algorithm MFW is stopped at a point C on the path. Hence, AB is an acceleration portion and BC is a deceleration portion of the straight line segment A C. From point C, the FW algorithm is applied again with point C as an initial location and F as a final target location. It is found that the algorithm FW is stopped by the stopping variable 61. Since the application of the algorithm is completed and stopped by 61, the algorithm BW and the procedure BR are applied to find a break point on the segment CF. The break point is denoted by a point D. From the break point D, the algorithm MFW is applied until it is stopped at point E by the stopping variable 61. Thus, CD is the second acceleration portion and DE is the second deceleration portion of the straight line trajectory. As a result of the application of the procedure ESLT to the given straight line path, the resultant trajectory is composed of two acceleration portions and two deceleration portions. The points B, C and D are the break points in this simula.

178 tion. The detailed data for these points are shown in Table 8.5. Also, the traveled length versus servo time curve is drawn in Figure 6.15 without the relaxation procesM. The joint position, velocity and acceleration trajectories are shown in Figures 86.18-.21, where markers are used for every SOmscc.

177 Acceleratiuu u'ceieratlon Acceleration Deceleration A B C D EF Initial Point Break Point Break Point Break Point / Final Search Point Final Point Figure 6.14 Straight Line Path for the Procedure ESLT

178 pNoi SW, p Aim p ps o M I s au i+ 4 t t 6 u e oroint Psim PA 'OA 4I1.34000000000 6N m._ --- — I0.06006 90.so. qA 0.000-00. O666.6a006.00oo0.-00000m *.le qA..00.000. t000M..oMM o *.00000.omowo *. PR g I 4.S11 &4"7M t63,Io 41.6 s454 65.3431 0SLIM6 3B. 4.L44t713M 311S1.1*tS36.41.10106 33.47 Qs I1L'E)8~ la7-Tl~lms it~I I l-8 l42s00 -~c5t~l q8 -13&TON6 2 14i1753 -2816M 4JMd5ll!,.61380 467663 Pc,c 4.10 44.2502S 4097~71.t2537 0.674t6" C qC -L4 4L7311N6 4.313:3.3.4Sg3 16114 -to104 qc Lr 66 -| 1 121416 |.455S 76.6643 | 47.0300 13 l651 PD D 4.0143.4270.24156 54. 335 7T2.1101676 TT.321 D Q 4.I64IS 6.56 -11.94486 41310 30.11012 -.10.7741 qD - -17.424 11f67171 - - 133 4L411 20.6675 -34 1t7644 Pt,' 4." 076.4043112 s1 "8 604 7&s4 74616O E qf.11046 2.a3 4.047lI -13.445010 6L8NM1 45.8477 qf IL441169M.IL48=1 259152 607.611873 47.5M040 27.n 174 op,4,, -o 0..07w 6.4040.5.000000. 02. 7.o. 74..OO0 F 0p.000 *.00oom 00 6.000 0.ooo000000 O._ _ *.- - q.000 ts.o 6.00e06.e00i0 t.Som0 6oW080 Units: p is in meters, + is in degrees, q is in deg/sec, and q is in deg/sec2 Table 6.5 Break Point Information from the Procedure ESLT

179 J o 000 3650 7300 1095 1 460 TIME( msec ) Figure 6.15 Traveled Length versus Servo Time Curve

180 -o oaO I *" TIME( msec >.)flriT I P3IrTIC'4 ( SEPuO T I r 10 mI se t 00 k O -\ r0 -6 155,, 0 000 22: 5840 8760 1 1 TII1E(s msec) JOIrT r 'o- CIr Y ( SERSO T IME 1C.se ) Q, Oa Li -17 u49 0 000 N2B. 8760 1 j?& TIME( msec ) JOINT I ACCELEPATION ( SERUO TIME 10,se:- Figure 6.16 Joint 1 Trajectory from the Procedure ESLT

181 TIMlE( msec) JOINT 2 POSITION ( SER'JO T[ME 10 -se- ) e n O 000 C. TIME( msec) JOIN" 2 UELOCITY ( SE0jo T[ ' I 0 T-e- ) 15 20,I I Q 000 29z: I IGO TIME( msec) JOINT 2 ACCELERATION ( SERUO TIre 10 sec ) Figure 8.17 Joint 2 Trajectory from the Procedure ESLT

182 JOINT 3 POSITI ' ( SEtUO T1I" It0 Osec aG LJ A I _ / Q-s? 07 C),-, / -J z -37 07 0 000 2U23 '0., '-: TI!ME( msec JOItNT 3 CCELETIO S1 T' r IM 10 se Figure 6.18 Joint 3 Trajectory from the Procedure ESLT

183 -qo Ir (L Z a - - o 00o oo 220 0s 60 1 I TIME( msec ) JOINT 4 POSITIO ( SEQ'JO TIME 10 n.ec 0 000 40 Q4,. -t0 91 Oi // 0 C: 293 70 0T - 4, 4 - 4, I IA 0 00 920 S$l1 JOINT 4 ACCELERATON ( SERUO TIrI 10 se \I. Ot Q~ \7 Figre 8.lg.lo]~& 4 Tr~jectory [rom &he Proceclure ESLT

I, 'C,,l f.6c~~~~~~ L o6~~~TIE( ose- ) J.INI 5 PO,,'IT C' SERVO TIM 10 I, a, LI -)Z t2-. o o0. 2~E: -4^ -160 ' 15G!,4. 0 IT 000 " ' ~ S_~OTTI b sec 61? - -5 ' 9 558. 4 0 000 2920 5,~ 867& I If is I tO TIME( msec _o[M T 5 AICCELEt.T I ot ( SERvO rIlE 10 se- ) Figure 6.20 Joint 5 Trajectory from the Procedure ESLT

185 e I 16 56 ". 0 000 292* ' 5O0 1 16: TlMtE(.ec ) JOINT 6 POCSITlO' ( SEr.'O TIM 1Q me ) @wP T IME '' -5 SZ7 ^.? cr a -I 9 o 0 2CC 5'3 870 1 69 1 92 TIE( msec ) JOINT 6 'CCELIERT ( cEP' ( TIMEr 1 la 10se e UFgr.2 oi jeo fr thPodeE/ 0 000 29)20 ~ 5(1 i.6 I [ ': '.JOINT 6 ~ CCtLEt2TI0N ~ SE t'uO T[I 10 I0 ) Figure 8.21 Joint 8 Trajectory from the Procedure ESLT

188 6.3. Trajectory Planning of Connected Straight Line Segments The simulation of two connected straight line segments is described in this section. To follow the connected straight line segment faithfully, the robot must stop near the intersection point of the connected straight line segment for changing the traveling direction to the final desired point. As discussed in Section 3.4.3, if we adopt the cubic splined relaxation process, the relaxation portion of the straight line trajectory can meet the desired boundary conditions at the final location [34]. That is, the intersection point can be reached exactly and the trajectory planning of a connected straight line segment can be performed as two independent straight line paths. Here, we present a simulation result from the discrete time relaxation process and show that there is an error near the intersection point of the connected straight line segments. In Figure 8.22, the connected straight line segments are shown schematically. The locations of various points are listed in Table 6.6, where break points, final points from the relaxation process, and initial and final points for the deviated straight line path are tabulated with corresponding servo time instants. The second straight line segment must be modified slightly because of the error in planning the first segment. The Cartesian distance error between the desired corner point and the actual corner point was found to be 0.274 mm with 0.04 degrees for the maximum Euler angle error. The collision-free path must tolerate this deviation for the collision-free motion planning. The joint position, velocity, and acceleration trajectories are shown in Figures 8.23 - 8.28, where markers are used on the trajectory for every 50 mace and arrows are shown at 1.71 sec for stopping instant at the corner point. Most of joint trajec

187 tories are found to be constrained by the limits from the jerk bound and the given straight line requirement. However, Figure 6.26 shows that the acceleration of joint:: 4 reaches the maximum allowable limit for the acceleration and deceleration portion of the connected straight line segments.

Relaxed Point D Final Search Point -Ice~le Break Point F Final Search point Corner Point I f Relaxed Point Final Point Break Point A Initial Point Figure 6.22 Simulated Connected Straight Line Segments

Point Intcrprctation PsP a -P sP Travel Time (rri) (in) (in) (dcgrcc) (dcgrcc) (dcRrcc) (SCC) A Initial Point -0.140000 0.560000 0.390000 0.000000 90.000000 90.000000 0 13 Break Point(ACC-DEC) -0.073779 0.503239 0.432570 14.190130 75.809088 75.009808 0.84 C 'Final Search Point -0.000021 0.440024 0.479902 29.994048 80.005951 80.005951 1.66 D Relaxed Point 0.000187 0.439840 0.480120 30.040067 59.959956 59.959956 1.71 E Corncr Point 0.000000 0.440000 0.480000 30.000000 60.000000 60.000000 F IBreak Point(ACC-D)E.C) 0.000097 0.391912 0.400062 37.221604 50.378154 55.178670 2.30 G Final Scarch PoinL 0.000000 0.3400.36 0.4110000 44.994643 40.007147 50.003566 2.81 11i I~cRaxcd Point -0.000001 0.339297 0.479999 45.105306 39.859365 49.9'29683 2.84 Final Point 0.000000 0.340000 0.4110000 45.000000 40.000000 50.000000 Table 6.6 Connected Straight Line Segment Location Information

190 8Z 44 0 000 Xal 1 139 17;0,. 2 2'2 2 0 TIME( sec ) JOI4T I POSITION ( SERUO TtMC tO asec ) -15 33. ui 0.II JOHNT I UCCELEROTY r SERUO RUOtE 10 Omsec 03 03 F uue82 on rjcoyo CnetdSrih ieSget

191 3 " a a eq 4,'o It'te nSec ) JOrItt 2 POSITIr ( C SERVO TIC 10 sec ) W 6 00 I 1G3 t o12 21 a" TIME(,sec) JOINT 2 UCLOCITY S< RO OttE~ tO sec ) % -t II011 U -I -20 22 000 I M 11 2 2 10z' * O JO Nt 2 1CCELERAtIt:I C SEROJ tl 1 O - se Figure 8.24 Joint 2 Trajectory of Connected Straight Line Segments

192 t. I '1 @060 s e as'o0 0 0o0 50 Is i. I afa TIrMEE( sec ) JO[NT 3 POSlTION ( SERUO TIME 10 msec JOINT 3 UCLOCITY C SERVO TIEC 10 nsec ) uI TPIE( msec) JOlITr 3 VCC~LERaTYrl ( SERUO TIME 10 msec ) Figure 6.25 Joint 3 Trajectory of Connected Straight Line Segments 30 9.9.9

193 I O1\ a, b, -I a.-n. - -.. a ogO o I J36 I 2O? 22 240 TIME( msec) JOINT 1 POSITI:Of ( SERtJO TIME 10 msec a, La -57 '47 0ooo s50 0t:' I; Soa..o TIME< msec JOINT 4 UELOCITY ~ SERCO TrtM 10 ~ sec 710 01ms _ \ - 03k't -0 07, _1 o ooo000 0 1I 3, s; 0o 2 21a 2,o T[ItlE( msec JOINT 4 nCCELERATION ( SERVO TIME 10 msec) Figure 8.26 Joint 4 Trajectory of Connected Straight Line Segments

194 P7 S) 4r,. ', o st 0 ooo 540 t is 2,o, 2 2a o T ME( sec ) JOINT 5 POSITIOrt SERUO TIME 10 lsec ) 600 u ap -14..,. @ 000 00 113 I t7' 2 217 2 l0o TIEC( msec ) JOINT S.U LOCITY SERVO TuME 10 nsec ) uI -J 2: 0000 I01 1136 tA,Q 2ao 4 TIME( msec) JQorIT 5 ACCELERATIOr ( SERUO TIME 10,sec) Figure 8.27 Joint 5 Trajectory of Connected Straight Line Segments

196 a, Sl 0 000 t 13 i, " 2 27d 2 II b 19 * ). '03 o 000 0 13 04 2 2P2 2 eBs TIME( msec ) JOaINT 6 PELOCITY ( SERVO TIMIE 10 sec ) iA o" 0 000 2 840 TIME( msec) JOINT 6 CCELERATIO ( SERU O TrIME 10 msec IA I 7 @9. @9 0 r8 ~ i ~ t u @9 ~ ~ ~ ~ ~ ~ ~~Ttt( ne

196 8.4. Time Scheduling of a Straight Line Trajectory The time scheduling of a straight line trajectory is performed in this section. The simulation was done by applying the procedure TS to the straight line trajectory. We assume that a collision map is given as shown in Figure 6.29, where the collision box covers the length from 4.9 cm to 10.8 cm during the time interval from 0.42 sec. to 0.74 sec.. That is,,, the collision starting length, is assumed to be 4.9 cm and I, the collision ending length, is assumed to be 10.8 cm. From the collision map, we have two straight line segments; one ranges from the initial point to the collision starting point, while the other ranges from the collision starting point to the desired final point. The first segment was planned to constitute an acceleration portion and a deceleration portion and was found able to avoid the collision box. The second segment was planned by applying the algorithms FW and MFW with the initial estimate AX1(k), which can be obtained from the last trajectory set point information of the rust straight line segment. Figure 6.30 shows the collision map, where the traveled length versus servo time curve avoids the collision box. Figure 6.31 and Table 8.7 show the details of the simulation results for the time scheduling of the straight line trajectory. The total traveling time without the relaxation process was found to be 1.68 sec. from Section 6.1.1, while the total traveling time after time scheduling was found to be 1.97 sec. Detailed joint position, velocity, and acceleration trajectories are shown in Figures 6.32 - 6.37, where markers are used on the trajectory for every 50 macc. We investigate the effect on the final arrival time when speed reduction of the robot motion is performed for avoiding the collision region in the collision map. As

197 - discussed in Section 4.4.1, when the speed reduction is performed on the segment of length,, the traveling time of the segment of length 1, from speed reduction must be equal to or greater than the traveling time from the original trajectory plus the. required time delay for avoiding the collision region. From the location of length I, to the final location, the traveling time from the original trajectory is shorter than that from the trajectory after speed reduction because the robot on the original trajectory follows a maximum accelerating and decelerating trajectory. If we assume that the robot motion can be delayed at the initial location, then the required time delay can be computed from the collision map and the trajectory information. In Figure 6.29, the time kl was found to be 0.60 sec from the original trajectory information, thus the required time delay at the initial location will be (0.74 - k l) sec or 0.14 sec. Then, the final arrival time will be 1.80 sec which is shorter than that of the curve by speed reduction (1.97 sec). Now, we investigate the effects on the final arrival time when the speed reduction of the robot motion is performed well before the robot reaches the collision starting location. Figure 8.38 is the same figure as for the previous example ( Figures 0.29 and 0.30 ), where we denote the original traveled length versus servo time curve as OM and the time scheduled traveled length versus servo time curve as OMCD. If we consider two possible collision regions C and D as shown in Figure 6.38, the curve OMCD can be regarded as a curve which can be obtained by performing the time scheduling of the original trajectory well before the robot reaches the collision starting location for the collision regions C and D, respectively. When we consider the collision region C, which the curve OMCD can avoid, time k,c was found to be 0.70 see and time kc to be 0.97 sec with corresponding

198 lengths of 6.69 cm and 9.11 cm, respectively. These times and lengths can be obtained from the trajectory information ( trajectory set points printed out from simulation ) corresponding to curve OM. If the time delay is allowed for robot 2 to avoid the collision region C, the required time delay will be 0.27 sec, which yields 1.93 sec as the final arrival time. We notice that this final arrival time (1.93 sec) from the time delay is shorter than that from the speed reduction (1.97 sec). This corresponds to the curve OMA as shown in Figure 4.13. When we consider another collision region D, which the curve OMCD can avoid, time k D was found to be 0.99 sec and time kD to be 1.36 sec with corresponding lengths of 13.29 cm and 15.32 cm, respectively. These times and lengths can be obtained from the trajectory information corresponding to curve OMCD. If the time delay is allowed for robot 2 to avoid the collision region D, the required time delay will be 0.37 sec, which yields 2.03 sec as the final arrival time. We notice that this final arrival time (2.03 sec) from the time delay is longer than that from the speed reduction (1.97 see). Therefore, we conclude the followings from discussions in Section 4.4.1. and this simulation: (1) If speed reduction of the robot 2 motion is performed on the segment of length I, for avoiding the collision region, then the final arrival time from pure time delay of the robot 2 motion can be shorter than that from speed reduction of the robot 2 motion. ( See the collision maps in Figures 8.29-30. ) (2) If speed reduction of the robot 2 motion is performed well before robot 2 reaches the collision starting length 1, for avoiding the collision region, then the final arrival time from pure time delay of the robot 2 motion can be shorter ( See the collision map with collision region C in Figure 6.38. ) or

199 longer ( See the collision map with collision region D in Figure 8.38. ) than that from speed reduction of the robot 2 motion.

200 2051 368 O 000 o ooo 4 ISO kk 8300 l 2 5 1 660 TIME( msec ) Fiure 6.290 Collision Map with Possible Collision

2051 1 36? Qi LI Li 068) 0 000 a] 000.39L4O 7860 1 re2 5IS76;70 a TIMIE( msec ) Figure 6.30 Time Scheduled Collsion Map

Final Search Point C Final Point oo~~~~~l FialPo Relaxed Point KCC~ Break Point C B3Break Point C A Break Point Initial Point Figure 8.31 Simulated Straight Line Segment

Point Interprelalion pX p1 Pp a P 7 Travel Time (in) (in) (i) (degree) (degree) (degree) (sec) A Initial Point -0.140000 0.680000 0.390000 0.000000 90.000000 90.000000 0 B Break Point(ACC-DEC) -0.123841 0.545978 0.A40016 3.505483 88.494517 86.494517 0.42 C Break Point(DEC-ACC) -0.107964 0.532640 0.410594 6.884928 83.135075 83.135076 0.71 0 Break Point(ACC-DlC) -0.068696 0.488818 0.443388 17.796375 72.204634 72.204634 1.34 E~I )Inal Search Point -0.000046 0.440039 0.479971 29.990182 60.009817 60.009817 1.97 V Relaxed Point 0.001157 0.439006 0.480746 30.247780 69.761724 59.761466 2.02 G Yinal Point 0.00000 0.440000 0.480000 30.000000 80.000000 60.000000 Table 0.7 Time Scheduled TraJectory Location Information

204 m L' o. 800 1 2.2 1 1.... 2a 020 T IME( sec ) JOIlT l POS[ltON ( SERUO ttIME tO se ) Q q - % TIME( msec ) OIhT I UELOCITY SERVO TIM[ I0 msec La TlfE( mse ) JOlN[T l ACCELERCTION ( SEIJ T IMERO r ec l ) Figure 8.32 Time Scheduled Joint 1 Trajectory

200, h a au 4p 0 000 '0 6060 1 21 1 1.. ~OO TIPIE( rmse: ) JOIrT 2 PosIrlcrl ' SERJO triM io,ec )0 I 800 4, 4,.. _J -9 32? '..-.. ~ '" * 00a EO'.Ow 800 1 212 1 616 2 020 TIME( msec ) JOINT 2 VCLOIrTY ( SERJO TIME 1 0 msec) 16 17 Li 4 9 13 O 000 sag0 a80 t 2l32 1 616 a oo TItlE( msec JOINT 2 nCCELERATION < SERUO rtME 10 msec) Figure 0.33 Time Scheduled Joint 2 Trajectory

t I0X g62 w O000 mcoo 0 2l 2 020 TIMEC (se ) JOI1T 3 POSIT[o 0N SERVO TIME 10 use~ Fe,t -, JOINT 3 VELOCITY C SERVO TIME 10 mec Lai '-I 0 000 qOqO 8080 SosIo 212 0l2. 0 TIM'E( msec ) JOINT 3 ACECIERATON ( SERvU rIE 1o -sec r. St -I 0 0 0 0, 09 1 141 8 Q,~ ~ ~ ~ ~ ~~~TIE s' U ~ ~ ~~ON CCE~IO1CSROTM 0me F8 ue63 ieShdldJit3Taetr

207 -4 63 a * 22 1 a 0 TIME( msec ) JO [MT q POSIT I1nl SER'JO TIME 10 nsec ) goo0 0. 0_4 000 4 1 212 1 2 0oo TIME( msec JOINT q 'JLOCtIrY SERVO TM"C 10 nsec ) F0 01 U 0 000 s. 0080 L2 1 i16 2 00 TIMe msec JOINlT 4 ACCELERPATION < SER'JO TIME 10 msec Figure 81.35 Time Scheduled Joint 4 Trajectory

208 O PI 0 4J 0 000 so I" 1 I 1 36 2 020 TIME( rse ) JO tNT 5 POSIT[ on < CRJO TIME: 10 nsec G, Li TIME( msec ) JOlNT 5 UELOSIry C SERVO T[IM 10 msec 16" LA a 000 0140 ~~8 21 2 1 616 2 020 T IME( sec JOINt 5 ACCELCRTAION < SEPS TIME 10, sec Figure 8.36 Time Scheduled Joint 5 Trajectory

209 S I I.j / 224I 0 01t gouO I" l 2t2 1.L? 2 020 T 1IME( t-e, ) JOlrlr 6 PO Liroari c EiRJO TIC I0 nse- ) I dqt T IME( rse, JOItT 6 VCCELTYR(r OII ( S.,t r ME 10 msec ) 6600 a oo0 4O 00ag 2 020 TIME( IseME JQ[(T 6 LCCELERitYI ( SRuC O rI E 10 t msec 3iur g., -ieShdldJif8Taefr

~~~O 51~~~~~O OMCD ID I mC M I~lC Lo Li~ 0683 IC 0 000 I I I 0 000 3940?9;Q k ckP k ea 2 kD 5?60 T[ME( mnsec ) Figure 8.36 Comparison of Final Arrival Time

211 6.5. Summary In this chapter, the proposed trajectory planning scheme of a straight line path was simulated on a VAX-11/780 computer. The error between the final location of the trajectory set point and the desired final location was found to be dependent on the location of the break point. Several servo time intervals were used in the simulation to investigate the effects of discrete time approximation of joint velocity, acceleration, and jerk. It was found that the servo time intervals, 10 macc and 20 mac, were successful. in generating the control set points subject to the smoothness constraints while the 30 macc servo time interval leads to the breakdown of the search algorithms. The existence of the straight line trajectory was investigated by using '-he procedure ESLT. Connected straight line segments were also simulated and the Cartesian deviation error at the intersection point was found to be less than 1 mm. The time scheduling of the trajectory was performed in an assumed collision situation. The traveled length versus servo time curve after time scheduling was found able to avoid the collision region in the collision map.

CHAPTER 7 DISCUSSION 7.1. Summary and Conclusion In this thesis, we discussed an approach to the motion planning and motion control of two robots in a common workspace. The approach involves an investigation of straight line trajectory planning, collision-free path planning, and an adaptive control strategy in Cartesian space for tracking the collision-free trajectory faithfully under various payloads. Research results from these three areas constitute the major contribution of the thesis and they are briefly summarized below. A discrete time trajectory planning scheme was developed for the straight line path. A real time servo interval instead of a normalized time interval is used for planning the joint trajectory. The planning of straight line trajectory is performed in Cartesian space and requires maximization of the local speed between two consecutive Cartesian set points. All the Cartesian set points are found on the straight line path subject to the torque and smoothness constraints. An iterative forward and backward search algorithm was developed to determine the joint values at each servo interval. Break point and relaxation points are introduced to satisfy the boundary conditions at the final location of the straight line path. The resultant straight line trajectory is composed of three segments: an 212

213 acceleration portion, a deceleration portion, and a relaxation portion. The control set points on the acceleration and deceleration portions satisfy the straight line requirement while those on the relaxation portion do not. The existence of a straight line trajectory was also discussed and a procedure was developed to plan a straight line trajectory when the application of the search algorithms FW and MFW was not successful. The sets of joint position, velocity and acceleration can be used as reference inputs to the manipulator controller in the joint-variable space. Also, the corresponding sets of Cartesian position, velocity, and acceleration can be used as reference inputs to the manipulator controller in the Cartesian space. The proposed straight line trajectory planning scheme was simulated on a VAX-11/780 computer to verify its performance. Using the straight line trajectory and a sphere model for the manipulator wrist, potential collisions can be detected easily by checking the distance between the origins of the two spheres. For the collision avoidance of a moving robot in the presence of a stationary robot, a collision-free path was found subject to a performance measure. For the collision avoidance of two moving robots, a notion of a collision map is introduced which incorporates the location and the corresponding servo time information of the robots simultaneously. Several collision situations have been identified when two robots are moving simultaneously. When the path modification was not allowed for collision avoidance, the collision map was utilized to plan a collision-free time scheduling of the trajectory on the same straight line path. When the path modification was allowed for collision avoidance, a procedure was developed to replan a collision-free

214 path subject to a performance measure and a constraint on time scheduling. The time scheduling of a straight line trajectory was performed on a VAX-11/780 computer to show its application. For real time tracking of the desired collision-free trajectory, an adaptive perturbation control was developed in the Cartesian space. The control method adopts the ideas of resolved motion rate control and resolved motion acceleration control. Equations of motion of the manipulator were developed in Cartesian coordinates from which the linearized perturbation equations were derived along the desired hand trajectory. The control system is characterized by feedforward and feedback components which can be computed separately and simultaneously. The feedforward component computes the nominal torques from the Newton-Euler equations of motion using the joint trajectory information from the trajectory planner. The feedback component, consisting of a recursive least square identification scheme and an optimal adaptive self-tuning controller for the linearized system, computes the perturbation torques which reduce the manipulator hand position and velocity errors along the nominal hand trajectory. A feasibility study of implementing the adaptive control for a sixjoint PUMA manipulator was explored using present-day low-cost microprocessors. 7.2. Future Research We discuss future research issues with possible extensions of this thesis. The future research issues are not difficult to think of. These include the following: (a) Simulation of the proposed straight line trajectory planning scheme with actual torque constraints, which depends on the instantaneous joint position and

215 velocity. (b) Investigation of a collision-free path for a moving robot in the presence of a stationary robot subject to different performance measures. (c) Investigation of a collision-free path for two moving robots subject to different performance measures. (d) Simulation and implementation of the proposed Cartesian space control method. (e) Investigation of collision-free path and trajectory planning scheme in multiple robot system ( robotic system with three or more robots ). (f) Investigation of other wrist modeling of various robots and investigation of the resultant computational complexity for the detection of potential collisions. (g) Investigation of the time scheduling effects on the performance of multiple robot system. (h) Investigation of a collision-free path of the two moving robots considering the whole arm of a manipulator.

APPENDICES 218

X17 APPENDIX A A PUMA Robot Arm Configuration L, 5zoLI~2 82 IY / X10 d2 d| YY....._/ - /~.. -6 194 dex4 X3 3 5. '.. 2 | I 432 mm 14.5 mm -225 to.+45 i 410 [90 0 432.0 -110 to. +170i s 5 o oo i o -I 00 to o +1.00o1 a l oi [ 0 8,.5 -26 to +266,

218 APPENDIX B Homogeneous Transformation Matrix of a PUMA Robot From equation (2.1), the 4X4 hand transformation matrix H(t) for the PUMA robot in Appendix A can be written as the following: ns(t),S(t) as(t) ps(t) n,(t) J,(t) a.(t) p,(t) HMt)= n,(t),Z(t) az(t) p,(t) 0 0 0 1 where n (t) = C1[C23(C4CsC6 - Sr S6)- S23SsC6J - S1(S4CsC6 + C4S6) ny(t) S1[C23(C4CSC6- Sr S6)- S23SSC6B1 + C1(S4CSC6 + C4S6) nz (t ) = - S23( C CC6 - S4S) - C23SS C6,(t) = C1[ - C23(C4C5S6 + S4C6) + S23S5S6J - S1( - S4CSS6 + C4C6) 8y(t) = S C[- C23(C4C5S6 + S4 C6) + S23SS6] + C1( - S4C5S6 + C4 C6) a, (t)= S23(C4C5S6 + S4C6) + C23S5S6 a (t) C1(C23C4S5 + S23Cs) - S1S4S5 a,(t) = S(C23C4Ss + S23CS) + C1S45S a:(t) = - S23C4Ss + C23Cs

219 ps(t) Cl[d6(C23C4Ss + S23CS) + S23d4 + a3C2 + a2C2] - S$(d65455 + d2) py(t) = S$[d6(C23C4S6 + S2sCs) + S23d4 + a3C23 + a2C21 + C (d65,4S + d2) pz(t) - d6(C23Cs - S23C4S5) + C23d4 - a3S23 - a2S2 where Ci _ cosli S;i sini; Cii - cos(ei + 1.) Sij — sin(Di + ei)

220 APPENDIX C Kinematic Equations for Euler Angles Using the elements of the homogeneous transformation matrix in Appendix B, we can obtain: a(t) = tan[- a.S7 + a C' I s, ) S + yn+ i 7] n, C3 + nsy

221 APPENDIX D Inverse Kinematic Equations for a PUMA Robot Given the Euler Angles ( a, fr ), we have the following equations from equal tion (2.2): n.(t) = SC n.(t) = -S,9,.(t) = C -s

222 where - r< 81 <r Joint 2 Solution From the geometry of the first three links, we have R -= /p++ pP+i;2 d2 Pz Ps sinu - -, R 2 - "+ p_+ p_* d2 ARM.p,2 + p,2 COSU -- Ip2 + py2 + Pz a2 + R2-(d2 + COSYt 2 4 2a2R sinv -1 - cos2v Then, sinO2 = sinu cosv + (ARM ELBOW) cosu sinv cos82 = cosu cosv - (ARM ELBOW) sinu sinv and =2 tan- sin82 Joint 3 Solution Again, from the geometry of the first three links, we have

223 a2+(d + (d + 32 )- R2 2 2a 2Vd2 + a3 sint = ARM ELBOWW Vl1 - cos2w d4 Sin3 =3-sinD CosZ - COS-W sin cos03 COto COs3 + inflW Sinz and 03 = tan' 3 ] Joint 4 Solution 04 = tan C1 - S oa,1 4= [ C23(C1 a + Sla,) - S23 jz Joint 5 Solution 05 - tan' [(CIC23C4 - SIS4)a, + (S1C23C4 + ClC4)ag - S23C4a. t Can (Cla, + Sla],)S23 + C23az Joint 6 Solution (- S C CC23S4)n, + (C1C4 - S C23S4)n, + S23S4n z 6 tan ( - S1C4 - C1C23S4)a, + (C1C4 - S1C23S4)e, + S23S4nj

224 APPENDIX E Direction Number of a Line For the definition of the direction number of a line, we define the direction cosines of the line first. The direction cosines of a line are defined as the cosines of its direction angles which range between 0 and r from the positive directions of the global coordinate frame axes, xO,ro, and so to the positive direction of a given directed line in space. Then, the direction number of a line is defined as any triple of numbers except [ 0, 0, O ] which is proportional to the direction cosines of the line. If we denote a plane equation as, a1z + bly + clz - dl then [ a,, b 1, c will be the direction number of any lines which are perpendicular to the plane. Let us denote two plane equations as, a1z + bly + c1Z da2z + b2y + 2z d2 Then, the direction number of the intersection line of two planes must be perpendicular to [ al, b 1, c1 I and [ a2, b2, cb 2. If we use [ a,, / ] to stand for the direction number of the intersection line, they must satisfy the following equations. 1,C + blff + C1' =.0 a 20 + b 21e + C217 0 Therefore, the direction number [ ac, i,, 7 ] must be a proportional set of

225 Iblel |l |2 lb I I alb b2C2 ' - 2C2 1' a2b2 which is equal to [lC2 - clb2, c1a2 - ac2, alb2 - bS12 ]

BIBLIOGRAPHY 228

227 1. N. Ahuja et al., "Interference Detection and Collision Avoidance Among Three Dimensinal Objects," Proc. 1st Annual Nat. Conf. on Artificial Intell., August 1980, pp. 44-48. 2. A.S. Albus, "A New Approach to Manipulator Control (CMAC)," Trans. of ASME, J. of Dyn. Sys., Meas. and Cont., Septmber 1975, pp. 220-227. 3. K.J. Astrom, "Theory and Application of Adaptive Control-A Survey," Proc. of nt 'l Federation of Automatic Control, Great Britian, 1983, pp. 471-483. 4. A.J. Barbera, M.L. Fitzgerald and J.S. Albus, "Concept for a Real Time Sensory Interactive Control System Architecture," Proc. of the 4th Southeastern Symp. on System Theory, April 1982, pp. 121-126. 5. A. K. Bejczy, "Robot Arm Dynamics and Control," Technical Memorandum 33-669, Jet Propulsion Laboratory, February 1974. 6. Beyer, Standard Math Tables, CRC Press, 1978, pp. 283-316. 7. J.E. Boblow, S. Dubowsky and J.S. Gibson, "On the Optimal Control of Robotic Manipulators with Actuator Constraints," Proc. of Automat. Contr. Conf., June 1983, pp. 782-787. 8. M. Brady, et al; (editors), Motion Planning, MIT Press, 1982, pp. 221-243. 9. R. A. Brooks, "Solving the Find-Path Problem by Good Representation of Free Space," IEEE Trans. on Systems, Man, and Cybernetics, vol. SMC-13, No. 3, March 1983. pp. 190-197.

228 10. R. A. Brooks, "Planning Collision-Free Motions for Pick-and-Place Operations," International Journal of Robotics Research, vol. 2., No. 4, Winter 1983, pp. 19-44. 11. R. T. Chien et. al., "Planning Collision-Free Paths for Robotic Arm among Obstacles," IEEE Trans. on Pattern Analysis and Machine Intelligence, vol. PAMI-8, No. 1, January 1984, pp. 91-98. 12. E. Delp, "The CRC Plotting Package," Computing Research Lab., Univ. of Michigan, CRL-TR-14-83, 1983 13. S. Dubowsky, D. T. DesForges, "The Application of Model Referenced Adaptive Control to Robotic Manipulators," Transaction of the ASME, Journal of Dynamic Systems, Measurement and Control, Vol. 101, September 1979, pp 193-200. 14. P. Eykhoff, System Identification: Parameter and State Estimation, Wiley-Interscience, 1974, pp 240-241. 15. R. A. Frazer et al., Elementary Matrices, Cambridge University Press, 1960, pp 250-258. 16. E. Freund, "Nonlinear Control with Hierarchy for Coordinated Operation of Robots," NATO Advanced Studies Institute on Robotics and Artificial Intelligence, June 28-July 8, 1983, Italy. 17. E. Freund, "Hierarchical Control of Guided Collision Avoidance for Robots in Assembly Automation," Proc. of 4th Int7 Conf. on Assembly Automation," pp. 91-103, September 1983. Japan

229 18. E.G. Gilbert and I.J. Ha, "An Approach to Nonlinear Feedback Control with Applications to Robotics," CRIM, Robot System Division RSD- TR-4-83, Univ. of Michigan, Ann Arbor, 1983, and IEEE Trans. on Syst. Man and Cyber. vol. SMC-14, No. 8, Nov/Dec 1984, pp 879-884. 19. E.G. Gilbert and D.W. Johnson, "Distance Functions and Their Application to Robot Path Planning in the presence of Obstacles," Proc. of 18th Annual Conf. on Information Sciences and Systems, Princeton Univ., March 1984 20. J. M. Hollerbach, "Dynamic Scaling of Manipulator Trajectories," Trans. of ASME, Journal of Dynamic Systems, Measurement, and Control vol. 106, March 1984, pp 102-108. 21. R. Horowitz and M. Tomizuka, "An Adaptive Control Scheme for Mechanical Manipulators," Trans. of ASME, Winter Meeting, November 1980, Chicago 22. M. E. Kahn, B. Roth, "The Near-Minimum-Time Control of Open-Loop Articulated Kinematic Chains," Transaction of the ASME, Journal of Dynamic Systems, Measurement and Control, Vol. 93, September 1971, pp 164-172. 23. A. J. Koivo, T. H. Guo, "Adaptive Linear Controller for Robotic Manipulators," IEEE Transactions on Automatic Control, Vol. AC-28, No. 1, February 1983, pp 162-171. 24. C. S. G. Lee, "Robot Arm Kinematics, Dynamics, and Control", IEEE Computer, Vol. 15, No. 12, December 1982, pp 62-80. 25. C. S. G. Lee and M. J. Chung, "An Adaptive Control Strategy for Mechanical Manipulators," IEEE Trans. Auto. Contr., Vol. AC-29, No. 9, pp. 837-840,

230 September 1984. 26. C. S. G. Lee, M. J. Chung, and B. H. Lee, "An Approach of Adaptive Control for Robot Manipulators," Journal of Robotic Systems, Spring 1984, pp. 27 -57. 27. C. S. G. Lee, K. S. Fu, and R. C. Gonzalez, "Tutorials in Robotics," IEEE Computer Press, November 1983. 28. C. S. G. Lee and B. H. Lee, "Resolved Motion Adaptive Control for Mechanical Manipulators," Trans. ASME, J. Dynamic Syst., Meas., Contr., Vol. 106, No. 2, pp. 134-142, June 1984. 29. C. S. G. Lee and B. H. Lee, "Planning of Straight Line Manipulator Trajectory with Torque Constraints," Proceedings of the 23rd CDC Conference, Las Vegas, December 1984. 30. C. S. G. Lee, B. H. Lee, and R. Nigam, "Development of Generalized D'Alembert Equations of Motion for Mechanical Manipulators," Proceedings of the 22nd CDC Conference, San Antonio, Texas, December 14-16, 1983. 31. C. S. G. Lee, T. N. Mudge, J. L. Turney, "Hierarchical Control Structure using Special Purpose Processors for the Control of Robot Arms," Proceedings of the 1982 Pattern Recognition and Image Processing Conference, Las Vegas, Nevada, June 14-17, 1982, pp 634;640. 32. C.S.G. Lee and M. Ziegler, "A Geometric Approach in Solving the Inverse Kinematics of PUMA Robots," IEEE Trans. on Aerospace and Electronic Systems, vol. AES-20, No. 8, November 1984, pp. 895-706.

231 33. R.A. Lewis, "Autonomous Manipulation on a Robot," Tech. Momo of JPL, California Institute of Technology, Memo-33879, March 1974. 34. C. S. Lin, P. R. Chang, and J. Y. S. Luh "Formulation and Optimization of Cubic Polynomial Joint Trajectories for Industrial Robots," IEEE Trans. Auto. Contr., Vol. AC-28, No. 12, December 1983, pp. 106881073. 35. T. Lozano-Peres, "Spatial Planning: A Configuration Space Approach," IEEE' Trans. on Computers, vol. C-32, No. 2, February 1983, pp. 108-120. 36. T. Lozano-Peres, "Automatic Planning of Manipulator Transfer Movements," IEEE Trans. on Sys., Man, and Cybern., vol. SMC-11, pp. 681-698, October 1981. 37. J. Y. S. Luh, "An Anatomy of Industrial Robots and Their Controls," IEEE Trans. on Auto. Cont/Vol, vol. 28, No. 2, February 1983, pp. 133-153. 38. J. Y. S. Luh, M.W.Walker, and R.P.C. Paul, "On-Line Computational Scheme for Mechanical Manipulators," Trans. of ASME, J. of Dynamic Sys., Meas. and Cont., vol. 102, June 1980, pp.69-76. 39. J. Y. S. Luh and C. E. Campbell, "Collision-Free Path Planning for Industrial Robots," Proc. of the 20th CDC, 1982, pp. 84-88. 40. J. Y. S. Luh and C.E. Campbell, "Minimum Distance Collision-Free Path Planning for Industrial Robots with a Prismatic Joint," IEEE Trans. on Automatic Control, vol. AC-29, August 1984, pp. 875-880.

232 41. J. Y. S. Luh and C. S. Lin, "Optimum Path Planning for Mechanical Manipulators," Trans. ASME, J. Dynamic Syst., Meas., Contr., Vol. 102, June 1981, pp. 142-151. 42. J. Y. S. Luh and C. S. Lin, "Approximate Joint Trajectories for Control of Industrial Robots along Cartesian Path," IEEE Trans. Syst., Man, Cybern., Vol. SMC-14, No. 3, May/June 1984, pp. 444-450. 43. J. Y. S. Luh, M. W. Walker, R. P. Paul, "Resolved-Acceleration Control of Mechanical Manipulators," IEEE Transactions on Automatic Control, Vol. AC-25, No. 3, June 1980, pp 468-474. 44. B. R. Markiewicz, "Analysis of the Computed Torque Drive Method and Comparison with Conventional Position Servo for a Computer-Controlled Manipulator," Technical Memo, Momo-33-601, Jet Propulsion Lab, March 1973. 45. D. Nitzan and C.A. Rosen, "Programmable Industrial Automation," IEEE Trans. on Computer, vol.25, No.12, December 1976, pp. 1259-1270. 46. R. P. Paul, Robot Manipulators: Mathematics, Programming, and Control, MIT Press, 1981. 47. R. P. Paul, "Modeling, Trajectory Calculation and Servoing of a Computer Controlled Arm," Stanford Artificial Intelligence Laboratory, A.I. Memo 177, September 1972. 48. R. P. Paul, "Manipulator Cartesian Path Control," IEEE Trans. Syst., Man, Cybern., Vol. SMC-9, No. 11, November 1979, pp. 702-711.

233 49. Alan de Pennington et. al., "Geometric Modelling: A Contribution Towards Intelligent Robots," Proc. of the 13th ISIR/Robot 7 Conf., pp. 7.35-7.54. 50. A.A. Petrov and I.M. Sirota, "Obstacle Avoidance by a Robot Manipulator under Limited Information about the Environment," Avtomatika i Telemekhanika, No.4, pp. 29-40, Moscow, April 1983. 51. G.N. Saridis, "Intelligent Robotic Control," IEEE Trans. on Automatic Control, vol. 28, No.5, May 1983, pp. 547-557. 52. G. N. Saridis, R. N. Lobbia, "Parameter Identification and Control of Linear Discrete-Time Systems," IEEE Transactions on Automatic Control, Vol. AC-17, No.1 February 1972, pp 52-80. 53. K.G. Shin and N.D. Mckay, "Minimum-time Control of Robotic Manipulators with Geometric Path Constraints," to appear in IEEE Trans. on Automatic Control. 54. R.C. Smith and D. Nitzan, "A Moduler Programmable Assembly Station," Proc. of 13th ISIR/Robot 7, Chicago, IL, April 1983. 55. M. Shneier, et al., "Robot Sensing for a Hierarchical Control System," Proc. of 13th ISIR/Robot 7, Chicago, IL, April 1983. 58. J.A. -Simpson, R.J. Hooken and J.S. Albus, "The Automated Manufacturing Research Facility of NBS," Journal of Manufacturing System, vol. 1, No. 1, 1982.

234 57. M. Takegaki, S. Arimoto, "A New Feedback Method for Dynamic Control of Manipulators," Transactions of the ASME, Journal of Dynamic Systems, Measurement, and Control, Vol. 102, June 1981, pp 119-125. 58. R. H. Taylor, "Planning and Execution of Straight Line Manipulator Trajectories," IBM J. Res. Develop., Vol. 23, No. 4, July 1979, pp. 424-438. 59. Udupa, S. M. "Collision Detection and Avoidance in Computer Controlled Manipulators," Proc. 5th Int. Joint Conf. Artificial Intell., Los Altos, California, 1977, pp. 737-748. 80. R. A. Volz, T. N. Mudge, and D. A. Gal, "Using Ada as a Programming Language for Robot-Based Manufacturing Cells," IEEE Trans. on System, Man, and Cybernetics, vol. SMC-14, No. 8, Nov/Dec 1984, pp 863-878. 81. D.E. Whitney, et al., "Design and Control of Adaptable Programmable Assembly Systems," NSF 1st Report, R-1284, Charles Stark Draper Lab., august 1979. 82. D. E. Whitney, "Resolved Motion Rate Control of Manipulators and Human Prostheses," IEEE Transactions on Man-Machine System, Vol. MMS-10, No. 2, June 1969, pp 47-53. 63. D. E. Whitney, "The Mathematics of Coordinated Control of Prosthetic Arms and Manipulators," Trans. ASME, J. Dynamic Syst., Meas., Contr., December 1972, pp. 303-309. 84. C. H. Wu, R. P. Paul, "Resolved Motion Force Control of Robotic Manipulator," IEEE Transactions on Systems, Man and Cybernetics, Vol. SMC-12,

No. 3, June 1982, pp 286-275. 65. K. K. D. Young, "Controller Design for a Manipulator Using Theory of Variable Structure Systems," IEEE Transactions on Systems, Man, and Cybernctics, Vol. SMC-8, No. 2, February 1978, pp. 101-109.

UNIVERSITY OF MICHIGAN 3111111111111111111 0 2311111 11 1 11111 3 9015 03023 0505