PGEE5558 Robótica Móvel

Robôs Móveis a Rodas Tipo Uniciclo

Seguimento de Trajetória
 Seguimento de Trajetória com Robôs Uniciclo

Sistema de controle de posicionamento visto

~ Conversão de
𝐗  𝑑 𝐗  ,𝜓 coordenadas 𝜌,𝜃,𝛼
  Controlador de 𝑣,𝜔  Modelo 𝐗,𝜓
 
+
- (cartesianas para posição cartesiano
polares)
𝐗,𝜓
 

⟨ 𝑑⟩ 𝜃   caso do controlador proposto para resolver o problema de

  No
  posicionamento, o ponto que se deseja atingir é fixo.
 𝑦 𝑑 (𝑡 )
𝛼
𝜌   Quando se deseja seguir uma trajetória, porém, deseja-se
 
alcançar, e então manter-se junto, um ponto que se move com
𝜔 velocidade . Ou seja, assim que o robô atingir a trajetória ele
  𝑣 𝜓
deve continuar se movendo com a mesma velocidade dela, para
   
𝑦 acompanhar o seu movimento. Portanto, o controlador visto
  ⟨ 𝑟⟩
 
anteriormente não pode ser usado, pois o ponto destino não é
⟨ 𝑤⟩ 𝑥 mais fixo, e devemos propor outro controlador.
    𝑥  𝑑 (𝑡 )
 Seguimento de Trajetória com Robôs Uniciclo
  Cinemática Direta
  Cinemática Direta
Ponto de interesse
Ponto de interesse para controle deslocado
no centro do eixo
(ponto , à distância do caso anterior)
virtual que une as
rodas

𝑌  𝑟 𝑋  𝑟
𝜓 
Umcontroladores
detalhe importante é que
𝑣 
𝑣  Os para seguir 𝜓 
𝜔
  𝜓  não é necessário
trajetórias controlar na
serão baseados a
orientação
técnica do robô no por
de linearização
seguimento de também
realimentação, trajetória. Como
referida
( 𝑥 , 𝑦 )
𝑤
𝑌  ele deve
como manter
técnica a mesma
de cinemática
velocidade
inversa da trajetória,
(veremos após
mais adiante
alcançá-la,
que se podeele automaticamente
utilizar também a
se orientará conforme
dinâmica inversa). a
𝑋 𝑤 tangente à trajetória.
 Seguimento de Trajetória com Robôs Uniciclo
 Da diapositiva anterior:

Para
  controlar somente a posição do ponto :

Equação
  cinemática (direta)

,,

𝜓   é a matriz de cinemática direta. Se , ela é singular ( Caso

𝑣  contrário, ela é não singular, e existe .
𝜓 
É
   discutidoinversa
Cinemática Importante
a seguir um controlador  para
seguimento de trajetória, chamado controlador de
ou Se a matriz de
cinemática inversa (porque usa a matriz de inversa
cinemática
cinemática inversa acima definida) paranãoo existe.
caso em
que .
 Seguimento de Trajetória com Robôs Uniciclo
Velocidade da trajetória Erro de posição
 𝐀 −1 syms a psi;
 Lei de Controle: A=[cos(psi) -a*sin(psi); sin(psi) a*cos(psi)];
InvA=simplify(inv(A));

A=
[ cos(psi), -a*sin(psi)]
[ sin(psi), a*cos(psi)]

𝜓  InvA =
𝑣  [ cos(psi),
 Se o controlador não ésin(psi)]
válido, pois se
𝜓  baseia na [cinemática
-sin(psi)/a, inversa.
cos(psi)/a]

 Reescrevendo a lei de controle obtemos:

 Seguimento de Trajetória com Robôs Uniciclo
𝑐
  𝑣 𝑣
 Substituindo no modelo, obtém-se que : [ ] [ ]
𝜔𝑐
=
𝜔

𝜓 
𝑣   Observações:
𝜓 
•
• é a saída do controlador de cinemática inversa
• e são os erros de posição em e em
• e são os ganhos
• e são constantes de saturação
• é a posição atual
• é a posição desejada, com derivada
 Seguimento de Trajetória com Robôs Uniciclo
    𝑘𝑥 ~

Daí:
𝑥 ˙ =~
˙ 𝑑−𝑥 𝑥
˙ =− 𝑙 𝑥 𝑡𝑎𝑛 h
𝑙𝑥
𝑥
( )
Assim sendo, vê-se que o controlador 𝑘
proposto é capaz de
𝑦 fazer
˙ 𝑑 − com
𝑦
˙ = ~
que
𝑦os
˙ =− erros𝑙de
𝑦 posição
𝑡𝑎𝑛 h em
𝑙𝑦
𝑦 ~
relação
𝑦 à trajetória
desejada sejam reduzidos a zero assintoticamente.
( )
 Seja a função candidata de Lyapunov , cuja derivada é , ou
Note-se, em particular, que quando1.5o robô estiver sobre a
1.5
então
trajetória, ou seja, e , ele desenvolverá a velocidade da
𝜓 
𝑣  trajetória,
1 ou seja, e , ou seja, 1

𝜓    𝑘 𝑥 ~0.5 ~ 𝑘𝑦 ~
0.5 𝑉˙ (~
𝑥 ,~
𝑦 )=− ~
𝑥 𝑙 𝑥 𝑡𝑎𝑛 h
( )
𝑙𝑥
𝑥 − 𝑦 𝑙 𝑦 𝑡𝑎𝑛 h
𝑙𝑦
𝑦 <0
( )
tanh(x)

tanh(x)
0 0

-0.5 Termos sempre positivos -0.5

-1 -1

-1.5 -1.5
-1.5 -1 -0.5 0 0.5 1 1.5 -10 -5 0 5 10
x x
 Seguimento de Trajetória com Robôs Uniciclo

 Seguimento de trajetória em forma de oito vertical:

• e
• e
Resultado de simulação
𝜓 
𝑣 
𝜓 
 Seguimento de Trajetória com Robôs Uniciclo
 Seguimento de trajetória em forma de oito vertical:
• e
• e

Resultado de simulação

𝜓 
𝑣 
𝜓 
 Seguimento de Trajetória com Robôs Uniciclo
 Seguimento de trajetória em forma de oito vertical:
• e
• e

Resultado experimental
𝜓 
𝑣 
𝜓 
 Seguimento de Trajetória com Robôs Uniciclo
O seguimento de velocidade não é perfeito, porque o controlador cinemático
não considera a massa do robô (é como se o robô fosse somente um ponto,
sem massa, logo sem inércia, e sua aceleração fosse infinita). Como a massa
é significativa, tem que ser considerada a dinâmica (aceleração finita).
Resultado experimental

𝜓 
𝑣 
𝜓 
 Seguimento
(  )
de Trajetória
𝑥𝑑 , 𝑦𝑑
com Robôs Uniciclo
𝜓  ( 0 𝑚 / 𝑠 , 0𝑚 / 𝑠 )
𝑣   Uma observação importante é que o controlador
𝜓  proposto para seguimento de trajetória pode ser
facilmente adaptado para também cumprir tarefas
de posicionamento. No seguimento de trajetória
temos que a posição que se deseja atingir varia com
o tempo, ou seja, é dada por , e se move com as
velocidades , que corresponde à derivada no tempo
da posição desejada.
 No posicionamento a posição que se deseja alcançar é um valor fixo de forma que .
Assim, para cumprir uma tarefa de posicionamento com o mesmo controlador basta
alterar a lei de controle para
, o que também preserva a estabilidade assintótica do sistema de controle em malha
fechada.
 Seguimento de Trajetória com Robôs Uniciclo
𝜓 
𝑣   O controlador
 No seguimentoproposto parao seguimento
de caminho de étrajetória
que se deseja também
permanecer
𝜓  pode ser
sobre um adaptado para cumprirque
caminho predefinido, tarefas
é umadecurva
seguimento deXY,
no plano
caminho.
ou No seguimento
seja, uma sucessão dede trajetória
pontos temos
tal que que a que
de forma posição
a que
se deseja atingir
velocidade varia
ao longo do com o tempo,
caminho ou seja,
é arbitrária (),é mas
dadatangente
por , e se
move
ao com asComo
caminho. velocidades , que
exemplo, sejacorresponde
o caminho em à derivada no
linha reta
tempo
dado da, posição
por que serádesejada.
percorrido com velocidade constante tal
que , para , sendo o vetor de velocidade dado por . Assim,
para cumprir tal tarefa usaríamos a lei de controle dada por
=. A estabilidade assintótica do sistema de controle em malha
fechada se mostra da mesma forma.
 Seguimento de Trajetória com Robôs Uniciclo
𝜓   Problema: caso
𝑣 
𝜓 
Equações de movimento:

Portanto, o controlador de cinemática inversa

visto não pode ser aplicado neste caso.

𝑌  𝑟 𝑋  𝑟

𝜔 𝑣  𝜓 
 

𝑤
( 𝑥 , 𝑦 )
𝑌 

𝑋 𝑤
 Robôs Móveis Uniciclo: Cinemática Estendida
Cinemática básica  
(com ponto de interesse para controle no
centro do eixo virtual que une as rodas)
𝑌  𝑟 𝑌 𝑤 𝑋  𝑟
𝜓 
 Modelo cinemático:

  𝜔
  𝑣  𝜓 
( 𝑥 , 𝑦 )
𝑋 𝑤
𝑌 𝑤
Estados:
Entradas:
𝑋 𝑤
 
Restrição não holonômica:
Ponto de interesse para o
controle situado no centro do
(não há velocidade na direção transversal do robô – eixo ). eixo virtual que une as rodas
 Robôs Móveis Uniciclo: Cinemática Estendida
 Cinemática básica  
(com ponto de interesse para controle fora do
centro do eixo virtual que une as rodas - )
𝜓 
Modelo cinemático: 𝑣 
𝜓 
 

  𝑥˙ 𝑐𝑜𝑠 𝜓 − 𝑎𝑠𝑒𝑛 𝜓   Estados:

[][
𝑦˙ = 𝑠𝑒𝑛 𝜓 𝑎𝑐𝑜𝑠 𝜓
𝜓˙ 0 1
𝑣
𝜔 ][ ] Entradas:
Ponto de interesse para o
controle situado fora do
centro do eixo virtual que
𝐱˙ = 𝐀 𝑐 𝐯
 
une as rodas, inclusive fora
da plataforma robótica
 Seguimento de Trajetória (já visto)
𝜓 
Ponto de interesse fora do eixo 𝑣 
virtual que une as rodas 𝜓 

  𝑥˙
[][ = 𝑐 𝑜𝑠 𝜓 −𝑎𝑠𝑒𝑛 𝜓 𝑣
][ ] Matriz de
𝑦˙ 𝑠𝑒𝑛 𝜓 +𝑎𝑐𝑜𝑠 𝜓 𝜔 cinemática
inversa

𝐱˙ = 𝐀 𝐯
 

   
~ −1 ~
Controle de cinemática inversa
𝐟 ( 𝐱 )= 𝐋𝑡𝑎𝑛h ( 𝐊 𝐋 𝐱 )
Observação:
Para o ponto de interesse sobre o eixo das rodas, não existe inversa de A! Daí
o controlador já visto não se aplica. Há que se buscar uma outra solução.
 Seguimento de Trajetória Usando Cinemática
Estendida
>> syms psi a alfaCinemática estendida para o robô com tração diferencial:
>> ma=[cos(psi) -sin(psi) -a*sin(alfa+psi); sin(psi) cos(psi) a*cos(alfa+psi);0 0 1]
 x   cos  sin  a sin(  )   u 
>> simplify(inv(ma)) Velocidade fictícia
 
y    sin
[ cos(psi), sin(psi),  a*sin(alfa)]
cos
 
a cos(  )   v  x  A eμ e no eixo y do robô.
[ -sin(psi), cos(psi), -a*cos(alfa)]
   0 0 1   
 
[ 0, 0, 1]

Lei de controle de cinemática inversa

 uc   cos sin a sin     x 
    
 vc     sin cos  a cos    y 
   0 0 1   
 c     Malha fechada: u  uc ; v  vc ;   c
 x    x 
Matriz de cinemática inversa   
 y    y 
 Restrição não holonômica:    
   
 Seguimento de Trajetória com a Cinemática Estendida

 Trajetória desejada: ,
Velocidade da trajetória: ,
  quando

  Para pontos de interesse fora do eixo das rodas ():

 Impondo a restrição x y
não holonômica
   sin  cos
a cos  a cos 
Sinais de controle: uc   x cos   y sin  a sin 
c   
 Seguimento de Trajetória Usando Cinemática
Estendida
 Para pontos de interesse sobre o eixo das rodas ():

  Matriz de cinemática
 uc   cos sin a   x 
     inversa com
 vc     sin cos 0   y 
  Impondo a restrição
não holonômica    0 0 1   
 c 
y
 d  arctan    d  k k  0
x

 x  x d  k x x  y  y d  k y y

Sinais de controle uc   x cos  y sin  a

c  
 Experimentos

Seguimento trajetória usando

controle por cinemática inversa

Controlando o ponto
Controlando o ponto fora do eixo que une
sobre o eixo que une as rodas
as rodas
Experimentos
Experimento de controle de postura

Controlando o ponto
sobre o eixo que une
as rodas
Modelo Dinâmico para Robôs Uniciclo
Parâmetros e variáveis:
u Velocidade linear
 Velocidade angular
ψ Orientação
h Ponto de interesse
para controle
G Centro de massa
E Ferramenta
a Distância entre o
ponto de interesse
e o centro do eixo
que une as rodas
 Modelo Dinâmico para Robôs Uniciclo
Cinemática
Incerteza
u cos  a sin    0 0
 x  u sin   a cos   0 0   x 
 y      
     0 0   uref   y

     3 2  4  1   0 

       u   0  ref   
 u   1 1   1   u
     5 u   6    0 1   
      2 
 2 2 
  Dinâmica
Referências
Parâmetros dinâmicos de velocidade
(identificados)

Referência:
Celso de La Cruz e Ricardo Carelli (2008). Dynamic model based formation control
and obstacle avoidance of multi-robot systems. Robotica, 26(3), 345-356.
doi:10.1017/S0263574707004092
Modelo Dinâmico para Robôs Uniciclo
 Modelo Dinâmico para Robôs Uniciclo
 3 2  4   1 
   u   0
 
u 1 1  1  uref 
   
 
    5 u   6    0 1  ref 
   2    2 
 2
 uref  1 0   u    4   3   u 
  
 ref
  
  0  2     5  6   
 uref 1 0   u    4   3 u  rad 2
         , I 1 2
 ref   0  2     I 3  6   5  I 3  u    m
 uref  1 0   u   0   3   Iu   4 0 u 
              
 ref   0  
2  
 
 3 0 
   0  6    5  I 3  u  
v r  Hv  C(v)v  F(v)v
 Propriedades do Modelo Dinâmico Utilizado
 uref  1 0   u   0   3   Iu   4 0 u 
   
 ref
  
  0  2     3 0      0  6   5  I 3  u   
  

v r  Hv  C(v)v  F(v)v

1.
•   H é constante
2. H=HT > 0 ( é simétrica e positiva definida, por ser diagonal
com elementos positivos)
3. H-1 existe, e H-1 > 0 (a inversa de existe e também é positiva
definida)
4. F=FT > 0 se ( é positiva definida, sob tal condição)
5. F(v) é constante se
6. C(v) é antissimétrica
  6   5  I 3  u e  6   5  I 3  u
Condições para Robôs Uniciclo

•   • Foi feita a identificação dos parâmetros para 3 robôs

móveis Pioneer e para uma cadeira de rodas robótica.
• Os robôs têm dinâmicas diferentes:
– 2 Pioneer 2-DX, um tendo computador de bordo;
– 1 Pioneer 3-DX cm um sensor laser de distância e uma
câmara omnidirecional;
– 1 cadeira de rodas robótica, primeiro carregando uma
pessoa de 55 Kg e depois uma pessoa de 125 Kg;
• De acordo com os valores obtidos para os parâmetros,
as condições acima são satisfeitas para todos os robôs
testados.
 Estrutura de Controle

vd vr v
Robô

c
x d u ref
s
x
h d uref x
c c
uref uref u 1/s
y d
Controlador  u~ Controlador
Cinemático Dinâmica Cinemática y y
~   ref
c Dinâmico 1/s
xd  x s [θˆ ]  θ
hd  x
~y
 c
ref  c
ref
ref   
yd 1/s

 ~


y


h 
u
 Controlador Cinemático (Cinemática Inversa)
 x  cos  a sin  
 y    sin   u 
h  x y
T
   a cos   

   0 1   
Cinemática Inversa
Cinemática Direta

 u   cos sin    x 
h   x   cos  a sin    u  u 

 y   sin      A       1 sin  1  
cos   y 
   a cos         a a 

Cinemática Inversa
  kx ~ 
x
sin    d x
  l tanh  x   ~
x  xd  x  ~
x  x d  x
 u   cos
c
  lx 
   1 sin  
ref
 1 , ~
y  yd  y  ~y  y d  y
   a
c
cos    ky ~  
ref
a   y d  l y tanh  y  k x , k y  0 ; a  0.
 l 
 y 
 Análise de Estabilidade

•• Considera-se
  que e que
• Isto quer dizer que não se consideram os efeitos dinâmicos (é o que se chama
seguimento perfeito de velocidade, ou seja, a aceleração é instantânea)

 Estabilidade Assintótica
 V  1h ~T~
h0
  kx   2
~
 l x tanh  
x  
~   lx  
h 
  ky     kx   ky 
l y tanh  ~ 
y     
V  h h   xlx tanh 
t 
 x   yl
 y tanh  y   0
  l 
  ly    lx   y 


Modelo Dinâmico para Robôs Uniciclo
Por que utilizá-lo?
Resultado simulado
Resultado Experimental

Controlador cinemático
 Controlador Dinâmico
•   • O controlador dinâmico é projetado com base no
modelo , considerando suas propriedades;
• Ele recebe referências de velocidade e compensa a
dinâmica, gerando comandos de velocidade para o
robô;
• Flexibilidade: pode ser usado para compensar a
dinâmica do veículo em conjunto com outros
controladores cinemáticos, como de seguimento de
caminhos ou posicionamento, considerando suas
propriedades.
 Controlador Dinâmico
Lei de Controle: v r  T ~
v   Hv d  Cvd  Fv d
  ku ~   ~
v  vd  v
 tanh  u  
lu 0   lu  
T v   
~ lu , l  
0 l    k ~  
 tanh   
  l  k u , k   

Equação do sistema em malha fechada

Hv  Cv  Fv  T ~
v   Hv d  Cvd  Fv d
H~
v  C~ v  T ~
v  F~ v  0
 Análise de Estabilidade
Função candidata de Lyapunov:
1 ~T ~
V  v Hv  0  V  ~
v T H~
v
2
Derivada da função candidata de Lyapunov:

v T T ~
V  ~ v  ~
v TC~
v~
v T F~
v
  Porque é assimétrica.

v T T ~
V  ~ v  ~
v T F~
v

V  0  ~
v  0, t  .
 Análise de Estabilidade

•   • Relaxando a condição de seguimento perfeito

de velocidade, pode-se mostrar que uma
condição suficiente para que é

~ A~
v cos
A
 a sin  
h   sin  a cos 
min( k x , k y )
k x  0, k y  0
  • Como quando se usa o controlador dinâmico, a
condição acima será verificada para qualquer
valor de . Então, , e
~
h (t )  0.
 Resultados de Simulação

Apenas o controlador   Com a compensação

cinemático dinâmica incluída
( identificados)
 Resultados Experimentais

• Pose inicial: (0.2 m; 0.0 m; 0º);

• Referência: trajetória em 8 começando
em (0.0 m; 0.0 m);
• Velocidade linear: 0.1 a 0.4m/s.
• Velocidade angular: -0.75 a 0.75 rad/s.
 Resultados Experimentais

Apenas o controlador   Com a compensação

cinemático dinâmica incluída
( identificados)