@prefix vivo: . @prefix edm: . @prefix ns0: . @prefix dcterms: . @prefix dc: . @prefix skos: . vivo:departmentOrSchool "Applied Science, Faculty of"@en, "Mechanical Engineering, Department of"@en ; edm:dataProvider "DSpace"@en ; ns0:degreeCampus "UBCV"@en ; dcterms:creator "Seethaler, Rudolf"@en ; dcterms:issued "2009-02-20T23:36:38Z"@en, "1993"@en ; vivo:relatedDegree "Master of Applied Science - MASc"@en ; ns0:degreeGrantor "University of British Columbia"@en ; dcterms:description """Conventional servo controllers are not able to provide the high degree of accuracy that is required in many robotic and contour machining applications. New control algorithms are usually difficult to implement in practical machining or robotics environments, since they require extremely fast computer hardware, or a very exact model of the plant. Such algorithms also usually assume that the drive system may be modeled as a linear system; unfortunately this assumption is often violated during contouring operations. The objective of the work reported in this thesis is to utilize the special architecture of the UBC controller to allow the development of control algorithms that allow high speed contouring operations to be undertaken with automatic, real time, error control. The system has been simulated and tested in situations which result in significant non linearity, (sharp corner tracking being a particularly important example.) The results of these tests indicate that the system is able to achieve contouring performance that is better than other systems described in the literature to date."""@en ; edm:aggregatedCHO "https://circle.library.ubc.ca/rest/handle/2429/4871?expand=metadata"@en ; dcterms:extent "2613967 bytes"@en ; dc:format "application/pdf"@en ; skos:note "A NEW CONTOURING ALGORITHM FOR THEUBC CONTROLLERbyRudolf SeethalerB .A .Sc., University of Toronto, 1991A THESIS SUBMITTED IN PARTIAL FULFILLMENT OFTHE REQUIREMENTS FOR THE DEGREE OFMASTER OF APPLIED SCIENCEinTHE FACULTY OF GRADUATE STUDIESMECHAMCAL ENGINEERINGWe accept this thesis as conformingto the required standardTHE UNIVERSITY OF BRITISH COLUMBIAApril 1993© Rudolf Seethaler P1 qIn 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 HECl4A’CAL C’fZjThe University of British ColumbiaVancouver, CanadaDate Y.lL 2.? , 1992DE-6 (2/88)AbstractConventional servo controllers are not able to provide the high degree of accuracythat is required in many robotic and contour machining applications. New controlalgorithms are usually difficult to implement in practical machining or roboticsenvironments, since they require extremely fast computer hardware, or a very exact modelof the plant. Such algorithms also usually assume that the drive system may be modeled asa linear system; unfortunately this assumption is often violated during contouringoperations. The objective of the work reported in this thesis is to utilize the specialarchitecture of the UBC controller to allow the development of control algorithms thatallow high speed contouring operations to be undertaken with automatic, real time, errorcontrol. The system has been simulated and tested in situations which result in significantnon linearity, (sharp corner tracking being a particularly important example.) The resultsof these tests indicate that the system is able to achieve contouring performance that isbetter than other systems described in the literature to date.IiTable of ContentsAbstractTable of Contents .ListofTablesListof FiguresNomenclatureAcknowledgments• . II• . Hi• . . vu.. yin• . XI• . . xiv1123568• • . 10• . 112 Previous Approaches to the Development of Fast ContouringControe . • . . . 132.1 Introduction . . 132.2 Comparison of Existing Approaches 142.3 Sources of Error • 152.4 Feedback Controllers 162.4.1 P-Controller 16An Introduction to Robots, Applications, and Previous Research1.1 Introduction1.2 Historical Development of Robots1.3 Kinematics and Dynamics •1.4 Drive Systems1.5 Trajectory Planning1.6 Motion Control . . .1.7 Sensing1.8 Programming Languagesill• • • . 17• . . . 17• . • • 17• . . . 19• . . . 20• . . . . . 22• 22• 24• 27• 29• 29• 303.5 Sampling Period . • 313.6 Filter Parameters • • • 323.7 Mismatched Gains . 3435• . . . . . 3737383939404142432.4.2 P113-Controller2.5 Feedforward Controllers2.5.1 Zero Phase Error Tracking Controller2.5.2 Inverse Compensation Filter .2.6 Cross Coupling Controller3 UBC Controller . • . . . . •3.1 Controller Architecture3.2 Slave Transfer Function3.3 Position Spline3.4 The Application of the State Line to the Minimization of Error.3.4.1 Velocity Spline3.4.2 State Line Triggering Logic3.8 Conclusions4 Simulation and Experimental Results4.1 Introduction to Computer Simulations4.1.1 Simple Servo System4.1.2 State Line Strategies4.1.2.1 Allowable Static Error4.1.2.2 The Use of a Velocity Spline.4.1.3 Velocity Feedworward • • .4.1.4 Constant Jerk Position Spline .4.1.5 Conclusions from Simulations .iv4.2 Experimental Results.4.2.1 Experimental Apparatus4.2.1.1 Actuators4.2.1.2 Amplifiers4.2.1.3 Controller4.2.1.4 Data Recording Device4.2.2 Circular Interpolation.4.2.2.1 Low Feedrate Experiment4.2.2.2 High Feedrate Experiment464648484951525456•. 586063• . 6365•. 67Bibliography . . . . . . . .687073Appendix 1: Derivation of Following Error in the z-domain 75Appendix 2: Derivation of Constant Jerk Position Spline . . . . . 77Appendix 3 Listings of Matlab Simulation Source CodeA3.1 Main Program4.2.3 Mismatched Gains4.2.4 Disturbance Torques4.2.5 Corner Tracking4.2.5.1 Reversal at 500 RPM4.2.5.2 Reversal at 1000 RPM4.2.5.3 Comparison with Other Proposed Controllers4.3 Conclusions . .5 Conclusions and Future Developments8181VA3.2A3.3A3.4A3.5Appendix 4:A4.1A4.2A4.3A4.4A4.5Listings of Forth Code for Controller ExperimentsProgram to Calibrate AmplifiersData Array RoutinesCorner Tracking Using XYLCorner Tracking Using Lower Level CommandsFrequency Response Experiment• 8383• 84• 85• 868889• 90• 90929595• 96• 97• 98• 99Subprogram To Create Master SamplesSubprogram to Create Slave SamplesA3.3.1 Subsample with Linear Acceleration Position SplineA3.3.2 Subsample with linear Jerk Position Spline •A3.3 Subprogram that Simulates the Position LoopA3.3.1 Function to Perform Runge Kutta Second Order IntegrationA3 .3.1.1 Function Containing Differential Equations ofVelocity LoopA3.3.2 Function Containing Error Checking for State LineSubprogram to Plot ResultsProgram to Calculate Filter ParametersviList of Tables4.1 Execution Time and Path Error of Simulated Strategies 434.2 Motor Parameters 484.3 Amplifier parameters 484.4 Table of Model Constants 494.5 Parameters for Low Feedrate Circular Interpolation 544.6 Parameters for High Feedrate Circular Interpolation 564.7 Performance in Corner Tracking 68vi’List of Figures1.1. Cartesian Robot . 41.2 Cylindrical Robot 41.3 Spherical Robot . 41.4 Revolute Robot . . 42.1 Typical Position Control Loop 142.2 P-Controller and PD-Controller Structure 172.3 Block Diagram of Zero Phase Error Tracking Controller 182.4 Block Diagram of Inverse Compensation Filter Controller . . 192.5 Block Diagram of Two Axis Cross Coupled Controller 213.1 UBC Controller Architecture 233.2 Schematic Block Diagram of a Two Axis Control System 243.3 Block Diagram of Position Loop 253.4 Block Diagram of Continuous Position Loop 263.5 Position Loop of Simplified Continuous Model 263.6 Position Spline 283.7 Velocity Spline 303.8 Block Diagram of Position Loop 323.9 Block Diagram of Reduced Position Loop . . . 333.10 Mismatched Gains 354.1 Corner Tracking with a Simple Servo controller 384.2 Corner Tracking with a Static Error Criterion . . 39vifi4.34.44.54.64.74.84.94.104.114.124.134.144.154.164.174.184.204.214.224.234.244.254.264.27• 40• . 41• . 42• • 44• 45• . 44• . 46• 50• 51• . 52• . 54• 55• . 55• • 56• . 57• 57• 58• • •. 59• . •. 5960• . . . 61• . • . 62• . • . 626464Corner Tracking with a Velocity SplineCorner Tracking with Velocity FeedforwardCorner Tracking with a Constant Jerk Position Spline .Enlargements of Paths in Corner Tracking at 500 RPM .Enlargements of Paths in Corner Tracking at 1000 RPMSchematic Diagram of Drive SystemSchematic Block DiagramBode Plots of Experimental SystemStep Response of Experimental SystemCircular Interpolation.Maximum Radius vs Maximum FrequencyPath of Low Feedrate Circular Interpolation ExperimentPath Error of Low Feedrate Circular Interpolation ExperimentPath of Simple Servo System During High Feedrate ExperimentPath Error of Simple Servo System During High Feedrate ExperimentPath of Servo System with Active State Line During High FeedrateExperiment4.19 Path Error of Servo System with Active State Line During High FeedrateExperimentPath with Mismatched GainsPath Error with Mismatched GainsPath of Simple Servo System with a Torque DisturbancePath Error of Simple Servo System with a Torque DisturbancePath of UBC Servo System with a Torque DisturbancePath Error of UBC Servo System with a Torque DisturbanceCorner Tracking of Simple Servo at 500 RPM . .Corner Tracking of UBC Controller at 500 RPM •ix4.28 Close-up of Corner Tracking of Simple Servo at 500 RPM 654.29 Close-up of Corner Tracking of UBC Controller at 500 RPM 654.30 Corner Tracking of Simple Servo at 500 RPM 664.31 Corner Tracking of UBC Controller at 500 RPM 664.32 Close-up of Corner Tracking of Simple Servo at 500 RPM 674.33 Close-up of Corner Tracking of UBC Controller at 500 RPM 67A2. 1 Constant Jerk Position Spline. . . 79xNomenclatureA: Filter lead constant in z-domainA(z): Denominator (poles) of position loop transfer functionB: Filter lag constant in z-domainB(z): Numerator (zeroes) of position loop transfer functionB+(z): Cancelable zeroes of position loop transfer functionB-(z): Non cancelable zeroes of position loop transfer functionD(z): Drive transfer functionE: Error signalEx \\ Ey: Position errorsF(s): Filter transfer function in s-domainF(z): Filter transfer function in z-domainG(s): Position open loop transfer functionG(z): Inverse of position loop transfer functionGf(z): ZPETC transfer functionH(z): Controller transfer functionI: Current‘max: Current saturation limitIKF(z): Inverse compensation filter transfer functionJe: Equivalent inertiaK: Filter gain in s-domainK 1: Loop gainKD Derivative gainKI Integral gainKP Proportional gainKa: Amplifier gainKd: D/A gainKe: Encoder gainKp: Filter gain in z-domainKss: Velocity loop gainKt: Torque constantKtsa: Velocity feedback gainLP(z): Linear phase low pass filter transfer functionxiMx \\ My: First Stage Interpolated Positions (Master Position)N: Order of linear phase low pass filterNa: Slave samples required for accelerationNd: Slave samples required for deceleration0: Filtered signalR: Angular reference positionRx \\ Ry: Reference positionsfor lead-lag controllersSLx \\ SLy: State line flagsSL: State lineSx \\ Sy: Second stage interpolated positionsT: Available torqueTd: Disturbance torqueU: Control signalVx \\ Vy: Velocity spline signalsV1: Master Controller velocitiesW(z): Cross coupling controllerXi: Master positionsa: Filter lead constant in s-domainao: initial acceleration in constant jerk position splineal,a2,a3: Denominator coefficients of position loop transfer function in thez-domainb: Filter lag constant in s-domainb1,b23: Numerator coefficients of position loop transfer function in the zdomainess: Ramp following erroreallowahie: Allowable errorCircular interpolation frequencyfh: Position loop bandwidthmax: Maximum circular interpolation frequencyio: Jerk in constant jerk position spliner: Circular interpolation radiusrmax: Maximum circular interpolation radiuss: Distance traveledt: Timev: Velocity spline velocityvo: Initial velocityxl’vf: Final velocityx: Interpolated positionxo: Initial positiona: Angular accelerationcxl, a: Parameters for transferring the velocity loop to the z-domainamax: Maximum angular acceleration: Parameters for transferring the velocity loop to the z-domainöstatic: Allowable static errorSlave sampling timeAt: Master sampling timeDamping ratio of position loopPerformance factor in corner contouring0: Angular actual positionAmplifier time constantti: Velocity loop time constantSteady state ramp phase lagAngle between master samples at circular interpolationAngular velocityCutoff frequency of linear phase low pass filterCI)max: Maximum angular velocityNatural frequency of position loopXII’AcknowledgmentsI wish to thank my supervisor Dr. Ian Yellowley for providing me with valuabletechnical and moral support during all stages of this research. He also gave me the benefitof extended commentaries rich in insight and helpful criticism.I would also like to thank my colleagues for their friendship and support during thecourse of this work.Finally funding from the Natural Science and Engineering Research Council ofCanada and the Advanced Systems Institute is gratefully acknowledged.xivChapter 1An Introduction to Robots, Applications, and Previous Research1.1 IntroductionThe necessity to achieve higher quality control and increased productivity haslead progressive companies to examine computer-based automation. At the present time,many tasks are carried out by special purpose machines that are able to perform specifictasks at high production rates. Such systems are generally referred to, in a generic sense,as hard automation systems. It is evident however that in the future, it may well be betterto have general purpose machines, that are able to overcome the inflexibility and the highcost of product changes. This usually implies the application of robotic devices.Early applications of robots usually involved only simple pick and placeoperations. The robots were required to accurately position objects at certain positions inspace, but they did not have to follow a specified path between the final positions with ahigh degree of accuracy. During the last decade, robots have been put into use incontinuous operations such as welding, abrasive finishing, and adhesive applications.These tasks require the actuator to follow the prescribed paths very accurately. At thesame time, one seeks to run at the highest speed possible, (consistent with path andprocess constraints.) Conventional servo controllers are not usually able to achieve thenecessary path accuracy at high speeds without extensive path preprocessing.The objective of the research described in this thesis is to develop a contouringcontrol algorithm that is able to address the requirements of high speed profiling andprocess control. In the course of the work a candidate robot, to be used in furtherresearch, has been selected and refurbished. The robot (a GMF S-108) was retrofitted1Chapter 1 An Introduction to Robots, Applications, and Previous Research 2with high performance actuators and PWM amplifiers. The overall aim of the controllerdesign was to perform all path control activities in real time, and to reduce thecumbersome preprocessing activity used in other approaches. The controller used is anexisting UBC designed, multi-processor based system. The architecture allows not onlymaster-slave communication, but also high speed interaction between slave processors.The latter feature has proven to be critical in this application.1.2 Historical Development of RobotsThe overview in this section is based on Craig [1]. References in this section aredirectly quoted from Craig.The first work on industrial robots started in the late 1940’s. The Oak Ridge andArgonne National Laboratories began work on a remotely controlled manipulator tohandle radioactive materials. These systems were designed to reproduce the hand andarm motions of the operator, and they were later equipped with force feedback, allowingthe operator to “feel” the forces acting between the slave manipulator and itsenvironment.In the 1950’s the first systems capable of autonomous, repetitive operations weredeveloped. The first industrial robot was introduced by Unimation INC. in 1959. It useda computer in conjunction with a manipulator that could be taught to carry out a varietyof tasks.It quickly became evident that sensory feedback would improve the usefulness ofrobots. Ernst[2] introduced a mechanical hand with tactile feedback that could feelblocks and use that information to stack them without operator assistance. In 1968 amanipulator equipped with a TV camera and a microphone was built at Stanford. It wasable to recognize spoken messages, see blocks scattered on a table, and manipulate theblocks (in a variety of ways.)Chapter 1 An Introduction to Robots, Applications, and Previous Research 3In the late 1960’s Pieper [3] studied the general kinematics problem ofmanipulators, while Kahn and Roth [4] analyzed robot dynamics and proposed the firstnear minimum time control strategy.During the 1970’s, most work was focused on the use of sensors to facilitatemanipulative or assembly tasks.Today robotics research is a very broad field that includes kinematics, dynamics,planning systems, control, sensors, programming languages, and machine intelligence.13 Kinematics and DynamicsKinematic analysis is used to study the geometry of motion of a robot withrespect to a base coordinate system. It describes the displacement of the manipulatorwithout regard for the forces and moments that cause the motions. The analysisexamines the relationship between the joint-variable space and the position andorientation of the end-effector in Cartesian space. Figures 1.1 to 1.4 show a number oftypical manupulator types. Paul [5] presents a comprehensive introduction to Kinematicsand Dynamics.There are two basic problems that are of interest. The so-called forwardkinematic solution describes the position of the end-effector in terms of the jointvariables, while the backward kinematic solution determines the joint-angles in terms ofthe position and orientation in Cartesian space. Usually the task is stated in terms of thereference coordinate frame. This requires that backward kinematic solutions must beavailable in order to determine the joint-angles.Chapter 1 An Introduction to Robots, Applications, and Previous Research 4Figure 1.1. Cartesian RobotFigure 1.3. Spherical Robot Figure 1.4. Revolide RobotChapter 1 An Introduction to Robots, Applications, and Previous Research 5A matrix representation of link geometries was first presented by Denavit andHartenberg [6]. This approach allows one to represent the spatial geometry of the linkswith respect to a base reference frame. It utilizes 4x4 homogeneous transformationmatrices that relate adjacent links, and reduces the problem to finding an equivalent 4x4matrix relating the hand coordinate frame to the reference coordinate frame.Robot dynamics deals with the mathematical formulation of the equations ofrobot arm motion. They are useful in computer simulations and the design of robotmotion controllers. The dynamic equations are derived from the laws of Newtonian andLagrangian Mechanics. Using the Denavit-Hartenberg convention it is possible todevelop standard procedures to derive the equations of motion in terms of specifiedgeometric and inertial parameters. Two standard procedures for this task are theLagrange-Euler formulation, first developed by Uicker [7], and the Newton-Eulerprocedure, introduced by Stepanenko and Vukobratovic [8].1.4 Drive SystemsThere are a number of different drive systems that can be used to power a robot.The type of drive is usually determined by the application (task) for which the robot isdesigned. Koren [9] describes some of the basic drive systems.Electrical drive systems are usually used for small to medium size manipulators.Direct current actuators provide good speed regulation, high torques and efficiencies, andhave therefore traditionally been used for high precision operations. However the lowpower to size ratio usually requires gearing systems. Being angular actuators, theyrequire ball screws or similar devices to provide linear motion.With the development of new high power electronics, alternating currentactuators have become more available. They are beginning to replace DC actuators,because they have slightly better characteristics.Chapter 1 An Introduction to Robots, Applications, and Previous Research 6Stepping motors are incremental digital drive systems that rotate one angularincrement per command pulse. They usually do not have position feedback. This meansthat they operate in an open loop control system that does not let the controller knowwhether or not the actuator has actually performed the prescribed motion. Large positionand velocity errors cannot be detected. Usually such actuators have a very limited powerand torque capacity.Hydraulic actuators are used for large robots. They provide high power withsmall units, and the response characteristics are very good. Linear and rotary actuatorsare readily available and no gearing is required. Hydraulic actuators however haveseveral disadvantages. They require highly pressurized liquid as the operating fluid,which often causes maintenance problems. The response characteristics change with thetemperature of the working fluid and they are also strongly influenced by entrainedmaterials. The relative cost of a hydraulic system does not decrease with size and hencethey are too expensive for small systems. Finally the various devices used in hydrauliccontrol elements are often distinctly nonlinear, leading to significant design problems.Pneumatic actuators are usually used in small manipulators, which are designedfor point to point operations. They have fast response times, and compressed air isusually readily available. On the other hand they require relatively large actuators,because of the low pressures, and they produce a considerable amount of noise.1,5 Trajectory PhinningWith the knowledge of kinematics and dynamics of the actuators one would liketo control the actuators to make the manipulator follow a desired path. The space curvethat the manipulator describes while moving from an initial location (position andorientation) to a final location, is called a path. In trajectory planning one is interested inChapter 1 An Introduction to Robots, Applications, and Previous Research 7constructing such a path. Craig [101 discusses trajectory planning in a broader scope thanthis introduction allows.There are three constraints that enter trajectory planning. The path constraintspecifies what path the manipulator has to follow. The obstacle constraint restricts thepath to manipulator motions that will not interfere with other objects (obstacles) alongthe trajectory of the manipulator. Finally it is of course impossible to put infinite powerinto an actuator, and hence discontinuities in the path and its first second and thirdderivative should be avoided. This last constraint is referred to as the manipulator’sdynamic constraint.There are basically two approaches to trajectory planning. In the first approachknot points are selected along the path in the manipulator’s generalized coordinate frame(joint coordinates). Using constraints such as continuity and smoothness of position,velocity, and acceleration, polynomials are fitted between the knot points. Theyguarantee that the constraints are met at the knot points. With this method it is difficultto check for obstacle avoidance, because the trajectory is planned in joint coordinates,making it difficult to trace the path of the manipulator in Cartesian space.The second approach specifies the path in an analytical function such as a straightline or a circle in Cartesian space. Thus, obstacle avoidance can be checked for in theinitial stage of calculation. The Cartesian path constraint must then be transferred tojoint path constraints. Again a splining operation is conducted during the fit between thejoint knot points, which will ensure continuity and smoothness of position, velocity, andacceleration.Usually the actuator dynamics are not considered in the trajectory planning phase,because of their computational complexity. Thus, large tracking errors will result withinthe servo control of the manipulator, if the actuators cannot supply the torque requiredfor the desired path.Chapter 1 An Introduction to Robots, Applications, and Previous Research 816 Motion ControlMotion control is the process of controlling the actual actuators of a manipulatorin order to achieve a desired trajectory. Motion control is usually divided into two parts.First the dynamic model of the manipulator is obtained. This model is then used todetermine a suitable control law. There are usually two distinct phases in a motionsegment. First the manipulator is moved from its initial position to the vicinity of thefinal location along a planned trajectory. In the second segment fine-motion controloften in combination with sensory feedback is employed to make the end-effectordynamically interact with the object.Currently most industrial robot controllers do not utilize information from thedynamic equations of motion, but rather use simple servo controllers. This leads to therequirement for slow speeds, (often accompanied by unnecessary vibrations). In order toimprove the performance, more sophisticated control approaches and dedicated computerarchitectures are often utilized. There are two basic philosophies in modern contouringcontrol. The first attempts to minimize path error by minimizing the position error ofeach axis. The usual way that this is achieved, is to insert a filter before the control loopthat is the exact inverse of the actual control ioop. In theory this should lead to a unitytransfer function. The initial problem, that such a controller is often unstable, was solvedby Tomizuka [11] by only canceling the stable zeroes of the system. Still this approachhad two flaws. First it requires a very accurate model of the system, since otherwise theposition loop cannot be canceled exactly. One would therefore have to use an exactdynamic model of the actuators, and the equations of motions of the robot arm, includingloads and disturbances. This is extremely computationally complex, and it is oftenimpossible to extract exact information on the load and disturbances. The secondproblem arises from the saturation limits of the amplifiers and motors. In order to cancelthe position loop dynamics, large control signals are required, which can saturate theChapter 1 An Introduction to Robots, Applications, and Previous Research 9amplifiers. This problem becomes more pronounced when the frequency components ofthe reference path increase. (Thus, the system performance deteriorates around corners.)Both problems have been addressed by Weck [121. He proposed the use of a regressivesystem characterization to determine the position loop model, and used this information,instead of an analytical model, to design the inverse filter. In his proposal the systemcharacterization is carried out in the start up phase. Therefore the system model does notcontain loads and position dependent arm dynamics. In order to get a truly accuratemodel it would be necessary to perform system characterizations throughout thecomplete operating cycle. This in turn poses problems with large computational efforts.Weck also proposed the use of a linear phase low pass filter in order to reduce the highfrequency components in the reference signal. This idea seems to work very well forimproving corner tracking.The second approach was first proposed by Koren [13]. This approach attemptsto minimize the path error directly instead of the single axis error. The approach requiresthe controller to calculate the path error from the single axes errors and then use this patherror to compensate the different axes, a strategy which clearly requires cross coupling oferrors. The controller is complicated and the computational effort rises with the squareof the number of axes. To this date the author has not been able to find a paper thatdiscusses the implementation of this system on high performance servos, which requirefast sampling times in order to fully utilize their capabilities. It seems that thecomputational effort is too large to actually implement this system with today’scomputing capacity.The algorithm that is proposed in this thesis does not rely on an exact dynamicmodel. It therefore does not require regressive system characterizations. The algorithmrelies on the principle that an error proportional to the reference velocity in each axisleads to a phase lag between the actual and the reference signal, but the path accuracyitself is not compromised by such an error. The algorithm then predicts an allowableChapter 1 An Introduction to Robots, Applications, and Previous Research 10error proportional to the velocity, compares it with the actual error, and inserts extra timesteps in those cases where the allowable error is exceeded. This algorithm is very fastand has been implemented in real time. It can be implemented on other industrialequipment without major changes, because it is not dependent on an exact dynamicmodel.1.7 SensingThe use of external sensors allows a robotic manipulator to interactively adapt tochanges in its environment. Even though, at present, most tasks only require the robot toperform preprogrammed repetitive tasks, a great deal of effort has gone into thedevelopment of new sensory techniques. Merrit [14] has presented a detailed surveyarticle on robot sensing.There are two categories of sensors. Internal sensors detect variables such as armjoint position that are used by the controller. External sensors detect variables such asrange, proximity, touch, force and torque, and vision. They are used for guidance as wellas object identification. Vision is potentially the most powerful robot sensory capability.It can be defined as the process of extracting, characterizing and interpreting informationfrom images of a three dimensional world. It is often divided into sensing,preprocessing, segmentation, description, and interpretation.The unique architecture of the UBC controller that is described in Chapter 3allows the integration of contouring control with external sensory inputs. Force sensorsin metal cutting operations have been used to ensure a constant cutting force and aconstant equivalent chip thickness.Chapter 1 An Introduction to Robots, Applications, and Previous Research 111.8 Programming LanguagesCommunication between the operator and the manipulator is the major obstacle inutilizing a general purpose assembly machine. Fu et al. [15] have presented anintroduction to this topic.There are three major approaches to this problem. Discrete word recognition isvery limited, because state of the art speech recognition can only recognize discretewords from a limited vocabulary. It is also speaker dependent, it needs very powerfulcomputers for efficient real time algorithms, and it requires a large memory space tostore the speech data. As a result discrete word recognition has not yet found its way intoreal industrial applications.When using teach and playback, an operator slowly leads the robot (off line)through the entire assembly task with a manual control. A computer records points alongthat path in order to be able to replay the motion at a later time. The taught motion isplayed back and edited until it contains no errors. Finally the robot is run on line at itsappropriate speed to perform the taught motion repetitively.The most advanced approach to solve the communication bottleneck between theoperator and the manipulator, is to use high level programming. When using a robot forsuch tasks as welding or spray painting, no interaction is required between the robot andits environment. If the manipulator is used for assembly purposes, sensory feedback isusually required. This can only be handled by conditionally programmed methods thatare best implemented using high level programming techniques. There are twoprogramming categories for robotics applications.In robot-oriented programming assembly tasks are divided into a number of robotmotions. The programmer has to guide the robot through the task by sequentiallyprogramming every single motion segment. This procedure is very cumbersome makingit difficult to use.Chapter 1 An Introduction to Robots, Applications, and Previous Research 12In task-oriented programming, assembly tasks are described in terms of objectsbeing manipulated rather than by robot motions. This means that the programmer canspecify a task such as picking up an object in a high-level language, and the programthen uses a database to convert the task to a robot-level program. This procedure is muchmore user friendly than robot-oriented task planning, but many problems, notably taskplanning, obstacle avoidance, and sensory information utilization still remain to besolved.The UBC controller, which is described in detail in Chapter 3, uses high levelprogramming that allows easy trajectory programming which can be integrated withtechnological sensory feedback to ensure optimal operating conditions.Chapter 2Previous Approaches to the Development of Fast Contouring Controllers2.1 IntroductionIn the last decade many researchers have begun to examine the development ofhigh speed contouring algorithms. This has been accompanied by the development offaster computer hardware that allows one to run control strategies that were notpreviously feasible.Two main approaches have arisen from this previous work. The first controlstrategy, proposed by Tomizuka [11] tries to eliminate path error by achieving zero axisfollowing error. It attempts to cancel the traditional control loop with an inverse filter ofthe position loop. For this system to perform adequately, it is necessary to have a veryexact model of the plant.This strategy unfortunately often leads to the saturation of amplifier current andthe violation of other torque-related constraints, during phases of large acceleration.Weck [12] proposed a solution to this problem which involves placing a low pass filter infront of the controller. This alleviated the saturation problem, and better corner trackingwas achieved.The second approach, introduced by Koren [13], tries to minimize path erroralone, while still allowing actual axial position errors. It calculates a path error from theindividual errors of the axes and then uses this path error to compensate the individualaxes. This strategy requires very fast hardware, and to date the author has not been ablefind a paper on an experimental setup using high performance servos in combinationwith this strategy.13Chapter 2 Previous Approaches to the Development ofFast Contouring Controllers 142.2 Comparison of Existing ApproachesA paper comparing the various advanced controller strategies has been presentedby Koren and Lo [16]. The course of their argument will be followed in this introduction,following which an introduction to existing methods and the new concepts arising fromthe author’s work, will be compared with the simulated and experimental results obtainedby Koren.Figure 2.1 shows a typical servo system. The actual position is fed back andsubtracted from the desired position. This error signal is fed into a controller. Thecontroller then feeds a velocity command to the plant. Usually the plant may be modeledas a first order lag. In the simplest case, the controller has a simple proportional gain.This system has been used in many NC machines. Advanced CNC applications requirevery high feedrates, which lead to large contouring errors with P-controllers. Thus, Pcontrollers are often not adequate for advanced CNC applications.Three basic methods have been developed to overcome the limitations of a simpleproportional control action:The first approach applies more sophisticated axial controllers such as P1]) orstate feedback controllers. These controllers attempt to reduce the position errors of theDisturbancesFigure 2.1. Typical Position Control Loopindividual axes.Chapter 2 Previous Approaches to the Development ofFast Contouring Controllers 15The second approach adds a feed forward controller, such as the ZPETC or IKFapproaches discussed in the introduction, to compensate for axial position errors.The third method uses cross coupling in order to reduce the contour error ratherthan the axial tracking error. The controller proposed in this thesis has some of thecharacteristics of the third method, since the axes can communicate with each other via astate line. Unlike the original system proposed by Koren, this proposed system is verysimple. It can handle more degrees of freedom and it is possible to optimize contouringperformance and technological factors such as maximum cutting forces, at the same timeand with very little more effort.There are variations and accessories to these three basic control approaches. Themost prominent are adaptive control and predictive control as mentioned in theintroductory chapter.2.3 Sources of ErrorThere are three different types of error:The first are due to mechanical errors. This category contains backlash ornonstraightness of the table motion. It cannot be improved upon by the simple additionof more sophisticated controllers. Those errors can only be reduced by using betterhardware or compensation techniques.The source for the second type of error is the effects of the production process.Machine tool operations for instance lead to errors due to tool wear or tool deflection.These errors must also be handled through compensation.The third type of error arises from the controller and drive dynamics. Theseerrors are often dominant in high speed operations, and they can be alleviated byimproving the control algorithm. The three above controller types and the one proposedChapter 2 Previous Approaches to the Development ofFast Contouring Controllers 16in this thesis all concentrate on this third class of error, which will further be subdividedinto three categories:The first category is parameter mismatch. In a type-I control system, a mismatchin the open loop gains will lead to a steady-state error when following a straight line.Different time constants on the other hand, will lead to transient contour errors that maybe more prominent than those due to mismatched gains.The second type of error is due to disturbances. The disturbances result fromcutting forces or other parameters imposed by the process.The third kind of error stems from the contour path and the machine dynamics.When the desired path contains a discontinuity or a sharp corner, the system is oftenunable to follow the path within the prescribed tolerances. The major problem in suchcases is the fact that the maximum acceleration is limited by the saturation current of theamplifier.Most of these errors increase with feeding speed. This means that more effectiveservo controllers are needed for high speed operations.2.4 Feedback ControllersIn this category controllers with basic feedback only will be listed.2.4.1 P-ControllerThe P-controller is a proportional gain that is usually tuned such that the closedloop damping ratio is approximately equal to 0.707. This controller works well for lowfeed rates.Chapter 2 Previous Approaches to the Development ofFast Contouring Controllers 172.4.2 Pm-ControllerThe PID-controller adds the integrated error and a derivative of the error to thecontrol signal. The integral term ensures that the system has zero tracking error for rampinputs (assuming the uncompensated system is type I), and the derivative term shapes thedynamic response of the system. The Pm-controller provides poor tracking andsignificant overshoots for nonlinear contours like sharp corners which cannot bedescribed with first order polynomials. These problems are addressed bypreprogramming a path with sufficiently small accelerations.Figure 2.2. P-Controller and PID-Controller Structure2.5 Feedforward ControllersThe idea behind this type of controller is to implement a filter in the control loopthat leads to a unity transfer function of the system.2.5.1 Zero Phase Error Tracking Controller (ZPETC)The ZPETC controller was first proposed by Tomizuka [11]. It introduces a filterbefore the position feedback loop in an attempt to cancel the zeroes and poles of thesystem. A block diagram of this controller is shown in Figure 2.3.P-Controller P1 D-ControllerR+Chapter 2 Previous Approaches to the Development ofFast Contouring Controllers 18G0(z) is the feedforward filter that is intended to cancel the position loop. Ideallyit should have the following form:G0(z) (B(z)1=1 H(z) D(z) (2.1)kA(z)J L\\l+H(z)D(z))It is not possible to cancel the zeroes outside the unit circle because to do sowould lead to an unstable controller. The zeroes are divided into cancelable and noncancelable zeroes:G0(z) = (B+(z) B(z))1 (2.2)A(z)Tomizuka only cancels the cancelable zeros B+and arrives at the followingcontroller:G(z) A(z1)B(z) 2 (2.3)B(z1)[B(1)]The phase angle of the overall transfer function of this system is zero, whichimplies that this system achieves zero phase error tracking. Haack and Tomizuka [17]proposed the reduction of the possible gain error by adding zeroes to the feedforwardcontroller.This system requires that the model of the system be very accurate. Disturbancetorques will severely reduce the performance of the system.Reference Error ActualPosition PositionRFigure 2.3. Block Diagram ofZPETCChapter 2 Previous Approaches to the Development ofFast Contouring Controllers 19This controller also produces very large control signals that can saturate theamplifiers. Buttler et a!. [18], recognizing this problem, have proposed the use of pathplanning to avoid large accelerations.2.5.2 Inverse Compensation Filter (IKF)The ZPETC controller inherently requires large control signals which saturate theamplifiers and motors during moves with high frequency components. This problem wasalleviated by Weck [12] by putting a low pass filter with linear phase in front of theZPETC. Figure 2.4 shows the a block diagram of the IKF Controller.The ideal IKF filter has the transfer function described in equation 2.4:IKF(z) = .ft3 AT + [i + cos(i it/N)] sin(i AT) + z’)l A(Z1) B (z) z: 1(2.4)i-i 2z it J I iHere N is the filter order, and Weck proposed that N be equal to the ratio of thedrive time constant and the controller sampling time. AT is the controller sampling time,c is the cutoff frequency of the speed controlled drive system, and d is the estimateddead time.Reference Signal after Signal after Error Control ActualPosition Low Pass ZPETC PositionRFigure 2.4. Block Diagram ofIKFChapter 2 Previous Approaches to the Development ofFast Contouring Controllers 20Week also proposed the use of a recursive least square model identificationaccording to the Bierman factorization method, instead of an arithmetical model. Thisshould eliminate large computing efforts and the insecurity of model design.The results obtained with this method seem to solve most of the ZPETCcontroller’s problem. Still the system tends to become unstable for sampling times of lessthan 5 ms. The threat of saturating the amplifiers has been considerably decreased, butnot eliminated.2.6 Cross Coupling Controller (CCC)This controller was first proposed by Koren [13]. It is based on the idea that, inorder to improve the contouring performance, it is necessary to eliminate the path errorrather than individual axis errors.This controller uses a contour error model that is then implemented in a simplecontrol law. Figure 2.5 shows a block diagram of a cross coupled controller.The error model is given by equations for Cx and Cy, which are functions of theinstantaneous slope and the radius of the contour, and the errors of the system,respectively.The cross coupling controller uses a simple P1]) control law of the form describedin equation 2.5:W(z)=KP+KITz+KDzl (2.5)z-1 ATzThis controller structure has been tested by Koren and Lo [19]. It was able togive an improvement in performance of 1:5 to 1:10 compared to a simple P-controller.The main disadvantage of this controller is that it requires very fast hardware inorder to perform in real time. At this time a three axis system is operating satisfactorily.This three axis system has a slow time constant, which in turn requires only a lowChapter 2 Previous Approaches to the Development ofFast Contouring Controllers 21sampling rate. It seems that it will be difficult to use this system with high performanceservos, as such servos need very fast sampling rates to give optimum results.Chapter 3UBC ControllerThe UBC controller is an open architecture multi-axis controller that wasspecifically designed to coordinate a large number of axes, and to allow integration ofprocess control with position and velocity control.3.1 Controller ArchitectureThe controller utilizes the STD 32 Bus. The master is an IBM PC compatiblecomputer, which can control up to 15 slave controllers. In the work described in thisthesis, the master updates the position of each slave every 16 ms; this corresponds toapproximately twice the time constant of the slave servos. The slaves are digital lead lagcontrollers that interpolate 32 times between every master position. They have asampling time of approximately 0.5 ms.The architecture ensures that most of the workload is taken from the master andtransferred to the slave controllers.Every slave controller is connected to a so-called state line. If the state line ishigh, the slaves perform as ordinary lead-lag controllers. If the state line is low however,the slaves will not increment position, but will continue to close the position loop. Eachslave is able to pull the state line low, in response to conditions where some errorcriterion is met. The arrangement described means that each slave is able to slow downor stop the complete system when it cannot follow the required path at the programmedvelocity.Figure 3.1 shows a schematic diagram of the controller architecture:22Chapter 3 UBC Controller 23STD BUSII II II IIMaster Slave Slave SlaveCPU CPU CPU CPU80x86 Axis 1 Axis 2 Axis 3Coordinating LineState Line Jj.. Jj,Front Plane BusFigure 3.1. UBC Controller ArchitectureThe slaves receive position incremnents every master sampling time. Theseposition increments are modified twice. First the slaves perform a second stageinterpolation. This procedure reduces the sampling time to half a millisecond and it alsoprovides velocity shaping in order to reduce acceleration levels between positionincrements. The second operation that is performed on the reference signal is velocitymodulation with the state line. This concept allows the reduction of acceleration levelsbelow the saturation limits of the amplifiers. The signal modified in this way is then fedinto a simple lead-lag servo controller. It should be noted, that it is quite difficult toanalyze the complete slave controller with conventional control techniques, because thesystem is not linear and the author was not able to obtain a closed transfer function of thesystem. A comprehensive stability analysis of the system has not been performed, butsimulations and experiments of the control system including the state line strategy tend tobe more stable than the simple servo controller. A schematic block diagram of a two axiscontrol system is shown in Figure 3.2.Chapter 3 UBC ControllerLineIf SL-1,Ry - SyElseRy - SyEndFigure 3.2. Schematic Block Diagram of a Two Axis Control SystemMx \\ My First Stage Interpolated PositionSx \\ Sy Second Stage Interpolated PositionRx \\ Ry Reference Position of Lead-Lag ControllerVx \\ Vy Velocity SplineEx \\ Ey Position ErrorSLx \\ Sly State Line FlagsSL State Line24In the following sections the lead-lag controller, the position spline, and velocitymodulation using the state line will be discussed.3.2 Slave Transfer FunctionThe block diagram of the position loop of the conventional lead-lag controller ofStateMyIRyVelocitySpline VyIf Vy•+ östat> Ey,Else SLy-OSLy— 1EndEythe slaves is shown in Figure 3.3:Chapter 3 UBC Controller 25The block diagram shows that the system is half continuous and half discrete. Inorder to analyze the system it is first necessary to transform it into either form.The easiest approach in this case, where the sampling frequencies are relativelyhigh, is to convert the digital filter and the zero order lag to a continuous filter. Thecontinuous filter will have the following form:0 s+aF(s)—K (3.1)E sbUsing forward difference approximations this can be rewritten as:KEk+l +K(aIT—1)Ek °k+1 +(bAT—1)Ok (3.2)This corresponds to the lead lag filter in the z-Domain given by:z+(aT—1) z—AF(z)K =Kp (3.3)z+(bT—l) zBThe transformations from the continuous to the discrete domain are:A1-aciTBbT—1 (3.4)Kp=KChapter 3 UBC Controller 26A simplified continuous block diagram is shown in Figure 3.4. (Both saturationlimits and the amplifier bandwidth are neglected in this case.)1Kss =____KtsaJeti =_ ____Ka Kt KtsaThe resulting block diagram is shown in Figure 3.5There are two very important characteristics of this system. First, it has a limitedacceleration due to current saturation. By inspecting Figure 3.3 it becomes apparent thatthe maximum achievable acceleration is:The velocity loop is a first order lag of the form:u Kss35i1 ts+1 .)where:Figure 3.5. Position loop ofsimplified continuousmodelChapter 3 UBC Controller 27Ke= IKt— (3.6)JeThe second important characteristic is the expected following error to a rampinput. The open ioop transfer function of the model in Figure 3.5 is:G(s)=KKdKssKe sa (37)(s + b)(t1 s + i) sThis is a type I system and the steady state error to a ramp input can be shown tobe:b Ktsae38 = v = (3.8)a K Kd KeThis means that the following error is proportional to velocity. In other wordsduring a ramp, the steady state response will result in the actual signal always lagging aconstant time behind the reference signal. If the different axes have the same gains, thepath error will be zero, even though the actual path lags behind the reference.Equation 3.8 also shows that it is possible to control the velocity of the system bycontrolling the error.An equivalent expression for the following error in the z-domain is derived inAppendix 1. It yields the same numerical values as equation 3.8, but it is much morecomplex. The simple expression derived in the s-domain is used for further analysis andcalculation.3.3 Position SplineBefore the reference positions are transferred from the master to the lead-lagslave controllers, the slaves performs an interpolation operation. This operation is calledposition splining and it aims to reduce large accelerations of the reference signal byshaping the velocity during master samples. The position spline also transfers some ofChapter 3 UBC Controller 28the work of the master to the slaves, because the master needs to send less positionincrements to the slaves. This enables the master to concentrate on other operations suchas task planning.The Interpolation scheme was developed by Yellowley and Pottier [201. Itassumes that points are evenly spaced in time, and it assumes a linear variation invelocity over each master sampling period. This leads to the following formula fordisplacement:= + —+ [x1+2 — X—X1+ Xe.., I [t2 - (3.9)where 0t&Figure 3.6 shows the velocity profile which results from this scheme. The splineassumes a constant acceleration and zero jerk over a master sampling interval. At everymaster sample a velocity discontinuity is encountered.velocity/ %— master samplesA—— position spline\\A--t-i ‘ -iimeFigure 3.6. Position SplineA position spline with linear acceleration and constant jerk that would have acontinuous velocity profile was investigated, as a first alternative, and its derivation isChapter 3 UBC Controller 29given in Appendix 2. The added computational complexity did not yield a significantimprovement in performance and so the constant acceleration spline was maintained,together with the addition of a velocity spline which is described in a later section.3.4 The Application of the State Line to the Minimization of ErrorIt was shown in the previous section that the position spline contains infiniteaccelerations. As shown in equation 3.5 however, there is a maximum achievableacceleration. The state line will be used to smooth the velocity discontinuities. Thebasic idea, which will be expanded upon in this section, is to allow the smoothing ofvelocity and acceleration by triggering the state line based upon error. The reader willrealize that this is the inverse of the normal procedure. This is achieved by controllingthe amount of error, and hence the velocity before and after a large acceleration. In theproposed system one is indirectly controlling the velocity through error.3.4.1 Velocity SplineEquation 3.8 allows one to stipulate an error criterion for the state line:F b 1e,JJk = v I + = + (3.10)LaKKdKssKeJThe first term allows for the expected error proportional to velocity and statallows for friction, and other imperfections in the system.Ideally an actual reference velocity should be used to calculate the allowableerror. The reference velocity based upon the position spline contains accelerations thatare beyond the possible system limits described by equation 3.5. It is thus necessary toform a reasonable independent velocity spline to permit the allowable error to beChapter 3 UBC Controller 30calculated. The most logical approach is to assume a velocity spline that linearlyconnects the average velocities of two adjacent master velocities and thus contains novelocity discontinuities. It takes two master sampling intervals to accelerate from zero toa constant velocity; this corresponds to about 4 times the time constant of the non-saturated position loop. The spline is illustrated in Figure 3.7:velocity—— velocity spline\\I I\\ I I- timeH AFigure 3.7. Velocity SplineThe spline itself can be described mathematically in the form:v(t) =— x_1)+ (x+2 —— x, +x1_)4] (3.11)3.4.2 State Line Triggering LogicThe intention of the approach should by now be clear; the author intends totrigger the state line when the actual error is greater than would be expected, should thevelocity be that of the velocity spline. There are however several ways in which the linemay be triggered.In the first series of experiments the state line was triggered using an absoluteerror criterion of the form:Chapter 3 UBC Controller 31IeaI (3.12)This criterion then allows an error band around the actual position. It means thatthe state line will be triggered when the actual position is leading the reference position.Such an approach forces the system to slow down more than necessary. A sign sensitiveerror criterion was therefore formulated.When the reference velocity is positive, the criterion is:v > 0: (3.13)< V) +In the case of negative reference velocity, the criterion changes to:v < 0:/ (3.14)> ‘4’$ v)—In order to combine both constraints into one single equation it is necessary tonegate the second constraint. This is equivalent to multiplying either equation by thesign of the velocity. Hence the final constraint used is given by:sign(v)ej0 < sign(v)4,3v+, (3.15)3.5 Sampling PeriodFor the velocity spline to give satisfactory results, it is necessary to choose amaster sampling time that allows the system sufficient time to slow down frommaximum speed to zero speed within one master sampling period.One way to determine the required master sampling period is to calculate thedistance required for the above mentioned deceleration, and transform it to the requirednumber of slave samples per master sample:The time required to accelerate from vo to v1 can be calculated using:Chapter 3 UBC Controller 32vi— vOt=This requires a distance of(3.16)1s=v0t+—at2 1 0 (3.17)2 2aDuring a deceleration phase, this requires that the state line starts triggering Ndslave samples before the velocity discontinuity, where:Nd= v12—v0v0AT 2vaAT (3.18)During an acceleration phase, this requires that the state line stops triggering Naslave samples after the velocity discontinuity, where:N0= = V12 —V02v1T 2vaET3.6 Filter ParametersThe position loop without the state line is a third order system:(3.19)This third order system can be reduced to a second order system by canceling thedynamics of the velocity loop with the filter. This leads to:UFigure 3.8. Block Diagram ofPosition LoopChapter 3 UBC Controller 33a= Ktsa Ka Kt (3.20)JeThe position loop block diagram reduces to the form shown in Figure 3.9:The transfer function of this system is given by:0 KKdKaKtKe (3.21)R Jes2+Jebs+KKdKaKtKeThe natural frequency of this second order system is given by:IKKdKaKtKe=(3.22)JeThe damping ratio of that system is given by:c b (3.23)2wThe final parameter of interest is the steady state phase lag to a unit ramp input:= bJe (3.24)KKdKaKtKeOne is thus left with three equations, but only two system parameters. In a normalservo system, one would pick a natural frequency and a damping ratio in order tocalculate the filter gain K and the lag time constant 1/b.In the case of the velocity spline, one actually needs to specify the phase lag andthe damping ratio of the system. This leads to a filter gain of:K= 4Je (3.25)Kd Ka Kt KeFigure 3.9. Block Diagram ofReduced Position LoopChapter 3 UBC Controller 34The resulting inverse of the filter lag time constant is given by:b=2i IKKdKaKt (3.26)Jeand the resulting natural frequency of the system is:(3.27)This parameter choice usually yields a stable and fast result. In the case of thesystem with a sampling time of 0.5 ms, a desired phase lag of 3 ms and a desireddamping ratio of one, the resulting natural frequency of the system was 666 rad/sec.3.7 Mismatched GainsIn some circumstances it is possible (but in the author’s opinion not veryprobable), that the gains of different axes will be mismatched. In such an event thefollowing error of the different axes will have different proportionality constants to thevelocity. A steady state velocity path error will then result. Simply choosing thesmallest proportionality constant as the common velocity error constant will trigger thestate line and slow down the system. Such an approach however will still result in a patherror which is proportional to the new, slower, velocity.In order to run at the maximum speed with no path error it is necessary tofeedforward the difference in errors between the fastest and the slower axes to the sloweraxes. In this manner all the axes have their own gain dependent velocity error constants,and the system can still run at the reference speed.It is relatively easy to implement this concept for linear motions within the UBCcontroller. The axis with the largest gain still operates in the usual fashion. Any sloweraxis will have a larger velocity error constant. The actual steady state error will be:(3.28)Chapter 3 UBC Controller 35The faster axis requires a steady state error of:ere..dred d?ssjasiest VThe error may be reduced by the difference of the above two equations:compensation v (ciasenax Pzsacnsai)(3.29)(3.30)velocity v ( fastest axis actuaIIn a linear move this is achieved by adding the compensation to the first mastersample and subtracting it from the last. The basic scheme is illustrated in Figure 3.10:The first and the last master sample position increments are updated in thefollowing fashion:x1’ = +(3.31)= x—ct)3.8 ConclusionsIn this chapter the UBC controller was described in detail. A new contouringalgorithm was introduced. This algorithm smoothes out velocity discontinuities byFigure 3.10. Mismatched Gains CompensationChapter 3 UBC Controller 36controlling the amount of error of each axis. The procedure ensures path accuracywithout extensive path preprocessing or fast computer hardware.Simulations and experimental results from a real system are presented in the nextchapter. They will show that the strategy utilizing a velocity spline in order to trigger thestate line provides good results which are comparable to the ones obtained with otheralgorithms described in the literature.Chapter 4Simulation and Experimental Results4.1 Introduction to Computer SimulationsThe first phase of the work concerned with the evaluation of new controlstrategies, involved the creation of a realistic simulation model of the system. Theprogram was developed using Matlab; it allows the examination of the path performanceof an arbitrary number of axes and the introduction of nonlinear influences. A typicalhigh speed servo system was also constructed and tested to validate the results from thesimulated model. Results from the experimental system are given later in this chapter.The actual MATLAB program is reproduced in its entirety in Appendix 3. Asmentioned previously, the program allows the calculation of actual path errors as well asindividual axis errors in an (N) axis system. The major non linearity in the basic systemcomprises the current limit of the amplifier, which is included in the simulation. Theordinary differential equations corresponding to the analog part of the loop were solvedusing a second order Runge-Kutta algorithm, while the time domain equations of thedigital part (including the state line), were solved directly. The values of the variousparameters in the model were chosen to correspond to the high performance experimentalservo system, which is described later.The majority of the simulations have been concerned with corner trackingcapability. In order to simulate this, the path shown in Figure 4.1 has been used. Thereader should realize that the path chosen requires one of the servos to change direction,while the second may, in essence continue at constant velocity. Most of the simulations37Chapter 4 Simulation and Experimental Results 38of this rather difficult contour have used a nominal speed of 500 RPM (one motor willthus reverse to -500 RPM during the corner.)4.1.1 Simple Servo SystemThe first set of simulations were conducted on a conventional system with nostate line in order to provide a reference from which the degree of improvement could begauged. Figure 4.1 shows the corner tracking ability of the system. It has largeovershoots that obviously cannot be tolerated in practical contouring operations. It isimportant for the comparison with attempted improved strategies, which are describedlater, (with a nominal speed of 500 RPM) to point out that the whole traverse timerequired, according to the simulation is 0.225 seconds.X—Y Positiona,I)c’1—1—100 0 100 200 300 400 500 500theta 1 [deg]Figure 4.1. Corner Tracking With a Simple Servo ControllerSolid: Master Position SplineDashed: Reference PositionDash-dot: Actual Position700Chapter 4 Simulation and Experimental Results4.1.2 State Line Strategies39There are many different approaches which may be used to trigger the state line.The most promising have been simulated.4.1.2.1 Allowable Static ErrorThis is the simplest way of using the state line to control error. The state line inthis strategy is triggered whenever the position error of a single axis exceeds a certain setconstant limit. The error criterion is given by equation 1.1.sign(v)e0, < (4.1)Figure 4.2 shows the simulation of this method. The path accuracy is very good,but the system has been slowed down to a third of the original speed; this results fromthe fact that only a very small following error is allowed. (The complete simulationlasted 0.68 seconds.)theta 1 [deg]Figure 4.2. Corner Tracking with Static Error CriterionSolid: Master Position SplineDashed: Reference PositionDash-dot: Actual PositionX—Y Po&tion100500—50 •- -L L. I.._ I..—100•—150—100 0 100 200 300 400 500 600 700Chapter 4 Simulation and Experimental Results 404.1.2.2 The Use of a Velocity SplineIt was shown in section 2.2 that the expected following error is proportional tovelocity. The fact that this kind of following error does not lead to a path error but ratherto a phase lag, was used in section 2.4.1 to derive an error criterion that uses a velocityspline. The simulated response resulting from this approach is presented in Figure 4.3.It shows that this method yields excellent path accuracy while only slowing down themean speed over the complete simulation by 20%, (the velocity spline approach took0.285 seconds compared to 0.225 seconds for the simple servo.) The actuators wereoperating at the reference speed during most of the simulation; at the corners howeverthe state line actively slowed down the system appreciably, allowing considerablyreduced errors.-150 ——100X—Y Positiona’I)150100•2000 100Figure 4.3.Solid:300 400 500 600theta 1 [deg)Corner Tracking with Velocity SplineMaster Position Spline700Dashed: Reference PositionDash-dot: Actual Position4.1.3 Velocity FeedforwardIt may be advantageous to eliminate the phase lag that is inherent in the velocityspline, (this is the normal approach taken in the literature). One way of achieving thisgoal is to feedforward the expected velocity error. A simulation of this approach isshown in Figure 4.4. The system is now able to eliminate the phase lag, but cornertracking performance has deteriorated. When the axis with no discontinuity is sloweddown at the corner, the velocity feedforward is no longer proportional to the actualvelocity (0 at the corner itself.) Thus when the state line is actuated, the velocityfeedforward for the second axis is larger than it should be, and a path error is encountered.150Chapter 4 Simulation and Experimental Results 41X—Y Position100’---50 - - - -0—50 . - --100 . - - - --150 ——100 0 100 200 00 400 500Figure 4.4. CornerSolid:Dashed:Dash-dot:500 700theta 1 [d.g3Tracking with Velocity FeedforwardMaster Position SplineRefrrence PositionActual PositionChapter 4 Simulation and Experimental Results 424.1.4 Constant Jerk Position SplineIt was shown in section 2.3 that the original position spline has velocitydiscontinuities. A new spline with constant jerk during a master sampling period wasderived and tested. Figure 4.5 shows the tracking ability of this new spline. It shows nogreat improvement over the existing constant acceleration spline.X—Y Position10050I I I I I II, I I I I II I I I I I I—50 — — — ——100I—100 0 100 200 300 400 500 500 700thetol [deg]Figure 4.5. Corner Tracking with Constant Jerk Position SplineSolid: Master Position SplineDashed: Refrrence PositionDash-dot: Actual PositionChapter 4 Simulation and Experimental Results 434.1.5 Conclusions from SimulationsFigures 4.6 and 4.7 show enlargements of the simulated system performance withvarious strategies (500 RPM and 1000 RPM). It is seen that the best approach involvesusing the velocity spline in combination with the original position spline. A static errorcriterion is not feasible, because it does not allow the system to speed up to the referencevelocity. The modified position spline does not improve the performance of the systemover the previous simpler approach. Velocity feedforward is not feasible, because itintroduces a path error when the state line is activated.Table 4.1 shows the path error encountered and the time the different strategiestake to contour the required path at 500 RPM:Table 4.1. Execution Time and Path Error ofSimulated StrategiesStrategy Time [sec] Path Error [BLU]Simple Servo 0.2250 260Static Error Criterion 0.6800 007Velocity Spline 0.2850 005Velocity Feedforward 0.2875 045Constant Jerk Position Spline 0.2795 005Figure4.6.Solid:Dashed:Dash-Dot:thetal[BLU]EnlargementsofPathsinCorner Trackingat 500RPMMaster SplineConstantAccelerationpositionSplineConstantJerkPositionSplineEnlargement ofCorner Trackingat500RPMIIIIIII100’F-J C—I/-00330032003100--3000--2900--28002700---”------++StatErrOrCflteflOfl:J’.***:VelocitySpline2600--oo;Velocity--/ocxConstantJerkPositionSpline2500I0/•1 I.SimpleS1Ørvo26002800300032003400360038004000SimpleServoVelocitySpline,Velocit’FeedfôrwardIFigure4.7.Solid:Dashed:Dash-Dot:thetal[BLUIEnlargementsofPathsinCornerTrackingat1000RPMMasterSplineConstantAccelerationpositionSplineConstantJerkPositionSplinexl Oxl 01.45IIEnlargement of CornerTrackingat1000RPM1.4——————————:‘——.——IIIIIII,_.IIIISIIIISII1.35D -J 0 40 a)-c000,10001.31.250xxxConstantJerkPositionSpline1.2—1.25I-—I$1.651.31.351.41.451.51.551.61.71.75VaChapter 4 Simulation and Experimental Results 464.2 Experimental ResultsAfter the velocity spline had been chosen as the most efficient method to triggerthe state line, actuators, amplifiers, and controllers from a retrofitted GMF s-108 robotwere used to validate the simulated results.4.2.1 Experimental ApparatusThe experimental apparatus consists of a controller, amplifiers, and actuators.The actuators are equipped with tachometers and encoders. The tachometer signals arefed back to the amplifiers, and the encoder signals are utilized by the control computer.Figure 4.8 shows a schematic diagram of the drive system.Figure 4.9 shows a schematic block diagram of a single axis of the system.1 IComputer ControllerRequiredAxisPositionDigitalFilterDIAConverter(BLUAmplifierActuatorIEncoder1IActualPositionIBLU1ActualPosition(BLUTachometerActuatcxspeed1[rad/secFigure4.9.SchematicBlockDiagramChapter 4 Simulation and Experimental Results 484.2.1.1 ActuatorsThe actuators are DC motors which are supplied by Inertial Motors. They areequipped with tachometers for measuring actuator velocity and encoders for measuringactual position. Table 4.2 lists the motor parameters:Table 4.2. Motor ParametersName Symbol Value UnitsMotor Torque Kt 0.2967 Nm/AConstantMotor Inertia Je 0.9636e-3 Nm s’2Tachometer Ktsa 0.047 V/(rad/sec)FeedbackEncoder Gain Ke 636.6198 BLU/radMaximum Current 1m 10 AMaximum Speed comax 2000 RPM4.2.1.2 AmplifiersThe amplifiers were built by Glentek specifically for the Inertial motors. Theyare PWM amplifiers with a bandwidth of more than 750 Hz. Table 4.3 shows allpertinent parameters of the amplifiers:Table 4.3. AmplflerparametersName Symbol Value UnitsAmplifier Gain Ka 13.6136 A/VMaximum Current ‘m 10 ABandwidth fcmn1jfir >750 HZChapter 4 Simulation and Experimental Results 494.2.1.3 ControllerThe control algorithm was implemented on a Ziatech STD32-bus industrialcomputer. The master is a V53 and the slaves are 80c 196 based STD32 boards fromUniversal Systems.The controller parameters are listed in Table 4.4:Table 4.4. Table ofModel ConstantsName [ Symbol Value UnitsD/A Converter Kd 0.0049GainFilter Gain Kp 34.1333Filter Lead A 0.9ParameterFilter Lag B-0.333ParameterMaster Sampling At 16 m seePeriodSlave Sampling AT 0.5 msPeriodPosition Loop C 1Damping_RatioPosition Loop 3 msSteady State RampPhase LagPosition Loop Un 666.67 rad / secNatural FrequencyPosition Loop h 67 HzBandwidthThe filter parameters were chosen to yield a damping ratio of unity and a steadystate ramp phase lag of 3 msec. Obviously the theoretical bandwidth of 67 Hz cannot beachieved, unless sinusoidal reference commands with very small amplitudes are utilized.Chapter 4 Simulation and Experimental Results 50The Bode plots of the position loop are plotted in Figure 4.10 and the stepresponse of the experimental system is shown Figure 4.11.The slave sampling period was chosen to be 0.5 ms to ensure that the slave boarditself and the communication protocol between the master and the slaves was alwaysreliable. At the same time this sampling period is small enough to ensure that thecontinuous analysis presented in this work remains valid.The master sampling period was chosen to provide a compromise between thepossible acceleration of the system and the bandwidth of the position spline. Accordingto equation 2.19, a master sampling period of 16 ms ensures that the system is able toreverse smoothly at 1000 RPM. Larger sampling periods are not advisable, because theywould decrease the reference bandwidth below an acceptable level. Increasing thecurrent limit to 20 A or averaging the velocity spline over 4 instead of 2 master sampleswould make a reversal at 2000 RPM possible.0 , , . . . . •;_:I I 111111 I I I 11111 I I 1111111 I I I I 1111—1510—1 10° 101 102Frequency (Hz)Phase response -—100—15C10—i 10° 101 102 10Frequency (Hz)Figure 4.10. Bode Plots ofExperimental System— _I_ _I_..I _I_IJ I_IL TI1hL I_I_I_I LI.Chapter 4 Simulation and Experimental Results4.2.1.4 Data Recording Device51It is necessary to have an accurate data recording scheme in order to achievereliable test results. In the experiments that follow, a forth program was run on themaster. This program continuously reads the values of position and error from the slavesand writes them to an array. The program is initiated after all the position increments foran experiment have been transferred to the slaves. At this point the slaves are slowlyemptying their buffers, and the recording program simply writes current positions anderrors to the array. As soon as the array is filled, the program stops. At a later stage thearray is transferred to a file and imported into Matlab for post processing. Thisprocedure is extremely simple, but unfortunately it cannot capture the beginning of anexperiment. It also does not provide an accurate measure of time, because data is simplyread as quickly as possible from the slaves. The data recording program and programs tocreate the input required for the experiments themselves are listed in Appendix 4.10.80.6Step Re.pon.eL.. --t -C---CI—r — - —r -r- -r0 0.005 0.01 0.015 0.02 0.025time [eec)Figure 4.11. Step Response ofExperimental System0.03Chapter 4 Simulation and Experimental Results 524.2.2 Circular InterpolationDuring machining operations, the contouring ofcircles is the second most common geometry, (aftercontouring of straight lines.) It is therefore important todetermine the limits of the system in circular interpolation.In addition, circular interpolation can give some insight into the frequency response ofthe system. In this section relationships will be derived which specifically define thelimits that the simple servo system and the modified system with a velocity spline canfollow. It will be shown that a frequency response in the usual sense is difficult to derivein those cases where the simple servo system would show a deteriorated performance dueto the imposed saturation limit. The state line system, when subjected to the sameconditions, will decrease the reference frequency to acceptable levels, hence avoidinglarge errors.In order to achieve good path accuracy, it is necessary to meet the constraint setby the position spline. A single axis is required to have a maximum path error of lessthan one BLU. (One BLU is the resolution of the encoder. One revolution of theactuator corresponds to 4000 BLUs.) According to Pottier [21] this requires:142.6667 (4.2)V rThis can be rewritten in the form:142.666721rtf,=4j (4.3)Thus relating the maximum radius of the circle to the maximum allowablefrequency.The maximum achievable velocity puts a second constraint on the system.During circular interpolation the actuator position is described by:Chapter 4 Simulation and Experimental Results 530 = rsin(2irft) (4.4)The corresponding velocity is:ci=2irfrcos(2irft) (4.5)The maximum velocity takes place when the cosine term equals one (or -1). Thisleads to the final form of the second constraint:=2fr (4.6)The third constraint is current saturation of the amplifier, which limits thepossible acceleration. The acceleration corresponding to the reference signal is:a = —(2irf)2rsin(2 t (4.7)The maximum acceleration occurs when the sine term equals one (or -1). Thisleaves a third constraint that correlates the maximum radius to the maximum frequency:a (4.8)JeThe maximum velocity constraint is only active at low frequencies and largeradii. The problem then is to determine which of the two other constraints is dominant.For the system under consideration with a slave sampling time of 0.5 ms and a mastersampling time of 16 ms, Figure 4.13 shows a plot of frequency versus circle radius.The two constraints meet at rcrit = 5900 BLU and crit = 2.9 Hz. At radii lessthan rent the position spline imposes the dominant constraint. At radii higher than thecritical radius, current saturation is dominant.If one attempts to use reference commands along the position spline constraint forradii larger than rerit, the state line should pull the actual response down to the limitingacceleration constraint line. This means that it would decrease the actual outputfrequency. The shape of the sine curve would of course change, because the state linewould only try to alter the portions that exceed the limiting acceleration. In the extremecase the final output would have a constant curvature that changes sign every half cycle.Chapter 4 Simulation and Experimental Results 5402 04 05 081i2182Rodlue [BLU] x104Figure 4.13. Maximum Radius vs Maximum FrequencyDotted Line: Limiting Velocity ConstraintDashed Line: Limiting Acceleration ConstraintSolid Line.• Position Spline ConstraintThis resulting non sinusoidal response complicates a true frequency analysis,because the error is superimposed on the non sinusoidal carrier signal. The tworesponses are difficult to separate, because one cannot easily determine the frequencycomponents of the carrier signal.4.2.2.1 Low Feedrate ExperimentIn this experiment ten circles were contoured and the third is shown in the Figuresbelow. A feedrate was chosen that would not saturate the system. The state line is notactivated and the system behaves as a normal servo system. Table 4.5 gives pertinentparameters:Table 4.5. Parameters for Low Feedrate Circular InterpolationName Value UnitsFrequency 3 HzRadius 5000 BLUFeedrate 94248 BLU/secRadius vs Fr.qu.ncyChapter 4 Simulation and Experimental Results 55Figure 4.14 shows the actual path and Figure 4.15 the path error during theexperiment.Path During Circuor Int.rpolation6000400020000—2000-4000—6000—5000 —4000 —3000 —2000 —1000 0x—axie [BLU]Figure 4.14. Path ofLow Feedrate Circular Interpolation Experiment1000 2000 3000 4000 5000I0 5 10 15 20 25 30 35 40 45Sampi..Figure 4.15. Path Error ofLow Feedrate Circular Interpolation ExperimentChapter 4 Simulation and Experimental Results 56Clearly the system is able to follow the contour well. No state line is needed,because the saturation limit is not reached. The maximum error is approximately 45BLUs.4.2.2.2 High Feedrate ExperimentIn this experiment the feedrate was chosen such that current saturation and themaximum required speed would make it impossible for the simple servo to follow thepath. Table 4.6 shows the parameters for this experiment:Table 4.6. Parameters for High Feedrate Circular InterpolationName Value UnitsFrequency 3 HzRadius 8000 BLUFeedrate 150796 BLU/secFigure 4.16 shows the actual path and Figure 4.17 the corresponding path errorduring the high feedrate experiment using a simple servo with no velocity spline:9000Path During Circular Interpolation—C—0 2000 4000 6000 8000 10000x—axis [BLIJ]Figure 4.16. Path ofSimple Servo System During High Feedrate Experiment12000Chapter 4 Sinudation and Experimental Results 57The system is not able to follow the reference path, because acceleration levelshave driven the amplifiers into saturation, and the required velocity exceeds themaximum velocity of the actuators..Figure 4.18 shows the path and Figure 4.19 the path error of a high feedrateexperiment with the state line being activated using the velocity spline error criterion.Path Error During Circular Interpolation0-f0SamplesFigure 4.17. Path Error ofSimple Servo System During High Feedrate Experiment25 30 35 40 45r —- —r—rC-r r - --I- --r—6000 —4000 —2000 0 2000 4000 6000x—oxls [BLU]Figure 4.18. Path ofServo System with Active State Line During High FeedrateExperiment8000Chapter 4 Simulation and Experimental Results 58Clearly the velocity spline has slowed down the system at the places of highacceleration to an acceptable level. The path error is approximately 78 BLUs. This isgood considering that the maximum velocity constraint and the saturation constraint fromFigure 4.13 were exceeded.4.2.3 Mismatched GainsAn experiment was arranged in which the second axis had half the gain of thefirst axis. Two straight line contouring experiments were performed with this system. Inthe first experiment no compensation was utilized, and in the second the algorithmdescribed in section 2.7 was implemented to compensate for the mismatched gains.Figure 4.20 shows the reference path, and the actual path of both experiments on thesame plot. Figure 4.21 shows the error for both experiments. In the first experimentL0 10 20 30 40 50 80 70 80 90Sample.Figure 4.19. Path Error ofServo System with Active State Line During High FeedrateExperiment100Chapter 4 Simulation and Experimental Results 59with no compensation, a steady state path error of approximately 75 BLU wasencountered. In the second experiment the compensation scheme yielded a steady stateerror of approximately 5 BLU. This shows that the scheme described in section 2.7works well for compensating mismatched gains.Path with Mimotched Axs7000 7500 8000X—dlr,ctlon [BLU]Figure 4.20. Path with Mismatched GainsSolid Line: Reference PathDashed Line: Simple Servo PathDotted Line: Compensated Pathsample.Figure 4.21. Path Error with Mismatched GainsSolid Line: Simple Servo ErrorDashed Line: Compensated ErrorI8500—.6500 8500 9000Path Error with Mlsmotoh.d Ax706050403020100—104A— —— —.1.10 5 10 15 20 25Chapter 4 Simulation and Experimental Results 604.2.4 Disturbance TorquesThe controller strategies described in the previous sections should be able tocompensate for disturbances that might result from loads on the actuator or cuttingtorques. It will simply slow down the system until the error criterion imposed on thesystem in order to achieve the necessary path accuracy is met.In order to check the performance of the system with severe disturbances, it wasdecided to abruptly stop the second axis during a linear move. This should then make thefirst axis stop at the same instant. The experiment was performed at 65 RPM for eachaxis respectively. The requested distance was 4000 BLU and the y-axis was stopped atapproximately 2200 BLU.For the system with no state line acting, the required and the actual path of thesystem in response to the described disturbance are shown in Figure 4.22. The path errorof this system is shown in Figure 4.23.Servo Path with Disturbance Torque40003500- -- - - - - - - -:—2500- - - - —2000 — — — —1500 a —1000—500- - - - - - - -—o—500 0 500 1000 1500 2000 2500 3000 3500 4000X—dlrectlDn [BLLJ]Figure 4.22: Path ofSimple Servo System with a Torque DisturbanceSolid: Reference positionDashed.• Actual PositionChapter 4 Simulation and Experimental Results 61Figures 4.22 and 4.23 show that when the second axis is stopped, the first axis (ofcourse) continues to operate normally. This introduces a large path error.Figure 4.24 shows the path in a similar experiment with the state line active. Thestate line stops the system as soon as it detects the large torque applied to the second axis.(this is done through monitoring error rather than torque itself.) Figure 4.25 shows thepath error in this experiment. It is reduced to 15 BLUs.This simple experiment would lead one to believe that the system is able tocompensate for disturbance torques, even in extreme cases.1400Path Error with Disturbance Torque0—2000 10 20 30 40 50 60 70Figure 4.23.80SampI.sPath Error ofSimple Servo System with a Torque Disturbance90 100Chapter 4 Simulation and Experimental Results 62Servo Path with Disturbance Torque2000 8.1500 - - -1000500 -—500 0 500 1000 1500X—directlon [BLU)Figure 4.24. Path of UBC Servo with Torque Disturbance2000 25000 10 20 30 40 50 60Figure 4.25. Path Error of UBC Servo with Torque Disturbance70Samples80 90 100Chapter 4 Simulation and Experimental Results 634.2.5 Corner TrackingIn terms of robotic and high speed machining operations, the most important testcriterion is corner tracking ability. This task requires the controller to compensate forlarge motor inertia torques that can lead to saturation of the amplifiers. Usually oneattempts to avoid saturation by choosing a sufficiently small speed around a corner. TheUBC controller on the other hand will recognize a large acceleration and slow down thesystem automatically, in order to allow the cornering to be achieved at maximumpossible deceleration and acceleration levels.The experiment was arranged as described in the previous section so that theactuator of the first axis would travel at constant speed while the second actuator wouldreverse at the same speed a number of times during the experiment. The data showndepict one such cycle in the middle of the experiment.4.2.5.1 Reversal at 500 RPMFigure 4.26 shows a plot of the first axis versus the second axis of a simple servosystem. Figure 4.27 shows the result of the same experiment with an active state line.Clearly there is a vast improvement in path accuracy.This experiment also allows the validation of the results obtained from thesimulations. Clearly Figures 4.1 and 4.26, and Figures 4.3 and 4.27 show similarresponses.Figure 4.29 shows a magnified view of the performance of the simple servo in thefirst corner. Figure 4.28 shows an enlargement of the first corner of the performance ofthe system with an active state line. The simple servo has a path error of approximately26° while the system with a state line shows less than 2° of path error. The “nonsmoothness” of the reference path is a result of the data acquisition program. ItContouring Path of Simp’e Seivo at 500 RPMI I I I• “ I I I I I• I I / I I:-\\I I • \\ / r/ • II I IChapter 4 Simulation and Experimental Results 64calculates the reference path by adding the actual position to the error. Data acquisitionis relatively slow and sometimes the error and the actual position may be a sample out ofphase.—100 0 100 200 300 400 500 600 700x..direction IdegiFigure 4.26. Corner Tracking ofSimple Servo at 500 RPMSolid: Reference PositionDashed: Actual PositionContouring Path of UBC Controller at 500 RPM15010050-50-1000 100 200 300 400 500 600xdiiction deg)Figure 4.27. Corner Tracking of UBC Controller at 500 RPMSolid: Reference PositionDashed: Actual Position700Chapter 4 Simulation and Experimental Results4.2.5.2 Reversal at 1000 RPM65Figure 4.30 shows a plot of the first axis versus the second axis of a simple servosystem. Figure 4.31 shows the response of this same system with an active state line.Figure 4.32 is a magnified view of the first corner of the simple servo system. Figure4.33 is an enlargement of the performance during the first corner of the system with anactive state line. The simple servo system now has a path error of approximately 900 andthe system with the active state line has a path error of approximately 7°./- /1/“1ReferenthNActual Path \\:-\\-•\\Enlargement of Contouring Pathof Simple Servo at 500 RPM150145 - - -0’4’140 - -13513060 80 100x-direction LdeglFigure 4.29. Enlargement of Corner Tracking ofSimple Servo at 500 RPMSolid. Reference Pos.Dashed: Actual Pos.\\ - /:AcPath:\\\\Reference Path-Enlargement of Contouring Path ofUBC Controller at 500 RPM150145 - - -a’140---135 -130- - -60 80 100x-direction [degiFigure 4.28. Enlargement of CornerTracking of UBC Controller at 500RPMSolid.• Reference Pos.Dashed: ActualPos.\\\\\\-Chapter 4 Simulation and Experimental Results 66Contouring Path of UBC Controller at 1000 RPM400IF-500 1000 1500 2000 2500x-direction (deg]Figure 4.30. Corner Tracking ofSimple Servo at 1000 RPMSolid.• Reference PositionDashed: Actual Position3000Contouring Path of UBC Controller at 1000 RPM600400200>-200—400—600 r0 500Figure 4.31.Solid:Dashed.1000 1500 2000 2500x-direction (deg]Corner Tracking of UBC Controller at 1000 RPMReference PositionActual Position3000400x-direction IdegiFigure 4.32. Close-up of Corner Tracking at 1000 RPMSolid: Reference PositionDashed: Actual PositionChapter 4 Simulation and Experimental Results 67Enlargement of Contouring Path of simple Servo at 1000 RPMI,1.1 .1 .2—\\2700680660640620600580560//2/RencePath‘ActualPath-4uaiPath- -\\• \\300Enlargement of Contouring Path ofUBC Controller at 1000 RPM660. 640620600580560300 400x-ciirection (deg)Figure 4.33. Close-up of CornerTracking of UBC Controller at1000 RPMSolid: Ref Pos.Dashed: Act. Pos.5004.2.5.3 Comparison with Other Proposed ControllersThe results shown in the previous section are fairly good. In order to comparethem with other algorithms described in the literature, it is necessary to select aperformance parameter (since no two researchers use the same experiment.) It wasdecided to choose path velocity over path error for this comparison:= path velocity (49)path errorThis parameter should stay almost constant for different velocities, but it mayshow poorer results for higher speeds. The arrangement should not to the advantage ofthe experiments reported here, since these experiments were carried out at very highspeeds.Chapter 4 Simulation and Experimental Results 68Koren has presented computer simulations for the ZPETC arrangement proposedby Tomizuka as well as his proposed CCC arrangement [16] (both of which have beendescribed in Chapter 3.) Motor velocity in this case was approximately 25 RPM. Itshould be noted that the data presented is derived from simulations only. Simulationswill often give better results than real experiments, because they utilize absolutelyaccurate models with matched gains, and no unexpected disturbances.Table 4.7 shows however, that the system proposed in this thesis performsfavorably when compared to the simulation results of other systems.Table 4.7. Performance in Corner TrackingControl Algorithm Single Axis Speed Performance Factor r jZero Phase Error Tracking Control 25 RPM 375Cross Coupled Control 25 RPM 750UBC Controller 500 RPM 1500UBC Controller 1000 RPM 8504.3 ConclusionsIn this chapter simulations of different controller strategies have been presented.It is concluded from these simulations that a velocity spline approach to trigger the stateline is the best compromise between computational effort and contouring performance.The experimental results are in good agreement with the simulations., validatingthe conclusions drawn from the simulations.Constraints have been derived for circular interpolation. They limit themaximum amplitude or frequency of the system according to saturation limits(acceleration), position spline accuracy, and maximum velocity. The experiments carriedChapter 4 Sinudation and Experimental Results 69out show that the velocity spline approach can in fact overcome the amplifier saturationlimit.It has also been shown that the compensation scheme for mismatched axesdescribed in section 2.7 successfully eliminates path errors in linear moves.The controller’s ability to compensate for disturbance torques has been proven byshowing that it is able to slow down the complete system almost instantaneously, whenone of the axes is stopped by an external torque. This result suggests that the controllerwill be able to compensate for disturbance torques that are not as drastic as the onechosen in this experiment.Finally the corner tracking ability of the control algorithm has been extensivelytested. The experimental results compare very well with the simulations obtained inprevious sections. The system can indeed track a sharp corner at high speeds. Theresults from the author’s experiments have been compared with simulated results, shownin the literature. The UBC controllers performed equally well or better than any othercontrol strategy found.Chapter 5Conclusions and Future DevelopmentsMany robotic and contour machining applications require a high degree ofaccuracy from the feed drives. This goal cannot be accomplished with conventional servocontrollers. A great deal of research has gone into the development of advanced servocontrollers, however most of these controllers are difficult to implement in practicalmachining or robotics environments. The latter point arises because the time constants ofthe plants are so short that digital controllers have difficulties performing the requiredarithmetic in the allowable time. Finally some of the algorithms rely on an exact model ofthe plant which may not be available in a real system.Computer simulations show that the limiting factor in many cases is not the controlalgorithm, but the physical limits of the system. When a system has to follow a path witha discontinuity or a sharp corner, it is often impossible to outperform a simple servocontroller, because the possible acceleration of the system is limited by the limiting currentof the actuator. This in turn means that the system will become nonlinear at highaccelerations. Such behavior is often neglected in algorithms and simulations of controllerperformance.In this work a new approach to contouring is proposed, based on a specialcontroller architecture developed at UBC. Most industrial machines today are servocontrolled. If one axis cannot follow its prescribed path due to a current limit or adisturbance torque, the whole system may have large errors. By using the UBC controllerarchitecture, it is possible to slow down the complete system in those cases where one axiscannot follow the prescribed path. This is achieved by using a state line on the front planebus, which connects all the slaves. When the state line is high, all slaves operate normally70Chapter 5 Conclusions and Future Developments 71as simple servos. If one of the slaves encounters a constraint, it pulls the state line low.This tells the other slaves to stop processing new position increments until the slave inquestion has resolved the problem and pulled the state line high again.The system has been simulated and the results from the simulations have been usedto choose an algorithm for the triggering of the state line, which represents a compromisebetween computational efficiency and resulting path accuracy.The system has been analyzed in Chapter 3 of this thesis. A proposal for theselection of all pertinent parameters is given. This selection process is quite simple, andusually provides reliable and stable controls.In a series of experimental tests, the simulated results have been validated. Theresults of these tests indicate that this system is able to achieve contouring performancesthat are better than other systems found in the literature to this point.A simple algorithm for compensation of mismatched gains was also developed andtested in a straight line move on the experimental system. The experiments indicate thatthe algorithm is able to compensate for mismatched gains.Since disturbance torques compromise the contouring performance of realsystems, a test was carried out to demonstrate the robustness to external disturbances ofthe controller. During a linear move, one axis was stopped abruptly. In a normalcontouring system other axes would continue to operate normally, and thus a large patherror would be introduced. The UBC controller on the other hand was able to detect theerror and stop the second axis in its path. A minimal path error was detected.In the future it would be of interest to combine the control algorithm with a pathplanning algorithm. A team under the leadership of Dr. Cherchas at UBC has beenworking on a path planning program for robotic applications, which utilizes Auto Cad forsolid modeling. It should be possible to utilize their findings in a real time as opposed topreprocessing stage. A separate master computer could be used to perform the pathChapter 5 Conclusions and Future Developments 72planning, which would then pass the required path directly over the STD-32 bus to themaster controller.This controller can be used in machining operations. These applications oftenrequire technological input to the controller. Mr. Ramin Ardekani [22], has developed acontrol algorithm for the UBC controller that is able to use the state line in order to limitthe cutting force or equivalent chip thickness on a lathe. It would be of interest to fusethis system with the new contouring control. This should be possible, because bothapplications rely on the state line to slow down the system when a constraint isencountered. This means that the system can either be slowed down by contouringconstraints or machining constraints, depending on which constraint is dominant.It should be mentioned that the control algorithm, as it stands, is designed forCartesian machines. Unfortunately there are a lot of non Cartesian machines used inindustry (especially robots). It is possible to transform the Cartesian contour errors tojoint coordinates using a Jacobean transformation. In this way the controller can bemodified for non Cartesian machines with little more computational effort.Bibliography[1] Craig, J. J., 1988, Adaptive Control ofMechanical Manipulators, Addison WesleyPublishing Company, Reading, Mass., pp. 2-4.[2] Ernst, H. A., 1962, “MH-l, A Computer-Oriented Mechanical Hand’, Proc. 1962Spring Joint Computer Conf, San Francisco, Calif., pp. 39-5 1.[3] Pieper, D. L., 1968, “The Kinematics of Manipulators under Computer Control”,Artificial Intelligence Project Memo no. 72, Computer Science Department,Stanford University, Palo Alto, Calif.[4] Kahn, M. E., Roth, B., 1971, “The Near-Minimum Time Control of Open LoopArticulated Kinematic Chains”, Trans. ASME, J. Dynamic Systems, Measurementand Control, vol. 93, March, pp. 164-172.[5] Paul, R. P., 1981, Robot Manipulator: Mathematics, Programming, and Control,MIT Press, Cambridge, Mass.[6] Denavit, J., Hartenberg, R. S., 1955, “ A Kinematic Notation for Lower-PairMechanisms Based on Matrices”, J. App. Mech. , vol 77, pp. 215-221.[7] Uicker, J. J., 1965, “On the Dynamic Analysis of Spatial Linkages using 4x4Matrices”, Ph.D. dissertation, Northwestern University, Evanston, Ill.[8] Stepanenko, Y., Vukobratovic, M., 1976, “Dynamics of Articulated Open-ChainActive Mechanisms”, Math-Biosciences, vol. 28, pp. 137-170.[9] Koren, Y., 1985, Robotics for Engineers, McGraw-Hill Book Company, NewYork, pp 5 1-59[10] Craig, J. J., 1986, Introduction to Robotics, Mechanics and Control, AddisonWesley Publishing Company, Reading, Mass., pp. 19 1-222.[11] Tomizuka, M., 1987, “Zero Phase Error Tracking Algorithm for Digital Control”,ASME Transaction, Journal ofDynamic Systems, Measurement and Control, vol109, March, pp 65-68[12] Week, M. and Ye, G., 1990, “Sharp Corner Tracking Using The IKF ControlStrategy”, Annals of CIRP, vol. 39, January, pp. 437-441.73Bibliography 74[13] Koren, Y., 1980, “Cross-Coupled Biaxial Computer Control for ManufacturingSystems”, ASME Transaction, Journal of Dynamic Systems, Measurement andControl, vol 102, no.4, December, pp 265-272[14] Merrit, R., 1982, “Industrial Robots: Getting Smarter All The Time”, Instrumentsand Control Systems, vol. 55, no. 7, pp. 32-38.[15] Fu, K. S., Gonzalez, R. C., Lee, C. S. 0., 1987, Robotics: Control, Sensing,Vision, and Intelligence, McGraw-Hill Book Company, New York., pp. 450-473.[16] Koren, Y. and Lo, C. C., 1992, “Advanced Controllers for Feed Drives”, Annalsof CIRP, vol. 41, Feb., pp. 689-698.[17] Haack, B. and Tomizuka, M., 1991, “The effect of Adding Zeroes to Feed forwardControllers”, ASME Journal ofDynamic Systems, Measurement and Control, vol.113, March, pp. 6-10.[18] Buttler, 3., Haack, B. and Tomizuka, M., 1991, “Reference Input Generation forHigh Speed Coordinated Motion of a Two Axis System”, ASME Journal ofDynamic Systems, Measurement and Control, vol. 113, March, pp. 67-74.[19] Koren, Y. and Lo, C. C., 1991, “Variable Gain Cross-Coupling Controller forContouring”, Annals of CIRP, vol. 104, Aug., pp. 37 1-374.[20] Yellowley, I. and Pottier, P., 1989, “ A Note On a Simple Method for theImprovement Of Interpolation accuracy in a General Purpose, MultiprocessorBased Motion Controller”, mt. J. Mach. Tools Manufacturing., vol. 29, no. 2, pp.287-292[21] Pottier, P., 1991, “Advanced Controller for Machine Tools”, M.A.Sc. Thesis,McMaster Univ.[22] Ardekani, R., 1992, “Integration of Manufacturing Process Control andOptimization With a General Purpose Multi-Processor Controller”, M.A.Sc.Thesis, Univ. of British Columbia.Appendix 1Derivation of Following Error in the z-domainThe derivations in Chapter 3 were based on a very simple forward differenceapproximation of the discrete filter. It was deemed desirable to prove that thisrepresentation is adequate for the design of the contouring algorithm of the UBCcontroller. Thus the following error was derived in the z-domain and compared with theresults obtained in chapter 3.The simplified continuous part of Figure 3.2 can be transformed into a third orderdiscrete system, assuming that the D/A converter behaves like a zero order hold. Theclosed loop position transfer function can be described by:O(z) — Kl(1z_1 ÷( A1)z2—A2z)Al 1R(z) -l+(B+aK+ a2a(I3where: a1 = _[i + e a2 = eK1 KpKdKssKeEquation A 1.1 can be rewritten into:O(z)= Ki b1z+b2z+b3z (Al.2)R(z) l+a1C+i.2 +a3zThe transfer function for the error is:75Appendix 1 Conclusions and Future Developments 76E(z)= Ki1 + (a1—÷ (a2—b2)z + (a3 — (A1.3)R(z) 1 +a1z +a2z +a3zApplying the final value theorem to a ramp input yields:e= Inn (i— z’) vtTz’ Ki1 + (a1—+ (a2—b2)z + (a3 —b3)z (A1.4)z—*l (1z1) 1+aC+z3Using long division this can be simplified to:e vEaKl1)(22) (A1.5)I, 1+a+23This representation yields the same result as the continuous solution, which waspresented in section 3.2. It is much longer and there is no advantage over the continuoussolution.Appendix 2Constant Jerk Position SplineThe original position spline that was proposed by Yellowley and Pottier [20] hasvelocity discontinuities. During the course of this research a new spline was proposed andtested. It has constant jerk and a linear change of acceleration over one master samplingperiod. This results in a continuous velocity, with acceleration discontinuities.The displacement over one master sampling interval for a path with constant jerk isdescribed by:1 f 1 /x=xo+vI+!aoi1 j1ui (A2.l)O& 2 k.&) 6 O&)where: 0t&The spline assumes that the starting and finishing velocities are the averages of thetwo adjacent velocities at those points. This assumption provides two boundaryconditions.The boundary conditions at t=0 is:v+v-1=2 (A2.2)1+1 1where : V =_______&The boundary condition at t=At is:v =‘‘ +(A2.3)2The instantaneous velocity is defined as:77Appendix 2 Constant Jerk Position Spline 78t 1 (t2V = V0 + a0 + (A2.4)The boundary condition at t=O is met automatically, but the boundary condition att=At requires:v1+v. 1.V1 =2 ‘= v0 + a0 + Jo (A2.5)Substituting equation A2.2 into equation A2.5 leads to:v-v i1+1 fri a0 +—J0 (A2.6)2 2It is necessary to have the correct mean velocity over a master sampling period.This requirement provides the second equation in order to solve for the initial accelerationand jerk:= v0 +!a0÷!0 (A2.7)Substituting equation A2.2 for vO leads to:v-v_ 1 (A2.8)2 2 6Solving equations A2.6 and A2.8 yields:a0 = +3i —2T’ (A2.9)= 3T’ — 61’ + 3T’ (A2.lO)This spline ensures continuous displacement and velocity proffles. It is animprovement over the old spline that assumes a constant acceleration over a mastersampling time interval. Figure A2. 1 shows an example of a velocity profile of the constantjerk position spline.Appendix 2 Constant Jerk Position Spline 79velocity/ %— master samples- - position spline--. -st-i imeFigure A2.1 Constant Jerk Position SplineThe velocity spline of this position spline is described by equation A2.4. In thosecases where the acceleration exceeds the maximum allowable limit imposed on the systemby current saturation, an new average velocity is chosen such that the acceleration stayswithin its bounds. Maximum acceleration can either occur at the beginning or at the endof the sampling period.case 1: maximum acceleration at t=O:a0 =a (A2.ll)Substituting into equation A2.9 and rearranging yields:a +J +2VV = 1+1 i-I (A2.12)This result can be used to calculate the remaining coefficients:V + T- = 2T’ + + 2a (A2.13)2 6=— 6T + 3 V11 V1 — 2a (A2.14)case 2: maximum acceleration at t=tt:a0+jcç_ (A2.15)Substituting into equations A2.9 & A2.1O leads to:=211+T’—a (A2.16)Appendix 2 Constant Jerk Position Spline 80This can be used to derive the remaining coefficients:= v + T’= 2T’+ +411a, (A2.17)2 6a0 =— J’ +3 J’— 21= — — a (A2. 18)= 3T’ —6V +3I’ = —1’ + 1’ t-2a (A2.19)Simulations showed that the added complexity of this spline does not significantlyimprove the performance of the system. It was decided to keep the original spline withconstant acceleration over one sampling period.Appendix 3Listings of Matlab Simulation Source CodeDuring the course of this thesis, a computer program was generated using Matlab,that is able to simulate an n-axis servo system with current amplifiers. The program uses aRunge-Kutta second order algorithm to solve the differential equations of the velocityloop. It should be mentioned that the program allows for current saturation and limitedbandwidth of the amplifiers.A3.1 Main ProgramFilename: main.m% *******************************% Main program to simulate servomotors*******************************clear; % Clear variables% Number of linksn=2; % 2 motors% Step response dataKss=2000*2*pi/60/lO*ones(n,1); % Steady state velocity 2000 rpm /bytau=5.e3*ones(n,l); % Time constants 5 ms% Global parametersglobal Je Kt Ka Itau Ktsa Ke VCMD Imax Vmax Kd Rt;Imax=10*ones(n,l); % 12 amp current limit duringsimulationVmax=10*ones(n,l); % 10 Volts D/A maximum outputDAres=2A 1 l*ones(n, 1); % 12-bit D/A resolution81Appendix 3 Listings ofMatlab Simuiation Source Code 82Je=(O. 135+1 .5e3)*7.O616e3*ones(n, 1); % Equivalent inertia [N*m*s ‘2/A]Kt=42*7.06 16e3*ones(n, 1); % Torque constant [N*m/AJKa=(Kssftau).*(Je./Kt); % Amplifier constant [AN]Itau=.63/750/2/pi*ones(n,1); % Amplifier time constantKtsa=Kss .‘ (-1); % Tach generator constantKe=4*1000/2/pi*ones(n,1); % Encoder gainKd=Vmax./DAres; % D/A gainC2D=Ke.’(1)*36O/2/pi; % Conversion counts to degreespsi=3e3*ones(n,1); % following error of 3 mszeta=.707*ones(n, 1); % damping ratio of one% Sampling timesTs=O.5e-3; % Slave sampling intervalTms=32*Ts; % Master sampling% Filter parameters in s-domaina=Ktsa.*Ka.*Kt./Je; % filter lead cancels plant lagK=4*Je./Kd./Ka./Kt./Ke.*(zeta./psi).A2; % filter gainb=K.*Kd.*Ka.*Kt.*Ke./Je.*psi; % filter lag% Filter parameters in z-domainKp=K; % filter gainA=ones(n, 1)Ts*a; % filter leadB=Ts*bones(n,1); % filter lag% Error limitsglobal Kve del amax;Kve=psi; % allowable velocity errordel=2*ones(n,1); % 5 BLU allowablwe static erroramax=Ivmax.*Kt.*KeJJe; % Acceleration limit% Master and slave sample generationsample % Calculate master samples% subsample using constant accelerationsubsample; % Calculate subsamplesAppendix 3 Listings ofMatlab Simulation Source Code 83% subsample using constant jerk% isubsample; % Calculate subsamples% Simulationposition; % Simulate motors% Plot resultsplo; % Plot resultsA3.2 Subprogram to Create Master SamplesFilename: sample.m% *******************% Create Master Samples% *******************% clear master samplesclear MR;% Master samples for 500 RPM reversal% MR=533*[[2:l:6;2:l:6],[7:l:12;5:_l:0],[13:l:16;l:l:4]];% Master samples for 1000 RPM reversalMR=1067*[[8:l:13;8:l:13],[14:l:26;12:_1:0J,[27:1:33;l:1:7}J;% Master samples for sin wave% MR= 1000*[sin(O:piJ8:2*pi);cos(0:piJ8:2*pi)];A3.3 Subprogram to Create Slave SamplesThere are two versions of this file. the first one creates a position spline withlinear acceleration, and the second one creates a position spline with linear jerk.Appendix 3 Listings ofMatlab Simulation Source Code 84A3.3.1 Subsample with Linear Acceleration Position SplineFilename: subsample.m% ****************************% Subsample with linear acceleration% ****************************clear R;clear Ve;[k,qJ =size(MR);m=Tms/Ts;k=2;MR=[MR( 1 :n, l)*ones( 1,k/2),MR( 1 :n,:),MR( 1 :n,q)*ones( 1,k/2+ 1)];V=(MR( 1 :n,2:q+k+ 1)-MR( 1 :n, 1 :q+k))/Tms;R(1 :n, 1)=MR( 1 :n, 1);Ve=sum(V( 1 :n, 1 :k)’)YkV1=Ve;for i=k12+ 1:q+kJ2-1,VO=Vl;V1=sum(V(1:n,i-k/2+ 1:i+k/2)’)’fk;a=(V 1-VO)/Tms;% Interpolatefor t=1:m,Ve( 1 :n,(i-k12- 1)*m+t+ 1)=VO+t/m*a*Tms;R(1 :n,(ik/21)*m+t+ 1)=MR( 1 :n,i)+(MR( 1 :n,i+ 1)-MR( 1 :n,i))*t/m+(tA2_m*t)*(MR( 1 :n,i+2)-MR( 1 :n,i+ 1)-MR( 1 :n,i)+MR( 1 :n,i- 1))/(4*m’2);end;end;MR=MR(:,k/2+ 1 :q+kJ2);Appendix 3 Listings ofMatlab Simulation Source Code 85A3.3.2 Subsample with Linear Jerk Position SplineFilename: lsubsample.m% ***********************************% subsample in with linear jerk position spline% ***********************************clear R;clear Ve;Amax=amax*TmsA2;[k,q] =size(MR);m=Tmslrs;MR=[MR( 1:n, 1),MR( 1 :n,:),MR(1 :n,q),MR( 1:n,q)];V=MR( 1 :n,2:q+3)-MR(1 :n,1 :q+2)R(1:n,1)=MR(1:n, 1);for i=2:q+ 1,vO=(V( 1:n,i)+V( 1 :n,i-1))/2;aO=-V(1:n,i+ 1)+3*V(1:n,i)_2*V(1:n,i1);jO=3*V( 1 :n,i+ 1)_6*V( 1 :n,i)+ 3*V( 1 :n,i-1);for j=1:n,if abs(aO(j,1))Amax(j,1),ve(j, 1)= 1/6*(2*V(j,i+ 1)+4*V(j,i_ 1)-Amax(j, 1)*sign(aO(j, 1)+jO(j, 1)));ae(j, 1)=V(j,i+ 1)-V(j,i- 1)-Amax(j, 1)*sign(aO(j, 1)+jO(j, 1));je(j, 1)=-V(j,i+ 1)+V(j,i1)+2*Amax(j,1)*sign(aO(j,1));end;end;for t= 1 :m,Appendix 3 Listings ofMatlab Simulation Source Code 86R(1 :n,(i2)*m+t+ 1)=MR( 1 :n,i)+vO*t/m+ 1/2*aO*(tlm)’2+ 1/6*jO* (tim)’ 3;Ve(1:n,(i2)*m+t+ 1)=(ve+ae*(tlm)+ 1/2*je*(t/m)A2)/Tms;Ae(1 :n,(i2)*m+t+ 1)=(ae+je*(tlm))/Tms;end;end;MR=MR(:,2:q+ 1);A3.3 Subprogram to Simulates the Position LoopFilename: position.m% ***********************************% Position loop subprogram called from main***********************************% Clear variablesclear theta;clear time;clear omega;clear error;clear filt;clear Vcmd;clear Rp;clear Vp;clearRi;% Order of predictorp0=1;tim=po+ 1;% Initial conditions[k,l]=size(R);time=Ts*[po:O];theta=[zeros(n,n-1),R(:, 1)] *ones(n,t);omega=zeros(n,tim);I=zeros(n,tim);error=zeros(n,tim);filt=zeros(n,tim);Vcmd=zeros(n,tim);Rp=[theta(:, 1:po—l),R,R(:,1),R(:,l),R(:,l),R(:,l),R(:,l),R(:,l),R(:,l),R(:,l),R(:,1)J;Vp=[zeros(k,po— 1),Ve,Ve(:,l),Ve(:,l),Ve(:,l),Ve(:,l),Ve(:,l),Ve(:,l),Ve(:,l),Ve(:,l),Ve(:,l)];Appendix 3 Listings ofMatlab Simulation Source Code 87[y,z] =size(Vp);Va=sign(Vp);for i=1:n,for j=2:z,if Va(ij) == 0,Va(i,j)=Va(i,j-1);end;end;end;R 1(:,1)=Rp(:,1);pf= 16;% Start simulationfor step=po+ l:l+po-l,sflag=1;while ((errflag(Rp,R1,Vp,Va,step,theta,tim,n) > 0.5) (sflag > 0.5)),sflag=0;R 1(:,tim)=Rp(:,step);if ((tim-po- 1)/16-floor((tim-po- 1)116)) < le-5,plot(time,C2D( 1)*R 1 ( 1,:),time,C2D(1)*theta(1 ,:),time,C2D(2)*R 1(2,:),time,C2D(2)*theta(2,:)),title(’Response, Motor 1 & 2’),xlabel(’time [see] ‘),ylabel(’angle [deg]’),grid;end;error(:,tim)=R 1 (:,tim)—theta(:,tim);Velocity feedforward% error=error+Kve.*Vp(:,step);filt(:,tim)=Kp.*(error(:,tim)_A.*error(:,tim_1))_B.*filt(:,tim_ 1);VCMD=filt(:,tim);for i=1:n,if abs(VCMD(i))>DAres(i),VCMD(i)=sign(VCMD(i))*DAres(i);end;end;Appendix 3 Listings ofMatlab Simulation Source Code 88VCMD=VCMD.*Kd;Vcmd(:,tim)=VCMD;% solve differential equationclear t;clear y;[t,y] =runge2((tim-po- 1)*Ts,Ts/3,3,[omega(:,tim);theta(:,tim);I(:,tim)J,n);tim=tim+ 1;[lcq] =size(t);k=k*q;time(tim)=t(k);omega(:,tim)=y(k, 1 :n)’;theta(:,tim)=y(k,n+ 1 :n*2y;I(:,tim)=y(k,2*n÷ 1:3*n)’;end;R1(:,tim)=Rp(:,step+ 1);end;% calculate currentfor i=1:n,for j=1:tim,if abs(I(ij))>linax(i),Is(ij)=sign(I(ij))*I.max(i);elseIs(ij)=I(ij);end;end;end;A3.3.1 Function to perform Runge Kutta Second Order IntegrationFilename: runge2.m% ******************************% Runge Kutta Second Order Algorithm% ******************************function [time,y 1J=runge2(tO,h,tn,yO,n)tim= 1;yl(tim,:)=yO’;Appendix 3 Listings ofMatlab Simulation Source Code 89time(tim)=tO;for t=1:tn,ki =h*rdyn(yl(tim,:),n);k2=h*rdyn((y 1 (tim, :)+kl)’,n);tim=tim+ 1;time(tim, 1)=tO+h*t;yl(tim,:)=yl(tim-1,:)+(kl+k2)/2;end;A3.3.1.1 Function Containing Differential Equations of Velocity LoopFilename: rdyn.m% ******************************% Velocity Loop Differential Equations% ******************************function yprime=rdyn(y,n)% current limit% I=y(2*n+1:3*n);I=(VCMDKtsa.*y( 1 :n)).*Ka;for i=1:n,if abs(I(i))>lmax(i),I(i)=sign(I(i))*Jnaax(i);end;end;% Disturbance torqueTd=O*ones(n, 1);% dw/dtyprime(1 :n)=(Kt.*ITd)JJe;% dthetaJdtyprime(n+ 1 :2*n)=y( 1 :n).*Ke;% Bandwidth of Amplifier% dL/dtyprime(2*n+ 1 :3*n)((VCMDKtsa.*y(1 :n)).*Ka...y(2*n+ 1:3*n))Jltau;Appendix 3 Listings ofMatlab Sinudation Source Code 90A3.3.2 Function Containing Error Checking for State LineFilename: errflag.m********************% State line Error Checking% ********************function f=errflag(Rp,R 1,Vp,Va,step,theta,tim,n);if tim> 3,error=(Rp(:,step)-theta(:,tim))+((Rp(:,step)-theta(:,tim))-(R1(:,tim-1)-theta(:,tim-else,error=(Rp(:,step)-theta(:,tim));end;% velocity feedforward% error=error+Kve.*Vp(:,step);f=0;if tim> 2,for i=l:n,if (sign(Va(i,step))*error(i))>(sign(Va(i,step))*Kve(i)*Vp(i,step)+del(i)),f= 1;end;end;endA3.4 Subprogram to Plot ResultsFilename: plot.m*********% plot results*********!rm metatmp.met[k,lJ=size(time);k=k*l;times( 1)=time( 1);Appendix 3 Listings ofMatlab Simulation Source Code 91R ls(:,1)=R1(:, 1);for i=2:k,times((i- 1)*2:(i_ 1)*2+ 1)=[time(i),time(i)];Rls(:,(i_1)*2:(i_1)*2+ 1)=[R1(:,i—1),R1(:,i)];end;% plot Theta 1 and Theta2 vs Timeplot(times,C2D(1)*R 1 s( 1,:),time,C2D( 1)*theta( 1,:),times,C2D(2)*R ls(2, :),time,C2D(2)*theta(2,:)),title(’Response, Motor 1 & 2’),xlabel(’time [sec]’),ylabel(’angle [deg]’),grid;gtext(’Ref 1 -->‘),gtext(’Ref 2 -->‘),gtext(’<-- Theta 1’),gtext(’<-- Theta 2’);meta metatmp;% plot Theta 1 vs Theta2plot(C2D( 1)*MR( 1,:),C2D(2)*MR(2, :),C2D( 1)*R 1 ( 1,:),C2D(2)*R 1 (2,:),C2D( 1)*theta(1:),C2D(2)*theta(2,:)),title(’X-Y Position’),xlabel(’thetal [deg]’),ylabel(’theta2 [deg]’),grid;gtext(’Master Ref ->‘),gtext(’Slave Ref ->‘),gtext(’<- Position’);meta metatmp;% plot closeup of theta 1 vs theta 2% 500RPM% axis([260,340,265,300]);axis([260,3 15,265,300]);% 1000RPM% axis([1200,1500,1200,1300]);% axis([1200,1300,1200,1300]);plot(C2D( 1)*MR( 1 ,:),C2D(2)*MR(2,:),C2D(1)*R1( 1,:),C2D(2)*R 1(2,:),C2D( 1)*theta( 1:),C2D(2)*theta(2,:)),title(’X-Y Position’),xlabel(’theta1 [deg] ‘),ylabel(’theta2 [deg]’),grid;meta metatmp;axis;% plot filter output vs timeplot(time( 1 :k- 1),filt( 1,:),time( 1 :k- 1),filt(2,:)),title(’Filter output’),xlabel(’time’),ylabel(’Magnitude’);pause;Appendix 3 Listings ofMailab Simulation Source Code 92% Plot command voltage vs timeplot(time( 1 :k- 1),Vcmd(1 ,:),time( 1:k- 1),Vcmd(2,:)),title(’Command Voltage’);xlabel(’time’),ylabel(’Volts’);pause;% plot current vs timeplot(time( 1 :k),I( 1 ,:),time( 1 :k),I(2, :)),title(’Current’);xlabel(’time’),ylabel(’Amperes’);pause;A3.5 Program to Calculate Filter ParametersFilename: param.m**********************% Determine filter parametersclear;% number of linksn=2; % 2 motors% Step response dataKss=2000*2*piJ6O/10*ones(n,1); % Steady state velocity 2000 rpm / 10 Vtau=5e3*ones(n,1); % Time constants 5 ms% Position loop parametersJe=(0. 135+1 .5e3)*7.06 16e3*ones(n, 1); % Equivalent inertia [N*m*s ‘2/A]Kt=42*7.0616e3*ones(n,1); % Torque constant [N*m/A]Ka=(Kss./tau).*(Je./Kt); % Amplifier constant [AN] (from stepresponse)Ktsa=Kss.” (-1); % Tach generator constantKe=4* 1000/2/pi*ones(n, 1); % Encoder gainKd=VmaxJDAres; % D/A gainC2D=Ke.’(1)*360/2/pi; % conversion counts to degrees% Sampling timesAppendix 3 Listings ofMatlab Simulation Source Code 93Ts=.5e-3; % Slave sampling intervalN=5;Tms=2’N*Ts; % Master sampling%Filter design parameterszeta=.707*ones(n,1); % damping ratio of position loopfw=6; % 6Ts (3 ms) following error% Filter parametersa=Ka.*Kt.*Ktsa./Je; % filter cancels dynamics% b for specified following error and zetaK= (2*zeta./a.IKss/(Ts*fw)). ‘(2) .*Ka.*Kt.JJeJKd./Ke;b=2*zeta.*(K.*Ke.*Kd.*Ka.*KtJJe).A (0.5);% forward differencesA=ones(n, 1)_a*Ts;B=b*Tsones(n, 1);Kp=K;% Tustins approximation% A=(2*ones(n, 1)a*Ts)./(2*ones(n,1)+a*Ts);% B=(b*Ts2*ones(n, 1)).J(b*Ts+2*ones(n, 1));% KpK.*(2*ones(n, 1)+a*Ts)./(2*ones(n, 1)+b*Ts);num=Kp(1)*Kd(1)*Kss(1)*Ke(1)*[O,0, 1,a(1)];den=num+[tau(1),b( l)*tau( 1)+ 1,b( 1),Oj;% bode plot of the position loopw = logspace(0,3);[mag,phase] = bode(num,den,w);dcsubplot(2 11)semilogx(w/2/pi,20*log(mag)flog( 10)), grid,title(’Magnitude response’),..xlabel(’Frequency (Hz)’), ylabel(’Gain dB’), subplot(2 12),..semilogx(w/2/pi,phase), grid,title(’Phase response’), xlabel(’Frequency (Hz)’),..ylabel(’Phase deg’),pause;!del bode.met;meta bode;subplot(1 11);Appendix 3 Listings ofMatlab Simulation Source Code 94dc;% Plot step responset=[O:.OO1 :.03];y=step(num,den,t);plot(t,y),title(Step Response’),xlabel(’time [sec]’),ylabel(’Magnitude),grid;!del step.metmeta step;pause;% print filter parameters for forth fileG4=ceil(log(Kp)/log(2));G1=Kp./2.O.G4G2=G1.*AG3=BG4% print velocity error constantKve=fw*TsjTmsKve_recommendet= 1 .25*Kve% print maximum accelerationamax=Imax.*Kt.*Ke./Je% print second order system characteristicsomega=(Kp.*Kd.*Ka.*Kt.*Ke.IJe).’(O.5)zeta=O.5*b./omegafe=b./a./Kd.fKss./Ke./KpAppendix 4Listings of Forth Code for Controller ExperimentsThe forth code listed in this appendix was used to calibrate the velocity loop andperform contouring experiments.A4.1 Program to Calibrate Amplifiers\\ AMPLIFIER CALIBRATIONHEX\\ PUT VOLTAGE ONTO DAC (FFF=1OV, 800=OV, 0=-by)DACX DUP OFF AND SWAP -8 SHIFT 1FD1 BWX IFDO BWX;DACY DUP OFF AND SWAP -8 SHIFT 1FD1 BWY 1FDO BWY;\\ STEP INPUT 1.875V=375RPM 980 680PXY 888 DACX 880 DACY;ZXY800DACX800DACY;NXY 780 DACX 780 DACY;DECIMAL\\ READ AND PRINT SLAVE VARIABLES\\ X-DIRECTIONPEX 102 WRX.; \\ POSITION ERRORPAX 144 LRX D.; \\ ACTUAL POSITIONRLX 156 WRX.; \\ VELOCITY SPLINE LIMIT\\ Y-DIORECTIONPEY 102 WRY.; \\ POSITION ERRORPAY 144 LRY D.; \\ ACTUAL POSITIONRLY 156 WRY.; \\ VELOCITY SPLINE LIMIT95Appendix 4 Listings ofForth Codefor Controller Experiments 96A4.2 Data Array Routines\\ STORE CURRENT POSITION INTO AN ARRAYVARIABLE POSSIZE \\ SIZE OF ARRAY12000 POSSIZE!CREATE POS POSSIZE @ ALLOT \\ ALLOCATE ARRAY:STO144 LRX 2 PICK 2! 4+ \\ STORE X & Y POSITION144LRY2PICK2!4+102 WRX 1 PICK ! 2 + \\ STORE POSITION ERROR1O2WRY1PICK !2+\\ 156 WRX 1 PICK ! 2+ \\ STORE R_LIMIT\\ 156 WRY 1PICK !2+;-->\\ FILL UP THE DATA ARRAYFILLPOS \\ FILL THE ARRAYPosPOSSIZE @ 0 DOSTO12+LOOP;-->\\ WRITE POSITION ARRAY TO THE SCREENPPPOS POSSIZE @ 0 DODUP 2@ D. 4+ \\ X-POSITIONDUP 2@ D. 4+ \\ Y-POSITIONDUP @ .2+ \\X-ERRORDUP @ .2+ \\Y-ERROR\\ DUP @ . 2 + \\ X-R_LIMIT\\ DUP @ . 2 + \\ Y-R_L1MITCR12+LOOP;-->Appendix 4 Listings qf Forth Codefor Controller Experiments 97\\ WRITE POSITION ARRAY TO A FILE:WPSHELL” ATTRIB -R TPOS” \\ DELETE OLD FILESHELL” DEL TPOS”>FILE TPOS \\ OPEN FILE TPOSPP \\ TRANSFER DATACONSOLE; \\ CLOSE FILE\\ COPY POSITION FILE TO A:SERVO.MATP2SwPSHELL” COPY TPOS A:SERVO.MAT”;\\ COPY POSITION FILE TO A:VELO.MATP2VwPSHELL” COPY TPOS A:VELO.MAT”;A4.3 Corner Tracking Using XYL\\ TESTRUN AXIS REVERSAL USING XYLDECIMALRUPERT 6075. X 6075. Y XYL;WORRY 6075. X -6075. Y XYL;DONE 1061. VEL100 DO RUPERT WORRY LOOPFILLPOS;Appendix 4 Listings ofForth Codefor Controller Experiments 98A4.4 Corner Tracking Using Lower Level Commands\\ TESTRUN REVERSAL USING STUFFX/STUUFYVARIABLE RVELVARIABLE MSRM1 DECIMAL5N!CALC_T2AN500.E04.0E3 F* 6.OE1 Fl FDUPT2AN F@ F* F>S RVEL!2.OE6F/T2ANF@ Fl6.OEO F* F>S DUP MS!S>F20.OEOF*K2F!-->\\ 2A3 SLAVE SAMPLES\\ CALCULATE MASTER SAMPLING TIME\\ SET A VELOCiTY OF 500 RPM\\ CORESPONDING BLU/SEC\\ VEL=BLU/SEC*T2’N=BLUfMAST.SAMP.\\ MS=MASTER SAMPLES IN 1 DIREC.\\ K2=# OF MASTER SAMPLESRMOVERM1 CALC_CVS STUFF_INrrX STUFF_INITY SEND_HEADER100DOMS @ ODORVEL @ STUFFX RVEL @ STUFFYLOOPMS @ ODORVEL @ STUFFX RVEL @ NEGATE STUFFYLOOPLOOP0 STUFFX 0 STUFFYDUMP_SMALLX DUMP_SMALLYFILLPOS;-->Appendix 4 Listings ofForth Codefor Controller Experiments 99A4.5 Frequency Response Experiment\\ FREQUNCY REPOMSE\\ SET UP FREQUENCY RESPOSE EXPEERIMENTFVARIABLE MAO \\ DESIRED MAGNITUDEFVARIABLE FREQ \\ DESIRED FREQUENCYVARIABLE TJME \\ LENGTH OF EXPERIMENT IN MASTER SAMPLES\\ WRITE MASTER SAMPLES TO TABLE:FRI5 N! CALC_T2’N \\ DEFINE MASTER SAMPL4.OEO FREQ F! 3.0E3 MAG F! \\ DEFINE MAO & FRE6.283185E0 FREQ F@ F* T2AN F@ F* \\ OMEGA * DELTA TIMEOEO \\ OMEGAO5E0 T2N F@ F! F>S TTIME! \\ EXP. LASTS 5 SEC.POSTr1ME@4*+POSDOFDUP FSIN MAG F@ F* F>S I! \\ MAG*SIN(OMEGAO*TIME)FDUP FCOS MAG F@ F* F>S 12+!I FPICK F+ \\ OMEGAO+OMEGA*DEL(TIM)4 +LOOP FDROP FDROP;\\ CONVERT SAMPLES TO INCREMENTSVARIABLE OXVARIABLE OYFR2POS@ OX!POS2+ @OY!POSTnME@4*+POSDOI@DUPOX@-I!OX!12+ @ DUPOY @ -12+! OY!4 +LOOP;-->Appendix 4 Listings ofForth Codefor Controller Experiments 100\\ SHUFFLE TABLE TO CONTROLLER BOARDSFR3TTIME S>F K2 F! \\# OF MASTER SAMPLESCALC_CVS \\ PREPARE TO SENDSTUFF_INITX STUFF_INITY \\ COMMANDS TO BOARDSSEND_HEADERPOS \\BEGINNING OF TABLETIME@ 4*OD()DUP @ STUFFX 2 + \\ STUFF POSITIONDUP@STUFFY2+4 +LOOI) DROP0 STUFFX 0 STUFFY \\ FLUSH PIPEDUMPSMALLX DUMP_SMALLY;\\ FILL UP TABLE WITH RESULTSFR4POS \\ BEGINNING OF TABLEPOSSIZE @ 0 DO144 LRX 2 PICK 2! 4 + \\ STORE POSITION144 LRY2PICK 2! 4+102 WRX1PICK !2+ \\STOREERROR102 WRY 1PICK !2+56WRX1PICK !2+ \\STORESTEP14 +LOOP DROP;\\ TRANSFER RESULTS FROM TABLES TO SCREENPRESPOS \\BEGINNING OF TABLEPOSSIZE @ 0 DODUP 2@ D. 4+ DUP 2@ D. 4+ \\TRANSFER POSiTIONDUP @ . 2 + DUP @ . 2 + \\ TRANSFER ERRORDUP @ . 2 + \\ TRANSFER STEPCR14+LOOP;-->Appendix 4 Listings ofForth Codefor Controller Experiments 101\\ TRANSFER RESULTS FROM TABLES TO FILEFR FRi FR2 FR3 FR4SHELL” DEL FREQRESP”>FILE FREQRESP \\ OPEN NEW FILEPRES \\ WRiTE DATACONSOLE \\ CLOSE FILESHELL” COPY FREQRESP B:FREQRESP.MAT”;"@en ; edm:hasType "Thesis/Dissertation"@en ; vivo:dateIssued "1994-05"@en ; edm:isShownAt "10.14288/1.0080894"@en ; dcterms:language "eng"@en ; ns0:degreeDiscipline "Mechanical Engineering"@en ; edm:provider "Vancouver : University of British Columbia Library"@en ; dcterms:publisher "University of British Columbia"@en ; dcterms: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."@en ; ns0:scholarLevel "Graduate"@en ; dcterms:title "A new contouring algorithm for the U.B.C. controller"@en ; dcterms:type "Text"@en ; ns0:identifierURI "http://hdl.handle.net/2429/4871"@en .