Desenvolvimento de um Sistema de Navegação Indoor por Imagens para um Veículo Aéreo não...
-
Upload
roberto-brusnicki -
Category
Engineering
-
view
212 -
download
20
description
Transcript of Desenvolvimento de um Sistema de Navegação Indoor por Imagens para um Veículo Aéreo não...
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
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
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
INSTITUTO TECNOLÓGICO DE AERONÁUTICA
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);
cam2.LEDOn(LED_BLINK);
delay(WAIT_TIME);
cam1.autoGainControl(false);
cam2.autoGainControl(false); cam1.autoWhiteBalance(false);
cam2.autoWhiteBalance(false);
cam1.LEDOn(CMUCAM4_LED_ON);
cam2.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);
cam2.trackColor(R_MIN_2, R_MAX_2, G_MIN_2, G_MAX_2,
B_MIN_2, B_MAX_2);
delay(WAIT_TIME);
cam1.getTypeTDataPacket(&data1);
cam2.getTypeTDataPacket(&data2);
Serial.println(data1.mx);
Serial.println(data1.my);
Serial.println(data2.mx);
Serial.println(data2.my);
for(;;){
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
//medidas feitas juntas
cam1.getTypeTDataPacket(&data1);
cam2.getTypeTDataPacket(&data2);
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);
Serial.println(data1.mx);
Serial.println(data1.my);
Serial.println(data2.mx);
Serial.println(data2.my);
}
}
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*));
for(int i=0; i<a; i++)
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*));
for(int i=0; i<a; i++)
C[i]=(double *) malloc (b * sizeof(double));
for(int i=0; i<a; i++)
for(int j=0; j<b; j++)
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*));
for(int i=0; i<a; i++)
C[i]=(double *) malloc (b * sizeof(double)); for(int i=0; i<a; i++)
for(int j=0; j<b; j++)
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();
cam1.LEDOn(LED_BLINK);
cam2.LEDOn(LED_BLINK);
delay(WAIT_TIME); cam1.autoGainControl(false);
cam2.autoGainControl(false);
cam1.autoWhiteBalance(false);
cam2.autoWhiteBalance(false); cam1.LEDOn(CMUCAM4_LED_ON);
cam2.LEDOn(CMUCAM4_LED_ON);
//----------------------------------------------------------
Wire.begin();
accelgyro.initialize(); //----------------------------------------------------------
C = (double **) malloc (4 * sizeof(double*));
for(int i=0; i<4; i++)
C[i]=(double *) malloc (4 * sizeof(double));
S = (double **) malloc (4 * sizeof(double*));
for(int i=0; i<4; i++)
S[i]=(double *) malloc (4 * sizeof(double));
fi = (double **) malloc (4 * sizeof(double*));
for(int i=0; i<4; i++)
fi[i]=(double *) malloc (4 * sizeof(double));
q=(double **) malloc (4 * sizeof(double*));
for(int i=0; i<4; i++)
q[i]=(double *) malloc (sizeof(double));
wm=(double **) malloc (3 * sizeof(double*));
for(int i=0; i<3; i++)
wm[i]=(double *) malloc (sizeof(double));
W=(double **) malloc (4 * sizeof(double*));
for(int i=0; i<4; i++)
W[i]=(double *) malloc (4 * sizeof(double));
Csi_k=(double **) malloc (4 * sizeof(double*));
for(int i=0; i<4; i++)
Csi_k[i]=(double *) malloc (3 * sizeof(double));
Gama_k=(double **) malloc (4 * sizeof(double*));
for(int i=0; i<4; i++)
Gama_k[i]=(double *) malloc (3 * sizeof(double));
Q_q_k=(double **) malloc (4 * sizeof(double*));
for(int i=0; i<4; i++)
Q_q_k[i]=(double *) malloc (4 * sizeof(double));
Q=(double **) malloc (3 * sizeof(double*));
for(int i=0; i<3; i++)
Q[i]=(double *) malloc (3 * sizeof(double));
Pk=(double **) malloc (4 * sizeof(double*));
for(int i=0; i<4; i++)
Pk[i]=(double *) malloc (4 * sizeof(double));
D_q_k=(double **) malloc (3 * sizeof(double*));
for(int i=0; i<3; i++)
D_q_k[i]=(double *) malloc (3 * sizeof(double));
b_e=(double **) malloc (6 * sizeof(double*));
for(int i=0; i<6; i++)
b_e[i]=(double *) malloc (6 * sizeof(double));
b1_e=(double **) malloc (3 * sizeof(double*));
for(int i=0; i<3; i++)
b1_e[i]=(double *) malloc (3 * sizeof(double));
b2_e=(double **) malloc (3 * sizeof(double*));
for(int i=0; i<3; i++)
b2_e[i]=(double *) malloc (3 * sizeof(double));
r1=(double **) malloc (3 * sizeof(double*));
for(int i=0; i<3; i++)
r1[i]=(double *) malloc (3 * sizeof(double));
r2=(double **) malloc (3 * sizeof(double*));
for(int i=0; i<3; i++)
r2[i]=(double *) malloc (3 * sizeof(double));
dDdq1=(double **) malloc (3 * sizeof(double*));
for(int i=0; i<3; i++)
dDdq1[i]=(double *) malloc (3 * sizeof(double));
dDdq2=(double **) malloc (3 * sizeof(double*));
for(int i=0; i<3; i++)
dDdq2[i]=(double *) malloc (3 * sizeof(double));
dDdq3=(double **) malloc (3 * sizeof(double*));
for(int i=0; i<3; i++)
dDdq3[i]=(double *) malloc (3 * sizeof(double));
dDdq4=(double **) malloc (3 * sizeof(double*));
for(int i=0; i<3; i++)
dDdq4[i]=(double *) malloc (3 * sizeof(double));
H_1_q1=(double **) malloc (3 * sizeof(double*));
for(int i=0; i<3; i++)
H_1_q1[i]=(double *) malloc (sizeof(double));
H_1_q2=(double **) malloc (3 * sizeof(double*));
for(int i=0; i<3; i++)
H_1_q2[i]=(double *) malloc (sizeof(double));
H_1_q3=(double **) malloc (3 * sizeof(double*));
for(int i=0; i<3; i++)
H_1_q3[i]=(double *) malloc (sizeof(double));
H_1_q4=(double **) malloc (3 * sizeof(double*));
for(int i=0; i<3; i++)
H_1_q4[i]=(double *) malloc (sizeof(double));
H_2_q1=(double **) malloc (3 * sizeof(double*));
for(int i=0; i<3; i++)
H_2_q1[i]=(double *) malloc (sizeof(double));
H_2_q2=(double **) malloc (3 * sizeof(double*));
for(int i=0; i<3; i++)
H_2_q2[i]=(double *) malloc (sizeof(double));
H_2_q3=(double **) malloc (3 * sizeof(double*));
for(int i=0; i<3; i++)
H_2_q3[i]=(double *) malloc (sizeof(double));
H_2_q4=(double **) malloc (3 * sizeof(double*));
for(int i=0; i<3; i++)
H_2_q4[i]=(double *) malloc (sizeof(double));
H_q=(double **) malloc (6 * sizeof(double*));
for(int i=0; i<6; i++)
H_q[i]=(double *) malloc (4 * sizeof(double));
P_b=(double **) malloc (6 * sizeof(double*));
for(int i=0; i<6; i++)
P_b[i]=(double *) malloc (6 * sizeof(double));
R=(double **) malloc (6 * sizeof(double*));
for(int i=0; i<6; i++)
R[i]=(double *) malloc (6 * sizeof(double));
P_b_q=(double **) malloc (4 * sizeof(double*));
for(int i=0; i<4; i++)
P_b_q[i]=(double *) malloc (6 * sizeof(double));
K=(double **) malloc (4 * sizeof(double*));
for(int i=0; i<4; i++)
K[i]=(double *) malloc (6 * sizeof(double));
P_b_n_inv=(double **) malloc (6 * sizeof(double*));
for(int i=0; i<6; i++)
P_b_n_inv[i]=(double *)malloc(6*sizeof(double));
b=(double **) malloc (6 * sizeof(double*));
for(int i=0; i<6; i++)
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;
for(int i=0; i<4; i++)
for(int j=0; j<4; j++)
if(i==j) Pk[i][j] = 0.01;
else Pk[i][j] = 0;
for(int i=0; i<6; i++)
for(int j=0; j<6; j++)
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() {
cam1.trackColor(R_MIN_1, R_MAX_1, G_MIN_1, G_MAX_1,
B_MIN_1, B_MAX_1);
cam2.trackColor(R_MIN_2, R_MAX_2, G_MIN_2, G_MAX_2,
B_MIN_2, B_MAX_2);
delay(WAIT_TIME);
cam1.getTypeTDataPacket(&data1);
cam2.getTypeTDataPacket(&data2);
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
cam1.getTypeTDataPacket(&data1);
cam2.getTypeTDataPacket(&data2);
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 i=0; i<6; i++)
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);
} }