Multiprocessor control system for industrial robots

Robotics & Computer-lmegrated Manufacturing, Vol. 8, No. 2, pp. 77-86, 1991 Printed in Great Britain

•

0736-5845/91 $3.00 + 0.00 © 1991 Pergamon Press plc

Paper MULTIPROCESSOR CONTROL SYSTEM FOR INDUSTRIAL ROBOTS N. KIR~ANSKI, M. VUKOBRATOVI(~, B. KARAN, M. KIR~ANSKI a n d A. TIM~ENKO Mihajlo Pupin Institute, Beograd, Yugoslavia

This paper describes an advanced robot control system based on three parallel processor boards. The controller has been designed as a general-purpose robot control system dedicated to controlling industrial robots with up to six degrees of freedom. The controller is based on a disk-oriented real-time operating system. The entire set of robot parameters can be monitored and changed by the user on-line. Source files, compilers, linkers and various utilities are provided as well. The main software layers include robot program interpreter, manipulator control facility, robot kinematics, dynamic feed-forward compensation, and digital servos. Basis, hand and joint coordinates are supported. Point-topoint and continuous path motions are provided. Digital and analog IO modules are included for synchronization with an environment. An acquisition system for monitoring and graphical presentation of robot coordinates, velocities and IO signals is provided. The paper ends with some experimental results for a six-degree of freedom robot.

1. INTRODUCTION Multiprocessor robot control systems have been described in many papers over the last several years. 1-5 The coordination between higher and lower control levels has usually been implemented using one or two address/data busses. Usually, one processor has been dedicated to the highest control level to perform robot program interpretation. One or multiple parallel processors were used to compute inverse kinematics. Similar processors were used to control the motion of individual robot axes. In order to decrease the time delay between the highest and the lowest control level, kinematics and regulators were sometimes divided into n pieces of software2 (where n = number of joints) and allocated to n local processors working in parallel. An example of an advanced control system is the space teleoperation robot controller 1 based on the Intel Multibus I and 32-bit NS 32000 microprocessors. It consists of one kinematic and several dynamic processors (regulators) working in parallel and sharing the same bus. The dynamic processor supports several multi-input, single-output regulators. Each regulator includes an interface to a digital tachometer and optical encoder, AD converter, and PWM output

stage. Instead of a 32-bit microprocessor, robot controllers often use 16-bit micros supported by arithmetic floating-point coprocessors as in Ref. 2. Several advanced robot controllers using the UNIX operating system have been described in the literature. 3 For example, the C/UNIX concurrent programming environment for a Motorola 68020 multiprocessor configuration connected to a SUN workstation was used to develop CHIMERA--a realtime environment for the CMU reconfigurable modular manipulator. 4 Similar hardware was used to develop the SAGE operating system designed for real-time supervisory control applications in robotics. 5 The programs are developed and cross-compiled on a SUN workstation, and then dynamically downloaded to the Motorola 68000 processor board with the SAGE kernel, which supports a variety of services including process and memory management, communication protocol support, and timing facilities. This paper describes a new robot controller (Fig. 1) based on three parallel processor boards using the Intel 8086 processor and the mathematical coprocessor Intel 8087. The main processor board is supported by Intel's iRMX-866 operating system. We have adopted this configuration in order to develop a lowcost but capable industrial controller. Unlike most of the commercially available controllers, this configuration includes hard and floppy disk drives and a realtime operating system which offers the possibility of developing all necessary controller routines without any operating system modification. In addition, the disk-oriented robot controller allows the user to store

Acknowledgements--The authors express their gratitude to Mr. M. Djurovic, Mrs. N. Djurovic, Mr. V. Devedzic, Mrs. O. TimSenko, Miss T. Petrovic and Mr. D. Urosevic for their softwaredevelopmentcontributions to this research; to Mr. M. Timotijevic, Mrs. B. Aleksic and Mr. B. Kovacevic for designingelectronic boards; and to Mr. B. Macasev and Mr. N. Damjanovic for assembling the control system. 77

78

Robotics & Computer-Integrated Manufacturing • Volume 8, Number 2, I~1

Fig. 1. Six-degree of freedom robol and the multiprocessor control sy.~tem

a large number of robot programs and to switch from one to another with minimal effort, which is very important for flexible manufacturing systems. Also. parameter modification and data acquisition system become built-in features of the robot controller. In addition to the three processor boards, the overall system includes a large number of 10 modules: 12 bit AD (40/~s) converter with a 16 channel multiplexer, 6 channel 12 bit DA converters, 6 channel shaft-encoder board, etc. The average computing time of a 32 bit floating-point operation is about 30/~s. Thus, the maximum throughput of the overall system is about 0.1 MFLOPs. The system software is stored on a 20 MB hard disk. A set of compilers (C, PL/M, FORTRAN-77, Pascal and Micro-Assembly language), linkers, locators, editors and utilities may be used during development of the multi-tasking and multi-processor applications. We have used these facilities to develop the universal robot controller software. Besides the application programs (user interface, robot language interpreter, kinematics, dynamics, regulators, hardware test programs, graphics), we have developed the utilities for down-loading tasks on the second and third processor board, communication routines between the processor boards, and so on. Robot language interpreter, user interface, start-up procedures, and hardware test utilities are implemented on the first processor, while the nonlinear

inverse kinematics together with a nonlinear ieedforward dynamics are implemented on the second one. The digital high speed regulators (3 ms sampling timei are implemented on the third processor board.

2. H A R D W A R E

OVERVIEW

The controller consists of the following: • •

•

• • • • •

• • • •

Master processor board based on Intel 8086/8087 processor/coprocessor and 512K RAM memory Two slave processor boards based on lntel 8086/ 8087 processor/coprocessor and 128K R O M RAM memory Winchester-disk controller and a 20MB Winchester disk Floppy disk controller and a floppy-disk drive for 680K diskettes Memory expansion module (512K) AD conversion module (16 channels, 12 bit accuracy) DA conversion module t6 channels, 12 bit accur acy ) Shaft-encoder interface (6 channels) Digital IO module (24 channels) PWM analog power amplifier modules to drive d.c. motors up to 100 W. Hand teaching-box with a keyboard Video terminal.

Multiprocessor control system for industrial robots • N. KIRt~ANSKIet al.

a Multibus interface board can be added to connect a digital camera to the controller.

Graphical [/ vldeo~er ml?l II

/ I

I

79

-[Winchester[ [Floppy disk 20

Master

I I I lsk o.ok I

Winchester

processor

and f 1oppy

512 K RAM

disk controller

board with

II

I Pr°cess°r I Iboard Slave 0 I

[128 K

I

Toachin with

~

" [

M

H

IProcessor board Slave l I

pendant

RAM.

/1 /L

.... tloto sll

II ii

and ~lltor~

a /

[ /

I

Digl tal

I I AX~r~

bot *-oncodersl r°-P°t°ntl°°°te slk tors III ISh III ROBOT

]

Fig. 2. Hardware organization. The interconnection among the hardware modules is presented in Fig. 2. The Multibus I is used for interprocessor communication. The master processor uses the iRMX86 operating system. Its 512K onboard memory has been expanded by an additional 512K on the memory expansion board. Each of the slave processor boards includes 128K of EPROM/ RAM. Floating-point math units are added to each of the processor boards. A 20MB Winchester disk is used to enhance the system flexibility; it contains a large number of robot programs and acts as a mass storage for the built-in acquisition system. The system can be expanded by additional processor boards to enhance the computational power necessary for the processing of various sensors. For example, a six-component force/torque transducer can be attached via an additional processor board, or

3. SOFTWARE OVERVIEW The controller software has three parts: a collection of utilities for robot programming; a set of controller maintenance routines, servicing and special-purpose utilities; and the robot control software. 7 The controller has been designed to facilitate autonomous robot programming by combining so-called robot teaching via a portable teaching pendant, and programming in a proprietary robot language, RL. Therefore, its software includes a complete environment for writing, debugging, and maintaining robot programs. Its programming interface is based on the iRMX86 multitasking real-time operating system. Through the dialog processor, e.g. a menu-driven shell, one can access a collection of application utilities or a set of iRMX86-supplied commands and utilities. Typical application utilities related to robot programming are an RL translator, an interpreter, and a debugger. Service and maintenance utilities include hardware test and controller setup programs (hardware configuration and parameter initialization). A special set of programs for system calibration and performance measurement includes routines for data acquisition, control-variables monitoring, and graphical presentation.

The RL langua#e The RL textual language is classified as a primitivemotion level,8 manipulator level,9 or robot level x° language. It represents a programming system in which the user specifies all necessary move statements and other actions to fulfill a robot task. RL programs consist of two parts: a declaration part and an executable part. The declaration part contains declarations for binding identifiers to objects in RL instructions (Table 1). An identifier has up to eight alphanumeric characters starting with a letter, and represents data variables of logical, byte, integer, real, point data type (the point type is a structure which can store robot position and orientation), or signal a pseudovariable used for communication with external equipment.

Table 1. Declarations of RL objects LOG id BYTE id INT id REAL id POINT id EXT id: line@group EXT id: type@9roup OUT id: line@group OUT id: type@group COND id: exprl relop expr2 COND id: expr

Logical variable. Byte variable. Integer variable. Real variable. Point (frame) variable. Input logical (single bit) signal at channel number group and line number line. Input signal of type type (BYTE, INT, REAL or POINT) at channel number group. Output logical (single bit) signal at channel number oroup and line number line. Output signal of type type (BYTE, INT, REAL or POINT) at channel number group. External condition: exprl, expr2 are either constants or references to variables, and relop is one of the operators , =, < =, > =, < > ; at/east one of the expressions exprl, expr2 must be a reference to an input signal; External condition which involves only a reference to the single bit signal expr.

80

Robotics & Computer-Integrated Manufacturing • Volume 8, Number 2, 1991

Finally, an identifier can be used for specifying external conditions. External conditions that can be handled by the RL language are relational expressions with at least one reference to an input signal. The R L language features scalar arithmetic as well as a set of operations applicable to point data. The point type is a structure which describes robot position and orientation by means of Cartesian coordinates (x, y, z, ~b, 0, q,) and r o b o t configuration selectors (mapping of Cartesian coordinates into r o b o t joint angles is not unique for the general case, and for most six-degree of freedom robots, up to three indicators are used for the selection of robot arm, elbow, and wrist configuration). The point data type can also be used for describing arbitrary coordinate frames in space. The configuration selectors are meaningless in that case. The list of arithmetic operations on point data is given in Table 2, where R i and P~ denote real-valued and point-valued expressions respectively. The executable part of R L p r o g r a m s is a sequence of R L instructions such as assignment, data input, and data output instructions; p r o g r a m flow and condition monitoring instructions; and motion specification instructions t~ (Table 3). In addition to data input from the system input device, there are two instructions for reading the current position of the robot ( H E R E and J H E R E ) . P r o g r a m flow control instructions provide: conditional instruction execution (the IF instruction), unconditional j u m p (the G O T O instruction), subroutine calls (instructions C A L L and RET), p r o g r a m stop (instructions P R O G S T O P and A B O R T ) , and execution of a new p r o g r a m (the R U N instruction). Instructions G O T O , C A L L and R U N are optionally preceded by the label identifier and the colon character. In parallel to the main flow of p r o g r a m execution, the RL executive is able to m o n i t o r a set of selected external conditions. M o n i t o r i n g a particular condition is done using the W H E N instruction. Additional control of monitored conditions is provided with instructions for disabling and enabling selected interrupts ( D I S A B L E and E N A B L E instructions), and

removing conditions from the list ( I G N O R E instruction). Service interrupts by the R L interpreter is done on a priority basis. The RL interpreter is busy process-, ing interrupt until the W H E N instruction is completed. If the W H E N instruction specifies a subrotttine call, the RL interpreter remains in interrupt-process-. ing state until executing a return from the subroutine. The motion specification instructions describe intended m o t i o n along the robot path segments. There are two groups of m o t i o n instructions: the tirsl specities straight line m o t i o n (e.g. motion with linear interpolation of Cartesian coordinates, M O V and M O V I instructions), and the second is used for specil~'ing motion with linear change of joint coordinates ( J M O V and J M O V I instructions). Additional instruclions are provided for describing more closely the intended motion. They specify' the robot configuration: after execution of the next motion instruction (ARM, E L B O W and W R I S T instructions), time for move. merit ( T I M E instruction), m o v e m e n t speed ( S P E E D instruction), and acceleration rate ( A C C R A T E n'struction). A special instruction G O H O M [: ptc,~ides motion to the control zero position with simtdtaneou:, calibration of the robot servo system. The interpretation of motion instructions is swlci~ronized with robot motion m ihe following re;tuner il an instruction that specifies ~ new path segment occurs before the current segment is completed, then the specifications for the new segment arc buffered and the RL interpreter continues lo interpre{ incoming instructions. This short review suggests that the RL language ~s a c o m p a c t and therefore easy to learn system for prog r a m m i n g small to medium size robot applications Interpretation of R L programs Execution of robot p r o g r a m s involves cooperative operation of three parallel computational processes: decoding and interpretation of R L p r o g r a m instructions, motion control, and monitoring of input signal lines. The R L interpreter is at the lop of the robot controi

Table 2. Operators and bult-in functions involving flames t~

PI +P2 P1 - P2 DISTANCE(PI, P2) MAKESEG(PI, P2, R) GETX(P) GETY(P) GETZ(P) GETFI(P) GETTHETA(P) GETPSI(P) MAKEPOINT(R1 .... , R6) P1 = P2 P1 < > P 2

Returns inversion of P, e.g. position and orientation of reference flame relative to the frame P Returns composition of P1 and P2: if the coordinates of the frame P2 are defined in the flame t'I, lhc~ P1 + P2 gives absolute coordinates of P2. Returns relative position and orientation of PI with respect to P2:P1 ( /'2) -= I7t 6 P2 Returns distance between origins of PI and P2. Returns frame whose origin is located in the line which connects origins ot PI and P2, at distance R from the origin of P1 to the origin of P2, and with linearly interpolated orientation angles: MAKt:,SEG(P1, P2, DISTANCE(PI, P2)) -7_P2. Returns x-coordinate of the frame P. Returns y-coordinate of the frame P. Returns z-coordinate of the frame P. Returns 4~-coordinate of the frame P. Returns 0-coordinate of the frame P. Returns y-coordinate of the frame P. Returns frame with coordinates (R1 .... , R6). Returns l if PI and P2 approximate the same frame, within predefined tolerances. Returns 1 if PI and P2 does not approximate the same frame, within predefined tolerances.

Multiprocessor control system for industrial robots • N. KIRt~ANSKIet al.

81

Table 3. RL instructions Variable assianment par := expr Data input HERE var JHERE var INPUT var Data output PRINT text PRINT expr Prooram flow IF expr THEN instr GOTO label CALL label

RET PROGSTOP ABORT RUN label

Assigns variable var the value of the expression expr. Assigns point variable var current position and orientation of robot tool center point. Assigns point variable vat current position of robot joints. Reads value of the variable vat from the system input device (e.g. from the terminal). Writes ASCII text to the system output device (e.g. to the terminal). Writes value of the expression expr to the system output device. Executes instruction instr if the logical expression expr is true. Jumps to the instruction labelled with label. Calls subroutine starting at label. Returns from subroutine. Terminates program execution. Signals abnormal condition and terminates program execution. Runs program starting at label.

Condition monitorin 9

WHEN p f t cond DO instr DISABLE expr ENABLE expr IGNORE expr Motion specification MOV expr JMOV expr MOVI expr

JMOVI expr ARM expr ELBOW expr WRIST expr TIME expr SPEED expr ACCRATE expr GOHOME SSTOP QSTOP

Initiates monitoring of the condition cond with specified priority p (1 to 15), scanning frequency f (SLOW, MED, or FAST), and type t (EDGE or LEVEL). When the condition becomes satisfied, instruction instr is executed. Prevents selected conditions to interrupt program; expr is condition identifier or priority. If the expr is omitted, then all monitored conditions are disabled. Enables previously disabled conditions; expr is the same as in ENABLE. Stops monitoring selected conditions; expr is the same as in ENABLE and DISABLE instructions. Straight line movement to the frame expr. Movement with linear interpolation of joint displacements to the frame expr. Straight line movement to the frame expr which is defined with respect to the current position and orientation of the robot tool center point. Movement with linear interpolation of joint displacements to the frame expr which is defined with respect to the current position and orientation of the robot tool center point. Configuration of the robot arm at the end of the next path segment: expr evaluated to 0 selects left configuration, and 1 selects right configuration. Configuration of the robot elbow at the end of the next path segment (0 selects elbow down, and 1 selects elbow up). Configuration of the robot hand at the end of the next path segment (0 selects hand not wristed, and 1 selects hand wristed). expr specifies time of movement, in seconds, for the next path segment. expr specifies speed of movement for the next path segment as percentage of the maximum attainable speed. expr specifies acceleration rate for the next path segment as percentage of time in which acceleration is to be performed. Movement with linear interpolation of joint displacements to the robot neutral position. Smooth stop of the robot motion. Quick stop of the robot motion.

hierarchy. Its i n p u t is either a single R L instruction or a complete RL robot program. The RL interpreter o p e r a t e s in one o f the three m o d e s : immediate, stepby-step, a n d a u t o m a t i c . The interpreter's o p e r a t i o n cycle starts b y selecting the next R L instruction, whose type d e t e r m i n e s w h e t h e r the task is c o m p l e t e l y p r o cessed o r the request is sent to lower level c o n t r o l processes. T h e m o t i o n c o n t r o l process is c o m p u t a t i o n a l l y the m o s t intensive. It essentially performs the s a m e o p e r a tion cycle that is to be r e p e a t e d with the rate i m p o s e d by the r o b o t dynamics. It is d e v e l o p e d b a s e d on the following r o b o t c o n t r o l h i e r a r c h y : 1. T h e k i n e m a t i c s m o d u l e resolves a sequence of lowlevel m o t i o n requests, specified by R L m o t i o n instructions. T h e c o m m a n d s are t r a n s f o r m e d into desired j o i n t c o o r d i n a t e s q ° ( k T k i n ) , a n d velocities q°(kTkin), i - 1. . . . . 6, each Tki~ = 40 ms. 2. T h e values of the j o i n t c o o r d i n a t e s qO, c o m p u t e d b y the k i n e m a t i c s m o d u l e , are sent to the d y n a m i c s

m o d u l e which calculates the desired d r i v i n g torques

(feed-forward c o m p e n s a t i o n ) . 3. The d i o i t a l s e r v o m o d u l e accepts the j o i n t c o o r d i n ates qO, velocities qO, a n d the feed-forward c o m p e n sation; a n d a c c o m p l i s h e s servoing of specified trajectory. T h e feedback i n f o r m a t i o n is supplied from incremental shaft encoders l o c a t e d in m a n i p u l a t o r joints. T h e i n t e r p r e t a t i o n of R L instructions a n d m o t i o n c o n t r o l are s e p a r a t e processes t h a t o p e r a t e asynchronously. T h e bridge between the two processes is p r o v i d e d b y the s e p a r a t e m o d u l e , the m o t i o n c o n t r o l i n t e r f a c e . This m o d u l e consists of two parts. T h e first p a r t is a set of m o t i o n c o n t r o l services, e.g. p r o c e d u r e s for queueing m o t i o n c o m m a n d s , setting m o v e m e n t p a r a m e t e r s , r e a d i n g the c u r r e n t m a n i p u l a t o r position, r e a d i n g the status of the m o t i o n c o n t r o l process, etc. The second part, which is s y n c h r o n i z e d with the m o t i o n c o n t r o l process, r e a d s a n d d e c o d e s the status of low-level c o n t r o l modules, a n d sets low-level con-

Robotics & Computer-Integrated Manufacturing • Volume 8, Number 2, 1991

82

trol requests, including the dequeueing of motion commands. The input~output processing module allows access to all programmable external signals. This module has two parts. The first part is a set of services for reading input signals, generating output signals, and posing requests for monitoring of selected input signals. The second part is responsible for the process of monitoring. It is periodically activated and, depending on the requested frequency of monitoring, selects a subset of external conditions that should be scanned. If any of the conditions is fulfilled, its reference is stored in a priority-based queue of conditions.

iRMX86 was too coarse for regular activation of motion control modules, and the time overhead introduced by the iRMX86 was high. That is the reason why motion control modules were implemented on raw machines: a synchronization signal is generated every 3 ms by the clock counter on the slave 1 processor, and distributed to other processors. It is used for regular activation of the servo module, while the kinematics and nominal dynamics modules are activated on every 13th signal. The global memory visible from all processors ~s divided into three parts: the first part contains system data (such as synchronization signals), the second is the common database for communication between the motion control modules, and the third is used as a buffer for high-speed data acquisition. The common database contains input/output variables of the motion control modules. It is divided into blocks of data which are accessed through specially developed services. The time diagram of the kinematic processor and the regulators is shown in Fig. 4. We see that communication with the global memory is accomplished at the beginning and the end of each computational period. The entire software is written in PL/M-86 and FORTRAN-77 languages, comprising about 50,000 source lines of code.

Software implementation issues The robot control software is distributed on the three microprocessors that communicate over the global memory (Fig. 3). Application utilities including the menu-driven robot programming shell run under the control of the iRMX86 operating system on the master processor. When the user activates the programing shell, two iRMX86 processes are actually created. The first high-priority process is activated with constant frequency. This process incorporates polling functions of the input/output processing module and the motion control interface, e.g. it performs external-conditions monitoring, polling, and decoding of controller status as well as dequeueing motion commands. The second process represents a dialog processor, which is active when the first high-priority process is inactive. This process communicates with the highpriorit3 process using the input/output processing module and the motion control interface services. These services have access to shared data. Protection is implemented using semaphores. The RL interpreter is a subroutine of the dialog processor. The motion control process runs on two slave processors: the slave 0 processor runs kinematics and nominal dynamics, and the slave 1 processor runs digital regulators. Time resolution supported by the

SLAVE 0

MASTER pROCESSOR

4. K I N E M A T I C S M O D U L E Due to the importance of minimizing the numerical burden during motion execution, we have used symbolic kinematic models, both for the direct and the inverse kinematics. The computer-generated program for direct kinematics ~2'~3 requires 52 multiplications, 23 additions and 6 trigonometric functions for the sixdegree of freedom manipulator shown in Fig. I. The analytical inverse kinematic solution requires 55 multiplications, 34 additions, 4 divisions, 3 square roots, l sine, 1 cosine and 6 ATAN2 functions. The world coordinates are adopted to be Cartesian coordinates of the manipulator tip in the base frame and Euler angles: yaw, pitch and roll between the end-effector frame and the base frame. The program provides for continuous change of Euler angles even in the range

SLAVE 1

INTERFACE I

LANGUAGE 1 TERP~EffJ

Kinematic

processor

!~

-

4()

ms

!

.....

D I C I rAL

SERVO

/i

[~egul~tt

or

,

{~ m.~ :

!

i ¢

fpom the g i o b ~ i

I - data tr'~msfep to the ppocessops

I

SHARED

MEMORY

Fig. 3. Distribution of computation among microprocessors.

i-

local

ineal,,::~

mem~l'y

d a t a t i ' ~ n s f ' e v Fl-om t b ~ [,t,,,v>~%or'; l o c a l memopy t o t h e g l o b a l memory

Fig. 4. Computational timing of the kinematic processor and the regulators.

Multiprocessor control systemfor industrialrobots • N. KIR~2ANSKIet al. outside the ( - 2 n , 2n) interval (no abrupt changes for 2~r may occur). The main functions realized by the kinematics module are the following: •

•

•

•

•

Joint-interpolated motion between two knots for a specified time or a speed factor, with various acceleration profiles; Cartesian-interpolated motion between two points for a specified time or a speed factor, with various acceleration profiles; Joint-interpolated continuous-path motion among an arbitrary number of knots for a specified time or speed on each trajectory segment; Cartesian-interpolated continuous-path motion among an arbitrary number of points, for a specified time or speed for each trajectory segment; Control of the manipulator end-effector by means of a joystick either in joint coordinates, Cartesian coordinates of the base frame, or in the direction of the axes of the end-effector coordinate frame.

The output of the kinematics module is the desired joint angles q ko( t) , velocities g°(t) and accelerations /~°(t), where t k + l - tk = Tki,. The trajectory knots may be specified either in joint or Cartesian coordinates, both absolutely or relatively to the instantaneous position, independently of the trajectory generation mode. The user may configure the kinematics to realize one of the acceleration profiles: rectangular, trapezoidal, parabolic and (1 -cos2(2r~t/T)) profile. The latter provides for the smooth change of the third derivative of the coordinates, but entails maximal acceleration, while the other profiles have greater or lesser values in the third derivative but demand lower accelerations. The manipulator speed is selected either by specifying the speed factor or motion execution time. Thus, given an RL motion command (like MOV), the kinematics module generates the trajectory in joint, Cartesian or end-effector coordinate space: x°(t); t = to, t o + Tkin, t o + 2Tkin...

(1)

in accordance with the selected acceleration profile. Further, these points are mapped into joint coordinates q°(t) = f - '(x°(t))

(2)

where f - ' is the inverse kinematic function. If x ° represents the joint coordinates vector, then q ° ( t ) =

x°(t). During any of these motions, stop commands (soft stop, quick stop and immediate stop) may arrive either from a higher control level or from the kinematics module itself. The stop process depends on the type of motion that has been executed at that instant. For example, if straight-line motion in Cartesian space has been performed, stopping will be performed along the same trajectory. Controlling robot motion by a teaching pendant

83

may easily lead to manipulator singularities, where moderate end-effector velocities cause high joint rates. In order to enable exit from the vicinity of a singularity without changing the control mode, the motions that lead deeper into the singularity are not executed, while the movements that force the robot to go out of the singularity are permitted. Also, if the user-specified velocities cause excessively high joint rates, the desired velocities are automatically diminished by a certain factor and the motion is performed in the userspecified direction, together with a diagnostic error setting. Our software for robot motion generation is well protected from various errors. First of all, these errors may originate from incorrect manipulation task specification in the robot language program (e.g. movement out of reachable workspace was requested) and of poor communication with the higher control level (e.g. hardware errors). The checks are performed both during the preparation of movements (does the end point lie within the reachable workspace; is the time too short, is the end point a singular point; do the arm indicators change their values during the movement; etc.) and during the motion execution (do the joints reach their limits; does the gripper base or gripper tip touch the floor or the robot column; are the joint rates greater than the maximal rates allowed; etc.) If any of the errors occur, a message is sent to the higher control level and the robot is stopped. In this way the system is protected from erroneous programming and hardware damage. The health of the processor itself is controlled by a special hardware system. The higher control level may obtain information continuously on instantaneous positions in joint space or Cartesian space, as well as other data. This information may be very important if the robot program is to replan the manipulation task in accordance with real-time sensor information.

5. F E E D - F O R W A R D

DYNAMICS

The output of the kinematics module represents the input for the dynamic module, both implemented on the slave 0 processor. The dynamic feed-forward compensator is of the form '4 u ° = ACI° + B~ ° + C(J(q°)~ ° + h(q °, 40))

(3)

where A, B and C are n x n diagonal matrices of parameters of actuators, J is the simplified or exact inertia matrix of the robot arm, and h is the simplified or exact vector due to Coriolis, centrifugal and gravitational forces. The output of the dynamic feed-forward compensator u ° is called nominal control; it is computed on-line with the kinematic processor. The output variables u °, qO, and 4 ° are transferred via common memory to the digital regulators implemented on the slave 1 processor (Fig. 2). Since the sampling time T of the regulators is much smaller compared to the computational period Tki, of the kinematics processor, the interpolation of the input

Robotics & Computer-Integrated Manufacturing • Volume 8, Number 2, 1991

84

variables u °, qO, and qO is performed, resulting in smooth variables u °, qO and ~o: u° = u ° + k A u ° ;

qO=qO+kAqO; 41o = q 0 + k A ~

°

(4)

where k = 1, 2 . . . . . ~, with ~ being the ratio between the computational times of slave 0 and slave 1 (N = 40/3 ~ 13). The computational time for the kinematic processor is Tki. = 40 ms, while the computational time (sampiing time) of the regulators is T = 3 ms.

The expressions (4)-(9) must be computed in each sampling interval n times (n = number of manipulator joints = 6). Since the number of multiplications in (4) (9) is about 60, while the number of additions is about 100, the computational burden is obviously high. Therefore, we have reorganized the computation of (4) (9) to minimize the computational burden of slave 1. Substituting (4) and (6)-(8) into (9), we obtain U = (:o + k U l + U2(k) + E(k) E(k + 1) = E o + kE1 + E2(k)

(10/

where U o = - a , , K e b q + a , , K r b f + b,, + a,,u °

6. R E G U L A T O R S

A control law of the form

+ GKeq ° + a.Kvdl °

u = u ° + K p ( q ° - q) + K,,(gt ° - ~t)

+ K~

fo

(qO_ q) dt + KFF

U 1 = a~ Au ° + a u K e A q ° + a, K v AO° (5)

E 1 = a , , K , T Aq °

is implemented in the discrete form u(k) = u°(k) + K p ( q ° ( k ) - q(k)) + Kv(gl°(k) - gl(k)) + K l ~ ( k ) + K r F ( k )

g(k + l) = ~(k) + (q°(k) - q(k))T.

(6)

Here K e, K v and K, represent diagonal gain matrices of the PID regulator, K F is the force-feedback gain, F is the measured force (in the case of a sliding joint) or torque (for a revolute joint) on the output shaft of the motor reducer, and u represents the output signal. The quantity (u °, qO c~o F) represents the input vector for the PID regulator, while ~(k) represents the integral error. The joint coordinate vector q and the velocity O are measured by incremental encoders and analog or digital tachometers. Instead of digital tachometers we can use the estimated joint speed in accordance to the two-point numerical differentiation formula 1 c~(k) = T (1.5q(k) - 2q(k - l) + 0.5q(k - 2))

and

F = a.fiy + by

(8)

where iq and if a r e signals from sensors. The constants a o and bq are obtained from the two known values q~ and q2, while the constants a f and b f are obtained for each joint after loading the system with two different masses/inertias. A special normalization/calibration program is developed to facilitate automatic evaluation of the constants in (8). The control signal u (6) is also scaled: U = G u + b~

are computed on the slave 0 processor in time slices Tkin. Slave 0 receives the integral error e(tR') from slave 1 at the beginning of each time period Tkin. At the end of each time slices, the variables U o, U~, E o and E~ arc sent via global memory to slave 1. The variables U~ and E2 are computed on slave 1 every T = 3 ms: U 2 ( k ) = W21iq(k ) + W22iq(k -

1)

+ W23iq(k - 2) + I&24i j E 2 ( k ) = W25iq(k )

(llJ

where the constants W,.j are given by W21 . . . . a~ K e + K v l'5~a T } ~, W22 = G K v g" aq, 0.5 W23 = - a u K v T aq, W2a = auKeaj-,

(7)

where k represents the sampling instant and T is the sampling time (3 ms). The control law (6) is implemented in 32-bit floating point arithmetic. Thus, the input signals from position and force/torque sensors are scaled as follows q = aqiq + bq

E o = e ( ~ ' ) + G K t T q ° - a, K t T b ~

(9)

where U is the integer scaled to fit the DAC range (0-4095), while a, and b~ are constants.

W25 = - a , K i Ta q,

The number of multiplications and additions in (10) and (11) is 5 and 8, respectively. Note that kU~ in (10) does not mean the multiplication, but rather recursive addition of U1 in each sampling interval. For the six-degree of freedom robot, the number of multiplications is 30, while the number of additions/subtractions is 48. In conclusion, compared to the computation of the control law (4)-(9), the computation of the equivalent control law (10), (11) involves only half the number of multiplications and additions. The increased numerical efficiency was of great significance for achieving the high sampling frequency of 330 Hz (T ~ 3 ms).

7. E X P E R I M E N T S This section presents the experimental results with the six-degree of freedom robot shown in Fig. I (the robot structure is similar to the well known PUMA-560

Multiprocessor control systemfor industrial robots • N. KIR(2ANSK1et robot). First, we will illustrate the programming language RL by a simple example:

o.50

0 40

; example.d--square writer ; counters INT I ; index of square INT N ; number of squares ; constant frames ; determined by teach-in P O I N T SO ; table frame P O I N T C ; correction for ; table orientation P O I N T U P ; approach distance P O I N T D ; distance between ; successive squares P O I N T LR ; lower right corner P O I N T UR ; upper right corner P O I N T U L ; upper left corner ; variable frames P O I N T R ; square frame ; (lower left corner) ; writer = program for writing ; squares writer: print ' N u m b e r of squares:' input N if N < = 0 then progstop R.-=SO + C I..-0 plot: call square R..=R + D I..=I + 1 if I < = N then goto plot progstop ; square--subroutine for ; writing a square square: mov R + U P - C mov R - C mov R + LR - C mov R + UR - C mov R + U L - C mov R - C mov R + U P - C ret The example causes the robot to write a specified number of squares on a plotting table. The executive part of the program starts at the label 'writer': it reads from the console terminal the number N of squares to be written, equates the frame R connected to the lower left corner of the first square with the table frame, and then in a loop starting at the label 'plot' calls the subroutine 'square' for writing squares. After each call, the variable square frame R is updated for the next square. All constant frames are determined by a teach-in program (not presented here). The next example illustrates the acquisition system of the controller. The robot was commanded to move

85

al.

solid line -

desired o n g l e - act

---

L

I 0.30 e c~ c o F

0.20

0.10

0.00

........ 000

~. . . . . . . . . ~. . . . . h , , ~ . . C 7 , . . 7 , . , . / ............. 1,00 2.00 3.00 4.00 5.00 time -sec

, ......... , ......... , 6.00 7.00 8.00

Fig. 5. Experimental acquisition data: desired and actual first joint angle during robot motion.

from an initial position to the position qO = (0, - 4 5 , 135, 0, 30, 0) whose arguments are in degrees, and then to pass 4 intermediate points defined by joint increments Aq ° = (5, - 5 , 18, 10, 5, - 1 0 ) : s p e e d := 40 int@ 1 := 0 jmov #joint: 0, - 4 5 , 135, 0, 30, 0 # again: movi #joint: 5, - 5 , 18, 10, 5, - 1 0 # int@l ,= int@l + 1 if int@ 1 < 5 then goto again progstop During the robot program execution the acquisition system was activated to follow the desired joint coordinates qO and the real joint coordinates q. Figures 5-7 show desired and actual joint coordinates of the first three degrees of freedom. The numerical data and the graphical presentation of the acquired variables are very important for evaluating the overall control system performance. For example, the PID gains can be tuned until the real joint angles approach the desired joint coordinates up to the specified allowable maximal error e.

400

-

350

I

500

c~ o r-~ 2 5 0

solid line d a s h e d line

2.00

1.50

.........

0.00

, .........

~00

, .........

2.00

-

desired a n g l e actual angle

~ . . . . . . . . .

3.00 time

i . . . . . . . . .

4.00 -

i T , , ......

5.00 sec

,

. . . . . . . . .

6.00

i

. . . . . . . . .

7.00

i

8.00

Fig. 6. Experimental acquisition data: desired and actual second joint angle during robot motion.

86

Robotics & Computer-lntegrated Manufacturing • Volume 8, Number 2, 1991 tional processor board. Since the software is developed in the PL/M and F O R T R A N - 8 6 programming languages, the processor boards based on lntel 8086 8087 processors can be directly replaced by the corresponding 80286/80287 or 80386/80387 processor boards, thus enhancing the computing speed several times. The experimental results with the prototype controi system show that the modular hardware supported b? a real-time operating system represents a promising strategy in robot controller design.

f ! ,,7:

? f •/

!, !: :

d

/

, , 3

,7) L-/C;

1 C)C,

2 C)O

30C/

4 C/C t[,/>-

5 ,::C,

f~ C:f~

;' ( :

:~,

Fig. 7. Experimental acquisition data: desired and actual third joint angle during robot motion.

8. C O N C L U S I O N S In this paper an advanced industrial robot controller under the iRMX86 operating system was described. The system is based on three parallel processor boards and a series of IO boards attached to the Multibus I. In comparison to the standard robot controllers the new system has several advantages: • • • • •

•

The user can easily modify the robot parameters on-line. The variables and signals can be monitored during manipulator motion, The programs can easily be created by the use of screen editors. The software modules can easily be modified by the use of various utilities. The controller can easily be adapted to a different manipulator by simply modifying the programs for inverse/direct kinematics, and for dynamic compensation. An acquisition system for monitoring and graphical presentation of robot coordinates, velocities and IO signals is provided.

The controller can be used in various research projects to test different control strategies. Both the standard nonlinear control and the sophisticated adaptive control laws can be investigated. The system is suitable for kinematic and dynamic parameter estimation, and can be used for examining calibrating algorithms. The controller hardware can easily be extended by additional processor and IO boards. For instance, force control can be dedicated to an addi-

REFERENCES 1. Bejczy, A., Szakaly, Z.: Universal computer control system (UCCS) for space telerobots. Proc. IEEE Int. Conf. on Robotics and Automation, Raleigh, U.S.A., pp. 318325, 1987. 2. Paul, R., Zhang, H.: Design of a robot force/motion server. Proc. IEEE Int. Conf. on Robotics and Automation, pp. 1878 1883, 1986. 3. Hayward, V., Paul, R. P.: Robot manipulator control under UNIX: RCCL, a robot control C library, lnt J Robotics Res, 5 4: 94-111, 1986. 4. Stewart, D, Schmitz, D., Khosla. P.: Implementing realtime robotic systems using CHIMERA II. Proc 1990 IEEE Int. Conf. on Robotics and Automation, Cincinnati, Ohio, pp. 598 603, 1990. 5. Salkind, L.: The SAGE operating system. Prt~c. 1'#St? IEEE Int. Conf. on Robotics and Automation, Scottsdale, Arizona, pp. 860 866, 1989 6. iRMX 86 Introduction and Operator's Reference MaJ)ual. lnte[ Corporation, U.S.A., 1~84. 7. ARC Robot Controller Users Manual. Mihailt~ Pupm Institute, Belgrade, Yugoslavia. 1989. 8. Bonner, S., Shin, K. G : A comparative study of robol languages. Computer 15(12): 82 96, 1982. 9. Gini, M.:The future of robot programming. Rohoti~a 5: 235 245, 1987. 10. Lozano-Perez, T.: Robot programming, t'r~,. 1EEl: 71(7): 821 841, 1983. 11. Karan, B., Timotijevic, M., Djurovic, M., Devedzic, V., Josifovic, N.: A system for programming the [!MS-7 industrial robot. Proc. 3rd Soviet Yugoslav Symposium on Robotics and Flexible Manufacturing, Moscow, USSR, pp. 128 135, 1986. 12. Vukobratovic, M., Kircanski, M.: Kinematics and lrajectory Synthesis of Manipulation Robots. Scientific Fun~ damentals of Robotics 3, Berlin, Springer, 1984. 13. Kircanski, M., Vukobratovic, M., Kircanski, N., Tm:cenko, A.: A new program package for the generation of efficient manipulator kinematic and dynamic equations in symbolic form. Robotica 6:311 318, 1988. 14. Vukobratovic, M., Stokic, 1)., Kircanski, N.: loward~ nonadaptive and adaptive control of manipulation robots. IEEE Trans. Automatic Control AC-29(9): 841- 844, 1984.