Professional Documents
Culture Documents
Tesis Doctoral
Departamento de Ingeniería de Sistemas y Automática
Universidad Politécnica de Valencia
A mi familia
RESUMEN
La presente tesis doctoral aborda el modelado cinemático y control de robots
móviles con ruedas. En concreto se profundiza en los siguientes temas:
- Se plantea el modelado de una rueda genérica que incluye todos los tipos
comunes: fija, orientable centrada, orientable descentrada (castor) y sueca
(también denominada universal, Mecanum ó Ilon).
- It is modeled a generic wheel that includes all the common types: fixed,
centered orientable, off-centered orientable (castor) and swedish (also
referred to as universal, Mecanum or Ilon).
- Finally two solutions have been developed for the robotic applications of
parallel parking and line tracking with a vision system.
Abstract 3
RESUM
La present tesi doctoral aborda el modelatge cinemàtic i control de robots mòbils
amb rodes. En concret s'aprofundix en els temes següents:
- Es planteja el modelatge d'una roda genèrica que inclou tots els tipus comuns:
fixa, orientable centrada, orientable descentrada (castor) i sueca (també
nomenada universal, Mecanum o Ilon).
- Es planteja un esquema de control del robot amb tres bucles de control niats
(dinàmic, cinemàtic i de planificació) que es conceptualment similar als
empleats per robots manipuladors. En particular s’aprofundeix en el bucle
cinemàtic i indirectament en el de planificació, al caracteritzar les referències
que pot seguir cada tipus de robot sense error.
1. Introducción ……….…………………………………………………. 31
1.1 Introducción …...………………………………………………… 31
1.2 Objetivos de la tesis ……………………………………………. 32
1.3 Estructura de la tesis …..………………………………………… 33
Índice general 7
8 Índice general
Índice de tablas 13
ÍNDICE DE FIGURAS
Índice de figuras 15
16 Índice de figuras
En el Capítulo 3
Notación 21
22 Notación
En el Capítulo 4
En el Capítulo 5
En el Capítulo 6
En el Capítulo 7
En el Capítulo 8
En el Capítulo 9
1.1 INTRODUCCIÓN
Así pues, la presente tesis surge ante la necesidad de dar respuesta a las
cuestiones cinemáticas (modelado, control, …) de los robots móviles con ruedas.
Capítulo 1. Introducción 31
32 Capítulo 1. Introducción
Por otro lado, para la robótica móvil existe una reciente bibliografía, ver por
ejemplo [Inoue et al. 97] [Lyshevski et al. 00] [O’Connor et al. 96] [Canudas et
al. 97] [Samson 95], que aborda su modelado cinemático, dinámico y/o control.
Para el control del vehículo o robot móvil algunos autores han planteado
métodos geométricos [Ollero et al. 94] [Shin 90] y otros la utilización de
herramientas de la teoría de control clásica: aproximación lineal [O’Connor et al.
96]; linealización exacta [d'Andréa-Novel et al. 95] [De Luca et al. 93] [Park et
al. 99] [Tzafestas et al. 01]; controlabilidad [Samson 95] [Monaco et al. 91]
[Murray et al. 93]; estabilidad por Lyapunov [Lyshevski et al. 00] [Canudas et al.
97] [Dixon et al. 00]; control adaptativo [Inoue et al. 97] [Dixon et al. 01] [Fukao
et al. 00]; control predictivo [Ollero et al. 91]; etc.
esta aplicación. La visión tiene algunas ventajas respecto a los otros métodos
aunque con el inconveniente del coste computacional asociado al tratamiento de
imagen: umbralizado, segmentación, detección de contornos, etc. Para mejorar
dicho tratamiento se utilizan herramientas como la transformada de Hough
[Hough 59]. En [Marchant 95] se establece el posicionamiento a partir de un
sistema de visión respecto a una línea, para lo cual se realiza una aproximación
(innecesaria como demuestra el Capítulo 9) que da lugar a relaciones inexactas.
3.1 INTRODUCCIÓN
Finalmente, [Ollero 01] [Leow et al. 02] [Low et al. 05] son ejemplos de
estudios donde se deducen de forma no sistemática fórmulas y relaciones
cinemáticas, válidas sólo para casos particulares.
– Supuestos de diseño:
1) Los vehículos no poseen en su estructura partes flexibles, es decir: toda la
estructura es rígida.
2) Por cada rueda puede haber una o ninguna articulación de dirección.
3) Todos los ejes de dirección existentes son perpendiculares a la superficie
por la que se desplaza el vehículo.
4) Las ruedas pueden estar directamente en contacto con el suelo o a través
de unos rodillos.
– Supuesto operacional: La superficie de desplazamiento (suelo) es plana.
Los tipos de rueda fija, orientable y castor se engloban dentro del tipo
convencional, y también el tipo castor y sueca dentro del omnidireccional. Como
se indica en el capítulo posterior, las ruedas omnidireccionales no restringen el
movimiento del vehículo mientras que las otras sí.
0 0 ω 0 −ri 0 −rri
βi
Ei
v Ei = Rot ( − Rθ Ei ) ⋅ R v Ei
p
d Ei
d Siy ri ⋅ sin ( θ Ei )
Ei Li
0 βi (3.6)
v slip i = Rot ( − Rθ Ei ) Ei Ry ⋅
− d Rx − Ei
d Six ri ⋅ cos ( Li θ Ei ) rri ϕi
ϕ
ri
γi
li di
δi
αi
βi
Figura 3.4- Vista de planta con variables y parámetros definidos en (3.8)
a) Rueda fija y orientable: Son nulos los parámetros {rri, di, δi , γi } (rueda
alineada con el eje Siy), y βi es constante y variable respectivamente::
cβ sβi li ⋅ s ( βi − α i ) 0 p
v slip i = i ⋅ (3.11)
−sβi cβ i li ⋅ c ( β i − αi ) ri ϕi
c) Rueda sueca: Son nulos los parámetros {di, δi } (rueda alineada con el eje
Siy) y el ángulo de dirección βi es constante, resultando:
50 Capítulo 3. Relaciones cinemáticas
p
c ( βi + γi ) s ( βi + γi ) li ⋅ s ( βi + γi − αi ) ri ⋅ s γi 0
v slip i = ⋅ ϕ (3.13)
−s ( βi + γi ) c ( βi + γi ) li ⋅ c ( βi + γi − αi ) ri ⋅ c γi rri i
ϕ ri
Rot Gθ
G
p =
( R ) 00 ⋅ R p = Rot ( Gθ ) ⋅ p , (3.15)
Z
0 0 1
R
d i ⋅ c βi ri ⋅ s ( βi + δi ) rri ⋅ s Rθ Ei li ⋅ sαi + d i ⋅ cβi βi
ϕ
p = d i ⋅ s βi −ri ⋅ c ( βi + δi ) −rri ⋅ c Rθ Ei −li ⋅ c αi + d i ⋅ s βi ⋅ i (3.17)
0 0 0 1 ϕ ri
ω
ri ⋅ sβi li ⋅ s α i
ϕi
J f/o i = − ri ⋅ cβi −li ⋅ c α i q wf/o i =
0 ω
1
ri ⋅ s βi rri ⋅ s(βi + γi ) li ⋅ s αi ϕi
J s i = − ri ⋅ c βi − rri ⋅ c(βi + γi ) − li ⋅ c α i q ws i = ϕri
(3.19)
0 1 ω
0
d i ⋅ c βi ri ⋅ s( βi + δi ) l i ⋅ s α i + d i ⋅ cβ i βi
J c i = d i ⋅ s βi − ri ⋅ c( βi + δi ) −li ⋅ c α i + d i ⋅ s βi q wc i = ϕi
0 ω
0 1
Una vez calculadas las relaciones cinemáticas entre el vector de velocidad del
vehículo y el de cada rueda en sus distintas formas {(3.5), (3.6), (3.19), ...}, cabe
combinar todas ellas en una sola ecuación compuesta. Esto tiene gran relevancia
para el tipo de análisis que posteriormente se podrá aplicar a dicha ecuación.
A continuación se detallan las dos formas de ecuación compuesta que son
consideradas en este estudio de mayor utilidad.
La primera ecuación compuesta que se plantea es la obtenida juntando las
matrices Jacobianas de rueda de (3.19):
Ι J1 0 0 q w1
Ι ⋅ p = 0 J 2 ⋅ q w 2 ⇒ Ι ⋅ p = J ⋅ q (3.20)
0 w
Ι 0 0 J N q wN
v slip 1
p
A p1 A w1 0
q w1
v slip = v slip 2 = ⋅
A pN 0 A wN (3.22)
v q wN
slip N
p p
(
v slip = A p )
A w ⋅ = A ⋅ = A ⋅ q
q w q w
Capítulo 3. Relaciones cinemáticas 53
También puede plantearse con (3.23) qué variables pueden funcionar como
incógnitas y cuáles pasar al otro miembro para que exista solución única, lo que
implica un análisis de rangos de matrices y submatrices. Esto se aborda también
en el capítulo siguiente.
54 Capítulo 3. Relaciones cinemáticas
Este tipo de rueda consiste en una rueda esférica con algún tipo de mecanismo de
accionamiento. Para el mecanismo de accionamiento se suelen utilizar un
conjunto de rodillos: en [West et al. 92] y [Luo et al. 98] se emplean dos rodillos
horizontales y paralelos; en [Ostrovskaya et al. 00] se usan tres rodillos formando
un rectángulo horizontal (Figura 3.8); mientras que en [West et al. 95] se utilizan
rodillos con posiciones más complejas. Otro tipo de mecanismo de dirección se
propone en [Ferrière et al. 98], el cual consiste en una rueda sueca que gira sobre
la rueda esférica.
Figura 3.8- Rueda tipo bola con tres rodillos formando un rectángulo horizontal
Capítulo 3. Relaciones cinemáticas 59
Este tipo de rueda fue presentada en [Killough et al. 92] [Pin et al. 94] y consiste
en dos esferas de igual diámetro que han sido rebanadas en dos extremos
opuestos para asemejarse a ruedas redondeadas anchas. Las dos quasi-esferas se
sitúan con sus ejes en direcciones perpendiculares u ortogonales. Las quasi-
esferas pueden rotar libremente entorno a sus ejes. Un soporte sostiene los
extremos de los ejes de rotación de las quasi-esferas, el cuál les permite rotar en
la parte esférica de su superficie, mientras que giran libremente en la dirección
ortogonal. Esto produce el mismo efecto que las ruedas suecas. De modo que,
con las dos ruedas quasi-esféricas correctamente alineadas y de movimiento
sincronizado, el contacto con el suelo de una rueda esférica u otra está asegurado
en todo momento. De forma que la rueda ortogonal se acciona en una dirección
mientras que la dirección perpendicular permanece sin restricción.
incompatibles con una velocidad angular del vehículo no nula. Por lo que, en
general una de las dos quasi-esferas deslizaría inevitablemente. Los dos
problemas indicados (cambio de cinemática/dinámica y deslizamiento) serían
menos importantes en la medida en que la distancia entre las dos quasi-esferas
fuera despreciable respecto a las dimensiones del vehículo.
Z
X Y
Z
X Y
• El modelado cinemático se basa en una rueda genérica que incluye los tipos
más comunes: fija, orientable, castor y sueca.
Por tanto, en general para ecuaciones en las que existan de forma implícita
sistemas de coordenadas instantáneamente coincidentes hay que explicitar (a
través de lema de velocidad en cascada) la derivación de cada matriz que
relaciona los sistemas de coordenadas y sustituirlas por su valor.
4.1 INTRODUCCIÓN
Por ejemplo, [Alexander et al. 89] plantea, para deslizamiento nulo, modelos
cinemáticos directos e inversos parciales que relacionan las variables de
velocidad de rueda con las de la postura del vehículo. Lo anterior, si bien es de
gran utilidad práctica en un control geométrico, no permite una perspectiva
global del vehículo.
Los supuestos considerados son los mismos que del segundo punto del
capítulo anterior (al utilizarse las relaciones cinemáticas allí obtenidas) y
adicionalmente el de deslizamiento nulo entre la rueda y el suelo.
v slip 1
p
A p1 A w1 0
q w1
v slip = slip 2 =
v ⋅ =0
(4.1)
A pN 0 A wN
v q wN
slip N
p p
(
v slip = A p )
A w ⋅ = A ⋅ = A ⋅ q = 0
q w q w
q ∈ N(A) → q = B ⋅ η (4.2)
donde la matriz B forma una base de N (A), η es un vector de dimensión m que
representa la movilidad del vehículo.
Además m es el grado de movilidad del vehículo, que coincide con los
grados de libertad en el movimiento (GDLs) o con los accionamientos
independientes descritos en el punto 4.4, y que viene dado por la nulidad de A
(teorema sobre la relación entre el rango y la nulidad de una matriz):
q na = B na ⋅ η (4.4)
q B
a a
if Ba ≠ 0 → q na = B na ⋅ Ba−1 ⋅ q a (4.5)
donde q na son las restantes velocidades (no asignadas) del vector de velocidades
q .
No obstante, existe un camino alternativo a lo anterior basado en la propia
obtención del espacio nulo. El algoritmo utilizado normalmente para obtener la
matriz B (que forma una base de N (A)), que computacionalmente es el más
eficiente, se basa en el método del pivote de Gauss–Jordan, haciendo nulos los
elementos por debajo y por encima del pivote. Llegándose a:
Ι − B na ⋅ B a−1 q na q na B na ⋅ Ba−1
⋅
= 0 → q = ⋅η (4.6)
0 0 qa a Ι
Por tanto, para obtener la solución para unas variables, que se desea
funcionen como asignadas, basta con pasar al algoritmo que calcula la matriz B
con las columnas de dichas variables en último lugar.
Con lo anterior se pueden conseguir todas las soluciones cinemáticas. En
general, haría falta calcular la matriz B un número de veces igual a:
k k!
Cmk = = (4.8)
m m!⋅ ( k − m )!
A na ⋅ q na = − Aa ⋅ q a (4.9)
−1
q na = − ( ATna ⋅ A na ) ⋅ ATna ⋅ Aa ⋅ q a (4.11)
Así pues, el punto de partida para el modelado son: por un lado las matrices
Jacobianas de rueda de (3.19); y por otro la ecuación compuesta de matrices
Jacobianas de (3.20). Ambas del capítulo de relaciones cinemáticas:
ri ⋅ sβi li ⋅ sα i
ϕi
J f/o i = − ri ⋅ cβi −li ⋅ c α i q wf/o i =
0 ω
1
ri ⋅ sβi rri ⋅ s(βi + γi ) li ⋅ sα i ϕi
J s i = − ri ⋅ cβi − rri ⋅ c(βi + γi ) − li ⋅ c α i q ws i = ϕri
(4.12)
0 0 1 ω
d i ⋅ c βi ri ⋅ s( βi + δi ) li ⋅ sα i + d i ⋅ cβi βi
J c i = d i ⋅ s βi − ri ⋅ c( βi + δi ) −li ⋅ c α i + d i ⋅ s βi q wc i = ϕi
0 0 1 ω
Ι J1 0 0 q w1
Ι ⋅ p = 0 J 2 ⋅ q w 2 ⇒ Ι ⋅ p = J ⋅ q (4.13)
0 w
Ι 0 0 J N q wN
donde el subíndice {f, o, c, s} es para rueda {fija, orientable, castor, sueca} e Ι
es la matriz identidad de dimensión 3.
Capítulo 4. Modelado cinemático sin deslizamiento 71
( −1
)
Error = A ⋅ x* − B ⋅ y = A ⋅ ( A T ⋅ A ) ⋅ AT − I ⋅ B ⋅ y = ∆ ( A ) ⋅ B ⋅ y (4.16)
−I para A = 0
donde: ∆ (A) = −1
(4.17)
A ⋅ ( A ⋅ A ) ⋅ A − I
T T
para A ≠ 0
rango [ Ι , J ⋅ q
w]= 3 rango [ Ι , J ⋅ q
w]> 3
ó ó
∆ ( Ι ) ⋅ J ⋅ q w = 0 ∆ ( Ι ) ⋅ J ⋅ q w ≠ 0
Consistente. Inconsistente.
Solución única. No hay solución.
No hay deslizamiento para Hay deslizamiento en alguna/s
ese q w concreto de la/s rueda/s
( )
H = dim ( q w ) − rango Ι , J − rango Ι = dim ( q w ) − rango ∆ ( I ) ⋅ J (4.19)
rango[J] =dim (q
w) J ⋅ q w = Ι ⋅ p
Siempre verdadero Ecuación del vehículo
Criterio movimiento resoluble
rango [ J , Ι ⋅ p ] = rango[J]
rango [ J , Ι ⋅ p ] > rango[J]
ó
ó
∆ ( J ) ⋅ Ι ⋅ p = 0
∆ ( J ) ⋅ Ι ⋅ p ≠ 0
{ ∆ ( J i ) ⋅ p = 0 para i =1...N}
{ ∆ ( J i ) ⋅ p ≠ 0 para algún i}
Restricciones al movimiento
Para saber el número de velocidades del vehículo sobre las que se puede
actuar independientemente (GDLs del vehículo) se utiliza:
( )
G = 3 − rango J, Ι − rango [ J ] = 3 − rango ∆
( J ) ⋅ I (4.22)
( J T ⋅ ∆ ( J ) ⋅ J ) −1 ⋅ J T ⋅ ∆ ( J )
1u 1nu 1u 1u 1nu
T
( J ⋅ ∆ ( J 2nu ) ⋅ J 2u ) ⋅ J 2u ⋅ ∆ ( J 2nu ) ⋅ p
−1 T
q wu = 2u
( J T ⋅ ∆ ( J ) ⋅ J ) −1 ⋅ J T ⋅ ∆ ( J )
Nu Nnu Nu Nu Nnu
(4.26)
( J T ⋅ ∆ ( J ) ⋅ J )−1 ⋅ J T ⋅ ∆ ( J )
1nu 1u 1nu 1nu 1u
T
( J ⋅ ∆ ( J 2u ) ⋅ J 2nu ) ⋅ J 2nu ⋅ ∆ ( J 2u ) ⋅ p
−1 T
q wnu = 2nu
( J T ⋅ ∆ ( J ) ⋅ J )−1 ⋅ J T ⋅ ∆ ( J )
Nnu Nu Nnu Nnu Nu
En particular, para una rueda i tipo omnidireccional (castor o sueca) con las
tres variables accionadas (o las tres no accionadas) la expresión se simplifica a
(ver Demostración 4.4 del subaparado 4.4.8):
Iu ⋅ p = J u ⋅ q wu
rango[ Ι u ] = 3
Ιu ⋅ p = J u ⋅ q wu
{ | Ι uT ⋅ Ι u |≠ 0 } Ecuación actuadores del vehículo rango[ Ι u ] <3
Criterio de actuadores
{ | Ι uT ⋅ Ι u |= 0 }
suficientes
rango Ιu , J u ⋅ q wu = rango Ιu
rango Ιu , J u ⋅ q wu > rango Ιu
ó ∆ ( Ιu ) ⋅ J u ⋅ q wu = 0 rango Ιu , J u ⋅ q wu > rango Ιu
ó ∆ ( Ιu ) ⋅ J u ⋅ q wu ≠ 0
Criterio de acoplamiento entre
actuadores Indeterminado.
Infinitas soluciones.
Consistente. Inconsistente. Actuadores insuficientes
Solución única. No hay solución.
No hay conflicto entre Conflicto entre actuadores.
actuadores Hay deslizamiento.
Para poder aplicar (4.31) Ιns ha de ser pseudo invertible, lo cual se cumple
siempre puesto que las matrices J e Ι son siempre pseudo invertibles.
Hay que tener en cuenta que, para que las matrices Jacobianas (sensorizada
y no sensorizada) de las ruedas con dirección (castor y orientable) sean
calculables, hay que sensorizar el ángulo de la dirección. Otra alternativa sería
realizar la integración numérica del ángulo a partir de la velocidad de giro
(calculada en (4.31) en caso de no sensorizarla) y el valor inicial del ángulo:
Ι − J ⋅ ( J T ⋅ ∆ ( J ) ⋅ J )−1 ⋅ J T ⋅ ∆ ( J )
1ns 1ns 1s 1ns 1ns 1s
J1s 0 0
−1 0
Ι − J 2ns ⋅ ( J 2ns ⋅ ∆ ( J 2s ) ⋅ J 2ns ) ⋅ J 2ns ⋅ ∆ ( J 2s ) ⋅ p =
T T
J 2s ⋅ q
0 ws (4.34)
Ι − J ⋅ ( J T ⋅ ∆ ( J ) ⋅ J ) −1 ⋅ J T ⋅ ∆ ( J ) 0 0 J Ns
Nns Nns Ns Nns Nns Ns
Ιs ⋅ p = J s ⋅ q ws
Capítulo 4. Modelado cinemático sin deslizamiento 81
rango[ Ιs ] = 3
Ιs ⋅ p = J s ⋅ q ws
{ | ΙsT ⋅ Ιs |≠ 0 } Ecuación de sensores del vehículo rango[ Ιs ] <3
Criterio de sensores { | ΙsT ⋅ Ιs |= 0 }
suficientes
( −1
∆( J ) ⋅ I = J⋅ ( JT ⋅ J) ⋅ JT − Ι ⋅ I =)
−1
J1 0 0 J1T 0 0 J1 0 0 J1 0 0 Ι
T
0 J2 ⋅ 0 J T2 0 J2 ⋅ 0 J 2 − Ι ⋅ Ι
T
⋅ 0
0 0 0
0
0 JN 0 0 J TN 0 0 JN 0 0 J TN Ι
(4.35)
Operando en la expresión anterior:
−1
T
J1 0 0 ( J1 ⋅ J1 ) 0 0
J1
T
0 0 Ι
0 J
2 ⋅ 0 (J T
2 ⋅ J2 )
⋅
0 J T2 Ι
−Ι ⋅ =
0 0 0
0
0 0 JN 0 0 (JN ⋅ JN )
T 0 J TN Ι
J T ⋅ J −1
J1 0 0 ( 1 1 )
0 0
J1T 0 0 Ι 0 0 Ι
−1 0
0 J2 ⋅ 0 (J T
2 ⋅ J2 ) ⋅ J T2 0 Ι Ι
− ⋅
0 0 0 0
0 0 JN 0 0 ( J TN ⋅ J N )
−1 0 0 J TN 0 0 Ι Ι
J ⋅ ( JT ⋅ J ) ⋅ JT − Ι
−1
0 0
1 1 1 1
Ι ∆ ( J1 )
−1 Ι ∆ J
0 J2 ⋅ ( J T2 ⋅ J 2 ) ⋅ J T2 − Ι ⋅ = ( 2 )
=
0
∆ ( J N )
0 J N ⋅ ( J TN ⋅ J N ) ⋅ J TN − Ι
−1 Ι
0
(4.36)
⋅ J )
⋅ u ⋅ I ⋅ p
T
J nu
(4.37)
nu nu
(J ⋅ (Ι − J
T
u nu ⋅ ( J ⋅ J nu ) ⋅ J
T
nu
−1 T
nu )⋅ J ) u = − ( J ⋅ ∆ ( J nu ) ⋅ J u )
T
u
−1
−1
(D − C ⋅ A −1 −1
(
⋅ B ) = J Tnu ⋅ J nu − J Tnu ⋅ J u ⋅ ( J Tu ⋅ J u ) ⋅ J Tu ⋅ J nu
−1
) =
(4.40)
−1
(J ⋅ (Ι − J ⋅ ( J ⋅ J ) ⋅ J ) ⋅ J )
T
nu u
T
u u
−1 T
u nu = − ( J ⋅ ∆ ( J u ) ⋅ J nu )
T
nu
−1
q wu ( u ( nu ) u ) u nu ( nu (
J T ⋅ ∆ J ⋅ J −1 ⋅ J T ⋅ J ⋅ J T ⋅ J −1 ⋅ J T − Ι
nu ) nu ) ⋅ I ⋅ p = ( J ⋅ ∆ ( J nu ) ⋅ J u ) ⋅ J Tu ⋅ ∆ ( J nu )
T
u
−1
⋅ I ⋅ p
=
q
1
( −1
wnu ( J Tnu ⋅ ∆ ( J u ) ⋅ J nu ) ⋅ J Tnu ⋅ J u ⋅ ( J Tu ⋅ J u ) ⋅ J Tu − Ι
−
) ( J T
nu ⋅ ∆ ( u ) nu ) nu ( u )
J ⋅ J
−1
⋅ J T
⋅ ∆ J
(4.42)
∆ ( J1 ) 0 0
∆(J) =
0 ∆ (J2 )
(4.43)
0
0 0 ∆ ( J N )
1u 0
0 J T2 u 0 ∆ ( J 2nu ) ⋅ 0 J2u
⋅ ⋅
0 0 0
0
T
q
wu
0 J Nu 0 0 ∆ ( J Nnu ) 0 0 J Nu
=
q wnu J T
−1
0 0 ∆ ( J1u ) 0 0 J1nu 0 0
1nu
0 J T2nu 0 ∆ (J2u ) 0 J 2nu
⋅ ⋅ ⋅
0 0 0
0 0 J TNnu 0 0 ∆ ( J Nu ) 0 0 J Nnu
J1u
T
0 0 ∆ ( J1nu ) 0 0
0 T
J2u 0 ∆ ( J 2nu )
⋅ ⋅
0 0 Ι
0 0 J TNu 0 0 ∆ ( J Nnu ) Ι
⋅ ⋅ p
J1nu
T
0 0 ∆ ( J1u ) 0 0
0 J T2nu 0 ∆ (J2u ) Ι
⋅ ⋅
0 0
0 0 J TNnu 0 0 ∆ ( J Nu )
(4.44)
Operando en la expresión anterior:
Capítulo 4. Modelado cinemático sin deslizamiento 87
JT ⋅ ∆ (J ) ⋅ J 0 0 J1u
T
−1
0 0
1u 1nu 1u
0 J T2u ⋅ ∆ ( J 2 nu ) ⋅ J 2 u ⋅ 0 J T2 u
⋅
0 0
q wu
0 0 J Nu ⋅ ∆ ( J Nnu ) ⋅ J Nu 0
T
0 J TNu
= T
q wnu J1nu ⋅ ∆ ( J1u ) ⋅ J1nu
−1
0 0 J1nu
T
0 0
T
0 J 2 nu ⋅ ∆ ( J 2u ) ⋅ J 2nu ⋅ 0 J T
2nu
⋅
0 0
0 0 J Nnu ⋅ ∆ ( J Nu ) ⋅ J Nnu 0
T
0 J TNnu
∆ ( J1nu ) 0 0
0 ∆ ( J 2 nu )
⋅
0 Ι
0 0 ∆ ( J Nnu ) Ι
⋅ ⋅ p
∆ ( J1u ) 0 0
0 ∆ (J2u ) Ι
⋅
0
0 0 ∆ ( J Nu )
(4.45)
J T ⋅ ∆ ( J ) ⋅ J −1 ⋅ J T ⋅ ∆ ( J )
( 1u 1nu 1u ) 1u 1nu 0
−1
0 (J T
2u ⋅ ∆ ( J 2 nu ) ⋅ J 2 u ) ⋅ J ⋅ ∆ ( J 2nu )
T
2u
0 0
q wu
=
q
wnu ( J T ⋅ ∆ ( J ) ⋅ J )−1 ⋅ J T ⋅ ∆ ( J ) 0
1nu 1u 1nu 1nu 1u
−1
0 (J T
2 nu ⋅ ∆ ( J 2u ) ⋅ J 2 nu ) ⋅ J T2 nu ⋅ ∆ ( J 2 u )
0 0 (4.46)
0
0
−1
Ι
( J TNu ⋅ ∆ ( J Nnu ) ⋅ J Nu ) ⋅ J TNu ⋅ ∆ ( J Nnu ) Ι
⋅ ⋅ p
0
Ι
0
−1
( J Nnu ⋅ ∆ ( J Nu ) ⋅ J Nnu ) ⋅ J Nnu ⋅ ∆ ( J Nu )
T T
88 Capítulo 4. Modelado cinemático sin deslizamiento
( J T ⋅ ∆ ( J ) ⋅ J )−1 ⋅ J T ⋅ ∆ ( J )
1u 1nu 1u 1u 1nu
T −1
( J 2u ⋅ ∆ ( J 2nu ) ⋅ J 2u ) ⋅ J 2u ⋅ ∆ ( J 2nu )
T
(JT ⋅ ∆ (J ) ⋅ J ) ⋅ JT ⋅ ∆ (J )
−1
q wu Nu
Nnu Nu Nu Nnu
⋅ p
=
(4.47)
q wnu
−1
( J1nu ⋅ ∆ ( J1u ) ⋅ J1nu ) ⋅ J1nu ⋅ ∆ ( J1u )
T T
T −1
( J 2nu ⋅ ∆ ( J 2u ) ⋅ J 2nu ) ⋅ J 2nu ⋅ ∆ ( J 2u )
T
T −1
( J Nnu ⋅ ∆ ( J Nu ) ⋅ J Nnu ) ⋅ J Nnu ⋅ ∆ ( J Nu )
T
Para ello se multiplica la matriz (sin invertir) del primer miembro de (4.48)
por la del segundo miembro y se comprueba que el resultado es la identidad,
como se muestra en (4.49), por tanto una es inversa de la otra.
• A continuación se demostrará:
−1
A nxn 0 A −1 0
= −1 (4.50)
Cmxn Dmxm −1
−D ⋅ C ⋅ A D−1
Para ello se multiplica la matriz (sin invertir) del primer miembro de (4.50)
por la del segundo miembro y se comprueba que el resultado es la identidad,
como se muestra en (4.51), por tanto una es inversa de la otra.
Capítulo 4. Modelado cinemático sin deslizamiento 89
A 0 A−1 0 Ιn 0 Ιn 0
⋅ −1 −1
= −1 −1 = (4.51)
C D −D ⋅ C ⋅ A
−1
D C⋅ A − C⋅ A Ιm 0 Ιm
• A continuación se demostrará:
−1 A −1 + A −1 ⋅ B ⋅ ( D − C ⋅ A −1 ⋅ B )−1 ⋅ C ⋅ A −1 − A −1 ⋅ B ⋅ ( D − C ⋅ A −1 ⋅ B )
−1
A nxn B nxm (4.52)
=
C mxn D mxm −1 −1
− ( D − C ⋅ A −1 ⋅ B ) ⋅ C ⋅ A −1 ( D − C ⋅ A −1
⋅ B )
A B A 0 Ιn A−1 ⋅ B
= ⋅ (4.53)
C D C Ι m 0 ( D − C ⋅ A ⋅ B )
−1
−1
A B
−1
Ιn A −1 ⋅ B A 0 −1
= ⋅ =
C D 0 ( D − C ⋅ A−1 ⋅ B ) C Ιm
Ι − A −1 ⋅ B ⋅ ( D − C ⋅ A −1 ⋅ B ) A −1
−1
n ⋅ 0
(4.54)
−1 −C ⋅ A −1 =
0 ( D − C ⋅ A −1
⋅ B )
Ιm
A −1 + A −1 ⋅ B ⋅ ( D − C ⋅ A −1 ⋅ B )−1 ⋅ C ⋅ A −1 − A −1 ⋅ B ⋅ ( D − C ⋅ A −1 ⋅ B )
−1
−1 −1
− ( D − C ⋅ A −1 ⋅ B ) ⋅ C ⋅ A −1 ( D − C ⋅ A −1
⋅ B )
• A continuación se demostrará:
−1 ( A − B ⋅ D−1 ⋅ C )
−1 −1
− ( A − B ⋅ D −1 ⋅ C ) ⋅ B ⋅ D −1
A nxn B nxm (4.55)
=
C mxn D mxm −1 −1 −1
−D ⋅ C ⋅ ( A − B ⋅ D ⋅ C )
−1
D −1 + D −1 ⋅ C ⋅ ( A − B ⋅ D −1 ⋅ C ) ⋅ B ⋅ D −1
−1 −1
A B (A − B ⋅ D −1 ⋅ C) 0 Ιn B −1
= ⋅ ⋅ =
C D D−1 ⋅ C Ι m 0 D
(A − B ⋅ D−1 ⋅ C)−1 0 Ι n
⋅
− B ⋅ D−1
= (4.57)
− D −1 ⋅ C ⋅ (A − B ⋅ D −1 ⋅ C)−1
Ι m 0 D −1
(A − B ⋅ D−1 ⋅ C)−1 −1
− (A − B ⋅ D −1 ⋅ C) ⋅ B ⋅ D −1
− D −1 ⋅ C ⋅ (A − B ⋅ D −1 ⋅ C)−1 −1
D −1 + D −1 ⋅ C ⋅ (A − B ⋅ D −1 ⋅ C) ⋅ B ⋅ D −1
En primer lugar Jinu es una matriz nula, para el caso de las tres variables
accionadas, por lo que según (4.17):
∆ ( J inu ) = −Ι (4.60)
Capítulo 4. Modelado cinemático sin deslizamiento 91
(Ι ⋅(Ι − J
T
ns
−1
) )
⋅ ( J Tns ⋅ J ns ) ⋅ J Tns ⋅ Ι = − ( Ι T ⋅ ∆ ( J ns ) ⋅ Ι )
−1
(4.63)
−1
(D − C ⋅ A −1 −1
(
⋅ B ) = J Tns ⋅ J ns − J Tns ⋅ Ι ⋅ ( Ι T ⋅ Ι ) ⋅ Ι T ⋅ J ns
−1
) =
−1
(J ⋅ ( Ι − Ι ⋅ ( Ι
T
ns
T −1
⋅ Ι ) ⋅ Ι T ⋅ J ns ) ) (
= − J Tns ⋅ ∆ ( Ι ) ⋅ J ns )
−1
− ( Ι T ⋅∆ ( J ns ) ⋅ Ι )
−1
− ( Ι T ⋅∆ ( J ns ) ⋅ Ι ) ⋅ Ι T ⋅ J ns ⋅ ( J Tns ⋅ J ns ) Ι T
−1 −1
p ⋅
= ⋅ J ⋅ q
q wns − J Tns ⋅∆ ( Ι ) ⋅ J ns ⋅ J Tns ⋅ Ι ⋅ ( Ι T ⋅ Ι )
(
−1
)
−1
(
− J Tns ⋅∆ ( Ι ) ⋅ J ns
−1
) −J Tns s ws
(4.64)
q =
ns ) (
Ι T ⋅∆ ( J ) ⋅ Ι −1 ⋅ Ι T ⋅ J ⋅ J T ⋅ J −1 ⋅ J T − Ι
p ( ns ( ns ns ) ns ) ⋅ J ⋅ q =
( Ι T ⋅∆ ( J ) ⋅ Ι )−1 ⋅ Ι T ⋅∆ ( J )
ns ns
⋅ J ⋅ q
−1
( −1
wns − J Tns ⋅∆ ( Ι ) ⋅ J ns ⋅ J Tns ⋅ Ι ⋅ ( Ι T ⋅ Ι ) ⋅ Ι T − Ι
( ) ) s ws
− J T ⋅∆ ( Ι ) ⋅ J −1 ⋅ J T ⋅∆ ( Ι ) s ws
( ns )
ns ns
(4.65)
Así pues, sustituyendo en la primera fila de (4.65) las matrices por sus
correspondientes valores, teniendo en cuenta (4.30), se tiene:
−1
∆ ( J1ns ) 0 0 Ι
0 ∆ ( J 2 ns ) ⋅ Ι ⋅(Ι Ι … Ι)⋅
p = ( Ι Ι … Ι ) ⋅
0
0 0 ∆ ( J Nns ) Ι
∆ ( J1ns ) 0 0 J1s 0 0
⋅
0 ∆ ( J 2 ns ) ⋅ 0 J 2s
⋅ q ws
0 0
0 0 ∆ ( J Nns ) 0 0 J Ns
−1
p = ( ∆(J1ns ) + ∆(J 2 ns ) + + ∆( J Nns ) ) ⋅ ( ∆(J1ns ) ⋅ J1s ∆( J 2ns ) ⋅ J 2s ∆(J Nns ) ⋅ J Ns ) ⋅ q ws
(4.67)
Se parte de (4.19):
(
H = dim ( q w ) − rango Ι , J − rango Ι ) (4.68)
rango Ι = 3 (4.70)
( ) ( )
H = 3 − rango Ι , J − rango [ J ] = 3 − rango J , Ι − rango [ J ] = G (4.71)
(
H = G = rango [ J ] − rango J , Ι − rango Ι ) (4.72)
= rango [ J ] + rango Ι − rango J , Ι
donde βo/βc son vectores con los ángulos de todas las ruedas orientables/castor.
Desde el punto de vista del control cinemático una velocidad puede cambiar
de instantáneamente, pero para cumplir el supuesto anterior y conseguir los
grados de libertad ampliados, surge la limitación (al no poderse dar valores
infinitos a β o ) de que las variables de velocidad q cambien de forma continua.
2
Ey3 Ey2
β3 Ex2 l12
3 Ry ,
,
Ex3 l3 Rx l12
, Ey1 ,
1
Ex1
Figura 4.6- Asignación de sistemas de coordenadas en el vehículo triciclo
vx 0 0
ϕ1
v r l ⋅ ω = J1 ⋅ q w1
= − −
y 12
ω 0
1
vx 0 0
v = − r l ⋅ ϕ2 = J ⋅ q (4.77)
y 12 2 w2
ω 0 1 ω
vx r ⋅ s β 3 − l3
ϕ3
v y = − r ⋅ c β 3 0 ⋅ ω = J 3 ⋅ q w3
ω 0
1
1 0 0 0 0 0 vx 0
0 1 l12 r 0 0 vy 0
1 0 0 0 0 0 ω 0
⋅ = → A ⋅ q = 0 (4.78)
0 1 −l12 0 r 0 ϕ1 0
1 0 l3 0 0 − r ⋅ s β 3 ϕ2 0
0 1 0 0 0 r ⋅ c β 3 ϕ3 0
0 0
−r ⋅ c β 3 −l ⋅ cot( β )
v
y r ⋅ s β l v x − tan( β 3 ) l 3 vx 3 3
3 3
ω v −l12 + l 3 ⋅ cot( β 3 )
ω l 3 ⋅ c β 3 − l12 ⋅ s β 3 − l 3 + l12 ⋅ tan( β 3 ) y
ϕ1 ϕ1 l3 ⋅ r ϕ1 r
M ⋅η
= l ⋅ c β + l ⋅ s β ⋅η = = l12 + l 3 ⋅ cot( β 3 ) ⋅ η
ϕ2 3 3 12 3 2 3 12 ⋅ tan ( β 3 )
ϕ − l − l ϕ
2 r
ϕ3 l3 ϕ3 l3 ⋅ r ϕ3
l3
vx 1 v
− sec( β ) r ω
y 3
r ⋅ s β3
0 1
1
0 0
−r ⋅ l ⋅ c β 0
r ⋅ l 3 ⋅ c β 3 3 3
− r ⋅ c β
vx
3
vx −l3 ⋅ c β3 + L ⋅ s β 3 v x l 3 ⋅ c β 3 + l12 ⋅ s β 3
v v v r ⋅ s β3
y r ⋅ s β3 y r ⋅ s β3 y
l
ω l 3 ⋅ c β 3 − l12 ⋅ s β 3 ω l 3 ⋅ c β 3 + l12 ⋅ s β 3 ω 3
= ⋅η = ⋅η = l 3 ⋅ c β 3 − l12 ⋅ s β 3 ⋅ η
ϕ2 l 3 ⋅ c β 3 + l12 ⋅ s β 3 ϕ1 l 3 ⋅ c β 3 − l12 ⋅ s β 3 ϕ1 l3
ϕ3 l 3 ⋅ c β 3 − l12 ⋅ s β 3 ϕ3 l 3 ⋅ c β 3 + l12 ⋅ s β 3 ϕ 2
l 3 ⋅ c β 3 + l12 ⋅ s β 3
ϕ1 l3 ϕ2 l3 ϕ3
l3 (4.80)
l 3 ⋅ c β 3 − l12 ⋅ s β 3 l 3 ⋅ c β 3 + l12 ⋅ s β 3
1
1 1
Solución 0 0
− tan( β ) l
Directa v
x 3 3 v x − l 3 ⋅ cot( β 3 )
ω − l + l ⋅ tan( β ) v
3 12 3
y − l12 + l3 ⋅ cot( β 3 )
ϕ1 = l3 ⋅ r ⋅ vy ϕ1 = r
⋅ω
ϕ 2 − l 3 − l12 ⋅ tan ( β 3 ) ϕ2 l12 + l3 ⋅ cot( β 3 )
ϕ l3 ⋅ r r Solución
3 ϕ3
− sec( β ) r l3 r ⋅ s β 3 Inversa
3 (4.81)
Capítulo 4. Modelado cinemático sin deslizamiento 99
⋅ ⋅
ω 3 ⋅ l12
2
+ 4 ⋅ l 32 3 ⋅ l12 −3 ⋅ l12 ⋅ c β 3 − 4 ⋅ l3 ⋅ s β 3 ϕ3
(3 ⋅ l − 2 ⋅ l ) r − 2 ⋅ l + 6 ⋅ l ⋅ c β r − 6 ⋅ l ⋅ l ⋅ s β r
ϕ2
2
12
2
3 ( 3 12 ) 3
2 2
3 12 3
⋅ ⋅
ω 3 ⋅ l12
2
+ 4 ⋅ l32 −3 ⋅ l12 3 ⋅ l12 ⋅ c β 3 − 4 ⋅ l3 ⋅ s β 3 ϕ3
(3 ⋅ l2 − 2 ⋅ l 2 ) r − 2 ⋅ l 2 + 6 ⋅ l 2 ⋅ c β r + 6 ⋅ l ⋅ l ⋅ s β r
ϕ1 12 3 ( 3 12 ) 3 3 12 3
vx −l12 ⋅ c β 3 ⋅ ( l 3 ⋅ c β 3 + l12 ⋅ s β 3 )
v 2
( l + 3 ⋅ l12 ) ⋅ c β 3 − l 3 ⋅ l12 ⋅ c β 3 ⋅ s β 3 + 2 ⋅ l12 ⋅ s β 3
2 2 2 2
y= −r
⋅ 3
ω 6 ⋅ l12 + l 3 ⋅ (1 + c(2 ⋅ β 3 ) )
2 2
3 ⋅ l12 − l 3 ⋅ s(2 ⋅ β 3 ) 2
ϕ3 − (l 2
+ 3 ⋅ 2
l12 ) ⋅ c β 3 r + 3 ⋅ l 3 ⋅ l12 ⋅ s β 3 r
3
l12 ⋅ c β 3 ⋅ ( l 3 ⋅ c β 3 − l12 ⋅ s β 3 )
( l + 3 ⋅ l ) ⋅ c β3 + l3 ⋅ l12 ⋅ c β3 ⋅ s β3 + 2 ⋅ l ⋅ s β3 ⋅ ϕ1
2
3
2
12
2 2
12
2
ϕ
−3 ⋅ l12 − l 3 ⋅ s(2 ⋅ β 3 ) 2 2
−(l 32 + 3 ⋅ l12
2
) ⋅ c β 3 r − 3 ⋅ l 3 ⋅ l12 ⋅ s β 3 r
vx r ⋅ s(2 ⋅ β 3 ) 2 ⋅ r ⋅ l3 ⋅ c 2 β 3
Solución
ϕ1 = −1 5 + c(2 ⋅ β 3 ) l12 ⋅ ( 5 + c(2 ⋅ β 3 ) ) v y Inversa
⋅ ⋅
ϕ2 ( 5 + c(2 ⋅ β 3 ) ) ⋅ r 5 + c(2 ⋅ β 3 ) −l12 ⋅ ( 5 + c(2 ⋅ β 3 ) ) ω
Ampliada
ϕ3 6 ⋅ c β3 −4 ⋅ l 3 ⋅ s β 3
(4.82)
100 Capítulo 4. Modelado cinemático sin deslizamiento
vy (
− atan l 3 ⋅ ω v y ) si v y > 0
ω → β3 =
π − atan l3 ⋅ ω v y( ) si v y < 0
En (4.84) se indica como queda (4.82) al sustituir (en caso de que se consiga
cierta simplificación) el valor de β3 de (4.83).
Capítulo 4. Modelado cinemático sin deslizamiento 101
⋅ ⋅
ω 3 ⋅ l12
2
+ 4 ⋅ l 23 3 ⋅ l12 −3 ⋅ l12 ⋅ c β 3 − 4 ⋅ l 3 ⋅ s β 3 ϕ3
(3 ⋅ l 2 − 2 ⋅ l 2 ) r − 2 ⋅ l 2 + 6 ⋅ l 2 ⋅ c β r − 6 ⋅ l ⋅ l ⋅ s β r
ϕ2 12 3 ( 3 12 ) 3 3 12 3
vx 0
ϕ v + l ⋅ω
1 = −1 ⋅ y 12
ϕ2 r v y − l12 ⋅ ω
2 2 2
ϕ3 v y + l3 ⋅ ω
(4.84)
102 Capítulo 4. Modelado cinemático sin deslizamiento
• El modelado del vehículo tipo triciclo ilustra los métodos descritos en los
apartados anteriores y verifica los paralelismos y conexiones entre ellos.
5.1 INTRODUCCIÓN
Por ejemplo, [Saha et al. 95] establece las condiciones de isotropía para un
vehículo genérico con ruedas suecas, y las particulariza para el caso de tres y
cuatro ruedas. Mientras que en [Low et al. 05] [Kim2 et al. 04] [Kim et al. 05] se
estudian las condiciones de isotropía para un vehículo omnidireccional con tres
ruedas castor.
∆ ( J1 )
∆ (J2 )
∆(J)⋅ Ι = = Kr (5.2)
∆ (JN )
Además K r ⋅ p = 0 son las ecuaciones de restricción al movimiento, como se
desprende del árbol de la Figura 4.3.
Las restricciones que aporta cada rueda vienen dadas por ∆ ( J i ) y las
aportadas por todas por Kr.
ri ⋅ sβi li ⋅ sα i
−2
∆ ( J f/o i ) = ∆ − ri ⋅ cβi −li ⋅ cαi = ⋅
2
0 1 2 + li ⋅ (1 − c(2 ⋅ ( βi − α i )))
(5.3)
c 2 βi c βi ⋅ s βi li ⋅ c βi ⋅ s( βi − α i )
c β ⋅ s β s 2
β li ⋅ s βi ⋅ s( βi − αi )
i i i
l ⋅ c β ⋅ s( β − α ) l ⋅ s β ⋅ s( β − α ) l 2 ⋅ s2 ( β − α )
i i i i i i i i i i i
d i ⋅ c βi ri ⋅ s( β i + δi ) li ⋅ s α i + d i ⋅ cβ i 0 0 0
π
∆ ( J c i ) = ∆ d i ⋅ s βi − ri ⋅ c( β i + δi ) − li ⋅ c α i + d i ⋅ s β i = 0 0 0 ∀ δi ≠ ± (5.4)
0 0 0 0 2
0 1
Por tanto las ruedas castor y suecas, siempre y cuando no sean singulares
(ver Capítulo 6), no aportan filas linealmente independientes a Kr.
Capítulo 5. Modelos cinemáticos e isotropía 107
Teniendo en cuenta (5.3), (5.4) y (5.5), la matriz Kr se puede reducir sin perder
generalidad a una matriz Kr* de dimensión ((Nf +No) x 3), donde Nf y No son el
número de ruedas fijas y orientables respectivamente, cumpliéndose:
K *r ⋅ p = 0 (5.7)
De hecho, este espacio (E.c.f.) establece las posibles ubicaciones del Centro
Instantáneo de Rotación (CIR) del vehículo, pudiendo ser:
• Si el vehículo tiene más de una rueda fija todas ellas tienen un mismo eje
común. Esto se deriva directamente de la condición a).
Por ejemplo, los vehículos de tipo 1 (3,0) y tipo 5 (1,2) tienen la misma
maniobrabilidad desde un punto de vista amplio, pero el primero de forma
instantánea y el segundo tras la reorientación de sus ruedas orientables.
110 Capítulo 5. Modelos cinemáticos e isotropía
La demostración se va realizar para tres ruedas del tipo fijo u orientables (misma
matriz Jacobiana de rueda), que son las que restringen el movimiento. En caso de
tener más ruedas la demostración se repetiría para cada terna de ruedas.
Puesto que cada rueda aporta una ecuación linealmente independiente a Kr*,
se coge (por ejemplo) la primera fila de (5.3), resultando Kr*:
c 2 β1 c β1 ⋅ s β1 l1 ⋅ c β1 ⋅ s( β1 − α1 )
2
2 + l1 ⋅ (1 − c(2 ⋅ ( β1 − α1 ))) 2 + l12 ⋅ (1 − c(2 ⋅ ( β1 − α1 ))) 2 + l12 ⋅ (1 − c(2 ⋅ ( β1 − α1 )))
c2 β2 c β2 ⋅ s β2 l 2 ⋅ c β 2 ⋅ s( β 2 − α 2 )
K *r = −2 ⋅ 2
(5.17)
2 + l 2 ⋅ (1 − c(2 ⋅ ( β 2 − α 2 ))) 2 + l 22 ⋅ (1 − c(2 ⋅ ( β 2 − α 2 ))) 2
2 + l 2 ⋅ (1 − c(2 ⋅ ( β 2 − α 2 )))
c2 β3 c β3 ⋅ s β3 l 3 ⋅ c β 3 ⋅ s( β 3 − α 3 )
2 + l 2 ⋅ (1 − c(2 ⋅ ( β − α ))) 2 + l 32 ⋅ (1 − c(2 ⋅ ( β 3 − α 3 ))) 2 + l 3 ⋅ (1 − c(2 ⋅ ( β 3 − α 3 )))
2
3 3 3
c β1 s β1 l1 ⋅ s( β1 − α1 )
a) K *
= 0 → c β 2 s β 2 l 2 ⋅ s( β 2 − α 2 ) = 0
rango K *r = 2 → r
(5.18)
c β 3 s β 3 l 3 ⋅ s( β 3 − α 3 )
b) Restricciones de las 3 ruedas NO coincidentes
x
(cβ i sβi ) ⋅ = − li ⋅ s ( βi − α i ) (5.19)
−y
El corte de los ejes de las tres ruedas viene dado por la ecuación matricial:
cβ1 sβ1 l1 ⋅ s ( β1 − α1 )
cβ sβ ⋅ x = − l ⋅ s ( β − α ) (5.20)
2 2
− y 2 2 2
c
3β sβ 3 l ⋅ s ( β − α 3 )
3 3
Por tanto, si se cumple (5.18) los ejes de las tres ruedas se cortan en un
punto (solución única en (5.20)) ó son paralelos si β1 = β 2 = β 3 (pero no los tres
coincidentes).
Así pues, si rango el rango de la matriz Kr* es 2 los ejes de tres ruedas fijas u
orientables se cortan en un único punto (incluyendo el infinito).
Puesto que cada rueda aporta una ecuación linealmente independiente a Kr*,
se coge (por ejemplo) la primera fila de (5.3), resultando Kr*:
112 Capítulo 5. Modelos cinemáticos e isotropía
c 2 β1 c β1 ⋅ s β1 l1 ⋅ c β1 ⋅ s( β1 − α1 )
2
2 + l1 ⋅ (1 − c(2 ⋅ ( β1 − α1 ))) 2 + l12 ⋅ (1 − c(2 ⋅ ( β1 − α1 ))) 2 + l12 ⋅ (1 − c(2 ⋅ ( β1 − α1 )))
K *r = −2 ⋅ (5.23)
c2 β2 c β2 ⋅ s β2 l 2 ⋅ c β 2 ⋅ s( β 2 − α 2 )
2
2 + l 2 ⋅ (1 − c(2 ⋅ ( β 2 − α 2 ))) 2 + l 22 ⋅ (1 − c(2 ⋅ ( β 2 − α 2 ))) 2 + l 22 ⋅ (1 − c(2 ⋅ ( β 2 − α 2 )))
−2 ⋅ c β1 c β2 c β s β1
K *r12 = ⋅ ⋅ 1 =0
2 + l12 ⋅ (1 − c(2 ⋅ ( β1 − α1 ))) 2 + l 22 ⋅ (1 − c(2 ⋅ ( β 2 − α 2 ))) c β 2 s β 2
−2 ⋅ c β1 c β2 c β l ⋅ s( β1 − α1 )
K *r13 = ⋅ ⋅ 1 1 =0 (5.24)
2 + l ⋅ (1 − c(2 ⋅ ( β1 − α1 ))) 2 + l 2 ⋅ (1 − c(2 ⋅ ( β 2 − α 2 ))) β 2 l 2 ⋅ s( β 2 − α 2 )
2
1
2 c
−2 ⋅ c β1 c β2 s β l ⋅ s( β1 − α1 )
K *r 23 = ⋅ ⋅ 1 1 =0
2 + l12 ⋅ (1 − c(2 ⋅ ( β1 − α1 ))) 2 + l 22 ⋅ (1 − c(2 ⋅ ( β 2 − α 2 ))) s β 2 l 2 ⋅ s( β 2 − α 2 )
cβ1 sβ1 x l1 ⋅ s ( β1 − α1 )
cβ sβ ⋅ − y = − l ⋅ s β − α (5.25)
2 2 2 ( 2 2 )
Por tanto, si se cumple (5.24) los ejes de las dos ruedas son coincidentes ó
linealmente dependientes.
Así pues si el rango de la matriz Kr* es 1, los ejes de dos ruedas fijas u
orientables coinciden.
En este caso propiamente no se puede hablar del corte de los ejes de las
ruedas fijas y orientables, pero sí se puede decir que el CIR puede situarse en
cualquier parte del espacio bidimensional, al no haber restricciones al
movimiento.
Capítulo 5. Modelos cinemáticos e isotropía 113
Η⋅x = y (5.27)
κ = H 2 ⋅ H† 2
= σ max σ min → κ ∈ [1, ∞[ (5.28)
donde σmax y σmin son los valores singulares más grande y más pequeño de la
matriz rectangular H.
q na_ns Bna_ns
q = B ⋅ η (5.30)
na_s na_s
q a Ba
Un posible criterio de diseño, para el segundo caso, podría ser calcular los
parámetros del vehículo (radios de ruedas, distancias entre ruedas, etc.) para
minimizar el error medio de las condiciones de isotropía (o la media del número
de condición) a lo largo del error de configuración. Esto conduce a un concepto
de isotropía global.
116 Capítulo 5. Modelos cinemáticos e isotropía
σ min ( H ( parámetros, x0 ) )
Isotropía Global = min (5.32)
x0 , x1 ∈ W σ max ( H ( parámetros, x1 ) )
Este vehículo tiene movilidad completa ruedas (m=3), por lo que puede moverse
en cada instante en cualquier dirección sin necesitar reorientación, de ahí que se
le llame vehículo omnidireccional.
Está construido sin ruedas fijas ni orientables, es decir que utilizaría ruedas
omnidireccionales (castor o suecas). En este estudio se considerarán las dos
opciones más habituales: tres ruedas suecas y tres ruedas castor.
Dado que hay tres GDLs instantáneos se necesitan al menos tres actuadores
118 Capítulo 5. Modelos cinemáticos e isotropía
/sensores para poder actuar/sensorizar sobre todos ellos. Por tanto no hay más
opción que actuar/sensorizar las tres variables de rotación de las ruedas suecas.
A p ⋅ p + A w ⋅ ϕ = 0 (5.33)
con
T
ϕ = (ϕ1 ϕ2 ϕ3 ) Aw = r ⋅ s γ ⋅ I
( 1
c β + γ ) ( 1 ) l1 ⋅ s (β1 + γ − α1 )
s β + γ (5.34)
A p = c (β 2 + γ ) s ( β 2 + γ ) l 2 ⋅ s (β 2 + γ − α 2 )
c ( β + γ ) s ( β + γ ) l ⋅ s (β + γ − α )
3 3 3 3 3
p − A w
ϕ = A ⋅ η (5.35)
p
p = − r ⋅ s γ ⋅ A-1p ⋅ ϕ
−1 (5.36)
ϕ = − ( r ⋅ s γ ) ⋅ A p ⋅ p
0 3 − 3 ϕ1
r
p = ⋅ −2 1 1 ⋅ ϕ2
3 − 1 L − 1 L − 1 L ϕ
3
(5.37)
ϕ1 1 0 −1 −L
ϕ2 = ⋅ 3 2 0.5 −L ⋅ p
ϕ r
3 − 3 2 0.5 −L
Figura 5.1- Vehículo omnidireccional (tipo 1) isotrópico con tres ruedas suecas
Desde un punto de vista práctico, suele ser preferible actuar sobre las
velocidades de rotación de las ruedas castor puesto que normalmente necesitan
menores pares motor que los requeridos por las velocidades de dirección.
Además, la tracción del vehículo es mejor sobre la superficie de trabajo al actuar
sobre la rotación de las ruedas.
β
Ap ⋅ p + Aw ⋅ = 0 (5.38)
ϕ
con
T
β = ( β1 β2 β3 )
T
ϕ = (ϕ1 ϕ2 ϕ3 )
cβ1 sβ1 l1 ⋅ s ( β1 − α1 ) − d
−d ⋅ I 0
Aw = Ap1 = cβ2 sβ2 l2 ⋅ s ( β2 − α2 ) − d
r ⋅ I
(5.39)
0
cβ3 sβ3 l3 ⋅ s ( β3 − α3 ) − d
− s β1 cβ1 l1 ⋅ c ( β1 − α1 )
A
Ap = p1 Ap2 = − s β2 cβ2 l2 ⋅ c ( β2 − α2 )
Ap2
− s β3 cβ3 l3 ⋅ c ( β3 − α3 )
p I
β
= (1 d ) ⋅ A p1 ⋅ η (5.40)
ϕ − (1 r ) ⋅ A
p2
A−p11 A−p21
p p
= d ⋅ Ap2 −1 ⋅ β = −r ⋅ Ap1 −1 ⋅ ϕ
ϕ − ⋅ Ap1 β
⋅ Ap2
r d (5.41)
β 1 d ⋅ Ap1
= −1 r ⋅ A ⋅ p
ϕ p2
En [Low et al. 05] se indica que la condición de isotropía para los modelos
directo e inverso anteriores se alcanza con los ejes de direccionamiento en los
vértices de un triángulo equilátero y con d = r. Sin embargo, este resultado de
isotropía sólo es válido si se desprecia la distancia d del brazo de dirección
respecto a la distancia entre el centroide del triángulo y el eje de dirección. En
caso contrario las condiciones de isotropía dependen de los ángulos de dirección.
Tanto [Low et al. 05] como [Kim2 et al. 04]-[Kim et al. 05] consideran
ocasionalmente más de tres velocidades asignadas, ej. articulaciones accionadas,
lo que no es lógico dentro del marco de modelos sin deslizamiento de (4.2).
β1
L
-β 2 r
-β3
Este vehículo tiene una rueda fija independiente y otras posibles ruedas
omnidireccionales, resultando una movilidad m=2. Este estudio considera dos
ruedas fijas, numeradas 1 y 2, con un eje de rotación común (el clásico
mecanismo diferencial) más una rueda omnidireccional, numerada 3, bien sueca
o castor. Por lo que surgen dos posibilidades.
Como excepción, para este vehículo no se asumirá el mismo radio para las
tres ruedas en las ecuaciones del modelo.
Capítulo 5. Modelos cinemáticos e isotropía 123
Para este vehículo, mostrado en la Figura 5.3, hay que utilizar dos veces la
ecuación (3.11) y una vez la (3.14), por lo que (4.1) resulta:
T
A p ⋅ p + A w ⋅ ϕ = A p ⋅ p + A w ⋅ (ϕ1 ϕ2 ϕ3 ) = 0 (5.42)
con
0 0 0 1 0 0
r1 0 0 0 1 l12
Aw = 0 0 0 Ap = 1 0 0 (5.43)
0 0 1 −l12
r2 0
0 0 r3 − sβ 3 c β 3 l3 ⋅ c ( β 3 − α 3 )
ϕ3 β3
l3
α3
ϕ1
ϕ2
l12 l12
0 0
−l12 −l12
−1 1
p = 2 ⋅ l12 r1 0
(5.44)
ϕ ⋅η
0 2 ⋅ l12 r2
l12 ⋅ c β 3 + l3 ⋅ c ( β3 − α 3 ) l12 ⋅ c β 3 − l3 ⋅ c ( β 3 − α 3 )
r3 r3
0 0
ϕ
p = − r1 2 − r2 2 ⋅ 1 (5.45)
ϕ2
− r1 ( 2 ⋅ l12 ) r2 ( 2 ⋅ l12 )
vx 0 0
ϕ1 = −1 r1 − l12 r1 ⋅ v y (5.46)
ϕ −1 r l r ω
2 2 12 2
−1 r1 − l12 r1 ⋅ vy = ϕ1
−1 r l r ϕ
2 12 2 ω 2
(5.47)
−1 r1 − l12 r1 vy ϕ1
− cβ r −l ⋅ c ( β − α ) r ⋅ = ϕ
3 3 3 3 3 3 ω 3
−1 r1 − l12 ( r1 ⋅ D1 ) v y ϕ1
−1 r l ( r ⋅ D ) ⋅ D ⋅ ω = ϕ
2 12 2 1 1 2
(5.48)
−1 r1 − l12 ( r1 ⋅ D 2 ) v y ϕ1
− c β r −l ⋅ c ( β − α ) ( r ⋅ D ) ⋅ D ⋅ ω = ϕ
3 3 3 3 3 3 2 2 3
l12
2
1 r1 + 1 r2
2
⋅ (1 r12 − 1 r22 )
D1 (5.49)
= k1 ⋅ I
l12 ⋅ 1 r 2 − 1 r 2 l122 2
D ( 1 2) ( )
2
⋅ 1 r1 + 1 r2
1 D12
1 cβ3
2
l12 l ⋅ c β3 ⋅ c ( β3 − α 3 )
2
+ + 3
r1 r3 D 2 ⋅ r12 r32 ⋅ D 2 (5.50)
2
= k2 ⋅ I
l12 l3 ⋅ c (β3 − α3 )
l12 + l3 ⋅ cβ3 ⋅ c ( β3 − α 3 )
2
D ⋅ r2 2 2 +
2 1 r32 ⋅ D 2 D 2 ⋅ r1 r3 ⋅ D 2
Capítulo 5. Modelos cinemáticos e isotropía 125
r1 = r2 D 1 = l12 (5.51)
Esta condición de isotropía significa, ver Figura 5.4 (a), que la componente
X (respecto al sistema R) del vector de distancia entre el origen de R y la línea
definida por el plano de la rueda sueca, es igual a la constante negativa –C1.
2
C1 = l12 ⋅ ( r3 r1 ) ϕ3
ϕ3
ϕ1 ϕ1
C1
Para este vehículo, mostrado en la Figura 5.5, hay que utilizar dos veces la
ecuación (3.11) y una vez la (3.12), por lo que (4.1) resulta:
T
(
A p ⋅ p + A w ⋅ q w = A p ⋅ p + A w ⋅ ϕ1 ϕ2 ϕ3 β3 ) = 0 (5.53)
con
0 0 0 0 1 0 0
r1 0 0 0 0 1 l12
0 0 0 0 1 0 0
Aw = Ap = (5.54)
0 r2 0 0 0 1 −l12
0 0 r3 0 −sβ 3 cβ 3 l3 ⋅ c ( β 3 − α 3 )
0 −d 3 cβ sβ l ⋅ s ( β − α ) − d
0 0 3 3 3 3 3 3
ϕ3
β3
l3
α3
ϕ1
ϕ2
l12 l12
p 0 −l12 −1 2 ⋅ l12 r1
T
0 a1 r3 − a3 d + 1
=
q 0 −l ⋅η (5.55)
w 12 1 0 2 ⋅ l12 r2 a2 r3 − a4 d −1
con
a1 = l12 ⋅ c β 3 + l3 ⋅ c ( β 3 − α 3 ) a2 = l12 ⋅ c β 3 − l3 ⋅ c ( β 3 − α 3 )
(5.56)
a3 = l12 ⋅ s β 3 + l3 ⋅ s ( β 3 − α 3 ) a4 = l12 ⋅ s β 3 − l3 ⋅ s ( β 3 − α 3 )
Para ese caso, las relaciones de (5.47) y las condiciones de isotropía (5.51) y
(5.52) son válidas. Pero ahora, dado que el ángulo de dirección β 3 es variable, la
isotropía de (5.52) se alcanza para una configuración particular, Figura 5.6 (a).
−1 r1 − l12 ( D4 ⋅ r1 )
ϕ1 vy
β = 3 3 ⋅ s ( β3 − α3 ) − d ⋅ D ⋅ ω
s β l
3 4
d d ⋅ D4
− c β3 −l3 ⋅ c ( β3 − α3 )
ϕ3 r3
r3 ⋅ D5
⋅ y
v
β = s β l3 ⋅ s ( β3 − α3 ) − d D5 ⋅ ω
3 3 (5.57)
d
d ⋅ D5
a4 + d d ⋅ a2
2 ⋅ l12 ⋅ r3 ⋅ r ⋅ a 2 ⋅ l12 ⋅
r1 ⋅ a5 ϕ3
ϕ1 = 1 5
⋅
ϕ d − a3 d ⋅ a1 β3
2
2 ⋅ l12 ⋅ r3 ⋅ r ⋅ a −2 ⋅ l12 ⋅ r ⋅ a
2 5 2 5
con a5 = 2 ⋅ l12 ⋅ ( l3 ⋅ s β3 + d ⋅ c β3 )
( d r3 )2 ⋅ c β 3 ⋅ l3 ⋅ c ( β 3 − α 3 ) = − s β 3 ⋅ ( l3 ⋅ s ( β 3 − α 3 ) − d ) (5.59)
128 Capítulo 5. Modelos cinemáticos e isotropía
las cuales, asumiendo el mismo radio r par alas tres ruedas y sustituyendo los
valores de a1, a2, a3, y a4, de (5.56), resultan:
( l3 ⋅ c ( β 3 − α 3 ) ) ⋅ ( d − l3 ⋅ s ( β 3 − α 3 )) = ( l12 ⋅ s β 3 ) ⋅ ( l12 ⋅ c β 3 )
→ c5 ⋅ c6 = c3 ⋅ c4
(5.61)
( l12 ⋅ s β3 ) + ( d − l 3 ⋅ s ( β 3 − α 3 ) ) = ( d 2 r 2 ) ⋅
2 2
ϕ3 β3
3
Ry ϕ1 ϕ1
2 C1 Rx C2
1
c1 = c2 ⋅ d 2 r32 β3 3
3
ϕ3 c5
Ry
1 2 c3 1
c2 c1 c6
2 Rx c4
(c) q a = (ϕ3 β3 )T q na_s = ( v y ω )T (d) q a = (ϕ1 ϕ2 )T q na_s = (ϕ3 β3 )T
Figura 5.6- Condiciones de isotropía para el vehículo diferencial con rueda castor
Capítulo 5. Modelos cinemáticos e isotropía 129
Las variables anteriores c3, c4, c5, y c6 se han representado en la Figura 9 (d).
La primera condición de isotropía de (5.61) indica la equivalencia del área de dos
rectángulos, mientras que la segunda no tiene un significado gráfico evidente.
Notar que para alcanzar simultáneamente las dos condiciones de isotropía, una
constante además de la variable de configuración β 3 (ej. α 3 ) se debe diseñar
apropiadamente.
Este vehículo tiene una rueda orientable independiente y otras posibles ruedas
omnidireccionales. El grado de movilidad, y por tanto el número de velocidades
que se pueden asignar independientemente, es dos. Esto se debe a tener una
ecuación de restricción. Además hay un GDL aparente aportado por la
reorientación de la rueda orientable.
con
0 0 0 cβ1 sβ1 0
r 0 0 −sβ1 cβ1 0
Aw = 1 Ap =
l 2 ⋅ c (β 2 − α 2 )
(5.63)
0 r2 0 − sβ 2 c β2
0 0 r3 − sβ 3 c β3 l3 ⋅ c (β3 − α 3 )
β3 β2
l3 l2
α2
π − α3
β1
Figura 5.7- Vehículo con una rueda orientable (tipo 3) y dos ruedas suecas
sβ1 0
−cβ1 0
p = 0 1
⋅ η (5.64)
ϕ
1 r1 0
c ( β1 − β2 ) r2 −l 2 ⋅ c ( β 2 − α 2 ) r2
c ( β − β ) r −l ⋅ c ( β − α ) r
1 3 3 3 3 3 3
mostrado para el vehículo de tipo 2, cada una de esas seis relaciones da lugar,
aplicando (5.29), a una condición de isotropía.
c ( β1 − β2 ) ⋅ c (β2 − α2 ) = 0
(5.65)
c ( β1 − β3 ) ⋅ l3 ⋅ c ( β3 − α3 ) = − ( r32 r22 ) ⋅ c ( β1 − β2 ) ⋅ l2 ⋅ c (β2 − α2 )
3 ϕ3 2 ϕ2
ϕ2 E1
ϕ1
c8
E1
c7 1
c7 = c8 ⋅ r32 r22
(a) q a = (ϕ1 ϕ2 )T (b) q a = (ϕ2 ϕ3 )T c7 = c8·(r3/ r2)2
Figura 5.8- Configuraciones isotrópicas para el vehículo tipo 3 con ruedas suecas
ϕ T
(
A p ⋅ p + A w ⋅ = A p ⋅ p + A w ⋅ ϕ1 ϕ2 ϕ3 β2 β3 ) = 0 (5.66)
β
con
cβ1 sβ1 0
− s β1 cβ1 0
0 0 −s β cβ 2 l2 ⋅ c ( β2 − α 2 )
Aw = r ⋅ I 0 Ap = 2 (5.67)
0 −d ⋅ I − s β3 cβ 3 l3 ⋅ c ( β 3 − α 3 )
cβ 2 sβ 2 l 2 ⋅ s ( β 2 − α 2 ) − d
cβ sβ3 l3 ⋅ s ( β 3 − α 3 ) − d
3
β3 β2
d3 d2
l3 l2
α2
π − α3
β1
Figura 5.9- Vehículo con una rueda orientable (tipo 3) y dos ruedas castor
sβ1 0
− c β1 0
0 1
p
ϕ = 1 r 0 ⋅ η
cβ12 r −l 2 ⋅ c β22 r (5.68)
β cβ13 r −l 3 ⋅ c β33 r
sβ d l ⋅ s β d − 1
12 2 22
sβ13 d l3 ⋅ s β33 d − 1
con: β12 = β1 − β2 , β13 = β1 − β3 , β22 = β2 − α 2 , β33 = β3 − α3
q a1 → cβ12 ⋅ c β22 = 0
q a2 → sβ12 ⋅ ( d − l2 ⋅ s β22 ) = 0
q a3 → l2 ⋅ c β22 ⋅ cβ12 = −l3 ⋅ c β33 ⋅ cβ13
(5.70)
q a4 → sβ12 ⋅ ( d − l2 ⋅ s β22 ) = − s β13 ⋅ ( d − l3 ⋅ s β33 )
q a5 → l2 ⋅ c β22 ⋅ cβ12 = − ( r d ) ⋅ sβ12 ⋅ ( d − l2 ⋅ s β22 )
2
2
q a6 → l2 ⋅ c β22 ⋅ cβ12 = − ( r d ) ⋅ sβ13 ⋅ ( d − l3 ⋅ s β33 )
ϕ2 β2
ϕ1 ϕ1
(a) q a1 (b) q a2
ϕ3 ϕ2 β2
-β3
c10 c12
c11 c13
c13
c10
(e) q a5 c10 = c12·(r/d)2 (f) q a6 c10 = c13·(r/d)2
Figura 5.10- Configuraciones isotrópicas para el vehículo tipo 3 con ruedas castor
Las anteriores configuraciones isotrópicas siempre existen.
Capítulo 5. Modelos cinemáticos e isotropía 135
Este vehículo tiene una rueda orientable independiente y otra rueda fija
independiente. En este estudio se considerará una rueda orientable (no. 1), una
rueda fija (no. 2) y una tercera rueda (no. 3): fija dependiente, sueca o castor.
Este vehículo tiene un grado de movilidad (m=1), por lo que no tiene sentido
aplicarle el análisis de isotropía ya que, dentro del marco cinemático sin
deslizamiento de (4.1), sólo una velocidad es libremente asignada, resultado
cualquier relación entre velocidades escalar. Además hay otro GDL aparente
aportado por la dirección de la rueda orientable.
Este vehículo clásico consiste en una rueda orientable y dos ruedas fijas
simétricas. Este vehículo se muestra en la Figura 5.11, donde el origen del
sistema R se ha situado en el punto medio del eje de rotación de las ruedas fijas,
y su eje X coincidente con este eje de rotación.
1 0 0
1 0 0
cβ sβ −l ⋅ c β ϕ1
0
Ap ⋅ p + Aw ⋅ ϕ = 3 3 3 3
⋅ p + r ⋅ ⋅ ϕ2 = 0 (5.71)
0 1 l12 I ϕ
0 1 −l12 3
− s β cβ l ⋅ s β
3 3 3 3
ϕ3 β3
l3
ϕ1
ϕ2
l12 l12
Figura 5.11- Vehículo triciclo (una rueda orientable y dos fijas dependientes)
0
l3 ⋅ cβ3
p = sβ3 (5.72)
− ( L ⋅ sβ + l ⋅ cβ ) r ⋅η
ϕ 3 3 3
( L ⋅ sβ3 − l3 ⋅ cβ3 ) r
−l 3 r
Este vehículo, mostrado en la Figura 5.12 (a), tiene una rueda fija alineada con
una rueda orientable, y otra rueda sueca. El sistema de coordenadas R se ha
situado en el centro de la rueda fija y con su eje X concidente con el eje de
rotación de dicha rueda.
Capítulo 5. Modelos cinemáticos e isotropía 137
0
l 2 ⋅ cβ 2
sβ 2
p = − l 2 ⋅ cβ 2 r
(5.74)
ϕ ⋅η
−l 2 r
1 l 3 ⋅ c ( β 3 − α 3 ) ⋅ sβ 2
− ⋅
r + l 2 ⋅ cβ 2 ⋅ cβ 3
ϕ2 β2 ϕ2 β2
ϕ3 β 3
ϕ3 β3
l2 l2
l3 l3
α3 α3
ϕ1 ϕ1
Este vehículo, mostrado en la Figura 5.12 (b), tiene una rueda fija alineada con
una rueda orientable, y otra rueda sueca. El sistema de coordenadas R se ha
situado como en el subapartado 5.5.5.B).
138 Capítulo 5. Modelos cinemáticos e isotropía
1 0 0
cβ2 sβ2 −l2 ⋅ c β2 ϕ1
0 1 0 0 ϕ
Ap ⋅ p + Aw ⋅ q w = ⋅ p + r ⋅ I ⋅ 2 = 0 (5.75)
− s β2 cβ2 l 2 ⋅ s β2 −d ϕ3
− s β3 cβ3 l3 ⋅ c ( β3 − α3 ) β3
cβ
sβ3 l3 ⋅ s ( β3 − α3 ) − d
3
0
l 2 ⋅ cβ 2
sβ 2
p − l 2 ⋅ cβ 2 r
ϕ = −l 2 r ⋅η (5.76)
1 l 3 ⋅ c ( β 3 − α 3 ) ⋅ sβ 2
β
3 − ⋅
r + l 2 ⋅ cβ 2 ⋅ cβ 3
1 ( l 3 ⋅ s ( β 3 − α 3 ) − d ) ⋅ sβ 2
⋅ + l ⋅ cβ ⋅ sβ
d 2 2 3
ϕ3 ϕ3 β3
β3
l3
l3
β2 α3 ϕ1 β1 β2 α3 ϕ1 β1
ϕ2 ϕ 2
l12 l12 l12 l12
Para el modelado de este vehículo con rueda sueca, ver Figura 5.13 (a), la
ecuación (3.11) se ha de utilizar dos veces y la (3.14) una, resultando (4.1):
Mientras que para el modelado del vehículo con rueda castor, ver la Figura
5.13 (b), la ecuación (3.11) se ha de utilizar dos veces y (3.12) una, resultando:
−2 ⋅ l12 ⋅ sβ1 ⋅ sβ 2
l12 ⋅ s ( β1 + β 2 )
s ( β 2 − β1 )
p = −2 ⋅ l12 ⋅ sβ 2 r
(5.79)
ϕ −2 ⋅ l12 ⋅ sβ1 r ⋅η
s ( β 2 − β1 ) ⋅ l 3 ⋅ c ( β 3 − α 3 )
1
− ⋅ +2 ⋅ l12 ⋅ sβ1 ⋅ sβ 2 ⋅ sβ 3
r +l ⋅ s ( β + β ) ⋅ cβ
12 1 2 3
−2 ⋅ l12 ⋅ sβ1 ⋅ sβ 2
l12 ⋅ s ( β1 + β 2 )
s ( β 2 − β1 )
−2 ⋅ l12 ⋅ sβ 2 r
p − 2 ⋅ l12 ⋅ sβ 1 r
ϕ = 2 ⋅ l12 ⋅ sβ1 ⋅ sβ 2 ⋅ sβ 3 ⋅η (5.80)
− 1 ⋅ + l ⋅ s ( β + β ) ⋅ cβ
β3 r
12 1 2 3
+s ( β 2 − β1 ) ⋅ l 3 ⋅ c ( β 3 − α 3 )
−2 ⋅ l12 ⋅ sβ1 ⋅ sβ 2 ⋅ cβ 3
1 ⋅ + l ⋅ s ( β + β ) ⋅ sβ
d 12 1 2 3
+s ( β 2 − β1 ) ⋅ ( l 3 ⋅ s ( β 3 − α 3 ) − d )
• Se han obtenido los modelos cinemáticos para cada tipo de vehículo, los
cuales son genéricos e incluyen variables de rueda, a diferencia de [Campion
et al. 96] que utiliza variables sin sentido físico.
6.1 INTRODUCCIÓN
p p
c( βci + δi ) s( βci + δi ) lci ⋅ s( βci + δi −αci ) −di ⋅ cδi −di ⋅ cδi 0 Fβci −di ⋅ cδi 0 (6.3)
vslip ci = ⋅ βci = ⋅ βci
−s( βci + δi ) c( βci + δi ) lci ⋅ c( βci + δi −αci ) + di ⋅ sδi di ⋅ sδi rci ϕ Fϕci di ⋅ sδi rci ϕ
ci ci
p p
vslip six = ( c (βsi + γi ) s ( βsi + γi ) lsi ⋅ s ( βsi + γi − αsi ) rsi ⋅ sγsi ) ⋅ = ( Fsi rsi ⋅ sγsi ) ⋅ (6.4)
ϕsi ϕsi
Ef 0 0 0 0 0
E p
0 0 0 0 0
o ϕ f
Ff rf 0 0 0 0
ϕ o
vslip = Fo 0 ro 0 0 0 ⋅
ϕ s
Fs 0 0 rs 0 0 (6.5)
β
Fβ c 0 0 0 dβ 0 c
ϕ
F
ϕc 0 0 0 dϕ rc c
E 0 p
v slip = ⋅ = A ⋅ q
F r q w
146 Capítulo 6. Singularidad de los modelos cinemáticos
donde {Ef, Eo, Ff, Fo, Fs, Fβc, Fφc} son matrices obtenidas por la combinación de
los vectores fila {Efi, Eoi, Ffi, Foi, Fsi, Fβci, Fφci}; {rf, ro, rs, rc, dβ, dφ} son matrices
diagonales con los elementos en la diagonal {rfi, roi, rsi ·sγsi, rci, -di·cδi, di·sδi};
{ϕ f , ϕ o , ϕ s , β c , ϕ c } son vectores compuestos de los elementos {ϕfi ,ϕoi ,ϕsi , βci ,ϕci } ;
q w es el vector de todas las velocidades de rueda; q el vector de todas la
velocidades; y vslip es el vector de todas las velocidades de deslizamiento.
A ⋅ q = 0 (6.6)
q ∈ N(A) → q = B ⋅ η (6.7)
q na = B na ⋅ η
q B (6.9)
a a
if Ba ≠ 0 → q na = B na ⋅ Ba−1 ⋅ q a (6.10)
A na ⋅ q na = − Aa ⋅ q a (6.11)
Incompatibilidad cinemática:
E 0
A na = Fa 0 (6.13)
F r
na na
donde la segunda fila matricial es para las velocidades asignadas y la tercera fila
matricial para las velocidades no asignadas.
Las condiciones de (6.14) significan que hay alguna rueda sueca singular
(rodillos con orientación paralela al plano de la rueda) o alguna rueda castor
singular (plano de la rueda perpendicular al brazo de dirección).
E
G = Fa (6.15)
Fna_sg
G 0
A na = (6.16)
F r
na_nsg na_nsg
donde Fna_sg es una matriz de tres columnas con los vectores fila Fsi y Fβ ci de las
150 Capítulo 6. Singularidad de los modelos cinemáticos
λ = (λ x λ y dλ ) (6.18)
1
λy
dλ λx
λ x ⋅ y − λ y ⋅ x + d λ = (λ x λ y d λ ) ⋅ ( y − x 1)T = 0 (6.19)
Capítulo 6. Singularidad de los modelos cinemáticos 151
λ xa λ ya d λa
λ xb λ yb d λb = 0 (6.20)
λ xc λ yc d λc
La condición (6.20) implica que las tres líneas (6.19) son paralelas o
intersectan en un punto común. Como extrapolación, cuando se consideran más
de tres vectores/rectas y no hay ningún conjunto de tres vectores fila
independientes, todas las rectas son paralelas o intersectan en un punto común.
En nuestro caso, los vectores {Efi, Eoi, Ffi, Foi, Fsi, Fβci, Fφci} (definidos en
(6.1),(6.2), (6.3) y (6.4)) representan con el significado de (6.18), las siguientes
rectas respecto al sistema de coordenadas R:
- Efi / Eoi: eje de rotación de la rueda fija / orientable i;
- Ffi / Foi / Fφci: recta definida por el plano de la rueda fija / orientable / castor i;
- Fsi: eje de rotación del rodillo de la rueda sueca i;
- Fβci: eje de rotación de la rueda castor i;
λ ya dλa
= λya ⋅ d λ b − λ yb ⋅ d λ a = 0 (6.21)
λ yb d λb
lo que significa que las rectas asociadas a los vectores fila de la matriz G son
paralelas al eje X del sistema de coordenadas R o intersectan en dicho eje.
Si las dos velocidades lineales del vehículo son asignadas, sólo la tercera
columna de G se considera para la degeneración:
lo que significa que las rectas asociadas con los vectores fila de la matriz G
intersectan en el origen del sistema de coordenadas R.
lo que significa que las rectas asociadas los vectores fila de G son paralelas.
lo que implica que todas las rectas son paralelas al eje X o Y de sistema R.
Capítulo 6. Singularidad de los modelos cinemáticos 153
Para los parámetros de las ruedas castor y suecas se utilizarán los valores
clásicos: {γ = 90º, δ = 0º}.
Este tipo de vehículo tiene movilidad completa (m=3) y no tiene ruedas fijas ni
orientables, es decir está construido con ruedas suecas o castor. Para este estudio
se considera el mismo tipo de rueda, muy común en la práctica. Por lo que hay
dos opciones: tres ruedas suecas o tres ruedas castor.
Este tipo de vehículo tiene una rueda fija y otras posibles ruedas
omnidireccionales (suecas o castor), resultando una movilidad de dos (m=2).
Aquí se considerarán dos ruedas fijas con un eje de rotación común (el clásico
mecanismo diferencial) y otra rueda omnidireccional, bien sueca o castor.
3 3
Ry
Rx
2 1 2 1
(a) q a1 = ( v x ω )T (b) q a2 = ( v y ω )T
(d) q a9 = ( βc2 βc3 )T (e) q a10 = (ϕc2 βc2 )T (f) q a11 = (ϕc 2 βc3 )T
22
3
22 22
3 3
1 1
2 1
2 1
La rueda doble y la castor doble (Figura 3.5) pueden considerarse como dos
ruedas singulares castor y dos ruedas castor, respectivamente, con mismo eje de
rotación Lxi y ángulo de dirección βi . Utilizando esta equivalencia, el modelado
cinemático y la caracterización de la singularidad anteriores siguen siendo
validos para este tipo de ruedas.
Por otro lado, las ruedas tipo bola (Figura 3.8) y ortogonal (Figuras 3.9 y
3.10) no han sido modeladas, pero dado que ambas son funcionalmente
equivalentes a las ruedas suecas, se pueden incluir en la caracterización de la
singularidad del punto 6.4 añadiendo la siguiente condición:
7.1 INTRODUCCIÓN
Muchas de las publicaciones que tratan el tema del modelado cinemático asumen
no-deslizamiento [Campion et al. 96] [Rajagopalan 97] [Low et al. 05] [Yi et al.
02] o acusan una falta de sentido físico en los modelos con deslizamiento [Muir
et al. 87] [Alexander et al. 89] [Kim et al. 04] [Tham et al. 98].
ωR = EiωEi − βi
R
(7.1)
Ffric_b = − f ( vb ) ⋅ vb vb (7.4)
dPdis_b dPdis_b d vb df ( vb ) vb
= ⋅ = f ( vb ) + vb ⋅ ⋅ v = 0 (7.6)
dvb d vb dvb d vb b
Con este objeto el punto 7.2 empieza con el modelado dinámico del
vehículo, al que posteriormente se le aplicarán tres aproximaciones sucesivas
(punto 7.4): el modelo de movimiento quasi-estático; el modelo cinemático con
deslizamiento y la solución de Mínimos Cuadrados ponderada.
d ∂T ∂T
− =Q (7.9)
dt ∂q ∂q
∂ri ∂v
Q = ∑ Fi i = ∑ FiT ⋅ i (7.10)
i ∂q i ∂q
N
∂ϕi No ∂β Nc
∂β N ∂v slip i
Q = ∑ τ ri ⋅ + ∑ τ osi ⋅ oi + ∑τ csi ⋅ ci + ∑ Ffric
T
i ⋅ (7.11)
i =1 ∂q i =1
∂q i =1
∂q i =1
∂q
R p
v slip 1 E1 v E1 A p1 A w1 0
q w1
v slip = = = ⋅
v A wN (7.12)
slip N EN v EN A pN 0
q wN
p
v slip = ( A p A w ) ⋅ = R A ⋅ R q
q w
G p
v slip = ( A p ⋅ Rot Z ( − Gθ R ) A w ) ⋅ = G A ⋅ G q (7.13)
q w
G
p =
( R ) 00 ⋅ R p = Rot ( Gθ ) ⋅ R p
Rot Gθ
(7.14)
Z R
0 0 1
cos ( x ) − sin ( x )
Rot ( x ) = (7.15)
sin ( x ) cos ( x )
con las propiedades Rot −1 ( x ) = Rot T ( x ) = Rot ( − x ) .
170 Capítulo 7. Modelado cinemático con deslizamiento
Notar que (7.17) tiene en cuenta que el centro de masas (CM) de todas las
ruedas centradas (fijas, orientables y castor) no cambia respecto a la estructura
del vehículo.
Capítulo 7. Modelado cinemático con deslizamiento 171
G
v cmi = G v R + f1i ⋅ ω + f2i ⋅ βi (7.19)
G
v cmi = G v R + f3i ⋅ ω + f4i ⋅ βi + f5i ⋅ ω 2 + f6i ⋅ βi2 + f7i ⋅ 2 ⋅ ω ⋅ βi (7.20)
Nc Nc Nc
IT + ∑ Icsi ⋅ ω + ∑ M ci ⋅ f1i ⋅ v cmi + ∑ Icsi ⋅ β ci
T G
i =1 i =1 i =1
0 (7.22)
No N
Rot ( Rθ Ei ) ⋅ Ffric i
+ ∑ Iosi ⋅ βoi = ∑ R d Ei × i 0
i =1 i =1 0 1
0
F
Icsj ⋅ ω + βcj + Mcj ⋅ f2Tj ⋅ G v cmj = τ csj − Ej dSj × fric j i 0
( ) (7.23)
0 1
va va va va
(a) Coulomb (b) Coulomb + viscosa (c) Ídem b. + estática (d) todos
Figura 7.1- Fenómenos estáticos en la fricción
Tanto los modelos estáticos como dinámicos pueden asumir la rueda con
fuerza de fricción puntual ó distribuida (ver Figura 7.2). Normalmente los
modelos distribuidos se suelen aproximar a modelos puntuales realizando un
promedio que depende de la distribución considerada para la fuerza de reacción
normal en la rueda.
µa µa max
1
0.8
Coeficiente de adhesión normalizado
0.6
0.4
0.2 Coulomb
0 Aprox. con alta k1
Aprox. con media k1
-0.2
Aprox. con baja k1
-0.4
-0.6
-0.8
-1
-1 -0.5 0 0.5
vslip 1
Velocidad de deslizamiento
( µ x µ x 0 )2 + ( µ y µ y 0 )2 ≤ 1 (7.29)
T
* *
µ = (µ µ ) = * T (µ 2
x0 ⋅ vslip x µ y20 ⋅ vslip y )
(7.30)
x y T
(µ x0 ⋅ vslip x µ y 0 ⋅ vslip y )
que está en el límite de la elipse de fricción dada por (7.29) (ver Figura 7.3).
µy
µ y0
µx
µ x0
Figura 7.4- Coeficientes admisibles para el caso bidimensional de Coulomb
s=
(ϕ ⋅ r − v ) =
wheel y v slip
(7.31)
max ( ϕ ⋅ r , v ) max
wheel y ( ⋅r , v
ϕ wheel y )
donde vwheel y es la velocidad longitudinal del centro de la rueda.
Capítulo 7. Modelado cinemático con deslizamiento 177
Por ejemplo, [Dugoff et al. 70] propuso la curva mostrada en la Figura 7.5 y
verificó su ajuste cualitativo para diferentes superficies y velocidades de rueda
vwheel y. Este modelo ha sido ampliamente utilizado para análisis de estabilidad en
la dinámica de vehículos [Matsumoto et al. 92] y para el control de tracción [Tan
et al. 91]. En concreto, el método estándar de diseño de sistemas ABS consiste en
una ley de control que fuerce el funcionamiento en (o cerca) el máximo del
coeficiente de fricción µ y max .
0.9
µ y max 1
µ y 0.8
Coeficiente de adhesión longitudinal
0.4
0.3
0.2 µ y max 3
Hielo / Alta wheel
0.1
0
0 0.2 0.4 0.6 0.8 1
Ratio de deslizamiento longitudinal s
Figura 7.5- Curva propuesta en [Dugoff et al. 70] para el coeficiente de fricción
en distintas condiciones de velocidad y adherencia
µ y = k2 ⋅ (1 − ( k3 s ) ⋅ (1 − e− s k ))
3
(7.32)
µ y = k 4 ⋅ s ( k5 ⋅ s 2 + k 6 ⋅ s + 1 ) (7.33)
0.8
µx 0.6
Coeficiente de adhesión lateral
0.4
0.2
-0.2
-0.4
-0.6
-0.8
-30 -20 -10 0 10 20 30
αs Ángulo de deslizamiento (grados)
Figura 7.6- Curva estática para el coeficiente de fricción lateral vs. ángulo de
deslizamiento
f (
x, x , x, u ) = 0 →
x (7.36)
f ( 0, x ∞ , x, u ) = 0 → x ∞ (7.37)
G
v R = Rot ( Gθ R ) ⋅ R v R + ω ⋅ ( ∂Rot ( Gθ R ) ∂ Gθ R ) ⋅ R v R (7.40)
i =1 i =1
N
Rot ( Rθ Ei ) ⋅ Ffric i
0 = ∑ R d Ei ×
T
i( 0 0 1) (7.42)
i =1 0
F T
(7.43)
0 = τ csj − Ej dSj × fric j i( 0 0 1)
0
0 = τ osj (7.44)
0 = τ rj + r j ⋅ ( s γ j c γ j ) ⋅ Ffric j (7.45)
T T
( Ej
dSjx Ej
d Sjy 0 ) × ( Ffric
T
j 0) =0 (7.47)
De igual forma, para una rueda j sin par de rotación (rotación libre) (7.45)
resulta (7.48), con el significado de que no hay fuerza de fricción en la dirección
definida por el plano de la rueda.
(s γ j c γ j ) ⋅ Ffric j = 0 (7.48)
Por otro lado, [Alexander et al. 89] define dos tipos de deslizamiento:
µ1 0 0
0 µ2 ⋅ A ⋅ q = µ ⋅ A ⋅ q = 0
0
0 0 (7.50)
µN
µ 0
with µ i = xi
0 µ yi
µ na ⋅ Ana ⋅ q na = − µ a ⋅ Aa ⋅ q a (7.51)
∂J LS N
= ∑ Rot ( Rθ Ei ) ⋅ µ i ⋅ v slip i = 0 (7.54)
∂ R vR i =1
0
∂J LS N
Rot ( Rθ Ei ) ⋅ µi ⋅ v slip i
= ∑ R d Ei × 0 = 0
i (7.55)
∂ω i =1 0 1
∂J LS µ ⋅ v T
(7.56)
= − Ei dSj × j slip j i( 0 0 1) = 0
∂β cj 0
∂J LS
= r j ⋅ ( s γ j c γ j ) ⋅ µ j ⋅ v slip j = 0 (7.57)
∂ϕ j
∂J LS
= rrj ⋅ ( 0 1) ⋅ µ j ⋅ v slip j = 0 (7.58)
∂ϕ rj
µ 0 vslip xi
Ffric i = − xi ⋅ = −µ i ⋅ v slip i (7.59)
0 µ yi vslip xi
Con objeto de probar los modelos con deslizamiento del punto anterior, se
utilizará la carretilla industrial de la Figura 7.7. Este vehículo representa un
vehículo próximo al mundo real.
Los modelos con deslizamiento se utilizarán como una solución directa para
estimar el movimiento del vehículo, con la información de sensores dada por tres
encoders que miden la rotación de las tres ruedas fijas (encoders incrementales
con 8290 pulsos por vuelta de la rueda) y la orientación de la rueda orientable
(encoder absoluto de 1.41º de resolución).
188 Capítulo 7. Modelado cinemático con deslizamiento
l 3 = 1.35m r = 0.23m
ϕ 2
l12
lCM
l12 = 0.45m
ϕ3
β3
ϕ1
Figura 7.8- Vista de planta de la carretilla industrial con la representación
esquemática tipo triciclo
1 0 0 0 0 0
0 1 l12 r 0 0 R p
v slip 1
1 0 0 0 0 0 ϕ1
v slip = v slip 2 = ⋅ (7.60)
0 1 − l12 0 r 0 ϕ2
v slip 3 cβ 3 sβ 3 l3 ⋅ c β 3 0 0 0 ϕ3
−sβ cβ − l ⋅ s β 0 0 r
3 3 3 3
Por tanto las ecuaciones del modelo con deslizamiento vienen dadas por:
0 = Ffric x1 + Ffric x 2 + c β 3 ⋅ Ffric x 3
0 = Ffric y1 + Ffric y 2 + s β 3 ⋅ Ffric x 3 (7.63)
0 = l12 ⋅ ( Ffric y1 − Ffric y 2 ) + l 3 ⋅ c β 3 ⋅ Ffric x 3
Además para todos los modelos anteriores {(7.61), (7.62), (7.63)} se tiene
que incluir (7.49) tres veces debido a las fuerzas de fricción en cada rueda.
2 ⋅ µ x12 0 0 0 0
µ ⋅r ϕ
0 µ y12 µ y12 ⋅ l12 R 0
⋅ p = − y12
⋅ 1
0 µ y12 − µ y12 ⋅ l12 0 µ y12 ⋅ r ϕ2 (7.65)
x 3 ⋅ cβ 3
µ µ x 3 ⋅ sβ 3 µ x 3 ⋅ l 3 ⋅ c β 3 0 0
ϕ
Aµna ⋅ R p = − Aµa ⋅ 1
ϕ
2
donde ( µ x12 , µ y12 ) son los coeficientes de fricción de las identicas ruedas fijas y
µ x 3 es el coeficiente de fricción de la rueda orientable en la dirección Lx3.
190 Capítulo 7. Modelado cinemático con deslizamiento
ϕ
p = − ( ATµna ⋅ Aµna ) ⋅ AµTna ⋅ Aµa ⋅ 1
−1
R
(7.66)
ϕ2
Para calcular la expresión anterior se tienen que conocer los tres coeficientes
de fricción o dos relaciones entre ellos a través de las constantes k11 y k12:
7.5.2 Estimación del vector de velocidad del vehículo con filtro de Kalman
R
p k +1 = R p k + T ⋅ R p
k (7.68)
ϕ1 ⋅ r 0 −1 − l12
ϕ2 ⋅ r = 0 −1 l ⋅ R p k → y1k = C1k ⋅ R p k (7.69)
0 cβ sβ l ⋅ 12
k 3 3 3 c β 3 k
La ecuación de salida (7.69) tiene tres elementos, uno para cada sensor,
obtenidos de la cinemática sin deslizamiento de (7.60). Notar que no se ha
despejado β 3 en la tercera ecuación escalar de (7.69) porque obligaría a trabajar
con el filtro de Kalman extendido (FKE) que utiliza una aproximación de primer
orden. Dicha aproximación no es necesaria con el planteamiento de (7.69).
0 1 0 0
ϕ1 ⋅ r 0 −1 −l12 R
ϕ ⋅ r = 0 −1 ⋅ p k → y 2 k = C2 k ⋅ R p k (7.70)
l12
2
0 k cβ 3 sβ 3 l 3 ⋅ c β 3 k
R
pˆ k +1 k = R pˆ k k (7.71)
Pk +1 k = Pk k + Q k (7.72)
−1
K k +1 = Pk +1 k ⋅ CTk +1 ⋅ ( Ck +1 ⋅ Pk +1 k ⋅ CTk +1 + R k +1 ) (7.73)
R
pˆ k +1 k +1 = R pˆ k +1 k + K k +1 ⋅ y k +1 − Ck +1 ⋅ R pˆ k +1 k
( ) (7.74)
Pk +1 k +1 = ( Ι − K k +1 ⋅ Ck +1 ) ⋅ Pk +1 k + Q k (7.75)
donde Ck = C1k ó C2k, yk = y1k ó y2k, {Qk, Rk} son las matrices de covarianza del
ruido de proceso y la covarianza del ruido de medida, Kk es la matriz de
corrección ó de ganancia del algoritmo y Pk es la matriz de covarianza del error.
En cualquier caso, se asumirá que las matrices de covarianza del ruido en el
proceso y en la medida son constantes, es decir que no cambian según el instante.
R
p k = C1−k1 ⋅ y k (7.76)
donde los encoders de las dos ruedas fijas y las ecuaciones escalares de estado se
han supuesto igualmente fiables a través de las desviaciones típicas k14/k17 y k13,
y los errores de las variables de estado y de salida se han asumido no correlados.
Notar que no es posible utilizar la solución estacionaria del FK, que vendría
dada por la matriz de corrección estacionaria K ∞ y la ecuación (7.74), porque la
matriz de salida C1k/ C2k es variable.
Los valores de las constantes utilizados en la simulación son: MT= 2500 Kg;
IT= 1500 Kg·m2; Ir1= Ir2 = 1 Kg·m2; Ir3 = 1.5 Kg·m2; Is3 = 0.8 Kg·m2; lCM = 0.45 m;
hCM = 0.7 m. Dichos valores son sólo orientativos, ya que no se ha aplicado
ningún proceso de identificación.
0.02
Error vx
-0.02
0 200 400 600 800 1000
0.1
Error vy
0
-0.1
0.2
Error w
0
-0.2
-0.4
0 200 400 600 800 1000
Figura 7.9- Diferencia en los elementos del vector de velocidad del vehículo R p R
al calcularlos con MCD y con WLS (para k11 y k12 optimizadas)
0.05
0
vx
-0.05
4
2
vy
0
-2
0 200 400 600 800 1000
10
5
w
-5
0 200 400 600 800 1000
R
Figura 7.10- Vector de velocidad p R calculado con MCD para los 1155 puntos
0.1
0.08
Error medio en (7.63)
0.06
0.04
0.02
0
0 200 400 600 800 1000
R
Figura 7.11- Error numérico medio cometido en (7.63) al calcular p R con MCD
Capítulo 7. Modelado cinemático con deslizamiento 195
5
Coordenada Y
5.32
4
5.3
3 5.28
5.26
2
5.24
5.22
1
5.2
-2.68 -2.67 -2.66 -2.65 -2.64 -2.63 -2.62 -2.61
Coordenada X
0
-7 -6 -5 -4 -3 -2 -1 0 1
Coordenada X
Figura 7.12- Caminos obtenidos con MD (––), MQE (– –), MCD (- -) y
WLS (– · –) para pares nulos y R vRy (0) = 2 m/s RωR (0) = 0.3 rad/s
∑( R
vRx_MD (i ) − R vRx_mod (i ) + R vRy_MD (i ) − R vRy_mod (i ) + RωR_MD (i ) − RωR_mod (i ) )
J mod = i =1
(7.78)
3⋅ Nm
En la Figura 7.13 se muestra el camino obtenido con cada modelo con las
condiciones iniciales de antes y pares de rotación sobre las ruedas fijas de 30
N·m, aplicados durante 1 segundos. Para este ejemplo la orientación de la rueda
orientable se ajusta en cada iteración, con el par de dirección, para evitar la
incompatibilidad cinemática.
20
18
16
14
Coordenada Y
12
10 15.2
8 15.15
6 15.1
4 15.05
2 15
-5 -4 -3 -2 -1 0
Coordenada X
Figura 7.13- Caminos obtenidos con MD (––), MQE (– –), MCD (- -) y
WLS (– · –) con pares de rotación no nulos y R vRy (0) = 2 m/s RωR (0) = 0.3 rad/s
Los índices para este segundo caso son: JMQE = 0.33 %, JMCD = 0.63 %,
JWLS= 0.84 % .
En dicha figura se aprecia que, con los pares aplicados, se gana en velocidad
de avance y se pierde en velocidad de rotación. También se observa que el
deslizamiento en la dirección perpendicular a las ruedas fijas disminuye, lo cual
es lógico porque se pasa de un movimiento circular inicial a otro más rectilíneo.
Capítulo 7. Modelado cinemático con deslizamiento 197
VRx
0.005
R 0
0 2 4 6 8 10
Tiempo
2.2
VRy
2
R
1.8
0 2 4 6 8 10
Tiempo
0.4
WR
0.2
R
0
0 2 4 6 8 10
Tiempo
Figura 7.14- Evolución de los elementos del vector de velocidad del vehículo
R
p R según el MD para el segundo ejemplo de simulación
0 0 ϕ
R
p k = − r 2 − r 2 ⋅ 1k (7.79)
− r ( 2 ⋅ l ) r ( 2 ⋅ l ) ϕ2k
12 12
Para establecer los valores de los parámetros de WLS {k11, k12}, del FK1
{k14, k15} y del FK2 {k16, k17, k18}, se minimizará el siguiente índice de error Jk:
Ne
1 ( x∞ − x0 )2 + ( y∞ − y0 )2 θ ∞ − θ0
Jk = ⋅∑ + (7.80)
2 ⋅ N e i =1 distancia recorrida ángulo recorrido
Obviamente las fuerzas de tracción sobre las ruedas fijas las coordina la
tarjeta de control de la carretilla con la orientación de la rueda orientable.
Capítulo 7. Modelado cinemático con deslizamiento 199
60
50
40
y (m)
30
20
10
0
-40 -30 -20 -10 0 10 20 30
x (m)
Figura 7.15- Camino obtenido con WLS optimizado (––) y no optimizado (- -)
(k11 = 0.32, k12 = 2, Jk = 11.55%) al recorrer aproximadamente un rectángulo
3
Wheel Velocities (m/s)
0
0 10 20 30 40 50 60 70 80
Time (sec)
Wheel Orientation (deg)
15
10
0 10 20 30 40 50 60 70 80
Time (sec)
Figura 7.16- Medidas de velocidades en las ruedas fijas ( -r ⋅ ϕ1 ,-r ⋅ ϕ2 ) y en la
orientación β 3 de la rueda orientable en experimento del rectángulo (Fig. 7.15)
200 Capítulo 7. Modelado cinemático con deslizamiento
50
40
30
y (m)
20
10
50
45
40
35
30
y (m)
25
20
15
10
0
-30 -20 -10 0 10 20
x (m)
Figura 7.18- Camino obtenido con el FK2 optimizado (––) y no optimizado (- -)
(k16 = k17 = k18 =200, Jk =6.85%) para los datos de la Figura 7.16
Las Figuras 7.20, 7.21, 7.22 y 7.23 muestran los caminos obtenidos con
WLS, FK1, FK2 y diferencial con otros caminos seguidos. En concreto, el
quinto camino seguido (triple bucle, ver la Figura 7.23) es el más crítico porque
el ángulo recorrido (movimiento de rotación acumulado) es 3 x 360º. De hecho el
FK1 y el modelo diferencial fallan claramente en la estimación mientras que
WLS y el FK2 la realizan bastante bien.
45
0
40
35
-10
30
-20 25
y (m)
y (m)
20
-30
15
10
-40
5
-50 0
-5
-20 -10 0 10 20 30 40 -30 -20 -10 0 10 20
x (m) x (m)
0 45
40
-10
35
30
-20
25
y (m)
y (m)
-30 20
15
-40 10
5
-50
0
-5
-30 -20 -10 0 10 20 30 -30 -20 -10 0 10
x (m) x (m)
Figura 7.19- Caminos obtenidos con WLS (––), el FK1 (– –), el FK2 (- -) y el
modelo diferencial (– · –) para distintos experimentos tipo rectángulo
Capítulo 7. Modelado cinemático con deslizamiento 203
35
30
25
20
y (m)
15
10
0
0 5 10 15 20 25 30 35
x (m)
Figura 7.20- Caminos obtenidos con WLS (––), el FK1 (– –), el FK2 (- -) y el
modelo diferencial (– · –) para el segundo tipo de camino en forma de tirabuzón
35
30
25
20
y (m)
15
10
-5
-5 0 5 10 15 20 25 30 35
x (m)
Figura 7.21- Caminos obtenidos con WLS (––), el FK1 (– –), el FK2 (- -) y el
modelo diferencial (– · –) para el tercer tipo de camino en forma de D
204 Capítulo 7. Modelado cinemático con deslizamiento
20
15
10
5
y (m)
-5
-10
-15
0 5 10 15 20 25 30 35
x (m)
Figura 7.22- Caminos obtenidos con WLS (––), el FK1 (– –), el FK2 (- -) y el
modelo diferencial (– · –) para el cuarto tipo de camino en forma de ocho
25
20
15
10
5
y (m)
-5
-10
-15
-20
-25
0 10 20 30 40 50 60
x (m)
Figura 7.23- Caminos obtenido con WLS (––), el FK1 (– –), el FK2 (- -) y el
modelo diferencial (– · –) para el quinto tipo de camino en forma de triple bucle
Capítulo 7. Modelado cinemático con deslizamiento 205
( (
csy = tan (π 180 ) ⋅ 84.21 + 5.16 ⋅ e
-6.094 (1+ vwheel y )
)) ,, S = s + ( 0.0213 + 0.0086 ⋅ vwheel y ) csy
− vwheel y 8
k8 y = 1.57,, k7 y = 0.8 − 0.0021⋅ vwheel y ,, k9 y = csy (k8 y ⋅ k7 y ),, sm = 0.08 + 0.75 ⋅ e
(7.82)
k10 y = ( k9 y ⋅ sm − tan ( 0.5 ⋅ π k8 y ) ) ( k9 y ⋅ sm − atan ( k9 y ⋅ sm ) ) − 0.0162 ⋅ vwheel y
0.8
µ y 0.6
0.2
-0.2
-0.4
-0.6
-0.8
-1 -0.5 0 0.5 1
Ratio de deslizamiento longitudinal s
1
µx
0.8
0.6
Coeficiente de adhesión lateral
0.4
0.2
-0.2
-0.4
-0.6
-0.8
-1
-60 -40 -20 0 20 40 60
Ángulo del deslizamiento (grados) αs
µy
0.6
0.4
0.2
1
0.8
0
60 0.6
50 0.4
40
30 0.2
20
10 0
0 s Ratio deslizamiento
Ángulo de deslizamiento α s
0.6
0.5
0.4
0.3
0.2
0.1
0
0 10 20 30 40 50 60
Ángulo de deslizamiento (grados) αs
Figura 7.28- Proyección de las curvas de nivel del coeficiente de adhesión
longitudinal en deslizamiento combinado respecto al ángulo de deslizamiento
212 Capítulo 7. Modelado cinemático con deslizamiento
µx
0.8
0.4
0.2
0
60
40
1
20 0.8
0.6
0.4
Ángulo de deslizamiento αs 0 0.2
0
Ratio de deslizamiento s
0.8
µ x 0.7
Coeficiente de adhesión lateral
0.6
0.5
0.4
0.3
0.2
0.1
0
0 0.2 0.4 0.6 0.8 1
Ratio de deslizamiento longitudinal s
Figura 7.30- Proyección de las curvas de nivel del coeficiente de adhesión lateral
en deslizamiento combinado respecto al ratio de deslizamiento
CAPÍTULO 8
CONTROL CINEMÁTICO DE
VEHÍCULOS CON RUEDAS
8.1 INTRODUCCIÓN
Para el diseño del control existen alternativas tan diversas como las
desarrolladas en [Ollero et al. 94] [Shin 90] [O’Connor et al. 96] [Inoue et al. 97]
[Lyshevski et al. 00] [Samson 95], entre muchas otras.
que tienen alto coste, poca robustez y baja precisión, o estimadores de similares
inconvenientes.
...
ϕi ref
p ref p control βi ref Vi τi
βi ref
θ βi castor
p p ϕi s βi s β i s
dp
+ Ac ⋅ (p − p ref ) = 0 (8.4)
dt
d(p − p ref )
+ Ac ⋅ (p − p ref ) = 0 (8.5)
dt
d 2 (p − p ref ) d(p − p ref )
2
+ Ac ⋅ + Bc ⋅ (p − p ref ) = 0 (8.6)
d t dt
El modelo cinemático inverso de rueda es aquel que toma como entrada el vector
de velocidad del vehículo p , respecto al sistema R o G, y obtiene las
velocidades de la rueda q wi y la orientación βi si la rueda es orientable.
− R v x + ω ⋅ li ⋅ s α i
βi = arctan
Rv + ω ⋅ l ⋅cα (8.9)
y i i
ϕi = − (1 ri ) ⋅ ( − R v x + ω ⋅ li ⋅ s α i )2 + ( R v y + ω ⋅ li ⋅ c α i )2 (8.12)
ϕi = − (1 ri ) ⋅ ( − G vx ⋅ c θ − G v y ⋅ s θ + ω ⋅ li ⋅ s α i )2 + ( − G vx ⋅ s θ + G v y ⋅ c θ + ω ⋅ li ⋅ c αi ) 2 (8.13)
( cβi sβi li ⋅ s ( βi − α i ) )⋅ R p = 0
(8.14)
( c(θ +βi ) s(θ +βi ) li ⋅ s ( βi − αi ) ) ⋅ G p = 0
R v = R v ⋅ tan βi
βf − α f = 0 → G x G y (8.15)
v x = v y ⋅ tan (θ +βi )
R
vx ⋅ cβi + R v y ⋅ sβi G
v x ⋅ c(θ +βi ) + G v y ⋅ s(θ +βi )
ω= = = f1 (θ ) ⋅ G v x + f 2 (θ ) ⋅ G v y (8.16)
− li ⋅ s ( βi − α i ) − li ⋅ s ( βi − α i )
f 3 (θ ) ⋅ G v x + f 4 (θ ) ⋅ G v y f 3 (θ ) + f 4 (θ ) ⋅ ( G v y G
vx )
βi = arctan = arctan (8.17)
G G
f5 (θ ) ⋅ vx + f 6 (θ ) ⋅ v y f5 (θ ) + f 6 (θ ) ⋅ ( v y
G G
vx )
Notar que la cinemática inversa de esta rueda, dada por (8.24) y (8.25),
depende del ángulo del brazo de dirección, por lo que si se acciona alguna de las
dos velocidades de la rueda castor hay que sensorizar el ángulo de la dirección
βi , como se indica en la Fig. 8.1.
1
ϕi = − ⋅ ( c ( βi + γi ) s ( βi + γi ) li ⋅ s ( βi + γ i − α i ) ) ⋅ R p
ri ⋅ s γi
(8.27)
1
ϕi = − ⋅ ( c (θ +βi + γ i ) s (θ +βi + γi ) li ⋅ s ( βi + γ i − α i ) ) ⋅ G p
ri ⋅ s γ i
8.5.1 Introducción
Como se puede comprobar en la Tabla 5.1, todos los vehículos tienen una
maniobrabilidad de 3 (tipo 1, tipo 3, tipo 5) ó de 2 (tipo 2, tipo 4). En los de
maniobrabilidad completa (3) se generan referencias para los 3 elementos de la
postura del vehículo p.
Capítulo 8. Control de vehículos 227
f1 (t ) f 2 (t )
t t
Figura 8.2: Ejemplos de función: f1 no es C (continua); f2 es C pero no C1
0 0
dy ( t ) dy ( t ) dt vy
χ ( t ) = arctan = arctan = arctan (8.29)
dx ( t ) dx ( t ) dt vx
Notar que un ángulo χ continuo (C0) no implica que las posiciones lineales
de x(t) e y(t) también lo sean (ej. tramos de línea recta en la dirección del eje X o
Y). No obstante, en lo que sigue se asumirá que las referencias lineales de
posición generadas por el planificador de alto nivel son, lógicamente, continuas.
228 Capítulo 8. Control de vehículos
va ( t ) = v x2 + v y2 (8.30)
d ( χ ( t ) ) v y ⋅ v x − v y ⋅ vx
ωg ( t ) = = (8.31)
dt vx2 + v y2
1 ωg v y ⋅ v x − v y ⋅ v x
κ c (t ) = = = 3/ 2
(8.33)
ρ c (t ) va ( vx2 + v 2y )
Utilizando (8.30), (8.32) y (8.33) se obtiene la implicación:
Si χ es C1 ⇒ {ωg , va } son C0 ⇒ κ c es C0 (8.34)
Y kc (t1 ) = 1/ ρ c (t1 )
Cc
ρ c (t1 ) χ (t1 )
( x(t1 ), y (t1 ) )
X
Figura 8.3: Variables del camino en un punto con ángulo de la tangente continuo
Capítulo 8. Control de vehículos 229
En caso de que el origen del sistema R sí esté sobre el eje de las ruedas fijas
( βf − α f = 0 ) la cinemática inversa de las ruedas fijas viene dada por (8.20).
Utilizando la ligadura de (8.15) debida a las ruedas fijas y (8.29), se obtiene la
relación:
G
vx
tan (θ +βi ) = G
→ θ + βi = α χ → θ = α χ → ω = ωg (8.36)
vy
Por tanto, dentro del marco cinemático se podrán seguir caminos con ángulo
χ de la tangente continuo (C0). No obstante, se podrán seguir caminos con ángulo
discontinuo si se detiene el avance del vehículo en las discontinuidades para
reorientarse. Desde el punto de vista dinámico se podrán seguir trayectorias 2D
con ángulo χ de la tangente de derivada continua (C1)
En caso de que el origen del sistema R (punto que hace el seguimiento de las
referencias de posición lineales) no esté sobre el eje de las ruedas fijas
( βf − α f ≠ 0 ), la cinemática inversa de la rueda orientable es {(8.17),(8.18)} y la
de las fijas (8.18). Usando (8.29), dichas expresiones resultan:
f 3 (θ ) + f 4 (θ ) ⋅ χ
βi = arctan (8.38)
f 5 (θ ) + f 6 (θ ) ⋅ χ
En caso de que el origen del sistema R sí esté sobre el eje de las ruedas fijas
( βf − α f = 0 ) la cinemática inversa de la rueda orientable es {(8.19), (8.20)} y la
de las ruedas fijas (8.20). Puesto que (8.36) es válido también para este caso,
(8.20) se convierte en (8.37) y (8.19) en se reescribe como:
Capítulo 8. Control de vehículos 231
La curvatura del camino seguido por este vehículo, utilizando (8.15), es:
ωg ωg
κc = = (8.41)
G 2
v + v
x
G 2
y
f19 (θ ) ⋅ G v y
8.5.5 Vehículo tipo 3 (una rueda orientable) y tipo 5 (dos ruedas orientables)
En el caso de que haya más de una rueda orientable o haya una única rueda
orientable pero el punto de seguimiento (origen del sistema R) no esté en su
centro, es decir existe alguna rueda orientable con el parámetro li distinto de cero
( ∃ l oi ≠ 0 ), la cinemática inversa de la/s rueda/s orientable/s con l oi ≠ 0 viene
dado por (8.10) y (8.13). Según estas fórmulas y considerando el marco
cinemático, se podrán seguir referencias de los tres elementos de la postura p de
derivadas continuas (C1). Mientras que, según el punto de vista dinámico se
podrán seguir referencias de p de derivadas segundas continuas (C2).
232 Capítulo 8. Control de vehículos
c θ + χ ⋅ sθ
βi = arctan (8.44)
sθ − χ ⋅ c θ
G
vx
ϕi = − ⋅ (c θ + χ ⋅ s θ )2 + ( − s θ + χ ⋅ c θ )2 (8.45)
ri
Por lo que, dentro del marco cinemático se podrán seguir referencias con
posición angular θ y ángulo χ de la tangente continuos (C0). No obstante, se
podrán seguir caminos con ángulo χ discontinuo si se detiene el vehículo en las
discontinuidades para reorientar la rueda orientable.
l 3 = 2.35m
e = 1m
r = 0.23m
ϕ 2
l12
l12 = 0.45m
ϕ3
β3
ϕ1
Figura 8.4: Variables y parámetros de la carretilla industrial (vehículo triciclo)
l1 = e 2 + l12
2
α1 = − atan(e / l12 ) β1 = 0
l2 = e + l 2 2
12 α1 = π + atan(e / l12 ) β2 = 0 (8.46)
l3 = l3 α 3 = −π / 2
La cinemática inversa de las ruedas fijas (8.18), dada por (8.12) y (8.47), es:
1 l12 R 1 l
ϕ1 = − ⋅ ( R v y − ⋅ vx ) = − ⋅ ( − G v x ⋅ s θ + G v y ⋅ c θ − 12 ⋅ ( G v x ⋅ c θ + G v y ⋅ s θ ))
r e r e (8.48)
1 R l12 R 1 l12 G
ϕ 2 = − ⋅ ( v y + ⋅ v x ) = − ⋅ ( − vx ⋅ s θ + v y ⋅ c θ + ⋅ ( v x ⋅ c θ + G v y ⋅ s θ ))
G G
r e r e
Capítulo 8. Control de vehículos 235
(l 3 − e) R v x (l − e) G v x ⋅ c θ + G v y ⋅ s θ
β 3 = arctan
e
⋅ R = arctan 3 ⋅ G (8.49)
v y e − vx ⋅ s θ + G v y ⋅ c θ
1 (l 3 − e)2 R 2 R 2
ϕ3 = − ⋅ ⋅ vx + v y
r e2
(8.50)
1 (l − e) 2
ϕ3 = − ⋅ 3 2 ⋅ ( G v x ⋅ c θ + G v y ⋅ s θ )2 + ( − G vx ⋅ s θ + G v y ⋅ c θ ) 2
r e
1 (l 3 − e) 2 R 2 R 2
ϕ3 = − ⋅ signo( R v y ) ⋅ ⋅ vx + v y (8.51)
r e2
donde G xR ref e G yR ref son las referencias en las coordenadas lineales del sistema
global G y la dinámica en ambas coordenada viene dada por ax y ay.
-1
-2
-3
-4
-5
-6 -4 -2 0 2 4
Figura 8.5: Ejemplo 1º de seguimiento de referencia: camino seguido por la
referencia (––), origen de R (– –) y punto medio de las rueda fijas (- -)
0.6
0.4
0.2
-0.2
-0.4
-0.6
-0.8
-1
-1.2
-1.4
0 5 10 15
Figura 8.6: Orientación β3 (––), error en x (– –) y error en y (- -) para ejemplo 1º
Capítulo 8. Control de vehículos 237
-1
-2
-3
-4
-5
-6
-7
-8
-9
0 2 4 6 8 10
Figura 8.7: Ejemplo 2º de seguimiento de referencia: camino seguido por la
referencia (––), origen de R (– –) y punto medio de las rueda fijas (- -)
1.5
0.5
-0.5
-1
-1.5
0 5 10 15 20 25
Figura 8.8: Orientación β3 (––), error en x (– –) y error en y (- -) para ejemplo 2º
238 Capítulo 8. Control de vehículos
Es interesante hacer notar que el camino seguido por el punto medio de las
ruedas fijas (Fig. 8.7) tiene continuo el ángulo de la tangente, a pesar de que el
ángulo de la tangente al camino seguido por la referencia es discontinuo.
Capítulo 8. Control de vehículos 239
G
vx control = G xR ref ⋅ f aux + a x ⋅ ( G xR ref − G xR )
G
v y control = G y R ref ⋅ f aux + a y ⋅ ( G yR ref − G yR ) (8.53)
1 si ( G xR ref − G xR )2 + ( G yR ref − G yR )2 ≤ d max
con f aux =
0 si ( G xR ref − G xR )2 + ( G yR ref − G yR )2 > d max
7 7
6 6
5 5
4 4
3 3
2 2
1 1
0 0
-1 -1
-4 -3 -2 -1 0 1 -4 -3 -2 -1 0 1
-1
a) ax = ay = 1 s b) ax = ay = 0.5 s-1
Figura 8.9: Experiencias de seguimiento de trayectoria rectilínea: camino seguido
por la referencia (––), origen de R (– –) y punto medio de las rueda fijas (- -)
ax = a y = 1 s -1
0.5
-0.5
-1
0 5 10 15
Tiempo (s)
1
ax = a y = 0.5 s -1
0.5
-0.5
-1
0 5 10 15
Tiempo (s)
Figura 8.10: Orientación β3 (––), error en x (– –) y error en y (- -) en caso de recta
-4 -3 -2 -1 0 1 2 3 4
Figura 8.11: Experiencia de seguimiento de trayectoria circular: camino seguido
por la referencia (––), origen de R (– –) y punto medio de las rueda fijas (- -)
242 Capítulo 8. Control de vehículos
1.4
1.2
0.8
0.6
0.4
0.2
-0.2
-0.4
-0.6
5 10 15 20 25 30 35
Tiempo (s)
Cabe indicar que un control acoplado de los dos motores (bucle dinámico de
bajo nivel) evitaría la perturbación que se infringe uno a otro, mejorando la
respuesta conjunta.
9.1 INTRODUCCIÓN
En ese capítulo se presentan dos aplicaciones a las que poder aplicar el control
cinemático presentado en el Capítulo 8.
9.2.1.A) Introducción
Dado que los parámetros del sistema de visión que intervienen en el cálculo
(distancia focal, inclinación, etc.) son complicados de obtener se ha desarrollado
un sistema de calibración especial sencillez.
Las líneas del plano imagen se pueden obtener por técnicas tradicionales basadas
en el análisis de imagen. En este caso se aplican las siguientes:
Los píxeles de interés pueden ser los del elemento que marque la
referencia a seguir (raya en el suelo, cultivo, etc.), para lo que se
aplicaría un filtrado, o los del contorno de dicha referencia, para lo que
se aplicaría un operador gradiente. La segunda opción es más
conveniente cuando el contorno de la referencia es rectilíneo y dicha
referencia tiene una anchura apreciable. De forma que, se identificarían
las dos líneas del contorno (computacionalmente más rápido) para
después interpolarlas.
Capítulo 9. Aplicaciones 249
3º) Calcular las líneas que se ajustan más a la ubicación de los píxeles de
interés detectados.
Lo anterior es válido, sobre todo, para el caso de que los puntos de interés
sean el contorno de la referencia. En el caso de que los puntos de interés sean la
referencia, y ésta tenga una anchura apreciable, conviene calcular la celda media
de entre todas las que, siendo adyacentes, sean atravesadas por un número de
rectas elevado. De forma que, se evita abatir la línea identificada dentro de la
referencia.
Modelo de cámara
(xu , yu )
Yu
Xu
Zc
Xc
Yc
(xc , y c , z c )
f
f f
xu = ⋅ xc yu = ⋅ yc (9.1)
zc zc
xu = ( x p − C x ) ⋅ d x yu = ( y p − C y ) ⋅ d y (9.2)
donde (Cx, Cy) son las coordenadas en píxeles del centro del plano imagen y (dx ,
dy) el tamaño de los elementos sensores en cada eje.
Capítulo 9. Aplicaciones 251
Xc
Yc Zc
(t x , t y , t z )
Zw
(x c , y c , z c )
Xw
(x w , y w , z w )
−ψ
φ
Yw
xc cψ sψ 0 t x xw
yc sψ ⋅ c φ − cψ ⋅ c φ − s φ t y yw
z = sψ ⋅ s φ cψ ⋅ s φ − c φ ⋅ (9.3)
c t z zw
1 0 0 0 1 1
Con (9.1), (9.2) y (9.3) se calcula la posición (tx, ty, tz) y orientación (φ, ψ)
de la cámara respecto al suelo. En particular sustituyendo (xc, yc, zc) de (9.3) en
(9.1) se obtiene:
252 Capítulo 9. Aplicaciones
f ⋅ ( cψ ⋅ xw + sψ ⋅ yw + t x )
xu =
s φ ⋅ (sψ ⋅ xw + cψ ⋅ yw ) − c φ ⋅ zw + t z
(9.4)
f ⋅ ( c φ ⋅ (sψ ⋅ xw − cψ ⋅ yw ) − s φ ⋅ zw + t y )
yu =
s φ ⋅ (sψ ⋅ xw + cψ ⋅ yw ) − c φ ⋅ zw + t z
En general, en (9.4) hay 5 incógnitas {tx, ty, tz, φ, ψ} (se ha hecho nulo el
ángulo de vasculación), mientras que cada punto (xui, yui) de la imagen, cuyas
coordenadas (xwi, ywi, zwi) se conozcan, aporta 2 ecuaciones. Así pues, con tres
puntos se obtiene el posicionamiento de la cámara respecto al sistema de
coordenadas del suelo.
zP
f tz
Y Z ty
c c
X
c
tx P ψ
h
Yw
Xw
Figura 9.3- Vista de planta del vehículo, cámara, línea a seguir y sus sistemas de
coordenadas
Capítulo 9. Aplicaciones 253
Xc Zc zP
φ
Yc tz
Zw ty P
Yw tx
Figura 9.4- Vista de perfil de la cámara, línea a seguir y sus sistemas de
coordenadas
Al tener libertad para elegir el sistema de coordenadas del suelo, se fija con
Yw sobre la línea a seguir (el sentido elegido establece el de Yu hacia abajo en la
imagen captada).
v
tz = (9.7)
cos (φ )
d d
a) Ej. Calzada b) Ej. Línea de cultivo
Figura 9.5- Posibilidades de líneas de seguimiento
cψ ⋅ ( t x ⋅ ( yu ⋅ s φ + f ⋅ cφ ) − v ⋅ xu ) + sψ ⋅ yu ⋅ v c φ + n ⋅ d ⋅ ( yu ⋅ s φ + f ⋅ cφ ) = 0 (9.9)
De (9.9) se obtienen los valores de (tx, ψ) para dos puntos del plano imagen,
de los que hay que conocer si están sobre la línea a seguir, a su derecha ó a su
izquierda. En el caso de utilizar dos puntos de la misma línea (n1 = n2 = n) se
llega a la solución analítica:
h = tan (φ ) ⋅ ( v − zP ⋅ c φ ) ⋅ sψ + t x ⋅ cψ (9.11)
Con objeto de conseguir una solución analítica sencilla para dicho proceso
inverso se fuerza:
ψ = 0 tx = 0 (9.12)
P2 x2 x2 P4 xw xw
P2 P4
yw
Xu
Yu
P3
x1 x1 P1 P3
Yw
P1 P3 Xw
a) Plano imagen b) Plano real
Figura 9.6- Situación de calibración
f ⋅ xw f ⋅ ( t y − c φ ⋅ yw )
xu = yu = (9.13)
s φ ⋅ yw + t z s φ ⋅ yw + t z
1 x xwj
cφ = ⋅ wi ⋅ yui − ⋅ yuj (9.15)
( ywj − ywi ) xui xuj
xu1 = − x1 y u1 = A xu 2 = − x2 y u1 = − A
(9.17)
xu 3 = x1 y u 3 = A xu 4 = x2 yu 4 = −A
Capítulo 9. Aplicaciones 257
xw ( x1 + x2 )
cφ = A ⋅ ⋅ (9.18)
yw x1 ⋅ x2
0 = cφ ⋅
(x ui − xuj )
+
s φ ( yuj ⋅ xui − yui ⋅ xuj )
⋅ (9.19)
(y ui − yuj ) f ( yui − yuj )
donde Pi y Pj son puntos sobre la misma línea.
Despejando f:
f = − tan (φ ) ⋅
(y uj ⋅ xui − yui ⋅ xuj )
(9.20)
(x ui − xuj )
Despejando v:
v = n ⋅ d ⋅ f ⋅ cφ ⋅
(y ui − yuj )
(9.23)
(y ui ⋅ xuj − yuj ⋅ xui )
Por tanto, a partir de la información del plano imagen, caracterizada por x1,
x2 y A, y de la ubicación real de los puntos, caracterizada por xw e yw, se obtiene
la calibración de los parámetros { φ , f, v}, al aplicar (9.18), (9.21) y (9.24).
Los valores de calibración obtenidos (Figura 9.7) son: altura 13.4 cm,
inclinación 45º, distancia focal 325.7 píxeles ≡ 325.7 x tamaño real del píxel.
Con lo que la precisión lograda es bastante alta, por tanto queda también
validado el sistema de posicionamiento desarrollado.
Las coordenadas de los puntos (en píxeles) para las líneas detectadas son:
a) {na=1} Pu1a=( –10, –93) Pu2a=( –90,93)
b) {nb= –1} Pu1b=(4, –93) Pu2b=( –2,93)
c) {nc=0} Pu1c=(18, –93) Pu2c=(83,93)
P ψ h
Ry
Rx
Gy
Gx
Figura 9.12- Posicionamiento obtenido con sistema de visión
G xR −h
G y = Indeterminado (9.25)
G
R
θR −ψ
2 2 2
1 1 1
0 0 0
-0.5 0 -0.5 0 -0.5 0
a) Ejemplo 1 b) Ejemplo 2 c) Ejemplo 3
Figura 9.13- Trazados en seguimiento de línea simulado: Referencia (––), P
ó origen de R (- -) y punto medio de las ruedas fijas (– –)
Con objeto de probar la robustez del control, en el segundo ejemplo se ha
introducido un ruido aleatorio en los valores del posicionamiento, acotado a 2cm
en separación (h) y 3º en orientación (ψ). Esto principalmente produce una
pequeña inestabilidad en la velocidad de control (Figura 9.14 (b)), pero no
impide que el seguimiento sea nuevamente aceptable (Figura 9.13 (b)).
En el tercer ejemplo se ha tomado ax = 0.8 s-1, que supondría asignar una
dinámica de seguimiento 2.5 veces más lenta que el periodo del control. Lo cual
entra en el límite de lo marcado por el Teorema de Shannon-Nyquist. De hecho,
el control ya no produce un acercamiento exponencial a la referencia (Figura
9.13 (c)), sino que hay una pequeña oscilación. Por lo que se empieza a atisbar
una posible inestabilidad, no en vano las velocidades de control (Figura 9.14 (c))
toman valores más grandes.
Un problema práctico adicional al asignar dinámicas rápidas es la pérdida de
la referencia en el plano imagen del sistema de visión, sobre todo si el punto P
que realiza el seguimiento no coincide con el corte entre eje del objetivo de la
cámara y el suelo.
De estas simulaciones se desprende que el control cinemático diseñado para
seguir una línea con un sistema por visión funciona correctamente siempre que se
respete una relación mínima entre la dinámica a asignar y el periodo de las
acciones de control, y a pesar de no tener situado longitudinalmente el vehículo.
Capítulo 9. Aplicaciones 265
2.5
1.5
1
0
0 2 4 6 8 10 12 14 16 18 20
a) Ejemplo 1
3
2.5
1.5
1
0
0 2 4 6 8 10 12 14 16 18 20
b) Ejemplo 2
4.5
3.5
2.5
1.5
0.5
-0 . 5
0 2 4 6 8 10 12 14 16 18 20
a) Ejemplo 3
Figura 9.14- Evolución de las velocidades de control de las ruedas fijas del
vehículo diferencial en el seguimiento de línea simulado: izda (––), dcha (- -)
266 Capítulo 9. Aplicaciones
9.3.1 Introducción
Fase 3
Fase 1 Fase 2
La Figura 9.19 muestra los parámetros geométricos del vehículo tipo coche
donde la rueda equivalente de dirección se ha representado. Si hay más de una
rueda el mecanismo de Ackerman garantiza su equivalencia. Los parámetros son:
- al es la distancia entre el eje trasero de ruedas y la parte delantera del vehículo;
- bl es la distancia entre eje trasero de ruedas y la parte de atrás del vehículo;
- hl es la distancia entre el eje de ruedas trasero y el centro de la rueda orientable;
- dl es la distancia entre los centros de las ruedas fijas del eje trasero;
- cl es el ancho del vehículo;
- r es el radio de las ruedas;
- φd max es la máxima orientación posible de la rueda de dirección equivalente;
- Pf es el punto medio entre las ruedas fijas que sigue la trayectoria.
Capítulo 9. Aplicaciones 271
φd max
Pf
d1
c1
Figura 9.19- Parámetros geométricos del vehículo tipo coche
hl
ρ= (9.27)
tan (φd )
P2 P4
φd
d2
d4
φd
Pf ρ CIR
d1 d3
P1 P3
c1
Figura 9.20- Descripción de CIR y distancias d1, d2, d3 y d4
P2
θ
P4
d2
d4
δ2
P1
d1 δ1 Y
δ4
δ3
P3 d3 θ
CIR
X
Figura 9.21- Relación entre el ángulo θ y la posición de Pi
1er arco
ξ
ρ
Y
ρ XP1
P1 Pf
ξ
2o arco θ
X
Figura 9.22- Desplazamiento entre dos arcos simétricos de valor ξ
274 Capítulo 9. Aplicaciones
Las posibles colisiones son con los obstáculos lateral, delantero y trasero al
maniobrar y con el delantero en la primera maniobra al abandonar del punto de
posicionamiento.
con el máximo:
Xˆ Pi = di + ρ ⋅ (1 − 2 ⋅ cos ( ξ ) ) para θˆPi = δ i (9.33)
Por tanto el punto singular (9.34) está fuera del rango de θ cuando:
ρ ≥ cl / 2 ó ρ ≤ c l / 2 − b l ⋅ tan ( ξ ) (9.35)
si ρ ≥ cl / 2
no hay colisión frontal/ trasera
si no si ρ ≤ c l / 2 − ab ⋅ tan (ξ )
{ ab=bl para P1, P3 y ab=al para P2, P4}
no hay colisión frontal/ trasera
si no hay colisión trasera/frontal
Algoritmo 1- Detección de colisión frontal/ trasera
P2
Y
Algoritmo 2- Cálculo de dx1 de modo que se pase por (xP2, yP2)= (0,0)
En el caso de que el valor asignado a dx1 sea menor de la mitad del ancho
del vehículo se hace que sea igual a la mitad del ancho del vehículo.
Una vez establecida la distancia de dx1, que caracteriza la posición del punto
de posicionamiento, se calcula la mínima distancia dy3 de la Figura 9.24 para
detener el primer movimiento en línea recta del vehículo (pasada de
reconocimiento).
dy 3
Este cálculo se realiza con un algoritmo con las entradas dx1 y dx2
(separación inicial del vehículo), que tiene en cuenta la evitación de colisión
lateral, utilizando (9.33), y con el obstáculo delantero en la segunda fase. Para
todo ello se ha desarrollado el Algoritmo 4.
278 Capítulo 9. Aplicaciones
45
40
35
30
maneuvers
25
20
15
10
0
15 20 25 30 35 40 45 50
ratio(%)
• Resulta útil en entornos tan variados como una carretera (tomando como
referencia sus marcas longitudinales), una nave industrial (tomando como
referencia una línea pintada al estilo filo guiado), un campo de cultivo
(tomando como referencia las hileras de cultivo), etc., y sirve como entrada
para realizar el seguimiento de línea por control cinemático.
Bibliografía 289
290 Bibliografía
[Bliman et al. 91] P.-A. Bliman and M. Sorine, “Friction modelling by hysteresis
operators: application to Dahl, sticktion and Stribeck effects”, In
Proceedings of the Conference Models of Hysteresis, Trento, Italy, 1991.
[Bliman et al. 95] P.-A. Bliman and M. Sorine, “Easy-to-use realistic dry friction
models for automatic control”, In Proceedings of the third European Control
Conference, Rome, Italy, pp. 3788-3794, 1995.
[De Luca et al. 93] A. De Luca and M.D. Benedetto, “Control of nonholonomic
systems via dynamic compensation”, Kybernetica, vol. 29 no. 6, pp. 593–
608, 1993.
[Dinesh et al. 92] K. Dinesh and M.C. Leu, “Genericity and singularities of robot
manipulators”, IEEE Transactions on Robotics and Automation, vol. 8 no. 5,
pp. 545-559, 1992.
[Dixon et al. 00] W.E. Dixon, Z.P. Jiang, D.M. Dawson “Global exponential
setpoint control of wheeled mobile robots: a Lyapunov approach”,
Automatica, vol. 36, pp. 1741-1746, 2000.
[Dugoff et al. 70] H. Dugoff, P. S.Fancher, L. Segel, “An analysis of tire traction
properties and their influence on vehicle dynamic performance”, SAE
Transactions, 1970.
[Ferrière et al. 98] L. Ferrière and B. Raucent, “Rollmobs, a new universal wheel
concept”, Proceedings of the International Conference on Field and Service
Robotics, Leuven, Belgium, pp. 1877-1882, 1998.
[Golub et al. 83] G. H. Golub, C. F. Van Loan, Matrix Computations, The Johns
Hopkins University Press, 3rd ed. Baltimore, MD 1996.
[Gracia2 et al. 02] L. Gracia, J. Tornero, “Kinematic control system for car-like
vehicles”, Advances in Artificial Intelligence – Iberamia 2002, Verlag-
Springer, pp. 882-892, 2002.
[Gracia3 et al. 03] L. Gracia, J. Tornero, “Geometric parallel parking planner for
car-like vehicles”, Industrial Simulation Conference, 2003.
[Haessig et al. 91] D.A. Haessig and B. Friedland, “On the modelling and
simulation of friction”, ASME Journal of Dynamic Systems, Measurement,
and Control, vol. 113 no. 3, pp. 354-362, 1991.
[Holve et al. 96] R. Holve, P. Protzel, “Reverse parking of a mobile car with
fuzzy control”, Proceedings of the 4th European Congress on Intelligent
Techniques and Soft Computing, pp. 2171-2175, 1996.
[Hough 59] P.V.C. Hough, “Machine analysis of bubble chamber pictures”, Int.
Conference on High Energy Accelerators and Instrumentation, 1959.
294 Bibliografía
[Jiang et al. 99] K. Jiang, D.Z. Zhang, L.D. Seneviratne, “A parallel parking
system for a car-like robot with sensor guidance”, Proc. IMechE, Part C,
Jour. of Mechanical Engineering Science, vol. 213 no. 6, pp. 591-600, 1999.
[Kiencke et al. 94] U. Kiencke, and A. Daiss, “Estimation of tyre friction for
enhanced ABS-systems”, In Proceedings of AVEG Congress, Tokyo, 1994.
[Kim et al. 04] W. Kim, B-J Yi, D. J. Lim, “Kinematic modeling of mobile
robots by transfer method of augmented generalized coordinates”, Journal of
Robotic Systems, vol. 21 no. 6, pp. 301-322, 2004.
[Kim2 et al. 04] S. Kim and H. Kim, “Isotropy analysis of caster wheeled
omnidirectional mobile robots”, Proc. of the Int. Conf. on Robotics and
Automation, New Orleans, pp. 3093-3098, 2004.
Bibliografía 295
[Kim et al. 05] S. Kim, H. Kim, B. Moon, “Local and global isotropy of caster
wheeled omnidirectional mobile robots”, Proc. of the Int. Conf. on Robotics
and Automation, Barcelona, pp. 3457-3462, 2005.
[Kleeman 95] L. Kleeman, “Odometry error covariance estimation for two wheel
robot vehicles”, Monash University, Technical Report MECSE-95-1, 1995.
[Lai et al. 90] J.Z.C. Lai, and D. Yang, “Efficient motion control algorithm for
robots with wrist singularities”, IEEE Transactions on Robotics and
Automation, vol. 6 no. 1, pp. 113-117, 1990.
[Laumond et al. 94] J.-P. Laumond, P.E. Jacobs, et al., “A motion planner for
nonholonomic mobile robots”, IEEE Transactions on Robotics and
Automation, vol. 10 no. 5, 1994.
[Leow et al. 02] Y. P. Leow, K.H. Low, W.K. Loh, “Kinematic Modelling and
Analysis of Mobile Robots with Omni-Directional Wheels”, Proc. of 7th Int.
Conf. on Control, Automation, Robotics and Vision, Singapore, 2002.
[Lipkin et al. 88] H. Lipkin and J. Duffy, “Hybrid twist and wrench control for a
robotic manipulator”, ASME Journal of Mechanisms, Transmissions, and
Automation in Design, vol. 110, pp. 138-144, 1988.
[Loh et al. 03] W.K. Loh, K. H. Low, Y. P. Leow, “Mechatronics design and
kinematic modeling of a singularityless omnidirectional wheeled mobile
robot”, Proc. of the Int. Conference of Robotics and Automation, Taiwan,
pp. 3237-3242, 2003.
[Lu et al. 05] C.Y. Lu and M.C. Shih, “An experimental study on the longitudinal
and lateral adhesive coefficients between the tyre and the road for a light
motorcycle”, Vehicle System Dynamics Supplement, vol. 43, pp. 168-178,
2005.
[Luo 98] D. Luo, “Pattern recognition and image processing”, Horwood Series in
Engineering Science, Chichester: Horwood Publishing Ltd., cap. 6, 1998.
[Luo et al. 98] Z.-H. Luo, K. Machida, M. Funaki, “Design and control of a
cartesian nonholonomic robot”, Journal of Robotic Systems, vol. 15 no. 2,
pp. 85-95, 1998.
[Marchant 95] J. Marchant, “Tracking of row structure in three crops using image
analasys”, Computer and Electronic in Agriculture, 1995.
[Marrs 85] T. Marrs, “The personal robot book”, Blue ridge summit,
Pennsylvania: Tab Books, Inc, 1985.
Bibliografía 297
[Okello et al. 94] J. Okello, M.J. Dwyer, F.B. Cottrell, “The tractive performance
of rubber tracks and a tractor driving wheel tyre as influenced by design
parameters”, J. Agricultural Eng. Research, Vol. 59 no. 1, pp. 33–43, 1994.
298 Bibliografía
[Pacejka et al. 93] H.B. Pacejka and E. Bakker, “The ‘magic formula’ tyre
model”, Vehicle System Dynamics, vol. 21, pp. 1-18, 1993.
[Pacejka et al. 97] H.B. Pacejka and I.J.M. Besselink, “Magic formula tyre model
with transient properties”, Vehicle System Dynamics Supplement, vol. 27,
pp. 234-249, 1997.
[Pacejka 02] H.B. Pacejka, Tyre and Vehicle Dynamics, Butterworth Heinemann,
2002.
[Park et al. 99] K. Park, H. Chung, J.G. Lee, “Point stabilization of mobile robots
via state space exact feedback linearization”, Proc of the IEEE International
Conference on Robotics and Automation, Detroit, pp. 2626-2631, 1999.
Bibliografía 299
[Pin et al. 94] F. Pin and S. Killough, “A new family of omnidirectional and
holonomic wheeled platforms for mobile robots”, IEEE Transactions on
Robotics and Automation, vol. 10 no. 4, pp. 480-489, 1994.
[Pinto et al. 99] F.A.C. Pinto, J.F. Reid, Q. Zhang, N. Noguchi, “Guidance
parameter determination using artificial neural network classifier”, ASAE
paper no. 993004, 1999.
[Rogers 84] M. Rogers, “Birth of the killer robots”, Newsweek CIII (26):51,
1984.
[Sánchez et al. 97] A. Sánchez, J. Marchant, “Fast and robust method for tracking
crop rows using a two point Hough transform”, Biorobotics’97, Valencia, ,
pp. 33-281, 1997.
300 Bibliografía
[Shin et al. 01] D. H. Shin and K. H. Park, “Velocity kinematic modeling for
wheeled mobile robots”, Proc of the 2001 IEEE International Conference on
Robotics and Automation, Seoul, Korea, pp. 3516-3522, 2001.
[Smith 73] M. H. Smith, “Design of a low cost, general purpose robot”, Proc. Of
3rd Int. Joint Conf. on Artificial Intelligence, Stanford (California), pp. 324-
335, 1973.
[Stanisic et al. 90] M.M Stanisic and O. Duta, “Symmetrically actuated double
pointing systems: the basis of singularity-free robot wrists”, IEEE
Transactions on Robotics and Automation, vol. 6 no. 5, pp. 562-569, 1990.
[Stocco et al. 99] L.J. Stocco, S.E. Salcudean, F. Sassani, “On the use of scaling
matrices for task-specific robot design”, IEEE Trans. Robotics and
Automation, vol. 15 no. 5, pp. 958-965, 1999.
Bibliografía 301
[Tan et al. 91] H . S . Tan and Y.-K. Chin, “Vehicle traction control: variable
structure control approach”, ASME Journal of Dynamic Systems,
Measurement, and Control, Vol. 113, pp. 223-230, 1991.
[Tourassis et al. 92] V.D. Tourassis and H. Ang Jr., “Identification and analysis
of robot manipulator singularities”, International Journal of Robotics
Research, vol. 11 no. 3, pp. 248-259, 1992.
[Tzafestas et al. 01] C.S. Tzafestas and S.G. Tzafestas, “Full-state modeling,
motion planning and control of mobile manipulators”, Studies in Informatics
and Control, vol. 10 no. 2, pp 109-128, 2001.
[Wada et al. 00] M. Wada, A. Takagi, S. Mori, “Caster drive mechanisms for
holonomic and omni-directional mobile platforms with no over constraint”,
Proceedings of the IEEE International Conference on Robotics and
Automation, San Francisco, USA, pp. 1531-1538, 2000.
[West et al. 95] M. West and H. Asada, “Design and control of ball wheel
omnidirectional vehicles”, Proc. of the IEEE International Conference on
Robotics and Automation, Nagoya, Japan, Vol. 2, pp. 1931-1938, 1995.
[Yi et al. 02] B.-J. Yi and W. K. Kim, “The kinematics for redundantly actuated
omnidirectional mobile robots”, Journal of Robotic Systems, vol. 19 no. 6,
pp. 255-267, 2002.
[Zhao et al. 05] Y. Zhao, E.G. Collins Jr., “Robust automatic parallel parking in
tight spaces via fuzzy logic”, Robotics and Autonomous Systems, vol. 51
no. 2, pp. 111-127, 2005.