Control y Programación del Robot IAR234 Robótica Control y Programación del Robot

Contenidos Arquitectura del sistema de visión Generación y control de trayectoria. Control visual. Arquitectura del sistema de visión Control basado en posición Control basado en imagen. Lenguajes de programación de robots. Lenguaje de Control Avanzado (Advanced Control Language – ACL). Aplicaciones.  Análisis de casos prácticos. Ejemplos prácticos.

Dr. Juan José Aranda Aboy Objetivos Modelar la trayectoria de un robot y el movimiento de sus partes así como programar el control del funcionamiento de éste teniendo en cuente los sensores que posee. Primavera-2009 Dr. Juan José Aranda Aboy

Niveles superiores de un Robot Manipulador Scorbot Primavera-2008 Dr. Juan José Aranda Aboy

Dr. Juan José Aranda Aboy Seguimiento visual Primavera-2008 Dr. Juan José Aranda Aboy

Enfoques para retroalimentación visual There are two approaches in visual feedback control: position based and feature based Primavera-2009 Dr. Juan José Aranda Aboy

position-based schemes With position-based schemes, the object position and orientation relative to the camera are computed by using photogrammetric, stereo, or "depth from motion" techniques. Because the position of the object is available as the output of the image processing part, a conventional position controller can be used to control the manipulator. However, geometric model of the object is required and the camera-robot system must be calibrated. Primavera-2009 Dr. Juan José Aranda Aboy

feature-based schemes feature-based schemes use the features directly for feedback control; that is, features are controlled in the image plane. Thus, the controller must be modified to close the feedback loop in the image plane or feature space. However, the computational burden is reduced. Also errors in the geometrical model and camera calibration may be eliminated. Primavera-2009 Dr. Juan José Aranda Aboy

Seguimiento visual en base a características Primavera-2008 Dr. Juan José Aranda Aboy

Enfoques para control visual There are many approaches that try to incorporate feedforward structure into the visual servo controller. If the object velocity is constant, a constant-gain velocity estimator is appropriate: For example α – β – γ filter. A Kalman filter: For many applications, the target acceleration is assumed to be zero-mean Gaussian. Then the Kalman filter estimates the target position and velocity with updating the filter gain. Similarly, an AR model can be used. Note that these estimates should be executed with the sampling rate of the joint servo, and the input to the filter must be generated appropriately during the vision sample interval. Primavera-2009 Dr. Juan José Aranda Aboy

Dr. Juan José Aranda Aboy Modelo del sistema Primavera-2008 Dr. Juan José Aranda Aboy

Dr. Juan José Aranda Aboy Modelo del robot The system model is considered as a map from the joint angle to the object image, which is composed of the kinematic model of the robot and the imaging model of the camera as shown in previous figure . The camera is assumed to be mounted on the robot hand. Then the kinematic model becomes a map from the joint angle to the camera position and orientation. The camera model is a map from the position and orientation of the camera to the image of the object. Primavera-2008 Dr. Juan José Aranda Aboy

Dr. Juan José Aranda Aboy Modelo del robot (2) The object motion is assumed to be an autonomous system that is independent of the robot motion. From now on, the position and orientation are called simply the position unless otherwise specified. The representation of orientation is not significant, and one can use any representations by using three parameters. However, the rotational velocity must not be considered as their time derivative. It must be the rotational velocity around three axes of the coordinate system in which the position is represented. Primavera-2008 Dr. Juan José Aranda Aboy

Modelos cinemático y dinámico del robot Assume that the robot has m (≤ 6) joints and the camera is mounted on the robot hand. Let Scam be the (6 x 1) vector of camera position, then the kinematic model of the robot is given by where q  Rm is the joint angle. The dynamic model of the robot is where r is the actuator torque vector, M is the inertia matrix, and h is the vector representing the Coriolis, centrifugal, and gravity forces. Primavera-2008 Dr. Juan José Aranda Aboy

Modelo del movimiento del objeto Assume that the object has mobj (≤ 6) degrees of freedom. Let Sobj be the (6 x 1) vector of object position and p be the (mobj X 1) vector of generalized coordinates representing the object position. Also assume that the object velocity is generated by an l (≤ mobj) dimensional parameter vector θ* such that is satisfied, where W(p) is an mobj X 1 matrix function of p. The vector θ* and the equation are called the velocity parameter and the object motion model. This motion model is simple, but it can model a fairly large class of autonomous motions including straight, circular, oval, and "figure 8" motions. Primavera-2008 Dr. Juan José Aranda Aboy

Dr. Juan José Aranda Aboy Modelo de la cámara The object image is generated by the perspective projection of the relative position between the camera and the object. The perspective projection is a map between two different representations of the position of the object, that is, the representations in the camera coordinate system and in the image plane Let be the (6 x 6) coordinate transformation matrix from the world coordinates to the camera coordinates. Note that includes the transformation of the orientation parameters. Then r is defined by Primavera-2008 Dr. Juan José Aranda Aboy

Modelo de imagen en perspectiva Primavera-2008 Dr. Juan José Aranda Aboy

Modelo de imagen en perspectiva (2) The vector consists of the coordinates of the feature point in the image plane expressed in pixels. Then the camera model is defined by: where f is the focal length of the lens. Note f<0 because the x and y coordinates of the image plane are aligned to the X and Y coordinates of the camera coordinate system. Thus Z < 0 for the object in the view area of the camera. Primavera-2008 Dr. Juan José Aranda Aboy

Factibilidad de la tarea The robot configuration should avoid singular points while tracking. Thus we restrict the robot configuration in a region that does not contain the singular points. Also, we assume that for all , where is a subset of that contains all solutions of equation, the solution q* of Scam(q* ) = Sobj(P) + rd is in . This is a feasibility condition for object tracking. To satisfy this condition m≥mobj is necessary. Also, it is useful to introduce the feature manifold , which is defined by The features on the feature manifold are called the admissible features. Primavera-2008 Dr. Juan José Aranda Aboy

Ejemplo: Transformación en perspectiva con cuatro características Primavera-2008 Dr. Juan José Aranda Aboy

Dr. Juan José Aranda Aboy Jacobianos The robot Jacobian that transforms the joint velocity to the hand velocity plays an important role in Cartesian space control. If one wants to control the robot in the tool frame, a Jacobian transforming the joint velocity to the tool velocity (expressed in the tool frame) will be needed. Similarly, we need a Jacobian that transforms the joint velocity to the feature velocity in the image coordinate system. Many important characteristics of the visual servo system are described by using the Jacobian. Two Jacobians called the image Jacobian and the motion Jacobian are defined, and then degenerateness and redundancy are introduced. Primavera-2008 Dr. Juan José Aranda Aboy

Obtención de los Jacobianos Differentiation of the camera model: donde The matrices J(2n x m) and L(2n x mo) are called the image Jacobian and motion Jacobian, respectively. The image Jacobian transform the joint velocity to the feature velocity, and the motion Jacobian transforms the target velocity to the feature velocity. Primavera-2009 Dr. Juan José Aranda Aboy

Dr. Juan José Aranda Aboy Obtención … (2) Since the vector r is expressed in the camera coordinate system, is the robot Jacobian expressed in the camera coordinate system. It is straightforward to see that where Primavera-2009 Dr. Juan José Aranda Aboy

Dr. Juan José Aranda Aboy Obtención … (3) Note that the submatrix expresses the infinitesimal change of the ith feature according to the infinitesimal change of the joint angles. Similarly, we have where Primavera-2009 Dr. Juan José Aranda Aboy

Características que degeneran Consider a feature point that lies on the optical axis of the camera. When the point moves on the optical axis, the image does not change. Thus, this point is not useful for controlling the camera position in the Z axis. On the other hand, when the camera rotates in any direction around the object, the image does not change. Thus, a point feature is not useful for controlling the camera orientation. In this sense, a point feature is degenerated for six-degree-of-freedom control of the camera. In general, the features that do not change when the robot's joint or the object itself moves are called degenerated features. A simple test for degenerateness is to check the rank of the Jacobian. Primavera-2008 Dr. Juan José Aranda Aboy

Características redundantes An example suggests using at least four feature points to control the six-degree-of-freedom robot. The features are called redundant if the number of features is larger than that of joints. For four feature points, the number of features is eight and they are redundant. A sufficient condition for the image Jacobian being full rank is given in the following lemma: Suppose that there are four points on a plane and the corresponding feature vector is admissible. Then the image Jacobian is full rank if any three of the feature points are not collinear in the image plane. Primavera-2008 Dr. Juan José Aranda Aboy

Cilindro singular caracterizado con tres puntos Primavera-2008 Dr. Juan José Aranda Aboy

Ley de control no lineal Variable controlada Controlador Observador Controlador en base al observador Primavera-2008 Dr. Juan José Aranda Aboy

Controlador en base al observador Primavera-2008 Dr. Juan José Aranda Aboy

Dr. Juan José Aranda Aboy Variable controlada Our goal is to track the object so as to keep the features of the object at the reference features. The models of robot, object motion, and camera are given by previous equations, respectively. On the basis of these models, it is natural to adopt the features as the controlled variables, joint angles and joint velocities as the state, and the joint torque as the input. For a minimum set of features (n = m/2), this selection is appropriate. However, for redundant features, the system becomes uncontrollable because the features cannot move in R2n arbitrarily. Primavera-2009 Dr. Juan José Aranda Aboy

Variable controlada (2) To resolve this problem, one has to solve nonlinear geometric constraints on the features that represent the rigidness of the object. Since these constraints are difficult to solve, we linearize the constraints at the reference point and reduce the dimension of the feature vector to the dimension of the joint space. Consider a nominal point rd that satisfies Define a matrix B as follows: Primavera-2009 Dr. Juan José Aranda Aboy

Variable controlada (3) Note that J is a function of q and p; in this equation, p* and q* are a typical position of the object and a typical configuration of the robot that satisfy The matrix B is the image Jacobian at the nominal point if the features are redundant. If the features are minimum, then B is the identity matrix. The controlled variable is defined by Primavera-2009 Dr. Juan José Aranda Aboy

Dr. Juan José Aranda Aboy Controlador Once the controlled variable is given, it is straightforward to compute a strictly linearizing controller. Taking the second derivative of z gives where and Primavera-2009 Dr. Juan José Aranda Aboy

Dr. Juan José Aranda Aboy Controlador (2) the actuator torque with new input v yields a linear dynamics . Theorem: Define the new input v by where K1 K2 are positive definite gain matrices. Then the equilibrium point becomes exponentially stable by using the nonlinear input transformation. Primavera-2009 Dr. Juan José Aranda Aboy

Dr. Juan José Aranda Aboy Observador The control law requires and q*, which are not usually known. Thus an estimator for these parameters is needed. Let the estimates of the parameter 0* and controlled variable z be and , respectively, and consider the following estimator : where H is any stable matrix and Q is any positive definite matrix. While P is selected to satisfy Primavera-2009 Dr. Juan José Aranda Aboy

Dr. Juan José Aranda Aboy Observador (2) Let the estimation error vectors be: Then we obtain the following theorem: Theorem: For all and the estimator makes the equilibrium point e = 0 asymptotically stable. It is easy to prove this theorem by taking the Lyapunov function candidate as follows: Primavera-2009 Dr. Juan José Aranda Aboy

Dr. Juan José Aranda Aboy Observador (3) This observer runs with the sampling rate of the joint servo. Thus the estimate of z is updated with the joint servo rate. Since new data z are not available during the vision sample interval, it is updated by using only the robot motion, Primavera-2009 Dr. Juan José Aranda Aboy

Control basado en el observador Primavera-2009 Dr. Juan José Aranda Aboy

Control basado en el observador (2) Consider the following controller based on the estimated velocity of the feature vector z we obtain the following closed-loop dynamics: where Primavera-2009 Dr. Juan José Aranda Aboy

Ejemplo: Robot con dos articulaciones To see the procedure for designing the observer-based controller introduced, an example of a planar two-link robot is given. Let us consider the robot shown in next figures First figure is the side view (from the + Yw direction). Second figure is the top view. The camera is mounted on the second link and looks upward. The object position is higher than the camera position. When the joint angle vector q equals zero, the robot is stretched out and the links are aligned with the Xw axis. Primavera-2008 Dr. Juan José Aranda Aboy

Dr. Juan José Aranda Aboy Ejemplo: Robot con dos articulaciones Modelo del robot en vista lateral Primavera-2008 Dr. Juan José Aranda Aboy

Dr. Juan José Aranda Aboy Ejemplo: Robot con dos articulaciones Modelo de cámara en vista superior Primavera-2008 Dr. Juan José Aranda Aboy

Ejemplo: Robot con dos articulaciones Modelo del robot The robot model is given by with where Primavera-2009 Dr. Juan José Aranda Aboy

Dr. Juan José Aranda Aboy Ejemplo: Robot con dos articulaciones Modelo de la posición orientación de cámara y objeto Camera position and orientation: Object position with respect to the camera coordinate system: Primavera-2009 Dr. Juan José Aranda Aboy

Ejemplo: Robot con dos articulaciones Modelo de la cámara camera model: Computing: where Primavera-2009 Dr. Juan José Aranda Aboy

Ejemplo: Robot con dos articulaciones Jacobianos Thus, J becomes singular only if the object is on the line connecting the first and second joints (just above the first link and its extension), and L is always nonsingular. To avoid the singular configuration, one may select more features than necessary. Primavera-2009 Dr. Juan José Aranda Aboy

Ejemplo: Robot con dos articulaciones Modelo de movimientos de objetos Lineal Circular Figura en 8 Primavera-2008 Dr. Juan José Aranda Aboy

Dr. Juan José Aranda Aboy Ejemplo: Robot con dos articulaciones Modelo de movimientos de objetos (2) In the previous example, the object height is constant. Thus the object degree of freedom is 2 and the object position is uniquely defined by Primavera-2009 Dr. Juan José Aranda Aboy

Dr. Juan José Aranda Aboy Ejemplo: Robot con dos articulaciones Modelo de movimientos de objetos (3) Straight Motion: As shown in figure, if the object motion is straight and the object velocities in the X and Y directions are vx and vy, respectively, then we have: Primavera-2009 Dr. Juan José Aranda Aboy

Dr. Juan José Aranda Aboy Ejemplo: Robot con dos articulaciones Modelo de movimientos de objetos (4) If the object motion is circular with constant velocity ω: The object position and its time derivative are described by where r is the radius of the circle. Thus the object velocity is given by Primavera-2009 Dr. Juan José Aranda Aboy

Dr. Juan José Aranda Aboy Ejemplo: Robot con dos articulaciones Modelo de movimientos de objetos (5) For the case of an unknown center of the circle, let the center be (cx, cy). Then the object becomes: Then, we have the following parameterization: Primavera-2009 Dr. Juan José Aranda Aboy

Dr. Juan José Aranda Aboy Ejemplo: Robot con dos articulaciones Modelo de movimientos de objetos (6) For "figure 8" motion the object position becomes: Then the motion is modeled with where Primavera-2009 Dr. Juan José Aranda Aboy

Ejemplo: Robot con dos articulaciones Controlador (1) Primavera-2009 Dr. Juan José Aranda Aboy

Ejemplo: Robot con dos articulaciones Controlador (2) Primavera-2009 Dr. Juan José Aranda Aboy

Dr. Juan José Aranda Aboy Ejemplo: Robot con dos articulaciones Sistema de control retroalimentado visual Primavera-2008 Dr. Juan José Aranda Aboy

Ejemplo: Robot PUMA con seis articulaciones Primavera-2008 Dr. Juan José Aranda Aboy

Configuraciones de puntos caracterísicos Features are selected as the x and y coordinates in the image plane of the center of each circle. The center mark of five marks has height 30 mm. Primavera-2008 Dr. Juan José Aranda Aboy

Configuración del robot y posición del objeto Primavera-2008 Dr. Juan José Aranda Aboy

Entorno para experimento Primavera-2008 Dr. Juan José Aranda Aboy

Entorno para experimento (2) Primavera-2008 Dr. Juan José Aranda Aboy

Entorno para situaciones de contacto Primavera-2009 Dr. Juan José Aranda Aboy

Configuración del sistema de tracking & grasping Primavera-2008 Dr. Juan José Aranda Aboy

Restricciones naturales y artificiales para dos tareas Ajuste de grúa Atornillar Primavera-2009 Dr. Juan José Aranda Aboy

Modelado del sistema de seguimiento y agarre (tracking & grasping) Secuencia con cuatro posiciones de contacto para colocar un remache. Primavera-2008 Dr. Juan José Aranda Aboy

Modelo de manipulador robótico con tres GDL contactando superficie Primavera-2008 Dr. Juan José Aranda Aboy

Modelado del sistema de cámaras Primavera-2008 Dr. Juan José Aranda Aboy

Modelado del movimiento de un objeto rígido Primavera-2008 Dr. Juan José Aranda Aboy

Movimiento de un objeto rígido Primavera-2008 Dr. Juan José Aranda Aboy

Estimación del campo de movimiento del punto de referencia Formulación del problema del punto de referencia 3-D Formulación del problema del punto de referencia 2-D Primavera-2008 Dr. Juan José Aranda Aboy

Diseño del controlador para tracking & grasping Primavera-2008 Dr. Juan José Aranda Aboy

Aproximación del modelo por el punto de referencia Primavera-2008 Dr. Juan José Aranda Aboy

Estimación de parámetros: Mínimos cuadrados recursivamente Primavera-2008 Dr. Juan José Aranda Aboy

Seguimiento del punto de referencia en base a imágenes Primavera-2008 Dr. Juan José Aranda Aboy

Seguimiento en cambio de orientación para asimiento (grasping) Primavera-2008 Dr. Juan José Aranda Aboy

Problema del punto de referencia 2-D Primavera-2008 Dr. Juan José Aranda Aboy

Seguimiento en base a imágenes Primavera-2008 Dr. Juan José Aranda Aboy

Fusión de múltiples sensores en planificación y control Primavera-2008 Dr. Juan José Aranda Aboy

Dr. Juan José Aranda Aboy Diagrama de bloques que muestra las estructuras de planificación y control Primavera-2008 Dr. Juan José Aranda Aboy

Dr. Juan José Aranda Aboy Agarre Primavera-2008 Dr. Juan José Aranda Aboy

Dr. Juan José Aranda Aboy Diagrama de bloques del sistema de control con sensores de visión y fuerza - torque Primavera-2008 Dr. Juan José Aranda Aboy

Conjunto de trabajo típico en manufactura Primavera-2008 Dr. Juan José Aranda Aboy

Esquema de rotación visual Primavera-2008 Dr. Juan José Aranda Aboy

Dr. Juan José Aranda Aboy Determinación de la posición de un punto a partir de su imagen utilizando rotación virtual Primavera-2008 Dr. Juan José Aranda Aboy

Integración de sensor para estimación en tiempo real Primavera-2008 Dr. Juan José Aranda Aboy

Dr. Juan José Aranda Aboy Calibración del robot Primavera-2008 Dr. Juan José Aranda Aboy

Planificación y control del robot Primavera-2008 Dr. Juan José Aranda Aboy

Dr. Juan José Aranda Aboy Seguimiento del robot con reducción planificada del error: Seguimiento paralelo Primavera-2008 Dr. Juan José Aranda Aboy

Seguimiento en paralelo óptimo basado en eventos Primavera-2008 Dr. Juan José Aranda Aboy

Seguimiento de una trayectoria desconocida sobre una superficie Diseño de control híbrido Movimiento restringido Desacople de variables de control Esquema de control híbrido Planificación de movimiento basada en imágenes Relación entre movimiento restringido y su imagen Planificación del movimiento Primavera-2008 Dr. Juan José Aranda Aboy

Control retroalimentado con fusión de sensores de fuerza y visual Primavera-2008 Dr. Juan José Aranda Aboy

Esquema convencional de la integración Primavera-2008 Dr. Juan José Aranda Aboy

Dr. Juan José Aranda Aboy Asimilación de la retroalimentación entregada por diferentes sensores orientada a tareas Primavera-2008 Dr. Juan José Aranda Aboy

Lazo de control híbrido de fuerza - posición Primavera-2008 Dr. Juan José Aranda Aboy

Definiciones del marco de tarea y del marco de cámara Primavera-2008 Dr. Juan José Aranda Aboy

Sistema binocular con ejes ópticos paralelos marco de tarea y marcos de cámaras Primavera-2008 Dr. Juan José Aranda Aboy

Sistema binocular con ejes ópticos perpendiculares marco de tarea y marcos de cámaras Primavera-2008 Dr. Juan José Aranda Aboy

Resolvability ellipsoids: monocular system f = 24 mm, depth = 1.0m, two features located in the task frame at (0.1 m, 0.1 m, 0) and (-0.1 m, 0.1 m, 0). Primavera-2008 Dr. Juan José Aranda Aboy

Resolvability ellipsoids: monocular system f = 12 mm, depth = 0.5 m, two features located in the task frame at (0.1 m, 0.1 m, 0) and (-0.1 m, 0.1 m, 0). Primavera-2008 Dr. Juan José Aranda Aboy

Resolvability ellipsoids: stereo pair--parallel optical axes f = 12 mm, b = 20 cm, depth = 1.0 m, one feature located in the task frame at (0, 0.2 m, 0). Primavera-2008 Dr. Juan José Aranda Aboy

Resolvability ellipsoids: stereo pair--perpendicular optical axes f = 12 mm, depth - 1.0m, two features located in the task frame at (-0.1 m, 0.1 m, 0), and (0.1 m,-0.1 m, -0.1 m). Primavera-2008 Dr. Juan José Aranda Aboy

Force and vision in the feedback loop Primavera-2008 Dr. Juan José Aranda Aboy

Tópico avanzado para investigación Uso de modelos deformables en visión robótica Primavera-2008 Dr. Juan José Aranda Aboy

Tópico avanzado para investigación Control de impactos Primavera-2008 Dr. Juan José Aranda Aboy

Tópico avanzado para investigación Planificación y control basada en sensores para telerobótica Primavera-2008 Dr. Juan José Aranda Aboy

Tópico avanzado para investigación Planificación y control de múltiples robots cooperando en un conjunto de tareas Primavera-2008 Dr. Juan José Aranda Aboy

Tópico avanzado para investigación Redes Percepción - Acción Primavera-2008 Dr. Juan José Aranda Aboy

Dr. Juan José Aranda Aboy Referencias en cursos Capítulos 6, 7, 8, 9 y 10 (curso_biom_ar) Temas 6, 7 y 8 (curso_umh_es) umh_es_vision Primavera-2008 Dr. Juan José Aranda Aboy

Referencias en Internet Introducción al Control de Robots Robótica Inteligente PRÁCTICA 4: CONTROL DE TRAYECTORIAS (Robótica Industrial – Apuntes: Tema 7) Pontryagin's Maximum Principle . Ver además http://en.wikipedia.org/wiki/Pontryagin_maximum_principle ENTORNO MATLAB PARA DISEÑO DE CONTROLADORES PID  Primavera-2008 Dr. Juan José Aranda Aboy

Documentos en archivos Control_in_Robotics_and_Automation (Caps 2 al 13) Introduction_to_Robotics_Mechanics_and_Control_-_J_J_Craig (Caps 9 al 11) Crc_Press_Mechanical_Engineering_Handbook_-_Robotics Fundamentos de Robótica McGraw-Hill Anatomy of a Robot (Cap 2) Robot_Mechanisms_And_Mechanical_Devices_Illustrated ACL Comandos_ACL_17793 Primavera-2008 Dr. Juan José Aranda Aboy

Dr. Juan José Aranda Aboy Bibliografía Angulo,J-M. y Avilés,R. “Curso de Robótica” Ed Paraninfo. Fu,K.S.; Gonzalez,R.C. y Lee,C.S.G. “Robotics: Control, Sensing, Vision and Intelligence” Ed Mc Graw Hill. Barrientos, Balaguer .C. “Fundamentos de Robótica” Ed Mc Graw Hill Abidi,M.A. y Gonzalez,R.C. “Data Fusion in Robotics and Machine Intelligence” Ed Academic Press. Haralick,R.M. y Shapiro,L.G. “Computer and Robot Vision” Ed Addison-Wesley Ogata. K. “Ingeniería de Control Moderno” Ed Prentice Hall. Primavera-2008 Dr. Juan José Aranda Aboy