MATLAB
APLICADO A ROBÓTICA
Y MECATRÓNICA
FERNANDO REYES CORTÉS
Editor
Francisco Javier Rodríguez Cruz
jrodriguez@alfaomega.com.mx
Director Editorial
Marcelo Grillo Giannetto
mgrillo@alfaomega.com.mx
Datos catalográficos
Reyes Cortés, Fernando
MATLAB aplicado a Robótica y Mecatrónica
Primera Edición
Alfaomega Grupo Editor, S.A. de C.V., México
ISBN: 978-607-707-357-4
Formato: 17 x 23 cm
Páginas: 460
MATLAB aplicado a Robótica y Mecatrónica
Fernando Reyes Cortés
Derechos reservados © Alfaomega Grupo Editor, S.A. de C.V., México.
Primera edición: Alfaomega Grupo Editor, México, enero 2012
© 2012 Alfaomega Grupo Editor, S.A. de C.V.
Pitágoras 1139, Col. Del Valle, 03100, México D.F.
Miembro de la Cámara Nacional de la Industria Editorial Mexicana
Registro No. 2317
Pág. Web: http://www.alfaomega.com.mx
E-mail: atencionalcliente@alfaomega.com.mx
ISBN: 978-607-707-357-4
Derechos reservados:
Esta obra es propiedad intelectual de su autor y los derechos de publicación en lengua
española han sido legalmente transferidos al editor. Prohibida su reproducción parcial o total
por cualquier medio sin permiso por escrito del propietario de los derechos del copyright.
Esta obra fue compuesta por el autor en LateX usando el compilador de PcTex 6.0.
Nota importante:
La información contenida en esta obra tiene un fin exclusivamente didáctico y, por lo tanto, no está
previsto su aprovechamiento a nivel profesional o industrial. Las indicaciones técnicas y programas
incluidos, han sido elaborados con gran cuidado por el autor y reproducidos bajo estrictas normas
de control. ALFAOMEGA GRUPO EDITOR, S.A. de C.V. no será jurídicamente responsable por:
errores u omisiones; daños y perjuicios que se pudieran atribuir al uso de la información
comprendida en este libro, ni por la utilización indebida que pudiera dársele.
Impreso en México. Printed in Mexico.
Empresas del grupo:
México: Alfaomega Grupo Editor, S.A. de C.V. – Pitágoras 1139, Col. Del Valle, México, D.F. – C.P. 03100.
Tel.: (52-55) 5575-5022 – Fax: (52-55) 5575-2420 / 2490. Sin costo: 01-800-020-4396
E-mail: atencionalcliente@alfaomega.com.mx
Colombia: Alfaomega Colombiana S.A. – Carrera 15 No. 64 A 29, Bogotá, Colombia,
Tel.: (57-1) 2100122 – Fax: (57-1) 6068648 – E-mail: cliente@alfaomega.com.co
Chile: Alfaomega Grupo Editor, S.A. – Dr. La Sierra 1437, Providencia, Santiago, Chile
Tel.: (56-2) 235-4248 – Fax: (56-2) 235-5786 – E-mail: agechile@alfaomega.cl
Argentina: Alfaomega Grupo Editor Argentino, S.A. – Paraguay 1307 P.B. Of. 11, C.P. 1057, Buenos Aires,
Argentina, – Tel./Fax: (54-11) 4811-0887 y 4811 7183 – E-mail: ventas@alfaomegaeditor.com.ar
Acerca del autor
Dr. José Fernando Reyes Cortés. Es profesor investigador
titular C de la Facultad de Ciencias de la Electrónica, Universidad
Autónoma de Puebla. En 1984 obtuvo la Licenciatura en Ciencias
de la Electrónica en la Facultad de Ciencias Fı́sico Matemáticas de
la Universidad Autónoma de Puebla. En 1990 obtuvo la Maestrı́a en
Ciencias con Especialidad en Electrónica en el Instituto Nacional de
Astrofı́sica, Óptica y Electrónica (INAOE). Bajo la asesorı́a del Dr. Rafael Kelly,
en 1997 culminó el Doctorado en Ciencias con Especialidad en Electrónica y
Telecomunicaciones en el Centro de Investigación Cientı́fica y de Educación Superior
de Ensenada (CICESE).
Es miembro del Sistema Nacional de Investigadores desde 1993 a la fecha.
Actualmente es nivel I. Es autor de más de 150 artı́culos nacionales e internacionales.
Ha dirigido 30 proyectos cientı́ficos. Ha graduado a más de 100 tesistas de los niveles
de ingenierı́a, maestrı́a y doctorado.
Es autor del libro Robótica. Control de robots manipuladores editado por
Alfaomega donde se presenta la dinámica de robots manipuladores y la técnica
moderna de moldeo de energı́a para diseñar nuevos algoritmos de control.
Como catedrático ha impartido más de 50 cursos del área de control y robótica a
nivel licenciatura y posgrado. Fundador del Laboratorio de Robótica y Control de
la Facultad de Ciencias de la Electrónica donde ha puesto a punto 30 prototipos de
mecatrónica y robótica. Es Premio Estatal de Tecnologı́as y Ciencias de la Ingenierı́a
en noviembre 2000 por el Gobierno del Estado de Puebla. Premio en Ingenierı́a y
Tecnologı́a de la Ciudad de Puebla en abril 2010.
Ha sido Secretario de Investigación y Estudios de Posgrado de la Facultad de Ciencias
de la Electrónica desde febrero del 2002 a la fecha. Fundador y responsable del
Cuerpo Académico de Robótica y Control en 2001. Fue responsable de la creación
de la Ingenierı́a Mecatrónica, y de los Posgrado de Ciencias de la Electrónica e
Ingenierı́a Electrónica e ingreso de ambos posgrados al PNPC de CONACyT.
A la persona que me dio la vida: Alicia Cortés
Castillo; por su apoyo y paciencia a mi esposa
Silvia y mis tesoros Luis Fernando y Leonardo.
Por la formación transmitida de Angela Castillo Merchant, Luis Manuel Cortés Castillo,
Chela, Lety y Jorge.
Por la ayuda brindada de la maestra Viky.
Mensaje del Editor
Una de las convicciones fundamentales de Alfaomega es que los conocimientos son
esenciales en el desempeño profesional, ya que sin ellos es imposible adquirir las
habilidades para competir laboralmente. El avance de la ciencia y de la técnica
hace necesario actualizar continuamente esos conocimientos, y de acuerdo con esto
Alfaomega publica obras actualizadas, con alto rigor cientı́fico y técnico, y escritas
por los especialistas del área respectiva más destacados.
Conciente del alto nivel competitivo que debe de adquirir el estudiante durante su
formación profesional, Alfaomega aporta un fondo editorial que se destaca por sus
lineamientos pedagógicos que coadyuvan a desarrollar las competencias requeridas
en cada profesión especı́fica.
De acuerdo con esta misión, con el fin de facilitar la comprensión y apropiación del
contenido de esta obra, cada capı́tulo inicia con el planteamiento de los objetivos
del mismo y con una introducción en la que se plantean los antecedentes y una
descripción de la estructura lógica de los temas expuestos, asimismo a lo largo de
la exposición se presentan ejemplos desarrollados con todo detalle y cada capı́tulo
concluye con un resumen y una serie de ejercicios propuestos.
Además de la estructura pedagógica con que están diseñados nuestros libros,
Alfaomega hace uso de los medios impresos tradicionales en combinación con
las Tecnologı́as de la Información y las Comunicaciones (TIC) para facilitar el
aprendizaje. Correspondiente a este concepto de edición, todas nuestras obras de
la Serie Profesional a la que pertenece este tı́tulo tienen su complemento en una
página Web en donde el alumno y el profesor encontrarán lecturas complementarias,
código fuente de los programas desarrollados ası́ como la solución y la respuesta de
los problemas propuestos.
Los libros de Alfaomega están diseñados para ser utilizados en los procesos de
enseñanza-aprendizaje, y pueden ser usados como textos en diversos cursos o como
apoyo para reforzar el desarrollo profesional, de esta forma Alfaomega espera
contribuir ası́ a la formación y al desarrollo de profesionales exitosos para beneficio
de la sociedad y del mundo del conocimiento.
Contenido
Plataforma de contenidos interactivos
Simbologı́a e iconografı́a utilizada
Prólogo
XV
XVI
XVIII
Parte I Programación
Capı́tulo 1
Conceptos básicos
1
3
1.1 Introducción
5
1.2 Componentes
7
1.2.1 Herramientas de escritorio y ambiente de desarrollo
9
1.2.2 Librerı́as
9
1.2.3 Lenguaje
10
1.2.4 Gráficos
10
1.2.5 Interfaces externas/API
10
1.3 Inicio
11
1.4 Lenguaje
15
1.4.1 Variables
15
1.4.2 Números
17
viii
1.4.3 Formato numérico
18
1.4.4 Operadores
21
1.5 Matrices y arreglos
26
1.5.1 Arreglos
41
1.6 Gráficas
43
1.7 Funciones
49
1.7.1 Funciones archivo
1.8 Programación
51
58
1.8.1 if
59
1.8.2 if, else, elseif
60
1.8.3 for
61
1.8.4 while
71
1.8.5 switch, case
72
1.8.6 break
73
1.8.7 return
73
1.8.8 continue
74
1.9 Formato para datos experimentales
1.10 Resumen
76
80
Capı́tulo 2
Métodos numéricos
81
2.1 Consideraciones computacionales
83
2.2 Sistemas de ecuaciones lineales
84
2.2.1 Regla de Cramer
Alfaomega
91
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
Contenido
2.3 Diferenciación numérica
ix
92
2.3.1 Función diff
97
2.4 Integración numérica
99
2.4.1 Regla trapezoidal
102
2.4.2 Regla de Simpson
108
2.4.3 Funciones de cuadratura
113
2.4.4 Método de Euler
114
2.5 Sistemas dinámicos de primer orden
117
2.5.1 Método de Runge-Kutta
118
2.5.2 Simulación de sistemas dinámicos ẋ = f (x)
124
2.6 Resumen
133
Parte I Referencias selectas
134
Parte I Problemas propuestos
135
Parte II Cinemática
Capı́tulo 3
Preliminares matemáticos
139
141
3.1 Introducción
143
3.2 Producto interno
144
3.3 Matrices de rotación
148
3.3.1 Matriz de rotación alrededor del eje z0
151
3.3.2 Matriz de rotación alrededor del eje x0
161
3.3.3 Matriz de rotación alrededor del eje y0
163
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
x
3.4 Reglas de rotación
164
3.5 Transformaciones de traslación
171
3.6 Transformaciones homogéneas
173
3.7 Librerı́as para matrices homogéneas
174
3.7.1 Matriz de transformación homogénea HRx(θ)
175
3.7.2 Matriz de transformación homogénea HRy (θ)
176
3.7.3 Matriz de transformación homogénea HRz (θ)
177
3.7.4 Matriz de transformación homogénea HTx(d)
178
3.7.5 Matriz de transformación homogénea HTy (d)
178
3.7.6 Matriz de transformación homogénea HTz (d)
179
3.7.7 Matriz de transformación DH
180
3.8 Resumen
181
Capı́tulo 4
Cinemática directa
183
4.1 Introducción
185
4.2 Cinemática inversa
186
4.3 Cinemática diferencial
187
4.4 Clasificación de robots industriales
189
4.5 Convención Denavit-Hartenberg
192
4.5.1 Algoritmo Denavit-Hartenberg
4.6 Resumen
Alfaomega
196
198
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
Contenido
Capı́tulo 5
Cinemática directa cartesiana
xi
199
5.1 Introducción
201
5.2 Brazo robot antropomórfico
202
5.3 Configuración SCARA (RRP)
234
5.4 Robot esférico (RRP)
245
5.5 Manipulador cilı́ndrico (RPP)
254
5.6 Configuración cartesiana (PPP)
264
5.7 Resumen
273
Parte II Referencias selectas
277
Parte II Problemas propuestos
278
Parte III Dinámica
Capı́tulo 6
Dinámica
283
285
6.1 Introducción
287
6.2 Estructura matemática para simulación
288
6.3 Sistema masa-resorte-amortiguador
291
6.4 Sistema lineal escalar
295
6.4.1 Estimador de velocidad y filtrado
6.5 Centrı́fuga
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
296
301
Alfaomega
xii
6.6 Péndulo
305
6.7 Robot de 2 gdl
310
6.8 Robot de 3 gdl
315
6.9 Robot cartesiano
321
6.10 Resumen
327
Capı́tulo 7
Identificación paramétrica
329
7.1 Introducción
331
7.2 Método de mı́nimos cuadrados
332
7.2.1 Linealidad en los parámetros
7.3 Librerı́a de mı́nimos cuadrados
332
334
7.3.1 Caso escalar
334
7.3.2 Caso multivariable
336
7.4 Ejemplos
338
7.5 Modelos de regresión del péndulo
346
7.5.1 Modelo dinámico del péndulo
346
7.5.2 Modelo dinámico filtrado del péndulo
350
7.5.3 Modelo de energı́a del péndulo
353
7.5.4 Modelo de potencia del péndulo
355
7.5.5 Modelo de potencia filtrada
356
7.5.6 Análisis comparativo de esquemas de regresión
359
7.6 Modelos de regresión del robot de 2 gdl
7.6.1 Modelo de regresión dinámico del robot de 2 gdl
Alfaomega
360
361
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
Contenido
xiii
7.6.2 Modelo de energı́a del robot de 2 gdl
366
7.6.3 Modelo de potencia del robot de 2 gdl
369
7.6.4 Análisis comparativo de resultados de regresión
372
7.7 Robot cartesiano de 3 gdl
373
7.7.1 Modelo de regresión dinámico del robot cartesiano
374
7.7.2 Modelo de potencia del robot cartesiano de 3 gdl
378
7.7.3 Análisis comparativo de identificación
381
7.8 Resumen
382
Parte III Referencias selectas
383
Parte III Problemas propuestos
385
Parte IV Control
Capı́tulo 8
Control de posición
389
391
8.1 Introducción
393
8.2 Control proporcional-derivativo (PD)
395
8.2.1 Control PD de un péndulo
397
8.2.2 Control PD de un brazo robot de 2 gdl
403
8.2.3 Control PD de un brazo robot de 3 gdl
408
8.2.4 Control PD de un robot cartesiano de 3 gdl
413
8.3 Control PID
8.3.1 Control PID de un robot de 2 gdl
8.4 Control punto a punto
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
417
418
422
Alfaomega
xiv
8.4.1 Control tangente hiperbólico
422
8.4.2 Control arcotangente
426
8.5 Resumen
429
Parte IV Referencias selectas
430
Parte IV Problemas propuestos
431
Índice analı́tico
Alfaomega
433
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
xv
Plataforma de contenidos
interactivos
Para tener acceso al código fuente de los programas de ejemplos y ejercicios presentados en MATLAB Aplicado a Robótica y Mecatrónica, siga los siguientes pasos:
1) Ir a la página
http://virtual.alfaomega.com.mx
2) Registrarse como usuario del sitio.
3) En el catálogo identificar este libro y descargar el material adicional.
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
Simbologı́a e iconografı́a utilizada
xvi
Simbologı́a e iconografı́a
utilizada
Diversos recursos didácticos están presentes en esta obra; particularmente se resaltan
las siguientes herramientas:
Librerı́as
La sintaxis de las librerı́as que se desarrollan son representadas como:
ẋ=robot(t, x)
Los ejemplos ilustrativos se presentan de la siguiente manera:
♣ ♣ Ejemplo 5.1
El enunciado de cada ejemplo se encuentra dentro de un recuadro con fondo gris.
Los ejemplos son presentados por nivel de complejidad, para el nivel simple o
básico se emplea la marca ♣, ejemplos del nivel intermedio ♣♣ y complejos son
denotados por ♣♣♣
Solución
Se detalla la respuesta de cada ejemplo por ecuaciones y programas en código fuente.
Adicionalmente, todos los ejemplos incluyen un número de referencia que identifica
al capı́tulo donde fue definido.
Todos los programas de esta obra han sido implementados en lenguaje MATLAB
versión 11. El código fuente se identifica por el siguiente recuadro:
5.1.
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
xvii
Código Fuente 5.1
robot.m
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Simulación de robots manipuladores
robot.m
1
2
3
4
5
6
7
8
9
10
clc;
clear all;
close all;
format short g
ti=0; h=0.001; tf = 10; %vector tiempo
t=ti:h:tf; %tiempo de simulación
ci=[0; 0; 0; 0];
opciones=odeset(’RelTol’,1e-3,’InitialStep’,1e-3,’MaxStep’,1e-3);
[t,x]=ode45(’robot’,t,ci,opciones);
plot(x(:,1),x(:,2))
Para representar una idea general de una instrucción de programación, se emplea
pseudo-código como el siguiente:
Estructura de código 5.1
Pseudo código
while k<1000
qp(k)=(q(k)-q(k-1))/h;
if q(k)>100
for j=1:1000
qpp(j)=robot(q(j),qp(j));
end
end
k=k+1;
otro grupo de instrucciones;
end
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
Prólogo
Robótica y mecatrónica representan en la actualidad áreas estratégicas y claves
para todo paı́s que aspire a la modernidad y bienestar, ya que su impacto no sólo
repercute en aspectos polı́ticos y económicos, también forma parte importante de la
vida cotidiana, educación, cultura, y en todos los ámbitos de la sociedad.
La simulación es una herramienta imprescindible para reproducir los fenómenos
fı́sicos de un robot o de un sistema mecatrónico, permite estudiar y analizar a
detalle los aspectos prácticos que intervienen en tareas especı́ficas que debe realizar
un robot industrial. La simulación es un proceso previo a la etapa experimental
donde es posible entender los conceptos claves de la robótica y mecatrónica. Bajo
este escenario se ubica la presente obra a través de la propuesta de un conjunto
de librerı́as para MATLAB que permitan realizar estudio, análisis, simulación y
aplicaciones de robots manipuladores y sistemas mecatrónicos.
Esta obra presenta la propuesta y desarrollo de una clase particular de librearı́as
toolbox para robótica y mecatrónica. Las librerı́as son funciones en código fuente para
MATLAB que permiten modelar la cinemática directa e inversa, transformaciones
homogéneas (rotación y traslación), dinámica, identificación paramétrica, control de
robots manipuladores. El contenido y material incluido en el libro está dirigido a
estudiantes de ingenierı́a en electrónica, sistemas, computación, industrial, robótica
y mecatrónica. No obstante, también puede ser adecuado para nivel posgrado.
La organización de este libro consta de cuatro partes: la Parte I Programación
incluye dos capı́tulos. El Cap tulo 1 Conceptos básicos de programación del
lenguaje de MATLAB tiene la finalidad de que el lector adquiera los conocimientos
necesarios para adquirir solvencia en programar aplicaciones en código fuente. El
capı́tulo 2 Métodos numéricos presenta aspectos relacionados con las técnicas de
métodos numéricos enfocados a resolver problemas de diferenciación e integración
numérica de sistemas dinámicos.
La Parte II Cinemática está relacionada con el análisis cinemático de las
principales clases de robots manipuladores. Se componen de tres capı́tulos; el
Prólogo
xix
capı́tulo 3 Preliminares matemáticos contiene las bases de las reglas de las
matrices de rotación, traslación y transformaciones homogéneas. En el capı́tulo
4 Cinemática directa se detalla la metodologı́a de Denavit-Hartenberg para
describir la cinemática directa; asimismo se describe la cinemática inversa,
cinemática diferencial y jacobianos. Un compendio de librerı́as se desarrollaron con
variables simbólicas y aplicaciones numéricas para los principales tipos de robots
manipuladores en el capı́tulo 5 Cinemática directa cartesiana.
La Parte III Dinámica describe la técnica de simulación para sistemas dinámicos
e identificación paramétrica. Formada por el capı́tulo 6 Dinámica donde se realiza
simulación de sistemas mecatrónicos y robots manipuladores con la estructura de
una ecuación diferencial ordinaria de primer orden descrita por variables de estado.
El capı́tulo 7 Identificación paramétrica presenta cinco esquemas de regresión
con la técnica de mı́nimos cuadrados. Se proponen extensos ejemplos para ilustrar
el procedimiento para encontrar el valor numérico de los parámetros del robot.
Finalmente, la Parte IV capı́tulo 8 Control de posición contiene la simulación
de algoritmos clásicos como el control PD y PID; ası́ como simulación de nuevas
estrategias de control con mejor desempeño que pertenecen a la filosofı́a de diseño
moderno conocida como moldeo de energı́a. Aplicaciones de control punto a punto
se ilustran para describir la técnica de simulación.
La realización de esta obra no hubiera sido posible sin el apoyo de la Benemérita
Universidad Autónoma de Puebla (BUAP), particularmente agradezco a la
Facultad de Ciencias de la Electrónica por todas las facilidades brindadas. El
autor desea agradecer principalmente al Dr. Jaime Cid, M. C. Fernando Porras,
Dra. Aurora Vargas, Dr. Sergio Vergara, Dra. Amparo Palomino y al M. C. Pablo
Sánchez, ası́ como al programa de financiamiento de proyectos de investigación de
la Vicerrectorı́a de Investigación y Estudios de Posgrado (VIEP), especialmente a la
Dra. Rosa Graciela Montes y al Dr. Pedro Hugo Hernández Tejeda.
Fernando Reyes Cortés
H. Puebla de Z., a 3 de diciembre de 2011
Facultad de Ciencias de la Electrónica
Benemérita Universidad Autónoma de Puebla
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
Parte I
Programación
La Parte I de la presente obra está destinada a estudiar el lenguaje de programación
de MATLAB y la implementación de métodos numéricos aplicados a la robótica
y mecatrónica. Esta primera parte se compone de dos capı́tulos. El capı́tulo 1
Conceptos básicos presenta los aspectos básicos del lenguaje MATLAB y tiene
la finalidad de que el lector adquiera los conocimientos fundamentales para adquirir
solvencia en programar en código fuente. El capı́tulo 2 Métodos numéricos
presenta aspectos relacionados con las técnicas de métodos numéricos enfocados
a resolver problemas de modelado y control de las áreas de robótica y mecatrónica.
Capı́tulo 1 Conceptos básicos
Descripción de los principales componentes del ambiente de programación de
MATLAB. Sintaxis y programación, declaración de variables, tipo de datos,
operadores, programación, manipulación numérica de matrices y arreglos, funciones, gráficas, funciones importantes, matrices, operadores, instrucciones, lazos de programación, etcétera. Un conjunto de ejemplos se desarrollan para
ilustrar los temas presentados.
Capı́tulo 2 Métodos numéricos
Conocer los principales métodos de diferenciación e integración numérica que
se aplican en robótica y mecatrónica. Sistemas de ecuaciones lineales, método
de Euler e integración numérica (trapezoidal, Simpson). Simulación de sistemas
dinámicos con la técnica de Runge-Kutta.
La parte I concluye con referencias selectas y un conjunto de problemas para mejorar
las habilidades y conocimientos de los capı́tulos tratados.
Referencias selectas
Problemas propuestos
Conceptos básicos
Capı́tulo
1
1.1 Introducción
1.2 Componentes
1.3 Inicio
1.4 Lenguaje
1.5 Matrices y arreglos
1.6 Gráficas
1.7 Funciones
1.8 Programación
1.9 Formato para datos experimentales
1.10 Resumen
Objetivos
Presentar la plataforma y ambiente de programación de MATLAB con
los fundamentos y bases de su lenguaje enfocado para adquirir solvencia
y dominio en la implementación de algoritmos con aplicaciones en
mecatrónica y robótica.
Objetivos particulares:
Aprender el Lenguaje MATLAB.
Conocer los operadores.
Programar con matrices y arreglos.
Manejar funciones e instrucciones.
Graficar funciones y datos en 2D y 3D.
Desarrollar formatos para manejar datos experimentales.
1.1
Introducción
5
1.1 Introducción
H
oy en dı́a, MATLAB es un lenguaje de programación matemático de alto
nivel integrado con entorno gráfico amigable, visualización de datos, funciones,
gráficas 2D y 3D, procesamiento de imágenes, video, computación numérica para
desarrollar algoritmos matemáticos con aplicaciones en ingenierı́a y ciencias exactas.
Particularmente, en ingenierı́a es una herramienta muy poderosa para realizar
aplicaciones en mecatrónica, robótica, control y automatización.
MATLAB es un acrónimo que proviene de matrix laboratory (laboratorio
matricial) creado por el profesor y matemático Cleve Moler en 1970. La primera
versión de MATLAB fue escrita en lenguaje fortran la cual contempló la idea de
emplear subrutinas para los cursos de álgebra lineal y análisis numérico de los
paquetes LINPACK y EISPACK; posteriormente se desarrolló software de matrices
para acceder a esos paquetes sin necesidad de usar programas en fortran. La
empresa The Mathworks (http://www.mathworks.com), propietaria de MATLAB
fue fundada por Jack Little y Cleve Moler en 1984. El actual paquete de MATLAB se
encuentra escrito en lenguaje C, y su primera versión en este lenguaje fue escrita por
Steve Bangert quien desarrolló el intérprete parser. Steve Kleiman implementó los
gráficos, las rutinas de análisis, la guı́a de usuario, mientras que la mayorı́a de los
archivos.m fueron escritos por Jack Little y Cleve Moler. Actualmente, el lenguaje
de programación de MATLAB proporciona un sencillo acceso a algoritmos numéricos
que incluyen matrices.
Las versiones recientes del lenguaje MATLAB se caracterizan por ser multiplataforma, es decir se encuentra disponible para sistemas operativos como Unix, Windows
y Apple Mac OS X. MATLAB posee varias caracterı́sticas computacionales y visuales, entre las que sobresalen la caja de herramientas (toolbox) la cual representa
un amplio compendio de funciones y utilerı́as para analizar y desarrollar una amplia gama de aplicaciones en las áreas de ingenierı́a y ciencias exactas. Un rasgo
distintivo de MATLAB es que ofrece un entorno gráfico de programación amigable
al usuario a través de herramientas y utilerı́as para realizar simulación de sistemas
dinámicos, análisis de datos, procesamiento de imágenes y video, gráficas y métodos
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
6
Capı́tulo 1: Conceptos básicos
de visualización, desarrollo de interfaces de usuario (GUI); también permite realizar interfaces para sistemas electrónicos, por ejemplo adquisición de datos con una
versatilidad de tarjetas de instrumentación electrónica comerciales con plataforma
de microprocesadores, DSP’s, PIC’s, FPGA’s, manejo de puertos como USB, COM,
paralelo y diseños electrónicos propios.
MATLAB dispone de dos herramientas adicionales que expanden sus prestaciones:
plataforma de simulación multidominio Simulink y GUIDE editor de interfaces
de usuario. Se recomienda al lector visitar:
www.mathworks.com/products/matlab
en ese sitio WEB se pueden encontrar productos actuales de MATLAB, manuales,
notas técnicas, cursos, aplicaciones y foros internacionales, ası́ como sociedades de
desarrollo. Actualmente, MATLAB se ha convertido en un referente de cómputo
a nivel internacional, debido que en la mayorı́a de las universidades, centros de
investigación e industria lo utilizan para desarrollar y llevar a cabo proyectos de
ciencia y tecnologı́a.
Especı́ficamente en el área de ingenierı́a MATLAB permite realizar simulaciones
de sistemas mecatrónicos y robots manipuladores. El proceso de simulación resulta
importante cuando no se dispone de una adecuada infraestructura experimental.
Sin embargo, la simulación depende de un buen modelo matemático que permita
reproducir fielmente todos los fenómenos fı́sicos del sistema real. La simulación es
flexible ya que permite detectar posibles deficiencias en el diseño del modelado. Una
vez depurado el modelo dinámico, entonces la simulación facilita el proceso para
analizar, estudiar y comprender el comportamiento de la dinámica del sistema. Esta
etapa es fundamental para el diseño de algoritmos de control. Cuando el modelo
matemático del sistema mecatrónico o robot manipulador es lo suficientemente
completo entonces la simulación proporciona un medio virtual del sistema real.
El propósito de este capı́tulo se ubica en preparar al lector para conocer mejor
el entorno de programación, programar y manejar en forma solvente el lenguaje
de MATLAB. En otras palabras, proporcionar todos los elementos necesarios de
programación (variables, datos, instrucciones y funciones) para realizar diversas
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
1.2
Componentes
7
aplicaciones para las áreas de robótica y mecatrónica.
1.2 Componentes
E
l ambiente de programación de MATLAB es amigable al usuario, y
está compuesto por una interface gráfica con varias herramientas distribuidas
en ventanas que permiten programar, revisar, analizar, registrar datos, utilizar
funciones, historial de comandos y desarrollar diversas aplicaciones. La forma
de iniciar el paquete MATLAB es realizando doble click sobre el icono
colocado en el escritorio de Windows (Windows desktop). La pantalla principal de
MATLAB se presenta en la figura 1.1 la cual contiene la interface gráfica de usuario
con varias ventanas como la de comandos (Command Window fx >>), manejo
de archivos, espacio de trabajo (Workspace), historial de comandos (Command
History), directorio de trabajo y aplicaciones como Simulink.
Figura 1.1 Ambiente de programación de MATLAB.
Es recomendable personalizar el ambiente de programación de MATLAB para una
fácil interacción con manejos de archivos, datos y herramientas de programación.
Por ejemplo, para integrar la ventana del editor dentro del ambiente gráfico se logra
oprimiendo el icono del editor de texto colocado en la esquina superior izquierda,
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
8
Capı́tulo 1: Conceptos básicos
justo abajo de la opción File del menú principal, posteriormente se hace click sobre
la flecha colocada en la esquina superior derecha de la ventana del editor. La figura
1.2 muestra el ambiente configurado.
Figura 1.2 Editor de texto integrado en el ambiente de programación.
Generalmente el usuario registra sus archivos en una carpeta o directorio predefinido;
para dar de alta dicho directorio al momento de simular aparecerá el mensaje que
se muestra en la figura 1.3. Seleccionar Add to Path para que MATLAB realice la
simulación desde esa trayectoria de trabajo, de esta forma las trayectorias de otros
directorios que estén habilitadas no se perderán, por lo que la trayectoria del usuario
se añade a las ya existentes.
Las componentes principales del ambiente de programación de MATLAB son:
Herramientas de escritorio y ambiente de desarrollo.
Librerı́as.
Lenguaje MATLAB.
Gráficas.
Interfaces externas/API.
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
1.2
9
Componentes
Figura 1.3 Habilitación del directorio de trabajo de usuario.
1.2.1
1.2.1. Herramientas de escritorio y ambiente de desarrollo
MATLAB tiene un conjunto de herramientas que facilitan el uso de funciones,
comandos, programas, datos, variables y archivos (desktop tools and development
environment). La mayorı́a de las herramientas son interfaces gráficas donde el usuario
interacciona con comandos, funciones, editor de texto, variables y datos. Por ejemplo
MATLAB desktop, Command Windows, Simulink, ayuda en lı́nea, analizador de
código, navegador de archivos, etcétera.
1.2.2
1.2.2. Librerı́as
Las librerı́as de MATLAB son un conjunto muy grande de funciones y algoritmos
matemáticos que van desde funciones trigonométricas, operaciones básicas con
matrices hasta funciones complejas como análisis de regresión, inversa de matrices,
funciones Bessel, transformada rápida de Fourier, Laplace, programación de DSP’s,
microprocesadores, procesamiento de imágenes, video y tarjetas de instrumentación
electrónica, entre otras ventajas.
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
10
Capı́tulo 1: Conceptos básicos
1.2.3
1.2.3. Lenguaje
El lenguaje de programación de MATLAB es de alto nivel y permite programar
matrices, arreglos e incorporar instrucciones de control de flujo del programa,
funciones, comandos y estructura de datos.
MATLAB contiene una diversidad en comandos que facilitan al usuario la
implementación del problema a simular. Los comandos son funciones muy especı́ficas
que tienen un código depurado y que no se encuentra disponible al usuario.
El lenguaje de programación de MATLAB contiene todos los elementos de programación necesarios para desarrollar aplicaciones en realidad virtual, programación de
robots manipuladores, análisis de regresión y estadı́sticos de señales experimentales, procesamiento y extracción de rasgos distintivos de imágenes y video, ası́ como
varias aplicaciones más en ingenierı́a y ciencias exactas.
1.2.4
1.2.4. Gráficos
MATLAB contiene un enorme número de funciones que facilitan la representación
gráfica y visualización de variables, funciones, vectores, matrices y datos que pueden
ser graficados en 2 y 3 dimensiones. Incluye también funciones de alto nivel para el
análisis y procesamiento de gráficas, datos experimentales o de simulación, video e
imágenes, animación y presentación de sólidos, ası́ como el desarrollo de aplicaciones
con interface gráfica para incluir menús con botones, barras deslizadoras, diagramas
a bloques, instrumentos de medición y ventanas para seleccionar opciones o
herramientas de la aplicación.
1.2.5
1.2.5. Interfaces externas/API
Hay varias herramientas especiales para escribir programas en lenguaje C, C++,
fortran y Java que interactúen con programas en lenguaje MATLAB facilitando el
proceso de llamada a rutinas desde MATLAB y pase de parámetros, ası́ como la
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
1.3
11
Inicio
programación de puertos (USB, COM, paralelo y serial) que permiten conectar
tarjetas de instrumentación electrónica para adquisición de datos y evaluación
experimental de algoritmos de control.
1.3 Inicio
U
na vez instalado MATLAB, la forma más simple de interaccionar con este
paquete es introduciendo expresiones directamente en la venta de comandos,
por ejemplo iniciando con el tradicional letrero “Hola mundo...” sobre el prompt:
fx >>
disp(’Hola mundo...’) ←
Hola mundo...
fx >>
9+9 oprimir la tecla Enter ←
ans=
18
fx >>
sin(10) oprimir Enter ←
ans=
−0.5440
Cuando no se especifique el nombre de la variable, MATLAB usará el nombre por
default ans que corresponde a un nombre corto de answer. Al especificar una variable
el resultado se desplegará con el nombre de esa variable, por ejemplo:
fx >>
z=9+9 Enter ←
z=
18
fx >>
y=sin(10) Enter
←
y=
−0.5440
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
12
Capı́tulo 1: Conceptos básicos
MATLAB funciona como calculadora, sobre el prompt de la ventana de comandos se
pueden escribir expresiones aritméticas mediante los operadores “+”,“-”,“/”, “*”,
“∧ ” para la suma, resta, división, multiplicación y potencia, respectivamente. La
tabla 1.1 contiene los operadores aritméticos básicos.
Por ejemplo, similar a una calculadora se pueden evaluar expresiones como:
fx >>
(10.3+8*5/3.33)∧5 ←
ans=
5.5296e + 006
fx >>
ans∧ 2 ←
ans=
3.0576e + 013
Tabla 1.1 Operadores aritméticos básicos
Adición
+
Sustracción
-
Multiplicación
*
División
/
División left
\
Potencia
∧
Evaluación
()
MATLAB utiliza el operador ; para deshabilitar la opción de desplegado en la
ventana de comandos. Por ejemplo,
fx >>
z=9+9; ←
fx >>
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
1.3
13
Inicio
es decir, con el operador ; MATLAB no exhibirá ningún valor de variable o función.
El lenguaje MATLAB permite insertar comentarios usando el operador % el cual
deberá emplearse por cada lı́nea de comentarios. Por ejemplo:
fx >>
w=10+20 %Suma de dos enteros. ←
w=
30
fx >>
sign(-8999.4) %Obtiene el signo de un número. ←
ans=
−1
Obsérvese que al ejecutarse la instrucción o la sentencia el comentario no se despliega.
Los comentarios sirven para documentar aspectos técnicos de un programa.
Herramienta de ayuda de MATLAB
MATLAB tiene una extensa gama de herramientas para solicitar ayuda en lı́nea
(help), ya sea de manera general o en forma especı́fica buscar información para una
instrucción comando o función, ası́ como sus principales caracterı́sticas y ejemplos
didácticos (demos).
En la ventana de comandos puede solicitar ayuda de la siguiente forma:
fx >>
help ←
fx >>
helpwin ←
fx >>
help general ←
fx >>
helpdesk ←
fx >>
help if ←
fx >>
help demo ←
La ayuda en lı́nea del programa MATLAB también proporciona información sobre
los demos y tutoriales que facilitan el aprendizaje del paquete.
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
14
Capı́tulo 1: Conceptos básicos
Para suspender la ejecución de un programa
Cuando MATLAB se encuentra realizando la ejecución de un programa o cálculo
computacional en la esquina inferior izquierda aparecerá un icono que indica busy.
Sin embargo, hay ocasiones que toma mucho tiempo la ejecución del programa debido
a que se encuentra realizando operaciones complicadas, funciones discontinuas o con
números muy grandes, en cuyo caso despliega Inf, que significa infinito.
fx >>
sinh(1345e34) %Obtiene el seno hiperbólico de un número. ←
ans=
Inf
La siguiente expresión genera un lazo infinito de ejecución. Es decir el programa se
quedará de manera indefinida en ejecución.
fx >>
←
while 1 end %Genera un ciclo infinito de ejecución del programa.
En este caso, hay ocasiones donde es recomendable interrumpir el proceso mediante
la combinación de teclas CRTL C (presionar simultáneamente CONTROL y la tecla C).
Para que tenga efecto CRTL C es muy importante que se encuentre sobre la
ventana de comandos; si no es el caso primero haga click sobre dicha ventana,
y posteriormente CRTL C. De lo contrario no tendrá efecto la acción de suspender
programa.
Finalizar la sesión de MATLAB
Para finalizar MATLAB simplemente escribir en la ventana de comandos lo siguiente:
fx >>
fx >>
quit ←
exit ←
También se pueden usar las siguientes opciones para finalizar la sesión con MATLAB:
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
1.4
15
Lenguaje
Desde el teclado, presionar simultáneamente CRTL Q o CONTROL con q
Presionar simultáneamente las teclas ALT F y después la opción Exit
Con el mouse sobre el menú de MATLAB click sobre File y click en Exit.
1.4 Lenguaje
E
l lenguaje de programación de MATLAB está compuesto por un conjunto
de reglas gramaticales y sintaxis para escribir correctamente las variables,
constantes, operadores, expresiones, funciones y todos los elementos que forman
parte de la programación.
Las variables, constantes, operadores y funciones forman una expresión la cual
será procesada por un analizador léxico y sintáctico antes de ser ejecutada por
la computadora. A diferencia de otros lenguajes, las expresiones en MATLAB
involucran matrices.
1.4.1
1.4.1. Variables
En el lenguaje de MATLAB las variables no requieren ningún tipo de declaración
o definición. Esto tiene varias ventajas ya que facilita la programación. Cuando
MATLAB encuentra el nombre de una nueva variable, automáticamente crea la
variable y le asigna una localidad de memoria. Por ejemplo,
fx >> error posición=20
esta variable es vista por MATLAB desde el punto de vista de programación como
una matriz de dimensión 1 × 1.
Los nombres usados para referenciar a las variables, funciones y otro tipo de objetos
o estructuras definidos por el usuario se les conocen como identificadores. Los
nombres de identificadores constan de una letra del idioma castellano como caracter
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
16
Capı́tulo 1: Conceptos básicos
inicial (con excepción de la letra ñ y sin acentos) seguido de cualquier
número de letras, dı́gitos, y también se puede usar el guión bajo (underscore ).
MATLAB distingue las letras mayúsculas de las minúsculas; por ejemplo la variable
A es diferente a la variable a. La longitud de los nombres de las variables puede
ser cualquier número de caracteres. Sin embargo, MATLAB usa los primeros 63
caracteres del nombre (namelengthmax=63) e ignora los restantes. Por lo tanto,
para distinguir variables es importante escribir los nombres de las variables con un
máximo de 63 caracteres.
La función genvarname es útil para crear nombres de variables que son válidas
y únicas. Por ejemplo, cadena=genvarname({’A’, ’A’, ’A’, ’A’}) produce la
siguiente salida: cadena = ’A’ ’A1’ ’A2’ ’A3.
MATLAB puede trabajar con varios tipos de variables: en punto flotante, flotante
doble, enteros, enteros dobles, enteros cortos o tipo char, cadenas de caracteres,
expresiones simbólicas, etc. Los diversos tipos de variables son interpretados como
matrices.
Para saber qué tipos de variables estamos usando podemos utilizar el comando whos
como en los ejemplos siguientes, primero se declaran las variables y posteriormente
se usa la función whos:
fx >>
i=9 ←
i=
9
fx >>
x=3.35 ←
x=
3.3500
fx >>
cadena= ’esta es una cadena de caracteres’ ←
cadena=
’esta es una cadena de caracteres’
fx >>
A=[2 3; 3 4] ←
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
1.4
17
Lenguaje
A=
fx >>
2
3
3
4
whos ←
=
Name
Size
Bytes Class Attributes
A
2×2
32
double
cadena
1 × 32
64
char
i
1×1
8
double
x
1×1
8
double
La columna Size indica la cantidad de memoria asignada a esa variable, mientras
que la columna Bytes Class indica el tipo de variable. Obsérvese que MATLAB
trata a todas las variables como matrices para propósitos de programación.
Al terminar una sesión, o cuando ya no se usen la variables debido a que se
desea trabajar con otro programa es recomendable limpiar todas las variables para
liberar memoria. Para realizar lo anterior, se puede hacer con cada variable, por
ejemplo clear i, posteriormente clear x y finalmente clear cadena. Otra forma
de realizarlo es limpiando simultáneamente las tres variables a través de: clear i x
cadena o cuando hay una gran cantidad de variables, entonces es conveniente usar
clear all.
1.4.2
1.4.2. Números
La notación convencional que usa MATLAB para la representación de números
es la decimal con un punto decimal, signo ± y un número determinado de dı́gitos
después del punto decimal, esto depende del tipo de formato numérico empleado (ver
format). En la notación cientı́fica se usan potencias de diez. También se emplean
constantes tales como e, π. Los números complejos o imaginarios emplean i o j. Todos
los números son almacenados internamente utilizando el formato long en notación
estándar de la IEEE para punto flotante con precisión finita de 16 dı́gitos decimales
y un rango finito de 10−308 a 10308 .
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
18
Capı́tulo 1: Conceptos básicos
Algunos ejemplos de números son:
2.33
9.999
−105
0.005
8.345e88 9 + i3.2
3.1416
8e5i
eπ
sen(π)
90
180
√
2
La tabla 1.2 muestra algunas de las constantes más usadas en el área de la ingenierı́a.
Tabla 1.2 Constantes útiles
pi
3.141592653589793.
√
Número imaginario i
−1
j
Igual que el número imaginario i
eps
Punto flotante con precisión relativa, ε = 2−52
realmin
El número en punto flotante más pequeño, 2−1022
El número más grande en punto flotante, (2−ε)1023
realmax
inf
Infinito ∞
NaN
No es un número
1.4.3
1.4.3. Formato numérico
La forma para desplegar funciones, variables y cualquier tipo de dato por MATLAB
se realiza a través del comando format la cual controla el formato numérico de los
valores desplegados. Es decir, modifica el número de dı́gitos para el desplegado de
los datos. Este comando solo afecta a los números que son desplegados, no al proceso
de cómputo numérico o al registro de las variables o datos.
format short ←
fx >>
fx >>
x= [ 9/8 8.3456e-8] ←
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
1.4
19
Lenguaje
x=
1.1250 0.0000
Observe que en el caso de la constante 8.3456e-8 lo despliega con 0.0000 debido a
los lı́mites del formato corto format short.
Ahora note la diferencia con format short e
fx >>
fx >>
format short e ←
x ←
x=
1.1250e + 000 8.3450e − 008
Es posible eliminar el formato extendido para desplegar el resultado de 9/8 y al
mismo tiempo mantener la caracterı́stica de desplegado para la segunda componente
de la variable x de la siguiente forma:
fx >>
fx >>
format short g ←
x ←
x=
1.1250 8.3450e − 008
Para desplegar la información de x con mayor número de dı́gitos sin utilizar la
notación cientı́fica, entonces
fx >>
format long ←
x=
1.125000000000000 0.000000083450000
Para saber qué tipo de format está actualmente activo se usa el comando get:
fx >>
get(0,’format’) ←
ans=
long
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
20
Capı́tulo 1: Conceptos básicos
El formato numérico que por default emplea MATLAB es el formato corto (short):
format short. Sin embargo, es posible utilizar otros tipos de formatos como:
extendido, notación cientı́fica, punto fijo, punto flotante, formato de ingenierı́a, etc.
Los formatos numéricos que contiene el comando format se encuentran resumidos
en la tabla 1.3.
Tabla 1.3 Opciones del comando format
Tipo
Descripción
short
Formato por default. Despliega 4 dı́gitos después del punto decimal. Por ejemplo
3.1416
long
Formato para punto fijo con 15 dı́gitos después del punto decimal para variables
tipo doble y 7 dı́gitos para variables tipo simple. Por ejemplo x=4.123456789012345
Formato para punto flotante con 4 dı́gitos después del punto decimal. Por ejemplo:
ShortE
longE
1.4567e+000.
Variables enteras que almacenan números flotantes con más de 9 dı́gitos no serán
desplegados en la notación cientı́fica.
Punto flotante con 15 dı́gitos después del punto decimal para datos tipo doble y 7
dı́gitos después del punto decimal para variables tipo simple.
Ejemplo x=4.123456789012345+e000.
Variables enteras que almacenan números flotantes con más de 9 dı́gitos no serán
desplegados en notación cientı́fica.
shortG
Formato para punto fijo o punto flotante, 4 dı́gitos después del punto decimal.
Ejemplo 3.1416.
longG
Formato para punto flotante o fijo con 14 o 15 dı́gitos después del punto decimal
para variables tipo doble y 6 o 7 dı́gitos después del punto decimal para variables
tipo simple. Ejemplo 8.01234567891234.
shortEng
longEng
Alfaomega
Formato para ingenierı́a tiene 4 dı́gitos después del punto decimal y una potencia
que es múltiplo de tres. Ejemplo 3.1416e+000.
Formato para ingenierı́a tiene 16 dı́gitos después del punto decimal y una potencia
que es múltipo de tres. Ejemplo 3.14159265358979e+000.
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
1.4
21
Lenguaje
El comando format también proporciona representación hexadecimal, tipo proporción ab y de tipo bancaria como se explica en la tabla 1.4.
Tabla 1.4 Opciones del comando format
Tipo
Descripción
rat
Despliega en forma de proporción de números enteros. Ejemplo 17/50
hex
Representación en formato hexadecimal. Ejemplo a0fcbdf34
bank
Despliega tipo cantidad bancaria (pesos y centavos). Ejemplo 3.14
1.4.4
1.4.4. Operadores
Los operadores en MATLAB juegan un papel determinante ya que manipulan a las
variables y funciones. Adicional a los operadores aritméticos de la tabla 1.1, hay
operadores especı́ficos para funciones, operaciones lógicas, relacionales, estructuras
de datos, uniones y matrices.
A continuación se presentan diversos tipos de operadores de utilidad en MATLAB.
Operador colon :
El operador colon : (dos puntos verticales) es uno de los operadores más importantes
para programar, se emplea para diferentes formas, como por ejemplo en MATLAB
técnicamente se conoce como vectorización al proceso de generar una secuencia de
número usando 1:10.
fx >>
1:10 ←
ans =
1 2
3 4 5 6 7
8 9 10
En este caso el incremento es de uno en uno. Si se requiere un paso de incremento
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
22
Capı́tulo 1: Conceptos básicos
especı́fico, por ejemplo 2, entonces se procede de la siguiente forma
fx >>
1:2:10 ←
ans=
1 3 5 7
9 10
El valor del paso de incremento también puede ser menor que 1, por ejemplo
fx >>
1:0.1:2 ←
ans=
1 1.1 1.2 1.3
1.4 1.5 1.6 1.7 1.8 1.9 2
En ocasiones es conveniente que sea negativo
fx >>
10:-1:2 ←
ans=
10
9
8
7
6
5
4
3
2
En otras aplicaciones es útil realizar una variación desde -8 a 8 con incrementos de
pasos de 0.5, por ejemplo
fx >>
-8:0.5:8 ←
ans=
−8
−7.5
−7
−6.5
−6
−5.5
−5
−4.5
−4
−3..5
−3
−2.5
−2
−1.5
−1
−0.5
0
0.5
1
1.5
2
2.5
3
3.5
4
4.5
5
5.5
6
6.5
7
7.5
8
Operador ()
Los operadores paréntesis () son utilizados en diversas operaciones matemáticas, por
ejemplo: evaluar funciones como en el caso de y=sin(t). También para referenciar
a elementos de matrices A(2, 3). En expresiones aritméticas indican precedencia
y la forma de agrupar variables x=3*(y+j)*r-i*(4+r). Resultan de utilidad para
evaluar condiciones lógicas if (x>3) x=3 end. Cuando se trata de evaluar potencias
los paréntesis determinan los niveles de jerarquı́a para realizar operaciones desde las
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
1.4
23
Lenguaje
más internas hasta las externas (7 + (r ∗ (x2 + 8))6 − j + sen(t3 )5 )3 .
Operador semicolon ;
El operador punto y coma alineados en forma vertical o mejor conocido como
semicolon ; tiene varias funciones. Una de ellas se encuentra relacionada con el
desplegar el resultado que tienen las variables, constantes, funciones o gráficas.
Cuando se inserta al final de la expresión, instrucción o comando se inhabilita el
desplegado. Por ejemplo:
fx >>
w=sin(pi/2); ←
fx >>
Si el operador ; no se coloca al final de la instrucción, entonces se produce el
desplegado del valor de la variable w
fx >>
w=sin(pi/2) ←
w=
1
Otra funcionalidad del operador ; es generar renglones en matrices. Por ejemplo,
fx >>
A=[ 19 3; 4 5 ] ←
A=
19 3
4
5
El operador ; que precede al número 3 y antecede al número 4 genera un renglón
de esta matriz. En este caso el primer renglón lo forman los números 19 y 3, y
para el segundo renglón formado por 4 y 5. Obsérvese que después del corchete ]
no se inserta el operador ; ya que en este caso afecta el desplegado, mientras que
dentro de los corchetes [ ; ] significa que genera un renglón en la matriz. Es decir
el operador ; tiene dos funciones muy diferentes. Por cada ; dentro de corchetes se
generará un renglón de la matriz. Es muy importante que quede claro la aplicación
de este operador para evitar problemas de programación.
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
24
Capı́tulo 1: Conceptos básicos
Si en el anterior ejemplo, en la matriz A no se insertara el operador ; entonces el
resultado quedarı́a de la siguiente forma:
fx >>
A=[ 19 3 4 5 ] ←
A=
19 3 4 5
Para ver más detalles del operador ; referirse a la sección de Matrices y arreglos.
Operador ,
El operador coma , tiene más de una función en MATLAB. Por ejemplo, cuando
se emplea en funciones indica la separación de los argumentos como en el caso
de y=sin(t,x) o en w=sinh(t,x,y,z,p). Para referenciar a los elementos de una
matriz se especifica el número de renglón y de la columna separados por una coma
A(3,4). En matrices indica la separación de los elementos de un renglón A=[ 5,6,7,8;
8,3,0,2].
Operador ’
El operador ’ se relaciona con el manejo de datos tipo char o cadena de caracteres.
También representa la transpuesta de una matriz.
En relación a caracteres se emplea de la siguiente forma:
fx >>
dato char= ’a’ ←
dato char=
a
Para almacenar una cadena de caracteres, se procede de la siguiente forma:
fx >>
cadena= ’una cadena de letras’ ←
cadena=
una cadena de letras
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
1.4
Lenguaje
25
El operador ’ es ampliamente utilizado en funciones donde el pase de parámetros es
a través de cadenas de texto como en disp(’Hola, mundo...’).
En matrices el operador ’ representa la matriz transpuesta, es decir A’=AT . Para
mayor detalle de este operador en matrices ver la sección de Matrices y arreglos.
Operador %
En el lenguaje de MATLAB se pueden insertar comentarios usando el operador %.
Los comentarios son importantes en todo programa ya que permiten la documentación técnica del algoritmo o aplicación a implementar. Por ejemplo,
clc ; %limpia la pantalla de la ventana de comandos\
clear ; %limpia espacio de trabajo\
; %ası́ como todas las variables que ocupen memoria
t=0:0.1:t; %genera base de tiempo
y=sin(t); %genera el vector senoidal
plot(t,y) %gráfica onda senoidal
Operador ˜
El operador tilde ˜ se emplea para deshabilitar una variable de salida de una función.
Es muy útil cuando la función retorna más de una variable y no se requiere usar
todas las variables; supóngase que la función control robot retorna dos variables
(error y par) y la sintaxis es: [error, par ]=control robot(q). No se requiere usar
la variable error, únicamente par, entonces se usa de la forma siguiente:
[~ , par]= control robot(q).
También se emplea como negación en operadores lógicos, por ejemplo en ˜ = que
significa no es igual a.
En las siguientes secciones se irán explicando otros tipos de operadores que se
encuentran directamente relacionados con matrices y en general con la programación
del lenguaje de MATLAB.
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
26
Capı́tulo 1: Conceptos básicos
1.5 Matrices y arreglos
E
n MATLAB una matriz es un arreglo rectangular de datos o números, tienen n
renglones por p columnas; la notación matemática más común para representar
a una matriz es: A ∈ IRn×p . Matrices con una sola columna o renglón significan
vectores para MATLAB, por ejemplo x ∈ IRn×1 o y ∈ IR1×n , respectivamente.
Especial significado representan los escalares cuya interpretación para propósitos de
programación corresponde a una representación de matriz del tipo IR1×1 .
Las entradas de la matriz se conocen como elementos y pueden ser números reales,
números complejos, funciones, operadores, inclusive también pueden ser matrices de
menor dimensión. Es importante destacar que una matriz es un objeto matemático
por sı́ misma, esto es, no representa un número o un escalar. Sin embargo, en lenguaje
MATLAB es posible trabajar a una matriz como un escalar, por ejemplo α ∈ IR1×1 .
La matrices tienen operaciones y propiedades bien definidas. Las operaciones entre
matrices producen una matriz, en contraste operaciones entre vectores pueden
producir un escalar, vector o matriz.
En la figura 1.4 se muestra la representación clásica de una matriz rectangular de n renglones por p columnas (matriz n por p). Las columnas son arreglos verticales. Por ejemplo, la segunda columna está formada por los elementos A(1, 2), A(2, 2), · · ·, A(n, 2) mientras que los renglones son filas horizontales dentro del arreglo rectangular; el segundo renglón está compuesto por
A(2, 1), A(2, 2), · · ·, A(2, p). Las columnas se incrementan de izquierda a derecha,
mientras que los renglones se incrementan de arriba hacia abajo.
⎡
Columna
A(1, 1)
⎢ A(2, 1)
⎢
A = ⎢
..
⎣
.
A(1, 2)
···
A(2, 2)
···
..
.
A(1, p)
⎤
A(2, p) ⎥
⎥
⎥
..
⎦
.
Renglón
A(n, 1) A(n, 2) · · · A(n, p)
Figura 1.4 Componentes de una matriz: elementos A(i, j), renglones y columnas.
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
1.5
27
Matrices y arreglos
Los elementos de una matriz se denotan en MATLAB por A(i, j) donde i = 1 . . . n
representa el i-ésimo renglón y j = 1 . . . p denota la j-ésima columna. Por ejemplo,
en la matriz A ∈ IR5×5 el elemento A(2, 5) significa la componente del segundo
renglón y quinta columna, el cual se muestra en la siguiente expresión:
⎡
A(1, 1) A(1, 2) A(1, 3) A(1, 4)
⎢
⎢ A(2, 1)
⎢
A = ⎢
⎢ A(3, 1)
⎢
⎣ A(4, 1)
A(5, 1)
A(2, 2) A(2, 3) A(2, 4)
A(3, 2) A(3, 3) A(3, 4)
A(4, 2) A(4, 3) A(4, 4)
A(5, 2) A(5, 3) A(5, 5)
A(1, 5)
⎤
⎥
A(2,5) ⎥
⎥
A(3, 5) ⎥
⎥.
⎥
A(4, 5) ⎦
A(5, 5)
En esta matriz en particular los pivotes pueden adquirir valores para referenciar a
los elementos de la matriz A(i, j). Para los renglones i = 1, 2, · · ·, 5 y en las columnas
j = 1, 2 · · · , 5.
Para los propósitos de identificar los elementos de una matriz, éstos pueden ser vistos
sobre un sistema de referencia cartesiano planar cuyo origen se encuentra localizado
en la esquina superior izquierda. Las referencias a elementos se hace con A(i, j).
Es importante aclarar que dicho origen no inicia en A(0, 0); lo que serı́a un error
de sintaxis. El origen inicia con los pivotes asignados en uno, como en A(1, 1); con
esta asignación se hace referencia al primer elemento de la matriz, ubicado en el
primer renglón y primera columna. El elemento A(1, 1) representa el origen o punto
de partida para los pivotes (i, j) donde el ı́ndice del i-ésimo renglón está dado antes
del j-ésimo ı́ndice de la columna. Los renglones de una matriz son numerados de
arriba hacia abajo y las columnas de izquierda a derecha.
Se debe tener claro que el lenguaje MATLAB no admite referencias a elementos de
una matriz con pivotes (i, j) asignados en cero, negativos, números reales o números
enteros de magnitud mayor a la dimensión de la matriz. En otras palabras, en las
referencias a elementos de la matriz únicamente con números enteros o naturales
dentro de los lı́mites de la dimensión de la matriz. Por ejemplo, los siguientes
elementos A(0, 0), A(0, 5), A(−5, −1), A(5, 0) son referencias inválidas de una matriz.
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
28
Capı́tulo 1: Conceptos básicos
Es muy común cometer errores al referenciar los elementos o entradas de una
matriz. Por ejemplo las entradas A(i, j) de una matriz A ∈ IRn×p deben satisfacer
lo siguiente:
No usar en la referencia de las entradas de una matriz A la posición A(0, 0)
o números negativos A(−3, −5), ni números reales como A(1.32, 2.67). En
todos estos ejemplos existe error de sintaxis y la ejecución de un programa
no puede realizarse hasta no corregir dicho error.
Los pivotes i, j son números acotados por 0 < i ≤ n, 0 < j ≤ p. Por ejemplo
si A ∈ IR3×5 , existe un error en la referencia del elemento A(4, 6), puesto
que i ≤ 3 y j ≤ 5
Los pivotes i, j son números enteros positivos (números naturales) i, j ∈ N .
Inicializando una matriz
La matriz se inicializa por corchetes A = [] directamente en la ventana de comandos
(Command Window) fx >> realizar los siguientes ejercicios:
Las entradas de una matriz puede ser de la siguiente forma: separando los
elementos de un renglón por espacios en blanco o por comas:
fx >>
A=[2, 3, 4, 78.3, 45] ←
A=
2 3 4 78.3 45
Es posible emplear únicamente espacios en blanco
fx >>
A=[2 3 4 78.3 45] ←
A=
2 3 4 78.3 45
Combinando comas y espacios en blanco es otra posibilidad de inicializar una
matriz
fx >>
Alfaomega
A=[2, 3 4, 78.3 45] ←
A=
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
1.5
29
Matrices y arreglos
2 3
4 78.3 45
Usar el operador semicolon ; para indicar el fin del renglón y generar otro más
fx >>
A=[2, 3 ,4 ; 5 6
A=
8 ; 7, 8, 9] ←
2 3 4
5 6 8
7 8 9
De tomarse en cuenta que el operador ; además de emplearse en matrices tiene
otra función cuando se inserta al finalizar una variable, constante o función
desactiva la opción de desplegado.
Los elementos del i-ésimo renglón o de la j-ésima columna de una matriz A son
denotados como: A(i, j), la expresión A(3, 4) representa un elemento de la matriz
que se encuentra localizado en el tercer renglón y cuarta columna.
Considérese la siguiente matriz A ∈ IR4×4
⎡
⎤
1 3 3 2
⎢ 0 3 4 12 ⎥
⎢
⎥
A = ⎢
⎥
⎣ 12 34 1 23 ⎦
9
7
2
3
Se inicializa como:
fx >>
A=[1,3,3,2; 0, 3, 4,12; 12,34,1,23;9,7,2,3] ←
Para sumar todos los elementos del cuarto renglón de A se procede de la siguiente
forma: A(4, 1) + A(4, 2) + A(4, 3) + A(4, 4) = 9 + 7 + 2 + 3 = 21, es decir
fx >>
A(4,1)+A(4,2)+A(4,3)+A(4,4) ←
ans =
21
El resultado es diferente a sumar A(1, 4)+A(2, 4)+A(3, 4)+A(4, 4) = 2+12+23+3 =
40.
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
30
fx >>
Capı́tulo 1: Conceptos básicos
A(1,4)+A(2,4)+A(3,4)+A(4,4) ←
=
40
En general A(i, j) = A(j, i) o A = AT . Si la matriz es simétrica, entonces se cumple
A(i, j) = A(j, i) o A = AT . En MATLAB el operador que representa la matriz
transpuesta es ’. Por ejemplo,
fx >>
B=A ’ ←
B=
1
0
12 9
3
3
34 7
3
4
1
2
2 12 23 3
Debe tenerse cuidado con el empleo del operador ’, además de su uso en matrices
también se emplea en cadenas de caracteres.
El operador colon : se puede emplear en matrices para referenciar a una porción de
la matriz. Por ejemplo, sea una matriz A ∈ IR5×5
⎡
⎤
1
2
3
4
5
⎢
⎥
7
8
9 10 ⎥
⎢ 6
⎢
⎥
⎥
A = ⎢
⎢ 11 12 13 14 15 ⎥
⎢
⎥
⎣ 16 17 18 19 20 ⎦
21 22 23 24 25
entonces las referencias A(1 : 5, 1), A(1 : 5, 2), A(1 : 5, 3), A(1 : 5, 4), A(1 : 5, 5)
representan los 5 renglones de las columnas j = 1 · · · 5, respectivamente
⎡ ⎤
⎡ ⎤
2
1
⎢ ⎥
⎢ ⎥
⎢ 7 ⎥
⎢ 6 ⎥
⎢ ⎥
⎢ ⎥
⎥
⎢ ⎥
A(1 : 5, 1) = ⎢
⎢ 11 ⎥ A(1 : 5, 2) = ⎢ 12 ⎥
⎢ ⎥
⎢ ⎥
⎣ 17 ⎦
⎣ 16 ⎦
21
Alfaomega
22
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
1.5
31
Matrices y arreglos
⎡
3
⎤
⎡
4
⎤
⎢ ⎥
⎢ ⎥
⎢ 8 ⎥
⎢ 9 ⎥
⎢ ⎥
⎢ ⎥
⎢
⎥
⎥
A(1 : 5, 3) = ⎢ 13 ⎥ A(1 : 5, 4) = ⎢
⎢ 14 ⎥
⎢ ⎥
⎢ ⎥
⎣ 18 ⎦
⎣ 19 ⎦
⎡
23
⎤
24
5
⎢ ⎥
⎢ 10 ⎥
⎢ ⎥
⎥
A(1 : 5, 5) = ⎢
⎢ 15 ⎥
⎢ ⎥
⎣ 20 ⎦
25
Para obtener las 5 columnas de cada renglón se representa por A(1, 1 : 5), A(2, 1 :
5), A(3, 1 : 5), A(4, 1 : 5), A(5, 1 : 5)
A(1, 1 : 5) = [ 1 2 3 4 5 ] A(2, 1 : 5) = [ 6 7 8 9 10 ]
A(3, 1 : 5) = [ 11 12 13 14 15 ] A(4, 1 : 5) = [ 16 17 18 19 20 ]
A(5, 1 : 5) = [ 21 22 23 24 25 ]
Generando matrices básicas
Existen varias formas para generar matrices básicas en MATLAB, se pueden generar
con funciones especı́ficas, con funciones definidas por el usuario, o con el uso del
operador colon :. A continuación se enlistan varias opciones para generar matrices
básicas en MATLAB:
Introduciendo una lista de elementos explı́citos: A = [2, 3, 67; 5.3, 3.3, 2.4; 2, 9, 1]
Cargando matrices de archivos de datos experimentales o externos en formato
de columnas: matriz=load(’datos.dat’)
Generando matrices usando funciones que retornan matrices: matriz=mi matriz(n,m)
Usando funciones de MATLAB por ejemplo: A=zeros(3,3).
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
32
Capı́tulo 1: Conceptos básicos
La tabla 1.5 muestra las opciones más comunes (funciones especiales y por asignación
de datos) que permiten generar matrices básicas.
Tabla 1.5 Funciones especiales para generar matrices básicas
Nombre de la función
zeros(n,m)
ones(n,m)
rand(n,m)
randn(n,m)
Descripción y caracterı́sticas
Genera una matriz IRn×m donde todos sus elementos son
cero.
Genera una matriz IRn×m donde todos los elementos tienen
el valor 1.
Genera una matriz IRn×m donde las entradas son elementos
aleatorios distribuidos de manera uniforme.
Genera una matriz IRn×m donde las entradas son elementos
aleatorios distribuidos normalmente.
Retorna una matriz cuadrada IRn×n donde las entradas son
magic(n)
números enteros desde 1 hasta n2 .
El número n debe ser mayor o igual a 3. .
Genera una matriz rectangular IRn×m donde los elementos
eye(n,m)
con ı́ndices (i, j) tal que i = j tienen el valor de 1, y para
entradas i = j tienen valor de 0.
Si n = m, entonces la matriz es cuadrada y diagonal.
Genera una matriz rectangular IRn×m donde todos los
A(1:n, 1:m)=α
elementos de la matriz A adquieren el valor del escalar α.
Cuando n = m, entonces retorna una matriz cuadrada.
Genera una matriz rectangular desde un archivo ASCII o
A=load
de texto load(’nombre archivo.tex’); el archivo puede
tener datos experimentales y asignarlos a una matriz para
su procesamiento o análisis.
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
1.5
33
Matrices y arreglos
Por ejemplo, en la ventana de comandos se puede comprobar lo siguiente:
fx >>
A=zeros(3,3) ←
A=
fx >>
0 0
0
0 0
0
0 0
0
B=10*ones(3,3) ←
B=
10 10 10
10 10 10
10 10 10
fx >>
C=randn(3,3) ←
C=
0.5377
0.8622
−0.4336
1.8339
0.3188
0.3426
−2.2588 −1.3077
3.5784
Otra forma simple de generar matrices es por medio de la siguiente expresión:
fx >>
D(1:3,1:3)=3.1416 ←
D=
3.1416 3.1416 3.1416
3.1416 3.1416 3.1416
3.1416 3.1416 3.1416
Por medio de un archivo con extensión m es posible generar una matriz de la
siguiente forma. Crear en el editor de texto de MATLAB un archivo con la siguiente
información:
A=[ 8
3
7;
7.3
3.1416;
7.98 6.67
0.001];
6
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
34
Capı́tulo 1: Conceptos básicos
salvar el archivo como datos.m en la carpeta del usuario, entonces sobre el
prompt de la ventana de comandos teclear:
fx >>
datos ←
A=
8.0000 3.0000 7.0000
6.0000 7.3000 3.1416
7.9800 6.6700 0.0010
Otra forma de generar matrices es por medio de la función load la cual lee archivos
binarios y de texto. Por medio de un editor de texto generar un archivo de datos de
texto en arreglo rectangular separados con espacios en blanco. Por ejemplo, grabar en
la carpeta de trabajo del usuario un archivo de texto denominado datos1.dat
con la siguiente información:
datos1.dat
2.0000
3.0000
4.0000
5.0000
6.0000
7.0000
6.7000
8.3000
4.5450
Para generar la matriz con la información del archivo datos1.dat
siguiente sentencia en el prompt de la ventana de comandos:
fx >>
utilizar la
A=load(’datos1.dat’) ←
A=
2.0000 3.0000 4.0000
5.0000 6.0000 7.0000
6.7000 8.3000 4.5450
Observe que con esta asignación en la matriz A, la información se encuentra lista para
ser procesada por cualquier operación de matrices previamente vista. En robótica
y mecatrónica es común grabar archivos experimentales o resultados de simulación
para ser analizados con MATLAB usando un procedimiento parecido al planteado.
El formato del archivo debe ser que los datos estén distribuidos en renglones y
columnas separadas por espacios en blanco.
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
1.5
35
Matrices y arreglos
Concatenación
Al proceso de unir pequeñas matrices para hacer una más grande se le denomina
concatenación. Los sı́mbolos [ ], mejor conocidos como corchetes, representan en
MATLAB el operador concatenación. Por ejemplo, se puede iniciar con una matriz
diagonal 4 × 4 de la siguiente forma:
fx >>
A=eye(4,4) ←
A=
1 0
0 0
0 1
0 0
0 0
1 0
0 0
0 1
Escribir en el prompt de la ventana de comandos la siguientes expresión para obtener
una matriz de dimensión 8 × 8.
fx >>
B=[A A+10; A+20 A+30] ←
B=
1
0
0
0
11 10 10 10
0
1
0
0
10 11 10 10
0
0
1
0
10 10 11 10
0
0
0
1
10 10 10 11
21 20 20 20 31 30 30 30
20 21 20 20 30 31 30 30
20 20 21 20 30 30 31 30
20 20 20 21 30 30 30 31
Borrando columnas y renglones
MATLAB permite borrar renglones y columnas de una matriz usando el operador
concatenación [ ]. Por ejemplo, para borrar la segunda columna de la matriz A
previamente definida en el apartado inmediato anterior se procede de la siguiente
forma:
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
36
fx >>
Capı́tulo 1: Conceptos básicos
A(:,2)=[] ←
A=
1
0 0
0
0 0
0
1 0
0
0 1
obsérvese que por borrar la segunda columna, la nueva matriz resultante A adquiere
una dimensión de 4 × 3.
Ahora, si se desea borrar el primer renglón de la anterior matriz resultante A,
entonces
fx >>
A(1,:)=[] ←
A=
0
0 0
0
1 0
0
0 1
resulta una matriz A∈ IR3×3 .
Si un elemento de una matriz se quiere borrar, resulta un error, la expresión
A(1,1)=[] es inválida. Sin embargo, usando subı́ndices borra un simple elemento
o una secuencia de elementos de la matriz y rehace los elementos restantes de la
matriz en un vector renglón.
Por ejemplo, para borrar el primer elemento de A se emplea la siguiente expresión
fx >>
A(1:1)=[] ←
A=
0 0 0
1 0 0 0 1
Nótese que los elementos restantes se quedan arreglados en la forma de un vector
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
1.5
37
Matrices y arreglos
renglón. Si en lugar de borrar el primer elemento, se desea borrar los 5 primeros
elementos, entonces se procede de la siguiente forma:
fx >>
A(1:5)=[] ←
A=
0 0
0 1
Expansión de escalares
Los escalares pueden ser combinados con algunas operaciones con matrices. Por
ejemplo, en la sustracción un escalar y una matriz A puede ser combinados como
A-3.3 de la siguiente manera:
fx >>
A=[1 2; 5 6]; ←
fx >>
B=A-3.3 ←
B=
−2.3000 −1.3000
1.7000
2.7000
es necesario resaltar que la expresión A-3.3 no produce error, es una operación válida
de acuerdo a la sintaxis del lenguaje de MATLAB.
Además debe tomarse en cuenta que las operaciones entre escalares y matrices
respetan la propiedad conmutativa. Es decir, A-3.3=-3.3+A. De manera similar
para la suma, división y multiplicación entre escalares y matrices: A+3.3=3.3+A,
A*3.3=3.3*A, A/3.3=1/3.3*A.
Usando la expansión de escalares, MATLAB asigna un escalar especı́fico a todos
los ı́ndices para el rango indicado en la matriz. Por ejemplo, la siguiente expresión
genera una matriz de dimensión 3 × 3 donde todos sus elementos tienen asignado el
valor 9:
fx >>
B(1:3,1:3)=9 ←
Si la matriz ya existe, entonces esta expresión solo asigna el valor del escalar 9 a
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
38
Capı́tulo 1: Conceptos básicos
los elementos de la matrices (renglones y columnas especificados) de acuerdo a los
ı́ndices indicados. Si la matriz no existe, entonces genera una nueva matriz en este
caso de dimensión 3 × 3 donde todos sus elementos son iguales a 9.
Operaciones básicas con matrices
Hay varias operaciones elementales entre matrices que se realizan con frecuencia
en mecatrónica y robótica. Hay que considerar que se deben respetar las reglas
del álgebra de matrices. Por ejemplo, se debe satisfacer la compatibilidad entre el
número de renglones y columnas de la matriz A con la matriz B:
1) Declarar las matrices por ejemplo: A=[ 2 3 4; 6 7 8; 9 0 9] y B=[ 3 0 1;
5 2 4; 7 8 1]
2) Usar el operador + para realizar la suma: A+B
3) La sustracción se realiza con: A-B
4) La multiplicación: A*B (el número de columnas de la matriz A debe ser igual
al número de renglones de la matriz B).
5) En general la multiplicación no es conmutativa: B*A (el número de columnas de
la matriz B debe ser igual al número de renglones de la matriz A). Cuando las
matrices son diagonales, entonces se satisface la propiedad de conmutatividad
de la multiplicación.
6) El determinante de una matriz se obtiene como: det(A)
7) La inversa de una matriz existe si su determinante es diferente a cero: inv(A)
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
1.5
39
Matrices y arreglos
Debido a que las expresiones de programación van creciendo, es conveniente que
el lector en lugar de seguir utilizando la ventana de comandos, utilice el editor de
texto para grabar todas las operaciones que realiza en nombre archivo.m (M-files)
el cual almacenará instrucciones, variables, operadores, comentarios, funciones,
etc. De esta forma el archivo queda grabado en el directorio del usuario y se
puede reproducir sin ningún problema. Por lo tanto, de aquı́ en adelante será de
utilidad emplear el editor de texto que se encuentra integrado en el ambiente de
programación de MATLAB.
♣ Ejemplo 1.1
Escribir un programa en MATLAB para realizar operaciones básicas entre
matrices (adición , sustracción, inversa, simétrica, antisimétrica, etc). Considere
las siguientes matrices A, B ∈ IR3×3 :
⎡
2 3 4
⎤
⎢
A = ⎣6
9
⎡
3
⎢
B = ⎣5
⎥
7 8⎦
0 9
⎤
0 1
⎥
2 4⎦
7 8 1
Solución
El cuadro de código fuente 1.1 contiene las operaciones más importantes que se
realizan en mecatrónica y robótica usando matrices. Edite el programa que se
muestra a continuación; se recomienda salvarlo como cap1 matrices1.m; y ejecútese
oprimiendo la tecla F5 o realizando click sobre el icono play que se encuentra
localizado en la parte superior del editor de texto.
Recuerde que para visualizar los resultados de las operaciones que le interesan al
final de la lı́nea correspondiente elimine el operador ;.
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
40
Capı́tulo 1: Conceptos básicos
Código Fuente 1.1
Operaciones básicas con matrices
%MATLAB Aplicado a Robótica y Mecatrónica
%Capı́tulo 1 Introducción
%Editorial Alfaomega Fernando Reyes Cortés
%Archivo cap1 matrices1.m
Operaciones básicas con matrices
1
%se definen las matrices de prueba
2
A=[ 2 3 4;
6 7 8;
9 0 9];
B=[ 3 0 1;
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
5 2 4;
7 8 1];
A+B; % suma de matrices
A-B; %resta de matrices
%en general la multiplicación no es conmutativa
A*B;
B*A;
A’ % transpuesta de la matriz A
(A’)’ % transpuesta de la transpuesta = A
As=(A+A’)/2; % parte simétrica
Ask=(A-A’)/2; % parte antisimétrica
As+Ask % matriz original A
A’*A %obtiene una matriz simétrica
A∧ (-1); %inversa de la matriz
det(A) % determinante
inv(A); %inversa de la matriz A
A*inv(A);
inv(A)*A; % matriz identidad
eig(A) % valores propios de la matriz A
26
norm(A) % norma de la matriz A
p=poly(A) %retorna coeficientes del polinomio caracterı́stico de A
27
r=root(p); %retorna raı́ces del polinomio caracterı́stico de A
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
1.5
41
Matrices y arreglos
1.5.1
1.5.1. Arreglos
Las operaciones aritméticas con arreglos o registros son realizadas elemento por
elemento. Es decir, la adición y sustracción son las mismas para arreglos y matrices,
pero la multiplicación es diferente. MATLAB usa como parte de la notación de
multiplicación con arreglos un punto decimal.
La tabla 1.6 muestra la lista de operadores que se utilizan en operaciones con
arreglos.
Tabla 1.6 Operadores de arreglos
Suma
+
Sustracción
-
Multiplicación elemento por elemento
.*
División elemento por elemento
./
División izquierda elemento por elemento
.\
Elevar a una potencia elemento por elemento
.∧
Arreglo transpuesto
.’
Como ejemplo ilustrativo de estos operadores considere lo siguiente:
fx >>
A=[1 3; 4 5] ←
A=
fx >>
1
3
4
5
A.*A ←
ans=
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
42
Capı́tulo 1: Conceptos básicos
1
9
16 25
obsérvese que cada elementos se multiplica por sı́ mismo, es decir los elementos de
la matriz resultante son el cuadrado de cada elemento. Lo mismo sucede para los
demás operadores:
fx >>
A./A ←
ans=
1 1
1 1
fx >>
A.∧ A ←
ans=
1
27
256 3125
Las operaciones con arreglos son muy útiles para construir tablas. Por ejemplo:
fx >>
x=(0:5) ’ ←
x=
0
1
2
3
4
5
fx >>
[n, m]=size(x); ←
n=
6
fx >>
tabla=[x x.∧ 2 x.∧ n] ←
tabla=
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
1.6
43
Gráficas
0
0
0
1
1
1
2
4
64
3
9
729
4 16
4096
5 25 15625
1.6 Gráficas
L
a representación gráfica y visualización de resultados es una herramienta muy
importante en el análisis y diseño de esquemas de control y comportamiento
dinámico. Permite exhibir como el robot o el sistema mecatrónico responde en sus
etapas transitoria y estacionaria cuando se encuentra bajo la acción de control de
una estructura matemática determinada. MATLAB tiene varios tipos de gráficas
(plots): incluye desde la estándar con ejes lineales, logarı́tmicas y semi-logarı́tmicas,
barras, polares, en el espacio tridimensional (3D), contornos de superficies entre
otras. Las gráficas se pueden formatear para tener una apariencia especı́fica, es decir
con un tipo de lı́nea y colores. También es posible utilizar una sola ventana con
varias gráficas, ası́ como tı́tulos y letreros sobre los ejes.
Para inicializar ventanas especı́ficas donde se despliegue resultados gráficos se emplea
la función figure; la cual crea figuras y objetos gráficos, para ser utilizados por
funciones como plot, subplot, plot3, mesh, etc. La información completa y
detallada de las funciones gráfica se puede consultar en helpwin, help plot, help
figure, etc. Sin embargo, a continuación se describe algunas funciones que de manera
particular se emplean en robótica y mecatrónica.
plot
La función plot es utilizada para generar una gráfica en dos dimensiones:
plot(t,x)
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
44
Capı́tulo 1: Conceptos básicos
Los argumentos t,y son vectores (arreglos de una dimensión), ambos vectores deben
tener el mismo número de elementos o dimensión. Cuando el comando plot se
ejecuta genera automáticamente una simple curva de los datos de la variable x
(eje vertical o eje de ordenadas) vs t (eje horizontal o eje de las abscisas). Con la
función plot también se puede graficar diagramas de fase, respuesta transitoria y
estacionaria de robots manipuladores. De esta forma en forma gráfica es más fácil
la interpretación de las variables de estado.
♣ Ejemplo 1.2
Escribir un programa en MATLAB para graficar la curva de la siguiente función:
y = 9x5 + 3 sen(x3 )
para el intervalo x ∈ [−10, 10].
Solución
El código fuente que se presenta en el cuadro 1.2 contiene la forma de obtener
la evolución en el tiempo de una función exponencial requerida para el intervalo
x ∈ [−10, 10]. En la lı́nea 1 se emplean las instrucciones clear all; close all;
clc; requeridas para liberar cualquier variable, figura, o archivo que ocupe espacio
de memoria.
En la lı́nea 3 se define el intervalo de la función con desplazamiento de una milésima.
La función a graficar y = 9x5 +3 sen(x3 ) se encuentra implementada con operaciones
de arreglos.
En esta parte del código se destaca la ventaja que ofrece emplear operaciones con
arreglos, debido que la variable x es definida como un vector de un renglón y
n columnas, entonces la forma más adecuada de realizar las operaciones con los
exponentes de la función y es a través de la manipulación de datos usando arreglos.
Finalmente, en la lı́nea 5 se gráfica la función y = 9x5 + 3 sen(x3 ) por medio de
plot(x,y) tal y como se muestra en la figura 1.5.
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
1.6
45
Gráficas
Código Fuente 1.2
Gráfica de la función 9x5 + 3 sen(x3 )
%MATLAB Aplicado a Robótica y Mecatrónica
%Capı́tulo 1 Introducción
%Editorial Alfaomega Fernando Reyes Cortés
%Archivo cap1 grafica.m
Gráfica de la función 9x5 + 3 sen(x3 )
1
clear all; close all; clc;
2
4
%vector del eje de ordenadas
x=-10:0.001:10;%intervalo de graficado
y=9*x.∧ 5+3*sin(x.∧3); y es el vector de las abscisas
5
plot(x,y)
3
Figura 1.5 Gráfica de 9x5 + 3 sen(x3 ).
Mayor información de la función plot se puede consultar en:
fx >>
help plot ←
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
46
Capı́tulo 1: Conceptos básicos
fplot
El comando fplot grafica una función de la forma y=f(x) entre lı́mites especı́ficos.
La sintaxis del comando fplot es la siguiente:
fplot(’function’,limits,line)
function: es la función a graficar. Puede ser escrita como una cadena de
caracteres entre comillas. La función puede ser propia de MATLAB (builtin) o definida por el usuario (M-file).
La función no puede incluir variables previamente definidas.
limits: puede ser de dos formas: la primera forma incluye un vector que
especifica el incio y término del intervalo a graficar (el dominio de x): [xmin,
xmax]. La segunda forma cosiste de 4 parámetros [xmin,xmax,ymin,ymax],
es decir los 2 primeros parámetros indican el dominio de x, mientras que los 2
últimos parámetros representan los lı́mites del eje de las abscisas (eje vertical
y).
line: indica especificaciones del tipo de lı́nea son las mismas que el comando
plot.
♣ Ejemplo 1.3
Escribir un programa en MATLAB para graficar en lı́nea la función:
y = 9x5 + 3 sen(x3 ) x ∈ [−10, 10].
Solución
El programa 1.3 contiene el código para obtener la gráfica de la función solicitada,
reproduciendo la misma gráfica de la figura 1.5.
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
1.6
47
Gráficas
Código Fuente 1.3
Gráfica de ’9*x∧ 5+3* sen(x∧ 3)’
%MATLAB Aplicado a Robótica y Mecatrónica
%Capı́tulo 1 Introducción
%Editorial Alfaomega Fernando Reyes Cortés
%Archivo cap1 exponencial.m
Gráfica de ’9*x∧5+3* sen(x∧ 3)’
1
clear all; close all; clc;
2
fplot(’9*x∧5+3* sen(x∧ 3)’,[-10,10])
subplot
La función subplot(m,n,p) divide una ventana en una matriz de m × n, p indica
la posición de la gráfica dentro de esa ventana.
plot3
La función plot3(x,y,z) grafica las coordenadas en el espacio tridimensional.
mesh
La función mesh genera superficies en el espacio tridimensional especificadas por las
coordenadas x, y, z. Un aspecto interesante de esta función es que la superficie de la
función la presenta con estilo de malla para resaltar el aspecto tridimensional. Hay
otras funciones para graficar superficies que se auxilian de mesh; como es el caso de
la función ezmesh.
♣ Ejemplo 1.4
Escribir un programa en MATLAB para graficar funciones en 3D
Solución
El programa que se presenta en el cuadro 1.4 muestra la forma de usar las funciones
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
48
Capı́tulo 1: Conceptos básicos
3D. Con el uso de la función figure (lı́nea 3) se abre una ventana para indicar las
gráficas generadas con subplot(2,2,1)...subplot(2,2,4). Observe la diferencia
de presentación entre las funciones surf(x,y,z) y plot3(x,y,z); la primera tiene
un aspecto de sólido, mientras que la segunda usa lı́neas suaves (ver figura 1.6).
Código Fuente 1.4
Funciones 3D.
%MATLAB Aplicado a Robótica y Mecatrónica
%Capı́tulo 1 Introducción
%Editorial Alfaomega Fernando Reyes Cortés
%Archivo cap1 graficas3D.m
Funciones 3D.
1
2
3
4
5
6
7
8
9
10
clc; clear all; close all;
t=0:pi/10:2*pi;
figure
[x,y,z]=cylinder(4*cos(t));
subplot(2,2,1); mesh(x,y,z)
subplot(2,2,2); ezmesh(’x.*exp(-x.∧ 2-y.∧2)’,40);
subplot(2,2,2); ezmesh(fh,40)
[x,y,z] = sphere;
subplot(2,2,3); plot3(x,y,z)
subplot(2,2,4); surf(x,y,z)
Figura 1.6 Gráficas en 3D.
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
1.7
Funciones
49
1.7 Funciones
Una de las principales caracterı́sticas de MATLAB es que provee un número muy
grande de funciones matemáticas. La lista completa de funciones matemáticas se
puede consultar en la ventana de comandos escribiendo:
fx >>
help elfun ←
Una lista de funciones matemáticas avanzadas se puede ver en:
fx >>
help specfun ←
fx >>
help elmat ←
La gran mayorı́a de las funciones matemáticas acepta como argumento números
imaginarios, de esta forma cuando se calcula por ejemplo la raı́z cuadrada o logaritmo
de un número negativo, no produce un error, automáticamente produce un número
complejo o imaginario.
Algunas funciones tales como sqrt, sin, cos son funciones propias (built in) de
MATLAB ya que contienen un código muy eficiente, entonces no se encuentran
disponibles los detalles computacionales y código fuente. Otras funciones como
gamma, sinh son implementadas en archivos (M-files), por lo que el código fuente
está disponible, y además se puede modificar.
Las funciones pueden generar o retornar infinity cuando ocurre una división entre
cero o también por generar un cálculo matemático más allá del lı́mite permitido por
la variable realmax, lo cual se le conoce técnicamente como overflow. Como ejemplos
de este escenario considere los siguientes ejercicios:
fx >>
z=pi*exp(log(realmax)) ←
z =
Inf
fx >>
z=cos(0)/sin(0) ←
z =
Inf
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
50
fx >>
Capı́tulo 1: Conceptos básicos
z=cosh(12e10) ←
z =
Inf
Los nombres de las funciones no son reservados, es decir se puede utilizar para
asignarlo como nombre de una nueva variable, por ejemplo:
sin=1.2345
La función original sin puede ser restaurada con
clear sin
Funciones en lı́nea
Cuando se requiere simples cálculos matemáticos pueden ser usarse las funciones en
lı́nea (inline functions).
La funciones en lı́nea pueden ser generadas con el comando inline(’expresión
matemática en forma de cadena’). Por ejemplo:
fx >>
z=inline(’x/(1+x∧2’)) ←
z =
inline function:
z(x)=x/(1+x∧2)
Entonces la función z(x) se puede evaluar numéricamente
fx >>
z(2) ←
ans
0.4000
Otra opción para evaluar funciones en lı́nea es por medio del comando feval
fx >>
feval(’sqrt’, 100) ←
ans
10
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
1.7
51
Funciones
El comando feval también puede evaluar funciones definidas por el usuario.
Cuando se requiere de complicadas funciones matemáticas, programación extensiva,
subprogramas, etc., lo adecuado son funciones archivo (M-file functions).
1.7.1
1.7.1. Funciones archivo
Las funciones archivo (M-file function) aceptan datos como argumentos de entrada
y retornan resultado a través de la variable de salida. Las funciones archivo tienen
una estructura sintáctica muy bien definida. Emplean la palabra reservada function
para indicar que es una función. En seguida, se especifica el nombre de la variable de
salida (por ejemplo w), seguida del signo =, a continuación del nombre de la función
y entre paréntesis los argumentos de entrada separados por comas. Posteriormente
viene el grupo de instrucciones que contenga la función, la variable de salida tiene
el conjunto de variables a retornar (w=[var1; var2]). Finalmente se inserta el
delimitador end.
La sintaxis de una función se ilustra en el código 1.1.
Estructura de código 1.1
Sintaxis de una función
function w=nombre funcion(x1, x2, x3,...,xn)
var1=grupo de instrucciones1;
var2=grupo de instrucciones2;
w=[var1; var2];
end
Usando el editor de textos integrado en MATLAB se pueden desarrollar funciones
como la que se muestra en el cuadro 1.5.
Una función archivo M-file function tiene que ser salvada antes que pueda ser
usada. Es recomendable salvar al archivo con el mismo nombre de la función y
verificar que tenga extensión m.
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
52
Capı́tulo 1: Conceptos básicos
Código Fuente 1.5
norma euclidiana de un vector
%MATLAB Aplicado a Robótica y Mecatrónica
%Capı́tulo 1 Introducción
%Editorial Alfaomega Fernando Reyes Cortés
%Archivo normaev.m
norma euclidiana de un vector
1
function y =normaev(x)
2
%Norma euclidiana de un vector x ∈ IRn
3
%hay varias formas para calcular la norma euclidiana
% x = x21 + x22 + · · · + x2n
√
% x = xT x
n
2
% x = x21 + x22 + · · · + x2n =
i=1 xi
4
5
6
%en MATLAB se calcula como:
y=norma(x,2);
7
8
9
end
Nótese que se utiliza la palabra reservada function para definir una función,
después se indica la variable donde regresa el valor (en este caso es la variable
y), posteriormente continúa el signo igual (=) y el nombre de la función definida
por el usuario normaev entre paréntesis el argumento de entrada x. Al finalizar el
código se inserta la palabra clave end para delimitar el grupo de instrucciones que
estará entre la palabras reservadas function y end.
MATLAB automáticamente ajustará la naturaleza del argumento de entrada,
ası́ como su salida. En otras palabras, el argumento x puede ser un escalar, vector
o matriz, de acuerdo al tipo de operaciones que se realizan dentro de la función la
naturaleza de la salida y se ajustará de manera adecuada. En el caso del código fuente
del cuadro 1.5 el argumento puede ser un vector o matriz y la salida correspondiente
es escalar. Algunas funciones pueden tener más de un argumento de entrada y
también retornar varias variables de salida, como el caso que a continuación se
presenta en el cuadro 1.6.
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
1.7
53
Funciones
Código Fuente 1.6
cinemática directa robot de 2 gdl
%MATLAB Aplicado a Robótica y Mecatrónica
%Capı́tulo 1 Introducción
%Editorial Alfaomega Fernando Reyes Cortés
%Archivo cinematica directa robot2gdl.m
cinemática directa robot de 2 gdl
1
function [y, x]=cinematica directa robot2gdl(l1,l2,q1,q2)
2
3
% convierte variables articulares a coordenadas cartesianas
x=l1*cos(q1)+l2*cos(q1+q2);
4
y=l1*sin(q1)+l2*sin(q1+q2);
5
end
Funciones anidadas
Una función anidada (nested function) es aquella función que se encuentra como
parte del código de cualquier función archivo (M-file function). En el siguiente
ejemplo la función B está anidada en la función A:
Estructura de código 1.2
Función anidada
function y=A(x1,x2)
y=B(x1)+x2;
function w=B(x3)
w=x3*x3*x3;
end
end
Variables locales y globales
Todas las variables que se definan en una función son locales por naturaleza, esto
significa que sólo en esa función existen y no pueden ser reconocidas por alguna otra
función. Aun cuando dos funciones tengan variables con el mismo nombre, dichas
variables sólo son reconocidas por sus respectivas funciones.
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
54
Capı́tulo 1: Conceptos básicos
Sin embargo, es posible lograr que una o más variables comunes sean reconocidas
por varias funciones (variables globales) listando el nombre de las variables y
separadas por espacios en blanco en el comando global (global command) sin comas
intermedias. Usar la siguiente declaración:
global X Y Z
Las variables han sido declaradas globales en cada función que el usuario quiere
que sean reconocidas. Las variables serán comunes sólo en esas funciones.
El comando global debe aparecer antes que la variable sea usada. Es
recomendable usar la declaración para ese comando al inicio del archivo.
Las variables globales están separadas por espacios en blanco, si usar comas y
se recomienda que al final de la lı́nea no se inserte el operador ;.
El valor de las variables globales puede ser modificado, asignado o reasignado
en cualquiera de las funciones comunes.
Se debe tener mucho cuidado en el empleo de variables globales. Es
recomendable usar letras mayúsculas o algún nombre que el usuario puede
identificar fácilmente para las variables globales con la finalidad de no
confundirlas con variables regulares o locales.
Comparación entre archivos scripts y archivos funciones
Con la finalidad de entender exactamente la diferencia entre archivos script
y funciones archivo (M-file function) se enlistan a continuación las principales
caracterı́sticas entre ellos:
Ambos archivos: script y funciones son salvados con la misma extensión .m por
lo cual se conocen como M-files.
La primera lı́nea de código en el archivo función es la lı́nea de definición de
una función. Ejemplo: function y=reloj(t)
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
1.7
55
Funciones
Las variables dentro de una función son locales.
Las variables en un archivo script son reconocidas en la ventana de comandos.
Los archivos script contienen una secuencia de comandos de MATLAB.
Los archivos funciones pueden aceptar datos a través de la entrada de sus
argumentos y pueden retornar datos por medio de sus variables de salida.
Cuando una función archivo es salvada, entonces el nombre del archivo es el
mismo que el nombre de la función.
Los nombres de los archivos script pueden tener cualquier nombre ya que no
tienen funciones.
Usando una función archivo
Una función definida por el usuario en un archivo función (M-file function) se usa
en la misma forma que una función propia (built-in function) de MATLAB. Esto
significa que la función puede ser llamada desde la ventana de comandos (Command
Window), desde un archivo script o desde cualquier función. Si la función del usuario
se encuentra en algún directorio diferente al de trabajo de MATLAB, entonces debe
ser habilitado dicho directorio con la opción add to path (ver página 9).
♣ ♣ Ejemplo 1.5
Escribir un programa principal para graficar la curva de un cı́rculo de radio r=0.25
m, con centro en x0 = 0.3m, y0 = 0.3m
x = x0 + r sen(t)
y = y0 + r cos(t)
z=t
donde t la variable tiempo.
Asimismo implementar la ecuación del cı́rculo en una función.m; utilizar variables
globales para almacenar la coordenada z.
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
56
Capı́tulo 1: Conceptos básicos
Solución
La función cı́rculo se encuentra descrita en el cuadro 1.7, los parámetros de entrada
de esta función son las coordenadas del centro del cı́rculo x0 , y0 , el radio r y el vector
tiempo. Las variables de salida son las coordenadas cartesianas x, y del cı́rculo.
En la lı́nea 2 se encuentra definida la variable global altura, para almacenar las
coordenadas sobre el eje z. Esta misma variable global altura se encuentra en el
programa principal 1.8 (ver la lı́nea 3). La función cı́rculo y el programa principal
pueden reconocer a la variable altura, lo que representa una ventaja debido a que
cualquier de los dos programas puede manipular datos de esa variable.
Código Fuente 1.7
Función cı́rculo
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 3 Cinemática función circulo.m
Función cı́rculo
1
2
function [x, y]=circulo(x0,y0,r,t)
global altura % variable global que se emplea en el programa principal 1.8
3
4
%ecuación del cı́rculo
x=x0+r*sin(t);
5
y=y0+r*cos(t);
6
%coordenada sobre el eje z
altura=t;
7
8
end
Observe que el programa principal 1.8 no es una función, puesto que no emplea la
palabra reservada function, entonces se puede salvar el código de este programa
con cualquier nombre, añadiendo la extensión m; es decir: (nombre archivo.m).
Para ejecutar el programa principal se puede hacer con la tecla F5 o realizando click
sobre el icono play localizado en el menú superior del editor de texto. Las gráficas
de x vs t, y vs t y (x ,y , z) se obtienen con las funciones subplot, plot y plot3
(lı́neas 9 a la 13) cuyo resultado se encuentra en la figura 1.7.
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
1.7
57
Funciones
Código Fuente 1.8
Programa principal del cı́rculo
%MATLAB Aplicado a Robótica y Mecatrónica
%Capı́tulo 1 Introducción
%Editorial Alfaomega Fernando Reyes Cortés
%archivo circulo simu.m
Programa principal del cı́rculo
1
2
3
clc; clear all; close all; %limpia área de memoria
format short
6
global altura % declaración de la variable global
ti=0; %tiempo inicial
h=0.001; %incremento de una milésima de segundo
tf = 10; %tiempo final
7
t=ti:h:tf; %vector tiempo
8
%se invoca a la función cı́rculo
[x, y]=circulo(0.3,0.3,0.25,t);
subplot(2,2,1); plot(t,x) % gráfica de x vs t
4
5
9
10
11
12
subplot(2,2,2); plot(t,y) %gráfica de y vs t
subplot(2,2,3); plot(x,y) % cı́rculo
13
subplot(2,2,4); plot3(x,y,altura)%figura tridimensional
Figura 1.7 Cı́rculo.
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
58
Capı́tulo 1: Conceptos básicos
1.8 Programación
U
n programa es una secuencia de instrucciones, expresiones, funciones,
comandos y declaraciones para realizar aplicaciones en ingenierı́a mecatrónica
y robótica. En MATLAB las instrucciones son ejecutadas una tras otra, en
forma secuencial, es decir en el mismo orden como van apareciendo. La secuencia
del programa depende de las instrucciones que controlan el flujo del programa,
dependiendo de ciertas condiciones se ejecuta un determinado conjunto de
instrucciones o bloque del programa.
Dentro de las condiciones que determinan el flujo del programa se encuentran
operadores lógicos como los que se presentan en la tabla 1.7. A través de ellos se
evalúa expresiones para realizar un cierto bloque de código o funciones especı́ficas
de la aplicación implementada.
Tabla 1.7 Operadores lógicos
Igual a
==
No es igual a
˜=
Menor que
<
Mayor que
>
Menor que o igual que
<=
Mayor que o igual que
>=
El lenguaje de programación MATLAB es muy rico en instrucciones y funciones
toolbox, ya que cuenta con un amplio compendio de librerı́as o funciones para diversas
áreas de ingenierı́a y ciencias exactas.
A continuación se describen las instrucciones condicionales y lazos de programas
como if, for, while, switch que permiten programar una gran cantidad de
aplicaciones en robótica y mecatrónica.
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
1.8
59
Programación
Instrucciones condicionales
Una instrucción condicional permite a MATLAB realizar decisiones para ejecutar
un grupo de funciones, comandos, etc. Si la condición en la instrucción condicional
es verdadera, entonces realiza un grupo de expresiones del programa. Si la condición
es falsa, entonces no ejecuta o salta ese grupo de expresiones.
1.8.1
1.8.1. if
La instrucción if evalúa si una expresión lógica es verdadera, entonces realiza un
conjunto de instrucciones o declaraciones del programa, de otra forma realiza la
ejecución de otro bloque del programa. La estructura de esta instrucción condicional
consta de la palabra clave if, un conjunto de instrucciones o bloque de programa
delimitadas por la palabra clave end.
Si la condición lógica que se evalúa por la instrucción if es falsa, el programa salta
ese bloque de instrucciones y continúa con las instrucciones que se encuentran justo
después de end.
La estructura de la instrucción condicional if se muestra en el código 1.3.
Estructura de código 1.3
Sintaxis de la instrucción if — end
if condicion verdadera if
instrucciones 1;
................
instrucciones n;
end
otro grupo de instrucciones;
La figura 1.8 muestra el diagrama de flujo de la instrucción if.
Es importante resaltar que en el entorno de programación de MATLAB las palabras
claves if end aparecen en color azul. Además, las expresiones que aparecen como
el bloque de instrucciones de la instrucción if aparecen como texto con sangrı́a
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
60
Capı́tulo 1: Conceptos básicos
Figura 1.8 Diagrama de flujo de la instrucción if.
(indented), lo que resulta una fácil lectura del código de programación.
1.8.2
1.8.2. if, else, elseif
La instrucción if puede tener más opciones dentro de su estructura if...end. Es
decir, se puede utilizar if con la palabra clave else para evaluar otro grupo de
instrucciones de la manera que se muestra en la estructura de código 1.4:
Estructura de código 1.4
Sintaxis de if...else...end
if condición verdadera
grupo de instrucciones;
else
grupo de instrucciones;
end
Además de la opicón else en la instrucción if...end, también se puede combinar
con la palabra clave elseif; en este caso incluye una declaración condicional
adicional para la ejecución de más grupos de instrucciones. Si en la instrucción if la
condición es verdadera, entonces el programa ejecuta el grupo (1) de instrucciones
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
1.8
Programación
61
y pregunta por una nueva expresión condicional elseif, si es verdadera entonces
ejecuta el grupo (2) de instrucciones y si es falsa se ejecuta el grupo (3) de
instrucciones.
La sintaxis de if...elseif...else...end se presenta en el código 1.5:
Estructura de código 1.5
Sintaxis de if...elseif...else...end
if condición verdadera if
grupo de instrucciones 1; %grupo(1)
elseif condición verdadera elseif
grupos de instrucciones 2; %grupo(2)
else
grupo de instrucciones 3; %grupo(3)
end
Debe tomarse en cuenta que tanto else, como elseif quedan dentro de la
instrucción if determinado por el delimitador end.
La figura 1.9 muestra el diagrama de flujo de la instrucción if...elseif...else...end.
1.8.3
1.8.3. for
La instrucción for genera lazos repetitivos (flujos o ciclos de instrucciones) para
repetirse un número determinado de veces. La instrucción for requiere de una
variable contador para realizar n veces la iteración, la condición de salida se alcanza
cuando la variable contador ha llegado a la cuenta especificada.
La sintaxis de la instrucción for inicia con esa palabra clave, a continuación la
inicialización de una variable contador que determinará el número de veces que
realizará el ciclo o flujo del grupo de instrucciones. Finalmente se incluye la palabra
clave end para delimitar la instrucción for.
El código 1.6 presenta la estructura de la instrucción for...end:
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
62
Capı́tulo 1: Conceptos básicos
Figura 1.9 Diagrama de flujo de la instrucción if...elsefi...end.
Estructura de código 1.6
Sintaxis de la instrucción for
for contador=1:n
instrucción 1;
..............;
instrucción n;
end
Como un ejemplo sencillo para mostrar la utilidad de la instrucción for...end se
presenta en el código 1.7 un programa que despliega 10 veces el contador i y la
variable j que contiene el valor de una función trigonométrica.
Estructura de código 1.7
Ejemplo de iteraciones con la instrucción for
j=0;
for i=1:10
disp(i,j)
j=sin(2*pi*i);
end
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
1.8
63
Programación
En este caso for ejecuta 10 veces el incremento sobre la variable i. Inicia i con
el valor de 1 hasta llegar al valor de i=10, entonces la variable i realiza el papel
de contador, con incrementos unitarios. La función disp(i,j) despliega los valores
numéricos de las variables i,j.
El siguiente ejemplo muestra el empleo de for usando incrementos en milésimas
para la variable t por medio del operador colon :. Esta aplicación de la instrucción
for es muy útil para graficar funciones como es el caso de la función seno dentro de
un intervalo de tiempo.
♣ Ejemplo 1.6
Escribir un programa en MATLAB para graficar una señal senoidal.
Solución
El programa 1.9 presenta el código fuente en MATLAB para graficar una señal
senoidal. En la lı́nea 3 se inicializa el vector tiempo de 0 a 10 segundos con
incrementos de una milésima de segundo. El ciclo iterativo de la instrucción for
abarca las lı́neas 3 a la 7. El lector puede realizar el paso del tiempo de simulación
más fino, por ejemplo en diezmilésimas. En la figura 1.10 se muestra la gráfica que
genera el programa 1.9.
Figura 1.10 Función senoidal.
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
64
Capı́tulo 1: Conceptos básicos
Código Fuente 1.9
Gráfica de una onda senoidal
%MATLAB Aplicado a Robótica y Mecatrónica
%Capı́tulo 1 Introducción
%Editorial Alfaomega Fernando Reyes Cortés
%Archivo senoide.m
Gráfica de una onda senoidal
1
2
3
4
disp(’Programa para graficar una onda senoidal’)
i=1; %inicializa pivote para registro
for t=0:0.001:10 % incrementos de t en milésimas
y(i)=sin(t); % arreglo de datos para la onda senoidal
5
tiempo(i)=t; % arreglo de datos para el tiempo
i=i+1; % incrementa pivote
6
7
end
8
disp(’valor de y’)
y
9
10
plot(tiempo,y)
El empleo de instrucciones for con la estructura if...elseif...else...end
diversifica las aplicaciones de programación. Como una aplicación de este conjunto
de instrucciones se presenta la implementación de la función signo la cual es
ampliamente utilizada en dinámica y esquemas de control de sistemas mecatrónicos
y robots manipuladores; especı́ficamente se emplea en el modelado del fenómeno de
fricción de Coulomb. También se usa en algunos algoritmos de control.
Una de las principales desventajas de la función signo es su discontinuidad que
presenta y debido a esta caracterı́stica muy particularidad produce retardo en el
proceso de integración numérica de sistemas dinámicos, por lo que no se recomienda
el emplear de esta función cuando se realice simulación de la dinámica de robots
manipuladores.
Una posible forma de implementar la función signo es por medio del uso de for y
if elseif else end, el cuadro 1.10 presenta la función signo ( ).
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
1.8
65
Programación
Código Fuente 1.10
Función signo
%MATLAB Aplicado a Robótica y Mecatrónica
%Capı́tulo 1 Introducción
%Editorial Alfaomega Fernando Reyes Cortés
%Archivo signo.m
Función signo
1
2
function y =signo(x)
[n,m]=size(x);
3
for i=1:n
if x(i) >0
y(i) = 1; %si es positivo retorna 1
elseif x(i) == 0
4
5
6
7
y(i) = 0; %si es cero retorna 0
else
y(i) = -1; % si es negativo retorna -1
8
9
10
end
11
12
end
end
♣ Ejemplo 1.7
Escribir un programa en MATLAB para convertir una onda senoidal en onda
cuadrada (emplear la función signo() para realizar dicha conversión).
Solución
Una forma de convertir una onda senoidal del tipo sen(t) a señal cuadrada (tren
de pulsos cuadrados) es por medio del uso de la función signo(x). Considere el
programa 1.11; el tiempo de simulación es de 0 a 10 segundos, en la lı́nea 5 se
obtiene la onda senoidal y la conversión a onda cuadrada se realiza en la lı́nea 6. La
figura 1.11 contiene la conversión de la onda senoidal a onda cuadrada.
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
66
Capı́tulo 1: Conceptos básicos
Código Fuente 1.11
Aplicación de la función signo
%MATLAB Aplicado a Robótica y Mecatrónica
%Capı́tulo 1 Introducción
%Editorial Alfaomega Fernando Reyes Cortés
%Archivo signo simu.m
Aplicación de la función signo
1
2
3
clc; clear all;
close all;
6
format short
ti=0;h=0.001; tf = 10; %parámetros del tiempo de simulación
t=(ti:h:tf)’; %vector columna de tiempo
y=sin(t); % onda senoidal y1=signo(y); % función signo
7
plot(t,y1)
4
5
Figura 1.11 Función signo().
Debido a que la función signo está implementada bajo la estructura function es
necesario realizar un programa principal donde se pueda invocar con el adecuado
pase de parámetros y su correcta sintaxis. Esta es la tarea que realiza el programa
principal 1.11
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
1.8
Programación
67
También es posible programar for anidados, es decir un for insertado dentro de otro
for. Este tipo de aplicaciones resulta de utilidad en esquemas de control, cálculos
iterativos o recursivos, simulación de sistemas dinámicos, procesamiento de señales,
etc.
El código 1.8 presenta la sintaxis gramatical para el caso de tener instrucciones for
anidadas. Obsérvese que el for externo realizará un número de veces determinado
por la variable contador 1 al grupo de instrucciones 1 y al segundo for. Por otro
lado, el grupo de instrucciones 2 será realizado un número de veces indicado por el
contador 2. El número total de veces que se ejecutará el grupo (2) de instrucciones
es contador 1 * contador 2.
Para un correcto funcionamiento de las instrucciones for anidadas es necesario
ubicar adecuadamente los delimitadores end. Por ejemplo para la instrucción for
externa abarca el grupo (1) de instrucciones, ası́ como el for interno con su segundo
grupo de instrucciones.
Por lo tanto, están delimitados por el for externo y la palabra clave end. Mientras
que para el segundo for el segundo grupo de instrucciones está delimitado por la
palabra clave end correspondiente a la instrucción for del contador 2.
Estructura de código 1.8
Sintaxis para el caso de for anidados
for contador 1
grupo de instrucciones 1;
for contador 2
......................;
grupo de instrucciones 2;
......................;
end
end
Un ejemplo tı́pico del empleo de for anidados es la aplicación de suma de matrices
realizada en forma recursiva. Este algoritmo emplea dos instrucciones for, el primero
sirve para recorrer los renglones usando la variable i, mientras que el segundo for
accede a las columnas de las matrices a través del pivote j.
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
68
Capı́tulo 1: Conceptos básicos
♣ Ejemplo 1.8
Escribir un programa en MATLAB para sumar dos matrices A, B ∈ IRn×p en
forma recursiva.
Solución
El código fuente para sumar matrices en forma recursiva se presenta en el programa
1.12:
Código Fuente 1.12
Algoritmo para sumar matrices
%MATLAB Aplicado a Robótica y Mecatrónica
%Capı́tulo 1 Introducción
%Editorial Alfaomega Fernando Reyes Cortés
%Archivo sumamatrices.m
Algoritmo para sumar matrices
1
disp(’Realiza la suma de dos matrices de orden n’)
2
%Inicialización de las matrices A, B dimensión 3 x 3
A=[ 1 1 3; 2 4 5; 8 9 0];
B=[3 2 5; 6 7 8; 9 0 0];
3
4
5
6
7
8
9
%Algoritmo de suma de matrices.
[ n p ] =size(A); % Obtiene orden de la matriz
%Algoritmo para sumar matrices
for i=1:n
for j=1:p
10
%Suma algorı́tmica
C(i,j)=A(i,j)+B(i,j);
11
12
13
14
15
end
end
disp(’Resultado:’)
C
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
1.8
69
Programación
♣ Ejemplo 1.9
Escribir un programa en MATLAB para encontrar el mı́nimo y máximo de la
siguiente función:
y =
sen(t) − cos(πt) − 2 tanh(t) + 0.1 log(t3 + 1)
determinar los tiempos de ocurrencia.
Solución
Para buscar el máximo y mı́nimo de la función indicada (ver figura 1.12) y tiempos
de ocurrencia puede usarse el código del programa 1.13.
Figura 1.12 Búsqueda de mı́nimo y máximo.
La salida del programa tiene el siguiente resultado:
Max Tmax Min Tmin
0.3887 0.9800 −3.3539 4.0600
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
70
Capı́tulo 1: Conceptos básicos
Código Fuente 1.13
Mı́nimos y máximos
%MATLAB Aplicado a Robótica y Mecatrónica
%Capı́tulo 1 Introducción
%Editorial Alfaomega Fernando Reyes Cortés
archivo cap1 minimax.m
Mı́nimos y máximos
1
2
clc; clear all; close all;
disp(’Encuentra mı́nimo y máximo de una función’)
3
%variables para registrar máximo y mı́nimo
4
max=0; min=0; t min=0; t max=0;
5
7
%generación del vector de tiempo
t=[0:0.01:10]’; %tiempo de 0 a 10 segundos con incremento de 0.01 seg.
[n m]=size(t); %dimensión del arreglo
8
y=zeros(n,m); %variables de registro
9
11
%función de prueba para generar la gráfica del proceso
for i=1:n
y(i)=sin(t(i))-cos(3.1416*t(i))-2*tanh(y(i))+0.1*log(t(i)∧3+1);
12
end
13
%gráfica de la función del proceso
plot(t,y)
for i=1:n
6
10
14
15
16
17
18
19
if (y(i)>= max)
max=y(i);
t max=t(i);
elseif (y(i)<= min)
20
min=y(i);
t min=t(i);
21
22
23
24
end
25
end
disp( ’Max Tmax Min Tmin’)
26
Resultado=[max t max min t min ];
27
disp(Resultado)
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
1.8
71
Programación
1.8.4
1.8.4. while
La instrucción while genera un lazo o ciclo de instrucciones que se repiten un número
finito o infinito de veces dependiendo de la condición lógica especificada. A diferencia
de la instrucción for, while puede ejecutar un grupo de instrucciones de manera
indefinida. La instrucción while también tiene su delimitador end. La forma general
del lazo de programa while es:
Estructura de código 1.9
Sintaxis para el caso de for anidados
while condición boleana verdadera
%grupo de instrucciones o sentencias
instrucción 1;
.............;
instrucción n;
end
grupo de instrucciones;
Un ejemplo de cómo utilizar la instrucción while se presenta a continuación:
Estructura de código 1.10
Uso de la instrucción while
a=10;
b=15;
i=0;
while a>i
i=i+1;
b=b-i;
if b>a
disp(’Robot activo’)
else disp(’Proceso desactivo’)
end
end
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
72
Capı́tulo 1: Conceptos básicos
1.8.5
1.8.5. switch, case
La instrucción switch ejecuta un grupo de sentencias con base al valor de la variable
o expresión que emplea al lado derecho de su sintaxis. Para dividir los grupos de
sentencias se emplean las palabras claves case y othewise. La instrucción switch
finaliza con end.
En el siguiente código se ilustra la forma en que funciona la instrucción switch:
Estructura de código 1.11
Sintaxis de la instrucción switch
for i=1:3
switch i
case 1
disp(’caso 1’)
grupo de instrucciones;
case 2
disp(’caso 2’)
grupo de instrucciones;
case 3
disp(’caso 3’)
grupo de instrucciones;
otherwise
disp(’fuera de rango’)
grupo de instrucciones;
end
end
continúa con otro grupo de instrucciones;
MATLAB distingue los diferentes grupos de instrucciones asociados a un respectivo
case. Por ejemplo, si entra a ejecutar el grupo de instrucciones del caso 1, se sale
de la instrucción switch cuando encuentra el caso 2 y continúa con otro grupo de
instrucciones ya fuera de switch. Para el caso de otherwise el grupo de instrucciones
queda delimitado entre otherwise y el delimitador end perteneciente a switch.
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
1.8
Programación
73
1.8.6
1.8.6. break
La sentencia break permite abandonar o salirse del lazo generado por las
instrucciones for o while. El uso de break se puede ilustrar por medio del siguiente
ejemplo. Se genera un lazo usando while, como la condición de salida es una
constante positiva, el lazo se generará de manera infinita. Sin embargo, la condición
de salida queda determinada por if t==0.0 y break. En este ejemplo se realiza la
(t)
operación sen
, la variable t se inicializa en -1 y se decrementa en 0.001 en cada
t
iteración. Cuando t=0 se evita realizar una división entre cero usando break.
Estructura de código 1.12
Forma de usarse la instrucción break
t=-10;
while 1
t=t+1;
% código correspondiente para detectar cruce por cero
if t==0
disp(’Advertencia división entre cero’)
break;
end
x= sin(t)/t;
end
disp(x)
También es posible usar break en lazos anidados.
1.8.7
1.8.7. return
La expresión return termina la secuencia actual de comandos y retorna el control
a la función que invocó. Una llamada a función normalmente transfiere el control a
la función que la invocó cuando alcanza el fin de la función. Es posible insertar una
sentencia return para forzar la terminación de la función y transferir el control a la
función que realizo la llamada.
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
74
Capı́tulo 1: Conceptos básicos
Estructura de código 1.13
Forma de usarse la instrucción return
function y =cap1 raiz(x)
if x<0
disp(’error: número imaginario’);
y=i;
return;
else y=sqrt(x);
end
end
1.8.8
1.8.8. continue
La sentencia continue temporalmente interrumpe la ejecución del lazo del programa
(for o while). A diferencia de break y return no causa una inmediata salida del
lazo de control, permanece ahı́ suspendiendo la ejecución del programa hasta que la
condición de salida del lazo del programa sea falsa.
Por ejemplo, en el siguiente código 1.14 la variable i se inicializa con cero y entra al
lazo while permaneciendo ahı́ mientras la condición i<10 sea verdadera. Dentro de
este lazo se pregunta si la variable i ya tiene el valor de 8 ( por medio de if i==8),
si esta condición es falsa, entonces continúa incrementando la variable i con i=i+1.
Cuando la variable i es igual a 8, entonces se despliega en la ventana de comandos
la leyenda Robot activo; se asigna el valor i=20; para que la condición de salida
de while sea falsa, el control del programa sale del lazo de while por medio de la
sentencia continue para ejecutar b=100 fuera de dicho lazo.
Observe que efectivamente la sentencia continue hace que el programa abandone
el lazo de while, ya que ya no pasará por la instrucción if (i>10) i=0; end, la
cual fue insertada intencionalmente para inicializar a la variable i en cero y provocar
que la condición sea verdadera dentro del lazo while. Es necesario recalcar que la
sentencia continue suspenderá el programa hasta que la condición de salida del lazo
de control sea falsa, a diferencia con la forma de trabajar en break o return.
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
1.8
Programación
75
Estructura de código 1.14
Forma de usarse la instrucción continue en el lazo while
i=0;
while i<10
if i==8
disp(’Robot activo’)
i=20; %condición de salida del lazo while
continue; %continúa en b=100
end
if i>9
i=0; %asegura condición verdadera para permanecer
dentro del lazo while
end
i=i+1;
end
b=100
otro grupo de instrucciones;
El siguiente ejemplo 1.15 muestra el uso de la sentencia continue dentro del lazo
generado por for:
Estructura de código 1.15
Forma de usarse la instrucción continue en el lazo for
for t=-10:0.1:10
y=sin(t);
if y==0.0
disp(’detecta cruce por cero’)
t=100; %condición de salida de lazo for
% evita división entre cero
continue; %salta en b=300
end
x=(sin(t)∧ 2+cos(t)∧2)/sin(t);
end
b=300
otro grupo de instrucciones;
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
76
Capı́tulo 1: Conceptos básicos
1.9 Formato para datos experimentales
E
n las áreas de robótica y mecatrónica es común trabajar archivos que tengan
datos experimentales o de simulación con información de variables como error
de posición, velocidad, aceleración y par aplicado. No hay un formato especı́fico
para generar los archivos. No obstante, es recomendable que tengan forma tabular,
tipo matriz teniendo la primera columna destinada al tiempo, la segunda columna y
posteriores para la información de las variables. Puede haber tantas columnas como
variables se requiera registrar. Este formato para archivos de datos experimentales
permite portabilidad para todos de paquetes de cómputo de cualquier plataforma.
Los datos experimentales de robots manipuladores son importantes para elaborar
reportes técnicos, artı́culos cientı́ficos, tesis, memorias para congresos, etc.
¿Cómo generar un formato de archivo con datos experimentales?
MATLAB tiene las funciones fopen, fprintf, fclose, load y save para realizar
dicho formato. El procedimiento para generar archivos de datos experimentales
consiste en generar un archivo de tipo ASCII o texto, con la función fopen o con la
función save por medio del siguiente procedimiento:
Indicar la trayectoria donde se generará el archivo de texto, por ejemplo con
nombre robot.dat
fid = fopen(’c:\robot\experimentos\robot.dat’, ’wt’);
donde el atributo ’wt’ significa que se crea un nuevo archivo de texto para
escritura y lectura. La variable fid es un identificador o apuntador asociado
al archivo robot.dat
Agrupar en una matriz todas la variables que se desean grabar, por ejemplo:
datos=[t, q, qp, qpp]; donde t es el tiempo, posición q, velocidad qp
y aceleración qpp. Posteriormente usar la función fprintf para grabar las
variables:
datos=[t, q, qp, qpp];
fprintf(fid,’\%3.3f \%3.3f
Alfaomega
\%3.3f \%3.3f \n’,datos);
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
1.9
Formato para datos experimentales
77
El formato ’ %3.3f %3.3f %3.3f %3.3f \n’ significa que se grabarán cuatro
columnas de datos en formato de punto flotante, manteniendo una precisión
numérica de tres dı́gitos con tres fracciones. Observe que para separar las
columnas de datos hay al menos un espacio en blanco. El comando \n se
usa para generar renglones, por cada dato t(i), q(i), qp(i), qpp(i), para
i = 1, 2, · · ·, n.
Cerrar el archivo robot.dat de la siguiente forma:
fclose(fid);
Otra opción para generar el archivo de datos experimentales es usando la
función save; por ejemplo:
save -ascii ’c:\robot\experimentos\robot.dat’ datos
Es recomendable que después de salvar el archivo, se libere la memoria ocupada
por la variable datos, por medio de clear datos; de esta forma no se tendrán
problemas de espacio.
Estructura de código 1.16
Generar formato de archivo con datos experimentales
clc; clear all; close all;
t=(0:0.001:10)’; %vector columna de la variable tiempo
q=sin(t); % posición
qp=cos(t); % velocidad
qpp=-sin(t); %aceleración
datos=[t, q, qp, qpp];
fid = fopen(’c:\robot\experimentos\robot.dat’, ’wt’);
fprintf(fid, ’ %3.3f %3.3f %3.3f %3.3f \n’,datos);
fclose(fid);
.
.
.
clear datos;
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
78
Capı́tulo 1: Conceptos básicos
Estructura de código 1.17
Opción para generar formato del archivo experimental
clc; clear all; close all;
t=(0:0.001:10)’; %vector columna de la variable tiempo
q=sin(t); % posición
qp=cos(t); % velocidad
qpp=-sin(t); %aceleración
datos=[t, q, qp, qpp];
%una forma alterna para grabar los datos
save -ascii ’c:\robot\experimentos\robot.dat’ datos
.
.
.
clear datos;
El programa que se muestra en la estructura de código 1.16 genera el formato
del archivo con datos experimentales. Mientras que en la estructura de código
1.17 se presenta otra opción de generar el mismo formato. Para el caso especı́fico
del archivo robot.dat el formato se indica en la tabla 1.8. Note que la primera
columna corresponde a la evolución del tiempo, la posición, velocidad y aceleración
corresponden a la segunda, tercera y cuarta columna, respectivamente.
¿Cómo leer el archivo de datos experimentales?
Una vez generado un archivo con datos experimentales, se emplea la función load
para abrir y manipular su información.
A continuación se describe el seguimiento procedimiento:
Usar la función load para cargar en memoria el archivo con datos
experimentales:
datos_experimentales=load(’c:\robot\experimentos\robot.dat});
t=datos_experimentales(:,1);
la primera columna tiene el tiempo y las demás columnas contienen
información de las variables de interés al usuario.
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
1.9
79
Formato para datos experimentales
Tabla 1.8 Formato de datos experimentales para robótica y mecatrónica
t
q
q̇
q̈
0
0
1.0000
0
0.0010 0.0010 1.0000 −0.0010
0.0020 0.0020 1.0000 −0.0020
0.0030 0.0030 1.0000 −0.0030
0.0040 0.0040 1.0000 −0.0040
..
..
..
..
.
.
.
.
0.0050 0.0050 1.0000 −0.0050
0.0060 0.0060 1.0000 −0.0060
0.0070 0.0070 1.0000 −0.0070
0.0080 0.0080 1.0000 −0.0080
0.0090 0.0090 1.0000 −0.0090
0.0100 0.0100 1.0000 −0.0100
0.0110 0.0110 0.9999 −0.0110
..
..
..
..
.
.
.
.
El programa que se indica en la estructura de código 1.18 permite abrir y manipular
la información del archivo de prueba robot.dat.
Estructura de código 1.18
Abrir formato de archivo experimental
clc;
clear all;
close all;
datos experimentales=load(’c:\robot\experimentos\robot.dat’);
t=datos experimentales(:,1); %tiempo
qe=datos experimentales(:,2); %posición
qpe=datos experimentales(:,3); % velocidad
qppe=datos experimentales(:,4); %aceleración
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
80
Capı́tulo 1: Conceptos básicos
1.10 Resumen
H
oy en dı́a, MATLAB es un ambiente de programación amigable e interactivo
que se ha ubicado como un referente a nivel internacional en todas las
universidades, institutos de investigación e industria.
El lenguaje MATLAB es una potente herramienta de diseño en simuladores de
sistemas mecatrónicos y robots manipuladores. Contiene un amplio conjunto de
instrucciones y librerı́as toolbox para estudiar en detalle todos los aspectos de
modelado dinámico, sistemas de control, identificación paramétrica, procesamiento
de imágenes, etcétera.
Particularmente, en los cursos de control automático se emplea para caracterizar,
evaluar y desarrollar esquemas de control. MATLAB tiene una gran cantidad de
aplicaciones para las áreas de robótica y mecatrónica. Facilita el desarrollo de
la investigación y fortalecer al mismo tiempo aspectos pedagógicos en docencia,
ası́ como la transmisión del conocimiento. Por eso, resulta clave dominar y programar
con solvencia sobre todo en código fuente para realizar cualquier tipo de aplicación.
En este capı́tulo se ha presentado las principales funciones e instrucciones del
lenguaje MATLAB. Desde los aspectos básicos del entorno de programación hasta
aspectos más detallados donde se involucra declaración de variables, manipulación
de matrices y arreglos, manejo de operadores, gráficas y funciones, ası́ como ejemplos
en código fuente que ilustran su inmediata aplicación a control y dinámica de robots
manipuladores y sistemas mecatrónicos.
Se propone un formato libre para archivos con datos de resultados experimentales
o de simulación; es muy importante tener un método computacional para registrar
variables de estado e información relevante del robot. Para este propósito se explica
el procedimiento para generar este tipo de archivos compuesto por columnas y
renglones tipo matriz, también se describe el procedimiento para cargarlo a memoria
y extraer la información registrada, para su análisis e interpretación.
Finalmente, se recomienda consultar periódicamente: www.mathworks.com/.
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
Capı́tulo
2
Métodos numéricos
t
I =
0
f˙ =
f (x)dx ⇒ Ik = Ik−1 + hf (xk−1 )
lı́m
f (t +
t→0
fk − fk−1
t) − f (t)
⇒ f˙k =
t
h
2.1 Consideraciones computacionales
2.2 Sistemas de ecuaciones lineales
2.3 Diferenciación numérica
2.4 Integración numérica
2.5 Sistemas dinámicos de primer orden
2.6 Resumen
Objetivos
Presentar los métodos numéricos más utilizados en el área de ingenierı́a
a través de ejemplos didácticos y aplicaciones.
Objetivos particulares:
Sistemas de ecuaciones lineales.
Métodos de diferenciación numérica (Euler, diff).
Métodos de integración numérica: Trapezoidal, Simpson, Euler.
Sistemas dinámicos de primer orden (Runge-Kutta).
Funciones ode (ecuaciones diferenciales ordinarias).
Simulación de sistemas dinámicos de primer orden.
2.1
Consideraciones computacionales
83
2.1 Consideraciones computacionales
R
esolver problemas de ingenierı́a requiere del conocimiento solvente de
métodos numéricos para su adecuada implementación práctica en simulación,
análisis y estudio de los resultados experimentales.
Un simulador se caracteriza por reproducir fielmente todos los fenómenos fı́sicos
del sistema mecatrónico o robot manipulador. Desarrollar el proceso de simulación
es una etapa no trivial, involucra aspectos desde seleccionar el método numérico
más adecuado tanto en exactitud de resultados como en desempeño computacional.
También depende de las finalidades del problema a resolver, puede ser control en
tiempo real o un escenario donde analizará resultados experimentales fuera de lı́nea
(off-line). Este es el caso de algunos tópicos de control y de automatización donde
interviene identificación paramétrica, caracterización de sensores y servomotores,
estimación de señales usando la posición articular por ejemplo, obtener velocidad y
aceleración de movimiento.
Cuando se llega a la etapa de resolver numéricamente un sistema dinámico (lineal
y no lineal), es importante seleccionar el método de integración más adecuado de
tal forma que no consuma muchos recursos computacionales (tiempo, eficiencia y
desempeño) y que tenga la exactitud adecuada en la solución numérica para que ésta
sea confiable para su correcta interpretación de resultados. Con estas caracterı́sticas
del método de integración, el esfuerzo de diseño se concentra en mejorar los atributos
y propiedades del esquema de control y posteriormente evaluar el desempeño en un
robot real. De esta forma, las potenciales aplicaciones del esquema de control son
mucho más amplias, ası́ como su correcta ejecución.
En el presente capı́tulo se presentan los principales métodos numéricos para abordar
problemas de control en sistemas mecatrónicos y robots manipuladores. Se hace
un especial énfasis en determinar cuáles son más adecuadas para su utilización en
control de procesos en tiempo real y cuáles son más apropiadas para aspectos de
simulación donde el grado de exactitud es el apropiado para analizar y estudiar
sistemas mecatrónicos, ası́ como la dinámica compleja no lineal y multivariable de
robots manipuladores.
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
84
Capı́tulo 2: Métodos numéricos
2.2 Sistemas de ecuaciones lineales
E
n el área de ingenierı́a es muy común encontrar sistemas de ecuaciones lineales,
en donde se involucran variables desconocidas que fundamentalmente definen
el problema a resolver; también participan matrices, ası́ como su matriz inversa y su
determinante.
Considérese un conjunto de n ecuaciones lineales con m incógnitas dadas de la
siguiente forma:
y1 = a11 x1 + a12 x2 + · · · + a1n xn
y2 = a21 x1 + a22 x2 + · · · + a2n xn
..
..
. =
.
(2.1)
ym = am1 x1 + am2 x2 + · · · + amn xn
donde aij son coeficientes conocidos, con i = 1, 2 · · ·, m, j = 1, 2, · · ·, n; xj son las
incógnitas, yi son variables conocidas. En forma compacta el sistema de ecuaciones
lineales 2.1 puede ser representado por:
y = Ax
(2.2)
donde A ∈ IRm×n , x ∈ IRn , y ∈ IRm
entonces la solución consiste en encontrar x = [x1 , x2 , · · · , xn]T en función de la
matriz A y de B, quedando de la siguiente forma:
x = A−1 y.
(2.3)
Obsérvese que la solución involucra la inversa de la matriz A. Por lo tanto, es
necesario primero abordar los aspectos computacionales a la inversión de matrices.
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
2.2
85
Sistemas de ecuaciones lineales
Determinantes de matrices
El determinante de una matriz cuadrada A ∈ IRn×n es un número o escalar y
tiene varias aplicaciones en ingenierı́a: se utiliza en matrices inversas, solución de
ecuaciones simultaneas; en robótica indica los valores numéricos de las variables
articulares del robot para producir singularidades en su jacobiano, etc.
La sintaxis en MATLAB de la función determinante det de una matriz A ∈ IRn×n
es la siguiente:
det(A)
donde A es una matriz cuadrada de dimensión n previamente definidas sus entradas.
♣ Ejemplo 2.1
Considere el siguiente sistema de ecuaciones con tres incógnitas:
9 = 8x1 + 5x2 + x3
6 = −x1 + 4x2 − 10x3
0 = 5x1 + 7x2 + 2x3
este sistema de ecuaciones puede ser re-escrito de la siguiente forma:
⎡ ⎤
⎡
⎤
⎡ ⎤
9
8 5
1
x1
⎢ ⎥
⎢
⎥
⎢ ⎥
B = Ax donde B = ⎣ 6 ⎦
A = ⎣ −1 4 −10 ⎦ x = ⎣ x2 ⎦
0
5 7
2
x3
Obtener el determinante de la matriz A.
Solución
En el cuadro 2.1 se presenta el código fuente para obtener el determinante de la
matriz A. El resultado es presentado en forma general usando variables simbólicas
para expresar el determinante analı́tico de una matriz cuadrada de dimensión 3. En
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
86
Capı́tulo 2: Métodos numéricos
varias aplicaciones de ingenierı́a y para propósitos didácticos es importante trabajar
con variables simbólicas; la función det también acepta representación simbólica.
De las lı́neas 12 a la 18, se asignan valores a las variables simbólicas de la matriz A
para mostrar el resultado en forma numérica.
Código Fuente 2.1
Determinante simbólico
%MATLAB Aplicado a Robótica y Mecatrónica
%Capı́tulo 2 Métodos numéricos
%Editorial Alfaomega
%Fernando Reyes Cortés
%Archivo cap2 detsim.m
Determinante simbólico
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
clear all;
close all;
clc;
syms a11 a12 a13 a21 a22 a23 a31 a32 a33
% matriz simbólica
Asim=[ a11, a12, a13;
a21, a22, a23;
a31, a32, a33];
disp(’Determinante simbólico’)
det sim=det(Asim)
% se asignan valores a las variables simbólicas
a11=8;
a12=5;
a13=1;
a21=-1;
a22=4; a23=-10;
a31=5; a32=7;
a33=2;
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
2.2
Sistemas de ecuaciones lineales
Código Fuente 2.2
87
Determinante simbólico
% Continúa programa 2.1
%MATLAB Aplicado a Robótica y Mecatrónica
%Capı́tulo 2 Métodos numéricos
%Editorial Alfaomega
%Fernando Reyes Cortés
%Archivo cap2 detsim.m
Determinante simbólico
19
20
21
%matriz numérica
Anum=[ a11, a12, a13;
23
a21, a22, a23;
a31, a32, a33];
disp(’Determinante numérico’)
24
det num=det(Anum)
22
El programa cap2 detsim.m produce el siguiente resultado:
det sim=
a11 a22 a33 − a11 a23 a32 − a12 a21 a33 + a12 a23 a31 + a13 a21 a32 − a13 a22 a31
det num=
357
Inversión de matrices
En mecatrónica y robótica la representación de una matriz inversa está dada
por A−1 . Una matriz A ∈ IRn×n tiene inversa si la matriz A es cuadrada y su
determinante es = 0. En MATLAB hay varias formas de obtener la inversa de una
matriz cuadrada:
Ainv=inv(A) retorna la inversa de la matriz A
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
88
Capı́tulo 2: Métodos numéricos
Un mensaje de advertencia se despliega si la matriz A tiene si tiene una
singularidad (su determinante es un escalar muy pequeño o cercano a cero).
Ainv=A∧(-1)
La ecuación lineal (2.3) considera los siguientes casos: m = n (A ∈ IRn×n matriz
cuadrada). Problema subdeterminado es cuando m < n el número de ecuaciones
es menor que el número de incógnitas. Para el caso en que m > n el número de
ecuaciones es mayor que el número de incógnitas se le conoce como problema sobre
determinado.
Para el caso m = n, la matriz A es cuadrada, entonces la solución se encuentra dada
por:
x = A−1 y.
(2.4)
La solución existe y es única si el determinante de la matriz A es diferente de cero.
Una forma de verificar si la matriz A existe es analizando su determinante como en
el código 2.1:
Estructura de código 2.1
Caso 1: m = n
if
det(A)~=0
x=inv(A)*y;
end
El código 2.1 no es único. Otro tipo de expresiones que producen el mismo resultado
son:
x=A∧ (-1)*y;
x=A\y; Esta forma es recomendable, ya que la solución emplea eliminación
gausiana, y por lo tanto no se utiliza la forma tradicional de obtener la inversa.
Usando la función: x=linsolve(A,y).
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
2.2
89
Sistemas de ecuaciones lineales
linsolve
La función linsolve representa una opción para resolver un sistema lineal de
ecuaciones de la forma 2.3. La sintaxis de la función linsolve está dada como:
y=linsolve(A,x)
donde A es la matriz del sistema de ecuaciones lineales y x es el vector incógnita.
Mayor información de la función linsolve es proporcionada en:
fx >>
help linsolve ←
♣ Ejemplo 2.2
Considere el siguiente sistema de ecuaciones:
1 = x1 + 4x2 + 3x3
(2.5)
4 = 8x1 + 5x2 + 9x3
(2.6)
6 = 3x1 + 2x2 + 4x3
(2.7)
Obtener la solución del sistema x y compararla con los métodos:
x = inv(A)y, x = A−1 y y x = A\y
Solución
Para el sistema de ecuaciones planteado se tiene que la matriz A y el vector y están
dados como:
⎡
⎤
⎡ ⎤
1 4 3
3
⎢
⎥
⎢ ⎥
A = ⎣8 5 9⎦ y = ⎣2⎦.
3 2 4
4
El cuadro 2.3 contiene el código fuente para resolver el problema planteado y donde
se han implementado los métodos solicitados.
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
90
Capı́tulo 2: Métodos numéricos
Código Fuente 2.3
Comparación de soluciones SLE
%MATLAB Aplicado a Robótica y Mecatrónica
%Capı́tulo 2 Métodos numéricos
%Editorial Alfaomega
%Fernando Reyes Cortés
%cap2 sle
Comparación de soluciones SLE
1
2
3
%matriz A
A=[ 1 4 3;
5
8 5 9;
3 2 4];
y=[1; 4; 6];
6
% primera solución
7
x=inv(A)*y;
8
% segunda solución
x1=A∧ (-1)*y;
4
9
10
11
12
13
14
% tercera solución
x2=A\y;
% cuarta solución
x3= linsolve(A,y);
15
% comparación de todas las soluciones
disp(’
x
x1
x2
x3 ’)
16
disp([x x1 x2 x3])
La salida del programa cap2 sle arroja resultados idénticos:
fx >>
x
x1
x2
x3
−5.8667 −5.8667 −5.8667 −5.8667
−4.3333 −4.3333 −4.3333 −4.3333
8.0667
Alfaomega
8.0667
8.0667
8.0667
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
2.2
91
Sistemas de ecuaciones lineales
2.2.1
2.2.1. Regla de Cramer
La regla de Cramer representa otra opción para encontrar la solución a sistemas de
ecuaciones lineales de la forma (2.3). El procedimiento de la regla de Cramer consiste
en obtener la solución para cada componente xi del vector x a través de calcular el
determinante i-ésimo obtenido como por sustituir el vector y en la i-ésima columna
de la matriz A vista como un determinante, y dividirlo entre el determinante de la
matriz A. Es decir:
⎡ ⎤ ⎡
⎤
x1
det1
⎢ x ⎥ ⎢ det ⎥
2⎥
⎢ 2⎥ ⎢
x = ⎢ .. ⎥ = ⎢ .. ⎥
(2.8)
⎣ . ⎦ ⎣ . ⎦
xn
deti =
a11
detn
columnai
..
···
.
···
a1n
a21
···
···
a2n
an1
···
y
..
.
· · · ann
para i = 1 · · · n.
det(A)
♣ Ejemplo 2.3
Considere nuevamente el sistema (2.5) del ejemplo 2.2. Obtener la solución para
x usando el método de Cramer.
Solución
De acuerdo con el procedimiento de la regla de Cramer se establece que:
det1 =
1 4 3
1
1 3
4 5 9
8
4 9
6 2 4
3
= −5.8667 det2 =
1 4 3
1
6 4
= −4.3333
4 3
8 5 9
8
5 9
3 2 4
3
2 4
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
92
Capı́tulo 2: Métodos numéricos
1 4 1
8 5 4
det3 =
3 2 6
= 8.0667.
1 4 3
8 5 9
3 2 4
Por lo tanto la solución está dada por:
⎡ ⎤ ⎡
⎤ ⎡
⎤
x1
det1
−5.8667
⎢ ⎥ ⎢
⎥ ⎢
⎥
x = ⎣ x2 ⎦ = ⎣ det2 ⎦ = ⎣ −4.3333 ⎦ .
8.0667
x3
det3
2.3 Diferenciación numérica
D
iferenciación numérica es la técnica de aproximar a la derivada de una
función. La derivada es una operación matemática sobre funciones o variables
de estado, y que tiene una diversidad de aplicaciones en ingenierı́a. La derivada
de una función representa la razón de cambio con respecto al tiempo; se utilizan
en esquemas de control y en procesos de automatización. Por ejemplo, en el área
de control de posición de robots manipuladores, los algoritmos incluyen un término
denominado acción de control derivativo con la finalidad de generar amortiguamiento
o freno mecánico y con dicha acción se pretende mejorar la respuesta transitoria del
robot. Para lograr esto, se inyecta la velocidad de movimiento, la cual representa la
variación temporal de la posición.
La fricción viscosa y de Coulomb es uno de los fenómenos fı́sicos que se encuentra
presente en los sistemas mecánicos, la derivada de la posición forma parte de
este fenómeno. El modelado dinámico de sistemas mecatrónicos y robots incluye
derivadas de las variables de estado para conformar la estructura matemática
adecuada para estudiar y analizar todos los fenómenos fı́sicos del sistema. La teorı́a
de estabilidad de sistemas dinámicos es otro ejemplo donde se utilizan derivadas; por
ejemplo, la derivada de la energı́a (potencia) se emplea para llevar a cabo el análisis
de estabilidad del punto de equilibrio. Hoy en dı́a, la derivada se ha convertido en
una herramienta imprescindible para realizar automatización de sistemas.
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
2.3
93
Diferenciación numérica
La notación de la derivada de una función f (t) con respecto al tiempo es definida
como f˙(t), la cual es igual a la razón de cambio de f con respecto al tiempo t.
Matemáticamente la derivada de una función se define como:
d
f (t +
f (t) = lı́m
t→0
dt
f˙(t) =
donde
t) − f (t)
t
(2.9)
t es un infinitésimo intervalo de tiempo.
En mecatrónica hay varios procesos donde se requiere medir la razón de cambio de
las variables del robot. Por ejemplo la razón de cambio de la posición es la velocidad
de movimiento; el cambio temporal de la velocidad es la aceleración.
Por otro lado, la integral de la aceleración es la velocidad, y la integral de la velocidad
es la posición. Es decir, existe una relación muy cercana entre la integral y la
derivada; se puede considerar que son operaciones inversas una de la otra. La integral
de una derivada:
t
t
f˙(t)dt =
0
0
df (t)
dt =
dt
t
t
= f (t) − f (0)
df (t) = f (t)
0
0
retorna la función original f (t) más una constante que depende de las condiciones
iniciales.
La derivada de una integral
d
dt
t
f (t)dt = f (t)
0
retorna la función original.
Geométricamente, la derivada f˙ puede ser descrita como la pendiente de la lı́nea
tangente en la función f (t) del punto especı́fico t. La lı́nea tangente está especificada
f (t+t)−f (t)
por la pendiente
(ver figura 2.3.)
t
Debido a esto, la derivada de una constante es cero, puesto que la lı́nea tangente es
horizontal (no hay variación temporal). Los puntos donde la derivada de la función
f es cero se llaman puntos crı́ticos y pueden representar regiones horizontales de
la función f o puntos extremos (puntos que son máximo o mı́nimo local o también
globales). Si evaluamos la derivada de la función en varios puntos y observamos que
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
94
Capı́tulo 2: Métodos numéricos
Figura 2.1 Lı́nea tangente de la función f en el punto tk .
el signo de la derivada cambia, entonces un mı́nimo o máximo local ocurre en ese
intervalo. La segunda derivada de la función f¨(t) determina cuándo el punto crı́tico es
mı́nimo o máximo. Si la segunda derivada en un punto extremo es positiva, entonces
el valor de la función en el punto extremo es un mı́nimo local. Por otro lado, si la
segunda derivada de la función en un punto extremo es negativo, entonces la función
evaluada en el punto extremo es un máximo local (ver figura 2.2).
Figura 2.2 Puntos extremos: mı́nimo y máximo local (global).
Las técnicas de diferenciación numérica estiman la derivada de una función f en
un punto tk aproxima la pendiente de la lı́nea tangente en tk usando valores de la
función en puntos cercanos a tk . Si denotamos al intervalo de tiempo t como la
diferencia entre dos puntos consecutivos, t = tk − tk−1 = h, donde h es la longitud
de t, (ver figura 2.3).
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
2.3
95
Diferenciación numérica
Figura 2.3 Lı́nea tangente de la función f en el punto tk .
En control digital y sistemas discretos la longitud del intervalo h se mantiene
constante (periodo de muestreo) y se toman datos cada determinado tiempo
h, igualmente espaciadas, esto significa que el tiempo discreto transcurre como
múltiplos del periodo de muestreo tk = t + h; para n muestras se tiene que el
tiempo discreto se puede expresar como tk = kh, donde k = 1, 2, 3, · · ·, n.
Una forma ampliamente usada es el método de Euler (diferenciación numérica)
f˙(tk )
f (tk ) − f (tk−1 )
f (tk ) − f (tk−1 )
=
tk − tk−1
h
particularmente esta aproximación es conocida como diferenciación con un paso
atrás (backward difference).
También se puede obtener la aproximación de la derivada por computar la
pendiente entre f (tk ) y f (tk+1 ) conocida como diferenciación hacia adelante (forward
difference):
f˙(tk )
f (tk+1 ) − f (tk )
f (tk+1 ) − f (tk )
=
tk+1 − tk
h
la figura 2.4 ilustra ambos métodos.
Evidentemente la calidad de la derivada depende de la distancia entre esos puntos o
del periodo de muestreo h = tk − tk−1 = tk+1 − tk . De esta forma se puede obtener
la aceleración por diferenciación numérica de la velocidad:
f (tk ) − 2f (tk−1 ) + f (tk−2 )
f˙(tk ) − f˙(tk−1 )
f¨(tk ) =
=
h
h2
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
96
Capı́tulo 2: Métodos numéricos
Figura 2.4 Diferenciación numérica: backward y forward.
♣ Ejemplo 2.4
Emplear el método de Euler para convertir la siguiente ecuación diferencial en su
equivalente expresión por diferenciación numérica.
αu(t) = a2 ÿ(t) + a1 ẏ(t) + a0 y(t)
(2.10)
Solución
Empleando el método de Euler, la velocidad y aceleración toman la siguiente forma:
y(tk ) − y(tk−1 )
h
y(tk )−y(tk−1 )
y(t
)−y(t
)
− k−1 h k−2
ẏ(tk ) − ẏ(tk−1 )
h
ÿ(t)
ÿ(tk ) =
=
h
h
y(tk ) − 2y(tk−1 ) + y(tk−2 )
ÿ(tk ) =
h2
ẏ(t)
ẏ(tk ) =
Por lo tanto la ecuación diferencial 2.10 queda como:
αu(tk ) = a2 ÿ(tk ) + a1 ẏ(tk ) + a0 y(tk )
αu(tk ) = a2 [
Alfaomega
y(tk )−2y(tk−1 )+y(tk−2 )
h2
] + a1
y(tk ) − y(tk−1 )
+ a0 y(tk ).
h
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
2.3
97
Diferenciación numérica
La solución para y(tk ) está dada por:
y(tk ) = [ a0 +
a1
h
+
a2 −1
]
h2
2
[ αu(tk ) + [ 2a
+
h2
a1
h
] y(tk−1 ) −
a2
y(tk−2 ) ] .
h2
Para implementar la solución discreta y(tk ) de la ecuación (2.10) se requieren
los estados y(tk−1 ) y y(tk−2 ) y el periodo de muestre h. El método de Euler
es ampliamente utilizado para resolver ecuaciones diferenciales lineales, usando el
procedimiento de este ejemplo se obtiene la solución en forma discreta de sistemas
dinámicos lineales.
2.3.1
2.3.1. Función diff
MATLAB tiene la función diff la cual realiza la diferenciación numérica entre
valores adyacentes de un vector x.
La sintaxis de la función diff es la siguiente:
y= diff(x)
Retorna un nuevo vector y conteniendo la diferenciadiff
ción entre valores adyacentes del vector de entrada x
La función diff también se aplica a matrices, entonces opera en cada columna de
la matriz, y retorna una matriz con el mismo número de columnas, en este caso la
sintaxis es:
B= diff(A)
Retorna una nueva matriz B conteniendo la diferendiff
ciación entre valores adyacentes de cada columna de
la matriz de entrada A.
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
98
Capı́tulo 2: Métodos numéricos
♣ Ejemplo 2.5
Aproximar la derivada con respecto al tiempo de la función sen(t) mediante
diferenciación.
Solución
Para obtener la derivada temporal por diferenciación numérica de la función sen(t)
se hace uso de diff. El intervalo de diferenciación numérica se selecciona de 0
a 10 segundos, con incrementos de un milisegundo. El método analı́tico conduce
d
dt sen(t) = cos(t). Por lo tanto, se comparará la solución aproximada con la solución
analı́tica.
El programa que contiene el código fuente para resolver este problema se presenta
en el cuadro 2.5. El resultado de dicho programa se exhibe en la figura 2.5 donde se
han superpuesto ambas gráficas: por diferenciación numérica y el método analı́tico.
Figura 2.5 Comparación del método analı́tico y por diferenciación numérica.
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
2.4
99
Integración numérica
Código Fuente 2.4
Diferenciación numérica
%MATLAB Aplicado a Robótica y Mecatrónica
%Capı́tulo 2 Métodos numéricos
%Editorial Alfaomega
%Fernando Reyes Cortés
%Archivo cap2 diffnum.m
Diferenciación numérica
1
3
clc;
clear all;
close all;
4
% intervalo de tiempo
5
t=0:0.001:10;
6
% función a derivar
f=sin(t);
2
7
8
9
% derivada de la función sen(t) con respecto al tiempo.
˙ k ) = df(tk )
% es decir se obtiene f(t
dtk
10
dfdt = diff(f)./diff(t);
11
% el vector df tiene dimensión n-1
t1=0:0.001:9.999; % nueva base de tiempo con dimensión n-1
12
13
14
%compara la derivada aproximada con el método analı́tico
plot(t1,dfdt,t1,cos(t1))
2.4 Integración numérica
I
ntegración numérica es la técnica de aproximar integrales de funciones. La
integración de funciones es un tópico especial y de interés en aplicaciones
prácticas para ingenierı́a mecatrónica y robótica. Desde el punto de vista analı́tico y
experimental las integrales representan una herramienta fundamental en el análisis y
diseño de algoritmos de control, desarrollo y construcción de robots manipuladores.
La aplicación inmediata de integración numérica se encuentra en el diseño de
simuladores, la calidad de los resultados depende en gran medida de la exactitud
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
100
Capı́tulo 2: Métodos numéricos
para aproximar a una integral de función.
Esquemas de control bien conocidos que se emplean en robots manipuladores como
es el caso del proporcional integral derivativo (PID) involucra la integral del error
de posición para mejorar las caracterı́sticas operativas de la estrategia de control
t
τ
= Kpq̃(t) + Ki
t0
q̃(t) dt − Kv q̇(t)
La acción de control integral almacena energı́a (área bajo la curva del error de
posición), y mediante una adecuada sintonı́a en la ganancia integral influye sobre
la cantidad de energı́a aplicada al robot, repercutiendo en la respuesta transitoria y
reduciendo el error en estado estacionario.
Es necesario remarcar que en el caso del esquema PID, la acción de control integral se
realiza en cada instante de tiempo conforme el tiempo evoluciona. Es decir, para cada
valor de t se calcula la integral, de tal forma que no significa realizar la integral una
sola vez, más bien es un cálculo continuo, uno tras otro como el tiempo evolucione.
En contraste con otros métodos, el cálculo de la integral sólo se requiere una vez,
tal es el caso de la medición de desempeño de reguladores usando la norma L2 [q̃],
cuya expresión matemática está dada por:
1 T
L2 [q̃] =
q̃(t) 2 dt
T 0
donde T representa el tiempo de experimentación o simulación (intervalo de
integración).
Una magnitud pequeña en la norma L2 significa alto desempeño del esquema
de control. Esto significa que el área bajo la curva del error de posición no fue
significativa debido a que el robot se posicionó de manera inmediata, logrando la
convergencia hacia cero del error de posición q̃(t) → 0. Por lo tanto, en el periodo
de integración el transitorio fue rápido, sin sobretiros y el desempeño del algoritmo
de control es muy bueno. En contra parte, un alto valor de la norma L2 representa
pobre desempeño. La norma L2 es muy útil cuando se compara varios algoritmos
de control, entonces se determina cuál de ese conjunto de esquemas tiene mejores
prestaciones y por lo tanto determina la selección para una aplicación especı́fica.
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
2.4
Integración numérica
101
La adecuada interpretación de resultados experimentales, analı́ticos y funcionamiento cualitativo de estrategias de control requiere del buen entendimiento de tópicos
especı́ficos de cálculo integral y diferencial. Más aún, cómo implementar una integral,
o seleccionar el método numérico más adecuado son conocimientos fundamentales
que se ven reflejados en la exactitud de posicionamiento de un robot manipulador y
en el desempeño del esquema de control.
La integral definida de una función f (x) sobre un intervalo finito [a, b] es interpretada
como el área sobre la curva de f (x) como se muestra en la figura 2.6. Para varias
funciones la integral se puede obtener en forma analı́tica. Sin embargo, para un tipo
de funciones su integral no se puede obtener por medios analı́ticos, y por lo tanto se
requiere de métodos numéricos para estimar su valor.
Figura 2.6 La integral de f (x) significa el área bajo la curva.
Hay varias técnicas para aproximar la integral de una función f (x); la evaluación
numérica de una integral se le denomina cuadratura cuyo nombre proviene de un
problema geométrico ancestral.
A continuación se presentan dos métodos geométricos ampliamente conocidos:
método trapezoidal y regla de Simpson.
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
102
Capı́tulo 2: Métodos numéricos
2.4.1
2.4.1. Regla trapezoidal
Cuando el área bajo la curva de la función f (x) es representada por trapezoides y
el intervalo [a, b] es dividido en n secciones del mismo tamaño, entonces el área de
f (x) puede ser aproximada por la siguiente expresión:
IT
=
b−a
[f (x0 ) + 2f (x1 ) + 2f (x2 ) + · · · + 2f (xn−1 ) + f (xn )]
2n
(2.11)
donde los valores xi representan los valores finales de los trapecios, para i = 1, 2, · · ·,
n − 1; x0 = a y xn = b.
MATLAB tiene la función trapz para aproximar la integral de una función f (x)
por el método trapezoidal, cuya sintaxis es la siguiente:
y=trapz(f)
y=trapz(x,f)
y=trapz(x,f,dim)
La función trapz(f) calcula la aproximación numérica de la integral de la función
f por el método trapezoidal.
Si f es un vector, entonces y=trapz(f) es la integral de f. Cuando f representa a una
matriz y=trapz(f) retorna un vector renglón con la integral sobre cada columna.
Cuando la función trapz tiene la sintaxis y=trapz(x,f) realiza la integral de f con
respecto a x.
Para el caso y=trapz(x,f, dim), dim es un escalar que indica la dimensión de la
función f. En ambos casos, el vector x debe tener la misma dimensión dim.
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
2.4
103
Integración numérica
♣ Ejemplo 2.6
Calcular la integral de la función
sen(x) por el método trapezoidal para el
intervalo x ∈ [0, π]
π
Itrapz =
sen(x) dx.
0
Solución
El programa 2.5 contiene el código fuente para calcular la integral de la función
f (x) = sen(x) en el intervalo [0, π]. De la lı́nea 4 a la 7 se define el intervalo de
π
tiempo de integración. El incremento del paso de integración es cada 1000
. En la
lı́nea 8 se define la función sen(x) y en la lı́nea 9 se realiza la integración numérica
usando la función y=trapz(x,f). Este resultado se compara con el valor analı́tico, es
π
decir Itrapz = 0 sen(x)dx = 1 − cos(π) = 2. El resultado que genera el programa
2.5 es: Itrapz = 2.0.
Código Fuente 2.5
Integración numérica trapezoidal
%MATLAB Aplicado a Robótica y Mecatrónica
%Capı́tulo 2 Métodos numéricos
%Editorial Alfaomega
%Fernando Reyes Cortés
%Archivo cap2 trap.m
Integración numérica trapezoidal
1
clc;
2
clear all;
close all;
tini=0;
tinc=pi/1000;
3
4
5
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
104
Capı́tulo 2: Métodos numéricos
Código Fuente 2.6
Integración numérica trapezoidal
%MATLAB Aplicado a Robótica y Mecatrónica
%Capı́tulo 2 Métodos numéricos
%Editorial Alfaomega
%Fernando Reyes Cortés
%Archivo cap2 trap.m Continúa 2.5
Integración numérica trapezoidal
6
tfinal=pi;
7
10
x=tini:tinc:tfinal;
f=sin(x);
I trapz= trapz(x,f);
I a= 1-cos(pi);
11
disp(’Resultado’)
12
%despliega los resultados comparativos
13
%del método trapezoidal con el analı́tico.
disp([I trapz I a])
8
9
14
En este ejemplo se ha calculado el área bajo la curva sobre el intervalo [0, π].
El cálculo se realiza una sola vez. Sin embargo, hay aplicaciones en control
de robots manipuladores donde se requiere realizar el cálculo de la integral de
manera sistemática conforme el tiempo evoluciona. A continuación se muestra un
procedimiento recursivo donde se obtiene la integral por el método trapezoidal para
π
pequeños intervalos del tamaño de 1000
sobre un intervalo amplio [0, 2π].
♣ ♣ Ejemplo 2.7
Calcular la integral de la función sen(x) por el método trapezoidal para cada
valor de intervalo
π
1000
contenido en el intervalo [0, 2π]
2π
Itrapz =
Alfaomega
sen(x) dx.
0
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
2.4
Integración numérica
105
Solución
π
La idea es obtener un barrido de la integral de sen(x) para cada intervalo 1000
, que
π
π
π
pertenece al intervalo [0, 2π]. Es decir, x ∈ [0, 1000 , 2 1000 , 3 1000 , · · · , 2π]. En otras
π
, hasta llegar a 2π,
palabras, el intervalo va evolucionando por incrementos de 1000
con esto se requieren 2000 iteraciones para alcanzar el valor de 2π.
El programa que se presenta en el cuadro de código 2.7 contiene la forma de
implementar el método trapezoidal para aproximar la integral de una función
f (x) = sen(x) en el intervalo [0, 2π]. De la lı́nea 4 a la 9 se define el intervalo
π
de tiempo de integración. El incremento del paso de integración es cada 1000
.
En la lı́nea 10 se obtiene la dimensión del vector de tiempo, con esa información se
realizará el algoritmo recursivo, es decir el número de veces (2000) que se realizará el
ciclo for para abarcar el intervalo [0, 2π] el cual se presenta de las lı́neas 11 a la 21.
En la lı́nea 12 se exploran los registros de la base de tiempo t(k); cuando k=1
pi
entonces se realiza el primer paso de integración del intervalo [0, 1000
] cuyo código
abarca las lı́neas 13 a la 16. Este código es necesario por razones técnicas del
lenguaje de MATLAB debido a que el primer elemento del vector de tiempo
t(1) es cero, ts=0, entonces no se podrı́a llevar a cabo el primer intervalo de
integración x=0:tinc:0, marcando un error de programación. En su lugar se realiza
x=0:tinc:tinc.
Para el segundo incremento del tiempo de integración y posteriores incrementos
π
se ubica en las lı́neas 17 a la 21 sobre intervalo [ 1000
, · · · , 2π]. En la lı́nea 23 se
asigna la función sen(x) para el correspondiente k-ésimo intervalo de integración
pi
pi
, k 1000
]. La lı́nea 24 tiene la función I trapz(k)=trapz(x,f). Este
[(k − 1) 1000
procedimiento se realiza un número de veces (2000) hasta igualar la dimensión del
vector de tiempo t.
En la lı́nea 28 se grafica la integral de la función sen(x) por el método trapezoidal
y se compara con el método analı́tico:
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
106
Capı́tulo 2: Métodos numéricos
2π
Itrapz =
sen(x) dx
0
= − cos(x)|2π
0
= 1 − cos(x), x ∈ [0, 2π]
Código Fuente 2.7
Método de integración trapezoidal
%MATLAB Aplicado a Robótica y Mecatrónica
%Capı́tulo 2 Métodos numéricos
%Editorial Alfaomega
%Fernando Reyes Cortés
%Archivo cap2 trapezoidal.m
% calcula la integral de la función sen(x)
% el cálculo se realiza por medio de
%integración iterativa (2000 iteraciones)
%con incrementos de
π
1000
% para el intervalo [0, 2π].
Método de integración trapezoidal
1
3
clc;
clear all;
close all;
4
% intervalo de integración
5
7
tini=0; % tiempo inicial de integración
tinc=pi/1000; % pasos de incremento del tiempo de integración
tfinal=2*pi; % tiempo final de integración
8
% base de tiempo de integración
9
t=tini:tinc:tfinal;
[m,n]=size(t); % dimensión del vector de tiempo
2
6
10
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
2.4
107
Integración numérica
Código Fuente 2.8
Método de integración trapezoidal
%Continúa programa 2.7
%MATLAB Aplicado a Robótica y Mecatrónica
%Capı́tulo 2 Métodos numéricos
%Editorial Alfaomega
%Fernando Reyes Cortés
%Archivo cap2 trapezoidal.m
Método de integración trapezoidal
11
for k=1:n
12
% registro de la base de tiempo
13
ts=t(k);
if k==1
14
15
% para el primer paso de integración
16
17
% se integra desde 0 a
x=0:tinc:tinc;
18
else
19
% para el segundo paso de integración y posteriores
20
% se integra desde x = 0 hasta el tiempo actual ts.
x=0:tinc:ts;
21
22
π
1000 .
end
f=sin(x); % función a integrar
I trapz(k)= trapz(x,f); % integración por método trapezoidal
23
24
25
end
26
% gráfica comparativa entre los métodos
27
% analı́tico y trapezoidal
plot(x,I trapz,x,1-cos(x))
28
La figura 2.7 muestra el resultado comparativo entre el método trapezoidal y el
analı́tico para la integral de la función sen(x) que genera el programa 2.7. Obsérvese
que el método trapezoidal es una aproximación muy buena comparada con el exacto
o método analı́tico, de tal forma que el resultado trapezoidal se superpone a la gráfica
del método analı́tico (cos(x)-1) en el intervalo [0, 2π].
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
108
Capı́tulo 2: Métodos numéricos
Figura 2.7 Comparación entre los métodos trapezoidal y analı́tico.
2.4.2
2.4.2. Regla de Simpson
Cuando el área bajo la curva de la función f (x) es representada sobre áreas de
secciones cuadráticas y si el intervalo [a, b] es dividido en 2n secciones iguales,
entonces el área de f (x) puede ser aproximada por la regla de Simpson cuya expresión
está dada de la siguiente manera:
Isimp =
b−a
[f (x0 ) + 4f (x1 ) + 2f (x2 ) + 4f (x3 ) + · · · + 2f (x2n−2 )
6n
+4f (x2n−1 ) + f (x2n )]
(2.12)
donde xi representan los valores finales de cada sección, para i = 1, 2, · · ·, n − 1;
x0 = a, y x2n = b.
En MATLAB, la regla de Simpson se implementa en forma adaptiva por medio de
la función quad; la tabla 2.1 muestra su sintaxis.
quad(f,a,b) aproxima la integral de una función f (x) dentro de los lı́mites finitos
[a, b], usando un algoritmo recursivo adaptable de cuadratura de Simpson, donde
a, b ∈ IR. El error de integración numérica que usa es 1e-06. La función f (x) puede
ser escalar o vectorial. En el caso vectorial quad retorna un vector de salida donde
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
2.4
109
Integración numérica
Tabla 2.1 Funciones de cuadratura
Retorna el área de la función en el
quad(’nombre funcion’,a,b)
intervalo comprendido entre a y b
usando la regla de Simpson.
Emplea un error absoluto de tolerancia tol en lugar del valor por default 1e-06 sobre el intervalo [a, b].
q = quad(fun,a,b,tol)
Valores grandes de tol producen integración numérica muy rápida, pero con menor exactitud.
cada componente tiene la integral de la correspondiente componente de la función
vectorial f (x). La función quad puede ser menos eficiente (pobre exactitud) con
funciones no suaves.
♣ Ejemplo 2.8
Calcular la integral por el método de Simpson de la función raı́z cuadrática
√
f (x) = x para intervalos no negativos [a, b].
Solución
√
La función raı́z cuadrada f (x) = x puede ser integrada analı́ticamente sobre el
intervalo [a, b], a, b ∈ IR+ de la siguiente forma:
b√
I=
a
x dx =
3
2 3
(b 2 − a 2 ).
3
(2.13)
Este procedimiento sirve de referencia para comparar el valor numérico que calcula
el método de Simpson.
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
110
Capı́tulo 2: Métodos numéricos
√
El cuadro 2.9 contiene el programa para calcular la integral de la función f (x) = x
por el método de Simpson sobre un intervalo no negativo [a, b] especificado por
el usuario, y el resultado es comparado con el método analı́tico (2.13) el cual se
encuentra implementado en la lı́nea 17.
Por precaución, sobre los datos del intervalo definido por el usuario se inserta la
instrucción if para verificar que los extremos del intervalo sean positivos y además
que cumplan a < b.
Se hace la aclaración que dentro del lenguaje de programación de MATLAB la
función sqrt acepta valores negativos y retorna números complejos. Sin embargo,
en modelado dinámico y control de robots manipuladores usando moldeo de energı́a
(estabilidad de Lyapunov) no se requiere la teorı́a de números complejos.
Código Fuente 2.9
Integración Simpson
%MATLAB Aplicado a Robótica y Mecatrónica
%Capı́tulo 2 Métodos numéricos
%Editorial Alfaomega
%Fernando Reyes Cortés
%Archivo cap2 simpson1.m
%Función quad para calcular la integral de la
%función raı́z cuadrada por el método de Simpson.
%El resultado se compara con la integral analı́tica.
%El intervalo de integración es definido por el usuario.
Integración Simpson
1
3
clc;
clear all;
close all;
4
disp(’Método de Simpson’)
5
% los valores del intervalo [a, b] son proporcionados
6
% por el usuario desde el teclado
a=input(’Introduzca el valor inicial del intervalo de integración: ’);
b=input(’Introduzca el valor final del intervalo de integración: ’);
2
7
8
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
2.4
111
Integración numérica
Código Fuente 2.10
Integración Simpson
% Continuación del programa 2.9
%MATLAB Aplicado a Robótica y Mecatrónica
%Capı́tulo 2 Métodos numéricos
%Editorial Alfaomega
%Fernando Reyes Cortés
%Archivo cap2 simpson1.m
Integración Simpson
9
10
11
12
if a>=0 && b>= 0
if a>b
disp(’Error: el intervalo positivo [a,b] debe cumplir a<b.’)
return
13
end
14
% regla de Simpson
15
% la función raı́z cuadrada (sqrt) es indicada como
16
% cadena de caracteres en la función quad
17
I simp=quad(’sqrt’,a,b);
18
% método analı́tico
I a=2/3*(b∧(3/2)-a∧ (3/2));
fprintf(’Valor analı́tico= %f \n Simpson: %f \n’,I a,I simp)
19
20
21
else % Para el caso de valores negativos
disp(’Error en los valores del intervalo de integración.’)
disp(’El intervalo [a,b] con a<b debe contener únicamente valores positivos.’)
22
23
24
end
La salida del programa cap2 simpson1.m produce resultados idénticos entre el
método de Simpson con el analı́tico:
Valor analı́tico=33.333333
Simpson=33.333333
corresponde al intervalo a=0 y b=10.
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
112
Capı́tulo 2: Métodos numéricos
♣ Ejemplo 2.9
Calcular la integral de la función f (x) = x5 por el método de Simpson.
Solución
La integral de la función f (x) = x5 está dada por:
b
I=
x5 dx =
a
1 6
(b − a6 ).
6
(2.14)
El cuadro 2.11 contiene el código fuente para aproximar la integral de la función
cuadrática f (x) = x2 por el método de Simpson sobre un intervalo de [0, 10]. En la
lı́nea 6, la función cuadrática es utilizada a través de un manejador de funciones:
f=@(x)x.∧ 5; e incorporada la función quad(f,a,b) en la lı́nea 7.
Código Fuente 2.11
Método de Simpson
%MATLAB Aplicado a Robótica y Mecatrónica
%Capı́tulo 2 Métodos numéricos
%Editorial Alfaomega
%Fernando Reyes Cortés
%Archivo cap2 simpson.m
Método de Simpson
1
2
3
4
clc;
clear all;
close all;
7
a=0; b=10;
x=a:b;
f= @(x)x.∧ 5; %función cuadrática
I simp=quad(f,a,b);
8
I a= (1/6)*(b∧ 6-a∧ 6); % método analı́tico
9
% comparación entre el método analı́tico y el de Simpson
5
6
10
fprintf(’Valor analı́tico= %f \n Simpson: %f \n’,I a,I simp)
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
2.4
Integración numérica
113
La salida del programa cap2 simpson.m es la siguiente:
Valor analı́tico=166666.666667
Simpson=166666.666667
corresponde al intervalo [0, 10].
2.4.3
2.4.3. Funciones de cuadratura
MATLAB tiene varias funciones que calculan la integral de funciones por métodos
numéricos conocidas como funciones de cuadratura. Adicional a la regla de Simpson
quad, existen más opciones que se presentan a continuación:
quad8
La función quad8(’nombre funcion’,a,b) retorna el área de la función en el
intervalo [a, b] usando la regla de Newton-Cotes 8 panel.
quadl
q = quadl(f,a,b) aproxima la integral de la función f en un intervalo finito [a, b]
dentro de un error 1e-06 usando el algoritmo recursivo adaptable de cuadratura de
Lobatto. La sintaxis de quadl requiere que la función f sea una función manejador.
quadl acepta funciones vectoriales y retorna un vector con la integral de cada
componente. quadl puede ser más eficiente (alta exactitud) que la función quad
con funciones suaves.
quadgk
La función quadgk tiene buena eficiencia para funciones oscilatorias, y soporta
intervalos de integración infinitos, ası́ como manejar moderadamente valores
singulares. Una ventaja de esta función es que soporta integración de contorno a lo
largo de trayectorias continuas por trozos. Si el intervalo de integración es infinito,
es decir [a, ∞], entonces para que la integral de f (x) exista, f (x) debe caer como
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
114
Capı́tulo 2: Métodos numéricos
x → ∞. Esta es una condición para poderse utilizar quadgk, particularmente para
funciones oscilatorias sobre intervalos infinitos, f (x) debe caer muy rápida.
Una ventaja que tiene la función quadgk es que integrará funciones que tienen
puntos singulares no muy fuertes. Si la función tiene puntos singulares dentro del
intervalo (a, b), entonces lo recomendable es escribir la integral como la suma de
integrales sobre subintervalos con puntos singulares como puntos finales, computar
cada integral con quadgk y sumar los resultados.
quadv
La función quadv vectoriza a quad para una función vectorial.
2.4.4
2.4.4. Método de Euler
Un método ampliamente utilizado en robótica y sistemas mecatrónicos es la
integración numérica discreta, la cual se puede establecer directamente del método
de Euler.
Por ejemplo al derivar con respecto al tiempo la integral I(t) de la función f (t) se
obtiene lo siguiente:
t2
I(t) =
˙ = f (t) ∀t ∈ [t1 , t2 ].
f (t)dt ⇒ I(t)
(2.15)
t1
Partiendo de la definición matemática de la derivada se tiene:
˙
I(t)
=
lı́m
t→0
I(t +
t) − I(t)
t
(2.16)
(2.17)
donde
t = t2 − t1 , representa un infinitésimo intervalo de tiempo.
˙
˙
˙ k ) entonces el
Si aproximamos a la derivada I(t)
en su forma discreta I(t)
I(t
tiempo discreto está dado por tk = kh, k = 1, 2, · · ·, n y h es el periodo de muestreo.
De esta forma se cumple tk−1 = (k − 1)h y tk = tk − tk−1 = kh − (k − 1)h = h.
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
2.4
Integración numérica
115
˙ k ) se puede realizar por diferenciación numérica
Por lo tanto la derivada discreta I(t
de la posición I(tk ) de la siguiente forma:
˙ k) =
I(t
I(tk ) − I(tk−1 )
I(tk ) − I(tk−1 )
= f (tk ).
=
tk − tk−1
h
(2.18)
La expresión 2.18 se conoce como método de Euler y sirve para estimar la velocidad
por diferenciación numérica de la posición.
Despejando I(tk ) de la expresión (2.18), la integral discreta adquiere la siguiente
forma:
I(tk ) = I(tk−1 ) + hf (tk )
(2.19)
La expresión (2.19) es muy simple, se convierte en una sumatoria y es adecuada para
poderse implementar como algoritmo recursivo.
♣ Ejemplo 2.10
Calcular la integral de la función f (t) = sen(t) por el método de Euler en forma
recursiva para el periodo de integración t ∈ [0, 10] segundos. El incremento del
tiempo es por pasos de un milisegundo.
Solución
En el cuadro 2.12 se presenta el código fuente que implementa el método de Euler
para integrar la función f (t) = sen(t). El intervalo de integración es de 0 a 10
segundos, con incremento de un milisegundo. Este mismo incremento se toma como
el valor del periodo de muestreo h = 0.001. Para propósitos de comparación el
resultado generado por el método de Euler se compara con el método trapezoidal.
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
116
Capı́tulo 2: Métodos numéricos
Código Fuente 2.12
Método de Euler
%MATLAB Aplicado a Robótica y Mecatrónica
%Capı́tulo 2 Métodos numéricos
%Editorial Alfaomega
%Fernando Reyes Cortés
%Archivo cap2 euler.m
Método de Euler
1
clc;
2
clear all;
close all;
h=0.001; % periodo de muestreo
t=0:h:10; % intervalo de integración
3
4
5
6
7
f=sin(t); % función a integrar
I e=0; % condición inicial de la integral por
8
% método de Euler
9
% método comparativo trapezoidal
I trap=trapz(t,f);
10
11
12
13
14
% método comparativo regla de Simpson
I simp=quad(’sin’,0,10);
[m, n]=size(t); % Orden del vector de tiempo
for k=1:n
15
I e=I e+h*f(k);
16
end
17
fprintf(’Método Euler= %f \n Método Trapezoidal= %f \n Regla de Simpson = %f
\n’,I e,I trap, I simp)
La salida del programa cap2 euler.m produce el siguiente resultado.
Método Euler= 1.838799
Método Trapezoidal= 1.839071
Regla de Simpson= 1.839072
Los resultados corresponden al intervalo [0, 10], con incrementos de 0.001.
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
2.5
Sistemas dinámicos de primer orden
117
Obsérvese que existe un mayor error de integración del método de Euler con
respecto a los métodos trapezoidal y el de Simpson. Sin embargo, en robótica se
utiliza el método de Euler para estimar la velocidad de movimiento del robot por
diferenciación numérica de la posición.
2.5 Sistemas dinámicos de primer orden
L
os sistemas dinámicos están formados por ecuaciones diferenciales, ya que
representan la estructura matemática fundamental para modelar la dinámica
de los robots manipuladores y de cualquier sistema mecatrónico. A través de estas
ecuaciones es posible estudiar en detalle todos los fenómenos fı́sicos que se encuentran
presentes en la estructura mecánica de los robots.
Particularmente la ecuación diferencial ordinaria de primer orden:
ẋ = f (x)
(2.20)
representa la base matemática del modelado dinámico y control de robots
manipuladores. Donde x ∈ IRn es la solución de la ecuación diferencial (2.20);
también representa la variable de estado, la cual proporciona información sobre
la dinámica del sistema, por esta razón x es que es una función continua del tiempo
x = x(t). En otras palabras dentro de las propiedades intrı́nsecas de las variables de
estado se encuentra que es una función implı́cita del tiempo. La variable ẋ representa
los cambios de estado dinámicos x(t) en el tiempo.
La ecuación 2.20 es un sistema dinámico autónomo para modelar dinámica lineal
y no lineal de sistemas mecatrónicos y robóticos. Este tipo de ecuación dinámica
se emplea en la simulación, diseño de algoritmos de control, análisis y estudio de
fenómenos fı́sicos y construcción de sistemas mecatrónicos.
Prácticamente cualquier sistema dinámico autónomo (que no depende de manera
explı́cita del tiempo) cuyo modelo matemático esté caracterizado por ecuaciones
diferenciales de cualquier orden puede ser convertido mediante un adecuado cambio
de variables de estado a la forma de la ecuación 2.20, entonces se puede simular
mediante un adecuado procedimiento de integración numérica.
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
118
Capı́tulo 2: Métodos numéricos
2.5.1
2.5.1. Método de Runge-Kutta
Uno de los métodos más importante para integración numérica de ecuaciones
diferenciales de primer orden (2.20) son los métodos de Runge-Kutta. Dichos
métodos se basan en aproximar la solución a través de series de Taylor. El método
más simple, el denominado método de primer orden, usa una serie de expansión de
Taylor de primer orden, el método de segundo orden usa la serie de expansión de
Taylor de segundo orden, y ası́ sucesivamente (el método de Euler es equivalente al
de primer orden de Runge-Kutta). MATLAB tiene funciones del método de RungeKutta para los órdenes segundo, tercero, cuarto y quinto.
La serie de Taylor para evaluarla en el tk -ésimo tiempo a la función x(tk ) está dada
por la siguiente expresión:
x(tk ) = x(tk−1 ) + (tk − tk−1 )ẋ(tk−1 ) +
(tk − tk−1 )n
n!
(tk − tk−1 )2
ẍ(tk−1 ) + · · · +
2!
n derivadas
· ·x· · · · + · · ·
(tk−1 )
(2.21)
El término tk − tk−1 representa un pequeño intervalo el cual será representado por
h = tk − tk−1 = kh − (k − 1)h, entonces la serie de Taylor queda de la siguiente
forma:
n derivadas
h2
hn
· ·x· · · · + · · ·
x(tk ) = x(tk−1 ) + hẋ(tk−1 ) + ẍ(tk−1 ) + · · · +
(tk−1 )
2!
n!
(2.22)
Método de Runge-Kutta de primer orden
La integración numérica de la ecuación 2.20 por el método de Runge-Kutta de primer
orden está dado de la siguiente manera:
x(tk ) = x(tk−1 ) + hẋ(tk−1 ) = x(tk−1 ) + hf (tk−1 )
Alfaomega
(2.23)
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
2.5
Sistemas dinámicos de primer orden
119
La interpretación geométrica de la ecuación (2.23) significa que el valor de estimación
de x(tk ) es igual a la lı́nea tangente que la une con el valor x(tk−1 ) como se muestra
en la figura 2.8. Este es un proceso iterativo para todos los puntos x(tk ), con
k = 1, 2, · · ·, n. El primer método de Runge-Kutta es muy simple, ya que aproxima
Figura 2.8 Cálculo de x(tk ) usando el primer el método de Runge-Kutta primer
orden.
la función con una serie de Taylor corta que une los puntos tk y tk−1 con lı́neas
tangentes, por lo cual la exactitud es pobre si el paso h = tk − tk−1 es grande o si la
pendiente de la función cambia rápidamente.
Método de Runge-Kutta de orden mayor
Los métodos de Runge-Kutta de orden mayor (dos, tercero, cuarto, y quinto) se usan
para aproximar funciones desconocidas; la aproximación se realiza a través de varias
lı́neas tangentes, por lo tanto la exactitud es mejorada. Por ejemplo, en el método de
integración de cuarto orden usa la serie de Taylor con las primeras cuatro derivadas,
es decir la estimación de la función x(tk ) es a través de 4 lı́neas tangentes.
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
120
Capı́tulo 2: Métodos numéricos
Funciones ode
MATLAB tiene las funciones para integrar la solución numérica de ecuaciones
diferenciales ordinarias de primer orden llamadas ode.
ode45
La sintaxis de la función ode45 es la siguiente:
[t,x]=ode45(’nombre funcion’,ts,cond iniciales,opciones)
La función ode45 utiliza los métodos de Runge-Kutta de cuarto y quinto orden.
nombre funcion representa una función M-File donde está implementado el sistema
dinámico en la estructura matemática ẋ = f (x).
La función ode45 retorna la solución del sistema ẋ = f(x), es decir x, ası́ como
el vector de tiempo t. El tiempo de simulación o el intervalo de integración se
encuentra especificado por ts = [tinicial, tfinal], por ejemplo se puede especificar
como un intervalo [0, 10]. También es válido expresarlo como: ts = 0 : 0.001 : 10, el
cual incluye incrementos de tiempo de un milisegundo. Las condiciones iniciales
se encuentran determinadas por cond iniciales, su forma depende del orden
del sistema. Por ejemplo, para un sistema escalar cond inciales=0; para el caso
vectorial x(0) ∈ IR3 , tenemos cond inciales=[0;0;0].
El cuarto parámetro de opciones es muy importante debido a que contiene las
propiedades de integración numérica, y de eso depende la simulación del sistema.
Para tal efecto se emplea la función odeset de la siguiente forma:
opciones=odeset(’RelTol’,1e-3,’AbsTol’,1e-6,’InitialStep’,1.0e-3,’MaxStep’,1.0e-3)
Error de integración
En cada i-ésimo paso de integración se estima un error local de la iésima componente de la solución; este error debe ser menor o igual
que el error aceptable, el cual es una función de la tolerancia relativa
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
2.5
121
Sistemas dinámicos de primer orden
RelTol y de la tolerancia absoluta AbsTol. La expresión para e(i)
satisface la siguiente condición: |e(i)| = max(RelTol|x(i), AbsTol(i)|).
RelTol representa el error relativo de tolerancia que se
aplica a todas las componentes xi del vector solución
RelTol
x ∈ IRn .Este parámetro también controla el número
correcto de dı́gitos en todas las componentes xi . El
valor tı́pico RelTol= 1e-3, el cual corresponde al 0.1 %
de exactitud.
Error absoluto de tolerancia que se aplica a todas
las componentes individuales xi del vector solución x.
También determina la exactitud cuando la solución se
aproxima a cero.
Si AbsTol es un vector, entonces su dimensión debe ser
AbsTol
la misma del vector x. El valor de cada componente
de AbsTol se aplica a la correspondiente componente
xi .
Si AbsTol es un escalar, ese valor se aplica a todas las
componentes xi del vector solución x.
Generalmente, el valor tı́pico que se le asigna a AbsTol
es: AbsTol= 1e-6.
NormControl significa la norma del error relativo. Esta opción solicita a la función ode45 que el error
de integración en cada paso cumpla con la siguiente
NormControl
condición: e ≤ max(RelTol x , AbsTol). Se habilita/deshabilita como: on (off). Para funciones suaves, la función ode45 entrega un error equivalente a la
exactitud solicitada. La exactitud es menor para problemas donde el intervalo de integración es grande y
para problemas moderadamente inestables.
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
122
Capı́tulo 2: Métodos numéricos
Paso de integración
Especifica el valor inicial del paso de integración.
Representa una cota superior en la magnitud del
primer paso. En caso de no especificarlo, entonces el
InitialStep
tamaño del paso se obtiene como la pendiente de la
solución en el tiempo inicial. Si la pendiente de todas
las componentes de la solución es cero, entonces el
procedimiento puede generar un tamaño de paso muy
grande. Por lo tanto, es recomendable iniciar con un
valor adecuado esta opción.
Representa una cota superior del tamaño del paso,
es un valor escalar positivo que se obtiene de la
MaxStep
siguiente forma: 0.1|tinicial − tfinal |. La ecuación
diferencial tiene soluciones o coeficientes periódicos,
es recomendable inicializar MaxStep= 14 del periodo.
La función ode45 es la que presenta mayor exactitud en el método de integración
para el sistema ẋ = f(x). Sin embargo, debido a esto también puede tomar más
tiempo en dicho proceso.
A continuación se describe otras opciones de funciones para resolver ecuaciones
diferenciales ordinarias de primer orden, cuya sintaxis es exactamente la misma que
la función ode45. Sin embargo, tienen otras caracterı́sticas operativas.
ode23
La función ode23 utiliza el segundo y tercer método de integración. Tiene menor
exactitud comparada con la función ode45. Sin embargo, puede realizar más rápido
el proceso de integración numérica. La sintaxis está dada por:
[t,x]=ode23(’nombre funcion’,ts,cond iniciales,opciones)
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
2.5
Sistemas dinámicos de primer orden
123
ode113
[t,x]=ode113(’nombre funcion’,ts,cond iniciales,opciones)
Esta función se recomienda para resolver sistemas dinámicos complicados; mantiene
un error de integración riguroso. La función ode113 es un método de integración de
orden variable propuesto por Adams-Bashforth-Moulton.
ode15s
[t,x]=ode15s(’nombre funcion’,ts,cond iniciales,opciones)
Para el caso en que el proceso de integración de la función ode45 sea muy lento
debido a la rigidez del sistema dinámico, entonces es recomendable usar la función
ode15s. Esta función es de orden variable, multi pasos basada en diferenciación
numérica (backward o método Gear).
ode23s
[t,x]=ode23s(’nombre funcion’,ts,cond iniciales,opciones)
Es muy útil para sistemas dinámicos masa-resorte con alta rigidez en que el resorte
la matriz de masas es constante. La función ode23s se basa en el método modificado
de segundo orden de Rosenbrock. Debido a que el proceso de integración es por
pequeños pasos, puede ser más eficiente que la función ode15s para tolerancias
pequeñas en el error de integración. Además es mucho más efectivo en sistemas
dinámicos con rigidez donde ode15s no lo es.
ode23t
[t,x]=ode23t(’nombre funcion’,ts,cond iniciales,opciones)
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
124
Capı́tulo 2: Métodos numéricos
La función ode23t emplea el método trapezoidal usando una interpolación libre. Es
recomendable para resolver sistemas dinámicos moderados en la rigidez del resorte,
sin utilizar amortiguamiento. La exactitud en la integración es baja.
ode23tb
[t,x]=ode23tb(’nombre funcion’,ts,cond iniciales,opciones)
La función ode23tb es una combinación de métodos de Runge-Kutta y diferenciación
numérica (backward) de segundo orden. Para sistemas dinámicos con alta rigidez del
resorte y errores de integración muy pequeños, la exactitud de integración es baja.
2.5.2
2.5.2. Simulación de sistemas dinámicos ẋ = f (x)
A continuación se presenta una serie de ejemplos ilustrativos cuya finalidad es
mostrar el empleo de las funciones ode en simulación de sistemas mecatrónicos y
robots manipuladores.
♣ Ejemplo 2.11
Considere un sistema dinámico lineal escalar dado de la siguiente forma:
ẋ(t) = −ax(t) + bu(t)
y(t) = cx(t)
donde x es la solución del sistema dinámico y representa la variable de estado.
La entrada del sistema es u(t) y la respuesta es y. Los parámetros están dados
por a, b, c ∈ IR+ .
Realizar la simulación del sistema usando integración numérica a través de la
función ode45. Para una entrada u = 1, y parámetros a = 1.33, b = 2.6 y c = 1.
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
2.5
Sistemas dinámicos de primer orden
125
Solución
El sistema dinámico lineal escalar tiene una función de transferencia (control clásico)
dada por:
x
u
=
b
s+a
y = cx =
cb
s+a
donde s = jw, j representa la parte imaginaria o compleja, y w ∈ IR+ es la frecuencia
en rad/seg. El parámetro a ∈ IR+ es el ancho de banda del sistema y b ∈ IR+ significa
la ganancia del sistema. Por otro lado, c ∈ IR+ es una ganancia de un amplificador
operacional que acopla de manera adecuada la impedancia entre la variable de estado
x y la respuesta del sistema y.
Para obtener la ecuación diferencial (modelo dinámico) a partir de la función de
transferencia, entonces s no representa frecuencia, en este caso se interpreta como
d
un operador s = dt
:
b
x
=
u
s+a
y
donde se ha empleado que sx =
d
dt x
⇒ sx + ax = bu
⇒ ẋ = −ax + bu
=
cx
= ẋ.
La figura 2.9 muestra el diagrama a bloques del sistema dinámico lineal.
Figura 2.9 Diagrama a bloques de un sistema dinámico lineal.
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
126
Capı́tulo 2: Métodos numéricos
El programa que implementa al sistema dinámico lineal o planta de estudio se
encuentra contenido en el cuadro 2.13 archivo cap2 ejemplo26 (M-File). Para
resolver el sistema dinámico, se emplea la función de integración numérica ode45.
Por facilidad, se emplea una entrada constante de magnitud unitaria. Los valores de
los parámetros de la planta son a = 1.33 y b = 2.6. La ganancia del amplificador es
unitaria c = 1.
Para ejecutar correctamente la función cap2 ejemplo26 se requiere del programa
simuejemplo26 que se encuentra listado en el cuadro 2.13, donde se definen las
propiedades de integración usando la función odeset, las cuales son incorporadas en
la función ode45. El tiempo de simulación es de 0 a 10 segundos con incrementos de
una milésima de segundo.
La salida del programa 2.13 se presenta en la figura 2.10, observe que la respuesta a
una entrada constante de magnitud unitaria no produce sobre impulso para sistemas
lineales de primer orden. En estado estacionario, la salida x(t) llega a un valor de
1.9549 (frecuencia cero o DC: s = 0) debido a que la función de transferencia adquiere
la forma ab = 1.9549.
Figura 2.10 Respuesta a una entrada constante unitaria del sistema lineal escalar.
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
2.5
Sistemas dinámicos de primer orden
Código Fuente 2.13
127
Sistema lineal escalar
%MATLAB Aplicado a Robótica y Mecatrónica
%Capı́tulo 2 Métodos numéricos
%Editorial Alfaomega
%Fernando Reyes Cortés
%Archivo cap2 ejemplo26
% Simulación de un sistema lineal escalar
% de la forma ẋ = −ax + bu
%Entradas de la función: t tiempo
% x vector de estados
% ẋ (xp) es la velocidad del estado x
% u es la entrada al sistema
% a, b son los parámetros
% Salida derivada de la variable de estados: xp
% Para su correcta ejecución requiere del archivo
%cap2 simuejemplo26
Sistema lineal escalar
1
function xp =cap2 ejemplo26(t,x)
2
%parámetros del sistema: a, b.
3
%el parámetro a representa el ancho de banda
4
5
%del sistema dinámico lineal.
a=1.33;
6
%la proporción
7
%de la función de transferencia en frecuencia cero.
b=2.6;
8
9
b
a
es la ganancia
%señal de entrada unitaria
u=1;
10
11
12
%sistema dinámico lineal escalar
xp=-a*x+b*u;
13
%fin de función
14
end
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
128
Capı́tulo 2: Métodos numéricos
Código Fuente 2.14
Simulación del ejemplo 2.11
%MATLAB Aplicado a Robótica y Mecatrónica
%Capı́tulo 2 Métodos numéricos
%Editorial Alfaomega
%Fernando Reyes Cortés
%Archivo cap2 simuejemplo26
% Realiza la simulación de un sistema dinámico lineal escalar
% Requiere del archivo cap2 ejemplo26.m
Simulación del ejemplo 2.11
1
2
clc;
clear all;
3
close all;
4
%tiempo de simulación
ti=0; %tiempo inicial
tf = 10; % tiempo de simulación (segundos)
5
6
7
8
9
h=0.001; % incremento de un milisegundo
ts=ti:h:tf; % vector de tiempo
cond iniciales=0;
10
%opciones del proceso de integración numérica
11
opciones=odeset(’RelTol’,h, ’AbsTol’,1e-06,’InitialStep’,h,’MaxStep’,h);
12
% función ode45 para integración numérica
13
% solución del sistema x(t) por integración numérica Runge-Kutta de cuarto y
quinto orden:
14
disp(’Simulación de un sistema lineal escalar’)
[t,x]=ode45(’cap2 ejemplo26’,ts,cond iniciales,opciones);
15
16
% ganancia del amplificador que acopla impedancia de la variable de estados x(t)
con la salida.
17
c=1;
18
%salida del sistema
y=c*x;
19
20
21
% gráfica del sistema lineal escalar
plot(t,y)
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
2.5
129
Sistemas dinámicos de primer orden
♣ ♣ Ejemplo 2.12
Considere un sistema dinámico lineal de segundo orden, dado por la siguiente
ecuación:
ẋ(t) =
0
1
−ωn2 −2ρωn
y(t) = [ 1 0 ] x(t)
x(t) +
0
ωn
u(t)
donde x ∈ IRn es el vector solución del sistema dinámico y representa la variable
de estado. La entrada del sistema es u(t) y la respuesta es y. Los parámetros están
dados por: ωn ∈ IR+ es la frecuencia natural del sistema, ρ ∈ IR+ es el factor de
amortiguamiento (damping) y s = jw, j es la componente imaginaria o compleja,
w es la frecuencia en rad/seg, t es la evolución del tiempo.
Solución
El sistema dinámico lineal de segundo orden puede ser deducido de la siguiente
función de transferencia
y
u
=
ωn
.
s2 + 2ρωn s + ωn2
Manipulando la función de transferencia y considerando que s =
s2 y(t) = ÿ(t), sy(t) = ẏ(t), se obtiene lo siguiente:
y
ωn
= 2
u
s + 2ρωn s + wn2
d
dt ,
s2 =
d2
dt2 ,
⇒ s2 y(t) + 2ρωn sy(t) + ωn2 y(t) = ωn u(t)
⇒ ÿ(t) + 2ρωn ẏ(t) + ωn2 y(t) = ωn u(t).
Ahora se realiza un cambio adecuado de variable. Sea x1 (t) = y(t), x2 (t) = ẋ1 (t) =
ẏ(t), entonces
ẋ2 (t) + 2ρωn x2 (t) + ωn2 x1 (t) = ωn u(t).
Por lo tanto, el sistema dinámico de la forma ẋ(t) = Ax(t) + Bu(t) está dado de la
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
130
siguiente forma:
Capı́tulo 2: Métodos numéricos
x1
0
1
0
d x1
u
=
+
2
dt x2
x2
−ωn −2ρωn
ωn
x
A
B
ẋ
x1
y = cT x = [ 1 0 ]
.
x2
El programa que implementa al sistema dinámico lineal de segundo orden se
encuentra descrito en el cuadro 2.15 que corresponde al archivo cap2 ejemplo27
(M-File). Para resolver el sistema dinámico por integración numérica, se emplea la
función ode45. Por facilidad, se emplea una entrada constante unitaria para estudiar
la respuesta del sistema para varios valores de amortiguamiento ρ y frecuencia de
resonancia ωn .
Para ejecutar correctamente la función cap2 ejemplo27 se requiere del programa
simuejemplo27 que se encuentra listado en el cuadro 2.15, donde se definen las
propiedades de integración usando la función odeset, las cuales son incorporadas en
la función ode45. El tiempo de simulación es de 0 a 10 segundos con incrementos de
una milésima de segundo.
La salida del programa 2.15 se presenta en la figura 2.11, observe que la respuesta
a una entrada constante de magnitud unitaria puede generar varios sobre impulsos,
esto depende del valor que tome ρ. Por ejemplo, para valores de 0 < ρ < 1 se le
conoce como sub-amortiguado, se caracteriza por presentar una etapa transitoria
muy abrupta, rápida y compuesta por varios sobre impulsos y oscilaciones. Cuando
ρ = 1 se le conoce como amortiguamiento crı́tico, disminuye considerablemente
las oscilaciones. Sobre amortiguado es para valores de ρ > 1 el cual no presenta
oscilaciones, es muy lento para llegar a la etapa transitoria.
La figura 2.11 fue obtenida variando los valores de ρ = 0.2, 0.4, 0.6, 0.8, 1, 2, para
generar la respuesta sub-amortiguada, amortiguamiento crı́tico y sobre amortiguada.
Se puede observar las oscilaciones muy pronunciadas que presenta en la etapa subamortiguada y a medida que ρ va aumentando su valor, entonces las oscilaciones
gradualmente van disminuyendo hasta desaparecer completamente para ρ > 1.
Cuando ρ = 0, el sistema oscila a la frecuencia natural de resonancia ωn .
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
2.5
Sistemas dinámicos de primer orden
Código Fuente 2.15
131
Sistema lineal de segundo orden
%MATLAB Aplicado a Robótica y Mecatrónica
%Capı́tulo 2 Métodos numéricos
%Editorial Alfaomega
%Fernando Reyes Cortés
%Archivo cap2 ejemplo27
%Sistema dinámico lineal de segundo orden
%Para su correcta ejecución requiere del
%programa cap2 simuejemplo27
%Entradas: t tiempo
% x vector de estados
% Salida xp=Ax+Bu
% la matriz A está formada por ρ factor
% de amortiguamiento y ωn la frecuencia natural
% de resonancia mecánica.
Sistema lineal de segundo orden
1
function xp =cap2 ejemplo27(t,x)
2
% frecuencia natural de resonancia
wn=1;
3
4
% factor de amortiguamiento
rho=1;
A=[ 0, 1;
-wn∧ 2, -2*rho*wn];
5
6
7
8
10
%matriz B
B=[0;
wn];
11
%señal de entrada
12
u=1;
13
%sistema dinámico lineal
xp=A*x+B*u;
9
14
15
end
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
132
Capı́tulo 2: Métodos numéricos
Código Fuente 2.16
Simulación del ejemplo 2.12
%MATLAB Aplicado a Robótica y Mecatrónica
%Capı́tulo 2 Métodos numéricos
%Editorial Alfaomega
%Fernando Reyes Cortés
%Archivo cap2 simuejemplo27
% Simulación: sistema dinámico lineal de primer orden
% requiere del archivo cap2 ejemplo27.m
Simulación del ejemplo 2.12
1
clc; clear all; close all;
2
% parámetros de simulación:
3
ti=0; tf = 10; h=0.001;
4
% intervalo de simulación
ts=ti:h:tf;
cond iniciales=[0;0];
5
6
7
10
opciones=odeset(’RelTol’,h, ’AbsTol’,1e-06,’InitialStep’,h,’MaxStep’,h);
disp(’Simulación de un sistema lineal de segundo orden’)
[t,x]=ode45(’cap2 ejemplo27’,ts,cond iniciales,opciones);
y=x(:,1); %salida del sistema
11
plot(t,y)
8
9
Figura 2.11 Respuesta a una entrada escalón.
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
2.6
Resumen
133
2.6 Resumen
M
étodos de integración y diferenciación numérica representan una herramienta poderosa de cómputo en la implementación práctica para análisis y estudios de esquemas de control en simulación y en evaluación práctica de sistemas
mecatrónicos y robots manipuladores.
Existen varios métodos prácticos para resolver una integral, entre los más
importantes se encuentran trapezoidal, Simpson, Euler recursivo, Runge-Kutta. Sin
embargo, dependiendo de la naturaleza del problema, la selección de un método
influye directamente en el desempeño, calidad y fidelidad del proceso de simulación
o experimentación.
Por ejemplo, el método recursivo de Euler es muy rápido y puede ser incluido en el
código del experimento para realizarse en tiempo real. Sin embargo, la exactitud con
que aproxima la integral es pobre en contraste con otros métodos como el trapezoidal
o el de Simpson, que pudieran ser mejores opciones para propósitos de simulación.
Para el caso de esquemas de control de robots manipuladores donde se involucre
alguna integral como el caso del algoritmo proporcional integral derivativo, el método
recursivo de Euler puede ser una buena opción a pesar de que la exactitud en la
aproximación de dicha integral no sea bueno. Esta caracterı́stica no es importante
debido a que el desempeño del algoritmo de control depende de la sintonı́a de las
ganancias y por lo tanto la pobre exactitud queda absorbida por la ganancia integral.
Para el estudio y análisis de la dinámica en sistemas mecatrónicos o robots
manipuladores es importante convertir mediante un adecuado cambio de variables
de estado (variables fase) el modelo dinámico a la forma de una ecuación diferencial
de primer orden ẋ = f (x).
La simulación de estos sistemas dinámicos se realiza por medio de la función ode45
(método de Runge-Kutta para orden cuarto y quinto), con pasos de integración de un
milisegundos. Las propiedades del método de integración, ası́ como las condiciones
iniciales del vector x(0) se configuran con la función odeset.
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
134
Capı́tulo 2: Métodos numéricos
Parte I referencias selectas
E
xiste una extensa literatura sobre los aspectos básicos de MATLAB,
ası́ como métodos numéricos. A continuación se presenta literatura selecta
correspondiente a la Parte I donde el lector puede profundizar sobre los tópicos
presentados.
Capı́tulo 1 Conceptos básicos
http://www.mathworks.com
Delores M. Etter. “Engineering problem solving with MATLAB”.
Second edition. Prentice-Hall. 1997.
Shoichiro Nakamura. “Análisis numérico y visualización gráfica
con MATLAB ”. Pearson Eduación. 1997.
Ashish Tewari. “Modern control design with MATLAB and Simulink”.
John Wiley & Sons, LTD. 2002.
The MathWorks. “Matehmatics: The language of technical computing”.
The MathWorks. 2005.
The MathWorks “Getting started with MATLAB 7 ”. The MathWorks. 2007.
David
Báez
López
&
Ofelia
Cervantes
Villagomez.
“ MatLab con aplicaciones a la ingenierı́a, fı́sica y finanzas”.
2da edición Alfaomega, diciembre 2011.
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
135
Parte I problemas propuestos
Capı́tulo 2 Métodos numéricos
Tom M. Apostol. ‘Calculus’. Reverté. 2da. ed., vols. I y II, 2006.
Norman B. Hasser, Joseph P. La Salle, Joseph A. Sullivan.
‘Análisis matemático’. Trillas, 1970.
Watson Fulks. ‘Cálculo avanzado’. Limusa. 1983.
Adrian Biran & Moshe Breiner. ‘MatLab for engineers’. Addison
Wesley, 1997.
William H Press, Saul A. Teukoisky, William T. Vetterling,
& Brian P. Flannery. ‘Numerical recipes in C++’. Cambridge
university Press. 2002.
Parte I problemas propuestos
E
n esta sección se presentan una serie de ejercicios con la finalidad que el lector
mejore sus conocimientos sobre métodos numéricos.
Capı́tulo 1 Conceptos básicos
1.1 Considere el programa 1.13, realizar los cambios necesarios en el código fuente
para encontrar los mı́nimos y máximos locales de la función.
1.2 Dado la siguiente secuencia de números:
datos = {1, 4, −3, 8, 12, 7, 2, 5, −12, 5, 7, 1, 0, 2, 22, 15, 4, 6, 2, −1, −3, 11}
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
136
Capı́tulo 2: Métodos numéricos
escribir un programa donde se use instrucciones if, for, while para realizar
lo siguiente:
(a) Ordenar los números de menor a mayor.
(b) Ordenar los números de mayor a menor.
(c) Encontrar el mı́nimo y máximo.
1.3 Calcular la norma euclidiana de los siguientes vectores:
⎡
1
⎤
⎢
⎥
⎢ −0.888 ⎥
⎢
⎥
⎥
x = ⎢
⎢ 4.35 ⎥
⎢
⎥
⎣ 3 ⎦
⎡
4
⎢ 6
⎢
y=⎢
⎣ 5.67
4
⎤
⎥
⎥
⎥
⎦
−0.90
⎡
0.001
⎤
⎢
⎥
4
⎢
⎥
⎢
⎥
⎢
⎥.
z=⎢
7
⎥
⎢
⎥
⎣ −12.333 ⎦
3
4
Para todos los vectores x ∈ IR5 , y ∈ IR4 , z ∈ IR5 , obtener la norma euclidiana
a través de los siguientes métodos.
(a) x =
(b) x =
(c) x =
√
xT x.
n
i=1 .
x21 + x22 + x23 + x24 + x25 .
(d) x =norma(x,2).
1.4 En referencia del ejercicio inmediato anterior considere los vectores x, y;
implementar la multiplicación y división entre las componentes de los vectores
a través operaciones con arreglos.
1.5 Obtener la gráfica de las siguientes funciones:
(a) f (x) = x2 , −10 < x < 10.
(b) f (x) = tan(x), −15 < x < 15.
(c) f (x) =
x
,
1+x2
−100 < x < 100.
(d) f (x) = [ 1 − e−0.01∗x ] senh(x), −3 < x < 3.
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
137
Parte I problemas propuestos
Capı́tulo 2 Métodos numéricos
2.1 Considere los siguientes sistemas de ecuaciones lineales:
⎤
⎡ ⎤
⎡
2
2x1 + 3.33x2 + 4.56x3
⎥
⎢ ⎥
⎢
⎣ 4 ⎦ = ⎣ 8x1 + 12.56x2 + 8.12x3 ⎦
9
−3x1 + 8.45x2 − 7.12x3
⎡
6
⎤
⎡
⎤
1.11x1 + 8.13x2 + 2.14x3 + 6.73x4 + 12.55x5
⎢
⎥
⎢
⎥
⎢ −4x1 + −8.34x2 + 4.67x3 + 4.10x4 + 2.48x5 ⎥
⎢ −3 ⎥
⎥
⎢
⎢
⎥
⎢ 10.44 ⎥ = ⎢ 1.2x1 + −5.6x2 + 2.12x3 + −3.24x4 + 12.55x5 ⎥ .
⎢
⎢
⎥
⎥
⎢
⎥
⎢
⎥
⎣ 2.35x1 + 4.5x2 + 5.89x3 + 0.78x4 + 0.34x5 ⎦
⎣ 6.12 ⎦
0
0.67x1 + 1.21x2 + −0.98x3 + 0.34x4 + 16.12x5
Resuelva el sistema de ecuaciones usando los siguientes métodos
(a) x = A−1 y.
(b) x=inv(A)y.
(c) x=A\y.
(d) x=linesolve(A,y).
(e) Regla de Cramer.
2.2 ¿Qué argumentos puede utilizar para obtener la derivada de la función |t|?
2.3 ¿Cómo puede obtener la derivada con respecto al tiempo de una función
discontinua como la función signo(t)?
2.4 Aproximar la derivada con respecto al tiempo por diferenciación numérica de
las siguientes funciones:
f1 (t) = t7
f2 (t) = |t|
f3 (t) = signo(t)
f4 (t) = tanh(t)
f5 (t) =
atan (t)
Comparar los resultados con el correspondiente método analı́tico.
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
138
Capı́tulo 2: Métodos numéricos
2.5 Calcular la integral de las siguientes funciones:
μ
I1 =
μ
x7 dx
I2 =
μi
μ
I3 =
signo(x)dx
I4 =
μi
μ
I5 =
|x|dx
μi
μ
tanh(x)dx
μi
atan (x) dx
μi
considere el intervalo de integración μ ∈ [−10, 10].
Realice el cálculo de la integral para cada función utilizando los siguientes
métodos:
(a) Trapezoidal.
(b) Simpson.
(c) Euler.
comparar y analizar los resultados.
2.6 Utilizar el programa del ejemplo 2.7 para calcular la integral de la siguientes
funciones
(a) e−x
(b) cos(x)
(c) x6
realizando un barrido del intervalo de integración por pasos de 0.001 para
x ∈ [−10, 10]. Comparar los resultados obtenidos con su respectivo método
analı́tico.
2.7 Realizar la simulación de los siguientes sistemas dinámicos (t ∈ [0, 10]):
(a)
ẋ =
(b)
ẋ =
Alfaomega
ẋ1
ẋ2
ẋ1
ẋ2
=
=
sen(x2 )
x31
atan (x1 + x2 )
cos(x2 x1 )
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
Parte II
Cinemática
La Parte II está dedicada al estudio de la cinemática directa de robots
manipuladores y sistemas mecatrónicos, y se componen de tres capı́tulos. El capı́tulo
3 incluye Preliminares matemáticos para apoyar la representación de matrices
homogéneas. El capı́tulo 4 Cinemática directa cubre los temas de cinemática
directa e inversa, cinemática diferencial y jacobiano de robots manipuladores,
ası́ como la metodologı́a Denavit-Hartenberg. El capı́tulo 5 Cinemática directa
cartesiana está destinado para analizar el modelo que relaciona las coordenadas
cartesianas con las coordenadas articulares para las principales configuraciones de
los robots industriales (sin tomar en cuenta la orientación de la herramienta de
trabajo).
Capı́tulo 3 Preliminares matemáticos
incluye
los temas de producto punto entre vectores, matrices ortogonales y sus
propiedades matemáticas, reglas de rotación, matrices de transformación
homogénea para traslación y rotación, ası́ como la presentación de varios
ejemplos ilustrativos y el desarrollo de librerı́as para MATLAB.
Capı́tulo 4 Cinemática directa
presenta los conceptos de
cinemática directa, cinemática inversa, cinemática diferencial y el jacobiano del
robot. También explica la metodologı́a de Denavit-Hartenberg la cual usa un
grupo de parámetros geométricos para determinar la matriz de transformación
homogénea que caracteriza al modelo cinemático de robots manipuladores
industriales.
Capı́tulo 5
Cinemática directa cartesiana
realiza un análisis detallado de la cinemática cartesiana de las principales
configuraciones de robots industriales. Un conjunto de librerı́as se desarrollan
para llevar a cabo simulación con robots manipuladores.
La parte II concluye con:
Referencias selectas
Problemas propuestos
Capı́tulo
3
Preliminares
matemáticos
i
Hi−1
=
3.1 Introducción
3.2 Producto interno
3.3 Matrices de rotación
3.4 Reglas de rotación
3.5 Transformaciones de traslación
3.6 Transformaciones homogéneas
3.7 Librerı́as para matrices homogéneas
3.8 Resumen
Rii−1
dii−1
0T
1
Objetivos
Presentar los preliminares matemáticos para la cinemática directa de
robots manipuladores y sistemas mecatrónicos, asimismo desarrollar librerı́as en lenguaje MATLAB (toolbox) para las matrices de transformación homogénea de rotación y traslación.
Objetivos particulares:
Producto interno de vectores.
Matrices de rotación y traslación.
Transformaciones homogéneas.
Librerı́as de matrices homogéneas para MATLAB.
3.1
Introducción
143
3.1 Introducción
E
l posicionamiento del extremo final del robot en el espacio tridimensional
(pose) requiere de 6 coordenadas: 3 coordenadas para la posición cartesiana
y 3 coordenadas para la orientación de la herramienta de trabajo. A la relación que
existe entre las coordenadas articulares del robot con las coordenadas cartesianas y
la orientación de la herramienta de trabajo colocada en el extremo final del robot se
le denomina cinemática directa.
Como parte de la representación matemática de la cinemática directa de robots
manipuladores se encuentra el uso de transformaciones homogéneas para representar
orientación y traslación de la herramienta de trabajo, con respecto al sistema de
referencia fijo ubicado generalmente en la base del robot.
Como preámbulo al tema de matrices de transformación homogénea están los
conceptos de producto punto o escalar entre vectores y matrices ortogonales.
El producto punto permite utilizar proyecciones de ortogonalidad de los ejes
principales de un sistema de referencia Σ1 (x1 , y1 , z1 ) relativo a otro sistema
de referencia Σ0 (x0 , y0 , z0 ). Otro de los temas de prerrequisito es el de las
matrices ortogonales para modelar la orientación y traslación de la herramienta
de trabajo respecto al sistema fijo del robot Σ0 (x0 , y0 , z0 ). Resaltan las propiedades
matemáticas de las matrices ortogonales que facilitan el análisis y descripción de
movimientos de rotación y traslación. En función de las propiedades matemáticas
de las matrices ortogonales se generan diferentes reglas de rotación.
Las matrices homogéneas incluyen estos conceptos para ofrecer una representación
compacta de la matriz de rotación, que determina la orientación relativa de un
sistema de referencia Σ1 (x1 , y1 , z1 ) con respecto a un sistema de referencia fijo
Σ0 (x0 , y0 , z0 ) y del vector de coordenadas o de traslación x0 = [ x0 , y0 , z0 ]T .
Con la estructura matemática de la transformación homogénea se desarrolla un
conjunto de librerı́as para propósitos de simulación de cinemática directa de robots
manipuladores y sistemas mecatrónicos.
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
144
Capı́tulo 3: Preliminares matemáticos
3.2 Producto interno
P
ara modelar la orientación de la herramienta de trabajo del robot se encuentra
el producto escalar o producto interno, también conocido como producto punto
(dot product), el cual permite utilizar los conceptos de la geometrı́a euclidiana
tradicionales como longitudes, ángulos, proyecciones geométricas, ortogonalidad en
dos y tres dimensiones de los sistemas de referencia asociados al robot y de la
herramienta de trabajo.
El término espacio euclidiano se denomina ası́ en honor del matemático y filósofo
griego Euclides, y se utiliza para distinguirlo de otro tipo de espacios como los
espacios curvos de la geometrı́a no euclidiana y la teorı́a de la relatividad de Einstein.
El espacio euclidiano es un tipo de espacio geométrico donde se satisfacen los axiomas
de la geometrı́a de Euclides. Como casos particulares del espacio euclidiano se
encuentran la recta real, el plano y el espacio tridimensional, que corresponden a las
dimensiones 1, 2 y 3, respectivamente.
El concepto abstracto de espacio euclidiano geométrico se generaliza para el caso
general de espacio euclidiano n-dimensional, el cual también se considera como un
espacio vectorial n-dimensional real.
Al producto interno x · y es una operación definida sobre dos vectores x, y ∈ IRn de
un espacio euclidiano cuyo resultado es un número o escalar (los espacios vectoriales
que incluyen al producto interno reciben el nombre de espacios prehilbertianos).
Considere los siguientes vectores x, y ∈ IRn , el producto interno vectorial se define
como:
x · y = xT y =
n
xi yi = x1 y1 + x2 y2 + · · · + xn yn .
(3.1)
i=1
El producto punto se le llama producto escalar debido a que también se puede
realizar como: x · y = xT y.
En un espacio euclidiano real, el producto interno (3.1) también acepta una definición
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
3.2
145
Producto interno
geométrica dada por:
x·y = x
y cos(θ)
(3.2)
donde θ es el ángulo que forman los vectores x y y (ver figura 3.1), y las
normas euclidianas se encuentran definidas por x = x21 + x22 + · · · + x2n , y =
y12 + y22 + · · · + yn2 , respectivamente.
Geométricamente y cos(θ) representa la proyección del vector y sobre la dirección
del vector x, como se presenta en la figura 3.1. El concepto de proyección geométrica
se usa particularmente en las matrices rotacionales para modelar la orientación del
sistema de referencia de la herramienta de trabajo del robot.
Figura 3.1 Producto interno vectorial x · y.
Considere x, y, z ∈ IRn , y α ∈ IR; el producto interno tiene las siguientes
propiedades:
Conmutativa: x · y = y · x.
Distributiva: z · (x + y) = z · x + z · y.
Asociativa: αx · y = x · αy = x · yα.
La expresión geométrica del producto escalar permite calcular el coseno del
ángulo θ existente entre los vectores x y y de la siguiente manera:
cos(θ) =
x·y
x1 y1 + x2 y2 + · · · + xn yn
=
.
2
x y
x1 + x22 + · · · + x2n y12 + y22 + · · · + yn2
Vectores ortogonales: x · y = 0 ⇒ x ⊥ y, θ =
π
2
rad (90 grados).
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
146
Capı́tulo 3: Preliminares matemáticos
Vectores paralelos o con la misma dirección si el ángulo que forman es 0 rad
(0 grados) o π rad (180 grados): x · y = x y .
x · x = 0 ⇐⇒ x = 0 ∈ IRn .
Si x = 0 ⇒ x · x > 0.
La norma
euclidiana de un vector x se puede expresar como: x =
√
n
2
xT x =
x21 + x22 + · · · + x2n .
i=1 xi =
√
x·x=
MATLAB contiene la función dot(x,y) para realizar la operación producto interno
x · y de los vectores x, y ∈ IRn , con la siguiente estructura de sintaxis:
x·y =
dot(x,y)
x · y = dot(x,y,n)
x · y=dot(x,y,n) retorna el producto escalar de los vectores x, y con dimensión n.
Nota: el producto punto x · y equivale a realizar la operación en MATLAB:
x · y=x’*y.
La norma euclidiana x de un vector x ∈ IRn , también conocida como norma 2, se
puede calcular utilizando la función norm con la siguiente sintaxis:
x =norm(x,2)
♣ Ejemplo 3.1
Sean x, y ∈ IR2 con las siguientes componentes respectivamente:
2
8
x =
y=
4
−3
obtener el producto punto x · y y el ángulo θ que forman entre los vectores x, y.
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
3.2
147
Producto interno
Solución
Puesto que los vectores x, y ∈ IR2 están formados por x = [ 2 4 ]T y y = [ 8 −3 ]T ,
y tomando en cuenta la definición del producto punto se obtiene lo siguiente:
x · y = x1 y1 + x2 y2 = 2 (8) − 4 (3) = 16 − 12 = 4.
El ángulo que forman los vectores x y y se obtiene como:
cos(θ) =
=
x·y
x1 y1 + x2 y2
=
2
x y
x1 + x22 y12 + y22
4
4
= 0.1046
√
=
2
2
2
2
(4.4721)(8.544)
2 + 4 8 + (−3)
lo que significa que θ =1.465 rad (83.99 grados).
Observe que evidentemente se cumple:
√ √
x · y = x y cos(θ) = 20 73 cos(1.465) = (4.472135)(8.544003)(0.1046) = 4.
El cuadro 3.1 presenta el código fuente para realizar el desarrollo del ejemplo 3.1.
Código Fuente 3.1
Producto interno o escalar
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 3 Cinemática %Programa cap3 prodint.m
Producto interno o escalar
1
2
3
4
5
6
clc; clear all; close all;
disp(’Producto interno o escalar de vectores’)
x = [2; 4 ]; %vector columna de dos renglones x ∈ IR2
y = [8; -3]; %vector columna de dos renglones y ∈ IR2
xdoty=dot(x,y) %producto interno x · y usando la función dot
xdoty m=x’*y %forma matemática del producto interno x · y
7
theta=acos(x’*y/(norm(x,2)*norm(y,2))) %ángulo que forman los vectores
x, y ∈ IR2
8
%forma geométrica del producto interno: x · y = x
xdoty geometrica=norm(x,2)*norm(y,2)*cos(theta)
9
y cos(θ)
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
148
Capı́tulo 3: Preliminares matemáticos
3.3 Matrices de rotación
L
a figura 3.2 muestra dos sistemas de referencia cartesianos, asociados a
un cuerpo rı́gido, representados por Σ0 ( x0 , y0 , z0 ) y Σ1 ( x1 , y1 , z1 ), ambos
sistemas comparten el mismo origen. El sistema de referencia Σ1 ( x1 , y1 , z1 )
mantiene una orientación relativa al sistema de referencia fijo Σ0 ( x0 , y0 , z0 ).
Figura 3.2 Sistemas de referencia fijo Σ0 y rotado Σ1 .
Considérese un punto p sobre el cuerpo rı́gido, con respecto al sistema Σ0 ( x0 y0 z0 )
tiene coordenadas p0 = [x0 , y0 , z0 ]T , el mismo punto p se representa como p1 =
[x1 , y1 , z1 ]T con respecto al sistema de referencia Σ1 ( x1 , y1 , z1 ). El problema que
se plantea es encontrar la relación que hay entre las coordenadas de un punto
p1 en el sistema de referencia Σ1 ( x1 , y1 , z1 ) con el vector p0 definido en el
sistema de referencia Σ0 ( x0 , y0 , z0 ). Considere vectores bases para cada sistema
de referencia Σ0 y Σ1 . Sean {i0 , j 0 , k0 } vectores unitarios a lo largo de los
ejes x0 , y0 , z0 , respectivamente. Es decir, i0 = [ 1, 0, 0 ]T , j 0 = [ 0, 1, 0 ]T , k0 =
[ 0, 0, 1 ]T . Similarmente se definen los vectores unitarios {i1 , j 1 , k1 } para el sistema
Σ1 ( x1 , y1 , z1 ).
Un vector que va desde el origen común para ambos sistemas hasta el punto p, puede
ser expresado en función de cualquiera de las dos bases de vectores unitarios de la
siguiente forma:
p0 = p0xi0 + p0y j 0 + p0z k0
Alfaomega
con respecto al sistema Σ0
(3.3)
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
3.3
149
Matrices de rotación
p1 = p1xi1 + p1y j 1 + p1z k1
con respecto al sistema Σ1
(3.4)
Los vectores p0 , p1 representan al mismo punto p. Tomando en cuenta las ecuaciones
(3.3) y (3.4) la relación que hay entre sus componentes adquiere la siguiente forma:
p0x = p0 · i0 = p1 · i0
= p1xi1 · i0 + p1y j 1 · i0 + p1z k1 · i0
(3.5)
p0y = p0 · j 0 = p1 · j 0
= p1xi1 · j 0 + p1y j 1 · j 0 + p1z k1 · j 0
(3.6)
p0z = p0 · k0 = p1 · k 0
= p1xi1 · k 0 + p1y j 1 · k0 + p1z k1 · k0 .
(3.7)
Estas ecuaciones pueden ser escritas de manera compacta como:
p0 = R10 p1
(3.8)
donde R10 representa la siguiente matriz
⎡
i1 · i0
⎢
1
R0 = ⎣ i1 · j 0
j1 · j0
⎥
k1 · j 0 ⎦ .
i1 · k 0
j 1 · k0
k1 · k0
j 1 · i0
k1 · i0
⎤
(3.9)
La matriz R10 ∈ IR3×3 es la matriz de transformación de las coordenadas del
punto p del sistema de referencia Σ1 ( x1 , y1 , z1 ) hacia las coordenadas del sistema
Σ0 ( x0 , y0 , z0 ). En otras palabras, dado un punto p1 en el sistema Σ1 ( x1 , y1 , z1 ),
entonces R10 p1 representa el mismo vector expresado con respecto al sistema de
referencia Σ0 ( x0 , y0 , z0 ).
Obsérvese que las columnas de R10 son los cosenos directores de los ejes coordenados
x1 , y1 , z1 respecto de los ejes coordenados x0 , y0 , z0 . Por ejemplo, la primera columna
[i1 · i0 , i1 · j0 , i1 · k0 ]T especifica la dirección del eje x1 relativa al sistema de referencia
Σ0 .
Similarmente se puede obtener el punto p1 en función del punto p0 :
p1x = p1 · i1 = p0 · i1
= p0x i0 · i1 + p0y j 0 · i1 + p0z k 0 · i1
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
150
Capı́tulo 3: Preliminares matemáticos
p1y = p1 · j 1 = p0 · j 1
= p0x i1 · j 1 + p0y j 1 · j 1 + p0z k1 · j 1
p1z = p1 · k 1 = p0 · k1
= p0x i1 · k1 + p0y j 1 · k 1 + p0z k 1 · k1
p1 = R01 p0
⎡
i0 · i1
⎢
0
R1 = ⎣ i0 · j 1
j0 · j 1
⎥
k0 · j 1 ⎦ .
i0 · k 1
j 0 · k1
k0 · k1
j 0 · i1
k0 · i1
⎤
(3.10)
(3.11)
La matriz R01 ∈ IR3×3 (3.11) representa la matriz inversa de la transformación R10
(3.9). Debido a que el producto interno de vectores unitarios cumple la propiedad
conmutativa, entonces i1 · j 1 = j 1 · i1 , i0 · j 0 = j 0 · i0 , etc., por lo que resulta:
−1 1 T
R01 = R10
= R0
(3.12)
La matriz R10 cuya inversa es su transpuesta se denomina matriz ortogonal. La
norma de los vectores columna de R10 son de magnitud unitaria y mutuamente
ortogonales, el determinante de R10 es ±1. Si el sistema de referencia se selecciona
de acuerdo con la regla de la mano derecha, entonces el determinante de R10 es 1.
La matriz R10 se denomina matriz de rotación y pertenece a la clase de matrices
ortogonales que se denotan como SO(3).
Notación
La matriz R10 representa la orientación del sistema Σ1 ( x1 , y1 , z1 ) respecto al sistema
Σ0 ( x0 , y0 , z0 ). R21 representa la orientación del sistema Σ2 ( x2 , y2 , z2 ) respecto al
sistema Σ1 ( x1 , y1 , z1 ), y ası́ sucesivamente. Mientras que la transformación inversa
R01 significa la orientación del sistema de referencia Σ0 ( x0 , y0 , z0 ) relativa al sistema
de referencia Σ1 ( x1 , y1 , z1 ). De manera análoga, R12 es la transformación inversa de
coordenadas del sistema de referencia Σ1 ( x1 , y1 , z1 ) hacia el sistema Σ2 ( x2 , y2 , z2 ).
Hay varios métodos para expresar la orientación del sistema de referencia
Σ1 (x1 , y1 , z1 ) con respecto al sistema de referencia fijo Σ0 (x0 , y0 , z0 ), entre los más
usuales se encuentra la rotación con respecto a uno de los ejes principales x0 , y0 , y0 .
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
3.3
151
Matrices de rotación
A continuación se tratan los casos de rotación alrededor de un eje principal.
3.3.1
3.3.1. Matriz de rotación alrededor del eje z0
Considere que el sistema de referencia Σ1 ( z1 , y1 , z1 ) el cual se encuentra rotado un
ángulo θ alrededor del eje z0 del sistema Σ0 ( z0 , y0 , z0 ). Obtener la matriz resultante
de transformación R10 .
Como se muestra en la figura 3.3, los ejes z0 y z1 son paralelos. El signo del ángulo
θ está dado por la regla de la mano derecha. Por convención, un ángulo positivo es
aquel cuyo sentido de rotación es contrario al movimiento de las manecillas del reloj.
Figura 3.3 Rotación de un ángulo θ alrededor del eje z.
De la figura 3.3 se obtienen las siguientes ecuaciones:
i1 · i0 = cos(θ)
j 1 · i0 = − sen(θ)
j 1 · j 0 = cos(θ)
i1 · j 0 = sen(θ)
k0 · k1 = 1
todos los demás productos punto son cero, puesto que el ángulo que existe entre los
vectores unitarios i0 , i1 , j 0 , y j 1 con k0 y k 1 es de 90 grados, excepto entre ellos
mismos, puesto que k0 y k1 forman un ángulo de 0 grados: k1 · i0 = cos( π2 ) =
0, k1 · j 0 = cos( π2 ), i1 · k0 = cos( π2 ) = 0, y j 1 · k 0 = cos( π2 ) = 0.
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
152
Capı́tulo 3: Preliminares matemáticos
Notación
La matriz (3.13) se conoce como matriz de rotación alrededor del eje z y es
representada por Rz (θ).
⎡
− sen(θ) 0
cos(θ)
⎢
Rz (θ) = ⎣ sen(θ)
0
⎤
⎥
0⎦.
cos(θ)
0
(3.13)
1
La matriz de rotación Rz (θ) se interpreta como una matriz que especifica la
orientación del sistema de referencia Σ1 ( x1 , y1 , z1 ) relativo al sistema de referencia
Σ0 ( x0 , y0 , z0 ).
⎡
⎤
cos(θ) − sen(θ) 0
⎢
⎥
p0 = Rz (θ)p1 = ⎣ sen(θ)
cos(θ)
0 ⎦ p1 .
(3.14)
0
0
1
Por convención considérese que el ángulo θ es positivo en el sentido contrario al
movimiento de las manecillas del reloj.
La relación inversa que determina la orientación del sistema Σ0 ( x0 , y0 , z0 ) con
respecto al sistema Σ1 ( x1 , y1 , z1 ) está dada por:
⎡
⎤
cos(θ)
sen(θ) 0
⎢
⎥
(3.15)
p1 = RTz ( θ )p0 = ⎣ − sen(θ) cos(θ) 0 ⎦ p0 .
0
0
1
Por lo tanto, un punto p0 en el sistema Σ0 ( x0 , y0 , z0 ) es transformado hacia un
punto p1 en el sistema Σ1 ( x1 , y1 , z1 ) incluyendo su orientación relativa.
Otra forma de obtener la matriz Rz ( θ ) que relaciona la orientación θ del sistema
de referencia Σ1 ( z1 , y1 , z1 ) con respecto al eje z0 del sistema de referencia
Σ0 ( z0 , y0 , z0 ) es por medio de una proyección geométrica, es decir, analizando la
proyección de los ejes x1 , y1 sobre los ejes x0 , y0 como se ve en la figura 3.4.
Los ejes z1 y z0 son paralelos entre sı́, el plano x1 − y1 se encuentra rotado un ángulo
θ con respecto al plano x0 − y0 , entonces la proyección del punto p1 = [p1x, p1y , p1z ]T
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
3.3
153
Matrices de rotación
Figura 3.4 Rotación θ grados del plano x1 − y1 con respecto al plano x0 − y0 .
sobre los ejes del sistema Σ0 son:
p0x = p1x cos(θ) − p1y sen(θ)
p0y = p1x sen(θ) + p1y cos(θ)
p0z = p1z
⎡
cos(θ)
⎢
p0 = Rz (θ)p1 = ⎣ sen(θ)
− sen(θ)
cos(θ)
0
0
0
⎤
⎥
0 ⎦ p1 .
1
Propiedades de la matriz de rotación Rz (θ)
La matriz de rotación Rz (θ) tiene varias propiedades importantes que a continuación
se presentan:
Rz ( 0 ) = I, I ∈ IR3×3 es la matriz identidad.
Rz ( θ )Rz ( β ) = Rz ( β )Rz ( θ ) = Rz ( θ + β ) = Rz ( β + θ ).
Rz ( θ )−1 = Rz ( −θ ).
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
154
Capı́tulo 3: Preliminares matemáticos
Rz ( θ )T = Rz ( θ )−1 .
Rz ( θ )Rz ( θ )T = Rz ( θ )T Rz ( θ ) = I.
det[Rz ( θ )] = 1 si el sistema de referencia cartesiano Σ ( z, y, z ) es seleccionado
por la regla de la mano derecha, en otro caso det[Rz ( θ )] = −1.
Función matriz de rotación Rz ( θ )
La función matriz de rotación Rz ( θ )
⎡
cos(θ)
⎢
Rz ( θ ) = ⎣ sen(θ)
0
− sen(θ) 0
cos(θ)
0
⎤
⎥
0⎦
1
tiene la siguiente sintaxis:
R=Rz(θ)
donde θ ∈ IR es el ángulo de rotación alrededor del eje z, y representa el argumento
de entrada. Retorna la matriz de rotación R.
El cuadro 3.2 contiene el programa en código fuente para evaluar en forma simbólica
las propiedades matemáticas de la matriz de rotación Rz ( θ ). Para simplificar el
álgebra simbólica es recomendable utilizar la función simplify.
Para expresiones matemáticas con variables simbólicas, la función simplify
resulta importante para obtener un resultado matemático compacto, es decir
factorizado y reducido.
Cuando un programa se encuentra combinando cálculos numéricos con variables
simbólicas, se recomienda usar la función vpa con una precisión de tres dı́gitos a
través de digits(3).
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
3.3
155
Matrices de rotación
Código Fuente 3.2
Función Rz (θ)
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 3 Cinemática %Archivo Rz.m
Función Rz (θ)
1
function R=Rz(theta)
2
dato=whos(’theta’);
if strcmp(dato.class, ’sym’) %para variables simbólicas
R=simplify([cos(theta), -sin(theta), 0;
sin(theta), cos(theta), 0;
3
4
5
6
0, 0, 1]);
else %cálculos numéricos
digits(3);
7
8
9
R=simplify([ double(cos(theta)), double(-sin(theta)), 0;
double(sin(theta)), double(cos(theta)), 0;
0, 0, 1]);
10
11
12
13
end
end
♣ ♣ Ejemplo 3.2
Escribir un programa en MATLAB para comprobar las propiedades de la matriz
de rotación Rz (θ)
Solución
En el cuadro 3.5 se presenta el programa en código fuente para MATLAB de las
propiedades de la matriz de rotación Rz ( θ ).
Para que el resultado simbólico sea compacto o simplificado se emplea la función
simplify.
Para visualizar los resultados de las propiedades matemáticas de la matriz Rz ( θ ) es
necesario no insertar en cada lı́nea el operador ;).
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
156
Capı́tulo 3: Preliminares matemáticos
Cuando se trabaja únicamente con variables simbólicas es importante
utilizar la función simplify para obtener resultados algebraicos compactos.
Cuando hay una combinación entre variables simbólicas y cálculos numéricos
es recomendable usar la función double como en el cuadro del código 3.2
de la matriz Rz ( θ ). Otras opciones que se pueden usar son la función vpa
y digits(3) (ver el código 3.8).
Código Fuente 3.5
Propiedades de la matriz de rotación Rz (θ)
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 3 Cinemática %Archivo cap3 propiedadesRz.m
Propiedades de la matriz de rotación Rz (θ)
1
clc;
2
4
clear all;
close all;
syms a b real
5
%resultados simbólicos
6
simplify(Rz(a)*Rz(b)) %Rz ( a )Rz ( b )
simplify(Rz(b)*Rz(a)) %Rz ( b )Rz ( a )
−1
simplify(inv(Rz(a))) %Rz ( a )
3
7
8
−1
12
simplify(Rz(-a)) %Rz ( a ) = Rz ( −a )
T
−1
simplify(Rz(a)’) %Rz ( a ) = Rz ( a )
T
simplify(Rz(a)’*Rz(a)) %Rz ( a ) Rz ( a ) = I
T
simplify(Rz(a)*Rz(a)’) %Rz ( a )Rz ( a ) = I
13
simplify(det(Rz(a))) %det[Rz ( a )]=1
14
15
%cálculo numérico
theta=90*pi/180; θ =
16
Rz(theta) %Rz ( θ )
9
10
11
Alfaomega
π
2
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
3.3
Matrices de rotación
157
♣ Ejemplo 3.3
Escribir un algoritmo en lenguaje MATLAB que realice la rotación de 90 grados
alrededor del eje z0 de una imagen de prueba (flecha).
Solución
El programa 3.4 contiene el código para realizar la rotación de la imagen de prueba.
El vector p0 = [ px0 , py0 , pz0 ]T tiene las coordenadas de la imagen de prueba definidas
en el sistema de referencia Σ0 (x0 , y0 , z0 ). El ángulo de rotación θ es de 90 grados con
respecto al eje z0 . El sistema Σ1 (x1 , y1 , z1 ) tiene la imagen rotada con respecto
al sistema Σ0 (x0 , y0 , z0 ). La matriz de rotación Rz0 ( π2 ) relaciona la orientación
que tiene la imagen rotada definida en Σ1 (x1 , y1 , z1 ) en relación al sistema fijo
Σ0 (x0 , y0 , z0 ).
En la figura 3.5 se muestra el resultado del programa en ambos sistemas de referencia
Σ0 (x0 , y0 , z0 ) y el sistema de referencia rotado Σ1 (x1 , y1 , z1 ).
Figura 3.5 Rotación de 90 grados alrededor del eje z0 .
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
158
Capı́tulo 3: Preliminares matemáticos
Código Fuente 3.4
Rotación alrededor del eje z0
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 3 Cinemática %Archivo cap3 rotacionRz.m
Rotación alrededor del eje z0
1
clc;
2
clear all;
close all;
3
4
5
6
7
8
9
⎡
⎤
px0
⎢
⎥
%coordenadas del vector p0 = ⎣ py0 ⎦
px0
pxo=[0,0,0,0,0,6,7,7, 8, 8, 9,9, 10,10, 10, 10, 10, 10, 10, 10, 10, 10, 11, 12, 13, 14,14,
14, 14, 14, 14, 14, 14,14, 14, 15, 15, 16,16, 17,17,18,19,20,21];
pyo=[0,0,0,0,0,9,9,10,9, 11, 9,12, 1, 2, 3, 4, 5, 6,7, 8, 9, 12, 12, 12, 12,1, 2, 3, 4, 5,
6, 7, 8, 9, 12, 9, 12, 9, 11, 9, 10, 9,0,0,0 ];
pzo=[0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0];
theta=90*3.1416/180.0;
12
%matriz de rotación Rz ( θ )
R ztheta=[ cos(theta), -sin(theta), 0;
sin(theta), cos(theta), 0;
0, 0,1];
13
%conversión de coordenadas del sistema Σ0 (x0 , y0 , z0 ) al sistema Σ1 (x1 , y1 , z1 )
14
% p1 = Rz ( θ )p0
Sigma1=R ztheta*[pxo; pyo; pzo];
⎡
⎤
px1
⎢
⎥
% p1 = ⎣ py1 ⎦
10
11
15
16
19
pz1
px1=Sigma1(1,:);
py1=Sigma1(2,:);
pz1=Sigma1(3,:);
20
%puntos graficados en los sistemas p0 ∈ Σ0 (x0 , y0 , z0 ) y p1 ∈ Σ1 (x1 , y1 , z1 )
21
plot3(pxo,pyo,pzo,’.’,px1,py1,pz1,’x’)
17
18
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
3.3
159
Matrices de rotación
♣ Ejemplo 3.4
Considere un punto p1 = [p1x, p1y , p1z ]T = [0.8, 0.5, 1]T en el sistema de referencia
Σ1 ( z1 , y1 , z1 ), el cual mantiene una orientación relativa de 90 grados alrededor
del eje z0 del sistema fijo Σ0 ( z0 , y0 , z0 ). Obtener la proyección del punto p1 en el
sistema fijo Σ0 .
Solución
El punto p1 = [p1x, p1y , p1z ]T = [0.8, 0.5, 1]T se encuentra en el sistema de referencia
Σ1 ( z1 , y1 , z1 ), la conversión de coordenadas al sistema de referencia fijo se realiza a
través de: p0 = R(z, θ)p1 , obteniendo lo siguiente:
⎡
⎤⎡
⎤
cos( π2 ) − sen( π2 ) 0
0.8
⎢
⎥⎢
⎥
p0 = ⎣ sen( π2 )
cos( π2 )
0 ⎦ ⎣ 0.5 ⎦
0
0
1
1
⎡
⎤
−0.5
⎢
⎥
= ⎣ 0.8 ⎦ .
1
Nótese que el punto p0 en el sistema
convertir al punto p1 en el sistema de
de la siguiente relación:
⎡
cos( π2 )
⎢
p1 = ⎣ − sen( π2 )
⎡
0
⎤
de referencia fijo Σ0 ( z0 , y0 , z0 ) se puede
referencia rotado Σ1 ( z1 , y1 , z1 ), a través
sen( π2 ) 0
cos( π2 )
0
⎤⎡
−0.5
⎤
⎥⎢
⎥
0 ⎦ ⎣ 0.8 ⎦
1
1
0.8
⎢
⎥
= ⎣ 0.5 ⎦ .
1
La figura 3.6 muestra la descripción geométrica del punto p1 y su proyección sobre
el sistema de referencia Σ0 ( z0 , y0 , z0 ). Observe que los ejes z0 y z1 se mantienen
paralelos entre sı́.
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
160
Capı́tulo 3: Preliminares matemáticos
Figura 3.6 Rotación relativa de 90 grados del sistema Σ1 alrededor del eje z0 .
♣ Ejemplo 3.5
Considere un paralelepı́pedo rectangular definido en el sistema de referencia
Σ0 ( z0 , y0 , z0 ); rotar el poliedro 180 grados alrededor del eje z0 . Describir la
proyección del sistema de referencia rotado Σ1 ( z1 , y1 , z1 ) en el sistema de
referencia fijo.
Solución
Sea p0 = [p0x, p0y , p0z ]T un punto sobre el paralelepı́pedo rectangular, al rotar este
punto p0 180 grados con respecto al eje z0 se mueve rı́gidamente junto con todo el
sólido quedando en el punto p1 = [p1x, p1y , p1z ]T .
Todos los puntos que pertenecen al poliedro están sujetos a la siguiente
transformación: p1 = Rz ( π )p0 , es decir los puntos p0 definidos en el sistema
Σ0 (x0 , y0 , z0 ) son transformados a coordenadas p1 en el sistema Σ1 (x1 , y1 , z1 ) por
medio de la matriz de rotación Rz ( π ). La interpretación geométrica de la rotación
del poliedro 90 grados alrededor del eje z0 se presenta en la figura 3.7.
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
3.3
161
Matrices de rotación
Figura 3.7 Rotación de 180 grados alrededor del eje z0 de un paralelepı́pedo
rectangular.
3.3.2
3.3.2. Matriz de rotación alrededor del eje x0
Considere el sistema de referencia Σ1 ( z1 , y1 , z1 ) el cual está rotado un ángulo θ
alrededor del eje x0 del sistema Σ0 ( z0 , y0 , z0 ); obtener la matriz resultante de
rotación.
En la figura 3.8 se muestra la rotación del sistema Σ1 ( z1 , y1 , z1 ) con respecto al eje
x0 del sistema fijo Σ0 ( z0 , y0 , z0 ); el ángulo de rotación θ gira alrededor del eje x0
en sentido positivo (contrario a las manecillas del reloj).
En este caso el ángulo de rotación que existe entre los ejes x0 y x1 es cero,
puesto que son ejes paralelos. Para la primera columna [ i1 · i0 i1 · j 0 i1 · k0 ]T
de la matriz de rotación R10 (θ) ecuación (3.9) tiene las siguientes componentes:
i1 · i0 = cos(0) = 1, i1 · j 0 = cos( π2 ) = 0; i1 · k0 = cos( π2 ) = 0. La segunda
columna [ j 1 · i0 j 1 · j 0 j 1 · k0 ]T adquiere la siguiente forma: j 1 · i0 = cos( π2 ) = 0,
j 1 · j 0 = cos(θ) y j 1 · k0 = cos(θ − π2 ) = sen(θ).
Finalmente, la tercera columna [ k1 · i0
k1 · j 0
k 1 · k0 ]T se encuentra definida
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
162
Capı́tulo 3: Preliminares matemáticos
Figura 3.8 Rotación de un ángulo θ alrededor del eje x0 .
como: k 1 · i0 = cos( π2 ) = 0, k1 · j 0 = cos(θ + π2 ) = − sen(θ) y k1 · k 0 = cos(θ).
Por lo tanto, la matriz correspondiente de rotación está dada por:
⎡
⎤
1
0
0
⎢
⎥
R10 (θ) = ⎣ 0 cos(θ) − sen(θ) ⎦ .
0 sen(θ)
cos(θ)
(3.16)
Observe que los ejes x0 y x1 coinciden entre sı́, y el plano z1 − y1 se desplaza un
ángulo θ de derecha a izquierda con respecto al plano x0 − y0 .
Notación
La matriz (3.16) cuyo ángulo de rotación θ se realiza alrededor del eje x se denota
por Rx ( θ ) y su estructura matemática es:
⎡
1
0
⎢
Rx( θ ) = ⎣ 0 cos(θ)
0
sen(θ)
0
⎤
⎥
− sen(θ) ⎦ .
(3.17)
cos(θ)
Las propiedades de la matriz de rotación Rx ( θ ) son las mismas para Rz ( θ ).
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
3.3
163
Matrices de rotación
3.3.3
3.3.3. Matriz de rotación alrededor del eje y0
Considere los sistemas de referencia Σ1 ( z1 , y1 , z1 ) y Σ0 ( z0 , y0 , z0 ), el sistema
Σ1 ( z1 , y1 , z1 ) se encuentra rotado un ángulo θ alrededor del eje y0 del sistema fijo
Σ0 ( z0 , y0 , z0 ). Obtener la matriz resultante de rotación.
De la figura 3.9 se puede obtener la matriz que relaciona la orientación relativa
del sistema de referencia Σ1 ( z1 , y1 , z1 ) con respecto al sistema de referencia fijo
Σ0 ( z0 , y0 , z0 ). El ángulo de rotación θ es alrededor del eje y0 .
La matriz R10 (θ) adquiere la siguiente expresión:
⎡
⎢
R10 (θ) = ⎣
cos(θ)
0
sen(θ)
0
1
0
− sen(θ) 0
⎤
⎥
⎦
(3.18)
cos(θ)
Figura 3.9 Rotación de un ángulo θ del sistema Σ1 alrededor del eje y0 del sistema
Σ0 .
Observe que la rotación del ángulo θ alrededor del eje y0 mueve el plano z1 − x1 .
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
164
Capı́tulo 3: Preliminares matemáticos
Notación
La matriz de rotación (3.18) describe la orientación relativa del sistema
Σ1 ( z1 , y1 , z1 ) con respecto al sistema de referencia fijo Σ0 ( z0 , y0 , z0 ) usando una
rotación alrededor del eje y0 , la cual se denota por:
⎡
⎤
cos(θ)
0 sen(θ)
⎢
⎥
Ry ( θ ) = ⎣
0
1
0 ⎦
− sen(θ) 0 cos(θ)
(3.19)
Las propiedades de Ry ( θ ) son las mismas que Rz ( θ ).
3.4 Reglas de rotación
G
eneralmente la descripción de la orientación del sistema de referencia de la
herramienta de trabajo del robot colocada en el extremo final del robot con
respecto al sistema de referencia fijo en la base del robot involucra varias rotaciones
sucesivas (composición de rotaciones). Existen varios métodos que permiten definir
el orden o reglas de las rotaciones consecutivas.
Composición de rotaciones para sistemas de referencia sucesivos
Considere tres sistemas de referencia cartesianos definidos por Σ0 ( x0 , y0 , z0 )
(sistema fijo), Σ1 ( x1 , y1 , z1 ) tiene una rotación relativa al sistema fijo y sea el
sistema de referencia Σ2 ( x2 , y2 , z2 ) cuya orientación relativa es con respecto al
sistema de referencia Σ1 ( x1 , y1 , z1 ). Todos los sistemas de referencia cartesianos
comparten el mismo origen como se presenta en la figura 3.10.
El problema que se plantea es encontrar la expresión matemática que relaciona la
orientación del sistema de referencia Σ2 ( x2 , y2 , z2 ) relativo al sistema de referencia
fijo Σ0 ( x0 , y0 , z0 ).
Puesto que los tres sistemas de referencia tienen el mismo origen, entonces un punto
p puede ser representado en función de las coordenadas p0 , p1 , p2 de la siguiente
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
3.4
165
Reglas de rotación
Figura 3.10 Composición de rotaciones.
forma:
p0 = R10 p1
(3.20)
p1 = R21 p2
(3.21)
donde la matriz R10 representa la orientación del sistema de referencia Σ1 ( x1 , y1 , z1 )
con respecto al sistema de referencia Σ0 ( x0 , y0 , z0 ), y la matriz R21 describe
la orientación del sistema de referencia Σ2 ( x2 , y2 , z2 ) respecto al sistema
Σ1 ( x1 , y1 , z1 ).
Por lo tanto, la relación que hay entre un punto p0 ∈ Σ0 ( x0 , y0 , z0 ) con las
coordenadas p2 ∈ Σ2 ( x2 , y2 , z2 ) está determinada por la siguiente expresión:
p0 = R10 R21 p2
(3.22)
donde R10 R21 representa la orientación del sistema de referencia Σ2 ( x2 , y2 , z2 )
respecto al sistema fijo Σ0 ( x0 , y0 , z0 ), y significa una regla de composición para
establecer el orden para transformar un punto p desde su representación en el sistema
Σ2 ( x2 , y2 , z2 ) a su correspondiente representación en el sistema Σ0 ( x0 , y0 , z0 ).
La regla de transformación R10 R21 establece que el primer paso es transformar el
punto p2 en coordenadas del sistema Σ1 ( x1 , y1 , z1 ) por medio de R21 , posteriormente
al punto p0 por medio de R10 . La regla de composición de rotaciones sucesivas se
establece como:
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
166
Capı́tulo 3: Preliminares matemáticos
R20 = R10 R21
(3.23)
La regla de composición de rotaciones (3.23) puede ser interpretada de la siguiente
manera: supóngase que inicialmente los tres sistemas Σ1 , Σ2 y Σ3 coinciden. Primero
se rota el sistema de referencia Σ1 ( x1 , y1 , z1 ) relativo a Σ0 ( x0 , y0 , z0 ) de acuerdo
a la transformación R10 . Ahora con los sistemas de referencia coincidentes Σ1 y
Σ2 , se rota el sistema Σ2 ( x2 , y2 , z2 ) relativo a Σ1 ( x1 , y1 , z1 ) de acuerdo a la
transformación R21 . El sistema de referencia Σ2 ( x2 , y2 , z2 ) tiene una orientación
con respecto al sistema de referencia Σ0 ( x0 , y0 , z0 ) dado por la matriz R10 R21 como
se muestra en la figura 3.11.
Figura 3.11 Regla de composición de rotaciones sucesivas.
La regla de composición de rotación sucesivas (3.23) se basa en generar nuevos
sistemas de referencia consecutivos. Por ejemplo, se forma el sistema de referencia
Σ1 ( x1 , y1 , z1 ) como consecuencia de rotar un ángulo determinado alrededor de
uno de los ejes principales del sistema Σ0 ( x0 , y0 , z0 ), seguido por otra rotación
ahora alrededor del eje y1 del sistema actual Σ1 ( x1 , y1 , z1 ) para obtener el sistema
de referencia Σ2 ( x2 , y2 , z2 ). Realizando giros en diferente orden, se obtienen otros
tipos de representaciones.
Es importante subrayar que la multiplicación de matrices de rotación no es
conmutativa, en consecuencia el orden de las rotaciones no es conmutativo.
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
3.4
Reglas de rotación
167
♣ Ejemplo 3.6
Considere el procedimiento de rotación que se muestra en la figura 3.12; obtener la
matriz resultante que define la rotación del sistema de referencia Σ2 con respecto
al sistema de referencia Σ0 .
Solución
En la figura 3.12 se pueden observar los tres sistemas de referencia Σ0 , Σ1 y Σ2 .
Todos los sistemas de referencia comparten el mismo origen.
Figura 3.12 Rotación con respecto al sistema actual.
En el esquema izquierdo de la figura 3.12 se encuentran definidos los sistemas de
referencia Σ0 ( x0 , y0 , z0 ) y Σ1 ( x1 , y1 , z1 ). Los ejes y0 y y1 son paralelos entre sı́;
existe una rotación por un ángulo φ alrededor del eje y0 que se representa por la
matriz Ry0 ,φ . Posteriormente, continúa una rotación por un ángulo θ alrededor del
eje z1 para generar el sistema de referencia Σ2 ( x2 , y2 , z2 ) (parte central de la figura
3.12), los ejes z1 y z2 son paralelos entre sı́. La orientación relativa del sistema Σ2
con respecto al sistema de referencia Σ1 está determinada por la matriz Rz1 ( θ ).
El esquema derecho de la figura 3.12 muestra la orientación del sistema de referencia
Σ2 con respecto al sistema de referencia Σ0 , cuya matriz de rotación se encuentra
dado por:
R20 = R10 R21 = Ry0 ( φ ) Rz1 ( θ )
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
168
Capı́tulo 3: Preliminares matemáticos
⎡
⎢
= ⎣
cos(φ)
0
sen(φ)
0
1
0
⎤⎡
cos(θ)
− sen(θ)
⎥⎢
⎦ ⎣ sen(θ) cos(cos(θ))
− sen(φ) 0 cos(φ)
0
0
⎡
⎤
cos(ψ) cos(θ)
− cos(ψ) sen(θ) sen(ψ)
⎢
⎥
= ⎣
sen(θ)
cos(θ)
0
⎦
− sen(φ) cos(θ)
sen(φ) sen(θ)
cos(φ)
0
⎤
⎥
0⎦
1
donde R10 = Ry0 ( φ ) y R21 = Rz1 ( θ ).
Para mostrar que el orden de las rotaciones es importante, considere el caso donde
las rotaciones se realizan en orden inverso: R20 = R21 R10 = Rz1 ( θ )Ry0 ( φ ). Es decir,
primero la rotación alrededor del eje z1 , seguida por una rotación alrededor del eje
y0 . Entonces, la matriz resultante está dada por:
R20 = Rz1 ( θ ) Ry0 ( φ )
⎡
⎤⎡
⎤
cos(θ)
− sen(θ) 0
cos(φ)
0 sen(φ)
⎢
⎥⎢
⎥
= ⎣ sen(θ) cos(cos(θ)) 0 ⎦ ⎣
0
1
0 ⎦
0
0
1
− sen(φ) 0 cos(φ)
⎡
⎤
cos(θ) cos(ψ) − sen(θ) cos(θ) sen(ψ)
⎢
⎥
= ⎣ sen(θ) cos(ψ)
cos(θ)
sen(θ) sen(ψ) ⎦
− sen(ψ)
0
cos(ψ)
Observe que es importante el orden de rotaciones: R20 = R10 R21 . Es decir, en general
R20 = R21 R10 .
♣ ♣ Ejemplo 3.7
Considere una figura de prueba (flecha) que se encuentra definida en el sistema
de referencia Σ0 (x0 , y0 , z0 ). Realizar rotaciones sucesivas por un ángulo θ =
π
2
alrededor de los eje z0 , del sistema resultante Σ1 (x1 , y1 , z1 ) realizar una rotación
θ=
π
2
alrededor del eje x1 , y finalmente del sistema generado Σ2 (x2 , y2 , z2 ) rotar
alrededor del eje y2 por θ = π2 .
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
3.4
Reglas de rotación
169
Solución
La figura de prueba es una flecha cuyas coordenadas se encuentran definidas en
el sistema de referencia Σ0 (x0 , y0 , z0 ), cada coordenada de la figura de prueba se
encuentra especificada por un punto p0 = [ px0 , py0 , pz0 ]T . Un observador en el
sistema de referencia Σ0 (x0 , y0 , z0 ) verá a la figura de prueba sin ningún efecto
de rotación. Sin embargo, después de realizar la rotación Rz0 ( π2 ) un observador
colocado en el sistema de referencia Σ0 (x0 , y0 , z0 ) verá a dicha figura rotada 90
grados en el nuevo sistema generado Σ1 (x1 , y1 , z1 ). Las coordenadas de la flecha en
el sistema Σ1 (x1 , y1 , z1 ) son p1 = Rz0 ( π2 )p0 . Posteriormente se realiza una nueva
rotación de 90 grados alrededor del eje x1 , dada por: Rx1 ( π2 ), con esto se genera
el sistema Σ2 (x2 , y2 , z2 ). Las coordenadas de la flecha en este sistema Σ2 (x2 , y2 , z2 )
están especificadas por p2 = Rx1 ( π2 )p1 . La última rotación consiste en girar a la
imagen de prueba (flecha) 90 grados alrededor del eje y2 . Esta rotación genera el
sistema de referencia Σ3 (x3 , y3 , z3 ) con coordenadas de la imagen p3 = Ry2 ( π2 )p2 .
La relación del sistema de referencia Σ3 (x3 , y3 , z3 ) con el sistema de referencia
Σ0 (x0 , y0 , z0 ) es: p3 = Ry2 ( π2 )Rx1 ( π2 )Rz0 ( π2 )p0 o p0 = [ Ry2 ( π2 )Rx1 ( π2 )Rz0 ( π2 ) ]T p3 .
La figura 3.13 muestra la sucesión de rotaciones.
Figura 3.13 Rotaciones sucesivas de la figura de prueba alrededor de los ejes
z0 , x1 , y2 .
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
170
Capı́tulo 3: Preliminares matemáticos
Código Fuente 3.5
Rotaciones alrededor de los ejes z0 , x1 , y2
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 3 Cinemática %Archivo cap3 rotacion.m
Rotaciones alrededor de los ejes z0 , x1, y2
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
clc; clear all; close all;
pzo=[0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0];
pyo=[0,0,0,0,0,9,9,10,9, 11, 9,12, 1, 2, 3, 4, 5, 6,7, 8, 9, 12, 12, 12, 12,1, 2, 3, 4, 5,
6, 7, 8, 9, 12, 9, 12, 9, 11, 9, 10, 9,0,0,0 ];
pxo=[0,0,0,0,0,6,7,7, 8, 8, 9,9, 10,10, 10, 10, 10, 10, 10, 10, 10, 10, 11, 12, 13, 14,14,
14, 14, 14, 14, 14, 14,14, 14, 15, 15, 16,16, 17,17,18,19,20,21];
theta=90*3.1416/180.0;
R ztheta=[ cos(theta), -sin(theta), 0; sin(theta), cos(theta), 0; 0, 0,1];
R xtheta=[ 1, 0, 0; 0, cos(theta), -sin(theta); 0, sin(theta), cos(theta)];
R ytheta=[ cos(theta), 0, sin(theta); 0, 1, 0; -sin(theta), 0, cos(theta)];
⎡ ⎤
⎡ ⎤T
x1
x0
⎢ ⎥
⎢ ⎥
%⎣ y1 ⎦ = Rz (θ) ⎣ y0 ⎦
z1
z0
Sigma1=R ztheta*[pxo; pyo; pzo];
px1=Sigma1(1,:);py1=Sigma1(2,:); pz1=Sigma1(3,:);
⎡ ⎤
⎡ ⎤
x2
x1
⎢ ⎥
⎢ ⎥
%⎣ y2 ⎦ = Rx(θ) ⎣ y1 ⎦
z2
z1
Sigma2=R xtheta*[px1; py1; pz1];
px2=Sigma2(1,:); py2=Sigma2(2,:); pz2=Sigma2(3,:);
⎡ ⎤
⎡ ⎤
x3
x2
⎢ ⎥
⎢ ⎥
%⎣ y3 ⎦ = Ry (θ) ⎣ y2 ⎦
z3
z2
Sigma3=R ytheta*[px2; py2; pz2];
px3=Sigma3(1,:); py3=Sigma3(2,:); pz3=Sigma3(3,:);
plot3(pxo,pyo,pzo,’.’,px1,py1,pz1,’x’,px2,py2,pz2,’o’,px3,py3,pz3,’o’)
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
3.5
171
Transformaciones de traslación
3.5 Transformaciones de traslación
C
onsidere el sistema de referencia cartesiano fijo Σ0 ( x0 , y0 , z0 ) y el sistema de
referencia Σ1 ( x1 , y1 , z1 ), donde sus respectivos orı́genes son no coincidentes.
El origen del sistema de referencia Σ1 se encuentra desplazado una distancia d10 con
respecto al origen del sistema Σ0 , como se muestra en la figura 3.14.
Figura 3.14 Transformaciones de traslación y rotación del sistema Σ1 con respecto
al sistema Σ0 .
El vector d10 está expresado en coordenadas del sistema Σ0 : d10 = [ d10x, d10y , d10z ]T ,
entonces cualquier punto p tiene representación p0 y p1 . La relación general entre
los sistemas de referencia Σ0 ( x0 , y0 , z0 ) y Σ1 ( x1 , y1 , z1 ) incluyendo la matriz de
rotación R10 y el vector de traslación d10 es:
p0 = d10 + R10 p1
⎡
d10x
⎤
(3.24)
⎡
p1x
⎤
⎥
⎢
⎥
⎢
= ⎣ d10y ⎦ + R10 ⎣ p1y ⎦ .
d10z
p1z
Para el caso de tres sistemas de referencia cartesianos Σ0 (x0 y0 z0 ), Σ1 (x1 y1 z1 ) y
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
172
Capı́tulo 3: Preliminares matemáticos
Σ2 (x2 y2 z2 ) (ver figura 3.15) se obtienen las siguientes expresiones:
p0 = d10 + R10 p1
(3.25)
d21
(3.26)
p1 =
+ R21 p2
Figura 3.15 Traslación y rotación de Σ1 y Σ2 con respecto al sistema Σ0 .
Sustituyendo la ecuación (3.26) en (3.25) se tiene:
p0 = d10 + R10 d21 + R10 R21 p2 .
(3.27)
Por lo tanto, la regla de transformación de traslación y orientación de un punto p2
en el sistema Σ2 (x2 y2 z2 ) hacia un punto p0 en el sistema Σ0 (x0 y0 z0 ) adquiere la
siguiente estructura:
p0 = d20 + R20 p2
(3.28)
donde
R20 = R10 R21
d20 = d10 + R10 d21
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
3.6
173
Transformaciones homogéneas
3.6 Transformaciones homogéneas
L
a notación más común para representar la transformación de traslación y
rotación en forma compacta se conoce como transformación homogénea.
Por ejemplo, para representar el caso de traslación y rotación del sistema Σ1 (x1 y1 z1 )
con respecto al sistema Σ0 (x0 y0 z0 )
p0 = d10 + R10 p1
la transformación homogénea se realiza con la siguiente notación:
1
R0 d10
1
H0 =
0T 1
⎡
⎤
.
Matriz
de
Vector
de
..
⎢
⎥
⎢
⎥
rotación
traslación
⎥
= ⎢
⎢
⎥
..
⎣
⎦
···
.
···
0T
(3.29)
1
donde R10 ∈ SO(3) y d10 ∈ IR3 . Para propósitos de acoplamiento en dimensiones, el
vector 0T y el número 1 aparecen en el último renglón.
La representación inversa que relaciona el punto p1 en función del punto p0 adquiere
la siguiente forma:
T
T
p1 = −R10 d10 + R10 p0
entonces, la transformación homogénea inversa está determinada por:
1T
T
R0
−R10 d10
1 −1
=
H0
0T
1
(3.30)
Las matrices de rotación permiten modelar la orientación de la herramienta de
trabajo colocada en el extremo final del robot, y junto con las transformaciones
homogéneas dentro de una sola matriz incluye la orientación y posición de la
herramienta de trabajo, formando la estructura del modelo cinemático directo.
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
174
Capı́tulo 3: Preliminares matemáticos
3.7 Librerı́as para matrices homogéneas
E
n esta sección se presentan las librerı́as de transformación homogénea de
rotación y traslación con respecto a los ejes principales x, y, z.
Matrices de transformación homogénea de rotación
Las matrices de transformación homogénea de rotación con respecto a los ejes x, y, z,
respectivamente, tienen la siguiente estructura:
3 2 3⎤
⎡21
⎡ 2 cos(θ) 0 sen(θ) 3 2 0 3 ⎤
0
0
0
cos(θ) − sen(θ) 5
0 sen(θ)
cos(θ)
[0
0
0]
⎡ 2 cos(θ) − sen(θ) 0 3
4 sen(θ)
cos(θ)
05
HRx(θ) = ⎣
40
HRz (θ) = ⎣
0
0
[0
0
1
0]
405
0
1
⎦ HRy (θ) = ⎣
2 3⎤
0
405
0
1
4
0
1
− sen(θ) 0
[0
0
5
0
cos(θ)
0]
405
0
1
⎦
⎦
Matrices de transformación homogénea de traslación
Las matrices de transformación homogénea de traslación con respecto a los ejes
x, y, z respectivamente tienen la siguiente estructura:
21 0 03 2d3 21 0 03 203 21 0 03 203 40 1 05 405
40 1 05 4d5
40 1 05 405
HTy (d) =
HTz (d) =
HTx(d) =
0
0
d
0 0 1
0 0 1
0 0 1
[0
0
0]
1
[0
0
0]
1
[0
0
0]
1
Matriz de transformación homogénea Denavit-Hartenberg
La matriz de transformación homogénea Denavit Hartenberg tiene la forma
siguiente:
⎡
⎢R
H = ⎢
⎣
0T
Alfaomega
⎡ ⎤
x ⎤
⎢ ⎥
⎣y ⎦⎥
⎥
z ⎦
1
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
3.7
Librerı́as para matrices homogéneas
175
3.7.1
3.7.1. Matriz de transformación homogénea HRx (θ)
La sintaxis de la función de transformación homogénea de rotación HRx(θ) alrededor
del eje x está dada por:
RHx=HRx(θ)
donde θ es el ángulo de rotación alrededor del eje x y es la variable de la función
HRx(θ). Retorna la matriz de transformación homogénea RHx.
Código Fuente 3.6
Función HRx(θ)
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 3 Cinemática %función HRx.m
Función HRx(θ)
1
2
function RHx=HRx(theta)
dato=whos(’theta’);
3
if strcmp(dato.class, ’sym’) %variables simbólicas
RHx=[1, 0, 0, 0;
0, cos(theta), -sin(theta), 0;
4
5
6
0, sin(theta), cos(theta), 0;
0, 0, 0, 1];
else digits(3); %cálculos numéricos
RHx=round([1, 0, 0, 0;
7
8
9
10
0, vpa(cos(theta),3), vpa(-sin(theta),3), 0;
0, vpa(sin(theta),3), vpa(cos(theta),3), 0;
0, 0, 0, 1]);
11
12
13
14
end
end
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
176
Capı́tulo 3: Preliminares matemáticos
3.7.2
3.7.2. Matriz de transformación homogénea HRy (θ)
La sintaxis de la función de transformación homogénea de rotación HRy (q) alrededor
del eje y está dada por:
RHy=HRy(θ)
donde θ es el ángulo de rotación alrededor del eje y y es la variable de la función
HRy(θ). Retorna la matriz de transformación homogénea RHy.
Código Fuente 3.7
Función HRy (θ)
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 3 Cinemática %función HRy.m
Función HRy (θ)
1
function RHy=HRy(theta)
2
dato=whos(’theta’); if strcmp(dato.class, ’sym’) %variables
simbólicas
RHy=[cos(theta), 0, sin(theta), 0;
0, 1, 0, 0;
3
4
5
-sin(theta), 0, cos(theta), 0;
0, 0, 0, 1];
else digits(3); %cálculos numéricos
6
7
8
RHy=round([ vpa(cos(theta),3), 0, vpa(sin(theta),3), 0;
0, 1, 0, 0;
vpa(-sin(theta),3), 0, vpa(cos(theta),3), 0;
0, 0, 0, 1]);
9
10
11
12
13
end
end
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
3.7
Librerı́as para matrices homogéneas
177
3.7.3
3.7.3. Matriz de transformación homogénea HRz (θ)
La sintaxis de la función de transformación homogénea de rotación HRz (θ) alrededor
del eje z está dada por:
RHz=HRz(θ)
donde θ es el ángulo de rotación alrededor del eje z y es la variable de la función
HRz(θ). Retorna la matriz de transformación homogénea RHz.
Código Fuente 3.8
Función HRz (θ)
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 3 Cinemática %función HRz.m
Función HRz (θ)
1
2
3
4
function RHz=HRz(theta)
dato=whos(’theta’);
if strcmp(dato.class, ’sym’) %variables simbólicas
RHz=[cos(theta), -sin(theta), 0, 0;
5
sin(theta), cos(theta), 0, 0;
0, 0, 1, 0;
0, 0, 0, 1];
else digits(3); %cálculos numéricos
6
7
8
9
RHz=round([ vpa(cos(theta),3), vpa(-sin(theta),3), 0, 0;
vpa(sin(theta),3), vpa(cos(theta),3), 0, 0;
0, 0, 1, 0;
10
11
12
0, 0, 0, 1]);
13
14
end
end
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
178
Capı́tulo 3: Preliminares matemáticos
3.7.4
3.7.4. Matriz de transformación homogénea HTx (d)
La sintaxis de la función de transformación homogénea de traslación HTx(d) a lo
largo del eje x está dada por:
THx=HTx(d)
donde d es el desplazamiento de traslación sobre el eje x. Retorna la matriz de
transformación homogénea THx.
Código Fuente 3.9
Función HTx(d)
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 3 Cinemática %función HTx.m
Función HTx(d)
1
2
3
function Tx=HTx(d)
Tx=[ 1 0 0 d; 0 1 0 0; 0 0 1 0; 0 0 0 1];
end
3.7.5
3.7.5. Matriz de transformación homogénea HTy (d)
La sintaxis de la función de transformación homogénea de traslación HTy (d) a lo
largo del eje y está dada por:
THy=HTy(d)
donde d es el desplazamiento lineal de traslación sobre el eje y. Retorna la matriz
de transformación homogénea THy.
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
3.7
Librerı́as para matrices homogéneas
Código Fuente 3.10
179
Función HTy (d)
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 3 Cinemática %función HTy.m
Función HTy (d)
1
function Tz=HTy(d)
2
3
Tz=[ 1 0 0 0; 0 1 0 d; 0 0 1 0; 0 0 0 1];
end
3.7.6
3.7.6. Matriz de transformación homogénea HTz (d)
La sintaxis de la función de transformación homogénea de traslación HTz (d) a lo
largo del eje z está dada por:
THz=HTz(d)
donde d es el desplazamiento lineal de traslación sobre el eje z. Retorna la matriz
de transformación homogénea THz.
Código Fuente 3.11
Función HTz (d)
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 3 Cinemática %función HTz.m
Función HTz (d)
1
2
3
function Tz=HTz(d)
Tz=[ 1 0 0 0; 0 1 0 0; 0 0 1 d; 0 0 0 1];
end
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
180
Capı́tulo 3: Preliminares matemáticos
3.7.7
3.7.7. Matriz de transformación DH
La función H DH(H) extrae de la matriz de transformación homogénea H la matriz
de rotación y el vector de coordenadas cartesianas.
La sintaxis de la función de transformación homogénea DH está dada por:
[R vect d vect cero c]=H DH(H)
donde H es la variable de entrada y representa la matriz de transformación
homogénea. Retorna la matriz de rotación R , el vector de coordenadas cartesianas
vect d, el vector vect cero=[0, 0, 0]T y la constante unitaria c=1.
Código Fuente 3.12
Función H DH
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 3 Cinemática %función H DH.m
Función H DH
1
2
3
function [R vect d vect cero c]=H DH(H)
for i=1:3
for j=1:3
4
R(i,j)=H(i,j);
5
end
6
end
7
%estructura de la matriz de transformación homogénea
8
vect d=[H(1,4); H(2,4); H(3,4)];
vect cero=[0;0;0]’;
c=1;
9
10
11
end
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
3.8
Resumen
181
3.8 Resumen
C
inemática de robots manipuladores y sistemas mecatrónicos encuentra su
fundamento en una clase particular de matrices denominadas ortogonales.
Estos preliminares matemáticos constituyen la base para desarrollar propiedades,
reglas y operaciones de traslación y rotación entre dos sistemas de referencia
Σ0 (x0 , y0 , z0 ) y Σ1 (x1 , y1 , z1 ) bajo una estructura matemática denominada matriz
de transformación homogénea.
La matriz ortogonal R10 representa la orientación del sistema de referencia
Σ0 (x0 , y0 , z0 ) con respecto al sistema Σ1 (x1 , y1 , z1 ). De particular interés son las
matrices de rotación Rx( θ ), Ry ( θ ), Rz ( θ ) ya que representan la rotación alrededor
de los ejes principales x, y, z, respectivamente. El orden como se realice la rotación
determina la regla o procedimiento de orientar un sistema de referencia con respecto
a otro.
En este capı́tulo se ha desarrollado un conjunto de librerı́as para realizar operaciones
de traslación y rotación. La librerı́a R=Rz(θ) realiza la rotación del sistema de
referencia Σ1 (x1 , y1 , z1 ) con respecto al sistema de referencia Σ0 (x0 , y0 , z0 ). Esta
función puede trabajar con variables simbólicas y también para realizar aplicaciones
numéricas. De manera análoga se puede implementar las funciones R=Rx(θ) y
R=Ry(θ).
Cuando hay combinación de cálculos numéricos con variables simbólicas, como es
el caso de las matrices de transformación homogénea (rotación y traslación) la
funciones vpa, round y double pueden ayudar a presentar el resultado numérico
de manera conveniente.
Por otro lado, para simplificar las expresiones simbólicas se recomienda usar la
función simplify, por lo que el lector puede modificar el código fuente de las
librerı́as desarrolladas para presentar resultados simbólicos o numéricos a su entera
conveniencia.
La tabla 3.1 contiene el resumen de las librerı́as para realizar las matrices de
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
182
Capı́tulo 3: Preliminares matemáticos
transformación homogénea de traslación y rotación.
Tabla 3.1 Matrices de transformación homogénea
Función
Sintaxis
Matriz de transformación
homogénea de rotación al-
RHx=HRx(θ) θ ∈ IR.
rededor del eje x
Matriz de transformación
homogénea de rotación al-
RHy=HRy(θ) θ ∈ IR.
rededor del eje y
Matriz de transformación
homogénea de rotación al-
RHz=HRz(θ) θ ∈ IR.
rededor del eje z
Matriz de transformación
homogénea de traslación
THx=HTx(d) d∈ IR.
sobre el eje x
Matriz de transformación
homogénea de traslación
THy=HTy(d) d∈ IR.
sobre el eje y
Matriz de transformación
homogénea de traslación
THz=HTz(d) d∈ IR.
sobre el eje z
[R vect d vect cero c]=H DH(H)
Matriz de transformación
homogénea
Hartenberg
Denavit-
R∈ SO(3) es la matriz de rotación
vect d es el vector con coordenadas cartsianas:
[x
y
z ]T
vect cero=[ 0, 0, 0 ]T
c=1.
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
Cinemática directa
Capı́tulo
4
li
αi
.
di .. βi
θi
.
i
Hi−1
= HRzi (θi )HTzi (di .. βi )HTxi (li)HRxi (αi )
4.1 Introducción
4.2 Cinemática inversa
4.3 Cinemática diferencial
4.4 Clasificación de robots industriales
4.5 Convención Denavit-Hartenberg
4.6 Resumen
Objetivos
Presentar el modelo de cinemática directa de las principales configuraciones de robots industriales considerando los parámetros geométricos y
desarrollar librerı́as en lenguaje MATLAB (toolbox) que permitan realizar
aplicaciones en el área de cinemática directa de robots manipuladores.
Objetivos particulares:
Matrices de rotación.
Transformaciones homogéneas.
Método de Denavit-Hartenberg.
Cinemática directa cartesiana.
Librerı́as de análisis y diseño de cinemática de robots industriales.
4.1
185
Introducción
4.1 Introducción
C
inemática es la parte de la fı́sica que estudia el movimiento de sistemas
mecánicos, sin tomar en cuenta las fuerzas que originan dicho movimiento,
por lo tanto no involucra ecuaciones diferenciales como en el caso de la dinámica.
Al estudio de la cinemática de sistemas mecatrónicos y robots manipuladores se
le denomina cinemática directa, se refiere al estudio analı́tico del movimiento
del robot con respecto a un sistema de referencia cartesiano fijo Σ0 ( x0 , y0 , x0 )
relacionando la dependencia que existe entre las coordenadas articulares o
generalizadas q ∈ IRn , y los parámetros geométricos (longitudes del i-ésimo eslabón
li ), con coordenadas cartesianas [ x, y, z ]T ∈ IR3 y la orientación [ θ, φ, ψ ]T ∈ IR3 del
extremo final del robot a través de una función vectorial f R continua y diferenciable
en la variable de estado q, generalmente no lineal.
Cinemática directa es una función vectorial f R (li, q) que relaciona las
coordenadas articulares q ∈ IRn y propiedades geométricas del sistema mecánico
li con las coordenadas cartesianas [ x, y, z ]T ∈ IR3 del robot y la orientación
[ θ, φ, ψ ]T ∈ IR3 de la herramienta colocada en el extremo final. Es decir f R :
IRn → IRm tal que:
⎡
x
⎤
⎢y⎥
⎢ ⎥
⎢ ⎥
⎢z⎥
⎢ ⎥ = f R (li, q)
⎢θ⎥
⎢ ⎥
⎢ ⎥
⎣φ⎦
(4.1)
ψ
donde n indica el número de grados de libertad y la dimensión del vector
de coordenadas articulares q, m es la dimensión conjunta de las coordenadas
cartesianas y la orientación de la herramienta de trabajo.
De manera general, el posicionamiento del extremo final del robot en el espacio
tridimensional (pose) requiere de 6 coordenadas (m = 6): 3 coordenadas para la
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
186
Capı́tulo 4: Cinemática directa
posición cartesiana y 3 coordenadas para la orientación de la herramienta de trabajo.
Dependiendo de la aplicación del robot se pueden requerir menos coordenadas
de posición y orientación. Por ejemplo, un robot para pintura de armaduras
automotrices requiere las 6 coordenadas, en contraste con un robot que corta figuras
de plástico sobre un plano requiere 2 coordenadas cartesianas de posición y ninguna
de orientación. Cuando n > m se denomina robots redundantes. El empleo de la
cinemática directa resulta de utilidad en la planificación de trayectorias y en el
control cartesiano. El papel fundamental de la cinemática directa, es computar la
posición y orientación del extremo final del robot manipulador como una función de
las variables articulares.
Un robot manipulador se considera como una serie de eslabones interconectados
a través de articulaciones (servomotores) rotacionales o prismáticas en forma de
cadena cinemática abierta, es decir el extremo final donde se coloca la herramienta
no se encuentra conectada mecánicamente a la primera articulación (base) del
robot. Desde el punto de vista topológico, la cadena cinemática se considera
abierta cuando los dos extremos de la cadena no se tocan. De otra manera la
cadena cinemática formarı́a un lazo si sus dos extremos están mecánicamente
unidos.
La estructura mecánica del robot manipulador se caracteriza por tener un número
de grados de libertad, los cuales determinan en forma única su configuración.
Tı́picamente, cada grado de libertad está asociado a una articulación (variable
articular q).
4.2 Cinemática inversa
D
ada la posición del extremo final del robot en coordenadas cartesianas
[x, y, z]T y la orientación [ψ, θ, φ]T , con respecto a un sistema de referencia fijo
Σ0 (x0 , y0 , z0 ), ası́ como los parámetros geométricos li , entonces surge la pregunta
natural: ¿pueden obtenerse las coordenadas articulares del robot q para que el
extremo final del robot se posicione en las coordenadas cartesianas solicitadas, con
la orientación requerida?
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
4.3
Cinemática diferencial
187
El problema planteado se conoce como cinemática inversa y representa un área
de la robótica de mayor complejidad que la cinemática directa. Para un robot
manipulador siempre es posible encontrar el modelo de cinemática directa, mientras
que en la cinemática inversa pueden haber varias soluciones e inclusive no existir
solución analı́tica; si este es el caso, entonces como posibles formas de solución pueden
proponerse redes neuronales, métodos numéricos, iterativos, geométricos, etcétera.
La cinemática inversa es un problema no lineal que relaciona las coordenadas
articulares en función de las coordenadas cartesianas y la orientación de la
herramienta del extremo final del robot manipulador
q = f −1
R (x, y, z, li, θ, φ, ψ)
(4.2)
donde f −1
R (x, y, z, li, θ, φ, ψ) es función inversa de la ecuación (4.1).
4.3 Cinemática diferencial
L
a cinemática diferencial directa es la derivada con respecto al tiempo de la
cinemática directa
v
d
d
= f R (q)
(4.3)
[ x y z θ φ ψ ]T =
dt
dt
w
∂f R (q)
=
q̇ = J(q)q̇.
∂q
Como se ve, ésta relaciona la velocidad articular q̇ ∈ IRn con la velocidad lineal
d
d
[ x, y, z ]T = [ ẋ, ẏ, ż ]T ∈ IR3 y la velocidad angular ẇ = dt
[ θ, φ, ψ ]T =
v = dt
[ θ̇, φ̇, ψ̇ ]T ∈ IR3 , además el mapeo es descrito en términos de una matriz J(q) =
∂ f R (q )
∈ IR6×n denominada jacobiano del robot o jacobiano analı́tico:
∂q
Jv (q)
J(q) =
.
(4.4)
Jw (q)
Jv (q) ∈ IR3×n relaciona la velocidad articular q̇ ∈ IRn con la velocidad lineal v ∈ IR3 ,
mientras que Jw (q) ∈ IR3×n relaciona la velocidad angular w ∈ IR3 con la velocidad
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
188
Capı́tulo 4: Cinemática directa
articular q̇ ∈ IRn , es decir:
v
w
= J(q)q̇ =
Jv (q)q̇
.
Jw (q)q̇
(4.5)
El jacobiano del robot representa una importante herramienta en robótica que sirve
para caracterizar a un robot manipulador, encontrar configuraciones singulares,
analizar redundancia, determinar la cinemática diferencial inversa, ası́ como describir
la relación entre la fuerza aplicada y los pares o torques resultantes del extremo final.
Es indispensable para el análisis y diseño de algoritmos de control cartesiano.
Hay varias formas de seleccionar la orientación de la herramienta del robot
manipulador: si de manera particular dicha orientación es representada por los
ángulos de Euler (un sistema de referencia asociado al extremo final del robot o
a la herramienta de trabajo), entonces la velocidad angular w = [ θ̇, φ̇, ψ̇ ]T ∈ IR3
relaciona la matriz jacobiano analı́tico, como se encuentra descrita en la ecuación
(4.3). Otra posible forma de modelar la orientación de la herramienta del robot es
expresarla directamente en un sistema de referencia especı́fico, por ejemplo al origen
localizado en la base del robot, entonces a la matriz J(q) se le denomina jacobiano
geométrico que depende de la configuración del robot manipulador. El jacobiano
analı́tico difiere del jacobiano geométrico: básicamente la diferencia se encuentra en
cómo modelar la orientación de la herramienta de trabajo del robot.
La cinemática diferencial inversa representa la relación entre la velocidad articular
q̇ con la velocidad lineal de movimiento v y la velocidad angular w, expresada en
términos de la matriz inversa del jacobiano del robot:
q̇ = J
−1
(q)
v
w
(4.6)
donde J −1 (q) ∈ IR6×n es la matriz inversa del jacobiano del robot, la cual existe si
es una matriz cuadrada y su determinante es diferente a cero.
Si el determinante del jacobiano del robot J(q) es cero, entonces se dice que no es
de rango completo y se presentan problemas de singularidades.
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
4.4
189
Clasificación de robots industriales
Singularidad significa que no es posible indicarle un movimiento arbitrario
al extremo final del robot, es decir para una velocidad lineal v y velocidad
angular w finitas puede corresponder una velocidad articular q̇ infinita.
Puede existir un conjunto infinito de soluciones para la cinemática directa.
La cinemática inversa diferencial tiene un número infinito de soluciones.
En control cartesiano la fuerza aplicada al robot puede provocar un par
infinito a las articulaciones del robot.
Dependiendo del tipo de robot, las singularidades pueden generar un número
infinito de puntos de equilibrio en la ecuación en lazo cerrado, formada por
la dinámica del robot y la estructura cartesiana de control.
4.4 Clasificación de robots industriales
U
n robot industrial está compuesto por una serie consecutiva de eslabones y
articulaciones para formar una cadena en cinemática abierta, la cual es la
estructura mecánica básica de un robot industrial. La cadena en cinemática abierta
está formada de la siguiente manera: la primera articulación sirve para formar la
base; a continuación siguen conexiones sucesivas entre articulaciones y eslabones, en
el extremo final del último eslabón no hay articulación, generalmente se destina a
colocar la herramienta de trabajo para llevar a cabo una aplicación especı́fica. El
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
190
Capı́tulo 4: Cinemática directa
extremo final del robot no se encuentra conectado fı́sicamente a la base como se
muestra en la figura 4.1:
Figura 4.1 Cadena en cinemática abierta.
Las articulaciones se construyen por medio de un servomotor y representan la
interconexión entre dos eslabones consecutivos. Una articulación puede realizar
sólo un tipo de movimiento, ya sea lineal, también conocida como prismática, y
rotacional. La figura 4.2 presenta el tipo de articulaciones:
Figura 4.2 Tipo de articulaciones: rotacional y lineal o prismática.
La figura 4.3 muestra la analogı́a entre el brazo humano y un brazo robot o robot
industrial. La articulación de la base corresponde a la cintura. La articulación
del hombro (shoulder) debe ser la de mayor capacidad con respecto a las otras
articulaciones, ya que es la que mueve y soporta el peso de la articulación del codo
(elbow) y de la herramienta de trabajo, ası́ como la carga de objetos que realice
durante una determinada aplicación.
Dependiendo del tipo de articulaciones (lineales o rotacionales) que se encuentran
incluidas en la estructura mecánica en cinemática abierta de la base, hombro y
codo del robot (sin incluir las articulaciones de la orientación de la herramienta de
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
4.4
Clasificación de robots industriales
191
Figura 4.3 Base, hombro y codo de un robot industrial.
trabajo), se desprende la clasificación general de robots manipuladores industriales,
también conocidos como brazos robots: antropomórfico, esférico, cilı́ndrico, SCARA
y cartesiano.
Tabla 4.1 Clasificación de robots industriales
Robot
Caracterı́sticas
Antropomórfico (RRR)
SCARA (RRP)
Esférico (RRP)
3 articulaciones rotacionales
2 articulaciones rotacionales y 1 prismática
2 articulaciones rotacionales y 1 prismática
Cilı́ndrico (RPP)
Cartesiano (PPP)
1 articulación rotacional y 2 prismáticas
3 articulaciones prismáticas
La nomenclatura empleada en robots industriales para representar el tipo de
movimiento que realizan sus articulaciones está dada de la siguiente manera:
R significa articulación tipo rotacional, mientras que la letra P representa una
articulación prismática. El orden en que se presentan corresponde a las articulaciones
de la base, hombro y codo, respectivamente. Por ejemplo, en la tabla 4.1 la notación
robot cilı́ndrico (RPP) significa que la base es una articulación rotacional, mientras
que el hombro y codo corresponden a articulaciones prismáticas.
En la figura 4.4 se muestra la clasificación de las 5 configuraciones de robots
industriales.
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
192
Capı́tulo 4: Cinemática directa
Figura 4.4 Clasificación de los robots industriales.
4.5 Convención Denavit-Hartenberg
E
l método de Denavit-Hartenberg es una herramienta ampliamente conocida en
el área de ingenierı́a, ya que ofrece un procedimiento sencillo para obtener el
modelo cinemático directo cuya estructura queda en términos de la transformaciones
homogéneas.
Jaques Denavit y Richard S. Hartenberg en 1955 presentaron un procedimiento
para obtener una representación mı́nima de la orientación y traslación de robots
manipuladores. Consiste en determinar una tabla de parámetros relacionados con
los eslabones del robot. La convención Denavit-Hartenberg toma como referencia el
diagrama de un robot manipulador en cadena cinemática abierta como se muestra
en la figura 4.5.
Las variables articulares en la representación Denavit-Hartenberg se denotan con θi
para el tipo rotacional, prismática o lineal por di; este parámetro di también hace
el papel de representar el ancho del servomotor de la articulación rotacional más el
espesor de la placa métalica del eslabón, en este caso se denota por el sı́mbolo βi ; la
longitud del eslabón se representa con li y el ángulo de separación entre los ejes zi
y zi−1 se denota con αi .
El ángulo θi es el ángulo entre los ejes xi−1 y xi medido alrededor del eje zi−1 ; di es
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
4.5
193
Convención Denavit-Hartenberg
Figura 4.5 Convención Denavit-Hartenberg para un robot manipulador.
la distancia del origen del sistema de referencia i − 1 a la intersección del eje xi con
el eje zi−1 . Su medición se realiza a lo largo del eje zi−1 , como se indica en la figura
4.6. Adicionalmente a las variables articulares θi y di, hay 2 parámetros constantes
que describen caracterı́sticas especı́ficas del eslabón i-ésimo. Esos parámetros son:
el parámetro li se define como la distancia a lo largo del eje xi desde el origen del
sistema de referencia coordenado i − 1 hasta la intersección del eje zi−1 con el eje xi .
El otro parámetro es el ángulo entre los ejes zi y zi−1 se denota por αi , su medición
es respecto a un plano normal a xi . Una medición de ángulo positivo para αi se toma
en dirección del eje zi−1 hacia zi . Por ejemplo, para un robot con 6 articulaciones
rotacionales se requieren de 24 elementos para describir completamente su modelo
cinemático (li, αi , βi, θi).
Selección de sistemas de referencia
En la metodologı́a Denavit-Hartenberg, primero se describirá la convención para
asignar los sistemas de referencia cartesianos asociados a los eslabones del robot.
En este punto es necesario aclarar que en la literatura de robótica, la convención
Denavit-Hartenberg no es única, depende de la selección de los sistemas de referencia
cartesianos en las articulaciones y eslabones, ası́ como en sus eslabones adyacentes.
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
194
Capı́tulo 4: Cinemática directa
Figura 4.6 Convención para medir θi y αi .
La cinemática directa del robot proporciona las coordenadas cartesianas del extremo
final del robot relativo a un sistema de referencia cartesiano fijo Σ0 (x0 , y0 , z0 ); en la
figura 4.5 se muestra la asignación de sistemas de referencia para las articulaciones
i − 1-ésima, i-ésima e i + 1 de un robot manipulador.
En general se tiene el siguiente procedimiento:
El eje zi se asigna rı́gidamente a la articulación i + 1. Es decir, z0 es el eje de
la articulación 1, z1 es el eje de la articulación 2, y ası́ sucesivamente.
Localizar el origen oi del sistema de referencia Σi ( xi , yi , zi ) en la intersección
del eje zi con la normal común a los ejes zi−1 y zi .
Seleccionar el eje xi−1 sobre la normal que une los ejes zi−1 y zi en dirección
de la articulación i − 1 hacia la articulación i.
Definir el ángulo de torsión αi , este es el ángulo entre los ejes zi y zi−1 y se
mide con valor positivo en el sentido de las manecillas del reloj sobre el eje xi .
Seleccionar el eje yi por la regla de la mano derecha.
La convención Denavit-Hartenberg proporciona una representación no única para
los siguientes casos:
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
4.5
Convención Denavit-Hartenberg
195
Para el sistema de referencia Σ0 ( x0 , y0 , z0 ) sólo la dirección del eje z0 es
especificada, entonces su origen o0 y el eje x0 pueden ser seleccionados de
manera arbitraria.
Para el sistema de referencia Σn ( xn , yn , zn ) no existe la articulación n + 1,
entonces el eje zn no está completamente definido, mientras que el eje xn es
normal al eje zn−1 . Tı́picamente la n-ésima articulación es rotatoria, por lo
tanto zn se alinea en la dirección de zn−1 .
Cuando dos ejes consecutivos zi y zi−1 son paralelos entre sı́, la normal común
entre ellos no es única.
Cuando dos ejes consecutivos zi y zi−1 se interceptan, la dirección del eje xi
es arbitraria.
Cuando la articulación i-ésima es lineal o prismática, entonces la dirección de
zi es arbitraria.
De acuerdo con esta convención previamente descrita, a continuación se resumen los
parámetros del i-ésimo eslabón:
1. li es la longitud del i-ésimo eslabón, es la distancia del eje zi−1 hacia el eje zi
medida sobre el eje xi−1 .
2. αi es el ángulo de torsión, el cual representa el ángulo entre los ejes zi−1 a zi
medido en el sentido de las manecillas del reloj sobre el eje xi .
3. di se emplea en articulaciones lineales o prismáticas y representa el desplazamiento lineal. Cuando la articulación es rotacional, entonces representa el offset o
espesor del servomotor (la distancia de xi−1 a xi medido sobre el eje zi−1 ), se
denota por βi .
4. θi es el desplazamiento rotacional de xi−1 a xi medido alrededor del eje zi−1 . El
signo positivo de θi es el sentido contrario a las manecillas del reloj.
Obsérvese que li y βi siempre serán positivos puesto que corresponden a longitudes,
mientras que αi , di, θi representan cantidades con signo.
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
196
Capı́tulo 4: Cinemática directa
4.5.1
4.5.1. Algoritmo Denavit-Hartenberg
A continuación se describe el procedimiento para encontrar la cinemática directa a
través de la convención Denavit-Hartenberg.
1. Localizar la dirección de los ejes z0 , z1 , · · · , zn−1 .
2. Establecer el sistema de referencia cartesiano fijo Σ0 ( x0 , y0 , z0 ) cuyo origen es
colocado sobre el sistema de referencia en la base del robot. Los ejes x0 , y0 son
determinados de acuerdo con la regla de la mano derecha.
Una vez que el sistema de referencia Σ0 ( x0 , y0 , z0 ) ha sido establecido, se inicia
un proceso iterativo en el cual se define el sistema de referencia Σi ( xi , yi , zi )
usando el sistema de referencia Σi−1 (xi−1 ,yi−1 , zi−1 ), iniciando con el sistema
de referencia Σ1 ( x1 , y1 , z1 ). En la figura 4.5 se muestra el procedimiento.
Llevar a cabo los pasos 3 al 5 para la articulaciones i = 1, · · ·, n − 1.
3. Localizar el origen oi en la intersección de la normal común que une al eje zi con
el eje zi−1 .
Si el eje zi intercepta al eje zi−1 colocar oi en la intercepción.
Para el caso en que los ejes zi y zi−1 son paralelos:
Si la articulación i-ésima es rotacional, colocar el origen oi sobre la
articulación i-ésima, tal que di = 0.
Si la articulación i-ésima es prismática, colocar el origen oi en un punto
lı́mite fı́sico de la articulación i-ésima, por ejemplo en un punto extremo.
4. Seleccionar el eje xi a lo largo de la normal común que une a los ejes zi−1 y zi ,
en dirección de la articulación i − 1 hacia la articulación i.
5. Determinar yi por la regla de la mano derecha.
6. Establecer el sistema de referencia del extremo final Σn ( xn , yn , zn ).
Si la articulación n-ésima es rotatoria, entonces alinear el eje zn con el
eje zn−1
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
4.5
197
Convención Denavit-Hartenberg
Si la articulación n-ésima es prismática, entonces seleccionar el eje zn de
forma arbitraria. El eje xn debe cumplir el paso 4.
7. Establecer la tabla 4.2 de parámetros de eslabones.
8. Obtener las matrices de transformaciones homogéneas (4.7):
i
Hi−1
para i = 1, 2, · · · , n − 1.
Tabla 4.2 Parámetros Denavit-Hartenberg
Caracterı́sticas de eslabones
li
di
αi
Longitud del eslabón i-ésimo.
Articulaciones lineales o prismáticas. Tambı́en
representa el espesor del servomotor (βi ).
Ángulo entre los ejes zi−1 y zi medido con respecto al eje xi .
articulaciones rotacionales; representa el ángulo
θi
entre los ejes xi−1 y xi medido alrededor del eje
zi−1 .
i
En la representación Denavit-Hartenberg cada transformación homogénea Hi−1
se
representa por el producto de cuatro transformaciones básicas:
i
= HRzi−1 (θi )HTzi−1 (di (βi ))HTxi−1 (li )HRxi−1 (αi)
Hi−1
⎡
⎤
⎤⎡
cos(θi ) − sen(θi ) 0 0
1 0 0
0
⎢ sen(θ )
⎢
cos(θi )
0 0⎥
0 ⎥
i
⎢
⎥
⎥⎢0 1 0
= ⎢
⎥
⎥⎢
⎣
0
0
1 1 ⎦ ⎣ 0 0 1 di (βi) ⎦
0
0
⎤⎡
0 0 li
1
⎢0 1 0 0 ⎥⎢0
⎢
⎥⎢
⎢
⎥⎢
⎣0 0 1 0 ⎦⎣0
⎡
⎡
0 1
0
1
cos(αi )
sen(αi)
0
0
0 0 0 1
cos(θi ) − sen(θi ) cos(αi )
⎢ sen(θ )
i
⎢
= ⎢
⎣
0
0
cos(θi ) cos(αi )
sen(αi )
0
0 0 0
0
0
(4.7)
1
⎤
− sen(αi ) 0 ⎥
⎥
⎥
cos(αi )
0⎦
0
1
sen(θi ) sen(αi )
li cos(θi )
0
1
⎤
− cos(θi ) sen(αi ) li sen(θi ) ⎥
⎥
⎥
cos(αi )
di (βi) ⎦
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
198
Capı́tulo 4: Cinemática directa
n−1 n
La transformación homogénea total se obtiene como H0n = H01 H12 · · · Hn−2
Hn−1 .
La cinemática directa es la forma general de transformaciones homogéneas que
concatena los sistemas de referencia cartesianos asociados a los eslabones del robot,
todos relativos al sistema de referencia fijo Σ0 .
4.6 Resumen
C
inemática directa relaciona las coordenadas articulares y propiedades
geométricas del sistema mecánico con las coordenadas cartesianas del robot
y la orientación de la herramienta colocada en el extremo final. En este capı́tulo
se ha presentado los conceptos de cinemática inversa, cinemática diferencial y la
importancia que presenta el jacobiano del robot en control cartesiano y en el tema
de singularidades. Para las finalidades de la presente obra, cuando se relaciona
las coordenadas articulares con las coordenadas cartesianas sin tomar en cuenta
la orientación de la herramienta de trabajo, se denomina cinemática directa
cartesiana, la cual será la base de análisis de las principales configuraciones de
robots industriales.
El procedimiento Denavit-Hartenberg permite obtener el modelo de cinemática
directa de robots manipuladores con eslabones en serie a través de la siguiente tabla
de parámetros 3.1:
Tabla 4.3 Parámetros DH
Eslabóni
li
αi
di
θi
La formulación Denavit-Hartenberg queda expresada en términos de matrices
homogéneas con el estricto orden de transformaciones de traslación y rotación:
i
= HRzi (θi+1 )HTzi (di+1 )HTxi (li+1 )HRxi (αi+1 )
Hi−1
⎡
⎤
cos(θi ) − sen(θi ) cos(αi )
sen(θi ) sen(αi ) li cos(θi )
⎢ sen(θ )
cos(θi ) cos(αi )
− cos(θi ) sen(αi) li sen(θi ) ⎥
i
⎢
⎥
= ⎢
⎥
⎣
⎦
cos(αi )
di
0
sen(αi )
0
Alfaomega
0
0
1
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
Capı́tulo
5
Cinemática directa
cartesiana
5.1 Introducción
5.2 Brazo robot antropomórfico
5.3 Configuración SCARA
5.4 Robot esférico
5.5 Manipulador cilı́ndrico
5.6 Configuración cartesiana
5.7 Resumen
Objetivos
Presentar el análisis de cinemática directa cartesiana, cinemática inversa y
matriz jacobiano de las principales configuraciones de robots industriales
considerando los parámetros geométricos y desarrollar librerı́as en
lenguaje MATLAB (toolbox) que permitan realizar aplicaciones en el área
de cinemática directa de robots manipuladores.
Objetivos particulares:
Modelo de cinemática directa.
Modelo de cinemática inversa.
Jacobiano.
Librerı́as de análisis y diseño de cinemática de robots industriales.
5.1
201
Introducción
5.1 Introducción
E
sta sección analiza la cinemática directa cartesiana de las configuraciones más
importantes en robótica industrial tales como el brazo robot (antropomórfico),
configuración SCARA, esférico, cilı́ndrico y cartesiano. El análisis cinemático que se
presenta es tomando en cuenta la relación que existe entre las coordenadas articulares
con las coordenadas cartesianas del extremo final del robot (cinemática directa
cartesiana), no se incluye la orientación de la herramienta de trabajo ubicada en
el extremo final del robot.
Para el análisis cinemático cartesiano se utiliza la metodologı́a Denavit-Hartenberg,
y por lo tanto dicho modelo queda en función de transformaciones homogéneas.
El jacobiano analı́tico del robot se deduce como la derivada parcial de la cinemática
directa cartesiana con respecto al vector de posición articular; el jacobiano analı́tico
del robot proporciona información sobre el problema de singularidades que puede
presentar el movimiento del robot en su espacio de trabajo. Esta información es muy
importante en aplicaciones donde interviene la inversa de la matriz jacobiana, ya que
una singularidad significa que el determinante es cero para un conjunto especı́fico
de valores de las posiciones articulares.
Debido a la importancia que tiene la cinemática inversa en la programación de tareas
en espacio cartesiano y la conversión al espacio articular, se presenta la solución
geométrica en detalle para cada una de las configuraciones de robots industriales.
Un conjunto de librerı́as en código fuente de MATLAB se desarrollan y documentan
de la matriz de transformación homogénea, cinemática directa cartesiana, jacobiano
y su determinante, cinemática inversa, en variables simbólicas y aplicaciones
numéricas de todas las configuraciones analizadas. El código fuente de los ejemplos,
aplicaciones y librerı́as de cada robot manipulador se encuentra disponible en el sitio
WEB del libro:
http://virtual.alfaomega.com.mx
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
202
Capı́tulo 5: Cinemática directa cartesiana
5.2 Brazo robot antropomórfico
D
entro de la clasificación de robots industriales, el robot antropomórfico
o brazo robot es la configuración que más se utiliza debido a la destreza
que presenta el movimiento del extremo final como una consecuencia de sus
tres articulaciones rotacionales, lo cual lo hace ideal para un amplio espectro de
aplicaciones tales como: industriales, quirófanos robotizados, fisioterapia, asistencia
a personas con capacidades diferenciadas, maniobras submarinas y espaciales, etc.
La configuración antropomórfica se divide en tres casos de estudio: péndulo robot,
robot planar de 2 grados de libertad y el brazo robot (movimiento tridimensional).
Péndulo robot
Como un caso particular del robot de 3 grados de libertad antropomórfico, se
encuentra el péndulo robot de 1 grado de libertad formado por un servomotor que
tiene acoplado mecánicamente una barra metálica de longitud l1 y está sometido al
fenómeno de gravedad. El movimiento del péndulo se encuentra en el plano vertical
x0 − y0 como se indica en la figura 5.1.
Figura 5.1 Péndulo robot.
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
5.2
203
Brazo robot antropomórfico
Cinemática directa cartesiana del péndulo robot
El origen del sistema de referencia cartesiano Σ0 (x0 , y0 , z0 ) se coloca sobre la
articulación del péndulo (en el respaldo del estator del servomotor), el eje z0 se
alinea con el eje de giro del servomotor el cual es perpendicular al plano x0 − y0 .
El servomotor del péndulo tiene un espesor de longitud β1 el cual ya incluye el
ancho de la barra metálica. El origen del sistema de referencia Σ1 (x1 , y1 , z1 ) se
coloca en el extremo final de la barra y se mueve de manera conjunta al péndulo, el
origen Σ1 tiene las siguientes coordenadas con respecto al origen de Σ0 (x0 , y0 , z0 ):
[l1 cos(q1 ), l1 sen(q1 ), β1 ]. Debido al espesor β1 el origen de Σ1 mantiene una distancia
β1 sobre el eje z0 . Los ejes z1 y z0 son paralelos entre sı́, por lo tanto el ángulo α1
que existe entre ellos es α1 = 0.
El plano x1 − y1 tiene una rotación de q1 grados con respecto al plano x0 − y0 del
sistema fijo Σ0 (x0 , y0 , z0 ), y la matriz Rz0 ( q1 ) determina dicha rotación:
⎡
− sen(q1 ) 0
cos(q1 )
⎢
R10 = Rz0 ( q1 ) = ⎣ sen(q1 )
cos(q1 )
0
0
⎤
⎥
0⎦.
1
Los parámetros Denavit-Hartenberg correspondientes al péndulo-robot se encuentran especificados en la tabla 5.1.
Tabla 5.1 DH del péndulo
Eslabón
l
α
d
θ
1
l1
0
β1
q1
La matriz de transformación homogénea H01 se obtiene de la ecuación (4.7), que para
el caso del péndulo-robot adquiere la siguiente forma:
H01 = HR (z0 , q1 )HT (z0 , β1 )HT (x0 , l1)HR (x0 , 0)
⎡
⎤
cos(q1 ) − sen(q1 ) 0 l1 cos(q1 )
⎢ sen(q )
cos(q1 )
0 l1 sen(q1 ) ⎥
1
⎢
⎥
= ⎢
⎥.
⎣
⎦
0
0
1
β1
0
0
0
(5.1)
(5.2)
1
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
204
Capı́tulo 5: Cinemática directa cartesiana
Las coordenadas cartesianas del extremo final del péndulo robot (cinemática directa
cartesiana), es decir el origen del sistema Σ1 (x1 , y1 , z1 ) con respecto al sistema fijo
Σ0 (x0 , y0 , z0 ) se encuentra determinado por:
⎡
x0
⎤
⎡
l1 cos(q1 )
⎤
⎢ ⎥
⎢
⎥
⎣ y0 ⎦ = ⎣ l1 sen(q1 ) ⎦ .
z0
β1
(5.3)
Las coordenadas cartesianas del extremo final del péndulo dependen de la ubicación
de la posición de casa (home position), la cual es el punto de reposo u origen del
péndulo que corresponde a q1 = 0. Como se puede apreciar en la figura 5.1, la
posición de casa está colocada sobre el eje x0+ . La matriz de rotación Rz0 ( q1 )
que determina la rotación entre los sistemas de referencia fijo Σ0 (x0 , y0 , z0 ) y
Σ1 (x1 , y1 , z1 ) fue obtenida con respecto al primer cuadrante, es decir en el plano
x0+ − y0+ .
Otra posibilidad para ubicar la posición de casa del péndulo es colocar el origen
q1 = 0 sobre el lado negativo del eje y0 como se muestra en la figura 5.2.
Figura 5.2 Cambio de posición de casa del péndulo sobre el eje y0− .
En este caso, para poder obtener la matriz de rotación resultante del péndulo en la
nueva ubicación de la posición de casa, la matriz Rz0 ( q1 ) debe ser precedida por
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
5.2
205
Brazo robot antropomórfico
una rotación de un ángulo igual a - π2 alrededor del eje z0 , es decir:
⎡
cos(q1 )
⎢
R10 = Rz0 ( − π2 ) ⎣ sen(q1 )
− sen(q1 ) 0
cos(q1 )
⎤
⎥
0⎦
0
⎤⎡
0
1
⎤
0 1 0
cos(q1 ) − sen(q1 ) 0
⎢
⎥⎢
⎥
= ⎣ −1 0 0 ⎦ ⎣ sen(q1 )
cos(q1 )
0⎦
0 0 1
0
0
1
⎡
⎤
sen(q1 ) cos(q1 ) 0
⎢
⎥
= ⎣ − cos(q1 ) sen(q1 ) 0 ⎦
0
0
1
⎡
La matriz de transformación homogénea para
está determinada como:
⎡
cos(q1 ) − sen(q1 )
⎡
π ⎢
cos(q1 )
⎢ Rz0 ( − 2 ) ⎣ sen(q1 )
H01 = ⎢
0
0
⎣
el péndulo-robot (IV cuadrante)
0
⎤
⎡
⎥
0⎦
1
Rz0 ( − π2
0T
⎡
sen(q1 )
⎢ − cos(q )
1
⎢
= ⎢
⎣
0
0
cos(q1 )
1
0
l1 sen(q1 )
⎤
sen(q1 ) 0 −l1 cos(q1 ) ⎥
⎥
⎥.
⎦
0
1
β1
0
⎤
⎤
⎥
⎢
) ⎣ l sen(q) ⎦ ⎥
⎥(5.4)
β1
⎦
l cos(q)
0
(5.5)
1
Las coordenadas del extremo final del péndulo respecto a la posición de casa colocada
sobre el eje y0− adquiere la siguiente forma:
⎡
x0
⎤
⎡
l1 sen(q1 )
⎤
⎢ ⎥
⎢
⎥
⎣ y0 ⎦ = ⎣ −l1 cos(q1 ) ⎦
β1
z0
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
(5.6)
Alfaomega
206
Capı́tulo 5: Cinemática directa cartesiana
Jacobiano del péndulo
Debido a que el péndulo robot es un sistema escalar, no tiene matriz jacobiana. Sin
embargo, se puede realizar la siguiente interpretación partiendo de la cinemática
diferencial:
−l1 sen(q1 )
d l1 cos(q1 )
∂f R (q1 )
v =
=
q̇1
q̇1 =
dt l1 sen(q1 )
∂q1
l1 cos(q1 )
q̇1
−l1 sen(q1 )
0
=
(5.7)
= J(q1 )q̇
q̇
0
l cos(q1 )
1
1
J(q1 )
q̇
el jacobiano J(q1 ) se puede pensar como está expresado en la ecuación (5.7). Nótese
que existe una singularidad cuando la variable articular q1 = 0, ±nπ, ya que su
determinante está dado como: det[J(q1 )] = −l12 sen(q1 ) cos(q1 ). También existen
singularidades en q1 = ±n π2 , esto último puede verificarse en la cinemática inversa
(5.8), si q1 = ±n π2 ⇒ x0 = 0, entonces se indefine la función arco-tangente (por
ejemplo división entre cero).
Cinemática inversa del péndulo
La cinemática inversa se obtiene despejando la variable q1 de la ecuación (5.3),
obteniendo:
y0
q1 = atan
.
(5.8)
x0
Función transformación homogénea del péndulo H01
La función transformación homogénea del péndulo H01 se encuentra expresada como:
H01 =H pendulo()
El código MATLAB de la función transformación homogénea del péndulo se describe
en el cuadro 5.1. En la lı́nea 5 despliega la tabla 5.1 de parámetros DH. La lı́nea 7
contiene el cálculo de la matriz homogénea H01 = HRz0 (q1 )HTz0 (β1 )HTx0 (l1 )HRx0 (0)
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
5.2
Brazo robot antropomórfico
207
y en la lı́nea 8 se emplea la función H DH(H10) para obtener la matriz de rotación y
las coordenadas cartesianas.
Código Fuente 5.1
H pendulo.m
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 5 Cinemática directa cartesiana.
H pendulo.m
1
2
function H=H pendulo()
syms l1 q1 beta1 real
3
5
disp(’Parámetros Denavit-Hartenberg del péndulo’)
disp(’[ l alpha d q]’) dh=[l1, 0, beta1, q1];;
disp(dh)
6
%H01 = HRz0 (q1 )HTz0 (β1 )HTx0 (l1 )HRx0 (0)
7
H10=HRz(q1)*HTz(beta1)*HTx(l1)*HRx(0) ;
[R10, cinemat pendulo, cero, c]=H DH(H10);
H=[R10, cinemat pendulo;;
cero, c];
4
8
9
10
11
end
Función cinemática directa cartesiana del péndulo
La función de cinemática directa cartesiana del péndulo tiene la siguiente sintaxis:
[x0 , y0 , z0 ]=cinematica pendulo(β1, l1, q1 )
donde β1 , l1, q1 representan el ancho del servomotor y espesor de la barra metálica,
longitud de la barra metálica y posición articular del péndulo, respectivamente. Esta
función retorna las coordenadas cartesianas del extremo final del péndulo ubicadas
en el sistema Σ0 (x0 , y0 , z0 ).
El código de la función de cinemática cartesiana del péndulo se encuentra en el
cuadro 5.2; se contempla el caso simbólico y numérico.
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
208
Capı́tulo 5: Cinemática directa cartesiana
Código Fuente 5.2
cinematica pendulo.m
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 5 Cinemática directa cartesiana.
cinematica pendulo.m
1
2
3
function [x0, y0, z0]=cinematica pendulo(beta1,l1,q1)
dato1=whos(’beta1’); dato2=whos(’l1’); dato3=whos(’q1’);
v1=strcmp(dato1.class, ’sym’); v2=strcmp(dato2.class, ’sym’);
4
v3=strcmp(dato3.class, ’sym’); digits(3);
if (v1 & v2 & v3) %caso simbólico
x0=l1*cos(q1);
y0=l1*sin(q1);
5
6
7
8
z0=beta1;
9
end
else %caso numérico
10
11
x0=double(simplify(vpa(l1*cos(q1),3)));
y0= double(simplify(vpa(l1*sin(q1),3)));
z0=double(simplify(vpa(beta1,3)));
12
13
14
end
Función cinemática inversa del péndulo
La función de cinemática inversa del péndulo se encuentra expresada como:
q1 =cinv pendulo(x0 , y0 )
donde x0 , y0 representan las coordenadas del extremo final del péndulo con respecto
al sistema Σ0 (x0 , y0 , z0 ). Esta función retorna la posición articular q1 del péndulo;
observe que el modelo de cinemática inversa no depende del grosor del servomotor
(β1 ).
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
5.2
209
Brazo robot antropomórfico
El código de la función de cinemática inversa se encuentra en el cuadro 5.3.
Código Fuente 5.3
cinv pendulo.m
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 5 Cinemática directa cartesiana.
cinv pendulo.m
1
function q1=cinv pendulo(x0,y0)
2
%cinemática inversa del péndulo
3
q1=atan(y0/x0);
4
end
♣ Ejemplo 5.1
Desarrollar un programa para MATLAB que presente de manera simbólica los
parámetros DH, cinemática cartesiana y el jacobiano del péndulo. Además,
contemple el análisis donde la posición de casa se traslada del primer al cuarto
cuadrante.
Solución
En el cuadro 5.4 se presenta el programa que permite desplegar en forma sı́mbolica
los parámetros DH, cinemática cartesiana y el jacobiano del péndulo.
La lı́nea 7 obtiene la transformación homogénea del péndulo H01 empleando la
función H01 =H pendulo(). La lı́nea 9 deduce la matriz de rotación R10 y la cinemática
cartesiana f R (q1 ) a través de la función H DH(H01 ).
En la lı́nea 11 se realiza una rotación HRz0 ( − π2 ) (-90 grados alrededor del eje z0 )
para trasladar la posición de casa al cuarto cuadrante. De la lı́nea 13 a la 20 se
presenta un ejemplo numérico, para trasladar las coordenadas del extremo final en
la rotación del primer al cuarto cuadrante.
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
210
Capı́tulo 5: Cinemática directa cartesiana
El programa 5.4 contiene la descripción completa del modelo cinemático directo del
péndulo robot, el cual emplea las funciones de transformación homogénea H01 incluye
la matriz de rotación R10 que relaciona la rotación del extremo final del eslabón con
respecto a la base del péndulo, ası́ como la cinemática directa cartesiana en variables
simbólicas como aplicación numérica.
Código Fuente 5.4
cap5 pendulo.m
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 5 Cinemática directa cartesiana.
cap5 pendulo.m
1
4
clc;
clear all;
close all;
format short
5
syms beta1 l1 q1 real
6
7
%H01 = HRz0 (q1 )HTz0 (β1 )HTx0 (l1 )HRx0 (0)
H10=H pendulo() %H01
8
%obtiene la matriz de rotación R10 y cinemática directa f r (q)
2
3
11
[R10, frq pendulo, cero, c]=H DH(H10) %R10 , f R (β1 , l1 , q1 )
jac pendulo=jacobian(frq pendulo, q1)
H10a=HRz(-pi/2)*H10 %H01 a = HRz0 ( − π2 ) H01
12
%ejemplo numérico
13
q1=55*pi/180; %posición angular del péndulo
beta1=0.1; %ancho del servomotor más espesor de la barra
l1=0.45; beta1=0.1; %longitud del péndulo y espesor del servomotor
9
10
14
15
16
17
%cinemática cartesiana cuadrante I: f RI (β1 , l1 , q1 )
[x0,y0,z0]=cinematica pendulo(beta1,l1,q1)
18
%cinemática directa en el cuadrante IV
19
%f RIV (β1 , l1 , q1 ) = Rz (− π2 )f RI (β1 , l1 , q1)
20
Rz(-pi/2)*cinematica pendulo(beta1,l1,q1)
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
5.2
Brazo robot antropomórfico
211
Robot planar vertical de dos grados de libertad
La figura 5.3 muestra un robot de dos grados de libertad con articulaciones
rotacionales que se mueve en el plano vertical x0 − y0 . El sistema de referencia
fijo Σ0 (x0 , y0 , z0 ) se coloca en el respaldo del servomotor del hombro, de tal forma
que el eje z0 coincida con el eje de rotación (perpendicular al plano de la hoja). El
ancho de cada servomotor y espesor de la barra metálica están determinados por β1
y β2 . Los ejes x0 , y0 se seleccionan con la regla de la mano derecha.
Cinemática directa cartesiana del brazo robot de 2 gdl
El sistema de referencia Σ1 (x1 , y1 , z1 ) se coloca en el extremo final del primer eslabón,
el eje z1 se coloca paralelo al eje z0 (α1 = 0). El origen del sistema Σ1 (x1 , y1 , z1 ) se
ubica en la intersección del eje x0 con el eje z1 (cuando q1 = 0) y está a una distancia
β1 sobre el eje z0 . El origen del sistema de referencia Σ2 (x2 , y2 , z2 ) se coloca en el
extremo final del robot. El ancho del servomotor del codo y el espesor de la segunda
barra miden β2 . Cuando q2 = 0 el origen de Σ2 se encuentra a una distancia l2 sobre
el eje x1 y a una distancia β2 sobre el eje z1 . Los ejes xi , yi , con i = 1, 2 se seleccionan
con la regla de la mano derecha. Los ejes z1 y z2 son paralelos entre sı́ (α2 = 0).
Figura 5.3 Robot planar vertical de dos gdl.
En la tabla 5.2 se muestran los parámetros de los eslabones para la convención
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
212
Capı́tulo 5: Cinemática directa cartesiana
Denavit-Hartenberg de un robot planar de dos grados de libertad.
Tabla 5.2 DH del robot de 2 gdl
Eslabón
li
αi
di
θi
1
l1
0
β1
q1
2
l2
0
β2
q2
Generalmente la posición de casa para un robot de dos grados de libertad se
selecciona sobre el eje x0+ , midiendo el ángulo q1 en sentido contrario a las manecillas
del reloj, es decir del eje x0+ hacia el eje y0+ , con esta consideración se obtienen las
siguientes matrices de transformación homogénea:
H01 = HRz0 ( q1 ) HTz0 ( β1 ) HTx0 ( l1 ) HRx0 ( 0 )
⎡
⎤
cos(q1 ) − sen(q1 ) 0 l1 cos(q1 )
⎢ sen(q )
cos(q1 )
0 l1 sen(q1 ) ⎥
1
⎢
⎥
= ⎢
⎥
⎣
⎦
0
0
1
β1
0
0
0
0
H02 = H01 H12
⎡ cos(q1 + q2 )
= ⎣
(5.10)
1
H12 = HRz1 ( q2 ) HTz1 ( β2 ) HTx1 ( l2 ) HRx1 ( 0 )
⎡
⎤
cos(q2 ) − sen(q2 ) 0 l2 cos(q2 )
⎢ sen(q )
cos(q2 )
0 l2 sen(q2 ) ⎥
2
⎢
⎥
= ⎢
⎥
⎣
⎦
0
0
1
β2
0
(5.9)
0
(5.11)
(5.12)
1
sen(q1 + q2 )
0
− sen(q1 + q2 )
cos(q1 + q2 )
0
0
0
1
0
0
0
l1 cos(q1) + l2 cos(q1 + q2 ) ⎤
l1 sen(q1) + l2 sen(q1 + q2 )
⎦.
β1 + β2
(5.13)
1
La cinemática directa de las coordenadas cartesianas del extremo final del robot (sin
tomar en cuenta la orientación de la herramienta de trabajo) está dada por:
⎡ ⎤
⎤
⎡
x0
l1 cos(q1 ) + l2 cos(q1 + q2 )
⎢ ⎥
⎥
⎢
(5.14)
⎣ y0 ⎦ = f R (q) = ⎣ l1 sen(q1 ) + l2 sen(q1 + q2 ) ⎦ .
z0
Alfaomega
β1 + β2
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
5.2
213
Brazo robot antropomórfico
Obsérvese que la coordenada z0 incluye el espesor de los dos servomotores con sus
respectivos espesores de las barras metálicas, de tal forma que el origen del sistema
Σ2 (x2 , y2 , z2 ) se encuentra a una distancia β1 + β2 sobre el eje z0 del sistema fijo
Σ0 (x0 , y0 , z0 ).
La posición de casa de un robot manipulador planar de dos grados de libertad
también puede ser ubicada sobre el eje y0− . Esto equivale a hacer una rotación
alrededor del eje z0 por -90 grados para la articulación del hombro, es decir:
HRz0 ( − π2 ) H01 y también para articulación del codo HRz1 ( − π2 ) H12 . El ángulo q1 se
mide del eje y0− hacia el eje x0+ (valor positivo en sentido contrario a las manecillas
del reloj) como se muestra en la figura 5.4.
Figura 5.4 Rotación de la posición de casa por -90 grados alrededor del eje z0 .
H01 y0−
π
H01
= H R z0 −
2
⎡ 0 1 0 0⎤⎡
−1
= ⎣ 0
0
0
0
0
0
1
0
cos(q1)
0
⎦ ⎣ sen(q1 )
0
0
1
0
⎤
− sen(q1)
0
l1 cos(q1 )
cos(q1 )
0
0
0
1
0
l1 sen(q1 )
⎦
β1
1
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
214
Capı́tulo 5: Cinemática directa cartesiana
⎡
sen(q1 )
cos(q1 )
⎢ − cos(q )
1
⎢
= ⎢
⎣
0
H12 y1−
sen(q1 ) 0
0
1
l1 sen(q1 )
⎣ −1
0
⎡
0
0
0
0
1
0
⎦ ⎣ sen(q2)
0
0
0
0
1
sen(q2 )
⎢ − cos(q )
2
⎢
= ⎢
⎣
0
=
=
⎣ − cos(q1 + q2 )
0
0
⎡
sen(q1 + q2 )
0
0
1
0
0
0
sen(q2 ) 0
0
H01 y0− H12 y1−
cos(q2 )
0
0
cos(q2 )
0
1
0
0
⎤
−l1 cos(q1 ) ⎥
⎥
⎥
⎦
β1
0
0
0
1
π
H12
= H R z1 −
2
⎡ 0 1 0 0 ⎤ ⎡ cos(q2 ) − sen(q2 )
=
H02 y0−
0
l2 cos(q2 ) ⎤
l2 sen(q2 )
⎦
β2
l2 sen(q2 )
⎤
1
−l2 cos(q2 ) ⎥
⎥
⎥
⎦
β2
1
⎤
cos(q1 + q2 )
0
l1 sen(q1 ) + l2 sen(q1 + q2 )
sen(q1 + q2 )
0
0
0
1
0
−l1 cos(q1 ) − l2 cos(q1 + q2 )
⎦
β1 + β2
1
El mismo resultado se puede obtener premultiplicando la matriz homogénea de
rotación HRz0 ( − π2 ) a la matriz de transformación homogénea H02 (5.13). Es decir,
H02 y0− = HRz0 ( − π2 ) H02 .
Por lo que la cinemática directa de coordenadas cartesianas para un robot planar
de dos grados de libertad cuya posición inicial es sobre el eje y0 negativo:
⎡
x0
⎤
⎡
l1 sen(q1 ) + l2 sen(q1 + q2 )
⎤
⎢ ⎥
⎢
⎥
⎣ y0 ⎦ = ⎣ −l1 cos(q1 ) − l2 cos(q1 + q2 ) ⎦
β1 + β2
z0
(5.15)
Debido a que la rotación se realiza alrededor de los ejes z1 y z2 , ambos paralelos al
eje z0 , no existe ninguna modificación de coordenadas de los parámetros geométricos
β1 y β2 . Por lo que el origen del sistema Σ2 (x2 , y2 , z2 ) siempre tendrá una distancia
β1 + β2 sobre el eje z0 .
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
5.2
215
Brazo robot antropomórfico
Jacobiano del brazo robot de 2 gdl
El jacobiano analı́tico del robot de 2 gdl se obtiene como:
J(q) =
∂f R (β1 , β2, l1 , l2, q)
∂q
tomando en cuenta que los parámetros geométricos β1 , β2 , l1, l2 son constantes, y
considerando la posición de casa sobre el eje x0 (primer cuadrante):
J(q) =
−l1 sen(q1 ) − l2 sen(q1 + q2 ) −l2 sen(q1 + q2 )
l1 cos(q1 ) + l2 cos(q1 + q2 )
l2 cos(q1 + q2 )
∈ IR2×2 (5.16)
El determinante del jacobiano es det[J(q)] = l1 l2 sen(q2 ), el cual es cero para
q2 = 0, ±nπ y q1 cualquier valor. Por lo tanto, cuando la articulación del codo
tiene alguno de esos valores para q2 el robot entra en una singularidad. Esto es
importante en aplicaciones donde interviene q̇ = J(q)−1 v.
Cinemática inversa del brazo robot de 2 gdl
La cinemática inversa (sin tomar en cuenta la orientación de la herramienta
de trabajo) de un robot manipulador de dos grados de libertad se obtiene por
procedimiento geométrico (ver figura 5.5).
Figura 5.5 Cinemática inversa de un robot planar de dos grados de libertad.
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
216
Capı́tulo 5: Cinemática directa cartesiana
El ángulo ϑ que se encuentra dentro del triángulo formado por los lados adyacente
l1 + l2 cos(q2 ), cateto opuesto l2 sen(q2 ) y la hipotenusa x20 + y02 , del teorema de
Pitágoras se obtiene la solución para la variable articular q2 :
x20 + y02 = [l1 + l2 cos(q2 )]2 + l22 sen2 (q2 )
:1
2
2
2
2
= l1 + l2 [cos (q
+ sen (q2 )] + 2l1 l2 cos(q2 )
2 )
= l12 + l22 + 2l1 l2 cos(q2 )
q2 = acos (
x20 +y02 −l21 −l22
2l1l2
)
Observe que el ángulo ϑ satisface:
ϑ =
l2 sen(q2 )
atan
l1 + l2 cos(q2 )
Ahora, tomando los ángulos ϑ + q1 dentro del triángulo formado por los catetos
adyacente x0 , opuesto y0 y la hipotenusa x20 + y02 se cumple la siguiente expresión:
y0
ϑ + q1 = atan
x0
Entonces, la variable articular q1 adquiere la siguiente forma:
y0
q1 = atan
−ϑ
x0
l2 sen(q2 )
y0
− atan
= atan
x0
l1 + l2 cos(q2 )
Por lo tanto, la cinemática inversa de un robot de 2 gdl no depende de β1 y β2
debido a que no hay proyecciones de estos parámetros sobre el plano x0 − y0 :
q2 = acos (
q1 =
Alfaomega
atan
x20 +y02 −l21 −l22
2l1 l2
y0
x0
)
− atan
l2 sen(q2 )
l1 + l2 cos(q2 )
(5.17)
(5.18)
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
5.2
217
Brazo robot antropomórfico
Función transformación homogénea del robot 2 gdl H02
La función transformación homogénea H02 del robot planar vertical de 2 gdl se
encuentra expresada como:
H02 =H r2gdl()
esta función retorna H02 la transformación homogénea del robot planar vertical de 2
gdl. El código MATLAB de la función transformación homogénea H02 del robot de
2 gdl se encuentra en el cuadro 5.5.
Código Fuente 5.5
H r2gdl.m
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 5 Cinemática directa cartesiana.
H r2gdl.m
1
2
3
function H=H r2gdl()
syms q1 q2 beta1 beta2 l1 l2 alpha1 alpha2 real
disp(’Parámetros Denavit-Hartenberg del robot planar vertical de 2 gdl’)
4
disp(’[ l alpha d q]’)
dh=[l1, 0, beta1, q1; l2, 0, beta2, q2];
disp(dh)
5
6
%H01 = HRz0 (q1 )HTz0 (β1 )HTx0 (l1 )HRx0 (0)
H10=HRz(q1)*HTz(beta1)*HTx(l1)*HRx(0)
7
8
%H12 = HRz0 (q2 )HTz0 (β2 )HTx0 (l2 )HRx0 (0)
H21=HRz(q2)*HTz(beta2)*HTx(l2)*HRx(0)
H20=simplify(H10*H21); % H02 = H01 H12
[R20, cinemat r2gdl, cero, c]=H DH(H20)
9
10
11
12
H=[R20, cinemat r2gdl; % R20 (q1 , q2 ), f R (q1 , q2)
cero, c];
13
14
15
end
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
218
Capı́tulo 5: Cinemática directa cartesiana
Función cinemática directa del robot de 2 gdl
La función de cinemática directa de un robot de 2 gdl se encuentra dada por:
[x0 , y0 , z0 ]=cinematica r2gdl(β1, l1 , q1 , β2, l2 , q2 )
donde los argumentos de entrada son los parámetros geométricos β1 , l1, β2 , l2 y
posiciones articulares q1 , q2 del hombro y codo, respectivamente. Esta función retorna
las coordenadas cartesianas (x0 , y0 , z0) en el sistema Σ0 (x0 , y0 , z0 ).
Código Fuente 5.6
cinematica r2gdl.m
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 5 Cinemática directa cartesiana.
cinematica r2gdl.m
1
2
3
function [x0, y0, z0]=cinematica r2gdl(beta1,l1,q1,beta2,l2,q2)
dato1=whos(’beta1’); dato2=whos(’l1’); dato3=whos(’q1’);
dato4=whos(’beta2’); dato5=whos(’l2’); dato6=whos(’q2’);
4
v1=strcmp(dato1.class, ’sym’); v2=strcmp(dato2.class, ’sym’);
v3=strcmp(dato3.class, ’sym’); v4=strcmp(dato4.class, ’sym’);
v5=strcmp(dato5.class, ’sym’); v6=strcmp(dato6.class, ’sym’);
digits(3);
5
6
7
8
if (v1 & v2 & v3 & v4 &v5 & v6) %caso simbólico
x0=simplify(vpa(l1*cos(q1)+l2*cos(q1+q2),3));
y0=simplify(vpa(l1*sin(q1)+l2*sin(q1+q2),3));
9
10
11
14
z0=vpa(beta1+beta2,3);
else %caso numérico
x0=l1*cos(q1)+l2*cos(q1+q2);
y0=l1*sin(q1)+l2*sin(q1+q2);
15
z0=beta1+beta2;
12
13
16
17
end
end
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
5.2
Brazo robot antropomórfico
219
Función cinemática inversa del robot de 2 gdl
La función de cinemática inversa de un robot de 2 gdl tiene la siguiente sintaxis:
[q1 , q2 ]=cinv r2gdl(l1, l2, x0 , y0 )
donde los argumentos de entrada son los parámetros geométricos l1 , l2 y las
coordenadas cartesianas del extremo final x0 , y0 en el sistema Σ0 (x0 , y0 , z0 ). Esta
función retorna las coordenadas articulares del hombro q1 y codo q2 .
Código Fuente 5.7
cinv r2gdl.m
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 5 Cinemática directa cartesiana.
cinv r2gdl.m
1
function [q1 q2]=cinv r2gdl(l1,l2,x0,y0)
2
q2=acos((x0.*x0+y0.*y0-l1*l1-l2*l2)/(2*l1*l2));
q1=atan(y0./x0)-atan((l2*sin(q2))./(l1+l2*cos(q2)));
3
4
end
♣ ♣ Ejemplo 5.2
Diseñar un programa para MATLAB que permita desplegar en forma simbólica
los parámetros DH, cinemática cartesiana y el jacobiano del robot vertical planar
de 2 gdl. Además, programar una aplicación donde el extremo final del robot
trace un cı́rculo de radio r = 0.2m, con centro en [ xc , yc ]T = [ 0.3, −0.3 ]T m, con
periodo de movimiento de 6.28 segundos.
Solución
El programa 5.8 contiene el código fuente para MATLAB que soluciona el problema
planteado. En la lı́nea 4 se obtiene la matriz de transformación homogénea H02 del
robot de 2 gdl y en la lı́nea 7 se obtiene la matriz de rotación R20 y la cinemática
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
220
Capı́tulo 5: Cinemática directa cartesiana
directa cartesiana f R (l1 , q1 , l2 , q2 ). Las lı́neas 12 y 13 despliegan en forma simbólica
el jacobiano y su determinante, respectivamente. La ecuación del cı́rculo se encuentra
implementada en las lı́neas 21 y 22:
x
y
=
xc + r sen(t)
yc + r cos(t)
donde xc , yc representan el centro del cı́rculo, r es el radio.
La lı́nea 24 convierte las coordenadas cartesianas del cı́rculo en coordenadas
articulares (q1 , q2 ) por medio de la función cinv r2gdl(l1, l2 , x, y), y en la lı́nea
26 se transforman las coordenadas articulares (q1 , q2 ) en las coordenadas cartesianas
del extremo final del robot en el sistema Σ0 (x0 , y0 , z0 ). Se genera una base de tiempo
de 100 segundos con incrementos de 1 mseg. El periodo de trazo del cı́rculo es de
6.28 segundos ( sen(wt), con w = 2π f = 1, entonces t = 2π segundos).
La figura 5.6 muestra el cı́rculo generado por el extremo final del robot (ver lı́nea
27) durante una simulación de 100 segundos, lo que significa que el robot traza 15
cı́rculos que se superponen en la misma figura.
Figura 5.6 Trayectoria circular que traza el robot de 2 gdl.
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
5.2
221
Brazo robot antropomórfico
Código Fuente 5.8
cap5 robot2gdl.m
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 5 Cinemática directa cartesiana.
cap5 robot2gdl.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
clc; clear all ;close all;
format short
syms q1 q2 beta1 beta2 l1 l2 alpha1 alpha2 real
H20=H r2gdl()
disp(’Transformación homogénea del robot 2 gdl’);
disp(H20);
[R20, cinemat r2gdl,cero, c]=H DH(H20) ;
disp(’Matriz de rotación’); disp(R20);
disp(’cinemática directa’);
disp(cinemat r2gdl);
[x0, y0, z0]=cinematica r2gdl(beta1,l1,q1,beta2,l2,q2)
jac r2gdl=jacobian([x0; y0], [q1;q2])
det r2gdl=simplify(det(jac r2gdl)) % det[J]= l1 l2 sen(q2 )
%ejemplo numérico
t=0:0.001:100;%parámetros del cı́rculo: [ xc , yc ]T = [ 0.3, −0.3 ]T y radio r = 0.2
18
xc=0.3; yc=-0.3; r=0.20;
l1=0.45; l2=0.45;
beta1=0.1; beta2=0.1;
19
q1=[]; q2=[];
20
22
% ecuación del cı́rculo
x=xc+r*sin(t) ;
y=yc+r*cos(t) ;
23
% cinemática inversa
24
[q1,q2]=cinv r2gdl(l1,l2,x,y) ;
25
%coordenas cartesianas del extremo final del robot de 2 gdl
[x0, y0, z0]=cinematica r2gdl(beta1,l1,q1,beta2,l2,q2) ;
plot(x0,y0)
17
21
26
27
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
222
Capı́tulo 5: Cinemática directa cartesiana
Robot antropomórfico (RRR)
La gran mayorı́a de los robots industriales tienen la configuración antropomórfica
(brazos robots) ya que presentan mayor destreza en su espacio de trabajo debido a
que sus eslabones están unidos por articulaciones rotacionales. El espacio de trabajo
de la configuración antropomórfica corresponde a una esfera, cuyo radio corresponde
a la suma de longitudes de los eslabones. Por similitud con la forma anatómica con
el brazo humano, la segunda articulación se conoce como hombro (shoulder) y
la tercera articulación se llama codo (elbow). La figura 5.7 muestra ejemplos de
robots industriales antropomórficos de la compañı́a ABB y FANUC que se utilizan
en aplicaciones de traslado de objetos.
Figura 5.7 Robots industriales en configuración antropomórfica: ABB y FANUC.
Cinemática directa cartesiana del brazo robot de 3 gdl
Considérese un robot manipulador antropomórfico de tres grados de libertad como
el que se presenta en la figura 5.8, donde el sistema de referencia fijo Σ0 (x0 , y0 , z0 )
se encuentra en la base del robot, el eje z0 coincide con el eje de rotación de la
articulación de la base. Los sistemas de referencia Σ1 (x1 , y1 , z1 ), Σ2 (x2 , y2 , z2 ) y
Σ3 (x3 , y3 , z3 ) están seleccionados de tal forma que sus ejes z1 , z2 y z3 coincidan
con sus respectivos ejes de rotación de cada articulación. El eje z1 del sistema
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
5.2
223
Brazo robot antropomórfico
Σ1 (x1 , y1 , z1 ) se encuentra ortogonal al eje z0 (α1 = π2 ); el eje z1 es paralelo al
eje z2 (α2 = 0) y el eje z2 se ha considerado paralelo al eje zh de la herramienta de
trabajo (α3 = 0). Note que el plano x2 − y2 se encuentra rotado un ángulo q3 con
respecto al plano x1 − y1 .
Figura 5.8 Robot antropomórfico.
La tabla 5.3 contiene los parámetros Denavit-Hartenberg para el robot antropomórfico de tres grados de libertad.
Tabla 5.3 DH del robot de 3 gdl
li
αi
di
θi
1
0
π
2
l 1 + β1
q1
2
l2
0
β2
q2
3
l3
0
β3
q3
Eslabón
Los parámetros geométricos de cada servomotor son: β1 , β2 , β3 representan el ancho
y espesor de las placas metálicas para la base, hombro y codo, respectivamente; l1
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
224
Capı́tulo 5: Cinemática directa cartesiana
es la distancia que se encuentra la articulación de la base con respecto al origen del
sistema fijo Σ0 (x0 , y0 , z0 ); l2 y l3 son las longitudes de los eslabones del hombro y
codo, respectivamente.
Las matrices de transformaciones homogéneas para la configuración antropomórfica
adquieren la siguiente forma:
H01 = HRz0 ( q1 ) HTz0 ( l1 + β1 ) HTx0 ( 0 ) HRx0 ( π2 )
⎤
⎡ cos(q1 ) 0 sen(q1)
0
= ⎣
H12
sen(q1 )
0
0
1
− cos(q1 )
0
0
0
0
(5.19)
0
⎦
l 1 + β1
(5.20)
1
= HRz1 ( q2 ) HTz1 ( β2 ) HTx1 ( l2 ) HRx1 ( 0 )
⎡ cos(q2 ) − sen(q2 ) 0 l2 cos(q2 ) ⎤
= ⎣
sen(q2 )
cos(q2 )
0
l2 sen(q2 )
0
0
0
0
1
0
β2
1
(5.21)
⎦
(5.22)
H23 = HRz2 ( q3 ) HTz2 ( β2 ) HTx2 ( l3 ) HRx2 ( 0 )
⎡ cos(q3 ) − sen(q3 ) 0 l3 cos(q3 ) ⎤
= ⎣
sen(q3 )
cos(q3 )
0
l3 sen(q3 )
0
0
0
0
1
0
β3
1
(5.23)
⎦
(5.24)
Por lo tanto, la matriz homogénea del robot antropomórfico de 3 gdl moviéndose en
su espacio tridimensional es:
H03 = H01 H12H23
cos(q ) cos(q
=
1
2 + q3 )
sen(q1 ) cos(q2 + q3 )
sen(q2 + q3 )
0
− cos(q1 ) sen(q2 + q3 )
− sen(q1 ) sen(q2 + q3 )
cos(q2 + q3 )
0
sen(q1 )
− cos(q1 )
0
0
(5.25)
[ β2 + β3 ] sen(q1 ) + cos(q1 ) [ l2 cos(q2 ) + l3 cos(q2 + q3 ) ]
− [ β2 + β3 ] cos(q1 ) + sen(q1 ) [ l2 cos(q2 ) + l3 cos(q2 + q3 ) ]
l1 + β1 + l2 sen(q2 ) + l3 sen(q2 + q3 )
1
Las coordenadas cartesianas del extremo final del robot manipulador antropomórfico
de 3 gdl se encuentran relacionadas con las coordenadas articulares de la siguiente
forma:
⎡
x0
⎤
⎡
[ β2 + β3 ] sen(q1 ) + cos(q1 ) [ l2 cos(q2 ) + l3 cos(q2 + q3 ) ]
⎤
⎢ ⎥
⎢
⎥
⎣ y0 ⎦ = ⎣ − [ β2 + β3 ] cos(q1 ) + sen(q1) [ l2 cos(q2 ) + l3 cos(q2 + q3 ) ] ⎦ (5.26)
l1 + β1 + l2 sen(q2 ) + l3 sen(q2 + q3 )
z0
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
5.2
Brazo robot antropomórfico
225
Cinemática inversa
La cinemática inversa del robot antropomórfico de 3 gdl consiste en obtener las
posiciones articulares en función de las coordenadas cartesianas de la ecuación (5.26).
Una forma para resolver dicho planteamiento es utilizar el método geométrico, con
esta intención considere la proyección de los eslabones del hombro y del codo sobre
el plano x0 − y0 (ver figura 5.8); dicha proyección se amplı́a en la figura 5.9 para
llevar a cabo el análisis geométrico correspondiente a la variable q1 .
Observe que existe proyección geométrica sobre el plano x0 − y0 de los parámetros
β2 y β3 de los servomotores, debido a que los ejes z1 y z2 quedan paralelos a dicho
plano, lo que no sucede para los casos del péndulo y robot de 2 gdl.
Figura 5.9 Método geométrico de la cinemática inversa 3 gdl para obtener q1 .
De la figura 5.9 tenemos que ϑ = ρ + q1 ⇒ q1 = ϑ − ρ. Observe que el ángulo ϑ se
y0
obtiene en función de las coordenadas cartesianas (x0 , y0 ) satisface ϑ = atan x0 .
Además, tomando en cuenta el triángulo con lı́nea punteada, el ángulo ρ queda en
función del cateto opuesto (formado por el ancho de los servomotores β2 + β3 ) y el
cateto adyacente formado por la diferencia de la hipotenusa (r = x20 + y02 ) y del
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
226
Capı́tulo 5: Cinemática directa cartesiana
cateto opuesto obtiene la siguiente forma:
⎛
ρ =
⎞
β2 + β3
atan ⎝ x20
+
y02
− ( β2 + β3 )
⎠
2
Por lo tanto, la variable articular q1 adquiere la siguiente estructura:
⎛
⎞
β2 + β3
y0
⎠
− atan ⎝ q1 = ϑ − ρ = atan
x0
2
2
2
x0 + y0 − ( β2 + β3 )
(5.27)
Para obtener la variable articular q3 considérese como referencia el ángulo ϑ del
triángulo formado por el cateto adyacente l2 + l3 cos(q3 ), cateto opuesto l3 sen(q3 )
y por la hipotenusa x20 + y02 + z02 (ver figura 5.10). Es importante recalcar que los
ángulos ϑ y q2 se miden en dirección positiva con respecto al plano horizontal x0 −y0
hacia el eje z0 .
Figura 5.10 Método geométrico para obtener q2 y q3 de la cinemática inversa 3 gdl.
Empleando el teorema de Pitágoras se obtiene una posible solución para q3 en
términos de la función arco-tangente:
x20 + y02 + z02 = [l2 + l3 cos(q3 )]2 + l32 sen2 (q3 )
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
5.2
227
Brazo robot antropomórfico
:1
l22 + l32 [cos2
(q
+sen2 (q3 )] +
3 )
l22 + l32 + 2l2 l3 cos(q3 )
x20 + y02 + z02 − l22 − l32
=
=
cos(q3 ) =
2l2 l3 cos(q3 )
2l2 l3
2
x20 + y02 + z02 − l22 − l32
2l2 l3
⎛#
$ 2 2 2 2 2 %2 ⎞
x0 +y0 +z0 −l2 −l3
⎟
⎜ 1−
2l2l3
⎟
atan ⎜
⎠
⎝
x20 +y02 +z02 −l22 −l23
1−
sen(q3 ) =
q3 =
2l2l3
En referencia a la figura 5.10, obsérvese que el triángulo formado por los lados
x20 + y02 , z0 y la hipotenusa x20 + y02 + z02 , tomando en cuenta que la tangente de
(q3 )
,
los ángulos ϑ + q2 satisface: tan (ϑ + q2 ) = √ z20 2 , además tan (ϑ) = l2l3+lsen
3 cos(q3 )
x0 +y0
usando identidades trigonométricas de la función tangente se obtiene:
tan (ϑ + q2 ) =
z0
tan (q2 ) + tan (ϑ)
=
2
1 − tan (q2 ) tan (ϑ)
x0 + y02
Realizando sencillos pasos algebraicos, para determinar la función tangente de q2 :
tan (q2 ) + tan ϑ =
'
tan (q2 ) 1 +
√ z20 2
x0 +y0
tan (ϑ)
(
=
z0
x20
+
z0
x20 + y02
√
tan (q2 ) =
=
y02
z0
−
tan (q2 ) tan (ϑ)
2
x0 + y02
− tan ϑ
z0
− tan ϑ
x20 +y02
(
'
1 + √ z20 2 tan (ϑ)
x0 +y0
='
(q3)
z0
− l2l3+lsen
3 cos(q3 )
x20 +y02
l sen(q )
+ √ z20 2 l23+l3 cos(q33 )
x0 +y0
√
1
[ l2 + l3 cos(q3 ) ] z0 − l3 sen(q3 ) x20 + y02
x20 + y02 [ l2 + l3 cos(q3 ) ] + z0 l3 sen(q3 )
Por lo tanto, la cinemática inversa que relaciona las coordenadas articulares q1 , q2 , q3
en función de las coordenadas cartesianas x0 , y0 , z0 del extremo final de un robot
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
(
228
Capı́tulo 5: Cinemática directa cartesiana
antropomórfico de 3 gdl que se mueve en su espacio tridimensional, considerando el
espesor de los servomotores tiene la siguiente forma:
q1 =
atan
y0
x0
− atan ⎝ q3 =
β2 + β3
⎠
x20
+ y02
(5.28)
2
− ( β2 + β3 )
*
[ l2 + l3 cos(q3 ) ] z0 − l3 sen(q3 ) x20 + y02
atan 2
x0 + y02 [ l2 + l3 cos(q3 ) ] + z0 l3 sen(q3 )
⎛
'
(2 ⎞
(2l2 l3 )2 − x20 + y02 + z02 − l22 − l32
⎠
atan ⎝
x20 + y02 + z02 − l22 − l32
)
q2 =
⎞
⎛
(5.29)
(5.30)
El jacobiano de un robot antropomórfico de tres grados de libertad está dado por:
⎤
⎡
j11 j12 j13
⎥
⎢
(5.31)
J(q) = ⎣ j21 j22 j23 ⎦
j31 j32 j33
j11 = [ β2 + β3 ] cos(q1 ) − sen(q1 ) [ l2 cos(q2 ) + l3 cos(q2 + q3 ) ]
j12 = − cos(q1 ) [ l2 sen(q2 ) + l3 sen(q2 + q3 ) ]
j13 = −l3 cos(q1 ) sen(q2 + q3 )
j21 = [ β2 + β3 ] sen(q1 ) + cos(q1 ) [ l2 cos(q2 ) + l3 cos(q2 + q3 ) ]
j22 = − sen(q1 ) [ l2 sen(q2 ) + l3 sen(q2 + q3 ) ]
j23 = −l3 sen(q1 ) sen(q2 + q3 )
j31 = 0,
j32 = l2 cos(q2 ) + l3 cos(q2 + q3 ),
j33 = l3 cos(q2 + q3 )
Jacobiano del brazo robot de 3 gdl
El determinante del jacobiano del brazo robot de 3 gdl está dado por:
det[J(q)]
=
−l22 l3 cos(q2 ) sen(q3 ) − l2 l23 sen(q2 ) cos(q3 )2 − l2 l23 cos(q2 ) sen(q3) cos(q3) + l2 l23 sen(q2 )
(5.32)
existen un número infinito de configuraciones singulares para q2 = 0 ± nπ y
q3 = 0 ± nπ. Sin embargo, el ángulo q1 no interviene con los puntos singulares.
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
5.2
229
Brazo robot antropomórfico
Función transformación homogénea del robot antropomórfico de 3 gdl H03
La función transformación homogénea H03 del robot antropomórfico de 3 gdl tiene
la siguiente sintaxis:
H03 =H r3gdl()
esta función retorna H03 la matriz de transformación homogénea del brazo robot con
articulaciones rotacionales de 3 gdl. El código en lenguaje MATLAB de la función
transformación homogénea H03 del robot de 3 gdl está en el cuadro 5.9.
Código Fuente 5.9
H r3gdl.m
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 5 Cinemática directa cartesiana.
H r3gdl.m
1
4
function H=H r3gdl()
syms q1 q2 q3 beta1 beta2 beta3 l1 l2 l3 alpha1 alpha2 alpha3 real
disp(’Parámetros Denavit-Hartenberg del robot planar vertical de 3 gdl’)
disp(’[ l alpha d q]’)
5
dh=[l1, pi/2, l1+beta1, q1; l2, 0, beta2, q2; l3, 0, beta3, q3]; disp(dh)
6
% H01 = HRz0 (q1 )HTz0 (l1 + β1 )HTx0 (0)HRx0 ( π2 )
H10=simplify(HRz(q1)*HTz(l1+beta1)*HTx(0)*HRx(pi/2));
2
3
7
% H12 = HRz1 (q2 )HTz1 (β2 )HTx1 (l2 )HRx1 (0)
H21=simplify(HRz(q2)*HTz(beta2)*HTx(l2)*HRx(0)) ;
8
9
% H23 = HRz2 (q3 )HTz2 (β3 )HTx2 (l3 )HRx2 (0)
H32=simplify(HRz(q3)*HTz(beta3)*HTx(l3)*HRx(0));
H30=simplify(H10*H21*H32,3); % H03 = H01H12 H23
[R30, cinemat r3gdl, cero, c]=H DH(H30); % R30 , f R (q1 , q2 , q3 )
10
11
12
13
14
H=[R30, cinemat r3gdl;
cero, c];
15
16
end
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
230
Capı́tulo 5: Cinemática directa cartesiana
Función cinemática directa del brazo robot de 3 gdl
La función de cinemática directa para las coordenadas cartesianas del extremo final
del brazo robot de 3 gdl se encuentra dada por:
[x0 , y0 , z0 ]=cinematica r3gdl(β1 , l1, q1 , β2 , l2, q2 , β3, l3 , q3 )
donde l1 es la distancia sobre el eje z0 donde se encuentra localizado el servomotor
de la articulación de la base; l2 , l3 son las longitudes del hombro y codo,
respectivamente; Ancho de los servomotores β1 , β2, β3 ; q1 , q2 , q3 son las posiciones
articulares. Esta función retorna las coordenadas cartesianas (x0 , y0 , z0 ).
Código Fuente 5.10
cinematica r3gdl.m
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 5 Cinemática directa cartesiana.
cinematica r3gdl.m
1 function [x0, y0, z0]=cinematica r3gdl(beta1,l1,q1,beta2,l2,q2,beta3,l3,q3)
2
3
4
dato1=whos(’beta1’); dato2=whos(’l1’); dato3=whos(’q1’);
dato4=whos(’beta2’); dato5=whos(’l2’); dato6=whos(’q2’);
dato7=whos(’beta3’); dato8=whos(’l3’); dato9=whos(’q3’);
5
6
7
8
v1=strcmp(dato1.class, ’sym’);v2=strcmp(dato2.class, ’sym’);
v3=strcmp(dato3.class, ’sym’);v4=strcmp(dato4.class, ’sym’);
v5=strcmp(dato5.class, ’sym’);v6=strcmp(dato6.class, ’sym’);
v7=strcmp(dato7.class, ’sym’);v8=strcmp(dato8.class, ’sym’);
9
10
11
v9=strcmp(dato9.class, ’sym’); digits(3);
if (v1 & v2 & v3 & v4 & v5 & v6 & v7 & v8 & v9) %caso simbólico
x0=simplify(vpa((beta2+beta3)*sin(q1)+cos(q1)*(l3*cos(q2 + q3) + l2*cos(q2)),3));
12
13
14
y0=simplify(vpa(-(beta2+beta3)*cos(q1)+sin(q1)*(l3*cos(q2 + q3) + l2*cos(q2)),3));
z0=simplify(vpa( l1 +beta1+ l3*sin(q2 + q3) + l2*sin(q2) ,3));
x0=vpa(x0); y0=vpa(y0);
15
16
17
18
else %caso numérico
x0=(beta2+beta3)*sin(q1)+cos(q1).*(l3*cos(q2 + q3) + l2*cos(q2));
y0=-(beta2+beta3)*cos(q1)+sin(q1).*(l3*cos(q2 + q3) + l2*cos(q2));
z0=l1 +beta1+ l3*sin(q2 + q3) + l2*sin(q2);
19
end
20 end
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
5.2
231
Brazo robot antropomórfico
Función cinemática inversa del brazo robot de 3 gdl
La función de cinemática inversa del brazo robot de 3 gdl tiene la siguiente forma
sintáctica:
[q1 , q2 , q3 ]=cinv r3gdl(β2, β3 , l2, l3 , x0, y0 , z0 )
donde los argumentos de entrada son los parámetros geométricos del robot β1 , β2 , β3
representan espesor de los servomotores de la base, hombro y codo, respectivamente;
l1 es la distancia sobre el eje z0 donde se ubica la articulación de la base; l2 y
l3 longitudes del hombro y codo, respectivamente. Las coordenadas cartesianas
del extremo final del robot x0 , y0 , z0 se encuentran expresadas en el sistema
Σ0 (x0 , y0 , z0 ). Esta función retorna las coordenadas articulares (q1 , q2 , q3 ) de la base,
hombro y codo, respectivamente.
Código Fuente 5.11
cinv r3gdl.m
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 5 Cinemática directa cartesiana.
cinv r3gdl.m
1
2
function [q1, q2, q3]=cinv r3gdl(beta2,beta3,l2,l3,x0,y0,z0)
v1=sqrt(x0.*x0+y0.*y0-(beta2+beta3)∧ 2);
3
5
%articulación de la base q1
q1=atan(y0./x0)-atan((beta2+beta3)./v1);
c3=(x0.*x0+y0.*y0+z0.*z0-l2∧2-l3∧ 2)./(2*l2*l3);
6
s3=sqrt(1-c3.*c3);
7
%articulación del codo q3
q3=atan(s3./c3);
s2=(z.*(l2+l3.*c3)-l3*s3.*sqrt(x0.*x0+y0.*y0))./(x0.*x0+y0.*y0+z0.*z0);
4
8
9
10
c2=((l2+l3.*c3).*sqrt(x0.*x0+y0.*y0)+l3*s3.*z)./(x0.*x0+y0.*y0+z0.*z0);
q2=atan(s2./c2); %articulación del hombro
11
12
end
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
232
Capı́tulo 5: Cinemática directa cartesiana
♣♣♣ Ejemplo 5.3
Diseñar un programa en lenguaje MATLAB que permita desplegar en forma
simbólica los parámetros DH, cinemática cartesiana y el jacobiano del robot
antropomórfico de 3 gdl. Asimismo programar una aplicación donde el extremo
final del robot trace trayectorias circulares sobre el eje z0 .
Solución
El programa 5.12 presenta el código fuente en lenguaje MATLAB que despliega en
forma simbólica los parámetros DH, cinemática cartesiana, matriz jacobiana y su
determinante (ver lı́neas 6 a la 16).
De la lı́nea 23 a la 25 se encuentra la implementación de la trayectoria circular con
barrido de 1 mm sobre el eje z0 : [ x y z ]T = [ xc + r sen(t) yc + r cos(t) t ],
siendo xc , yc el centro del cı́rculo, r es el radio, t es la evolución del tiempo. En
la lı́nea 26 la trayectoria circular es convertida a coordenadas articulares usando la
cinemática inversa y en la lı́nea 27 se obtiene las coordenadas que el extremo final
del robot traza como se muestra en la figura 5.11.
Figura 5.11 Trazo que realiza el extremo final del brazo robot de 3 gdl.
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
5.2
Brazo robot antropomórfico
Código Fuente 5.12
233
cap5 r3gdl.m
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 5 Cinemática directa cartesiana.
cap5 r3gdl.m
1
clc;
2
clear all;
close all;
format short
syms q1 q2 q3 beta1 beta2 beta3 l1 l2 l3 alpha1 alpha2 alpha3 real
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
H30=H r3gdl() ;
disp(’Tansformación homogénea del robot antropomórfico de 3 gdl’);
disp(H30);
[R30, cinemat r3gdl,cero, c]=H DH(H30) ;
disp(’Matriz de rotación’);
disp(R30);
disp(’cinemática directa’);
disp(cinemat r3gdl);
[x0, y0,z0]= cinematica r3gdl(beta1,l1,q1,beta2,l2,q2,beta3,l3,q3); disp([x0; y0;z0])
jac r3gdl=jacobian([x0; y0;z0], [q1;q2;q3])
det r3gdl=simplify(vpa(det(jac r3gdl),3))
%ejemplo numérico
l1=0; l2=0.45; l3=0.45;
t=0:0.001:100;
xc=0.3; yc=-0.3; r=0.20;
beta1=0.12; beta2=0.01; beta3=0.01;
q1=[]; q2=[]; q3=[];
x=xc+r*sin(t) ;
y=yc+r*cos(t);
z=t;
[q1, q2, q3]=cinv r3gdl(beta2,beta3,l2,l3,x,y,z);
[x0, y0,z0]=cinematica r3gdl(beta1,l1,q1,beta2,l2,q2,beta3,l3,q3);
plot3(x0,y0,z0)
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
234
Capı́tulo 5: Cinemática directa cartesiana
5.3 Configuración SCARA (RRP)
L
a configuración SCARA (Selective Compliance Assembly Robot Arm)
representa una geometrı́a especial de robots industriales; es un brazo planar
antropomórfico con dos articulaciones rotacionales y la tercera articulación es
prismática o lineal para manipular objetos. La figura 5.12 muestra un ejemplo de
un robot industrial en la configuración SCARA. Esta configuración aprovecha las
ventajas que proporciona el brazo robot antropomórfico de 2 gdl moviéndose en el
plano horizontal; en este caso la energı́a potencial es constante (par gravitacional
cero), la estructura mecánica es de alta rigidez para soportar cargas en forma vertical
y para control de fuerza. La configuración SCARA es adecuada para tareas de
ensamble con pequeños objetos.
Figura 5.12 Robot KR 10 SCARA R600 (Compañı́a KUKA).
Cinemática directa cartesiana
El espacio de trabajo del robot SCARA es un cilindro que se describe en la figura
5.13, el radio del cilindro es igual a la suma de las longitudes de los eslabones
de las articulaciones rotacionales l2 + l3 . El origen del sistema de referencia fijo
Σ0 (x0 , y0 , z0 ) se coloca rı́gidamente en el piso, de tal forma que sobre el eje z0 la
articulación de la base del robot queda a una distancia l1 más el ancho del servomotor
y espesor de la barra metálica representado por β1 . El eje de rotación de la primera
articulación rotacional q1 (base) coincide con el eje z0 . El sistema de referencia
Σ1 (x1 , y1 , z1 ) se encuentra colocado rı́gidamente en el extremo final del primer
eslabón, donde se intercepta al eje de rotación de la segunda articulación rotacional
q2 con el eje x0 ; las coordenadas del origen del sistema Σ1 (x1 , y1 , z1 ) con respecto
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
5.3
Configuración SCARA (RRP)
235
a Σ0 (x0 , y0 , z0 ) son [ l2 cos(q1 ) l2 sen(q1 ), l1 + β1 ]T . Cuando q1 = 0 significa que el
plano x1 − y1 mantiene una rotación con referencia al plano fijo x0 − y0 , entonces
la matriz R10 = Rz0 ( q1 ) describe la rotación del sistema Σ1 (x1 , y1 , z1 ) con respecto
al sistema fijo Σ0 (x0 , y0 , z0 ). Los ejes z0 y z1 son paralelos entre sı́ y mantienen la
misma dirección, por lo tanto el ángulo que existe entre ellos es α1 = 0. El sistema
de referencia Σ1 (x1 , y1 , z1 ) mide la variable articular q2 alrededor del eje z1 . El plano
x1 − y1 tiene una rotación por un ángulo q1 con respecto al plano x0 − y0 .
Figura 5.13 Espacio de trabajo del robot SCARA.
El sistema de referencia Σ2 (x2 , y2 , z2 ) se coloca en la parte final del segundo
eslabón, justo en la intersección del eje x1 con el desplazamiento lineal d3 . Las
coordenadas del origen del sistema Σ2 (x2 , y2 , z2 ) con respecto a Σ1 (x1 , y1 , z1 ) son
[ l3 cos(q2 ) l3 sen(q2 ) l1 + β1 + β2 ]T , siendo β2 el ancho del segundo servomotor
más el espesor de la segunda barra metálica. El ángulo que hay entre los ejes x1 y x2
es q2 . Cuando q2 = 0 se encuentran alineados dichos ejes, para el caso donde q2 = 0,
la rotación que hay entre los sistemas de referencia Σ1 (x1 , y1 , z1 ) y Σ2 (x2 , y2 , z2 ) es
descrita por la matriz R21 = Rz1 ( q2 ). La medición de la tercera articulación lineal d3
se realiza sobre el eje z2 , por lo tanto este eje está alineado sobre su desplazamiento
lineal teniendo movimiento positivo en dirección hacia abajo, que corresponde a una
dirección negativa del eje z1 o z0 como se muestra en la figura 5.13. El sistema de
referencia Σ2 (x2 , y2 , z2 ) se obtiene a partir del sistema Σ1 (x1 , y1 , z1 ), rotando un
ángulo α2 = π alrededor del eje x1 , por lo tanto los ejes z2 y z1 son paralelos entre
sı́, pero con dirección contraria.
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
236
Capı́tulo 5: Cinemática directa cartesiana
En la configuración SCARA todos los ejes zi con i = 0, 1, 2 son paralelos entre sı́,
pero debe notarse que el eje z2 mantiene una rotación de 180 grados con respecto al
eje z1 o al eje z0 . La razón por la cual el eje z2 mantiene un dirección positiva hacia
abajo es por aspectos técnicos, es decir los objetos a manipular se encuentran frente
de la herramienta de trabajo. Se ha considerado que el eje z2 es paralelo al eje de la
herramienta (α3 = 0).
Con la anterior descripción de los sistemas de referencia Σ0 (x0 , y0 , z0 ) al Σ2 (x2 , y2 , z2 )
para la configuración SCARA produce el siguiente conjunto de parámetros DenavitHartenberg que se presenta en la tabla 5.4:
Tabla 5.4 Parámetros DH del robot SCARA
Eslabón
li
αi
di
θi
1
0
0
l 1 + β1
q1
2
l2
π
β2
q2
3
l3
0
d3
0
De acuerdo con la tabla 5.4 el robot SCARA tiene las siguientes matrices de
transformación homogénea:
H01 = HRz0 ( q1 ) HTz0 ( l1 + β1 ) HTx ( l2 ) HRx ( 0 )
⎡
⎤
cos(q1 ) − sen(q1 ) 0 l2 cos(q1 )
⎢ sen(q )
cos(q1 )
0 l2 sen(q1 ) ⎥
1
⎢
⎥
= ⎢
⎥
⎣
0
0
1
l 1 + β1 ⎦
H12
0
0
0
1
= HRz1 ( q2 ) HTz1 ( β2 ) HTx1 ( l3 ) HRx1 ( π )
⎡
⎤
sen(q2 )
0
l3 cos(q2 )
cos(q2 )
⎢ sen(q ) − cos(q ) 0 l sen(q ) ⎥
2
2
3
2 ⎥
⎢
= ⎢
⎥
⎣
⎦
0
0
−1
β2
0
H23
Alfaomega
0
0
(5.33)
(5.34)
1
= HRz2 ( 0 ) HTz2 ( d3 ) HTx2 ( 0 ) HRx2 ( 0 )
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
5.3
237
Configuración SCARA (RRP)
⎡
1 0 0
0
⎤
⎢0 1 0 0 ⎥
⎢
⎥
= ⎢
⎥
⎣ 0 0 1 d3 ⎦
H03
(5.35)
0 0 0 1
= H01 H12 H23
⎡ cos(q1 + q2 )
⎣
sen(q1 + q2 )
0
0
sen(q1 + q2 )
− cos(q1 + q2 )
0
0
0
0
−1
0
l2 cos(q1 ) + l3 cos(q1 + q2 ) ⎤
l2 sen(q1 ) + l3 sen(q1 + q2 )
⎦
l1 + β1 + β2 − d3
1
(5.36)
El modelo de cinemática directa para el robot SCARA está dada por la siguiente
expresión:
⎡
x0
⎤
⎡
l2 cos(q1 ) + l3 cos(q1 + q2 )
⎤
⎢ ⎥
⎥
⎢
⎣ y0 ⎦ = f R (q) = ⎣ l2 sen(q1 ) + l3 sen(q1 + q2 ) ⎦
z0
l 1 + β1 + β2 − d 3
(5.37)
Función transformación homogénea SCARA
El conjunto de ecuaciones que describen la rotación y coordenadas cartesianas del
extremo final del robot SCARA (5.33)-(5.37) permiten desarrollar una librerı́a con
variables simbólicas para MATLAB H SCARA() con la siguiente sintaxis:
H03 = H SCARA()
esta función retorna la matriz de transformación homogénea H03 la cual está formada
con la matriz de rotación R30 que relaciona la rotación del extremo final del robot
con respecto al sistema fijo Σ0 (x0 , y0 , z0 ) y la cinemática directa que relaciona las
coordenadas articulares con las coordenadas cartesianas f R (q). Además, también
despliega la tabla 5.4 con los parámetros Denavit-Hartenberg del robot SCARA.
El programa 5.13 contiene el código MATLAB de la función transformación
homogénea del robot SCARA:
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
238
Capı́tulo 5: Cinemática directa cartesiana
Código Fuente 5.13
H SCARA.m
%Transformación homogénea del robot SCARA H03
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 5 Cinemática directa cartesiana.
H SCARA.m
1
function H=H SCARA()
2
syms q1 q2 q3 beta1 beta2 l1 l2 l3 d1 d2 d3 alpha1 alpha2 alpha3 real
disp(’Transformación Homogénea H03 del robot SCARA’)
disp(’Parámetros Denavit-Hartenberg del robot SCARA’)
disp(’[ l alpha d q]’)
3
4
5
6
8
%tabla de parámetros DH del robot SCARA
dh=[0, 0,l1+beta1, q1; l2, pi, beta2, q2; l3, 0, d3, 0]; %despliega parámetros
DH del robot SCARA
disp(dh)
9
%cálculo de las matrices de transformación homogénea de cada articulación
7
11
%H01 = HRz0 ( q1 ) HTz0 ( l1 + β1 ) HTx ( l2 ) HRx ( 0 )
H10=HRz(q1)*HTz(l1+beta1)*HTx(l2)*HRx(0);
12
%H12 = HRz1 ( q2 ) HTz1 ( β2 ) HTx1 ( l3 ) HRx1 ( π )
13
H21=HRz(q2)*HTz(beta2)*HTx(l3)*HRx(pi);
14
15
%H23 = HRz2 ( 0 ) HTz2 ( d3 ) HTx2 ( 0 ) HRx2 ( 0 )
H32=HRz(0)*HTz(d3)*HTx(0)*HRx(0);
16
%transformación homogénea del robot SCARA
17
%H03 = H01 H12 H23
18
H30=simplify(H10*H21*H32);
19
%deducción de la matriz de rotación R30
20
21
%ası́ como la cinemática directa cartesiana f R (q1 , q2, d3 ) del robot SCARA
[R30, cinemat scara, cero, c]=H DH(H30);
22
%estructura de la matriz homogénea
23
H=[R30, cinemat scara;
cero, c];
10
24
25
end
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
5.3
239
Configuración SCARA (RRP)
Función cinemática directa
La función cinemática directa del robot SCARA se define como:
[x0 , y0 , z0 ]=cinematica SCARA(β1, β2 , l1 , l2, l3 , q1 , q2 , d3 )
donde los argumentos de entrada son los parámetros geométricos:β1, β2 , l1, l2 , l3 y
las variables articulares q1 , q2 , d3 . Esta función retorna las coordenadas x0 , y0 , z0 en
espacio cartesiano del sistema de referencia fijo Σ0 (x0 , y0 , z0 ).
En el cuadro 5.14 se presenta el código en lenguaje MATLAB de la cinemática
directa del robot SCARA.
Código Fuente 5.14
cinematica SCARA.m
%cinemática directa robot SCARA
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 5 Cinemática directa cartesiana.
cinematica SCARA.m
1
2
3
function [x0, y0, z0]=cinematica SCARA(beta1,beta2,l1,l2,l3,q1,q2,d3)
x0=l2*cos(q1)+l3*cos(q1+q2);
y0=l2*sin(q1)+l3*sin(q1+q2);
4
5
z0=l1+beta1+beta2-d3;
end
Jacobiano del robot SCARA
El jacobiano del robot SCARA se encuentra de la siguiente forma:
∂f R (q)
∂q
⎡
−l2 sen(q1 ) − l3 sen(q1 + q2 )
⎢
= ⎣ l2 cos(q1 ) + l3 cos(q1 + q2 )
J(q1 , q2 , d3) =
−l3 sen(q1 + q2 )
l3 cos(q1 + q2 )
0
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
0
0
(5.38)
⎤
⎥
0 ⎦
−1
Alfaomega
240
Capı́tulo 5: Cinemática directa cartesiana
el determinante del jacobiano del robot SCARA tiene la siguiente forma:
det[J(q1 , q2 , d3 )] = −l2 l3 sen(q2 )
(5.39)
lo que significa que se presentan problemas de singularidad cuando q2 = 0, ±nπ.
Cinemática inversa del robot SCARA
El ancho de los servomotores y espesor de las barras metálicas (β1 , β2 ) no tienen
proyección sobre el plano x0 − y0 ; sólo contribuyen en la coordenada z0 , como en el
caso del péndulo y robot antropomórfico de 2 gdl, entonces la cinemática inversa del
robot SCARA toma la siguiente expresión:
q2 = acos (
)
y0
l3 sen(q2 )
− atan
x0
l2 + l3 cos(q2 )
= l1 + β1 + β2 − z0
q1 =
d3
x20 +y02 −l22 −l23
2l2 l3
atan
(5.40)
(5.41)
(5.42)
Función cinemática inversa del robot SCARA
La función cinemática inversa del robot SCARA tiene la siguiente sintaxis:
[d3 , q2 , q1 ]=cinv SCARA(β1 , β2 , l1, l2, l3 , x0 , y0 , z0 )
donde β1 , β2, l1 , l2, l3 son los parámetros geométricos del robot SCARA, x0 , y0 , z0 son
las coordenadas en espacio cartesiano en el sistema fijo Σ0 (x0 , y0 , z0 ); esta función
retorna las coordenadas articulares d3 , q2 , q1 .
El programa 5.15 contiene el código fuente MATLAB de la cinemática inversa del
robot SCARA, para facilitar la implementación de procedimientos recursivos con
pase de parámetros escalares o vectoriales, la cinemática inversa es programada con
operaciones entre arreglos.
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
5.3
241
Configuración SCARA (RRP)
Código Fuente 5.15
cinv SCARA.m
%cinemática inversa robot SCARA
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 5 Cinemática directa cartesiana.
cinv SCARA.m
1
function [d3 q2 q1]=cinv SCARA(beta1,beta2,l1,l2,l3,x0,y0,z0)
2
q2=acos((x0.*x0+y0.*y0-l2*l2-l3*l3)/(2*l2*l3));
q1=atan(y0./x0)-atan((l3*sin(q2))./(l2+l3*cos(q2)));
d3=l1+beta1+beta2-z0;
3
4
5
end
♣ ♣ Ejemplo 5.4
Realizar una aplicación numérica del robot SCARA para que el extremo final
lleve a cabo el trazo de una rosa polar de radio r = 0.1 m y con centro en
las coordenadas cartesianas [ x0
y0
z0 ]T = [ 0.3 −0.3 −0.5 ]T m. Asimismo
presentar en forma simbólica la matriz jacobiana, determinante y cinemática
directa.
Solución
En el cuadro 5.16 se presenta el código en lenguaje MATLAB en variables
simbólicas de la matriz de transformación homogénea H03 del robot SCARA usando la función H30=H SCARA()
(ver lı́nea 4). En la lı́nea 5 empleando la función H DH(H30) se obtiene la
matriz de rotación R30 a través de la
función [R30 , f R (q), 0T , 1]=H DH(H30).
La matriz jacobiano se obtiene en la
lı́nea 6 y su determinante en la lı́nea
7. La aplicación numérica consiste en
que el extremo final del robot SCARA trace una rosa polar de radio
r = 0.1 m, y con centro en las coordenadas cartesianas [ x0 y0 z0 ]T =
[ 0.3 −0.3 −0.5 ]T m. La ecuación
en coordenadas cartesianas de la rosa
polar (lineas 19 a la 21) está dada por:
6t
r = 0.1 sen( ) x = xc + r sen3 (t)
7
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
242
y = yc + r cos3 (t)
Capı́tulo 5: Cinemática directa cartesiana
z = −0.5
donde xc , yc representan el centro de la
flor, r es el radio de los pétalos y t es
el tiempo.
El cálculo de las coordenadas cartesianas de la rosa polar se realiza en
forma iterativa usando la instrucción
for· · ·end (lı́neas 16 a la 24). En la
lı́nea 24 se convierten las coordenadas cartesianas de la rosa polar a
coordenadas articulares del robot empleando la función cinv SCARA(· · ·).
En la lı́nea 27 se usa la función
cinematica SCARA(· · ·) para convertir las coordenadas articulares del robot a coordenadas cartesianas, entonces el extremo final del robot realiza
el trazo de la figura 5.14. El programa
que se presenta en el cuadro 5.17 realiza la misma función del programa 5.16;
la finalidad de mostrarlo es ilustrar el
cálculo de las coordenadas cartesianas
de la rosa polar sin utilizar la forma recursiva de la instrucción for, es decir
se emplean operaciones con arreglos en
lugar del proceso iterativo con escalares.
Observe que en las funciones
cinv SCARA y cinematica SCARA se
encuentran implementadas para soportar ambos tipos de pase de parámetros (recursivo y por arreglos), lo que
facilita la implementación de varias
Alfaomega
aplicaciones en control de robots manipuladores.
Es muy importante aclarar que el dibujo realizado por el robot SCARA (figura 5.14) no implica control del robot
o una simulación de la dinámica del
mismo. Estos programas ilustran sólo
el empleo de la cinemática inversa y su
conversión a coordenadas cartesianas
del extremo final del robot para trazar
una figura en su espacio de trabajo.
Figura 5.14 Trayectoria del robot SCARA.
Cuando las coordenadas articulares
del robot provienen de un sistema
dinámico como puede ser la ecuación
en lazo cerrado (modelo dinámico del
robot y algoritmo de control), entonces se trata de una simulación de un
esquema de control. La cinemática no
reproduce los fenómenos fı́sicos del robot.
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
5.3
Configuración SCARA (RRP)
Código Fuente 5.16
243
SCARA.m
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 5 Cinemática directa cartesiana.
SCARA.m
1
2
3
4
5
clc; clear all; close all; format short
syms theta l d alpha q real
syms q1 q2 d3 beta1 beta2 l1 l2 l3 alpha1 alpha2 alpha3 real
H30=H SCARA() ;
7
[R30, frq scara, cero, c]=H DH(H30);
jac scara=jacobian(frq scara, [q1;q2;d3]);
det scara=simplify(det(jac scara))
8
%ejemplo numérico
9
11
%parámetros geométricos del robot SCARA
l1=0.45; l2=0.45; l3=0.45; beta1=0.1; beta2=0.1;
t=0:0.001:100;
12
%ecuación de la figura centro en xc,yc y radio r
13
xc=0.3; yc=-0.3;
q1=[]; q2=[];z0=[]; d3=[];
[n m]=size(t); % dimensión del vector de tiempo
for k=1:m %forma recursiva
6
10
14
15
16
17
20
%ecuación cartesiana de la figura rosa polar
r=0.1*sin(6*t(k)/7); % radio de la figura
x=xc+r*(sin(t(k)))∧ 3 ;
y=yc+r*(cos(t(k)))∧ 3;
21
z=-0.5 ;
22
% conversión de coordenadas cartesianas de la rosa polar
23
%a coordenadas articulares del robot SCARA
[d3(k), q2(k), q1(k)]=cinv SCARA(beta1,beta2,l1,l2,l3,x,y,z) ;
18
19
24
25
end
26
%trayectoria del robot SCARA en el sistema Σ0 (x0 , y0 , z0 )
[x0, y0, z0]=cinematica SCARA(beta1,beta2,l1,l2,l3,q1,q2,d3);
plot3(x0,y0,z0)
27
28
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
244
Capı́tulo 5: Cinemática directa cartesiana
Código Fuente 5.17
SCARA1.m
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 5 Cinemática directa cartesiana.
SCARA1.m
1
2
3
4
5
6
7
clc; clear all; close all;
format short
syms theta l d alpha q real
syms q1 q2 d3 beta1 beta2 l1 l2 l3 alpha1 alpha2 alpha3 real
H30=H SCARA();
[R30, frq scara, cero, c]=H DH(H30);
8
jac scara=jacobian(frq scara, [q1;q2;d3]);
det scara=simplify(det(jac scara))
9
%ejemplo numérico
10
%parámetros geométricos del robot SCARA
11
l1=0.45; l2=0.45; l3=0.45; beta1=0.1; beta2=0.1;
t=0:0.001:100;
12
13
14
15
%ecuación de la rosa polar centro en xc,yc y radio r
xc=0.3; yc=-0.3;
17
q1=[]; q2=[];z0=[]; d3=[];
[n m]=size(t); % dimensión del vector de tiempo
r=0.1*sin(6*t/7); %radio de la rosa polar
18
%ecuación cartesiana de la rosa polar
19
21
x=xc+r.*(sin(t).∧ 3);
y=yc+r.*(cos(t).∧ 3);
z(1:m)=-0.5; %coordenada sobre el eje z0
22
% conversión de coordenadas cartesianas de la rosa polar
23
%a coordenadas articulares del robot SCARA
24
[d3, q2, q1]=cinv SCARA(beta1,beta2,l1,l2,l3,x,y,z);
25
%trayectoria del robot SCARA en el sistema Σ0 (x0 , y0 , z0 )
[x0, y0, z0]=cinematica SCARA(beta1,beta2,l1,l2,l3,q1,q2,d3);
plot3(x0,y0,z0)
16
20
26
27
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
5.4
Robot esférico (RRP)
245
5.4 Robot esférico (RRP)
L
a configuración esférica de robots manipuladores presenta dos articulaciones
rotacionales (base y hombro) y la articulación del codo corresponde al tipo
prismática o lineal como se muestra en la figura 5.15. Los ejes de movimiento de las
articulaciones son mutuamente perpendiculares entre sı́. Dentro de los ejemplares
de esta configuración se encuentra el robot Standford, cuya principal aplicación es
el mecanizado de piezas automotrices y la manipulación de objetos sobre piso.
Figura 5.15 Robot manipulador en configuración esférica.
Cinemática directa cartesiana
El espacio de trabajo de la configuración esférica corresponde a una esfera hueca,
cuyo radio se encuentra en función del desplazamiento lineal d3 de la articulación
prismática del codo. El eje z0 se alinea con el eje de giro de la articulación rotacional
de la base q1 y el origen del sistema de referencia fijo Σ0 (x0 , y0 , z0 ) se ubica en
el piso de tal forma que la articulación de la base está a una altura l1 sobre el
eje z0 . El ancho del servomotor de la base y el espesor de la placa metálica están
representados por β1 . Por otro lado, el eje z1 determina la medición de la variable
articular q2 del hombro y se encuentra alineado con el eje de rotación de esta
articulación. Los ejes z1 y z0 son perpendiculares entre sı́. El sistema de referencia
Σ1 (x1 , y1 , z1 ) se coloca en el dorso del servomotor de la articulación del hombro y las
coordenadas del origen de este sistema respecto al sistema fijo Σ0 (x0 , y0 , z0 ) están en:
[ β1 cos(q1 ) β1 sen(q1 ) l1 + β1 ]T . El eje z2 se alinea con el desplazamiento lineal de
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
246
Capı́tulo 5: Cinemática directa cartesiana
la articulación prismática d3 y las coordenadas del origen del sistema Σ2 (x2 , y2 , z2 )
con referencia al sistema Σ1 (x1 , y1 , z1 ) se encuentran localizadas en: [ 0 0 β2 ]T .
Los ejes z0 , z1 y z2 son perpendiculares entre sı́.
Figura 5.16 Configuración esférica.
La tabla 5.5 presenta los parámetros de la convención Denavit-Hartenberg para el
robot en la configuración esférica.
Tabla 5.5 DH del robot esférico
li
αi
di
θi
1
0
l 1 + β1
q1
2
0
- π2
π
2
β2
q2
3
0
0
d3
0
Eslabón
Las matrices de transformación homogénea para el robot en la configuración esférica
tienen la siguiente estructura:
H01 = HRz0 ( q1 ) HTz0 ( l1 + β1 ) HTx0 ( 0 ) HRx0 ( − pi
2 )
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
5.4
⎡
cos(q1 )
0
⎢ sen(q ) 0
1
⎢
= ⎢
⎣
0
−1
H12
H23
247
Robot esférico (RRP)
− sen(q1 )
cos(q1 )
0
0
0
0
= HRz1 ( q2 ) HTz1 ( β2 ) HTx0
⎡
sen(q2 )
cos(q2 ) 0
⎢ sen(q ) 0 − cos(q )
2
2
⎢
= ⎢
⎣
0
1
0
0
⎤
⎥
⎥
⎥
l 1 + β1 ⎦
0
(5.43)
1
( 0 ) HRx1 ( π2 )
⎤
0
0 ⎥
⎥
⎥
β2 ⎦
(5.44)
0
0
0
1
= HRz2 ( 0 ) HTz2 ( d3 ) HTx2 ( 0 ) HRx2 ( 0 )
⎡
⎤
1 0 0 0
⎢0 1 0 0 ⎥
⎢
⎥
= ⎢
⎥
⎣ 0 0 1 d3 ⎦
0 0 0
(5.45)
1
(5.46)
H03
=
H01 H12 H23
⎡ cos(q1 ) cos(q2 )
= ⎣
sen(q1) cos(q2 )
− sen(q2 )
0
− sen(q1 )
cos(q1 ) sen(q2 )
cos(q1 )
0
0
sen(q1 ) sen(q2)
cos(q2 )
0
−β2 sen(q1 ) + d3 sen(q2 ) cos(q1) ⎤
β2 cos(q1 ) + d3 sen(q2 ) sen(q1 )
l1 + β1 + d3 cos(q2 )
1
⎦(5.47)
La cinemática directa de un robot manipulador en configuración esférica es:
⎡ ⎤
⎡
⎤
x
−β2 sen(q1 ) + d3 sen(q2 ) cos(q1 )
⎢ ⎥
⎢
⎥
⎣ y ⎦ = ⎣ β2 cos(q1 ) + d3 sen(q2 ) sen(q1 ) ⎦
z
l1 + β1 + d3 cos(q2 )
(5.48)
El jacobiano del robot esférico tiene la siguiente forma:
J(q1 , q2 , d3) =
−β2 cos(q1 ) − d3 sen(q1 ) sen(q2)
d3 cos(q1 ) cos(q2)
cos(q1 ) sen(q2 )
d3 cos(q1 ) sen(q2 ) − β2 sen(q1 )
0
d3 cos(q2 ) sen(q1 )
−d3 sen(q2 )
sen(q1 ) sen(q2)
cos(q2 )
(5.49)
Determinante det[J(q1 , q2 , d3 )] = −d23 sen(q2 )
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
248
Capı́tulo 5: Cinemática directa cartesiana
Función transformación homogénea robot esférico
La función matriz de transformación homogénea H esferico() del brazo robot en
la configuración esférica tiene la siguiente sintaxis:
H03 = H esferico()
esta función retorna la matriz de transformación homogénea H03 integrada por la
matriz de rotación R30 que describe la rotación del extremo final del robot con
respecto al sistema fijo Σ0 (x0 , y0 , z0 ) y la cinemática directa cartesiana f R (q) que
relaciona las coordenadas articulares. Además, también despliega en forma simbólica
la tabla 5.5 con los parámetros Denavit-Hartenberg. El programa 5.18 contiene el
código MATLAB de la función transformación homogénea del robot esférico.
Código Fuente 5.18
H esferico.m
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 5 Cinemática directa cartesiana.
H esferico.m
1
function H=H esferico()
2
syms q1 q2 q3 beta1 beta2 l1 l2 l3 d1 d2 d3 alpha1 alpha2 alpha3 real
disp(’Parámetros Denavit-Hartenberg del robot SCARA’) disp(’[ l alpha d q]’)
dh=[0, -pi/2, l1+beta1, q1; 0, pi/2, beta2, q2; 0, 0, d3, 0]; disp(dh)
3
4
%H01 = HRz0 (q1 )HTz0 (l1 + β1 )HTx0 (0)HRx0 (− π2 )
H10=HRz(q1)*HTz(l1+beta1)*HTx(0)*HRx(-pi/2);
5
6
%H12 = HRz1 (q2 )HTz1 (β2 )HTx1 (0)HRx1 ( π2 )
H21=HRz(q2)*HTz(beta2)*HTx(0)*HRx(pi/2);
H32=HRz(0)*HTz(d3)*HTx(0)*HRx(0); %H23 = HRz2 (0)HTz2 (d3 )HTx2 (0)HRx2 (0)
H30=simplify(H10*H21*H32);%H03 = H01 H12 H23
7
8
9
10
11
[R30, cinemat esferico, cero, c]=H DH(H30);
H=[R30, cinemat esferico; cero, c];
12
13
end
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
5.4
249
Robot esférico (RRP)
Función cinemática directa del robot esférico
La función de cinemática directa que relaciona las coordenadas cartesianas del
extremo final con las coordenadas articulares del brazo robot en configuración
esférica está dada por la siguiente sintaxis (el código en lenguaje MATLAB se
encuentra en el cuadro 5.19):
[x0 , y0 , z0 ]=cinematica esferico(β1, β2 , l1, q1 , q2 , d3)
Código Fuente 5.19
cinematica esferico.m
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 5 Cinemática directa cartesiana.
cinematica esferico.m
1
2
3
4
function [x0, y0,z0]=cinematica esferico(beta1,beta2,l1,q1,q2,d3)
dato1=whos(’beta1’); dato2=whos(’beta2’); dato3=whos(’l1’);
dato4=whos(’q1’); dato5=whos(’q2’); dato6=whos(’d3’);
v1=strcmp(dato1.class, ’sym’); v2=strcmp(dato2.class, ’sym’);
5
v3=strcmp(dato3.class, ’sym’); v4=strcmp(dato4.class, ’sym’);
v5=strcmp(dato5.class, ’sym’); v6=strcmp(dato6.class, ’sym’);
digits(3);
if ( v1 & v2 & v3 & v4 & v5 &v6) % caso simbólico
6
7
8
9
x0=simplify(vpa(-beta2*sin(q1)+d3*cos(q1)*sin(q2),3));
y0=simplify(vpa(beta2*cos(q1) + d3*sin(q1)*sin(q2),3));
z0=simplify(vpa(beta1 + l1 + d3.*cos(q2),3));
10
11
12
15
x0=vpa(x0); y0=vpa(y0); z0=vpa(z0);
else %caso numérico
x0=-beta2*sin(q1)+d3.*cos(q1).*sin(q2);
y0= beta2*cos(q1) + d3.*sin(q1).*sin(q2);
16
z0=beta1 + l1 + d3.*cos(q2);
13
14
17
18
end
end
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
250
Capı́tulo 5: Cinemática directa cartesiana
donde l1 es la distancia sobre el eje z0 donde está ubicada la articulación de la base;
q1 , q2 , d2 , d3 son las posiciones articulares de la base, hombro y codo, respectivamente.
Retorna las coordenadas cartesianas (x0 , y0 , z0 ) en el sistema Σ0 (x0 , y0 , z0 ).
Cinemática inversa
Para obtener la cinemática inversa de la configuración esférica se emplea el
método geométrico que se presenta a detalle en la figura 5.17. Los ángulos ϑ + q1
satisfacen tan (ϑ + q1 ) = xy00 . Note que el ángulo ϑ queda ubicado en el interior
del triángulo formado por el cateto adyacente x20 + y02 − β22 , cateto opuesto β2
y la hipotenusa
x20 + y02 , entonces tan (ϑ) = √ 2 β2 2 2 . La proyección del
x0 +y0 −β2
movimiento de la articulación d3 sobre el plano x0 − y0 es d3 cos(q2 − π2 ) =
x20 + y02 − β22 . Ahora, tomando en cuenta la siguiente identidad
d3 sen(q2 ) =
tan (ϑ)+tan (q1 )
trigonométrica tan (ϑ + q1 ) = 1−tan
(ϑ) tan (q1 ) , se obtiene la expresión tan (q1 ) =
√ 2 2 2
−x0 β2 +y0 x0 +y0 −β
z0 −(l1 +β1 )
√
. Por otro lado, se tiene que tan (q2 − π2 ) = √
y para
y0 β2 +x0 x20 +y02 −β22
x20 +y02 −β22
d3 = x20 + y02 − β22 + [z0 − (l1 + β1 )]2 .
Figura 5.17 Cinemática inversa de la configuración esférica.
La cinemática inversa de la configuración esférica toma la siguiente forma:
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
5.4
251
Robot esférico (RRP)
)
q1
q2
d3
*
−x0 β2 + y0 x20 + y02 − β 2
= atan
y0 β2 + x0 x20 + y02 − β22
*
)
z0 − (l1 + β1 )
π
+ atan =
2
x20 + y02 − β22
=
x20 + y02 − β22 + [z0 − (l1 + β1 )]2
(5.50)
(5.51)
(5.52)
Función cinemática inversa robot esférico
La sintaxis de la función de cinemática inversa de la configuración esférica es (ver
código en el cuadro 5.20); esta función retorna las coordenadas articulares.
[q1, q2, d3]=cinv esferico(β1, β2 , l1, x0 , y0 , z0 )
Código Fuente 5.20
cinv esferico.m
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 5 Cinemática directa cartesiana.
cinv esferico.m
1
2
function [q1, q2, d3]=cinv esferico(beta1,beta2,l1,x0,y0,z0)
w1=y0.*sqrt(x0.*x0+y0.*y0-beta2*beta2)-x0*beta2;
3
w2=x0.* sqrt(x0.*x0+y0.*y0-beta2*beta2)+y0*beta2;
q1=atan(w1./w2);
w3=z0-(l1+beta1);
w4=sqrt(x0.*x0+y0.*y0-beta2*beta2);
4
5
6
7
q2=pi/2+atan(w3./w4);
d3=sqrt(x0.*x0+y0.*y0-beta2*beta2+(z0-(l1+beta1)).*(z0-(l1+beta1)));
8
9
end
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
252
Capı́tulo 5: Cinemática directa cartesiana
♣♣♣ Ejemplo 5.5
Escribir un programa en lenguaje MATLAB que permita desplegar en forma
simbólica los parámetros DH, matriz homogénea, cinemática cartesiana, jacobiano
y determinante del robot esférico. Además, programar una aplicación donde el
extremo final del robot trace un conjunto de figuras circulares. El radio de cada
cı́rculo es r = 0.25 m, con centro en el plano (x0 , y0 ) = (0.3, −0.3) m, cada cı́rculo
estará desplazado un milı́metro sobre el eje z0 (de 0 a 100 mm).
Solución
El programa que permite resolver el
problema planteado se encuentra en el
cuadro 5.21. La información simbólica del robot esférico se encuentra en
las lı́neas 4 a la 15 (parámetros DH,
matriz homogénea, cinemática directa, jacobiano y determinante del robot
cilı́ndrico).
La figura circular se encuentra implementada en la lı́nea 18 a la 24, con la
siguiente ecuación:
x = xc + r sen(t)
x0 , y0 y el desplazamiento variable
está dado por z. La lı́nea 25 convierte
las coordenadas cartesianas del cı́rculo a coordenadas articulares del robot usando la función cinv esferico.
La lı́nea 26 realiza la conversión de
coordenadas articulares a coordenadas
cartesianas del extremo final del robot (cinematica esferico). La figura 5.18 muestra el trazo realizado por
el extremo final del robot esférico (ver
lı́nea 27). .18
5.6.
y = yc + r cos(t)
z = kt
donde r es el radio del cı́rculo, t es la
evolución del tiempo, k [mm/seg] es
una constante positiva que convierte
el tiempo a milı́metros, las coordenadas [ xc , yc ]T = [ 0.3, −0.3 ]T m representan el centro del cı́rculo en el plano
Alfaomega
Figura 5.18 Aplicación del robot esférico.
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
5.4
253
Robot esférico (RRP)
Código Fuente 5.21
cap5 esferico.m
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 5 Cinemática directa cartesiana.
cap5 esferico.m
1
clc; clear all; close all;
2
format short
syms q1 q2 d3 beta1 beta2 l1 alpha1 alpha2 alpha3 real
H30=H esferico();
disp(’Transformación homogénea del robot esférico’);
3
4
5
6
7
8
9
10
11
12
13
disp(H30);
[R30, cinemat esferico,cero, c]=H DH(H30);
disp(’Matriz de rotación’);
disp(R30);
disp(’cinemática directa’);
disp(cinemat esferico);
[x0, y0,z0]=cinematica esferico(beta1,beta2,l1,q1,q2,d3);
15
disp([x0; y0;z0])
jac esferico=jacobian([x0; y0;z0], [q1;q2;d3])
det esferico=simplify(vpa(det(jac esferico),3))
16
%ejemplo numérico
17
l1=0.5; beta1=0.10; beta2=0.05;
t=0:0.001:100; [n m]=size(t);
14
18
19
20
21
22
23
24
25
%ecuación del cı́rculo con centro en xc,yc y radio r
xc=0.3; yc=-0.3; r=0.25;
q1=[]; q2=[]; d3=[]; k=1;
x=xc+r.*sin(t);
y=yc+r.*cos(t);
z=k*t;
26
[q1, q2, d3]=cinv esferico(beta1,beta2,l1,x,y,z);
[x0, y0,z0]=cinematica esferico(beta1,beta2,l1,q1,q2,d3);
27
plot3(x0,y0,z0)
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
254
Capı́tulo 5: Cinemática directa cartesiana
5.5 Manipulador cilı́ndrico (RPP)
L
a configuración cilı́ndrica tiene la articulación de la base rotacional,
mientras que las articulaciones del hombro y codo son lineales o prismáticas.
Entre las aplicaciones más importantes de los robots manipuladores en esta
configuración se encuentran procesos para desbastar moldes, traslado de objetos,
ensamble de piezas en espacios horizontales y transporte de objetos. En la figura
5.19 se muestra la configuración cilı́ndrica.
Figura 5.19 Robot en configuración cilı́ndrica.
Cinemática directa cartesiana
El espacio de trabajo de la configuración cilı́ndrica que se muestra en la figura
5.21 tiene la forma de un cilindro hueco con radio directamente proporcional al
desplazamiento lineal d3 y altura variable d2 . El origen del sistema de referencia
cartesiano fijo Σ0 (x0 , y0 , z0 ) se ubica convenientemente en el piso, de tal forma que
la articulación rotacional de la base se encuentra a una distancia l1 sobre el eje z0 .
Este eje se alinea con el eje de rotación del servomotor, de esta forma la variable
articular q1 gira alrededor del eje z0 . El ancho del servomotor y espesor de las placas
metálicas que se utilizan para acoplar mecánicamente la siguiente articulación se
encuentra expresado por el parámetro geométrico β1 . Note que el parámetro l1
produce sólo coordenadas positivas para z0 , cuando este parámetro es cero o el
origen de Σ0 (x0 , y0 , z0 ) se coloca sobre la articulación de la base, entonces puede
haber coordenadas negativas en z0 .
El sistema Σ1 (x1 , y1 , z1 ) sirve para medir la coordenada lineal d2 de la articulación
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
5.5
Manipulador cilı́ndrico (RPP)
255
del hombro, las coordenadas de Σ1 (x1 , y1 , z1 ) con respecto al sistema Σ0 (x0 , y0 , z0 )
son: [ 0, 0, l1 + β1 ]T . El eje z1 se alinea con el desplazamiento lineal de la variable
d2 y los ejes z0 y z1 son paralelos entre sı́. El sistema de referencia Σ1 (x1 , y1 , z1 ) se
encuentra rotado un ángulo q1 con respecto al sistema fijo Σ0 (x0 , y0 , z0 ), la matriz
de rotación que relaciona dicha rotación entre estos dos sistemas de referencia es:
R10 = Rz0 ( q1 ).
Figura 5.20 Espacio de trabajo del robot cilı́ndrico.
La articulación del codo tiene asociado el sistema de referencia Σ2 (x2 , y2 , z2 ) para
medir el desplazamiento lineal de la variable d3 . Este sistema se genera por rotar
un ángulo − π2 radianes alrededor del eje x1 , de tal forma que el eje z2 determina
el desplazamiento lineal d2 . Note que una consecuencia de esta rotación alrededor
del eje x1 , es que el eje z2 queda alineado sobre el eje y1 , de tal forma que para
q1 = 0, el desplazamiento lineal de d3 coincide sobre el eje y0 . Debido a esto, para la
articulación lineal del codo el ángulo alrededor del eje z0 es q1 + π2 . Observe también
la dirección para coordenadas negativas del eje y2 coincide con la dirección positiva
de los ejes z0 y z1 .
Las coordenadas del origen del sistema Σ2 (x2 , y2 , z2 ) con respecto al sistema
Σ1 (x1 , y1 , z1 ) son: [ 0, 0, l1 + β1 ]T . El eje z2 es perpendicular a los ejes z0 y z1 . La
forma en que están indicados los sistemas de referencia Σ0 (x0 , y0 , z0 ), Σ1(x1 , y1 , z1 )
y Σ2 (x2 , y2 , z2 ) en la figura 5.21 determinan la posición de casa del brazo robot en
configuración cilı́ndrica, es decir para esa configuración de sistemas de referencia las
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
256
Capı́tulo 5: Cinemática directa cartesiana
variables articulares tienen un valor de [ q1 , d2, d3 ]T = [ 0, 0, l1 + β1 ]T . La variable
lineal de codo normalmente es calibrada para dar valor cero en z0 = l1 + β1 ,
desplazamientos mayores a esta longitud son positivos, o en caso contrario negativos.
Para otro tipo de posición de casa, la cinemática directa del robot cilı́ndrico adquiere
una forma cartesiana diferente.
Los parámetros geométricos de espesor y ancho β2 , β3 de los servomotores de las
articulaciones del hombro y codo son absorbidos como parte de las caracterı́sticas
mecánicas de sus articulaciones, por lo que no influyen directamente en la expresión
analı́tica de la cinemática directa del brazo robot en configuración cilı́ndrica.
En la tabla 5.6 se muestran los parámetros del robot manipulador en la configuración
cilı́ndrica.
Tabla 5.6 DH del robot cilı́ndrico
Eslabón
li
αi
di
θi
1
0
0
l 1 + β1
q1
2
0
- π2
d2
0
3
0
0
d3
0
Con la tabla 5.6 el robot en la configuración cilı́ndrica tiene las siguientes matrices
de transformación homogénea:
H01 = HRz0 ( q1 ) HTz0 ( β1 + l1 ) HTx0 ( 0 ) HRx0 ( 0 )
⎡
⎤
cos(q1 ) − sen(q1 ) 0
0
⎢ sen(q )
cos(q1 )
0
0 ⎥
1
⎢
⎥
= ⎢
⎥
⎣
0
0
1 l 1 + β1 ⎦
H12
0
0
0
1
= HRz0 ( 0 ) HTz0 ( d2 ) HTx0 ( 0 ) HRx0 ( − pi
2 )
⎡
⎤
1 0 0 0
⎢0 0 1 0 ⎥
⎢
⎥
= ⎢
⎥
⎣ 0 −1 0 d2 ⎦
0
H23
Alfaomega
0
0
(5.53)
(5.54)
1
= HRz0 ( 0 ) HTz0 ( d3 ) HTx0 ( 0 ) HRx0 ( 0 )
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
5.5
257
Manipulador cilı́ndrico (RPP)
⎡
1
⎢0
⎢
= ⎢
⎣0
0
0 0
⎤
0 ⎥
⎥
⎥
0 1 d3 ⎦
1 0
0 0
H03 = H01 H12 H23
⎡ cos(q1 )
= ⎣
0
(5.55)
1
sen(q1 )
0
0
0
−1
− sen(q1 )
cos(q1 )
0
0
0
0
− sen(q1 )d3 ⎤
d3 cos(q1 )
⎦
l1 + β1 + d2
(5.56)
1
El modelo de cinemática directa para el robot manipulador cilı́ndrico está dado de
la siguiente forma:
⎡ ⎤
⎡
⎤
x
−d3 sen(q1 )
⎢ ⎥
⎢
⎥
(5.57)
⎣ y ⎦ = f R (q) = ⎣ d3 cos(q1 ) ⎦
z
l 1 + β1 + d 2
Jacobiano del robot cilı́ndrico
El jacobiano del brazo robot en configuración cilı́ndrica se expresa de la siguiente
forma:
⎡
⎤
− cos(q1 )d3 0 − sen(q1 )
∂f R (q) ⎢
⎥
J(q1 , d2, d3 ) =
= ⎣ −d3 sen(q1 ) 0
cos(q1 ) ⎦
(5.58)
∂q
0
1
0
cuyo determinante está dado por:
det[J(q1 , d2, d3 )] = d3
(5.59)
la única singularidad que presenta el brazo robot en configuración cilı́ndrica es
cuando la articulación lineal del codo es d3 = 0, mientras que las variables q1 , d2 ∈ IR
pueden tener cualquier valor. De ahı́ que es recomendable para algunas aplicaciones
donde se involucra la inversa de la matriz jacobiana que la variable articular lineal
d3 = 0. En algunos robots este problema se evita insertando sensores, interruptores
o por programación para deshabilitar el movimiento del robot cuando la articulación
lineal d3 pasa por el origen del sistema Σ0 (x0 , y0 , z0 ).
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
258
Capı́tulo 5: Cinemática directa cartesiana
Función transformación homogénea robot cilı́ndrico
La función matriz de transformación homogénea del brazo robot en configuración
cilı́ndrica tiene la siguiente sintaxis:
H03 = H cilindrico()
retorna la matriz de transformación homogénea H03 compuesta por la matriz de
rotación R30 y la cinemática directa cartesiana f R (q) que relaciona las coordenadas
cartesianas del extremo final con las coordenadas articulares. También despliega los
parámetros Denavit-Hartenberg en forma simbólica que se muestran en la tabla 5.6.
El programa 5.22 contiene el código de la función transformación homogénea.
Código Fuente 5.22
H cilindrico.m
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 5 Cinemática directa cartesiana.
H cilindrico.m
1 function H=H cilindrico()
2
syms q1 d2 d3 beta1 l1 alpha1 alpha2 alpha3 real
3
disp(’Parámetros Denavit-Hartenberg del robot cilı́ndrico’);
4
5
6
disp(’[ l alpha d q]’);
dh=[0, 0, l1+beta1, q1; 0, -pi/2, d2, 0; 0, 0, d3, 0];
disp(dh)
7
%H01 = HRz0 (q1 )HTz0 (l1 + β1 )HTx0 (0)HRx0 (0)
8
H10=HRz(q1)*HTz(l1+beta1)*HTx(0)*HRx(0);
9
%H12 = HRz1 (0)HTz1 (d2 )HTx1 (0)HRx1 (− π2 )
10
H21=HRz(0)*HTz(d2)*HTx(0)*HRx(-pi/2);
11
%H23 = HRz2 (0)HTz2 (d3 )HTx2 (0)HRx2 (0)
12
13
14
H32=HRz(0)*HTz(d3)*HTx(0)*HRx(0);
H30=simplify(H10*H21*H32); % H03 = H01 H12 H23
[R30, cinemat cilindrico, cero, c]=H DH(H30);
15
16
H=[R30, cinemat cilindrico;
cero, c];
17 end
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
5.5
259
Manipulador cilı́ndrico (RPP)
Función cinemática directa del robot cilı́ndrico
La función de cinemática directa del brazo robot cilı́ndrico está por:
[x0 , y0 , z0 ]=cinematica cilindrico(β1, l1, q1 , d2, d3 )
donde q1 , d2 , d3 son las posiciones articulares de la base, hombro y codo,
respectivamente. Retorna las coordenadas cartesianas (x0 , y0 , z0 ) en el sistema
Σ0 (x0 , y0 , z0 ). El cuadro 5.23 contiene el código en lenguaje MATLAB para la función
cinemática del robot cilı́ndrico:
Código Fuente 5.23
cinematica cilindrico.m
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 5 Cinemática directa cartesiana.
cinematica cilindrico.m
1
2
function [x0, y0,z0]=cinematica cilindrico(beta1,l1,q1,d2,d3)
dato1=whos(’beta1’); dato2=whos(’l1’); dato3=whos(’q1’);
3
dato4=whos(’d2’); dato5=whos(’d3’); v1=strcmp(dato1.class, ’sym’);
v2=strcmp(dato2.class, ’sym’); v3=strcmp(dato3.class, ’sym’);
v4=strcmp(dato4.class, ’sym’); v5=strcmp(dato5.class, ’sym’); digits(3);
if ( v1 & v2 & v3 & v4 & v5) % caso simbólico
4
5
6
7
x0= -d3*sin(q1);
y0= d3*cos(q1);
z0=l1+beta1+d2;
8
9
10
13
x0=vpa(x0); y0=vpa(y0); z0=vpa(z0);
else %caso numérico
x0= -d3.*sin(q1);
y0= d3.*cos(q1);
14
z0=l1+beta1+d2;
11
12
15
16
end
end
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
260
Capı́tulo 5: Cinemática directa cartesiana
Cinemática inversa
La cinemática inversa del brazo robot en configuración cilı́ndrica se obtiene de la
cinemática directa (5.57) de la siguiente forma: primero que nada, debe considerarse
que debido a que la posición de casa [ q1 , d2, d3 ]T = [ 0, 0, 0 ]T seleccionada para este
brazo robot se caracteriza por la articulación del codo d3 está alineada sobre el eje
y0 (los eje y0 y z2 coinciden cuando q1 = 0), entonces el desplazamiento rotacional
de la base alrededor del ejez0 espor un ángulo q1 + π2 (ver figura 5.21). Por lo tanto,
se cumple: q1 +
π
2
= atan − xy00 .
La articulación del hombro tiene un desplazamiento lineal sobre el eje z0 , entonces
d2 = z0 − (l1 + β1 ). La articulación del codo d3 proyecta su movimiento sobre el
plano x0 − y0 , por lo que puede tomar dos posibles soluciones: d3 = ± x20 + y02 .
Figura 5.21 Cinemática inversa del robot cilı́ndrico.
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
5.5
Manipulador cilı́ndrico (RPP)
261
La cinemática inversa del brazo robot en configuración cilı́ndrica está dada como:
q1
d2
d3
x0
π
= − + atan −
2
y0
= z0 − (l1 + β1 )
= ± x20 + y02
(5.60)
(5.61)
(5.62)
Función cinemática inversa robot cilı́ndrico
La función de cinemática inversa del robot cilı́ndrico convierte las coordenadas
cartesianas del extremo final (x0 , y0 , z0 ) en función de las coordenadas articulares
(q1 , d2, d3 ) con la siguiente sintaxis:
[q1 , d2, d3 ]=cinv cilindrico(β1, l1, x0 , y0 , z0 )
donde β1 , l1 son los parámetros geométricos, x0 , y0 , z0 son las coordenadas
cartesianas del extremo final. Esta función retorna las coordenadas articulares
q1 , d2, d3 de la base, hombro y codo, respectivamente. El cuadro 5.24 contiene la
implementación de la función cinemática inversa del brazo robot cilı́ndrico.
Código Fuente 5.24
cinv cilindrico.m
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 5 Cinemática directa cartesiana.
cinv cilindrico.m
1
2
function [q1, d2,d3]=cinv cilindrico(beta1,l1,x0,y0,z0)
q1= -pi/2+atan(-x0./y0);
3
d2=z0-(l1+beta1);
d3= sqrt(x0.*x0+y0.*y0);
4
5
end
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
262
Capı́tulo 5: Cinemática directa cartesiana
♣♣♣ Ejemplo 5.6
Escribir un programa en lenguaje MATLAB que permita desplegar en forma
simbólica los parámetros DH, cinemática cartesiana, jacobiano y determinante
del robot cilı́ndrico. Asimismo programar una aplicación donde el extremo final
del robot realice el trazo de una rosa polar con centro en [ 0.3, −0.3, 0.5] y radio
r = 0.15.
Solución
El desplegado simbólico de los parámetros DH, cinemática directa y jacobiano del robot cilı́ndrico se encuentra en el cuadro 5.25. En la lı́nea 4
se obtiene la matriz de transformación
homogénea H03 utilizando la función
H cilindrico(). La matriz de transformación homogénea H03 se utiliza como argumento de entrada en la función D HD(H03 ) para obtener la matriz
de rotación R30 y la cinemática directa
(lı́nea 5). El jacobiano y su determinante se despliegan en las lı́neas 11 y
12, respectivamente.
el centro de la figura (ver lı́neas 1922), l1 + β1 = 0.45 El vector tiempo es de 0 a 100 segundos, con incrementos de un milisegundo. Las
coordenadas cartesianas de la rosa polar son convertidas a coordenadas articulares usando la función
cinv cilindrico(β1, l1 , x, y, z) (ver
lı́nea 24). La lı́nea 26 contiene la conversión de coordenadas cartesianas
del extremo final del robot a coordenadas articulares usando la función
cinematica cilindrico(q1, d2 , d3).
La lı́nea 27 exhibe la figura 5.22 trazada por el robot cilı́ndrico.
La figura “rosa polar”tiene la siguiente
ecuación:
r = 0.05 + 0.1 sen(t)
x = xc + r sen(t)
y = yc + r cos(t)
z = l1 + β1 + 0.05
donde las coordendas [ xc , yc , z ]T son
Alfaomega
Figura 5.22 Aplicación del robot cilı́ndrico.
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
5.5
263
Manipulador cilı́ndrico (RPP)
Código Fuente 5.25
cap5 cilı́ndrico.m
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 5 Cinemática directa cartesiana.
cap5 cilı́ndrico.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
clc; clear all; close all;
format short
syms q1 d2 d3 beta1 l1 alpha1 alpha2 alpha3 real
H30=H cilindrico(); disp(’Transformación homogénea del robot cilı́ndrico’);
[R30, cinemat cilindrico,cero, c]=H DH(H30);
disp(’Matriz de rotación’);
disp(R30);
disp(’cinemática directa’); disp(cinemat cilindrico);
[x0, y0,z0]=cinematica cilindrico(beta1,l1,q1,d2,d3);
disp([x0; y0;z0])
jac cilindrico=jacobian([x0;y0;z0], [q1;d2;d3])
det cilindrico=simplify(vpa(det(jac cilindrico),3))
%ejemplo numérico
t=0:0.001:100;
%ecuación de la rosa polar con centro en (xc,yc) y radio r
xc=0.3; yc=-0.3; beta1=0.05; l1=0.40;
q1=[]; d2=[]; d3=[]; r=0.2;
[n m]=size(t);
21
r=0.05+0.1*sin(t);
x=xc+r.*sin(t);
y=yc+r.*cos(t);
22
z(1:m)=l1+beta1+0.05;
23
%conversión de coordenadas cartesianas a coordenadas articulares del robot
[q1, d2, d3]=cinv cilindrico(beta1,l1,x,y,z);
20
24
25
26
%coordenadas cartesianas del extremo final del robot
[x0, y0,z0]=cinematica cilindrico(beta1,l1,q1,d2,d3);
27
plot3(x0,y0,z0) %trayectoria trazada por el robot
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
264
Capı́tulo 5: Cinemática directa cartesiana
5.6 Configuración cartesiana (PPP)
L
os robots manipuladores que incluyen sus tres primeras articulaciones del tipo
prismático o lineales se denominan robots cartesianos, también conocidos
como robots pórticos o lineales, por lo que no tienen articulaciones rotacionales.
Existen varias aplicaciones de los robots cartesianos, entre ellas: corte de mascarillas,
graficadores o plotters, taladros automáticos, mesas de medición de coordenadas
como la que se muestra la figura 5.23, impresoras láser o de matriz son otros ejemplos
de robots cartesianos con 2 gdl.
Figura 5.23 Robot cartesiano.
Particularmente, la configuración cartesiana facilita la programación del extremo
final del robot y sus potenciales aplicaciones, ya que la cinemática directa es un
mapa lineal entre coordenadas articulares y cartesianas. Sin embargo, la estructura
mecánica del robot cartesiano presenta baja destreza de movilidad comparado con la
configuración antropomórfica debido a que todas sus articulaciones son prismáticas.
Cinemática cartesiana
En la figura 5.24 se muestra el espacio de trabajo del robot cartesiano, tiene forma
de un paralelepı́pedo recto. El sistema de referencia cartesiano fijo Σ0 (x0 , y0 , z0 ) se
selecciona de manera conveniente. El eje z0 determina el desplazamiento lineal de
la primera articulación prismática d1 , el origen del sistema Σ0 (x0 , y0 , z0 ) se ubica
en [ 0, 0, l1 ] siendo l1 una longitud con respecto al nivel del piso, l1 representa la
longitud de las barras del robot cartesiano. Los ejes x0 y y0 quedan alineados por la
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
5.6
Configuración cartesiana (PPP)
265
regla de la mano derecha. El siguiente sistema de referencia Σ1 (x1 , y1 , z1 ) se obtiene
rotando el eje x0 un ángulo α1 = − π2 , quedando el eje z1 en dirección positiva de
la variable d2 . Observe que el eje y1 apunta en dirección negativa del eje z0 y el eje
x1 es paralelo al eje x0 , separado evidentemente por la dimensión fı́sica de la mesa
cartesiana denotada por la longitud l2 . El sistema Σ2 (x2 , y2 , z2 ) sirve para medir el
desplazamiento lineal de la variable articular d3 . Sin embargo, la obtención de este
tercer sistema de referencia no es trivial, ya que no se puede deducir directamente
del sistema Σ1 (x1 , y1 , z1 ) debido a que no existe un ángulo α2 alrededor del eje x1
que genere directamente el sistema de referencia Σ2 (x2 , y2 , z2 ).
Figura 5.24 Espacio de trabajo del robot cartesiano.
Para obtener el sistema de referencia Σ2 (x2 , y2 , z2 ), primero se emplea un sistema
de referencia auxiliar denominado Σ2a(x2a , y2a, z2a) el cual se obtiene realizando
una rotación de 90 grados con respecto al eje z1 , esta rotación hace que el eje x2a
permanezca paralelo al eje y1 . Debe observarse que los ejes z1 y z2a son paralelos
entre sı́ de tal manera que sus coordenadas (d2 ) entre estos ejes sean idénticas. El
origen del sistema de referencia auxiliar Σ2a(x2a, y2a, z2a) es el mismo que el sistema
de referencia Σ1 (x1 , y1 , z1 ), pero el sistema Σ2a(x2a, y2a, z2a) mantiene una rotación
relativa al sistema Σ1 (x1 , y1 , z1 ) expresada por la matriz rotación Rz1 ( π2 ), es decir:
⎡
⎡ ⎤
⎤
x2a
x2
⎢
⎢
⎥
⎥
T
⎣ y2a ⎦ = Rz1 ( π2 ) ⎣ y2 ⎦
z2a
z2
Posteriormente, girando un ángulo α2 =
π
2
alrededor del eje x2a se obtiene el sistema
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
266
Capı́tulo 5: Cinemática directa cartesiana
de referencia Σ2 (x2 , y2 , z2 ) donde la articulación prismática d3 se mueve linealmente
sobre el eje z2 . Esta última rotación α2 alrededor del eje x2 determina el ángulo
entre los ejes z2 y z1 . Los ejes z0 , z1 y z2 son mutuamente perpendiculares entre sı́.
En la figura 5.25 se muestra la secuencia de rotaciones. El paso 1 corresponde al
sistema fijo Σ0 (x0 , y0 , z0 ), rotando un ángulo α1 = − π2 alrededor del eje x0 genera el
sistema Σ1 (x1 , y1 , z1 ). El paso 2 consiste en rotar por un ángulo π2 alrededor del eje
z1 para generar el sistema de referencia Σ2a(x2a , y2a, z2a), este sistema es auxiliar y
sirve para generar el sistema Σ2 (x2 , y2 , z2 ). Para eso el paso 3 consiste en rotar un
ángulo α2 = − π2 alrededor del eje x2a .
Figura 5.25 Sistemas de referencia del robot cartesiano.
La tabla de parámetros del robot manipulador de 3 gdl en configuración cartesiana
queda de la siguiente forma:
Tabla 5.7 Parámetros Denavit-Hartenberg para el robot cartesiano de 3 gdl
Eslabón
Alfaomega
li
αi
di
θi
1
0
d1
0
2
0
- π2
- π2
d2
0
3
0
0
d3
0
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
5.6
267
Configuración cartesiana (PPP)
La matriz de transformación homogénea para la primera articulación toma la
siguiente forma:
H01 = HRz0 ( 0 ) HTz0 ( d1 ) HTx0 ( 0 ) HRx0 ( − π2 )
⎡
⎤
1 0 0 0
⎢0 0 1 0 ⎥
⎢
⎥
= ⎢
⎥
⎣ 0 −1 0 d1 ⎦
(5.63)
0
H12
0 0 1
= HRz1 ( 0 ) HTz1 ( d2 ) HTx1 ( 0 ) HTx1 ( − π2 )
⎡
⎤
1 0 0 0
⎢0 0 1 0 ⎥
⎢
⎥
= ⎢
⎥
⎣ 0 −1 0 d2 ⎦
0
0
H2a = HRz1 (
= H R z1 (
⎡
0 −1
⎢1
⎢
= ⎢
⎣0
0
0
π T
2 )
π T
2 )
1
H12
HRz1 ( 0 ) HTz2 ( d2 ) HTx1 ( 0 ) HTx1 ( − π2 )
0 0
⎤⎡
H12
1
0
0
⎢
0 0⎥
⎥⎢0 0
⎥⎢
1 0 ⎦ ⎣ 0 −1
0
0 1
0
0
0
0
0
⎤
⎡
0
0
⎢
0 ⎥
⎥ ⎢ −1 0
⎥=⎢
0 d2 ⎦ ⎣ 0 −1
1
0
1
H23 = HRz2 ( 0 ) HTz2 ( d3 ) HTx2 ( 0 ) HTx2
1
0
⎤
0 ⎥
⎥
⎥ (5.65)
0 d2 ⎦
0
0
⎡
0 0 1
⎤
1 0 0 0
⎢0 1 0 0 ⎥
⎢
⎥
(0) = ⎢
⎥
⎣ 0 0 1 d3 ⎦
⎤
⎡
H03
(5.64)
0 0 0
(5.66)
1
0 0 1 d3
⎢ 0 −1 0 d ⎥
2⎥
⎢
= H01 RTz, π H12 H23 = ⎢
⎥
2
⎣
1 0 0 d1 ⎦
H2a
0
0
0
1
La cinemática directa del robot cartesiano está dado por:
⎡ ⎤
⎡ ⎤
x0
d3
⎢ ⎥
⎢ ⎥
⎣ y0 ⎦ = ⎣ d2 ⎦
z0
(5.67)
d1
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
268
Capı́tulo 5: Cinemática directa cartesiana
Jacobiano del robot cartesiano
La matriz jacobiano del robot cartesiano está dado por:
⎡
J(d3 , d2, d1 ) =
1 0 0
⎤
∂f R (q) ⎢
⎥
= ⎣0 1 0⎦
∂q
0 0 1
(5.68)
cuyo determinante es unitario det[J(d3 , d2 , d1)] = 1.
La configuración cartesiana ofrece ventajas debido que no tiene singularidades.
Función transformación homogénea robot cartesiano
La sintaxis de la función transformación homogénea del brazo robot cartesiano es:
H03 =H cartesiano()
esta función retorna la matriz de transformación homogénea H03 compuesta por
la matriz de rotación R30 que describe la rotación del extremo final del robot con
respecto al sistema fijo Σ0 (x0 , y0 , z0 ) y la cinemática directa cartesiana f R (q) que
relaciona las coordenadas articulares. Además, también despliega los parámetros
Denavit-Hartenberg en forma simbólica de la tabla 5.7.
El programa 5.26 contiene el código MATLAB de la función transformación
homogénea del robot cartesiano. Utiliza las matrices de transformación homogéneas
de rotación HRz ( qi ) y traslación HTz ( d ) para el cálculo de cada una de las
matrices homogéneas de las articulaciones: H01 , H12 , H23 . También emplea la función
H DH(H30) para obtener la matriz de rotación R30 y la cinemática directa cartesiana
f R (d1 , d2, d3 ).
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
5.6
269
Configuración cartesiana (PPP)
Código Fuente 5.26
H cartesiano.m
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 5 Cinemática directa cartesiana.
H cartesiano.m
1
function H=H cartesiano()
2
syms l1 l2 l3 d1 d2 d3 alpha1 alpha2 alpha3 real
disp(’Parámetros Denavit-Hartenberg del robot cartesiano’)
disp(’[ l alpha d q]’)
dh=[0, -pi/2, d1, 0; 0, -pi/2, d2, 0; 0, 0, d3, 0];
3
4
5
6
7
%despliega tabla de parámetros DH
disp(dh)
8
%cálculo de las matrices de transformación homogénea de cada articulación
9
%H01 = HRz0 ( 0 ) HTz0 ( d1 ) HTx0 ( 0 ) HRx0 ( − π2 )
H10=HRz(0)*HTz(d1)*HTx(0)*HRx(-pi/2);
10
%H12 = HRz1 ( 0 ) HTz1 ( d2 ) HTx1 ( 0 ) HTx1 ( − π2 )
H21=HRz(0)*HTz(d2)*HTx(0)*HRx(-pi/2);
11
12
14
%H12 a = HRz1 ( π2 ) H12
H21a=simplify((HRz(pi/2))’*H21);
15
%H23 = HRz2 ( 0 ) HTz2 ( d3 ) HTx2 ( 0 ) HTx2 ( 0 )
16
H32=HRz(0)*HTz(d3)*HTx(0)*HRx(0);
17
18
%H03 = H01 H12 a H23
H30=simplify(H10*H21a*H32);
19
%matriz de rotación R30 y cinemática directa f R (d1 , d2, d3 )
20
[R30, cinemat cartesiano, cero, c]=H DH(H30);
21
%Forma la matriz de transformación homogénea del robot cartesiano
3
R0 f R (d1 , d2 , d3)
%H=
1
0T
H=[R30, cinemat cartesiano;
cero, c];
T
13
22
23
24
25
end
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
270
Capı́tulo 5: Cinemática directa cartesiana
Función cinemática directa del robot cartesiano
La sintaxis de la función de cinemática directa de la configuración cartesiana es:
[x0 , y0 , z0 ]=cinematica cartesiano(d3, d2, d1 )
donde d3 , d2 , 13 son las posiciones articulares del robot en configuración cartesiana.
Esta función retorna las coordenadas cartesianas (x0 , y0 , z0 ) en el sistema
Σ0 (x0 , y0 , z0 ). El cuadro 5.27 contiene función cinemática del robot cartesiano.
Código Fuente 5.27
cinematica cartesiano.m
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 5 Cinemática directa cartesiana.
cinematica cartesiano.m
1
2
3
function [x0, y0, z0]=cinematica cartesiano(d3,d2,d1)
dato1=whos(’d1’); dato2=whos(’d2’); dato3=whos(’d3’);
v1=strcmp(dato1.class, ’sym’); v2=strcmp(dato2.class, ’sym’);
4
v3=strcmp(dato3.class, ’sym’); digits(3);
if ( v1 & v2 & v3) %caso simbólico
x0=d3; y0=d2; z0=d1; x0=vpa(x0); y0=vpa(y0); z0=vpa(z0);
else %caso numérico
5
6
7
8
x0=d3; y0=d2; z0=d1;
9
10
end
end
Función cinemática inversa robot cartesiano
La función de cinemática inversa del robot cartesiano convierte las coordenadas
cartesianas del extremo final (x0 , y0 , z0 ) en el sistema Σ0 (x0 , y0 , z0 ) a las coordenadas
articulares (d1 , d2, d3 ), bajo la siguiente sintaxis:
[d3 , d2, d1 ]=cinv cartesiano(x0, y0 , z0 )
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
5.6
271
Configuración cartesiana (PPP)
Esta función retorna las coordenadas articulares. El cuadro 5.28 contiene el código
de la cinemática inversa del robot cartesiano:
Código Fuente 5.28
cinv cartesiano.m
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 5 Cinemática directa cartesiana.
cinv cartesiano.m
1
function [d3, d2, d1]=cinv cartesiano(x0,y0,z0)
2
3
d3=x0; d2=y0; d1=z0;;
end
♣♣♣ Ejemplo 5.7
Escribir un programa en lenguaje MATLAB que permita desplegar en forma
simbólica los parámetros DH, cinemática directa y el jacobiano del robot
cartesiano. Además, implementar una aplicación donde el extremo final del robot
trace una rosa polar, con centro en [ 0.3, −0.3, 0.5 ]T m y radio r = 0.1 m.
Solución
El programa que se presenta en el
cuadro 5.29 permite desplegar en forma simbólica los parámetros DH, cinemática directa, jacobiano del robot
cartesiano y matriz de transformación
homogénea H03 . De la lı́nea 19 a la 25
se obtienen las coordenadas cartesianas de la figura rosa polar, dichas coordenadas son trazadas por el extremo
final del robot como se indica en la figura 5.26.
Figura 5.26 Rosa polar.
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
272
Capı́tulo 5: Cinemática directa cartesiana
Código Fuente 5.29
cap5 cartesiano.m
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 5 Cinemática directa cartesiana.
cap5 cartesiano.m
1
clc;
2
clear all;
close all;
format short
syms d1 d2 d3 alpha1 alpha2 alpha3 real
3
4
5
6
7
8
9
10
11
12
13
H30=H cartesiano();
disp(’Transformación homogénea del robot cartesiano’); disp(H30);
[R30, cinemat cartesiano,cero, c]=H DH(H30);
disp(’Matriz de rotación’); disp(R30);
disp(’cinemática directa’);
disp(cinemat cartesiano);
[x0, y0,z0]=cinematica cartesiano(d3,d2,d1);
15
disp([x0; y0;z0])
jac cartesiano=jacobian([x0;y0;z0], [d3;d2;d1])
det cartesiano=simplify(vpa(det(jac cartesiano),3))
16
%ejemplo numérico
17
t=0:0.001:100;
18
%ecuación de la flor con 8 pétalos centro en xc,yc y radio r
xc=0.3; yc=-0.3 ;
d1=[]; d2=[]; d3=[]; r=0.2;
14
19
20
21
22
23
24
25
26
27
28
[n m]=size(t);
r=0.1*cos(pi*t);
x=xc+r.*sin(t);
y=yc+r.*cos(t);
z(1:m)=0.5 ;
[d3, d2, d1]=cinv cartesiano(x,y,z);
[x0, y0,z0]=cinematica cartesiano(d3,d2,d1);
plot3(x0,y0,z0)
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
5.7
Resumen
273
5.7 Resumen
C
inemática directa cartesiana de robots manipuladores permite una descripción entre las coordenadas cartesianas del extremo final del robot manipulador
y las coordenadas articulares. La convención Denavit-Hartenberg es una herramienta de la ingenierı́a de suma utilidad para la obtención del modelo de cinemática
directa de robots manipuladores la cual establece que la matriz de transformación
homogénea de la i-ésima articulación se compone de 4 parámetros: li es la longitud
de los eslabones, αi el ángulo que hay entre el eje zi−1 y zi , di es el desplazamiento
lineal sobre el eje zi−1 ; cuando la i-ésima articulación no es prismática o lineal, entonces di representa el ancho del servomotor y espesor de la placa metálica, en este
caso se especifica por βi . Para articulaciones rotacionales alrededor del eje zi−1 se
denota por qi .
De acuerdo al análisis de cinemática directa cartesiana para cada configuración de
robots industriales (antropomórfica, SCARA, esférico, cilı́ndrico y cartesiano), se han
desarrollado un conjunto de librerı́as de cinemática directa de robots manipuladores
para MATLAB (toolbox) que permiten obtener los parámetros Denavit-Hartenberg,
la matriz homogénea, cinemática directa, matriz jacobiano y su determinante en
función de los parámetros geométricos como longitudes li y también tomando en
cuenta el ancho de la i-ésima articulación y espesor de la placa metálica (βi ) que se
emplea para acoplar mecánicamente la siguiente articulación del robot.
Este conjunto de librerı́as tienen la ventaja de trabajar en forma simbólica, ya
que despliegan no sólo las ecuaciones de cinemática, también permiten realizar
aplicaciones numéricas como manejo de coordenadas con valores especı́ficos o
trayectorias de curvas para que el extremo final del robot pueda trazarlas.
La programación indistinta en lenguaje MATLAB de variables simbólicas y
numéricas resulta de gran utilidad sobre todo en el análisis cinemático, planeación
de trayectorias, guiado del robot en su espacio de trabajo y detección de puntos
singulares en la cinemática diferencial o en procesos donde involucra la matriz inversa
del jacobiano del robot.
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
274
Capı́tulo 5: Cinemática directa cartesiana
En el sitio WEB del libro se encuentran disponibles todos los programas fuentes en
MATLAB de las funciones de cinemática cartesiana, matrices de transformación
homogénea, cinemática inversa, jacobianos y determinantes, ası́ como diversas
aplicaciones de las configuraciones analizadas de robots manipuladores.
Para una rápida identificación de las librerı́as desarrolladas, a continuación se resume
la sintaxis del conjunto de funciones para cinemática directa cartesiana con pase de
parámetros de entrada y datos que retorna de cada las configuraciones de robots
manipuladores analizados.
La tabla 5.8 contiene la descripción de funciones de cinemática para la configuración
antropomórfica considerando los casos de estudio del péndulo robot, robot planar
vertical de 2 gdl y robot con movimiento tridimensional en su espacio de trabajo (3
gdl).
Tabla 5.8 Configuración antropomórfico
Función
Sintaxis
H01 =H pendulo()
[x0 , y0 , z0 ]=cinematica pendulo(β1, l1 , q1 )
Péndulo
q1 =cinv pendulo(x0, y0 )
H02 =H r2gdl()
Robot antropomórfico vertical planar de 2 gdl
[x0 , y0 , z0 ]=cinematica r2gdl(β1, l1 , q1 , β2 , l2, q2 )
J(q1 , q2 )=jacobiano r2gdl(β1 , l1, q1 , β2 , l2, q2 )
det[J(q1 , q2 )]=det jac r2gdl(β1 , l1 , q1 , β2, l2 , q2 )
[q1 , q2 ]=cinv r2gdl(l1, l2 , x0 , y0 )
H03 =H r3gdl()
Robot antropomórfico de 3
gdl
[x0 , y0 , z0 ]=cinematica r3gdl(β1, l1 , q1 , β2 , l2 , q2 , β3 , l3 , q3 )
J(q1 , q2 , q3 )=jacobiano r3gdl(β1, l1 , q1 , β2 , l2 , q2 , q3 )
det[J(q1, q2 , q3 )]=detjac r3gdl(β1, l1 , q1 , β2 , l2 , q2 , q3 )
[q1 , q2 , q3 ]=cinv r3gdl(β2 , β3, l2 , l3, x0 , y0 , z0 )
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
5.7
275
Resumen
En la tabla 5.9 se resume las funciones de transformación homogénea H03 , cinemática
directa, cinemática inversa, matriz jacobiano y su determinante del brazo robot en
configuración SCARA.
Tabla 5.9 Configuración SCARA
Función
Sintaxis
Matriz de transformación
homogénea
[x0 , y0 , z0 ]=cinematica SCARA(β1 , β2, l1 , l2, l3 , q1 , q2 , d3 )
Cinemática directa
J(q1 , q2 , d3)=jacobiano SCARA(β1, l1 , q1 , β2 , l2, q2 )
Jacobiano
Determinante
H03 =H SCARA()
del
jaco-
biano
det[J(q1 , q2 , d3 )]=detjac SCARA(β1 , l1, q1 , β2 , l2 , q2 )
[d3 , q2 , q1 ]=cinv SCARA(β1 , β2, l1 , l2, l3 , x0 , y0 , z0 )
Cinemática inversa
El conjunto de librerı́as para la configuración esférica se presenta en la tabla 5.10.
Tabla 5.10 Configuración esférica
Función
Sintaxis
Matriz de transformación
homogénea
[x0 , y0 , z0 ]=cinematica esferico(β1, β2 , l1, q1 , q2 , d3)
Cinemática directa
J(q1 , q2 , d3)=jacobiano esferico(β1, l1 , q1 , β2, l2 , q2 )
Jacobiano
Determinante
H03 = H esferico()
del
jaco-
det[J(q1 , q2 , d3 )]=detjac esferico(β1, l1 , q1 , β2 , l2, q2 )
biano
Cinemática inversa
[q1, q2, d3]=cinv esferico(β1, β2 , l1, x0 , y0 , z0 )
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
276
Capı́tulo 5: Cinemática directa cartesiana
La tabla 5.11 contiene las librerı́as del brazo robot en configuración cilı́ndrica.
Tabla 5.11 Configuración cilı́ndrica
Función
Sintaxis
Matriz de transformación
homogénea
[x0 , y0 , z0 ]=cinematica cilindrico(β1, l1 , q1 , d2, d3 )
Cinemática directa
J(q1 , q2 , d3 )=jacobiano cilindrico(β1, l1, q1 , β2, l2 , q2 )
Jacobiano
Determinante
H03 = H cilindrico()
del
jaco-
biano
det[J(q1 , q2 , d3 )]=detjac cilindrico(β1, l1 , q1 , β2 , l2, q2 )
[q1 , d2, d3 ]=cinv cilindrico(β1, l1, x0 , y0 , z0 )
Cinemática inversa
Para el caso del brazo robot cartesiano, las librerı́as que corresponde a esta
configuración se encuentran en la tabla 5.12. La cinemática directa resulta muy
simple debido a que su cinemática es lineal: [ x0 y0 z0 ]T = [ d1 d2 d3 ]T , por
lo que el jacobiano es la matriz identidad y su determinante es unitario, es decir:
det[J(d1 , d2 , d3 )] = 1. La cinemática inversa también es lineal [ d1 d2 d3 ]T =
[ x0 y0 z0 ]T .
Tabla 5.12 Configuración cartesiana
Función
Sintaxis
Matriz de transformación
homogénea
H03 = H cartesiano()
Cinemática directa
[x0 , y0 , z0 ]=cinematica cartesiano(d3, d2 , d1)
Cinemática inversa
[d3 , d2 , d1]=cinv cartesiano(x0 , y0 , z0 )
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
277
Parte II: Referencias selectas
Parte II: Referencias selectas
E
xiste una extensa literatura sobre la cinemática directa de robots manipuladores. Particularmente se recomienda al lector la siguiente bibliografı́a:
J. Denavit &
R.S. Hartenberg.
“A kinematic notation for lower-pair mechanisms based on matrices”.
Trans ASME J. Appl. Mech, 23:215-221,1955
R.S. Hartenberg & J. Denavit. “Kinematic synthesis of linkages”.
McGraw-Hill, New York, NY, 1964.
R.M. Murray,
Z.
Li
and S.S.
Sastry.
“A mathematical introduction to robotic manipulation”.
CRC
Press (1994).
Lorenzo Sciavicco & Bruno Siciliano.
“Modeling and control for robot manipulators”.
McGraw
Hill
International Editions. 1996.
Mark W.
Spong
and
Seth
Hutchinson,
M.
Vidyasagar.
“Robot modeling and control”. John Wiley and Sons, Inc. 2006.
John J. Craig. “Robótica”. Tercera edición. Pearson Prentice-Hall.
2006.
Fernando Reyes.
“Robótica: control de robots manipuladores ”.
Editorial Alfaomega, 2011.
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
278
Capı́tulo 5: Cinemática directa cartesiana
Parte II: Problemas propuestos
E
n esta sección se presenta una serie de ejercicios con la finalidad de que el lector
mejore sus conocimientos sobre métodos numéricos.
Capı́tulo 3 Preliminares matemáticos
3.1 Considere los siguientes vectores:
⎡ ⎤
x0
⎢ ⎥
x = ⎣ y0 ⎦
z0
⎡
8
⎤
⎢
⎥
y = ⎣ 10.2 ⎦
3.12
Realizar un programa en MATLAB para obtener:
(a) La norma euclidiana de cada vector.
(b) El producto punto: x · y.
(c) Calcular el ángulo θ que existe entre los vectores.
Realice la programación considerando variables simbólicas y aspectos
numéricos.
3.2 Considere las siguientes coordenadas en
⎡ ⎤
x0
⎢ ⎥
⎣ y0 ⎦ =
z0
el sistema de referencia Σ0 (x0 , y0 , z0 )
⎡
⎤
8
⎢
⎥
⎣ 10.2 ⎦
3.12
Realizar un programa en MATLAB que realice las siguientes rotaciones:
(a) Alrededor del eje z0 por un ángulo
π
2.
(b) Alrededor del eje x0 por un ángulo − π2 .
(c) Alrededor del eje y0 por un ángulo 43 π
Analice y discuta sus resultados.
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
279
Parte II: Problemas propuestos
3.3 Sea Ry ( θ ) ∈ SO(3) una matriz ortogonal donde θ ∈ IR es el ángulo de
rotación alrededor del eje y. Implementar el código MATLAB correspondiente
para comprobar las siguientes propiedades (usar programación simbólica y
numérica):
(a) Ry ( 0 ) = I, I ∈ IR3×3 es la matriz identidad.
(b) Ry ( θ )Ry ( β ) = Ry ( β )Ry ( θ ) = Ry ( θ + β ) = Ry ( β + θ ).
(c) R−1
y (θ) = Ry ( −θ )
(d) RTy ( θ ) = R−1
y (θ).
(e) Ry ( θ )RTy ( θ ) = RTy ( θ )Ry ( θ ) = I.
(f) det[Ry ( θ )] = 1 si el sistema de referencia Σ0 (x0 , y0 , z0 ) es seleccionado
por la regla de la mano derecha, en otro caso det[Ry ( θ )] = −1
3.4 Describir el significado de una matriz de rotación.
3.5 Describir el significado de una matriz homogénea.
3.6 ¿Cuáles son las principales propiedades de una matriz homogénea?
Capı́tulo 4 Cinemática
4.1 ¿Cómo define cinemática?
4.2 ¿Qué es cinemática directa?
4.3 ¿Cuáles son las caracterı́sticas principales de la cinemática directa?
4.4 ¿Qué es cinemática inversa? Explicar claramente su función principal.
4.5 Explicar el concepto de cinemática diferencial.
4.6 ¿Qué es el jacobiano del robot?
4.7 ¿Cuál es la diferencia que existe entre el jacobiano analı́tico con el geométrico?
4.8 Explicar el concepto de posición y rotación de robots manipuladores.
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
280
Capı́tulo 5: Cinemática directa cartesiana
4.9 Explicar el significado de una singularidad.
4.10 Explicar el concepto de posición de casa de un robot manipulador.
4.11 ¿Cuál es la diferencia entre rotación y orientación?
4.12 ¿Por qué la expresión analı́tica de la cinemática directa depende de la posición
de casa?
4.13 ¿Cuáles son los tipos de robots industriales que existen?
4.14 Explicar cinco aplicaciones para cada una de las configuraciones: antropomórfica, SCARA, esférica, cilı́ndrica y cartesiana.
4.15 ¿Qué robot representa mayores ventajas?
4.16 Desde el punto de vista mecánico, ¿qué tipo de robot presenta mayor
complejidad?
4.17 Explicar las ventajas que presenta la convención Denavit-Hartenberg.
4.18 Describir los parámetros de análisis que presenta la convención DenavitHartenberg.
4.19 De manera general, se requieren seis coordenadas para posicionar y orientar
el extremo final del robot en el espacio tridimensional: ¿Por qué la convención
Denavit-Hartenberg sólo utiliza cuatro parámetros?
4.20 ¿Cuales son las principales hipótesis que debe satisfacer la convención DenavitHartenberg?
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
Parte II: Problemas propuestos
281
Capı́tulo 5 Cinemática directa cartesiana
5.1 Considere un sistema mecatrónico denominado centrı́fuga como el que se
muestra en en la figura 5.27.
Figura 5.27 Centrı́fuga.
Obtener:
(a) La tabla de parámetros DH.
(b) La matriz homogénea.
(c) Cinemática directa.
(d) Cinemática inversa.
Asimismo, realizar un programa que despliegue en forma simbólica los
anteriores incisos.
5.2 Considere el robot manipulador en configuración cartesiana. Suponga que se
selecciona el sistema de referencia fijo Σ0 (x0 , y0 , z0 ) de tal forma que el eje z0
determina el desplazamiento de la variable d1 como se muestra en la figura
5.28.
Llevar a cabo los pasos necesarios en los respectivos sistemas de referencia
Σ0 (x0 , y0 , z0 ), Σ1 (x1 , y1 , z1 ), Σ2 (x2 , y2 , z2 ) para obtener el modelo de cinemática directa. ¿En este caso se requiere un sistema auxiliar Σ2a(x2a, y2a, z2a)?
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
282
Capı́tulo 5: Cinemática directa cartesiana
¿Este procedimiento presenta alguna ventaja con el desarrollado en la sección
5.6 del robot cartesiano?
Figura 5.28 Robot cartesiano.
Del procedimiento obtenido, deducir el jacobiano del robot.
5.3 Considere un robot industrial en la configuración antropomórfica; desarrolle
un programa en MATLAB para que el extremo final del robot trace una rosa
polar de 12 pétalos con centro en [ 0.3, −0.3, 0.5 ]m.
5.4 Desarrollar un programa en MATLAB para que el extremo final de un
brazo robot en la configuración SCARA trace la palabra ROBOT en
estilo caligráfico. Dicha palabra deberá iniciar en las siguientes coordenadas:
[ x0 , y0 , z0 ]T = [ 0.3, 0.5, 0.2] m. La longitud y altura de la palabra es propuesta
por el usuario.
5.5 Desarrollar un programa en MATLAB para que el extremo final de un brazo
robot en la configuración cilı́ndrica trace la palabra HOLA en estilo italizada.
Dicho trazo deberá empezar en las siguientes coordenadas: [ x0 , y0 , z0 ]T =
[ 0.4, 0.7, 0.8 ] m.
5.6 Desarrollar un programa en MATLAB para que el extremo final de un
brazo robot en la configuración esférica trace la palabra HOLA con tipo de
letra romana. Dicha palabra deberá comenzar en las siguientes coordenadas:
[ x0 , y0 , z0 ]T = [ 0.35, 0.6, 0.2] m.
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
Parte III
Dinámica
La Dinámica como área de las ciencias exactas permite explicar todos los fenómenos
fı́sicos de un sistema mecatrónico o robot manipulador y llevar a cabo procesos de
simulación. Bajo esta temática se ubican los objetivos de la Parte III compuesta
por dos capı́tulos: Capı́tulo 6 Dinámica, presenta la técnica de simulación para
sistemas mecatrónicos y robots manipuladores en base a una ecuación diferencial
ordinaria de primer orden descrita por variables de estado; modelos dinámicos de
varios sistemas mecatrónicos y robots manipuladores se desarrollan en código fuente
para MATLAB. La técnica de mı́nimos cuadrados para obtener el valor numérico
de los parámetros del modelo dinámico se detalla en el Capı́tulo 7 Identificación
paramétrica.
Capı́tulo 6 Dinámica
presenta la forma de simular modelos
dinámicos descritos en variables de estado. Generalmente la mayorı́a de los
sistemas fı́sicos tienen su modelo dinámico con ecuaciones diferenciales de
orden mayor o igual a dos en variables fı́sicas o generalizadas. La técnica de
simulación consiste en convertir el modelo dinámico a una ecuación diferencial
de primer orden mediante un adecuado cambio de variables de estado. Se
desarrollan librerı́as en lenguaje MATLAB para simular la dinámica de
sistemas mecatrónicos y robots manipuladores.
Capı́tulo 7 Identificación paramétrica
presenta
cincos esquemas de regresión lineal para realizar identificación paramétrica
de sistemas mecatrónicos y robots manipuladores. Los esquemas de regresión
presentados son los modelos: dinámico, dinámico filtrado, energı́a, potencia y
potencia filtrada. La técnica de mı́nimos cuadrados recursivo es implementada
como librerı́a para MATLAB cubriendo los casos: escalar y multivariable de
sistemas dinámicos.
La Parte III también incluye:
Referencias selectas
Problemas propuestos
Dinámica
Capı́tulo
6
6.1 Introducción
6.2 Estructura matemática para simulación
6.3 Sistema masa-resorte-amortiguador
6.4 Sistema lineal escalar
6.5 Centrı́fuga
6.6 Péndulo
6.7 Robot de 2 gdl
6.8 Robot de 3 gdl
6.9 Robot cartesiano de 3 gdl
6.10 Resumen
Objetivos
Presentar la técnica de simulación de modelado de robots manipuladores
y sistemas mecatrónicos a través de la estructura de ecuación diferencial
ordinaria en variables fase.
Objetivos particulares:
Estructura matemática fundamental de simulación.
Sistema masa resorte amortiguador.
Centrı́fuga.
Robot antropomórfico: péndulo, robot de 2 y 3 gdl.
Robot cartesiano de 3 gdl.
Aplicaciones en control de posición.
6.1
Introducción
287
6.1 Introducción
R
obots manipuladores son sistemas mecánicos muy complejos cuya descripción
analı́tica requiere de ecuaciones diferenciales. La naturaleza no lineal,
multivariable y fuerte acoplamiento en su comportamiento dinámico ofrece un amplio
espectro en la formulación de problemas de control teóricos y prácticos. El modelo
dinámico del robot manipulador permite explicar todos los fenómenos fı́sicos que
se encuentran en su estructura mecánica, tales como efectos inerciales, fuerzas
centrı́petas y de Coriolis, par gravitacional y fricción, los cuales son fenómenos fı́sicos
propios de la naturaleza dinámica del robot. Hay varios métodos de modelado de la
fı́sica como el de Newton o el de Hamilton. Sin embargo, la mejor opción como
metodologı́a de modelado la representa las ecuaciones de movimiento de EulerLagrange debido a las propiedades matemáticas que se deducen de manera natural,
ya que facilitan el análisis y diseño de algoritmos de control.
El modelo dinámico de robots manipuladores es fundamental en aplicaciones de
simulación, diseño y construcción del sistema mecánico, ası́ como en análisis y diseño
de algoritmos de control. En el área de simulación el modelo dinámico es estratégico
debido a que puede reproducir todos los fenómenos fı́sicos del robot sin la necesidad
de usar un robot real (realidad virtual), y esta caracterı́stica resulta clave para
evaluar algoritmos de control, técnicas de planeación de trayectorias, programación
de aplicaciones industriales, etc. La simulación es el empleo del modelo dinámico
para analizar y describir su comportamiento dinámico en una computadora o sistema
mı́nimo digital y de ahı́ inferir aplicaciones. Es importante no confundir simulación
con animación, ya que son procesos diferentes; la animación no requiere incorporar
efectos dinámicos en el movimiento del robot, generalmente son ecuaciones estáticas
como la cinemática directa.
La construcción de un robot se fundamenta en el modelo dinámico; los esquemas y
planos de ingenierı́a de los eslabones se deducen directamente del modelo dinámico
y se trasladan a un programa CAD para su maquinado y construcción mecánica.
De esta forma, un robot industrial puede ser estudiado y se pueden hacer las
adecuaciones pertinentes antes de llegar a la etapa de construcción fı́sica.
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
288
Capı́tulo 6: Dinámica
6.2 Estructura matemática para simulación
L
a gran mayorı́a de los sistemas mecánicos contienen como parte del modelo
dinámico ecuaciones diferenciales de orden mayor o igual a 2. Sin embargo,
para propósitos de simulación es recomendable transformar ese modelo a un sistema
dinámico compuesto por una ecuación diferencial ordinaria de primer orden debido
a que este tipo de ecuaciones es conocida, está ampliamente documentada y es fácil
de programar.
La estructura de una ecuación diferencial ordinaria (ode) de primer orden tiene la
siguiente forma:
ẋ = f (x)
(6.1)
donde x ∈ IRn se conoce como variable de estado fase, la cual proporciona
información interna sobre la dinámica del sistema, es una función continua en el
tiempo x = x(t), n ∈ N es un número natural que indica la dimensión euclidiana,
la derivada temporal de la variable de estado ẋ ∈ IRn existe y también es continua
en el tiempo ẋ = ẋ(t). La notación ẋ significa ẋ = dxdt(t) . La función f ∈ IRn es un
mapa vectorial continua en la variable de estado x(t).
La estructura matemática ẋ = f (x) es para ambos sistemas: lineales y no lineales.
La caracterı́stica de ser lineal o no lineal es referente a la variable de estado.
El modelo (6.1) se conoce como sistema dinámico autónomo, debido a que la
variable tiempo t se encuentra presente de manera implı́cita, es decir no aparece
como parte de la estructura matemática, se encuentra incluida como parte de las
propiedades de la variable de estado x(t). La gran mayorı́a de los sistemas mecánicos
corresponden a esta forma, sobre todo considerando que no hay variación temporal
de sus parámetros, los cuales se consideran constantes al menos en un tiempo
suficientemente grande.
De otra manera se tendrı́a como parte de la ecuación (6.1) dependencia explı́cita
del tiempo, es decir ẋ = f (x, t); a este tipo de sistemas se les conoce como sistema
dinámicos no autónomos, los cuales no non objetos de estudio en la presente obra.
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
6.2
289
Estructura matemática para simulación
♣ Ejemplo 6.1
Convertir a la forma ẋ = f (x) el siguiente sistema lineal expresado por la función
de transferencia:
y
u
=
2
s2 + 3s + 1
donde y ∈ IR es la respuesta del sistema, u ∈ IR es la entrada.
Solución
El concepto de función de transferencia es exclusivo para sistemas lineales, se define
como la relación de la salida y la entrada cuando las condiciones iniciales del sistema
son cero.
Es necesario aclarar que el término s que aparece en una función de transferencia
significa frecuencia y es un número imaginario formado como: s = jw, siendo w una
variable que indica frecuencia, cuyas unidades son rad/seg. Cuando transformamos
la función de transferencia a una ecuación diferencial, entonces s adquiere otra
d
interpretación, en este caso funciona como un operador s = dt
. Bajo este escenario,
d d
d2
2
debe interpretarse que s = dt dt = dt2 (teniendo en mente condiciones iniciales
cero).
Por lo tanto, el procedimiento para convertir la función de transferencia del sistema
a una ecuación diferencial ordinaria de primer orden es el siguiente:
y
u
2
⇒ s2 y + 3sy + y = 2u
+ 3s + 1
⇒ ÿ + 3ẏ + y = 2u
=
s2
donde se ha empleado s2 y = ÿ, sy = ẏ. La notación ẏ =
dy(t)
dt ,
ÿ =
d2 y(t)
dt2
Ahora, se requiere un cambio de variable de estado, sea x1 = y, x2 = ẋ1 ,
ẋ2 = −3x2 − x1 + 2u entonces:
ÿ + 3ẏ + y = 2u ⇒
ẋ2 +3 x2 + x1 = 2u
ÿ
ẏ
y
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
290
Capı́tulo 6: Dinámica
d x1
dt x
2 ẋ
=
0
1
−1
−3
x1
x
2
f ( x)
+
0
2
u
donde x = [ x1 , x2 ]T .
Cuando la ecuación diferencial original se encuentra en la forma de variables
fase ẋ = f(x), entonces el sistema dinámico (convertido) ya tiene la estructura
matemática adecuada para realizar simulación usando la función ode45(· · ·).
♣ Ejemplo 6.2
Convertir a la forma ẋ = f(x) el siguiente sistema no lineal:
α2 ÿ + α1 sen(ẏ) + α0 cos(y) = βu
donde y ∈ IR es la respuesta del sistema, u ∈ IR es la entrada, α2 , α1 , α0 , β ∈ + .
Solución
La función de transferencia es exclusiva de sistemas lineales, para un sistema no
lineal no existe dicho concepto.
La conversión a la forma de ecuación diferencial ordinaria de primer orden en
variables fase se realiza de la siguiente forma. Sea x1 = y, x2 = ẋ1 , x2 =
(−α1 sen(x2 )−α0 cos(x1 )+βu)/α2 . Por lo tanto, la estructura matemática solicitada
es:
x2
d x1
=
1
dt x2
[ βu − α1 sen(x2 ) − α0 cos(x1 ) ]
α0
ẋ
f ( x)
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
6.3
291
Sistema masa-resorte-amortiguador
6.3 Sistema masa-resorte-amortiguador
U
no de los sistema mecánicos tı́picos que ha sido estudiado ampliamente
en cursos de control automático de la carreras de ingenierı́a mecatrónica y
electrónica es el sistema masa resorte amortiguador. La importancia de este sistema
radica en comprender su comportamiento dinámico para entender el funcionamiento
cualitativo de otros sistemas, por ejemplo: circuitos eléctricos (resistencia, capacitor,
inductancia), esquemas de control (algoritmo proporcional derivativo de robots
manipuladores).
Considere un sistema dinámico como el que se muestra en la figura 6.1, formado
por una masa m que se encuentra unida a un resorte de rigidez k y constante
de amortiguamiento b debido a la viscosidad del medio ambiente. La entrada
está especificada por una fuerza f y el desplazamiento de la masa por x.
Figura 6.1 Sistema masa resorte amortiguador.
El modelo dinámico que describe al sistema masa resorte amortiguador se encuentra
dado por:
f
Fuerza externa
=
mẍ
Fuerza inercial
+
bẋ
Fricción viscosa
+
kx
Ley de Hooke
La función de transferencia del sistema masa resorte amortiguador está dada por:
x
1
=
(6.2)
2
f
ms + bs + k
la ecuación caracterı́stica de los polos del sistema está dada por
√
−b ± b2 − 4mk
s =
(6.3)
2k
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
292
Capı́tulo 6: Dinámica
El sistema masa resorte amortiguador oscilará cuando se cumpla (b2 − 4mk) < 0, y
será amortiguado si (b2 − 4mk) > 0.
La conversión de la ecuación (6.2) a una ecuación diferencial ordinaria de primer
orden en variables fase ẋ = [ x, ẋ ]T se realiza de la siguiente forma:
ẋ
d x
(6.4)
=
dt ẋ
m−1 [ f − bẋ − kx ]
ẋ
f ( x)
♣ Ejemplo 6.3
Considere el modelo dinámico del sistema masa resorte amortiguador (6.4)
utilizando los valores numéricos de los parámetros del sistema de la tabla 6.1.
Realizar un programa en MATLAB para graficar el desplazamiento lineal x y la
velocidad de la masa cuando se le aplica una fuerza constante f = 50N durante 5
segundos y después de este tiempo desaparece la fuerza aplicada.
Tabla 6.1 Parámetros del sistema masa resorte amortiguador
Masa
Fricción viscosa
m=5 kg
b =0.16 kg/seg
Rigidez
k =0.6 kg/seg2
Solución
El programa en código MATLAB que contiene el modelo dinámico del sistema masa
resorte amortiguador (6.4) se describe en el cuadro 6.2. En este programa la variable
x mra representa el desplazamiento lineal que tiene de la masa al aplicarse una
fuerza constante de 5N durante 5 segundos. Por medio del programa 6.1 se realiza
la simulación del sistema.
En la figura 6.2 (modo oscilatorio) se muestra el desplazamiento lineal x mra
y la velocidad de movimiento xp mra que tiene la masa. Por los valores que
presentan en la tabla 6.1 el sistema masa resorte amortiguador satisface la condición
(b2 − 4mk) < 0 de la ecuación (6.3) que corresponde a un oscilador, por eso cuando
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
6.3
Sistema masa-resorte-amortiguador
293
desaparece la fuerza constante después de 5 segundos el sistema se queda oscilando
permanentemente (el tiempo de simulación es de 100 segundos).
En realidad el sistema masa resorte amortiguador cuando se encuentra en la
configuración de oscilador debido a los valores numéricos de sus parámetros, el
sistema empezará a oscilar con una condición diferente a cero, independientemente
de la magnitud de la fuerza aplicada.
Ahora, supóngase que los valores de los parámetros son: m=5 kg, la constante de
rigidez del resorte k = 2 kg/seg2 y el coeficiente de fricción viscosa b = 4 Kg/seg; con
estos valores se configura el sistema masa resorte amortiguador en modo amortiguado
de tal forma que en la ecuación (6.3) satisface (b2 − 4mk) > 0, es decir el factor de
amortiguamiento es adecuado para evitar oscilaciones y grandes sobre impulsos en
la respuesta transitoria.
La figura 6.2 muestra la simulación en modo amortiguado. Únicamente, durante los
primeros 5 segundos se le aplica la fuerza de f =5 N. Después de este transitorio
la masa regresa a su posición inicial, restableciéndose la elongación del resorte y
desapareciendo el efecto de amortiguamiento. Obsérvese que cuando desaparece
la fuerza constante, la posición del sistema es atraı́da por el punto de equilibrio,
quedando en reposo.
Código Fuente 6.1
Simulador de smr.m
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés
%Capı́tulo 4 Dinámica. Programa smr simu.m
Simulador de smr.m
1
clc; clear all; close all; format short
2
%parámetros de simulación
ti=0; h=0.0025; tf = 100; ts=ti:h:tf;
3
4
5
opciones=odeset(’RelTol’,1e-3,’InitialStep’, 2.5e-3, ’MaxStep’,2.5e-3);
[t,x]=ode45(’mra’,ts,[0; 0],opciones);
6
plot(t,x(:,1),t,x(:,2))
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
294
Capı́tulo 6: Dinámica
Código Fuente 6.2
Sistema masa resorte amortiguador
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés
%Capı́tulo 4 Dinámica. Programa mra.m
%requiere del programa mra simu.m
Sistema masa resorte amortiguador
1
function xp =mra(t,x)
2
%vector de estados
3
4
%posición articular
x mra=x(1);
5
%velocidad articular
6
xp mra=x(2);
7
%parámetros del sistema masa resorte amortiguador
8
%masa
m=5;
9
10
%constante de rigidez del resorte
11
k=0.6;
12
13
%amortiguamiento: coeficiente de fricción viscosa
b=0.16;
14
%fuerza aplicada durante 5 segundos
15
18
if (t<5)
f=5;
else
f=0;
19
end
20
21
%aceleración del sistema masa resorte amortiguador
xpp mra=(f-b*xp mra-k*x mra)/m;
22
%vector de salida
23
xp=[ xp mra ; %velocidad articular
xpp mra] ; %aceleración articular
16
17
24
25
end
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
6.4
295
Sistema lineal escalar
Figura 6.2 Respuesta del sistema masa resorte amortiguador.
6.4 Sistema lineal escalar
S
istemas lineales se utilizan ampliamente como estimadores de velocidad,
filtrado en robots manipuladores y sistemas mecatrónicos. La estructura
matemática básica de un sistema lineal escalar de primer orden en variables fase
está dada por:
ẋ = −ax + bu
(6.5)
y = cx
(6.6)
donde a, b, c ∈ IR+ son los parámetros del sistema, x, ẋ ∈ IR son la variable de
estado y la derivada temporal de la variable de estado, respectivamente; la entrada
es u ∈ IR y la salida del sistema está dada por y ∈ IR.
La función de transferencia del sistema lineal (6.5)-(6.6) tiene la siguiente forma:
y
u
=
1
b 1
cb
=c
=c b1 a
s+a
a Ts + 1
as + 1
(6.7)
donde T = a1 , se conoce como la constante de tiempo. Para una constante de tiempo
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
296
Capı́tulo 6: Dinámica
pequeña la respuesta del sistema es rápida, de otra manera si la constante de tiempo
es grande, la respuesta del sistema es lenta.
Por otro lado, la función de transferencia representa un filtro pasa-bajas, donde a
significa la frecuencia de corte, también determina el ancho de banda del filtro y ab
es la ganancia (frecuencia cero).
6.4.1
6.4.1. Estimador de velocidad y filtrado
Entre las aplicaciones del sistema lineal (6.5)-(6.6) se encuentra como estimador de
velocidad y filtrado para robots manipuladores y sistemas mecatrónicos. Por ejemplo,
si qi representa la i-ésima posición articular de un robot, la manera de estimar la
i-ésima velocidad q̇i es por medio de la siguiente forma:
q̇i ≈ Ḟi = −λFi + λqi
(6.8)
donde λ es la frecuencia de corte del filtro de ganancia unitaria, Fi es i-ésimo estado
del filtro, representa la señal filtrada de la entrada (qi ), y Ḟi es la estimación de la
velocidad q̇i .
Para comprobar la afirmación que Fi representa la señal filtrada de qi , considere la
función de transferencia de (6.8):
Fi =
λ
qi
λ+s
observe que la señal de entrada qi es procesada por el filtro pasa bajas
(6.9)
λ
s+λ .
Además, la derivada temporal de la señal filtrada de la entrada qi aproxima a la
derivada temporal de q̇i (velocidad), es decir q̇i ≈ Ḟi ; este procedimiento es válido
para cualquier articulación del robot.
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
6.4
297
Sistema lineal escalar
♣ Ejemplo 6.4
Escribir un programa en MATLAB para analizar la respuesta del sistema lineal
(6.5)-(6.6) para una entrada unitaria u = 1.
Solución
El programa que realiza la implementación del sistema lineal escalar de primer orden
(6.5-6.6) se encuentra en el cuadro 6.3. La señal de entrada u es una constante de
magnitud unitaria. El programa principal que realiza la simulación se encuentra
en el cuadro 6.4. El tiempo de simulación es de 5 segundos y la respuesta a la
entrada unitaria no contiene sobre impulsos en régimen transitorio tal y como se
describe en la figura 6.3. Dentro de las caracterı́sticas en la respuesta a un escalón o
entrada constante de un sistema lineal, el transitorio no presentará sobre impulsos
y de manera suave convergerá al valor de la entrada. En la primera constante de
tiempo T , la respuesta ha alcanzado el 63.2 % del valor de la magnitud de la entrada
u. Para este ejemplo la constante de tiempo es T = 13 = 0.3333 segundos, en la
segunda constante de tiempo 2T la respuesta del sistema ha alcanzado el 86.5 % del
valor final.
Código Fuente 6.3
Sistema lineal escalar
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés
%Capı́tulo 4 Dinámica. Programa sle.m
Sistema lineal escalar
1
2
3
function xp =sle(t,x)
a=3;
b=3;
4
u=1;
xp=-a*x+b*u;
5
6
end
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
298
Capı́tulo 6: Dinámica
Para las constantes de tiempo 3T, 4T y 5T la respuesta tiene 95 %, 98.2 % y 99.3 %
del valor final, respectivamente. Para finalidades prácticas después de 5T , se puede
decir que el sistema ha entrado en estado estacionario.
Figura 6.3 Respuesta a un escalón del sistema lineal escalar.
Código Fuente 6.4
sle simu
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés
%Capı́tulo 4 Dinámica. Programa sle simu.m
% este programa requiere del archivo: sle.m
sle simu
1
clc; clear all; close all;
2
format short
3
%tiempo de simulación (segundos)
ti=0; h=0.0025; tf = 5;
ts=ti:h:tf; %vector tiempo
4
5
6
7
opciones=odeset(’RelTol’,1e-3,’InitialStep’,2.5e-3,’MaxStep’,2.5e-3);
[t,x]=ode45(’sle’,ts,0,opciones);
8
plot(t,x)
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
6.4
299
Sistema lineal escalar
♣ Ejemplo 6.5
Escribir un programa en MATLAB para que un sistema lineal escalar realice el
filtrado de una señal contaminada con ruido u = 5 sen(t) + ruido(t).
Solución
Considere una señal u(t) con una envolvente de ruido de la siguiente forma: u =
5 sen(t) + random(’Normal’,t,t), se empleará el sistema lineal ẋ = −3x + 3u(t) para
obtener la señal filtrada de la entrada u(t). El programa que implementa la función
filtro a través de un sistema lineal se encuentra en el cuadro 6.5. Como generador
de ruido se emplea la función de datos aleatorios random utilizando distribución
’Normal’ de datos, la cual se añade a la función senoidal 5 sen(t).
El programa principal que realiza la simulación se encuentra en el cuadro 6.6, en
la lı́nea 7 se resuelve numéricamente el filtro, en la lı́nea 8 se reproduce la señal
de entrada contaminada, y se compara con la señal libre de ruido contenida en la
variable x(t) (lı́neas 8 y 9). La figura 6.4 muestra la comparación entre la señal
original u con ruido y filtrada (figura izquierda).
Código Fuente 6.5
Aplicación filtrado
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés
%Capı́tulo 4 Dinámica. Programa filtro.m
Aplicación filtrado
1
2
3
function xp=filtro(t,x)
u=5*sin(t)+random(’Normal’,t,t);
a=3;
4
b=3;
xp=-a*x+b*u;
5
6
end
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
300
Capı́tulo 6: Dinámica
En la gráfica derecha de la figura 6.4 se presentan los detalles de la señal filtrada;
observe que esta señal está restaurada lo mejor posible después de haber sido
contaminada con ruido. El filtro tiene un parámetro de sintonı́a llamado frecuencia
de corte, que en este caso es a = 3rad/seg. Este parámetro puede adquirir otro valor
para mejorar la calidad de filtrado de la señal de entrada u.
Figura 6.4 Filtrado de señales.
Código Fuente 6.6
sle simu
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés
%Capı́tulo 4 Dinámica. Programa sle simu.m
% este programa requiere del archivo: filtro.m
sle simu
1
2
3
4
5
6
7
8
9
10
clc; clear all; close all;
format short
%tiempo de simulación (segundos)
ti=0; h=0.0025; tf = 5;
ts=ti:h:tf; %vector tiempo
opciones=odeset(’RelTol’,1e-3,’InitialStep’,2.5e-3,’MaxStep’,2.5e-3);
[t,x]=ode45(’filtro’,ts,0,opciones);
u=5*sin(t)+random(’Normal’,t,t);
subplot(2,2,1); plot(t,u)
subplot(2,2,2); plot(t,x)
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
6.5
301
Centrı́fuga
6.5 Centrı́fuga
L
a centrı́fuga es un sistema mecatrónico que se describe en la figura 6.5, el cual
consta de un servomotor que gira alrededor del eje z0 , el ángulo de rotación
está descrito por la variable q1 . La distancia del origen del sistema de referencia
fijo Σ0 (x0 , y0 , z0 ) al extremo final está representado por l1 + l2 sen(δ), donde l1 es
la longitud del servomotor más la longitud del rotor, l2 es la longitud de la barra
metálica que se encuentra acoplada al rotor, y δ es un ángulo constante que significa
la inclinación de la barra l2 con respecto a la horizontal, lc2 representa el centro
de masa y el momento de inercia I del servomotor de la centrı́fuga. El movimiento
rotatorio de la centrı́fuga describe un cı́rculo sobre el plano x0 − y0 . La acción de la
gravedad g se encuentra en dirección contraria al eje z0 .
Figura 6.5 Centrı́fuga.
El modelo dinámico de una centrı́fuga es una ecuación diferencial de segundo orden
expresada de la siguiente manera:
τ
2 sen(δ) + I
= [ mlc2
sm ] q̈ + bq̇ + fc signo(q̇) + fe [1 − |signo(q̇)|]
(6.10)
La ecuación (6.10) puede ser convertida a una ecuación diferencial ordinaria de
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
302
Capı́tulo 6: Dinámica
primer orden de la forma ẋ = f (x), a través de la siguiente expresión:
q̇1
d q1
(6.11)
=
−1
2 sen(δ) + I
dt q̇1
[ m1 lc2
[ τ1 − b1 q̇ − fc1 signo(q̇1) − fe1 [1 − |signo(q̇1 )|] ]
sm ]
ẋ
f ( x)
6.6.
♣ Ejemplo 6.6
Considere el modelo dinámico de la centrı́fuga expresado como una ecuación
diferencial de primer orden (6.11). Tomar en cuenta los valores numéricos de los
parámetros de la centrı́fuga que aparecen en la tabla 6.2. Realizar un programa
en MATLAB para que la centrı́fuga gire a una revolución por segundo en ambos
sentidos (positivo/negativo).
Tabla 6.2 Parámetros de la centrı́fuga
Masa
de
la
centrı́fuga
Fricción viscosa
m1 =5 kg
b1 =0.12 Nm-seg/rad
Centro de ma-
Longitud de la
sa de la barra
barra
lc2=0.01m
l2 =0.15m
Fricción
de
Coulomb
fc1 =0.11 Nm
Fricción estática
fe1 =0.09 Nm
Momento
de
Ángulo de in-
inercia
del
clinación de la
rotor
Ism =0.08 Nm-seg2 /rad
barra
δ=
45π
180
rad
Solución
El modelo numérico se obtiene combinando el modelo dinámico (6.11) con los valores
numéricos de los parámetros de la centrı́fuga contenidos en la tabla 6.2.
q̇1
d q1
(6.12)
=
1
dt q̇1
0.0804 [ τ1 − 0.12 q̇ − 0.11 signo(q̇1 ) − 0.09 [1 − |signo(q̇1 )|] ]
para lograr que el sistema mecatrónico oscile en ambos sentidos a una revolución
por segundo (360 grados/seg), se requiere que su movimiento sea generado por una
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
6.5
303
Centrı́fuga
energı́a tipo senoidal. Es decir, al servomotor se le aplica una energı́a de la forma
τ1 = 2 sen(t), para generar un movimiento cı́clico. Con este tipo de energı́a la
centrı́fuga funciona como una lavadora; cuando se requiera implementar la función
centrifugado, entonces el tipo de energı́a aplicada al servomotor es a través de un
par constante.
El programa centrifuga.m 6.7 contiene el modelo dinámico de la centrı́fuga
con los parámetros de la tabla 6.2. La ecuación programada corresponde a la
estructura de variables de estados de los modelos 6.11 o 6.12. El programa principal
centrifuga simu 6.8 realiza la solución numérica del modelo dinámico de la
centrı́fuga usando el método de integración numérica de Runge-Kutta de la función
ode45(· · ·).
Código Fuente 6.7
centrifuga
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés
%Capı́tulo 4 Dinámica. Programa centrifuga.m
centrifuga
1
2
3
function xp = centrifuga( t,x)
m1=5; %masa de la centrı́fuga
lc2=0.01; % centro de masa de la barra inclinada
4
delta=45*pi/180; %ángulo de inclinación de la barra l2
I sm=0.08; % momento de inercia del rotor del servomotor
b1=0.12; fc1=0.11; fe1=0.09; %coeficientes de fricción
q1=x(1); % posición articular
5
6
7
8
qp1=x(2); % velocidad articular
tau=2*sin(t); % energı́a aplicada al servomotor
9
10
11
%aceleración rotacional de la centrı́fuga
qpp1=(tau1-b1*qp1-fc1*sign(qp1)-fe1*(1-abs(sign(qp1))))/
12
(m1*lc2*lc2*sin(delta)+I sm);
xp=[ qp1; qpp1 ]; % vector de salida
13
end
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
304
Capı́tulo 6: Dinámica
Código Fuente 6.8
centrifuga simu
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés
%Capı́tulo 4 Dinámica. Programa centrifuga simu.m
% este programa requiere del archivo: centrifuga.m
centrifuga simu
1
2
3
4
5
6
7
clc; clear all; close all;
format short
%tiempo de simulación (segundos)
ti=0; h=0.0025; tf = 10;
ts=ti:h:tf; %vector tiempo
opciones=odeset(’RelTol’,1e-3,’InitialStep’,2.5e-3,’MaxStep’,2.5e-3);
8
%integración numérica del modelo dinámico de la centrı́fuga
[t,x]=ode45(’centrifuga’,ts,[0; 0],opciones);
9
%graficar variable articular de la posición q1 =x(:,1)
10
%la velocidad de movimiento está dada por q̇1 =x(:,2)
11
plot(t,x(:,1),t,x(:,2))
La figura 6.6 describe la forma de
desplazamiento rotacional que tiene la variable articular q1 cuando se aplica energı́a de la forma
τ1 = 2 sen(t), el movimiento de
la centrı́fuga es oscilatorio; este es
el principio básico de funcionamiento de las lavadoras. Con el tipo de
energı́a senoidal aplicada al servomotor, la centrı́fuga gira 360 grados por segundo alrededor del eje z0 ,
cambiando su sentido de giro cada
Figura 6.6 Movimiento de la centrı́fuga.
Alfaomega
periodo t =6.28 segundos.
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
6.6
305
Péndulo
6.6 Péndulo
A
ctualmente el péndulo es una planta de estudio vigente debido a su dinámica
no lineal y sus aplicaciones prácticas, lo que lo convierte en un sistema clave
para propósitos de investigación cientı́fica, y en docencia representa una herramienta
pedagógica muy útil para la enseñanza de la dinámica de sistemas. La figura 6.7
muestra un péndulo robot el cual está formado por un servomotor y una barra
metálica de longitud l1 . El sistema de referencia se elige de tal manera que el eje z0
coincida con el eje de rotación del servomotor (perpendicular al plano de la hoja).
La acción de la gravedad g está dirigida en dirección del eje y0 negativo. El momento
de inercia se denota por Ism , el centro de masa se representa como lc1 y la masa del
péndulo por m1 .
Figura 6.7 Péndulo robot.
El modelo dinámico de un péndulo-robot está dado por:
(6.13)
τ
=
[ mlc2
+ I ] q̈ + mglc sen(q) + bq̇ + fc signo(q̇) + fe [1 − |signo(q̇)|]
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
306
Capı́tulo 6: Dinámica
♣ Ejemplo 6.7
Considere el modelo dinámico del péndulo (6.13) y realice un programa en lenguaje
MATLAB para describir los fenómenos fı́sicos del sistema. Para llevar a cabo la
simulación, considere los valores numéricos de los parámetros del péndulo que
presenta la tabla 6.3.
Solución
El péndulo robot prototipo de la figura 6.7 tiene un servomotor de transmisión
directa con un torque máximo de 15Nm (±15 Nm para el primer y tercer cuadrante,
respectivamente). La longitud de la barra l1 es de 0.45m, los valores numéricos de
los parámetros que forman el modelo dinámico como el momento de inercia, centro
de gravedad, coeficientes de fricción viscosa, Coulomb y estática están concentrados
en la tabla 6.3.
Tabla 6.3 Parámetros del péndulo robot
Masa
m1 =3.88 kg
Centro de masa
Fricción viscosa
b1 =0.16 Nm-seg/rad
Longitud
lc1=0.081 m
l1 =0.45 m
Fricción de Coulomb
fc1 =0.19 Nm
Mom. de inercia del rotor
Fricción estática
fe1 =0.20 Nm
Cap. del servomotor
Ir1 =0.16 Nm-seg2 /rad
±15Nm
La masa m1 del péndulo incluye la masa de la barra, rotor del servomotor y tornillos.
El centro de masa lc1 del péndulo se calcula de la siguiente manera:
lc1 =
lcrotor mrotor + lcbarra mbarra + lctornillos mtornillos
mrotor + mbarra + mtornillos
El centro de masa del rotor lcrotor es cero debido a las propiedades geométricas del
servomotor, y además el origen del sistema de referencia Σ0 (x0 , y0 , z0 ) se coloca
en el centro de masa lcrotor . Los tornillos sujetan la barra metálica de aluminio al
rotor del servomotor; estos tornillos se encuentran igualmente espaciados sobre una
circunferencia cuyo centro pasa el eje de rotación, el cual está alineado con el eje
z0 . Por lo tanto, el centro de masa de los tornillos lctornillos es cero. El momento de
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
6.6
307
Péndulo
inercia del péndulo I1 es igual a la contribución del momento de inercia del rotor del
2
servomotor Ir1 más el momento de inercia de la barra de aluminio m1 lc1
(teorema
de ejes paralelos de inercias).
Los programas 6.9 y 6.10 realizan la simulación del péndulo. La energı́a aplicada al
servomotor del péndulo es tipo senoidal, por ejemplo de la forma τ1 = sin(t), donde
t es la evolución del tiempo. Este tipo de torque produce un movimiento oscilatorio
en su espacio de trabajo sobre el plano x0 − y0− , entonces se genera desplazamiento
rotacional q1 alrededor del eje z0 y velocidad de movimiento articular q̇1 ; con estas
variables es posible obtener la dinámica del péndulo tales como efector inercial (I1 q̈),
fricción (b1 q̇1 + fc1 signo ( q̇1 )) y par gravitacional (m1 glc1 sen(q1 )).
La figura 6.8 muestra la respuesta
del péndulo robot a una señal de entrada τ 1 = sen(t); se emplea la cinemática directa del péndulo (y0 vs
x0 ) para graficar el desplazamiento
del extremo final del robot en su espacio de trabajo. En este caso, la posición de casa del péndulo se ubica
sobre el eje y0− . La posición articular en función del tiempo q1 (t) toma un perfil parecido a la señal de
entrada τ1 (t); la variable articular
q̇1 (t) representa la velocidad de movimiento, y adquiere magnitudes de
0.5 rad/seg (90 grados/seg) en amFigura 6.8 Respuesta del péndulo.
bas direcciones (positivo/negativo).
El fenómeno de fricción viscosa y de Coulomb se grafica en función de la velocidad
de movimiento (fricción vs velocidad q̇1 ); note que dicho fenómeno disipativo se
encuentra ubicado dentro del primer y tercer cuadrante. La magnitud que alcanza el
fenómeno de fricción es de 0.4 Nm, que representa el 2.6 % de la capacidad máxima
del servomotor. El fenómeno de fricción convierte la energı́a cinética en energı́a
térmica.
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
308
Capı́tulo 6: Dinámica
Código Fuente 6.9
Péndulo
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés
%Capı́tulo 4 Dinámica. Programa pendulo.m
% este programa requiere del archivo pendulo simu.m
Péndulo
1
function xp =pendulo(t,x)
2
3
%vector de estados
q1=x(1); %posición articular
4
qp1=x(2); %velocidad articular
5
%parámetros del péndulo
m1=3.88; %masa
lc1=0.081; %centro de masa
6
7
8
g=9.81; %constante de aceleración gravitacional
I r1=0.16; %momento de inercia del rotor
9
10
11
%momento de inercial total del péndulo
I1=m1*lc1*lc1+I r1;
12
%fricción del péndulo
13
15
b1=0.16; %coeficiente de fricción viscosa
fc1=0.19; %fricción de Coulomb
fe1=0.2; %fricción de estática
16
%par aplicado al servomotor del péndulo
17
tau1=sin(t);
18
%aceleración articular del péndulo
qpp1=(tau1-b1*qp1-fc1*sign(qp1)- fe1*(1-abs(sign(qp1)))-m1*g*lc1*sin(q1))
/gamma1;
14
19
20
%vector de salida
xp=[ qp1 ; %xp(1)=qp1=x(2) velocidad articular
qpp1] ; %xp(2)=qpp1 aceleración articular
21
22
23
end
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
6.6
309
Péndulo
Código Fuente 6.10
péndulo simu
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 7 Identificación paramétrica.
péndulo simu
1
2
3
4
5
6
7
8
9
10
clear; close all;
clc;
format short g
x0=[0; 0]; %condición inicial
ti=0; h=0.0025; tf = 5; %tiempo de simulación
ts=ti:h:tf; %vector de tiempo
opciones=odeset(’RelTol’,1e-3,’InitialStep’,2.5e-3,’MaxStep’,2.5e-3);
%simulación del péndulo robot
[t, x]=ode45(’pendulo’,[ti:0.01:tf], x0,opciones);
11
%asignación de posiciones y velocidades articulares
q1 =x(:,1); %Posición articular
12
qp1=x(:,2); %Velocidad articular
13
%cinemática directa del péndulo
14
15
%posición de casa sobre el eje y0−
x ef=0.45*sin(q1);
16
y ef=-0.45*cos(q1);
17
%fenómeno de fricción
b1=0.16; %coeficiente de fricción viscosa
fc1=0.19; %fricción de Coulomb
18
19
20
21
22
23
24
25
26
fe1=0.20; %fricción estática
friccion=b1*qp1+fc1*sign(qp1)+fe1*(1-abs(sign(qp1)));
%gráficas de cinemática directa, posición, velocidad, y fricción, respectivamente.
subplot(2,2,1); plot(x ef,y ef)
subplot(2,2,2); plot(t,q1)
subplot(2,2,3); plot(t,qp1)
subplot(2,2,3); plot(qp,friccion)
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
310
Capı́tulo 6: Dinámica
6.7 Robot de 2 gdl
R
obots manipuladores de 2 gdl tienen amplias aplicaciones sobre el plano
x0 − y0 : pintado de objetos, ensamble, estibado, etc. Considere el robot
manipulador de 2 gdl que se muestra en la figura 6.9.
Figura 6.9 Robot manipulador de 2 gdl.
El modelo dinámico de un robot manipulador antropomórfico de 2 gdl está dado por
la siguiente expresión:
τ = M (q)q̈ + C(q, q̇)q̇ + g(q) + f f (q̇, f e )
=
2
2
+ m2l12 + m2 lc2
+ 2m2l1lc2 cos(q2 ) + I1 + I2
m1lc1
2
m2lc2
+ m2 l1lc2 cos(q2 ) + I2
2 + m l l cos(q ) + I
m2 lc2
2 1 c2
2
2
2 +I
m2 lc2
2
Alfaomega
M (q)
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
q̈
6.7
311
Robot de 2 gdl
+
g
−2m2 l1 lc2 sen(q2 )q̇2
−m2 l1 lc2 sen(q2 )q̇2
m2 l1 lc2 sen(q2 )q̇1
0
C(q ,q̇ )
q̇ +
lc1m1 sen(q1 ) + m2 l1 sen(q1 ) + m2 lc2 sen(q1 + q2 )
m2 lc2 sen(q1 + q2 )
g (q )
+
b1 q̇1 + fc1 signo(q̇1 ) + [1 − |signo(q̇1 )|]sat(τ1 ; f 1 )
b2 q̇2 + fc2 signo(q̇2 ) + [1 − |signo(q̇2 )|]sat(τ2 ; f 2 )
f f (q̇,f e )
La terminologı́a y significado de los parámetros del robot de 2 gdl se encuentran en
la tabla 6.4.
Tabla 6.4 Parámetros del robot planar de 2 gdl
Eslabón
Hombro
Codo
Significado
Notación
Valor
Masa del eslabón 1
m1
23.902 kg
Longitud del eslabón 1
l1
0.45 m
Inercia del eslabón 1
I1
1.266 Nm seg2/rad
Centro de masa del eslabón 1
lc1
0.091 m
Coeficiente de fricción viscosa
b1
2.288 Nm seg/rad
Coeficiente de fricción de Coulomb
fc1
7.17 Nm
Coeficiente de fricción estática
fe1
8.8 Nm
Masa del eslabón 2
m2
3.88 kg
Longitud del eslabón 2
l2
0.45 m
Inercia del eslabón 2
I2
0.093 Nm seg2/rad
Centro de masa del eslabón 2
lc2
0.048
Coeficiente de fricción viscosa
b2
0.175 Nm seg/rad
Coeficiente de fricción de Coulomb
fc2
1.734 Nm
Coeficiente de fricción estática
fe2
1.87 Nm
g
9.81 m/seg2
Aceleración debida a la gravedad
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
312
Capı́tulo 6: Dinámica
τ1
τ2
=
2.351 + 0.1676 cos(q2 ) 0.102 + 0.0838 cos(q2 )
0.102 + 0.0838 cos(q2 )
−0.1676 sen(q2 )q̇2
−0.0838 sen(q2 )q̇2
0.084 sen(q2 )q̇1
0.0
C(q ,q̇ )
M (q)
g
0.102
3.9211 sen(q1 ) + 0.1862 sen(q1 + q2 )
0.1862 sen(q1 + q2 )
g (q )
q̇1
q̈1
q̈2
+
q̇2
+
+
2.288q̇1 + 7.17 signo ( q̇1 ) + 8.8 [ 1 − | signo ( q˙2 ) | ]
0.175q̇2 + 1.734 signo ( q̇2 ) ) + 1.87 [ 1 − | signo ( q˙1 ) | ]
f f (q̇,f e )
La figura 6.10 muestra la respuesta
de las posiciones articulares (q1 , q2 )
del robot manipulador de 2 gdl en
configuración antropomórfica cuando se le aplica señales de prueba del
par aplicado (6.14). La sintonı́a de
las amplitudes de estas señales para
τ1 y τ2 se deben seleccionar de manera conveniente de tal forma que no
se sature los servoamplificadores, y
no producir velocidades articulares
a 720 grados/seg.
Figura 6.10 Respuesta del robot de 2 gdl.
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
6.7
313
Robot de 2 gdl
Código Fuente 6.11
robot2gdl.m
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 7 Identificación paramétrica.
robot2gdl.m
1
2
3
4
function xp =robot2gdl(t,x)
q1=x(1); q2=x(2); q = [q1; q2]; %vector de posición articular
qp1=x(3); qp2=x(4); qp = [qp1; qp2]; %vector de velocidad articular
m1=23.902; l1=0.45; lc1=0.091; I1=1.266;
5
b1=2.288; fc1=7.17; fe1=8.8;
m2=3.880; l2=0.45;
lc2=0.048; I2=0.093;
6
7
8
b2=0.175; fc2=1.734; fe2=1.87; g=9.81;
m11=m1*lc1*lc1+m2*l1*l1+m2*lc2*lc2+2*m2*l1*lc2*cos(q2)+I1+I2;
m12=m2*lc2*lc2+m2*l1*lc2*cos(q2)+I2;
m21=m12;
9
10
11
12
m22=m2*lc2*lc2+I2;
M=[m11, m12; m21, m22];
c11=-2*m2*l1*lc2*sin(q2)*qp2;
c12=-m2*l1*lc2*sin(q2)*qp2;
13
14
15
16
c21=m2*l1*lc2*sin(q2)*qp1;
c22=0;
C=[ c11, c12; c21, c22];
gq11=(lc1*m1++m2*l1)*sin(q1)+m2*lc2*sin(q1+q2);
17
18
19
20
gq21=m2*lc2*sin(q1+q2);
gq=g*[gq11; gq21];
fr=[b1*qp1+fc1*sign(qp1)+fe1*(1-abs(sign(qp1)));
21
22
23
26
b2*qp2+fc2*sign(qp2)+fe2*(1-abs(sign(qp2)))];
tau=[(1-exp(-0.8*t))*29.0+ 68*sin(16*t+0.1) + 9*sin(20*t+0.15);
(1-exp(-1.8*t))*1.2+ 8*sin(26*t+0.08)+2*sin(12*t+0.34) ];
qpp = M∧ (-1)*(tau-C*qp-gq-fr);
27
xp = [qp1; qp2; qpp(1); qpp(2)];
24
25
28
end
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
314
Capı́tulo 6: Dinámica
Las señales de prueba para el par aplicado se encuentran expresadas por:
τ1 = [ 1 − e−0.8t ] 29.0 + 68 sen(16t + 0.1) + 9 sen(20t + 0.15)
(6.14)
τ2 = [ 1 − e−1.8t ] 1.2 + 8 sen(26t + 0.08) + 2 sen(12t + 0.34)
(6.15)
Código Fuente 6.12
cap6 robot2gdlsimu.m
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 7 Identificación paramétrica.
cap6 robot2gdlsimu.m
1
2
clc; clear all; close all;
format short
3
ti=0; h=0.0025; tf = 10; ts=ti:h:tf; %vector tiempo
4
5
%configuración de la función ode45()
opciones=odeset(’RelTol’,1e-3,’InitialStep’,2.5e-3,’MaxStep’,2.5e-3);
6
%condiciones iniciales [ q(0), q̇(0) ] = [ 0, 0, 0, 0 ] . %integración numérica de la
7
dinámica del robot de 2 gdl
[t,x]=ode45(’robot2gdl’,ts,[0; 0; 0; 0],opciones);
8
%retorna x = [ q1 (t), q2 (t), q̇1 (t), q̇2 ] (t)
9
%vector de posición articular
T
10
% q = [q1, q2 ]T
11
12
q1=x(:,1);
q2=x(:,2);
13
%vector de velocidad articular
14
% q̇ = [q̇1, q̇2 ]T
15
16
qp1=x(:,3);
qp2=x(:,4);
17
%grafica posiciones articulares en función del tiempo
18
%conversión de radianes a grados
plot(t,180*q1/pi,t,180*q2/pi)
19
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
6.8
315
Robot de 3 gdl
6.8 Robot de 3 gdl
E
l modelo dinámico de un robot de 3 gdl en configuración antropomórfica es
una ecuación diferencial muy complicada, el proceso de simulación a través de
la función ode45(· · ·) puede consumir tiempo de cómputo de manera significativa
si se introducen funciones discontinuas como el fenómeno de fricción de Coulomb.
En este sentido, se recomienda al lector eliminar la fricción de Coulomb y la fricción
estática, conservando la fricción viscosa para una rápida simulación. Evidentemente,
la simulación estarı́a incompleta por falta de estos fenómenos. Sin embargo, en un
robot de transmisión directa las magnitudes de fricción son bajas.
La figura 6.11 muestra un robot industrial de la compañı́a FANUC en configuración
antropomórfica; los 3 gdl se refieren a las articulaciones de la base, hombro y codo,
sin tomar en cuenta la orientación del robot.
Figura 6.11 Robot manipulador FANUC.
La gran mayorı́a de los
tienen la configuración
sus movimientos y por
tipo de robots no sólo
robots que se encuentran operativos en el sector industrial
antropomórfica por ser la que mejor destreza presenta en
lo tanto la que mayor número de aplicaciones tiene. Este
se emplea en la industria, sino también es importante en
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
316
Capı́tulo 6: Dinámica
la automatización de quirófanos robotizados, asistencia personalizada a un sector
vulnerable de la población con capacidades diferenciadas, etcétera.
La utilidad del modelo dinámico resulta importante cuando se dispone de los valores
numéricos de todos los parámetros del robot; bajo este enfoque la tabla 6.5 muestra
la terminologı́a, significado y valores de un robot prototipo de 3 gdl con motores de
transmisión directa.
Tabla 6.5 Parámetros del robot antropomórfico de 3 gdl
Eslabón
Base
Hombro
Codo
Significado
Notación
Valor
Masa del eslabón 1
m1
26.9 kg
Longitud del eslabón 1
Inercia del eslabón 1
Inercia del eslabón 1
l1
Iz1
Iy1
0.45 m
1.266 Nm-seg2 /rad
0.089 Nm-seg2 /rad
Inercia del eslabón 1
Centro de masa del eslabón 1
Coeficiente de fricción viscosa
Ix1
lc1
b1
0.03 Nm-seg2 /rad
0.091 m
2.288 Nm-seg/rad
Coeficiente de fricción de Coulomb
Coeficiente de fricción estática
fc1
fe1
7.17 Nm
8.8 Nm
Masa del eslabón 2
Longitud del eslabón 2
Inercia del eslabón 1
m2
l2
Iz2
30 kg
0.45 m
0.084 Nm-seg2 /rad
Inercia del eslabón 1
Inercia del eslabón 1
Centro de masa del eslabón 2
Iy2
Ix2
lc2
0.003 Nm-seg2 /rad
0.05 Nm-seg2 /rad
0.038 m
Coeficiente de fricción viscosa
Coeficiente de fricción de Coulomb
Coeficiente de fricción estática
b2
fc2
fe2
0.2 Nm-seg/rad
1.9 Nm
2.1 Nm
Masa del eslabón 3
Longitud del eslabón 2
m3
l3
3.88
0.45 m
Inercia del eslabón 1
Inercia del eslabón 1
Inercia del eslabón 1
Iz3
Iy3
Ix3
0.056 Nm-seg2 /rad
0.0012 Nm-seg2 /rad
0.009 Nm-seg2 /rad
Centro de masa del eslabón 3
Coeficiente de fricción viscosa
Coeficiente de fricción de Coulomb
Coeficiente de fricción estática
lc3
b3
fc3
fe3
0.048
0.175 Nm-seg/rad
1.734
1.87
g
9.81 m/seg2
Aceleración debida a la gravedad
El modelo dinámico del robot manipulador en configuración antropomórfica se
detalla en la ecuación (6.16); de acuerdo con los datos de la tabla 6.5 se obtiene
un modelo dinámico para propósitos de simulación.
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
1
τ3
⎢ ⎥
⎢ ⎥
⎢ ⎥
⎢ ⎥
⎢ ⎥
⎢τ ⎥
⎢ 2 ⎥=
⎢ ⎥
⎢ ⎥
⎢ ⎥
⎢ ⎥
⎣ ⎦
⎡τ ⎤
|
c31
m32
c12
{z
C(q ,q̇ )
c32
l1 m3 lc2 sen(q3 )q̇2
|
{z
}
c22
−l1 m3 lc2 sen(q3 )q̇3
|
{z
}
}
3
c33
q̈3
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
5
3
3
(6.16)
}
7
7
[ Iy3 − Iz3 ] cos (q2 + q3 ) sen(q2 + q3 )q̇1 7
7
72
−m3 lc2 sen(q2 + q3 )lc1 cos(q2 )q̇1
7 q̇1
7
2
76
−m3 lc2 sen(q2 + q3 ) cos (q2 + q3 )q̇1
6
|
{z
}7
76
76
c13
76
76
76
76
76
76
76
76
7 6 q̇2
76
76
76
76
−l1 m3 lc2 sen(q3 )(q̇2 + q̇3 )
76
76
|
{z
}
76
76
c23
76
76
76
74
7
7
7 q̇3
7
7
0
7
|{z}
5
m33
0
|{z}
2 q̈
1
6
76
76
76
m13
76
76
76
76
76
76
76
76
2
6
Ix3 + m3 lc3 + m3 l2 lc3 cos(q3 ) 7
7 6 q̈2
6
{z
}7
|
76
76
m23
76
76
76
76
76
76
2
Ix3 + m3 lc3
56
4
|
{z
}
[ Iy2 − Iz2 − m2 l2
c2 ] cos(q2 ) sen(q2 )q̇1 +
[ Iy3 − Iz3 ] cos (q2 + q3 ) sen(q2 + q3 )q̇1 −
m3 [ l2 cos(q2 ) + lc3 cos (q2 + q3 ) ] l2 sen(q2 )q̇1 −
m3 [ l2 cos(q2 ) + lc3 cos (q2 + q3 ) ] lc2 sen(q2 + q3 )q̇1
|
{z
}
{z
M (q )
2
Ix3 + m3 l2
c3 + m3 l2 lc3 cos(q3 )
{z
}
|
m22
2
2
Ix2 + Ix3 + m3 l2
2 + m2 lc3 + m3 lc3
+2m3 l2 lc3 cos(q3 )
|
{z
}
m12
0
|{z}
2
3 2
3
0
b1 q̇1 + fc1 signo(q̇1 ) + fe1 [1 − |signo(q̇1 )|]
+g 4 lc2 m2 sen(q2 ) + m3 l2 sen(q2 ) + m3 lc3 sen(q2 + q3 ) 5 + 4 b2 q̇2 + fc2 signo(q̇2 ) + fe2 [1 − |signo(q̇2 )|] 5
m3 lc3 sen(q2 + q3 )
b3 q̇3 + fc3 signo(q̇3 ) + fe3 [1 − |signo(q̇3 )|]
{z
} |
{z
}
|
g (q )
f f (q̇ ,f e )
|
[ Iy2 − Iz2 − m2 l2
c2 ] cos(q2 ) sen(q2 )q̇2
+ [ Iy3 − Iz3 ] cos (q2 + q3 ) sen(q2 + q3 )q̇2 −
6
6
6
m3 [ l2 cos(q2 ) + lc3 cos (q2 + q3 ) ] l2 sen(q2 )q̇2 −
6
6 m [ l cos(q ) + l cos (q + q ) ] l
3
2
2
c3
2
3
c3 sen(q2 + q3 )q̇2 +
6
6
6
[ Iy3 − Iz3 ] cos (q2 + q3 ) sen(q2 + q3 )q̇3 −
6
6
m3 lc3 sen(q2 + q3 )l2 sen(q2 )q̇3 −
6
6
m3 lc3 sen(q2 + q3 )lc3 sen(q2 + q3 )q̇3
6
6|
{z
}
6
6
c11
6
6
6
+6
6
[ Iz2 − Iy2 + m2 l2
c1 ] cos(q2 ) sen(q2 )q̇1
6
6
+ [ Iz3 − Iy3 ] cos (q2 + q3 ) sen(q2 + q3 )q̇1
6
6
+m3 [ l1 cos(q2 ) + lc2 cos (q2 + q3 ) ] l1 sen(q2 )q̇1
6
6
6 +m3 [ l1 cos(q2 ) + lc2 cos (q2 + q3 ) ] lc2 sen(q2 + q3 )q̇1
6
{z
}
6|
6
c21
6
6
6
6
6
[ Iz3 − Iy3 ] cos (q2 + q3 ) sen(q2 + q3 )q̇1
6
6
4 +m3 lc2 sen(q2 + q3 ) [ l1 cos(q2 ) + lc2 cos (q2 + q3 ) ] q̇1
|
{z
}
2
m31
Iy2 sen2 (q2 ) + Iy3 sen2 (q2 + q3 )
2
2
2
2
6 +Iz1 + Iz2 cos (q2 ) + Iz3 cos (q2 + q3 ) + m2 lc2 cos (q2 )
6
6
+m3 [ l2 cos(q2 ) + lc3 cos(q2 + q3 ) ]2
6|
{z
}
6
6
m11
6
6
6
6
6
6
0
6
|{z}
6
6
m21
6
6
6
6
6
6
6
0
4
|{z}
2
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
5
3
6.8
Robot de 3 gdl
317
Alfaomega
318
Capı́tulo 6: Dinámica
♣♣♣ Ejemplo 6.8
Emplear el modelo dinámico de un robot antropomórfico (6.16) de 3 gdl y los
valores numéricos de los parámetros contenidos en la tabla 6.5 para realizar un
programa en MATLAB que controle las articulaciones de la base, hombro y codo
en las posiciones deseadas [ qd1 , qd2 , qd3 ]T = [ 45, 45, 90 ]T grados, respectivamente.
Presentar la evolución en el tiempo de las posiciones articulares.
Solución
Una de las aplicaciones que tiene el modelo dinámico de robots manipuladores
es en control de posición; a través de la simulación es posible estudiar en detalle
la respuesta que presenta el robot con una determinada estructura de control,
sintonizar de manera adecuada las ganancias proporcional y derivativa, para obtener
un transitorio pequeño sin sobre tiro y oscilaciones y reducir significativamente el
error de posición en régimen estacionario.
El modelo dinámico del robot antropomórfico de 3 gdl junto con el algoritmo
de control proporcional derivativo (PD) está implementado con la estructura de
una ecuación diferencial ordinaria de primer orden ẋ = f (x) en la función
robot3gdl(t,x) descrita en el cuadro 6.13; el programa principal para realizar la
simulación se presenta en el cuadro 6.14.
La sintaxis de la función del modelo dinámico del robot antropomórfico de 3 gdl
está dada como:
ẋ=robot3gdl(t, x)
donde t es la evolución del tiempo, x es la variable de estado, contiene las posiciones
y velocidades articulares, ẋ es la derivada temporal de la variable de estado x,
contiene las velocidades y aceleraciones articulares del robot, es decir:
Alfaomega
x = [ q1 (t) q2 (t) q3 (t) q̇1 (t) q̇2 (t)
q̇3 (t) ]T
ẋ = [ q̇1 (t) q̇2 (t) q̇3 (t) q̈1 (t) q̈2 (t)
q̈3 (t) ]T
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
6.8
319
Robot de 3 gdl
Código Fuente 6.13
robot3gdl
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés
%Capı́tulo 4 Dinámica. Función robot3gdl.m (robot3gdl simu.m)
robot3gdl
1 function xp = robot3gdl(t,x)
2
q1=x(1); q2=x(2); q3=x(3); q = [q1; q2; q3]; %vector de posición articular
3
qp1=x(4); qp2=x(5); qp3=x(6); qp = [qp1; qp2; qp3]; %vector de velocidad articular
4
Iz1=1.26; Iz2=0.084; Iz3=0.056; Iy1=0.089; Iy2=0.003; Iy3=0.0012; Ix1=0.03; Ix2=0.05;
Ix3=0.009; m1=26.902; l1=0.45; b1=2.288; fc1=7.17; fe1=8.8; m2=30; l2=0.45; lc2=0.038;
b2=0.2; fc2=1.9; fe2=2.1; m3=3.880; l3=0.45; lc3=0.048; b3=0.175; fc3=1.734; fe3=1.87;
5
m11=Iy2*sin(q2)*sin(q2)+Iy3*sin(q2+q3)*sin(q2+q3)+Iz1+Iz2*cos(q2)*cos(q2)
+Iz3*cos(q2+q3)*cos(q2+q3)+m2*lc2*lc2*cos(q2)*cos(q2)+
m3*(l2*cos(q2)+lc3*cos(q2+q3))*(l2*cos(q2)+lc3*cos(q2+q3));
6
m12=0; m13=0; m21=0; m22=Ix2+Ix3+m3*l2*l2+m2*lc2*lc2+m3*lc3*lc3+
2*m3*l2*lc3*cos(q3); m23=Ix3+m3*lc3*lc3+m3*l2*lc3*cos(q3);
m31=0; m32=Ix3+m3*lc3*lc3+m3*l2*lc3*cos(q3); m33=Ix3+m3*lc3*lc3;
M=[m11, m12, m13; m21, m22, m23; m31, m32, m33];
7
8
9
10
11
12
gamma112=(Iy2-Ix2-m2*lc2*lc2)*cos(q2)*sin(q2)+(Iy3-Iz3)*cos(q2+q3)*sin(q2+q3)
-m3*(l2*cos(q2)+lc3*cos(q2+q3))*(l2*sin(q2)+lc3*sin(q2+q3));
gamma113=(Iy3-Iz3)*cos(q2+q3)*sin(q2+q3)-m3*lc3*sin(q2+q3)*
(l2*cos(q2)+lc3*cos(q2+q3));
gamma121=(Iy2-Iz2-m2*lc2*lc2)*cos(q2)*sin(q2)+(Iy3-Iz3)*cos(q2+q3)*sin(q2+q3)m3*(l2*cos(q2)+lc3*cos(q2+q3))*(l2*sin(q2)+lc3*sin(q2+q3));
gamma131=(Iy3-Iz3)*cos(q2+q3)*sin(q2+q3)-m3*lc3*sin(q2+q3)*
(l2*cos(q2)+lc3*cos(q2+q3)); gamma211=(Ix2-Iy2+m2*lc2*lc2)*cos(q2)*sin(q2)+
(Iz3-Iy3)*cos(q2+q3)*sin(q2+q3)+m3*(l2*cos(q2)+lc3*cos(q2+q3))*
(l2*sin(q2)+lc3*sin(q2+q3)); gamma223=-l2*m3*lc3*sin(q3);
13
14
gamma232=-l2*m3*lc3*sin(q3); gamma233=-l2*m3*lc3*sin(q3) ;
gamma311=(Iz3-Iy3)*cos(q2+q3)*sin(q2+q3)+m3*lc3*sin(q2+q3)*(l2*cos(q2)+
lc3*cos(q2+q3)); gamma322=l2*m3*lc3*sin(q3);
15
16
17
18
c11=gamma112*qp2+gamma113*qp3; c12=gamma121*qp1; c13=gamma131*qp1;
c21=gamma211*qp1; c22=gamma223*qp3; c23=gamma232*qp2+gamma233*qp3;
c31=gamma311*qp1; c32=gamma322*qp2; c33=0; C=[c11, c12, c13; c21, c22, c23;
c31, c32, c33]; gq11=0; gq21=(lc2*m1++m2*l2)*sin(q1)+m2*lc3*sin(q1+q2);
19
20
21
gq31=m2*lc3*sin(q1+q2);g=9.81; gq=g*[gq11; gq21; gq31]; fr=[b1*qp1; b2*qp2; b3*qp3];
qd=[45*pi/180; 45*pi/180; 90*pi/180]; qt=[qd(1)-q1;qd(2)-q2; qd(3)-q3];
Kp=[5, 0,0;0, 5,0; 0,0,5]; Kv=[3, 0,0;0, 3,0;0,0,3]; tau=Kp*qt-Kv*qp+gq;
22
23
qpp = inv(M)*(tau-C*qp-gq-fr);%aceleración articular
xp = [qp1; qp2; qp3; qpp(1); qpp(2); qpp(3)];%vector de salida
24 end
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
320
Capı́tulo 6: Dinámica
Código Fuente 6.14
robot3gdl simu
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés
%Capı́tulo 4 Dinámica. Programa robot3gdl simu.m
%requiere la función robot3gdl.m
robot3gdl simu
1
clear; close all; clc;
2
format short g
3
5
%parámetros de simulación:
ti=0; h=0.0025; tf = 10; %tiempo de simulación 0 a 10 segundos
ts=ti:h:tf; %vector tiempo
6
%configuración de la función ode45
7
opciones=odeset(’RelTol’,1e-3,’InitialStep’, 2.5e-3, ’MaxStep’, 2.5e-3);
8
%solución numérica del sistema, con condición iniciales cero
[t,x]=ode45(’robot3gdl’,ts,[0; 0; 0; 0; 0; 0], opciones);
4
9
10
%gráficas de las 3 articulaciones; q1 =x(:,1), q2 =x(:,2), q3 =x(:,3)
11
plot(t,180*x(:,1)/pi,t,180*x(:,2)/pi, t,180*x(:,3)/pi)
La figura 6.12 presenta la respuesta del robot a una entrada τ de la
ley de control PD. Las posiciones de
las articulaciones de la base, hombro
y codo convergen a [ 45, 45, 90] grados, respectivamente. La sintonı́a de
las ganancias se hizo para que el
transitorio no tuviera sobre impulsos y error en estado estacionario
menor a 0.001 grados en las 3 articulaciones.
Figura 6.12 Respuesta del robot 3 gdl.
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
6.9
321
Robot cartesiano
6.9 Robot cartesiano
E
l robot cartesiano de 3 gdl tiene los siguientes fenómenos: inercial, fricción
(viscosa, Coulomb y estática) y par gravitacional, debido a que la articulación
d1 tiene desplazamiento en el eje z0 , su energı́a potencial es variable. Para las
variables d2 y d3 tienen movimiento en el plano x0 −y0 , entonces la energı́a potencial
de esas variables es constante. Por lo tanto el par gravitacional para aquellas
componentes es cero. Por otro lado, el robot cartesiano no tiene variables rotacionales
qi , mientras que las fuerzas centrı́petas y de Coriolis son cero.
Figura 6.13 Robot cartesiano.
El modelo dinámico de un robot cartesiano de 3 gdl está dado por:
¨ τ
=
m1 + m2 + m3
0
0
m1 + m2
0
0
+
fc1
0
0
0
fc2
0
0
0
fc3
0
0
d1
d¨2
+g
m1 + m2 + m3
0
d¨3
m1
0
˙
signo ( d1 )
fe1 [ 1 − | signo ( d˙1 ) | ]
signo ( d˙2 ) + fe2 [ 1 − | signo ( d˙2 ) | ]
signo ( d˙3 )
fe3 [ 1 − | signo ( d˙3 ) | ]
+
b1
0
0
b2
0
0
0
0
b3
d˙1
d˙2
d˙3
(6.16)
donde m1 , m2 , m3 son las masas de los servomotores e incluyen las barras y piezas
metálicas de acoplamiento mecánico, g es la constante debido a la aceleración
gravitacional (g=9.81 m/seg2 ), b1 , b2, b2 , fc1 , fc2 , fc3 y fe1 , fe2, fe3 representan los
coeficientes de fricción viscosa, Coulomb y estática, respectivamente.
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
322
Capı́tulo 6: Dinámica
Los fenómenos de fricción pueden ser de magnitud considerable si los servomotores
emplean reductores de velocidad por medio de sistemas de engranes, los cuales
introducen juego mecánico que repercute directamente en errores de posición.
Además, debido al efecto disipativo de la fricción (conversión de energı́a mecánica
en energı́a térmica), se degradan las piezas o componentes del robot, lo que produce
un pobre desempeño. Cuando la tecnologı́a de los servomotores del robot son
de transmisión directa (direct drive), entonces el fenómeno de fricción se reduce
considerablemente, de ahı́ que la mayorı́a de la bibliografı́a especializada de robótica
presenta el modelo dinámico del robot cartesiano sin el fenómeno de fricción; debido
a este hecho el modelo dinámico resultante del robot cartesiano es lineal con respecto
a las variables de estado d1 , d2 , d3. También debe recordarse, como se demostró en la
sección 5.6 que el modelo cinemático cartesiano es lineal. Por ambas caracterı́stica,
al robot cartesiano también se le denomina robot lineal.
A continuación se presenta el modelo dinámico en forma numérica de un prototipo
robot cartesiano; los valores numéricos de los parámetros se encuentran descritos en
la tabla 6.6.
Tabla 6.6 Parámetros del robot cartesiano
Articulación
τ1max
Masa
Fricción viscosa
Fricción de Coulomb
Fricción estática
=50 N, d1
m1 =0.7 kg
b1 =0.02 kg/seg
fc1 =0.01 N
fe1 =0.015 N
τ2max =4 N, d2
m2 =0.28 kg
b2 =0.08 kg/seg
fc2 =0.07 N
fe2 =0.076 N
τ3max
m3 =0.28 kg
b3 =0.02 kg/seg
fc3 =0.02 N
fe3 =0.022 N
=4 N, d3
donde τimax (con i = 1, 2, 3) representa las capacidades máximas de los servomotores
de las articulaciones (d1 , d2, d3 ), respectivamente.
La mesa del robot cartesiano tiene una longitud de 1m por cada lado. La posición
de casa [ d1 , d2 , d3 ]T = [ 0, 0, 0 ]T está ubicada de manera conveniente sobre el piso,
en el centro geométrico de la mesa. De esta forma, la variable articular d1 que
se desplaza sobre el eje z0 tiene coordenadas positivas y acotada en el intervalo
0 ≤ d1 ≤ 1, para las variables d2 , d3 que se mueven en el plano x0 − y0 pueden
tener coordenadas positivas y negativas; ambas variables se encuentran restringidas
al intervalo: −0.5 ≤ di ≤ 0.5, para i = 2, 3. También es posible ubicar otro lugar
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
6.9
323
Robot cartesiano
para la posición de casa, lo cual depende finalmente de la facilidad que permita
programar la aplicación que va a realizar el robot.
⎡
τ
1.26
⎢
= ⎣ 0
0
⎡
0.01
⎢
+⎣ 0
0
⎤⎡
⎤ ⎡
⎤⎡ ⎤ ⎡
⎤
d¨1
0.02
0
0
12.36
d˙1
⎥⎢ ⎥ ⎢
⎥⎢ ⎥ ⎢
⎥
0.98 0 ⎦ ⎣ d¨2 ⎦ + ⎣ 0
0.08
0 ⎦ ⎣ d˙2 ⎦ + ⎣ 0 ⎦
0
0.7
0
0
0.02
0
d¨3
d˙3
0
0
⎤⎡
⎤ ⎡
⎤
signo ( d˙1 )
0.015 [ 1 − | signo ( d˙1 ) | ]
⎥⎢
⎥ ⎢
⎥
0.07
0 ⎦ ⎣ signo ( d˙2 ) ⎦ + ⎣ 0.076 [ 1 − | signo ( d˙2 ) | ] ⎦
0
0.02
signo ( d˙3 )
0.022 [ 1 − | signo ( d˙3 ) | ]
(6.17)
0
0
En el caso del robot cartesiano debe notarse que en el modelo dinámico (6.17) la
entrada τ representa una fuerza cartesiana y no un par aplicado o torque.
Cuando recién se construye o se adquiere un prototipo nuevo es recomendable
aplicarle una señal de energı́a apropiada para moverlo en su espacio de trabajo por
un periodo de una semana (dı́a y noche). Lo anterior tiene la finalidad de “asentar”
los servomotores y bajar los niveles de fricción a sus estados normales.
Es conveniente aplicar una señal de entrada al robot como la que se presenta a
continuación:
⎡ ⎤
⎡
⎤
τ1
12.36 + 0.05 sen(t)
⎢ ⎥
⎢
⎥
(6.18)
⎣ τ2 ⎦ = ⎣ 0.13 [ 1 − e−0.5t ] sen(t) ⎦
τ3
0.06 sen(t)
donde t es la variable del tiempo; las amplitudes y parámetros son seleccionados de
manera conveniente para no saturar a los servoamplificadores del robot.
El modelo dinámico (6.17) es una ecuación diferencial de segundo orden; para poder
simular dicho modelo es necesario convertirlo al formato ẋ = f (x), es decir a una
ecuación diferencial ordinaria de primer orden y de esta forma estar en condiciones
de utilizar la función de MATLAB ode45(· · ·). Con esta finalidad, observe que
x = [ d1 , d2 , d3, ḋ1 , ḋ2, d˙3 ], entonces la ecuación (6.17) adquiere la forma (6.19):
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
324
Capı́tulo 6: Dinámica
d
dt
|
Una vez que el modelo dinámico ya se encuentra correc-
2d 3
1
6 7
6 d2 7
6 7
6 d3 7
6 7
6 d˙ 7 =
6 17
4 d˙2 5
d˙3
{z }
ẋ
tamente expresado como una ecuación diferencial ordinaria de primer orden, puede ser implementado en lenguaje
2
0
0.98
0
En el cuadro 6.15 se encuentra el código fuente del mo-
6
6
6
6
6
6
62
6 1.26
6
44 0
0
|
MATLAB a través de la función ode45(· · ·).
delo dinámico del robot cartesiano. Se ha tomado la en-
3
22
3
2
0 −1
τ1
0.02
4 4 τ2 5 − 4 0
0 5
0.7
0
τ3
trada de prueba (6.18) para estudiar su comportamiento
dinámico. El cuadro 6.16 contiene el programa principal
para realizar la simulación con el robot cartesiano. La figura 6.14 muestra el desplazamiento articular de las tres
variables d1 , d2, d3 . La oscilación que presenta cada arti-
0
0.08
0
culación es intencional con la finalidad de mover a cada
mayor parte de su espacio de trabajo. Es recomendable
este procedimiento antes de utilizar el robot, al menos
durante una semana dejarlo con la trayectoria de prueba
se puede realizar identificación paramétrica (ver sección
7.7) o realizar experimentos con el robot.
ḋ1
ḋ2
ḋ3
f (x)
ayuda a normalizar los niveles de fricción. Posteriormente
32 ˙ 3
3
2
2
d1
0.01
0
12.36
0 5 4 d˙2 5 − 4 0 5 − 4 0
0
0.02
0
d˙3
{z
servo en ambas direcciones y al mismo tiempo recorran la
0
0.07
0
3
7
7
7
7
7
7
32
3
2
337
signo ( ḋ1 )
0
0.015 [ 1 − | signo ( d˙1 ) | ]
7
7
0 5 4 signo ( ḋ2 ) 5 − 4 0.076 [ 1 − | signo ( d˙2 ) | ] 5 5 5
0.02
signo ( ḋ3 )
0.022 [ 1 − | signo ( d˙3 ) | ]
}
Alfaomega
(6.19)
Figura 6.14 Robot cartesiano.
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
6.9
325
Robot cartesiano
Código Fuente 6.15
robot cartesiano3gdl
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés
%Capı́tulo 4 Dinámica. Programa robot cartesiano3gdl.m
robot cartesiano3gdl
1
function xp = robot cartesiano3gdl(t,x)
2
d = [x(1); x(2); x(3)]; % Vector de posición articular
dp = [x(4); x(5); x(6)]; % vector de velocidad articular
3
4
%parámetros del robot cartesiano
m1=0.7; m2=0.28; m3=0.28; % masas de los servomotores y partes mecánicas
5
6
b1=0.02; b2=0.08; b3=.02; % coeficientes de fricción viscosa
fc1=0.01; fc2=0.07; fc3=.02; % coeficientes de la fricción de Coulomb
fe1=0.015; fe2=0.076; fe3=0.022; % coeficientes de la fricción estática
g=9.81; % constante de aceleración gravitacional
7
8
9
10
B=[b1, 0, 0;
0, b2,0;
0, 0, b3];
Fc=[fc1, 0, 0;
11
12
13
14
0, fc2,0;
0, 0, fc3];
15
16
% matriz de masas
M = [m1+m2+m3, 0, 0;
17
18
20
0 m1+m2, 0;
0 0 m3];
par grav = g*[m1+m2+m3; 0;0]; %vector de par de gravitacional
21
%fricción estática
22
fe=[ fe1*(1-abs(sign(dp(1)))); fe2*(1-abs(sign(dp(2))));
fe3*(1-abs(sign(dp(3))))];
fr= B*dp+Fc*sign(dp)+fe;
19
23
24
tau=[ g*(m1+m2+m3)+ 0.05*sin(t); (1-exp(-0.5*t))*0.13*sin(t);0.06*sin(t)];
dpp = inv(M)*(tau- par grav-fr);
xp = [dp(1); dp(2); dp(3); dpp(1); dpp(2); dpp(3)];
25
26
27
end
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
326
Capı́tulo 6: Dinámica
Código Fuente 6.16
simu robotcartesiano3gdl
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés
%Capı́tulo 4 Dinámica. Programa simu robotcartesiano3gdl.m
% este programa requiere del archivo: robot cartesiano3gdl.m
simu robotcartesiano3gdl
1
3
clc;
clear all;
close all;
4
format short
5
%parámetros de simulación:
ti=0; %tiempo inicial
h=0.0025; %incremento del tiempo
2
6
7
8
9
tf = 600; % tiempo de simulación (segundos)
ts=ti:h:tf; %vector de tiempo de simulación
11
%para un correcto desempeño de integración de la función ode45(· · ·) se requieren:
opciones=odeset(’RelTol’,1e-3,’InitialStep’,2.5e-3,’MaxStep’,2.5e-3);
12
% solución numérica del sistema ẋ = f (x):
13
14
%las condiciones iniciales del robot cartesiano han sido puestas en cero
T
T
%[ d1 (0), d2 (0), d3 (0), d˙1 (0), d˙2 (0),
d˙3 (0) ] = [ 0, 0, 0, 0, 0, 0]
15
%advertencia: debido al uso de funciones discontinuas en el modelo dinámico del
16
%robot cartesiano como la función signo(· · ·), el proceso de simulación puede
17
tardar
[t,x]=ode45(’robot cartesiano3gdl’,ts,[0; 0; 0; 0; 0; 0],opciones);
18
% t representa el tiempo, x representa la solución de la ecuación (6.19)
19
% Las componentes del vector x representan
20
% componentes de posiciones
21
%x(:, 1) = d1
22
% componentes de velocidades
%x(:, 4) = d˙1 x(:, 5) = d˙2 x(:, 6) = d˙3
10
23
24
25
x(:, 2) = d2
x(:, 3) = d3
%presenta gráfica de posiciones en función del tiempo
plot(t,x(:,1),t,x(:,2),t,x(:,3))
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
6.10
327
Resumen
6.10 Resumen
D
inámica es la parte de la fı́sica que explica el movimiento de un sistema
mecánico tomando en cuenta las fuerzas que lo producen. El modelo dinámico
está formado por ecuaciones diferenciales no lineales que reproducen ı́ntegramente
todos los fenómenos fı́sicos del robot.
En este capı́tulo se ha presentado la técnica para simular diversos sistemas dinámicos,
lo que facilita el estudio, análisis y aplicaciones del sistema o prototipo. Para un
adecuado entendimiento de la dinámica del sistema la mejor herramienta es la
simulación. El paso previo a una etapa experimental es la simulación, ya que conocer
a detalle todos los aspectos dinámicos del robot o del sistema mecatrónico facilita
su control, ası́ como llevar a cabo en forma práctica aplicaciones potenciales del
prototipo. El método de integración numérica es un factor fundamental para una
adecuada solución numérica del modelo dinámico, por lo tanto es recomendable
utilizar el método de Runge Kutta 4/5, el cual lo tiene implementado MATLAB a
través de la función ode45(· · ·).
Se han presentado diversos ejemplos para ilustrar la técnica de simulación
de sistemas dinámicos: sistemas lineales y no lineales simples, sistema masa
resorte amortiguador, centrı́fuga y péndulo, hasta sistemas dinámicos de robots
manipuladores más complejos como el robot antropomórfico de 2 y 3 gdl, y robot
cartesiano de 3 gdl. Dichos modelos se encuentran estructurados de acuerdo con
la forma: function ẋ=modelo dinamico(t, x) para fácil implementación y uso en
diversas aplicaciones de robótica y mecatrónica.
Es importante resaltar que la estructura matemática del modelo dinámico debe ser
de la forma tradicional de variables fase como una ecuación diferencial ordinaria de
primer orden:
ẋ = f (x)
si el modelo original del sistema es una ecuación diferencial de orden superior,
entonces mediante un adecuado cambio de variables de estados siempre es posible
transformarlo a la estructura requerida.
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
328
Capı́tulo 6: Dinámica
A continuación se resume la sintaxis de los modelos dinámicos desarrollados para
MATLAB:
xp = centrifuga( t,x)
xp =pendulo(t,x)
xp =mra(t,x)
xp =robot2gdl(t,x)
xp=robot3gdl(t, x)
xp = robot cartesiano3gdl(t,x)
Para un adecuado funcionamiento de las librerı́as es recomendable utilizar en todos
los casos un código parecido al siguiente:
ti=0; h=0.0025; tf =5; ts=ti:h:tf;
opciones=odeset(’RelTol’,1e-3,’InitialStep’,1e-3,’MaxStep’,1e-3);
Condiciones iniciales (dependiendo del robot):
ci=[0;0] (centrı́fuga); ci=[0;0] (péndulo); ci=[0;0;0;0] robot de 2 gdl;
ci=[0;0;0;0;0;0] robot de 3 gdl (antropomórfico y cartesiano).
[t,x]=ode45(’nombre funcion’,ts,ci,opciones);
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
Capı́tulo
7
Identificación
paramétrica
θ̂(k)
=
P (k) =
θ̂(k − 1) +
P (k − 1) Ψ(k) [ y(k) − Ψ(k)T θ̂(k − 1) ]
1 + Ψ(k − 1)T P (k − 1)Ψ(k)
P (k − 1) −
P (k−1)Ψ(k)Ψ(k)T P (k−1)
1+Ψ(k−1)T P (k−1)Ψ(k)
7.1 Introducción
7.2 Método de mı́nimos cuadrados
7.3 Librerı́a de mı́nimos cuadrados
7.4 Ejemplos
7.5 Modelos de regresión del péndulo
7.6 Modelos de regresión del robot de 2 gdl
7.7 Robot cartesiano de 3 gdl
7.8 Resumen
Objetivos
Presentar la técnica de estimación de mı́nimos cuadrados y su
aplicación en identificación paramétrica de sistemas mecatrónicos y robots
manipuladores.
Objetivos particulares:
Esquemas de regresión escalar y multivariable.
Regresión lineal del modelo dinámico.
Regresión del modelo de energı́a.
Esquema de regresión del modelo de potencia.
7.1
Introducción
331
7.1 Introducción
E
l modelo dinámico de sistemas mecatrónicos y robots manipuladores contiene
en su estructura matemática parámetros tales como centros de gravedad,
masas, momentos de inercia y coeficientes de fricción. Estos parámetros generalmente
son desconocidos; este es el caso de la mayorı́a de los robots comerciales donde el
fabricante no proporciona sus valores nominales. Si bien existen herramientas de la
teorı́a de control como esquemas adaptables y controladores robustos que permiten
tolerar errores en los parámetros dinámicos, el conocimiento de éstos es crucial para
la mayorı́a de los esquemas basados en el modelo dinámico del robot manipulador.
Identificación paramétrica es una herramienta atractiva para determinar los
parámetros dinámicos de robots manipuladores, sobre todo cuando existe dificultad
para medirlos directamente. Sin embargo, la naturaleza no lineal del modelo
dinámico de robots manipuladores hace que la tarea de identificación paramétrica
no sea trivial.
El modelo dinámico de robots manipuladores posee propiedades que permiten
generar varios esquemas de regresión lineal como los siguientes: modelo dinámico
se le denomina ası́ debido a que emplea la estructura del modelo dinámico para
expresarlo como un regresor lineal vectorial. Sin embargo, este modelo requiere medir
la aceleración articular lo que representa una desventaja práctica. La estimación de
la velocidad puede realizarse mediante técnicas de filtrado, lo que da origen al modelo
de regresión dinámico filtrado. Por otro lado, existen esquemas de regresión escalares
como los modelos de energı́a, potencia y potencia filtrada; estos modelos permiten
identificar los mismos parámetros de los esquemas vectoriales dinámico y dinámico
filtrado.
Para poder utilizar el método de mı́nimos cuadrados se requiere que el modelo
matemático del sistema a identificar pueda ser expresado como un regresor lineal en
los parámetros desconocidos. La técnica de mı́nimos cuadrados recursivo permite
obtener el valor numérico de los parámetros del sistema sin la necesidad de
desarmar al sistema mecatrónico o robot manipulador. Se requieren de contar con
las mediciones u observaciones de las posiciones y velocidades articulares, ası́ como
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
332
Capı́tulo 7: Identificación paramétrica
la señal del par aplicado para que el sistema pueda moverse en su espacio de trabajo.
7.2 Método de mı́nimos cuadrados
D
e particular relevancia para los esquemas de identificación paramétrica y
de control adaptable es expresar el modelo dinámico no lineal del robot
manipulador como el producto de una matriz de regresión compuesta de funciones no
lineales (dependientes de la posición, velocidad y aceleración articular) y un vector
de parámetros constantes dependiente de masas, momentos de inercias, distancias a
centros de masa y coeficientes de fricción.
Los esquemas de identificación que se describirán en esta sección son sistemas de
identificación que pertenecen a la filosofı́a de identificación hı́brida, es decir el modelo
de regresión es formulado en tiempo continuo mientras que la identificación se realiza
a través de un estimador recursivo llamado mı́nimos cuadrados, el cual se utiliza
ampliamente en la literatura debido a su sencillez y a su propiedad de recursividad,
atributo que lo hace atractivo para su implementación.
Esta técnica es particularmente simple si el modelo tiene la propiedad de linealidad
en los parámetros del modelo. El método de mı́nimos cuadrados es un esquema
estándar que aproxima la solución de sistemas sobre determinados, por ejemplo
cuando hay más variables incógnitas que ecuaciones. Mı́nimos cuadrados minimiza
la suma de cuadrados de los errores; un error es la diferencia entre un valor observado
y el valor proporcionado por el modelo matemático (robot).
El algoritmo recursivo de mı́nimos cuadrados se describe a continuación. Considérese
el siguiente modelo de regresión:
7.2.1
7.2.1. Linealidad en los parámetros
Considere el modelo matemático que está expresado como un regresor lineal de la
siguiente forma:
y(k) = Ψ(k)T θ
Alfaomega
(7.1)
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
7.2
333
Método de mı́nimos cuadrados
donde y(k) ∈ IRr es un vector de mediciones (entradas o salidas) del sistema,
Ψ(k) ∈ IRm×r es la matriz de regresión compuesta por observaciones de funciones
conocidas y el vector de parámetros desconocidos está representado por θ ∈ IRm . El
modelo (7.1) está indexado por la variable k, la cual denota el tiempo discreto; se
asume que el conjunto de ı́ndices τ (k), Ψ(k) forman un conjunto discreto.
El algoritmo de mı́nimos cuadrados para el caso vectorial tiene la siguiente forma:
θ̂(k) =
P (k) =
θ̂(k − 1) + P (k − 1) Ψ(k)[I + Ψ(k − 1)T P (k − 1)Ψ(k)]−1 e(k)
(7.2)
P (k − 1) − P (k − 1)Ψ(k)[I + Ψ(k)T P (k − 1)Ψ(k)]−1 Ψ(k)T P (k − 1)
(7.3)
e(k) = y(k) − Ψ(k)T θ̂(k − 1)
(7.4)
donde P (k) ∈ IRm×m es la matriz de covarianza, la cual es una matriz definida
positiva, e(k) ∈ IRr es el error de predicción, θ̂(k) es el vector de parámetros
estimados; θ es el vector de parámetros reales.
El algoritmo de mı́nimos cuadrados para el caso escalar tiene la siguiente forma:
θ̂(k)
=
P (k) =
θ̂(k − 1) +
P (k − 1) Ψ(k)e(k)
1 + Ψ(k − 1)T P (k − 1)Ψ(k)
P (k − 1) −
T
P (k−1)Ψ(k)Ψ(k) P (k−1)
1+Ψ(k−1)T P (k−1)Ψ(k)
e(k) = y(k) − Ψ(k)T θ̂(k − 1)
(7.5)
(7.6)
(7.7)
Propiedades: El algoritmo de mı́nimos cuadrados puede identificar los parámetros
de cualquier modelo matemático lineal, no lineal, dinámico, estático, continuo o
discreto (no depende del periodo de muestreo, inclusive puede ser aperiódico). La
estructura matemática del modelo debe cumplir con la condición de linealidad en los
parámetros, es decir que pueda ser expresado como un regresor lineal manteniendo la
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
334
Capı́tulo 7: Identificación paramétrica
estructura de una matriz de observaciones o mediciones y un vector de parámetros.
7.3 Librerı́a de mı́nimos cuadrados
L
a sintaxis de la función de mı́nimos cuadrados varı́a dependiendo de que el
modelo sea expresado en forma escalar o multivariable (vectorial) de la siguiente
forma:
7.3.1
7.3.1. Caso escalar
Para el caso de modelos matemáticos con estructura escalar
y(k) = Ψ(k)T θ̂(k − 1)
la sintaxis de la función de mı́nimos cuadrados tiene la siguiente forma:
function [r, θ̂] =mincuad(y,fi)
donde y(k) ∈ IR es un escalar definido como arreglo con n renglones con el registro
de las correspondientes n mediciones del sistema; indexado por el pivote k para
k = 1, 2, · · ·, n; fi ∈ IRn×m es la matriz de regresión compuesta por n renglones
de observaciones de funciones conocidas y m columnas (m indica el número de
parámetros). Esta función retorna el vector de parámetros estimados θ̂ ∈ IRm
y r es una matriz de dimensión n × m que contiene el registro temporal de las
componentes θ̂i del vector de parámetros estimados θ̂ que tuvieron durante el proceso
de identificación.
En el cuadro 7.1 se presenta el código fuente del algoritmo de mı́nimos cuadrados en
su versión escalar. En la lı́nea 2 se obtiene el número de observaciones o mediciones
(n= número de renglones) que contiene cada columna de la matriz de regresión fi
(m= número de columnas). La lı́nea 3 genera el vector de parámetros estimados θ̂,
con condición inicial cero θ̂(0) (lı́nea 4). En realidad, la condición inicial puede ser
puesta en cualquier valor. La lı́nea 5 contiene la formación del vector columna ψ con
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
7.3
Librerı́a de mı́nimos cuadrados
335
las observaciones o mediciones. La matriz de covarianza P ∈ IRm×m se genera en
la lı́nea 6; observe que la condición inicial P (0) es una matriz diagonal con valores
pii = 1020 , i = 1, 2 · · ·, m.
De las lı́neas 8 -14 se realiza el algoritmo recursivo de identificación paramétrica de
mı́nimos cuadrados de acuerdo a las ecuaciones (7.5)-(7.7). El error de predicción
e(k) se calcula en la lı́nea 12, el vector de parámetros θ̂ en la lı́nea 13, mientras que
la matriz de covarianza P en la lı́nea 14. La forma como varı́an las componentes θ̂i
del vector de parámetros estimados θ̂ se registra en la lı́nea 16. Esta información es
muy útil para analizar a través de una gráfica la convergencia paramétrica de cada
componente θ̂i en función del tiempo t.
Código Fuente 7.1
mincuad.m
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 7 Identificación paramétrica.
mincuad.m
1
function [r, theta] =mincuad(y,fi)
2
3
[n,m]=size(fi); %n=número de renglones, m=número de columnas
theta=[1:m]’; %vector columna de parámetros
4
5
6
theta(1)=0; %Condición inicial del vector de parámetros
psi=[1:m]’; %vector columna de observaciones
P=eye(m,m)*10e20; %matriz de covarianza P
7
8
9
10
r=eye(n,m); %registro para los parámetros estimados
for k=1:n
% algoritmo recursivo de mı́nimos cuadrados
for i=1:m %se forma el regresor
psi(i,1)=fi(k,i);
11
12
13
end
e=y(k)-theta’*psi;%error de regresión
theta= theta+(P*psi*e)/(1+psi’*P*psi); %vector estimado
14
15
16
17
P=P-(P*psi*(psi’)*P)/(1+psi’*P*psi); %matriz de covariancia
for i=1:m
r(k,i)=theta(i,1);%registro por cada iteración de parámetros estimados
end
18
19
end
end
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
336
Capı́tulo 7: Identificación paramétrica
7.3.2
7.3.2. Caso multivariable
Cuando el sistema está expresado como multivariable, es decir tiene n entradas o
salidas y1 , y2 , · · · , yn bajo la siguiente estructura matemática:
⎡
⎤
y1 (k)
⎢ y2 (k) ⎥
⎢
⎥
⎢ .. ⎥ = Ψ(k)T θ̂(k − 1)
⎣ . ⎦
yn (k)
entonces la librerı́a del algoritmo de mı́nimos cuadrados recursivo correspondiente
se denomina mincuadm teniendo la sintaxis que a continuación se describe:
function θ̂ =mincuadm(y,fi,Nob,p,n)
donde y es un vector de dimensión n×Nob con las mediciones u observaciones de
las entradas (salidas) del sistema a identificar; n indica el número de variables
y1 , y2 , · · · , yn ; Nob representa el número total de observaciones. Cada variable yi tiene
un conjunto de Nob observaciones o renglones, para i = 1, · · ·n; fi es el regresor
de observaciones compuesto por p columnas (p= número de parámetros). Cada
columna del regresor fi tiene un número Nob de observaciones; cada observación
evidentemente está registrada en su respectivo renglón.
En la lı́nea 2 se genera el vector de parámetros estimados θ̂. La condición inicial de
este vector es cero. Sin embargo, puede ser inicializado con cualquier valor (lı́nea 3).
La matriz de covarianza P ∈ IRp×p se define en la lı́nea 21. Note que el valor inicial
de esta matriz P (0) corresponde a una matriz diagonal con valores 1020 (ver lı́nea
5). La matriz identidad con las dimensiones adecuadas se define en la lı́nea 6. El
vector de observaciones y ∈ IRn para entradas (salidas) del sistema se define en la
lı́nea 7.
El algoritmo de mı́nimos cuadrados recursivo multivariable de las ecuaciones (7.2)(7.4) se implementa de las lı́neas 8 a la 21. Observe que en la lı́nea 14 se forma el
vector y ∈ IRn , de manera recursiva a cada renglón se le asigna la correspondiente
medición u observación.
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
7.3
Librerı́a de mı́nimos cuadrados
337
En la lı́nea 17 se calcula el vector de error de predicción e(k), en la lı́nea 18 el vector
de parámetros estimados θ̂ y en la lı́nea 19 la matriz de covarianza P .
La función mincuadm retorna el vector de parámetros estimados θ̂ ∈ IRp . En el cuadro
7.2 se presenta el código fuente de la función minicuad.m (caso multivariable).
Código Fuente 7.2
mincuadm.m
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 7 Identificación paramétrica.
mincuadm.m
1
2
3
4
function theta =mincuadm(y,fi,Nob,p,n)
theta=[1:p]’; %vector columna de parámetros
theta(1)=0; %condición inicial del vector de parámetros
psi=zeros(p,n); %vector columna de observaciones
5
P=eye(p,p)*10e20; %matriz de covarianza P
I=eye(n,n); %matriz identidad
ys=zeros(n,1);
6
7
8
for k=1:Nob
for j=1:n
for i=1:p %se forma el regresor
psi(i,j)=fi(k+Nob*(j-1),i);
9
10
11
12
end
for i=1:n
ys(i,1)=y(k+Nob*(i-1));
end
13
14
15
16
18
end
e=ys-psi’*theta; %error de regresión
theta= theta+P*psi*(I+psi’*P*psi)∧ (-1)*e; %vector estimado
19
P=P-(P*psi*(I+psi’*P*psi)∧ (-1)*(psi’)*P); %Matriz de covarianza
17
20
21
end
end
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
338
Capı́tulo 7: Identificación paramétrica
7.4 Ejemplos
E
n esta sección se presentan ejemplos sencillos que ilustran la forma de utilizar
las librerı́as de mı́nimos cuadrados recursivo para el caso escalar mincuad y
multivariable mincuadm.
♣ Ejemplo 7.1
Identificar el parámetro a1 del siguiente modelo:
y = a1 t3
donde el valor teórico de a1 =8.034.
Solución
El sistema y = a1 t3 corresponde al caso escalar. Para propósitos de simulación la
variable t estará definida en el intervalo de 0 a 10 segundos, con incrementos de un
milisegundo.
En este caso el regresor está formado por
y(k) = Ψ(k)T θ
donde y = y(k), Ψ(k) = t3 , θ = a1 .
Por lo tanto, el error de predicción e(k) está dado por:
e(k) = t3 θ̂
Ψ(k)
En el cuadro 7.3 se muestra el código del programa cap7 ejemplo1.m; en la lı́nea
7 se forma la ecuación del sistema a identificar, el vector de mediciones o regresor
está definido en la lı́nea 8. El algoritmo de mı́nimos cuadrados se emplea en la lı́nea
10; el resultado se presenta en la lı́nea 11.
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
7.4
339
Ejemplos
El resultado del programa 7.3 es:
theta=
8.034
Código Fuente 7.3
cap7 ejemplo1.m
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 7 Identificación paramétrica.
cap7 ejemplo1.m
1
clc;
2
clear all;
close all;
format short g
3
4
5
6
7
8
9
t=[0:0.001:10]’; %vector columna de tiempo
a1=8.034; %parámetro del sistema
y=a1*t.*t.*t; %sistema a identificar
fi=[t.*t.*t ] ;
10
[m, p]=size(fi);
[˜ theta] =mincuad(y,fi);
11
theta %resultado de la identificación
♣ Ejemplo 7.2
Identificar los parámetros del siguiente sistema:
y = a1 t + a2 sen(t) + a3 cos(t) + a4 tanh(t)
donde
los
valores
de
los
parámetros
son:
[ a1 , a2 , a3, a4 ]T
=
T
[ 4.456, 7.456, −0.089, 12.37 ]
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
340
Capı́tulo 7: Identificación paramétrica
Solución
El error de predicción se encuentra dado por
e(k) = y(k) − [ t
sen(t)
cos(t)
tanh(t) ] θ̂(k − 1)
Ψ(k)
donde θ̂ = [ â1
â2
â3
â4 ]T .
Para propósitos de simulación, la variable tiempo t estará definida en un intervalo
de 0 a 10 segundos, con incrementos de un milisegundo. El cuadro 7.4 contiene el
programa cap7 ejemplo2.m; en la lı́nea 5 se define el sistema a identificar. El vector
de observaciones o regresor se encuentra construido en la lı́nea 6 y en la lı́nea 7 se
emplea la función de mı́nimos cuadrados para obtener el vector de parámetros θ̂ en
la lı́nea 8.
El resultado del programa cap7 ejemplo2.m es el siguiente:
theta=
[4.456
Código Fuente 7.4
7.456 −0.089
12.37]T
cap7 ejemplo2.m
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 7 Identificación paramétrica.
cap7 ejemplo2.m
1
clc; clear all; close all; format short g
2
t=[0:0.001:10]’;%vector columna de tiempo
3
%parámetros del sistema
a1=4.456; a2=7.456; a3=-0.089; a4=12.37; %sistema a identificar
y=a1*t+a2*sin(t)+a3*cos(t)+a4*tanh(t);
4
5
6
7
fi=[t, sin(t), cos(t), tanh(t)];
[˜ theta] =mincuad(y,fi);
8
theta %resultado de la identificación
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
7.4
341
Ejemplos
♣ Ejemplo 7.3
Identificar los parámetros del siguiente sistema:
y(t) = a1 cosh (t) + a2 senh(t) + a3 cos(t)
donde los valores de los parámetros son:
⎡ ⎤
⎡
⎤
a1
0.001
⎢ ⎥
⎢
⎥
⎣ a2 ⎦ = ⎣ 456 ⎦
a3
−345
Solución
El error de regresión está formado de la siguiente forma:
⎡
e(k) = y(k) − [ cosh (k)
â1
⎤
⎢ ⎥
senh(k) cos(k) ] ⎣ â2 ⎦
â3
ψ (k)
θ̂ (k−1)
(7.8)
El cuadro 7.5 contiene el programa cap7 ejemplo3.m con el código fuente en
MATLAB para llevar a cabo el proceso de identificación paramétrica del sistema.
Se emplea un vector tiempo definido de 0 a 10 segundos con intervalos de un
milisegundo. El sistema a identificar se encuentra definido en la lı́nea 8; el regresor fi
se forma en la lı́nea 9. La función de mı́nimos cuadrados recursivo mincuad se emplea
en la lı́nea 10 para obtener el vector de parámetros estimados θ̂, cuyo resultado se
presenta en la lı́nea 11:
theta=
0.001
456
−345
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
342
Capı́tulo 7: Identificación paramétrica
Código Fuente 7.5
cap7 ejemplo3.m
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 7 Identificación paramétrica.
cap7 ejemplo3.m
1
clc;
2
clear all;
close all;
format short g
t=[0:0.001:10]’;%vector columna de tiempo
3
4
5
6
7
8
9
%parámetros del sistema
a1=0.001; a2=456; a3=-345; %sistema a identificar
y=a1*cosh(t)+a2*sinh(t)+a3*cos(t);
fi=[cosh(t), sinh(t), cos(t)];
10
[˜ , theta] =mincuad(y,fi);
11
theta %resultado de la identificación
♣ Ejemplo 7.4
Identificar los parámetros del siguiente sistema dinámico lineal:
ẋ = −ax + bu
donde los valores de los parámetros son: [ a, b ]T = [ 6, 2 ]T
Solución
El error de predicción está definido por:
â
e(k) = ẋ − [ −x u ]
b̂
y(k)
ψ (k)
θ̂ (k−1)
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
7.4
343
Ejemplos
El cuadro 7.7 contiene el código del programa cap7 ejemplo4.m. Debido a que el
sistema a identificar es dinámico, se empleará la función ode45() para llevar a cabo
el proceso de integración numérica. El programa cap7 se1.m tiene implementado al
sistema dinámico ẋ = −ax + bu, cuyo código se encuentra en el cuadro 7.6. Esta
declaración también se encuentra en el programa cap7 se1.m.
Es importante resaltar la utilidad de la variable global u que registra la entrada
del sistema dinámico u = sen(t) (ver lı́nea 4 ). La lı́nea 12 del programa
cap7 ejemplo4.m realiza la solución numérica del sistema ẋ = −ax + bu para un
intervalo de tiempo de 0 a 5 segundos, con incrementos de un milisegundo. En esta
fase se obtiene la variable de estado x, la cual se emplea en la lı́nea 13 para obtener
la variable velocidad ẋ. El regresor de observaciones fi se forma en la lı́nea 14. El
vector de parámetros estimados θ̂ se obtiene en la lı́nea 16; usando el algoritmo de
mı́nimos cuadrados implementado en la función mincuad y el resultado se presenta
en la lı́nea 17:
theta=
6
2
Código Fuente 7.6
cap7 se1.m
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 7 Identificación paramétrica.
cap7 se1.m
1
2
3
function xp =se1(t,x)
global qpp u
u=sin(t); a=6; b=2;
4
xp=-a*x+b*u;
qpp=xp;
5
6
end
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
344
Capı́tulo 7: Identificación paramétrica
Código Fuente 7.7
cap7 ejemplo4.m
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 7 Identificación paramétrica.
cap7 ejemplo4.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
clc;
clear all;
close all;
global u %variable global almacena la entrada del sistema
format short g
ti=0;
h=0.001;
tf = 5;
ts=ti:h:tf; %vector tiempo de simulación
opciones=odeset(’RelTol’,1e-3,’InitialStep’,2.5e-3,’MaxStep’,2.5e-3);
%solución numérica del sistema dinámico lineal
[t,x]=ode45(’cap7 se1’,ts,0,opciones);
xp=se1(t,x); %se obtiene la velocidad
fi=[-x, u]; %regresor
%algoritmo de mı́nimos cuadrados
[r,theta] =mincuad(xp,fi);
theta %resultado de la identificación
♣♣♣ Ejemplo 7.5
Identificar los parámetros del siguiente sistema multivariable:
y1
8.7 sen(t) + 0.001 cos(t)
=
y2
109.789 atan (t) − 0.23 ∗ tanh t
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
7.4
345
Ejemplos
Solución
En el cuadro 7.8 se muestra el programa cap7 ejemplo5.m; observe que en la lı́nea
3 se forma el vector columna, en la lı́nea 4 se obtiene el número de observaciones o
muestras Nob. En las lı́neas 5-6 se define el sistema a identificar; en la lı́nea 7 se forma
el vector de salida del sistema y = [y1 ; y2 ], el regresor o vector de observaciones se
forma en la lı́nea 8 y 9. La función mincuadm es utilizada en la lı́nea 10 para obtener el
vector de estimación paramétrica cuyo resultado es: θ̂ = [ 8.7, 0.001, 109.79, −0.23].
Código Fuente 7.8
cap7 ejemplo5.m
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 7 Identificación paramétrica.
cap7 ejemplo5.m
1
2
3
4
5
6
7
8
9
clc; clear all; close all;
format short g
t=(0:0.001:10)’;
[Nob, ˜ ]=size(t);
y1=8.7*sin(t)+0.001*cos(t);
y2=109.789*atan(t)-0.23*tanh(t);
y=[y1; y2];
fi=[sin(t), cos(t), zeros(Nob,1), zeros(Nob,1);
10
zeros(Nob,1), zeros(Nob,1), atan(t), tanh(t)];
theta=mincuadm(y,fi,Nob,4,2);
11
theta
A continuación se presentan aplicaciones de identificación paramétrica de sistemas
mecatrónicos y robots manipuladores. Particularmente se explotan las propiedades
matemáticas del modelo dinámico de los robots manipuladores para expresarlo
como un regresor lineal teniendo casos escalares (modelo de energı́a y potencia)
y multivariable (modelo dinámico).
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
346
Capı́tulo 7: Identificación paramétrica
7.5 Modelos de regresión del péndulo
L
a metodologı́a de Euler-Lagrange permite obtener el modelo dinámico de robots
manipuladores con propiedades importantes para diseñar y analizar esquemas
de control e identificación paramétrica.
Dentro de las propiedades del modelo dinámico de robots manipuladores se encuentra
la de linealidad en los parámetros, es decir: a pesar de que la dinámica del robot es
no lineal con respecto a las variables de estado de la posición y velocidad, es posible
expresar dicha dinámica como un regresor lineal de una matriz de observaciones y un
vector columna de parámetros desconocidos. Además, las propiedades de linealidad
también se satisfacen en los modelos de energı́a y potencia que seleccionando de
manera conveniente el regresor de observaciones, resultan los mismos parámetros
que los modelos de energı́a y potencia.
Como caso particular de robots manipuladores de 1 gdl se encuentra el péndulo
que tiene las mismas ventajas de linealidad en los parámetros. En esta sección se
presenta la implementación y análisis comparativo de cinco esquemas de regresión
paramétrica: modelo dinámico, dinámico filtrado, energı́a, potencia y potencia
filtrada.
7.5.1
7.5.1. Modelo dinámico del péndulo
E
l modelo dinámico del péndulo robot se encuentra dado por la siguiente
ecuación:
2 ] q̈ + b q̇ + f signo ( q̇ ) + m gl sen(q )
τ1 = [ Ir1 + m1 lc1
1
1 1
c1
1
1 c1
1
(7.9)
donde Ir1 es el momento de inercia del rotor, m1 es la masa del péndulo, lc1
representa el centro de masa, b1 , fc1 son los coeficientes de fricción viscosa y de
Coulomb, respectivamente; las variables articulares son la aceleración q̈1 , velocidad
q̇1 y posición q1 ; la constante de la aceleración debido a la gravedad es g y τ1 es el
par aplicado al servomotor.
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
7.5
Modelos de regresión del péndulo
347
♣ ♣ Ejemplo 7.6
Obtener la identificación paramétrica del modelo dinámico del péndulo-robot.
Tomar en cuenta los valores de los parámetros del péndulo que se encuentran
contenidos en la tabla 6.3.
Solución
El esquema de regresión del modelo dinámico del péndulo está dado por:
⎡ˆ
2 ⎤
Ir1 + m̂1 l̂c1
⎢
⎥
b̂1
⎢
⎥
e(k) = τ1 (k) − [ q̈1 (k) q̇1 (k) signo ( q̇1 (k) ) sen(q1 (k)) ] ⎢
⎥
⎣
⎦
fˆc1
y(k)
ψ (k)
m̂1 ĝlˆc1
θ̂ (k−1)
2 , b̂ , fˆ , y m̂ ĝ ˆ
2
donde Iˆr1 + m̂1 l̂c1
1
c1
1 lc1 son los parámetros estimados de Ir1 + m1 glc1 ,
b1 , fc1 , y m1 glc1, respectivamente.
El modelo dinámico del péndulo está contenido en el programa cap7 pendulo.m
(ver cuadro 7.10). Para los propósitos de identificación paramétrica es conveniente
registrar la señal del par aplicado τ1 al servomotor y la aceleración del péndulo q̈1 a
través de variables globales tal y como se define en la lı́nea 2. En la declaración de la
función del péndulo (lı́nea 1) se ha incluido la variable bandera para poder obtener la
aceleración articular q̈1 empleando las variables de estado q1 y q̇1 (ver lı́neas 4 y 5),
o realizar integración numérica usando el algoritmo de Runge-Kutta ode45(...),
que en este caso se emplean en las lı́neas 7 y 8.
Los parámetros del péndulo se encuentran declarados en la lı́nea 11 que corresponden
a la tabla 6.3. De acuerdo con esos valores numéricos, el vector de parámetros
θ = [ 0.18546 0.16 0.19 3.0831 ]T . La aceleración articular del péndulo se obtiene
en la lı́nea 20 y el vector de salida en la lı́neas 22 y 23.
El programa principal cap7 pendulosimu.m permite realizar la identificación
paramétrica del modelo dinámico y se presenta en el cuadro 7.9; en la lı́nea 2 se
declaran las variables globales tau1 qpp1 que también están definidas en el programa
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
348
Capı́tulo 7: Identificación paramétrica
del modelo dinámico del péndulo 7.10. El tiempo de simulación es de 0 a 5 segundos
con pasos de 0.001 milisegundo (lı́nea 3); las condiciones iniciales del péndulo son:
[ q(0)1 , q̇1 (0) ]T = [ 0, 0 ]T (lı́nea 4).
En la lı́nea 7 se realiza la integración numérica del modelo dinámico del péndulo.
En esta fase se obtienen las variables de estado q1 (t), q̇1 (t) contenidas en el vector
x=[ q1 (t), q̇1(t) ]T . En la lı́nea 8 se emplea la función cap7 pendulo(t,x,9); note que
el valor del tercer argumento tiene asignado el valor 9 para no realizar el proceso de
integración numérica, solamente obtener la aceleración articular del péndulo usando
la variable de estado x; la información de la aceleración articular se registra en la
variable global qpp1 y la energı́a aplicada al servomotor en la variable global tau1.
En las lı́neas 9 y 10 se realiza un cambio de notación con la finalidad presentar más
claro el vector de observaciones que se forma en la lı́nea 11 y el algoritmo de mı́nimos
cuadrados es llamado en la lı́nea 12).
Código Fuente 7.9
cap7 pendulosimu.m
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 7 Identificación paramétrica.
cap7 pendulosimu.m
1
clc; clear all; close all; format short g
2
global tau1 qpp1 %variables globales
ti=0; h=0.001; tf = 5 ;
ci=[0; 0]; %condiciones iniciales
opciones=odeset(’RelTol’,1e-3,’InitialStep’, 1e-3,’MaxStep’,1e-3);
3
4
5
6
7
8
9
10
11
12
13
%solución numérica del modelo dinámico del péndulo
[t,x]=ode45(’cap7 pendulo’,t,ci,opciones);
xp=pendulo(t,x,9);
q1=x(:,1) ; %posición articular
qp1=x(:,2); %velocidad articular
fi=[qpp1, qp1, sign(qp1), sin(q1)]; %regresor ψ
[r,theta] =mincuad(tau1,fi); %mı́nimos cuadrados
theta
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
7.5
349
Modelos de regresión del péndulo
Código Fuente 7.10
cap7 pendulo.m
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 7 Identificación paramétrica.
cap7 pendulo.m
1
function xp =cap7 pendulo(t,x, bandera)
2
global tau1 qpp1
if (bandera==9) %evalúa la aceleración qpp1 en función de q1 y qp1
q1=x(:,1) ;
qp1=x(:,2);
3
4
5
6
else %integración numérica
q1=x(1); %posición articular
qp1=x(2); %velocidad articular
7
8
9
end
10
11
%parámetros del péndulo
m1=3.88; lc1=0.081; ;
12
%coeficientes de fricción viscosa y de Coulomb
13
b1=0.16; fc1=0.19;
Ir1=0.16;
Ip=Ir1+m1*lc1*lc1;
g=9.81;
14
15
16
17
%par aplicado al servomotor
tau1=sin(t);
18
19
%aceleración articular del péndulo
qpp1=(tau1-b1*qp1-fc1*sign(qp1)-m1*g*lc1*sin(q1))/Ip;
20
21
%vector de salida
xp=[ qp1 ; %xp(1)=x(2) %velocidad articular
qpp1] ; %xp(2)=qpp %aceleración articular
22
23
24
end
Los resultados de la identificación paramétrica del modelo dinámico del péndulo se
presentan en la tabla 7.1.
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
350
Capı́tulo 7: Identificación paramétrica
7.5.2
7.5.2. Modelo dinámico filtrado del péndulo
El modelo dinámico presupone que el sistema cuenta con los sensores necesarios para
medir la posición q1 , velocidad q̇1 y aceleración q̈1 . Sin embargo, generalmente los
robots industriales sólo cuentan con el encoder o sensor de posición. Esto representa
una desventaja para medir la aceleración. Por lo tanto, el problema consiste en
usar la información de la posición articular para generar las señales de velocidad y
aceleración.
λ
Para resolver este problema práctico se puede usar un conjunto de filtros Fu = s+λ
u
para obtener la señal filtrada de u (representada por Fu ) y la velocidad u̇ es
aproximada por:u̇ ≈ Ḟu , entonces el modelo dinámico filtrado del péndulo se obtiene
λ
a la ecuación (7.9) obteniendo:
por aplicar el operador filtro s+λ
λτ1
s+λ
Fτ
λq̈1
λq̇1
λ signo ( q̇1 )
λ sen(q1 )
+ b1
+ fc1
+ m1 glc1
s+λ
s+λ
λ+s
s+λ
λ
signo
(
q̇
)
λ sen(q1 )
λ
q̇
λq
1
1
1
2
= [ Ir1 + m1 lc1
+ b1 s
+ fc1
+ mglc1
] s
s+λ
s+λ
λ+s
s+λ
2 ] Ḟ
+
mgl
= [ Ir1 + m1 lc1
+
b
+
f
F
F
Ḟ
Fq̇1
1 q1
c1 signo( q )
c1 sen(q1 )
2 ]
= [ Ir1 + m1 lc1
1
d
donde s = dt
y λ es la frecuencia de corte del filtro, y las señales filtradas están
dadas en forma recursiva como:
Fτ1 (k) = e−λh Fτ1 (k − 1) + [ 1 − e−λh ] τ1 (k − 1)
Ḟτ1 (k) = −λFτ1 (k) + λτ1 (k)
Fq1 (k) = e−λh Fq1 (k − 1) + [ 1 − e−λh ] q1 (k − 1)
Ḟq1 (k) = −λFq1 (k) + λq1 (k)
Fq̇1 (k) = e−λh Fq̇1 (k − 1) + [ 1 − e−λh ] Fq1 (k − 1)
ḞḞq (k) = −λFḞq (k) + λḞq1 (k)
1
1
−λh
F sen(q1) (k) = e
F sen(q1 )(k − 1) + [ 1 − e−λh ] sen(q1 )(k − 1)
Ḟ sen(q1) (k) = −λF sen(q1 ) (k) + λ sen(q1 )(k)
F signo( q1 ) (k) = e−λh F signo( q1 ) (k − 1) + [ 1 − e−λh ] signo ( q1 ) (k − 1)
Ḟ signo( q1 ) (k) = −λF signo( q1 )(k) + λ signo ( q1 ) (k)
Fq1 representa la señal filtrada de la posición q1 ; la velocidad q̇1 es aproximada por
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
7.5
Modelos de regresión del péndulo
351
el filtro Ḟq1 y la aproximación de la aceleración es q̈1 ≈ ḞFq̇1 .
♣♣♣ Ejemplo 7.7
Obtener la identificación paramétrica del modelo dinámico filtrado del péndulorobot. Para propósitos de simulación del modelo dinámico tomar en cuenta los
valores de los parámetros del péndulo que se encuentran contenidos en la tabla
6.3.
Solución
El esquema de regresión del modelo dinámico filtrado del péndulo está dado por:
⎡ˆ
2 ⎤
Ir1 + m̂1 ˆlc1
⎢
⎥
b̂1
λτ1 (k)
λ signo( q̇1 (k) )
⎥
q̇1 (k)
λ sen(q1 (k)) ⎢
1 (k)
e(k) =
− [ s λs+λ
]
s λqs+λ
⎢
⎥
λ+s
s+λ
ˆ
+ λ ⎣
⎦
fc1
s (k)
ψ
y(k)
m̂1 ĝ l̂c1
θ̂ (k−1)
El programa cap7 pendulodinafiltsimu.m tiene el código MATLAB que permite
realizar identificación paramétrica del modelo dinámico filtrado del péndulo (ver
cuadro 7.11). El vector tiempo varı́a de 0 a 5 segundos con incrementos de 0.001.
En la lı́nea 8 se emplea la función ode45 para obtener la solución numérica q1 , q̇1
del péndulo. Con estas variables de estado en la lı́nea 9 se llama a la función
cap7 pendulo.m para registrar la señal del par aplicado τ1 = sen(t) la cual fue
declarada como variable global (lı́nea 3). De la lı́nea 11 a la 14 se generan los vectores
correspondientes para implementar los filtros. La frecuencia de corte λ ha sido puesta
de manera empı́rica en 25 Hertz. Los filtros se realizan en las lı́neas 16-24. El regresor
de observaciones se forma en la lı́nea 25 y se emplea como parámetro de entrada con
el registro del par aplicado filtrado en la función de mı́nimos cuadrados (lı́nea 26).
El vector de parámetros estimado θ̂ se encuentra indicado en la tabla 7.1. Nota:
el tiempo de ejecución del programa cap7 pendulodinafiltsimu.m puede demorar
varios segundos por utilizar la función discontinua signo.
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
352
Capı́tulo 7: Identificación paramétrica
Código Fuente 7.11
cap7 pendulodinafiltsimu.m
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 7 Identificación paramétrica.
cap7 pendulodinafiltsimu.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
clc; clear all; close all;
format short g
global tau1 %variable global almacena la entrada del sistema
ti=0; h=0.001; tf = 5; t=ti:h:tf; %vector tiempo
ci=[0; 0]; %condiciones iniciales
opciones=odeset(’RelTol’,1e-3,’InitialStep’,1e-3,’MaxStep’,1e-3);
%solución numérica del modelo dinámico del péndulo
[t,x]=ode45(’cap7 pendulo’,t,ci,opciones) ;
xp=pendulo(t,x,9);
[m n]=size(t); q1=x(:,1); qp1=x(:,2);
Ftau1=zeros(m,1);
Fq1=zeros(m,1); sFq1=zeros(m,1);
Fqp1=zeros(m,1); sFqp1=zeros(m,1);
Fsign=zeros(m,1); Fsin=zeros(m,1);
lambda=25 ;
for k=2:m %filtros recursivos
Ftau1(k)=exp(-h*lambda)*Ftau1(k-1)+(1-exp(-h*lambda))*tau1(k-1);
18
Fq1(k)=exp(-h*lambda)*Fq1(k-1)+(1-exp(-h*lambda))*q1(k-1);
sFq1(k)=-lambda*Fq1(k)+lambda*q1(k);
Fqp1(k)=exp(-h*lambda)*Fqp1(k-1)+(1-exp(-h*lambda))*sFq1(k-1);
sFqp1(k)=-lambda*Fqp1(k)+lambda*sFq1(k);
19
20
21
22
Fsign(k)=exp(-h*lambda)*Fsign(k-1)+(1-exp(-h*lambda))*sign(sFq1(k-1));
Fsin(k)=exp(-h*lambda)*Fsin(k-1)+(1-exp(-h*lambda))*sin(q1(k-1));
23
24
end
25
26
fi=[sFqp1, sFq1, Fsign, Fsin] ;
[r,theta] =mincuad(Ftau1,fi);
27
theta
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
7.5
353
Modelos de regresión del péndulo
7.5.3
7.5.3. Modelo de energı́a del péndulo
Un enfoque particularmente atractivo para el diseño de esquemas de identificación
paramétrica de robots manipuladores se basa en el modelo de energı́a aplicada al
robot manipulador, el cual puede ser descrito como un modelo de regresión lineal en
términos de los parámetros dinámicos.
El modelo de energı́a del péndulo consiste de la suma de la energı́a cinética K(q1 , q̇1 ),
la energı́a potencial U (q1 ) más la energı́a disipativa de la siguiente manera:
t
q1 τ1 dt =
0
1
2 ] q̇ 2 + m gl [ 1 − cos(q ) ] + b
[ Ir1 + m1 lc1
1 c1
1
1
2
0
t
q̇12 dt
+ fc1
t
|q̇1 |dt.
0
♣ ♣ Ejemplo 7.8
Realizar el proceso de identificación paramétrica del modelo de energı́a del
péndulo-robot. Emplear los valores numéricos de los parámetros del péndulo
indicados en la tabla 6.3.
Solución
El modelo de regresión lineal de la energı́a para el péndulo se encuentra dado por:
⎡ˆ
2 ⎤
Ir1 + m̂1 l̂c1
t
⎢ m̂ ĝ ˆl
⎥
t
t
1 1 c1 ⎥
⎢
e(k) =
q1 τ1 dt − [ 12 q̇12 [ 1 − cos(q1 ) ] 0 q̇12 dt 0 |q̇1 |dt ] ⎢
⎥
⎣
⎦
b̂1
0
ψ (k)
y(k)
fˆc1
θ̂ (k−1)
En el cuadro 7.12 se muestra el programa cap7 penduloenergsimu.m; en la lı́nea 7 se
obtiene la solución numérica de la dinámica del péndulo. Por medio de las variables
de estado de posición y velocidad en la lı́nea 8 se emplea el modelo dinámico para
registrar la señal de par aplicado τ1 = sen(t). La integral de la energı́a, fricción
viscosa y de Coulomb se calcula de la lı́nea 15 a la 19. La técnica de integración
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
354
Capı́tulo 7: Identificación paramétrica
numérica que se utiliza es la del método de Euler. El regresor del modelo de energı́a
se forma en la lı́nea 20 y el vector de parámetros estimados θ̂ se obtiene en la lı́nea
21 por medio de la función mincuad; el valor de los parámetros identificados se
encuentran en la tabla 7.1.
Código Fuente 7.12
cap7 penduloenersimu.m
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 7 Identificación paramétrica.
cap7 penduloenersimu.m
1
clc; clear all; close all; format short g
2
4
global tau1 %variable global almacena la entrada del sistema
ti=0; h=0.001; tf = 5; t=ti:h:tf; %vector tiempo
ci=[0; 0]; %condición inicial
5
opciones=odeset(’RelTol’,1e-3,’InitialStep’,1e-3,’MaxStep’,1e-3);
6
%solución numérica del modelo dinámico del péndulo
[t,x]=ode45(’cap7 pendulo’,t,ci,opciones) ;
xp=pendulo(t,x,9) ;
3
7
8
9
10
11
12
13
14
15
16
[m n]=size(t);
q1=x(:,1);
qp1=x(:,2);
Int1=zeros(m,1);
Int2=zeros(m,1);
Int3=zeros(m,1);
for k=2:m
Int1(k,1)=Int1(k-1,1)+h*qp1(k,1)*tau1(k,1);
17
Int2(k,1)=Int2(k-1,1)+h*qp1(k,1)*qp1(k,1);
Int3(k,1)=Int3(k-1,1)+h*abs(qp1(k,1)) ;
18
19
end
20
21
fi=[1/2*qp1.*qp1, Int2, Int3, (1-cos(q1))] ;
[r,theta] =mincuad(Int1,fi) ;
22
theta
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
7.5
355
Modelos de regresión del péndulo
7.5.4
7.5.4. Modelo de potencia del péndulo
El modelo de potencia del péndulo está dado por la siguiente ecuación:
2
τ1 q̇1 = [ Ir1 + m1 lc1
] q̇1 q̈ + b1 q̇12 + fc1 |q̇1 | + m1 glc1 sen(q1 )q̇1 .
(7.10)
El modelo de la potencia involucra la medición de la aceleración articular q̈1 .
♣ ♣ Ejemplo 7.10
Realizar el proceso de identificación paramétrica para el modelo de potencia del
péndulo. Utilice los parámetros del péndulo indicados en la tabla 6.3.
Solución
El modelo de potencia del péndulo tiene la propiedad de linealidad en los parámetros;
por lo que el regresor lineal adquiere la siguiente forma:
⎡ˆ
2 ⎤
Ir1 + m̂1 lˆc1
⎢
⎥
b̂1
⎢
⎥
e(k) = τ1 q̇1 − [ q̇1 q̈ q̇12 |q̇1 | sen(q1 )q̇1 ] ⎢
(7.11)
⎥
ˆ
⎣
⎦
fc1
y(k)
ψ (k)
m̂1 ĝ l̂c1
θ̂ (k−1)
El programa en MATLAB cap7 pendulopotsimu.m se encuentra descrito en el
cuadro 7.13. De manera análoga a los anteriores programas, el uso de las variables
globales es adecuado para registrar señales importantes en la formación del regresor.
En la lı́nea 3 se encuentra la definición de las variables tau1 y qpp1 para registrar la
información en el modelo dinámico del péndulo (cap7 pendulo.m); la lı́nea 8 resuelve
numéricamente la dinámica del péndulo y en la lı́nea 9 se registran las señales tau1
y qpp1 para formar el regresor de observaciones (lı́nea 12). La función mincuad es
empleada para obtener la estimación de los parámetros (lı́nea 13).
El resultado del vector de parámetros θ̂ se encuentra indicado en la tabla 7.1.
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
356
Capı́tulo 7: Identificación paramétrica
Código Fuente 7.13
cap7 pendulopotsimu.m
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 7 Identificación paramétrica.
cap7 pendulopotsimu.m
1
2
3
clc; clear all; close all;
format short g
6
global tau1 qpp1 %variables globales para almacenar par aplicado y aceleración
ti=0; h=0.001; tf = 5; t=ti:h:tf; %vector de tiempo
ci=[0; 0]; %condiciones iniciales
opciones=odeset(’RelTol’,1e-3,’InitialStep’,1e-3,’MaxStep’,1e-3);
7
%solución numérica de la dinámica del péndulo
8
[t,x]=ode45(’cap7 pendulo’,t,ci,opciones);
xp=pendulo(t,x,9);
q1=x(:,1);
4
5
9
10
11
12
13
14
qp1=x(:,2);
fi=[qpp1.*qp1, qp1.*qp1, abs(qp1), sin(q1).*qp1];
[r,theta] =mincuad(tau1.*qp1,fi);
theta
7.5.5
Modelo de potencia filtrada
El modelo de la potencia filtrada del péndulo se obtiene aplicando el operador filtro
λ
s+λ al modelo de potencia (7.10) obteniendo la siguiente expresión:
λτ1 q̇1
s+λ
λq̇1 q̈1
λq̇12
λ|q˙1 |
λ sen(q1 )q̇1
+ b1
+ fc1
+ m1 glc1
s+λ
s+λ
s+λ
s+λ
2
2
1
2 ] s λq̇1 + b λq̇1 + f λ|q̇1 | + m gl λ sen(q1 )q̇1
[ Ir1 + m1 lc1
1
c1
1 c1
2
s+λ
s+λ
s+λ
s+λ
(7.12)
2
= [ Ir1 + m1 lc1
]
=
El modelo de potencia del péndulo tiene la desventaja de requerir la medición de
la aceleración q̈1 como en el caso del modelo dinámico. Sin embargo, la aceleración
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
7.5
357
Modelos de regresión del péndulo
puede ser obtenida por aproximación de técnicas de filtrado. La aceleración puede
λq̇ 2
q̇1q̈1
d
ser aproximada de la siguiente forma: λs+λ
= 12 s s+λ1 , s = dt
.
♣♣♣ Ejemplo 7.10
Obtener el valor numérico de los parámetros del péndulo empleando el modelo de
potencia filtrada. Utilice los parámetros del péndulo indicados en la tabla 6.3.
Solución
El modelo de regresión de la potencia filtrada para el péndulo es:
⎡ˆ
2 ⎤
Ir1 + m̂1 ˆlc1
⎢
⎥
2
b̂1
λτ1 q1
⎥
λq̇12
λ sen(q1 )q̇1
λ|q̇1| ⎢
1 λq̇1
e(k) =
− [ 2 s s+λ
⎥
s+λ
s+λ
s+λ ] ⎢
ˆ
s
+
λ
⎣
⎦
fc1
ψ (k)
ˆ
y(k)
m̂1 ĝlc1
θ̂ (k−1)
El proceso de identificación paramétrica del modelo dinámico filtrado del péndulo
se implementó en el programa cap7 pendulopotfilsimu.m (cuadro 7.14). A través
de la señal filtrada de la posición (lı́neas 13-21) se puede obtener la estimación de
la velocidad y aceleración. Por ejemplo, la señal filtrada de la posición se obtiene en
la lı́nea 15 y la aproximación de la velocidad articular se realiza en la lı́nea 16. La
señal filtrada de la velocidad articular se genera en la lı́nea 17, la cual se emplea para
aproximar a la aceleración en la lı́nea 18. Evidentemente la señal de la aceleración
es de baja calidad debido a que fue obtenida de la aproximación de la velocidad; lo
más adecuado es obtener la aceleración por aproximación de la señal de velocidad
que entrega el sensor del robot. No siempre es posible realizar eso, ya que los robots
sólo están equipados con el sensor del encoder para proporcionar información de
la posición. Sin embargo, este es un escenario real que corresponde al ambiente
industrial donde la contaminación de ruido es alto en las variables de estado. En
este contexto, la sintonı́a de la frecuencia de corte λ presenta un problema práctico
el cual se determina de manera empı́rica. El resultado del vector θ̂ se encuentra en
la tabla 7.1.
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
358
Capı́tulo 7: Identificación paramétrica
Código Fuente 7.14
cap7 pendulopotfilsimu.m
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 7 Identificación paramétrica.
cap7 pendulopotfilsimu.m
1
2
3
4
clc; clear all; close all;
format short g
global tau1 qpp1 %variables global para almacenar par y aceleración
6
ti=0; h=0.001; tf = 5; t=ti:h:tf; %vector tiempo
ci=[0; 0]; %condiciones iniciales
opciones=odeset(’RelTol’,1e-3,’InitialStep’,1e-3,’MaxStep’,1e-3);
7
%solución numérica de la dinámica del péndulo
8
[t,x]=ode45(’cap7 pendulo’,t,ci,opciones);
xp=pendulo(t,x,9); [m n]=size(t);
q1=x(:,1); qp1=x(:,2);
Ftau1qp1=zeros(m,1); Fq1=zeros(m,1); sFq1=zeros(m,1); Fqp1qp1=zeros(m,1);
5
9
10
11
12
13
14
15
16
17
18
19
20
sFqp1=zeros(m,1); Fabs=zeros(m,1); Fsinqp1=zeros(m,1); lambda=25;
for k=2:m %filtros
Ftau1qp1(k)=exp(-h*lambda)*Ftau1qp1(k-1)+(1-exp(-h*lambda))*tau1(k1)*qp1(k-1);
Fq1(k)=exp(-h*lambda)*Fq1(k-1)+(1-exp(-h*lambda))*q1(k-1);
sFq1(k)=-lambda*Fq1(k)+lambda*q1(k) ;
Fqp1qp1(k)=exp(-h*lambda)*Fqp1qp1(k-1)+(1-exp(-h*lambda))*sFq1(k1)*sFq1(k-1);
sFqp1(k)=-lambda*Fqp1qp1(k)+lambda*sFq1(k)*sFq1(k);
Fabs(k)=exp(-h*lambda)*Fabs(k-1)+(1-exp(-h*lambda))*abs(sFq1(k-1));
Fsinqp1(k)=exp(-h*lambda)*Fsinqp1(k-1)+(1-exp(-h*lambda))*sin(q1(k1,1))*sFq1(k-1,1);
21
22
23
24
end
fi=[(1/2)*sFqp1, Fqp1qp1, Fabs, Fsinqp1];
[r,theta] =mincuad(Ftau1qp1,fi);
theta
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
7.5
359
Modelos de regresión del péndulo
7.5.6
7.5.6. Análisis comparativo de esquemas de regresión
La tabla 7.1 contiene el resumen de resultados de los cinco esquemas de regresión
evaluados para el péndulo. Para propósitos de comparación se presentan los valores
reales de los parámetros θi , con i = 1, · · · , 4.
Los modelos de regresión dinámico y de potencia generan una estimación exacta,
libre de error, en contraste el modelo de regresión de la energı́a tiene un 99.956 %
de exactitud en el parámetro del momento de inercia, tiene una deriva del 3.43 % en
referencia al valor del coeficiente de fricción viscosa, la exactitud de estimación en
el coeficiente de fricción de Coulomb es del 99.91 %, mientras que para el parámetro
del par gravitacional es del 100 %. En este procedimiento hay que tomar en cuenta
que el método de integración numérica seleccionado fue el de Euler y también influye
el espaciamiento que existe entre las muestras (h = 0.001). Los modelos de regresión
filtrado presentan pobre exactitud en la identificación paramétrica; errores grandes
alrededor del 40 %. En este punto es conveniente resaltar que la frecuencia de corte
de los filtros λ influye notablemente en la convergencia paramétrica, su valor se
λ
determina en forma empı́rica; además la función de transferencia del filtro λ+s
distorsiona la fase, lo que trae como consecuencia errores de aproximación en la
aceleración articular.
Tabla 7.1 Comparación de resultados de identificación paramétrica
Parámetro
Ir1 + m1 l2c1
Nm seg2
rad
b1
Nm seg
rad
fc1
Nm
m1 glc1
Nm
0.18546
0.16
0.19
3.0831
l2c1
Iˆr1 + m̂1 ˆ
b̂1
fˆc1
m̂1 ĝ ˆ
lc1
Modelo dinámico
0.18546
0.16
0.19
3.0831
Modelo dinámico filtrado
0.17682
0.25021
0.15068
3.0384
Modelo de energı́a
0.18538
0.16055
0.18983
3.0831
Modelo de potencia
0.18546
0.16
0.19
3.0831
Modelo de potencia filtrada
0.24657
0.26366
0.11265
3.1154
Valor real
Esquema de regresión
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
360
Capı́tulo 7: Identificación paramétrica
7.6 Modelos de regresión del robot de 2 gdl
E
l modelo dinámico de un robot antropomórfico planar de 2 gdl está determinado por la matriz de inercia, fuerzas centrı́petas y de Coriolis, par o torque
gravitacional y fenómeno de fricción. Una parte clave del procedimiento de identificación paramétrica consiste en seleccionar un adecuado conjunto de parámetros;
para el caso particular del robot de 2 gdl se pueden seleccionar 11 parámetros:
⎡
⎤
θ1 + 2θ2 cos(q2 ) θ3 + θ2 cos(q2 )
⎢
⎥
M (q) = ⎣
⎦ ,
θ3 + θ2 cos(q2 )
θ3
⎡
⎤
−2θ2 sen(q2 ) q̇2 −θ2 sen(q2 ) q̇2
⎢
⎥
C(q, q̇) = ⎣
⎦ ,
θ2 sen(q2 ) q̇1
0
⎤
θ4 sen(q1 ) + θ5 sen(q1 + q2 )
⎢
⎥
g(q) = ⎣
⎦ ,
θ5 sen(q1 + q2 )
θ6 q̇1 + θ8 sgn(q̇1 ) + θ10 [ 1 − | signo ( q̇1 ) | ]
f (q̇) =
θ7 q̇2 + θ9 sgn(q̇2 ) + θ11 [ 1 − | signo ( q̇2 ) | ]
⎡
donde
2
2
θ1 = m1 lc1
+ m2 l12 + m2 lc2
+ I1 + I2 ,
θ2 = l1 m2 lc2 ,
2
θ3 = m2 lc2
+ I2 ,
θ4 = g(lc1m1 + m2 l1 ),
θ5 = gm2 lc2 ,
θ6 = b1 ,
θ7 = b2 ,
θ8 = fc1 ,
θ9 = fc2 ,
θ10 = fe1 ,
θ11 = fe2 .
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
7.6
361
Modelos de regresión del robot de 2 gdl
7.6.1
7.6.1. Modelo de regresión dinámico del robot de 2 gdl
El modelo dinámico del robot antropomórfico de 2 gdl puede ser expresado de manera
conveniente como el producto de una matriz de regresión y un vector de parámetros.
Observe que los componentes del vector de parámetros θ = [θ1 · · · θ11 ]T dependen
de una combinación de los parámetros fı́sicos de los eslabones como masas, centros
de masas, momentos de inercia y coeficientes de fricción, los cuales se consideran invariantes en el tiempo. En este caso, el orden de la matriz de regresión P es 11 × 11.
⎡
τ1
τ2
=
ψ
11
ψ12
ψ13
ψ14
ψ15
ψ16
ψ17
ψ18
ψ19
ψ110
ψ21
ψ22
ψ23
ψ24
ψ25
ψ26
ψ27
ψ28
ψ29
ψ210
⎢
⎢
⎢
⎢
⎢
⎢
ψ111 ⎢
⎢
ψ211 ⎢
⎢
⎢
⎢
⎢
⎣
θ1
θ2
θ3
θ4
θ5
θ6
θ7
θ8
θ9
θ10
θ11
⎤
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥ (7.13)
⎥
⎥
⎥
⎥
⎥
⎦
donde los elementos del regresor ψ están dados como:
ψ11 = q̈1
ψ12 = 2 cos(q2 )q̈1 + cos(q2 )q̈2 − 2 sen(q2 )q̇2 q̇1 − sen(q2 )q̇22
ψ13 = q̈2
ψ14 =
sen(q1 )
ψ15 =
sen(q1 + q2 )
ψ16 = q̇1
ψ17 = 0
ψ18 =
signo ( q̇1 )
ψ19 = 0
ψ110 = 1 − | signo ( q̇1 ) |
ψ111 = 0
ψ21 = 0
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
362
Capı́tulo 7: Identificación paramétrica
ψ22 = cos(q2 )q̈1 + sen(q2 )q̇12
ψ23 = q̈1 + q̈2
ψ24 = 0
ψ25 =
sen(q1 + q2 )
ψ26 = 0
ψ27 = q̇2
ψ28 = 0
ψ29 =
signo ( q̇2 )
ψ210 = 0
ψ211 = 1 − | signo ( q̇2 ) |
♣♣♣ Ejemplo 7.12
Realizar el procedimiento de identificación paramétrica de un robot manipulador
de 2 gdl empleando el modelo de regresión dinámico. Emplear los valores
numéricos de los parámetros del robot de 2 gdl indicados en la tabla 6.4.
Solución
El modelo de regresión del modelo dinámico del robot antropomórfico de 2 gdl
está dado por la siguiente expresión:
2
e(k) =
»
ψ11
−
τ2 | ψ21
τ1
y(k)
ψ12
ψ22
ψ13
ψ23
ψ14
ψ24
ψ15
ψ25
ψ16 ψ17
ψ26 ψ27
{z
ψ(k)
ψ18
ψ28
ψ19
ψ29
ψ110
ψ210
θ̂1
3
6 θ̂2 7
6
7
6
7
6 θ̂3 7
6
7
6 θ̂4 7
6
7
6
7
– 6 θ̂5 7
7
ψ111 6
6 θ̂6 7
7
ψ211 6
6
7
} 6 θ̂7 7
6
7
6 θ̂8 7
6
7
6
7
6 θ̂9 7
6
7
4 θ̂10 5
θ̂11
| {z }
θ̂(k−1)
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
7.6
Modelos de regresión del robot de 2 gdl
363
Note que el problema de identificación paramétrica para un robot de 2 gdl es
complejo, ahora el modelo de regresión es multivariable y no escalar como los
esquemas de regresión del péndulo, por lo que deberá usarse la función mincuadm.
Una fase importante del procedimiento consiste en elegir una adecuada trayectoria
de entrada al robot manipulador que permita obtener cierto grado de excitación en
la matriz de regresión debido a que la calidad de estimación del vector de parámetros
depende de los niveles de excitación del regresor; por ejemplo el algoritmo de mı́nimos
cuadrados recursivo usualmente requiere satisfacer ciertas condiciones de excitación
persistente. Sin embargo, la selección de la trayectoria no es una tarea trivial, debido
a que la matriz de regresión está compuesta por una combinación no lineal de
las variables dinámicas del robot tal como sen(q2 )q̇12 , cos(q2 )q̇1 . Para el diseño de
la trayectoria de excitación persistente se recomienda superposición de funciones
senoidales como los siguientes pares aplicados:
τ1 = [ 1 − e−0.8t ] 29.0 + 68 sen(16t + 0.1) + 9 sen(20t + 0.15)
(7.14)
τ2 = [ 1 − e−1.8t ] 1.2 + 8 sen(26t + 0.08) + 2 sen(12t + 0.34)
(7.15)
El cuadro 7.15 contiene implementado el modelo dinámico del robot de 2 gdl en
lenguaje MATLAB (programa cap7 iderobot2gdl.m); se han utilizado los valores
numéricos de los parámetros del robot mostrados en la tabla 6.4, ası́ como las
ecuaciones del par aplicado (7.14)-(7.15).
El programa principal cap7 iderobot2gdlsimu.m permite realizar el procedimiento
de identificación paramétrica del robot de 2 gdl, y está descrito en el cuadro 7.16.
El registro de las señales de pares aplicados τ1 , τ2 se realiza a través de variables
globales (lı́nea 2). La solución numérica del modelo dinámico del robot de 2 gdl se
encuentra en la lı́nea 7; empleando dicha solución numérica q1 (t), q2 (t), q̇1 (t), q̇2 (t)
en el modelo dinámico del robot se registran la aceleración q̈ = [ q̈1 , q̈2 ] y pares
aplicados τ1 , τ2 (ver lı́neas 11 a la 16).
Las componentes ψij , con i = 1, 2, · · ·, 11 y j = 1, 2 del regresor de observaciones
se forman entre las lı́neas 18 y 24. El regresor ψ queda formado en las lı́neas 25 y
26. La función de mı́nimos cuadrados recursivo para el caso multivariable mincuadm
se usa en la lı́nea 27 para obtener el vector de parámetros estimado θ ∈ˆIR11 . Los
resultados se encuentran en la tabla 7.2.
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
364
Capı́tulo 7: Identificación paramétrica
Código Fuente 7.15
cap7 iderobot2gdl.m
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 7 Identificación paramétrica.
cap7 iderobot2gdl.m
1
function xp = cap7 iderobot2gdl(t,x)
2
global tau1 tau2
q1=x(1); q2=x(2); q = [q1; q2]; %vector de posición articular
qp1=x(3); qp2=x(4); qp = [qp1; qp2]; % vector de velocidad articular
m1=23.902; l1=0.45; lc1=0.091; %parámetros del robot
3
4
5
6
I1=1.266; b1=2.288; fc1=7.17; fe1=8.8; m2=3.880; l2=0.45; lc2=0.048;
I2=0.093; b2=0.175; fc2=1.734; fe2=1.87; g=9.81;
theta1=m1*lc1*lc1+m2*l1*l1+m2*lc2*lc2+I1+I2;
theta2=l1*m2*lc2; theta3=m2*lc2*lc2+I2; theta4=g*(lc1*m1+m2*l1);
7
8
9
10
theta5=g*m2*lc2; theta6=b1; theta7=b2; theta8=fc1; theta9=fc2;
theta10=fe1; theta11=fe2;
11
12
%matriz de inercia
M=[theta1+2*theta2*cos(q2), theta3+theta2*cos(q2);
13
14
theta3+theta2*cos(q2), theta3];
15
%matriz de Coriolis y fuerzas centrı́petas
C=[ -2*theta2*sin(q2)*qp2, -theta2*sin(q2)*qp2;
theta2*sin(q2)*qp1, 0];
16
17
18
gq11=theta4*sin(q1)+theta5*sin(q1+q2); gq21=theta5*sin(q1+q2);
gq=[gq11; gq21]; %par gravitacional
19
20
%par de fricción viscosa, Coulomb y estática
fr=[theta6*qp1+theta8*sign(qp1)+theta10*(1-abs(sign(qp1)));
21
22
theta7*qp2+theta9*sign(qp2)+theta11*(1-abs(sign(qp2)))];
tau1=(1-exp(-0.8*t))*29.0+ 68*sin(16*t+0.1) + 9*sin(20*t+0.15);
tau2=(1-exp(-1.8*t))*1.2+ 8*sin(26*t+0.08)+2*sin(12*t+0.34);
tau=[tau1; tau2]; %señal de excitación persistente
23
24
25
qpp = M∧ (-1)*(tau-C*qp-gq-fr); %aceleración articular
xp = [qp1; qp2; qpp(1); qpp(2)]; %vector de salida
26
27
28
end
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
7.6
365
Modelos de regresión del robot de 2 gdl
Código Fuente 7.16
cap7 iderobot2gdlsimu.m
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 7 Identificación paramétrica.
cap7 iderobot2gdlsimu.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
clc; clear all; close all; format short g
global tau1 tau2
ti=0; h=0.001; tf = 5; t=ti:h:tf; %tiempo de simulación
ci=[0; 0; 0; 0]; %condiciones iniciales
opciones=odeset(’RelTol’,1e-3,’InitialStep’, 1e-3, ’MaxStep’, 1e-3);
%solución numérica del robot antropomórfico de 2 gdl
[t,x]=ode45(’cap7 iderobot2gdl’,t,ci,opciones);
T
T
%vectores de posición q = [ q1 , q2 ] y velocidad q̇ = [ q̇1 , q̇2 ]
q1=x(:,1); q2=x(:,2); qp1=x(:,3); qp2=x(:,4);
[m n]=size(t); qpp1=zeros(m,1); qpp2=zeros(m,1); u1=zeros(m,1); u2=zeros(m,1);
for k=1:m
xp=cap7 iderobot2gdl(t(k),[x(k,1),x(k,2),x(k,3), x(k,4)]);
qpp1(k,1)=xp(3,1);
qpp2(k,1)=xp(4,1);
u1(k,1) =tau1; u2(k,1) =tau2;
end
tau=[u1; u2];
fi11=qpp1;
fi12=2*cos(q2).*qpp1+cos(q2).*qpp2-2*sin(q2).*qp2.*qp1-sin(q2).*qp2.*qp2;
fi13=qpp2; fi14=sin(q1); fi15=sin(q1+q2); fi16=qp1; fi17=zeros(m,1);
fi18=sign(qp1); fi19=zeros(m,1); fi110=(1-abs(sign(qp1))); fi111=zeros(m,1);
fi21=zeros(m,1); fi22=cos(q2).*qpp1+sin(q2).*qp1.*qp1; fi23=qpp1+qpp2;
fi24=zeros(m,1); fi25=sin(q1+q2); fi26=zeros(m,1); fi27=qp2;
fi28=zeros(m,1); fi29=sign(qp2); fi210=zeros(m,1); fi211=(1-abs(sign(qp2)));
fi=[fi11, fi12, fi13, fi14, fi15, fi16, fi17, fi18, fi19, fi110, fi111 ;
27
fi21, fi22, fi23, fi24, fi25, fi26, fi27, fi28, fi29, fi210, fi211];
theta=mincuadm(tau,fi,m,11,2);
28
theta
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
366
Capı́tulo 7: Identificación paramétrica
7.6.2
7.6.2. Modelo de energı́a del robot de 2 gdl
El modelo de energı́a de un robot de 2 gdl con condiciones iniciales cero está dado
por la siguiente ecuación:
t
[ τ1
τ2 ]
q̇1
q̇2
0
⎤
θ1 + 2θ2 cos(q2 ) θ3 + θ2 cos(q2 ) 1 q̇1
⎢
⎥ q̇1
dt =
⎣
⎦
2 q̇1
q̇1
θ3 + θ2 cos(q2 )
θ3
⎡ t 2 ⎤
θ6 0 q̇1 dt
θ4 [ 1 − cos(q1 ) ] + θ5 [ 1 − cos(q1 + q2 ) ]
⎥
⎢
+
+⎣
⎦
t 2
θ5 [ 1 − cos(q1 + q2 ) ]
θ7 0 q̇2 dt
t
⎡ t
⎤ ⎡
⎤
θ8 0 |q̇1 |dt
θ10 0 q̇1 [ 1 − | signo ( q̇1 | ) ]dt
⎢
⎥ ⎢
⎥
+⎣
⎦+⎣
⎦
t
t
θ9 0 |q̇2 |dt
θ11 0 q̇2 [ 1 − | signo ( q̇2 | ) ]dt
(7.16)
T
⎡
El modelo de energı́a del robot manipulador de 2 gdl no requiere de la medición de
la aceleración articular, y a demás se convierte en un modelo de regresión escalar.
♣♣♣ Ejemplo 7.12
Llevar a cabo la identificación paramétrica de un robot manipulador de 2 gdl
empleando el modelo de regresión de la energı́a. Emplear los valores numéricos de
los parámetros del robot de 2 gdl indicados en la tabla 6.4.
Solución
El error de predicción del modelo de regresión de la energı́a del robot antropomórfico
de 2 gdl se encuentra expresado de la siguiente manera:
Rt
e(k) = [
Alfaomega
0
τ1 q̇1 + τ2 q̇2 ] dt − [ ψ11
y(k)
ψ12
ψ13
ψ14
ψ15
ψ16
ψ (k)
ψ17
ψ18
ψ19
ψ110
ψ111 ] θ̂
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
7.6
Modelos de regresión del robot de 2 gdl
367
siendo las componentes ψij del vector de regresión:
1 2
q̈
2 1
= cos(q2 )q̇1 (q̇1 + q̇2 )
ψ11 =
ψ12
ψ13 = q̇2 [ 12 q̇2 + q̇1 ]
ψ14 = 1 − cos(q1 )
ψ15 = 1 − cos(q1 + q2 )
t
q̇12 dt
ψ16 =
0
t
ψ17 =
0
t
ψ18 =
q̇22 dt
|q̇1 |dt
0
t
ψ19 =
|q̇2 |dt
0
t
ψ110 =
q̇1 [ 1 − | signo ( q̇1 | ) ]dt
0
ψ111 =
t
q̇2 [ 1 − | signo ( q̇2 | ) ]dt
0
El programa cap7 energiarobot2gdlsimu.m que se encuentra en el cuadro 7.17
lleva a cabo la implementación de identificación paramétrica para el robot de 2 gdl.
De manera análoga al anterior ejemplo se emplea el modelo dinámico del robot de
2 gdl para obtener la solución numérica (lı́nea 10) y el registro de la señal del par
aplicado en la lı́nea 15.
El conjunto de integrales del regresor de observaciones se encuentra implementado
de la lı́nea 20 a la 28 a través de la técnica de integración numérica de Euler. Las
componentes individuales ψij del regresor se forman en las lı́neas 30-32, mientras que
el regresor completo está declarado en la lı́nea 33. La función de mı́nimos cuadrados
para la versión escalar mincuad se emplea en la lı́nea 34. El resultado del vector de
parámetros estimados θ̂ está contenido en la tabla 7.2.
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
368
Capı́tulo 7: Identificación paramétrica
Código Fuente 7.17
cap7 energiarobot2gdlsimu.m
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 7 Identificación paramétrica.
cap7 energiarobot2gdlsimu.m
1 clc; clear all; close all;
2
3
4
5
format short g
global tau1 tau2
ti=0; h=0.001; tf = 5; t=ti:h:tf; %vector tiempo
ci=[0; 0; 0; 0]; %condiciones iniciales
6 opciones=odeset(’RelTol’,1e-3, ’InitialStep’,1e-3, ’MaxStep’,1e-3);
7 %solución numérica del robot de 2 gdl
8 disp(’Advertencia: la simulación puede durar varios segundos......’)
9 disp(’Espere por favor..............’)
10 [t,x]=ode45(’cap7 iderobot2gdl’,t,ci,opciones);
11 q1=x(:,1); q2=x(:,2);%vector de posición
12 qp1=x(:,3); qp2=x(:,4); %vector de velocidad
13 [m n]=size(t); Int1=zeros(m,1); Int2=zeros(m,1); Int3=zeros(m,1); Int4=zeros(m,1);
14 Int5=zeros(m,1); Int6=zeros(m,1); Int7=zeros(m,1); u1=zeros(m,1); u2=zeros(m,1);
15 for k=1:m
%registro de la se~
nal del par aplicado τ
16
17
18
xp=cap7 iderobot2gdl(t(k),[x(k,1),x(k,2),x(k,3), x(k,4)]);
u1(k,1) =tau1;
u2(k,1) =tau2;
19 end
20 for k=2:m
%integrales del regresor
21
Int1(k,1)=Int1(k-1,1)+h*(qp1(k,1)*u1(k,1)+qp2(k,1)*u2(k,1));
22
Int2(k,1)=Int2(k-1,1)+h*qp1(k,1)*qp1(k,1);
23
24
25
Int3(k,1)=Int3(k-1,1)+h*qp2(k,1)*qp2(k,1);
Int4(k,1)=Int4(k-1,1)+h*abs(qp1(k,1));
Int5(k,1)=Int5(k-1,1)+h*abs(qp2(k,1));
26
27
Int6(k,1)=Int6(k-1,1)+h*qp1(k,1)*(1-abs(sign(qp1(k,1))));
Int7(k,1)=Int7(k-1,1)+h*qp2(k,1)*(1-abs(sign(qp2(k,1))));
28 end
29
30
31
32
%regresor ψ
fi11=(1/2)*qp1.*qp1 ; fi12=cos(q2).*qp1.*(qp1+qp2); fi13=qp2.*((1/2)*qp2+qp1);
fi14=1-cos(q1); fi15=1-cos(q1+q2); fi16=Int2; fi17=Int3;
fi18=Int4; fi19=Int5; fi110=Int6; fi111=Int7;
33 fi=[fi11, fi12, fi13, fi14, fi15, fi16, fi17, fi18, fi19, fi110, fi111];
34 [r,theta] =mincuad(Int1,fi) ;
35 theta
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
7.6
Modelos de regresión del robot de 2 gdl
369
7.6.3
7.6.3. Modelo de potencia del robot de 2 gdl
El modelo de potencia del robot de 2 gdl representa la derivada temporal del modelo
de energı́a. La siguiente ecuación describe el modelo de potencia:
[ τ1
τ2 ]
q̇1
q̇2
⎤
θ3 + θ2 cos(q2 ) q̇1
⎢
⎥ q̈1
=
⎣
⎦
q̇1
q̈1
θ3 + θ2 cos(q2 )
θ3
⎡
⎤
T −2θ2 sen(q2 )q̇2 −θ2 sen(q2 )q̇2 1 q̇1
⎢
⎥ q̇1
+
⎣
⎦
2 q̇1
q̇1
−θ2 sen(q2 )q̇2
0
⎡
⎤
θ6 q̇12
θ4 sen(q1 )q̇1 + θ5 sen(q1 + q2 ) [ q̇1 + q̇2 ]
⎢
⎥
+
+⎣
⎦
θ5 sen(q1 + q2 ) [ q̇1 + q̇2 ]
2
θ7 q̇2
⎡
⎤ ⎡
⎤
θ8 |q̇1 |
θ10 q̇1 [ 1 − | signo ( q̇1 | ) ]
⎢
⎥ ⎢
⎥
+⎣
⎦+⎣
⎦
θ9 |q̇2 |
θ11 q̇2 [ 1 − | signo ( q̇2 | ) ]
(7.17)
T
⎡
θ1 + 2θ2 cos(q2 )
El modelo de potencia del robot manipulador de 2 gdl requiere la medición de la
aceleración articular, y pertenece a la clase de modelos de regresión escalar.
♣♣♣ Ejemplo 7.13
Obtener el valor numérico de los parámetros de un robot de 2 gdl empleando el
modelo de potencia. Utilice los parámetros del robot de 2 gdl indicados en la tabla
6.4.
Solución
Para llevar a cabo la identificación paramétrica del robot de 2 gdl usando el modelo
de potencia es necesario obtener el modelo de regresión. Con esta finalidad considere
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
370
Capı́tulo 7: Identificación paramétrica
el siguiente regresor:
⎡
e(k) = [ τ1
q̇1
τ2 ]
−[ ψ11
q̇2
ψ12
ψ13
ψ14
ψ15
ψ16
ψ17
y(k)
ψ18
ψ19
ψ110
⎢
⎢
⎢
⎢
⎢
⎢
⎢
ψ111 ] ⎢
⎢
⎢
⎢
⎢
⎢
⎣
θ1
θ2
θ3
θ4
θ5
θ6
θ7
θ8
θ9
θ10
θ11
donde los elementos del regresor ψ están dados como:
ψ11 = q̇1 q̈1
ψ12 = − sen(q2 )q̇2 q̇1 [ q̇1 + q̇2 ] + cos(q2 )q̈1 [ q̇1 + q̇2 ] + cos(q2 )q̇1 [ q̈1 + q̈2 ]
ψ13 = q̈2 [ 12 q̇2 + q̇1 ] + q̇2 [ 12 q̈2 + q̇1 ] + q̇2 [ 12 q̇2 + q̈1 ]
ψ14 =
sen(q1 )q̇1
ψ15 =
sen(q1 + q2 ) [ q̇1 + q̇2 ]
ψ16 = q̇12
ψ17 = q̇22
ψ18 = |q̇1 |
ψ19 = |q̇2 |
ψ110 = q̇1 [ 1 − | signo ( q̇1 ) | ]
ψ111 = q̇2 [ 1 − | signo ( q̇2 ) | ]
El programa cap7 potenciarobot2gdlsimu.m contiene el código para realizar la
identificación paramétrica de un robot de 2 gdl a través del modelo de potencia
(ver cuadro 7.18). En forma similar a los anteriores ejemplos, primero se realiza
la integración numérica sobre la dinámica del robot, con las variables de estado se
obtiene el registro de aceleración y señales con los pares aplicados a los servomotores
(lı́neas 18-24). Las componentes del vector de observaciones se indican en las lı́neas
25 a la 31. El regresor se forma en la lı́nea 32 y la función de mı́nimos cuadrados
escalar se emplea en la lı́nea 33. El vector de predicción θ̂ se encuentra indicado en
la tabla 7.2.
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
⎤
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎦
7.6
371
Modelos de regresión del robot de 2 gdl
Código Fuente 7.18
cap7 potenciarobot2gdlsimu.m
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 7 Identificación paramétrica.
cap7 potenciarobot2gdlsimu.m
1 clc; clear all;
2 close all;
3 format short g
4
5
6
7
global tau1 tau2
ti=0; h=0.001; tf = 5;
t=ti:h:tf; %vector tiempo
ci=[0; 0; 0; 0]; %condiciones iniciales
8 opciones=odeset(’RelTol’, 1e-3, ’InitialStep’,1e-3, ’MaxStep’,1e-3);
9 %solución numérica del robot de 2 gdl
10 disp(’Advertencia: la simulación puede durar varios segundos......’)
11
12
13
14
disp(’Espere por favor..............’)
[t,x]=ode45(’cap7 iderobot2gdl’,t,ci,opciones);
q1=x(:,1); q2=x(:,2);%vector de posición
qp1=x(:,3); qp2=x(:,4);%vector de velocidad
15 [m n]=size(t);
16 qpp1=zeros(m,1); qpp2=zeros(m,1);%vectores de aceleración
17 u1=zeros(m,1); u2=zeros(m,1); %vectores de pares aplicados
18 for k=1:m
%registro de la aceleración q̈ y par aplicado τ
19
xp=cap7 iderobot2gdl(t(k),[x(k,1),x(k,2),x(k,3), x(k,4)]);
qpp1(k,1)=xp(3,1);%información de aceleración q̈1 (t)
20
qpp2(k,1)=xp(4,1);%información de aceleración q̈2 (t)
21
22
23
u1(k,1) =tau1; %par aplicado τ1 (t)
u2(k,1) =tau2; %par aplicado τ2 (t)
24 end
25 y=qp1.*u1+qp2.*u2; %regresor y
26 %regresor ψ
27 fi11=qp1.*qpp1;
28
29
30
31
fi12=-sin(q2).*qp2.*qp1.*(qp1+qp2)+cos(q2).*qpp1.*(qp1+qp2)+cos(q2).*qp1.*(qpp1+qpp2);
fi13=qpp2.*((1/2)*qp2+qp1)+qp2.*((1/2)*qpp2+qp1)+qp2.*((1/2)*qp2+qpp1);
fi14=sin(q1).*qp1; fi15=sin(q1+q2).*(qp1+qp2); fi16=qp1.*qp1; fi17=qp2.*qp2;
fi18=abs(qp1); fi19=abs(qp2); fi110=qp1.*(1-abs(sign(qp1))); fi111=qp2.*(1-abs(sign(qp2)));
32 fi=[fi11, fi12, fi13, fi14, fi15, fi16, fi17, fi18, fi19, fi110, fi111];
33 [r,theta] =mincuad(y,fi);
34 theta
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
372
Capı́tulo 7: Identificación paramétrica
7.6.4
7.6.4. Análisis comparativo de resultados de regresión
La tabla 7.2 muestra los resultados de identificación paramétrica obtenidos a través
de los esquemas de regresión dinámico, energı́a y potencia para el robot manipulador
de 2 gdl en configuración antropomórfica.
De acuerdo a los resultados, el modelo dinámico es el que mejor estimación realiza,
inclusive identifica de manera exacta los parámetros de fricción estática. Estos
parámetros son particularmente difı́ciles de estimar debido a que la trayectoria
de excitación persistente produce movimiento en todo momento y por lo tanto el
fenómeno de fricción estática desaparece, es decir el término [ 1 − | signo ( q̇i ) | ] = 0
cuando el robot está en movimiento (q̇i > 0 o q̇i < 0), entonces esta componente no
mostrará variación temporal de tal forma que el algoritmo de mı́nimos cuadrados
tendrá divergencia en estos parámetros. Uno de los motivos por lo cual se obtiene
convergencia paramétrica en los parámetros de fricción estática es debido al fuerte
acoplamiento de la dinámica no lineal que se ve reflejado en el regresor multivariable.
En contraste con los métodos escalares este acoplamiento es más débil. De ahı́ que la
deriva en dichos parámetros es más pronunciada. El método de la potencia también
Tabla 7.2 Resultados de identificación paramétrica del robot de 2 gdl
Parámetro
Valor real
Dinámico
Energı́a
Potencia
θ̂1
2.351
2.351
2.358
2.353
θ̂2
0.0838
0.0838
0.0841
0.0843
θ̂3
0.1019
0.1019
0.1025
0.1017
θ̂4
38.466
38.466
38.461
38.412
θ̂5
1.827
1.827
1.8134
1.812
θ̂6
2.288
2.288
2.282
2.522
θ̂7
0.175
0.175
0.1616
0.140
θ̂8
7.17
7.17
7.1082
6.865
θ̂9
1.734
1.734
1.8289
1.761
θ̂10
8.8
8.8
10
10
θ̂11
1.87
1.87
11
11
proporciona buena exactitud en los resultados de identificación paramétrica, debido
que no se realiza ningún tipo de aproximación de la aceleración. No obstante, el
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
7.7
373
Robot cartesiano de 3 gdl
requerir medir la aceleración representa una desventaja práctica. Por otro lado,
el método de regresión de energı́a tiene pobre desempeño, realiza la identificación
con mayor margen de error. Note que los dos esquemas escalares divergen en los
parámetros de fricción estática
Otro aspecto importante que afecta la calidad en el proceso de identificación paramétrica es la forma de diseñar la señal de excitación persistente. Preferentemente
debe tener estructuras senoidales cuyas amplitudes no saturen a los servoamplificadores.
7.7 Robot cartesiano de 3 gdl
E
l modelo dinámico del robot manipulador en configuración cartesiana de 3 gdl
está formado por la matriz de masas, para gravitacional y fenómeno de fricción:
⎡ ⎤ ⎡
⎤⎡ ⎤ ⎡
⎤⎡ ⎤ ⎡
⎤⎡
⎤
0
τ1
θ1 0
d¨1
θ4 0 0
d˙1
θ7 0 0
signo ( d˙1 )
⎢ ⎥ ⎢
⎥⎢ ⎥ ⎢
⎥⎢ ⎥ ⎢
⎥⎢
⎥
0 ⎦ ⎣ d¨2 ⎦ + ⎣ 0 θ5 0 ⎦ ⎣ d˙2 ⎦ + ⎣ 0 θ8 0 ⎦ ⎣ signo ( d˙2 ) ⎦
⎣ τ2 ⎦= ⎣ 0 θ2
τ3
0
⎡
0
⎤
θ3
d¨3
0
0
θ6
d˙3
0
0
θ9
signo ( d˙3 )
θ10
⎢
⎥
+⎣ 0 ⎦
0
donde
θ1 = m1 + m2 + m3
θ2 = m1 + m2
θ3 = m3
θ4 = b1
θ5 = b2
θ6 = b3 ,
θ7 = fc1 ,
θ8 = fc2 ,
θ9 = fc3 ,
θ10 = g ( m1 + m2 + m3 )
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
374
Capı́tulo 7: Identificación paramétrica
7.7.1
7.7.1. Modelo de regresión dinámico del robot cartesiano
El modelo de regresión para el robot cartesiano de 3 gdl tiene la siguiente estructura:
⎡
⎤
θ̂1
⎢
⎥
⎢ θ̂2 ⎥
⎢
⎥
⎢ θ̂3 ⎥
⎢
⎥
⎢
⎥
⎡ ⎤
⎡
⎤ ⎢ θ̂4 ⎥
⎢
⎥
τ1
ψ11 ψ12 ψ13 ψ14 ψ15 ψ16 ψ17 ψ18 ψ19 ψ110 ⎢
⎥
θ̂
5
⎢ ⎥
⎥
⎢
⎥⎢
⎥
⎣ τ2 ⎦ = ⎣ ψ21 ψ22 ψ23 ψ24 ψ25 ψ26 ψ27 ψ28 ψ29 ψ210 ⎦ ⎢
⎢ θ̂6 ⎥
⎥
τ3
ψ31 ψ32 ψ33 ψ34 ψ35 ψ36 ψ37 ψ38 ψ39 ψ310 ⎢
⎥
⎢
⎢ θ̂7 ⎥
⎢
⎥
y(k)
ψ (k)
⎢ θ̂8 ⎥
⎢
⎥
⎢
⎥
⎣ θ̂9 ⎦
θ̂10
θ̂ (k−1)
donde las componentes del vector de regresión ψij , i = 1, 2, 3 y j = 1, 2, · · ·, 10 se
encuentran dados por:
ψ11 = d¨1
ψ21 = 0
ψ22 = d¨2
ψ12 = 0
ψ13 = 0
ψ14 = d˙1
ψ23 = 0
ψ31 = 0
ψ32 = 0
ψ33 = d¨3
ψ15 = 0
ψ24 = 0
ψ25 = d˙2
ψ16 = 0
ψ26 = 0
ψ35 = 0
ψ36 = d˙3
ψ27 = 0
ψ37 = 0
ψ18 = 0
ψ28 = signo ( d˙2 )
ψ38 = 0
ψ19 = 0
ψ29 = 0
ψ39 = signo ( d˙3 )
ψ110 = 1
ψ210 = 0
ψ310 = 0
ψ17
= signo ( d˙1 )
ψ34 = 0
En el modelo dinámico del robot cartesiano no se ha considera la fricción estática, que
particularmente es muy pequeña para servomotores con tecnologı́a de transmisión
directa. Note que dicho modelo dinámico no tiene el fenómeno de fuerzas centrı́petas
y de Coriolis, esto se debe a que en esta configuración sólo tiene articulaciones lineales
o prismáticas.
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
7.7
Robot cartesiano de 3 gdl
375
♣♣♣ Ejemplo 7.14
Realizar identificación paramétrica de un robot en configuración cartesiana de
3 gdl empleando el modelo dinámico. Utilice los parámetros del robot de 3 gdl
indicados en la tabla 6.6.
Solución
El error de predicción del modelo dinámico del robot cartesiano de 3 gdl está dado
por:
⎡
⎤
θ̂1
⎢
⎥
⎢ θ̂2 ⎥
⎢
⎥
⎢ θ̂3 ⎥
⎢
⎥
⎢
⎥
⎡ ⎤ ⎡
⎤ ⎢ θ̂4 ⎥
⎥
τ1
ψ11 ψ12 ψ13 ψ14 ψ15 ψ16 ψ17 ψ18 ψ19 ψ110 ⎢
⎢
⎥
⎢ ⎥ ⎢
⎥ ⎢ θ̂5 ⎥
e(k) = ⎣ τ2 ⎦ − ⎣ ψ21 ψ22 ψ23 ψ24 ψ25 ψ26 ψ27 ψ28 ψ29 ψ210 ⎦ ⎢
⎥
⎢ θ̂6 ⎥
⎢
⎥
τ3
ψ31 ψ32 ψ33 ψ34 ψ35 ψ36 ψ37 ψ38 ψ39 ψ310 ⎢
⎢ θ̂7 ⎥
⎥
⎢
⎥
y(k)
ψ (k)
⎢ θ̂8 ⎥
⎢
⎥
⎢
⎥
θ̂
⎣ 9 ⎦
θ̂10
θ̂ (k−1)
El cuadro 7.19 contiene el código MATLAB donde se ha implementado el modelo
dinámico del robot cartesiano de 3 gdl (programa cap7 idecartesiano3gdl.m),
el cual contiene el efecto inercial, fenómeno de fricción viscosa y Coulomb, par
gravitacional, y la señal de prueba o par aplicado a los servomotores (excitación
persistente). Los valores de los parámetros del robot se han tomado de la tabla 6.6.
El programa cap7 idecartesiano3gdlsimu.m permite llevar a cabo el proceso de
identificación paramétrica (ver cuadro 7.20). En la lı́nea 2 se declaran las variables
globales para el registro de las señales del par aplicado. La integración numérica del
modelo dinámico del robot cartesiano de 3 gdl se realiza en la lı́nea 7. A través de
la solución numérica se obtiene la aceleración articular y pares aplicados (lı́nea 13).
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
376
Capı́tulo 7: Identificación paramétrica
Las componentes del vector de observaciones se realiza de la lı́nea 22 a la lı́nea 30;
el regresor completo se obtiene en las lı́neas 31-33. La función mı́nimos cuadrados
multivariable se emplea en la lı́nea 34 para obtener el vector de parámetros estimados
θ̂ cuyo resultado está indicado en la tabla 7.3.
Código Fuente 7.19
cap7 idecartesiano3gdl.m
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 7 Identificación paramétrica.
cap7 idecartesiano3gdl.m
1
function xp = cap7 idecartesiano3gdl(t,x)
2
global tau1 tau2 tau3
d = [x(1); x(2); x(3)]; %vector de posición articular
dp =[x(4); x(5); x(6)]; %vector de velocidad articular
3
4
5
m1=0.7; m2=0.28; m3=0.28;
b1=0.02; b2=0.08; b3=.02;
fc1=0.01; fc2=0.07; fc3=.02; g=9.81;
theta1=m1+m2+m3; theta2=m1+m2; theta3=m3;
6
7
8
9
theta4=b1; theta5=b2; theta6=b3; theta7=fc1;
theta8=fc2; theta9=fc3; theta10=g*(m1+m2+m3);
10
11
%modelo dinámico del robot
M = [theta1, 0, 0; 0 theta2, 0; 0 0 theta3]; %matriz de inercia
12
13
B=[ theta4, 0, 0; 0, theta5, 0; 0, 0, theta6]; %fricción viscosa
Fc=[theta7, 0, 0; 0, theta8, 0; 0, 0, theta9]; %fricción de Coulomb
par grav = [theta10; 0; 0]; %vector de pares de gravitacionales
fr= B*dp+Fc*sign(dp); %vector de fricción
14
15
16
17
tau1=12.36+ 0.05*sin(t);
tau2=(1-exp(-0.5*t))*0.13*sin(t);
tau3=0.06*sin(t);
tau=[tau1; tau2; tau3];
18
19
20
21
dpp = inv(M)*(tau- par grav-fr); %vector de aceleración articular
xp = [dp(1); dp(2); dp(3); dpp(1); dpp(2); dpp(3)]; %vector de salida
22
23
end
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
7.7
Robot cartesiano de 3 gdl
Código Fuente 7.20
377
cap7 idecartesiano3gdlsimu.m
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 7 Identificación paramétrica.
cap7 idecartesiano3gdlsimu.m
1 clc; clear all; close all; format short g
2 global tau1 tau2 tau3
3 ti=0; h=0.001; tf = 5; t=ti:h:tf; %vector tiempo
4 ci=[0; 0; 0; 0;0; 0]; %condiciones iniciales
5 opciones=odeset(’RelTol’,1e-3,’InitialStep’,1e-3,’MaxStep’,1e-3);
6 %solución numérica del robot cartesiano de 3 gdl
7 [t,x]=ode45(’cap7 idecartesiano3gdl’,t,ci,opciones);
8 d1=x(:,1); d2=x(:,2); d3=x(:,3); %posiciones articulares
9 dp1=x(:,4); dp2=x(:,5); dp3=x(:,6); %velocidades articulares
10
11
12
13
[m n]=size(t); % dimensión del vector tiempo
dpp1=zeros(m,1); dpp2=zeros(m,1); dpp3=zeros(m,1);%registro para d̈
u1=zeros(m,1); u2=zeros(m,1); u3=zeros(m,1); %registros para pares aplicados τ
for k=1:m
14
15
16
xp=cap7 idecartesiano3gdl(t(k),[x(k,1),x(k,2),x(k,3), x(k,4), x(k,5), x(k,6)]);
dpp1(k,1)=xp(4,1);
dpp2(k,1)=xp(5,1);
17
18
19
20
dpp3(k,1)=xp(6,1);
u1(k,1) =tau1;
u2(k,1) =tau2;
u3(k,1) =tau3;
21 end
22 tau=[u1; u2; u3]; %vector de pares aplicados
23
24
25
26
fi11=dpp1; fi12=zeros(m,1); fi13=zeros(m,1); fi14=dp1;
fi15=zeros(m,1); fi16=zeros(m,1); fi17=sign(dp1);
fi18=zeros(m,1); fi19=zeros(m,1); fi110=ones(m,1);
fi21=zeros(m,1); fi22=dpp2; fi23=zeros(m,1); fi24=zeros(m,1); fi25=dp2;
27 fi26=zeros(m,1); fi27=zeros(m,1); fi28=sign(dp2); fi29=zeros(m,1); fi210=zeros(m,1);
28 fi31=zeros(m,1); fi32=zeros(m,1); fi33=dpp3;
29 fi34=zeros(m,1); fi35=zeros(m,1); fi36=dp3;
30
31
32
33
fi37=zeros(m,1); fi38=zeros(m,1); fi39=sign(dp3); fi310=zeros(m,1) ;
fi=[fi11, fi12, fi13, fi14, fi15, fi16, fi17, fi18, fi19, fi110;
fi21, fi22, fi23, fi24, fi25, fi26, fi27, fi28, fi29, fi210;
fi31, fi32, fi33, fi34, fi35, fi36, fi37, fi38, fi39, fi310];
34 theta=mincuadm(tau,fi,m,13,3);
35 theta
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
378
Capı́tulo 7: Identificación paramétrica
7.7.2
7.7.2. Modelo de potencia del robot cartesiano de 3 gdl
El modelo dinámico de potencia del robot manipulador cartesiano de 3 gdl está dado
por:
⎡
⎤T ⎡
d˙1
θ1
⎢
⎥
⎢
[ τ1 d˙1 + τ2d˙2 + τ3 d˙3 ] = ⎣ d˙2 ⎦ ⎣ 0
0
d˙3
[ θ7
θ8
⎤ ⎡ ⎤T ⎡
d¨1
d˙1
θ4
⎥⎢ ¨ ⎥ ⎢ ˙ ⎥ ⎢
0 ⎦ ⎣ d2 ⎦ + ⎣ d2 ⎦ ⎣ 0
θ2
0 θ3
0
d¨3
d˙3
⎡ ˙ ⎤
|d1 |
⎢ ˙ ⎥
θ9 ] ⎣ |d2| ⎦ + θ10 d˙1
|d˙3|
0
0
⎤⎡
0
θ5
0
⎤
d˙1
⎥⎢ ⎥
0 ⎦ ⎣ d˙2 ⎦ +
θ6
d˙3
0
⎤⎡
el modelo de potencia del robot cartesiano de 3 gdl pertenece al tipo de regresores
escalares.
♣♣♣ Ejemplo 7.15
Realizar el procedimiento de identificación paramétrica de un robot cartesiano de
3 gdl empleando el modelo de potencia. Utilice los parámetros del robot de 3 gdl
indicados en la tabla 6.6.
Solución
El error de predicción del regresor lineal del modelo de potencia del robot cartesiano
de 3 gdl está dado por:
e(k) =
Alfaomega
[ τ1 d˙1 + τ2 d˙2 + τ3 d˙3 ] − [ ψ11
ψ12
ψ13
ψ14
ψ15
ψ16
ψ17
ψ18
ψ19
2 θ̂ 3
1
6 θ̂2 7
7
6
6 θ̂ 7
6 3 7
7
6
6 θ̂4 7
7
6
6 θ̂ 7
6 5 7
ψ110 ] 6
7
6 θ̂6 7
7
6
6 θ̂ 7
6 7 7
7
6
6 θ̂8 7
7
6
4 θ̂9 5
θ̂10
| {z }
θ̂(k−1)
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
7.7
379
Robot cartesiano de 3 gdl
donde los componentes del regresor están compuestas por:
ψ11 = d˙1 d¨1
ψ12 = d˙2 d¨2
ψ13 = d˙3 d¨3
ψ14 = d˙2
1
ψ15
ψ16
= d˙22
= d˙23
ψ17 = |ḋ1 |
ψ18 = |ḋ2 |
ψ19 = |ḋ3 |
ψ110 = d˙1
El programa cap7 potenciacartesiano3gdlsimu.m muestra el código para realizar
la identificación paramétrica de un robot cartesiano de 3 gdl empleando su modelo
de potencia (ver cuadro 7.21). De la lı́nea 15 a 22 se forman las componentes del
regresor o vector de observaciones. De tal forma que dicho regresor está definido en
la lı́nea 23. La función de mı́nimos cuadrados versión escalar se emplea para obtener
el vector de parámetros estimados θ̂ (lı́nea 24). En la lı́nea 26 se grafica la variación
temporal que tienen las componentes del vectore θ̂ (ver figura 7.1).
Figura 7.1 Comportamiento de los componentes θ̂i .
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
380
Capı́tulo 7: Identificación paramétrica
Código Fuente 7.21
cap7 potenciacartesiano3gdlsimu.m
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 7 Identificación paramétrica.
cap7 potenciacartesiano3gdlsimu.m
1
2
3
clc; clear all; close all; format short g
global tau1 tau2 tau3
ti=0; h=0.001; tf = 5; t=ti:h:tf; %vector tiempo
4
ci=[0; 0; 0; 0;0; 0]’;
opciones=odeset(’RelTol’,1e-3,’InitialStep’,1e-3,’MaxStep’,1e-3);
5
%solución numérica de la dinámica del robot cartesiano de 3 gdl
[t,x]=ode45(’cap7 idecartesiano3gdl’,t,ci,opciones);
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
d1=x(:,1); d2=x(:,2); d3=x(:,3); dp1=x(:,4); dp2=x(:,5); dp3=x(:,6);
[m n]=size(t); dpp1=zeros(m,1); dpp2=zeros(m,1); dpp3=zeros(m,1);
u1=zeros(m,1); u2=zeros(m,1); u3=zeros(m,1);
for k=1:m
xp=cap7 idecartesiano3gdl(t(k),[x(k,1),x(k,2),x(k,3), x(k,4), x(k,5), x(k,6)]);
dpp1(k,1)=xp(4,1); dpp2(k,1)=xp(5,1); dpp3(k,1)=xp(6,1);
u1(k,1) =tau1; u2(k,1) =tau2; u3(k,1) =tau3;
end
y=dp1.*u1+dp2.*u2+dp3.*u3 ;
fi11=dp1.*dpp1;
fi12=dp2.*dpp2;
fi13=dp3.*dpp3;
fi14=dp1.*dp1;
fi15=dp2.*dp2;
fi16=dp3.*dp3;
24
fi17=abs(dp1); fi18=abs(dp2); fi19=abs(dp3); fi110=dp1;
fi=[fi11, fi12, fi13, fi14, fi15, fi16, fi17, fi18, fi19, fi110];
[r,theta] =mincuad(y,fi);
25
theta %vector estimado θ̂
26
plot(t,r) %grafica la variación temporal de los componentes del vector θ̂
23
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
7.7
381
Robot cartesiano de 3 gdl
7.7.3
7.7.3. Análisis comparativo de identificación
Los resultados de identificación paramétrica del robot cartesiano de 3 gdl de los
modelos de regresión dinámico y potencia se encuentran contenidos en la tabla 7.3.
Para propósitos de comparación se proporcionan los valores reales de cada uno de
los componentes del vector θ.
El modelo de regresión dinámico tiene una identificación exacta de los diez
parámetros, este modelo de regresión es multivariable ya que toma como parte del
regresor las señales de pares τ1 , τ2 , τ3 , la matriz de regresión con las observaciones
de posición, velocidad y aceleración tiene dimensión: ψ ∈ IR3×10 .
Por otro lado, el modelo de regresión de la potencia resulta un esquema escalar, en
este caso las señales del par no forman parte del vector de mediciones de la entrada
del robot, más bien, forman parte de la potencia de entrada al robot en forma escalar
q̇1 τ1 + q̇2 τ2 + q̇3 τ3 ; se incorpora un vector de observaciones en lugar de la matriz de
mediciones. Dicho vector de observaciones tiene dimensión: ψ ∈ IR10×1 . El esquema
escalar es mucho más atractivo que la versión vectorial o multivariable debido a que
su implementación es mucho más fácil.
Tabla 7.3 Parámetros identificados del robot cartesiano de 3 gdl
Parámetro
Valor real
Dinámico
Potencia
θ̂1
1.26
1.26
1.26
θ̂2
0.98
0.98
0.98
θ̂3
0.28
0.28
0.28
θ̂4
0.02
0.02
0.02
θ̂5
0.08
0.08
0.08
θ̂6
0.02
0.02
0.02
θ̂7
0.01
0.01
0.01
θ̂8
0.07
0.07
0.07
θ̂9
0.02
0.02
0.02
θ̂10
12.361
12.361
12.361
Ambos esquemas de regresión dinámico (multivaribale) y potencia (escalar)
requieren la medición de la aceleración, lo que representa una desventaja cuando
el robot no dispone de los sensores adecuados para medir esa señal.
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
382
Capı́tulo 7: Identificación paramétrica
7.8 Resumen
E
l método de mı́nimos cuadrados es una técnica de identificación paramétrica
que permite obtener el valor numérico de los parámetros del sistema empleando
mediciones experimentales de posiciones y velocidades articulares.
La naturaleza del modelo matemático a identificar puede ser diversa, por ejemplo:
dinámico (lineal y no-lineal), estático, continuo o discreto. Sin embargo, una
condición necesaria del método de mı́nimos cuadrados es que el modelo matemático
del sistema puede ser expresado como un regresor lineal de los parámetros a
identificar, es decir como el producto de una matriz de observaciones y un vector
columna de parámetros desconocidos; esta estructura genera un regresor vectorial
o multivariable. Ejemplos de esta categorı́a son los modelos dinámico y dinámico
filtrado de robots manipuladores.
Cuando el regresor lineal se puede expresar como el producto de un vector
transpuesto de observaciones y un vector columna de parámetros desconocidos,
entonces el regresor lineal se denomina escalar. Los modelos de regresión de la
energı́a, potencia y potencia filtrada son ejemplos del regresor lineal escalar.
Se han desarrollado dos librerı́as en código fuente MATLAB para resolver el
problema de identificación paramétrica:
[r, θ̂] =mincuad(y,fi)
θ̂ =mincuadm(y,fi,Nob,p,n)
Estas funciones permiten obtener la identificación paramétrica de sistemas
mecatrónicos y robots manipuladores reales. La calidad de estimación paramétrica
está en función del tipo de señal aplicada (excitación persistente), la exactitud de
aproximar las señales de velocidad y aceleración y en la medida de lo posible no
incluir en la matriz o vector de observaciones funciones discontinuas.
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
383
Parte III Referencias selectas
Parte III Referencias selectas
D
entro de la extensa literatura sobre dinámica de sistemas mecánicos, se
recomiendan ampliamente los siguientes textos para que el lector pueda
profundizar en dinámica de sistemas mecatrónicos y robots manipuladores.
Dare A. Wells. “Lagrangian dynamics”. Schaum’s outlines 1967.
Leonard Meirovitch. “Methods of analytical dynamics”. McGrawHill. 1970.
A. P. French. “Mecánica newtoniana”. Editorial Reverté, S. A.
1978.
Wolfram
Stadler. “Analytical robotics and mechatronics”.
McGraw Hill International Editions. 1995.
Donald T. Greenwood. “Classical dynamics”. Dover Publications
Inc. 1997.
Jerry B. Marion. “Dinámica clásica de las partı́culas y sistemas”.
Editorial Reverté, S. A. 2000.
H. Goldstein. “Mecánica clásica”. Editorial Reverté, S. A. 2000.
G. C. Goodwin & K. S. Sin. “Adaptive filtering prediction and
control ”. Prentice-Hall, NY. 1984.
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
384
Capı́tulo 7: Identificación paramétrica
M. Gautier & W. Khalil. “On the identification of the inertial
parameters of robot ”. Proceedings of the 27th Conference on
Decision and Control. Austin, Texas, pp. 2264–2269. 1988.
C. Canudas and A. Aubin. “Parameters identification of robot
manipulators via sequential hybrid estimation algorithms ”. Proc.
IFAC’90 Congress, Tallin, pp. 178–183, (1990).
M. Prüfer, C. Schmidt & F. Wahl. “Identification of robot
dynamics with differential and integral models: a comparison
”. Proceedings IEEE International Conference on Robotics and
Automation. San Diego, California, Vol 1, pp. 340–345. 1994.
F. Reyes F. & R. Kelly. “A direct drive robot for control research”.
Proc. IASTED International Conference, Applications of Control
and Robotics. Orlando, FL. pp. 181–184. 1996.
F. Reyes & R. Kelly. “Experimental evaluation of identification
schemes on a direct drive robot ”. Robotica. Cambridge University
Press. 15, pp. 563-571. 1997.
F. Reyes F. & R. Kelly. “On parameter identification of robot
manipulators ”. Proceedings of the 1997 IEEE International
Conference on Robotics and Automation. Albuquerque, New
Mexico. April. pp. 1910-1915. 1997.
F. Reyes, & E. Lopez. “A new hamiltonian-based identification
scheme for robot manipulators ”. Proceedings of the 5th WSEASIMMCCAS. Cancun, Mexico, May 11-14. pp. 126-131. 2005.
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
385
Parte III Problemas propuestos
Parte III Problemas propuestos
E
n esta sección se presenta una serie de ejercicios con la finalidad de que el lector
mejore sus conocimientos sobre dinámica e identificación paramétrica.
Capı́tulo 6 Dinámica
6.1 Considere la siguiente función de transferencia
y
u
= c
wn2
s2 + 2ρwn + wn2
donde y ∈ IR es la salida del sistema, u ∈ IR es la entrada , ρ ∈ IR+ es el factor
de amortiguamiento, wn ∈ IR+ es la frecuencia natural de resonancia:
(a) Obtener el modelo dinámico.
(b) Realizar la conversión a variables de estado fase.
(c) Escribir un programa en MATLAB para simular el sistema con los
siguientes valores:
wn = 1, ρ = 0.1, c = 1.
wn = 1, ρ = 0.9, c = 1.
wn = 1, ρ = 1.5, c = 1.
6.2 Considere el sistema masa resorte amortiguador, en referencia a los programas
6.1 y 6.2 (modo oscilador), modifique el valor de la entrada de la fuerza a cero
Newtons, además considere condiciones iniciales diferentes a cero.
Realizar la simulación del sistema masa resorte amortiguador y analizar:
(a) La respuesta del sistema (posición y velocidad).
(b) Explicar el comportamiento del sistema.
Fundamente sus respuestas.
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
386
Capı́tulo 7: Identificación paramétrica
6.3 Modificar los programas 6.9 y 6.10 para simular el péndulo robot usando la
siguiente entrada:
τ
= 5 sen(t) + 0.33 cos(t + 0.1random(’Normal’,t,t))
Obtener:
(a) Filtrado de la posición q1 (t).
(b) Estimación de la velocidad.
6.4 Implementar un control tipo proporcional derivativo
τ1
τ2
=
kp1
0
0
kp2
q̃1
q̃2
−
kv1
0
0
kv2
q̇1
q̇2
+ g(q)
para posicionar al robot antropomórfico de 2 gdl en las siguientes
configuraciones:
(a) [ qd1 , qd2 ]T = [ 45, 90 ]T grados.
(b) [ qd1 , qd2 ]T = [ 180, 0 ]T grados.
¿Qué método de sintonı́a en las ganancias proporcional derivativa puede
proponer?
6.5 Implementar un control tipo proporcional derivativo saturado dado por
⎡ ⎤
⎡
⎤⎡
⎤⎡
⎤ ⎡
⎤
τ1
kp1 0
tanh(q̃1 )
tanh(q̇1 )
0
0
kv1 0
⎢ ⎥
⎢
⎥⎢
⎥⎢
⎥ ⎢
⎥
⎣ τ2 ⎦ = ⎣ 0 kp2 0 ⎦ ⎣ tanh(q̃2 ) ⎦ − ⎣ 0 kv2 0 ⎦ ⎣ tanh(q̇2 ) ⎦ + g(q)
τ3
0
0
kp3
tanh(q̃3 )
0
0
kv3
tanh(q̇3 )
para controlar las posiciones articulares de la base, hombro y codo del robot
antropomórfico de 3 gdl en las siguientes configuraciones:
(a) [ qd1 , qd2 , qd3 ]T = [ 45, 90, 90]T grados.
(b) [ qd1 , qd2 , qd3 ]T = [ 45, 180, 0]T grados.
¿Qué método de sintonı́a en las ganancias proporcional derivativa puede
proponer, para el robot de 3 gdl?
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
387
Parte III Problemas propuestos
6.6 Realizar un programa en MATLAB para controlar al robot cartesiano de 3
gdl usando un control proporcional derivativo tipo saturado
⎡ ⎤
⎡
⎤⎡
⎤⎡
⎤ ⎡
⎤
0
0
τ1
kp1 0
atan (q̃1 )
atan (q̇1 )
kv1 0
⎢ ⎥
⎢
⎥⎢
⎥⎢
⎥ ⎢
⎥
⎣ τ2 ⎦ = ⎣ 0 kp2 0 ⎦ ⎣ atan (q̃2 ) ⎦ − ⎣ 0 kv2 0 ⎦ ⎣ atan (q̇2 ) ⎦ + g(q)
τ3
0
0 kp3
atan (q̃3 )
0
0 kv3
atan (q̇3 )
en las siguientes configuraciones:
(a) [ dd1 , dd2, dd3 ]T = [ 0.5, 0.3, .65]T m.
(b) [ dd1 , dd2, dd3 ]T = [ 0.8, 0.2, 0.34 ]T m.
¿Qué método de sintonı́a en las ganancias proporcional derivativa puede
proponer, para el robot cartesiano?
Capı́tulo 7 Identificación paramétrica
7.1 Sean los siguientes sistemas dinámicos escalares:
ẋ = −3x + 8 sen(x) + 8.98x3
ẏ = 6.66y + 5.61e−|y| + 1.23 senh(y)
ż = 2.345z 3 + 6.1234z 4 − 2.31z 5
Para cada uno de los sistemas propuestos implementar un esquema de
identificación paramétrica para obtener el valor de sus respectivos parámetros.
Tome un intervalo para y ∈ [−10, 10].
7.2 Sean los siguientes sistemas multivariables:
ẋ1
0
1
x1
0
=
+
sen(t)
−3 −4
2
ẋ2
x2
⎡ ⎤
⎡
⎤⎡ ⎤ ⎡ ⎤
ẋ1
0
1
1
x1
4
⎢ ⎥
⎢
⎥⎢ ⎥ ⎢ ⎥
⎣ ẋ2 ⎦ = ⎣ −1 −2 −2 ⎦ ⎣ x2 ⎦ + ⎣ 2 ⎦ sen(t)
1
4 −6
9
ẋ3
x3
⎡ ⎤
⎡
⎤⎡ ⎤ ⎡
⎤
ẋ1
0.01
3.1
4.1
0.9
x1
3.2 sen(t)
⎢ ẋ ⎥
⎢ 2.2
⎢ ⎥ ⎢
⎥
1.11 −8.1 7.09 ⎥
⎢ 2⎥
⎢
⎥ ⎢ x2 ⎥ ⎢ 2.1 sen(t) cos(t) ⎥
⎢ ⎥ = ⎢
⎥⎢ ⎥ + ⎢
⎥
⎣ ẋ3 ⎦
⎣ 2.22 4.56 8.23 6.7 ⎦ ⎣ x3 ⎦ ⎣
⎦
0.56t
ẋ4
−0.09 2.33
1.21
4.55
x4
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
sen3 (t)
Alfaomega
388
Capı́tulo 7: Identificación paramétrica
para un intervalo de t ∈ [0, 5].
7.3 Considere el
⎡ ⎤
y1
⎢ ⎥
⎢ y2 ⎥
⎢ ⎥
⎢ y3 ⎥ =
⎢ ⎥
⎢ ⎥
⎣ y4 ⎦
y5
siguiente modelo matemático:
⎡
⎤
0.33t3 + 0.24 sen(t) + cos(t) + 0.35t2 − 4.56 senh(t)
⎢
⎥
⎢ 0.33t1 + 0.48 sen3 (t) + 0.66 cos(t) + 0.70t2 + 0.33 cos(t) ⎥
⎢
⎥
⎢ 0.66t4 + 0.24 sen4 (t) + 0.99 cos(t) + 0.35t2 − 0.66 tan(t) ⎥
⎢
⎥
⎢
⎥
⎣ 0.99t5 + 0.72 sen5 (t) + 0.48 cos(t) + 0.24t2 + 0.33 atan (t) ⎦
0.33t3 + 0.24 sen(t) + cos(t) + 0.48t2 − 0.24 sen(t)
Seleccione un vector de parámetros θ̂ conveniente y diseñe un esquema de
regresión para llevar a cabo el proceso de identificación paramétrica para un
intervalo de t ∈ [0, 5].
7.4 Considere los esquemas de regresión modelo dinámico filtrado y potencia
filtrada del péndulo
(a) En el regresor del modelo dinámico filtrado sustituir la técnica de filtrado
q̇ (k)−q̇1 (k−1)
para obtener la aceleración q̈1 por el método de Euler: q̈1 ≈ 1
y
h
q1 (k)−q1 (k−1)
q̇1 ≈
. Llevar a cabo el proceso de identificación paramétrica
h
y comparar los resultados con el método de filtrado.
(b) En el regresor del modelo de potencia filtrada sustituir la técnica
de filtrado para obtener la aceleración q̈1 por el método de Euler:
q̈1 ≈ q̇1 (k)−hq̇1 (k−1) y q̇1 ≈ q1 (k)−qh1 (k−1) . Llevar a cabo el proceso de
identificación paramétrica y comparar los resultados con el método de
filtrado.
7.5 Considere un brazo robot antropomórfico de 2 gdl:
(a) Implementar el esquema de regresión lineal del modelo dinámico filtrado.
(b) Implementar el esquema de regresión lineal del modelo de la potencia
filtrada.
7.6 Considere un robot manipulador en configuración cartesiana de 3 gdl:
(a) Implementar el esquema de regresión lineal dinámico filtrado.
(b) Implementar el esquema de regresión lineal del modelo de energı́a.
(c) Implementar el esquema de regresión lineal del modelo de la potencia
filtrada.
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
Parte IV
Control
Control de robots manipuladores es un tema vigente en la actualidad y de interés
para la comunidad cientı́fica. El diseño de sistemas de control en aplicaciones
industriales representa no sólo un reto teórico, también resuelve sustancialmente
problemas prácticos para mejorar deficiencias de algoritmos de control tradicionales.
La Parte IV está dedicada a presentar aplicaciones de la metodologı́a de
moldeo de energı́a la cual hoy en dı́a representa una técnica moderna de diseño
de algoritmos de control para robots manipuladores.
La Parte IV se compone del Capı́tulo 8 Control
Capı́tulo 8 Control de posición contiene aplicaciones
de algoritmos de control diseñados directamente por la técnica de moldeo de
energı́a, y además presenta una explicación cualitativa de esquemas clásicos
como lo son el control proporcional derivativo (PD) y el control proporcional
integral derivativo (PID); también se incluye nuevas estrategias de control
como el control tangente hiperbólico y arcto tangente.
La Parte IV finaliza con:
Referencias selectas
Problemas propuestos
Capı́tulo
8
Control de
posición
τ
= ∇Ua (Kp, q̃) − f v (Kv , q̇) + g(q)
8.1 Introducción
8.2 Control proporcional derivativo (PD)
8.3 Control proporcional integral derivativo (PID)
8.4 Control punto a punto
8.5 Resumen
Objetivos
Presentar aplicaciones de la técnica de moldeo de energı́a de robots
manipuladores.
Objetivos particulares:
Control proporcional derivativo.
Control proporcional integral derivativo.
Control tangente hiperbólico.
Control arcotangente.
Control punto a punto.
8.1
393
Introducción
8.1 Introducción
C
ontrol de robots manipuladores es un tema de control automático vigente
y de gran interés para la comunidad cientı́fica de robótica debido a los retos
teóricos y prácticos que involucra el diseño de nuevas estrategias de control con alto
desempeño y exactitud en aplicaciones industriales tales como estibado de cajas,
ensamble, traslado, pintado de objetos, etc. El diseño de nuevos esquemas de control
implica mejorar sustancialmente el desempeño de algoritmos de control tradicionales.
El problema más simple de control de robots manipuladores se denomina control
de posición o regulación el cual se refiere a ubicar o posicionar al extremo final del
robot en un punto constante en el tiempo al que se le denomina posición deseada o
set point. El objetivo de control es posicionar al extremo final del robot en el punto
deseado y que permanezca ahı́ de manera indefinida.
El problema de control de posición o regulación consiste en mover el
extremo final del robot manipulador hacia una posición deseada constante q d ;
independientemente de su posición inicial q(0). Los puntos intermedios entre la
posición inicial y la referencia deseada no son controlados como tal, forman parte
de la etapa transitoria de la respuesta del robot incluido en la señal de error q̃(t)
definida como la diferencia entre la posición deseada y la posición actual del robot
q̃(t) = qd − q(t). El punto final q d es la posición clave donde se realiza el control.
El objetivo del problema de control de posición está determinado por encontrar
una ley de control τ que proporcione los pares aplicados a las articulaciones o
servomotores del robot, de tal forma que la posición actual del robot q(t) y la
velocidad articular de movimiento q̇(t) tiendan asintóticamente hacia la posición
deseada q d y velocidad cero, respectivamente, sin importar las condiciones iniciales.
Es decir,
lı́m
t→∞
q(t)
q̇(t)
=
qd
0
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
(8.1)
Alfaomega
394
Capı́tulo 8: Control de posición
El objetivo de control se cumple cuando:
lı́m
t→∞
q̇(t)
q̃(t)
=
0
0
Figura 8.1 Control de posición
Nótese que en la figura 8.1 el robot se encuentra en su punto de equilibrio, lo que
significa que el objetivo de control (8.1) se cumplió sin depender de las condiciones
iniciales, entonces la posición deseada se alcanza, por lo que la posición del extremo
final del robot permanece constante (q(t) = qd ) y por lo tanto la velocidad de
movimiento es cero (q̇ = 0).
Evidentemente, para propósitos industriales, una vez que el extremo final del robot
ha alcanzado el punto deseado, en el siguiente periodo de muestreo el punto
deseado cambiará su valor, entonces el robot se moverá hacia al siguiente punto;
el anterior punto deseado representa la condición inicial y el extremo final del robot
se estará posicionando en la siguiente coordenada; ası́ sucesivamente el robot sigue
una curva compuesta de puntos cercanos entre sı́. Esta forma de controlar a un robot
se denomina control punto a punto. Para realizar esta aplicación es necesario que
el esquema de control forme parte de la ecuación en lazo cerrado y que genere un
punto de equilibrio asintóticamente estable, ya que no dependerı́a de las condiciones
iniciales. Es importante aclarar que en control punto a punto no se controla la
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
8.2
Control proporcional-derivativo (PD)
395
velocidad de movimiento como en el caso de control de trayectoria, donde el error
de posición y error de velocidad son controlados simultáneamente. La gran mayorı́a
de las aplicaciones actuales de la industria son realizadas a través de control punto
a punto como en los casos de traslado y pintado de objetos, estibado y pintura de
carrocerı́as, etcétera.
El desempeño de un algoritmo de control se refiere a realizar de manera correcta y
exacta la tarea programada al robot, lo que lo habilita a llevar a cabo diversas
aplicaciones de control punto a punto. Por lo tanto, el espectro de aplicaciones
comerciales, domésticas, cientı́ficas e industriales se incrementa.
A continuación se describen los algoritmos tradicionales de control de robots
manipuladores como lo son el esquema proporcional derivativo (PD) y el
regulador proporcional integral derivativo (PID). Posteriormente se presentan nuevas
estrategias de control usando la técnica de moldeo de energı́a.
8.2 Control proporcional-derivativo (PD)
E
l algoritmo de control proporcional-derivativo (PD) es el esquema de
controlador más simple y más popular que se puede usar en robots
manipuladores. En 1981, Takegaki y Arimoto demostraron que la ecuación en
lazo cerrado del modelo dinámico del robot manipulador y el control proporcional
derivativo tiene un punto de equilibrio global y asintóticamente estable.
El control proporcional derivativo más compensación de gravedad (PD) está dado
por la siguiente ecuación:
τ
= Kpq̃ − Kv q̇ + g(q)
(8.2)
donde q̃ ∈ IRn es el vector de error de posición que se define como la diferencia entre
la posición deseada q d ∈ IRn y la posición actual del robot q ∈ IRn ; Kp ∈ IRn×n es
una matriz definida positiva, Kv ∈ IRn×n es la ganancia derivativa la cual es una
matriz definida positiva.
La ecuación en lazo cerrado en variables de estado que definen el problema de control
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
396
Capı́tulo 8: Control de posición
de posición es:
−q̇
d q̃
=
dt q̇
M (q)−1 [ Kpq̃ − Kv q̇ − C(q, q̇)q̇ ]
(8.3)
la cual es una ecuación diferencial autónoma.
El diagrama a bloques del control proporcional derivativo con compensación de
gravedad se muestra en la figura 8.2.
Figura 8.2 Diagrama a bloques del control PD.
La posición articular del robot q se retroalimenta para generar la señal de
error de posición q̃ = q d − q. La velocidad de movimiento q̇ se emplea para
inyección de amortiguamiento. Obsérvese que el signo menos de la acción de control
derivativa sirve para contrarrestar la energı́a al control proporcional. El término
de compensación de gravedad g(q) es una componente esencial para la unicidad del
punto de equilibrio. Sin embargo, esta condición no es suficiente, también se requiere
que las ganancias proporcional y derivativa sean matrices definidas positivas.
Desde el punto de vista práctico el término g(q) se requiere en la implementación
del control PD, y se debe conocer como parte de la dinámica parcial del robot si el
movimiento es en un plano vertical o en general en su espacio tridimensional. Para
el caso en que el movimiento sea en un plano horizontal, la energı́a potencial es
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
8.2
397
Control proporcional-derivativo (PD)
constante y el gradiente de la energı́a potencial o par gravitacional g(q) =
cero.
∂U(q )
∂q
es
8.2.1
8.2.1. Control PD de un péndulo
La forma inmediata de entender el funcionamiento cualitativo del control PD es a
través del péndulo.
El modelo dinámico de un péndulo está dado por:
2
] q̈1 + bq̇1 + m1 glc1 sen(q1 )
τ1 = [ Ir1 + m1 lc1
mientras que el control proporcional derivativo es:
τ1 = kp1 q̃1 − kv1 q̇1 + m1 glc1 sen(q1 )
donde el término kp1 q̃1 corresponde al regulador proporcional; siendo el error de
posición q̃1 definido como la diferencia entre la posición deseada qd1 y la posición
actual del péndulo q1 (t). El nombre del esquema proporcional se debe al producto
de la ganancia kp1 y el error de posición q̃1 .
Por otro lado, el término −kv1 q̇1 se denomina acción de control derivativo y no
debe ser confundido con un esquema de control; este término se incluye con la
finalidad de mejorar la respuesta del control proporcional a través de la inyección de
amortiguamiento que se logra restando al control proporcional el producto entre la
ganancia derivativa kv1 y la velocidad articular q̇1 . En términos prácticos el efecto
de amortiguamiento significa freno mecánico para dosificar la energı́a aplicada al
servomotor τ1 y de esta forma lograr que la respuesta transitoria del péndulo no
tenga sobre impulsos y entre de manera suave al régimen estacionario.
Por lo tanto, la ecuación en lazo cerrado está determinada de la siguiente forma:
−q̇1
d q̃1
=
1
dt q̇1
2 [ kp1 q̃1 − kv1 q̇1 − b1 q̇1 ]
[ Ir1 + m1 lc1
]
se ha eliminado la fricción de Coulomb y estática con la finalidad que los programas
de simulación no demoren dicho proceso.
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
398
Capı́tulo 8: Control de posición
♣ Ejemplo 8.1
Controlar la posición del extremo final del péndulo en el siguiente punto deseado:
qd1 = 90 grados
Solución
La estructura matemática del algoritmo de control proporcional derivativo del
péndulo está dada por la siguiente expresión:
τ1 = kp1 q̃1 − kv1 + m1 glc1 sen(q1 )
donde kp1 es la ganancia proporcional; kv1 representa la ganancia derivativa, q̃1 es
el error de posición definido como la diferencia q̃1 = qd1 − q1 (t) entre la posición
deseada qd1 y la posición actual del robot q1 (t); τ1 es el par aplicado al servomotor
del péndulo.
El cuadro 8.1 contiene el modelo dinámico del péndulo cap8 pendulo.m; observe que
en la lı́nea 18 se utiliza el esquema de control proporcional derivativo [tau1, ~ ] =
cap8 pdpendulo(q1,qp1), el cual proporciona la energı́a τ1 necesaria para mover al
péndulo desde la posición inicial q(0) hacia la posición deseada qd1 = 90 grados.
El programa cap8 pdpendulo.m tiene el código fuente en MATLAB del control
proporcional derivativo, el cual se encuentra en el cuadro 8.2 y a través del programa
principal cap8 pdpendulosimu.m (ver cuadro 8.3) se puede llevar a cabo el proceso
de simulación.
En una primera fase de la simulación las ganancias del control (PD) se han
sintonizado con los siguientes valores: kp1 = 10 y kv1 =0.1; lo que significa que la
inyección de amortiguamiento del término derivativo es pobre, causa sobre impulsos
en la etapa transitoria y fluctuación abrupta de la respuesta, marcando oscilaciones
sucesivas y sostenidas en la etapa transitoria como se muestra en las gráficas del
error de posición y par aplicado de la figura 8.3. Note que en el espacio de trabajo
(cinemática directa) también la trayectoria del extremo final del péndulo tiene
oscilaciones; lo que se confirma por el diagrama fase (q̇1 vs q̃1 ) en que el atractor del
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
8.2
Control proporcional-derivativo (PD)
399
punto de equilibrio tiene un conjunto de espirales que van disminuyendo conforme
las señales q̇1 (t) y q̃( t) evoluciona el tiempo t.
Figura 8.3 Péndulo: control PD, kp1 = 10, kv1 = 0.1
La segunda fase de la simulación consiste en cambiar el valor de la ganancia
derivativa kv1 = 2 (manteniendo el valor de la ganancia proporcional), es decir
aumentar el efecto de amortiguamiento o freno mecánico para obtener una respuesta
más suave en régimen transitorio, y en estado estacionario libre de oscilaciones. La
figura muestra la respuesta del péndulo con la nueva sintonı́a de ganancia derivativa.
Figura 8.4 Péndulo: control PD, kp = 10, kv = 2
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
400
Capı́tulo 8: Control de posición
Código Fuente 8.1
cap8 pendulo.m
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 8 Control de posición.
cap8 pendulo.m
1
function xp=cap8 pendulo(t,x)
2
%modelo dinámico del péndulo
3
%no se incluye el fenómeno de fricción de Coulomb, ni estática
4
%variables de estados (entradas) ——————————————– %
5
%posición articular
q1=x(1);
6
7
%velocidad articular
qp1=x(2);
8
9
%parámetros del péndulo ————————————————– %
m1=5; %masa
10
11
14
lc1=0.01; %centro de gravedad
g=9.81; %constante de aceleración gravitacional
b1=0.17; %coeficiente de fricción viscosa
Ir1=0.16; %momento de inercia del rotor
15
Ip=Ir1+m1*lc1*lc1; %momento de inercia del péndulo
16
%en este punto se inserta el algoritmo de control
17
%par aplicado (controlador) ———————————————-%
[tau1, ˜ ] = cap8 pdpendulo(q1,qp1) ;
12
13
18
19
%aceleración articular del péndulo ———————————————%
qpp1=(tau1-b1*qp1-m1*g*lc1*sin(q1))/Ip;
20
21
%vector de salida ——————————————————– %
xp=[qp1; %xp(1)=x(2) velocidad articular
22
23
24
qpp1]; %xp(2)=qpp aceleración articular
end
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
8.2
Control proporcional-derivativo (PD)
Código Fuente 8.2
401
cap8 pdpendulo.m
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 8 Control de posición.
Esta función retorna:
error de posición q̃1
par aplicado τ1
cap8 pdpendulo.m
1
function [tau1, qt1] = cap8 pdpendulo(x,xp)
2
%variables de estados (entradas) ——————————————– %
3
%posición articular
4
q1=x;
5
6
%velocidad articular
qp1=xp;
7
%parámetros de la compensación de gravedad del péndulo
8
10
m1=5; %masa
lc1=0.01; %centro de gravedad
g=9.81; %constante de aceleración gravitacional
11
%ganancias del controlador ———————————————-%
12
kp1=10; %ganancia proporcional
kv1=2; %ganancia derivativa
9
13
14
15
%referencia (posición deseada en grados)
qd1=90;
16
%vector de pares de gravitacionales ————————————– %
17
par grav = m1*g*lc1*sin(q1);
18
19
%error de posición
qt1=pi*qd1/180-q1;
20
%control proporcional derivativo
21
tau1=kp1*qt1-kv1*qp1+par grav;
22
end
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
402
Capı́tulo 8: Control de posición
Código Fuente 8.3
cap8 pdpendulosimu.m
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 7 Identificación paramétrica.
cap8 pdpendulosimu.m
1
clc; clear all; close all;
2
format short g
3
%parámetros de simulación ————————————————%
ti=0; %tiempo inicial (segundos)
tf=5; %tiempo de final (segundos)
h=0.001; %incremento de tiempo (segundos)
4
5
6
7
8
9
ts=ti:h:tf; %vector de tiempo
ci=[0;0]; %condiciones iniciales
opciones=odeset(’RelTol’,1e-3,ÍnitialStep’,1e-3,’MaxStep’,1e-3);
10
%solución numérica del sistema ——————————————-%
11
[t,x]=ode45(’cap8 pendulo’,ts,ci,opciones);
q1=x(:,1);
qp1=x(:,2);
12
13
14
15
16
17
18
19
20
21
[n,m]=size(t);
tau1=zeros(n,1);
qt1=zeros(n,1);
xef=zeros(n,1);
yef=zeros(n,1);
for k=1:n
[tau1(k), qt1(k)] = cap8 pdpendulo(q1(k),qp1(k));
frq=Rz(-pi/2)*cinematica pendulo(0.45,q1(k),0.1);
22
xef(k)=frq(1);
yef(k)=frq(2);
23
24
end
25
subplot(2,2,1);
subplot(2,2,2);
subplot(2,2,3);
subplot(2,2,4);
26
27
28
Alfaomega
plot(t,qt1)
plot( t, tau1)
plot(xef,yef)
plot(qp1, qt1)
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
8.2
403
Control proporcional-derivativo (PD)
8.2.2
8.2.2. Control PD de un brazo robot de 2 gdl
El control proporcional derivativo (PD) para un robot en configuración antropomórfica de 2 gdl está dado por la siguiente ecuación:
τ1
kp1 0
q̃1
q̇1
kv1 0
=
−
+g(q)
0 kp2
0 kv2
τ2
q̃2
q̇2
τ
Kp
Kv
q̃
q̇
donde Kp , Kv ∈ IR2×2 son la ganancia proporcional y derivativa, respectivamente; la
velocidad articular es representada por q̇ = [ q̇1 , q̇2 ]T ; el error de posición q̃ se define
como la diferencia entre la posición deseada q d y la posición actual del robot q; g(q)
es la compensación de gravedad; τ1 , τ2 son los pares aplicados a las articulaciones
del hombro y codo, respectivamente.
♣ ♣ Ejemplo 8.2
Controlar la posición del extremo final de un robot de 2 gdl en configuración
antropomórfica en el siguiente punto deseado: [ qd1 , qd2 ]T = [ 90, 45 ] grados.
Solución
El cuadro 8.8 contiene el modelo dinámico del brazo robot de 2 gdl (programa
cap8 robot2gdl.m); en referencia a este programa es muy importante aclarar que
la ecuación en lazo cerrado está formada por la dinámica no lineal del robot y el
esquema de control (ver lı́nea 18). Por lo tanto, en la lı́nea 22 es el lugar adecuado
para insertar el algoritmo de control, en este caso es el proporcional derivativo cuyo
código MATLAB se presenta en el programa cap8 PDrobot2gdl.m (ver cuadro 8.8).
Las ganancias proporcional Kp y derivativa Kv son sintonizadas de tal forma que
no produzcan sobre impulsos, ni oscilaciones en el régimen transitorio, ası́ como una
respuesta suave en estado estacionario.
El valor de la sintonı́a de las ganancias proporcional para las articulaciones del
hombro y codo, respectivamente son: kp1 =3 Nm/grados y kp2 =0.15 Nm/grado;
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
404
Capı́tulo 8: Control de posición
mientras que las ganancias derivativas has sido seleccionadas con un factor de
amortiguamiento del 20 % del valor de las ganancias proporcional: kv1 = 0.20kp1
Nm-seg/grados y kv2 = 0.20kp2 Nm-seg/grados.
La figura 8.5 muestra un perfil suave de los errores de posición, sin sobre impulsos, no
hay oscilaciones pronunciadas, teniendo una respuesta suave; ambos componentes
de error de posición tienden asintóticamente a cero como el tiempo evoluciona a
infinito (estabilidad asintótica).
Las señales de pares aplicados τ1 y τ2 se aplican directamente a las articulaciones del
hombro y codo, respectivamente; el perfil de estas señales muestran de igual forma
un comportamiento suave, sin oscilaciones que hace que el robot no vibre tal como
se aprecia en la trayectoria que describe el extremo final del robot en su espacio
de trabajo, la cual inicia desde la posición de casa ubicada en el eje y− hasta el
punto deseado. Observe también que en el diagrama fase del robot las velocidades
articulares como las señales de error de posición convergen asintóticamente al punto
de equilibrio. Este tipo de respuesta es consecuencia de una adecuada sintonı́a de las
ganancias, ası́ como del factor de amortiguamiento, que en este caso es del 20 %. La
sintonı́a de las ganancias es empı́rica, es decir a prueba y error; este proceso depende
en gran medida de la experiencia que tenga el usuario.
Figura 8.5 Robot de 2 gdl: control PD, kp = 10, kv = 0.1
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
8.2
405
Control proporcional-derivativo (PD)
Código Fuente 8.8
cap8 robot2gdl.m
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 8 Control de posición.
cap8 robot2gdl.m
1
function xp = cap8 robot2gdl(t,x)
2
3
%vector de posición articular
q1=x(1); q2=x(2); q = [q1; q2];
4
%vector de velocidad articular
5
qp1=x(3); qp2=x(4); qp = [qp1; qp2];
m1=23.902; l1=0.45; lc1=0.091; I1=1.266; b1=2.288;
m2=3.880; l2=0.45; lc2=0.048; I2=0.093; b2=0.175; g=9.81;
6
7
8
theta1=m1*lc1*lc1+m2*l1*l1+m2*lc2*lc2+I1+I2;
theta2=l1*m2*lc2;
theta3=m2*lc2*lc2+I2;
theta4=g*(lc1*m1+m2*l1);
9
10
11
12
theta5=g*m2*lc2; theta6=b1; theta7=b2;
M=[theta1+2*theta2*cos(q2), theta3+theta2*cos(q2);
theta3+theta2*cos(q2), theta3];
C=[ -2*theta2*sin(q2)*qp2, -theta2*sin(q2)*qp2;
13
14
15
16
theta2*sin(q2)*qp1, 0];
gq11=theta4*sin(q1)+theta5*sin(q1+q2);
gq21=theta5*sin(q1+q2);
gq=[gq11; gq21];
17
18
19
20
22
fr=[theta6*qp1;
theta7*qp2];
[ ˜ , tau ]=cap8 PDrobot2gdl(q,qp); %en esta lı́nea se inserta el controlador
23
qpp = M∧ (-1)*(tau-C*qp-gq-fr);
24
%vector de salida
xp = [qp1; qp2; qpp(1); qpp(2)];
21
25
26
end
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
406
Capı́tulo 8: Control de posición
Código Fuente 8.8
cap8 PDrobot2gdl.m
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 8 Control de posición.
cap8 PDrobot2gdl.m
1
2
3
function [qtilde tau] = cap8 PDrobot2gdl(x,xp)
q1=x(1); q2=x(2); q = [q1; q2]; %vector de posiciones
qp1=xp(1); qp2=xp(2); qp = [qp1; qp2]; %vector de velocidades
4
%parámetros del robot: par gravitacional
5
m1=23.902; lc1=0.091; l1=0.45;
m2=3.880; lc2=0.048; g=9.81;
theta4=g*(lc1*m1+m2*l1);
theta5=g*m2*lc2;
6
7
8
9
11
%par gravitacional
par grav = [theta4*sin(q1)+theta5*sin(q1+q2);
g*m2*lc2];
12
%ganancia proporcional
13
15
kp1=3; kp2=0.15;
Kp=[kp1, 0;
0, kp2]; %ganancia proporcional
16
%ganancia derivativa
17
kv1=0.20*kp1; kv2=0.20*kp2;
Kv=[kv1, 0;
0, kv2]; %ganancia derivativa
qd1=45; qd2=90;
10
14
18
19
20
21
qd=[qd1;
qd2]; %vector de referencias
qtilde=pi/180*qd-q;
qtgrados=(180/pi)*qtilde; %error de posición en grados
22
23
24
25
qpgrados=180*qp/pi; %velocidad en grados/segundo.
tau=Kp*qtgrados-Kv*qpgrados+par grav;
26
27
end
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
8.2
Control proporcional-derivativo (PD)
Código Fuente 8.6
407
cap8 robot2gdlsimu.m
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 8 Control de posición.
cap8 robot2gdlsimu.m
1
clc; clear all; close all;
2
format short g
ti=0; h=0.001; tf = 5; t=ti:h:tf; %vector tiempo
ci=[0; 0; 0; 0]; %condiciones iniciales
opciones=odeset(’RelTol’,1e-3,’InitialStep’, 1e-3,’MaxStep’,1e-3);
3
4
5
6
7
8
9
10
11
12
13
14
15
16
%solución numérica del sistema dinámico lineal
[t,x]=ode45(’cap8 robot2gdl’,t,ci,opciones);
q1=x(:,1); q2=x(:,2);
qp1=x(:,3); qp2=x(:,4);
[n,m]=size(t);
tau1=zeros(n,1); tau2=zeros(n,1);
qtilde1=zeros(n,1); qtilde2=zeros(n,1);
xef=zeros(n,1); yef=zeros(n,1);
beta1=0.15; beta2=0.15; l1=0.45; l2=0.45;
for k=1:n
[qt tau] = cap8 PDrobot2gdl( [q1(k);q2(k)], [qp1(k); qp2(k)]);
17
tau1(k)=tau(1); tau2(k)=tau(2);
qtilde1(k)=qt(1); qtilde2(k)=qt(2);
[xef(k), yef(k),˜ ]=cinematica r2gdl( beta1, l1, q1(k), beta2, l2, q2(k));
p=Rz(-pi/2)*[xef(k), yef(k), beta1+beta2]’;
18
19
20
21
xef(k)=p(1);
yef(k)=p(2);
22
23
end
24
subplot(2,2,1); plot(t,qtilde1,t,qtilde2)
subplot(2,2,2); plot( t, tau1, t,tau2)
subplot(2,2,3); plot(xef,yef)
subplot(2,2,4); plot(qtilde1, qp1, qtilde2,qp2)
25
26
27
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
408
Capı́tulo 8: Control de posición
8.2.3
8.2.3. Control PD de un brazo robot de 3 gdl
El algoritmo de
está dado por:
⎡ ⎤
τ1
⎢ ⎥
⎣ τ2 ⎦ =
τ3
τ
control proporcional derivativo para un brazo robot de 3 gdl
⎡
kp1
⎢
⎣ 0
o
0
kp2
0
Kp
0
⎤⎡
q̃1
⎤
⎡
kv1
⎥⎢ ⎥ ⎢
0 ⎦ ⎣ q̃2 ⎦ − ⎣ 0
q̃3
kp3
0
q̃
0
kv2
0
Kv
0
⎤⎡
q̇1
⎤
⎥⎢ ⎥
0 ⎦ ⎣ q̇2 ⎦ +g(q)
q̇3
kv3
q̇
donde Kp , Kv ∈ IR3×3 son la ganancia proporcional y derivativa, respectivamente;
el error de posición q̃ ∈ IR3 se define como la diferencia entre la posición deseada
qd ∈ IR3 y la posición actual del robot q ∈ IR3 .
Por conveniencia se seleccionan las ganancias proporcional y derivativa como
matrices diagonales definidas positivas con la finalidad de desacoplar la estructura
de control; de esta forma la sintonı́a de la i-ésima ganancia sólo depende de la iésima articulación. Es decir, el par aplicado a la i-ésima articulación depende de:
τi = τ (kpi, q̃i, q̇i ).
♣ Ejemplo 8.3
Controlar la posición del extremo final de un robot de 3 gdl en configuración
antropomórfica en las siguientes coordenadas: [ qd1 , qd2 , qd3 ]T = [ 30, 45, 90 ] grados
Solución
El modelo dinámico del robot antropomórfico de 3 gdl se encuentra en el programa
cap8 robot3gdl.m como se describe en el cuadro 8.7. De particular interés
representa la lı́nea de código 23, debido a que en este lugar se inserta el algoritmo de
control que envı́a la señal de par aplicado para que el robot se mueva a la posición
indicada.
El esquema de control proporcional derivativo se encuentra implementado en el
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
8.2
409
Control proporcional-derivativo (PD)
programa cap8 PDrobot3gdl.m del cuadro 8.8. La sintonı́a de las ganancias han sido
pensadas en la capacidad de par aplicado que puedan suministrar las articulaciones
del robot; es decir, la idea es no saturar a los servoamplificadores de tal forma que
se encuentren trabajando en la parte lineal, lejos de los lı́mites de saturación.
Supóngase que se cuenta con un prototipo de un robot manipulador de 3 gdl,
cuyos servomotores corresponden a las siguientes caracterı́sticas: 50 Nm, 150 Nm
y 15 Nm para las articulaciones de la base, hombro y codo, respectivamente. El
τ max
criterio para sintonizar las ganancias es el siguiente: k ≤ 0.80 i
, donde τ max
pi
qdi
i
corresponde al par máximo de la i-ésima articulación. En la lı́nea 18 del programa
cap8 PDrobot3gdl.m se encuentra la sintonı́a final de la ganancia proporcional; el
factor de amortiguamiento de la ganancia derivativa se ha seleccionado en el 20 %
de la ganancia proporcional.
El código fuente que contiene la simulación del control proporcional derivativo del
robot manipulador en configuración antropomórfica se encuentra en el programa
cap8 robot3gdlsimu.m (cuadro 8.9). La figura 8.6 muestra el comportamiento de
los errores de posición para las tres articulaciones; presentan un perfil suave, sin
ruido mecánico, ni sobre impulsos, tampoco se observan oscilaciones en régimen
estacionario. Los pares aplicados evolucionan dentro de los lı́mites de saturación y
la trayectoria que describe el extremo final del robot en su espacio de trabajo es
suave, tal y como se muestra en el diagrama fase.
Figura 8.6 Control PD: robot cartesiano de 3 gdl.
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
410
Capı́tulo 8: Control de posición
Código Fuente 8.7
cap8 robot3gdl.m
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 8 Control de posición.
cap8 robot3gdl.m
1
function xp = cap8 robot3gdl(t,x)
2
q1=x(1); q2=x(2); q3=x(3); q = [q1; q2; q3]; %vector de posición articular
3
4
5
qp1=x(4); qp2=x(5); qp3=x(6); qp = [qp1; qp2; qp3]; %vector de velocidad articular
Iz1=1.26; Iz2=0.084; Iz3=0.056; Iy1=0.089; Iy2=0.003; Iy3=0.0012; Ix1=0.03; Ix2=0.05;
Ix3=0.009; m1=26.902; l1=0.45; b1=2.288; m2=30; l2=0.45; lc2=0.038; b2=0.2; m3=3.880;
6
l3=0.45; lc3=0.048; b3=0.175; g=9.81;
m11=Iy2*sin(q2)*sin(q2)+ Iy3*sin(q2+q3)*sin(q2+q3)+ Iz1+Iz2*cos(q2)*cos(q2)+
Iz3*cos(q2+q3)*cos(q2+q3)+ m2*lc2*lc2*cos(q2)*cos(q2)+ m3*(l2*cos(q2)+
lc3*cos(q2+q3))*(l2*cos(q2)+lc3*cos(q2+q3));
m12=0; m13=0; m21=0;
m22=Ix2+ Ix3+m3*l2*l2+ m2*lc2*lc2+m3*lc3*lc3+ 2*m3*l2*lc3*cos(q3);
m23=Ix3+m3*lc3*lc3+m3*l2*lc3*cos(q3); m31=0;
7
8
9
10
11
12
m32=Ix3+m3*lc3*lc3+m3*l2*lc3*cos(q3); m33=Ix3+m3*lc3*lc3;
M=[m11, m12, m13; m21, m22, m23 ;m31, m32, m33]; %matriz de inercia
gamma112=(Iy2-Ix2-m2*lc2*lc2)*cos(q2)*sin(q2)+(Iy3-Iz3)*cos(q2+q3)*sin(q2+q3)m3*(l2*cos(q2)+lc3*cos(q2+q3))*(l2*sin(q2)+ lc3*sin(q2+q3));
gamma113=(Iy3-Iz3)*cos(q2+q3)*sin(q2+q3)- m3*lc3*sin(q2+q3)*(l2*cos(q2)+lc3*cos(q2+q3));
gamma121=(Iy2-Iz2-m2*lc2*lc2)*cos(q2)*sin(q2)+(Iy3-Iz3)*cos(q2+q3)*sin(q2+q3)m3*(l2*cos(q2)+lc3*cos(q2+q3))*(l2*sin(q2)+lc3*sin(q2+q3));
gamma131=(Iy3-Iz3)*cos(q2+q3)*sin(q2+q3) -m3*lc3*sin(q2+q3)*(l2*cos(q2)+ lc3*cos(q2+q3));
gamma211=(Ix2-Iy2+m2*lc2*lc2)*cos(q2)*sin(q2)+(Iz3-Iy3)*cos(q2+q3)*sin(q2+q3)+
m3*(l2*cos(q2)+lc3*cos(q2+q3))*(l2*sin(q2)+lc3*sin(q2+q3));
gamma223=-l2*m3*lc3*sin(q3); gamma232=-l2*m3*lc3*sin(q3);
gamma233=-l2*m3*lc3*sin(q3); gamma311=(Iz3-Iy3)*cos(q2+q3)*sin(q2+q3)+
m3*lc3*sin(q2+q3)*(l2*cos(q2)+lc3*cos(q2+q3));
gamma322=l2*m3*lc3*sin(q3); c11=gamma112*qp2+gamma113*qp3;
c12=gamma121*qp1; c13=gamma131*qp1; c21=gamma211*qp1; c22=gamma223*qp3;
c23=gamma232*qp2+gamma233*qp3; c31=gamma311*qp1; c32=gamma322*qp2; c33=0;
13
14
15
16
17
18
19
20
21
22
23
24
C=[c11, c12, c13; c21, c22, c23; c31, c32, c33];
gq11=0; gq21=(lc2*m1++m2*l2)*sin(q1)+ m2*lc3*sin(q1+q2);
gq31=m2*lc3*sin(q1+q2);
25
26
27
gq=g*[gq11; gq21; gq31];
fr=[b1*qp1; b2*qp2; b3*qp3];
[˜ , tau] = cap8 PDrobot3gdl(q,qp);
28
29
qpp = inv(M)*(tau-C*qp-gq-fr);
xp = [qp1; qp2; qp3; qpp(1); qpp(2); qpp(3)];%vector de salida
30
end
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
8.2
Control proporcional-derivativo (PD)
Código Fuente 8.8
411
cap8 PDrobot3gdl.m
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 8 Control de posición.
cap8 PDrobot3gdl.m
1
2
3
4
function [qtildeg tau] = cap8 PDrobot3gdl(x,xp)
q1=x(1); q2=x(2); q3=x(3);
q = [q1; q2; q3]; %posiciones
qp1=xp(1); qp2=xp(2); qp3=xp(3);
5
qp = [qp1; qp2; qp3]; %velocidades
6
%parámetros del robot: par gravitacional
m1=26.902; l1=0.45; m2=30; l2=0.45; lc2=0.038;
m3=3.880; l3=0.45; lc3=0.048; g=9.81;
7
8
9
gq11=0;
gq21=(lc2*m1++m2*l2)*sin(q1)+m2*lc3*sin(q1+q2);
gq31=m2*lc3*sin(q1+q2);
10
11
12
15
gq=g*[gq11; gq21; gq31]; %par gravitacional
qd=[30; 45; 90]; %referencias o posiciones deseadas
qtilde=qd*pi/180-q; %error de posición q̃
qtildeg=(180/pi)*qtilde; %error de posición q̃ en grados
16
qpgrados=180*qp/pi; %velocidad q̇ en grados/segundo.
17
%ganancia proporcional
Kp=[1, 0,0 ;
0, 2,0;
13
14
18
19
20
0,0,0.15];
21
23
%ganancia derivativa
Kv=[0.3, 0,0;
0, 0.6,0;
24
0,0,0.015];
22
25
% control proporcional derivativo PD
tau=Kp*qtildeg-Kv*qpgrados+gq;
26
27
end
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
412
Capı́tulo 8: Control de posición
Código Fuente 8.9
cap8 robot3gdlsimu.m
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 8 Control de posición.
cap8 robot3gdlsimu.m
1
3
clc; clear all;
close all;
format short
4
%parámetros de simulación:
5
ti=0; h=0.0025; tf = 5; ts=ti:h:tf; %vector tiempo
opciones=odeset(’RelTol’,1e-3,’InitialStep’, 2.5e-3,’MaxStep’,2.5e-3);
ci=[0; 0; 0; 0;0;0];
[t,x]=ode45(’cap8 robot3gdl’,ts,ci,opciones);
2
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
q1=x(:,1); q2=x(:,2); q3=x(:,3); %posiciones articulares
qp1=x(:,4); qp2=x(:,5); qp3=x(:,6); %velocidades articulares
[n,m]=size(t);
tau1=zeros(n,1); tau2=zeros(n,1); tau3=zeros(n,1);
qtilde1=zeros(n,1); qtilde2=zeros(n,1); qtilde3=zeros(n,1);
xef=zeros(n,1); yef=zeros(n,1); zef=zeros(n,1);
beta1=0.15; beta2=0.15; beta3=0.15; l1=1; l2=0.45; l3=0.45;
for k=1:n
[qt tau] = cap8 PDrobot3gdl([q1(k);q2(k)q3(k)],[qp1(k);qp2(k); qp3(k)]);
tau1(k)=tau(1); tau2(k)=tau(2); tau3(k)=tau(3);
qtilde1(k)=qt(1); qtilde2(k)=qt(2); qtilde3(k)=qt(3);
[xef(k), yef(k),zef(k)]=cinematica r3gdl (beta1, l1, q1(k), beta2,
l2,q2(k),beta3,l3,q3(k));
end
subplot(2,2,1); plot(t,qtilde1,t,qtilde2, t,qtilde3)
24
subplot(2,2,2); plot( t, tau1, t,tau2, t,tau3)
subplot(2,2,3); plot3(xef,yef,zef)
25
subplot(2,2,4); plot(qtilde1, qp1, qtilde2,qp2, qtilde3,qp3)
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
8.2
413
Control proporcional-derivativo (PD)
8.2.4
8.2.4. Control PD de un robot cartesiano de 3 gdl
El algoritmo de control proporcional derivativo para el robot manipulador en
configuración cartesiana está dado por la siguiente estructura:
⎡ ⎤
⎤⎡ ˜ ⎤ ⎡
⎤⎡ ˙ ⎤
⎡
⎡
⎤
τ1
0
0
d1
d1
kv1 0
m1 + m2 + m3
kp1 0
⎢ ⎥
⎥⎢ ⎥ ⎢
⎥⎢ ⎥
⎢
⎢
⎥
0
⎣ τ2 ⎦ = ⎣ 0 kp2 0 ⎦ ⎣ d˜2 ⎦ − ⎣ 0 kv2 0 ⎦ ⎣ d˙2 ⎦ +g ⎣
⎦
˜
˙
τ3
d3
d3
o
0 kp3
0
0 kv3
0
τ
Kp
Kv
d̃
ḋ
donde Kp , Kv ∈ IR3×3 son la ganancia proporcional y derivativa, respectivamente;
el error de posición d̃ ∈ IR3 se define como la diferencia entre la posición deseada
dd ∈ IR3 y la posición actual del robot d ∈ IR3 .
♣ Ejemplo 8.4
Controlar la posición del extremo final de un robot de 3 gdl en configuración
cartesiana en la siguiente ubicación: [ dd1 , dd2, dd3 ]T = [ 0.5, 0.4, 0.2 ] m
Solución
El modelo dinámico del robot cartesiano se encuentra en el cuadro 8.10 que describe el programa
cap8 cartesiano3gdl.m; en la lı́nea
23 de este programa se inserta el algoritmo de control PD definido por
la función cap8 PDcartesiano3gdl.m
(ver cuadro 8.11). El programa principal para realizar la simulación del
control PD en el robot cartesiano de
3 gdl se encuentra en el cuadro 8.12
que describe el contenido del programa cap8 cartesiano3gdlsimu.m; los
resultados se encuentran en la figura
8.7.
Figura 8.7 Control PD.
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
414
Capı́tulo 8: Control de posición
Código Fuente 8.10
cap8 cartesiano3gdl.m
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 8 Control de posición.
cap8 cartesiano3gdl.m
1
function xp = cap8 cartesiano3gdl(t,x)
2
%vector de posición articular
d = [x(1);
3
4
x(2);
x(3)];
5
6
%vector de velocidad articular
dp =[x(4);
7
8
x(5);
x(6)];
m1=0.7; m2=0.28; m3=0.28; g=9.81;
b1=0.02; b2=0.08; b3=.02;
9
10
11
12
14
B=[b1, 0, 0;
0, b2,0;
0, 0, b3];
15
%modelo dinámico del robot
16
M = [m1+m2+m3, 0, 0; % Matriz de inercia
0 m1+m2, 0;
0 0 m3];
par grav = g*[m1+m2+m3; % vector de pares de gravitacionales
13
17
18
19
20
0;
0];
fr= B*dp;
[˜ , tau] = cap8 PDcartesiano3gdl(d,dp);
21
22
23
24
dpp = inv(M)*(tau- par grav-fr); %vector de aceleración articular
xp = [dp(1); dp(2); dp(3); dpp(1); dpp(2); dpp(3)]; %vector de salida
25
26
end
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
8.2
Control proporcional-derivativo (PD)
Código Fuente 8.11
415
cap8 PDcartesiano3gdl.m
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 8 Control de posición.
cap8 PDcartesiano3gdl.m
1
2
3
4
function [dtilde tau] = cap8 PDcartesiano3gdl (d,dp)
d1=d(1);
d2=d(2);
d3=d(3);
5
7
dp1=dp(1);
dp2=dp(2);
dp3=dp(3);
8
%parámetros del robot: par gravitacional
9
m1=0.7; m2=0.28; m3=0.28; g=9.81;
par grav = g*[m1+m2+m3;
0;
0];
6
10
11
12
13
dd=[0.5;
0.4;
0.2];
dtilde=dd-[d1; d2;d3];
14
15
16
17
kp1=3; kp2=2.5;kp3=1.7;
kv1=3; kv2=2.5; kv3=1.4;
Kp=[kp1, 0,0;
0, kp2,0;
18
19
20
21
23
0,0,kp3];
Kv=[kv1, 0,0;
0, kv2,0;
24
0,0,kv3];
22
25
%Control proporcional derivativo
tau=Kp*dtilde-Kv*dp+par grav;
26
27
end
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
416
Capı́tulo 8: Control de posición
Código Fuente 8.12
cap8 cartesiano3gdlsimu.m
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 8 Control de posición.
cap8 cartesiano3gdlsimu.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
clc;
clear all;
close all;
format short
%parámetros de simulación:
ti=0; h=0.001; tf = 10; ts=ti:h:tf; %vector tiempo
opciones=odeset(’RelTol’,1e-3,’InitialStep’,1e-3, ’MaxStep’,1e-3);
ci=[0; 0; 0; 0; 0; 0];
[t,x]=ode45(’cap8 cartesiano3gdl’,ts,ci,opciones);
d1=x(:,1); d2=x(:,2); d3=x(:,3);
dp1=x(:,4); dp2=x(:,5); dp3=x(:,6);
[n,m]=size(t);
tau1=zeros(n,1); tau2=zeros(n,1); tau3=zeros(n,1);
dtilde1=zeros(n,1); dtilde2=zeros(n,1); dtilde3=zeros(n,1);
xef=zeros(n,1); yef=zeros(n,1); zef=zeros(n,1);
for k=1:n
[dt tau] = cap8 PDcartesiano3gdl([d1(k); d2(k); d3(k)],[dp1(k);dp2(k);
dp3(k)]);
tau1(k)=tau(1); tau2(k)=tau(2); tau3(k)=tau(3);
19
dtilde1(k)=dt(1); dtilde2(k)=dt(2); dtilde3(k)=dt(3);
[xef(k), yef(k),zef(k)]=cinematica cartesiano (d3(k), d2(k), d1(k));
20
21
end
22
subplot(2,2,1); plot(t,dtilde1,t,dtilde2, t,dtilde3)
subplot(2,2,2); plot( t, tau1, t,tau2, t,tau3)
subplot(2,2,3); plot3(xef,yef,zef)
subplot(2,2,4); plot(dtilde1, dp1, dtilde2,dp2, dtilde3,dp3)
23
24
25
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
8.3
417
Control PID
8.3 Control PID
E
l esquema de control proporcional integral derivativo (PID) tiene la siguiente
estructura:
τ
= Kp q̃ − Kv q̇ + Ki
t
q̃(μ)dμ + g(q)
(8.4)
0
donde Kp, Kv ∈ IRn×n son matrices definidas positivas y se les denomina ganancias
proporcional y derivativa, respectivamente. Ki ∈ IRn×n es una matriz definida
positiva denominada ganancia integral. Todas las ganancias Kp, Kv y Ki son
matrices definidas positivas y generalmente son diagonales.
La acción de control PID introduce una nueva variable de estado ε, la cual se
encuentra relacionada con la derivada temporal del error de posición ε̇ = q̃. Por
t
lo tanto, ε = 0 q̃dt + ε(0), con ε(0) ∈ IRn es un vector constante. La ecuación en
lazo cerrado en términos de las variables de estado [ ε, q̃, q̇ ]T adquiere la siguiente
forma:
⎤
⎡ ⎤
⎡
ε
q̃
d ⎢˙⎥
⎥
⎢
−q̇
⎦
⎣ q̃ ⎦ = ⎣
dt
−1
q̇
M (q) [ Kp q̃ − Kv q̇ + Kiε − C(q, q̇)q̇ − B q̇ ]
(8.5)
El algoritmo de control PID no representa una nueva estructura de control; es una
versión modificada del control PD más la acción de control integral. El algoritmo
de control PD tiene la desventaja que en régimen estacionario presenta un error
permanente de posición denominado offset; para disminuir este error se incorpora
en la estructura matemática del PD un término más que se denomina acción de
control integral. El proceso de integración es la suma del área bajo la curva del error
de posición, por lo tanto acumula energı́a la cual canalizada adecuadamente puede
disminuir la magnitud del error en régimen estacionario.
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
418
Capı́tulo 8: Control de posición
8.3.1
8.3.1. Control PID de un robot de 2 gdl
La ecuación del esquema de control proporcional derivativo para un brazo robot de
2 gdl está dado por:
t
τ1
kp1 0
q̃1
ki1 0
0 q̃1 dt
t
=
+
+
0 kp2
0 ki2
τ2
q̃2
0 q̃2 dt
g(lc1m1 + m2 l1 ) sen(q1 ) + gm2 lc2 sen(q1 + q2 )
gm2 lc2 sen(q1 + q2 )
♣ ♣ Ejemplo 8.5
Controlar la posición del extremo final de un robot de 2 gdl en configuración
antropórfica en la siguiente ubicación: [ qd1 , qd2 ]T = [ 45, 90 ] m
Solución
El programa cap8 PIDrobot2gdl.m tiene implementado el modelo dinámico del
robot de 2 gdl (cuadro 8.13). La figura 8.8 muestra los resultados del algoritmo
de control PID sobre el brazo robot de 2 gdl.
Figura 8.8 Control PID de un robot de 2 gdl.
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
8.3
419
Control PID
El esquema de control PID se encuentra en el programa cap8 PIDrobot2gdl.m que
se muestra en el cuadro 8.13. La regla de sintonı́a de la ganancia derivativa e integral
se ha fijado respectivamente en 20 % y 10 % de la ganancia proporcional. La integral
del error introduce una nueva variable de estado (varepsilon) cuya integración
numérica se realiza en el modelo dinámico del robot.
Código Fuente 8.13
cap8 PIDrobot2gdl.m
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 8 Control de posición.
cap8 PIDrobot2gdl.m
1
2
function [qtilde tau] = cap8 PIDrobot2gdl(x, xp, varepsilon)
q = [x(1); x(2)]; %posiciones
3
qp = [xp(1); xp(2)]; %velocidades
kp1=2.666; kp2=0.133;
Kp=[kp1, 0; 0, kp2]; %ganancia proporcional
4
5
6
ki1=0.1*kp1;
ki2=0.1*kp2;
Ki=[ki1, 0; 0, ki2]; %ganancia integral
kv1=0.2*kp1;
7
8
9
10
kv2=0.2*kp2;
Kv=[kv1, 0; 0, kv2]; %ganancia derivativa
11
12
13
%referencias
qd1=45; qd2=90; qd=[qd1; qd2]; %vector de posiciones deseadas
14
%vector de pares de gravitacionales
15
17
par grav = [38.46*sin(q(1))+1.82*sin(q(1)+q(2));
1.82*sin(q(1)+q(2)) ];
qtilde=[pi*qd(1)/180-q(1); pi*qd(2)/180-q(2)]; %error de posición en grados
18
qtgrados=(180/pi)*[qtilde(1) ;qtilde(2)];
19
%velocidad en grados/segundo. qpgrados=180*qp/pi;
tau=Kp*qtgrados+Ki*varepsilon-Kv*qpgrados+par grav;
16
20
21
end
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
420
Capı́tulo 8: Control de posición
Código Fuente 8.14
cap8 robot2gdlPID.m
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 8 Control de posición.
cap8 robot2gdlPID.m
1
function xp = cap8 robot2gdlPID(t,x)
2
%vector de posición articular
q1=x(1); q2=x(2); q = [q1; q2];
3
4
%vector de velocidad articular
qp1=x(3); qp2=x(4); qp = [qp1; qp2];
5
6
varepsilon = [x(5); x(6)]; %integral de qtilde
m1=23.902; l1=0.45; lc1=0.091; I1=1.266; b1=2.288;
m2=3.880; l2=0.45; lc2=0.048; I2=0.093; b2=0.175; g=9.81;
7
8
9
theta1=m1*lc1*lc1+m2*l1*l1+m2*lc2*lc2+I1+I2;
theta2=l1*m2*lc2;
theta3=m2*lc2*lc2+I2;
theta4=g*(lc1*m1+m2*l1);
10
11
12
13
theta5=g*m2*lc2; theta6=b1; theta7=b2;
M=[theta1+2*theta2*cos(q2), theta3+theta2*cos(q2);
theta3+theta2*cos(q2), theta3];
C=[ -2*theta2*sin(q2)*qp2, -theta2*sin(q2)*qp2;
14
15
16
17
20
theta2*sin(q2)*qp1, 0];
gq11=theta4*sin(q1)+theta5*sin(q1+q2);
gq21=theta5*sin(q1+q2);
gq=[gq11; gq21];
21
fr=[theta6*qp1; theta7*qp2];
22
%ley de control
[qtilde tau] = cap8 PIDrobot2gdl(q,qp,varepsilon);
varepsilonp=qtilde;
18
19
23
24
qpp = M∧ (-1)*(tau-C*qp-gq-fr);
xp = [qp1; qp2; qpp(1); qpp(2);varepsilonp(1); varepsilonp(2)];
25
26
27
end
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
8.3
421
Control PID
El programa cap8 robot2gdlPIDsimu.m que se presenta en el cuadro 8.15 permite
realizar la simulación del control PID con el robot de 2 gdl
Código Fuente 8.15
cap8 robot2gdlPIDsimu.m
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 8 Control de posición.
cap8 robot2gdlPIDsimu.m
1 clc;
2 clear all;
3 close all;
4 format short g
5
6
7
8
ti=0; h=0.001; tf = 5; t=ti:h:tf; %vector tiempo
ci=[0; 0; 0; 0;0;0]; %condiciones iniciales
opciones=odeset(’RelTol’,1e-3,’InitialStep’, 1e-3, ’MaxStep’,1e-3);
[t,x]=ode45(’cap8 robot2gdlPID’,t,ci,opciones);
9 q1=x(:,1); q2=x(:,2);
10 qp1=x(:,3); qp2=x(:,4);
11 int1=x(:,3); int2=x(:,4);
12
13
14
15
[n,m]=size(t);
tau1=zeros(n,1); tau2=zeros(n,1);
qtilde1=zeros(n,1); qtilde2=zeros(n,1);
xef=zeros(n,1); yef=zeros(n,1);
16 beta1=0.15; beta2=0.15; l1=0.45; l2=0.45;
17 for k=1:n
18
[qt tau] = cap8 PIDrobot2gdl([q1(k); q2(k)], [qp1(k); qp2(k)],[int1(k); int2(k)]);
19
20
21
tau1(k)=tau(1); tau2(k)=tau(2);
qtilde1(k)=qt(1); qtilde2(k)=qt(2);
[xef(k), yef(k),˜ ]=cinematica r2gdl(beta1, l1, q1(k), beta2,l2,q2(k));
22
23
24
p=Rz(-pi/2)*[xef(k), yef(k), beta1+beta2]’;
xef(k)=p(1);
yef(k)=p(2);
25
26
27
28
end
subplot(3,3,1); plot(t,qtilde1,t,qtilde2)
subplot(3,3,2); plot( t, tau1, t,tau2)
subplot(3,3,3); plot( t,int1,t,int2)
29 subplot(3,3,4); plot(xef,yef)
30 subplot(3,3,5); plot(qtilde1, qp1, qtilde2,qp2)
31 subplot(3,3,6); plot(t, qp1, t,qp2)
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
422
Capı́tulo 8: Control de posición
8.4 Control punto a punto
U
na aplicación inmediata de control de posición es el que se denomina control
punto a punto, consiste en mover el extremo final del robot para cada una de
las posiciones desdeas q di , con i = 1, 2, · · ·, n. De esta forma es posible parametrizar
lı́neas, cı́rculos, elipses, figuras y superficies en función de un conjunto de puntos
qdi . Los algoritmos de control de posición de alto desempeño pueden ser empleados
en control punto a punto como son los casos de los esquemas tangente hiperbólico y
arcotangente.
8.4.1
8.4.1. Control tangente hiperbólico
La estructura matemática del control tangente hiperbólica es la siguiente:
τ
= Kp tanh(q̃) − Kv tanh(q̇) + g(q)
♣ ♣ Ejemplo 8.6
Posicionar el extremo final de un robot de 2 gdl en: [ qd1 , qd2 ]T = [ 45, 90 ] m.
Solución
El programa cap8 robot2gdlTANH.m
8.9.
que se presenta en el cuadro 8.16
tiene el modelo dinámico del robot
de 2 gdl; en la lı́nea 22 se conecta el archivo cap8 TANHrobot2gdl.m
(ver cuadro 8.17) para realizar el control tangente hiperbólico. El programa
cap8 TANHrobot2gdlsimu.m del cuadro 8.18 permite realizar la simulación
del control tangente hiperbólica cuyos
Figura 8.9 Control tanh().
resultados se muestran en la figura 8.8.
.9
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
8.4
423
Control punto a punto
Código Fuente 8.16
cap8 robot2gdlTANH.m
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 8 Control de posición.
cap8 robot2gdlTANH.m
1
function xp = cap8 robot2gdlTANH(t,x)
2
3
%vector de posición articular
q1=x(1); q2=x(2); q = [q1; q2];
4
%vector de velocidad articular
5
qp1=x(3); qp2=x(4); qp = [qp1; qp2];
m1=23.902; l1=0.45; lc1=0.091; I1=1.266; b1=2.288;
m2=3.880; l2=0.45; lc2=0.048; I2=0.093; b2=0.175; g=9.81;
6
7
8
theta1=m1*lc1*lc1+m2*l1*l1+m2*lc2*lc2+I1+I2;
theta2=l1*m2*lc2;
theta3=m2*lc2*lc2+I2;
theta4=g*(lc1*m1+m2*l1); theta5=g*m2*lc2;
9
10
11
12
theta6=b1; theta7=b2;
M=[theta1+2*theta2*cos(q2), theta3+theta2*cos(q2);
theta3+theta2*cos(q2), theta3];
C=[ -2*theta2*sin(q2)*qp2, -theta2*sin(q2)*qp2;
13
14
15
16
theta2*sin(q2)*qp1, 0];
gq11=theta4*sin(q1)+theta5*sin(q1+q2);
gq21=theta5*sin(q1+q2);
gq=[gq11; gq21];
17
18
19
20
22
fr=[theta6*qp1;
theta7*qp2];
[˜ , tau]=cap8 TANHrobot2gdl(q,qp);
23
qpp = M∧ (-1)*(tau-C*qp-gq-fr);
24
%vector de salida
xp = [qp1; qp2; qpp(1); qpp(2)];
21
25
26
end
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
424
Capı́tulo 8: Control de posición
Código Fuente 8.17
cap8 TANHrobot2gdl.m
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 8 Control de posición.
cap8 TANHrobot2gdl.m
1
function [qtilde tau] = cap8 TANHrobot2gdl(x,xp)
2
q1=x(1); q2=x(2); q = [q1; q2]; %posiciones
qp1=xp(1); qp2=xp(2); qp = [qp1; qp2]; %velocidades
3
4
%parámetros del robot: par gravitacional
m1=23.902; lc1=0.091; l1=0.45;
5
6
8
m2=3.880; lc2=0.048; g=9.81;
theta4=g*(lc1*m1+m2*l1);
theta5=g*m2*lc2;
9
%par gravitacional
7
10
par grav = [theta4*sin(q1)+theta5*sin(q1+q2);
g*m2*lc2];
11
12
13
%ganancia proporcional
kp1=10; kp2=2;
14
Kp=[kp1, 0; 0, kp2];
15
16
%ganancia derivativa
kv1=8; kv2=1; Kv=[kv1, 0; 0, kv2];
17
%referencias
18
qd1=45; qd2=90; qd=[qd1; qd2];
qtilde=pi/180*qd-q;
19
20
21
%error de posición en grados
qtgrados=(180/pi)*qtilde;
22
%velocidad en grados/segundo
23
qpgrados=180*qp/pi;
tau=Kp*tanh(qtgrados)-Kv*tanh(qpgrados)+par grav;
24
25
end
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
8.4
425
Control punto a punto
Código Fuente 8.18
cap8 TANHrobot2gdlsimu.m
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 8 Control de posición.
cap8 TANHrobot2gdlsimu.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
clc;
clear all;
close all;
format short g
ti=0; h=0.001; tf = 5; t=ti:h:tf; %vector tiempo
ci=[0; 0; 0; 0]; %condciones iniciales
opciones=odeset(’RelTol’,1e-3,’InitialStep’,1e-3,’MaxStep’,1e-3);
[t,x]=ode45(’cap8 robot2gdlTANH’,t,ci,opciones);
q1=x(:,1); q2=x(:,2);
qp1=x(:,3); qp2=x(:,4);
[n,m]=size(t);
tau1=zeros(n,1); tau2=zeros(n,1); qtilde1=zeros(n,1); qtilde2=zeros(n,1);
xef=zeros(n,1); yef=zeros(n,1);
beta1=0.15; beta2=0.15; l1=0.45; l2=0.45;
for k=1:n
15
[qt tau] = cap8 TANHrobot2gdl([q1(k);q2(k)],[qp1(k);qp2(k)]);
tau1(k)=tau(1); tau2(k)=tau(2);
qtilde1(k)=qt(1); qtilde2(k)=qt(2);
[xef(k), yef(k),˜ ]=cinematica r2gdl(beta1,l1,q1(k),beta2,l2,q2(k));
16
17
18
19
p=Rz(-pi/2)*[xef(k), yef(k), beta1+beta2]’;
xef(k)=p(1); yef(k)=p(2);
20
21
end
22
subplot(2,2,1); plot(t,qtilde1,t,qtilde2)
subplot(2,2,2); plot( t, tau1, t,tau2)
subplot(2,2,3); plot(xef,yef)
subplot(2,2,4); plot(qtilde1, qp1, qtilde2,qp2)
23
24
25
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
426
Capı́tulo 8: Control de posición
8.4.2
8.4.2. Control arcotangente
El control arcotangente está dado por la siguiente expresión:
τ
= Kp atan (q̃) − Kv atan (q̇) + g(q)
♣ ♣ Ejemplo 8.7
Generar una flor de 8 pétalos con un robot de 2 gdl usando el control arcotangente.
Solución
Dentro de las aplicaciones de control punto a punto se encuentra el trazo de figuras.
El programa cap8 robot2gdlatansimu.m (cuadro 8.19) realiza la simulación para
que un robot antropomórfico de 2 gdl trace una figura tipo flor con 8 pétalos usando
el control arcotangente como se muestra en la figura 8.10.
Figura 8.10 Control atan().
El control arcotangente es de alto desempeño y la sintonización de las ganancias
proporcional y derivativa deben satisfacer ser menor al par máximo que pueda
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
8.4
427
Control punto a punto
entregar el servoamplificador. Con esta sintonı́a de las ganancias es posible controlar
todos los puntos de la figura a trazar.
El programa cap8 robot2gdlflor8p.m que se encuentra en el cuadro 8.20 tiene el
código fuente del robot de 2 gdl; particularmente en la lı́nea 11 se inserta el algoritmo
de control arcotangente el cual se encuentra en el programa cap8 atanflor8p.m del
cuadro 8.21. Observe que el pase de parámetros del control arcotangente son: t, q, q̇;
el tiempo t es necesario para generar la figura en forma de flor con 8 pétalos.
Código Fuente 8.19
cap8 robot2gdlatansimu.m
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 8 Control de posición.
cap8 robot2gdlatansimu.m
1
2
3
4
5
6
7
8
9
10
11
clc; clear all; close all;
format short g
ti=0; h=0.001; tf = 10; %vector tiempo
t=ti:h:tf; %tiempo de simulación
ci=[0; 0; 0; 0];
opciones=odeset(’RelTol’,1e-3,’InitialStep’,1e-3,’MaxStep’,1e-3);
[t,x]=ode45(’cap8 robot2gdlflor8p’,t,ci,opciones);
q1=x(:,1);
q2=x(:,2);
[n,m]=size(t);
14
xef=zeros(n,1);
yef=zeros(n,1);
beta1=0.15; beta2=0.15; l1=0.45; l2=0.45;
[xef, yef,˜ ]=cinematica r2gdl(beta1,l1,q1,beta2,l2,q2);
15
plot(xef,yef)
12
13
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
428
Capı́tulo 8: Control de posición
Código Fuente 8.20
cap8 robot2gdlflor8p.m
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 8 Control de posición.
cap8 robot2gdlflor8p.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
function xp = cap8 robot2gdlflor8p(t,x)
q = [x(1); x(2)]; %vector de posición articular
qp = [x(3); x(4)]; %vector de velocidad articular
M = [2.351+0.168*cos(q(2)) 0.102+0.084*cos(q(2)); %matriz de inercia
0.102+0.084*cos(q(2)) 0.102 ];
C = [-0.168*sin(q(2))*qp(2) -0.084*sin(q(2))*qp(2); %matriz de fuerzas centrı́petas y de Coriolis
0.084*sin(q(2))*qp(1) 0.0 ];
par grav = [38.46*sin(q(1))+1.82*sin(q(1)+q(2)); %vector de pares de gravitacionales
1.82*sin(q(1)+q(2)) ];
fr = [2.288*qp(1); 0.175*qp(2)];
[˜ , tau] = cap8 atanflor8p(t,q,qp) ; %ley de control arcotangente
qpp = inv(M)*(tau-C*qp- par grav-fr); %vector de aceleración articular
xp = [qp(1); qp(2);qpp(1); qpp(2) ]; %vector de salida
end
Código Fuente 8.21
cap8 atanflor8p.m
%MATLAB Aplicado a Robótica y Mecatrónica.
%Editorial Alfaomega,
Fernando Reyes Cortés.
%Capı́tulo 8 Control de posición.
cap8 atanflor8p.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
function [qtilde tau] = cap8 atanflor8p(t,x,xp)
q = [x(1); x(2)]; %posiciones
qp = [xp(1); xp(2)]; %velocidades
kp1=100; kp2=15; kv1=kp1;kv2=kp2;
Kp=[kp1, 0; 0, kp2]; %ganancia proporcional
Kv=[kv1, 0; 0, kv2]; %ganancia derivativa
lambda1=180/pi; lambda2=180/pi; Lambda=[lambda1 0; 0 lambda2];
gamma1=1; gamma2=1; Gamma=[gamma1 0; 0 gamma2];
par grav = [38.46*sin(q(1))+1.82*sin(q(1)+q(2)); %vector de pares de gravitacionales
1.82*sin(q(1)+q(2)) ];
xc=0.3; yc=0.3; r=0.2; l1=0.45; l2=0.45; alfa=0.07; beta=4; x=xc+alfa+r*(cos(t))*(sin(beta*t));
y=-yc+alfa+r*(sin(t))*(sin(beta*t));
[qd1 qd2]=cinv r2gdl(l1,l2,x,y);
qd=[qd1; qd2];
qtilde=[qd(1)-q(1); qd(2)-q(2)]; %error de posición
%ley de control PD
tau=Kp*atan(Lambda*qtilde)-Kv*atan(Gamma*qp)+par grav;
end
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
8.5
429
Resumen
8.5 Resumen
C
ontrol de posición robots manipuladores permite realizar una amplia gama
de aplicaciones industriales, comerciales, cientı́ficas, etc. La aplicación más
popular del control de robot manipuladores es la que se denomina control punto
a punto. Prácticamente todas las aplicaciones industriales pueden ser realizadas a
través de esta técnica de control.
En el presente capı́tulo se presentó las bases para entender el funcionamiento
cualitativo de los algoritmos de control, partiendo desde los tradicionales como el
proporcional derivativo (PD) y control proporcional integral derivativo (PID), hasta
nuevos esquemas como las funciones tangente hiperbólica y arcotangente.
El modelo dinámico de robots manipuladores permite evaluar cualitativamente el
desempeño de nuevos esquemas de control como:
[qtilde tau] = pd(x, xp)
[qtilde tau] = pid(x, xp, varepsilon)
[qtilde tau] = atan(x, xp)
[qtilde tau] = tanh(x, xp)
El diseño de nuevas estrategias de control es posible gracias a la técnica de moldeo de
energı́a la cual genera una familia muy extensa de esquemas de control para robots
manipuladores.
En el Web del libro se encuentran los programas fuentes de las librerı́as de control
de posición.
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
430
Capı́tulo 8: Control de posición
Parte IV Referencias selectas
H
ay una extensa literatura para el área de control de robots manipuladores,
particularmente se recomienda los siguientes textos:
8 Control de posición
M. Vidyasagar.
“Nonlinear systems analysis”. Prentice-Hall,
Englewood Cliffs, NJ (1993).
R.M.
Murray,
Z.
Li
and
S.S.
Sastry.
“A mathematical introduction to robotic manipulation”.
CRC
Press (1994).
A. Isidori. “Nonlinear control systems”. Springer-Verlag, London,
UK (1995).
Karl
J.
Åström
and
Björn
“Computer controlled systems: theory and design”.
Wittenmark.
Third
Edi-
tion. Prentice Hall Inc. 1997.
Shankar Sastry. “Nonlinear systems: analysis stability and control”.
Springer 1999.
Hassan K. Khalil. “Nonlinear systems”. Third Edition. Prentice
Hall Inc. 2002.
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
431
Parte IV Problemas propuestos
R. Kelly y V. Santibánez. “Control de movimiento de robots manipuladores”.
Printice-Hall, Pearson. 2003.
R.
Kelly,
V.
Santibánez
and
“Control of robot manipulators in joint space”.
A.
Lorı́a.
Springer-Verlag
London 2005.
Mark W.
Spong
and
Seth
Hutchinson,
M.
Vidyasagar.
“Robot modeling and control”. John Wiley and Sons, Inc. 2006.
Fernando Reyes
“Robótica: control de robots manipuladores”.
Alfaomega, 2011.
Parte IV Problemas propuestos
E
n esta sección se presenta una serie de ejercicios con la finalidad de mejorar
las habilidades en simulación de control de robots manipuladores.
Capı́tulo 8 Control de posición
8.1 Implementar el control PID para los siguientes robots:
(a) Péndulo qd1 = 90 grados.
(b) Robot antropomórfico de 3 gdl q d = [ 30, 45, 90] grados.
(c) Control cartesiano de 3 gdl dd = [ 0.3, 0.4, 0.5 ] m.
8.2 Considere la siguiente estructura de control:
τ
= Kp sen(q̃) − Kv sen(q̇) + g(q)
implementar esta estructura de control en los siguientes robots:
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
432
Capı́tulo 8: Control de posición
(a) Péndulo qd1 = 90 grados.
(b) Robot antropomórfico de 2 gdl q d = [ 45, 90 ]T grados.
(c) Robot antropomórfico de 3 gdl qd = [ 45, 90, 135]T grados.
(d) Control cartesiano de 3 gdl dd = [ 0.5, 0.8, 0.9]T m.
8.3 Considere la siguiente familia de algoritmos de control tipo PD:
τ
= Kp q̃2m−1 − Kv q̇2m−1 + g(q)
donde m ∈ N .
Implemente esta estructura de control en los casos para m = 1, 2, 3, 4, 5 en los
siguientes robots:
(b) Robot antropomórfico de 2 gdl qd = [ 90, 135 ]T grados.
(c) Control cartesiano de 3 gdl dd = [ 0.5, 0.8, 0.9]T m.
¿Qué reglas de sintonı́a puede proponer para obtener el valor de las ganancias
de tal forma que no sature a los servoamplificadores cuya capacidad son de
τ1max = 150 Nm, τ2max = 15 Nm para el robot de 2 gdl y τ1max = 50 Nm,
τ2max = 150, τ3max = 15 para el robot de 3 gdl.
8.4 Sea la siguiente familia de controladores
τ
= Kp
coshm−1 (q̃) senh(q̃)
− Kv atan (q̇) + g(q)
1 + cosh(q̃)
considere los casos de estudio para m = 1, 2, 3, 4, 5 en un robot cartesiano de
3 gdl para controlar la posición del extremo final del robot en las siguientes
coordenadas: [ 0.4, 0.3, 0.8] m.
(a) ¿Cómo varı́a la respuesta del robot (errores de posición) conforme se
incrementa el exponente m.
(b) ¿Describa el desempeño de este esquema de control en función de las
ganancias y el exponente m.
Alfaomega
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
Índice analı́tico
A
clear, 17
cinemática, 140, 185
algoritmo
diferencial, 187
recursivo, 332
de control, 393
inversa, 186
columna de una matriz, 27
amortiguamiento, 129
columnas, 26, 35
antropomórfico, 408
concatenación, 35
archivos experimentales, 76
arreglos, 26, 41
constante de tiempo, 297
constantes útiles, 18
continue, 74
B
backward difference, 95
borrando, 35
control, 242
conveción DH, 192
arcotangente, 390
de posición, 393
brazo robot de 2 gdl, 403
de robots, 390, 393
PD, 396
break, 73
C
PD,
péndulo, 397
robot cartesiano, 413
case, 72
centrı́fuga, 301
robot de 2 gdl, 403
robot de 3 gdl, 408
434
Índice analı́tico
PID, 417
elseif, 60, 64
proporcional
390, 395
derivativo,
proporcional integral derivativo, 390
punto a punto, 395
end, 51, 52
error de posición, 395, 417
esquema escalar, 381
esquemas de regresión, 332, 359
tangente hiperbólico, 390
estimador de velocidad, 296
CRTL Q, 15
exit, 14
D
expansión de escalares, 37
Denavit-Hartenberg, 192
eye, 32
demos, 13
ezmesh, 47
F
derivada, 93
desempeño, 395
det, 85
feval, 50
determinante simbólico, 86
determinantes, 85
diferenciación numérica, 92
diff, 94, 97
filtro, 299
fopen, 76
format, 18
E
formato numérico, 18
ecuación diferencial ordinaria,
117, 288
Alfaomega
filtrado, 296, 299, 350
for, 58, 61, 64, 67, 73
dinámica, 284
else, 60
fclose, 76
forward difference, 95
fplot, 46
fprintf, 76
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
435
Índice analı́tico
frecuencia de corte, 357
gráficas, 43
H
frecuencia natural, 129
fricción de Coulomb, 306
fricción viscosa, 306
help, 13, 43
elfun, 49
elmat, 49
función, 51
specfun, 49
función
helpwin, 43
cı́rculo, 56
I
transferencia, 289
discontinua, 351
identificación, 331
genvarname, 16
ode, 120
identificadores, 15
ode23, 122
if, 58–60, 64
anidada, 53
inercia del rotor, 306
archivo, 51
inercia, 311
cuadratura, 113
inercia, robot 3 gdl
en lı́nea, 50
instrucciones condicionales, 59
quad, 108
integración numérica, 100
G
integral, 93
inversa de matriz, 87
ganancia
L
derivativa, 417
proporcional, 417
integral, 417
globales, 53
librerı́as de
cinemática, 274
Denavit-Hartenberg, 180
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
436
Índice analı́tico
Traslación y rotación, 182
potencia, 355, 359, 369, 378
matrices homogéneas, 174
regresión, 346
mı́nimos cuadrados, 334
regresión dinámico, 361
dinámico, 287, 331, 346, 359
linesolve, 89
dinámico filtrado, 350
load, 32, 76
moldeo de energı́a, 390, 393
Lobatto, 113
multivariable, 287, 336, 381
locales, 53
O
M
objetivo de control, 394
matrices de rotación, 148
ode, 120, 288
máximo, 69
ode113, 123
método de Euler, 95, 114
ode15s, 123
método de primer orden, 118
ode23, 122
método de Runge-Kutta, 118,
119
ode23s, 123
mı́nimo, 69
ode23tb, 124
mı́nimos cuadrados, 332, 334
ode45, 120
magic, 32
off-line, 83
Mathworks, 5, 80
ones, 32
matrices, 26, 38
matriz de covarianza, 333
ode23t, 123
operaciones básicas, 38, 40
operaciones con arreglos, 41
operadores
mesh, 47
[], 28
modelo
’, 24
energı́a, 353, 366
Alfaomega
(), 23
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.
437
Índice analı́tico
,, 24
quadgk, 114
:, 21
quadl, 113
;, 23
quadv, 114
%, 25
MATLAB, 5, 7
quit, 14
˜ , 25
R
aritméticos, 12
arreglos, 41
rand, 32
lógicos, 58
randn, 32
P
regla
de Cramer, 91
péndulo, 305, 346, 353, 397
de Simpson, 108
parámetros, 331
trapezoidal, 102
PD, 390, 395
PID, 100, 390, 417
rotación, 164
regresor, 332
regulación, 393
pivote, 27
renglón de una matriz, 27
plot3, 43
renglones, 26, 35
potencia, 381
return, 73
potencia filtrada, 357
robótica, 393
problema de control, 393
robot
producto interno, 144
antropomórfico, 408
prompt, 11
cartesiano, 373
Q
cartesiano de 3 gdl, 413
de 3gdl, 321
quad, 113
de 2 gdl, 310, 360
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés
Alfaomega
438
Índice analı́tico
manipulador de 3 gdl, 315,
408
sistemas dinámicos no autónomos, 288
manipuladores, 287, 393
sistemas lineales, 289
ruido, 299
sistemas mecatrónicos, 331
Runge Kutta, 327
subplot, 48
Runge-Kutta, 118
S
suma de matrices, 68
switch, 58, 72
T
save, 76
script, 54
transformación
serie de Taylor, 118
set point, 393, 395
homogénea, 173
traslación, 171
signo, 65
V
Simpson, 113
simulación, 287
simulación de sistemas dinámicos, 124
Simulink, 7
variables, 15
vector de mediciones, 333
vector de parámetros, 333
ventana de comandos, 11
sintaxis, 15
W
sistema lineal escalar, 295
sistema masa resorte amortiguador, 291
while, 58, 71, 73
whos, 16
sistemas dinámicos, 117
sistemas dinámicos autónomos,
288
Alfaomega
Z
zeros, 32
Matlab Aplicado a Robótica y Mecatrónica • Fernando Reyes Cortés.