Estas son las escuelas y colegios que tendrán modalidad no presencial este lu...
Practicas de robotica utilizando matlab - Roque
1.
2. Prácticas de Robótica utilizando Matlab®
Práctica 1 .- Pág. 1
Práctica 1
Introducción
La aparición de la robótica en la industria ha supuesto en los últimos 40 años una
importante revolución industrial, que sin embargo no ha tenido el crecimiento
exponencial que los pioneros de la robótica habían supuesto. Una de las razones para
este no tan rápido crecimiento ha sido la complejidad de los cálculos dinámicos de los
mecanismos robotizados. Las elegantes formulaciones matemáticas realizadas por los
padres de la dinámica requieren un gran número de cálculos que obligaba a realizar un
gran número de simplificaciones. Históricamente, el análisis, simulación y diseño de
robots ha supuesto una gran inversión de tiempo y capital, reservado para centros de
investigación y grandes fabricantes.
Sin embargo, la revolución del PC, está permitiendo la implantación de
herramientas sencillas de análisis y simulación de robots. Los programas de propósito
general para el modelado y la simulación de los sistemas mecánicos de múltiples
cuerpos han llamado poderosamente la atención durante esta ultima década. Un
programa CAD para la dinámica de los sistemas multicuerpos minimiza la posibilidad
del error humano en el manejo de modelos complejos, pero en la mayoría de ocasiones,
paquetes de software como IMP [Sheth-1972 ], ADAMS [Orlandea-1977], DISCOS
[Bodley-1978], DADS [ Nikravesh-1985], MEDYNA [Jaschinsky-1986] ofrecen unas
prestaciones mayores de las requeridas por el usuario. Además, utilizando paquetes de
análisis matemático general como MatLab se pueden implementar los algoritmos
clásicos de la dinámica computacional, y obtener una serie de herramientas que
permitan al usuario del PC realizar un diseño y análisis de su propio mecanismo.
En este sentido, este libro quiere presentar una serie de herramientas sencillas de
modelado computacional y simulación de robots, enfocadas a ser utilizadas en el diseño
paramétrico de robots y mecanismos tanto planares como espaciales en computadoras
personales. En contraste con los libros clásicos de robótica, en los cuales la teoría ocupa
un espacio predominante y es posible encontrar toda las bases de las herramientas que
aquí se utilizan, este libro pretende tener un enfoque eminentemente práctico, aportando
varias referencias para aquellos lectores que quieran ampliar las bases teóricas.
3. Prácticas de Robótica utilizando Matlab®
Práctica 1 .- Pág. 2
1.1.- Clasificación de robots. Morfología.
La palabra robot proviene del eslavo robota y fue utilizada por primera vez por el
escritor checo Karel Capec en 1917 para referirse en su obra Rossum’s Universal Robot
R.U.R. a máquinas con forma humanoide, pero fue el escritor americano de origen ruso
Isaac Asimov el que dotó de popularidad a esta palabra. En octubre de 1945 publicó en
la revista Galaxy Science Fiction una historia en la que se enuncian por primera vez sus
tres leyes de la robótica;
1. Un robot no puede perjudicar a un ser humano, ni con su inacción permitir que
un ser humano sufra daño.
2. Un robot ha de obedecer las órdenes recibidas de un ser humano, excepto si tales
órdenes entran en conflicto con la primera ley.
3. Un robot debe proteger su propia existencia mientras tal protección no entre en
conflicto con la primera o segunda ley.
Sin pretender ser exhaustivos con la historia y el desarrollo de la robótica se puede
citar que la primera patente de un dispositivo robótico fue solicitada en marzo de 1954
por el inventor británico C.W. Kenward, si bien la primera patente de un dispositivo de
transferencia de artículos programada que se asemeja más al concepto de robot
industrial que hoy conocemos fue presentada por el ingeniero americano George D.
Devol en 1961. En ella se dice:
"The present invention makes available for the first time a more or less general purpose
machine that has universal application to a vast diversity of applications where cyclic
control is desired."
Se pueden distinguir cuatro fechas puntuales en el desarrollo de la robótica industrial:
En 1948 R.C. Goertz diseñan en el Argonne National Laboratory el primer
sistema telemanipulador para manejar materiales radioactivos.
Joseph F. Engelberger y G.C. Devol fundan en 1958 la primera empresa
dedicada a la fabricación de robots industriales; Unimation, e instalan su primera
máquina. En 1967 instalaron un conjunto de robots en la factoría de General
Motors y tres años después se inicia la implantación de los robots en Europa,
especialmente en el sector automovilístico.
En 1970 la Universidad de Stanford y el MIT empiezan un proyecto destinado a
controlar un robot mediante computadora.
A partir de 1975, la revolución de la microelectrónica y la aparición del
microprocesador relanza el sector de la robótica.
4. Prácticas de Robótica utilizando Matlab®
Práctica 1 .- Pág. 3
Figura 1.1.- Primer sistema de teleoperación maestro-esclavo con servoaccionamientos eléctricos.
(desarrollado en el Argonne National Laboratory)
A la hora de definir el concepto de robot de distinguir entre el robot industrial o
robot de producción y los robots especiales o también denominados robots de servicio.
La definición más comúnmente aceptada de robot industrial es la de la Asociación de
Industrial Robóticas (RIA), según la cual:
Un robot industrial es un manipulador multifuncional reprogramable, capaz de
mover materias, piezas, herramientas o dispositivos especiales, según
trayectorias variables, programadas para realizar tareas diversas.
Figura 1.2.- Robot Industrial UNIMATE de la serie 5000
5. Prácticas de Robótica utilizando Matlab®
Práctica 1 .- Pág. 4
Mientras que los robots de servicio se definen como:
Dispositivos electromecánicos, móviles o estacionarios, dotados normalmente de
uno o varios brazos mecánicos independientes, controlados por un programa de
ordenador y que realizas tareas no industriales de servicio [Nostrand 90]
Para el estudio de la morfología de un robot se debe tener en cuenta que un robot
está formado por los siguientes elementos; estructura mecánica, transmisiones,
actuadores, sistema sensorial, sistema de control y elementos terminales.
Figura 1.3.- Elementos constitutivos de un robot industrial clásico
Estructura mecánica
Un robot está formado por una serie de elementos o eslabones unidos mediante
articulaciones que permiten un movimiento relativo entre cada dos eslabones
consecutivos. La constitución física de la mayor parte de los robots industriales está
inspirada en la anatomía del brazo y la mano humana.
Se definen como grados de libertad como los posibles movimientos básicos
(giros y desplazamientos) independientes. La figura 1.4 muestra diferentes tipos de
articulaciones y los grados de libertad que aportan cada una de ellas.
6. Prácticas de Robótica utilizando Matlab®
Práctica 1 .- Pág. 5
Figura 1.4.- Diferentes articulaciones utilizadas en robótica
Las diferentes combinaciones de estas articulaciones dan lugar a distintas
configuraciones o arquitecturas mecánicas, cada una de ellas con características que la
hacen más o menos recomendable para la realización de una determinante tarea. Las
siguientes figuras muestran las configuraciones más típicas utilizadas en robots
industriales, y en ellas se distinguen los eslabones y los grados de libertad de cada
configuración.
Robot cartesiano Robot Cilíndrico
Robot Polar Robot SCARA Robot antropomórfico
Figura 1.5.- Diferentes arquitecturas mecánicas
7. Prácticas de Robótica utilizando Matlab®
Práctica 1 .- Pág. 6
Es también muy común usar en robótica terminología inspirada en la anatomía
del brazo humano, así en la mayoría de arquitecturas mecánicas de robots industriales,
las tres últimas articulaciones, que normalmente se utilizan para proporcionar
orientación al elemento terminal (se estudiará esto más profundamente en el capítulo
siguiente), forman lo que se conoce como muñeca del robot (figura 1.6).
Figura 1.6.- Muñeca del Robot
Otro concepto importante que debe tenerse en cuenta en la descripción de un
robot son las dimensiones del robot y el espacio de trabajo que define el movimiento
de las diferentes articulaciones. El espacio de trabajo de un robot se subdivide en áreas
diferenciadas entre sí por la accesibilidad del elemento terminal en cada una de ellas.
Por ejemplo, es diferente la zona en la que el elemento terminal solo puede orientarse
horizontalmente o aquella en la que también puede orientarse verticalmente. La figura
siguiente muestra un robot de la casa STAÜBLI y su espacio de trabajo.
Figura 1.7.- Dimensiones y espacio de trabajo del Robot Industrial STAÜBLI PUMA RX-60
La capacidad de carga que puede transportar la garra del robot, la precisión en
la repetibilidad y la velocidad de las articulaciones son también características muy
importantes del robot que deben ser citadas en la descripción de un robot.
8. Prácticas de Robótica utilizando Matlab®
Práctica 1 .- Pág. 7
Transmisiones y reductores
Las transmisiones son los elementos encargados de transmitir el movimiento
desde los actuadores hasta las articulaciones. Los reductores se incluyen con las
transmisiones y son los encargados de adaptar el par y la velocidad de la salida del
actuador a los valores adecuados para el movimiento de los elementos del robot.
Normalmente los actuadores se intentan situar lo más cerca de la base del robot,
con el fin de reducir al máximo el peso estático y la inercia de los eslabones finales, que
deben moverse con aceleraciones altas. Para ello es necesario utilizar sistemas de
transmisión que trasladen el movimiento hasta las articulaciones. La siguiente tabla,
extraída de [1] resume los sistemas de transmisión para robots. El lector que quiera
profundizar en el tema puede utilizar las referencias [1],[2] y [3] para obtener más
información.
Entrada-Salida Denominación Ventajas Inconvenientes
Circular-Circular Engranaje
Correa dentada
Cadena
Paralelogramo
Cable
Pares altos
Distancia grande
Distancia grande
---
---
Holguras
---
ruido
giro limitado
deformabilidad
Circular-lineal Tornillo sin fin
Cremallera
Poca holgura
Holgura media
Rozamiento
Rozamiento
Lineal- Circular Paral. Articulado
Cremallera
---
Holgura media
Control difícil
Rozamiento
Tabla 1.1.- Sistemas de transmisión para robots
Los reductores utilizados en robótica pueden tener diferentes morfologías y estar
basados en distintas tecnologías, y en muchas ocasiones vienen incorporados con el
actuador.
Actuadores
Para el estudio de los actuadores y sus modelos matemáticos se ha dedicado la
práctica cuatro de este libro. Sin embargo el lector debe conocer que los elementos
motrices que generan el movimiento de las articulaciones pueden ser, según la energía
que consuman, de tipo hidráulico, neumático, eléctrico o basado en las propiedades de
las aleaciones con memoria de forma (SMA).
9. Prácticas de Robótica utilizando Matlab®
Práctica 1 .- Pág. 8
Elementos terminales
A la muñeca del manipulador se acopla una garra o una herramienta, que será la
encargada de materializar el trabajo previsto. El elemento terminal debe ser capaz de
agarrar la carga máxima del robot, a la vez que debe ser de dimensiones y peso
reducido. Esto obliga en muchas ocasiones a soluciones de compromiso en que el
elemento terminal se diseña para una aplicación concreta. Normalmente se opta por
garras neumáticas. Algunas garras de sujeción son las mostradas en las figuras
siguientes.
Figura 1.8.- Morfología de distintas garras neumáticas
La siguiente figura muestra diferentes formas de sujetar la carga. La fricción
entre la carga y los dedos de la pinza es un factor determinante a la hora de elegir o
diseñar una pinza. El la literatura especializada y en los catálogos de los fabricantes
suelen aparecer tablas que permiten seleccionar la pinza adecuada para cada aplicación.
Figura 1.9.- Sujeción de una pieza por distintas pinzas neumáticas
10. Prácticas de Robótica utilizando Matlab®
Práctica 1 .- Pág. 9
1.2.- Sistemas de representación utilizados en robótica.
Tal y como se vio en el epígrafe anterior, las diferentes arquitecturas mecánicas
de robots aconsejan el uso de distintos sistemas de representación de acuerdo con la
morfología de cada robot.
Representación de la posición
Es común en robótica el uso de coordenadas cartesianas para localizar un cuerpo
en el espacio, sin embargo es igualmente válido y el lector encontrará varios autores que
hacen uso de las coordenadas polares o esféricas en sus desarrollos matemáticos.
Coordenadas cartesianas en 2 y 3 dimensiones
Coordenadas polares en 2 y 3 dimensiones
Coordenadas esféricas
Figura 1.10.- Sistemas de representación de posición utilizados en Robótica
11. Prácticas de Robótica utilizando Matlab®
Práctica 1 .- Pág. 10
Representación de la orientación
La orientación de un sólido en el espacio se puede especificar utilizando varios
sistemas de referencia. La orientación en el espacio tridimensional viene definida por
tres grados de libertad, luego será necesario un mínimo de tres parámetros linealmente
independientes.
En robótica es común el uso de matrices de rotación, debido a la comodidad
que proporciona el uso del álgebra matricial. La matriz de rotación es una matriz 3x3
ortogonal de cosenos directores que define la orientación del sistema OUVW móvil
respecto al sistema OXYZ fijo, y que tiene la forma siguiente:
⎥
⎥
⎥
⎦
⎤
⎢
⎢
⎢
⎣
⎡
=
wzvzuz
wyvyuy
wxvxux
kkjkik
kjjjij
kijiii
R
donde [ ]zyx kji son los vectores unitarios del sistema OXYZ y [ ]wvu kji los
vectores unitarios del sistema OUVW.
El ejemplo siguiente muestra la matriz de rotación del sistema OUVW que ha
girado un ángulo α respecto a OXYZ.
( )
⎥
⎥
⎥
⎦
⎤
⎢
⎢
⎢
⎣
⎡
−=
αα
ααα
cos0
cos0
001
,
sen
senxR
Figura 1.11.- Rotación del sistema OUVW respecto al eje OX
Las matrices ortonormales de rotación pueden componerse para expresar la
aplicación continua de varias rotaciones (Recuerde el lector que el orden en que se
realizan las rotaciones es importante, pues el producto de matrices no es conmutativo).
También es común en robótica el uso de cuaternios, pero debido a que no se
utilizan en los desarrollos de las prácticas de este libro, no van a ser estudiados aquí.
12. Prácticas de Robótica utilizando Matlab®
Práctica 1 .- Pág. 11
1.3.- Herramientas para la Simulación de Sistemas Robotizados.
Ámbito de aplicación.
Como ya se comentó en la introducción, la simulación de sistemas robotizados
ha estado íntimamente ligada a la potencia computacional de los procesadores de
cálculo. El gran avance producido con los microprocesadores actuales ha permitido
obtener paquetes de simulación dinámica como ADAMS de MDI
(http://www.adams.com) capaces de simular el comportamiento dinámico de casi
cualquier mecanismo multicuerpo. Estos paquetes incorporan amplias librerías de
articulaciones y fuerzas que permiten al usuario construir su modelo en un tiempo
relativamente corto. Estos paquetes son utilizados en los centros de investigación y en
las empresas de tecnología para el diseño de prototipos mecánicos. Sin embargo las
altas prestaciones de este tipo de paquetes hacen que su coste económico sea alto para
una primera aproximación al mundo de la robótica.
Figura 1.12.- Paquete de simulación dinámica ADAMS
Asimismo, en la red pueden encontrarse multitud de paquetes gratuitos o de
demostración de software especializado en la simulación de robots clásicos. La mayoría
de este software incorpora una interfaz gráfica de simulación avanzada, dando lugar a
resultados muy vistosos. Sin embargo, ya se ha comentado que estos paquetes suelen ser
cerrados desde el punto de vista del código fuente, y por lo tanto están limitados a las
capacidades que el programador haya incorporado antes de su publicación. El
comportamiento dinámico de los mecanismos no suele estar implementado en estos
simuladores, o bien lo está de una manera oscura y sin especificar las simplificaciones
que se han realizado en él. Estos paquetes están destinados normalmente a la educación,
y a la realización de prácticas por parte de los alumnos. La siguiente tabla muestra una
lista de algunos de estos simuladores que los autores han encontrado en la red.
13. Prácticas de Robótica utilizando Matlab®
Práctica 1 .- Pág. 12
Simulador Dirección web Imágenes
Easy Robot http://www.easy-rob.de/
Roboworks http://www.newtonium.com
Simrobot University of Bremen
Wits http://mars.graham.com/mplwits/
Workspace http://www.rosl.com
Tabla 1.2.- Algunos simuladores de robots disponibles
En otro nivel se sitúan aquellas herramientas diseñadas para el análisis de
sistemas robotizados que se presentan con el código fuente accesible al usuario. La
14. Prácticas de Robótica utilizando Matlab®
Práctica 1 .- Pág. 13
filosofía del código abierto pretende ampliar continuamente las capacidades de ese
código, permitiendo que los lectores y programadores añadan su contribución a este
código. A los autores nos ha parecido interesante este enfoque, pues este libro pretende
servir como un primer paso para la formación de expertos en robótica que conozcan el
cómo funcionan las cosas. La magnifica Robotics Toolbox de Matlab
de Peter I. Corke
y el código desarrollado por los autores en este libro pertenecen a esta clasificación.
Figura 1.13.- Herramienta de simulación desarrollada en el libro
1.4.- Presentación de los dos robots ficticios.
Finalmente en esta práctica se presentan los dos robots ficticios que se utilizan a
lo largo del libro para la realización de las prácticas, ejemplos y ejercicios
recomendados. Se trata de dos robots industriales ficticios, con cadenas cinemáticas
distintas de 4 y 6 grados de libertad respectivamente.
El robot de 4 grados de libertad tiene una configuración de robot cilíndrico en el
que las articulaciones 2 y 3 son prismáticas, mientras que las articulaciones 1 y 4 son
rotacionales. Las dimensiones del robot son las indicadas en la figura siguiente (en
metros) en la que d2 y d3 corresponden a coordenadas articulares, y por tanto son
variables del robot, al igual que θ1 y θ4.
15. Prácticas de Robótica utilizando Matlab®
Práctica 1 .- Pág. 14
Figura 1.14.- Robot prismático de 4 gdl
El robot de 6 grados de libertad tiene una configuración antropomórfica o
angular con todas las articulaciones rotacionales. Las dimensiones en metros están
indicadas en la figura 1.15.
Figura 1.15.- Robot rotacional de 6 gdl
0.4
0.2
0.1
d2
d3
θ1
θ4
Robot Prismático de 4 gdl
Robot Rotacional de 6 gdl
0.315
0.45
0.5
0.08
16. Prácticas de Robótica utilizando Matlab®
Práctica 1 .- Pág. 15
1.5.-PRÁCTICA.Presentación de la Robotics Toolbox de Matlab
Se trata de una toolbox de Matlab
que proporciona varias funciones que son
útiles en robótica. Su conocimiento va a permitir disponer de una serie de herramientas
para la verificación de los resultados obtenidos.
La Robotics Toolbox fue desarrollada por Peter I. Corke en 1996 y puede
obtenerse libremente desde MathWorks en la siguiente dirección:
ftp.mathworks.com
en el direcctorio /pub/contrib/misc/robot.
Una vez instalada la toolbox se ejecutará en primer lugar el comando rtdemo que
ofrece una demo de algunas de las funciones implementadas en la toolbox. Al ejecutar
la demo aparece un menu (fig. 1.16) a partir del cual se ejecutan varios comandos de la
toolbox.
Figura 1.16.- Ejecución de la demo de Robotics Toolbox
17. Prácticas de Robótica utilizando Matlab®
Práctica 1 .- Pág. 16
Como todos los comandos de Matlab
la Robotics Toolbox nos ofrece ayuda en
línea en la que se puede conocer el uso de cada una de las funciones:
» help jtraj
JTRAJ Compute a joint space trajectory between two points
[Q QD QDD] = JTRAJ(Q0, Q1, N)
[Q QD QDD] = JTRAJ(Q0, Q1, N, QD0, QD1)
[Q QD QDD] = JTRAJ(Q0, Q1, T)
[Q QD QDD] = JTRAJ(Q0, Q1, T, QD0, QD1)
Returns a joint space trajectory Q from state Q0 to Q1. The number
of points is N or the length of the given time vector T. A 7th
order polynomial is used with default zero boundary conditions for
velocity and acceleration. Non-zero boundary velocities can be
optionally specified as QD0 and QD1.
The function can optionally return a velocity and acceleration
trajectories as QD and QDD.
Each trajectory is an mxn matrix, with one row per time step, and
one column per joint parameter.
»
La demo de Matlab
incluye gráficas de planificación de trayectorias y
animación alámbrica de robots donde se pueden visualizar los trayectorias calculadas.
Figura 1.17.- Ejecución de la demo de Robotics Toolbox
18. Prácticas de Robótica utilizando Matlab®
Práctica 1 .- Pág. 17
EJERCICIOS PROPUESTOS
Utilizando las funciones de la Toolbox de Matlab
:
dar la matriz de transformación de una translación de 6 unidades en el eje X, -3
unidades en el eje Y y 8 unidades en el eje Z.
dar la matriz de transformación de una rotación de 0º en X, 0º en Y y –90º en Z.
dar la matriz de transformación de un sistema que se ha desplazado 6 unidades
en X, ha rotado 45º en Y y -90º en Z
19. Prácticas de Robótica utilizando Matlab®
Práctica 2 .- Pág. 1
Práctica 2
Cinemática de Robots
2.1.- Introducción
Los robots clásicos presentan una arquitectura antropomórfica serial, semejante al
brazo humano. Consisten de una serie de barras rígidas unidas entre sí a través de
articulaciones de un grado de libertad del tipo rotacional o prismática. En general cada
articulación logra su movimiento a través de un accionamiento de potencia e incluye
otros dispositivos como reductores de velocidad, frenos y sensores de posición o
velocidad.
Aunque al definir las relaciones cinemáticas de un robot no se suelen consideran los
aspectos dinámicos, nada más alejado de la realidad cuando se quiere diseñar un robot
ya que existe una inevitable relación causa-efecto entre la cinemática y la dinámica.
Nada más claro resulta que al pensar en las dimensiones de un robot, la longitud de un
brazo afecta al cuadrado la inercia de los eslabones y por lo tanto el peso del robot y la
potencia requerida en los actuadores.
Las arquitecturas de los robots clásicos presentan una serie de propiedades
dinámicas y estructurales caracterizadas por una gran rigidez estructural, repetibilidad y
elevado peso propio. El elevado peso propio de los robots clásicos limita la capacidad
carga útil y las velocidades de trabajo, las cuales usualmente están en torno a los 60
grados/seg. para las primeras tres articulaciones de los robots industriales (robots de
soldadura) y 250 grados/seg. para los robots pequeños de altas prestaciones como el
STÄUBLI RX90.
20. Prácticas de Robótica utilizando Matlab®
Práctica 2 .- Pág. 2
Un aspecto importante que refleja las relaciones dinámicas del robot respecto de la
carga útil que pueden manipular, puede estudiarse en la siguiente tabla.
Robot Peso Carga útil Repetibilidad Carga útil/peso
ABB IRB 2000 370 10 0.100 0.0270
ABB IRB 4400 940 45 0.100 0.0047
ABB IRB 6400/3.0 1450 75 0.100 0.0510
STÄUBLI RX 90 120 6 0.002 0.0500
GMF S 10 200 10 0.200 0.0417
Hitachi M6100 410 10 0.100 0.0243
Puma 550 63 4 0.100 0.0063
SCARA Adept 3 205 25 0.025 0.1220
SCARA GMF A-600 120 6 0.013 0.0500
Al calcular la cinemática de los robots clásicos debe considerarse que dependiendo
de las dimensiones de sus primeras articulaciones, el peso de los robots de tipo
industrial oscila en torno a valores que tienen una relación en el mejor de los casos de
0.150 (Carga útil/peso). Por lo cual, por ejemplo un robot industrial con un alcance de
3.0 metros con capacidad para mover cargas de 75 kg puede tener un peso de 1450 kg
(ABB IRB 6400).
Las siguientes son algunas recomendaciones que deben tenerse en cuenta al definir
la cinemática de un robot, la cual debe hacerse en consideración de la dinámica que
imponen las dimensiones de las barras que lo forman:
El espacio de trabajo del robot debe ser cuidadosamente estudiado para definir el
volumen justo de trabajo del robot
En un robot de seis grados de libertad rotacional, las primeras tres barras son las que
aportan la mayor dinámica debido a su peso. A menudo es posible localizar los
primeros tres accionamientos de potencia en la base del robot, pero para lograr esto
se debe ser cuidadoso en el uso de mecanismos de cuatro barras que mueven el
brazo mas alejado (robot ABB IRB2400).
En un robot de seis grados de libertad, las tres primeras articulaciones del robot
deben dar las condiciones de posición y las tres últimas articulaciones del extremo
del robot deben concentrar en un punto de la mano, los tres grados de libertad de
orientación.
En esta práctica se van a presentar las herramientas necesarias para resolver los dos
problemas fundamentales en el estudio de la cinemática del robot. El primero de ellos,
consiste en determinar la posición y orientación del extremo final de la cadena
cinemática conocidos los valores de las coordenadas articulares y las características
geométricas del robot, y es conocido como problema cinemático directo. La solución
del problema inverso permite hallar las variables articulares conocida la posición y
orientación del extremo de la cadena cinemática. Para la resolución de estos problemas
se utiliza la representación de Denavit-Hartenberg y las matrices de transformación
homogénea.
21. Prácticas de Robótica utilizando Matlab®
Práctica 2 .- Pág. 3
De acuerdo con la estructura del libro, todos los apartados presentan
herramientas desarrolladas en MatLab
para la solución de los problemas planteados.
2.2- Cinemática directa del brazo de un robot manipulador
Las técnicas que se estudian aquí, se aplican a un manipulador mecánico de
cadena abierta y tratan el estudio analítico y el modelado en MatLab
de la geometría
del movimiento de un robot con respecto a un sistema de referencia fijo como una
función del tiempo sin considerar la dinámica.
2.2.1 El problema cinemático directo
El problema cinemático directo se plantea en términos de encontrar una matriz
de transformación que relaciona el sistema de coordenadas ligado al cuerpo en
movimiento respecto a un sistema de coordenadas que se toma como referencia. Para
lograr esta representación se usan las matrices de transformación homogénea 4x4, la
cual incluye las operaciones de traslación y la orientación.
La matriz de transformación homogénea es una matriz de 4x4 que transforma un
vector expresado en coordenadas homogéneas desde un sistema de coordenadas hasta
otro sistema de coordenadas. Para una descripción más amplia acerca de las bases
algebraicas de las transformaciones homogéneas se recomienda estudiar las referencias:
[1] y [2].
La matriz de transformación homogénea tiene la siguiente estructura:
⎥
⎥
⎥
⎥
⎦
⎤
⎢
⎢
⎢
⎢
⎣
⎡
=⎥
⎦
⎤
⎢
⎣
⎡
=
1000
31 zzzz
yyyy
xxxx
x pasn
pasn
pasn
escaladof
posicióndevectorrotacióndematriz
T
⎥
⎦
⎤
⎢
⎣
⎡
=
1000
pasn
T
donde los vectores n, s, a, son vectores ortogonales unitarios y p es un vector que
describe la posición x, y, z del origen del sistema actual respecto del sistema de
referencia.
Para entender las propiedades de la matriz de transformación homogénea nos fijamos en
el siguiente gráfico.
22. Prácticas de Robótica utilizando Matlab®
Práctica 2 .- Pág. 4
Figura-2.1. Interpretación geométrica de la matriz de transformación homogénea
⎥
⎥
⎥
⎥
⎦
⎤
⎢
⎢
⎢
⎢
⎣
⎡
−
−
−
=
1000
3001
4010
10100
j
i
T
Al analizar las columnas de la submatriz de rotación de la matriz de
transformación homogénea i
Tj, un observador localizado en el origen de sitema-i, puede
ver como están orientados los ejes x, y, z del sistema-j, además también observa como
se ha desplazado en coordenadas cartesianas el origen del sistema-j respecto del origen
del sistema de referencia con la información del vector de posición.
2.2.2 La representación de Denavit-Hartenberg
La representación de D-H, se aplica a robots de cadena cinemática abierta y consiste
en una serie de reglas para colocar los sistemas de referencia de cada eslabón del robot.
Antes de aplicar el método de D-H es importante tener en cuenta los siguientes
comentarios:
• Se parte de una configuración cualesquiera del robot, si bien es aconsejable
colocarlo en una posición sencilla de analizar.
• Se numeran los eslabones, asignando el 0 para la base y n-1 para el último eslabón,
siendo n el número de grados de libertad del robot.
• El sistema de coordenadas ortonormal dextrógiro de la base (x0, y0, z0) se establece
con el eje z0 localizado a lo largo del eje de movimiento de la articulación 1 y
apuntando hacia fuera del hombro del brazo del robot.
• El sistema de referencia de cada eslabón se coloca al final del mismo, en el extremo
de la articulación a la cual esta conectado el eslabón siguiente.
• El ángulo ó desplazamiento de cada eslabón siempre se mide tomando como base el
sistema de referencia del eslabón anterior.
x
y
z
z
x
y
i
Tj
Sistema-i
Sistema-j
p=(10, 4, 3)
23. Prácticas de Robótica utilizando Matlab®
Práctica 2 .- Pág. 5
• Al colocar el sistema de referencia del eslabón-i, se deben seguir las siguientes
reglas:
- El eje zi del sistema de referencia debe quedar alineado a lo largo de la
articulación
- El eje xi debe colocarse con orientación normal al plano formado por los ejes zi-1
y zi
• Al establecer los sistemas de coordenadas de la mano se debe tener en cuenta el
principio de Pieper’s en el cual se establece que los tres últimos sistemas de
referencia se intercepten en un punto, lo cual permite obtener una solución para el
problema cinemático inverso de forma cerrada para estas articulaciones.
Además de las reglas anteriores la convención de D-H establece las siguientes
condiciones para los demás parámetros geométricos, de acuerdo a la figura-2.2.
Figura-2.2. Sistemas de coordenadas para la convención de D-H.
Cada sistema de coordenadas se establece sobre las siguientes reglas.
θi: Es el ángulo de la articulación desde el eje xi-1 hasta el eje xi, medido
respecto del eje zi-1, usando la regla de la mano derecha.
di: Es la distancia medida desde el origen del sistema i-1, a lo largo del eje zi-1
hasta la intersección del eje zi-1 con el eje xi.
ai: Es la distancia de separación entre los orígenes de los sistemas de referencia
i-1 e i, medida a lo largo del eje xi hasta la intersección con el eje zi-1. (o la
distancia más corta entre los ejes zi-1 y zi, cuando estos no se interceptan)
αi: Es el ángulo que separa los ejes zi y zi-1, medido respecto del eje xi
di
ai
θi
αi
xi-1
zi-1
xi
zi
Eslabón i-1
Eslabón i
Eslabón i+1
Articulación i Articulación i+1
θi θi+1
24. Prácticas de Robótica utilizando Matlab®
Práctica 2 .- Pág. 6
Con base en la figura-2.2 y de acuerdo a las reglas de D-H, se determina la siguiente
matriz de transformación homogénea:
⎥
⎥
⎥
⎥
⎦
⎤
⎢
⎢
⎢
⎢
⎣
⎡
−
⎥
⎥
⎥
⎥
⎦
⎤
⎢
⎢
⎢
⎢
⎣
⎡
⎥
⎥
⎥
⎥
⎦
⎤
⎢
⎢
⎢
⎢
⎣
⎡ −
⎥
⎥
⎥
⎥
⎦
⎤
⎢
⎢
⎢
⎢
⎣
⎡
=−
0000
00
00
0001
1000
0100
0010
001
1000
0100
00
00
1000
100
0010
0001
1
ii
ii
i
ii
ii
i
i
i
cs
sc
a
cs
sc
d
A
αα
ααθθ
θθ
⎥
⎥
⎥
⎥
⎦
⎤
⎢
⎢
⎢
⎢
⎣
⎡
−
−
=−
1000
0
1
iii
iiiiiii
iiiiiii
i
i
dcs
sacsccs
cassscc
A
αα
θθαθαθ
θθαθαθ
Código en Matlab
. La función DENAVIT realiza los cálculos anteriores devolviendo
la matriz de transformación homogénea
% DENAVIT Matriz de transformación homogénea.
% DH = DENAVIT(TETA, D, A, ALFA) devuelve la matriz de transformacion
% homogénea 4 x 4 a partir de los parametros de Denavit-Hartenberg
% D, ALFA, A y TETA.
%
% See also DIRECTKINEMATIC.
function dh=denavit(teta, d, a, alfa)
dh=[cos(teta) -cos(alfa)*sin(teta) sin(alfa)*sin(teta) a*cos(teta);
sin(teta) cos(alfa)*cos(teta) -sin(alfa)*cos(teta) a*sin(teta);
0 sin(alfa) cos(alfa) d;
0 0 0 1];
25. Prácticas de Robótica utilizando Matlab®
Práctica 2 .- Pág. 7
2.2.3 Representación de la cinemática directa de robots manipuladores
En esta sección se explican algunas arquitecturas de robots y como construir la
tabla de los parámetros de D-H. Para una información más detallada sobre este tema, se
recomienda estudiar las referencias [1] y [2].
Ejemplo 2.1
Figura-2.3 Parámetros de D-H para un robot cilíndrico
Eslabón θi di ai αi
1 θ1 l1 0 0
2 0 d2 - a2 -π/2
3 0 d3 0 0
4 θ4 l4 0 0
Tabla 2.1 Parámetros de D-H para el robot cilíndrico de la figura-2.3
• Note el lector el signo negativo del parámetro a2 así como la localización del origen
del sistema de coordenadas (x2, y2, z2)
• Las variables articulares son en este caso θ1, d2, d3, θ4
z1
θ1
x0
y0
z0
x1 y1
z2
y2
x2
x3
y3
l1
d2
d3
a2
l4
0
z4
1
2
3
4 θ4
x4
y4
z3
26. Prácticas de Robótica utilizando Matlab®
Práctica 2 .- Pág. 8
Ejemplo 2.2
Figura-2.4 Parámetros de D-H para un robot rotacional
Tabla 2.2 Parámetros de D-H para el robot cilíndrico de la figura-2.4
Eslabón θi di ai αi
1 θ1+π/2 l1 0 -π/2
2 θ2-π/2 0 a2 0
3 θ3+π 0 0 π/2
4 θ4 l4 0 −π/2
5 θ5 0 0 π/2
6 θ6 l6 0 0
• Note el lector que en este caso todas las variables articulares corresponden a los
giros en las articulaciones.
• Obsérvese la rotación en θ4 entre los eslabones 3 y 4.
• Obsérvese que el sistema de referencia (x3, y3, z3) se ha trasladado al principio del
eslabón 3 cumpliendo con la representación de D-H.
x0
z0
y0
θ1
z2
y2
x2
θ3
z4
y4
x4
s
a
n
θ6
θ5
y5
z5
x5
y3
z3
x3
θ4
z1
y1
x1
θ2
l1
a2
l4
l6
0
1
2
3
4
5
6
27. Prácticas de Robótica utilizando Matlab®
Práctica 2 .- Pág. 9
Código en Matlab
. La función DIRECTKINEMATIC resuelve la cinemática directa de
robots de cadena abierta. Para utilizarla debe invocarse junto con el vector de
coordenadas articulares Q de n componentes siendo n el número de eslabones del robot.
A continuación se muestra el código fuente utilizado para cada uno de los robots
que se están utilizando como ejemplos. El lector debe notar las modificaciones
realizadas en el código, debido al diferente número de eslabones de cada robot y a los
parámetros D-H de cada robot. Se recomienda que se realicen estos ejemplos como
práctica de los conocimientos adquiridos.
Ejemplo 2.3
Código fuente de la función DIRECTKINEMATIC para el robot cilíndrico de 4
grados de libertad del ejemplo 2.1
Debe notarse que el vector de coordenadas articulares Q representa los 4 grados
de libertad del robot, 2 rotacionales y 2 prismáticas y se introduce en los parámetros D-
H como variables.
Las dimensiones del robot, introducidas en los parámetros de D-H, son las
indicadas en el capítulo 1.
% DIRECTKINEMATIC4 Direct Kinematic.
% A04 = DIRECTKINEMATIC4(Q) devuelve la matriz de transformación del
% primer sistema de coordenadas al último en función del vector Q
% de variables articulares.
%
% See also DENAVIT.
function A04 = directkinematic4(q)
% Parámetros Denavit-Hartenberg del robot
teta = [q(1) 0 0 q(4)];
d = [0.4 q(2) q(3) 0.2 ];
a = [0 -0.1 0 0 ];
alfa = [0 -pi/2 0 0 ];
% Matrices de transformación homogénea entre sistemas de coordenadas
consecutivos
A01 = denavit(teta(1), d(1), a(1), alfa(1));
A12 = denavit(teta(2), d(2), a(2), alfa(2));
A23 = denavit(teta(3), d(3), a(3), alfa(3));
A34 = denavit(teta(4), d(4), a(4), alfa(4));
% Matriz de transformación del primer al último sistema de
coordenadas
A04 = A01 * A12 * A23 * A34;
28. Prácticas de Robótica utilizando Matlab®
Práctica 2 .- Pág. 10
A continuación se presenta la matriz obtenida para una configuración en la que
todas las coordenadas articulares tienen valor nulo:
» q=zeros(4,1)
q =
0
0
0
0
» T=directkinematic4(q)
T =
1.0000 0 0 -0.1000
0 0.0000 1.0000 0.2000
0 -1.0000 0.0000 0.4000
0 0 0 1.0000
»
⇒ Se recomienda analizar la matriz homogénea obtenida tal y como se indicó en
el apartado 2.2.1, comprobando que tanto la posición como la orientación del efector
final coinciden con lo esperado cuando se introducen las coordenadas articulares del
ejemplo.
Ejemplo 2.4
Código fuente de la función DIRECTKINEMATIC para el robot rotacional de 6
grados de libertad del ejemplo 2.2
Debe notarse que en el caso particular de un robot enteramente rotacional, las
variables articulares representan los parámetros teta de D-H, como se estudió en el
apartado 2.2.3
Las dimensiones del robot, introducidas en los parámetros de D-H, son las
indicadas en la práctica 1.
29. Prácticas de Robótica utilizando Matlab®
Práctica 2 .- Pág. 11
% DIRECTKINEMATIC6 Direct Kinematic.
% A06 = DIRECTKINEMATIC6(Q) devuelve la matriz de transformación del
% primer sistema de coordenadas al último en función del vector Q
% de variables articulares.
%
% See also DENAVIT.
function A06 = directkinematic6(q)
% Parámetros Denavit-Hartenberg del robot
teta = q;
d = [0.315 0 0 0.5 0 0.08];
a = [0 0.45 0 0 0 0];
alfa = [-pi/2 0 pi/2 -pi/2 pi/2 0];
% Matrices de transformación homogénea entre sistemas de coordenadas
consecutivos
A01 = denavit(teta(1), d(1), a(1), alfa(1));
A12 = denavit(teta(2), d(2), a(2), alfa(2));
A23 = denavit(teta(3), d(3), a(3), alfa(3));
A34 = denavit(teta(4), d(4), a(4), alfa(4));
A45 = denavit(teta(5), d(5), a(5), alfa(5));
A56 = denavit(teta(6), d(6), a(6), alfa(6));
% Matriz de transformación del primer al último sistema de
coordenadas
A06 = A01 * A12 * A23 * A34 * A45 * A56;
A continuación se presenta la matriz obtenida para una configuración en la que todas las
coordenadas articulares tienen valor nulo:
» q=zeros(6,1)
q =
0
0
0
0
0
0
» T=directkinematic6(q)
T =
1.0000 0 0 0.4500
0 1.0000 0 0
0 0 1.0000 0.8950
0 0 0 1.0000
30. Prácticas de Robótica utilizando Matlab®
Práctica 2 .- Pág. 12
2.3.- Cinemática inversa del brazo de un robot manipulador
La cinemática inversa consiste en hallar los valores de las coordenadas
articulares del robot [ ]T
nqqqq ,....,, 21= conocida la posición y orientación del extremo
del robot.
A pesar de que en la literatura [1] y [2] se pueden encontrar diversos métodos
genéricos para la resolución de la cinemática inversa que pueden ser implementados en
computadora, suele ser habitual la resolución por medio de métodos geométricos. La
mayor parte de los robots suelen tener cadenas cinemáticas relativamente sencillas, que
facilitan la utilización de los métodos geométricos. Para muchos robots, si se consideran
sólo los tres primeros grados de libertad, se tiene una estructura planar. Este hecho
facilita la resolución del problema. Asimismo los últimos tres grados de libertad suelen
usarse para la orientación de la herramienta, lo cual permite una resolución geométrica
desacoplada de la posición de la muñeca del robot y de la orientación de la herramienta.
En esta sección se va a resolver el problema cinemático inverso para los dos
robots anteriores, utilizando el método geométrico e implementándolo en Matlab
.
Ejemplo 2.5
Solución del robot cilíndrico de 4 grados de libertad.
En este caso particular, la solución geométrica es inmediata. Se parte de que la posición
del extremo del robot es conocida (px, py, pz) y se va a calcular los valores de las
coordenadas articulares.
Articulación 1
Para obtener el valor de θ1 (TETA1 en el código de Matlab
.) se proyecta el
punto del extremo del robot (px, py, pz) sobre el plano (x0, y0, z0) obteniendo una
sencilla relación angular. Sabiendo que θ1 es el ángulo entre x0 y x1, se obtienen las
siguientes gráficas.
31. Prácticas de Robótica utilizando Matlab®
Práctica 2 .- Pág. 13
Figura-2.5 Cinemática inversa del robot de 4 gdl.
De las que se deducen las siguientes relaciones:
22
yx ppR +=
43
2
2
2
ldaRr +=−=
R
p
sin
y
=φ
R
px
=φcos
R
a
sin 2
=β
R
r
=βcos
utilizando la función atan2 de Matlab
se calculan los valores de los ángulos:
)cos,(2 φφφ sinatan=
)cos,(2 βββ sinatan=
que permiten el cálculo de θ1 como φ-β.
Articulación 2
De la figura 2.3 se obtiene la siguiente fórmula:
1221 lpdpdl zz −=⇒=+
Articulación 3
De la figura 2.5:
43
2
2
2
ldaRr +=−=
px,py,pz
orientación
y0
x0
z0
φ
x0
y0
a2
d3+l4
θ1
β
R
r
32. Prácticas de Robótica utilizando Matlab®
Práctica 2 .- Pág. 14
Articulación 4
Para calcular la última articulación se necesita el cálculo previo del sistema de
referencia (x3, y3, z3), que se resolverá mediante la cinemática directa explicada en el
ejemplo 2.3. Dado que el vector a de aproximación es necesariamente paralelo a z4 se
deben cumplir las siguientes relaciones:
34 ·ynsin =θ
)cos,(2 444 θθθ sinatan=
34 ·cos ys=θ
Código en Matlab
. La función INVERSEKINEMATIC4 resuelve la cinemática inversa
del robot cilíndrico de 4 gdl. Para ello toma como parámetros la matriz homogénea T,
que representa la orientación y posición del extremo del robot y devuelve el vector de
coordenadas articulares.
% Q = INVERSEKINEMATIC4(T) devuelve el vector de coordenadas
% articulares correspondiente a la solución cinemática inversa de
% la mano del manipulador en la posición y orientación expresadas
% en la matriz T.
%
% See also DIRECTKINEMATIC4, DENAVIT.
function q = inversekinematic4(T)
p = T(1:3,4); % Posición de la mano del manipulador
% Inicialización de las variables articulares a calcular
q1 = 0;
q2 = 0;
q3 = 0;
q4 = 0;
% Parámetros Denavit-Hartenberg del robot
teta = [q1 0 0 q4 ];
d = [0.4 q2 q3 0.2];
a = [0 -0.1 0 0 ];
alfa = [0 -pi/2 0 0 ];
% Solución de la primera articulación: q1
R = sqrt(p(1)^2+p(2)^2);
r = sqrt(R^2-a(2)^2);
sphi = -p(1)/R;
cphi = p(2)/R;
phi = atan2(sphi, cphi);
y3
x4
x3
θ4
donde n y s son vectores de orientación del extremo
del robot.
33. Prácticas de Robótica utilizando Matlab®
Práctica 2 .- Pág. 15
sbeta = -a(2)/R;
cbeta = r/R;
beta = atan2(sbeta, cbeta);
q1 = phi - beta;
% Solución de la segunda articulación: q2
q2 = p(3) - d(1);
% Solución de la tercera articulación: q3
q3 = r - d(4);
% Solución de la cuarta articulación: q4
% Cálculo de la matriz de transformación A03
A01 = denavit(q1, d(1), a(1), alfa(1));
A12 = denavit(teta(2), q2, a(2), alfa(2));
A23 = denavit(teta(3), q3, a(3), alfa(3));
A03 = A01 * A12 * A23;
y3 = A03(1:3,2);
sq4 = dot(T(1:3,1), y3); % Vector orientación n: T(1:3,1)
cq4 = dot(T(1:3,2), y3); % Vector orientación s: T(1:3,2)
q4 = atan2(sq4, cq4);
% Vector de variables articulares
q = [q1 q2 q3 q4]';
⇒ Se observa como la cinemática directa está incluida en los cálculos necesarios
para obtener la matriz 0
A3.
En el ejemplo mostrado a continuación se puede comprobar como después de
asignar un vector de coordenadas articulares aleatorio, y obtener la matriz homogénea
del extremo de robot correspondiente a este vector, si sobre esta matriz se aplica la
función INVERSEKINEMATIC4 se obtiene el vector q original.
34. Prácticas de Robótica utilizando Matlab®
Práctica 2 .- Pág. 16
» q=rand(4,1)
q =
0.8913
0.7621
0.4565
0.0185
» T=directkinematic4(q)
T =
0.6283 -0.0116 -0.7779 -0.5735
0.7778 -0.0144 0.6284 0.3347
-0.0185 -0.9998 0.0000 1.1621
0 0 0 1.0000
» inversekinematic4(T)
ans =
0.8913
0.7621
0.4565
0.0185
Ejemplo 2.6
Solución del robot rotacional de 6 grados de libertad.
En contraste con el ejemplo anterior, la solución de la cinemática inversa de un robot de
6 grados de libertad requiere del lector un mayor esfuerzo de comprensión. En este
ejemplo se va a resolver mediante el método geométrico. Para ello, en virtud del
principio de Pieper’s, es necesario separar el cálculo de la posición y orientación del
extremo del robot.
En primer lugar se calcula la posición del punto de intersección de los 3 últimos grados
de libertad, que se conoce como MUÑECA del robot. Con las coordenadas (px, py, pz)
de la muñeca se resuelve el problema de la posición, de manera similar al ejemplo
anterior.
Los 3 últimos grados de libertad se utilizan para orientar la herramienta colocada en el
extremo del robot, y se resuelven en un paso posterior.
Como se observa en la figura 2.6, la articulación 3 (θ3), en adelante CODO del robot,
puede tener dos valores distintos, conocidos como configuración CODO ARRIBA y
CODO ABAJO, y que representan los casos en que la articulación 3 está situada por
35. Prácticas de Robótica utilizando Matlab®
Práctica 2 .- Pág. 17
debajo o por encima de la articulación 2, para lograr una misma posición de la muñeca.
Asimismo, la MUÑECA puede tomar también dos configuraciones distintas, MUÑECA
ARRIBA y MUÑECA ABAJO, para obtener una misma orientación del efector final.
Debido a las anteriores consideraciones, es necesario introducir en la resolución del
problema cinemático inverso dos parámetros, llamados por similitud CODO y
MUÑECA, que representen las cuatro posibles configuraciones que puede adoptar el
robot cuando se le solicita alcanzar una posición y orientación determinadas.
Para una descripción más amplia acerca de la resolución del problema cinemático
inverso se recomienda estudiar las referencias: [1] y [2].
Figura-2.6 Cinemática inversa del robot de 6 gdl.
A continuación se muestra la solución geométrica del robot estudiado y su
implementación en Matlab
.
⇒ Se recomienda seguir las explicaciones con el código fuente de la función
INVERSEKINEMATIC6.
Problema de posición. Cálculo de las tres primeras articulaciones.
Articulación 1
22
yx ppR +=
R
p
sin
y
=1θ
R
px
=1cosθ
)cos,(2 111 θθθ sinatan=
MUÑECA
px, py, pzCODO
a
s
n
y0
x0
z0
R
c
px,py
θ1
36. Prácticas de Robótica utilizando Matlab®
Práctica 2 .- Pág. 18
Articulación 2
Para la resolución de la segunda articulación, se ha de tener en cuenta la posibilidad de
CODO ARRIBA y CODO ABAJO. En cada caso el ángulo θ2 se calculará como la
suma o la resta de los ángulos α y β calculados.
Configuración CODO ARRIBA Configuración CODO ABAJO
Identificando las relaciones geométricas presentes en los anteriores esquemas:
2
1
22
)( lpppr zyx −++=
βcos2 2
2
2
22
3 rllrl −+=
βcos
2 2
2
2
22
3
=
−
+−
rl
lrl
ββ 2
cos1−±=sin
r
pl
sin z−
= 1
α
r
R
=αcos
βαθ +=2 βαθ −=2
Configuración CODO ARRIBA Configuración CODO ABAJO
l1
l2
l3
r
px ,py, pz
α
β
θ2
x2
x1
l1
l2
l3r
px ,py, pz
α
β
θ2
x2
x1
37. Prácticas de Robótica utilizando Matlab®
Práctica 2 .- Pág. 19
Articulación 3
Al igual que en la articulación 2, se debe tener en cuenta las dos posibles
configuraciones del codo.
β
π
θ −=
2
3
3
2
3
π
βθ −=
Problema de orientación. Cálculo de las tres últimas articulaciones.
Conocidos los tres primeros ángulos θ1, θ2 y θ3 se resuelve la cinemática directa para
los tres primeros eslabones, obteniéndose la matriz 0
A3 necesaria para la resolución de
las últimas tres articulaciones. Siguiendo la referencia [1], para resolver el problema de
la orientación se han de conseguir que las tres últimas articulaciones cumplan los
criterios siguientes:
1. Establecer la articulación 4 de forma tal que una rotación respecto de la
rotación 5 alineará el eje de movimiento de la articulación 6 con el vector de
aproximación dado (a).
2. La articulación 5 alineará el eje de movimiento de la articulación 6 con el
vector de aproximación.
3. Fijar la articulación 6 para alinear el vector de orientación dado (s) (o el de
deslizamiento (n)) y el vector normal.
Matemáticamente, estos criterios significan:
1.
( )
xaz
xaz
z
3
3
4
±
=
2. a = z5
3. s = y6
Ambas orientaciones de la muñeca (ARRIBA y ABAJO) se definen observando la
orientación del sistema de coordenadas de la mano (n,s,a) con respecto al sistema de
coordenadas (x5, y5, z5). Para analizar las configuraciones de MUÑECA ARRIBA y
MUÑECA ABAJO, se utiliza un parámetro de orientación Ω (omega en el código de
Matlab
) que hace referencia a la orientación del vector unitario n (o s) con respecto al
vector unitario x5 (o y5) y que viene definido en la referencia Fu [1] como:
0 si se está en el caso degenerado
Ω= s·y5 si s·y5 ≠0
n·y5 si s·y5=0
l2
l1
l3
x2
x3
β
θ3
l2
l1
l3
x2
x3
β
θ3
38. Prácticas de Robótica utilizando Matlab®
Práctica 2 .- Pág. 20
Articulación 4
Para conocer el signo de la ecuación
( )
xaz
xaz
z
3
3
4
±
= se determina conociendo la
orientación Ω y la configuración de MUÑECA. Ω se calcula según su definición, y
MUÑECA es un parámetro del robot. Se parte de la suposición de que el signo es
positivo, y esta hipótesis se corrige con el parámetro M que se calcula como
combinación de la configuración de la muñeca y la orientación requeridas.
Utilizando los parámetros Ω y MUÑECA se construye la siguiente tabla:
Configuración MUÑECA Ω M=MUÑECA*sign(Ω)
ABAJO +1 ≥0 +1
ABAJO +1 <0 -1
ARRIBA -1 ≥0 -1
ARRIBA -1 <0 +1
que permite obtener M como parámetro para calcular la rotación θ4 en cualquiera de las
configuraciones posibles.
De la figura 2.4 se puede demostrar las siguientes relaciones:
)( 344 xzMsin •−=θ
)(cos 344 yzM •=θ
)cos,(2 444 θθθ sinatan=
Articulación 5
De la figura 2.4 se obtiene:
45 xasin •=θ
)(cos 45 ya •−=θ
)cos,(2 555 θθθ sinatan=
Articulación 6
56 ynsin •=θ
56cos ys •=θ
)cos,(2 666 θθθ sinatan=
x3
x4
y3
z4
θ4
θ4
y4
x4
n
a
θ5
x5
n
y5
s
θ6
θ6
39. Prácticas de Robótica utilizando Matlab®
Práctica 2 .- Pág. 21
Código en Matlab
.La función INVERSEKINEMATIC6 resuelve la cinemática inversa
del robot rotacional de 6 gdl. Para ello toma como parámetros la matriz homogénea T,
y los parámetros CODO y MUÑECA para definir las posibles configuraciones. Ha de
tenerse en consideración que determinados puntos y orientaciones que pertenezcan al
espacio de trabajo del robot, no podrán alcanzarse con algunas configuraciones. Por
ello se recomienda al lector que trate de experimentar con estos puntos extremos con la
herramienta que a continuación se presenta, y analice los resultados.
% INVERSEKINEMATIC6 Inverse Kinematic
% Q = INVERSEKINEMATIC6(T, CODO, MUNECA) devuelve el vector de coordenadas
% articulares correspondiente a la solución cinemática inversa de la mano
% del manipulador en la posición y orientación expresadas en la matriz T.
% CODO = 1 indica codo del robot arriba, es decir, que la articulación 3 se
% sitúa por encima de la articulación 2, mientras que CODO = -1 indica codo
% abajo, es decir que la articulación 2 se sitúa por encima de la 3.
% MUNECA = 1 indica que la muñeca del robot se sitúa por debajo de la
coordenada
% expresada en T, mientras que MUNECA = -1 significa que la muñeca se sitúa
% por arriba.
%
% See also DIRECTKINEMATIC6, DENAVIT.
function q = inversekinematic6(T,codo,muneca)
% Parámetros Denavit-Hartenberg del robot
d = [0.315 0 0 0.5 0 0.08];
a = [0 0.45 0 0 0 0];
alfa = [-pi/2 0 pi/2 -pi/2 pi/2 0];
% Posición de la mano del manipulador
p = T(1:3,4)-d(6)*T(1:3,3);
% Solución de la primera articulación: q1
R = sqrt(p(1)^2+p(2)^2);
sq1=p(2)/R;
cq1=p(1)/R;
q1 = atan2(sq1,cq1);
% Solución de la segunda articulación: q2
r = sqrt(R^2+(p(3)-d(1))^2);
salfa = (d(1)-p(3))/r;
calfa = R/r;
cbeta = (r^2+a(2)^2-d(4)^2)/(2*r*a(2));
sbeta = sqrt(1-cbeta^2);
if codo == -1 % Codo abajo
sq2 = salfa*cbeta+sbeta*calfa;
cq2 = calfa*cbeta-salfa*sbeta;
else % Codo arriba
sq2 = salfa*cbeta-sbeta*calfa;
cq2 = calfa*cbeta+salfa*sbeta;
end
q2 = atan2(sq2,cq2);
% Solución de la tercera articulación: q3
cbeta=(a(2)^2+d(4)^2-r^2)/(2*a(2)*d(4));
sbeta=sqrt(1-cbeta^2);
beta=atan2(sbeta,cbeta);
40. Prácticas de Robótica utilizando Matlab®
Práctica 2 .- Pág. 22
if codo == 1 % Codo arriba
q3 = 3*pi/2-beta;
else % Codo abajo
q3 = beta - pi/2;
end
% Solución de la cuarta articulación: q4
% Cálculo de la matriz de transformación A03
A01 = denavit(q1, d(1), a(1), alfa(1));
A12 = denavit(q2, d(2), a(2), alfa(2));
A23 = denavit(q3, d(3), a(3), alfa(3));
A03 = A01 * A12 * A23;
x3 = A03(1:3,1);
y3 = A03(1:3,2);
z3 = A03(1:3,3);
z4 = cross(z3,T(1:3,3)); % Vector orientación a: T(1:3,3)
% Determinación del indicador de orientación omega
aux = dot(T(1:3,2),z4); % Vector orientación s: T(1:3,2)
if aux ~= 0
omega = aux;
else
aux=dot(T(1:3,1),z4); % Vector orientación n: T(1:3,1)
if aux ~=0
omega=aux;
else
omega=0;
end
end
M = muneca*sign(omega);
sq4 = -M*dot(z4,x3);
cq4 = M*dot(z4,y3);
q4 = atan2(sq4,cq4);
% Solución de la quinta articulación: q5
z5 = T(1:3,3); % Vector de orientación a: T(1:3,3)
A34 = denavit(q4, d(4), a(4), alfa(4));
A04 = A03 * A34;
x4 = A04(1:3,1);
y4 = A04(1:3,2);
sq5 = dot(T(1:3,3),x4); % Vector de orientación a: T(1:3,3)
cq5 = -dot(T(1:3,3),y4); % Vector de orientación a: T(1:3,3)
q5 = atan2(sq5,cq5);
% Solución de la sexta articulación: q6
y6 = T(1:3,2); % Vector de orientación s: T(1:3,2)
A45 = denavit(q5, d(5), a(5), alfa(5));
A05 = A04 * A45;
y5 = A05(1:3,2);
sq6 = dot(T(1:3,1),y5); % Vector de orientación n: T(1:3,1)
cq6 = dot(T(1:3,2),y5); % Vector de orientación s: T(1:3,2)
q6 = atan2(sq6,cq6);
% Vector de coordenadas articulares
q = [q1 q2 q3 q4 q5 q6]';
41. Prácticas de Robótica utilizando Matlab®
Práctica 2 .- Pág. 23
En el ejemplo mostrado a continuación se puede comprobar como después de
asignar un vector de coordenadas articulares aleatorio, lo cual ya incluye una
determinada configuración de CODO y MUÑECA y obtener la matriz homogénea del
extremo de robot correspondiente a este vector, si sobre esta matriz se aplica la función
INVERSEKINEMATIC6 con los valores correctos de CODO y MUÑECA para el
vector de coordenadas articulares, se obtiene el vector q original.
⇒ Se recomienda experimentar la función INVERSEKINEMATIC6 con
vectores de coordenadas articulares sencillos de analizar.
» q=rand(6,1)
q =
0.6721
0.8381
0.0196
0.6813
0.3795
0.8318
» T=directkinematic6(q)
T =
-0.7400 -0.3846 0.5518 0.5756
0.6484 -0.1900 0.7372 0.4819
-0.1787 0.9033 0.3900 0.3387
0 0 0 1.0000
» inversekinematic6(T,-1,1)
ans =
0.6721
0.8381
0.0196
0.6813
0.3795
0.8318
2.4.- Representación gráfica en MatLab
usando alambres.
En este apartado se van a presentar una sencilla herramienta gráfica válida para
comprobar los resultados anteriores utilizando las funciones ya estudiadas. Se
recomienda al lector que realice estos ejemplos modificando el código de Matlab
. Para
comprobar los resultados se ha utilizado una nueva función nombrada como
DRAWROBOT3D, que permite una sencilla representación 3D utilizando líneas de la
configuración del manipulador. Ha de considerarse que esta función realiza el trazado
del robot conforme a su representación D-H, por lo que los sistemas de referencia que
han sido desplazados dan lugar a eslabones de longitud nula, dando la impresión óptica
de que han desaparecido. Para comprobar que el efecto de estas rotaciones si es tomado
en consideración se sugiere al lector que coloque un pequeño sistema de referencia en el
extremo del robot.
42. Prácticas de Robótica utilizando Matlab®
Práctica 2 .- Pág. 24
Ejemplo 2.7
Código en Matlab
. La función DRAWROBOT3D realiza una representación 3D de un
robot en función del vector de variables articulares Q
Particularizando DRAWROBOT3D para los ejemplos anteriores se obtiene las
funciones que se han llamado DRAWROBOT3D4 y DRAWROBOT3D6.
% DRAWROBOT3D4 Representación 3D de un robot.
% DRAWROBOT3D4(Q) realiza una representación 3D de un robot
% en función del vector de variables articulares Q.
%
% See also DENAVIT, DIRECTKINEMATIC4.
function drawrobot3d4(q)
% Parámetros Denavit-Hartenberg del robot
teta = [q(1) 0 0 q(4)];
d = [0.4 q(2) q(3) 0.2 ];
a = [0 -0.1 0 0 ];
alfa = [0 -pi/2 0 0 ];
% Matrices de transformación homogénea entre sistemas de coordenadas
consecutivos
A01 = denavit(teta(1), d(1), a(1), alfa(1));
A12 = denavit(teta(2), d(2), a(2), alfa(2));
A23 = denavit(teta(3), d(3), a(3), alfa(3));
A34 = denavit(teta(4), d(4), a(4), alfa(4));
% Matrices de transformación del primer sistema al correspondiente
A02 = A01 * A12;
A03 = A02 * A23;
A04 = A03 * A34;
% Vector de posicion (x, y, z) de cada sistema de coordenadas
x0 = 0; y0 = 0; z0 = 0;
x1 = A01(1,4); y1 = A01(2,4); z1 = A01(3,4);
xi = x1; yi = y1; zi = z1 + d(2);
x2 = A02(1,4); y2 = A02(2,4); z2 = A02(3,4);
x3 = A03(1,4); y3 = A03(2,4); z3 = A03(3,4);
x4 = A04(1,4); y4 = A04(2,4); z4 = A04(3,4);
% Se dibuja el robot
x = [x0 x1 xi x2 x3 x4];
y = [y0 y1 yi y2 y3 y4];
z = [z0 z1 zi z2 z3 z4];
plot3(x,y,z);
% Se coloca una rejilla a los ejes
grid;
% Se establecen los límites de los ejes
axis([-1.5 1.5 -1.5 1.5 0 1.5]);
Si se representa la configuración en la que todas las coordenadas articulares son
nulas, se obtiene el siguiente dibujo:
43. Prácticas de Robótica utilizando Matlab®
Práctica 2 .- Pág. 25
Ejemplo 2.8
Se presenta ahora el código y la representación gráfica del robot rotacional de 6
grados de libertad.
% DRAWROBOT3D6 Representación 3D de un robot.
% DRAWROBOT3D6(Q) realiza una representación 3D de un robot
% en función del vector de variables articulares Q.
%
% See also DENAVIT, DIRECTKINEMATIC6.
function drawrobot3d6(q)
% Parámetros Denavit-Hartenberg del robot
teta = q;
d = [0.315 0 0 0.5 0 0.08];
a = [0 0.45 0 0 0 0 ];
alfa = [-pi/2 0 pi/2 -pi/2 pi/2 0 ];
% Matrices de transformación homogénea entre sistemas de coordenadas
consecutivos
A01 = denavit(teta(1), d(1), a(1), alfa(1));
A12 = denavit(teta(2), d(2), a(2), alfa(2));
A23 = denavit(teta(3), d(3), a(3), alfa(3));
A34 = denavit(teta(4), d(4), a(4), alfa(4));
A45 = denavit(teta(5), d(5), a(5), alfa(5));
A56 = denavit(teta(6), d(6), a(6), alfa(6));
44. Prácticas de Robótica utilizando Matlab®
Práctica 2 .- Pág. 26
% Matrices de transformación del primer sistema al correspondiente
A02 = A01 * A12;
A03 = A02 * A23;
A04 = A03 * A34;
A05 = A04 * A45;
A06 = A05 * A56;
% Vector de posicion (x, y, z) de cada sistema de coordenadas
x0 = 0; y0 = 0; z0 = 0;
x1 = A01(1,4); y1 = A01(2,4); z1 = A01(3,4);
x2 = A02(1,4); y2 = A02(2,4); z2 = A02(3,4);
x3 = A03(1,4); y3 = A03(2,4); z3 = A03(3,4);
x4 = A04(1,4); y4 = A04(2,4); z4 = A04(3,4);
x5 = A05(1,4); y5 = A05(2,4); z5 = A05(3,4);
x6 = A06(1,4); y6 = A06(2,4); z6 = A06(3,4);
% Se dibuja el robot
x = [x0 x1 x2 x3 x4 x5 x6];
y = [y0 y1 y2 y3 y4 y5 y6];
z = [z0 z1 z2 z3 z4 z5 z6];
plot3(x,y,z);
% Se coloca una rejilla a los ejes
grid;
% Se establecen los límites de los ejes
axis([-1 1 -1 1 0 1.5]);
Si se representa la configuración en la que todas las coordenadas articulares son
nulas, se obtiene el siguiente dibujo:
45. Prácticas de Robótica utilizando Matlab®
Práctica 2 .- Pág. 27
2.5.- PRACTICA. Animación de los robots.
En este apartado se va a realizar un sencillo ejemplo de utilización de las funciones
estudiadas hasta ahora. Se trata de colocar al robot en dos posiciones distintas y animar
una trayectoria recta entre esas dos configuraciones. Para ello se deberán generar varias
posiciones intermedias.
Se utiliza la función planifica(p1,p2,ni) en la que se introducen las coordenadas
cartesianas de los puntos inicial y final y el número de puntos intermedios.
Ejemplo 2.9
En este ejemplo se utiliza la cinemática inversa del robot rotacional de 6 gdl para
trazar una línea recta entre un punto p1 inicial y un punto p2 final. El número de puntos
intermedios es variable.
Se ha utilizado la función PLANIFICA6(P1,P2,N,S,A,CODO,MUÑECA,NPUNTOS)
en el que se introduce las coordenadas cartesianas de los puntos inicial y final, la
orientación (n,s,a) del punto final, los parámetros CODO y MUÑECA para seleccionar
la configuración del robot y el número de puntos intermedio. Esta función proporciona
una matriz de (npuntos+2) columnas por 6 filas que se utilizará por la función
ANIMACION6(MAT_Q) para dibujar la trayectoria entre los dos puntos.
Código en Matlab
. La función PLANIFICA6(P1,P2,N,S,A,CODO,MUÑECA,
NPUNTOS) calcula la matriz de coordenadas articulares utilizada para graficar el
movimiento del robot.
% PLANIFICA6 Planificación de trayectorias
% MAT_Q = PLANIFICA6(P1, P2, N, S, A, CODO, MUNECA, NPUNTOS) realiza
% una planificación de trayectoria en línea recta desde la coordenada
% cartesiana P1 hasta la P2, de manera que la mano del manipulador
% posee la orientación expresada por [N S A]. CODO = 1 indica codo del
% robot arriba, es decir, que la articulación 3 se sitúa por encima de
% la articulación 2, mientras que CODO = -1 indica codo abajo, es decir
% que la articulación 2 se sitúa por encima de la 3. MUNECA = 1 indica que
% la muñeca del robot se sitúa por debajo de la coordenada cartesiana,
% mientras que MUNECA = -1 significa que la muñeca se sitúa por arriba.
% NPUNTOS indica el número de puntos en los que se divide la trayectoria.
% En MAT_Q se devuelve las coordenadas articulares, almacenadas por
% columnas, correspondientes a cada uno de los puntos cartesianos en los
% que se divide la trayectoria. MAT_Q es una matriz de NPUNTOS + 2 columnas
% y 6 filas.
%
% See also INVERSEKINEMATIC6.
function mat_q = planifica6(p1, p2, n, s, a, codo, muneca, npuntos)
% Cálculo del vector unitario
u = p2-p1;
mu = sqrt(u(1)^2+u(2)^2+u(3)^2);
u = (1/mu)*u;
% Cálculo de la distancia entre puntos
d = mu/(npuntos+1);
46. Prácticas de Robótica utilizando Matlab®
Práctica 2 .- Pág. 28
for i=0:(npuntos+1)
% Cálculo de la posición cartesiana actual de la mano del
manipulador
p = p1+(i*d)*u;
T = [n s a p];
% Cálculo de las coordenadas articulares
q = inversekinematic6(T,codo,muneca);
mat_q(:,i+1) = q;
end
Utilizando ahora la función ANIMACION6(MAT_Q) se presenta el movimiento del
robot entre los 2 puntos especificados.
% ANIMACION6 Animación de la trayectoria de un robot
% ANIMACION(MAT_Q) realiza la animación de la trayectoria, expresada
% en la matriz MAT_Q, de un brazo robot de 6 GDL. MAT_Q contiene 6 filas
% y una columna para cada disposición del robot.
%
% See also PLANIFICA6, DRAWROBOT3D6.
function animacion6(mat_q)
% Parámetros Denavit-Hartenberg del robot
d = [0.315 0 0 0.5 0 0.08];
a = [0 0.45 0 0 0 0 ];
alfa = [-pi/2 0 pi/2 -pi/2 pi/2 0 ];
% Vector de posicion (x, y, z) del sistema de coordenadas de
referencia
x0 = 0; y0 = 0; z0 = 0;
% Se dibuja el sistema de coordenadas de referencia. Se asigna el modo
XOR para borrar
% sólo el robot dibujado anteriormente. Se utiliza un grosor de línea
de 2 unidades
p = plot3(x0,y0,z0,'EraseMode','xor','LineWidth',2);
% Se asigna una rejilla a los ejes
grid;
% Se establecen los límites de los ejes
axis([-1 1 -1 1 0 1.5]);
% Mantiene el gráfico actual
hold on;
% Número de columnas de la matriz
n = size(mat_q,2);
% Se dibuja la disposición del robot correspondiente a cada columna
for i=1:n
% Variables articulares del brazo robot
teta1 = mat_q(1,i);
teta2 = mat_q(2,i);
teta3 = mat_q(3,i);
teta4 = mat_q(4,i);
teta5 = mat_q(5,i);
teta6 = mat_q(6,i);
47. Prácticas de Robótica utilizando Matlab®
Práctica 2 .- Pág. 29
% Matrices de transformación homogénea entre sistemas de coordenadas
consecutivos
A01 = denavit(teta1, d(1), a(1), alfa(1));
A12 = denavit(teta2, d(2), a(2), alfa(2));
A23 = denavit(teta3, d(3), a(3), alfa(3));
A34 = denavit(teta4, d(4), a(4), alfa(4));
A45 = denavit(teta5, d(5), a(5), alfa(5));
A56 = denavit(teta6, d(6), a(6), alfa(6));
% Matrices de transformación del primer sistema al correspondiente
A02 = A01 * A12;
A03 = A02 * A23;
A04 = A03 * A34;
A05 = A04 * A45;
A06 = A05 * A56;
% Vector de posicion (x, y, z) de cada sistema de coordenadas
x1 = A01(1,4); y1 = A01(2,4); z1 = A01(3,4);
x2 = A02(1,4); y2 = A02(2,4); z2 = A02(3,4);
x3 = A03(1,4); y3 = A03(2,4); z3 = A03(3,4);
x4 = A04(1,4); y4 = A04(2,4); z4 = A04(3,4);
x5 = A05(1,4); y5 = A05(2,4); z5 = A05(3,4);
x6 = A06(1,4); y6 = A06(2,4); z6 = A06(3,4);
% Se dibuja el robot
x = [x0 x1 x2 x3 x4 x5 x6];
y = [y0 y1 y2 y3 y4 y5 y6];
z = [z0 z1 z2 z3 z4 z5 z6];
set(p,'XData',x,'YData',y,'ZData',z);
% Se fuerza a MATLAB a actualizar la pantalla
drawnow;
end
⇒ Notar que la función plot3 ha permitido dibujar sobre la animación la trayectoria
seguida por el extremo del robot.
48. Prácticas de Robótica utilizando Matlab®
Práctica 2 .- Pág. 30
EJERCICIOS PROPUESTOS
Se pide implementar las funciones PLANIFICA4 y ANIMACIÓN4 para el ejemplo
con el robot prismático de 4 gdl y realizar una animación entre dos puntos del espacio
de trabajo del robot.
49. Prácticas de Robótica utilizando Matlab®
Práctica 3 .- Pág. 1
Práctica 3
Dinámica de Robots
3.1.-Introducción
La dinámica del robot relaciona el movimiento del robot y las fuerzas implicadas
en el mismo. El modelo dinámico establece relaciones matemáticas entre las
coordenadas articulares (o las coordenadas del extremo del robot), sus derivadas
(velocidad y aceleración), las fuerzas y pares aplicados en las articulaciones (o en el
extremo) y los parámetros del robot (masas de los eslabones, inercias, etc).
Siguiendo con la filosofía de este libro, se recomienda al lector que quiera
profundizar sobre la dinámica de robots, la lectura de los textos [1],[2],[3], donde se
estudian varias formulaciones clásicas como Lagrange-Euler o las ecuaciones
generalizadas de D’Alembert. Hay que tener en cuenta que las ecuaciones de
movimiento obtenidas con estas formulaciones son equivalentes en el sentido que
describen la conducta dinámica del robot, sin embargo, cada una de ellas presenta
características diferentes que la hacen más apropiada para ciertas tareas. Por ejemplo, la
formulación de Lagrange-Euler presenta un modelo simple y elegante, dando como
resultado una serie de ecuaciones diferenciales no lineales de 2º orden acopladas útiles
para el estudio de estrategias de control en el espacio de estados de las variables
articulares del robot, pero que se presentan ineficaces para aplicaciones en tiempo real
dado el elevado tiempo de computación que requieren las operaciones con matrices de
transformación homogénea.
Los modelos dinámicos que se estudian en esta práctica están basados en el
algoritmo recursivo de Newton-Euler (N-E) desarrollado por Luh [1]. Aunque las
formulaciones recursivas destruyen la estructura del modelo dinámico analítico y dan
lugar a la falta de ecuaciones cerradas necesarias para el análisis del control, la
dificultad de un análisis clásico es enorme debido a que se obtienen expresiones
fuertemente no-lineales que constan de cargas inerciales, fuerzas de acoplo entre las
articulaciones y efectos de las cargas de gravedad, con la dificultad añadida de que los
pares/fuerzas dinámicos dependen de los parámetros físicos del manipulador, de la
configuración instantánea de las articulaciones, de la velocidad, de la aceleración y de la
50. Prácticas de Robótica utilizando Matlab®
Práctica 3 .- Pág. 2
carga que soporta el robot. Aunque las ecuaciones del movimiento son equivalentes ya
sean analíticas o recursivas, los diferentes planteamientos dependen de los objetivos que
se quieran conseguir con ellos. En algunos casos es necesario solucionar el problema
dinámico de un robot para lograr tiempos de calculo rápidos en la evaluación de los
pares y fuerzas articulares para controlar el manipulador, y en otros casos son necesarios
planteamientos para facilitar el análisis y la síntesis del control.
3.2.-Dinámica inversa. La formulación de Newton-Euler
El método de Newton-Euler permite obtener un conjunto de ecuaciones
recursivas hacia delante de velocidad y aceleración lineal y angular las cuales están
referidas a cada sistema de referencia articular. Las velocidades y aceleraciones de cada
elemento se propagan hacia adelante desde el sistema de referencia de la base hasta el
efector final. Las ecuaciones recursivas hacia atrás calculan los pares y fuerzas
necesarios para cada articulación desde la mano (incluyendo en ella efectos de fuerzas
externas), hasta el sistema de referencia de la base.
3.2.1. Sistemas de coordenadas en movimiento.
La formulación de N-E se basa en los sistemas de coordenadas en movimiento
[1].
Figura 3.1. Sistemas de coordenadas en movimiento
Con respecto a la figura 3.1 se tiene que el sistema de coordenadas 0*
se desplaza y gira
en el espacio respecto del sistema de referencia de la base 0, el vector que describe el
origen del sistema en movimiento es h y el punto P se describe respecto del sistema 0*
a
través del vector r*
, de acuerdo a esto, la descripción del punto P respecto del sistema
de la base es:
hrr += *
(3.1)
hvv
dt
dh
dt
dr
dt
dr
+=+= *
*
(3.2)
donde ν*
es la velocidad del punto P respecto del origen del sistema 0*
en movimiento y
νh es la velocidad del origen del sistema 0*
respecto de la base.
y0
h
r*
0*
y*
x*
z*
0
r
x0
P
z0
51. Prácticas de Robótica utilizando Matlab®
Práctica 3 .- Pág. 3
Si el punto P se desplaza y gira respecto del sistema 0*
la ecuación (3.2) debe escribirse
como:
dt
dh
rw
dt
rd
dt
dh
dt
dr
v +⎟⎟
⎠
⎞
⎜⎜
⎝
⎛
×+=+= *
***
(3.3)
donde
dt
rd **
es la velocidad lineal del punto P respecto del origen 0*
y *
rw× es la
velocidad angular del punto P respecto del origen 0*
.[1]
De manera similar la aceleración general del sistema de puede describir como:
haa
dt
hd
dt
rd
dt
dv
a +=+== *
2
2
2
*2
(3.4)
( ) 2
2
*
**
2
*2*
2
dt
hd
r
dt
dw
rww
dt
rd
w
dt
rd
a +×+××+×+= (3.5)
3.2.2. Cinemática de los eslabones del Robot.
A partir de las ecuaciones (3.1) a (3.5) de la sección anterior se desarrolla a
continuación el planteamiento general para la cinemática de los eslabones del robot [1]
Figura 3.2. Relaciones vectoriales entre los sistemas de referencia 0,0*
y 0’
De acuerdo a la figura 3.2 las ecuaciones cinemáticas para los eslabones de un robot, se
pueden escribir como:
1
*
1
**
−− +×+= iii
i
i vpw
dt
pd
v (3.6)
*
1 iii www += −
0 y0
z0
x0
zi-1
zi-2
zi
xi
yi
pi
pi-1
0*
yi-1
xi-1
ai-1
ai
0’
p*
i
αi
θi
articulación
i-1
articulación i
articulación
i+1
52. Prácticas de Robótica utilizando Matlab®
Práctica 3 .- Pág. 4
Debe notarse que la velocidad angular del sistema de referencia iw es igual a la suma de
la velocidad angular absoluta del sistema i-1 más la velocidad angular relativa *
iw del
eslabón referida a su propio sistema de coordenadas.
La aceleración lineal del sistema de coordenadas de la articulación i es:
( ) 1
*
11
**
1
*
12
*2*
2 −−−−− +××+×+×+= iiii
i
iii
i
i vpww
dt
pd
wpw
dt
pd
v (3.7)
*
1 iii www += − (3.8)
La aceleración angular del sistema de referencia i (xi, yi, zi) respecto del sistema
(xi-1, yi-1, zi-1) se consigue de manera similar a la ecuación (3.3)
*
1
**
*
ii
i
i ww
dt
wd
w ×+= −
(3.9)
por lo que la ecuación (3.8) queda como:
*
1
**
1 ii
i
ii ww
dt
wd
ww ×++= −−
(3.10)
En general para un robot los sistemas de coordenadas (xi-1, yi-1, zi-1) y (xi, yi, zi) están
unidos a los eslabones i-1 e i. La velocidad del eslabón i respecto del sistema de
coordenadas i-1 es iq . Si el eslabón es prismático, la velocidad será una velocidad de
traslación relativa respecto del sistema (xi-1, yi-1, zi-1) y si es rotacional le corresponderá
una velocidad rotacional relativa del eslabón i respecto del sistema (xi-1, yi-1, zi-1), por
lo tanto:
ii qz 1− si el eslabón i es rotacional
*
iw = (3.11)
0 si el eslabón i es traslacional
donde iq es la magnitud de la velocidad angular del eslabón i con respecto al sistema de
coordenadas (xi-1, yi-1, zi-1). De manera similar:
ii qz 1− si el eslabón i es rotacional
dt
wd **
= (3.12)
0 si el eslabón i es traslacional
Debe notarse que el vector 1−iz es igual a (0, 0, 1)T
.
53. Prácticas de Robótica utilizando Matlab®
Práctica 3 .- Pág. 5
Las velocidades y aceleraciones de los sistemas de coordenadas ligados a cada eslabón
son absolutas y se calculan como:
iii qzw 11 −− + si el eslabón i es rotacional
iw = (3.13)
1−iw si el eslabón i es traslacional
( )iiiiii qzwqzw 1111 −−−− ×++ si el eslabón i es rotacional
iw = (3.14)
1−iw si el eslabón i es traslacional
Las velocidades lineales de los sistemas de referencia de cada eslabón se calculan como:
*
ii pw × si el eslabón i es rotacional
dt
pd i
*
= (3.15)
ii qz 1− si el eslabón i es traslacional
( )****
**
iiii
i
pwwp
dt
wd
××+× si el eslabón i es rotacional
2
*2*
dt
pd i
= (3.16)
ii qz 1− si el eslabón i es traslacional
por lo que la velocidad lineal absoluta del sistema de coordenadas ligado a cada eslabón
se calcula como:
1
*
−+× iii vpw si el eslabón i es rotacional
iv = (3.17)
1
*
1 −− +×+ iiiii vpwqz si el eslabón i es traslacional
La aceleración se calcula como:
( ) 1
**
−+××+× iiiiii vpwwpw si el eslabón i es rotacional
iv = (3.18)
( ) ( ) 1
*
1
*
1 2 −−− +××+×+×+ iiiiiiiiiii vpwwqzwpwqz si el eslabón i es traslacional
54. Prácticas de Robótica utilizando Matlab®
Práctica 3 .- Pág. 6
3.2.3. Ecuaciones de movimiento recursivas.
A partir de las ecuaciones cinemáticas del apartado anterior y aplicando el principio de
D’Alembert del equilibrio estático para todos los instantes de tiempo, se obtienen las
ecuaciones recursivas de Newton-Euler.[1]
Si se utiliza la nomenclatura de la figura 3.2 sobre un eslabón cualquiera del robot, tal y
como se muestra en la figura 3.3
Figura 3.3.Fuerzas y momentos sobre el elemento i
Donde:
im masa total del eslabón i,
ir posición del centro de masa del elemento i desde el origen del
sistema de referencia de la base,
is posición del centro de masa del elemento i desde el origen del
sistema de coordenadas (xi, yi, zi),
*
ip posición del origen de coordenadas i-ésimo con respecto al
sistema de coordenadas (i-1)-ésimo,
dt
rd
v i
i = velocidad lineal del centro de masa del elemento i,
dt
vd
a i
i = aceleración lineal del centro de masa del elemento i,
iF fuerza total externa ejercida sobre el elemento i en el centro de
masa,
iN momento total externo ejercido sobre el elemento i en el centro de
masa,
iI matriz de inercia del elemento i respecto de su centro de masa con
respecto al sistema de coordenadas (x0, y0, z0),
Centro de masa del elemento i
O’
is
ir
1
*
−−=+ iiii prsp
*
ip
O*
ni+1
fi+1
y0
x0
z0
ni
fi
pi-1
pi
(xi, yi, zi)(xi-1, yi-1, zi-1)
Elemento i+1
Elemento i-1
55. Prácticas de Robótica utilizando Matlab®
Práctica 3 .- Pág. 7
if fuerza ejercida sobre el elemento i por el elemento (i-1) en el
sistema de coordenadas (xi-1, yi-1, zi-1) para soportar al elemento i
y a los elementos por encima de él,
in momento ejercido sobre el elemento i por el elemento (i-1) en el
sistema de coordenadas (xi-1, yi-1, zi-1),
⇒ NOTA: Es importante que se identifiquen estas variables sobre el dibujo del robot,
para poder seguir los siguientes desarrollos.
Si se omiten los efectos del rozamiento viscoso en las articulaciones, y se aplica el
principio de D’Alembert, se obtiene para cada eslabón:
ii
ii
i am
dt
vmd
F ==
)(
(3.18)
)(
)(
iiiii
ii
i wIwwI
dt
wId
N ×+== (3.19)
realizando el balance de pares y fuerzas en la figura 3.3:
1+−= iii ffF (3.20)
111 )()( +−+ ×−−×−+−= iiiiiiiii frpfrpnnN (3.21)
1
*
11 )( +−+ ×−×−+−= iiiiiii fpFrpnn (3.22)
que utilizando la relación geométrica:
iiii sppr +=− −
*
1 (3.23)
se obtienen las ecuaciones recursivas:
11 ++ +=+= iiiiii famfFf (3.24)
iiiiiiii NFspfpnn +×++×+= ++ )( *
1
*
1 (3.25)
Se observa que estas ecuaciones son recursivas y permiten obtener las fuerzas y
momentos en los elementos i =1,2,...,n para un robot de n elementos. 1+if y 1+in
representan la fuerza y momento ejercidos por la mano del robot sobre un objeto
externo.
Por lo tanto, el par/fuerza para cada articulación se expresa como:
iii
T
qbzni
+−1
si el eslabón i es rotacional
iτ = (3.26)
iii
T
i qbzf +−1 si el eslabón i es traslacional
donde bi es el coeficiente de rozamiento viscoso de la articulación.
56. Prácticas de Robótica utilizando Matlab®
Práctica 3 .- Pág. 8
3.2.4. Algoritmo computacional.
En resumen de los dos apartados anteriores, las ecuaciones de N-E consisten en un
conjunto de ecuaciones recursivas [(3.13),(3.14), (3.17), (3.18)] hacia delante y
[(3.18),(3.19), (3.20), (3.21),(3.26)] hacia atrás.
Para obtener un algoritmo computacional, se debe tener en cuenta que en las anteriores
ecuaciones, las matrices de inercia iI y los parámetros del robot, ir , is , *
ip están
referenciados respecto del sistema de coordenadas de la base.
Luh y col. [1980] utilizaron las matrices de rotación 3x3 i
i
R1−
, que ya han sido
estudiadas en la práctica 2 pues corresponden a la submatriz superior izquierda de las
matrices de transformación homogénea i
i
A1−
, para expresar las variables
iw , iw , iv , iv , ia ,
*
ip , is , iF , iN , if , in y iτ que se refieren al sistema de coordenadas de
la base como i
i
wR0 , i
i
wR 0 , i
i
vR0 , i
i
vR 0 , i
i
aR0 ,
*
0 i
i
pR , i
i
sR0 , i
i
FR0 , i
i
NR0 , i
i
fR0 , i
i
nR0 y
i
i
R τ0 , que están referenciados al propio sistema de coordenadas del elemento i.
De esta manera, las ecuaciones recursivas de N-E quedan expresadas en la siguiente
tabla:
57. Prácticas de Robótica utilizando Matlab®
Práctica 3 .- Pág. 9
Ecuaciones hacia delante: i = 1,2,....,n
)( 010
1
1 ii
i
i
i
qzwRR +−
−
− si el eslabón i es rotacional
i
i
wR0 = (3.27)
)( 10
1
1 −
−
− i
i
i
i
wRR si el eslabón i es traslacional
( ) ]ii
i
ii
i
i
i
qzwRqzwRR 010
1
010
1
1 )([ ×++ −
−
−
−
− si el eslabón i es rotacional
i
i
wR 0 = (3.28)
)( 10
1
1 −
−
− i
i
i
i
wRR si el eslabón i es traslacional
[ ]
( )10
1
1
*
000
*
00 )()()()()(
−
−
−+
+××+×
i
i
i
i
i
i
i
i
i
i
i
i
i
i
vRR
pRwRwRpRwR
si el eslabón i es rotacional
i
i
vR 0 = (3.29)
[ ])()()(
)()(2
)()()(
*
000
010
*
0010
1
01
i
i
i
i
i
i
ii
i
i
i
i
i
i
i
i
i
ii
i
pRwRwR
qzRwR
pRwRvRqzR
××+
+×+
+×++
−
−
−
−
si el eslabón i es traslacional
( ) ( ) ( ) ( ) ( )[ ] i
i
i
i
i
i
i
i
i
i
i
i
i
i
vRsRwRwRsRwRaR 0000000 +××+×= (3.30)
Ecuaciones hacia atrás: i =n,n-1,....,1
i
i
ii
i
i
i
i
i
aRmfRRfR 010
1
10 )( += +
+
+ (3.31)
]
( )( ) ( ) ( )( )[ ]i
i
ii
i
i
i
i
i
ii
i
i
i
i
i
i
i
i
i
i
i
i
i
i
i
i
i
wRRIRwRwRRIR
FRsRpRfRpRnRRnR
0
0
000
0
0
00
*
010
1*
0
1
10
1
10 )()()()([
×++
+×++×+= +
++
+
+
+
(3.32)
iii
iTi
qbzRnR i
+− )()( 010
si el eslabón i es rotacional
i
i
R τ0 = (3.33)
iii
iT
i
i
qbzRfR +− )()( 010 si el eslabón i es traslacional
Tabla 3.1. Algoritmo recursivo de N-E
58. Prácticas de Robótica utilizando Matlab®
Práctica 3 .- Pág. 10
⇒ NOTA: Para la utilización de este algoritmo en la solución del problema dinámico
inverso de un manipulador de cadena abierta se tienen que tener en cuenta las siguientes
consideraciones:
n = número de elementos de la cadena.
0w = 0w = 0v = 0, (la base del robot se considera en reposo)
( )T
zyx ggggv ,,0 == con 8.9=g m/s2
( )T
z 1,0,00 = lo que implica ( )T
ggv ,0,00 == con 8.9=g
Ejemplo 3.1
A continuación se presenta la implementación del algoritmo de N-E para el robot
prismático de 4 gdl.
Cada una de las ecuaciones (3.27) a (3.33) han sido
implementadas por los autores como una función de
MatLab
, y se proporcionarán para la realización de la
práctica.
La notación que se ha utilizado en las funciones
necesarias para poder ejecutar el algoritmo se
corresponde con la siguiente tabla:
Dh Calcula la matriz de rotación i
i
R1−
Ri0pi *
0 i
i
pR
Ri0si i
i
sR0
Ri0wi i
i
wR0
Ri0wpi i
i
wR 0
Ri0vpi i
i
vR 0
Ri0ai i
i
aR0
Ri0fi i
i
FR0
Ri0ni i
i
NR0
Ri0fia i
i
fR0
Ri0nia i
i
nR0
T_r i
i
R τ0 para una articulación rotacional
T_p i
i
R τ0 para una articulación prismática
59. Prácticas de Robótica utilizando Matlab®
Práctica 3 .- Pág. 11
⇒ NOTA: Se recomienda que desde el entorno MatLab
, se consulte la ayuda de todas
las funciones anteriores, fijándose en los parámetros que se le pasan a la función en cada
caso.
» help ri0pi
RI0PI Vector ri0pi.
Y = RI0PI(A, D, ALFA) calcula el vector 3x1 que representa la
localización de (xi,yi,zi) desde el origen de (xi-1,yi-1,zi-1)
con respecto al sistema de coordenadas i-ésimo utilizando los
parámetros de Denavit-Hartenberg A, D y ALFA.
»
Código en MatLab
. A continuación se presenta la función NEWTONEULER4,
utilizada para resolver la dinámica inversa del robot prismático de 4 gdl. Realizando
help NEWTONEULER4, la función nos informa sobre los parámetros necesarios para
realizar los cálculos.
» help newtoneuler4
NEWTONEULER4 Dinámica inversa utilizando el método de Newton-Euler.
TAU = NEWTONEULER4(Q, QP, QPP, G, M5, IEXTER) calcula el vector
4x1 de pares/fuerzas de entrada a las articulaciones. Q el
vector 4x1 de coordenadas articulares. QP es el vector 4x1 que
representa la velocidad de cada articulación. QPP es el vector
4x1 que indicala aceleración de cada articulación. G es el valor
de la gravedad (m/s^2).
M5 es la masa de la carga externa(Kg) que transporta el brazo
robot.
IEXTER es la matriz 3x3 de inercia de la carga exterior(Kg-m^2).
See also DH, RI0PI, RI0SI, RI0WI, RI0WIP, RI0VPI_R, RI0AI,
RI0FI, RI0NI,RI0FIA, RI0NIA, T_R, F_P.
»
Es importante que el lector repare en los parámetros que se pasan en cada caso a la
función correspondiente.
En este ejemplo, para contabilizar los efectos de la carga exterior, se ha introducido
como un eslabón nº 5. Para ello se ha necesitado definir una articulación entre el
eslabón 4 y el 5, que puede definirse utilizando las librerías de enlace rotacional o
prismático, dado que como condiciones de este eslabón imponen 055 == qq , de manera
que las ecuaciones (3.27),(3.28) y (3.29) coinciden para ambas librerías.
60. Prácticas de Robótica utilizando Matlab®
Práctica 3 .- Pág. 12
% NEWTONEULER4 Dinámica inversa utilizando el método de Newton-
Euler.
% TAU = NEWTONEULER4(Q, QP, QPP, G, M5, IEXTER) calcula el vector
% 4x1 de pares/fuerzas de entrada a las articulaciones. Q el vector
% 4x1 de coordenadas articulares. QP es el vector 4x1 que representa
% la velocidad de cada articulación. QPP es el vector 4x1 que indica
% la aceleración de cada articulación. G es el valor de la gravedad
(m/s^2).
% M5 es la masa de la carga externa(Kg) que transporta el brazo robot.
% IEXTER es la matriz 3x3 de inercia de la carga exterior(Kg-m^2).
%
% See also DH, RI0PI, RI0SI, RI0WI, RI0WIP, RI0VPI_R, RI0AI, RI0FI, RI0NI,
% RI0FIA, RI0NIA, T_R, F_P.
function tau = newtoneuler4(q,qp,qpp,g,m5,Iexter);
% ------------------------------------------------------------
% Parámetros Denavit-Hartenberg del robot
% ------------------------------------------------------------
teta = [q(1) 0 0 q(4)];
d = [0.4 q(2) q(3) 0.2 ];
a = [0 -0.1 0 0 ];
alfa = [0 -pi/2 0 0 ];
% ------------------------------------------------------------
% Factores de posicionamiento de los centros de gravedad
% ------------------------------------------------------------
factor1 = -0.5; factor2 = -0.5; factor3 = -0.5; factor4 = -0.5;
% ------------------------------------------------------------
% Masa de cada elemento (Kg)
% ------------------------------------------------------------
m1 = 10; m2 = 5; m3 = 5; m4 = 3;
% ------------------------------------------------------------
% Coeficiente de rozamiento viscoso de cada articulacion
% ------------------------------------------------------------
b1 = 0.05; b2 = 0.05; b3 = 0.05; b4 = 0.05;
% ------------------------------------------------------------
% Matrices de Inercia (Kg-m^2)
% ------------------------------------------------------------
r10I_r01 = [0.0191 0 0;0 0.0191 0;0 0 0.0068];
r20I_r02 = [0.0031 0 0;0 0.0484 0;0 0 0.0484];
r30I_r03 = [0.0606 0 0;0 0.0053 0;0 0 0.0606];
r40I_r04 = [0.0022 0 0;0 0.0022 0;0 0 0.0014];
% ------------------------------------------------------------
% Vectores ri0pi, ri0si.
% ------------------------------------------------------------
r10p1 = ri0pi(a(1), d(1), alfa(1));
r20p2 = ri0pi(a(2), d(2), alfa(2));
r30p3 = ri0pi(a(3), d(3), alfa(3));
r40p4 = ri0pi(a(4), d(4), alfa(4));
r50p5 = zeros(3,1);
r10s1 = ri0si(a(1), d(1), alfa(1), factor1);
r20s2 = ri0si(a(2), d(2), alfa(2), factor2);
r30s3 = ri0si(a(3), d(3), alfa(3), factor3);
r40s4 = ri0si(a(4), d(4), alfa(4), factor4);
r50s5 = zeros(3,1);
62. Prácticas de Robótica utilizando Matlab®
Práctica 3 .- Pág. 14
r40n4 = ri0ni(r40wp4, r40w4, r40I_r04);
r30n3 = ri0ni(r30wp3, r30w3, r30I_r03);
r20n2 = ri0ni(r20wp2, r20w2, r20I_r02);
r10n1 = ri0ni(r10wp1, r10w1, r10I_r01);
% ------------------------------------------------------------
% Fuerzas articulares
% ------------------------------------------------------------
r50f5a = r50f5;
r40f4a = ri0fia(r45, r50f5a, r40f4);
r30f3a = ri0fia(r34, r40f4a, r30f3);
r20f2a = ri0fia(r23, r30f3a, r20f2);
r10f1a = ri0fia(r12, r20f2a, r10f1);
% ------------------------------------------------------------
% Pares articulares
% ------------------------------------------------------------
r20p1 = r21*(r10p1); r30p2 = r32*(r20p2);
r40p3 = r43*(r30p3); r50p4 = r54*(r40p4);
r50n5a = r50n5;
r40n4a = ri0nia(r45, r50n5a, r50f5a, r40n4, r40f4, r50p4, r40p4,
r40s4);
r30n3a = ri0nia(r34, r40n4a, r40f4a, r30n3, r30f3, r40p3, r30p3,
r30s3);
r20n2a = ri0nia(r23, r30n3a, r30f3a, r20n2, r20f2, r30p2, r20p2,
r20s2);
r10n1a = ri0nia(r12, r20n2a, r20f2a, r10n1, r10f1, r20p1, r10p1,
r10s1);
% ------------------------------------------------------------
% Fuerzas y pares de accionamientos
% ------------------------------------------------------------
t_1 = t_r(r10, r10n1a, qp(1), b1);
t_2 = f_p(r21, r20f2a, qp(2), b2);
t_3 = f_p(r32, r30f3a, qp(3), b3);
t_4 = t_r(r43, r40n4a, qp(4), b4);
tau = [t_1; t_2; t_3; t_4];
⇒ NOTA: Debe notarse que los parámetros físicos y geométricos del robot se han
introducido en el código. Se recuerda que los parámetros de D-H son característicos de
cada robot. El factor de posición del centro de masas de cada eslabón, su masa y su
inercia son datos del robot, así como los coeficientes de rozamiento viscoso aplicables
en cada articulación.
Se recomienda que compruebe la función para velocidades y aceleraciones nulas,
observando los resultados que se obtienen. En el ejemplo siguiente, se introducen
velocidades y aceleraciones nulas en todas las articulaciones. Se puede comprobar como
la fuerza en la articulación 2 (156 N) corresponde con el peso de los eslabones 2,3,4 y la
masa exterior que debe ser soportado por la articulación.
63. Prácticas de Robótica utilizando Matlab®
Práctica 3 .- Pág. 15
» q=[0 0.5 0.4 0];
» qp=[0 0 0 0];
» qpp=qp;
» m4=3;
» iext=0.05*eye(3);
» tau=newtoneuler4(q,qp,qpp,9.8,m4,iext)
tau =
0.0000
156.8000
0.0000
0
»
Ejemplo 3.2
Se muestra a continuación la implementación del algoritmo de N-E para el robot
rotacional de 6 gdl.
Se observa que a diferencia del ejemplo
anterior, en este caso solamente se utilizan las
librerías de las funciones de enlaces
rotacionales.
Código en MatLab
. A continuación se presenta la función NEWTONEULER6,
utilizada para resolver la dinámica inversa del robot de 6 gdl. Realizando help
NEWTONEULER6, la función nos informa sobre los parámetros necesarios para
realizar los cálculos.
% NEWTONEULER6 Dinámica inversa utilizando el método de Newton-Euler.
% TAU = NEWTONEULER6(Q, QP, QPP, G, M7, IEXTER) calcula el vector
% 6x1 de pares/fuerzas de entrada a las articulaciones. Q el vector
% 6x1 de coordenadas articulares. QP es el vector 6x1 que representa
% la velocidad de cada articulación. QPP es el vector 6x1 que indica
% la aceleración de cada articulación. G es el valor de la gravedad (m/s^2).
% M7 es la masa de la carga externa(Kg) que transporta el brazo robot.
% IEXTER es la matriz 3x3 de inercia de la carga exterior(Kg-m^2).
%
% See also DH, RI0PI, RI0SI, RI0WI, RI0WIP, RI0VPI_R, RI0AI, RI0FI, RI0NI,
% RI0FIA, RI0NIA, T_R.
function tau = newtoneuler6(q,qp,qpp,g,m7,Iexter);
% ------------------------------------------------------------
% Parámetros Denavit-Hartenberg del robot
% ------------------------------------------------------------