UNIVERSIDADEFEDERALDO RIO GRANDE DO NORTE
UNIVERSIDADEFEDERAL DORIOGRANDE DONORTE
PROGRAMA DEPÓS-GRADUAÇÃO EM
ENGENHARIAELÉTRICA E DECOMPUTAÇÃO
Navegação Cooperativa de um Robô
Humanóide e um Robô com Rodas usando
Informação Visual
Gutemberg Santos Santiago
Orientador: Prof. Dr. Adelardo Adelino Dantas de Medeiros
Dissertação de Mestrado apresentada ao
Programa de Pós-Graduação em Engenharia Elétrica e de Computação da UFRN (área de concentração: Engenharia de Computação) como parte dos requisitos para obtenção do título de Mestre em Ciências.
Divisão de Serviços Técnicos
Catalogação da Publicação na Fonte. UFRN / Biblioteca Central Zila Mamede
Santiago, Gutemberg Santos.
Navegação cooperativa de um robô humanóide e um robô com rodas usando informação visual / Gutemberg Santos Santiago - Natal, RN, 2008.
61. f.: il.
Orientador: Adelardo Adelino Dantas de Medeiros
Dissertação (Mestrado) - Universidade Federal do Rio Grande do Norte. Pro-grama de Pós-Graduação em Engenharia Elétrica e de Computação.
1. Robôs - Dissertação. 2. Cooperação multi-robôs - Dissertação. 3. SLAM visual - Dissertação. 4. Fusão sensorial - Dissertação. 5. Filtro de Kalman - Dissertação. 6. Calibração de câmera - Dissertação. 7. Transformadora de Hough - Dissertação. I. Medeiros, Adelardo Adelino Dantas. II. Título.
U
NIVERSIDADEF
EDERAL DOR
IOG
RANDE DON
ORTEP
ROGRAMA DEP
ÓS-G
RADUAÇÃO EME
NGENHARIAE
LÉTRICA E DEC
OMPUTAÇÃONavegação Cooperativa de um Robô
Humanóide e um Robô com Rodas usando
Informação Visual
Gutemberg Santos Santiago
Dissertação de Mestrado aprovada em 30 de maio de 2008 pela banca examinadora com-posta pelos seguintes membros:
Prof. Dr. Adelardo Adelino Dantas de Medeiros (orientador) . . . UFRN
Prof. Dr. Pablo Javier Alsina (examinador interno) . . . UFRN
Aos meus pais,
Agradecimentos
Agradeço a Deus pela constante presença, mostrando-me os caminhos da verdade e da vida.
Aos meus pais, pelo carinho, atenção, confiança, incentivo e por tudo que têm feito por mim.
Ao Professor Adelardo Medeiros pela sua orientação, paciência, ensinamentos, conselhos e pelo amadurecimento científico provido.
Ao Professor Pablo Alsina pelo apoio e presença em todos os momentos auxiliando o desenvolvimento do trabalho.
A Judson pelas horas de conversa e cumplicidade em todos esses anos de desenvolvimento científico e de vida.
A todos os meus colegas de laboratório. Em especial a André, Marcelo, Djalma, Len-nedy, Filipe, Ellon, Fábio, Márcio e Elber, que contribuíram diretamente na realização do trabalho.
Resumo
Este trabalho apresenta um sistema de navegação cooperativa de um robô humanóide e um robô com rodas usando informação visual, com o objetivo de efetuar a navega-ção do robô humanóide não instrumentado utilizando-se das informações obtidas do robô com rodas instrumentado. Apesar do humanóide não possuir sensores para sua navega-ção, pode ser remotamente controlado por sinal infravermelho. Assim, o robô com rodas pode controlar o humanóide posicionando-se atrás dele e, através de informação visual, localizá-lo e navegá-lo. A localização do robô com rodas é obtida fundindo-se informa-ções de odometria e detecção de marcos utilizando o filtro de Kalman estendido. Os mar-cos são detectados visualmente, e suas características são extraídas pelo o processamento da imagem. As informações das características da imagem são utilizadas diretamente no filtro de Kalman estendido. Assim, enquanto o robô com rodas localiza e navega o hu-manóide, realiza também sua localização e o mapeamento do ambiente simultaneamente (SLAM). A navegação é realizada através de algoritmos heurísticos baseados nos erros de pose entre a pose dos robôs e a pose desejada para cada robô. A principal contribui-ção desse trabalho foi a implementacontribui-ção de um sistema de navegacontribui-ção cooperativa entre dois robôs baseados em informação visual, que pode ser estendido para outras aplicações robóticas, dado a possibilidade de se controlar robôs sem interferir em seu hardware, ou acoplar dispositivos de comunicação.
Palavras-chave: Cooperação multi-robôs, SLAM visual, fusão sensorial, filtro de
Abstract
This work presents a cooperative navigation system of a humanoid robot and a wheeled robot using visual information, aiming to navigate the non-instrumented humanoid robot using information obtained from the instrumented wheeled robot. Despite the humanoid not having sensors to its navigation, it can be remotely controlled by infra-red signals. Thus, the wheeled robot can control the humanoid positioning itself behind him and, through visual information, find it and navigate it. The location of the wheeled robot is obtained merging information from odometers and from landmarks detection, using the Extended Kalman Filter. The marks are visually detected, and their features are ex-tracted by image processing. Parameters obtained by image processing are directly used in the Extended Kalman Filter. Thus, while the wheeled robot locates and navigates the humanoid, it also simultaneously calculates its own location and maps the environment (SLAM). The navigation is done through heuristic algorithms based on errors between the actual and desired pose for each robot. The main contribution of this work was the implementation of a cooperative navigation system for two robots based on visual in-formation, which can be extended to other robotic applications, as the ability to control robots without interfering on its hardware, or attaching communication devices.
Keywords: Cooperative robotics, visual SLAM, sensor fusion, Kalman filter, camera
Sumário
Sumário i
Lista de Figuras iii
Lista de Tabelas v
1 Introdução 1
1.1 Problema . . . 3
1.2 Objetivo . . . 3
1.3 Metodologia . . . 3
1.4 Sistemas Utilizados . . . 4
1.4.1 Robô com Rodas . . . 4
1.4.2 Robô Humanóide . . . 5
1.4.3 Plataforma Computacional . . . 6
1.5 SLAM Visual . . . 6
1.6 Organização do Texto . . . 8
2 Ferramentas 9 2.1 Filtro de Kalman . . . 9
2.2 Filtro de Kalman Estendido . . . 10
2.3 Transformada de Hough . . . 12
2.4 Calibração de Câmera . . . 13
2.5 Método de Zhang . . . 15
2.6 Método P4P . . . 19
3 SLAM Visual 23 3.1 Descrição do Sistema de SLAM . . . 23
3.2 Filtragem . . . 25
3.2.1 Filtro de Kalman Estendido . . . 25
3.2.2 EKF-SLAM . . . 26
3.3 Modelagem . . . 26
3.3.1 Fase de Predição: modelo do processo . . . 26
3.3.2 Fase de Atualização: modelo do sensor . . . 29
3.3.3 Correspondência . . . 32
3.4 Processamento da Imagem . . . 32
3.4.1 Detecção das Linhas . . . 32
3.4.2 De Imagens para o Mundo . . . 33
3.5 Resultados . . . 33
4 Navegação Cooperativa 37 4.1 Descrição do Sistema de Navegação . . . 37
4.2 Processamento da Imagem . . . 39
4.3 Determinação dos Pontos do Padrão de Calibração . . . 40
4.4 Determinação dos Vértices do Losango . . . 41
4.5 Movimentação Cooperativa . . . 43
4.6 Movimentação do Robô Humanóide . . . 45
4.7 Movimentação do Robô com Rodas . . . 47
4.8 Resultados . . . 48
5 Conclusões e Perspectivas 57
Lista de Figuras
1.1 RobôKarel . . . 5
1.2 RobôRobosapien . . . 6
2.1 Parâmetros de uma representação normal de uma retar . . . 12
2.2 Representação gráfica da transformada de Hough . . . 13
2.3 Problema da projeção 2D . . . 14
2.4 Transformação entre o referencial da câmera e o referencial da imagem . 15 3.1 Modelagem do sistema de SLAM para o robô com rodas . . . 24
3.2 Variáveis da cinemática do modelo . . . 27
3.3 Parâmetros da retaρeα . . . 30
3.4 Sistema de coordenadas fixo e móvel . . . 30
3.5 Detecção de linhas . . . 32
3.6 O robô Karel e o padrão de calibração . . . 34
3.7 Trajetória executada . . . 35
3.8 Linhas obtidas através do EKF-SLAM . . . 35
4.1 Modelagem do sistema de navegação cooperativa . . . 38
4.2 Padrão utilizado na calibração intrínseca da câmera . . . 41
4.3 Detecção de pontos pertencentes às arestas do losango . . . 42
4.4 Robô com rodas seguindo robô humanóide . . . 44
4.5 Restrição visual para o seguimento do robô humanóide . . . 45
4.6 Estados do sistema de navegação do humanóide. . . 46
4.7 Movimentação do humanóide . . . 47
4.8 Situação de ângulo crítico na movimentação da câmera . . . 48
4.9 Padrão utilizado no processo de calibração . . . 49
4.10 Detecção das retas e seus pontos de interseções . . . 49
4.11 Sistema de navegação cooperativa . . . 50
4.12 Imagem da obtenção dos quatro pontos do marco . . . 50 4.13 Interfacegráfica dosoftwarede gerenciamento do sistema de navegação . 50
4.14 Representação gráfica do robô humanóide e do robô com rodas . . . 52
4.15 Navegação cooperativa em linha reta . . . 52
4.16 Navegação cooperativa em linha reta: humanóide, posição x . . . 53
4.17 Navegação cooperativa em linha reta: humanóide, posição y . . . 53
4.18 Navegação cooperativa em linha reta: humanóide, posição angular . . . . 54
4.19 Navegação cooperativa em linha reta: robô com rodas, posição x . . . 54
4.20 Navegação cooperativa em linha reta: robô com rodas, posição y . . . 55
4.21 Navegação cooperativa em linha reta: robô com rodas, posição angular . . 55
Lista de Tabelas
3.1 Lista de símbolos para as equações do EKF . . . 25
3.2 Resultados experimentais do SLAM . . . 35
4.1 Detecção dos pontos pertencentes às arestas do losango . . . 42
4.2 Detecção das arestas do losango . . . 43
Capítulo 1
Introdução
“Aprendemos a voar como pássaros, e a nadar como peixes, mas não aprendemos a conviver como irmãos”.
Martin Luther King, (1929 - 1968)
A necessidade de tornar robôs cada vez mais capazes de interagir com o ambiente em que se encontram tem motivado o uso de informação visual. As imagens obtidas por câmeras acopladas a robôs são ricas de informação, que se bem extraídas provêem ao robô a capacidade de realização de tarefas que outros tipos de sensores não proveriam.
A informação visual pode fornecer ao robô dados para solução de problemas de forma a possibilitar que um robô realize diversas tarefas. Problemas clássicos da robótica como localização, navegação, reconhecimento de objetos, interface com humanos podem ser solucionados com o auxílio de um sistema de visão. Para o caso da localização e navega-ção de robôs, vários trabalhos científicos têm abordado o problema propondo diferentes soluções.
A localização de um robô consiste na capacidade do robô navegar por um determi-nado ambiente e poder responder pela sua posição em relação ao ambiente em todos os momentos [Borenstein & Feng 1996]. Esse problema é geralmente resolvido com a uti-lização das informações obtidas de diversos sensores, dado a dificuldade de obter uma informação sensorial precisa, por exemplo por problemas com a imprecisão da odometria do robô.
Além da dificuldade de obter informações sensoriais precisas, a localização de um robô é um processo caro dentre o processamento que um robô precisa executar, e que vai competir com recursos disponíveis na realização de uma tarefa. Então para que um robô móvel seja capaz de obter as informações de localização ele tem que estar equipado com diversos sensores e de uma capacidade de mínima de processamento de informação.
2 CAPÍTULO 1. INTRODUÇÃO
carga de instrumentação. Mas dependendo da aplicação em que o robô será utilizado, provê-lo de uma grande instrumentação inviabiliza os propósitos da aplicação em si. Por exemplo, é difícil instrumentar um robô que por características da aplicação tem que ser de tamanho reduzido.
A utilização de um sistema de localização relativa se configura como uma solução para o problema da localização de um robô não instrumentado, pois permite realizar a localização de um robô não instrumentado a partir da localização já sabida de um robô instrumentado. Essa idéia permite que o robô não instrumentado realize uma tarefa de-terminada sem ter a carga de processamento da localização que é realizada por um outro robô.
A localização relativa pode ser realizada utilizando-se a informação visual. Através da utilização de marcas visuais é possível determinar a relação existente entre a marca e o robô cuja localização já é conhecida. Se essa marca for parte constituinte do robô não instrumentado, é possível localizar o robô não instrumentado, baseando-se apenas na informação visual obtida.
Com as informações de localização de cada robô, o robô instrumentado tem a capa-cidade de navegar o robô não instrumentado através do envio de comandos para a sua movimentação. Assim os robôs podem realizar uma navegação cooperativa baseando-se nas informações visuais obtidas do robô não instrumentado e nos comandos gerados pelo robô instrumentado.
Na literatura existem diferentes casos que utilizam detecção de marcas e marcos na-turais para a realização da navegação de robôs. Jang et al. (2002) utilizaram como marco a figura de um quadrado dividido horizontalmente em dois retângulos coloridos. Launay et al. (2002) usaram as lâmpadas do teto de um corredor para localizar o robô. Jim et al. (2003) usaram uma câmera externa e determinaram, na imagem, a forma geométrica do robô. Bezerra (2004) utiliza como marcos naturais as linhas do piso que compõem o am-biente e identifica-as utilizando a transformada de Hough [Hough 1962]. Nogueira (2005) utiliza uma marca em formato de losango em um robô humanóide e localiza-o utilizando o Método P4P [Kamata et al. 1992]. Santana (2007) simulou um sistema de localização e um sistema de planejamento de caminho para um sistema robótico formado por um hu-manóide não instrumentado e um robô com rodas. O sistema de localização é baseado em filtro de Kalman e em informação visual. O sistema de planejamento de caminho é baseado no algoritmo A* [P. E. Hart & Raphael 1968].
1.1. PROBLEMA 3
localização absoluta do robô instrumentado.
As informações visuais obtidas de câmeras do robô instrumentado provêem a capa-cidade de realizar uma navegação cooperativa entre os dois robôs, de modo que o robô instrumentado navega o robô não instrumentado baseando-se nas informações visuais ob-tidas. Com a informação visual obtida dos marcos naturais o robô instrumentado obtém sua localização e realiza o mapeamento do ambiente simultaneamente (SLAM - Simulta-neous Localization and Mapping). Com a informação visual obtida da marca do robô não instrumentado, o robô instrumentado obtém a localização relativa do robô não instrumen-tado e realiza a navegação cooperativa dos dois robôs.
1.1
Problema
A utilização de robôs desprovidos de instrumentação é limitada. Robôs que por defini-ção de uso, concepdefini-ção e construdefini-ção são desenvolvidos embarcando-se pouca instrumen-tação tornam sua capacidade de realização de tarefas limitada. Esses robôs geralmente têm restringida a sua aplicabilidade e também a sua possibilidade de interagir com o am-biente, com outros dispositivos e robôs. A utilização de informações visuais de um robô não instrumentado permite sua interação com o ambiente e a realização de tarefas que não seriam possíveis apenas com a sua instrumentação embarcada.
1.2
Objetivo
O objetivo desse trabalho é navegar um robô humanóide não instrumentado, utilizando-se de um robô com rodas instrumentado, através de informações visuais, utilizando-sendo o robô com rodas capaz de se localizar e mapear simultaneamente o ambiente em que se locomove.
Os objetivos específicos são:
• realizar uma tarefa de SLAM com o robô com rodas a partir de informações visuais
obtidas de marcos do ambiente.
• navegar o robô não instrumentado a partir de movimentações básicas como seguir em linha reta e fazer curva.
1.3
Metodologia
mar-4 CAPÍTULO 1. INTRODUÇÃO
cos utilizados foram as linhas encontradas no chão que são definidas pelo encontro dos blocos utilizados no piso. Essas linhas são captadas por uma câmera e são encontradas utilizando-se a transformada de Hough. As linhas encontradas são transformadas por um processo de homografia de forma a corresponderem a distâncias e ângulos referentes ao robô. Essas linhas são comparadas com as linhas previamente encontradas tornando o robô capaz de se localizar e mapear o ambiente.
A metodologia utilizada para a tarefa de navegação cooperativa foi a utilização de uma marca no robô humanóide não instrumentado de modo que o robô com rodas ins-trumentado possa localizá-lo através da utilização de uma câmera. Quatro pontos são obtidos da marca de modo que, dessa forma, é possível calcular a posição do robô huma-nóide referente ao robô com rodas. O método utilizado nesse procedimento é o método de P4P [Kamata et al. 1992]. A realização do método do P4P é dependente da calibração intrínseca da câmera que é realizada pelo método de Zhang [Zhang 2000].
Assim o robô com rodas utiliza duas câmeras de modo que uma é utilizada no processo de SLAM e a outra é utilizada no processo da navegação cooperativa. O robô com rodas realiza a tarefa de cooperação de forma que a marca não é perdida, enquanto está sendo processada.
1.4
Sistemas Utilizados
No desenvolvimento do trabalho foram utilizados dois sistemas robóticos, um robô com rodas e um robô humanóide, e um sistema computacional para o acionamento do robô com rodas e processamento das imagens.
1.4.1
Robô com Rodas
O robô com rodas utilizado é o robô Karel1, desenvolvido no Laboratório de
Robó-tica da UFRN (Figura 1.1). Karel é um robô com duas rodas, acionamento diferencial, controlado por computador, equipado com duas câmeras Logitech QuickCam Pro 5000 [Logitech 2008] fixadas em um sistema de movimentação pan-tilt; e um dispositivo de comunicação infravermelho IRTrans USB [IRTrans 2008]. Os comandos para o aciona-mento e leitura de dados do robô são enviados via barraaciona-mento USB (Universal Serial Bus) para barramento CAN (Controller Area Network) interno. Na rede CAN estão conectados as placas de acionamento dos motores referentes a cada roda, e a placa para o acionamento
1Uma homenagem a Karel ˇCapek, um escritor Checo que primeiro usou publicamente a palavra “robô”
1.4. SISTEMAS UTILIZADOS 5
Figura 1.1: RobôKarel.
dos servo-motores do sistema de movimentação pan-tilt. Nas placas de acionamento dos motores estão embarcados controladores PI de velocidade. O controle embarcado é base-ado na informação de odometria, obtida pelosencodersfixados nas rodas.
O controle de posição do robô utiliza uma estratégia associada a uma mudança nas variáveis de controladas do robô, dex, yeθparas(representando o deslocamento linear do robô) eθ, e na representação em coordenadas polares do modelo cinemático do robô. O controle de posição do robô utiliza um controlador PI [Vieira 2005].
1.4.2
Robô Humanóide
O robô humanóide utilizado é o Robosapienda empresa WowWee [WowWee 2008] (Figura 1.2). Ele é acionado por controle remoto infravermelho e possui quatro formas de movimentação: andar para frente, andar para trás, girar para a direita e girar para a esquerda. Além disso, ele é capaz de movimentar os braços e controlar uma garra. O robô humanóide também é equipado com um conjunto de quatro sensores de toque (nas garras e nos pés), além de um sensor auditivo.
6 CAPÍTULO 1. INTRODUÇÃO
Figura 1.2: RobôRobosapien.
princípio biomórfico, o qual tem como objetivo imitar mecanismos, sensores, estruturas computacionais e metodologias utilizadas por animais.
1.4.3
Plataforma Computacional
O sistema de navegação cooperativa foi implementado na plataforma computacional Linux, utilizando-se a linguagem de programação C++. A comunicação com as câme-ras é realizada através da API de captura de vídeoVideo4Linux, através dodriver USB Video Class. O sistema de envio de comando infravermelho é realizado pelo dispositivo IRTrans através de uma conexão via socket com o servidor do dispositivo. O software
de gerenciamento do sistema de navegação utiliza SDL [SDL 2008] para sua interface gráfica.
1.5
SLAM Visual
1.5. SLAM VISUAL 7
esse mapa. A representação desses mapas pode ser feita de diversas formas, tais como grades de ocupação e mapas de características. Esse trabalho se interessa pela segunda representação.
Devido aos avanços da visão computacional e à diminuição de preço das câmeras, SLAM baseados em visão tem se tornado uma área bastante explorada [Gonçalves et al. 2005, Mouragnon et al. 2006, Jensfelt et al. 2006]. No trabalho de Folkesson et al. (2005), linhas e pontos são extraídos por processamento de imagem e usados para resolver pro-blemas de SLAM. Chen & Jagath (2006) propuseram um método de SLAM com duas fases. Primeiramente, a informação geométrica de alto nível, tais como linhas e triân-gulos, é incorporada ao filtro de Kalman estendido (EKF) para reforçar a robustez; após isso, uma abordagem de medição visual, baseada em uma geometria de múltiplas visões, é empregada para inicializar novos pontos característicos.
Uma abordagem clássica para o SLAM é usar o filtro de Kalman estendido (EKF) [Dissanayake et al. 2001, Castellanos et al. 2001]. Esses algoritmos de SLAM geral-mente requerem um modelo de sensor que descreva a observação esperada do robô dada sua posição. Davison (2003) usa uma câmera, assumindo um modelo de sensor de ruído gaussiano tal que a covariância é determinada pela resolução da imagem. Zucchelli & Košecká (2001) discutem como propagar a incerteza dos parâmetros intrínsecos da câ-mera na matriz de covariância que representa a característica ruidosa de posição no espaço 3D. Wu & Zhang (2007) apresentam um trabalho cujo foco principal é como modelar a incerteza do sensor e construir um modelo probabilístico de câmera.
Os principais desafios em SLAM visual são: a) como detectar características em ima-gens; b) como reconhecer que uma característica detectada é ou não a mesma que uma detectada previamente; c) como decidir se uma nova característica detectada será ou não adotada como uma nova marca; (d) como calcular a posição 3D de marcas a partir de imagens 2D; e (e) como estimar a incerteza associada com os valores calculados. Porém, em situações particulares, às vezes é possível desenvolver uma estratégia específica para superar essas dificuldades.
8 CAPÍTULO 1. INTRODUÇÃO
(uma homografia) entre o plano da imagem e o plano do chão, sem incertezas a respeito da informação 3D de profundidade dos pontos; e e) depois do processamento, o número de pixels na imagem que pertencem à linha é uma boa medida de confiança da marca detectada.
1.6
Organização do Texto
Capítulo 2
Ferramentas
“Se eu tenho visto mais adiante foi por estar sustentado no ombro de Gigantes”
Sir Isaac Newton, 1675
Nesse Capítulo é apresentada a base teórica das ferramentas utilizadas no desenvolvi-mento do trabalho. Dentre as ferramentas está o filtro de Kalman, utilizado para realizar a fusão dos dados de odometria e marcos do ambiente de modo a realizar o SLAM; a
transformada de Houghutilizada para encontrar na imagem as retas que serão utilizadas como marcos do ambiente; ométodo de Zhang, utilizado para encontrar a homografia e os parâmetros intrínsecos da câmera; e ométodo P4Putilizado para encontrar os parâmetros extrínsecos do sistema.
2.1
Filtro de Kalman
O filtro de Kalman é um processo de estimação recursiva ótimo para solução de problemas lineares relacionados à filtragem de dados, onde o erro quadrático é mini-mizado [Kalman 1960]. Através da observação da variável denominada “variável de observação” outra variável, não observável, denominada “variável de estado” pode ser estimada.
A modelagem tradicional do filtro de Kalman pressupõe que o sistema seja linear e descrito pelo modelo de equações do Sistema 2.1:
⎧ ⎨
⎩
st =Atst−1+Btut−1+γt
zt =Ctst+δt
(2.1)
onde s∈Rn é o vetor de estados; u∈Rl é o vetor das entradas de controle; z∈Rm é o
10 CAPÍTULO 2. FERRAMENTAS
matriz de coeficientes de entrada; a matriz C, m×n, é a matriz de observação; γ∈Rn
representa o vetor de ruídos do processo eδ∈Rmo vetor de erros de medição. Os índices
t et−1 representam os instantes de tempo atual e anterior respectivamente.
O filtro opera em modo de predição-atualização levando em consideração as proprie-dades estatísticas do ruído. Um modelo interno do sistema é usado para atualização e um esquema de realimentação realiza as medições. As etapas de predição e atualização para o filtro de Kalman podem ser descritas pelos Sistemas 2.2 e 2.3 respectivamente.
⎧ ⎨
⎩
¯
µt =Atµt−1+Btut−1
¯
Σt =AtΣt−1ATt +Rt
(2.2)
⎧ ⎪ ⎪ ⎪ ⎨
⎪ ⎪ ⎪ ⎩
Kt =Σ¯tCTt (CtΣ¯tCTt +Qt)−1
µt =µ¯t+Kt(zt−Ctµ¯t)
Σt = (I−KtCt)Σ¯t
(2.3)
O filtro de Kalman representa o vetor de estados st no tempo t por sua média µt e
covariânciaΣt. As matrizesR,n×n, eQ,l×l, são as matrizes de covariância dos ruídos
de processo (γ) e medição (δ) respectivamente e a matrizK,n×m, representa o ganho do
filtro.
A partir da especificação matemática, um algoritmo para o filtro de Kalman é apre-sentado na Listagem 1 [Thrun et al. 2005].
Algoritmo 1Filtro de Kalman. 01: FK(µt−1,Σt−1,ut,zt) 02:µ¯t=Atµt−1+Btut−1 03:Σ¯t =AtΣt−1ATt +Rt
04: Kt=Σ¯tCtT(CtΣ¯tCTt +Qt)−1 05:µt=µ¯t+Kt(zt−Ctµ¯t) 06:Σt = (I−KtCt)Σ¯t 07:return(µt,Σt)
2.2
Filtro de Kalman Estendido
2.2. FILTRO DE KALMAN ESTENDIDO 11
corrente usando as derivadas parciais do processo e das funções de medição para calcular as estimações, mesmo em face a relações não-lineares.
O modelo do sistema para o EKF é dado pelo Sistema 2.4:
⎧ ⎨
⎩
st =g(ut−1,st−1) +γt
zt =h(st) +δt
(2.4)
ondeg(ut−1,st−1)é uma função não-linear que representa o modelo do sistema, eh(st)é
uma função não-linear que representa o modelo das medições. Suas etapas de predição e atualização podem ser obtidas pelos Sistemas de Equações 2.5 e 2.6 respectivamente.
⎧ ⎨
⎩
¯
µt =g(ut−1,µt−1)
¯
Σt =GtΣt−1GTt +Rt
(2.5) ⎧ ⎪ ⎪ ⎪ ⎨ ⎪ ⎪ ⎪ ⎩
Kt =Σ¯tHTt (HtΣ¯tHTt +Qt)−1
µt =µ¯t+Kt(zt−h(µ¯t))
Σt = (I−KtHt)Σ¯t
(2.6)
A matrizG, n×n, é o jacobiano que lineariza o modelo, eH, l×n, o jacobiano que
lineariza o vetor de medições. Tais matrizes são definidas pelas Equações 2.7 e 2.8.
Gt= ∂g(ut−1,st−1)
∂st−1 (2.7)
Ht =∂h(st)
∂st (2.8)
Um algoritmo para o EKF é apresentado na Listagem 2 [Thrun et al. 2005].
Algoritmo 2Filtro de Kalman Estendido. 01: EKF(µt−1,Σt−1,ut,zt)
02: µ¯t =g(ut−1,µt−1) 03: Σ¯t=GtΣt−1GTt +Rt
12 CAPÍTULO 2. FERRAMENTAS
Figura 2.1: Parâmetros de uma representação normal de uma retar
2.3
Transformada de Hough
A transformada de Hough é um método utilizado para detectar, em uma imagem di-gital, uma classe de formas geométricas conhecidas, que podem ser representadas como uma curva paramétrica, como por exemplo, retas, círculos e elipses [Hough 1962]. A transformada mapeia pontos entre o espaço cartesiano e o espaço de parâmetros no qual a curva foi definida. Dessa maneira, os pontos que compõem a curva desejada são concen-trados no espaço de parâmetros de acordo com as características que unem tais pontos no espaço cartesiano.
Uma forma de parametrizar uma reta no plano X-Y é pela representação normal,
ρ=xcosθ+ysinθ, (2.9)
onde os parâmetrosρeθrepresentam, respectivamente, o comprimento de um vetor que passa pela origem do plano cartesiano e corta a reta com um ângulo de 90◦, e o ângulo
formado entre tal vetor e o eixo X (Figura 2.1).
Utilizando-se a transformada de Hough para detectar retas, a acumulação dos pontos no espaço de parâmetros informará os parâmetros que definem a reta, que passa por tais pontos no espaço cartesiano. Assim, para cada ponto classificado como pertencente à reta, calcular os parâmetros de todas as possíveis retas que passam por tal ponto.
2.4. CALIBRAÇÃO DE CÂMERA 13
Figura 2.2: Representação gráfica da transformada de Hough: (a) Pontos em espaço car-tesiano; (b) Transformada de Hough utilizando apresentação normal para os pontos A, B e C e sua interseção
parâmetros que identificam uma reta que une os pontos no espaço cartesiano (Figura 2.2). No caso de uma imagem digital, os pontos classificados como pertencentes à reta poderão não estar colineares, devido a ruídos. Desta forma, não será possível encontrar um único ponto de interseção entre todas as senóides no espaço de parâmetros. Deve-se então encontrar o ponto que contém o maior número de Deve-senóides, achando assim a equação da reta que comporta o maior número possível de pontos no espaço cartesiano.
Para a implementação do algoritmo, o espaço de parâmetros é discretizado. Se na discretização poucos valores forem fornecidos, o algoritmo se torna mais rápido, porém também se torna menos preciso. Portanto, para uma precisão aceitável, o algoritmo se torna complexo, exigindo portanto certo esforço computacional.
2.4
Calibração de Câmera
A calibração de câmera é o processo realizado para que seja possível a obtenção de informações métricas 3D a partir de uma imagem 2D [Zhang 2000]. Nesse processo é obtido um conjunto de parâmetros que são utilizados para converter as informações de uma imagem 2D em informações métricas 3D do ambiente. Esse processo pode ser dividido em duas etapas de aquisições de parâmetros:
• Determinação dos seus parâmetros intrínsecos
14 CAPÍTULO 2. FERRAMENTAS
Figura 2.3: Problema da projeção 2D
• Determinação dos seus parâmetros extrínsecos
Os parâmetros extrínsecos indicam a posição e a orientação da câmera com relação ao sistema de coordenadas do mundo.
Suponha{R}o sistema de coordenadas 3D, em centímetros, do mundo,{C}o sistema de coordenadas 3D, em centímetros, da câmera, {I} o sistema de coordenadas 2D, em centímetros, da imagem, e{F}o sistema de coordenadas 2D, em pixels, da imagem.
Suponha um ponto Pem relação ao sistema {R}, isto éRP= [ Rx Ry Rz ]T. Este
mesmo ponto pode ser expresso em relação ao sistema {C} como mostrado na Equa-ção 2.10, ondeCP= [ Cx Cy Cz ]T é um ponto em relação ao referencial da câmera e
CT
Ré a matriz de transformação entre{C}e{R}, ou a matriz de parâmetros extrínsecos.
CP 1T =CT R·
RP 1T (2.10)
Cada ponto no referencial do mundo tem uma representação no plano de projeção da imagem, como mostrado na Figura 2.3. Utilizando o modelo de câmera pinhole, a relação entre as coordenadas da imagemIP= [ Ix Iy ]T e as coordenadas da câmeraCPé mostrada nas Equações 2.11 e 2.12, ondeλé a distância focal da câmera em centímetros (Figura 2.4).
Ix= λ
CzCx (2.11)
Iy= λ
CzCy (2.12)
2.5. MÉTODO DE ZHANG 15
Figura 2.4: Transformação entre o referencial da câmera e o referencial da imagem
FP 1T =D·IP 1T (2.13)
Sendo FP = [ Fx Fy ]T um ponto no referencial {F}. A matriz D é a matriz de
conversão de centímetros para pixels, ou a matriz de parâmetros intrínsecos. A matrizD
possui o seguinte formato:
D=
⎡
⎢ ⎣
a b c
0 d e
0 0 1
⎤
⎥
⎦. (2.14)
A matriz de transformaçãoCTR da Equação 2.10 é matriz de parâmetros extrínsecos e
a matrizDda Equação 2.13 é a matriz de parâmetros intrínsecos.
2.5
Método de Zhang
O método de Zhang é utilizado para obter os parâmetros intrínsecos de uma câmera. Ele também pode ser utilizado para obter uma homografia [Zhang 2000].
16 CAPÍTULO 2. FERRAMENTAS
Utilizando as Equações 2.10, 2.11, 2.12 e 2.13, tem-se:
⎡ ⎢ ⎢ ⎢ ⎢ ⎣ Cz
λ 0 0
0 Cλz 0 0 0 Cz
0 0 1
⎤ ⎥ ⎥ ⎥ ⎥ ⎦
·D−1·
FP 1T =CT R·
RP 1T (2.15)
⎡ ⎢ ⎢ ⎢ ⎢ ⎣ Cz
λ 0 0
0 Cλz 0 0 0 Cz
0 0 1
⎤ ⎥ ⎥ ⎥ ⎥ ⎦
·D−1· F P 1 =
r1 r2 r3 t
0 0 0 1
· ⎡ ⎢ ⎢ ⎢ ⎢ ⎣ Rx Ry Rz 1 ⎤ ⎥ ⎥ ⎥ ⎥ ⎦ . (2.16) Se
D−1=
⎡
⎢ ⎣
a′ b′ c′
0 d′ e′
0 0 1
⎤
⎥
⎦, (2.17)
então se pode obter:
⎡ ⎢ ⎢ ⎢ ⎢ ⎣
Cza′
λ Cz
b′
λ Cz
c′
λ
0 Czd′
λ Cze
′
λ
0 0 Cz
0 0 1
⎤ ⎥ ⎥ ⎥ ⎥ ⎦ · F P 1 =
r1 r2 r3 t
0 0 0 1
· ⎡ ⎢ ⎢ ⎢ ⎢ ⎣ Rx Ry Rz 1 ⎤ ⎥ ⎥ ⎥ ⎥ ⎦ . (2.18)
Já que a equação fornecida pela última linha das matrizes não contém informação, tais linhas podem ser removidas:
⎡
⎢ ⎣
Cza′
λ Cz
b′
λ Cz
c′
λ
0 Czd′
λ Cze
′
λ
0 0 Cz
⎤ ⎥ ⎦· F P 1
= r1 r2 r3 t
· ⎡ ⎢ ⎢ ⎢ ⎢ ⎣ Rx Ry Rz 1 ⎤ ⎥ ⎥ ⎥ ⎥ ⎦ (2.19) Cz ⎡ ⎢ ⎣ a′ λ b ′ λ c ′ λ
0 dλ′ eλ′ 0 0 1
⎤ ⎥ ⎦· F P 1
= r1 r2 r3 t
2.5. MÉTODO DE ZHANG 17
Fazendo
A−1=
⎡ ⎢ ⎣ a′ λ b ′ λ c ′ λ
0 dλ′ eλ′ 0 0 1
⎤
⎥
⎦, (2.21)
tem-se que
CzA−1·
F P
1
= r1 r2 r3 t
· ⎡ ⎢ ⎢ ⎢ ⎢ ⎣ Rx Ry Rz 1 ⎤ ⎥ ⎥ ⎥ ⎥ ⎦ (2.22) Cz F P 1
=A· r1 r2 r3 t
· ⎡ ⎢ ⎢ ⎢ ⎢ ⎣ Rx Ry Rz 1 ⎤ ⎥ ⎥ ⎥ ⎥ ⎦ , (2.23)
ondeAé a matriz de parâmetros intrínsecos, dada por:
A=
⎡
⎢ ⎣
α γ Fx0
0 β Fy0
0 0 1
⎤
⎥
⎦, (2.24)
onde(Fx0,Fy0)são as coordenadas do centro de imagem, em pixels,α eβsão os fatores
de escala nos eixosxeyda imagem, respectivamente, eγé a distorção do ângulo formado pelos eixos.
Como o padrão adotado na calibração é plano, pode-se assumir sem perda de genera-lidade queRz=0, resultando em:
Cz
F P
1
=A· r1 r2 t
· ⎡ ⎢ ⎣ Rx Ry 1 ⎤ ⎥
⎦. (2.25)
Pode-se dizer, então, que um pontoRP′= [ Rx Ry 1 ]T e sua imagemFPse
relacio-nam pela homografiaH:
qFP 1T =HRP′ 1T ondeH=A·[ r
1 r2 t ]eq=Cz. (2.26)
18 CAPÍTULO 2. FERRAMENTAS
SendoH = [ h1 h2 h3 ], tem-se:
[ h1 h2 h3 ] =sA[ r1 r2 t ] (2.27)
ondesé um fator de escala arbitrário. Sabendo quer1er2são ortonormais, e sendoA−T
a abreviação para(A−1)T ou(AT)−1, pode-se deduzir que:
h1TA−TA−1h2=0 (2.28)
h1TA−TA−1h1=h2TA−TA−1h2 (2.29)
SejaB=A−TA−1(note que ela é uma matriz simétrica) representada pelo vetor
b= [ B11 B12 B22 B13 B23 B33 ]
e sejahi= [ hi1 hi2 hi3 ]T ai-ésima coluna de H, tem-se que
hiTBhj=vi jTb (2.30)
onde
vi j = [ hi1hj1 hi1hj2+hi2hj1 hi2hj2 hi3hj1+hi1hj3 hi3hj2+hi2hj3 hi3hj3 ]T
Dessa forma, pode-se reescrever as Equações 2.28 e 2.29 na seguinte forma:
v12T
(v11−v22)T
b=0 (2.31)
Portanto, observando nimagens do padrão, obtém-senequações do tipo 2.31, o que nos dá:
V b=0 (2.32)
ondeV é uma matriz 2nx 6.
2.6. MÉTODO P4P 19
Fx
0= BB12B13−B11B23
11B22−B122
s=B33−[B132+v0B(B1112B13−B11B23)]
α=Bs
11 β= sB11
B11B22−B122 γ= −B12α2β
s Fy
0= γvβ0 −B13α
2
s
(2.33)
2.6
Método P4P
O Método P4P (Perspective Four-Points problem) é utilizado para obter a matriz de parâmetros extrínsecos, ou seja, matriz que é utilizada para determinar a posição e orien-tação de um determinado objeto a partir das imagens, supondo que os parâmetros intrín-secos da câmera são conhecidos [Kamata et al. 1992]. Esse método tem como principal vantagem possuir uma única solução.
Suponha conhecer a posição de quatro pontos coplanares não alinhados relativos ao referencial{R}: RP
0aRP3, e os pontos 2D correspondentes na imagemFP0aFP3. Para
cal-cular a matrizCT
R foi introduzido dois sistemas de coordenadas intermediários, o sistema
{A}e o{B}, tal queCT
Ré dada por:
CT
R=CTBBTAATR (2.34)
Portanto, para determinarCT
R deve-se calcular as três matrizesCTB,BTAeATR.
Cálculo da Matriz
AT
REscolheu-se o sistema{A}para que os pontos notáveis estejam nastandard position:
RP
0 está na origem, RP1 está na parte positiva do eixo x e RP2 está no primeiro ou no
20 CAPÍTULO 2. FERRAMENTAS
significaR⊖i−R⊖j:
θ=
⎧ ⎨
⎩
0 se∆y10=∆x10=0
tan−1∆y
10
∆x10
caso contrário (2.35)
β=tan−1
−∆z10
∆x10cθ+∆y10sθ
(2.36)
α=tan−1
(∆x20cθ+∆y20sθ)sβ+∆z20cβ
−∆x20sθ+∆y20cθ
(2.37)
Os elementosai j da matriz ATR são calculados de acordo com as seguintes equações,
ondes⊙,c⊙e∆⊖i j, ⊙ ∈(α,β,θ), ⊖ ∈(x,y,z), significam sin(⊙), cos(⊙)eR⊖i−R⊖j,
respectivamente:
a11=cβcθ (2.38)
a12=cβsθ (2.39)
a13=−sβ (2.40)
a14=−Rx0cβcθ−Ry0cβsθ+Rz0sβ (2.41)
a21=−cαsθ+sαsβcθ (2.42)
a22=cαcθ+sαsβsθ (2.43)
a23=sαcβ (2.44)
a24=−Rx0(−cαsθ+sαsβcθ)−Ry0(cαcθ+sαsβsθ)−Rz0(sαcβ) (2.45)
a31=sαsθ+cαsβcθ (2.46)
a32=−sαcθ+cαsβsθ (2.47)
a33=cαcβ (2.48)
a34=−Rx0(sαsθ+cαsβcθ)−Ry0(−sαcθ+cαsβsθ)−Rz0(cαcβ) (2.49)
a41=a42=a43=0 (2.50)
a44=1 (2.51)
Cálculo da Matriz
CT
BO sistema{B}é escolhido de tal forma que se alinhe com uma câmera fictícia, a qual tem sua lente centrada na mesma posição que a câmera real, porém é orientada tal que
JP
2.6. MÉTODO P4P 21
x. Esta câmera fictícia se encontra emideal position. Utilizando a notaçãos⊙=sin(⊙)e
c⊙=cos(⊙),⊙ ∈(φ,ω,ρ), a matrizCTB é dada por:
φ=tan−1−Ix0
k
(2.52)
ω=tan−1
Iy
0cφ
k
(2.53)
ρ=tan−1Iy1cω+Ix1sφsω−kcφsω Ix1cφ+ksφ
(2.54) CT B= ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣
cφcρ+
sφsωsρ −csφsφωsρc+ρ sφcω 0
cωsρ cωcρ −sω 0
−sφcρ+
cφsωsρ csφφssωρ+cρ cφcω 0
0 0 0 1
⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ (2.55)
ondeké a distância focal em pixels.
Cálculo da Matriz
BT
AA matrizBTArepresenta a solução para um problema no qual os pontos notáveis estão
emstandard positione são observados por uma câmera emideal position. As coordena-das dos pontos notáveis emstandard position,AP
i, são calculadas aplicando-se a
transfor-madaAT
R aos pontos notáveis originais, RPi. As coordenadas destes pontos, na imagem,
vistos por uma câmera emideal position,JPi, são calculadas aplicando-se a transformada (CTB)−1[= (CTB)T] aos pontos 3D(Fxi,Fyi,k), obtendo-se os pontos (Bxi,Byi,Bzi), e
de-pois fazendoJxi=−kBxi/BzieJyi=−kByi/Bzi.
Devido à maneira pela qual foram definidos os sistemas {A} e {B}, os parâmetros
22 CAPÍTULO 2. FERRAMENTAS
alguns parâmetros intermediários:
b1=k(Ax2Ay3−Ax3Ay2) (2.56)
b2=Bx2Ax2Ay3−Bx3Ax3Ay2 (2.57)
b3= (Bx2−Bx3)Ay2Ay3 (2.58)
b4=Bx2Ay3−Bx3Ay2 (2.59)
b5=By2Ax2Ay3−By3Ax3Ay2 (2.60)
b6= (By2−By3)Ay2Ay3 (2.61)
b7=By2Ay3−By3Ay2 (2.62)
b8=kAx1(b4b6−b7b3)−Bx1b1b6 (2.63)
b9=Bx1[Ax1(b4b6−b7b3)−(b2b6−b5b3)] (2.64)
Finalmente, os coeficientes são dados por:
c11=
⎧ ⎨
⎩
+1/[1+ (b8/b9)2] sek/Bx1≤b8/b9
−1/[1+ (b8/b9)2] caso contrário
(2.65)
c31=−b8c11/b9 c34=−Ax1(kc11/Bx1+c31) (2.66)
c12= k Ax
2c11+Bx2Ax2c31+Bx2Ay2c32+Bx2c34
−kAy2 (2.67)
c32=
⎧ ⎨
⎩
−(b5c31+b7c34)/b6 seb6=0
−(b1c11+b2c31+b4c34)/b3 caso contrário
(2.68)
c22=−By2(Ax2c31+Ay2c32+c34)/(kAy2) (2.69)
c13=−c22c31 (2.70)
c23=c12c31−c11c32 (2.71)
c14=c21=c24=c41=c42=c43=0 (2.72)
c33=c11c22 (2.73)
Capítulo 3
SLAM Visual
“Inferir modelos a partir de observações e estudar suas proprie-dades é o que realmente faz a ciência. Os modelos (“hipóteses”, “leis da natureza”, “paradigmas”, etc.) podem apresentar um caráter mais ou menos formal, mas todos eles possuem uma mesma característica básica, que é a tentativa de encontrar algum padrão em observações.”
Ljung1
Nesse Capítulo é apresentada uma técnica para SLAM (Simultaneous Localization and Mapping) baseada no filtro de Kalman estendido (EKF) para navegar um robô em um ambienteindoorusando odometria e linhas pré-existentes no chão como marcos. As linhas são identificadas usando a transformada de Hough. A fase de predição do EKF é feita usando o modelo de odometria do robô. A fase de atualização usa diretamente os parâmetros das linhas detectados pela transformada de Hough sem cálculos adicionais intermediários. Um experimento de SLAM é realizado com o robô, e dados reais obtidos são apresentados2.
3.1
Descrição do Sistema de SLAM
O sistema de SLAM proposto é capaz de localizar um robô móvel em um ambiente onde as linhas do chão formam uma grade bidimensional. Para tal, as linhas são identifica-das como marcos naturais e suas características, juntamente com o modelo de odometria do robô, são incorporados em um filtro de Kalman a fim de obter sua pose (Figura 3.1).
Nesse sistema as arestas do piso são identificadas como marcos naturais utilizando a transformada de Hough. A fase de predição do filtro é implementada usando o modelo
1Trecho extraído de [Ljung 1987, p. 1].
2O conteúdo deste Capítulo foi submetido como artigo a ser julgado para publicação para o Congresso
24 CAPÍTULO 3. SLAM VISUAL
3.2. FILTRAGEM 25
de odometria do robô e a fase de atualização usa os parâmetros das linhas detectadas por transformada de Hough diretamente nas suas equações para corrigir a pose do robô.
3.2
Filtragem
3.2.1
Filtro de Kalman Estendido
O filtro de Kalman estendido trabalha com um modelo segundo o Sistema 3.1, cujas variáveis são descritas na Tabela 3.1. εt eδt são supostas ruído gaussiano de médio zero.
st =p(st−1,ut−1,εt−1)
zt =h(st) +δt
(3.1)
Em cada amostragem de tempo, o EKF calcula a melhor estimativa do vetor de estados em duas fases:
• a fase de prediçãousa o Sistema 3.2 para predizer o estado corrente baseado no
estado anterior e nos sinais de entrada aplicados;
• a fase de atualização usa o Sistema 3.3 para corrigir a predição do estado pela
verificação de sua compatibilidade com as medidas atuais dos sensores.
Tabela 3.1: Símbolos nas Equações (3.1), (3.2) and (3.3)
st vetor de estado (ordemn) no instantet
p(·) modelo não linear do sistema
ut−1 sinais de entrada (ordeml), instantet−1
εt−1 ruído de processo (ordemq), instantet−1
zt vetor de medições (ordemm) retornada pelos sensores
h(·) modelo não linear dos sensores
δt ruído de medição
¯
µt,µt média (ordemn) do vetor de estadost, antes e depois da fase de atualização
¯
Σt,Σt covariância (n×n) do vetor de estadosst
Gt matriz Jacobiana (n×n) que lineariza o modelo do sistemap(·) Vt matriz Jacobiana (n×q) que lineariza o ruído de processoεt Mt covariância (q×q) do ruído de processoεt
Kt ganho do filtro de Kalman (n×m)
26 CAPÍTULO 3. SLAM VISUAL
¯
µt=p(µt−1,ut−1,0)
¯
Σt=GtΣt−1GtT+VtMtVTt
(3.2) ⎧ ⎪ ⎪ ⎨ ⎪ ⎪ ⎩
Kt=Σ¯tHTt (HtΣ¯tHTt +Qt)−1
µt=µ¯t+Kt(zt−h(µ¯t))
Σt= (I−KtHt)Σ¯t
(3.3)
onde:
Gt= ∂p(s,u,ε)
∂s
s=µ
t−1,u=ut−1,ε=0
(3.4)
Vt= ∂p(s,u,ε)
∂ε
s=µt−1,u=ut−1,ε=0
(3.5)
Ht= ∂h(s)
∂s
s=µt−1
(3.6)
3.2.2
EKF-SLAM
Na tarefa de SLAM, além de estimar a pose do robô, também são estimadas as coor-denadas de todos os marcos encontrados pelo caminho. Assim, torna-se necessário incluir as coordenadas do marco no vetor de estado. Seicé o vetor de coordenadas doi-ésimo
marco e existemkmarcos, então o vetor de estados é:
st=
xt yt θt1cTt · · · kcTt
T
(3.7)
Quando o número de marcos (k) éa prioriconhecido, a dimensão do vetor de estados é estática; se não, ele cresce quando um novo marco é encontrado.
3.3
Modelagem
3.3.1
Fase de Predição: modelo do processo
Considere um robô com acionamento diferencial em que∆θR e∆θL são
3.3. MODELAGEM 27 ∆L y ∆θ x b r ∆θL ∆θR
Figura 3.2: Variáveis da cinemática do modelo
determinar o modelo geométrico cinemático do movimento do robô (Sistema 3.8):
⎧ ⎪ ⎪ ⎪ ⎪ ⎨ ⎪ ⎪ ⎪ ⎪ ⎩
xt=xt−1+∆L
∆θ[sin(θt−1+∆θ)−sin(θt−1)]
yt=yt−1−∆L
∆θ[cos(θt−1+∆θ)−cos(θt−1)] θt=θt−1+∆θ
(3.8)
na qual:
∆L= (∆θRrR+∆θLrL)/2
∆θ= (∆θRrR−∆θLrL)/b
(3.9)
onde ∆L e ∆θ são os deslocamentos linear e angular do robô; b representa a distância entre as rodas erR erL são os raios da roda direita e esquerda, respectivamente. Quando
∆θ→0, o modelo torna-se o da Equação 3.10, obtida do limite do Sistema 3.8.
⎧ ⎪ ⎪ ⎨ ⎪ ⎪ ⎩
xt=xt−1+∆Lcos(θt−1)
yt=yt−1+∆Lsin(θt−1)
θt=θt−1
(3.10)
Assim, o modelo utilizado é dependente do valor de ∆θ. Se ∆θ →0 utiliza-se a Equação 3.10, senão, utiliza-se a Equação 3.8.
Foi adotada a abordagem defendida por Thrun et al. (2005), que considera a informa-ção de odometria como sinais de entrada a serem incorporados ao modelo do robô, ao invés das medições sensoriais. As diferenças entre o deslocamento angular real das rodas (∆θRe∆θL) e os deslocamentos medidos pelosencoders(∆θ˜R e∆θ˜L) são modelados por
um ruído branco gaussiano de médio zero, de acordo com o Sistema 3.11.
∆θR=∆θ˜R+εR
∆θL=∆θ˜L+εL
28 CAPÍTULO 3. SLAM VISUAL
As medidas∆L˜ e∆θ˜ são definidas substituindo-se (∆θRe∆θL) por (∆θ˜Re∆θ˜L) nas
Equa-ções 3.9.
Se o vetor de estados sé dado pela Equação 3.7, o modelo do Sistema p(·)pode ser
obtido do Sistema 3.8 ou 3.10 e do fato que as coordenadas dos marcosicsão estáticas:
p(·) =
⎧ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎨ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎩
xt=· · ·
yt=· · ·
θt=· · · 1c
t=1ct−1
...
kc
t=kct−1
n=3+k·ordem(ic) (3.12)
ut =∆θ˜R∆θ˜LT l=2 (3.13)
εt = [εRεL]T q=2 (3.14)
As matrizesGeVsão obtidas derivando o modelop(·)3usando as Equações 3.4 and 3.5:
G= ⎛ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎝
1 0 g13 0 · · · 0
0 1 g23 0 · · · 0
0 0 1 0 · · · 0
0 0 0 1 · · · 0
... ... ... ... ...
0 0 0 0 · · · 1
⎞ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎠ (3.15) onde:
g13= ∆
˜
L
∆θ˜[cos(θt−1+∆θ˜)−cos(θt−1)]
g23= ∆
˜
L
∆θ˜[sin(θt−1+∆θ˜)−sin(θt−1)]
3Somente foram apresentadas as matrizesGe Vusando o modelo p(·) baseadas nas Equações 3.8.
3.3. MODELAGEM 29 V= ⎛ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎝
v11 v12
v21 v22
rR/b −rL/b
0 0 ... ... 0 0 ⎞ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎠ (3.16)
onde, considerandorR=rL=r:
v11 =V1cos(β) +V2[sin(β)−sin(θt−1)]
v12 =−V1cos(β) +V3[sin(β)−sin(θt−1)]
v21 =V1sin(β)−V2[cos(β)−cos(θt−1)]
v22 =−V1sin(β)−V3[cos(β)−cos(θt−1)]
β=θt−1+r(∆
˜
θR−∆θ˜L)
b V1=
r(∆θ˜R+∆θ˜L)
2(∆θ˜R−∆θ˜L)
V2= −b∆
˜
θL
(∆θ˜R−∆θ˜L)2 V3=
b∆θ˜R
(∆θ˜R−∆θ˜L)2
É sabido que a odometria introduz erros acumulativos. Portanto, o desvio padrão dos ruídosεR eεL é assumido ser proporcional ao módulo do deslocamento angular de cada
roda medido. Essas considerações levam à definição da matrizMdada pela Equação 3.17.
M=
"
(MR|∆θ˜R|)2 0
0 (ML|∆θ˜L|)2
#
(3.17)
3.3.2
Fase de Atualização: modelo do sensor
Os marcos adotados nesse trabalho são linhas formadas pela junção do piso do am-biente onde o robô navega. Os marcos são detectados processando as imagens usando transformada de Hough (Seção 3.4). As linhas detectadas são descritas por parâmetrosρ
eαna Equação 3.18:
ρ=xcos(α) +ysin(α) (3.18)
A Figura 3.3 mostra a representação geométrica desses parâmetros:ρé o módulo eα
é o ângulo do menor vetor que conecta a origem do sistema de coordenadas à linha. Definiu-se um sistema de coordenadas fixo (F) e um móvel (M), anexado ao robô, ambos ilustrados na Figura 3.4. A origem do sistema móvel tem coordenadas (xFM,yFM)
30 CAPÍTULO 3. SLAM VISUAL
Y
α ρ
X
Figura 3.3: Parâmetros da retaρeα
XF YF
F
xM
F M
θ
YM XM
F
yM
Figura 3.4: Sistema de coordenadas fixo e móvel
Deve-se notar que existe uma relação estreita entre essas variáveis (xF
M,yFM,θFM) e a pose
do robô (xt,yt,θt), que é dada pelas Equações 3.19.
xt=xFM yt=yMF θt=θFM+π/2 (3.19)
Cada linha no chão é descrita por dois parâmetros estáticos (ρF,αF). O mapa a
ser produzido pelo processo de SLAM é composto de um conjunto desses pares de pa-râmetros. Então, o ic vetor de coordenadas do i-ésimo marco que aparece nas Equa-ções 3.7 e 3.12 é dado pela Equação 3.20:
ic=
i
ρF
iαF
(3.20)
A cada passo o robô captura uma imagem e identifica os parâmetros (ρ˜,α˜)4 das li-nhas detectadas. Esses parâmetros de imagem são então convertidos para os parâmetros correspondentes(ρ˜M,α˜M)no sistema de coordenadas móvelManexado ao robô, usando
os parâmetros da câmera (Seção 3.4). O vetor de medições zt a ser usado na fase de
3.3. MODELAGEM 31
atualização do algoritmo do EKF (Equação 3.3) é definido pela Equação 3.215:
zt=
˜
ρM
˜
αM
(3.21)
Para usar a informação diretamente obtida pelo processamento de imagem(ρ˜M,α˜M)
na fase de atualização do EKF-SLAM, deve-se deduzir o modelo do sensorh(·), que é o
valor esperado desses parâmetros em função das variáveis de estado.
Usou-se a relação entre as coordenadas nos sistemasMeF (Sistema 3.22) e a Equa-ção 3.18 em ambos os sistemas de coordenada (Equações 3.23 e 3.24):
xF =cos(θMF)xM−sin(θMF)yM+xFM
yF =sin(θMF)xM+cos(θMF)yM+yFM (3.22)
iρF =xFcos(iαF) +yFsin(iαF) (3.23)
ρM=xMcos(αM) +yMsin(αM) (3.24)
Substituindo-se as Equações 3.22 na Equação 3.23, fazendo as equivalências neces-sárias com a Equação 3.24 e substituindo algumas variáveis usando as Equações 3.19, obtêm-se os Sistemas 3.25 e 3.26, que representam dois possíveis modelos de sensorh(·)
a ser usado no filtro. Para decidir qual modelo usar, calcula-se ambos os valores deαM e usa-se o modelo que gerar o valor mais próximo ao valor medido ˜αM.
ρM =iρF−xtcos(iαF)−ytsin(iαF)
αM =iαF−θt+π/2
(3.25)
ρM =−iρF+xtcos(iαF) +ytsin(iαF)
αM =iαF−θt−π/2
(3.26)
O modelo do sensor é incorporado no EKF através da matrizH(Equação 3.6), dada
pela Equação 3.276:
H=
"
−cosiαF −siniαF 0 · · · 1 0 · · ·
0 0 −1 · · · 0 1 · · ·
#
(3.27)
As colunas finais da matrizHsão quase todas nulas, exceto pelas colunas que
corres-5Para simplificar a notação, assumiu-se que existe exatamente uma linha por imagem. De fato, pode-se
ter nenhuma, uma, ou mais de uma linha por imagem. Em cada passo, executa-se a fase de atualização do EKF tantas vezes quantas o número de linhas detectadas na imagem, mesmo com nenhuma.
32 CAPÍTULO 3. SLAM VISUAL
Figura 3.5: Detecção de linhas
pondem ao marco no vetor de estado que corresponde à linha detectada na imagem.
3.3.3
Correspondência
Um aspecto crucial do algoritmo de SLAM é estabelecer uma correspondência entre a linha detectada na imagem e um dos marcos representado no vetor de estado. Para esco-lher o marco correto, primeiramente calculam-se os valores preditos de(ρF,αF)usando os valores medidos de(ρ˜M,α˜M)e o modelo nas Equações 3.25, se ˜αM ≥0, ou nas Equa-ções 3.26, se ˜αM<0. Então, esses valores preditos são comparados com cada um dos
va-lores(iρF,iαF)no vetor estado. Se a diferença entre o valor predito e o melhor(iρF,iαF)
é suficientemente pequena, a correspondência foi encontrada. Se não, considera-se que a nova marca foi detectada e o tamanho do vetor de estado é aumentado.
3.4
Processamento da Imagem
3.4.1
Detecção das Linhas
Devido à escolha das linhas do chão como marcos, a técnica adotada para identificá-las foi a transformada de Hough [Gonzalez & Woodes 2000]. O propósito dessa técnica é encontrar instâncias imperfeitas de objetos dentro de certa classe de formas por processo de votação. Esse processo de votação é efetuado no espaço de parâmetros, do qual objetos candidatos são obtidos como máximo local em uma grade de acumulação que é construída pelo algoritmo para a computação da transformada de Hough.
3.5. RESULTADOS 33
3.4.2
De Imagens para o Mundo
É assumido que o chão é plano e que a câmera é fixa. Então, existe uma relação constante (uma homografia A) entre os pontos no plano do chão (x,y) e os pontos no
plano da imagem(u,v):
s· ⎛ ⎜ ⎝ u v 1 ⎞ ⎟ ⎠=A·
⎛ ⎜ ⎝ x y 1 ⎞ ⎟ ⎠ (3.28)
O fator de escalasé determinado para cada ponto de tal forma que o valor do terceiro elemento do vetor seja sempre 1.
A homografia pode ser calculadaoff-lineusando um padrão contendo quatro ou mais pontos notáveis com coordenadas conhecidas (Figura 3.6). Depois de detectar os pontos notáveis na imagem, têm-se diversas correspondências entre coordenadas de pontos no sistema de coordenadas móvel M e na imagem. Substituindo-se esses pontos na Equa-ção 3.28, obtém-se um sistema linear no qual se pode determinar os oito elementos7 da
matriz de homografiaA.
Para obter as informações métricas das linhas obtidas no plano da imagem, utilizou-se o seguinte procedimento para cada linha:
1. usando os valores de(ρ˜,α˜)na imagem, obtidos pela transformada de Hough,
cal-cular dois pontos pertencentes à linha na imagem.
2. converter as coordenadas desses dois pontos para o sistema de coordenadas móvel
M, usando a homografiaA;
3. determinar(ρ˜M,α˜M)da linha que passa através desses dois pontos.
3.5
Resultados
Foi realizado um experimento em que o robô executa um movimento para frente, vira em torno de si mesmo, e retorna aproximadamente pelo mesmo caminho. Na Fi-gura 3.7 é indicado a posição das linhas, a trajetória aproximada do robô e o tamanho comparativo do campo de visão da câmera. A posição inicial do robô é aproximada-mente(3.3,3.1,−110o), mas não é usada no algoritmo de SLAM: o robô assume que sua posição inicial é(0,0,0o).
A Tabela 3.2 apresenta os marcos detectados após aproximadamente 250 iterações. As primeiras duas colunas contêm os valores reais. O resultado do algoritmo de SLAM está
7Apode somente ser determinada por um fator de escala. Para obter uma resposta única, foi imposto
34 CAPÍTULO 3. SLAM VISUAL
Figura 3.6: O robô Karel e o padrão de calibração
nas colunas do meio: os valores são expressos no sistema de coordenadas assumindo que a posição inicial do robô é (0,0,0o). Essas linhas são desenhadas na Figura 3.8. Final-mente, as duas últimas colunas mostram o resultado do SLAM, mas corrigidos assumindo a posição inicial do robô como(3.3,3.1,−110o), para melhor comparação (Figura 3.7).
A câmera captura imagens em tamanho 160×120 (como visto nas Figura 3.5) e cada
3.5. RESULTADOS 35
XF YF
1.08 2.16 3.24 0.98
1.96 2.94
Figura 3.7: Trajetória executada
Tabela 3.2: Resultados experimentais Real Calculado Corrigido
ρF αF ρF αF ρF αF
1.08 0 1.88 108 1.13 1.7 2.16 0 0.82 108 2.19 1.3 3.24 0 0.11 110 3.26 0 0.98 90 2.57 23 0.97 87 1.96 90 1.24 22 1.95 87 2.94 90 0.20 22 2.93 88
