Tiempo y Movimiento

Ricardo Rodrguez Bustinza

robust@uni.edu.pe

Indice
1. Trayectorias
1.1. Trayectorias Suaves en una Dimension . . . . . . . . . . . . . . . . . . . . . . . . . . .

2
2

2. Polinomios Cubicos

3. Polinomios de Quinto Orden

4. Segmentos Lineales con Enlace Parabolico

5. Trayectorias de Orden Alto

12

1 TRAYECTORIAS

1. Trayectorias
Una camino en una construccion espacial, un lugar en el espacio que va desde una postura inicial a una
postura final. Una trayectoria en un camino con especificacion del tiempo. Por ejemplo un camino de A
hacia B, pero la trayectoria de A a B en 10s o 2ms1 . Una caracterstica importante de la trayectoria es de
posicion y orientacion suave (smooth) que vara suavemente con el tiempo. Empezaremos a estudiar como
generar una trayectoria en una dimension. Posteriormente puede ser extendida a multiples dimensiones y
trayectoria lineales por partes que visitar una serie de puntos intermedios sin parar.

1.1. Trayectorias Suaves en una Dimension

Comenzaremos nuestro estudia desde la funcion escalar en el tiempo. Una caracterstica importante de
la funcion es que los valores inicial y final son especificados y deben ser suaves. El termino de suavidad
quiere decir que debe ser derivable y continua. Tpicamente la velocidad y aceleracion requeridos deben
ser continuos.
Obviamente una funcion candidata es un polinomio en el tiempo. El polinomio es simplemente computado
y facilmente proporciona condiciones de suavidad y lmite. Un polinomio de quinto orden es a menudo
requerido, este es dado por:
S(t) = At 5 + Bt 4 +Ct 3 + Dt 2 + Et + F

(1)

Donde el tiempo t [0, T ]. La segunda derivada tambien es un polinomio suave

= 5At 4 + 4Bt 3 + 3Ct 2 + 2Dt + E
S(t)

(2)

= 20At 3 + 12Bt 2 + 6Ct + 2D

S(t)

(3)

La trayectoria es definida por la condicion de limite para la posicion, velocidad y aceleracion. Comunmente
las condiciones para la velocidad y aceleracion son todas cero. Escribiendo las ecuaciones (1) a (3) para
las condiciones de limite t = 0 a t = T dando seis ecuaciones que podemos expresarlas en forma matricial:

s0
sT
s0
sT
s0
sT

0
0
0
0
5
4
3
T
T
T2
T
0
0
0
0
5T 4 4T 3 3T 2 2T
0
0
0
2
2
20T 3 12T 2 6T

A
0 1

T 1
B

1 0 C

1 0
D

E
0 0
0 0
F

Ademas:
Tiempo
t=0
t =T

s
s0
sT

s
s0
sT

s
s0
sT

Dado que la matriz es cuadrada podemos resolver para el vector de coeficientes (A, B,C, D, E, F) usando
el metodo estandar de a lgebra lineal tales como el operador \ de Matlab. El Toolbox de Robotica tiene la
funcion tpoly que genera el polinomio de quinto orden y describe la trayectoria, ingresamos la lnea de
codigo:

M.Sc. Ricardo Rodrguez Bustinza

1.1 Trayectorias Suaves en una Dimension

1 TRAYECTORIAS

s=tpoly(0,1,50)
plot(s)
Los resultados se muestran en la Figura 1.
1

S(t)

0.8
0.6
0.4
0.2
0

10

15

20

25
t(seg)

30

35

40

45

50

Figura 1: Trayectorias de polinomios de quinto orden.

En este caso, una velocidad inicial de 0.5 y una velocidad final de 0. Los resultados muestran en la Figura
2 illustrado el problema con polinomios. La velocidad inicial distinto de cero hace que el polinomio rebase
el valor terminal, alcanza un maximo a 5 en una trayectoria de 0 a 1.

S (t)

6
4
2
0

10

15

20

25

30

35

40

45

50

10

15

20

25

30

35

40

45

50

10

15

20

25
t(s)

30

35

40

45

50

S(t)

0.5

0.5

0.02

S(t)

0
0.02
0.04

Figura 2: Trayectorias de polinomios de quinto orden y condiciones iniciales.

Otro problema con polinomios, uno muy practico, se puede ver en la grafica media de la Figura2. Los
picos de velocidad en t = 25 cual significa que para la mayora de las veces la velocidad es mucho menor
que el maximo. La velocidad media en este caso es:
vm=mean(sd)/max(sd);
disp(vm)
disp(vm)
% 0.05
Es solo 5 % del pico, en un robot real la juntura esta definida por la velocidad maxima y por el movimiento
del tiempo mnimo que operan al maximo para la mayor cantidad de tiempo. Nos gustara que la curva de
velocidad sea mas plana en la parte superior. Una alternativa es realizar una trayectoria hibrida que consta
M.Sc. Ricardo Rodrguez Bustinza


2 POLINOMIOS CUBICOS

de segmentos de velocidad con segmentos de polinomios para aceleracion y desaceleracion. Una primera
alternativa de una trayectoria hibrida es:
[s,sd,sdd]=lspb(s0,sF,M);
Los argumentos de la funcion lspb son los mismos que para tpoly. En la Figura 3 se muestra los
segmentos lineales con enlace parabolico.

S (t)

0.5

10

15

20

25

30

35

40

45

50

10

15

20

25

30

35

40

45

50

10

15

20

25
t(s)

30

35

40

45

50

0.04

S(t)

0.03
0.02
0.01
0

0
3

x 10

S(t)

1
0
1
2

Figura 3: Segmentos lineales con elcae parabolico.

2. Polinomios Cubicos
Considere el problema de movimiento de la herramienta desde una posicion inicial a una posicion objetivo en una cierta cantidad de tiempo. La cinematica inversa proporciona un conjunto de a ngulos de
las articulaciones que corresponden a una posicion final y orientacion que seran calculadas. La posicion
inicial del manipulador es tambien conocida en la forma de un conjunto de a ngulos articulares. Lo que
se requiere es una funcion para cada articulacion con valor t0 en una posicion inicial de la articulacion y
el valor t f es la posicion objetivo de la articulacion. En la Figura 4 se muestra algunas funciones smooth
(superficie suave), (t), que pueden ser usados para interpolar los valores de las articulaciones.
Realizando un movimiento smooth, por lo menos cuatro restricciones sobre (t) son evidentes. Dos restricciones sobre el valor de la funcion vienen de la seleccion de valores iniciales y finales.

(0) = 0
(t f ) = f

(4)

Adicionalmente se consideran dos restricciones que son la funcion continua de la velocidad, que en este
caso quiere decir que la velocidad inicial y final son cero:

(0) = 0
(t f ) = 0
M.Sc. Ricardo Rodrguez Bustinza

(5)
4


2 POLINOMIOS CUBICOS

1.2

0.8

0.6

0.4

0.2

0
t0

tf

0.2
0

10

Figura 4: Posibles formas de trayectorias para una articulacion.

Estas cuatro restricciones pueden ser satisfechas por un polinomio de por lo menos de tercer grado. (Un
polinomio cubico tiene cuatro coeficientes, as que puede satisfacer las cuatro restricciones dadas por 1( 9)
y (20). Estas restricciones u nicamente especifican un particular polinomio cubico de la forma:

(t) = a0 + a1 t + a2t 2 + a3t 3

(6)

La velocidad de juntura y aceleracion a lo largo de esta trayectoria viene dada por:

(t) = a1 + 2a2 t + 3a3t 2

(t) = 2a2 + 6a3t

(7)

Combinando (21) y (22) con las cuatro restricciones deseadas produce cuatro ecuaciones desconocidas:

0
f
0
0

= a0
= a0 + a1t f + a2t 2f + a3t 3f
= a1
=

(8)

a1 + 2a2 t f + 3a3t 2f

Resolviendo estas ecuaciones para ai , obtenemos:

a0 = 0
a1 = 0
3
a2 = 2 ( f 0 )
tf
2
a3 = 3 ( f 0 )
tf

(9)

Usando (24), podemos calcular el polinomio cubico que conecta cualquier posicion del a ngulo de una
articulacion inicial a cualquier posicion final deseada. La solucion para este caso cuando la articulacion
comienza y finaliza con velocidad cero.
M.Sc. Ricardo Rodrguez Bustinza


2 POLINOMIOS CUBICOS

Problema # 1
Un robot de un solo eslabon con una articulacion rotatoria es inmovil en = 15 . Se desea cambiar de
lugar la articulacion en una manera suave a = 75 en 3 segundos. Encontrar los coeficientes de un
cubico que logra hacer el movimiento y trae al manipulador para descansar en el objetivo.
Desarrollar un algoritmo para la trayectoria de la articulacion.
Plotear la posicion, velocidad, y aceleracion angular de la articulacion como una funcion de tiempo
(ver Figuras 56). Usar la herramienta de simulacion Toolbox de Matematica Simbolica para la
Figura 5.

Codigo Matlab
% Parametros
q0=15; qf=75; ti=0; tf=3;
% Resolviendo las ecuaciones
a0=q0; a1=0; a2=(3/tf^2)*(qf-q0); a3=-(2/tf^3)*(qf-q0);
% Ecuaciones en el tiempo
syms t
q=a0 + a1*t + a2*t^2 + a3*t^3; % Polinomio c
ubico
dq=diff(q);
% 40*t-40/3*t^2
ddq=diff(dq);
% 40-80/3*t
15+20 t240/9 t3
80
60
40
20
0

0.5

1.5
t
2
40 t40/3 t

2.5

0.5

1.5
t
4080/3 t

2.5

0.5

1.5
t

2.5

30
20
10
0

40
20
0
20
40

Figura 5: Posicion, velocidad y aceleracion para un segmento cubico.

f=40; % Hz
T=1/f;
t=ti:T:tf;
q = a0 + a1*t + a2*t.^2 + a3*t.^3;
dq = 40*t-(40/3)*t.^2;
ddq= 40-(80/3)*t;

M.Sc. Ricardo Rodrguez Bustinza


2 POLINOMIOS CUBICOS

80

30

70

25
POSICION
Grados/seg

Grados

60
50
40
30

15
VELOCIDAD

10
5

20
10

20

t (seg)

t (seg)

40

80
60
ACELERACION

Grados/seg2

20

40
0

20
0

20
20
40

40

t (seg)

Figura 6: Posicion, velocidad y aceleracion para un segmento cubico.

Problema # 2
Se desea tener la primera articulacion del robot de seisejes con un a ngulo inicial de 30 a un a ngulo final
de 75 en 5 segundos. Usando un polinomio de tercer orden, calcular el a ngulo de la articulacion en 1, 2,
3 y 4 segundos, es decir, (1) = 34.68 , (2) = 45.84 , (3) = 59.16 , y (4) = 70.32 respectivamente.
Nota: Los a ngulos de las articulaciones, velocidades, y aceleraciones son mostrados en la Figura7. En
este caso, la aceleracion necesita comenzar el movimiento en 10.8 grados/seg2 (tambien como una desaceleracion de 10.8 grados/seg2 en la conclusion del movimiento).
54/5108/25 t
80
Posicin

60
40

Velocidad

20
0
Aceleracin
20

3
t

Figura 7: Conjunto de posicion, velocidad y aceleracion.

M.Sc. Ricardo Rodrguez Bustinza

3 POLINOMIOS DE QUINTO ORDEN

3. Polinomios de Quinto Orden

Los polinomios de quinto orden son algunas veces usados para segmentos de caminos. Por ejemplo se
quiere especificar la posicion, velocidad y aceleracion en el inicio y final del segmento, para ello requerimos un polinomio de quinto orden, concretamente:

(t) = a0 + a1t + a2t 2 + a3t 3 + a4t 4 + a5t 5

(10)

consideramos las siguientes restricciones:

0
f
0
f

= a0
= a0 + a1t f + a2t 2f + a3t 3f + a4t 4f + a5t 5f
= a1
= a1 + 2a2 t f + 3a3t 2f + 4a4 t 3f + 5a5t 4f

(11)

0 = 2a2
f = 2a2 + 6a3t f + 12a4 t 2f + 20a5 t 3f
Las restricciones dadas en (26) especifican un conjunto lineal de seis ecuaciones con seis desconocidas,
la solucion es:
a0 = 0
a1 = 0
0
a2 =
2
20 f 200 (8 f + 120 )t f (30 f )t 2f
a3 =
2t 3f
a4 =
a5 =

300 30 f + (14 f + 160 )t f + (30 2 f )t 2f

2t 4f

(12)

12 f 120 (6 f + 60 )t f (0 f )t 2f
2t 5f

Problema # 3
Se desea tener la primera articulacion del robot de seisejes con un a ngulo inicial de 30 a un a ngulo final
de 75 en 5 segundos. Usando un polinomio de tercer orden, calcular el a ngulo de la articulacion en 1, 2,
3 y 4 segundos, es decir, (1) = 34.68 , (2) = 45.84 , (3) = 59.16 , y (4) = 70.32 respectivamente.
Asuma la aceleracion inicial y desaceleracion final de 5 grados/seg2 .

0 = 30 ,
f = 75 ,

0 = 0 grados/seg,
f = 0 grados/seg,

0 = 5 grados/seg2
f = 5 grados/seg2

Encontrando los coeficientes dadas las condiciones inicial y final:

M.Sc. Ricardo Rodrguez Bustinza


4 SEGMENTOS LINEALES CON ENLACE PARAB OLICO

a0 = 30,

a1 = 0,

a2 = 2.5

a3 = 1.6,

a4 = 0.58,

a5 = 0.0464

Estos resultados nos proporcionan los siguientes movimientos:

(t) = 30 + 2.5t 2 + 1.6t 3 0.58t 4 + 0.0464t 5

(t) = 5t + 4.8t 2 2.32t 3 + 0.232t 4
(t) = 5 + 9.6t 6.96t2 + 0.928t 3
3
2
(116 t )/125 (174 t )/25 + (48 t)/5 + 5

80
Posicin
60
40
20

Velocidad

0
Aceleracin
20

0.5

1.5

2.5
t

3.5

4.5

Figura 8: Conjunto de posicion, velocidad y aceleracion.

Codigo MATLAB
% Par
ametros
q0=30; qf=75; dq0=0; dqf=0; ddq0=5; ddqf=-5; ti=0; tf=5;
% Resolviendo las ecuaciones
a0=q0; a1=dq0; a2=ddq0/2;
a3=(20*qf-20*q0-(8*dqf+12*dq0)*tf-(3*ddq0-ddqf)*tf^2)/(2*tf^3);
% 1.6
a4=(30*q0-30*qf+(14*dqf+16*dq0)*tf+(3*ddq0-2*ddqf)*tf^2)/(2*tf^4); % -0.58
a5=(12*qf-12*q0-(6*dqf+6*dq0)*tf-(ddq0-ddqf)*tf^2)/(2*tf^5);
% 0.0464
% Ecuaciones en el tiempo
syms t
q=a0 + a1*t + a2*t^2 + a3*t^3 + a4*t^4 + a5*t^5;
dq=diff(q);
ddq=diff(dq);

4. Segmentos Lineales con Enlace Parabolico

Otra alternativa para el camino del espacioarticular es ejecutar la articulaciones con velocidad constante
entre las ubicaciones inicial y final. Esto es equivalente a polinomios de primer orden, donde la velocidad es constante y la aceleracion es cero. Sin embargo, esto tambien representa el principio y el final del
segmento de movimiento, las aceleraciones deben ser infinitas para crear velocidades instantaneas en los
M.Sc. Ricardo Rodrguez Bustinza


4 SEGMENTOS LINEALES CON ENLACE PARAB OLICO

1.2

0.8

0.6
i+
0.4

0.2

tb

0.2
0

tftb

t
6

10

Figura 9: Esquema para segmentos lineales con enlace parabolico.

lmites. Para prevenir esto, el segmento lineal puede ser enlazado con secciones parabolicos en el comienzo y final del segmento de movimiento, creando posiciones continuas y velocidades como la mostrada en
la Figura 9.
Asumiendo que las posiciones inicial y final son i y f en un tiempo ti = 0 y t f y el segmento parabolico
son simetricamente conectados con la seccion lineal y considerando la conexion de tiempos bt y t f tb
podemos escribir:
1
(t) = c0 + c1t + c2 t 2
2
(t) = c1 + c2t
(t) = c2

(13)

Obviamente, en este escenario, la aceleracion es constante para las secciones parabolicas, produce una
velocidad continua en un punto comun A y B (llamados puntos de nudo). Sustituyendo las condiciones
de lmite en la ecuacion del segmento de parabola tenemos:

c0 = i
(t = 0) = i = c0
(t = 0) = 0 = c1

c =0
1

(t) = c2
c2 =
Esto da los segmentos parabolicos en la forma:
1
(t) = i + c2 t 2
2
(t) = c2t
(t) = c2

(14)

Observamos claramente, que para el segmento lineal la velocidad es constante y puede ser escogida basado
en las capacidades fsicas de los actuadores. Sustituyendo la velocidad inicial cero, una constante conocida
para la velocidad de la articulacion en la porcion lineal, y la velocidad final cero en (29), hallamos las
posiciones y velocidades para los puntos A y B y el punto final como se explica a continuacion:
M.Sc. Ricardo Rodrguez Bustinza

10


4 SEGMENTOS LINEALES CON ENLACE PARAB OLICO

1
A = i + c2 tb2
2
A = c2 tb =
B = A + ((t f tb ) tb ) = A + (t f 2tb )
B = A =
f = B + (A i )
f = 0

(15)

Es necesario conectar el tiempo tb que puede ser obtenido desde las ecuaciones descritas en (30).


,
tb
f = i + c2tb2 + (t f 2tb )
c2 =

Entonces:

f = i +


t 2 + (t f 2tb )
tb b

(16)

Desde la ecuacion (31), obtenemos:

i f + t f
(17)

El tiempo tb no puede ser mas alto que la mitad del tiempo total tf , que resulta en una parabola con incremento y una parabola con retardo, con un segmento no lineal. La correspondiente velocidad maxima
de max = 2( f i )/t f puede ser hallada desde la ecuacion (32). Debemos mencionar que en cualquier
segmento, el tiempo inicial es no cero, pero tb , por simplificacion matematica, podemos cambiar el eje de
tiempo por ta para hacer que el tiempo inicial sea cero.
tb =

El segmento parabolico final, es simetrico con el segmento inicial de la parabola, pero con aceleracion
negativa, esto puede ser expresado de la siguiente forma:
1
(t) = f c2 (t f t)2 ,
2

(t) = f

tb

(t f t)2
2tb

(t f t)
tb

(t) =
tb
=

c2 =

(18)

Problema # 4
La articulacion 1 del robot de seis ejes del problema 2, va desde un a ngulo inicial de i = 30 a un a ngulo
final de f = 70 en 5 segundos, con una velocidad de crucero de 1 = 10 grados/seg. Hallar el tiempo
necesario para realizar el enlace, y plotear la posicion, velocidad y aceleracion de la articulacion.
Usando las ecuaciones (29), (32), y (33), obetenemos las ecuaciones que son desarrolladas en MATLAB.

M.Sc. Ricardo Rodrguez Bustinza

11

5 TRAYECTORIAS DE ORDEN ALTO

tb =
Para = i a A :

i f + t f
30 70 + 10(5)
= 1 seg
=

10

= 30 + 5t 2

= A + 10t

= 10t
= 10

Para = A a B :

= 10
= 0

Para = B a f :

= 70 5(5 t)2
= 10(5 t)
= 10

5. Trayectorias de Orden Alto

Cuando, se suman los puntos de destino inicial y final, otras vas de puntos (incluyendo puntos de elevacion y ensilladura) que son especificados, podemos ajustar la posicion, velocidad y aceleracion de dos
segmentos en cada punto para planificar una trayectoria continua. Incluyendo las condiciones de lmite
iniciales y finales juntos con esta informacion nos permite el uso de polinomios de orden mas altos de la
forma:

(t) = c0 + c1t + c2t 2 + + cn1t n1 + cnt n

(19)

Con el proposito de que la trayectoria pasara a traves de todos los puntos especificados. As mismo,
resolviendo los polinomios d orden alto para cada articulacion requiere un extenso calculo.
Es posible usar combinaciones de polinomios de orden mas bajo para diferentes segmentos de la trayectoria y enlazar juntos para satisfacer todos los requerimientos de lmite, incluyendo una trayectoria 4 3 4,
una trayectoria 3 5 3, y una trayectoria cubica-5 que reemplacen a un polinomio de septimo orden.
Por ejemplo, una trayectoria 4 3 4, un polinomio de cuarto orden es usado para planificar una trayectoria entre un punto inicial y la primera va del punto (ejemplo, elevacion), un polinomio de tercer orden
es usado para planificar la trayectoria entres dos vas de puntos (ejemplo, elevacion y ensilladura), y otro
polinomio de cuarto orden es usado para planificar la trayectoria de la u ltima va de punto (ejemplo,
ensilladura) y el destino final. Similarmente se analiza la otras trayectorias, por ejemplo 3 5 3.
Para ver como se realiza este trabajo, veamos el caso de la trayectoria 4 3 4. Un polinomio de cuarto
orden tiene cinco coeficientes desconocidos. Es decir, la trayectoria 4 3 4 tiene un total de 14 coeficientes desconocidos de la siguiente forma:
M.Sc. Ricardo Rodrguez Bustinza

12

5 TRAYECTORIAS DE ORDEN ALTO

30+5 t2

705 (5t)2

35+10 t
70
70

34

68

60

32

66

50
30
0

0.5
t
10 t

4.5
t
5010 t

4.5
t
10

4.5
x

t
10
11

10

10

10.5
5

10

9.5
0
0

0.5
t
10

0
1

x
0

11

10.5

0.5

9.5

10

10

9.5

0.5

10.5

0.5
x

11

Figura 10: Graficos de posicion, velocidad y aceleracion para la articulacion 1.

(t)1 = a0 + a1t + a2t 2 + a3 t 3 + a4t 4

(t)2 = b0 + b1t + b2t 2 + b3 t 3
(t)4 = c0 + c1t + c2t 2 + c3t 3 + c4t 4

(20)

Sin embargo, tambien hay 14 condiciones de lmite y enlaces disponible que pueden ser usados para
resolver los coeficientes desconocidos para todos los planes de trayectoria.
1. La posicion inicial para i es conocida.
2. La velocidad inicial puede ser especificada.
3. La aceleracion inicial puede ser especificada.
4. La posicion del primer punto va 2 es conocido, y es el mismo que la posicion final en el primer
segmento del polinomio de cuarto orden.
5. La primera posicion de los punto vas es el mismo que la posicion inicial del segmento de tercer
orden para continuidad.
6. La velocidad continua debe ser mantenida en el punto va.
7. La aceleracion continua debe ser mantenida en el punto va.
M.Sc. Ricardo Rodrguez Bustinza

13

5 TRAYECTORIAS DE ORDEN ALTO

8. La posicion del segundo punto va n (y subsiguiente) es especificado y es el mismo que la posicion
final en el segmento de tercer orden.
9. La posicion del segundo punto va (y subsiguiente) es el mismo que la posicion inicial del siguiente
segmento para continuidad.
10. La velocidad continua debe ser mantenida en el siguiente punto va.
11. La aceleracion continua debe ser mantenida en el siguiente punto va.
12. La posicion de destino f es especificada.
13. La velocidad de destino es especificada.
14. La aceleracion de destino es especificada.
Denotaremos la variable de tiempo global y normalizada para todo el movimiento de t, mientras i denota
las e pocas locales especficas por cada segmento j. Tambien asumiremos que el arranque inicial local
ji para cada segmento es cero y que el local final termina en tiempo j f para cada segmento que es
especificado. Esto quiere decir que todos los segmentos empiezan en el tiempo local cero y terminan en
un especificado, dado, el tiempo local, mientras que el siguiente segmento comienza en el tiempo local
ji . Basado en el procedimiento asumido y dado, el segmento 4 3 4 y sus derivadas pueden ser escritos
como:
(1) El primer segmento de cuarto orden en el tiempo local i = 0 produce la posicion inicial conocida
de 1 .

1 = a0

(21)

(2) La velocidad se inicia en 1 = 0 para el primer segmento conocido.

1 = a1

(22)

(3) La aceleracion se inicia en 1 = 0 para el primer segmento conocido.

1 = 2a2

(23)

(4) La posicion del primer punto va 2 es la conclusion del primer segmento en el tiempo local 1 f
conocido.

2 = a0 + a1 (1 f ) + a2 (1 f )2 + a3 (1 f )3 + a4 (1 f )4

(24)

(5) La posicion del primer punto va es el mismo que la posicion inicial del polinomio de tercer orden
en el tiempo 2 = 0.

2 = b0

(25)

(6) La velocidad continua debe ser mantenida en el punto va.

a1 + 2a2 (1 f ) + 3a3 (1 f )2 + 4a4 (1 f )3 = b1
M.Sc. Ricardo Rodrguez Bustinza

(26)
14

5 TRAYECTORIAS DE ORDEN ALTO

(7) La aceleracion continua debe ser mantenida en el punto va.

2a2 + 6a3 (1 f ) + 12a4 (1 f )2 = 2b2

(27)

(8) La posicion del segundo punto va 3 es la conclusion del segmento de tercer orden en el tiempo
2 f es especificado.

3 = b0 + b1 (2 f ) + b2 (2 f )2 + b3 (2 f )3

(28)

(9) La posicion del punto va 3 es el mismo que la posicion inicial del siguiente segmento en 3 = 0
para continuidad.

3 = c0

(29)

(10) La velocidad continua debe ser mantenida en el siguiente punto va.

b1 + 2b2 (2 f ) + 3b3 (2 f )2 = c1

(30)

(11) La aceleracion continua debe ser mantenida en el siguiente punto va.

2b2 (2 f ) + 6b3 (2 f ) = 2c2

(31)

(12) La posicion de destino en la conclusion del u ltimo segmento 3 f es especificada como f .

4 = c0 + c1 (3 f ) + c2 (3 f )2 + c3 (3 f )3 + c4 (3 f )4

(32)

(13) La velocidad de destino en la conclusion del u ltimo segmento en el tiempo 3 f es especificada.

4 = c1 + 2c2 (3 f ) + 3c3 (3 f )2 + 4c4 (3 f )3

(33)

(14) La aceleracion de destino en la conclusion del u ltimo segmento en el tiempo 3 f es especificada.

4 = 2c2 + 6c3 (3 f ) + 12c4 (3 f )2

(34)

Las ecuaciones (21) a (34) pueden ser escritas en forma matricial:

1
1
1
2
2
0
0
3
3
0
0
4
4
4

1 0
0 1
0 0
1 1 f
0 0
0 1
0 0
0 0
0 0
0 0
0 0
0 0
0 0
0 0

0
0
2
12f
0
21 f
2
0
0
0
0
0
0
0

0
0
0
13f
0
312f
6 1 f
0
0
0
0
0
0
0

0
0
0
14f
0
413f
1212f
0
0
0
0
0
0
0

0 0
0
0 0
0
0 0
0
0 0
0
1 0
0
0 1
0
0 0
2
1 2 f 22f
0 0
0
0 1 2 2 f
0 0
2
0 0
0
0 0
0
0 0
0

0
0
0
0
0
0
0
23f
0
322f
6 2 f
0
0
0

0 0
0
0 0
0
0 0
0
0 0
0
0 0
0
0 0
0
0 0
0
0 0
0
1 0
0
0 1
0
0 0
2
1 3 f 32f
0 1 2 3 f
0 0
2

0
0
0
0
0
0
0
0
0
0
0
33f
332f
6 3 f

0
0
0
0
0
0
0
0
0
0
0
34f
433f
1232f

a0
a1
a2
a3
a4
b0
b1
b2
b3
c0
c1
c2
c3
c4

(35)

M.Sc. Ricardo Rodrguez Bustinza

15

5 TRAYECTORIAS DE ORDEN ALTO

Tambien puede ser expresado: [ ] = [M][C], despejando el vector de coeficientes:

[C] = [M]1 [ ]

(36)

Los coeficientes desconocidos pueden ser encontrados de la ecuacion 36 para calcular M1 . Las ecuaciones del movimiento de los tres segmentos conocidos y el robot puede ser llevados a traves de posiciones
especficas. Lo mismo debe ser hecho para otras articulaciones.

Problema # 5
Un robot debe ser conducido desde una posicion inicial a traves de dos vas de puntos despues de alcanzar
el destino final usando una trayectoria 4 3 4. Determine las ecuaciones de trayectoria y plotear las
curvas de la posicion, velocidad, y aceleracion, en un tiempo de duracion para tres segmentos par una de
las articulaciones:

1
2
3
4

= 30 ,

= 50 ,

= 90 ,

= 70 ,

1 = 0,
2i = 0,
3i = 0,
4 = 0,

1
2 f
3 f
4

= 0,

1i = 0,

1 f = 2

=4
=2
=0

120
100
80

Posicin

60
40

Velocidad

20
0
Aceleracin

20
0

Figura 11: Curvas en movimiento para la trayectoria 4-3-4.

Codigo MATLAB
% Par
ametros
q1=30; dq1=0; ddq1=0; tau_1i=0; tau_1f=2;
q2=50; tau_2i=0; tau_2f=4; q3=90; tau_3i=0; tau_3f=2; q4=70; dq4=0; ddq4=0;
M=[1 0 0 0 0 0 0 0 0 0 0 0 0 0
0 1 0 0 0 0 0 0 0 0 0 0 0 0
0 0 2 0 0 0 0 0 0 0 0 0 0 0
1 tau_1f tau_1f^2 tau_1f^3 tau_1f^4 0 0 0 0 0 0 0 0 0
0 0 0 0 0 1 0 0 0 0 0 0 0 0
0 1 2*tau_1f 3*tau_1f^2 4*tau_1f^3 0 -1 0 0 0 0 0 0 0
0 0 2 6*tau_1f 12*tau_1f^2 0 0 -2 0 0 0 0 0 0
0 0 0 0 0 1 tau_2f tau_2f^2 tau_2f^3 0 0 0 0 0
0 0 0 0 0 0 0 0 0 1 0 0 0 0
M.Sc. Ricardo Rodrguez Bustinza

16

5 TRAYECTORIAS DE ORDEN ALTO

0 0 0 0 0 0 1 2*tau_2f 3*tau_2f^2 0 -1 0 0 0
0 0 0 0 0 0 0 2 6*tau_2f 0 0 -2 0 0
0 0 0 0 0 0 0 0 0 1 tau_3f tau_3f^2 tau_3f^3 tau_3f^4
0 0 0 0 0 0 0 0 0 0 1 2*tau_3f 3*tau_3f^2 4*tau_3f^3
0 0 0 0 0 0 0 0 0 0 0 2 6*tau_3f 12*tau_3f^2];
M_inv=inv(M);
Q=[q1;dq1;ddq1;q2;q2;0;0;q3;q3;0;0;q4;dq4;ddq4];
C=M_inv*Q;
% Construyendo los tres segmentos:
syms t
qt1=C(1)+C(2)*t+C(3)*t^2+C(4)*t^3+C(5)*t^4;
qt1=vpa(qt1,3); % 30.+4.88*t^3-1.19*t^4, 0<t<=2
dq1=diff(qt1);
ddq1=diff(dq1)
qt2=C(6)+C(7)*t+C(8)*t^2+C(9)*t^3;
qt2=vpa(qt2,4); % 50.+20.48*t+.7143*t^2-.8333*t^3, 0<t<=4
dq2=diff(qt2);
ddq2=diff(dq2)
qt3=C(10)+C(11)*t+C(12)*t^2+C(13)*t^3+C(14)*t^4;
qt3=vpa(qt3,4); % 90.-13.81*t-9.286*t^2+9.643*t^3-2.024*t^4, 0<t<=2
dq3=diff(qt3);
ddq3=diff(dq3)

M.Sc. Ricardo Rodrguez Bustinza

17