- Email: [email protected]
Real-Time Multiprocessor Controller for Robot with Vision System
Cop n' i ~ h t
©
IF.-\ C Theon' of Rohots.
\ ·ieIlTl
REAL-TIME MULTIPROCESSOR CONTROLLER FOR ROBOT WITH VISION SYSTEM A. Staszulonek and H. Van Brussel l\atho/il'kl' L'lIil wrsiteit Lfll1'fII , Depurtelllellt Wnktlligklllllfl-, Celtstljlll'll/a(1I1 JOU H. B-JOJO Lel/JlfII (HeJll'riet), Bdgi l/III
Abstract. Design ideas for a real-time control system for a robot provided with a vision system are presented in t hi s work. Functional aspects of the controller are discussed and solution of the particular problems associated is given . A partly model based solution for the system inertia matrix evaluation and an inertially decoupled sliding mode control algorithm are presented. A hardware structure of the r eal - time multiprocessing control system i s proposed and justified. Keywords. Robots; sliding mode control;inertial decoupling; vision system; multiprocessing control . INTRODUCTION AND PROBLEM STATEMENT
control theory point of view, locating and grasping moving objects represents so called continuous path or tracking problems. To specify the continuous path problem we start from the general form of the dynamics equation
Grasping moving objects is a task which in robotics is frequently met but up to now seldom satisfactorily solved. There are several reasons for this. One of the most important is the lack of vision systems fast enough to enable accurate and reliable recognition of objects and the parameters of their motion. As important a reason as the first one is the necessity to apply multiprocessor, multitasking, real-time control systems with software and hardware dedicated to this particular task. Such systems are not readily available. The problems associated with the design of control systems dedicated for robots with vision systems for tracking and grasping moving objects are presented in this work and solutions are proposed. Among the basic requirements which have to be satisfied simultaneously by the control system we may specify the following: - the system must be stable, - high accuracy of positioning and/or tracking is expected, - high transient speed (better than the speed of an overdamped PID controller), - high speed of motion is usually necessary for cost effective applications, - repeatability of trajectories in presence of various disturbance such as changes of load and moments of inertia, - simplicity of design and servicing, - reliabilty, - the control law has to be easy to implement in a real time multiprocessor system, - lack of the overshoots during transients.
A(q)q=B(q,q)+C(q)+T
(1 )
where q is the vector of joint positions, A(q) is the system inertia matrix, B(q,q) is the vector representing Coriolis and centripetal forces, C(q) represents the vector of gravitational forces and T is the vector of control forces and torques applied at the jOints. In case of continuous path control, the desired robot trajectory may be prespecified by its parametric equations or estimated through consecutive measurement of the object's position. The second applies to the robot with vision system under consideration. Assuming that the desired robot trajectory is expressed by qd(t) where T qd (t)=(qld(t),q2d(t), •.• ,qnd(t» n=1, ... ,6 (2) we can define the position and velocity errors as:
{
e(t)=q(t)-q~(t)
(3)
ev( t )=
(4)
Substituting (3) anQl(4) into (1) and premultiplying by A (q) the general dynamic equations (1) can be transformed into following state space representation
The last requirement is a critical one and introduces severe requirements on the quality of the transient process. From the
Our goal is the design of such a control
303
.-\ . Slaszulonek and H . \ 'a ll Brllssel
304
FIL TEW~ AND
-Q
CAAT - JlINT TR .
o
00-;-
Q o
IU1XIf£OOS
I--
,
TRAA~ffil'ATIOO
DI~~EHE NTlATION
1t'Aff:
VISION
I--
SENSOR
PROCESSING
I~(T). Q( T)
CALCULATION OF
r
ROBOT'S VELOCITY
r-
AND FILTER I NG Qo·Qo·Qo
L-.-
AND POS ITION
SYSTEM I NERTI AS A(Q). X(Q)
CALCULAT ION OF POSI TI ON & VELO-C ITY ERRORS
:= . ':::.. --=- ~ '--
-'
//
E. Ev
=> v ~
0
--
I--
~ E COUPLE D
f---
CO N T R 0 L
wl 1_:
MANIPULATOR
Wj
f---
t--~
SYS TEM
w6
FIG.1. FUNCTI ONAL DIAGRAM OF THE CONTROLLER STRUCTURE.
vector T, that beginning from any initial conditions e(to), ev(to) position error e(t). 0 and velocity error ev(t)· 0 if t • Thus the continuous path control problem has been defined. 00
•
First the general functional description of the considered control system will be given and then more detailed solution of the associated problems will follow together with some hardware considerations. FUNCTIONAL STRUCTURE OF THE ROBOT CONTROLLER Suppose that objects to be grasped appear randomly positioned and oriented on the conveyor belt. They have to be first recognised and then their position, orientation and velocity have to be calculated. These functions are performed by the vision system with a single camera attached to the robot arm. The data received from the picture analysis have to be expressed with respect to a fixed reference coordinate frame. Considering that the camera is attached to one of the robot's links and that the pictures are taken when the robot is in motion, we also have to take into account the instant position and velocity of the robot arm. Thus there must exist a direct feedback between the robot motion parameters and vision system as indicated in fig.l. Attaching the camera to the reference coordinate frame would significantly simplify the calculations, however this would have been achieved at the expense of limitation of the space covered by the camera.
the velocity and acceleration of the robot links, and thus of the object have to be known. Therefore position information has to be differentiated twice. The results of the differentiation have to be filtered to reduce the significant amount of the noise introduced into the system by performing differentiation. The final output of these operations is the specification of the desired position, velocity and acceleration. These are the inputs for the servocontrol part of the system. Direct control of the robot servos is performed with the use of a sliding mode type algorithm resulting from the variable structure systems theory (Itkis 1976) . A short description of the algorithm applied is presented in subsequent sections. A separate 16 bit microcomputer based control card is applied to control each degree of freedom of the manipulator. The reason for applying dedicated control cards also follows directly from the theorems establishing necessary and sufficient conditions for sliding mode existence in sampled data variable structure systems (Itkis 1976). However in case of the robot system an inertially decoupling algorithm has to be used to eliminate cross influence of switching of the control signals which is spread in the controller via the system's inertial couplings. For this purpose a new, inertially decoupling algorithm has been developed and is presented below. The functional relations between the specific parts of the control system are presented in fig.l. Mathematical description of the tasks performed by these functional subsystems is given subsequently.
After the picture has been processed by the vision system we receive the information about the position and orientation of the object expressed in Cartesian coordinates. Two operations have to be performed by the control system at this stage. First the information received from vision system has to be filtered and Cartesian to joint coordinate transformation has to be performed .
A large variety of servocontrol methods have been already considered and the results were presented in references (Golla, 1981; Freund, 1982; Van Brussel, 1982). However only linearised state-space control and conventional PID controllers were practically applied.
In case of continuous path problem knowledge of the position and orientation of the object is not enough to control the manipulator. As it can be already seen from equations (5), also information about
A control approach with much potential to satisfy the above listed requirements is the variable structure systems control. Research carried out recently (Staszulonek 1986, 1984, 1985) proves that different
SERVOCONTROL ALGORITHM
COlltroll e r for Robo t \\'ith \ ' isioll Syste m
contro l laws result ing from the variab le struct ure system theory may be succes sfully applie d to contro l indust rial manip ulators . The genera l form of a variab le struct ure contro l law is given by Wi+(e, ev) if eisi(e ,ev) wi=
~O
{ wi-(e, ev) if eisi(e ,ev) < 0
(6)
(7 )
has to be satisf ied for slidin g mode occure nce in the system descri bed by equati on (5). The possib ilities of applic ation of the differ ent contro l laws are discus sed in (Stasz ulonek , 1984) and some of the results of resear ch carrie d out are presen ted in (Stasz ulonek , Van Brusse l, 1986). In this work we use a pulse- width modula ted contro l input descri bed by -ASign (e(t v )s(t V {
»
if
tv
if t v + 0ft5..t v +l
o
(8 )
where e i is the linear functio n with satura tion given by ei ={ki1e i(t v )1 if
kilei(tv )l~l
if kilei( tv)l>l
t
(9)
INERTIA L DECOUPLING
(10)
is not possib le. The reason for this is the presen ce of the inerti al cross coupli ngs in the system . Theref ore instea d of direct applic ation of (10) we introd uce an inerti ally decoup ling contro l law in the form (11) T=(l-X )w where w is the contro l input define d by (8), (9) and X is the inerti ally decoup ling matrix whose compon ents are chosen so, that below presen ted condit ion
[r::::J dnnwn
where * dij =
{o
(14) (i=l, ... ,n)
for
i=j,
for
i=j, (j=l, ... ,n)
(15)
is satisf ied. The compon ents of the decoup ling matrix X can be algebr aically calcul ated from (14). Indeed premu ltiplyi ng (14) by the system inerti a matrix A we obtain a solutio n in the form X=A(e (t)+qd (t»D * (e(t)+q d(t»
(16)
It should not be forgot ten that the genera l condit ion for slidin g motion occure nce (7) has to be still satisf ied which leads to the solutio n presen ted in (Stasz ulonek , Van Brusse l, 1986). As it can be seen, to obtain the solutio n for the decoup ling matrix X we have to perform real-ti me system inerti a matrix evalua tion and invers ion. Since these operat ions have to be perform ed in floatin g point arithm etics, this task repres ents signif icant data proces sing load. Theref ore, a dedica ted microc ompute r card with floatin g point coproc essor has to be used. After the soluti on is found the result s have to be transfo rmed from real to intege r values to enable fast calcul ation of the decoup led contro l vector T acordi ngly to equati on To perform real-ti me system inerti as matrix evalua tion we use the model based proced ure which calcul ates the compon ents of the system inerti a matrix in the form T
a Ep a Ep Tr(--- -J p -----) aij= a qi a qj p=max i,j n E
(17)
where Tr is the matrix trace operat or, Ep descri bes positio n and orient ation of the link p in base coordi nates and (18 )
The contro l law (8), (9) gives very good result s in case of a one link system . In case of multid egree of freedom system s the direct applic ation of contro l
D(l-X) =
dijXij =dij *
(11).
where i=l, •.• ,n; v =O,l, •... , t is the sampli ng interv al and t v+l=tv +T . The contro l law (8), (9) is partic ularly suitab le for applic ation in real-ti me microp rocess or based contro ller and the experi ments carrie d out with the 16 bit Motoro la 68000 system fully proved its applic ability .
T=w
(13)
or the equiva lent condit ion
dij
where wi are the contro l torque s or momen ts and si define the switch ing hyperp lanes. It should be noted here that slidin g mode contro llers are the most suitab le from the class of variab le struct ure system s. Theref ore the follow ing necess ary and suffic ient condit ion given by
Wi=
D=A- l
where
(12)
where Ai are 4x4 matric es descri bing the positio n and orient ation of the link i with respec t to the link i-I, Jp is the inerti a tensor with respec t to the proxim al joint of link p in p'S body coordi nates. Since the system inerti a matrix is symme tric it is suffic ient to calcul ate only the diagon al and upper or lower off diagon al compon ents of matrix A, thus the number of the program loops equals 1 nr=--- n(n-l)+ n (19) 2 It is also to the great advant age of slidin g mode contro l that we do not have to formul ate fully the dynami c model of the system to perform the contro l. It is enough to know the kinema tic equati ons and to calcul ate the system inerti as matrix . Thus the proces sing load has been signif icantl y decrea sed and the real-ti me, partly model based contro l become s more
306
.\ . Slas llll o n e k a nd H . Va n Brllssel
realistic, particularly if a dedicated single board microcomputer with floating point coprocessor is to be used. CARTESIAN TO JOINT COORDINATE TRANSFORMATION As it has already been mentioned in previous sections the vision system supplies data in Cartesian coordinates while the robot servocontrol system always operates in joint coordinates. Therefore the solution of the inverse kinematic problem in real-time has to be found. Several possible solutions have been presented in (Van Aken, Van Brussel, 1984). Among those methods we can mention explicit and i mplicit solution, linearisation around the working point and table look-up procedures. The last methods could be the fastest ones but since they require very large computer memory we do not consider them suitable for the application . Instead we chose the most general of the above mentioned methods e.g. solution by linearisation around the working point. This solution has been fully discussed in (Van Aken, Van Brussel, 1984), and therefore only a short outline is presented below. The data output by the vision system describes the desired location of the end effector. This means that the position and orientation of the coordinate system associated with the end effector is known . This can be represented by means of a 4x4 homogeneous matrix in+l'X jn+l,x kn+l,x sn+l'X~ En= ~n+l,y ~n+l,y kn+l,y Sn+l,y In+l,z kn+l,z Sn+l,z [ 1n+l,z o 0 0 1
(20)
On the other hand, having fixed reference frame associated to the robot we may express the end effector position in the form
~
~2l(q)
all(q) a12(q) a13(q) a14(q) AlA2 · ···· An= a22(q) a23(q) a24(q) a3l(q) a32(q) a33(q) a34(q)1 000 1 ~ (21)
Equating (20) and (21) we receive a set of 12 equations. From those equations only 6 can be independent. Three of these independent equations represent translational part and remaining three represent rotational part of the homoge-neouse transformation . This set of equations has to be solved to obtain manipulator joint position. Linearisation around the working point has been chosen to obtain the solution. The matrices Ai can be approximated by the Taylor series around the initial configuration a Ai(qi) Ai(qi+ 6 qi)=
AiO(qi)+-------- .~i+
. ••
(22)
o qi introducing symbol Ai for Ai(qi) we obtain
E6=(AlO+B1 6q1)·(A20+ B26q2) · ••. ·(A60+ B6 6q6)=AlOA20 · ••• ·A60+BlA20A30·····A60 ~ ql+ +AlOB2 A30····· B60 6 q2+ + ... +AlOA20····· A50B6 6 q6= =CO+C1 6ql+C2 6q2+ ... +C6 6 q6 where CO=AlOA20· ... ·A60 Ci=AlO·····Ai-l,OBiAi+l,O. • .. · A60
(25) (26) (27)
Rewriting (25) in the form 6 i=l
(28)
we receive a set of 12 equations of which six independent equations can be chosen 6 (E6-CO)14=( r Ci 6qi)14 (29) i=l 6 (E6- CO)24 = ( r Ci 6qi)24 (30) i=l 6 (31) (E6- CO)34=( i: Ci 6qi)34 i=l 6 (32) (E6- Co1l2=( r Ci 6qi1l2 i=l 6 (33) (E6-CO)13=( r Ci 6qi1l3 i=l 6 (34) (E6-CO)23=( L Ci6qi)23 i=l Equations (29)-(34) represent a set of linear equations which may be solved for qi by the appropriate numerical method . The general method proposed here is the most computing time consuming but accordingly to the results of research project no. 3930 sponsored by IWONL (Instituut voor Wetenschappelijk Onderzoek in Nijverheid en Landbouw) it may still satisfy real-time system requirements, particularly if some preprocessing will be introduced. It is desirable that also coordinate transformation would be performed by the dedicated single board computer. Each time a new set of the desired end effector location has been calculated it has to be converted into integer values and placed into proper sys-tem memory location so that new values of the position error may be calculated . After the inverse kinematic problem has been solved and new values of the desired position found, we have to calculate desired velOCity and acceleration . This is done by numerical differentiation. The results of the differentiation have to be filtered because of the substantial amount of noise introduced into the system by this operation and the vision system as well. To solve this problem an approach similar to that presented in (Katupitiya, 1985) has been adopted.
6. VISION SYSTEM. Ai=AiO+ Bi 6qi
(23)
where oAi Bi= oqi
(24)
The desired end-effector location expressed by (21) can be presented in the form
For this work the commercially available system IeOS 20000 has been chosen, the block diagram of which is presented in fig.2. The camera yields an analog signal which is transformed by the analog - todigital converter into 8 bit binary word since this is the format determined by the sensor controller module (fig.2). Images
307
Controller for Robot " 'ith \'isiol1 S\'Stem
of very high resolution are required for robotics applications as well as an 8 bit grey scale is preferred. Since these images of a high data content are to be processed at high speed, the resulting data throughput is very high. However one important fact can be exploited: there exist in general some redundancy in the image data content. Therefore feature extraction algorithms are implemented in order to reduce the quantity of information. This operation is performed in the analytic processor module where the amount of information can be reduced by a factor 100 to 1000. The set of reduced data, generated by the analytic processor is
~
V I S ION
SENSOR
S ENS 0 R
COfmOLLER
I
~J
Z A
ANAL YTI C PROCESSOR
"-
K.-
ME M0 R Y
-V
~.)
~)'
MONOBOARD COMPUTER
8. CONCLUSIONS.
~ OUTPUT TO THE CONTROLLER
FIG.2. BLOCK DIAGRAM OF THE VISION SYSTEM. directly accessible by the monoboard computer performing the reduced image interpretation. HARDWARE STRUCTURE As it can be noticed by comparing fig. 1 and fig. 3 the general concept of the controller's hardware structure reflects the functional structure presented in fig.l. Two levels of the control are
PRIMARY MASTER
SECOND. MASTER
<:ll I tMX BU~ I \ I
~
rMEBUS
VISION SYSTEM
I
SYST~M r1EMORY
present in the system. At the lower level only servomechanism control is performed using the inertially decoupled sliding mode control. The calculations performed at this level are only of the integer type. Thus the Master DeCoupLer (MDCL) receives from the Primary Master via the VMX bus updated values of the inertially decoupling matrix and calculates the decoupled control vector T. Then the appropriate components of this vector are transferred to the individual ServoControl Boards (SCB). The communication between the MDCL and each SCB is performed via the standard 32 bit VME bus. At the higher level the floating point arithmetic is performed. This part of the system should be built around both the VME and VMX buses as well, so that information exchange does not slow down the processors and the interrupt system simplifies. Access to the local extension bus (VMX) is arbitrated by the Primary Master calculating also in real-time, model based system inertia matrix and decoupling matrix components as well. The task of the second master is the Cartesian to joint coordinate transformation, desired position differentiation and filtering. The vision system presented in fig.2 is connected to the controller via the VME bus. Since both masters perform relatively sophisticated calculations and for the real-time application the speed of calculations is essential it is desired, as it also has been concluded in (Nigam, Lee, 1985), that 32 bit Motorola 68020 based monoboard computers with floating point extension are to be used.
MASTER DCl
The inertially decoupling algorithm presented in this work can be implemented in the system with parallel processing thus giving the opportunity to increase the speed of the system which is the essential factor in sliding mode controller design. Real time calculation of the system inertia matrix is implemented through the use of a modelbased procedure represented by the equation (17), and it should be noted here that we do not have to calculate the full model of the robot dynamics. Only the system inertias matrix has to be calculated, thus a significant shortcut in the computational load is achieved. This
SCB
SCB
SCB
1
2
3
SCB
SCB
SCB
4
5
6
I
~
VME
V
BUS
FIG.3. BLOCK DIAGRAM OF THE CONTROllER'S HARDWARE STRUCTURE.
A. Staszlllone k a nd H . Va n Brllssel
:~ 0 8
advantage is the direct result of applying the sliding mode control algorithm. Computer simulations of the presented ideas have been carried out and their applicability has been fully confirmed. Some experiments with pulse-width modulated sliding motion servocontrollers (SBC) based on 16 bit Motorola 68000 microprocessors were carried out and very good results were obtained in case of set pOint control as well as for continuous path control. The details of the higher level of the controller presented in this work are presently in the stage of development in the Robotics Laboratory of the K.U. Leuven. REFERENCES Angeles, J. (1986) Iterative Kinematic Inversion of General Five-Axis Robot Manipulators. The International J. of Robotics Research. Vol. 4, No. 4, MIT. Freund, E.(1982) Fast Nonlinear Control With Arbitrary Pole Placement for Industrial Robots and Manipulators. Int. J. of Robotics Research, 1,1 pp. 65-78 Golla, D.F., C.S. Garg, and P.C . Hughes, (1981). Linear State Feedback Control of Manipulators. Mechanical Machine Tcheory, No. 16, pp. 93-101 . Hollerbach, J.M. (1980). A Recursive Lagrangian Formulation of Manipulator Dynamics and a Comparative Study of Formulation Complexity. IEEE Trans on Systems, Man and Cybernetics, vol. SMC-lO, no. 11. Itkis, u. (1976) Control Systems of Variable Structure, Keter Publishing House, Jerusalem Ltd. Katupitiya, J. (1985). Vision Assisted Tracking and Time Optimal Acquisition of Moving Objects Using a Manipulator Ph.D. Thesis, K.U. Leuven, Afdeling Mechanische Konstruktie en Produktie. Nigam, R., G.C.S. Lee, (1985). A Multiprocessor-Based Controller for the Control of Mechanical Manipulators. IEEE Journal of Robotics Research and Automation, Vol. RA-I, No. 4 Paul, R.P. (1981). Robot Manipulators: Mathematics, Programing, and Control. MIT Press, Cambridge, Ma. Staszulonek, A., H. Van Brussel, (1986). Inertially Decoupled, Sliding Mode Controller Design for Trace and Pick-Up Robot. Proc. of The 16th International Symposium on Industrial Robots. Brussels. Staszulonek, A. (1984). Perspectives of Variable Structure Systems Application in Robot Controller. Proc . of The 1st World Conference on Robotics Research, MS 84-497, Bethlehem, Pa. USA. Staszulonek, A. (1985) . Projektowanie i badanie symulacyjne sterowania robotow przemyslowych. Ph.D. Thesis. Instytut Automatyki, Silesian Technical University, Gliwice. Van Aken, L., H. Van Brussel, (1984). Software for Solving the Inverse Kinematic Problem for Robots and Manipulators in Real Time, in Danthine A. and Geradin M. (eds) Advanced Software in Robotics, Elsevier Science Publishers B.V. (North Holland).
Van Brussel, H., J. Simons, and J. Deschutter, (1982). An Intelligent Force Controlled Robot. Annals of the CIRP, vol. 31/1, pp. 391-396.
©
IF.-\ C Theon' of Rohots.
\ ·ieIlTl
REAL-TIME MULTIPROCESSOR CONTROLLER FOR ROBOT WITH VISION SYSTEM A. Staszulonek and H. Van Brussel l\atho/il'kl' L'lIil wrsiteit Lfll1'fII , Depurtelllellt Wnktlligklllllfl-, Celtstljlll'll/a(1I1 JOU H. B-JOJO Lel/JlfII (HeJll'riet), Bdgi l/III
Abstract. Design ideas for a real-time control system for a robot provided with a vision system are presented in t hi s work. Functional aspects of the controller are discussed and solution of the particular problems associated is given . A partly model based solution for the system inertia matrix evaluation and an inertially decoupled sliding mode control algorithm are presented. A hardware structure of the r eal - time multiprocessing control system i s proposed and justified. Keywords. Robots; sliding mode control;inertial decoupling; vision system; multiprocessing control . INTRODUCTION AND PROBLEM STATEMENT
control theory point of view, locating and grasping moving objects represents so called continuous path or tracking problems. To specify the continuous path problem we start from the general form of the dynamics equation
Grasping moving objects is a task which in robotics is frequently met but up to now seldom satisfactorily solved. There are several reasons for this. One of the most important is the lack of vision systems fast enough to enable accurate and reliable recognition of objects and the parameters of their motion. As important a reason as the first one is the necessity to apply multiprocessor, multitasking, real-time control systems with software and hardware dedicated to this particular task. Such systems are not readily available. The problems associated with the design of control systems dedicated for robots with vision systems for tracking and grasping moving objects are presented in this work and solutions are proposed. Among the basic requirements which have to be satisfied simultaneously by the control system we may specify the following: - the system must be stable, - high accuracy of positioning and/or tracking is expected, - high transient speed (better than the speed of an overdamped PID controller), - high speed of motion is usually necessary for cost effective applications, - repeatability of trajectories in presence of various disturbance such as changes of load and moments of inertia, - simplicity of design and servicing, - reliabilty, - the control law has to be easy to implement in a real time multiprocessor system, - lack of the overshoots during transients.
A(q)q=B(q,q)+C(q)+T
(1 )
where q is the vector of joint positions, A(q) is the system inertia matrix, B(q,q) is the vector representing Coriolis and centripetal forces, C(q) represents the vector of gravitational forces and T is the vector of control forces and torques applied at the jOints. In case of continuous path control, the desired robot trajectory may be prespecified by its parametric equations or estimated through consecutive measurement of the object's position. The second applies to the robot with vision system under consideration. Assuming that the desired robot trajectory is expressed by qd(t) where T qd (t)=(qld(t),q2d(t), •.• ,qnd(t» n=1, ... ,6 (2) we can define the position and velocity errors as:
{
e(t)=q(t)-q~(t)
(3)
ev( t )=
(4)
Substituting (3) anQl(4) into (1) and premultiplying by A (q) the general dynamic equations (1) can be transformed into following state space representation
The last requirement is a critical one and introduces severe requirements on the quality of the transient process. From the
Our goal is the design of such a control
303
.-\ . Slaszulonek and H . \ 'a ll Brllssel
304
FIL TEW~ AND
-Q
CAAT - JlINT TR .
o
00-;-
Q o
IU1XIf£OOS
I--
,
TRAA~ffil'ATIOO
DI~~EHE NTlATION
1t'Aff:
VISION
I--
SENSOR
PROCESSING
I~(T). Q( T)
CALCULATION OF
r
ROBOT'S VELOCITY
r-
AND FILTER I NG Qo·Qo·Qo
L-.-
AND POS ITION
SYSTEM I NERTI AS A(Q). X(Q)
CALCULAT ION OF POSI TI ON & VELO-C ITY ERRORS
:= . ':::.. --=- ~ '--
-'
//
E. Ev
=> v ~
0
--
I--
~ E COUPLE D
f---
CO N T R 0 L
wl 1_:
MANIPULATOR
Wj
f---
t--~
SYS TEM
w6
FIG.1. FUNCTI ONAL DIAGRAM OF THE CONTROLLER STRUCTURE.
vector T, that beginning from any initial conditions e(to), ev(to) position error e(t). 0 and velocity error ev(t)· 0 if t • Thus the continuous path control problem has been defined. 00
•
First the general functional description of the considered control system will be given and then more detailed solution of the associated problems will follow together with some hardware considerations. FUNCTIONAL STRUCTURE OF THE ROBOT CONTROLLER Suppose that objects to be grasped appear randomly positioned and oriented on the conveyor belt. They have to be first recognised and then their position, orientation and velocity have to be calculated. These functions are performed by the vision system with a single camera attached to the robot arm. The data received from the picture analysis have to be expressed with respect to a fixed reference coordinate frame. Considering that the camera is attached to one of the robot's links and that the pictures are taken when the robot is in motion, we also have to take into account the instant position and velocity of the robot arm. Thus there must exist a direct feedback between the robot motion parameters and vision system as indicated in fig.l. Attaching the camera to the reference coordinate frame would significantly simplify the calculations, however this would have been achieved at the expense of limitation of the space covered by the camera.
the velocity and acceleration of the robot links, and thus of the object have to be known. Therefore position information has to be differentiated twice. The results of the differentiation have to be filtered to reduce the significant amount of the noise introduced into the system by performing differentiation. The final output of these operations is the specification of the desired position, velocity and acceleration. These are the inputs for the servocontrol part of the system. Direct control of the robot servos is performed with the use of a sliding mode type algorithm resulting from the variable structure systems theory (Itkis 1976) . A short description of the algorithm applied is presented in subsequent sections. A separate 16 bit microcomputer based control card is applied to control each degree of freedom of the manipulator. The reason for applying dedicated control cards also follows directly from the theorems establishing necessary and sufficient conditions for sliding mode existence in sampled data variable structure systems (Itkis 1976). However in case of the robot system an inertially decoupling algorithm has to be used to eliminate cross influence of switching of the control signals which is spread in the controller via the system's inertial couplings. For this purpose a new, inertially decoupling algorithm has been developed and is presented below. The functional relations between the specific parts of the control system are presented in fig.l. Mathematical description of the tasks performed by these functional subsystems is given subsequently.
After the picture has been processed by the vision system we receive the information about the position and orientation of the object expressed in Cartesian coordinates. Two operations have to be performed by the control system at this stage. First the information received from vision system has to be filtered and Cartesian to joint coordinate transformation has to be performed .
A large variety of servocontrol methods have been already considered and the results were presented in references (Golla, 1981; Freund, 1982; Van Brussel, 1982). However only linearised state-space control and conventional PID controllers were practically applied.
In case of continuous path problem knowledge of the position and orientation of the object is not enough to control the manipulator. As it can be already seen from equations (5), also information about
A control approach with much potential to satisfy the above listed requirements is the variable structure systems control. Research carried out recently (Staszulonek 1986, 1984, 1985) proves that different
SERVOCONTROL ALGORITHM
COlltroll e r for Robo t \\'ith \ ' isioll Syste m
contro l laws result ing from the variab le struct ure system theory may be succes sfully applie d to contro l indust rial manip ulators . The genera l form of a variab le struct ure contro l law is given by Wi+(e, ev) if eisi(e ,ev) wi=
~O
{ wi-(e, ev) if eisi(e ,ev) < 0
(6)
(7 )
has to be satisf ied for slidin g mode occure nce in the system descri bed by equati on (5). The possib ilities of applic ation of the differ ent contro l laws are discus sed in (Stasz ulonek , 1984) and some of the results of resear ch carrie d out are presen ted in (Stasz ulonek , Van Brusse l, 1986). In this work we use a pulse- width modula ted contro l input descri bed by -ASign (e(t v )s(t V {
»
if
tv
if t v + 0ft5..t v +l
o
(8 )
where e i is the linear functio n with satura tion given by ei ={ki1e i(t v )1 if
kilei(tv )l~l
if kilei( tv)l>l
t
(9)
INERTIA L DECOUPLING
(10)
is not possib le. The reason for this is the presen ce of the inerti al cross coupli ngs in the system . Theref ore instea d of direct applic ation of (10) we introd uce an inerti ally decoup ling contro l law in the form (11) T=(l-X )w where w is the contro l input define d by (8), (9) and X is the inerti ally decoup ling matrix whose compon ents are chosen so, that below presen ted condit ion
[r::::J dnnwn
where * dij =
{o
(14) (i=l, ... ,n)
for
i=j,
for
i=j, (j=l, ... ,n)
(15)
is satisf ied. The compon ents of the decoup ling matrix X can be algebr aically calcul ated from (14). Indeed premu ltiplyi ng (14) by the system inerti a matrix A we obtain a solutio n in the form X=A(e (t)+qd (t»D * (e(t)+q d(t»
(16)
It should not be forgot ten that the genera l condit ion for slidin g motion occure nce (7) has to be still satisf ied which leads to the solutio n presen ted in (Stasz ulonek , Van Brusse l, 1986). As it can be seen, to obtain the solutio n for the decoup ling matrix X we have to perform real-ti me system inerti a matrix evalua tion and invers ion. Since these operat ions have to be perform ed in floatin g point arithm etics, this task repres ents signif icant data proces sing load. Theref ore, a dedica ted microc ompute r card with floatin g point coproc essor has to be used. After the soluti on is found the result s have to be transfo rmed from real to intege r values to enable fast calcul ation of the decoup led contro l vector T acordi ngly to equati on To perform real-ti me system inerti as matrix evalua tion we use the model based proced ure which calcul ates the compon ents of the system inerti a matrix in the form T
a Ep a Ep Tr(--- -J p -----) aij= a qi a qj p=max i,j n E
(17)
where Tr is the matrix trace operat or, Ep descri bes positio n and orient ation of the link p in base coordi nates and (18 )
The contro l law (8), (9) gives very good result s in case of a one link system . In case of multid egree of freedom system s the direct applic ation of contro l
D(l-X) =
dijXij =dij *
(11).
where i=l, •.• ,n; v =O,l, •... , t is the sampli ng interv al and t v+l=tv +T . The contro l law (8), (9) is partic ularly suitab le for applic ation in real-ti me microp rocess or based contro ller and the experi ments carrie d out with the 16 bit Motoro la 68000 system fully proved its applic ability .
T=w
(13)
or the equiva lent condit ion
dij
where wi are the contro l torque s or momen ts and si define the switch ing hyperp lanes. It should be noted here that slidin g mode contro llers are the most suitab le from the class of variab le struct ure system s. Theref ore the follow ing necess ary and suffic ient condit ion given by
Wi=
D=A- l
where
(12)
where Ai are 4x4 matric es descri bing the positio n and orient ation of the link i with respec t to the link i-I, Jp is the inerti a tensor with respec t to the proxim al joint of link p in p'S body coordi nates. Since the system inerti a matrix is symme tric it is suffic ient to calcul ate only the diagon al and upper or lower off diagon al compon ents of matrix A, thus the number of the program loops equals 1 nr=--- n(n-l)+ n (19) 2 It is also to the great advant age of slidin g mode contro l that we do not have to formul ate fully the dynami c model of the system to perform the contro l. It is enough to know the kinema tic equati ons and to calcul ate the system inerti as matrix . Thus the proces sing load has been signif icantl y decrea sed and the real-ti me, partly model based contro l become s more
306
.\ . Slas llll o n e k a nd H . Va n Brllssel
realistic, particularly if a dedicated single board microcomputer with floating point coprocessor is to be used. CARTESIAN TO JOINT COORDINATE TRANSFORMATION As it has already been mentioned in previous sections the vision system supplies data in Cartesian coordinates while the robot servocontrol system always operates in joint coordinates. Therefore the solution of the inverse kinematic problem in real-time has to be found. Several possible solutions have been presented in (Van Aken, Van Brussel, 1984). Among those methods we can mention explicit and i mplicit solution, linearisation around the working point and table look-up procedures. The last methods could be the fastest ones but since they require very large computer memory we do not consider them suitable for the application . Instead we chose the most general of the above mentioned methods e.g. solution by linearisation around the working point. This solution has been fully discussed in (Van Aken, Van Brussel, 1984), and therefore only a short outline is presented below. The data output by the vision system describes the desired location of the end effector. This means that the position and orientation of the coordinate system associated with the end effector is known . This can be represented by means of a 4x4 homogeneous matrix in+l'X jn+l,x kn+l,x sn+l'X~ En= ~n+l,y ~n+l,y kn+l,y Sn+l,y In+l,z kn+l,z Sn+l,z [ 1n+l,z o 0 0 1
(20)
On the other hand, having fixed reference frame associated to the robot we may express the end effector position in the form
~
~2l(q)
all(q) a12(q) a13(q) a14(q) AlA2 · ···· An= a22(q) a23(q) a24(q) a3l(q) a32(q) a33(q) a34(q)1 000 1 ~ (21)
Equating (20) and (21) we receive a set of 12 equations. From those equations only 6 can be independent. Three of these independent equations represent translational part and remaining three represent rotational part of the homoge-neouse transformation . This set of equations has to be solved to obtain manipulator joint position. Linearisation around the working point has been chosen to obtain the solution. The matrices Ai can be approximated by the Taylor series around the initial configuration a Ai(qi) Ai(qi+ 6 qi)=
AiO(qi)+-------- .~i+
. ••
(22)
o qi introducing symbol Ai for Ai(qi) we obtain
E6=(AlO+B1 6q1)·(A20+ B26q2) · ••. ·(A60+ B6 6q6)=AlOA20 · ••• ·A60+BlA20A30·····A60 ~ ql+ +AlOB2 A30····· B60 6 q2+ + ... +AlOA20····· A50B6 6 q6= =CO+C1 6ql+C2 6q2+ ... +C6 6 q6 where CO=AlOA20· ... ·A60 Ci=AlO·····Ai-l,OBiAi+l,O. • .. · A60
(25) (26) (27)
Rewriting (25) in the form 6 i=l
(28)
we receive a set of 12 equations of which six independent equations can be chosen 6 (E6-CO)14=( r Ci 6qi)14 (29) i=l 6 (E6- CO)24 = ( r Ci 6qi)24 (30) i=l 6 (31) (E6- CO)34=( i: Ci 6qi)34 i=l 6 (32) (E6- Co1l2=( r Ci 6qi1l2 i=l 6 (33) (E6-CO)13=( r Ci 6qi1l3 i=l 6 (34) (E6-CO)23=( L Ci6qi)23 i=l Equations (29)-(34) represent a set of linear equations which may be solved for qi by the appropriate numerical method . The general method proposed here is the most computing time consuming but accordingly to the results of research project no. 3930 sponsored by IWONL (Instituut voor Wetenschappelijk Onderzoek in Nijverheid en Landbouw) it may still satisfy real-time system requirements, particularly if some preprocessing will be introduced. It is desirable that also coordinate transformation would be performed by the dedicated single board computer. Each time a new set of the desired end effector location has been calculated it has to be converted into integer values and placed into proper sys-tem memory location so that new values of the position error may be calculated . After the inverse kinematic problem has been solved and new values of the desired position found, we have to calculate desired velOCity and acceleration . This is done by numerical differentiation. The results of the differentiation have to be filtered because of the substantial amount of noise introduced into the system by this operation and the vision system as well. To solve this problem an approach similar to that presented in (Katupitiya, 1985) has been adopted.
6. VISION SYSTEM. Ai=AiO+ Bi 6qi
(23)
where oAi Bi= oqi
(24)
The desired end-effector location expressed by (21) can be presented in the form
For this work the commercially available system IeOS 20000 has been chosen, the block diagram of which is presented in fig.2. The camera yields an analog signal which is transformed by the analog - todigital converter into 8 bit binary word since this is the format determined by the sensor controller module (fig.2). Images
307
Controller for Robot " 'ith \'isiol1 S\'Stem
of very high resolution are required for robotics applications as well as an 8 bit grey scale is preferred. Since these images of a high data content are to be processed at high speed, the resulting data throughput is very high. However one important fact can be exploited: there exist in general some redundancy in the image data content. Therefore feature extraction algorithms are implemented in order to reduce the quantity of information. This operation is performed in the analytic processor module where the amount of information can be reduced by a factor 100 to 1000. The set of reduced data, generated by the analytic processor is
~
V I S ION
SENSOR
S ENS 0 R
COfmOLLER
I
~J
Z A
ANAL YTI C PROCESSOR
"-
K.-
ME M0 R Y
-V
~.)
~)'
MONOBOARD COMPUTER
8. CONCLUSIONS.
~ OUTPUT TO THE CONTROLLER
FIG.2. BLOCK DIAGRAM OF THE VISION SYSTEM. directly accessible by the monoboard computer performing the reduced image interpretation. HARDWARE STRUCTURE As it can be noticed by comparing fig. 1 and fig. 3 the general concept of the controller's hardware structure reflects the functional structure presented in fig.l. Two levels of the control are
PRIMARY MASTER
SECOND. MASTER
<:ll I tMX BU~ I \ I
~
rMEBUS
VISION SYSTEM
I
SYST~M r1EMORY
present in the system. At the lower level only servomechanism control is performed using the inertially decoupled sliding mode control. The calculations performed at this level are only of the integer type. Thus the Master DeCoupLer (MDCL) receives from the Primary Master via the VMX bus updated values of the inertially decoupling matrix and calculates the decoupled control vector T. Then the appropriate components of this vector are transferred to the individual ServoControl Boards (SCB). The communication between the MDCL and each SCB is performed via the standard 32 bit VME bus. At the higher level the floating point arithmetic is performed. This part of the system should be built around both the VME and VMX buses as well, so that information exchange does not slow down the processors and the interrupt system simplifies. Access to the local extension bus (VMX) is arbitrated by the Primary Master calculating also in real-time, model based system inertia matrix and decoupling matrix components as well. The task of the second master is the Cartesian to joint coordinate transformation, desired position differentiation and filtering. The vision system presented in fig.2 is connected to the controller via the VME bus. Since both masters perform relatively sophisticated calculations and for the real-time application the speed of calculations is essential it is desired, as it also has been concluded in (Nigam, Lee, 1985), that 32 bit Motorola 68020 based monoboard computers with floating point extension are to be used.
MASTER DCl
The inertially decoupling algorithm presented in this work can be implemented in the system with parallel processing thus giving the opportunity to increase the speed of the system which is the essential factor in sliding mode controller design. Real time calculation of the system inertia matrix is implemented through the use of a modelbased procedure represented by the equation (17), and it should be noted here that we do not have to calculate the full model of the robot dynamics. Only the system inertias matrix has to be calculated, thus a significant shortcut in the computational load is achieved. This
SCB
SCB
SCB
1
2
3
SCB
SCB
SCB
4
5
6
I
~
VME
V
BUS
FIG.3. BLOCK DIAGRAM OF THE CONTROllER'S HARDWARE STRUCTURE.
A. Staszlllone k a nd H . Va n Brllssel
:~ 0 8
advantage is the direct result of applying the sliding mode control algorithm. Computer simulations of the presented ideas have been carried out and their applicability has been fully confirmed. Some experiments with pulse-width modulated sliding motion servocontrollers (SBC) based on 16 bit Motorola 68000 microprocessors were carried out and very good results were obtained in case of set pOint control as well as for continuous path control. The details of the higher level of the controller presented in this work are presently in the stage of development in the Robotics Laboratory of the K.U. Leuven. REFERENCES Angeles, J. (1986) Iterative Kinematic Inversion of General Five-Axis Robot Manipulators. The International J. of Robotics Research. Vol. 4, No. 4, MIT. Freund, E.(1982) Fast Nonlinear Control With Arbitrary Pole Placement for Industrial Robots and Manipulators. Int. J. of Robotics Research, 1,1 pp. 65-78 Golla, D.F., C.S. Garg, and P.C . Hughes, (1981). Linear State Feedback Control of Manipulators. Mechanical Machine Tcheory, No. 16, pp. 93-101 . Hollerbach, J.M. (1980). A Recursive Lagrangian Formulation of Manipulator Dynamics and a Comparative Study of Formulation Complexity. IEEE Trans on Systems, Man and Cybernetics, vol. SMC-lO, no. 11. Itkis, u. (1976) Control Systems of Variable Structure, Keter Publishing House, Jerusalem Ltd. Katupitiya, J. (1985). Vision Assisted Tracking and Time Optimal Acquisition of Moving Objects Using a Manipulator Ph.D. Thesis, K.U. Leuven, Afdeling Mechanische Konstruktie en Produktie. Nigam, R., G.C.S. Lee, (1985). A Multiprocessor-Based Controller for the Control of Mechanical Manipulators. IEEE Journal of Robotics Research and Automation, Vol. RA-I, No. 4 Paul, R.P. (1981). Robot Manipulators: Mathematics, Programing, and Control. MIT Press, Cambridge, Ma. Staszulonek, A., H. Van Brussel, (1986). Inertially Decoupled, Sliding Mode Controller Design for Trace and Pick-Up Robot. Proc. of The 16th International Symposium on Industrial Robots. Brussels. Staszulonek, A. (1984). Perspectives of Variable Structure Systems Application in Robot Controller. Proc . of The 1st World Conference on Robotics Research, MS 84-497, Bethlehem, Pa. USA. Staszulonek, A. (1985) . Projektowanie i badanie symulacyjne sterowania robotow przemyslowych. Ph.D. Thesis. Instytut Automatyki, Silesian Technical University, Gliwice. Van Aken, L., H. Van Brussel, (1984). Software for Solving the Inverse Kinematic Problem for Robots and Manipulators in Real Time, in Danthine A. and Geradin M. (eds) Advanced Software in Robotics, Elsevier Science Publishers B.V. (North Holland).
Van Brussel, H., J. Simons, and J. Deschutter, (1982). An Intelligent Force Controlled Robot. Annals of the CIRP, vol. 31/1, pp. 391-396.