Ficheiro PDF

Transcrição

Ficheiro PDF

Relatório de Trabalho de Fim
de Curso de Enga. Aeroespacial
Estudo e Implementação
de um Sistema de Atitude
AHRS
Realizado por
: Rui Simões no 43367
Orientador
: António Pascoal
Docente Acompanhante : Paulo Oliveira
Junho 2002
Prefácio
No inı́cio do século 20, uma aeronave dispunha de uma bússola, um altı́metro barométrico e
um sensor de velocidade relativa ao ar, para além da perı́cia do piloto a quem cabia a tarefa
de guiar e orientar a aeronave. Hoje, são utilizados centenas de sensores que medem os mais
variados parâmetros relacionados com movimento de um veı́culo. Isto acontece tanto em
aplicações militares como civis. Estes sensores são a base de diversos e sofisticados sistemas
de navegação que, por vezes, actuam sobre o veı́culo de modo completamente autónomo.
Uma parte integrante de um sistema de navegação é o subsistema que determina a atitude
do veı́culo ao longo da sua trajectória. Da correcta determinação desta depende a precisão
obtida na navegação e guiamento do veı́culo. Esta dependência acentua-se quando o veı́culo
apenas dispõe de informação interna, isto é, quando o veı́culo não recebe ajudas proveniente
do exterior tal como o recurso ao GP S.
Assim, este trabalho de fim de curso de Enga Aeroespacial do ramo de Aviónica, tem
como objectivo o estudo e implementação de um sistema de atitude de apoio à navegação. O
presente documento tem como objectivo a exposição do trabalho efectuado e dos resultados
obtidos encontrando-se dividido em sete capı́tulos e dois anexos.
No capı́tulo 1 é feita uma introdução geral aos conceitos mais importantes associados à
navegação. Este capı́tulo serve também como motivação para o objectivo final do trabalho desenvolvido. No capı́tulo 2 são apresentadas as caracterı́sticas principais dos sensores
utilizados no sistema que se pretende implementar.
Os dois capı́tulos seguintes são dedicados ao processo de filtragem. No capı́tulo 4 fazse uma apresentação da filtragem complementar e da filtragem de Kalman e realçam-se
os pontos em comum de ambas. Em particular, apresentam-se as condições sob as quais
estes processos são idênticos. No capı́tulo 5 formula-se o filtro de atitude que é a estrutura
principal do sistema de atitude, tendo como base as técnicas descritas no capı́tulo 4.
Nos dois capı́tulos seguintes faz-se a apresentação e discussão de resultados obtidos. No
capı́tulo 5 apresentam-se resultados relativos à simulação efectuada em MATLAB. O capı́tulo
6 diz respeito aos resultados obtidos com sensores reais. Para tal, são injectados os dados
referentes aos sensores em dois testes distintos no filtro de atitude implementado.
No capı́tulo 7 apresentam-se as principais conclusões extraı́das no decorrer do trabalho.
Por fim, seguem-se dois apêndices. No primeiro faz-se a descrição da simulação efectuada
em M AT LAB e no segundo apresenta-se o código relativo aos ficheiros da simulação.
2
Conteúdo
1 Introdução
1.1 Referenciais . . . . . . . . . . . . . . . . .
1.2 Ângulos e transformação de Euler . . . .
1.3 Navegação inercial . . . . . . . . . . . . .
1.3.1 Plataforma mecânica ou Gimballed
1.3.2 Plataforma analı́tica ou Strapdown
1.4 Sistemas de atitude . . . . . . . . . . . . .
.
.
.
.
.
.
7
7
8
10
11
13
14
2 Sensores
2.1 Giroscópios . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
2.2 Acelerómetros . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
2.3 Magnetómetros . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
16
16
17
18
3 Filtragem
3.1 Filtragem complementar . . . . . . . . . . . . . . .
3.2 Filtragem Kalman discreta . . . . . . . . . . . . .
3.2.1 Amostragem de sistemas contı́nuos no tempo
3.2.2 Entradas determinı́sticas . . . . . . . . . . .
3.3 Filtro de Kalman estacionário . . . . . . . . . . . .
.
.
.
.
.
21
21
25
27
28
29
.
.
.
.
.
.
.
.
.
.
32
32
32
33
34
35
35
36
37
38
39
4 Filtro de Atitude
4.1 Filtro de atitude inicial . . . .
4.1.1 Modelo de estado . . .
4.1.2 Discretização . . . . .
4.1.3 Caracterı́sticas do filtro
4.2 Filtro de atitude Final . . . .
4.2.1 Modelo de estado . . .
4.2.2 Discretização . . . . .
4.2.3 Fecho do filtro . . . . .
4.2.4 Função de peso Pk . .
4.2.5 Caracterı́sticas do filtro
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
3
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
SISTEMA DE ATITUDE
CONTEÚDO
5 Simulação MATLAB
5.1 Experiência 1 - Atitude constante 1 . . . . .
5.2 Experiência 2 - Atitude constante 2 . . . . .
5.3 Experiência 3 - Rolamento sinusoidal 1 . . .
5.4 Experiência 4 - Rolamento sinusoidal 2 . . .
5.5 Experiência 4 - Mudança de rumo constante
5.6 Experiência 5 - Aceleração horizontal . . . .
.
.
.
.
.
.
40
40
40
43
44
44
47
6 Simulação com Dados Reais
6.1 Sensores utilizados e configuração do filtro . . . . . . . . . . . . . . . . . . .
6.2 Experiência 1 - Plataforma de atitude estacionária . . . . . . . . . . . . . . .
6.3 Experiência 2 - Rectângulo . . . . . . . . . . . . . . . . . . . . . . . . . . . .
50
50
51
53
7 Conclusões
56
A Simulação em MATLAB
A.1 Simulação . . . . . . .
A.2 Entradas . . . . . . . .
A.3 Modelo cinemático . .
A.4 Sensores . . . . . . . .
A.5 Filtragem . . . . . . .
.
.
.
.
.
58
58
59
59
60
62
.
.
.
.
.
.
.
64
64
65
66
67
68
69
71
B Código
B.1 rui
B.2 rui
B.3 rui
B.4 rui
B.5 rui
B.6 rui
B.7 rui
Desenvolvido
gyro s . . . . .
acel s . . . . . .
magnet s . . . .
acel contas s . .
magnet contas s
filtro kalman s .
matrizQ s . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
4
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
Lista de Figuras
1.1
1.2
1.3
1.4
1.5
Referenciais . . . . . . . . . . . . .
Referencial do veı́culo . . . . . . . .
Ângulos de Euler . . . . . . . . . .
Plataforma mecânica de navegação
Filtro de atitude . . . . . . . . . .
.
.
.
.
.
7
9
10
12
15
2.1
Campo magnético terrestre . . . . . . . . . . . . . . . . . . . . . . . . . . . .
19
3.1
3.2
3.3
3.4
3.5
3.6
3.7
3.8
Filtro complementar
Diagramas de Bode .
Ângulo de rumo . . .
Polarização . . . . .
Rejeição do filtro . .
Filtro de Kalman . .
Processo a estimar .
Filtro de Kalman . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
22
23
24
25
25
27
30
31
4.1
4.2
4.3
4.4
4.5
Modelo de estado inicial
Filtro de atitude inicial .
Modelo de estado final .
Filtro de atitude final . .
Função Peso . . . . . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
32
34
35
37
39
5.1
5.2
5.3
5.4
5.5
5.6
5.7
5.8
5.9
5.10
5.11
Polarizações e ângulo de rolamento
Ângulos de picada e de rumo . . . .
Ângulo de rolamento . . . . . . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
41
41
42
42
43
43
44
45
45
46
46
.
.
.
.
.
.
.
.
.
.
.
.
.
5
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
SISTEMA DE ATITUDE
LISTA DE FIGURAS
5.12
5.13
5.14
5.15
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
47
47
48
48
6.1
6.2
6.3
6.4
6.5
6.6
6.7
Saı́das dos acelerómetros e giroscópios .
Saı́das dos magnetómetros . . . . . . .
Polarizações e atitude . . . . . . . . . .
Saı́das dos acelerómetros e giroscópios .
Saı́das dos magnetómetros . . . . . . .
Ângulos de rolamento e picada . . . . .
Polarizações e atitude . . . . . . . . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
51
52
52
53
54
54
55
A.1
A.2
A.3
A.4
A.5
A.6
A.7
A.8
Simulação em Simulink R12 . .
Bloco das entradas . . . . . . .
Modelo cinemático . . . . . . .
Bloco dos sensores . . . . . . .
Parâmetros dos giroscópios . . .
Parâmetros dos acelerómetros .
Parâmetros dos magnetómetros
Parâmetros de filtragem . . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
58
59
60
60
61
61
62
63
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
6
.
.
.
.
.
.
.
.
Capı́tulo 1
Introdução
Neste capı́tulo introdutório, faz-se uma apresentação de sistemas de navegação, nomeadamente os sistemas inerciais e os sistemas de atitude de apoio à navegação, tendo como
referências principais [1] e [2]. Definem-se também os referenciais a utilizar e o modo como
se relacionam.
1.1
Referenciais
A posição, a velocidade e a atitude de um veı́culo estimadas por um navegador inercial têm
que ser expressas num determinado referencial de navegação. Para diferentes aplicações,
diferentes referencias poderão ser utilizados por questões de simplicidade de analise e processamento dos dados disponı́veis. Na figura (1.1), estão representados vários referenciais
assim como a superfı́cie terrestre.
Figura 1.1: Referenciais
De seguida descrevem-se os referenciais mais utilizados e os usados no decorrer deste
trabalho.
7
SISTEMA DE ATITUDE
1. INTRODUÇÃO
• Ref. Inercial Centrado na Terra (ECI - Earth Centered Inercial) - Este sistema de
coordenadas tem a sua origem no centro da Terra que está fixo relativamente às estrelas
que definem a orientação dos seus eixos. Um dos eixos deste sistema aponta na direcção
do eixo polar médio (médio porque o eixo polar precessa), estando outro eixo a apontar
para uma estrela cujo movimento é já conhecido formando o terceiro eixo um referencial
direito com os anteriores. Desta forma, podemos considerar que o referencial é inercial
para aplicações em que a precisão das observações não seja superior a 5 × 10−5 o /hr e
2 × 10−7 g. É usado para tabelar as coordenadas angulares de estrelas e em navegadores
inerciais
• Ref. Centrado e Fixo na Terra (ECEF - Earth Centered Earth Fixed) - É o referencial
tipicamente utilizado para a navegação próxima da Terra, sendo representado na figura
(1.1) pela moldura [y1 y2 y3 ]. Este referencial tem a sua origem no centro da Terra; o
eixo y3 coincide com o eixo de rotação da mesma passando o eixo y1 pelo meridiano de
Greenwich. Este referencial está fixo à Terra rodando com esta com ω = 15.04107 o /hr
relativamente ao referencial ECI. É o sistema de eixos usado na navegação baseada
em satélites, como o GPS (Global Positioning System), para o cálculo de posição de
veı́culos com receptores deste sistema de navegação
• Referencial Terrestre Local (RTC - Ref. Tangent Plane Coordinate) - Este sistema de
eixos é caracterizado pelo paralelismo existente entre o plano formado pelos eixos z1
e z2 e o nı́vel local. Tipicamente, z1 aponta para Este e z2 para Norte. z3 é assim
perpendicular à superfı́cie da Terra e aponta, usualmente, no sentido oposto ao centro
desta. É um referencial útil para operações com um raio de acção de algumas centenas
de Km da origem do referencial
Para além do referencial de navegação, há também que definir o referencial do veı́culo,
uma vez que é a bordo deste que estão os sensores que disponibilizam a informação de
navegação tais como acelerações, velocidades angulares, velocidades lineares, intensidades de
campos magnéticos, etc.
Mostra-se na figura (1.2) a definição do referencial do veı́culo (Body Axes). Neste caso,
a origem do referencial encontra-se no centro de massa do veı́culo; o seu eixo x aponta na
direcção longitudinal do mesmo; o eixo y define com x o plano horizontal do veı́culo sendo
o eixo z perpendicular a estes dois.
Neste trabalho, serão usados o referencial do veı́culo, ou corpo, que contém os sensores de
navegação e o referencial terrestre local que será designado também por referencial terrestre
ou inercial. Este pode ser considerado inercial, numa primeira abordagem, para navegação
de veı́culos de baixa velocidade cujo raio de acção é relativamente pequeno.
1.2
Ângulos e transformação de Euler
Para um dado referencial inercial, as três componentes de posição de um veı́culo são obtidas
por,
8
SISTEMA DE ATITUDE
1. INTRODUÇÃO
Figura 1.2: Referencial do veı́culo
xI = x0 +
yI = y0 +
zI = z0 +
Z t
0
Z t
0
Z t
0
uI (t)dt
(1.1)
vI (t)dt
(1.2)
wI (t)dt
(1.3)
em que [uI vI wI ]0 representa o vector velocidade nesse mesmo referencial.
Um sistema de navegação inercial possui informação do vector velocidade mas expresso no
referencial do corpo. Sendo assim, torna-se necessário aplicar uma transformação vectorial
em que se obtenha o vector velocidade no referencial terrestre. Na figura (1.3) estão representados os referenciais terrestre e do veı́culo, assim como as grandezas que os relacionam
em cada instante.
Os ângulos φ, θ e ψ designam-se por ângulos de Euler. Mais especificamente, φ é o ângulo
de rolamento (roll na literatura anglo-saxónica) do veı́culo, θ o ângulo de picada (pitch na
literatura anglo-saxónica) e ψ o ângulo de rumo (yaw na literatura anglo-saxónica).
A relação entre os referenciais terrestre e do veı́culo podem ser expressas em função destes
ângulos através da matriz expressa na equação (1.4) designada por matriz de transformação
de Euler.


c(ψ)c(θ)
s(ψ)c(θ)
−s(θ)

[E] = 
 c(ψ)s(θ)s(φ) − s(ψ)c(φ) s(ψ)s(θ)s(φ) + c(ψ)c(φ) c(θ)s(φ) 
c(ψ)s(θ)c(φ) + s(ψ)s(φ) s(ψ)s(θ)c(φ) − c(ψ)s(φ) c(θ)c(φ)
(1.4)
Este matriz [E] é ortonormal donde,
[E]T = [E]−1
9
(1.5)
SISTEMA DE ATITUDE
1. INTRODUÇÃO
Figura 1.3: Ângulos de Euler
kEk = [E]T [E] = 1
(1.6)
A transformação de um referencial para o outro é finalmente dada por,





l
i




[E]  m  =  j  ;
n T
k V



i
l



−1 
[E]  j  =  m 
k V
n T
(1.7)
em que os ı́ndices V e T se dizem respeito ao referencial do veı́culo e referencial terrestre
respectivamente.
Deste modo, para um navegador inercial é fundamental o conhecimento da atitude (ângulos
de Euler) do veı́culo que permitem relacionar a informação obtida no interior do mesmo,
com o referencial inercial.
1.3
Navegação inercial
Navegação inercial consiste na determinação da posição e velocidade de um veı́culo através de
observações da sua aceleração que são processadas num computador de bordo. Comparada
com outros métodos de navegação, a navegação inercial tem as seguintes vantagens:
• as indicações de posição e velocidade são instantâneas e contı́nuas sendo possı́vel utilizar sistemas caracterizados por grandes larguras de banda, requerendo no entanto
capacidade cálculo apreciável
10
SISTEMA DE ATITUDE
1. INTRODUÇÃO
• é completamente autónoma uma vez que se baseia em medidas de aceleração linear e
angular que são efectuadas no interior do próprio veı́culo. É um sistema que não radia
ondas electromagnéticas e não está sujeito a sabotagem exterior
• a informação de navegação pode ser obtida para todas as latitudes, incluindo as regiões
polares, independentemente do estado do tempo e sem necessidade se estações terrestres
• um sistema inercial disponibiliza informação de velocidade e posição em relação a um
referencial terrestre assim como a atitude do veı́culo em relação ao referencial terrestre
local
As desvantagens principais associadas a um navegador inercial são:
• a posição e velocidade degradam-se com o tempo, quer o veı́culo esteja em movimento
ou estacionário
• o equipamento é caro ($50.000 até $120.000 em 1996)
• é necessário alinhar o equipamento antes de cada missão; este processo torna-se mais
difı́cil para latitudes superiores a 75o e para veı́culos em movimento
• a precisão da navegação está dependente das manobras do veı́culo
As primeiras e mais dispendiosas aplicações práticas foram os sistemas balistı́cos para
lançamento de mı́sseis e os sistemas de navegação inercial para navios. Em finais dos anos
cinquenta, este tipo de sistemas começaram a ser aplicados na aviação militar. Muitas
das desvantagens dos sistemas inerciais podem ser ultrapassados com o apoio de outros
sensores como o GPS (Global Positionig System - sistema de rádio-navegação baseado em
emissores colocados em satélites) ou o Radar Doppler (sistema que permite a determinação
da velocidade de um veı́culo relativamente ao solo).
Em 1996, os sistemas de navegação inercial eram muito usados em todos os tipos de
veı́culos militares. Muitos navios, submarinos, mı́sseis guiados, aeronaves e outros veı́culos
militares estavam equipados com sistemas inerciais devido ao facto de serem autónomos e
não sabotaveis. Grandes companhias aéreas comerciais também usavam estes sistemas para
navegação e guiamento.
A navegação inercial, assenta na informação de sensores que inicialmente se encontravam
numa plataforma que se mantinha orientada com o referencial inercial. Com o aparecimento
dos computadores de grande velocidade de processamento, apareceu outra alternativa em
que os sensores passaram a estar fixos ao veı́culo, rodando com este. Distinguem-se assim,
dois tipos de plataforma de navegação que são descritas de seguida: plataforma mecânica e
plataforma analı́tica.
1.3.1
Plataforma mecânica ou Gimballed
Neste tipo de plataforma, o veı́culo pode rodar em torno dos seus eixos sem que o elemento
estável, onde se encontra a instrumentação de navegação, altere a sua orientação.
Na figura (1.4) mostra-se o diagrama de uma plataforma mecânica de 4 graus de liberdade.
As funções de uma plataforma estável são:
11
SISTEMA DE ATITUDE
1. INTRODUÇÃO
Figura 1.4: Plataforma mecânica de navegação
• orientar os sensores presentes no elemento estável (zona central da plataforma) segundo
um referencial fixo relativamente ao referencial inercial, apesar da rotação do veı́culo
• fornecer a atitude do veı́culo através da leitura dos ângulos de gimbal
• isolar os sensores de grandes velocidades angulares que poderiam alterar o funcionamento destes
• isolar os sensores termicamente e magneticamente
• amortecer as vibrações a que está sujeito o veı́culo
A presença de 4 eixos na plataforma evita a situação de gimbal lock em que para picadas
de 90o , os eixos de rolamento e rumo passam a ser colineares passando a plataforma a ter
apenas 2 graus de liberdade. Para plataformas de 3 eixos, a leitura da picada do veı́culo fica
limitada para ± 70o .
A orientação do elemento estável, pode ser mantida apenas através dos momentos de
inercia do elemento estável e do momento angular associado aos giroscópios, mas apenas
para alguns segundos de navegação em que a precessão da plataforma devido a momentos de
fricção é pequena. Na maior parte das vezes, a estabilização através de servo-mecanismos é
necessária. Os sinais de alimentação do circuito de estabilização são geralmente giroscópios de
precisão TDF (Two Degrees of Fredoom). Os actuadores deste sistema são motores aplicados
nos eixos da plataforma que corrigem a orientação desta.
A capacidade do elemento estável manter a sua orientação, depende de:
• erros associados aos giroscópios
• precisão dos motores de correcção
• movimento angular do veı́culo
• ortogonalidade dos componentes da plataforma
12
SISTEMA DE ATITUDE
1. INTRODUÇÃO
Parâmetro
Precisão na posição
Precisão na velocidade
Precisão na picada-rolamento
Precisão no rumo
Tempo de alinhamento giroscópico de rumo
Dimensões
Peso
Potência consumida
Aceleração máxima
Velocidade angular máxima
Tempo médio entre falhas em combate
Valor
0.8 nmi/hr
2.5 ft/seg RMS
0.05 o rms
0.05 o rms
3-8 min
500-1000 in3
20-30 lb
30-150 W
30 g
400 o /seg
3500 hr
Tabela 1.1: Especificações tı́picas de um navegador inercial em 1996
1.3.2
Plataforma analı́tica ou Strapdown
Neste tipo de plataforma, giroscópios e acelerómetros são montados e fixos directamente à
estrutura do veı́culo. Os giroscópios fazem o seguimento da rotação angular do veı́culo e
fornecem medidas ao algoritmo de cálculo do mesmo, de modo a que os ângulos de Euler, e
assim a orientação do veı́culo relativamente ao referencial terrestre, sejam conhecidos para
cada instante t. Os acelerómetros fornecem ao sistema as acelerações nos eixos do veı́culo
que são transformadas para o referencial terrestre e posteriormente integradas para se obter
o vector velocidade e posição no referencial inercial. Este tipo de plataforma, usa assim a
informação dos giroscópios para se poder fazer a transformação de qualquer vector expresso
no referencial do veı́culo para o referencial inercial, de navegação. Esta transformação para
o referencial inercial pode ser aplicada não só a acelerações mas também a velocidades,
intensidades de campos magnéticos, etc.
A precisão destas plataformas depende grandemente da precisão das estimativas dos
ângulos de Euler uma vez que um erro associado a estes, introduz imediatamente erros
em toda e qualquer transformação de referenciais. Sendo a posição obtida a partir de dupla integração da aceleração, um pequeno erro DC presente no vector aceleração provoca
uma divergência quadrática da posição real. Por outro lado, para muitas aplicações, são
necessários giroscópios de grande largura de banda, desde 0.005o /hr a 400o /seg.
As plataformas analı́ticas atingem um desempenho similar às plataformas mecânicas em
meados da década de oitenta graças ao aparecimento dos giroscópios ópticos de grande
largura de banda e ao desenvolvimento de computadores com grande capacidade de processamento. Na tabela (1.3.2) podem-se ver as caracterı́sticas tı́picas de uma plataforma
analı́tica.
Quando comparadas com as plataformas mecânicas, este tipo de plataforma proporciona
maior largura de banda, menor complexidade mecânica, maior fiabilidade, maior gama de
temperaturas de funcionamento e menor peso e volume. Por outro lado, a propagação
de erros nestas plataformas depende mais das trajectórias seguidas pelo veı́culo já que os
sensores estão sujeitos às manobras deste.
Nestas plataformas, a calibração dos sensores é fundamental, sendo que durante o processo
13
SISTEMA DE ATITUDE
1. INTRODUÇÃO
de fabrico é feita uma calibração que necessita de ser corrigida durante o funcionamento do
sensor para um melhor desempenho. Os factores mais importantes na calibração de fábrica
são as polarizações, os factores de escala e os erros de alinhamento. Estes variam com a
temperatura sendo fornecidos modelos em função da temperatura de funcionamento para se
poderem efectuar correcções durante a navegação do veı́culo.
De referir ainda que a própria plataforma analı́tica tem a capacidade de calcular e cancelar
alguns destes erros, como as polarizações, através de algoritmos de filtragem caso estejam
disponı́veis outros tipos de sensores de apoio à navegação como o GPS ou GLONASS, Radar
Doppler ou Star-Tracker.
1.4
Sistemas de atitude
Para se efectuar a estimação de atitude são necessários dois tipos distintos de informação: informação relativa ao vector gravidade local ou simplesmente referência vertical, e informação
respeitante ao rumo seguido (referência de rumo).
As referências verticais mais comuns são os nı́veis electrolı́ticos, pêndulos, acelerómetros
e giroscópios verticais. O uso de qualquer um destes sensores como referência vertical está
limitado às situações em que a única aceleração que lhes está aplicada é a gravı́tica. Nos
capı́tulos que se seguem esta situação é abordada com mais detalhe.
Como referências horizontais são usados tipicamente dispositivos que dependem do campo
magnético terrestre (bússolas e magnetómetros) ou giroscópios direccionais. Tanto os giroscópios direccionais como os giroscópios verticais mantêm a direcção do seu eixo de rotação
fixo no espaço. No caso dos primeiros a direcção de referência é o nı́vel local sendo o norte
geográfico usualmente escolhido como referência para o segundo.
Pode-se também utilizar simplesmente uma trı́ade de giroscópios sensı́veis às velocidades
angulares (rate gyros na literatura anglo-saxónica). Estes giroscópios seguem os movimentos
angulares do veı́culo permitindo o conhecimento da orientação do referencial deste em relação
a um referencial terrestre, sendo deste modo possı́vel a obtenção da atitude do veı́culo.
Muitas vezes, mesmo os melhores giroscópios disponı́veis levam a erros na determinação
de atitude incomportáveis. Por outro lado, os custos de instrumentação de grande precisão
são também demasiado elevados, e assim proibitivos, para determinadas aplicações, especialmente as civı́s.
Aparece assim uma nova abordagem na estimação de atitude que assenta no uso de
diferentes sensores que se complementam entre si. Na figura (1.5) está representada a estrutura de um possı́vel filtro de atitude baseado em três tipos de sensores: acelerómetros,
magnetómetros e giroscópios.
Deste modo, aparecem os veı́culos com sistemas inerciais de atitude que disponibilizam
informação aos outros sistemas de navegação em tempo real. Estes sistemas designam-se por
AHRS, Attitude and Heading Reference System.
14
SISTEMA DE ATITUDE
1. INTRODUÇÃO
Figura 1.5: Filtro de atitude
15
Capı́tulo 2
Sensores
Neste capı́tulo são apresentados os três tipos de sensores inerciais que são utilizados no
desenvolvimento do sistema de atitude: os acelerómetros, os giroscópios e os magnetómetros.
2.1
Giroscópios
O sistema de atitude mais simples necessita apenas de três giroscópios que fornecem medidas de velocidade angular (Rate Gyros) ortogonais entre si, que medem directamente as
velocidades angulares P , Q e R no referencial do veı́culo. Estas últimas, relacionam-se com
os ângulos de Euler e as suas derivadas por,
P = φ̇ − ψ̇ sin θ
(2.1)
Q = θ̇ cos φ + ψ̇ cos θ sin φ
(2.2)
R = −θ̇ sin φ + ψ̇ cos θ cos φ
(2.3)
Resolvendo para [φ̇ θ̇ ψ̇] em função de P , Q e R obtém-se,
φ̇ = P + Q sin φ tan θ + R cos φ tan θ
(2.4)
θ̇ = Q cos φ − R sin φ
sin φ
cos φ
ψ̇ = Q
+R
cos θ
cos θ
(2.5)
(2.6)
Este sistema de equações é frequentemente apresentado na forma matricial da equação (2.7).







φ̇
1 sin φ · tan θ cos φ · tan θ
P
P







cos φ
− sin φ 
  Q  = [Q]  Q 
 θ̇  =  0
sin φ
cos φ
R
R
0
ψ̇
cos θ
cos θ
16
(2.7)
SISTEMA DE ATITUDE
2. SENSORES
A matriz Q é uma matriz de rotação que transforma as velocidades angulares P , Q e R nas
derivadas dos ângulos de Euler [φ̇ θ̇ ψ̇].
Assim, dada a atitude inicial do veı́culo [φ0 θ0 ψ0 ], da integração das equações (2.4), (2.5)
e (2.6) resulta a atitude do veı́culo ao longo do tempo. No entanto, as velocidades angulares
medidas pelos giroscópios têm sempre erros associados.
Para estes sensores, atritos mecânicos e desiquilı́brios de massa nos elementos móveis do
giroscópio levam a que na saı́da do sensor apareçam termos não relacionados com a grandeza
que se pretende medir. Por outro lado, pequenas variações de temperatura também influenciam o sinal de saı́da dos giroscópios. A soma destes efeitos de baixa frequência designa-se
por taxa de deslizamento ou polarização (drif t rate ou bias na literatura ânglo-saxónica):
polarização caso o sinal de erro seja constante e taxa de deslizamento caso contrário.
A integração temporal destes erros, leva à divergência da atitude estimada ao fim de algum
tempo. Por este motivo, sistemas de atitude baseados na integração das equações de Euler,
são viáveis apenas quando estão disponı́veis giroscópios de grande precisão (implicando isto
um custo elevado), ou em aplicações de curta duração em que a integração é suficientemente
curta de modo a que o erro final obtido esteja dentro dos parâmetros da missão em causa.
2.2
Acelerómetros
O acelerómetro é um sensor cujo sinal de saı́da é proporcional à aceleração a que está sujeito.
É assim um dispositivo fundamental num navegador inercial.
A aceleração a que está sujeita uma trı́ade de acelerómetros alinhados com os eixos da
moldura fixa ao corpo de um veı́culo é dada por,










U̇ + QW − RV
anx
agx
ax
g · sin θ


 n   g 

 

 ay  =  ay  +  ay  =  V̇ + RU − P W  +  −g · cos θ · sin φ 
az
−g · cos θ · cos φ
anz
agz
Ẇ + P V − QU
(2.8)
O vector an contém as acelerações lineares aplicadas ao veı́culo e as acelerações centrı́petas
resultantes de ω × v. O vector ag representa o vector gravidade aplicado à trı́ade de acelerómetros instalados no veı́culo. Existem ainda outras acelerações como a aceleração de
Coriolis e a aceleração induzida quando a trajectória seguida pelo veı́culo não coincide
com um grande circulo (trajectórias fora do plano do equador e que não sigam a direcção
Norte). Estes efeitos, são no entanto de magnitude bastante menor que os presentes na
equação (2.8), sendo desprezáveis, numa primeira abordagem, para veı́culos de velocidade
relativamente baixa e com raio de acção pequeno.
Num sistema de atitude, a parcela útil na equação (2.8), é a que diz respeito à aceleração
gravı́tica que é sempre vertical relativamente à superfı́cie terrestre. Deste modo, uma trı́ade
ortogonal de acelerómetros pode funcionar como um pêndulo na obtenção dos ângulos φ e θ
do veı́culo. Considerando apenas os termos de aceleração gravı́tica, os ângulos de rolamento
e picada são obtidos por,
17
SISTEMA DE ATITUDE
2. SENSORES
φ = arctan
Ã g!
a
y

(2.9)
agz
agx


θ = arctan  q
(agy )2 + (agz )2
(2.10)
De notar que em ambas as equações não aparece a constante gravı́tica g que, como se sabe,
tem ligeiras variações com a latitude e a altitude.
Voltando à equação (2.8), a saı́da de um acelerómetro não inclui apenas a componente
gravı́tica donde a a extracção de φ e θ só é possı́vel quando o veı́culo não está sujeito a
acelerações lineares ou centrı́petas. Outra solução, consiste no cancelamento destas últimas
na saı́da dos acelerómetros, desde que estas grandezas sejam conhecidas através da utilização
de outros sistemas de navegação.
2.3
Magnetómetros
Grande parte dos sistemas de navegação usados em aeronaves, utiliza como referência de
rumo ψ, o campo magnético terrestre. Para tal usa-se uma trı́ade ortogonal de magnetómetros.
O campo magnético B, é medido em Gauss(W eber/cm2 ) e em T esla (104 Gauss). Numa
primeira abordagem, o campo magnético terrestre pode ser representado por um dipolo
cujo eixo está desviado 11o do eixo de rotação da Terra. A intensidade do campo varia
de 0.3 Gauss perto do equador a 0.6 Gauss nas regiões polares. Em qualquer ponto, o
ângulo de inclinação magnética (dip angle) representa o desvio do vector campo magnético
da horizontal local. A declinação magnética é o ângulo entre a horizontal do campo e o
Norte geográfico. Na figura (2.1) pode-se ver a configuração das linhas do campo magnético
da Terra.
O campo magnético terrestre, pode ser modelado mais exactamente, por uma série de
harmónicas esféricas. Quanto maior o detalhe pretendido no conhecimento do campo terrestre, maior a ordem do modelo. Modelos nacionais são desenvolvidos em função da latitude,
longitude e altitude. Deste modo é possı́vel conhecer com maior precisão as caracterı́sticas
do campo em qualquer ponto do globo.
Em navegação, a declinação magnética tem particular interesse uma vez que a soma desta
ao Norte magnético, permite a obtenção do Norte geográfico. De notar que a declinação
magnética sofre diferentes perturbações com o tempo tais como: oscilações diárias de ±0.1o ,
oscilações aleatórias devido a tempestades magnéticas na ordem dos 0.1o , variações devido
à migração dos pólos magnéticos na ordem dos 0.1o /ano.
Uma trı́ade ortogonal de magnetómetros, mede as três projecções do campo magnético
no referencial do veı́culo. No referencial de navegação, o vector campo H é dado por H =
A[cos λ 0 sin λ]T em que A corresponde à intensidade do campo, representando λ a inclinação
magnética. No referencial do veı́culo, usando (1.7) temos assim,
18
SISTEMA DE ATITUDE
2. SENSORES
Figura 2.1: Campo magnético terrestre






Hx
A cos λ
c(ψ)c(θ)c(λ) − s(θ)s(λ)






0
 Hy  = [E] 
 = A  c(ψ)s(θ)s(φ)c(λ) − s(ψ)c(φ)c(λ) + c(θ)s(φ)s(λ)  (2.11)
A sin λ
c(ψ)s(θ)c(φ)c(λ) + s(ψ)s(φ)c(λ) + c(θ)c(φ)s(λ)
Hz
Caso o veı́culo esteja nivelado (φ = θ = 0), o rumo ψ é obtido por,
µ
Hy
ψ = arctan −
Hx
¶
(2.12)
No caso geral, para quaisquer ângulos de rolamento e picada, há que rodar as componentes
obtidas pelos magnetómetros de φ e θ,

0






Hx
Hx
cos(θ) sin(φ) sin(θ) cos(φ) sin(θ)
Hx
0 





−1
−1 
0
cos(φ)
− sin(φ) 
 Hy  = [θ]R [φ]R  Hy  = A 
  Hy  (2.13)
0
Hz
− sin(θ) sin(φ) cos(θ) cos(φ) cos(θ)
Hz
Hz
Após a transformação da equação (2.13), pode-se então aplicar a equação (2.12) para a
obtenção do rumo seguido pelo veı́culo. De notar que este rumo tem a direcção do Norte
magnético tendo-se que se somar a declinação magnética, como referido atrás, para se obter
a direcção do Norte geográfico.
Um sistema baseado no campo magnético terrestre tem sempre associado erros devido à
presença de outros campos magnéticos nas proximidades dos magnetómetros. Estes podem
dever-se à estrutura do veı́culo ou aos sistemas electromecânicos deste. Distinguem-se assim
três tipos de erro:
• Hard Iron - Diz respeito às partes do veı́culo que se comportam como magnetos permanentes que mantêm a sua polaridade independentemente da atitude do mesmo. Este
19
SISTEMA DE ATITUDE
2. SENSORES
efeito pode ser representado por três magnetos e pode ser medido e compensado por
computador (depois de efectuadas as medições do campo estas são corrigidas) ou anulado através da introdução no veı́culo de três magnetos que cancelam este efeito
• Soft Iron - Este erro, deve-se aos componentes do veı́culo cuja permeabilidade magnética
é muito elevada. Estes, são assim magnetizados pela Terra por indução mantendo
sempre o pólo norte na direcção do pólo Norte terrestre. Desta forma, no referencial
do veı́culo, esta perturbação não é constante dependendo da atitude do veı́culo sendo a
sua compensação muito difı́cil. Este erro é particularmente importante nos navios, em
que estão presentes materiais ferromagnéticos. Na aviação, os materiais mais usados
são o alumı́nio e compósitos não magnetizáveis, materiais estes de baixa permeabilidade
magnética
• Equipamento Eléctrico - Como é sabido, a presença de correntes eléctricas elevadas
implica o aparecimento de campo magnéticos associados. Estes podem perturbar o
campo magnético terrestre. Por exemplo, uma corrente de 20Amp numa espira com
um metro de diâmetro causa um campo de 30 Gauss a 10 cm de distância do condutor.
Esta intensidade de campo é superior em mais de 100 vezes à intensidade do campo
terrestre. Assim, os magnetómetros terão que ser colocados em locais não expostos a
campos provenientes de outros equipamentos do veı́culo
20
Capı́tulo 3
Filtragem
Com o objectivo de efectuar, solucionar e resolver as tarefas de navegação, controlo e guiamento de grande precisão procurou-se, inicialmente, desenvolver o fabrico de sensores de
modo a que estes tivessem erros associados cada vez menores. Muitas vezes, o desenvolvimento deste tipo de sensores levava a custos elevadı́ssimos. Isto acontecia especialmente
nas aplicações militares. Por vezes, nem a melhor tecnologia disponı́vel permitia cumprir os
requisitos das missões a cumprir. Por exemplo, os melhores giroscópios mecânicos com erros
da ordem de 0.001o /hora, causam um erro de posição de cerca de 150m ao fim de uma hora
de voo.
Os erros tı́picos associados aos sensores são geralmente erros de alta frequência, associados a ruı́do e erros de banda estreita, ou erros próximos de DC, devidos a polarizações não
compensadas ou a termos de variação temporal pequena (long term drift). Aparece assim
a noção de fusão sensorial, em que se juntam pelo menos dois tipos de sensores distintos,
com erros complementares em frequência, que se ajudam mutuamente nas regiões do espectro em que cada um tem pior desempenho. Esta junção de observações é feita recorrendo
ao método da filtragem complementar, inicialmente implementado por tecnologia analógica
e, mais tarde, com o aparecimento de computadores de grande memória e velocidade de
processamento, pela filtragem de Kalman.
Este capı́tulo, é assim dedicado a estes dois tipos de filtragem e aos seus pontos em
comum.
3.1
Filtragem complementar
Na figura (3.1) mostra-se a estrutura base de um filtro complementar em que U1 (s) e U2 (s)
são as entradas do filtro, que tipicamente são sinais de saı́da de sensores, e Y (s) contém as
estimativas de filtragem.
No domı́nio do tempo a equação que caracteriza o filtro é a seguinte,
"
˙
b(t)
˙
Y (t)
#
"
=
0 0
1 0
#"
b(t)
Y (t)
#
"
+
21
k1
k2
#
"
[U1 (t) − Y (t)] +
0
U2 (t)
#
(3.1)
SISTEMA DE ATITUDE
3. FILTRAGEM
Figura 3.1: Filtro complementar
Passando agora para o domı́nio da frequência, a relação entre Y (s) e as entradas U1 (s) e
U2 (s) é dada por,
·
¸
·
K2 + K1 s
s
Y (s) = U1 (s) 2
+ U2 (s) 2
s + K1 s + K2
s + K1 s + K2
¸
(3.2)
Se U2 (t) = dtd U1 (t) => U2 (s) = sU1 (s), significando isto que U1 (s) fornece as observações do
sinal a estimar e U2 (s) as observações da derivada temporal desse mesmo sinal, a equação
(3.2) reduz-se a,
·
"
¸
#
K2 + K1 s
s2
Y (s) = U1 (s) 2
+U1 (s) 2
= U1 (s)
s + K1 s + K2
s + K1 s + K2
|
{z
}
{z
|
FT 1 (s)
(3.3)
}
FT 2 (s)
A saı́da Y (s) é assim o resultado da soma de duas parcelas. Na primeira, temos o produto
do sinal U1 (s) com a função de transferência FT 1 (s) que corresponde a um filtro passa-baixo.
Na segunda parcela, temos o produto do mesmo sinal U1 (s) por FT 2 (s), um filtro passa-alto.
A soma de FT 1 (s) e FT 2 (s) resulta na unidade obtendo-se assim na saı́da do filtro o sinal
U1 (s) para todo o espectro de frequências.
Na figura (3.2), estão representadas as funções FT 1 (s), FT 2 (s) assim como a a soma destas
duas.
A estabilidade do filtro depende da localização das raı́zes do denominador de FT 1 (s) e
FT 2 (s). Por sua vez, estas raı́zes dependem de K1 e K2 e são dadas por,
q
s1,2
K12 − 4K2
2
K1
±
=−
2
(3.4)
Como é sabido, a estabilidade do filtro é garantida quando ambas as raı́zes têm a sua parte
real negativa. Quando isto acontece, s1 e s2 definem um par de pólos complexos conjugados
com frequência natural e amortecimento dados por,
q
ω0 =
K2 rad/s
22
(3.5)
SISTEMA DE ATITUDE
3. FILTRAGEM
Bode Magnitude Diagram
5
0
−5
Magnitude (dB)
−10
−15
−20
−25
−30
−35
−40
−1
10
0
1
10
10
Frequency (rad/sec)
Figura 3.2: Diagramas de Bode de FT 1 (s), FT 2 (s) e da soma destas
ξ
K1
= √
2 K2
(3.6)
2
Por outro lado, FT 1 (s) tem um zero em s = − K
e FT 2 (s) tem um zero duplo na origem.
K1
Voltando à equação (3.3), tem-se que a saı́da Y (s) corresponde à soma do mesmo sinal
U1 (s) filtrado por um passa baixo e por um passa alto, de forma a que para toda a gama de
frequências a saı́da corresponda ao sinal U1 (s). Isto mesmo se observa na figura (3.2).
Assuma-se agora que U1 (s) = [ψ(s)+ RAF (s)] e U2 (s) = [sψ(s)+ Bias(s)], em que RAF (s)
tem um conteúdo espectral de alta frequência e Bias(s) é um sinal de variações temporais
pequenas sendo assim o seu espectro próximo de DC. Desta feita, obtemos a seguinte relação
entre a saı́da e entradas no sistema,
·
Y (s) = ψ(s) + RAF (s)
¸
·
K2 + K1 s
s
+
Bias(s)
s 2 + K1 s + K2
s2 + K1 s + K2
¸
(3.7)
Assim, na saı́da do filtro temos a grandeza a estimar, ψ(s) (que pode representar,por exemplo,
o ângulo de rumo de um veı́culo), e dois termos de ruı́do. No entanto, ambos os termos são
atenuados: o sinal de ruı́do RAF (s), de alta frequência, é multiplicado por FT 1 (s), um filtro
passa-baixo; o sinal Bias(s), de baixa frequência, é multiplicado por FT 2s(s) , um filtro passa
banda.
O filtro complementar, consiste pois na combinação de dois sinais cujos ruı́dos associados
sejam complementares em frequência (um sinal tem associado ruı́do de alta frequência, enquanto o outro tem ruı́do tı́pico de baixa frequência) de forma a aproveitar o espectro menos
ruidoso de cada um deles.
Na figura (3.3) mostra-se Y (t) e U1 (t) com ψ(t) = 0. Nesta situação, fez-se K1 = K2 = 1.
Após o regime oscilatório amortecido inicial, a estimativa de filtragem converge para o
valor correcto, registando oscilações de amplitude muito menores que as obtidas directamente
da leitura de U1 (s). Estas oscilações devem-se à introdução de ruı́do branco Gaussiano no
23
SISTEMA DE ATITUDE
3. FILTRAGEM
Ang. Rumo vs Tempo
3
Rumo Observado
Rumo Estimado
2
Rumo[º]
1
0
−1
−2
−3
0
20
40
60
80
100
Tempo[s]
120
140
160
180
200
Figura 3.3: Ângulo de rumo obtido na saı́da do filtro complementar
sinal U1 (s), cujo espectro não se restringe às altas frequências. Sendo assim, FT 1 (s) não
atenua a banda dos 0 rad/s a 1 rad/s estando o sinal Y (s) sujeito ao ruı́do presente nessa
banda de frequências.
Voltando à figura (3.1), pode-se mostrar que b(s) resulta de,
·
¸
·
K2
sK2
b(s) = −Bias(s) 2
+ RAF (s) 2
s + K1 s + K2
s + K1 s + K2
¸
(3.8)
Tendo agora em conta que ao sinal de alta frequência RAF (s) é aplicado um passa-banda,
obtém-se finalmente,
b(s) ≈ −Bias(s)
(3.9)
Assim, uma das caracterı́sticas da filtragem complementar consiste na estimação de polarizações (bias) presentes em U2 (s) sendo possı́vel o seu cancelamento. Qualquer polarização
presente em U1 (s) resulta num desvio da estimativa do filtro do valor real já que FT 1 (s)
possui ganho unitário em DC.
Na figura (3.4), pode-se comparar a polarização existente no sinal U2 (t) com −b(t), nas
mesmas condições das verificadas para a figura (3.3). Mais uma vez, verifica-se a convergência
da estimativa para o valor correcto. O facto de termos −bt=0 = 0 tem a ver com a condição
inicial introduzida no integrador que precede b(s), que neste caso é 0.
Por fim, na figura (3.5) mostram-se mais dois resultados obtidos com o filtro complementar
com K1 = K2 = 1. Desta vez, o ruı́do aplicado em U1 (s) consiste no sinal sin(ωt) com
ω1 = 3 rad/s no primeiro caso e ω2 = 10 rad/s no segundo. Em termos de frequência, U1 (s)
corresponde a um Delta de Dirac em ω. Em ambas as situações, se verifica que o filtro
atenua a frequência ω, sendo essa atenuação maior para ω = 10 rad/s, o que era esperado
já que a frequência de corte do filtro está localizada em ωc = 1 rad/s. A partir deste ponto,
o ganho do filtro decai a um ritmo de 20dB/dec. Quanto mais afastado ω de ωc , maior a
24
SISTEMA DE ATITUDE
3. FILTRAGEM
Bias Estimado vs Tempo
Bias Estimado
Bias Real
1.6
1.4
1.2
Bias[º/s]
1
0.8
0.6
0.4
0.2
0
0
20
40
60
80
100
Tempo[s]
120
140
160
180
200
Figura 3.4: Polarização estimada pelo filtro complementar
atenuação. Para ω = 10 rad/s essa atenuação é de 20dB = 10 sendo possı́vel verificar que a
amplitude do sinal estimado se diminuiu de 1 para 0.1.
Ang. Rumo vs Tempo
Ang. Rumo vs Tempo
1.5
1.5
Rumo Observado
Rumo Estimado
1
1
0.5
0.5
Rumo[º]
Rumo[º]
Rumo Observado
Rumo Estimado
0
0
−0.5
−0.5
−1
−1
−1.5
0
2
4
6
8
10
Tempo[s]
12
14
16
18
20
−1.5
0
2
4
6
8
10
Tempo[s]
12
14
16
18
20
Figura 3.5: Comparação da rejeição do filtro para ω = 3 rad/s e ω = 10 rad/s
Por outro lado, para ω < 1 rad/s o ganho de FT 1 (s) é unitário, não existindo assim atenuação do sinal de entrada. Mais uma vez se chama a atenção que qualquer polarização
existente em U1 (s) aparece na saı́da do filtro, Y (s). É assim fundamental proceder à calibração dos sensores usados nas entradas do filtro, em especial o sensor associado a U1 (s) no
que diz respeito a polarizações.
3.2
Filtragem Kalman discreta
Nesta secção descrevem-se as equações recursivas do filtro de Kalman discreto. Começa-se
por se assumir que o processo aleatório a ser estimado pode ser descrito por
25
SISTEMA DE ATITUDE
3. FILTRAGEM
xk+1 = φk xk + wk
(3.10)
As observações do processo ocorrem em intervalos discretos no tempo respeitando,
zk = Hk xk + vk
(3.11)
em que os vários termos das equações (3.10) e (3.11) são definidos por,
• xk = (n × 1) vector de estado no instante tk
• φk = (n × n) matriz de transição de estado que relaciona xk+1 com xk
• wk = (n × 1) vector de ruı́do branco do estado com covariância conhecida
• zk = (m × 1) vector das observações em tk
• Hk = (m × n) matriz que relaciona o vector de estado com as observações na ausência
de ruı́do
• vk = (m × 1) vector de ruı́do branco das observações com covariância conhecida
As matrizes de covariância de wk e vk são dados por
½
Qk ,
0,
½
Rk ,
E[vk viT ] =
0,
T
E[wk vi ] = ∀ (k, i)
E[wk wiT ]
=
para i = k
para i 6= k
para i = k
para i 6= k
(3.12)
(3.13)
(3.14)
Assumindo uma estimativa inicial do processo x̂−
k , obtém-se a estimativa x̂k recorrendo à
observação zk por
−
x̂k = x̂−
k + Kk (zk − Hk x̂k )
(3.15)
O erro cometido na estimativa de xk é caracterizado pela matriz de covariância Pk ,
Pk = E[(xk − x̂k )(xk − x̂k )T ] = (I − Kk Hk )Pk− (I − Kk Hk )T + Kk Rk KkT
(3.16)
Os termos da diagonal de Pk representam as variâncias dos erros de estimação das componentes do vector xk . Deste modo, quanto menor o valor da diagonal de Pk melhores as
estimativas do filtro.
26
SISTEMA DE ATITUDE
3. FILTRAGEM
É possı́vel demonstrar que a minimização da diagonal de Pk leva a que
Kk = Pk− HkT (Hk Pk− HkT + Rk )−1
(3.17)
Temos assim determinado o ganho Kk que aparece na equação (3.15). Este ganho, é obtido
fazendo a minimização do erro quadrático médio da estimativa xk e designa-se de ganho de
Kalman.
Substituindo (3.17) em (3.16) obtém-se a equação da matriz de covariância do erro, Pk ,
Pk = (I − Kk Hk )Pk−
(3.18)
Obtida a estimativa xk , pode-se fazer a predição da estimativa do processo em tk+1 com
x̂−
k+1 = φk x̂k
(3.19)
Por fim, faz-se a predição da matriz de covariância para tk+1
−
Pk+1
= φk Pk φTk + Qk
(3.20)
De notar que em (3.17) e (3.18) se tem que assumir a existência de uma estimativa inicial
para a matriz de covariância Pk− , tal como se fizera para estimativa do estado.
Tem-se finalmente, todas as quantidades necessárias para se calcular os passos seguintes de
filtragem. As equações (3.15), (3.17), (3.18), (3.19) e (3.20) são as equações finais recursivas
do filtro de Kalman discreto. Na figura (3.6) pode-se observar o algoritmo seguido pelo filtro.
Figura 3.6: Filtro de Kalman
3.2.1
Amostragem de sistemas contı́nuos no tempo
A filtragem de Kalman abordada, tem em conta processos discretos no tempo. Desta forma,
a aplicação a sistemas contı́nuos depende de uma prévia discretização do sistema a modular.
27
SISTEMA DE ATITUDE
3. FILTRAGEM
Assume-se que um processo contı́nuo pode ser descrito pela seguinte equação diferencial
ẋ = F x + Gu
(3.21)
em que x corresponde ao vector de estado a estimar e u é um vector cujos termos são ruı́do
branco. Considerando instantes discretos no tempo deste processo em t0 , t1 , ...tk , obtém-se
x(tk+1 ) = φ(tk+1 , tk )x(tk ) +
Z tk+1
tk
φ(tk+1 , τ )G(τ )U (τ ) dτ
(3.22)
A equação anterior pode ser apresentada simplesmente como
xk+1 = φk xk + wk
(3.23)
A matriz φk corresponde à matriz de transição referida atrás. O vector wk é um vector de
ruı́do originado pela presença de u no intervalo (tk , tk+1 ). Este vector wk é caracterizado
pela matriz de covariância do ruı́do de estado, Qk .
Assumindo que F é constante no intervalo (tk , tk+1 ), pode-se obter a matriz de transição
de estado por,
φk = eF ∆t = I + F ∆t +
(F ∆t)2
+ ...
2!
(3.24)
Quando ∆t assume valores muito pequenos, a equação anterior resume-se aos dois primeiros
termos, sendo o efeito dos restantes praticamente desprezável. Existem também diversos
métodos numéricos que permitem o cálculo desta exponencial. Por exemplo, em ambiente
M AT LAB está disponı́vel a função expm.
A matriz Qk , por definição, é dada por
Qk =
E[wk wkT ]
=
Z tk+1 Z tk+1
tk
tk
φ(tk+1 , ξ)G(ξ)E[u(ξ)uT (η)]GT (η)φT (tk+1 , η) dξ dη
(3.25)
A obtenção de uma expressão explicita para esta matriz é possı́vel para sistemas de dimensão
reduzida. Quando isto não acontece, há que recorrer a métodos numéricos na determinação
da matriz.
3.2.2
Entradas determinı́sticas
Muitas vezes, o processo a estimar contém também termos determinı́sticos. Um processo
contı́nuo deste tipo é descrito por
ẋ = F x + Gu + Bud
28
(3.26)
SISTEMA DE ATITUDE
3. FILTRAGEM
em que Bud corresponde ao termo determinı́stico do processo. Esta alteração do sistema
contı́nuo traduz-se na filtragem discreta de Kalman na equação de predição de x̂−
k+1 .Esta fica
na forma
x̂−
k+1
= φk x̂k +
Z tk+1
tk
φ(tk+1 , τ )B(τ )U (τ ) dτ
(3.27)
O integral representa a contribuição do termo determinı́stico no intervalo (tk , tk+1 ). De notar
que as restantes equações de filtragem permanecem inalteradas.
3.3
Filtro de Kalman estacionário
Considera-se agora o filtro de Kalman contı́nuo e as suas equações, ainda que de modo
resumido. Mais pormenores poderão ser obtidos em [3]
Assumam-se as seguintes equações que descrevem o processo a estimar e as observações
do mesmo,
ẋ = F x + Gu
z = Hx + v
(3.28)
(3.29)
E[u(t)uT (τ )] = Qδ(t − τ )
E[v(t)v T (τ )] = Rδ(t − τ )
E[u(t)v T (τ )] = 0
(3.30)
(3.31)
(3.32)
em que
Os vectores u(t) e v(t) representam ruı́do branco com correlação cruzada nula.
As relações entre as matrizes do filtro discreto Qk e Rk com Q e R são
Qk = GQGT ∆t
R
Rk =
∆t
(3.33)
(3.34)
A equação principal do filtro contı́nuo é assim,
x̂˙ = F x̂ + K(z − H x̂)
Mais uma vez, o ganho K representa o ganho de Kalman que pode ser obtido por,
29
(3.35)
SISTEMA DE ATITUDE
3. FILTRAGEM
K = P H T R−1
(3.36)
sendo que a matriz P é novamente a matriz de covariância do erro das estimativas de filtragem. Esta matriz é finalmente dada por,
Ṗ = F P + P F T − P H T R−1 HP + GQGT , P (0) = P0
(3.37)
Esta última equação, tem a estrutura de uma equação de Riccati. A resolução desta equação
diferencial, fornece a matriz P da qual depende o ganho de Kalman.
As equações (3.35), (3.36) e (3.37) constituem as três equações do filtro de Kalman
contı́nuo.
Assuma-se agora o processo representado na figura (3.7), em que as variâncias dos ruı́dos
w1 e w2 são σ1 e σ2 respectivamente, e ud representa um termo determinı́stico.
Figura 3.7: Processo a estimar
A equação que define o processo é
"
ẋ1
ẋ2
#
"
=
0 0
1 0
#"
x1
x2
#
"
+
1 0
0 1
#"
w1
w2
#
"
+ ud
0
1
#
(3.38)
Estando disponı́veis observações da variável x2 , o modelo das observações é dado por
z=
h
0 1
i
"
x1
x2
#
+ uσ v
(3.39)
em que σv é a variância do ruı́do branco associado às observações de x2 . A equação do filtro
fica assim na seguinte forma,
"
x˙1
x˙2
#
"
=
0 0
1 0
#"
x1
x2
#
"
+
k1
k2
#
"
[z − x2 ] +
0
ud
#
(3.40)
Na figura (3.8), tem-se a representação do filtro de Kalman obtido. Como se pode observar, este filtro tem exactamente a mesma estrutura do filtro complementar da figura (3.1).
Pode-se também ver que a equação (3.40) é idêntica à equação (3.1) relativa ao filtro complementar. A única diferença reside no facto de os ganhos no filtro de Kalman serem variáveis e
30
SISTEMA DE ATITUDE
3. FILTRAGEM
Figura 3.8: Filtro de Kalman
resultarem de uma optimização das estimativas obtidas em função da estatı́stica dos ruı́dos
associados aos estados e às observações do processo.
Voltando à equação (3.37), verifica-se que após o regime inicial de filtragem, Ṗ → 0, significando isto que a matriz de covariância das estimativas converge para um valor estacionário,
solução da equação (3.41).
0 = F P + P F T − P H T R−1 HP + GQGT
(3.41)
Sendo assim, o ganho de Kalman converge para um valor estacionário após o regime transitório do filtro. Pode-se assim definir um filtro de Kalman com ganho constante e igual ao
ganho estacionário. Nestas condições, o filtro designa-se por filtro de Kalman estacionário.
Obteve-se pois exactamente o filtro complementar descrito no inicio deste capı́tulo em
que os ganhos K1 e K2 correspondem aos ganhos estacionários de Kalman obtidos através
da resolução da equação (3.41). De notar, que o ganho estacionário é óptimo apenas se a
estatı́stica dos ruı́dos de estado e das observações não se alterar durante a filtragem.
Para sistemas de dimensão elevada, a solução da equação (3.41) é, na prática, obtida
numericamente. Em MATLAB, foi desenvolvido o procedimento lqe (linear quadratic estimator) que calcula o ganho estacionário de Kalman e a matriz de covariância em função das
caracterı́sticas do processo a estimar. Paralelamente, foi desenvolvido o algoritmo equivalente
para sistemas discretos, o dlqe (digital linear quadratic estimator).
31
Capı́tulo 4
Filtro de Atitude
Neste capı́tulo, apresentam-se os dois filtros de atitude simulados. O primeiro, é um filtro
mais simples em termos de modelo de estado e discretização, sendo que o segundo apresenta
melhores propriedades no que respeita à estimação das variáveis de estado. Apresentam-se
de seguida, os modelos de estado e as equações discretas de ambos os filtros assim como as
vantagens e desvantagens de cada um deles.
4.1
4.1.1
Filtro de atitude inicial
Modelo de estado
Na figura (4.1), está representado o modelo de estado assumido inicialmente para o filtro de
atitude.
Figura 4.1: Modelo de estado inicial
Como se pode ver, o filtro tem 6 variáveis de estado, 6 termos de ruı́do branco e 3 entradas
determinı́sticas, representando cada um destes termos o seguinte:
• x1 , x2 , x3 - variáveis de estado respeitantes às estimativas dos ângulos de Euler φ, θ e ψ
respectivamente
32
SISTEMA DE ATITUDE
4. FILTRO DE ATITUDE
• x4 , x5 , x6 - variáveis de estado correspondentes aos bias provenientes das entradas determinı́sticas
• w1 , w2 , w3 , w4 , w5 , w6 - ruı́do associado aos estados x1 a x6 com variâncias σ1 a σ6
• φ̇, θ̇, ψ̇ - entradas das derivadas dos ângulos de Euler provenientes dos giroscópios (após
a aplicação da matriz Q), associadas a um termo constante bφ , bθ , bψ que simula as
polarizações caracterı́sticas de cada uma destas entradas
A equação que define este processo é assim
ẋ = F x + Gw + Bud





"
# x˙1
x˙1


 . 

 .  = 03 I3  ..  + [I6 ] 
.
 . 



03 03
x˙6
x˙6
(4.1)



"
# φ̇
ẇ1
I3 

.. 
 θ̇ 
. 
+ 0
3
ψ̇
ẇ6
(4.2)
em que I e 0 representam as matrizes identidade e nula respectivamente. O ı́ndices destas
matrizes representam as suas dimensões.
Como observações dispomos dos ângulos φ, θ e ψ provenientes dos acelerómetros e magnetómetros. Temos assim a seguinte equação respeitante ao modelo das observações,
z = Hx + v



(4.3)



z˙1
x˙
vφ
h
i 1 
 . 

 .  = I3 03  ..  + 
 . 
 .   vθ 
vψ
z˙3
x˙6
(4.4)
em que v corresponde ao vector de ruı́do associado às observações zi com variâncias σφ , σθ
e σψ .
4.1.2
Discretização
Tendo em conta as equações (3.24) e (3.25), obtém-se para o filtro as seguintes matrizes φk
e Qk ,
(F ∆t)2
+ ··· =
φk = I + F ∆t +
2!






Qk = 




3
σ1 ∆t + σ4 ∆t3
0
0
σ4 ∆t2
0
0
"
I3 ∆tI3
0
I3
#
0
0
σ4 ∆t2
0
0
3
2
∆t
σ2 ∆t + σ5 3
0
0
σ5 ∆t
0
∆t3
0
σ3 ∆t + σ6 3
0
0
σ6 ∆t2
0
0
σ4 ∆t
0
0
σ5 ∆t2
0
0
σ5 ∆t
0
2
0
σ6 ∆t
0
0
σ6 ∆t
33
(4.5)











(4.6)
SISTEMA DE ATITUDE
No modelo discreto das observações temos,
Hk =
h
I3 03
i

(4.7)

σφ 0 0


Rk =  0 σθ 0 
0 0 σψ
(4.8)
Finalmente, tendo em conta a equação (3.27), o termo determinı́stico para este filtro é dado
por,
udk = ∆tφk Bk ud
(4.9)
em que Bk ≡ B. Tem-se finalmente, todos os termos necessários às equação do filtro de
Kalman para o modelo de estado assumido.
4.1.3
Caracterı́sticas do filtro
Na figura (4.2), está representado o filtro de Kalman obtido com o anterior modelo de estado.
Figura 4.2: Filtro de atitude inicial
As equações discretas do filtro são as seguintes,
x̂k = xˆ− + Kk [z − Hk xˆ− ]
x̂−
k+1 = φk xˆk + ∆tφk Bk u
(4.10)
(4.11)
O ganho estacionário de Kalman Kk é obtido recorrendo à função dlqe da seguinte forma,
34
SISTEMA DE ATITUDE
Kk = dlqe(φk , G, Hk , Qk , Rk )
(4.12)
Como caracterı́sticas principais temos que o filtro é estável, linear e invariante no tempo, o
que não se verifica no filtro de atitude apresentado de seguida.
Os resultados obtidos para este filtro foram positivos, ou seja, após o regime inicial as
variáveis [x1 x2 x3 ] convergem para os ângulos de Euler enquanto [x4 x5 x6 ] convergem
para os valores simétricos das polarizações anulando assim este erro de baixa frequência. O
grande inconveniente deste filtro está na variação das polarizações com a atitude do veı́culo
uma vez que estas dependem da matriz Q. Deste modo, cada variação de atitude do veı́culo
implica um novo regime transitório até o filtro anular convenientemente as novas polarizações
e, fornecer assim a atitude com a máxima correcção possı́vel.
4.2
4.2.1
Filtro de atitude Final
Modelo de estado
Assuma-se agora o modelo de estado da figura (4.3).
Figura 4.3: Modelo de estado final
Como se pode ver, neste modelo está já presente a matriz Q e a sua inversa. Desta feita, as
saı́das dos giroscópios P , Q e R entram directamente no filtro sendo internamente passadas
pela matriz Q, de modo a que o integrador tenha na sua entrada as grandezas φ̇, θ̇ e ψ̇.
0
A matriz Q−1 desempenha a função inversa da matriz Q. De facto, as variáveis [x4 x5 x6 ]
têm a função de reter as polarizações presentes directamente na saı́da dos giroscópios. São
assim polarizações respeitantes ao referencial do veı́culo. Por outro lado, o erro proveniente
da comparação das estimativas de atitude com as observações da mesma diz respeito ao
referencial inercial. Desta forma, a matriz Q−1 transforma este erro do referencial inercial
para o referencial do veı́culo, sendo depois integrado.
Podemos pois considerar que existem dois espaços distintos no modelo de estado da figura
(4.1). O primeiro espaço contem os sinais entre as matrizes Q−1 e Q está associado ao
referencial do veı́culo, estando os restantes sinais associados ao referencial inercial.
35
SISTEMA DE ATITUDE
A equação relativa a este modelo de estado é a seguinte,
ẋ = F x + Gw + Bud
(4.13)







"
# x˙1
"
# ẇ1
"
# P
x˙1


 . 


03

 .  = 03 Q  ..  + Q
 ..  + Q 
 Q 
 . 
03 03  . 
03 Q−1  . 
03
R
x˙6
x˙6
ẇ6
(4.14)

De notar que neste filtro as entradas determinı́sticas são directamente as saı́das dos giroscópios P , Q, e R, o que não se verificava no filtro anterior.
O modelo das observações é idêntico ao anterior em que,
z = Hx + v



(4.15)



z˙1
x˙
vφ
h
i 1 
 . 
 .  = I3 03  ..  + 
vθ 


 . 
 . 
vψ
z˙3
x˙6
(4.16)
Como se pode observar das equações do modelo de estado, este filtro não é linear nem
invariante no tempo devido à presença das matrizes Q e Q−1 . Esta é a principal desvantagem
deste filtro em relação ao filtro analisado anteriormente.
4.2.2
Discretização
Assumindo que F é constante no intervalo [tk , tk+1 ], o que equivale a dizer que a matriz Q é
também constante neste intervalo (esta aproximação é tanto mais válida quanto menor for
∆t), a matriz de transição é dada por,
(F ∆t)2
+ ··· =
φk = I + F ∆t +
2!
"
I3 ∆tQ
0
I3
#
(4.17)
Para este modelo de estado, o cálculo da matriz Qk torna-se muito pesado uma vez que
envolve as matrizes G e φk que dependem directamente da matriz de rotação Q que depende
de t. Optou-se assim por se fazer uma Q ≡ I3 o que é válido apenas para atitudes próximas
0
de 0. A matriz Qk é pois dada por,





0

Qk = 




σ1 ∆t + σ4 ∆t3
0
0
σ4 ∆t2
0
0
3
0
0
σ4 ∆t2
0
0
3
2
∆t
0
0
σ5 ∆t
0
σ2 ∆t + σ5 3
∆t3
0
σ3 ∆t + σ6 3
0
0
σ6 ∆t2
0
0
σ4 ∆t
0
0
σ5 ∆t2
0
0
σ5 ∆t
0
2
0
σ6 ∆t
0
0
σ6 ∆t
36











(4.18)
SISTEMA DE ATITUDE
Para o modelo das observações tem-se,
Hk =
h
I3 03
i
(4.19)


σφ 0 0


Rk =  0 σθ 0 
0 0 σψ
(4.20)
Finalmente, a matriz Bk , que relaciona o vector das entradas com o vector de estado, é agora
dada por,
"
Bk =
4.2.3
Q
03
#
(4.21)
Fecho do filtro
Na figura (4.4) está representado o filtro em malha fechada relativo ao modelo de estado
assumido.
Figura 4.4: Filtro de atitude final
As equações discretas que o caracterizam são,
x̂k = xˆ− + Mk Kk [z − Hk xˆ− ]
x̂−
k+1 = φk xˆk + ∆tφk Bk u
(4.22)
(4.23)
A matriz Kk é, mais uma vez, dada pelo ganho estacionário de Kalman obtido através da
função dlqe por,
0
0
0
Kk = dlqe(φk , G , Hk , Qk , Rk )
37
(4.24)
SISTEMA DE ATITUDE
Mais uma vez, o cálculo do ganho óptimo é apenas possı́vel para filtros invariantes no tempo.
0
Seguiu-se a mesma aproximação para a obtenção da matriz Qk , ou seja, substituiu-se a matriz
Q pela matriz identidade nas matrizes φk e G. O ganho é assim óptimo para φ = 0 e θ = 0.
Na equação (4.22), nota-se a presença de uma nova matriz, a matriz Mk . Esta matriz
tem a função de relacionar os erros de realimentação do filtro com o vector de estado. Desta
forma, Mk é dada por,
"
Mk =
I3 03
03 Q−1
#
(4.25)
Na prática, esta matriz permite que o erro relativo às variaveis [x4 x5 x6 ] seja multiplicado
pela matriz Q−1 , de acordo com a figura (4.4). No filtro abordado anteriormente, esta matriz
não aparecia nas equações do filtro, uma vez que os erros de realimentação eram directamente
injectados nos integradores do modelo de estado.
4.2.4
Função de peso Pk
Como foi referido no capı́tulo 2, as medições dos ângulos de rolamento e picada apresentam
valores correctos sempre que aos acelerómetros apenas esteja aplicada a aceleração gravı́tica.
Quando isto não acontece, as estimativas de φ e θ, que entram no filtro como observações,
apresentam valores incorrectos sendo a atitude estimada completamente desviada da atitude
real do veı́culo.
Nesta situação, uma solução possı́vel consiste em usar apenas os giroscópios na determinação da atitude. Caso isto aconteça após algum tempo de correcto funcionamento do
filtro, as polarizações associadas aos giroscópios poderão estar já anuladas apresentando a
integração directa dos giroscópios resultados aceitáveis.
Define-se assim uma função de peso pk (∆g) por,
pk (∆g) =
kn
k n + (∆g)n
¯
¯
1
¯
¯
, ∆g = ¯(a2x + a2y + a2z ) 2 − g ¯
(4.26)
Na figura (4.5) tem-se a representação desta função com k = 1 e n = 6.
Para além de ∆g, esta função depende também de dois outros parâmetros que podem ser
escolhidos para ajuste da curva.
• k - Este parâmetro, é tal que quando ∆g = k se tem pk = 0.5. Desta forma, pode-se
definir a largura da ”janela”da função peso. Esta largura dependerá do ruı́do presente
nos acelerómetros. Caso fossem perfeitos, este valor poderia ser muito próximo de zero
funcionando a função como um interruptor. Existindo ruı́do nos acelerómetros, k é
escolhido de modo a que a aceleração gravı́tica somada ao ruı́do esteja no interior desta
janela, prevenindo-se a aplicação indevida desta estratégia
• n(≥ 1) - Define o declive da zona de transição da função peso. Quanto maior o seu
valor, mais rápida a transição de 1 para 0 aproximando-se de uma onda rectangular
quanto n → ∞
38
SISTEMA DE ATITUDE
1
0.9
0.8
0.7
Peso
0.6
0.5
0.4
0.3
0.2
0.1
0
−5
−4
−3
−2
−1
0
dg
1
2
3
4
5
Figura 4.5: Função Peso vs. ∆g , k = 1 e n = 6
4.2.5
Caracterı́sticas do filtro
Em primeiro lugar, este filtro não é linear e é variante no tempo devido à introdução da matriz
Q no modelo de estado. Este facto impossibilita ainda a análise clássica da estabilidade do
filtro, (as caracterı́sticas deste filtro serão objecto de estudo num futuro próximo). Por
outro lado, como visto anteriormente, os ganhos obtidos através da função dlqe são óptimos
apenas para ângulos de rolamento e picada nulos. No entanto, como se verá nos dois capı́tulos
seguintes, o filtro tem um bom desempenho global no que respeita à estimação da atitude
e das polarizações provenientes dos giroscópios. Tendo ainda em conta a estimação das
polarizações, esta é feita independentemente da atitude do veı́culo, sendo esta a principal
vantagem deste filtro face ao anterior. Desta feita, as alterações de atitude não originam
alterações nas polarizações a estimar. Isto é importante para a aplicação da função P eso(k)
uma vez que quando o filtro funciona em malha aberta, este recorre apenas à integração dos
giroscópios sendo fundamental que as polarizações estejam correctamente anuladas.
39
Capı́tulo 5
Simulação MATLAB
Neste capı́tulo são mostrados os resultados obtidos na simulação do filtro de atitude estudado.
A simulação foi feita em ambiente MATLAB recorrendo ao Simulink R12. No anexo A
são mostrados os vários blocos que constituem a simulação implementada assim como os
parâmetros de entrada de cada um deles.
De seguida, são analisadas algumas situações em que são abordados os aspectos considerados mais relevantes no filtro de atitude. De referir que se mostram apenas resultados
relativos ao filtro final, desenvolvido no capı́tulo anterior, uma vez que este foi o que apresentou melhor desempenho.
5.1
Experiência 1 - Atitude constante 1
Começou-se por testar o filtro para uma atitude fixa. Neste caso, fez-se φ = 5o , θ = 10o
e ψ = 15o , sendo as estimativas iniciais do filtro iguais a cinco graus. Utilizou-se também
uma frequência de filtragem de 50Hz sendo a variância do ruı́do associado a cada sensor
da ordem de 0.001 (m2 /s4 , rad2 /seg 2 ou Gauss2 ). Por fim, as polarizações associadas aos
giroscópios foram de 0.01rad/s, 0.02rad/s e 0.03rad/s sendo a primeira relativa ao eixo x,
a segunda ao y e a terceira ao eixo z.
Como se pode ver nas figuras (5.1) e (5.2), após um regime transitório bastante pequeno
(está associado aos ganhos do filtro, ou seja, quanto maiores os ganhos mais curto é o
transitório) as polarizações estimadas pelo filtro convergem para os valores correctos, ou
seja, os simétricos das polarizações de saı́da de cada giroscópio. Os picos verificados no
regime transitório são tanto maiores quanto maiores são os desvios das estimativas iniciais
de atitude dos ângulos de Euler reais.
Quanto à atitude estimada, pode-se ver que após o transitório esta converge para a atitude
real.
5.2
Experiência 2 - Atitude constante 2
Nesta simulação, repetiram-se todas as condições da simulação anterior, alterando-se apenas
a variância dos ruı́dos associados aos sensores, aumentando-a dez vezes.
40
SISTEMA DE ATITUDE
5. SIMULAÇÃO MATLAB
Ang. Rolamento vs Tempo
Bias Giroscopios vs Tempo
Fi estimado
Fi
6
BFi
BTeta
BPsi
0.12
5.8
0.1
5.6
5.4
Rolamento[º]
Bias (rad/s)
0.08
0.06
0.04
5.2
5
4.8
0.02
4.6
0
4.4
−0.02
4.2
4
0
5
10
15
20
25
Tempo[s]
30
35
40
45
0
50
5
10
15
20
25
Tempo[s]
30
35
40
45
50
Figura 5.1: Polarizações e ângulo de rolamento
Ang. Rumo vs Tempo
12
18
11
16
10
14
9
12
Rumo[º]
Picada[º]
Ang. Picada vs Tempo
8
10
Teta estimado
Teta
7
8
6
6
5
Psi estimado
Psi
4
0
5
10
15
20
25
Tempo[s]
30
35
40
45
50
0
5
10
15
Figura 5.2: Ângulos de picada e de rumo
41
20
25
Tempo[s]
30
35
40
45
50
SISTEMA DE ATITUDE
0.14
BFi
BTeta
BPsi
0.12
Fi estimado
Fi
6
5.8
0.1
5.6
5.4
Rolamento[º]
Bias (rad/s)
0.08
0.06
0.04
5.2
5
4.8
0.02
4.6
0
4.4
−0.02
4.2
−0.04
4
0
5
10
15
20
25
Tempo[s]
30
35
40
45
0
50
5
10
15
20
25
Tempo[s]
30
35
40
45
50
Ang. Rumo vs Tempo
12
18
11
16
14
9
Rumo[º]
Picada[º]
10
8
12
10
Teta estimado
Teta
7
Psi estimado
Psi
8
6
6
5
4
0
5
10
15
20
25
Tempo[s]
30
35
40
45
50
0
5
10
15
42
20
25
Tempo[s]
30
35
40
45
50
SISTEMA DE ATITUDE
Como esperado, os resultados mostrados nas figuras (5.3) e (5.4) são idênticos aos obtidos
anteriormente, verificando-se apenas que o ruı́do obtido na estimação tanto das polarizações
como da atitude é bastante superior ao registado na primeira simulação.
5.3
Experiência 3 - Rolamento sinusoidal 1
Optou-se agora por se simular uma situação em que um veı́culo se desloca com velocidade
constante segundo o eixo x de 10m/s e com velocidade angular sinusoidal em torno do mesmo
eixo com frequência de 0.1Hz e amplitude de 5rad/s.
Fi estimado
Fi
18
−0.01
16
−0.02
14
−0.03
Rolamento[º]
Bias (rad/s)
12
−0.04
−0.05
10
8
−0.06
6
−0.07
4
2
BFi
BTeta
BPsi
−0.08
0
0
5
10
15
20
25
Tempo[s]
30
35
40
45
0
50
5
10
15
20
25
Tempo[s]
30
35
40
45
50
Ang. Rumo vs Tempo
5
5
Psi estimado
Psi
Teta estimado
Teta
4.5
4
4
3.5
3
Rumo[º]
Picada[º]
3
2.5
2
2
1.5
1
1
0
0.5
0
0
5
10
15
20
25
Tempo[s]
30
35
40
45
50
0
5
10
15
20
25
Tempo[s]
30
35
40
45
50
Verifica-se mais uma vez, que após o regime transitório o filtro converge para os valores
pretendidos. Em particular, para o ângulo de rolamento vê-se que o filtro acompanha a
43
SISTEMA DE ATITUDE
evolução sinusoidal do ângulo de rolamento. Lembra-se que a frequência de filtragem é de
50Hz, sendo portanto muito superior à frequência do movimento a seguir.
5.4
Experiência 4 - Rolamento sinusoidal 2
Nesta simulação testa-se uma situação idêntica à anterior mas com um movimento de
frequência de 1Hz e amplitude de 50rad/s. No primeiro caso a frequência de filtragem
usada é de 1Hz e de 5Hz no segundo.
16
Fi estimado
Fi
Fi estimado
Fi
20
14
18
16
12
14
Rolamento[º]
Rolamento[º]
10
8
6
12
10
8
6
4
4
2
2
0
0
0
5
10
15
20
0
2
4
Tempo[s]
6
8
10
Tempo[s]
12
14
16
18
20
Figura 5.7: Ângulo de rolamento
Como se pode observar na figura (5.7), no primeiro teste, a estimativa do ângulo de
rolamento não acompanha o movimento sinusoidal desta. Isto pode ser explicado recorrendo
ao teorema de N yquist que diz que a frequência de amostragem de um sinal deve ser superior
ao dobro da frequência máxima existente nesse mesmo sinal. Isto não se verifica neste caso.
Acontece que em cada instante de filtragem o ângulo de rolamento observado pelo filtro é de
0o , motivo pelo qual a estimativa deste ângulo se mantém próxima deste valor.
No segundo teste efectuado, pode-se ver que o filtro já consegue acompanhar o ângulo
de rolamento sinusoidal imposto com maior exactidão. Desta feita o teorema de N yquist é
satisfeito. Caso a frequência de amostragem fosse ainda maior, melhor seria o seguimento
do ângulo de rolamento.
5.5
Experiência 4 - Mudança de rumo constante
Nesta simulação pretende-se mostrar o efeito de uma aceleração centrı́fuga no desempenho
do filtro desenvolvido. Simulou-se assim uma situação em que a velocidade de um veı́culo
segundo o eixo x é de 10m/s sendo a velocidade angular em torno de z de 1o /s. Na prática,
trata-se de uma mudança de rota de um veı́culo em movimento uniforme.
Como se pode ver nas figuras (5.8) e (5.9), apesar de as polarizações convergiram para
os valores esperados, os ângulos de rumo e rolamento estimados pelo filtro estão desviados,
44
SISTEMA DE ATITUDE
16
Fi estimado
Fi
−0.01
14
−0.02
12
−0.03
10
Rolamento[º]
Bias (rad/s)
−0.04
−0.05
8
6
−0.06
4
−0.07
2
−0.08
BFi
BTeta
BPsi
0
−0.09
0
2
4
6
8
10
Tempo[s]
12
14
16
18
0
20
2
4
6
8
10
Tempo[s]
12
14
16
18
20
14
16
18
20
Ang. Rumo vs Tempo
25
5
Psi estimado
Psi
Teta estimado
Teta
4.5
20
4
3.5
15
Rumo[º]
Picada[º]
3
2.5
2
10
1.5
1
5
0.5
0
0
2
4
6
8
10
Tempo[s]
12
14
16
18
20
0
2
4
6
45
8
10
Tempo[s]
12
SISTEMA DE ATITUDE
ainda que ligeiramente, do valor correcto. Este facto deve-se à introdução de uma força
adicional (para além da força gravı́tica) que é sentida pelos acelerómetros. Esta força está
no plano xy e tem a direcção de y no referencial do corpo. Esta força, ”leva o filtro a
assumir”que existe um ângulo de rolamento não nulo por forma a que esta força não seja
mais que uma projecção da força da gravidade.
16
Fi estimado
Fi
−0.01
14
−0.02
12
−0.03
10
Rolamento[º]
Bias (rad/s)
−0.04
−0.05
8
6
−0.06
4
−0.07
−0.08
2
BFi
BTeta
BPsi
−0.09
0
2
4
6
8
10
Tempo[s]
12
14
16
18
0
0
20
2
4
6
8
10
Tempo[s]
12
14
16
18
20
14
16
18
20
Ang. Rumo vs Tempo
5
Psi estimado
Psi
Teta estimado
Teta
4.5
20
4
3.5
15
Rumo[º]
Picada[º]
3
2.5
2
10
1.5
1
5
0.5
0
0
0
2
4
6
8
10
Tempo[s]
12
14
16
18
20
0
2
4
6
8
10
Tempo[s]
12
Na segunda situação, a velocidade segundo x é anulada. Assim, a força centrı́fuga deixa
de existir e passa-se para uma situação de um veı́culo estacionário que altera a sua rota a
ritmo constante. Como se pode verificar nas figuras (5.10) e (5.11), a estimativa do ângulo de
rolamento passa a estar correcta assim como a estimativa do ângulo de rumo. De referir na
situação inicial, o ângulo de rumo também estava desviado do correcto já que as observações
do ângulo ψ dependem directamente do ângulo φ estimado. Assim, o erro na estimativa do
ângulo de rolamento propagava-se para a estimação do ângulo de rumo.
46
SISTEMA DE ATITUDE
5.6
Experiência 5 - Aceleração horizontal
Na última simulação, procura-se mostrar o efeito que as acelerações lineares têm no filtro.
Mostra-se também o efeito que a aplicação da função Peso provoca nas estimativas de atitude
devolvidas pelo filtro.
Inicialmente assume-se uma velocidade em x de 10m/s com atitude nula (ângulos de
Euler todos iguais a zero). Aplica-se depois uma aceleração constante igual a 5m/s2 no
intervalo [10s, 35s]. Testam-se duas situações: na primeira os ganhos são fixos e na segunda
aplicou-se a função Peso com k = 0.5 e n = 20.
16
Fi estimado
Fi
BFi
BTeta
BPsi
0.3
14
0.1
10
Rolamento[º]
12
Bias (rad/s)
0.2
0
−0.1
8
6
−0.2
4
−0.3
2
−0.4
0
0
5
10
15
20
25
Tempo[s]
30
35
40
45
0
50
5
10
15
20
25
Tempo[s]
30
35
40
45
50
Ang. Rumo vs Tempo
5
Teta estimado
Teta
Psi estimado
Psi
30
4
25
3
Rumo[º]
Picada[º]
20
15
2
1
10
0
5
−1
0
0
5
10
15
20
25
Tempo[s]
30
35
40
45
50
0
5
10
15
20
25
Tempo[s]
30
35
40
45
50
Como se pode observar nas figuras (5.12) e (5.13) referentes ao primeiro teste, até aos
15s o filtro estima a atitude e as polarizações correctas. Durante a aplicação da aceleração
longitudinal, o filtro estima um ângulo de picada de 27o sendo o valor real de 0o . Mais
47
SISTEMA DE ATITUDE
uma vez, o filtro ”assume”que a presença de uma aceleração em x se deve a uma projecção
da força gravı́tica segundo este eixo devido a um ângulo de picada de 27o . Terminada a
aceleração, o filtro volta a fornecer estimativas correctas.
16
Fi estimado
Fi
0.02
14
0
12
10
Rolamento[º]
Bias (rad/s)
−0.02
−0.04
8
6
4
−0.06
2
−0.08
0
BFi
BTeta
BPsi
0
5
10
15
20
25
Tempo[s]
30
35
40
45
−2
0
50
5
10
15
20
25
Tempo[s]
30
35
40
45
50
Ang. Rumo vs Tempo
5
5
Teta estimado
Teta
Psi estimado
Psi
4
4
3
2
Rumo[º]
Picada[º]
3
2
1
1
0
−1
0
−2
0
5
10
15
20
25
Tempo[s]
30
35
40
45
50
0
5
10
15
20
25
Tempo[s]
30
35
40
45
50
No segundo teste, acontece que mal se inicia a aceleração segundo x, o filtro passa a
funcionar em malha aberta ”congelando”as polarizações estimadas até aı́. Desta forma,
o filtro passa a depender apenas das indicações dos giroscópios que não estão sujeitos a
acelerações lineares. Como se pode ver, apesar de as estimativas do filtro começarem a
divergir, os desvios verificados são muito inferiores aos verificados no primeiro caso. Estes
desvios serão tanto menores quanto melhores forem as estimativas das polarizações antes de
o filtro iniciar o funcionamento em malha aberta. Estes resultados podem ser observados
nas figuras (5.14) e (5.15).
A estratégia implementada permite que durante a aplicação de acelerações extra gravı́ticas
48
SISTEMA DE ATITUDE
ao veı́culo, os erros obtidos nas estimativas sejam apenas função das polarizações presentes
nas saı́das dos giroscópios. O sucesso desta estratégia é tanto maior quanto menores forem
estas polarizações. Existe ainda a vantagem de as polarizações poderem ter sido anuladas
pelo filtro antes de este começar a funcionar em malha aberta. A não aplicação desta
estratégia leva a que o filtro tenha observações incorrectas dos ângulos de Euler sendo as
estimativas obtidas bastante piores.
49
Capı́tulo 6
Simulação com Dados Reais
Neste capı́tulo analisa-se o desempenho do filtro de atitude estudado, recorrendo a dados
obtidos em duas simulações efectuadas na Alameda do Instituto Superior Técnico em Fevereiro de 2002. Estes testes fizeram parte do trabalho de fim de curso de dois colegas de Enga
Electrotécnica do ramo de Controlo.
Foram efectuadas duas experiências com uma plataforma de atitude recorrendo a três
trı́ades ortogonais de giroscópios, acelerómetros e magnetómetros. Adicionalmente, dispunhase do hardware necessário à armazenagem dos dados provenientes dos sensores. Assim, foi
possı́vel proceder ao teste do filtro fornecendo como entradas estes dados. Em ambas as
experiências a informação foi adquirida a um ritmo de 50Hz.
6.1
Sensores utilizados e configuração do filtro
Com os dados disponı́veis na primeira experiência, que será analisada de seguida, pôde-se calcular a variância que cada sensor utilizado apresenta. Na tabela (6.1) estão as representadas
as variâncias obtidas.
Tendo em conta os valores obtidos, fez-se a configuração do filtro de atitude. Assim,
introduziu-se o vector [5 × 10−3 5 × 10−3 5 × 10−6 ] para a parametrização das variâncias dos
ruı́dos presentes nas observações. Os dois primeiros dizem respeito aos ângulos de rolamento
Parâmetro
Acelerómetro x (m/s2 )
Acelerómetro y (m/s2 )
Acelerómetro z (m/s2 )
Giroscópio x (rad/s)
Giroscópio y (rad/s)
Giroscópio z (rad/s)
Magnetómetro x (Gauss)
Magnetómetro y (Gauss)
Magnetómetro z (Gauss)
Valor
5 × 10−3
6 × 10−3
4 × 10−3
6 × 10−5
6 × 10−5
6 × 10−5
3 × 10−6
5 × 10−6
4 × 10−7
Tabela 6.1: Variâncias dos sensores utilizados
50
SISTEMA DE ATITUDE
6. SIMULAÇÃO COM DADOS REAIS
e de picada que são obtidos a partir dos acelerómetros. O último, corresponde ao ruı́do
associado às observações do rumo que depende essencialmente dos magnetómetros.
Para o ruı́do associado aos seis estados do filtro, optou-se pelo valor 6 × 10−5 que corresponde à variância de ruı́do na saı́da dos giroscópios.
Com estas escolhas obteve-se a seguinte matriz de ganhos:





K=





0.0289
0
0

0
0.0289
0


0
0
0.1594 


0.0153
0
0


0
0.0153
0

0
0
0.1420
(6.1)
Por fim, a frequência de funcionamento do filtro foi a mesma usada na extracção dos dados,
ou seja, 50Hz.
6.2
Experiência 1 - Plataforma de atitude estacionária
Na primeira experiência efectuada, a plataforma de atitude foi mantida imóvel no solo com
φ = θ = 0o (garantidos por um nı́vel de bolha) durante 220s. Nas figuras (6.1) e (6.2) estão
representadas as saı́das dos nove sensores utilizados.
Acelerometros vs Tempo
Giroscopios vs Tempo
12
Acelx
Acely
Acelz
Gyrox
Gyroy
Gyroz
0.04
0.03
10
0.02
Velocidade Angular[rad/s]
Aceleracao[m/s2]
8
6
4
0.01
0
−0.01
−0.02
2
−0.03
0
0
5
10
15
20
25
Tempo[s]
30
35
40
45
50
0
5
10
15
20
25
Tempo[s]
30
35
40
45
50
Figura 6.1: Saı́das dos acelerómetros e giroscópios
Quanto aos acelerómetros, é possı́vel verificar que tanto o sensor respeitante ao eixo x
como ao eixo y, apresentam aceleração nula sendo a aceleração em z da ordem dos 9.81m/s2 .
Isto era o esperado já que para ângulos de picada e rolamento nulos a aceleração gravı́tica
está toda aplicada no eixo z. Já os giroscópios apresentam todos valores médios próximos
dos 0rad/s, resultando isto de a plataforma se ter mantido estática durante o teste.
Observando as saı́das dos magnetómetros, pode-se ver que a componente de campo mais
forte é segundo a vertical local. Pode-se mesmo concluir que a inclinação magnética é de
51
SISTEMA DE ATITUDE
Magnetometros vs Tempo
Magx
Magy
Magz
2.5
Intensidade Campo[Volt]
2
1.5
1
0.5
0
−0.5
0
5
10
15
20
25
Tempo[s]
30
35
40
45
50
Figura 6.2: Saı́das dos magnetómetros
cerca de λ = 49o . A conversão de unidade V olt para Gauss é feita dividindo a leitura pelo
ganho 5V olt/Gauss. Pode-se também concluir que a intensidade de campo total é um pouco
inferior a 0.6Gauss.
Na figura (6.3) estão os resultados obtidos após a filtragem. Quanto às polarizações,
pode-se ver que estão todas próximas dos 0rad/s. De notar que antes de serem efectuadas
as experiências, se procedia sempre à calibração da instrumentação utilizada. A atitude
obtida corresponde à esperada, ou seja, os ângulos de ³rolamento
e picada estão próximos de
´
Hy
o
o
0 e o ângulo de rumo dos 10.5 resultando de arctan − Hx .
Atitude vs Tempo
0.16
Fi estimado
Teta estimado
Psi estimado
BFi
BTeta
BPsi
0.14
12
0.12
10
0.1
Atitude[º]
Bias (rad/s)
8
0.08
6
0.06
4
0.04
2
0.02
0
0
0
5
10
15
20
25
Tempo[s]
30
35
40
45
50
0
5
10
15
20
25
Tempo[s]
30
35
40
45
50
Figura 6.3: Polarizações e atitude
Resta acrescentar que, dadas as caracterı́sticas desta experiência, apesar de estarem disponı́veis 220s de simulação se optou por mostrar apenas os primeiros 50s uma vez no restante
tempo todas as grandezas se mantêm inalteradas.
52
SISTEMA DE ATITUDE
6.3
Experiência 2 - Rectângulo
Na segunda experiência efectuada, com duração de 172s, testou-se o sistema com a plataforma em movimento. Até cerca dos 69s manteve-se a plataforma estática à semelhança da
experiência anterior. De seguida, no intervalo [69s , 83s] andou-se em linha recta procurandose manter a atitude da plataforma. Nesta altura procedeu-se a uma alteração de rumo de
cerca de 90o seguindo-se novamente em linha recta. Aos 114s fez-se nova rotação em torno
de z de 90o seguindo-se em linha recta até aos 128s quando se alterou novamente o rumo em
mais 90o . Finalmente, aos 156s após nova mudança de rumo, retomou-se, aproximadamente
a atitude inicial.
O movimento descrito, visto de cima, consiste num rectângulo em que o ponto de partida
e chegada do movimento coincidem. De notar que esta experiência foi feita recorrendo ao
transporte a pé da plataforma de atitude.
Nas figuras (6.4) e (6.5) podem-se ver os dados registados nas saı́das dos sensores. Quanto
aos acelerómetros, pode-se ver que os resultados são idênticos aos obtidos na experiência
anterior notando-se apenas que quando se inicia o movimento da plataforma os nı́veis de
ruı́do (amplitude das oscilações em torno do valor médio) aumentam muito. Isto deve-se ao
modo como foi feito o deslocamento da plataforma. De resto, este aumento verifica-se em
todas as figuras respeitantes a esta experiência.
Acelerometros vs Tempo
Giroscopios vs Tempo
Acelx
Acely
Acelz
20
Gyrox
Gyroy
Gyroz
1
Velocidade Angular[rad/s]
Aceleracao[m/s2]
15
10
5
0.5
0
−0.5
0
0
20
40
60
80
100
Tempo[s]
120
140
160
0
20
40
60
80
100
Tempo[s]
120
140
160
Figura 6.4: Saı́das dos acelerómetros e giroscópios
Quanto aos giroscópios, pode-se ver que apresentam valores sempre próximos dos 0rad/s
(em média) exceptuando-se o giroscópio associado ao eixo z. Aqui, notam-se algumas sobreelevações quando se verificam as alterações de rumo, o que seria de esperar.
Analisando agora os magnetómetros, pode-se observar que o campo no eixo z se mantém
aproximadamente constante, devendo-se isto ao facto de se ter procurado manter sempre
os ângulos de picada e rolamento nulos. Nos outros dois eixos, verificaram-se quatro combinações de valores para cada rumo obtido (10o , 94o , 176o , 272o ).
Na figura (6.6) vê-se a evolução dos ângulos de rolamento e picada. Pode-se observar
que após o inicio do movimento da plataforma, ambos os ângulos se mantêm abaixo dos 0o ,
53
SISTEMA DE ATITUDE
Magnetometros vs Tempo
Magx
Magy
Magz
3
Intensidade Campo[Volt]
2
1
0
−1
−2
0
20
40
60
80
100
Tempo[s]
120
140
160
Figura 6.5: Saı́das dos magnetómetros
10
Fi estimado
Teta estimado
8
5
6
4
0
Picada[º]
Rolamento[º]
2
0
−2
−5
−4
−6
−10
−8
−10
0
20
40
60
80
100
Tempo[s]
120
140
160
0
20
40
60
Figura 6.6: Ângulos de rolamento e picada
54
80
100
Tempo[s]
120
140
160
SISTEMA DE ATITUDE
embora com valores pequenos. Este facto deve-se novamente ao método usado no transporte
da plataforma. A intenção seria fazer estes ângulos próximos dos 0o .
Atitude vs Tempo
BFi
BTeta
BPsi
0.15
Fi estimado
Teta estimado
Psi estimado
450
400
350
0.1
0.05
Atitude[º]
Bias (rad/s)
300
250
200
0
150
100
−0.05
50
−0.1
0
0
20
40
60
80
100
Tempo[s]
120
140
0
160
20
40
60
80
100
Tempo[s]
120
140
160
Figura 6.7: Polarizações e atitude
Finalmente, na figura (6.7) estão representadas as polarizações assim como os ângulos
de Euler obtido após a filtragem. Mais uma vez se pode observar que as polarizações
convergem imediatamente para 0rad/s havendo perturbações significativas apenas quando
se inicia o movimento da plataforma. Quanto à atitude obtida, pode-se ver, para além dos
ângulos de rolamento e picada analisados atrás, que o ângulo de rumo segue o perfil esperado.
Inicialmente, este está perto dos 10o , à semelhança da experiência anterior, percorrendo
em seguida os patamares correspondentes aproximadamente às direcções 94o , 176o , 272o e
novamente o rumo inicial.
55
Capı́tulo 7
Conclusões
Neste capı́tulo apresentam-se as conclusões e considerações finais respeitantes ao filtro desenvolvido, à sua implementação e ao seu desempenho.
Destacam-se primeiro os aspectos teóricos que estão na base do trabalho efectuado. Assim,
inicialmente verificou-se o efeito positivo que a complementaridade de diversos sensores traz
nos resultados obtidos quando comparados com os resultados isolados de cada grupo de
sensores. Realce-se aqui o conceito de complementaridade em frequência dos vários sensores
em que cada um deles contribui para o resultado final com a zona de espectro em que
apresenta melhores caracterı́sticas. Paralelamente, e falando ainda de filtragem, verificou-se
a extrema utilidade das técnicas de filtragem de Kalman e de discretização na estruturação
do filtro de atitude, com seis estados internos, permitindo uma fácil e rápida implementação
deste numa plataforma digital.
Analisando agora os resultados obtidos nos capı́tulos 5 e 6, pôde-se verificar que o filtro teve um bom desempenho quer a situação em teste implicasse uma atitude constante
ou variável com o tempo. Pôde-se ver que um dos factores que mais influencia os resultados finais é o nı́vel de ruı́do presente na saı́da dos sensores, isto é, a qualidade destes tem
um peso fundamental na precisão obtida na estimação da atitude. Confirmou-se também,
que a aplicação de acelerações centrı́fugas ou lineares aos sensores, nomeadamente aos acelerómetros, levava a desvios muito significativos da atitude estimada para a real, funcionando
estas acelerações extras como se de uma polarização se tratasse nas observações dos ângulos
de rolamento e picada.
Quando se aplicavam acelerações extra-gravı́ticas, verificou-se também que a implementação
da estratégia de se fazer a filtragem em malha aberta, usufruindo-se da vantagem do filtro
estimar as polarizações dos giroscópios, levava a resultados bastante satisfatórios. Apesar de
se verificar um afastamento gradual da atitude real, este é bastante lento (tanto mais lento
quanto melhores forem as estimativas das polarizações dos giroscópios). Esta estratégia serve
essencialmente para cobrir curtos espaços de tempo em que por algum motivo as observações
baseadas nos acelerómetros estão indisponı́veis ou deturpadas. Lembra-se, no entanto, que
esta analise é feita tendo em conta que apenas se dispõe dos sensores utilizados neste trabalho.
Assim, caso se existissem outros sistemas auxiliares, poder-se-ia solucionar este problema de
maneiras bastante mais eficientes. Por exemplo, com acesso ao vector velocidade do veı́culo,
56
SISTEMA DE ATITUDE
7. CONCLUSÕES
por meio de um radar Doppler, poder-se-ia anular o efeito das acelerações centrı́fugas na
estimação da atitude.
Como conclusão final, pode-se afirmar que o sistema de atitude desenvolvido apresentou
resultados positivos permitindo o acompanhamento dos três ângulos de Euler de um veı́culo
para diferentes trajectórias seguidas por este.
57
Apêndice A
Simulação em MATLAB
Neste apêndice mostram-se os principais blocos constituintes da simulação do filtro de atitude
desenvolvida em MATLAB, recorrendo ao Simulink R12, assim como os parâmetros que os
caracterizam.
A.1
Simulação
Na figura (A.1) podem-se ver os blocos principais da simulação e o modo como estes se
relacionam. Tem-se como entrada um bloco onde se podem introduzir as condições iniciais
da simulação, assim como as caracterı́sticas de diversas grandezas fı́sicas ao longo desta.
Segue-se um bloco cinemático que tem na sua saı́da todas as grandezas que afectam os
sensores. Logo a seguir vem o bloco relativo aos sensores. Este produz na saı́da os sinais que
os sensores apresentam.
Figura A.1: Simulação em Simulink R12
Por fim, tem-se a filtragem que tem como entradas os sinais de saı́da dos sensores e como
58
SISTEMA DE ATITUDE
A. SIMULAÇÃO EM MATLAB
saı́das os sinais que se pretende estimar. Existe ainda um bloco com as funções de permitir
a visualização das estimativas do filtro e de guardar os dados obtidos numa variável de
MATLAB.
A.2
Entradas
Na figura (A.2) está representada a tabela com os parâmetros que caracterizam o bloco das
entradas.
Figura A.2: Bloco das entradas
Neste bloco, podem-se definir os movimentos translacional e rotacional de um veı́culo
ao longo da simulação. Para tal, fornece-se a atitude e velocidade iniciais assim como as
velocidades angulares e acelerações lineares a que o veı́culo está sujeito. Estes dois últimos
parâmetros podem ser alterados no decorrer da simulação. No entanto, usando este bloco,
apenas se conseguem introduzir constantes. Quando se pretende simular acelerações ou
velocidades angulares não constantes, há que recorrer a outros blocos elementares do Simulink
para se gerar os sinais de entrada pretendidos.
A.3
Modelo cinemático
Na figura (A.3) está representado o bloco cinemático. Este possui na sua saı́da todas as
grandezas às quais os sensores reagem: ângulos de Euler, velocidades angulares, velocidades
lineares e acelerações lineares. A atitude é obtida integrando as equações de Euler e a
velocidade linear por integração da aceleração.
59
SISTEMA DE ATITUDE
Figura A.3: Modelo cinemático
A.4
Sensores
Na figura (A.4) pode-se ver a constituição do bloco dos sensores. Este é constituı́do por três
subsistemas: giroscópios, acelerómetros e magnetómetros.
Figura A.4: Bloco dos sensores
Nas figuras (A.5), (A.6) e (A.7) podem-se ver os parâmetros que definem cada um destes
subsistemas. Para cada tipo de sensor, existem três termos relativos à variância de ruı́do
branco presente na saı́da dos sensores. Da mesma forma, existe também um termo de ruı́do
DC para cada sensor respeitante ao desvio da temperatura ideal de funcionamento. Nos
giroscópios definem-se ainda os desvios para cada sensor respeitantes às polarizações.
60
SISTEMA DE ATITUDE
Figura A.5: Parâmetros dos giroscópios
Figura A.6: Parâmetros dos acelerómetros
61
SISTEMA DE ATITUDE
Por fim, para os magnetómetros define-se a intensidade do vector campo magnético assim
como a inclinação deste.
Figura A.7: Parâmetros dos magnetómetros
A.5
Filtragem
O bloco de filtragem tem como entradas os sinais provenientes dos sensores e como saı́das
os estados do filtro de atitude: os três ângulos de Euler e as três polarizações associadas aos
giroscópios.
Na figura (A.8) podem-se ver os parâmetros que definem este bloco. Estes, podem-se
dividir em três grupos principais. No primeiro definem-se a frequência de filtragem e as
estimativas iniciais para os seis estados. Os três parâmetros seguintes dizem respeito às
variâncias associadas às observações e aos seis estados do filtro. Por fim, define-se o tipo
de ganhos que se pretende. Caso sejam P esados os dois últimos parâmetros caracterizam a
função P eso.
62
SISTEMA DE ATITUDE
Figura A.8: Parâmetros de filtragem
63
Apêndice B
Código Desenvolvido
Neste apêndice mostra-se o código desenvolvido em MATLAB que serve de suporte à simulação em Simulink. Seguem-se assim sete ficheiros .m que são chamados durante a simulação através dos blocos do tipo s − f untions.
B.1
rui gyro s
64
SISTEMA DE ATITUDE
B. CÓDIGO DESENVOLVIDO
%
B.2
rui acel s
65
SISTEMA DE ATITUDE
%
B.3
rui magnet s
66
SISTEMA DE ATITUDE
%
B.4
rui acel contas s
67
SISTEMA DE ATITUDE
%
B.5
rui magnet contas s
68
SISTEMA DE ATITUDE
%
B.6
rui filtro kalman s
69
SISTEMA DE ATITUDE
%
70
SISTEMA DE ATITUDE
%
B.7
rui matrizQ s
71
SISTEMA DE ATITUDE
%
72
Bibliografia
[1] Myron Kayton e Walter R. Fried, Avionics Navigation Systems, John Wiley & Sons,
1997
[2] Shmuel Merhav, Aerospace Sensor Systems and Applications, Springer-Verlag New York,
1996
[3] R.G. Brown e Patrick Y.C. Hwang, Introduction to Random Signals and Applied Kalman
Fieltering, John Wiley & Sons, 1997
[4] Fernando Nunes, Folhas da Cadeira de Sistemas de Controlo de Tráfego Aéreo, 2000
[5] G.F. Franklin, J.D. Powell e A.E. Naeini, Feedback Control of Dynamics Systems,
Addison-Wesley Publishing Company, 1994
[6] Donald McLean, Automatic Flight Control Systems, Prentice Hall, 1990
[7] Vincent P. Coletter, College Physics, Mosby, 1995
[8] Walter T. Higgins, Jr., A Comparision of Complementary and Kalman Filtering, 1975
[9] Karl-Petter Lindegaard, Thor I. Fossen, On Global Model Based Observer Designs For
Surface Vessels, 2001
[10] Demoz Gebre-Egziabher, Roger C. Hayward, J. David Powel, A Low-Cost GPS/Inertial
Attitude Heading Reference System (AHRS) for General Aviation Applications
[11] Demoz Gebre-Egziabher, Roger C. Hayward, Matt Schwall, J. David Powel, Inertially
Aided GPS Based Attitude Heading Reference System (AHRS) for General Aviation
Aircraft
[12] Michèlle Neuilly, Modeling and Estimation Measurement Errors, Intercept
[13] J.R. Azinheira, Folhas da Cadeira de Controlo de Voo, 1998
[14] Folhas da Cadeira de Instrumentação e Medidas, 2001
73

Ficheiro PDF

Transcrição

Documentos relacionados

Autorização de Troca de Óleo

Aermec TA

BOLETIM INFORMATIVO Errata

FT COMAC ASPIRADOR PROFISSIONAL CA 80 100170

FICHA TÉCNICA

melhora de performance em 5 minutos!

dalia black

AVS-BRF-1000 - Aviso - (11) 3017-2760

ESCOLA BASICA JÚLIO DINIS Correção da cor dos olhos

Filtros de Reposição Trumpf Para Coletores de Pó Handte