Curso Master

Robtica Mvil y Navegacin

Captulo 4: Modelo Cinemtico y Dinmico
de Robots Mviles
Profesor:
Alberto Sanfeliu Corts
ndice
Restricciones Cinemticas
El Jacobiano
Configuraciones
Estimacin de la posicin y orientacin
Modelo probabilstico de la estimacin
Modelo dinmico
Restricciones Cinemticas
Hiptesis 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 control
Restricciones Cinemticas
Sea p=[p
1
, ,p
r
]
T
un 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
l
y
l
x
0
2 2 2
= + l y x
Restricciones Cinemticas
En general es posible formular restricciones del tipo
es 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 holnomas
Holnomas: Restricciones entre las variables necesarias para determinar su
posicin y orientacin donde no intervienen las velocidades
No holnomas: Dependen de las velocidades y adems no son integrables; es
decir que no se deduzca por derivacin con respecto al tiempo de una holnoma
s k t p p G
k
,..., 1 0 ) , , ( = =
s k t p G
k
,..., 1 0 ) , ( = =
s k t p p G
k
,..., 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 rueda
Restriccin Holnoma
Las dos variables obedecen a la condicin de rodadura:
que depende de las velocidades, pero puede deducirse por
derivacin de la restriccin holnoma
u

c x =
cte c x = u
Restriccin 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 rueda
Restriccin 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 tipo a partir de ellas.
0 cos
cos
= +
= +
| |
u | |
sen y x
c y sen x

0 ) , , , , ( = t y x G
k
| u
Vehculos con Ruedas
Configuraciones:
Sncrona
Direccionamiento diferencial
Triciclo clsico
Bicicleta
Configuracin 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 Y
L
en la
direccin del eje longitudinal del
vehculo
Configuracin 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 :
t
s
v
A
A
=
X
L
Y
L
s

y
x
t
w
A
A
=
|
| A = A R s
Supngase que el vehculo se desplaza
en un intervalo de control segn un arco
de circunferencia como muestra la figura
adjunta
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 = A
A = A
Rsen y
R R x
L
L
) cos( ) ( ) ( ] 1 ) [cos(
) ( ) ( ) cos( ] 1 ) [cos(
| | | |
| | | |
A + A = A
A A = A
sen R sen R y
sen sen R R x
Configuracin 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 = A
A = A
R y
sen R x
Configuracin 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
|
|
cos
t
s
t
y
sen
t
s
t
x
A
A
=
A
A
A
A
=
A
A
|
|
cos v y
sen 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:
(

(
(
(

=
(
(
(

=
w
v
sen
y
x
J x
1 0
0 cos
0
) (
|
|
|
u u

Jacobiano 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)
y
x
tg sen y x

= = + | | | 0 cos
x J J J
T T

1
) (

= u
Jacobiano de la configuracin Sncrona
Y as se obtiene:
De la primera se deduce
Que coincide con la ecuacin vista anteriormente
(
(
(

=
(

|
|

y
x
sen
w
v
1 0 0
0 cos
| | cos y sen x v + =
Direccionamiento Diferencial
En este caso las variables de control son las velocidades de las ruedas
laterales w
i
y w
d
Si el radio de las ruedas es c, las velocidades lineales correspondientes son
v
i
= w
i
c e v
d
= w
d
c
Direccionamiento 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
w
i
y w
d
b
c w w
b
v v
w
c w w v v
v
i d i d
d i i d
) (
2
) (
2

=
+
=
+
=
c
w b v
w
c
w b v
w
d
i
) 2 / (
) 2 / (
+
=

=
Jacobiano del Direccionamiento Diferencial
Sustituyendo v y w en
Obtenemos el Jacobiano del Direccionamiento Diferencial
(

(
(
(

=
(
(
(

w
v
sen
y
x
1 0
0 cos
0
|
|
|

(
(
(

=
(
(
(

d
i
w
w
b c b c
c c
sen c sen c
y
x
/ /
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 w
t
(o su velocidad lineal correspondiente
v
t
= c w
t
)
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 segn
o
o
o o
w
cw v v
t t
=
= =

cos cos
o o | sen
l
v
sen
l
cw
t t
= =

Jacobiano del Triciclo Clsico

Sustituyendo v y w en
Obtenemos el Jacobiano del Triciclo Clsico
(

(
(
(

=
(
(
(

w
v
sen
y
x
1 0
0 cos
0
|
|
|

(
(
(
(

=
(
(
(
(

o
o
o |
o |
o
| w
v
l sen
sen
y
x
t
1 0
0 / ) (
0 cos cos
0 cos

Triciclo 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
c
l w v
c
v
w
v
lw
arctg
R
l
arctg
t
t
2 2 2
+
= =
|
.
|

\
|
=
|
.
|

\
|
= o
(
(
(
(

(
(

+ + +

=
(

o
|
o o
o
o o
o |
o o
o |
o

y
x
sen l
lsen
sen l
l
sen l
sen l
w
v
t
1 0 0 0
0
) cos ( ) cos (
cos cos
) cos (
cos
2 2 2 2
2
2 2
2
Bicicleta
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 sncrona
Bicicleta
de modo que
y de la expresin de la curvatura
donde
|
|
cos v y
sen v x
=
=

| v =

l tg / ) ( o =
Bicicleta
El Jacobiano es
(

(
(
(

=
(
(
(

w
v
sen
y
x
0
0 cos
0

|
|
|

Estimacin de la Posicin y Orientacin

Dadas la posicin y orientacin iniciales x
0
, y
0
,
0
para 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 Orientacin
Ejercicio: 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 finita
Modelo 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 forma
0 cos = u u u


d sen x y
c c
| |

0 cos
0 ) (
) (
=
(
(
(

=
q
q A
y
x
d sen
decir es
da generaliza n articulaci q con q q A

u
u u
Modelo dinmico
Otra forma de formar el Jacobiano
es por medio del Kernel (espacio nulo) de A
T
de modo que
0 ) ( ) ( = q A q S
T
0 cos
1 cos
0 cos
) (
) (
=
(
(
(

q A
q S
T
T
d
sen
d sen d
sen
u
u
u u
u u
1
) ( ) ( ) (

= = J q S donde t v q S q

Modelo dinmico
As
(

(
(
(

=
(
(
(

=
(

=
(
(
(

=
=
2
1
2
1
1 0
cos
cos
,
1 0
cos
cos
) (
) ( ) (
v
v
d sen
sen d
y
x
v
v
w
v
v d sen
sen d
q S
t v q S q
c
c
u u
u u
u
u u
u u

Modelo dinmico
El modelo dinmico de cualquier robot viene dado por
s cinemtica nes restriccio de Matriz : ) (
entrada la de acin transform de Matriz : ) (
as desconocid ones perturbaci y modelo el en Errores :
gravedad la de Efecto : 0 ) (
gravedad la de Friccin : ) (
Coriolis de y s centrpeto Trminos : ) , (
positiva) definida - semi , (simtrica inercia de Matriz : ) (
donde
) ( ) ( ) ( ) ( ) , ( ) (
q A
q B
q G
q F
q q V
q M
q A q B q G q F q q q V q q M
d
T
d m

= + + + +
t
t t

Modelo dinmico
0
potencial Energa
2
1
)) cos ( 2 (
2
1
cintica Energa
como define se par El
Lagrange de funcin la Siendo
2 2 2
=
+ + + =
c
c

c
c
=
=
p
c
p c
E
I y sen x d y x m E
q
L
q
L
dt
d
E E L L
u u u u
t t

Mtodo de Lagrange
Modelo dinmico
(
(
(

+
+
+ +
=
c
c

c
c
(
(
(

+
=
c
c
(
(
(

+
=
c
c
u u u
u u u u
u u u u
u u u u u u
u u
u u

I y sen x md
sen md md y m
md sen md x m
q
L
q
L
dt
d
as
sen y x md I y sen x md
md y m
sen md x m
q
L
) cos (
cos
cos
) cos (
0
0
q
L
y
cos (
cos
2
2
Modelo dinmico
Cada uno de los parmetros del modelo dinmico se calcula de la siguiente
forma:
u u u
u
u
t
t
t u u
u u
u u
u u
u u
u
u

) cos (
cos ) ( , ,
cos cos
1
) ( , 0 ) (
0
cos
) , (
cos
cos 0
0
) (
2
2
sen y x m
d
sen
q A
R R
sen sen
r
q B q G
sen md
md
q q V
I md sen md
md m
sen md m
q M
c c
T
l
r
+ =
(
(
(

=
(

=
(
(
(

= =
(
(
(

=
(
(
(

=
Modelo dinmico
Las propiedades estructurales se pueden aadir al modelo dinmico.
Derivando en:
sustituyendo en
y multiplicando por S
T
, es posible eliminar la restriccin A
T

Sv q =

t t
T
d m
A B G F q V q M = + + + +

Modelo dinmico
As el modelo dinmico queda
(

= + +
(

+
(

= + + + + +
l
r
d
T
d m
T T
R R r
F
w
v
md
w
v
B S G F v S V S M S v MS S
t
t
t
u
t t
1 1
1
0 0
0
md - I 0
0 m
decir es
) (