MATLAB aplicado a Robótica y Mecatrónica

MATLAB APLICADO A ROBÓTICA Y MECATRÓNICA FERNANDO REYES CORTÉS

Editor Francisco Javier Rodríguez Cruz [email protected] Director Editorial Marcelo Grillo Giannetto [email protected]

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: [email protected] 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: [email protected] Colombia: Alfaomega Colombiana S.A. – Carrera 15 No. 64 A 29, Bogotá, Colombia, Tel.: (57-1) 2100122 – Fax: (57-1) 6068648 – E-mail: [email protected] 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: [email protected] 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: [email protected]

Acerca del autor Dr. Jos´ e Fernando Reyes Cort´ es. Es profesor investigador titular C de la Facultad de Ciencias de la Electr´onica, Universidad Aut´ onoma de Puebla. En 1984 obtuvo la Licenciatura en Ciencias de la Electr´ onica en la Facultad de Ciencias F´ısico Matem´aticas de la Universidad Aut´ onoma de Puebla. En 1990 obtuvo la Maestr´ıa en Ciencias con Especialidad en Electr´ onica en el Instituto Nacional de ´ Astrof´ısica, Optica y Electr´ onica (INAOE). Bajo la asesor´ıa del Dr. Rafael Kelly, en 1997 culmin´ o el Doctorado en Ciencias con Especialidad en Electr´onica y Telecomunicaciones en el Centro de Investigaci´on Cient´ıfica y de Educaci´ on Superior de Ensenada (CICESE). Es miembro del Sistema Nacional de Investigadores desde 1993 a la fecha. Actualmente es nivel I. Es autor de m´as de 150 art´ıculos nacionales e internacionales. Ha dirigido 30 proyectos cient´ıficos. Ha graduado a m´as de 100 tesistas de los niveles de ingenier´ıa, maestr´ıa y doctorado. Es autor del libro Rob´ otica. Control de robots manipuladores editado por Alfaomega donde se presenta la din´ amica de robots manipuladores y la t´ecnica moderna de moldeo de energ´ıa para dise˜ nar nuevos algoritmos de control. Como catedr´atico ha impartido m´as de 50 cursos del a´rea de control y rob´ otica a nivel licenciatura y posgrado. Fundador del Laboratorio de Rob´ otica y Control de la Facultad de Ciencias de la Electr´onica donde ha puesto a punto 30 prototipos de mecatr´onica y rob´ otica. 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´on y Estudios de Posgrado de la Facultad de Ciencias de la Electr´ onica desde febrero del 2002 a la fecha. Fundador y responsable del Cuerpo Acad´emico de Rob´ otica y Control en 2001. Fue responsable de la creaci´on de la Ingenier´ıa Mecatr´onica, y de los Posgrado de Ciencias de la Electr´ onica e Ingenier´ıa Electr´ onica e ingreso de ambos posgrados al PNPC de CONACyT.

A la persona que me dio la vida: Alicia Cort´es Castillo; por su apoyo y paciencia a mi esposa Silvia y mis tesoros Luis Fernando y Leonardo. Por la formaci´on transmitida de Angela Castillo Merchant, Luis Manuel Cort´es 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˜ no profesional, ya que sin ellos es imposible adquirir las habilidades para competir laboralmente. El avance de la ciencia y de la t´ecnica hace necesario actualizar continuamente esos conocimientos, y de acuerdo con esto Alfaomega publica obras actualizadas, con alto rigor cient´ıfico y t´ecnico, y escritas por los especialistas del ´area respectiva m´as destacados. Conciente del alto nivel competitivo que debe de adquirir el estudiante durante su formaci´ on profesional, Alfaomega aporta un fondo editorial que se destaca por sus lineamientos pedag´ ogicos que coadyuvan a desarrollar las competencias requeridas en cada profesi´ on espec´ıfica. De acuerdo con esta misi´on, con el fin de facilitar la comprensi´on y apropiaci´on del contenido de esta obra, cada cap´ıtulo inicia con el planteamiento de los objetivos del mismo y con una introducci´ on en la que se plantean los antecedentes y una descripci´ on de la estructura l´ ogica de los temas expuestos, asimismo a lo largo de la exposici´on se presentan ejemplos desarrollados con todo detalle y cada cap´ıtulo concluye con un resumen y una serie de ejercicios propuestos. Adem´as de la estructura pedag´ ogica con que est´an dise˜ nados nuestros libros, Alfaomega hace uso de los medios impresos tradicionales en combinaci´on con las Tecnolog´ıas de la Informaci´ on y las Comunicaciones (TIC) para facilitar el aprendizaje. Correspondiente a este concepto de edici´on, todas nuestras obras de la Serie Profesional a la que pertenece este t´ıtulo tienen su complemento en una p´ agina Web en donde el alumno y el profesor encontrar´an lecturas complementarias, c´odigo fuente de los programas desarrollados as´ı como la soluci´on y la respuesta de los problemas propuestos. Los libros de Alfaomega est´ an dise˜ nados para ser utilizados en los procesos de ense˜ nanza-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´on 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´ ologo

XV

XVI

XVIII

Parte I Programaci´on Cap´ıtulo 1 Conceptos b´ asicos

1

3

1.1 Introducci´ on

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´aficos

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´ umeros

17

viii 1.4.3 Formato num´erico

18

1.4.4 Operadores

21

1.5 Matrices y arreglos

26

1.5.1 Arreglos

41

1.6 Gr´ aficas

43

1.7 Funciones

49

1.7.1 Funciones archivo 1.8 Programaci´ on

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´ etodos num´ ericos

81

2.1 Consideraciones computacionales

83

2.2 Sistemas de ecuaciones lineales

84

2.2.1 Regla de Cramer Alfaomega

91

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

Contenido 2.3 Diferenciaci´ on num´erica

ix 92

2.3.1 Funci´ on diff

97

2.4 Integraci´ on num´erica

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´etodo de Euler

114

2.5 Sistemas din´amicos de primer orden

117

2.5.1 M´etodo de Runge-Kutta

118

2.5.2 Simulaci´ on de sistemas din´amicos x˙ = f (x)

124

2.6 Resumen

133

Parte I Referencias selectas

134

Parte I Problemas propuestos

135

Parte II Cinem´atica Cap´ıtulo 3 Preliminares matem´ aticos

139

141

3.1 Introducci´ on

143

3.2 Producto interno

144

3.3 Matrices de rotaci´on

148

3.3.1 Matriz de rotaci´ on alrededor del eje z0

151

3.3.2 Matriz de rotaci´ on alrededor del eje x0

161

3.3.3 Matriz de rotaci´ on alrededor del eje y0

163

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

x 3.4 Reglas de rotaci´ on

164

3.5 Transformaciones de traslaci´ on

171

3.6 Transformaciones homog´eneas

173

3.7 Librer´ıas para matrices homog´eneas

174

3.7.1 Matriz de transformaci´ on homog´enea HRx(θ)

175

3.7.2 Matriz de transformaci´ on homog´enea HRy (θ)

176

3.7.3 Matriz de transformaci´ on homog´enea HRz (θ)

177

3.7.4 Matriz de transformaci´ on homog´enea HTx(d)

178

3.7.5 Matriz de transformaci´ on homog´enea HTy (d)

178

3.7.6 Matriz de transformaci´ on homog´enea HTz (d)

179

3.7.7 Matriz de transformaci´on DH

180

3.8 Resumen

181

Cap´ıtulo 4 Cinem´ atica directa

183

4.1 Introducci´ on

185

4.2 Cinem´atica inversa

186

4.3 Cinem´atica diferencial

187

4.4 Clasificaci´on de robots industriales

189

4.5 Convenci´ on Denavit-Hartenberg

192

4.5.1 Algoritmo Denavit-Hartenberg 4.6 Resumen Alfaomega

196 198

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

Contenido

Cap´ıtulo 5 Cinem´ atica directa cartesiana

xi

199

5.1 Introducci´ on

201

5.2 Brazo robot antropom´orfico

202

5.3 Configuraci´ on SCARA (RRP)

234

5.4 Robot esf´erico (RRP)

245

5.5 Manipulador cil´ındrico (RPP)

254

5.6 Configuraci´ on cartesiana (PPP)

264

5.7 Resumen

273

Parte II Referencias selectas

277

Parte II Problemas propuestos

278

Parte III Din´amica Cap´ıtulo 6 Din´ amica

283

285

6.1 Introducci´ on

287

6.2 Estructura matem´atica para simulaci´ on

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 ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

296 301 Alfaomega

xii 6.6 P´endulo

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´ on param´ etrica

329

7.1 Introducci´ on

331

7.2 M´etodo de m´ınimos cuadrados

332

7.2.1 Linealidad en los par´ ametros 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´ on del p´endulo

346

7.5.1 Modelo din´ amico del p´endulo

346

7.5.2 Modelo din´ amico filtrado del p´endulo

350

7.5.3 Modelo de energ´ıa del p´endulo

353

7.5.4 Modelo de potencia del p´endulo

355

7.5.5 Modelo de potencia filtrada

356

7.5.6 An´ alisis comparativo de esquemas de regresi´on

359

7.6 Modelos de regresi´ on del robot de 2 gdl 7.6.1 Modelo de regresi´on din´ amico del robot de 2 gdl Alfaomega

360 361

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

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´ alisis comparativo de resultados de regresi´ on

372

7.7 Robot cartesiano de 3 gdl

373

7.7.1 Modelo de regresi´on din´ amico del robot cartesiano

374

7.7.2 Modelo de potencia del robot cartesiano de 3 gdl

378

7.7.3 An´ alisis comparativo de identificaci´ on

381

7.8 Resumen

382

Parte III Referencias selectas

383

Parte III Problemas propuestos

385

Parte IV Control Cap´ıtulo 8 Control de posici´ on

389

391

8.1 Introducci´ on

393

8.2 Control proporcional-derivativo (PD)

395

8.2.1 Control PD de un p´endulo

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 ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

417 418 422 Alfaomega

xiv 8.4.1 Control tangente hiperb´ olico

422

8.4.2 Control arcotangente

426

8.5 Resumen

429

Parte IV Referencias selectas

430

Parte IV Problemas propuestos

431

´ Indice anal´ıtico

Alfaomega

433

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

xv

Plataforma de contenidos interactivos Para tener acceso al c´odigo fuente de los programas de ejemplos y ejercicios presentados en MATLAB Aplicado a Rob´ otica y Mecatr´ onica, siga los siguientes pasos:

1) Ir a la p´ agina http://virtual.alfaomega.com.mx

2) Registrarse como usuario del sitio.

3) En el cat´ alogo identificar este libro y descargar el material adicional.

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

Simbolog´ıa e iconograf´ıa utilizada

xvi

Simbolog´ıa e iconograf´ıa utilizada Diversos recursos did´ acticos est´an presentes en esta obra; particularmente se resaltan las siguientes herramientas: Librer´ıas La sintaxis de las librer´ıas que se desarrollan son representadas como: ˙ x=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´ asico se emplea la marca ♣, ejemplos del nivel intermedio ♣♣ y complejos son denotados por ♣♣♣ Soluci´ on Se detalla la respuesta de cada ejemplo por ecuaciones y programas en c´odigo fuente. Adicionalmente, todos los ejemplos incluyen un n´ umero de referencia que identifica al cap´ıtulo donde fue definido. Todos los programas de esta obra han sido implementados en lenguaje MATLAB versi´ on 11. El c´ odigo fuente se identifica por el siguiente recuadro: 5.1.

Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

xvii

C´ odigo Fuente 5.1

robot.m

otica y Mecatr´ onica. %MATLAB Aplicado a Rob´ %Editorial Alfaomega,

Fernando Reyes Cort´ es.

%Simulaci´ on 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´on 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´ on de programaci´ on, se emplea pseudo-c´odigo como el siguiente: Estructura de c´ odigo 5.1 Pseudo c´ odigo while k100 for j=1:1000 qpp(j)=robot(q(j),qp(j)); end end k=k+1; otro grupo de instrucciones; end

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

Pr´ologo Rob´ otica y mecatr´onica representan en la actualidad a´reas estrat´egicas y claves para todo pa´ıs que aspire a la modernidad y bienestar, ya que su impacto no s´ olo repercute en aspectos pol´ıticos y econ´ omicos, tambi´en forma parte importante de la vida cotidiana, educaci´on, cultura, y en todos los a´mbitos de la sociedad. La simulaci´on es una herramienta imprescindible para reproducir los fen´ omenos f´ısicos de un robot o de un sistema mecatr´onico, permite estudiar y analizar a detalle los aspectos pr´ acticos que intervienen en tareas espec´ıficas que debe realizar un robot industrial. La simulaci´ on es un proceso previo a la etapa experimental donde es posible entender los conceptos claves de la rob´otica y mecatr´onica. Bajo este escenario se ubica la presente obra a trav´es de la propuesta de un conjunto de librer´ıas para MATLAB que permitan realizar estudio, an´ alisis, simulaci´on y aplicaciones de robots manipuladores y sistemas mecatr´ onicos. Esta obra presenta la propuesta y desarrollo de una clase particular de librear´ıas toolbox para rob´ otica y mecatr´onica. Las librer´ıas son funciones en c´ odigo fuente para MATLAB que permiten modelar la cinem´ atica directa e inversa, transformaciones homog´eneas (rotaci´ on y traslaci´ on), din´ amica, identificaci´ on param´etrica, control de robots manipuladores. El contenido y material incluido en el libro est´ a dirigido a estudiantes de ingenier´ıa en electr´onica, sistemas, computaci´ on, industrial, rob´ otica y mecatr´onica. No obstante, tambi´en puede ser adecuado para nivel posgrado. La organizaci´ on de este libro consta de cuatro partes: la Parte I Programaci´ on incluye dos cap´ıtulos. El Cap tulo 1 Conceptos ba ´sicos de programacio ´n del lenguaje de MATLAB tiene la finalidad de que el lector adquiera los conocimientos necesarios para adquirir solvencia en programar aplicaciones en c´odigo fuente. El cap´ıtulo 2 M´ etodos num´ ericos presenta aspectos relacionados con las t´ecnicas de m´etodos num´ericos enfocados a resolver problemas de diferenciaci´on e integraci´ on num´erica de sistemas din´amicos. La Parte II Cinem´ atica est´a relacionada con el an´ alisis cinem´atico de las principales clases de robots manipuladores. Se componen de tres cap´ıtulos; el

Pr´ ologo

xix

cap´ıtulo 3 Preliminares matem´ aticos contiene las bases de las reglas de las matrices de rotaci´on, traslaci´ on y transformaciones homog´eneas. En el cap´ıtulo 4 Cinem´ atica directa se detalla la metodolog´ıa de Denavit-Hartenberg para describir la cinem´atica directa; asimismo se describe la cinem´ atica inversa, cinem´atica diferencial y jacobianos. Un compendio de librer´ıas se desarrollaron con variables simb´ olicas y aplicaciones num´ericas para los principales tipos de robots manipuladores en el cap´ıtulo 5 Cinem´ atica directa cartesiana. La Parte III Din´ amica describe la t´ecnica de simulaci´on para sistemas din´amicos e identificaci´ on param´etrica. Formada por el cap´ıtulo 6 Din´ amica donde se realiza simulaci´on de sistemas mecatr´onicos y robots manipuladores con la estructura de una ecuaci´ on diferencial ordinaria de primer orden descrita por variables de estado. El cap´ıtulo 7 Identificaci´ on param´ etrica presenta cinco esquemas de regresi´on con la t´ecnica de m´ınimos cuadrados. Se proponen extensos ejemplos para ilustrar el procedimiento para encontrar el valor num´erico de los par´ametros del robot. Finalmente, la Parte IV cap´ıtulo 8 Control de posici´ on contiene la simulaci´on de algoritmos cl´ asicos como el control PD y PID; as´ı como simulaci´ on de nuevas estrategias de control con mejor desempe˜ no que pertenecen a la filosof´ıa de dise˜ no moderno conocida como moldeo de energ´ıa. Aplicaciones de control punto a punto se ilustran para describir la t´ecnica de simulaci´on. La realizaci´on de esta obra no hubiera sido posible sin el apoyo de la Benem´ erita Universidad Aut´ onoma de Puebla (BUAP), particularmente agradezco a la Facultad de Ciencias de la Electr´onica 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´ anchez, as´ı como al programa de financiamiento de proyectos de investigaci´ on de la Vicerrector´ıa de Investigaci´ on y Estudios de Posgrado (VIEP), especialmente a la Dra. Rosa Graciela Montes y al Dr. Pedro Hugo Hern´ andez Tejeda.

Fernando Reyes Cort´es H. Puebla de Z., a 3 de diciembre de 2011 Facultad de Ciencias de la Electr´onica Benem´ erita Universidad Aut´ onoma de Puebla ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

Parte I Programaci´on

La Parte I de la presente obra est´a destinada a estudiar el lenguaje de programaci´on de MATLAB y la implementaci´ on de m´etodos num´ericos aplicados a la rob´otica y mecatr´onica. Esta primera parte se compone de dos cap´ıtulos. El cap´ıtulo 1 Conceptos b´ asicos presenta los aspectos b´asicos del lenguaje MATLAB y tiene la finalidad de que el lector adquiera los conocimientos fundamentales para adquirir solvencia en programar en c´ odigo fuente. El cap´ıtulo 2 M´ etodos num´ ericos presenta aspectos relacionados con las t´ecnicas de m´etodos num´ericos enfocados a resolver problemas de modelado y control de las ´areas de rob´ otica y mecatr´onica.

Cap´ıtulo 1 Conceptos b´asicos Descripci´on de los principales componentes del ambiente de programaci´ on de MATLAB. Sintaxis y programaci´ on, declaraci´ on de variables, tipo de datos, operadores, programaci´ on, manipulaci´ on num´erica de matrices y arreglos, funciones, gr´aficas, funciones importantes, matrices, operadores, instrucciones, lazos de programaci´ on, etc´etera. Un conjunto de ejemplos se desarrollan para ilustrar los temas presentados.

Cap´ıtulo 2 M´etodos num´ericos Conocer los principales m´etodos de diferenciaci´on e integraci´on num´erica que se aplican en rob´otica y mecatr´onica. Sistemas de ecuaciones lineales, m´etodo de Euler e integraci´ on num´erica (trapezoidal, Simpson). Simulaci´ on de sistemas din´ amicos con la t´ecnica 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´asicos

Cap´ıtulo

1

1.1 Introducci´ on 1.2 Componentes 1.3 Inicio 1.4 Lenguaje 1.5 Matrices y arreglos 1.6 Gr´ aficas 1.7 Funciones 1.8 Programaci´ on 1.9 Formato para datos experimentales 1.10 Resumen

Objetivos Presentar la plataforma y ambiente de programaci´ on de MATLAB con los fundamentos y bases de su lenguaje enfocado para adquirir solvencia y dominio en la implementaci´ on de algoritmos con aplicaciones en mecatr´onica y rob´ otica.

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´ on

5

1.1 Introducci´ on

H

oy en d´ıa, MATLAB es un lenguaje de programaci´on matem´ atico de alto nivel integrado con entorno gr´ afico amigable, visualizaci´ on de datos, funciones, gr´ aficas 2D y 3D, procesamiento de im´ agenes, video, computaci´on num´erica para desarrollar algoritmos matem´aticos con aplicaciones en ingenier´ıa y ciencias exactas. Particularmente, en ingenier´ıa es una herramienta muy poderosa para realizar aplicaciones en mecatr´onica, rob´ otica, control y automatizaci´ on. MATLAB es un acr´ onimo que proviene de matrix laboratory (laboratorio matricial) creado por el profesor y matem´atico Cleve Moler en 1970. La primera versi´ on de MATLAB fue escrita en lenguaje fortran la cual contempl´o la idea de emplear subrutinas para los cursos de a´lgebra lineal y an´ alisis num´erico de los paquetes LINPACK y EISPACK; posteriormente se desarroll´o 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´ on en este lenguaje fue escrita por Steve Bangert quien desarroll´ o el int´erprete parser. Steve Kleiman implement´o los gr´ aficos, las rutinas de an´alisis, 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´ on de MATLAB proporciona un sencillo acceso a algoritmos num´ericos 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 ´areas de ingenier´ıa y ciencias exactas. Un rasgo distintivo de MATLAB es que ofrece un entorno gr´ afico de programaci´ on amigable al usuario a trav´es de herramientas y utiler´ıas para realizar simulaci´ on de sistemas din´ amicos, an´ alisis de datos, procesamiento de im´ agenes y video, gr´ aficas y m´etodos ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

6

Cap´ıtulo 1: Conceptos b´ asicos

de visualizaci´ on, desarrollo de interfaces de usuario (GUI); tambi´en permite realizar interfaces para sistemas electr´onicos, por ejemplo adquisici´ on de datos con una versatilidad de tarjetas de instrumentaci´on electr´onica comerciales con plataforma de microprocesadores, DSP’s, PIC’s, FPGA’s, manejo de puertos como USB, COM, paralelo y dise˜ nos electr´onicos propios. MATLAB dispone de dos herramientas adicionales que expanden sus prestaciones: plataforma de simulaci´on 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´ecnicas, cursos, aplicaciones y foros internacionales, as´ı como sociedades de desarrollo. Actualmente, MATLAB se ha convertido en un referente de c´ omputo a nivel internacional, debido que en la mayor´ıa de las universidades, centros de investigaci´ on e industria lo utilizan para desarrollar y llevar a cabo proyectos de ciencia y tecnolog´ıa. Espec´ıficamente en el a´rea de ingenier´ıa MATLAB permite realizar simulaciones de sistemas mecatr´onicos y robots manipuladores. El proceso de simulaci´on resulta importante cuando no se dispone de una adecuada infraestructura experimental. Sin embargo, la simulaci´ on depende de un buen modelo matem´ atico que permita reproducir fielmente todos los fen´ omenos f´ısicos del sistema real. La simulaci´ on es flexible ya que permite detectar posibles deficiencias en el dise˜ no del modelado. Una vez depurado el modelo din´ amico, entonces la simulaci´on facilita el proceso para analizar, estudiar y comprender el comportamiento de la din´ amica del sistema. Esta etapa es fundamental para el dise˜ no de algoritmos de control. Cuando el modelo matem´atico del sistema mecatr´onico o robot manipulador es lo suficientemente completo entonces la simulaci´on proporciona un medio virtual del sistema real. El prop´ osito de este cap´ıtulo se ubica en preparar al lector para conocer mejor el entorno de programaci´ on, programar y manejar en forma solvente el lenguaje de MATLAB. En otras palabras, proporcionar todos los elementos necesarios de programaci´ on (variables, datos, instrucciones y funciones) para realizar diversas Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

1.2

Componentes

7

aplicaciones para las ´areas de rob´otica y mecatr´onica.

1.2 Componentes

E

l ambiente de programaci´ on de MATLAB es amigable al usuario, y est´a compuesto por una interface gr´afica 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 afica de usuario MATLAB se presenta en la figura 1.1 la cual contiene la interface gr´ 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´ on de MATLAB. Es recomendable personalizar el ambiente de programaci´on de MATLAB para una f´ acil interacci´ on con manejos de archivos, datos y herramientas de programaci´ on. Por ejemplo, para integrar la ventana del editor dentro del ambiente gr´ afico se logra oprimiendo el icono del editor de texto colocado en la esquina superior izquierda, ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

8

Cap´ıtulo 1: Conceptos b´ asicos

justo abajo de la opci´ on File del men´ u 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´ on. Generalmente el usuario registra sus archivos en una carpeta o directorio predefinido; para dar de alta dicho directorio al momento de simular aparecer´ a el mensaje que se muestra en la figura 1.3. Seleccionar Add to Path para que MATLAB realice la simulaci´on desde esa trayectoria de trabajo, de esta forma las trayectorias de otros directorios que est´en habilitadas no se perder´an, por lo que la trayectoria del usuario se a˜ nade a las ya existentes. Las componentes principales del ambiente de programaci´on de MATLAB son:

Herramientas de escritorio y ambiente de desarrollo. Librer´ıas. Lenguaje MATLAB. Gr´ aficas. Interfaces externas/API. Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

1.2

9

Componentes

Figura 1.3 Habilitaci´on 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´ aficas 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´odigo, navegador de archivos, etc´etera.

1.2.2 1.2.2. Librer´ıas Las librer´ıas de MATLAB son un conjunto muy grande de funciones y algoritmos matem´aticos que van desde funciones trigonom´etricas, operaciones b´ asicas con matrices hasta funciones complejas como an´alisis de regresi´ on, inversa de matrices, funciones Bessel, transformada r´apida de Fourier, Laplace, programaci´ on de DSP’s, microprocesadores, procesamiento de im´agenes, video y tarjetas de instrumentaci´on electr´onica, entre otras ventajas. ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

10

Cap´ıtulo 1: Conceptos b´ asicos

1.2.3 1.2.3. Lenguaje El lenguaje de programaci´on 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´on del problema a simular. Los comandos son funciones muy espec´ıficas que tienen un c´odigo depurado y que no se encuentra disponible al usuario. El lenguaje de programaci´on de MATLAB contiene todos los elementos de programaci´on necesarios para desarrollar aplicaciones en realidad virtual, programaci´ on de robots manipuladores, an´alisis de regresi´ on y estad´ısticos de se˜ nales experimentales, procesamiento y extracci´on de rasgos distintivos de im´agenes y video, as´ı como varias aplicaciones m´as en ingenier´ıa y ciencias exactas.

1.2.4 1.2.4. Gr´ aficos MATLAB contiene un enorme n´ umero de funciones que facilitan la representaci´on gr´ afica y visualizaci´on de variables, funciones, vectores, matrices y datos que pueden ser graficados en 2 y 3 dimensiones. Incluye tambi´en funciones de alto nivel para el an´ alisis y procesamiento de gr´aficas, datos experimentales o de simulaci´on, video e im´agenes, animaci´ on y presentaci´ on de s´olidos, as´ı como el desarrollo de aplicaciones con interface gr´ afica para incluir men´ us con botones, barras deslizadoras, diagramas a bloques, instrumentos de medici´on y ventanas para seleccionar opciones o herramientas de la aplicaci´on.

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´ uen con programas en lenguaje MATLAB facilitando el proceso de llamada a rutinas desde MATLAB y pase de par´ ametros, as´ı como la Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

1.3

11

Inicio

programaci´ on de puertos (USB, COM, paralelo y serial) que permiten conectar tarjetas de instrumentaci´on electr´onica para adquisici´ on de datos y evaluaci´on experimental de algoritmos de control.

1.3 Inicio

U

na vez instalado MATLAB, la forma m´as 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´ a el nombre por default ans que corresponde a un nombre corto de answer. Al especificar una variable el resultado se desplegar´a con el nombre de esa variable, por ejemplo: fx >>

z=9+9 Enter ← z= 18

fx >>

y=sin(10) Enter

←

y=

−0.5440 ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

12

Cap´ıtulo 1: Conceptos b´ asicos

MATLAB funciona como calculadora, sobre el prompt de la ventana de comandos se pueden escribir expresiones aritm´eticas mediante los operadores “+”,“-”,“/”, “*”, “∧ ” para la suma, resta, divisi´on, multiplicaci´on y potencia, respectivamente. La tabla 1.1 contiene los operadores aritm´eticos b´asicos. 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´ eticos b´ asicos Adici´ on

+

Sustracci´ on

-

Multiplicaci´on

*

Divisi´on

/

Divisi´on left

\

Potencia

∧

Evaluaci´ on

()

MATLAB utiliza el operador ; para deshabilitar la opci´on de desplegado en la ventana de comandos. Por ejemplo, fx >>

z=9+9; ←

fx >> Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

1.3

13

Inicio

es decir, con el operador ; MATLAB no exhibir´ a ning´ un valor de variable o funci´on. El lenguaje MATLAB permite insertar comentarios usando el operador % el cual deber´ a 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´ umero. ← ans= −1

Obs´ervese que al ejecutarse la instrucci´on o la sentencia el comentario no se despliega. Los comentarios sirven para documentar aspectos t´ecnicos 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´ on para una instrucci´ on comando o funci´ on, as´ı como sus principales caracter´ısticas y ejemplos did´ acticos (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 ←

on sobre La ayuda en l´ınea del programa MATLAB tambi´en proporciona informaci´ los demos y tutoriales que facilitan el aprendizaje del paquete. ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

14

Cap´ıtulo 1: Conceptos b´ asicos

Para suspender la ejecuci´ on de un programa Cuando MATLAB se encuentra realizando la ejecuci´on de un programa o c´ alculo computacional en la esquina inferior izquierda aparecer´ a un icono que indica busy. Sin embargo, hay ocasiones que toma mucho tiempo la ejecuci´on del programa debido a que se encuentra realizando operaciones complicadas, funciones discontinuas o con n´ umeros muy grandes, en cuyo caso despliega Inf, que significa infinito. fx >>

sinh(1345e34) %Obtiene el seno hiperb´ olico de un n´ umero. ← ans= Inf

La siguiente expresi´on genera un lazo infinito de ejecuci´ on. Es decir el programa se quedar´ a de manera indefinida en ejecuci´ on. fx >> ←

while 1 end %Genera un ciclo infinito de ejecuci´ on del programa.

En este caso, hay ocasiones donde es recomendable interrumpir el proceso mediante la combinaci´on de teclas CRTL C (presionar simult´ aneamente 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´a efecto la acci´on de suspender programa.

Finalizar la sesi´ on de MATLAB Para finalizar MATLAB simplemente escribir en la ventana de comandos lo siguiente: fx >> fx >>

quit ← exit ←

Tambi´en se pueden usar las siguientes opciones para finalizar la sesi´on con MATLAB: Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

1.4

15

Lenguaje

Desde el teclado, presionar simult´aneamente CRTL Q o CONTROL con q Presionar simult´aneamente las teclas ALT F y despu´es la opci´on Exit Con el mouse sobre el men´ u de MATLAB click sobre File y click en Exit.

1.4 Lenguaje

E

l lenguaje de programaci´on de MATLAB est´a 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´ on. Las variables, constantes, operadores y funciones forman una expresi´ on la cual ser´a procesada por un analizador l´exico y sint´actico 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´ un tipo de declaraci´ on o definici´ on. Esto tiene varias ventajas ya que facilita la programaci´on. Cuando MATLAB encuentra el nombre de una nueva variable, autom´ aticamente crea la variable y le asigna una localidad de memoria. Por ejemplo, fx >> error posici´ on=20 esta variable es vista por MATLAB desde el punto de vista de programaci´on como una matriz de dimensi´on 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 ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

16

Cap´ıtulo 1: Conceptos b´ asicos

inicial (con excepci´ on de la letra n ˜ y sin acentos) seguido de cualquier n´ umero de letras, d´ıgitos, y tambi´en se puede usar el gui´ on bajo (underscore ). usculas de las min´ usculas; por ejemplo la variable MATLAB distingue las letras may´ A es diferente a la variable a. La longitud de los nombres de las variables puede ser cualquier n´ umero 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´aximo de 63 caracteres. La funci´on genvarname es u ´ til para crear nombres de variables que son v´ alidas y u ´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´olicas, etc. Los diversos tipos de variables son interpretados como matrices. Para saber qu´e 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´on 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

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

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´ervese que MATLAB trata a todas las variables como matrices para prop´ ositos de programaci´ on. Al terminar una sesi´on, 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´ aneamente las tres variables a trav´es 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´ umeros La notaci´on convencional que usa MATLAB para la representaci´ on de n´ umeros es la decimal con un punto decimal, signo ± y un n´ umero determinado de d´ıgitos despu´es del punto decimal, esto depende del tipo de formato num´erico empleado (ver format). En la notaci´ on cient´ıfica se usan potencias de diez. Tambi´en se emplean constantes tales como e, π. Los n´ umeros complejos o imaginarios emplean i o j. Todos los n´ umeros son almacenados internamente utilizando el formato long en notaci´ on est´andar de la IEEE para punto flotante con precisi´ on finita de 16 d´ıgitos decimales −308 308 y un rango finito de 10 a 10 . ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

18

Cap´ıtulo 1: Conceptos b´ asicos

Algunos ejemplos de n´ umeros 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´as usadas en el ´area de la ingenier´ıa. Tabla 1.2 Constantes u ´tiles pi

3.141592653589793. √

N´ umero imaginario i

−1

j

Igual que el n´ umero imaginario i

eps

Punto flotante con precisi´ on relativa, ε = 2−52

realmin

El n´ umero en punto flotante m´as peque˜ no, 2−1022 El n´ umero m´as grande en punto flotante, (2−ε)1023

realmax inf

Infinito ∞

NaN

No es un n´ umero

1.4.3 1.4.3. Formato num´ erico La forma para desplegar funciones, variables y cualquier tipo de dato por MATLAB se realiza a trav´es del comando format la cual controla el formato num´erico de los valores desplegados. Es decir, modifica el n´ umero de d´ıgitos para el desplegado de los datos. Este comando solo afecta a los n´ umeros que son desplegados, no al proceso de c´omputo num´erico o al registro de las variables o datos. format short ←

fx >> fx >>

x= [ 9/8 8.3456e-8] ←

Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

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´on de x con mayor n´ umero de d´ıgitos sin utilizar la notaci´ on cient´ıfica, entonces fx >>

format long ← x= 1.125000000000000 0.000000083450000

Para saber qu´e tipo de format est´a actualmente activo se usa el comando get: fx >>

get(0,’format’) ← ans= long

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

20

Cap´ıtulo 1: Conceptos b´ asicos

El formato num´erico que por default emplea MATLAB es el formato corto (short): format short. Sin embargo, es posible utilizar otros tipos de formatos como: extendido, notaci´ on cient´ıfica, punto fijo, punto flotante, formato de ingenier´ıa, etc. Los formatos num´ericos que contiene el comando format se encuentran resumidos en la tabla 1.3. Tabla 1.3 Opciones del comando format Tipo

Descripci´on

short

Formato por default. Despliega 4 d´ıgitos despu´es del punto decimal. Por ejemplo 3.1416

long

Formato para punto fijo con 15 d´ıgitos despu´es 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´es del punto decimal. Por ejemplo: ShortE

longE

1.4567e+000. Variables enteras que almacenan n´ umeros flotantes con m´as de 9 d´ıgitos no ser´an desplegados en la notaci´ on cient´ıfica. Punto flotante con 15 d´ıgitos despu´es del punto decimal para datos tipo doble y 7 d´ıgitos despu´es del punto decimal para variables tipo simple. Ejemplo x=4.123456789012345+e000. Variables enteras que almacenan n´ umeros flotantes con m´as de 9 d´ıgitos no ser´an desplegados en notaci´ on cient´ıfica.

shortG

Formato para punto fijo o punto flotante, 4 d´ıgitos despu´es del punto decimal. Ejemplo 3.1416.

longG

Formato para punto flotante o fijo con 14 o 15 d´ıgitos despu´es del punto decimal para variables tipo doble y 6 o 7 d´ıgitos despu´es del punto decimal para variables tipo simple. Ejemplo 8.01234567891234.

shortEng

longEng

Alfaomega

Formato para ingenier´ıa tiene 4 d´ıgitos despu´es del punto decimal y una potencia que es m´ ultiplo de tres. Ejemplo 3.1416e+000. Formato para ingenier´ıa tiene 16 d´ıgitos despu´es del punto decimal y una potencia que es m´ ultipo de tres. Ejemplo 3.14159265358979e+000. ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

1.4

21

Lenguaje

El comando format tambi´en proporciona representaci´ on hexadecimal, tipo propora ci´on b y de tipo bancaria como se explica en la tabla 1.4.

Tabla 1.4 Opciones del comando format Tipo

Descripci´on

rat

Despliega en forma de proporci´ on de n´ umeros enteros. Ejemplo 17/50

hex

Representaci´on 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´eticos de la tabla 1.1, hay operadores espec´ıficos para funciones, operaciones l´ ogicas, relacionales, estructuras de datos, uniones y matrices. A continuaci´ on se presentan diversos tipos de operadores de utilidad en MATLAB. Operador colon : El operador colon : (dos puntos verticales) es uno de los operadores m´as importantes para programar, se emplea para diferentes formas, como por ejemplo en MATLAB t´ecnicamente se conoce como vectorizaci´on al proceso de generar una secuencia de n´ umero 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 ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

22

Cap´ıtulo 1: Conceptos b´ asicos

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´en 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 u ´ til realizar una variaci´ on 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´entesis () son utilizados en diversas operaciones matem´aticas, por ejemplo: evaluar funciones como en el caso de y=sin(t). Tambi´en para referenciar a elementos de matrices A(2, 3). En expresiones aritm´eticas indican precedencia y la forma de agrupar variables x=3*(y+j)*r-i*(4+r). Resultan de utilidad para evaluar condiciones l´ogicas if (x>3) x=3 end. Cuando se trata de evaluar potencias los par´entesis determinan los niveles de jerarqu´ıa para realizar operaciones desde las Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

1.4

23

Lenguaje

m´as 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´aficas. Cuando se inserta al final de la expresi´on, instrucci´ on 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´ on, 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´ umero 3 y antecede al n´ umero 4 genera un rengl´ on de esta matriz. En este caso el primer rengl´on lo forman los n´ umeros 19 y 3, y para el segundo rengl´on formado por 4 y 5. Obs´ervese que despu´es 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´ on en la matriz. Es decir el operador ; tiene dos funciones muy diferentes. Por cada ; dentro de corchetes se generar´ a un rengl´ on de la matriz. Es muy importante que quede claro la aplicaci´ on de este operador para evitar problemas de programaci´ on. ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

24

Cap´ıtulo 1: Conceptos b´ asicos

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´ as detalles del operador ; referirse a la secci´on de Matrices y arreglos. Operador , El operador coma , tiene m´as de una funci´ on en MATLAB. Por ejemplo, cuando se emplea en funciones indica la separaci´on 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´ umero de rengl´ on y de la columna separados por una coma A(3,4). En matrices indica la separaci´on de los elementos de un rengl´on 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´en representa la transpuesta de una matriz. En relaci´ on 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

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

1.4

Lenguaje

25

El operador ’ es ampliamente utilizado en funciones donde el pase de par´ametros es a trav´es 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´on 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´on t´ecnica del algoritmo o aplicaci´ on 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´ afica onda senoidal

Operador ˜ El operador tilde ˜ se emplea para deshabilitar una variable de salida de una funci´ on. Es muy u ´til cuando la funci´ on retorna m´as de una variable y no se requiere usar todas las variables; sup´ongase que la funci´ on control robot retorna dos variables (error y par) y la sintaxis es: [error, par ]=control robot(q). No se requiere usar la variable error, u ´nicamente par, entonces se usa de la forma siguiente: [~ , par]= control robot(q). Tambi´en se emplea como negaci´on en operadores l´ ogicos, por ejemplo en ˜ = que significa no es igual a. En las siguientes secciones se ir´an explicando otros tipos de operadores que se encuentran directamente relacionados con matrices y en general con la programaci´ on del lenguaje de MATLAB. ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

26

Cap´ıtulo 1: Conceptos b´ asicos

1.5 Matrices y arreglos

E

n MATLAB una matriz es un arreglo rectangular de datos o n´ umeros, tienen n renglones por p columnas; la notaci´ on matem´ atica m´ as com´ un para representar n×p . Matrices con una sola columna o rengl´ on significan a una matriz es: A ∈ IR n×1 1×n o y ∈ IR , respectivamente. vectores para MATLAB, por ejemplo x ∈ IR Especial significado representan los escalares cuya interpretaci´ on para prop´ ositos de 1×1 programaci´ on corresponde a una representaci´ on de matriz del tipo IR .

Las entradas de la matriz se conocen como elementos y pueden ser n´ umeros reales, n´ umeros complejos, funciones, operadores, inclusive tambi´en pueden ser matrices de menor dimensi´on. Es importante destacar que una matriz es un objeto matem´ atico por s´ı misma, esto es, no representa un n´ umero 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´ on cl´asica 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´a 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´ on est´a 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´on

A(n, 1) A(n, 2) · · · A(n, p) Figura 1.4 Componentes de una matriz: elementos A(i, j), renglones y columnas. Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

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-´esimo rengl´on y j = 1 . . . p denota la j-´esima columna. Por ejemplo, en la matriz A ∈ IR5×5 el elemento A(2, 5) significa la componente del segundo rengl´on y quinta columna, el cual se muestra en la siguiente expresi´on:

⎡

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´ositos de identificar los elementos de una matriz, ´estos 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´on se hace referencia al primer elemento de la matriz, ubicado en el primer rengl´ on 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-´esimo rengl´on est´a dado antes del j-´esimo ´ı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´ umeros reales o n´ umeros enteros de magnitud mayor a la dimensi´on de la matriz. En otras palabras, en las referencias a elementos de la matriz u ´ nicamente con n´ umeros enteros o naturales dentro de los l´ımites de la dimensi´ on de la matriz. Por ejemplo, los siguientes elementos A(0, 0), A(0, 5), A(−5, −1), A(5, 0) son referencias inv´ alidas de una matriz. ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

28

Cap´ıtulo 1: Conceptos b´ asicos

Es muy com´ un 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´ on A(0, 0) o n´ umeros negativos A(−3, −5), ni n´ umeros reales como A(1.32, 2.67). En todos estos ejemplos existe error de sintaxis y la ejecuci´on de un programa no puede realizarse hasta no corregir dicho error. Los pivotes i, j son n´ umeros 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´ umeros enteros positivos (n´ umeros 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´ on 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 u ´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= ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

1.5

29

Matrices y arreglos

2 3

4 78.3 45

Usar el operador semicolon ; para indicar el fin del rengl´ on y generar otro m´ as 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´as de emplearse en matrices tiene otra funci´ on cuando se inserta al finalizar una variable, constante o funci´on desactiva la opci´ on de desplegado.

Los elementos del i-´esimo rengl´on o de la j-´esima columna de una matriz A son denotados como: A(i, j), la expresi´on A(3, 4) representa un elemento de la matriz que se encuentra localizado en el tercer rengl´on y cuarta columna. Consid´erese 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´ on 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. ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

30 fx >>

Cap´ıtulo 1: Conceptos b´ asicos 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´etrica, 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´as de su uso en matrices tambi´en se emplea en cadenas de caracteres. El operador colon : se puede emplear en matrices para referenciar a una porci´ on de 5×5 la matriz. Por ejemplo, sea una matriz A ∈ IR ⎡ ⎤ 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

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

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´ on 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´ asicas Existen varias formas para generar matrices b´ asicas en MATLAB, se pueden generar con funciones espec´ıficas, con funciones definidas por el usuario, o con el uso del operador colon :. A continuaci´ on se enlistan varias opciones para generar matrices b´ asicas 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). ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

32

Cap´ıtulo 1: Conceptos b´ asicos

La tabla 1.5 muestra las opciones m´as comunes (funciones especiales y por asignaci´on de datos) que permiten generar matrices b´asicas.

Tabla 1.5 Funciones especiales para generar matrices b´ asicas Nombre de la funci´ on

zeros(n,m)

ones(n,m)

rand(n,m)

randn(n,m)

Descripci´on 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´ umeros enteros desde 1 hasta n2 . El n´ umero 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´alisis.

Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

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´on: 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´ on m es posible generar una matriz de la siguiente forma. Crear en el editor de texto de MATLAB un archivo con la siguiente informaci´on: A=[ 8

3

7;

7.3

3.1416;

7.98 6.67

0.001];

6

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

34

Cap´ıtulo 1: Conceptos b´ asicos

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´on 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´ on: 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´ on 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´ on en la matriz A, la informaci´ on se encuentra lista para ser procesada por cualquier operaci´on de matrices previamente vista. En rob´ otica y mecatr´onica es com´ un grabar archivos experimentales o resultados de simulaci´ on para ser analizados con MATLAB usando un procedimiento parecido al planteado. El formato del archivo debe ser que los datos est´en distribuidos en renglones y columnas separadas por espacios en blanco. Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

1.5

35

Matrices y arreglos

Concatenaci´ on Al proceso de unir peque˜ nas matrices para hacer una m´ as grande se le denomina concatenaci´on. Los s´ımbolos [ ], mejor conocidos como corchetes, representan en MATLAB el operador concatenaci´ on. 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´on para obtener una matriz de dimensi´on 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´on [ ]. Por ejemplo, para borrar la segunda columna de la matriz A previamente definida en el apartado inmediato anterior se procede de la siguiente forma: ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

36 fx >>

Cap´ıtulo 1: Conceptos b´ asicos A(:,2)=[] ← A= 1

0 0

0

0 0

0

1 0

0

0 1

obs´ervese que por borrar la segunda columna, la nueva matriz resultante A adquiere una dimensi´on de 4 × 3. Ahora, si se desea borrar el primer rengl´on 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´ on A(1,1)=[] es inv´alida. 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´ on. Por ejemplo, para borrar el primer elemento de A se emplea la siguiente expresi´on fx >>

A(1:1)=[] ← A= 0 0 0

1 0 0 0 1

N´otese que los elementos restantes se quedan arreglados en la forma de un vector

Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

1.5

37

Matrices y arreglos

rengl´on. 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´ on de escalares Los escalares pueden ser combinados con algunas operaciones con matrices. Por ejemplo, en la sustracci´on 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´on A-3.3 no produce error, es una operaci´ on v´ alida de acuerdo a la sintaxis del lenguaje de MATLAB. Adem´as 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´on y multiplicaci´on 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´ on de escalares, MATLAB asigna un escalar espec´ıfico a todos los ´ındices para el rango indicado en la matriz. Por ejemplo, la siguiente expresi´on genera una matriz de dimensi´on 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´on solo asigna el valor del escalar 9 a ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

38

Cap´ıtulo 1: Conceptos b´ asicos

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´on 3 × 3 donde todos sus elementos son iguales a 9. Operaciones b´ asicas con matrices Hay varias operaciones elementales entre matrices que se realizan con frecuencia en mecatr´onica y rob´ otica. Hay que considerar que se deben respetar las reglas del ´algebra de matrices. Por ejemplo, se debe satisfacer la compatibilidad entre el n´ umero 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´on se realiza con: A-B

4) La multiplicaci´on: A*B (el n´ umero de columnas de la matriz A debe ser igual al n´ umero de renglones de la matriz B).

5) En general la multiplicaci´ on no es conmutativa: B*A (el n´ umero de columnas de la matriz B debe ser igual al n´ umero de renglones de la matriz A). Cuando las matrices son diagonales, entonces se satisface la propiedad de conmutatividad de la multiplicaci´on.

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

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

1.5

39

Matrices y arreglos

Debido a que las expresiones de programaci´on 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´a instrucciones, variables, operadores, comentarios, funciones, etc. De esta forma el archivo queda grabado en el directorio del usuario y se puede reproducir sin ning´ un problema. Por lo tanto, de aqu´ı en adelante ser´ a de utilidad emplear el editor de texto que se encuentra integrado en el ambiente de programaci´ on de MATLAB. ♣ Ejemplo 1.1 Escribir un programa en MATLAB para realizar operaciones b´ asicas entre matrices (adici´on , sustracci´on, inversa, sim´etrica, antisim´etrica, 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´ on El cuadro de c´ odigo fuente 1.1 contiene las operaciones m´as importantes que se realizan en mecatr´onica y rob´ otica usando matrices. Edite el programa que se muestra a continuaci´on; se recomienda salvarlo como cap1 matrices1.m; y ejec´ utese 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 ;. ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

40

Cap´ıtulo 1: Conceptos b´ asicos

C´ odigo Fuente 1.1

Operaciones b´ asicas con matrices

otica y Mecatr´ onica %MATLAB Aplicado a Rob´ %Cap´ ıtulo 1 Introducci´ on %Editorial Alfaomega Fernando Reyes Cort´ es %Archivo cap1 matrices1.m Operaciones b´ asicas 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´on 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´etrica Ask=(A-A’)/2; % parte antisim´etrica As+Ask % matriz original A A’*A %obtiene una matriz sim´etrica 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

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

1.5

41

Matrices y arreglos

1.5.1 1.5.1. Arreglos Las operaciones aritm´eticas con arreglos o registros son realizadas elemento por elemento. Es decir, la adici´on y sustracci´on son las mismas para arreglos y matrices, pero la multiplicaci´ on es diferente. MATLAB usa como parte de la notaci´on de multiplicaci´on 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´ on

-

Multiplicaci´ on elemento por elemento

.*

Divisi´ on elemento por elemento

./

Divisi´ on 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=

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

42

Cap´ıtulo 1: Conceptos b´ asicos

1

9

16 25 obs´ervese 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´as operadores: fx >>

A./A ← ans= 1 1 1 1

fx >>

A.∧ A ← ans= 1

27

256 3125 Las operaciones con arreglos son muy u ´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

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

1.6

43

Gr´ aficas

0

0

0

1

1

1

2

4

64

3

9

729

4 16

4096

5 25 15625

1.6 Gr´ aficas

L

a representaci´on gr´ afica y visualizaci´on de resultados es una herramienta muy importante en el an´ alisis y dise˜ no de esquemas de control y comportamiento din´ amico. Permite exhibir como el robot o el sistema mecatr´onico responde en sus etapas transitoria y estacionaria cuando se encuentra bajo la acci´ on de control de una estructura matem´ atica determinada. MATLAB tiene varios tipos de gr´aficas (plots): incluye desde la est´andar con ejes lineales, logar´ıtmicas y semi-logar´ıtmicas, barras, polares, en el espacio tridimensional (3D), contornos de superficies entre otras. Las gr´aficas se pueden formatear para tener una apariencia espec´ıfica, es decir con un tipo de l´ınea y colores. Tambi´en es posible utilizar una sola ventana con varias gr´aficas, as´ı como t´ıtulos y letreros sobre los ejes. Para inicializar ventanas espec´ıficas donde se despliegue resultados gr´aficos se emplea la funci´ on figure; la cual crea figuras y objetos gr´ aficos, para ser utilizados por funciones como plot, subplot, plot3, mesh, etc. La informaci´ on completa y detallada de las funciones gr´ afica se puede consultar en helpwin, help plot, help figure, etc. Sin embargo, a continuaci´on se describe algunas funciones que de manera particular se emplean en rob´ otica y mecatr´onica. plot

La funci´on plot es utilizada para generar una gr´afica en dos dimensiones: plot(t,x) ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

44

Cap´ıtulo 1: Conceptos b´ asicos

Los argumentos t,y son vectores (arreglos de una dimensi´on), ambos vectores deben tener el mismo n´ umero de elementos o dimensi´on. Cuando el comando plot se ejecuta genera autom´aticamente 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´ on plot tambi´en se puede graficar diagramas de fase, respuesta transitoria y estacionaria de robots manipuladores. De esta forma en forma gr´ afica es m´as f´ acil la interpretaci´on de las variables de estado. ♣ Ejemplo 1.2 Escribir un programa en MATLAB para graficar la curva de la siguiente funci´ on: y = 9x5 + 3 sen(x3 ) para el intervalo x ∈ [−10, 10].

Soluci´ on El c´odigo fuente que se presenta en el cuadro 1.2 contiene la forma de obtener la evoluci´ on en el tiempo de una funci´ on 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´on con desplazamiento de una mil´esima. La funci´on a graficar y = 9x5 +3 sen(x3 ) se encuentra implementada con operaciones de arreglos. En esta parte del c´ odigo se destaca la ventaja que ofrece emplear operaciones con arreglos, debido que la variable x es definida como un vector de un rengl´ on y n columnas, entonces la forma m´as adecuada de realizar las operaciones con los exponentes de la funci´ on y es a trav´es de la manipulaci´ on de datos usando arreglos. Finalmente, en la l´ınea 5 se gr´afica la funci´on y = 9x5 + 3 sen(x3 ) por medio de plot(x,y) tal y como se muestra en la figura 1.5. Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

1.6

45

Gr´ aficas

C´ odigo Fuente 1.2

Gr´ afica de la funci´ on 9x5 + 3 sen(x3 )

otica y Mecatr´ onica %MATLAB Aplicado a Rob´ %Cap´ ıtulo 1 Introducci´ on %Editorial Alfaomega Fernando Reyes Cort´ es %Archivo cap1 grafica.m Gr´ afica de la funci´ on 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´ afica de 9x5 + 3 sen(x3 ). Mayor informaci´ on de la funci´on plot se puede consultar en: fx >>

help plot ←

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

46

Cap´ıtulo 1: Conceptos b´ asicos

fplot El comando fplot grafica una funci´ on 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´ on a graficar. Puede ser escrita como una cadena de caracteres entre comillas. La funci´on puede ser propia de MATLAB (builtin) o definida por el usuario (M-file). La funci´on no puede incluir variables previamente definidas. limits: puede ser de dos formas: la primera forma incluye un vector que especifica el incio y t´ermino del intervalo a graficar (el dominio de x): [xmin, xmax]. La segunda forma cosiste de 4 par´ametros [xmin,xmax,ymin,ymax], es decir los 2 primeros par´ametros indican el dominio de x, mientras que los 2 u ´ltimos par´ametros 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´ on: y = 9x5 + 3 sen(x3 ) x ∈ [−10, 10].

Soluci´ on El programa 1.3 contiene el c´odigo para obtener la gr´ afica de la funci´on solicitada, reproduciendo la misma gr´ afica de la figura 1.5. Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

1.6

47

Gr´ aficas

C´ odigo Fuente 1.3

Gr´ afica de ’9*x∧ 5+3* sen(x∧ 3)’

otica y Mecatr´ onica %MATLAB Aplicado a Rob´ %Cap´ ıtulo 1 Introducci´ on %Editorial Alfaomega Fernando Reyes Cort´ es %Archivo cap1 exponencial.m Gr´ afica 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´on subplot(m,n,p) divide una ventana en una matriz de m × n, p indica la posici´ on de la gr´ afica dentro de esa ventana. plot3 La funci´on plot3(x,y,z) grafica las coordenadas en el espacio tridimensional. mesh La funci´on mesh genera superficies en el espacio tridimensional especificadas por las coordenadas x, y, z. Un aspecto interesante de esta funci´on es que la superficie de la funci´ on 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´ on ezmesh. ♣ Ejemplo 1.4 Escribir un programa en MATLAB para graficar funciones en 3D Soluci´ on El programa que se presenta en el cuadro 1.4 muestra la forma de usar las funciones ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

48

Cap´ıtulo 1: Conceptos b´ asicos

3D. Con el uso de la funci´ on figure (l´ınea 3) se abre una ventana para indicar las gr´ aficas generadas con subplot(2,2,1)...subplot(2,2,4). Observe la diferencia de presentaci´ on entre las funciones surf(x,y,z) y plot3(x,y,z); la primera tiene un aspecto de s´olido, mientras que la segunda usa l´ıneas suaves (ver figura 1.6).

C´ odigo Fuente 1.4

Funciones 3D.

%MATLAB Aplicado a Rob´ otica y Mecatr´ onica %Cap´ ıtulo 1 Introducci´ on %Editorial Alfaomega Fernando Reyes Cort´ es %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´ aficas en 3D. Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

1.7

Funciones

49

1.7 Funciones Una de las principales caracter´ısticas de MATLAB es que provee un n´ umero muy grande de funciones matem´ aticas. La lista completa de funciones matem´ aticas se puede consultar en la ventana de comandos escribiendo: fx >>

help elfun ←

Una lista de funciones matem´ aticas avanzadas se puede ver en: fx >>

help specfun ←

fx >>

help elmat ←

La gran mayor´ıa de las funciones matem´aticas acepta como argumento n´ umeros imaginarios, de esta forma cuando se calcula por ejemplo la ra´ız cuadrada o logaritmo de un n´ umero negativo, no produce un error, autom´ aticamente produce un n´ umero complejo o imaginario. Algunas funciones tales como sqrt, sin, cos son funciones propias (built in) de MATLAB ya que contienen un c´ odigo muy eficiente, entonces no se encuentran disponibles los detalles computacionales y c´ odigo fuente. Otras funciones como gamma, sinh son implementadas en archivos (M-files), por lo que el c´odigo fuente est´a disponible, y adem´ as se puede modificar. Las funciones pueden generar o retornar infinity cuando ocurre una divisi´on entre cero o tambi´en por generar un c´ alculo matem´atico m´ as all´a del l´ımite permitido por la variable realmax, lo cual se le conoce t´ecnicamente 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

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

50 fx >>

Cap´ıtulo 1: Conceptos b´ asicos 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´on original sin puede ser restaurada con clear sin Funciones en l´ınea Cuando se requiere simples c´alculos matem´aticos pueden ser usarse las funciones en l´ınea (inline functions). La funciones en l´ınea pueden ser generadas con el comando inline(’expresi´ on matem´ atica 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´ on z(x) se puede evaluar num´ericamente fx >>

z(2) ← ans 0.4000

Otra opci´on para evaluar funciones en l´ınea es por medio del comando feval fx >>

feval(’sqrt’, 100) ← ans 10

Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

1.7

51

Funciones

El comando feval tambi´en puede evaluar funciones definidas por el usuario. Cuando se requiere de complicadas funciones matem´aticas, programaci´ on 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´es de la variable de salida. Las funciones archivo tienen una estructura sint´ actica muy bien definida. Emplean la palabra reservada function para indicar que es una funci´ on. En seguida, se especifica el nombre de la variable de salida (por ejemplo w), seguida del signo =, a continuaci´ on del nombre de la funci´ on y entre par´entesis los argumentos de entrada separados por comas. Posteriormente viene el grupo de instrucciones que contenga la funci´on, 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´ on se ilustra en el c´odigo 1.1. Estructura de c´ odigo 1.1 Sintaxis de una funci´ on 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´ on 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´on y verificar que tenga extensi´on m. ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

52

Cap´ıtulo 1: Conceptos b´ asicos

C´ odigo Fuente 1.5

norma euclidiana de un vector

otica y Mecatr´ onica %MATLAB Aplicado a Rob´ %Cap´ ıtulo 1 Introducci´ on %Editorial Alfaomega Fernando Reyes Cort´ es %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´otese que se utiliza la palabra reservada function para definir una funci´ on, despu´es se indica la variable donde regresa el valor (en este caso es la variable y), posteriormente contin´ ua el signo igual (=) y el nombre de la funci´ on definida por el usuario normaev entre par´entesis el argumento de entrada x. Al finalizar el c´odigo se inserta la palabra clave end para delimitar el grupo de instrucciones que estar´a entre la palabras reservadas function y end. MATLAB autom´aticamente ajustar´ a 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´ on la naturaleza de la salida y se ajustar´a de manera adecuada. En el caso del c´odigo fuente del cuadro 1.5 el argumento puede ser un vector o matriz y la salida correspondiente es escalar. Algunas funciones pueden tener m´ as de un argumento de entrada y tambi´en retornar varias variables de salida, como el caso que a continuaci´ on se presenta en el cuadro 1.6. Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

1.7

53

Funciones

C´ odigo Fuente 1.6

cinem´ atica directa robot de 2 gdl

%MATLAB Aplicado a Rob´ otica y Mecatr´ onica %Cap´ ıtulo 1 Introducci´ on %Editorial Alfaomega Fernando Reyes Cort´ es %Archivo cinematica directa robot2gdl.m cinem´atica 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´ on anidada (nested function) es aquella funci´on que se encuentra como parte del c´ odigo de cualquier funci´ on archivo (M-file function). En el siguiente ejemplo la funci´ on B est´a anidada en la funci´ on A: Estructura de c´ odigo 1.2 Funci´ on 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´on son locales por naturaleza, esto significa que s´ olo en esa funci´ on existen y no pueden ser reconocidas por alguna otra funci´ on. Aun cuando dos funciones tengan variables con el mismo nombre, dichas variables s´ olo son reconocidas por sus respectivas funciones. ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

54

Cap´ıtulo 1: Conceptos b´ asicos

Sin embargo, es posible lograr que una o m´as 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´on: global X Y Z

Las variables han sido declaradas globales en cada funci´ on que el usuario quiere que sean reconocidas. Las variables ser´an comunes s´olo en esas funciones. El comando global debe aparecer antes que la variable sea usada. Es recomendable usar la declaraci´on para ese comando al inicio del archivo. Las variables globales est´an 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´ usculas o alg´ un nombre que el usuario puede identificar f´acilmente para las variables globales con la finalidad de no confundirlas con variables regulares o locales. Comparaci´ on 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´on las principales caracter´ısticas entre ellos:

Ambos archivos: script y funciones son salvados con la misma extensi´ on .m por lo cual se conocen como M-files. La primera l´ınea de c´odigo en el archivo funci´on es la l´ınea de definici´on de una funci´ on. Ejemplo: function y=reloj(t) Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

1.7

55

Funciones Las variables dentro de una funci´ on 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´es de la entrada de sus argumentos y pueden retornar datos por medio de sus variables de salida. Cuando una funci´ on archivo es salvada, entonces el nombre del archivo es el mismo que el nombre de la funci´on. Los nombres de los archivos script pueden tener cualquier nombre ya que no tienen funciones.

Usando una funci´ on archivo Una funci´ on definida por el usuario en un archivo funci´ on (M-file function) se usa en la misma forma que una funci´on propia (built-in function) de MATLAB. Esto significa que la funci´on puede ser llamada desde la ventana de comandos (Command Window), desde un archivo script o desde cualquier funci´ on. Si la funci´ on del usuario se encuentra en alg´ un directorio diferente al de trabajo de MATLAB, entonces debe ser habilitado dicho directorio con la opci´ on add to path (ver p´agina 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´on del c´ırculo en una funci´ on.m; utilizar variables globales para almacenar la coordenada z. ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

56

Cap´ıtulo 1: Conceptos b´ asicos

Soluci´ on La funci´on c´ırculo se encuentra descrita en el cuadro 1.7, los par´ ametros de entrada de esta funci´ on 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´ on 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´ odigo Fuente 1.7

Funci´ on c´ırculo

%MATLAB Aplicado a Rob´ otica y Mecatr´ onica. %Editorial Alfaomega,

Fernando Reyes Cort´ es.

%Cap´ ıtulo 3 Cinem´ atica funci´ on circulo.m Funci´ on 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´on 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´ on, puesto que no emplea la palabra reservada function, entonces se puede salvar el c´odigo de este programa con cualquier nombre, a˜ nadiendo la extensi´on 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´ u superior del editor de texto. Las gr´ aficas 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

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

1.7

57

Funciones

C´ odigo Fuente 1.8

Programa principal del c´ırculo

%MATLAB Aplicado a Rob´ otica y Mecatr´ onica %Cap´ ıtulo 1 Introducci´ on %Editorial Alfaomega Fernando Reyes Cort´ es %archivo circulo simu.m Programa principal del c´ırculo 1 2 3

clc; clear all; close all; %limpia a´rea de memoria format short

6

global altura % declaraci´ on de la variable global ti=0; %tiempo inicial h=0.001; %incremento de una mil´esima de segundo tf = 10; %tiempo final

7

t=ti:h:tf; %vector tiempo

8

%se invoca a la funci´ on c´ırculo [x, y]=circulo(0.3,0.3,0.25,t); subplot(2,2,1); plot(t,x) % gr´ afica de x vs t

4 5

9 10 11 12

subplot(2,2,2); plot(t,y) %gr´ afica 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.

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

58

Cap´ıtulo 1: Conceptos b´ asicos

1.8 Programaci´ on

U

n programa es una secuencia de instrucciones, expresiones, funciones, comandos y declaraciones para realizar aplicaciones en ingenier´ıa mecatr´onica y rob´ otica. 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´ ogicos como los que se presentan en la tabla 1.7. A trav´es de ellos se eval´ ua expresiones para realizar un cierto bloque de c´ odigo o funciones espec´ıficas de la aplicaci´ on implementada. Tabla 1.7 Operadores l´ ogicos Igual a

==

No es igual a

˜=

Menor que

<

Mayor que

>

Menor que o igual que

=

El lenguaje de programaci´on MATLAB es muy rico en instrucciones y funciones toolbox, ya que cuenta con un amplio compendio de librer´ıas o funciones para diversas ´areas de ingenier´ıa y ciencias exactas. A continuaci´ on se describen las instrucciones condicionales y lazos de programas como if, for, while, switch que permiten programar una gran cantidad de aplicaciones en rob´otica y mecatr´onica. Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

1.8

59

Programaci´ on

Instrucciones condicionales Una instrucci´ on condicional permite a MATLAB realizar decisiones para ejecutar un grupo de funciones, comandos, etc. Si la condici´ on en la instrucci´on condicional es verdadera, entonces realiza un grupo de expresiones del programa. Si la condici´on es falsa, entonces no ejecuta o salta ese grupo de expresiones.

1.8.1 1.8.1. if La instrucci´on if eval´ ua si una expresi´ on l´ ogica es verdadera, entonces realiza un conjunto de instrucciones o declaraciones del programa, de otra forma realiza la ejecuci´on de otro bloque del programa. La estructura de esta instrucci´on condicional consta de la palabra clave if, un conjunto de instrucciones o bloque de programa delimitadas por la palabra clave end. Si la condici´ on l´ ogica que se eval´ ua por la instrucci´ on if es falsa, el programa salta ese bloque de instrucciones y contin´ ua con las instrucciones que se encuentran justo despu´es de end. La estructura de la instrucci´ on condicional if se muestra en el c´odigo 1.3.

Estructura de c´ odigo 1.3 Sintaxis de la instrucci´ on 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´on if. Es importante resaltar que en el entorno de programaci´on de MATLAB las palabras claves if end aparecen en color azul. Adem´ as, las expresiones que aparecen como el bloque de instrucciones de la instrucci´ on if aparecen como texto con sangr´ıa ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

60

Cap´ıtulo 1: Conceptos b´ asicos

Figura 1.8 Diagrama de flujo de la instrucci´ on if. (indented), lo que resulta una f´ acil lectura del c´odigo de programaci´ on.

1.8.2 1.8.2. if, else, elseif La instrucci´on if puede tener m´ as 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´odigo 1.4: Estructura de c´ odigo 1.4 Sintaxis de if...else...end if condici´ on verdadera grupo de instrucciones; else grupo de instrucciones; end Adem´as de la opic´ on else en la instrucci´ on if...end, tambi´en se puede combinar con la palabra clave elseif; en este caso incluye una declaraci´on condicional adicional para la ejecuci´ on de m´ as grupos de instrucciones. Si en la instrucci´ on if la condici´ on es verdadera, entonces el programa ejecuta el grupo (1) de instrucciones Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

1.8

Programaci´ on

61

y pregunta por una nueva expresi´ on 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´odigo 1.5:

Estructura de c´ odigo 1.5 Sintaxis de if...elseif...else...end if condici´ on verdadera if grupo de instrucciones 1; %grupo(1) elseif condici´ on 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´ on if determinado por el delimitador end. La figura 1.9 muestra el diagrama de flujo de la instrucci´on if...elseif...else...end.

1.8.3 1.8.3. for La instrucci´on for genera lazos repetitivos (flujos o ciclos de instrucciones) para repetirse un n´ umero determinado de veces. La instrucci´on for requiere de una variable contador para realizar n veces la iteraci´on, la condici´on de salida se alcanza cuando la variable contador ha llegado a la cuenta especificada. La sintaxis de la instrucci´ on for inicia con esa palabra clave, a continuaci´ on la inicializaci´on de una variable contador que determinar´ a el n´ umero de veces que realizar´ a el ciclo o flujo del grupo de instrucciones. Finalmente se incluye la palabra clave end para delimitar la instrucci´ on for. El c´odigo 1.6 presenta la estructura de la instrucci´ on for...end: ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

62

Cap´ıtulo 1: Conceptos b´ asicos

Figura 1.9 Diagrama de flujo de la instrucci´ on if...elsefi...end.

Estructura de c´ odigo 1.6 Sintaxis de la instrucci´ on for for contador=1:n instrucci´ on 1; ..............; instrucci´ on n; end Como un ejemplo sencillo para mostrar la utilidad de la instrucci´ on for...end se presenta en el c´odigo 1.7 un programa que despliega 10 veces el contador i y la variable j que contiene el valor de una funci´on trigonom´etrica. Estructura de c´ odigo 1.7 Ejemplo de iteraciones con la instrucci´on for j=0; for i=1:10 disp(i,j) j=sin(2*pi*i); end Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

1.8

63

Programaci´ on

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´ on disp(i,j) despliega los valores num´ericos de las variables i,j. El siguiente ejemplo muestra el empleo de for usando incrementos en mil´esimas para la variable t por medio del operador colon :. Esta aplicaci´on de la instrucci´on for es muy u ´til para graficar funciones como es el caso de la funci´ on seno dentro de un intervalo de tiempo. ♣ Ejemplo 1.6 Escribir un programa en MATLAB para graficar una se˜ nal senoidal.

Soluci´ on El programa 1.9 presenta el c´odigo fuente en MATLAB para graficar una se˜ nal senoidal. En la l´ınea 3 se inicializa el vector tiempo de 0 a 10 segundos con incrementos de una mil´esima de segundo. El ciclo iterativo de la instrucci´on for abarca las l´ıneas 3 a la 7. El lector puede realizar el paso del tiempo de simulaci´on m´as fino, por ejemplo en diezmil´esimas. En la figura 1.10 se muestra la gr´afica que genera el programa 1.9.

Figura 1.10 Funci´on senoidal. ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

64

Cap´ıtulo 1: Conceptos b´ asicos

C´ odigo Fuente 1.9

Gr´ afica de una onda senoidal

otica y Mecatr´ onica %MATLAB Aplicado a Rob´ %Cap´ ıtulo 1 Introducci´ on %Editorial Alfaomega Fernando Reyes Cort´ es %Archivo senoide.m Gr´ afica 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´ esimas 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´on. Como una aplicaci´ on de este conjunto de instrucciones se presenta la implementaci´on de la funci´ on signo la cual es ampliamente utilizada en din´ amica y esquemas de control de sistemas mecatr´onicos y robots manipuladores; espec´ıficamente se emplea en el modelado del fen´omeno de fricci´ on de Coulomb. Tambi´en se usa en algunos algoritmos de control. Una de las principales desventajas de la funci´ on signo es su discontinuidad que presenta y debido a esta caracter´ıstica muy particularidad produce retardo en el proceso de integraci´on num´erica de sistemas din´amicos, por lo que no se recomienda el emplear de esta funci´on cuando se realice simulaci´on de la din´ amica de robots manipuladores. Una posible forma de implementar la funci´ on signo es por medio del uso de for y if elseif else end, el cuadro 1.10 presenta la funci´on signo ( ). Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

1.8

65

Programaci´ on

C´ odigo Fuente 1.10

Funci´ on signo

otica y Mecatr´ onica %MATLAB Aplicado a Rob´ %Cap´ ıtulo 1 Introducci´ on %Editorial Alfaomega Fernando Reyes Cort´ es %Archivo signo.m Funci´ on 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´ on signo() para realizar dicha conversi´ on). Soluci´ on Una forma de convertir una onda senoidal del tipo sen(t) a se˜ nal cuadrada (tren de pulsos cuadrados) es por medio del uso de la funci´on signo(x). Considere el programa 1.11; el tiempo de simulaci´ on es de 0 a 10 segundos, en la l´ınea 5 se obtiene la onda senoidal y la conversi´ on a onda cuadrada se realiza en la l´ınea 6. La figura 1.11 contiene la conversi´ on de la onda senoidal a onda cuadrada. ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

66

Cap´ıtulo 1: Conceptos b´ asicos

C´ odigo Fuente 1.11

Aplicaci´ on de la funci´ on signo

otica y Mecatr´ onica %MATLAB Aplicado a Rob´ %Cap´ ıtulo 1 Introducci´ on %Editorial Alfaomega Fernando Reyes Cort´ es %Archivo signo simu.m Aplicaci´on de la funci´on signo 1 2 3

clc; clear all; close all;

6

format short ti=0;h=0.001; tf = 10; %par´ ametros del tiempo de simulaci´on t=(ti:h:tf)’; %vector columna de tiempo y=sin(t); % onda senoidal y1=signo(y); % funci´ on signo

7

plot(t,y1)

4 5

Figura 1.11 Funci´on signo(). Debido a que la funci´on signo est´a implementada bajo la estructura function es necesario realizar un programa principal donde se pueda invocar con el adecuado pase de par´ ametros y su correcta sintaxis. Esta es la tarea que realiza el programa principal 1.11 Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

1.8

Programaci´ on

67

Tambi´en 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´ alculos iterativos o recursivos, simulaci´ on de sistemas din´amicos, procesamiento de se˜ nales, etc. El c´odigo 1.8 presenta la sintaxis gramatical para el caso de tener instrucciones for anidadas. Obs´ervese que el for externo realizar´ a un n´ umero 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´a realizado un n´ umero de veces indicado por el umero total de veces que se ejecutar´a el grupo (2) de instrucciones contador 2. El n´ 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´ on for externa abarca el grupo (1) de instrucciones, as´ı como el for interno con su segundo grupo de instrucciones. Por lo tanto, est´an delimitados por el for externo y la palabra clave end. Mientras que para el segundo for el segundo grupo de instrucciones est´a delimitado por la palabra clave end correspondiente a la instrucci´ on for del contador 2. Estructura de c´ odigo 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´on 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´es del pivote j. ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

68

Cap´ıtulo 1: Conceptos b´ asicos

♣ Ejemplo 1.8 Escribir un programa en MATLAB para sumar dos matrices A, B ∈ IRn×p en forma recursiva.

Soluci´ on El c´odigo fuente para sumar matrices en forma recursiva se presenta en el programa 1.12:

C´ odigo Fuente 1.12

Algoritmo para sumar matrices

otica y Mecatr´ onica %MATLAB Aplicado a Rob´ %Cap´ ıtulo 1 Introducci´ on %Editorial Alfaomega Fernando Reyes Cort´ es %Archivo sumamatrices.m Algoritmo para sumar matrices 1

disp(’Realiza la suma de dos matrices de orden n’)

2

%Inicializaci´ on de las matrices A, B dimensi´ on 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

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

1.8

69

Programaci´ on

♣ Ejemplo 1.9 Escribir un programa en MATLAB para encontrar el m´ınimo y m´ aximo de la siguiente funci´on: y =

sen(t) − cos(πt) − 2 tanh(t) + 0.1 log(t3 + 1)

determinar los tiempos de ocurrencia. Soluci´ on Para buscar el m´ aximo y m´ınimo de la funci´ on indicada (ver figura 1.12) y tiempos de ocurrencia puede usarse el c´odigo del programa 1.13.

Figura 1.12 B´ usqueda de m´ınimo y m´ aximo. La salida del programa tiene el siguiente resultado:

Max Tmax Min Tmin

0.3887 0.9800 −3.3539 4.0600 ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

70

Cap´ıtulo 1: Conceptos b´ asicos

C´ odigo Fuente 1.13

M´ınimos y m´ aximos

otica y Mecatr´ onica %MATLAB Aplicado a Rob´ %Cap´ ıtulo 1 Introducci´ on %Editorial Alfaomega Fernando Reyes Cort´ es archivo cap1 minimax.m M´ınimos y m´aximos 1 2

clc; clear all; close all; disp(’Encuentra m´ınimo y m´aximo de una funci´ on’)

3

%variables para registrar m´ aximo y m´ınimo

4

max=0; min=0; t min=0; t max=0;

5 7

%generaci´on 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´on del arreglo

8

y=zeros(n,m); %variables de registro

9 11

%funci´ on de prueba para generar la gr´ afica 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´ afica de la funci´ on 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)i i=i+1; b=b-i; if b>a disp(’Robot activo’) else disp(’Proceso desactivo’) end end ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

72

Cap´ıtulo 1: Conceptos b´ asicos

1.8.5 1.8.5. switch, case La instrucci´on switch ejecuta un grupo de sentencias con base al valor de la variable o expresi´on que emplea al lado derecho de su sintaxis. Para dividir los grupos de sentencias se emplean las palabras claves case y othewise. La instrucci´on switch finaliza con end. En el siguiente c´odigo se ilustra la forma en que funciona la instrucci´on switch: Estructura de c´ odigo 1.11 Sintaxis de la instrucci´ on 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´ ua 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´ on switch cuando encuentra el caso 2 y contin´ ua 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

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

1.8

Programaci´ on

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´on de salida es una constante positiva, el lazo se generar´ a de manera infinita. Sin embargo, la condici´ on de salida queda determinada por if t==0.0 y break. En este ejemplo se realiza la (t) operaci´on sen , la variable t se inicializa en -1 y se decrementa en 0.001 en cada t iteraci´on. Cuando t=0 se evita realizar una divisi´on entre cero usando break.

Estructura de c´ odigo 1.12 Forma de usarse la instrucci´on break t=-10; while 1 t=t+1; % c´ odigo correspondiente para detectar cruce por cero if t==0 disp(’Advertencia divisi´ on entre cero’) break; end x= sin(t)/t; end disp(x) Tambi´en es posible usar break en lazos anidados.

1.8.7 1.8.7. return La expresi´on return termina la secuencia actual de comandos y retorna el control a la funci´ on que invoc´ o. Una llamada a funci´ on normalmente transfiere el control a la funci´ on que la invoc´ o cuando alcanza el fin de la funci´ on. Es posible insertar una sentencia return para forzar la terminaci´ on de la funci´on y transferir el control a la funci´ on que realizo la llamada. ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

74

Cap´ıtulo 1: Conceptos b´ asicos

Estructura de c´ odigo 1.13 Forma de usarse la instrucci´on return function y =cap1 raiz(x) if x n el n´ umero de ecuaciones es mayor que el n´ umero de inc´ognitas se le conoce como problema sobre determinado. Para el caso m = n, la matriz A es cuadrada, entonces la soluci´on se encuentra dada por: x = A−1 y.

(2.4)

La soluci´on existe y es u ´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´odigo 2.1: Estructura de c´ odigo 2.1 Caso 1: m = n if

det(A)~=0 x=inv(A)*y;

end El c´odigo 2.1 no es u ´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´on emplea eliminaci´on gausiana, y por lo tanto no se utiliza la forma tradicional de obtener la inversa. Usando la funci´ on: x=linsolve(A,y). Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

2.2

89

Sistemas de ecuaciones lineales

linsolve La funci´on linsolve representa una opci´ on para resolver un sistema lineal de ecuaciones de la forma 2.3. La sintaxis de la funci´ on linsolve est´a dada como: y=linsolve(A,x) donde A es la matriz del sistema de ecuaciones lineales y x es el vector inc´ognita. Mayor informaci´ on de la funci´on 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´ on del sistema x y compararla con los m´etodos: x = inv(A)y, x = A−1 y y x = A\y

Soluci´ on Para el sistema de ecuaciones planteado se tiene que la matriz A y el vector y est´an dados como: ⎡ ⎤ ⎡ ⎤ 1 4 3 3 ⎢ ⎥ ⎢ ⎥ A = ⎣8 5 9⎦ y = ⎣2⎦. 3 2 4

4

El cuadro 2.3 contiene el c´odigo fuente para resolver el problema planteado y donde se han implementado los m´etodos solicitados. ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

90

Cap´ıtulo 2: M´ etodos num´ ericos

C´ odigo Fuente 2.3

Comparaci´ on de soluciones SLE

otica y Mecatr´ onica %MATLAB Aplicado a Rob´ %Cap´ ıtulo 2 M´ etodos num´ ericos %Editorial Alfaomega %Fernando Reyes Cort´ es %cap2 sle Comparaci´ on 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´ on

7

x=inv(A)*y;

8

% segunda soluci´ on ∧ x1=A (-1)*y;

4

9 10 11 12 13 14

% tercera soluci´on x2=A\y; % cuarta soluci´ on x3= linsolve(A,y);

15

% comparaci´on de todas las soluciones disp(’ x x1 x2 x3 ’)

16

disp([x x1 x2 x3])

La salida del programa cap2 sle arroja resultados id´enticos: 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

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

2.2

91

Sistemas de ecuaciones lineales

2.2.1 2.2.1. Regla de Cramer La regla de Cramer representa otra opci´on para encontrar la soluci´ on a sistemas de ecuaciones lineales de la forma (2.3). El procedimiento de la regla de Cramer consiste en obtener la soluci´ on para cada componente xi del vector x a trav´es de calcular el determinante i-´esimo obtenido como por sustituir el vector y en la i-´esima 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) ⎣ . ⎦ ⎣ . ⎦

deti =

xn      a11    a21  a n1

detn columnai  .. ··· . ···

y .. .

···

    · · · a1n   · · · a2n   · · · ann 

det(A)

para i = 1 · · · n.

♣ Ejemplo 2.3 Considere nuevamente el sistema (2.5) del ejemplo 2.2. Obtener la soluci´ on para x usando el m´etodo de Cramer.

Soluci´ on De acuerdo con el procedimiento de la regla de Cramer    1 4 3 1       4 5 9 8    6 2 4 3  = −5.8667 det2 =  det1 =   1 1 4 3     8 5 9 8    3 2 4 3

se establece que:  1 3   4 9  6 4  = −4.3333 4 3   5 9  2 4

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

92

Cap´ıtulo 2: M´ etodos num´ ericos

det3 =

 1   8  3  1   8  3

 4 1   5 4  2 6  = 8.0667. 4 3   5 9  2 4

Por lo tanto la soluci´on est´a dada por: ⎡ ⎤ ⎡ ⎤ ⎡ ⎤ x1 det1 −5.8667 ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ x = ⎣ x2 ⎦ = ⎣ det2 ⎦ = ⎣ −4.3333 ⎦ . 8.0667 x3 det3

2.3 Diferenciaci´ on num´ erica

D

´ n num´erica es la t´ecnica de aproximar a la derivada de una iferenciacio funci´ on. La derivada es una operaci´ on matem´ atica sobre funciones o variables de estado, y que tiene una diversidad de aplicaciones en ingenier´ıa. La derivada de una funci´ on representa la raz´ on de cambio con respecto al tiempo; se utilizan en esquemas de control y en procesos de automatizaci´on. Por ejemplo, en el ´area de control de posici´ on de robots manipuladores, los algoritmos incluyen un t´ermino denominado acci´ on de control derivativo con la finalidad de generar amortiguamiento o freno mec´anico y con dicha acci´on se pretende mejorar la respuesta transitoria del robot. Para lograr esto, se inyecta la velocidad de movimiento, la cual representa la variaci´ on temporal de la posici´on. La fricci´on viscosa y de Coulomb es uno de los fen´omenos f´ısicos que se encuentra presente en los sistemas mec´anicos, la derivada de la posici´ on forma parte de este fen´omeno. El modelado din´amico de sistemas mecatr´onicos y robots incluye derivadas de las variables de estado para conformar la estructura matem´atica adecuada para estudiar y analizar todos los fen´ omenos f´ısicos del sistema. La teor´ıa de estabilidad de sistemas din´amicos es otro ejemplo donde se utilizan derivadas; por ejemplo, la derivada de la energ´ıa (potencia) se emplea para llevar a cabo el an´alisis de estabilidad del punto de equilibrio. Hoy en d´ıa, la derivada se ha convertido en una herramienta imprescindible para realizar automatizaci´ on de sistemas.

Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

2.3

93

Diferenciaci´ on num´ erica

La notaci´on de la derivada de una funci´ on f (t) con respecto al tiempo es definida como f˙(t), la cual es igual a la raz´ on de cambio de f con respecto al tiempo t. Matem´aticamente la derivada de una funci´ on se define como: f˙(t) =

d f (t + t) − f (t) f (t) = l´ım t→0 dt t

(2.9)

donde t es un infinit´esimo intervalo de tiempo. En mecatr´ onica hay varios procesos donde se requiere medir la raz´on de cambio de las variables del robot. Por ejemplo la raz´ on de cambio de la posici´on es la velocidad de movimiento; el cambio temporal de la velocidad es la aceleraci´on. Por otro lado, la integral de la aceleraci´ on es la velocidad, y la integral de la velocidad es la posici´on. Es decir, existe una relaci´ on 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

t

t  df (t) ˙ f (t)dt = dt = df (t) = f (t) = f (t) − f (0) dt 0 0 0 0 retorna la funci´ on original f (t) m´as 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´ on original. Geom´etricamente, la derivada f˙ puede ser descrita como la pendiente de la l´ınea tangente en la funci´ on f (t) del punto espec´ıfico t. La l´ınea tangente est´a 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´ on temporal). Los puntos donde la derivada de la funci´ on f es cero se llaman puntos cr´ıticos y pueden representar regiones horizontales de la funci´ on f o puntos extremos (puntos que son m´aximo o m´ınimo local o tambi´en globales). Si evaluamos la derivada de la funci´ on en varios puntos y observamos que ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

94

Cap´ıtulo 2: M´ etodos num´ ericos

Figura 2.1 L´ınea tangente de la funci´ on f en el punto tk . el signo de la derivada cambia, entonces un m´ınimo o m´ aximo local ocurre en ese intervalo. La segunda derivada de la funci´ on f¨(t) determina cu´ando el punto cr´ıtico es m´ınimo o m´ aximo. Si la segunda derivada en un punto extremo es positiva, entonces el valor de la funci´ on en el punto extremo es un m´ınimo local. Por otro lado, si la segunda derivada de la funci´on en un punto extremo es negativo, entonces la funci´ on evaluada en el punto extremo es un m´aximo local (ver figura 2.2).

Figura 2.2 Puntos extremos: m´ınimo y m´ aximo local (global). Las t´ecnicas de diferenciaci´on num´erica estiman la derivada de una funci´ on f en un punto tk aproxima la pendiente de la l´ınea tangente en tk usando valores de la funci´ on 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

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

2.3

95

Diferenciaci´ on num´ erica

Figura 2.3 L´ınea tangente de la funci´ on 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´ ultiplos 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´etodo de Euler (diferenciaci´ on num´erica) f˙(tk )

f (tk ) − f (tk−1 ) f (tk ) − f (tk−1 ) = tk − tk−1 h

particularmente esta aproximaci´on es conocida como diferenciaci´on con un paso atr´ as (backward difference). Tambi´en se puede obtener la aproximaci´on de la derivada por computar la pendiente entre f (tk ) y f (tk+1 ) conocida como diferenciaci´on 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´etodos. 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´on por diferenciaci´ on num´erica de la velocidad: f (tk ) − 2f (tk−1 ) + f (tk−2 ) f˙(tk ) − f˙(tk−1 ) f¨(tk ) = = h h2 ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

96

Cap´ıtulo 2: M´ etodos num´ ericos

Figura 2.4 Diferenciaci´on num´erica: backward y forward.

♣ Ejemplo 2.4 Emplear el m´etodo de Euler para convertir la siguiente ecuaci´on diferencial en su equivalente expresi´ on por diferenciaci´on num´erica. ˙ + a0 y(t) αu(t) = a2 y¨(t) + a1 y(t)

(2.10)

Soluci´ on Empleando el m´etodo de Euler, la velocidad y aceleraci´on toman la siguiente forma: y(tk ) − y(tk−1 ) h y(tk )−y(tk−1 ) y(t )−y(t ) − k−1 h k−2 y(t ˙ k ) − y(t ˙ k−1 ) h y¨(t) y¨(tk ) = = h h y(tk ) − 2y(tk−1 ) + y(tk−2 ) y¨(tk ) = h2 y(t) ˙

y(t ˙ k) =

Por lo tanto la ecuaci´ on diferencial 2.10 queda como: ˙ k ) + a0 y(tk ) αu(tk ) = a2 y¨(tk ) + a1 y(t αu(tk ) = a2 [ Alfaomega

y(tk )−2y(tk−1 )+y(tk−2 ) h2

] + a1

y(tk ) − y(tk−1 ) + a0 y(tk ). h

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

2.3

97

Diferenciaci´ on num´ erica

La soluci´on para y(tk ) est´a 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´on discreta y(tk ) de la ecuaci´on (2.10) se requieren los estados y(tk−1 ) y y(tk−2 ) y el periodo de muestre h. El m´etodo de Euler es ampliamente utilizado para resolver ecuaciones diferenciales lineales, usando el procedimiento de este ejemplo se obtiene la soluci´on en forma discreta de sistemas din´ amicos lineales.

2.3.1 2.3.1. Funci´ on diff MATLAB tiene la funci´ on diff la cual realiza la diferenciaci´ on num´erica entre valores adyacentes de un vector x. La sintaxis de la funci´on diff es la siguiente: y= diff(x)

Retorna un nuevo vector y conteniendo la diferenciadiff

ci´ on entre valores adyacentes del vector de entrada x

La funci´on diff tambi´en se aplica a matrices, entonces opera en cada columna de la matriz, y retorna una matriz con el mismo n´ umero de columnas, en este caso la sintaxis es: B= diff(A)

Retorna una nueva matriz B conteniendo la diferendiff

ciaci´on entre valores adyacentes de cada columna de la matriz de entrada A.

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

98

Cap´ıtulo 2: M´ etodos num´ ericos

♣ Ejemplo 2.5 Aproximar la derivada con respecto al tiempo de la funci´on sen(t) mediante diferenciaci´on.

Soluci´ on Para obtener la derivada temporal por diferenciaci´ on num´erica de la funci´ on sen(t) se hace uso de diff. El intervalo de diferenciaci´ on num´erica se selecciona de 0 a 10 segundos, con incrementos de un milisegundo. El m´etodo anal´ıtico conduce d a la soluci´on aproximada con la soluci´ on dt sen(t) = cos(t). Por lo tanto, se comparar´ anal´ıtica. El programa que contiene el c´odigo 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´ aficas: por diferenciaci´ on num´erica y el m´etodo anal´ıtico.

Figura 2.5 Comparaci´ on del m´etodo anal´ıtico y por diferenciaci´on num´erica. Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

2.4

99

Integraci´ on num´ erica

C´ odigo Fuente 2.4

Diferenciaci´ on num´ erica

otica y Mecatr´ onica %MATLAB Aplicado a Rob´ %Cap´ ıtulo 2 M´ etodos num´ ericos %Editorial Alfaomega %Fernando Reyes Cort´ es %Archivo cap2 diffnum.m Diferenciaci´on num´erica 1 3

clc; clear all; close all;

4

% intervalo de tiempo

5

t=0:0.001:10;

6

% funci´ on a derivar f=sin(t);

2

7 8 9

% derivada de la funci´ on sen(t) con respecto al tiempo. ˙ % es decir se obtiene f(tk ) = df(tk ) dtk

10

dfdt = diff(f)./diff(t);

11

% el vector df tiene dimensi´on n-1 t1=0:0.001:9.999; % nueva base de tiempo con dimensi´ on n-1

12 13 14

%compara la derivada aproximada con el m´etodo anal´ıtico plot(t1,dfdt,t1,cos(t1))

2.4 Integraci´ on num´ erica

I

´ n num´erica es la t´ecnica de aproximar integrales de funciones. La ntegracio integraci´ on de funciones es un t´ opico especial y de inter´es en aplicaciones pr´ acticas para ingenier´ıa mecatr´onica y rob´ otica. Desde el punto de vista anal´ıtico y experimental las integrales representan una herramienta fundamental en el an´ alisis y dise˜ no de algoritmos de control, desarrollo y construcci´ on de robots manipuladores. La aplicaci´ on inmediata de integraci´on num´erica se encuentra en el dise˜ no de simuladores, la calidad de los resultados depende en gran medida de la exactitud ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

100

Cap´ıtulo 2: M´ etodos num´ ericos

para aproximar a una integral de funci´ on. 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´on para mejorar las caracter´ısticas operativas de la estrategia de control

t ˜ ˙ τ = Kpq (t) + Ki q˜(t) dt − Kv q(t) t0

La acci´on de control integral almacena energ´ıa (´ area bajo la curva del error de posici´on), 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´on 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´as bien es un c´ alculo continuo, uno tras otro como el tiempo evolucione. En contraste con otros m´etodos, el c´alculo de la integral s´ olo se requiere una vez, tal es el caso de la medici´on de desempe˜ no de reguladores usando la norma L2 [˜ q], cuya expresi´on matem´ atica est´ a dada por: 

1 T L2 [˜ q] =

˜ q(t) 2 dt T 0 donde T representa el tiempo de experimentaci´on o simulaci´ on (intervalo de integraci´ on). Una magnitud peque˜ na en la norma L2 significa alto desempe˜ no del esquema de control. Esto significa que el a´rea bajo la curva del error de posici´ on no fue significativa debido a que el robot se posicion´o de manera inmediata, logrando la convergencia hacia cero del error de posici´ on q˜(t) → 0. Por lo tanto, en el periodo de integraci´ on el transitorio fue r´ apido, sin sobretiros y el desempe˜ no del algoritmo de control es muy bueno. En contra parte, un alto valor de la norma L2 representa ´til cuando se compara varios algoritmos pobre desempe˜ no. La norma L2 es muy u de control, entonces se determina cu´al de ese conjunto de esquemas tiene mejores prestaciones y por lo tanto determina la selecci´on para una aplicaci´on espec´ıfica. Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

2.4

Integraci´ on num´ erica

101

La adecuada interpretaci´ on de resultados experimentales, anal´ıticos y funcionamiento cualitativo de estrategias de control requiere del buen entendimiento de t´opicos espec´ıficos de c´alculo integral y diferencial. M´ as a´ un, c´ omo implementar una integral, o seleccionar el m´etodo num´erico m´as adecuado son conocimientos fundamentales que se ven reflejados en la exactitud de posicionamiento de un robot manipulador y en el desempe˜ no del esquema de control. La integral definida de una funci´on f (x) sobre un intervalo finito [a, b] es interpretada como el ´area 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´etodos num´ericos para estimar su valor.

Figura 2.6 La integral de f (x) significa el ´area bajo la curva. Hay varias t´ecnicas para aproximar la integral de una funci´on f (x); la evaluaci´ on num´erica de una integral se le denomina cuadratura cuyo nombre proviene de un problema geom´etrico ancestral. A continuaci´on se presentan dos m´etodos geom´etricos ampliamente conocidos: m´etodo trapezoidal y regla de Simpson. ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

102

Cap´ıtulo 2: M´ etodos num´ ericos

2.4.1 2.4.1. Regla trapezoidal Cuando el a´rea bajo la curva de la funci´ on f (x) es representada por trapezoides y el intervalo [a, b] es dividido en n secciones del mismo tama˜ no, entonces el ´area de f (x) puede ser aproximada por la siguiente expresi´on:

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´ on trapz para aproximar la integral de una funci´ on f (x) por el m´etodo trapezoidal, cuya sintaxis es la siguiente: y=trapz(f)

y=trapz(x,f)

y=trapz(x,f,dim) La funci´on trapz(f) calcula la aproximaci´on num´erica de la integral de la funci´on f por el m´etodo 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´on con la integral sobre cada columna. Cuando la funci´ on 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´on de la funci´ on f. En ambos casos, el vector x debe tener la misma dimensi´on dim. Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

2.4

103

Integraci´ on num´ erica

♣ Ejemplo 2.6 Calcular la integral de la funci´ on

sen(x) por el m´etodo trapezoidal para el

intervalo x ∈ [0, π]

Itrapz =

π

sen(x) dx. 0

Soluci´ on El programa 2.5 contiene el c´odigo fuente para calcular la integral de la funci´on f (x) = sen(x) en el intervalo [0, π]. De la l´ınea 4 a la 7 se define el intervalo de π tiempo de integraci´ on. El incremento del paso de integraci´ on es cada 1000 . En la l´ınea 8 se define la funci´ on sen(x) y en la l´ınea 9 se realiza la integraci´on num´erica usando la funci´ on 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´ odigo Fuente 2.5

Integraci´ on num´ erica trapezoidal

%MATLAB Aplicado a Rob´ otica y Mecatr´ onica %Cap´ ıtulo 2 M´ etodos num´ ericos %Editorial Alfaomega %Fernando Reyes Cort´ es %Archivo cap2 trap.m Integraci´ on num´erica trapezoidal 1

clc;

2

clear all; close all; tini=0; tinc=pi/1000;

3 4 5

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

104

Cap´ıtulo 2: M´ etodos num´ ericos

C´ odigo Fuente 2.6

Integraci´ on num´ erica trapezoidal

otica y Mecatr´ onica %MATLAB Aplicado a Rob´ %Cap´ ıtulo 2 M´ etodos num´ ericos %Editorial Alfaomega %Fernando Reyes Cort´ es %Archivo cap2 trap.m Contin´ ua 2.5 Integraci´ on num´erica 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´etodo trapezoidal con el anal´ıtico. disp([I trapz I a])

8 9

14

En este ejemplo se ha calculado el ´area bajo la curva sobre el intervalo [0, π]. El c´alculo se realiza una sola vez. Sin embargo, hay aplicaciones en control de robots manipuladores donde se requiere realizar el c´ alculo de la integral de manera sistem´atica conforme el tiempo evoluciona. A continuaci´ on se muestra un procedimiento recursivo donde se obtiene la integral por el m´etodo trapezoidal para π peque˜ nos intervalos del tama˜ no de 1000 sobre un intervalo amplio [0, 2π]. ♣ ♣ Ejemplo 2.7 Calcular la integral de la funci´ on sen(x) por el m´etodo trapezoidal para cada valor de intervalo

π 1000

contenido en el intervalo [0, 2π]

Itrapz =

Alfaomega

2π

sen(x) dx. 0

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

2.4

Integraci´ on num´ erica

105

Soluci´ on π 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´odigo 2.7 contiene la forma de implementar el m´etodo trapezoidal para aproximar la integral de una funci´ on 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´ on. El incremento del paso de integraci´ on es cada 1000 . En la l´ınea 10 se obtiene la dimensi´on del vector de tiempo, con esa informaci´on se realizar´ a el algoritmo recursivo, es decir el n´ umero de veces (2000) que se realizar´a 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´ on del intervalo [0, 1000 ] cuyo c´ odigo abarca las l´ıneas 13 a la 16. Este c´ odigo es necesario por razones t´ecnicas 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´ on x=0:tinc:0, marcando un error de programaci´ on. En su lugar se realiza x=0:tinc:tinc. Para el segundo incremento del tiempo de integraci´ on 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´ on sen(x) para el correspondiente k-´esimo intervalo de integraci´ on pi pi on I trapz(k)=trapz(x,f). Este [(k − 1) 1000 , k 1000 ]. La l´ınea 24 tiene la funci´ procedimiento se realiza un n´ umero de veces (2000) hasta igualar la dimensi´ on del vector de tiempo t. En la l´ınea 28 se grafica la integral de la funci´ on sen(x) por el m´etodo trapezoidal y se compara con el m´etodo anal´ıtico:

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

106

Cap´ıtulo 2: M´ etodos num´ ericos

Itrapz =

2π

sen(x) dx 0

= − cos(x)|2π 0 = 1 − cos(x), x ∈ [0, 2π]

C´ odigo Fuente 2.7

M´ etodo de integraci´ on trapezoidal

%MATLAB Aplicado a Rob´ otica y Mecatr´ onica %Cap´ ıtulo 2 M´ etodos num´ ericos %Editorial Alfaomega %Fernando Reyes Cort´ es %Archivo cap2 trapezoidal.m % calcula la integral de la funci´ on sen(x) % el c´ alculo se realiza por medio de %integraci´ on iterativa (2000 iteraciones) %con incrementos de

π 1000

% para el intervalo [0, 2π]. M´etodo de integraci´ on trapezoidal 1 3

clc; clear all; close all;

4

% intervalo de integraci´on

5 7

tini=0; % tiempo inicial de integraci´on tinc=pi/1000; % pasos de incremento del tiempo de integraci´on tfinal=2*pi; % tiempo final de integraci´ on

8

% base de tiempo de integraci´ on

9

t=tini:tinc:tfinal; [m,n]=size(t); % dimensi´ on del vector de tiempo

2

6

10

Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

2.4

107

Integraci´ on num´ erica

C´ odigo Fuente 2.8

M´ etodo de integraci´ on trapezoidal

%Contin´ ua programa 2.7 otica y Mecatr´ onica %MATLAB Aplicado a Rob´ %Cap´ ıtulo 2 M´ etodos num´ ericos %Editorial Alfaomega %Fernando Reyes Cort´ es %Archivo cap2 trapezoidal.m M´etodo de integraci´ on 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´ on

16 17

% se integra desde 0 a x=0:tinc:tinc;

18

else

19

% para el segundo paso de integraci´on 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´ on a integrar I trapz(k)= trapz(x,f); % integraci´ on por m´etodo trapezoidal

23 24 25

end

26

% gr´afica comparativa entre los m´etodos

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´etodo trapezoidal y el anal´ıtico para la integral de la funci´ on sen(x) que genera el programa 2.7. Obs´ervese que el m´etodo trapezoidal es una aproximaci´on muy buena comparada con el exacto o m´etodo anal´ıtico, de tal forma que el resultado trapezoidal se superpone a la gr´afica del m´etodo anal´ıtico (cos(x)-1) en el intervalo [0, 2π]. ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

108

Cap´ıtulo 2: M´ etodos num´ ericos

Figura 2.7 Comparaci´ on entre los m´etodos trapezoidal y anal´ıtico.

2.4.2 2.4.2. Regla de Simpson Cuando el a´rea bajo la curva de la funci´ on f (x) es representada sobre a´reas de secciones cuadr´aticas y si el intervalo [a, b] es dividido en 2n secciones iguales, entonces el ´area de f (x) puede ser aproximada por la regla de Simpson cuya expresi´ on est´a 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´on, 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´ on quad; la tabla 2.1 muestra su sintaxis. quad(f,a,b) aproxima la integral de una funci´on 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´ on num´erica que usa es 1e-06. La funci´on f (x) puede ser escalar o vectorial. En el caso vectorial quad retorna un vector de salida donde Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

2.4

109

Integraci´ on num´ erica

Tabla 2.1 Funciones de cuadratura Retorna el ´area de la funci´ on 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´on num´erica muy r´apida, pero con menor exactitud.

cada componente tiene la integral de la correspondiente componente de la funci´on vectorial f (x). La funci´on quad puede ser menos eficiente (pobre exactitud) con funciones no suaves. ♣ Ejemplo 2.8 Calcular la integral por el m´etodo de Simpson de la funci´ on ra´ız cuadr´atica √ f (x) = x para intervalos no negativos [a, b].

Soluci´ on √ La funci´on ra´ız cuadrada f (x) = x puede ser integrada anal´ıticamente sobre el intervalo [a, b], a, b ∈ IR+ de la siguiente forma:

I= a

b√

x dx =

3 2 3 (b 2 − a 2 ). 3

(2.13)

Este procedimiento sirve de referencia para comparar el valor num´erico que calcula el m´etodo de Simpson. ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

110

Cap´ıtulo 2: M´ etodos num´ ericos

√ El cuadro 2.9 contiene el programa para calcular la integral de la funci´ on f (x) = x por el m´etodo de Simpson sobre un intervalo no negativo [a, b] especificado por el usuario, y el resultado es comparado con el m´etodo anal´ıtico (2.13) el cual se encuentra implementado en la l´ınea 17. Por precauci´ on, sobre los datos del intervalo definido por el usuario se inserta la instrucci´ on if para verificar que los extremos del intervalo sean positivos y adem´as que cumplan a < b. Se hace la aclaraci´on que dentro del lenguaje de programaci´on de MATLAB la funci´ on sqrt acepta valores negativos y retorna n´ umeros complejos. Sin embargo, en modelado din´ amico y control de robots manipuladores usando moldeo de energ´ıa (estabilidad de Lyapunov) no se requiere la teor´ıa de n´ umeros complejos.

C´ odigo Fuente 2.9

Integraci´ on Simpson

otica y Mecatr´ onica %MATLAB Aplicado a Rob´ %Cap´ ıtulo 2 M´ etodos num´ ericos %Editorial Alfaomega %Fernando Reyes Cort´ es %Archivo cap2 simpson1.m %Funci´ on quad para calcular la integral de la %funci´ on ra´ ız cuadrada por el m´ etodo de Simpson. %El resultado se compara con la integral anal´ ıtica. %El intervalo de integraci´ on es definido por el usuario. Integraci´ on Simpson 1 3

clc; clear all; close all;

4

disp(’M´etodo 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´ on: ’); b=input(’Introduzca el valor final del intervalo de integraci´on: ’);

2

7 8

Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

2.4

111

Integraci´ on num´ erica

C´ odigo Fuente 2.10

Integraci´ on Simpson

% Continuaci´ on del programa 2.9 %MATLAB Aplicado a Rob´ otica y Mecatr´ onica %Cap´ ıtulo 2 M´ etodos num´ ericos %Editorial Alfaomega %Fernando Reyes Cort´ es %Archivo cap2 simpson1.m Integraci´ on Simpson 9 10 11 12

if a>=0 && b>= 0 if a>b disp(’Error: el intervalo positivo [a,b] debe cumplir a 1. Cuando ρ = 0, el sistema oscila a la frecuencia natural de resonancia ωn . Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

2.5

Sistemas din´ amicos de primer orden

C´ odigo Fuente 2.15

131

Sistema lineal de segundo orden

%MATLAB Aplicado a Rob´ otica y Mecatr´ onica %Cap´ ıtulo 2 M´ etodos num´ ericos %Editorial Alfaomega %Fernando Reyes Cort´ es %Archivo cap2 ejemplo27 %Sistema din´ amico lineal de segundo orden %Para su correcta ejecuci´ on requiere del %programa cap2 simuejemplo27 %Entradas: t tiempo % x vector de estados % Salida xp=Ax+Bu % la matriz A est´ a formada por ρ factor % de amortiguamiento y ωn la frecuencia natural % de resonancia mec´ anica. 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˜ nal de entrada

12

u=1;

13

%sistema din´amico lineal xp=A*x+B*u;

9

14 15

end

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

132

Cap´ıtulo 2: M´ etodos num´ ericos

C´ odigo Fuente 2.16

Simulaci´ on del ejemplo 2.12

%MATLAB Aplicado a Rob´ otica y Mecatr´ onica %Cap´ ıtulo 2 M´ etodos num´ ericos %Editorial Alfaomega %Fernando Reyes Cort´ es %Archivo cap2 simuejemplo27 % Simulaci´ on: sistema din´ amico lineal de primer orden % requiere del archivo cap2 ejemplo27.m Simulaci´on del ejemplo 2.12 1

clc; clear all; close all;

2

% par´ametros de simulaci´on:

3

ti=0; tf = 10; h=0.001;

4

% intervalo de simulaci´on 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´on 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´on.

Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

2.6

Resumen

133

2.6 Resumen

M

´ etodos de integraci´ on y diferenciaci´ on num´erica representan una herramienta poderosa de c´ omputo en la implementaci´on pr´ actica para an´alisis y estudios de esquemas de control en simulaci´on y en evaluaci´ on pr´ actica de sistemas mecatr´onicos y robots manipuladores. Existen varios m´etodos pr´ acticos para resolver una integral, entre los m´ as importantes se encuentran trapezoidal, Simpson, Euler recursivo, Runge-Kutta. Sin embargo, dependiendo de la naturaleza del problema, la selecci´on de un m´etodo influye directamente en el desempe˜ no, calidad y fidelidad del proceso de simulaci´ on o experimentaci´on. Por ejemplo, el m´etodo recursivo de Euler es muy r´ apido y puede ser incluido en el c´odigo del experimento para realizarse en tiempo real. Sin embargo, la exactitud con que aproxima la integral es pobre en contraste con otros m´etodos como el trapezoidal o el de Simpson, que pudieran ser mejores opciones para prop´ ositos de simulaci´ on. 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´etodo recursivo de Euler puede ser una buena opci´ on a pesar de que la exactitud en la aproximaci´on de dicha integral no sea bueno. Esta caracter´ıstica no es importante debido a que el desempe˜ no 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´ alisis de la din´ amica en sistemas mecatr´ onicos o robots manipuladores es importante convertir mediante un adecuado cambio de variables de estado (variables fase) el modelo din´amico a la forma de una ecuaci´on diferencial de primer orden x˙ = f (x).

La simulaci´on de estos sistemas din´amicos se realiza por medio de la funci´ on ode45 (m´etodo de Runge-Kutta para orden cuarto y quinto), con pasos de integraci´ on de un milisegundos. Las propiedades del m´etodo de integraci´ on, as´ı como las condiciones iniciales del vector x(0) se configuran con la funci´ on odeset. ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

134

Cap´ıtulo 2: M´ etodos num´ ericos

Parte I referencias selectas

E

xiste una extensa literatura sobre los aspectos b´asicos de MATLAB, as´ı como m´etodos num´ericos. A continuaci´ on se presenta literatura selecta correspondiente a la Parte I donde el lector puede profundizar sobre los t´opicos presentados.

Cap´ıtulo 1 Conceptos b´ asicos

http://www.mathworks.com

Delores M. Etter. “Engineering problem solving with MATLAB”. Second edition. Prentice-Hall. 1997. Shoichiro Nakamura. “An´ alisis num´erico y visualizaci´ on gr´ afica con MATLAB ”. Pearson Eduaci´ on. 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´ aez

L´opez

&

Ofelia

Cervantes

Villagomez.

“ MatLab con aplicaciones a la ingenier´ıa, f´ısica y finanzas”. 2da edici´ on Alfaomega, diciembre 2011. Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

135

Parte I problemas propuestos

Cap´ıtulo 2 M´ etodos num´ ericos

Tom M. Apostol. ‘Calculus’. Revert´e. 2da. ed., vols. I y II, 2006.

Norman B. Hasser, Joseph P. La Salle, Joseph A. Sullivan. ‘An´ alisis matem´atico’. Trillas, 1970.

Watson Fulks. ‘C´alculo 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´on se presentan una serie de ejercicios con la finalidad que el lector mejore sus conocimientos sobre m´etodos num´ericos.

Cap´ıtulo 1 Conceptos b´ asicos

1.1 Considere el programa 1.13, realizar los cambios necesarios en el c´odigo fuente para encontrar los m´ınimos y m´aximos locales de la funci´on. 1.2 Dado la siguiente secuencia de n´ umeros: datos = {1, 4, −3, 8, 12, 7, 2, 5, −12, 5, 7, 1, 0, 2, 22, 15, 4, 6, 2, −1, −3, 11} ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

136

Cap´ıtulo 2: M´ etodos num´ ericos escribir un programa donde se use instrucciones if, for, while para realizar lo siguiente: (a) Ordenar los n´ umeros de menor a mayor. (b) Ordenar los n´ umeros de mayor a menor. (c) Encontrar el m´ınimo y m´ aximo.

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´es de los siguientes m´etodos. (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´ on y divisi´on entre las componentes de los vectores a trav´es operaciones con arreglos. 1.5 Obtener la gr´ afica 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

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

137

Parte I problemas propuestos

Cap´ıtulo 2 M´ etodos num´ ericos

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´etodos (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´e argumentos puede utilizar para obtener la derivada de la funci´ on |t|? 2.3 ¿C´omo puede obtener la derivada con respecto al tiempo de una funci´ on discontinua como la funci´on signo(t)? 2.4 Aproximar la derivada con respecto al tiempo por diferenciaci´ on num´erica 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´etodo anal´ıtico. ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

138

Cap´ıtulo 2: M´ etodos num´ ericos

2.5 Calcular la integral de las siguientes funciones:

μ

μ 7 I1 = x dx I2 = |x|dx μ μ

μi

μi I3 = signo(x)dx I4 = tanh(x)dx μi μi

μ atan (x) dx I5 = μi

considere el intervalo de integraci´ on μ ∈ [−10, 10]. Realice el c´alculo de la integral para cada funci´ on utilizando los siguientes m´etodos: (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´on por pasos de 0.001 para x ∈ [−10, 10]. Comparar los resultados obtenidos con su respectivo m´etodo anal´ıtico. 2.7 Realizar la simulaci´on de los siguientes sistemas din´amicos (t ∈ [0, 10]): (a)

 x˙ =

(b)

 x˙ =

Alfaomega

x˙ 1



x˙ 2 x˙ 1 x˙ 2



 =

 =

sen(x2 )



x31

atan (x1 + x2 )



cos(x2 x1 )

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

Parte II Cinem´atica

La Parte II est´a dedicada al estudio de la cinem´atica directa de robots manipuladores y sistemas mecatr´onicos, y se componen de tres cap´ıtulos. El cap´ıtulo 3 incluye Preliminares matem´ aticos para apoyar la representaci´ on de matrices homog´eneas. El cap´ıtulo 4 Cinem´ atica directa cubre los temas de cinem´atica directa e inversa, cinem´atica diferencial y jacobiano de robots manipuladores, as´ı como la metodolog´ıa Denavit-Hartenberg. El cap´ıtulo 5 Cinem´ atica directa cartesiana est´a 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´ on de la herramienta de trabajo).

Cap´ıtulo 3 Preliminares matem´aticos

incluye los temas de producto punto entre vectores, matrices ortogonales y sus propiedades matem´aticas, reglas de rotaci´on, matrices de transformaci´ on homog´enea para traslaci´ on y rotaci´ on, as´ı como la presentaci´on de varios ejemplos ilustrativos y el desarrollo de librer´ıas para MATLAB.

Cap´ıtulo 4 Cinem´atica directa

presenta los conceptos de cinem´atica directa, cinem´atica inversa, cinem´atica diferencial y el jacobiano del robot. Tambi´en explica la metodolog´ıa de Denavit-Hartenberg la cual usa un grupo de par´ ametros geom´etricos para determinar la matriz de transformaci´ on homog´enea que caracteriza al modelo cinem´atico de robots manipuladores industriales.

Cap´ıtulo 5

Cinem´atica directa cartesiana

realiza un an´alisis detallado de la cinem´ atica cartesiana de las principales configuraciones de robots industriales. Un conjunto de librer´ıas se desarrollan para llevar a cabo simulaci´on con robots manipuladores. La parte II concluye con:

Referencias selectas Problemas propuestos

Cap´ıtulo

3

Preliminares matem´aticos  i Hi−1

=

3.1 Introducci´ on 3.2 Producto interno 3.3 Matrices de rotaci´ on 3.4 Reglas de rotaci´ on 3.5 Transformaciones de traslaci´ on 3.6 Transformaciones homog´ eneas 3.7 Librer´ıas para matrices homog´ eneas 3.8 Resumen

Rii−1

dii−1

0T

1



Objetivos Presentar los preliminares matem´aticos para la cinem´atica directa de robots manipuladores y sistemas mecatr´ onicos, asimismo desarrollar librer´ıas en lenguaje MATLAB (toolbox) para las matrices de transformaci´on homog´enea de rotaci´on y traslaci´ on.

Objetivos particulares: Producto interno de vectores. Matrices de rotaci´on y traslaci´ on. Transformaciones homog´eneas. Librer´ıas de matrices homog´eneas para MATLAB.

3.1

Introducci´ on

143

3.1 Introducci´ on

E

l posicionamiento del extremo final del robot en el espacio tridimensional (pose) requiere de 6 coordenadas: 3 coordenadas para la posici´ on cartesiana y 3 coordenadas para la orientaci´ on de la herramienta de trabajo. A la relaci´ on que existe entre las coordenadas articulares del robot con las coordenadas cartesianas y la orientaci´ on de la herramienta de trabajo colocada en el extremo final del robot se le denomina cinem´atica directa. Como parte de la representaci´on matem´atica de la cinem´atica directa de robots manipuladores se encuentra el uso de transformaciones homog´eneas para representar orientaci´ on y traslaci´ on de la herramienta de trabajo, con respecto al sistema de referencia fijo ubicado generalmente en la base del robot. Como pre´ambulo al tema de matrices de transformaci´ on homog´enea est´an 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´on y traslaci´on de la herramienta de trabajo respecto al sistema fijo del robot Σ0 (x0 , y0 , z0 ). Resaltan las propiedades matem´aticas de las matrices ortogonales que facilitan el an´alisis y descripci´ on de movimientos de rotaci´on y traslaci´ on. En funci´ on de las propiedades matem´aticas de las matrices ortogonales se generan diferentes reglas de rotaci´on. Las matrices homog´eneas incluyen estos conceptos para ofrecer una representaci´on compacta de la matriz de rotaci´on, que determina la orientaci´on 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´on x0 = [ x0 , y0 , z0 ]T .

Con la estructura matem´atica de la transformaci´ on homog´enea se desarrolla un conjunto de librer´ıas para prop´ ositos de simulaci´on de cinem´atica directa de robots manipuladores y sistemas mecatr´onicos. ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

144

Cap´ıtulo 3: Preliminares matem´ aticos

3.2 Producto interno

P

ara modelar la orientaci´ on de la herramienta de trabajo del robot se encuentra el producto escalar o producto interno, tambi´en conocido como producto punto (dot product), el cual permite utilizar los conceptos de la geometr´ıa euclidiana tradicionales como longitudes, a´ngulos, proyecciones geom´etricas, ortogonalidad en dos y tres dimensiones de los sistemas de referencia asociados al robot y de la herramienta de trabajo. El t´ermino espacio euclidiano se denomina as´ı en honor del matem´ atico y fil´ osofo 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´etrico 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´etrico se generaliza para el caso general de espacio euclidiano n-dimensional, el cual tambi´en se considera como un espacio vectorial n-dimensional real. Al producto interno x · y es una operaci´on definida sobre dos vectores x, y ∈ IRn de un espacio euclidiano cuyo resultado es un n´ umero 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´en se puede realizar como: x · y = xT y. En un espacio euclidiano real, el producto interno (3.1) tambi´en acepta una definici´on Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

3.2

145

Producto interno

geom´etrica dada por: x · y = x

y cos(θ)

(3.2)

donde θ es el ´angulo 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´etricamente y cos(θ) representa la proyecci´ on del vector y sobre la direcci´on del vector x, como se presenta en la figura 3.1. El concepto de proyecci´on geom´etrica se usa particularmente en las matrices rotacionales para modelar la orientaci´ on 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´on geom´etrica del producto escalar permite calcular el coseno del angulo θ 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).

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

146

Cap´ıtulo 3: Preliminares matem´ aticos Vectores paralelos o con la misma direcci´on si el ´angulo 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´ on dot(x,y) para realizar la operaci´on 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´on n. on en MATLAB: Nota: el producto punto x · y equivale a realizar la operaci´ x · y=x’*y. La norma euclidiana x de un vector x ∈ IRn , tambi´en conocida como norma 2, se puede calcular utilizando la funci´on 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 a´ngulo θ que forman entre los vectores x, y.

Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

3.2

147

Producto interno

Soluci´ on Puesto que los vectores x, y ∈ IR2 est´an formados por x = [ 2 4 ]T y y = [ 8 −3 ]T , y tomando en cuenta la definici´ on del producto punto se obtiene lo siguiente: x · y = x1 y1 + x2 y2 = 2 (8) − 4 (3) = 16 − 12 = 4. El ´angulo 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´ odigo fuente para realizar el desarrollo del ejemplo 3.1.

C´ odigo Fuente 3.1

Producto interno o escalar

%MATLAB Aplicado a Rob´ otica y Mecatr´ onica. %Editorial Alfaomega,

Fernando Reyes Cort´ es.

%Cap´ ıtulo 3 Cinem´ atica %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´ on dot xdoty m=x’*y %forma matem´atica del producto interno x · y

7

theta=acos(x’*y/(norm(x,2)*norm(y,2))) %´ angulo que forman los vectores 2 x, y ∈ IR

8

%forma geom´etrica del producto interno: x · y = x

y cos(θ) xdoty geometrica=norm(x,2)*norm(y,2)*cos(theta)

9

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

148

Cap´ıtulo 3: Preliminares matem´ aticos

3.3 Matrices de rotaci´ on

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´ on relativa al sistema de referencia fijo Σ0 ( x0 , y0 , z0 ).

Figura 3.2 Sistemas de referencia fijo Σ0 y rotado Σ1 . Consid´erese 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´on 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´ un para ambos sistemas hasta el punto p, puede ser expresado en funci´ on 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)

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

3.3

149

Matrices de rotaci´ on 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´ on 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´on 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´ervese 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´on del eje x1 relativa al sistema de referencia Σ0 . on del punto p0 : Similarmente se puede obtener el punto p1 en funci´ p1x = p1 · i1 = p0 · i1 = p0x i0 · i1 + p0y j 0 · i1 + p0z k 0 · i1 ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

150

Cap´ıtulo 3: Preliminares matem´ aticos 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´on 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´on y pertenece a la clase de matrices ortogonales que se denotan como SO(3). Notaci´on La matriz R10 representa la orientaci´ on del sistema Σ1 ( x1 , y1 , z1 ) respecto al sistema Σ0 ( x0 , y0 , z0 ). R21 representa la orientaci´on del sistema Σ2 ( x2 , y2 , z2 ) respecto al sistema Σ1 ( x1 , y1 , z1 ), y as´ı sucesivamente. Mientras que la transformaci´on inversa on del sistema de referencia Σ0 ( x0 , y0 , z0 ) relativa al sistema R01 significa la orientaci´ aloga, R12 es la transformaci´ on inversa de de referencia Σ1 ( x1 , y1 , z1 ). De manera an´ coordenadas del sistema de referencia Σ1 ( x1 , y1 , z1 ) hacia el sistema Σ2 ( x2 , y2 , z2 ).

Hay varios m´etodos para expresar la orientaci´on del sistema de referencia Σ1 (x1 , y1 , z1 ) con respecto al sistema de referencia fijo Σ0 (x0 , y0 , z0 ), entre los m´as usuales se encuentra la rotaci´ on con respecto a uno de los ejes principales x0 , y0 , y0 . Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

3.3

151

Matrices de rotaci´ on

A continuaci´ on se tratan los casos de rotaci´on alrededor de un eje principal.

3.3.1 3.3.1. Matriz de rotaci´ on alrededor del eje z0 Considere que el sistema de referencia Σ1 ( z1 , y1 , z1 ) el cual se encuentra rotado un ´angulo θ alrededor del eje z0 del sistema Σ0 ( z0 , y0 , z0 ). Obtener la matriz resultante de transformaci´ on R10 . Como se muestra en la figura 3.3, los ejes z0 y z1 son paralelos. El signo del a´ngulo θ est´a dado por la regla de la mano derecha. Por convenci´ on, un a´ngulo positivo es aquel cuyo sentido de rotaci´on es contrario al movimiento de las manecillas del reloj.

Figura 3.3 Rotaci´on de un a´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´as productos punto son cero, puesto que el a´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 a´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. ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

152

Cap´ıtulo 3: Preliminares matem´ aticos

Notaci´on La matriz (3.13) se conoce como matriz de rotaci´ on 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´ on Rz (θ) se interpreta como una matriz que especifica la orientaci´ on 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´ on consid´erese que el ´angulo θ es positivo en el sentido contrario al movimiento de las manecillas del reloj. La relaci´on inversa que determina la orientaci´ on del sistema Σ0 ( x0 , y0 , z0 ) con respecto al sistema Σ1 ( x1 , y1 , z1 ) est´a 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 on relativa. punto p1 en el sistema Σ1 ( x1 , y1 , z1 ) incluyendo su orientaci´ Otra forma de obtener la matriz Rz ( θ ) que relaciona la orientaci´on θ 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´on geom´etrica, es decir, analizando la proyecci´on 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 a´ngulo θ con respecto al plano x0 − y0 , entonces la proyecci´on del punto p1 = [p1x, p1y , p1z ]T Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

3.3

153

Matrices de rotaci´ on

Figura 3.4 Rotaci´on θ 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´ on Rz (θ) La matriz de rotaci´ on Rz (θ) tiene varias propiedades importantes que a continuaci´on se presentan: Rz ( 0 ) = I, I ∈ IR3×3 es la matriz identidad. Rz ( θ )Rz ( β ) = Rz ( β )Rz ( θ ) = Rz ( θ + β ) = Rz ( β + θ ). Rz ( θ )−1 = Rz ( −θ ). ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

154

Cap´ıtulo 3: Preliminares matem´ aticos 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´ on matriz de rotaci´ on Rz ( θ ) La funci´on matriz de rotaci´ on R z ( θ ) ⎡

cos(θ)

⎢ Rz ( θ ) = ⎣ sen(θ) 0

− sen(θ) 0 cos(θ) 0

⎤

⎥ 0⎦

1

tiene la siguiente sintaxis: R=Rz(θ) donde θ ∈ IR es el ´angulo de rotaci´ on alrededor del eje z, y representa el argumento de entrada. Retorna la matriz de rotaci´ on R. El cuadro 3.2 contiene el programa en c´odigo fuente para evaluar en forma simb´ olica las propiedades matem´aticas de la matriz de rotaci´ on Rz ( θ ). Para simplificar el ´algebra simb´ olica es recomendable utilizar la funci´on simplify. Para expresiones matem´aticas con variables simb´ olicas, la funci´ on simplify resulta importante para obtener un resultado matem´atico compacto, es decir factorizado y reducido.

Cuando un programa se encuentra combinando c´ alculos num´ericos con variables simb´olicas, se recomienda usar la funci´on vpa con una precisi´ on de tres d´ıgitos a trav´es de digits(3).

Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

3.3

155

Matrices de rotaci´ on

C´ odigo Fuente 3.2

Funci´ on Rz (θ)

otica y Mecatr´ onica. %MATLAB Aplicado a Rob´ %Editorial Alfaomega,

Fernando Reyes Cort´ es.

%Cap´ ıtulo 3 Cinem´ atica %Archivo Rz.m Funci´ on Rz (θ) 1

function R=Rz(theta)

2

dato=whos(’theta’); if strcmp(dato.class, ’sym’) %para variables simb´ olicas R=simplify([cos(theta), -sin(theta), 0; sin(theta), cos(theta), 0;

3 4 5 6

0, 0, 1]); else %c´alculos num´ericos 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´ on Rz (θ) Soluci´ on En el cuadro 3.5 se presenta el programa en c´odigo fuente para MATLAB de las propiedades de la matriz de rotaci´ on Rz ( θ ). Para que el resultado simb´ olico sea compacto o simplificado se emplea la funci´on simplify. Para visualizar los resultados de las propiedades matem´aticas de la matriz Rz ( θ ) es necesario no insertar en cada l´ınea el operador ;). ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

156

Cap´ıtulo 3: Preliminares matem´ aticos

Cuando se trabaja u ´nicamente con variables simb´olicas es importante utilizar la funci´ on simplify para obtener resultados algebraicos compactos. Cuando hay una combinaci´on entre variables simb´olicas y c´alculos num´ericos es recomendable usar la funci´ on double como en el cuadro del c´odigo 3.2 de la matriz Rz ( θ ). Otras opciones que se pueden usar son la funci´on vpa y digits(3) (ver el c´odigo 3.8).

C´ odigo Fuente 3.5

Propiedades de la matriz de rotaci´ on Rz (θ)

%MATLAB Aplicado a Rob´ otica y Mecatr´ onica. %Editorial Alfaomega,

Fernando Reyes Cort´ es.

%Cap´ ıtulo 3 Cinem´ atica %Archivo cap3 propiedadesRz.m Propiedades de la matriz de rotaci´ on Rz (θ) 1

clc;

2 4

clear all; close all; syms a b real

5

%resultados simb´olicos

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´alculo num´erico theta=90*pi/180; θ =

16

Rz(theta) %Rz ( θ )

9 10 11

Alfaomega

π 2

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

3.3

Matrices de rotaci´ on

157

♣ Ejemplo 3.3 Escribir un algoritmo en lenguaje MATLAB que realice la rotaci´ on de 90 grados alrededor del eje z0 de una imagen de prueba (flecha).

Soluci´ on El programa 3.4 contiene el c´odigo para realizar la rotaci´ on de la imagen de prueba. T El vector p0 = [ px0 , py0 , pz0 ] tiene las coordenadas de la imagen de prueba definidas on θ es de 90 grados con en el sistema de referencia Σ0 (x0 , y0 , z0 ). El a´ngulo de rotaci´ respecto al eje z0 . El sistema Σ1 (x1 , y1 , z1 ) tiene la imagen rotada con respecto on Rz0 ( π2 ) relaciona la orientaci´ on al sistema Σ0 (x0 , y0 , z0 ). La matriz de rotaci´ que tiene la imagen rotada definida en Σ1 (x1 , y1 , z1 ) en relaci´ on 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´on de 90 grados alrededor del eje z0 . ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

158

Cap´ıtulo 3: Preliminares matem´ aticos

C´ odigo Fuente 3.4

Rotaci´ on alrededor del eje z0

%MATLAB Aplicado a Rob´ otica y Mecatr´ onica. %Editorial Alfaomega,

Fernando Reyes Cort´ es.

%Cap´ ıtulo 3 Cinem´ atica %Archivo cap3 rotacionRz.m Rotaci´on 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´ on R z ( θ ) R ztheta=[ cos(theta), -sin(theta), 0; sin(theta), cos(theta), 0; 0, 0,1];

13

%conversi´on 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

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

3.3

159

Matrices de rotaci´ on

♣ 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´ on relativa de 90 grados alrededor on del punto p1 en el del eje z0 del sistema fijo Σ0 ( z0 , y0 , z0 ). Obtener la proyecci´ sistema fijo Σ0 .

Soluci´ on 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´on de coordenadas al sistema de referencia fijo se realiza a trav´es 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´otese que el punto p0 en el sistema convertir al punto p1 en el sistema de de la siguiente relaci´ on: ⎡ cos( π2 ) ⎢ p1 = ⎣ − sen( π2 ) ⎡

0 ⎤

de referencia fijo Σ0 ( z0 , y0 , z0 ) se puede referencia rotado Σ1 ( z1 , y1 , z1 ), a trav´es 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´on geom´etrica del punto p1 y su proyecci´ on sobre el sistema de referencia Σ0 ( z0 , y0 , z0 ). Observe que los ejes z0 y z1 se mantienen paralelos entre s´ı. ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

160

Cap´ıtulo 3: Preliminares matem´ aticos

Figura 3.6 Rotaci´on 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´on del sistema de referencia rotado Σ1 ( z1 , y1 , z1 ) en el sistema de referencia fijo.

Soluci´ on 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´ olido quedando en el punto p1 = [p1x, p1y , p1z ]T . Todos los puntos que pertenecen al poliedro est´an sujetos a la siguiente transformaci´ on: 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 on geom´etrica de la rotaci´on medio de la matriz de rotaci´on Rz ( π ). La interpretaci´ del poliedro 90 grados alrededor del eje z0 se presenta en la figura 3.7. Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

3.3

161

Matrices de rotaci´ on

Figura 3.7 Rotaci´on de 180 grados alrededor del eje z0 de un paralelep´ıpedo rectangular.

3.3.2 3.3.2. Matriz de rotaci´ on alrededor del eje x0 Considere el sistema de referencia Σ1 ( z1 , y1 , z1 ) el cual est´a rotado un a´ngulo θ alrededor del eje x0 del sistema Σ0 ( z0 , y0 , z0 ); obtener la matriz resultante de rotaci´ on. En la figura 3.8 se muestra la rotaci´ on del sistema Σ1 ( z1 , y1 , z1 ) con respecto al eje on θ gira alrededor del eje x0 x0 del sistema fijo Σ0 ( z0 , y0 , z0 ); el ´angulo de rotaci´ en sentido positivo (contrario a las manecillas del reloj). En este caso el ´angulo de rotaci´ on 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´ on R10 (θ) ecuaci´on (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

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

162

Cap´ıtulo 3: Preliminares matem´ aticos

Figura 3.8 Rotaci´on de un a´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´on est´a 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 ´angulo θ de derecha a izquierda con respecto al plano x0 − y0 . Notaci´on La matriz (3.16) cuyo a´ngulo de rotaci´ on θ se realiza alrededor del eje x se denota atica es: por Rx ( θ ) y su estructura matem´ ⎡ 1 0 ⎢ Rx( θ ) = ⎣ 0 cos(θ) 0

sen(θ)

0

⎤

⎥ − sen(θ) ⎦ .

(3.17)

cos(θ)

Las propiedades de la matriz de rotaci´ on Rx ( θ ) son las mismas para Rz ( θ ). Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

3.3

163

Matrices de rotaci´ on

3.3.3 3.3.3. Matriz de rotaci´ on 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 a´ngulo θ alrededor del eje y0 del sistema fijo Σ0 ( z0 , y0 , z0 ). Obtener la matriz resultante de rotaci´on. De la figura 3.9 se puede obtener la matriz que relaciona la orientaci´on relativa del sistema de referencia Σ1 ( z1 , y1 , z1 ) con respecto al sistema de referencia fijo on θ es alrededor del eje y0 . Σ0 ( z0 , y0 , z0 ). El a´ngulo de rotaci´ La matriz R10 (θ) adquiere la siguiente expresi´on: ⎡ ⎢ R10 (θ) = ⎣

cos(θ)

0

sen(θ)

0

1

0

− sen(θ) 0

⎤ ⎥ ⎦

(3.18)

cos(θ)

Figura 3.9 Rotaci´on de un a´ngulo θ del sistema Σ1 alrededor del eje y0 del sistema Σ0 . Observe que la rotaci´on del ´ angulo θ alrededor del eje y0 mueve el plano z1 − x1 . ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

164

Cap´ıtulo 3: Preliminares matem´ aticos

Notaci´on La matriz de rotaci´ on (3.18) describe la orientaci´ on relativa del sistema Σ1 ( z1 , y1 , z1 ) con respecto al sistema de referencia fijo Σ0 ( z0 , y0 , z0 ) usando una rotaci´ on 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´ on

G

eneralmente la descripci´on de la orientaci´ on 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´on de rotaciones). Existen varios m´etodos que permiten definir el orden o reglas de las rotaciones consecutivas. Composici´ on 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´ on relativa al sistema fijo y sea el sistema de referencia Σ2 ( x2 , y2 , z2 ) cuya orientaci´on 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´on matem´ atica que relaciona la orientaci´ on 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´ on de las coordenadas p0 , p1 , p2 de la siguiente Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

3.4

165

Reglas de rotaci´ on

Figura 3.10 Composici´ on de rotaciones. forma: p0 = R10 p1

(3.20)

p1 = R21 p2

(3.21)

donde la matriz R10 representa la orientaci´on 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´ on del sistema de referencia Σ2 ( x2 , y2 , z2 ) respecto al sistema Σ1 ( x1 , y1 , z1 ). Por lo tanto, la relaci´ on que hay entre un punto p0 ∈ Σ0 ( x0 , y0 , z0 ) con las coordenadas p2 ∈ Σ2 ( x2 , y2 , z2 ) est´a determinada por la siguiente expresi´on: p0 = R10 R21 p2

(3.22)

donde R10 R21 representa la orientaci´on del sistema de referencia Σ2 ( x2 , y2 , z2 ) respecto al sistema fijo Σ0 ( x0 , y0 , z0 ), y significa una regla de composici´on para establecer el orden para transformar un punto p desde su representaci´ on en el sistema Σ2 ( x2 , y2 , z2 ) a su correspondiente representaci´on en el sistema Σ0 ( x0 , y0 , z0 ). La regla de transformaci´on 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´on de rotaciones sucesivas se establece como: ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

166

Cap´ıtulo 3: Preliminares matem´ aticos

R20 = R10 R21

(3.23)

La regla de composici´ on de rotaciones (3.23) puede ser interpretada de la siguiente manera: sup´ongase 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´on 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 on transformaci´ on R21 . El sistema de referencia Σ2 ( x2 , y2 , z2 ) tiene una orientaci´ 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´on de rotaciones sucesivas. La regla de composici´ on de rotaci´on 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 a´ngulo determinado alrededor de on uno de los ejes principales del sistema Σ0 ( x0 , y0 , z0 ), seguido por otra rotaci´ 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´ on de matrices de rotaci´on no es conmutativa, en consecuencia el orden de las rotaciones no es conmutativo. Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

3.4

Reglas de rotaci´ on

167

♣ Ejemplo 3.6 Considere el procedimiento de rotaci´ on que se muestra en la figura 3.12; obtener la matriz resultante que define la rotaci´on del sistema de referencia Σ2 con respecto al sistema de referencia Σ0 . Soluci´ on 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´on 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´ on por un a´ngulo φ alrededor del eje y0 que se representa por la matriz Ry0 ,φ . Posteriormente, contin´ ua una rotaci´ on por un a´ngulo θ alrededor del eje z1 para generar el sistema de referencia Σ2 ( x2 , y2 , z2 ) (parte central de la figura on relativa del sistema Σ2 3.12), los ejes z1 y z2 son paralelos entre s´ı. La orientaci´ con respecto al sistema de referencia Σ1 est´a determinada por la matriz Rz1 ( θ ). El esquema derecho de la figura 3.12 muestra la orientaci´on del sistema de referencia Σ2 con respecto al sistema de referencia Σ0 , cuya matriz de rotaci´on se encuentra dado por: R20 = R10 R21 = Ry0 ( φ ) Rz1 ( θ ) ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

168

Cap´ıtulo 3: Preliminares matem´ aticos ⎡ ⎢ = ⎣

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´ on alrededor del eje z1 , seguida por una rotaci´ on alrededor del eje y0 . Entonces, la matriz resultante est´a 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 a´ngulo θ =

π 2

alrededor de los eje z0 , del sistema resultante Σ1 (x1 , y1 , z1 ) realizar una rotaci´ on θ=

π 2

alrededor del eje x1 , y finalmente del sistema generado Σ2 (x2 , y2 , z2 ) rotar

alrededor del eje y2 por θ = π2 . Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

3.4

Reglas de rotaci´ on

169

Soluci´ on 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 un efecto sistema de referencia Σ0 (x0 , y0 , z0 ) ver´a a la figura de prueba sin ning´ π de rotaci´ on. Sin embargo, despu´es de realizar la rotaci´on Rz0 ( 2 ) un observador colocado en el sistema de referencia Σ0 (x0 , y0 , z0 ) ver´a 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´ on 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 ) ´ltima rotaci´on consiste en girar a la est´an especificadas por p2 = Rx1 ( π2 )p1 . La u on genera el imagen de prueba (flecha) 90 grados alrededor del eje y2 . Esta rotaci´ sistema de referencia Σ3 (x3 , y3 , z3 ) con coordenadas de la imagen p3 = Ry2 ( π2 )p2 . La relaci´on 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´ on de rotaciones.

Figura 3.13 Rotaciones sucesivas de la figura de prueba alrededor de los ejes z0 , x1 , y2 . ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

170

Cap´ıtulo 3: Preliminares matem´ aticos

C´ odigo Fuente 3.5

Rotaciones alrededor de los ejes z0 , x1 , y2

%MATLAB Aplicado a Rob´ otica y Mecatr´ onica. %Editorial Alfaomega,

Fernando Reyes Cort´ es.

%Cap´ ıtulo 3 Cinem´ atica %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

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

3.5

171

Transformaciones de traslaci´ on

3.5 Transformaciones de traslaci´ on

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´ on y rotaci´ on del sistema Σ1 con respecto al sistema Σ0 . El vector d10 est´a expresado en coordenadas del sistema Σ0 : d10 = [ d10x, d10y , d10z ]T , entonces cualquier punto p tiene representaci´on p0 y p1 . La relaci´on general entre los sistemas de referencia Σ0 ( x0 , y0 , z0 ) y Σ1 ( x1 , y1 , z1 ) incluyendo la matriz de rotaci´ on R10 y el vector de traslaci´on 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 ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

172

Cap´ıtulo 3: Preliminares matem´ aticos

Σ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´on y rotaci´ on de Σ1 y Σ2 con respecto al sistema Σ0 . Sustituyendo la ecuaci´on (3.26) en (3.25) se tiene: p0 = d10 + R10 d21 + R10 R21 p2 .

(3.27)

Por lo tanto, la regla de transformaci´ on de traslaci´ on y orientaci´ on 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

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

3.6

173

Transformaciones homog´ eneas

3.6 Transformaciones homog´ eneas

L

a notaci´ on m´ as com´ un para representar la transformaci´ on de traslaci´ on y rotaci´ on en forma compacta se conoce como transformaci´ on homog´ enea. Por ejemplo, para representar el caso de traslaci´on y rotaci´ on del sistema Σ1 (x1 y1 z1 ) con respecto al sistema Σ0 (x0 y0 z0 ) p0 = d10 + R10 p1 la transformaci´ on homog´enea se realiza con la siguiente notaci´ on:   1 R0 d10 1 H0 = 0T 1 ⎡ ⎤ . Matriz de Vector de .. ⎢ ⎥ ⎢ ⎥ rotaci´ o n traslaci´ o n ⎥ = ⎢ ⎢ ⎥ .. ⎣ ⎦ ··· . ··· 0T

(3.29)

1

ositos de acoplamiento en dimensiones, el donde R10 ∈ SO(3) y d10 ∈ IR3 . Para prop´ T umero 1 aparecen en el u ´ltimo rengl´on. vector 0 y el n´ La representaci´on inversa que relaciona el punto p1 en funci´ on del punto p0 adquiere la siguiente forma: T

T

p1 = −R10 d10 + R10 p0 entonces, la transformaci´on homog´enea inversa est´a determinada por:  1T  T R0 −R10 d10 1 −1 = H0 0T 1

(3.30)

Las matrices de rotaci´on permiten modelar la orientaci´ on de la herramienta de trabajo colocada en el extremo final del robot, y junto con las transformaciones homog´eneas dentro de una sola matriz incluye la orientaci´ on y posici´ on de la herramienta de trabajo, formando la estructura del modelo cinem´ atico directo. ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

174

Cap´ıtulo 3: Preliminares matem´ aticos

3.7 Librer´ıas para matrices homog´ eneas

E

n esta secci´on se presentan las librer´ıas de transformaci´ on homog´enea de rotaci´ on y traslaci´ on con respecto a los ejes principales x, y, z.

Matrices de transformaci´ on homog´ enea de rotaci´ on Las matrices de transformaci´on homog´enea de rotaci´on 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´ on homog´ enea de traslaci´ on Las matrices de transformaci´on homog´enea de traslaci´on 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´ on homog´ enea Denavit-Hartenberg La matriz de transformaci´ on homog´enea Denavit Hartenberg tiene la forma siguiente: ⎡ ⎢R H = ⎢ ⎣ 0T Alfaomega

⎡ ⎤ x ⎤ ⎢ ⎥ ⎣y ⎦⎥ ⎥ z ⎦ 1

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

3.7

Librer´ıas para matrices homog´ eneas

175

3.7.1 3.7.1. Matriz de transformaci´ on homog´ enea HRx (θ) La sintaxis de la funci´on de transformaci´ on homog´enea de rotaci´on HRx(θ) alrededor del eje x est´a dada por: RHx=HRx(θ) donde θ es el ´angulo de rotaci´ on alrededor del eje x y es la variable de la funci´on HRx(θ). Retorna la matriz de transformaci´ on homog´enea RHx.

C´ odigo Fuente 3.6

Funci´ on HRx(θ)

otica y Mecatr´ onica. %MATLAB Aplicado a Rob´ %Editorial Alfaomega,

Fernando Reyes Cort´ es.

%Cap´ ıtulo 3 Cinem´ atica %funci´ on HRx.m Funci´ on HRx(θ) 1 2

function RHx=HRx(theta) dato=whos(’theta’);

3

if strcmp(dato.class, ’sym’) %variables simb´ olicas 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´ alculos num´ericos 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

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

176

Cap´ıtulo 3: Preliminares matem´ aticos

3.7.2 3.7.2. Matriz de transformaci´ on homog´ enea HRy (θ) La sintaxis de la funci´on de transformaci´ on homog´enea de rotaci´on HRy (q) alrededor del eje y est´a dada por: RHy=HRy(θ) donde θ es el ´angulo de rotaci´ on alrededor del eje y y es la variable de la funci´on HRy(θ). Retorna la matriz de transformaci´ on homog´enea RHy.

C´ odigo Fuente 3.7

Funci´ on HRy (θ)

otica y Mecatr´ onica. %MATLAB Aplicado a Rob´ %Editorial Alfaomega,

Fernando Reyes Cort´ es.

%Cap´ ıtulo 3 Cinem´ atica %funci´ on HRy.m Funci´ on HRy (θ) 1

function RHy=HRy(theta)

2

dato=whos(’theta’); if strcmp(dato.class, ’sym’) %variables simb´ olicas 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´ alculos num´ericos

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

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

3.7

Librer´ıas para matrices homog´ eneas

177

3.7.3 3.7.3. Matriz de transformaci´ on homog´ enea HRz (θ) La sintaxis de la funci´on de transformaci´ on homog´enea de rotaci´on HRz (θ) alrededor del eje z est´a dada por: RHz=HRz(θ) donde θ es el ´angulo de rotaci´ on alrededor del eje z y es la variable de la funci´on HRz(θ). Retorna la matriz de transformaci´ on homog´enea RHz.

C´ odigo Fuente 3.8

Funci´ on HRz (θ)

otica y Mecatr´ onica. %MATLAB Aplicado a Rob´ %Editorial Alfaomega,

Fernando Reyes Cort´ es.

%Cap´ ıtulo 3 Cinem´ atica %funci´ on HRz.m Funci´ on HRz (θ) 1 2 3 4

function RHz=HRz(theta) dato=whos(’theta’); if strcmp(dato.class, ’sym’) %variables simb´ olicas 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´ alculos num´ericos

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

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

178

Cap´ıtulo 3: Preliminares matem´ aticos

3.7.4 3.7.4. Matriz de transformaci´ on homog´ enea HTx (d) La sintaxis de la funci´on de transformaci´ on homog´enea de traslaci´on HTx(d) a lo largo del eje x est´a dada por: THx=HTx(d) donde d es el desplazamiento de traslaci´on sobre el eje x. Retorna la matriz de transformaci´ on homog´enea THx.

C´ odigo Fuente 3.9

Funci´ on HTx(d)

%MATLAB Aplicado a Rob´ otica y Mecatr´ onica. %Editorial Alfaomega,

Fernando Reyes Cort´ es.

%Cap´ ıtulo 3 Cinem´ atica %funci´ on HTx.m Funci´ on 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´ on homog´ enea HTy (d) La sintaxis de la funci´on de transformaci´ on homog´enea de traslaci´on HTy (d) a lo largo del eje y est´a dada por: THy=HTy(d) donde d es el desplazamiento lineal de traslaci´on sobre el eje y. Retorna la matriz de transformaci´ on homog´enea THy.

Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

3.7

Librer´ıas para matrices homog´ eneas

C´ odigo Fuente 3.10

179

Funci´ on HTy (d)

otica y Mecatr´ onica. %MATLAB Aplicado a Rob´ %Editorial Alfaomega,

Fernando Reyes Cort´ es.

%Cap´ ıtulo 3 Cinem´ atica %funci´ on HTy.m Funci´ on 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´ on homog´ enea HTz (d) La sintaxis de la funci´on de transformaci´ on homog´enea de traslaci´on HTz (d) a lo largo del eje z est´a dada por: THz=HTz(d) donde d es el desplazamiento lineal de traslaci´on sobre el eje z. Retorna la matriz de transformaci´ on homog´enea THz.

C´ odigo Fuente 3.11

Funci´ on HTz (d)

otica y Mecatr´ onica. %MATLAB Aplicado a Rob´ %Editorial Alfaomega,

Fernando Reyes Cort´ es.

%Cap´ ıtulo 3 Cinem´ atica %funci´ on HTz.m Funci´ on 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

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

180

Cap´ıtulo 3: Preliminares matem´ aticos

3.7.7 3.7.7. Matriz de transformaci´ on DH La funci´on H DH(H) extrae de la matriz de transformaci´ on homog´enea H la matriz de rotaci´ on y el vector de coordenadas cartesianas. La sintaxis de la funci´on de transformaci´ on homog´enea DH est´a dada por: [R vect d vect cero c]=H DH(H) donde H es la variable de entrada y representa la matriz de transformaci´on homog´enea. Retorna la matriz de rotaci´on R , el vector de coordenadas cartesianas vect d, el vector vect cero=[0, 0, 0]T y la constante unitaria c=1.

C´ odigo Fuente 3.12

Funci´ on H DH

otica y Mecatr´ onica. %MATLAB Aplicado a Rob´ %Editorial Alfaomega,

Fernando Reyes Cort´ es.

%Cap´ ıtulo 3 Cinem´ atica %funci´ on H DH.m Funci´ on 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´on homog´enea

8

vect d=[H(1,4); H(2,4); H(3,4)]; vect cero=[0;0;0]’; c=1;

9 10 11

end

Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

3.8

Resumen

181

3.8 Resumen

C

´ tica de robots manipuladores y sistemas mecatr´onicos encuentra su inema fundamento en una clase particular de matrices denominadas ortogonales. Estos preliminares matem´aticos constituyen la base para desarrollar propiedades, reglas y operaciones de traslaci´on y rotaci´ on entre dos sistemas de referencia Σ0 (x0 , y0 , z0 ) y Σ1 (x1 , y1 , z1 ) bajo una estructura matem´ atica denominada matriz de transformaci´ on homog´enea. La matriz ortogonal R10 representa la orientaci´on del sistema de referencia Σ0 (x0 , y0 , z0 ) con respecto al sistema Σ1 (x1 , y1 , z1 ). De particular inter´es son las on alrededor matrices de rotaci´on Rx( θ ), Ry ( θ ), Rz ( θ ) ya que representan la rotaci´ de los ejes principales x, y, z, respectivamente. El orden como se realice la rotaci´on 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´on y rotaci´ on. La librer´ıa R=Rz(θ) realiza la rotaci´on del sistema de referencia Σ1 (x1 , y1 , z1 ) con respecto al sistema de referencia Σ0 (x0 , y0 , z0 ). Esta funci´ on puede trabajar con variables simb´ olicas y tambi´en para realizar aplicaciones num´ericas. De manera an´ aloga se puede implementar las funciones R=Rx(θ) y R=Ry(θ). Cuando hay combinaci´on de c´alculos num´ericos con variables simb´ olicas, como es el caso de las matrices de transformaci´on homog´enea (rotaci´on y traslaci´ on) la funciones vpa, round y double pueden ayudar a presentar el resultado num´erico de manera conveniente. Por otro lado, para simplificar las expresiones simb´ olicas se recomienda usar la funci´ on simplify, por lo que el lector puede modificar el c´odigo fuente de las librer´ıas desarrolladas para presentar resultados simb´ olicos o num´ericos a su entera conveniencia.

La tabla 3.1 contiene el resumen de las librer´ıas para realizar las matrices de ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

182

Cap´ıtulo 3: Preliminares matem´ aticos

transformaci´ on homog´enea de traslaci´on y rotaci´ on. Tabla 3.1 Matrices de transformaci´ on homog´ enea Funci´ on

Sintaxis

Matriz de transformaci´ on homog´enea de rotaci´on al-

RHx=HRx(θ) θ ∈ IR.

rededor del eje x Matriz de transformaci´ on homog´enea de rotaci´on al-

RHy=HRy(θ) θ ∈ IR.

rededor del eje y Matriz de transformaci´ on homog´enea de rotaci´on al-

RHz=HRz(θ) θ ∈ IR.

rededor del eje z Matriz de transformaci´ on homog´enea de traslaci´on

THx=HTx(d) d∈ IR.

sobre el eje x Matriz de transformaci´ on homog´enea de traslaci´on

THy=HTy(d) d∈ IR.

sobre el eje y Matriz de transformaci´ on homog´enea de traslaci´on

THz=HTz(d) d∈ IR.

sobre el eje z [R vect d vect cero c]=H DH(H) Matriz de transformaci´ on homog´enea Hartenberg

Denavit-

R∈ SO(3) es la matriz de rotaci´on vect d es el vector con coordenadas cartsianas: [x

y

z ]T

vect cero=[ 0, 0, 0 ]T c=1.

Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

Cinem´atica directa

Cap´ıtulo

4

li

αi

. di .. βi

θi

. i Hi−1 = HRzi (θi )HTzi (di .. βi )HTxi (li)HRxi (αi )

4.1 Introducci´ on 4.2 Cinem´ atica inversa 4.3 Cinem´ atica diferencial 4.4 Clasificaci´ on de robots industriales 4.5 Convenci´ on Denavit-Hartenberg 4.6 Resumen

Objetivos Presentar el modelo de cinem´atica directa de las principales configuraciones de robots industriales considerando los par´ ametros geom´etricos y desarrollar librer´ıas en lenguaje MATLAB (toolbox) que permitan realizar aplicaciones en el ´area de cinem´atica directa de robots manipuladores.

Objetivos particulares: Matrices de rotaci´on. Transformaciones homog´eneas. M´etodo de Denavit-Hartenberg. Cinem´atica directa cartesiana. Librer´ıas de an´ alisis y dise˜ no de cinem´atica de robots industriales.

4.1

185

Introducci´ on

4.1 Introducci´ on

C

´ tica es la parte de la f´ısica que estudia el movimiento de sistemas inema mec´anicos, sin tomar en cuenta las fuerzas que originan dicho movimiento, por lo tanto no involucra ecuaciones diferenciales como en el caso de la din´amica. Al estudio de la cinem´atica de sistemas mecatr´ onicos y robots manipuladores se le denomina cinem´ atica 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´ ametros geom´etricos (longitudes del i-´esimo eslab´on on [ θ, φ, ψ ]T ∈ IR3 del li ), con coordenadas cartesianas [ x, y, z ]T ∈ IR3 y la orientaci´ extremo final del robot a trav´es de una funci´on vectorial f R continua y diferenciable en la variable de estado q, generalmente no lineal. Cinem´ atica directa es una funci´ on vectorial f R (li, q) que relaciona las coordenadas articulares q ∈ IRn y propiedades geom´etricas del sistema mec´anico on li con las coordenadas cartesianas [ x, y, z ]T ∈ IR3 del robot y la orientaci´ [ θ, φ, ψ ]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´ umero de grados de libertad y la dimensi´on del vector de coordenadas articulares q, m es la dimensi´on conjunta de las coordenadas cartesianas y la orientaci´ on 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 ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

186

Cap´ıtulo 4: Cinem´ atica directa

posici´on cartesiana y 3 coordenadas para la orientaci´ on de la herramienta de trabajo. Dependiendo de la aplicaci´on del robot se pueden requerir menos coordenadas de posici´ on y orientaci´on. Por ejemplo, un robot para pintura de armaduras automotrices requiere las 6 coordenadas, en contraste con un robot que corta figuras de pl´astico sobre un plano requiere 2 coordenadas cartesianas de posici´on y ninguna de orientaci´ on. Cuando n > m se denomina robots redundantes. El empleo de la cinem´atica directa resulta de utilidad en la planificaci´ on de trayectorias y en el control cartesiano. El papel fundamental de la cinem´ atica directa, es computar la posici´on y orientaci´ on del extremo final del robot manipulador como una funci´ on de las variables articulares. Un robot manipulador se considera como una serie de eslabones interconectados a trav´es de articulaciones (servomotores) rotacionales o prism´ aticas en forma de cadena cinem´atica abierta, es decir el extremo final donde se coloca la herramienta no se encuentra conectada mec´anicamente a la primera articulaci´on (base) del robot. Desde el punto de vista topol´ogico, la cadena cinem´ atica se considera abierta cuando los dos extremos de la cadena no se tocan. De otra manera la cadena cinem´atica formar´ıa un lazo si sus dos extremos est´an mec´anicamente unidos. La estructura mec´anica del robot manipulador se caracteriza por tener un n´ umero de grados de libertad, los cuales determinan en forma u ´nica su configuraci´on. T´ıpicamente, cada grado de libertad est´ a asociado a una articulaci´on (variable articular q).

4.2 Cinem´ atica inversa

D

ada la posici´ on del extremo final del robot en coordenadas cartesianas T on [ψ, θ, φ]T , con respecto a un sistema de referencia fijo [x, y, z] y la orientaci´ Σ0 (x0 , y0 , z0 ), as´ı como los par´ametros geom´etricos 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´ on requerida? Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

4.3

Cinem´ atica diferencial

187

El problema planteado se conoce como cinem´ atica inversa y representa un a´rea de la rob´ otica de mayor complejidad que la cinem´atica directa. Para un robot manipulador siempre es posible encontrar el modelo de cinem´atica directa, mientras que en la cinem´atica inversa pueden haber varias soluciones e inclusive no existir soluci´on anal´ıtica; si este es el caso, entonces como posibles formas de soluci´on pueden proponerse redes neuronales, m´etodos num´ericos, iterativos, geom´etricos, etc´etera. La cinem´ atica inversa es un problema no lineal que relaciona las coordenadas articulares en funci´ on de las coordenadas cartesianas y la orientaci´on de la herramienta del extremo final del robot manipulador q = f −1 R (x, y, z, li, θ, φ, ψ)

(4.2)

on inversa de la ecuaci´ on (4.1). donde f −1 R (x, y, z, li, θ, φ, ψ) es funci´

4.3 Cinem´ atica diferencial

L

a cinem´atica diferencial directa es la derivada con respecto al tiempo de la cinem´atica 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, ´esta relaciona la velocidad articular q˙ ∈ IRn con la velocidad lineal d d ˙ = dt [ x, y, z ]T = [ x, ˙ y, ˙ z˙ ]T ∈ IR3 y la velocidad angular w [ θ, φ, ψ ]T = v = dt ˙ φ, ˙ ψ˙ ]T ∈ IR3 , adem´as el mapeo es descrito en t´erminos 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 ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

188

Cap´ıtulo 4: Cinem´ atica 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´ otica que sirve para caracterizar a un robot manipulador, encontrar configuraciones singulares, analizar redundancia, determinar la cinem´ atica diferencial inversa, as´ı como describir la relaci´on entre la fuerza aplicada y los pares o torques resultantes del extremo final. Es indispensable para el an´alisis y dise˜ no de algoritmos de control cartesiano. Hay varias formas de seleccionar la orientaci´on de la herramienta del robot manipulador: si de manera particular dicha orientaci´ on es representada por los ´angulos de Euler (un sistema de referencia asociado al extremo final del robot o ˙ φ, ˙ ψ˙ ]T ∈ IR3 a la herramienta de trabajo), entonces la velocidad angular w = [ θ, relaciona la matriz jacobiano anal´ıtico, como se encuentra descrita en la ecuaci´on (4.3). Otra posible forma de modelar la orientaci´ on 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´ etrico que depende de la configuraci´ on del robot manipulador. El jacobiano anal´ıtico difiere del jacobiano geom´etrico: b´ asicamente la diferencia se encuentra en c´omo modelar la orientaci´ on de la herramienta de trabajo del robot. La cinem´atica diferencial inversa representa la relaci´ on entre la velocidad articular q˙ con la velocidad lineal de movimiento v y la velocidad angular w, expresada en t´erminos 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

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

4.4

189

Clasificaci´ on 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´ atica directa.

La cinem´atica inversa diferencial tiene un n´ umero 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´ umero infinito de puntos de equilibrio en la ecuaci´on en lazo cerrado, formada por la din´ amica del robot y la estructura cartesiana de control.

4.4 Clasificaci´ on de robots industriales

U

n robot industrial est´a compuesto por una serie consecutiva de eslabones y articulaciones para formar una cadena en cinem´ atica abierta, la cual es la estructura mec´anica b´ asica de un robot industrial. La cadena en cinem´ atica abierta est´a formada de la siguiente manera: la primera articulaci´ on sirve para formar la base; a continuaci´ on siguen conexiones sucesivas entre articulaciones y eslabones, en el extremo final del u ´ltimo eslab´on no hay articulaci´on, generalmente se destina a colocar la herramienta de trabajo para llevar a cabo una aplicaci´ on espec´ıfica. El ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

190

Cap´ıtulo 4: Cinem´ atica 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´atica abierta. Las articulaciones se construyen por medio de un servomotor y representan la interconexi´on entre dos eslabones consecutivos. Una articulaci´on puede realizar s´ olo un tipo de movimiento, ya sea lineal, tambi´en conocida como prism´atica, y rotacional. La figura 4.2 presenta el tipo de articulaciones:

Figura 4.2 Tipo de articulaciones: rotacional y lineal o prism´ atica. La figura 4.3 muestra la analog´ıa entre el brazo humano y un brazo robot o robot industrial. La articulaci´on de la base corresponde a la cintura. La articulaci´on 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´on del codo (elbow) y de la herramienta de trabajo, as´ı como la carga de objetos que realice durante una determinada aplicaci´on. Dependiendo del tipo de articulaciones (lineales o rotacionales) que se encuentran incluidas en la estructura mec´anica en cinem´atica abierta de la base, hombro y codo del robot (sin incluir las articulaciones de la orientaci´ on de la herramienta de Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

4.4

Clasificaci´ on de robots industriales

191

Figura 4.3 Base, hombro y codo de un robot industrial.

trabajo), se desprende la clasificaci´on general de robots manipuladores industriales, tambi´en conocidos como brazos robots: antropom´orfico, esf´erico, cil´ındrico, SCARA y cartesiano.

Tabla 4.1 Clasificaci´ on de robots industriales Robot

Caracter´ısticas

Antropom´ orfico (RRR) SCARA (RRP) Esf´ erico (RRP)

3 articulaciones rotacionales 2 articulaciones rotacionales y 1 prism´ atica 2 articulaciones rotacionales y 1 prism´ atica

Cil´ındrico (RPP) Cartesiano (PPP)

1 articulaci´ on rotacional y 2 prism´ aticas 3 articulaciones prism´ aticas

La nomenclatura empleada en robots industriales para representar el tipo de movimiento que realizan sus articulaciones est´a dada de la siguiente manera: R significa articulaci´on tipo rotacional, mientras que la letra P representa una articulaci´on prism´atica. 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´on robot cil´ındrico (RPP) significa que la base es una articulaci´ on rotacional, mientras que el hombro y codo corresponden a articulaciones prism´ aticas. En la figura 4.4 se muestra la clasificaci´ on de las 5 configuraciones de robots industriales. ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

192

Cap´ıtulo 4: Cinem´ atica directa

Figura 4.4 Clasificaci´on de los robots industriales.

4.5 Convenci´ on Denavit-Hartenberg

E

l m´etodo de Denavit-Hartenberg es una herramienta ampliamente conocida en el ´area de ingenier´ıa, ya que ofrece un procedimiento sencillo para obtener el modelo cinem´atico directo cuya estructura queda en t´erminos de la transformaciones homog´eneas. Jaques Denavit y Richard S. Hartenberg en 1955 presentaron un procedimiento para obtener una representaci´ on m´ınima de la orientaci´ on y traslaci´on de robots manipuladores. Consiste en determinar una tabla de par´ ametros relacionados con los eslabones del robot. La convenci´ on Denavit-Hartenberg toma como referencia el diagrama de un robot manipulador en cadena cinem´ atica abierta como se muestra en la figura 4.5.

Las variables articulares en la representaci´on Denavit-Hartenberg se denotan con θi para el tipo rotacional, prism´atica o lineal por di; este par´ ametro di tambi´en hace el papel de representar el ancho del servomotor de la articulaci´on rotacional m´as el espesor de la placa m´etalica del eslab´on, en este caso se denota por el s´ımbolo βi ; la on entre los ejes zi longitud del eslab´on se representa con li y el a´ngulo de separaci´ y zi−1 se denota con αi .

El ´angulo θi es el ´angulo entre los ejes xi−1 y xi medido alrededor del eje zi−1 ; di es Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

4.5

193

Convenci´ on Denavit-Hartenberg

Figura 4.5 Convenci´ on Denavit-Hartenberg para un robot manipulador.

la distancia del origen del sistema de referencia i − 1 a la intersecci´on del eje xi con el eje zi−1 . Su medici´on se realiza a lo largo del eje zi−1 , como se indica en la figura ametros constantes 4.6. Adicionalmente a las variables articulares θi y di, hay 2 par´ que describen caracter´ısticas espec´ıficas del eslab´ on i-´esimo. Esos par´ ametros son: el par´ametro 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´on del eje zi−1 con el eje xi . El otro par´ametro es el ´angulo entre los ejes zi y zi−1 se denota por αi , su medici´on es respecto a un plano normal a xi . Una medici´on de a´ngulo positivo para αi se toma en direcci´on 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´atico (li, αi , βi, θi). Selecci´ on de sistemas de referencia En la metodolog´ıa Denavit-Hartenberg, primero se describir´a la convenci´on 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´ otica, la convenci´on Denavit-Hartenberg no es u ´ nica, depende de la selecci´on de los sistemas de referencia cartesianos en las articulaciones y eslabones, as´ı como en sus eslabones adyacentes. ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

194

Cap´ıtulo 4: Cinem´ atica directa

Figura 4.6 Convenci´on para medir θi y αi . La cinem´atica 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´on de sistemas de referencia para las articulaciones i − 1-´esima, i-´esima e i + 1 de un robot manipulador. En general se tiene el siguiente procedimiento: El eje zi se asigna r´ıgidamente a la articulaci´ on i + 1. Es decir, z0 es el eje de la articulaci´ on 1, z1 es el eje de la articulaci´on 2, y as´ı sucesivamente. Localizar el origen oi del sistema de referencia Σi ( xi , yi , zi ) en la intersecci´on del eje zi con la normal com´ un 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´on de la articulaci´ on i − 1 hacia la articulaci´ on i. Definir el ´angulo de torsi´ on αi , este es el ´angulo 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´on Denavit-Hartenberg proporciona una representaci´ on no u ´nica para los siguientes casos: Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

4.5

Convenci´ on Denavit-Hartenberg

195

Para el sistema de referencia Σ0 ( x0 , y0 , z0 ) s´olo la direcci´ on 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´ on n + 1, entonces el eje zn no est´a completamente definido, mientras que el eje xn es on es rotatoria, por lo normal al eje zn−1 . T´ıpicamente la n-´esima articulaci´ tanto zn se alinea en la direcci´on de zn−1 . Cuando dos ejes consecutivos zi y zi−1 son paralelos entre s´ı, la normal com´ un entre ellos no es u ´ nica. Cuando dos ejes consecutivos zi y zi−1 se interceptan, la direcci´on del eje xi es arbitraria. Cuando la articulaci´ on i-´esima es lineal o prism´ atica, entonces la direcci´on de zi es arbitraria. De acuerdo con esta convenci´ on previamente descrita, a continuaci´on se resumen los par´ ametros del i-´esimo eslab´on: 1. li es la longitud del i-´esimo eslab´on, es la distancia del eje zi−1 hacia el eje zi medida sobre el eje xi−1 . 2. αi es el ´angulo de torsi´ on, el cual representa el a´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´aticas y representa el desplazamiento lineal. Cuando la articulaci´ on 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´ervese que li y βi siempre ser´an positivos puesto que corresponden a longitudes, mientras que αi , di, θi representan cantidades con signo. ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

196

Cap´ıtulo 4: Cinem´ atica directa

4.5.1 4.5.1. Algoritmo Denavit-Hartenberg A continuaci´ on se describe el procedimiento para encontrar la cinem´atica directa a trav´es de la convenci´on Denavit-Hartenberg. 1. Localizar la direcci´on 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´on de la normal com´ un que une al eje zi con el eje zi−1 . on. Si el eje zi intercepta al eje zi−1 colocar oi en la intercepci´ Para el caso en que los ejes zi y zi−1 son paralelos: Si la articulaci´ on i-´esima es rotacional, colocar el origen oi sobre la articulaci´on i-´esima, tal que di = 0. Si la articulaci´ on i-´esima es prism´atica, colocar el origen oi en un punto l´ımite f´ısico de la articulaci´on i-´esima, por ejemplo en un punto extremo. 4. Seleccionar el eje xi a lo largo de la normal com´ un que une a los ejes zi−1 y zi , en direcci´on de la articulaci´ on i − 1 hacia la articulaci´ on 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´ on n-´esima es rotatoria, entonces alinear el eje zn con el eje zn−1 Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

4.5

197

Convenci´ on Denavit-Hartenberg

Si la articulaci´ on n-´esima es prism´atica, entonces seleccionar el eje zn de forma arbitraria. El eje xn debe cumplir el paso 4. 7. Establecer la tabla 4.2 de par´ ametros de eslabones. 8. Obtener las matrices de transformaciones homog´eneas (4.7): i Hi−1 para i = 1, 2, · · · , n − 1.

Tabla 4.2 Par´ ametros Denavit-Hartenberg Caracter´ ısticas de eslabones li di αi

Longitud del eslab´ on i-´ esimo. Articulaciones lineales o prism´ aticas. Tamb´ ıen representa el espesor del servomotor (βi ). ´ Angulo entre los ejes zi−1 y zi medido con respecto al eje xi . articulaciones rotacionales; representa el ´ angulo

θi

entre los ejes xi−1 y xi medido alrededor del eje zi−1 .

i En la representaci´ on Denavit-Hartenberg cada transformaci´ on homog´enea Hi−1 se representa por el producto de cuatro transformaciones b´ asicas: 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) ⎦

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

198

Cap´ıtulo 4: Cinem´ atica directa

n−1 n La transformaci´ on homog´enea total se obtiene como H0n = H01 H12 · · · Hn−2 Hn−1 .

La cinem´atica directa es la forma general de transformaciones homog´eneas 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

´ tica directa relaciona las coordenadas articulares y propiedades inema geom´etricas del sistema mec´anico con las coordenadas cartesianas del robot y la orientaci´ on de la herramienta colocada en el extremo final. En este cap´ıtulo se ha presentado los conceptos de cinem´atica inversa, cinem´atica 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´ on de la herramienta de trabajo, se denomina cinem´ atica directa cartesiana, la cual ser´ a la base de an´alisis de las principales configuraciones de robots industriales. El procedimiento Denavit-Hartenberg permite obtener el modelo de cinem´ atica directa de robots manipuladores con eslabones en serie a trav´es de la siguiente tabla de par´ ametros 3.1: Tabla 4.3 Par´ ametros DH Eslab´oni

li

αi

di

θi

La formulaci´on Denavit-Hartenberg queda expresada en t´erminos de matrices homog´eneas con el estricto orden de transformaciones de traslaci´on y rotaci´ on: 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

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

Cap´ıtulo

5

Cinem´atica directa cartesiana

5.1 Introducci´ on 5.2 Brazo robot antropom´ orfico 5.3 Configuraci´ on SCARA 5.4 Robot esf´ erico 5.5 Manipulador cil´ındrico 5.6 Configuraci´ on cartesiana 5.7 Resumen

Objetivos Presentar el an´ alisis de cinem´atica directa cartesiana, cinem´atica inversa y matriz jacobiano de las principales configuraciones de robots industriales considerando los par´ ametros geom´etricos y desarrollar librer´ıas en lenguaje MATLAB (toolbox) que permitan realizar aplicaciones en el ´area de cinem´atica directa de robots manipuladores.

Objetivos particulares: Modelo de cinem´atica directa. Modelo de cinem´atica inversa. Jacobiano. Librer´ıas de an´ alisis y dise˜ no de cinem´atica de robots industriales.

5.1

201

Introducci´ on

5.1 Introducci´ on

E

sta secci´on analiza la cinem´ atica directa cartesiana de las configuraciones m´ as importantes en rob´ otica industrial tales como el brazo robot (antropom´orfico), configuraci´ on SCARA, esf´erico, cil´ındrico y cartesiano. El an´ alisis cinem´atico que se presenta es tomando en cuenta la relaci´on que existe entre las coordenadas articulares con las coordenadas cartesianas del extremo final del robot (cinem´atica directa cartesiana), no se incluye la orientaci´on de la herramienta de trabajo ubicada en el extremo final del robot.

Para el an´ alisis cinem´atico cartesiano se utiliza la metodolog´ıa Denavit-Hartenberg, y por lo tanto dicho modelo queda en funci´on de transformaciones homog´eneas. El jacobiano anal´ıtico del robot se deduce como la derivada parcial de la cinem´atica directa cartesiana con respecto al vector de posici´on articular; el jacobiano anal´ıtico del robot proporciona informaci´ on sobre el problema de singularidades que puede presentar el movimiento del robot en su espacio de trabajo. Esta informaci´on 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´atica inversa en la programaci´ on de tareas en espacio cartesiano y la conversi´ on al espacio articular, se presenta la soluci´ on geom´etrica en detalle para cada una de las configuraciones de robots industriales. Un conjunto de librer´ıas en c´ odigo fuente de MATLAB se desarrollan y documentan de la matriz de transformaci´ on homog´enea, cinem´atica directa cartesiana, jacobiano y su determinante, cinem´atica inversa, en variables simb´ olicas y aplicaciones num´ericas de todas las configuraciones analizadas. El c´odigo 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 ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

202

Cap´ıtulo 5: Cinem´ atica directa cartesiana

5.2 Brazo robot antropom´ orfico

D

entro de la clasificaci´on de robots industriales, el robot antropom´ orfico o brazo robot es la configuraci´ on que m´as 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´ofanos robotizados, fisioterapia, asistencia a personas con capacidades diferenciadas, maniobras submarinas y espaciales, etc. La configuraci´ on antropom´ orfica se divide en tres casos de estudio: p´endulo robot, robot planar de 2 grados de libertad y el brazo robot (movimiento tridimensional). P´ endulo robot

Como un caso particular del robot de 3 grados de libertad antropom´orfico, se encuentra el p´endulo robot de 1 grado de libertad formado por un servomotor que tiene acoplado mec´anicamente una barra met´alica de longitud l1 y est´ a sometido al fen´ omeno de gravedad. El movimiento del p´endulo se encuentra en el plano vertical x0 − y0 como se indica en la figura 5.1.

Figura 5.1 P´endulo robot.

Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

5.2

203

Brazo robot antropom´ orfico

Cinem´ atica directa cartesiana del p´ endulo robot El origen del sistema de referencia cartesiano Σ0 (x0 , y0 , z0 ) se coloca sobre la articulaci´on del p´endulo (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´endulo tiene un espesor de longitud β1 el cual ya incluye el ancho de la barra met´ alica. 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´endulo, 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 a´ngulo α1 que existe entre ellos es α1 = 0. El plano x1 − y1 tiene una rotaci´on de q1 grados con respecto al plano x0 − y0 del sistema fijo Σ0 (x0 , y0 , z0 ), y la matriz Rz0 ( q1 ) determina dicha rotaci´ on: ⎡

− sen(q1 ) 0

cos(q1 )

⎢ R10 = Rz0 ( q1 ) = ⎣ sen(q1 )

cos(q1 )

0

0

⎤

⎥ 0⎦.

1

Los par´ ametros Denavit-Hartenberg correspondientes al p´endulo-robot se encuentran especificados en la tabla 5.1. Tabla 5.1 DH del p´ endulo Eslab´ on

l

α

d

θ

1

l1

0

β1

q1

La matriz de transformaci´ on homog´enea H01 se obtiene de la ecuaci´on (4.7), que para el caso del p´endulo-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

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

204

Cap´ıtulo 5: Cinem´ atica directa cartesiana

Las coordenadas cartesianas del extremo final del p´endulo robot (cinem´ atica 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´endulo dependen de la ubicaci´ on de la posici´on de casa (home position), la cual es el punto de reposo u origen del p´endulo que corresponde a q1 = 0. Como se puede apreciar en la figura 5.1, la posici´on de casa est´a colocada sobre el eje x0+ . La matriz de rotaci´on Rz0 ( q1 ) que determina la rotaci´on 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´on de casa del p´endulo 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´ on de casa del p´endulo sobre el eje y0− . En este caso, para poder obtener la matriz de rotaci´on resultante del p´endulo en la nueva ubicaci´ on de la posici´ on de casa, la matriz Rz0 ( q1 ) debe ser precedida por Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

5.2

205

Brazo robot antropom´ orfico

una rotaci´ on de un a´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´ on homog´enea para est´a determinada como: ⎡ cos(q1 ) − sen(q1 ) ⎡ π ⎢ cos(q1 ) ⎢ Rz0 ( − 2 ) ⎣ sen(q1 ) H01 = ⎢ 0 0 ⎣

el p´endulo-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´endulo respecto a la posici´on de casa colocada sobre el eje y0− adquiere la siguiente forma: ⎡

x0

⎤

⎡

l1 sen(q1 )

⎤

⎢ ⎥ ⎢ ⎥ ⎣ y0 ⎦ = ⎣ −l1 cos(q1 ) ⎦ β1 z0

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

(5.6)

Alfaomega

206

Cap´ıtulo 5: Cinem´ atica directa cartesiana

Jacobiano del p´ endulo Debido a que el p´endulo robot es un sistema escalar, no tiene matriz jacobiana. Sin embargo, se puede realizar la siguiente interpretaci´ on partiendo de la cinem´ atica 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˙1 0 l1 cos(q1 ) 

   J(q1 ) q˙ el jacobiano J(q1 ) se puede pensar como est´a expresado en la ecuaci´on (5.7). N´ otese que existe una singularidad cuando la variable articular q1 = 0, ±nπ, ya que su determinante est´a dado como: det[J(q1 )] = −l12 sen(q1 ) cos(q1 ). Tambi´en existen ´ltimo puede verificarse en la cinem´atica inversa singularidades en q1 = ±n π2 , esto u π (5.8), si q1 = ±n 2 ⇒ x0 = 0, entonces se indefine la funci´on arco-tangente (por ejemplo divisi´ on entre cero). Cinem´ atica inversa del p´ endulo La cinem´atica inversa se obtiene despejando la variable q1 de la ecuaci´on (5.3), obteniendo:   y0 q1 = atan . (5.8) x0 Funci´ on transformaci´ on homog´ enea del p´ endulo H01 La funci´on transformaci´ on homog´enea del p´endulo H01 se encuentra expresada como: H01 =H pendulo() El c´odigo MATLAB de la funci´ on transformaci´ on homog´enea del p´endulo se describe en el cuadro 5.1. En la l´ınea 5 despliega la tabla 5.1 de par´ ametros DH. La l´ınea 7 1 contiene el c´alculo de la matriz homog´enea H0 = HRz0 (q1 )HTz0 (β1 )HTx0 (l1 )HRx0 (0) Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

5.2

Brazo robot antropom´ orfico

207

y en la l´ınea 8 se emplea la funci´on H DH(H10) para obtener la matriz de rotaci´on y las coordenadas cartesianas.

C´ odigo Fuente 5.1

H pendulo.m

%MATLAB Aplicado a Rob´ otica y Mecatr´ onica. %Editorial Alfaomega,

Fernando Reyes Cort´ es.

%Cap´ ıtulo 5 Cinem´ atica directa cartesiana. H pendulo.m 1 2

function H=H pendulo() syms l1 q1 beta1 real

3 5

disp(’Par´ ametros Denavit-Hartenberg del p´endulo’) 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´ on cinem´ atica directa cartesiana del p´ endulo La funci´on de cinem´atica directa cartesiana del p´endulo 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´alica, longitud de la barra met´alica y posici´ on articular del p´endulo, respectivamente. Esta funci´ on retorna las coordenadas cartesianas del extremo final del p´endulo ubicadas en el sistema Σ0 (x0 , y0 , z0 ). El c´odigo de la funci´ on de cinem´atica cartesiana del p´endulo se encuentra en el cuadro 5.2; se contempla el caso simb´ olico y num´erico. ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

208

Cap´ıtulo 5: Cinem´ atica directa cartesiana

C´ odigo Fuente 5.2

cinematica pendulo.m

otica y Mecatr´ onica. %MATLAB Aplicado a Rob´ %Editorial Alfaomega,

Fernando Reyes Cort´ es.

%Cap´ ıtulo 5 Cinem´ atica 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´ olico x0=l1*cos(q1); y0=l1*sin(q1);

5 6 7 8

z0=beta1;

9

end else %caso num´erico

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´ on cinem´ atica inversa del p´ endulo La funci´on de cinem´atica inversa del p´endulo se encuentra expresada como: q1 =cinv pendulo(x0 , y0 ) donde x0 , y0 representan las coordenadas del extremo final del p´endulo con respecto on retorna la posici´ on articular q1 del p´endulo; al sistema Σ0 (x0 , y0 , z0 ). Esta funci´ observe que el modelo de cinem´atica inversa no depende del grosor del servomotor (β1 ).

Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

5.2

209

Brazo robot antropom´ orfico

El c´odigo de la funci´ on de cinem´atica inversa se encuentra en el cuadro 5.3.

C´ odigo Fuente 5.3

cinv pendulo.m

%MATLAB Aplicado a Rob´ otica y Mecatr´ onica. %Editorial Alfaomega,

Fernando Reyes Cort´ es.

%Cap´ ıtulo 5 Cinem´ atica directa cartesiana. cinv pendulo.m 1

function q1=cinv pendulo(x0,y0)

2

%cinem´atica inversa del p´endulo

3

q1=atan(y0/x0);

4

end

♣ Ejemplo 5.1 Desarrollar un programa para MATLAB que presente de manera simb´ olica los par´ ametros DH, cinem´atica cartesiana y el jacobiano del p´endulo. Adem´ as, contemple el an´alisis donde la posici´ on de casa se traslada del primer al cuarto cuadrante.

Soluci´ on En el cuadro 5.4 se presenta el programa que permite desplegar en forma s´ımbolica los par´ ametros DH, cinem´atica cartesiana y el jacobiano del p´endulo. La l´ınea 7 obtiene la transformaci´ on homog´enea del p´endulo H01 empleando la on R10 y la cinem´ atica funci´ on H01 =H pendulo(). La l´ınea 9 deduce la matriz de rotaci´ 1 cartesiana f R (q1 ) a trav´es de la funci´ on H DH(H0 ). En la l´ınea 11 se realiza una rotaci´on HRz0 ( − π2 ) (-90 grados alrededor del eje z0 ) para trasladar la posici´on de casa al cuarto cuadrante. De la l´ınea 13 a la 20 se presenta un ejemplo num´erico, para trasladar las coordenadas del extremo final en la rotaci´ on del primer al cuarto cuadrante. ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

210

Cap´ıtulo 5: Cinem´ atica directa cartesiana

El programa 5.4 contiene la descripci´ on completa del modelo cinem´atico directo del p´endulo robot, el cual emplea las funciones de transformaci´on homog´enea H01 incluye la matriz de rotaci´ on R10 que relaciona la rotaci´on del extremo final del eslab´on con respecto a la base del p´endulo, as´ı como la cinem´atica directa cartesiana en variables simb´olicas como aplicaci´ on num´erica.

C´ odigo Fuente 5.4

cap5 pendulo.m

otica y Mecatr´ onica. %MATLAB Aplicado a Rob´ %Editorial Alfaomega,

Fernando Reyes Cort´ es.

%Cap´ ıtulo 5 Cinem´ atica 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´on R10 y cinem´ atica 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´erico

13

q1=55*pi/180; %posici´ on angular del p´endulo beta1=0.1; %ancho del servomotor m´as espesor de la barra l1=0.45; beta1=0.1; %longitud del p´endulo y espesor del servomotor

9 10

14 15 16 17

%cinem´atica cartesiana cuadrante I: f RI (β1 , l1 , q1 ) [x0,y0,z0]=cinematica pendulo(beta1,l1,q1)

18

%cinem´atica 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

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

5.2

Brazo robot antropom´ orfico

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´on (perpendicular al plano de la hoja). El ancho de cada servomotor y espesor de la barra met´ alica est´an determinados por β1 y β2 . Los ejes x0 , y0 se seleccionan con la regla de la mano derecha. Cinem´ atica 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´on, el eje z1 se coloca paralelo al eje z0 (α1 = 0). El origen del sistema Σ1 (x1 , y1 , z1 ) se a a una distancia ubica en la intersecci´on del eje x0 con el eje z1 (cuando q1 = 0) y est´ β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´ ametros de los eslabones para la convenci´ on ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

212

Cap´ıtulo 5: Cinem´ atica directa cartesiana

Denavit-Hartenberg de un robot planar de dos grados de libertad. Tabla 5.2 DH del robot de 2 gdl Eslab´ on

li

αi

di

θi

1

l1

0

β1

q1

2

l2

0

β2

q2

Generalmente la posici´ on de casa para un robot de dos grados de libertad se selecciona sobre el eje x0+ , midiendo el a´ngulo q1 en sentido contrario a las manecillas del reloj, es decir del eje x0+ hacia el eje y0+ , con esta consideraci´on se obtienen las siguientes matrices de transformaci´ on homog´enea: 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´atica directa de las coordenadas cartesianas del extremo final del robot (sin tomar en cuenta la orientaci´ on de la herramienta de trabajo) est´ a 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 ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

5.2

213

Brazo robot antropom´ orfico

Obs´ervese que la coordenada z0 incluye el espesor de los dos servomotores con sus respectivos espesores de las barras met´alicas, 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´on de casa de un robot manipulador planar de dos grados de libertad tambi´en puede ser ubicada sobre el eje y0− . Esto equivale a hacer una rotaci´on on del hombro, es decir: alrededor del eje z0 por -90 grados para la articulaci´ π 1 HRz0 ( − 2 ) H0 y tambi´en para articulaci´ on del codo HRz1 ( − π2 ) H12 . El a´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´on de la posici´ on 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

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

214

Cap´ıtulo 5: Cinem´ atica 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´enea de rotaci´ on HRz0 ( − π2 ) a la matriz de transformaci´ on homog´enea H02 (5.13). Es decir, H02 y0− = HRz0 ( − π2 ) H02 . Por lo que la cinem´atica directa de coordenadas cartesianas para un robot planar de dos grados de libertad cuya posici´on 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´ on se realiza alrededor de los ejes z1 y z2 , ambos paralelos al on de coordenadas de los par´ametros geom´etricos eje z0 , no existe ninguna modificaci´ β1 y β2 . Por lo que el origen del sistema Σ2 (x2 , y2 , z2 ) siempre tendr´a una distancia β1 + β2 sobre el eje z0 . Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

5.2

215

Brazo robot antropom´ orfico

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´ ametros geom´etricos β1 , β2 , l1, l2 son constantes, y considerando la posici´ on 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´ on 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´ atica inversa del brazo robot de 2 gdl La cinem´atica inversa (sin tomar en cuenta la orientaci´ on de la herramienta de trabajo) de un robot manipulador de dos grados de libertad se obtiene por procedimiento geom´etrico (ver figura 5.5).

Figura 5.5 Cinem´atica inversa de un robot planar de dos grados de libertad. ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

216

Cap´ıtulo 5: Cinem´ atica directa cartesiana

El ´angulo ϑ que se encuentra dentro del tri´ angulo formado por los lados adyacente  l1 + l2 cos(q2 ), cateto opuesto l2 sen(q2 ) y la hipotenusa x20 + y02 , del teorema de Pit´agoras se obtiene la soluci´on 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 a´ngulo ϑ satisface: 

ϑ =

l2 sen(q2 ) atan l1 + l2 cos(q2 )



angulo formado por los catetos Ahora, tomando los a´ngulos ϑ + q1 dentro del tri´  2 2 on: adyacente x0 , opuesto y0 y la hipotenusa x0 + y0 se cumple la siguiente expresi´   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´atica inversa de un robot de 2 gdl no depende de β1 y β2 debido a que no hay proyecciones de estos par´ ametros 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)

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

5.2

217

Brazo robot antropom´ orfico

Funci´ on transformaci´ on homog´ enea del robot 2 gdl H02 La funci´on transformaci´ on homog´enea H02 del robot planar vertical de 2 gdl se encuentra expresada como: H02 =H r2gdl() esta funci´ on retorna H02 la transformaci´ on homog´enea del robot planar vertical de 2 on transformaci´ on homog´enea H02 del robot de gdl. El c´ odigo MATLAB de la funci´ 2 gdl se encuentra en el cuadro 5.5.

C´ odigo Fuente 5.5

H r2gdl.m

otica y Mecatr´ onica. %MATLAB Aplicado a Rob´ %Editorial Alfaomega,

Fernando Reyes Cort´ es.

%Cap´ ıtulo 5 Cinem´ atica directa cartesiana. H r2gdl.m 1 2 3

function H=H r2gdl() syms q1 q2 beta1 beta2 l1 l2 alpha1 alpha2 real disp(’Par´ ametros 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

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

218

Cap´ıtulo 5: Cinem´ atica directa cartesiana

Funci´ on cinem´ atica directa del robot de 2 gdl La funci´on de cinem´atica 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´ametros geom´etricos β1 , l1, β2 , l2 y posiciones articulares q1 , q2 del hombro y codo, respectivamente. Esta funci´on retorna las coordenadas cartesianas (x0 , y0 , z0) en el sistema Σ0 (x0 , y0 , z0 ).

C´ odigo Fuente 5.6

cinematica r2gdl.m

otica y Mecatr´ onica. %MATLAB Aplicado a Rob´ %Editorial Alfaomega,

Fernando Reyes Cort´ es.

%Cap´ ıtulo 5 Cinem´ atica 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´ olico 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´erico 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

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

5.2

Brazo robot antropom´ orfico

219

Funci´ on cinem´ atica inversa del robot de 2 gdl La funci´on de cinem´atica 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´ametros geom´etricos l1 , l2 y las coordenadas cartesianas del extremo final x0 , y0 en el sistema Σ0 (x0 , y0 , z0 ). Esta funci´ on retorna las coordenadas articulares del hombro q1 y codo q2 .

C´ odigo Fuente 5.7

cinv r2gdl.m

otica y Mecatr´ onica. %MATLAB Aplicado a Rob´ %Editorial Alfaomega,

Fernando Reyes Cort´ es.

%Cap´ ıtulo 5 Cinem´ atica 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˜ nar un programa para MATLAB que permita desplegar en forma simb´olica los par´ ametros DH, cinem´atica cartesiana y el jacobiano del robot vertical planar de 2 gdl. Adem´as, programar una aplicaci´ on 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´ on El programa 5.8 contiene el c´odigo fuente para MATLAB que soluciona el problema planteado. En la l´ınea 4 se obtiene la matriz de transformaci´ on homog´enea H02 del robot de 2 gdl y en la l´ınea 7 se obtiene la matriz de rotaci´ on R20 y la cinem´ atica ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

220

Cap´ıtulo 5: Cinem´ atica directa cartesiana

directa cartesiana f R (l1 , q1 , l2 , q2 ). Las l´ıneas 12 y 13 despliegan en forma simb´olica el jacobiano y su determinante, respectivamente. La ecuaci´ on 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´on 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´on 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

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

5.2

221

Brazo robot antropom´ orfico

C´ odigo Fuente 5.8

cap5 robot2gdl.m

otica y Mecatr´ onica. %MATLAB Aplicado a Rob´ %Editorial Alfaomega,

Fernando Reyes Cort´ es.

%Cap´ ıtulo 5 Cinem´ atica 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´ on homog´enea del robot 2 gdl’); disp(H20); [R20, cinemat r2gdl,cero, c]=H DH(H20) ; disp(’Matriz de rotaci´ on’); disp(R20); disp(’cinem´atica 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´erico t=0:0.001:100;%par´ ametros 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´on del c´ırculo x=xc+r*sin(t) ; y=yc+r*cos(t) ;

23

% cinem´atica 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

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

222

Cap´ıtulo 5: Cinem´ atica directa cartesiana

Robot antropom´ orfico (RRR) La gran mayor´ıa de los robots industriales tienen la configuraci´ on antropom´ orfica (brazos robots) ya que presentan mayor destreza en su espacio de trabajo debido a que sus eslabones est´an unidos por articulaciones rotacionales. El espacio de trabajo de la configuraci´on antropom´ orfica corresponde a una esfera, cuyo radio corresponde a la suma de longitudes de los eslabones. Por similitud con la forma anat´omica con el brazo humano, la segunda articulaci´ on se conoce como hombro (shoulder) y la tercera articulaci´ on se llama codo (elbow). La figura 5.7 muestra ejemplos de robots industriales antropom´orficos de la compa˜ n´ıa ABB y FANUC que se utilizan en aplicaciones de traslado de objetos.

Figura 5.7 Robots industriales en configuraci´on antropom´ orfica: ABB y FANUC. Cinem´ atica directa cartesiana del brazo robot de 3 gdl Consid´erese un robot manipulador antropom´ orfico 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´on de la articulaci´ on de la base. Los sistemas de referencia Σ1 (x1 , y1 , z1 ), Σ2 (x2 , y2 , z2 ) y Σ3 (x3 , y3 , z3 ) est´an seleccionados de tal forma que sus ejes z1 , z2 y z3 coincidan con sus respectivos ejes de rotaci´on de cada articulaci´on. El eje z1 del sistema Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

5.2

223

Brazo robot antropom´ orfico

Σ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 a´ngulo q3 con respecto al plano x1 − y1 .

Figura 5.8 Robot antropom´ orfico. La tabla 5.3 contiene los par´ ametros Denavit-Hartenberg para el robot antropom´ orfico 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´ on

Los par´ ametros geom´etricos de cada servomotor son: β1 , β2 , β3 representan el ancho y espesor de las placas met´alicas para la base, hombro y codo, respectivamente; l1 ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

224

Cap´ıtulo 5: Cinem´ atica directa cartesiana

es la distancia que se encuentra la articulaci´on 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´eneas para la configuraci´ on antropom´ orfica 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´enea del robot antropom´ orfico de 3 gdl movi´endose 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´orfico 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

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

5.2

Brazo robot antropom´ orfico

225

Cinem´ atica inversa La cinem´atica inversa del robot antropom´ orfico de 3 gdl consiste en obtener las posiciones articulares en funci´ on de las coordenadas cartesianas de la ecuaci´on (5.26). Una forma para resolver dicho planteamiento es utilizar el m´etodo geom´etrico, con esta intenci´on considere la proyecci´ on de los eslabones del hombro y del codo sobre el plano x0 − y0 (ver figura 5.8); dicha proyecci´ on se ampl´ıa en la figura 5.9 para llevar a cabo el an´ alisis geom´etrico correspondiente a la variable q1 . ametros Observe que existe proyecci´ on geom´etrica sobre el plano x0 − y0 de los par´ β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´endulo y robot de 2 gdl.

Figura 5.9 M´etodo geom´etrico de la cinem´ atica inversa 3 gdl para obtener q1 . De la figura 5.9 tenemos que ϑ = ρ + q1 ⇒ q1 = ϑ − ρ. Observe que el a´ngulo ϑ  se y0 obtiene en funci´ on de las coordenadas cartesianas (x0 , y0 ) satisface ϑ = atan x0 . Adem´as, tomando en cuenta el tri´angulo con l´ınea punteada, el a´ngulo ρ queda en funci´ on 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 ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

226

Cap´ıtulo 5: Cinem´ atica 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´erese como referencia el ´angulo ϑ del tri´ angulo 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 ´angulos ϑ y q2 se miden en direcci´on positiva con respecto al plano horizontal x0 −y0 hacia el eje z0 .

Figura 5.10 M´etodo geom´etrico para obtener q2 y q3 de la cinem´atica inversa 3 gdl. Empleando el teorema de Pit´agoras se obtiene una posible soluci´on para q3 en t´erminos de la funci´ on arco-tangente: x20 + y02 + z02 = [l2 + l3 cos(q3 )]2 + l32 sen2 (q3 ) Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

5.2

227

Brazo robot antropom´ orfico :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´ervese que el tri´angulo formado por los lados   x20 + y02 , z0 y la hipotenusa x20 + y02 + z02 , tomando en cuenta que la tangente de (q3 ) , los ´angulos ϑ + q2 satisface: tan (ϑ + q2 ) = √ z20 2 , adem´as tan (ϑ) = l2l3+lsen 3 cos(q3 ) x0 +y0

usando identidades trigonom´etricas de la funci´on tangente se obtiene: tan (ϑ + q2 ) =

z0 tan (q2 ) + tan (ϑ) = 2 1 − tan (q2 ) tan (ϑ) x0 + y02

Realizando sencillos pasos algebraicos, para determinar la funci´on 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´atica inversa que relaciona las coordenadas articulares q1 , q2 , q3 en funci´ on de las coordenadas cartesianas x0 , y0 , z0 del extremo final de un robot ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

(

228

Cap´ıtulo 5: Cinem´ atica directa cartesiana

antropom´ orfico 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´ orfico de tres grados de libertad est´ a 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´ a 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´ umero infinito de configuraciones singulares para q2 = 0 ± nπ y q3 = 0 ± nπ. Sin embargo, el a´ngulo q1 no interviene con los puntos singulares. Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

5.2

229

Brazo robot antropom´ orfico

Funci´ on transformaci´ on homog´ enea del robot antropom´ orfico de 3 gdl H03 La funci´on transformaci´ on homog´enea H03 del robot antropom´ orfico de 3 gdl tiene la siguiente sintaxis: H03 =H r3gdl() esta funci´ on retorna H03 la matriz de transformaci´ on homog´enea del brazo robot con on articulaciones rotacionales de 3 gdl. El c´odigo en lenguaje MATLAB de la funci´ 3 transformaci´ on homog´enea H0 del robot de 3 gdl est´ a en el cuadro 5.9.

C´ odigo Fuente 5.9

H r3gdl.m

%MATLAB Aplicado a Rob´ otica y Mecatr´ onica. %Editorial Alfaomega,

Fernando Reyes Cort´ es.

%Cap´ ıtulo 5 Cinem´ atica 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´ ametros 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

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

230

Cap´ıtulo 5: Cinem´ atica directa cartesiana

Funci´ on cinem´ atica directa del brazo robot de 3 gdl La funci´on de cinem´atica 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´ on 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´ on retorna las coordenadas cartesianas (x0 , y0 , z0 ).

C´ odigo Fuente 5.10

cinematica r3gdl.m

otica y Mecatr´ onica. %MATLAB Aplicado a Rob´ %Editorial Alfaomega,

Fernando Reyes Cort´ es.

%Cap´ ıtulo 5 Cinem´ atica 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´ olico 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´erico 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

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

5.2

231

Brazo robot antropom´ orfico

Funci´ on cinem´ atica inversa del brazo robot de 3 gdl La funci´on de cinem´atica inversa del brazo robot de 3 gdl tiene la siguiente forma sint´ actica: [q1 , q2 , q3 ]=cinv r3gdl(β2, β3 , l2, l3 , x0, y0 , z0 ) donde los argumentos de entrada son los par´ametros geom´etricos 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´on 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 on retorna las coordenadas articulares (q1 , q2 , q3 ) de la base, Σ0 (x0 , y0 , z0 ). Esta funci´ hombro y codo, respectivamente.

C´ odigo Fuente 5.11

cinv r3gdl.m

otica y Mecatr´ onica. %MATLAB Aplicado a Rob´ %Editorial Alfaomega,

Fernando Reyes Cort´ es.

%Cap´ ıtulo 5 Cinem´ atica 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´on 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´on 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´ on del hombro

11 12

end

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

232

Cap´ıtulo 5: Cinem´ atica directa cartesiana

♣♣♣ Ejemplo 5.3 Dise˜ nar un programa en lenguaje MATLAB que permita desplegar en forma simb´olica los par´ametros DH, cinem´atica cartesiana y el jacobiano del robot antropom´ orfico de 3 gdl. Asimismo programar una aplicaci´ on donde el extremo final del robot trace trayectorias circulares sobre el eje z0 .

Soluci´ on El programa 5.12 presenta el c´odigo fuente en lenguaje MATLAB que despliega en forma simb´ olica los par´ametros DH, cinem´atica 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´on 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´on del tiempo. En la l´ınea 26 la trayectoria circular es convertida a coordenadas articulares usando la cinem´atica 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

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

5.2

Brazo robot antropom´ orfico

C´ odigo Fuente 5.12

233

cap5 r3gdl.m

otica y Mecatr´ onica. %MATLAB Aplicado a Rob´ %Editorial Alfaomega,

Fernando Reyes Cort´ es.

%Cap´ ıtulo 5 Cinem´ atica 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´ on homog´enea del robot antropom´ orfico de 3 gdl’); disp(H30); [R30, cinemat r3gdl,cero, c]=H DH(H30) ; disp(’Matriz de rotaci´ on’); disp(R30); disp(’cinem´atica 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´erico 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)

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

234

Cap´ıtulo 5: Cinem´ atica directa cartesiana

5.3 Configuraci´ on SCARA (RRP)

L

a configuraci´ on SCARA (Selective Compliance Assembly Robot Arm) representa una geometr´ıa especial de robots industriales; es un brazo planar antropom´ orfico con dos articulaciones rotacionales y la tercera articulaci´ on es prism´atica o lineal para manipular objetos. La figura 5.12 muestra un ejemplo de un robot industrial en la configuraci´ on SCARA. Esta configuraci´ on aprovecha las ventajas que proporciona el brazo robot antropom´ orfico de 2 gdl movi´endose en el plano horizontal; en este caso la energ´ıa potencial es constante (par gravitacional cero), la estructura mec´anica es de alta rigidez para soportar cargas en forma vertical y para control de fuerza. La configuraci´ on SCARA es adecuada para tareas de ensamble con peque˜ nos objetos.

Figura 5.12 Robot KR 10 SCARA R600 (Compa˜ n´ıa KUKA). Cinem´ atica 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´ on de la base del robot queda a una distancia l1 m´as el ancho del servomotor on de la primera y espesor de la barra met´alica representado por β1 . El eje de rotaci´ articulaci´on 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´on, donde se intercepta al eje de rotaci´ on de la segunda articulaci´ on rotacional q2 con el eje x0 ; las coordenadas del origen del sistema Σ1 (x1 , y1 , z1 ) con respecto Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

5.3

Configuraci´ on 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´ on con referencia al plano fijo x0 − y0 , entonces 1 on del sistema Σ1 (x1 , y1 , z1 ) con respecto la matriz R0 = Rz0 ( q1 ) describe la rotaci´ al sistema fijo Σ0 (x0 , y0 , z0 ). Los ejes z0 y z1 son paralelos entre s´ı y mantienen la misma direcci´on, por lo tanto el a´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´on por un a´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´ on, justo en la intersecci´on 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´as el espesor de la segunda barra met´alica. El a´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´ on que hay entre los sistemas de referencia Σ1 (x1 , y1 , z1 ) y Σ2 (x2 , y2 , z2 ) es on lineal d3 descrita por la matriz R21 = Rz1 ( q2 ). La medici´on de la tercera articulaci´ se realiza sobre el eje z2 , por lo tanto este eje est´ a alineado sobre su desplazamiento lineal teniendo movimiento positivo en direcci´on hacia abajo, que corresponde a una direcci´on 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 ´angulo α2 = π alrededor del eje x1 , por lo tanto los ejes z2 y z1 son paralelos entre s´ı, pero con direcci´ on contraria. ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

236

Cap´ıtulo 5: Cinem´ atica directa cartesiana

En la configuraci´ on SCARA todos los ejes zi con i = 0, 1, 2 son paralelos entre s´ı, pero debe notarse que el eje z2 mantiene una rotaci´ on de 180 grados con respecto al on por la cual el eje z2 mantiene un direcci´on positiva hacia eje z1 o al eje z0 . La raz´ abajo es por aspectos t´ecnicos, 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´ on de los sistemas de referencia Σ0 (x0 , y0 , z0 ) al Σ2 (x2 , y2 , z2 ) para la configuraci´ on SCARA produce el siguiente conjunto de par´ ametros DenavitHartenberg que se presenta en la tabla 5.4: Tabla 5.4 Par´ ametros DH del robot SCARA Eslab´ on

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´ on homog´enea:

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 ) ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

5.3

237

Configuraci´ on 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´atica directa para el robot SCARA est´ a dada por la siguiente expresi´on: ⎡

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´ on transformaci´ on homog´ enea SCARA El conjunto de ecuaciones que describen la rotaci´ on y coordenadas cartesianas del extremo final del robot SCARA (5.33)-(5.37) permiten desarrollar una librer´ıa con variables simb´ olicas para MATLAB H SCARA() con la siguiente sintaxis: H03 = H SCARA() esta funci´ on retorna la matriz de transformaci´on homog´enea H03 la cual est´a formada con la matriz de rotaci´on R30 que relaciona la rotaci´on del extremo final del robot con respecto al sistema fijo Σ0 (x0 , y0 , z0 ) y la cinem´atica directa que relaciona las as, tambi´en coordenadas articulares con las coordenadas cartesianas f R (q). Adem´ despliega la tabla 5.4 con los par´ ametros Denavit-Hartenberg del robot SCARA. on transformaci´ on El programa 5.13 contiene el c´odigo MATLAB de la funci´ homog´enea del robot SCARA: ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

238

Cap´ıtulo 5: Cinem´ atica directa cartesiana

C´ odigo Fuente 5.13

H SCARA.m

%Transformaci´ on homog´ enea del robot SCARA H03 %MATLAB Aplicado a Rob´ otica y Mecatr´ onica. %Editorial Alfaomega,

Fernando Reyes Cort´ es.

%Cap´ ıtulo 5 Cinem´ atica 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´ on Homog´enea H03 del robot SCARA’) disp(’Par´ ametros Denavit-Hartenberg del robot SCARA’) disp(’[ l alpha d q]’)

3 4 5 6

8

%tabla de par´ ametros DH del robot SCARA dh=[0, 0,l1+beta1, q1; l2, pi, beta2, q2; l3, 0, d3, 0]; %despliega par´ ametros DH del robot SCARA disp(dh)

9

%c´alculo de las matrices de transformaci´on homog´enea de cada articulaci´on

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´on homog´enea del robot SCARA

17

%H03 = H01 H12 H23

18

H30=simplify(H10*H21*H32);

19

%deducci´on de la matriz de rotaci´on R30

20 21

%as´ı como la cinem´atica directa cartesiana f R (q1 , q2, d3 ) del robot SCARA [R30, cinemat scara, cero, c]=H DH(H30);

22

%estructura de la matriz homog´enea

23

H=[R30, cinemat scara; cero, c];

10

24 25

end

Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

5.3

239

Configuraci´ on SCARA (RRP)

Funci´ on cinem´ atica directa La funci´on cinem´ atica 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´ametros geom´etricos:β1, β2 , l1, l2 , l3 y las variables articulares q1 , q2 , d3 . Esta funci´ on 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´ odigo en lenguaje MATLAB de la cinem´atica directa del robot SCARA.

C´ odigo Fuente 5.14

cinematica SCARA.m

%cinem´ atica directa robot SCARA %MATLAB Aplicado a Rob´ otica y Mecatr´ onica. %Editorial Alfaomega,

Fernando Reyes Cort´ es.

%Cap´ ıtulo 5 Cinem´ atica 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 ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

0

0

(5.38) ⎤

⎥ 0 ⎦ −1 Alfaomega

240

Cap´ıtulo 5: Cinem´ atica 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´ atica inversa del robot SCARA El ancho de los servomotores y espesor de las barras met´alicas (β1 , β2 ) no tienen proyecci´on sobre el plano x0 − y0 ; s´olo contribuyen en la coordenada z0 , como en el caso del p´endulo y robot antropom´ orfico de 2 gdl, entonces la cinem´atica inversa del robot SCARA toma la siguiente expresi´ on:

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´ on cinem´ atica inversa del robot SCARA La funci´on cinem´ atica 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´ ametros geom´etricos del robot SCARA, x0 , y0 , z0 son on las coordenadas en espacio cartesiano en el sistema fijo Σ0 (x0 , y0 , z0 ); esta funci´ retorna las coordenadas articulares d3 , q2 , q1 . El programa 5.15 contiene el c´odigo fuente MATLAB de la cinem´atica inversa del robot SCARA, para facilitar la implementaci´on de procedimientos recursivos con pase de par´ ametros escalares o vectoriales, la cinem´atica inversa es programada con operaciones entre arreglos. Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

5.3

241

Configuraci´ on SCARA (RRP)

C´ odigo Fuente 5.15

cinv SCARA.m

%cinem´ atica inversa robot SCARA %MATLAB Aplicado a Rob´ otica y Mecatr´ onica. %Editorial Alfaomega,

Fernando Reyes Cort´ es.

%Cap´ ıtulo 5 Cinem´ atica 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´ on num´erica 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´ olica la matriz jacobiana, determinante y cinem´ atica directa. Soluci´ on En el cuadro 5.16 se presenta el c´ odigo en lenguaje MATLAB en variables simb´olicas de la matriz de transformaci´on homog´enea H03 del robot SCARA usando la funci´ on H30=H SCARA() (ver l´ınea 4). En la l´ınea 5 empleando la funci´ on H DH(H30) se obtiene la matriz de rotaci´ on R30 a trav´es de la funci´ on [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´ on num´erica 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´on en coordenadas cartesianas de la rosa polar (lineas 19 a la 21) est´ a dada por: 6t r = 0.1 sen( ) x = xc + r sen3 (t) 7

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

242 y = yc + r cos3 (t)

Cap´ıtulo 5: Cinem´ atica directa cartesiana z = −0.5

donde xc , yc representan el centro de la flor, r es el radio de los p´etalos y t es el tiempo. El c´alculo de las coordenadas cartesianas de la rosa polar se realiza en forma iterativa usando la instrucci´ on 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´ on cinv SCARA(· · ·). En la l´ınea 27 se usa la funci´on 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´ on del programa 5.16; la finalidad de mostrarlo es ilustrar el c´alculo de las coordenadas cartesianas de la rosa polar sin utilizar la forma recursiva de la instrucci´on 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´ametros (recursivo y por arreglos), lo que facilita la implementaci´on 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´on de la din´ amica del mismo. Estos programas ilustran s´ olo el empleo de la cinem´atica inversa y su conversi´on 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´ amico como puede ser la ecuaci´ on en lazo cerrado (modelo din´ amico del robot y algoritmo de control), entonces se trata de una simulaci´on de un esquema de control. La cinem´atica no reproduce los fen´omenos f´ısicos del robot.

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

5.3

Configuraci´ on SCARA (RRP)

C´ odigo Fuente 5.16

243

SCARA.m

otica y Mecatr´ onica. %MATLAB Aplicado a Rob´ %Editorial Alfaomega,

Fernando Reyes Cort´ es.

%Cap´ ıtulo 5 Cinem´ atica 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´erico

9 11

%par´ametros geom´etricos 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´on 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´on del vector de tiempo for k=1:m %forma recursiva

6

10

14 15 16 17

20

%ecuaci´on 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´on 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

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

244

Cap´ıtulo 5: Cinem´ atica directa cartesiana

C´ odigo Fuente 5.17

SCARA1.m

otica y Mecatr´ onica. %MATLAB Aplicado a Rob´ %Editorial Alfaomega,

Fernando Reyes Cort´ es.

%Cap´ ıtulo 5 Cinem´ atica 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´erico

10

%par´ametros geom´etricos 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´on 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´on del vector de tiempo r=0.1*sin(6*t/7); %radio de la rosa polar

18

%ecuaci´on 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´on 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

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

5.4

Robot esf´ erico (RRP)

245

5.4 Robot esf´ erico (RRP)

L

a configuraci´ on esf´ erica de robots manipuladores presenta dos articulaciones rotacionales (base y hombro) y la articulaci´ on del codo corresponde al tipo prism´atica 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´ on se encuentra el robot Standford, cuya principal aplicaci´ on es el mecanizado de piezas automotrices y la manipulaci´on de objetos sobre piso.

Figura 5.15 Robot manipulador en configuraci´ on esf´erica. Cinem´ atica directa cartesiana El espacio de trabajo de la configuraci´ on esf´erica corresponde a una esfera hueca, cuyo radio se encuentra en funci´ on del desplazamiento lineal d3 de la articulaci´ on prism´atica del codo. El eje z0 se alinea con el eje de giro de la articulaci´on 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´on de la base est´a a una altura l1 sobre el eje z0 . El ancho del servomotor de la base y el espesor de la placa met´alica est´an representados por β1 . Por otro lado, el eje z1 determina la medici´ on de la variable articular q2 del hombro y se encuentra alineado con el eje de rotaci´on de esta articulaci´on. 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´on del hombro y las coordenadas del origen de este sistema respecto al sistema fijo Σ0 (x0 , y0 , z0 ) est´an en: [ β1 cos(q1 ) β1 sen(q1 ) l1 + β1 ]T . El eje z2 se alinea con el desplazamiento lineal de ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

246

Cap´ıtulo 5: Cinem´ atica directa cartesiana

la articulaci´ on prism´atica 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´ on esf´erica. La tabla 5.5 presenta los par´ ametros de la convenci´ on Denavit-Hartenberg para el robot en la configuraci´ on esf´erica.

Tabla 5.5 DH del robot esf´ erico li

αi

di

θi

1

0

l 1 + β1

q1

2

0

- π2 π 2

β2

q2

3

0

0

d3

0

Eslab´ on

Las matrices de transformaci´on homog´enea para el robot en la configuraci´ on esf´erica tienen la siguiente estructura: H01 = HRz0 ( q1 ) HTz0 ( l1 + β1 ) HTx0 ( 0 ) HRx0 ( − pi 2 ) Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

5.4

⎡

cos(q1 )

0

⎢ sen(q ) 0 1 ⎢ = ⎢ ⎣ 0 −1 H12

H23

247

Robot esf´ erico (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´atica directa de un robot manipulador en configuraci´ on esf´erica 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´erico 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 ) ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

248

Cap´ıtulo 5: Cinem´ atica directa cartesiana

Funci´ on transformaci´ on homog´ enea robot esf´ erico La funci´on matriz de transformaci´ on homog´enea H esferico() del brazo robot en la configuraci´ on esf´erica tiene la siguiente sintaxis: H03 = H esferico() esta funci´ on retorna la matriz de transformaci´on homog´enea H03 integrada por la matriz de rotaci´ on R30 que describe la rotaci´on del extremo final del robot con respecto al sistema fijo Σ0 (x0 , y0 , z0 ) y la cinem´atica directa cartesiana f R (q) que relaciona las coordenadas articulares. Adem´as, tambi´en despliega en forma simb´olica la tabla 5.5 con los par´ ametros Denavit-Hartenberg. El programa 5.18 contiene el on transformaci´ on homog´enea del robot esf´erico. c´odigo MATLAB de la funci´

C´ odigo Fuente 5.18

H esferico.m

otica y Mecatr´ onica. %MATLAB Aplicado a Rob´ %Editorial Alfaomega,

Fernando Reyes Cort´ es.

%Cap´ ıtulo 5 Cinem´ atica 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´ ametros 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

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

5.4

249

Robot esf´ erico (RRP)

Funci´ on cinem´ atica directa del robot esf´ erico La funci´on de cinem´atica directa que relaciona las coordenadas cartesianas del extremo final con las coordenadas articulares del brazo robot en configuraci´ on esf´erica est´a dada por la siguiente sintaxis (el c´ odigo en lenguaje MATLAB se encuentra en el cuadro 5.19): [x0 , y0 , z0 ]=cinematica esferico(β1, β2 , l1, q1 , q2 , d3)

C´ odigo Fuente 5.19

cinematica esferico.m

otica y Mecatr´ onica. %MATLAB Aplicado a Rob´ %Editorial Alfaomega,

Fernando Reyes Cort´ es.

%Cap´ ıtulo 5 Cinem´ atica 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´ olico

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´erico 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

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

250

Cap´ıtulo 5: Cinem´ atica directa cartesiana

donde l1 es la distancia sobre el eje z0 donde est´a ubicada la articulaci´ on 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´ atica inversa Para obtener la cinem´atica inversa de la configuraci´ on esf´erica se emplea el m´etodo geom´etrico que se presenta a detalle en la figura 5.17. Los ´angulos ϑ + q1 satisfacen tan (ϑ + q1 ) = xy00 . Note que el a´ngulo ϑ queda ubicado en el interior  del tri´ angulo formado por el cateto adyacente x20 + y02 − β22 , cateto opuesto β2  y la hipotenusa x20 + y02 , entonces tan (ϑ) = √ 2 β2 2 2 . La proyecci´ on del x0 +y0 −β2

movimiento de la articulaci´on 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 ) on tan (q1 ) = trigonom´etrica tan (ϑ + q1 ) = 1−tan (ϑ) tan (q1 ) , se obtiene la expresi´ √ 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´atica inversa de la configuraci´ on esf´erica. La cinem´atica inversa de la configuraci´ on esf´erica toma la siguiente forma: Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

5.4

251

Robot esf´ erico (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´ on cinem´ atica inversa robot esf´ erico La sintaxis de la funci´on de cinem´atica inversa de la configuraci´on esf´erica es (ver c´odigo en el cuadro 5.20); esta funci´ on retorna las coordenadas articulares. [q1, q2, d3]=cinv esferico(β1, β2 , l1, x0 , y0 , z0 )

C´ odigo Fuente 5.20

cinv esferico.m

otica y Mecatr´ onica. %MATLAB Aplicado a Rob´ %Editorial Alfaomega,

Fernando Reyes Cort´ es.

%Cap´ ıtulo 5 Cinem´ atica 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

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

252

Cap´ıtulo 5: Cinem´ atica directa cartesiana

♣♣♣ Ejemplo 5.5 Escribir un programa en lenguaje MATLAB que permita desplegar en forma simb´olica los par´ametros DH, matriz homog´enea, cinem´atica cartesiana, jacobiano y determinante del robot esf´erico. Adem´as, programar una aplicaci´ on 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´a desplazado un mil´ımetro sobre el eje z0 (de 0 a 100 mm).

Soluci´ on El programa que permite resolver el problema planteado se encuentra en el cuadro 5.21. La informaci´ on simb´ olica del robot esf´erico se encuentra en las l´ıneas 4 a la 15 (par´ ametros DH, matriz homog´enea, cinem´atica 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´on: x = xc + r sen(t)

x0 , y0 y el desplazamiento variable est´a dado por z. La l´ınea 25 convierte las coordenadas cartesianas del c´ırculo a coordenadas articulares del robot usando la funci´ on cinv esferico. La l´ınea 26 realiza la conversi´on 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´erico (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´on 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´on del robot esf´erico.

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

5.4

253

Robot esf´ erico (RRP)

C´ odigo Fuente 5.21

cap5 esferico.m

otica y Mecatr´ onica. %MATLAB Aplicado a Rob´ %Editorial Alfaomega,

Fernando Reyes Cort´ es.

%Cap´ ıtulo 5 Cinem´ atica 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´ on homog´enea del robot esf´erico’);

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´ on’); disp(R30); disp(’cinem´atica 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´erico

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´on 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)

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

254

Cap´ıtulo 5: Cinem´ atica directa cartesiana

5.5 Manipulador cil´ındrico (RPP)

L

a configuraci´ on cil´ındrica tiene la articulaci´ on de la base rotacional, mientras que las articulaciones del hombro y codo son lineales o prism´aticas. Entre las aplicaciones m´as importantes de los robots manipuladores en esta configuraci´ on 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´on cil´ındrica.

Figura 5.19 Robot en configuraci´ on cil´ındrica. Cinem´ atica directa cartesiana El espacio de trabajo de la configuraci´ on 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´ on rotacional de la base se encuentra a una distancia l1 sobre el eje z0 . Este eje se alinea con el eje de rotaci´on del servomotor, de esta forma la variable articular q1 gira alrededor del eje z0 . El ancho del servomotor y espesor de las placas met´alicas que se utilizan para acoplar mec´anicamente la siguiente articulaci´ on se encuentra expresado por el par´ ametro geom´etrico β1 . Note que el par´ametro l1 produce s´ olo coordenadas positivas para z0 , cuando este par´ametro es cero o el origen de Σ0 (x0 , y0 , z0 ) se coloca sobre la articulaci´on 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´ on Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

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 a´ngulo q1 con respecto al sistema fijo Σ0 (x0 , y0 , z0 ), la matriz de rotaci´ on que relaciona dicha rotaci´on entre estos dos sistemas de referencia es: 1 R0 = Rz0 ( q1 ).

Figura 5.20 Espacio de trabajo del robot cil´ındrico. La articulaci´on 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 a´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´on 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´ on lineal del codo el ´angulo alrededor del eje z0 es q1 + π2 . Observe tambi´en on positiva la direcci´ on para coordenadas negativas del eje y2 coincide con la direcci´ 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´ an indicados los sistemas de referencia Σ0 (x0 , y0 , z0 ), Σ1(x1 , y1 , z1 ) on de casa del brazo robot en y Σ2 (x2 , y2 , z2 ) en la figura 5.21 determinan la posici´ configuraci´ on cil´ındrica, es decir para esa configuraci´ on de sistemas de referencia las ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

256

Cap´ıtulo 5: Cinem´ atica 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´on de casa, la cinem´atica directa del robot cil´ındrico adquiere una forma cartesiana diferente. Los par´ ametros geom´etricos 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´anicas de sus articulaciones, por lo que no influyen directamente en la expresi´ on anal´ıtica de la cinem´ atica directa del brazo robot en configuraci´ on cil´ındrica. En la tabla 5.6 se muestran los par´ ametros del robot manipulador en la configuraci´ on cil´ındrica. Tabla 5.6 DH del robot cil´ındrico Eslab´ on

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´ on cil´ındrica tiene las siguientes matrices de transformaci´ on homog´enea: 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 ) ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

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´atica directa para el robot manipulador cil´ındrico est´a 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´on 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´a dado por: det[J(q1 , d2, d3 )] = d3

(5.59)

la u ´nica singularidad que presenta el brazo robot en configuraci´ on cil´ındrica es cuando la articulaci´ on 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´ on para deshabilitar el movimiento del robot cuando la articulaci´on lineal d3 pasa por el origen del sistema Σ0 (x0 , y0 , z0 ). ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

258

Cap´ıtulo 5: Cinem´ atica directa cartesiana

Funci´ on transformaci´ on homog´ enea robot cil´ındrico La funci´on matriz de transformaci´ on homog´enea del brazo robot en configuraci´ on cil´ındrica tiene la siguiente sintaxis: H03 = H cilindrico() retorna la matriz de transformaci´on homog´enea H03 compuesta por la matriz de atica directa cartesiana f R (q) que relaciona las coordenadas rotaci´ on R30 y la cinem´ cartesianas del extremo final con las coordenadas articulares. Tambi´en despliega los par´ ametros Denavit-Hartenberg en forma simb´olica que se muestran en la tabla 5.6. El programa 5.22 contiene el c´odigo de la funci´ on transformaci´ on homog´enea.

C´ odigo Fuente 5.22

H cilindrico.m

%MATLAB Aplicado a Rob´ otica y Mecatr´ onica. %Editorial Alfaomega,

Fernando Reyes Cort´ es.

%Cap´ ıtulo 5 Cinem´ atica directa cartesiana. H cilindrico.m 1 function H=H cilindrico() 2 syms q1 d2 d3 beta1 l1 alpha1 alpha2 alpha3 real 3 disp(’Par´ ametros 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

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

5.5

259

Manipulador cil´ındrico (RPP)

Funci´ on cinem´ atica directa del robot cil´ındrico La funci´on de cinem´atica directa del brazo robot cil´ındrico est´a 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 on Σ0 (x0 , y0 , z0 ). El cuadro 5.23 contiene el c´odigo en lenguaje MATLAB para la funci´ cinem´atica del robot cil´ındrico:

C´ odigo Fuente 5.23

cinematica cilindrico.m

%MATLAB Aplicado a Rob´ otica y Mecatr´ onica. %Editorial Alfaomega,

Fernando Reyes Cort´ es.

%Cap´ ıtulo 5 Cinem´ atica 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´ olico

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´erico x0= -d3.*sin(q1); y0= d3.*cos(q1);

14

z0=l1+beta1+d2;

11 12

15 16

end end

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

260

Cap´ıtulo 5: Cinem´ atica directa cartesiana

Cinem´ atica inversa La cinem´atica inversa del brazo robot en configuraci´ on cil´ındrica se obtiene de la cinem´atica directa (5.57) de la siguiente forma: primero que nada, debe considerarse que debido a que la posici´ on de casa [ q1 , d2, d3 ]T = [ 0, 0, 0 ]T seleccionada para este brazo robot se caracteriza por la articulaci´on del codo d3 est´a 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 a´ngulo q1 + π2 (ver figura 5.21). Por lo tanto, se cumple: q1 +

π 2

= atan − xy00 .

La articulaci´on del hombro tiene un desplazamiento lineal sobre el eje z0 , entonces d2 = z0 − (l1 + β1 ). La articulaci´on 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´atica inversa del robot cil´ındrico. Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

5.5

Manipulador cil´ındrico (RPP)

261

La cinem´atica inversa del brazo robot en configuraci´ on cil´ındrica est´a dada como:

q1 d2 d3

  x0 π = − + atan − 2 y0 = z0 − (l1 + β1 )  = ± x20 + y02

(5.60) (5.61) (5.62)

Funci´ on cinem´ atica inversa robot cil´ındrico La funci´on de cinem´atica inversa del robot cil´ındrico convierte las coordenadas on de las coordenadas articulares cartesianas del extremo final (x0 , y0 , z0 ) en funci´ (q1 , d2, d3 ) con la siguiente sintaxis: [q1 , d2, d3 ]=cinv cilindrico(β1, l1, x0 , y0 , z0 ) donde β1 , l1 son los par´ ametros geom´etricos, x0 , y0 , z0 son las coordenadas cartesianas del extremo final. Esta funci´ on retorna las coordenadas articulares q1 , d2, d3 de la base, hombro y codo, respectivamente. El cuadro 5.24 contiene la implementaci´on de la funci´on cinem´ atica inversa del brazo robot cil´ındrico.

C´ odigo Fuente 5.24

cinv cilindrico.m

otica y Mecatr´ onica. %MATLAB Aplicado a Rob´ %Editorial Alfaomega,

Fernando Reyes Cort´ es.

%Cap´ ıtulo 5 Cinem´ atica 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

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

262

Cap´ıtulo 5: Cinem´ atica directa cartesiana

♣♣♣ Ejemplo 5.6 Escribir un programa en lenguaje MATLAB que permita desplegar en forma simb´olica los par´ametros DH, cinem´atica cartesiana, jacobiano y determinante del robot cil´ındrico. Asimismo programar una aplicaci´ on 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´ on El desplegado simb´ olico de los par´ametros DH, cinem´atica 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´on homog´enea H03 utilizando la funci´ on H cilindrico(). La matriz de transformaci´ on homog´enea H03 se utiliza como argumento de entrada en la funci´on D HD(H03 ) para obtener la matriz atica directa de rotaci´ on R30 y la cinem´ (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´on cinv cilindrico(β1, l1 , x, y, z) (ver l´ınea 24). La l´ınea 26 contiene la conversi´on de coordenadas cartesianas del extremo final del robot a coordenadas articulares usando la funci´on 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´on: 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´on del robot cil´ındrico.

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

5.5

263

Manipulador cil´ındrico (RPP)

C´ odigo Fuente 5.25

cap5 cil´ındrico.m

%MATLAB Aplicado a Rob´ otica y Mecatr´ onica. %Editorial Alfaomega,

Fernando Reyes Cort´ es.

%Cap´ ıtulo 5 Cinem´ atica 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´ on homog´enea del robot cil´ındrico’); [R30, cinemat cilindrico,cero, c]=H DH(H30); disp(’Matriz de rotaci´ on’); disp(R30); disp(’cinem´atica 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´erico t=0:0.001:100; %ecuaci´on 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´on 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

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

264

Cap´ıtulo 5: Cinem´ atica directa cartesiana

5.6 Configuraci´ on cartesiana (PPP)

L

os robots manipuladores que incluyen sus tres primeras articulaciones del tipo prism´atico o lineales se denominan robots cartesianos, tambi´en conocidos como robots p´ orticos 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´aticos, mesas de medici´on de coordenadas como la que se muestra la figura 5.23, impresoras l´ aser o de matriz son otros ejemplos de robots cartesianos con 2 gdl.

Figura 5.23 Robot cartesiano. Particularmente, la configuraci´ on cartesiana facilita la programaci´on del extremo final del robot y sus potenciales aplicaciones, ya que la cinem´atica directa es un mapa lineal entre coordenadas articulares y cartesianas. Sin embargo, la estructura mec´anica del robot cartesiano presenta baja destreza de movilidad comparado con la configuraci´ on antropom´ orfica debido a que todas sus articulaciones son prism´aticas. Cinem´ atica 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´ on prism´atica 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

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

5.6

Configuraci´ on cartesiana (PPP)

265

regla de la mano derecha. El siguiente sistema de referencia Σ1 (x1 , y1 , z1 ) se obtiene rotando el eje x0 un a´ngulo α1 = − π2 , quedando el eje z1 en direcci´on positiva de on negativa del eje z0 y el eje la variable d2 . Observe que el eje y1 apunta en direcci´ x1 es paralelo al eje x0 , separado evidentemente por la dimensi´on f´ısica de la mesa cartesiana denotada por la longitud l2 . El sistema Σ2 (x2 , y2 , z2 ) sirve para medir el on de este desplazamiento lineal de la variable articular d3 . Sin embargo, la obtenci´ 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 ´angulo α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´ on de 90 grados con respecto al eje z1 , esta rotaci´ on 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´enticas. El origen del sistema de referencia auxiliar Σ2a(x2a, y2a, z2a) es el mismo que el sistema on de referencia Σ1 (x1 , y1 , z1 ), pero el sistema Σ2a(x2a, y2a, z2a) mantiene una rotaci´ π on Rz1 ( 2 ), es decir: relativa al sistema Σ1 (x1 , y1 , z1 ) expresada por la matriz rotaci´ ⎡ ⎡ ⎤ ⎤ x2a x2 ⎢ ⎢ ⎥ ⎥ T ⎣ y2a ⎦ = Rz1 ( π2 ) ⎣ y2 ⎦ z2a z2 Posteriormente, girando un a´ngulo α2 =

π 2

alrededor del eje x2a se obtiene el sistema

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

266

Cap´ıtulo 5: Cinem´ atica directa cartesiana

de referencia Σ2 (x2 , y2 , z2 ) donde la articulaci´ on prism´atica d3 se mueve linealmente sobre el eje z2 . Esta u ´ltima rotaci´on α2 alrededor del eje x2 determina el ´angulo 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 a´ngulo α1 = − π2 alrededor del eje x0 genera el sistema Σ1 (x1 , y1 , z1 ). El paso 2 consiste en rotar por un a´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 ´angulo α2 = − π2 alrededor del eje x2a .

Figura 5.25 Sistemas de referencia del robot cartesiano.

La tabla de par´ ametros del robot manipulador de 3 gdl en configuraci´on cartesiana queda de la siguiente forma:

Tabla 5.7 Par´ ametros Denavit-Hartenberg para el robot cartesiano de 3 gdl Eslab´on

Alfaomega

li

αi

di

θi

1

0

d1

0

2

0

- π2 - π2

d2

0

3

0

0

d3

0

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

5.6

267

Configuraci´ on cartesiana (PPP)

La matriz de transformaci´ on homog´enea para la primera articulaci´ on 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´atica directa del robot cartesiano est´ a dado por: ⎡ ⎤ ⎡ ⎤ x0 d3 ⎢ ⎥ ⎢ ⎥ ⎣ y0 ⎦ = ⎣ d2 ⎦ z0

(5.67)

d1

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

268

Cap´ıtulo 5: Cinem´ atica directa cartesiana

Jacobiano del robot cartesiano La matriz jacobiano del robot cartesiano est´a 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´ on cartesiana ofrece ventajas debido que no tiene singularidades. Funci´ on transformaci´ on homog´ enea robot cartesiano La sintaxis de la funci´on transformaci´ on homog´enea del brazo robot cartesiano es: H03 =H cartesiano() esta funci´ on retorna la matriz de transformaci´on homog´enea H03 compuesta por la matriz de rotaci´ on R30 que describe la rotaci´on del extremo final del robot con respecto al sistema fijo Σ0 (x0 , y0 , z0 ) y la cinem´atica directa cartesiana f R (q) que relaciona las coordenadas articulares. Adem´as, tambi´en despliega los par´ametros Denavit-Hartenberg en forma simb´ olica de la tabla 5.7. El programa 5.26 contiene el c´odigo MATLAB de la funci´ on transformaci´ on homog´enea del robot cartesiano. Utiliza las matrices de transformaci´on homog´eneas alculo de cada una de las de rotaci´ on HRz ( qi ) y traslaci´on HTz ( d ) para el c´ 1 2 3 matrices homog´eneas de las articulaciones: H0 , H1 , H2 . Tambi´en emplea la funci´on atica directa cartesiana H DH(H30) para obtener la matriz de rotaci´on R30 y la cinem´ f R (d1 , d2, d3 ). Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

5.6

269

Configuraci´ on cartesiana (PPP)

C´ odigo Fuente 5.26

H cartesiano.m

%MATLAB Aplicado a Rob´ otica y Mecatr´ onica. %Editorial Alfaomega,

Fernando Reyes Cort´ es.

%Cap´ ıtulo 5 Cinem´ atica directa cartesiana. H cartesiano.m 1

function H=H cartesiano()

2

syms l1 l2 l3 d1 d2 d3 alpha1 alpha2 alpha3 real disp(’Par´ ametros 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´ ametros DH disp(dh)

8

%c´alculo de las matrices de transformaci´on homog´enea de cada articulaci´on

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´ on R30 y cinem´ atica directa f R (d1 , d2, d3 )

20

[R30, cinemat cartesiano, cero, c]=H DH(H30);

21

%Forma la matriz de transformaci´ on homog´enea 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

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

270

Cap´ıtulo 5: Cinem´ atica directa cartesiana

Funci´ on cinem´ atica directa del robot cartesiano La sintaxis de la funci´on de cinem´atica directa de la configuraci´ on cartesiana es: [x0 , y0 , z0 ]=cinematica cartesiano(d3, d2, d1 ) donde d3 , d2 , 13 son las posiciones articulares del robot en configuraci´on cartesiana. Esta funci´ on retorna las coordenadas cartesianas (x0 , y0 , z0 ) en el sistema on cinem´ atica del robot cartesiano. Σ0 (x0 , y0 , z0 ). El cuadro 5.27 contiene funci´ C´ odigo Fuente 5.27

cinematica cartesiano.m

otica y Mecatr´ onica. %MATLAB Aplicado a Rob´ %Editorial Alfaomega,

Fernando Reyes Cort´ es.

%Cap´ ıtulo 5 Cinem´ atica 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´ olico x0=d3; y0=d2; z0=d1; x0=vpa(x0); y0=vpa(y0); z0=vpa(z0); else %caso num´erico

5 6 7 8

x0=d3; y0=d2; z0=d1;

9 10

end end

Funci´ on cinem´ atica inversa robot cartesiano La funci´on de cinem´ atica 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

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

5.6

271

Configuraci´ on cartesiana (PPP)

Esta funci´ on retorna las coordenadas articulares. El cuadro 5.28 contiene el c´odigo de la cinem´atica inversa del robot cartesiano:

C´ odigo Fuente 5.28

cinv cartesiano.m

%MATLAB Aplicado a Rob´ otica y Mecatr´ onica. %Editorial Alfaomega,

Fernando Reyes Cort´ es.

%Cap´ ıtulo 5 Cinem´ atica 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´olica los par´ametros DH, cinem´atica directa y el jacobiano del robot cartesiano. Adem´as, implementar una aplicaci´on 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´ on El programa que se presenta en el cuadro 5.29 permite desplegar en forma simb´olica los par´ametros DH, cinem´ atica directa, jacobiano del robot cartesiano y matriz de transformaci´ on 3 homog´enea H0 . 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.

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

272

Cap´ıtulo 5: Cinem´ atica directa cartesiana

C´ odigo Fuente 5.29

cap5 cartesiano.m

otica y Mecatr´ onica. %MATLAB Aplicado a Rob´ %Editorial Alfaomega,

Fernando Reyes Cort´ es.

%Cap´ ıtulo 5 Cinem´ atica 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´ on homog´enea del robot cartesiano’); disp(H30); [R30, cinemat cartesiano,cero, c]=H DH(H30); disp(’Matriz de rotaci´ on’); disp(R30); disp(’cinem´atica 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´erico

17

t=0:0.001:100;

18

%ecuaci´on de la flor con 8 p´etalos 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

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

5.7

Resumen

273

5.7 Resumen

C

´ tica directa cartesiana de robots manipuladores permite una descripinema ci´on entre las coordenadas cartesianas del extremo final del robot manipulador y las coordenadas articulares. La convenci´on Denavit-Hartenberg es una herramienta de la ingenier´ıa de suma utilidad para la obtenci´ on del modelo de cinem´atica directa de robots manipuladores la cual establece que la matriz de transformaci´ on homog´enea de la i-´esima articulaci´ on se compone de 4 par´ ametros: li es la longitud de los eslabones, αi el ´angulo que hay entre el eje zi−1 y zi , di es el desplazamiento on no es prism´ atica o lineal, enlineal sobre el eje zi−1 ; cuando la i-´esima articulaci´ tonces di representa el ancho del servomotor y espesor de la placa met´alica, en este caso se especifica por βi . Para articulaciones rotacionales alrededor del eje zi−1 se denota por qi . De acuerdo al an´alisis de cinem´atica directa cartesiana para cada configuraci´ on de robots industriales (antropom´orfica, SCARA, esf´erico, cil´ındrico y cartesiano), se han desarrollado un conjunto de librer´ıas de cinem´atica directa de robots manipuladores para MATLAB (toolbox) que permiten obtener los par´ ametros Denavit-Hartenberg, la matriz homog´enea, cinem´atica directa, matriz jacobiano y su determinante en funci´ on de los par´ ametros geom´etricos como longitudes li y tambi´en tomando en cuenta el ancho de la i-´esima articulaci´ on y espesor de la placa met´ alica (βi ) que se emplea para acoplar mec´anicamente la siguiente articulaci´ on del robot. Este conjunto de librer´ıas tienen la ventaja de trabajar en forma simb´olica, ya que despliegan no s´ olo las ecuaciones de cinem´atica, tambi´en permiten realizar aplicaciones num´ericas como manejo de coordenadas con valores espec´ıficos o trayectorias de curvas para que el extremo final del robot pueda trazarlas. La programaci´ on indistinta en lenguaje MATLAB de variables simb´ olicas y num´ericas resulta de gran utilidad sobre todo en el an´alisis cinem´atico, planeaci´ on de trayectorias, guiado del robot en su espacio de trabajo y detecci´on de puntos singulares en la cinem´atica diferencial o en procesos donde involucra la matriz inversa del jacobiano del robot. ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

274

Cap´ıtulo 5: Cinem´ atica directa cartesiana

En el sitio WEB del libro se encuentran disponibles todos los programas fuentes en MATLAB de las funciones de cinem´atica cartesiana, matrices de transformaci´ on homog´enea, cinem´atica inversa, jacobianos y determinantes, as´ı como diversas aplicaciones de las configuraciones analizadas de robots manipuladores. Para una r´ apida identificaci´on de las librer´ıas desarrolladas, a continuaci´ on se resume la sintaxis del conjunto de funciones para cinem´ atica directa cartesiana con pase de par´ ametros de entrada y datos que retorna de cada las configuraciones de robots manipuladores analizados. La tabla 5.8 contiene la descripci´on de funciones de cinem´ atica para la configuraci´ on antropom´ orfica considerando los casos de estudio del p´endulo robot, robot planar vertical de 2 gdl y robot con movimiento tridimensional en su espacio de trabajo (3 gdl).

Tabla 5.8 Configuraci´ on antropom´ orfico Funci´ on

Sintaxis H01 =H pendulo() [x0 , y0 , z0 ]=cinematica pendulo(β1, l1 , q1 )

P´endulo

q1 =cinv pendulo(x0, y0 ) H02 =H r2gdl() Robot antropom´ orfico 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´ orfico 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

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

5.7

275

Resumen

En la tabla 5.9 se resume las funciones de transformaci´on homog´enea H03 , cinem´atica directa, cinem´atica inversa, matriz jacobiano y su determinante del brazo robot en configuraci´ on SCARA. Tabla 5.9 Configuraci´ on SCARA Funci´ on

Sintaxis

Matriz de transformaci´ on homog´enea

[x0 , y0 , z0 ]=cinematica SCARA(β1 , β2, l1 , l2, l3 , q1 , q2 , d3 )

Cinem´atica 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´atica inversa

El conjunto de librer´ıas para la configuraci´on esf´erica se presenta en la tabla 5.10. Tabla 5.10 Configuraci´ on esf´ erica Funci´ on

Sintaxis

Matriz de transformaci´ on homog´enea

[x0 , y0 , z0 ]=cinematica esferico(β1, β2 , l1, q1 , q2 , d3)

Cinem´atica 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´atica inversa

[q1, q2, d3]=cinv esferico(β1, β2 , l1, x0 , y0 , z0 )

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

276

Cap´ıtulo 5: Cinem´ atica directa cartesiana

La tabla 5.11 contiene las librer´ıas del brazo robot en configuraci´ on cil´ındrica.

Tabla 5.11 Configuraci´ on cil´ındrica Funci´ on

Sintaxis

Matriz de transformaci´ on homog´enea

[x0 , y0 , z0 ]=cinematica cilindrico(β1, l1 , q1 , d2, d3 )

Cinem´atica 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´atica inversa

Para el caso del brazo robot cartesiano, las librer´ıas que corresponde a esta configuraci´ on se encuentran en la tabla 5.12. La cinem´atica directa resulta muy simple debido a que su cinem´atica 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´atica inversa tambi´en es lineal [ d1 d2 d3 ]T = [ x0 y0 z0 ]T .

Tabla 5.12 Configuraci´ on cartesiana Funci´ on

Sintaxis

Matriz de transformaci´ on homog´enea

H03 = H cartesiano()

Cinem´atica directa

[x0 , y0 , z0 ]=cinematica cartesiano(d3, d2 , d1)

Cinem´atica inversa

[d3 , d2 , d1]=cinv cartesiano(x0 , y0 , z0 )

Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

277

Parte II: Referencias selectas

Parte II: Referencias selectas

E

xiste una extensa literatura sobre la cinem´atica 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´ otica”. Tercera edici´on. Pearson Prentice-Hall. 2006.

Fernando Reyes.

“Rob´ otica: control de robots manipuladores ”.

Editorial Alfaomega, 2011. ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

278

Cap´ıtulo 5: Cinem´ atica directa cartesiana

Parte II: Problemas propuestos

E

n esta secci´on se presenta una serie de ejercicios con la finalidad de que el lector mejore sus conocimientos sobre m´etodos num´ericos.

Cap´ıtulo 3 Preliminares matem´ aticos

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 ´angulo θ que existe entre los vectores. Realice la programaci´ on considerando variables simb´ olicas y aspectos num´ericos. 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 a´ngulo

π 2.

(b) Alrededor del eje x0 por un a´ngulo − π2 . (c) Alrededor del eje y0 por un a´ngulo 43 π Analice y discuta sus resultados. Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

279

Parte II: Problemas propuestos

3.3 Sea Ry ( θ ) ∈ SO(3) una matriz ortogonal donde θ ∈ IR es el ´angulo de rotaci´ on alrededor del eje y. Implementar el c´ odigo MATLAB correspondiente para comprobar las siguientes propiedades (usar programaci´ on simb´ olica y num´erica): (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´on. 3.5 Describir el significado de una matriz homog´enea. 3.6 ¿Cu´ales son las principales propiedades de una matriz homog´enea?

Cap´ıtulo 4 Cinem´ atica

4.1 ¿C´omo define cinem´atica? 4.2 ¿Qu´e es cinem´atica directa? 4.3 ¿Cu´ales son las caracter´ısticas principales de la cinem´atica directa? 4.4 ¿Qu´e es cinem´atica inversa? Explicar claramente su funci´ on principal. 4.5 Explicar el concepto de cinem´atica diferencial. 4.6 ¿Qu´e es el jacobiano del robot? 4.7 ¿Cu´al es la diferencia que existe entre el jacobiano anal´ıtico con el geom´etrico? 4.8 Explicar el concepto de posici´ on y rotaci´ on de robots manipuladores. ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

280

Cap´ıtulo 5: Cinem´ atica directa cartesiana

4.9 Explicar el significado de una singularidad. 4.10 Explicar el concepto de posici´on de casa de un robot manipulador. 4.11 ¿Cu´al es la diferencia entre rotaci´ on y orientaci´ on? 4.12 ¿Por qu´e la expresi´on anal´ıtica de la cinem´ atica directa depende de la posici´on de casa? 4.13 ¿Cu´ales son los tipos de robots industriales que existen? 4.14 Explicar cinco aplicaciones para cada una de las configuraciones: antropom´orfica, SCARA, esf´erica, cil´ındrica y cartesiana. 4.15 ¿Qu´e robot representa mayores ventajas? 4.16 Desde el punto de vista mec´anico, ¿qu´e tipo de robot presenta mayor complejidad? 4.17 Explicar las ventajas que presenta la convenci´on Denavit-Hartenberg. 4.18 Describir los par´ametros de an´alisis que presenta la convenci´ on 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´e la convenci´on Denavit-Hartenberg s´olo utiliza cuatro par´ ametros? 4.20 ¿Cuales son las principales hip´ otesis que debe satisfacer la convenci´on DenavitHartenberg?

Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

Parte II: Problemas propuestos

281

Cap´ıtulo 5 Cinem´ atica directa cartesiana

5.1 Considere un sistema mecatr´ onico 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´ametros DH. (b) La matriz homog´enea. (c) Cinem´atica directa. (d) Cinem´atica inversa. Asimismo, realizar un programa que despliegue en forma simb´ olica los anteriores incisos. 5.2 Considere el robot manipulador en configuraci´ on 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´atica directa. ¿En este caso se requiere un sistema auxiliar Σ2a(x2a, y2a, z2a)? ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

282

Cap´ıtulo 5: Cinem´ atica directa cartesiana ¿Este procedimiento presenta alguna ventaja con el desarrollado en la secci´on 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´on antropom´ orfica; desarrolle un programa en MATLAB para que el extremo final del robot trace una rosa polar de 12 p´etalos 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´ on SCARA trace la palabra ROBOT en estilo caligr´afico. Dicha palabra deber´ a iniciar en las siguientes coordenadas: T [ x0 , y0 , z0 ] = [ 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´ on cil´ındrica trace la palabra HOLA en estilo italizada. Dicho trazo deber´a 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´ on esf´erica trace la palabra HOLA con tipo de letra romana. Dicha palabra deber´ a comenzar en las siguientes coordenadas: T [ x0 , y0 , z0 ] = [ 0.35, 0.6, 0.2] m. Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

Parte III Din´amica

La Din´amica como ´area de las ciencias exactas permite explicar todos los fen´omenos f´ısicos de un sistema mecatr´onico o robot manipulador y llevar a cabo procesos de simulaci´on. Bajo esta tem´ atica se ubican los objetivos de la Parte III compuesta por dos cap´ıtulos: Cap´ıtulo 6 Din´ amica, presenta la t´ecnica de simulaci´on para sistemas mecatr´onicos y robots manipuladores en base a una ecuaci´ on diferencial ordinaria de primer orden descrita por variables de estado; modelos din´ amicos de varios sistemas mecatr´onicos y robots manipuladores se desarrollan en c´odigo fuente para MATLAB. La t´ecnica de m´ınimos cuadrados para obtener el valor num´erico de los par´ ametros del modelo din´amico se detalla en el Cap´ıtulo 7 Identificaci´ on param´ etrica.

Cap´ıtulo 6 Din´amica

presenta la forma de simular modelos din´ amicos descritos en variables de estado. Generalmente la mayor´ıa de los sistemas f´ısicos tienen su modelo din´amico con ecuaciones diferenciales de orden mayor o igual a dos en variables f´ısicas o generalizadas. La t´ecnica de simulaci´on consiste en convertir el modelo din´ amico a una ecuaci´on diferencial de primer orden mediante un adecuado cambio de variables de estado. Se desarrollan librer´ıas en lenguaje MATLAB para simular la din´ amica de sistemas mecatr´onicos y robots manipuladores.

Cap´ıtulo 7 Identificaci´on param´etrica

presenta cincos esquemas de regresi´on lineal para realizar identificaci´ on param´etrica de sistemas mecatr´onicos y robots manipuladores. Los esquemas de regresi´ on presentados son los modelos: din´amico, din´ amico filtrado, energ´ıa, potencia y potencia filtrada. La t´ecnica de m´ınimos cuadrados recursivo es implementada como librer´ıa para MATLAB cubriendo los casos: escalar y multivariable de sistemas din´amicos.

La Parte III tambi´en incluye:

Referencias selectas Problemas propuestos

Din´amica

Cap´ıtulo

6

6.1 Introducci´ on 6.2 Estructura matem´ atica para simulaci´ on 6.3 Sistema masa-resorte-amortiguador 6.4 Sistema lineal escalar 6.5 Centr´ıfuga 6.6 P´ endulo 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´ecnica de simulaci´on de modelado de robots manipuladores y sistemas mecatr´onicos a trav´es de la estructura de ecuaci´on diferencial ordinaria en variables fase.

Objetivos particulares: Estructura matem´ atica fundamental de simulaci´on. Sistema masa resorte amortiguador. Centr´ıfuga. Robot antropom´ orfico: p´endulo, robot de 2 y 3 gdl. Robot cartesiano de 3 gdl. Aplicaciones en control de posici´ on.

6.1

Introducci´ on

287

6.1 Introducci´ on

R

obots manipuladores son sistemas mec´anicos muy complejos cuya descripci´on anal´ıtica requiere de ecuaciones diferenciales. La naturaleza no lineal, multivariable y fuerte acoplamiento en su comportamiento din´amico ofrece un amplio espectro en la formulaci´ on de problemas de control te´ oricos y pr´ acticos. El modelo din´ amico del robot manipulador permite explicar todos los fen´ omenos f´ısicos que se encuentran en su estructura mec´ anica, tales como efectos inerciales, fuerzas centr´ıpetas y de Coriolis, par gravitacional y fricci´on, los cuales son fen´omenos f´ısicos propios de la naturaleza din´ amica del robot. Hay varios m´etodos de modelado de la f´ısica como el de Newton o el de Hamilton. Sin embargo, la mejor opci´ on como metodolog´ıa de modelado la representa las ecuaciones de movimiento de EulerLagrange debido a las propiedades matem´ aticas que se deducen de manera natural, ya que facilitan el an´alisis y dise˜ no de algoritmos de control.

El modelo din´amico de robots manipuladores es fundamental en aplicaciones de simulaci´on, dise˜ no y construcci´ on del sistema mec´anico, as´ı como en an´ alisis y dise˜ no de algoritmos de control. En el a´rea de simulaci´ on el modelo din´amico es estrat´egico debido a que puede reproducir todos los fen´omenos 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´ecnicas de planeaci´on de trayectorias, programaci´ on de aplicaciones industriales, etc. La simulaci´ on es el empleo del modelo din´amico para analizar y describir su comportamiento din´ amico en una computadora o sistema m´ınimo digital y de ah´ı inferir aplicaciones. Es importante no confundir simulaci´ on con animaci´ on, ya que son procesos diferentes; la animaci´on no requiere incorporar efectos din´amicos en el movimiento del robot, generalmente son ecuaciones est´aticas como la cinem´atica directa. La construcci´on de un robot se fundamenta en el modelo din´amico; los esquemas y planos de ingenier´ıa de los eslabones se deducen directamente del modelo din´amico y se trasladan a un programa CAD para su maquinado y construcci´ on mec´anica. De esta forma, un robot industrial puede ser estudiado y se pueden hacer las adecuaciones pertinentes antes de llegar a la etapa de construcci´on f´ısica. ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

288

Cap´ıtulo 6: Din´ amica

6.2 Estructura matem´ atica para simulaci´ on

L

a gran mayor´ıa de los sistemas mec´anicos contienen como parte del modelo din´ amico ecuaciones diferenciales de orden mayor o igual a 2. Sin embargo, para prop´ ositos de simulaci´on es recomendable transformar ese modelo a un sistema din´ amico compuesto por una ecuaci´ on diferencial ordinaria de primer orden debido a que este tipo de ecuaciones es conocida, est´a ampliamente documentada y es f´acil de programar. La estructura de una ecuaci´ on diferencial ordinaria (ode) de primer orden tiene la siguiente forma: x˙ = f (x)

(6.1)

donde x ∈ IRn se conoce como variable de estado fase, la cual proporciona informaci´ on interna sobre la din´ amica del sistema, es una funci´on continua en el tiempo x = x(t), n ∈ N es un n´ umero natural que indica la dimensi´ on euclidiana, n la derivada temporal de la variable de estado x˙ ∈ IR existe y tambi´en es continua ˙ en el tiempo x˙ = x(t). La notaci´on x˙ significa x˙ = dxdt(t) . La funci´ on f ∈ IRn es un mapa vectorial continua en la variable de estado x(t). La estructura matem´ atica x˙ = 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´amico aut´ onomo, debido a que la variable tiempo t se encuentra presente de manera impl´ıcita, es decir no aparece como parte de la estructura matem´atica, se encuentra incluida como parte de las propiedades de la variable de estado x(t). La gran mayor´ıa de los sistemas mec´anicos corresponden a esta forma, sobre todo considerando que no hay variaci´ on temporal de sus par´ ametros, los cuales se consideran constantes al menos en un tiempo suficientemente grande. De otra manera se tendr´ıa como parte de la ecuaci´on (6.1) dependencia expl´ıcita del tiempo, es decir x˙ = f (x, t); a este tipo de sistemas se les conoce como sistema din´ amicos no aut´ onomos, los cuales no non objetos de estudio en la presente obra. Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

6.2

289

Estructura matem´ atica para simulaci´ on

♣ Ejemplo 6.1 Convertir a la forma x˙ = f (x) el siguiente sistema lineal expresado por la funci´on de transferencia: y u

=

2 s2 + 3s + 1

donde y ∈ IR es la respuesta del sistema, u ∈ IR es la entrada.

Soluci´ on El concepto de funci´ on de transferencia es exclusivo para sistemas lineales, se define como la relaci´on de la salida y la entrada cuando las condiciones iniciales del sistema son cero. Es necesario aclarar que el t´ermino s que aparece en una funci´ on de transferencia significa frecuencia y es un n´ umero imaginario formado como: s = jw, siendo w una variable que indica frecuencia, cuyas unidades son rad/seg. Cuando transformamos la funci´ on de transferencia a una ecuaci´ on diferencial, entonces s adquiere otra d interpretaci´ on, 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´ on de transferencia del sistema a una ecuaci´ on diferencial ordinaria de primer orden es el siguiente: y u

2 ⇒ s2 y + 3sy + y = 2u + 3s + 1 ⇒ y¨ + 3y˙ + y = 2u =

s2

˙ La notaci´ on y˙ = donde se ha empleado s2 y = y¨, sy = y.

dy(t) dt ,

y¨ =

d2 y(t) dt2

Ahora, se requiere un cambio de variable de estado, sea x1 = y, x2 = x˙ 1 , x˙ 2 = −3x2 − x1 + 2u entonces: y¨ + 3y˙ + y = 2u ⇒

x˙ 2 +3 x2 + x1 = 2u    y¨

y˙

y

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

290

Cap´ıtulo 6: Din´ amica   d x1 dt x   2  x˙

 = 

0

1

−1

−3



x1



x  2 f ( x)

+

  0 2

u 

donde x = [ x1 , x2 ]T . Cuando la ecuaci´on diferencial original se encuentra en la forma de variables fase x˙ = f(x), entonces el sistema din´amico (convertido) ya tiene la estructura matem´atica adecuada para realizar simulaci´on usando la funci´ on ode45(· · ·).

♣ Ejemplo 6.2 Convertir a la forma x˙ = f(x) el siguiente sistema no lineal: α2 y¨ + α1 sen(y) ˙ + α0 cos(y) = βu donde y ∈ IR es la respuesta del sistema, u ∈ IR es la entrada, α2 , α1 , α0 , β ∈ + .

Soluci´ on La funci´on de transferencia es exclusiva de sistemas lineales, para un sistema no lineal no existe dicho concepto. La conversi´on a la forma de ecuaci´on diferencial ordinaria de primer orden en variables fase se realiza de la siguiente forma. Sea x1 = y, x2 = x˙ 1 , x2 = atica solicitada (−α1 sen(x2 )−α0 cos(x1 )+βu)/α2 . Por lo tanto, la estructura matem´ es:     x2 d x1 = 1 dt x2 [ βu − α1 sen(x2 ) − α0 cos(x1 ) ]    α0    x˙ f ( x)

Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

6.3

291

Sistema masa-resorte-amortiguador

6.3 Sistema masa-resorte-amortiguador

U

no de los sistema mec´anicos t´ıpicos que ha sido estudiado ampliamente en cursos de control autom´ atico de la carreras de ingenier´ıa mecatr´onica y electr´onica es el sistema masa resorte amortiguador. La importancia de este sistema radica en comprender su comportamiento din´ amico para entender el funcionamiento cualitativo de otros sistemas, por ejemplo: circuitos el´ectricos (resistencia, capacitor, inductancia), esquemas de control (algoritmo proporcional derivativo de robots manipuladores). Considere un sistema din´amico 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´a especificada por una fuerza f y el desplazamiento de la masa por x.

Figura 6.1 Sistema masa resorte amortiguador. El modelo din´amico que describe al sistema masa resorte amortiguador se encuentra dado por: f  Fuerza externa

=

m¨ x  Fuerza inercial

+

bx˙  Fricci´on viscosa

+

kx  Ley de Hooke

La funci´on de transferencia del sistema masa resorte amortiguador est´ a dada por: x 1 = (6.2) 2 f ms + bs + k la ecuaci´on caracter´ıstica de los polos del sistema est´a dada por √ −b ± b2 − 4mk s = (6.3) 2k ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

292

Cap´ıtulo 6: Din´ amica

El sistema masa resorte amortiguador oscilar´ a cuando se cumpla (b2 − 4mk) < 0, y ser´a amortiguado si (b2 − 4mk) > 0. La conversi´on de la ecuaci´ on (6.2) a una ecuaci´on diferencial ordinaria de primer orden en variables fase x˙ = [ x, x˙ ]T se realiza de la siguiente forma:     x˙ d x (6.4) = dt x˙ m−1 [ f − bx˙ − kx ]       x˙ f ( x) ♣ Ejemplo 6.3 Considere el modelo din´amico del sistema masa resorte amortiguador (6.4) utilizando los valores num´ericos de los par´ametros 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´es de este tiempo desaparece la fuerza aplicada. Tabla 6.1 Par´ ametros del sistema masa resorte amortiguador Masa

Fricci´on viscosa

m=5 kg

b =0.16 kg/seg

Rigidez k =0.6 kg/seg2

Soluci´ on El programa en c´odigo MATLAB que contiene el modelo din´amico 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´on 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´ on 2 (b − 4mk) < 0 de la ecuaci´on (6.3) que corresponde a un oscilador, por eso cuando Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

6.3

Sistema masa-resorte-amortiguador

293

desaparece la fuerza constante despu´es de 5 segundos el sistema se queda oscilando permanentemente (el tiempo de simulaci´on es de 100 segundos). En realidad el sistema masa resorte amortiguador cuando se encuentra en la configuraci´ on de oscilador debido a los valores num´ericos de sus par´ametros, el sistema empezar´a a oscilar con una condici´ on diferente a cero, independientemente de la magnitud de la fuerza aplicada. Ahora, sup´ ongase que los valores de los par´ametros son: m=5 kg, la constante de rigidez del resorte k = 2 kg/seg2 y el coeficiente de fricci´on 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´on (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´ on en modo amortiguado. Unicamente, durante los primeros 5 segundos se le aplica la fuerza de f =5 N. Despu´es de este transitorio la masa regresa a su posici´on inicial, restableci´endose la elongaci´on del resorte y desapareciendo el efecto de amortiguamiento. Obs´ervese que cuando desaparece la fuerza constante, la posici´on del sistema es atra´ıda por el punto de equilibrio, quedando en reposo.

C´ odigo Fuente 6.1

Simulador de smr.m

%MATLAB Aplicado a Rob´ otica y Mecatr´ onica. %Editorial Alfaomega,

Fernando Reyes Cort´ es

%Cap´ ıtulo 4 Din´ amica. Programa smr simu.m Simulador de smr.m 1

clc; clear all; close all; format short

2

%par´ametros de simulaci´on 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))

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

294

Cap´ıtulo 6: Din´ amica

C´ odigo Fuente 6.2

Sistema masa resorte amortiguador

%MATLAB Aplicado a Rob´ otica y Mecatr´ onica. %Editorial Alfaomega,

Fernando Reyes Cort´ es

%Cap´ ıtulo 4 Din´ amica. 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´on articular x mra=x(1);

5

%velocidad articular

6

xp mra=x(2);

7

%par´ametros 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´on viscosa b=0.16;

14

%fuerza aplicada durante 5 segundos

15

18

if (t 0 o q˙i < 0), entonces esta componente no mostrar´ a variaci´on temporal de tal forma que el algoritmo de m´ınimos cuadrados tendr´ a divergencia en estos par´ ametros. Uno de los motivos por lo cual se obtiene convergencia param´etrica en los par´ametros de fricci´on est´atica es debido al fuerte acoplamiento de la din´amica no lineal que se ve reflejado en el regresor multivariable. En contraste con los m´etodos escalares este acoplamiento es m´as d´ebil. De ah´ı que la deriva en dichos par´ ametros es m´as pronunciada. El m´etodo de la potencia tambi´en Tabla 7.2 Resultados de identificaci´ on param´ etrica del robot de 2 gdl Par´ ametro

Valor real

Din´ amico

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´on param´etrica, debido que no se realiza ning´ un tipo de aproximaci´ on de la aceleraci´on. No obstante, el Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

7.7

373

Robot cartesiano de 3 gdl

requerir medir la aceleraci´ on representa una desventaja pr´actica. Por otro lado, el m´etodo de regresi´on de energ´ıa tiene pobre desempe˜ no, realiza la identificaci´on con mayor margen de error. Note que los dos esquemas escalares divergen en los par´ ametros de fricci´ on est´atica Otro aspecto importante que afecta la calidad en el proceso de identificaci´on param´etrica es la forma de dise˜ nar la se˜ nal de excitaci´ on persistente. Preferentemente debe tener estructuras senoidales cuyas amplitudes no saturen a los servoamplificadores.

7.7 Robot cartesiano de 3 gdl

E

l modelo din´amico del robot manipulador en configuraci´ on cartesiana de 3 gdl est´a formado por la matriz de masas, para gravitacional y fen´ omeno de fricci´ on: ⎡ ⎤ ⎡ ⎤⎡ ⎤ ⎡ ⎤⎡ ⎤ ⎡ ⎤⎡ ⎤ 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 ) ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

374

Cap´ıtulo 7: Identificaci´ on param´ etrica

7.7.1 7.7.1. Modelo de regresi´ on din´ amico del robot cartesiano

El modelo de regresi´on 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´on ψ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´ amico del robot cartesiano no se ha considera la fricci´on est´atica, que particularmente es muy peque˜ na para servomotores con tecnolog´ıa de transmisi´ on directa. Note que dicho modelo din´ amico no tiene el fen´omeno de fuerzas centr´ıpetas y de Coriolis, esto se debe a que en esta configuraci´ on s´olo tiene articulaciones lineales o prism´aticas. Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

7.7

Robot cartesiano de 3 gdl

375

♣♣♣ Ejemplo 7.14 Realizar identificaci´ on param´etrica de un robot en configuraci´ on cartesiana de 3 gdl empleando el modelo din´amico. Utilice los par´ametros del robot de 3 gdl indicados en la tabla 6.6.

Soluci´ on El error de predicci´ on del modelo din´ amico del robot cartesiano de 3 gdl est´ a 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´odigo MATLAB donde se ha implementado el modelo din´ amico del robot cartesiano de 3 gdl (programa cap7 idecartesiano3gdl.m), el cual contiene el efecto inercial, fen´omeno de fricci´on viscosa y Coulomb, par gravitacional, y la se˜ nal de prueba o par aplicado a los servomotores (excitaci´ on persistente). Los valores de los par´ ametros del robot se han tomado de la tabla 6.6. El programa cap7 idecartesiano3gdlsimu.m permite llevar a cabo el proceso de identificaci´on param´etrica (ver cuadro 7.20). En la l´ınea 2 se declaran las variables globales para el registro de las se˜ nales del par aplicado. La integraci´ on num´erica del modelo din´amico del robot cartesiano de 3 gdl se realiza en la l´ınea 7. A trav´es de la soluci´ on num´erica se obtiene la aceleraci´on articular y pares aplicados (l´ınea 13). ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

376

Cap´ıtulo 7: Identificaci´ on param´ etrica

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´on m´ınimos cuadrados multivariable se emplea en la l´ınea 34 para obtener el vector de par´ ametros estimados ˆ θ cuyo resultado est´ a indicado en la tabla 7.3.

C´ odigo Fuente 7.19

cap7 idecartesiano3gdl.m

%MATLAB Aplicado a Rob´ otica y Mecatr´ onica. %Editorial Alfaomega,

Fernando Reyes Cort´ es.

%Cap´ ıtulo 7 Identificaci´ on param´ etrica. 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´on 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´amico 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´ on viscosa Fc=[theta7, 0, 0; 0, theta8, 0; 0, 0, theta9]; %fricci´ on de Coulomb par grav = [theta10; 0; 0]; %vector de pares de gravitacionales fr= B*dp+Fc*sign(dp); %vector de fricci´ on

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´on articular xp = [dp(1); dp(2); dp(3); dpp(1); dpp(2); dpp(3)]; %vector de salida

22 23

end

Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

7.7

Robot cartesiano de 3 gdl

C´ odigo Fuente 7.20

377

cap7 idecartesiano3gdlsimu.m

otica y Mecatr´ onica. %MATLAB Aplicado a Rob´ %Editorial Alfaomega,

Fernando Reyes Cort´ es.

%Cap´ ıtulo 7 Identificaci´ on param´ etrica. 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´ on num´erica 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´ on 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

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

378

Cap´ıtulo 7: Identificaci´ on param´ etrica

7.7.2 7.7.2. Modelo de potencia del robot cartesiano de 3 gdl El modelo din´amico de potencia del robot manipulador cartesiano de 3 gdl est´ a 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´on param´etrica de un robot cartesiano de 3 gdl empleando el modelo de potencia. Utilice los par´ ametros del robot de 3 gdl indicados en la tabla 6.6.

Soluci´ on El error de predicci´on del regresor lineal del modelo de potencia del robot cartesiano de 3 gdl est´a 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 6 ˆ 7 6 θ4 7 7 6 6 θˆ 7 6 5 7 ψ110 ] 6 ˆ 7 6 θ6 7 7 6 6 θˆ 7 6 7 7 6 ˆ 7 6 θ8 7 7 6 4 θˆ9 5 θˆ10 | {z } ˆ (k−1) θ

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

7.7

379

Robot cartesiano de 3 gdl

donde los componentes del regresor est´an compuestas por: ψ11 = d˙1 d¨1 ψ12 = d˙2 d¨2 ψ13 = d˙3 d¨3 ψ14 = d˙2 1

ψ15 ψ16 ψ17

= d˙22 = d˙23 = |d˙1 |

ψ18 = |d˙2 | ψ19 = |d˙3 | ψ110 = d˙1 El programa cap7 potenciacartesiano3gdlsimu.m muestra el c´odigo para realizar la identificaci´on param´etrica 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´ a definido en la l´ınea 23. La funci´ on de m´ınimos cuadrados versi´on escalar se emplea para obtener ˆ (l´ınea 24). En la l´ınea 26 se grafica la variaci´ el vector de par´ametros estimados θ on ˆ temporal que tienen las componentes del vectore θ (ver figura 7.1).

Figura 7.1 Comportamiento de los componentes θˆi . ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

380

Cap´ıtulo 7: Identificaci´ on param´ etrica

C´ odigo Fuente 7.21

cap7 potenciacartesiano3gdlsimu.m

%MATLAB Aplicado a Rob´ otica y Mecatr´ onica. %Editorial Alfaomega,

Fernando Reyes Cort´ es.

%Cap´ ıtulo 7 Identificaci´ on param´ etrica. 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´ on num´erica de la din´ amica 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;

25

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); ˆ theta %vector estimado θ

26

ˆ plot(t,r) %grafica la variaci´on temporal de los componentes del vector θ

23 24

Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

7.7

381

Robot cartesiano de 3 gdl

7.7.3 7.7.3. An´ alisis comparativo de identificaci´ on Los resultados de identificaci´ on param´etrica del robot cartesiano de 3 gdl de los modelos de regresi´on din´ amico y potencia se encuentran contenidos en la tabla 7.3. Para prop´ ositos de comparaci´on se proporcionan los valores reales de cada uno de los componentes del vector θ. El modelo de regresi´on din´ amico tiene una identificaci´on exacta de los diez par´ ametros, este modelo de regresi´on es multivariable ya que toma como parte del regresor las se˜ nales de pares τ1 , τ2 , τ3 , la matriz de regresi´on con las observaciones de posici´ on, velocidad y aceleraci´on tiene dimensi´on: ψ ∈ IR3×10 . Por otro lado, el modelo de regresi´on de la potencia resulta un esquema escalar, en este caso las se˜ nales del par no forman parte del vector de mediciones de la entrada del robot, m´ as 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´on: ψ ∈ IR10×1 . El esquema escalar es mucho m´as atractivo que la versi´ on vectorial o multivariable debido a que su implementaci´on es mucho m´as f´ acil. Tabla 7.3 Par´ ametros identificados del robot cartesiano de 3 gdl Par´ ametro

Valor real

Din´ amico

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´on din´ amico (multivaribale) y potencia (escalar) requieren la medici´on de la aceleraci´on, lo que representa una desventaja cuando el robot no dispone de los sensores adecuados para medir esa se˜ nal. ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

382

Cap´ıtulo 7: Identificaci´ on param´ etrica

7.8 Resumen

E

l m´etodo de m´ınimos cuadrados es una t´ecnica de identificaci´on param´etrica que permite obtener el valor num´erico de los par´ ametros del sistema empleando mediciones experimentales de posiciones y velocidades articulares. La naturaleza del modelo matem´ atico a identificar puede ser diversa, por ejemplo: din´ amico (lineal y no-lineal), est´atico, continuo o discreto. Sin embargo, una condici´ on necesaria del m´etodo de m´ınimos cuadrados es que el modelo matem´atico del sistema puede ser expresado como un regresor lineal de los par´ametros a identificar, es decir como el producto de una matriz de observaciones y un vector columna de par´ ametros desconocidos; esta estructura genera un regresor vectorial o multivariable. Ejemplos de esta categor´ıa son los modelos din´ amico y din´amico 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´ ametros desconocidos, entonces el regresor lineal se denomina escalar. Los modelos de regresi´on de la energ´ıa, potencia y potencia filtrada son ejemplos del regresor lineal escalar.

Se han desarrollado dos librer´ıas en c´ odigo fuente MATLAB para resolver el problema de identificaci´ on param´etrica: ˆ =mincuad(y,fi) [r, θ]

θˆ =mincuadm(y,fi,Nob,p,n) Estas funciones permiten obtener la identificaci´on param´etrica de sistemas mecatr´onicos y robots manipuladores reales. La calidad de estimaci´ on param´etrica est´a en funci´ on del tipo de se˜ nal aplicada (excitaci´ on persistente), la exactitud de aproximar las se˜ nales de velocidad y aceleraci´on y en la medida de lo posible no incluir en la matriz o vector de observaciones funciones discontinuas. Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

383

Parte III Referencias selectas

Parte III Referencias selectas

D

entro de la extensa literatura sobre din´ amica de sistemas mec´anicos, se recomiendan ampliamente los siguientes textos para que el lector pueda profundizar en din´ amica de sistemas mecatr´onicos y robots manipuladores.

Dare A. Wells. “Lagrangian dynamics”. Schaum’s outlines 1967.

Leonard Meirovitch. “Methods of analytical dynamics”. McGrawHill. 1970.

A. P. French. “Mec´ anica newtoniana”. Editorial Revert´e, 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´ amica cl´ asica de las part´ıculas y sistemas”. Editorial Revert´e, S. A. 2000.

H. Goldstein. “Mec´ anica cl´ asica”. Editorial Revert´e, S. A. 2000.

G. C. Goodwin & K. S. Sin. “Adaptive filtering prediction and control ”. Prentice-Hall, NY. 1984. ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

384

Cap´ıtulo 7: Identificaci´ on param´ etrica

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¨ ufer, 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

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

385

Parte III Problemas propuestos

Parte III Problemas propuestos

E

n esta secci´on se presenta una serie de ejercicios con la finalidad de que el lector mejore sus conocimientos sobre din´amica e identificaci´on param´etrica.

Cap´ıtulo 6 Din´ amica

6.1 Considere la siguiente funci´ on 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´amico. (b) Realizar la conversi´ on 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´as considere condiciones iniciales diferentes a cero. Realizar la simulaci´ on del sistema masa resorte amortiguador y analizar: (a) La respuesta del sistema (posici´on y velocidad). (b) Explicar el comportamiento del sistema. Fundamente sus respuestas. ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

386

Cap´ıtulo 7: Identificaci´ on param´ etrica

6.3 Modificar los programas 6.9 y 6.10 para simular el p´endulo robot usando la siguiente entrada: τ

= 5 sen(t) + 0.33 cos(t + 0.1random(’Normal’,t,t))

Obtener: (a) Filtrado de la posici´ on q1 (t). (b) Estimaci´ on 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´ orfico de 2 gdl en las siguientes configuraciones: (a) [ qd1 , qd2 ]T = [ 45, 90 ]T grados. (b) [ qd1 , qd2 ]T = [ 180, 0 ]T grados. ¿Qu´e m´etodo 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(˜ q1 ) tanh(q˙1 ) 0 0 kv1 0 ⎢ ⎥ ⎢ ⎥⎢ ⎥⎢ ⎥ ⎢ ⎥ q2 ) ⎦ − ⎣ 0 kv2 0 ⎦ ⎣ tanh(q˙2 ) ⎦ + g(q) ⎣ τ2 ⎦ = ⎣ 0 kp2 0 ⎦ ⎣ tanh(˜ τ3

0

0

kp3

tanh(˜ q3 )

0

0

kv3

tanh(q˙3 )

para controlar las posiciones articulares de la base, hombro y codo del robot antropom´ orfico 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´e m´etodo de sinton´ıa en las ganancias proporcional derivativa puede proponer, para el robot de 3 gdl? Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

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 (˜ q1 ) atan (q˙1 ) kv1 0 ⎢ ⎥ ⎢ ⎥⎢ ⎥⎢ ⎥ ⎢ ⎥ q2 ) ⎦ − ⎣ 0 kv2 0 ⎦ ⎣ atan (q˙2 ) ⎦ + g(q) ⎣ τ2 ⎦ = ⎣ 0 kp2 0 ⎦ ⎣ atan (˜ τ3 0 0 kp3 atan (˜ q3 ) 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´e m´etodo de sinton´ıa en las ganancias proporcional derivativa puede proponer, para el robot cartesiano?

Cap´ıtulo 7 Identificaci´ on param´ etrica

7.1 Sean los siguientes sistemas din´amicos escalares: x˙ = −3x + 8 sen(x) + 8.98x3 y˙ = 6.66y + 5.61e−|y| + 1.23 senh(y) z˙ = 2.345z 3 + 6.1234z 4 − 2.31z 5 Para cada uno de los sistemas propuestos implementar un esquema de identificaci´on param´etrica para obtener el valor de sus respectivos par´ametros. Tome un intervalo para y ∈ [−10, 10]. 7.2 Sean los siguientes sistemas multivariables:        x˙ 1 0 1 x1 0 = + sen(t) −3 −4 2 x˙ 2 x2 ⎡ ⎤ ⎡ ⎤⎡ ⎤ ⎡ ⎤ x˙ 1 0 1 1 x1 4 ⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥ ⎣ x˙ 2 ⎦ = ⎣ −1 −2 −2 ⎦ ⎣ x2 ⎦ + ⎣ 2 ⎦ sen(t) 1 4 −6 9 x˙ 3 x3 ⎡ ⎤ ⎡ ⎤⎡ ⎤ ⎡ ⎤ x˙ 1 0.01 3.1 4.1 0.9 x1 3.2 sen(t) ⎢ x˙ ⎥ ⎢ 2.2 ⎢ ⎥ ⎢ ⎥ 1.11 −8.1 7.09 ⎥ ⎢ 2⎥ ⎢ ⎥ ⎢ x2 ⎥ ⎢ 2.1 sen(t) cos(t) ⎥ ⎢ ⎥ = ⎢ ⎥⎢ ⎥ + ⎢ ⎥ ⎣ x˙ 3 ⎦ ⎣ 2.22 4.56 8.23 6.7 ⎦ ⎣ x3 ⎦ ⎣ ⎦ 0.56t x˙ 4

−0.09 2.33

1.21

4.55

x4

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

sen3 (t) Alfaomega

388

Cap´ıtulo 7: Identificaci´ on param´ etrica para un intervalo de t ∈ [0, 5].

7.3 Considere el ⎡ ⎤ y1 ⎢ ⎥ ⎢ y2 ⎥ ⎢ ⎥ ⎢ y3 ⎥ = ⎢ ⎥ ⎢ ⎥ ⎣ y4 ⎦ y5

siguiente modelo matem´atico: ⎡ ⎤ 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)

ˆ conveniente y dise˜ Seleccione un vector de par´ametros θ ne un esquema de regresi´ on para llevar a cabo el proceso de identificaci´on param´etrica para un intervalo de t ∈ [0, 5]. 7.4 Considere los esquemas de regresi´on modelo din´amico filtrado y potencia filtrada del p´endulo (a) En el regresor del modelo din´amico filtrado sustituir la t´ecnica de filtrado q˙ (k)−q˙1 (k−1) para obtener la aceleraci´ on q¨1 por el m´etodo de Euler: q¨1 ≈ 1 y h q1 (k)−q1 (k−1) q˙1 ≈ . Llevar a cabo el proceso de identificaci´ on param´etrica h y comparar los resultados con el m´etodo de filtrado. (b) En el regresor del modelo de potencia filtrada sustituir la t´ecnica de filtrado para obtener la aceleraci´ on q¨1 por el m´etodo de Euler: q˙1 (k)−q˙1 (k−1) q1 (k)−q1 (k−1) y q˙1 ≈ . Llevar a cabo el proceso de q¨1 ≈ h h identificaci´on param´etrica y comparar los resultados con el m´etodo de filtrado. 7.5 Considere un brazo robot antropom´ orfico de 2 gdl: (a) Implementar el esquema de regresi´on lineal del modelo din´amico filtrado. (b) Implementar el esquema de regresi´on lineal del modelo de la potencia filtrada. 7.6 Considere un robot manipulador en configuraci´ on cartesiana de 3 gdl: (a) Implementar el esquema de regresi´on lineal din´ amico filtrado. (b) Implementar el esquema de regresi´on lineal del modelo de energ´ıa. (c) Implementar el esquema de regresi´on lineal del modelo de la potencia filtrada. Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

Parte IV Control

Control de robots manipuladores es un tema vigente en la actualidad y de inter´es para la comunidad cient´ıfica. El dise˜ no de sistemas de control en aplicaciones industriales representa no s´olo un reto te´orico, tambi´en resuelve sustancialmente problemas pr´acticos para mejorar deficiencias de algoritmos de control tradicionales. La Parte IV est´a dedicada a presentar aplicaciones de la metodolog´ıa de moldeo de energ´ıa la cual hoy en d´ıa representa una t´ecnica moderna de dise˜ no de algoritmos de control para robots manipuladores. La Parte IV se compone del Cap´ıtulo 8 Control

Cap´ıtulo 8 Control de posici´on contiene aplicaciones de algoritmos de control dise˜ nados directamente por la t´ecnica de moldeo de energ´ıa, y adem´as presenta una explicaci´on cualitativa de esquemas cl´asicos como lo son el control proporcional derivativo (PD) y el control proporcional integral derivativo (PID); tambi´en se incluye nuevas estrategias de control como el control tangente hiperb´olico y arcto tangente.

La Parte IV finaliza con:

Referencias selectas Problemas propuestos

Cap´ıtulo

8

Control de posici´on τ

˜ ) − f v (Kv , q) ˙ + g(q) = ∇Ua (Kp, q

8.1 Introducci´ on 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´ecnica de moldeo de energ´ıa de robots manipuladores.

Objetivos particulares: Control proporcional derivativo. Control proporcional integral derivativo. Control tangente hiperb´ olico. Control arcotangente. Control punto a punto.

8.1

393

Introducci´ on

8.1 Introducci´ on

C

ontrol de robots manipuladores es un tema de control autom´ atico vigente y de gran inter´es para la comunidad cient´ıfica de rob´otica debido a los retos te´oricos y pr´ acticos que involucra el dise˜ no de nuevas estrategias de control con alto desempe˜ no y exactitud en aplicaciones industriales tales como estibado de cajas, ensamble, traslado, pintado de objetos, etc. El dise˜ no de nuevos esquemas de control implica mejorar sustancialmente el desempe˜ no de algoritmos de control tradicionales. El problema m´ as simple de control de robots manipuladores se denomina control de posici´on o regulaci´on 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´on 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´ on o regulaci´on consiste en mover el extremo final del robot manipulador hacia una posici´ on deseada constante q d ; independientemente de su posici´on inicial q(0). Los puntos intermedios entre la posici´on 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˜ nal de error q˜(t) definida como la diferencia entre la posici´on deseada y la posici´ on actual del robot q˜(t) = qd − q(t). El punto final q d es la posici´on clave donde se realiza el control.

El objetivo del problema de control de posici´ on est´a 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´ on actual del robot q(t) y la ˙ velocidad articular de movimiento q(t) tiendan asint´oticamente hacia la posici´ on deseada q d y velocidad cero, respectivamente, sin importar las condiciones iniciales. Es decir,  l´ım

t→∞

q(t) ˙ q(t)



 =

qd



0

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

(8.1)

Alfaomega

394

Cap´ıtulo 8: Control de posici´ on

El objetivo de control se cumple cuando:  l´ım

t→∞

˙ q(t) q˜ (t)

 =

  0 0

Figura 8.1 Control de posici´ on N´ otese 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´ o sin depender de las condiciones iniciales, entonces la posici´on deseada se alcanza, por lo que la posici´ on 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´ ositos industriales, una vez que el extremo final del robot ha alcanzado el punto deseado, en el siguiente periodo de muestreo el punto deseado cambiar´a su valor, entonces el robot se mover´ a hacia al siguiente punto; el anterior punto deseado representa la condici´ on inicial y el extremo final del robot se estar´a 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´on es necesario que el esquema de control forme parte de la ecuaci´ on en lazo cerrado y que genere un punto de equilibrio asint´oticamente estable, ya que no depender´ıa de las condiciones iniciales. Es importante aclarar que en control punto a punto no se controla la Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

8.2

Control proporcional-derivativo (PD)

395

velocidad de movimiento como en el caso de control de trayectoria, donde el error de posici´on y error de velocidad son controlados simult´ aneamente. La gran mayor´ıa de las aplicaciones actuales de la industria son realizadas a trav´es de control punto a punto como en los casos de traslado y pintado de objetos, estibado y pintura de carrocer´ıas, etc´etera. El desempe˜ no 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´esticas, cient´ıficas e industriales se incrementa. A continuaci´ on 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´ecnica 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´ as simple y m´ as popular que se puede usar en robots manipuladores. En 1981, Takegaki y Arimoto demostraron que la ecuaci´ on en lazo cerrado del modelo din´ amico del robot manipulador y el control proporcional derivativo tiene un punto de equilibrio global y asint´ oticamente estable.

El control proporcional derivativo m´ as compensaci´on de gravedad (PD) est´ a dado por la siguiente ecuaci´on: τ

= Kpq˜ − Kv q˙ + g(q)

(8.2)

donde q˜ ∈ IRn es el vector de error de posici´on que se define como la diferencia entre la posici´ on deseada q d ∈ IRn y la posici´on 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´on en lazo cerrado en variables de estado que definen el problema de control ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

396

Cap´ıtulo 8: Control de posici´ on

de posici´on es:     −q˙ d q˜ = dt q˙ ˙ q˙ ] M (q)−1 [ Kpq˜ − Kv q˙ − C(q, q)

(8.3)

la cual es una ecuaci´on diferencial aut´onoma. El diagrama a bloques del control proporcional derivativo con compensaci´ on de gravedad se muestra en la figura 8.2.

Figura 8.2 Diagrama a bloques del control PD. La posici´on articular del robot q se retroalimenta para generar la se˜ nal de error de posici´ on q˜ = q d − q. La velocidad de movimiento q˙ se emplea para inyecci´on de amortiguamiento. Obs´ervese que el signo menos de la acci´on de control derivativa sirve para contrarrestar la energ´ıa al control proporcional. El t´ermino de compensaci´on de gravedad g(q) es una componente esencial para la unicidad del punto de equilibrio. Sin embargo, esta condici´ on no es suficiente, tambi´en se requiere que las ganancias proporcional y derivativa sean matrices definidas positivas. Desde el punto de vista pr´actico el t´ermino g(q) se requiere en la implementaci´on del control PD, y se debe conocer como parte de la din´amica 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

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

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´ endulo La forma inmediata de entender el funcionamiento cualitativo del control PD es a trav´es del p´endulo. El modelo din´amico de un p´endulo est´a 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´ermino kp1 q˜1 corresponde al regulador proporcional; siendo el error de posici´on q˜1 definido como la diferencia entre la posici´on deseada qd1 y la posici´on actual del p´endulo q1 (t). El nombre del esquema proporcional se debe al producto on q˜1 . de la ganancia kp1 y el error de posici´ Por otro lado, el t´ermino −kv1 q˙1 se denomina acci´on de control derivativo y no debe ser confundido con un esquema de control; este t´ermino se incluye con la finalidad de mejorar la respuesta del control proporcional a trav´es de la inyecci´on de amortiguamiento que se logra restando al control proporcional el producto entre la ganancia derivativa kv1 y la velocidad articular q˙1 . En t´erminos pr´ acticos el efecto de amortiguamiento significa freno mec´anico para dosificar la energ´ıa aplicada al servomotor τ1 y de esta forma lograr que la respuesta transitoria del p´endulo no tenga sobre impulsos y entre de manera suave al r´egimen estacionario. Por lo tanto, la ecuaci´ on en lazo cerrado est´a 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´ on de Coulomb y est´ atica con la finalidad que los programas de simulaci´ on no demoren dicho proceso. ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

398

Cap´ıtulo 8: Control de posici´ on

♣ Ejemplo 8.1 Controlar la posici´ on del extremo final del p´endulo en el siguiente punto deseado: qd1 = 90 grados

Soluci´ on La estructura matem´ atica del algoritmo de control proporcional derivativo del p´endulo est´a dada por la siguiente expresi´ on: τ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´ on definido como la diferencia q˜1 = qd1 − q1 (t) entre la posici´ on deseada qd1 y la posici´on actual del robot q1 (t); τ1 es el par aplicado al servomotor del p´endulo. El cuadro 8.1 contiene el modelo din´amico del p´endulo 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´endulo desde la posici´on inicial q(0) hacia la posici´ on deseada qd1 = 90 grados. El programa cap8 pdpendulo.m tiene el c´odigo fuente en MATLAB del control proporcional derivativo, el cual se encuentra en el cuadro 8.2 y a trav´es del programa principal cap8 pdpendulosimu.m (ver cuadro 8.3) se puede llevar a cabo el proceso de simulaci´on. En una primera fase de la simulaci´on las ganancias del control (PD) se han sintonizado con los siguientes valores: kp1 = 10 y kv1 =0.1; lo que significa que la inyecci´on de amortiguamiento del t´ermino derivativo es pobre, causa sobre impulsos en la etapa transitoria y fluctuaci´ on abrupta de la respuesta, marcando oscilaciones sucesivas y sostenidas en la etapa transitoria como se muestra en las gr´ aficas del error de posici´ on y par aplicado de la figura 8.3. Note que en el espacio de trabajo (cinem´atica directa) tambi´en la trayectoria del extremo final del p´endulo tiene oscilaciones; lo que se confirma por el diagrama fase (q˙1 vs q˜1 ) en que el atractor del Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

8.2

Control proporcional-derivativo (PD)

399

punto de equilibrio tiene un conjunto de espirales que van disminuyendo conforme las se˜ nales q˙1 (t) y q˜( t) evoluciona el tiempo t.

Figura 8.3 P´endulo: control PD, kp1 = 10, kv1 = 0.1 La segunda fase de la simulaci´ on 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´anico para obtener una respuesta m´as suave en r´egimen transitorio, y en estado estacionario libre de oscilaciones. La figura muestra la respuesta del p´endulo con la nueva sinton´ıa de ganancia derivativa.

Figura 8.4 P´endulo: control PD, kp = 10, kv = 2 ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

400

Cap´ıtulo 8: Control de posici´ on

C´ odigo Fuente 8.1

cap8 pendulo.m

otica y Mecatr´ onica. %MATLAB Aplicado a Rob´ %Editorial Alfaomega,

Fernando Reyes Cort´ es.

%Cap´ ıtulo 8 Control de posici´ on. cap8 pendulo.m 1

function xp=cap8 pendulo(t,x)

2

%modelo din´amico del p´endulo

3

%no se incluye el fen´ omeno de fricci´on de Coulomb, ni est´ atica

4

%variables de estados (entradas) ——————————————– %

5

%posici´on articular q1=x(1);

6 7

%velocidad articular qp1=x(2);

8 9

%par´ametros del p´endulo ————————————————– % m1=5; %masa

10 11

14

lc1=0.01; %centro de gravedad g=9.81; %constante de aceleraci´on gravitacional b1=0.17; %coeficiente de fricci´on viscosa Ir1=0.16; %momento de inercia del rotor

15

Ip=Ir1+m1*lc1*lc1; %momento de inercia del p´endulo

16

%en este punto se inserta el algoritmo de control

17

%par aplicado (controlador) ———————————————-% [tau1, ˜ ] = cap8 pdpendulo(q1,qp1) ;

12 13

18 19

%aceleraci´on articular del p´endulo ———————————————% 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´ on articular end

Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

8.2

Control proporcional-derivativo (PD)

C´ odigo Fuente 8.2

401

cap8 pdpendulo.m

%MATLAB Aplicado a Rob´ otica y Mecatr´ onica. %Editorial Alfaomega,

Fernando Reyes Cort´ es.

%Cap´ ıtulo 8 Control de posici´ on. Esta funci´ on retorna: error de posici´ on q˜1 par aplicado τ1 cap8 pdpendulo.m 1

function [tau1, qt1] = cap8 pdpendulo(x,xp)

2

%variables de estados (entradas) ——————————————– %

3

%posici´on articular

4

q1=x;

5 6

%velocidad articular qp1=xp;

7

%par´ametros de la compensaci´on de gravedad del p´endulo

8 10

m1=5; %masa lc1=0.01; %centro de gravedad g=9.81; %constante de aceleraci´on gravitacional

11

%ganancias del controlador ———————————————-%

12

kp1=10; %ganancia proporcional kv1=2; %ganancia derivativa

9

13 14 15

%referencia (posici´on deseada en grados) qd1=90;

16

%vector de pares de gravitacionales ————————————– %

17

par grav = m1*g*lc1*sin(q1);

18 19

%error de posici´on qt1=pi*qd1/180-q1;

20

%control proporcional derivativo

21

tau1=kp1*qt1-kv1*qp1+par grav;

22

end

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

402

Cap´ıtulo 8: Control de posici´ on

C´ odigo Fuente 8.3

cap8 pdpendulosimu.m

otica y Mecatr´ onica. %MATLAB Aplicado a Rob´ %Editorial Alfaomega,

Fernando Reyes Cort´ es.

%Cap´ ıtulo 7 Identificaci´ on param´ etrica. cap8 pdpendulosimu.m 1

clc; clear all; close all;

2

format short g

3

%par´ametros de simulaci´on ————————————————% 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,´InitialStep’,1e-3,’MaxStep’,1e-3);

10

%soluci´ on num´erica 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)

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

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´ on antropom´orfica de 2 gdl est´ a dado por la siguiente ecuaci´on:         τ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 on q˜ se define velocidad articular es representada por q˙ = [ q˙1 , q˙2 ]T ; el error de posici´ como la diferencia entre la posici´ on deseada q d y la posici´on actual del robot q; g(q) es la compensaci´on de gravedad; τ1 , τ2 son los pares aplicados a las articulaciones del hombro y codo, respectivamente. ♣ ♣ Ejemplo 8.2 Controlar la posici´ on del extremo final de un robot de 2 gdl en configuraci´ on antropom´ orfica en el siguiente punto deseado: [ qd1 , qd2 ]T = [ 90, 45 ] grados. Soluci´ on El cuadro 8.8 contiene el modelo din´amico del brazo robot de 2 gdl (programa cap8 robot2gdl.m); en referencia a este programa es muy importante aclarar que la ecuaci´on en lazo cerrado est´ a formada por la din´ amica 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´odigo 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´egimen 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; ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

404

Cap´ıtulo 8: Control de posici´ on

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´ on, sin sobre impulsos, no hay oscilaciones pronunciadas, teniendo una respuesta suave; ambos componentes de error de posici´ on tienden asint´ oticamente a cero como el tiempo evoluciona a infinito (estabilidad asint´otica). Las se˜ nales de pares aplicados τ1 y τ2 se aplican directamente a las articulaciones del hombro y codo, respectivamente; el perfil de estas se˜ nales 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´ on de casa ubicada en el eje y− hasta el punto deseado. Observe tambi´en que en el diagrama fase del robot las velocidades articulares como las se˜ nales de error de posici´ on convergen asint´ oticamente 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

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

8.2

405

Control proporcional-derivativo (PD)

C´ odigo Fuente 8.8

cap8 robot2gdl.m

otica y Mecatr´ onica. %MATLAB Aplicado a Rob´ %Editorial Alfaomega,

Fernando Reyes Cort´ es.

%Cap´ ıtulo 8 Control de posici´ on. cap8 robot2gdl.m 1

function xp = cap8 robot2gdl(t,x)

2 3

%vector de posici´on 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

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

406

Cap´ıtulo 8: Control de posici´ on

C´ odigo Fuente 8.8

cap8 PDrobot2gdl.m

otica y Mecatr´ onica. %MATLAB Aplicado a Rob´ %Editorial Alfaomega,

Fernando Reyes Cort´ es.

%Cap´ ıtulo 8 Control de posici´ on. 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´ametros 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´ on 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

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

8.2

Control proporcional-derivativo (PD)

C´ odigo Fuente 8.6

407

cap8 robot2gdlsimu.m

otica y Mecatr´ onica. %MATLAB Aplicado a Rob´ %Editorial Alfaomega,

Fernando Reyes Cort´ es.

%Cap´ ıtulo 8 Control de posici´ on. 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´ on num´erica del sistema din´amico 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

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

408

Cap´ıtulo 8: Control de posici´ on

8.2.3 8.2.3. Control PD de un brazo robot de 3 gdl El algoritmo de est´a 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´ on q˜ ∈ IR3 se define como la diferencia entre la posici´on deseada qd ∈ IR3 y la posici´on 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-´esima ganancia s´olo depende de la i´esima articulaci´ on. Es decir, el par aplicado a la i-´esima articulaci´ on depende de: τi = τ (kpi, q˜i, q˙i ). ♣ Ejemplo 8.3 Controlar la posici´ on del extremo final de un robot de 3 gdl en configuraci´ on antropom´ orfica en las siguientes coordenadas: [ qd1 , qd2 , qd3 ]T = [ 30, 45, 90 ] grados

Soluci´ on El modelo din´amico del robot antropom´ orfico de 3 gdl se encuentra en el programa cap8 robot3gdl.m como se describe en el cuadro 8.7. De particular inter´es representa la l´ınea de c´odigo 23, debido a que en este lugar se inserta el algoritmo de control que env´ıa la se˜ nal de par aplicado para que el robot se mueva a la posici´ on indicada. El esquema de control proporcional derivativo se encuentra implementado en el Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

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´on. Sup´ ongase 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´ aximo de la i-´esima articulaci´ on. 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´odigo fuente que contiene la simulaci´ on del control proporcional derivativo del robot manipulador en configuraci´ on antropom´ orfica se encuentra en el programa cap8 robot3gdlsimu.m (cuadro 8.9). La figura 8.6 muestra el comportamiento de los errores de posici´ on para las tres articulaciones; presentan un perfil suave, sin ruido mec´anico, ni sobre impulsos, tampoco se observan oscilaciones en r´egimen estacionario. Los pares aplicados evolucionan dentro de los l´ımites de saturaci´ on 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. ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

410

Cap´ıtulo 8: Control de posici´ on

C´ odigo Fuente 8.7

cap8 robot3gdl.m

otica y Mecatr´ onica. %MATLAB Aplicado a Rob´ %Editorial Alfaomega,

Fernando Reyes Cort´ es.

%Cap´ ıtulo 8 Control de posici´ on. 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´ on 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

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

8.2

Control proporcional-derivativo (PD)

C´ odigo Fuente 8.8

411

cap8 PDrobot3gdl.m

otica y Mecatr´ onica. %MATLAB Aplicado a Rob´ %Editorial Alfaomega,

Fernando Reyes Cort´ es.

%Cap´ ıtulo 8 Control de posici´ on. 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´ametros 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

gq=g*[gq11; gq21; gq31]; %par gravitacional qd=[30; 45; 90]; %referencias o posiciones deseadas qtilde=qd*pi/180-q; %error de posici´ on q˜ qtildeg=(180/pi)*qtilde; %error de posici´ on q˜ en grados qpgrados=180*qp/pi; %velocidad q˙ en grados/segundo.

13 14 15 16 17

%ganancia proporcional Kp=[1, 0,0 ; 0, 2,0;

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

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

412

Cap´ıtulo 8: Control de posici´ on

C´ odigo Fuente 8.9

cap8 robot3gdlsimu.m

%MATLAB Aplicado a Rob´ otica y Mecatr´ onica. %Editorial Alfaomega,

Fernando Reyes Cort´ es.

%Cap´ ıtulo 8 Control de posici´ on. cap8 robot3gdlsimu.m 1 3

clc; clear all; close all; format short

4

%par´ametros de simulaci´on:

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

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

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´ on cartesiana est´a 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 d˙ donde Kp , Kv ∈ IR3×3 son la ganancia proporcional y derivativa, respectivamente; ˜ ∈ IR3 se define como la diferencia entre la posici´on deseada el error de posici´ on d dd ∈ IR3 y la posici´on actual del robot d ∈ IR3 . ♣ Ejemplo 8.4 Controlar la posici´ on del extremo final de un robot de 3 gdl en configuraci´ on cartesiana en la siguiente ubicaci´on: [ dd1 , dd2, dd3 ]T = [ 0.5, 0.4, 0.2 ] m

Soluci´ on El modelo din´amico 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´ on cap8 PDcartesiano3gdl.m (ver cuadro 8.11). El programa principal para realizar la simulaci´ on 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.

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

414

Cap´ıtulo 8: Control de posici´ on

C´ odigo Fuente 8.10

cap8 cartesiano3gdl.m

otica y Mecatr´ onica. %MATLAB Aplicado a Rob´ %Editorial Alfaomega,

Fernando Reyes Cort´ es.

%Cap´ ıtulo 8 Control de posici´ on. cap8 cartesiano3gdl.m 1

function xp = cap8 cartesiano3gdl(t,x)

2

%vector de posici´on 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´amico 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´on articular xp = [dp(1); dp(2); dp(3); dpp(1); dpp(2); dpp(3)]; %vector de salida

25 26

end

Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

8.2

Control proporcional-derivativo (PD)

C´ odigo Fuente 8.11

415

cap8 PDcartesiano3gdl.m

%MATLAB Aplicado a Rob´ otica y Mecatr´ onica. %Editorial Alfaomega,

Fernando Reyes Cort´ es.

%Cap´ ıtulo 8 Control de posici´ on. 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´ametros 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

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

416

Cap´ıtulo 8: Control de posici´ on

C´ odigo Fuente 8.12

cap8 cartesiano3gdlsimu.m

%MATLAB Aplicado a Rob´ otica y Mecatr´ onica. %Editorial Alfaomega,

Fernando Reyes Cort´ es.

%Cap´ ıtulo 8 Control de posici´ on. 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´ametros de simulaci´on: 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

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

8.3

417

Control PID

8.3 Control PID

E

l esquema de control proporcional integral derivativo (PID) tiene la siguiente estructura:  τ

˜ − Kv q˙ + Ki = Kp q

t

˜ (μ)dμ + g(q) 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´on de control PID introduce una nueva variable de estado ε, la cual se ˜ . Por encuentra relacionada con la derivada temporal del error de posici´ on ε˙ = q t n lo tanto, ε = 0 q˜dt + ε(0), con ε(0) ∈ IR es un vector constante. La ecuaci´on en ˜, q˙ ]T adquiere la siguiente lazo cerrado en t´erminos de las variables de estado [ ε, q forma: ⎤ ⎡ ⎤ ⎡ ε q˜ d ⎢˙⎥ ⎥ ⎢ −q˙ ⎦ ⎣ q˜ ⎦ = ⎣ dt −1 ˙ q˙ − B q˙ ] q˙ M (q) [ Kp q˜ − Kv q˙ + Kiε − C(q, q) (8.5)

El algoritmo de control PID no representa una nueva estructura de control; es una versi´ on modificada del control PD m´ as la acci´on de control integral. El algoritmo de control PD tiene la desventaja que en r´egimen estacionario presenta un error permanente de posici´on denominado offset; para disminuir este error se incorpora en la estructura matem´atica del PD un t´ermino m´ as que se denomina acci´on de control integral. El proceso de integraci´ on es la suma del a´rea bajo la curva del error de posici´ on, por lo tanto acumula energ´ıa la cual canalizada adecuadamente puede disminuir la magnitud del error en r´egimen estacionario. ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

418

Cap´ıtulo 8: Control de posici´ on

8.3.1 8.3.1. Control PID de un robot de 2 gdl La ecuaci´on del esquema de control proporcional derivativo para un brazo robot de 2 gdl est´a 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´ on del extremo final de un robot de 2 gdl en configuraci´ on antrop´ orfica en la siguiente ubicaci´on: [ qd1 , qd2 ]T = [ 45, 90 ] m Soluci´ on El programa cap8 PIDrobot2gdl.m tiene implementado el modelo din´ amico 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

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

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´on num´erica se realiza en el modelo din´ amico del robot.

C´ odigo Fuente 8.13

cap8 PIDrobot2gdl.m

otica y Mecatr´ onica. %MATLAB Aplicado a Rob´ %Editorial Alfaomega,

Fernando Reyes Cort´ es.

%Cap´ ıtulo 8 Control de posici´ on. 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´on 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

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

420

Cap´ıtulo 8: Control de posici´ on

C´ odigo Fuente 8.14

cap8 robot2gdlPID.m

%MATLAB Aplicado a Rob´ otica y Mecatr´ onica. %Editorial Alfaomega,

Fernando Reyes Cort´ es.

%Cap´ ıtulo 8 Control de posici´ on. cap8 robot2gdlPID.m 1

function xp = cap8 robot2gdlPID(t,x)

2

%vector de posici´on 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

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

8.3

421

Control PID

El programa cap8 robot2gdlPIDsimu.m que se presenta en el cuadro 8.15 permite realizar la simulaci´on del control PID con el robot de 2 gdl

C´ odigo Fuente 8.15

cap8 robot2gdlPIDsimu.m

otica y Mecatr´ onica. %MATLAB Aplicado a Rob´ %Editorial Alfaomega,

Fernando Reyes Cort´ es.

%Cap´ ıtulo 8 Control de posici´ on. 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)

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

422

Cap´ıtulo 8: Control de posici´ on

8.4 Control punto a punto

U

na aplicaci´ on inmediata de control de posici´ on 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´ on de un conjunto de puntos qdi . Los algoritmos de control de posici´on de alto desempe˜ no pueden ser empleados en control punto a punto como son los casos de los esquemas tangente hiperb´olico y arcotangente.

8.4.1 8.4.1. Control tangente hiperb´ olico La estructura matem´ atica del control tangente hiperb´ olica es la siguiente: τ

˙ + g(q) q ) − Kv tanh(q) = Kp tanh(˜

♣ ♣ Ejemplo 8.6 Posicionar el extremo final de un robot de 2 gdl en: [ qd1 , qd2 ]T = [ 45, 90 ] m. Soluci´ on El programa cap8 robot2gdlTANH.m 8.9. que se presenta en el cuadro 8.16 tiene el modelo din´amico 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´ olico. El programa cap8 TANHrobot2gdlsimu.m del cuadro 8.18 permite realizar la simulaci´on del control tangente hiperb´ olica cuyos Figura 8.9 Control tanh(). resultados se muestran en la figura 8.8. .9 ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Alfaomega Matlab Aplicado a Robo es.

8.4

423

Control punto a punto

C´ odigo Fuente 8.16

cap8 robot2gdlTANH.m

otica y Mecatr´ onica. %MATLAB Aplicado a Rob´ %Editorial Alfaomega,

Fernando Reyes Cort´ es.

%Cap´ ıtulo 8 Control de posici´ on. cap8 robot2gdlTANH.m 1

function xp = cap8 robot2gdlTANH(t,x)

2 3

%vector de posici´on 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

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

424

Cap´ıtulo 8: Control de posici´ on

C´ odigo Fuente 8.17

cap8 TANHrobot2gdl.m

%MATLAB Aplicado a Rob´ otica y Mecatr´ onica. %Editorial Alfaomega,

Fernando Reyes Cort´ es.

%Cap´ ıtulo 8 Control de posici´ on. 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´ametros 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´on 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

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

8.4

425

Control punto a punto

C´ odigo Fuente 8.18

cap8 TANHrobot2gdlsimu.m

otica y Mecatr´ onica. %MATLAB Aplicado a Rob´ %Editorial Alfaomega,

Fernando Reyes Cort´ es.

%Cap´ ıtulo 8 Control de posici´ on. 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

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

426

Cap´ıtulo 8: Control de posici´ on

8.4.2 8.4.2. Control arcotangente El control arcotangente est´a dado por la siguiente expresi´ on: τ

˙ + g(q) = Kp atan (˜ q ) − Kv atan (q)

♣ ♣ Ejemplo 8.7 Generar una flor de 8 p´etalos con un robot de 2 gdl usando el control arcotangente.

Soluci´ on Dentro de las aplicaciones de control punto a punto se encuentra el trazo de figuras. on para El programa cap8 robot2gdlatansimu.m (cuadro 8.19) realiza la simulaci´ que un robot antropom´ orfico de 2 gdl trace una figura tipo flor con 8 p´etalos usando el control arcotangente como se muestra en la figura 8.10.

Figura 8.10 Control atan(). El control arcotangente es de alto desempe˜ no y la sintonizaci´on de las ganancias proporcional y derivativa deben satisfacer ser menor al par m´aximo que pueda Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

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´odigo 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´ ametros del control arcotangente son: t, q, q; el tiempo t es necesario para generar la figura en forma de flor con 8 p´etalos.

C´ odigo Fuente 8.19

cap8 robot2gdlatansimu.m

otica y Mecatr´ onica. %MATLAB Aplicado a Rob´ %Editorial Alfaomega,

Fernando Reyes Cort´ es.

%Cap´ ıtulo 8 Control de posici´ on. 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´on 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

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

428

Cap´ıtulo 8: Control de posici´ on

C´ odigo Fuente 8.20

cap8 robot2gdlflor8p.m

otica y Mecatr´ onica. %MATLAB Aplicado a Rob´ %Editorial Alfaomega,

Fernando Reyes Cort´ es.

%Cap´ ıtulo 8 Control de posici´ on. 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´ on 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´ on articular xp = [qp(1); qp(2);qpp(1); qpp(2) ]; %vector de salida end

C´ odigo Fuente 8.21

cap8 atanflor8p.m

otica y Mecatr´ onica. %MATLAB Aplicado a Rob´ %Editorial Alfaomega,

Fernando Reyes Cort´ es.

%Cap´ ıtulo 8 Control de posici´ on. 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´ on %ley de control PD tau=Kp*atan(Lambda*qtilde)-Kv*atan(Gamma*qp)+par grav; end

Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

8.5

429

Resumen

8.5 Resumen

C

ontrol de posici´on robots manipuladores permite realizar una amplia gama de aplicaciones industriales, comerciales, cient´ıficas, etc. La aplicaci´on m´ as popular del control de robot manipuladores es la que se denomina control punto a punto. Pr´ acticamente todas las aplicaciones industriales pueden ser realizadas a trav´es de esta t´ecnica de control. En el presente cap´ıtulo se present´ o 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´olica y arcotangente. El modelo din´amico de robots manipuladores permite evaluar cualitativamente el desempe˜ no 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˜ no de nuevas estrategias de control es posible gracias a la t´ecnica 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´on. ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

430

Cap´ıtulo 8: Control de posici´ on

Parte IV Referencias selectas

H

ay una extensa literatura para el a´rea de control de robots manipuladores, particularmente se recomienda los siguientes textos:

8 Control de posici´ on

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

˚ Astr¨om

J.

and

Bj¨ orn

“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

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

431

Parte IV Problemas propuestos

R. Kelly y V. Santib´ anez. “Control de movimiento de robots manipuladores”. Printice-Hall, Pearson. 2003.

R.

Kelly,

V.

Santib´anez

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´ otica: control de robots manipuladores”.

Alfaomega, 2011.

Parte IV Problemas propuestos

E

n esta secci´on se presenta una serie de ejercicios con la finalidad de mejorar las habilidades en simulaci´ on de control de robots manipuladores.

Cap´ıtulo 8 Control de posici´ on

8.1 Implementar el control PID para los siguientes robots: (a) P´endulo qd1 = 90 grados. (b) Robot antropom´ orfico 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: τ

˙ + g(q) = Kp sen(˜ q) − Kv sen(q)

implementar esta estructura de control en los siguientes robots: ´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

432

Cap´ıtulo 8: Control de posici´ on (a) P´endulo qd1 = 90 grados. (b) Robot antropom´ orfico de 2 gdl q d = [ 45, 90 ]T grados. (c) Robot antropom´ orfico 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´ orfico de 2 gdl qd = [ 90, 135 ]T grados. (c) Control cartesiano de 3 gdl dd = [ 0.5, 0.8, 0.9]T m. ¿Qu´e 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) ˙ + g(q) − Kv atan (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´on del extremo final del robot en las siguientes coordenadas: [ 0.4, 0.3, 0.8] m. (a) ¿C´omo var´ıa la respuesta del robot (errores de posici´ on) conforme se incrementa el exponente m. (b) ¿Describa el desempe˜ no de este esquema de control en funci´on de las ganancias y el exponente m.

Alfaomega

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

´ Indice anal´ıtico

A

clear, 17 cinem´atica, 140, 185

algoritmo

diferencial, 187

recursivo, 332 de control, 393

inversa, 186 columna de una matriz, 27

amortiguamiento, 129

columnas, 26, 35

antropom´ orfico, 408

concatenacio´n, 35

archivos experimentales, 76 arreglos, 26, 41

constante de tiempo, 297 constantes u ´tiles, 18 continue, 74

B backward difference, 95 borrando, 35

control, 242 conveci´on DH, 192 arcotangente, 390 de posici´on, 393

brazo robot de 2 gdl, 403

de robots, 390, 393 PD, 396

break, 73

C

PD, p´endulo, 397 robot cartesiano, 413

case, 72 centr´ıfuga, 301

robot de 2 gdl, 403 robot de 3 gdl, 408

´ Indice anal´ıtico

434 PID, 417

elseif, 60, 64

proporcional 390, 395

derivativo,

proporcional integral derivativo, 390 punto a punto, 395

end, 51, 52 error de posici´ on, 395, 417 esquema escalar, 381 esquemas de regresi´on, 332, 359

tangente hiperb´ olico, 390 estimador de velocidad, 296 CRTL Q, 15 exit, 14

D

expansio´n de escalares, 37

Denavit-Hartenberg, 192

eye, 32

demos, 13

ezmesh, 47

F

derivada, 93 desempe˜ no, 395 det, 85

feval, 50

determinante simb´ olico, 86 determinantes, 85 diferenciaci´on num´erica, 92 diff, 94, 97

filtro, 299 fopen, 76

format, 18

E

formato num´erico, 18

ecuaci´on diferencial ordinaria, 117, 288

Alfaomega

filtrado, 296, 299, 350

for, 58, 61, 64, 67, 73

din´ amica, 284

else, 60

fclose, 76

forward difference, 95 fplot, 46 fprintf, 76

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

´ Indice anal´ıtico frecuencia de corte, 357

435 gr´ aficas, 43

H

frecuencia natural, 129 fricci´ on de Coulomb, 306 fricci´ on viscosa, 306

help, 13, 43 elfun, 49 elmat, 49

funci´ on, 51

specfun, 49

funci´ on

helpwin, 43 c´ırculo, 56

I

transferencia, 289 discontinua, 351

identificaci´on, 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´ on num´erica, 100

G

integral, 93 inversa de matriz, 87

ganancia

L

derivativa, 417 proporcional, 417 integral, 417 globales, 53

librer´ıas de cinem´atica, 274 Denavit-Hartenberg, 180

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

´ Indice anal´ıtico

436 Traslaci´on y rotaci´ on, 182

potencia, 355, 359, 369, 378

matrices homog´eneas, 174

regresi´ on, 346

m´ınimos cuadrados, 334

regresi´ on din´ amico, 361 din´ amico, 287, 331, 346, 359

linesolve, 89

din´ amico 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´on, 148

ode, 120, 288

m´aximo, 69

ode113, 123

m´etodo de Euler, 95, 114

ode15s, 123

m´etodo de primer orden, 118

ode23, 122

m´etodo 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´asicas, 38, 40 operaciones con arreglos, 41 operadores

mesh, 47

[], 28 modelo

’, 24

energ´ıa, 353, 366 Alfaomega

(), 23

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.

´ Indice anal´ıtico

437

,, 24

quadgk, 114

:, 21

quadl, 113

;, 23

quadv, 114

%, 25 MATLAB, 5, 7

quit, 14

˜ , 25

R

aritm´eticos, 12 arreglos, 41

rand, 32

l´ ogicos, 58

randn, 32

P

regla de Cramer, 91

p´endulo, 305, 346, 353, 397

de Simpson, 108

par´ ametros, 331

trapezoidal, 102

PD, 390, 395 PID, 100, 390, 417

rotaci´on, 164 regresor, 332 regulaci´on, 393

pivote, 27

rengl´on de una matriz, 27

plot3, 43

renglones, 26, 35 potencia, 381 return, 73 potencia filtrada, 357 rob´ otica, 393 problema de control, 393 robot producto interno, 144 antropom´ orfico, 408 prompt, 11

cartesiano, 373

Q

cartesiano de 3 gdl, 413 de 3gdl, 321

quad, 113

de 2 gdl, 310, 360

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es

Alfaomega

´ Indice anal´ıtico

438 manipulador de 3 gdl, 315, 408

sistemas din´amicos no aut´ onomos, 288

manipuladores, 287, 393

sistemas lineales, 289

ruido, 299

sistemas mecatr´onicos, 331

Runge Kutta, 327

subplot, 48

Runge-Kutta, 118

S

suma de matrices, 68 switch, 58, 72

T

save, 76 script, 54

transformaci´ on

serie de Taylor, 118 set point, 393, 395

homog´enea, 173 traslaci´ on, 171

signo, 65

V

Simpson, 113 simulaci´on, 287 simulaci´on de sistemas din´amicos, 124 Simulink, 7

variables, 15 vector de mediciones, 333 vector de par´ ametros, 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´amicos, 117 sistemas din´amicos aut´ onomos, 288 Alfaomega

Z zeros, 32

´ tica y Mecatro ´ nica • Fernando Reyes Cort´ Matlab Aplicado a Robo es.