Anais do 15O Encontro de Iniciação Científica e Pós-Graduação do ITA – XV ENCITA / 2009
Instituto Tecnológico de Aeronáutica, São José dos Campos, SP, Brasil, Outubro, 19 a 21, 2009.
IMPLEMENTAÇÃO DE SISTEMA DE ATITUDE EM DSP
Jorge Augusto Melegati Gonçalves
Instituto Tecnológico de Aeronáutica
H8B, apto. 208, CTA
12228-461 – São José dos Campos/SP
Bolsista PIBIC-CNPq
[email protected]
Elder Moreira Hemerly
Instituto Tecnológico de Aeronáutica
Departamento de Sistemas e Controle
Praça Marechal Eduardo Gomes, 50
12228-900 – São José dos Campos/SP
[email protected]
Resumo: Este trabalho tem como objetivo simular e analisar um algoritmo de determinação de atitude baseado na utilização de um
filtro de Kalman para combinar as inferências realizadas com tríades de acelerômetros, girômetros e magnetômetros. Por fim, o
trabalho propõe uma implementação em uma linguagem que permita a sua implantação em um dispositivo processador digital de
sinais.
Palavras-chave: Sistemas de determinação de atitude, Navegação, Filtro de Kalman, Processador digital de sinais.
1. Introdução
Os sistemas de determinação de atitude estão, na atualidade, em diversos dispositivos não mais se limitando a sua
área de aplicação tradicional, a indústria aeronáutica, mas, sim, podendo se encontrar em produtos como dispositivos de
interface homem-máquina em equipamentos de entretenimento eletrônico e até em dispositivos portáteis como
detectores de queda com o intuito de prevenção de maiores danos. Essa nova demanda exige que os algoritmos de
determinação de atitude sejam implementados em processadores menores, como é o caso dos processadores digitais de
sinais. Esse trabalho se propõe, então, à implementação de um algoritmo de determinação de atitude em um ambiente
que permita a sua instalação em processadores digitais de sinais. Num primeiro momento, dar-se-á um estudo de um
algoritmo de determinação de atitude, simulando-o e verificando a sua robustez ao caso do bias dos girômetros e, então,
a verificação da implementação em uma outra linguagem.
2. Estudo de um sistema de determinação de atitude
O algoritmo proposto compreende basicamente duas etapas: obtenção da atitude através da integração da
velocidade angular nos diferentes eixos através das leituras dos girômetros. Com isso, obtém-se a atitude propagada.
Paralelamente, são calculados, através da comparação das leituras do acelerômetro e do magnetômetro com os
vetores gravidade e campo magnético local, os ângulos de Euler do móvel.
Obteve-se, então, de duas formas diferentes uma mesma informação as quais deverão ser levadas em conta na
obtenção da atitude estimada. Isso é feito através de um filtro de Kalman.
2.1. Atitude propagada
Essa etapa consiste na obtenção da atitude de um próximo instante através da integração da velocidade angular
obtida pelos girômetros com a atitude anterior.
Isso é realizado através da conversão dos ângulos de Euler iniciais em uma matriz de transformação do referencial
de navegação para o referencial do corpo, denominada Cnb.
A essa matriz é, então, adicionado um fator de correção da atitude obtido através do produto do período de tempo
transcorrido por matriz anti-simétrica obtida das componentes da velocidade angular lido através do girômetro, é feita,
então, a normalização da matriz Cnb, obtendo-se a matriz de atitude propagada que entrará novamente no passo
seguinte da iteração.
2.2 Atitude obtida através do acelerômetro e do magnetômetro
Anais do XV ENCITA 2009, ITA, Outubro, 19-21, 2009
,
As leituras do acelerômetro e do magnetômetro fornecem os vetores, no referencial do corpo, força e campo
magnético ao qual ele está submetido. Através da comparação com os valores conhecidos desses vetores no referencial
de navegação, é possível obter a atitude do corpo.
2.3. Estimação da atitude
Para o cálculo da atitude estimada, são utilizados os dados obtidos anteriormente, ou seja: a) propaga-se a atitude,
isto é, os ângulos de Euler, usando as leituras dos girômetros, e b) com base nos valores de referência dos vetores
gravidade e campo magnético, um Filtro de Kalman corrige a atitude.
A cada iteração é calculada a matriz ganho do estimador (L, ganho de Kalman) dado por:
L  PH T ( H T PH  Rz )
(1)
onde H, é uma matriz com três linhas e seis colunas, onde as três primeiras colunas representam a matriz identidade,
essa matriz leva em conta o fato de que não há uma medida real do bias dos girômetros, Rz é a matriz responsável pela
modelagem dos ruídos de medida e a matriz de covariância dos erros de estimação, P, é atualizada em duas etapas:
primeiramente, antes do cálculo de L, P é propagada através da Eq. (2).
Pk 1  k Pk k T  Cd
onde
(2)
  I 3 x 3  T  F , onde T é o intervalo de integração e F    I 3 x 3 , onde β é um fator escalar de sintonia do
filtro, e
Cd  G  GT com G = 0,1.I 3 x 3 e σω modelo o ruído de entrada, utilizando o valor de Pk, atualizado na
iteração anterior. E, então, após o cálculo de L, P é atualizado seguindo a Eq. (3).
P  ( I3 x3  LH ) P
(3)
Com o valor de L, é possível obter um vetor contendo seis elementos que são, a estimativa da atitude, três
componentes, e do bias dos girômetros, três componentes, através de:
Ee  L( y  Eˆ (k ))
(5)
Eˆ (k  1)  Eˆ (k )  Ee
(6)
onde Ê(k) é o valor obtido através de integração e y é o vetor obtido do cálculo envolvendo os dados do acelerômetro e
do magnetômetro. Ê(k+1) é o resultado final desta iteração e será utilizado no próximo passo da iteração como entrada
no algoritmo de obtenção da atitude propagada.
3. Simulação em Matlab
Dado um conjunto de dados de teste constituído de valores de medidas de tríades de acelerômetros,
magnetômetros e girômetros e a atitude obtida com um sistema comercial, as etapas da implementação do algoritmo
foram acompanhadas através da comparação do resultado obtido com a implementação em desenvolvimento com os
resultados de referência, utilizando o software Matlab. Utilizando o Filtro de Kalman exposto, obteve-se os seguintes
gráficos comparando o valor esperado (linha verde) com o valor obtido (linha amarela) para a atitude.
Nas figuras 1, 2 e 3, observa-se uma boa aproximação dos valores esperados, principalmente, para os ângulos de
rolagem e arfagem. Entretanto, em relação ao ângulo de guinada, há uma piora nos resultados devido ao fato que
acelerômetro não agrega informação relativa a esse ângulo pois, o vetor aceleração da gravidade é perpendicular ao
plano em que se forma esse ângulo.
Anais do XV ENCITA 2009, ITA, Outubro, 19-21, 2009
,
Figura 1. Comparação do ângulo de arfagem obtido através do algoritmo proposto e de um algoritmo consagrado.
Figura 2. Comparação do ângulo de guinada obtido através do algoritmo proposto e de um algoritmo consagrado.
Anais do XV ENCITA 2009, ITA, Outubro, 19-21, 2009
,
Figura 3. Comparação do ângulo de guinada obtido através do algoritmo proposto e de um algoritmo consagrado.
4. Implementação em linguagem C++
Em busca do objetivo principal do projeto, iniciou-se a implementação do mesmo em uma linguagem que
possibilitasse a sua implantação em um dispositivo processador digital de sinais(DSP), assim, a linguagem escolhida foi
C++. Nessa implementação, inclui-se a escolha de algoritmos para a solução de problemas que, no Matlab, já haviam
sido implementados como o produto de matrizes. Os resultados obtidos são mostrados nas figuras 4, 5 e 6, as linhas
verdes representam o valor obtido com um sistema comercial enquanto que a linha vermelha representa o valor obtido
com o algoritmo implementado.
Anais do XV ENCITA 2009, ITA, Outubro, 19-21, 2009
,
Figura 4. Comparação entre valores obtidos para o ângulo de rolagem com implementações em Matlab e em C++.
Figura 5. Comparação entre valores obtidos para o ângulo de arfagem com implementações em Matlab e em C++.
Anais do XV ENCITA 2009, ITA, Outubro, 19-21, 2009
,
Figura 6. Comparação entre valores obtidos para o ângulo de arfagem com implementações em Matlab e em C++.
A partir dos gráficos expostos nas figuras 4, 5 e 6, pode-se concluir que o algoritmo implementado em C++
atingiu qualidade similar ao do algoritmo implementado em Matlab.
5. Conclusões
Durante a execução do trabalho, foi discutido um projeto de sistema de determinação de atitude com o qual
prosseguiu-se com a simulação em ambiente Matlab. Dado os resultados obtidos terem sido satisfatórios, partiu-se para
a implementação do mesmo algoritmo em linguagem C++, o que possibilitaria a sua implementação em um dispositivo
DSP. Os resultados apresentados pela implementação em um novo ambiente proporcionaram um resultado dentro do
esperado.
6. Agradecimentos
Ao Conselho Nacional de Desenvolvimento Científico e Tecnológico (CNPq) por proporcionar o apoio necessário
à execução do projeto.
5. Referências
Zhu, R. et al., 2007, “A linear fusion algorithm for attitude determination using low cost MEMS-based sensor”,
Measurement, No. 40, pp. 322-328.
Gebre-Egziabher, D. 2001, “Design and performance analysis of a low-cost aided dead reckoning navigator”,
Dissertação de Doutorado, Stanford.
Gebre-Egziabher, D. et al, 2004, “Design of Multi-Sensor Attitude Determination Systems”, IEEE transaction on
aerospace and electronic systems, Vol. 40, No. 2, pp. 627-648.