Marcela Gonçalves dos Santos, Walter Fetter Lages Universidade Federal do Rio Grande do Sul - Departamento de Engenharia Elétrica

Transcrição

1 FUSÃO DE DADOS DE SENSORES PARA ESTIMAÇÃO DE POSIÇÃO E ORIENTAÇÃO DO APM AEROMÓVEL Marcela Gonçalves dos Santos, Walter Fetter Lages Universidade Federal do Rio Grande do Sul - Departamento de Engenharia Elétrica s: marcela@ece.ufrgs.br, fetter@ece.ufrgs.br Abstract APMs (Automated People Movers) are light vehicles used to move people in urban areas, implementing a concept similar to a horizontal elevator. The Aeromóvel system is an APM system where the vehicle is moved by air flow and requires its position to be known with a precision better then 10cm. In order to ensure such a precision, this papers proposes a data fusion system, integrating data from different sensors by using an extended Kalman filter. The system uses data from optical encoders in the train wheels and GPS data to estimate the vehicle position and orientation. Since the vehicle has two trucks, with four independent wheels each one, the proposed method groups the wheels in pairs. Each pair of wheels is used to compute an estimate of position and orientation that is fused with estimations from other pairs of wheels and the GPS receiver. Simulated results are presented to show the effectiveness of the approach. Keywords Automatic people mover, APM, Aeromóvel, Extended Kalman filter, Sensor fusion, Global positioning system, GPS, Localization. Resumo APMs (Automated People Movers) são veículos leves destinados a movimentar pessoas em ambientes urbanos, em um conceito semelhante a um elevador, porém movendo-se na horizontal. O sistema Aeromóvel é um sistema de APM onde o veículo é movido a ar e requer que a sua posição seja conhecida com precisão melhor do que 10cm. Afim de garantir essa precisão, este trabalho propõe um sistema que integra os dados de diferentes sensores através do filtro de Kalman Estendido. O sistema proposto utiliza os dados obtidos de encoders ópticos presentes nas rodas do trem juntamente com os dados de um receptor GPS para estimar a posição e orientação. Como o veículo possui dois conjuntos de eixos com quatro rodas independentes cada, a estratégia proposta aqui agrupa as rodas em pares. Cada par de rodas fornece uma estimativa da posição e orientação do veículo que é fundida juntamente com as estimativas obtidas pelas demais rodas e pelo GPS. Resultados simulados são apresentados afim de ser verificar a validade do sistema proposto. Palavras-chave Automatic people mover, APM, Aeromóvel, filtro de Kalman estendido, fusão de sensores, Sistema de Posicionamento Global, GPS, Localização. 1 Introdução O sistema Aeromóvel é um sistema de APM (Automated People Mover) onde o veículo é movido a ar e requer que a sua posição veículo seja conhecida com precisão melhor do que 10cm. O problema da localização de um veículo autônomo pode ser resumido como a capacidade do veículo de estimar sua posição tendo como informações uma representação do ambiente onde ele está inserido e a leitura de seus sensores (de Miranda e Ribeiro, 2006). A localização pode ser realizada de forma relativa e/ou absoluta (Borenstein et al., 1996). A localização relativa utiliza valores de posição e orientação obtidas em instantes anteriores para assim estimar valores atuais da localização do veículo em relação a uma localização inicial. Já a localização absoluta não necessita de valores calculados anteriormente e calcula a localização global do veículo. Pode-se citar a odometria como um método de localização relativa. Esse método está sujeito a erros que são acumulados ao longo da sua aplicação pois a posição atual depende da medição do instante anterior (Filho e Campos, 2002), tornando esse método ineficiente quando utilizado isoladamente. Um exemplo de localização absoluta é o GPS (Global Positioning System). Esse sistema permite obter a posição do veículo de forma absoluta em relação à Terra. Existe um grande número de trabalhos que utilizam GPS para estimação de posição e orientação de veículos autônomos (Rezaei e Sengupta, 2007),(Kao, 1991),(Hemerly e Schad, 2004),(Everett, 1995),(Roy et al., 1999). A precisão do GPS pode ser afetada por alguns erros que estão relacionados com as condições atmosféricas, a quantidade de prédios e árvores existentes na área na qual está se utilizando o sistema. Afim de garantir a precisão requerida, este trabalho propõe um sistema que integra os dados de diferentes sensores através do filtro de Kalman Estendido. O sistema proposto utiliza os dados obtidos de encoders ópticos presentes nas rodas do trem juntamente com os dados de um receptor GPS para estimar a posição e orientação. Como o veículo possui dois conjuntos de eixos com quatro rodas independentes cada, a estratégia proposta aqui agrupa as rodas em pares. Cada par de rodas fornece uma estimativa da posição e orientação do veículo que é fundida juntamente com as estimativas obtidas pelas demais rodas e pelo GPS. 2 Modelagem do Veículo Para se determinar a pose de um veículo autônomo é necessário a determinação de um modelo cinemático adequado. Um modelo cinemático é um

2 modelo que descreve o veículo em função da velocidade e orientação das rodas, considerando como estado apenas a posição e orientação do veículo. O modelo utilizado neste trabalho considera o veículo como sendo um corpo rígido dotado de rodas que não sofrem deformação e que movimentamse no plano horizontal e satisfazem à condição de rolamento sem deslizamento, ou seja, tanto as componentes de velocidade paralela e ortogonal ao plano a roda são nulas. Porém a condição de rolamento sem deslizamento não é satisfeita pelo carro modelado neste trabalho, a solução encontrada foi dividir as rodas em pares e obter a posição e orientação do carro através de cada par e por meio de odometria. Os dados obtidos por meio de odometria são integrados com a localização do veículo obtidas através do receptor GPS. Seja o veículo da Figura 1, deseja-se estabelecer seu modelo cinemático de postura. O modelo cinemático de postura descreve o veículo em função da velocidade e orientação das rodas, considerando como estado apenas a posição e orientação do veículo. que em notação matricial fica como demonstrado em (3). ẋ c cosθ c 0 ẏ c = senθ c 0 u (3) θ c 0 1 com u sendo o vetor definido de acordo com (4) e w o ruído de processo que segue uma distribuição gaussiana e possui média nula e variância conhecida. u = [ v ω (4) onde v é a velocidade do veículo na direção do seu eixo de simetria, denominada velocidade linear e ω a velocidade angular, ambas com relação ao sistema de coordenadas {X 0,Y 0 }. 3 Sistema Proposto É proposto um sistema de localização de veículos que faz uso da fusão de dados obtidos de um GPS e encoders. Os dados obtidos por esses sensores são integrados através do filtro de Kalman estendido. O veículo utilizado é o carro existente na linha piloto do sistema Aeromóvel da cidade de Porto Alegre - RS (Fig. 3). As rodas apresentam uma curvatura em sua interface de rolagem e são engastadas ao eixo com o objetivo de manter a estabilidade do trem em curvas. O sistema Aeromóvel utiliza rodas independentes, o que proporciona ao veículo a possibilidade de realizar curvas com raios muito menores que veículos com conjuntos de eixos tradicionais (Britto, 2008). Figura 1: Definição dos sistemas de coordenadas. (a) Veículo. (b) Linha piloto. A posição e orientação do veículo da Figura 1 são descritas por x = [ x c y c θ c (1) onde x c e y c são coordenadas do centro de massa do veículo e θ c é o ângulo de orientação do veículo. Além do sistema de coordenadas inercial é estabelecido o sistema de coordenadas {X c,y c } associado ao centro de massa do carro. A velocidade linear do veículo da Figura 1 pode ser escrita em termos da posição e orientação do mesmo, ou seja ẋ c = vcosθ c ẏ c = vsenθ c θ c = ω (2) Figura 2: Aeromóvel Porto Alegre (Cortesia Aeromóvel Brasil S.A.). 3.1 Odometria Uma das técnicas mais simples para estimação de posição (e orientação) consiste em contar-se a quantidade de pulsos obtidos a partir dos encoders de cada roda em um certo intervalo de tempo (Borenstein et al., 1997). Assumindo-se que não há escorregamento e conhecendo-se o raio das rodas (r) e a resolução do encoder que é o número de pulsos por volta dos encoders (N), pode-se calcular o deslocamento angular de cada roda pode ser obtido através de (5-6) ϕ d (k) = 2πr NP d(k) N (5)

3 ϕ e (k) = 2πr NP e(k) N (6) com ϕ d (k) e ϕ d (k) sendo o deslocamento linear e NP d (k) e NP e (k) o número de pulsos obtidos nos encoders das rodas direita e esquerda respectivamente, no intervalo de tempo entre kt e (k+1)t, onde T é o período de amostragem e k é o instante de amostragem. Neste trabalho as rodas foram separadas em pares, com uma roda localizada a direita (roda i, i = 1,3,5,7) e uma localizada a esquerda (roda j, j = 2,4,6,5.5), operando com acionamento diferencial. Um sistema de coordenadas para cada par de rodas {X ij,y ij } foi definido (Figura 3). A origem deste sistema pode ser escolhida de forma arbitrária, para este trabalho foi escolhido o ponto de intersecção entre o eixo de simetria e o eixo das rodas e aponta na direção do movimento da roda. Aplicando-se (9) ao modelo (3), obtém-se o vetor s. s ij = [ ẋij ẏ ij θij (10) onde (ẋ ij, ẏ ij ) são componentes da velocidade linear do carro na direção x e y e θ ij a velocidade angular do carro, ambas com relação ao sistema de coordenadas {X ij,y ij }. Substituindo-se (10) em (11). ẋ ij = s + ẐT sẑ c P ij (11) onde c P ij a posição de origem do sistema {X c,y c } em relação ao sistema {X ij,y ij } dada por c P ij = [l ij senβ i 0 0, Ẑ o versor da coordenada θ, que indica a orientação do carro e dado por [ e denota o produto vetorial. Substituindo-se (10) em (11) e integrando-se chega-se ao vetor x ij, que é a posição e orientação veículo estimadas pelo par de rodas ij. x ij = [ x cij y cij θ cij (12) onde x cij e y cij são coordenadas do centro de massa do veículo e θ cij o ângulo de orientação do veículo a partir do par de rodas ij, através de odometria. 4 Localização baseada em GPS Figura 3: Definição dos sistemas de coordenadas de cada par de roda. De posse do deslocamento angular da roda direita ( ϕ i ) e roda esquerda ( ϕ j ) em um dado intervalo de tempo é possível obter o deslocamento linear e angular do carro tendo como referência o ponto no centro do eixo axial das rodas (7). D(k) = ( ϕ i + ϕ j ) 2 ω(k) = ( ϕ i ϕ j ) b (7) com b sendo a distância axial entre as rodas. E a velocidade linear e angular tendo o mesmo ponto como referência dada por: v ij = D(k) T w ij = ω(k) T formando assim o vetor u ij (9) (8) u ij = [ v ij ω ij (9) O receptor GPS trabalha com o sistema de coordenadas geográficas,usadas para medir posição sobre a superfície da Terra. Nesse sistema as coordenadas são latitude e a longitude. De posse da latitude e longitude do ponto inicial e do ponto final de uma determinada trajetória é possível determinar a distância entre esses pontos e assim determinar sua posição e orientação. Existem duas maneiras de se realizar este cálculo: rhumb line navigation e great circle navigation. Para se exemplificar os métodos de cálculo da distância entre dois pontos tendo como dados a latitude e longitude de cada um deles, considere λ 1 e λ 2 a latitude do ponto inicial e do ponto final respectivamente e φ 1 e φ 2 a longitude do ponto inicial e do ponto final respectivamente. A rhumb line (linha loxodrômica) é uma linha que intercepta vários meridianos com um ângulo constante. Para se determinar a distância entre dois pontos através desse método utiliza-se as equações (13-14). onde Distancia AB = tan 1 (cosκ λ 2 λ 1 φ 2 φ 1 ) (13) κ = φ 1 + φ 2 (14) 2 Já o método great circle navigation faz uso da linha ortodrômica que é um segmento circular

4 na superfície terrestre que define a menor distância entre dois pontos. A determinação da distância entre dois pontos através deste método é feita através de (15-16) D = cos 1 (sin φ 1 sin φ 2 +cos φ 1 cos φ 2 cos(λ 1 λ 2 )) (15) Distancia AB = DR Terra π (16) 180 onde R Terra é o raio da Terra. A longitude geográfica é o ângulo medido ao longo do equador da Terra, tendo origem em um meridiano de referência (o meridiano de Greenwich), e extremidade no meridiano do lugar. Na Conferência Internacional Meridiana, realizada em Washington em outubro de 1884, foi definida como variando de 0 a 180 (Oeste de Greenwich) e de 0 a -180 (Leste). A latitude é o ângulo medido ao longo do meridiano do lugar, com origem no equador e extremidade no zênite do lugar. Zênite é o ponto superior da esfera celeste, segundo a perspectiva de um observador estacionado num plano sobre a Terra, o exato ponto acima de sua cabeça projetado na abóboda celeste, ou a intersecção da vertical superior do lugar com a esfera celeste. A latitude então pode variar entre -90 e 90. O sinal negativo indica latitudes do hemisfério sul e o sinal positivo hemisfério norte. As incertezas do sistema são modeladas pelo ruído do processo w(k), já v(k) é o ruído de medida. Esses ruídos possuem média nula e covariância conhecida e são descorrelacionados, além disso satisfazem as relações (19-21). E{w(k)w T (k)} = P w (k)δ(k i) (19) E{v(k)v T (k)} = P v (k)δ(k i) (20) E{w(k)v T (i)} = 0 (21) E{ } é o operador valor esperado e P w (k) e P v (k) são matrizes diagonais formadas pelas variâncias de w(k) e v(k), respectivamente. O EKF é implementado utilizando-se matrizes jacobianas de f e h que são obtidas através de linearização de tais funções em torno de uma estimativa do estado no instante k. Assim expandindo-se em série de Taylor as equações (17-18) em torno da estimativa do estado no instante k e desprezandose os termos de ordem superior tem-se (22-23). x(k +1) f(ˆx(k k), u(k))+f(k)[x(k) ˆx(k k)]+w(k) (22) y(k) h(ˆx(k k), u(k)) + H(k)[x(k) ˆx(k k)] + v(k) (23) onde F(k) = H(k) = f(x(k), u(k)) x(k) h(x(k), u(k)) x(k) x(k)=ˆx(k k) (24) x(k)=ˆx(k k) 4.1 Filtro de Kalman O filtro de Kalman (Kalman, 1960) é um estimador recursivo ótimo com relação a diversos critérios que calcula uma estimativa de variância mínima para um estado que evolui no tempo a partir de observações relacionadas linearmente com este estado. O problema de estimação utilizando-se o filtro de Kalman pode ser resumido da seguinte forma: deseja-se encontrar uma estimativa do vetor de estado (ˆx) dado o modelo do sistema, as condições iniciais do estado e da matriz de covariância dos ruídos a que está sujeito o processo e as medições, bem como uma sequência de entrada u e as medições y. A fusão de dados através do filtro de Kalman visa integrar a informação proveniente de vários sensores para assim construir uma estimativa mais precisa, mais robusta. Para sistemas não lineares utiliza-se o filtro de Kalman Estendido (EKF,Extended Kalman Filter). O EKF consiste em linearizar de forma analítica o sistema em torno da estimativa do estado. Seja o modelo não linear descrito por (17 e 18). x(k + 1) = f(x(k),u(k)) + w(k) (17) y(k) = h(x(k), u(k)) + v(k) (18) K(k) = P(k k 1)H T (H(k)P(k k 1)H T (k)+p v(k)) 1 (25) P(k k) = (I K(k)H(k))P(k k 1) (26) ˆx(k k) = ˆx(k k 1) + K(k)[y(k) h(ˆx(k k 1), u(k))] (27) ˆx(k + 1 k) = f(ˆx(k k), u(k)) + w(k) (28) P(k + 1 k) = F(k)P(k k)f T (k) + P w(k) (29) Assim tem-se uma maneira de se obter o estado estimado ˆx(k k) a partir de ˆx(k k 1) e da medida y(k) para um sistema não-linear como o descrito pelas equações (17-18). Onde a fusão de dados de sensores está justamente na utilização de informações provenientes de diferentes sensores para a formação do vetor y(k). 4.2 Integração dos sistemas Os sistemas que irão ser integrados para obtenção da pose do veículo é formado por encoders e um receptor GPS, a integração será realizada por meio do filtro de Kalman Estendido. Definidas essas equações para se realizar a fusão de dados via filtro de Kalman para este sistema, deve-se escrever o sistema de acordo com

5 as equações de Kalman, ou seja, definir as funções f(x(k),u(k)) e h(x(k),u(k)). Onde x(k) é o vetor de estado, y(k) é o vetor de observação e u(k) é a entrada do sistema, todos discretizados. Sendo x(k + 1) a discretização de x(t), tem-se x c (k + 1) = x c (k) + v(k)cosθ c (k)t + w 1 (k) (30) y c (k + 1) = y c (k) + v(k)senθ c (k)t + w 2 (k) (31) θ c (k + 1) = θ c (k) + ω(k)t + w 3 (k) (32) As expressões (30-32) podem ser escritas no espaço de estados na forma então e x(k + 1) = f(x(k),u(k)) + w(k) (33) f(x(k),u(k)) = w(k) = v(k)cosθ c (k) v(k)senθ c (k) ω w 1 (k) w 2 (k) w 3 (k) (34) (35) O vetor de observação y(k) pode ser descrito por y(k) = h(x(k),k) + v(k) (36) e h(x(k),u(k)) = v(k) = x c (k) y c (k) θ c (k) x c12 (k) y c12 (k) θ c12 (k) x c34 (k) y c34 (k) θ c34 (k) x c56 (k) y c56 (k) θ c56 (k) x c78 (k) y c78 (k) θ c78 (k) v GPS1 (k) v GPS2 (k) v GPS3 (k) v ODO1 (k) v ODO2 (k) v ODO3 (k) v ODO4 (k) v ODO5 (k) v ODO6 (k) v ODO7 (k) v ODO8 (k) v ODO9 (k) v ODO10 (k) v ODO11 (k) v ODO12 (k) v ODO13 (k) (37) (38) E as suas derivadas parciais com relação a estimativa de x (ˆx(k)). 0 0 vsenθ c F(k) = 0 0 vcosθ c (39) H(k) = 5 Resultados de Simulação (40) Utilizou-se um período de amostragem de 0.05s, durante um intervalo de 160s. O ruído de medida segue uma distribuição gaussiana, bem como o ruído de processo. Tendo como o vetor de entrada do sistema (41). Foram utilizados os parâmetros que podem ser vistos na Figura 4 e com r igual a m e b igual a 1.75 m foram determinados os valores de l, α e β de cada roda. Figura 4: Dimensões do carro. [ u(k) = [ π π ], k 80 ], k > 80. (41) O ruído de medida tem a covariância dada por P w =diag[0.8,0.8,4], já o ruído de medida v(t), tem a covariância dada por P v =diag[0.04, 0.04, 4, 2rπ/N p, 2rπ/N p, 4, 2rπ/N p, 2rπ/N p, 4, 2rπ/N p, 2rπ/N p, 4, 2rπ/N p, 2rπ/N p, 4], que contém o desvio padrão dos elementos do vetor de observação, com N p igual a 512.

6 O valor inicial da estimativa da posição e da orientação do veículo é dada pela tripla ([ ]) e o comportamento das estimativas podem ser vistos na Figuras (5a, 5b), na qual pode-se ver também o valor nominal da posição e orientação. 30 Trajetoria estimada baseada em técnica bayesiana para localização de robôs móveis, XIV Congresso Brasileiro de Automática 17: Everett, H. R. (1995). Sensors for mobile robots: theory and application, A. K. Peters, Ltd., Natick, MA, USA. Y[m] Nominal Odometros e GPS (Kalman) Filho, J. H. V. e 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 X[m] (a) Posição. Hemerly, E. M. e Schad, V. R. (2004). Sistema de navegação de baixo custo baseado na fusão ins/gps usando filtro de kalman, XV Congresso Brasileiro de Automática 1. Theta[rad] Orientacao estimada Nominal Odometros e GPS (Kalman) t[s] (b) Orientação. Figura 5: Posição e orientação estimadas. 6 Considerações Finais Neste trabalho foi feita a modelagem do veículo autônomo da Figura 1. Foi implementado um sistema de localização deste veículo. Se propôs realizar fusão de dados de sensores utilizados para localização de veículos, para o estudo apresentado neste trabalho os sensores foram encoders ópticos presentes nas rodas e receptores GPS. Para a fusão dos dados dos sensores utilizou-se o filtro de Kalman estendido. Kalman, R. E. (1960). A new approach to linear filtering and prediction problems, Transactions of the ASME Journal of Basic Engineering 82(Series D): Kao, W.-W. (1991). Integration of gps and deadreckoning navigation systems, Vehicle Navigation and Information Systems Conference, : Rezaei, S. e Sengupta, R. (2007). Kalman filterbased integration of dgps and vehicle sensors for localization, Control Systems Technology, IEEE Transactions on 15(6): Roy, N., Burgard, W., D. Fox, D. e Thrun, S. (1999). Coastal navigation-mobile robot navigation with uncertainty in dynamic environments, Robotics and Automation, Proceedings IEEE International Conference on 1: vol.1. Referências Borenstein, J., Everett, H. R. e Feng, L. (1996). Where am i? Sensors and methods for mobile robot positioning, University of Michigan. Borenstein, J., Everett, H. R., Feng, L. e Wehe, D. (1997). Mobile robot positioning: Sensors and techniques, Journal of Robotic Systems pp Britto, J. F. F. H. (2008). Modelo Computacional do sistema Aeromóvel de transportes, Dissertação (mestrado em engenharia mecânica), Universidade Federal do Rio Grande do Sul. de Miranda, F. L. N. e Ribeiro, C. H. C. (2006). Extração automática de mapas de atributos