TRAJECTORY PLANNING ALONG CONTINUOUS PATHS SUBJECT TO PROCESS CONSTRAINTS By Dan Li B.A.Sc. (Mechanical Engineering) TsingHua University, 1985 A THESIS SUBMITTED IN PARTIAL FULFILLMENT OF THE REQUIREMENTS FOR THE DEGREE OF MASTER OF APPLIED SCIENCE in THE FACULTY OF GRADUATE STUDIES MECHANICAL ENGINEERING We accept this thesis as conforming to the required standard THE UNIVERSITY OF BRITISH COLUMBIA December 1993 © Dan Li, 1993 _____________ In presenting this thesis in partial fulfilment of the requirements for an advanced degree at the University of British Columbia, I agree that the Library shall make it freely available for reference and study. I further agree that permission for extensive copying of this thesis for scholarly purposes may be granted by the head of my department or by his or her representatives. It is understood that copying or publication of this thesis for financial gain shall not be allowed without my written permission. (Signature) Department of P31J The University of British Columbia Vancouver, Canada Date DE.6 (2/88) F 9 3 Abstract This thesis presents a new trajectory planning algorithm for planning safe and smooth tra jectories of a robot tool moving along a specified curve subject to application constraints. The application constraints include geometric constraints (allowable orientation and po sition of the tool) and motion constraints (joint limits, robot configuration inversions and collision avoidance). The algorithm, which is based on a configuration space approach for robot motion planning, addresses the problem of motion planning along a curve rather than point-to-point motion associated with pick and place operation. Configuration spaces are searched to assess the locations of the obstacles in the vicinity of the curve and the curvature of the curve in the work space. The trajectory is planned by searching between path segments connecting successive curve points, generating the path along a curve linking the path segments, and selecting the optimal configurations along the path to construct the final trajectory. If more than one paths exist, an A* search is em ployed to optimize the path. The selected path specifies a sequence of tool configuration ranges, and each configuration produces several inverse kinematic solutions. A second A* search is used to select inverse kinematic solutions, which avoid major changes of robot configuration (i.e. joint inversion). Since the algorithm searches the allowable tool orientation range, the optimal trajectory which maximizes process quality is obtained. The trajectory allows the tool to translate and rotate simultaneously without collision, and the tool remains within the allowable orientation range and tracks the desired curve within a given tolerance. Since the step size between curve points is varied dynamically as the search proceeds, the path is planned based on a minimum number of curve points, resulting in fast path planning. A smaller step size is used if the curvature of the curve 11 or the likelihood of collision is high. The primary advantage of the algorithm is not only in finding a collision free path but in planning a trajectory which satisfies application constraints. It is felt that this algorithm provides a high quality process which is difficult to achieve with manual robot programming. An off-line simulation program, TRAJPLAN, has been developed based on this al gorithm. After the user inputs the application specifications, selects the curves to be processed and the environment objects of interest, TRAJPLAN automatically plans the trajectory of robot tool and generates the required set of sequential robot joint angles. TRAJPLAN plans the motion with different precision depending on the requirements of the application. This algorithm has been demonstrated for robotic fish butchering and welding, and the issues related to these applications are discussed in this thesis. As an example of a typical problem, the system automatically plans the trajectory of a robot tool moving along a 3-D curve with 31 points and surrounded by 5 obstacles in 1’20”. The algorithm is suitable for a variety of robot applications which require the robot tool to move along a curve, keep a certain orientation and position relative to the curve, and avoid collision with the environment. 111 Table of Contents Abstract List of Figures vii List of Symbols x Acknowledgment 1 2 3 xiii Introduction 1 1.1 Motivation 1 1.2 Midpoint Algorithm: a Novel Approach to Motion Planning 4 1.3 Outline of Thesis Content 5 Literature Review 6 2.1 A Basic Motion Planning Problem: Collision Avoidance 6 2.2 Path Planning Methods Based on the Configuration Space Approach 8 2.3 Path Planning for Robot Tool Along a Continuous Curve 11 2.4 The Spline of Curve 13 2.5 Searching Path Through a Net 14 2.6 Conclusion Based on Literature Review 15 Configuration Space of Robot Tool 16 3.1 Configuration of Robot Tool 16 3.2 Configuration Space of Robot Tool 17 iv 4 Description of the Midpoint Algorithm 32 4.1 Overview of the Midpoint Algorithm 32 4.2 The Description of the Midpoint Algorithm 32 Reconstruct the Curve by Catmull-Rom spline 35 4.2.2 Point Check for the First Point 37 Point Check 37 Search the First Curve Point 40 4.2.3 Curvature Check 42 4.2.4 Overlap Check 48 4.2.5 Translation and Rotation Check 51 Configuration Space Search 51 The Swept Volumes Used in Translation and Rotation Check 57 4.2.6 Inverse Kinematics Check 64 4.2.7 Control the Path Planning Using A* Search Algorithm 67 4.2.8 Optimize Trajectory 69 Summary of the Midpoint Algorithm 72 Computer Implementation and Examples ‘75 4.3 5 4.2.1 5.1 Overview 75 5.2 The Simulation Program TRAJPLAN 76 5.3 Down Load the Swept Volume Into Memory 78 5.4 Plan the Trajectory as Desired 80 5.5 The Precision of Trajectory 82 5.6 The Specification of Optimal Tool Orientation 82 5.7 Examples: Automatic Welding and Fish Butchering Processing 83 5.7.1 Automatic Robot Welding Process V 83 5.7.2 6 Automatic Fish Butchering Process Conclusion and Future Work 86 91 6.1 Conclusion 91 6.2 Recommendations for Future Work 93 Appendices 95 A Data Structure 95 B TRAJPLAN 101 C Inverse Kinematic Modules for PUMA 560 and CRS A460 108 Bibliography 112 vi List of Figures 1.1 The robot welding workcell 2 1.2 The optimal orientation of robot tool 3 2.1 A Catmull-Rom spline 14 3.1 Robot tool 17 3.2 Definition of robot tool configuration 18 3.3 The c and 3 ranges of Entire C-space and the allowable orientation range at different curve points. (a) Entire C-space. (b) Allowable orientation range 19 3.4 Entire C-space and Local C-space 20 3.5 The shape of local 0 region 22 3.6 The calculation of local 0 region of Local C-space 23 3.7 Entire C-space and local 0 region 28 3.8 The local 0 region of Local C-space in the boundary of Entire C-space 4.1 The relation between step size and the condition of environment and curve 35 4.2 A Catmull-Rom spline 36 4.3 The definitions of cell, 0 region and -y range 37 4.4 The swept volumes used in the Point Check. (a) The 0 swept region and . 30 the -y swept region. (b) The 0 swept volume and the y swept volume. (c) The cross-section of 0 swept volume. (d) The cross-section of 7 swept volume 39 Vu 4.5 Swept volumes used in Point Check (a) The division of 0 swept volume (b) The division of 7 swept volume (c) The binary tree structure of 7 swept volume (d) The combination of a 0 swept volume and a 7 swept volume 41 4.6 Intermediate points selected from curve segment and line segment 43 4.7 Adding midpoints using Curvature Check 45 4.8 Deleting curve points using Curvature Check 46 4.9 The relationship between track and constraints: dmax and dmin 47 4.10 The intermediate number and the precision of Curvature Check 48 4.11 The search for overlapping region 49 4.12 The effect of adding midpoint 50 4.13 Adding midpoints in a discontinuous curve 51 4.14 The overlap of 0 regions which have small a angles 52 4.15 The path found though Pi-1, p and 52 . . Pi+1 4.16 The overlap between free-region_i, and overlap-region,i 54 4.17 The shrink step, shrink region and minimum shrink region 56 1 toward free-region:_i,j in order to find free 4.18 Shrinking overlap-region, regionj,ji . . . 4.19 The 0-trans swept volumes used in Translation and Rotation Check. 4.20 The 7-trans swept volume used in Translation and Rotation Check 4.21 The shrink swept volume used in Translation and Rotation Check . . . . . . 58 60 62 63 4.22 A series of coordinate frames 66 4.23 Different robot configurations 72 4.24 Flowchart for the Midpoint Algorithm 74 5.1 PathList data structure 77 viii 5.2 The procedure for searching for collision free swept volume (a) The 0 swept volume collides with environment, (b) Shrinking 0 swept volume for the collision free 0 swept volume, (c) The swept volume collides with environment, (d) Search -y swept volumes for the collision free 7 swept volume 79 5.3 The optimal orientation of robot tool 83 5.4 The automatic robot welding workcell 84 5.5 Procedure for searching for collision free paths 87 5.6 Procedure for searching for collision free paths for butchering 89 5.7 The automatic fish butchering workcell 90 C.1 The PUMA 560 robot and robot tool ix 109 List of Symbols a the angle rotating around y axis; 3 the angle rotating around z axis; the angle rotating around tool axis; 0 a the a angle of optimal orientation; /3 the 3 angle of optimal orientation; fo the y angle of optimal orientation; amin the minimum a angle in Local C-space; amax the maximum a angle in Local C-space; min the minimum 3 angle in Local C-space; 3 /‘ max the maximum /3 angle in Local C-space; 3 / x, y, z the coordination of a curve point. p the ith curve point. m the ith midpoint. dmin the minimum distance between robot tool tip and curve. dmax the maximum distance between robot tool tip and curve. List of Key Words Local C-space the local configuration space. Entire C-space the entire configuration space. local 0 region the region of Local C-space in a-/3 plane. local the range of Local C-space in 7 axis. range x o swept volume the tool swept volume corresponding to 0 region. swept volume the tool swept volume corresponding to 7 range. cell the space in Local C-space. free cell the cell associated with a collision free swept volume. o the projective region of cell on a region — /3 plane. 7 range the projective range of cell on path list the linked list containing all of the potential paths along the curve. minimum step size the predetermined minimum step size between successive curve points. track point the point on the track of robot tool tip. axis. intermediate curve point the intermediate point selected from the curve segment between successive curve points. intermediate track point the intermediate point selected from the track segment between successive track points. intermediate number the number of intermediate points. overlap-cell,+i the overlapping space of Local C-spaces at point pj and overlap-regioni the projection of overlap-cell, 1 on a-fl plane. free-cel4,+i the space inside overlap-cell,+i, associated with a collision free Pi+1. swept volume. free-region,i the projection of free-cel4, +i on a-fl plane. 1 free-range:,z+i the projection of free-cel4,+i on 7 axis. shrink step the shrink amount at each shrink. shrink region the region left in overlap-regionj,i after each shrink. 0-trans swept volume the tool head swept volume corresponding to overlap-region, . 1 7-trans swept volume the tool handle swept volume corresponding to overlap-region,j. intersection orientations the orientations corresponding to the intersection points of two 8 regions xi shrink swept volume the swept volume corresponding to shrink region. Path Cost the total cost of going through a path. TrajCost the total cost of going through a trajectory. xl’ Acknowledgment I would like to thank my supervisor, Dr. R. G. Gosine, for his guidance in developing the motion planning algorithm. This work has been undertaken with the support of the NSERC Chair in Fish Processing Automation. Discussion with B. Konesky, Dr. D. Cherchas, and Dr. F. Sassani have been of assistance in integrating this algorithm with AutoRoboWeld, an off-line programming system for welding robots. Thanks also to G. Wallace and M. Lookman of A & R Metals for useful feedback regarding motion planning requirements for welding applications. XIII Chapter 1 Introduction 1.1 Motivation When using a teach pendant to program robots for contour following applications such as welding, cutting or painting, the operator must manually manipulate a robot tool along a desired curve. It is generally agreed that this process is slow and demanding on the user, and an automatic process to aid in motion planning is necessary. A typical welding robot workcell is illustrated in Figure 1.1. Safe and smooth motion of the robot and the tool is expected, and good process quality is significant in such applications. Using a robot instead of a human arm to handle the tool presents a significant planning problem. An intelligent agent is required to plan and control the motion of the robot and the tool in order to achieve good process quality. The primary contribution of this thesis is the development of a planning strategy that considers the following requirements: 1. The tip of the robot tool must move along the curve and within a certain distance from the curve. 2. The optimal orientation, as specified by the process, is relative to the tangent of the curve and the normals of two surfaces at the each side of the curve as shown in Figure 1.2. This orientation v&ies with the changes to the curve ud the supporting surfaces. The robot tool has to maintain the optimal orientation within a given tolerance. 1 Chapter 1. Introduction 2 tool seam PUMA 560 robot obstacle Figure 1.1: The robot welding workcell Chapter 1. Introduction normal of surface 2 surface 1 3 optimal orientation tangent surface 2 Figure 1.2: The optimal orientation of robot tool 3. The robot tool must avoid collisions with the curve and the obstacles around the curve. 4. The position and orientation of robot tool result in a set of robot joint angles which do not violate the joint limits. 5. A significant change of robot configuration (i.e. arm inversion) must be avoided. 6. Smooth motion of the robot and tool is expected. In the above requirements, the tool tip must trace the curve within a given tolerance, and the tool must maintain an orientation close to the optimal one within a given toler ance. These tolerances are specified by the application in order to maintain acceptable process quality. Collision avoidance is a basic requirement for tool motion. Within the allowable and collision free orientation range, some tool orientations may lead to invalid robot inverse kinematics solution (the joint angles in the solution violate the joint limits). It is difficult for a human operator to select orientations which lead to valid solutions. The motion planner must maintain a path history in order to provide smooth motion of the tool. Changes of robot configuration will also reduce process quality because of Chapter 1. Introduction 4 discontinuous robot motion. The robot configuration, however, may have to change as the tool travels a curve. It is difficult for the human programmer to pre-judge such changes and prevent them by selecting alternate robot configuration at the beginning of process. It is impossible for the human programmer to consider all of these constraints simul taneously while using the teach pendant to program the robot. Programming the robot motion becomes difficult and the process quality is compromised. The subject of this research is to to plan the motion of the robot tool automatically and meet the above requirements. 1.2 Midpoint Algorithm: a Novel Approach to Motion Planning A more intelligent robot tool trajectory planner is required and this thesis proposes such an algorithm, the Midpoint Algorithm, to plan the trajectory of robot tool tracing a curve subject to process constraints. A safe and smooth robot tool trajectory is selected by the algorithm, which is based on the configuration space approach [Loza 83], to provide high process quality. Since a curve is represented by discrete points, the motion of the tool along the curve links the motion between successive curve points. By studying the configuration spaces at successive curve points and the relationship between the configuration spaces, the algorithm assesses the locations of obstacles in the vicinity of the curve and the curvature of the curve in work space, finds the collision free path and plans an optimal trajectory for the tool. If more than one paths exists, an A* search optimizes the path. A polyline approximating the curve is used as a reference for the tool tip and the algorithm varies the step size between points on the polyline according to the condition of the curve and the environment. A large step size is used to speed up the path planning if the curvature of the curve or the likelihood of collision is low, and Chapter 1. Introduction 5 a small step size is used for more intricate robot motion if the curvature of the curve or the likelihood of collision is high. In order to assure that the orientation of the robot tool is within the allowable orientation range as it moves along the curve, exploration of the space around the optimal orientation at each curve point is necessary. The configuration space represents the interaction of the robot and environment at each curve point. A path is found through the configuration space using an A* search for possible solutions. An off-line simulation program, TRAJPLAN, implements the algorithm in order to demonstrate the application potential of the work. Examples based on the PUMA 560 and CRS A460 robots have been developed and tested on an real robot. 1.3 Outline of Thesis Content The remainder of this thesis is arranged as follows. Chapter 2 reviews previous related work in the areas of robot motion planning based on the configuration space, path plan ning for robotic tools, curve representation, and search techniques. Chapter 3 details the basic concepts behind the Midpoint Algorithm, while Chapter 4 provides details of the Midpoint Algorithm. Chapter 5 describes the simulation program TRAJPLAN and its application to typical planning problems. Finally, Chapter 6 presents conclusions for this work and suggestions for future work. Chapter 2 Literature Review Two general path planning problems are involved in the motion planning of a robot tool tracing the specified curves: collision avoidance and the motion along the curve. 2.1 A Basic Motion Planning Problem: Collision Avoidance Robot motion planning has been extensively studied and good survey articles include [Cann 88] [Lato 91] [Shar 89]. One of the basic problems in robot motion planning is to find a collision free path for the robot. This problem is to search for a path from an initial position and orientation to a goal position and orientation of robot. The path specifies a continuous sequence of positions and orientations of the robot avoiding contact with the obstacles. If the obstacles are static and the locations of obstacles in the work space are avail able, a popular robot path planning method is the configuration space approach. This approach is initialized by Udupa [Udup 77] who proposed the motion planning amidst obstacles and described the idea of using an appropriate space and shrinking the robot to a point in the space. This idea is exploited by Lozano-Pérez and Wesley [Loza 79]. They proposed a path planning algorithm for polygonal and polyhedral robots and obstacles without rotation. Lozano-Pérez [Loza 81] extended the algorithm proposed in [Loza 79] and used the notion of configuration spa Ce. Some methods based on this approach, such as roadmap, cell decomposition and potential field, are discussed in Section 2.2. Dynamic motion planning [Cann 88] [Lato 91] [Reif 85] deals with moving obstacles 6 Chapter 2. Literature Review 7 in work space. A continuous function of time is used to specify the robot configuration at each instant. In effect, a dimension representing time is added to the conventional configuration space. In this configuration-time space, the robot is shrunk to a point moving among stationary obstacles. Since no object can move back in time, the path found is monatomic in time. Constraints on robot velocity and acceleration have to be considered. In the presence of multiple robots in the work space, one of the motion constraints is that the robots cannot occupy the same space at the same time. Centralized planning [Schw 83] and [Tour 86] generates the composite configuration space of the robots and plans the coordinated paths in the space. Some general path planning methods such as cell decomposition and potential field can be used to plan a path in the composite configu ration space. Another approach to this problem is the decoupled planning method which plans the motion of each robot independently of the other robots and then considers the interactions among the paths. There are two decoupled planning approaches: prioritized planning and path coordination. Prioritized planning [Buck 89] considers the motions of all robots, one robot at a time. The path of a robot is generated in a configuration-time space of the robot at each iteration, avoiding the collisions with both the obstacles and the other robots. The motion of the robot is planned as if the robot was moving among stationary obstacles and some moving obstacles (other robots). To arrange the order of robots to be planned, a priority is assigned to each robot. Buckley [Buck 89] assigned priorities to the robots which move in straight line from their initial configurations to their goal configurations. Path coordination [Odon 89] is applicable when the planning problem involves only two robots. A free path for each of the two robots is generated independently of the other robot. The two free paths are coordinated in order to avoid the collision between the robots. Another general approach to solving path planning is to search the free space directly Chapter 2. Literature Review 8 without first transforming the problem to configuration space. Kambhampati and Davis [Kamb 86] proposed a path planning approach based on a Quadtree representation. They described the hierarchical path-searching methods which make use of multiresolution rep resentation to speed up the path planning process considerably. Canny and Lin [Cann 90] presented a planning algorithm which traces out curves of maximal clearance from obsta cles and creates a one-dimensional roadmap of the free space of a robot. Their method takes advantage of the fact that the closest features (vertex, edge, or face) change only infrequently as the objects move along finely discreted paths. Gupta [Gupt 90] described a method of planning the motion of each link of a manipulator successively, initializing from the base link. The n-D problem for an n-link manipulator arm transforms to one 1-D and n — 1 2-D planning problems. This 2-D motion planning problem is to plan the motion of a single link which has one end moving along a fixed path determined by the motion of the previous links. An interesting collision avoidance algorithm is reported by Shaffer and Herb [Shaf 92]. They described a data structure and data structure update algorithm for a real-time collision avoidance safety system. The N-objects octree is used to index a collection of 3-D primitive solids which make up the robots and obstacles in work space. As robots move, the octree is updated and the octree nodes recursively decompose 3-D space into eight equal cubic octants until each octant contains less than a predetermined number N of primitives. Since a given primitive does not change which octree nodes it is in during most update cycles, the octree is rarely modified. The system provides the information about possible collision. 2.2 Path Planning Methods Based on the Configuration Space Approach The configuration space method [Udup 77] [Loza 79] [Loza 81] [Loza 83] [Loza 87] shrinks the robot as a point in robot configuration space and maps the obstacles in this space. Chapter 2, Literature Review 9 The motion planning of a point instead of a dimensioned object is more explicit. The configuration space represents the robot and obstacles in a coordinate system defined by the joints of the robot. There exists a large number of methods of solving the motion planning problem based on the concept of configuration space. Three general approaches are roadmap, cell decomposition, and potential field [Lato 91]. The roadmap approach represents the free space in the form of a network of onedimensional curves, called roadmap, which is used as a set of standardized paths. Path planning becomes a search for three path segments: the path segment connecting the initial configuration to the roadmap, the path segment contained in the roadmap, and the path segment connecting the roadmap to the goal configuration. The visibility graph method, Voronoi diagram method and freeway method are all based on this idea. The visibility graph method [Nils 69] is suitable to a two-dimensional configuration space with polygon configuration obstacles. The nodes of the graph are the initial and goal config urations and the vertices of the configuration obstacles. Two nodes are connected by a straight line segment if the line does not intersect the interior of the configuration ob stacles. The path is found by searching for the shortest path in the graph. The Voronoi diagram method [Taka 89] generates the Voronoi diagram which is in the free regions of configuration space. The Voronoi diagram is the locus of points which are equidistant from two or more obstacle boundaries including the work space boundary. The advan tage of this diagram is that it produces free paths which tend to maximize the clearance between the robot and the obstacles. The freeway method [Broo 83(a)] plans the path for a polygonal object (robot) translating and rotating in a two-dimensional space. A is straight 1ineor gicriied cylinder. The straight a.is of the cylinder called the spine, is the path of the reference point of the robot. A free space is represented by a freeway net. In a freeway net, if two spines intersect, the robot can transfer from one free way to the other if the ranges of free orientations of the robot along both splines have a Chapter 2. Literature Review 10 non-empty intersection at the intersection point. The freeway net is a representation of the possible motion of a robot along spines and between spines. The cell decomposition method decomposes the robot free space into some simple regions, called cells. A graph representing the adjacency relation between the cells is constructed by extracting the cells from the free space and connecting two graph nodes if these cells are adjacent. The outcome of the graph search is a sequence of cells and a path is extracted from this sequence. There are two approaches in this area: exact cell decomposition and approximate cell decomposition. The exact cell decomposition method Chaz 87] decomposes the free space into trapezoidal and triangular cells, represents the adjacency relation between the cells by constructing the connectivity graph, and searches for a path in this graph. The cells used in the exact cell decomposition method are required to have a simple pre-specified shape, e.g. a rectangle. Such cells do not provide an exact representation of free space. The approximate cell decomposition method [Loza 81] [Broo 83(b)] uses Quadtree decomposition to divide a cell into four subcells. A cell is recursively decomposed until a predetermined resolution is attained, or the cell lies completely in free space or in the configuration obstacle. The path is searched through the cells lying in free space. Since the shape of a cell is relatively insensitive to numerically approximate computations, this method is usually much easier to implement than the exact cell decomposition method. In the potential field method [Khat 86], the robot is represented as a particle in configuration space. The goal configuration generates an “attractive potential” which pulls the robot toward the goal, and the configuration obstacles produce a “repulsive potential” which pushes the robot away from them. Robot moves under the influence of an artificial potential produced by the goal configuration and the configuration obstacles. Based on the configuration space approach, most studies about collision avoidance focus on the trajectory of manipulators [Sing 91] [Warr 89] aild mobile robots [Kamb 86] Chapter 2. Literature Review 11 [Taka 89] in point-to-point motion problems. There is little reported, however, on the problem of planning the robot tool motion along a 3-D curve subject to physical (obstacle) and process constraints. 2.3 Path Planning for Robot Tool Along a Continuous Curve The principles and techniques of a high-speed spatial seam tracking system are described in [Boll 71]. A five degree of freedom seam tracer is developed, which positions a welding gun to maintain a fixed relationship to a weld line. The proper angular relationships are achieved by utilizing velocity information from three tachometers mounted on the three rectangular coordinate drives. Collision avoidance is not considered in the system, and the seam discussed in the paper is constrained to lie on a cylindrical surface. Tomizuka, Dornfeld and Purcell [Tomi 80] discussed the characteristics of the gas metal arc welding process and the relationship between welding parameters, the desired output of the welding process, and the automation of the process. A strategy for two-axis welding torch positioning and velocity control is developed. The torch positioning and velocity control is achieved by keeping the relative position error between the seam and torch small and maintaining uniform tracking speed. In this approach, the algorithm constrains the seam to a plane and only two-axis control is available. Collision avoidance is not considered. An algorithm for tracing a seam in real time is developed in [Khos 85]. The seam tracking control traces a curve in 3-D space and maintains proper tool orientation with respect to the surface that contains the curve. The curve lies on a surface and is dis cretized lengthwise. The discretization provides a piece-wise linear approximation of the curve between two adjacent sample points. The tool orientation is obtained by discretiz ing the surface in the vicinity of the two sample points, and this is held constant when Chapter 2. Literature Review 12 the tool travels between the two points. The position of the torch is specified by the coordinates of the sample points. Three unit vectors are used to describe the tool orien tation relative to the base frame but the rotation about tool axis is not discussed. This may be suitable for the CYRO robot, a special robot with three revolute joints and three prismatic joints, but is not suitable for robots like the PUMA, CRS or GMF with which the rotation about the tool axis changes the position of the robot end-effector if the tool handle is held by the robot. The allowable tool orientation range and the optimal tool orientation subject to process constraints are not discussed in the paper. The sample points are used directly in seam tracking regardless of the condition of the curve and the environment around the points. This reduces the accuracy along complex curve segments and wastes time along simple curve segments. A trajectory planner and a method for interference detection between objects in a robot welding workstation are presented in [Buch 89]. The trajectory planner searches for a kinetically feasible, interference-free robot trajectory in order to follow a specified weld path in 3-D space. The algorithm provides rapid interference detection between convex polyhedras using a steepest descent graph search. If desired, exact separating distances and directions can be found. Interference-free trajectories that do not violate kinematics constraints are found by searching possible inverse kinematic solutions for the desired welding torch trajectory. This approach only deals with tool rotation about its axis, but not the allowable tool orientation range. This paper and the associated work form the starting point for the application of the work described in this thesis. Angeles, Rojas and Lopez-Cajun [Ange 88] discussed the angle velocity and angle acceleration of the end effector which holds a robot tool and travels along a continuous path. The technique proposed allows the trajectory-planning engineer to specify the orientation of the end effector via the orientation of the path at a given point. The latter is defined as that of the orthonormal triad of vectors associated with the curve, namely, Chapter 2. Literature Review 13 its tangent, normal, and binomial vectors. The angular acceleration of the end effector requires the computation of the derivatives of the curvature and torsion with respect to the arc length. Thus the coordinate frame defined by the triad of unit tangent, normal, and binormal vector defines not only the orientation, but also the angular velocity and acceleration of a body tracking the curve with a given speed, whose relative orientation with respect to the curve remains constant, or a given function of time. 2.4 The Spline of Curve Curve representation is an important aspect of this research, and many algorithms for curve splines have been reported. A good survey of the algorithms is found in [Fole 91]. Cubic polynomials are most often used since lower-degree polynomials give too little flexibility in controlling the shape of the curve, and higher-degree polynomials can in troduce unwanted wiggles and also require more computation. The spline is a piece-wise continuous curve made up of cubic polynomials. The famous B-splines method [Bars 80] [Bart 87] consists of curve segments whose polynomial coefficients depend on just a few control points and movement of a control point affects only a small part of a curve. This is called local control. B-splines, however, do not interpolate the control vertices. In this research, a curve representation based on some sample points must be reconstructed and the reconstructed curve has to pass through the sample points. The B-spline is not suitable in this case. One member of the Catmull-Rom Spline [Catm 74] [Barr 89] family interpolates the points P 1 to Pm1 from the sequence of points Po to Fm. In addition, the tangent vector 1 and F+i, as shown in Figure at point P is parallel to the line connecting point P_ 2.1. The Catmull-Rom Spline interpolates control points, shares many properties such as global smoothness and local control with B-spline curves, and has continuity of the Chapter 2. Literature Review 1 P 14 0 p 5 p 2 p . 9 P 4 p 6 p Figure 2.1: A Catmull-Rom spline first derivative, which is required by the proposed algorithm. Given a series of points, the Catmull-Rom Spline method generates a curve smoothly to pass through them. The Catmull-Rom Spline method is suitable in this research. 2.5 Searching Path Through a Net A further requirement of the research is an algorithm for evaluating potential tool paths and for guiding the search through path options. Procedures for finding the shortest path through a net are surveyed in [Wins 84]. The British Museum Procedure finds all possible paths and select the best from them. Since this method looks everywhere and is inefficient, it is not suitable for our path planning where search time is of concern. The Branch-and-Bound search extends the search tree in the direction of the “best” partial path. The A* search improves the Branch-and-Bound search by including an estimate of remaining distance, combined with the dynamic-programming principle. If the estimate of remaining distance is a lower bound on the actual distance, then the A* search achieves the optimal solution. The A* search is used in the proposed algorithm to search multiple paths along the curve and to select the optimal trajectory from the tool configurations in the path. Chapter 2. 2.6 Literature Review 15 Conclusion Based on Literature Review In view of this literature survey, it is concluded that little work has been done on the problem of robot tool planning along a desired trajectory of the tip of the tool. Further more, conventional planning techniques have been largely concerned with point-to-point problems and did not consider application constraints associated with the required mo tion of the tool. The configuration space method appears promising in terms of a general approach, however, the proposed method studies the relationship between configuration spaces rather than searching for paths inside a single configuration space. For the ancil lary functions, such as for effective curve representation and search procedures, existing techniques will be employed directly in this thesis. Chapter 3 Configuration Space of Robot Tool 3.1 Configuration of Robot Tool In this section the representation of the position and orientation of the robot tool is discussed. The foundation for this representation is a 3-D coordinate system, and Figure 3.1 illustrates the tool representation, in this case for a welding torch, at the origin of the 3-D coordinate system. Note that all objects in the environment are described with respect to this 3-D reference frame. As illustrated in Figure 3.2, the tool has six degrees of freedom, x, y, z, a, /3 and y, where x, y and z are the position coordinates of the tool tip, a is the angle of rotation about the Y axis, /3 is the angle of rotation about the Z axis, and ‘y is the angle of rotation about the tool axis. The tool axis passes though the tool tip and represents the critical orientation of the tool, as shown in Figure 3.1. The configuration of the tool in the base coordinate system is determined by rotating the tool to an orientation (a,/3,7) and translating the tool from the origin to (x,y,z) while maintaining this orientation. As mentioned in Chapter 1, the six requirements as the tool travels a curve include (1) the distance between the tool tip and the curve, (2) the optimal orientation, (3) collision avoidance, (4) robot joint constraint, (5) robot arm inversion avoidance and (6) smooth motion. The distance between the tool tip and the curve point can be adjusted easily by changing the tool tip position (x,y,z). The tool orientation requirement specifies the tool orientation in the direction of the optimal orientation. The critical tool orientation is determined by the a and /3 angles. The initial position of tool 16 Chapter 3. Configuration Space of Robot Tool 17 z tool axis tool head V Figure 3.1: Robot tool in Figure 3.2 (a) shows the tool orientation where a, and y are all equal to 00. The reference orientation of the 7 angle is along the negative Y axis. Although it does not constitute a tool orientation constraint, the ‘y angle is very important in the calculation of the robot inverse kinematic solutions since a small change in 7 can result in a large difference of the position and orientation of the robot end effector and the associated joint values for the robot as illustrated in Figure 3.2 (b). As discussed in Chapter 4, the angle is critical in the collision-free path search. Configuration Space of Robot Tool 3.2 An alternative representation of tool orientation is the configuration space (C-space), and here two new concepts, Entire C-space and Local C-space, are introduced. The Entire C-space is the configuration space with its origin at a, /3 and the a, /3 and 7 angles in equal to 00. The ranges of Entire C-space are [0°,180°j, [0°,360°] and [0°,360°] respectively. The Entire C-space represents the complete set of tool orientations at any curve point in the work space. The a and 3 ranges of Entire C-space correspond to the sphere in Chapter 3. Configuration Space of Robot Tool 18 z robot tool Initial position V (a) z robot tool position 2 robot tool position 1 V 4— robot end effector (x,y,z) (b) Figure 3.2: Definition of robot tool configuration Chapter 3. Configuration Space of Robot Tool opilmal orientation 19 z Enlire C-space range z y y x point I+1 Curve point I (a) z allowed orientation range y Curve Curve point 1+1 V x Curve point i (b) Figure 3.3: The a and /3 ranges of Entire C-space and the allowable orientation range at different curve points. (a) Entire C-space. (b) Allowable orientation range. Chapter 3. Configuration Space of Robot Tool 13 20 Entire Cspace 360 Local local 0 region 180 Local Cspace at point 1 Y 360 Figure 3.4: Entire C-space and Local C-space work space as shown in Figure 3.3 (a). Using the Entire C-space directly in trajectory planning is not practical since a high resolution Entire C-space would require a large data structure, which would waste time and space as a result of creating, searching and storing much useless information in the data structure. Due to process constraints, such as allowable deviation in tool orientation from a user-specified ideal orientation, only part of the Entire C-space is required. To address the problem of the large Entire C-space, the concept of Local C-space is introduced. At each curve point the optimal tool orientation depends on the process. For example, the optimal welding torch orientation may be selected by an expert welder. Similarly there will be optimal cutter orientations for an automatic butching operation. The ac ceptable orientations covers only a subset of the Entire C-space, as illustrated in Figure 3.3 (b), and only the configurations within this subset need to be considered. The and /3 ranges of the Local C-space represent the allowable deviation from the optimal orien tation for a particular application. Since the acceptable range of deviation is generally Chapter 3. Configuration Space of Robot Tool 21 much smaller than the range in Entire C-space, the Local C-space requires a condensed data structure in order to represent the configuration space with the same resolution as the Entire C-space. With reference to Figure 3.4, the Entire C-spaces at different curve points have the same a and 3 ranges, i.e. [0,180] and [0,360] respectively. The a and /9 ranges of Local C-space are different and correspond to the different regions in the a projections of Local C-space on the a and the local — /3 plane and the — 3 plane. The axis are called the local 0 region range respectively. In order to visualize the approximate shape of the local 0 region, the bounding box of the region is studied. In Figure 3.5 (a), the maximum allowable deviation of tool orientation from the optimal orientation is 00 and the optimal orientation is 0 (a /3). , The amin, amax, min 3 / and maa, 3 / angles in Figure 3.5 (b) (c) (d) are the minimum and maximum a and /3 angles in the local 0 region respectively. In Figure 3.5 (a), points 1, , 0 2, 3 and 4 are at the boundary of the cone centered at (a Figure 3.5 (b) illustrates the viewing direction in the negative optimal orientation. The line connecting point 2 to point 4 is perpendicular to the line connecting point 1 to point 3. Figure 3.5 (c) and (d) illustrate that mim 3 / and /3max are not the /3 angles at point 2 and point 4, while amin and amar correspond to the a angles at point 1 and point 3 respectively. Note that amjn and amax are equal to a 0 — 0 and a 0 + 00 respectively. The a angles at point 2 and point 4 are equal but they are not equal to a . The corresponding local 0 region has an irregular 0 shape as shown in Figure 3.5 (e). From the above discussion, it is clear that the local 0 region is hard to represent using a simple geometric model, such as a rectangle, circle or ellipse. With reference to Figure 3.6, equations defining the local 0 region are derived. In the figure, the sphere represents an Entire C-space, which has origin or and radius p. The cone represents the local 0 region of Local C-space. The intersection of optimal orientation and the sphere is Chapter 3. Configuration Space of Robot Tool The circle contains the same a angle 22 oplimal orientation 4 2 y x 3 viewing direction in the negalive oplimal orientation (b) (a) mox mm mm viewing direction In the negative z axis Cc) viewing direction perpendicular to z-o-13o plane amln The a and (d) Figure 3.5: The shape of local 0 region 0 a a 2 amox a range of Local C-space (e) Chapter 3. Configuration Space of Robot Tool 23 optimal orientation Local C-space range V x Figure 3.6: The calculation of local 0 region of Local C-space denoted as o, p is a random point within the Local C-space and located on the sphere, and 0 is the angle between r 0 and orp, where 0 0. The projections of o and p 0 into X-Y plane are denoted as o’ and p’ respectively, while r’ is the line connecting o’ and p’, and pq is a line parallel to p’o’. Given the optimal orientation (cv , ,6) and the 0 maximum allowable orientation deviation 00, the calculation of the corresponding local 0 region is now described. Using the cosine law in triangle OrOP 2 2p — cos0 2 2p = 2 r (3.1) Using the cosine law in triangle oro’p’ 2a + p 2a 0 2 sin p 2 sin — sinasina 2 2p c 0 os(/3 = — 2 r’ (3.2) Since pq is parallel to p’o’ and oo’ is perpendicular to p’o’, the triangle opq is a right triangle, and 2 + (pcosa r’ 0 — 2 pcosa) = 2 r (3.3) Chapter 3. Configuration Space of Robot Tool 24 Combining Equations (3.1), (3.2) and (3.3), 2 2p — cosO 2 2p = 2a 2a + p 0 2 sin p 2 sin — sinasina 2 2p c 0 os(/3 — i3) + (pcosa 0 — 2 (3.4) pcoscx) which may be reduced to cosO = cos(a + a ) + [cos(/3 0 — 0 /3) + ljsinasina (3.5) by the following steps, from Equation (3.4), 2 2 — — 2a 2cosO = sin 2 a + sin 0 2cosO 2 a + sin sin 2a 0 2 — 2cosO = — 2 — cos(/3 0 2sinasina cos(/3 0 2sincrsina — cos(/3 0 2sinasincr cosO = sinasina cos(/3 0 cos(O) — = sinasina co.s(f3 0 — — — 0 /3c,) + (cosa 0 a 2 /3) + cos — i3) — 0 cosa + cos 0 2cosa a 2 i3) + cosa,cosa 0 3) + 0 cosa c osa + sinasina = cos(a + at,) + [cos(/3 — 2 cosa) cosa 0 2cosa = —sinasina 0 + coscr cosa + sinasina 0 (cos(i3 0 Since 0 — — — — 0 sinasina /3) + 1) /3,) + 1]sincrsina, 0, 1 cosO 0 cos0 and from Equation (3.5), we can write, 1 1 — cos(a + a ) + [cos(3 0 cos(a + a ) 0 [cos(/3 — — i3) + llsinasina 0 ) 0 0 > cos(0 f3) + ljsinasincx Next, consider the left hand side of Equation (3.6). Since 1 2 cos(/3 — /3,) + 1 ) 0 cos(0 — cos(a + a ) 0 cos(/3 — (3.6) Chapter 3. Configuration Space of Robot Tool 25 Since both c and cr are in the range [0, 180], and 0 sinasincr 0 we can write, 0 2sincsinc Since 1 cos(c [cos(/3 — j9) + 1]sincvsincx (3.7) c), we know — ) 0 1—cos(a—o 0 and 1 — 0 + sinasina (coscco.sa ) + 2sinasino 0 0 2sinasino or 1 — cos(c + cr ) 0 2sinasinac, (3.8) Combine Equation (3.7) and Equation (3.8) 1 — cos(a + c,) 0 2sinasinc [cos(f3 — 3) + 1]sincsinc 0 The left hand side of Equation (3.6) is always satisfied. Now, consider the right hand side of Equation (3.6). 0 are in the range of [0, 180], Since both a and a 0 sinusina 0 and [cos(3 — 0 3) + 1]sinasirta 0 First consider the case when ) 0 cos(0 — cos(a + a ) 0 0 (3.9) Chapter 3. Configuration Space of Robot Tool 26 We can write, [cos(/3 — 0 /3) + 1]sinc.sina ) 0 cos(0 — cos(a + a ) 0 The right hand side of Equation (3.6) is always satisfied, and fF3 can be any value in the range [0, 360]. The condition )—cos(a+a cos(0 ) 0 0 requires 0> a+a or a 00— Since a is defined as positive, and if a 0 0 a Oo 0 and a is in the range [0°, a 00 — ], 0 a /3 can be any value in range [0°, 360°]. Next consider the case when ) 0 cos(0 — cos(a + a ) 0 > 0 We get <a+a 0 Since a is defined as positive, from Equation (3.10), —a <0 0 0 a > 00 (3.10) Chapter 3. Configuration Space of Robot Tool With reference to Figure (3.5) 27 (a), amin Since both a 0 and a are greater than = 00, 0 a — 90> 0 from the right hand side of Equation (3.6) we write, [cos(/3 j3) + 1]sinasina 0 — ) 0 cos(0 — cos(a + a ) 0 00, Since both a 0 and a are greater than 0 .sinasina 0 > cos(9 cos(a + a ) 0 ) 0 1 0 sinasina 0 cosa cosO cosa 0 cos(f3—i3) 0 sznaszna 0 cosa cosO cosa 0 Ii /I <arccos( sznaszna ) 0 cos(/3 — /3) — > — — — — . — (3.11) where cos9 co.sa — 0 cosa 0 0 .sznaszna (3.12) Equation (3.12) can be rewritten as, 0 —sina.sina cosa 0 cosa — 0 cos9 0 .sinasina — cosa 0 cosa 0 cosO < 0 sinasina cosa + sinasina 0 coscr 0 0 <cos(a co.s(a + a ) <cosO 0 — ) 0 a (3.13) Since angle 9 is the maximum a deviation from a , 0 Oo a — 1 0 a and the right hand side of Equation (3.13) is always satisfied. The left hand side of Equation (3.13) requires 0 a+a Oo Chapter 3. Configuration Space of Robot Tool 28 x 03 cx 1304 P01,01.03 P05 viewing direction in negalive y axis viewing direction in negative z axis (a) (C) (b) 360 Entire Cspace Pr P05 P2’ 133’ Po1,01,o3 133 1304 132 131 —-7 Local cJJfJT +\J 0 0 oe 01 a optimal orientation - 03 02 a 180 03 a region a 05 a (d) Figure 3.7: Entire C-space and local 0 region Chapter 3. Configuration Space of Robot Tool 29 which is satisfied by the condition (3.10), and condition (3.12) is always true. Summarizing this discussion, 1. If a, > 00, Equation (3.11) represents the relationships between a, /3, a,, and O. Each a angle is associated with two /3 angles, which correspond to two /3 boundaries corresponding to the a angle on the local 0 region. The local 0 region is obtained by calculate the /3 range for every a value as a increases from (a,, O) with step size 2. If a 0 - 0 + O) to (a 10. 0, the orientation where a equals 00 is included in the local 0 region. -0 cr ] , the 3 is the full range [0, 360]. (a) If a is in the range [0, 0 (b) If a is in the range 0 [ao-0 ] , +0 0 a , the /3 range is calculated with Equation (3.11). , 0 In Equation (3.11), the 3 range of the local 0 region depends on a /30 and 0. The geometric representation of a local 0 region is given in Figure 3.7. Figure 3.7 (a) illustrates the local 0 regions for different a 0 and angle sequence is: /34 < /3o2, /30 angles. Figure 3.7 (b) illustrates that the /30 j3 </ ,s, and different /9 angles correspond to different 3 /3 range in local 0 region. Figure 3.7 (c) illustrates that the a 0 angle is in the order of: 01 <a a 02 <a 3 0 < 0 <a a . The a range is 20 and does not vary with a 05 0 for different 0 to 90°, the smaller the local 0 regions. With reference to Figure 3.7 (d), the closer the a /3 range. A smaller a 0 angle, however, does not change the a range. Different a 0 angles, therefore, produce different position, size and shape of local 0 region, while different angles only change the position of the local 0 regions. If a local 0 region crosses the boundary of the Entire C-space region, the local 0 region has different shape from those discussed above. In Figure 3.8 (a) cone a is divided by the plane where 3 is equal to 00, and the corresponding 0 region is divided into two parts Chapter 3. Configuration Space of Robot Tool contains a angle “( 30 13 360 equal to 0 position Entire Cspace a y Ibo x 0 90 0 (a) 180 (b) f3 360 Entire Cspace Ibo 0 0 OoO 180 (C) 0 a 0 180 (d) Figure 3.8: The local 0 region of Local C-space in the boundary of Entire C-space Chapter 3. Configuration Space of Robot Tool 31 denoted by region a in Figure 3.8 (b). Cone b includes the orientation with both a and 3 equal to 00. Tn this case, a 0 is less than 00 as discussed above. With reference to Figure 3.8 (c), The corresponding local 0 region consists of two parts: (1) the a range is -0 and 3 range is [0,360], and (2) the a range is 0 0 a -0 ] [a , [0, ] +0 and the /3 range is 0 a calculated with Equation (3.11). The special case of a 0 equal to 00 is illustrated in cone c, and the corresponding local 0 region is rectangular, where a is in the range [0, 0] and /3 is in the range [0, 360], as illustrated in Figure 3.8 (d). Since the and /3 are at 00 00 position of the -y angle is defined at the negative Y axis when both a position, refer to Section 3.1, the 00 position of 7 angle always represents the tangent of a circle on the sphere. The circle is determined by the a angle, as shown in Figure 3.8 (a). Chapter 4 Description of the Midpoint Algorithm 4.1 Overview of the Midpoint Algorithm The trajectory for a robot tool tracing a pre-defined curve is planned in three stages. First, an extensive search along path segments between successive points is carried out. Each path segment contains two parts: the track of the tool tip and the acceptable orien tation range of the tool head. Robot constraints (joint limits), motion constraints (simul taneous translation and rotation which provides for smooth motion), collision avoidance, and the allowable distance between the tool tip and the curve are also considered within this search. Second, the path segments between successive points are linked together using an A* search to generate a continuous path along the curve. Third, tool con figurations along the path are selected using a second A* search in order to build the optimal trajectory which avoids robot configuration changes. Acceptable process quality is maintained along this trajectory. 4.2 The Description of the Midpoint Algorithm Given a curve with sufficient curve points to represent the curve shape, the trajectory is planned as follows, STEP 1.0 Reconstruct the curve using a Catmull-Rom spline in order to get curve point information at any curve position. Details of this step are presented in Section 4.2.1. 32 Chapter 4. Description of the Midpoint Algorithm 33 STEP 2.0 Use the Point Check procedure to identify collision free regions at the first curve point. Store the free regions in a cost-ordered linked list (called path list), and use each free region as the beginning of a potential path along the curve. Details of this step are discussed in Section 4.2.2, and the cost function is described in Section 4.2.7. STEP 3.0 Extend the lowest cost path in the path list along the curve by exploring the path segment between successive points in the following steps, STEP 3.1 Curvature Check: Check if successive points are close enough to repre sent the curve segment. Details of this step are discussed in Section 4.2.3. STEP 3.2 Overlap Check: Check the overlap between the Local C-spaces at suc cessive points. This ensures that the trajectory remains within the allowable deviation in tool orientation. Details of this step are discussed in Section 4.2.4. If this check fails, add a midpoint between the points and return to STEP 3.1. STEP 3.3 Translation and Rotation Check: Ensure that the tool can translate and rotate along the path segment without collision. The details of this step are discussed in Section 4.2.5. If this check fails and the step size between current two points is larger than the minimum step size , add a midpoint and 1 return to STEP 3.1. Otherwise, the failure in finding a path is reported to user and trajectory planning stops. STEP 3.4 Inverse Kinematics Check: Check the validity 2 of robot inverse kine matic solutions corresponding to the robot tool configurations at the point. The details of this step are discussed in Section 4.2.6. If this check fails, delete the current path from the path list. If the path list is empty, the trajectory The minimum step size is predetermined in order to avoid the infinite exploration of a path segment. 1 2 valid inverse kinematic solution does not violate robot joint limits. A Chapter 4. Description of the Midpoint Algorithm 34 planning stops. Otherwise, resume search at STEP 3.0. STEP 3.5 Repeat STEP 3.0 until the final curve point is included in a path in the path list or no possible path remains in the path list. STEP 4.0 The resulting path contains different a, /3 and -y ranges between successive curve points. Each combination of a, 3 and -y is the tool configuration that may be included in the final trajectory. An A* search is used to select the configurations in the path in order to obtain the optimal trajectory along the curve. The details of this step are discussed in Section 4.2.8. A fundamental component of this algorithm is the addition of midpoints between successive curve points as the search progresses. The reason for adding midpoints is that if successive points are close enough, the straight line motion of the tool tip between these two points will match the curve segment within a given tolerance. In this case, the optimal orientations at the points will be “close”, and some tool configurations at successive points will share collision-free space. As a result, there will be translation and rotation collision-free path segments between successive points. The concept of “close” depends on the curvature of the curve and environment in the vicinity of the points. The more complex the curve or the environment, the closer the successive points. For example, in Figure 4.1 (a), the curve, the optimal orientation and the environment do not change significantly, and the points pi and Pi+i are considered as close enough for the process. In Figure 4.1 (b), the environment and the optimal orientations do not change significantly but the curve changes sharply. In Figure 4.1 (c), the environment does not change but the optimal orientations and the curve changes sharply between points pj and In Figure 4.1(d), the curve and the optimal orientations do not change much but the environment changes sharply. In case (b), (c) and (d), the two curve points p. and are not close enough for the motion planning process. A smaller step size is required Chapter 4. Description of the Midpoint Algorithm 35 optimal orientation P curve 1 pI+ P+i (b) (a) Cc) 1 pi+ pi (d) Figure 4.1: The relation between step size and the condition of environment and curve to explore the path in Figure 4.1 (b), (c) and (d). A larger step size, however, tends to speed up motion planning in Figure 4.1 (a). The insertion of midpoints between p and Pi+1 controls the step size. A midpoint is added in the way that the location of the midpoint is on the curve midway between two curve points. The optimal orientation at the midpoint is determined as described in Section 1.1 for determining the optimal orientation at the original curve points. 4.2.1 Reconstruct the Curve by Catmull-Rom spline A Catmull-Rom spline [Catm 74] is used to reconstruct a curve based on the sample points which lie on the desired curve. The sample points are used as the control vertices, and the Catmull-Rom splines interpolate the control vertices. These splines share many properties with B-splines, such as global smoothness and local control. One member of Chapter 4. Description of the Midpoint Algorithm 36 0 p 5 p 2 p 7 p .p 9 4 p 6 p Figure 4.2: A Catmull-Rom spline 1 to Fm Catmull-Rom spline family is able to interpolate the points P 1 from the sequence of points Po to Pm. The tangent vector at point F: is parallel to the line connecting point 1 and P:+i, as shown in Figure 4.2. P_ Catmull-Rom spline uses parametric representation of curves. Here a curve is ap proximated by a piece-wise polynomial curve. Each segment Q(t) of the overall curve is given by three functions, x parameter t, (0 <t < y = 1). Given points y(t), z = j, Pi+1 t is the ratio of the distance between pj and z(t), which are cubic polynomials in the and new point Pnew between pj and Pi+i, Pnew and the distance between p and Pi+1• Designating MCR as the Catmull-Rom basis matrix, G 3 as the geometry matrix, and T as the matrix of parameter t. With T t’l], the representation is 2 t 3 [t —1 3 Q(t) T MCR• G 3 = . 11 [t3t2tll j 1 3 Pi- 4 —1 2 Pi- —1 0 1 0 Pi-’ 02 0 0 Pi 2 —5 —3 (4.1) , P_ 3 1 and F: can be any of x, y, or z coordinates of the point Where Pj , P_ 2 Pi—3,Pi—2,Pi—1 curve. and p, and Q(t) is the corresponding function for x, y or z along the Chapter 4. Description of the Midpoint Algorithm 360 37 Entire Cspace cell a I range Local Cspace 1 360 local 0 region 1 range 0 region Figure 4.3: The definitions of cell, 0 region and range Using a Catmull-Rom Spline, the first and last points are not included in the curve. The problem can be solved as follows, 1. If the curve is closed, the last and first points are used again as the first and last points. 2. If the curve is open, the first and last points are used twice. 4.2.2 Point Check for the First Point Point Check The goal of the Point Check is to explore the collision-free space around a curve point. Converting the problem into the configuration space, this check becomes to search for free cells in the Local C-space. With reference to Figure 4.3, a cell is defined as the space in Local C-space which projects as the 0 region on the c — /3 plane. The cell also projects Chapter 4. Description of the Midpoint Algorithm as 7 range on the 38 axis. If the corresponding swept volume of a cell is placed at the curve point corresponding to the Local C-space, and it does not collide with the environment, the cell is called a free cell. The swept volumes of the robot tool are the bridges between the configuration space and the work space. Each curve point is associated with a Local C-space and each cell in the Local C-space is associated with a swept volume of the robot tool. The shape of the swept volume is defined by the o, /3 and ranges of the cell. Although the Local C-spaces with different c and j3 angles have different shapes and sizes as discussed in Chapter 3, the shapes of the corresponding swept volumes are the same. As a result, only one set of swept volumes and its subdivision parts are required, and this feature makes the Midpoint Algorithm practical for implementation on existing PC hardware. Two kinds of swept volumes are used in the check: the 9 swept volume and the swept volume which correspond to the 9 region and the 7 range respectively, i.e. the 8 swept volume and the swept volume represent the space occupied by the tool head and the tool handle respectively. In Figure 4.4 (a), placing the tool with the critical tool orientation parallel to the Z axis and rotating the tool ±9 angle (0 < 9 < 8) about X axis generate the region swept by the tool. The regions swept by the tool head and the tool handle are called the 9 swept region and the 7 swept region respectively. The 0 swept volume and the 7 swept volume are generated by rotating the 0 swept region and the 7 swept region 1800 about Z axis as shown in Figure 4.4 (b). Figure 4.4 (c) and (d) provide cross-sectional views of the 0 swept volume and the 7 swept volume. A set of 0 swept volumes with different 0 angles are generated as shown in Figure 4.5 (a). For each 0 swept volume, there is a set of corresponding 7 swept volumes associated with. With reference to Figure 4.5 (b), the set of 7 swept volumes are generated by dividing the full range 7 swept volume with two planes which are the tangent planes of the 0 swept volume. The angle b between these planes is equal to where n = 1, 2, ..., and a binary tree data Chapter 4. Description of the Midpoint Algorithm 39 z 7 Oswept volume 0 swept region (a) cross-section of o swept volume (C) (b) cross-section of y swept volume Cd) Figure 4.4: The swept volumes used in the Point Check. (a) The 0 swept region and the swept region. (b) The 0 swept volume and the 7 swept volume. (c) The cross-section of 0 swept volume. (d) The cross-section of y swept volume. Chapter 4. Description of the Midpoint Algorithm structure is generated. Note that the full range 7 swept volume corresponds to n The cases of n axis, i.e. c = <00, 1 and n = 40 = 0. 2 is shown in Figure 4.5 (c). If the 0 swept volume contains Z the corresponding 7 swept volume and its decomposed swept volumes are always the full range 7 swept volume. The combination of a 0 swept volume and a swept volume is shown in Figure 4.5 (d). At a curve point, the corresponding 0 region of a 0 swept volume at a curve point is computed based on the optimal orientation at the point and the 0 angle of the swept volume. The details of this computation are discussed in Chapter 3. The Point Check searches the free cell in a Local C-space in two steps: (1) shrink the 0 region in order to search for the collision-free region (free 0 region), and (2) search the binary tree for collision-free ranges in 7 range for collision-free ranges (free 7 ranges). The combination of a free 0 region and a free 7 range corresponds to a free cell. Search the First Curve Point The Point Check is applied at the first curve point in order to search for a free cell in the corresponding Local C-space. Each free cell is the beginning of a potential path along the curve. The free cells are stored in the path list and sorted by the Path Cost of the free cell. The details of the Path Cost calculation are discussed in Section 4.2.7. If no free cell is found, trajectory planning fails since the robot tool cannot be placed at the first curve point without collision. A high resolution representation of the Local C-space is required to find small free cells. A high resolution Local C-space, however, requires a huge tree structure, which takes much time to store and search the data, and can lead to implementation problems. This detail is discussed in Chapter 5. Chapter 4. Description of the Midpoint Algorithm 41 tangent plane of o swept volume z “( swept volume 0 swept volume o swept volumes ‘( (a) swept volume (b) n=O czd c:: E ‘ swept volumes (C) n=2 0 andY swept volumes (d) Figure 4.5: Swept volumes used in Point Check (a) The division of 0 swept volume (b) The division of swept volume (c) The binary tree structure of 7 swept volume (d) The combination of a 0 swept volume and a 7 swept volume Chapter 4. 4.2.3 Description of the Midpoint Algorithm 42 Curvature Check Since a curve is represented by a set of discrete curve points, the motion of the tool along the curve is decomposed into the motion between successive curve points. Using a fixed step size between the curve points may not be practical depending on the complexity of the environment in the vicinity of the curve and the degree of curvature. For instance, using a very small step size along the curve may waste search time along the curve segments where the curvature is low and the surrounding environment is simple. Using a step size which is large, however, may result in the tool tip deviating from the required curve along curve segments where the curvature is high. This will result in poor process quality, or damage to the tool. It is difficult to predetermine a suitable fixed step size, and a mechanism for automatically adjusting the step size based on the nature of the curve and the environment is necessary. One of the advantages of the Midpoint Algorithm is that the step size is varied automatically along the curve. If the curvature along a segment is low, a large step size is used to speed up the trajectory planning along the segment. Otherwise, a small step size is used to ensure acceptable translation of the tool along the curve. In general, the more complex the curve, the smaller the step size. Adjustment of the step size based on curvature is discussed in this section, while adjustment based on the complexity of the environment is discussed in Section 4.2.5. The Curvature Check not only varies the step size along different curve segments, it also searches a polyline which matches the shape of curve within a given tolerance and is used as a reference for the tool tip. The polyline is generated by detecting if the straight line connecting successive points can represent the curve segment within the tolerance. Some points may be added or deleted based on the curvature of a curve, 1. If the curvature of a curve segment between successive points is high, the curve Chapter 4. Description of the Midpoint Algorithm 43 optimal orientation • intermediate track points between track point ti and t2 K intermediate line points between curve point p1 and p2 Figure 4.6: Intermediate points selected from curve segment and line segment segment cannot be represented by the straight line between the points. Further midpoints are added until the curvature of the new curve segments are within an acceptable tolerance of the original curve. 2. If there are several points on a flat section of the curve, some points are deleted in order to speed up the search process. The tolerance discussed above corresponds to the two parameters, dmax and dmin, the maximum and minimum distances between the tool tip and the curve. Both of these distances are specified by the application. In most of the cases, dmin is greater than o to ensure that the tool does not collide with the workpiece. For each curve point, a track point is generated that is positioned a distance dmax in the direction of optimal tool orientation. These track points correspond to the reference position for the tool tip. With reference to Figure 4.6, a number of intermediate points, called intermediate curve points, are selected from the curve segment between successive curve points Pi and P2, and the same number of points, called intermediate line points, are selected from the line segment connecting the corresponding track points ti and t . The number is called 2 the intermediate number. The optimal orientations at the intermediate curve points are specified as described in Section 1.1 for the original curve points. The following two tests examine if the line segment connecting successive track points is an acceptable Chapter 4. Description of the Midpoint Algorithm 44 representation of the required tool path. 1. Vector Test: for each intermediate line point, if the angle between the optimal orientation at the corresponding intermediate curve point and the vector from this intermediate line point to the corresponding intermediate curve point is greater than 90°, the Vector Test succeeds. 2. Distance Test: for each intermediate line point, if the distance between the inter mediate line point and its corresponding intermediate curve point are greater than dmin and less than dmax, the Distance Test succeeds. If one of the above tests fails, midpoints and corresponding track points must be added until both tests succeed. The addition of midpoints is illustrated in Figure 4.7 and summarized as follows, 1. Since the Vector Test fails between intermediate line point l and intermediate curve 1 is added between curve points point c 1 in Figure 4.7 (a), midpoint m P1 and P2 in Figure 4.7 (b). 2. Since the Distance Test fails between intermediate line point 12 and intermediate curve point c 2 in Figure 4.7 (c), midpoint m 2 is added between curve points P1 and 1 as illustrated in Figure 4.7 (d) m 3. In Figure 4.7 (d), no midpoint is required between curve points between m 2 and 1 in P1 and m , and 2 since both the Vector Test and the Distance Test succeed in these curve segments. 4. Since the Vector Test fails between intermediate line point 13 and intermediate curve point c 3 in Figure 4.7 (e), midpoint rn 3 is added between curve points m 1 and In Figure 4.7 (f) P2 Chapter 4. Description of the Midpoint Algorithm 45 p2 p1 Vector Test fails (a) (b) track of tool tip track of tool tip p2 p1 p2 Distance Test foiis Cc) Cd) Vector Test foils Ce) Vector Test and Distance Test succeed Vector Test and Distance Test succeed (g) Figure 4.7: Adding midpoints using Curvature Check Chapter 4. Description of the Midpoint Algorithm 46 (a) p1 (b) p1 p7 (C) Figure 4.8: Deleting curve points using Curvature Check 5. In Figure 4.7 (f), no midpoint is required between curve points m 1 and m 3 or between m 3 and P2 since both the Vector Test and the Distance Test succeed in these curve segments. 6. The final track of the tool tip redefined by the Curvature Check is shown in Figure 4.7(g). The Curvature Check is also used to delete curve points which are not necessary in the curve segment where the curvature is row. If the Curvature Gheck between pj nd Pi--i succeeds, the curvature between p and Pi+2 is checked. The number of intermediate line points and intermediate curve points selected must have the same density as the points specified by the intermediate number. For instance, if the intermediate number is Chapter 4. Description of the Midpoint Algorithm dmax - 47 drrin p1 Figure 4.9: The relationship between track and constraints: dma and dmin 3, three intermediate curve points and three intermediate line points are selected between p and Pi+i and their corresponding track points, and six intermediate curve points and six intermediate line points are selected between pj and Pi+2 and their corresponding track points, and so on. If the Curvature Check between p and Pi+2 fails, the Curvature Check between pj and Pi+i finishes, and no curve point is deleted from the curve. Otherwise, the curvature between p and Pi+3 is checked, and so on, until the Curvature Check between p and Pi+k fails. All the curve points between p and p+k1 are then deleted from the curve and the path segment between pj and Pi+k—1 is planned directly. The deletion of curve points is illustrated in Figure 4.8 and summarized as follows, 1. In Figure 4.8 (a), since the Vector Test and the Distance Test succeed between and Pi between P2, and P6, Pi P1 and p, between P1 and p4, between p and p, but fail between the curve points P2, p3 and p4 and the corresponding track points are deleted. 2. In Figure 4.8 (b), since the Vector Test and the Distance Test succeed between p and P6, between p5 and p, and p7 is the last curve point, the curve point P6 and its corresponding track point are deleted. 3. The final track of the tool tip searched by Curvature Check is shown in Figure 4.8 (c). The precision of the Curvature Check is determined by the parameter dma, dmin and Chapter 4. Description of the Midpoint Algorithm 48 p15 intermediate number s 2 (a) intermediate number is 5 (b) Figure 4.10: The intermediate number and the precision of Curvature Check intermediate number. Since the upper and lower boundaries of the track of the tool tip are specified by dmax and dmjm respectively as shown in Figure 4.9, a small dma and a large dmin result in a highly precise track of the tool tip. This will slow down the path planning, however, by adding more midpoints and searching more path segments between successive track points. Since the curvature change may not be detected by a small intermediate number, as shown in Figure 4.10 (a), and a large intermediate number as shown in Figure 4.10 (b) is necessary. Since the distance calculations are computationally inexpensive, a large intermediate number 4.2.4 ( e.g. 10) is recommended. Overlap Check Since the 7 ranges in the Local C-spaces are identical, the overlap discussed here is the overlap between the 0 regions. The intersection of two 0 regions is determined through a search for the overlapping ranges and the /3 range at every overlapping a value, as illustrated in Figure 4.11 (a). If no overlapping a range or overlapping 3 range at overlapping a value is found, such as regions 1 and 3, and regions 1 and 2 in Figure Chapter 4. Description of the Midpoint Algorithm 49 360 360 0 0 overappingx range a overiappingcx range (a) a (b) Figure 4.11: The search for overlapping region 4,11 (b), the two 0 regions do not overlap. The intersection points of the boundary of two 0 regions are obtained by searching the a value which has the same /3 value at the boundary in both regions. If the 0 regions at successive points overlap, the curve segment between these points can be searched. As illustrated in Figure 4.12 (a) (b), a valid path cannot be found within the allowable orientation range between p1 and P2. If the points on a continuous curve are very close, the optimal orientations should also be very close. Since the position of the 0 region is determined by the optimal orientation, the addition of midpoints eventually makes the 0 regions at successive points overlap as illustrated in Figure 4.12 (c). If the step size between two points is less than the minimum step size, a user defined parameter, and the 0 regions do not overlap, the curve is assumed to be discontinuous between the two points. In this case, the optimal orientation and the position of the midpoint are defined as the bisection of the orientations and the positions at the two points, and midpoints are added until the 0 regions overlap. This process, which is illustrated in Chapter 4. Description of the Midpoint Algorithm P1 50 Ml P2 (a) 360 180 360 — P2 Ml 180 0— 90 c’. a 0 (b) (C) Figure 4.12: The effect of adding midpoint Figure 4.13, improves the robot motion passing a corner or a discontinuous curve point. The addition of a midpoint may not lead to smooth changing orientations since the optimal orientation at the midpoint of two curve points may not be between the optimal orientations at the curve points. With reference to Figure 4.12 (a) and (c), the optimal 1 is not between the optimal orientations at P1 and orientation at midpoint m P2 If m 1 is not added, the robot tool will move between pi and P2 with the orientation interpolated between the optimal orientations at Pi and P2, which results in an incorrect trajectory. The addition of m 1 detects the change of curvature and avoids this error. The optimal 2 is between the optimal orientations at Pr and m , and the 1 orientation at midpoint m . 2 3 is between the optimal orientations at p and m optimal orientation at midpoint m The additions of m 2 and m 3 make the optimal orientations at curve points close enough 1n Figure 4.13, the curve is on the Y-Z plane and the optimal orientation is defined as the normal 3 of the curve, which maintains an acute angle with the Z axis. Chapter 4. Description of the Midpoint Algorithm midpoint 2 51 midpoint 1 midpoint 3 point I point i+1 Figure 4.13: Adding midpoints in a discontinuous curve since their 0 regions overlap. The additions of m 4 and m 5 are necessary since the 0 regions of 1 ni and , 2 rn and the 0 regions of m 1 and P2 do not overlap 4 respectively If . the curve is not continuous, the addition of midpoints provides for a smooth change of trajectory as illustrated in Figure 4.13. It is important to consider the relationship between the shape and size of the U region which depends on the a angle. In the a 0 range [0, 90], the smaller the a 0 angle, the larger the 3 range. In Figure 4.14 (a), orientations 1 and 4 are far apart, while orientations 2 and 3 are close together. Each orientation in the pair have the same c angle while /3, for orientations 1 and 2 and is 180° for orientations 3 and 4. The corresponding is 00 0 regions of 2 and 3 overlap, while the 0 regions of 1 and 4 do not overlap due to the different 3 ranges shown in Figure 4.14 (b). 4.2.5 Translation and Rotation Check Configuration Space Search The 0 regions of m2 and 4 P2, and the 0 regions of m 1 and m 5 happen to overlap in this example. Chapter 4. Description of the Midpoint Algorithm z 52 360 180 x 0 (a) cx (b) Figure 4.14: The overlap of 0 regions which have small angles pan of and free-cell 1+1 free-cell i+1 Figure 4.15: The path found though pj, p 2 and Pt+i Chapter 4. Description of the Midpoint Algorithm 53 This check detects the environment between two track points and searches for a path within the allowable orientation range. The path that is found allows the tool to trans late and rotate simultaneously between the points without collision. To meet these requirements, only the overlapping space between two Local C-spaces are considered. The overlapping space between the Local C-spaces at track points p and P1+1 5 is termed overlap-cell 1 , 1 1 6• If the robot tool maintains the configurations within part of overlap cel4,+i and translates between pj and P1+1 without collision, this part of overlap-cel4,+i is called free-cell,+i If free-cellj, +i overlaps free-cel4_j ,, free-cel4_j ,, free-cel4, 1 +i and 1 . their overlapping space compose of a safe path through curve points Pj—i, p and P1+1. This is illustrated in Figure 4.15. The projections of overlap-cel4, 1 and free-cel4.. , on the c 1 — ,8 plane are called 1 respectively, and the projection of free-cellj_ overlap-region,i and free-region_i, , on 1 the y axis is called 1 _i, The overlap between free-cell free-range . , and free-cell _ 1 +i , 1 are projected as the overlap between 1 _i, and free-region free-region ±i and the overlap , 1 between free-range_i, 1 and free-rangej,ji. The overlap between 1 _i, and free free-range rangej,ji is quite straight forward. We mainly discuss the overlap between free-region , _ 1 and free-region,÷i. With reference to Figure 4.16 (a), the 1 _i, does not overlap free-region , a midpoint m 11 1 should be added between p. and with the overlap-region, Pi+1. If free 1 as shown in Figure 4.16 (b), further regionj_l,j still does not overlap the overlap-regionj,m midpoints are added between p and m until 1 _i, overlaps with the overlap free-region regionj,m where mk is the closest midpoint to pj, as illustrated in Figure 4.16 (c). A _i, requires the addition of more midpoints, and a smaller step size is free-region small 1 automatically achieved and more careful motion of the robot is obtained as the path Because of the addition and deletion of curve points, the indices of curve points may not be sequential, 5 and be expressed by integers. In the following text, P1+1 represents the next point to pj and P11 represents the previous point to p. 1n the following text, “1,1+1” and 6 mean “between p and the next point to pt” and “between the previous point to respectively. and P1 P1” Chapter 4. Description of the Midpoint Algorithm 54 13 1 free-region 1.1+1 0 oregion 1÷1 (b) (a) L overlap regIon Cc) ri,÷ gion_ and overlap-regio 1 Figure 4.16: The overlap between , 1 free-re Chapter 4. Description of the Midpoint Algorithm 55 becomes narrow. After 1 _i, overlaps with the overlap-region,i where P1+1 is the next point free-region to p ‘. The free-cel4,+i is found by searching for (1) the free-region ÷i in the over , 1 lapping region between free-region _i, and overlap-region 1 ÷i, and (2) the free-range , 1 i , 1 ,÷ The tool head can rotate in free-region free-region . associated with the 1 i, and the , 1 tool handle can be placed in any position in free-range ±i as it moves between p , 1 1 and P1+1. The concepts of shrink step, shrink region and minimum shrink region are illustrated in Figure 4.17. In Figure 4.17 (a), the overlap-region, 1 shifts in the direction from the center of overlap-region,i to the center of 1 _i, The overlapping region free-region . between the shifted overlap-region ± and the original overlap-region , 1 8 is called the i , 1 shrink region. The shift distance of overlap-region, 11 is called the shrink step. The i shifts by the shrink step each time until the shifted overlap-region , 1 overlap-region 1 ,j+l does not overlap the original overlap-region . In Figure 4.17 (a) (b) and (c), three , 1 shrink regions are generated. If 1 _i, is not totally included in the original free-region +i, the minimum shrink region is the shrink region with minimum area 1 overlap-region, , is totally included in the original overlap _ 1 as shown in Figure 4.17 (c). If free-region regioni,il, the minimum shrink region is the minimum shrink region which overlaps _i, In Figure 4.17 (d), the overlap-region free-region . 1 11 overlaps with 1 , 1 _i, free-region . i does not overlap with 1 , 1 In Figure 4.17 (e), the shifted overlap-region _i, free-region . The shrink region in Figure 4.17 (d) is the minimum shrink region. The free-region i is found by shrinking overlap-region , 1 , toward 1 1 _i, if free-region 11 collides with objects in the envi , 1 the corresponding swept volume of overlap-region ronment, as illustrated in Figure 4.18 (a). In Figure 4.18 (b), the corresponding swept 7 1 n the above example, we redefined mk as pi-i-i. 1 is the over1ap-region, The original overlap-regionj, 8 i without shift. 1 Chapter 4. Description of the Midpoint Algorithm 13 56 13 free-region i 1 free-regioi I-LI , shrink region shrink region overlap-region u+1 shrink overlap-region i.i+1 a (a) (b) 13 free-region shrink region and also minimum shrink region free-region overlap-region iJ+ 1 erIap-reghn a (C) Cd) p shrink region free-region j,j+1 a. Ce) Figure 4.17: The shrink step, shrink region and minimum shrink region Chapter 4. Description of the Midpoint Algorithm 57 volume of the shrink region again collides with objects in the environment. In Figure 4.18 (c), the corresponding swept volume of the shrink region is collision-free and this shrink region corresponds to free-region,i. If free-region,i is not found when mini mum shrink region has been checked, a midpoint is added between p and Pi+1• Adding a midpoint changes the overlapping region between local 0 regions at successive points and allows some new configurations to be considered. A minimum step size is specified, and if free-region,i is not found and the step size between successive points is smaller than the minimum step size, the current path is deleted from the path list and other paths in the path list are planned. If the path list is empty, the trajectory planning stops. If free-region ,+j is found, a search for the corresponding 1 1 free-range, ÷ i is carried out. The 7 range is decomposed as a binary tree structure, and free-rangej,j÷i is searched using a breadth-first search. Only the 7 ranges that has a common range with free-rangej_i, are searched. The free-range_i,, free-range,i and their common range contribute to the path of the tool handle. If no free-rangez,ji is found, the corresponding free-region,i is invalid and must be shrunk by one shrink step, and the free-rangej,ji is searched again for the new free-region,i. The Swept Volumes Used in Translation and Rotation Check Three kinds of swept volume between curve points p and Pi+1 are used in Translation and Rotation Check, the 0-trans swept volume, the 7-trans swept volume and the shrink swept volume which correspond to overlap-region,i, the range and shrink region respectively. The 0-trans swept volume is generated by extruding the intersection of two full range 0 swept volumes in the direction of tool motion, i.e. the direction from p to P1+1, as illustrated in Figure 4.19 (a). With reference to Figure 4.19 (a) (b), the size of the intersection volume depends on the angles 4 the full range 0 swept volumes at the points and i c2 and between the optimal orientations of Pi+i. The length of the 9-trans swept Chapter 4. Description of the Midpoint Algorithm free-region free-region 1-1,1 overlap-region 58 shrink region 1.1÷1 — (0) cx (b) free-region shrink region 1 free-region (C) , in order to find 1 Figure 4.18: Shrinking overlap-region,i toward free-region_ 1 free-region, Chapter 4. Description of the Midpoint Algorithm 59 volume corresponds to the distance between successive track points. With reference to Figure 4.19 (c) (d), the intersection orientations are defined as the orientations at the intersection points of the two 0 regions. The minimum angle w between the direction of tool motion and the plane defined by the two intersection orientations can assume any value, as illustrated in Figure 4.19 (d). The angle w determines the shape of the 0-trans swept volume. Two 0-trans swept volumes with the same size but different w angles are shown in Figure 4.19 (e) and (f). The 0-trans swept volumes corresponding to different sizes, lengths and angles are generated in advance. During the check, a 0-trans swept volume is selected based on the overlapping size, distance between successive track points and the angle w with respect to the direction of tool motion. The 7-trans swept volume corresponding to a 0-trans swept volume is approximated by using two vertical 0 swept volumes shown in Figure 4.20 (b) to displace the 0 swept volumes used to generate the 0-trans swept volume, as shown in Figure 4.20 (a). The length L, the angle and the allowable angle range 0 of the 0-trans swept volume are known. In Figure 4.20 (a), line segment oa and line segment ob are on the boundary of the intersection of 0 swept volumes and are also on the plane defined by the axes of 0 swept volumes. The distance between o and a, and the distance between o and b are L. The distance between a and b is D, and the angle between oa and ob is . Using the cosine law in triangle oab = 2 2L — cos 2 2L (4.2) Since = 40 — 2 we get (4.3) Chapter 4. Description of the Midpoint Algorithm optimal orientation optimal orIentation 1+1 60 optimal orientation I (a) p (b) Intersection orientation o region Intersection orientation 9 region 1+1 plane determined by Intersection orientation (C) (d) plane determined by intersection orientation plane determined by Intersection orientation direction of tool top view of the swept volume In (b) (e) direction of tool motion top view of the swept volume In (d) U) Figure 4.19: The 0-trans swept volumes used in Translation and Rotation Check Chapter 4. Description of the Midpoint Algorithm 61 From Equation (4.2) and (4.3), we get D = \/L/1 — 0 cos(20 (4.4) — In Figure 4.20 (b), d is the distance between the tips of the 0 swept volumes. d = 0 2LsinO — D (4.5) From Equation (4.4) and (4.5), we get d = 0 2Lsin0 — v’L1 — 0 cos(20 — ) (4.6) Given a 0-trans swept volume, the distance d between the two 0 swept volumes can be calculated using Equation (4.6), and the intersection of two corresponding full range 7 swept volumes is obtained as illustrated in Figure 4.20 (c). The corresponding full range 7-trans swept volume is generated by extruding the intersection along the direction of tool motion, as illustrated in Figure 4.20 (d). The length of the extrusion and the angle are the same as those of the 0-trans swept volume. The full range 7-trans swept volume is decomposed into the binary tree structure illustrated in Figure 4.20 (e). Each 7-trans swept volume binary tree associates with the corresponding 0-trans Swept volume. Since the shrink region between pj and Pi+1 is the overlapping region between the shifted overlap-region,i and the original , 11 the shrink swept volume is 1 overlap-region generated by taking the copy of the intersection of two 0 swept volumes, called the inter volume, refer to Figure 4.21 (a), and rotating the inter volume angle about the line which is in the plane defined by the two intersection orientations and is perpendicular to the line from the tip of the intersection to the center of the intersection, as shown in Figure 4.21 (b), and then extruding the intersection of the two inter volumes along the direction of tool motion, as illustrated in Figure 4.21 (c). Here is the angle between 1 and the center of original overlap-region, the center of shifted overlap-region, . 11 w is the minimum angle between the direction of tool motion and the plane defined by the two 9 intersection orientations as shown in Figure 4.19 (c). Chapter 4. Description of the Midpoint Algorithm 62 L (a) fufl range 7 swept volume (b) overlapping 7 swept volume direction of tool motion 7 trans swept volume - (C) (d) position of 0 swept volume (e) Figure 4.20: The 7-trans swept volume used in Translation and Rotation Check Chapter 4. Description of the Midpoint Algorithm 63 optimal inter volume (a) (b) 7”\\\ (C) (d) Figure 4.21: The shrink swept volume used in Translation and Rotation Check Chapter 4. Description of the Midpoint Algorithm 64 The parameters in Translate and Rotate Check are minimum step size and shrink step. A large minimum step size results in a coarse search for a trajectory, while a small minimum step size increases the search resolution between successive track points. The shrink step determines shrinking resolution. 4.2.6 Inverse Kinematics Check The motion of the robot tool is also limited by the robot joint constraints. Computation of the robot inverse kinematics solution requires the configuration and position of the robot wrist to be calculated from the position of the track point and the selected configuration of the robot tool. With reference to Figure 4.22, the following homogeneous coordinate frames are used in the computation: 1. Robot Base Frame Fr: the frame with origin at the robot base. 2. Track Point Frame F: the frame located at the track point having the same ori entation as Fr. : the frame with an origin coincident with F but 3 3. Selected Configuration Frame F rotated by a 8 about ‘, and j3 about Zr,. 8 but rotated by -y about 4. Tool Frame F: the frame with an origin coincident with F . 8 z 5. End Effector Frame Fe and i1 are in the plane determined by the tool (, ñ, : axis and the tool handle, and points to the track point. H, 8 , H 1 and The homogeneous transformations [Wolo 91] H,.,, 8 t,e 11 defining the robot position and orientation can be obtained from the frame definitions listed above. The robot base to tool configuration coordinate transformation H is calculated as, = l:Ir,pIIp,8.l:Ig,t Ht,e (4.7) Chapter 4. Description of the Midpoint Algorithm 65 The right hand side of Equation (4.7) corresponds to the homogeneous transformations in the sequence of the robot base, tool tip, tool head, tool handle, and end effector. Given a tool configuration ( cv, /3, y, X, y, z ), the right hand side of Equation (4.7) is known. The left hand side of Equation (4.7) represents the homogeneous transformations in the sequence of the robot base, linki, link2,..., end effector. For a given robot, the final overall (base to tool) configuration coordinate transformation r,e 11 which contains the required joint angles, is fixed [Wolo 91]. So the robot joint angles can be calculated in Equation (4.7). The details are discussed in Appendix C. The configurations in the path comprising free-region_i, and free-region , are de 1 termined as follows: 1. The configurations in free-region_i, are the configurations of the robot tool at the midpoint between Pi-1 and pj. 2. The configurations in the overlapping region between free-region_i, and free-region,i are the configurations of the robot tool at p. 3. The configurations in free-region,j+i are the configurations of the robot tool at the midpoint between pj and Pi+1. Multiple inverse kinematics solutions can be computed based on one position/orientation of the robot tool. For instance, eight solutions are obtained for the PUMA 560 robot. If all joint angles in an inverse kinematics solution fall within the robot joint constraints, the inverse kinematics solution is valid. In each region, only the configurations which are near the center of a region are used to compute the inverse kinematics solutions. This provides a degree of safety in the trajectory since there is a collision-free region between the selected configuration and the environment. If none of the configurations produce a valid inverse kinematics solution, Chapter 4. Description of the Midpoint Algorithm 66 Zp - “S N. Zs selected tool --..orientction “s IS , I / toolframe 5 a a Vp frock Xr Figure 4.22: A series of coordinate frames Chapter 4. Description of the Midpoint Algorithm 67 this check fails and other paths in the path list are explored. If valid inverse kinematics solutions are computed, these solutions and the corresponding configurations are stored at a subsequent stage in the planning. A cost function is used to select the inverse kinematics solutions and their corresponding configurations at each point to generate the optimal trajectory. Details of this search are discussed in Section 4.2.8. The number of configurations selected for the inverse kinematics computation is very important. If just a few configurations are selected, the check may fail although there may be unexplored configurations that correspond to valid inverse kinematics solutions. Increasing the number of configurations results in a better chance of finding valid inverse kinematics solutions, and a better trajectory will be planned since more configurations and inverse kinematics solutions can be used by the cost function. This, however, takes more computation time and requires more memory to store the data. Control the Path Planning Using A* Search Algorithm 4.2.7 The path consists of a list of free-regions at the curve points which have been included in the path. If there is more than one path in the path list, an A* search algorithm [Wins 84] is used to control the path planning for multiple paths. This search is summarized as follows, Until the path list is empty, determine if the first path in the path list includes the final point on the curve, 1. If the first path includes the final curve point, do nothing. 2. If the first path does not include the final curve point, (a) Remove the first path from the path list. (b) Form new paths from the removed path by extending one step. Chapter 4. Description of the Midpoint Algorithm 68 (c) Add the new paths to the path list. (d) Sort the path list in the order of increasing cost, where the cost is the accu mulated Path Cost so far plus a lower-bound estimate of the cost remaining. (e) If two or more paths reach a common curve point, delete all those paths except for the path that reaches the common curve point with the minimum cost. The Path Cost is calculated as PathCost = C 0 WvCv + W — WpCp (4.8) Where Wv, Wo and Wp are the weights of cost Cv, C 0 and Cp respectively. Cv, the cost measuring the average area of the path, is calculated as (4.9) where V is the volume of free-cel4,+i and n is the number of current track points. This cost is used to select a wider path. Go, the cost measuring the average departure of the path segment away from the optimal orientation, is calculated as = where a is the average of and max 3 / crmjn /(a — 2+ cr) di 3 (/ /3)2 — (4.10) and crmax in the free-region ,i, f3 is the average of 1 ,÷ and crj and free-region , in the 1 di 3 ! mim 3 I are the averages of cr and ,8 angles in the optimal orientations at pj and pf 1 respectively. This cost is used to select the path which is closer to the optimal orientation. Cp, the cost measuring the percent of the curve that has been travelled, is calculated as Cp (4.11) Chapter 4. Description of the Midpoint Algorithm 69 where n is the current point number and n is the number of track points travelled. This cost is used to select the path which is close to the end of the curve. The parameters in this check are the weights in the cost function. By changing the weights, the trajectory is planned in different ways. For example, a large value of Wv means a wider path is preferred, a large value of Wo means that the application constraint is important, and a large value of Wp means the speed of finding a path is important. The selection of the weights is discussed in 5.4. 4.2.8 Optimize Trajectory In each path segment, the inverse kinematics solutions of a number of tool configurations are calculated as described in Section 4.2.6. After finding the path along the curve, the optimal trajectory along the curve is obtained using a second A* search to select both the robot inverse kinematics solutions and corresponding tool configurations from the path. The A* search is discussed in Section 4.2.7. The cost used in the second search is calculated as k TrajCostk = Wj k k 1 + WD C CDi + WH 1=1.0 J k + (WR j=1 Where 1.0 k 11.O k ) 1 C 1=1.0 +W — WC (4.12) 1=1.0 n, k is current track point number, n is the final track point number in the track, and J is the number of robot joints. Since some midpoints are added in the track, the track point index (k) is not an integer. In Equation (4.12), W and C represent weights and costs respectively. is the cost measuring the joint angle displacement from the midpoint of the valid joint angle. If several joint angles are available, the joint angle closest to the midpoint of joint range is preferred. This avoids joint boundaries as the tool travels the curve. The cost C 1 is Description of the Midpoint Algorithm Chapter 4. calculated as 70 (6—6m3)2 Ofj j = (4.13) j=1 where O and Ornj are the jth joint angle and the midpoint of jth joint range, and is the full range of the jth joint. CD, the cost measuring deviation of the tool orientation from the optimal orientation, is calculated as j 3 / (‘2 _‘ / Li Di— + 2 / Li 3 I where aj and /3 are the a and /3 values of the optimal orientation at the ith track point, are the a and /3 values of the selected orientation at the ith track point, and a and aL and Li 3 / are the a and 3 ranges of the Local C-space at the ith track point. CH, the cost measuring the difference between the selected configurations at the ith and i — ith track points is calculated as (ai—ap2 \ / + Li 3 I “ / + (‘YiYip2 “ 7Li / 3 where a , /3 and 1 track point, at the i — (4 15 are the a, /3 and y values of selected tool configuration at the ith ,B,, and are the a, /3 and y values of the selected tool configuration ith track point, and a,j, I Li and 3 7Li are the a, /3 and 7 ranges of the Local C-space at the ith track point. CR1, the cost measuring the difference between the robot joint angles at the ith and i — ith track points, is calculated as CRij— 1•L i / I.._iL. 2 Vj — 1=1 ,f 3 v \2 ) are the jth joint angles at the ith and i where n, is the joint number, and track point respectively, and is the full range of the jth joint angle. — ith Chapter 4. Description of the Midpoint Algorithm Cci, the cost measuring the change of robot configuration at the ith and i 71 — ith track points, is calculated as = where T and (1 — 1)2 (4.17) are the robot configuration indices at the ith track point and i + ith track point respectively. Cp, the cost measuring the percent of the curve that has been travelled, is defined by Equation (4.11). The cost function weights are used to bias the trajectory as shown in following table, Weights Effect on trajectory planning Wj safe robot motion WD maintaining optimal tool orientation WH and WR 3 smooth motion Wa robot configuration avoidance Wp speed of search optimal trajectory Normally, a large value of Wc is applied to keep the same robot configuration along the curve. If the robot configuration changes from “elbow up” to “elbow down”, or changes from “right” to “left”, as illustrated in Figure 4.23, discontinuous motion of robot disrupts the smooth motion of the tool. If a robot configuration selected at the beginning of search changes in the middle of the curve, all proposed trajectories using this configuration will have a very high cost. Details of the selection of weights for an example application of this algorithm are given in Chapter 5. Chapter 4. Description of the Midpoint Algorithm (a) 72 (b) Figure 4.23: Different robot configurations 4.3 Summary of the Midpoint Algorithm The Midpoint Algorithm includes the following steps: 1. Reconstruct the curve with Catmull-Rom splines. 2. Predetermine the optimal orientation at each curve point according to the geometric criterion of the robot tool specified by the application. 3. Predetermine the parameters in trajectory planning according to the requirements of applications. 4. Store the curve point information into a linked list. 5. Initialize the path list by using the Point Check procedure at the first point. If the path list is empty, the path planner stops. 6. Use the A* search algorithm to explore multiple paths in the path list. The minimum cost path is extended to the next point. The extension of a path between successive Chapter 4. Description of the Midpoint Algorithm 73 points is subject to the following checks: Curvature Check, Overlap Check, Trans lation and Rotation Check, Inverse Kinematics Check and Path Cost Check. The track of tool tip is built and points may added or deleted along the track. 7. If no path though the track can be found after trying all the path in the path list, the path planner stops. 8. If the minimum cost path in the path list is found, an A* search selects the tool configurations and robot joint values in the final path. The flowchart of the Midpoint Algorithm is illustrated in Figure 4.24. Chapter 4. Description of the Midpoint Algorithm Figure 4.24: Flowchart for the Midpoint Algorithm 74 Chapter 5 Computer Implementation and Examples 5.1 Overview An off-line simulation program, TRAJPLAN, has been developed in order to test the Midpoint Algorithm. Using TRAJPLAN, the user only needs to input the application specification, to select the curves to be processed and to indicate the environment objects that may collide with a robot tool during the process. A trajectory for the robot tool will then be automatically planned. The output of TRAJPLAN is a set of sequential joint angles which can be downloaded to the robot. TRAJPLAN has been developed using Zortech C++ release 3.0, AutoCAD release 12, AutoCAD Advanced Modeling Extension (AutoCAD AME) release 2.1, and AutoCAD Development System (AutoCAD ADS). As a solid (3-D) and region (2-D) modeling pro gram, AutoCAD AME is used to generate the swept volumes of the robot tool. AutoCAD ADS is a C language programming environment for developing AutoCAD applications. In TRAJPLAN, the AutoCAD ADS is used to move the swept volumes of tool and to check for interference between swept volumes and the environment. Since TRAJPLAN only deals with two adjacent points on a curve, the complexity of the curve and environment does not have any effect in the algorithm. A practical limitation to TRAJPLAN is that the amoiant of data to be stored may exceed the system memory model if many curve points are used. This has not been a problem in the trials of the system to date where there are typically 100 points have been selected. TRAJPLAN 75 Chapter 5. Computer Implementation and Examples 76 is compiled using the Zortech C++ 3.0 compiler using the P memory model (Phar Lap 386—DOS Extender, a full 32 bit 386 DOS Extender) to allow the program to operate in 32 bit protected mode with a linear address space of 4 Gbytes. While PUMA 560 and CRS A460 inverse kinematics modules have been developed to work with TRAJPLAN, kinematic modules for other robots can be easily linked with TRAJPLAN. TRAJPLAN has successfully demonstrated automatical programming of a robot welding system and a fish butchering system. 5.2 The Simulation Program TRAJPLAN The simulation program TRAJPLAN is initiated by identifying the curves to be processed and the environment objects which may collide with the robot tool. The user still needs to select the desired way to plan the trajectory (refer to Section 5.4), precision level of trajectory (refer to Section 5.5), and the optimal orientation (refer to Section 5.6). If no selection is given, default settings are used. After the user initiates the TRAJPLAN, the path planning starts by reconstructing the identified curve based on sample curve points using the Catmull-Rom Spline method. A linked list is used to store the reconstructed curve. The advantage of the linked list is that a node can be easily inserted and deleted. The path planning procedure builds and updates the path list, a linked list which records all of the potential paths. Each path in the path list is also a linked list which links the path segments between successive points. Since different paths may pass through different environments and add different midpoints into the curve, each path must record its points. The path list is updated with following steps: STEP 1 Use check loop, including Curvature Check, Overlap Check, Translation and Rotation Check, Inverse Kinematics Check and Path Cost Check, to extend the top Chapter 5. Computer Implementation and Examples 77 CurvePoint 1 2 2.5 3 4 5 ‘ii n Path 1 Path 2 PathList Path 3 Path k El free-part 11+1 Figure 5.1: PathList data structure path in the path list. If the check ioop fails, add a midpoint and repeat STEP 1. STEP 2 Substitute top path by extended path and sort the path list by the PathCost, placing the path with minimum cost at the top of the path list. Finish the path planning if the last curve point is reached. Otherwise, return to STEP 1. One of the basic tasks of planning is to place swept volumes of the tool at a track point or between successive track points and to check for the interference between the swept volumes and environment in order to find a free cell or free-cell,+i. At the first point, free cells are found by shrinking the 0 swept volumes in order to obtain collision free 0 swept volumes, and then using breadth-first search to find the collision free swept volumes in corresponding y swept volume binary tree. If no collision free y swept volume is found, the 0 swept volume is shrunk again and the corresponding 7 swept volume binary tree is searched. Between subsequent curve points, the search for free-cellj,+i is very similar to the search for free cell, i.e. search 9-trans swept volumes first and then search 7-trans swept volumes corresponding to the collision free 9-trans swept volume. The procedure Chapter 5. Computer Implementation and Examples 78 is illustrated in Figure 5.2. In order to access the swept volumes efficiently during path planning, the swept volumes are down loaded into memory. This detail is discussed in Section 5.3. For the PUMA 560 and CRS A460 robots, each robot tool configuration can be used to calculate eight robot inverse kinematics solutions. The trajectory of the robot tool has two parts: the configurations and positions of the tool, and the corresponding inverse kinematics solutions. If TRAJPLAN cannot find a path, or the final trajectory based on the selected path contains a change of robot configuration, TRAJPLAN reports the error or warning message to the user. 5.3 Down Load the Swept Volume Into Memory Loading the swept volume into memory provides efficient access to the swept volumes during the interference check, which speeds up the trajectory planning. Loading all of the swept volumes into memory, however, results in a simple tree structure to be used, which cannot represent a configuration space with high resolution, even for Local C space. It is impossible to load a huge data structure into memory due to the limitations on memory space and the restrictions of MS DOS. The 0 swept volumes are stored in a one dimensional array and 0-trans swept volumes are stored in a three dimensional array based on the size, length and angle of the swept volume. Each 0 swept volume has a full range 7 swept volume associated with it. The ,‘ swept volume is decomposed into a binary tree. If each dimension in the 0 swept volume array or the 0-trans swept volume array contains three elements, and a five level 7 swept volume binary tree 1 is associated with each array element, the total number of swept volumes is 930 (3 x 31+3 x 3 x 3 x 31). MS DOS, however, only allows 250 files to be opened simultaneously. To solve this problem, A five level binary tree contains 31 nodes. 1 Chapter 5. Computer Implementation and Examples (a) (C) 79 (b) (d) Figure 5.2: The procedure for searching for collision free swept volume (a) The 0 swept volume collides with environment, (b) Shrinking 0 swept volume for the collision free 0 swept volume collides with environment, (d) Search swept volume, (c) The swept volumes for the collision free 7 swept volume. Chapter 5. Computer Implementation and Examples 80 oniy the swept volumes with different shapes are loaded into memory. The remaining swept volumes can be obtained by rotating the loaded swept volume having the same shape. The rotation angle , /3 and ‘y are determined by the difference in the , /3 and angles between two corresponding swept volumes. In this way, 120 (3 x 4 +3 x 3 x 3 x 4) swept volumes instead of 930 swept volumes are loaded into memory, and the exploration of a high resolution configuration space is possible. 5.4 Plan the Trajectory as Desired Several options for planning the trajectory can be selected by the user: 1. Select a trajectory which meets the application requirements as quickly as possible. 2. Select the optimal trajectory from all of the potential trajectories. 3. Select a trajectory which maintains the orientation close to the user specified ori entation. 4. Select a trajectory which avoids a change of robot configuration. 5. Select a trajectory which provides for smooth motion of the tool. If time is not of great concern, the user may require TRAJPLAN to provide the optimal trajectory. Or, if the user has limited time, he can require TRAJPLAN find a path as quickly as possible. The weights in cost functions (Equation (4.8) and Equation (4.12)) determine the options of TRAJPL.. N The default values of the weights to calculate Path Cost are Chapter 5. Computer Implementation and Examples 81 shown in following table, Weight Effect on trajectory planning Value Comments Wp speed of planning path 0.6 the most important Wo path close to optimal orientations 0.3 important Wv width of path 0.1 not very important The default values of the weights to calculate TrajCost are shown in following table , 2 weight effect aspect in trajectory value comments Wj joint angle is close to the center of joint angle range 0.01 not very important WD trajectory is close to optimal orientation 0.15 important WH trajectory keeps its history 0.2 important Wp speed of planning path 0.15 important WR1 the first joint angle keeps its history 0.15 important WR2 the second joint angle keeps its history 0.15 important WR3 the third joint angle keeps its history 0.1 important WR4 the fourth joint angle keeps its history 0.03 not very important WR5 the fifth joint angle keeps its history 0.03 not very important WR6 the sixth joint angle keeps its history 0.03 not very important Wc the change of robot configuration 100 very important A large value is assigned to Wc in order to avoid the robot arm inversion. The values of WR1, WR2, W , WR4, W 1 5 and WRG indicate that the motion of the robot wrist is A large value is assigned to Wc in order to avoid robot arm inversion. 2 82 Chapter 5. Computer Implementation and Examples more flexible than the motions of upper arms. 5.5 The Precision of Trajectory The precision of trajectory is very important since it affects the quality of process. Differ ent applications have different precision requirements. For example, a welding application may require the tool tip to follow the curve more strictly than a painting application. The higher the required precision, the more careful the motion, although the precision should be determined by the user according to the requirement of application. Otherwise the following default values are assumed, dmin 0.4 inch , 2 dma 0.8 inch number of intermediate points 8 minimum step size 0.02 inch number of configurations picked up from path segment 24 For the convenience of inexperienced users, the values of parameters have been set at different accuracy levels. If the default accuracy level is not suitable for the applica tion, the user can simply select the accuracy level to alter the speed or precision of the trajectory planning algorithm. 5.6 The Specification of Optimal Tool Orientation To use TRAJPLAN, user need to input the application specification including the op timal orientation of robot tool relative to the curve, the allowable deviation of the tool orientation away from the optimal orientation, and the allowable deviation in the dis tance between the tool tip and the curve. Without the input, TRAJPLAN will plan Chapter 5. Computer Implementation and Examples normal of plane 2 opflmal orientation p’ane 1 83 optimal orientation tangent 1 plane 2 curve curve (a) (b) Figure 5.3: The optimal orientation of robot tool a trajectory based on the default specifications. With reference to Figure 5.3 (a), the default geometric criterion of the tool optimal orientation at a curve point is defined as follows, 1. The tool optimal orientation is perpendicular to the tangent of curve at the point. 2. The tool optimal orientation corresponds to the vector that bisects the normals of the two planes on each side of the curve. 3. The tolerance of the orientation deviation 9 from the tool optimal orientation is 300 5.7 5.7.1 as shown in Figure 5.3 (b). Examples: Automatic Welding and Fish Butchering Processing Automatic Robot Welding Process An automatic robot welding system is shown in Figure 5.4. The process constraints include the distance between the welding head and the seam, the optimal welding ori entation, collision avoidance, valid robot inverse kinematic solutions, and robot arm inversion avoidance. Complete knowledge of the system including the position of objects Chapter 5. Computer Implementation and Examples tod seam 84 PUMA 560 robot obstacle Figure 5.4: The automatic robot welding workcell Chapter 5. Computer Implementation and Examples 85 in the workcell and the seams is provided by a computer-aided design system, AutoCAD. In the workcell shown in Figure 5.4, TRAJPLAN finds a collision free path subject to process constraints in 1’20”. Most of this time is taken by the AutoCAD interference checks. An example procedure for searching for collision free paths by placing various swept volumes on the seam is illustrated in Figure 5•53 and described as follows, (a) Place the full range 0 swept volume at the first seam point. The 0 swept volume collides with obstacles. (b) Shrink the 0 swept volume until it does not collide with obstacles. (c) Place the full range 7 swept volume corresponding to the selected 8 swept volume at the first seam point. The swept volume collides with obstacles. (d) Search the 7 swept volumes at lower levels in the swept volume binary tree. The first 7 swept volume selected collides with obstacles. (e) Select another 7 swept volume. This 7 swept volume is collision free. (f) Test the 0-trans swept volume corresponding to the selected collision free 0 swept volume. The swept volume is collision free. (g) Place the full range 7-trans swept volume corresponding to the selected 0-trans swept volume on the seam. The 7-trans swept volume collides with obstacles. (h) Search the 7-trans swept volumes at lower levels in the 7-trans swept volume binary tree. The selected 7-trans swept volume collides with obstacles. (1) Select another 7-trans swept volume. This 7-trans swept volume also collides with obstacles. The dash line is the seam and the cylinder and the plate are the obstacles. 3 Chapter 5. Computer Implementation and Examples (j) 86 Search the 7-trans swept volumes one level down in the 7-trans swept volume binary tree. The selected 7-trans swept volume collides with obstacles. (k) Select another 7-trans swept volume. The selected 7-trans swept volume is collision free. (1) Select another 7-trans swept volume. The selected 7-trans swept volume collides with obstacles. (m) Select another 7-trans swept volume. The selected 7-trans swept volume is collision free. Up to now, there are two possible paths found in (k) and (m) respectively. 5.7.2 Automatic Fish Butchering Process The Industrial Automation Laboratory at UBC has developed a system for automatically selecting cutting contours for a fish butchering process [Gama 93]. The fish butchering workcell is shown in Figure 5.7. When fish passes along the convey or the position and the shape of fish contour is calculated by a knowledge-based computer image process ing system. The process constraints include the distance between fish and a water-jet cutter, the optimal cutter orientation, collision avoidance, valid robot inverse kinematic solutions, and robot arm inversion avoidance. Using the workcell shown in Figure 5.7, TRAJPLAN finds a collision free path subject to process constraints in 15 seconds. An example procedure for searching for collision free paths by placing various swept volumes on the fish contours is illustrated in Figure 5.6 and described as follows, (a) Place the full range 0 swept volume at the first contour point. The 0 swept volume does not collide with obstacles. The dash line is the fish contour. 4 Chapter 5. Computer Implementation and Examples (a) (b) Ce) (f) (I) Ci) 87 (d) (C) (g) (h) (k) (m) Figure 5.5: Procedure for searching for collision free paths. (I) Chapter 5. Computer Implementation and Examples 88 (b) Place the full range 7 swept volume corresponding to the selected 0 swept volume at the first contour point. The 7 swept volume collides with obstacles. (c) Search the y swept volumes at lower levels in the 7 swept volume binary tree. The first swept volume selected collides with obstacles. (d) Select another 7 swept volume. This 7 swept volume is collision free. (e) Test the 0-trans swept volume corresponding to the selected collision free 0 swept volume. The swept volume is collision free. (f) Place the full range 7-trans swept volume corresponding to the selected 0-trans swept volume on the contour. The 7-trans swept volume collides with obstacles. (g) Search the 7-trans swept volumes at lower levels in the 7-trans swept volume binary tree. The selected 7-trans swept volume collides with obstacles. (h) Select another 7-trans swept volume. This 7-trans swept volume also collides with obstacles. (i) Search the 7-trans swept volumes one level down in the 7-trans swept volume binary tree. The selected 7-trans swept volume collides with obstacles. (j) Select another 7-trans swept volume. The selected 7-trans swept volume is collision free. (k) Select another 7-trans swept volume. The selected 7-trans swept volume collides with obtcles. (1) Select another 7-trans swept volume. The selected 7-trans swept volume is collision free. Up to now, there are two possible paths found in (i) and (1) respectively. o;q 0 tz_ CD 0 s-a CO p CD C CO C-) C C-) p CD Co CD C 0 CD CD C;’ c C’ C ‘.-‘ ‘— :r n n C n § a C) C’ § 00 Co I I f Chapter 5. Computer Implementation and Examples camera tool obstacle 90 obstacle camera PUMA 560 robot contour / fish obstacle Figure 5.7: The automatic fish butchering workcell Chapter 6 Conclusion and Future Work 6.1 Conclusion In this thesis a new trajectory planning algorithm, the Midpoint Algorithm, has been de veloped. The algorithm plans smooth trajectories for a robot tool moving along specified curves subject to application constraints. Geometric constraints ( allowable orientation and position of tool) and motion constraints (joint limits, robot configuration change avoidance and collision avoidance) are also integrated into the algorithm. The trajectory is planned by searching for path segments between successive curve points, linking valid path segments to generate a path along the curve and selecting the optimal configurations inside the path to specify the final trajectory. The configuration space of a robot tool forms a critical component of the algorithm. The configuration of robot tool has three degrees of freedom, o, /3 and 7. This config uration gives a general description of robot tool, which includes the tool axis derived from optimal orientation within an allowable tolerance and the tool itself rotating about the tool axis. The concepts of Entire C-space and Local C-space were introduced as a convenient representation of the tool configuration for planning purposes. Since the Local C-space only covers useful parts of the Entire C-space, a condensed data structure is used to represent the configuration space with high resolution. The Local C-space is the com bination of the 0 region and the range, which correspond to the constraints of allowable tool orientation and robot inverse kinematics respectively. Different Local C-spaces have 91 Chapter 6. Conclusion and Future Work 92 different shapes, sizes and positions of the corresponding 0 regions. A mathematical ex pression for Local C-space has been derived, and given the optimal orientation (cr , 3) of 0 the Local C-space and the maximum allowable orientation 0, the corresponding 0 region can be calculated. The trajectory planning is initiated using a Point Check procedure to search the Local C-space at the first curve point and initializes a path list of potential paths. A Translation and Rotation Check identifies collision free path segments by searching overlapping 0 regions at successive points, shrinking the overlapping regions in order to find collision free regions. If no collision free 7 ranges are found even though the minimum 0 region has been tested, a midpoint is added if the step size is greater than the minimum step size, while the current path cannot be extended if the step size is less than the minimum step size Corresponding to the 9 regions and y ranges, swept volumes of the tool are gen erated and used to detect collision between the tool and the environment. Since only swept volumes which have different shapes are loaded into memory, the system mem ory requirements are reduced and exploration of high resolution configuration space is possible. To address the problem of how to guarantee that the track of the tool tip matches the curve, a method of automatically varying the step size between curve points was developed. The step size is examined to determine: (1) if the straight line connecting successive curve point can approximate the shape of this curve segment, (2) if the optimal orientation at the points are close enough, and (3) if the environment between the two points is simple enough. After adjusting the step size, a minimum number of curve points are used to search for the path, which results in fast path planning. The small step size used in complex situations leads to safer paths. A polyline models the track of tool tip, which ensure that the tool does not collide with the workpieces. Chapter 6. Conclusion and Future Work An A* search optimizes the path planning for multiple paths. 93 Since the planner searches configurations along the entire curve, robot configuration changes can be avoided, which provides smooth motion of the tool. The planner also allows the tool to translate and rotate simultaneously without collision, which provides for a smooth motion of tool. An off-line simulation program, TRAJPLAN, based on the Midpoint Algorithm has been developed. After the user inputs the application specifications, selects the curve to be processed, and identifies the environmental objects, TRAJPLAN automatically plans the trajectory of robot tool and generates a set of sequential joint angles corresponding to a particular robot. Currently the system supports the PUMA 560 and CRS A460 robots. The Midpoint Algorithm has been demonstrated in software simulation of fish butcher ing and robotic welding. The algorithm is suitable to a variety of robot applications which require collision free motion subject to process constraints. By changing the accuracy levels of the parameters, the user specifies the precision of the trajectory or the kind of trajectory preferred in order to meet the different requirements of robot applications. 6.2 Recommendations for Future Work This approach only considers the collision free motion of the robot tool, but not that of robot manipulator. Also dynamic constraints are not considered. An immediate extension to this algorithm includes the consideration of the entire robot and the inclusion of dynamic constraints in the planning process. Also more extensive testing on industrial applications needs to be done. TRAJPLAN is a general algorithm for CAD environments which provide functions for building solids, moving solids and checking for interference between solids. To date TRAJPLAN has only been implemented for use with AutoCAD. Chapter 6. Conclusion and Future Work 94 TRAJPLAN can also be used without a graphics interface after creating the data structures for storing solids and the interference check tool. Most of the time in path planning is spent on the interference check by AutoCAD and the display of graphics on the screen. Without the graphics interface, TRAJPLAN can plan the trajectory much faster and it may be possible to use it as a real-time software. There are three steps to build such a system: 1. Use sensors to obtain the information of environment and the curves to be pro cessed. 2. Build a data structure to hold the information of the robot, tool and environment. 3. Create an interference check function using the above data structure. Appendix A Data Structure Some data structures used in TRAJPLAN are described in this section. 1. The structures about curve and track, Config = record alpha, beta, gamma: double; end; Point = record x, y, z: double; end; CurvePoint = record num: double; curve_point : Point; track_point : Point optimal: Config end; Curve = record data: CurvePoint; 95 Appendix A. Data Structure ‘ 96 next : Curve end; Here is the descriptions of above structures: alpha, beta, gamma the angle of tool configuration. num the number of curve point. optimal the optimal orientation of robot tool at the point. curve_point point in the curve. track_point point in the track of robot tool tip. x, y, z the coordinates of point. 2. The structures used to represent region and range in Local C-space and the corre sponding swept volumes, Region = record boundary : array [0. .20] [0. .2] of double; j next : Region end; Thnode = record i, j, color : integer region : Region Id: apObjid; t fname : char end; Appendix A. Data Structure Range = 97 record MinG, MaxG : double; end; Gnode = record i, j, color : integer range : Range id: ap...Objid; I fname: char end; Here is the descriptions of above structures: boundary the boundary of 0 region calculated as discussed in Chapter 3. MinG, MaxG the boundary of ‘y range. i, j the index of a Gnode in binary tree. color the color of the node. region the 0 region in Thnode. range the corresponding ‘y range of the node. id the swept volume object ID used for moving the swept volume and checking the interference. fname the swept volume file name used for loading the swept volume into memory. 3. The structures about robot inverse kinematic solution, Appendix A. Data Structure InvKinSol = 98 record angle : array [0. .6] of double; end; InvKinSols = record config: Config; solution : array [0. .8] of InvKinSol; vali&num: double; end; Here is the descriptions of above structures: angle the array storing six robot joint angles. config the robot tool configuration. solution the array storing eight robot inverse kinematics solutions based on one tool orientation. valicL.num the number of valid robot inverse kinematics solutions. 4. The structures about path list, Part = record poinLnum: double; point : CurvePoint region : Region range: Range; Tsolutions : InvKinSols; Appendix A. Data Structure 99 end; Path = record data: Part Inext: Path; end; PathList = t record path : Path PathCost : double; next : PathList end; Here is the descriptions of above structures: poinLnum the number of the track point. point the information of track point. region the 0 region of the free-cell,+i. range the 7 range of the 1 free-cel4, + i. solutions the inverse kinematics solutions. data the information of a region. path ,i. 1 the list of free-cell PathCost the cost of the path. 5. The structure of robot tool trajectory composed of two parts: the configuration and position of tool, and the corresponding robot inverse kinematic solutions. Appendix A. Data Structure Trajectory = record poinLnum: double; point : Point select : Config solution : InvKinSol next : Trajectory end; Here is the descriptions of Trajectory structures: poinLnum the number of the curve point. point the track point. optimal the optimal orientation at the point. select the selected configuration. solution the robot inverse kinematics solution. 100 Appendix B TRAJPLAN The pseudo-code of the main procedure in TRAJPLAN is listed as follow: var addmid, finish: integer; obstaclelD : ap_objid; Cspacel, Cspace2 : Part; program Trajplan() { main procedure for trajectory planning var curve: Curve; I paths : PathList; I point, I last_point : Curve; I trajectory : Trajectory; begin addmid := 0; finish := 0; SpecificationQ; curve := SelectCurveQ; obstaclelD : = SelectEnvironmentQ; LoadSweptVolumeQ; paths := PointCheck(point); 101 } Appendix B. TRAJPLAN if paths 102 NULL then exit; {No path found} = while (finish not 1)or (finish not -1) begin 1astpoint NULL; := last_point : = SearchLastPoint (pathspathtdata.point_num, curve); if last_point = NULL then exit; else if lastpointnext finish := = NULL then 1; {trajectory planing finish.} else begin paths : = CheckLoop(paths, last_point, lastpointnext); if addmid = 0 then begin pathstPathCost := PathCost(pathspath); paths := SortPathList(paths); finish := 0; end end end if finish = -1 then exit; {No path found} trajectory : = OptimizeTrajectory(pathstpath); end function CheckLoop( paths:PathList, 1ast_point:Curve, nextpoint:Curve): PathList; { Check loop is applied to successive points to search the path segment between them. The Overlap Check and Inverse Kinematics Check are included in TranslateRotateCheck. } var Appendix B. TRAJPLAN 103 retval : integer; begin addmid 0; Cspacel = BuildCspace( 1ast_pointdata.optima1); Cspace2 = BuildCspace( nextpointdata.optima1); if not CurvatureCheck(1astpointdata, nextpointdata) then begin addmid := 1; curve := AddMidpiont(curve, 1astpoint, next_point); return paths; end retval : = TranslateRotateCheck(paths, 1ast_pointdata, next_point Idata) if retval = 0 then begin addmid { fres-part do not overlap.} 1; curve : = AddMidpiont (curve, lasLpoint, next_point); return paths; end else if retval = -1 then begin {no path segment found.} if PointCheck finish := = NULL then begin -1; exit; end else begin paths := DeletePath(paths); return paths; end Appendix B. TRAJPLAN 104 else begin {the minimum cost path is extended} addmid 0; := pathspathdata.poinLnum: next..pointldata.num; return paths; end end function TranslateRotateCheck( Ipaths:PathList, last: CurvePoint, next: CurvePoint) : integer; { This function searches the free-region between last and next points.} var length : double; shrink_center: Config; 1’ I I I ov_region, free_region, ov_free_region: Region; free_range: Range; free_part : Part; solutions : InvKinSols; begin length : = Distance(last .data.track_point, next .data.track_point); if length < minimun...step_size then return -1;{avoid infinitive adding midpoint.} ov_region : = OverlapCheck( Cspacel .region, Cspace2.region); if ov_region free_region NULL then return 0 = := ShrinkOvRegion(ov_region, length, last, next); NULL then return -1; if free_region = free_range FreeRange(free_region, length, last, next); := if free_range = NULL then Appendix B. TRAJPLAN 105 free_region : = SmallRegion(free_region); if free_region ov_free_region curve := NULL then finish = := 0; return -1; OverlapCheck(pathspath data.region, free_region); AddMidpiont(curve, last_point, next_point); solutions : = InverseKinematicCheck (ov_free_region, last_pointlnext); if solutions = NULL then return -1; pathspathdata.region.solutions := InverseKinematicCheck (ov_free_region, last_point tnext); if solutions = NULL then return -1; {If the inverse kinematic solution available, add midpoint region between last and next points.} pathstpath: = AddPart (ov_trans_free_region, last_point tnext); pathspath data. region. solutions : = InverseKinematicCheck(trans_free_region, next_point); if solutions = NULL then return -1; pathspath: = AddPart (trans_free_region, next_point); { add region at next point.) return 1;; end { The following primitive routines are needed to perform above procedures} procedure Specification; { Input the specification of robot application. function SelectCurve() { } Curve; Read the curve point information of the selected curve and reconstruct curve. } function SelectEnvironment() : ap_objid;; { Union the selected environment objects and return the objectlD.} procedure LoadSweptVolumeQ; Appendix B. TRAJPLAN { 106 Load the swept volumes into memory. } function SearchFirstPoint (point) : RegionList; { Search white part in the Local C-space at the first point and store the parts into path list. } function SortPathList( paths) : PathList { Sort the paths in the path list according to the PathCost and put the minimum cost path at the front of the list. ) function SearchLastPoint( path { } : Curve; Search the last point to be traveled by current path.} function BuildCspace( optimal) : Part; { Calculate the 0 region based on optimal orientation 0 ,j3 and 0.} (c ) function DeletePath( paths { ) : PathList; Delete the top path from the path list and return the path list.} function ShrinkOvRegion( ov_region, length, last, next { ) : Region; Shrink the overlapping 0 region to search the region of free-cel4,+i and return the region} function Distance( pointi, point2 { : double; Calculate the distance between two track points and return the distance. function AddPart( part, path { ) ) : Path; Add free part into path and return the path. function FreeRange( free_region, length, last, next { ) } : Range; Search collision free 7 range of free_region and return the range. function BuildPart( free_region, free_range { } ) } : Part; Build free_part based on free_region and free_range and return the part. function AddMidPoint( point, next_point ) : Curve; } Appendix B. TRAJPLAN { 107 Add midpoint between lasLpoint and nexLpoint and return the curve. function CurvatureCheck( lasLpoint, nextpoint { : boolean; Return True if two points can pass curvature check.} function OverlapCheck(regionl, region2 { ) } ) : Region; Check the overlap between two regions and return the overlapping region. } function PathCost(path) : double; { Calculate the PathCost of a path.} function InverseKinematicCheck(region, point) : ISolutions; { Compute the robot inverse kinematics solutions. There are 24 configurations in the region to be selected and 8 inverse kinematics solutions can be obtained from each configuration. } function OptimizeTrajectory(point) : Trajectory; { Use cost function to optimize the trajectory. } Appendix C Inverse Kinematic Modules for PUMA 560 and CRS A460 With reference to [Wolo 91] and Figure (C.1), the six Homogeneous transformation ma trices of PUMA 560 robot are as follows: H= 1 cosO 1 —sinO 0 0 1 .sinO 1 cosO 0 0 0 0 1k 0 0 01 2 cosO H= (C.1) 2 0 0 —sinO 0 0 10 (C.2) 2 2 —sinO —cosO 0 0 0 0 01 3 —sinO cosO 3 0 e H= 3 sin9 3 cosO 0 0 0 0 lg 0 0 01 4 cosO H= 4 0 —sin0 0 0 (C.3) 0 if 4 —cosO —sinO 4 0 0 0 0 108 01 (C.4) Appendix 0. Inverse Kinematic Modules for PUMA 560 and CRS A460 Zp N. selected tool orientation \Zs /7Z S \Vs / /7o4 track point -- Vp e 03 Xp Xs Xr Figure C.1: The PUMA 560 robot and robot tool 109 Appendix C. Inverse Kinematic Modules for PUMA 560 and CRS A460 5 cosO 5 0 0 —sinO 0 0 10 5 —sinO 5 —c080 0 0 0 0 01 6 cosO 6 —sinO 0 0 0 0 —1 —d 6 sinO 6 COSO 0 0 0 0 H= 110 (C.5) (C.6) 01 The only difference between CRS A460 robot and PUMA 560 robot is the different values of parameters: d, e, f, g, and h. The Homogeneous transformation matrices (C.1), (C.2), (C.3), (C.4), (C.5), and (C.6) can also be used to describe CRS A460 robot. The computation subsequently discussed is for both the PUMA 560 robot and the CRS A460 robot. Equation (4.7) Hr,e = Hr,pHp,sHs,tHt,e is used in inverse kinematics computation. The Hr,e can be rewritten as H and is calculated as = (C.7) From Equation (C.1) (C.2) (C.3) (C.4) (C.5) and (C.6), we know that Equation (C.7) only has variables: parameters, d, e, Oi, 02, f, g, 03, 04, 05, 06, d, e, f, g, and h. The PUMA 560 geometry and h are known. The right hand side of Equation 4.7 represents the Homogeneous transformations from robot base to seam point and then end-effector. The End Effector Frame Fe (, i, .‘) and end-effector position p are attained, as shown in Figure C.1. The desired wrist position used in later computation is Appendix C. Inverse Kinematic Modules for PUMA 560 and CRS A460 111 calculated as I The calculations for 02 04 = = = Atan2 Man 2 [ Atan2 J the robot joint = Atan2 03 pyw [ (P—da P,—da, Pz_daz) = angles are as follows — j I + Atan2 r± L r [Wolo 91], +P g — 2 g ] + e + — 2 P—P g fyw — FZW 2 1 —h) ±4e2f2_[e2+f2+g2_P 2 _p2 212 yw (zw —h’ / I (C.9) ] (C.1O) )1 3 I—(-Pcos0i + Psin0i)fcos0 3 — (Pzw — h)(e — fsin0 ) — (P — h)fcos0 3 j 3 (Pcos0i + Psin0i)(e fsin0 (C.11) 1 asinOi — acos0 acos0icos(0 + 03) + aysinOlcos( 2 2 + 03) — 2 0 asin(0 + 03)] (C.12) L — ±180° 5 and—0 05 = Atan2 1 c a 2 os9 + (acos0icos(0 2 + 03) — asin(0 2 + 03) + asin0icos(0 [V(axsin0i + ) 2 + 03))2 sinOisin(0 + 03) — acos(0 a 2 + 03) — 2 —acos0isin(0 2 + 03) (C.13) and — 05 06 = Atan2 2 s 1 I—scos0 in(0 + 03) — .ssin0isin(0 2 + 03) — scos(0 2 + 03)1 2 + 03) + nysinOlsin( ncos0isin(0 2 + 03) + ncos(0 0 2 + 03) j [ ±180° 6 and—0 (C.14) Bibliography [Ange 88] Angeles, J., Rojas, A. and Lopez-Cajun, C.S., “Trajectory Planning in Robotics Continuous-Path Applications”, IEEE Transactions on Robotics and Automation, Vol.4, No.4, 1988. . 385 380 pp. [Barr 89] Barry, P. and Goldman, R., “A Recursive Evaluation Algorithm for a Class of Catmull-Rom Splines”, SIGGRAPH 89, . 204 199 pp. [Bars 80] Barsky, B.A. and Greenberg D.P. “Determining a Set of B-spline Control Ver tices to Generate an Interpolating Surface”, Computer Graphics and Image Processing, No.14, 1980, . 226 203 pp. [Bart 87] Bartels, R., Beatty J. and Barsky, B.A., “An Introducetion to Spline for Use in Computer Graphics and Geometric Modeling”, by Morgan Kaufmann, Los Altos, CA, 1987. [Boll 71] Bollinger, J.G. and Harrison, H.L., “Automatied Welding Using Spacial Seam Tracing”, Welding Journal, November 1971. . 792 787 pp. [Broo 83(a)] Brooks, R.A., “Solving the Find-Path Problem by Good Representation of Free Space”, IEEE Transactions on Systems, Man and Cybernetics, SMC Vol.13, No.3, 1983, . 197 190 pp. [Broo 83(b)] Brooks, R.A. and Lozano-Pérez, T., “A Subdivision Algorithm in Configu ration Space for Findpath with Rotation”, Proceedings of the 8th International Conference on Artificial Intelligence, Karlsruhe, FRG, 1983, . 806 799 pp. 112 Bibliography 113 [Buch 89] Buchal, R.O., Cherchas, D.B., Sassani, F. and Duncan, J.P., “Simulated OffLine Programming of Welding Robots”, International Journal of Robotics Re search. Vol.8, No.3, 1989, . 43 31 pp. [Buck 89] Buckley, S.J., “Fast Motion Planning for Multiple Moving Robots”, Proceed ings of the IEEE International Conference on Robotics and Automation, Scottsdale, 1989, . 326 322 pp. [Cann 88] Canny, J.F., “The Complexity of Robot Motion Planning”, by The MIT Press, 1988. [Cann 90] Canny, J.F. and Lin, M.C., “An Opportunistic Global Path Planner”, Pro ceedings of the IEEE International Conference on Robotics and Automation, 1990, . 1559 1554 pp. {Catm 74] Catmul, E. and Rom, R., “A Class of Local Interpolating Splines”, in Barnhill, R.E. and Riesenfeld, R.F. eds., Computer Aided Geometric Design, Academic Press, San Francicisco, 1974. pp.317-326. [Chaz 87] Chazelle, B., “Approximation and Decomposition of Shapes”, in Schwartz, J.T. and Yap, C.K., Algorithmic and Geometric Aspects of Robotics, Lawrence Erlbaum Associates, Hillsdale,NJ, 1987. pp. 185 145 [Fole 91] Foley, J.D., Van dam, A., Feiner, S.K. and Hughes, J.F., “Computer Graphics”, by Addison-Wesley Publishing Company, 1991. [Garna 93] Gamage, LD.K.B., “A Model-Bud ApprocLek to Gomplex Gontour Genera tion for Process Automation Using Computer Vision Ph.D Dissertation, De partment of Mechanical Engineering, University of British Columbia, 1993. Bibliography 114 [Gupt 90] Gupta, K.K., “Fast Collision Avoidance for Manipulator Arms: A Sequential Search Strategy”, IEEE Transactions on Robotics and Automation, Vol.6, No.5, 1990. pp.522-532. [TKamb 86] Kambhampati, S. and Davis, L.S., “Multiresolution path Planning for Mobile Robots”, IEEE Journal of Robotics and Automation, vol. RA-2, No.3. 1986. 145 135 pp. . [Khat 86] Khatib, 0., “Real-Time Obstacle Avoidance for Manipulators and Mobile Robots”, International Journal of Robotics Research. Vol.5, No.1, 1986. pp.2741. [Khos 85] Khosla, P.K., Neuman, C.P. and Prinz, F.B., “An Algorithm for Seam Tracking Applications”, International Journal of Robotics Research. Vol.4, No.1. 1985. pp.27-41. [Lato 91] Latombe, J., “Robot Montion Planning”, by Kluwer Acakemic Publishers, 1991. [Loza 79] Lozano-Pérez, T. and Wesley, M.A., “An Algorithm for Planning Collisionfree Paths Among Polyhedral Obstacles”, Communications of the ACM, Vol.22, No.10, 1979, . 570 560 pp. [Loza 81] Lozano-Pérez, T., “Automatic Planning of Manipulator Transfer Movements”, IEEE Transactions on Systems, Man and Cybernetics. Vol.11, No.10, 1981, pp.681-698. [Loza 83] Lozano-Prez, T., “Spatial Planning: A Configuration Space Approach”, IEEE Trans. on Computer. vol. c-32. no.2 February, 1983, . 119 108 pp. Bibliography 115 [Loza 87] Lozano-Pérez, T., “A Simple Motion-Plannining Algorithm for General Robot Manipulators”, IEEE Journal of Robotics and Automation, Vol. RA-3, No.3, June 1987, pp.224-237. [Nils 69] Nilsson, N.J., “A Mobile Automaton: An Application of Artificial Intelligence Techniques”, Proceedings of the 1st International Joint Conference on Robotics and Automation, Washington D.C., 1969, pp. . 5 509 20 [Odon 89] O’donnell, P.A. and Lozano-Pérez, T., “Deadlock-Free and Collision-Free Co ordination of Two Robot Manipulators”, Proceedings of the IEEE International Conference on Robotics and Automation, 1989, pp. . 4 484 89 [Reif 85] Reif, J.H. and Sharir, M., “Motion Planning in the Presence of Moving Obsta cles”, Proceedings of the 25th IEEE Symposum on Foundations of Computer Science, 1985, pp. . 1 144 54 [Schw 83] Schwartz, J.T. and Sharir, M., “On the Piano Movers’ Problem:3. Coordinat ing the Motion of Several Independent Bodies: The Special Case of Circular Bodies Moving Amidst Polygonal Barriers, “, International Journal of Robotics Research. Vol.2, No.10, 1983, pp. . 1 97 40 [Shaf 92] Shaffer, C.A. and Herb, G.M., “A Real-Time Robot Arm Collision Avoidance System”, IEEE Transactions on Robotics and Automation, Vol.8, No.2, 1992. pp.149-160. [Shar 89] Sharir, M., “Algorithmic Motion Planning in Robotics”, IEEE Computer. March, 1989, pp. . 1 9 9 [Sing 91] Singh, S.K. and Leu, M.C., “Manipulator Motion Planning in the Presence of Bibliography 116 Obstacles and Dynamic Constraints”, International Journal of Robotics Re search. Vol.10, No.2, April 1991, . 187 171 pp. [Taka 89] Takahashi, 0. and Schilling, R.J., “Motion Planning in a Plane Using Gen eralized Vorinoi Diagrams”, IEEE Transactions on Robotics and Automation, Vol.5, No.2, April 1989. . 150 143 pp. [Tomi 801 Tomizuka, M., Dornfeld, D. and Purcell, M., “Application of Microcomputers to Automatic Weld Quality Control”, Journal of Dynamic Systems, Measure ment and Control. Vol.102, June 1980, . 68 63 pp. [Tour 86] Tournassoud, P., “A Strategy for Obstacle Avoidance and Its Application to Multi-Robot Systems”, Proceedings of the IEEE International Conference on Robotics and Automation, San Francisco, 1986, pp.1224-1229. [Udup 77] Udupa, S.M., “Collision Detection and Avoidance in Computer Controlled Manipulators”, Proc. 5th International Conferenec of Artificial Intelligence, 1977, . 748 737 pp. [Warr 89] Warren, C.W., Danos, J.C. and Mooring, B.W., “An Approach To Manipula tor Path Planning”, International Journal of Robotics Research. Vol.8, No.5, October 1989. [Wang 92] Wang, D. and Hamam, Y., “Optimal Trajectory Planning of Manipulators With Collision Detection and Avoidance”, International Journal of Robotics Research. Vol.11, No.5, October 1992, . 498 460 pp. [Wins 84] Winston, P.H., “Artificial Intellengence”, by Addison-Wesley Publishing Com pany, Inc. 1984. Bibliography 117 [Wolo 91] Wolovich, W.A., “Robotics: Basic Analysis and Design”, by CBS College Pub lishing, 1991. [Zhu 91] Zhu, D. and Latombe, J., “New Heuristic Algoriths for Efficient Hierarchi cal Path Planning”, International Journal of Robotics Research. Vol.7, No.1, February 1991, . 20 9 pp.
- Library Home /
- Search Collections /
- Open Collections /
- Browse Collections /
- UBC Theses and Dissertations /
- Trajectory planning along continuous paths subject...
Open Collections
UBC Theses and Dissertations
Featured Collection
UBC Theses and Dissertations
Trajectory planning along continuous paths subject to process constraints Li, Dan 1993
pdf
Page Metadata
Item Metadata
Title | Trajectory planning along continuous paths subject to process constraints |
Creator |
Li, Dan |
Date Issued | 1993 |
Description | This thesis presents a new trajectory planning algorithm for planning safe and smooth tra jectories of a robot tool moving along a specified curve subject to application constraints. The application constraints include geometric constraints (allowable orientation and po sition of the tool) and motion constraints (joint limits, robot configuration inversions and collision avoidance). The algorithm, which is based on a configuration space approach for robot motion planning, addresses the problem of motion planning along a curve rather than point-to-point motion associated with pick and place operation. Configuration spaces are searched to assess the locations of the obstacles in the vicinity of the curve and the curvature of the curve in the work space. The trajectory is planned by searching between path segments connecting successive curve points, generating the path along a curve linking the path segments, and selecting the optimal configurations along the path to construct the final trajectory. If more than one paths exist, an A* search is em ployed to optimize the path. The selected path specifies a sequence of tool configuration ranges, and each configuration produces several inverse kinematic solutions. A second A* search is used to select inverse kinematic solutions, which avoid major changes of robot configuration (i.e. joint inversion). Since the algorithm searches the allowable tool orientation range, the optimal trajectory which maximizes process quality is obtained. The trajectory allows the tool to translate and rotate simultaneously without collision, and the tool remains within the allowable orientation range and tracks the desired curve within a given tolerance. Since the step size between curve points is varied dynamically as the search proceeds, the path is planned based on a minimum number of curve points, resulting in fast path planning. A smaller step size is used if the curvature of the curve or the likelihood of collision is high. The primary advantage of the algorithm is not only in finding a collision free path but in planning a trajectory which satisfies application constraints. It is felt that this algorithm provides a high quality process which is difficult to achieve with manual robot programming. An off-line simulation program, TRAJPLAN, has been developed based on this al gorithm. After the user inputs the application specifications, selects the curves to be processed and the environment objects of interest, TRAJPLAN automatically plans the trajectory of robot tool and generates the required set of sequential robot joint angles. TRAJPLAN plans the motion with different precision depending on the requirements of the application. This algorithm has been demonstrated for robotic fish butchering and welding, and the issues related to these applications are discussed in this thesis. As an example of a typical problem, the system automatically plans the trajectory of a robot tool moving along a 3-D curve with 31 points and surrounded by 5 obstacles in 1’20”. The algorithm is suitable for a variety of robot applications which require the robot tool to move along a curve, keep a certain orientation and position relative to the curve, and avoid collision with the environment. |
Extent | 2354914 bytes |
Genre |
Thesis/Dissertation |
Type |
Text |
File Format | application/pdf |
Language | eng |
Date Available | 2009-02-24 |
Provider | Vancouver : University of British Columbia Library |
Rights | For non-commercial purposes only, such as research, private study and education. Additional conditions apply, see Terms of Use https://open.library.ubc.ca/terms_of_use. |
DOI | 10.14288/1.0080927 |
URI | http://hdl.handle.net/2429/4991 |
Degree |
Master of Applied Science - MASc |
Program |
Mechanical Engineering |
Affiliation |
Applied Science, Faculty of Mechanical Engineering, Department of |
Degree Grantor | University of British Columbia |
Graduation Date | 1994-05 |
Campus |
UBCV |
Scholarly Level | Graduate |
Aggregated Source Repository | DSpace |
Download
- Media
- 831-ubc_1994-0091.pdf [ 2.25MB ]
- Metadata
- JSON: 831-1.0080927.json
- JSON-LD: 831-1.0080927-ld.json
- RDF/XML (Pretty): 831-1.0080927-rdf.xml
- RDF/JSON: 831-1.0080927-rdf.json
- Turtle: 831-1.0080927-turtle.txt
- N-Triples: 831-1.0080927-rdf-ntriples.txt
- Original Record: 831-1.0080927-source.json
- Full Text
- 831-1.0080927-fulltext.txt
- Citation
- 831-1.0080927.ris
Full Text
Cite
Citation Scheme:
Usage Statistics
Share
Embed
Customize your widget with the following options, then copy and paste the code below into the HTML
of your page to embed this item in your website.
<div id="ubcOpenCollectionsWidgetDisplay">
<script id="ubcOpenCollectionsWidget"
src="{[{embed.src}]}"
data-item="{[{embed.item}]}"
data-collection="{[{embed.collection}]}"
data-metadata="{[{embed.showMetadata}]}"
data-width="{[{embed.width}]}"
async >
</script>
</div>
Our image viewer uses the IIIF 2.0 standard.
To load this item in other compatible viewers, use this url:
http://iiif.library.ubc.ca/presentation/dsp.831.1-0080927/manifest