Documente Academic
Documente Profesional
Documente Cultură
NACIONAL
T E S I S
QUE PARA OBTENER EL TTULO DE MAESTRO EN
CIENCIAS EN INGENIERA ELECTRNICA
PRESENTA:
ING. EDWARDS ERNESTO SNCHEZ RAMREZ
ASESOR:
DISEO DE UN
CONTROLADOR GPI
DESCENTRALIZADO
PARA TAREAS DE
SEGUIMIENTO DE
TRAYECTORIAS EN LAS
EXTREMIDADES DE UN
ROBOT CAMINANTE
Agradecimientos
Agradezco a mi esposa Mayte y mi hija Loreto, por su ininito apoyo en estos aos,
y por comprender esos momentos de ausencia.
Agradezco infinitamente a mis padres Jos Ernesto y Flor de Azalia por la mejor
herencia que me pudieron dar: Eduacin y Estudio.
Agradezco a mis hermanos Gerardo y Cristian, por sus consejos y apoyos brinda-
dos.
Agradezco a mi asesor de Tesis el Dr. Alberto Jorge Rosales, por su destacable
dedicacin y empeo en la direccin de este trabajo de Tesis.
Agradezco a los Doctores Alberto Luviano y Alfredo Ramrez, por sus importantes
consejos, comentarios y sugerencias que fueron de gran ayuda en el desarrollo de este
trabajo.
Agradezco al Dr. Francisco Javier Gallegos, por supervisar el desarrollo de este
trabajo y por los consejos, sugerencias y apoyos brindados.
Agradezco al Instituto Politcnico Nacional, por la formacin acadmica y profe-
sional de calidad que me ha otorgado.
Agradezco al Instituto de Investigacin y Desarrollo Tecnolgico de la Armada de
Mxico por el apoyo brindado en la realizacin del presente trabajo de Tesis.
Agradezco al Consejo Nacional de Ciencia y Tecnologa por la beca de manutencin
otorgada en apoyo a la realizacin del presente trabajo.
Agradezco infinitamente a mis compaeros Belmar Garca, Mario Dehesa, Isabel
Hernndez, Josafath Cruz, Mariana Vzquez, Fernando Gamino, Diana Contreras,
Samuel Souverville, Jean Marie Vianney, Javier Zenteno, Arturo Gonzalez y Sergio
Gonzalez, por el apoyo brindado en todo momento durante mi estancia en el Labora-
torio de Seales y Sistemas.
Resumen
Agradecimientos . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . ix
Resumen . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . xi
Abstract . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . xiii
ndice de figuras . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . xix
ndice de tablas . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . xxi
Lista de Acrnimos . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . xxiii
Objetivo general . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . xxv
Objetivos particulares . . . . . . . . . . . . . . . . . . . . . . . . . . . . . xxv
Justificacin . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . xxvii
xv
xvi ndice general
Bibliografa 91
A. Publicaciones en Congresos 97
ndice general xvii
xix
xx ndice de figuras
xxi
Lista de acrnimos
Siglas Descripcin
3-D Tres dimensiones
DH Denavit-Hartenberg
GDL Grado de libertad
GPI Generalized Proportional Integral (Proporcional Integral Generalizado)
LabVIEW Laboratory Virtual Instrumentation Engineering Workbench
NI National Instruments
VI Virtual Instrument (Instrumento Virtual)
DSP Digital Signal Processor (Procesador Digital de Seales)
FPGA Field Programmable Gate Array (Arreglo Programable de Compuertas
en Campo)
Objetivo general
Disear e implementar un controlador Proporcional-Integral Generalizado (GPI) con pro-
piedades robustas para garantizar que la extremidad de un robot caminante tengan un
ptimo desempeo en el seguimiento de trayectorias deseadas.
Objetivos particulares
1. Obtener los modelos cinemtico y dinmico para una de las extremidades del robot
hexpodo.
1.1. Introduccin
Una rama de la robtica que hemos visto desarrollarse en estos ltimos aos es la
robtica mvil. Las principales aplicaciones de este tipo de robots se han registrado en
las reas de asistencia mdica, exploracin militar, exploracin de reas siniestradas
o contaminadas, exploracin espacial y minera.
Cabe mencionar que se han derivado diversas lneas de investigacin para el de-
sarrollo de los robots mviles entre las que destacan:
Los trabajos realizados a la fecha han permitido realizar una clasificacin de es-
te tipo de robots con respecto a los aditamentos y medios que utilizan para poder
moverse de un punto a otro en el espacio abierto. As pues, dependiendo del tipo de
locomocin que tengan, se pueden clasificar en Sistemas de locomocin por ruedas,
Sistemas de Locomocin por Extremidades y Sistemas de Locomocin por Propulsin
[9] (Fig. 1.1).
El desplazamiento de los robots mviles en ausencia de superfices de contacto
requiere de sistemas de locomocin por propulsin que permitan dar el empuje y el
1
2 Captulo 1: Estado del arte
rumbo deseado. Los entornos en los que han trabajado los robots mviles son el aire,
el agua y el espacio exterior. El desplazamiento en el aire se ha realizado mediante
helicpteros, aviones y cuadrpteros. Para el agua se han desarrollado vehculos ro-
bticos submarinos, y para el espacio exterior se han diseado sistemas de propulsin
basados en combustin. Un ejemplo del desarrollo de un cuadricptero se muestra en
[6].
Los robots mviles que emplean un sistema de locomocin por ruedas utilizan
un nmero determinado de ruedas acopladas a su base que a su vez se mantienen
en contacto con una superficie en la que se trasladan de un punto a otro. Existen
numerosas variantes para este tipo de robots, principalmente modificando el nmero
de ruedas y los grados de libertad de cada una de ellas, consiguiendo con esto diversas
configuraciones que han sido ampliamente estudiadas y que brindan caractersticas
tiles tales como alta eficiencia energtica, reduccin de dimensiones del robot y
capacidad de omnidireccionalidad.
A pesar de que los sistemas de locomocin por ruedas ofrecen una alta eficiencia
y maniobrabilidad a la hora de desplazarse de un punto a otro sobre una superficie,
Captulo 1: Estado del arte 3
En general, es de gran utilidad modelar cada una de las extremidades como brazos
manipuladores individuales, creando un sistema de control descentralizado, en el que
cada brazo ser considerado como una planta con el fin de reducir el proceso de
modelado dinmico. De esta forma es posible utilizar teora de robots manipuladores
ampliamente utilizada con el fin de realizar el proceso de modelado y control de
cada brazo. En la siguiente seccin describiremos en forma general lo concerniente al
modelado cinemtico y dinmico de un brazo robtico.
(GPI, por sus siglas en ingls) integra tanto observador como controlador en un mismo
algoritmo.
El control GPI es una tcnica de control basada en la idea de evitar el diseo de
observadores utilizando reconstructores integrales como estimadores de las variables
de estado no medidas. Una de las caractersticas atractivas del control GPI radica
en el hecho de que se basa slo en mediciones de entradas, salidas y combinaciones
lineales de un nmero finito de integraciones de las mismas despreciando cualquier
condicin inicial desconocida. Como resultado, el reconstructor integral difiere del va-
lor real de la seal estimada, este error estructural es fundamentalmente inestable y
puede ser caracterizado mediante un polinomio de orden finito. De esta manera, em-
pleando el principio de superposicin, el controlador se complementa con un nmero
finito de integradores que cancelan el efecto desestabilizante del estimador estructu-
ral. Como resultado, se obtiene un controlador PI de n-simo orden, el cual estabiliza
el sistema en lazo cerrado mediante la asignacin arbitraria de los polos del polinomio
caracterstico del sistema.
El Control GPI apareci inicialmente en [29], y fue utilizado en diversos trabajos
posteriores. En la actualidad sigue predominando como una potente tcnica de control
de sistemas lineales debido a su principal caracterstica intrnseca que se manifiesta
como rechazo activo de perturbaciones. Algunos trabajos recientes en los que que
utilizan esta tcnica de control se muestran en [19] y [20]. El desarrollo del controlador
GPI, as como sus principales caractersticas de diseo, se presentan en el Captulo 4
a ms detalle.
El primer trabajo de un controlador GPI no lineal aplicado a robots manipuladores
se present en [42]. En el Captulo 4 del presente trabajo de tesis se aplica este
controlador a la extremidad de un robot caminante tipo hexpodo, y adems se
propone una modificacin a la ley de control que permite rechazar de una mejor
forma las perturbaciones constantes y no lineales presentes en el sistema de lazo
cerrado.
1.4. Conclusiones
En este captulo se abord lo concerniente al modelado cinemtico y dinmico
de un brazo robtico, adems de diferentes tcnicas de control que se han propuesto
en estas ltimas dcadas. Esto debido a que las extremidades del robot caminante
hexpodo a tratar en este trabajo de tesis, pueden modelarse como brazos robticos
independientes y de esta forma, utilizar teora de robots manipuladores con el fin de
realizar su estudio, modelado y control.
El proceso de modelado cinemtico y dinmico de un brazo robtico se vuelve
ms complejo al aumentar el nmero de grados de libertad. Como se ver en el
Captulo 1: Estado del arte 11
Modelado cinemtico de la
extremidad del robot Hexapod
Mark II
13
14 Captulo 2: Modelado cinemtico de la extremidad del robot Hexapod Mark II
los vectores unitarios x0 , y0 y z0 (ver Fig. 2.3). A partir de este marco de referencia
se van a ubicar los movimientos de la extremidad. Se necesitan otros tres marcos
de referencia para ubicar a cada eslabn, para ello haremos uso de una convencin
ampliamente usada en el modelado de robots manipuladores.
cos i sen i 0 0 1 0 0 0 1 0 0 ai 1 0 0 0
sen i cos i 0 0
0 1 0 0 0 1 0 0 0 cos i sen i 0
=
0
0 1
0
0 0 1 0
di
0 1 0 0 sen i
cos i 0
0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 1
cos i sen i cos i sen i sen i ai cos i
sen i cos i cos i cos i sen i ai sen i
=
. (2.1)
0 sen i cos i di
0 0 0 1
En este caso, los parmetros a utilizar para una extremidad del robot Hexapod
Mark II se muestran en la Tabla 2.1, de acuerdo al nmero de eslabones y de las ubi-
caciones de los grados de libertad como se muestra en la Fig. 2.4. Asimismo L1 , L2 yL3
representan las longitudes de cada eslabn.
Tabla 2.1: Parmetros Denavit-Hartenberg (DH) para una extremidad del robot He-
xapod Mark II.
Eslabn i di ai i
1 q1 0 L1 90o
2 q2 0 L2 0
3 q3 0 L3 0
Hii1 para cada uno de los eslabones. Los resultados obtenidos se muestran en las
ecuaciones 2.2, 2.3 y 2.4.
cos q1 0 sen q1 L1 cos q1 sen q1
sen q1 0 cos q1 L1 cos q1 sen q1
H10 =
0
, (2.2)
1 0 0
0 0 0 1
cos q2 sen q2 0 L2 cos q2
sen q2 cos q2 0 L2 sen q2
H21 =
0
, (2.3)
0 1 0
0 0 0 1
cos q3 sen q3 0 L3 cos q3
sen q3 cos q3 0 L3 sen q3
H32 =
0
. (2.4)
0 1 0
0 0 0 1
La matriz H30 que representa la posicin y orientacin del efector final se obtiene al
multiplicar sucesivamente las matrices H10 , H21 y H32 , como se muestra en la ecuacin
2.5.
H30 = H10 H21 H32
cos(q2 + q3 ) cos q1 sen(q2 + q3 ) cos q1 sen q1 A cos q1
cos(q2 + q3 ) sen q1 sen(q2 + q3 ) sen q1 cos q1 A sen q1 (2.5)
=
sen(q + q )
,
2 3 cos(q2 + q3 ) 0 B
0 0 0 1
Con este resultado, se obtiene la matriz de rotacin R30 y el vector r30 que represen-
tan la orientacin y posicin del efector final respectivamente, con respecto al marco
de referencia inercial, denotados por las ecuaciones 2.6 y 2.7.
sen(q2 + q3 ) cos(q2 + q3 ) 0
A cos q1
r30 = A sen q1 . (2.7)
B
En el apndice B.1 se muestra el cdigo fuente utilizado para representar grfica-
mente los eslabones y los marcos de referencia de la figura 2.4.
20 Captulo 2: Modelado cinemtico de la extremidad del robot Hexapod Mark II
B 2 + z 2 L23 L22
cos(180o q3 ) = cos q3 = = C. (2.10)
2L2 L3
Mediante la identidad Pitagrica dada por la ecuacin 2.11, despejamos el valor
de sen q3 obteniendo una expresin que incluye una raz cuadrada, como se muestra
en la ecuacin 2.12. Finalmente se obtiene el valor de tan q3 y se despeja el ngulo,
quedando como en la ecuacin 2.14.
sen q3 = 1 C 2, (2.12)
donde para simplificar:
B 2 + z 2 L23 L22
C= ,
2L2 L3
22 Captulo 2: Modelado cinemtico de la extremidad del robot Hexapod Mark II
sen q3
tan q3 = , (2.13)
cos q3
1 C2
q3 = arctan . (2.14)
C
Para obtener el valor del ngulo q2 se procede de la siguiente forma: primero se
obtienen los catetos del tringulo rectngulo O2 VC O3 , que a su vez forman parte de
los catetos del tringulo O1 VC O3 . Con los catetos de este ltimo y aplicando una
tangente inversa, se puede obtener una parte del ngulo q2 (segundo trmino de la
parte derecha de la ecuacin 2.15). La otra parte se obtiene con ayuda del tringulo
rectngulo cuyos catetos son B y z (primer trmino de la parte derecha de la ecuacin
2.15). Siguiendo este procedimiento, obtenemos el valor del ngulo q2 como se muestra
en la ecuacin 2.15.
z L3 sen q3
q2 = arctan arctan (2.15)
B L2 + L3 cos q3
En forma matricial, la solucin de la cinemtica inversa dada por las ecuaciones
Captulo 2: Modelado cinemtico de la extremidad del robot Hexapod Mark II 23
arctan xy
q1
z L3 sen q3
arctan B arctanL2 +L3 cos q3 .
q2 = (2.16)
2
q3 q3 = arctan 1C
C
vn0 = Jv q, (2.17)
n0 = J q. (2.18)
0
Sea zi1 el vector unitario que coincide con el eje de rotacin del i-simo eslabn,
expresado con respecto al marco de referencia inercial. La matriz Jacobiana angular
est determinanda por la ecuacin 2.19, donde vale 1 si la articulacin es de tipo
rotacional, y vale 0 si es de tipo prismtica [24].
Sea o0n el vector posicin del origen del marco de referencia del efector final re-
presentado por el punto P (x, y, z) en la Fig. 2.7 ). La i-sima columna del la matriz
Jacobiana lineal Jvi est dada por la ecuacin 2.20. La deduccin de las matrices
Jacobianas puede consultarse en el apndice C[24].
o0n
Jvi = . (2.20)
qi
En la cadena cinemtica abierta que constituye una extremidad del robot Hexapod
Mark II, aplicando las ecuaciones 2.19 y 2.20 se obtienen las matrices Jacobianas que
se muestran en las ecuaciones 2.21 y 2.22.
0 sen q1 sen q1
J = 0 cos q1 cos q1 , (2.21)
1 0 0
0 F + L2 cos q2 F
Figura 2.9: Singularidad para el caso en el que el efector final toca el eje z.
2.4. Conclusiones
En este captulo se analiz a detalle la cinemtica de la extremidad del robot
Hexapod Mark II en cuanto a cinemtica directa, inversa y diferencial.
En la cinemtica directa nos encontramos con una cadena abierta compuesta de
tres eslabones y tres grados de libertad. Para la ubicacin de los extremos de cada
eslabn se requirieron asignar cuatro marcos de referencia, utilizando la convencin
Captulo 2: Modelado cinemtico de la extremidad del robot Hexapod Mark II 27
Modelado dinmico de la
extremidad del robot Hexapod
Mark II
3.1. Introduccin
Una vez realizado el anlisis cinemtico, se utilizarn los parmetros DH y las
matrices Jacobianas para la obtencin de un modelo dinmico que permita establecer
una relacin entre las fuerzas involucradas en la extremidad del robot Hexapod Mark
II y su movimiento en un espacio tridimiensional. Dicha extremidad puede ser tratada
como un robot manipulador.
Un manipulador es un sistema mecnico muy complejo cuya descripcin analtica
requiere de ecuaciones diferenciales. La naturaleza no lineal, multivariable y acopla-
da de su comportamiento dinmico ofrece un amplio espectro en la formulacin de
problemas de control tericos y prcticos. El modelo dinmico del robot manipulador
permite explicar todos los fenmenos fsicos que se encuentran en su estructura mec-
nica, tales como efectos inerciales, fuerzas centrpetas y de Coriolis, pares gravitacio-
nales y friccin, los cuales son fenmenos fsicos intrnsecos o propios de la naturaleza
dinmica del robot [8]. Aunque se han reportado diversos mtodos para la obtencin
de este modelo dinmico, los ms conocidos son el mtodo de Euler-Lagrange y el de
Newton-Euler. El primero de ellos presenta las siguientes ventajas y caractersticas:
29
30 Captulo 3: Modelado dinmico de la extremidad del robot Hexapod Mark II
3. La matriz de rotacin Ri0 del i-simo marco de referencia con respecto al marco
de referencia inercial, a partir de la ecuacin 2.1.
4. El Tensor de Inercia Ii del i-simo eslabn, que est representado por una matriz
diagonal compuesta por los momentos principales de inercia Ixi , Iyi e Izi , con
respecto al centro de masa de dicho eslabn.
5. El vector de posicin del extremo final del i-simo eslabn ubicado en el origen
del i-simo marco de referencia, mediante la ecuacin 2.1. Recordando que Li
representa la longitud del i-simo eslabn, y Lci es la distancia desde el origen
del marco de referencia i 1 al centro de masa del i-simo eslabn.
n
mi Jvi (q)T Jvi + Ji (q)T Ri (q)Ii Ri (q)T Ji (q).
X
D(q) = (3.3)
i=1
32 Captulo 3: Modelado dinmico de la extremidad del robot Hexapod Mark II
0 1 1 0 0 Iz1
cos q1 cos q2 cos q1 sen q2 sen q1
Ix2 0 0
R20 (q) = sen q1 cos q2 sen q1 sen q2 cos q1 I2 = 0 Iy2 0
0 1
(L1 + Lc2 cos q2 ) sen q1 Lc2 cos q1 sen q2
0 sen q1
Jv2 (q) = (L1 + Lc2 cos q2 ) cos q1 Lc2 sen q1 sen q2 J2 = 0 cos q1
0 Lc2 cos q2 1 0
B sen q1 A cos q1 Lc3 sen(q2 + q3 ) cos q1
La matriz C(q, q)se obtiene elemento a elemento, de tal forma que el (k, j)-simo
elemento de dicha matriz est dado por la ecuacin 3.4, donde dij representa el (i, j)-
simo elemento de D(q).
n
( )
X dkj dki dij
ckj = + qi . (3.4)
i=1 qj qj qk
P
gk = . (3.6)
qk
Las matrices obtenidas utilizando los trminos de la Tabla 3.1 se pueden consultar
en el apndice D. En el apndice B.2 se muestra el cdigo fuente utilizado para el
desarrollo algebraico que involucra la obtencin de estas matrices mediante el uso de
variable simblica.
i
3. La matriz de rotacin Ri+1 .
0
4. El vector zi1 , representa el eje de rotacin del i-simo eslabn con respecto al
marco de referencia inercial.
7. El vector ri1,ci , representa el vector que va desde Oi1 hasta el centro de masa
del i-simo eslabn.
8. El vector ri,ci , representa el vector que va desde Oi hasta el centro de masa del
i-simo eslabn.
1 0 0
0 g sen q2 g(cos q2 sen q3 + cos q3 sen q2 )
gi g1 = g g2 = g cos q2 g3 = g(cos q2 cos q3 sen q2 sen q3 )
0 0 0
L1 L2 L3
ri1,i r0,1 = 0 r1,2 = 0 r2,3 = 0
0 0 0
Lc1 Lc 2 Lc3
ri1,ci r0,c1 = 0 r1,c2 = 0 r2,c3 = 0
0 0 0
Lc1 L1 Lc2 L2 Lc3 L3
ri,ci r1,c1 = 0 r2,c2 = 0 r3,c3 = 0
Captulo 3: Modelado dinmico de la extremidad del robot Hexapod Mark II
0 0 0
35
36 Captulo 3: Modelado dinmico de la extremidad del robot Hexapod Mark II
En la Fig. 3.1 se pueden apreciar los marcos de referencia, el eslabn, las fuerzas
y pares aplicados. Los vectores y matrices mencionados han sido obtenidos para cada
eslabn de la extremidad del robot y se muestran en la Tabla 3.2.
Una vez obtenidas las expresiones anteriores, se debe comenzar con la obtencin
de velocidades y aceleraciones de forma iterativa. Comenzando con el primer eslabn
se obtienen las ecuaciones 3.7, 3.8, 3.9, 3.10 y 3.11 [24]. Despus se procede con el
segundo eslabn de la misma forma y as hasta llegar al ltimo. Observemos que cada
una de estas ecuaciones depende de la ecuacin anterior, a excepcin de la ecuacin
i
3.7. Los trminos zi1 , i , i , aei y aci asociados al i-simo eslabn representan el
eje de rotacin, velocidad angular, aceleracin angular, aceleracin lineal del extremo
final y aceleracin lineal del centro de masa respectivamente, todos referidos al i-simo
marco de referencia.
i
zi1 = (Ri0 )T zi1
0
, (3.7)
i = (Rii1 )T i1 + zi1
i
qi , (3.8)
i = (Rii1 )T i1 + i (zi1
i
qi ), (3.9)
i
fi = (Ri+1 )T fi+1 + mi ac,i mi gi , (3.12)
i
i = (Ri+1 )T i+1 fi ri1,ci + (Ri+1
i
fi+1 ) ri,ci + Ii i + i (Ii i ). (3.13)
eslabn que ocasionar que gire sobre el eje correspondiente al grado de libertad. Hay
que puntualizar que este mtodo no considera las restricciones holonmicas2 de forma
automtica, a diferencia del mtodo anterior. Para ubicar cual es la componente de
inters de un eslabn en particular, solo hay que ubicar cual es la componente de i
que coincide con el eje de rotacin de dicho eslabn. Para el caso de la extremidad
del robot Hexapod Mark II se obtuvieron los vectores 1 , 2 y 3 . De la figura 2.4
observamos que para el primer eslabn, la componente de 1 que coincide con el eje
de rotacin es 1y . Para el segundo eslabn, la componente de 2 que coincide con el
eje de rotacin es 2z . Para el tercer eslabn, la componente de 3 que coincide con
el eje de rotacin es 3z . De esta forma concluimos que las componentes 1y , 2z y 3z
son las que ocasionan que los eslabones roten alrededor de los ejes representados por
los vectores unitarios z0 , z1 y z2 . Para validar el modelo se construy un vector dado
por A = [1y , 2z , 3z ]T , se obtuvo el vector B mediante las matrices D(q), C(q, q)y
G(q)obtenidas por el mtodo de Euler-Lagrange, y finalmente se realiz la resta entre
ambos vectores A B = 0 con lo cual todos los trminos quedaron cancelados.
El cdigo fuente de este procedimiento se puede consultar en el apndice B.4. Esto
resulta indispensable ya que en todo sistema de control la respuesta depende del
correcto modelado de la planta.
m (s) Km
= , (3.14)
V (s) s[(Ls + R)(Jm s + Bm ) + Kb Km ]
m (s) Km /R
= , (3.16)
V (s) s(Jm s + Bm + Kb Km /R)
m (s) 1/r
= . (3.17)
(s) s(Jm s + Bm + Kb Km /R)
Para combinar las ecuaciones 3.16 y 3.17, se transforman al dominio del tiempo
y se suman, con lo cual obtenemos la ecuacin 3.18. Utilizando la relacin mi =
ri qi , despejando de 3.18 y sustituyendo en 3.2 obtenemos la ecuacin 3.19, donde
M (q) = D(q)+J siendo J una matriz diagonal cuya diagonal principal est compuesta
por elementos dados por ri2 Jm i. B representa una matriz diagonal de coeficientes de
friccin cuyos elementos estn dados por Bmi + Kbi Kmi /Ri . El vector de entrada i
est dado por la ecuacin 3.20. Recordemos que el subndice i en cada uno de estos
trminos est asociado al actuador que provoca la rotacin del i-simo eslabn.
Kmi
i = ri Vi . (3.20)
Ri
La ecuacin 3.19 representa el modelo dinmico combinado, es decir el modelo
dinmico del manipulador con el modelo del actuador. Para efectos de simulaciones e
implementaciones se simplificar esta ecuacin despreciando la matriz de coeficientes
de friccin (B = 0) como es costumbre en muchas aplicaciones de robtica [24].
Captulo 3: Modelado dinmico de la extremidad del robot Hexapod Mark II 39
Tabla 3.3: Parmetros dinmicos de la extremidad del robot Hexapod Mark II.
mr2
J= . (3.24)
2
Podemos notar que para todos los servomotores los valores de J y r pueden ser los
mismos, mientras que los valores R y Km pueden variar ligeramente. Por consiguiente
en la Tabla 3.6 se muestran estos valores estimados para los servomotores 7, 9 y 11,
que corresponden a la extremidad con la que se va a trabajar. Para esto se utiliz el
mismo mtodo de caracterizacin utilizado en el servomotor 11.
Captulo 3: Modelado dinmico de la extremidad del robot Hexapod Mark II 43
3.6. Conclusiones
En este captulo se realiz el modelado dinmico de la extremidad del robot cami-
nante Hexapod Mark II mediante dos mtodos conocidos: el mtodo de Newton Euler
y el de Euler-Lagrange. El primero de ellos realiza un anlisis de fuerzas y pares apli-
cados a cada eslabn. El segundo hace uso del concepto de lagrangiano para obtener
el modelo del sistema.
Debido a que ambos mtodos entregaron los mismos resultados, se pudo verificar
que la aproximacin del modelo dinmico obtenida fue adecuada. Esto resulta indisc-
pensable, ya que en todo sistema de control, el correcto modelado de la planta influye
directamente en la respuesta del sistema ante una entrada de control.
Observando el modelo dinmico obtenido en el apndice D podemos notar que po-
see una gran cantidad de trminos algebraicos, por lo que en el proceso de implemen-
tacin nos limita a utilizar microcontroladores de alta capacidad de procesamiento,
con el fin de estimar adecuadamente el modelo dinmico de la planta en tiempo real.
Asimismo se consider tambin el modelado dinmico de los actuadores, siendo de
suma importancia debido a que son los generadores de torque, teniendo como entradas
seales de voltaje o de corriente. Combinando el modelo dinmico de los actuadores
con el modelo de la extremidad del robot se obtuvo el modelo dinmico combinado
que en lugar de tener como entrada seales de torque, tiene como entrada seales de
voltaje. Esto resulta indispensable ya que en la prtica no resulta conveniente tener
un sistema que tenga como entradas de control seales de torque. Tambin se realiz
la caracterizacin de los motores de C. D. mediante mtodos conocidos.
Con estos resultados obtenidos finalmente se tiene un adecuado modelado y carac-
terizacin de la extremidad del robot Hexapod Mark II, la cual por sus caractersticas
propias se considera como un sistema no lineal multivariable. En la actualidad existen
diversas tcnicas para linealizar y tratar este tipo de sistemas. La ventaja de utilizar
este modelo no lineal, es que en l se incluyen todos los efectos inerciales causados
por los movimientos de cada eslabn, por lo que resulta interesante el diseo de un
controlador que permita que el sistema siga referencias deseadas, y que adems re-
chaze perturbaciones presentes. En el siguiente captulo se propone un controlador
Proporcional Integral Generalizado que cumple con estas caractersticas.
Captulo 4
4.1. Introduccin
Las herramientas desarrolladas en el rea de control automtico en las ltimas
dcadas han permitido resolver una serie de problemas presentes en la dinmica de
diversos sistemas. Aspectos tales como no linealidades, rechazo de perturbaciones,
incertidumbre paramtrica entre otros, han sido tratados ampliamente. En la robtica
mvil se han desarrollado tcnicas de control clsico, adaptivo y robusto, aplicado a
posicionamiento y seguimiento de trayectorias [28].
En el presente Captulo se propone una ley de Control basada en una tcnica
denominada Control Proporcional-Integral Generalizado (GPI)[29] que a su vez em-
plea teora de sistemas diferencialmente planos [30][31][32] e introduce el concepto
de reconstructor integral1 . Dicha tcnica fue aplicada inicialmente a sistemas lineales
[33], y posteriormente fue extendida a sistemas no lineales como en [34]. Se utilizar
el modelo dinmico de la extremidad del robot Hexapod Mark II obtenido en el ca-
ptulo 3, combinado con la dinmica de los actuadores. Los parmetros obtenidos al
caracterizar los actuadores y la extremidad sern utilizados para obtener resultados
de simulacin.
Para poder llevar a cabo el control de movimiento, se disearn tambien trayecto-
rias de referencia que representarn los movimientos deseados, basadas en polinomios
de Bzier[35]. Asimismo se disear una ley de control Par-Calculado ampliamente
utilizada en aplicaciones de Robots Manipuladores. Los resultados de simulacin ob-
tenidos al utilizar esta ley servirn como referencia para comparar cuantitativamente
el desempeo del controlador GPI propuesto.
1 El
reconstructor integral permite observar una variable no medible a partir de la integral
de una cierta relacin entre la entrada y la salida de un sistema.
45
46 Captulo 4: Control GPI de la extremidad del Robot Hexapod Mark II
La extremidad del robot siguiendo los puntos deseados (P0 ...P8 ) se puede observar
en la Fig. 4.1, que como se puede ver simula la trayectoria que seguira para poder dar
un paso y avanzar hacia adelante. Asimismo se propone que la trayectoria completa la
efecte en dos segundos. Finalmente las trayectorias expresadas en funcin del tiempo
para cada grado de libertad se muestran en las ecuaciones 4.1, 4.2 y 4.3, en donde qid
representa la trayectoria angular a seguir por la i-sima articulacin. Llamaremos a
estas trayectorias como trayectorias de locomocin.
3 3
q3d = (1 e2t ) + (1 e2t )sen(10t). (4.6)
2 12
q = v. (4.9)
v = qd (t) Kp q Kd q. (4.10)
q(t) + Kd q + Kp q = 0. (4.12)
De esta ltima se obtiene el denominador de la funcin de transferencia en lazo
cerrado para la i-sima articulacin dado por: s2 + Kdi s + Kpi . Mediante el mtodo de
asignacin de polos[44][46] es posible establecer un polinomio deseado de tal forma que
el sistema responda a un cierto factor de amortiguamiento y frecuencia natural ,
como se muestra en la ecuacin 4.13. El factor de amortiguamiento determinar el tipo
de oscilacin que tendr el sistema, mientras que la frecuencia natural determinar
el ancho de banda y por consiguiente, la velocidad de respuesta.
a) Trayectorias de locomocin
d) Trayectorias de prueba
2 2
q 1(t) q 1(t)
1.5 q 2(t) q 2(t)
q 3(t) 1.5 q 3(t)
1 q 1d (t) q 1d (t)
q 2d (t) q 2d (t)
q 3d (t) 1 q 3d (t)
0.5
[rads]
[rads]
0
0.5
-0.5
0
-1
-1.5 -0.5
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5
[segs] [segs]
0
-0.02
[rads]
[rads]
-0.01
-0.04
-0.02
-0.06
-0.03
-0.04 -0.08
-0.05 -0.1
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5
[segs] [segs]
0 0
[N-m]
[N-m]
-0.05 -0.05
-0.1 -0.1
-0.15 -0.15
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5
[segs] [segs]
Las integrales de la salida, que son introducidas para garantizar la robustez ante
perturbaciones externas.
Las integrales de la entrada, que pueden ser introducidas a travs de los recons-
tructores integrales y directamente para facilitar la seleccin de las ganancias
del controlador.
y = u, (4.17)
con condiciones iniciales
x1 = y, x2 = y, (4.19)
las condiciones iniciales y(0) y y(0) pueden ser reescritas como:
en donde se observa que los polos del sistema estn ubicados en s1,2 = 0, es decir,
el sistema es crticamente estable, por lo tanto ante una perturbacin infinitesimal se
vuelve inestable.
Por otro lado, la ley de control por retroalimentacin de estados dada por:
u = k2 x2 k1 x1 , (4.22)
estabiliza el sistema mediante la asignacin arbitraria de polos. Ahora, asumiendo
que x2 no est disponible, necesita ser aproximada por medio de un observador; sin
embargo integrando x2 de 4.21 tenemos:
Z t
x2 = u( )d + x20 (4.23)
0
u = k2 x2 k1x1 , (4.25)
en donde la relacin del reconstructor integral x2 con la variable real x2 est dada
por:
x2 = x2 x20 . (4.26)
Sustituyendo la ecuacin anterior en 4.25 tenemos:
x2 + k2 x1 + k1 x1 = k2 x20 . (4.28)
Aplicando el teorema del valor final para analizar la respuesta en estado estable
[40][41], vemos que:
k2
x1 x20 6= 0. (4.29)
k1
54 Captulo 4: Control GPI de la extremidad del Robot Hexapod Mark II
Tomando nuevamente a x1 como salida, el sistema en lazo cerrado est dado por:
Z
x2 + k2 x1 + k1 x1 + k0 x1 = k2 x20 . (4.31)
k2
Definiendo = x1
R
k0
, el sistema en lazo cerrado puede ser reescrito como:
x2 + k2 x1 + k1 x1 = k0 ,
= x1 , (4.32)
k2
(0) = x20 .
k0
Como resultado, en estado estable se tiene:
x1 0. (4.33)
Regresando a las coordenadas originales, la ley de control est dada por:
Z Z
u = k2 u k1 y k0 y. (4.34)
y + k2 y + k1 y = k0 , (4.35)
= y. (4.36)
Finalmente, tomando la transformada de Laplace de la ecuacin 4.38, se tiene
que la ley de control y la funcin de transferencia del sistema en lazo cerrado en el
dominio de la frecuencia quedan como:
" #
k1 s + k0
U (s) = Y (s), (4.37)
s + k2
Captulo 4: Control GPI de la extremidad del Robot Hexapod Mark II 55
k1 s + k0
G(s) = . (4.38)
s3 + k2 s2 + k1 s + k0
Observamos que el polinomio caracterstico pc (s) = s3 + k2 s2 + k1 s + k0 tiene polos
completamente asignables de manera arbitraria.
Como se puede observar, los estados del sistema estimados mediante reconstruc-
tores integrales presentan un error respecto al valor real del estado.
El uso del vector de estado reconstruido integralmente en lugar del vector de estado
real genera una ley de control que desestabiliza el sistema en lazo cerrado debido al
error de la reconstruccin. Para cancelar dicho efecto desestabilizante el controlador
se complementa con un nmero suficiente de integrales iteradas del error de la salida.
Adicionalmente, si se desea que el controlador GPI sea robusto ante perturbaciones
constantes se aade una accin integral extra. En la siguiente seccin presentaremos
un controlador GPI pero aplicado a un sistema no lineal que consiste en un brazo
robtico.
Entonces la ley de control GPI no lineal presentada en [42] est dada por la
ecuacin 4.40, donde las matrices Kd , Kp , Ki y Kii son matrices diagonales de n n
que contienen elementos kd , kp , ki y kii respectivamente.
56 Captulo 4: Control GPI de la extremidad del Robot Hexapod Mark II
Z t
= D(q)[qd Kd (q qd ) Kp (q qd )] D(q)Ki [q(s) qd (s)] ds...
Z t Z 0s
... + D(q)[D1 (qd )C(qd , qd )qd ] + G(q) D(q)Kii [q(z) qd (z)] dzds... (4.40)
Z t 0 0
... + D(q)Kd D1 (qd )C(qd , qd )qd ds.
0
Z t
q = D1 (q)[G(q) + ] ds. (4.41)
0
Z t
q = q D1 (q)[C(q, q)q L ] ds + q(0). (4.42)
0
En los sistemas lineales, la diferencia entre las seales reconstruidas y las verda-
deras est dada por polinomios del tiempo que dependen de las condiciones iniciales
del estado. En el caso de sistemas no lineales, esta diferencia puede tambin incluir
los trminos del modelo de la planta que, debido a las no linealidades, no pueden
calcularse en funcin de integrales de la salida y de la entrada. Estos trminos no
lineales, aparecen como perturbaciones no lineales de la dinmica del error en lazo
cerrado. La manera de cancelar estas perturbaciones es utilizar en la ley de control,
prealimentaciones de la trayectoria a seguir con el fin de construir una perturbacin
no lineal que desaparece con el tiempo. [42] Con la teora conocida sobre sistemas
exponencialmente estables [43] se demuestra estabilidad en el equilibrio deseado. De
esta forma, sustituyendo la ley de control GPI en la ecuacin 4.39 tenemos un sistema
de la forma:
x = Ax + g(q, q, qd , qd ), (4.43)
...
q, q ] representa el vector de estados y g(q, q, qd , qd ) representa el
donde x = [q, q,
vector de trminos no lineales que desaparecen con el tiempo. Del sistema nominal,
Captulo 4: Control GPI de la extremidad del Robot Hexapod Mark II 57
kd = 21 1 + 22 2 , (4.47)
kp = 12 + 41 2 1 2 + 22 , (4.48)
ki = 22 12 2 + 21 1 22 , (4.49)
kii = 12 22 . (4.50)
Se realiz la simulacin del controlador GPI aplicado a la dinmica de la extre-
midad del robot Hexapod Mark II caracterizada en el captulo 3. Las simulaciones se
realizaron tanto para las trayectorias de prueba como para las trayectorias de loco-
mocin, utilizando como parmetros transitorios 1 = 1, 1 = 30, 2 = 0.9 y 2 = 31.
El vector de perturbaciones se estableci como L = [0.01, 0.01, 0.01]T [N m]. Los
resultados se muestran en la Fig. 4.3.
Observando las Fig. 4.3 a) y d) notamos que las trayectorias se siguien de una mejor
manera en comparacin con el controlador Par-Calculado, y esto lo podemos verificar
en la Figs. 4.3 b) y e) donde se muestran los errores de seguimiento cuyas magnitudes
han decaido a pesar de la perturbacin constante de par aadida al sistema.
58 Captulo 4: Control GPI de la extremidad del Robot Hexapod Mark II
a) d)
Trayectorias de locomocin Trayectorias de prueba
2 2
q 1(t) q 1(t)
1.5 q 2(t) q 2(t)
q 3(t) 1.5 q 3(t)
1 q 1d (t) q 1d (t)
q 2d (t) q 2d (t)
q 3d (t) 1 q 3d (t)
0.5
[rads]
[rads]
0
0.5
-0.5
0
-1
-1.5 -0.5
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5
[segs] [segs]
b) e)
Error de seguimiento para trayectorias de locomocin Error de seguimiento para trayectorias de prueba
0.035 0.08
q~1 (t) q~1 (t)
0.03
q~2 (t) q~2 (t)
0.06
0.025 q~3 (t) q~3 (t)
0.02
0.04
0.015
[rads]
[rads]
0.01 0.02
0.005
0
0
-0.005
-0.02
-0.01
-0.015 -0.04
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5
[segs] [segs]
0.05 0
[N-m]
[N-m]
0
-0.05
-0.05
-0.1
-0.1
-0.15
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2 -0.15
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5
[segs]
[segs]
Z t
= D(q)[qd Kd (q qd ) Kp (q qd )] D(q)Ki [q(s) qd (s)] ds...
Z t Z 0s
... + D(q)[D1 (qd )C(qd , qd )qd ] + G(q) D(q)Kii [q(z) qd (z)] dzds... (4.51)
Z t Z t0Z s0Z z
... + D(q)Kd D1 (qd )C(qd , qd )qd ds D(q)Kiii [q(r) qd (r)] drdzds,
0 0 0 0
de donde observamos que se tiene una nueva matriz diagonal Kiii de n n con
60 Captulo 4: Control GPI de la extremidad del Robot Hexapod Mark II
elementos dados por kiii , y que adems se utiliza el mismo reconstructor integral dado
por:.
Z t
q = D1 (q)[G(q) + ] ds. (4.52)
0
Tambin, el modelo dinmico perturbado del brazo robtico est determinado por:
Z t
q + D1 C(q, q)q = D1 (q)L + qd Kd [q + D1 (q)[C(q, q)q L ] ds + q(0)]...
0
Z t
1
... Kp (q qd ) + D (q)C(qd , qd )qd + Kd D1 (qd )C(qd , qd )qd ds... (4.55)
0
Z t Z tZ s
... Ki [q(s) qd (s)] ds Kii [q(z) qd (z)] dzds...
0 0 0
Z tZ sZ z
... D(q)Kiii [q(r) qd (r)] drdzds.
0 0 0
q = q qd ,
d3 d2
() = + K d ,
dt3 dt2
d3 1 d2 1
1 (L ) = D (q)L + K d D (q)L .
dt3 dt2
Representando la ecuacin anterior en el espacio de estados obtenemos la siguiente
dinmica en lazo cerrado:
0 I 0 0 0
0 0 I 0 0
A= 0
0 0 I 0 , (4.58)
0 0 0 0 I
Kiii Kii Ki Kp Kd
0
0
B= 0 .
(4.59)
0
I
De la misma forma que en [42], () y 1 (L ) representan trminos no lineales
acotados que desaparecen con el tiempo. Asimismo I representa una matriz identidad
de nn, donde n es el nmero de grados de libertad de un brazo robtico con uniones
rotativas. Ocupndonos ahora del sistema nominal, el polinomio caracterstico de A
queda como:
s5 +kd s4 +kp s3 +ki s2 +kii s+kiii = (s+a)(s2 +21 1 s+12 )(s2 +22 2 s+22 ), (4.61)
kd = a + 21 1 + 22 2 , (4.62)
62 Captulo 4: Control GPI de la extremidad del Robot Hexapod Mark II
[rads]
0
0.5
-0.5
0
-1
-1.5 -0.5
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5
[segs] [segs]
b) x 10
-3 Error de seguimiento para trayectorias de locomocin e) Error de seguimiento para trayectorias de prueba
8 0.02
q~1 (t) q~1 (t)
q~2 (t) q~2 (t)
6 0.015
q~3 (t) q~3 (t)
4 0.01
[rads]
[rads]
2 0.005
0 0
-2 -0.005
-4 -0.01
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5
[segs] [segs]
c) Torque aplicado para trayectorias de locomocin f) Torque aplicado para trayectorias de prueba
0.6 0.1
=1 (t) =1 (t)
0.5 =2 (t) =2 (t)
=3 (t) 0.05 =3 (t)
0.4
0.3
0
[N-m]
[N-m]
0.2
0.1 -0.05
0
-0.1
-0.1
-0.2 -0.15
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5
[segs] [segs]
kp = 12 + 22 + 22 2 (a + 21 1 ) + 2a1 1 , (4.63)
Para poder comparar de una mejor manera los ndices de desempeo, se colocaron
en un grfico de barras, tomando como referencia al 100 % el controlador con el
desempeo mas pobre, como se muestra en la Fig. 4.5.
Captulo 4: Control GPI de la extremidad del Robot Hexapod Mark II 65
4.8. Conclusiones
En este captulo se disearon trayectorias deseadas mediante polinomios de B-
zier para simular el movimiento de la extremidad del robot Hexapod Mark II. Como
resultado, las trayectorias de locomocin diseadas se aproximan a puntos de refe-
rencia propuestos y quedan representadas por funciones en el tiempo, siendo de gran
utilidad en el diseo e implementacin de controladores de movimiento en robots
manipuladores.
Se utiliz un controlador tipo Par-Calculado basado en el mtodo de dinmica
inversa para aplicarlo a la extremidad del Robot con el fin de evaluar la respuesta
transitoria. Se construy un sistema en lazo cerrado con la dinmica perturbada de
la extremidad del Robot, se sintonizaron las ganancias y en las simulaciones se obtu-
vieron resultados aceptables en el error de seguimiento, tanto para las trayectorias de
prueba como para las trayectorias de locomocin a pesar de incluir una perturbacin
constante en el sistema en lazo cerrado.
Los resultados en el error de seguimiento mejoraron al utilizar una ley de control
GPI no lineal[42] donde de la misma forma se sintonizaron las ganancias mediante el
mtodo de asignacin de polos y se realizaron las simulaciones correspondientes para
los dos tipos de trayectorias. Con ayuda del reconstructor integral se evit hacer uso
del vector de velocidades q. Los resultados mostraron una reduccin significativa en
el error de seguimiento, mantieniendo una magnitud mnima pero aceptable asociada
a la dinmica no lineal presentada en el sistema en lazo cerrado.
Para corregir an ms dicho error de seguimiento, se propuso modificar la ley de
Captulo 4: Control GPI de la extremidad del Robot Hexapod Mark II 67
control GPI no lineal aadiento un trmino que contiene la integral triple del error
de seguimiento. Este controlador modificado se aplic a la dinmica perturbada de
la extremidad del robot creando un sistema en lazo cerrado, para lo cual siguiendo
el mismo camino se sintonizaron las ganancias y se realizaron las simulaciones de la
respuesta del sistema para las trayectorias de prueba y de locomocin. Se observ que
el error de seguimiento disminuy an ms con esta ley de control GPI modificada.
Para obtener de forma cuantitativa el desempeo de los controladores emplea-
dos se utiliz el ndice de desempeo. Poniendo los resultados en un grfico de barras
observamos una mejora significativa en el desempeo de los controladores GPI en com-
paracin con el controlador Par-Calculado. Asimismo se observa un mejor desempeo
del controlador GPI modificado en comparacin con el GPI. Con esto se comprue-
ba que el trmino aadido a la ley de control GPI compensa de manera efectiva la
dinmica no lineal presente en el sistema de lazo cerrado.
Hay que reconocer la gran carga computacional requerida en las simulaciones de los
controladores GPI debido a las integraciones numricas necesarias para su ejecucin,
aunque en la actualidad se dispone de dispositivos de alta capacidad de procesamiento
capaces de realizar tanto simulaciones como implementacin en Hardware de dichos
controladores.
Los resultados de simulacin obtenidos son en condiciones ideales ya que en la
prctica observamos diferentes tipos de ruido. Para el caso del subsistema de sensado
de posicin de los motores de CD podemos encontrar fuentes de ruido tales como
ruido trmico, ruido blanco y ruido rosa [47]. En el subsistema mecnico de los motores
encontramos otras fuentes de ruido como el cascabeleo, friccin de Coulomb y friccin
viscosa[25], entre otros encontrados en la literatura.
Estos efectos aparecen como perturbaciones en el sistema, por lo que el hecho de
cancelarlos parcialmente depender de la instrumentacin realizada, del desempeo
del controlador utilizado y de la programacin numrica efectuada.
En el siguiente captulo se realizar la implementacin en Hardware de los con-
troladores Par-Calculado y GPI no lineal.
Captulo 5
5.1. Introduccin
Implementar un controlador para un robot manipulador implica acondicionar cada
uno de los subsistemas que componen el lazo cerrado. Como se muestra en la figura
5.1 se necesita de un planificador de trayectorias el cual determinar la posicin
angular a seguir en un instante de tiempo determinado. Dependiendo del recorrido
deseado del robot se pueden construir funciones en el tiempo que determinen de
forma automtica la posicin deseada, tal y como se hizo en el captulo anterior. Este
constructor de trayectorias puede ser una computadora, o bien podemos tener una
entrada de referencia externa manipulada por una persona.
Se necesita tambin de un sistema de cmputo que procese la ley de control con
el fin de calcular el torque o voltaje que deber aplicarse a cada una de las articu-
laciones del robot. Este sistema computacional requiere forzosamente del clculo en
tiempo real de los valores de las trayectorias reales y deseadas. Generalmente se utili-
za la modulacin por ancho de pulso (PWM) [51] para generar como salida el voltaje
aplicado a los actuadores y el sentido de giro. En este campo es muy utilizado las
computadoras personales, o tambin hardware de propsito general como las tarjetas
arduino, en las cuales es posible programar directamente los algoritmos de control o
bien establecer interfaces de comunicacin con otras plataformas [52].
La etapa de potencia es la que recibe y amplifica la seal PWM de voltaje pro-
veniente del sistema de cmputo. Para ello normlamente se utilizan circuitos en con-
figuracin Puente H [53] para accionar a los motores de corriente directa adems de
controlar su sentido de giro.
Los robots manipuladores normalmente constan de motores de corriente direc-
ta. Los mas modernos y los industriales constan de servomotores de transmisin
69
70 Captulo 5: Implementacin en Hardware del Controlador GPI
12 E/S digitales.
Contador de 32 bits.
En la Fig. 5.5 se observan los pines de entrada y salida de este dispositivo, donde
los pines AI son las entradas analgicas, AO las salidas analgicas y P las salidas
digitales.
Captulo 5: Implementacin en Hardware del Controlador GPI 73
POTENCIMETROS
MOTORES DE CD
PWM +/- 12V
0-5V
+12VDC
+5V
ENTRADAS ANALGICAS
PWM SALIDAS DIGITALES
L298N
GIRO
+12 V
USB
0 4
0 4
0 4
0 4
0 4
0 4
5.7. Conclusiones
En este captulo se present una breve descripcin del software de instrumentacin
virtual LabVIEW donde se programaron los Controladores Par-Calculado y GPI no
lineal. Se programaron tambin las matrices del modelo dinmico de la extremidad
del robot Hexapod Mark II, las trayectorias deseadas y la generacin de seales PWM,
encontrndonos con una manera prctica de realizar la programacin de toda la di-
nmica del sistema y de los procesos multitareas involucrados. Se present la tarjeta
de adquisicin de datos NI USB-6008 la cual sirvi como interfaz entre la computado-
ra y la extremidad del robot, teniendo como ventaja el tener en un solo dispositivo
la adquisicin y generacin de seales tanto analgicas como digitales. Asimismo se
explic la composicin de la plataforma experimental montada que se utiliz en la
implementacin de los controladores mencionados.
En los resultados experimentales del Controlador Par Calculado se observa un
aceptable seguimiento de las trayectorias deseadas. Esto demuestra que las estima-
ciones de los parmetros dinmicos de la extremidad del Robot y de los actuadores
es adecuada. El error de seguimiento presentado se encuentra tambin en un margen
Captulo 5: Implementacin en Hardware del Controlador GPI 83
85
86 Captulo 6: Conclusiones y trabajo a futuro
las magnitudes de los errores de seguimiento son suficientemente mnimas para apli-
caciones de locomocin en el robot caminante Hexapod Mark II.
Adems, con estos resultados obtenidos podemos notar que se ha realizado un
correcto modelado y caracterizacin de la extremidad del Robot Hexapod Mark II
representado por un sistema no lineal mutlivariable.
En el proceso de implementacin del controlador GPI se aproximaron las integrales
numricas mediante sumas de Riemman. Como trabajo a futuro se puede continuar
perfeccionando el cdigo fuente del controlador GPI para obtener mejores aproxi-
maciones de las integrales involucradas y con ello obtener una mejor respuesta del
sistema en lazo cerrado. Por ejemplo, se podra utilizar un mtodo de integracin
trapezoidal o el mtodo de Runge-Kutta.
Tambin se deja como trabajo a futuro la implementacin del controlador GPI
modificado en una tarjeta DSP o FPGA. Esto debido al gran esfuerzo computacional
que se requiere al calcular esta ley de Control propuesta, la cual ha demostrado tener
un mejor desempeo, tal y como se observa en los resultados de simulacin efectuados
en el Captulo 4.
Otro punto importante sera realizar la programacin necesaria para poder con-
trolar de manera simultnea las seis extremidades implementando las trayectorias de
locomocin diseadas en este trabajo de Tesis. De la misma forma se requiere tambin
de dispositivos de alta capacidad de procesamiento como los DSP o FPGA.
Con el control de las seis extremidades se podran generar patrones de marcha con
el fin de que el robot pueda efectuar movimientos hacia adelante, hacia atrs, giros,
etc. Como trabajo a futuro tambin se deja la realizacin e implementacin de una
mquina de estado finito que permita al robot realizar movimientos especficos.
Una mquina de estado finito puede definirse como un modelo computacional que
realiza cmputos en forma automtica sobre una entrada para producir una salida.
En [62] se define como:
Un conjunto de estados
Una funcin que relaciona los estados y las entradas con las salidas
Una funcin que relaciona los estados y las entradas con los estados (funcin de
transicin)
3. Partir del reposo - avanzar hacia adelante - avanzar hacia atrs - regresar al
reposo
En primer lugar se proponen los estados del robot Hexapod Mark II como se
describe en la Fig. 6.1. En ella se observan once estados diferentes E0, EF, E1, E2,
E3, E4, E5, E6, E7, E8, EB, donde las posiciones de cada extremidad cambian. Se
observa la simbologa ocupada para las extremidades que estn en contacto con el
suelo, y para aquellas que no lo estn. Tambin se observa la simbologa para ubicar
el frente del robot.
Figura 6.1: Estados de las extremidades del robot Hexapod Mark II.
[1] Sandoval X. Y., Garca M., Prez L. A. & Castillo E., Kinematics of Hex-Piderix
- A six legged robot - Using Screw Theory, International Journal of Advanced
Robotic Systems, Vol. 10, 2013.
[2] Arikan K. B., A Test Bench to Study Bioinspired Control for Robot Walking, Con-
trol Engineering and Applied Informatics, Vol. 13, Nm. 2, pp. 76-80, Rumania,
2011.
[3] Garca M., Gorrostieta E., et. al., Kinematic analysis for trajectory generation in
one leg of a hexapod robot, Procedia Technology 3, pp. 342-350, 2012.
[5] Habib M. K., Bioinspiration and Robotics: Walking and Climbing Robots, I-Tech
Education and Publishing, Vienna, 2007.
[6] Pounds P., Mahony R. & Corke P., Modeling and Control of a Quad-Rotor Robot,
Australian National University, Canberra, Australia, 2007.
[7] Raibert M., BigDog, the Rough-Terrain Quadruped Robot, Boston Dynamics,
Waltham, USA, 2008.
[8] Reyes F., Robtica, Control de Robots Manipuladores, Alfaomega, p. 522, 2011.
[9] Ollero A., Robtica, Manipuladores y Robots Mviles, Alfaomega, pp. 29-30, 2001.
[10] Takegaki M. and Arimoto S., A new feedback method for dynamic control of
manipulators, ASME J. Dyn. Syst. Meas. Control, 1981, pp. 119-125.
[11] Paul R. P., Modeling, Trajectory Calculation, and Servoing of a Computer Con-
trolled Arm, reporte tcnico AIM-, Laboratorio de Inteligencia Artificial de la
Universidad de Stanford, 1972.
91
92 Bibliografa
[12] Markiewicz B., Analysis of the Computed Torque Drive Method and Compari-
son with Conventional Position Servo for a Computed-Controlled Manipulator,
memorndum tcnico 33-601, Jet Propulsion Laboratory, marzo de 1973.
[13] Bejczy A., Robot Arm Dynamics and Control, memorndum tcnico 33-669, Jet
Propulsion Laboratory, febrero de 1974.
[14] Irving M., Electric Motors and Control Techniques, TAB Books, New York, 1994.
[15] Craig J, Hsu P. y Sastry S., Adaptive Control of Mechanical Manipulators, IEEE
Conference on Robotics and Automation, San Francisco, 1986.
[17] Brogliato B., Landau I. D. and Lozano R., Adaptive Control of Mechanical Ma-
nipulators: A unified approach based on pasivity, Int. J. of Robust and Nonlinear
Control, 1:187-202, 1991.
[19] Jurez J. A., Linares J., Guzmn E. and Sira-Ramrez H., Generalized Proportio-
nal Integral Tracking Controller for a Single-Phase Multilevel Cascade Inverter:
An FPGA Implementation, IEEE Transactions on Industrial Informatics, Vol. 10,
No. 1, 2014, pp. 256-266.
[20] Blanco A., Vela L. G., Lpez G., and Madagn A., A Generalized Proportional In-
tegral Controller for the Robust Perturbation Rejection in an Ankle Rehabilitation
Machine, International Conference on Mechatronics Electronics and Automotive
Engineering, 2013, pp. 140-145.
[21] Uicker J. J., Denavit J., and Hartenberg R. S., An iterative method for the dis-
placement analysis of spacial mechanisms, Trans. Applied Mechanics, 31 Series
E:309-314, 1964.
[23] Fu K. S., Gonzalez R. C. & Lee C. S. G., Robtica, control, deteccin, vision e
inteligencia, Mc Graw Hill, 1998.
[24] Spong M. W. & Vidyasagar, Robot modeling and control, Ed. Wiley & Sons,
2006.
Bibliografa 93
[25] De Silva C. W., Sensors and actuators, control system instrumentation, CRC
Press, 2007.
[27] Mecatronics Systems Inc., Mechatronics control kit model M-1, users manual
and software included, P. O. Box 11196, Champaign IL 61826-1196, 2001.
[28] Grizzle J. W., Chevallereau C., Choi J. H and Morris B., Feedback control of
dynamic bipedal locomotion, Boca Raton: CRC press, 2007.
[29] Fliess M., Marquez R., Delaleau E. and Sira-Ramrez H., Correcteurs
proportionnels-intgraux gnraliss, ESAIM: Control, Optimisation and Calcu-
lus of Variations, Vol. 7, pp. 23-41, 2002.
[30] Fliess M., Lvin J., Martin P. and P. Rouchon, Flatness and defect of nonlinear
systems: introductory theory and examples, Int. J. Control, Vol. 61, No. 6, 1327-
1361, 1995.
[31] Fliess M., Lvin J., Martin P. and P. Rouchon, Systmes diffrentiellement plats:
Intrt et examples, En Ecole dEt d Automatique de Grenoble. Flat systems:
theory and practice, 1996.
[32] Sira-Ramrez H. and Kumar S., Differentially flat systems, CRC press, 2004.
[33] Fliess M., Sur des pensers nouveaux faisons des vers anciens, in Actes Conference
Internationale Francophone d Automatique (CIFA-2000), pp. 26-36, Lille, France,
July 2000.
[35] Farin G. E., Curves and Surfaces for CAGD: A Practical Guide, 5ta Ed., Ed.
Morgan Kaufmann, San Francisco E. U., 2002.
[37] Sira-Ramrez H., et. al., Control de sistemas no lineales, linealizacin aproxima-
da, extendida, exacta, Ed. Pearson, 2005.
[38] Fliess M., Marquez R. and Delaleau E., State feedbacks without asymptotic ob-
servers and generalized PID regulators, in Nonlinear Control in the Year 2000,
Lecture Notes in Control and Information Sciences, Vol. 258, pp. 367-384, Sprin-
ger, London 2001.
94 Bibliografa
[40] Nise N. S., Sistemas de Control para Ingeniera, Compaa Editorial Continental,
1ra. Ed., 2002.
[41] Ogata K., Ingeniera de Control Moderna, Pearson Education, 5ta. Ed., 2010.
[42] Hernndez V.M. and Sira-Ramrez H., Generalized Proportional Integral Control
of Rigid Robots, Proc. of the 41st IEEE Conf. on Decision and Control, Las Vegas
E. U., 2002.
[43] Khalil H. Nonlinear Systems, Mcmillan Publishing Company, New York, 1992.
[44] Chen Ch., Linear System Theory and Design,Oxford University Press Inc., Third
Edition, 1999.
[46] Domnguez S., et. al., Control en el Espacio de Estado, Pearson Prentice Hall,
2nda. Ed., 2006.
[48] De Jager B. and Banens J., Experimental evaluations of robot controllers, Pro-
ceedings of the 33rd. Conference on Decision and Control. Lake Buena Vista, FI.
U.S.A.,pp. 363-368, December, 1994.
[50] Jaritz A. and Spong M., An experimental comparison of robust control algorithms
on a direct drive manipulator, IEEE Transactions on Control Systems Technology,
pp. 363-368, 1996.
[52] Lajara J. R. y Pelegr J., Sistemas integrados con arduino, Alfaomega Grupo
Editor, 1ra Ed., 2014.
[53] Torrente O., Ardunino, Curso Prctico de Formacin, Alfaomega Grupo Editor,
1ra Ed., 2013.
[54] Asada H. and Youcef K., Direct-Drive Robots: Theory and Practice, MIT Press
Classics, 1987.
[57] Nof S. Y., Handbook of Industrial Robotics, John Wiley & Sons Inc., Second
Edition, 1999.
[59] Del Ro J., Lzaro A. M., Shariat P., et. al., LabVIEW, Programacin Para
Sistemas De Instrumentacin, Alfaomega Grupo Editor, 1ra Ed., 2013.
[61] Hamed B., Application of a LabVIEW for Real-Time Control of Ball and Beam
System, IACSIT International Journal of Engineering Technology, Vol. 2, No.4,
pp. 401-407, August 2010.
[62] Selic B., Gullekson G. and Ward P., Real-Time Object-Oriented Modeling, Wiley
Professional Computing, 1994.
Apndice A
Publicaciones en Congresos
97
Apndice B
99
100 Apndice B: Cdigos escritos en MATLABr
47 '\it{x_0}','HorizontalAlignment','left','Fontsize',12,'Fontweight','b');
48 text(pos_texto_y0(1,1),pos_texto_y0(2,1)0.8,pos_texto_y0(3,1)offsetz_origin+0.5,...
49 '\it{y_0}','HorizontalAlignment','left','Fontsize',12,'Fontweight','b');
50 text(pos_texto_z0(1,1)+0.25,pos_texto_z0(2,1)1,pos_texto_z0(3,1)offsetz_origin,...
51 '\it{z_0}','HorizontalAlignment','left','Fontsize',12,'Fontweight','b');
52 %grid on;
53 axis([10 30 20 20 10 20]);
54 [A,B] = meshgrid(10:10:30, 20:10:20);
55 surf(A,B,zeros(size(A))10*ones(size(A)));
56 colormap([0 1 0; 0 1 1]);
57
58
59 %Marco de referencia 1
60 thetai=q1;
61 ai=l1;
62 di=0;
63 alphai=pi/2;
64 Hdesp=[ 0 0 0 3;
65 0 0 0 0;
66 0 0 0 0;
67 0 0 0 1];
68
69 H10=[cos(thetai) sin(thetai)*cos(alphai) sin(thetai)*sin(alphai) ai*cos(thetai) ;
70 sin(thetai) cos(thetai)*cos(alphai) cos(thetai)*sin(alphai) ai*sin(thetai) ;
71 0 sin(alphai) cos(alphai) di ;
72 0 0 0 1 ];
73
74 xo1=H10(1,4); %Posicin del origen del marco de ref. 1
75 yo1=H10(2,4);
76 zo1=H10(3,4);
77 MR0=[xvu,zeros(size(xvu)),zeros(size(xvu)); ...
78 zeros(size(yvu)),yvu,zeros(size(yvu)); zeros(size(zvu)),zeros(size(zvu)),zvu];
79 MR1=H10(1:3,1:3)*MR0+[xo1*ones(size(MR0(1,:))); yo1*ones(size(MR0(2,:))); zo1*ones(size(MR0(3,:)))];
80 plot3(MR1(1,:),MR1(2,:),MR1(3,:),'LineWidth',2.5,'Color','red');
81 pos_texto_x1=H10(1:3,1:3)*pos_texto_x0+[xo1;yo1;zo1];
82 pos_texto_y1=H10(1:3,1:3)*pos_texto_y0+[xo1;yo1;zo1];
83 pos_texto_z1=H10(1:3,1:3)*pos_texto_z0+[xo1;yo1;zo1];
84 text(pos_texto_x1(1,1),pos_texto_x1(2,1),pos_texto_x1(3,1),...
85 '\it{x_1}','HorizontalAlignment','left','Fontsize',12,'Fontweight','b');
86 text(pos_texto_y1(1,1),pos_texto_y1(2,1),pos_texto_y1(3,1),...
87 '\it{y_1}','HorizontalAlignment','left','Fontsize',12,'Fontweight','b');
88 text(pos_texto_z1(1,1),pos_texto_z1(2,1),pos_texto_z1(3,1),...
89 '\it{z_1}','HorizontalAlignment','left','Fontsize',12,'Fontweight','b');
90
91
92 %Marco de referencia 2
93 thetai=q2;
94 ai=l2;
95 di=0;
96 alphai=0;
97
98 H21=[cos(thetai) sin(thetai)*cos(alphai) sin(thetai)*sin(alphai) ai*cos(thetai) ;
99 sin(thetai) cos(thetai)*cos(alphai) cos(thetai)*sin(alphai) ai*sin(thetai) ;
100 0 sin(alphai) cos(alphai) di ;
101 0 0 0 1 ];
102
103 H20=H10*H21;
104 xo2=H20(1,4); %Posicin del origen del marco de ref. 2
105 yo2=H20(2,4);
106 zo2=H20(3,4);
107 MR2=H20(1:3,1:3)*MR0+[xo2*ones(size(MR0(1,:))); yo2*ones(size(MR0(2,:))); zo2*ones(size(MR0(3,:)))];
108 plot3(MR2(1,:),MR2(2,:),MR2(3,:),'LineWidth',2.5,'Color','red');
109 pos_texto_x2=H20(1:3,1:3)*pos_texto_x0+[xo2;yo2;zo2];
110 pos_texto_y2=H20(1:3,1:3)*pos_texto_y0+[xo2;yo2;zo2];
111 pos_texto_z2=H20(1:3,1:3)*pos_texto_z0+[xo2;yo2;zo2];
112 text(pos_texto_x2(1,1),pos_texto_x2(2,1),pos_texto_x2(3,1),...
113 '\it{x_2}','HorizontalAlignment','left','Fontsize',12,'Fontweight','b');
114 text(pos_texto_y2(1,1),pos_texto_y2(2,1),pos_texto_y2(3,1),...
115 '\it{y_2}','HorizontalAlignment','left','Fontsize',12,'Fontweight','b');
116 text(pos_texto_z2(1,1),pos_texto_z2(2,1),pos_texto_z2(3,1),...
117 '\it{z_2}','HorizontalAlignment','left','Fontsize',12,'Fontweight','b');
118
119
120 %Marco de referencia 3
121 thetai=q3;
122 ai=l3;
123 di=0;
124 alphai=0;
125
126 H32=[cos(thetai) sin(thetai)*cos(alphai) sin(thetai)*sin(alphai) ai*cos(thetai) ;
127 sin(thetai) cos(thetai)*cos(alphai) cos(thetai)*sin(alphai) ai*sin(thetai) ;
128 0 sin(alphai) cos(alphai) di ;
129 0 0 0 1 ];
130
131 H30=H10*H21*H32;
132 xo3=H30(1,4); %Posicin del origen del marco de ref. 3
133 yo3=H30(2,4);
134 zo3=H30(3,4);
Apndice B: Cdigos escritos en MATLABr 101
34 %
35 %Paso 1: Cinematica directa con respecto al centro de masa de cada eslabn:
36 fprintf('\n\n\n_______________________________________________________________');
37 fprintf('\nPaso 1: Cinemtica directa con respecto al centro de masa de cada eslabn:\n');
38
39 %Eslabn 1
40 %li=LC1;
41 %alphai=pi/2;
42 %ai=0;
43 %thetai=q1;
44 H10=[ cos(q1) 0 sin(q1) LC1*cos(q1);
45 sin(q1) 0 cos(q1) LC1*sin(q1);
46 0 1 0 0 ;
47 0 0 0 1 ];
48 R10=H10(1:3,1:3);
49 dLC1=H10(1:3,4);
50 x1=H10(1,4);
51 y1=H10(2,4);
52 z1=H10(3,4);
53 fprintf('\n| x1 | \n| y1 | = \n| z1 | ');
54 pretty([x1;y1;z1]);
55
56
57 %Eslabn 2
58 %li=LC2;
59 %alphai=0;
60 %di=0;
61 %thetai=q2;
62 H10=[ cos(q1) 0 sin(q1) L1*cos(q1);
63 sin(q1) 0 cos(q1) L1*sin(q1);
64 0 1 0 0 ;
65 0 0 0 1 ];
66
67 H21=[ cos(q2) sin(q2) 0 LC2*cos(q2);
68 sin(q2) cos(q2) 0 LC2*sin(q2);
69 0 0 1 0 ;
70 0 0 0 1 ];
71 H20=H10*H21;
72 R20=H20(1:3,1:3);
73 dLC2=H20(1:3,4);
74 x2=H20(1,4);
75 y2=H20(2,4);
76 z2=H20(3,4);
77 fprintf('\n| x2 | \n| y2 | = \n| z2 | ');
78 pretty([x2;y2;z2]);
79
80
81 %Eslabn 3
82 %li=LC3;
83 %alphai=0;
84 %di=0;
85 %thetai=q3;
86 H10=[ cos(q1) 0 sin(q1) L1*cos(q1);
87 sin(q1) 0 cos(q1) L1*sin(q1);
88 0 1 0 0 ;
89 0 0 0 1 ];
90
91 H21=[ cos(q2) sin(q2) 0 L2*cos(q2);
92 sin(q2) cos(q2) 0 L2*sin(q2);
93 0 0 1 0 ;
94 0 0 0 1 ];
95
96 H32=[ cos(q3) sin(q3) 0 LC3*cos(q3);
97 sin(q3) cos(q3) 0 LC3*sin(q3);
98 0 0 1 0 ;
99 0 0 0 1 ];
100
101 H30=H10*H21*H32;
102 R30=H30(1:3,1:3);
103 dLC3=H30(1:3,4);
104 x3=H30(1,4);
105 y3=H30(2,4);
106 z3=H30(3,4);
107 fprintf('\n\n\n| x3 | \n| y3 | = \n| z3 | ');
108 pretty([x3;y3;z3]);
109
110
111 %
112 %Paso 2: Obtener la cinemtica diferencial del centro de masa de cada
113 %eslabn
114 fprintf('\n\n\n________________________________________________________________');
115 fprintf('\nPaso 2: Cinemtica diferencial con respecto al centro de masa de cada eslabn:\n');
116
117 %Jacobiano J1
118 J1=[ diff(x1,q1) 0 0;
119 diff(y1,q1) 0 0;
120 diff(z1,q1) 0 0;
Apndice B: Cdigos escritos en MATLABr 103
121 0 0 0;
122 0 0 0;
123 1 0 0];
124 fprintf('\nJacobianos: \n\nJ1 = '); pretty(simplify(J1));
125
126 %Jacobiano J2
127 J2=[ diff(x2,q1) diff(x2,q2) 0;
128 diff(y2,q1) diff(y2,q2) 0;
129 diff(z2,q1) diff(z2,q2) 0;
130 0 sin(q1) 0;
131 0 cos(q1) 0;
132 1 0 0];
133 fprintf('\nJ2 = '); pretty(simplify(J2));
134
135 %Jacobiano J3
136 J3=[ diff(x3,q1) diff(x3,q2) diff(x3,q3);
137 diff(y3,q1) diff(y3,q2) diff(y3,q3);
138 diff(z3,q1) diff(z3,q2) diff(z3,q3);
139 0 sin(q1) sin(q1) ;
140 0 cos(q1) cos(q1) ;
141 1 0 0 ];
142 fprintf('\nJ3 = '); pretty(simplify(J3));
143
144 %Velocidades lineales de los centros de masa de cada eslabn
145 v1=J1(1:3,1)*q1p;
146 v2=J2(1:3,1:2)*[q1p;q2p];
147 v3=J3(1:3,1:3)*[q1p;q2p;q3p];
148 fprintf('\n\n\nVelocidades lineales de los centros de masa de cada eslabn: \n\nv1 = '); ...
pretty(simplify(v1));
149 fprintf('\nv2 = '); pretty(simplify(v2));
150 fprintf('\nv3 = '); pretty(simplify(v3));
151
152
153 %Paso 3: Obtencin de la matriz de Inercia
154 fprintf('\n\n\n_________________________________________________________________');
155 fprintf('\nPaso 3: Obtencin de la matriz de inercia\n');
156 TI1=[0,0,0;0,0,0;0,0,Iz1];
157 TI2=[Ix2,0,0;0,Iy2,0;0,0,Iz2];
158 TI3=[Ix3,0,0;0,Iy3,0;0,0,Iz3];
159
160 MI = simplify(m1*transpose(J1(1:3,:))*J1(1:3,:) + ...
transpose(J1(4:6,:))*R10*TI1*transpose(R10)*J1(4:6,:) + ...
161 m2*transpose(J2(1:3,:))*J2(1:3,:) + ...
transpose(J2(4:6,:))*R20*TI2*transpose(R20)*J2(4:6,:) + ...
162 m3*transpose(J3(1:3,:))*J3(1:3,:) + ...
transpose(J3(4:6,:))*R30*TI3*transpose(R30)*J3(4:6,:)) ;
163 fprintf('\nMI = '); pretty(simplify(MI));
164
165
166 %Paso 4: Obtencin de la matriz de fuerzas centrpetas y coriolis
167 fprintf('\n\n\n_________________________________________________________________');
168 fprintf('\nPaso 4: Obtencin de la matriz de fuerzas centrpetas y de coriolis\n');
169 MFCC = x*zeros(3,3);
170 temp1= x*zeros(1);
171 q=[q1;q2;q3];
172 qp=[q1p;q2p;q3p];
173 qpp=[q1pp;q2pp;q3pp];
174 for k=1:3
175 for j=1:3
176 for i=1:3
177 qi=q(i);
178 qj=q(j);
179 qk=q(k);
180 qip=qp(i);
181 temp1 = temp1 + (1/2)* (diff(MI(k,j),qi) + diff(MI(k,i),qj) diff(MI(i,j),qk))*qip;
182 end
183 MFCC(k,j)=simplify(temp1);
184 temp1= x*zeros(1);
185 end
186 end
187 fprintf('\nMFCC = '); pretty(simplify(MFCC));
188
189
190 %Paso 5: Obtencin de la pares gravitacionales
191 fprintf('\n\n\n__________________________________________________________________');
192 fprintf('\nPaso 5: Obtencin de los pares gravitacionales\n');
193 P1=m1*[0,0,g]*dLC1;
194 P2=m2*[0,0,g]*dLC2;
195 P3=m3*[0,0,g]*dLC3;
196 P=P1+P2+P3;
197 G=[diff(P,q1);diff(P,q2);diff(P,q3)];
198 fprintf('\nG = '); pretty(simplify(G));
199
200
201 %Paso 6: Obtencin de los momentos de torsin
202 fprintf('\n\n\n__________________________________________________________________');
203 fprintf('\nPaso 6: Obtencin de los momentos de torsin\n');
104 Apndice B: Cdigos escritos en MATLABr
66 b2=simplify(transpose(R10*R21)*[sin(q1);cos(q1);0]);
67 b3=simplify(transpose(R10*R21*R32)*[sin(q1);cos(q1);0]);
68
69 %Velocidades angulares
70 w1=simplify( b1*q1p);
71 w2=simplify(transpose(R21)*w1 + b2*q2p);
72 w3=simplify(transpose(R32)*w2 + b3*q3p);
73
74 %Aceleraciones angulares
75 alpha1=simplify( b1*q1pp + cross(w1,b1*q1p));
76 alpha2=simplify(transpose(R21)*alpha1 + b2*q2pp + cross(w2,b2*q2p));
77 alpha3=simplify(transpose(R32)*alpha2 + b3*q3pp + cross(w3,b3*q3p));
78
79 %Derivadas de las velocidades angulares d(w1)/dt, d(w2)/dt, d(w3)/dt
80 w1p=[ 0 ;
81 q1pp ;
82 0 ];
83
84 w2p=[ q1pp*sin(q2) + q2p*cos(q2)*q1p;
85 q1pp*cos(q2) q2p*sin(q2)*q1p;
86 q2pp ];
87
88 w3p=[ q1pp*sin(q2+q3) + cos(q2+q3)*q1p*(q2p+q3p);
89 q1pp*cos(q2+q3) sin(q2+q3)*q1p*(q2p+q3p);
90 q2pp + q3pp ];
91
92 %Tensores de inercia
93 TI1=[0,0,0;0,0,0;0,0,Iz1];
94 TI2=[Ix2,0,0;0,Iy2,0;0,0,Iz2];
95 TI3=[Ix3,0,0;0,Iy3,0;0,0,Iz3];
96
97 %
98 %Ecuaciones hacia adelante
99 %aceleraciones de los finales de los eslabones
100 ae1=simplify( cross(w1p,r0_1) + cross(w1,cross(w1,r0_1)));
101 ae2=simplify(transpose(R21)*ae1 + cross(w2p,r1_2) + cross(w2,cross(w2,r1_2)));
102 ae3=simplify(transpose(R32)*ae2 + cross(w3p,r2_3) + cross(w3,cross(w3,r2_3)));
103
104 %Aceleraciones de los centros de masa
105 ac1=simplify( cross(w1p,r0_c1) + cross(w1,cross(w1,r0_c1)));
106 ac2=simplify(transpose(R21)*ae1 + cross(w2p,r1_c2) + cross(w2,cross(w2,r1_c2)));
107 ac3=simplify(transpose(R32)*ae2 + cross(w3p,r2_c3) + cross(w3,cross(w3,r2_c3)));
108
109 %Aceleracin de la gravedad expresada en el isimo marco de referencia: gi
110 g1=transpose(R10)*g*[0;0;1];
111 g2=transpose(R10*R21)*g*[0;0;1];
112 g3=transpose(R10*R21*R32)*g*[0;0;1];
113
114 %
115 %Ecuaciones hacia atrs
116 %Fuerzas
117 f3=simplify( m3*ac3+m3*g3);
118 f2=simplify(R32*f3+m2*ac2+m2*g2);
119 f1=simplify(R21*f2+m1*ac1+m1*g1);
120
121 %Torques
122 tau3= simplify( 0 cross(f3,r2_c3) + 0 + TI3*alpha3 + cross(w3,TI3*w3));
123 tau2= simplify(R32*tau3 cross(f2,r1_c2) + cross(R32*f3,r2_c2) + TI2*alpha2 + cross(w2,TI2*w2));
124 tau1= simplify(R21*tau2 cross(f1,r0_c1) + cross(R21*f2,r1_c1) + TI1*alpha1 + cross(w1,TI1*w1));
125
126 fprintf('tau1y = '); pretty(simplify(tau1(2)));
127 fprintf('\n\ntau2z = '); pretty(simplify(tau2(3)));
128 fprintf('\n\ntau3z = '); pretty(simplify(tau3(3)));
10
11 %EulerLagrange
12 tau1_el= Ix2*q1pp + (Ix3*q1pp)/2 + (Iy3*q1pp)/2 Ix2*q1pp*cos(q2)^2 + Iy2*q1pp*cos(q2)^2 ...
(Ix3*q1pp*cos(2*q2 + 2*q3))/2 + (Iy3*q1pp*cos(2*q2 + 2*q3))/2 + L1^2*m2*q1pp + L1^2*m3*q1pp + ...
LC1^2*m1*q1pp + (LC3^2*m3*q1pp)/2 + L2^2*m3*q1pp*cos(q2)^2 + LC2^2*m2*q1pp*cos(q2)^2 + ...
(LC3^2*m3*q1pp*cos(2*q2 + 2*q3))/2 + Ix3*q1p*q2p*sin(2*q2 + 2*q3) + Ix3*q1p*q3p*sin(2*q2 + ...
2*q3) Iy3*q1p*q2p*sin(2*q2 + 2*q3) Iy3*q1p*q3p*sin(2*q2 + 2*q3) + ...
2*Ix2*q1p*q2p*cos(q2)*sin(q2) 2*Iy2*q1p*q2p*cos(q2)*sin(q2) + 2*L1*LC3*m3*q1pp*cos(q2 + q3) ...
+ 2*L1*L2*m3*q1pp*cos(q2) + 2*L1*LC2*m2*q1pp*cos(q2) + L2*LC3*m3*q1pp*cos(q3) ...
LC3^2*m3*q1p*q2p*sin(2*q2 + 2*q3) LC3^2*m3*q1p*q3p*sin(2*q2 + 2*q3) + ...
L2*LC3*m3*q1pp*cos(2*q2 + q3) 2*L2^2*m3*q1p*q2p*cos(q2)*sin(q2) ...
2*LC2^2*m2*q1p*q2p*cos(q2)*sin(q2) 2*L1*L2*m3*q1p*q2p*sin(q2) 2*L1*LC2*m2*q1p*q2p*sin(q2) ...
L2*LC3*m3*q1p*q3p*sin(q3) 2*L2*LC3*m3*q1p*q2p*sin(2*q2 + q3) L2*LC3*m3*q1p*q3p*sin(2*q2 ...
+ q3) 2*L1*LC3*m3*q1p*q2p*sin(q2 + q3) 2*L1*LC3*m3*q1p*q3p*sin(q2 + q3);
13 tau2_el= Iz2*q2pp + Iz3*q2pp + Iz3*q3pp (Ix2*q1p^2*sin(2*q2))/2 + (Iy2*q1p^2*sin(2*q2))/2 + ...
L2^2*m3*q2pp + LC2^2*m2*q2pp + LC3^2*m3*q2pp + LC3^2*m3*q3pp (Ix3*q1p^2*sin(2*q2 + 2*q3))/2 ...
+ (Iy3*q1p^2*sin(2*q2 + 2*q3))/2 LC3*g*m3*cos(q2 + q3) L2*g*m3*cos(q2) LC2*g*m2*cos(q2) ...
+ (L2^2*m3*q1p^2*sin(2*q2))/2 + (LC2^2*m2*q1p^2*sin(2*q2))/2 + (LC3^2*m3*q1p^2*sin(2*q2 + ...
2*q3))/2 + L1*L2*m3*q1p^2*sin(q2) + L1*LC2*m2*q1p^2*sin(q2) L2*LC3*m3*q3p^2*sin(q3) + ...
L2*LC3*m3*q1p^2*sin(2*q2 + q3) + 2*L2*LC3*m3*q2pp*cos(q3) + L2*LC3*m3*q3pp*cos(q3) + ...
L1*LC3*m3*q1p^2*sin(q2 + q3) 2*L2*LC3*m3*q2p*q3p*sin(q3);
14 tau3_el= Iz3*q2pp + Iz3*q3pp + LC3^2*m3*q2pp + LC3^2*m3*q3pp (Ix3*q1p^2*sin(2*q2 + 2*q3))/2 + ...
(Iy3*q1p^2*sin(2*q2 + 2*q3))/2 LC3*g*m3*cos(q2 + q3) + (LC3^2*m3*q1p^2*sin(2*q2 + 2*q3))/2 + ...
(L2*LC3*m3*q1p^2*sin(q3))/2 + L2*LC3*m3*q2p^2*sin(q3) + (L2*LC3*m3*q1p^2*sin(2*q2 + q3))/2 + ...
L2*LC3*m3*q2pp*cos(q3) + L1*LC3*m3*q1p^2*sin(q2 + q3);
15
16 %NewtonEuler
17 tau1_ne= Ix2*q1pp + (Ix3*q1pp)/2 + (Iy3*q1pp)/2 Ix2*q1pp*cos(q2)^2 + Iy2*q1pp*cos(q2)^2 ...
(Ix3*q1pp*cos(2*q2 + 2*q3))/2 + (Iy3*q1pp*cos(2*q2 + 2*q3))/2 + L1^2*m2*q1pp + L1^2*m3*q1pp + ...
LC1^2*m1*q1pp + (LC3^2*m3*q1pp)/2 + L2^2*m3*q1pp*cos(q2)^2 + LC2^2*m2*q1pp*cos(q2)^2 + ...
(LC3^2*m3*q1pp*cos(2*q2 + 2*q3))/2 + Ix3*q1p*q2p*sin(2*q2 + 2*q3) + Ix3*q1p*q3p*sin(2*q2 + ...
2*q3) Iy3*q1p*q2p*sin(2*q2 + 2*q3) Iy3*q1p*q3p*sin(2*q2 + 2*q3) + ...
2*Ix2*q1p*q2p*cos(q2)*sin(q2) 2*Iy2*q1p*q2p*cos(q2)*sin(q2) + 2*L1*LC3*m3*q1pp*cos(q2 + q3) ...
+ 2*L1*L2*m3*q1pp*cos(q2) + 2*L1*LC2*m2*q1pp*cos(q2) + L2*LC3*m3*q1pp*cos(q3) ...
LC3^2*m3*q1p*q2p*sin(2*q2 + 2*q3) LC3^2*m3*q1p*q3p*sin(2*q2 + 2*q3) + ...
L2*LC3*m3*q1pp*cos(2*q2 + q3) 2*L2^2*m3*q1p*q2p*cos(q2)*sin(q2) ...
2*LC2^2*m2*q1p*q2p*cos(q2)*sin(q2) 2*L1*L2*m3*q1p*q2p*sin(q2) 2*L1*LC2*m2*q1p*q2p*sin(q2) ...
L2*LC3*m3*q1p*q3p*sin(q3) 2*L2*LC3*m3*q1p*q2p*sin(2*q2 + q3) L2*LC3*m3*q1p*q3p*sin(2*q2 ...
+ q3) 2*L1*LC3*m3*q1p*q2p*sin(q2 + q3) 2*L1*LC3*m3*q1p*q3p*sin(q2 + q3);
18 tau2_ne= Iz2*q2pp + Iz3*q2pp + Iz3*q3pp (Ix2*q1p^2*sin(2*q2))/2 + (Iy2*q1p^2*sin(2*q2))/2 + ...
L2^2*m3*q2pp + LC2^2*m2*q2pp + LC3^2*m3*q2pp + LC3^2*m3*q3pp (Ix3*q1p^2*sin(2*q2 + 2*q3))/2 ...
+ (Iy3*q1p^2*sin(2*q2 + 2*q3))/2 LC3*g*m3*cos(q2 + q3) L2*g*m3*cos(q2) LC2*g*m2*cos(q2) ...
+ (L2^2*m3*q1p^2*sin(2*q2))/2 + (LC2^2*m2*q1p^2*sin(2*q2))/2 + (LC3^2*m3*q1p^2*sin(2*q2 + ...
2*q3))/2 + L1*L2*m3*q1p^2*sin(q2) + L1*LC2*m2*q1p^2*sin(q2) L2*LC3*m3*q3p^2*sin(q3) + ...
L2*LC3*m3*q1p^2*sin(2*q2 + q3) + 2*L2*LC3*m3*q2pp*cos(q3) + L2*LC3*m3*q3pp*cos(q3) + ...
L1*LC3*m3*q1p^2*sin(q2 + q3) 2*L2*LC3*m3*q2p*q3p*sin(q3);
19 tau3_ne= Iz3*q2pp + Iz3*q3pp + LC3^2*m3*q2pp + LC3^2*m3*q3pp (Ix3*q1p^2*sin(2*q2 + 2*q3))/2 + ...
(Iy3*q1p^2*sin(2*q2 + 2*q3))/2 LC3*g*m3*cos(q2 + q3) + (LC3^2*m3*q1p^2*sin(2*q2 + 2*q3))/2 + ...
(L2*LC3*m3*q1p^2*sin(q3))/2 + L2*LC3*m3*q2p^2*sin(q3) + (L2*LC3*m3*q1p^2*sin(2*q2 + q3))/2 + ...
L2*LC3*m3*q2pp*cos(q3) + L1*LC3*m3*q1p^2*sin(q2 + q3);
20
21 dif1=tau1_eltau1_ne;
22 dif2=tau2_eltau2_ne;
23 dif3=tau3_eltau3_ne;
24
25 fprintf('dif1='); pretty(simplify(dif1));
26 fprintf('\ndif2='); pretty(simplify(dif2));
27 fprintf('\ndif3='); pretty(simplify(dif3));
Apndice C
Matrices, Jacobianos y
Trigonometra
xi x j
xi yj ,
xi zj
de la misma forma, la proyeccin del vector yi queda como:
yi x j
yi yj ,
yi zj
y la proyeccin del vector zi como:
z i xj
zi yj ,
zi zj
107
108 Apndice C: Matrices, Jacobianos y Trigonometra
De esta forma, se tienen las proyecciones de los tres vectores unitarios del i-
simo marco de referencia, con respecto a los vectores unitarios del j-simo marco.
Reordenando las proyecciones en una matriz de 3x3 obtenemos una matriz de rotacin
Rij (ecuacin C.1), la cual representa la proyeccin del i-simo marco de referencia
con respecto al j-simo. Debido a que el producto punto est en funcin del ngulo
existente entre los vectores unitarios, se puede considerar que uno de ellos ha rotado
con respecto al otro. Es por ello que a la matriz anterior se le conoce como matriz de
rotacin.
x i x j yi x j z i xj
j
Hi = xi yj yi yj zi yj , (C.1)
xi zj yi zj zi zj
Sea oji un vector de posicin que inicia en el origen del j-simo marco de refe-
rencia y termina en el origen del i-simo marco. Sean xji , yij y zij sus componentes
vectoriales. Aumentando la matriz de rotacin Rij con las componentes vectoriales de
oji , y representndola como una matriz de 4 4 obtenemos una matriz de transfor-
macin homognea Hij , que representa la posicin y orientacin del j-simo marco de
referencia con respecto al i-simo, como se muestra en la ecuacin C.2.
xi xj yi xj zi xj xji
xi yj yi yj zi yj yij
Rij = j ,
(C.2)
xi zj yi zj zi zj zi
0 0 0 1
vn0 = Jv q, (C.3)
n0 = J q. (C.4)
La matriz Jacobiana angular J puede deducirse de la siguiente forma: conside-
rando que el vector unitario zi1 es el eje de rotacin del i-simo eslabn, y adems
Apndice C: Matrices, Jacobianos y Trigonometra 109
que el vector de velocidad angular del efector final n0 es la suma de los vectores de
velocidades angulares de cada eslabn, se tiene que n0 est dado por la ecuacin
C.5, donde vale 1 para las articulaciones de tipo rotacional, y 0 para las de tipo
prismtica.
n0 = 10 + ... + n1
0
= [z0 q1 + ... + zn1 qn ],
o0n
Jvi = . (C.8)
qi
0 d32 d33
c11 c12 c13
C(q, q) = c21 c22 c23
, (D.2)
c31 c32 0
111
112 Apndice D: Matrices D(q), C(q, q) y G(q)
Apndice D: Matrices D(q), C(q, q) y G(q) 113
0
G(q) = g2 . (D.3)
g3