SLAM para Cromos

$\newcommand{\deg}{^\mathrm{o}} \newcommand{\dpart}[2]{\frac{\mathrm{d} #1}{\mathrm{d} #2}} \newcommand{\J}{\mathrm{J}} \newcommand{\FN}[1]{\mathrm{#1}} \newcommand{\TR}{^{\top}} $ Uma tradução livre por Francisco Coelho do tutorial escrito pelos "Cromos" Søren Riisgaard e Morten Rufus Blas (disponível neste link).

(versão 0.1.2015-11-19 - tradução direta, sem gráficos)

Introdução

O objetivo deste documento é apresentar uma introdução tutorial à técnica de SLAM (Simultaneous Localization And Mapping) para robots móveis. Há imensa literatura sobre este assunto mas quem está a iniciar-se precisa de muitas horas de investigação para absorver os pormenores intrincados envolvidos na implementação do SLAM. Aqui espera-se apresentar este assunto de forma clara e concisa, minimizando os requisitos para acompanhar o texto. Deverá ser possível fazer uma implementação básica do SLAM depois de ter lido este tutorial.

O SLAM pode ser implementado de muitas formas. Primeiro, há a enorme variedade de hardware onde que pode ser usada. Segundo, o SLAM é mais um conceito que um algoritmo. Há muitos passos envolvidos e esses diferentes passos podem ser implementados com vários algoritmos diferentes. Em geral será explicada uma única abordagem mas serão sugeridas abordagens alternativas para leituras posteriores.

A motivação primária para escrever (e traduzir) este artigo é para ajudar os autores (e o tradutor) a melhor entender o SLAM. Ensinar um assunto melhora sempre o entendimento do mesmo. Em segundo lugar (quase) todos os artigos sobre SLAM são essencialmente teóricos e focados em inovações de pequenas áreas do SLAM — naturalmente é esse o seu objetivo. Este tutorial pretende ser muito prático e focado num algoritmo básico e simples de SLAM que possa ser usado como ponto inicial para se conhecer melhor esta técnica. Para aqueles com algum conhecimento básico em SLAM, apresentamos aqui uma solução completa que usa EKF (Extended Kalman Filter). Por completa não queremos dizer perfeita. Mas pretendemos cobrir todos os passos básicos necessários para se obter uma implementação a correr. Também deve ser notado que o problema do SLAM per se não foi ainda completamente resolvido e motiva (em 2005!) considerável investigação.

(NT: segue-se um parágrafo sobre código que é proporcionado no artigo original mas irrelevante para esta tradução)

Sobre o SLAM

O termo SLAM é um acrónimo de Simultaneous Localization And Mapping. Foi inicialmente desenvolvido por [Hugh Durrant-Whyte and John J. Leonard][7] com base no trabalho prévio de [Smith, Self and Cheeseman][6]. O SLAM trata o problema de construir um mapa de um ambiente desconhecido por um robot móvel e, simultaneamente, navegar nesse ambiente usando o mapa.

O SLAM consiste em várias partes: Extração (deteção?) de marcos geográficos, associação de dados, estimação do estado e atualização dos marcos. Há várias formas de resolver cada uma dessas partes. Vamos mostrar exemplos para cada parte. Isto também significa que algumas partes podem ser substituídas por abordagens novas. Como exemplo particular vamos resolver o problema de extração de marcos de duas formas diferentes e comentar esses métodos. A ideia é que o leitor possa usar a sua própria implementação e estendê-la com uma outra abordagem (a sua?) a esses problemas. Decidimos focar-nos num robot móvel num ambiente fechado (indoor). Pode optar por modificar os algoritmos de forma a serem usados num ambiente diferente.

O SLAM é aplicável a movimentos 2D e 3D. Vamos considerar apenas o caso 2D.

Seria útil se o leitor já tivesse algum conhecimento dos conceitos gerais do SLAM a um nível introdutório, eg através de um curso universitário sobre o assunto. Há imensas introduções excelentes a esta área de investigação incluindo [6] e [4]. Também seria útil saber um pouco sobre o Filtro Estendido de Kalman (EKF); algumas introduções são [3] e [5]. O conhecimento base é sempre útil pois permite entender mais facilmente este tutorial mas não é estritamente necessário para o compreender na totalidade.

O hardware

O hardware do robot é muito importante. Para fazer SLAM é necessário um robot móvel e um sensor de distância. Os robots móveis que consideramos são robots móveis com rodas para ambientes fechados. O foco deste documento é principalmente a implementação em software do SLAM e não explora robots com modelos de movimento (modelos de como o robot se move) complicados tais como robots humanóides, veículos autónomos submarinos, aviões autónomos, robots com configurações pouco usuais das rodas, etc. Aqui apresentamos alguns sensores básicos comummente usados para SLAM em robots móveis.

O robot

As características importantes a considerar num robot são a facilidade de uso, o desempenho da odometria e o preço. O desempenho da odometria mede como o robot pode estimar a sua própria posição apenas a partir do movimento das rodas. O robot não deve ter um erro superior a 2cm por metro percorrido e $2\deg$ por $45\deg$ rodados. As drivers comuns de robots proporcionam a posição $(x,y)$ num certo referencial cartesiano e também a atual direção do robot.

Há a opção de construir o robot de base. Isto pode consumir muito tempo, mas também é uma experiência de aprendizagem. Também é possível comprar robots prontos a usar, como o [Real World Interface] ou o robot [Evolution Robotics ER1][10]. De facto, o robot Rw1 já não é comercializado mas pode ser encontrado em muitos laboratórios de informática. No entanto este robot tinha uma odometria notavelmente má. O ER1 é o robot que os autores usaram. É pequeno e muito barato. Podias ser comprado por 250USD para uso académico e 300USD para uso privado. (NT: os autores também proporcionam drivers básicos no apêndice e no website)

Os sensores de distância

Usualmente o sensor de distância é um scanner laser. Estes aparelhos são muito precisos, eficientes e com o output facilmente processado. Por outro lado também são muito caros. Um scanner SICK custa cerca de 5000USD. Os scanners laser têm problemas com certas superfícies, incluindo vidro, onde dão leituras muito más. Além disso não podem ser usados debaixo de água devido à distorção e o alcance fica drasticamente reduzido.

Em segunda opção há o sonar, muito usado há alguns anos atrás pois eram muito baratos em comparação com os scanners laser. As suas leituras não são muito boas em comparação com os scanners laser e frequentemente dão más leituras. Enquanto que os scanners laser emitem com uma amplitude de cerca de $0.25\deg$, os sonares facilmente emitem em $30\deg$. No entanto em ambientes submarinos são a melhor escolha e assemelham-se à forma como os golfinhos navegam. O tipo normalmente usado é um sonar Polaroid, inicialmente desenvolvido para medir distâncias para tirar fotografias com câmaras Polariod. Os sonares foram usados com sucesso em [7].

A terceira opção é usar a visão. Tradicionalmente, usar a visão é computacionalmente muito exigente e também suscetível a erro devido a mudanças na luz. Numa sala sem luz um sistema de visão quase certamente não funcionará. Nos anos recentes, no entanto, têm havido alguns avanços interessantes nesta área. Frequentemente os sistemas usam montagens estéreo ou triclops para medir a distância. Usar a visão é semelhante à forma como os humanos vêm o mundo e pode ser intuitivamente mais apelativo que um sistema laser ou sonar. Além disso há muito mais informação numa imagem do que numa sondagem laser ou sonar. Este costumava ser o ponto de estrangulamento, mas com os avanços recentes nos algoritmos e na capacidade computacional tem-se tornado menos problemático. Medições de distância com base em visão foram usadas com sucesso em [8].

Escolhemos usar um [scanner laser da SICK][9]. É muito usado, não apresenta perigo para os olhos e tem propriedades interessantes para o SLAM. O erro da medida é $\pm 50mm$, o que pode parecer muito, mas na prática o erro é muito menor. Os scanners laser mais recentes têm erros inferiores a $\pm 5mm$.

O processo do SLAM

O processo do SLAM consiste num número de passos. O objetivo é usar o ambiente para atualizar a posição do robot. Como a odometria (que dá a posição) tem alguma margem de erro, não podemos confiar apenas na odometria. Podemos usar medições laser do ambiente para corrigir a (estimativa) da posição do robot. Isto resulta de extrair (detetar?) marcos do ambiente e fazer re-observações enquanto o robot se desloca. Um EKF (Extended Kalman Filter) é o núcleo do processo de SLAM, responsável por atualizar a estimativa da posição do robot com base nesses marcos, frequentemente designadas marcos geográficos e serão explicados juntamente com o EKF no próximo par de capítulos. O EKF mantém um registo da estimativa da incerteza sobre a posição do robot e também da incerteza sobre esses marcos como detetados no ambiente.

A figura seguinte ilustra o processo de SLAM.

                                        Leitura laser
                                            |
                                            V                       
        nova odometria                  Detetar marcos
            |                               |
            V                               V
    +-> EKF: atualizar odometria        Associar marcos
    |       |                               |
    |       V                               |
    |   EKF: re-observar        <-----------+
    |       |                               |
    |       V                               |
    |   EKF: novas observações  <-----------+
    |       |
    +-------+

    Figura 1: Esquema do processo de SLAM

Quando há uma odometria nova porque o robot se move, a incerteza sobre a nova posição do robot é atualizada no passo EKF: atualizar odometria. De seguida, é necessário detetar marcos no ambiente a partir da nova posição e associar marcos aos marcos previamente considerados. De seguida, o com EKF: re-observar os marcos previamente conhecidos agora contribuem para atualizar a posição do robot pelo EKF. Os marcos que não tinham sido previamente detetados são acrescentados em EKF: novas observações de forma a serem re-observados posteriormente.

Os diagramas seguinte pretendem explicar este processo em maior detalhe.

[ROBOT] -+- mede    --->    [marco #1]
         |
         +- mede    --->    [marco #2]
         |
         +- mede    --->    [marco #3]

Figura 2: Inicialmente o robot usa os sensores para localizar os marcos.

Posição inicial 
    |
    |
movimento
    |
    V
EKF: atualizar odometria: nova posição, estimada

Figura 3: O robot move-se e estima a nova posição com base na odometria.

[ROBOT] -+- mede    --->    [marco #1]
         |
         +- mede    --->    [marco #2]
         |
         +- mede    --->    [marco #3]
         |
         +- mede    --->    [marco #4]

Figura 4: De novo, o robot usa os sensores para localizar os marcos.
Porém estes podem não estar nas posições estimadas o que significa que
o robot pode não estar onde tinha estimado.

Posição inicial 
    |
    |
movimento
    |
    V
Nova posição estimada [EKF: atualizar odometria]
    |
    |
sensores
    |
    V
Nova Posição estimada e corrigida [EKF: re-observar]

Figura 5: Como os sensores têm menos erro que a odometria,
a informação sobre as posições dos marcos, 
ganha nas novas leituras é usada para 
corrigir a estimativa da localização do robot.

Posição inicial 
    |
    |
movimento
    |
    V
Nova posição estimada [EKF: atualizar odometria]
    |
    |
sensores
    |
    V
Nova Posição estimada e corrigida [EKF: re-observar]
    ~
    ~
Nova Posição exacta

Figura 6: De facto, o robot está numa posição diferente da posição inicial,
estimada pela odometria e da posição corrigida pelos sensores. Estes não são
perfeitos portanto não é possível determinar exatamente a posição.
No entanto a estimativa corrigida é melhor do que a inicial,
que depende apenas da odometria.

Dados laser

O primeiro passo no processo de SLAM consiste em obter dados da vizinhança do robot. Como escolhemos usar um scanner laser, obtivemos dados laser. O laser SICK que usámos produz medições de distâncias de um ângulo de $100\deg$ ou de $180\deg$. Tem uma resolução vertical de $0.25\deg, 0.5\deg$ ou $1.0\deg$, o que significa que a área que os raios laser medem tem amplitude respetiva. O resultado típico de uma leitura laser é

2.98, 2.99, 3.00, 3.01, 3.00, 3.49, 3.50, ..., 2.20, 8.17, 2.21

O resultado de uma leitura laser indica as distâncias, em metros, da direita para a esquerda. Se por alguma razão o scanner laser não poder indicar a distância exacta de um certo ângulo o resultado correspondente será um valor grande. Usámos $8.1$ como limiar de erro. Alguns scanners laser podem ser configurados para alcances superiores a $8.1$ metros. Por fim deve ser referido que os scanners laser são muito rápidos. COm uma porta série produzem resultados a cerca de $11Hz$.

Dados de odometria

Um aspeto importante do SLAM são os dados de odometria. O objetivo da odometria é proporcionar uma posição aproximada do robot a partir da medição do movimento das rodas do robot, para funcionar como aproximação inicial, no EKF, de onde o robot possa estar. É muito simples obter dados de odometria de um robot ER1 através do servidor telnet. Basta enviar uma string de texto para o servidor telnet numa porta específica e o servidor devolve a resposta.

A parte difícil acerca dos dados de odometria e de laser é obter os tempos corretos. Os dados laser a um certo instante $t$ podem ficar obsoletos de os dados de odometria forem obtidos (muito) mais tarde. Para garantir que estão corretos num certo instante é possível extrapolar os dados. É mais fácil extrapolar os dados de odometria porque os controlos são conhecidos enquanto que extrapolar os dados laser pode ser realmente difícil. Se for possível controlar quando as medições são devolvidas é mais simples pedir ambos os dados de odometria e de laser para o mesmo momento.

(NT: o código para a interface com o robot ER1 é omitida nesta tradução mas no documento original é mostrada no apêndice C)

Marcos geográficos

Marcos geográficos (ou apenas "marcos") são características que podem ser facilmente re-observadas e distintas do resto do ambiente. São usados pelo robot para descobrir onde está (para se localizar). Uma forma de pensar como isto funciona para o robot consiste em imaginar-se vendado. Se se deslocar desta forma pode chegar a, e tocar em, objetos ou acompanhar paredes de forma a não se perder. Coisas características tal como as ombreiras de uma porta podem ajudar a estimar onde está. Sonares e lasers correspondem ao sentido do toque nos robots.

Alguns exemplos de bons marcos geográficos:

• Um monumento ou construção única (como a ponte 25 de Abril) destacam-se e podem ser facilmente vistos em vários ambientes ou localizações em terra, no mar ou no ar;
• Os pilares de uma doca podem ser bons marcos para um veículo submarino;

Como se pode constatar, o tipo de marcos que um robot usa depende do ambiente em que opera.

Os marcos devem ser re-observáveis de forma a poderem ser vistos (detetados) de posições diferentes e portanto de ângulos diferentes. Também devem ser suficientemente distintos para não serem confundidos entre momentos consecutivos. Por outras palavras, se dois marcos forem posteriormente re-observados, deve ser simples decidir qual dos marcos foi previamente visto. Se forem semelhantes isto pode ser difícil.

A escolha dos marcos que o robot deve reconhecer deve permitir que existam suficientes para que o robot não fique muito tempo sem referências e perder-se.

Os marcos devem ser estacionários. Usar uma pessoa como marco é má ideia. A razão pare este critério é bastante direta. Se o marco não está sempre na mesma posição como é que o robot se pode posicionar dado esse marco?

Os pontos chave para marcos adequados são:

• Os marcos devem ser facilmente re-observáveis;
• Os marcos individuais devem ser distintos uns dos outros;
• Os marcos devem ser abundantes no ambiente;
• Os marcos devem ser estacionários;

Extração de marcos geográficos

Depois de escolhermos os marcos que o robot deve usar precisamos de um processo seguro para os extrair dos dados provenientes dos sensores.

Conforme mencionado na introdução, há várias formas de extrair os marcos que dependem dos tipos de marcos que se está a tentar detetar assim como dos sensores usados.

Vamos apresentar alguns algoritmos básicos de extração de marcos usando um scanner laser, Spikes e RANSAC

Marcos spike (pico)

O algoritmo de extracção de marcos spike usa extremos para encontrar marcos. A identificação de marcos encontra valores na distância da leitura laser onde dois valores diferem mais do que uma certa quantidade, eg $0.5 m$. Isto irá encontrar grandes diferenças na forma da leitura laser, por exemplo quando alguns raios laser são refletidos por uma parede e outros não atingem essa parede mas são refletidos por outras coisas mais afastadas.

Os picos também podem ser encontrados usando três valores próximos, $a, b$ e $c$, calculado $a+c-2b = (a-b) + (c-b)$. Este método é melhor para encontrar picos uma vez que deteta de facto picos e não apenas mudanças permanentes na distância.

Este método depende de mudanças significativas no ambiente para detetar um pico. Portanto não é um bom algoritmo para cenários suaves.

RANSAC

O RANSAC (Random Sampling Consensus) é um método que pode ser usado para extrair linhas de uma leitura laser. Por sua vez essas linhas podem ser usadas como marcos. Em ambientes fechados é frequente haverem linhas retas uma vez que são características de paredes.

O método RANSAC encontra esses marcos lineares a partir de uma amostra aleatória da leitura laser e usando mínimos quadrados para encontrar a melhor linha que passa por esses valores. De seguida verifica quantas leituras estão próximas dessa linha. Se esse número for superior a um certo limiar assume-se que foi detetada uma linha (e portanto um segmento de uma parede). Este limiar é o consenso.

O algoritmo abaixo resume o processo de extração de marcos lineares de um scanner laser com abertura $180\deg$ e uma media de distância por grau. Também assume que os dados da leitura são convertidos para um referencial cartesiano — ver Apêndice A. Inicialmente assume-se que nenhuma a leitura laser está associadas a uma linha. No algoritmo apenas são amostradas leituras não associadas.

while (len(readings) > 0  and len(readings) > C and num_try < N):
    #
    #   Select a random new reading
    #
    reading = random.choice( readings )
    #
    #   Select S readings close to D degrees
    #
    neigbours = select_near_deg(reading, D, S, readings )
    #
    #   Use the reading and neigbours to compute the best line using least squares method
    #
    A = ...
    b = ...
    best_approx, _, _, _ = np.linalg.lstsq(A,b)
    #
    #   Count readings closer than X centimeters of best_approx
    #
    close_neighbours = select_near_cm(best_approx, X, readings)
    if (len(close_neighbours) > C):
        #
        #   Improve approx using all close neighbours
        #
        A = ...
        b = ...
        best_approx, _, _, _ = np.linalg.lstsq(A,b)
        #
        #   Join best_approx to extracted lines
        #
        extracted.append( best_approx )
        #
        #   Remove close_neighbours from readings
        #
        readings.remove(close_neighbours)
    num_try += 1

Este algoritmo pode ser afinado com os seguintes parâmetros:

• N, número máximo de tentativas para encontrar linhas;
• S, número de amostras para calcular a linha inicial;
• D, graus de amplitude para obter amostras;
• X, distância máxima de associação de uma leitura a uma linha;
• C, número de pontos que devem estar numa linha para a qualificar como um marco;

A implementação do EKF assume que os marcos são representados em coordenadas polares $(r,\theta)$ centradas nas posições dos robots. A representação de uma linha $a$ por um ponto fixo pode ser feita escolhendo outro ponto fixo, $c$, nas coordenadas do mundo e encontrando o ponto, $b$ da linha $a$ mais próximo do ponto fixo $c$ (de forma que $b$ é o pé da perpendicular a $a$ que passa em $c$). Usando a posição do robot e a posição deste ponto fixo na linha é trivial encontrar a distância e o ângulo.

Usando trigonometria é fácil encontrar este ponto, como ilustrado na figura seguinte:

         ponto b ~ representa a linha a
--------*------------- linha a
        |
        |
        * ponto c, fixo

Outra possibilidade é modificar o EKF de forma a aceitar linhas em vez de pontos. No entanto esta forma é complexa e não tratada neste tutorial.

Múltiplas Estratégias

Apresentámos duas abordagens diferentes à deteção de marcos. Ambas detetam tipos diferentes de marcos e são adequadas para ambientes fechados. No entanto o método spikes é muito simplista e não é robusto em ambientes com pessoas porque, teoricamente, estas destacam-se do fundo.

Como o RANSAC deteta linhas não escolhe pessoas uma vez que estas não têm a forma caraterística de linhas.

Um terceiro método que não iremos explorar, scan-matching, tenta emparelhar duas leituras consecutivas e referimos aqui para quem possa estar interessado noutras abordagens.

(NT: os autores referem o apêndice D, não incluído aqui, para a implementação da deteção de marcos)

Associação de dados

O problema da associação de dados, também referido por re-observação de marcos, consiste em emparelhar marcos observados em diferentes leituras.

Para ilustrar o que isto significa damos um exemplo:

Nós, humanos, podemos considerar uma cadeira como um marco. Supondo que estamos numa sala e vemos uma cadeira específica. Agora saímos da sala e mais tarde voltamos a entrar. Se, então, virmos uma cadeira na sala e dissermos que é a mesma cadeira que tínhamos visto previamente então associámos esta cadeia à anterior. Isto pode parecer simples mas a associação de dados é difícil de fazer bem. Digamos que a sala tem duas cadeiras muito semelhantes. Quando entramos na sala pela segunda vez podemos não conseguir distinguir acertadamente qual das cadeiras vimos inicialmente (dado que são semelhantes). A nossa melhor aposta é dizer que a que está à esquerda é a que vimos antes à esquerda e a que está à direita a que vimos antes à direita.

Na prática podem ocorrer os seguintes problemas na associação de dados:

• não re-observar marcos em cada passo;
• observar algo como um marco mas não conseguir repetir a observação;
• associar erradamente um marco a outro previamente observado;

Como enunciado no capítulos sobre marcos geográficos, estes devem ser escolhidos de forma a serem facilmente re-observados. Portanto os dois primeiros casos correspondem a más escolhas. Mesmo com um bom algoritmo de deteção de marcos, pode-se colocar uma situação destas portanto é preferível definir uma política de associação de dados adequada, de forma a minimizar estes problemas.

O último problema, onde um marco é erradamente associado a outro, pode ser devastador pois significa que o robot irá considerar que está numa posição diferente daquela em que efetivamente se encontra.

Vamos agora definir uma política de associação de dados adequada para estes problemas. Assumimos que é montada uma base de dados para manter marcos previamente observados. Inicialmente esta base está vazia. A primeira regra que definimos é a de não considerar um marco adequado enquanto não for observado N vezes. Isto elimina os casos em que foi detetado um mau marco.

1. Assim que está disponível uma nova leitura laser são extraídos todos os marcos visíveis.

2. Cada marco detetado é associado ao marco mais próximo que foi visto mais do que N vezes.

3. Cada par (marco detetado, marco na base) é processado por um teste de validação:

    a. se o par passa a validação deve ser o mesmo marco que tem sido re-observado;
        nesse caso o contador desse marco é incrementado.

    b. se par falhar a validação este marco é adicionado à base;
        o número de observações é colocado a 1.

Esta técnica é conhecida como a abordagem do vizinho mais próximo (nearest-neighbor approach) uma vez que cada marco observado é associado ao marco da base mais próximo.

A forma mais simples de calcular o marco mais próximo é pela distância euclideana. Outros métodos incluem a distância de Mahalanobis e é melhor mas mais complicada. Não foi usada na nossa abordagem uma vez que os marcos RANSAC estão muito separados, o que os torna adequados para a distância euclidiana.

O teste de validação usa o facto que a nossa implementação EKF tem um limite na incerteza de uma observação como marco. Assim podemos determinar se um marco observado está na base testando se o marco está dentro da área de incerteza. Esta área pode ser desenhada graficamente e é conhecida como a elipse de erro. Fazendo $\lambda$ constante, uma marca geográfica observada é associada a uma marca da base se $$ \begin{align} \nu_i^T S_i^{-1}\nu_i \leq \lambda \end{align} $$ onde $\nu_i$ é a inovação e $S_i$ a covariância da inovação definida no capítulo EKF.

O EKF

O Filtro Estendido de Kalman (EKF) é usado para estimar o estado (posição) do robot a partir dos dados de odometria e observações dos marcos. O EKF é normalmente descrito apenas em termos de estimativa do estado (supondo que o robot tem um mapa perfeito). Isto é, não tem a atualização do mapa necessária quando aplicado a SLAM. Nesse caso a estimação EKF, especialmente as matrizes, são diferentes e pode ser difícil descobrir como implementar, um vez que quase nunca é mencionado. Aqui iremos passar por todos esses passos. Muito do EKF é normal, desde que as matrizes estejam definidas trata-se essencialmente de um conjunto de equações.

Perspetiva do processo

Assim que os passos de deteção de marcos e de associação de dados estejam completos o processo de SLAM pode ser descrito em três passos:

1. Atualizar a atual estimativa de estado com os dados de odometria;
2. Atualizar a estimativa com a re-observação dos marcos;
3. Juntar novos marcos ao estado atual;

O primeiro passo é muito simples. Trata-se apenas de aplicar os controlos do robot à anterior estimativa do estado. Por exemplo, se a pose do robot é $p = (x,y,\theta)$ e os controlos definem um movimento (relativo) $\delta p = (\delta x, \delta y, \delta\theta)$ a estimativa é $$ \begin{align} p + \delta p &= \left\{ \begin{matrix} \hat{x} &= x + \delta x\\ \hat{y} &= y + \delta y\\ \hat{\theta} &= \theta + \delta\theta \end{matrix} \right. \end{align} $$

No segundo passo são consideradas as re-observações dos marcos. Com a estimativa do estado atual é possível estimar onde os marcos devem estar. A diferença entre esta estimativa e as re-observações é designada inovação. Dito de outra forma, a inovação é o erro entre a estimativa e o valor observado do estado. No segundo passo também é atualizada a incerteza sobre cada marco observado de forma a refletir as alterações mais recentes. Por exemplo se a incerteza de um certo marco é muito pequena, a re-observação desse marco irá aumentar a certeza sobre o marco ie a variância do marco em relação ao estado atual do robot.

No terceiro passo são acrescentados novos marcos ao estado, o mapa que o robot tem do mundo. Isto faz-se usando a informação sobre a posição atual e juntado informação sobre a relação entre marcos novos e antigos.

As matrizes

Deve-se notar que existem muitas notações para as mesmas variáveis em artigos diferentes. Aqui usamos convenções razoavelmente comuns.

O estado do sistema: $X$

A matriz $X$, juntamente com a matriz de covariância, é provavelmente uma das matrizes mais importantes do sistema. Contém a pose $(x_r, y_r, \theta_r)$ do robot e as posições $(x_i,y_i)$ de cada marco: $$ \begin{align} X &= \begin{bmatrix} x_r\\ y_r\\ \theta_r\\ x_1\\ y_1\\ \vdots\\ x_n\\ y_n \end{bmatrix}. \end{align} $$

A representação do estado como uma matriz coluna é importante para as restantes equações. A dimensão desta matriz é $3 + 2\times n$ em que $n$ é o número de marcos. Normalmente os valores estão metros ou milímetros (e radianos). A escolha de uma grandeza ou outra é pouco importante, desde que consistente.

A matriz de covariância: $P$

Breve lembrete matemático: A covariância de duas variáveis proporciona uma medida da correlação entre essas variáveis. A correlação é mede o grau de dependência linear entre variáveis.

A matriz de covariância é central no EKF. Contém a covariância da posição do robot, a covariância entre as posições do robot e dos marcos e finalmente entre os marcos: $$ \begin{align} P &= \begin{bmatrix} A & E & \cdots & \cdots \\ D & B & \cdots & G \\ \cdots & \cdots & \cdots & \cdots \\ \cdots & F & \cdots & C \end{bmatrix}. \end{align} $$

A primeira sub-matriz, $A$, contém a covariância da pose do robot. É uma matriz $3\times 3$. A sub-matriz $B$, de dimensão $2\times 2$, tem a covariância da posição do primeiro marco. Continuando na diagonal a sub-matriz $C$ tem a covariância do último marco. A sub-matriz $D$ contém a covariância entre a pose do robot e o primeiro marco e $E = D^T$ contém a covariância contrária, entre o primeiro marco e a pose do robot. Analogamente $F$ tem a covariância entre o último e o primeiro marco enquanto que $G = F^T$ contém a covariância entre o primeiro e o último marco.

Portanto, embora a matriz de covariância possa parecer complicada de facto pode ser construída sistematicamente. Inicialmente o robot ainda não observou nenhum marco pelo que $P = A$, que deve ser inicializada com valores na diagonal que representem a incerteza na pose inicial do robot. Dependendo da implementação poderá ocorrer um erro de singularidade se a incerteza inicial não estiver incluída nos cálculos pelo que é mais seguro incluir algum erro na pose mesmo na crença da exatidão da pose inicial.

O ganho de Kalman: $K$

O ganho de Kalman é calculado para descobrir quanto iremos confiar nos marcos observados e quando queremos ganhar com o novo conhecimento que proporcionam. Por exemplo, se virmos que, de acordo com os marcos, o robot deve mover-se $10cm$ para a direita, usamos o ganho de Kalman para descobrir quanto devemos de facto corrigir a posição, que pode ser apenas $5cm$ porque não confiamos completamente nos marcos, encontrando um compromisso entre a odometria e as observações dos marcos. Isto resulta da incerteza entre os marcos observados juntamente com uma medida da qualidade dos sensores de distância e de odometria.

Se os sensores de distância forem fracos em relação à odometria não iremos confiar muito neles e o ganho de Kalman será pequeno. Se, pelo contrário, os sensores de distância forem significativamente melhores do que os de odometria o ganho de Kalman será grande.

A matriz de ganhos é $$ \begin{align} K &= \begin{bmatrix} x_d & x_\beta\\ y_d & y_\beta\\ \theta_d & \theta_\beta\\ x_{1,d} & x_{1,\beta}\\ y_{1,d} & y_{1,\beta}\\ \vdots & \vdots\\ x_{n,d} & x_{n,\beta}\\ y_{n,d} & y_{n,\beta} \end{bmatrix}. \end{align} $$

A primeira linha mostra o ganho com a inovação para a primeira linha do estado $X$. O valor $x_d$ indica quanto deve ser ganho com a inovação em termos de distância e $x_\beta$ indica o ganho com a inovação em termos de orientação. Note-se que estes valores dizem respeito à primeira componente do estado, a coordenada $x$ da pose do robot. A matriz continua de forma semelhante nas linhas seguintes; as três primeira linhas e os marcos, cada um com duas linhas. A dimensão de $K$ é $2 \times 3 + 2n$ onde $n$ é o número de marcos.

A jacobiana do modelo de observação: $H$

A jacobiana do modelo de observação está diretamente relacionada com o modelo de observação de que vamos tratar primeiro. Este modelo define como calcular uma observação esperada $(d,\beta)$ (distância, orientação) a partir das medições de um marco: $$ \begin{align} h(x_i,y_i ; x_r,y_r,\theta_r; \nu_d,\nu_\beta) &= \begin{bmatrix} d_i\\ \beta_i \end{bmatrix} = \begin{bmatrix} \sqrt{(x_i - x_r)^2 + (y_i - y_r)^2} + \nu_d\\ \mathrm{atan2}(y_i - y_r, x_i - x_r) - \theta_r + \nu_\beta \end{bmatrix} \end{align} $$ onde $x_i$ é a coordenada $x$ do marco $i$, $x_r$ é a estimativa atual da coordenada $x$ do robot, $y_i$ é a coordenada $y$ desse marco, $y_r$ é a estimativa atual da coordenada $y$ do robot e $\theta_r$ a orientação do robot. Esta expressão produz a distância e orientação $(d_i,\beta_i)$ estimadas do marco. A jacobiana desta matriz em relação a $x,y,\theta$ é $$ \begin{align} H &= \begin{bmatrix} \dpart{d_i}{x_r} & \dpart{d_i}{y_r} & \dpart{d_i}{\theta_r} \\ \dpart{\beta_i}{x_r} & \dpart{\beta_i}{y_r} & \dpart{\beta_i}{\theta_r} \\ \end{bmatrix} = \begin{bmatrix} \frac{x_r - x_i}{d_i} & \frac{y_r - y_i}{d_i} & 0 \\ \frac{y_i - y_r}{d_i^2} & \frac{x_i - x_r}{d_i^2} & -1 \end{bmatrix}. \end{align} $$

Esta matriz mostra a variação da distância e a orientação do marco $i$ em função da variação da pose $(x_r,y_r,\theta_r)$ do robot. O elemento $H_{1,1} = \dpart{d_i}{x_r}$ é a mudança em $d_i$ relativa à mudança em $x_r$. Os restantes elementos são calculados de forma semelhante. Este é a forma comum da matriz $H$ para estimativas EKF usuais. No processo de SLAM são necessários alguns valores adicionais sobre os marcos. No caso do segundo marco esses valores ficam arrumados por $$ \begin{align} H_2 &= \begin{bmatrix} H & C_1 & C_2 & C_3 \end{bmatrix} = \begin{array}{ccc|cc|cc|cc} X_d & Y_d & \Theta_d & X_1 & Y_1 & X_2 & Y_2 & X_3 & Y_3 \\ \hline a & b & c & 0 & 0 & -a & -b & 0 & 0 \\ d & e & f & 0 & 0 & -d & -e & 0 & 0 \end{array} \end{align} $$ onde a primeira linha $X_d~Y_d~\Theta_d~\cdots~$ é apenas informativa. As três primeiras colunas formam a matriz $H$ como definida acima. Para cada marco, excepto o segundo, são adicionadas duas colunas zero. Para o segundo marco as colunas são como indicadas. Para os restantes marcos a forma é semelhante.

A jacobiana do modelo de transição: $A$

Como a matriz $H$, também a jacobiana do modelo de transição está diretamente relacionada com o modelo de previsão, que vamos tratar primeiro. Este modelo indica como calcular a pose esperada do robot dada uma pose atual e o movimento de controlo: $$ \begin{align} f(d_u, \theta_u; x_r,y_r,\theta_r; q) &= \begin{bmatrix} x_r + (d_u + q) \cos\theta_r \\ y_r + (d_u + q) \sin\theta_r \\ \theta_r + (1 + q)\theta_u \end{bmatrix} \end{align} $$ onde $(x_r,y_r,\theta_r)$ é a pose do robot, $(d_u,\theta_u)$ é o controlo (distância, ângulo) e $q$ é o termo de erro.

Como a odometria do sistema ER1 proporciona diretamente a mudança de pose $(\delta x, \delta y, \delta \theta)$ o sistema de previsão fica $$ \begin{align} f(\delta x, \delta y, \delta \theta ; x_r, y_r, \theta_r; q) &= \begin{bmatrix} x_r + (1 + q) \delta x \\ y_r + (1 + q) \delta y \\ \theta_r + (1 + q)\delta \theta \end{bmatrix}. \end{align} $$

De qualquer forma para o cálculo da jacobiana $A$ usa-se a versão linearizada de $f$, que produz $$ \begin{align} A &= \begin{bmatrix} 1 & 0 & - d_u \sin\theta_r \\ 0 & 1 & d_u\cos\theta_r \\ 0 & 0 & 1 \end{bmatrix}. \end{align} $$

Os cálculos são semelhantes ao caso da matriz $H$, expecto que agora há mais uma linha para a orientação do robot. Como $A$ só é usada para estimar a pose do robot, não tem de ser estendida com informação sobre os marcos. Além disso, usando $-d_u \sin\theta_r = -\delta y$ e $-d_u\cos\theta_r = \delta x$ fica apenas $$ \begin{align} A &= \begin{bmatrix} 1 & 0 & -\delta y \\ 0 & 1 & \delta x \\ 0 & 0 & 1 \end{bmatrix}. \end{align} $$

As jacobianas específicas do SLAM: $\J_{xr}$ e $\J_z$

Algumas jacobianas são específicas ao SLAM devido ao passo de adição de novos marcos — o único passo que difere da estimação EKF comum. A primeira dessas matrizes jacobianas é $\J_{xr}$, que basicamente é a jacobiana do modelo de transição sem o termo de orientação, ie, trata-se da jacobiana para a transição dos marcos (que não inclui a orientação) com respeito à pose do robot $(x_r,y_r,\theta_r)$ em $X$: $$ \begin{align} \J_{xr} &= \begin{bmatrix} 1 & 0 & -d_u \sin\theta_r \\ 0 & 1 & d_u \cos\theta_r \end{bmatrix} \end{align} $$

A matriz $\J_z$ também é a jacobiana do modelo de transição dos marcos, mas com respeito à transição em coordenadas polares $(d_u, \theta_u)$: $$ \begin{align} \J_z &= \begin{bmatrix} \cos(\theta_r + \theta_u) & - d_u\sin(\theta_r + \theta_u) \\ \sin(\theta_r + \theta_u) & d_u\cos(\theta_r + \theta_u) \end{bmatrix} \end{align} $$

O ruído do processo: $Q$ e $W$

Supõe-se que o processo tem ruído gaussiano proporcional aos controlos $\delta x, \delta y, \delta \theta$, representado pela matriz $Q$ de dimensão $3 \times 3$, que é normalmente calculada pela multiplicação de uma amostra gaussiana $C$ da seguinte forma $$ \begin{align} W &= \begin{bmatrix} t_u \cos\theta & t_u \sin\theta & \delta\theta \end{bmatrix}^T \\ Q &= WCW^T = \begin{bmatrix} c \delta x^2 & 0 & 0\\ 0 & c \delta y^2 & 0\\ 0 & 0 & c \delta \theta ^2 \end{bmatrix} \end{align} $$ onde $C$ é uma representação da precisão da odometria. Estes valores devem ser afinados pelo desempenho da odometria do robot e normalmente é preferível obter dados experimentais para os encontrar.

O ruído da observações: $R$ e $V$

Também se supõe que o dispositivo de medida tem ruído gaussiano proporcional à distância e à orientação $d,\beta$, representado por $VRV^T$ em que $V$ e $R$ são matrizes $2 \times 2$: $$ \begin{align} V &= \mathrm{I}_2 \\ R &= \begin{bmatrix} p d & 0 \\ 0 & q\theta \end{bmatrix} \end{align} $$ onde $p$ e $q$ representam a precisão do dispositivo. Em geral não faz sentido que o erro na orientação seja proporcional à amplitude do ângulo.

Passo 1 (Previsão): Estimar o estado atual com dados de odometria

Neste passo, designado previsão, o estado é atualizado com os dados da odometria. Isto é, os controlos do robot são usado para estimar a nova pose.

A atualização da pose do robot é calculada por $$ \begin{align} \hat{p} &= \begin{bmatrix} x_r + (1+q)d_u \cos\theta_r \\ y_r + (1+q)d_u \sin\theta_r \\ \theta_r + (1+q)\beta_u \end{bmatrix} \end{align} $$ que, usando diretamente os valores da odometria, fica $$ \begin{align} \hat{p} &= \begin{bmatrix} x_r + \delta x \\ y_r + \delta y \\ \theta_r + \delta \theta \end{bmatrix} \end{align} $$

Além desta atualização parcial da matriz de estado $X$ também é necessário usar os valores $\delta x$ e $\delta y$ para atualizar...

• a matriz jacobiana do modelo de transição $$ \begin{align} A &= \begin{bmatrix} 1 & 0 & -\delta y \\ 0 & 1 & \delta x \\ 0 & 0 & 1 \end{bmatrix} \end{align} $$
• a matriz do ruído do processo $$ \begin{align} Q &= \begin{bmatrix} c\delta x^2 & c\delta x\delta y & c\delta x d_u \\ c\delta x \delta y & c\delta y^2 & c \delta y d_u \\ c \delta x d_u & c \delta y d_u & c d_u^2 \end{bmatrix} \end{align} $$
• a covariância da pose $$ \begin{align} P'_{1:3, 1:3} &= A P_{1:3,1:3} A + Q \end{align} $$
• e propagar a covariância com os marcos $$ \begin{align} P'_{1:3, :} &= A P_{1:3,:} \end{align} $$

Passo 2 (Correção): Estimar o estado atual com re-observações dos marcos geográficos

A estimativa do estado também pode ser atualizada com os dados de observação dos marcos. Supondo que os marcos foram recentemente observados e feita a associação de dados, pode-se estimar o movimento do robot em relação à atual estimativa da pose. Este processo é aplicado para cada marco re-observado e os marcos adicionados são tratados no passo 3.

Inicialmente tenta-se prever onde está o marco usando a estimativa atual da pose $(x_r, y_r, \theta_r)$ e a posição do marco $(x_i, y_i)$ calculando $$ \begin{align} h = \begin{bmatrix} d_i \\ \beta_i \end{bmatrix} &= \begin{bmatrix} \sqrt{(x_i - x_r)^2 + (y_i - y_r)^2}+\nu_d \\ \FN{atan2}(y_i - y_r, x_i - x_r) - \theta_r + \nu_\beta \end{bmatrix} \end{align} $$

Estes valores são comparados com os obtidos na observação desse marco, que denotamos por $z$. Para essa comparação é necessário usar a jacobiana $$ \begin{align} H_i &= \begin{bmatrix} a & b & c & 0 & \cdots & -a & -b & 0 & \cdots \\ d & e & f & 0 & \cdots & -d & -e & 0 & \cdots \end{bmatrix} \end{align} $$ com $$ \begin{align} a &= \frac{x_r - x_i}{d_i} & b &= \frac{y_r - y_i}{d_i} & c = 0 \\ d &= \frac{y_i - y_r}{d_i^2} & e &= \frac{x_i - x_r}{d_i^2} & f = -1 \end{align} $$ notando que esta alteração é aplicada apenas às colunas da pose do robot (as três primeiras) e do marco em questão (as colunas $3+2i$ e $3+2i+1$).

A matriz do ruído nas observações, $R$, também deve ser corrigida de forma a refletir a distância e a orientação da observação atual. Bons valores iniciais são $pd=0.01 d_i$, o que significa que há um erro de $1\%$ na distância e $q\theta = 1$, o que significa que há um erro de um grau na orientação: $$ \begin{align} R' &= \begin{bmatrix} 0.01d_i & 0 \\ 0 & 1 \end{bmatrix}. \end{align} $$

De seguida é necessário calcular o ganho de Kalman, com $$ \begin{align} K' &= PH\TR S^{-1} = PH\TR (H P H\TR + V R V\TR)^{-1} \end{align} $$ que indica quanto deve ser corrigida a posição dos marcos e a pose do robot. O termo $$ \begin{align} S &= H P H\TR + V R V\TR \end{align} $$ é a covariância da inovação, usado na associação de dados (e referido no respetivo capítulo).

Finalmente, a estimativa do estado é corrigida com o ganho de Kalman: $$ \begin{align} X' &= X + K(z-h). \end{align} $$

Passo 3: Acrescentar marcos geográficos ao estado atual

TODO

Observações finais

O método de SLAM apresentado aqui é muito elementar. Deixa muito espaço para melhorias e algumas áreas nem sequer foram abordadas. Por exemplo, o problema de "fechar o ciclo" (closing the loop) trata de detetar se o robot regressa a um local previamente visitado. Esta situação deve ser reconhecida e integrada na atualização do estado. Além disso a correção deve ser propagada ao caminho percorrido, atualizando os marcos. Este problema é tratado por exemplo, no sistema ATLAS [2].

Também é possível combinar este SLAM com uma grelha de ocupação, mapeando o mundo num formato legível por humanos. Além disso as grelhas de ocupação também podem ser usadas para planear percursos através de algoritmos A* e D* [1].

Referências

[1]: Koenig, Likhachev: Incremental A* (D*)

[2]: Bosse, Newman, Leonard, Soika, Feiten, Teller: An ATLAS framework

[3]: Roy: Foundations of state estimation (lecture):

[4]: Zunino: SLAM in realistic environments: [http://www.nada.kth.se/utbildning/forsk.utb/avhandlingar/lic/020220.pdf]

[5]: Welch, Bishop: An introduction to the Kalman Filter:

[6]: Smith, Self, Cheesman: Estimating uncertain spatial relationships in _robot_ics

[7]: Leonard, Durrant-Whyte: Mobile robot localization by tracking geometric beacons:

[8]: Se, Lowe, Little: Mobile robot Localization and Mapping using Scale-Invariant Visual Landmarks: [http://www.cs.ubc.ca/~se/papers/ijrr02.pdf]

[9]: SICK, industrial sensors: [http://www.sick.de]

[10]: Evolution _robot_ics [http://www.evolution.com]

Apêndice A: Conversão de coordenadas

Conversão geral de distancia $r$ e ângulo $\theta$ para coordenadas cartesianas $x,y$:

$$ \begin{align*} x &= r \cos \theta \\ y &= -r \sin \theta \end{align*} $$

Para converter uma observação dos sensores do robot para coordenadas cartesianas em que $\theta_o$ é o ângulo com que a observação é feita, $\theta_r$ é o ângulo do robot no referencial do mapa do mundo e $r$ é o valor reportado: $$ \begin{align*} x &= r \cos (\theta_o + \theta_r) \\ y &= -r \sin (\theta_o + \theta_r) \end{align*} $$