CA1279882C - Multiaxis robot control having capability for executing timed moves - Google Patents

Abstract

ABSTRACT OF THE DISCLOSURE
A digital robot control is provided with digital position, velocity and torque control looping to operate joint motors associated with multiple axes of the robot.
Motion programming includes planning and trajectory pro-grams for generating position commands for the control looping on the basis of acceleration, slew and deceleration values that specify robot program specifications for timed moves.

Description

1~79~

The present invention relates to robots and more particularly to robot controls that are capable of controlling the time with which programmed moves are executed.
Related subjsct matter is found in the following copending Canadian applications of the applicant.
Serial Number Filinq Date 551,818 November 13, 1987 550,685 October 30, 1987 550,686 October 30, 1987 550,949 November 3, 1987 550,154 October 23, 1987 550,155 October 23, 1987 569,043 June 9, 1988 551,820 Novsmber 13, 1987 551,807 November 13, 1987 569,034 June 9, 1988 550,684 October 30, 1987 550,683 October 30, 1987 552,260 November 19, 1987 551,817 November 13, 1987 550,687 October 30, 1987 550,688 October 30, 1987 550,g48 November 3, 1987 In the operation of robots, end effector moves are normally executed with programmed acceleration, slew and dsceleration subject to limiting robot parameters.
Accordingly, the time required for robot arm movement over rn/~ `

~ \ ~

each path segment or over an entire path normally is the time required to make the move with the programmed parameters.
In various system applications, robot operation must be time coordinated with the operation of other items of equipment. To satisfy this requirement, programmed robot - moves may have to be executed with a prescribed or desired cycle time or with a time that is less than a prescribed limit value.
While acceleration, slew and deceleration values can be selected during robot program generation in effect to set the time with which robot moves are to be made, this is a somewhat cumbersome timing control process especially where a number of timed moves are needed in a robot program. For greater convenience, it is desirable that a robot control be structured to implement automatically timed moves for which time specifications have been provided in the robot program.
In the referenced patent applications, there is disclosed a new completely digital multiaxis robot control which facilitates the achievement of timing control over robot motion. The present invention is set forth herein as embodied in that digital robot control and it is directed rn/~
6`;~

-` 3 ~ ~98~3~
~ 53,490 to multiaxis robots for which motion timing control is integrated into the robot motion control system.
SUMMARY OF THE INVENTION
A digital control for a robot having a plurality of arm joints includes an electric motor for driving each of the robot arm joints. A power amplifier operates to supply drive current to each joint motor.
Feedback control loop means are provided for each joint motor and include digital position and velocity control loops operable at a predetermined sampling rate to control the associated power amplifier. Digital control means generate position commands for the feedback control loops in accordance with predefined moves set forth in a robot program. The position command generating means includes pla,nning program means for generating a motion profile including acceleration, slew and deceleration segments for implementing each robot program motion command in accordance with specified time for acceleration, slew and deceleration.
Trajectory program means generate trajectory position commands for the feedback loops in accordance with the computed acceleration, velocity and deceleration values for the motion profile applicable to the current move segment.
BRIEF DESCRIPTION OF THE DRAWINGS
Figure 1 shows a perspective view of a robot which is operated with more advanced and more accurate performance when controlled by a system making use of the invention;
Figure 2 shows a generalized block diagram of a control loop arrangement employing position, velocity and torque control loops in which the invention may be embodied;
Figure 3 shows a more detailed block diagram of a control loop arrangement employing position, velocity and torque control loops in which the invention preferably is embodied;

~ 53,490 Figure 4 shows an overview of an arrangement of electronic boards on which circuitry is arranged to imple-ment the robot control system including the path timing control of the present invention;
Figure 5 shows a block diagram of a robot motion timing control system employed to implement motion time specifications in accordance with the invention;
Fig~re 6A shows a block diagram illustrating the generation o a robot program;
Figure 6B is a graph showing a typical velocity profile for a segment of a programmed move;
Figures 7A, 7B-1 and 7B-2 show general flow charts for system motion software employed in the system of Figure 5 to implement the invention;
Eigure 8 shows the manner in which the motion software, i.e.~ a path planning program, can be structured to implement path timing specifications;
Figures 9A1-2, lOA1-2 and 11 show respective block diagrams for servo control, torque processor and arm - 20 interface boards employed in the system of Figure S;
Figures 12A, 12B and 13 show bridge configuration for brushless and brush-type DC joint motors; and Figure 14 shows a block diagram for a PWM circuit used on the AIF board to generate control signals for the joint motors.
DESCRIPTION OF THE PREFERRED EMBODIMENT
ROBOTS - GENERALLY
Robot capabilities generally range from simple repetitive point~to-point motions to complex motions that are computer controlled and sequenced as part of an inte-grated manufacturing system. In factory applications, robots can perform a wide variety of tasks in various manufacturing applications including: die casting, spot welding, arc welding, investment casting, forging, press working, spray painting, plastic molding, machine tool loading, heat treatment, metal deburring, palletizing, brick manufacturing, glass manufacturing, etc. For more ~ ~27~ 53,490 complete consideration of robots and their uses, reference is made to a book entitled "Robotics In Practice" published in 1~80 by Joseph F. Engelberger.
To perform work within its sphere of influence, a robot typically is provided with an arm, a wrist subassam-bly and an end effector. The coordinate system employed for the robot arm typically is Cartesian, cylindrical, polar or revolute. Generally, three motion axes are employed to deliver the wrist subassembly anywhere within the sphere of influence and three additional motion axes are employed for universal orientation of the end effector.
A drive system is used for each motion axis, and it may be electrical, hydraulic or pneumatic.
PUMA ROBOT
More particularly, there is shown in Figure 1 a six-axis industrial electric robot 20 which is illustrative of a wide variety of robots that can be operated in accor-dance with the principles of the invention. The robot 20 is a relatively powerful electric robot sold by Unimation Company, a wholly-owned company of the present assignee, under the trade name UNIMATE PUMA SERIES 700. The Model 761 PUMA has a 22 pound payload capacity and a reach of 59.1 inches. The Model 762 PUMA has a 44 pound payload capacity and a reach of 49.2 inches.
PUMA 700 Series robots are designed with flexi-bility and durability to ensure long life and optimum performance in even the harshest, most demanding manufac-turing environments. Specific customer needs for either higher payload or extended reach determine which model is suitable for a particular task.
With its longer reach, the PU~ 761 is ideally suited for precise, repetitive tasks such as arc welding and sealant dispensing. The PUMA 762 performs high-precision material handling, machine loading, inspection, testing, joining and assembly in medium and heavier weight applications. The PUMA robots occupy minimal floor space, 9!3~3 yet a large work envelope allows the robots to service multiple machines and work surfaces.
Each axis motion is generated by a brush type DC
electric motor, with axis position feedback generated by incremental e~coders. As shown, the wrist is provided with three articulations, i.e., an up/down rotation indicated by arrow 21 and a left/right rotation indicated by arrow 22 and a third motion indicated by arrow 23. Elbow and shoulder rotations in the up/down direction are respectively indicated by arrows 24 and 250 Finally, a left/right arm rotation on a base 27 is indicated by arrow 26.
ROBOT CONTROL
The present invention is directed to a robot control 30 (Figures 2, 3 or 4) which can operate the robot 20 of Figure l and other robots including the larger UnimationTM 860 robot which employs brushless DC axis motors and absolute position feedback. Generally, however, the robot control 30 is universally and flexibly applicable to differing kinds and sizes of robots in stand alone or robotic network operation.
As a result of its universality, the control 30 can be arranged to operate a complete family of robots. Thus, all hydraulically and electrically driven robot arms manufactured by Unimation, a company of Westinghouse can be operated by the control 30. The key to the family usage, or more generally the universality of the control 30 lies in modularization and in minimizing the use of arm dependent hardware and avoiding the use of any arm dependent hardware in as much of the modular control structure as possible. The robot control 30 is identified by the acronym UNIVAL and operates with completely digital servo control to provide better robot performance with lower cost.
CONTROL LOOPS
In Figure 2, there is shown an embodiment of a generalized control loop configuration lOO employable in the UNIVALTM robot control. Thus, each robot arm joint motor rn/

7 ~ 88~ 53,~90 102 is operated by a torque control loop 104. An outer position control loop 106 is tandem connected to a velocity control loop 108 which in turn drives the torque control loop 104. A feedforward acceleration control loop 110 is responsive to acceleration command 112 and arm and load inertia 114 is also directly coupled to the input of the torque control loop 104. The robot arm is operated by the control loop 100 in accordance with a robot program through a stream of program position commands 116 applied to the position control loop.
Figure 3 shows the preferred generalized control loop configuration 118 presently employed in the UNIVAL
robot control. It is preferably implemented as a complete-ly digital control. With the provision of hierarchical architecture and multiprocessor architecture and flcating point hardware as described herein or in other patent applications referenced above, the trajectory cycle can be characterized with a cycle time in the range of 32 to 8 milliseconds depending on the employed modular configuration.
In the preferred control loop arrangement 118, position control loop 122 and velocity control loop 120 are paral~
lel fed to the input of a torque control loop 124. Veloci-ty commands are generated by block 126 from position commands received by block 128. In turn, feedforward acceleration commands are generated by block 130 from the velocity commands. Computed inertia (load and arm) 132 is multiplied against the acceleration command as indicated by reference character 134 in the feedforward acceleration control loop 136.
In the velocity loop 120, the velocity command in the present embodiment is generated once every 8 to 32 milliseconds depending on the modular configuration of the robot control. The basic robot control described subse-quently herein has a trajectory cycle time of 32 millisec-onds while the enhanced contact has a trajectory cycle of 8 milliseconds.

ç, ,~
~ ,.

~ ~9~
. 8 53,490 In any case, a velocity command generator 138 interpolates velocity commands at the rate of 1 each millisecond which corresponds with the velocity feedback samplin~ rate in velocity feedback path 140. As shown, velocity feedback for a Unimation 860 robot is produced by tachometer signals which are converted from analog to digital by converter 142. A scaler 144 and a filter 1~6 supplement the velocity feedback circuitry.
Simil~rly, in the position control loop 122, an interpolator 148 generates position commands every milli~
second in correspondence with the position feedback sam~
pling rate in feedback path 150. In the Unimation 860 robot control, position feedback is absolute and the velocity and position feedback paths l40 and 150 operate as just described (with switch 151 as shown). For Unimation PUMA robots, tachometers are not available and velocity feedback is computed from incremental position feedback as indicated by block 152 (with the switch 151 swinging to its other position) as described more fully in referenced 20 Serial Nos. 550,685 and 550,155.
Velocity error is generated by summer 154 with ~ain applied by Loop 156. Similarly, position error is generated by summer 158 with gain applied by box 160.
Velocity and position errors and feedforward acceleration command are summed in summer 16Z. Gain is ~: applied in box 166 to generate a torque command which is applied to the input of torque control loop 16a every millisecond. Torque error is generated in summer 168 by summing the torque command (motor curre~t command) with 30 current feedback from feedback path 170. Box 172 applies a torque loop gain to the torque error and pulse width modulated (PWM) output commands tmotor voltage commands) are applied to a power amplifier 174 which supplies the motor drive current for robot joint operation. Current feedback 35 from resistor 175 is generated every 250 microseconds (see referenced Serial No. 551,818) and converted to digital signals by box 176 with scaling applied by box 178.

9 53,490 OVERVIEW - ELECTRONIC BOARDS
Implementation of the control looping for the robot control 30 in Figure 4 is achieved by the use o~ digital control circultry disposed on a plurality of electronic boards.
The organization of the circuitry on the boards and the partitioning of programming among various microprocessors enables advanced robot control performance to be achieved - with a modular control configuration characterized with economy of manufacture, facilitates variability of configu-ration which enables universality of use, and flexibility in choice of level of control performance.
As shown in Figure 4, the control board configu-ration includes an arm interface board 800 which preferably houses all circuitry dependent on the type of robot arm being control,led. For example, position feedback circuitry will differ according to whether absolute or incremental position feedback is used by the robot arm to be con-trolled. Thus, two or possibly more varieties of the arm interface board 800 can be employed to provide digital control systems for any of a variety of different sizes or types of robot arms. Any particular robot arm would require use o the arm interface board which is structured to work with that robot arm.
The arm interface (AIF) board 800 also houses generic circuitry such as VME bus control circuitry which is generally related to two or more boards and not to any one board in particular.
Control signals (pulse width modulated~ are generated fr.cm the AIF board 800 to control power amplifier blocks 150 which supply motor currents to the robot joint motors.
The AIF board 800 also operates as a channel for external coupling of the robot control 30 to other robot controls in a work cell as indicated by the reference character 152, to programmable controllers and other input/output devices 153 in an area network and to higher level computers 154 for supervisory control.

~ r ~'' ' ~ 7~ 53,~90 A torque processor (TP) board 600 and a servo control board 400 are generic circuit boards used with the AIF board 800 and power ampllfier blocks 150 in all robot control systems for all robot types. The three circult boards 400, 600 and 800 provide complete 6 axis control for a robot arm and thus form a basic control configuration for the UNIVAL family of robot controls as well as other robot controls.
The torque processor board 600 provides motor torque control in response to commands from the servo control board 400. In turn, the servo control board 400 provides arm solutions and position and velocity control in accordance with a robot control program.
Extended control capability and/or system func-tioning is achieved by interconnecting additional electron-ic boards or devices to the basic control 400, 600, 800.
For example, with the addition of a system control board 500 and partitioning of predetermined progra~ functions including the arm solutions from the servo control board 400 to the system control board 500, the UNIVAL control can operate the robot 20 and other robots with significantly faster control action, i.e., with a trajectory cycle shortened from thirty-two milliseconds to eight milliseconds.
Interboard data communications for control and other purposes occur over multiple signal paths in a VME
bus 155. Additionally, a V~X bus 156 is provided for connection between the torque processor board 600 and the AIF board BOO.
Multiple pin interconnectors (not shown in Figure 4) are provided on the AIF, TP and SCM boards and any other connectable units to facilitate VME and VMX interboard bus connections modular and board assembly for the robot control 30. Other connectors are provided on the AIF board 800 for external input/output connections.

~ 8~ 53,~90 More detail on the board circuit structure is presented herein or elsewhere in the writeups for the cross-referenced patent applications.
ROBOT MOTION TIMING CONTROL SYSTEM
As shown in Figure 6A, a program editor 413C is employed to generate a robot control program 202T which in - Figure 5 is then placed in storage in a robot controller to operate a robot 204T (see Figure 5). The program editor is arranged to enable the robot user to write a robot program that specifies the robot moves, parameters, tasks, etc. In ~he case of the Unimation robots to which the specific embodiment herein~relates, a programming language called Unival (abbreviated VAL) is employed to write robot programs. To make use of the present invention, the program writer enters time, specifications for robot moves into the robot program 202T as indicated by box 203T. The time to which a robot move is to be con-trolled may be a particular time which is to occur for the move to be completed or it may be a time limit, such as a maximum ~ime which is not to be exceeded in completing the move.
With reference now to Figure 5, the robot control is shown in a basic configuration, i.e. one in which the SCM board 400 is the highest level electronic control board and thus is employed to store the robot program 202T and to execute system motion software 206T that generates arm solutions from the robot program 202T for execution by the robot control. As shown, the robot control embraces a system 205T for implementing robot motion timing control.
The system 205T includes various elements of the robot control including various elements of the electronic SCM, TP and AIF boards.
The system motion software 206T generally in-cludes a path planning program 207T and a trajectory interpolation program 209T that are stored on the SCM board 400 in the basic robot control configuration. These programs plan and interpolate the actual robot motion so as s "
,~, . .
-` ~2 ~i9~
to embody the time specifications 203T entered for robot moves in the robot program.
In an expanded performance robot control configuration, a system control board 200T (shown dotted) is included in the robot control (reference is made to Serial No.
551,820 for additional description of the expanded control).
Generally, in the expanded control the robot program 202T
is stored on the system control board and microprocessor circuitry on that board implements the motion software.
lQ Arm solutions are thus generated and position commands for all of the axes are then transmitted to the SCM board for implementation.
In executing the arm solutions in the basic control, position commands for each axis produced by the motion software 206T on the SCM board 400 are acted upon by a position and velocity control on the SCM board 400 for that axis, and resultant torque commands are applied to the torque processor board 600. In turn, a torque control on the TP board 600 for each axis generates voltage commands that operate a pulse width modulator (PWM) 208T on the AIF
board 800.
Generally, the SCM board 400 includes a micropro-cessor called a servo control manager that executes the system motion software in conjunction with a calculator 2S coprocessor. The servo control manager thus develops the position command signals for all of the axes. Another microprocessor on the SCM board 400 acts on the position commands in the position/velocity control looping to generate the torque commands for the TP board 600.
Finally, digital signals from the PWM 208T
operate power switches 210T to control the on/off time of the switches and thereby control the effective voltage applied across each joint motor. Accordingly, motor current and torque are determined. Current and position/
velocity ~eedback signals are returned to the torque and position/velocity controls to place these parameters under feedback control operation.

~i 98~
~3 53,~90 The robot arm joints are thus moved coordinately so that the robot tool tip goes through the commanded motion with the entered time specifications.
SYSTEM MOTION SOFTWARE
5kobot position commands are generated by motion software at the system level. In the basic robot control, system motion software is resident on the SCM board 400.
As shown in Figure 6~,the system motion software includes a path planning program 410C and a trajectory interpolation program 412C. A robot program 414C prepared ~y the user, in this case preferably with use of a program editor with the present assignee manufacturer's programming language called VAL, specifies the robot destination points along its programmed path as well as certain other reyuests and specifications regarding robot operation. In effect, the planning and trajectory programs operate at the system level to process robot program outputs so as to enable robot controller execution of the robot program.
Thus, where a complicated path has been pro-grammed, the robot program normally includes additional intermediate path points. Additionally, the robot program specifies tool tip speed, acceleration and deceleration as a percentage of maxamum, and the type of path control, i.e., continuous or point-to-point.
25The planning and trajectory programs can be installed on internal UNIVAL board memory, preferably EPROM, or it may be loaded when placed in us~ into general board memory from floppy disk or other storage means. The user robot program is loaded into UNIVAL board memory at the time of use.
In the case of the basic UNIVAL robot control, system motion software is resident on the SCM board 400.
In expanded versions of the UNIVAL robot control, the system motion software is resident on the system control board. Of course, other variations are possible.
The planning program 410C runs on a demand basis, i.e., when a new destination point is received from the . ~ ~

,.~ ..

14 ~ 8~ S3,~90 robot program. The trajectory program 412C runs cyclicall~
at the system cycle rate, i.e., at the rate of once each 32 or 16 or 8 milliseconds in the preferred embodiment depend-ing on the conîiguration of the UNIVAL robot control system as explained elsewhere herein or in the referenced patent applications.
1. PLANNING PROGRAM
Basically, planning is performed by the robot control to define how the robot tool tip is to move from its present position to its commanded destination. Thus, the planning program generates a time profile for accelera-tion, slew and deceleration for successive segments of motion defined by the robot program.
As shown in Figure 7A, the planning program 410C
determines t~e type of move to be made for each segment and then computes the segment time profile in accordance with the type o move.
Thus, block 414A determines whether a Cartesian move in the form of a curved path has been specified. If so, box 416A computes the distance to be traveled along the path using spline fit e~ua~ions (up to third order polyno-mials in the present embodiment) and~or circular arc equations. Block 418A then computes the acceleration, slew and deceleration times for the tool tip in Cartesian space making use of selected acceleration and deceleration profiles (squarewave, sinusoidal wave in the present embodiment or table of values or other profiles in other embodiments).
Generally, the software architecture of the present embodiment has flexibility to accommodate a wide variety of acceleration/deceleration profiles according to a user's needs.
The following equations are used to compute the times Ta, Ts and Td:
Calculation of ta, tS, td for normal Cartesian move:

., ~ ,.

~ 3 ~ 9 ~ 53 490 t - velocity factor * max velocity a accel factor * accel_max t velocity factor * velocity max d decel_factor * decel_max da ~ ta ~ velocity_factor * max_velocity dd = td * velocity_factor * max_velocity User specified through UNIVAL:
velocity_factor = 0-1 normalized scaling to max_velocity;
accel_factor = 0-l normalized scallng to max_accel;
decel_factor = 0-1 normalized scaling to max_decel.
Test for "short" move or "long" move:
If dto~al ~ (da + dd) then "short" move otherwise "long" move.
Short move:
ta ta ldtotal/(da + dd) ]75 td = td Idtotal/(da + dd)]
tS =
Long move:
`~ ts = (dtotal ~ da ~ dd)/(VelCitY_factory x max-velocity`~ - ta = acceleration time : td = deceleration time 25 da = distance travelled during acceleration dd = distance travelled during deceleration dtotal = total distance to be travelled ts = slew time (constant velocity) 16 ~-Lx7988~
Reference is made to Serial No. 552,260 for more disclosure on the planning feature that employs acceleration/deceleration profile selection.
If a curved path move has not been directed, block ~20A detects whether a Cartesian move in the form of a straight line is specified. In a straight line move, the tip of the tool moves along a straight line in Cartesian space and thus moves across the shortest distance between its present location and the destination location. A
straight line move may not be the fastest move between two points since a joint move may be faster. The nature of the user application determines whether and when straight line moves are needed.
As in the case of a curved path move, the dis-tance to be traveled in Cartesian space is computed when astraight line move has been directed. Thus, block 422A
computes the straight line distance with use of the indi-cated formula.
A joint move is employed when the user wants the tool tip to be moved from one point to another in the shortes~ time. However, joint moves are avoided if obsta-cles exist in the field of possible motion. In a joint move, all of the joints are moved in coordination to produce the fastest tool tip move regardless of the path of the tool tip.
Box 424A determines whether a joint move is to be executed. If so, bloc~ 4Z6A computes the endpoint joint angles for all axes for the joint move. Next, the joint distance is computed in joint space by taking the differ-ence between the destination and present joint angles foreach axis. The joint move for each joint is accordingly established.
Similarly, block 426A computes the joint distanc-es in joint space for curved path and straight line moves.
Block 428A then computes the acceleration, slew and decel-eration times in joint space from the joint distances using 17 ~ 388~
the selected acceleration/deceleration profiles, l.e. the time profile for each joint motion is basically determined.
This computation is performed as a check on block 418A in the case of Cartesian moves (curved or straight line) since it is possible for individual joint motions to be outside prescribed limits even though the planning tool tip motion is within prescribed limits.
Box 430A accordingly determines what limits apply to jolnt acceleration, slew and deceleration times to modify the time profiles in accordance with joint limits if necessary. The longest limit time for any particular joint is set as a limit time for all of the joints, i.e. for the move as a whole. The acceleration/deceleration (torque) and velocity capabilities of the robot being controlled are used in setting time limits.
If box 432A determines that continuous path operation has been directed by the user, box 434A computes ! the continuous path coefficients to be used for smooth merging of the present motion segment with the upcoming motion segment. In smoothing the transition between the slews of successive motion segments, box 434A essentially eliminates unnecessary deceleration/acceleration in chang-ing from one velocity to another. Reference is made to Serial No. 551,817 for more disclosure on the operation of the 2~ continuous path ~eature of the UNIVAL robot control.
Once continuous path smoothing calculations have been completed or if continuous path smoothing has not been required, block 436A ends execution of the planning program.

3 a 2. TRAJECTORY PROGRAM
The trajectory program 412A is executed during each system cycle to generate joint position commands for the next system cycle. Generally, the trajectory program 412A computes for each upcoming system cycle the accumu-lated distance to be traveled for each joint when the upcoming cycle is completed. A factor referred to as "S"
is computed in the distance calculation. Thus, S is the 79~2 ~ 53,490 accumulated distance (S) as a percentage of the total distance to be traveled in terms of a normalized path length (0-1).
A new set of interpolated endpoints are generated to provide position commands for the robot axes for execu-tion during the upcoming system cycle, i.e. the total user specified segment move is subdivided into cycle moves to be executed in successive system cycles (which may in this case have a length of 32, or 16 or 8 milliseconds).
The new position commands are based on the S
calculation and the type of move being performed. In implementing the position commands, the servo control provides further real time interpolation by dividing each system cycle into millisecond intervals during which moves of equal distance are executed for each joint.
As shown in the flow chart of Figure 7B-l, block 440A first makes the distance computation for the upcoming system cycle, i.e. S is calculated. In boxes 442A, the type of move is determined and the interpolation calcula tions for that type move are executed.
Specifically, block 444A determines whether a prescribed path move has been directe~, and if so box 446A
calculates the Cartesian X, Y, Z, 0, A, and T interpola-tions using the S distance factor, spline fit equations (up to 3rd order polynomials) and circular arc equations. The 0, A, and T coordinates represent the orientation of the tool tip in angles.
Generally, in making interpolation calculations for the various types of path moves, the S factor operates as a percentage multiplier in computing for each axis the fraction of the total commanded move to be completed in the upcoming system cycle.
aox 448A determines whether a continuous path move has been directed, and if so box 45~A calculates the X, Y, ~, 0, A, and T interpolations using the S distance factor and stored continuous path coefficients. ~he latter are employed in equations that are used to produce smooth ~.~7~1882 transitioning between different slew values in successive path segments. Reference is made to Serial No. 551,817 where further information is provided on the continuous path mode of operation.
In block 452A, a determination is made as to whether a straight line move has been directed. If so, box 454A makes the interpolation calculations for X, Y, Z, 0, A
and T using the S distance factor and straight line equations.
If a joint move is detected in box 456A, block 458A makes interpolation calculations for all of ,he joint angles using the S distance factor. Box 460A converts Cartesian interpolations to joint angles for the case of a prescribed path move, a continuous path move or a straight line move. The conversion to joint angles represents the arm solution and involves knowledge of kinematics of the robot arm, specifically the lengths of the various links and the angles between axes of rotation.
Finally, block 462A converts the interpolated joint angle commands to respective encoder counts opexable as a set of position commands applied to the various robot axes by the servo controller 464A. The encoder count conversion reflects the resolution of the encoders, gear ratios and any mechanical coupling between joints (usually only wrist joints are coupled).
The S calculation is shown in greater detail in the flow chart in Figure 7B-2. Block 441A first inc~ements the system cycle counter. Blocks 445A then the current cycle count to the time profile computed in the planning program for the current path segment. In this manner, the segment por~ion (acceleration, slew, deceleralion) in ~hich the tool tip is currently located is determined. The applicable acceleration, slew or deceleration value is accordingly identified for the S computation.
If box 445A detects that the cycle count exceeds the segment time, S is set equal to 1 by block 447A. If the acceleration segment portion is detected by box 445A, ~. ~
~ c~r~ 82 53,4~

block 451A uses acceleration equations to compute S.
Similarly, if block 453A detects the slew segment por~ion, box 455A uses slew equations to compute S.
Block 457A employs deceleration equations to compute S if the acceleration, slew and segment terminated ~locks 445A, 449A, and 453A are negated. The S calculation routine is then completed and trajectory program execution returns to block 444A.
Tha equations employed in making the S calcula-tion as follows:
Acceleration Equations - square wave s = t /(t * (2 * t + t + td) - sinusoidal ta ~[(t/t ) - (sin (180 * (ta/t)))/pi profile s - [ta + 2 * tS + td ]

Deceleration Equation_ - square wave s = ((2 : t + t ~ t ) - ((tt-t):~2/td))/(2 ~:t + t + td) . . t + t + (td/pi) * [sin (180 * y/t )]
slnusoldal s ~ 5 ~.
; profile (ta + 2 * tS + td) where: y = (t - tt + td) .

Slew Equation s = (2 * t - ta)/(2 * tS + ta + td) -~

~ 798~ 53,4~0 ta = acceleration time td = deceleration time tS = sle~ time (constant velocity) t = current time 5 tt = (t ~ t + td ~ t) pi = 3.1417 PATH TIMING CONTROL
In planning the timing of path moves, the plan-ning program is arranged as shown in more detail in Figure 8.
After program entry, block 411T determines whether a timed move is specified by the robot program. If not, and if block 412T indicates a Cartesian move, Car-tesian path lengths are calculated in block 413T for curved path and straight line msves and acceleration time Ta, deceleration time Ta and slew time Ts are calculated in block 415T as previously described for blocks 414A, 420A, etc. If the move is a joint move as indicated by the block 424A, the joint distance is calculated in block 426A as previously described for these blocks.
If a timed move is detected~ by the block 411T, block 417T calculates velocity, acceleration and decelera-- tion from user specified values of Ta, Td and Ts.
Calculation of accel, decel, slew vel. for time - 25 Cartesian move:
User specified ta, td~ tS for the move ~\
` 22 ~2798~ 53,~?0 V = dtotal ta f td + tS

V
Accel = ts a V

Decel = s Slew Velocity = Vs Parform check on accel, decel, slew velocity that they don't exceed known physical limits of the joint motors.
ta = time to accelerate td = time to decelerate tS,- time at contstant velocity (slew time) Vs = velocity in ~'s" (pathlengths) domain Thereafter, the block 426A calculates the joint distances and program execution continues as previously described. Upon execution o the planned path, the robot arm traver~es the path in the specified path time. If planning block determines that robot limits will be exceed-ed, the move time is modified to produce a motion profile (accel, slew and decel) that is within robot limits.
FURTHER DESCRIPTION OF BROAD IMPLEMENTATION CIRCUITRY
SERVO CONTROL BOARD
A servo control module (SCM) or board 400 (Fig-ures 4 and 9A1-2) is structured in accon~ce with the modular . architecture of the robot control system to operate as a core board for a complete basic robot control and generate arm solutions from stored robot program commancls or to operate as part of an expanded robot control ancl receive for implementation arm solutions produced from robot program commands by the higher level system control board ~ a41~.,.~c :. ,.

7 ~ 53,~90 350. The generation of arm solutions involves the execu-tion of robot control functions including robot program language interpretation, path planning, trajectory calcula-tions (intermediate position commands and axis coordina-tion) and transformation of position information betweenCartesian and robot joint and robot tool coordinate sys-tems. The SCM board 400 additionally provides communica-tions interfacing with related peripherals and a host controller if provided.
The SCM board 400 is provided with program controlled digital circuitry to implement arm motion control loops for the robot control system. Motion control is achieved for each axis through a control loop arrange-ment which preferably includes interrelated position, velocity, and acceleration control loops from which torque commands are developed for implementation by the torque processor module 600. The digital servo control is a coordinated multiprocessor servo control that generates output torque commands from (1) position and velocity commands provided for each axis by the arm solution and (2) position and velocity feedback signals obtained from the position encoders and the tachometers through the arm interface module 800. .
In the SCM control loop operation, a position error is calculated for each axis from the applied axis position command and the axis position feedback. A veloci-ty error is calculated for each axis from a velocity command derived from successive position commands and from the axis velocity feedback. Preferably, the position and velocity control loops are operated in parallel, i.e., the position and velocity errors are summed to produce a torque command for the torque control loop on the tor~ue control module 600. Additionally, an acceleration command prefera-bly is derived from successive velocity commands and applied in a feedforward acceleration control loop which 24 ~ 53,~g0 generates an acceleration based torque command for summa-tion with the position and velocity errors in generating the SCM output tor~ue command.
The frequency with which loop calculations are made is selected to produce robot arm motion which is fast, accurate, smooth and stable. For example, the frequency employed can be such as to provide a trajectory cycle of 32 milliseconds as in the present case. If desired, a faster trajectory cycle, i.e., as short as 8 milliseconds, can be achieved.
SCM DIGITAL CIRCUITRY
As observed in Figures 9Al--2, the SCM board 400 generally comprises two sections, i.e.,'a local processor section 401 and a system resource section 403. The system 15 resource section 403 employs a bus 405 and provides func-tions related to the overall robot control system and not specifically xelated to execution of the position and velocity control loops.
These functions include EPROM 408 for storage of the robot arm solutions, battery backed-up RAM 410 for storage of non-volatile data, static RAM 412, real-time clock 415, a DMA
controller 414 and two multi-protocol, dual channel - communications controllers 416 and 418.
The system resource area is implemented as dual-port memory. As such, equal access to the system resource section is provided from either a local processor 401 or from the VME bus 420. The system resource functions appear as a bus slave to the VME bus. This provides the capabili-ty for these related functions to be controlled either from the SCM local processor, or from an optional processor connected to the system bus.
In the local processor section 401, the SCM
digital circuitry includes coordinated digital coprocessors and interface and resou~ce circuitry needed for specified performance, i.e., to provide control calculations and control data management needed for accurate and efficient ~~`~ 25 ~ 9~82 53 490 control of all axes and to provide interfacing communica-tion with a host controller, peripheral devices and other robot controllers. Preferably, a servo control manager 402 operates with a servo calculator 404 which functions as a slave processor principally to make position and velocity control loop calculations (i.e., feedback filters, loop gains, position and velocity errors, etc.).
The servo control manager 402 directs control, status and program data to and from the SCM board 400 and to and from the servo position/velocity control calculator 404. The servo control manager 402 can be a Motorola 68000 which has a high data processing capability. By separating data management and control calculation tasks in accordance with the respective capabilities of the processors 402 and 404, a basic circuit organization is provided as a basis for achieving substantially improved control performance with manufacturing and user economy.
In the illustrated embodiment, implementation of the local processor section of the SCM board 400 is based on usage of a 68000 processor as the servo control manager 402 and two coprocessors. Both coprocessors serve as peripheral devices to the 68000. One of the coprocessors 406 (preferably National Semiconductor 32081), provides 10ating-point calculation capability when arm solutions are to be provided by the SCM board 400. The other co-processor, or slave processor, is the position/velocity servo calculator 404 and is implemented with a Texas Instruments TMS-32010 Digital Signal Processor. The position/velocity processor provides high speed fixed point calculation capability.
The remaining functions which are a part of the local processor section include local memory, both EPROM
422 and RAM 424, a peripheral timer/counter device, interrupt control 430, and system error monitoring devices 428. The local processor 402 can serve as a master to the VME bus for access to the TPM or other related type functions. Howev-er, the SCM board 400 does not provide VME bus system 88~ ' controller type functions which normally include system reset generation, bus arbitration for access to the bus and system bus clock generation, since these functions are implemented on the arm interface board 800.
The SCM board 400 is arranged to provide as much systems flexibility as is reasonably possible, and to obtain the maximum performance from available large scale integrated (LSI) circuitry. This is one of the reasons that the DMA and communications facilities are implemented in the system resource area as opposed to being directly connected to the local processor bus. This architecture ; not only frees the servo control manager 400 from direct intervention in communications data movement, it also eliminates the local processor bus communications related overhead, thus allowing high speed serial communications to be conducted without significant impact on program execu-tion time in the servo control manager 400. Also, by placing these functions in the system resource area, these facilities can ~e operated by any other optional processor ~ith capability of serving as a VME bus master. This would then totally free the servo control manager 400 from communications related processing. This organization allows the complete functionality re~uired for a robot control system to be implemented in a cost effective manner and on a minimal set of boards while also allowing in-creased performance controllers to be implemented without impacting the overall system design.
Another significant area is the interface between the servo control manager 402 and the servo calculator 404.
Here, a special dual port memory organization, referred to as "ping-pong" or "bank switched" memory allows either processor to communicate with the other without impacting the processing performance of either processor.
PROGRAMMED OPERATION OF SERVO CONTROL BOARD
The program system (not shown) for the servo .~control data manager 402 of Figure 9A-l comprises a background program called MAIN and a cyclically operated foreground interrupt a,~

27 ~79~ 53,490 routine called SERV0 as seen in greater detail in Fiqures 7B, 7C
and 7D of applicant's Serial No. 550,683~ l~en the system is started by RESEI', an initialization routine is executed prior to continuous running of the MAIN proyram. In addition to the cyclically executed SERV0 interrupt, an interrupt routine called C&UNEX operates in the foreground on demand to process unscheduled or unexpected interrupts. Further, a special highest priority routine called the watch dog timer inter-rupt functions in response to operation of the external watch dog hardware.
Where the robot control system includes the system control board 500 in Fiqure 4 for higher performance throuqh higher computing capacity, the MAIN program provides for receiving and distributing position commands from the 15 system control board 500~ In the minimum or basic robot control system configuration, the system control board 350 is not included and the MAIN program further performs arm solutions to generate position commands locally on the servo control board 400. Additional description on the minimum robot control is presented subsequently herein.
The rate at which the MAIN program is interrupted for the cyclical execution of the SERV0 routine is con-trolled by the signal VTICK generated once each millisecond on the VME bus 155 from the arm interface board 800. The basic functions provided by the SERV0 routine are:
1) transfer control data to and from the servo calculator 404;
2) transfer control data to and rom the torque processor board 600;
3) receive sensor feedback data over the VME
bus 155 from the arm interface board 800;

4) interface to the supporting background task RDMASC (in Figure 7D of applicant's Serial No. 550,683);

5) perform synchronous data logging;

6) perform one shot data logging;

7) place broadcast data in a blackboard storage area;

.....

28 ~ 9~8~ 53,490 8) shut the system down if serious error conditions occu~.
In the servo calculator ~02, tw~ basic functions are performed. First, downloaded position command data is interpolated for each of the 31 ticks between long ticks in the VALCYCLE, and velocity and acceleration command data are computed from the position command data for each tick.
Next, servo calculations are made for each axis after each tick for the position, velocity and acceleration commands then applicable and the concurrently received position and velocity feedback. As a result, a torque command is computed for each axis after every tick for execution by the torque processor board.
The SCM programming including the control algo-rithms executed by the servo calculator 404 are described ingreater detail in application Serial No, 550,6~3.
TOR~UE PROCESSOR BOARD CONCEPTS
The torque processor (TP) board 600 provides a functional interface to the robot joint drive motors.
Functionally, the TP board 800 implements the lowest level of control in the hierarchical control system, providing closed loop servo torque control for six robot axes.
Physically, the TP board 600 electrically interfaces the robot path planning control system and the servo control (SCM) board with the arm interface (AIF) board 800, which in turn interfaces to the robot joint drive motors. The primary function of the TP board 600 is to regulate robot joint motor currents to commanded values by generating motor winding voltage commands which are executed using a pulse width modulation scheme on the AIF board.
The TP board 600 interfaces at one level to the SCM board, accepts from the SCM board torque commands and servo parameters for six axes and returns status data. The TP board 600 interfaces at a second lower level to the AIF
board 800 providing servo voltage commands for the six ., c~' l ,, ..~..

29 ~ ~ 9 ~ 53,~90 robot axes. The AIF board 800 receives drive motor cur-rent, positlon and velocity feedback for closed loop control on the SCM and TP boards.
The TP board 600 employs the paired microproces-sor to provide a number of features including the following:
1. Torque loop control for six axes (250 micro sec per 6 axes) for brush and brushless ~otors;
2. Software adjustable current offset - eliminates potentiometers;
3. Downloadable gains - arm dapendent parameters can be downloaded from the SCM board;
4. PWM compensation;
5. Commutation compensation;
6. Current averaging for data logging and other purposes;
7. Current limit check;
8. Velocity monitoring (back emf) for safety check;

9. Energy check (IIT) to test stall condition;
lO. Power-up self diagnostics; and 11. Downloadable diagnostics system.
TORQUE PROCESSOR BOARD
More advanced robot performance is produced by digitally controlling the torque applied at the arm work-point whe~ the arm is in motion to control the arm work-point position in accordance with a command trajectory.
Axis drive forces are adjusted in accordance with actually experienced workpiece loading to satisfy position and trajectory commands with greater speed, accuracy and efficiency. Reference is made to 550,154 for a related invention directed to the control of-torque as an end controlled variable.
The torque co~trol is embodied on a generic control circuit board 600 (Figures 4 and lOA-l and lOA-2) called a torque . . .

-~ i 3~ 9~8~ 53 490 processor (TP) board i.e., an electronic board usable to provide torque control for a wide variety of robots having different load capacities, different types of drives, different numbers of axes, etc.
The torque processor board 600 employs digital circuitry to generate voltage commands for each joint motor or axis drive on the basis of torque commands obtained from a higher control level (SCM board) and feedback currents obtained through the arm interface (AIF) board 800 ~rom the axis drives. Thus, the torque control loops for all of the joint motors are closed through the TP board circuitry.
In the case of electric drives, the feedback current is the motor winding current which is proportional to actual motor torque. For hydraulic drives, the feedback current is also proportional to actual motor tor~ue.
The digital torque control circuitry is prefera-bly structured with multiple digital processors so that needed control computation and control support functions can be achieved for all axes accurately and efficiently within sampling frequency requirements.
~ In particular, a torque control manager 602 - interfaces with a dual port S~M interface memory 604 for the exchange of store~ torque control data between the SCM
(servo control module) and the TP (torque processor) control levels. Axis torque commands and control loop parameters are downloaded from the SCM to the TP interface memory 604 through a data bus 606 preferably of the VME
type. In return, status data is uploaded to the servo control level (SCM). The memory interface 604 between the TP and SCM boards is a dual port shared memory scheme which serves as a slave to the VME bus 606. Other board memories include a ping-pong memory 608, program EPROM, local RAM, and TP calculator memory.
The torque control manager 602 also directs the flow of current feedback from circuitry on the AIF board 800 at the next lower control level to the torque processor board 600 for torque control loop operation. Drive voltage ' `3/ ~z7~ 8~2 53,490 commands resulting from torque control calculations are directed to the arm interface (AIF) board 800 by the torque control manager 602. The ping-pong (bank switched) memory 608 operates under the control of handshake flags to store command, feedback, and status data so that it is available when needed for torque control calculations or for higher ! control level reporting requirements or for axis drive control.
A coprocessor 610 provided in the form of a digital signal processor operates as a tor~ue loop calcula-tor which receives torque commands and feedback currents from the tor~ue control manager 602 through the ping-pong memory 608, calculates drive voltage commands for the various robot axes from the torque errors computed from the lS torque commands and feedback currents, and transfers the drive voltaye commands through the ping-pong memory 608 to the arm interface circuitry on command from the torque control manager 602.
With the described digital circuit structure, all 20 needed torque control functions are able to be performed rapidly (250 microsecond sampling rate or better) and accurately within frequency bandwidth re~uirements.
Specifically, the rapid calculating capability of the digital signal processor 610 is employed for the torque control calculations as the data organizing and directing capability of the torque control manager 602 is e~ployed for most other functions thereby enabling highly improved control performance to be achieved efficiently and economically.
The torque control manager 602 has an architec-ture well suited for the tasks described for data manage-ment but which has a calculating speed (i.e., over 4 microseconds for a 16 x 16 bit multiplication) too limited to meet torque control bandwidth requirements. The digital signal processor-610 has an architecture set for Z trans-form calculations (i.e., a calculating speed of 200 nano-seconds for a 16 x 16 bit multiplication) but which is 32 ~ 88~ 53~490 otherwise generally unsuitable for the kinds of tasks assigned to the data manager processor 602. These two microprocessors function together as a unit or, in other terms, as a servo engine.
For more detail on the torque board circuitry, - reference is made to Serial No. 550,684 or 550,686.
TORQUE CONTROL PROGRAMMING
The tor~ue processor board 600 is operated under the control of programs executed i~ the on board processors 602 and 610 to implement torque command signals from the higher SCM control level.
The tor~ue processor software generally performs the following tasks which are partitioned as indicated:
Torque Control Manager 602 Co~munication wit SCM
Command handling Current sa~pling, conversation and offset adjustment Commutation switch flag ~state reading) Pin~-pong memory management PWM chip management Diagnostics Error repsrting Torque LooP Calculator 610 (program cycling based on 250 microsecond interrupt) Overcurrent check - absolute and average Torque loop calculations Current averaging PWM compensation Commutation compensation Back emf check - monitors velocity for safety Energy check - tests for stall conditions Reference is made to Serial No. 550,949 for more detail on TP software structure and operation.
ARM DRIVE CONTROL
As now further described, with reference to Figure 11, the higher level control looping generates voltage command signals to be executed through the AIF
board 800 for the arm axes so that the arm effector is moved to commanded positions under controlled '' '} A~, . .

33 ~98~ 53,490 velocity, acceleration and torque in accordance with a user's robot program. Pulse width modulation circuitry 801 and drive circuitry 802 are provided on the AIF board 800 to develop axis drive signals, in this instance for appli-cation to power amplifiers which provide the drive currentsto DC brushless electric motors respectively associated with the six axes of arm motion.
The AIF board circuitry processes the voltage command data to develop digital TTL logic level signals to control the base or gate drive circuitry of the power amplifiers which supply the motor drive currents to the axis motors. As previously indicated, the motor currents and axis position and velocity data are fed back through the AIF board 800 to the higher level control loops for closed loop position, velocity and torque control.
AIF BOARD - PULSE WIDTH MODULATION SCHEME
The pulse width modulation circuitry 801 on the AIF board 800 provides a digital interface or closing the torque or current control loop through the axis motor drive circuitry. The pulse width modulation concept is applied to control the conduction time width for the joint motor power switches and thereby satisfying motor voltage and torque commands.
As shown in the generalized block diagram of Figure 14, a digital PWM generator 825A receives 9 bit data commands and 3 register address bits on the torque micropro-cessor P2 bus. Additionally, device select logic, read/
write, reset (initialization) and data strobe signals are received from the P2 bus. A DTACK (acknowledge) signal is ; 3Q returned to the bus by the PWM genera~or 825A after each reception from the torque calculator on the torque proces-sor ~oard.
The digital PWM generator 825A is preferably arranged to service three axes where, for example, either brushless or brush type DC motors are employed as the axis drives. Thus, a set of digital signals (in this instance ~,, ~ ~ ,~

34 ~79882 53,490 four such signals A1, A2, B1, B2) is generated for control-ling the amplifier base or gate drive circuitry associated with each axis motor whether the motor is the brushless type or the DC brush type.
Four digital PWM control signals are employed to control the direction and magnitude of current flow through the motor windings through on/off power switch control. In - the brushless DC motor embodiment, the three phase windings of the brushles~ DC motor are interconnected in a bridge circuit (Figure 12B) such that the motor drive current is always directed through a pair of windings and the motor conduction path is rotated or commutated through successive winding pairs to produce the motor clrive torque. In this arrangement, the PWM pulses determine the time span of motor current, flow and commutation switching logic based on the PWM pul~es and Hall effect sensor eedback signals determine the winding pairs through which, and the direc-tion in which, drive current is to flow.
In the DC brush type embodiment where an H type 20 power amplifier brid~e circuit is employed, DC brush type motor 826A (Figure 13) is operated in one direction when power amplifier switches 827A and 828A are opened under control of PWM output signals Al and B2, and it is operated in the opposite direction when power amplifier switches 25 829A and 830A are opened under control of PWM output signals Bl and A2.
The pulse width modulation circuitry is prefera-bly embodied in a pair of large scale integrated pulse ; width modulation (~WM) chips. Generally, each PWM chip operates as a microprocessor peripheral device (i.e., under the control of a microprocessor higher in ~he conirol loop configuration) to provide digital pulse width modulated signal generation for control of three axes having DC brush type motor drives.
For more detail on motor current, position and velocity faedback and other AIF board circuitry reference is made to application Serial No. 569,043. For more detail on the PWM scheme raference is made to application Serial No. 580,685.
"
', ' t ,..

Claims (5)

1. A digital control for a robot having a plurality of arm joints, said control comprising:
an electric motor for driving each of the robot arm joints;
a power amplifier operable to supply drive current to each motor;
feedback control loop means for each joint motor including digital position and velocity control loops connected in a circuit operable at a predetermined sampling rate to control the associated power amplifier;
digital control means for generating position commands for said feedback control loop means in accordance with predefined moves set forth in a robot program;
said position command generating means including planning program means for generating a motion profile including acceleration, slew and deceleration segments for implementing each robot program motion command in accor-dance with specified time for acceleration, slew and deceleration;
means for computing slew velocity for the motion profile from specified slew time;
means for computing acceleration for the motion profile from specified acceleration time;
means for computing deceleration for the motion profile from specified deceleration time;

36 53,490 said position command generating means further including trajectory program means for generating traject-ory position commands for said feedback loop control means in accordance with the computed acceleration, velocity and deceleration values for the motion profile applicable to the current move segment.

2. A robot control as set forth in claim 1 wherein said control loop means includes means for generat-ing voltage commands from the position commands and pulse width modulating means for generating digital motor control signals for said power amplifiers from the voltage commands.

3. A robot control as set forth in claim wherein said planning program means includes means for adjusting the segment time specification and for applying acceleration, slew and deceleration values that satisfy stored robot limits.

4. A robot comprising:
an arm having a plurality of joints;
an electric motor for driving each of the robot arm joints;
a power amplifier operable to supply drive current to each motor;
feedback control loop means for each joint motor including digital position and velocity control loops connected in a circuit operable at a predetermined sampling rate to control the associated power amplifier;
digital control means for generating position commands for said feedback control loop means in accordance with predefined moves set forth in a robot program;
said position command generating means including planning program means for generating a motion profile including acceleration, slew and deceleration segments for implementing each robot program motion command in accor-dance with specified time for acceleration, slew and deceleration;

53,490 means for computing slew velocity for the motion profile from specified slew time;
means for computing acceleration for the motion profile from specified acceleration time;
means for computing deceleration for the motion profile from specified deceleration time;
said position command generating means further including trajectory program means for generating trajecto-ry position commands for said feedback loop control means in accordance with the computer acceleration, velocity and deceleration values for the motion profile applicable to the current move segment.

5. A robot as set forth in claim 4 wherein said control loop means includes means for generating voltage commands from the position commands and pulse width modu-lating means for generating digital motor control signals for said power amplifiers from the voltage commands.