Abstract
Industrial applications demand that robots operate in agreement with the position and orientation of their end effector. It is necessary to solve the kinematics inverse problem. This allows the displacement of the joints of the manipulator to be determined, to accomplish a given objective. Complete studies of dynamical control of joint robotics are also necessary. Initially, this article focuses on the implementation of numerical algorithms for the solution of the kinematics inverse problem and the modeling and simulation of dynamic systems. This is done using real time implementation. The modeling and simulation of dynamic systems are performed emphasizing offline programming. In sequence, a complete study of the control strategies is carried out through the study of several elements of a robotic joint, such as: DC motor, inertia, and gearbox. Finally a trajectory generator, used as input for a generic group of joints, is developed and a proposal of the controller's implementation of joints, using EPLD development system, is presented.
Kinematics; modeling; dynamical control
Proposal of methodology for the modeling and control of manipulators
J. M. Rosário; C. de Oliveira; C. E. A. de Sá^{I}; C. R. E. Lima^{II}
^{I}UNICAMP State University of Campinas Department of Mechanical Engineering 13083970 Campinas, SP. Brazil
^{II}Department of Electronics CEFET PR. Brazil
ABSTRACT
Industrial applications demand that robots operate in agreement with the position and orientation of their end effector. It is necessary to solve the kinematics inverse problem. This allows the displacement of the joints of the manipulator to be determined, to accomplish a given objective. Complete studies of dynamical control of joint robotics are also necessary.
Initially, this article focuses on the implementation of numerical algorithms for the solution of the kinematics inverse problem and the modeling and simulation of dynamic systems. This is done using real time implementation. The modeling and simulation of dynamic systems are performed emphasizing offline programming.
In sequence, a complete study of the control strategies is carried out through the study of several elements of a robotic joint, such as: DC motor, inertia, and gearbox. Finally a trajectory generator, used as input for a generic group of joints, is developed and a proposal of the controller's implementation of joints, using EPLD development system, is presented.
Keywords: Kinematics, modeling, dynamical control
Introduction
Nowadays, the number of robots installed in industry increases progressively, because of their capacity to carry out operations that demand flexibility, speed and precision.
In most industrial applications, robot tasks are programmed by learning without the need of a geometrical model. In this way, their trajectory is defined through a set of angles associated to the angular movement of each degree of freedom of the robot, which after interpolation by an algorithm, will act as a reference signal for positioning controllers, located at each joint, and then compare the signals deriving from the position sensors of the joints.
For many operations, the operator defines the tasks or reference movements of the controller with regards a coordinate system that is fixed in the end effector of the robot (in Cartesian space), Fig. 1. In this way, the desired movements (expressed in angular coordinates) and the control laws are in different coordinate systems, demanding the implementation of fast algorithms for the inverse kinematics model necessary for the generation of the reference trajectory in angular coordinates.
This reference trajectory, after being compared with the final position of the load, would generate an error (e) that would be minimized by the controller through a nonlinear algorithm. Fig. 2 illustrates inverse kinematics model through the representation of a generic number of joints.
A pick and place operation needs only the study of the kinematics aspects (initial and final position), but the evolution of a complete trajectory requires the study of dynamic aspects. In particular, if we are given initial and final points trajectories and we wish to find the required vector of joint torque during the trajectory evolution, a complete study of the dynamical model of the most important joints is necessary.
This paper is subdivided into four topics described below:
1) Implementation of numerical algorithms for the solution of the kinematics inverse problem;
2) Development of an offline program for simulation and automatic generation of the joint trajectories;
3) Development of modeling and simulation of dynamic systems, with emphasis on the study and controllers of positions of joint robotics;
4) Proposal of a joint controller using EPLD electronics devices (Erasable Programmable Logic Device).
Kinematics Model Direct and Inverse Problem
The kinematics model of a robot expresses the position and orientation of its end effector with regards a coordinate system fixed in the base of the robot, in relation to its generalized coordinates (angular coordinates for rotational joints). This is represented by the following expression:
where
= (q_{1}, q_{2}, ......, q_{n}): angular position vectors for the joints;= (X, Y, Z, y, q, f ): position vector, where the first three terms denote the cartesian position and the last three terms denote the orientation of the end effector.
where
This relation may be expressed mathematically by a matrix that relates the system of coordinates fixed in the base of the robot with a system of coordinates associated to its end effector. This matrix is called homogeneous passage matrix and is obtained from the product of the homogeneous transformations matrix, A_{i i1}, that relates the system of coordinates of an element i with the system of the previous element i1 (Fu, 1987), that is
where:
p = [ p_{x} , p_{y} , p_{z} ] : position vector;
n = [ n_{x} n_{y} n_{z} ], s = [ s_{x} s_{y} s_{z} ] and a = [ a_{x} a_{y} a_{z} ]: orthonormal vector that describes the orientation.
The description of the transformation matrix is made through the DenavitHartenberg convention, after obtaining the four parameters q_{i}, .a_{i}, d_{i} and a_{i}.
The inverse kinematics model is necessary for the generation of the trajectories in angular coordinates. It is obtained through the inversion of the function ƒ that is nonlinear with a nontrivial analytical solution.
PseudoInverse Matrix
In many cases, there is a solution for a system of linear equations even if there is no associated inverse matrix. This problem and many others may be solved through the use of the pseudoinverse matrix (A^{+}), also called generalized inverse matrix. The solution for the generalized inverse of a given matrix must obey the following properties (Nashed, 1976):
It must always exist and be reduced to A^{1} , in the case that A is nonsingular;
Possess some of the properties of the inverse matrix (or modifications of the same);
When used instead of the inverse matrix, it must be able to provide sensible responses for important questions such as: equation consistency or leastsquare solutions.
Moore and Penrose (Huang, 1983) defined the principle of the pseudoinverse, A+, as the unique solution to the following set of equations:
Algorithm
The development of a numerical algorithm (Sá, 1996) to find the angular positions for a task defined in terms of its end effector in cartesian space, contains the solution of the inverse kinematics problem through the use of a recursive numerical method that uses the calculation of the kinematics model and of the Jacobian inverse matrix for the manipulator. Greville's method is used for the calculation of the pseudoinverse matrix of the Jacobian matrix.
With the aim of validating the algorithm, shown in Fig. 3, different simulations are performed, assessing the behavior of the trajectory (angular space). For this purpose, the geometrical model of the submarine manipulator Kraft, with 6 rotational joints, is used. Moreover, the trajectory, followed by the end effector (Cartesian space) of the manipulator, was plotted for all simulations.
Figures 4 and 5, show the trajectory obtained for the manipulator, moving from an initial position (in mm) and orientation (in degrees), x_{i}, (776.9, 0, 933.1, 0, 90, 0), that correspond to the angular configuration, in degrees, (0, 90, 90, 0, 90, 0), to a final target, x_{d}, (776.9, 0, 933.1, 25, 63, 75). The final angular configuration achieved, in degrees, is (11.05, 37.81, 139.67, 131.42, 113.03, 167.51).
OffLine Programming
Offlineprogramming is used for the simulation and generation of the joint trajectories. In this software, the direct and inverse kinematics model of the robot and its complete environment is implemented, with the possibility of collision avoidance tests. In this software, three procedures described below (Sá, 2000), are implemented:
Simulation software
Simulation software permits the complete visualization of the robot and its environment and facilitates the graphic and interactive edition of the trajectory, , which contains the robotics device, dedicated tools, accessories, etc. The main screen of this program is shown in figure 6. It also allows the accomplishment of collision avoidance tests.
Planning of manipulator trajectories software
This procedure, denominated "Trajectory", is responsible for the planning of manipulator trajectories. It can be generated through interpolation of points of passage obtained in a simulation module and through the inverse kinematics model (offline model). The main screen of this program is shown in fig. 7.
After the complete generation of joint trajectories, it communicates with the electronic interface and transmits the reference signals to the joint controller (online).
Environment Creation Software
This permits the construction of a complete environment including external obstacles and tools for the manipulator, after the compilation of the geometric models of the tools and their environment.
It generates file contents, different environment types and tools used during the automatic operation. The main screen of this program is shown in figure 8.
The geometric models create ASCII and binary files with the complete definition of the obstacle or tool (height, width, position). The obstacles or tools are also created starting from simple primitive elements (sphere, cylinder and paving stone).
Dynamical Model and Control
After the solution of the trajectories problem using the kinematics model, the purpose of robot arm control is to maintain dynamic response. In general, the control problem consists of obtaining dynamic models of the manipulator and using these models to determine control laws or strategies to achieve the desired response and performance. Current industrial approaches towards robot arm control system design treat each joint of the robot arm as a simple joint servomechanism (decoupling joint). This approach models the varying dynamics of a manipulator inadequately because it neglects the motion and configuration of the whole arm mechanism. A complete block diagram for the nonlinear dynamic model system with its position controller is shown in Fig. 9.
The purpose of a positional controller is to drive the motor of each joint so that the actual angular displacement of the joint will track a desired angular displacement specified by a preplanned trajectory, as discussed in 3. The technique is based on using the error signal between the desired and actual angular positions of the joint.
Actuator Model
In this research an armaturecontrolled DC motor, traditionally used in industrial robots, is used. The same analysis can be accomplished for other motors types. The complete model of an armaturecontrolled DC permanent torque motor, with electric, mechanics and joint equations are presented below and the block diagram of a single joint robot arm, using openloop transfer function, is shown in Fig. 10.
where e(t)=K_{e}W(t) electrical equation
where W(t)= q mechanical equation
Coupling equation
where:
i(t)  current (A);
R  induced resistance (W)
L  inductance (H);
u  tension applied in the circuit of the field (V);
Jm  moment of inertia of the motor (kg m^{2}).
Ke  constant of the electromechanical induced force (V/rad s^{1});
K_{t}  constant of torque (Nm/A);
Tr  resistant torque (Nm);
Tm  mechanical torque (Nm).
The torque developed at the motor shaft with the complete dynamic model can be described for the equation as:
System of Control
The control of a system can be defined as a system, the proposition of which is to calibrate or to adjust the flow of energy as desired. A system of control mesh in shut uses the signs of the exit to modify the actions of the system, with the aim of reaching the specified objective. For comparison of the results, of the nonlinear controller, presented in the next section, a classical PID controller was used in the loop control of the system in study, Fig. 11.
The relevant data for the simulation are as follows: R = 3.0 W ; L = 0.005 H; K_{T} = 0.001 Nm, Jm = 1.412e4 Kg.m^{2}; Cm = 2.7e4 Nm/rad s^{1}; g = 9.8 m/s^{2} . The reference trajectory (fig. 12) used in this work is built considering the parameters of the complete joint (time constant, speed).
The proportional, integrative and derivative gains of the controller are 10, 5 and 2 respectively. The control results of the complete joint are presented in Fig. 13.
Development of Position Controller Using EPLD
An alternative to PID controller implemented by software is the control of the robot's joints using EPLD. Nowadays, the EPLD's are important devices in hardware projects. Their speed, integration capacity, flexibility and easy programming also present an attractive solution in controllers' projects (Altera, 1998).
The objective of an EPLD controller is the control of a DC motor, following the proposed model in the previous section. However, this proposal considers that the potency block is a PWM (pulse width modulation) block. This programmable controller is capable of processing the digital signs originating from an encoder coupled to the motor (sign ENCODER) and of a digital sign of control representative of a trajectory (sign TRAJETORY). In EPLD, a PID digital controller is implemented. This PID has its gain parameters fitting through external programming. The controller's output is a digital sign for the PWM potency block. Digitalanalog or analogdigital converters are not necessary in the control loop.
The implemented controller can be visualized in the diagram of blocks presented in figure 14. In this figure, the control of an only joint is represented, but the synchronized control of all the joints can be easily kept through the same EPLD.
Four main blocks are observed:
Error Detecting Block: This block is used for the comparison of the signs ENCODER and TRAJETORY, allowing the generation of a proportional binary word to the error among the periods of the signs.
PID Controller Block: it implements a PID digital controller, using the gain parameters contained in the control registers.
Control Register Block: it implements the control registers, responsible for the programming of parameters in PLD.
Power Interface Block: it converts the binary word supplied by PID controller in a pattern of digital signs to control the PWM potency block.
The following input digital signs are considered:
Trajectory: it is the sign responsible for the system control. The period of this sign is associated with the speed with which the trajectory is executed. The number of periods is associated to the displacement executed by the trajectory.
Encoder: it is the sign coming from a position encoder coupled to the axis of the motor. In the same way, this sign is associated to the rotation period of the motor. The number of periods is associated to the angular displacement of the axis of the motor.
Clock: it is the sign responsible for the rate of increment of the EPLD internal elements.
Address: this binary sign allows us to define which register is programmed. It is possible to program up to 16 registers, containing information such as: gains of the controller, number of bits used in internal operations or habilitation of state flag.
Data: this 16 bits binary sign allows us to program the EPLD internal registers.
EPLD Enable: habilitation sign for programming of the EPLD internal registers.
Read/Write: this sign selects a write cycle or a read cycle in the EPLD internal register.
Conclusions
This article presents the implementation of numerical algorithms for the solution of the kinematics inverse problem and modeling and simulation of dynamic systems. The algorithms of the kinematics inverse problem are implemented in real time. The modeling and simulation of dynamic systems are performed emphasizing offline programming.
A study of the control strategies, using several elements of a robotic joint, such as: DC motor, inertia, gearbox is carried out. Finally a generator trajectories, used as input for a generic group of joints is developed and a proposal of the controller's implementation of joints using EPLD development system is presented.
Based on the above, the aspects below are concluded:
A systematic procedure is proposed for mathematical modeling and includes the kinematics inverse numerical methods. The dynamic modeling of the different elements of a robotic system is presented.
The planning of trajectories through the use of the kinematics inverse model presented excellent results in real time applications and the computational simplicity of the method allows the implementation of collision avoidance strategy algorithms.
The EPLD solution presents low cost and fast implementation of the desire controller. Other positive characteristics of EPLD can be considered: high integration capacity, availability of a great number of projects and simulation tools on the market and easy correction and maintenance.
Through simulations, using the parameters of the system, the viability of the project was verified and with supported experiments, described previously, the validation of the obtained data, will be possible.
Presented at COBEM 99 15th Brazilian Congress of Mechanical Engineering, 2226 November 1999, São Paulo. SP. Brazil. Technical Editor: José Roberto F. Arruda.
 Ferreira, E. P. (1991). "Robótica Básica". Versão Preliminar Publicada para a V Escola Brasileiro  Argentina de Informática, Rio de Janeiro.
 Fu, K. S. (1987), Gonzales, R. C., Lee, C.S.G.: "Robotics: control, sensing, vision, and intelligence", Mc GrawHill Company.
 Huang, C. H., Klein, C. A. (1983): "Review of Pseudoinverse Control for Use With Kinematically Redundant Manipulators", IEEE Transaction on Systems, Man and Cybernetics, vol. smc13, no2, March/April, p 245250.
 Nashed, M. Z. (1976): "Generalized Inverses and Applications", Academic Press, Inc.
 Sá, C. E. A. (1996): "Implementação de Métodos Númericos para a Resolução do Problema Cinemático Inverso de Manipuladores Robóticos  Com Ênfase em Controle de Posição". Master Degree Thesis, Unicamp.
 Sá, C. E. A. (2000). "Desenvolvimento e Implementação de um Programa Computacional para a Supervisão de Manipuladores Industriais", Tese de Doutorado, Faculdade de Engenharia Mecânica, Unicamp.
 Altera (1998), MAX 7000  Programmable Logic Family Data Sheet, Altera Corporation.
Publication Dates

Publication in this collection
27 Aug 2003 
Date of issue
July 2002