Desenvolvimento de um Sistema de Navegação Indoor por Imagens para um Veículo Aéreo não Tripulado do tipo Quadricóptero

INSTITUTO TECNOLÓGICO DE AERONÁUTICA

CONSELHO NACIONAL DE DESENVOLVIMENTO CIENTÍFICO E

TECNOLÓGICO - CNPq

CNPqCONSELHO NACIONAL DE DESENVOLVIMENTO

CIENTÍFICO E TECNOLÓGICO

PROGRAMA INSTITUCIONAL DE BOLSAS DE INICIAÇÃO CIENTÍFICA -

PIBIC

Desenvolvimento de um Sistema de

Navegação Indoor por Imagens para um

Veículo Aéreo não Tripulado do tipo

Quadricóptero

Roberto Brusnicki

RELATÓRIO FINAL DE ATIVIDADES

Orientador:

Dr. Davi Antônio dos Santos

08 / 2013


CONSELHO NACIONAL DE DESENVOLVIMENTO CIENTÍFICO E

TECNOLÓGICO - CNPq

CNPqCONSELHO NACIONAL DE DESENVOLVIMENTO

CIENTÍFICO E TECNOLÓGICO

PROGRAMA INSTITUCIONAL DE BOLSAS DE INICIAÇÃO CIENTÍFICA -

PIBIC

Relatório Final de Atividades

Desenvolvimento de um Sistema de

Navegação indoor por Imagens para um

Veículo Aéreo não Tripulado do tipo

Quadricóptero

São José dos Campos, 03 / 08 / 2013

Nome do aluno

Roberto Brusnicki

Assinatura do aluno

Nome do orientador Davi Antônio dos Santos

Assinatura do orientador


PROGRAMA INSTITUCIONAL DE BOLSAS DE INICIAÇÃO CIENTÍFICA - PIBIC

Formulário de Aprovação de Relatório pelo Orientador

Relatório: Rel. Parcial X Rel. Final

1- CONSIDERO O RELATÓRIO APROVADO COM BASE NOS SEGUINTES

ASPECTOS

2- APRECIAÇÕES DO ORIENTADOR SOBRE O DESEMPENHO DO BOLSISTA NA

EXECUÇÃO DO TRABALHO DE INICIAÇÃO CIENTÍFICA

Local e data:

Assinatura do Orientador:

ÍNDICE

1. INTRODUÇÃO .................................................................................................................... 4 2. ANDAMENTO E PLANEJAMENTO ........................................................................................... 6 2.1. RESUMO DO PLANO INICIAL ............................................................................................. 6 2.2. RESUMO DAS ATIVIDADES REALIZADAS ............................................................................... 7 3. DEFINIÇÃO DO PROBLEMA ................................................................................................... 9 4. FORMULAÇÃO DO FILTRO .................................................................................................. 10 4.1. ESTIMAÇÃO DE QUATÉRNIO ........................................................................................... 10 4.2. QEKF ....................................................................................................................... 13 4.2. OBTENÇÃO DOS VETORES UNITÁRIOS .............................................................................. 14 5. SIMULAÇÕES COMPUTACIONAIS ......................................................................................... 16 6. OBTENÇÃO DOS DADOS EM TEMPO REAL POR COMUNICAÇÃO SERIAL ........................................ 21 7. CONCLUSÕES .................................................................................................................. 22 8. AGRADECIMENTOS........................................................................................................... 23 9. BIBLIOGRAFIA ................................................................................................................. 23 CÓDIGOS DO FILTRO DE KALMAN PARA ESTIMAÇÃO DE QUATÉRNIO DE ROTAÇÃO ANEXO 1: (MATLAB) ............................................................................................................. 24 ANEXO 2: (C - ARDUINO)....................................................................................................... 27

1. INTRODUÇÃO

Este trabalho está envolvido com VANTS do tipo quadricóptero, que consiste

em veículo com capacidade de decolagem e aterrissagem verticais constituído de quatro

rotores acionados por motores elétricos individuais e independentes.

Para que um VANT quadricóptero tenha sua trajetória controlada em malha

fechada e de forma autônoma, faz-se necessário que esse veículo embarque um Sistema

de Navegação (SN), que consiste em um dispositivo capaz de estimar, em tempo real,

sua posição, sua velocidade linear, sua atitude (orientação) e sua velocidade angular. A

função de um sistema de controle por realimentação é essencialmente fazer com que

medidas/estimativas de variáveis dinâmicas (como atitude, posição e velocidade)

rastreiem trajetórias desejadas. Por essa razão, um controle confiável, acurado e robusto

requer um SN com as mesmas propriedades qualitativas. A importância de um SN para

o funcionamento adequado de VANTs autônomos motiva uma avaliação detalhada de

seus componentes e métodos.

Em particular, a chamada navegação inercial é a forma tradicional de se realizar

navegação por meio da integração das medidas dos sensores inerciais (acelerômetros e

girômetros) embarcados no veículo de interesse. A principal vantagem da navegação

inercial pura é sua independência em relação a sinais externos e a condições de

visibilidade e sua imunidade a interferências intencionais, como o jamming. Graças a

essas características, os sensores inerciais são bastante apropriados para operar em

ambientes como túneis, cavernas e interiores de prédios.

A despeito de sua independência em relação a sinais externos ao sistema, a

navegação inercial pura apresenta a significativa desvantagem de produzir estimativas

de posição, velocidade e atitude cujos erros divergem sem limites com o tempo. Essa é

uma característica bem conhecida, que é explicada, em breves palavras, pelo fato de que

junto com acelerações e velocidades angulares, o sistema também integra ao longo do

tempo os vieses e derivas dos sensores. De forma a limitar os erros divergentes de

sistemas de navegação inerciais puros, invoca-se frequentemente o uso de fusão de

sensores para permitir a combinação das medidas inerciais com aquelas providas por

outros sensores, como magnetômetros, sensores ultrassônicos, GPS, câmera, entre

outros.

Os métodos de fusão sensorial mais comuns são aqueles baseados no filtro de

Kalman, ferramenta que provê um framework para estimar variáveis dinâmicas

(estados) de um sistema a partir de sequências de medidas adquiridas de diversos

sensores.

A fusão de dados de GPS e sensores inerciais é um tópico importante e já bem

estudado. Os esquemas de fusão GPS-INS têm aplicações em diversas áreas, como

aviação, guiamento de satélites, aproximação e pouso automáticos de aeronaves,

aplicações marítimas e, em especial, em navegação de VANTs. O sucesso desses

esquemas se deve principalmente à complementaridade das características típicas dos

erros de medição de GPS e de sensores inerciais. A navegação baseada apenas na

integração de medidas dos sensores inerciais (navegação inercial pura) é precisa num

curto intervalo de tempo, no entanto, resulta em estimativas de posição que se degradam

sem limites devido à existência de vieses em acelerômetros e de derivas em girômetros.

Com base na estrutura do filtro de Kalman, medidas de sensores inerciais e de GPS

podem ser fundidas, segundo uma variedade de esquemas de implementação,

permitindo um desempenho de estimação (de posição, velocidade e atitude) superior a

ambos os subsistemas individualmente.

Infelizmente, a navegação inercial assistida por GPS pode carecer de

confiabilidade quando opera em lugares com linhas de visada deficientes em relação aos

satélites do sistema GPS. Exemplos de tais lugares são: interiores de prédios, cavernas,

minas, cânions urbanos e em florestas (entre árvores, por exemplo). De forma a

compensar essas limitações, o uso de câmera digital como auxílio à navegação tem

recentemente atraído o interesse de pesquisadores. A fusão INS-Câmera basicamente

consiste no uso de câmera(s) digital(is) para medir/rastrear a posição de features numa

sequência de imagens e, com base na estrutura do filtro de Kalman, usar tais

informações para estimar erros de navegação/erros de sensores e compensá-los nas

estimativas de posição, velocidade e atitude. Esses esquemas de fusão parecem ser

especialmente adequados para a navegação em ambientes indoor contendo landmarks,

que oferecem informações substancialmente ricas para a navegação do veículo.

O restante deste trabalho está organizado da seguinte maneira. O Capítulo 2

apresenta um resumo da proposta inicial, a descrição dos trabalhos realizados durante o

período 2012/2 e o planejamento para as próximas atividades. O Capítulo 3 define o

problema de controle de determinação de atitude e navegação usando sensores inerciais

e uma câmera embarcada apontada verticalmente para baixo. O Capítulo 4 apresenta a

formulação de um filtro de Kalman que soluciona o problema de determinação de

atitude. O Capítulo 5 apresenta alguns resultados de avaliações realizadas por

simulações computacionais. Por fim, o Capítulo 6 apresenta as conclusões da etapa

parcial a que se refere este relatório.

2. ANDAMENTO E PLANEJAMENTO

2.1. RESUMO DO PLANO INICIAL

Este trabalho tem como objetivo desenvolver um sistema de navegação de baixo

custo apropriado para voo indoor equipado com, dentre outros sensores, uma câmera, a

ser utilizado em um VANT (Veículo Aéreo Não Tripulado) de pequeno porte do tipo

quadricóptero (que consiste de um veículo com capacidade de decolagem e aterrissagem

verticais constituído de quatro rotores acionados por motores elétricos individuais e

independentes), que será projetado, fabricado e posteriormente avaliado

experimentalmente.

Para alcançar-se tal objetivo, faz-se necessário dotar o sistema de navegação a

ser desenvolvido para o VANT de vários sensores que por meio de medidas possibilite

o voo autônomo estável e controlado. Esse sistema deve ser equipado com um

acelerômetro de 3 eixos para determinação de acelerações lineares, um giroscópio de 3

eixos para determinação de velocidades angulares, um sensor ultrassônico para medição

de altitude, e de uma câmera para obtenção de imagens.

A Metodologia adotada consiste em inicialmente construir-se um quadricóptero

elétrico utilizando componentes de aeromodelismo, e verificar seu funcionamento

através de testes rádio controlados. Dado isto, integrar ao quadricóptero a parte física do

sistema de navegação a ser desenvolvido, ou seja, uma plataforma em que se encontrem

os sensores supracitados. Posteriormente, o desenvolvimento do software do sistema de

navegação, o qual consiste basicamente de um filtro de Kalman para fusão dos sensores

inerciais, sensor ultrassônico de altitude e a câmera. Este último passo possui implícito

todo o estudo a cerca de processos estocásticos, bem como o estudo do filtro de

Kalman, e da física do problema, assim como simulação de seu funcionamento

anteriormente a sua utilização no VANT propriamente dito.

2.2. RESUMO DAS ATIVIDADES REALIZADAS

1) Construção

Inicialmente construiu-se o quadricóptero com peças de aeromodelismo. O que

se consiste basicamente do frame em formato de cruz, os 4 motores elétricos do tipo

brushless, suas respectivas hélices, e o sistema de alimentação, constituído pelos

eletronic speed controllers (ESC’s) cabos de alimentação e da bateria de polímero de

Lítio. Além disto, tal quadricóptero possui também um Arduíno Mega como micro

controlador utilizado para seu sistema de navegação. O software utilizado para fazer a

estabilização de atitude consiste do Aeroquad open-sourse.

Esta primeira fase permitiu a familiarização com o funcionamento deste modelo

relativamente novo de VANTs, sem que houvesse excessiva preocupação com eventuais

danos causados aos seus componentes devido a um mau controle de seu voo, pois não

estaria voando de maneira autônoma, mas sim de maneira radio-controlada.

2) Embasamento Teórico

Com a finalidade de se adquirir o embasamento matemático necessário para a

realização deste trabalho, foram estudados os seguintes tópicos:

Variáveis aleatórias e processos estocásticos. Parte dessa preparação foi

realizada através da disciplina de graduação MOQ-13 (Probabilidade e

Estatística);

Filtro de Kalman. Foram estudadas diversas formulações do filtro de

Kalman: discreta-discreta, contínua-discreta tando para o filtro linear

quanto para o filtro de Kalman estendido. As referências utilizadas foram

(Gelb, 1974) [1] e (Hwang e Brown, 1997) [2];

Cinemática de atitude. Foram estudadas as diversas representações de

atitude bem como as respectivas equações diferenciais que modelam a

dinâmica de atitude em função das velocidades angulares do veículo ao

longo do tempo. Existem várias possibilidades de parametrização de

atitude, dentre elas, as mais recorrentes são: ângulos de Euler, matriz de

cossenos diretores (DCM), quaternion de rotação e parâmetros

modificados de Rodrigues. A referência adotada foi (Shuster, 1993) [4].

3) Simulações computacionais

Adotou-se o MATLAB como ambiente de simulação. Foram simuladas a

cinemática de atitude do veículo bem como as medidas dos sensores, que são uma

câmera embarcada com apontamento vertical voltado para baixo e uma tríade de

girômetros. Nesse mesmo ambiente, implementou-se também as equações de um filtro

de Kalman estendido discreto-discreto para estimação da atitude representada por

quaternion de rotação. Foram realizados diversos testes que possibilitaram uma boa

sintonia do filtro e comprovaram sua capacidade de convergência e robustez.

4) Familiarização com os sensores utilizados

Adotou-se o uso do sensor GY-521 para obtenção das medidas de velocidade

angular, e a CMUCam4 como sensor de rastreamento de cores para obtenção dos

vetores unitários indicados na figura 3. Ambos os sensores escolhidos atendem ao

proposto pelo projeto, sendo sensores de baixo custo, mas que com sistema de

navegação desenvolvido fornecem dados bom o suficiente para se fazer a estimação da

atitude do quadricóptero com alta precisão. Uma das vantagens de se ter trabalho com o

sensor GY-521, é que este também possui um acelerômetro de 3 eixos, sendo estas

medidas necessárias em um trabalho posterior em que utilizará o sistema de navegação

em movimento. A CMUCam4 consiste em um sensor de visão computacional

programável, que pode ser utilizado como rastreador de cor, entre inúmeras outras

funções. Ambos os sensores foram utilizados neste projeto em conjunto com o

microprocessador Arduino Mega, tendo sido necessário um tempo de estudo de suas

bibliotecas, funções, configurações, e utilização de modo a otimizar a obtenção dos

dados necessários.

Figura 1: (a) Sensor de rastreamento de cores CMUCam4. (b) IMU de 6 graus GY-521.

5) Simulações computacionais com obtenção dos dados em tempo real, e

familiarização com comunicação serial entre Arduino e MATLAB

Tendo-se o código no MATLAB funcionando de maneira precisa com a

simulação, adaptou-se este para que utilizasse os dados provenientes em tempo real dos

sensores acoplados ao Arduino Mega. Para tal utilização, foi-se necessário significativo

tempo na familiarização com a comunicação serial necessária entre ambos os softwares

(Arduino IDE, e MATLAB). Além disto, foi-se necessário escrever o código do

Arduíno que obtivesse os dados dos sensores, e os enviassem para o computador através

da porta serial. Foram realizados alguns testes, como levantamento das distribuições das

medidas dos sensores, que possibilitaram uma excelente sintonia do filtro e uma vez

mais comprovaram sua capacidade de convergência e robustez.

6) Tradução do código do Filtro de Kalman para linguagem C.

Após atingido o sucesso na simulação com com obtenção de dados em tempo

real, passou-se à tradução completa do código do MATLAB para a linguagem C

utilizada pelo Arduino, a fim de poder-se testar a viabilidade de utilização do sistema de

navegação desenvolvido de maneira totalmente embarcada, não dependendo mais de um

computador para nenhum tipo de cálculo. O código completo em linguagem C encontra-

se ao término deste relatório no anexo 2.

3. DEFINIÇÃO DO PROBLEMA

Com o intuito de modelar o problema de determinação de atitude a partir de

medidas vetoriais obtidas pela câmera, considera-se um sistema de eixos cartesianos SB

solidário ao corpo do quadricóptero, um sistema de eixos cartesianos SR de referencia, e

outro sistema de eixos cartesianos em um referencial inercial (a Terra, por exemplo,

assumindo que seu movimento é desprezível). Tais sistemas podem ser visualizados na

figura 2. Tanto SB como SR possuem suas origens no centro geométrico do

quadricóptero.

A câmera utilizada será a CMUCam, que já predispõe de um processamento de

imagens e fornece alguns dados, como por exemplo o centroide de regiões de

determinada cor. Juntamente com o sensor ultrassônico utilizado (Maxbotix LV-EZ0)

obtêm-se as medidas vetoriais que fornecem a direção (do ponto de vista da câmera) de

landmarks coloridas dispostas no chão. A câmera encontrara-se fixa ao corpo do

quadricóptero, portanto as medidas vetoriais obtidas serão em relação ao sistema de

eixos SB. As landmarks estão dispostas no chão de modo a sempre terem-se ao menos

duas no campo de visão da câmera. Tais especificações podem ser visualizadas na

figura 3.

O problema proposto consiste em estimar em tempo real a posição e atitude do

quadricóptero como exposto acima. Para estimar-se a atitude, além das medidas

vetoriais, utilizam-se também os valores de velocidade angular obtidos pelo girômetro.

Além disso, para estimar-se a posição utilizam-se também acelerômetros.

Figura 2: Sistemas de eixos cartesianos utilizados para a determinação de atitude. SB, SR e SI

correspondem respectivamente aos sistemas de eixos coordenados solidário ao corpo do VANT,

de referência e inercial (i.e. no referencial da Terra).

Figura 3: Câmera fixa ao quadricóptero, e disposição das landmarks no chão de modo à sempre

haver no mínimo duas no campo de visão da câmera a fim de se obter vetores unitários que

apontam para as mesmas.

4. FORMULAÇÃO DO FILTRO

O método escolhido para resolver o problema definido na Seção 2 consiste num

filtro de Kalman estendido para estimação de quatérnio a partir de medidas vetoriais.

Para manter a norma do quatérnio estimado próxima de unitária, emprega-se

simplesmente uma etapa de normalização Euclidiana das estimativas. Esse filtro será

referido como QEKF (quaternion extended Kalman filter). Em seguida, as equações do

QEKF são revisadas com base em (SANTOS, 2008).

4.1. Estimação de quatérnio

O vetor de estado do estimador de quatérnio de rotação, QEKF, é dado por

, onde q1 e são as representações escalar e

vetorial, respectivamente, dos componentes real e imaginário de q. Assim, tem-se que a

equação de medidas não-linear discreta no tempo dos estimadores de quatérnio de

rotação é:

( ) (1)

onde,

( ) [

( ) ( )

( )

( )

( ) ( )

]

: medidas vetoriais feitas no sistema de eixos cartesianos do VANT;

: medidas vetoriais em relação ao sistema de referência;

: ruído de medição.

O modelo de cinemática de atitude para o quatérnio de rotação é dado pela seguinte

equação diferencial linear:

( ) ( ) ( ) (2)

onde,

( )

[

( ) ( ) ( )

( )

( ) ( )

( ) ( )

( )

( ) ( ) ( )

]

(3)

Em que ( ) ( ) ( ) correspondem respectivamente as velocidades angulares

do VANT em relação aos eixos .

Integrando (2) de , obtém-se:

( ) (4)

Onde ( ) é a matriz de transição de estado. Considerando-se que a

velocidade angular ( ) ( ) ( ) ( ) seja constante durante o

intervalo de tempo entre amostragens sucessivas das medidas

vetoriais, essa matriz é dada por:

( ) (5)

onde é computada por (3), mas utilizando em vez de ( ) Substituindo, então,

( ) ( ) ( ) na equação anterior:

( ) ( ) (6)

onde as matrizes são dadas por (3), porém substituindo

( ) , respectivamente. Representando o segundo fator do lado direito

de (6) pela correspondente série de potências, obtém-se:

( ) ( ) (7)

Considerando-se que sejam ambos pequenos, o truncamento da série em (7)

após termos de primeira ordem permite que (4) seja aproximada por:

(8)

Pode-se verificar que:

(9)

onde,

[

]

(10)

Sendo assim, utilizando (9) na manipulação do segundo termo do lado direito de (8),

finalmente se obtém a seguinte equação de estado discreta no tempo:

(11)

Onde é dado por (10), mas utilizando o quatérnio verdadeiro no instante , no

lugar de . Para fins de implementação dos estimadores de quatérnio, a covariância

do ruído de estado (segundo termo à direita em (11)) é aproximada por:

(12)

onde,

(13)

e é computada por (10), porém utilizando no lugar de .

A exponencial matricial que define a matriz de transição de estado em (5)

é aqui computada por:

(‖ ‖

)

‖ ‖ (‖ ‖

)

(14)

O quatérnio de rotação apresenta norma unitária e, dessa forma, seus

componentes obedecem à restrição . No entanto, a atualização linear do

KF, por envolver a operação de soma, não garante que as estimativas de

mantenham essa propriedade ao longo da estimação. A forma mais simples de lidar

com tal restrição consiste no uso de normalização Euclidiana das estimativas após

o estágio de atualização aditiva.

Em seguida é apresentado o estimador QEKF, o qual utiliza normalização

Euclidiana para garantir que as estimativas tenham normas unitárias.

4.2. QEKF

Mediante uso do EKF (Extended Kalman Filter) e tendo-se em vista a

equação de estado linear (11) e a equação de medidas não linear (1), obtêm-se os

estágios de propagação e de atualização do QEKF. Para garantir que as normas de

suas estimativas sejam unitárias, utiliza-se normalização Euclidiana.

A estimativa normalizada ‖

‖, apresenta um erro cuja covariância é

aproximada no QEKF simplesmente por:

(15)

O QEKF requer a matriz Jacobiana das medidas,

( )

|

(16)

A Tabela 1 apresenta o QEKF.

Tabela 1: Estimador de quatérnio de rotação – QEKF (SANTOS, 2008 – pg. 31).

Em anexo, ao fim deste relatório encontram-se os códigos do Filtro descrito acima.

4.3. Obtenção dos vetores unitários

Após algumas soluções iniciais, em que seriam necessários um sensor a mais a

fim de obter-se também uma medida de distância entre a câmera, e o plano em que se

encontra a landmark, (neste caso utilizar-se-ia o sensor sonar: Parallax's PING), ou então

obter-se a altitude do quadricóptero (para este caso, foi escolhido o sensor altímetro:

Sparkfun’s BMP085), e após alguns experimentos com ambos os sensores para

determinação da qualidade das medidas fornecidas por estes, abandonou-se ambas as

abordagens iniciais, e optou-se por outra cuja necessidade de novo sensor tornou-se

desnecessária.

Somente com a imagem fornecida pela CMUCam4, é possível determinar-se as

coordenadas do vetor unitário que aponta para uma landmark no sistema de eixos do

quadricóptero (que consiste no mesmo da câmera). Para tal, faz-se necessário o

levantamento de um parâmetro da câmera, denotado aqui por K.

K representa a distância entre o plano da imagem vista pela câmera, e a câmera

propriamente dita. Porém, o valor de K é dado pelo número de pixels que esta distancia

ocuparia (no eixo x ou y) em uma imagem obtida pela câmera, caso esta medida fosse

posta em um plano perpendicular ao eixo ótico da câmera afastado exatamente da

mesma medida.

O valor obtido para K foi de aproximadamente 171, isto significa que para a

câmera, em uma imagem visualizada a 1 metro de distância, um objeto com 171 pixels

em linha reta (no eixo x ou y da imagem obtida) de dimensão, possui na realidade

dimensão de 1 metro. Em uma imagem obtida pela câmera a uma distancia 2 metros de

uma parede, 171 pixels desta imagem corresponderiam a um comprimento de 2 metros

na parede. E assim por diante.

Figura 4: Significado do parâmetro K: Na imagem obtida, K pixels em linha reta correspondem

a uma medida no anteparo equivalente à distância entre a câmera e o anteparo.

A imagem de trabalho da CMUCam4 consiste em uma matriz de pixels de

120x160, sendo estes numerados de 0 a 119 para o primeiro eixo, e de 0 a 159 para o

segundo eixo da imagem. Sendo possível obter esta numeração para os pixels

rastreados. Apesar de a CMUCam4 ser, em sua essência, um sensor de cor, por

fornecer os valores de RGB de cada pixel de seu campo de visão, é possível utiliza-la

para tirar fotos (porém de maneira muito ineficiente e demorada). Sendo assim, foi-se

possível realizar experimentos de medições para determinar-se o valor de K com alta

precisão.

Tendo este parâmetro determinado, por simplicidade supondo-se o centro da

imagem vista pela câmera como sendo o pixel com coordenadas ( ), e tendo o eixo

ótico da câmera perfeitamente alinhado com o eixo z do sistema de eixos , um vetor

que aponta para uma landmark com coordenadas do centroide dadas por ( ) (na

imagem fornecida pela CMUCam4, ou seja, x e y são a numeração dos pixels), e com

origem na própria câmera, é dado por:

( ) (17)

Foi-se utilizado o sinal de menos na coordenada z, pois a câmera estará acoplada

à parte inferior do quadricóptero, tendo assim sem campo de visão direcionado à parte

negativa do semi-eixo z do sistema de eixos . Portanto, sendo a norma do vetor dada

por:

‖ ‖ √ (18)

Tem-se então que o vetor unitário necessário ao algoritmo desenvolvido é dado

por:

( ‖ ‖) (19)

5. SIMULAÇÕES COMPUTACIONAIS

Concentram-se aqui os resultados posteriores as fases de construção e estudos, já

que estes são mais significativos como um todo para o trabalho.

Inicialmente, a fim de simular os dados de entrada provenientes dos sensores

relativos à determinação de atitude, ou seja, girômetros e câmera, plotou-se

arbitrariamente funções senoidais bem comportadas para as velocidades angulares (o

motivo desta escolha deve-se puramente a tentativa de evitar-se que as velocidades

angulares escolhidas impliquem em um movimento muito brusco e distante do que

ocorre na realidade com o VANT, fornecendo assim dados mais fidedignos a posterior

análise da simulação). A partir destas funções, colhendo-se seus valores em pontos

igualmente espaçados de tempo, e adicionando-se um ruído aleatório normalmente

distribuído, obtém-se um bom exemplo de valores de medidas provenientes dos

giroscópios. Um exemplo deste procedimento pode ser verificado na figura 5.

Figura 5: Simulação de velocidades angulares medidas pelos girômetros. Respectivamente em

azul, vermelho e verde tem-se: velocidade angular em relação ao eixo x, y e z. Onde x, y e z

correspondem a um sistema de eixos ortogonais solidário ao quadricóptero.

A partir das funções que geraram as medidas de velocidades angulares, obteve-

se qual seria o movimento descrito pelo VANT, ou seja, como os valores dos ângulos

que descrevem sua atitude variam com o tempo. Para tal, utilizou-se o método de

Runge-Kutta de 4ª ordem, o resultado pode ser visto na figura 6.

Figura 6: Atitude parametrizada pelos ângulos de Euler obtida a partir das velocidades

angulares escolhidas. Respectivamente em verde, azul e vermelho tem-se 𝜑, 𝜃 e 𝜙.

Este último gráfico foi plotado apenas para fim de melhor visualização do

movimento descrito pelo quadricóptero. O estudo apresentado aqui trabalha com a

parametrização de atitude através de quatérnio de rotação. Sendo assim, segue o

correspondente gráfico da variação dos parâmetros do real quatérnio de rotação que

descreve o movimento:

Figura 7: Variação dos parâmetros do real quatérnio de rotação q=[q0 q1 q2 q3]T.

Respectivamente em verde, azul, vermelho e preto tem-se q0, q1, q2 e q3.

A partir dos valores do real quatérnio de rotação correspondente ao movimento,

determinou-se quais deveriam ser os reais vetores unitários que, a parte de um ruído

gaussiano, correspondem as medidas vetoriais a serem obtidas pela câmera que

representam as direções de landmarks postas no chão.

Para obterem-se tais medidas, rotacionou-se dois vetores conhecidos, supostos

serem as medidas iniciais da câmera para as landmarks, utilizando-se os valores do

quatérnio do gráfico anterior. Os vetores iniciais escolhidos foram r1=[0, 5/13, -12/13]T

e r2=[0, -5/13, -12/13]T (representações em relação a um sistema de eixos fixo ao solo

coincidente com o sistema de eixos solidário ao VANT na situação inicial). No intuito

de fornecer uma visão mais limpa deste último resultado, na figura 8 é fornecida uma

visualização das primeiras 30 medidas dos vetores, ainda não acrescidas do respectivo

ruído.

Acrescentando-se o devido ruído a este ultimo resultado, obtemos assim dados

coerentes que simulam bem velocidades angulares e medidas vetoriais obtidos

respectivamente pelos girômetros e pela câmera a partir de um determinado movimento

do quadricóptero. Estes dados podem agora ser utilizados como dados de entrada para a

implementação do filtro de Kalman que irá estimar o quatérnio de rotação a partir destes

dado.

Figura 8: Primeiras 30 medidas dos vetores unitários que apontam para as landmarks em

relação ao sistema de eixos fixo ao quadricóptero. Nesta imagem, para melhor visualização, está

suprimido o ruído acrescido aos vetores, o que corresponde de maneira mais fidedigna às

medidas vetoriais obtida pela câmera.

Na figura 9 pode-se verificar a estimação obtida para o quatérnio de rotação

correspondente ao caso real da figura 7. Percebem-se pouquíssimas diferenças. Na

figura 10 é possível verificar-se em detalhe ambos os gráficos superpostos para outro

caso. Nota-se que o filtro estima de maneira bastante razoável os parâmetros do

quatérnio de rotação, mesmo depois de passado um grande período de tempo, e

inúmeras medições já efetuadas.

Figura 9: Variação dos parâmetros do quatérnio de rotação estimado pelo filtro de Kalman

qe=[q0 q1 q2 q3]T. Respectivamente em verde, azul, vermelho e preto tem-se q0, q1, q2 e q3.

Figura 10: Detalhe de comparação entre quatérnio real e quatérnio estimado pelo filtro

de Kalman. Em linha continua encontram-se os valores reais, enquanto os pontos

correspondem às estimativas obtidas pelo filtro.

Figura 11: Índice de ortogonalidade e erro angular em função do tempo. A escala

vertical do primeiro gráfico está multiplicada pelo fator .

6. OBTENÇÃO DOS DADOS EM TEMPO REAL POR

COMUNIÇÃO SERIAL

Concentram-se aqui os resultados posteriores à fase de formulação e simulação

do filtro proposto.

Inicialmente, pretendia-se reescrever todo o código implementado no MATLAB

em linguagem C (o que foi feito posteriormente), e verificar diretamente seu

funcionamento final rodando o algoritmo de maneira totalmente embarcada no

quadricóptero. Porém, em vista dos inúmeros problemas que poderiam surgir nesta

grande passagem do projeto, optou-se por um passo intermediário, que consiste na

simulação com obtenção dos dados em tempo real. A rotina implementada

anteriormente foi adaptada para utilizar os dados provenientes dos sensores, através de

comunicação serial entre o Arduino e o MATLAB, correspondendo assim ao real

movimento destes, e não de uma simulação. Houve a necessidade de abordar-se o

problema da determinação dos vetores unitários que apontam da câmera para a

landmark, explicitada na seção 4.3.

Os demais valores que antes eram provenientes da simulação de uma dinâmica

do quadricóptero, ou seja, as velocidades angulares, são diretamente lidas através do

valor fornecido pelo girômetro, sendo apenas necessário fazer uma correção de unidade.

O giroscópio presente no sensor GY-521 possui 4 configurações possíveis de

ranges para se trabalhar, sendo a escolhida para este trabalho a que apresenta maior

precisão das medidas, porém menor range. Esta decisão foi tomada levando-se em

consideração que os testes preliminares seriam executados com movimentos de

pequenas e médias amplitudes, pois o campo de visão da câmera escolhida apresenta um

campo de visão de cerca de 30º apenas. Além disto, verificou-se experimentalmente que

com o menor range, a distribuição das medidas obtidas com o sensor aproxima-se

melhor de uma gaussiana, o que melhoraria o desempenho do filtro, dado que esta é

uma das premissas de sua formulação. Esta verificação está indicada na figura 12.

Figura 12: Dados de velocidade angular em relação ao mesmo eixo obtido com o giromêtro

configurado para as duas opções extremas de range. Em (a) e (b), tem-se respectivamente as

configurações que fornecem ranges com amplitudes de 2000º/s, e 250º/s. Os dados apresentados

na figura encontram-se em rad/s. O pico de (a) encontra-se com cerca de 1600 amostras, e o

pico de (b) com cerca de 500 amostras.

Com isso, pode-se executar-se novamente a rotina do filtro, e plotar os valores

das inovações de todas as componentes de ambos os vetores. Verificou-se que como

esperado, estes gráficos possuem média nula, são descorrelacionados. Para cada um dos

6 gráficos, plotou-se também o sigma correspondente (raiz quadrada do respectivo

elemento da diagonal principal da matriz de covariância da inovação).

Figura 13: Em azul, valores da inovação de cada uma das componentes de ambos os

vetores obtidos. E verde, respectivo valor de sigma, proveniente da matriz de

covariância da inovação. Sendo de cima para baixo, as coordenadas de x, y e z, sendo os

3 gráficos iniciais correspondentes aos valores do vetor B1 (no código em MATLAB) e

os últimos 3, correspondentes aos valores do vetor B2.

7. CONCLUSÕES

O projeto obtido significativo progresso, obtendo bom resultados nas

simulações, e nas primeiras fases de sua parte prática. Pode-se verificar até o momento

que o filtro tem bom desempenho não só em simulações, mas também em na prática, em

que o sistema de navegação apesar de ainda não ter sido testado em voo no VANT

construído inicialmente, foi testado inúmeras vezes em uma plataforma similar,

estimando a atitude da mesma de maneira acurada. Contudo, o teste em voo é sem

dúvida uma fase crucial do projeto, e que se espera ser executado em breve a fim de

alcançar-se o objetivo pré-estabelecido, isto é, desenvolver um sistema de navegação de

baixo custo, porém de excelente qualidade. Ainda assim, a idealização do sistema, e a

verificação de sua viabilidade, e performance já foram plenamente confirmadas neste

projeto.

Como extensão ao trabalho aqui realizado, sugere-se as seguintes atividades:

Alterar o firmware da CMUCam4 de modo a obter dados de maneira mais

rápida, e assim não haver-se a necessidade de utilizar mais de uma câmera. No

estágio atual em que se encontra o projeto, a alteração dos parâmetros de qual

cor deve ser rastreada pela câmera faz com que sua taxa de amostragem caia de

30 fps para 4 fps, os motivos desta perda está relacionado com o modo de

implementação das funções utilizadas no código deste sensor. A possibilidade de

utilização de uma firmware costumizada já foi esclarecida e confirmada com o

fabricante do mesmo;

.

Realizar testes experimentais de bancada, tendo uma plataforma com

determinação de atitude de modo independente (por exemplo: guimbals

instrumentados com encoders), sendo assim, poder-se-ia comparar a atitude

estimada pelo sistema de navegação desenvolvido com a fornecida pela

plataforma.

8. AGRADECIMENTOS

Agradeço ao Prof. Davi Antônio dos Santos por ter cedido seu tempo e paciência

ao fornecer auxílio no desenvolvimento deste projeto, e pelo seu sempre constante

incentivo. Ao Instituto Tecnológico de Aeronáutica pelo ensino de qualidade, sem o

qual este trabalho não teria sido possível, e pelas inúmeras outras oportunidades

oferecidas. Agradeço ainda em especial ao CNPq que financiou este projeto através do

programa PIBIC. Este incentivo é de suma importância ao desenvolvimento da pesquisa

científica no Brasil.

9. BIBLIOGRAFIA

1. GELB, A. Applied Optimal Estimation. Boston: M.I.T. Press, 1974.

2. BROWN, R.G.; HWANG, P.Y.C. Introduction to Random Signals and Applied

Kalman Filtering. New York: John Wiley & Sons, 1997.

3. ANDERSON, B.D.O.; MOORE, J.B. Optimal Filtering. New Jersey: Prentice-

Hall, 1979.

4. SHUSTER, M. D. A Survey of Attitude Representations. The Journal of the

Astronautical Sciences, Vol. 41, No. 4. October-December 1993, pp.439-517.

5. SANTOS, D. A. Estimação de Atitude e Velocidade Angular de Satélites

Utilizando Medidas do Campo Geomagnético e da Direção do Sol. Dissertação

de Mestrado. Instituto Tecnológico de Aeronáutica, São José dos Campos, 2008.

ANEXO 1: CÓDIGOS DO FILTRO DE KALMAN PARA

ESTIMAÇÃO DE QUATÉRNIO DE ROTAÇÃO

(MATLAB)

A) TESTE_GERAL.m

clc; clear; % parâmetros de tempo dt = 0.1; %intervalo de tempo entre medidas Tmax=100; %intervalo total de tempo em que são feitas medidas %% covariâncias usadas na simulação do sistema sigma_w = 0.005; %incerteza na medição da velocidades angulares sigma_b=0.01; %incerteza nas medidas das coordenadas dos

vetores b1 e b2 Rb = sigma_b^2*eye(6); %covariância do ruído de medidas no sistema Sb Rw = sigma_w^2*eye(3); %covariância do ruído de medidas dos girômetros %% Covariância usadas no filtro R = Rb; Q = 0.005^2*eye(3); %% posições das landmarks r1= [0; 5/13; -12/13]; %representações em Sr das observações vetoriais r2= [0; -5/13; -12/13]; %% Condições iniciais do sistema P(:,:,1)=0.01*eye(4); %covariância do erro de estimação no instante 0 q(:,1) = [1 0 0 0]'; %% Condições iniciais do filtro qe(:,1) = [1 0 0 0]';

for k=1:Tmax/dt+1, %loop único para a simulação e o filtro de Kalman

[q(:,k+1),wm(:,k), b1k1, b2k1]=simulacao(q(:,k),r1,r2,Rb,Rw,dt,k);

[qe(:,k+1),P(:,:,k+1)] =

KalmanFilter(qe(:,k),P(:,:,k),wm(:,k),Q,R,r1,r2,b1k1,b2k1,dt); % Calcula figuras de mérito

% Ik - erro angular; Jk - índice de ortogonalidade [Ik(k+1),Jk(k+1)] = figuras_de_merito(q(:,k+1),qe(:,k+1)); end figure; hold on;

plot(wm');title('Velocidade angular medida em rad/s');

figure; plot(qe','b'); plot(q','r'); figure; plot(Jk); title('Índice de Ortogonalidade'); figure; plot(Ik); title('Erro Angular em graus');

B) simulacao.m

function [qk1,wm, b1k1, b2k1] = simulacao(qk,r1,r2,Rb,Rw,dt,i) %% simulação da velocidade angular.

wk = [0.1*sin(i*dt) 0.1*cos(i*dt) -0.1*sin(i*dt)*cos(i*dt)]'; %% propagação de atitude [qk1,fi]=propaga_q(qk,wk,dt); qk1 = (1/norm(qk1))*qk1; % atitude representada em DCM D = q2DCM(qk1); %% simulação das medidas wm = wk + sqrtm(Rw)*randn(3,1); % girômetros no instante k b=[D*r1;D*r2]+sqrtm(Rb)*randn(6,1);% medidas vet. no instante k+1 b1k1 = b(1:3); b2k1 = b(4:6);

C) KalmanFilter.m

function [qk1,Pk1] = KalmanFilter(qk,Pk,wk,Q,R,r1,r2,b1,b2,dt) % Inputs: % qk – estimativa do quaternio no instante k % Pk - convariância do erro estimado correspondente à qk % wk – medida de velocidade angular no instante k % dt – tempo de amostra % Q - covariância do ruído de processamento % R - covariância do ruído das medidas % r1, r2, b1, b2 – medidas vetoriais no instante k+1 % Outputs: % qk1 - quaternio estimado no instante k+1 % Pk1 – covariância do erro de estimação correspondente à qk1 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %1a - Propagação do estado estimado [q_,fi] = propaga_q(qk,wk,dt); Csi_k = [-qk(2) -qk(3) -qk(4); qk(1) -qk(4) qk(3); qk(4) qk(1) -qk(2); -qk(3) qk(2) qk(1)]; Gama_k=dt/2*fi*Csi_k; Q_q_k=Gama_k*Q*Gama_k'; P_=fi*Pk*fi'+Q_q_k; %--------------------------------------------------------------------- %1b - Predição das medidas D_q_k= q2DCM(q_); b_estimado=[(D_q_k*r1)' (D_q_k*r2)']'; dDdq1 = [ 2*q_(1) 2*q_(4) -2*q_(3); -2*q_(4) 2*q_(1) 2*q_(2); 2*q_(3) -2*q_(2) 2*q_(1)]; dDdq2 = [ 2*q_(2) 2*q_(3) 2*q_(4); 2*q_(3) -2*q_(2) 2*q_(1); 2*q_(4) -2*q_(1) -2*q_(2)]; dDdq3 = [-2*q_(3) 2*q_(2) -2*q_(1); 2*q_(2) 2*q_(3) 2*q_(4); 2*q_(1) 2*q_(4) -2*q_(3)];

dDdq4 = [-2*q_(4) 2*q_(1) 2*q_(2); -2*q_(1) -2*q_(4) 2*q_(3); 2*q_(2) 2*q_(3) 2*q_(4)]; H_1_q = [dDdq1*r1 dDdq2*r1 dDdq3*r1 dDdq4*r1]; H_2_q = [dDdq1*r2 dDdq2*r2 dDdq3*r2 dDdq4*r2]; H_q = [H_1_q; H_2_q]; P_b = H_q*P_*H_q' + R; %--------------------------------------------------------------------- %1c - Covariância cruzada P_b_q = P_*H_q';

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %2a - Ganho K = P_b_q*inv(P_b); %--------------------------------------------------------------------- %2b - Atualização da estimativa qk1 = q_ + K*( [b1' b2']' - b_estimado ); Pk1 = P_ - K*P_b*K'; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %3 - Normalização qk1 = (1/norm(qk1))*qk1;

D) Figuras_de_merito.m

function [Ik,Jk]=figuras_de_merito(q,qe) De = q2DCM(qe); % DCM estimada D = q2DCM(q); % DCM verdadeira Ik = 180/pi*abs(acos(0.5*(trace(De'*D) - 1))); %erro angular em graus Jk = trace((De'*De-eye(3))*(De'*De-eye(3))');%Índice de ortogonalidade

E) propaga_q.m

function [q2,fi] = propaga_q(q1,w1,dt) % cálculo da matriz de transição de estado W = 0.5*[0 -w1(1) -w1(2) -w1(3); w1(1) 0 w1(3) -w1(2); w1(2) -w1(3) 0 w1(1); w1(3) w1(2) -w1(1) 0]; n_w = norm(w1); fi=cos(n_w*dt/2)*eye(4)+1/n_w*sin(n_w*dt/2)*W;

%matriz de transição de estado q2 = fi*q1; % propagacão do quatérnion

F) q2DCM.m

function D = q2DCM(q) D11 = 1-2*q(3)^2-2*q(4)^2;

D12 = 2*(q(2)*q(3)+q(1)*q(4));

D13 = 2*(q(2)*q(4)-q(1)*q(3)); D21 = 2*(q(2)*q(3)-q(1)*q(4));

D22 = 1-2*q(2)^2-2*q(4)^2;

D23 = 2*(q(4)*q(3)+q(1)*q(2)); D31 = 2*(q(2)*q(4)+q(1)*q(3));

D32 = 2*(q(4)*q(3)-q(1)*q(2));

D33 = 1-2*q(2)^2-2*q(3)^2;

D = [ D11 D12 D13;

D21 D22 D23;

D31 D32 D33];

ANEXO 2: CÓDIGOS DO FILTRO DE KALMAN PARA

ESTIMAÇÃO DE QUATÉRNIO DE ROTAÇÃO

(C - ARDUINO)

A) arduino_matlab_serial.ino

#include <CMUcam4.h>

#include <CMUcom4.h> #define R_MIN_1 235 //amarelo

#define R_MAX_1 255

#define G_MIN_1 235 #define G_MAX_1 255

#define B_MIN_1 160 #define B_MAX_1 190

#define R_MIN_2 235 //vermelho #define R_MAX_2 255 #define G_MIN_2 160

#define G_MAX_2 210 #define B_MIN_2 140 #define B_MAX_2 200 #define LED_BLINK 5 // 5 Hz

#define WAIT_TIME 1000 // 5 seconds #define FILTRO 12 #define K1 683

#define pi 3.1416 #define LED_PIN 13 //-----------------------------------------------------------

#include "Wire.h"

#include "I2Cdev.h" #include "MPU6050.h" //-----------------------------------------------------------

//########################################################### // Variáveis globais bool blinkState = false; CMUcam4 cam1(CMUCOM4_SERIAL1);

CMUcam4 cam2(CMUCOM4_SERIAL2); CMUcam4_tracking_data_t data1, data2; MPU6050 accelgyro;

int16_t ax, ay, az; int16_t gx, gy, gz; float a_x, a_y, a_z; float g_x, g_y, g_z; unsigned long T1, T2; //###########################################################

//########################################################### void setup()

{

cam1.begin();

cam2.begin();

cam1.LEDOn(LED_BLINK);


delay(WAIT_TIME);

cam1.autoGainControl(false);

cam2.autoGainControl(false); cam1.autoWhiteBalance(false);

cam2.autoWhiteBalance(false);

cam1.LEDOn(CMUCAM4_LED_ON);


//-----------------------------------------------------------

Wire.begin();

accelgyro.initialize();

//----------------------------------------------------------- Serial.begin(9600);

} //###########################################################

void loop(){

cam1.trackColor(R_MIN_1, R_MAX_1, G_MIN_1, G_MAX_1,

B_MIN_1, B_MAX_1);


B_MIN_2, B_MAX_2);

delay(WAIT_TIME);

cam1.getTypeTDataPacket(&data1);


Serial.println(data1.mx);

Serial.println(data1.my);



for(;;){

accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

//medidas feitas juntas



g_x=gx*pi/(131*180)-0.0244; // conversão para

g_y=gy*pi/(131*180)-0.0155; // a unidade correta

g_z=gz*pi/(131*180)+0.0069; // e calibração

Serial.println(g_y,5);

Serial.println(g_x,5);

Serial.println(g_z,5);





}

}

B) KF_C.ino

#include <CMUcam4.h>

#include <CMUcom4.h>

#define R_MIN_1 75 #define R_MAX_1 255

#define G_MIN_1 0 #define G_MAX_1 125 #define B_MIN_1 0 #define B_MAX_1 100

#define R_MIN_2 75 #define R_MAX_2 255 #define G_MIN_2 0

#define G_MAX_2 125 #define B_MIN_2 0 #define B_MAX_2 100

#define LED_BLINK 5 // 5 Hz

#define WAIT_TIME 5000 // 5 seconds #define FILTRO 12 #define K1 171

#define pi 3.1416 //-----------------------------------------------------------

#include "Wire.h" #include "I2Cdev.h"

#include "MPU6050.h" //########################################################### // Variáveis globais

CMUcam4 cam1(CMUCOM4_SERIAL1);

CMUcam4 cam2(CMUCOM4_SERIAL2); CMUcam4_tracking_data_t data1, data2; MPU6050 accelgyro;

int16_t gx, gy, gz;

double n_w, c, s, **C, **S, **fi, **q, **wm, dt=0.03, **W,

**Csi_k, **Gama_k, **Q_q_k, **Q, **Pk, **D_q_k,

**b_e, **b1_e, **b2_e, **r1, **r2, **dDdq1,

**dDdq2, **dDdq3, **dDdq4, **H_1_q1, **H_1_q2,

**H_1_q3, **H_1_q4, **H_2_q1, **H_2_q2, **H_2_q3,

**H_2_q4, **H_q, **P_b, **R, **P_b_q, **K,

**P_b_n_inv, **b; //########################################################### // Produto de Escalar por Matriz

double** PdeEeM(double e, double** A, int a, int b ) {

for(int i=0; i<a; i++)

for(int j=0; j<b; j++)

A[i][j]=e*A[i][j];

return A; } //###########################################################

// Produto de matrizes

double** PdeM(double** A, double** B, int a, int b, int c ) {

double **P, aux; P=(double **) malloc (a * sizeof(double*));


P[i]=(double *) malloc (c * sizeof(double)); for(int i=0; i<a; i++)

for(int j=0; j<c; j++){

aux=0;

for(int k=0; k<b; k++)

aux=aux+A[i][k]*B[k][j];

P[i][j]=aux;

}

return P;

} //############################################################

// Transposta de Matriz double** TdeM(double** A, int a, int b)

{

double **B;

B=(double **) malloc (b * sizeof(double*));

for(int i=0; i<b; i++) B[i]=(double *) malloc (a * sizeof(double)); for(int i=0; i<b; i++)

for(int j=0; j<a; j++)

B[i][j]=A[j][i];

return B; } //############################################################

// Soma de Matriz double** SdeM(double** A, double** B, int a, int b) {

double **C; C=(double **) malloc (a * sizeof(double*));


C[i]=(double *) malloc (b * sizeof(double));



C[i][j]=B[i][j]+A[i][j];

return C; } //############################################################ // Subtração de Matriz

double** SUdeM(double** A, double** B, int a, int b) {

double **C;

C=(double **) malloc (a * sizeof(double*));


C[i]=(double *) malloc (b * sizeof(double)); for(int i=0; i<a; i++)


C[i][j]=A[i][j]-B[i][j];

return C; }

//###########################################################

void propaga_q (){ W[0][0] = 0;

W[0][1] = -wm[0][0]/2;

W[0][2] = -wm[1][0]/2;

W[0][3] = -wm[2][0]/2;

W[1][0] = wm[0][0]/2;

W[1][1] = 0;

W[1][2] = wm[2][0]/2;

W[1][3] = -wm[1][0]/2; W[2][0] = wm[1][0]/2;

W[2][1] = -wm[2][0]/2;

W[2][2] = 0;

W[2][3] = wm[0][0]/2;

W[3][0] = wm[2][0]/2;

W[3][1] = wm[1][0]/2;

W[3][2] = -wm[0][0]/2;

W[3][3] = 0;

n_w = sqrt(wm[0][0]*wm[0][0]

+wm[1][0]*wm[1][0]

+wm[2][0]*wm[2][0]);

c = cos(n_w*dt/2);

s = sin(n_w*dt/2)/n_w;

for(int i=0; i<4; i++)

for(int j=0; j<4; j++)

if(i==j){

S[i][j] = s;

C[i][j] = c;

}

else{

S[i][j] = 0;

C[i][j] = 0;

} S = PdeM(S, W, 4, 4, 4); fi = SdeM(C, S, 4, 4); //matriz de transição de estado q = PdeM(fi, q, 4, 4, 1); // propagacão do quatérnion

} //###########################################################

void FCsi_k(){

Csi_k[0][0] = -q[1][0];

Csi_k[0][1] = -q[2][0];

Csi_k[0][2] = -q[3][0];

Csi_k[1][0] = q[0][0];

Csi_k[1][1] = -q[3][0];

Csi_k[1][2] = q[2][0];

Csi_k[2][0] = q[3][0];

Csi_k[2][1] = q[0][0];

Csi_k[2][2] = -q[1][0];

Csi_k[3][0] = -q[2][0];

Csi_k[3][1] = q[1][0];

Csi_k[3][2] = q[0][0];

}

//##########################################################

void q2DCM() { D_q_k[0][0] = 1-2*(q[2][0]*q[2][0]-q[3][0]*q[3][0]);

D_q_k[0][1] = 2*(q[1][0]*q[2][0]+q[0][0]*q[3][0]);

D_q_k[0][2] = 2*(q[1][0]*q[3][0]-q[0][0]*q[2][0]);

D_q_k[1][0] = 2*(q[1][0]*q[2][0]-q[0][0]*q[3][0]);

D_q_k[1][1] = 1-2*(q[1][0]*q[1][0]-q[3][0]*q[3][0]);

D_q_k[1][2] = 2*(q[3][0]*q[2][0]+q[0][0]*q[1][0]);

D_q_k[2][0] = 2*(q[1][0]*q[3][0]+q[0][0]*q[2][0]);

D_q_k[2][1] = 2*(q[3][0]*q[2][0]-q[0][0]*q[1][0]);

D_q_k[2][2] = 1-2*(q[1][0]*q[1][0]-q[2][0]*q[2][0]); }

//########################################################## void dDdq() { dDdq1[0][0] = 2*q[0][0];

dDdq1[0][1] = 2*q[3][0];

dDdq1[0][2] = -2*q[2][0];

dDdq1[1][0] = -2*q[3][0];

dDdq1[1][1] = 2*q[0][0];

dDdq1[1][2] = 2*q[1][0];

dDdq1[2][0] = 2*q[2][0];

dDdq1[2][1] = -2*q[1][0];

dDdq1[2][2] = 2*q[0][0]; dDdq2[0][0] = 2*q[1][0];

dDdq2[0][1] = 2*q[2][0];

dDdq2[0][2] = 2*q[3][0];

dDdq2[1][0] = 2*q[2][0];

dDdq2[1][1] = -2*q[1][0];

dDdq2[1][2] = 2*q[0][0];

dDdq2[2][0] = 2*q[3][0];

dDdq2[2][1] = -2*q[0][0];

dDdq2[2][2] = -2*q[1][0]; dDdq3[0][0] = -2*q[2][0];

dDdq3[0][1] = 2*q[1][0];

dDdq3[0][2] = -2*q[0][0];

dDdq3[1][0] = 2*q[1][0];

dDdq3[1][1] = 2*q[2][0];

dDdq3[1][2] = 2*q[3][0];

dDdq3[2][0] = 2*q[0][0];

dDdq3[2][1] = 2*q[3][0];

dDdq3[2][2] = -2*q[2][0]; dDdq4[0][0] = -2*q[3][0];

dDdq4[0][1] = 2*q[0][0]; dDdq4[0][2] = 2*q[1][0];

dDdq4[1][0] = -2*q[0][0];

dDdq4[1][1] = -2*q[3][0];

dDdq4[1][2] = 2*q[2][0];

dDdq4[2][0] = 2*q[1][0];

dDdq4[2][1] = 2*q[2][0];

dDdq4[2][2] = 2*q[3][0]; }

//##########################################################

void FH_q() { H_1_q1 = PdeM(dDdq1, r1, 3, 3, 1);

H_1_q2 = PdeM(dDdq2, r1, 3, 3, 1);

H_1_q3 = PdeM(dDdq3, r1, 3, 3, 1);

H_1_q4 = PdeM(dDdq4, r1, 3, 3, 1); H_2_q1 = PdeM(dDdq1, r2, 3, 3, 1);

H_2_q2 = PdeM(dDdq2, r2, 3, 3, 1);

H_2_q3 = PdeM(dDdq3, r2, 3, 3, 1);

H_2_q4 = PdeM(dDdq4, r2, 3, 3, 1);

H_q[0][0] = H_1_q1[0][0];

H_q[0][1] = H_1_q2[0][0];

H_q[0][2] = H_1_q3[0][0];

H_q[0][3] = H_1_q4[0][0];

H_q[1][0] = H_1_q1[1][0];

H_q[1][1] = H_1_q2[1][0];

H_q[1][2] = H_1_q3[1][0];

H_q[1][3] = H_1_q4[1][0];

H_q[2][0] = H_1_q1[2][0];

H_q[2][1] = H_1_q2[2][0];

H_q[2][2] = H_1_q3[2][0];

H_q[2][3] = H_1_q4[2][0];

H_q[3][0] = H_2_q1[0][0];

H_q[3][1] = H_2_q2[0][0];

H_q[3][2] = H_2_q3[0][0];

H_q[3][3] = H_2_q4[0][0];

H_q[4][0] = H_2_q1[1][0];

H_q[4][1] = H_2_q2[1][0];

H_q[4][2] = H_2_q3[1][0];

H_q[4][3] = H_2_q4[1][0];

H_q[5][0] = H_2_q1[2][0];

H_q[5][1] = H_2_q2[2][0];

H_q[5][2] = H_2_q3[2][0];

H_q[5][3] = H_2_q4[2][0]; }

//##########################################################

//Inversão de Matriz utilizando o método de Gauss-Jordam

int inv(double **A, int n) { // A = matriz de entrada e também de saida

int pivrow;

int k,i,j;

int* pivrows; pivrows=(int *) malloc (n * sizeof(int)); float tmp;

for (k = 0; k < n; k++) {

tmp = 0;

for (i = k; i < n; i++)

if (abs(A[i][k]) >= tmp) { tmp = abs(A[i][k]); pivrow = i;

}

if (A[pivrow][k] == 0.0f)

return 0; //Inversão falha pois é matriz

//singular

if (pivrow != k) {

for (j = 0; j < n; j++) { tmp = A[k][j];

A[k][j] = A[pivrow][j];

A[pivrow][j] = tmp;

}

}

pivrows[k] = pivrow;

tmp = 1.0f/A[k][k];

A[k][k] = 1.0f;

for (j = 0; j < n; j++)

A[k][j] = A[k][j]*tmp;

for (i = 0; i < n; i++) {

if (i != k) { tmp = A[i][k];

A[i][k] = 0.0f;

for (j = 0; j < n; j++) {

A[i][j] = A[i][j] - A[k][j]*tmp; }

}

}

}

for (k = n-1; k >= 0; k--) {

if (pivrows[k] != k) {

for (i = 0; i < n; i++) { tmp = A[i][k]; A[i][k] = A[i][pivrows[k]]; A[i][pivrows[k]] = tmp;

}

}

}

return 1;

}

//##########################################################

void setup() {

cam1.begin();

cam2.begin();



delay(WAIT_TIME); cam1.autoGainControl(false);

cam2.autoGainControl(false);

cam1.autoWhiteBalance(false);

cam2.autoWhiteBalance(false); cam1.LEDOn(CMUCAM4_LED_ON);


//----------------------------------------------------------

Wire.begin();

accelgyro.initialize(); //----------------------------------------------------------

C = (double **) malloc (4 * sizeof(double*));


C[i]=(double *) malloc (4 * sizeof(double));

S = (double **) malloc (4 * sizeof(double*));


S[i]=(double *) malloc (4 * sizeof(double));

fi = (double **) malloc (4 * sizeof(double*));


fi[i]=(double *) malloc (4 * sizeof(double));

q=(double **) malloc (4 * sizeof(double*));


q[i]=(double *) malloc (sizeof(double));

wm=(double **) malloc (3 * sizeof(double*));


wm[i]=(double *) malloc (sizeof(double));

W=(double **) malloc (4 * sizeof(double*));


W[i]=(double *) malloc (4 * sizeof(double));

Csi_k=(double **) malloc (4 * sizeof(double*));


Csi_k[i]=(double *) malloc (3 * sizeof(double));

Gama_k=(double **) malloc (4 * sizeof(double*));


Gama_k[i]=(double *) malloc (3 * sizeof(double));

Q_q_k=(double **) malloc (4 * sizeof(double*));


Q_q_k[i]=(double *) malloc (4 * sizeof(double));

Q=(double **) malloc (3 * sizeof(double*));


Q[i]=(double *) malloc (3 * sizeof(double));

Pk=(double **) malloc (4 * sizeof(double*));


Pk[i]=(double *) malloc (4 * sizeof(double));

D_q_k=(double **) malloc (3 * sizeof(double*));


D_q_k[i]=(double *) malloc (3 * sizeof(double));

b_e=(double **) malloc (6 * sizeof(double*));


b_e[i]=(double *) malloc (6 * sizeof(double));

b1_e=(double **) malloc (3 * sizeof(double*));


b1_e[i]=(double *) malloc (3 * sizeof(double));

b2_e=(double **) malloc (3 * sizeof(double*));


b2_e[i]=(double *) malloc (3 * sizeof(double));

r1=(double **) malloc (3 * sizeof(double*));


r1[i]=(double *) malloc (3 * sizeof(double));

r2=(double **) malloc (3 * sizeof(double*));


r2[i]=(double *) malloc (3 * sizeof(double));

dDdq1=(double **) malloc (3 * sizeof(double*));


dDdq1[i]=(double *) malloc (3 * sizeof(double));










H_1_q1=(double **) malloc (3 * sizeof(double*));


H_1_q1[i]=(double *) malloc (sizeof(double));






















H_q=(double **) malloc (6 * sizeof(double*));


H_q[i]=(double *) malloc (4 * sizeof(double));

P_b=(double **) malloc (6 * sizeof(double*));


P_b[i]=(double *) malloc (6 * sizeof(double));

R=(double **) malloc (6 * sizeof(double*));


R[i]=(double *) malloc (6 * sizeof(double));

P_b_q=(double **) malloc (4 * sizeof(double*));


P_b_q[i]=(double *) malloc (6 * sizeof(double));

K=(double **) malloc (4 * sizeof(double*));


K[i]=(double *) malloc (6 * sizeof(double));

P_b_n_inv=(double **) malloc (6 * sizeof(double*));


P_b_n_inv[i]=(double *)malloc(6*sizeof(double));

b=(double **) malloc (6 * sizeof(double*));


b[i]=(double *) malloc (sizeof(double));

for(int i=0; i<3; i++) for(int j=0; j<3; j++)

if(i==j)

Q[i][j] = 0.000025;

else

Q[i][j] = 0;



if(i==j) Pk[i][j] = 0.01;

else Pk[i][j] = 0;



if(i==j)

R[i][j] = 0.0001;

else

R[i][j] = 0; q[0][0]=1;

q[1][0]=0;

q[2][0]=0;

q[3][0]=0; } //########################################################## void loop() {


B_MIN_1, B_MAX_1);


B_MIN_2, B_MAX_2);

delay(WAIT_TIME);



r1[0][0] = sin(atan((data1.mx-80)/K1));

r1[1][0] = sin(atan((data1.my-60)/K1));

r1[2][0] = sqrt(1-r1[0][0]*r1[0][0]-r1[1][0]*r1[1][0]);

r2[0][0] = sin(atan((data2.mx-80)/K1));

r2[1][0] = sin(atan((data2.my-60)/K1));

r2[2][0] = sqrt(1-r2[0][0]*r2[0][0]-r2[1][0]*r2[1][0]);

for(;;) { accelgyro.getRotation(&gx, &gy, &gz);

//medidas feitas juntas



wm[0][0]=gx*pi/(16.4*180);

wm[1][0]=gy*pi/(16.4*180);

wm[2][0]=gz*pi/(16.4*180);

//----------------------------------------------------------

//1a - Propagação do estado estimado

FCsi_k();

propaga_q();

Gama_k = PdeEeM((dt/2),PdeM(fi,Csi_k,4,4,3),4,3);

Q_q_k = PdeM(Gama_k,PdeM(Q,TdeM(Gama_k,4,3),

3,3,4),4,3,4);

Pk = SdeM(PdeM(PdeM(fi,Pk,4,4,4),

TdeM(fi,4,4),4,4,4),Q_q_k,4,4);

//----------------------------------------------------------

//1b - Predição das medidas

q2DCM();

b1_e = PdeM(D_q_k, r1, 3, 3, 1);

b2_e = PdeM(D_q_k, r2, 3, 3, 1);

b_e[0][0] = b1_e[0][0];

b_e[1][0] = b1_e[1][0];

b_e[2][0] = b1_e[2][0];

b_e[3][0] = b2_e[0][0];

b_e[4][0] = b2_e[1][0];

b_e[5][0] = b2_e[2][0];

dDdq();

FH_q();

P_b = SdeM(PdeM(PdeM(H_q,Pk,6,4,4),

TdeM(H_q,6,4),6,4,6),R,6,6);

//----------------------------------------------------------

//1c - Covariância cruzada P_b_q = PdeM(Pk, TdeM(H_q, 6, 4), 4, 4, 6);

//----------------------------------------------------------

//2a - Ganho


for(int k=0; k<6; k++)

P_b_n_inv[i][k]=P_b[i][k];

inv(P_b, 6); //retorna 0 em falha, e 1 em sucesso

K = PdeM(P_b_q, P_b, 4, 6, 6);

//----------------------------------------------------------

//2b - Atualização da estimativa

b[0][0] = sin(atan((data1.mx-80)/K1));

b[1][0] = sin(atan((data1.my-60)/K1));

b[2][0] = sqrt(1-b[0][0]*b[0][0]-b[1][0]*b[1][0]);

b[3][0] = sin(atan((data2.mx-80)/K1));

b[4][0] = sin(atan((data2.my-60)/K1));

b[5][0] = sqrt(1-b[3][0]*b[3][0]-b[4][0]*b[4][0]);

q = SdeM(q,PdeM(K,SUdeM(b,b_e,6,1),4,6,1),4,1);

Pk = SUdeM(Pk,PdeM(PdeM(K,P_b_n_inv,4,6,6),

TdeM(K,4,6),4,6,4),4,4);

//##########################################################

//3 - Normalização

q = PdeEeM((1/sqrt( q[0][0]*q[0][0]

+q[1][0]*q[1][0]

+q[2][0]*q[2][0]

+q[3][0]*q[3][0])),q,4,1);

} }

Engineering

Desenvolvimento de um Sistema de Navegação Indoor por Imagens para um Veículo Aéreo não Tripulado do tipo Quadricóptero