UBC Theses and Dissertations

UBC Theses Logo

UBC Theses and Dissertations

A new contouring algorithm for the U.B.C. controller Seethaler, Rudolf 1993-02-20

You don't seem to have a PDF reader installed, try download the pdf

Item Metadata

Download

Media
831-ubc_1994-0029.pdf [ 2.49MB ]
Metadata
JSON: 831-1.0080894.json
JSON-LD: 831-1.0080894-ld.json
RDF/XML (Pretty): 831-1.0080894-rdf.xml
RDF/JSON: 831-1.0080894-rdf.json
Turtle: 831-1.0080894-turtle.txt
N-Triples: 831-1.0080894-rdf-ntriples.txt
Original Record: 831-1.0080894-source.json
Full Text
831-1.0080894-fulltext.txt
Citation
831-1.0080894.ris

Full Text

A NEW CONTOURING ALGORITHMFOR THEUBC CONTROLLERbyRudolf SeethalerB .A .Sc., University of Toronto,1991A THESIS SUBMITTED IN PARTIALFULFILLMENT OFTHE REQUIREMENTS FORTHE DEGREE OFMASTER OF APPLIED SCIENCEinTHE FACULTY OF GRADUATESTUDIESMECHAMCAL ENGINEERINGWe accept this thesis as conformingto the required standardTHE UNIVERSITY OF BRITISHCOLUMBIAApril 1993© Rudolf Seethaler P1qIn presenting this thesis in partialfulfilment of the requirements for an advanceddegree at the University of British Columbia, Iagree that the Library shall make itfreely available for reference and study. I furtheragree that permission for extensivecopying of this thesis for scholarly purposesmay be granted by the head of mydepartment or by his or her representatives.It is understood that copying orpublication of this thesis for financialgain 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 thehigh degree of accuracythat is required in many robotic and contour machiningapplications. New controlalgorithms are usually difficult to implement in practicalmachining or roboticsenvironments, since they require extremely fast computerhardware, or a very exact modelof the plant. Such algorithms also usually assume thatthe drive system may be modeled asa linear system; unfortunately this assumptionis often violated during contouringoperations. The objective of the work reportedin this thesis is to utilize the specialarchitecture of the UBC controller to allow thedevelopment of control algorithms thatallow high speed contouring operations to be undertakenwith automatic, real time, errorcontrol. The system has been simulated and tested insituations which result in significantnon linearity, (sharp corner tracking being a particularlyimportant example.) Theresultsof these tests indicate that the system is able toachieve contouring performance thatisbetter than other systems described in the literatureto date.IiTable of ContentsAbstractTable of Contents .ListofTablesListof FiguresNomenclatureAcknowledgments• . II• . Hi• . . vu.. yin• . XI• . . xiv1123568• • . 10• . 112 Previous Approaches to the Developmentof Fast ContouringControe . • . ..132.1 Introduction . .132.2 Comparison of Existing Approaches142.3 Sources of Error •152.4 Feedback Controllers162.4.1 P-Controller16An 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 TrackingController2.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 Lineto 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-domain75Appendix 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 andPath Error of SimulatedStrategies434.2 Motor Parameters484.3 Amplifier parameters484.4 Table of Model Constants494.5 Parameters for LowFeedrate Circular Interpolation544.6 Parameters for HighFeedrate Circular Interpolation564.7 Performance in Corner Tracking68vi’List of Figures1.1. Cartesian Robot .41.2 Cylindrical Robot41.3 Spherical Robot .41.4 Revolute Robot . .42.1 Typical Position Control Loop142.2 P-Controller and PD-Controller Structure172.3 Block Diagram of Zero Phase Error TrackingController 182.4 Block Diagram of Inverse CompensationFilter Controller . . 192.5 Block Diagram of Two Axis Cross CoupledController 213.1 UBC Controller Architecture233.2 Schematic Block Diagram of a Two Axis Control System243.3 Block Diagram of Position Loop253.4 Block Diagram of Continuous Position Loop263.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 Gains354.1 Corner Tracking with a Simple Servo controller384.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 Trackingat 500 RPM .Enlargements of Paths in Corner Tracking at 1000RPMSchematic 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 InterpolationExperimentPath of Simple Servo System During High FeedrateExperimentPath Error of Simple Servo System During HighFeedrate ExperimentPath of Servo System with ActiveState Line During High FeedrateExperiment4.19 Path Error of Servo System withActive 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 aTorque DisturbancePath of UBC Servo System with aTorque DisturbancePath Error of UBC Servo System witha Torque DisturbanceCorner Tracking of Simple Servo at500 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 inz-domainA(z): Denominator (poles) of positionloop transfer functionB: Filter lag constant in z-domainB(z): Numerator (zeroes) ofposition loop transfer functionB+(z):Cancelable zeroes of position loop transferfunctionB-(z): Non cancelable zeroesof position loop transfer functionD(z): Drive transferfunctionE: Error signalEx \ Ey: Position errorsF(s): Filter transfer function in s-domainF(z): Filter transfer function in z-domainG(s): Position open loop transferfunctionG(z): Inverse of position looptransfer functionGf(z): ZPETC transfer functionH(z): Controller transfer functionI: Current‘max:Current saturation limitIKF(z): Inverse compensationfilter 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 lowpass 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 interpolatedpositionsT: Available torqueTd: Disturbance torqueU: Control signalVx \ Vy: Velocity spline signalsV1: Master Controller velocitiesW(z): Cross coupling controllerXi:Master positionsa: Filter lead constantin s-domainao:initial acceleration in constant jerk positionsplineal,a2,a3:Denominator coefficients of position looptransfer function in thez-domainb: Filter lag constant in s-domainb1,b23: Numerator coefficientsof position loop transfer functionin the zdomainess:Ramp following erroreallowahie:Allowable errorCircular interpolation frequencyfh:Position loop bandwidthmax:Maximum circular interpolationfrequencyio:Jerk in constant jerk position spliner: Circular interpolation radiusrmax:Maximum circular interpolationradiuss: Distance traveledt: Timev: Velocity spline velocityvo:Initial velocityxl’vf: Final velocityx: Interpolated positionxo: Initial positiona:Angular accelerationcxl, a:Parameters for transferring thevelocity loop to the z-domainamax: Maximum angular acceleration:Parameters for transferringthe velocity loop to the z-domainöstatic: Allowable static errorSlave sampling timeAt:Master sampling timeDamping ratio of position loopPerformance factor in cornercontouring0:Angular actual positionAmplifier time constantti: Velocity loop time constantSteady state ramp phaselagAngle between mastersamples at circular interpolationAngular velocityCutoff frequency of linear phaselow pass filterCI)max: Maximum angular velocityNatural frequency of positionloopXII’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 methe 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 ResearchCouncil 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 productivityhaslead progressive companies to examine computer-based automation. Atthe present time,many tasks are carried out by special purpose machinesthat are able to perform specifictasks at high production rates. Such systems are generally referredto, in a generic sense,as hard automation systems. It is evident however that in thefuture, it may well be betterto have general purpose machines, that are able to overcome the inflexibilityand the highcost of product changes. This usually implies the application of roboticdevices.Early applications of robots usually involved only simplepick and placeoperations. The robots were required to accuratelyposition objects at certain positions inspace, but they did not have to follow a specified pathbetween the final positions with ahigh degree of accuracy. During the lastdecade, robots have been put into use incontinuous operations such as welding, abrasive finishing,and adhesive applications.These tasks require the actuator to follow the prescribedpaths very accurately. At thesame time, one seeks to run at the highest speed possible,(consistent with path andprocess constraints.) Conventional servo controllers are not usuallyable to achieve thenecessary path accuracy at high speeds without extensive path preprocessing.The objective of the research described in this thesis is todevelop a contouringcontrol algorithm that is able to address the requirements of highspeed 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 Research2with high performance actuators andPWM amplifiers. The overall aim of the controllerdesign was to perform all path control activitiesin real time, and to reduce thecumbersome preprocessing activity used in otherapproaches. The controller used is anexisting UBC designed, multi-processorbased system. The architecture allows not onlymaster-slave communication, but also highspeed interaction between slave processors.The latter feature has provento be critical in this application.1.2 Historical Development of RobotsThe overview in this section is basedon Craig [1]. References in this section aredirectly quoted fromCraig.The first work on industrial robots started in the late1940’s. The Oak Ridge andArgonne National Laboratoriesbegan work on a remotely controlled manipulatortohandle radioactive materials. These systemswere designed to reproduce the hand andarm motions of the operator, and they werelater equipped with force feedback, allowingthe operator to “feel” the forcesacting between the slave manipulator and itsenvironment.In the 1950’s the first systems capable of autonomous,repetitive operations weredeveloped. The first industrial robot was introducedby Unimation INC. in 1959. It useda computer in conjunction with a manipulator that couldbe taught to carry out a varietyof tasks.It quickly became evident that sensory feedback would improvethe usefulness ofrobots. Ernst[2] introduced a mechanical hand withtactile feedback that could feelblocks and use that information to stack them withoutoperator assistance. In 1968 amanipulator equipped with a TV camera and a microphone was builtat Stanford. It wasable to recognize spoken messages, see blocks scatteredon a table, and manipulate theblocks (in a variety of ways.)Chapter 1 An Introduction to Robots, Applications, and Previous Research3In the late 1960’s Pieper [3] studied the general kinematicsproblem ofmanipulators, while Kahn and Roth [4] analyzed robot dynamics andproposed the firstnear minimum time control strategy.During the 1970’s, most work was focused on the use of sensorsto facilitatemanipulative or assembly tasks.Today robotics research is a very broad field thatincludes kinematics, dynamics,planning systems, control, sensors, programminglanguages, and machine intelligence.13 Kinematics and DynamicsKinematic analysis is used to studythe geometry of motion of a robot withrespect to a base coordinate system.It describes the displacement of the manipulatorwithout regard for the forces and momentsthat cause the motions. The analysisexamines the relationship between the joint-variablespace and the position andorientation of the end-effector in Cartesianspace. Figures 1.1 to 1.4 show a number oftypical manupulator types. Paul [5] presentsa comprehensive introduction to Kinematicsand Dynamics.There are two basic problems thatare of interest. The so-called forwardkinematic solution describes the position of theend-effector in terms of thejointvariables, while the backward kinematic solutiondetermines the joint-angles in terms ofthe position and orientation in Cartesian space.Usually the task is stated in termsof thereference coordinate frame. This requires that backwardkinematic solutions mustbeavailable in order to determine the joint-angles.Chapter 1 AnIntroduction to Robots,Applications, and PreviousResearch4Figure 1.1. CartesianRobotFigure 1.3. SphericalRobot Figure 1.4.Revolide RobotChapter 1 An Introduction to Robots, Applications,and Previous Research5A matrix representation of link geometries wasfirst presented by DenavitandHartenberg [6]. This approach allows oneto represent the spatial geometry ofthe linkswith respect to a base referenceframe. It utilizes 4x4 homogeneoustransformationmatrices that relate adjacent links, andreduces the problem to finding anequivalent 4x4matrix relating the hand coordinateframe to the reference coordinateframe.Robot dynamics deals with the mathematicalformulation of the equations ofrobot arm motion. They are useful incomputer simulations and the designof robotmotion controllers. The dynamicequations are derived from the laws ofNewtonian andLagrangian Mechanics. Using the Denavit-Hartenbergconvention it is possible todevelop standard procedures to derivethe equations of motion in termsof specifiedgeometric and inertial parameters.Two standard procedures for thistask are theLagrange-Euler formulation, firstdeveloped by Uicker [7], and theNewton-Eulerprocedure, introduced by Stepanenko andVukobratovic [8].1.4 Drive SystemsThere are a number of differentdrive systems that can beused to power a robot.The type of drive is usually determinedby the application (task) for whichthe robot isdesigned. Koren [9] describes someof the basic drive systems.Electrical drive systems are usuallyused for small to medium size manipulators.Direct current actuators provide good speedregulation, high torquesand efficiencies, andhave therefore traditionally beenused for high precision operations.However the lowpower to size ratio usuallyrequires gearing systems. Beingangular actuators, theyrequire ball screws or similardevices to provide linear motion.With the development of newhigh power electronics,alternating currentactuators have become more available.They are beginning to replaceDC actuators,because they have slightly bettercharacteristics.Chapter 1 An Introduction to Robots, Applications, and Previous Research6Stepping motors are incremental digital drive systems that rotate oneangularincrement per command pulse. They usually do not haveposition feedback. This meansthat they operate in an open loop control systemthat does not let the controller knowwhether or not the actuator has actually performedthe prescribed motion. Large positionand velocity errors cannot be detected. Usually suchactuators have a very limited powerand torque capacity.Hydraulic actuators are used for large robots. They provide high powerwithsmall units, and the response characteristics are verygood. Linear and rotary actuatorsare readily available and no gearing is required. Hydraulicactuators however haveseveral disadvantages. They require highly pressurizedliquid as the operating fluid,which often causes maintenance problems.The response characteristics change with thetemperature of the working fluid and they are also stronglyinfluenced by entrainedmaterials. The relative cost of a hydraulic system does not decrease with size andhencethey are too expensive for small systems. Finally the various devices used in hydrauliccontrol elements are often distinctly nonlinear, leadingto 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 requirerelatively 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 Research7constructing such a path. Craig[101discusses trajectory planning in a broader scope thanthis introduction allows.There are three constraints that enter trajectory planning. The pathconstraintspecifies what path the manipulator has to follow. The obstacle constraint restrictsthepath to manipulator motions that will not interfere with other objects (obstacles)alongthe trajectory of the manipulator. Finally it is of course impossibleto put infinite powerinto an actuator, and hence discontinuities in the path and itsfirst second and thirdderivative should be avoided. This last constraint is referredto as the manipulator’sdynamic constraint.There are basically two approaches to trajectory planning. In the firstapproachknot points are selected along the path in the manipulator’s generalizedcoordinate frame(joint coordinates). Using constraints such as continuity and smoothness ofposition,velocity, and acceleration, polynomials are fitted betweenthe knot points. Theyguarantee that the constraints are met at the knot points. Withthis 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 Cartesianspace.The second approach specifies the path in an analytical function suchas a straightline or a circle in Cartesian space. Thus, obstacleavoidance can be checked for in theinitial stage of calculation. The Cartesian path constraintmust then be transferred tojoint path constraints. Again a splining operation isconducted during the fit between thejoint knot points, which will ensure continuity and smoothnessof position, velocity, andacceleration.Usually the actuator dynamics are not considered in the trajectoryplanning phase,because of their computational complexity. Thus, large tracking errors willresult withinthe servo control of the manipulator, if the actuators cannot supply the torquerequiredfor the desired path.Chapter 1 An Introduction toRobots, Applications, and Previous Research816 Motion ControlMotion control is the process of controllingthe actual actuators of a manipulatorin order to achievea desired trajectory. Motion control is usually dividedinto two parts.First the dynamic model of themanipulator is obtained. This modelis then used todetermine a suitable control law. Thereare usually two distinct phases in a motionsegment. First the manipulatoris moved from its initial position to the vicinityof thefinal location alonga planned trajectory. In the second segment fine-motioncontroloften in combination with sensoryfeedback is employed to makethe end-effectordynamically interact with theobject.Currently most industrial robot controllersdo not utilize information from thedynamic equations of motion,but rather use simple servo controllers. This leadsto therequirement for slow speeds,(often accompanied by unnecessary vibrations). In ordertoimprove the performance, more sophisticatedcontrol approaches and dedicated computerarchitectures are often utilized. There are twobasic philosophies in modern contouringcontrol. The first attempts to minimizepath error by minimizing the position error ofeach axis. The usual way that thisis achieved, is to insert a filter before the controlloopthat is the exact inverse of the actual controlioop. 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 ofthe system. Still this approachhad two flaws. First it requires a very accuratemodel of the system, since otherwise theposition loop cannot be canceled exactly. Onewould therefore have to use an exactdynamic model of the actuators, andthe equations of motions of the robot arm, includingloads and disturbances. This is extremelycomputationally complex, and it is oftenimpossible to extract exact information onthe load and disturbances. The secondproblem arises from the saturation limitsof the amplifiers and motors. In order to cancelthe position loop dynamics, large controlsignals are required, which can saturate theChapter 1 An Introduction to Robots, Applications, and Previous Research9amplifiers. This problem becomes more pronounced whenthe frequency components ofthe reference path increase. (Thus, the system performancedeteriorates around corners.)Both problems have been addressed by Weck[121.He proposed the use of a regressivesystem characterization to determinethe position loop model, and used this information,instead of an analytical model, to designthe inverse filter. In his proposalthe systemcharacterization is carried out in the startup phase. Therefore the system model doesnotcontain loads and position dependentarm dynamics. In order to get a truly accuratemodel it would be necessary to performsystem characterizations throughoutthecomplete operating cycle. This in turnposes problems with large computationalefforts.Weck also proposed the use of a linear phase lowpass filter in order to reduce the highfrequency components in the reference signal. This ideaseems to work very well forimproving corner tracking.The second approach was first proposedby Koren [13]. This approach attemptsto minimize the path error directly instead of thesingle axis error. The approach requiresthe controller to calculate the path error fromthe single axes errors and thenuse this patherror to compensate the different axes,a strategy which clearly requires cross couplingoferrors. The controller is complicatedand the computational effort rises with thesquareof the number of axes. Tothis date the author has not been ableto find a paper thatdiscusses the implementation of this systemon high performance servos, which requirefast sampling times in order to fully utilizetheir capabilities. It seemsthat thecomputational effort is too large toactually implement this system withtoday’scomputing capacity.The algorithm that is proposed in this thesisdoes not rely on an exact dynamicmodel. It therefore does not require regressivesystem characterizations. Thealgorithmrelies on the principle that an error proportionalto the reference velocity ineach axisleads to a phase lag between the actual andthe reference signal, but the path accuracyitself is not compromisedby such an error. The algorithm then predicts anallowableChapter 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 canbe 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 requirethe robot toperform preprogrammed repetitive tasks,a great deal of effort has gone into thedevelopment of new sensory techniques. Merrit [14] has presenteda 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, andPrevious Research111.8ProgrammingLanguagesCommunication between theoperator and the manipulator is the majorobstacle inutilizing a general purpose assembly machine.Fu et al. [15] have presentedanintroduction to this topic.There are three major approachesto this problem. Discrete word recognitionisvery limited, because state ofthe art speech recognition can only recognizediscretewords from a limited vocabulary.It is also speaker dependent, it needs verypowerfulcomputers for efficient real time algorithms,and it requires a large memoryspace tostore the speech data. As a result discreteword recognition has notyet found its way intoreal industrial applications.When using teach and playback,an operator slowly leads therobot (off line)through the entire assembly task witha manual control. A computer recordspoints alongthat path in order to be able to replay the motionat a later time. The taught motionisplayed back and edited until it contains noerrors. Finally the robot is run online at itsappropriate speed to performthe taught motion repetitively.The most advanced approachto solve the communicationbottleneck 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 betweenthe robot andits environment. If the manipulatoris used for assembly purposes,sensory feedback isusually required. This can only be handledby conditionally programmedmethods thatare best implemented using high levelprogramming techniques.There are twoprogramming categoriesfor robotics applications.In robot-oriented programmingassembly tasks are dividedinto a number of robotmotions. The programmer hasto guide the robot throughthe task by sequentiallyprogrammingevery single motion segment. Thisprocedure is very cumbersomemakingit difficult to use.Chapter 1 An Introduction toRobots, Applications, and Previous Research12In task-oriented programming, assemblytasks are described in terms ofobjectsbeing manipulated rather thanby robot motions. This means thatthe programmer canspecify a task such as pickingup an object in a high-level language, andthe programthen uses a database to convert thetask to a robot-level program. Thisprocedure is muchmore user friendly thanrobot-oriented task planning, butmany problems, notably taskplanning, obstacle avoidance,and sensory information utilizationstill remain to besolved.The UBC controller, which is describedin detail in Chapter 3, uses high levelprogramming that allowseasy trajectory programming which canbe integrated withtechnological sensory feedbackto 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 accompaniedby the development offaster computer hardware that allows one to run control strategies that werenotpreviously feasible.Two main approaches have arisen from this previous work. The first controlstrategy, proposed by Tomizuka [11] tries to eliminate path errorby 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 necessaryto have a veryexact model of the plant.This strategy unfortunately often leads to the saturation of amplifiercurrent andthe violation of other torque-related constraints, duringphases of large acceleration.Weck [12] proposed a solution to this problem which involves placinga low pass filter infront of the controller. This alleviated the saturation problem, and bettercorner trackingwas achieved.The second approach, introduced by Koren [13], tries to minimizepath erroralone, while still allowing actual axial position errors. It calculates a patherror from theindividual errors of the axes and then uses this path error to compensatethe individualaxes. This strategy requires very fast hardware, and to date the authorhas not been ablefind a paper on an experimental setup using high performance servosin combinationwith this strategy.13Chapter 2 Previous Approaches to theDevelopment ofFast Contouring Controllers142.2 Comparison of ExistingApproachesA paper comparing the various advancedcontroller strategies has been presentedby Koren and Lo [16]. The courseof their argument will be followed in this introduction,following which an introductionto existing methods and the new conceptsarising fromthe author’s work, will be comparedwith the simulated and experimentalresults obtainedby Koren.Figure 2.1 shows a typical servosystem. The actual position is fed backandsubtracted from the desired position. Thiserror signal is fed into a controller. Thecontroller then feeds a velocity commandto the plant. Usually the plant may be modeledas a first order lag. In the simplestcase, the controller has a simple proportional gain.This system has been used in manyNC machines. Advanced CNC applicationsrequirevery high feedrates, whichlead to large contouring errors with P-controllers.Thus, Pcontrollers are often not adequate foradvanced CNC applications.Three basic methods have been developedto overcome the limitations ofa simpleproportional control action:The first approach applies more sophisticatedaxial controllers such as P1]) orstate feedback controllers. These controllersattempt to reduce the position errors oftheDisturbancesFigure 2.1. Typical Position ControlLoopindividual axes.Chapter 2 Previous Approaches to the DevelopmentofFast Contouring Controllers15The second approach addsa feed forward controller, suchas the ZPETC or IKFapproaches discussed in the introduction,to compensate for axial positionerrors.The third method uses cross couplingin order to reduce thecontour error ratherthan the axial tracking error. Thecontroller proposed in thisthesis has some of thecharacteristics of the third method,since the axes can communicatewith each other viaastate line. Unlike the original systemproposed by Koren, this proposedsystem is verysimple. It can handle more degreesof freedom and it is possibleto optimize contouringperformance and technologicalfactors such as maximumcutting forces, at the sametimeand with very little more effort.There are variations andaccessories to these threebasic control approaches.Themost prominent are adaptivecontrol and predictivecontrol as mentionedin theintroductory chapter.2.3 Sources of ErrorThere are three different types of error:The first are due to mechanicalerrors. This category containsbacklash ornonstraightness ofthe table motion. It cannotbe improved upon by the simpleadditionof more sophisticated controllers.Those errors can onlybe reduced by usingbetterhardware or compensationtechniques.The source for the second typeof error is the effects ofthe production process.Machine tool operations for instancelead to errors due totool wear or tooldeflection.These errors must alsobe handled through compensation.The third type of error arisesfrom the controllerand drive dynamics.Theseerrors are often dominantin high speed operations,and they canbe alleviated byimproving the controlalgorithm. The three abovecontroller types andthe 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, amismatchin the open loop gains will lead to a steady-state error when following a straightline.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 disturbancesresult fromcutting forces or other parameters imposed by the process.The third kind of error stems from the contour path and the machinedynamics.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 saturationcurrent of theamplifier.Most of these errors increase with feeding speed. This meansthat more effectiveservo controllers are needed for high speed operations.2.4 Feedback ControllersIn this category controllers with basic feedback onlywill be listed.2.4.1 P-ControllerThe P-controller is a proportional gain that is usually tunedsuch that the closedloop damping ratio is approximately equal to 0.707. This controllerworks well for lowfeed rates.Chapter 2 Previous Approaches to the Development ofFast Contouring Controllers172.4.2 Pm-ControllerThe PID-controller adds the integrated error anda derivative of the error to thecontrol signal. The integral term ensuresthat the system has zero tracking error for rampinputs (assuming the uncompensated system istype I), and the derivative term shapes thedynamic response of the system. The Pm-controllerprovides poor tracking andsignificant overshoots for nonlinear contourslike sharp corners which cannot bedescribed with first order polynomials. Theseproblems are addressedbypreprogramming a path with sufficiently small accelerations.Figure 2.2. P-Controller and PID-ControllerStructure2.5 Feedforward ControllersThe idea behind this type of controlleris to implement a filter in the control loopthat leads to a unity transfer function ofthe 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 attemptto cancel the zeroes and poles of thesystem. A block diagram of this controller is shown in Figure2.3.P-Controller P1D-ControllerR+Chapter 2 Previous Approaches to the DevelopmentofFast Contouring Controllers 18G0(z) is the feedforward filter thatis intended to cancel the position loop.Ideallyit should have the following form:G0(z)(B(z)1=1H(z) D(z)(2.1)kA(z)J L\l+H(z)D(z))It is not possible to cancel thezeroes outside the unit circlebecause to do sowould lead to an unstablecontroller. The zeroes are divided intocancelable and noncancelable zeroes:G0(z)= (B+(z) B(z))1(2.2)A(z)Tomizuka only cancels the cancelablezerosB+andarrives at the followingcontroller:G(z)A(z1)B(z)2(2.3)B(z1)[B(1)]The phase angle of the overall transferfunction of this system is zero, whichimplies that this system achieves zero phaseerror tracking. Haack and Tomizuka [17]proposed the reduction ofthe possible gain error by adding zeroesto the feedforwardcontroller.This system requires that the model ofthe system be very accurate. Disturbancetorques will severely reduce the performance ofthe system.Reference ErrorActualPositionPositionRFigure 2.3. Block Diagram ofZPETCChapter 2 Previous Approaches to the Development ofFast Contouring Controllers19This controller also produces very large control signals that can saturate theamplifiers. Buttler et a!. [18], recognizing this problem, have proposed theuse of pathplanning to avoid large accelerations.2.5.2 Inverse Compensation Filter (IKF)The ZPETC controller inherently requires large control signals which saturatetheamplifiers and motors during moves with high frequency components. Thisproblem wasalleviated by Weck [12] by putting a low pass filter withlinear phase in front of theZPETC. Figure 2.4 shows the a block diagram ofthe IKF Controller.The ideal IKF filter has the transfer function described in equation 2.4:IKF(z)= .ft3AT +[i + cos(i it/N)] sin(i AT)+ z’)lA(Z1)B (z)z:1(2.4)i-i2z itJI iHere N is the filter order, and Weck proposed that Nbe equal to the ratio of thedrive time constant and the controller sampling time. AT is the controller samplingtime,cis 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 ZPETCPositionRFigure 2.4. Block Diagram ofIKFChapter 2 Previous Approaches to the Development ofFast Contouring Controllers20Week 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 insecurityof model design.The results obtained with this method seemto solve most of the ZPETCcontroller’s problem. Still the system tendsto become unstable for sampling times of lessthan 5 ms. The threat of saturatingthe amplifiers has been considerably decreased, butnot eliminated.2.6 Cross Coupling Controller(CCC)This controller was first proposedby Koren [13]. It is based on the idea that, inorder to improve the contouring performance, it is necessaryto eliminate the path errorrather than individual axis errors.This controller uses a contour error model that is then implementedin a simplecontrol law. Figure 2.5 shows a block diagram ofa 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]. Itwas able togive an improvement in performance of 1:5to 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 DevelopmentofFast Contouring Controllers21sampling rate. It seems thatit will be difficult to use thissystem with highperformanceservos, as such servos needvery fast sampling ratesto 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 integrationofprocess 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 correspondstoapproximately twice the time constant of the slave servos. The slaves aredigital lead lagcontrollers that interpolate 32 times between every master position. Theyhave 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 Controller23STD BUSII IIII IIMaster SlaveSlave SlaveCPU CPUCPU CPU80x86 Axis 1Axis 2 Axis 3Coordinating LineState LineJj.. Jj,Front Plane BusFigure 3.1. UBC Controller ArchitectureThe slaves receive positionincremnents every master sampling time.Theseposition increments aremodified twice. First the slaves performa second stageinterpolation. This procedurereduces the sampling timeto half a millisecond and it alsoprovides velocity shaping in orderto reduce acceleration levels between positionincrements. The second operationthat is performed on the referencesignal is velocitymodulation with the stateline. This concept allows the reductionof acceleration levelsbelow the saturation limitsof the amplifiers. The signal modifiedin this way is then fedinto a simple lead-lagservo controller. It should be noted, thatit is quite difficult toanalyze the complete slave controllerwith conventional control techniques, becausethesystem is not linear and the authorwas not able to obtain a closed transferfunction of thesystem. A comprehensive stabilityanalysis of the system has not beenperformed, butsimulations and experiments of thecontrol system including the state line strategytend tobe more stable than the simple servocontroller. A schematic block diagramof a two axiscontrol system is shown inFigure 3.2.Chapter 3 UBC ControllerLineIf SL-1,Ry - SyElseRy - SyEndFigure 3.2. Schematic Block Diagram of a Two AxisControl 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-lagcontroller ofStateMyIRyVelocitySplineVyIf Vy•+östat>Ey,ElseSLy-OSLy— 1EndEythe slaves is shown in Figure 3.3:Chapter 3 UBC Controller25The 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 arerelativelyhigh, is to convert the digital filter and the zero order lag to a continuousfilter. Thecontinuous filter will have the following form:0 s+aF(s)—K(3.1)E sbUsing forward difference approximations this can berewritten 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 givenby: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 Controller26A simplified continuous block diagram is shown in Figure 3.4.(Both saturationlimits and the amplifier bandwidth are neglectedin this case.)1Kss =____KtsaJeti =_________Ka Kt KtsaThe resulting block diagram is shown inFigure 3.5There are two very important characteristics of thissystem. First, it has a limitedacceleration due to current saturation. By inspectingFigure 3.3 it becomes apparentthatthe 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)=KKdKssKesa(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 someofChapter 3 UBC Controller28the work of the master to the slaves, because the master needs tosend less positionincrements to the slaves. This enables the master to concentrate on otheroperations suchas task planning.The Interpolation scheme was developed by Yellowley and Pottier[201.Itassumes that points are evenly spaced in time, and it assumes alinear variation invelocity over each master sampling period. Thisleads 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 thisscheme. The splineassumes a constant acceleration and zero jerk over a master samplinginterval. 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 wouldhave acontinuous velocity profile was investigated, as a first alternative, and its derivationisChapter 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=vI + = +(3.10)LaKKdKssKeJThe first term allows for the expected error proportional to velocity andstatallows 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 Controller30calculated. The most logical approach is to assumea velocity spline that linearlyconnects the average velocities of two adjacent mastervelocities and thus contains novelocity discontinuities. It takes two master samplingintervals to accelerate from zero toa constant velocity; this corresponds to about 4 timesthe time constant of the non-saturated position loop. The spline is illustrated in Figure3.7:velocity—— velocity spline\I I\ I I-timeH AFigure 3.7. Velocity SplineThe spline itself can be described mathematically inthe form:v(t) = — x_1)+ (x+2 — — x,+x1_)4] (3.11)3.4.2 State Line Triggering LogicThe intention of the approach shouldby now be clear; the author intendstotrigger the state line when the actual erroris greater than would be expected,should thevelocity be that of the velocity spline. There arehowever several ways in which thelinemay be triggered.In the first series of experiments the state linewas triggered using an absoluteerror criterion of the form:Chapter 3 UBC Controller31IeaI(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 referenceposition.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 itis necessary tonegate the second constraint. This is equivalent to multiplyingeither 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 necessaryto choose amaster sampling time that allows the system sufficient time toslow down frommaximum speed to zero speed within one master sampling period.One way to determine the required master sampling periodis 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 fromvoto v1 can be calculated using:Chapter3 UBC Controller32vi— vOt=This requiresa distance of(3.16)1s=v0t+—at21 0(3.17)2 2aDuring a deceleration phase,this requiresthat the state line startstriggeringNdslave samples before thevelocity discontinuity,where:Nd= v12—v0v0AT 2vaAT(3.18)During an accelerationphase, this requiresthat the state line stopstriggering Naslave samples after thevelocity discontinuity,where:N0== V12 —V02v1T 2vaET3.6 Filter ParametersThe position loopwithout the state lineis a third order system:(3.19)This third ordersystem can be reducedto a second ordersystem by cancelingthedynamics of the velocityloop with the filter.This leads to:UFigure 3.8. Block DiagramofPosition LoopChapter 3 UBC Controller33a= Ktsa Ka Kt(3.20)JeThe position loop block diagramreduces to the form shown in Figure 3.9:The transfer function of this systemis given by:0 KKdKaKtKe(3.21)R Jes2+Jebs+KKdKaKtKeThe natural frequency of this second order systemis given by:IKKdKaKtKe=(3.22)JeThe damping ratio of that system isgiven by:cb(3.23)2wThe final parameter of interest is the steady statephase 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 frequencyand a damping ratio in order tocalculate the filter gain K and the lag time constant1/b.In the case of the velocity spline, one actually needsto specify the phase lag andthe damping ratio of the system. This leadsto a filter gain of:K=4Je(3.25)Kd Ka Kt KeFigure 3.9. Block Diagram ofReduced PositionLoopChapter 3 UBC Controller34The resulting inverse of the filter lag time constantis given by:b=2iIKKdKaKt(3.26)Jeand the resulting natural frequency of the system is:(3.27)This parameter choice usually yields astable and fast result. In the case of thesystem with a sampling time of0.5 ms, a desired phase lag of 3 ms anda desireddamping ratio of one, the resulting naturalfrequency of the system was 666rad/sec.3.7 Mismatched GainsIn some circumstances it is possible (butin the author’s opinion not veryprobable), that the gains of different axes willbe mismatched. In such an event thefollowing error of the different axes willhave different proportionalityconstants to thevelocity. A steady state velocitypath error will then result. Simplychoosing thesmallest proportionality constantas the common velocity error constant willtrigger thestate line and slow down the system. Suchan approach however will still resultin a patherror which is proportional to the new,slower, velocity.In order to run at the maximum speed withno path error it is necessarytofeedforward the difference in errorsbetween the fastest and the sloweraxes to the sloweraxes. In this manner all the axes have their own gaindependent velocity errorconstants,and the system can still run at the referencespeed.It is relatively easy to implement thisconcept for linear motions withinthe UBCcontroller. The axis with the largest gainstill operates in the usual fashion.Any sloweraxis will have a larger velocity error constant.The actual steady state error willbe:(3.28)Chapter 3 UBC Controller 35The faster axis requires a steady state error of:ere..dred d?ssjasiestVThe error may be reduced by the difference of the above two equations:compensation v(ciasenaxPzsacnsai)(3.29)(3.30)velocityv(fastest axisactuaIIn 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 Controller36controlling the amount of error of eachaxis. The procedure ensurespath accuracywithout extensive path preprocessing orfast computer hardware.Simulations and experimental resultsfrom a real system are presentedin the nextchapter. They will show that the strategyutilizing a velocity spline inorder to trigger thestate line provides good results which arecomparable to the ones obtainedwith otheralgorithms described in the literature.Chapter 4Simulation and Experimental Results4.1 Introduction to Computer SimulationsThe first phase of the work concerned with theevaluation of new controlstrategies, involved the creation of a realistic simulationmodel of the system. Theprogram was developed using Matlab; it allowsthe examination of the path performanceof an arbitrary number of axes and the introduction of nonlinearinfluences. A typicalhigh speed servo system was also constructed andtested to validate the results from thesimulated model. Results from the experimental systemare given later in this chapter.The actual MATLAB program is reproduced in its entirety inAppendix 3. Asmentioned previously, the program allows the calculationof actual path errors as wellasindividual axis errors in an (N) axis system. The majornon linearity in the basic systemcomprises the current limit of the amplifier,which is included in the simulation. Theordinary differential equations correspondingto the analog part of the loop were solvedusing a second order Runge-Kutta algorithm,while the time domain equations ofthedigital part (including the state line), were solveddirectly. The values of the variousparameters in the model were chosen to correspondto the high performance experimentalservo system, which is described later.The majority of the simulations have been concernedwith corner trackingcapability. In order to simulate this, the path shownin Figure 4.1 has been used. Thereader should realize that the path chosen requires oneof the servos to change direction,while the second may, in essence continueat 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 ServoControllerSolid: 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 thestate 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. Thestate line inthis strategy is triggered whenever the position error ofa 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. Thepath accuracy is very good,but the system has been slowed down to a third of the original speed; this resultsfromthe 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 500600 700Chapter 4 Simulation and ExperimentalResults 404.1.2.2 The Use of a Velocity SplineIt was shown in section 2.2 that theexpected following error is proportional tovelocity. The fact that this kind offollowing error does not lead to a path errorbut ratherto a phase lag, was used in section 2.4.1to derive an error criterion that uses a velocityspline. The simulated response resultingfrom this approach is presented in Figure 4.3.It shows that this method yieldsexcellent path accuracy while only slowing down themean speed over the complete simulationby 20%, (the velocity spline approach took0.285 seconds compared to 0.225seconds for the simple servo.) The actuators wereoperating at the reference speed duringmost of the simulation; at the corners howeverthe state line actively slowed downthe 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 wayof achieving thisgoal is to feedforward the expected velocity error. A simulation ofthis approach isshown in Figure 4.4. The system is now able to eliminate thephase lag, but cornertracking performance has deteriorated. When the axis with no discontinuityis sloweddown at the corner, the velocity feedforward is no longer proportionalto the actualvelocity (0 at the corner itself.) Thus when the state line isactuated, the velocityfeedforward for the second axis is larger than it should be, and a path error is encountered.150Chapter 4 Simulation and Experimental Results41X—Y Position100’---50 - - - -0—50 . - --100 . - - - --150 ——100 0 100 200 00 400500Figure 4.4. CornerSolid:Dashed:Dash-dot:500 700theta 1 [d.g3Tracking with Velocity FeedforwardMaster Position SplineRefrrence PositionActual PositionChapter 4 Simulation and ExperimentalResults 424.1.4 ConstantJerk Position SplineIt was shown in section 2.3 that the original positionspline has velocitydiscontinuities. A new spline with constant jerk duringa master sampling period wasderived and tested. Figure 4.5 shows the trackingability of this new spline. It shows nogreat improvement over the existing constant accelerationspline.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 thedifferent 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.6800007Velocity Spline 0.2850005Velocity Feedforward 0.2875 045Constant Jerk Position Spline 0.2795 005Figure4.6.Solid: Dashed: Dash-Dot:thetal[BLU]EnlargementsofPathsinCornerTrackingat500RPMMasterSplineConstantAccelerationpositionSplineConstantJerkPositionSplineEnlargementofCornerTrackingat500RPMIIIIIII100’F-JC—I/-003300 3200 3100--3000--2900-- 2800 2700---”------++StatErrOrCflteflOfl:J’.***:VelocitySpline2600--oo;Velocity--/ocxConstantJerkPositionSpline2500I0/•1I.SimpleS1Ørvo26002800300032003400360038004000SimpleServoVelocitySpline,Velocit’FeedfôrwardIFigure4.7.Solid: Dashed: Dash-Dot:thetal[BLUIEnlargementsofPathsinCornerTrackingat1000RPMMasterSplineConstantAccelerationpositionSplineConstantJerkPositionSplinexlOxl01.45IIEnlargementofCornerTrackingat1000RPM1.4——————————:‘——.——IIIIIII,_.IIIISIIIISII1.35D -J040a)-c000,10001.31.250xxxConstantJerkPositionSpline1.2—1.25I-—I$1.651.31.351.41.451.51.551.61.71.75VaChapter 4 Simulation and ExperimentalResults 464.2 Experimental ResultsAfter the velocity spline had beenchosen as the most efficient method to triggerthe state line, actuators, amplifiers,and controllers from a retrofitted GMFs-108 robotwere used to validate the simulatedresults.4.2.1 Experimental ApparatusThe experimental apparatus consistsof a controller, amplifiers, and actuators.The actuators are equippedwith tachometers and encoders. The tachometersignals arefed back to the amplifiers, and the encodersignals 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.1IComputerControllerRequiredAxisPositionDigitalFilterDIAConverter(BLUAmplifierActuatorIEncoder1IActualPositionIBLU1ActualPosition(BLUTachometerActuatcxspeed1[rad/secFigure4.9.SchematicBlockDiagramChapter 4 Simulation and Experimental Results484.2.1.1 ActuatorsThe actuators are DC motors which are suppliedby Inertial Motors. They areequipped with tachometers for measuring actuatorvelocity and encoders for measuringactual position. Table 4.2 lists the motor parameters:Table 4.2. Motor ParametersName Symbol Value UnitsMotor Torque Kt 0.2967Nm/AConstantMotor Inertia Je0.9636e-3 Nm s’2Tachometer Ktsa 0.047 V/(rad/sec)FeedbackEncoder Gain Ke 636.6198BLU/radMaximum Current1m10 AMaximum Speedcomax 2000 RPM4.2.1.2 AmplifiersThe amplifiers were built by Glentek specifically for the Inertial motors.Theyare PWM amplifiers with a bandwidth of more than750 Hz. Table 4.3 shows allpertinent parameters of the amplifiers:Table 4.3. AmplflerparametersName Symbol Value UnitsAmplifier Gain Ka 13.6136A/VMaximum Current‘m10 ABandwidthfcmn1jfir >750 HZChapter 4 Simulation and Experimental Results494.2.1.3 ControllerThe control algorithm was implemented ona Ziatech STD32-bus industrialcomputer. The master is a V53 and the slavesare 80c 196 based STD32 boards fromUniversal Systems.The controller parameters are listed in Table 4.4:Table 4.4. TableofModel ConstantsName[Symbol ValueUnitsD/A Converter Kd 0.0049GainFilter Gain Kp34.1333Filter Lead A 0.9ParameterFilter Lag B-0.333ParameterMaster Sampling At16 m seePeriodSlave Sampling AT0.5 msPeriodPosition LoopC 1Damping_RatioPosition Loop3 msSteady State RampPhase LagPosition LoopUn 666.67 rad / secNatural FrequencyPosition Looph67 HzBandwidthThe filter parameters were chosento yield a damping ratio of unity anda steadystate ramp phase lag of 3 msec. Obviously the theoreticalbandwidth of 67 Hz cannot beachieved, unless sinusoidal reference commands with verysmall 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—110°101 102Frequency (Hz)Phase response -—100—15C10—i 10°101 10210Frequency (Hz)Figure 4.10. Bode Plots ofExperimental System— _I_ _I_ ..I _I_IJ I_ILTI1hLI_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 orderto achievereliable test results. In the experiments that follow, a forthprogram was run on themaster. This program continuously reads the values of position and error fromthe slavesand writes them to an array. The program is initiated after all theposition increments foran experiment have been transferred to the slaves. At this point the slaves areslowlyemptying their buffers, and the recording program simply writes current positionsanderrors to the array. As soon as the array is filled, the program stops. At a laterstage thearray is transferred to a file and imported into Matlab for post processing.Thisprocedure is extremely simple, but unfortunately it cannot capture the beginningof anexperiment. It also does not provide an accurate measure of time, becausedata is simplyread as quickly as possible from the slaves. The data recording program andprograms tocreate the input required for the experiments themselves are listed in Appendix4.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 Results524.2.2 Circular InterpolationDuring machining operations, thecontouring ofcircles is the second most commongeometry, (aftercontouring of straight lines.) It is therefore importanttodetermine the limits of the system in circular interpolation.In addition, circular interpolation cangive some insight into the frequency response ofthe system. In this section relationships willbe derived which specifically define thelimits that the simple servo system andthe modified system with a velocity spline canfollow. It will be shown that a frequency response inthe usual sense is difficult to derivein those cases where the simple servosystem would show a deteriorated performance dueto the imposed saturation limit. The state line system, whensubjected to the sameconditions, will decrease the reference frequencyto acceptable levels, hence avoidinglarge errors.In order to achieve good path accuracy, it is necessaryto meet the constraint setby the position spline. A single axis is requiredto have a maximum path error of lessthan one BLU. (One BLU is theresolution of the encoder. One revolution of theactuator corresponds to 4000 BLUs.) Accordingto Pottier [21] this requires:142.6667(4.2)VrThis 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 secondconstraint on the system.During circular interpolation the actuator position is describedby:Chapter 4 Simulation and Experimental Results530 = rsin(2irft)(4.4)The corresponding velocity is:ci=2irfrcos(2irft)(4.5)The maximum velocity takes place whenthe cosine term equals one (or -1).Thisleads to the final form of the second constraint:=2fr(4.6)The third constraint is current saturationof the amplifier, whichlimits thepossible acceleration. The acceleration correspondingto the reference signal is:a = —(2irf)2rsin(2irft)(4.7)The maximum acceleration occurs whenthe sine term equals one (or -1).Thisleaves a third constraint that correlatesthe maximum radius to the maximumfrequency:a(4.8)JeThe maximum velocity constraintis only active at low frequenciesand largeradii. The problem then isto determine which of the two other constraintsis dominant.For the system under consideration witha slave sampling time of0.5 ms and a mastersampling time of 16 ms, Figure 4.13shows a plot of frequency versuscircle radius.The two constraints meet atrcrit= 5900 BLU andcrit= 2.9 Hz. At radii lessthanrentthe position spline imposes thedominant constraint. At radiihigher than thecritical radius, current saturation isdominant.If one attempts to use reference commandsalong the position splineconstraint forradii larger thanrerit,the state line should pull the actualresponse down to thelimitingacceleration constraint line. This meansthat it would decrease the actualoutputfrequency. The shape of the sine curvewould of course change,because the state linewould only try to alter the portions thatexceed the limiting acceleration.In the extremecase the final output would have a constant curvaturethat changes sign every half cycle.Chapter 4 Simulation and Experimental Results5402 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 complicatesa true frequency analysis,because the error is superimposed on the non sinusoidal carrier signal. The tworesponses are difficult to separate, because one cannot easily determinethe 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 FeedrateCircular Interpolation ExperimentChapter 4 Simulation and ExperimentalResults56Clearly the system is able to followthe contour well. No state line is needed,because the saturation limit isnot reached. The maximum error is approximately45BLUs.4.2.2.2 High Feedrate ExperimentIn this experiment the feedrate was chosensuch that current saturation and themaximum required speed would makeit impossible for the simple servo to follow thepath. Table 4.6 shows the parametersfor this experiment:Table 4.6. Parameters for High FeedrateCircular InterpolationName ValueUnitsFrequency3 HzRadius8000 BLUFeedrate 150796BLU/secFigure 4.16 shows the actual path andFigure 4.17 the corresponding path errorduring the high feedrate experiment usinga simple servo with no velocity spline:9000Path During Circular Interpolation—C—0 2000 4000 60008000 10000x—axis [BLIJ]Figure 4.16. Path ofSimple Servo System During High FeedrateExperiment12000Chapter 4 Sinudation and Experimental Results57The system is not able to follow the reference path, becauseacceleration levelshave driven the amplifiers into saturation, and the requiredvelocity exceeds themaximum velocity of the actuators..Figure 4.18 shows the path and Figure 4.19 the path error ofa high feedrateexperiment with the state line being activated using thevelocity 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 Results58Clearly 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 Results59with no compensation, a steady state path errorof approximately 75 BLU wasencountered. In the second experiment the compensation schemeyielded a steady stateerror of approximately 5 BLU. This shows that the schemedescribed 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—.65008500 9000Path Error with Mlsmotoh.d Ax706050403020100—104A— — — —.1.10 5 10 1520 25Chapter 4 Simulation and Experimental Results604.2.4 Disturbance TorquesThe controller strategies described inthe previous sections should be able tocompensate for disturbances that might resultfrom loads on the actuator or cuttingtorques. It will simply slow down the systemuntil the error criterion imposed on thesystem in order to achieve the necessarypath accuracy is met.In order to check the performance of the system withsevere disturbances, it wasdecided to abruptly stop the second axisduring 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 requesteddistance 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 disturbanceare shown in Figure 4.22. The path errorof this system is shown in Figure 4.23.Servo Path with Disturbance Torque40003500 - -- - - - - - - -: —2500- - - - —2000— — — —1500a —1000—500 - - - - - - - - —o—500 0 500 1000 1500 20002500 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 thestate 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 showsthepath error in this experiment. It is reduced to 15 BLUs.This simple experiment would lead one to believe that the systemis 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 ExperimentalResults 62Servo Path with Disturbance Torque20008.1500 - - -1000500 -—500 0 500 10001500X—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 Results634.2.5 Corner TrackingIn terms of robotic and high speed machining operations, the most importanttestcriterion is corner tracking ability. This task requires the controller to compensateforlarge motor inertia torques that can lead to saturation of the amplifiers. Usually oneattempts to avoid saturation by choosing a sufficiently small speed arounda 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 achievedat maximumpossible deceleration and acceleration levels.The experiment was arranged as described in the previous sectionso that theactuator of the first axis would travel at constant speed while the second actuatorwouldreverse at the same speed a number of times during the experiment. Thedata 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 anactive state line.Clearly there is a vast improvement in path accuracy.This experiment also allows the validation of the results obtained fromthesimulations. Clearly Figures 4.1 and 4.26, and Figures 4.3and 4.27 show similarresponses.Figure 4.29 shows a magnified view of the performanceof the simple servo in thefirst corner. Figure 4.28 shows an enlargement of the firstcorner of the performance ofthe system with an active state line. The simple servohas a path error of approximately26° while the system with a state line shows lessthan 2° of path error. The “nonsmoothness” of the reference path is a result of thedata 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 andthe actual position may be a sample out ofphase.—100 0 100 200 300 400 500 600 700x..directionIdegiFigure 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 ofthe first axis versus the second axisof a simple servosystem. Figure 4.31 shows the response of thissame system with an activestate line.Figure 4.32 is a magnified view of the first cornerof the simple servo system.Figure4.33 is an enlargement of the performanceduring the first corner of thesystem with anactive state line. The simple servo systemnow has a path error of approximately900andthe system with the active state line hasa 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 TrackingofSimple 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 at500RPMSolid.• Reference Pos.Dashed: ActualPos.\\\-Chapter 4 Simulation and Experimental Results66Contouring Path of UBC Controller at 1000 RPM400IF-500 1000 1500 2000 2500x-direction (deg]Figure 4.30. Corner Tracking ofSimple Servoat 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 necessaryto select aperformance parameter (since no two researchers use the same experiment.) Itwasdecided 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 ExperimentalResults 68Koren has presented computer simulations forthe ZPETC arrangement proposedby Tomizuka as well as his proposedCCC arrangement [16] (both of which have beendescribed in Chapter 3.) Motor velocityin this case was approximately 25 RPM. Itshould be noted that the data presentedis derived from simulations only. Simulationswill often give betterresults than real experiments, because they utilizeabsolutelyaccurate models with matched gains, andno unexpected disturbances.Table 4.7 shows however, that the system proposed inthis thesis performsfavorably when compared to the simulationresults of other systems.Table 4.7. Performance in Corner TrackingControl AlgorithmSingle Axis Speed Performance Factor rjZero Phase Error Tracking Control25 RPM 375Cross Coupled Control25 RPM 750UBC Controller500 RPM 1500UBC Controller1000 RPM 8504.3 ConclusionsIn this chapter simulations of differentcontroller strategies have been presented.It is concluded from these simulations thata velocity spline approach to trigger the stateline is the best compromise betweencomputational effort and contouring performance.The experimental results are ingood agreement with the simulations., validatingthe conclusions drawn from the simulations.Constraints have been derived for circularinterpolation. They limit themaximum amplitude or frequency of thesystem according to saturation limits(acceleration), position spline accuracy, and maximumvelocity. The experiments carriedChapter 4 Sinudation and Experimental Results69out show that the velocity spline approachcan in fact overcome the amplifiersaturationlimit.It has also been shown thatthe compensation scheme for mismatchedaxesdescribed in section 2.7 successfullyeliminates path errors in linearmoves.The controller’s ability to compensatefor disturbance torques hasbeen proven byshowing that it is able to slow downthe complete system almost instantaneously,whenone of the axes is stopped by an externaltorque. This result suggeststhat the controllerwill be able to compensate fordisturbance torques that are notas drastic as the onechosen in this experiment.Finally the corner trackingability of the control algorithmhas been extensivelytested. The experimental resultscompare very well withthe simulations obtained inprevious sections. The system can indeedtrack a sharp corner at high speeds.Theresults from the author’s experimentshave been compared with simulatedresults, shownin the literature. The UBC controllersperformed equally well or betterthan any othercontrol strategy found.Chapter 5Conclusions and Future DevelopmentsMany robotic and contour machining applications requirea high degree ofaccuracy from the feed drives. This goal cannot be accomplishedwith conventional servocontrollers. A great deal of research has gone intothe development of advanced servocontrollers, however most of these controllers aredifficult to implement in practicalmachining or robotics environments. The latterpoint arises because the time constants ofthe plants are so short that digital controllers havedifficulties performing the requiredarithmetic in the allowable time. Finally some of the algorithms relyon an exact model ofthe plant which may not be available in a real system.Computer simulations show that the limiting factor in manycases is not the controlalgorithm, but the physical limits ofthe system. When a system has to followa path witha discontinuity or a sharp corner, it is often impossibleto outperform a simple servocontroller, because the possible accelerationof the system is limited by the limitingcurrentof the actuator. This in turn means thatthe system will become nonlinear at highaccelerations. Such behavior is often neglectedin 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 prescribedpath due to a current limit oradisturbance torque, the whole system may have largeerrors. By using the UBC controllerarchitecture, it is possible to slow down the complete systemin those cases where one axiscannot follow the prescribed path. This is achievedby using a state line on the front planebus, which connects all the slaves. When the state line is high, all slaves operatenormally70Chapter 5 Conclusionsand Future Developments71as simple servos. Ifone of the slaves encountersa constraint, it pulls the stateline low.This tells theother slaves to stop processingnew position incrementsuntil the slave inquestion has resolved theproblem and pulled thestate line high again.The system hasbeen simulated and the resultsfrom the simulations havebeen usedto choose an algorithmfor the triggeringof the state line, whichrepresents a compromisebetween computationalefficiency and resultingpath accuracy.The system has beenanalyzed in Chapter3 of this thesis. A proposalfor theselection of all pertinentparameters is given.This selection process isquite simple, andusually provides reliableand stable controls.In a series of experimentaltests, the simulatedresults have been validated.Theresults of thesetests indicate that thissystem is able to achievecontouring performancesthat are better than othersystems found in theliterature to this point.A simple algorithm forcompensation of mismatchedgains was also developedandtested in a straight line moveon the experimental system.The experiments indicatethatthe algorithm isable to compensate formismatched gains.Since disturbancetorques compromise thecontouring performance ofrealsystems, a test was carriedout to demonstrate the robustnessto external disturbances ofthe controller. Duringa linear move, one axiswas stopped abruptly.In a normalcontouring system otheraxes would continueto operate normally,and thus a large patherror would be introduced.The UBC controlleron the other hand was ableto detect theerror and stop the secondaxis in its path. A minimalpath error was detected.In the future it wouldbe of interest to combine thecontrol algorithm witha pathplanning algorithm.A team under the leadershipof Dr. Cherchasat UBC has beenworking on a pathplanning program for roboticapplications, which utilizesAuto Cad forsolid modeling.It should be possibleto utilize their findings ina real time as opposed topreprocessingstage. A separate mastercomputer couldbe used to perform the pathChapter 5 Conclusions and Future Developments72planning, which would then pass therequired path directly over the STD-32bus to themaster controller.This controller can be used in machiningoperations. These applicationsoftenrequire technological input to thecontroller. Mr. Ramin Ardekani[22], has developed acontrol algorithm for the UBCcontroller that is able to use thestate line in order to limitthe cutting force or equivalent chip thicknesson a lathe. It wouldbe of interest to fusethis system with the new contouringcontrol. This should be possible,because bothapplications rely on the state lineto slow down the system whena constraint isencountered. This means that the systemcan either be slowed down bycontouringconstraints or machining constraints,depending on which constraintis dominant.It should be mentioned thatthe control algorithm, as it stands,is designed forCartesian machines. Unfortunatelythere are a lot of non Cartesianmachines used inindustry (especially robots). It ispossible to transform the Cartesiancontour errors tojoint coordinates usinga Jacobean transformation. Inthis way the controller canbemodified for non Cartesian machineswith little more computationaleffort.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 MechanicalHand’, 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 ScienceDepartment,Stanford University, Palo Alto, Calif.[4] Kahn, M. E., Roth, B., 1971, “The Near-Minimum Time Controlof 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 Notationfor Lower-PairMechanisms Based on Matrices”, J. App. Mech. , vol77, pp. 215-221.[7] Uicker, J. J., 1965, “On the Dynamic Analysis of SpatialLinkages using 4x4Matrices”, Ph.D. dissertation, NorthwesternUniversity, Evanston, Ill.[8] Stepanenko, Y., Vukobratovic, M., 1976, “Dynamics of ArticulatedOpen-ChainActive Mechanisms”, Math-Biosciences, vol. 28,pp.137-170.[9] Koren, Y., 1985, Robotics for Engineers, McGraw-Hill BookCompany, NewYork,pp5 1-59[10] Craig, J. J., 1986, Introduction to Robotics, Mechanicsand Control, AddisonWesley Publishing Company, Reading, Mass.,pp.19 1-222.[11] Tomizuka, M., 1987, “Zero Phase Error Tracking Algorithmfor Digital Control”,ASME Transaction, Journal ofDynamic Systems, Measurementand Control, vol109, March,pp65-68[12] Week, M. and Ye, G., 1990, “Sharp Corner TrackingUsing The IKF ControlStrategy”, Annals of CIRP, vol. 39, January,pp.437-441.73Bibliography74[13] Koren, Y., 1980, “Cross-Coupled Biaxial Computer Controlfor ManufacturingSystems”, ASME Transaction, Journal of Dynamic Systems, Measurement andControl, vol 102, no.4, December, pp265-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, “AdvancedControllers 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 JournalofDynamic Systems, Measurement and Control, vol. 113, March,pp.67-74.[19] Koren, Y. and Lo, C. C., 1991, “Variable GainCross-Coupling Controller forContouring”, Annals of CIRP, vol. 104, Aug.,pp. 37 1-374.[20] Yellowley, I. and Pottier, P., 1989, “ A Note Ona 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 MachineTools”, 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 Errorin the z-domainThe derivations in Chapter3 were based on a very simple forwarddifferenceapproximation of the discretefilter. It was deemed desirableto prove that thisrepresentation is adequate for thedesign of the contouring algorithmof the UBCcontroller. Thus the following error wasderived in the z-domain andcompared with theresults obtained in chapter3.The simplified continuous part of Figure3.2 can be transformedinto a third orderdiscrete system, assuming that theD/A converter behaves likea zero order hold. Theclosed loop position transfer functioncan be described by:O(z)— Kl(1z_1÷(A1)z2—A2z)Al 1R(z) -l+(B+a+K)z+(Ba+a2-KA+Ba(I3where: a1 = _[i + ea2 = eK1 KpKdKssKeEquation A 1.1 can be rewritten into:O(z)= Kib1z+b2z+b3z(Al.2)R(z) l+a1C+i.2+a3zThe transfer function for the erroris: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+az3Using long division this can be simplified to:evEaKl1)(22)(A1.5)I,1+a1+a23This 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 wasproposed by Yellowley and Pottier[20] hasvelocity discontinuities. Duringthe course of this researcha new spline was proposed andtested. It has constant jerk anda linear change of acceleration overone master samplingperiod. This results ina continuous velocity, with accelerationdiscontinuities.The displacement overone master sampling intervalfor a path with constant jerkisdescribed by:1 f 1 /x=xo+vI+!aoi1j1ui(A2.l)O&2 k.&) 6O&)where: 0t&The spline assumes that the starting andfinishing velocities are the averagesof thetwo adjacent velocities atthose points. This assumptionprovides two boundaryconditions.The boundary conditions att=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 definedas:77Appendix 2 Constant Jerk PositionSpline 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 fria0+—J0(A2.6)2 2It is necessary to have the correct mean velocityover a master sampling period.This requirement provides the second equation in orderto 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 andvelocity proffles. It is animprovement over the old spline that assumesa constant acceleration over a mastersampling time interval. Figure A2. 1 shows anexample of a velocity profile of the constantjerk position spline.Appendix 2 Constant Jerk Position Spline79velocity/%— master samples- -position spline--. -st-i imeFigure A2.1 Constant Jerk Position SplineThe velocity spline of this position spline is describedby equation A2.4. In thosecases where the acceleration exceeds the maximum allowablelimit imposed on the systemby current saturation, an new average velocity is chosen such thatthe acceleration stayswithin its bounds. Maximum acceleration can eitheroccur 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 Spline80This 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 splinedoes 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 usingMatlab,that is able to simulate an n-axis servo system with current amplifiers. The programuses aRunge-Kutta second order algorithm to solve the differential equations of thevelocityloop. It should be mentioned that the program allows for current saturationand 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=2A1l*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 SimulationSource Code83% 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 CreateSlave SamplesThere are two versionsof this file. the first onecreates a position splinewithlinear acceleration, andthe second one creates a positionspline 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 ofMatlabSimulation Source Code85A3.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)=vO(j,1);ae(j,1)=aO(j,1);je(j,1)=jO(j,1);else,ve(j, 1)=1/6*(2*V(j,i+ l)+7*V(ji_1)+2*Amax(j,1)*sign(aO(j,1)));ae(j,1)=Amax(j,1)*sign(aO(j,1));je(j,1)=3*V(j,i’ 1)_6*V(j,i)+3*V(j,i_1)_2*ae(j,1);end;if abs(aO(j,1)+jO(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 Code86R(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 calledfrom 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 Code87[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)*R1(1,:),time,C2D(1)*theta(1 ,:),time,C2D(2)*R1(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 ofMatlabSimulation Source Code88VCMD=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 RungeKutta 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 SimulationSource Code89time(tim)=tO;for t=1:tn,ki=h*rdyn(yl(tim,:)t,n);k2=h*rdyn((y1 (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 ContainingDifferential Equations of VelocityLoopFilename: 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 SinudationSource Code 90A3.3.2 Function Containing ErrorChecking 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 Code91R 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)*R1 s( 1,:),time,C2D(1)*theta( 1,:),times,C2D(2)*Rls(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)*R1(1,:),C2D(2)*R1 (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)*R1(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(’Filteroutput’),xlabel(’time’),ylabel(’Magnitude’);pause;Appendix 3 Listings ofMailabSimulation Source Code92% Plot command voltage vs timeplot(time( 1 :k- 1),Vcmd(1 ,:),time( 1:k- 1),Vcmd(2,:)),title(’CommandVoltage’);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 ofMatlabSimulation Source Code93Ts=.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(’Magnituderesponse’),..xlabel(’Frequency (Hz)’), ylabel(’Gain dB’),subplot(2 12),..semilogx(w/2/pi,phase), grid,title(’Phaseresponse’), 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 ControllerExperimentsThe forth code listed in this appendixwas used to calibrate thevelocity 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 Experiments96A4.2 Data Array Routines\ STORE CURRENT POSITION INTOAN 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+ \ STORER_LIMIT\ 156 WRY 1PICK !2+;-->\ FILL UP THE DATA ARRAYFILLPOS \ FILL THEARRAYPosPOSSIZE @ 0 DOSTO12+LOOP;-->\ WRITE POSITION ARRAY TOTHE 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 ControllerExperiments 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 Experiments98A4.4 Corner Tracking Using Lower Level Commands\ TESTRUN REVERSAL USING STUFFX/STUUFYVARIABLE RVELVARIABLE MSRM1 DECIMAL5N!CALC_T2AN500.E04.0E3F*6.OE1 Fl FDUPT2ANF@F*F>S RVEL!2.OE6F/T2ANF@Fl6.OEOF*F>S DUP MS!S>F20.OEOF*K2F!-->\2A3SLAVE SAMPLES\ CALCULATE MASTER SAMPLING TIME\ SET A VELOCiTY OF 500 RPM\ CORESPONDING BLU/SEC\VEL=BLU/SEC*T2’N=BLUfMAST.SAMP.\ MS=MASTER SAMPLES IN1 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 Experiments99A4.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* T2ANF@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 CodeforController Experiments101\ TRANSFER RESULTS FROM TABLES TO FILEFR FRi FR2 FR3 FR4SHELL” DEL FREQRESP”>FILE FREQRESP\ OPEN NEW FILEPRES\ WRiTE DATACONSOLE\ CLOSE FILESHELL” COPY FREQRESPB:FREQRESP.MAT”;

Cite

Citation Scheme:

        

Citations by CSL (citeproc-js)

Usage Statistics

Share

Embed

Customize your widget with the following options, then copy and paste the code below into the HTML of your page to embed this item in your website.
                        
                            <div id="ubcOpenCollectionsWidgetDisplay">
                            <script id="ubcOpenCollectionsWidget"
                            src="{[{embed.src}]}"
                            data-item="{[{embed.item}]}"
                            data-collection="{[{embed.collection}]}"
                            data-metadata="{[{embed.showMetadata}]}"
                            data-width="{[{embed.width}]}"
                            async >
                            </script>
                            </div>
                        
                    
IIIF logo Our image viewer uses the IIIF 2.0 standard. To load this item in other compatible viewers, use this url:
http://iiif.library.ubc.ca/presentation/dsp.831.1-0080894/manifest

Comment

Related Items