UM CONTROLADOR PARA ROBÔS MÓVEIS BASEADO EM NUVEM DE PARTÍCULAS

Transcrição

1 UM CONTROLADOR PARA ROBÔS MÓVEIS BASEADO EM NUVEM DE PARTÍCULAS Jorge Augusto Vasconcelos Alves, Walter Fetter Lages Universidade Federal do Rio Grande do Sul Departamento de Engenharia Elétrica Av Osvaldo Aranha, Porto Alegre, RS, Brasil s: Abstract Common control systems for mobile robots include the use of deterministic control laws together with state estimation approaches and the consideration of the certainty equivalence principle Recent approaches consider the use of partially observable Markov decision process strategies together with Bayesian estimators In order to reduce processing power and yet allow for multimodal or non-gaussian distributions, a scheme based on a particle filter and a corresponding cloud of input signals is proposed in this paper Results are presented and compared to a scheme with extended Kalman filter and the assumption that the certainty equivalence holds Keywords Mobile robotics, Particle filter, Nonlinear control, Stochastic control, Stochastic estimation Resumo O controle usual de robôs móveis inclui o uso de leis de controle determinísticas juntamente com estimadores de estados e a consideração do princípio da equivalência à certeza Resultados recentes consideram o uso de processo de decisão de Markov parcialmente observáveis juntamente com o uso de estimadores bayesianos De forma a reduzir o esforço computacional e mesmo assim considerar distribuições multimodais ou não gaussianas, este artigo propõe um esquema baseado em um filtro de partículas para estimação do estado e uma nuvem de partículas para os sinais de controle Os resultados são comparados com um esquema utilizando o filtro de Kalman estendido e o princípio da equivalência a certeza Palavras-chave Robôs móveis, Filtro de partículas, Controle não linear, Controle estocástico, Estimação estocástica 1 Introdução É sabido que robôs móveis estão sujeitos à incerteza tanto no seu comportamento quanto no ambiente no qual o robô navega (Thrun et al, 5) O enfoque clássico para estimação de estados e controle de sistemas estocásticos é considerar o valor esperado das variáveis de estado e o princípio da equivalência à certeza (Anderson e Moore, 1989) No entanto, enfoques baseados no valor esperado não podem ser utilizados quando distribuições multimodais, ou mesmo assimétricas, estão presentes Por outro lado, distribuições assimétricas ou multimodais podem surgir devido à fusão de sensores e problemas típicos de robótica (Kaelbling et al, 1998) Adicionalmente, a dinâmica não linear frequentemente gera distribuições multimodais ou assimétricas a partir de distribuições normais ou simétricas O estado da arte atual para tratar incertezas, especialmente as com distribuições de probabilidade não gaussianas, é utilizar filtros bayesianos para estimar o estado do sistema e então obter um sinal de controle baseado no resultados da estimação Tipicamente, o resultado da estimação é uma densidade de probabilidadeouumhistogramaouumconjuntodepartículas ou probabilidades sobre um mapa topológico Este sinal pode ser obtido a partir de um dos modos da distribuição ou através de otimização, como nos enfoques de processo de decisão de Markov parcialmente observáveis (POMDP) (Blanco etal,1;thrunetal,5) OusodePOMDP para sistemas com estados contínuos requer aproximações, pois caso contrário, o problema torna-se intratável(baral et al, ; Littman et al, 1995) Este artigo propõe um esquema de controle para um robô móvel com acionamento diferencial que mapeia um conjunto de possíveis estados em um espaço de sinais de controle Tanto as transições de estado quanto as observações são sujeitas à incerteza Portanto, um filtro de partículas é proposto para estimação de estados No entanto, a estimativa do estado não é considerada como sendo apenas um vetor, mas toda a nuvem de partículas que representa a probabilidade de cada vetor de estado A seguir, uma lei de controle globalmente estável é considerada para o mapeamento da nuvem de partículas no espaço de estados em um nuvem de partículas no espaço de entradas da planta O sinal de entrada a ser efetivamente aplicado no robô é, então, escolhido entre os pertencentes às regiões mais populadas do espaço de entradas Descrição do Sistema A cinemática de um robô móvel com acionamento diferencial movendo-se em um plano horizontal pode ser descrita, em tempo contínuo por cosx 3 ẋ = f(x,u) = senx 3 u (1) 1

2 onde x = [ x 1 x x 3 ] T é o vetor de estados e u = [ u 1 u ] T é o vetor de entradas As variáveisdeestadox 1 ex sãoascoordenadasnoplano, x 3 é o ângulo de orientação, e as variáveis de entrada u 1 e u são as velocidades linear e angular, respectivamente Uma realização em tempo discreto (Lages, 1998) é dada por x(k +1) = f d (x(k),u(k)) () [ ( ) ( )] Tu (k) Tu 1 (k) sinc cos x 3 (k)+ Tu (k) [ ( ) ( )] = x(k)+ Tu (k) Tu 1 (k) sinc sen x 3 (k)+ Tu (k) Tu (k) onde sincx senx x e T é o período de amostragem O comportamento do robô pode ser melhor descrito por modelos estocásticos, como descrito por Thrun et al (5) e Rekleitis (4) Os efeitos estocásticos podem ser observados no movimento do robô como uma deriva com relação à trajetória nominal, tanto na distância trafegada quanto na orientação Estes erros crescem com o tempo, e portanto, serão modelados aqui como velocidades lineares e angulares incertas Adicionalmente, os efeitos estocásticos estão fortemente relacionados com a velocidade linear do robô (Rekleitis, 4) Assim, a versão estocástica de (1) é ẋ(t) = f (x(t),u(t)+w(t)), (3) com w(t) = u 1 (t) [ w t (t) w D (t) ] T, onde wt (t) N (,σ t) ewd (t) N (,σ D) sãoprocessosgaussianos representando as incertezas nas velocidades linear e angular, respectivamente Para obter um modelo discreto que pode representar adequadamente a incerteza de orientação em k + 1, será assumido aqui, com base em Rekleitis (4), que metade dos efeitos da incerteza na velocidade angular atua através da transição de estado, portanto afetando tanto a posição quanto a orientação em k + 1, e a outra metade atua diretamente na orientação em k +1 Consequentemente, a incerteza w D (t) no modelo contínuo será representada por duas incertezas no modelo discreto: w d1 (k) N (,σ d), que atua através da transição de estado e w d (k) N (,σ d), que atua diretamente no estado em k+1 Os efeitos de w t (t) podem ser mapeados diretamente em w t (k) N (,σ t) Logo, o modelo discreto pode ser escrito como: x(k +1) = f d (x(k),u(k)+w 1 (k))+w (k), (4) onde a transição de estados f d (, ) é dada por () e w 1 (k) u 1 (k) [ ] wt (k), w d1 k) w (k) Tu 1 (k) w d (k) Como assume-se que w d1 (k) e w d (k) representam metade dos efeitos de w D (t), sua variância deve ser a metade de σ D, ou σ d = σd É importante notar que embora w 1 (k), w 1 (k) e w (k) apareçam como termos aditivos em (3) e (4), eles não são, na realidade, incerteza aditiva, pois dependem da velocidade linear u 1 (k) e tanto f(, ) quanto f d (, ) são não lineares O modelo (4) será utilizado para estimar as transições de estado para um conjunto de possíveis valores do vetor de estado, como será explicado em detalhes na seção 31 É importante notar que mesmo que w t (k), w d1 (k) e w d (k) sejam assumidos gaussianos, o estado resultante x(k+1) não é gaussiano, devido às não-linearidades O modelo (3) será usado para simular o robô na seção 4, enquanto o modelo (4) será utilizado para estimação de estado 3 Sistema de Controle 31 Estimação de Estado Robôs móveis não podem, em geral, determinar sua pose (ie, posição e orientação) através de sensores internos A sua pose deve ser estimada Aqui assume-se que o robô está equipado com encoders incrementais nas rodas e um GPS As leituras dos encoders são consideradas como medidas das entradas de velocidade do robô, enquanto o GPS fornece uma medida da pose Tanto as leituras dos encoders quando as leituras do GPS são corrompidas por incertezas, de forma que não é possível saber-se a pose real do robô Além disso, as medidas dos encoders estão disponíveis a cada instante de amostragem, enquanto as observações do GPS são disponíveis a uma taxa menor do que a taxa de amostragem do sistema Como as leituras dos encoders são mapeadas como medidas das entradas do sistema, u(k), o vetor de observação y(k), corrompido pela incerteza v(k), é restrito à observação do GPS y(k) = h(x(k),v(k)) = x(k)+v(k) (5) O filtro de partículas, no entanto, poderia ser estendido para considerar outros sensores, incluindo sensores de outros tipos, simplesmente considerando-os na definição de h(x(k), v(k)) Note que para outros tipos de sensores, o mapeamento de x(k) e v(k) em y(k) pode ser não linear e que quando sensores redundantes forem usados, a dimensão de y(k) pode ser maior do que a de x(k)

3 Como é o caso para muitas técnicas de filtragem bayesiana, os algoritmos de filtro de partículas podem ser divididos em dois estágios, denominados predição e atualização O funcionamento do filtro de partículas é resumido a seguir Uma descrição mais detalhada está disponível em Thrun et al (5) A cada instante de amostragem, os possíveis valores do vetor de estado x i (k), i [1,M], são considerados, com base nas observações anteriores do sistema Cada vetor x i (k) é denominado uma partícula e M é o número total de partículas A crença do estado bel p (x(k)) é dada pelo conjunto de todas as partículas, ou seja: bel p (x(k)) = {x 1 (k),x (k),,x M (k)} (6) A crença do estado é uma aproximação de uma função de densidade de probabilidade no seguinte sentido: regiões do espaço de estados com um número relativamente grande de partículas tem altos valores de densidade de probabilidade, enquanto regiões com relativamente poucas partículas representam baixos valores de densidade de probabilidade A etapa de predição do algoritmo calcula a crença a priori do estado no instante k + 1, bel p (x(k +1)), a partir da crença do estado no instante k e do vetor de entradas do sistema Para cada partícula da crença do estado, uma nova partícula é gerada conforme a função de transição de estado do sistema (4), com os termos de incerteza obtidos de um gerador de números aleatórios com distribuição apropriada, o que pode ser feito para qualquer distribuição A crença a priori do estado é denotada por bel p(x(k +1)) = {x 1(k+1),x (k+1),,x M(k+1)}, (7) onde cada partícula x i (k+1), i [1,M], é obtida de x i (k +1) = f d (x i (k),u(k)+w 1i (k))+w i (k) (8) O conjunto de partículas bel p (x(k +1)) é obtido sem informação da observação do sistema em k + 1: é calculado apenas a partir da conjunto de partículas bel p (x(k)) e as entradas da planta Este conjunto de partículas é, então, atualizado com a informação das observações, obtendo-se a nova crença do estado no instante k + 1, bel p (x(k +1)) Isto é feito a partir do chamado fator de importância ι i (k + 1) de cada partícula x i (k +1), dado por com ι i (k) = f y (y(k) x i (k)), f y (y(k) x(k)) = e( 1 [y(k) Cx(k)] T P[y(k) Cx(k)]), (π)n P onde P é a matriz de covariância da incerteza e n é o número de linhas de y(k) A crença do estado bel p (x(k +1)) é obtida através da seleção das partículas, entre aquelas em bel p (x(k +1)), com uma probabilidade proporcional ao seu fator de importância (Thrun et al, 5) A atualização da crença requer que seja feita uma observação Neste artigo, o GPS é considerado ter um período de amostragem maior do que o dos encoders incrementais Como resultado, a etapa de atualização não ocorre a cada instante de amostragem, mas apenas quando as observações do GPS são obtidas 3 Controle Um resultado importante sobre o controle de robôs móveis é que não é possível estabilizar assintoticamente o sistema em um ponto arbitrário através de uma lei de controle suave e invariante no tempo (Brockett, 198) No entanto, apesar disto, o sistema é controlável (Astolfi, 1994) Considera-se, aqui, um mapeamento do espaço de estados para o espaço de entradas proposto por Lages e Hemerly (1998), que permite a obtenção de uma lei de controle assintoticamente estável Se o mapeamento for representado por g : X U, x X e u U, então o sistema autônomo ẋ = f (x,g(x)) onde f(, ) é descrito por (1), é assintoticamente estável na origem No entanto, desejase estabilizar o robô em qualquer ponto x r, ou seja, qualquer posição e orientação (x r1,x r,x r3 ) Isso pode ser feito pela mudança de coordenadas x(x,x r ),obtida pela transformação [ ] R(xr3 ) x = (x x 1 r ) (9) onde R(x r3 ) é uma matriz de rotação -D, ou seja, [ ] cosxr3 senx R(x r3 ) = r3 senx r3 cosx r3 Logo, seosistema x = f ( x,g( x))éestávelem x =, então ẋ = f (x,g(x)) é estável em x = x r Portanto, para estabilizar o sistema em qualquer ponto arbitrário x r, com base na lei de controle g que leva o estado para a origem, é suficiente utilizar g( x) (Sørdalen, 1993) Aqui, o resultado da estimação de estados é um conjunto de partículas Esta estimativa pode ter pontos agrupados em torno de regiões distintas do espaço de estados, devido a crenças multimodais Como consequência, nem estimadores baseados em média quadrática, nem estimadores baseados em valor esperado produzem resultados apropriados e o princípio da equivalência a certeza não pode ser aplicado Propõe-se, neste caso, gerar o sinal de controle a partir da crença atual do estado considerando o sinal de controle que seria

4 gerado para cada partícula pertencente à crença do estado, mapeando-se com isto a crença do estado para o espaço de entradas do sistema, cuja distribuição refletirá os valores adequados para a entrada do sistema A cada instante de amostragem k, a crença bel p (x(k)) representa os possíveis valores para o vetor de estado Para cada partícula x i (k), desta crença, uma partícula de controle u i (k) é obtida por u i (k) = g( x i (k)), com x i (k) computada por (9), resultando bel p (u(k)) = {u 1 (k),u (k),,u M (k)} (1) onde g( x i (k)) é um mapeamento apropriado do espaço de estados para o espaço de entradas Para cada partícula da crença do estado, é utilizada a seguinte transformação de coordenadas (Lages e Hemerly, 1998): e = x 1 + x (11) ψ = atan( x, x 1 ) (1) α = x 3 ψ, (13) que, com o sinal de controle u i (k) = [u i1 (k) u i (k)] T u i1 = γ 1 ecosα (14) u i = γ α γ 1 cosα senα (α hψ), α (15) com h, γ 1, γ >, faz (1) ser assintoticamente estável (Lages e Hemerly, 1998) Consequentemente, a crença de controle contém os sinais de controle que estabilizam em um ponto as partículas da crença do estado O critério para escolher um vetor de controle dentre as partículas pertencentes à bel p (u(k)) é selecionar a partícula com o maior suporte local, ou seja, escolher a partícula cujas vizinhas também estão contidas em bel p (u(k)) A vizinhança de cada partícula u i (k) foi escolhida com uma região elipsoidal S i, centrada em u i (k), dada por S i = {u(k) : (u 1 u i1 ) a 1 + (u u i ) a onde a 1 e a são os raios da elipse 33 Filtro de Kalman Estendido } < 1, Para avaliar o desempenho da estratégia de controle proposta, que usa um filtro de partículas e uma nuvem de sinais de controle, será feita uma comparação com a estratégia clássica de utilizar um filtro de Kalman estendido e calcular-se o controle utilizando-se o princípio da equivalência à certeza Ressalte-se que em ambos os casos será utilizado o mesmo mapeamento de estados para entradas apresentado na seção 3 Os dois casos consideram aproximações para gerar uma estimativa para o estado O filtro de partícula produz uma estimativa representada por um conjunto de valores possíveis, as partículas, que são uma aproximação da distribuição de probabilidade conjunta dos estados O filtro de Kalman estendido produz uma aproximação da distribuição de probabilidade conjunta dos estados dada por uma gaussiana multivariável e considera uma aproximação das funções de transição de estados e de observação dadas pelo primeiro termo da expansão em série de Taylor das respectivas funções A atualização da estimativa do estado feita pelo filtro de Kalman estendido é baseada no ganho de Kalman e na observação Como a crença é resumida por uma média (ˆx(k)) e uma matriz de covariância Q(k), as equações de atualização podem ser escritas em função destas variáveis As equações do filtro também dependem do Jacobiano F(k) de f d ( ) F(k) = f d(x(k),u(k)) = x(k)=ˆx(k) x(k) ( ) Tu (k) 1 Tu 1 (k)sinc ( Tu (k) 1 Tu 1 (k)sinc 1 do jacobiano H(k) de h( ), ( sen ( ) cos H(k) = h(x(k),u(k)) x(k) ) x 3 (k)+ Tu (k) ) x 3 (k)+ Tu (k) ; = I; x(k)=ˆx(k) e das matrizes de covariância Q(k) e R(k), relativasàsincertezasdef d ( )eh( ), respectivamente As etapas de atualização da estimativa do estado e predição são computadas conforme as equações usuais do filtro de Kalman estendido 4 Resultados de Simulação O robô foi simulado através do modelo estocástico em tempo contínuo dado por (3), integrado utilizando-se um algoritmo de Runge-Kutta de 4 a ordem O filtro de partículas utilizou na sua etapa de predição o modelo estocástico do robô em tempo discreto, dado por (4) Os parâmetros das incertezasforamσ t =,5eσ D =,1745rad/m A matriz de covariância da observação foi P = diag(σ y1,σ y,σ y ), comσ y1 = σ y = 1 meσ y3 = 1 A velocidade máxima das rodas é 471 m/s O período de amostragem do controle é T = 5ms e o GPS fornece medidas (com incertezas) a cada ms Um total de 9 partículas foram usadas para estimação de estados, sendo que na crença inicial todas foram espaçadas igualmente em uma área de 1m centrada em x() Os parâmetros do controlador foram γ 1 =,5, γ =,5 rh = 1, Os raios do elipsoide utilizado para determinar a vizinhança do sinal de controle foram,5 e, A pose inicial foi x() = [ 4 π ] T e a referência foi ajustada em x r = [ 1 3 π/ ] T

5 A crença do controle em k = é mostrada na figura 1 Como as partículas da crença inicial do estado estão estruturadas em uma grade, a crença do controle resultante mantém parte daquela estrutura u (rad/s) u (rad/s) u1 (m/s) Figura 4: Crença do controle em k = u1 (m/s) Figura 1: Crença do controle em k = A crença do estado em k = 5 é mostrada na figura Afigura3mostraasuaprojeçãonoplano X 1 X com a orientação omitida A crença do controle correspondente é mostrada na figura 4 u1 (m/s) u (rad/s) k k 44 x Figura 5: Sinais de controle em função do tempo Linha sólida: método proposto Linha pontilhada: EKF e equivalência à certeza 13 x Figura : Crença do estado em k = 5 x (m) x1 (m) Figura 3: Projeção da crença do estado em k = 5 no plano X 1 X A figura 5 mostra os sinais de controle em função do tempo A trajetória do robô no plano é mostrada na figura 6, juntamente com a trajetória gerada pelo método utilizando filtro de x Kalman estendido A posição final do robô é x(k) = [ 1,44 3,9 1,571 ] T O experimento foi repetido 5 vezes, como forma de verificar os efeitos estocásticos em regime A média e o desvio padrão do estado no último instante de amostragem são mostrados na tabela 1 A tabela mostra os resultados equivalentes utilizando o filtro de Kalman estendido e o princípio da equivalência à certeza Nota-se que, embora os valores das médias sejam semelhantes para ambos os controladores, utilizando-se a estratégia proposta neste artigo tem-se desvios padrão bem menores, o que significa que o desempenho do controlador é bem mais consistente e robusto às incertezas presentes no sistema do que a estratégia de utilizar EKF com um controlador baseado na equivalência a certeza Tabela 1: Pose final média e desvio padrão com o método proposto Média Desvio padrão x 1 (m),9973,3 x (m) 3,7,139 x 3 (rad) 1,578,14

6 35 Referências x (m) x1 (m) Figura 6: Trajetória do robô Linha sólida: método proposto Linha pontilhada: EKF e equivalência à certeza Tabela : Pose final média e desvio padrão com EKF e equivalência à certeza Média Desvio padrão x 1 (m) 1,538,75 x (m),976,17 x 3 (rad) 1,431, Conclusões Um método para controlar um robô móvel com acionamento diferencial que pode acomodar incertezas não gaussianas na transição de estados e nos sensores foi proposto neste artigo e teve seu desempenho comparado com o enfoque clássico de utilizar um filtro de Kalman estendido e o princípio da equivalência à certeza Foi utilizada uma estratégia e estimação de estados baseada em filtro de partículas e a nuvem de partículas representando a crença do estado foi mapeada para o espaço de entradas através da utilização de uma lei de controle não suave, resultando em uma distribuição de partículas no espaço de entradas, representando a crença do controle O sinal de controle foi então obtido desta crença através de um método que considera o suporte local para o sinal de controle com maior probabilidade Embora os valores das médias dos erros sejam semelhantes para ambos os controladores, utilizando-se a estratégia proposta neste artigo tem-se desvios padrão bem menores, o que significa que o desempenho do controlador é bem mais consistente e robusto às incertezas presentes no sistema do que a estratégia de utilizar EKF com um controlador baseado na equivalência à certeza Agradecimentos Os autores agradecem à CAPES e à FAPERGS pelo apoio financeiro Anderson, B D O e Moore, J B (1989) Optimal Control: Linear Quadratic Methods, Prentice-Hall Information and System Sciences Series, Prentice-Hall, Englewood Cliffs, NJ, USA Astolfi, A(1994) On the stabilization of nonholonomic systems, Proceedings of the 33rd IEEE American Conference on Decision and Control, Piscataway, NJ, IEEE Press, Lake Buena Vista, FL, pp Baral, C, Kreinovich, V e Trejo, R () Computational complexity of planning and approximate planning in the presence of incompleteness, Artificial Intelligence 1(1): Blanco, J-L, González, J e Fernández-Madrigal, J-A (1) Optimal filtering for nonparametric observation models: Applications to localization and slam, International Journal of Robotics Research 9(14): Brockett, R W(198) New Directions in Applied Mathematics, Springer-Verlag, New York Kaelbling, L P, Cassandra, A R e Littman, M L (1998) Planning and acting in partially observable stochastic domains, Artificial Intelligence 11(1): Lages, W F (1998) Controle e Estimação de Posição e Orientação de Robôs Móveis, Tese de doutorado, Instituto Tecnológico de Aeronáutica, São José dos Campos, SP Orientador: Elder M Hemerly Lages, W F e Hemerly, E M (1998) Smooth time-invariant control of wheeled mobile robots, Proceedings of The XIII International Conference on Systems Science, Technical University of Wroc law, Wroc law, Poland Littman, M L, Cassandra, A R e Kaelbling, L P (1995) Efficient dynamic-programming updates in partially observable markov decision processes, Technical Report CS-95-19, Brown University, Providence, RI, USA Rekleitis, I M (4) A particle filter tutorial for mobile robot localization, Technical Report TR-CIM-4-, Centre for Intelligent Machines, McGill University, Montreal, Québec, Canada Sørdalen, O J (1993) Feedback Control of Nonholonomic Mobile Robots, Thesis (dr ing), The Norwegian Institute of Technology, Trondheim, Norway Thrun, S, Burgard, W e Fox, D (5) Probabilistic Robotics, MIT Press, Cambridge, MA