Curso MasterRobtica Mvil y Navegacin Captulo 4: Modelo Cinemtico y Dinmicode Robots MvilesProfesor:Alberto Sanfeliu Cortsndice Restricciones Cinemticas El Jacobiano Configuraciones Estimacin de la posicin y orientacin Modelo probabilstico de la estimacin Modelo dinmicoRestricciones CinemticasHiptesis bsicas: El robot se mueve sobre una superficie plana Los ejes de guiado son perpendiculares al suelo El deslizamiento es despreciable en el periodo de control El robot no tiene partes flexibles Durante un periodo de tiempo suficientemente pequeo el vehculo se mover de un punto al siguiente a lo largo de un arco de circunferencia El robot se comporta como un slido rgido, de forma que si existen partes mviles, stas se situarn en la posicin adecuada mediante el sistema de controlRestricciones CinemticasSea p=[p1, ,pr]Tun vector de las variables necesarias para determinar la posicin y orientacin de todas las partes de un sistema fsico.Cuando se utilizan estas variables para describir un movimiento, es necesario tener en cuenta que las variables pueden no ser independientes. Por ejemplo en un pndulo simple, donde hay un grado de libertad, las coordenadas (x,y) de la masa deben cumplir la restriccin lylx02 2 2= + l y xRestricciones CinemticasEn general es posible formular restricciones del tipoes decir, se deben satisfacer s restricciones o ecuaciones en la variable p , sus derivadas y posiblemente, el tiempo t. Las restricciones en robtica mvil pueden ser: holnomas y no holnomasHolnomas: Restricciones entre las variables necesarias para determinar su posicin y orientacin donde no intervienen las velocidadesNo holnomas: Dependen de las velocidades y adems no son integrables; es decir que no se deduzca por derivacin con respecto al tiempo de una holnomas k t p p Gk,..., 1 0 ) , , ( = = s k t p Gk,..., 1 0 ) , ( = =s k t p p Gk,..., 1 0 ) , , ( = =Restriccin Holnoma Rueda que gira en una direccin fija sin posibilidad de giro La variable del actuador es el giro La variable en el espacio cartesiano x indica el espacio recorrido, la posicin de la ruedaRestriccin Holnoma Las dos variables obedecen a la condicin de rodadura: que depende de las velocidades, pero puede deducirse por derivacin de la restriccin holnomauc x =cte c x = uRestriccin No Holnoma Movimiento de una rueda en un plano Las variables del actuador son el giro y la orientacin Las variables en el espacio cartesiano son x, y e que indican el espacio recorrido y la orientacin de la ruedaRestriccin No Holnoma La condicin de rodadura sin deslizamiento introduce dos restricciones ya que el espacio que el punto de contacto recorre sobre el borde de la rueda es igual al que recorre en el plano. Por consiguiente proyectando la velocidad del punto de contacto en el plano, paralela y perpendicularmente al disco, se obtiene: Estas dos restricciones no son integrables con lo que no se puede obtener una relacin del tipoa partir de ellas.0 coscos= += + | |u | |sen y xc y sen x 0 ) , , , , ( = t y x Gk| uVehculos con RuedasConfiguraciones: Sncrona Direccionamiento diferencial Triciclo clsico BicicletaConfiguracin Sncrona (synchro-drive) Dos grados de libertad y una restriccin holnoma Comandos de velocidad lineal v y velocidad angular w Posicin y orientacin x, y, Configuracin Sncrona (synchro-drive) Existen trasmisiones que permiten orientar las tres ruedas simultneamente con una velocidad angular w y una lineal v Consideremos un sistema de referencia {G} y un sistema {L}con centro en el punto de guiado del vehculo y el eje YLen la direccin del eje longitudinal del vehculoConfiguracin Sncrona (synchro-drive) La velocidad lineal vendr dada por y la angular por La longitud del arco recorrido por el robot en un incremento de t viene dada por : tsvAA=XLYLsyxtwAA=|| A = A R s Supngase que el vehculo se desplazaen un intervalo de control segn un arco de circunferencia como muestra la figuraadjunta Configuracin Sncrona (synchro-drive) Las ecuaciones de movimiento en el sistema {L} en la posicin inicial son: Si la orientacin inicial del vehculo con respecto al sistema {G} es de , el movimiento en el sistema {G} se determina rotando , con lo que: ) ( ) ()) cos( ( ) (||A = AA = ARsen yR R xLL) cos( ) ( ) ( ] 1 ) [cos() ( ) ( ) cos( ] 1 ) [cos(| | | || | | |A + A = AA A = Asen R sen R ysen sen R R xConfiguracin Sncrona (synchro-drive) Suponiendo que el intervalo de control es suficientemente pequeo, tambin lo sera el cambio de orientacin, con lo que Sustituyendo en las ecuaciones anteriores | ||A ~ A~ A) (1 ) cos(sen) cos() (| || |A = AA = AR ysen R xConfiguracin Sncrona (synchro-drive) Tomando en cuenta que s = R y dividiendo ambas ecuaciones por t, tenemos y haciendo tender t a cero se llega a adems||coststysentstxAA=AAAA =AA||cos v ysen v x= =w = |Jacobiano de la configuracin Sncrona Posicin y orientacin en el plano [x, y, ]T Variables del actuador: = [v, w]T El modelo del Jacobiano se puede expresar de la siguiente forma:((

((((

=((((

=wvsenyxJ x1 00 cos0) (|||u uJacobiano de la configuracin Sncrona De las dos primeras expresiones el Jacobiano se deduce fcilmente la restriccin perpendicular no holnoma del movimiento: Para recuperar la otra restriccin no holnoma (tangencial al movimiento) es necesario resolver el modelo inverso del Jacobiano (ya que el Jacobiano no es cuadrado, es necesario emplear la seudoinversa)yxtg sen y x = = + | | | 0 cosx J J JT T1) (= uJacobiano de la configuracin Sncrona Y as se obtiene: De la primera se deduce Que coincide con la ecuacin vista anteriormente((((

((

=((

| |yxsenwv1 0 00 cos| | cos y sen x v + =Direccionamiento Diferencial En este caso las variables de control son las velocidades de las ruedas laterales wiy wd Si el radio de las ruedas es c, las velocidades lineales correspondientes son vi = wi c evd = wd cDireccionamiento Diferencial As, las velocidades lineal y angular del modelo vienen dadas por donde b es la distancia que separa ambas ruedas Si la tarea est especificada por v y w, entonces ser necesario convertir a wiy wdbc w wbv vwc w w v vvi d i dd i i d) (2) (2==+=+=cw b vwcw b vwdi) 2 / () 2 / (+==Jacobiano del Direccionamiento Diferencial Sustituyendo v y w en Obtenemos el Jacobiano del Direccionamiento Diferencial((

((((

=((((

wvsenyx1 00 cos0|||((

((((

=((((

diwwb c b cc csen c sen cyx/ /2 / ) cos ( 2 / ) cos (2 / ) ( 2 / ) (| || ||Triciclo Clsico La rueda delantera se utiliza tanto como para orientacin como para traccin Las variables de control suelen tomarse como el ngulo de direccin (o su velocidad angular w) de la rueda delantera y la velocidad de giro o traccin de la misma rueda wt(o su velocidad lineal correspondiente vt = c wt)Triciclo Clsico Se supone que el punto de gua (x, y) esta en el centro del eje trasero Las velocidades lineal v y angular w del vehculo son: El ngulo de orientacin del vehculo vara segnooo owcw v vt t== =cos coso o | senlvsenlcwt t= =Jacobiano del Triciclo Clsico Sustituyendo v y w en Obtenemos el Jacobiano del Triciclo Clsico((

((((

=((((

wvsenyx1 00 cos0|||((

(((((

=(((((

ooo |o |o| wvl sensenyxt1 00 / ) (0 cos cos0 cosTriciclo Clsico Conocidas la velocidad lineal v y angular w, deseadas del vehculo, las variables de control pueden calcularse mediante La cinemtica inversa se puede calcular invirtiendo el Jacobiano con lo que obtenemos cl w vcvwvlwarctgRlarctgtt2 2 2+= =|.|

\|=|.|

\|= o(((((

(((

+ + +=((

o|o ooo oo |o oo |oyxsen llsensen llsen lsen lwvt1 0 0 00) cos ( ) cos (cos cos) cos (cos2 2 2 222 22Bicicleta Un modelo simplificado del triciclo, o para la configuracin de Ackerman (vehculo de 4 ruedas con direccin delantera) es el denominado modelo bicicleta. Este modelo adopta las expresiones de la configuracin sncronaBicicleta de modo que y de la expresin de la curvatura donde||cos v ysen v x= = | v =l tg / ) (o =Bicicleta El Jacobiano es((

((((

=((((

wvsenyx00 cos0 |||Estimacin de la Posicin y Orientacin Dadas la posicin y orientacin iniciales x0 , y0 , 0para estimar la posicin y orientacin de un vehculo mvil en t y asumiendo que no existen deslizamientos ni otros errores en los modelos, la nueva posicin se obtiene integrando, as: Para una configuracin asncrona:Estimacin de la Posicin y Orientacin Para direccionamiento diferencial:Estimacin de la Posicin y OrientacinEjercicio: Derivar una expresin para la estimacin de la posicin y orientacin de un triciclo clsico y del modelo de la bicicleta. Modelo Probabilstico de la Estimacin En realidad el movimiento del robot est sujeto a distintas fuentes de ruido: Deslizamiento de ruedas Efectos de linealizacin (al decir que para pequeos ngulos se cumple Inexactitudes en el modelo, por ejemplo en el dimetro de la rueda, etc. Estas diferencias se modelan como variables aleatorias de media cero y varianza finitaModelo Probabilstico de la Estimacin Adems la restriccin no holnoma precisa aadir una tercera variable aleatoria para la orientacin final: De modo que la orientacin y posicin del modelo sncrono al incluir el ruido quedar como sigue:Modelo dinmico Se va a resolver el modelo dinmico del siguiente vehculo de 3 grados de libertad (x, y, )Modelo dinmico La restriccin no holnoma es que se puede reescribir de la forma0 cos = u u u d sen x yc c| |0 cos0 ) () (=((((

=qq Ayxd sendecir esda generaliza n articulaci q con q q A uu uModelo dinmico Otra forma de formar el Jacobiano es por medio del Kernel (espacio nulo) de AT de modo que0 ) ( ) ( = q A q ST0 cos1 cos0 cos) () (=((((

((

q Aq STTdsend sen dsenuuu uu u1) ( ) ( ) (= = J q S donde t v q S qModelo dinmico As((

((((

=((((

((

=((

=((((

==21211 0coscos,1 0coscos) () ( ) (vvd sensen dyxvvwvv d sensen dq St v q S qccu uu uuu uu uModelo dinmico El modelo dinmico de cualquier robot viene dado pors cinemtica nes restriccio de Matriz : ) (entrada la de acintransform de Matriz : ) (as desconocid ones perturbaci ymodelo el enErrores :gravedad la de Efecto : 0 ) (gravedad la de Friccin: ) (Coriolis de ys centrpeto Trminos : ) , (positiva) definida - semi , (simtrica inercia de Matriz : ) (donde) ( ) ( ) ( ) ( ) , ( ) (q Aq Bq Gq Fq q Vq Mq A q B q G q F q q q V q q MdTd m= = + + + +t t t Modelo dinmico0potencial Energa21)) cos ( 2 (21cintica Energacomo define se parEl Lagrange de funcinla Siendo2 2 2=+ + + =cccc= =pcp cEI y sen x d y x m EqLqLdtdE E L Lu u u ut t Mtodo de LagrangeModelo dinmico((((

+ + + +=cccc((((

+=cc((((

+ +=ccu u uu u u uu u u uu u u u u uu uu u I y sen x mdsen md md y mmd sen md x mqLqLdtdassen y x md I y sen x mdmd y msen md x mqL) cos (coscos) cos (00qLy cos (cos22Modelo dinmico Cada uno de los parmetros del modelo dinmico se calcula de la siguiente forma:u u u uuttt u uu uu uu uu uuu ) cos (cos ) ( , ,cos cos1) ( , 0 ) (0cos) , (coscos 00) (22sen y x mdsenq AR Rsen senrq B q Gsen mdmdq q VI md sen mdmd msen md mq Mc cTlr+ =((((

=((

=((((

= =((((

=((((

=Modelo dinmico Las propiedades estructurales se pueden aadir al modelo dinmico. Derivando en: sustituyendo en y multiplicando por ST, es posible eliminar la restriccin ATSv q = t tTd mA B G F q V q M = + + + + Modelo dinmico As el modelo dinmico queda((

((

= + +((

((

+((

((

= + + + + +lrdTd mT TR R rFwvmdwvB S G F v S V S M S v MS Stttut t1 110 00md - I 00 mdecir es) (