TRABAJO FIN DE GRADO -...
Transcript of TRABAJO FIN DE GRADO -...
TRABAJO FIN DE GRADO
DINÁMICA DIRECTA DE LA MARCHA HUMANA POR MÉTODOS DE CONTROL
AUTOR Santiago Rodríguez Díaz
TUTORES Javier Cuadrado Aranda Urbano Lugrís Armesto
JULIO, 2015ESCUELA POLITÉCNICA SUPERIOR DE
FERROL
30/07/2015 ESCUELA POLITÉCNICA SUPERIOR1
INTRODUCCIÓN
Motivación:
Comprensión de la marcha normal y
patológica.
Aplicación médica.
30/07/2015 ESCUELA POLITÉCNICA SUPERIOR
2
Objetivo: Reproducir un movimiento capturado
mediante dinámica directa.
Ecuaciones del movimiento.
Integración.
DEFINICIÓN DEL MODELO
30/07/2015 ESCUELA POLITÉCNICA SUPERIOR
4
Modelo 3D
18 sólidos.
17 articulaciones
57 GDL.
Modelo 2D
12 sólidos.
11 articulaciones
14 GDL
Cadena compuesta por un conjunto de sólidos rígidos unidos mediante articulaciones ideales.
OBTENCIÓN DE DATOS
Sujeto con 37 markers en su cuerpo camina por una pasarela con dos placas de fuerza.
El movimiento es capturado por un sistema óptico compuesto por 12 cámaras.
Software: Computa la posición de los markers y proporciona su trayectoria.
BSP: Escalado y medición
30/07/2015 ESCUELA POLITÉCNICA SUPERIOR
5
OBTENCIÓN DE DATOS: PARÁMETROS DE ENTRADA
30/07/2015 ESCUELA POLITÉCNICA SUPERIOR
6
Historia de posiciones de losmarkers.
Historias de posiciones dearticulaciones y puntosextremos en coordenadasnaturales (𝑞𝑞3𝐷𝐷∗ ).
Relaciones
algebraicas
OBTENCIÓN DE DATOS: PARÁMETROS DE ENTRADA
30/07/2015 ESCUELA POLITÉCNICA SUPERIOR
7
𝑞𝑞3𝐷𝐷∗ 𝑞𝑞2𝐷𝐷∗Proyección
Inconsistencia cinemática.
Errores causados por:
Movimiento de los markers.
Ruidos en medición.
Paso a 2D
Modificación de la posición de las
articulaciones para satisfacer restricciones.
Problema de minimización.
�min𝑓𝑓 𝒒𝒒 = 𝒒𝒒 − 𝒒𝒒∗ 𝑇𝑇𝑾𝑾 𝒒𝒒 − 𝒒𝒒∗𝑠𝑠𝑠𝑠. 𝚽𝚽 𝒒𝒒 = 0
𝜙𝜙1 = 𝑥𝑥1 − 𝑥𝑥2 2 + 𝑦𝑦1 − 𝑦𝑦2 2 − 𝑙𝑙122
OBTENCIÓN DE DATOS: PARÁMETROS DE ENTRADA
Coordenadas naturales consistentes
30/07/2015 ESCUELA POLITÉCNICA SUPERIOR
8
Coordenadas independientes𝒛𝒛 = 𝒙𝒙4,𝒚𝒚4,𝜽𝜽1, … ,𝜽𝜽12
Filtrado de datos sobre variables independientes
Velocidades y aceleraciones (��𝒛, ��𝒛)
DINÁMICA INVERSA: ECUACIONES DE NEWTON - EULER
Se conoce: 𝑎𝑎𝑥𝑥,𝑎𝑎𝑦𝑦 = aceleración CDG. 𝜃𝜃 = ángulo en el plano. 𝛼𝛼 = aceleración angular. 𝑅𝑅𝑥𝑥𝑥𝑥 ,𝑅𝑅𝑦𝑦𝑥𝑥 = fuerzas de reacción en la
articulación distal. 𝑀𝑀𝑥𝑥 = momento muscular en la articulación
distal.
Se desconoce 𝑅𝑅𝑥𝑥𝑥𝑥,𝑅𝑅𝑦𝑦𝑥𝑥 = fuerzas de reacción en la
articulación proximal. 𝑀𝑀𝑥𝑥 = momento en la articulación proximal.
30/07/2015 ESCUELA POLITÉCNICA SUPERIOR
9
∑𝐹𝐹𝑥𝑥 = 𝑚𝑚𝑎𝑎𝑥𝑥; ∑𝐹𝐹𝑦𝑦 = 𝑚𝑚𝑎𝑎𝑦𝑦; ∑𝑀𝑀 = 𝐼𝐼𝛼𝛼
DINÁMICA DIRECTA: ECUACIONES DE LAGRANGE
Energía cinética
𝑇𝑇 =12𝑚𝑚𝑣𝑣𝐺𝐺2 +
12𝐼𝐼𝐺𝐺𝛼𝛼
30/07/2015 ESCUELA POLITÉCNICA SUPERIOR
10
Función Lagrangiana.
𝐿𝐿 = 𝑇𝑇 − 𝑉𝑉
Energía potencial
𝑉𝑉 = 𝑚𝑚𝑚𝑚𝑦𝑦𝐺𝐺
Ec. Lagrange𝜕𝜕𝜕𝜕𝑠𝑠
𝜕𝜕𝐿𝐿𝜕𝜕��𝑧𝑖𝑖
−𝜕𝜕𝐿𝐿𝜕𝜕𝑧𝑧𝑖𝑖
= 𝑄𝑄𝑖𝑖 , 𝑖𝑖 = 1, … ,𝑛𝑛
Sistema de ecuaciones𝑴𝑴 𝒛𝒛 ��𝒛 = 𝑸𝑸𝒘𝒘 𝒛𝒛, ��𝒛 + 𝑸𝑸𝒗𝒗 𝒛𝒛 + 𝑸𝑸𝒄𝒄
Duplicación de variables
𝒚𝒚 = 𝒛𝒛��𝒛 → ��𝒚 = ��𝒛
��𝒛
Posiciones y velocidades de las
coordenadas independientes 𝒛𝒛
Integración de la forma:��𝒚 = 𝒇𝒇 𝒕𝒕,𝒚𝒚
DINÁMICA DIRECTA: CONTROLADOR
Computed feedforward (Lazo abierto) 𝑸𝑸𝒄𝒄 = 𝑴𝑴 𝒛𝒛 ��𝒛 − 𝑸𝑸𝒘𝒘 𝒛𝒛, ��𝒛 − 𝑸𝑸𝒗𝒗 𝒛𝒛 Sistema inestable: necesidad de un
controlador.
El controlador debe seguir una trayectoria
deseada (𝒛𝒛𝑥𝑥).
Feedback: controlador PD. 𝑸𝑸𝒄𝒄 = 𝑲𝑲𝑥𝑥 𝒛𝒛𝒅𝒅 − 𝒛𝒛𝒓𝒓 + 𝑲𝑲𝒗𝒗 ��𝒛𝒅𝒅 − ��𝒛𝒓𝒓
CTC: Computed Torque Control.
30/07/2015 ESCUELA POLITÉCNICA SUPERIOR
11
DINÁMICA DIRECTA: CTC
𝑴𝑴 𝒛𝒛𝒓𝒓 ��𝒛𝒓𝒓 = 𝑸𝑸𝑤𝑤 𝒛𝒛𝒓𝒓, ��𝒛𝒓𝒓 + 𝑸𝑸𝑣𝑣 𝒛𝒛𝒓𝒓 + 𝑸𝑸𝑐𝑐
30/07/2015 ESCUELA POLITÉCNICA SUPERIOR
12
𝑸𝑸𝑐𝑐 = 𝑴𝑴 𝒛𝒛𝒂𝒂 ��𝒛𝑥𝑥 + 𝑲𝑲𝒗𝒗 ��𝒛𝒅𝒅 − ��𝒛𝒓𝒓 + 𝑲𝑲𝒑𝒑 𝒛𝒛𝒅𝒅 − 𝒛𝒛𝒓𝒓 − 𝑸𝑸𝑊𝑊 ��𝒛𝒓𝒓, 𝒛𝒛𝒓𝒓 − 𝑸𝑸𝑣𝑣 𝒛𝒛𝒓𝒓
��𝒛𝑥𝑥 − ��𝒛𝑟𝑟 + 𝑲𝑲𝑣𝑣 ��𝒛𝒅𝒅 − ��𝒛𝒓𝒓 + 𝑲𝑲𝑥𝑥 𝒛𝒛𝑥𝑥 − 𝒛𝒛𝒓𝒓 = 0
Para proporcionar una respuesta críticamente amortiguada: 𝐾𝐾𝑣𝑣𝑖𝑖 = 2 𝐾𝐾𝑥𝑥𝑖𝑖Podemos despejar e integrar las posiciones del sistema volviendo a emplear la duplicación de
variables:��𝒛𝑟𝑟 = 𝑲𝑲𝑣𝑣 ��𝒛𝑥𝑥 − ��𝒛𝒓𝒓 + 𝑲𝑲𝑥𝑥 𝒛𝒛𝑥𝑥 − 𝒛𝒛𝒓𝒓 + ��𝒛𝑥𝑥
𝜖𝜖 + 𝑲𝑲𝑣𝑣 ��𝝐 + 𝑲𝑲𝑥𝑥 𝝐𝝐 = 0
DINÁMICA DIRECTA: CTC
30/07/2015 ESCUELA POLITÉCNICA SUPERIOR
13
Dinámica directa con el integrador CTCMovimiento de captura
RESULTADOS
30/07/2015 ESCUELA POLITÉCNICA SUPERIOR
14
ERRORES EN POSICIÓN
0 20 40 60 80 100 120 140 1601.35
1.4
1.45
1.5
1.55
1.6
Centésimas de segundo
Ra
d
Comparación en θ1. Gain = 10000
zrzd
0 20 40 60 80 100 120 140 1601.35
1.4
1.45
1.5
1.55
1.6
Centésimas de segundo
Ra
d
Comparación en θ1. Gain = 0.0001
zrzd
RESULTADOS
30/07/2015 ESCUELA POLITÉCNICA SUPERIOR
15
DIFERENCIAS EN FUERZAS
0 20 40 60 80 100 120 140 160-40
-20
0
20
40
60
80
Centésimas de segundo
Ne
wto
n·m
etr
o
Comparación en θ1. Gain = 0.0001
FcQext
0 20 40 60 80 100 120 140 160-40
-20
0
20
40
60
80
Centésimas de segundo
Ne
wto
n·m
etr
o
Comparación en θ1. Gain = 10000
FcQext
RESULTADOS
30/07/2015 ESCUELA POLITÉCNICA SUPERIOR
16
DIFERENCIAS EN FUERZASERRORES EN POSICIÓN
1 2 3 4 50
0.2
0.4
0.6
0.8
1
1.2
1.4
1.6
1.8x 10-5
GAIN: 1 - 0.00001; 2 - 0.01; 3 - 1; 4 - 100; 5 - 10000
RM
S
Error en posición para θ1 y distintas ganancias
RMSθ1
1 2 3 4 50
0.02
0.04
0.06
0.08
0.1
0.12
0.14
0.16
GAIN: 1 - 0.00001; 2 - 0.01; 3 - 1; 4 - 100; 5 - 10000
RM
S
Diferencia de par para θ1 y distintas ganancias
RMSθ1
CONCLUSIONES
30/07/2015 ESCUELA POLITÉCNICA SUPERIOR
17
Pares articulares por dinámica inversa mediante Newton – Euler.
Ecuaciones del movimiento de Lagrange de forma simbólica función de las 14 variables independientes.
Integración de las Ecuaciones del movimiento empleando pares articulares de la dinámica inversa:
Inestabilidad por errores de integración.
Necesidad de un controlador
CTC: controlador más adecuado.
Buen funcionamiento para diferentes ganancias.
A mayores ganancias mejor se adapta al seguimiento de la trayectoria.
A mayor ganancia, mayores diferencias de fuerzas.
TRABAJO FUTURO
30/07/2015 ESCUELA POLITÉCNICA SUPERIOR
18
Planteamiento 3D.
Modelo de articulación.
Implementación de un sistema infraactuado.
Modelo de contacto.
Predicción del movimiento.
Aplicaciones a otras situaciones: carrera,
subir escaleras, etc.