Open Collections

UBC Theses and Dissertations

UBC Theses Logo

UBC Theses and Dissertations

Multiprocessor-compatible inverse kinematics and path planning for robots Poon, Joseph Kin-Shing 1988

Your browser doesn't seem to have a PDF viewer, please download the PDF to view this item.

Item Metadata


831-UBC_1988_A1 P66.pdf [ 6.83MB ]
JSON: 831-1.0098218.json
JSON-LD: 831-1.0098218-ld.json
RDF/XML (Pretty): 831-1.0098218-rdf.xml
RDF/JSON: 831-1.0098218-rdf.json
Turtle: 831-1.0098218-turtle.txt
N-Triples: 831-1.0098218-rdf-ntriples.txt
Original Record: 831-1.0098218-source.json
Full Text

Full Text

MULTIPROCESSOR-COMPATIBLE INVERSE KINEMATICS AND PATH PLANNING FOR ROBOTS By JOSEPH KIN-SHING POON B.Sc, Queen's University at Kingston, 1983 M.A.Sc, The University of British Columbia, 1985 A THESIS SUBMITTED IN PARTIAL FULFILLMENT OF THE REQUIREMENTS FOR THE DEGREE OF DOCTOR OF PHILOSOPHY in THE FACULTY OF GRADUATE STUDIES (Department of Electrical Engineering) We accept this thesis as conforming to the required standard THE UNIVERSITY OF BRITISH COLUMBIA May 1988 © Joseph Kin-Shing Poon, 1988 In presenting this thesis in partial fulfilment of the requirements for an advanced degree at the University of British Columbia, I agree that the Library shall make it freely available for reference and study. I further agree that permission for ex-tensive copying of this thesis for scholarly purposes may be granted by the head of my department or by his or her representatives. It is understood that copying or publication of this thesis for financial gain shall not be allowed without my written permission. Department of Electrical Engineering The University of British Columbia 1956 Main Mall Vancouver, Canada V6T 1Y3 Date: August 30, 1988 Abstract Novel algorithms in robot inverse kinematics and path planning are proposed. Em-phasis is placed on real-time execution speed with multiprocessors and adaptability to unpredictable environments. The inverse kinematics algorithm is an iterative so-lution which is applicable to many classes of industrial robots, and is stable at and around singularities. The method is based on a simple functional analysis of each link of a manipulator and projecting vectors on the coordinate frame of each joint. Heuristic rules are used to control a mobile manipulator base and to guide the ma-nipulator in the case of non-convergence caused by joint limits. The path planning algorithm uses a potential surface in a quantized configuration space. Paths are guaranteed to be collision-free for all parts of the robot. Local minimum regions on the potential surface are filled on demand by extending the obstacles. Arbitrarily shaped obstacles in 3-dimensions can be handled. Using a hierarchical collision de-tection technique, high execution speed can be maintained even with many complex shaped obstacles in the workspace. The path planning method can theoretically be applied to any manipulator with any degrees of freedom. The implementation of the inverse kinematics and path planning algorithms in a parallel hierarchical mul-ii tiprocessor computer structure designed for the control of robots is proposed and investigated. Communication among the processors is by point-to-point message passing via asynchronous serial links with message buffers. Computer simulations are used to demonstrate the appropriateness and feasibility of this computer struc-ture for robot control. iii Table of Contents Abstract ii List of Tables viii List of Figures x Acknowlegement xiii 1 Introduction 1 1.1 Robot Control Structures 7 1.1.1 Suitable Computer Architectures for Robotics 7 1.1.2 Previous Research 9 1.2 Organization 11 2 Kinematics 14 2.1 Coordinate Transformations 15 2.2 Differential Relationships 18 2.3 Previous Research 20 2.4 Problems with Closed-Form Solutions 24 2.5 Functional Joint Control 25 2.5.1 The projection technique for joints controlling orientation . . 27 2.5.2 Joints whose function is to change length 29 2.5.3 Overall manipulator control 30 iv 2.6 Evaluation of FJC 33 2.6.1 PUMA Simulations 33 2.6.2 Major Linkage Classes 37 2.6.3 Non-ideal functionally partitionable manipulators 38 2.6.4 FJC applied to redundant manipulators . 40 2.7 Computational Requirements 41 2.8 Joint Limits 42 2.9 Mobile Robots 45 2.10 Simulation of FJC with Joint Limits and Mobile Base 47 2.10.1 Fixed Base 47 2.10.2 Based Moved Only to Resolve Joint Limits 50 2.10.3 Mobile Base . . 54 8 Path Planning 57 3.1 Difficulty in Path Planning for Robots , 58 3.2 Operational Space and Configuration Space 60 3.3 Previous Research 63 3.4 The Proposed Approach 66 3.4.1 Potential Surface 67 3.4.2 Collision Detection 69 3.4.3 Local Minimum Regions 74 3.4.4 Memory Requirement Considerations . . . 77 3.4.5 Strengths and Weaknesses of Approach 79 3.5 Evaluation of Method 80 3.5.1 Two Link Manipulator 81 3.5.2 PUMA Manipulator 88 4 Hierarchical Architecture for Robotics 93 4.1 Parallel Architectures 94 v 4.2 Structure of Proposed Computer Architecture 96 4.3 Simulation Setup 98 4.4 Task Partitioning 101 4.4.1 Path Planner . 103 4.4.2 Kinematics 105 4.4.3 The Joint Coordinator 106 4.5 Multiprocessor Speed-Up 107 4.5.1 Kinematics 108 4.5.2 Path Planner 109 4.5.3 Interprocessor Messages 112 4.5.4 PHASAR vs Bus Architectures 115 4.6 Physical Implementation 118 4.6.1 General Hardware Structure . 118 4.7 Extensions to PHASAR 121 4.7.1 Task Planner 121 4.7.2 Trajectory Planner 122 4.7.3 Dynamics Compensation 122 5 Conclusions 124 5.1 Contributions 125 5.1.1 Kinematics 125 5.1.2 Path Planning 126 5.2 Suggestions for Further Research 128 References 131 A Analytical Kinematic Transformations 140 A.l Two-Link Manipulator 141 A.2 PUMA 142 vi A.3 The Twelve Major Linkage Classes 144 B Intersection Between Two Triangles 156 C Emulation of Multi-Processors 161 C.l The Time Manager 162 C.2 Passing Messages 164 D Design of the Mail Manager 166 E Proportional-Derivative Controller 171 vii List of Tables 2.1 Joint Functions of PUMA 25 2.2 Options Used in Simulations 33 2.3 Denavit-Hartenberg Parameters of Modified PUMA 34 2.4 Convergence Rate of Major Robot Linkages 37 2.5 Degradation of FJC 39 2.6 Computational Requirements of Approximate Transcendental Func-tions 41 2.7 Iterative Inverse Transform Computational Requirements 42 4.1 Interprocessor Messages, Part 1 113 4.2 Interprocessor Messages, Part 2 114 A.l Link Parameters for 2-Link Manipulator 141 A.2 Link Parameters for PUMA Manipulator 142 A.3 Link Parameters for Class 1 Major Linkage 144 A.4 Link Parameters for Class 2 Major Linkage 145 A.5 Link Parameters for Class 3 Major Linkage 146 A.6 Link Parameters for Class 4 Major Linkage 147 A.7 Link Parameters for Class 5 Major Linkage 148 A.8 Link Parameters for Class 6 Major Linkage 149 A.9 Link Parameters for Class 7 Major Linkage 150 A.10 Link Parameters for Class 8 Major Linkage 151 A. 11 Link Parameters for Class 9 Major Linkage 152 viii A.12 Link Parameters for Class 10 Major Linkage 153 A.13 Link Parameters for Class 11 Major Linkage 154 A. 14 Link Parameters for Class 12 Major Linkage 155 ix List of Figures 1.1 Basic Real-Time Robot Control Modules 4 2.1 Definition of n, o, a, and p for a Manipulator End Effector 17 2.2 PUMA Manipulator . . . ? 26 2.3 FJC Projection Technique 28 2.4 Adjustment of Gains 31 2.5 Start and End Configuration with Target in Good Manipulability Region 35 2.6 Simulation Results in Good Region 35 2.7 Start and End Configuration with Target in Singular Region 36 2.8 Simulation Results in Singular Region 37 2.9 Appearance of Standard PUMA and Most Degraded Manipulator . . 39 2.10 4-Link Planar Manipulator 40 2.11 Simulation of 4-Link Redundant Manipulator 41 2.12 Joint Limits of a 2-Link Manipulator 44 2.13 FJC with Heuristic Rules, Base Not Moved, Part 1 48 2.14 FJC with Heuristic Rules, Base Not Moved, Part 2 49 2.15 Base Moveable by Rules System, Part 1 51 2.16 Base Moveable by Rules System, Part 2 52 2.17 Base Moveable by Rules System, Part 3 53 2.18 Mobile Base, Part 1 55 2.19 Mobile Base, Part 2 56 x 3.1 Path Planning for a Point Object 59 3.2 Path Planning in Operational and Configuration Space 61 3.3 Obstacle Shadow in Operational Space 62 3.4 A Necessary Detour in a Manipulator Path 65 3.5 Neighbors in 2-D Space 68 3.6 Hierarchical Collision Detection Algorithm 72 3.7 Growing Obstacle Extensions 75 3.8 Path Planning for Two Link Manipulator, Part 1 82 3.9 Path Planning for Two Link Manipulator, Part 2 83 3.10 Path Planning for Two Link Manipulator, Part 3 84 3.11 Potential Surface of 2-D Complex Obstacles 87 3.12 Path Planning for PUMA, Part 1 89 3.13 Path Planning for PUMA, Part 2 90 3.14 Path Planning for PUMA, Part 3 91 4.1 Example of Graphics Screen 100 4.2 Task Partitioning in Simulated PHASAR System 102 4.3 Multiprocessor for Collision Detection Ill 4.4 Structure of One Processor Node 120 A.l Coordinate Frames of 2-Link Manipulator 141 A.2 Coordinate Frames of PUMA Manipulator 142 A.3 Coordinate Frames of Class 1 Major Linkage 144 A.4 Coordinate Frames of Class 2 Major Linkage 145 A.5 Coordinate Frames of Class 3 Major Linkage 146 A.6 Coordinate Frames of Class 4 Major Linkage 147 A.7 Coordinate Frames of Class 5 Major Linkage 148 A.8 Coordinate Frames of Class 6 Major Linkage 149 A.9 Coordinate Frames of Class 7 Major Linkage 150 xi A. 10 Coordinate Frames of Class 8 Major Linkage 151 A.11 Coordinate Frames of Class 9 Major Linkage 152 A. 12 Coordinate Frames of Class 10 Major Linkage 153 A.13 Coordinate Frames of Class 11 Major Linkage 154 A. 14 Coordinate Frames of Class 12 Major Linkage 155 B. l Intersection Between Two Triangles 157 B.2 Real and Phantom Intersection Points 158 B.3 One Real and One Phantom Intersection Point 159 B.4 Two Phantom Intersection Points 159 C l Multiprocessor Emulation Scheme 163 D.l Structure of Mail Manager 167 D. 2 Block Diagram of Communications Test Chip 169 E. l Block Diagram of PD controller 172 xii Acknowlegement I would like to thank my supervisor, Dr. Lawrence, for his helpful suggestions and constant encouragement throughout the course of my thesis. I must express my special gratitude to Dr. Pulfrey, who co-supervised my work at the beginning of this thesis. His knowledgeable guidance and continuing concern is much appreciated. I would also like to thank my colleagues, Derek Hutchison, Stephen Chan, David Gagne, Dr. James Reimer, and Darrell Wong, for the helpful discussions I had with them. Finally, I would like to thank Roderick Barman and Mike Bolotski for their help with the word processing tools used to produce this thesis. xiii Chapter 1 Introduction Currently industrial robots are used in applications limited to routine repetitive work requiring little intelligence. For instance, some are used in spray-painting and spot-welding. These robots have little ability to adapt to changing environ-ments[Nagel84], and they are often programmed by physically being guided between a series of set-points. The robot records the joint values at each set-point, which it can repeat afterwards. Since most robots have excellent repeatability, this ap-proach works well with strictly repetitive tasks, provided that the workpieces are always positioned precisely. Some advanced robot systems allow the user to enter the desired paths from a computer terminal, so that physically guiding the robot is not necessary. Programming robots by physical guidance is extremely tedious and can be dan-1 gerous to the human operator. Without the incorporation of advanced sensors, the robot cannot adapt itself to even slight changes in its working environment. For example, picking up randomly placed pins from a bin would not be possible. The use of graphics workcell simulators which allow off-line programming in Cartesian coordinates eases programming considerably. However, complete and detailed robot paths still have to be entered precisely. There is also the problem that the workcell model stored in the simulator may not correspond exactly to the real world, so a perfectly planned task may not be equally perfectly executed. Robots should be able to function in the presence of some uncertainty in the environment. For example, in transferring payloads, the weight of the payloads, their location and orientation, or even their shape, may not be known a priori. Dealing with uncertain environments requires the incorporation of advanced sensor technologies. Vision is useful for object recognition, position and velocity control. Tactile sensing is useful for force control and determining exact grasping orienta-tions. Proximity sensors can help prevent collisions. If such sophisticated sensors are integrated with robot systems, it may be pos-sible to program robots in a higher level, and hence more convenient manner. For example, instead of using basic commands such as: Move to (1.00, 2.13, -0.40) Close gripper Move to (3.23, 2.83, 1.00) Open gripper etc. 2 a typical robot task might be specified at a high level, such as: Take object O and put it in position P. This apparently trivial command is enough to illustrate the many decisions and calculations necessary in the execution of a simple robotic task. A qualitative description of how such a task can be accomplished will be presented below to illus-trate the basic ideas involved and the corresponding computational requirements in executing ordinary robotic tasks. Currently most of this work is done by the programmer. In an unpredictable environment, these decisions must be made in real-time. See [Albus81b] for an introduction to basic robotics. It will be observed in this simple example that although many issues have to be addressed in controlling a robot, they can be handled in a modularized man-ner. Fig. 1.1 shows the basic real-time control modules required. Such modules can be implemented with separate processors or processor systems that run in par-allel. Communication between modules will be seen to be frequent but short. A multiprocessor system using message passing is thus a viable way to achieve high performance robot control at low cost. Referring back to the example of transferring object O to location P, the current location of O must first be identified. This in turn requires that O be recognized, using perhaps a vision system. Much success in computational vision has been achieved in controlled environments, where structured lighting can be used, and where only a limited variety of objects will ever be encountered. See [Nagel84] and [Albus84] for a survey of recent progresses in artificial intelligence and robotics, 3 user interface other sensors teach-by-doing kinematics dynamics compensator x j r x joint servos Figure 1.1: Basic Real-Time Robot Control Modules 4 including robot vision. After the object is recognized, its coordinates must be computed. For simplicity, Cartesian coordinates are assumed to be used in the following discussions. The vision system can be made as a module that takes object names as inputs and returns object positions and orientations as outputs. Although extensive computation is performed within the vision module, the input and output messages are short, but they should be processed without delay. After the object is located (this analysis also applies to the target position, and any position in Cartesian coordinates that the robot has to reach), it is necessary to find out the corresponding pose, or configuration, of the robot that will allow it to grasp the object. The transformation from Cartesian coordinates to joint angles is called manipulator inverse kinematics. General information about manipulator kinematics can be found in [Paul8l|. The inverse kinematics module is thus another control module. It takes Cartesian coordinates as input and generates joint angles as output. This module is quite independent from the vision module and other modules that will be discussed later. Except for the highly controlled environments that current factory robots work in, it is quite possible that there may be obstacles in the workspace of the robot. These obstacles may be payloads, tools, other factory equipment including other robots, and humans. The robot must never run into any of these obstacles. A path planning module is thus required to compute detailed collision-free paths, given a starting point and a target as inputs. The paths generated by the path planner are only geometrical. Robot manipu-lator actuators, which may be hydraulic or electric, can only provide limited torque, 5 acceleration, and speed. Therefore, the geometric paths must be sufficiently smooth to allow the actuators to track the path. Execution time of the path should be short in order to increase factory throughput. A trajectory-planning module can there-fore be added. It takes in a geometrical path as input, and outputs a smooth path with the time coordinate added. Robot manipulators typically have highly coupled non-linear dynamics. Mo-tion in one joint will affect all coordinates. Although it is possible to control a manipulator that takes all of its dynamics into consideration, it is more common to compensate for the non-linear dynamics and use simple linear control methods. Such a dynamic compensation module will need information on position, velocity, and acceleration, and provide compensation torques to the joint controllers. See [Lee82a] for an introduction to manipulator dynamics, and [Luh83] for basic indus-trial robot control. The above decomposition of a complex robotic tasks into simpler modules is not the only way that the tasks can be tackled. For example, some researchers have worked on the possibility of handling inverse kinematics, force control, and obstacle avoidance with a unified approach[Khatib87,Hogan84b], while others tried to merge path finding and trajectory planning together[Sahar86]. These methods are compatible with the modular approach, although the breakdown of the task into modules will be different. 6 1.1 Robot Control Structures With the strictly repetitive tasks that most industrial robots today are employed in performing, fairly simple control systems are sufficient. The ability to record certain manipulator positions and go back to these locations on command is often all that is required. However, the next generation of robots will likely have to deal with task-level commands as discussed in the previous section. They may have to operate in continually changing environments which may be partially or completely unpredictable. Advanced sensing and control technology will be necessary to handle these situations while increasing robot throughput and work quality. In order to incorporate more advanced capabilities into robots, such as dynamic compensation and path planning, more sophisticated robot control systems are required. 1.1.1 Suitable Computer Architectures for Robotics Before designing a robot control system, it is worthwhile to review the various ap-proaches to high-performance computing in general, and then decide which structure is most suitable to robot control. As more research is done in the robotics area, problems and corresponding solutions will surface which may require computational structures that are difficult to anticipate today. However, there is now sufficient de-mand for high-power computational systems to implement existing robot control techniques. Investigations into special purpose architectures for robot control is therefore justified. If the architecture is flexible enough, future enhancements to the system will be feasible. 7 Much computation is necessary in the execution of a robotic task. High-speed feedback control is needed to accurately servo the joints of the manipulator. Joint angles have to be transformed from Cartesian position and orientation. Reasonable velocity and acceleration along the manipulator path must be determined. Paths should be geometrically feasible and collision-free. Manipulator dynamics have to be compensated for smooth motion. Contact forces have to be controlled. Objects have to be recognized, located. Their attributes, such as weight, optimal grasp ori-entation, have to be determined, to cite a few of the problems that are encountered. In an unpredictable environment, all these decisions must be made quickly and with minimal human intervention. The ability to react to environmental changes without prolonged delay (human reaction time may be used as a yardstick) will be referred to as real-time control in this thesis. Clearly ordinary mini-computers are insufficient to meet these demanding computational requirements. Perhaps the most straightforward way to achieve more computing power is to build a faster processor. Very high speed uniprocessors are however extremely expensive to build and maintain. Their application to individual robot control is thus not feasible, even if their speed is sufficient. It is also likely that because of the multitude of tasks involved, even supercomputers may have difficulty with sophisticated real-time control of robots, especially with so many I/O operations between the controller, sensors, and actuators. Although a large amount of computation is required in controlling a robot, much of it can proceed in parallel. For instance, while the kinematics module is computing the joint angles required to reach a certain target, the path planner may be working on what path to follow for the next target, the trajectory planner may 8 be smoothing out some previously generated path, and the vision system can be scanning for obstacles in the workspace. Multiprocessors can be built with low-cost microprocessors. They are simpler to implement and maintain than extremely fast uniprocessors, and they do not require special installation environments, which are not practical for most robots. Multiprocessors are therefore more suitable to advanced robot control than high-performance uniprocessors. 1.1.2 Previous Research Many researchers have studied various robot control structures that can incorpo-rate advanced control concepts. One of the first special purpose robot control structures came from Albus[Albus72,81b]. Albus attempted controlling manipula-tors with a software model based on a model of the human cerebellum put forward by Marr[Marr69], using a table look-up system. By repeatedly executing the same task a large number of times, the robot was able to improve its own performance automatically. Learning was effected by adjusting some stored coefficients using a simple increment/decrement technique, based on the difference between the in-tended output and the observed actual output. Due to the inherent parallelism of this control structure and the simplicity of its processing elements, the system promised great processing speed if a custom computer was built for it. The system was demonstrated to be highly successful for simple actions, such as 'reach out' or 'swap', but might encounter difficulties for more complex tasks or tasks that require higher accuracy. 9 Klett[Klett79] improved Albus's approach by using multinomials instead of look-up tables to learn manipulative tasks. He showed that satisfactory robot behavior could be achieved with smaller memory requirements by using multinomials rather than look-up tables. Huscroft[Huscro84] furthered Klett's work and applied the idea to the kinematic and dynamic control of a two link manipulator executing in a small workspace. Results were encouraging. The use of multinomials offered an even more exciting possibility of a highly parallel computer system, due to the regularity of the interconnections and the simplicity of the processing units. This direction has been pursued by the author. However, it was found that more research is needed to solve some fundamental problems if arbitrarily complex tasks are to be performed by a multi-link manipulator. Recently there has been some new work on the cerebellum model control struc-ture. The approach was recently adopted by Miller[Miller86,87] for learning manip-ulator dynamics, and by Kuperstein[Kupers87] for visual-motor coordination. In a series of papers[Albus81a,83,84,Barber79], Albus proposed a hierarchical computer system for robot control. This should be a good approach since there is ev-idence that the human brain is also constructed in a hierarchical fashion[Albus81b], and humans are generally the best manipulator controllers by far. In Albus's system, tasks were broken down in a top-down fashion. The system consisted of separate processors for each level of the control hierarchy connected by a shared bus. Ap-propriate sensors were also provided at each level. Each processor was programmed as a finite state machine that executed its task depending on the input from the other processors and from its own sensors. The application emphasis of this control structure was high level robot control, such as user friendly interfaces and overall 10 factory throughput. Tasks such as inventory control and multi-robot coordination were thus considered as part of the system. Orlando[Orland84] proposed a similar hierarchical system called DAISIE. Like Albus's system, DAISIE emphasized high-level robot control using distributed ar-tificial intelligence. Brooks[Brooks86,87] proposed a loosely coupled asynchronous computer system for use in mobile robot navigation. His system was built with a layered approach, and was called a 'subsumption architecture.' Processors communicate by passing messages via point-to-point asynchronous serial links with a variety of media, from simple twisted wire connections to radio links. Each layer utilized functions from the levels below it to provide more powerful capabilities. Every layer was fully tested before additional layers were added. A higher level controller could modify the inputs and outputs of lower level controllers, without the lower level controllers even being informed of its actions. The system was designed so that although inter-processor messages could often be lost, performance of the system would not be compromised. This robot control system had been custom built and quite satisfac-tory results were reported. 1.2 Organization Since robot control can be broken up into modules with simple inputs and out-puts, a parallel computer architecture using message passing is proposed and in-11 vestigated in this thesis. Two major issues in robot control: inverse kinematics and path planning, are incorporated into the proposed computing structure. The emphasis of the thesis is on real-time execution speed and adaptability to unpre-dictable environments. After a careful study of existing approaches in these areas, it was found that the basic requirement of well behaved real-time performance in unknown environments could not be met. Novel ideas that utilize the power of the multiprocessor system were therefore introduced, integrated into the multiprocessor framework, and tested by extensive computer simulations. The major contribution of this thesis is the unification of these novel ideas in kinematics and path plan-ning in a real-time package. The architecture should be expandable to deal with other manipulator control issues such as dynamics compensation, force control, and processing of various types of sensory information in the future. A novel approach to manipulator inverse kinematics is described in Chapter 2. The basic terminology in robot kinematics is first reviewed, followed by a summary of the virtues and shortcomings of previous research work. A new inverse kinematics solution which addresses some of these shortcomings is then presented. The method is tested with computer simulations and its strengths and weaknesses are addressed. Heuristic rules are proposed as a way to deal with convergence problems and improve manipulator positioning performance. In Chapter 3, a new real-time path planning technique is proposed. The difficulty of planning robot paths is discussed and simplifications taken by other researchers are outlined. Based some of the ideas of these researchers, a new real-time path planning technique is then proposed. The technique is able to handle arbitrarily shaped 3-dimensional obstacles. 12 A new parallel computer architecture is proposed in Chapter 4. The incor-poration of the inverse kinematics and path planning systems into the proposed computer architecture is discussed. The partitioning of the computational tasks into processors executing in parallel is explained. Detailed inter-processor messages are listed and the simulation setup is described. Conclusions of this dissertation are outlined in Chapter 5. 13 Chapter 2 Kinematics A robot manipulator reaches a target point in Cartesian space by setting its joints to some appropriate values. Therefore, the target point can also be expressed in the n-dimensional joint space of the manipulator, where n is the number of joints. For the robot programmer, locations of payloads or tools are usually conveniently specified in Cartesian space. On the other hand, the manipulator joint actuators must work in joint space. Therefore, coordinates often need to be transformed from one space to another. The transformation from joint space to Cartesian space is called the direct transform in robotics. The inverse transform is the reverse, from Cartesian space to joint space. The inverse transform is much more difficult to formulate than direct kinematics. In addition, there are regions in the Cartesian workspace, called singular regions, in which numerical problems with inverse transforms tend to cause undesirable behavior of the manipulator, or numerical problems with the program. 14 In this chapter, general coordinate transformations in robotics are first reviewed. Previous research in inverse kinematics solutions is then discussed, followed by the introduction of a novel iterative algorithm which enables more natural behavior around singularities. Iterative solutions have the possibility of getting trapped in a local minimum. A system of heuristic rules is employed to recover from these difficulties. Heuristic rules are also used to control a mobile base to further improve the positioning versatility of the robot. 2.1 Coordinate Transformations To describe the translational and rotational relationships between adjacent links of a robot manipulator, Denavit and Hartenberg[Denavi55] proposed a matrix method of systematically establishing a coordinate system to each link of an articulated chain. The relationship of the coordinate frame of link t to the coordinate frame of link i — 1 is called an A matrix, which is given by -cais9. CLiC9. Se< caic9i -sa;cti OiSfi 0 sai di 0 0 0 1 (2.1) where C${ = costfj, S9i = sin0,-, and a,-,0,- are angular parameters and d,-,a,- are length parameters specific to the links (see [Paul8l] for more detailed explanation of these terms). If joint i is revolute (or rotary, see [Paul8l]), then 0, is the joint 15 variable. If it is prismatic (or telescopic, or sliding), then d\ is the joint variable. The homogeneous matrix T%Q, which specifies the position and orientation of the end point of link i with respect to the base coordinate system, is the chain product of successive coordinate transformation matrices A\_v That is, T*o = ASAj . . .A |_ 1 . (2.2) If the manipulator has n links, then T 0 n = flM-i 1=1 nx ox ax px ny ov av pv nz Og az pz 0 0 0 1 where the vectors n, o, a, and p stands for normal, orientation, approach, and position, respectively, n, o, and a are mutually orthogonal unit vectors specifying the orientation of the end effector (or hand) of the manipulator with respect to the base coordinate frame, p specifies the position of the origin of the end effector with respect to the base frame. The names of these vectors originate from the descrip-tion of the orientation and position of a manipulator end effector, as illustrated in Fig. 2.1. Since a rigid body in space has 6 degrees of freedom (3 translational and 3 rotational), a manipulator must have at least 6 degrees of freedom (DOF) if its end (2.3) 16 Figure 2.1: Definition of n, o, a, and p for a Manipulator End Effector 17 effector is to attain any arbitrary position and orientation. If the manipulator has more than 6 DOF, then it is redundant. Redundant manipulators are more difficult to control, but they provide better flexibility, useful in tasks such as reaching around obstacles. Since the position and orientation of the manipulator end effector can be speci-fied either in terms of Cartesian coordinates or joint coordinates (angles for revolute joints or distances for prismatic joints), one can transform from one set of coordi-nates to the other. The direct transform expresses the Cartesian coordinates as a function of the joint coordinates. It is given by TQ. The inverse transform gives the joint coordinates corresponding to any set of Cartesian coordinates. Unlike the direct transform, there is no systematic way to obtain a closed-form solution for the inverse transform for an arbitrarily complicated manipulator[Paul81]. If the final three joints of a manipulator are of the revolute type and their axes intersect at one point, then the manipulator is said to have a spherical wrist. A closed-form solution can be obtained quite easily for this important class of manipulators, which include many industrial manipulators[Feathe83]. 2 . 2 Differential Relationships From the direct transform relationship x - /(q), (2.4) 18 one can derive the differential relationship x = J (q )q , (2.5) where J, the manipulator Jacobian, is defined by x -q = dxi -it x y z <pt <j>v <pt position and orientation in Cartesian coordinates, [«7i • • • qn]* generalized joint coordinates. (2.6) (2.7) (2.8) Usually it is the inverse relationship q = J - 1 x (2.9) that is of interest. The inverse of the Jacobian can be used in the iterative solution of the inverse kinematics problem[Golden85]. It can also be used directly on-line in resolved motion controllers [Luh80b,Whitne72]. A major problem with the use of J - 1 is that it may not exist. If |J| = 0, then the manipulator becomes degen-erate, and the corresponding point in space is called a singularity. A singularity represents a manipulator configuration in which the manipulator loses one or more degrees of freedom. Around singularities, very high joint rates are required for rela-tively low Cartesian velocities, so that singularities should be considered as a region rather than just a point[Paul83]. A quantitative measure of the manipulability of 19 a manipulator has been suggested by Yoshikawa[Yoshik85]: w = y'det (J(q)J'(q)), (2.10) which is an extension of the definition given in [Paul83] for redundant manipulators. 2.3 Previous Research Many methods have been proposed for the solution of the inverse transformation problem. The most straightforward way is to derive the inverse kinematics equations for specific manipulators manually. Examples of analytical derivation of closed-form solutions can be found in [Duffy80,Elgazz85,Lee82a,Paul84]. Closed-form inverse kinematics solutions are however very sensitive to inaccuracies in the measured link parameters, which are reflected directly as positional errors. For many manipula-tors, closed-form solutions are not even obtainable. For these manipulators, the inverse transform can be computed using an iterative procedure. Iterative solutions are attractive even for other kinds of manipulators because the solutions are less dependent on the availability of exact link parameters. Usually iterative kinematics algorithms are driven by errors in Cartesian space. Therefore, even if the exact values of some joint parameters are not known, such as the lengths of the joints, an iterative algorithm often can still drive the errors to zero. It is required, however, that there is some means of determining the Cartesian errors accurately, such as a vision system. 20 Most iterative techniques make use of the manipulator Jacobian, J. As discussed in Section 2.2, there are problems with using J at singularities. Many attempts have been made to deal with singularities. Most industrial robots are programmed in joint space, or they are programmed to avoid singularities[Mayorg87]. Some ma-nipulators are designed so that singularities are outside their workspaces[Trevel86]. Featherstone[Feathe83] considered manipulators with spherical wrists, and used ap-proximate formulas and interpolation to obtain joint rates in singular regions. Aboaf and Paul[Aboaf87] improved manipulator motion around wrist singularities by a two-part control policy. When a joint rate has exceeded its maximum, it is set to the maximum rate and the column of the Jacobian that represents the contribution of that joint is dropped. The Cartesian effect of the maximum joint rate is then subtracted off the desired trajectory to form a compensated trajectory. The relationship between joint torques, T, and Cartesian forces, F, is given by the transpose of the Jacobian, r=j'F (2.11) Since |J'| tends to zero rather than infinity as a singularity is approached, it does not have the numerical problems that calculating J - 1 has. Khatib [Khatib78,86a,86b,87] and Hogan[Hogan84a,84b] both used J' to solve inverse kinematics. However, when singularities are approached, |J| becomes vanishingly small, and so the joint torques given by Eqn. 2.11 also become vanishingly small, for any finite force F. The result is that the manipulator becomes extremely sluggish around singularities, thereby never reaching the target. Wampler[Wample86] used damped least-squares to limit the joint rates. The 21 cost of a large joint rate is balanced against the cost of a large residual error by minimizing the sum where I is the identity matrix, and a is a scalar damping coefficient. With this method, joint rates are forced to zero as a singularity is approached. However, since joint rates are low, sluggish motion will result in singular regions, and it is quite possible that the actual target will never be reached. When the manipulator does manage to reach a singularity point, it will never be able to move out of it because the joint rates would be set to zero regardless of the desired Cartesian velocity. Nakamura[Nakamu85] used a variable damping factor which was increased as the manipulability of the robot decreased. Although this method allows a manip-ulator to go through a singular region, it still does not permit the manipulator to maneuver within the region, since joint rates still tend to zero as the damping increases. Chan[Chan87j varied the damping factor according to the residual er-ror, and reported that manipulation within singular regions was possible with this approach. Milenkovic[Milenk83] simplified the computation by considering only those ma-nipulators that were separable. A separable manipulator has a major linkage por-||Jq-x||2 + a2||q||2. (2.12) The unique minimizer q a is given by (2.13) 22 tion (the axm) that serves to produce gross motion of the end effector, and a minor linkage (the wrist) whose purpose is to orient the end effector. Most industrial manipulators fall into this category. Inverse kinematics solutions were assumed to be known for both linkages. The overall inverse kinematics problem was solved by alternately computing the solution for the two sets of linkages. Based on a model of the structure of the human cerebellum, originally proposed by Marr[Marr69], Albus[Albus72] developed a new method of manipulator control, which included inverse kinematics, using table look-up. With this method, the manipulator can learn to position and move itself by repeated practice. However, it has the problem of requiring too much computer memory. Positioning accuracy is also poor. Klett[Klett79] and Huscroft[Huscro84] improved the approach by using a polynomial approximation instead of straight table look-up. However, since there are usually discontinuities in the inverse kinematics solution of a manipulator, an infinite degree polynomial is required to fit the actual relation. The polynomial coefficients may not converge with the learning identification algorithm proposed in [Huscro84], if discontinuities in the inverse kinematics solution are encountered during training. Much work has also been done to permit real-time computation of manipu-lator kinematics. Many methods for calculating Jacobians efficiently have been developed [Orin84a,Wample86]. Special computer architectures have been pro-posed[Orin84b,86]. Lee[Lee82b] proposed a VLSI architecture for use in robotics. Poon[Poon85a,85b] designed and physically implemented a custom VLSI chip that enables the direct transform to be computed at high speed, using table look-up for trigonometric functions. Leung [Leung87] proposed an architecture that had an 23 on-chip trigonometric function generator. 2.4 Problems with Closed-Form Solutions Many industrial manipulators have been designed carefully to facilitate the deriva-tion of closed-form solutions. However, because of manufacturing tolerances when the manipulator is constructed, link bending, sensor errors, and wear and tear, the ideal closed-form solutions often do not give precise results. For example, the PUMA has a closed-form solution. But if the elbow is fully extended, then a 1° error in joint 2 (the shoulder joint) will result in a 1 cm error in the end point position. This error is far greater than the repeatability of the manipulator, which is around 0.1 mm. Similar numbers were reported by Chen and Chao[Chen87] in their study of the effect of joint errors on the manipulator Cartesian position and orientation. If an accurate vision system is available, so that the exact Cartesian position and orientation errors can be measured directly, a good iterative inverse kinematics algorithm will be able to reduce the errors to zero. An example of such a vision system was reported by Ishii et o/[Ishii87]. Iterative solutions are therefore useful even for manipulators that have closed-form solutions. In this thesis, a new iterative inverse kinematics algorithm which is computa-tionally efficient and well-behaved around singularities is proposed. The method is called Functional Joint Control. 24 Joint Function Side Effects 1 orientation of p n, o, a 2 orientation of p n, o, a 3 length of p n, o, a, p 4 orientation of a n, o, p 5 orientation of a n, o, p 6 orientation of n or o nil Table 2.1: Joint Functions of PUMA 2.5 Functional Joint Control Many industrial manipulators have links that serve particular functions which are easily identifiable. The links can be partitioned into a major and minor linkage set[Milenk83]. The major linkage controls the end effector position and the mi-nor linkage changes its orientation. This class of manipulators will be referred to as functionally partitionable manipulators. Examples of functionally partitionable manipulators include the Unimation PUMA and Unimate, ASEA IRB, Cincinnati Milacron T3, and the Stanford manipulator. Table 2.1 shows the functions of the joints of the PUMA manipulator (Fig. 2.2). See Appendix A for detailed kinematic information concerning the PUMA, and Fig. 2.1 for the meaning of n, o, a and p. The proposed inverse transform solution works well only with functionally parti-tionable manipulators, hence the method is called Functional Joint Control (FJC). FJC has the disadvantage that its performance is dependent on the manipulator structure. As the design of a manipulator departs from the ideals of a functionally partitionable manipulator, the functions of joints become less well-defined, and FJC will require more iterations to converge. At some point FJC will fail altogether. It should noted that functionally partitionable manipulators do not necessarily pos-25 Figure 2.2: PUMA Manipulator 26 sess closed-form solutions. For instance, even if tiny offsets are added to each link of the PUMA 560 manipulator, an exact closed-form solution would be extremely difficult to derive, but the manipulator is still functionally partitionable. Often general solutions work at the expense of performance, and particular solutions are sometimes preferred[Hunt86]. If each joint of a functionally partitionable manipulator is controlled indepen-dently so that its function is fulfilled, assuming all other joints are correct, the target can be reached. However, most joints have side effects in addition to their main function. For example, the arm joints (joints 1, 2 and 3) of the PUMA affect the hand orientation, and the wrist joints in turn affect the arm position slightly. Therefore, FJC must approach the target in an iterative fashion. Milenkovic[Milenk83] classified all the distinct and useful major robot linkages into 12 categories. Kinematic information, including joint functions, about these linkages can be found in Appendix A. Identification of joint functions of wrist angles should be straightforward. 2.5.1 The projection technique for joints controlling orien-tation Given that the function of a joint is known, an effective way must still be devised to fulfill it. For any joint that determines orientation, such as the wrist joints, a projection technique was found to be useful. Notice that the shoulder joints of 27 Figure 2.3: FJC Projection Technique the PUMA (joints 1 and 2) determine the orientation of p (p/||p||, see Fig. A.2). Therefore, the projection technique also applies to the shoulder joints. Suppose the function of joint i is to control the orientation of a vector, v. The target orientation is v^ , and the current orientation is v (see Fig. 2.3). For joint t, the corresponding coordinate frame is Tj, with axes X,,Y,-,Z,. v,* and v can be projected onto the X-Y plane of Tj. Let the projections be \pj and vp. v p i. = v-X,-vPI/, = v-Y,. (2.14) All that joint i can do to help align v with vd is to align vp with vpd. The angle between vp and vpd, A0,, is given by AOi = arctan(vpdy./vpda;.) - arctan(vpv./vpi.), (2.15) 28 where \ p X i stands for the x-component of v p on the coordinate frame of link i, etc. The current positions and orientations in Eqn. 2.15, 2.16, and 2.19 are ei-ther observed by a vision system or are calculated by accurate knowledge of the manipulator forward kinematics. 2.5.2 Joints whose function is to change length The function of some joints is to alter a certain length, L (e.g. joint 3 of PUMA and Stanford manipulator, L = ||p||). For a sliding joint (e.g. Stanford manipulator), the relationship can be used. However, more care must be taken with a revolute joint. If the function of the joint is taken to be the alignment of p with Pd, and the projection method applied to it, the manipulator becomes sluggish when the target point requires the joint to be fully extended. If the relationship between L and the joint angle, Oi, is differentiated, excessive joint rates will result around singularities. The problem can be solved by writing A L=|M - | | p | | (2.16) (2.17) 29 and L \ = L \ _ x + L] - 2L,_1L, cos (| + ft,) , (2.18) where L< = length of link t, = desired length, and 0< is angle between links i and i — 1. Solving for 0,. = arccos cos (Ld + L) • (L, - L) 2L,_iL, 7T (2.19) With this equation, 0ii will be adjusted as long as there is still an error in AL = Ld — L . Owing to possible errors in the measured parameters, the argument to the arccos call must be hard-limited to lie between —1 and 1. This formulation not only causes Oi to converge in a well-behaved manner, it also has the following desirable property: If the target is beyond the reach of the manipulator, the arm will be fully extended and stationary. In contrast, with an algorithm that uses J - 1 , the manipulator will oscillate back and forth about the fully extended position. 2.5.3 Overall manipulator control With FJC used for each link of the manipulator, a vector of joint variable changes, Aq, can be calculated for each iteration. If Aq is added directly to the current joint angles to get the new angles, significant overshoot may result because of joint 30 Figure 2.4: Adjustment of Gains side effects. By incorporating a gain factor, <- Qki + Gi • Aqki, (2.20) where G,- = gain factor for joint i, and 0 < G,- < 1, the rate of convergence and overshoot can be controlled by adjusting G. However, the optimum value of G is different for different positions of the arm, and since it is not practical to adjust G manually, it must be adjusted adaptively. Suppose that Fig. 2.4 shows a joint oscillating. The amplitude of oscillation is changing from Aqk-i to Ag*. G can be set as follows: where 0 < 3 < 1, 7 > 1, 0 < 6 < 1, Aqk is the k-th oscillation extent. B, 7, and 6 are constants selected by the user. Usually, when an iterative algorithm is used to solve manipulator inverse kine-matics, the correct joint angles will be computed before the angles are sent to the manipulator joint servos. This approach has the advantage that the inverse kine-(2.21) 31 matics algorithm does not have to converge to the target smoothly. In fact, it might be desirable in some situations to return the manipulator to some fixed location be-fore attempting to reach each new target. However, since a number of iterations are required before the exact joint angles can be decided, real-time execution speed is difficult to achieve. With the amount of overshoot limited by a proper selection of G 0 (the starting value for G), /?, 7 , and 8, joint overshoots are typically less than 0.1 radian. It is thus possible to send the new joint values out to the ma-nipulator directly after each iteration. This reduces the demand on computation requirements for a real-time implementation significantly. This property is one of the major strengths of the FJC algorithm. The overall FJC control strategy is summarized below: While there is a position or orientation error do for every joint do If the joint function is to align orientation. then use Eqn. 2.15 to obtain Ag,-: / / the joint is prismatic, then use Eqn. 2.16; / / the joint is revolute but its function is to alter length. then use Eqn. 2.19; Adjust the joint angle with Eqn. 2.20; Adjust the gain with Eqn. 2.21; end while; 32 Cartesian position tolerance (mm) 0.1 Orientation tolerance (rad) 0.01 Aqmax(rad) 0.05 Go 0.3 0 0.5 1 1.2 6 0.8 Table 2.2: Options Used in Simulations 2.6 Evaluation of F J C In order to investigate the feasibility of FJC, over 100 simulations with a variety of targets have been tried. Most of these simulations were performed using a model of the PUMA manipulator. Some simulations were also performed with the 12 major linkage classes. Throughout all the simulations, the values in Table 2.2 were used. 2.6.1 P U M A Simulations Two of the simulations using the PUMA are presented in this thesis. Table 2.3 shows the Denavit-Hartenberg parameters of the modified PUMA used in the simulations. The parameters for a standard PUMA can be found in Appendix A. In the modified PUMA, the twist angles, a, are deviated by as much as 0.1 radians, and the offsets, a and d are deviated by as much as 1 cm. Table 2.2 summarizes the FJC options used in all the simulations. • P=\\Pd-V\\ 33 Joint 1 2 3 4 5 6 a (rad) -1.6 0.1 1.6 — 1.6 1.6 0.1 a (m) 0.01 0.432 0.01 0.01 0.01 0.01 d (m) 0.01 0.1495 0.01 0.432 0.01 0.056 $m a x (rad) 2.79 0.7 3.92 2.9 1.74 4.6 0min (rad) -2.79 -3.92 -0.7 -1.9 -1.74 -4.6 Table 2.3: Denavit-Hartenberg Parameters of Modified PUMA • Orientation error: A = arccos (A<j • A) N = arccos (Nd • N) • Aqmax = maximum joint rate • G 0 = starting gain In Fig. 2.5, the manipulator is moving from a position in a good manipulability region to another position, also in the good manipulability region. The joint angles at the starting position are (0.9, 0.4, 0.9, 0.9, 0.9, 0.9). The target corresponds to a location with the joint angles at (1.0, 0.5, 1.0, 1.0, 1.0, 1.0). The target is given in Cartesian coordinates to the FJC algorithm. Joint 2 is commanded to go from 0.4 to 0.5 because its range only goes from —0.7 radians to 0.7 radians. A small amount of movement is chosen because most other iterative algorithms have the most significant overshoots when both the starting and target locations are in a singular region. A comparatively small amount of movement is thus chosen for the good region simulation so as to compare the relative performance of FJC in good and singular situations. The changes in the joint angles and the Cartesian position and orientation errors are shown in Fig. 2.6a and 2.6b respectively. Notice the smoothness of variations in joint angles and Cartesian distance error. The non-ideal joint offsets can be seen quite clearly in Fig. 2.5. For example, the base of the 34 2 k target Figure 2.5: Start and End Configuration with Target in Good Manipulability Re-gion. 35 target Figure 2.7: Start and End Configuration with Target in Singular Region. manipulator is not exactly at the origin, but FJC assumes it is. Nevertheless, FJC can still servo the end effector to the target. In Fig. 2.7, the manipulator has to move from a point in a singular region (elbow almost fully extended and wrist aligned with forearm) to another point in the singular region. The starting configuration is (0.0, 0.0, 1.57, 0.0, 0.0, 0.0). The target is (0.1, 0.1, 1.4, 0.1, 0.1, 0.1). The corresponding joint angles and Cartesian errors are shown in Fig. 2.8a and 2.8b respectively. Although there are minor oscillations in converging to the specified target, FJC managed to reach the target in 14 iterations. The maximum overshoot (joint 4) is less than 0.1 radian. If J " 1 is used, the overshoots will be much more extensive. 36 Class 1 2 3 4 5 6 Iterations 9 27 121 12 18 22 Class 7 8 9 10 11 12 Iterations 9 13 9 21 33 13 Table 2.4: Convergence Rate of Major Robot Linkages. 2.6.2 Major Linkage Classes In order to demonstrate the generality of FJC, a PUMA-type spherical wrist was added to each category of the major linkage classes presented in Appendix A. Each arm was commanded to move from the zero position to a target, 0.5 m away for prismatic joints and 0.5 radians for revolute joints. The convergence rates are compared in Table 2.4. Although FJC converges for all the linkage classes, some classes are more suitable for FJC than others. 37 2.6.3 Non-ideal functionally partitionable manipulators Simulations have been performed to investigate the performance of FJC with non-ideal functionally partitionable manipulators. The paths and simulation options are the same as in the previous section. Table 2.5 shows the performance degradation of FJC with such manipulators. The standard PUMA manipulator is designated as Robot No. 1. The manipulator is degraded each step by adding errors to some of the Denavit-Hartenberg parameters as follows: a,\t=i,...,3: 1 cm/step a;|f-=4 6^  1 mm/step ai,f=i,...,6: l ° / s t e P Manipulator 2 is the one used in all the previous simulations. The parameters of the standard PUMA are used in the FJC equations in all the cases. Fig. 2.9 shows the comparison between the appearance of the standard PUMA (No. 1) and the most degraded manipulator (No. 6) used in the simulations. It can be seen that FJC degrades only slightly with good targets, but degrades faster when the target is singular. Manipulator No. 6 can only get within 0.6 mm of the singular target. 38 (a) (b) Figure 2.9: Appearance of Standard PUMA and Most Degraded Manipulator Robot Number No. of Iterations Good Target Singular Target 1 11 10 2 13 14 3 13 13 4 13 14 5 13 23 6 13 oo Table 2.5: Degradation of FJC 39 Figure 2.10: 4-Link Planar Manipulator. 2 . 6 . 4 FJC applied to redundant manipulators Because adaptive gains are used in FJC, redundant manipulators can also be con-trolled with FJC, provided 8 and 7 are chosen properly. Fig. 2.10 shows a planar 4-link manipulator with 4 revolute joints used in the simulations. Each link is 1 m long. The function of the first joint is to change the orientation of the p vector, and the rest of the joint control the length of p. No optimization function is used. The first solution that is found is accepted. The manipulator is commanded to move from an initial position with joint values (0.0, 0.0, 0.0, 0.0) to a target Cartesian po-sition of (2.0, 2.0). Fig. 2.11 shows the Cartesian distance error and corresponding joint values. 40 No. of Iterations No. of Iterations (a) (b) Figure 2.11: Simulation of 4-Link Redundant Manipulator. Function Multiplications Divisions Additions cos 1 0 7 arccos 1 0 6 atan2 3 1 1 sqrt 1 0 5 Table 2.6: Computational Requirements of Approximate Transcendental Functions 2.7 Computational Requirements Although transcendental function calls are required by FJC, the values returned by these functions calls do not need to be exact, since the gain factor G is used. From simulations, piecewise linear approximations were found to be adequate for these functions. Table 2.6 summarizes the computational requirements of such approximations. FJC compares favorably with other iterative inverse transform solutions in terms 41 Method Multiplications Additions FJC Vector Formulations 122 210 148 140 Table 2.7: Iterative Inverse Transform Computational Requirements of speed. The evaluation of J and its inverse is very computationally intensive. Orin and Schrader[Orin84b] made significant improvements in computational speed of J. The most efficient iterative inverse transform solution to date is the one proposed in [Wample86], where vector formulations of the inverse kinematic problem were used. A comparison of the computational requirements of FJC and the vector for-mulations and damped least-squares methods used in [Wample86] can be seen in Table 2.7. The figures are for one iteration of the Stanford Arm. Piecewise linear approximations are used for transcendental function calls with FJC. Because FJC controls each joint independently of one another, the computation burden can be naturally partitioned to one processor per joint. This is discussed in more detail in Chapter 4. 2.8 Joint Limits Manipulator joints almost invariably have a physically limited range. One of the problems with iterative inverse kinematics solutions is that sometimes even if a solution is available, the local searching used in iterative techniques may cause 42 the manipulator to become stuck. This is illustrated in Fig. 2.12a for a 2-link manipulator. The shaded triangular region represents the forbidden range of joint 1 of the 2-link manipulator. If the manipulator starts out from position 1, it will converge easily to the target. But if it starts out from position 2, joint 1 reaches its limit before the target is reached. Iterative algorithms will try to move the manipulator towards the target along the shortest path. Such a strategy will not be able to bring the manipulator out from this situation. In the situation depicted in Fig. 2.12a, the difficulty can be overcome if the manipulator starts out anywhere close to the target. If an approximate closed-form solution is available, then we can resolve the stagnant situation when the manipulator starts out from position 2 by employing the closed-form solution to obtain a starting position close to the target. The required tolerance can then be satisfied by applying the iterative solution. A more difficult arrangement is shown in Fig. 2.12b. Here the target is placed so that only one of the two solutions (elbow down) is feasible. In this case, even if the manipulator starts from near the target, for example, from position 2, the manipulator cannot reach the target, since the manipulator will try to reach the closest solution, which is not acceptable. Therefore, when the approximate closed-form solution set is computed, we must select only the legal solutions. If more than one solution is available, then the one with the smallest joint movements should be chosen. The requirement of the availability of closed-form solutions, albeit only approxi-mate, will of course make the inverse kinematics solution algorithm even less general. This is a tradeoff between generality and degree of autonomy. However, most in-dustrial robots are functionally partitionable, so even this technique of dealing with 43 Figure 2 . 1 2 : Joint Limits of a 2-Link Manipulator 44 joint limits with approximate closed-form solutions is still widely applicable. The following heuristic rules are used to handle joint limits: / / the manipulator has stopped moving for more than 10 sampling periods and the target is still not reached then manipulator-is-stuck. / / manipulator-is-stuck and any joint has reached its limit then compute closed-form solutions: select as target the legal solution with the minimum joint movements; disable FJC; / / FJC is disabled and manipulator is close enough to target then enable FJC. 2.9 Mobile Robots The case of a mobile robot with 2 degrees of translational freedom at the base has also been studied. The function of a mobile base can be considered as coarse adjustment of | | p | | . This can be useful in the following cases: 1. If the target is outside the workspace, it can be brought within by moving the base; 45 2. If the target is too close, manipulability [Yoshik85] is reduced. Moving the base away will improve manipulability; 3. Sometimes, because of joint limits, a desired orientation cannot be attained from a fixed base location. The mobile base can be considered as providing redundant degrees of freedom and hence controlled with the basic FJC algorithm. However, since the base usu-ally has a large inertia and cannot be controlled precisely, it was considered more practical to control it with heuristic rules. Let the distance between the base and the target be D. For PUMA-like manipulators, D o p t i m u m is about half the maxi-mum reach of the arm. At this distance, the elbow is half extended, yielding best manipulability[Yoshik85]. Let pe = ||pd — p||. The manipulator can be considered to be in a good manipulability region if Dmin < pe < Dmax, where Z?m,„, Dmax are constants specified by the user. The following rules can be used: / / no legal solutions for arm can be found and approach vector, a. is pointing towards the base then move the base to the opposite side until p e = Doptim If pc < Dmin then move the base away from the target until pe = Dopu If p e > Dmax then move the base towards the target until pe = Doptimu 46 2.10 Simulation of F J C with Joint Limits and Mobile Base Three simulations are presented in this section to demonstrate how the rules system can help FJC in arriving at a target. Manipulator 3 in Table 2.9 is used in these simulations. The simulations are shown with 'snap-shots' of the screen taken at regular simulated time intervals. The target is shown in each frame in the shape of a manipulator gripper. The inverse kinematics system always attempts to align the manipulator gripper with this desired gripper position and orientation. 2.10.1 Fixed Base In this simulation, the base of the manipulator is fixed on the floor. The purpose of this simulation is to test how the rules system can reconfigure the manipulator when joint limits are hit. The behavior of the manipulator is shown in Fig. 2.13 and Fig. 2.14. The target cannot be seen in the first 7 frames because it was very close to the gripper, so some explanation is necessary. The manipulator started with the posture in frame 1. A target was set up and the manipulator moved quickly and had arrived very close to the target in frame 2. However, the orientation was not correct yet. That was accomplished by frame 4. The target was changed in frame 5, and was reached in frame 7. New targets were set again in frames 8 and 10, and 47 L 1 2 3 4 5 V 4. 6 I 7 8 10 L 11 12 13 14 15 L Figure 2.13: FJC with Heuristic Rules, Base Not Moved, Part 1. 48 4 9 reached in frames 9 and 11 respectively. In frame 12, the target was set very low from the floor. Since the manipulator was in an elbow down position at this time, it could not reach the target because joint 2 (the shoulder) reached its limit. The manipulator became stationary from frames 13 to 15. The rules system constantly monitored the motion of the manip-ulator. When it detected that the manipulator had reached a joint limit and the target was still not reached, it applied its approximate closed-form inverse kinemat-ics equations and chose the solution most likely to succeed. It took over control of the joint servos and guided the manipulator to the approximate solution. This was completed in frame 18. At this point the FJC processors were reactivated and managed to servo the manipulator exactly to the target. A target that was outside the workspace of the manipulator was set in frame 20. The manipulator tried to reach it, but stopped with the elbow fully straightened (frames 22-30). Notice that there were no oscillations. 2 . 1 0 . 2 Based Moved Only to Resolve Joint Limits In the second simulation, the rules system was allowed to move the base if it consid-ered that would resolve a joint limit difficulty. The same sequence of targets as in the first simulation set was used. Extra targets were added at the end to illustrate other aspects of the rules system. The simulation is shown in Figures 2.15, 2.16, and 2.17. 50 1 [ V 2 [ V 3 > 4 > V 6 <^ 1 e 7 8 » 10 n 12 \{ 13 14 \ | 15 1 Figure 2.15: Base Moveable by Rules System, Part 1. 51 16 17 18 A 18 20 21 22 23 J < 24 , - I 25 MA 26 < 27 ' 4 . 28 I 4 . < 30 \ , 4 . Figure 2.16: Base Moveable by Rules System, Part 2. 52 31 \ A y < 32 J 33 <±> ^> 34 35 b-.. 4 . 36 IN 4 _ <±> v 87 A 7 88 4 . I c 39 f x . A 7 \ <• / A > 41 t _ 4 . Gj A c. 43 > 7 44 1 4 . a 45 > 4 . 7 Figure 2.17: Base Moveable by Rules System, Part 3. 53 The motion of the manipulator in frames 1-23 was the same as in the first simulation. The base was moved in frame 24 after the manipulator had been stuck for 2 frames. While the base was moving, the joints of the manipulator were not active. The base was moved to a position in which the manipulator had good manipulability. The FJC processors were reactivated in frame 26. However, the target was too low, and the rules systems took control again and commanded the manipulator to an elbow up position. The target was reached in frame 32. The new target specified in frame 33 caused the shoulder joint to reach its limit in frame 36. The base was moved, but the shoulder again reached its limit in frame 38. The rules systems computed the best approximate closed-form solution and the target was eventually reached in frame 45. 2.10.3 Mobile Base In the third and last simulation, the base was freely moved to maximize manipu-lability at all times. The motions are shown in Figures 2.18 and 2.19. The same target sequence was used. The base started moving in frame 8 (cf. previous simulation). The same se-quence of targets were tracked in 30 frames instead of the 45 frames in the previous simulation. A mobile base thus cut the execution time of the same sequence of targets by 50%. 54 L J 1 a L V 2 L V 3 L 4 V 5 •I 6 \ 7 •L Vf 8 -1 M 8 > 11 12 M 13 14 \ 15 X Figure 2.18: Mobile Base, Part 1. 55 18 \ X y 17 \ X y 18 \ X y • < 18 X I-20 X 21 X 'A 22 Ki 23 - I y \ 24 - I A 25 X A 26 - A 27 X Kt> A 28 X <> A 28 - I A 30 Figure 2.19: Mobile Base, Part 2. 56 Chapter 3 Path Planning The workspace of a robot may contain obstacles such as equipment, payloads, other robots, and sometimes humans. The robot must not hit these objects when it is executing a task, so robot paths must be planned carefully. In order to improve throughput, the robot should take the shortest collision-free path from the starting position to the target position. One of most important uses of robots is doing work in hazardous environments, such as in space, under water, or in nuclear plants. Unlike the factory environment, many of these surroundings axe uncontrollable. It cannot be known a priori when and where obstacles may get in the way of the robot. It is therefore important that path planning be done in real-time. This chapter starts with a general discussion of the requirements and difficulties of path planning. Some approaches of other researchers are then presented. A novel 57 path planning method is then proposed. The method can deal with arbitrarily shaped obstacles in 3-D. Real-time execution speed can be achieved with the use of parallel processors. The effectiveness of the method is demonstrated with computer simulations. 3.1 Difficulty in Path Planning for Robots Finding a collision-free path for a robot is an extremely difficult task. Consider the basic problem of moving a point object from point A to point B, in the presence of some obstacles (Fig. 3.1). In 2-D (Fig. 3.1a), one can approximate the obstacles with polygons without too much loss of generality. Since the shortest path will only go through the vertices of the polygons, it is feasible to search all possible combination of vertices to determine the shortest path. The search space is finite, albeit very large when there are many objects with complex shapes. The problem becomes much more difficult in 3-D (Fig. 3.1b). Even if the ob-stacles are approximated by polyhedrons, the shortest path does not necessarily go through only the vertices of the polyhedrons. It will consist of straight line seg-ments whose ends fall on the edges of the polygonal surfaces of the objects, but not necessarily the vertices. The search space will be infinite since there is an infinite number of points on the polygonal edges. Therefore it is not practical to find paths in 3-D by exhaustive searching. Some heuristics must be employed, and one must be satisfied with sub-optimal paths. 58 While planning a path for a point object is difficult enough, planning for a multi-link robot presents even more difficulties. Because robots may be used in hazardous environments which contain unpredictable or moving obstacles, path planning must be done in real-time for such robot applications. In a factory environment, which is often carefully controlled, path planning may be done off-line. A robot is not a point object. Collisions between obstacles and any part of the robot must be avoided. Paths for a point object are difficult to compute even though they can be easily visualized. Even visualizing a path for a multi-link robot is very hard. Much difficulty should thus be expected in the computation of such paths. 3.2 Operational Space and Configuration Space A robotic task involves a set of actions. For example, some parts are put together to form a product. These actions must be described with respect to a certain coordinate frame. The space in which robotics tasks are described has been termed operational space by Khatib[Khatib86a]. Usually, operational space is Cartesian space, although sometimes other coordinate systems such as spherical or cylindrical may be more appropriate. An n-DOF manipulator works in its own n-dimensional joint space. Each dimension in this space represents a joint variable. Lozano-Perez[Lozano79] called it configuration space. While robots may come in a variety of shapes and sizes in operational space, they are invariably represented by only a point in configuration space. Operational space and configuration space are related 60 direct kinematics robot: obstacles: configuration operational space space Inverse kinematics direct kinematics planning In configuration space planning In operational space Inverse kinematics actual path Figure 3.2: Path Planning in Operational and Configuration Space by the manipulator direct and inverse kinematic transforms. Objects and actions can be described in either operational or configuration space. When planning a path for a robot, one can therefore work in either of these spaces. Fig. 3.2 illustrates the two different approaches. Each approach has its advantages and disadvantages. Paths planned in operational space usually consist of straight line segments in Cartesian space and therefore look natural. Since humans are more used to visualizing events in Cartesian space, it would also seem easier to do motion planning 61 'shadow Figure 3.3: Obstacle Shadow in Operational Space in this space. The difficult part is that some locations in operational space may not contain any obstacles and yet are inaccessible to the manipulator, because some parts of the manipulator other than its tip will collide with the obstacles if the robot tip is at these places. These locations have been referred to as shadows of the obstacles. Fig. 3.3 shows an obstacle and its shadow in the operational space of a 2-link manipulator. If the robot were a point object, then there would be no obstacle shadows. How-ever, the robot is indeed only a point in configuration space and planning for the point robot can be done in configuration space if all the obstacles are transformed to that space. Any path found in configuration space guarantees collision-free mo-tion for all parts of the robot, since there are no obstacle shadows. Paths consist of straight line segments in configuration space. They are thus efficient for the robot, since they represent the shortest distances that the joints have to move in order to reach the targets. However, the difficult part is transforming the obstacles from Cartesian space to configuration space. In general, the transformation is highly non-linear, except in the case of a Cartesian robot. Small simple objects in Carte-sian space can map to large and complicated objects in configuration space. Some 62 examples of such mappings can be found in Section 3.5. Because of the irregular object shapes, storing objects in configuration space also poses serious difficulties, since it is difficult to find a mathematical description of the objects. Transforming objects to configuration space does not reduce the dimensionality of the path planning problem (in fact, the number of dimensions will increase if the manipulator is redundant). Therefore, whether paths are planned in operational space or configuration space, the inherent difficulty of planning paths in dimensions more than 2 still remains. 3.3 Previous Research In order to simplify the path finding problem, many researchers chose to tackle simplified worlds. For example, Singh and Wagh[Singh87] dealt with rectangular obstacles. Gilbert and Johnson[Gilber85] considered convex polygons in 2-D and used calculus of variations with cost functions to optimize paths for a Cartesian ma-nipulator. Joint limits, manipulator dynamics, energy expenditure, etc., could all be incorporated in the planning process. Buchal[Buchal87] used a similar technique with internal penalty functions which allowed unfeasible paths to be used as start-ing paths. Manipulator inverse kinematics were also incorporated into the planning process. Kant and Zucker[Kant86], Montgomery et a/[Montgo87] relaxed the con-vexity constraint, but used graph techniques instead of calculus of variations. Luh and Campbell[Luh84] considered polygonal obstacles and a robot with a prismatic joint. Lumelsky[Lumels87a,87b] had obstacles in 2 and 2|-D. Brooks[Brooks83,84] 63 used generalized cones to generate freeways among prism-shaped obstacles. Col-lisions between parts of the robot other than the end effector and obstacles were handled by having other sets of freeways. Freund[Freund83,84,85,86] used non-linear control techniques to coordinate multi-robot systems. The manipulators considered had simple structures. Khatib[Khatib78,86a,86b,87] and Hogan[Hogan84a,84b,Newman87] both used artificial potential fields to repel the manipulator from obstacles, utilizing the trans-pose of the manipulator Jacobian to transform Cartesian space forces to joint space generalized torques. The potential at the target was set to zero, while the potential increased as obstacles were neared. The robot was thus attracted by the target but repelled from the obstacles. By following the potential gradient, the robot could reach the target while avoiding the obstacles. To tackle the problem of avoiding col-lisions between obstacles and parts of the robot other than its tip, Khatib[Khatib87] suggested considering strategically selected points on the manipulator and generate repulsive forces between these points and the obstacles too. Real-time performance was reported. Theoretically collisions with objects of any shape can be avoided by defining as many protection points on the manipulator as needed. However, the computation burden will be greatly increased. Also, in some situations local minimum regions in the artificial potential field exists and a detour is necessary. Concave obstacle surfaces are obvious examples. A less obvious example is depicted in Fig. 3.4. The potential gradient will tend to guide the manipulator straight towards the target, when in reality it must follow the detour path shown by the dashed line. Even if protection points are placed on the manipulator, the repulsive forces will not be able to guide the manipulator to the target in such a case. 64 Figure 3.4: A Necessary Detour in a Manipulator Path Espiau[Espiau86] adopted Khatib's approach to control redundant manipulators by assuming proximity sensors on the robot surface. Kir6anski[Kircan87] also used J' to control redundant manipulators, assuming the point on the manipulator which is closest to an obstacle was determined by a high-level artificial intelligent system. Working in configuration space, Lozano-Perez[Lozano79,86a] obtained the op-timal path in a 2-D world with polygonal objects using a graph search. However, extensions to 3-D posed serious problems. He proposed quantizing configuration space so that objects can be represented easily. The slicing of sub-spaces and the idea of kernels were used in multi-dimensional situations to obtain globally good but not necessarily optimal paths[Lozano86b,87]. The approach has the advantage that there are no restrictions on the shapes of the obstacle, all collisions can be avoided, and it can theoretically be extended to any number of dimensions. How-ever, computational complexity grows exponentially with the degrees of freedom of the manipulator, and heuristics were used to simplify path planning for a manipu-lator with six degrees of freedom[Lozano87]. Paths were reportedly generated in a 65 few seconds for a planar manipulator, and an example of a 3-D manipulator path with rectangular prism shaped obstacles was accomplished in 40 seconds. Faverjon[Faverj84] performed the searching in configuration space using an oc-tree. 3.4 The Proposed Approach Of the ideas developed by previous researchers, the artificial potential field approach and configuration space planning seem most promising. The potential field approach is interesting because it allows paths to be generated at very high speed, since only obstacles in the vicinity of the robot need to be considered. By setting the potential to infinity where the obstacles are located, collisions will never occur. Configuration space planning is attractive because it simplifies the problem to planning a path for a point object. There is no need to worry about different strategies to deal with different robot structures, since all robots are point objects in configuration space. If potentials are defined in configuration space, then the benefits of both approaches can be reaped. However, the problems that come with either approach must be solved. The potential field method has problems with local minimum regions, while mapping obstacles from operational space to configuration space is very time consuming. A new approach is proposed that uses a potential surface in a quantized configuration space. Obstacles are represented as plateaus with infinite potential. The plateau edges are vertical. Local minimum regions are filled on demand by growing extensions on the obstacles incrementally in equi-66 potential steps. Obstacles are only mapped to configuration space on demand, using a fast hierarchical collision detection scheme with objects approximated by triangular surfaces. 3.4.1 Potential Surface For an n-DOF manipulator, the configuration space is n-dimensional. Let the start-ing point and target be x4 and xt respectively, where x is an n-dimensional vector, (xi,X2> • • • j ^ n ) ' - An artificial potential surface can be generated around the target with the potential at any point in the n-dimensional space defined as the distance squared from the point to the target: P = { y~] (x, — xt)2 no obstacle .=i ' (3.1) oo obstacle In a quantized space, each point has N = 3n — 1 neighbors (e.g. Fig.3.5 shows the 2-D case). Suppose the current location of the manipulator is xc, where xc = xg initially. Let the potential at xc be Pc. In order to move the manipulator towards the target, the neighbor with the lowest potential is chosen as the next position of the manipulator. Let this location be x c + 1, with a potential Pc+i = min (PJ)J=i,2,...,7v), where Pj is the potential of the j-th neighbor. In order to avoid collisions with obstacles, the potential at any point which is part of an obstacle is set to infinity. In contrast to Khatib's approach[Khatib87], 67 Figure 3.5: Neighbors in 2-D Space the potential rises abruptly at obstacle boundaries. This is important for memory requirement considerations and dealing with local minima on the potential surface. Both considerations will be discussed in later sections. Because of the abrupt ob-stacle boundaries, the term 'potential surface? has been used instead of 'potential field' in order to avoid confusion. When planning a collision-free path in configuration space, both the starting position and the target are first transformed to configuration space (joint space) with an inverse kinematics transform. The configuration space obstacle map is initially empty. The current location of the manipulator, xc, is set to x„. The potentials of the neighbors of xc are computed by using Eqn. 3.1. Eqn. 3.1 requires testing whether a point is part of an obstacle or not. Each point in configuration space represents a certain configuration of the robot. The appearance of this configuration in Cartesian space is described by a set of trans-68 formation matrices for each link of the manipulator — the direct transform. If a collision occurs when the robot takes on this configuration, then the configuration point must be inside an obstacle. Collision detection is done in Cartesian space, where all the obstacle information is known. If any part of the robot intersects any part of an obstacle, a collision is reported. It should be noted that, because of the quantization, this mapping of objects from Cartesian space to configuration space is only approximate. The collision detection algorithm is discussed in Section 3.4.2. The information of whether the point is in free-space or is part of an obstacle is stored in the configuration space obstacle map (e.g. neighbors 4 and 5 in Fig.3.5 are marked as part of an obstacle). If the same point is encountered again, then its status can be simply read from the obstacle map without invoking the collision detector. By choosing the neighbor with the lowest potential as the new xc, the searching process can be applied iteratively until the target xt is reached, provided that there are no local minimum points on the potential surface. There are no local minimum points if for every location xe, a neighbor with a lower potential can be found. 3.4.2 Collision Detection The configuration space potential surface path planning method requires a fast yes-or-no collision detection algorithm. Usually a robot must avoid obstacles with a certain clearance, both as a safety precaution and also to avoid actual contact with the obstacles. The common way to provide such clearances is to grow the 69 obstacles by the amount of the desired clearance. Collision detection is then per-formed with the grown obstacles [Faverj84,Lozano86a], For the rest of this chapter, obstacles are assumed to have been grown to allow some clearance between the manipulator and the obstacles. A method for growing obstacles has been suggested by Faverjon[Faverj84]. Various schemes for collision detection have been used before: Brooks [Brooks84] computed Cartesian space freeways by geometric analysis of the robot and the obstacles. Objects were represented as polyhedra. Lozano-P6rez[Lozano86b,87] used a similar approach to compute the collision-free ranges of each joint vari-able. Buchal[Buchal87] also modelled objects as polyhedra and proposed a method to compute the penetration distance between two polyhedra. Lumel-sky[Lumels87a,87b] assumed a skin of proximity sensors on the surface of the mar nipulator, so that impending collisions were detected dynamically by the sensors. In this thesis, a hierarchical collision detection algorithm using triangular sur-faces to approximate objects is used. Even a large number of very complex shaped objects can be easily handled with this scheme. Object Representation Objects are represented as sets of triangular surfaces. There are alternate ways to represent objects, such as spheres or a collection of basic shapes which may include cones, prisms, spheres, and polygons. The advantages of using triangular surfaces are: 70 Arbitrarily shaped objects can be represented with any degree of precision; Calculating the intersection line segment between two triangles is easy; Since all objects are represented the same way, the same intersection algorithm can be applied for all objects; It is possible to generate a triangulated approximation to an object with com-puter vision[Boisso86,Keppel75]; Objects can be represented roughly with a few triangles for fast computation, or accurately with many smaller triangles if precision handling is required. There are some shapes that are difficult to approximate with triangles, e.g. spheres, but this should not be a serious problem. Hierarchical Collision Detection Although computing whether two triangles intersect or not is straightforward, it is nevertheless computationally expensive, since many floating-point operations are required. The amount of computation required can be greatly reduced by taking advantage of the fact that the triangles are not unrelated but represent real objects. A hierarchical collision detection algorithm can therefore be used. A block diagram of the scheme is depicted in Fig. 3.6. An object in general contains a large number of triangular surfaces. A bounding box can be computed for the entire object. If the object is far from the manipulator, then the whole object can be rejected with a single bounding box check. Often it is 71 1. 2. 3. 4. 5. .1.--i .-• Level 1 i -•4-' 1 I I '"K 1(! 41^ Level 2 • v. 1 Level 3 Level 4 Figure 3.6: Hierarchical Collision Detection Algorithm 72 beneficial to break up a large object into components so that whole sections of the object can be rejected with bounding box checks. This is the highest level (level 1) of collision detection. Level 1 may involve a few internal levels if the objects have been broken up into sub-parts. If the bounding box of the object intersects that of the manipulator, then collisions must be checked at the next level: level 2. At level 2, a bounding box is computed for each triangle of the robot. The bounding box is then checked against the bounding box of the whole obstacle for possible intersections. If the bounding boxes intersect, then checking goes down to level 3. At level 3, the bounding box of the triangle from the obstacle is checked against the bounding box of each triangle of the manipulator. If the boxes still intersect, then one must go down to level 4, the lowest level, where the actual intersection line between the two triangles is computed. Deciding whether two triangular surfaces in 3-D intersect or not is a straightfor-ward problem. However, the average computation time can be reduced by applying a sequence of progressively more complicated tests. The test sequence is terminated as soon as a decision can be made. The formulation of such tests does not require much innovation. Nevertheless, an algorithm is presented in Appendix B for the sake of completeness. 73 3 . 4 . 3 Local Minimum Regions A local imnimum exists when all of the neighbors of the current point have an equal or higher potential than the current point. If the configuration space is continuous, local minima are difficult to deal with. But since the space is quantized, one can get around a local minimum point by setting the potential at that point to infinity. Such a point will be referred to as part of the extension of the obstacle. Searching can then resume. If the local minimum region consists of more than one point, then the obstacle will be extended until the local minimum region has been filled. The effect of growing obstacle extensions in the 2-D case is illustrated in Fig. 3.7. In (a), a local minimum is encountered, an extension point is formed at the current location, and the current point falls 'outward' from the local minimum. In (b), growing of the extensions continues in equi-potential regions, which are concentric arcs with the target as the center in the 2-D case. In (c), the extension has been grown large enough to allow the current point to move outside the local ininimum region. The target can be reached without further problems from this location. Once an extension has been grown, it is stored in the obstacle map so that if a path search is performed again for the same target, the point robot will not go inside the local minimum region again. The resulting path will be different and may be shorter and more efficient. A path can therefore be refined to various degrees depending on the amount of time available, by simply applying the path planner repeatedly with the same starting point and target. However, since the size and shape of extensions depend on the location of the target, all extensions must be erased once the target has moved. 74 Start extension (c) Figure 3.7: Growing Obstacle Extensions 75 Cluttered Environments Sometimes the starting point of a path is inside a local minimum region. If there are not too many obstacles in the workspace, chances are that as the obstacles are extended, the robot will be forced out of the local minimum region area anyway, in much the same way that a rubber ball is floated out from a hole when water is poured into the hole. In very cluttered environments, it is possible for the obstacle extensions to block some channels in free-space and thus block the robot. This is analogous to the problem of having air pockets in casting objects from molds. The extension technique will fail in such cases. Some more powerful global searching algorithm must be employed. Since the configuration space obstacle map is built incrementally, the obstacle map can be completed in these circumstances, and more powerful global search algorithms can then be applied, such as the one proposed by Lozano-P£rez[Lozano87|. Thus, the work done in the local search attempts will not be totally wasted if a global search is required. The method therefore provides a bridge between reflex type action and calculated motions for the manipulator. It should be noted that if the robot does not start from a local minimum region, then the robot will always reach the target. This is because since the starting point is not in a local minimum region, then there exists a path with monotonically decreasing potential from the starting point to the target. Since obstacle extensions are only added at points whose neighbors all have a higher potential, the path cannot be blocked by any obstacle extensions. 76 3.4.4 Memory Requirement Considerations Since the number of points in the quantized configuration space grows exponentially with the number of joints, one must be careful to find a compact way to store the potential surface. Lozano-Perez[Lozano86b,87] represented configuration space obstacles with slices for all dimensions except the lowest, which was represented with lines. The lines represent the forbidden range of joint values. The advantages of using lines rather than further quantizing the last joint to form points in n-D are that less memory is required to store lines than a collection of points, and there are efficient algorithms for dealing with straight lines. However, lines are not necessarily easier to store than points. The two ends of each line must be stored. Each end requires at least an integer-sized memory location. Sometimes there is more than one range of forbidden joint values, so more ends will need to be stored. Since theoretically there is no limit to the number of forbidden ranges, dynamic memory allocation will probably have to be used to store the configuration space obstacles, or else an excessive amount of memory will be wasted. If the configuration space is fully quantized to n-D points, then each point requires only one bit to represent — either there is an obstacle or there is not. The memory required to represent the entire configuration space is thus fixed in size, and so can be conveniently stored in a multi-dimensional array. Addressing elements in an array is quicker than searching a dynamically allocated storage area. Although there are some situations for which a linear representation is more efficient, it was decided that n-D points will be used in this thesis because of its simplicity. By merging the artificial potential field approach to configuration space path 77 finding, one might be tempted to represent the potential of each quantized point in configuration space as a floating-point number or a properly scaled integer. This will result in the requirement of an enormous memory. Since the potential of a point in an obstacle-free world can be easily calculated, it is better to store only obstacle information and compute the potential due to the target on demand. Where there is an obstacle, the potential is set to infinity. The potential due to obstacles and the potential due to the target can then be merged to give a floating-point potential. This potential is not stored, but used only in the local potential searches. The representation of extensions and the fact that a point in configuration space is tested for the existence of obstacles only on demand complicates the storage problem slightly. For every quantized configuration space point, there are four possibilities: 1. The potential has not been computed before; 2. The point is in free space; 3. The point is inside an obstacle; 4. The point is inside the extension of an obstacle. Therefore, two bits are required to represent each point in the quantized configura-tion space. For a 3-DOF manipulator with 100 divisions per joint, 1003 x | » 250kBytes will be needed. This is a reasonable memory size. 78 3.4.5 Strengths and Weaknesses of Approach The proposed path planning method has both advantages and disadvantages. Its advantages are: • Real-time execution speed; • No restriction on object shapes; • Retains speed with many complex shaped objects; • Works in 3-D; • Builds configuration space obstacle map incrementally. When the situation is very complex and the local search technique has failed, the map can be completed and then more powerful graph searching techniques can be used. The work is thus not all wasted. But it has the following disadvantages: • The paths may not be optimal, although they are usually reasonably good; • In difficult situations, a path may not be found, even if one actually exists; • Computational complexity grows exponentially as the number of links of the manipulator. For a (Mink manipulator, each point in configuration space has 36 - 1 = 728 neighbors! 79 Despite the disadvantages of the method, it nevertheless provides improvements on existing techniques. Lozano-Perez's approach has the same disadvantages, al-though his method is less likely to fail in cluttered environments. Perhaps the best way to program an actual path planner is to use a number of techniques together so that if one fails, another one might succeed. For example, while the proposed configuration space potential surface approach is used for high-speed reflex action in unpredictable environments, another control module can be applying Lozano-P^ rez kernel approach simultaneously to the same situation, and yet another module may be using artificial intelligence to tackle the problem. There is still much room for further work in this area. 3.5 Evaluation of Method Many simulations have been performed to evaluate the proposed path planning method. Since it is difficult to visualize a 3-D potential surface, the basic ideas are illustrated with a planar 2-link manipulator. The idea is easily extended to multi-dimensional cases, and a PUMA manipulator has been simulated to illustrate the functionality of the technique in 3-D. For demonstration purposes, each joint position is quantized to 50 divisions. Depending on the joint limits, the actual joint resolution will be different. For example, if a joint has a range of 6 radians, then each step increment is 0.12 radians, or about 7°. In reality, a much finer resolution will be required. However, the path planner only needs to generate an approximate path. Exact positioning is the job 80 for the inverse kinematics module. Therefore, a resolution of about 3° is probably adequate for most applications[Lozano87]. 3.5.1 Two Link Manipulator The kinematic information about the 2-link manipulator used in the following simu-lations can be found in Appendix A. Many (over 20) different obstacle arrangements have been tested with the path planning module. One of the simulations is shown in detail in Figures 3.8 to 3.10. Each frame contains two views: operation space on the left and configuration space on the right. The shots are not taken at regular intervals, but rather at interesting situations where the manipulator gets close to a collision. The target is shown with an asterisk. The starting point is represented by a circle. The dashed line is the path found by the path finder. In the configuration space views, the current location of the robot is also shown with a circle. Obstacle points are represented as a solid square, and extension as hollow squares. The axes in the configuration space views represent the two joint angles in radians. Notice the joint limits imposed. A succession of targets were given to the manipulator. The first path (frames 1, 2) was a simple one. It took less than 1 second to generate. All timing figures given were elapsed time on the HP9050 computer, unless otherwise stated. Only a small portion of the configuration space obstacle map needed be computed to generate this path. Tracking the computed path took longer, about 5 seconds. Tracking speed is 81 82 83 0 3 1.5 0 0 / ft • • -2 0 Q1 7 2 A, e CO o 0 • -2 0 01 e 2 CO o o / l i • -2 0 Q1 s 2 Figure 3.10: Path Planning for Two Link Manipulator, Part 3 84 only an indication of the graphics speed of the computing hardware, and should not be included in the timing performance of the path finder. Tracking speed will not be cited for the rest of the paths. In computing a path segment, many of the obstacles are outside the desired path. They do not significantly affect the execution speed of the hierarchical collision de-tector since whole objects can be rejected by one simple bounding box check. Other simulations with different number of obstacles have demonstrated this feature. Since the configuration space is quantized, the path finder cannot move the manipulator accurately to the target. It can be seen from the figures that the ends of the paths generated by the path finder did not fall exactly on the targets. The targets were reached by using FJC at the end of each path. Cooperation between the path finder and the inverse kinematics module in the simulations will be discussed in Chapter 4. Another target was then set in frame 3. This path illustrates what happens when there is a local minimum region in the configuration space. The local minimum region was filled with obstacle extension points. The outermost extension points lay roughly on an arc with the target as the center. Since the configuration space map was computed incrementally, the path planner initially was unaware of the local minimum region and went inside it. When it discovered that the robot was inside a local minimum region, it grew the obstacle until the robot could clear it. The target was reached in frame 4. Notice that the path looks rather clumsy in operational space, but is quite natural in configuration space. This is one of the drawbacks of configuration space path planning: the paths look unnatural. About 85 2 seconds was required for this path. Since the local minimum region is filled by growing the obstacle, a more efficient path can be generated by calling the path planner again. This is illustrated in frames 5 and 6. In actual application, the path finder will be repeatedly called until either the path can no longer be improved or when time has come for the robot to start moving. It is difficult to estimate the total time required to generate the improved path in frame 5, since the CPU is time-shared with the graphics package while the path finder is improving the path. By deducting the approximate tracking time, total planning time is estimated to be about 6 seconds. A third target was set in frame 7. This target is tricky because a collision-free straight line can be drawn in Cartesian space from the starting point to the target, yet such a path is unfeasible because on the way to the target, the elbow will hit the obstacle on the lower left. A feasible path was easily found in configuration space, though, with two small local minimum regions. Without the obstacle extension scheme, even these small local minimum regions will stall the robot. This path took about 2 Beconds. The complete obstacle map at the situation in frame 5 together with the po-tential surface is shown in Fig. 3.11. Shadow points have a potential slightly lower than actual obstacle points, for the purpose of illustration. The path finder treats their potentials identically. One can visualize the robot as a marble rolling on the quantized potential surface. The marble may be trapped in a local minimum region. When the marble cannot move, the potential of the grid point under the marble is raised to infinity. The complete obstacle map required about 2 minutes to compute. 86 O o Figure 3.11: Potential Surface of 2-D Complex Obstacles 87 3.5.2 P U M A Manipulator The proposed path planning technique has also been tested in 3-D using simulations of a PUMA manipulator. The path planner only considers the first three joints of the PUMA. Orientation is not considered. The wrist joints are fixed when the manipulator is tracking a path. This simplification is sufficient in many tasks where the payload is small, so that motion of the wrist joints do not alter significantly the ability of the manipulator to move among obstacles. Since it is difficult to show a 3-dimensional configuration space, only the paths in Cartesian space are shown in the simulation pictures. Over 10 different workspace environments have been tried. The one simulation shown here contains fairly complicated obstacles: a table, two chairs, a cupboard on top, and a small object on the table (Figures 3.12 to 3.14). As in the simulation for the 2-link manipulator, the pictures are not taken at fixed intervals, but at interesting moments during the simulation. Two views are presented for each frame so that the 3-D picture can be more easily visualized . Because of the large number of lines, the path is shown as a bold line rather than a dashed line, so that it can be seen more easily. The target is shown in the shape of the robot end effector, in bold. There are a total of 3 targets and 3 corresponding paths. Tracking of the first path is shown in frames 1 to 3. The path was relatively simple and took about 5 seconds. The only potential collision was between the tip of the manipulator and the corner of the table. 88 Figure 3.12: Path Planning for PUMA, Part 1 89 5 e Figure 3.13: Path Planning for PUMA, Part 2 9 0 1-] ix. h 7 - J K A x 8 Figure 3.14: Path Planning for PUMA, Part 3 91 A second target was set in frame 4. Tracking of the path is shown in frames 4 to 6. This path is rather tricky since a straight line can be drawn in Cartesian space from the starting point to the target. This line is under the table top and hence is not feasible: the forearm of the robot will hit the table top. Nevertheless, the path planner managed to find a feasible and reasonably efficient path. The different views of the scene show that the path is indeed collision-free. This path took about 45 seconds to generate. The third path is shown in frames 7 and 8. This time the target is higher — above the cupboard. This path is also quite simple since the only obstacles that are in the way are the chair on the left of the frames and the cupboard. Because collision detection is done hierarchically, the other objects can be rejected very quickly. The path was found in about 10 seconds. With the HP9050 computer, which is a uniprocessor computer, path planning speed for the 3-D case can hardly be considered real-time. However, if a multipro-cessor computer is available, execution speed can be greatly improved. This will be shown in the next chapter. Also, although in the simulation examples presented, the robot only starts moving after the entire path has been planned, it is possible to move the robot as the path is being planned. Except in situations where local minimum regions in the potential arise, the path taken by the robot will be the same as if the entire path is precomputed before the robot moves. This is similar to Khatib's [Khatib86b] approach. When local minimum regions exist, the robot will have to try different approaches physically. 9 2 Chapter 4 Hierarchical Architecture for Robotics After the detailed analysis of manipulator kinematics in Chapter 2 and path plan-ning in Chapter 3 , the computational requirements for these aspects of robot control can now be analyzed with confidence. In this chapter, an attempt is made to par-tition these robot control tasks for execution in a parallel hierarchical architecture with message passing. The overall system that has been simulated and studied will be presented, and possible extensions to incorporate other robot control issues in the system are outlined. 9 3 4.1 Parallel Architectures There are many methods of implementing multiprocessor systems. A good survey of highly parallel computing structures has been presented by Haynes et a/[Haynes82]. They divided these machines into multiple special-purpose functional units, asso-ciative processors, array processors, data flow processors, functional programming language processors, and multiple CPUs. All except multiple CPUs have very sim-ple individual processing elements. The elements only need to perform basic arith-metic operations on the data supplied to them. These structures are not suitable for robot control, where interaction with a wide range of sensors is necessary and computational requirements range from simple arithmetic to complex problem solv-ing. Multiple CPUs, or multiprocessors, are thus most suitable for robot control. Indeed most of the architectures previously proposed fall into this category. Processors in a multiple CPU machine can be connected in a wide variety of ways. For example, the cross-point switch (e.g. C.mmp[Wulf74] at Carnegie-Mellon University), hierarchical clusters (e.g. Cm*[Jones80], also at CMU), per-fect shuffle (e.g. the NYU Ultracomputer[Edler85,Gottli82]), Banyan network (e.g. TRAC[Goke73] at the University of Texas), n-Cube (e.g. cosmic cube[Seitz85]) have all been tried, as well as the more commonly used parallel bus architectures (e.g. the FLEX/32 [Matela85], the CCAHPP [Naedel85], the Nonstop TXP [Horst85], and the Synapse N+l [Nestle85]). In robotics, the shared bus structure was used by Albus[Albus81a] and Or-lando[Orland84]. Brooks[Brooks86] used point-to-point connections for message 94 passing between processors, somewhat similar to the n-Cube. Using point-to-point message passing has the following advantages: • Processors can be added without causing bus contention problems and affect-ing performance of other processors; • Different clock rates can be used for different processors, as long as there is a common communication rate. Therefore each processor can run at maximum speed; • The processors can be distributed over long distances, since different commu-nication media can be used for different connections; • Overall system bandwidth increases as more processors and links are added; But there are also some disadvantages: • Because of the large number of links, serial communication will likely have to be used, thus narrowing the bandwidth of individual channels; • The irregular connections make hardware maintenance more difficult. Overall, the advantages seem to outweight the disadvantages in the context of robot control. A point-to-point message passing architecture was therefore chosen for investigation in this thesis. Since the n-Cube architecture uses point-to-point message passing, and is com-mercially available[Wiley87], it would seem convenient to build the robot control 95 modules on an n-Cube system. However, n-Cube architectures are not perfectly suitable for robot control. Because robot control modules have different functions, different processors should be used for different modules. For instance, a very-high-speed special purpose digital controller should be used for joint servoing, an arithmetic processor should be used for kinematics, and a symbolic processor used for task analysis. Commercial n-Cube processors do not provide such flexibility. Secondly, the number of connections at each node of an n-Cube system is fixed. If an n-Cube is used for modularized robot control, some nodes will have wasted connections while other nodes will have insufficient connections and thus require message re-routing. As a general computing structure, the n-Cube can be reconfig-ured in software to emulate different hardware structures, such as trees or arrays. In robotics, the general structure of the control modules is fixed, although the pro-grams within each module may change as the application task changes. Therefore, a custom architecture with communication links only where necessary is more suit-able. Given the advent of VLSI design methodology, it is feasible to implement special purpose architectures in order to improve performance[Haynes82]. 4.2 Structure of Proposed Computer Architec-ture In Chapter 1, it was seen that the robot control modules have a basic hierarchical structure, starting from the user interface at the top, and ending at joint servos at the bottom. Using point-to-point message passing, each module can be imple-96 mented with a processor or a multiprocessor sub-system. The arrows in Fig. 1.1 then represent communication channels. At each level the task is broken down into a number of simpler sub-tasks that lower levels can execute. The hierarchical structure is bypassed in some places in order to improve system performance. For example, object information from the vision system is broadcast directly to most of the modules. An alternate way is to connect the vision system to only one of the modules and then distribute the information via message relays, but this would incur unnecessary delays which can be crucial in reflex type actions. The archi-tecture is therefore not strictly hierarchical. Because of the number of individual communication channels, serial asynchronous lines have an advantage in keeping down the number of physical connectors. The proposed system has been named PHASAR, which stands for Parallel Hierarchical Asynchronous System Applied to Robotics. PHASAR is similar, though not identical, to Brooks's[Brooks86,87] loosely-coupled distributed system. Brooks's subsumption architecture allowed a higher level to modify inputs and outputs of lower levels directly without the lower levels being informed of its actions. While this structure permitted interesting possi-bilities, the author has chosen the simpler approach of sending a message to the lower level controller, requesting it to modify its own inputs or outputs. The low level controllers always controls their outputs directly, which might be important in situations where reflex actions must override planned movements. Each level in Brooks's system generated autonomous actions which resembled low-level in-telligence. Under modifications from higher levels, these autonomous actions can accomplish complicated tasks. In contrast, PHASAR control modules do not gen-97 erate any action in the absence of messages from other modules. Finally, Brooks applied his system primarily to mobile robot control, whereas PHASAR has been applied to manipulator inverse kinematics and collision-free path planning. The kinematics and path planning modules have been studied in detail in the previous chapters. The algorithms that have been developed are suitable to the proposed PHASAR architecture. The performance of these algorithms running on PHASAR is evaluated with extensive computer simulations running on an HP9050 computer system with interactive computer graphics. 4.3 Simulation Setup The PHASAR simulation package runs on an HP9050 computer running HP-UX, a version of UNIX. The program consists of about 7,000 lines of C code. Although there are two processors in the HP9050, they run in parallel only on a process ba-sis. Since the simulation package runs as a single UNIX process, it is necessary to emulate the behavior of a multiprocessor system. The emulation scheme is dis-cussed in Appendix C. As far as the user is concerned, the package behaves as a multiprocessor system. A real multiprocessor system will run much faster, though. The user communicates with the program via a keyboard and a set of ana-log knobs. Digital commands are entered from the keyboard. Commands can be classified into a number of categories: 98 Basic manipulator setup E.g.: enabling/disabling motion of some manipulator joints, enabling/disabling a math co-processor; Performance parameters E.g.: changing a, 3 used in FJC, or the sampling pe-riod of processors; Viewing options E.g.: setting exact (therefore repeatable) viewing positions, or selecting view in Cartesian space or configuration space; Task execution E.g.: fetching previously stored targets, or choice of targets in Cartesian space or joint space; Data logging E.g.: recording targets on files for later re-execution, or making hard copies of the current view; Because of the large number of commands, a hierarchical menu is used to simplify usage. Since the user interface runs on a separate virtual processor, execution of the robot is not interrupted as commands are entered. Partitioning of tasks among virtual processors is the topic of the next section. Feedback to the user is provided via an HP98710 high-speed color graphics ter-minal with a hardware graphics accelerator. Fig.4.1 shows a typical graphics display of the PHASAR simulation package. The user can see the robot and the obstacles in either Cartesian space or configuration space interactively. Except in the presence of highly complex obstacle collections, graphics response is close to real-time (about a quarter as fast as 'natural-looking' speed, more quantitative measurements have been given in Chapters 2 and 3). Other information, such as joint angles, joint adjustments, Cartesian errors, virtual processor status, etc., are displayed on a 99 01 8.6964 02 -0.1728 03 J.9826 04 1323 05 8.I38B OS B.4IBE JCB Running TZ2 | W*r | | J O | | &pt Figure 4.1: Example of Graphics Screen 100 dashboard on the bottom of the screen. One of two available dashboards can be chosen at any one time. Such information is important when studying the effects of adjusting certain gain parameters, observing the communication among the virtual processors, studying the effect of joint limits, etc. More detailed information about the simulation package can be found in [Poon87|. 4.4 Task Partit ioning Manipulator inverse kinematics using FJC and path planning using the configu-ration space potential surface has been simulated with the PHASAR simulation package. Task partitioning among multiple virtual processors in the PHASAR ar-chitectural framework is shown in Fig. 4.2. Blocks that have been simulated as virtual processors are marked with a clock on the upper right corner. The sampling period of each virtual processor can be altered interactively from the user interface. There are a total of 10 virtual processors: 1. User Interface 2. Path Planner 3. Joint Coordinator 4. Heuristic Rules System 5. Joint 1 Controller 6. Joint 2 Controller 101 path finder collision detector graphics output © Joint coordinator F J C 0 controler Joint j 0 servos robot model Joint limits mobile base hierarchical control processors Figure 4.2: Task Partitioning in Simulated PHASAR System 102 7. Joint 3 Controller 8. Joint 4 Controller 9. Joint 5 Controller 10. Joint 6 Controller The virtual processors can be grouped into three divisions: 1. User interface; 2. Path planner; 3. The kinematics system which consists of the joint coordinator, the heuristics rules system, and the individual joint controllers. The user interface has been described in the previous section. The path planner and kinematics systems will be discussed in the following sub-sections. 4.4.1 Path Planner The user interface continually monitors the requests of the user. When a new target is entered, it is sent to the path planner. The path planner transforms the target and the current position of the manipulator to configuration space, and attempts to find a collision-free path from the current position to the target. 103 For simplicity, the path planner has been simulated as one processor, although it can be broken into many processors. One possible arrangement has been illustrated in Fig. 4.3. The path planner node has a large memory for storing configuration space obstacles. As each discrete point in configuration space is tested for the presence of obstacles, the configuration space obstacle map is updated. Collision detection is done by a cluster of processors. Each collision detector node has a record of a subset of obstacle surfaces and a subset of manipulator surfaces. If the manipulator or the objects has moved, the appropriate transformations are sent to the collision detector nodes, so that the stored surfaces can be transformed cor-respondingly. Since objects typically have many surfaces (from tens to hundreds, depending on the requirement), significant improvement in execution speed can be achieved by distributing the surfaces to a large number of collision detectors. A collision occurs if any collision detector reports a collision. In the current imple-mentation, no collision detectors are included. Collision detection is performed by the path finder processor. When the manipulator is tracking a path generated by the path finder, the path planner system sends the joint angles to the joint coordinator, which passes them directly to the joint servos, bypassing the FJC joint controllers. The path planner can only guide the manipulator close to, but not exactly at, the target, since the configuration space is quantized. After the manipulator has reached the end of the path generated by the path planner, the path planner informs the joint coordinator that it has done its best. The joint coordinator then sends the desired Cartesian target to the FJC joint controllers to servo the manipulator exactly to the target. 104 4.4.2 Kinematics The inverse kinematics of each joint is handled by a separate processor, which will be referred to as the joint controller. The desired position and orientation in workspace coordinates is passed from the path planner to the joint coordinator, whose job is to distribute the inverse kinematics task to the joint controllers, and to coordinate the outputs of the joint controllers so that the manipulator will behave in a coordi-nated fashion. The joint coordinator sends the appropriate vectors (in workspace coordinates) to be controlled to each joint controller, and reads the resulting joint variables from them. These new joint variable values are processed collectively by the joint coordinator. Details of processing are discussed later. The processed joint values are then passed to the joint servos, which are simple digital proportional-derivative controllers in real application. However, since manipulator dynamics are not simulated in the simulation package used, the joint servos are modelled simply as QK+I — <IK + AQ [min(|At7|,A<7mai)]. (4.1) | A ? I where = current joint value Qk+i = new joint value, A? = desired joint change from joint coordinator, AQ, 'max = fixed maximum allowable joint change. Each joint controller has the following structure: 105 Check incoming mail; Compute error of function that the joint controls; Determine the change in joint value that will eliminate that error; Mail the joint change to the joint coordinator; For example, the joint controller for joint 2 of the PUMA manipulator, whose function is to alter the orientation of the position vector, is implemented as follows: Check incoming mail for: current position. P. desired position. P d, transformation of joint 2. Ti; Compute Pc = | | l \ i-P| |: Apply Eqn. 2.14 and Eqn. 2.15 to obtain A02: Mail A02 to the joint coordinator; 4 . 4 . 8 T h e J o i n t C o o r d i n a t o r With a simple 6-DOF manipulator working in a good manipulability region, the joint changes returned by the joint controllers can be sent directly to the joint servos. However, in many more complicated situations, the joint coordinator must perform some coordinating action in order to drive the manipulator properly. When the manipulator nears a singularity, oscillations may occur due to over-106 lapping of joint functions. Eqn. 2.20 and Eqn. 2.21 are used to adjust the each joint value. These equations could have been incorporated into the joint controllers were it not for even more complex situations that might arise (explained below), in which case the values returned by the joint controllers are completely overridden by the joint coordinator. Since Eqn. 2.21 requires that the previous joint value be known, the equation is most conveniently applied in the joint coordinator. More difficult situations arise when joint limits are reached. Such problems are resolved with the use of the heuristic rules module. While the rules module is enabled, outputs from the FJC joint controllers are suppressed. When the rules module has finished guiding the manipulator to the region of the new solution set, the FJC joint controller outputs will again be used. If the manipulator has a mobile base, it is also better to freeze the joints while the base is being moved. 4.5 Multiprocessor Speed-Up In applying a multiprocessor architecture to solve a finite size problem, the perfor-mance gain by adding more processors will decrease as the total number of pro-cessors increases. The partitioning of kinematics and path planning in the present PHASAR simulation package has been discussed in the previous section. In this section, the performance gain of parallelizing these control tasks is analyzed. 107 4.5.1 Kinematics With the use of FJC, the maximum degree of parallelism that is achievable is one processor for each joint of the manipulator. This is done in the present PHASAR simulation package. The sampling period for the inverse kinematics modules was obtained by timing the code for the appropriate sections running on the HP9050. The Joint Coordinator has a sampling period of about 1.18 ms, while each of the FJC joint controllers has a sampling period of about 0.65 ms. (These figures are for the HP9050 only, the sampling periods will probably be much shorter for state-of-art microprocessors.) Thus, if a separate processor is used for each of the joint controllers and the joint coordinator, then the joint controllers will spend half their idling. Therefore, it may be best to have one processor for every two FJC joint controllers. Each iteration of the inverse kinematics module thus takes about 1.2 ms. As a comparison, Lilly [Lilly 85] proposed a multiprocessor system for real-time kinematics and dynamics in inverse plant plus Jacobian control[Orin84a] using Intel 8086 and 8087 processors running on a Multibus I architecture. A total of 11 control processes were used. The calculation time for the Jacobian and inverse Jacobian processes was reported to be 33 ms. The modules (processes) employed by Lilly were macro modules, such as Jaco-bian, Inverse Dynamics, and Motion Planning. With such a partition, Lilly con-cluded that maximum performance was reached with only 3 processors. Adding more processors would not improve system performance because there was limited parallelism in her task decomposition scheme. By further decomposing the macro control modules into smaller micro modules, such as is done for inverse kinematics 108 and path planning proposed in this thesis, parallelism can be greatly increased. In the PHASAR simulation package, the Joint Coordinator and FJC joint con-trollers run concurrently. This means that the FJC joint controllers are always using input data from a previous sampling period. However, as the simulations in Chapter 2 show, the overall FJC algorithm still works fine. 4.5.2 Path Planner Most of the computation in planning a path with the proposed configuration space potential surface approach is in collision detection. The hierarchical collision detec-tion scheme with triangular surfaces is amenable to multiprocessor implementation. Although in the current PHASAR simulation package, the path planner is imple-mented with only a single processor, separate processors may be assigned to check collisions between the robot and each obstacle in the workspace. At the extreme, one processor may be assigned to each pair of triangles between the robot and the obstacles. Since collisions must be detected for all the neighbors of the current con-figuration space position, it is also possible to further parallelize the computation of groups of neighbors to separate processors. Therefore, at the limit, one can have linear speed-up with up to Np = NrxN0x (3n - 1) processors, where Nr is the number of surfaces for the robot, N0 is the number of surfaces for the obstacles, and n is the number of DOF of the robot. Np is in 109 general a very large number (100 — 10000). This collision detection scheme can thus exploit the power of as many processors as available in a practical system. In practice, a much smaller number of processors will probably be used. Also, because of the use of the hierarchical collision detection scheme, linear speed-up is only realized with NP processors in the worst case environment. This happens when all the obstacle surfaces are so close to all the robot surfaces that no surface can be rejected by bounding box checks. This is hardly likely in realistic situations. The most practical way to partition the collision detection task is to have several processors to handle all the possible neighbors. These processors calculate the direct transforms for the neighbors in configuration space and broadcast them to their own sub-processors. Each of these sub-processors will check for collisions between a certain subset of the robot surfaces with a subset of the obstacle surfaces. An example of such an arrangement is illustrated in Fig. 4.3. To take a numerical example, suppose the robot is modelled with 72 surfaces (12 surfaces per link, 6 links), i.e. JVP = 72. If there are 20 obstacles, each modelled with 20 surfaces, then N0 = 400. To plan a path for the first 3 links of the robot, 26 neighbors will have to be considered for each location of the robot. The maximum number of processors that can be used to advantage in this situation is thus 748,800. A more realistic way to partition the computation is to group the neighbors into 5 groups, each responsible for the 5 of the neighbors (except for the last group, which is responsible for 6). Each neighbor group is handled by a separate processor. If each neighbor group has 5 sub-processors, then the obstacles can be separated into 5 groups, each group having 5 obstacles. Each sub-processor is responsible for detecting collisions between the surfaces of the robot, suitably transformed, with 110 Path Planner Neighbor Group 1 Neighbor Group 2 Surface Group 1 Figure 4.3: Multiprocessor for Collision Detection 111 its share of obstacles. The total number of processors with this arrangement is 25. Often an obstacle can be rejected quickly with a bounding box check. By having each surface group processor handle more than one obstacle, the chances that the processor will go idle long before the others have completed their job is reduced. By timing the appropriate code section for checking the intersection of two triangular surfaces, it was found that if the bounding boxes of the triangles do not intersect, then the algorithm takes 0.8 ms. If the triangles intersect, 4.35 ms are required. Therefore, the hierarchical collision scheme will indeed save much computation time in many cases. 4.5.3 Interprocessor Messages PHASAR processors communicate by passing messages via asynchronous serial links. Because of the relatively low bandwidth of individual links, messages should be short. The input and output messages of each virtual processor in the simulated PHASAR system are tabulated in Tables 4.1 and 4.2. The length of each message in number of bytes is shown. The length includes the identifier for the message. Identifiers are coded and thus can be represented by a single byte. It can be seen that the anticipation of short interprocessor messages is indeed justified. 112 Input Out] put Processor Name Length Name Length (bytes) (bytes) User Interface Pd 13 a d 13 13 Bypass 1 Path Planner Pd 13 Pd 13 a d 13 arf 13 ND 13 nd 13 Reached 1 ?d, 5 Bypass 1 9d2 5 9d3 5 9d4 5 ?d6 5 ?d6 5 Joint Coordinator Agi 5 Pd 13 AQ2 5 a d 13 Ag3 5 n d 13 Ag4 5 P 13 Ag5 5 a 13 Ag6 5 n 13 5 Pe 5 9d2 5 ae 5 9d3 5 n e 5 9d4 5 Tl 25 ?d5 5 T3 25 <LDE 5 T4 25 BasePosition 9 T5 25 Pd 13 93 5 a<i 13 Reached 1 13 Table 4.1: Interprocessor Messages, Part 1 113 Input Output Processor Name Length Name Length (bytes) (bytes) Rules Pd 13 9* 5 *d 13 9d2 5 nd 13 <ld3 5 P 13 <Jd4 5 a 13 9D, 5 n 13 9d6 5 Joint Controller 1 Pe 5 5 P 9 Pd 9 Joint Controller 2 Pc 5 5 P 13 Pd 13 Tl 25 Joint Controller 3 Pc 5 A1J3 5 P 13 Pd 13 5 Joint Controller 4 ae 5 A<74 5 a 13 ad 13 T3 25 Joint Controller 5 ae 5 Aft 5 a 13 ad 13 T4 25 Joint Controller 6 n e 5 Ac76 5 n 13 ND 13 T5 25 Table 4.2: Interprocessor Messages, Part 2 4 . 5 . 4 PHASAR vs Bus Architectures The major advantage of a point-to-point message passing architecture such as PHASAR over conventional bus architectures is that with a point-to-point message passing architecture, there is no limit to the number of processors in the multi-processor computer system. In contrast, the Multibus II architecture from Intel limits the number of processors in a system to 20. In robotics, many processors are required. For instance, even implementing the small PHASAR system proposed in this thesis will require over 41 processors: • User interface: 1 • Path Planner: 1 • Collision Detector: 25 • Joint Coordinator: 1 • Heuristic Rules: 1 • FJC Controllers: 6 • Joint Servos: 6 While a bus architecture places severe limitations on the number of processors in a system, it does have advantages in communication bandwidth when the number of processors is small. For example, Multibus II runs at 10 MHz. Since the bus is 32 bits wide, the bandwidth is 40 MByte/s. Overhead time for switching bus masters is negligible. In contrast, assuming a serial communication bit rate of 20 115 MBit/s for PHASAR, and with 1 start bit and 1 stop bit for each byte, so that each byte requires 10 bits to be transmitted, then the bandwidth between each pair of processors is 2 MByte/s in each direction. The total bandwidth of 1 bidirectional link is then 4 MByte/s. Many pairs of processors can communicate at the same time in PHASAR. If each processor pair is connected by 1 bidirectional link, then the total number of links for a system with n processors is JVj = "fr"1). The total system bit rate is then Ni x 4 MByte/s, assuming that the links are 100% utilized. The theoretical break-even point between the two architectures thus occurs at 5 processors, when JVj = 10 and the total system bandwidth is 40 MByte/s. Ideally, above 5 processors, PHASAR may begin to outperform Multibus II in terms of overall communication speed. In reality, it is seldom that all the links in a point-to-point communication architecture will be used at the same time, so the break-even point will occur at more than 5 processors. To obtain an idea of the magnitude of time required for serial communication within PHASAR control modules, consider Joint Controller 5, which requires a total maximum of 61 bytes of I/O per sampling period. Sometimes a smaller number of bytes is required. For instance, it is not necessary to transmit the desired a vector to Joint Controller 5 every sampling period. At 20 MBit/s, and 1 start bit and 1 stop bit for every byte, 61 bytes will take 30.5 s^. This communication delay will have to be added to the sampling period of the processor if the communication is handled by the main processor. But communications in each PHASAR processor node is handled concurrently by a communications processor (see Section 4.6.1), communications can thus occur in parallel with normal computation. 116 As a comparison, the multiprocessor system proposed by Lilly[Lilly85] used the Multibus I with Intel 8086 and 8087 boards. Mailboxes were used for communication between pairs of processes. The communication time required for each of the 11 processes ranged from 0.6 ms for the Output Control process to 8 ms for the Joint Accelerations process. In contrast, even if all the inter-processor messages in the proposed basic PHASAR system (the messages listed in Tables 4.1 and 4.2) were to be transmitted sequentially with no parallelism, the total time required will be 0.18 ms (total number of output bytes for all modules is 368, communication speed at 2 MBytes/s). This is even shorter than the time required for the Output Control process alone in Lilly's Multibus I system. However, Multibus I is no longer a state-of-the-art system bus. With current hardware, communication speed will be much faster with Lilly's scheme. A true comparison between the serial point-to-point link and parallel bus approaches can therefore only be made when actual systems for both approaches using comparable hardware have been built. A point-to-point communication architecture is particularly suitable for robotics because communication in robotics tends to occur in bursts. For instance, when a new target is given to the robot, the path planner passes the link transforms to the collision detector processors. This is followed by a period of communication inactivity, when the collision detection processors are working. Ideally, if the load is distributed evenly among the collision detection processors, they will finish pro-cessing at about the same time. Another burst of communication traffic thus occurs when the collision detection processors report their results back to the path plan-ner. Bursty messages are unsuitable for bus architectures because there is heavy bus contention during a burst. 117 4.6 Physical Implementation It is possible to build PHASAR with off-the-shelf components, using general pur-pose microprocessors and asynchronous communication adaptors. Because of the advent of VLSI design methodology, it is now economically feasible to design appli-cation specific integrated circuits (ASIC) in order to improve system performance. An experienced IC designer can probably design a large but structured full-custom chip, such as a RlSC[Patter82] type microprocessor containing 10,000 to 100,000 transistors, in a few months. Semi-custom chips using standard cells or gate ar-rays can be designed even faster. With the advanced IC simulators now available, working designs on first silicon is common. It is therefore reasonable to consider the possibility of using ASICs to implement a high-performance and compact PHASAR system. 4.6.1 General Hardware Structure The tasks that need to be executed in each PHASAR control module are quite different. Different processor types can thus be used in each module to improve overall system performance. The following processor types should be useful: PD Controller At the lowest level of the control hierarchy are the actuators con-trollers. Here the use of digital proportional-derivative circuits is common. With a custom PD controller, sampling period can be greatly shortened to improve stability, and digital-to-analog converters for driving the actuators 118 can be integrate on-chip. An architectural design of a suitable PD controller can be found in Appendix E. Floating-Point Arithmetic Processor The kinematics and path planning mod-ules require high-speed moderate precision floating-point arithmetic, with oc-casional integer and logical operations. From the simulations, it has been found that 32-bit floating-point arithmetic with a bank of about 16 registers is most suitable. A 24-bit ALU can easily be integrated. Transcendental functions are not necessary with the use of piecewise linear approximations. Symbolic Processor For interpreting user commands and for task planning, pro-cessors for symbolic languages such as Lisp are most effective. All PHASAR modules need an asynchronous communications adaptor. If cus-tom processors are used for each module, then the adaptors can be integrated on the processor chip for compactness. This approach is used by the Transputer [Whitby85]. However, Transputer systems are programmed with the Occam lan-guage which requires that the processor at the receiving end be expecting and waiting for a message before communication can proceed. This is not suitable for PHASAR since its control modules should run continuously, checking for incoming messages only when it is convenient to do so. Typically each processors runs on an infinite loop, and messages are checked at the beginning of the loop. This incompat-ibility is not a major problem, since extra processes can be created in Occam that monitor communication channels continuously. A Transputer system would indeed be a convenient way to implement a prototype PHASAR system. If one is prepared to design ASICs, custom communication processors can be made to interface the 119 communication lines Figure 4.4: Structure of One Processor Node different control processors to the communication links. Fabricating the communi-cation processor as a separate front-end processor makes it more convenient to use different processors for each PHASAR module. The number of communication links can be increased by simply adding more communication processors to the control processor. By designing one's own communication circuit, one can also integrate the circuit with the main processor if it is desirable. Assuming the use of custom communication processors, each processor node in PHASAR will then consist of two processors: a main processor (MP) that does most of the computation, and a mail manager (MM) that handles communication with other processors. A block diagram of the arrangement of each processor node is shown in Fig. 4.4, which is self-explanatory. Parallel communication is used between MP and MM. The MP can be either an off-the-shelf general purpose microprocessor, in which case the control lines to the MM would be treated as I/O ports. If a custom designed processor is used for the MP, special control lines can be made in hardware. Because of the importance of the MM, its design has been considered in some detail and is presented in Appendix D. A CMOS test chip that contains a parallel-to-serial and serial-to-parallel converter has been fabricated in Northern Telecom's 120 data bus main push mail POP , processor empty manager (MP) full (MM) CM0S3 single level metal process. The full-custom chip contains over 900 transis-tors and was designed in 2 weeks from initial concept to complete physical layout. The prototype chips were tested and bit rates of 1.5 MBit/s were achieved. By suitable fine tuning and reducing the number of counts at which bits are sampled, it should be able to support bit rates of over 20 MBit/s. This is discussed in more detail in Appendix D. 4.7 Extensions to P H A S A R Although PHASAR has only been applied to manipulator kinematics and path planning, it should be suitable for other robot control issues such as dynamics compensation, trajectory planning, and vision. 4.7.1 Task Planner Robot programming will be greatly simplified if tasks can be specified as a single complex job, such as 'Clean the workspace' or 'Polish the object.' A job must be decomposed into appropriate motions which the path planner can understand. A multiprocessor sub-system can probably be used to speed up this highly complicated process. The task planning module should contain a natural language interface to com-municate with the user. Inputs to the module are natural language sentences or 121 phrases. Object names are sent to the vision system, from which corresponding coordinates are returned. Coordinates of targets are sent to the path planner. 4.7.2 Trajectory Planner The paths generated by the path planner consist of straight line segments joined together with abrupt turns. A manipulator usually cannot track such a path, nor is it desirable to have it try to do so. Also, if only the corner points are sent to the joint servos, manipulator speed will not be uniform: slowing down at each corner and jerking into motion again when a new corner point is to be reached. A trajectory planner module can smooth out the corners with consideration of manipulator dynamics and actuator limits[Paul85]. Speed can be made uniform using the methods proposed by Taylor[Taylor79] and Shin and Malin[Shin84]. 4.7.3 Dynamics Compensation Manipulator dynamics is highly coupled and non-linear. By predicting and com-pensating the non-linear effects, simple linear controllers can be used for the joints. Analytic models of the manipulator can be used to cancel most of these non-linear effects[Luh80a,80b]. Because it is difficult to measure precisely link parameters, there will be significant residual non-linear effects. These effects can be further reduced with adaptive control techniques[Koivo83,86,Lee84a,84b]. When the ma-nipulator paths are repetitive, compensation torques or forces can also be estimated 122 by learning[Arimot85,Miller86,87]. 123 Chapter 5 Conclusions New techniques in both inverse kinematics and path planning have been devised. A parallel computer architecture suitable for advanced robot control has been pro-posed. The architecture is called PHASAR (Parallel Hierarchical Asynchronous System Applied to Robotics). The application of PHASAR to manipulator inverse kinematics and path planning has been investigated in detail. In this chapter, the contributions of this thesis are summarized, and suggestions for further research are outlined. 124 5.1 Contributions The contributions of this thesis can be roughly divided into two areas: kinematics and path planning. 5.1.1 Kinematics In the area of manipulator kinematics, a novel iterative inverse kinematics solution is proposed. The method is based on the observation that many industrial manipu-lators have joints with readily identifiable functions in workspace coordinates, and hence is termed functional joint control (FJC). If each joint is controlled so that its function is fulfilled assuming that all other joints are correct already, the target can be reached in an iterative fashion. The method has been tested extensively with a PUMA-like manipulator. The manipulator has small non-zero offsets at all the links, to model the effect of incorrect link parameters due to calibration errors or link bending and twisting. A closed-form solution for this manipulator would thus be difficult to derive. Applications of FJC to other types of major linkages have also been studied. FJC is well-behaved at or around singularities. Since each joint is controlled separately, the computation burden can be naturally partitioned to a maximum of one processor per function (joint). Even with a single processor (HP 9050), inverse kinematics can be performed in real-time with the PUMA-like robot, with a typical iteration number of about 15. A maximum error tolerance of 0.1 mm can 125 be achieved with single-precision arithmetic. However, FJC has the disadvantage that its performance is dependent on the manipulator structure. As the joint offsets grow larger, the functions of joints be-come less well-defined, and FJC will require more iterations to converge. At some point FJC will fail altogether. Although FJC is not a truly general solution, it is useful for functionally partitionable manipulators, which most industrial manipula-tors are. Quantitative measures of how FJC degrades with non-ideal functionally partitionable manipulators have been presented. FJC can be extended to redundant manipulators if the extra degrees of freedom serve identifiable functions. Any iterative inverse kinematics solution has the problem of getting trapped in a local minimum. Heuristic rules utilizing an approximate closed-form solution are used to resolve such difficulties should they arise. It has also been shown that heuristic rules can be used to control a mobile base added to the PUMA-like robot in order to improve positioning capability of the robot. 5.1.2 Path Planning In path planning, a technique which employs a quantized configuration space poten-tial surface is proposed. The potential of each point in the configuration space of the robot is defined to be proportional to the distance from the point to the target. If a point is inside an obstacle, its potential is set to infinity. Objects are transformed from Cartesian space representations to configuration space. The transformation is only performed at points in the vicinity of the current robot location in configura-126 tion space, so that real-time execution speed is possible. If a certain configuration results in a collision, the corresponding point in configuration space is inside an obstacle. A hierarchical collision detection scheme is employed so that execution speed is seldom compromised even when many complex obstacles are present in the workspace. Arbitrarily shaped obstacles in 3-D can be handled. By following the potential gradient, the target can be reached and collisions with obstacles will be avoided. As the robot moves, only the potential of its neighbors in configuration space is checked to determine which point the robot should move to next. Sometimes there are local minimum regions on the potential surface, and a local search would fail to yield a plausible move. This problem is handled by extending obstacles at local minimum points. Every time a point is found to be a local minimum, the potential at that point is set to infinity. By extending the obstacle in this fashion, the robot can get out from simple local minimum regions. This path planning method allows real-time execution and avoids collisions for all parts of the manipulator. The algorithm has the desirable behavior that simple paths take less time to generate than complex paths. With the HP9050 computer, difficult paths among fairly complex 3-D obstacles (about 250 triangular surfaces total) for a 3-link manipulator (9 triangular surfaces) can be computed in less than a minute. The path planning method is suitable for multiple processor implemen-tation. Real-time execution speed is possible if enough processors are available. Parallel execution of sub-tasks with PHASAR has been simulated in conjunction with the inverse kinematics and obstacle avoidance modules. A single C program 127 running on an HP9050 computer with interactive computer graphics was used. Dif-ferent sampling frequencies for each of the control modules have been tried in the simulations. The overall functionality of the control architecture has been demon-strated. The possibility of VLSI implementation of the communication processors which are required in PHASAR has been studied through the physical implementation of a high-speed serial-to-parallel and parallel-to-serial converter using a shift register as a fast counter. The chip was designed in a 3 /xm CMOS3 process. The full custom chip contains over 900 transistors and was completed and thoroughly simulated. The chip has been tested and the maximum serial bit-rate is very close to what the simulations have predicted — about 1.5 MBit/s. By careful redesign, it is expected that the bit rate can be increased to over 20 MBit/s. 5.2 Suggestions for Further Research Many problems have still to be solved before real-time task-level robot programming becomes a reality. The following is a list of possible extensions to the work that has been done in this thesis: Local minimum regions in cluttered environments. Although the proposed obstacle extension technique can guide a robot out from simple local minimum regions, it may fail if only very narrow channels exist that connect the region with the outside world. A global search is probably required. However, some 128 heuristics must be used, since mapping all the obstacles from Cartesian space to configuration space will be too time consuming. Better multiprocessor emulation package. The multiprocessor emulation package that is used in this thesis is very simple. Messages can only be sent or re-ceived once every sampling period. Sampling periods are fixed. Often some processors have variable sampling periods. The path planner is one example: it completes its task much faster for a simple path than for a complex path with many obstacles in the way. At present a fixed medium sampling period is used for the path planner. For simple paths, the processor becomes idle before the end of the sampling period. For complex paths, more sampling periods will be required before a path is found. This arrangement is satisfactory for complex paths, but too conservative for simple paths. It would be better if an emulated processor can be made to sample as soon as it finishes its job. Physical implementation of PHASAR. Simulations provide an easy way to inves-tigate new ideas and concepts. However, approximations and simplifications are almost invariably made in any simulation. One can only be sure of the true value of an idea when its functionality has been demonstrated in the real world. A small prototype for PHASAR, using off-the-shelf components, can be built to evaluate the path planning and kinematics ideas proposed in this thesis. Physical implementation of VLSI chips. The mail manager and PD controller are good candidates for ASICs. Their designs are simple, and similar chips are not available on the market. 129 Other robot control modules. The PHASAR simulation package is designed so that additional control modules can be added easily. Some of the modules that should be added are: trajectory planner, dynamics compensator, path memorizer (for teach-by-doing), problem solver, natural language user inter-face, etc., in that order. 130 References [Aboaf87] Aboaf.E.W., Paul.R.P., "Living with the Singularity of Robot Wrists," IEEE Int. Conf. Robotics Auto., pp.1713-1717, 1987. [Albus72] Albus,J.S., Theoretical and Experimental Aspects of a Cerebellar Model, Ph.D. Thesis, University of Maryland, December 1972. [Albus81a] Albus,J.S., Barbera,A.J., and Fitzgerald,M.L., "Hierarchical Con-trol for Sensory Interactive Robots," Proc. 11th Int. Symp. Indus. Robots, pp. 497-505, 1981. [Albus81b] Albus,J.S., Brains, Behavior, and Robotics, BYTE/McGraw-Hill, 1981. [Albus83] Albus,J.S., McLean,C.R., Barbera,A.J., and Fitzgerald,M.L., "Hier-archical Control for Robots in an Automated Factory," Proc. IS Int. Symp. Indus. Robots, pp.13.29-13.43, 1983. [Albus84] Albus,J.S., "Robotics," Robotics and Artificial Intelligence, Brady,M. et al., ed., Springer-Verlag Berlin Heidelberg, pp.65-93, 1984. [Arimot85] Arimoto,S., Kawamura,S., "Can Mechanical Robots Learn by Them-selves?," Proc. 2nd Int. Symp. Robotics Res., pp.127-134, 1985. [Barber79] Barbera,A.J., Albus,J.S., Fitzgerald,M.L., "Hierarchical Control of Robots Using Microcomputers," Proc. 9th Int. Symp. Indus. Robots, pp. 405-422, 1979. [Boisso86] Boissonat,J.D., "An Automatic Solid Modeler for Robotics Applica-tions," Proc. 3rd Int. Symp. Robotics Res., pp.65-72, 1986. 131 [Brooks83] Brooks,R.A., "Solving the Find-Path Problem by Good Represen-tation of Free Space," IEEE Tran. SMC, vol.SMC-13, no.3, pp.190-197, 1983. [Brooks84] Brooks,R.A., "Planning Collision Free Motions for Pick and Place Operations," Proc. 1st Int. ;Symp. Robotics Res., pp.5-37, 1984. [Brooks86] Brooks,R.A., "A Layered Intelligent Control System for a Mobile Robot," Proc. 3rd Int. Symp. Robotics Res., pp.365-372, 1986. [Brooks87] Brooks,R.A., "A Hardware Retargetable Distributed Layered Archi-tecture for Mobile Robot Cfemtrol," IEEE Int. Conf. Robotics Auto., pp.106-110, 1987. [Buchal88] Buchal,R.O., Cherchas,DJE.n "An Iterative Method for Generating Kinematically Feasible Interference-Free Robot Trajectories", Robot-»co,in press, 1988. [Chan87] Chan,S.K.C, An Iterative Cfesnera/ Inverse Kinematics Solution with Variable Damping, M.A.Sr., Thesis, University of British Columbia, April, 1987. [Chen87] Chen,J., Chao,L., "Positioning Error Analysis for Robot Manipu-lators with All Rotary Isshgfes," IEEE J. Robotics Auto., vol.RA-3, no.6, pp.539-545, 1987. [Denavi55] Denavit,J., Hartenberg,R.SL, mA Kinematic Notation for Lower-Pair Mechanisms Based on MaferjEss," ASME J. Appl. Mech., pp.215-221, 1955. [Duffy80] Duffy,J., Analysis of Mt&mtikms and Robot Manipulators, Edward Arnold, 1980. [Edler85] Edler,J. et al., "Issues Rela'teSsto MIMD Shard-memory Computers: the NYU Ultracomputer Apjoioach," Int. Conf. Computer Architec-ture, pp.126-135, 1985. [Elgazz85] Elgazzar,S., "Efficient KrnsEcrattic Transformations for the PUMA 560 Robot," IEEE J. Rotextims Auto., vol.RA-1, no.3, pp.142-151, 1985. [Espiau86] Espiau,B., Boulic,R., "Coffiaion Avoidance for Redundant Robots with Proximity Sensors," Proc. 3rd Int. Symp. Robotics Res., pp.243-251, 1986. 132 [Faverj84] Faverjon,B., "Obstacle Avoidance using an Octree in the Configu-ration Space of a Manipulator," IEEE Int. Conf. Robotics Auto., pp.504-512, 1984. [Feathe83] Featherstone,R., "Position and Velocity Transformations between Robot End Effector Coordinates and Joint Angles," Int. J. Robotics Res., vol.2, no.2, pp.35-45, 1983. [Freund83] Freund,E., "Hierarchical Nonlinear Control for Robots," Proc. 1st Int. Symp. Robotics Res., pp.12.1-12.14, 1983. [Freund84] Freund,E., "Non-Linear Control with Hierarchy for Coordinated Op-eration of Robots," Robotics and Artificial Intelligence, Brady,M. et al., ed., Springer-Verlag Berlin Heidelberg, pp.321-344, 1984. [Freund85] Freund,E., Hoyer,H., "Collision Avoidance in Multi-Robot Systems," Proc. 2nd Int. Symp. Robotics Res., pp.136-146, 1985. [Freund86] Freund,E., Hoyer,IL, "On the On-Line Solution of the Findpath Problem in Multi-Robot Systems," Proc. 3rd Int. Symp. Robotics Res., pp.253-262, 1986. [Gilber85] Gilbert,E.G., Johnson,D.W., "Distance Functions and Their Appli-cation to Robot Path Planning in the Presence of Obstacles," IEEE J. Robotics Auto., vol.RA-1, no.l, pp.21-30, 1985. [Goke73] Goke,R., Lipovski,G.J., "Banyan Networkds for Partitionaing on Multiprocessor Systems," Proc. 1st Ann. Symp. Computer Archi-tecture, pp.21-30, 1973. [Golden85] Goldenberg,A.A., Benhabib,B., Fenton,R.G., "A Complete General-ized Solution to the Inverse Kinematics of Robots," IEEE J. Robotics Auto., vol.RA-1, no.l, pp.14-20, 1985. [Gottli82] Gottlieb,A. et al., "The NYU Ultracomputer - Designing an MIMD Shared Memory Parallel Computer," Int. Conf. Computer Architec-ture, pp.27-42, 1982. [Haynes82] Haynes,L.S. et al., "A Survey of Highly Parallel Computing," IEEE Computer, pp.9-24, January 1982. [Hogan84a] Hogan,N., "Impedance Control of Industrial Robots," Robotics & Computer-Integrated Manufacturing, vol.1, no.l, pp.97-113, 1984. [Hogan84b] Hogan,N., "Impedance Control: An Approach to Manipulation," Proc. ACC, pp.304-313, 1984. 133 [Horst85] Horst,R.W., Chou,T.C.K., "An Architecture for High Volume Trans-action Processing," Int. Conf. Computer Architecture, pp.240-245, 1985. [Hunt86] Hunt,K.H., "The Particular of the General? (Some Examples from Robot Kinematics)," Mechanism and Machine Theory, vol.21, No.6, pp.481-487, 1986. [Huscro84] Huscroft,C.K., Learning Algorithms for Manipulator Control, M.A.Sc. Thesis, University of British Columbia, October, 1984. [Ishii87j Ishii,M. et al, "A 3-D Sensor system for Teaching Robot Paths and Environments," Int. J. Robotics Res., vol.6, no.2, pp.45-59, 1987. [Jones80] Jones,A.K., Schwarz,P., "Experience Using Multiprocessor Systems — A. Status Report," Computing Surveys, vol.12, no.2, pp.121-165, 1980. [Kant86] Kant,K., Zucker,S.W., "Toward Efficient Trajectory Planning: The Path-Velocity Decomposition," Int. J. Robotics Res., vol.5, no.3, pp.72-89, 1986. [Keppel75] Keppel,E., "Approximating Complex Surfaces by Triangulation of Contour Lines," IBM J. Res. Develop., pp.2-11, January 1975. [Khatib78] Khatib,0., Le Maitre,J.F., "Dynamic Control of Manipulators Oper-ating in a Complex Environment," 3rd Symp. Theory Practice Robots Manipulators, Udine, Italy, pp.267-282, 1978. [Khatib86a] Khatib,0., "The Operational Space Formulation in the Analysis, Design, and Control of Robot Manipulators," Proc. 3rd Int. Symp. Robotics Res., pp.263-270, 1986. [Khatib86b] Khatib,0., "Real-Time Obstacle Avoidance for Manipulators and Mobile Robots," Int. J. Robotics Res., vol5, no.l, pp.90-98, 1986. [Khatib87] Khatib,0., "A Unified Approach for Motion and Force Control of Robot Manipulators: The Operational Space Formulation," IEEE J. Robotics Auto., vol.RA-3, no.l, pp.43-53, 1987. [Kircan87j Kircanski,M., Vukobratovid,M., "Contribution to Control of Redun-dant Robotic Manipulators in an Environment with Obstacles," Int. J. Robotics Res., vol.5, no.4, pp.112-119, 1986. [Klett79] Klett,D., A Cerebellum-Like Learning Machine, M.A.Sc. Thesis, University of British Columbia, July, 1979. 134 [Koivo83] Koivo,A.J., Guo,T.H., "Adaptive Linear Controller for Robotic Ma-nipulators," IEEE Trans. Auto. Control, vol.AC-28, no.2, pp.233-242, 1983. [Koivo86] Koivo,A.J., "Force-Position-Velocity Control with Self-Tuning for Robotics Manipulators," IEEE Int. Conf. Robotics Auto., pp.1563-1568, 1986. [Kupers87] Kuperstein,M., "Adaptive Visual-Motor Coordination in Multijoint Robots using Parallel Architecture," IEEE Int. Conf. Robotics Auto., pp.1595-1602, 1987. [Lee82a] Lee,C.S.G., "Robot Arm Kinematics, Dynamics, and Control," IEEE Computer, pp.62-80, December 1982. [Lee82b] Lee,C.S.G. et al., "Hierarchical Control Structure using Special Pur-pose Processors for the Control of Robot Arms," IEEE Proc. Pattern Recognition Image Processing Conf., pp.634-640, 1982. [Lee84a] Lee,C.S.G., Lee,B.H., "Resolved Motion Adaptive Control for Me-chanical Manipulators," Proc. ACC, pp.314-319, 1984. [Lee84b] Lee,C.S.G. et al., "Adaptive Control for Robot Manipulators in Joint and Cartesian Coordinates," IEEE Int. Conf. Robotics Auto., pp.530-539, 1984. [Leung87] Leung,S.S., Shanblatt,M.A., "Real-Time DKS on a Single Chip," IEEE J. Robotics Auto., vol.RA-3, no.4, pp.281-290, 1987. [Lilly85] Lilly,K.W., Multiprocessor Implementation of Real-Time Control Schemes for Robot Manipulators, M.Sc. Thesis, Ohio State Univer-sity, 1985. [Lozano79] Lozano-Perez,T., Wesley,M.A., "An Algorithm for Planning Collision-Free Paths Among Polyhedral Obstacles," Comm. ACM, vol.22, no.10, pp.165-175, 1979. [Lozano86a] Lozano-Perez,T., "Spatial Planning: A Configuration Space Ap-proach," IEEE Trans. Computers, vol.C-32, no.2, pp.108-120, 1986. [Lozano86b] Lozano-Perez,T., "Motion Planning for Simple Robot Manipula-tors," Proc. 3rd Int. Symp. Robotics Res., pp.133-140, 1986. [Lozano87] Lozano-Perez,T., "A Simple Motion-Planning Algorithm for Gen-eral Robot Manipulators," IEEE J. Robotics Auto., vol.RA-3, no.3, pp.224-238, 1987. 135 [Luh80a] Luh,J.Y.S. et al., "On-Line Computational Scheme for Mechanical Manipulators," ASME J. Dyn. Sys. Measurement Control, vol.102, pp.69-76, June 1980. [Luh80b] Luh,J.Y.S. et al., "Resolved-Acceleration Control of Mechanical Ma-nipulators," IEEE Trans. Auto. Control, vol.AC-25, no.3, pp.468-474, 1980. [Luh83] Luh,J.Y.S., "An Anatomy of Industrial Robots and Their Controls," IEEE Trans. Auto. Control, vol.AC-28, no.2, pp.5-25, 1983. [Luh84] Luh,J.Y.S., Campbell,C.E.,Jr., "Minimum Distance Collision-Free Path Planning for Industrial Robots with a Prismatic Joint," IEEE Trans. Auto. Control, vol.AC-29, no.8, pp.675-680, 1984. [Lumels87a] Lumelsky,V., Sun,K., "Gross Motion Planning for a Simple 3D Ar-ticulated Robot Arm Moving Amidst Unknown Arbitrarily Shaped Obstacles," IEEE Int. Conf. Robotics Auto., pp.1929-1934, 1987. [Lumels87b] Lumelsky,V.J., "Effect of Kinematics on Motion Planning for Planar Robot Arms Moving Amidst Unknown Obstacles," Int. J. Robotics Res., vol.RA-3, no.3, pp.207-223, 1987. [Marr69] Marr,D., "A Theory of the Cerebellar Cortex," J. Physiol. Lond., 202, p.437, 1969. [Matela85] Matelan,N., "The FLEX/32 Multicomputer," Int. Conf. Computer Architecture, pp.209-213, 1985. [Mayorg87j Mayorga,R.V., Wong,A.K.C, "A Singularities Avoidance Method for the Trajectory Planning of Redundant and Nonredundant Robot Manipulators," IEEE Int. Conf. Robotics Auto., pp. 1707-1712, 1987. [Milenk83] Milenkovic,V., Huang,B., "Kinematics of Major Robot Linkage," Proc. ISth Int. Symp. Indus. Robots, pp.16.31-16.47, 1983. [Miller86] Miller III,W.T. et al., "Application of a General Learning Algorithm to the Control of Robotic Manipulators," Int. J. Robotics Res., vol.6, no.2, pp.84-98, 1987. [MiHer87] Miller III,W.T., "Sensor-Based Control of Robotic Manipulators Us-ing a General Learning Algorithm," IEEE J. Robotics Auto., vol.RA-3, no.2, pp.157-165, 1987. [Montgo87] Montgomery,M. et al., "Navigation Algorithm for a Nested Hierar-chical System of Robot Path Planning Among Polyhedral Obsta-cles," IEEE Int. Conf. Robotics Auto., pp.1616-1622, 1987. 136 [Naedel85] Naedel,D., "Closely Coupled Asynchronous Hierarchical and Parallel Processing in an Open Architecture," Int. Conf. Computer Architec-ture, pp.215-220, 1985. [Nagel84] Nagel,R.N., "State of the Art and Predictions for Artificial Intelli-gence and Robotics," Robotics and Artificial Intelligence, Brady,M. et al., ed., Springer-Verlag Berlin Heidelberg, pp.3-45, 1984. [Nakamu85] Nakamura,Y., Kinematical Studies on the Trajectory Control of Robot Manipulators, Ph.D. dissertation, Kuoto, Japan, June 1985. [Nestle85] Nestle,E., Inselberg,A., "The Synapse N+l System: Architectural Characteristics and Performance Data of a Tightly-Coupled Multi-processor System," Int. Conf. Computer Architecture, pp.233-239, 1985. [Newman87] Newman,W.S., Hogan,N., "High Speed Robot Control and Obstacle Avoidance using Dynamic Potential Functions," IEEE Int. Conf. Robotics Auto., pp.14-24, 1987. [Orin84a] Orin,D.E., "Pipelined Approach to Inverse Plant Plus Jacobian Control of Robot Manipulators," IEEE Int. Conf. Robotics Auto., pp.169-175, 1984. [Orin84b] Orin,D.E., Schrader,W.W., "Efficient Jacobian Determination for Robot Manipulators," Proc. 1st Int. Symp. Robotics Res., pp.728-734, 1984. [Orin86] Orin,D.E., Tsai,Y.T., "A Real-Time Computer Architecture for In-verse Kinematics," IEEE Int. Conf. Robotics Auto., pp.843-850, 1986. [Orland84] Orlando,N.E., "An Intelligent Robotics Control Scheme," Proc. ACC, pp.204-209, 1984. [Patter82] Patterson,D.A., Sequin,C.H., "A VLSI RISC," IEEE Computer, vol.15, no.9, pp.8-22, 1982. [Paul8l] Paul,R.P.C, Robot Manipulators: Mathematics, Programming, and Control, MIT Press, Massachusetts, 1981. [Paul83] Paul,R.P.C., Stevenson,C.N., "Kinematics of Robot Wrists," Int. J. Robotics Res., vol.2, no.l, pp.31-38, 1983. [Paul84] Paul,R.P.C. et al., "A Systematic Approach for Obtaining the Kine-matics of Recursive Manipulators Based on Homogeneous Transfor-mations," Proc. 1st Int. Symp. Robotics Res, pp.707-726, 1984. 137 [Paul85] Paul,R.P.C, Zhang,H., "Robot Motion Trajectory Specification and Generation," Proc. 2nd Int. Symp. Robotics Res., pp.188-194, 1985. [Poon85a] Poon,J.K., Pulfrey,D.L., and Lawrence,P.D., "Arithmetic Processor Chip for Robot Control," Int. sym. VLSI tech. sys. applications, pp.337-341, 1985. [Poon85b] Poon,J.K., An Arithmetic Processor for Robotics, M.A.Sc. Thesis, University of British Columbia, May, 1985. [Poon87] Poon,J.K., The PHASAR Simulation Package, technical report, Dept. of Elect. Eng., U. of British Columbia, 1987. [Sahar86] Sahar,G., Hollerback,J.M., "Planning of Minimum-Time Trajecto-ries for Robot Arms," Int. J. Robotics Res., vol.5, no.3, pp.90-100, 1986. [Seitz85] Seitz,C.L., "The Cosmic Cube," Comm. ACM, vol.28, no.l, pp.22-33, 1985. [Shin84] Shin,K.G., Malin,S.B., "A Hierarchical System Structure for Coordi-nated Control of Industrial Manipulators," IEEE Int. Conf. Robotics Auto., pp.609-619, 1984. [Singh87] Singh,J.S., Wagh,M.D., "Robot Path Planning using Intersecting Convex Shapes: Analysis and Simulation," IEEE J. Robotics Auto., vol.RA-3, no.2, pp.101-108, 1987. [Taylor79] Taylor,R.H., "Planning and Execution of Straight line Manipulator Trajectories," IBM J. Res. Develop., vol.23, no.4, pp.424-436, July 1979. [Trevel86] Trevelyan,J.P. et al., "ET: A Wrist Mechanism without Singular Positions," Int. J. Robotics Res., vol.4, no.4, pp.71-85, 1986. [Wample86] Wampler,C.W., "Manipulator Inverse Kinematic Solutions Based on Vector Formulations and Damped Least-Squares Methods," IEEE Trans. SMC, vol.SMC-16, no.l, pp.93-101, 1986. [Whitby85] Whitby-Strevens,C, "The Transputer," Int. Conf. Computer Archi-tecture, pp.292-300, 1985. 0 [Whitne72] Whitney,D.E., "The Mathematics of Coordinated Control of Pros-thetic Arms and Manipulators," ASME J. Dyn. Sys. Measurement Control, vol.122, pp.303-309, December 1972. 138 [Wiley87] Wiley,P., "A Parallel Architecture Comes of Age At Last," IEEE Spectrum, pp.46-50, June 1987. [Wulf74] Wulf,W.A. et al., "HYDRA: The Kernel of a Multiprocessor Oper-ating System," Commun. ACM, pp.337-345, June 1974. [Yoshik85] Yoshikawa,T., "Manipulability of Robotic Mechanisms," Int. J. Robotics Res., vol.4, no.2, pp.3-9, 1985. 139 Appendix A Analytical Kinematic Transformations The Denavit-Hartenberg coordinate frame assignments and parameter values of the manipulators discussed in this thesis are presented below. The quantity listed in the joint function row is the most distinct quantity that a joint controls. ||p|| stands for the length of the position vector p, px is the x-component of p, etc., and Zp is the orientation of p. Where the parameter is the joint variable, '-' is shown. There are three links in each of the major linkage classes. However, because of the way coordinate frames are assigned, it is necessary to specify the parameters for the fourth coordinate frame also. If there is no fourth link, then the fourth coordinate frame is the tool frame. 140 A . l Two-Link Manipulator Figure A.l: Coordinate Frames of 2-Link Manipulator Joint 1 2 6 (rad) - -a (rad) 0 0 a (m) 1 1 d (m) 0 0 Table A.l: Link Parameters for 2-Link Manipulator Inverse kinematics solution: q2 = arccos x2 + y2 - «V - a 2 r qi — arctan —^^  — arctan ( — 2a\a2 ) a2 sin (172) + a2 cos (q2) 141 2 P U M A Figure A.2: Coordinate Frames of PUMA Manipulator Joint 1 2 3 4 5 6 0 (rad) - - - - - -a (rad) - T T / 2 0 TT/2 - T T / 2 IT/2 0 a (m) 0 0.43 0 0 0 0 d (m) 0 0.15 0 0.43 0 0.056 0maz (rad) 2.79 0.7 3.92 2.9 1.74 4.6 0mm (rad) -2.79 -3.92 -0.7 -1.9 -1.74 -4.6 Function LV I I P I I Za Za Zn,o Table A.2: Link Parameters for PUMA Manipulator 142 Inverse kinematics solution: <7i = arc tan qi = arctan g 3 = arctan 94 9s 96 arctan arctan arctan ±Pv\/Pz2 + Pv2 ~ dl ~ diVz ±Px y^l + P2y ~ A + d2pv - (p* (a2 + <US3) + (d4C3) (±y/pl + pl- dl)) P, (d4C3) - (o2 + d4Ss) (±yjp\ + p\-d\) Pl + P2v+P2z-d\-a\ ±yjid\a\ - (pi + P2v + p l - d l - al - drf C\C2zax + SiC23ay — S2zaz  (C1C23C4 — 515*4) ax + {S1C23C4 + C1S4) av — C4S23az CiS2$ax + S\S2$ay + C2zaz  (—S1C4 — C1C2354) re,, + (C tC4 — S1C23S4) n-y + (5*4523) nz ( — 5 i C 4 — C1C2354) ox + (C1C4 — S1C23S4) oy + (54523) oz • = cos (g x), St = sin (ft), C 2 3 = cos (q2 + 9s), etc. • If (9i ,92,93,94,9s,96) is a solution, then (91,92,93,94 + * " , - 9 5 , 9 6 + IT) is also a solution. 143 A.3 The Twelve Major Linkage Classes Figure A.3: Coordinate Frames of Class 1 Major Linkage Joint 1 2 3 4 6 (rad) 0 TT/2 0 0 a (rad) -7I-/2 - T T /2 0 - T T /2 a (m) 0 0 0 0 d (m) - - - 0 Function Pz Pv Px ? Table A.3: Link Parameters for Class 1 Major Linkage 144 Figure A.4: Coordinate Frames of Class 2 Major Linkage Joint 1 2 3 4 6 (rad) 0 0 - -a (rad) -TT/2 0 TT/2 -TT/2 a (m) 0 0 0 0 d (m) - - 0 1 Function Pz Pv ? Table A.4: Link Parameters for Class 2 Major Linkage 145 Figure A.5: Coordinate Frames of Class 3 Major Linkage Joint 1 2 3 4 0 (rad) n/2 - - -a (rad) n/2 -n/2 TT/2 -TT/2 a (m) 0 0 0 0 d (m) - 0 0 1 Function Pz LV ? Table A.5: Link Parameters for Class 3 Major Linkage 146 nZ2 Figure A.6: Coordinate Frames of Class 4 Major Linkage Joint 1 2 3 4 0 (rad) - 0 0 -a (rad) 0 - T T /2 0 -TT/2 a (m) 0 0 0 0 d (m) 0 - - 0 Function I I P I I ? Table A.6: Link Parameters for Class 4 Major Linkage 147 Figure A.7: Coordinate Frames of Class 5 Major Linkage Joint 1 2 3 4 0 (rad) - 0 - -a (rad) 0 0 -TT/2 -n/2 a (m) 1 0 0 0 d (m) 0 - 0 1 Function Pz ? Table A.7: Link Parameters for Class 5 Major Linkage 148 x2 Figure A.8: Coordinate Frames of Class 6 Major Linkage Joint 1 2 3 4 0 (rad) - 0 - -a (rad) 0 TT/2 - T T /2 -TT/2 o (m) 1 0 0 0 d (m) 0 - 0 1 Function Pz ? Table A.8: Link Parameters for Class 6 Major Linkage 149 Figure A.9: Coordinate Frames of Class 7 Major Linkage Joint 1 2 3 4 6 (rad) - - 0 -a (rad) - T T /2 TT/2 0 - T T /2 a (in) 0 0 0 0 d (m) 0 0 - 0 Function P^ IIPlI ? Table A.9: Link Parameters for Class 7 Major Linkage 150 I I I I *1 Figure A.10: Coordinate Frames of Class 8 Major Linkage Joint 1 2 3 4 9 (rad) - - - -a (rad) - T T / 2 TT/2 TT/2 -TT/2 a (m) 0 0 0 0 d (m) 0 0 1 1 Function P^ P^ ? Table A. 10: Link Parameters for Class 8 Major Linkage 151 hZ3 Figure A . 1 1 : Coordinate Frames of Class 9 Major Linkage Joint 1 2 3 4 0 (rad) - - - -a (rad) - T T / 2 0 T T / 2 - T T / 2 a (m) 0 1 0 0 d (m) 0 0 0 1 Function ^ P LV I I P I I ? Table A . 1 1 : Link Parameters for Class 9 Major Linkage 1 5 2 Figure A. 12: Coordinate Frames of Class 10 Major Linkage Joint 1 2 3 4 0 (rad) - 0 - -a (rad) - T T /2 0 TT/2 -tt/2 a (m) 0 0 0 0 d (m) 0 - 0 1 Function HPII LV ? Table A.12: Link Parameters for Class 10 Major Linkage 153 Figure A.13: Coordinate Frames of Class 11 Major Linkage Joint 1 2 3 4 0 (rad) - - - -a (rad) 0 - T T /2 tt/2 - T T /2 a (m) 1 1 0 0 d (m) 0 0 0 1 Function Pz P^ ? Table A.13: Link Parameters for Class 11 Major Linkage 154 y3 Figure A.14: Coordinate Frames of Class 12 Major Linkage Joint 1 2 3 4 6 (rad) - TT/2 - -a (rad) - T T /2 TT/2 - T T /2 - T T /2 a (m) 0 0 0 0 d (m) 0 - 0 1 Function IIPII ^ P ? Table A.14: Link Parameters for Class 12 Major Linkage 155 Appendix B Intersection Between Two Triangles Consider two triangular surfaces in 3-D, Si and S2 (Fig. B.l). Each triangle defines a plane, ITi and II2. Each side of a triangle is a line segment, represented by a ray, Ri - -=1<2a3 and i2 2 y y = 1 2 3 . Each ray in turn defines an infinite line, L i i j = l 2 3 and In general, the line segments i?2y will intersect TIi at either 0 or 2 points. If there are 0 intersection points, then 5X does not intersect S2, a n a * the check is finished. If there are 2 intersection points, let them be Pi and P2. Pi and P2 defines a line segment, Ri, which is the intersection line between S2 and ITi. If any portion of Ri is within S\, then Si and S2 intersect. 156 Figure B.l: Intersection Between Two Triangles Ri defines an infinite line L^. Let the intersect points between L, and L l y be Qj. If Qj is within Ri and R\., then the point is a real intersection point. If Qj is within RXi but outside Ri, then let it be called a phantom intersection point. In general, the following cases are possible (Fig. B.2): 1. 2 real points; 2. 1 real point, 1 phantom point; 3. 2 phantom points. Let Qj,j=i,2 be the 2 real or phantom intersection points. 157 Figure B.2: Real and Phantom Intersection Points Let Ri be represented by the parametric equation x - P u y-Ply z- Plt = t. (B.l) Piz - PU P2V - Ply Pin - Pi, Let the location of Qj on iZ, be tj. Without loss of generality, one can let ti <t2. When there are two real intersection points, the intersection line between Si and S2 is given by the ray from ti to t2. If one intersection point is real and one is phantom, let Qr and Qp be the real and phantom point respectively. Let tr and tp be the corresponding parameter on Ri. If tp < 0, then the ray from Pi to Qr is the intersection line between Si and S2. If tp > 1, then the ray from Qr to P2 is the intersection line (See Fig. B.3). If both intersection points are phantom, then Ri is either completely inside Si or completely outside (Fig. B.4). If tt • t2 > 0, i.e., ^ and t2 have the same sign, 158 Pi Figure B.3: One Real and One Phantom Intersection Point then Si does not intersect S2. Otherwise, tx • t2 < 0, and .Rj is the intersection line between Si and S2. In actual implementation, there are numerical problems with degenerate cases, such as when Si touches 52, or when they are parallel. See [Poon87] for the details concerning these situations. 160 Appendix C Emulation of Multi-Processors Building an actual custom multi-processor system is both expensive and time con-suming. Modification of the system is also difficult. Therefore, it is best to emulate the multi-processor with a conventional computer. Real-time execution speed can-not be expected from such a program, but otherwise the emulation program will behave similar to a real multi-processor system. The emulation program is implemented by modelling each processor as a func-tion with static local variables, i.e., the variables retain their values between calls to the function. These functions will be referred to as processor-functions. Com-munication among the processor-functions is done by pushing and popping data on a queue. There is one queue for each processor. The queue represents the incoming buffer of the processor, which in turn serves as a mailbox in a real multi-processor 161 system. Fig. C.l outlines the emulator structure. C . l The Time Manager Since each processor may have a different sampling frequency, and the effect of asynchronous program execution must be studied, the processor-functions cannot be called in a fixed sequence. This effect is simulated with the use of a function that serves as a time manager. Each processor-function has an associated sampling period and stop-watch that records how long the processor has been executing since the time it was last staxted. The sampling periods can be changed interactively by the user for evaluation purposes. The time manager is an infinite loop that keeps track of a clock. The clock is ticked every time the time manager goes through a loop. In the current implemen-tation, each clock tick represents 1 (is of simulated time. At every clock tick, the time count of every processor-function is also incremented. If the stop-watch of a processor reaches its sampling period, that function is called and its stop-watch is reset to zero. 162 mall manager mall boxes processor-function processor-function processor-function time manager Figure C l : Multiprocessor Emulation Scheme 163 C.2 Passing Messages When a processor-function needs to pass a message to another processor-function, it sends the message to the post master function. The messages that a processor-function sends is of the form [destination] [message name] [no. of words] [word ... word] . where destination: Name of the destination processor-function, message name: Name of the variable(s) that is being passed, no. of words: Length of the passed variable(s) in number of words, word ... word: Each word as a separate actual parameter. Each processor-function has an associated queue which resides inside the mail manager. The queue tail pointers are globally accessible, but a processor-function only needs access to its own tail position. The mail manager pushes message names and associated values on the queue of destination processors. The queue tail position for the destination processor is then updated. When a processor-function is called by the time manager, it starts by checking its queue tail position. If the position is non-zero, then there are messages waiting on its queue. The processor reads all the messages and stores them in its own local 164 memory, depending on the message name. It is assumed that a processor always has enough time to read all of its incoming messages. After the messages have been read, the queue tail pointer is reset to zero. The processor-function can then in turn send messages to other processors. This is usually done at the end of the function. 165 Appendix D Design of the Mail Manager To achieve high-speed serial communication capability and system compactness, the mail managers (MM) should be VLSI devices. The structure of each of these MMs is shown in Fig. D.l. The MM is a communication front end processor for the MP. When the MP wants to write to another processor, it can dump the message on the outgoing buffer and then resume its other tasks. The message contains the address (name) of the destination processor. Upon receipt of a message from its host processor, the MM will look at the destination and send the message serially to that processor. Choice of destination is done via a multiplexer, which also allows broadcasting of the message to more than one processor. 166 data bus control bus controller » a- I -bu Her parallel-to-serial *J m ux buffer I serlal-to-parallel communication lines Figure D.l: Structure of Mail Manager 167 Messages from other processors are read by the MM and stored in the incoming buffers. Each communication line has its own buffer, so that messages will not be lost, provided that they do not overflow the buffer. The buffers are daisy-chained together so that the MP only sees one buffer. The MP can test whether the incoming buffer is empty or not, and read from it at its convenience. Control signals are generated by a simple controller which communicates with the MP through special signal lines. Each of the processor sets (MP + MM) in PHASAR has its own clock. The clock frequency must be fairly accurate since all the MMs should communicate with the same bit rate. Because the serial bits are sampled in the middle, clocks do not need to have the same phase. The critical parts of the MM are the serial-to-parallel and parallel-to-serial con-verters, since they dictate the maximum communication speed in PHASAR. In order to enable the serial-to-parallel and parallel-to-serial to run as fast as the fabrication process will allow, a ring-connected dynamic shift register is used for a counter in-stead of the usual ring counter implemented with full-adders. The minimum clock width required is thus the time it takes for the shift register to shift one position, which is approximately two gate delays. With the 3 micron CMOS process that is available from Northern Telecom, one gate delay is about 3 nanoseconds. If each bit is sampled by counting to 16, the maximum bit rate will be approximately 10 MBit/s. If the clock rates are accurate enough, one can probably sample by count-ing to 8 instead, doubling the bit rate to 20 MBit/s. In fact, Inmos Transputers already support serial communication at 20 MBit/s. 168 1 data in 10-blt shift register up 16 counter data out start up 11 counter paraliel-to-serfal converter data In 1 data out 8-btt shift register ~1_ detector shift start up 8 counter up 16 counter start start up 8 counter reset serial-to-parallel converter Figure D.2: Block Diagram of Communications Test Chip A test chip has been designed in Northern Telecom's CMOS3 process and sub-mitted to the Canadian Micro-Electronics Corporation in May 1986 for fabrication. This test chip contains one serial-to-parallel and one parallel-to-serial converter. A block diagram of the circuit is shown in Fig. D.2. The full custom chip contains over 900 transistors and was completed and thoroughly simulated in two weeks from ini-tial concept. Both switch-level (rnl) and device-level simulators (SPICE) were used. Because extra circuitry was added to make the counters self-correcting, simulations indicate that the maximum bit rate is only 2.0 MBit/s. The chip has been fabricated 169 and tested. The maximum serial bit-rate is very close to what the simulators have predicted — about 1.5 MBit/s. The delay caused by the self-correcting circuitry can probably be removed by correcting only at the end of each byte received, rather than testing continuously. The buffers can be made basically dual-port random access memories (RAM). It can be made with a single-port RAM with ring counters that update the first and last address of the queue. The full and empty flags can be generated with some simple random logic. Very compact buffers can be made this way. Faster accessing speed can be achieved with the use of special hardware queues, at the expense of more chip area. For PHASAR, the RAM approach is preferred. If a host (MP or MM) wants to push a byte onto the buffer, it must first check if the buffer is full. If the host proceeds to push the byte even when the buffer is full, the data will be lost. Similarly, the host must check whether the buffer is empty or not before attempting to read from it. Reading from the queue is blocked while it is being written to. 170 Appendix E Proportional-Derivative Controller At the lowest end of PHASAR, control signals for the joint actuators must be generated. In order to improve the stability and speed of the manipulator, a high-speed PD controller chip can be used. The chip implements the equation: U = KP {0d -0) + Kv (0d -6)+Uc (E.l) where 171 control bus data bus A data bus B Figure E.l: Block Diagram of PD controller U — control signal applied to joint actuator Kp = proportional gain dd = desired angle 0 = current angle Kv = velocity gain (damping) dd — desired angular velocity 0 = current angular velocity Uc = compensation term for dynamics and disturbances A block diagram of the PD controller is shown in Fig. E.l. The current joint position and velocity in digital form are read from sensors. The desired values are supplied by the joint coordinator, which is one level up in the control hierarchy. For high-speed manipulation, a dynamics compensation term, Uc, is fed from the dynamics module. By selectively changing the coefficients Kp, Kv, 0d, 0d, and Uc, it is possible to have any or all or position, velocity, and force control. It is also 172 possible to completely bypass the PD controller by setting Kp and Kv to zero, in which case the output of the actuator is completely determined by Uc. A 16-bit word size is sufficient for the PD controller. Multiplies are best im-plemented with shift-and-add operations, in order to save die area, improving yield and reducing cost. A parallel 16-bit adder can perform an addition in less than 30 ns. Using a non-overlapping two-phase clock, the combined cycle time should be about 50 ns. With a hardware configured serial multiplier, multiplies can be accomplished in 0.8 ps. The minimum sampling period of the PD controller is thus about 2 ps, corresponding to a maximum sampling frequency of 500 kHz. A digital-to-analog converter (DAC) for the analog output U can be integrated on the PD controller chip to make the system more compact. From circuit level simulations using SPICE, it was found that the most accurate DAC that can be made with the CMOS technology available and using a simple R — 2R network is a 5-bit DAC. This is enough for controlling actuators. The layout of such a DAC is also quite compact. 173 


Citation Scheme:


Citations by CSL (citeproc-js)

Usage Statistics



Customize your widget with the following options, then copy and paste the code below into the HTML of your page to embed this item in your website.
                            <div id="ubcOpenCollectionsWidgetDisplay">
                            <script id="ubcOpenCollectionsWidget"
                            async >
IIIF logo Our image viewer uses the IIIF 2.0 standard. To load this item in other compatible viewers, use this url:


Related Items