Reconstrucci on cinem atica del brazo humano … · cados en lugares claves del brazo del paciente, pa-ra a partir de la din amica de estos dispositivos, ... resulta de la acumulaci

Actas de las XXXV Jornadas de Automática, 3-5 de septiembre de 2014, ValenciaISBN-13: 978-84-697-0589-6 © 2014 Comité Español de Automática de la IFAC (CEA-IFAC)

Reconstruccion cinematica del brazo humano mediante matricesde transformacion

J.M. Catalan, R. Morales, L.D. Lledo, J.A. Dıez, J.M. Sabater, N. Garcıa-AracilNeuroingeneirıa Biomedica, Universidad Miguel Hernandez de Elche

email: [email protected], [email protected]

Resumen

En este artıculo se presenta un metodo para la re-construccion cinematica del brazo humano orien-tado a tareas de neuro-rehabilitacion asistida porrobots. La finalidad es obtener una aproximaciona los movimientos del brazo de un paciente du-rante la administracion de la terapia, con la in-tencion de poder extraer datos clave que permitancuantificar y evaluar de forma objetiva la calidadde la terapia. Para ello, se emplean tres Unida-des Inerciales (IMU: Inertial Measurement Unit)como sistemas de referencia independientes colo-cados en lugares claves del brazo del paciente, pa-ra a partir de la dinamica de estos dispositivos,reconstruir el modelo cinematico simplificado desiete Grados de Libertad (GDL) del brazo del pa-ciente.

1. Introduccion

Actualmente, la robotica aborda nuevos camposde aplicacion con objetivos muy diversos. Un ejem-plo de ello, es la neuro-rehabilitacion, donde un te-rapeuta con la ayuda de un robot aplica sesionesde rehabilitacion con el objetivo de maximizar lamovilidad perdida de algun miembro del paciente[1, 2]. La robotica aplicada sobre este campo pue-de, ademas, proveer nuevas herramientas para laevaluacion de la evolucion del paciente, aportan-do datos mas objetivos que los empleados hasta lafecha por los propios terapeutas [3].

Una de las problematicas en la administracion deterapias de neuro-rehabilitacion asistida por ro-bots de configuracion de efector final, donde elcontacto entre el robot y el paciente se realizaa traves de un unico punto, es la imposibilidadde determinar la configuracion cinematica del bra-zo del paciente [4]. Esta informacion es necesariatanto por motivos de seguridad como para la eva-luacion de la eficacia de la terapia y el progre-so del paciente. Para solventar este inconvenientese propone emplear Unidades Magentico Inerciales(IMUs) con el fin de determinar la configuracioncinematica del brazo paciente para proporcionarinformacion robusta y objetiva al terapeuta.

El metodo desarrollado en este artıculo consiste enrelacionar directamente la informacion cinematicade los sistemas de referencia de tres unidades iner-ciales colocadas en una posicion fija en el brazodel paciente con el modelo cinematico del brazohumano para obtener su posicion en el espacio.Con esto queremos desarrollar una herramientaque no solo nos permita obtener una representa-cion virtual de los movimientos del paciente, sinoque ademas nos sirva para extraer informacion so-bre las articulaciones del brazo del paciente mien-tras realiza los ejercicios, y ası poder tener unavaloracion objetiva de la eficiencia de las terapias.

1.1. Modelo cinematico del brazohumano

Debido a la dificultad que implica reconstruir elmodelo cinematico que implementa todos los mo-vimientos del brazo humano, se emplea el modelocinematico simplificado de siete Grados de Liber-tad (GDL) que se muestra en la Figura 1.

Tal y como puede observarse en la Figura 1, elmodelo consta de una sucesion en cadena de sieteGDL.

Figura 1: Descripcion grafica del modelo cinemati-co simplificado de 7 GDL empleado.

Tal y como se muestra en la Figura 2,a cada una


de las articulaciones del modelo le corresponde unsistema de referencia independiente.

Figura 2: Sistemas de referencia de las articulacio-nes del modelo cinematico simplificado.

2. Unidades Inerciales

Las unidades inerciales necesarias para este meto-do constan de 6 o 9 grados de libertad (DoF: De-grees of Freedom). Esto significan que pueden serdispositivos IMU, de un acelerometro y un giros-copio de tres ejes, o dispositivos MARG (Magne-tic, Angular Rate, and Gravity) que ademas deestos dos sensores incorporan un magnetometrotambien de tres ejes. De esta manera, utilizandocualquiera de los dos tipos mencionados anterior-mente, la informacion cinematica obtenida de lasIMUs correspondera a un espacio de tres dimensio-nes. Los datos de los sensores estan referenciadoscon respecto al sistema de referencia del campomagnetico terrestre (Ver Fig.3). Esto quiere de-cir que la matriz de transformacion resultante es-tara referenciada al sistema de referencia terrestre.

2.1. Algoritmo de Sebastian O.H.Madgwick

El algoritmo desarrollado por Sebastian O.H.Madgwick [5] es un filtro de orientacion aplicablea IMUs y a dispositivos MARG. Este filtro com-pensa la deriva termica del giroscopio y eliminala distorsion magnetica del magnetometro ademas

Figura 3: Sistema de referencia del campomagnetico terrestre.

de permitir obtener el vector de cuaternios q resul-tante del movimiento del dispositivo en el espaciocon respecto al sistema de referencia del campomagnetico terrestre. En este metodo se implemen-ta este filtro para aplicarlo independientemente acada una de las matrices de transformacion adqui-ridas de las IMUs.

q = [qw + iqx + jqy + kqz]; (1)

Sebastian O.H. Madgwick, tambien realizo unaimplementacion del filtro de Mahony [6], por loque este metodo permitira seleccionar entre los dosalgoritmos que este nos proporciona.

2.2. Matriz de transformacion

A partir del vector de cuaternios q que propor-ciona el filtro de Madgwick, se obtiene su matrizde transformacion correspondiente. Para ello serealizara la conversion del cuaternio a la matrizde transformacion homogenea representada en laEcuacion 2.

2.3. Inicializacion de las IMUs

Dado que la aplicacion de los filtros de Mahony yMadgwick toman como sistema de referencia orto-normal el formado por la lınea del campo magneti-co terreste y el vector de gravedad, es necesariodefinir una serie de matrices de inicializacion quepermitan orientar los sistemas de referencia de lasIMUs en funcion la orientacion en la que se ubicael paciente, con la finalidad de evitar la dependen-cia con el norte terrestre que se provoca al adquirir


wTimu =

1− 2q2y − 2q2z 2qxqy − 2qzqw 2qxqz + 2qyqw 02qxqy + 2qzqw 1− 2q2x − 2q2z 2qyqz − 2qxqw 02qxqz − 2qyqw 2qyqz + 2qxqw 1− 2q2x − 2q2y 0

0 0 0 1

(2)

la informacion de estas referenciada con respectoal sistema de referencia del campo magnetico te-rrestre. Para ello se fijara como sistema de refe-rencia el de la aplicacion. En nuestro caso, comose trabaja con Matlab R©, se elegira como sistemade referencia el valor nulo del sistema de visuali-zacion.

Primero se fijara una posicion de inicializacion a laque se debe recurrir cuando se realice la conexionde las IMUs con Matlab R©. En esta posicion, seobtendra una primera matriz de transformacionde la IMU llamada wSTS . Con ella se obtiene lamatriz de inicializacion 0TwS

a partir del calculode su inversa:

0TwS1= (wS1TS1

)−1 (3)

0TwS1= (wS2TS2)−1 (4)

0TwS1= (wS3TS3

)−1 (5)

2.3.1. Posicion de las IMU en el brazo

El posicionamiento de las IMUs en el brazo delpaciente obliga a definir tres matrices de transfor-macion (una por cada IMU) que hacen referenciaa los dispositivos con respecto la posicion de ini-cializacion de nuestra aplicacion. A estas matricesse las llamara HT0 (para la IMU del hombro) CT0(para la IMU del codo) y MT0 (para la IMU de laMano). En la Figura 4 puede observarse un ejem-plo de la disposicion de las IMUs en el brazo.

Figura 4: Ejemplo de posicionamiento de las IMUs

2.4. Cadena de inicializacion

Llegados a este punto, se define una matriz detransformacion TS para cada una de las IMUs queresulta de la acumulacion de las transformacionesmencionadas en las secciones anteriores y que ser-vira para inicializar cada IMU para la aplicacion:

TS1 = HT00TwS1

wS1TS1 (6)

TS2= CT0

0TwS2

wS2TS2(7)

TS3= MT0

0TwS3

wS3TS3(8)

Siendo H el sistema de referencia del hombro, Cel del codo y M el de la muneca, tenemos unaprimera matriz que nos relaciona la posicion de laIMU en el brazo del paciente con la posicion inicialde nuestra aplicacion. Los sistemas de referencia0TwSi

donde para i = 1, 2, 3 corresponden a lasmatrices que relacionan el sistema de referenciaglobal de la IMU con la posicion inicial definidapara nuestra aplicacion. Finalmente wSiTSi repre-senta la rotacion de la IMU correspondiente en elsistema de referencia terrestre.

3. Implementacion MedianteMatrices de Transformacion

Teniendo la informacion de las IMUs referencia-da con respecto la aplicacion, solo queda aplicarlas transformaciones sufridas por los dispositivosen tiempo real a un modelo virtual que nos per-mita visualizar los movimientos del brazo del pa-ciente. El metodo empleado para reconstruir losmovimientos mediante estas matrices de transfor-macion es el siguiente.

Partiendo de los sistemas de referencia de las arti-culaciones del brazo que se observan en la Figura2, se aplicaran las transformaciones de cada IMUa un segmento distinto del brazo. Dividiremos elbrazo en tres bloques tal y como se observa en laFigura 5.

A cada uno de estos bloques, le asignaremos unsistema de referencia independiente de los que semuestran en la Figura 5 y a los que aplicaremosla matriz de transformacion correspondiente. Es-tos sistemas de referencia no dependen del meto-do planteado en este artıculo, por lo que se esco-gera aquel que convenga para la aplicacion final.


Figura 5: Representacion de los bloques movilesdel brazo

3.1. Cadenas de transformacion

Suponiendo que la informacion obtenida de lasIMUs ya ha sido referencia con nuestra aplica-cion,se implementara una cadena de transforma-ciones que partiendo de nuestro sistema de refe-rencia inicial nos permita llegar al sistema de re-ferencia escogido para cada bloque. Sin embargodebe tenerse en cuenta las siguientes condiciones.La manera de aplicar las tranformaciones de lasIMUs al modelo del brazo, siempre se definira co-menzando por el hombro, siguiendo con el codo yacabando con la mano. Esto es a causa de que lasposiciones en el espacio de la mano y el codo, seven afectadas por el movimiento del hombro, dela misma manera que la posicion de la mano seve afectada por los movimientos del hombro y elcodo. La segunda cosa que debe tenerse en cuentason el uso de las matrices de inicializacion de cadaIMU. Observemos como ejemplo el desarrollo dela cadena de transformaciones para el hombro:

0TH = 0TwwTS1

(TS1

S1TH) (9)

Siendo wTS1la rotacion de la IMU localizada en el

bloque del hombro, se le aplica la transformacionTS1

, correspondiente a la cadena de inicializaciondefinida en secciones anteriores, y S1TH , la matrizque relaciona el sistema de referencia de esta IMUcon el escogido para dicho bloque. La matriz wTS1

relaciona el sistema de referencia de la IMU conel global de nuestra aplicacion. Y por ultimo, lamatriz 0TH relaciona el sistema de referencia glo-bal de nuestro modelo con el de inicilizacion quese definio en secciones anteriores. De esta maneraobtenemos la matriz 0TH .

Partiendo de esta cadena, y utilizando el mismometodo definido anteriormente, calcularemos ladel codo:

0TC = 0THHTS2

(TS2

S2TC)

(10)

El problema reside en la aplicacion de 0TH parael calculo de 0TC . El uso de la cadena del hombroes necesario, ya que como se ha comentado pre-viamente en este mismo capitulo, los movimientosdel hombro influyen en la posicion del codo en elespacio y por tanto en la cadena que queremoscalcular. Para el calculo de la cadena 0TH que seha definido anteriormente, se aplica la matriz detransformacion TS1

, que como se ha explicado ensecciones anteriores, corresponde con la matriz deinicializacion calculada para la matriz del hombro(Ver seccion 2.4). Al aplicar la cadena del hombropara el calculo del codo estamos aplicando inne-cesariamente esta matriz. Esto nos provocara unatransformacion erronea, por lo que se debe elimi-nar su efecto.

0TC = 0THHTS2

(T−1S1

TS2

S2TC)

(11)

De la misma forma que se ha hecho con la cadenadel codo, se procede con la de la mano:

0TM = 0TCCTS3

(T−1S2

TS3

S3TM)

(12)

4. Obtencion de las coordenadasarticulares

Obtenidas las matrices de transformacion que re-lacionan la posicion y orientacion de los puntoshombro, codo y mano; 0TH , 0TC y 0TM respectiva-mente, es posible calcular los valores cinematicosde las articulaciones del brazo por medios analıti-cos como se describe en este apartado.

Dado que la articulacion del hombro esta formadapor las tres primeras articulaciones en configura-cion esferica segun el modelo cinematico presen-tado, se puede determinar que la rotacion del sis-tema de coordenadas del codo respecto al sistemade coordenadas del codo tendra la forma:

HRC (q1,q2,q3) =

nx ny nzox oy ozax ay az

(13)

Donde la matriz hRc se obtiene extrayendo la ma-triz de rotacion de la matriz de transformaciondefinida por:

HTC = 0T−1H

0TC (14)


Por tanto, los valores articulares del hombro (q1,q2 y q3) se obtienen analıticamente para el rango[0 π] mediante las siguientes ecuaciones:

q1 = atan2 (−ny, oy)

q2 = atan2(ay,√n2y + o2y

)q3 = atan2 (az,−ax)

(15)

y para q2 ∈ [−π 0]:

q1 = atan2 (ny,−oy)

q2 = atan2(ay,−

√n2y + o2y

)q3 = atan2 (−az, ax)

(16)

Como puede observarse se obtienen dos posiblessoluciones debido a que la articulacion del hombrose trata de una articulacion esferica.

Por otra parte, la variable articular q4 influye so-bre la distancia del segmento HM y, por tanto,puede ser obtenida directamente. Considerando elvector normal, del plano que contiene el puntohombro, codo y muneca paralelo al eje de rota-cion del codo, la variable articular q4 puede sercalculada mediante la ley de cosenos:

q4 = arcsin

(l2u + l2f − ||M −H||

2

2lulf

)(17)

Donde H y M corresponde al vector de posiciondel hombro y de la mano con respecto al sistemade coordenadas del hombro. Esta ecuacion al tra-tarse de un arcoseno, posee dos posibles solucionesq4 y π − q4. Sin embargo, la solucion definida enel rango [π/2 π], no es valida por quedar fuerade los lımites articulares anatomicos.El rango deesta articulacion esta definido sobre el intervalo[−π/2 π/2]. Por tanto, la unica solucion validase encuentra sobre este intervalo.

Una vez establecido la variable articular q4, se pue-de determinar la matriz transformacion CT4 quesegun los parametros DH del Cuadro 1 valdra:

CT4 =

− sin (q4) 0 cos (q4) 0cos (q4) 0 sin (q4) 0

0 1 0 00 0 0 1

(18)

Y por tanto, se puede determinar el sistema decoordenadas 4 sobre el sistema de coordenadas delhombro mediante:

HT4 = HTCCT4 (19)

Cuadro 1: Parametros DH del modelo cinematicosimplicado del brazo humano

θi di ai αi

π/2 + q1 0 0 π/23π/2 + q2 0 0 π/2

q3 lu 0 −π/2π/2 + q4 0 0 π/2π/2 + q5 lf 0 π/2π/2 + q6 0 0 π/2π/2 + q7 0 0 π/2

Con la ayuda de esta matriz de puede obtener lamatriz de transformacion entre el sistema de lamano y el sistema de coordenadas 4:

4TM = HT−14

HTM (20)

La matriz 4TH depende de los valores articularesq5, q6 y q7, que forman una articulacion esfericay segun los parametros DH del Cuadro 1 y consi-derando el sistema de coordenadas s4 desplazadosobre el punto muneca:

iTi+1 =

− sin (qi+1) 0 cos (qi+1) 0cos (qi+1) 0 sin (qi+1) 0

0 1 0 00 0 0 1

(21)

sustituyendo para i = {4, 5, 6}, y aplicando el pro-ducto matricial 4TM = 5T5

5T66TM , se obtiene la

Ecuacion 22.

Considerando la misma forma de la matriz de ro-tacion de la Ecuacion 13 se pueden determinar lasvariables articulares correspondientes a las articu-laciones q5, q6, q7 mediante las siguientes ecuacio-nes:

q5 = −atan2 (ny, oy)

q6 = arcsin (ay)

q7 = −atan2 (ax, az)

(23)

Dado el valor articular q6 se obtiene mediante unarcoseno, su solucion no es unica, por tanto existeotra solucion definida por las siguentes ecuaciones:

q5 = π − atan2 (ny, oy)

q6 = π − arcsin (ay)

q7 = π − atan2 (ax, az)

(24)

Llegados a este punto se obtiene cuatro solucio-nes posibles; dos debidas a las soluciones obteni-das para el calculo de la articulacion esferica del


4Th =

c5 c7 − s5 s6 s7 −c6 s5 c5 s7 + c7 s5 s6 0c7 s5 + c5 s6 s7 c5 c6 s5 s7 − c5 c7 s6 0−c6 s7 s6 c6 c7 0

0 0 0 1

(22)

hombro; y otras dos soluciones debidas a la ar-ticulacion esferica definida por las articulacionesde la muneca. Por tanto, se obtienen cuatros solu-ciones que satisfacen las restricciones cinematicas.Sin embargo, debido a las propiedades anatomi-cas del brazo humano, los lımites articulares estanperfectamente definidos, por tanto, se puede esta-blecer que solo una de las cuatro soluciones obte-nidas es valida porque el resto, al menos una delas articulaciones esta fuera de los lımites articu-lares anatomicos. Esta afirmacion se demuestra yaque para cada articulacion esferica, sus dos solu-ciones estan definidas en el rango [0 π] o sobre elrango [0 − π]. Por tanto, no existe ninguna arti-culacion cuyos lımites articulares esten fuera delrango [−π/2 π/2].

Segun el modelo cinematico simplificado del brazohumano extraıdo del [7] se define la funcion nece-sario para determinar si un vector de coordenadasarticulas esta dentro del los lımites anatomicos delbrazo humano. Este procedimiento debe ser apli-cado para cada iteracion para obtener las variablesarticulares en cada instante de tiempo.

5. Experimentacion

Se propone realizar una actividad a modo de expe-rimentacion con el objetivo de cuantificar errores ala hora de realizar la reconstruccion del modelo. Laactividad propuesta consistira en un ejercicio desimulacion, donde se le pedira al usuario que imi-te una serie de movimientos especificados por unmodelo virtual. Se registraran los valores articula-res del brazo del usuario durante todo el ejercicio,para posteriormente compararlos con los valoresarticulares que se marcan como objetivo en la ac-tividad. Con esto se desea cuantificar cuan precisaes la modelizacion de un brazo humano realizadacon dispositivos IMU.

Como puede observarse en la Figura 6, la interfazgrafica consta de una visualizacion a tiempo realde la reconstruccion del brazo del usuario utilizan-do el metodo planteado en este articulo, y de unaindicacion de cual es la posicion que debe adoptarel usuario en cada caso (en modo transparente enla Figura 6).

Para esta actividad, el usuario debera imitar pororden las posiciones definidas en la Figura 7.

Figura 6: Representacion grafica del interfaz de laactividad durante el ejercicio, donde puede obser-varse la posicion objetivo en modo transparente yla posicion en tiempo real del brazo del usuario

Figura 7: Representacion grafica de las posicionesa imitar por el usuario


5.1. Resultados

En la Figura 8, se muestran varios resultados de lareconstruccion del brazo por el metodo presenta-do en esta artıculo. Se puede observar que aunqueexiste un pequeno error entre la posicion calculaday la posicion objetivo. Este error es asumible parala realizacion y valoracion de terapias de rehabili-tacion.

Sin embargo, este metodo tiene una serie de venta-jas. El sistema de reconstruccion es independientede la actividad a desempenar. Eso quiere decir quepuede servir tanto para monitorizar una tarea co-mo para realizar ejercicios de rehabilitacion por sisolo. Para ninguna de estas aplicaciones se requie-re una gran precision, ya que el objetivo consistirıaen registrar que el ejercicio se realiza correctamen-te.

Figura 8: Resultados de la reconstruccion del bra-zo para la posicion 1 y 3.

6. Conclusiones

En este artıculo se presenta un metodo para la re-construccion cinematica del brazo humano orien-tado a tareas de neuro-rehabilitacion asistida porrobots. Para ello, se emplean tres Unidades Iner-ciales (IMU: Inertial Measurement Unit) como sis-temas de referencia independientes colocados enlugares claves del brazo del paciente, para a partirde la dinamica de estos dispositivos, reconstruir elmodelo cinematico simplificado de siete Grados deLibertad (GDL) del brazo del paciente. Los resul-tados obtenidos son prometedores aunque es ne-cesario realizar mas pruebas para valorar la exac-titud del sistema y repetitibilidad.

Agradecimientos

Este trabajo has sido financiado por el Plan Na-

cional de I+D+i a traves del proyecto “Interpreta-cion de la intencion y actuacion humana median-te senales biomedicas y el analisis cinematico ydinamico del movimiento” (DPI2011-29660-C04-04).

Referencias

[1] Ju MS., Lin CCK., Chen SM., Hwang IS.,Kung PC., Wu ZW., (2007), Applications ofRobotics to Assessment and Physical The-rapy of Upper Limbs of Stroke Patients, I-Tech Education and Publishing, pp 243-260.

[2] Badesa, F. J., Llinares, A., Morales, R.,Garcia-Aracil, N., Sabater, J. M., Perez-Vidal, C. (2014). Pneumatic planar rehabi-litation robot for post-stroke patients. Bio-medical Engineering: Applications, Basis andCommunications, 26(02).

[3] Murphy A., Willen C., and SunnerhagenKS., (2011), Kinematic variables quantifyingupper-extremity performance after stroke du-ring reaching and drinking from a glass, Neu-rorehabil Neural Repair, Vol. 25, No.1, pp 71-80.

[4] Burgar G., Lum P., Shor P., Van der Loos M.,(2000), Development of robots for rehabilita-tion therapy: The Palo Alto VA/Stanford ex-perience, Journal of Rehabilitation Researchand Development, Vol.37, No.6, pp 663-673.

[5] Madgwick, S.O.H., Vaidyanathan, R., Ha-rrison, A.J.L., (2010), An Efficient Orienta-tion Filter for Inertial Measurement Units(IMUs) and Magnetic Angular Rate and Gra-vity (MARG) Sensor Arrays.

[6] Mahony RE., Tarek H., Pflimlin JM., (2008),Nonlinear Complementary Filters on the Spe-cial Orthogonal Group, IEEE Trans. Auto-mat. Contr., Vol.53, No.5, pp 1203-1217.

[7] Lenarcic, J, Umek, A. ,(1994), Simple modelof human arm reachable workspace, Systems,Man and Cybernetics, IEEE Transactions on,Vol. 24, No.8, pp 1239-1246.

Documents

Reconstrucci on cinem atica del brazo humano … · cados en lugares claves del brazo del paciente, pa-ra a partir de la din amica de estos dispositivos, ... resulta de la acumulaci