Practicas de robotica utilizando matlab - Roque

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.

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.

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

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.

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

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.

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).

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

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

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í.

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.

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

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.

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

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

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

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

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.

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.

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.

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)

• 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

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];

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

Ejemplo 2.2
Figura-2.4 Parámetros de D-H para un robot rotacional
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

. 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));
% Matriz de transformación del primer al último sistema de
coordenadas
A04 = A01 * A12 * A23 * A34;

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.

% 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)
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];
consecutivos
% 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 =
1.0000 0 0 0.4500
0 1.0000 0 0
0 0 1.0000 0.8950
0 0 0 1.0000

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.

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

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=θ
. 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;
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.

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));
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.

» q=rand(4,1)
q =
0.8913
0.7621
0.4565
0.0185
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

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θ
MUÑECA
px, py, pzCODO
a
s
n
y0
x0
z0
R
c
px,py
θ1

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

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

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 •=θ
Articulación 5
De la figura 2.4 se obtiene:
45 xasin •=θ
)(cos 45 ya •−=θ
Articulación 6
56 ynsin •=θ
56cos ys •=θ
x3
x4
y3
z4
θ4
θ4
y4
x4
n
a
θ5
x5
n
y5
s
θ6
θ6

.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)
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
% 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);

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
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);
% Solución de la quinta articulación: q5
z5 = T(1:3,3); % Vector de orientación a: T(1:3,3)
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)
% Solución de la sexta articulación: q6
y6 = T(1:3,2); % Vector de orientación s: T(1:3,2)
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)
% Vector de coordenadas articulares
q = [q1 q2 q3 q4 q5 q6]';

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 =
-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.

Ejemplo 2.7
. 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)
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 ];
consecutivos
% 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:

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)
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 ];
consecutivos

A02 = A01 * A12;
A03 = A02 * A23;
A04 = A03 * A34;
A05 = A04 * A45;
A06 = A05 * A56;
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);
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;
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:

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.
. 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);

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)
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;
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);

consecutivos
A01 = denavit(teta1, d(1), a(1), alfa(1));
A02 = A01 * A12;
A03 = A02 * A23;
A04 = A03 * A34;
A05 = A04 * A45;
A06 = A05 * A56;
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);
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.

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.

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

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

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

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
.

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

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

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.

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:

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


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
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

⇒ 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

⇒ 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.

% 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);
% ------------------------------------------------------------
% ------------------------------------------------------------
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);
r50s5 = zeros(3,1);

% ------------------------------------------------------------
% Matrices de transformacion
% ------------------------------------------------------------
r01 = dh(teta(1), alfa(1)); r10 = r01';
r12 = dh(teta(2), alfa(2)); r21 = r12';
r23 = dh(teta(3), alfa(3)); r32 = r23';
r34 = dh(teta(4), alfa(4)); r43 = r34';
r45 = eye(3); r54 = r45';
% ------------------------------------------------------------
% Velocidad angular de las articulaciones
% ------------------------------------------------------------
r00w0 = zeros(3,1);
r10w1 = ri0wi(r10, r00w0, qp(1));
r20w2 = r21*r10w1;
r30w3 = r32*r20w2;
r40w4 = ri0wi(r43, r30w3, qp(4));
r50w5 = ri0wi(r54, r40w4, 0);
% ------------------------------------------------------------
% Aceleracion angular de las articulaciones
% ------------------------------------------------------------
r00wp0 = zeros(3,1);
r10wp1 = ri0wpi(r10, r00wp0, r00w0, qp(1), qpp(1));
r20wp2 = r21*r10wp1;
r30wp3 = r32*r20wp2;
r50wp5 = ri0wpi(r54, r40wp4, r40w4, 0, 0);
% ------------------------------------------------------------
% Aceleracion lineal articular
% ------------------------------------------------------------
r00vp0 = [0; 0; g];
r10vp1 = ri0vpi_r(r10, r00vp0, r10wp1, r10w1, r10p1);
r20vp2 = ri0vpi_p(r21, r10vp1, r20wp2, r20w2, r20p2, qp(2), qpp(2));
r30vp3 = ri0vpi_p(r32, r20vp2, r30wp3, r30w3, r30p3, qp(3), qpp(3));
% ------------------------------------------------------------
% Aceleracion del centro de masa de cada elemento
% ------------------------------------------------------------
r10a1 = ri0ai(r10vp1, r10wp1, r10w1, r10s1);
% ------------------------------------------------------------
% Fuerza en el centro de masa de cada elemento
% ------------------------------------------------------------
r50f5 = ri0fi(r50a5, m5);
r40f4 = ri0fi(r40a4, m4);
r30f3 = ri0fi(r30a3, m3);
r20f2 = ri0fi(r20a2, m2);
r10f1 = ri0fi(r10a1, m1);
% ------------------------------------------------------------
% Par en el centro de masa de cada elemento
% ------------------------------------------------------------
r50n5 = ri0ni(r50wp5, r50w5, Iexter);

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);
r30s3);
r20s2);
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.

» 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.
. 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);
% ------------------------------------------------------------
% ------------------------------------------------------------

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 ];
% ------------------------------------------------------------
% Factores de posicionamiento de los centros de gravedad
% ------------------------------------------------------------
factor1 = -0.5; factor2 = -0.5; factor3 = -0.5;
factor4 = -0.5; factor5 = -0.5; factor6 = -0.5;
% ------------------------------------------------------------
% Masa de cada elemento (Kg)
% ------------------------------------------------------------
m1 = 2.78; m2 = 10.25; m3 = 0;
m4 = 5.57; m5 = 0; m6 = 1.98;
% ------------------------------------------------------------
% Coeficiente de rozamiento viscoso de cada articulacion
% ------------------------------------------------------------
b1 = 0.05; b2 = 0.05; b3 = 0.05;
b4 = 0.05; b5 = 0.05; b6= 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 = zeros(3,3);
r40I_r04 = [0.0606 0 0;0 0.0053 0;0 0 0.0606];
r50I_r05 = zeros(3,3);
r60I_r06 = [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 = ri0pi(a(5), d(5), alfa(5));
r60p6 = ri0pi(a(6), d(6), alfa(6));
r70p7 = zeros(3,1);
r70s7 = zeros(3,1);
% ------------------------------------------------------------
% Matrices de transformacion
% ------------------------------------------------------------
r01 = dh(teta(1), alfa(1)); r10 = r01';
r12 = dh(teta(2), alfa(2)); r21 = r12';
r23 = dh(teta(3), alfa(3)); r32 = r23';
r34 = dh(teta(4), alfa(4)); r43 = r34';
r45 = dh(teta(5), alfa(5)); r54 = r45';
r56 = dh(teta(6), alfa(6)); r65 = r56';
r67 = eye(3); r76 = r67';

% ------------------------------------------------------------
% Velocidad angular de las articulaciones
% ------------------------------------------------------------
r00w0 = zeros(3,1);
r10w1 = ri0wi(r10, r00w0, qp(1));
r20w2 = ri0wi(r21, r10w1, qp(2));
r30w3 = ri0wi(r32, r20w2, qp(3));
r40w4 = ri0wi(r43, r30w3, qp(4));
r50w5 = ri0wi(r54, r40w4, qp(5));
r60w6 = ri0wi(r65, r50w5, qp(6));
r70w7 = ri0wi(r76, r60w6, 0);
% ------------------------------------------------------------
% Aceleracion angular de las articulaciones
% ------------------------------------------------------------
r00wp0 = zeros(3,1);
r70wp7 = ri0wpi(r76, r60wp6, r60w6, 0, 0);
% ------------------------------------------------------------
% Aceleracion lineal articular
% ------------------------------------------------------------
r00vp0 = [0; 0; g];
% ------------------------------------------------------------
% Aceleracion del centro de masa de cada elemento
% ------------------------------------------------------------
% ------------------------------------------------------------
% Fuerza en el centro de masa de cada elemento
% ------------------------------------------------------------
r70f7 = ri0fi(r70a7, m7);
r60f6 = ri0fi(r60a6, m6);
r50f5 = ri0fi(r50a5, m5);
r40f4 = ri0fi(r40a4, m4);
r30f3 = ri0fi(r30a3, m3);
r20f2 = ri0fi(r20a2, m2);
r10f1 = ri0fi(r10a1, m1);
% ------------------------------------------------------------
% Par en el centro de masa de cada elemento
% ------------------------------------------------------------

r70n7 = ri0ni(r70wp7, r70w7, Iexter);
r60n6 = ri0ni(r60wp6, r60w6, r60I_r06);
r50n5 = ri0ni(r50wp5, r50w5, r50I_r05);
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
% ------------------------------------------------------------
r70f7a = r70f7;
r60f6a = ri0fia(r67, r70f7a, r60f6);
r50f5a = ri0fia(r56, r60f6a, 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);
r60p5 = r65*(r50p5); r70p6 = r76*(r60p6);
r70n7a = r70n7;
r60s6);
r50s5);
r40s4);
r30s3);
r20s2);
r10s1);
% ------------------------------------------------------------
% Fuerzas y pares de accionamientos
% ------------------------------------------------------------
t_1 = t_r(r10, r10n1a, qp(1), b1);
t_2 = t_r(r21, r20n2a, qp(2), b2);
t_3 = t_r(r32, r30n3a, qp(3), b3);
t_4 = t_r(r43, r40n4a, qp(4), b4);
t_5 = t_r(r54, r50n5a, qp(5), b5);
t_6 = t_r(r65, r60n6a, qp(6), b6);
tau = [t_1; t_2; t_3; t_4; t_5; t_6];
⇒ 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.

3.3.-Dinámica directa. Método de Walker-Orin.
Las ecuaciones de movimiento planteadas en el apartado anterior permiten
resolver el problema de la dinámica directa.
M.W.Walker y D.E.Orin [3 ] presentaron en 1982 cuatro métodos para la resolución del
problema dinámico directo de una cadena cinemática abierta utilizando la formulación
de N-E. En el artículo se realiza una comparación de la eficiencia computacional de los
cuatro métodos presentados, concluyendo que el tercero de los presentados es el más
eficiente frente al tiempo de cómputo. Para la realización de esta práctica los autores
han implementado el tercer método de Walker-Orin. (los ficheros se proporcionan
durante la práctica).
⇒ NOTA: Se recomienda al lector interesado la lectura del artículo de M.W.Walker y
D.E.Orin, dónde se presentan el resto de métodos que no han sido usados en este libro.
Walker y Orin resuelven la ecuación general del robot:
τ=+++ kqKqGqqqCqqH T
)()(),()(  (3.34)
donde:
)(qH Matriz no singular NxN de los momentos de inercia.
),( qqC  Matriz NxN que contabiliza los efectos de las aceleraciones centrífugas y
de Coriolis.
)(qG Vector Nx1 que contabiliza los efectos de la gravedad.
)(qK Matriz Jacobiana 6xN que especifica los pares (fuerzas) creados en cada
articulación debido a las fuerzas y momentos externos aplicados sobre el
elemento N.
k Vector 6x1 de los momentos y fuerzas externas ejercidos sobre el
elemento N.
τ Vector Nx1 de los pares (fuerzas) de cada articulación. (tau en los
ejemplos anteriores).
q Vector Nx1 de las variables articulares.
De la ecuación (3.34) se observa que las fuerzas y pares en las articulaciones son
funciones lineales de las aceleraciones articulares. Se define b como un vector
equivalente a los efectos de la gravedad, las aceleraciones centrifugas y de Coriolis y las
fuerzas externas aplicadas sobre el elemento N:
kqKqGqqqCb T
)()(),( ++=  (3.35)
Entonces, la ecuación (3.34) se puede poner como:
)()( bqqH −= τ (3.36)

El vector b se puede calcular fácilmente utilizando la función NEWTONEULER del
apartado anterior. Se llama a la función NEWTONEULER con los parámetros q, qp,
masaext e inerciaext (efectos de las fuerzas externas) correspondientes a la
configuración estudiada, y colocando a cero el parámetro qpp correspondiente a la
aceleración. De la ecuación (3.36) se observa que en este caso b=τ .
En el cálculo de la matriz )(qH es donde los diferentes métodos de Walker y Orin
difieren.
Para el cálculo de )(qH se va a utilizar el tercer método de Walker y Orin. Este
algoritmo aprovecha la simetría de la matriz )(qH para calcular la diagonal principal y
los términos de la mitad superior. Estos componentes se calculan con el siguiente orden
HN,N, HN-1,N,....., H1,N,; HN-1,N-1, HN-2,N-1,....., H1,N-1,;.....etc. Para el cálculo de la
articulación j, los últimos N-j+1 elementos aparecen como un cuerpo rígido, luego la
articulación j es la única con movimiento. En este caso, y conocida la localización del
centro de masas y el momento de inercia de este elemento ficticio, la fuerza total Fj y el
momento Nj ejercidos en el sistema compuesto por los elementos j hasta N se calcula
como:
)()( 11 jjjjjjjjj cMzczMvMF ×=×== −−
 para j rotacional
1−= jjj zEN (3.37)
1−= jjj vMF para j traslacional
0=jN (3.38)
donde:
jM masa total de los elementos j hasta N
jv aceleración lineal del centro de masas del cuerpo rígido compuesto por los
elementos j hasta N
jc localización del c.d.m. del cuerpo rígido compuesto respecto a las coordenadas
del elemento j-1
jE la matriz de momentos de inercia del cuerpo rígido compuesto por los elementos
j hasta N
Puesto que Fi y Ni son cero para i<j, utilizando las ecuaciones (3.24) y (3.25) se obtiene
que:
1+= ii ff
1
*
1 ++ ×+= iiii fpnn para i=1 ... j-1 y
(3.39)
jj Ff =
jjjj FcNn ++=

luego empezando con i=j , las ecuaciones anteriores pueden ser usadas para obtener ni y
fi para i≤ j.
Los componentes de la matriz de momentos de inercia a lo largo de la columna j son
iguales a los pares y fuerzas generados en las articulaciones. Luego para i≤ j:
ii nz 1− para articulación i rotacional
ijH (3.40)
ii fz 1− para articulación j traslacional
Los parámetros Mj,cj yEj utilizados en las ecuaciones (3.37) y (3.38) se calculan con las
siguientes ecuaciones:
 para el elemento N:
NN mM =
*
NNN psc +=
NN JE =
 y para el resto:
jjj mMM += +1
( ) ( )[ ]*
11
*1
jjjjjj
j
j pcMpsm
M
c +++= ++
( ) ( ) ( )( )[ ]
( ) ( ) ( )( )[ ]T
jjjjjjjjjjjjjj
T
jjjjjjjjjjjjjjj
cpccpsIcpscpsmJ
cpccpcIcpccpcMEE
−+−+−−+−+++
+−+−+−−+−++=
+
++++−−
*
1
***
*
1
*
1
*
1
*
111
*
*
donde:
mj masa del elemento j
sj posición del centro de masas del elemento i respecto a las coordenadas del
elemento j
Jj matriz de momentos de inercia del elemento j
I matriz identidad 3x3

Ejemplo 3.3
Se muestra a continuación un ejemplo en el que se resuelve la dinámica directa del
robot prismático de 4 gdl.
. A continuación se presenta la función WALKERORIN4, utilizada
para resolver la dinámica directa del robot prismático de 4 gdl. Realizando help
WALKERORIN4, la función nos informa sobre los parámetros necesarios para realizar
los cálculos.
% WALKERORIN4 Tercer método de Walker & Orin.
% QPP = WALKERORIN4(Q, QP, TAU, MASAEXT, INERCIAEXT) calcula la cinemática
% inversa del robot de 4GDL devolviendo el vector 4x1 que representa la
% aceleración de cada articulación utilizando el tercer método de Walker y
Orin.
% Q es el vector 4x1 de variables articulares. QP es el vector 4x1 que
% representa la velocidad de cada articulación. TAU es el vector 4x1
% que representa el par de entrada a cada articulación. MASAEXT es
% la masa de la carga externa. INERCIAEXT es la inercia de la carga externa.
%
% See also NEWTONEULER4, H4.
function qpp = walkerorin4(q,qp,tau,masaext,inerciaext)
% Se calcula el vector b.
b = newtoneuler4(q,qp,zeros(4,1),9.8,masaext,inerciaext);
% Se calcula la matriz de momentos de inercia H.
H = h4(q,masaext,inerciaext);
% Se calcula el vector de aceleración de cada articulación.
qpp = inv(H)*(tau-b);
Para comprobar el funcionamiento del código presentado se sugiere al lector que realice
tanto la dinámica inversa como la directa del robot estudiado, tal y como muestra el
siguiente ejemplo.

» q=rand(4,1);
» qp=rand(4,1);
» qpp=rand(4,1);
» m4=3;
» iext=0.05*eye(3);
» tau=newtoneuler4(q,qp,qpp,9.8,m4,iext)
tau =
0.5229
160.0619
2.0591
0.0315
» acel=walkerorin4(q,qp,tau,m4,iext)
acel =
0.1389
0.2028
0.1987
0.6038
» qpp
qpp =
0.1389
0.2028
0.1987
0.6038
»
Ejemplo 3.4
Se muestra a continuación un ejemplo en el que se resuelve la dinámica directa del
robot rotacional de 6 gdl.
. A continuación se presenta la función WALKERORIN6, utilizada
para resolver la dinámica directa del robot rotacional de 6 gdl. Realizando help
WALKERORIN6, la función nos informa sobre los parámetros necesarios para realizar
los cálculos.

% WALKERORIN6 Tercer método de Walker & Orin.
% QPP = WALKERORIN6(Q, QP, TAU, MASAEXT, INERCIAEXT) calcula la cinemática
% inversa del robot de 6GDL devolviendo el vector 6x1 que representa la
% aceleración de cada articulación utilizando el tercer método de Walker y
Orin.
% Q es el vector 6x1 de variables articulares. QP es el vector 6x1 que
% representa la velocidad de cada articulación. TAU es el vector 6x1
% que representa el par de entrada a cada articulación. MASAEXT es
% la masa de la carga externa. INERCIAEXT es la inercia de la carga externa.
%
% See also NEWTONEULER6, H6.
function qpp = walkerorin6(q,qp,tau,masaext,inerciaext)
% Se calcula el vector b.
b = newtoneuler6(q,qp,zeros(6,1),9.8,masaext,inerciaext);
% Se calcula la matriz de momentos de inercia H.
H = h6(q,masaext,inerciaext);
% Se calcula el vector de aceleración de cada articulación.
qpp = inv(H)*(tau-b);
» q=rand(6,1);
» qp=rand(6,1);
» qpp=rand(6,1);
» m7=3;
» iext=0.05*eye(3);
» tau=newtoneuler6(q,qp,qpp,9.8,m7,iext);
» acel=walkerorin6(q,qp,tau,m7,iext)
acel =
0.8537
0.5936
0.4966
0.8998
0.8216
0.6449
» qpp
qpp =
0.8537
0.5936
0.4966
0.8998
0.8216
0.6449
»

3.4.- PRACTICA. Simulación péndulo de 3 gdl.
Utilizando el método de Walker y Orin para el cálculo de la dinámica directa de
un robot, se va a simular cual sería el comportamiento de un péndulo de 3GDL, ver
figura 3.4, si no se le aplica ningún par en ninguna de las articulaciones. Los parámetros
de Denavit – Hartenberg del péndulo se muestran en la tabla 3.2.
Figura 3.4. Representación D-H del péndulo de 3 gdl
1 θ1 0 1m 0
2 θ2 0 1m 0
3 θ3 0 1m 0
Se ha simulado el comportamiento del péndulo suponiendo que la posición y la
velocidad de cada articulación son inicialmente nulas. Así mismo se ha considerado que
el péndulo no posee ninguna masa externa.
A continuación se presenta la función SIMULA3, que simula el comportamiento del
péndulo de 3 GDL.
% SIMULA3 Simulación del péndulo de 3GDL.
% MAT_Q = SIMULA3(TSIM, PINT) simula el comportamiento del péndulo
% de 3 GDL suponiendo que no existe ningún par en ninguna de las
% articulaciones. TSIM indica el tiempo total de la simulación en
% segundos. PINT es el periodo de integración en segundos. Este
% periodo se utiliza para dividir el tiempo de simulación en intervalos.
% En MAT_Q se devuelven las coordenadas articulares del robot, almacenadas
% por columnas, correspondientes a cada intervalo de tiempo. MAT_Q es una
% matriz de 3 filas y una columna por cada intervalo de simulación.
%
% See also WALKERORINRUNGE3.
function mat_q = simula3(tsim, pint)
% Características de la carga externa
masaext = 0;
inerciaext = zeros(3);
% Posición y velocidad inicial de las articulaciones del robot
q = zeros(3,1);
qp = zeros(3,1);
z0
x0
y0
z1
x1
y1
z2
x2
y2
z3
x3
y3θ1
θ2
θ3

% Vector de tiempo dividido en intervalos
t = 0:pint:tsim;
% Número de intervalos + 1
n = length(t);
% Inicialización de la matriz de coordenadas articulares
mat_q(:,1) = q;
% Se calcula las coordenadas articulares del robot
% en cada intervalo de la simulación
for i=2:n
% Se calcula la posición y la velocidad de cada articulación
% combinando el tercer método de Walker & Orin con el método
% de integración de Runge-Kutta.
[q qp] = walkerorinrunge3(q,qp,zeros(3,1),t(i-1),t(i),masaext,inerciaext);
% Se almacenan las coordenadas articulares
mat_q(:,i) = q;
end
Hay que destacar que para actualizar la posición y la velocidad de cada articulación se
utiliza el método de Walker y Orin en combinación con el método de integración de
Runge-Kutta de orden cuatro. Esto se debe a que el tercer método de Walker y Orin
proporciona la aceleración de cada articulación, por lo que es necesario utilizar un
método de integración para obtener la posición y la velocidad de cada articulación. A
continuación se muestra el código en Matlab
de la función WALKERORINRUNGE3,
que combina el método de Walker y Orin junto con el de Runge-Kutta para el cálculo de
la posición y velocidad de cada articulación:
% WALKERORINRUNGE3 Tercer método de Walker & Orin.
% [QFIN, QPFIN] = WALKERORINRUNGE3(QINI, QPINI, PAR, T0, TF, MASAEXT,
% INERCIAEXT) calcula la posición y la velocidad de cada articulación
% del péndulo de 3 GDL combinando el tercer método de Walker & Orin
% con el método de integración de Runge-Kutta de orden cuatro.
% QINI es el vector 3x1 de variables articulares en el instante de
% tiempo T0. QPINI es el vector 3x1 que representa la velocidad de
% cada articulación en el instante de tiempo T0. PAR es el vector
% 3x1 que representa el par de entrada a cada articulación. T0 y TF
% representan, respectivamente, los valores de inicio y de fin del
% intervalo de tiempo. MASAEXT es la masa de la carga externa.
% INERCIAEXT es la inercia de la carga externa. En QFIN y QPFIN se
% devuelven, respectivamente, los vectores 3x1 de posición y
% velocidad de cada articulación en el instante de tiempo TF.
%
% See also WALKERORIN3.
function [qfin, qpfin] =
walkerorinrunge3(qini,qpini,par,t0,tf,masaext,inerciaext);
h = (tf-t0);
% Primer termino.
t1 = t0;
q1 = qini;
qp1= qpini;
v1 = h*qpini;
w1 = h*walkerorin3(q1,qp1,par,masaext,inerciaext);
% Segundo termino.
t2 = t0 + .5*h;
q2 = qini + .5*v1;
qp2= qpini + .5*w1;

v2 = h*(qpini + .5*w1);
% Tercer termino.
t3 = t0 + .5*h;
q3 = qini + .5*v2;
qp3= qpini + .5*w2;
v3 = h*(qpini + .5*w2);
% Cuarto termino.
t4 = t0 + h;
q4 = qini + v3;
qp4= qpini + w3;
v4 = h*(qpini + w3);
% Formula de Runge-Kutta.
qfin = qini + (v1 + 2*v2 + 2*v3 + v4)/6;
qpfin = qpini + (w1 + 2*w2 + 2*w3 + w4)/6;
% Redondeo para evitar oscilacion numerica.
qfin = round(qfin*1e13)/1e13;
qpfin = round(qpfin*1e13)/1e13;
En el código anterior se hacen varias llamadas a la función WALKERORIN3. Esta
función calcula la dinámica directa del péndulo de 3GDL utilizando el tercer método de
Walker y Orin. Para el cálculo de la dinámica directa se utilizan las funciones
NEWTONEULER3 y H3.
Para simular el comportamiento del péndulo de 3GDL durante 1 segundo considerando
un periodo de integración de 0.001 segundos se procedería de la siguiente manera en
Matlab
:
» mat_q=simula3(1,0.001);
»
Para comprobar visualmente el comportamiento del péndulo de 3GDL se ha
desarrollado la función ANIMACION3. El código de esta función se muestra a
continuación:
% ANIMACION3 Animación del movimiento del péndulo de 3GDL.
% ANIMACION3(MAT_Q) realiza la animación del movimiento del
% péndulo de 3GDL a partir de las coordenadas articulares
% almacenadas en la matriz MAT_Q. MAT_Q contiene 3 filas
% y una columna para cada disposición del robot durante el
% movimiento.
d = [0 0 0];
a = [1 1 1];
alfa = [0 0 0];
% Vector de posicion (x, y) del sistema de coordenadas de referencia
x0 = 0; y0 = 0;

% Se dibuja el sistema de coordenadas de referencia. Se asigna el modo XOR
para borrar
% sólo el robot dibujado anteriormente.
p = plot(x0,y0,'EraseMode','xor');
grid;
axis([-3.2 3.2 -3.1 1]);
hold on;
n = size(mat_q,2);
for i=1:n
teta1 = mat_q(1,i);
teta2 = mat_q(2,i);
teta3 = mat_q(3,i);
consecutivos
A02 = A01 * A12;
A03 = A02 * A23;
% Vector de posicion (x, y) de cada sistema de coordenadas
x1 = A01(1,4); y1 = A01(2,4);
x2 = A02(1,4); y2 = A02(2,4);
x3 = A03(1,4); y3 = A03(2,4);
x = [x0 x1 x2 x3];
y = [y0 y1 y2 y3];
set(p,'XData',x,'YData',y);
drawnow;
end
Para realizar la animación del comportamiento simulado anteriormente se procedería de
la siguiente manera en Matlab:
» animacion3(mat_q)
»
Al ejecutar la función en Matlab
nos aparecerá una ventana similar a mostrada en la
figura 3.5 en la que se visualizará la animación.

Figura. 3.5. Aspecto de la visualización de la animación del péndulo de 3GDL.

Práctica 4
Selección de
Servoaccionamientos
4.1.-Introducción
La selección de los servoaccionamientos y el modelo los mismos en base a la
dinámica inversa de la máquina y catálogos comerciales de motores es un paso clave en
el proceso de análisis, simulación o diseño de un robot. La presente práctica pretende
mostrar los pasos a seguir para una correcta elección de los actuadores del robot. En
primer lugar se requiere un conocimiento previo de los tipos de actuadores. En la
práctica 1 se presentó una clasificación de las diferentes tecnologías de actuadores
utilizadas en robótica. En esta práctica van a utilizarse los motores eléctricos, y para ello
el primer paso es realizar un modelo dinámico que se ajuste al comportamiento real del
motor. Este modelo se utilizará en las prácticas siguientes dentro del esquema de control
y simulación de robots. Para asignar los valores de las variables que aparezcan en el
modelo del actuador se debe utilizar la información proporcionada por los fabricantes de
los motores, y para ello el siguiente paso es seleccionar de los catálogos existentes el
actuador que se adecue a los requerimientos del robot. Estos requerimientos van a ser
los pares máximos que deban ser soportados por el robot.
 Objetivos
• Modelo dinámico de un motor DC: brushless y brush.
• Análisis de la aplicación particular. Robot de 3 grados de libertad.
• Cálculo de los pares máximos requeridos.
• Búsqueda en catálogos comerciales de motores.

4.2.- Modelo dinámico de un motor DC
Se va a utilizar un modelo eléctrico válido en régimen permanente.
El comportamiento dinámico del motor para régimen permanente se puede representar
mediante el siguiente circuito eléctrico, como se observa en la figura y ecuaciones
siguientes:
wk
dt
di
LRiV v++=
El par del motor es proporcional a la corriente circulante.( τ = kt i )
El diagrama de bloques del modelo es el siguiente:
R L
i fem = kv
w
V

4.3.- Cálculo de los pares máximos requeridos
Para seleccionar los motores adecuados para un robot es necesario conocer los
requerimientos máximos a los cuales van a estar sometidos los motores. Para ello se
sitúa el robot en su peor configuración y se le aplica un perfil de velocidad trapezoidal a
cada una de las articulaciones para obtener, utilizando la dinámica inversa del robot, el
par pico y el par nominal de cada motor. Un perfil de velocidad trapezoidal se
caracteriza por la gráfica que se muestra en la figura 4.1. El perfil de velocidad se divide
en tres intervalos:
- Intervalo de aceleración (tacel): Durante este tiempo se introduce a la articulación la
aceleración máxima a la que va a estar sometida hasta alcanzar la velocidad máxima
a la que puede operar. El par pico se obtendrá, utilizando la dinámica inversa,
cuando se introduzca a la articulación la velocidad máxima y la aceleración máxima.
- Intervalo constante (tconst): Durante este tiempo se introduce a la articulación la
velocidad máxima a la que va a estar sometida considerando aceleración nula. El par
nominal corresponderá al par producido en estas condiciones utilizando la dinámica
inversa.
- Intervalo de deceleración (tdecel): Durante este intervalo se introduce a la articulación
una deceleración hasta alcanzar una velocidad nula.
Figura 4.1. Perfil de velocidad trapezoidal.
Los pares obtenidos mediante la dinámica inversa son divididos por el reductor
seleccionado para cada motor. La elección del reductor vendrá determinada por la
velocidad máxima permitida para el accionamiento y por las revoluciones a las que
trabaje el motor. Por último hay que señalar que, generalmente, la relación entre el par
nominal y el par pico es del orden de 3~4 en los motores brushless.
Ejemplo 4.1
Cálculo de los pares máximos requeridos para el robot de 4gdl.
Para calcular los pares máximos requeridos de cada articulación se ha desarrollado una
herramienta en Simulink que, especificando la peor posición en la que puede
encontrarse el robot y el perfil de velocidad que se le aplica a cada articulación, se
obtiene una gráfica del par o fuerza de la articulación. En esta gráfica aparece tanto el
par o fuerza pico como el par o fuerza nominal. El fichero en el que se encuentra
t
ve
l
vmax
tace
l
tconst tdecel

implementada la herramienta se denomina selmotor4.mdl. Si se ejecuta desde el entorno
de Matlab
“selmotor4” aparecerá en pantalla la herramienta descrita anteriormente, que
se muestra en la figura 4.2.
Figura 4.2. Herramienta desarrollada en Simulink para calcular
el par o fuerza pico y nominal de cada articulación.
Para generar el perfil de velocidad trapezoidal se ha creado el subsistema que se
muestra en la figura 4.3. Para tener acceso a dicho subsistema es necesario seleccionar
el bloque y elegir la opción Look Under Mask del menú Edit de Simulink
.
Figura 4.3. Subsistema Perfil de Velocidad Trapezoidal.

El subsistema mostrado en la figura 4.3 hace uso de una función desarrollada en Matlab
trapezoidal.m. Esta función sirve para generar la velocidad y aceleración de la
articulación en cada instante de tiempo según los parámetros elegidos del perfil de
velocidad, que recordemos que son la velocidad máxima, el tiempo de aceleración, el
tiempo constante y el tiempo de deceleración. El código fuente de esta función se
muestra a continuación:
% TRAPEZOIDAL Perfil de velocidad trapezoidal
% PERFIL=TRAPEZOIDAL(U) devuelve un vector 1x2 que contiene la velocidad
% y la aceleración de un perfil de velocidad trapezoidal en un determinado
% instante de tiempo. PERFIL(1) contiene la velocidad y PERFIL(2) la
% aceleración. U(1) representa el instante de tiempo actual(seg). U(2) es
% la velocidad máxima. U(3) es el tiempo de aceleración(seg). U(4) es el
% tiempo de velocidad constante(seg). U(5) es el tiempo de
deceleración(seg).
function perfil=trapezoidal(u)
t = u(1); % Instante de tiempo actual
velmax = u(2); % Velocidad máxima
tacel = u(3); % Tiempo de aceleración
tconst = u(4); % Tiempo de velocidad constante
tdecel = u(5); % Tiempo de deceleración
% Se calcula la velocidad y la aceleración
if t <= tacel
% Intervalo de aceleración
acel = velmax/tacel;
vel = acel*t;
else
if t < (tacel+tconst)
% Intervalo de velocidad constante
vel = velmax;
acel = 0;
else
if t>(tacel+tconst+tdecel)
% Velocidad y aceleración nula
vel = 0;
acel = 0;
else
% Intervalo de deceleración
acel = -velmax/tdecel;
vel = velmax+acel*(t-tacel-tconst);
end
end
end
% Se devuelve la velocidad y la aceleración
perfil = [vel acel];
Para obtener el par o fuerza de cada articulación se ha desarrollado en Matlab
la
función dininv4gdl.m; que calcula el par o fuerza de cada articulación cuando el robot
transporta una determinada carga en función de la posición, velocidad y aceleración de
cada articulación. Esta función hace uso de la función newtoneuler4.m, explicada en la
práctica 3. El código fuente de dininv4gdl.m se muestra a continuación:

Una vez presentada la herramienta, así como los ficheros que se van a utilizar para
calcular los pares y fuerzas pico y nominal de cada articulación, se va a proceder a
obtener estos pares y fuerzas en el caso del robot de 4 GDL.
En primer lugar hay que destacar que la peor posición en la que se puede encontrar el
robot se produce cuando las dos articulaciones prismáticas han alcanzado su valor
máximo. Se considerará que el valor máximo de la segunda articulación (q2) es de 1m,
mientras que el de la tercera (q3) es de 1.2m. El valor de la primera y cuarta articulación
no influye en el cálculo de los pares/fuerzas máximas. Por lo tanto, por comodidad, la
peor posición del robot vendrá dada por:
( )02.110=q
Para ver la configuración del robot en la peor posición podemos utilizar la función
drawrobot3d4 que vimos en la práctica 2:
» drawrobot3d4([0 1 1.2 0]')
La ejecución de la función anterior muestra, efectivamente, que el vector de
coordenadas articulares anterior corresponde a la peor disposición del robot, como se
puede apreciar en la figura 4.4.
% DININV4GDL Dinámica inversa de un robot de 4GDL.
% PAR = DININV4GDL(ENTRADA) calcula el vector 4x1 de pares/fuerzas de
% entrada a las articulaciones utilizando el método de Newton-Euler.
% ENTRADA(1:4) representa la posición de cada articulación.
% ENTRADA(5:8) es la velocidad de cada articulación. ENTRADA(9:12)
% es la aceleración de cada articulación.
%
% See also NEWTONEULER4.
function par = dininv4gld(entrada)
q = entrada(1:4); % Posición de cada articulación
qp = entrada(5:8); % Velocidad de cada articulación
qpp = entrada(9:12); % Aceleración de cada articulación
% Parámetros de la carga
masaext = 10;
inerciaext = [0.0167 0 0;0 0.0167 0;0 0 0.0167];
% Se calcula el vector de pares/fuerzas utilizando Newton-Euler
par = newtoneuler4(q,qp,qpp,9.8,masaext,inerciaext);

Figura 4.4. Peor disposición del robot. Se obtiene al ejecutar drawrobot3d4([0 1 1.2 0]').
Como ya se ha comentado anteriormente, los parámetros que caracterizan a un perfil de
velocidad trapezoidal son:
- Velocidad máxima
- Tiempo de aceleración
- Tiempo constante
- Tiempo de deceleración
Para el caso del robot de 4 GDL se va a suponer que la velocidad máxima para las
articulaciones rotacionales es de π/3 rad/s, mientras que en el caso de las prismáticas es
de 1 m/s. Se va a considerar que el tiempo de aceleración es 0.1 segundos, lo que quiere
decir que se dispone de 0.1 segundos para alcanzar la velocidad máxima. Como tiempo
constante se va a tomar 0.4 segundos y como tiempo de deceleración 0.1 segundos. Es
importante destacar que en función de la articulación se considerará la velocidad
máxima positiva o negativa. En el caso de las articulaciones 1 y 4 es independiente
considerar la velocidad máxima positiva o negativa, ya que a efectos de requerimientos
de par, no hay diferencia en girar la articulación en un sentido o en otro. Para la
obtención de los pares pico y nominal se ha considerado la velocidad máxima positiva.
En las articulaciones 2 y 3 los mayores requerimientos de par se producirán cuando la
articulación se desplace en el sentido positivo del eje z. Por lo tanto, para ambas
articulaciones se ha considerado la velocidad máxima positiva. Ver figura 4.5.

Figura 4.5. Robot de 4 GDL.
Para las articulaciones rotacionales se van a utilizar reductores de 500, ya que
suponemos que se van a utilizar motores que trabajan a 5000 rpm. Esto se debe que la
velocidad máxima permitida es de π/3 rad/s, o sea 10 rpm, por lo que si se utilizan
motores que funcionan a 5000 rpm, se obtiene un factor de reducción de 5000/10=500.
Este factor puede ser comercialmente obtenido colocando en serie un reductor de 200
por piñones y un reductor de factor 1:2.5 obtenido por transmisión de correa.
Para las articulaciones prismáticas se usarán, como reductores, husillos de bolas de paso
25 mm. Para calcular el factor de reducción en primer lugar es necesario obtener las rpm
que corresponden a la velocidad máxima de las articulaciones prismáticas. Para ello se
utiliza la siguiente ecuación:
60x
p
v
=η
donde v es la máxima velocidad (mm/s) y p es el paso del husillo (mm). Puesto que la
máxima velocidad de las articulaciones prismáticas es de 1000 mm/s y el paso del
husillo es 25 mm, se obtiene que la velocidad máxima es de 2400 rpm. Por tanto si se
utilizan motores que funcionan a 5000 rpm, se obtiene un factor de reducción de
5000/2400 ≈ 2.
Para obtener los pares y fuerzas de los motores teniendo en cuenta los reductores que se
van a emplear, únicamente, como se muestra en la figura 4.2, es necesario dividir el
par/fuerza proporcionado por la dinámica inversa por el valor del reductor.
Para obtener los pares máximos requeridos hay que considerar que el robot tiene en su
extremo la máxima carga permitida. Para nuestro robot hemos considerado que la
carga máxima es de 10 Kg y que su matriz de inercia es:
x
0
y
0
z0
x
1
y
1
z2
y
2
x
2
x
3
y
3
l1
d
2
d
3
a2
l4
0
z4
1
2 3
4 θ4
x
4
y
4
z3

⎥
⎥
⎥
⎦
⎤
⎢
⎢
⎢
⎣
⎡
=
0167.000
00167.00
000167.0
Inercia
Estas características de la carga externa se especifican en el fichero dininv4gdl.m.
Una vez especificadas las características relativas al robot, el siguiente paso es realizar
la simulación del modelo desarrollado en Simulink. Pero antes es necesario configurar
los parámetros de la simulación. Para ello ejecutamos la opción Parameters del menú
Simulation. Los valores seleccionados de los parámetros son los que se muestran en la
figura 4.6.
Figura 4.6. Configuración de los parámetros de la simulación.
Para realizar la simulación del modelo desarrollado en Simulink para obtener los pares y
fuerzas pico y nominal de cada articulación únicamente debemos ejecutar la opción
Start del menú Simulation. Cuando finalice la simulación podemos pulsar dos veces con
el ratón en cualquier gráfica que muestre el par o fuerza de la articulación para obtener
su par o fuerza pico y nominal. Por ejemplo, si pulsamos dos veces sobre el bloque
correspondiente al par de la articulación 1, obtenemos la gráfica del par de la
articulación que se muestra en la figura 4.7. El par pico corresponde al par máximo en
valor absoluto que aparece en la gráfica, que en este caso es 0.612 Nm. El par nominal
es el valor absoluto del par en el intervalo constante del perfil trapezoidal, cuyo valor es,
en este caso, 0.088 Nm.

Figura 4.7. Gráfica correspondiente al par de la articulación 1.
En el caso de la cuarta articulación el par pico es 4.85·10-4
Nm y el par nominal es
1.05·10-4
Nm.
En la segunda articulación la fuerza pico es 228 N y la nominal es 113 N. En la tercera,
la fuerza pico es de |–93| = 93 N y la nominal |–12| = 12 N. Para obtener el par
equivalente a una fuerza se utiliza la siguiente fórmula:
n
pF
π
τ
2
=
donde F es la fuerza (N), p es el paso del husillo (m) y n es la eficiencia del husillo.
Suponiendo que la eficiencia del husillo es de 0.85, puesto que el paso del husillo es
0.025 m, los pares pico y nominal de las articulaciones 2 y 3 son:
- Articulación 2: Par pico: 1.0716 Nm; Par nominal: 0.5311 Nm.
- Articulación 3: Par pico: |-0.4371| Nm; Par nominal: |–0.0564| Nm.
En la tabla 4.1 se muestran los pares pico y nominal de cada una de las articulaciones
del robot de 4 gdl.
Articulación 1 2 3 4
τpico (Nm) 0.612 1.0716 0.4371 4.85·10-4
τnominal (Nm) 0.088 0.5311 0.0564 1.05·10-4
Tabla 4.1. Par pico y nominal de cada articulación.

Obtención de los datos de los motores
El par pico y nominal obtenido para cada actuador es necesario multiplicarlo por un
factor de seguridad de 1.5 antes de proceder a buscar los datos de los motores en
catálogos comerciales. A partir de los pares obtenidos al aplicar el factor de seguridad
se procede a buscar en catálogos comerciales los datos de los motores que satisfagan
los requerimientos de par. Los datos que se deben extraer de los motores son los
correspondientes a los parámetros requeridos por el modelo dinámico del motor, ver
tabla 4.3.
Parámetro Símbolo Unidades
Resistencia R Ohms (Ω)
Inductancia L mH
Constante de par KT Nm/A
Constante de voltaje KV V/rad/s
Corriente máxima Imáx A
Tabla 4.3. Parámetros requeridos por el modelo dinámico del motor.
Obtención de los datos de los motores del robot de 4gdl.
En la tabla 6.4 se muestra el par pico y nominal de cada articulación después de
multiplicar los datos mostrados en la tabla 6.1 por el factor de seguridad de 1.5.
Articulación 1 2 3 4
τpico (Nm) 0.918 1.6074 0.6556 7.275·10-
4
τnominal (Nm) 0.132 0.7967 0.0846 1.575·10-
4
Tabla 4.4. Par pico y nominal de cada articulación considerando
un factor de seguridad de 1.5.
Se ha considerado que los actuadores del robot corresponden a motores brushless DC.
Para obtener los datos de los parámetros de los motores que satisfacen los
requerimientos de par especificados en la tabla 4.4 se ha consultado el catálogo de
motores brushless DC de Eastern Air Devices, Inc. Los valores seleccionados de los
parámetros del modelo dinámico del motor se muestran en la tabla 4.5.
Articulación Nombre Motor R(Ω) L(mH) KT (Nm/A) KV (V/rad/s) Imáx (A)
1 DA23GBB 0.8 0.93 0.058 0.058 18.5
2 DA34HBB 1.6 1.56 0.176 0.176 23.7
3 DA23GBB 0.8 0.93 0.058 0.058 18.5
4 DB17CDB 6.9 1.28 0.035 0.035 3.6
Tabla 4.5. Datos de los motores seleccionados para cada articulación.

4.3.- PRACTICA. Robot de 3 grados de libertad.
Se van a estudiar los servoaccionamientos que se emplean en un robot de 3 gdl. El
robot de 3 gdl considerado corresponde al robot Mitsubishi PA-10 tomando únicamente
sus tres primeras articulaciones. El modelo cinemático y dinámico de este robot se ha
desarrollado con las herramientas estudiadas en las prácticas 2 y 3 aplicadas a un robot
con los DH indicados en la tabla 4.6.
Figura 4.6. Representación del robot de 3 gdl.
 cinemática
A continuación se muestra la tabla con los parámetros de Denavit-Hartenberg
del robot considerado.
Articulación θ α a (mm) d (mm)
1 θ1+π/2 -π/2 0 315
2 θ2 0 450 0
3 θ3 0 0 400
Tabla 4.1. Parámetros DH del robot de 3 gdl
 dinámica
Para realizar la dinámica inversa del robot se ha utilizado el método de Newton-
Euler, y para la dinámica directa se ha implementado el tercer método de
Walker-Orin, ya estudiados

 Cálculo de los pares máximos requeridos.
Para seleccionar los motores adecuados a la aplicación es necesario conocer los
requerimientos máximos a los cuales va a estar sometido el motor. Para ello se coloca al
robot en su peor configuración y se aplica un movimiento a las velocidades y
aceleraciones máximas permitidas. Se debe calcular por tanto el par nominal y el par
pico para cada motor, siendo el nominal a aceleración nula y velocidad máxima y el
pico en condiciones de aceleración y velocidad máximas.
Se ha implementado en Simulink una herramienta que nos permite obtener una
gráfica de los pares pico y nominal en una determinada posición fija: “selmotor.mdl”
Se ha de observar que la relación entre el par nominal y el par pico es del orden de
3~4 en los motores brushless.
En este caso se ha utilizado un reductor de 500, basándonos en que la velocidad
máxima permitida para un accionamiento es de π/3 rad/seg, y suponiendo que vamos a
utilizar motores que trabajan a 5000 rpm se obtiene un factor de reducción de 500. Este
factor puede ser comercialmente obtenido colocando en serie un reductor de 200 por
piñones y un reductor de factor 1:2.5 obtenido por transmisión de correa.
 Búsqueda en catálogos comerciales de motores.
Una vez obtenidos los pares nominal y pico para cada actuador, se multiplican estos
valores por un factor de seguridad de 1.5, y con estos valores se busca en catálogos
comerciales.

Se debe rellenar una tabla como la adjunta en la que figuren los datos requeridos
por el modelo dinámico del motor que se ha visto anteriormente y que será el que se
utilice en la aplicación del robot.
articulación nombre motor R(Ω) L(mH) kt (Nm/A) kv (V/rad/s) Imax (A)
1
2
3
Para ello se utilizará los catálogos proporcionados por el profesor durante la
sesión de prácticas.
Implementar las funciones del ejemplo 4.1 para el caso del robot rotacional de 6
gdl mostrado en las prácticas anteriores.
Ejercicio 1
Ejercicio 2

Práctica 5
Planificación de Trayectorias
5.1.-Introducción
Una vez obtenidos los modelos cinemáticos y dinámicos del robot se puede
abordar el problema del control de los mismos. Definir el movimiento de un robot
implica controlar dicho robot de manera que siga un camino preplanificado. El objetivo
es por tanto establecer cuales son las trayectorias que debe seguir cada articulación del
robot a lo largo del tiempo para conseguir los objetivos fijados, a la vez que se exige
cumplir una serie de restricciones físicas impuestas por los actuadores y de calidad de la
trayectoria, como son suavidad, precisión, etc.
Para un estudio más estructurado del problema de control del robot, éste suele
dividirse en dos bloques:
• Control cinemático o planificación de trayectorias. Consiste en describir
el movimiento deseado del manipulador como una secuencia de puntos
en el espacio (con posición y orientación). El control cinemático
interpola el camino deseado mediante una clase de funciones
polinomiales y genera una secuencia de puntos a lo largo del tiempo.
• Control dinámico o control de movimiento. Trata de conseguir que el
robot siga realmente las trayectorias marcadas por el control cinemático
teniendo en cuenta las limitaciones de los actuadores y el modelo
dinámico del robot. Tal y como se estudió en la práctica 3, el modelo
dinámico del robot es fuertemente no lineal, multivariable y acoplado.
Este aspecto del control será abordado en la práctica 6 de simulación y
control de robots.
Esta práctica aborda el control cinemático del robot, revisando los conceptos de
planificación de trayectorias y las herramientas necesarias para la interpolación
polinomial entre los puntos de inicio y final del camino. En primer lugar se presenta un
esquema general del problema de la planificación, más tarde se distingue entre el
espacio cartesiano y el espacio de las articulaciones del robot para poder abordar en

ellos los problemas de interpolación entre puntos espaciales. Se presenta en el apartado
5.4 un método de interpolación 4-3-4 válido en muchos casos como esquema de
interpolación. Finalmente se utilizan las herramientas desarrolladas en un ejemplo.
5.2.-Esquema General de Planificación de Trayectorias.
En la práctica 2 se presentó la función PLANIFICA6 en la cual se mostraba una
animación del robot rotacional de 6 gdl. Para realizar esta animación se necesitó realizar
una planificación simple de la trayectoria que debía seguir el robot. En aquel caso no se
tuvo en consideración ninguna restricción sobre el camino que debía trazar el robot, y
simplemente se obtuvieron una serie de posiciones por las que fue pasando el robot
cuando se resolvía su cinemática inversa. En ningún momento se tuvo en cuenta la
realidad física de los actuadores que proporcionan el movimiento a los eslabones, y sus
limitaciones de proporcionar velocidades instantáneas con aceleraciones infinitas.
Asimismo en aquel ejemplo tan sólo se consideró la planificación en posición, sin
observar los cambios en la orientación de la herramienta.
La realidad del problema de planificación de trayectorias exige sin embargo
tener en consideración las prestaciones reales de los actuadores, de tal manera que el
movimiento del robot sea un movimiento suave y coordinado.
Para llegar a obtener un planificador que funcione correctamente, los pasos a
seguir son los siguientes:
1. Estudiar las necesidades de movimiento especificadas por el usuario o por los
sensores propios del sistema robotizado, evitando colisiones con el entorno etc.,
obteniendo una expresión analítica en coordenadas cartesianas de la trayectoria
deseada en función del tiempo (libre de colisiones).
2. Muestrear la trayectoria anterior en una serie finita de puntos nudo de control
que se utilizan como puntos inicial y final de cada segmento. Cada uno de estos
puntos está especificado por sus componentes cartesianas de posición y
orientación (x,y,z,α,β,γ).
3. Pasar cada uno de estos puntos a coordenadas articulares del robot, utilizando
para ello la transformación homogénea inversa estudiada en la práctica 2.
4. Realizar la interpolación entre los puntos de las coordenadas articulares y
obtener para cada articulación una expresión del tipo qi(t) para cada segmento de
control.
Se observa que un planificador consiste en obtener una función de trayectoria
q(t) que se modifica en cada intervalo de control.
Hay que hacer notar que el paso 3 debe tratarse con cuidado, pues hay que tener
en cuenta las posibles soluciones múltiples de la transformación inversa, como se vio en
el ejemplo 2.6 de la práctica 2, así como la posible existencia de configuraciones
singulares que impidan la continuidad de la trayectoria deseada.
Una posible variación de este esquema es realizar el estudio de las necesidades
de movimiento en el espacio de las articulaciones del robot, con la ventaja de que se
está realizando sobre las variables a controlar directamente y que se evita la utilización

intensiva de las transformaciones homogéneas inversas, pero tiene la dificultad de que
es difícil realizar un movimiento libre de colisiones al ser éstas difíciles de detectar
trabajando con coordenadas articulares. Además las coordenadas articulares no
distinguen entre posición y orientación.
Si consideramos el planificador de trayectorias como un bloque de control,
encontramos el esquema siguiente:
Figura 5.1. Esquema general del control cinemático.
5.2.1.- Espacio cartesiano y Espacio articular
La siguiente figura pretende clarificar al lector el esquema de planificación de
trayectorias presentado. En ella se muestra un ejemplo de un robot de 2 gdl. Se quiere
que el robot se mueva en línea recta desde la posición cartesiana j1
hasta j4
. Para ello se
añaden como puntos auxiliares j2
y j3
en el espacio cartesiano. Cada uno de estos puntos
nudo se pasan al espacio articular (en este caso bidimensional). El siguiente paso es
realizar la interpolación en el espacio articular, encontrando un polinomio que cumpla
con las especificaciones requeridas. La trayectoria cartesiana del robot pasará en este
caso por los puntos nudo, si bien entre ellos puede ser que no realice una trayectoria
perfectamente recta.
Necesidades de movimiento
(libre de colisiones)
Coordenadas cartesianas
(x,y,z,α,β,γ)
GENERADOR DE
TRAYECTORIAS
Trayectorias articulares
)(),(),( tqtqtq iii

• Restricciones de los
actuadores.
• Modelo cinemático.

Figura 5.2. Espacio cartesiano y espacio articular.
5.3.-Tipos de Trayectorias.
La mejora tecnológica ha permitido que los robots puedan realizar trayectorias
cada vez más complejas, al poder ser éstas calculadas previamente. A continuación se
cita brevemente una clasificación de tipos de trayectorias de robots comerciales
clásicos.
• Trayectorias punto a punto.
En este tipo de trayectorias cada articulación se mueve independientemente, sin
considerar el efecto del resto de las articulaciones. Dentro de esta tipo se engloban
las trayectorias con movimiento eje a eje y las de movimiento simultáneo de ejes.
En las trayectorias con movimiento eje a eje en primer lugar se actúa sobre un
motor, y cuando este ha finalizado su recorrido, se activa el siguiente motor. Este
tipo de movimiento tiene como única ventaja el ahorro energético.
• Trayectorias coordinadas o isocronas.
En este tipo de trayectorias se procura que el movimiento de todos los actuadores
sea coordinado e isocrona. Esto quiere decir que el actuador que tarda más tiempo
en alcanzar la posición requerida ralentiza al resto, de manera que ningún
movimiento acaba antes que el de otra articulación. El tiempo total invertido en el
movimiento es el menor posible, y los requerimientos de velocidad y aceleración de
los motores son menores que en otro tipo de movimiento. El inconveniente de este
tipo de planificadores es que la trayectoria que describe el extremo del robot es
desconocida a priori. El esquema de planificador que se explica en el siguiente
apartado corresponde a este tipo de planificadores.

• Trayectorias continuas.
En este tipo de trayectorias se pretende que el camino seguido por el extremo del
robot sea conocido. Para ello las trayectorias articulares deben acomodarse
conjuntamente. Cada articulación por separado parece tener un movimiento
desordenado, sin embargo el resultado es que el extremo se mueve siguiendo el
camino previsto.
5.4.-.Interpolación. Cálculo de una trayectoria 4-3-4.
Siguiendo con el esquema general presentado en el apartado 5.2 se va a
desarrollar en MatLab 
una herramienta de interpolación en el espacio de las variables
articulares que proporciona resultados aceptables con trayectorias de articulación
suaves. La trayectoria de la articulación se divide en algunos segmentos de trayectoria y
cada uno de éstos se ajusta mediante un polinomio de bajo grado.
Se parte del supuesto que se conoce la posición y orientación de dos puntos
nudos en coordenadas cartesianas (x,y,z,α,β,γ). Se quiere que el robot evolucione desde
el punto inicial al punto final en un tiempo conocido. Asimismo se tiene una serie de
especificaciones máximas en los motores de las articulaciones. De acuerdo con el
esquema general, el paso siguiente es obtener las coordenadas articulares de los puntos
inicial y final.
Este primer paso se resuelve con las herramientas desarrolladas en la práctica 2,
concretamente con la solución de la cinemática inversa. En el caso del robot rotacional
de 6 gdl se ha estudiado que la solución de la cinemática inversa puede ser múltiple.
Además debe tenerse en cuenta la posible existencia de singularidades en alguna de las
configuraciones del robot.
En segundo lugar, debe hallarse un polinomio interpolador entre los dos puntos
nudos que cumpla con las especificaciones de los actuadores y que proporcione un
movimiento suave al robot. Como se ha visto anteriormente, este punto puede ser
abordado bajo varios enfoques y diferentes tipos de trayectorias. Cada enfoque produce
un resultado distinto pero igualmente válido dependiendo de los requerimientos del
usuario. Se puede pretender que el robot siga una determinada trayectoria en línea recta
o mover el manipulador a lo largo de una trayectoria polinomial uniforme que satisface
las ligaduras de posición y orientación en ambos puntos extremos.
Se remite al lector a la literatura específica en la cual encontrará varios ejemplos
con distintos enfoques en los que se utiliza desde interpoladores lineales simples hasta
series de polinomios encadenados que satisfacen todas las restricciones de la trayectoria.
Se muestra a continuación un método para obtener trayectorias de articulación
interpoladas que ofrecen un resultado físicamente realizable. Los perfiles de aceleración
y velocidad obtenidos para cada articulación son realizables por actuadores comerciales.
La trayectoria obtenida es del tipo isocrona.

5.4.1.- Trayectorias de articulación interpolada
Con el objetivo de que el movimiento del robot sea suave y esté de acuerdo con
las restricciones de los actuadores se divide cada trayectoria articular en tres segmentos,
correspondientes a la aceleración del motor, el periodo de velocidad máxima y la
deceleración o frenado del motor. La gráfica siguiente muestra esta división en la que se
puede observar como los periodos de aceleración y frenado ofrecen un movimiento
suave del robot.
Las restricciones en los puntos de control que se utilizan para resolver el cálculo de los
polinomios interpoladores son las siguientes:
⇒ Posición inicial (t0)
 Posición θ(t0)
 Velocidad inicial (normalmente nula)
 Aceleración inicial (normalmente nula)
⇒ Posición intermedia (t1)
 Continuidad en posición
 Continuidad en velocidad
 Continuidad en aceleración
⇒ Posición intermedia (t2)
 Continuidad en posición
 Continuidad en velocidad
 Continuidad en aceleración
⇒ Posición final (t3)
 Velocidad final (normalmente nula)
 Aceleración final (normalmente nula)
Se pueden encontrar en la literatura específica muchos polinomios que permiten
cumplir con las anteriores restricciones. Una de estas soluciones es la combinación 4-3-
4 [1] que se explica brevemente a continuación.
Articulación i
Tiempo
Posición
t0 t1 t2 t3
θ(t0)
θ(t1)
θ(t2)
θ(t3) Aceleración
Vel.Máx.
Frenado

5.4.2.- Polinomios 4-3-4
Utilizando este tipo de polinomios interpoladores se tiene los siguientes tres
segmentos de trayectorias para cada articulación:
• 1er
segmento de trayectoria. Es un polinomio de cuarto grado que especifica el
intervalo de aceleración del motor.
1011
2
12
3
13
4
141 )( atatatatath ++++= (5.2)
• 2º segmento de trayectoria. Es un polinomio de tercer grado que especifica la
trayectoria desde la primera posición intermedia (t1) hasta la posición t2.
2021
2
22
3
232 )( atatatath +++= (5.3)
• 3er
segmento de trayectoria. Es un polinomio de cuarto grado que especifica la
trayectoria durante el período final de frenado del motor.
3031
2
32
3
33
4
343 )( atatatatath ++++= (5.4)
En primer lugar hay que considerar que al estar hallando las trayectorias
articulares para las N articulaciones del manipulador, es conveniente introducir una
variable de tiempo normalizada t∈[0,1], que permita simplificar los cálculos al evitar
tener que contabilizar los desfases temporales, de tal manera que t=0 corresponda al
inicio de cualquiera de los tres segmentos en que se ha dividido el camino y t=1
corresponda al final del segmento de trayectoria.
Matemáticamente, este cambio de variable queda expresado como:
1
1
−
−
−
−
=
i
i
i
t
ττ
ττ
(5.5)
Donde:
t variable de tiempo normalizado, t∈[0,1]
τ tiempo real en segundos
τi tiempo real la final del segmento de trayectoria i-ésimo
Las siguientes expresiones permiten obtener las relaciones necesarias para el
cálculo de los coeficientes de los tres polinomios buscados. Estas expresiones se
obtienen aplicando la regla de la cadena con el cambio de variable temporal descrito
anteriormente:
)(
1
th
t
v i
i
i
= (5.6)
)(
1
2
th
t
a i
i
i
= (5.7)

Cálculo de los coeficientes.
Primer segmento de trayectoria. Aceleración
De las ecuaciones (5.2),(5.6)y (5.7) se obtienen las expresiones:
1
11
2
12
2
13
3
14
1
1
1
234
)(
1
t
atatata
th
t
v
+++
==  (5.8)
2
1
1213
2
14
1
1
1
2612
)(
1
t
atata
th
t
a
++
==  (5.9)
aplicando las condiciones de contorno para t=0 y t=1:
0110 )0( θ== ha (5.10)
1
11
1
1
0
)0(
t
a
t
h
v ==

(5.11)
2
1
12
2
1
1
0
2)0(
t
a
t
h
a ==

(5.12)
010
2
2
103
13
4
141 )(
2
)( θ++⎟⎟
⎠
⎞
⎜⎜
⎝
⎛
++= ttvt
ta
tatath (5.13)
1
10
2
101314
1
34
)1(
t
tvtaaa
v
+++
= (5.14)
2
1
2
101314
1
612
)1(
t
taaa
a
++
= (5.15)
Segundo segmento de trayectoria. Velocidad Máxima
Aplicando las condiciones de contorno para t=0 y t=1 para los puntos intermedios se
obtienen las siguientes relaciones:
)0()0( 2220 θ== ha (5.16)
2
21
2
2
1
)0(
t
a
t
h
v ==

(5.17)
2
2
22
2
2
2
1
2)0(
t
a
t
h
a ==

(5.18)
2
212223
2
23
)1(
t
aaa
v
++
= (5.19)
2
2
2223
2
26
)1(
t
aa
a
+
= (5.20)
y aplicando continuidad:

1
1
2
2 )1()0(
t
h
t
h 
= y 2
1
1
2
2
2 )1()0(
t
h
t
h 
= (5.21)
de donde se obtienen las igualdades siguientes:
0
34
1
10
1
2
10
1
13
1
14
2
21
=++++
−
t
tv
t
ta
t
a
t
a
t
a
(5.22)
0
6122
2
1
2
10
2
1
13
2
1
14
2
2
22
=+++
−
t
ta
t
a
t
a
t
a
(5.23)
Tercer segmento de trayectoria. Frenado
Para aplicar más cómodamente las condiciones de contorno conviene realizar un cambio
de variable, sustituyendo 1−= tt por t en la ecuación (5.4), de tal manera que 0=t
corresponde al instante final del segmento y 1−=t al instante inicial. La ecuación que
gobierna este segmento pasa a formularse como:
3031
2
32
3
33
4
343 )( atatatatath ++++= con [ ]0,1−∈t (5.24)
Aplicando las condiciones para 0=t y 1−=t
fha θ== )0(330 (5.25)
3
31
3
3 )0(
t
a
t
h
vf ==

(5.26)
2
3
32
2
3
3 2)0(
t
a
t
h
af ==

(5.27)
3
2
33334
3
3
34)1(
t
tvtaaa
t
h nff +−+−
=
−
(5.28)
2
3
2
33334
2
3
3
612)1(
t
taaa
t
h f+−
=
−
(5.29)
y aplicando continuidad:
3
3
2
2 )1()1(
t
h
t
h −
=

y 2
3
3
2
2
2 )1()1(
t
h
t
h −
=

(5.30)
0
2334
2
21
2
22
2
23
3
3
2
33332
=+++
−+−
t
a
t
a
t
a
t
tvtaaa ff
(5.31)
0
66612
2
2
22
2
2
23
2
3
2
33334
=++
−+−
t
a
t
a
t
taaa f
(5.32)

Otras relaciones
El ángulo recorrido por la articulación en cada segmento puede escribirse como:
10
2
10
131411011
2
)0()1( tv
ta
aahh +++=−=−= θθδ (5.33)
21222322122 )0()1( aaahh ++=−=−= θθδ (5.34)
3
2
3
333433233
2
)1()0( tv
ta
aahh f
f
+−+−=−−=−= θθδ (5.35)
Rescribiendo en notación matricial las ecuaciones (5.22), (5.23), (5.31), (5.32), (5.33),
(5.34), (5.35) se obtiene:
Cxy = (5.36)
donde:
T
f
f
fff tv
ta
avtaavtatv
ta
y
⎟
⎟
⎠
⎞
⎜
⎜
⎝
⎛
−−+−−−−−−= 3
2
3
332001010
2
10
1
2
,,,,,,
2
δδδ
( )T
aaaaaaax 34332322211413 ,,,,,,=
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎦
⎤
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎣
⎡
−
−
−
−
−
=
1100000
12662000
4332100
0011100
00020126
0000143
0000011
2
3
2
3
2
2
2
2
33222
2
2
2
1
2
1
211
tttt
ttttt
ttt
ttt
C
Resolviendo las ecuaciones anteriores se calculan todos los coeficientes de los
tres polinomios 4-3-4 que definen la trayectoria.
5.4.3.- Código en MatLab®
Se presenta a continuación el código en MatLab®
desarrollado por los autores
que permite obtener los polinomios )(),(),( tqtqtq iii
 a partir de unas posiciones inicial
y final dadas en coordenadas articulares del robot y de las características de los
actuadores.
Por defecto se ha considerado que la articulación con una solicitud temporal más
exigente frenará al resto de actuadores, trazándose por tanto la trayectoria más corta
posible entre los puntos inicial y final. Para ello se ha escrito la función
SINCRONIZADOR que calcula la nueva velocidad máxima de cada motor necesaria
para sincronizar las trayectorias articulares. Conocidas las posiciones iniciales y finales
de las articulaciones y la velocidad máxima de los motores que las accionan, se

recalculan estas velocidades máximas admisibles que proporcionan un movimiento
coordinado. La función SINCRONIZADOR se muestra a continuación:
% SINCRONIZADOR Funcion que sincroniza los movimientos teniendo en
consideración
% las condiciones iniciales, finales, características de las articulaciones y
% velocidades nominales.
% [VELO2,TMAXIMO]=SINCRONIZADOR(Q0,QF,VELO) devuelve la nueva velocidad
% máxima de cada
% motor.
% Q0 posición inicial (coordenadas articulares).
% QF posición final (coordenadas articulares).
% VELO velocidad máxima nominal de cada actuador.
%
% Ver también PLANIFICADOR, CALCULOCOEF, SINCRONIZADOR, EVALPOS, EVALACEL.
function [velo2,tmaximo]=sincronizador(q0,qf,velo)
%--Calculo de tiempos aproximados.
taprox = abs((qf(:,1)-q0(:,1)))./velo;
tmaximo = max(taprox);
%--Nueva velocidad maxima de cada motor.
velo2=(qf(:,1)-q0(:,1))/tmaximo;
return
En la función CALCULOCOEF que se muestra a continuación se han
contemplado los casos en que un motor no necesite alcanzar su velocidad máxima
durante la trayectoria, debido a que el recorrido que debe efectuar es pequeño (o incluso
nulo). En estos casos el perfil de velocidades no tendrá la típica forma de trapecio, dado
que los tres segmentos de trayectoria se reducirán a dos.
% CALCULOCOEF Función de cálculo de los coeficientes de los polinomios 4-3-4.
%
% [CASO, A ,TT]=CALCULOCOEF(ELEM,VEL,Q0,QF,TMOTOR) calcula los
coeficientes
% de los polinomios interpolados 4-3-4 en base a las especificaciones de
% los actuadores y los puntos de inicio y final.
% La matriz A de 5x3 contiene los coeficientes de los polinomios interpolados.
% TT es el vector 3x1 de los intervalos de tiempo de aceleración, velocidad
% maxima y deceleración de los motores.
% caso=1: Cada motor logra alcanzar su velocidad maxima.
% caso=2: No hay tiempo de alcanzar la velocidad maxima de cada motor.
%
% Ver también PLANIFICADOR, SINCRONIZADOR, EVALPOS, EVALVEL, EVALACEL.
function [caso,A,tt] = calculocoef(elem,vel,q0,qf,tmotor)
%---------------------------------------------------------------------
%Tiempos de respuesta del motor (aceleración y frenado)
%---------------------------------------------------------------------
ti = tmotor(elem,1);
tf = tmotor(elem,2);
%---------------------------------------------------------------------
%Cálculo de los coeficientes.
%---------------------------------------------------------------------

if vel(elem) ~= 0
%-Determinacion del caso
desp = (qf(elem,1) - q0(elem,1));
ttot = abs(desp/vel(elem));
if ttot > (ti + tf)
caso = 1;
else
caso = 2;
end;
%****************************** CASO 1********************************
if caso == 1
%vector de tiempo: Arranque - Velocidad Max - Desaceleracion
tt = [ ti ttot-(ti+tf) tf ];
%Determinacion de los coeficientes polinomiales
A = zeros(3,5);
%Coeficientes del polinomio de posicion
A(1,1) = q0(elem,1);
A(1,2) = q0(elem,2)*tt(1);
A(1,3) = q0(elem,3)*tt(1)^2/2;
A(1,4) = tt(1)*vel(elem) - A(1,2) - 4*A(1,3)/3;
A(1,5) = -tt(1)*vel(elem)/2 + A(1,2)/2 + A(1,3)/2;
%Coeficientes del polinomio de aceleracion
A(3,1) = qf(elem,1);
A(3,2) = qf(elem,2)*tt(3);
A(3,3) = qf(elem,3)*tt(3)^2/2;
A(3,4) = tt(3)*vel(elem) - A(3,2) + 4*A(3,3)/3;
A(3,5) = (tt(3)*vel(elem) - A(3,2) + A(3,3))/2;
%Espacio recorrido en los anteriores intervalos
x1 = A(1,2) + A(1,3) + A(1,4) + A(1,5);
x3 = A(3,2) - A(3,3) + A(3,4) - A(3,5);
x2 = qf(elem,1) - q0(elem,1) - ( x1 + x3);
%Tiempo real a velocidad maxima.
tt(2) = x2/vel(elem);
%Coeficientes del polinomio de velocidad.
A(2,1) = A(1,1) + A(1,2) + A(1,3) + A(1,4) + A(1,5);
A(2,2) = vel(elem)*tt(2);
%******************************** CASO 2******************************
elseif caso == 2
t = (ti + tf)/2;
tt = [ t t ];
A = zeros(2,5);
% Arranque del motor.
A(1,1) = q0(elem,1);
A(1,2) = q0(elem,2)*tt(1);
A(1,3) = q0(elem,3)*tt(1)^2/2;
% Desaceleracion del motor.
A(2,1) = qf(elem,1);

A(2,2) = qf(elem,2)*tt(2);
A(2,3) = qf(elem,3)*tt(2)^2/2;
% Los polinomios para este caso son de quinto orden.
B = [ 1 1 1 -1;
6/tt(1)^2 12/tt(1)^2 0 0;
3/tt(1) 4/tt(1) -3/tt(2) 4/tt(2);
0 0 -6/tt(2)^2 12/tt(2)^2 ];
b = [ -A(1,1) + A(1,2) + A(1,3) + A(2,1) - A(2,2) + A(2,3);
-2*A(1,3)/tt(1)^2;
-(A(1,2) + 2*A(1,3))/tt(1) + (A(2,2) - 2*A(2,3))/tt(2);
-2*A(2,3)/tt(2)^2 ];
x = inv(B)*b;
% Coeficientes 4 y 5 de cada segmento.
A(1,4) = x(1);
A(1,5) = x(2);
A(2,4) = x(3);
A(2,5) = x(4);
end;
% Para cuando el motor no se mueve.
else
caso = 2;
tt = [0 0 0];
A = zeros(2,5);
end;
return
⇒ El lector debe notar la redefinición del segundo elemento del vector TT que
define los intervalos de tiempo de aceleración, velocidad máxima y frenado en el caso 1,
ajustando la aproximación que se había realizado con la variable TTOT.
La función PLANIFICADOR hace uso de las funciones definidas anteriormente,
y permite obtener los polinomios )(),(),( tqtqtq iii
 . Debe observarse que en el interior
de esta función se encuentran los parámetros de los actuadores del robot. Se han
introducido valores estándar para articulaciones robotizadas como son velocidades
máximas de π/3 radianes/seg. (1.0472 rad/seg.) y tiempos de aceleración y frenado de
0.1 seg.

% PLANIFICADOR Planificador de trayectorias con interpolador 4-3-4.
%
% [T,POS_PLAN, VEL_PLAN, ACE_PLAN]=PLANIFICADOR(Q1,Q2) calcula las
matrices
% de posición, velocidad y aceleración de los puntos nudo que representan
% la planficiación de trayectorias entre los punto Q1 y Q2 cumpliendo con
% las restricciones de trayectoria suave y prestaciones de los actuadores.
% Utiliza los polinomios 4-3-4 en los tres segmentos de trayectoria.
%
% Ver también CALCULOCOEF, SINCRONIZADOR, EVALPOS, EVALVEL, EVALACEL.
function [t, pos_plan, vel_plan, ace_plan] = planificador(q1,q2)
%**********************parámetros de los accionamientos**************
%---------------------------------------------------------------------
% Especificaciones de los tiempos de arranque y frenado de cada motor.
%---------------------------------------------------------------------
tmotor = 0.1*ones(6,2);
%---------------------------------------------------------------------
%Velocidades Maxima de cada motor.
%---------------------------------------------------------------------
velmax = [1.0472;1.0472;1.0472;1.0472;1.0472;1.0472];
%**********************planificador coordinado***********************
%---------------------------------------------------------------------
% Inicialización de los vectores posicion - Velocidad - aceleracion.
%---------------------------------------------------------------------
q = zeros(6,1);
q0 = [q1 q q];
qf = [q2 q q];
%---------------------------------------------------------------------
% Sincronización de los motores para que realizen un movimiento
coordinado
%---------------------------------------------------------------------
[velo2,tmaximo]=sincronizador(q0,qf,velmax);
%---------------------------------------------------------------------
%Inicialización de la escala de tiempo y las matrices.
%---------------------------------------------------------------------
t = 0:0.01:(tmaximo+0.15);
% +0.15 se suma con el fin de aumentar el intervalo de tiempo y
muestrear
% todo el intervalo de frenado de la articulación, asumiendo las
% aproximaciones realizadas en la función SINCRONIZADOR.
ini=zeros(length(t),1);
pos_plan(:,1)=ini;
vel_plan(:,1)=ini;
ace_plan(:,1)=ini;
%---------------------------------------------------------------------

% Cálculo de los coeficientes de los polinomios y evaluación de los
% polinomios de interpolación.
%---------------------------------------------------------------------
for i = 1:6
[caso,A,tt] = calculocoef(i,velo2,q0,qf,tmotor);
posi=evalpos(t,tt,caso,A);
pos_plan(:,i)=posi';
ve=evalvel(t,tt,caso,A);
vel_plan(:,i)=ve';
ace=evalacel(t,tt,caso,A);
ace_plan(:,i)=ace';
end;
return
Finalmente se han desarrollado unas funciones que evalúan los polinomios en
los puntos estudiados de acuerdo a los coeficientes calculados, permitiendo la
representación gráfica de los resultados. Estas funciones son EVALPOS, EVALVEL y
EVALACEL. Se muestra a continuación la función EVALPOS que evalúa la posición
de la articulación. Las otras funciones las proporcionará el profesor en durante la sesión
de prácticas.
% EVALPOS Funcion que evalua el polinomio de la funcion de posición de
% acuerdo al caso y al tiempo.
% POS=EVALPOS(T,TT,CASO,A) evalúa el polinomio de aceleración.
% A matriz 5x3 que contiene los coeficientes de los polinomios
interpolados.
% T vector tiempo normalizado.
% TT segmentos de tiempo de aceleración, velocidad máxima y deceleración.
% caso=1: Cada motor logra alcanzar su velocidad maxima.
% caso=2: No hay tiempo de alcanzar la velocidad maxima de cada motor.
%
% Ver también PLANIFICADOR, CALCULOCOEF, SINCRONIZADOR, EVALVEL, EVALACEL.
function pos = evalpos(t,tt,caso,A)
for i = 1:length(t)
if caso == 1
if (t(i) <= 0)
p = A(1,1);
elseif (t(i)>0)&(t(i)<=tt(1))
ti = t(i)/tt(1);
p = A(1,1)+ A(1,2)*ti+ A(1,3)*ti^2+A(1,4)*ti^3+A(1,5)*ti^4;
elseif (t(i)>tt(1))&(t(i)<=tt(2)+tt(1))
ti = (t(i)-tt(1))/tt(2);
p = A(2,1)+A(2,2)*ti;
elseif (t(i)>tt(2)+tt(1))&(t(i)<=tt(3)+tt(2)+tt(1))
ti = (t(i)-tt(2)-tt(1))/tt(3);
p = A(3,1)+A(3,2)*(ti-1)+A(3,3)*(ti-1).^2+A(3,4)*(ti-
1).^3+A(3,5)*(ti-1).^4;
elseif (t(i)>tt(3)+tt(2)+tt(1))
p = A(3,1);
end;
elseif caso==2
if (t(i) <= 0)
p = A(1,1);

elseif (t(i)>0)&(t(i)<=tt(1))
ti = t(i)/tt(1);
p = A(1,1)+ A(1,2)*ti+ A(1,3)*ti^2+A(1,4)*ti^3+A(1,5)*ti^4;
elseif (t(i)>tt(1))&(t(i)<=tt(2)+tt(1))
ti = (t(i)-tt(1))/tt(2);
p = A(2,1)+A(2,2)*(ti-1)+A(2,3)*(ti-1).^2+A(2,4)*(ti-
1).^3+A(2,5)*(ti-1).^4;
elseif (t(i)>tt(2)+tt(1))
p = A(2,1);
end;
end;
pos(i)=p;
end;
return
Para comprobar el funcionamiento del código desarrollado se sugiere al lector ejecute
varias veces el fichero ejemplo.m que se encuentra en el directorio /Capítulo5 en el que
se generan dos vectores aleatorios que definen dos posiciones de un robot de 6 grados
de libertad, se calculan las trayectorias articulares haciendo uso de las funciones
estudiadas y se grafican los polinomios )(),(),( tqtqtq iii
 . Debe notarse que para cada
ejecución, y dependiendo del máximo recorrido que deba efectuar la articulación más
desfavorable en este sentido, el tiempo que tardará en alcanzar la posición final es
diferente.
Ejemplo 5.1
Al ejecutar ejemplo.m se obtiene las gráficas de la posición, velocidad y
aceleración de las seis articulaciones de un robot de 6 gdl que evolucione entre dos
puntos aleatorios de su espacio de trabajo.
%----------------------------------------------------------
%----programa de prueba del software de planificación -----
%---- interpolación 4-3-4 -----
%----------------------------------------------------------
clear
%---------------------------------------------------------------------
% q1 y q2 son las coordenadas articulares inicial y final
%---------------------------------------------------------------------
disp([' ']);
disp([' Vectores q1 y q2 de las coordenadas articulares ']);
disp([' inicial y final. ']);
q1 = rand(6,1)
q2 = rand(6,1)
%---------------------------------------------------------------------
% Llamada a la función PLANIFICADOR
%---------------------------------------------------------------------
[t,pos, vel, ace] = planificador(q1,q2);

%---------------------------------------------------------------------
% gráficas de los resultados
%---------------------------------------------------------------------
figure
plot(t,pos)
grid
title('Perfil de posición dada por el planificador')
xlabel('Tiempo (seg)'), ylabel('Posición (rad)')
figure
plot(t,vel)
grid
title('Perfil de Velocidad dada por el planificador')
xlabel('Tiempo (seg)'), ylabel('Velocidad (rad/seg)')
figure
plot(t,ace)
grid
title('Perfil de Aceleracion Dada por el planificador')
xlabel('Tiempo (seg)'), ylabel('Aceleracion (rad/seg2)')
Las siguientes figuras muestran ejemplos de ejecución del fichero ejemplo.m en
los que se puede observar varios casos.

Figura 5.3.- Ejemplos de planificador coordinado.

⇒ Se recomienda al lector que realice varios ejemplos observando que la
velocidad máxima siempre es la nominal de la articulación que mayores solicitudes
temporales tiene. Es igualmente recomendable que el estudiante introduzca vectores q1
y q2 definidos por él, que definan posiciones fácilmente identificables y que observe las
gráficas que resultan. Por ejemplo puede hacerse partir todas las articulaciones de su
posición cero (q1=[0 0 0 0 0 0 ]T
) y observar lo que sucede con cada articulación.
5.5.-PRACTICA. Animación de la trayectoria.
En este apartado se ilustra la herramienta teórica que se ha estudiado con los
ejemplos de los robots de 4 y 6 gdl que sirven de apoyo a lo largo del libro.
Ejemplo 5.2
En este ejemplo se implementa en MatLab
un planificador de trayectorias
coordinado. Se utiliza el robot rotacional de 6 gdl de la figura 5.4
Basándose en el esquema general presentado en el apartado 5.2, el primer paso
es estudiar las necesidades de movimiento, especificar un punto inicial (posición y
orientación) y un punto final (posición y orientación) y obtener las coordenadas
articulares correspondientes a estos puntos nudo. Para ello se utilizan las funciones
desarrolladas en la práctica 2 DENAVIT y INVERSEKINEMATIC6.
Hay que recordar en este punto lo estudiado
en la práctica 2 sobre la posible existencia de
múltiples soluciones en la cinemática inversa
debido a las posibles configuraciones del codo y la
muñeca del manipulador.
Se parte de dos puntos del espacio de
trabajo del robot definidos por :
P1=(x1,y1,z1,α1,β1,γ1)
P2=(x2,y2,z2,α2,β2,γ2)
A partir de estos puntos se obtienen los vectores en
coordenadas articulares correspondientes a los
puntos inicial y final del camino que se quiere
recorrer.
s
a
n
Figura 5.4.- Robot
rotacional de 6 gdl

El siguiente ejemplo muestra como se ha utilizado la función
INVERSEKINEMATIC6 para obtener los vectores q1 y q2 a partir de unas matrices
homogéneas T1 y T2 que definen en coordenadas cartesianas la posición y orientación
de dos puntos en el espacio de trabajo del robot.
A partir de los vectores q1 y q2 se realiza un ejecuta el planificador que me
proporciona los polinomios )(),(),( tqtqtq iii
 con las trayectorias articulares.
» T1=[0 1 0 -0.3;1 0 0 -0.2;0 0 1 0.6; 0 0 0 1]
T1 =
0 1.0000 0 -0.3000
1.0000 0 0 -0.2000
0 0 1.0000 0.6000
0 0 0 1.0000
» T2=[0 0 1 -0.3;0 1 0 0.4;1 0 0 0.5; 0 0 0 1]
T2 =
0 0 1.0000 -0.3000
0 1.0000 0 0.4000
1.0000 0 0 0.5000
0 0 0 1.0000
» q1=inversekinematic6(T1,-1,1)
q1 =
-2.5536
0.7137
-0.6729
0.0000
-0.0408
-0.9828
» q2=inversekinematic6(T2,-1,1)
q2 =
2.3306
0.6579
-0.2558
-2.2892
1.8437
-0.2990
»
» [t,pos, vel, ace] = planificador(q1,q2);
»

Las polinomios pueden graficarse directamente en MatLab®
mediante la orden
plot.
Figura 5.5.- Gráficas de la planificación del robot de 6 gdl
» plot(t,pos)
» plot(t,vel)
» plot(t,ace)

Debe notarse que en este caso el tiempo invertido en realizar la trayectoria es de
casi 5 segundos, debido a que la articulación 1 realiza un recorrido de más de 1.5π
radianes, siendo la velocidad máxima del motor que mueve esta articulación de 1/3π
rad/seg.
Utilizando la misma función ANIMACION6 que se utilizó en la práctica 2 se
puede visualizar el movimiento del robot observando el control de velocidad de las
articulaciones.
Se debe observar que en esta ocasión se introduce como mat_q la traspuesta de
la matriz pos que se ha obtenido del planificador, pues está ordenada de diferente
manera que la matriz utilizada en la práctica 2.
Las siguientes imágenes muestran alguna secuencia de la animación calculada.
Figura 5.6.- Animación de la planificación del robot de 6 gdl
» animacion6(pos')

Práctica 6
Simulación y Control de Robots
6.1.-Introducción
En los últimos años han aparecido numerosos proyectos de investigación
relacionados con disciplinas de simulación virtual. Una correcta simulación dinámica es
necesaria como primer paso para la obtención de herramientas capaces de ser utilizadas
para el análisis y diseño de robots.
Además de necesitar una metodología numéricamente correcta, es de gran
importancia el disponer de unas herramientas de visualización que permitan comprobar
los resultados obtenidos, de manera que la detección de errores sea lo más intuitiva
posible. Siguiendo esta idea, en esta práctica se va a presentar una serie de ejemplos en
los que se utilizan los modelos dinámicos presentados anteriormente con la finalidad de
poder realizar un correcto control del robot estudiado.
En esta práctica se detalla la simulación de un robot efectuando una
planificación del extremo del robot en línea recta entre dos posiciones del espacio
cartesiano. Se va ha realizar el ejemplo con el robot de 4 gdl, dejando como práctica
para el alumno la realización de los mismos ejercicios para el robot de 6 gdl.
6.2.-Sintonizado de los motores.
Una vez que se han seleccionado los servoaccionamientos para cada una de las
articulaciones del robot (practica 4), el paso siguiente es realizar un sintonizado
adecuado de los mismos con el fin de que satisfagan un determinado comportamiento.
Para realizar la sintonización en primer lugar se debe especificar cuál es la estructura de
control a utilizar, detallando los controladores a utilizar. A continuación, mediante una
determinada técnica de control, se realizará el sintonizado de los motores ajustando los
valores de los controladores especificados. Para efectuar el sintonizado de los motores
haremos uso del modelo dinámico directo del robot.

6.2.1. Estructuras de control de robots.
Hay dos estructuras de control típicas utilizadas en los robots: control acoplado y
control desacoplado.
• Control desacoplado
En el control desacoplado se considera que las articulaciones del robot están
desacopladas, de modo que un par en un determinado actuador únicamente tendrá
efecto sobre el movimiento de la articulación correspondiente. De esta forma
existirá un controlador para cada articulación. La ventaja del control desacoplado
radica en que el diseño del regulador más adecuado para cada articulación puede
hacerse utilizando las técnicas más frecuentes de diseño.
• Control acoplado
En ocasiones, la suposición de que el robot es una serie de eslabones dinámicamente
desacoplados, de forma que el movimiento de uno de ellos no afecta a los demás, no
es siempre aceptable. Las técnicas de control acoplado consideran el modelo
dinámico real del robot, haciendo uso del conocimiento del mismo para tratar de
desacoplar el sistema.
Adoptaremos una estructura de control desacoplado, de manera que en cada articulación
existe un regulador PID. Durante el sintonizado de una articulación, los actuadores del
resto de articulaciones permanecen parados. En la figura 6.1 se muestra la estructura de
control desacoplado típica de un robot de tres grados de libertad.
Figura 6.1. Estructura de control desacoplado de un robot de 3 grados de libertad. En este caso, la entrada
a cada articulación es un escalón.

6.2.2. Técnicas de sintonizado.
El sintonizado de un robot consiste en ajustar cada uno de los controladores para que el
robot satisfaga un comportamiento especificado. Para ello, en un esquema de control
desacoplado, se deberá sintonizar de manera independiente cada articulación del robot.
El sintonizado de una articulación consiste en ajustar los parámetros del regulador que
actúa sobre ella para que su respuesta cumpla unas determinadas especificaciones.
El controlador que utilizaremos en cada una de las articulaciones será un regulador PID.
La función de transferencia de un regulador PID es la siguiente:
Ds
s
I
Psk
s
k
ksT
sT
ksG d
i
pd
i
pPID ++=++=++= )1()
1
1()( (6.1)
Para realizar el ajuste de los parámetros de un regulador PID existen múltiples métodos.
A continuación se van a estudiar dos métodos muy utilizados: el manual y el de Ziegler-
Nichols.
• Método manual (prueba y error)
Este método propone una manera de ir modificando los parámetros del regulador para
conseguir que la respuesta de la articulación cumple las especificaciones impuestas.
1- En primer lugar se aumenta la constante proporcional P hasta obtener el tiempo de
cruce deseado (el menor posible sin tener una sobreoscilación exagerada).
2- A continuación se procede a aumentar la constante derivativa D para disminuir la
sobreoscilación ( esto incrementará el tiempo de cruce).
3- Por último se aumenta la constante integral I para eliminar el error que exista en
régimen permanente (esto incrementará la sobreoscilación y disminuirá el tiempo de
respuesta).
• Método de Ziegler-Nichols
Es un método experimental en el que es necesario que la respuesta de la articulación en
bucle cerrado ante escalón sea periódica. Para ello solamente se introduce una ganancia
proporcional que se va aumentando hasta conseguir la respuesta periódica. Si no se
consigue que la respuesta sea periódica, no se puede aplicar este método.
Sea kcr la ganancia proporcional crítica a la cual la respuesta del sistema se vuelve
periódica y Pcr el período de la respuesta. En base a estos valores, el método de Ziegler-
Nichols proporciona los valores del regulador que aseguran una respuesta aceptable.
Los valores que proporciona este método se muestran en la tabla 6.1.
kp =0.6 kcr
Ti = 0.5 Pcr
Td = 0.125 Pcr
Tabla 6.1. Valores de los parámetros de un regulador PID
proporcionados por el método de Ziegler-Nichols.

Hay que destacar que es posible que la respuesta obtenida aplicando los valores
obtenidos por este método no satisfaga las especificaciones impuestas, por lo que será
necesario modificar estos valores mediante prueba y error. La utilidad de este método es
que nos proporciona un punto de partida para realizar el ajuste de los parámetros.
Ejemplo 6.1
Sintonizado del robot de cuatro grados de libertad.
Vamos a suponer que el esquema de control de este robot es desacoplado, por lo que
cada articulación dispondrá de un regulador PID y durante el proceso de sintonizado de
una articulación concreta, los actuadores del resto de articulaciones permanecerán
apagados.
Para efectuar el sintonizado del robot hay que situarlo en la peor posición, introducir a
la articulación que se desea ajustar un escalón y comprobar que la respuesta de la
articulación cumple las especificaciones deseadas. La peor posición en la que se puede
encontrar el robot viene dada por:
( )02.110=q
Para un funcionamiento satisfactorio del robot exigiremos que la respuesta de cada
articulación cumpla las siguientes especificaciones:
- Tiempo de cruce: < 100 mseg
- Sobreoscilación: < 20 %
- Tiempo de establecimiento: < 200 mseg
La entrada a las articulaciones rotacionales (articulación 1 y 4) para realizar el
sintonizado será un escalón de 0.08 rad. En las articulaciones prismáticas se introducirá
un escalón de 100mm (0.1m) para sintonizar los reguladores. El escalón será positivo o
negativo en función de la articulación que se desee sintonizar. El escalón siempre debe
elegirse de modo que precise los máximos requerimientos de par. En el caso de las
articulaciones 1 y 4 es independiente considerar el escalón positivo o negativo, ya que a
efectos de requerimientos de par, no hay diferencia en girar la articulación en un sentido
o en otro. En el caso de las articulaciones 2 y 3 el máximo requerimiento de par se
produciría cuando la articulación se desplace en el sentido positivo del eje z. Sin
embargo, puesto que la peor posición del robot se produce cuando la articulación 2 y 3
poseen su extensión máxima, no es posible introducir el escalón en el sentido positivo
del eje z. Por esta razón, en las articulaciones 2 y 3 se ha introducido un escalón
negativo.
Se ha desarrollado en Simulink
un esquema de control en bucle cerrado, para cada una
de las articulaciones, que permite ajustar un regulador PID y comprobar si la respuesta
del sistema ante una entrada escalón satisface las especificaciones impuestas.

Motor 1
Para realizar el sintonizado del motor 1 se ha desarrollado en Simulink
el modelo que
se encuentra en el fichero sintonizar1d4.mdl. Este fichero contiene el esquema de
control en bucle cerrado de la primera articulación que permite ajustar su regulador PID
para que la respuesta satisfaga las especificaciones. Si se ejecuta desde el entorno de
Matlab “sintonizar1d4” aparecerá en pantalla la aplicación para el sintonizado de la
primera articulación, que se muestra en la figura 6.2.
Figura 6.2. Herramienta desarrollada en Simulink
para realizar el sintonizado de la primera articulación.
Para obtener la aceleración de cada articulación se ha desarrollado en Matlab
la función
dindir4gdl.m; que calcula la aceleración de cada articulación cuando el robot transporta
una determinada carga en función de la posición y velocidad actual de la articulación y
de su fuerza/par de entrada. Esta función hace uso de la función walkerorin4.m,
explicada en la práctica 3. El código fuente de dindir4gdl.m se muestra a continuación:
% DINDIR4GDL Dinámica directa de un robot de 4GDL.
% QPP = DINDIR4GDL(ENTRADA) calcula el vector 4x1 de aceleración de
% cada articulación utilizando el tercer método de Walker y Orin.
% ENTRADA(1:4) representa el par de entrada a cada articulación.
% ENTRADA(5:8) es la posición de cada articulación. ENTRADA(9:12)
% es la velocidad de cada articulación.
%
% Ver también WALKERORIN4.
function qpp = dindir4gdl(entrada)
tau = entrada(1:4); % Par de entrada a cada articulación
q = entrada(5:8); % Posición de cada articulación
qp = entrada(9:12); % Velocidad de cada articulación
% Parámetros de la carga
masaext = 10;
inerciaext = [0.0167 0 0;0 0.0167 0;0 0 0.0167];
% Se convierten los pares de la articulación 2 y 3 en fuerzas.
n = 0.85; % Eficiencia husillo
p = 0.025; % Paso del husillo (mm)
tau(2:3) = 2*pi*tau(2:3)/p;
% Se calcula la aceleración utilizando el método de Walker y Orin.
qpp = walkerorin4(q,qp,tau,masaext,inerciaext);

En la función dindir4gdl.m el par proporcionado por los motores 2 y 3 se convierte a
fuerza, ya que las articulaciones 2 y 3 son prismáticas, utilizando la siguiente ecuación:
p
nτπ2
F =
donde τ es el par (Nm), p es el paso del husillo (m) y n es la eficiencia del husillo.
Las entradas al esquema de control son:
- El escalón de posición que se aplica a la primera articulación. El escalón es de 0.08
rad y, puesto que para esta articulación es independiente que sea positivo o negativo,
se ha considerado positivo. El valor de la posición de esta primera articulación en la
peor configuración del robot es 0 rad, por lo tanto el escalón será de 0 rad a 0.08 rad.
- Los pares del resto de motores. Puesto que los actuadores de las articulaciones que
no están siendo sintonizadas son apagadas, los pares del resto de motores son nulos.
- La posición del resto de articulaciones en su peor configuración.
- La velocidad de las restantes articulaciones. La velocidad de las restantes
articulaciones siempre será cero, puesto que sus actuadores están apagados.
El bloque PID representa a un regulador convencional PID con la siguiente función de
transferencia:
Ds
s
I
PsGPID ++=)(
Asignando los valores adecuados a P, I y D se conseguirá que la respuesta del sistema
sea la deseada.
En el bloque correspondiente al motor de la articulación 1 se deben colocar los datos de
los parámetros del motor que se seleccionó en la practica 4 para esa articulación. Estos
datos se muestran en la tabla 6.2.
1 DA23GBB 0.8 0.93 0.058 0.058 18.5
Tabla 6.2. Valores de los parámetros del motor utilizado para la primera articulación.
Los bloques Ordena Pares, Ordena Posiciones y Ordena Velocidades, simplemente
indican el orden del vector de pares, posiciones y velocidades del robot. En este caso los
vectores poseen el orden adecuado, ya que se está sintonizando la primera articulación,
de manera que el orden que indicaremos en los tres selectores será [1 2 3 4]. Sin
embargo, veremos que esto no es así para el sintonizado del resto de articulaciones.

Puesto que estamos ajustando la primera articulación, la única aceleración que nos
interesa es la primera, por lo que a continuación del bloque Robot 4 GDL, que
corresponde a la dinámica directa del robot, utilizamos un selector para elegir la
aceleración de la primera articulación. A continuación, se emplea un bloque integrador
para obtener la velocidad de la articulación. En este bloque especificamos que la
condición inicial es cero, ya que la velocidad inicial de esta articulación es de 0 rad/s.
Para calcular la posición de la articulación se integra la velocidad utilizando otro
bloque integrador. En este integrador la condición inicial será cero, puesto que la
posición inicial de la articulación es 0 rad.
Antes de realizar la simulación del modelo para determinar si la respuesta del sistema es
la adecuada, es necesario configurar los parámetros de la simulación con los valores que
se muestran en la figura 6.3.
Figura 6.3. Configuración de los parámetros de la simulación.
Cuando finalice la simulación pulsando dos veces con el ratón sobre el bloque Gráfica
Respuesta aparece la posición de la articulación y el escalón introducido.
En el caso de esta articulación, no ha sido posible realizar el sintonizado que cumpla las
especificaciones establecidas. Esto se debe a que el motor elegido responde más lento
de lo esperado. Para solucionar este problema se va a seleccionar un motor que
proporciona un par mayor. Las características de este motor se muestran en la tabla 6.3.
1 DA23JBB 0.39 0.65 0.054 0.054 25.3
Tabla 6.3. Valores de los parámetros del motor utilizado para la primera articulación.
Con el motor seleccionado ya es posible realizar el sintonizado de forma que se
satisfagan las especificaciones. Un posible regulador que proporciona que la respuesta
de la primera articulación se ajuste a las especificaciones establecidas es el siguiente:
ssPIDG 6.190)( +=

En la figura 6.4 se muestra como varía la posición de la articulación 1 ante el escalón
introducido. Como se puede apreciar en la gráfica, con el regulador escogido se
satisfacen los requerimientos impuestos.
Figura 6.4. Posición de la articulación 1 ante entrada en escalón.
Motor 2
Para realizar el sintonizado del motor 2 se utiliza el modelo implementado en Simulink
que se encuentra en el fichero sintonizar2d4.mdl. Este modelo contiene el esquema de
control en bucle cerrado de la segunda articulación que permite ajustar su regulador PID
para que la respuesta satisfaga las especificaciones.
- El escalón de posición que se aplica a la segunda articulación. Este escalón es de
100 mm. El valor de la posición de esta articulación en la peor configuración del
robot es 1 m. El mayor requerimiento de par se produce cuando el escalón es
positivo. Sin embargo como la articulación posee su extensión máxima, el escalón
no es realizable físicamente, por lo que se introducirá un escalón negativo. Por lo
tanto el escalón será de 1 m a 0.9 m.
- Los pares del resto de motores. Al igual que en el caso anterior, los pares del resto
de motores son nulos.
- La velocidad de las restantes articulaciones. Como en el caso anterior, la velocidad
de las restantes articulaciones es cero.
los parámetros del motor que se seleccionó en la práctica 4 para esa articulación.
2 DA34HBB 1.6 1.56 0.176 0.176 23.7

En los bloques Ordena Pares, Ordena Posiciones y Ordena Velocidades se indica el
orden del vector de pares, posiciones y velocidades del robot. El orden que indicaremos
en los tres selectores será [2 1 3 4].
Puesto que se está ajustando la segunda articulación, a continuación del bloque Robot 4
GDL, se indicará en el selector que se elige la aceleración de la segunda articulación. En
el bloque Integrador Aceleración especificamos que la condición inicial es cero, ya que
la velocidad inicial de esta articulación es de 0 m/s. En el bloque Integrador Velocidad
se indica que la condición inicial es 1, puesto que la posición inicial de la articulación es
1 m.
Un posible regulador que proporciona que la respuesta de la segunda articulación se
ajuste a las especificaciones establecidas es el siguiente:
ssPIDG 6300)( +=
Si se realiza la simulación del modelo se obtiene la gráfica que muestra como varía la
posición de la articulación 2 ante el escalón introducido. Como se puede apreciar en la
gráfica, al igual que en el caso anterior, con el regulador escogido se satisfacen los
requerimientos impuestos. Ver figura 6.5.
Motor 3
Para realizar el sintonizado del motor 3 se utiliza el modelo implementado en Simulink
que se encuentra en el fichero sintonizar3d4.mdl. Este modelo contiene el esquema de
control en bucle cerrado de la tercera articulación que permite ajustar su regulador PID
para que la respuesta satisfaga las especificaciones.
- El escalón de posición que se aplica a la tercera articulación. Este escalón es de 100
mm. El valor de la posición de esta articulación en la peor configuración del robot es

1.2 m. Al igual que en el caso anterior, el mayor requerimiento de par se produce
cuando el escalón es positivo. Sin embargo como la articulación posee su extensión
máxima, el escalón no es realizable físicamente, por lo que se introducirá un escalón
negativo. Por lo tanto el escalón será de 1.2 m a 1.1 m.
- Los pares del resto de motores. Al igual que en los casos anteriores, los pares del
resto de motores son nulos.
- La velocidad de las restantes articulaciones. Como en los casos anteriores, la
velocidad de las restantes articulaciones es cero.
3 DA23GBB 0.8 0.93 0.058 0.058 18.5
Puesto que se está ajustando la tercera articulación, a continuación del bloque Robot 4
GDL, se indicará en el selector que se elige la aceleración de la tercera articulación. En
la velocidad inicial de esta articulación es de 0 m/s. En el bloque Integrador Velocidad
se indica que la condición inicial es 1.2, puesto que la posición inicial de la articulación
es 1.2 m.
Un posible regulador que proporciona que la respuesta de la segunda articulación se
ajuste a las especificaciones establecidas es el siguiente:
ssPIDG 7.5200)( +=
gráfica, al igual que en el caso anterior, con el regulador escogido se satisfacen los

Motor 4
Para realizar el sintonizado del motor 4 se utiliza el modelo que se encuentra en el
fichero sintonizar4d4.mdl. Este modelo contiene el esquema de control en bucle cerrado
de la cuarta articulación que permite ajustar su regulador PID para que la respuesta
cumpla las especificaciones.
- El escalón de posición que se aplica a la cuarta articulación. El valor de la posición
de esta articulación en la peor configuración del robot es 0 rad. Para esta
articulación es independiente que el escalón sea positivo o negativo.
Consideraremos que el escalón es de 0 rad a 0.08 rad.
- Los pares del resto de motores. Al igual que en los casos anteriores, los pares del
resto de motores son nulos.
- La velocidad de las restantes articulaciones. Como en los casos anteriores, la
velocidad de las restantes articulaciones es cero.
4 DB17CDB 6.9 1.28 0.035 0.035 3.6
Puesto que se está ajustando la cuarta articulación, a continuación del bloque Robot 4
GDL, se indicará en el selector que se elige la aceleración de la cuarta articulación. En
la velocidad inicial de esta articulación es de 0 rad/s. En el bloque Integrador Velocidad

se indica que la condición inicial es 0, puesto que la posición inicial de la articulación es
0 rad.
Un posible regulador que proporciona que la respuesta de la cuarta articulación se ajuste
a las especificaciones establecidas es el siguiente:
100)( =sPIDG
gráfica, al igual que en los casos anteriores, con el regulador escogido se satisfacen los
6.3.- PRACTICA. Simulación de robots.
Una vez que se han seleccionado los servoaccionamientos del robot y se ha efectuado el
sintonizado, ya puede realizarse una simulación completa del funcionamiento del robot.
Ejemplo 6.2
Simulación del robot de 4 GDL.
Se ha realizado la simulación del robot de 4 GDL para que realice una planificación de
su extremo en línea recta entre dos puntos del espacio cartesiano. La planificación
realizada únicamente es en posición, de manera que el robot parte de la posición inicial
con velocidad nula. Para realizar la planificación se ha dividido la trayectoria en un
número determinado de intervalos, de forma que en cada iteración de la simulación, se
proporciona la posición que debe alcanzar el extremo del robot. Para realizar la

simulación se ha desarrollado en Simulink el modelo que se encuentra en el fichero
simulador4.mdl. Ver figura 6.8.
Velocidades
Posiciones
SIM ULACIÌ N DE LA TRAY ECTORIA EN LÈNEA RECTA DEL ROBOT DE 4GDL
m at_q
T o Workspace
Sum 4
Sum 3
Sum 2
Sum 1
Saturation 4
Saturation 3
Saturation 2
Saturation 1
M AT LAB
Function
Robot 4GDL
[0 -0.6 0.5]
Posición Inicial
[0 -1 1.2]
Posición Final
Planificación
Planificación de
trayectorias
M ux
Pares
PID
PID 4
PID
PID 3
PID
PID 2
PID
PID 1
M ux
M ux
Volt
Vel
Par
M otor 4
Volt
Vel
Par
M otor 3
Volt
Vel
Par
M otor 2
Volt
Vel
Par
M otor 1
1/s
Integrator1
1/s
Integrator
Graficas
Dem ux
Dem ux
Dem ux
Dem ux1
M AT LAB
Function
Cinem ática
Inversa
25
Am plificador 4
25
Am plificador 3
25
Am plificador 2
25
Am plificador 1
Figura 6.8. Simulador del robot de 4 GDL.
Como puede apreciarse en la figura 6.8, para realizar la simulación se utiliza un
esquema de control desacoplado, de forma que cada articulación posee un regulador
PID. Los valores de los motores del robot que se utilizan para realizar la simulación son
aquellos que se emplearon en el proceso de sintonizado mostrado en el ejemplo 6.5.
Estos valores se recogen en la tabla 6.4. Así mismo los valores de los reguladores PID
son los que se obtuvieron en el ejemplo 6.1, que se muestran en la tabla 6.5.
1 DA23JBB 0.39 0.65 0.054 0.054 25.3
2 DA34HBB 1.6 1.56 0.176 0.176 23.7
3 DA23GBB 0.8 0.93 0.058 0.058 18.5
4 DB17CDB 6.9 1.28 0.035 0.035 3.6
Tabla 6.4. Motores utilizados en el simulador del robot de 4 GDL.
Articulación P I D
1 90 0 1.6
2 300 0 6
3 200 0 5.7
4 100 0 0
Tabla 6.5. Valores de los reguladores del robot de 4 GDL.
El bloque Robot 4 GDL representa la dinámica directa del robot de 4 GDL. Como se
mostró en el ejemplo 6.3, para realizar la dinámica directa se ha desarrollado en Matlab
la función dindir4gdl.m. Esta función calcula la aceleración de cada articulación
cuando el robot transporta una determinada carga en función de la posición y velocidad
actual de la articulación y de su fuerza/par de entrada.

El simulador efectúa una planificación de la trayectoria del extremo del robot en línea
recta entre la posición cartesiana inicial y final especificadas. Para realizar la
planificación de trayectorias se ha desarrollado el subsistema Planificación. Este
subsistema se muestra en la figura 6.9.
Figura 6.9. Subsistema de planificación de trayectorias.
El subsistema de planificación es el encargado de suministrar al esquema de control de
cada articulación la posición articular de cada articulación en cada instante de
simulación. Para obtener las posiciones articulares se utiliza la función implementada en
Matlab planifica4.m. El código fuente de esta función es el siguiente:
% PLANIFICA4 Planificación de trayectorias en línea recta de un robot de 4GDL
% Q = PLANIFICA4(ENTRADA) devuelve las coordenadas articulares
% correspondientes al instante actual de simulación en una planificación
% de trayectoria en línea recta entre dos puntos cartesianos. ENTRADA(1)
% representa el instante de tiempo actual (seg). ENTRADA(2:4) es la
% posición cartesiana inicial de la trayectoria. ENTRADA(5:7) es la
% posición cartesiana final de la trayectoria.
%
% Ver también CININV4GDL.
function q = planifica4(entrada)
t = entrada(1); % Instante actual de simuación (seg)
p1 = entrada(2:4); % Posición cartesiana inicial
p2 = entrada(5:7); % Posición cartesiana final
ts = 1; % Tiempo de simulación (seg)
intervalo = 1e-4; % Intervalo de integración (seg)
% Número de segmentos de la trayectoria
nseg = ts/intervalo;
% 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/nseg;
% Número de punto actual en la trayectoria (el inicial es 0)
i = t*nseg;
% Cálculo de la posición cartesiana actual de la mano del manipulador
ps = p1+(i*d)*u;
% Cálculo de las coordenadas articulares
q = cininv4gdl(ps);

Esta función proporciona las coordenadas articulares del robot del instante actual de
simulación a partir del instante de tiempo y de la posición inicial y final expresadas en
coordenadas cartesianas. La función realiza una planificación de trayectorias en línea
recta entre la posición inicial y la final. Para ello divide la trayectoria en un número de
segmentos que viene impuesto por los parámetros con los que se desea simular en
Simulink:
nintegraciódeintervalo
simulacióndetiempo
segmentosnº =
En función del instante de simulación calcula qué posición cartesiana debe tener el
extremo del robot y, realizando la cinemática inversa del robot mediante la función
cininv4gdl.m, calcula las variables articulares del robot. El código fuente de la función
cininv4gdl.m es el siguiente:
% CININV4GDL Cinemática inversa de un robot de 4GDL.
% Q = CININV4GDL(ENTRADA) devuelve el vector 4x1 de coordenadas
% articulares que contiene la solución cinemática inversa.
% ENTRADA es un vector 3x1 que representa la posición expresada
% en coordenadas cartesianas.
%
% Ver también INVERSEKINEMATIC4.
function q = cininv4gdl(entrada)
% Posición cartesiana
p = entrada;
% Orientación deseada en el extremo del robot
n = [1 0 0]';
s = [0 0 -1]';
a = [0 1 0]';
% Matriz con la orientación y posición deseadas en el extremo del robot
T = [n s a entrada];
% Se calculan las coordenadas articulares
q = inversekinematic4(T);
La función cininv4gdl.m utiliza la función inversekinematic4.m obtenida en la práctica 2
para realizar la cinemática inversa del robot de 4 GDL. Como se puede apreciar en el
código fuente presentado, se especifica la orientación deseada en el extremo del robot.
Por lo tanto la orientación del extremo del robot permanecerá fija durante toda la
planificación. Para que un robot pueda posicionarse en cualquier punto con cualquier
orientación necesita un mínimo de 6 GDL. Este robot es de 4 GDL, por lo que existen
trayectorias que no pueden realizarse si la orientación del extremo es fija. Para realizar
cualquier trayectoria correctamente sería necesario modificar en cada momento la
orientación del extremo. Es decir, habría que realizar una planificación de la posición
del extremo, así como de su orientación. Sin embargo, como la única finalidad en este
apartado es simular el comportamiento del robot de 4 GDL, la orientación del extremo
permanecerá fija, de forma que se realizarán trayectorias en las que el extremo no
cambie su orientación.

El bloque Cinemática Inversa calcula la cinemática inversa de la posición cartesiana
inicial de la trayectoria mediante la función cininv4gdl.m. Este cálculo es necesario, ya
que el integrador del modelo que obtiene las posiciones articulares del robot a partir de
las velocidades, necesita como condición inicial las variables articulares
correspondientes a la posición cartesiana inicial del extremo del robot.
Con la finalidad de poder efectuar una animación de la trayectoria del extremo del robot
desde Matlab se guarda la posición de cada articulación en la matriz mat_q. Esta matriz
posee una fila por cada instante de simulación y una columna por cada variable
articular.
Para realizar la simulación de la trayectoria en primer lugar es necesario configurar los
parámetros de la simulación. Los valores seleccionados de los parámetros son los que se
muestran en la figura 6.10. Es muy importante destacar que el tipo de paso seleccionado
del Solver debe ser fijo y además el valor del paso debe coincidir con el intervalo de
integración especificado en la función planifica4.m. Así mismo, el tiempo de
simulación deber ser idéntico al considerado en dicha función.
Figura 6.10. Parámetros para realizar la simulación del robot de 4 GDL.
Una vez que se han establecido los parámetros de simulación de Simulink
, ya puede
realizarse la simulación del modelo. Una vez finalizada la simulación, podremos, desde
el entorno de Matlab
, realizar una animación para comprobar visualmente que el robot
realiza la trayectoria deseada. Para efectuar la animación se utilizará la función
desarrollada en Matlab
animacion4.m. El código de esta función se muestra a
continuación:
% ANIMACION4 Animación de la trayectoria de un robot de 4 GDL
% ANIMACION(MAT_Q) realiza la animación de la trayectoria, expresada
% en la matriz MAT_Q, de un brazo robot de 4 GDL. MAT_Q contiene 4 filas
% y una columna para cada disposición del robot.
%
% Ver también DRAWROBOT3D4.
% Parámetros Denavit-Hartenberg del robot. Los parámetros correspondientes
% a variables articulares aparecen con valor 0
teta = [0 0 0 0 ];

d = [0.4 0 0 0.2];
a = [0 -0.1 0 0 ];
alfa = [0 -pi/2 0 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);
grid;
axis([-1.5 1.5 -1.5 1.5 0 1.5]);
hold on;
n = size(mat_q,2);
for i=1:n
q1 = mat_q(1,i);
q2 = mat_q(2,i);
q3 = mat_q(3,i);
q4 = mat_q(4,i);
consecutivos
A02 = A01 * A12;
A03 = A02 * A23;
A04 = A03 * A34;
x1 = A01(1,4); y1 = A01(2,4); z1 = A01(3,4);
xi = x1; yi = y1; zi = z1 + q2;
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);
x = [x0 x1 xi x2 x3 x4];
y = [y0 y1 yi y2 y3 y4];
z = [z0 z1 zi z2 z3 z4];
set(p,'XData',x,'YData',y,'ZData',z);
drawnow;
end
Para realizar la animación simplemente se ejecutará desde Matlab:
» animacion4(mat_q')

En la figura 6.11 se muestra el instante final de la animación del robot cuyo extremo
realiza una trayectoria en línea recta entre la posición (0,-0.6,0.5) y (0,-1,1.2).
Figura 6.11. Instante final de la animación del robot de 4 GDL.
Se ha añadido la trayectoria seguida por el extremo del robot.
Ejercicio 1
Ejercicio 2

Practicas de robotica utilizando matlab - Roque

Más contenido relacionado

La actualidad más candente

Similar a Practicas de robotica utilizando matlab - Roque

Último

Practicas de robotica utilizando matlab - Roque