Avaliação e Correção do Modelo Cinemático de Robôs Móveis

Evaluation and Correction of the Kinematic Model of Mobile Robots

Jonas Pereira

Silas do Amaral

Universidade do Estado de Santa Catarina Departamento de Engenharia Elétrica [email protected]

Universidade do Estado de Santa Catarina Departamento de Engenharia Elétrica [email protected]

Abstract — In Mobile Robotics, many tasks require accuracy, overcoat with relation to the end position and orientation of the robots. This problem is closely tied to the path planning, wich, from of the information of the starting posture (position and orientation), the expectation is that the robot, from among innumerables possibles pathes, chooses the more convenient and achieves the desired end posture. However this posture, due to the some inexact robot’s kinematic parameters, not always it is possible to be reached in open-loop, resulting in odometry errors. In this paper, one method of evaluation and correction for the systematic odometry errors is presented. Results of simulation and experiments for situations of interesting are presented with the intention to validate the study. Index Terms — Mobile Robots; Kinematic Model; Odometry; Control.

I. INTRODUÇÃO Um método bastante utilizado para se obter a localização de robôs móveis é a odometria [1]. Em [2] a odometria é combinada com a técnica de landmarks (marcos dispostos no ambiente), com o intuito de se obter uma localização mais robusta; em [3] a forma de propagação dos erros de odometria é abordada, porém, ao invés de landmarks, utiliza-se linhas pintadas no chão em forma de grid; já em [4] investiga-se as causas e propõe-se um método experimental de correção para os erros sistemáticos de odometria. O presente artigo tem por base o trabalho [4] e suas dissidências [5][6]. Assim, foram estudados todos os aspectos envolvidos na obtenção do modelo cinemático do robô Khepera e, a partir deste, as possíveis fontes dos erros de odometria, quando do seguimento de trajetórias. O restante deste artigo está dividido como segue: na seção II é descrito o robô móvel Khepera. A seção III trata da influência dos parâmetros cinemáticos na odometria e a forma de corrigí-los. Na seção IV, é discutida a forma de controle, juntamente com um resultado de simulação e experimental realizado para uma trajetória de interesse. A última, a seção V, apresenta as conclusões alcançadas e algumas propostas de trabalhos futuros.

II. O ROBÔ MÓVEL KHEPERA Uma das plataformas robóticas que tem obtido grande aceitação perante a comunidade científica mundial é a dos

robôs Khepera “Fig. 1”. Isto se deve principalmente as suas características, que são relatadas a seguir.

Fig. 1. O robô móvel Khepera [1]

A. Arquitetura Apesar de pequeno ( ≅ 30 mm de altura, 55 mm de diâmetro e 70 g de peso), o Khepera possui uma grande capacidade computacional embarcada, baseada em um sistema composto por um microcontrolador de 32 bits (MC68331 – 16 MHZ), memória SRAM de 256 Kb e EEPROM de 128 Kb a 512 Kb, barramento de extensão paralelo, porta serial (RS232), portas de entrada analógica, além de uma pequena rede de comunicação local. Com relação ao seu controle, este pode ser feito por intermédio de softwares como o LabVIEW®, MATLAB® ou diretamente através de palavras de controle (código ASCII) via RS232 [1]. Como atuadores, o Khepera possui 2 motores DC acoplados às rodas através de uma redução de 25:1. Um encoder incremental localizado no eixo do motor permite medidas de odometria, fornecendo 24 pulsos/revolução do motor. Isto proporciona uma resolução de 600 pulsos/revolução da roda, o que corresponde a 12 pulsos/mm da trajetória do robô. Os motores são controlados através do método PWM (chaveamento a 20 KHz) ou através de um controlador PID [1]. B. Modelagem Cinemática Para a análise cinemática do Khepera e posterior cálculo da odometria, assume-se que o Khepera seja um corpo rígido, cujas rodas não sofrem deformação ou derrapagens e que se movimentam apenas no plano horizontal. A posição e a orientação do robô são descritas em relação a um sistema de coordenadas inercial {Xb, Yb} com origem denotada por O, enquanto que um sistema de coordenadas {Xm, Ym} com origem P é associado ao corpo do robô, conforme mostra a “Fig. 2”.

Evaluation and Correction of the Kinematic Model of Mobile Robots D  D    ΔDE = Δθ ⋅  DP − r  ; ΔDD = Δθ ⋅  DP + r  (3) 2  2    Por outro lado, ΔD e Δθ são dados por: ΔDD − ΔDE ΔDD + ΔDE (4) ; Δθ = Dr 2 Outra forma de se obter o deslocamento linear: a partir do conhecimento do raio das rodas (R) e do número de pulsos por revolução das rodas nos encoders (Nr), pode-se calcular o deslocamento linear das rodas no intervalo [t, t+∆t] através de [1][8]: 2π ⋅ R ⋅ N pE 2π ⋅ R ⋅ N pD (5) ; ΔDD = ΔDE = Nr Nr onde NpE e NpD correspondem aos números de pulsos obtidos nos encoders das rodas direita e esquerda. ΔD =

Fig. 2. Coordenadas de postura (posição e orientação) [1]

Desta forma, as coordenadas de postura do robô descritas no sistema {Xb, Yb} são dadas pelo vetor: ξ = [x

y θ ]T

(1)

onde x e y representam a posição do robô, e θ representa a orientação do robô em relação ao eixo Xb.

III. ODOMETRIA

O modelo cinemático do Khepera é obtido com base na “Fig. 3”, onde P e P’ representam respectivamente as posições (x, y) e (x’, y’) do centro geométrico do robô nos instantes t e t+∆t; Dr é a distância entre rodas e DP é a distância do centro do robô ao Centro Instantâneo de Rotação (CIR); ∆DE, ∆DD e ∆D são os deslocamentos, assim como VE, VD e V se referem às velocidades lineares das rodas esquerda, direita e do centro do robô no intervalo de discretização; e ∆θ é a variação do ângulo de orientação no mesmo intervalo.

Uma das técnicas mais utilizadas em robótica móvel, devido a sua simplicidade de implementação e baixo custo ainda é a odometria. A odometria é um método de estimação de posição e orientação que consiste no cálculo da posição atual de um robô móvel, integrando a distância percorrida, a qual é medida por intermédio da rotação de suas rodas [1][3]. A base deste método é a utilização de encoders incrementais, que acoplados ao eixo das rodas do robô, realizam a leitura dos pulsos de revolução das mesmas. Embora seja um método clássico e de boas características, o mesmo não é adequado para trajetórias longas, onde os erros vão se acumulando. A. Definição dos Erros de Odometria Erro de odometria é a diferença entre a posição sobre a qual o robô “pensa” estar (estimado por cálculo) e a posição atual (medida por dispositivos externos). Este erro pode ser isolado e dividido em duas classes [1][3][8]: 1) Erros sistemáticos: são causados por incertezas nos parâmetros geométricos utilizados para calcular a posição e orientação do robô. As principais causas destes erros são: diâmetro das rodas desigual e/ou diferente do valor nominal, distância entre rodas diferente da nominal e incerteza sobre o ponto de contato das rodas. 2) Erros não-sistemáticos: são característicos da relação do robô com o ambiente. As principais causas destes erros são: escorregamento das rodas, imperfeições no solo ou outros fenômenos similares.

Fig. 3. Modelo cinemático [1]

No limite quando ∆t → 0, obtém-se [1][7]: cos θ 0 VD + VE   x  cos θ 0  2  V   ξ =  y  = sen θ 0   = sen θ 0  V −  D VE  ω     0 θ   0 1 1  D  r  onde (ω) é a velocidade angular do robô.

B. Erros Sistemáticos em Robôs com Tração Diferencial (2)

Os deslocamentos lineares das rodas esquerda e direita são dados por:

Não ocorrendo erros de ordem não-sistêmica, dois erros dominantes podem ser observados [1][5]: a) rodas com diâmetros diferentes: a maioria dos robôs móveis utiliza pneus feitos de borracha. A diferença nas dimensões e o seu desgaste podem provocar substanciais erros de odometria, que são dependentes da relação:

Evaluation and Correction of the Kinematic Model of Mobile Robots (6) Ed = DRD DRE onde DRD e DRE são os diâmetros atuais das rodas direita e esquerda respectivamente. b) incerteza quanto à distância entre rodas: define-se a distância entre rodas (Dr) como a distância compreendida entre os pontos de contato das rodas do robô com o solo. A incerteza desta distância pode provocar diferenças na contagem dos encoders, e tem efeito somente quando o robô é submetido a trajetos curvilíneos ou rotações em torno de seu próprio eixo (ver (2)). Esta incerteza é dependente da relação: EDr = Dr , a Dr , n (7)

2. Análise dos Resultados A análise dos resultados é realizada em quatro etapas: 1) Erros de posicionamento; 2) Erros de orientação; 3) Obtenção de E d e E Dr ; e 4) Fator de correção. 1) Erros de Posicionamento Após a realização do experimento, têm-se, portanto, cinco pontos de erro de posicionamento (εxi,H, εyi,H) para o sentido Horário (H), e cinco pontos de erro de posicionamento (εxi,AH, εyi,AH) para o sentido Anti-Horário (AH). Se estes pontos forem plotados em um mesmo gráfico obtém-se algo semelhante à “Fig. 5”.

onde Dr,a e Dr,n representam as distâncias entre rodas atual e nominal, respectivamente. C. Método UMBmark Um método muito utilizado para avaliar, comparar e corrigir os erros sistemáticos de odometria em robôs móveis de tração diferencial é o UMBmark1 – University of Michigan Benchmark [1][4]. A proposta do método é submeter o robô a trajetórias quadradas (sentido horário e anti-horário), calcular os erros de posicionamento e ajustar alguns parâmetros do modelo cinemático (Dr e DRD/RE), através da alteração da constante de proporcionalidade entre os pulsos lidos pelos encoders e a distância efetivamente percorrida pelas rodas do robô. O método consiste de duas etapas [1][6]: 1) Avaliação Experimental; e 2) Análise dos Resultados. 1. Avaliação Experimental A primeira etapa consiste em fazer com que o robô percorra um caminho quadrado diversas vezes (cinco vezes) e em ambos os sentidos “Fig. 4”.

Fig. 5. Resultados típicos do método UMBmark (adaptado de [5])

O centro de gravidade das duas áreas formadas pelo agrupamento de pontos é definido por xc.g.,H/AH e yc.g.,H/AH, calculados a partir de:

xc.g , H / AH = yc. g , H / AH =

1 n 1 n

n

∑ε

xi , H / AH

(8)

i =1 n

∑ε

yi , H / AH

(9)

i =1

O erro de posicionamento absoluto do robô a partir da origem pode ser obtido através de: rc. g ., H / AH = ( xc. g ., H / AH ) 2 + ( yc. g ., H / AH ) 2

(10)

Assim, o maior valor entre rc.g.,H e rc.g.,AH é definido como a “medida da precisão de odometria dos erros sistemáticos” [1][4]: E max ., sist . = max(rc. g ., H ; rc. g ., AH ) . Fig. 4. Erro de postura para uma trajetória quadrada (adaptado de [4])

Devido aos erros de odometria presentes, a posição e a orientação final do robô não coincidirá necessariamente com a posição inicial.

1

Método criado e desenvolvido nos laboratórios de Robótica Móvel da Universidade de Michigan em 1994, pelos Drs. Johann Borenstein e Liqiang Feng, ambos do Depto. de Engenharia Mecânica daquele centro.

2) Erros de Orientação Quanto à orientação do robô, duas novas características podem ser definidas [1][6]: • Erros do Tipo A: erro de orientação que reduz ou aumenta a quantidade total de rotação do robô durante o experimento em ambos os sentidos; • Erros do Tipo B: erro de orientação que aumenta a quantidade total de rotação do robô durante o experimento em um sentido, mas reduz no outro.

Evaluation and Correction of the Kinematic Model of Mobile Robots No entanto, quando da movimentação do robô, erros do Tipo A e B ocorrem juntos. A distinção entre os dois tipos de erro e seus agentes causadores pode ser obtida a partir da análise das “Figs. 6 e 7”.

Fig. 7. Erros do Tipo B (adaptado de [6])

Da análise das “Figs. 6 e 7”, pode-se concluir que os erros do Tipo A (denotados por α) ocorrem devido à diferença da distância atual e nominal entre as rodas do robô, ao passo que os erros do Tipo B (denotados por β) ocorrem devido à diferença de seus diâmetros atuais. Considerando o eixo x, β e α são dados por:

α=

(xc.g., H − xc.g ., AH ) ⋅ 180$

(11)

(

(12)

− 4L xc. g ., H + xc. g ., AH − 4L

π

) ⋅ 180$ π

Dr , a Dr , n

=

90$ 90$ − α

3) Obtenção de E d e E Dr O raio Rc do trajeto curvilíneo pode ser obtido a partir do triângulo ABM, onde M é o CIR “Fig. 8”, L2 (13) Rc = sen( β 2)

4) Fator de Correção A correção de E Dr é trivial: a distância entre rodas Dr

D. Aplicação do Método UMBmark ao Khepera Para a realização do método UMBmark, utilizou-se uma cartolina com o desenho de uma trajetória quadrada (30 x 30 cm), sobre uma mesa plana. Para garantir que o robô não derrapasse nas curvas de 90°, adotou-se uma velocidade baixa (≈ 8 mm/s), e nas retas (≈ 16 mm/s). Após a realização do experimento (cinco vezes em cada sentido), os seguintes resultados foram obtidos: β = 0,9167°; α = 0,8976°; Rc = 1875,02 cm; Ed = 1,002831; E Dr = 1,010074; Dr,a = 5,353393 cm; CE = 0,998587; CD = 1,001413; Emax.,sist. = 2,65 cm. considerando L = 30 cm; Rn = 7,81 mm e Dr,n = 5,30 cm. Na “Fig. 9” é apresentado o resultado do experimento UMBmark (Khepera descalibrado).

Fig. 8. Raio Rc da curvatura devido a Ed (adaptado de [4])

Ed =

D RD Rc + D r 2 = D RE Rc − D r 2

Semelhantemente, E Dr pode ser calculado,

(15)

pode ser redefinida isolando Dr,a em (15). No entanto, a correção de Ed é um pouco mais complexa: a partir do conhecimento de Ed (14), do diâmetro médio atual das rodas (DM=(DRD+DRE)/2) e com base em (6), 2 2 (16) D RE = ⋅ D M ; D RD = ⋅ DM 1 Ed + 1 +1 Ed A partir de (16) dois parâmetros podem ser obtidos, 2 2 ; CD = (17) CE = 1 Ed + 1 +1 Ed E reescrevendo (5): 2π ⋅ RM ⋅ N pE 2π ⋅ RM ⋅ N pD ; ΔDD = CD . (18) ΔDE = CE ⋅ Nr Nr onde RM é o raio médio atual das rodas.

Fig. 6. Erros do Tipo A (adaptado de [6])

β=

E Dr =

(14) Fig. 9. Resultado UMBmark para o Khepera descalibrado [1]

Evaluation and Correction of the Kinematic Model of Mobile Robots Pela “Fig. 9” observa-se que quando o robô realiza o trajeto no sentido anti-horário, os erros provenientes de Ed e E Dr praticamente se cancelam (α e β semelhantes). No outro sentido, os erros provenientes de Ed e E Dr se somam. A correção dos parâmetros Ed e EDr no Khepera é feita da seguinte maneira [1]: a) Correção de Ed: realiza-se a correção dos pulsos desejados a partir dos pulsos lidos dos encoders das rodas ao final de cada marco artificial (MA),

A. Rastreamento Para o rastreamento da trajetória, considera-se a seguinte referência:  xr = Vr cos θr  y = V sen θ r r  r (21) θr = ωr  pr = ( xr yr θr )T ν = (V ω )T r r  r

(19)

onde pr representa o vetor de postura de referência e vr o vetor das velocidades linear e angular de referência [1][9]. O objetivo é encontrar um controlador suave de velocidade, ν c (t ) = f c (e p , ν r , K ) tal que lim ( p r − p) = 0 ,

onde ud é a unidade de distância do encoder e vale 0,0081786 cm e L a distância do trajeto. b) Correção de E Dr : redefine-se Dr para Dr,a, ou seja,

onde ep e K são, respectivamente, o erro de rastreamento (22) e o vetor de ganho do controlador.

 L  1 L ⋅  M A ≅   −   ud   C E / D ud 

DE

D

2π Dr π ⋅ Dr D = τ⋅ r = ⋅ = 2 4 2 4

(20)

t →∞

B. Controle de Robôs Móveis segundo o Modelo Cinemático de Postura em Coordenadas Cartesianas

onde DE/D é a distância linear a ser percorrida pelas rodas esquerda e direita a cada 90° (τ = 2π/4). Após as devidas correções, foram obtidos os seguintes resultados para o experimento UMBmark: β = 0,0048°; α = -0,0048°; Rc = -360000,00 cm; Ed = 0,999985; E Dr = 1,000053; Dr,a = 5,350284 cm;

Para o caso do robô móvel rastreando uma trajetória de referência, considera-se o erro de postura ep, como a diferença entre a postura de referência e atual do robô, com a devida transformação de coordenadas [1][9], e p = Te ( p r − p)

CE = 1,000007; CD = 0,999993; Emax.,sist. = 0,23 cm. Na “Fig. 10” é apresentado o resultado do experimento UMBmark (Khepera calibrado).

(22)

 e1   cos θ sen θ 0 e  = − sen θ cos θ 0   2  e3   0 0 1

 xr − x   y − y   r  θr − θ 

onde Te é a matriz de rotação do sistema de coordenada inercial {Xb, Yb} em relação ao sistema de coordenada móvel do robô {Xm, Ym}. C. O Controlador de Rastreamento Para o sistema de controle de rastreamento, as entradas do sistema são a postura de referência (pr) e a velocidade de referência (vr), que são variantes no tempo. A saída do sistema é a postura atual p do robô “Fig. 11”. O objetivo do controlador é obter a convergência do erro de postura ep para zero. Em (23) é fornecida a lei de controle.

Fig. 10. Resultado UMBmark para o Khepera calibrado [1]

Pela “Fig. 10”, observa-se uma melhora significativa quanto ao erro de posicionamento. Levando-se em conta o Emax.,sist, esta melhora foi da ordem de 11,5 vezes.

IV. CONTROLE E RESULTADOS Quanto a forma de controle, utilizou-se a técnica de controle de rastreamento considerando o modelo cinemático de postura em coordenadas cartesianas (2).

Fig. 11. Diagrama de blocos do sistema de controle utilizado [1]

Vr cos e3 + k1e1  V   νc =   =   ω ωr + k2Vr e2 + k3Vr sen e3 

(23)

com ν c (t ) = f c (e p , ν r , K , t ) e K = (k1 k 2 k 3 ) T , sendo os parâmetros de projeto ki’s (ganhos do controlador) positivos e ajustados experimentalmente [1][9].

Evaluation and Correction of the Kinematic Model of Mobile Robots Na “Fig. 12” é mostrado um resultado de simulação para uma trajetória sinuosa.

REFERÊNCIAS [1] Pereira, J. (2003). Avaliação e Correção do Modelo Cinemático de Robôs Móveis Visando a Redução de Erros no Seguimento de Trajetórias. (Dissertação de Mestrado), Universidade do Estado de Santa Catarina – UDESC, Joinville – SC, Brasil. [2] Valgas Filho, J. H. Á., Campos, M. F. M., (2002). Uma Metodologia de Correção Automática de Erros de Odometria Durante a Navegação do Robô. XIV Congresso Brasileiro de Automática, Natal – RN.

[3] Mächler, P. (1998). Robot Positioning By Supervised and Unsupervised Odometry Correction. (Thèse de Docteur), École Polytechnique Fédérale de Lausanne – EPFL, A postura de partida do robô na “Fig. 12” foi atribuída Switzerland. T como p = (40 10 π) e a de chegada desejada como p = T (24,7 41,5 π/2) . A posição de chegada obtida [4] Borenstein, J.; Feng, L. (1994). UMBmark – A Method for (considerando o erros de odometria) foi x = 23,2 cm; y = Measuring, Comparing, and Correcting Dead-reckoning 42 cm. Os ganhos do controlador foram ajustados como: Errors in Mobile Robots. University of Michigan Tecnical k1=48; k2=0,12 e k3=0,48. Report, EUA. A “Fig. 13” apresenta os resultados experimentais obtidos para a trajetória sinuosa: [5] Borenstein, J.; Feng, L. (1996). Measurement and Y [cm] Correction of Systematic Odometry Errors in Mobile Robots. University of Michigan, EUA. IEEE Transactions on Robotics and Automation, Vol. 12, N° 5. Fig. 12. Khepera rastreando uma trajetória sinuosa [1]

42,6

42,4

42,2

42

41,8

41,6

X [cm] 22,8

23

23,2

23,4

23,6

23,8

24

24,2

24,4

24, 41,64

41,2

24,8

25

25,2

Sem Correção Com Correção

41

[6] Borenstein, J.; Feng, L. (1995). Correction of Systematic Odometry Errors in Mobile Robots. University of Michigan Tecnical Report, EUA. Procedings of the International Conference on Intelligent Robots and Systems (IROS’95) – Pitsburgh, Pennsylvania, págs. 569574.

c.g.Sem Correção

[7] Campion, G., Bastin, G., D’ Andréa-Novel, B. (1996). Structural Properties and Classification of Kinematic and Fig. 13. Resultados obtidos para a trajetória sinuosa [1] Dynamic Models of Wheeled Mobile Robots. IEEE Da análise da “Fig. 13”, obteve-se rc.g. = 1,53 cm (sem Transactions on Robotics and Automation, Vol. 12, N° 1, correção) e rc.g. = 0,10 cm (com correção). A melhora no págs. 47-62. erro do posicionamento final foi ≈ 15 vezes. [8] Lages, W. F. (1998). Controle e Estimação de Posição e Orientação de Robôs Móveis. (Tese de Doutorado), V. CONCLUSÃO Instituto Tecnológico de Aeronáutica – ITA, São José dos Campos – SP, Brasil. Este trabalho realizou a avaliação e a correção do modelo cinemático de um robô móvel. Diante dos resultados apresentados, pode-se verificar uma melhora [9] Pimentel, J. C. (2000). Técnicas de Visão Computacional e de Controle para Aplicação em Robótica Móvel. (Tese significativa (acima de uma década) quanto ao erro de de Mestrado), Instituto Tecnológico de Aeronáutica – posicionamento final, validando o estudo. ITA, São José dos Campos – SP. Com base neste artigo, trabalhos futuros como a fusão da odometria com outras técnicas de estimação da posição e orientação, emprego da estatística na análise dos resultados e/ou a incorporação dos erros não-sistemáticos são alguns temas sugeridos, cujas investigações produzirão acréscimos significativos aos resultados já alcançados. c.g.Com Correção

40,8