TRAJECTORY PLANNING ALONG CONTINUOUS PATHS SUBJECTTO PROCESS CONSTRAINTSByDan LiB.A.Sc. (Mechanical Engineering) TsingHua University, 1985A THESIS SUBMITTED IN PARTIAL FULFILLMENT OFTHE REQUIREMENTS FOR THE DEGREE OFMASTER OF APPLIED SCIENCEinTHE FACULTY OF GRADUATE STUDIESMECHANICAL ENGINEERINGWe accept this thesis as conformingto the required standardTHE UNIVERSITY OF BRITISH COLUMBIADecember 1993© Dan Li, 1993In presenting this thesis in partial fulfilment of the requirements for an advanceddegree at the University of British Columbia, I agree that the Library shall make itfreely available for reference and study. I further agree that permission for extensivecopying of this thesis for scholarly purposes may be granted by the head of mydepartment or by his or her representatives. It is understood that copying orpublication of this thesis for financial gain shall not be allowed without my writtenpermission.(Signature)_________________________________Department of P31JThe University of British ColumbiaVancouver, CanadaDate F 9 3DE.6 (2/88)AbstractThis thesis presents a new trajectory planning algorithm for planning safe and smooth trajectories of a robot tool moving along a specified curve subject to application constraints.The application constraints include geometric constraints (allowable orientation and position of the tool) and motion constraints (joint limits, robot configuration inversions andcollision avoidance). The algorithm, which is based on a configuration space approach forrobot motion planning, addresses the problem of motion planning along a curve ratherthan point-to-point motion associated with pick and place operation. Configurationspaces are searched to assess the locations of the obstacles in the vicinity of the curveand the curvature of the curve in the work space. The trajectory is planned by searchingbetween path segments connecting successive curve points, generating the path alonga curve linking the path segments, and selecting the optimal configurations along thepath to construct the final trajectory. If more than one paths exist, an A* search is employed to optimize the path. The selected path specifies a sequence of tool configurationranges, and each configuration produces several inverse kinematic solutions. A secondA* search is used to select inverse kinematic solutions, which avoid major changes ofrobot configuration (i.e. joint inversion). Since the algorithm searches the allowable toolorientation 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 curvewithin a given tolerance. Since the step size between curve points is varied dynamicallyas 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 curve11or the likelihood of collision is high. The primary advantage of the algorithm is not onlyin finding a collision free path but in planning a trajectory which satisfies applicationconstraints. It is felt that this algorithm provides a high quality process which is difficultto achieve with manual robot programming.An off-line simulation program, TRAJPLAN, has been developed based on this algorithm. After the user inputs the application specifications, selects the curves to beprocessed and the environment objects of interest, TRAJPLAN automatically plans thetrajectory of robot tool and generates the required set of sequential robot joint angles.TRAJPLAN plans the motion with different precision depending on the requirements ofthe application.This algorithm has been demonstrated for robotic fish butchering and welding, andthe issues related to these applications are discussed in this thesis. As an example of atypical problem, the system automatically plans the trajectory of a robot tool movingalong a 3-D curve with 31 points and surrounded by 5 obstacles in 1’20”. The algorithmis suitable for a variety of robot applications which require the robot tool to move alonga curve, keep a certain orientation and position relative to the curve, and avoid collisionwith the environment.111Table of ContentsAbstractList of Figures viiList of Symbols xAcknowledgment xiii1 Introduction 11.1 Motivation 11.2 Midpoint Algorithm: a Novel Approach to Motion Planning 41.3 Outline of Thesis Content 52 Literature Review 62.1 A Basic Motion Planning Problem: Collision Avoidance 62.2 Path Planning Methods Based on the Configuration Space Approach 82.3 Path Planning for Robot Tool Along a Continuous Curve 112.4 The Spline of Curve 132.5 Searching Path Through a Net 142.6 Conclusion Based on Literature Review 153 Configuration Space of Robot Tool 163.1 Configuration of Robot Tool 163.2 Configuration Space of Robot Tool 17iv4 Description of the Midpoint Algorithm 324.1 Overview of the Midpoint Algorithm 324.2 The Description of the Midpoint Algorithm 324.2.1 Reconstruct the Curve by Catmull-Rom spline 354.2.2 Point Check for the First Point 37Point Check 37Search the First Curve Point 404.2.3 Curvature Check 424.2.4 Overlap Check 484.2.5 Translation and Rotation Check 51Configuration Space Search 51The Swept Volumes Used in Translation and Rotation Check 574.2.6 Inverse Kinematics Check 644.2.7 Control the Path Planning Using A* Search Algorithm 674.2.8 Optimize Trajectory 694.3 Summary of the Midpoint Algorithm 725 Computer Implementation and Examples ‘755.1 Overview 755.2 The Simulation Program TRAJPLAN 765.3 Down Load the Swept Volume Into Memory 785.4 Plan the Trajectory as Desired 805.5 The Precision of Trajectory 825.6 The Specification of Optimal Tool Orientation 825.7 Examples: Automatic Welding and Fish Butchering Processing 835.7.1 Automatic Robot Welding Process 83V5.7.2 Automatic Fish Butchering Process 866 Conclusion and Future Work 916.1 Conclusion 916.2 Recommendations for Future Work 93Appendices 95A Data Structure 95B TRAJPLAN 101C Inverse Kinematic Modules for PUMA 560 and CRS A460 108Bibliography 112viList of Figures1.1 The robot welding workcell 21.2 The optimal orientation of robot tool 32.1 A Catmull-Rom spline 143.1 Robot tool 173.2 Definition of robot tool configuration 183.3 The c and 3 ranges of Entire C-space and the allowable orientation rangeat different curve points. (a) Entire C-space. (b) Allowable orientationrange 193.4 Entire C-space and Local C-space 203.5 The shape of local 0 region 223.6 The calculation of local 0 region of Local C-space 233.7 Entire C-space and local 0 region 283.8 The local 0 region of Local C-space in the boundary of Entire C-space . 304.1 The relation between step size and the condition of environment and curve 354.2 A Catmull-Rom spline 364.3 The definitions of cell, 0 region and -y range 374.4 The swept volumes used in the Point Check. (a) The 0 swept region andthe -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 sweptvolume 39Vu4.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 sweptvolume (d) The combination of a 0 swept volume and a 7 swept volume 414.6 Intermediate points selected from curve segment and line segment . . 434.7 Adding midpoints using Curvature Check 454.8 Deleting curve points using Curvature Check 464.9 The relationship between track and constraints: dmax and dmin 474.10 The intermediate number and the precision of Curvature Check 484.11 The search for overlapping region 494.12 The effect of adding midpoint 504.13 Adding midpoints in a discontinuous curve 514.14 The overlap of 0 regions which have small a angles 524.15 The path found though Pi-1, p and Pi+1 524.16 The overlap between free-region_i, and overlap-region,i 544.17 The shrink step, shrink region and minimum shrink region 564.18 Shrinking overlap-region,1toward free-region:_i,j in order to find freeregionj,ji . . . . 584.19 The 0-trans swept volumes used in Translation and Rotation Check. . 604.20 The 7-trans swept volume used in Translation and Rotation Check . . 624.21 The shrink swept volume used in Translation and Rotation Check . . 634.22 A series of coordinate frames 664.23 Different robot configurations 724.24 Flowchart for the Midpoint Algorithm 745.1 PathList data structure 77viii5.2 The procedure for searching for collision free swept volume (a) The 0swept volume collides with environment, (b) Shrinking 0 swept volume forthe collision free 0 swept volume, (c) The swept volume collides withenvironment, (d) Search -y swept volumes for the collision free 7 sweptvolume 795.3 The optimal orientation of robot tool 835.4 The automatic robot welding workcell 845.5 Procedure for searching for collision free paths 875.6 Procedure for searching for collision free paths for butchering 895.7 The automatic fish butchering workcell 90C.1 The PUMA 560 robot and robot tool 109ixList of Symbolsa the angle rotating around y axis;3 the angle rotating around z axis;the angle rotating around tool axis;a0 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;/‘3min the minimum 3 angle in Local C-space;/3max the maximum /3 angle in Local C-space;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 WordsLocal 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 range the range of Local C-space in 7 axis.xo 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 region the projective region of cell on a — /3 plane.7 range the projective range of cell on axis.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.intermediate curve point the intermediate point selected from the curve segmentbetween successive curve points.intermediate track point the intermediate point selected from the track segmentbetween successive track points.intermediate number the number of intermediate points.overlap-cell,+i the overlapping space of Local C-spaces at point pj and Pi+1.overlap-regioni the projection of overlap-cell,1on a-fl plane.free-cel4,+i the space inside overlap-cell,+i, associated with a collision freeswept volume.free-region,i the projection of free-cel4,1+i on a-fl plane.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 regionsxishrink 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’AcknowledgmentI would like to thank my supervisor, Dr. R. G. Gosine, for his guidance in developingthe motion planning algorithm. This work has been undertaken with the support ofthe 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 withAutoRoboWeld, 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 planningrequirements for welding applications.XIIIChapter 1Introduction1.1 MotivationWhen using a teach pendant to program robots for contour following applications such aswelding, cutting or painting, the operator must manually manipulate a robot tool alonga desired curve. It is generally agreed that this process is slow and demanding on theuser, and an automatic process to aid in motion planning is necessary. A typical weldingrobot workcell is illustrated in Figure 1.1. Safe and smooth motion of the robot and thetool is expected, and good process quality is significant in such applications. Using arobot 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 toolin order to achieve good process quality. The primary contribution of this thesis is thedevelopment 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 distancefrom the curve.2. The optimal orientation, as specified by the process, is relative to the tangent ofthe curve and the normals of two surfaces at the each side of the curve as shown inFigure 1.2. This orientation v&ies with the changes to the curve ud the supportingsurfaces. The robot tool has to maintain the optimal orientation within a giventolerance.1Chapter 1. Introduction 2seamtool PUMA 560 robotobstacleFigure 1.1: The robot welding workcellChapter 1. Introduction 3normal of surface 2optimal orientationFigure 1.2: The optimal orientation of robot tool3. The robot tool must avoid collisions with the curve and the obstacles around thecurve.4. The position and orientation of robot tool result in a set of robot joint angles whichdo 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 tolerance. These tolerances are specified by the application in order to maintain acceptableprocess quality.Collision avoidance is a basic requirement for tool motion. Within the allowable andcollision free orientation range, some tool orientations may lead to invalid robot inversekinematics solution (the joint angles in the solution violate the joint limits). It is difficultfor a human operator to select orientations which lead to valid solutions.The motion planner must maintain a path history in order to provide smooth motionof the tool. Changes of robot configuration will also reduce process quality because ofsurface 1 tangentsurface 2Chapter 1. Introduction 4discontinuous robot motion. The robot configuration, however, may have to change asthe tool travels a curve. It is difficult for the human programmer to pre-judge suchchanges and prevent them by selecting alternate robot configuration at the beginning ofprocess.It is impossible for the human programmer to consider all of these constraints simultaneously while using the teach pendant to program the robot. Programming the robotmotion becomes difficult and the process quality is compromised.The subject of this research is to to plan the motion of the robot tool automaticallyand meet the above requirements.1.2 Midpoint Algorithm: a Novel Approach to Motion PlanningA more intelligent robot tool trajectory planner is required and this thesis proposes suchan algorithm, the Midpoint Algorithm, to plan the trajectory of robot tool tracing a curvesubject to process constraints. A safe and smooth robot tool trajectory is selected bythe algorithm, which is based on the configuration space approach [Loza 83], to providehigh process quality. Since a curve is represented by discrete points, the motion of thetool along the curve links the motion between successive curve points. By studyingthe configuration spaces at successive curve points and the relationship between theconfiguration spaces, the algorithm assesses the locations of obstacles in the vicinity ofthe curve and the curvature of the curve in work space, finds the collision free path andplans an optimal trajectory for the tool. If more than one paths exists, an A* searchoptimizes the path. A polyline approximating the curve is used as a reference for thetool tip and the algorithm varies the step size between points on the polyline accordingto the condition of the curve and the environment. A large step size is used to speed upthe path planning if the curvature of the curve or the likelihood of collision is low, andChapter 1. Introduction 5a small step size is used for more intricate robot motion if the curvature of the curve orthe likelihood of collision is high. In order to assure that the orientation of the robot toolis within the allowable orientation range as it moves along the curve, exploration of thespace around the optimal orientation at each curve point is necessary. The configurationspace represents the interaction of the robot and environment at each curve point. Apath is found through the configuration space using an A* search for possible solutions.An off-line simulation program, TRAJPLAN, implements the algorithm in order todemonstrate the application potential of the work. Examples based on the PUMA 560and CRS A460 robots have been developed and tested on an real robot.1.3 Outline of Thesis ContentThe remainder of this thesis is arranged as follows. Chapter 2 reviews previous relatedwork in the areas of robot motion planning based on the configuration space, path planning for robotic tools, curve representation, and search techniques. Chapter 3 details thebasic concepts behind the Midpoint Algorithm, while Chapter 4 provides details of theMidpoint Algorithm. Chapter 5 describes the simulation program TRAJPLAN and itsapplication to typical planning problems. Finally, Chapter 6 presents conclusions for thiswork and suggestions for future work.Chapter 2Literature ReviewTwo general path planning problems are involved in the motion planning of a robot tooltracing the specified curves: collision avoidance and the motion along the curve.2.1 A Basic Motion Planning Problem: Collision AvoidanceRobot 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 tofind a collision free path for the robot. This problem is to search for a path from an initialposition and orientation to a goal position and orientation of robot. The path specifies acontinuous sequence of positions and orientations of the robot avoiding contact with theobstacles.If the obstacles are static and the locations of obstacles in the work space are available, a popular robot path planning method is the configuration space approach. Thisapproach is initialized by Udupa [Udup 77] who proposed the motion planning amidstobstacles and described the idea of using an appropriate space and shrinking the robot toa point in the space. This idea is exploited by Lozano-Pérez and Wesley [Loza 79]. Theyproposed a path planning algorithm for polygonal and polyhedral robots and obstacleswithout 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, suchas roadmap, cell decomposition and potential field, are discussed in Section 2.2.Dynamic motion planning [Cann 88] [Lato 91] [Reif 85] deals with moving obstacles6Chapter 2. Literature Review 7in work space. A continuous function of time is used to specify the robot configurationat each instant. In effect, a dimension representing time is added to the conventionalconfiguration space. In this configuration-time space, the robot is shrunk to a pointmoving among stationary obstacles. Since no object can move back in time, the pathfound is monatomic in time. Constraints on robot velocity and acceleration have to beconsidered.In the presence of multiple robots in the work space, one of the motion constraintsis 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 andplans the coordinated paths in the space. Some general path planning methods such ascell decomposition and potential field can be used to plan a path in the composite configuration space. Another approach to this problem is the decoupled planning method whichplans the motion of each robot independently of the other robots and then considers theinteractions among the paths. There are two decoupled planning approaches: prioritizedplanning and path coordination. Prioritized planning [Buck 89] considers the motions ofall robots, one robot at a time. The path of a robot is generated in a configuration-timespace of the robot at each iteration, avoiding the collisions with both the obstacles andthe other robots. The motion of the robot is planned as if the robot was moving amongstationary obstacles and some moving obstacles (other robots). To arrange the order ofrobots to be planned, a priority is assigned to each robot. Buckley [Buck 89] assignedpriorities to the robots which move in straight line from their initial configurations totheir goal configurations. Path coordination [Odon 89] is applicable when the planningproblem involves only two robots. A free path for each of the two robots is generatedindependently of the other robot. The two free paths are coordinated in order to avoidthe collision between the robots.Another general approach to solving path planning is to search the free space directlyChapter 2. Literature Review 8without first transforming the problem to configuration space. Kambhampati and Davis[Kamb 86] proposed a path planning approach based on a Quadtree representation. Theydescribed the hierarchical path-searching methods which make use of multiresolution representation 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 obstacles and creates a one-dimensional roadmap of the free space of a robot. Their methodtakes advantage of the fact that the closest features (vertex, edge, or face) change onlyinfrequently as the objects move along finely discreted paths. Gupta [Gupt 90] describeda method of planning the motion of each link of a manipulator successively, initializingfrom the base link. The n-D problem for an n-link manipulator arm transforms to one1-D and n — 1 2-D planning problems. This 2-D motion planning problem is to plan themotion of a single link which has one end moving along a fixed path determined by themotion of the previous links. An interesting collision avoidance algorithm is reported byShaffer and Herb [Shaf 92]. They described a data structure and data structure updatealgorithm for a real-time collision avoidance safety system. The N-objects octree is usedto index a collection of 3-D primitive solids which make up the robots and obstaclesin work space. As robots move, the octree is updated and the octree nodes recursivelydecompose 3-D space into eight equal cubic octants until each octant contains less thana predetermined number N of primitives. Since a given primitive does not change whichoctree nodes it is in during most update cycles, the octree is rarely modified. The systemprovides the information about possible collision.2.2 Path Planning Methods Based on the Configuration Space ApproachThe configuration space method [Udup 77] [Loza 79] [Loza 81] [Loza 83] [Loza 87] shrinksthe robot as a point in robot configuration space and maps the obstacles in this space.Chapter 2, Literature Review 9The motion planning of a point instead of a dimensioned object is more explicit. Theconfiguration space represents the robot and obstacles in a coordinate system defined bythe joints of the robot. There exists a large number of methods of solving the motionplanning problem based on the concept of configuration space. Three general approachesare roadmap, cell decomposition, and potential field [Lato 91].The roadmap approach represents the free space in the form of a network of one-dimensional curves, called roadmap, which is used as a set of standardized paths. Pathplanning becomes a search for three path segments: the path segment connecting theinitial configuration to the roadmap, the path segment contained in the roadmap, andthe path segment connecting the roadmap to the goal configuration. The visibility graphmethod, Voronoi diagram method and freeway method are all based on this idea. Thevisibility graph method [Nils 69] is suitable to a two-dimensional configuration space withpolygon configuration obstacles. The nodes of the graph are the initial and goal configurations and the vertices of the configuration obstacles. Two nodes are connected by astraight line segment if the line does not intersect the interior of the configuration obstacles. The path is found by searching for the shortest path in the graph. The Voronoidiagram method [Taka 89] generates the Voronoi diagram which is in the free regions ofconfiguration space. The Voronoi diagram is the locus of points which are equidistantfrom two or more obstacle boundaries including the work space boundary. The advantage of this diagram is that it produces free paths which tend to maximize the clearancebetween the robot and the obstacles. The freeway method [Broo 83(a)] plans the pathfor a polygonal object (robot) translating and rotating in a two-dimensional space. Ais straight 1ineor gicriied cylinder. The straight a.is of the cylinder calledthe spine, is the path of the reference point of the robot. A free space is represented by afreeway net. In a freeway net, if two spines intersect, the robot can transfer from one freeway to the other if the ranges of free orientations of the robot along both splines have aChapter 2. Literature Review 10non-empty intersection at the intersection point. The freeway net is a representation ofthe possible motion of a robot along spines and between spines.The cell decomposition method decomposes the robot free space into some simpleregions, called cells. A graph representing the adjacency relation between the cells isconstructed by extracting the cells from the free space and connecting two graph nodesif these cells are adjacent. The outcome of the graph search is a sequence of cells and apath is extracted from this sequence. There are two approaches in this area: exact celldecomposition and approximate cell decomposition. The exact cell decomposition methodChaz 87] decomposes the free space into trapezoidal and triangular cells, representsthe adjacency relation between the cells by constructing the connectivity graph, andsearches for a path in this graph. The cells used in the exact cell decomposition methodare required to have a simple pre-specified shape, e.g. a rectangle. Such cells do notprovide 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. Acell is recursively decomposed until a predetermined resolution is attained, or the cell liescompletely in free space or in the configuration obstacle. The path is searched throughthe cells lying in free space. Since the shape of a cell is relatively insensitive to numericallyapproximate computations, this method is usually much easier to implement than theexact cell decomposition method.In the potential field method [Khat 86], the robot is represented as a particle inconfiguration space. The goal configuration generates an “attractive potential” whichpulls the robot toward the goal, and the configuration obstacles produce a “repulsivepotential” which pushes the robot away from them. Robot moves under the influence ofan artificial potential produced by the goal configuration and the configuration obstacles.Based on the configuration space approach, most studies about collision avoidancefocus 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 theproblem 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 CurveThe principles and techniques of a high-speed spatial seam tracking system are describedin [Boll 71]. A five degree of freedom seam tracer is developed, which positions a weldinggun to maintain a fixed relationship to a weld line. The proper angular relationships areachieved by utilizing velocity information from three tachometers mounted on the threerectangular coordinate drives. Collision avoidance is not considered in the system, andthe seam discussed in the paper is constrained to lie on a cylindrical surface.Tomizuka, Dornfeld and Purcell [Tomi 80] discussed the characteristics of the gasmetal arc welding process and the relationship between welding parameters, the desiredoutput of the welding process, and the automation of the process. A strategy for two-axiswelding torch positioning and velocity control is developed. The torch positioning andvelocity control is achieved by keeping the relative position error between the seam andtorch small and maintaining uniform tracking speed. In this approach, the algorithmconstrains the seam to a plane and only two-axis control is available. Collision avoidanceis not considered.An algorithm for tracing a seam in real time is developed in [Khos 85]. The seamtracking control traces a curve in 3-D space and maintains proper tool orientation withrespect to the surface that contains the curve. The curve lies on a surface and is discretized lengthwise. The discretization provides a piece-wise linear approximation of thecurve between two adjacent sample points. The tool orientation is obtained by discretizing the surface in the vicinity of the two sample points, and this is held constant whenChapter 2. Literature Review 12the tool travels between the two points. The position of the torch is specified by thecoordinates of the sample points. Three unit vectors are used to describe the tool orientation relative to the base frame but the rotation about tool axis is not discussed. Thismay be suitable for the CYRO robot, a special robot with three revolute joints and threeprismatic joints, but is not suitable for robots like the PUMA, CRS or GMF with whichthe rotation about the tool axis changes the position of the robot end-effector if the toolhandle is held by the robot. The allowable tool orientation range and the optimal toolorientation subject to process constraints are not discussed in the paper. The samplepoints are used directly in seam tracking regardless of the condition of the curve and theenvironment around the points. This reduces the accuracy along complex curve segmentsand wastes time along simple curve segments.A trajectory planner and a method for interference detection between objects in arobot welding workstation are presented in [Buch 89]. The trajectory planner searchesfor a kinetically feasible, interference-free robot trajectory in order to follow a specifiedweld path in 3-D space. The algorithm provides rapid interference detection betweenconvex polyhedras using a steepest descent graph search. If desired, exact separatingdistances and directions can be found. Interference-free trajectories that do not violatekinematics constraints are found by searching possible inverse kinematic solutions for thedesired welding torch trajectory. This approach only deals with tool rotation about itsaxis, but not the allowable tool orientation range. This paper and the associated workform 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 angleacceleration of the end effector which holds a robot tool and travels along a continuouspath. The technique proposed allows the trajectory-planning engineer to specify theorientation of the end effector via the orientation of the path at a given point. The latteris defined as that of the orthonormal triad of vectors associated with the curve, namely,Chapter 2. Literature Review 13its tangent, normal, and binomial vectors. The angular acceleration of the end effectorrequires the computation of the derivatives of the curvature and torsion with respect tothe 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 andacceleration of a body tracking the curve with a given speed, whose relative orientationwith respect to the curve remains constant, or a given function of time.2.4 The Spline of CurveCurve representation is an important aspect of this research, and many algorithms forcurve 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 littleflexibility in controlling the shape of the curve, and higher-degree polynomials can introduce unwanted wiggles and also require more computation. The spline is a piece-wisecontinuous 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 fewcontrol points and movement of a control point affects only a small part of a curve. Thisis called local control. B-splines, however, do not interpolate the control vertices. Inthis research, a curve representation based on some sample points must be reconstructedand the reconstructed curve has to pass through the sample points. The B-spline is notsuitable in this case.One member of the Catmull-Rom Spline [Catm 74] [Barr 89] family interpolates thepoints P1 to Pm1 from the sequence of points Po to Fm. In addition, the tangent vectorat point P is parallel to the line connecting point P_1 and F+i, as shown in Figure2.1. The Catmull-Rom Spline interpolates control points, shares many properties suchas global smoothness and local control with B-spline curves, and has continuity of theChapter 2. Literature Review 14. P9Figure 2.1: A Catmull-Rom splinefirst 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. TheCatmull-Rom Spline method is suitable in this research.2.5 Searching Path Through a NetA further requirement of the research is an algorithm for evaluating potential tool pathsand for guiding the search through path options. Procedures for finding the shortestpath through a net are surveyed in [Wins 84]. The British Museum Procedure finds allpossible paths and select the best from them. Since this method looks everywhere and isinefficient, it is not suitable for our path planning where search time is of concern. TheBranch-and-Bound search extends the search tree in the direction of the “best” partialpath. The A* search improves the Branch-and-Bound search by including an estimate ofremaining distance, combined with the dynamic-programming principle. If the estimateof remaining distance is a lower bound on the actual distance, then the A* search achievesthe optimal solution. The A* search is used in the proposed algorithm to search multiplepaths along the curve and to select the optimal trajectory from the tool configurationsin the path.P1p0p2p5p4p6Chapter 2. Literature Review 152.6 Conclusion Based on Literature ReviewIn view of this literature survey, it is concluded that little work has been done on theproblem of robot tool planning along a desired trajectory of the tip of the tool. Furthermore, conventional planning techniques have been largely concerned with point-to-pointproblems and did not consider application constraints associated with the required motion of the tool. The configuration space method appears promising in terms of a generalapproach, however, the proposed method studies the relationship between configurationspaces rather than searching for paths inside a single configuration space. For the ancillary functions, such as for effective curve representation and search procedures, existingtechniques will be employed directly in this thesis.Chapter 3Configuration Space of Robot Tool3.1 Configuration of Robot ToolIn this section the representation of the position and orientation of the robot tool isdiscussed. The foundation for this representation is a 3-D coordinate system, and Figure3.1 illustrates the tool representation, in this case for a welding torch, at the origin ofthe 3-D coordinate system. Note that all objects in the environment are described withrespect to this 3-D reference frame. As illustrated in Figure 3.2, the tool has six degreesof freedom, x, y, z, a, /3 and y, where x, y and z are the position coordinates of the tooltip, a is the angle of rotation about the Y axis, /3 is the angle of rotation about the Zaxis, and ‘y is the angle of rotation about the tool axis. The tool axis passes though thetool tip and represents the critical orientation of the tool, as shown in Figure 3.1. Theconfiguration of the tool in the base coordinate system is determined by rotating thetool to an orientation (a,/3,7) and translating the tool from the origin to (x,y,z) whilemaintaining this orientation. As mentioned in Chapter 1, the six requirements as the tooltravels a curve include (1) the distance between the tool tip and the curve, (2) the optimalorientation, (3) collision avoidance, (4) robot joint constraint, (5) robot arm inversionavoidance and (6) smooth motion. The distance between the tool tip and the curve pointcan be adjusted easily by changing the tool tip position (x,y,z). The tool orientationrequirement specifies the tool orientation in the direction of the optimal orientation. Thecritical tool orientation is determined by the a and /3 angles. The initial position of tool16Chapter 3. Configuration Space of Robot Tool 17Vin Figure 3.2 (a) shows the tool orientation where a, and y are all equal to 00. Thereference orientation of the 7 angle is along the negative Y axis. Although it does notconstitute a tool orientation constraint, the ‘y angle is very important in the calculationof the robot inverse kinematic solutions since a small change in 7 can result in a largedifference of the position and orientation of the robot end effector and the associatedjoint values for the robot as illustrated in Figure 3.2 (b). As discussed in Chapter 4, theangle is critical in the collision-free path search.3.2 Configuration Space of Robot ToolAn alternative representation of tool orientation is the configuration space (C-space), andhere two new concepts, Entire C-space and Local C-space, are introduced. The EntireC-space is the configuration space with its origin at a, /3 and equal to 00. The ranges ofthe a, /3 and 7 angles in 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 pointin the work space. The a and 3 ranges of Entire C-space correspond to the sphere inztool axistool headFigure 3.1: Robot toolChapter 3. Configuration Space of Robot Tool 18(b)robot toolposition 2robot toolposition 1V4— robot end effectorzrobot toolInitial position(a)zV(x,y,z)Figure 3.2: Definition of robot tool configurationChapter 3. Configuration Space of Robot Tool 19Figure 3.3: The a and /3 ranges of Entire C-space and the allowable orientation range atdifferent curve points. (a) Entire C-space. (b) Allowable orientation range.zzopilmal orientationEnlire C-spacerangexyyI+1Curve point Ipoint(a)zallowed orientationrangeCurvexVyCurve point iCurve point 1+1(b)Chapter 3. Configuration Space of Robot Tool 20180Local Cspaceat point 1work space as shown in Figure 3.3 (a). Using the Entire C-space directly in trajectoryplanning is not practical since a high resolution Entire C-space would require a largedata structure, which would waste time and space as a result of creating, searching andstoring much useless information in the data structure. Due to process constraints, suchas allowable deviation in tool orientation from a user-specified ideal orientation, only partof 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. Similarlythere will be optimal cutter orientations for an automatic butching operation. The acceptable orientations covers only a subset of the Entire C-space, as illustrated in Figure3.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 orientation for a particular application. Since the acceptable range of deviation is generallyEntire Cspace13360LocalYlocal0 region360Figure 3.4: Entire C-space and Local C-spaceChapter 3. Configuration Space of Robot Tool 21much smaller than the range in Entire C-space, the Local C-space requires a condenseddata structure in order to represent the configuration space with the same resolution asthe Entire C-space.With reference to Figure 3.4, the Entire C-spaces at different curve points have thesame a and 3 ranges, i.e. [0,180] and [0,360] respectively. The a and /9 ranges of LocalC-space are different and correspond to the different regions in the a — 3 plane. Theprojections of Local C-space on the a — /3 plane and the axis are called the local 0 regionand the local range respectively.In order to visualize the approximate shape of the local 0 region, the bounding boxof the region is studied. In Figure 3.5 (a), the maximum allowable deviation of toolorientation from the optimal orientation is 00 and the optimal orientation is (a0, /3).The amin, amax, /3min and /3maa, angles in Figure 3.5 (b) (c) (d) are the minimum andmaximum a and /3 angles in the local 0 region respectively. In Figure 3.5 (a), points 1,2, 3 and 4 are at the boundary of the cone centered at (a0, Figure 3.5 (b) illustratesthe viewing direction in the negative optimal orientation. The line connecting point 2 topoint 4 is perpendicular to the line connecting point 1 to point 3. Figure 3.5 (c) and (d)illustrate that /3mim and /3max are not the /3 angles at point 2 and point 4, while amin andamar correspond to the a angles at point 1 and point 3 respectively. Note that amjn andamax are equal to a0 — 0 and a0 + 00 respectively. The a angles at point 2 and point 4are equal but they are not equal to a0. The corresponding local 0 region has an irregularshape as shown in Figure 3.5 (e).From the above discussion, it is clear that the local 0 region is hard to representusing a simple geometric model, such as a rectangle, circle or ellipse. With reference toFigure 3.6, equations defining the local 0 region are derived. In the figure, the sphererepresents an Entire C-space, which has origin or and radius p. The cone represents thelocal 0 region of Local C-space. The intersection of optimal orientation and the sphere isChapter 3. Configuration Space of Robot Tool 22viewing direction Inthe negative z axisyviewing direction in thenegalive oplimal orientation(b)The circle containsthe same a angle oplimal orientationx2 43(a)moxmmCc)mmviewing directionto z-o-13o planeperpendicular(d)amln a0 a2 amox aThe a and range of Local C-space(e)Figure 3.5: The shape of local 0 regionChapter 3. Configuration Space of Robot Tool 23Local C-space rangeVxdenoted as o, p is a random point within the Local C-space and located on the sphere,and 0 is the angle between 0r and orp, where 0 0 0. The projections of o and pinto 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 (cv0, ,6) and themaximum allowable orientation deviation 00, the calculation of the corresponding local 0region is now described.Using the cosine law in triangle OrOP2p — 2pcos0 = r2 (3.1)Using the cosine law in triangle oro’p’p2sin2a + p2sin2a0 —2psinasinacos(/3—= r’2 (3.2)Since pq is parallel to p’o’ and oo’ is perpendicular to p’o’, the triangle opq is a righttriangle, andoptimal orientationFigure 3.6: The calculation of local 0 region of Local C-spacer’2 + (pcosa0 — pcosa)2 = r2 (3.3)Chapter 3. Configuration Space of Robot Tool 24Combining Equations (3.1), (3.2) and (3.3),2p — 2pcosO = p2sin2a + p2sin2a0 —2psinasinacos(/3— i3) + (pcosa0— pcoscx)2 (3.4)which may be reduced tocosO = cos(a + a0) + [cos(/3 — /3) + ljsinasina0 (3.5)by the following steps,from Equation (3.4),2 — 2cosO = sin2a + sin2a0 — 2sinasinacos(/3— /3c,) + (cosa0 — cosa)22 — 2cosO sin2a + sin2a0 — 2sincrsinacos(/3 — /3) + cos2a0 — 2cosa0cos + cos2a2 — 2cosO = 2 — 2sinasincr0cos(/3— i3) — 2cosa0coscosO = sinasina0cos(/3 — i3) + cosa,cosacos(O) = sinasina0co.s(f3 — 3) + cosa0+ sinasina0— sinasina0= —sinasina0+ coscr0sa + sinasina(co (i3 — /3) + 1)= cos(a + at,) + [cos(/3—/3,) + 1]sincrsina,Since 0 0 0,1 cosO cos00and from Equation (3.5), we can write,1 cos(a + a0) + [cos(3 — i3) + llsinasina0 cos(00)1 — cos(a + a0) [cos(/3 — f3) + ljsinasincx0> cos(00)— cos(a + a0) (3.6)Next, consider the left hand side of Equation (3.6). Since 1 cos(/3—2 cos(/3—/3,) + 1Chapter 3. Configuration Space of Robot Tool 25Since both c and cr are in the range [0, 180], andsinasincr0 0we can write,2sincsinc0 [cos(/3— j9) + 1]sincvsincx (3.7)Since 1 cos(c — c), we know1—cos(a—o0) 0and1 — (coscco.sa0+ sinasina0)+ 2sinasino0 2sinasinoor1 — cos(c + cr0) 2sinasinac, (3.8)Combine Equation (3.7) and Equation (3.8)1 — cos(a + c,) 2sinasinc0 [cos(f3 — 3) + 1]sincsinc0The left hand side of Equation (3.6) is always satisfied. Now, consider the right handside of Equation (3.6).Since both a and a0 are in the range of [0, 180],sinusina0 0and[cos(3—3) + 1]sinasirta0 0 (3.9)First consider the case whencos(00)— cos(a + a0) 0Chapter 3. Configuration Space of Robot Tool 26We can write,[cos(/3— /3) + 1]sinc.sina0 cos(00)— cos(a + a0)The right hand side of Equation (3.6) is always satisfied, and fF3 can be any value in therange [0, 360].The conditioncos(00)—cos a+a 0requires0> a+aora 00—Since a is defined as positive,a0 Ooand if a0 0 and a is in the range [0°, a 00 — a0], /3 can be any value in range [0°,360°].Next consider the case whencos(00)— cos(a + a0) > 0We get0<a+a (3.10)Since a is defined as positive, from Equation (3.10),0—a <0a0 > 00Chapter 3. Configuration Space of Robot Tool 27With reference to Figure (3.5) (a),amin = a0— 90> 0Since both a0 and a are greater than 00, from the right hand side of Equation (3.6) wewrite,[cos(/3—j3) + 1]sinasina0 cos(00)— cos(a + a0)Since both a0 and a are greater than 00,.sinasina0> 0cos(/3— /3) > cos(90)— cos(a + a0)— 1— sinasina0cosO0— cosa0cos(f3—i3)sznaszna0cosO0— cosa0Ii — /I <arccos( . ) (3.11)sznaszna0wherecos90— co.sa0 (3.12).sznaszna0Equation (3.12) can be rewritten as,—sina.sina0 cos90 — cosa0 sinasina0cosa0— .sinasina0 cosO0 < coscr0sa + sinasina0co.s(a + a0) <cosO0 <cos(a — a0) (3.13)Since angle 9 is the maximum a deviation from a0,Oo a— a01and the right hand side of Equation (3.13) is always satisfied. The left hand side ofEquation (3.13) requiresa+a0 OoChapter 3. Configuration Space of Robot Tool 28360Entire Cspacexcx03(a)1304 P01,01.03 P05viewing direction in viewing direction innegative z axis negalive y axis(b) (C)—-7PrP05 P2’133’Po1,01,o31331304 1321310cJJfJT+\J0Local regionoptimalorientationaoe 03a01 a02 a03(d)-180a05Figure 3.7: Entire C-space and local 0 regionChapter 3. Configuration Space of Robot Tool 29which 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 boundariescorresponding to the a angle on the local 0 region. The local 0 region is obtainedby calculate the /3 range for every a value as a increases from (a,,- O) to (a0 +O) with step size 10.2. If a0 0, the orientation where a equals 00 is included in the local 0 region.(a) If a is in the range [0, cr0-0], the 3 is the full range [0, 360].(b) If a is in the range [ao-00,a0+0], the /3 range is calculated with Equation(3.11).In Equation (3.11), the 3 range of the local 0 region depends on a0, /30 and 0. Thegeometric representation of a local 0 region is given in Figure 3.7. Figure 3.7 (a) illustratesthe local 0 regions for different a0 and /30 angles. Figure 3.7 (b) illustrates that the /30angle sequence is: /34 < /3o2, j3 </3,s, and different /9 angles correspond to different/3 range in local 0 region. Figure 3.7 (c) illustrates that the a0 angle is in the order of:a01 <a02 <a03 < a0 <a05. The a range is 20 and does not vary with a0 for differentlocal 0 regions. With reference to Figure 3.7 (d), the closer the a0 to 90°, the smaller the/3 range. A smaller a0 angle, however, does not change the a range. Different a0 angles,therefore, produce different position, size and shape of local 0 region, while differentangles 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 regionhas different shape from those discussed above. In Figure 3.8 (a) cone a is divided bythe plane where 3 is equal to 00, and the corresponding 0 region is divided into two partsChapter 3. Configuration Space of Robot Tool 30axcontains 13a angle 360y Ibo00Entire Cspace90(b)18000 OoO(C)f3360“( equal to 0position(a)Entire CspaceIbo180 00(d)a180Figure 3.8: The local 0 region of Local C-space in the boundary of Entire C-spaceChapter 3. Configuration Space of Robot Tool 31denoted by region a in Figure 3.8 (b). Cone b includes the orientation with both a and3 equal to 00. Tn this case, a0 is less than 00 as discussed above. With reference toFigure 3.8 (c), The corresponding local 0 region consists of two parts: (1) the a range is[0, a0-0] and 3 range is [0,360], and (2) the a range is [a0-0,a0+0] and the /3 range iscalculated with Equation (3.11). The special case of a0 equal to 00 is illustrated in conec, 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 00 position of the -y angle is defined at the negative Y axis when both aand /3 are at 00 position, refer to Section 3.1, the 00 position of 7 angle always representsthe tangent of a circle on the sphere. The circle is determined by the a angle, as shownin Figure 3.8 (a).Chapter 4Description of the Midpoint Algorithm4.1 Overview of the Midpoint AlgorithmThe 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 orientation range of the tool head. Robot constraints (joint limits), motion constraints (simultaneous translation and rotation which provides for smooth motion), collision avoidance,and the allowable distance between the tool tip and the curve are also considered withinthis search. Second, the path segments between successive points are linked togetherusing an A* search to generate a continuous path along the curve. Third, tool configurations along the path are selected using a second A* search in order to build theoptimal trajectory which avoids robot configuration changes. Acceptable process qualityis maintained along this trajectory.4.2 The Description of the Midpoint AlgorithmGiven a curve with sufficient curve points to represent the curve shape, the trajectory isplanned as follows,STEP 1.0 Reconstruct the curve using a Catmull-Rom spline in order to get curve pointinformation at any curve position. Details of this step are presented in Section 4.2.1.32Chapter 4. Description of the Midpoint Algorithm 33STEP 2.0 Use the Point Check procedure to identify collision free regions at the firstcurve point. Store the free regions in a cost-ordered linked list (called path list), anduse each free region as the beginning of a potential path along the curve. Details ofthis step are discussed in Section 4.2.2, and the cost function is described in Section4.2.7.STEP 3.0 Extend the lowest cost path in the path list along the curve by exploring thepath segment between successive points in the following steps,STEP 3.1 Curvature Check: Check if successive points are close enough to represent 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 successive points. This ensures that the trajectory remains within the allowabledeviation 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 translateand rotate along the path segment without collision. The details of this stepare discussed in Section 4.2.5. If this check fails and the step size betweencurrent two points is larger than the minimum step size1, add a midpoint andreturn to STEP 3.1. Otherwise, the failure in finding a path is reported touser and trajectory planning stops.STEP 3.4 Inverse Kinematics Check: Check the validity2 of robot inverse kinematic 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, deletethe current path from the path list. If the path list is empty, the trajectory1The minimum step size is predetermined in order to avoid the infinite exploration of a path segment.2A valid inverse kinematic solution does not violate robot joint limits.Chapter 4. Description of the Midpoint Algorithm 34planning 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 inthe 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 successivecurve points. Each combination of a, 3 and -y is the tool configuration that may beincluded in the final trajectory. An A* search is used to select the configurationsin the path in order to obtain the optimal trajectory along the curve. The detailsof this step are discussed in Section 4.2.8.A fundamental component of this algorithm is the addition of midpoints betweensuccessive curve points as the search progresses. The reason for adding midpoints is thatif successive points are close enough, the straight line motion of the tool tip betweenthese 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 atsuccessive points will share collision-free space. As a result, there will be translation androtation 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. Forexample, in Figure 4.1 (a), the curve, the optimal orientation and the environment donot change significantly, and the points pi and Pi+i are considered as close enough for theprocess. In Figure 4.1 (b), the environment and the optimal orientations do not changesignificantly but the curve changes sharply. In Figure 4.1 (c), the environment does notchange but the optimal orientations and the curve changes sharply between points pj andIn Figure 4.1(d), the curve and the optimal orientations do not change much butthe environment changes sharply. In case (b), (c) and (d), the two curve points p. andare not close enough for the motion planning process. A smaller step size is requiredChapter 4. Description of the Midpoint Algorithm 35_pi pi+1(d)Figure 4.1: The relation between step size and the condition of environment and curveto explore the path in Figure 4.1 (b), (c) and (d). A larger step size, however, tends tospeed up motion planning in Figure 4.1 (a). The insertion of midpoints between p andPi+1 controls the step size.A midpoint is added in the way that the location of the midpoint is on the curvemidway between two curve points. The optimal orientation at the midpoint is determinedas described in Section 1.1 for determining the optimal orientation at the original curvepoints.4.2.1 Reconstruct the Curve by Catmull-Rom splineA Catmull-Rom spline [Catm 74] is used to reconstruct a curve based on the samplepoints 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 manyproperties with B-splines, such as global smoothness and local control. One member ofoptimal orientationP curve P+i(a) (b)pI+1Cc)Figure 4.2: A Catmull-Rom splineCatmull-Rom spline family is able to interpolate the points P1 to Fm1 from the sequenceof points Po to Pm. The tangent vector at point F: is parallel to the line connecting pointP_1 and P:+i, as shown in Figure 4.2.Catmull-Rom spline uses parametric representation of curves. Here a curve is approximated by a piece-wise polynomial curve. Each segment Q(t) of the overall curve isgiven by three functions, x y = y(t), z = z(t), which are cubic polynomials in theparameter t, (0 <t < 1). Given points j, Pi+1 and new point Pnew between pj and Pi+i,t is the ratio of the distance between pj and Pnew and the distance between p and Pi+1•Designating MCR as the Catmull-Rom basis matrix, G3 as the geometry matrix, and Tas the matrix of parameter t. With T [t3t2’l], the representation is11Q(t) T. MCR• G3 = [t3t2tll j—1 3 —3 12 —5 4 —1—1 0 1 002 0 0Where Pj3,P_2,P_1 and F: can be any of x, y, or z coordinates of the pointPi—3,Pi—2,Pi—1 and p, and Q(t) is the corresponding function for x, y or z along thecurve.p0Chapter 4. Description of the Midpoint Algorithm 36. p9p2p5p4p7p6Pi-3Pi-2Pi-’Pi(4.1)Chapter 4. Description of the Midpoint Algorithm 37alocal 0 region 0 region 1 rangeFigure 4.3: The definitions of cell, 0 region and rangeUsing 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 lastpoints.2. If the curve is open, the first and last points are used twice.4.2.2 Point Check for the First PointPoint CheckThe 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 forfree cells in the Local C-space. With reference to Figure 4.3, a cell is defined as the spacein Local C-space which projects as the 0 region on the c — /3 plane. The cell also projects360 Entire Cspacecell1360I rangeLocal CspaceChapter 4. Description of the Midpoint Algorithm 38as 7 range on the axis. If the corresponding swept volume of a cell is placed at the curvepoint 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 betweenthe configuration space and the work space. Each curve point is associated with a LocalC-space and each cell in the Local C-space is associated with a swept volume of the robottool. 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 andsizes as discussed in Chapter 3, the shapes of the corresponding swept volumes are thesame. 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 existingPC hardware.Two kinds of swept volumes are used in the check: the 9 swept volume and theswept volume which correspond to the 9 region and the 7 range respectively, i.e. the 8swept volume and the swept volume represent the space occupied by the tool head andthe tool handle respectively. In Figure 4.4 (a), placing the tool with the critical toolorientation parallel to the Z axis and rotating the tool ±9 angle (0 < 9 < 8) about Xaxis generate the region swept by the tool. The regions swept by the tool head and thetool handle are called the 9 swept region and the 7 swept region respectively. The 0 sweptvolume and the 7 swept volume are generated by rotating the 0 swept region and the 7swept region 1800 about Z axis as shown in Figure 4.4 (b). Figure 4.4 (c) and (d) providecross-sectional views of the 0 swept volume and the 7 swept volume. A set of 0 sweptvolumes with different 0 angles are generated as shown in Figure 4.5 (a). For each 0 sweptvolume, there is a set of corresponding 7 swept volumes associated with. With referenceto Figure 4.5 (b), the set of 7 swept volumes are generated by dividing the full range 7swept volume with two planes which are the tangent planes of the 0 swept volume. Theangle b between these planes is equal to where n = 1, 2, ..., and a binary tree dataChapter 4. Description of the Midpoint Algorithm 39cross-section of o swept volume cross-section of y swept volume(C)Figure 4.4: The swept volumes used in the Point Check. (a) The 0 swept region and theswept region. (b) The 0 swept volume and the 7 swept volume. (c) The cross-sectionof 0 swept volume. (d) The cross-section of y swept volume.z7Oswept volume0 swept region(a) (b)Cd)Chapter 4. Description of the Midpoint Algorithm 40structure is generated. Note that the full range 7 swept volume corresponds to n = 0.The cases of n = 1 and n = 2 is shown in Figure 4.5 (c). If the 0 swept volume contains Zaxis, i.e. c <00, the corresponding 7 swept volume and its decomposed swept volumesare always the full range 7 swept volume. The combination of a 0 swept volume and aswept volume is shown in Figure 4.5 (d).At a curve point, the corresponding 0 region of a 0 swept volume at a curve pointis computed based on the optimal orientation at the point and the 0 angle of the sweptvolume. 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 the0 region in order to search for the collision-free region (free 0 region), and (2) search thebinary 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 PointThe Point Check is applied at the first curve point in order to search for a free cell in thecorresponding Local C-space. Each free cell is the beginning of a potential path alongthe curve. The free cells are stored in the path list and sorted by the Path Cost of the freecell. 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 placedat the first curve point without collision. A high resolution representation of the LocalC-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, andcan lead to implementation problems. This detail is discussed in Chapter 5.Chapter 4. Description of the Midpoint Algorithm 41o swept volumes(a) ‘( swept volume(b)tangent plane ofo swept volume“( swept volumen=Oczd c:: E n=2‘ swept volumes(C)0 andY swept volumesFigure 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) Thecombination of a 0 swept volume and a 7 swept volumez0 swept volume(d)Chapter 4. Description of the Midpoint Algorithm 424.2.3 Curvature CheckSince a curve is represented by a set of discrete curve points, the motion of the tool alongthe curve is decomposed into the motion between successive curve points. Using a fixedstep size between the curve points may not be practical depending on the complexity ofthe 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 curvesegments where the curvature is low and the surrounding environment is simple. Usinga step size which is large, however, may result in the tool tip deviating from the requiredcurve along curve segments where the curvature is high. This will result in poor processquality, 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 thecurve and the environment is necessary.One of the advantages of the Midpoint Algorithm is that the step size is variedautomatically along the curve. If the curvature along a segment is low, a large step sizeis used to speed up the trajectory planning along the segment. Otherwise, a small stepsize is used to ensure acceptable translation of the tool along the curve. In general, themore complex the curve, the smaller the step size.Adjustment of the step size based on curvature is discussed in this section, whileadjustment 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 alsosearches a polyline which matches the shape of curve within a given tolerance and is usedas a reference for the tool tip. The polyline is generated by detecting if the straight lineconnecting successive points can represent the curve segment within the tolerance. Somepoints 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 curveChapter 4. Description of the Midpoint Algorithm 43optimal orientation• intermediate track points between track point ti and t2K intermediate line points between curve point p1 and p2Figure 4.6: Intermediate points selected from curve segment and line segmentsegment cannot be represented by the straight line between the points. Furthermidpoints are added until the curvature of the new curve segments are within anacceptable tolerance of the original curve.2. If there are several points on a flat section of the curve, some points are deleted inorder to speed up the search process.The tolerance discussed above corresponds to the two parameters, dmax and dmin, themaximum and minimum distances between the tool tip and the curve. Both of thesedistances are specified by the application. In most of the cases, dmin is greater thano to ensure that the tool does not collide with the workpiece. For each curve point, atrack point is generated that is positioned a distance dmax in the direction of optimaltool 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 curvepoints, 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 theline segment connecting the corresponding track points ti and t2. The number is calledthe intermediate number. The optimal orientations at the intermediate curve points arespecified as described in Section 1.1 for the original curve points. The following twotests examine if the line segment connecting successive track points is an acceptableChapter 4. Description of the Midpoint Algorithm 44representation of the required tool path.1. Vector Test: for each intermediate line point, if the angle between the optimalorientation at the corresponding intermediate curve point and the vector from thisintermediate line point to the corresponding intermediate curve point is greaterthan 90°, the Vector Test succeeds.2. Distance Test: for each intermediate line point, if the distance between the intermediate line point and its corresponding intermediate curve point are greater thandmin and less than dmax, the Distance Test succeeds.If one of the above tests fails, midpoints and corresponding track points must beadded until both tests succeed. The addition of midpoints is illustrated in Figure 4.7 andsummarized as follows,1. Since the Vector Test fails between intermediate line point l and intermediate curvepoint c1 in Figure 4.7 (a), midpoint m1 is added between curve points P1 and P2 inFigure 4.7 (b).2. Since the Distance Test fails between intermediate line point 12 and intermediatecurve point c2 in Figure 4.7 (c), midpoint m2 is added between curve points P1 andm1 as illustrated in Figure 4.7 (d)3. In Figure 4.7 (d), no midpoint is required between curve points P1 and m2, andbetween m2 and in1 since both the Vector Test and the Distance Test succeed inthese curve segments.4. Since the Vector Test fails between intermediate line point 13 and intermediate curvepoint c3 in Figure 4.7 (e), midpoint rn3 is added between curve points m1 and P2In Figure 4.7 (f)Chapter 4. Description of the Midpoint Algorithm 45Vector Test fails(a)p1(b)p2track of tool tipp1 p2Distance Test foiisCc)track of tool tipVector Test and Distance Test succeedCd)p2Vector Test foilsCe) Vector Test and Distance Test succeed(g)Figure 4.7: Adding midpoints using Curvature CheckChapter 4. Description of the Midpoint Algorithm 46p1p7(C)Figure 4.8: Deleting curve points using Curvature Check5. In Figure 4.7 (f), no midpoint is required between curve points m1 and m3 orbetween m3 and P2 since both the Vector Test and the Distance Test succeed inthese curve segments.6. The final track of the tool tip redefined by the Curvature Check is shown in Figure4.7(g).The Curvature Check is also used to delete curve points which are not necessary inthe curve segment where the curvature is row. If the Curvature Gheck between pj ndPi--i succeeds, the curvature between p and Pi+2 is checked. The number of intermediateline points and intermediate curve points selected must have the same density as thepoints specified by the intermediate number. For instance, if the intermediate number is(a)(b)p1Chapter 4. Description of the Midpoint Algorithm 47Figure 4.9: The relationship between track and constraints: dma and dmin3, three intermediate curve points and three intermediate line points are selected betweenp and Pi+i and their corresponding track points, and six intermediate curve points andsix intermediate line points are selected between pj and Pi+2 and their corresponding trackpoints, and so on. If the Curvature Check between p and Pi+2 fails, the Curvature Checkbetween pj and Pi+i finishes, and no curve point is deleted from the curve. Otherwise, thecurvature between p and Pi+3 is checked, and so on, until the Curvature Check betweenp and Pi+k fails. All the curve points between p and p+k1 are then deleted from thecurve and the path segment between pj and Pi+k—1 is planned directly. The deletion ofcurve 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 P1and P2, between Pi and p, between P1 and p4, between p and p, but fail betweenPi and P6, the curve points P2, p3 and p4 and the corresponding track points aredeleted.2. In Figure 4.8 (b), since the Vector Test and the Distance Test succeed between pand P6, between p5 and p, and p7 is the last curve point, the curve point P6 andits corresponding track point are deleted.3. The final track of the tool tip searched by Curvature Check is shown in Figure 4.8(c).dmax- drrinp1The precision of the Curvature Check is determined by the parameter dma, dmin andChapter 4. Description of the Midpoint Algorithm 48p15intermediate number s 2(a)intermediate number is 5(b)Figure 4.10: The intermediate number and the precision of Curvature Checkintermediate number. Since the upper and lower boundaries of the track of the tool tip arespecified by dmax and dmjm respectively as shown in Figure 4.9, a small dma and a largedmin 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 successivetrack points. Since the curvature change may not be detected by a small intermediatenumber, as shown in Figure 4.10 (a), and a large intermediate number as shown in Figure4.10 (b) is necessary. Since the distance calculations are computationally inexpensive, alarge intermediate number ( e.g. 10) is recommended.4.2.4 Overlap CheckSince the 7 ranges in the Local C-spaces are identical, the overlap discussed here is theoverlap between the 0 regions. The intersection of two 0 regions is determined througha 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 atoverlapping a value is found, such as regions 1 and 3, and regions 1 and 2 in FigureChapter 4. Description of the Midpoint Algorithm 49Figure 4.11: The search for overlapping region4,11 (b), the two 0 regions do not overlap. The intersection points of the boundary oftwo 0 regions are obtained by searching the a value which has the same /3 value at theboundary in both regions.If the 0 regions at successive points overlap, the curve segment between these pointscan be searched. As illustrated in Figure 4.12 (a) (b), a valid path cannot be found withinthe allowable orientation range between p1 and P2. If the points on a continuous curve arevery close, the optimal orientations should also be very close. Since the position of the0 region is determined by the optimal orientation, the addition of midpoints eventuallymakes the 0 regions at successive points overlap as illustrated in Figure 4.12 (c). If thestep 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 thetwo points. In this case, the optimal orientation and the position of the midpoint aredefined as the bisection of the orientations and the positions at the two points, andmidpoints are added until the 0 regions overlap. This process, which is illustrated in360 3600 0overappingx range a overiappingcx range(a) (b)aChapter 4. Description of the Midpoint Algorithm 50360180 P2 Ml 1800— c’. 090(b) (C)Figure 4.12: The effect of adding midpointFigure 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 theoptimal orientation at the midpoint of two curve points may not be between the optimalorientations at the curve points. With reference to Figure 4.12 (a) and (c), the optimalorientation at midpoint m1 is not between the optimal orientations at P1 and P2 If m1 isnot added, the robot tool will move between pi and P2 with the orientation interpolatedbetween the optimal orientations at Pi and P2, which results in an incorrect trajectory.The addition of m1 detects the change of curvature and avoids this error. The optimalorientation at midpoint m2 is between the optimal orientations at Pr and m1, and theoptimal orientation at midpoint m3 is between the optimal orientations at p and m2.The additions of m2 and m3 make the optimal orientations at curve points close enough31n Figure 4.13, the curve is on the Y-Z plane and the optimal orientation is defined as the normalof the curve, which maintains an acute angle with the Z axis.P1 Ml(a)360 —P2aChapter 4. Description of the Midpoint Algorithm 51midpoint 1since their 0 regions overlap. The additions of m4 and m5 are necessary since the 0regions of ni1 and rn2, and the 0 regions of m1 and P2 do not overlap respectively4. Ifthe curve is not continuous, the addition of midpoints provides for a smooth change oftrajectory as illustrated in Figure 4.13.It is important to consider the relationship between the shape and size of the U regionwhich depends on the a angle. In the a0 range [0, 90], the smaller the a0 angle, the largerthe 3 range. In Figure 4.14 (a), orientations 1 and 4 are far apart, while orientations 2and 3 are close together. Each orientation in the pair have the same c angle while /3,is 00 for orientations 1 and 2 and is 180° for orientations 3 and 4. The corresponding0 regions of 2 and 3 overlap, while the 0 regions of 1 and 4 do not overlap due to thedifferent 3 ranges shown in Figure 4.14 (b).4.2.5 Translation and Rotation CheckConfiguration Space Searchmidpoint 2point Imidpoint 3point i+1Figure 4.13: Adding midpoints in a discontinuous curve4The 0 regions of m2 and P2, and the 0 regions of m1 and m5 happen to overlap in this example.Chapter 4. Description of the Midpoint Algorithm 52x3601800Figure 4.14: The overlap of 0 regions which have small anglespan ofand free-cell 1+1i+1z(a) cx(b)free-cellFigure 4.15: The path found though pj, p2 and Pt+iChapter 4. Description of the Midpoint Algorithm 53This check detects the environment between two track points and searches for a pathwithin the allowable orientation range. The path that is found allows the tool to translate and rotate simultaneously between the points without collision. To meet theserequirements, 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+15 is termedoverlap-cell1,16• If the robot tool maintains the configurations within part of overlapcel4,+i and translates between pj and P1+1 without collision, this part of overlap-cel4,+iis called free-cell,+i . If free-cellj,1+i overlaps free-cel4_j ,, free-cel4_j ,, free-cel4,1+i andtheir 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..1,on the c — ,8 plane are calledoverlap-region,i and free-region_i, respectively, and the projection of free-cellj_1,onthe y axis is called free-range1_i,. The overlap between free-cell1_,and free-cell1,+iare projected as the overlap between free-region1_i, and free-region± and the overlapbetween free-range_i,1and free-rangej,ji. The overlap between free-range1_i, and freerangej,ji is quite straight forward. We mainly discuss the overlap between free-region1_and free-region,÷i. With reference to Figure 4.16 (a), the free-region1_i,does not overlapwith the overlap-region,11,a midpoint m1 should be added between p. and Pi+1. If freeregionj_l,j still does not overlap the overlap-regionj,m as shown in Figure 4.16 (b), furthermidpoints are added between p and m until free-region1_i, overlaps with the overlapregionj,m where mk is the closest midpoint to pj, as illustrated in Figure 4.16 (c). Asmall free-region1_i, requires the addition of more midpoints, and a smaller step size isautomatically achieved and more careful motion of the robot is obtained as the path5Because of the addition and deletion of curve points, the indices of curve points may not be sequential,and be expressed by integers. In the following text, P1+1 represents the next point to pj and P11represents the previous point to p.61n the following text,“1,1+1” and mean “between p and the next point to pt” and “betweenP1 and the previous point to P1” respectively.Chapter 4. Description of the Midpoint Algorithm 54(a)L overlap regIon13free-region11.1+10oregion1÷1(b)Cc)Figure 4.16: The overlap between free-region_1,and overlap-regiori,÷1Chapter 4. Description of the Midpoint Algorithm 55becomes narrow.After free-region1_i, overlaps with the overlap-region,i where P1+1 is the next pointto p ‘. The free-cel4,+i is found by searching for (1) the free-region1÷ in the overlapping region between free-region1_i, and overlap-region1,÷i,and (2) the free-range1,associated with the free-region,÷.The tool head can rotate in free-region1,,and thetool handle can be placed in any position in free-range1,±i as it moves between p1 andP1+1.The concepts of shrink step, shrink region and minimum shrink region are illustratedin Figure 4.17. In Figure 4.17 (a), the overlap-region,1shifts in the direction fromthe center of overlap-region,i to the center of free-region1_i,.The overlapping regionbetween the shifted overlap-region1,±and the original overlap-region18is called theshrink region. The shift distance of overlap-region,11 is called the shrink step. Theoverlap-region1,shifts by the shrink step each time until the shifted overlap-region1,j+ldoes not overlap the original overlap-region1,.In Figure 4.17 (a) (b) and (c), threeshrink regions are generated. If free-region_i, is not totally included in the originaloverlap-region,1+i, the minimum shrink region is the shrink region with minimum areaas shown in Figure 4.17 (c). If free-region1_,is totally included in the original overlapregioni,il, the minimum shrink region is the minimum shrink region which overlapsfree-region1_i,. In Figure 4.17 (d), the overlap-region1,1 overlaps with free-region1_i,.In Figure 4.17 (e), the shifted overlap-region does not overlap with free-region_i,The shrink region in Figure 4.17 (d) is the minimum shrink region.The free-region1,is found by shrinking overlap-region1,toward free-region1_i, ifthe corresponding swept volume of overlap-region1,1 collides with objects in the environment, as illustrated in Figure 4.18 (a). In Figure 4.18 (b), the corresponding swept71n the above example, we redefined mk as pi-i-i.8The original overlap-regionj,1is the over1ap-region, without shift.Chapter 4. Description of the Midpoint Algorithmshrink regionoverlap-regionu+1ashrink regionoverlap-regioni.i+113pj,j+1a.shrink regionand alsominimum shrink regionfree-regionerIap-reghnCe)Figure 4.17: The shrink step, shrink region and minimum shrink region135613free-regioi1i,free-regionI-LIshrink(a) (b)free-regionoverlap-regioniJ+ 1a(C)Cd)shrink regionfree-regionChapter 4. Description of the Midpoint Algorithm 57volume of the shrink region again collides with objects in the environment. In Figure4.18 (c), the corresponding swept volume of the shrink region is collision-free and thisshrink region corresponds to free-region,i. If free-region,i is not found when minimum shrink region has been checked, a midpoint is added between p and Pi+1• Adding amidpoint changes the overlapping region between local 0 regions at successive points andallows some new configurations to be considered. A minimum step size is specified, andif free-region,i is not found and the step size between successive points is smaller thanthe minimum step size, the current path is deleted from the path list and other paths inthe path list are planned. If the path list is empty, the trajectory planning stops.If free-region1,+j is found, a search for the corresponding free-range,1÷iis carried out.The 7 range is decomposed as a binary tree structure, and free-rangej,j÷i is searched usinga breadth-first search. Only the 7 ranges that has a common range with free-rangej_i, aresearched. The free-range_i,, free-range,i and their common range contribute to thepath of the tool handle. If no free-rangez,ji is found, the corresponding free-region,i isinvalid and must be shrunk by one shrink step, and the free-rangej,ji is searched againfor the new free-region,i.The Swept Volumes Used in Translation and Rotation CheckThree kinds of swept volume between curve points p and Pi+1 are used in Translation andRotation Check, the 0-trans swept volume, the 7-trans swept volume and the shrink sweptvolume 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 0swept volumes in the direction of tool motion, i.e. the direction from p to P1+1, asillustrated in Figure 4.19 (a). With reference to Figure 4.19 (a) (b), the size of theintersection volume depends on the angles 4 and c2 between the optimal orientations ofthe full range 0 swept volumes at the points i and Pi+i. The length of the 9-trans sweptChapter 4. Description of the Midpoint Algorithm 58(C)free-region 1-1,1overlap-region 1.1÷1free-regionshrink regionfree-region1(b)free-regionshrink region— cxFigure 4.18:free-region,1Shrinking overlap-region,i toward free-region_1, in order to find(0)Chapter 4. Description of the Midpoint Algorithm 59volume corresponds to the distance between successive track points. With reference toFigure 4.19 (c) (d), the intersection orientations are defined as the orientations at theintersection points of the two 0 regions. The minimum angle w between the direction oftool motion and the plane defined by the two intersection orientations can assume anyvalue, as illustrated in Figure 4.19 (d). The angle w determines the shape of the 0-transswept volume. Two 0-trans swept volumes with the same size but different w angles areshown in Figure 4.19 (e) and (f). The 0-trans swept volumes corresponding to differentsizes, lengths and angles are generated in advance. During the check, a 0-trans sweptvolume is selected based on the overlapping size, distance between successive track pointsand the angle w with respect to the direction of tool motion.The 7-trans swept volume corresponding to a 0-trans swept volume is approximatedby using two vertical 0 swept volumes shown in Figure 4.20 (b) to displace the 0 sweptvolumes used to generate the 0-trans swept volume, as shown in Figure 4.20 (a). Thelength L, the angle and the allowable angle range 0 of the 0-trans swept volume areknown. In Figure 4.20 (a), line segment oa and line segment ob are on the boundary ofthe intersection of 0 swept volumes and are also on the plane defined by the axes of 0swept 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= 2L — 2Lcos (4.2)Since= 40— 2we get(4.3)Chapter 4. Description of the Midpoint Algorithm 60optimalorientationoptimalorientation IIntersectiono region orientationIntersectionorientationtop view of the swept volume In (b)(e)plane determined byIntersection orientationtop view of the swept volume In (d)U)optimalorIentation 1+1(a)p(b)9 region1+1(C)plane determined byIntersection orientationplane determined byintersection orientation(d)direction of tooldirection of tool motionFigure 4.19: The 0-trans swept volumes used in Translation and Rotation CheckChapter 4. Description of the Midpoint Algorithm 61From Equation (4.2) and (4.3), we getD = \/L/1 — cos(200—(4.4)In Figure 4.20 (b), d is the distance between the tips of the 0 swept volumes.d = 2LsinO0— D (4.5)From Equation (4.4) and (4.5), we getd = 2Lsin00— v’L1 — cos(200— ) (4.6)Given a 0-trans swept volume, the distance d between the two 0 swept volumes can becalculated using Equation (4.6), and the intersection of two corresponding full range 7swept volumes is obtained as illustrated in Figure 4.20 (c). The corresponding full range7-trans swept volume is generated by extruding the intersection along the direction oftool motion, as illustrated in Figure 4.20 (d). The length of the extrusion and the angleare the same as those of the 0-trans swept volume. The full range 7-trans swept volumeis decomposed into the binary tree structure illustrated in Figure 4.20 (e). Each 7-transswept 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 theshifted overlap-region,i and the original overlap-region,11the shrink swept volume isgenerated by taking the copy of the intersection of two 0 swept volumes, called the intervolume, refer to Figure 4.21 (a), and rotating the inter volume angle about the linewhich is in the plane defined by the two intersection orientations and is perpendicularto the line from the tip of the intersection to the center of the intersection, as shown inFigure 4.21 (b), and then extruding the intersection of the two inter volumes along thedirection of tool motion, as illustrated in Figure 4.21 (c). Here is the angle betweenthe center of shifted overlap-region,1and the center of original overlap-region,11.9w is the minimum angle between the direction of tool motion and the plane defined by the twointersection orientations as shown in Figure 4.19 (c).Chapter 4. Description of the Midpoint Algorithm 62fufl range 7swept volumeoverlapping 7swept volumedirection of tool motion7 - trans swept volume(d)position of 0swept volume(e)L(a) (b)(C)Figure 4.20: The 7-trans swept volume used in Translation and Rotation CheckChapter 4. Description of the Midpoint Algorithm 63optimalinter volume(a) (b)7”\\\(C) (d)Figure 4.21: The shrink swept volume used in Translation and Rotation CheckChapter 4. Description of the Midpoint Algorithm 64The parameters in Translate and Rotate Check are minimum step size and shrinkstep. A large minimum step size results in a coarse search for a trajectory, while a smallminimum step size increases the search resolution between successive track points. Theshrink step determines shrinking resolution.4.2.6 Inverse Kinematics CheckThe motion of the robot tool is also limited by the robot joint constraints. Computation ofthe robot inverse kinematics solution requires the configuration and position of the robotwrist to be calculated from the position of the track point and the selected configurationof the robot tool. With reference to Figure 4.22, the following homogeneous coordinateframes 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 orientation as Fr.3. Selected Configuration Frame F3: the frame with an origin coincident with F butrotated by a8 about ‘, and j3 about Zr,.4. Tool Frame F: the frame with an origin coincident with F8 but rotated by -y aboutz8.5. End Effector Frame Fe (, ñ, : and i1 are in the plane determined by the toolaxis and the tool handle, and points to the track point.The homogeneous transformations [Wolo 91] H,.,, H,8, H81 and 11t,e defining the robotposition and orientation can be obtained from the frame definitions listed above. Therobot 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 65The right hand side of Equation (4.7) corresponds to the homogeneous transformationsin the sequence of the robot base, tool tip, tool head, tool handle, and end effector. Givena 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 thesequence of the robot base, linki, link2,..., end effector. For a given robot, the finaloverall (base to tool) configuration coordinate transformation 11r,e which contains therequired joint angles, is fixed [Wolo 91]. So the robot joint angles can be calculated inEquation (4.7). The details are discussed in Appendix C.The configurations in the path comprising free-region_i, and free-region1,are determined as follows:1. The configurations in free-region_i, are the configurations of the robot tool at themidpoint between Pi-1 and pj.2. The configurations in the overlapping region between free-region_i, and free-region,iare the configurations of the robot tool at p.3. The configurations in free-region,j+i are the configurations of the robot tool at themidpoint between pj and Pi+1.Multiple inverse kinematics solutions can be computed based on one position/orientationof the robot tool. For instance, eight solutions are obtained for the PUMA 560 robot. Ifall 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 usedto compute the inverse kinematics solutions. This provides a degree of safety in thetrajectory since there is a collision-free region between the selected configuration and theenvironment. If none of the configurations produce a valid inverse kinematics solution,Chapter 4. Description of the Midpoint Algorithm 66ZpN. Zs selected tool---..orientction“S “s,ISI / toolframea5afrockVpXrFigure 4.22: A series of coordinate framesChapter 4. Description of the Midpoint Algorithm 67this check fails and other paths in the path list are explored. If valid inverse kinematicssolutions are computed, these solutions and the corresponding configurations are storedat a subsequent stage in the planning. A cost function is used to select the inversekinematics solutions and their corresponding configurations at each point to generatethe optimal trajectory. Details of this search are discussed in Section 4.2.8.The number of configurations selected for the inverse kinematics computation is veryimportant. If just a few configurations are selected, the check may fail although theremay be unexplored configurations that correspond to valid inverse kinematics solutions.Increasing the number of configurations results in a better chance of finding valid inversekinematics solutions, and a better trajectory will be planned since more configurationsand inverse kinematics solutions can be used by the cost function. This, however, takesmore computation time and requires more memory to store the data.4.2.7 Control the Path Planning Using A* Search AlgorithmThe path consists of a list of free-regions at the curve points which have been included inthe 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 asfollows,Until the path list is empty, determine if the first path in the path list includes thefinal 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 accumulated 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 exceptfor the path that reaches the common curve point with the minimum cost.The Path Cost is calculated asPathCost = WvCv + W0C — WpCp (4.8)Where Wv, Wo and Wp are the weights of cost Cv, C0 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. Thiscost is used to select a wider path.Go, the cost measuring the average departure of the path segment away from theoptimal orientation, is calculated as=/(a—cr)2 + (/3di — /3)2 (4.10)where a is the average of crmjn and crmax in the free-region1,i, f3 is the average ofI3mimand /3max in the free-region1,÷and crj and !3di are the averages of cr and ,8 angles inthe optimal orientations at pj and pf1 respectively. This cost is used to select the pathwhich is closer to the optimal orientation.Cp, the cost measuring the percent of the curve that has been travelled, is calculatedasCp (4.11)Chapter 4. Description of the Midpoint Algorithm 69where n is the current point number and n is the number of track points travelled. Thiscost 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 theweights, the trajectory is planned in different ways. For example, a large value of Wvmeans a wider path is preferred, a large value of Wo means that the application constraintis 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 TrajectoryIn each path segment, the inverse kinematics solutions of a number of tool configurationsare calculated as described in Section 4.2.6. After finding the path along the curve, theoptimal trajectory along the curve is obtained using a second A* search to select boththe robot inverse kinematics solutions and corresponding tool configurations from thepath. The A* search is discussed in Section 4.2.7. The cost used in the second search iscalculated ask k kTrajCostk = Wj C1 + WD CDi + WH1=1.0 11.OJ k k+ (WR C1)+ W — WC (4.12)j=1 1=1.0 1=1.0Where 1.0 k n, k is current track point number, n is the final track point numberin the track, and J is the number of robot joints. Since some midpoints are added in thetrack, the track point index (k) is not an integer.In Equation (4.12), W and C represent weights and costs respectively. is thecost 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 rangeis preferred. This avoids joint boundaries as the tool travels the curve. The cost C1 isChapter 4. Description of the Midpoint Algorithm 70calculated asj (6—6m3)2=Ofj (4.13)j=1where O and Ornj are the jth joint angle and the midpoint of jth joint range, and isthe full range of the jth joint.CD, the cost measuring deviation of the tool orientation from the optimal orientation,is calculated as(‘2 +_‘ Li / I3Li /Di— 2where aj and /3 are the a and /3 values of the optimal orientation at the ith track point,a and/3jare the a and /3 values of the selected orientation at the ith track point, andaL and /3Li 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 ithand i — ith track points is calculated as(ai—ap2+ + (‘YiYip2\ /“ I3Li / “ 7Li / (4 153where a1, /3 and are the a, /3 and y values of selected tool configuration at the ithtrack point, ,B,, and are the a, /3 and y values of the selected tool configurationat the i — ith track point, and a,j, I3Li and 7Li are the a, /3 and 7 ranges of the LocalC-space at the ith track point.CR1, the cost measuring the difference between the robot joint angles at the ith andi — ith track points, is calculated as1 i I.._iL.C —•L / Vj2 \2Rij— )1=1 v3,fwhere n, is the joint number, and are the jth joint angles at the ith and i — ithtrack point respectively, and is the full range of the jth joint angle.Chapter 4. Description of the Midpoint Algorithm 71Cci, the cost measuring the change of robot configuration at the ith and i — ith trackpoints, is calculated as=(1 — 1)2 (4.17)where T and are the robot configuration indices at the ith track point and i + ithtrack point respectively.Cp, the cost measuring the percent of the curve that has been travelled, is defined byEquation (4.11).The cost function weights are used to bias the trajectory as shown in following table,Weights Effect on trajectory planningWj safe robot motionWD maintaining optimal tool orientationWH and WR3 smooth motionWa robot configuration avoidanceWp speed of search optimal trajectoryNormally, a large value of Wc is applied to keep the same robot configuration alongthe curve. If the robot configuration changes from “elbow up” to “elbow down”, orchanges from “right” to “left”, as illustrated in Figure 4.23, discontinuous motion ofrobot disrupts the smooth motion of the tool. If a robot configuration selected at thebeginning of search changes in the middle of the curve, all proposed trajectories usingthis configuration will have a very high cost. Details of the selection of weights for anexample application of this algorithm are given in Chapter 5.Chapter 4. Description of the Midpoint Algorithm 72Figure 4.23: Different robot configurations4.3 Summary of the Midpoint AlgorithmThe 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 geometriccriterion of the robot tool specified by the application.3. Predetermine the parameters in trajectory planning according to the requirementsof 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 thepath list is empty, the path planner stops.6. Use the A* search algorithm to explore multiple paths in the path list. The minimumcost path is extended to the next point. The extension of a path between successive(a) (b)Chapter 4. Description of the Midpoint Algorithm 73points is subject to the following checks: Curvature Check, Overlap Check, Translation and Rotation Check, Inverse Kinematics Check and Path Cost Check. Thetrack 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 toolconfigurations 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 74Figure 4.24: Flowchart for the Midpoint AlgorithmChapter 5Computer Implementation and Examples5.1 OverviewAn off-line simulation program, TRAJPLAN, has been developed in order to test theMidpoint Algorithm. Using TRAJPLAN, the user only needs to input the applicationspecification, to select the curves to be processed and to indicate the environment objectsthat may collide with a robot tool during the process. A trajectory for the robot tool willthen be automatically planned. The output of TRAJPLAN is a set of sequential jointangles 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 AutoCADDevelopment System (AutoCAD ADS). As a solid (3-D) and region (2-D) modeling program, AutoCAD AME is used to generate the swept volumes of the robot tool. AutoCADADS 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 tocheck for interference between swept volumes and the environment.Since TRAJPLAN only deals with two adjacent points on a curve, the complexityof the curve and environment does not have any effect in the algorithm. A practicallimitation to TRAJPLAN is that the amoiant of data to be stored may exceed the systemmemory model if many curve points are used. This has not been a problem in the trials ofthe system to date where there are typically 100 points have been selected. TRAJPLAN75Chapter 5. Computer Implementation and Examples 76is compiled using the Zortech C++ 3.0 compiler using the P memory model (Phar Lap386—DOS Extender, a full 32 bit 386 DOS Extender) to allow the program to operatein 32 bit protected mode with a linear address space of 4 Gbytes.While PUMA 560 and CRS A460 inverse kinematics modules have been developedto work with TRAJPLAN, kinematic modules for other robots can be easily linked withTRAJPLAN. TRAJPLAN has successfully demonstrated automatical programming of arobot welding system and a fish butchering system.5.2 The Simulation Program TRAJPLANThe simulation program TRAJPLAN is initiated by identifying the curves to be processedand the environment objects which may collide with the robot tool. The user still needsto select the desired way to plan the trajectory (refer to Section 5.4), precision level oftrajectory (refer to Section 5.5), and the optimal orientation (refer to Section 5.6). If noselection is given, default settings are used.After the user initiates the TRAJPLAN, the path planning starts by reconstructingthe 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 listis that a node can be easily inserted and deleted. The path planning procedure buildsand updates the path list, a linked list which records all of the potential paths. Eachpath in the path list is also a linked list which links the path segments between successivepoints. Since different paths may pass through different environments and add differentmidpoints into the curve, each path must record its points. The path list is updated withfollowing steps:STEP 1 Use check loop, including Curvature Check, Overlap Check, Translation andRotation Check, Inverse Kinematics Check and Path Cost Check, to extend the topChapter 5. Computer Implementation and Examples 77CurvePoint1 2 2.5 3 4 5 ‘ii nPath 1Path 2PathList Path 3Path kEl free-part 11+1Figure 5.1: PathList data structurepath 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 pathplanning 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 pointor between successive track points and to check for the interference between the sweptvolumes and environment in order to find a free cell or free-cell,+i. At the first point, freecells are found by shrinking the 0 swept volumes in order to obtain collision free 0 sweptvolumes, and then using breadth-first search to find the collision free swept volumes incorresponding 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 treeis searched. Between subsequent curve points, the search for free-cellj,+i is very similarto the search for free cell, i.e. search 9-trans swept volumes first and then search 7-transswept volumes corresponding to the collision free 9-trans swept volume. The procedureChapter 5. Computer Implementation and Examples 78is illustrated in Figure 5.2. In order to access the swept volumes efficiently during pathplanning, the swept volumes are down loaded into memory. This detail is discussed inSection 5.3.For the PUMA 560 and CRS A460 robots, each robot tool configuration can be usedto calculate eight robot inverse kinematics solutions. The trajectory of the robot toolhas two parts: the configurations and positions of the tool, and the corresponding inversekinematics solutions. If TRAJPLAN cannot find a path, or the final trajectory basedon the selected path contains a change of robot configuration, TRAJPLAN reports theerror or warning message to the user.5.3 Down Load the Swept Volume Into MemoryLoading the swept volume into memory provides efficient access to the swept volumesduring the interference check, which speeds up the trajectory planning. Loading all ofthe 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 Cspace. It is impossible to load a huge data structure into memory due to the limitationson memory space and the restrictions of MS DOS. The 0 swept volumes are stored in aone dimensional array and 0-trans swept volumes are stored in a three dimensional arraybased on the size, length and angle of the swept volume. Each 0 swept volume has a fullrange 7 swept volume associated with it. The,‘swept volume is decomposed into a binarytree. If each dimension in the 0 swept volume array or the 0-trans swept volume arraycontains three elements, and a five level 7 swept volume binary tree 1 is associated witheach array element, the total number of swept volumes is 930 (3 x 31+3 x 3 x 3 x 31). MSDOS, however, only allows 250 files to be opened simultaneously. To solve this problem,1A five level binary tree contains 31 nodes.Chapter 5. Computer Implementation and Examples 79Figure 5.2: The procedure for searching for collision free swept volume (a) The 0 sweptvolume collides with environment, (b) Shrinking 0 swept volume for the collision free 0swept volume, (c) The swept volume collides with environment, (d) Search sweptvolumes for the collision free 7 swept volume.(a) (b)(C) (d)Chapter 5. Computer Implementation and Examples 80oniy the swept volumes with different shapes are loaded into memory. The remainingswept volumes can be obtained by rotating the loaded swept volume having the sameshape. The rotation angle , /3 and ‘y are determined by the difference in the , /3 andangles 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 explorationof a high resolution configuration space is possible.5.4 Plan the Trajectory as DesiredSeveral 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 orientation.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 theoptimal trajectory. Or, if the user has limited time, he can require TRAJPLAN find apath as quickly as possible.The weights in cost functions (Equation (4.8) and Equation (4.12)) determine theoptions of TRAJPL..N The default values of the weights to calculate Path Cost areChapter 5. Computer Implementation and Examples 81shown in following table,Weight Effect on trajectory planning Value CommentsWp speed of planning path 0.6 the most importantWo path close to optimal orientations 0.3 importantWv width of path 0.1 not very importantThe default values of the weights to calculate TrajCost are shown in following table2,weight effect aspect in trajectory value commentsWj joint angle is close to the center of joint angle range 0.01 not very importantWD trajectory is close to optimal orientation 0.15 importantWH trajectory keeps its history 0.2 importantWp speed of planning path 0.15 importantWR1 the first joint angle keeps its history 0.15 importantWR2 the second joint angle keeps its history 0.15 importantWR3 the third joint angle keeps its history 0.1 importantWR4 the fourth joint angle keeps its history 0.03 not very importantWR5 the fifth joint angle keeps its history 0.03 not very importantWR6 the sixth joint angle keeps its history 0.03 not very importantWc the change of robot configuration 100 very importantA large value is assigned to Wc in order to avoid the robot arm inversion. The valuesof WR1, WR2, W1 WR4, W5 and WRG indicate that the motion of the robot wrist is2A large value is assigned to Wc in order to avoid robot arm inversion.Chapter 5. Computer Implementation and Examples 82more flexible than the motions of upper arms.5.5 The Precision of TrajectoryThe precision of trajectory is very important since it affects the quality of process. Different applications have different precision requirements. For example, a welding applicationmay 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 precisionshould be determined by the user according to the requirement of application. Otherwisethe following default values are assumed,dmin 0.4 inchdma2, 0.8 inchnumber of intermediate points 8minimum step size 0.02 inchnumber of configurations picked up from path segment 24For the convenience of inexperienced users, the values of parameters have been setat different accuracy levels. If the default accuracy level is not suitable for the application, the user can simply select the accuracy level to alter the speed or precision of thetrajectory planning algorithm.5.6 The Specification of Optimal Tool OrientationTo use TRAJPLAN, user need to input the application specification including the optimal orientation of robot tool relative to the curve, the allowable deviation of the toolorientation away from the optimal orientation, and the allowable deviation in the distance between the tool tip and the curve. Without the input, TRAJPLAN will planChapter 5. Computer Implementation and Examples 83normal of plane 2p’ane 1opflmal orientation1optimal orientation(b)Figure 5.3: The optimal orientation of robot toola trajectory based on the default specifications. With reference to Figure 5.3 (a), thedefault geometric criterion of the tool optimal orientation at a curve point is defined asfollows,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 ofthe two planes on each side of the curve.3. The tolerance of the orientation deviation 9 from the tool optimal orientation is300 as shown in Figure 5.3 (b).5.7 Examples: Automatic Welding and Fish Butchering Processing5.7.1 Automatic Robot Welding ProcessAn automatic robot welding system is shown in Figure 5.4. The process constraintsinclude the distance between the welding head and the seam, the optimal welding orientation, collision avoidance, valid robot inverse kinematic solutions, and robot arminversion avoidance. Complete knowledge of the system including the position of objectstangentcurveplane 2(a)curveChapter 5. Computer Implementation and Examples 84seamtod PUMA 560 robotobstacleFigure 5.4: The automatic robot welding workcellChapter 5. Computer Implementation and Examples 85in 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 toprocess constraints in 1’20”. Most of this time is taken by the AutoCAD interferencechecks. An example procedure for searching for collision free paths by placing variousswept 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 volumecollides 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 atthe first seam point. The swept volume collides with obstacles.(d) Search the 7 swept volumes at lower levels in the swept volume binary tree. Thefirst 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 sweptvolume. The swept volume is collision free.(g) Place the full range 7-trans swept volume corresponding to the selected 0-trans sweptvolume 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 binarytree. The selected 7-trans swept volume collides with obstacles.(1) Select another 7-trans swept volume. This 7-trans swept volume also collides withobstacles.3The dash line is the seam and the cylinder and the plate are the obstacles.Chapter 5. Computer Implementation and Examples 86(j) Search the 7-trans swept volumes one level down in the 7-trans swept volume binarytree. The selected 7-trans swept volume collides with obstacles.(k) Select another 7-trans swept volume. The selected 7-trans swept volume is collisionfree.(1) Select another 7-trans swept volume. The selected 7-trans swept volume collides withobstacles.(m) Select another 7-trans swept volume. The selected 7-trans swept volume is collisionfree. Up to now, there are two possible paths found in (k) and (m) respectively.5.7.2 Automatic Fish Butchering ProcessThe Industrial Automation Laboratory at UBC has developed a system for automaticallyselecting cutting contours for a fish butchering process [Gama 93]. The fish butcheringworkcell is shown in Figure 5.7. When fish passes along the convey or the position andthe shape of fish contour is calculated by a knowledge-based computer image processing system. The process constraints include the distance between fish and a water-jetcutter, the optimal cutter orientation, collision avoidance, valid robot inverse kinematicsolutions, 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. Anexample procedure for searching for collision free paths by placing various swept volumeson 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 volumedoes not collide with obstacles.4The dash line is the fish contour.Chapter 5. Computer Implementation and Examples 87(k)(a) (b) (C) (d)Ce) (f) (g) (h)(I)Ci) (I)(m)Figure 5.5: Procedure for searching for collision free paths.Chapter 5. Computer Implementation and Examples 88(b) Place the full range 7 swept volume corresponding to the selected 0 swept volume atthe 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. Thefirst 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 sweptvolume. The swept volume is collision free.(f) Place the full range 7-trans swept volume corresponding to the selected 0-trans sweptvolume 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 binarytree. The selected 7-trans swept volume collides with obstacles.(h) Select another 7-trans swept volume. This 7-trans swept volume also collides withobstacles.(i) Search the 7-trans swept volumes one level down in the 7-trans swept volume binarytree. The selected 7-trans swept volume collides with obstacles.(j) Select another 7-trans swept volume. The selected 7-trans swept volume is collisionfree.(k) Select another 7-trans swept volume. The selected 7-trans swept volume collideswith obtcles.(1) Select another 7-trans swept volume. The selected 7-trans swept volume is collisionfree. Up to now, there are two possible paths found in (i) and (1) respectively.CD C;’‘—C 0 CD CD CoCCD‘.-‘p C-)§f I I§n Cnn :rC-) C CO CC’CD p CO s-a 0 0 tz_cCD o;qC’ C) a00 CoChapter 5. Computer Implementation and Examples 90cameratoolobstacle obstaclecameraPUMA 560 robotcontour/fish obstacleFigure 5.7: The automatic fish butchering workcellChapter 6Conclusion and Future Work6.1 ConclusionIn this thesis a new trajectory planning algorithm, the Midpoint Algorithm, has been developed. The algorithm plans smooth trajectories for a robot tool moving along specifiedcurves subject to application constraints. Geometric constraints ( allowable orientationand position of tool) and motion constraints (joint limits, robot configuration changeavoidance and collision avoidance) are also integrated into the algorithm. The trajectoryis planned by searching for path segments between successive curve points, linking validpath segments to generate a path along the curve and selecting the optimal configurationsinside 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 configuration gives a general description of robot tool, which includes the tool axis derivedfrom optimal orientation within an allowable tolerance and the tool itself rotating aboutthe tool axis. The concepts of Entire C-space and Local C-space were introduced as aconvenient representation of the tool configuration for planning purposes. Since the LocalC-space only covers useful parts of the Entire C-space, a condensed data structure is usedto represent the configuration space with high resolution. The Local C-space is the combination of the 0 region and the range, which correspond to the constraints of allowabletool orientation and robot inverse kinematics respectively. Different Local C-spaces have91Chapter 6. Conclusion and Future Work 92different shapes, sizes and positions of the corresponding 0 regions. A mathematical expression for Local C-space has been derived, and given the optimal orientation (cr0, 3) ofthe Local C-space and the maximum allowable orientation 0, the corresponding 0 regioncan be calculated.The trajectory planning is initiated using a Point Check procedure to search the LocalC-space at the first curve point and initializes a path list of potential paths. A Translationand Rotation Check identifies collision free path segments by searching overlapping 0regions at successive points, shrinking the overlapping regions in order to find collisionfree regions. If no collision free 7 ranges are found even though the minimum 0 regionhas been tested, a midpoint is added if the step size is greater than the minimum stepsize, while the current path cannot be extended if the step size is less than the minimumstep sizeCorresponding to the 9 regions and y ranges, swept volumes of the tool are generated and used to detect collision between the tool and the environment. Since onlyswept volumes which have different shapes are loaded into memory, the system memory requirements are reduced and exploration of high resolution configuration space ispossible.To address the problem of how to guarantee that the track of the tool tip matchesthe curve, a method of automatically varying the step size between curve points wasdeveloped. The step size is examined to determine: (1) if the straight line connectingsuccessive curve point can approximate the shape of this curve segment, (2) if the optimalorientation at the points are close enough, and (3) if the environment between the twopoints is simple enough. After adjusting the step size, a minimum number of curve pointsare used to search for the path, which results in fast path planning. The small step sizeused 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 93An A* search optimizes the path planning for multiple paths. Since the plannersearches 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 translateand rotate simultaneously without collision, which provides for a smooth motion of tool.An off-line simulation program, TRAJPLAN, based on the Midpoint Algorithm hasbeen developed. After the user inputs the application specifications, selects the curve tobe processed, and identifies the environmental objects, TRAJPLAN automatically plansthe trajectory of robot tool and generates a set of sequential joint angles correspondingto a particular robot. Currently the system supports the PUMA 560 and CRS A460robots.The Midpoint Algorithm has been demonstrated in software simulation of fish butchering and robotic welding. The algorithm is suitable to a variety of robot applications whichrequire collision free motion subject to process constraints. By changing the accuracylevels of the parameters, the user specifies the precision of the trajectory or the kind oftrajectory preferred in order to meet the different requirements of robot applications.6.2 Recommendations for Future WorkThis approach only considers the collision free motion of the robot tool, but not thatof robot manipulator. Also dynamic constraints are not considered. An immediateextension to this algorithm includes the consideration of the entire robot and the inclusionof dynamic constraints in the planning process. Also more extensive testing on industrialapplications needs to be done.TRAJPLAN is a general algorithm for CAD environments which provide functionsfor building solids, moving solids and checking for interference between solids. To dateTRAJPLAN has only been implemented for use with AutoCAD.Chapter 6. Conclusion and Future Work 94TRAJPLAN can also be used without a graphics interface after creating the datastructures for storing solids and the interference check tool. Most of the time in pathplanning is spent on the interference check by AutoCAD and the display of graphics onthe screen. Without the graphics interface, TRAJPLAN can plan the trajectory muchfaster and it may be possible to use it as a real-time software. There are three steps tobuild such a system:1. Use sensors to obtain the information of environment and the curves to be processed.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 AData StructureSome data structures used in TRAJPLAN are described in this section.1. The structures about curve and track,Config = recordalpha, beta, gamma: double;end;Point = recordx, y, z: double;end;CurvePoint = recordnum: double;curve_point : Point;track_point : Pointoptimal: Configend;Curve = recorddata: CurvePoint;95Appendix A. Data Structure 96‘next : Curveend;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 corresponding swept volumes,Region = recordboundary : array [0. .20] [0. .2] of double;j next : Regionend;Thnode = recordi, j, color : integerregion : RegionId: apObjid;t fname : charend;Appendix A. Data Structure 97Range = recordMinG, MaxG : double;end;Gnode = recordi, j, color : integerrange : Rangeid: ap...Objid;I fname: charend;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 volumeand checking the interference.fname the swept volume file name used for loading the sweptvolume into memory.3. The structures about robot inverse kinematic solution,Appendix A. Data Structure 98InvKinSol = recordangle : array [0. .6] of double;end;InvKinSols = recordconfig: 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 basedon one tool orientation.valicL.num the number of valid robot inverse kinematics solutions.4. The structures about path list,Part = recordpoinLnum: double;point : CurvePointregion : Regionrange: Range;Tsolutions : InvKinSols;Appendix A. Data Structure 99end;Path = recorddata: PartInext: Path;end;PathList = recordt path : PathPathCost : double;next : PathListend;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 free-cel4,1+i.solutions the inverse kinematics solutions.data the information of a region.path the list of free-cell1,i.PathCost the cost of the path.5. The structure of robot tool trajectory composed of two parts: the configurationand position of tool, and the corresponding robot inverse kinematic solutions.Appendix A. Data Structure 100Trajectory = recordpoinLnum: double;point : Pointselect : Configsolution : InvKinSolnext : Trajectoryend;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.Appendix BTRAJPLANThe pseudo-code of the main procedure in TRAJPLAN is listed as follow:varaddmid, finish: integer;obstaclelD : ap_objid;Cspacel, Cspace2 : Part;program Trajplan(){ main procedure for trajectory planning }varcurve: Curve;I paths : PathList;I point, I last_point : Curve;I trajectory : Trajectory;beginaddmid := 0; finish := 0;SpecificationQ;curve := SelectCurveQ;obstaclelD : = SelectEnvironmentQ;LoadSweptVolumeQ;paths := PointCheck(point);101Appendix B. TRAJPLAN 102if paths = NULL then exit; {No path found}while (finish not 1)or (finish not -1) begin1astpoint := NULL;last_point : = SearchLastPoint (pathspathtdata.point_num, curve);if last_point = NULL then exit;else if lastpointnext = NULL thenfinish := 1; {trajectory planing finish.}else beginpaths : = CheckLoop(paths, last_point, lastpointnext);if addmid = 0 then beginpathstPathCost := PathCost(pathspath);paths := SortPathList(paths);finish := 0;endendendif finish = -1 then exit; {No path found}trajectory : = OptimizeTrajectory(pathstpath);endfunction CheckLoop( paths:PathList, 1ast_point:Curve, nextpoint:Curve): PathList;{ Check loop is applied to successive points to search the path segment betweenthem. The Overlap Check and Inverse Kinematics Check are included inTranslateRotateCheck. }varAppendix B. TRAJPLAN 103retval : integer;beginaddmid 0;Cspacel = BuildCspace( 1ast_pointdata.optima1);Cspace2 = BuildCspace( nextpointdata.optima1);if not CurvatureCheck(1astpointdata, nextpointdata) then beginaddmid := 1;curve := AddMidpiont(curve, 1astpoint, next_point);return paths;endretval : = TranslateRotateCheck(paths, 1ast_pointdata, next_point Idata)if retval = 0 then begin { fres-part do not overlap.}addmid 1;curve : = AddMidpiont (curve, lasLpoint, next_point);return paths;endelse if retval = -1 then begin {no path segment found.}if PointCheck = NULL then beginfinish := -1;exit;endelse beginpaths := DeletePath(paths);return paths;endAppendix B. TRAJPLAN 104else begin {the minimum cost path is extended}addmid := 0;pathspathdata.poinLnum: next..pointldata.num;return paths;endendfunction TranslateRotateCheck( Ipaths:PathList, last: CurvePoint, next:CurvePoint) : integer;{ This function searches the free-region between last and next points.}varlength : double;shrink_center: Config;1’ ov_region, free_region, ov_free_region: Region;I free_range: Range;I free_part : Part;I solutions : InvKinSols;beginlength : = 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 = NULL then return 0free_region := ShrinkOvRegion(ov_region, length, last, next);if free_region = NULL then return -1;free_range := FreeRange(free_region, length, last, next);if free_range = NULL thenAppendix B. TRAJPLAN 105free_region : = SmallRegion(free_region);if free_region NULL then finish := 0; return -1;ov_free_region = OverlapCheck(pathspath data.region, free_region);curve := 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);pathspathdata. 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 storethe parts into path list. }function SortPathList( paths) : PathList{ Sort the paths in the path list according to the PathCost and put theminimum 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 (c0,j3) and 0.}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,+iand 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 configurationsin the region to be selected and 8 inverse kinematics solutions can be obtainedfrom each configuration. }function OptimizeTrajectory(point) : Trajectory;{ Use cost function to optimize the trajectory. }Appendix CInverse Kinematic Modules for PUMA 560 and CRS A460With reference to [Wolo 91] and Figure (C.1), the six Homogeneous transformation matrices of PUMA 560 robot are as follows:cosO1 —sinO1 0 0.sinO1 cosO1 0 0H= (C.1)0 0 1k0 0 01cosO2 —sinO2 0 00 0 10H= (C.2)—sinO2 —cosO2 0 00 0 01cosO3 —sinO3 0 esin93 cosO3 0 0H= (C.3)0 0 lg0 0 01cosO4 —sin04 0 00 0 ifH= (C.4)—sinO4 —cosO4 0 00 0 01108Appendix 0. Inverse Kinematic Modules for PUMA 560 and CRS A460 109Zpselected toolN. orientation\ZsS /7Z \Vs//7o4track point --eVp 03XpXsXrFigure C.1: The PUMA 560 robot and robot toolAppendix C. Inverse Kinematic Modules for PUMA 560 and CRS A460 110cosO5 —sinO5 0 00 0 10H= (C.5)—sinO5 —c0805 0 00 0 01cosO6 —sinO6 0 00 0 —1 —d (C.6)sinO6 COSO6 0 00 0 01The only difference between CRS A460 robot and PUMA 560 robot is the differentvalues 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. Thecomputation subsequently discussed is for both the PUMA 560 robot and the CRS A460robot.Equation (4.7)Hr,e = Hr,pHp,sHs,tHt,eis used in inverse kinematics computation. The Hr,e can be rewritten as H and iscalculated 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: Oi, 02, 03, 04, 05, 06, d, e, f, g, and h. The PUMA 560 geometryparameters, d, e, f, g, and h are known. The right hand side of Equation 4.7 representsthe 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 shownin Figure C.1. The desired wrist position used in later computation isAppendix C. Inverse Kinematic Modules for PUMA 560 and CRS A460 111calculated as(P—daI pyw J = P,—da, IPz_daz)The calculations for the robot joint angles are as follows [Wolo 91],—-r ± + P — g2= Atan2 [ j + Atan2 L g ] (C.9)r e2+fg—P—P —1F —h)2yw ZW03 = Atan2 I__________________________________________±4e2f2_[e2+f2+g2_P yw zw / ] (C.1O)2 _p2 ( —h’2122 I—(-Pcos0i + Psin0i)fcos03— (Pzw — h)(e — fsin03)1 (C.11)02 = Man L (Pcos0i + Psin0i)(e — fsin03)— (P — h)fcos0jasinOi — acos0104 = Atan2 [ acos0icos(0 + 03) + aysinOlcos(02+ 03) — asin(02+ 03)] (C.12)and—05±180°05 = Atan2[V(axsin0i +a2cos91)+ (acos0icos(02+ 03) + asin0icos(02+ 03) — asin(02+ 03))2—acos0isin(0 + 03) —a2sinOisin(0 + 03) — acos(02+ 03)(C.13)and — 05I—scos01sin(02+ 03) — .ssin0isin(02+ 03) — scos(02+ 03)106 = Atan2 [ ncos0isin(0 + 03) + nysinOlsin(0+ 03) + ncos(02+ 03) j (C.14)and—06±180°Bibliography[Ange 88] Angeles, J., Rojas, A. and Lopez-Cajun, C.S., “Trajectory Planning inRobotics Continuous-Path Applications”, IEEE Transactions on Robotics andAutomation, Vol.4, No.4, 1988. pp.380-5[Barr 89] Barry, P. and Goldman, R., “A Recursive Evaluation Algorithm for a Class ofCatmull-Rom Splines”, SIGGRAPH 89, pp.199-204.[Bars 80] Barsky, B.A. and Greenberg D.P. “Determining a Set of B-spline Control Vertices to Generate an Interpolating Surface”, Computer Graphics and ImageProcessing, No.14, 1980, pp.203-26.[Bart 87] Bartels, R., Beatty J. and Barsky, B.A., “An Introducetion to Spline for Usein Computer Graphics and Geometric Modeling”, by Morgan Kaufmann, LosAltos, CA, 1987.[Boll 71] Bollinger, J.G. and Harrison, H.L., “Automatied Welding Using Spacial SeamTracing”, Welding Journal, November 1971. pp.787-92[Broo 83(a)] Brooks, R.A., “Solving the Find-Path Problem by Good Representation ofFree Space”, IEEE Transactions on Systems, Man and Cybernetics, SMCVol.13, No.3, 1983, pp.190-7.[Broo 83(b)] Brooks, R.A. and Lozano-Pérez, T., “A Subdivision Algorithm in Configuration Space for Findpath with Rotation”, Proceedings of the 8th InternationalConference on Artificial Intelligence, Karlsruhe, FRG, 1983, pp.799-806.112Bibliography 113[Buch 89] Buchal, R.O., Cherchas, D.B., Sassani, F. and Duncan, J.P., “Simulated Off-Line Programming of Welding Robots”, International Journal of Robotics Research. Vol.8, No.3, 1989, pp.31-43.[Buck 89] Buckley, S.J., “Fast Motion Planning for Multiple Moving Robots”, Proceedings of the IEEE International Conference on Robotics and Automation,Scottsdale, 1989, pp.322-6.[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”, Proceedings of the IEEE International Conference on Robotics and Automation,1990, pp.1554-9.{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, AcademicPress, 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, LawrenceErlbaum Associates, Hillsdale,NJ, 1987. pp.145-8[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 Generation for Process Automation Using Computer Vision Ph.D Dissertation, Department of Mechanical Engineering, University of British Columbia, 1993.Bibliography 114[Gupt 90] Gupta, K.K., “Fast Collision Avoidance for Manipulator Arms: A SequentialSearch 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 MobileRobots”, IEEE Journal of Robotics and Automation, vol. RA-2, No.3. 1986.pp.135-4.[Khat 86] Khatib, 0., “Real-Time Obstacle Avoidance for Manipulators and MobileRobots”, International Journal of Robotics Research. Vol.5, No.1, 1986. pp.27-41.[Khos 85] Khosla, P.K., Neuman, C.P. and Prinz, F.B., “An Algorithm for Seam TrackingApplications”, 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 Collision-free Paths Among Polyhedral Obstacles”, Communications of the ACM, Vol.22,No.10, 1979, pp.560-7.[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”, IEEETrans. on Computer. vol. c-32. no.2 February, 1983, pp.108-19.Bibliography 115[Loza 87] Lozano-Pérez, T., “A Simple Motion-Plannining Algorithm for General RobotManipulators”, 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 IntelligenceTechniques”, Proceedings of the 1st International Joint Conference on Roboticsand Automation, Washington D.C., 1969, pp.509-20.[Odon 89] O’donnell, P.A. and Lozano-Pérez, T., “Deadlock-Free and Collision-Free Coordination of Two Robot Manipulators”, Proceedings of the IEEE InternationalConference on Robotics and Automation, 1989, pp.484-9.[Reif 85] Reif, J.H. and Sharir, M., “Motion Planning in the Presence of Moving Obstacles”, Proceedings of the 25th IEEE Symposum on Foundations of ComputerScience, 1985, pp.144-5.[Schw 83] Schwartz, J.T. and Sharir, M., “On the Piano Movers’ Problem:3. Coordinating the Motion of Several Independent Bodies: The Special Case of CircularBodies Moving Amidst Polygonal Barriers, “, International Journal of RoboticsResearch. Vol.2, No.10, 1983, pp.97-140.[Shaf 92] Shaffer, C.A. and Herb, G.M., “A Real-Time Robot Arm Collision AvoidanceSystem”, 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.9-19.[Sing 91] Singh, S.K. and Leu, M.C., “Manipulator Motion Planning in the Presence ofBibliography 116Obstacles and Dynamic Constraints”, International Journal of Robotics Research. Vol.10, No.2, April 1991, pp.171-87.[Taka 89] Takahashi, 0. and Schilling, R.J., “Motion Planning in a Plane Using Generalized Vorinoi Diagrams”, IEEE Transactions on Robotics and Automation,Vol.5, No.2, April 1989. pp.143-50[Tomi 801 Tomizuka, M., Dornfeld, D. and Purcell, M., “Application of Microcomputersto Automatic Weld Quality Control”, Journal of Dynamic Systems, Measurement and Control. Vol.102, June 1980, pp.63-8.[Tour 86] Tournassoud, P., “A Strategy for Obstacle Avoidance and Its Application toMulti-Robot Systems”, Proceedings of the IEEE International Conference onRobotics and Automation, San Francisco, 1986, pp.1224-1229.[Udup 77] Udupa, S.M., “Collision Detection and Avoidance in Computer ControlledManipulators”, Proc. 5th International Conferenec of Artificial Intelligence,1977, pp.737-48.[Warr 89] Warren, C.W., Danos, J.C. and Mooring, B.W., “An Approach To Manipulator Path Planning”, International Journal of Robotics Research. Vol.8, No.5,October 1989.[Wang 92] Wang, D. and Hamam, Y., “Optimal Trajectory Planning of ManipulatorsWith Collision Detection and Avoidance”, International Journal of RoboticsResearch. Vol.11, No.5, October 1992, pp.460-98.[Wins 84] Winston, P.H., “Artificial Intellengence”, by Addison-Wesley Publishing Company, Inc. 1984.Bibliography 117[Wolo 91] Wolovich, W.A., “Robotics: Basic Analysis and Design”, by CBS College Publishing, 1991.[Zhu 91] Zhu, D. and Latombe, J., “New Heuristic Algoriths for Efficient Hierarchical Path Planning”, International Journal of Robotics Research. Vol.7, No.1,February 1991, pp.9-20.
- 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 |
FileFormat | 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 |
GraduationDate | 1994-05 |
Campus |
UBCV |
Scholarly Level | Graduate |
AggregatedSourceRepository | 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:
https://iiif.library.ubc.ca/presentation/dsp.831.1-0080927/manifest