Seguimiento de Tayectoias de un Robot Móvil Omnidieccional Basado en el Modelo Dinámico J. A. Vázquez, M. Velasco-Villa CINVESTAV-IPN, Depatamento de Ingenieía Eléctica, Sección de Mecatónica, A.P. 14-74, 7, México D.F., México. {javazquez,velasco}@cinvestav.mx Resumen En este tabajo se pesenta el poblema de seguimiento de tayectoias de un obot móvil omnidieccional. A difeencia del enfoque clásico de contol basado en el modelo cinemático se popone el análisis y contol del poblema planteado mediante la consideación del modelo dinámico y la utilización del esquema de contol del tipo pa calculado desaollado oiginalmente en la liteatua de obots manipuladoes. El esquema popuesto es evaluado mediante su aplicación a un vehículo móvil del tipo (3,) paa el cual se muesta analíticamente la convegencia de los eoes de seguimiento y la estabilidad en lazo ceado. La evaluación final del esquema popuesto se ealiza mediante simulación, mostando un adecuado desempeño. Deecho esevado c UNAM-AMCA. Palabas Clave: Robot Mobil, Modelo Dinámico, Seguimiento. I. INTRODUCCIÓN El poblema de contol de obot móviles ha ecibido una gan atención en los últimos años, en paticula se han consideado fundamentalmente los poblemas de egulación y seguimiento de tayectoias paa difeentes tipos de obots móviles populsados po uedas de los tipos (2,) y (3,) (Bétouné and Campion, 1996), (Kalmá-nagy et al., 24). Desde el punto de vista cinemático, el poblema de contol de un obot móvil omnidieccional ha sido tatado desde difeentes pespectivas, en este sentido en (Canudas et al., 1996), (Campion et al., 1996) se pesentan el desaollo de modelos cinemáticos paa difeentes tipos de obots móviles. Po ota pate, en (Liu et al., 23) se pesenta el diseño de un contolado no lineal usando la técnica de contol po linealización de tayectoia. Recientemente en (Velasco-Villa, Alvaez-Aguie and Rivea-Zago, 27) se pesenta el poblema de contol a distancia de esta clase de sistemas po medio de un enfoque basado en un modelo disceto exacto con etados de tiempo del sistema paa el cual es posible sintetiza una etoalimentación causal que esuelve el poblema de seguimiento de tayectoias. En (Velasco-Villa, del Muo-Cuella and Alvaez- Aguie, 27) se tata el mismo poblema po medio de la consideación de un esquema de estimación de valoes futuos basado en el modelo no lineal del sistema en tiempo continuo. Po oto lado se han desaollado también difeentes tabajos basados en el contol de un obot móvil omnidieccional consideando su modelo dinámico. Po ejemplo en (Cate et al., 21) se descibe el diseño mecánico y se obtiene el modelo dinámico del obot móvil (3,) poponiendo un contol PID paa cada una de las uedas del obot independientemente del modelo no lineal del sistema. En (Bétouné and Campion, 1996) se aboda el modelado y contol de esta clase de vehículos obteniéndose su modelo dinámico mediante la fomulación de Eule-Lagange y se plantea una ley de contol po etoalimentación de salida paa esolve el poblema de seguimiento de tayectoias. A su vez, en (Williams et al., 22) se deduce un modelo dinámico consideando los efectos de deslizamiento ente las uedas del vehículo y la supeficie de tabajo, po ota pate, en (J. H. Chung et al., 23) se obtiene la dinámica de un obot móvil omnidieccional consideando uedas de tipo casto. En (Kalmá-nagy et al., 24) se lleva a cabo el contol de un obot móvil omnidieccional poponiendo la optimización del seguimiento de las tayectoias deseadas consideando las caacteísticas estuctuales del vehículo y po medio de la consideación de entadas de contol admisibles, la síntesis de la ley de contol se ealiza con base en los modelos dinámico y cinemático del obot. En la liteatua de contol de sistemas Eule-Lagange existen difeentes estategias sumamente utilizadas las cuales han pobado su efectividad fundamentalmente en el áea de obots manipuladoes, po ejemplo, las técnicas de contol basadas en pasividad como el contolado PD+ (Paden and Panja, 1998), o como el contolado adaptable desaollado en (Slotine and Li, 1988) con el cual se aboda el poblema de seguimiento de tayectoias paa obots ígidos. En (Loia, 1996) se popone un contolado basado en pa calculado más un PD no lineal paa esolve el poblema de seguimiento con etoalimentación de la salida en un sistema de un gado de libetad. En este tabajo se considea una estategia de contol (Loia and Otega, 1995) desaollada oiginalmente paa el caso de obots manipuladoes y adaptada al caso específico del obot (3,), la cual esuelve el poblema de seguimiento de tayectoias de manea semiglobal en el caso oiginal de obot manipuladoes. La oganización de este tabajo se descibe a conti-
nuación: En la Sección 2 se pesentan los modelos cinemático y dinámico del vehículo consideando la fomulación Eule-Lagange haciendo énfasis en algunas popiedades estuctuales del modelo obtenido. Posteiomente en la Sección 3, se pesenta la estategia de contol a utiliza paa la solución del poblema de seguimiento de tayectoias, mostando fomalmente la estabilidad en lazo ceado del esquema popuesto. En la Sección 4 se muesta mediante expeimentos en simulación la efectividad de la estategia de contol y finalmente en la Sección 5 se pesentan las conclusiones del tabajo. II. ROBOT MÓVIL OMNIDIRECCIONAL Consideando la clase de obots móviles populsados po uedas, una clasificación ampliamente difundida es la basada en los gados de movilidad δ m y dieccionabilidad δ s (Canudas et al., 1996). El obot móvil consideado en este tabajo es del tipo (δ m,δ s )=(3, ), es deci, cuenta con tes gados de movilidad y ceo de dieccionabilidad, lo que pemite que se pueda desplaza en cualquie diección de manea instantánea. Esta es la caacteística pincipal de este tipo de obots, lo cual epesenta una ventaja con especto a los otos tipos. Una vista supeio de la configuación del obot móvil omnidieccional se muesta en la Figua 1, donde puede identificase el maco de efeencia móvil X m Y m situado en el cento del vehículo, en el cual el eje X m se encuenta alineado con el eje pependicula de la ueda 3, mientas que las uedas 1 y 2 están dispuestas siméticamente con un ángulo δ =3 con especto al eje Y m. De la misma foma, en la Figua 1, se muesta el eje de efeencia fijo X Y el cual poveeá la localización absoluta del vehículo. Y y 2 Y m L Figua 1. u 2 Y m u 3 x G L 1 L X m 3 Robot móvil omnidieccional u 1 X m X El modelo cinemático del obot móvil omnidieccional puede se obtenido fácilmente al considea las elaciones de velocidad a pati de las coodenadas fijas mostadas en la Figua 1, poduciendo la cinemática invesa dada po, θ 1 θ 2 θ 3 = 1 sin(δ + φ) cos(δ + φ) L sin(δ φ) cos(δ φ) L cos(φ) sin(φ) L (1) donde θ 1, θ 2, θ 3 epesentan los desplazamientos angulaes de las uedas 1, 2y3espectivamente; (x, y) epesenta la posición del cento del obot con especto al eje X Y y φ el desplazamiento angula del obot. M p e I p son la masa total y el momento de inecia del obot, y se considea además I i como el momento de inecia de la ueda i; es el adio de las ueda y L es la distancia del oigen del maco de efeencia móvil a cada ueda. A. Modelo dinámico El modelo dinámico del obot móvil puede obtenese a pati del fomalismo de Eule-Lagange (Balakishna and Ghosal, 1995). Paa tal efecto, considee que el cento de masa G se encuenta en el oigen de los ejes de efeencia móvil. Consideando además que la enegía cinética povocada po los odillos de cada ueda es despeciable, se definen las enegías cinética K y potencial P ; con lo cual el Lagangiano del sistema esta dado po: L = K P L = 1 2 [M p(vgx 2 + V Gy 2 )+I p 2 ]+ 1 3 2 i=1 I 2 (2) iθ 1 donde V Gx, V Gy epesentan las velocidades a lo lago de los ejes X m, Y m espectivamente. La aplicación de las ecuaciones de Eule-Lagange, d L dt q j = F j j =1,..., 6 poducen el sistema dado po, M p x M p y m m I p φ m + M p M p I 1 I 2 I 3 donde, R = 1 θ 1 θ 2 θ 3 L q j ẋ m ẏ m m = [R] T F t1 F t2 F t3 ẋ ẏ (3) (4) + F t1 F t2 = τ 1 τ 2 (5) F t3 τ 3 sin(δ) cos(δ) L sin(δ) cos(δ) L 1 L, F ti es la fueza de ficción de la ueda i con especto al piso y τ i es el pa aplicado a la ueda i.
Despeciando los efectos de ficción y deslizamiento ente las uedas y la supeficie, así como consideando que las inecias de cada ueda son iguales, las expesiones (4) y (5) se tansfoman en, 3I 2 + M 2 p 3I 2 + M 2 p I p + 3L2 I 2 + M p x ṁ M p y m = m sin(δ) sin(δ) 1 = 1 cos(δ) cos(δ) L L L Es deci: τ 1 τ 2 τ 3 x y m m φ m D q m + C( q m ) q m = Bτ (7) Donde D es la matiz de inecia del sistema; C( q m ) es la matiz de Coiolis y fuezas centípetas y B epesenta la elación ente las entadas de contol con el sistema físico. Sin embago, el modelo (6) está expesado en coodenadas de efeencia móvil, po lo que paa tasfomalo a la efeencia inecial considee la tansfomación q m = T q donde, cos(φ) sin(φ) T = T (φ) = sin(φ) cos(φ). (8) 1 A pati del sistema (7) se obtiene, DT q +[DT + C( q)t ] q = Bτ. (9) Es fácil veifica que en este último caso, la matiz de inecias obtenida DT no es simética definida positiva. Con el fin de simplifica la epesentación anteio considee ahoa la pemultiplicación de la ecuación ( 9) po la invesa de la tansfomación (8), poduciéndose, T 1 DT q +[T 1 DT + T 1 C( q)t ] q = T 1 Bτ Así, po simplicidad, este modelo se expesa en la foma: (6) D q + C( q) q = Bτ (1) con: M p + 3I 2 D = 2 M p + 3I 2 2 I p + 3IL2 2 C( q) = 3I 2 2 B = 1 sin (δ + φ) sin (δ φ) cosφ cos (δ + φ) cos (δ φ) sinφ L L L. B. Popiedades estuctuales Antes de pesenta la estategia de contol se puntualizaan algunas obsevaciones y se detallaan algunas popiedades estuctuales del modelo dinámico del obot móvil dado po la ecuación (1) las cuales seán de gan utilidad en el establecimiento de un contol basado en pa calculado equivalente al utilizado en el caso de obots manipuladoes. Obsevación 1: Dado que la matiz D del modelo (1) es diagonal con entadas positivas, entonces D es simética definida positiva. Obsevación 2: Como la matiz D es constante, la popiedad de antisimetia clásica en obots manipuladoes N(q, q) =Ḋ 2C( q) se satisface tivialmente. Popiedad 3: Nótese que C( q) q no tiene una epesentación única, en paticula, C( q) q = 3I 2 2 q = 3I ẏ 2 2 ẋ q ẏ ẋ = C a ( q) q con lo cual, en este caso se obtiene una matiz C a ( q) de nueva cuenta antisimética. Popiedad 4: Consideando las matices C( q), C a ( q) es posible establece que paa cualquie vecto z R n, C a ( q)z = 3I 2 2 = 3I 2 2 = 3I 2 2 + ẏ ẋ ẏ ẋ z 2 + z 3 ẏ z 1 z 3 ẋ z 2 ẋ z 1 ẏ z 3 z 2 z 3 z 1 z 2 z 1 2z 2 2z 1 z 1 z 2 z 3 ẋ ẏ ẋ ẏ (11) esto es, C a ( q)z = C a (z) q + C (z) q. (12) Popiedad 5: A pati de la nueva estuctua de la matiz C a ( q) y de la epesentación obtenida paa C ( q) es fácil detemina cotas supeioes de la foma, C a ( q) k c q, C ( q) k q (13) donde es posible elegi k c = 3I 2 2 y k =2.
III. PROBLEMA DE SEGUIMIENTO DE TRAYECTORIAS En este tabajo se consideaá el poblema clásico de contol del seguimiento de una tayectoia pedeteminada q d (t). Se equiee diseña una etoalimentación de la foma, τ(t) =α(q (i) (t),q (i) d (t)) tal que en lazo ceado con el obot móvil dado en ( 1) se gaantice la convegencia del eo de seguimiento q = q q d a ceo, esto es, lim [q(t) q d(t)] =. t Con el fín de considea el poblema de contol descito anteiomente, considee el contolado: Bτ = D q d + C( q d ) q d K p q K d q + C ( q d ) q (14) donde como se mencionó anteiomente q d coesponde a la tayectoia deseada y el eo de seguimiento esta dado po q = q q d. K p y K d son matices de ganancias popocionales y deivativa espectivamente las cuales son diagonales definidas positivas, mientas que C ( q d ) está definido en (11). Considee ahoa el sistema (1) en lazo ceado con la etoalimentación (14), bajo estas condiciones se obtiene el sistema, D q + C( q) q C( q d ) q d + K p q + K d q C ( q d ) q = (15) Con el fin de obtene una expesión en téminos del eo de seguimiento, nótese que, C( q) q C( q d ) q d = C( q) q C( q d ) q d ±C( q) q d = C( q) q C( q d ) q d + C( q d ) q + C ( q d ) q [C( q)+c( q d )] q + C ( q d ) q donde se ha consideado la popiedad dada en ( 12). De lo anteio, la ecuación (15) puede ahoa eescibise en téminos del eo de seguimiento en la foma, D q +[C( q)+c( q d )] q + K d q + K p q =. (16) Con el fin de simplifica los desaollos posteioes, en lo que esta del tabajo se consideaá la notación C =C( q), C d = C( q d ) y C = C( q). A. Estabilidad en lazo ceado La dinámica del obot móvil omnidieccional ( 1) en lazo ceado con la etoalimentación ( 14) puede analizase a pati de la ecuación del eo dada en (16). Nótese en pincipio que ( q, q) =(.) es un punto de equilibio del sistema en el eo (16), po lo tanto la estabilidad del sistema puede analizase a pati de la función candidata de Lyapunov de la foma, V ( q, q) = 1 2 q T D q + 1 2 qt K p q + ɛ q T D q + 1 2 ɛ qt K d q (17) donde D esta definida en (1) y las matices K p, K d se definen en (14). Es fácil veifica que la función V ( q, q) dada en (17) es una función definida positiva paa todo ɛ> suficientemente pequeño. Consideando ahoa la deivada de V con especto al tiempo, se tiene que: V = q T D q + q T K p q + ɛ q T D q + ɛ q T D q + ɛ q T K d q esto es, V = q T [C + C d + K d ɛd] q ɛ q T K p q ɛ q T (C + C d ) q. (18) Dado que las matices C, C d son antisiméticas se tienen entonces que la ecuación (18) se eescibe en la foma, V = q T [K d ɛd] q ɛ q T K p q ɛ q T (C + C d ) q. Con el fin acota adecuadamente la ecuación anteio nótese que: C( q)+c( q d ) = C( q + q d + q d ) = C( q)+c(2 q d ) (19) = C+2C d Po lo que (18) se expesa como, V = q T K d q+ɛ q T D q ɛ q T K p q ɛ q T C( q) q 2ɛ q T C( q d ) q En vitud de las popiedades mencionadas peviamente, es posible acota la expesión anteio en la foma, V λ(k d ) q 2 + ɛ λ(d) q 2 ɛλ(k p ) q 2 +ɛk c q q 2 +2ɛk c q d q q donde λ(a) y λ(a) epesentan espectivamente el mínimo y el máximo valo popio de una matiz dada A y la constante k c esta dada en (13). La expesión anteio puede se eescita en la foma, V [ q q ] [ ] q P (2) q donde [ ] ɛλ(kp ) ɛk P = c q d. ɛk c q d λ(k d ) ɛ λ(d) ɛk c q A pati de los desaollos anteioes, el sistema (16) seá estable si la matiz P es definida positiva. Esta última condición se satisface a pati de las siguientes condiciones: i) ɛλ(k p ) > ii) det{p } > La condición i) se satisface tivialmente dado que la matiz K p es simética definida positiva. La condición ii) es equivalente a, ɛλ(k p ) [ λ(k d ) ɛ λ(d) ɛk c q ] ɛ 2 kc 2 q d 2 > lo cual es equivalente a: λ(k p )λ(k d ) kc 2 q 2 + λ(k p )[ λ(d)+k c q ] >ɛ. Dado que esta condición siempe puede se satisfecha paa ɛ suficientemente pequeño, se concluye po lo tanto la estabilidad asintótica del sistema.
IV. RESULTADOS EN SIMULACIÓN El esquema de contol popuesto anteiomente es evaluado en esta sección mediante expeimentos de simulación consideando el seguimiento de un cículo como tayectoia deseada. Los expeimentos se ealizan mediante una platafoma Matlab-Simulink. La evolución de la tayectoia cicula deseada se obtiene po medio de las ecuaciones, x d (t) = sin(t p (t)) (21) y d (t) = cos(t p (t)) donde es el adio del ciculo popuesto y el agumento t p esta dado po la elación paametizada en el tiempo de la foma, t p (t) =2d m ( t ) 3 3d m ( t ) 4 +12d m ( t ) 5 (22) t f t f t f donde t f el tiempo total de ejecución y d m es el valo medio de la tayectoia. Además (22) satisface las condiciones iniciales t p () =, dtp dt () = y t p(t f )=2d m, dtp dt (t f )=. Adicionalmente, el ángulo deseado de oientación φ d del obot móvil se obtiene mediante una evolución senoidal independiente de la tayectoia descita en el plano ( 21), en la foma, φ d (t) =φ max sin(ft). (23) donde φ max es el ángulo de oientación máximo deseado y f es la fecuencia deseada en la oientación. El esquema de contol consideado en este tabajo se muesta en la Figua 2 en la cual se desciben los elementos pincipales de la estuctua popuesta. ERROR DE SEGUIMIENTO en la figua 5 se muesta la convegencia de los eoes en velocidad. Finalmente en la figua 6 se muesta la evolución de la tayectoia en el plano X Y. Paes de contol (Nm) Eoes de seguimiento (m, ad) 2 1.5 1.5.5 1 1.5 2 5 1 15 2 25 3 35 4 Tiempo (s).1.8.6.4.2 Figua 3. Paes aplicados a las uedas del obot τ 1 τ 2 τ 3 e x e y e φ PARAMETRIZACION EN EL TIEMPO REFERENCIA CONTROLADOR PAR CALCULADO ROBOT MOVIL POSICION Y VELOCIDAD Figua 2. Esquema de simulación utilizado.2 5 1 15 2 25 3 35 4 Tiempo (s) Paa la ealización de los expeimentos se consideaon los paámetos del obot móvil (1) como, Mp =9.58 Kg, I =.52 Kgm 2, Ip =.17 Kgm 2 L =.25 m, =.3965 m Los paámetos consideados en la etoalimentación ( 14) esultaon se, K p1 = 6, K p2 = 45, K p3 = 1 K d1 = 7, K d2 = 65, K d3 = 2 La tayectoia deseada es de adio =.5 m. con condiciones iniciales (x, y, φ, ẋ, ẏ, ) =(.1,.4,,,, ) y un tiempo total de t f =4seg. En la Figua 3 se muestan los paes aplicados a cada una de las uedas del obot móvil, mientas que en la Figua 4 se muestan los eoes de seguimiento e x = x x d, e y = y y d y e φ = φ φ d los cuales pesentan una adecuada convegencia. A su vez, Figua 4. Eoes de seguimiento de la tayectoia V. CONCLUSIONES En este tabajo se pesenta una extensión de las técnicas de contol utilizadas en obótica de manipuladoes al áea de obots móviles. Con tal fin se popone una eescitua del modelo dinámico comúnmente encontado en la liteatua de obot móviles a el caso de un sistema con matiz de inecias constante simética definida positiva. En paticula se considea la utilización de la estategia de contol denominada pa calculado al caso específico de un obot móvil omnidieccional. De esta foma se pesenta una manea altenativa de esolve el poblema de seguimiento de tayectoias basado en el modelo dinámico del obot. La consideación de este modelo pemite el tatamiento
Eoes en velocidad (m/s, ad/s) Y (m).1.1.2.3.4.5.6 5 1 15 2 25 3 35 4 Tiempo (s).6.4.2.2.4 Figua 5. e x e y e φ Eoes en velocidad de la tayectoia Tayectoia deseada Tayectoia eal Campion, G., Bastin, G. and D Andéa-Novel, B. (1996). Stuctual popeties and clasification of kinematics and dynamics models of wheeled mobile obots, IEEE Tansactions on Robotics and Automation Vol. 12(No. 1): pp. 47 61. Canudas, C., Siciliano, B., Bastin, G., Bogliato, B., Campion, G., D Andea-Novel, B., Luca, A. D., Khalil, W., Lozano, R., Otega, R., Samson, C. and Tomei, P. (1996). Theoy of Robot Contol, Spinge-Velag, London. Cate, B., Good, M., Doohoff, M., Lew, J., II, R. L. W. and Gallina, P. (21). Mechanical design and modeling of an omni-diectional obocup playe, Poceedings RoboCup 21 Intenational Symposium, Seattle, WA, USA. J. H. Chung, B. J. Y., Kim, W. K. and Lee, H. (23). The dynamic modeling and analysis fo an omnidiectional mobile obot with thee caste wheels, Poceedings of the 23 IEEE Int. Confeence on Robotics and Automation, Taipei, Taiwan, pp. 521 527. Kalmá-nagy, T., D Andea, R. and Ganguly, P. (24). Nea-optimal dynamic tajectoy and contol of an omnidiectional vehicle, Robotics and Autonomous Systems 46: 47 64. Liu, Y., Wu, X., Zhu, J. and Lew, J. (23). Omni-diectional mobile obot contolle design by tajectoy lineaization, Poceedings of the Ameican Contol Confeence, Vol. 4, Denve, Coloado, USA, pp. 3423 3428. Loia, A. (1996). Global tacking contol of one degee of feedom eulelagange systems without velocity measuements, Euopean Jounal of Contol 2: 144 151. Loia, A. and Otega, R. (1995). On tacking contol of igid and flexible joints obot, Appl. Math. and Comp. Sci. 5(2): 11 113. Paden, B. and Panja, R. (1998). Globally asymptotically stable pd+ contolle fo obot manipulatos, Int. J. Contol 47: 1697 1712. Slotine, J. and Li, W. (1988). Adaptive manipulato contol: A case study, EEE Tans. Autom. Contol 33: 995 13. Velasco-Villa, M., Alvaez-Aguie, A. and Rivea-Zago, G. (27). Discete-time contol of an omnidiectional mobile obot subject to tanspot delay, Ameican Contol Confeence, New Yok City, USA, pp. 2171 2176. Velasco-Villa, M., del Muo-Cuella, B. and Alvaez-Aguie, A. (27). Smith-pedicto compensato fo a delayed omnidiectional mobile obot, 15th Mediteanean Confeence on Contol and Automation, Athens, Geece, pp. T3 27. Williams, R. L., Cate, B. E., Gallina, P. and Rosati, G. (22). Dynamic model with slip fo wheeled omnidiectional obots, IEEE Tansactions on Robotics and Automation 18: 285 293..5.5 X (m) Figua 6. Tayectoia esultante de dinámicas no contempladas po el modelo cinemático utilizado comúnmente en la liteatua de contol de obots móviles. Aunque en el pesente tabajo se despecian los efectos de ficción y deslizamiento ente las uedas y el piso, los esultados obtenidos muestan un desempeño aceptable. La estabilidad en lazo ceado del sistema es analizada y mostada fomalmente. VI. AGRADECIMIENTOS Apoyado po CONACyT, México. Poyecto 61713. REFERENCIAS Balakishna, R. and Ghosal, A. (1995). Modelling of slip fo wheeled mobile obots, IEEE Tansactions on Robotics and Automation Vol.11(No. 1): 126 132. Bétouné, A. and Campion, G. (1996). Dynamic modelling and contol design of a class of omnidiectional mobile obots, Poceedings of the 1996 IEEE Int. Confeence on Robotics and Automation, Vol. 3, Minneapolis, USA, pp. 281 2815.