Dinámica Robot

Descargar como pdf o txt
Descargar como pdf o txt
Está en la página 1de 16

Benemérita Universidad Autónoma de Puebla

Facultad Ciencias de la Electrónica

Ingeniera en Mecatrónica

Robótica
Reporte 4: dinámica

Alumno:
Eduardo Arriaga Fernández

Profesor:
Dr. Fernando Reyes Cortes

21 de noviembre de 2019

24 de mayo de 2019
Resumen:

En el presente reporte se realizara el proceso de un control de posición


de un robot de 2 GDL, se describe los pasos para obtener la ecuación
en lazo cerrado expresada en variables de estado, se comprobara si
existe punto de equilibrio y si este es único, además de la propuesta de
una función definida positiva de Lyapunov para así demostrar su
estabilidad, posteriormente se hará la simulación ocupando el software
de MATLAB del algoritmo de control con el modelo dinámico del robot
para posicionar el hombro en 45° y codo a 90°, se propone una
sintonización de ganancia para que los servomotores no se saturen, por
último se trazara con el extremo final del robot una flor de 4 pétalos.
Propósitos:
Competencias profesionales:

 Desarrollar un reporte de calidad el cual debe de estar organizado y bien


desarrollado en cada una de sus secciones.
 Realizar un análisis de datos correcto, para poder resolver el problema de
manera correcta.
 Hacer uso de programas matemáticos para poder agilizar y comprobar la
solución del problema.

Competencias generales
 Realizar y comprender los pasos a seguir para desarrollar el control de
posición de un robot de 2 GDL.
 Realizar en MATLAB las simulaciones solicitadas, ocupando los códigos
vistos en clase y agregando el control de posición requerido.

Desarrollo de habilidades
Para realizar esta práctica del curso de robótica se necesita ocupar todas las
habilidades que fueron desarrolladas durante el transcurso del periodo ya que este
reporte involucra todos los temas antes vistos:
 Hacer uso de los preliminares matemáticos para poder encontrar el punto de
equilibrio del control y demostrar su estabilidad.
 Desarrollar y entender los códigos de MATLAB vistos en clase.
 Analizar e interpretar los resultados obtenidos en la simulación y desarrollo
matemático.

1) Introducción
Control de robots manipuladores es un tema de control automático vigente y de gran
interés para la comunidad científica de robótica debido a los retos teóricos y
prácticos que involucra él diseño de nuevas estrategias de control con alto
desempeño y exactitud en aplicaciones industriales tales como estibado de cajas,
ensamble, traslado, pintado de objetos, etc. El diseño de nuevos esquemas de
control implica mejorar sustancialmente el desempeño˜no de algoritmos de control
tradicionales.

El problema más simple de control de robots manipuladores se denomina control


de posición o regulación el cual se refiere a ubicar o posicionar al extremo final del
robot en un punto constante en el tiempo al que se le denomina posición deseada
o set point. El objetivo de control es posicionar al extremo final del robot en el punto
deseado y que permanezca ahí de manera indefinida. (Reyes 2012).

El problema de regulación o control de posición de robots manipuladores consiste


en colocar el extremo final del robot en una posición deseada 𝑞𝑑 (constante en el
tiempo) para cualquier condición inicial [𝑞(0) 𝑞̇ (0)] 𝒯 ∈ ℝ2𝑛 . Es decir,
matemáticamente el problema se describe como encontrar un regulador τ tal que la
velocidad de movimiento 𝑞̇ (𝑡) y el error de posición 𝑞̃(𝑡) convergen asintóticamente
a cero ∀t ≥ 0 sin importar las condiciones iniciales 𝑞̇ (0) 𝑦 𝑞̃(0) (Reyes, 2019) es
decir:
̃(𝒕)
𝒒 0
lim [ ] → [ ] ∈ ℝ2𝑛 ∀t ≥ 0
𝑡→∞ 𝒒̇ (𝒕) 0

Se necesita un moldeo de Energía, el cual responde a la siguiente ecuación:

̃) − 𝑓𝑣 (𝐾𝑣 , 𝒒̇ ) + 𝑔(𝒒)
𝜏 = ∇𝑈𝑎 (𝐾𝑝 , 𝒒

Donde:

 ∇𝑈𝑎 (𝐾𝑝 , 𝑞̃) > 0 ; ∇𝑈𝑎 𝒯 (𝐾𝑝 , 𝑞̃)𝑞̃ > 0 es la energía potencial artificial.
 𝐾𝑝 ∈ ℝ𝑛x𝑛 es la ganancia proporcional, matriz diagonal definida positiva.
 𝐾𝑣 ∈ ℝ𝑛x𝑛 es la ganancia derivativa, matriz definida positiva.
 𝑓𝑣 (𝐾𝑣 , 𝑞̇ ) es una función disipativa: 𝑞̇ 𝒯 𝑓𝑣 (𝐾𝑣 , 𝑞̇ ) > 0.

1
̃) = 𝒒̇ 𝒯 𝑀(𝒒)𝒒̇ + 𝛻𝑈𝑎 (𝐾𝑝 , 𝑞̃) ⇒ 𝑉̇ (𝒒̇ , 𝒒
𝑉(𝒒̇ , 𝒒 ̃) < 0
2

2) Planteamiento y descripción del problema.

Sea el siguiente algoritmo de control de posición:


2
1 − 𝛼𝑒 −𝛼𝑐𝑜𝑠ℎ (𝒒̃)
𝜏 = 𝐾𝑝 [ ̃)𝑠𝑒𝑛ℎ(𝒒
] 𝑐𝑜𝑠ℎ(𝒒 ̃) − 𝐾𝑣 𝑠𝑒𝑛ℎ(𝒒̇ ) + 𝑔(𝒒)
̃) + 𝑒 −𝛼𝑐𝑜𝑠ℎ2 (𝒒̃)
𝑠𝑒𝑛ℎ2 (𝒒
a) Demostrar la estabilidad del punto de equilibrio de la ecuación en lazo cerrado
(modelo dinámico del robot manipulador (2) y estructura de control (1)).
b) ¿Es único el punto de equilibrio de la ecuación en lazo cerrado?
c) Proponer una función candidata de Lyapunov (demostrar que dicha función sea
definida positiva).
d) Realizar la simulación en MATLAB del algoritmo de control con el modelo
dinámico numérico del robot. prototipo de 2 gdl, para las posiciones deseadas 𝑞𝑑1 =
45° y 𝑞𝑑2 = 90°. Presente las gráficas de los errores de posición y pares aplicados.
Considere el tiempo de simulación 𝑑𝑒 𝑡 = 0 𝑎 5 segundos y un período de muestreo
ℎ = 2.5 𝑚𝑠𝑒𝑔. Sintonice las ganancias del algoritmo de control de tal forma que el
torque de control no sature los servomotores: 𝑇1𝑚𝑎𝑥 = 150 𝑁 ∙ 𝑚, : 𝑇2𝑚𝑎𝑥 = 15 𝑁 ∙ 𝑚
e) Realizar la simulación en MATLAB del algoritmo de control con el modelo
dinámico numérico del robot prototipo de 2 gdl, tal que el extremo final del robot
trace una flor con 4 pétalos de radio 0.3 m y con centro en 𝑥 = 0: 3; 𝑦 = −0: 3𝑚.
Tiempo de simulación de 𝑡 = 0 𝑎 30 𝑠𝑒𝑔𝑢𝑛𝑑𝑜𝑠 y un período de muestreo ℎ =
2.5 𝑚𝑠𝑒𝑔.

Para propósitos de simulación y desarrollo de aplicaciones se requiere el modelo


dinámico numérico del robot manipulador, a continuación, se presenta el modelo
numérico de un robot prototipo de 2 gdl:

3) Solución del problema

El modelo dinámico de un robot manipulador, formado con eslabones rígidos


conectados por articulaciones libres de elasticidad en cadena abierta es el siguiente:
𝜏 = 𝑀(𝐪)𝒒̈ + 𝐶(𝒒, 𝒒̇ )𝒒̇ + 𝑔(𝒒) + 𝐵𝒒̇
Se considera el siguiente algoritmo de control.
2
1 − 𝛼𝑒 −𝛼𝑐𝑜𝑠ℎ (𝒒̃)
𝜏 = 𝐾𝑝 [ ̃)𝑠𝑒𝑛ℎ(𝒒
] 𝑐𝑜𝑠ℎ(𝒒 ̃) − 𝐾𝑣 𝑠𝑒𝑛ℎ(𝒒̇ ) + 𝑔(𝒒)
̃) + 𝑒 −𝛼𝑐𝑜𝑠ℎ2 (𝒒̃)
𝑠𝑒𝑛ℎ2 (𝒒

Igualando ambas expresiones se puede notar que el par gravitacional se elimina,


para demostrar la estabilidad del punto de equilibrio de la ecuación en lazo cerrado
se debe construir la ecuación en variables de estado del sistema resultando lo
siguiente.

−𝒒̇
𝑑 𝒒̃ 2 ̃
1 − 𝛼𝑒 −𝛼𝑐𝑜𝑠ℎ (𝒒 ) 0
[ ]= ̇ = [0]
2 ̃ ] 𝑐𝑜𝑠ℎ 𝒒 𝑠𝑒𝑛ℎ 𝒒 − 𝐾𝑣 𝑠𝑒𝑛ℎ 𝒒 − 𝐶(𝒒, 𝒒)𝒒 − 𝐵𝒒]
𝑑𝑡 𝒒̇ 𝑀(𝑞)−1 [𝐾𝑝 [ (̃) (̃) ( ̇) ̇ ̇
2
[ 𝑠𝑒𝑛ℎ (𝒒̃ ) + 𝑒−𝛼𝑐𝑜𝑠ℎ (𝒒) ]

Ahora se comprueba que el punto de equilibrio existe y es único.


i) −𝒒̇ = −(𝐼)𝒒̇ = 0 ⟺ 𝒒̇ = 0 ∈ ℝ𝑛
ii)
a. 𝐵𝒒̇ ⟺ 𝒒̇ = 0 𝐵 > 0
b. 𝐶(𝒒, 0) = 0 ℝ𝑛𝑥𝑛 , 𝐶(𝒒, 𝒒̇ )𝒒̇ = 0 ⟺ 𝑞́ = 0
c. 𝑠𝑒𝑛ℎ(0) = 0, 𝐾𝑣 𝑠𝑒𝑛ℎ(𝒒̇ ) ⟺ 𝒒̇ = 0 , 𝐾𝑣 > 0
2 ̃)
2 (𝒒
̃) 2 (𝒒
̃) 1−𝛼𝑒 −𝛼𝑐𝑜𝑠ℎ (𝒒
d. 𝑒 −𝛼𝑐𝑜𝑠ℎ > 0; 1 − 𝛼𝑒 −𝛼𝑐𝑜𝑠ℎ > 0 𝐾𝑝 [ )𝑠𝑒𝑛ℎ(𝒒
̃
2 ̃) ] 𝑐𝑜𝑠ℎ(𝒒
̃)+𝑒 −𝛼𝑐𝑜𝑠ℎ (𝒒
𝑠𝑒𝑛ℎ 2 (𝒒
̃) = 0 ⟺
𝒒̇ = 0 , 𝐾𝑝 > 0

∴ El punto de equilibrio existe y es único.

Posteriormente se propone una función candidata de Lyapunov, con el objetivo de


realizar un análisis de estabilidad y demostrar que esta es definida positiva. La
función de Lyapunov propuesta tiene la siguiente estructura.

1
𝑉(𝑞̇ , 𝑞̃) = 𝑞̇ 𝑇 𝑀(𝑞)𝑞̇ + 𝒰𝑎 (𝐾𝑝 , 𝑞
̃)
2

Donde la energía potencia está definida por:


𝑇
1
̃) = √ln(𝑠𝑒𝑛ℎ2 (𝒒̃) + 𝑒
𝒰𝑎 (𝐾𝑝 , 𝒒 −𝛼𝑐𝑜𝑠ℎ2 𝒒 (̃ ) ) + 1 𝐾𝑝 √ln(𝑠𝑒𝑛ℎ2 (𝒒̃) + 𝑒−𝛼𝑐𝑜𝑠ℎ (𝒒̃) ) + 1
2

Posteriormente se comprueba que la función candidata es positiva a través de una


notación adecuada para poder ocupar el teorema de Sylvester.
𝒒̇ 𝑀(𝒒) 𝑞̇
𝑇
0
𝑉(𝑞̇ , 𝑞̃) = ⌊ ⌋ [ 2 ] ⌊ ⌋
2 2
√ln(𝑠𝑒𝑛ℎ2 (𝒒̃ ) + 𝑒−𝛼𝑐𝑜𝑠ℎ (𝒒̃ ) ) + 1 ̃ ) + 𝑒−𝛼𝑐𝑜𝑠ℎ (𝒒̃ ) ) + 1
𝐾𝑝 √ln(𝑠𝑒𝑛ℎ2 (𝒒
0
2

𝑀(𝒒)
0
[ 2 ]=𝐴
𝐾𝑝
0
2

Ocupando el teorema de Sylvester se puede notar que:

1. 𝐴 = 𝐴𝑇
𝑀(𝒒)
2. 𝑎11 = >0
2
𝑀(𝒒)𝐾𝑝
3. 𝑑𝑒𝑡|𝐴| = 4
>0

∴ La función propuesta es definida positiva, entonces se puede comprobar la


estabilidad del sistema.
1 𝑑
Se debe obtener 𝑉̇ (𝑞̇ , 𝑞̃) = 𝑞̇ 𝑇 𝑀(𝑞)𝑞̈ + 2 𝑞̇ 𝑇 𝑀(𝑞)𝑞̇ + 𝑑𝑡 𝒰𝑎 (𝐾𝑝 , 𝑞̃)

𝑑 𝑇 𝑑
Recordando 𝑑𝑡
𝒙 𝐴𝒙 = 𝟐𝒙𝑇 𝐴 𝑑𝑡 𝒙 entonces.

𝑇
𝑑 2 𝑑 2
𝒰𝑎 (𝐾𝑝 , 𝑞̃) = 2√ln(𝑠𝑒𝑛ℎ2 (𝒒̃ ) + 𝑒−𝛼𝑐𝑜𝑠ℎ (𝒒̃ ) ) + 1 𝐾𝑝 √ln(𝑠𝑒𝑛ℎ2 (𝒒̃ ) + 𝑒−𝛼𝑐𝑜𝑠ℎ (𝒒̃ ) ) + 1
𝑑𝑡 𝑑𝑡

𝑇
𝑞̃̇ (1 − 𝛼𝑒−𝛼𝑐𝑜𝑠ℎ (𝒒̃) )𝑐𝑜𝑠ℎ(𝒒̃ )𝑠𝑒𝑛ℎ(𝒒̃ )
2
2 2 1
̃ ) + 𝑒 −𝛼𝑐𝑜𝑠ℎ (𝒒̃) ) + 1 𝐾𝑝
= √𝑙𝑛(𝑠𝑒𝑛ℎ2 (𝒒 ∙ 2
2 2 𝑠𝑒𝑛ℎ2 (𝒒̃ ) + 𝑒 −𝛼𝑐𝑜𝑠ℎ (𝒒̃)
̃) + 𝑒 −𝛼𝑐𝑜𝑠ℎ (𝒒̃) ) + 1
√𝑙𝑛(𝑠𝑒𝑛ℎ2 (𝒒
[ ]

2 (𝒒
̃)
Se puede notar que puede eliminarse el valor de √ln(𝑠𝑒𝑛ℎ2 (𝒒
̃) + 𝑒 −𝛼𝑐𝑜𝑠ℎ ) + 1 al
desarrollar la notación de la siguiente forma.

2 (𝒒
= [√ln(𝑠𝑒𝑛ℎ2 (𝒒
̃𝟏 ) + 𝑒 −𝛼𝑐𝑜𝑠ℎ ̃ 𝟏) ̃𝒏 ) + 𝑒 −𝛼𝑐𝑜𝑠ℎ2 (𝒒̃𝒏) ) + 1] ∙
) + 1 ⋯ √ln(𝑠𝑒𝑛ℎ2 (𝒒

2
1 (1 − 𝛼𝑒 −𝛼𝑐𝑜𝑠ℎ (𝒒̃𝟏 ))𝑐𝑜𝑠ℎ(𝒒
̃ 𝟏 )𝑠𝑒𝑛ℎ(𝒒̃𝟏 )
∙ ⋯ 0
𝑠𝑒𝑛ℎ2 (𝒒̃𝟏 ) + 𝑒 −𝛼𝑐𝑜𝑠ℎ2 (𝒒̃𝟏 )
̃ 𝟏 ) + 𝑒 −𝛼𝑐𝑜𝑠ℎ2 (𝒒̃𝟏 )) + 1
√ln(𝑠𝑒𝑛ℎ2 (𝒒
⋮ ⋱ ⋮ ̃̇
𝒒 𝐾𝑝
2
1 (1 − 𝛼𝑒 −𝛼𝑐𝑜𝑠ℎ (𝒒̃𝒏) )𝑐𝑜𝑠ℎ(𝒒
̃𝒏 )𝑠𝑒𝑛ℎ(𝒒
̃𝒏)
0 ⋯ ∙ 2
𝑠𝑒𝑛ℎ2 (𝒒̃𝒏 ) + 𝑒 −𝛼𝑐𝑜𝑠ℎ (𝒒̃𝒏)
̃ 𝒏 ) + 𝑒 −𝛼𝑐𝑜𝑠ℎ2 (𝒒̃𝒏) ) + 1
√ln(𝑠𝑒𝑛ℎ2 (𝒒
[ ]
El valor de la derivada de la energía potencial es:
2 𝑇
𝑑 (1 − 𝛼𝑒−𝛼𝑐𝑜𝑠ℎ (𝒒̃ ) )𝑐𝑜𝑠ℎ(𝒒
̃ )𝑠𝑒𝑛ℎ(𝒒̃ )
̃) = [
𝒰𝑎 (𝐾𝑝 , 𝒒 2 ] 𝒒̃ ̇ 𝐾𝑝
𝑑𝑡 ̃ ) + 𝑒−𝛼𝑐𝑜𝑠ℎ (𝒒̃ )
𝑠𝑒𝑛ℎ2 (𝒒

2 𝑇
1 (1−𝛼𝑒 −𝛼𝑐𝑜𝑠ℎ (𝑞̃) )𝑐𝑜𝑠ℎ(𝑞̃)𝑠𝑒𝑛ℎ(𝑞̃)
Entonces 𝑉̇ (𝑞̇ , 𝑞̃) = 𝑞̇ 𝑇 𝑀(𝑞)𝑞̈ + 2 𝑞̇ 𝑇 𝑀(𝑞)𝑞̇ + [ 2 ] 𝑞̃̇ 𝐾𝑝
𝑠𝑒𝑛ℎ2 (𝑞̃)+𝑒 −𝛼𝑐𝑜𝑠ℎ (𝑞̃)

Sustituyendo los valores de 𝑞̈ 𝑦 𝑞̃̇ .


2
1 − 𝛼𝑒 −𝛼𝑐𝑜𝑠ℎ (𝒒̃) 1
𝑉̇ (𝒒̇ , 𝒒
̃) = 𝑞̇ 𝑇 𝐾𝑝 [ 2 (𝒒 )
̃) − 𝒒̇ 𝑇 𝐾𝑣 𝑠𝑒𝑛ℎ(𝒒̇ ) − [𝒒̇ 𝑇 𝐶(𝒒, 𝒒̇ )𝒒̇ + 𝒒̇ 𝑇 𝑀(𝒒)𝒒̇ ]
̃)𝑠𝑒𝑛ℎ(𝒒
] 𝑐𝑜𝑠ℎ(𝒒
2
𝑠𝑒𝑛ℎ (𝒒̃) + 𝑒 −𝛼𝑐𝑜𝑠ℎ ̃ 2
2 𝑇
𝑇
(1 − 𝛼𝑒 −𝛼𝑐𝑜𝑠ℎ (𝒒̃) )𝑐𝑜𝑠ℎ(𝒒̃)𝑠𝑒𝑛ℎ(𝒒 ̃)
− 𝒒̇ 𝐵𝒒̇ − [ 2 −𝛼𝑐𝑜𝑠ℎ 2 (𝒒
̃ )
] 𝒒̇ 𝐾𝑝
𝑠𝑒𝑛ℎ (𝒒̃) + 𝑒

El elemento [𝒒̇ 𝑇 𝐶(𝒒, 𝒒̇ )𝒒̇ + 12 𝒒̇ 𝑇 𝑀(𝒒)𝒒̇ ] puede apreciarse que por la propiedad
2 ̃)
1−𝛼𝑒 −𝛼𝑐𝑜𝑠ℎ (𝒒
antisimétrica es 0 además el elemento 𝒒̇ 𝑇 𝐾𝑝 [ 2 ̃) ] se elimina con
̃)+𝑒 −𝛼𝑐𝑜𝑠ℎ (𝒒
𝑠𝑒𝑛ℎ2 (𝒒
2 ̃)
(1−𝛼𝑒 −𝛼𝑐𝑜𝑠ℎ (𝒒 ̃)𝑠𝑒𝑛ℎ(𝒒
)𝑐𝑜𝑠ℎ(𝐪 ̃)
[ 2 ̃)
̃)+𝑒 −𝛼𝑐𝑜𝑠ℎ (𝒒
𝑠𝑒𝑛ℎ2 (𝒒
] 𝒒̇ 𝐾𝑝 , recordando la propiedad 𝒙𝑻 𝒚 = 𝒚𝑻 𝒙.

Por ultimo: 𝑉̇ (𝒒̇ , 𝒒


̃) = −𝒒̇ 𝑇 𝐾𝑣 𝑠𝑒𝑛ℎ(𝒒̇ ) − 𝒒̇ 𝑇 𝐵𝒒̇ ≤ 0

∴ Queda demostrada la estabilidad del punto de equilibrio de la ecuación en


lazo cerrado.
Para realizar la simulación en MATLAB del algoritmo de control se debe hacer la
sintonización de ganancias de tal forma que el torque de control no sature los
servomotores: 𝑇1𝑚𝑎𝑥 = 150 𝑁 ∙ 𝑚, : 𝑇2𝑚𝑎𝑥 = 15 𝑁 ∙ 𝑚.

̃ = 𝒒𝒅 − 𝒒̇ si la velocidad es 0 entonces 𝒒
Tomando en cuenta que 𝒒 ̃ = 𝒒𝒅 por lo tanto
se puede escribir.
2 2
(1−𝛼𝑒 −𝛼𝑐𝑜𝑠ℎ (𝒒𝒅 ) )𝑐𝑜𝑠ℎ(𝒒𝒅)𝑠𝑒𝑛ℎ(𝒒𝒅 ) (1−𝛼𝑒 −𝛼𝑐𝑜𝑠ℎ (𝒒𝒅 ) )𝑐𝑜𝑠ℎ(𝒒
̃)𝑠𝑒𝑛ℎ(𝒒𝒅 )
𝜏 𝑚𝑎𝑥 = 𝐾𝑝 2 ; 2 = 𝑓𝑞𝑑
𝑠𝑒𝑛ℎ2 (𝒒𝒅 )+𝑒 −𝛼𝑐𝑜𝑠ℎ (𝒒𝒅 ) 𝑠𝑒𝑛ℎ2 (𝒒𝒅 )+𝑒 −𝛼𝑐𝑜𝑠ℎ (𝒒𝒅 )

𝜏𝑚𝑎𝑥
Despejando: 𝐾𝑝 = 0.8 ; 0.8 se agrega para no ocupar el valor máximo y así
𝑓𝑞𝑑
no saturar los servomotores.
Además de esto es necesario notar que 𝑞𝑑 se compone de 𝑞𝑑1 𝑦 𝑞𝑑2.

Por lo tanto:

𝜏𝑚𝑎𝑥 𝜏𝑚𝑎𝑥
𝑘𝑝1 = 0.8 𝑓1 𝑦 𝑘𝑝2 = 0.8 𝑓2
𝑞𝑑1 𝑞𝑑2

La ganancia 𝑘𝑣 se define como 𝑘𝑣1 = 𝛽𝑘𝑝1 y 𝑘𝑣2 = 𝛽𝑘𝑝2 donde 𝛽 está acotado
por 0 < 𝛽 ≤ 1 tambien se debe mencionar que el valor de 𝛼 cumple con lo mismo
0 < 𝛼 ≤ 1.

Se hizo uso de MATLAB para encontrar los valores de las ganancias, dicho código
se encuentra anexado en el apéndice.

Obteniendo: 𝑘𝑝1 = 181.42 𝑦 𝑘𝑝2 = 13.467

Para hallar los valores de 𝛽 y 𝛼 se realizó de manera de prueba y error, esto


intentando tener el menor el menor valor de error posible.

𝛽1 = 0.3 y 𝛽2 = 0.3
𝛼 = 0.1

Una vez hecho esto se realiza la simulación en MATLAB del algoritmo de control
con el modelo dinámico numérico del robot (que se encuentra en el planteamiento
y descripción del problema). prototipo de 2 gdl, para las posiciones deseadas 𝑞𝑑1 =
45° y 𝑞𝑑2 = 90°. Se consideró un tiempo de simulación 𝑑𝑒 𝑡 = 0 𝑎 5 segundos y un
período de muestreo ℎ = 2.5 𝑚𝑠𝑒𝑔. Los códigos ocupados se encuentran en el
apéndice.

Los valores de tau se obtuvieron de la función ode realizando una ligera


modificación.

Se muestran 3 graficas donde el hombro es la gráfica azul y el codo la gráfica roja,


la primera corresponde a la posición de las articulaciones, la segunda al error de
posición de estas mismas y la tercera al par aplicado.

Observando la primera y segunda grafica podemos notar que el error de posición


tiende a 0, por lo que se llega a la posición requerida de 45 y 90 grados
respectivamente en un tiempo de 2 segundos.

Con la gráfica 3 podemos afirmar que los servomotores no se saturan ya que no


sobrepasan los torques máximos de 150 y 15 N.m.
Figura 1. Graficas de simulación robot 2 GDL.

Por último se debe trazar una flor con 4 pétalos de radio 0.3 m y con centro en 𝑥 =
0: 3𝑚; 𝑦 = −0: 3𝑚. Tiempo de simulación de 𝑡 = 0 𝑎 30 𝑠𝑒𝑔𝑢𝑛𝑑𝑜𝑠 y un período de
muestreo ℎ = 2.5 𝑚𝑠𝑒𝑔.

Para esto se hizo uso de la cinemática inversa para calcular los valores de 𝒒𝒅 ,
después se ocupa la cinemática directa para obtener la trayectoria deseada. Las
ganancias ocupadas fueron las mismas para las ganancias 𝐾𝑝 y para las ganancias
𝐾𝑣 se necesitó un pequeño ajuste ya que la figura de la flor no se realizaba de una
manera correcta.

Figura 2. Flor de 4 pétalos.


Como se puede apreciar la flor de 4 pétalos tiene su centro en 𝑥 = 0: 3 𝑚; 𝑦 =
−0: 3𝑚 y tiene un radio de 0.3 m. Los códigos se encuentran anexados en el
apéndice.

Conclusión
Realizar todo el proceso de un algoritmo de control en un robot no es una tarea
sencilla puesto que se debe tener conocimientos de la cinemática y la dinámica del
robot. Es importante antes de simular el control, demostrar la estabilidad del punto
de equilibrio de la ecuación en lazo cerrado, ya que sin esto no se podría demostrar
que el control propuesto servirá de una manera correcta, además, es necesario
sintonizar las ganancias para que los servomotores no se saturen; una vez hecho
esto en la simulación es esencial ajustar la ganancia derivativa para lograr que en
las articulaciones del robot exista el menor error posible y así llegue a la posición
deseada, para lograr que se trace una figura con el extremo final del robot, se debe
hacer uso de la cinemática inversa ya que es la herramienta adecuada para poder
trazar la trayectoria de la figura puesto que calcula punto por punto la trayectoria
deseada, si se conoce la ecuación.

Referencias bibliográficas
1. Reyes Cortés Fernando, MATLAB: Aplicado a robótica y mecatrónica, Alfaomega,
México, 2012.
2. Dr. Fernando Reyes Cortés, “Capítulo 3: Dinamica” 2019
Apéndice:
Sintonía de ganancias:
alpha=0.1;
qd1=45*pi/180; %radianes
qd2=90*pi/180;
Tmax1=150;
Tmax2=15;
c1=exp(-(alpha*(cosh(qd1)).^2));
c2=exp(-(alpha*(cosh(qd2)).^2));
fqd1=((1-alpha*c1)./(((sinh(qd1)).^2)+c1)).*(sinh(qd1).*cosh(qd1));
fqd2=((1-alpha*c2)./(((sinh(qd2)).^2)+c1)).*(sinh(qd2).*cosh(qd2));
kp1=0.8*Tmax1/fqd1
kp2=0.8*Tmax2/fqd2

Función de la dinámica de robot de 2 GDL.

function xp =robot2gdl(t,x)
q1=x(1);
q2=x(2);
q = [q1;
q2];
%vector de posición articular
qp1=x(3);
qp2=x(4);
qp = [qp1;
qp2];%vector de velocidad articular
M=[2.351+0.1676*cos(q2), 0.102+0.0838*cos(q2);
0.102+0.0838*cos(q2), 0.102];
%matriz de fuerzas centrípetas y de Coriolis
C=[-0.1676*sin(q2)*qp2, -0.0838*sin(q2)*qp2;
0.084*sin(q2)*qp1, 0.0];
%par gravitacional
gq=9.81*[3.9211*sin(q1)+0.1862*sin(q1+q2);
0.1862*sin(q1+q2)];
%fricción viscosa
fr=[2.288*qp1;
0.175*qp2];
[~, tau]=alg_control(t, q,qp);%en esta línea se inserta el controlador
qpp = M^(-1)*(tau-C*qp-gq-fr);
%vector de salida
xp = [qp1; qp2; qpp(1); qpp(2); tau(1); tau(2)];
end
Función del algoritmo de control.

function [qt, tau] = alg_control(t,x,xp)


alpha=0.1;
q = [x(1); x(2)];% posiciones articulares.
qp = [xp(1); xp(2)];% velocidades articulares.
qd1=45; qd2=90;%referencias.
qd=[qd1; qd2];% vector de referencias.
kp1= 181.42;
kp2= 13.467;
Kp=[kp1, 0; 0, kp2];% ganancia proporcional.
kv1=.3*kp1;
kv2=.3*kp2;
Kv=[kv1, 0; 0, kv2];% ganancia derivativa.
% qd1=45; qd2=90;%referencias.
% qd=[qd1; qd2];% vector de referencias.
par_grav = [38.46*sin(q(1))+1.82*sin(q(1)+q(2));% vector de pares de
gravitacionales.
1.82*sin(q(1)+q(2)) ];
qt=[pi*qd(1)/180-q(1); pi*qd(2)/180-q(2)];% error de posición en radianes.
% qtgrados=(180/pi)*[qt(1) ;qt(2)];% error de posición en grados.
% qpgrados=180*qp/pi;% velocidad en grados/segundo.
c1=exp(-(alpha*(cosh(qt)).^2));
tau=Kp*((1-alpha.*c1)./(((sinh(qt)).^2)+c1)).*(sinh(qt).*cosh(qt))-
Kv*sinh(qp)+par_grav; %Control
end

Programa principal de la simulación inciso d)

clc;
clear all;
close all;
format short g
ti=0; h=0.0025; tf =5;
t=ti:h:tf;%vector tiempo
ci=[0; 0; 0; 0; 0 ;0];%condiciones iniciales
opciones=odeset('RelTol',1e-6,'InitialStep', h,'MaxStep',h);
%solución numérica del sistema dinámico del robot de 2gdl
[t, x]=ode45('robot2gdl',t,ci,opciones);
q1=x(:,1); q2=x(:,2);
qp1=x(:,3); qp2=x(:,4);
tauf1=x(:,5); tauf2=x(:,6);

tau1=fliplr(tauf1); tau2=fliplr(tauf2);
qt1=45*pi/180-q1;
qt2=90*pi/180-q2;

subplot(3,1,1); plot( t, 180*q1/pi, t,180*q2/pi)


subplot(3,1,2); plot( t, (180*qt1/pi), t,(180*qt2/pi))
subplot(3,1,3); plot(t,tau1,t,tau2)

Función de la dinámica para flor.

function xp =robot2FLORgdl(t,x)
q1=x(1);
q2=x(2);
q = [q1;
q2];
%vector de posición articular
qp1=x(3);
qp2=x(4);
qp = [qp1;
qp2];%vector de velocidad articular
M=[2.351+0.1676*cos(q2), 0.102+0.0838*cos(q2);
0.102+0.0838*cos(q2), 0.102];
%matriz de fuerzas centrípetas y de Coriolis
C=[-0.1676*sin(q2)*qp2, -0.0838*sin(q2)*qp2;
0.084*sin(q2)*qp1, 0.0];
%par gravitacional
gq=9.81*[3.9211*sin(q1)+0.1862*sin(q1+q2);
0.1862*sin(q1+q2)];
%fricción viscosa
fr=[2.288*qp1;
0.175*qp2];
[~, tau]=alg_controlflor4p(t, q,qp);%en esta línea se inserta el controlador
MODIFICAR ESTE
qpp = M^(-1)*(tau-C*qp-gq-fr);
%vector de salida
xp = [qp1; qp2; qpp(1); qpp(2)];
end

Función del algoritmo de control para flor.

function [qt ,tau] = alg_controlflor4p(t,x,xp)


alpha=0.1;
r=0.3; % radio petalo
R =0.3*sin(2*t);
xc=0.3; %Posicion x centro de la figura
yc=0.3; %Posicion y centro de la figura
l1=0.45; l2=0.45;
q = [x(1); x(2)];% posiciones articulares.
qp = [xp(1); xp(2)];% velocidades articulares.
x = xc+R.*cos(t); % valores de x para obtener el petalo con centro en x= 0.3
y = yc+R.*sin(t); % valores de y para obtener el petalo con centro en y= -0.3
[qd1 ,qd2]=cinv_r2gdl(l1,l2,x,y);
qd=[qd1; qd2];
kp1= 181.42;
kp2= 13.467;
kv1=.1*kp1;
kv2=.1*kp2;
Kp=[kp1, 0; 0, kp2];%ganancia proporcional
Kv=[kv1, 0; 0, kv2];%ganancia derivativa
par_grav = [38.46*sin(q(1))+1.82*sin(q(1)+q(2)); %vector de pares de
gravitacionales
1.82*sin(q(1)+q(2))];
qt=[qd(1)-q(1); qd(2)-q(2)]; %error de posicion
c1=exp(-(alpha*(cosh(qt)).^2));
tau=Kp*((1-alpha*c1)./(((sinh(qt)).^2)+c1)).*(sinh(qt).*cosh(qt))-
Kv*sinh(qp)+par_grav; %Control
end

Programa principal de la simulación inciso e)

clc;
clear all;
close all;
format short g
ti=0; h=0.0025; tf =30;
t=ti:h:tf;%vector tiempo
ci=[0; 0; 0; 0];%condiciones iniciales
opciones=odeset('RelTol',1e-6,'InitialStep', h,'MaxStep',h);
%solución numérica del sistema dinámico del robot de 2gdl
[t, x]=ode45('robot2FLORgdl',t,ci,opciones);
q1=x(:,1); q2=x(:,2);
qp1=x(:,3); qp2=x(:,4);
%cinemática directa asociada al cuarto cuadrante
l1=0.45; l2=0.45; beta1=0.15; beta2=0.15;
[n,m]=size(t);
x0=zeros(n,1);
y0=zeros(n,1);
[x0, y0, ~]=cinematica_r2gdl(beta1,l1,q1,beta2,l2,q2);%coordenas cartesianas del
extremo final del robot de 2 gdl
plot(x0,y0)

También podría gustarte