Distributed Digital Control of a Robot Arm

Gordon Wyeth, James Kennedy and Jared LillywhiteComputer Science and Electrical Engineering

University of Queensland8t Lucia, Queensland, 4072

Australia

Abstract

Most robot arms use a central control box thatcontains the power electronics, .. the motorcontrollers and the coordinating computer. Thispaper presents a distributed control network thatallows the power electronics and motorcontrollers to be placed adjacent to the motors.Each controller uses a DSP for all motor controlw~th the individual controllers communicatingwIth each other and a host computer using aController Area Network (CAN). The designeliminates the bulky multi-core cable that is amajor source of downtime for robots such as thePUMA 560, while improving controllerbandwidth and facilitating greater softwarecontrol ofmotor response.

1 IntroductionThis paper describes the design of a distributed digitalcontrol system in a PUMA 560 six DOF robot ann. Thecontrol system presented consists of a controller for eachjoint .networked together with a PC which provides theuser mterface. Each controller contains a TMS320F241DSP operating a full bridge power stage to control thevoltage applied. to the DC motor on each joint based onposition, velocity and armature current mea~urements.The individual controllers are located physically close tothe m.o~or they are controlling, enormously simplifyingthe wrrmg harness of the robot down to a pair of powerand network cables. Additionally, the abilities of modemDSP devices allow greater capability and flexibility in thecontrol system design.

1.1 PUMA 560 RobotThe PUMA 560 robot arm continues to be used inindustrial situations for assembly operations and materialhandling as it has since 1979. Each joint is driven by a40V brushed DC motor, with the motors for the bottomthree joints rated at around 160W and the motors in thewrist rated at around 80W. Each of the fITst three joints(JT1: waist, JT2: shoulder, JT3: elbow) are also equippedwith a 24V electromagnetic brake which is required to beenergised (released) before that joint can operate. Thesebrakes stop the arm collapsing or swinging when thepower is removed. All joints are also fitted, on the motor,with 250 line count sin-cos encoders giving positionfeedback to the controller.

217

The PUMA 560 has an analog / digital hybridcontrol system, contained in a large racking box alongwith a terminal system for operator control andprogramming of the robot. The motor and brake controlsignals to the robot arm, and the encoder andpotentiometer signals coming back from it aretransmitted along an 80 core, 5 metre wiring bundl~. Thecontrol system consisted of an analog and a digital controlboard for each joint. These boards, along with others tosupply power, interface to the terminal, and interface to!he. robot arm, we~e connected via a custom backplanemSIde the electronIcs box. Also housed inside the boxwere the linear power amplifiers to run the motorson each joint, along with the robot's power supply. Eachanalog. b.oard imple~ented the inner current loop controlof th~ Jomt, and prOVIded the signal conditioning betweenthe sm-cos encoders and the digital board. The digitalbo~rd ~ontro~l~d the outer position loop fgr the joint,takmg Its pOSItIon target commands from the coordinatingcomputer every 28ms. The controller on each joint runs aPD control loop at approximately 1 kHz.

. These components have remained fairlyconsIstent over the past twenty years, and although thephysical volume of the off-board control box beenreduced, the bulky, expensive and unreliable cableremains. The cable is an artifact of a centralised controlsystem. To remove it, a distributed control methodologymust be adopted.

1.2 Distributed Control SystemsResearch into the area of distributed control systems haslargely centred on the networking of 'dumb' sensors andremote input / output modules with a central mastercontroller (such as described in [1]). The inclusion of thistype of system into robotic control has only been a recentdevelopment, and has not been included into the robotitself. Bezi and Tevesz outline a system developed forcontrol of a PUMA 560 robot arm that uses a i486 masterPC to control, via an Ethernet network, six i386 basedjoint controllers, which controlled a joint each through aservo amplifier, and received the encoder data through I/Ocards inside each PC [2]. This system does not eliminatethe large, unreliable cable to the robot, and its onlydevelopment is the change from a backplane to theEthernet network.

Some more recent work has been done withimplementing distributed motor control systems withDigital Signal Processors (DSP). Overmars and Toncichused two networked low power TMS320E14 DSPs (6.4

Figure 1: Block diagram 0 the joint controller system.

2 Control System Hardware DesignEach joint controller system can be separated into severaldiscrete blocks. Their interactions are shown below inFigure 1. Each of these blocks are detailed in thefollowing subsections.

PowerSupply

Motor andBrake

Electronics

()o::J.........,oCD...,

CAN, Network

CurrentSense

Electronics

EncoderAnalog

Electronics

2.1 Performance SpecificationsThe new control system was specified with three maincriteria:• It should exceed or at least equal the performance of

the old system.• It should eliminate any complex wiring harness.• It should be as. simple, reliable and low cost as

employed a mathematical model of the robot tocompensate in advance for gravity and dynamic effects. Itdid not include coriolis and centripetal tenns, orcompensate for motor stiction or backlash. However,when running on a Silicon Graphics SGI/340 VGXmachine, a servo rate of 1000Hz led to a tracking error of0.7mm. Extrapolating the performance increases of thePD control loop to higher sampling frequencies indicatesthat it should be possible to obtain even better accuracywithout using approach to the control system using a highsampling speed and high loop gains instead of modelbased control.

The whole concept of a model-based controllerrelies on the accuracy of the model. As reported by Corkeand Armstrong-Helouvry [9], there is a considerablevariation in the model data available for the PUMA 560.Corke states "the success of the model-based controllersmay be interpreted as a demonstration of the robustness ofmodel-based control approaches when applied to therelatively slow and rigid Pl..JMA 560 manipulator." Forexample, the reported mass of link 2 (upper ann section)varies anywhere between 10.2 and 22.4 kg. The model­based controllers are usually robust enough to tolerateminor changes in the model parameters, however, such alarge change as doubling the mass of the arm sectionwould be likely to impair the performance of controller.The model-based type controllers are less stable in otherways, as Tarn [8] notes that when the third-order modelcontrol system was modified (to increase theperformance) by increasing the position and velocityfeedback gains, the model-based controller rapidlybecame unstable. The PD controller did not exhibit thisundesirable effect until much higher gains [7].

MIPS), to calculate the response in the PID control loopsto run two DC motors [3]. Their application was for use inCNC milling machines, and the network between jointswas limited both in speed (II5kb/s) and distance (40m).

1.3 Enabling TechnologiesThe CAN bus is a highly reliable standard .developed byRobert Bosch GmbH for use in the automotiveenvironment [4]. It is a m1;llti-master system, withsophisticated error checking and arbitration, so that anyhigh priority message will always get through fIrstwithout corruption by other messages. All data containedin each packet (up to eight bytes) is also checked with aCyclic Redundancy Check (CRC) error checking scheme,that can correct up to five random errors, and will beautomatically retransmitted if not correct. The errorchecking capabilities make an extremely reliable androbust network: CAN operating at 500 kbit/sec overstandard networking twisted pair cable, at 60% capacityfor eight hours per day, 365 days per year, has a of anundetected fault of one bit error per thousand years [5].The network operates at up to 1 Mbit/sec at up to 30m, orup to a distance of 5kIn at 10 kbit/sec. The CAN protocoluses an 11 bit node identifier, allowing ~048 jointcontrollers to be on the network at once.

Recent improvements in. DSP technology haverevolutionised the field of embedded motor contro1. DSPsallow high speed execution of common control schemessuch as PID control, and have the processing power toimplement more advanced control techniques. SeveralDSPs have just been released from Analog Devices andTexas Instruments which are designed to perfonn digitalmotor control. They are with on-board peripherals such ashigh speed, multi channel AID converters, multiple PWMoutput channels, flexible timers, quadrature encoderinputs, many external interrupts, and network interfaces(CAN, SCI, SPI). For example, the TMS320F241 fromTexas Instruments [6], operates at 20MHz, and can readthe AID converter, calculating a PID control law, currentlimit, and generate the required PWM output, in under 10J.!s.

1.4 Desired BenefitsThis high' speed operation is very important to thetracking accuracy possible with the control. system. Tamet.al [7], [8] describes the effect of four different controlalgorithms on the accuracy of a· PUMA 560 tracking acircular path. While operating with a PD control loop, themaximum position error (while tracking) was IOmm when.sampling at 100 Hz, but had dropped to 2mm at 500. Hz.The. potential for an accurate control system using theTMS320F241 is very good, with a sampling rate of 20kHz readily obtainable. Tam states that:

" ... the tracking perfonnance is highlysensitive to feedback gains. Higher gains can· onlybe used a higher sampling frequencies. Thus a highsampling rate is imperative for good trackingperformance." [8]

Since the sampling rate possible with the networked DSPbased controller is orders of magnitude higher than thoseused in Tarn's motor controller system, the. DSPshouldbe capable of running an extremely high performancesystem. The other control schemes used by Tam

218

possible, but· still have the flexibility to allow forfuture control technique experimentation.

To be able to obtain these three specifications, the newelectronics hardware must have several major differenceswhen compared to the old system used previously. Togive the required high performance motion control, and tokeep the design simple, a highly integrated controllercontaining sufficient processing power and the requiredperipherals on chip is necessary. This processor needs tobe able to efficiently interface to many subsystems in therobot environment. These include the motor driver, brakedriver, current sensing, quadrature encoder, potentiometerinput, emergency shutdown circuitry, communications,and self monitoring. To adhere to the low cost, theprocessor should have internal non-volatile memory, thatcan be reprogrammed without expensive equipment ­preferably without removing the IC from the board.

To eliminate the complex wiring harness of over80 cores in the existing robot, the control of each jointwould have to be done with individual control modules,and the control modules would need to fit inside the robot,with each controller next to the motor/joint that it wascontrolling. This leads to a further requirement in the jointcontrol modules of a high speed, highly robust networkinterface between joints and to the host controller.

2.2 DSP Considerations. The TMS320F24x series is a 32 bit DSP designed for

motor control. The availability of the Control AreaNetwork (CAN) module in this series, along withbootloader programmable internal Flash memory makesthe device particularly attractive for this application.Furthermore the device features 8k words of internal flashmemory, 8 PWM channels with deadband generation,quadrature input circuitry, an 8 channel 10 bit analog todigital converter with a conversion time of 800ns, a powerdrive protection external interrupt, and a SOns instructiontime.

2.3 Power ElectronicsThe two choices for the motor drive electronics wereeither a linear amplifier similar to the original PUMAoutput stage, or a modem design using a switchmode(Class D) power amplifier in a H-Bridge configuration. ,The switchmode power stage was the obvious choice forthis application, as it needs only a single supply rail and ithas a high efficiency of over 96%. This efficiency resultsin several advantages over the linear amplifier such assmall size, lower cost power devices, no heatsink, and theelimination of extra analog circuitry needed for biasingand removing crossover distortion. The individual devicesin the H-Bridge can be driven from separate PWMoutputs of the DSP, allowing the deadband features of thePWM peripheral to be used, along with the immediate«12ns) shutdown of these pins in the event of a faultwhich triggers the Power Drive Protect Interrupt (PDPInt)pin on the DSP. .

A semi-discrete solution was chosen for thisdesign. This allows the choice of low on-resistance andfast switching MOSFETs, to give maximum efficiencyand best control. To switch the high side MOSFETs in theH-Bridge, a bootstrapped driver chip is required. This isbecause to switch an N-Channel MOSFET on, a voltage

219

of around 8 - 12V is required on the gate, relative to thesource. Since the source of the high side MOSFET couldbe at any voltage up to 40V, at least 48-52V is required toapply to the gate. The Harris Semiconductor HIP4081Adriver has a voltage rating of 80V, giving a wide safetyfactor, and also very high speed switching times of 45nsfor the lower device, and 60ns for the upper device.Another useful feature, not found on any other drivers,was the addition of a charge pump circuit to keep thebootstrap capacitor charged if the high side was switchedon for a long time, such as running at 100% duty cycle.For the power devices, IRF520 N-Channel MOSFETSwere selected, for their fast switching time of 9ns,continuous c~ent rating of9A, peak current of 37A, on­resistance of 0.250, breakdown voltage of 100V and lowcost.

The electromagnetic brakes (1.2H, 1600) onJoints 1,2 & 3 need 24V to operate, although a muchlower voltage than this is required to hold them on. Alogic level MOSFET is used to modulate the 40V fromthe main bus to maintain the current through the brakes.This allows an intelligent approach to give fast brake tumon (release to allow robot movement) and cooleroperation, by applying 40V across the brake. inductorinitially, then reducing to a PWM value required tomaintain the brakes in their on state, thus reducing powerdissipation in the windings compared to if a constant 24Vwas used.

1.4 Power SupplyOne important constraint for the main 40V power supplyis that the kinetic energy of the robot can flow back to thevoltage bus under certain conditions. This reducesdissipation in the output devices and reduces losses andgeneral power consumption. However the bus must beable to absorb power as well as supply it. A direct mainspowered supply would have destructive increases involtage under these circumstances. Currently the solutionto this problem is to use large lead-acid batteries to storeregenerated energy, although this does lead to someirregularity in the 40V supply rail as it depends on thecharge state of the batteries. Power for the nsp andanalog electronics is derived from a 12V supply on theCAN connector.

1.5 CAN NetworkThe CAN controller peripheral requires minimal externalhardware, using only one bus driver chip, containing thepower devices to drive the network, and the receiverhardware to read data off it. The CAN network isterminated at either end with 1200 resistors, to eliminatesignal reflections in the wiring. The CAN is a multi­master bus network, with any node being able to transmita message at any time. To eliminate collisions on' thenetwork, a combined hardware/fmnware approach isused. The bus is capable of two states: recessive, in whichboth CAN-H and CAN-L lines are pulled via resistors to2.5V; and dominant, where CAN-H is driven to 5V, andCAN-L is driven to ground. So this means if two or morenodes try to transmit data at the same time, a nodetransmitting a dominant state will read back a dominantstate, whereas the nodes transmitting a recessive will readback with an error. If two or more bus nodes start their

transmission at the very same time after having found thebus to be idle, collision of the messages is avoided byeach node monitoring each bit of its unique identifier. forerrors. Devices detecting a collision (with a non-dominantbit) will drop out.

1.6 Sensor InterfacesCurrent sensing is perfonned in the three base joints by a0.066Q resistance in each leg of the H-Bridge; while thewrist motors have a O.096Qresistance; each is made up oftwo 1W resistors in series. 'The voltage from these 'senseresistors is amplified by differential amplifiers andmeasured by the ADC. Current is also cheCKed against ascrewdriver adjustable hard limit that is used to trigger thePower Drive Protect interrupt. .

The position feedbacK from the sin-cos encodersprovides a count on every' edge of both quadraturechannels. This provides 1000 counts per motor revo[utionfrom the standard 250 line count encoder wheels.Ambitiously, each channel is also interfaced to the DSP'sADC, providing a theoretical 3 micron accuracy at thetool point. While this accuracy is unlikely to be realisable,the extra position sensing ability may prove of benefit infme motor control.

In addition, the nsp can measure the busvoltage, the position' potentiometer sensor, and thetemperature of the M0SFETs.

3 Control System Software DesignThe software for the DSP on each joint controller has toperfonn many tasKS at high speed. The major fi.u1ctions ofthe controller are perfonned in the operation of the jointposition control loop, with only the host communicationssection operating outside this loop. The communicationssection receives new commands and passes fault anddiagnostic information to the host and the other joints.The whole controller is interrupt driven, with most of theexecution time spent in a null loop. The different interruptservice routines (ISRs) do the actual worK of thecontroller, and described in the following sections. '

3.1 CAN SoftwareThe communications ISR in each DSP is .triggered on .receiving a pacKet addressed either to the specific joint oran overall (broadcast) address. As described previously,messages on the CAN bus will be transmitted in order ofpriority, with the highest priority having the lowestmessage address value. 'The robot communicationsnetworK is configured with Address Oas a broadcastaddress, . which all joints and the host controller willreceive in their second receive mailbox. These· dataframes. are reserved for urgent robot-wide messages suchas a request for immediate' joint shutdown. Address 1corresponds to messages specifically for Joint 1, up toaddress 6 for JT6. 'This is arranged so that commands tothe joints with the largest ranges of movement (andtherefore the most dangerous in the event of a delayedpacket) will always get through fIrSt. Address 7 is for jointto host communication, with the DSP· attached to the hostPC via its RS-232 serial port listening for data frameswith this address.

The advantage .of the multi master networK

220

configuration is readily apparent in the event of a jointsuffering a major fault such as a hardware overcurrent tripor a low (or non-existent) bus voltage. The joint does nothave to wait to send a message to the host and then waitfor the host to broadcast to the other Joints, it canimmediately broadcast the message to the other joints andthe host. This way, the other robot joints will have alreadybegunto shutdown and come to a safe halt by the time thehost interface is aware of the problem. Since it is apriority 0 message, it will get transmitted as soon as thecurrent transmission on the network is fmished - in theworst case time it will be approximately O.2ms beforeeach joint begins shutdown.

. When each joint controller receives a data framewhich is addressed to it specifically, it will contain the full8 bytes of data possible in the packet. 'The fIrst word isused for defining the command to the joint. Although awhole word is not necessary to define the messages, itmaKes the subsequent coding much easier, and ensuresthat the networK will run satisfactorily at the slowest datarate possible. The individual joint .controllers can alsosend status information to the host controller (address 7)of their own accord. The fIrst byte contains the address ofthe joint sending the pacKet. The second contains themessage type number, and the rest of the data framecontains any more relevant information, depending on themessage.

3.2 Control Loop'The CAN pacKets deliver commands from the host to thecontroller as desired joint angles along with maximumvelocity and acceleration for the move. The host may alsospecify other parameters such as the maximum currentpermissible during a move. This primitive scheme hasbeen implemented as a starting point for the progressivedevelopment of the ann software.

A 20 KHz timer ISR implements the mainfunctionality of the joint controller. The controllergenerates a trapezoidal velocity profile to move from thecurrent position to the desired position. The desiredvelocity is compared to the setpoint velocity from theprofile generator, and compensated using a PI controller.Once the profile has been completed, the control loopruns a PI compensator based on the position rather thanthe velocity. This serves to further reduce steady stateerror at the path endpoints, and increases tolerance todisturbance forces.

Whenever .a desired PWM is calculated, it ischecked against the 'soft' over-current limit. If there was ahigher motor current than the limit last PWM cycle, itreduces the calculated PWM by a proportional amount,Koc, to limit the motor current on the next cycle.

3.3 Safety.As soon as the'overcurrent detection circuitry triggers theactive low PDPINT pin, the six full compare outputschange to a high impedance state. A selection of pull upand pull down resistors brings each line to the appropriateinactive level. In the software, the next interruptscheduled is the PDP ISR - the only higher priorityinterrupt is the Reset signal or the NMI (Non MaskableInterrupt). The PDP ISR broadcasts a message on CANaddress 0 to all joints and the host to begin an immediate

Figure 2: The step response of joint 3 from a vertical positionusing a 7V input.

Table 1 summarises the fIrst order approximations madeof the voltage to velocity transfer function for joints 1 and3 three joints in various configurations. In all cases, non­activated joints had their brakes locked. Further results arepresently being gathered to better characterise the system.

Joint Configuration DC Gain 't(RPMIV) (ms)

1 2 & 3 vertical 0.48 24.31 2 & 3 horizontal 0.48 48.52 2 & 3 horizontal, up 0.47 49.12 2 & 3 horizontal, down 0.78 51.23 2 & 3 vertical 2.23 19.2

4.2 Velocity ControlThe velocity controller outlined in section 3.2 has onlybeen recently completed. Quantitative data is yet to begathered to show the tracking capabilities, the accuracyand resistance to disturbance of the control system. Thecontroller has been implemented on the fIrst three jointswith hand chosen PI values for both position and velocitycontrol. Qualitatively, the arm is able to move swiftlyfrom set-point to set-point and readily holds the set-pointangle against disturbances. The effect of the soft currentlimit can be felt in testing of disturbance response. Whenthe current limit is kept low, the robot becomes torquelimited in its response and exhibits a degree ofcompliance in its movement.

5 DiscussionFrom initial testing performed on the entire robot as asystem, the new type of controller has the capacity toexceed the performance of the original system inaccuracy, speed, efficiency and reliability. Also, with themajor reduction in the complexity of the wiring harness,and with the new compact, efficient electronics, theoverall system cost is substantially cheaper than the oldsystem, both to construct and operate.

The cost for components, connectors, printedcircuit board, and DSP for each joint controller is under$180. The cost of the wiring harness is negligible, atapproximately $10. The total prototype system cost was$1200 in parts on a "one-off' basis. The labour of wiringthe old complex harness has been saved, and the newboards are simple to construct, resulting in a substantialsaving in manufacturing time compared to the originalsystem.

Another feature of the new system is that it ischeaper to operate than the old controller. There are nocooling fans or other moving parts to wear, and the powerdrive electronics are highly efficient, at approximately 96percent. The linear amplifiers (Class A-B) used in the oldsystem have a maximum theoretical efficiency of 78percent, but are probably substantially lower «70%) dueto the inductive nature of the motor and brake loads. For arobot working continuously at 500W - approximately75% of maximum power - this efficiency gain results in areduction in load of about 195W. Over a year ofoperation, this will save over 1200 kWh of electricity.

With a small amount of control systemdevelopment, and a better user interface, this controlsystem has the potential for widespread use in theindustrial robotic environment. The nature of the CANbus used does not limit the use of the robot in largeinstallations, because in expanded mode, over 536 milliondifferent nodes can be addressed on the network. Also, theworkload on the host controller is very low, consistingonly of transmitting position commands to the joints.Many robots could be run from the one host, resulting inanother substantial cost and complexity reduction.

0.200.150.10

Time (s)

0.05

Joint 3 Step Response (7V)

en 3500

]j 3000t:g 2500

~ 2000

.!. 1500

~ 1000'g 500Q)> a

0.00

shutdown sequence. The program halts at this point andwill not restart until a complete system reset is perfonnedby cycling the power to the robot.

. The NMI routine is triggered if the DSP tries toaccess an illegal address location or tries to execute anillegal opcode. It immediately shuts down the PWMoutputs and broadcasts a message on address 0 of theCAN network. The response of the rest of the robot to thismessage is the same as to the PDP ISR message ­immediate shutdown.

4 ResultsPreliminary testing of the ann has been used to evaluatethe system response of some of the joints. The velocitycontrol loop and profiler described above has beenimplemented on joints 1,2 and 3.

4.1 Open Loop ResponseSeveral measurements of system response to a step inputhave been measured with the new hardware. Naturally thestep response of the joints will vary depending on the annconfiguration. Figure 2 shows a sample response from a7V step input to th~ motor of joint 3 (the elbow). Theelbow was configured vertically upwards so that theresponse would be minimally assisted by gravity.

Table 1: First order approximation results for a step inputto some of the joints.

6 Conclusions and Future WorkThe fIrst stage of this project was entirely successful inits aim of producing the hardware for a. mod~mdistributed control system for the PUMA 560 mdustrlalrobot. More work is needed on the software before the

221

robot is complete and the full extent of the new electronic,design can be tested.

Currently work is under way to develop improvedcontrol strategies for the robot allowing trade-off betweenposition, velocity tracking and force control. In parallel, aportable robot programming environment is beingdeveloped that can take aavantage of the capabilities ofthe distributed control system. With tliese modules inplace, tlie robot will used to explore new applicationdomains for the PUMA 560. '

References[1] Parkin, R, Distributed control systems within a

mechatronic environment, Mechatronics. The Basis forNew Industrial Development. 1994; 825 pp. p.521-6,Comput. Mech. Publications, Southampton, UK.

[2] Bezi, I, Tevesz, G, PC Based Robot Controller forAdvanced Control Algorithms, Mechatronics. The Basisfor New llidustrial Development. 1994; 825 pp. p.743­8, Comput. Mech. Publications, Southampton, UK.

[3] Overmars, A.H, Toncicli, D.J,.A.ppUcation of DSP

222

Technology to Closed-position-Ioop Servo Drive Systems, IntJ Adv ManufTechnol, 11:p27-33, 1996.

[4] ISO 11898 (1993): Roaa vehicles - Interchange ofdigital information .2...Controller area network (CAN) forhigh-speea communication.

[5] CAN in Automation Working Group, CAN Data LinkLayer, http:rrwww.can-cia.der, 1999

[6] Irfan Ahmed, nsp Products Group, TexasInstruments, Implementation of PID and Deadbeatcontrollers with the TMS320 Family, Texas InstrumentsLiterature SPRA083, 1997.

[7] Tam, T.l, et.al., Effect of Motor Dynamics on NonlinearFeedback Robot Arm Control, IEEE Transactions onRobotics at;ld Automation, Vol 7. No.1, February 1991.

[8] Tam, T.l et.al, Performance Comparison of FourManipulator Servo Schemes, IEEE Control SystemsJournal, 1993.

[9] Corke P.I., Armstrong-Helouvry B, A Meta-study ofPUMA 560 Dynamics: A critical appraisal o/literature data,Robotica,