planificación de trayectorias

Andrea Ismeneé Herrera Huerta Página 1

Planificación de Trayectorias

Este trabajo tiene por objetivo proporcionar un algoritmo para establecer cuáles son las trayectorias que debe seguir cada articulación del robot a lo largo del tiempo para conseguir los puntos fijados por el usuario (Punto de destino, Tipo de trayectoria del extremo, Tiempo invertido, entre otros parámetros), atendiendo las restricciones físicas. Se hará referencia al término de trayectoria, entiendo por este la referencia hecha a una serie de polinomios cúbicos concatenados escogidos de forma que exista continuidad en posición y velocidad, denominados splines. Por lo tanto un curva Spline es un polinomio de grado k (con k=3,4,.., n-2) con continuidad en la derivada de orden k-1 en los puntos de interpolación. A continuación se consideran Splines cúbicos debido a que son los polinomios de menor grado (con continuidad en la velocidad y aceleración) posibles, que nos permiten de manera sencilla unir puntos de una manera suave y continua, estos son de la forma de la ecuación 1.

��1 = �� + �� + �� + �� Ecuación 1 Si usamos este tipo de polinomios para la planificación de trayectorias del robot

PowerCube debemos obtener un polinomio para cada articulación de forma que su valor para cada �� sea el valor inicial � de la variable articulada y su valor para �� el valor final �. Entonces dado n puntos � , � = 1, . . , � e instantes correspondientes de tiempo ��, �, … �� con valores de � � ( � � ��) desconocidos e instantes � � �� que cumplen con la condición de �� < � < �� (�� < �� < ��) dados se debe cumplir ecuación (2).

�� = � ��′�� = �� ′′�� = �� Ecuación 2 ��′�� = �� ′′�� = �� = ��′�� = ��′′�� Con i=2,…,n-1

Dado que Q (t) es cúbica, la segunda vez derivado ��debe ser una función lineal de t. Por lo tanto, �� puede expresarse como la Ecuación 3.

�� = �� − �� + �� − �� Ecuación 3

1 Es un polinomio cubico definido en el intervalo de tiempo [�� , ��]


Donde se debe de cumplir con �� = �� − �� Especificaciones 1

i=1,2,…,n-1 Si integramos la ecuación 3 considerando que �� = � y que �� = �� obtenemos el valor de �� (ecuación 4).

�� = ��6�� − �� + ��6�� − �� + # �� − ��6 $ �� − ��+ # �� − ��6 $ �� − ��

Derivando la ecuación 4, obtenemos ��, esto es ��

�� = − ��2 �� + �� − ��6 �� − �� + ��6 ��

A partir de la ecuación 4 y 5, se puede generar un sistema de n-2 ecuaciones que pueden ser escritas matricialmente como se muestra a continuación.

1

1

2

1 2 22

2

2 2 3 32

3 3 4 4

4 4 3 3

21

3 3 2 22

21

2 1 22

3 2

2( ) 0

2( )

2( )

0 2( )

3 2

n n n n

nn n n n

n

nn n n

n

UU U U

U

UU U U U

U

U U U UA

U U U U

UU U U U

U

UU U U

U

− − − −

−− − − −

−

−− − −

−

+ +

− + + =

+ + − + −

M

Entonces podemos expresar que &�'�� = (', donde

�′′'''' =)**+��,��,��-��-�./

/0 que es un vector de aceleraciones.


(' =

)*********+ 6 12324 + 56768 − 6 1 �76 + �748 1 � + �� + 764

� ��8 − ��6 1 � + �� + 764

� ��8 + 92:73 − 6 1 �74 + �738 �6 123�2:7: − 2:�2;73 8

6 12<=4�2<=37<=3 − 2<=3�2<=:7<=: 86 1 � − �� + 7<=6� ��8 − 6 1 �7<=4 − �7<=38 �� + 97<=3 ��

−6 1 �7<=6 + �7<=48 1 � − �� + 7<4�� 8 + 6 2<7<=6 + 6 2<=47<=4 − ��./////////0

Ecuación 6

Donde (' es un vector que contiene los valores de las q´s, v´s, a´s. La matriz A, es llamada matriz de banda, y a partir de del cálculo de está podemos obtener las ��. Es importante resalta que el resultado de la solución se encuentra en términos de intervalos de tiempo (��>). Para los tiempos dados, se define el vector X: ? = ��, �, . . , ��

Formulación del problema

Dadas las restricciones del problema @�A�′@ ≤ CDA, E = 1, . . , F G � = 1, … , � − 1 @�A�′′@ ≤ &DA , E = 1, . . , F G � = 1, … , � − 1

Donde CDA = DG>�H�II�J�G> KG L� �GLJI�K�K G� L� �H��I�L�I�ó� E − G>�N� &DA = DG>�H�II�J�G> KG L� �IGLGH�I�ó� G� L� �H��I�L�I�ó� E − G>�N�

Restricciones de velocidad N�OP�A�′��Q = max {@�A�� @, @�A�� @, @�A�� V��@} ≤ CD� Especificaciones 2

��A� �� =XYZY[5\]7\ �� − �V� + 5],\^67\ ��V − �� + _5]\�5],\^6`9 �� + 2],\^6�2]\7\ ,>� �A� ≠ �A,�� Vb[�� , ��]0>� ��A = �A,�� Vb[��, ��]

d Ecuación 7

De donde �e[�� , ��] y �V corresponde con el máximo de ��A′ �� dentro del intervalo [��, ��] . �V = 5\]f\^6�5],\^6f\5]\�5],\^6 Ecuación 8


Restricciones de aceleración

@��A��@ ≤ &DA , E = 1,2, … , F; � = 1,2 … . , � − 1

Las restricciones son

maxh@�A�@, @�i@, . . , @�A�@j ≤ &DA , E = 1,2,3, … , F Especificaciones 3

Restricciones de impulso

@��A��@ ≤ lDA , E = 1,2, … , F; � = 1,2 … . , � − 1

De otra manera, podemos decirlo como:

m�A,�� − �A�� m ≤ lDA , E = 1,2, … , F; � = 1,2 … . , � − 1

Por otra parte conocemos de forma directa los valores q inicial ( �), q final ( �), y

casi todos los puntos; de forma indirecta podemos conocer el valor de los puntos propuestos ( y ��) para asegurar la continuidad y suavidad, mediante la Ecuación 9.

= 26�23 �� = 2<=4�2< Especificaciones 4

Figura 1. Valores de q

Funciona Objetivo

Para maximizar la velocidad de operación, el tiempo que le lleva al robot alcanzar un punto dado debe ser reducido al mínimo. Por lo tanto la función objetivo está dada por la ecuación 10.

Minimizar n = ∑ ��p� Ecuación 10

��

�

t

qn

�

. . .

. . .

��


Paso 1. Selección de �� Es por esto que el primer punto del algoritmo para la planificación de trayectorias

consiste en realizar una selección del valor del intervalo de tiempo, esto es, el valor de ��. Esta selección del valor del intervalo de tiempo se realiza en base a la experiencia o a una sintonización de ��. Los valores de �� pueden variar a lo largo del tiempo (Figura 2).

Figura 2. Intervalo de tiempo ��

Para cada ��; � = 1,2 … , � calculamos los valores de �� … �� y de ′′. Obtenemos ? = ��, �, . . , ��.

Paso 2. Calculo de Q

Usando de la ecuación 4, se debe de encontrar las Q. Paso 3. Encontramos las velocidades

Debemos calcular el valor de las y ��, mediante las especificaciones 4 y la velocidad de cada punto de � … �.

Paso 4. Optimización de la solución factible (Ajuste de q, FSC)

Para encontrar una solución viable, se pueden expandir los intervalos como r = q�. El intervalo nuevo estará dado por ��∗ = q��. Las aceleraciones nuevas serán iguales a ��∗ = 5\t4. Mientras que ��∗�r� está definida en el intervalo [q�� , q��]. Por lo tanto

tenemos que u�vwxy

t , u��vwxyt , u��vwxy

t , respectivamente. El valor de q es llamado factor de

ajuste. El valor de lambda puede obtenerse mediante el uso de la ecuación 11, 12 y 13. El valor de lambda es el valor máximo de q�, q, q�; esto es como la ecuación 14. Si el resultado de la ecuación 14 es 1 no se hace nada. De lo contrario los valores de velocidad y

aceleración pueden ser cambiados por factores de �t , �t4 , �t3 . Estos cambios aseguran que se

satisfaga las limitaciones de velocidad, aceleración, e impulsos.

q� = N�OE z N�O�b[�� , ��]∀� |u]\} �f�|~�] � Ecuación 11

t1 t2 t3 t4 tn . . .


q = N�OE z N�O�b[��, ��]∀� |u]\}}�f�|��] � Ecuación 12

q� = N�OE z N�O�b[��, ��]∀� |u]\}}}�f�|��] � Ecuación 13

q = N�OP1, q�, q�/, q��/�Q Ecuación 14

Si un punto no fuera factible, se puede volver factible mediante el siguiente procedimiento.

1. Decida el valor de lambda (q), de acuerdo a las ecuaciones 11 y 14. 2. Remplace los intervalos de tiempo �� (��, �, … , ��) por q�� (q��, q�, … , q��)

3. Remplace �A, �A�, … , �A,�� por 5]4t4 , 5]3t4 , … , 5],<=6t4 ; respectivamente para

E = 1,2, . . , F. Paso 5. Optimización del algoritmo (Simplex)

A continuación formamos el vector n' = ��, �, . . , �� con los n vértices factibles, el cual será colocado en el origen y será el simplex inicial (asumiendo que es factible, de lo contrario se regresará al paso 4, para obtener lambda y el nuevo vector ?'). Para la construir el simplex se debe de seguir pasos, estos son:

5.1 Calcular el valor de la función objetivo para cada vértice del simplex. (Por ejemplo, utilizar por función objetivo ��O�, O� = �O� + O� + �O − 1�.

5.2 Tomar el vértice con el menor valor de la función objetivo (O�, vértice peor) y

con el mayor valor de la función objetivo (O�). Podemos calcular el nuevo centroide ( O�), utilizando la ecuación 15 O� = �� P∑ O� − O��p� Q Ecuación 15

Esto es debido a que el algoritmo intenta tomar el mejor vértice factible, que tenga un valor de función objetivo menor, a lo largo de la línea recta que conecta a O� con O�� .

Las operaciones para buscar un mejor vértice y para reducir el tamaño del poliedro incluyen la reflexión, la extensión, la contracción, y la reducción. Estos se definen de la siguiente manera:

5.2.1 Reflexión

O� debe ser reflejado a la cara opuesta (del vértice peor) y calculamos las

coordenadas del vértice de reflexión (Ecuación 16).

O�� = O�� + ��O�� − O�'''� Ecuación 16


Donde

O� corresponde con el vértice peor.

O�''' = �� ∑ O��p Ecuación 17

� > 0 (Coeficiente de reflexión mayor a cero) Especificación 5

El valor del coeficiente de reflexión debe de ser inicialmente 1, en dado caso de que

el valor de uno de los intervalos de tiempo en O��''''''''' sea negativo, el valor del coeficiente de reflexión debe de ser cambiado por un valor más pequeño.

� =XZ[ 1, �� 2�� − �� > 0 ∀�

�� N��∀�IJ� 2�� − �� 7\<^67\��7\<^6� , �� 2�� − �� < 0 ∀� d Ecuación 18

5.2.2 Expansión

Expandir el vector (O�� − O�) mediante el calculo O�� = O� + ��O�� − O��. Con las siguientes especificaciones � > 1 (� es el coeficiente de expansión). Ahora bien para mantener los valores de O�� positivos, � puede ser determinado mediante la Ecuación 19.

� =XZ[ 2, �� 2�� − �� > 0 ∀�

� N��∀�IJ� 2�� − �� 7\�7\��7\<^4� , �� 2�� − �� ≤ 0 ∀� d Ecuación 19

5.2.3 Contracción

Contraer el vector O� − O� mediante el cálculo de la ecuación 20.

O��, = O� + ��O� − O�� Ecuación 20

Donde � es el coeficiente de contracción

5.2.4 Reducción

Reducir el tamaño de los vectores (O� − O�) con i=1,2,…,n por la mitad de O�, mediante el empleo de la Ecuación 21.

O� ← O� + 0.5�O� − O�� Ecuación 21


Para iniciar la búsqueda, se seleccionan n vértices. Sean A�, A�, … , A,��, A� la secuencia

de desplazamiento de la unión j. En tanto que el segundo y el penúltimo se calculan mediante la Especificación 4. A partir de aquí obtenemos el vector de intervalos de tiempo estimado

? = ��, �, . . , �� = �max @ A − A�@CDA , N�OE @ A� − A@CDA , … , N�OE @ A� − A,��@CDA �

El primer vértice 01X es entonces seleccionado como 'X si 'X cumple con ser

factible, o seleccionar el vértice factible mediante la conversión de 'X por medio del paso

4. Los vértices restantes (n-1) ( 0 0 02 3, ,..., nX X X ), son calculados como en la ecuación 23.

' 02 1 1 2 2 2

' 03 1 2 1 2 2

' 01 2 2 2 1

[ , , ,..., ]

[ , , ,..., ]

[ , , ,..., ]n

X X d d d d

X X d d d d

X X d d d d

= +

= +

= +M

Ecuación 23

Donde

1

2

( 2)2( 1)

( 1)2( 1)

Dd n n

n

Dd n

n

= + −−

= −−

Ecuación 24

y D es una distancia seleccionada.


Algoritmo2

Paso 1: Inicialización Inicializar kk=0,k3=0.

Seleccionar 1 2,, β∂ ∂

Seleccionar 1 2,∈ ∈

Inicializar VIEJOX=[0,0,…,0] Paso 2. Calcular el primer vértice

Calcular X’ en ecuación (17). Si X’ es factible, entonces

( )1 ';kX X=

Sino

( )1 El vértice factible de la conversión de ' usando FSC.kX X=

Paso 3. Calcular los otros n-1 vértices Para i=2,3,…,n, calcular 'iX

Si 'iX es factible, entonces

( ) ' ;ki iX X=

Sino

( ) El vértice factible de la conversión de ' usando FSC.ki iX X=

Paso 4. De los n vértices de iX , determinar

a) Determinar ( )ksX el cual tiene el valor mínimo de la función objetivo, y

b) Determinar ( )kgX el cual tiene el mayor valor de la función objetivo.

Esto es ( ) ( )( ) min ( )k ks i iT X T X= y ( ) ( )( ) max ( )k k

g i iT X T X= .

Paso 5.

Calcular el centroide ( )kcX de la ecuación (9)

Paso 6. Reflexión.

Calcular el vector reflejado ( )2

knX + mediante las ecuaciones (10) y (12).

Si ( )2

knX + es factible, entonces

2 By Will

3 kk es el número de ciclo, iniciando en cero.

k es el número de etapa, el cual inicia en cero en cada ciclo.


Dejar ( )2

knX + igual.

Sino

( ) ( )2 2 El vértice factible de la conversión de usando FSC.k k

n nX X+ +=

Paso 7. Expandir si la función objetivo de ( )

2k

nX + es menor que la función objetivo de ( )

2k

nX + Si ( ) ( )

2( ) ( )k kn sT X T X+ < entonces

Realizar el inciso a), b), c). Sino Ir al paso 8.

a) Si 1λ < entonces ( ) ( )

2k k

g nX X +=

1k k= + Ir al paso 13.

Sino Continuar.

b) Expandir para obtener ( )3

knX + mediante (13) y (14). Convertir ( )

3k

nX + a un vector

factible mediante FSC en caso de no serlo originalmente. c) Actualizar ( )k

gX mediante ( ) ( ) ( )

( ) 2 2 3( ) ( ) ( )

3 2 3

si ( ) ( )

si ( ) ( )

k k kk n n n

g k k kn n n

X T X T XX

X T X T X+ + +

+ + +

≤= >

1k k= + Ir al paso 13.

Paso 8.

Si ( ) ( )2( ) ( )k k

n pT X T X+ ≤ para algún ( ) ( )k kp gX X≠

( ) ( )2

k kg nX X +=

1k k= + Ir al paso 13

Paso 9.

Si ( ) ( )2( ) ( )k k

n gT X T X+ <

( ) ( )2

k kg nX X +=

Paso 10. Contracción.

Contraer para obtener ( )4

knX + mediante (15). Convertir ( )

4k

nX + para que sea factible

mediante FSC en caso de no serlo originalmente. Paso 11.


Si ( ) ( )4( ) ( )k k

n gT X T X+ <

( ) ( )4

k kg nX X +=

1k k= + Ir al paso 13

Sino Continuar. Paso 12.

Reducir el poliedro mediante el establecimiento de ( ) ( ) ( ) ( )0.5( ), para 1,...,k k k ki s i sX X X X i n← + − =

Paso 13. Checar si el poliedro es muy pequeño.

Si ( ) ( )1

1

nk k

i si

X X=

− <∈∑ entonces

Ir al paso 14 Sino

Ir al paso 4. Paso 14. Checar si las soluciones de dos ciclos están muy cercanas.

Si ( )2

kx sX OLDX− <∈ entonces

La salida ( )ksX es la respuesta. El algoritmo termina.

Sino 0 ( )1

ksX X=

( )ks xOLDX X=

1kk kk= + 0k =

Ir al paso 3.

planificación de trayectorias

Documents