ads:
MINISTÉRIO DA DEFESA
EXÉRCITO BRASILEIRO
DEPARTAMENTO
DE CIÊNCIA E TECNOLOGIA
INSTITUTO MILITAR DE ENGENHARIA
CURSO DE MESTRADO EM ENGENHARIA MECÂNICA
Maj
QEM
LUCIANO VASCONCELOS ROCHA
ALGORITMO DE NIVELAMENTO E ALINHAMENTO DE UM
SISTEMA DE NAVEGAÇÃO INERCIAL DO TIPO SOLIDÁRIO
( STRAPDOWN )
Rio de Janeiro
2006
ads:
Livros Grátis
http://www.livrosgratis.com.br
Milhares de livros grátis para download.
2
INSTITUTO MILITAR DE ENGENHARIA
Maj LUCIANO VASCONCELOS ROCHA
ALGORITMO DE NIVELAMENTO E ALINHAMENTO DE UM SISTEMA
DE NAVEGAÇÃO INERCIAL DO TIPO SOLIDÁRIO ( STRAPDOWN )
Dissertação de Mestrado apresentada ao Curso de
Mestrado em Engenharia Mecânica do Instituto Militar
de Engenharia, como requisito parcial para a obtenção
do título de Mestre em Ciências em Engenharia
Mecânica.
Orientador: Prof. Clódio Alberto Pastro Sarzeto
D.C.
Co
-
orientador: Prof. Carlos Renato Caputo Durão
M.C.
Rio de Janeiro
2006
ads:
3
c200
6
INSTITUTO MILITAR DE ENGENHARIA
Pr
aça General Tibúrcio, 80
Praia Vermelha
Rio de Janeiro
-
RJ CEP: 22290
-270
Este exemplar é de propriedade do Instituto Militar de Engenharia, que poderá incluí-lo em base
de dados, armazenar em computador, microfilmar ou adotar qualquer fo
rma de arquivamento.
É permitida a menção, reprodução parcial ou integral e a transmissão entre bibliotecas deste
trabalho, sem modificação de seu texto, em qualquer meio que esteja ou venha a ser fixado, para
pesquisa acadêmica, comentários e citações, d
esde que sem finalidade comercial e que seja feita a
referência bibliográfica completa.
Os conceitos expressos neste trabalho são de responsabilidade do autor e do orientador.
R672
Rocha, Luciano
Vasconcelos
Algoritmo de Al
inhamento e Navegação de um Sistema de
Navegação Inercial do tipo solidário ( strapdown )
/
Luciano
Vasconcelos Rocha. -
Rio de Janeiro : Instituto Militar de
Engenharia, 200
6.
165 p
. : il., graf., tab.
Diss
ertação (mestrado) - Instituto Militar de
Engenh
aria
Rio de Janeiro, 200
6.
1. Sistemas de Navegação Inercial. 2. Algoritmo de
Nivelamento e Alinhamento. I. Título. II. Instituto
Militar de Engenharia
CDD 629
.
13251
4
INSTITUTO MILITAR DE ENGENHARIA
Maj LUCIANO VASCONCELOS ROCHA
ALGORIT
MO DE ALINHAMENTO E NIVELAMENTO DE UM SISTEMA
DE NAVEGAÇÃO INERCIAL DO TIPO SOLIDÁRIO ( STRAPDOWN )
Dissertação de Mestrado apresentada no Curso de Mestrado em Engenharia Mecânica do
Instituto Militar de Engenharia, como requisito parcial para obtenção do título de Mestre em
Ciências em Engenharia Mecânica.
Orientador: Prof. Clódio Alberto Pastro Sarzeto
D.C.
Co
-
orientador: Prof. C
arlos Renato Caputo Durão
M.C.
Aprovada em 05
de
janeiro
de 2006
pela seguinte Banca Examinadora:
____________________________________________________
Prof. Clódio Alberto Pastro Sarzeto
D.C. do IME
-
Presidente
_________________
_____________
___________________________________
Prof. Geraldo Magela Pinheiro
Dr. ENSAE do IME
______
____________
__________
____________________________________
Maj
QEM Arma
ndo Morado Ferreira
Ph. D. do IME
__________________
___________
__________________________________
Pedro Cunha
Campos Roquette
D.Sc
. do I
PqM
__________________________
___________
_______________________
___
Carlos Renato Caputo Durão
M.C
. do I
PqM
Rio de Janeiro
2006
5
AGRADECIMENTOS
A Deus em primeiro lugar, que, por sua graça e misericórdia, guiou os meus passos na
escolha do tema e das cadeiras a cursar, na busca das fontes de consulta, além de colocar no meu
caminho diversas pessoas que tiveram papel decisivo na elaboração deste trabalho.
Aos meus pais, pela dedicação, apóio e esforços envidados, desde a minha tenra idade, que
me deram o alicerce necessário para todos os empreendimentos de minha vida adulta, inclusive
este.
A meu orientador, Dr. Clodio Alberto Pastro Sarzeto por sua valiosa orientação,
apo
io
e
todos os conhecimentos transmitidos.
Ao meu co-orientador, Prof. Carlos Renato Caputo Durão, que foi uma destas pessoas
colocadas por Deus no meu caminho num momento de muitas dificuldades na consecução deste
trabalho
e, sem cuja preciosa colaboração, a chegada a um desfecho com êxito dificilmente seria
possível.
6
Levantarei os meus olhos para os montes, de onde vem o
meu socorro.
O meu socorro vem do SENHOR que fez o céu e a
Terra
.
Não deixará vacilar o teu pé; aquele que te guarda não
tosquenejará.
Eis que não tosquenejará nem dormirá o guarda de Israel.
O SENHOR é quem te guarda; o SENHOR é a t
ua sombra à
tua direita.
O sol não te molestará de dia nem a lua de noite.
O SENHOR te guardará de todo o mal; guardará a tua alma.
O SENHOR guardará a tua entrada e a tua saída, desde
agora e para sempre .
(Salmo 121
Bíblia Sagrada)
7
SUMÁRIO
LISTA DE
ILUSTRAÇÕES.........................................................................................................
11
LISTA DE ABRAVIATURAS E SÍMBOLOS
............................................................................
14
1.
INTRODUÇÃO................................................................................................
19
1.1. Conceitos Gerais................................................................................................ 19
1.2.
Histórico.............................................................................................................
20
1.3.
Descrição de um Sistema de Navegação Inercial (SNI)....................................
21
1.4. Objetivo do Trabalho......................................................................................... 21
1.5.
Relevância do Trabalho.....................................................................................
22
1.6.
Organização do Trabalho...................................................................................
23
1.7.
Posiciona
mento do
Trabalho.............................................................................. 24
1.8.
Revisão Bibliográfica
......................
................................................................... 24
2.
CONCEITOS DE NAVEGAÇÃO INERCIAL....................................
......... 26
2.1. Transformação de Coordenadas......................................................................... 26
2.1.1.
Co
-
senos Diretores............................................................................................
. 27
2.1.2.
Parâme
tros de Euler ou Quaternions..................................................................
30
2.1.3. Ângulos de Euler................................................................................................ 31
2.2.
Variáveis
de Navegação................
..................................................................... 36
2.2.1.
Variáveis
de Posição..........................................................................................
36
2.2.2.
Variáveis
de Velocidade.............
....................................................................... 37
2.2.3.
Variáveis
de Atitude...........................................................................................
38
2.3.
Referenciais.......................................................................................
................. 38
2.3.1.
Referencial inercial com origem no centro da
Terra.............
..
...........................
39
2.3.2.
Referencial da
Terra
...........................................................................................
39
2.3.3.
Referencial do corpo..........................................................................................
40
2.3.4.
Referencial Local ou de Navegação
...................................................................
41
2.3.4.1.
Referencial
Estável no Espaço ( Space Stable )............................................... 41
2.3.4.2.
Referencial
Localmente Nivelado ( Local Level )............
...............................
41
8
2.3.4.2.1.
Configuração Geográfica...................................................................................
42
2.3.4.2.2. Configuração Free Azimuth ........................................................................... 45
2.3.4.2.3. Configuração Wander Azimuth ...................................................................... 46
2.4.
Navegação Inercial.............................................................................................
46
2.4.1. Fundamentos...................................................................................................... 46
2.4.1.1.
Princípio da Relatividade de Einstein
-
Galileu...................................................
46
2.4.2.
Sistemas de Navegação Inercial........................................................................
. 49
2.5.
Mecanizações Típicas de um Sistema de Navegação Inercial.....
...................... 49
2.6.
Algoritmos
de Alinhamento, de Navega
ção e de Atitude..
................................
51
2.6.1.
Algoritimo de Alinhamento
...............................................................................
52
2.6.2.
Algoritimo de
Navegação..................................................................................
53
2.6.3. Alg
oritimo de Atitude........................................................................................
53
3.
ALGORITMO DE NAVEGAÇÃO E ATITUDE.........................................
54
3.1.
Navegação..........................................................................................................
54
3.1.1.
Princípio de Funcionamento de um Acelerômetro............................................
54
3.1.2.
Obtenção da Força Específica......
......................................................................
55
3.1.3.
Obte
nção da posição do veículo em relação ao referencial da
Terra.........
........
56
3.1.4.
Equações de Navegação.....................................................................................
60
3.2.
Atitud
e...............
................................................................................................. 60
3.2.1.
Considerações Básicas.......................................................................................
60
3.2.2. Equação de Atualização da Matriz de Rotação do Referencial do Corpo para o
Local (Equ
ação de Atitude)
...............................................................................
62
3.2.3.
Modelo do Formato da
Terra
.............................................................................
65
3.2.4.
Modelo da Atração Gravitacional da
Terra
........................................................
67
3.3.
Algoritmo de Navegação................................................................
................... 67
3.4.
Validação do Algoritmo de Navegação.............................................................
70
3.4.1.
Trajetória Proposta.............................................................................................
70
3.4.1.1.
Simulação das medidas dos acelerômetros........................................................
72
3.4.1.2.
Simulação das medidas dos giroscópios.........
...................................................
76
3.4.1.3.
Resultados da simulação..........................
.......................................................... 78
9
4.
TRATAMENTO DOS ERROS
.
......................................................................
87
4.1. Tipos de Erros.................................................................................................... 87
4.2.
Model
agem
dos Erros dos Sensores...
................................................................ 89
4.2.1.
Model
agem
dos Erros do
Giroscópio.................................................................
90
4.2.2.
Model
agem
dos Erros do Acelerômetro............................................................
87
4.3.
Equações de Propagação de Erros.....................................................................
88
5.
ALGORITMO DE
ALINHAMENTO.............................
.............................. 94
5.1.
Definição............................................................................................................
94
5.2.
Alinhamento.......................................................................................................
94
5.3.
Nivelamento.......................................................................................................
94
5.4.
Alinhamento Grosseiro.................
..................................................................... 94
5.5.
Alinhamento Fino.............................................................................................
102
6.
SIMULAÇÕES................................................................................................
107
6.1.
Objetivos das Simulações.................................................................................
107
6.2.
Parâmetros Usados nas Simulações........................................................
......... 107
6.3.
Des
crição..........................................................................................................
108
7. ANÁLISE DOS RESULTADOS................................................................... 110
8. C
ONCLUSÃO....................................................................................
............. 122
9.
REFERÊNCIAS BIBLIOGRÁFICAS
..........................................................
123
10.
APÊNDICE
S
...................................................................................................
125
10.1.
APÊNDICE 1:
E
STIMAÇÃO ÓTIMA.........
................................................... 126
10.1.1.
MÉTODO DOS MÍNIMOS Q
UADRADOS ...
126
10.1.1.1.
O CASO MONOVARIÁVEL ......
127
10.1.1.2.
CASO MULTIVARIÁVEL
...... 133
10.1.1.3.
INTERP
RETAÇÃO ESTOCÁSTICA
DOS MÍNIMOS QUADRADO
S
.......
135
10
10.1.1.4.
MÍNIMOS QUADRADOS CO
M PONDERAÇÃO
.
.......................................
136
10.1.1.5.
EXEMPLO DE APLICAÇÃO
.........................................................................
137
10.1.2.
O FILTRO DE KALMAN..........
.....................................................................
139
10.1.2.1.
POSIÇÃO DO PROBLEMA
............................................................................ 139
10.1.2.2.
O MODELO MATEMÁTICO
.........................................................................
140
10.1.2.3.
EXEMPLO DE PROBLEMA
DE FILTRAGEM
............................................
141
10.1.2.4.
A FORMULAÇÃO DO FILT
RO DE KALMAN
...........................................
. 143
10.1.2.4.1. DETERMINAÇÃO INTUITI
VA DO FILTRO
...............................................
144
10.1.2.4.2.
O FILTRO DE KALMAN D
ISCRETO
...........................................................
146
10.1.2.4.3.
O FILTRO DE KALMAN CONTÍNUO
.................
........................................ 148
10.1.2.4.4.
GENERALIZAÇÃO DO MOD
ELO DO PROCESSO
...................................
. 150
10.1.2.4.4.1.
RUÍDOS CORRELACIONAD
OS
...................................................................
150
10.1.2.4.4.1.1.
ESTIMAÇÃO LINEAR COM
O UM PROBLEMA DE PRO
JEÇÃO
............
150
10.1.2.4.4.1.2.
FORMA RECURSIVA PARA
A ESTIMAÇÃO LINEAR
.
...........................
154
10.1.2.4.4.1.3.
FILTRO DE KALMAN
...................................................................................
156
10.1.2.4.4.1.4. FILTRO DE KALMAN PARA SISTEMAS COM ENTRA
DAS
DETERMINÍSTICAS
.......................................................................................160
10.2.
APÊNDICE 2:
LEMA DA I
NVERSÃO MATRICIAL
..................................
161
11.
ANEXO............................................................................................................
162
FLUXOGRAMA DOS ALGORITMOS DE
ALINHAMENT
O, NAVEGAÇÃO E
ATITUDE....................................................................................................................................
163
11
LISTA DE ILUSTRAÇÕES
FIG. 2.1
Ponto P e referenciais
o -x -y -z
e
o-x-y-z
...............................................................
26
FIG
. 2.2
Ilustração dos ângulos cujos co
-
senos vêem a se constituir nos co
-
senos diretores
28
FIG. 2.3
Dois sistemas de coordenadas, o-x-y-z
e
o-x -y -z .................................................. 29
FIG. 2.
4
Ângulos de E
uler
......................................................................................................
31
FIG. 2.
5
Rotação de um ângulo
em torno do eixo
z
..........................................................
. 32
FIG. 2.
6
Componentes do v
etor r nos referenciais
o-x-y-
z e o
-x -y -z
.................................
. 32
FIG. 2.
7
Variáveis
de Posição
...............................................................................................
37
FIG. 2.
8
Variáveis
de Atitude
................................................................................................
38
FIG. 2.
9
Referencial Inercial
o-x
i
-y
i
-z
i
....................................................................................
39
FIG. 2.10
Referencial da
Ter
ra
o-x
e
-y
e
-z
e
.................................................................................
40
FIG. 2.1
1
Referencial do veículo
o-x
b
-y
b
-z
b
.............................................................................
41
FIG. 2.1
2
Referencial Local
na configuração geográfica e orientação do tipo ENU...............
42
FIG. 2.1
3
Referencial Local na configuração geográfica e orientação do tipo NED...............
43
FIG. 2.1
4
Navegação nas proximidades de um pólo............................................................... 43
FIG. 2.1
5
Componentes da Velocidade Angular da Terra no Referencial Local na configuração
geográfica e orientação do tipo NED, no hemisfério Norte
...................................
. 44
FIG. 2.1
6
Componentes da Velocidade Angular da Terra no Referencial Local na configuração
geográfica e orientação do tipo NED, no hemisfério Sul
......................................
45
FIG. 2.1
7
Referencial Local na configuração free-azimuth e orientação do tipo ENU....... 45
FI
G. 2.1
8
Cápsula auto
-
suficiente isolada do mundo exterior................................................
47
FIG. 2.1
9
Diagrama de Blocos de obtenção da velocidade e da posição em relação ao
referencial da
Terra
, numa plataforma estabilizada...............................................
50
FIG. 2.20
Diagrama de Blocos de obtenção da velocidade e da posição em relação ao
referencial da
Terra
, numa plataforma solidária....................................................
51
FIG. 3.1
Acelerômetr
o........................................................................................................... 54
FIG. 3.2
Aceleração gravitacional, w, e aceleração da gravidade,
g .................................... 59
FIG. 3.3
Diagrama de Blocos de obtenção da velocidade e da posição em relação ao
referencial da
Terra
.................................................................................................
59
12
FIG. 3.4
Diagrama das Operações Realizadas para a Transfo
rmação de Coordenadas........
61
FIG. 3.5
Ponto P e referenciais
x -y -z
e
x-y-z...................................................................... 65
FIG. 3.6
Elipsóide de Referência
..........................................................................................
. 65
FIG. 3.7
Representação esquemática da trajetória do veículo...............................................
. 72
FIG. 3.8
Representação esquemática da aceleração centrípeta na manobra ascendente da
terceira etapa...........................................................................................................
73
FIG. 3.9
Latitude x tempo
......................................................................................................
79
FIG. 3.10
Err
os em Latitude
.....................................................................................................
80
FIG. 3.11
Longitude x tempo
...................................................................................................
80
FIG. 3.12
Erros em Longitude
..................................................................................................
81
FIG. 3.13
Altura x tempo......................................................................................................... 81
FIG. 3.14
Erros em Altura
........................................................................................................
82
FIG. 3.15
Longitude x Latitude
................................................................................................
82
FIG. 3.16
Ângulo de roll x tempo......................................................................................... 83
FIG. 3.17
Ângulo pitch x tempo........................................................................................... 83
FIG. 3.18
Ângulo heading x tempo...................................................................................... 84
FIG. 3.19
Velocidade Norte x tempo.......................................................................................
85
FIG. 3.20
Velocidade Vertical para baixo x tempo
..................................................................
85
FIG. 3.21
Velocidade Leste x tempo........................................................................................ 86
FIG. 5.1
Indicação de acelerômetro colocado em elevador em três situações distintas.........
95
FIG. 5.
2
Representação das linhas da Matriz de Transformação de Coordenadas como vetores
C
1
, C
2
e
C
3
............................................................................................................... 99
FIG. 7.1
Evolução da estimativa do desalinhamento em roll, durante o alinhamento fino....
111
Fig. 7.2
Evolução da estimativa do desalinhamento em pitch, durante o alinhame
nto fino..111
FIG. 7.3
Evolução da estimativa do desalinhamento em heading, durante o alinhamento
fino..........................................................................................................................
112
FIG. 7.4
Evolução da
estimativa do bias do acelerômetro alinhado com o eixo
x
do corpo..
113
FIG. 7.5
Evolução da estimativa do bias do giroscópio com eixo de entrada alinhado com o
eixo z do corpo........................................................................................................
113
FIG. 7.6
Latitude x tempo...................................................................................................... 114
FIG. 7.7
Longitude x tempo
...................................................................................................
115
13
FIG. 7.8
Altura x tempo......................................................................................................... 115
FIG. 7.9
Erro em Latitude
......................................................................................................
116
FIG. 7.10
Erro em Longitude
...................................................................................................
116
FIG. 7.11
Erro em Altura
.........................................................................................................
117
FIG. 7.12
Latitude x tempo
......................................................................................................
118
FIG. 7.13
Longitude x tempo
...................................................................................................
118
FIG. 7.14
Altura x tempo......................................................................................................... 119
FIG. 7.15
Erro em Lati
tude
......................................................................................................
119
FIG. 7.16
Erro em Longitude
...................................................................................................
120
FIG. 7.17
Erro em Altura
.........................................................................................................
120
FIG. 10.
1
Medidas de velocidades e reta de coeficiente angular igual a
g.............................. 128
FIG. 10.
2
Evo
lução da estimativa de g
, na ausência de ruído de medida................................
132
FIG. 10.
3
Evolução do parâmetro
P
, em presença de ruído de medida...................................
132
FIG. 10.
4
Evolução da estimativa de
g, em presença
de ruído de medida...............................
133
FIG. 10.
5
Trajetórias ideal e real, em altura, do foguete..........................................................
142
FIG. 10.
6
Estrutura do filtro discreto.......................................................................................
147
FIG. 10.
7
Estrutura do filtro contínuo......................................................................................
150
FIG. 10.
8
Estimativa de uma grandeza interpretada como uma projeção d
a mesma
...............
153
FIG. 10.
9
Subespaço das observações......................................................................................
154
FIG. 10.
10
Interpretação vetorial de Z
2
..................................................................................... 155
FIG. 10.11
Projeções de
xk
em
S(Y
k-1
)
e
S(Y
k
)
............................................................................
156
14
LISTA DE ABREVIATURAS E SÍMBOLOS
ABREVIATURAS
SNI
Sistema de Navegação Inercial
C.M.
Centro de Massa
MTC
Matriz de Transformação de Coordenadas
EN
U
East North Up
NED
North East Down
GPS
Global Positioning System
VANT
Veículo Aéreo Não Tripulado
PIG
Pipeline Inspection Gauge
WGS
-84
World Geodetic System 1984
15
SÍMBOLOS
r
Vetor r.
ângulo de
roll
ângul
o de pitch
ângulo de
heading
l
b
C
Matriz de transformação de coordenadas do referencial do corpo (b) para o
referencial local (
l)
h
altura
longitude
L
gc
latitude geocêntrica
L
latitude
geodética
l
ie
velocidade angular do referencial da
Terra
em relação ao referencial inercial, em
componentes no referencial local
l
el
velocidade angular do referencial
lo
cal
em relação ao referencial da
Terra
, em
componentes no referencial local
l
il
velocidade angular do referencial
local
em relação ao referencial inercial, em
componentes no referencial local
f
força especí
fica ou aceleração não gravitacional
W
força gravitacional ou gravitação
w
aceleração gravitacional
g
aceleração da gravidade
a
i
aceleração em relação ao referencial inercial
e
e
dt
rd
v
é o vetor variação no tempo, em relação ao referencial da
Terra
, do vetor posição
do veículo, ou seja, é o vetor velocidade relativa à Terra;
N
v
componente de
e
v
na direção Norte
E
v
componente de
e
v
na direção
Leste
D
v
componente de
e
v
na direção
da vertical local, com orientação positiva para baixo
16
R
N
raio de curvatura meridiano, ou seja, na direção Norte
Sul
R
E
raio de curv
atura transverso, ou seja, na direção Leste
Oeste
e
excentricidade do elipsóide de referência do modelo do formato da Terra
x
estimativa do valor da grandeza x
x
~
erro entre a estimativa e o valor ve
rdadeiro da grandeza x
ac
bias
bias, ou desvio do zero da escala, dia a dia do acelerômetro
g
bias
bias, ou desvio do zero da escala, dia a dia do giroscópio
)(r
corresponde ao vetor
r
na for
ma de matriz anti
-
simétrica
kk
P
/
matriz de covariância do erro de estimação, no instante k, tendo sido realizada
observação (ões) no instante
k
kk
x
/
melhor estimativa da grandeza x, no instante k, tendo sido realizada observação
(ões) no instante
k
Q
k
matriz de covariância dos ruídos das medidas, no instante
k
R
k
matriz de covariância dos ruídos do modelo, no instante
k
S
k
matriz de covariância de correlacionamento dos ruídos das medidas e do modelo,
no instante
k
E[x
]
expectância ou esperança do valor da grandeza x
17
RESUMO
A exatidão das informações de posição e orientação fornecidas por um Sistema de
Navegação Inercial é fortemente dependente tanto da exatidão dos dados de posição e orientaç
ão
inicial do veículo, quanto da precisão dos giroscópios e acelerômetros.
Este trabalho objetiva, de forma precípua,
prop
or
um algoritmo de alinhamento e
nivelamento para
um Sistema de Navegação Inercial do tipo solidário ( strapdown ), que permite
a obte
nção de estimativas d
os erros de orientação inicial
,
bem como
de componentes de erros dos
sensores, conhecidas como bias dia a dia
.
Tais estimativas permitem, respectivamente, a correção
da atitude inicial do Sistema de Navegação Inercial e das leituras dos giroscópios e
acelerômetros
. U
sa
-
se
, na obtenção de tais estimativas, a formulação do Filtro de Kalman para o
caso de ruídos correlacionados.
Um algoritmo de simulação de navegação também foi implementado, de forma a
permitir
a análise dos efeitos
da
s correções realizadas nos erros, tanto de orientação inicial,
quanto de
leitura dos sensores.
Simulações permit
em
a verificação da melhora da exatidão das informações de posição
e orientação do veículo fornecidas pelo Sistema de Navegação Inercial, quando as correções
supracitadas são realizadas.
18
ABSTRACT
The accuracy of the position and attitude
information
provided by a Inertial Navigation
System depends strongly on the accuracy of the
initial
position and a
ttit
ude data and on the
accuracy of the gyros
copes and
accelerometers
.
This work intends, as a main goal, to propose an alignment algorith
m
for
a Strapdown
Inertial Navigation System, as a way to estimate the initial attitude erro
rs
as well as sensor errors
component
s
, known as turn on
-
turn off
bias
.
With s
uch estimates,
corrections
can be made on the
initial a
tti
tude and sensor lectures. A Kalman Filter for correlated noise formulation was used in
such
estimation
.
An algorithm to simulate the navigation was also implemented, so that
effect
s of
the
co
rrections
could be checked
.
From the simulations we verify an accuracy enhancement, in terms of
the
position and
at
ti
tude information
provided by the Inertial
Navigation System
.
19
1. I
NTRODUÇÃO
1.1. CONCEITOS GERAIS
Navegação consiste basicamente no conhecimento, a cada instante, da localização de um
corpo móvel ou veículo, em um sistema de referência bem determinado.
Guiagem vem a ser a condução deste veículo de um determinado local a um outro desejado.
Pode ser, portanto, definida como o processo de
comandar
os movimentos de um corpo móvel
modo a fazê-lo seguir para o ponto desejado. Entretanto, faz-se necessário ainda que
os
comandos realizados se tornem em forças e torques e, efetivamente, possam alterar o movimento
do corpo,
conforme o desejado. É aí que surg
e
o controle.
Desta forma, pode-se dizer que o processo de partida de um veículo de uma determinada
posição até a sua chegada a outra desejada envolve três operações: a primeira, chamada de
navegação, que diz respeito à determinação da posição e velocidade do tal corpo móvel em
relação a um sistema referencial conhecido; a segunda, chamada de guiagem, que consiste
no
envio dos comandos para alteração do curso do
veículo
tal que o destino desejado seja alcançado
e, por fim, a terceira, o controle, que faz c
om que a guiagem se torne efetiva, fazendo com que os
comandos gerem, de fato, a alteração desejada no curso do veículo
.
A navegação, em sua forma mais simples, vale-se de referências externas para determinar a
posição. Por exemplo, caso se esteja a pé, no terreno, pode-se recorrer a um rio, um bosque, uma
montanha ou algum outro acidente geográfico como referência no deslocamento. Na hipótese de
se estar em um automóvel, numa cidade, pode-se valer de uma praça, um posto de gasolina
ou
alguma outra referênc
ia destacada.
Nos mares, d
urante muitos anos,
os exploradores
utilizaram
as
estrelas
como referências para a navegação.
A subordinação à necessidade da existência de um auxílio externo constitui-se, no entanto,
num fator limitante da navegação, tendo em vista que este auxílio pode, por vezes, não ser
disponível ou sua utilização não ser desejada. Neste contexto, surge o conceito de navegação
inercial.
A Navegação Inercial é definida como a navegação baseada em informações pro
venientes de
sensores inerciais, que recebem esta denominação pelo fato de usarem como princípio de
20
funcionamento o fato de que, devido à inércia dos corpos, estes resistirem a mudanças em suas
quantidades de movimento linear e angular.
Estes sensores inerciais são os giroscópios* e ace
lerômetros.
Eles
detectam
mudanças de
posição e de orientação
angular
do veículo, o que permite, a partir do conhecimento da posição e
orientação iniciais do mesmo, determinar estas grandezas a cada instante e, assim, permitir a
guiagem do veículo ao ponto
final desejado, podendo prescindir de informações externas.
1.2. H
ISTÓRICO
Desde a década de 40, os sistemas de navegação, em especial os Sistemas de Navegação
Inercial (SNI) tornaram
-se importantes componentes em aplicações científicas e militares.
De 1940 a 1944, durante a Segunda Guerra Mundial, foram desenvolvidos na Alemanha os
mísseis balísticos V-1 e V-2, empregando, pela primeira vez, um sistema simples de guiagem
inercial.
Com o término da guerra, a maioria dos cientistas envolvidos nos projetos dos
referidos
mísseis transferiu-se para os Estados Unidos da América, onde a engenharia inercial ganhou
grande impulso.
O Sistema de Navegação Inercial para Navios (SINS
Ship s Inertial Navigation System )
foi desenvolvido no final dos anos 1950 e início da década seguinte, para preencher os requisitos
de posicionamento preciso dos submarinos nucleares portadores de mísseis balísticos. Após um
primeiro modelo experimental instalado no submarino Nautilus , que cruzou o Pólo Norte
navegando submerso, em 3 de agosto de 1958, o Sistema de Navegação Inercial foi empregado a
bordo do submarino George Washington , em 1960. Desde então, tem sido continuamente
refinado, aperfeiçoado e reduzido em tamanho e custo, de modo que, atualmente, seu uso foi
estendido aos s
ubmarinos de ataque, navios
-
aeródromos e outros meios de superfície.
____________________
* Alguns autores, no que diz respeito aos sensores de rotação, usam o termo giroscópio para
os que fornecem medidas de deslocamento angular e girômetro para os que
fornecem velocidades
angulares. Neste trabalho, foi utilizado o termo giroscópio para um e outro caso.
21
1.3. D
ESCRIÇÃO DE UM SISTEMA DE NAVEGAÇÃO INERCIAL
(SNI)
Um Sistema de Navegação Inercial usa sensores chamados inerciais como forma de obter a
posição e a orientação, ou atitude, do veículo a cada instante, possibilitando o guiamento do
mesmo. Estes sensores são os giroscópios, que medem as rotações, e os acelerômetros, que
medem a chamada força específica, grandeza que, como será visto adiante, permite a obt
enção do
valor da aceleração.
A integração das velocidades angulares medidas possibilita a obtenção da orientação do
veículo em relação a um referencial desejado, o que permite que se chegue, a partir das forças
específicas medidas pelos acelerômetros, às componentes das acelerações em relação a este
referencial. As acelerações são, então, integradas para obtenção da velocidade e
da
posição, em
relação a este referencial.
O Sistema de Navegação Inercial permite ao veículo conhecer sua posição e orientação
,
independente de qualquer sinal transmitido externamente. Há uma grande vantagem em um
sistema de navegação que possa fornecer a posição do veículo continuamente e com exatidão,
sem necessitar de qualquer informação externa. Esse sistema não requer a emissão ou recepção
de sinais e é imune a interferências. Isto é de particular importância para os submarinos
nucleares, que são projetados para permanecerem submersos durante suas patrulhas, por
prolongados períodos.
Qualquer sistema de navegação inercial precisa resolver a equação de aceleração do corpo
para obter a velocidade e a posição, a partir de informações da posição e orientação iniciais do
corpo móvel. Para tal, torna-se necessário escolher o tipo de mecanização adequada a cada
situação, tais como o melhor arranjo mecânico, o sistema de coordenadas apropriado e o sistema
de hardware a ser empregado.
1.4.
OBJETIVO DO TRABALHO
A estimação mais exata da posição e orientação iniciais reveste-se de fundamental
importância no processo de navegação inercial, posto que é a partir destas informações que o
Sistema de Navegação Inercial, após computar as informações recebidas dos giroscópios e
acelerômetros, fornece a posição
e a orientação
do veículo a cada instante.
22
Este trabalho propõe-se a conceber um algoritmo que permita uma redução dos erros na
obtenção das informações concernentes à orientação inicial do Sistema de Navegação Inercial
,
provendo assim o alinhamento da mesma. O algoritmo concebido, além de contribuir para a
redução
dos erros citados, produz informações que permitem corrigir os sinais recebidos dos
giroscópios e acelerômetros, no que diz respeito a uma fonte de erro caracterizada pelo desvio
do zero na escala
ou bias
, de caráter aleatório, ocorrido sempre que os sensores são ligados.
1.5.
RELEVÂNCI
A DO TRABALHO
A importância da tecnologia de navegação inercial pode ser visualizada pelos esforços
envidados pelos países que dominam a mesma de cercear o acesso dos demais países a ela,
sobretudo no que diz respeito à fabricação de sensores inerciais.
O domínio da tecnologia de navegação inercial faz-se imprescindível para qualquer país que
deseje avançar nas áreas de fabricação de aviões, de veículos aeroespaciais, de submarinos
nucleares, de mísseis de médio e longo curso. Daí o interesse dos países que integram o pequeno
grupo dos
que possuem tal tecnologia de impedir o ingresso de outros países.
Além do que foi exposto, o fato de a tecnologia de navegação inercial estar conhecendo, nos
últimos anos, avanços significativos, permite a visualização do uso cada vez maior de tal
tecnologia.
O uso de sistemas de navegação inercial em veículos terrestres já é uma realidade e não seria
utopia falar, num futuro próximo, em navegação inercial sem auxílio para percursos de algumas
horas, tal o ritmo de aperfeiç
oamento que se tem conseguido nos sensores inerciais.
Sob o risco de ver o abismo tecnológico nesta área entre os países que dominam a tecnologia
de navegação inercial e países como o Brasil aumentar cada vez mais, faz-se necessário o estudo,
a publicação
e implementação de trabalhos e a consecução de projetos na área.
Este trabalho foi concebido visando sua implementação num projeto de um Veículo Aéreo
Não Tripulado (VANT), que conta com esforços conjuntos das três Forças Armadas. O trabalho
foi proposto inicialmente com vistas à estimação dos valores dos erros de orientação inicial, de
sorte que se pudesse corrigi
-
los, real
izando assim o alinhamento fino. P
rocedimento
este que é
de
grande importância na navegação inercial. No entanto, o sucesso
conseguido
na estimação dos
valores dos desvios do zero nas escalas dos sensores, de caráter aleatório, permitiu a
23
diminuição dos erros de leitura por parte dos sensores, contribuindo para a melhoria dos
resultados obtidos durante a navegaç
ão
.
Uma vez que este trabalho tem aplicações tanto na parte do alinhamento de um Sistema de
Navegação Inercial quanto na parte de navegação
,
pode
-
se, inclusive, visualizar a contribuição da
metodologia aqui proposta em outros projetos de interesse das Forças Armadas brasileiras, e
m
particular, como o do Veículo Lançador de Satélites, e do Brasil, em geral, como em PIG
inerciais para mapeamento de tubulações petrolíferas.
1.6. ORGANIZAÇÃO DO TRABAL
HO
O presente trabalho foi dividido em 09 (nove) capítulos, 01 (um) apêndice e 01 (um)
anexo.
No primeiro capítulo (Introdução) são apresentados, além de conceitos gerai
s
que situam o
assunto de Navegação Inercial, no campo do conhecimento, um breve histórico sobre o referido
tema. O objetivo, a organização e o posicionamento do trabalho são também parte integrante
deste capítulo.
No capítulo 2 (Conceitos de Navegação Inercial) diversos conceitos utilizados na Navegação
Inercial são abordados, de forma a facilitar a compreensão dos assuntos desenvolvidos nos
demais capítulos.
No capítulo 3 (Algoritmo de Navegação e Atitude), após uma primeira parte onde
são
apresentados, além do princípio de funcionamento do acelerômetro, alguns conceitos, como
aceleração gravitacional, aceleração da gravidade, a equação diferencial de navegação e a
equação diferencial de atualização no tempo da orientação são desenvolvidas. Por fim são
apresentados, os passos do algoritmo de Navegação e Atitude e, em seguida, os resultados de
uma simulação, com vistas à validação de tal algoritmo.
Os tipos de erros que ocorrem
nos sensores, as equações de compensação e de propagação no
tempo dos mesmos são apresentados no capítulo 4 (Tratamento dos Erros).
No capítulo 5 (Algoritmo de Alinhamento e Nivelamento), os conceitos de alinhamento e de
nivelamento são explicados. Em seguida, as duas etapas do alinhamento e nivelamento, quais
sejam, o alinhamento grosseiro e o fino são apresentadas, com seus respectivos algoritmos.
O capítulo 6 (Simulações) aborda as simulações levadas a efeito no trabalho, apresentando os
objetivos, os p
arâmetros
dos sensores utilizados e uma descrição das mesmas.
24
No capítulo 7 (Análise dos Resultados), os resultados das simulações são expostos e
analisados.
O capítulo 8 (Conclus
ão)
consta de uma breve descrição do trabalho realizado, destacando a
contri
buição principal do mesmo, seguida de algumas sugestões de trabalhos futuros.
O capítulo 9 (Referências Bibliográficas) lista a bibliografia utilizada na realização deste
trabalho.
O capítulo 10 (Apêndices) é constituído por 2 (dois) apêndices. O primeiro apresenta a
Estimação Ótima, partindo do método dos mínimos quadrados até a filtragem de Kalman, que foi
a ferramenta principal usada na consecução do trabalho. O segundo apresenta o Lema da Inversão
Matricial
, formulação que é empregada no primeiro apêndi
ce
.
Há ainda um anexo com um fluxograma que abrange tanto a parte de alinhamento, quanto de
navegação.
1.7.
POSICIONAMENTO DO TRABALHO
A
QUINO,
em 1984, apresenta algoritmos básicos de navegação para Sistemas de Navegação
Inercial do tipo solidário, a bordo de
engenhos destinados a trajetórias de curta d
uração
.
D
URÃO,
em 1992,
desenvolve um algoritmo de navegação i
nercial,
usando um referencial
local com configuração free-azimut e orientação ENU.
IORIO,
em 1995, usa a filt
ragem de Kalman
na n
avega
ção inercial
auxiliada por GPS.
CAMPOS,
em 2004,
compra o uso da filtragem de K
alman
com os filtros de p
artículas
na
Estimação de trajetórias em n
avegação
inercial auxiliada por velocímetro.
1.8.
REVISÃO BIBLIOGRÁFICA
SHABANA, em 1988 e HAUG, em 1989, ao tratarem do problema da Dinâmica de Multi-
corpos,
apresentam
as ferramentas necessárias ao desenvolvimento das equações de navegação e
atitude.
SAVAGE, em 1985, aborda de forma ampla o assunto Navegação Inercial com plataforma
solidária, apresentando desde os conceitos mais importantes até algoritmo de navegação e
tratamento de erros.
25
TITTERTON, em 1997, também apresenta o problema da Navegação Inercial com
plataforma solidária. E o faz de forma ampla, abrangendo os vários tipos de sensores, navegação,
alinhamento e mesm
o um exemplo de projeto de um Sistema de Navegação Inercial.
FRANKLIN e POWELL,
em 1997
abordam o problema do controle discreto, apresentando,
entre outros assuntos, a filtragem de Kalman. Este último assunto também é apresentado de
forma bastante completa
por
LABARRERE,
KRIEF
e GIMONET e por HERMERLY.
GUIZIOU, em 2004, apresenta conceitos e formulações importantes para Navegação
Inercial, inclusive o princípio de Einstein-
Galileu.
De igual forma, A
LONZO
, em 1994, faz uma
apresentação
geral
e sucinta do ass
unto.
N
EBOT,
em 1997
, aborda o assunto da calibração e alinhamento iniciais de um Sistema de
Navegação Inercial, servindo de subsídio a este trabalho quanto aos auxílios utilizados no
procedimento de alinhamento fino.
UM, L
EE,
S
EONG
-TAEK P
ARK
e G
OOK
P
ARK
,
em
2000,
apresenta
m uma metodologia
para tratar o problema dos bias dia a dia dos sensores.
26
2.
CONCEITOS DE NAVEGAÇÃO INERCIAL
2.1.
TRANSFORMAÇÃO DE COO
RDENADAS
Algo bastante comum no estudo da navegação inercial é a necessidade de mudança de
sistema de coordenadas, matematicamente falando, ou de referencial, fisicamente falando. Serão
usados, neste trabalho, basicamente, 04 (quatro) referenciais, que serão vistos mais adiante. Faz-
se mister conhecimentos básicos de álgebra linear. HAUG, em 1989, trata do assunto e serviu de
base para a explanação que se segue.
Conforme a FIG. 2.1, a localização de um ponto P no espaço pode ser definida tanto em
relação a
o referencial
x-y-z
quanto ao
x -y -z .
Ainda, de acordo com a FIG. 2.1, pode-
se escrever
:
r
P
= r
+ C.
s
P
(2.1)
onde:
r
é o vetor das coordenadas da origem do referencial
x -y -z
no referencial
x-y-z;
s
P
é o vetor das coordenadas de P no referencial
x -y -z
; e
C é a Matriz de Transformação de Coordenadas do referencial x -y -z
para o referencial x-y-
z.
x
y
z
x
'
z
'
y
'
s
P
r
r
P
P
FIG. 2.1
Ponto P e referenciais
o -x -y -z
e
o-x-y-z
o
o
27
Basicamente as componentes de um vetor qualquer em relação a um sistema de coordenadas
podem ser transformadas em componentes do mesmo vetor em relação a um outro sistema de
coordenadas. Para isso bastam, no espaço tridimensional, 6 (seis) grandezas independentes: 03
(três) para definir o vetor que localiza a origem do novo sistema de coordenadas e outras 03 (três)
para definir a orientação dos eixos do novo sistema.
É evidente que, se os dois sistemas de coordenadas têm a
mesma origem, bastam, neste caso,
03 (três) grandezas para definir a orientação no novo sistema.
A definição da orientação de um sistema de coordenadas em relação a outro é feita através da
Matriz C, que é chamada Matriz de Transformação de
Coordenadas
do sistema antes da rotação
,
ou sistema original,
para o
sistema
após
a rotação, ou sistema novo
. Esta matriz C será função de
apenas 3 (três) variáveis independentes.
Esta Matriz de Transformação de Coordenadas C pode ser expressa a partir de diferentes
va
riáveis
. Entre outros podem ser citados como os mais usuais os seguintes:
-
Co
-
senos Diretores;
-
Ângulos de Euler;
-
Parâmetros de Euler
.
2.1.1.
CO
-
SENOS DIRETORES
Os co-senos diretores correspondem aos co-senos dos ângulos formados entre cada eixo do
sistema novo e cada eixo do sistema original; ângulos estes tomados no sentido anti-
horário
. No
espaço tridimensional, pelo fato de cada sistema de coordenadas possuir 03 (três) eixos, ao serem
tomados dois a dois, esses eixos formam 09 ângulos e, portanto, tem-
se
09 (nove) co-
senos
diretores e a matriz
de tran
tem a seguinte forma:
333231
232221
131211
ccc
ccc
ccc
C
(2.
2)
onde o elemento c
11
representa o co-seno formado entre o eixo x do sistema de coordenadas
novo e o eixo x do sistema original, o elemento c
12
representa o co-seno formado entre o eixo x
do sistema de coordenadas novo
e o eixo
y do
original
, e assim por diante.
28
A
FIG. 2.
2 ilustra, através de uma rotação em torno do eixo z, como se obtém os elementos
da matriz C em função dos co-senos diretores. O sistema o-x-y-z corresponde ao sistema original
e o o-x -y -z , ao novo. Na figura, o co-seno do ângulo 11 vem a ser o elemento c
11
e assim por
diante.
Tomando
-se os eixos dos sistemas de coordenadas, dois a dois, sendo o primeiro eixo o do
sistema após a rotação e o segundo do sistema
original
, e medindo-se os ângulos no sentido anti-
horário, pode-se, tomando-se os co-senos destes ângulos, obter, em função destes co-
senos
, a
matriz C,
que é a Matriz de Transformação de Coordenadas do sistema original para o novo
.
O fato de a Matriz de Transformação de Coordenadas C em função dos co-senos diretores
depender de 09 (nove) parâmetros diferentes implica na existência de relações de dependência
que permitem, partindo-se de 03 (três) valores conhecidos dos co-senos diretores, obter-se os
demais, pois, como foi visto, a orientação de um sistema de coordenadas em relação a outro fica
completamente definida por apenas 03 (três) parâmetros.
Se o sistema original sofre uma rotação de um ângulo
em torno do eixo z,
conforme
ilustrado na FIG. 2.2,
tem
-
se:
ângulo 11 = 360º
cos 11 = cos
;
ângulo 1
2
=
9
0º
cos 12 = sen
;
ângulo 21 = 270º
cos 11 =
sen
;
FIG 2.2
Ilustração dos ângulos cujos co
-
senos vêem a se constituir
nos co
-
senos diretores
â
ngulo
21
x
y
z
z
x'
y'
o
â
ngulo 1
2
â
ngulo 22
â
ngulo
11
(â
ngulo
33=0)
É ainda fácil de perceber que, neste caso:
â
ngulo
13=
â
ngulo
23=
â
ngulo
31=
â
ngulo
32=90º
29
ângulo 22
= 360º
cos 11 = cos
;
ângulo 13= ângulo 23= ângulo 31= ângulo 32=90º
cos 13 = cos 23 = cos 31=cos 32 = 0
; e
ângulo 33 = 0º
cos 33 = 1.
Portanto, tem
-
se, neste caso:
100
0
cos
0
cos
sen
sen
C
Sejam
i, j
e
k
os vetores unitários no sitema
o-x-y-z
e de
i , j
e
k
os no sistema
o-x -y -k .
Pela decomposição de vetores ilustrada na
FIG
2.3
, pode
-
se escrever:
'.0'.'.
cos
kj
sen
ii
(2.3)
'.0'.
cos
'. kji
sen
j
(2.4)
E, obviamente:
'.1'.0'.0 kjik
(2.5)
FIG. 2.3
Dois sistemas de coordenadas,
o-x-y-z
e
o-x -y -z
x
y
x'
y'
o
i
i'
j'
j
j
sen
.i
cos j'
-sen j
cos i'
i
'
z
z
'kk
30
Comparando
-se a EQ. 2.3, a EQ. 2.4 e a EQ. 2.5 com a Matriz C, percebe-se que as colunas
da mesma correspondem às componentes dos vetores unitários i,
j
e k. Uma vez que tais vetor
es
são ortogonais entre si e de módulo unitário, tem
-
se:
I
k
j
i
CCCC
TT
100
010
001
00
00
00
.
2
2
2
(2.6)
onde
I
é a matriz identidade de dimensão 3x3.
Ou seja, a matriz C é ortonormal.
Então,
a EQ. 2.6 fornece seis relações de dependência para os nove co-senos diretores, d
e
sorte que se tem apenas três deles independentes.
2.1.2.
PARÂMETROS DE EULER
OU QUATERNIONS
Os elementos da Matriz de Transformação de Coordenadas C também podem ser expressos
em função dos chamados Parâmetros de Euler ou Quaternions , cuja definição baseia-se no
teorema de Euler:
Se as origens de dois referenciais cartesianos (com eixos orientados segundo a regra da mão
direita) coincidem, então eles podem ser levados à coincidência através de uma simples rotação
sobre um eixo .
Sendo u o vetor unitário que define a orientação do eixo de rotação e
a magnitude do
ângulo de rotação entre os dois referenciais, os parâmetros de Euler são definidos da seguinte
forma:
2
.
2
cos
3
2
1
0
sen
u
e
e
e
e
e
(2.
7)
Os parâmetros de Euler estão relacionados através da seguinte expressão, de sorte a se ter
apenas 03 (três
) parâmetros independentes:
1
2
3
2
2
2
1
2
0
eeee
(2.
8)
31
Os elementos da matriz C podem ser expressos em função dos parâmetros de Euler da
seguinte forma:
2
1
....
..
2
1
..
....
2
1
2
3
2
010322031
1032
2
2
2
03021
20313021
2
1
2
0
eeeeeeeeee
eeeeeeeeee
eeeeeeeeee
C
(2.
9)
2.1.3.
ÂNGULOS DE EULER
Os elementos da Matriz de Transformação de Coordenadas C podem também ser expressos
em função dos ângulos de Euler.
Os ângulos de Euler são coordenadas angulares definidas por uma seqüência de 03 (três)
rotações sobre os eixos coordenados do referencial. Uma seqüência usual de rotações é a
seguinte: rota
ção de um ângulo
sobre o eixo
z (ângulo de yaw ); rotação de um ângulo
sobre
o novo eixo y (ângulo de pitch ) e, finalmente, rotação de um ângulo
sobre o novo eixo x
(ângulo de roll ).
FIG. 2.4
Ângulos de Euler
32
A obtenção da Matriz de Transformação de Coordenadas C quando o referencial sofre
apenas uma rotação em torno de um dos eixos é muito simples.
Seja, portanto, o referencial o-x-y-z, que, ao sofrer uma rotação de um ângulo
em torno do
eixo z, passa a ser denominado de referencial
o-x -y -z .
r
x
y
z
z
x'
y'
o
FIG. 2.5
Rotação de um ângulo
em torno do eixo z
x
y
x'
y'
o
r
x
FIG. 2.6
Componentes do vetor
r
nos referenciais
o-x-y-z
e
o-x -y -z
r
x
r
y
r
y
.sen
r
x
.cos
r
x
r
x
.sen
r
y
r
y
.cos
33
Observando
-se a FIG. 2.6, pode-se, através de trigonometria elementar, obter as seguintes
igualdades:
r
x
= r
x
.cos
+ r
y
.sen
+ r
z
.0
(2.10
)
r
y
= -r
x
.sen
+ r
y
.cos
+ r
z
.0
(2.11
)
E
, evidentemente, tem
-
se:
r
z'
= r
x
.0 + r
y
.0 + r
z
.1
(2.12
)
Escrevendo as EQ. 2.7, 2.
8
e 2.
9
na forma matricial, tem
-
se:
z
y
x
z
y
x
r
r
r
sen
sen
r
r
r
100
0
cos
0
cos
'
'
'
Ou seja:
100
0
cos
0
cos
1
sen
sen
C
Analogamente, tem
-
se:
-
Para rotação de um ângulo
sobre o eixo
y
:
cos
0
010
0
cos
2
sen
sen
C
;
(2.1
3)
-
Para rotação de um ângulo
sobre o eixo
x:
cos
0
cos
0
001
3
sen
sen
C
.
(2.1
4)
Se as três rotações acima forem executadas na seqüência em que foram descritas, chamando-
se o sistema de coordenadas, após a primeira rotação, de II, o após a segunda de III e o após a
terceira de final, pode
-
se escrever:
1
CC
II
inicial
;
2
CC
III
II
e
3
CC
final
III
(2.1
5)
onde o índice subescrito designa o sistema de coordenadas antes da rotação e o sobrescrito o
depois.
34
Desta f
orma, a Matriz de Transformação de Coordenadas do sistema inicial para o final, após
uma rotação em torno do eixo z, seguida de uma rotação em torno do novo eixo y e, finalmente,
após uma em torno do novo eixo
x
, é obtida da seguinte maneira:
123
.... CCCCCCC
II
inicial
III
II
final
III
final
inicial
(2.1
6)
ou seja:
cos
.
coscos
...
coscos
.
cos
..
cos
.
cos
.
cos
..
cos
.
cos
..
cos
.
cos
.
cos
sensensensensensen
sensensensensensensen
sensen
C
final
inicial
(2.1
7)
É importante ressaltar que a ordem das rotações sobre cada eixo tem que ser respeitada. Essa
característica das rotações no espaço é facilmente verificada ao recordar
-
se que, na álgebra linear,
o produto de matrizes não é comutativo. No caso de rotações infinitesimais, no entanto, a ordem
não mais influi na formulação da Matriz de Transformação de Coordenadas. Se não, conside-
se
as rotações acima como infinitesimais, de sorte que se pode aproximar os co-senos dos ângulos
por um e os senos pelo valor do próprio ângulo, bem como se desprezar os valores decorrentes do
produto de dois ângulos. Assim, a expressão da matriz C, no caso de rotações infinitesimais, tem
a forma que se segue e, neste caso, a ordem das rotações não é mais importante:
0
0
0
1
1
1
)33( x
final
inicial
IC
(2.1
8)
onde
I
(3x3)
é a matriz identidade de dimensão 3 x 3 e
0
0
0
corresponde ao vetor
dos ângulos de Euler, , na forma anti
-
simétrica.
Como cada um dos ângulos de Euler, ,, , são medidos em relação a sistemas de
coordenadas diferentes, a velocidade angular do referencial final em relação ao referencial inicial,
com componentes no referencial final, não é a simples derivada temporal dos ângulos de Euler;
antes se relacionam através da seguinte expressão:
35
II
III
II
final
III
III
final
III
final
final
z
y
x
CCC 0
0
.
0
0
0
0
(2.1
8)
Portanto:
0
0
cos
.
cos
.
cos
cos
.
cos
.
0
cos
0
0
cos
0
cos
0
001
0
0
sensen
sensensen
sen
sen
sen
z
y
x
sen
sen
sen
z
y
x
.
cos
.
cos
.
cos
..
cos
.
.
(2.1
9)
Dividindo a segunda equação da EQ. 2.17 por
cos
e a terceira por
sen
e, em seguida,
somando as duas equações, tem
-
se:
sen
sen
sen
z
y
cos
cos
.
cos
.
cos
Assim, obtém
-
se:
sen
sen
sen
sen
zy
.
cos
cos
.
cos
.
.
cos
cos
..
22
E, portanto:
sec
).
cos
..(
zy
sen
(2.20
)
Substituindo a EQ. 2.20
na primeira equação da EQ. 2.1
9
, tem
-
se:
tan
).
cos
..(
zy
sen
(2.21
)
Por fim, dividindo a segunda equação da EQ.
2.1
9 por
sen
e a terceira por
cos
e, em
seguida, somando as duas equações, tem
-
se:
cos
cos
.
cos
sen
sensen
z
y
Assim, obtém
-
se:
sen
sen
sen
sen
zy
.
cos
cos
.
.
cos
.
cos
.
22
E
, portanto:
36
sen
zy
.
cos
.
(2.22
)
Desta maneira, as EQ.
2.20, 2.21
e 2.22
são as equações de atualização dos ângulos de Euler,
em função das componentes da velocidade angular do referencial final em relação ao referencial
inicial:
sec
).
cos
..(
.
cos
.
tan
).
cos
..(
zy
zy
xzy
sen
sen
sen
(2.
23)
2.2.
VARIÁVEIS
DE NAVEGAÇÃO
As variáveis de navegação utilizadas neste trabalho, bem como os seus conceitos seguem o
apresentedo por
SAVAGE, em 1985,
D
URÃO,
em 1992
e
IORIO,
em 1995.
A
s
variáveis de navegação são constituídos pela
s
variáveis
de posição, que definem a
posição do corpo, variáveis de velocidade, que especificam o movimento do corpo e variáveis de
atitude, que especificam a orientação do corpo.
2.2.1.
VARIÁVEIS
DE POSIÇÃO
A posição de um veículo (corpo) em relação à
Terra
é descrita através de três parâmetros:
longitude, latitude e altura.
Altitude (h) é definida como a distância vertical acima da
Terra
, ao longo da perpendicular à
superfície da
Terra.
Longitude ( ) é definida como a distância angular, medida no plano equatorial, entre o plano
que contém o eixo polar e o meridiano correspondente à posição do veículo e o que contém o
eixo polar e o meridiano que passa por Greenwich, na Ingla
Terra.
Para definir latitude, tem-se que primeiramente definir a posição de referência na superfície
como a interseção com a superfície da Terra da perpendicular à superfície da
Terra
baixada a
partir da posição real do veículo. Latitude é o ângulo medido em relação ao plano equatorial a
partir da posição de referência na superfície. A latitude pode ser geocêntrica ou geodética.
37
Latitude Geocêntrica (L
gc
) é o ângulo formado entre o plano equatorial e a linha formada
entre a posição do veículo e o centro da
Terra
.
Latitude Geodética (L) é o ângulo formado entre o plano equatorial e a linha perpendicular à
superfície da
Terra
passando pela posição real do veículo. É a mais comumente utilizada como
referência de posicionamento em relação à
Terra
.
A FIG.
2.7 ilustra as variáveis de pos
ição.
2.2.2.
VARIÁVEIS
DE VELOCIDADE
Os
variáveis
de velocidade são em geral expressos em termos de componentes verticais e
horizontais do movimento translacional do veículo em relação à
Terra
.
A componente v
ertical da velociade é definida como a variação temporal da altura.
A componente horizontal da velocidade é a projeção, no plano tangente à superfície da
Terra
na posição de referência do corpo, do vetor velocidade do corpo em relação à
Terra
. Esta
compone
nte, por sua vez, é expressa em duas componentes, nas direções norte e leste.
FIG. 2.7
Variáveis de Posição
L
Plano tangente à
superfície da
Vertical Local
Posição de referência
na superfície
h
h
= altitude do veículo;
L
gc
= Latitude geocêntrica;
L
= Latitude geodética;
= Longitude
L
gc
Superfície
da
Terra
Plano Equatorial
Plano do Meridiano
Local
Plano do Meridiano
de Greenwich
Posição real do veículo
Eixo
Polar
38
2.2.3.
VARIÁVEIS
DE ATITUDE
A FIG. 2.8 mostra os ângulos usualmente usados como parâmetros para definir a atitude
(orientação) de um corpo veículo.
As
variáveis
de atitude
são, portanto, os seguintes:
Ângulo de rolagem ( roll ) - : é o ângulo medido em torno do eixo longitudinal do
veículo.
Ângulo de inclinação ( pitch ) - : é definido como o ângulo, medido no plano vertical
(plano que contém o eixo longitudinal do corpo e que é perpendicular ao plano tangente à
superfície da
Terra
na posição de referência do corpo), entre o eixo longitudinal do corpo e um
plano que, contendo o Centro de Massa (C.M.) do corpo, é paralelo ao plano tangente à superfície
da
Ter
ra
na posição de referência do corpo.
Ângulo de guinagem ( yaw ) - : é o ângulo, medido num plano paralelo ao plano
horizontal (plano tangente à superfície da
Terra
na posição de referência do corpo), contendo o
C.M. do corpo, entre uma direção de referência e o eixo longitudinal do corpo. Se a direção de
referência é o
Norte Geográfico, o ângulo de guinagem recebe a denominação de ângulo de rumo
( heading ).
2.3.
REFERENCIAIS
Na referenciação dos parâmetros de posição, de velocidade e de orientação são usados 04
(quatro) sistemas de coordenadas:
FIG. 2.8
Variáveis de Atitude
39
-
referencial inercial (
i
) c
om origem no centro da
Terra
;
-
referencial da
Terra
(
e
). Este acompanha a rotação da
Terra
; e
- ref
erencial local ou de navegação (
l. Algumas vezes representado pela letra n
).
- referencial do corpo (b
).
2.3.1.
REFERENCIAL INERCIAL COM ORIGEM NO CENTRO DA
TERRA
Ne
ste sistema, o eixo
z
i
é alinhado com o
eixo polar da
Terra
e os eixos
x
i
e
z
i
se estendem no
plano equatorial. O referencial não acompanha a rotação da
Terra
e sua origem coincide com o
seu centro de massa, como pode ser observado na
FIG. 2.
9.
2.3.2.
REFERENCIAL DA
TERRA
O referencial da
Terra
é composto por 3 (três) eixos ortogonais (orientados segundo a regra
da mão direita) com origem no centro da
Terra
, em que o eixo z
e
está orientado na direção do
pólo Norte, os eixos x
e
e y
e
estão no plano equatorial, sendo que o eixo x
e
aponta sempre para o
meridiano que passa pela cidade de Greenwich, na Ingla
Terra
, e o eixo y
e
encontra-se a 90º
daquele.
FIG. 2.9
Referencial Inercial
o-x
i
-y
i
-z
i
y
i
x
i
z
i
o
40
Este referencial gira em relação ao referencial inercial segundo uma velocidade angular
de
aproxi
madamente 15,04 graus por hora (correspondente à rotação da
Terra
), tendo o eixo
z
e
como
eixo de rotação.
2.3.3.
REFERENCIAL DO CORPO
Este sistema é um conjunto de três eixos ortogonais fixos na estrutura do veículo. A origem
do sistema é o cen
tro de gravidade do corpo e as direções são os eixos principais de inércia.
Os sentidos dos eixos são:
x
b
: positivo para a frente.
y
b
: positivo para a direita.
z
b
: positivo para baixo.
A
FIG. 2.11
apresenta o referencial do veículo.
FIG. 2.10
Referencial da Terra
o-x
e
-y
e
-z
e
PLANO EQUAT
ORIAL
x
e
y
i
x
i
y
e
MERIDIANO
DE GREENWICH
MERIDIANO
DE
REFERÊNCIA
INERCIAL
z
i
, z
e
L
. t
. t
41
2.3.4.
RE
FERENCIAL LOCAL
OU DE NAVEGAÇÃO
O referencial local pode ser configurado, basicamente, de 02 (duas) formas:
- Estável no Espaço (
Space Stable
); e
- Localmente Nivelado (
Local Level
).
2.3.4.1.
REFERENCIAL ESTÁVEL NO ESPAÇO (
SPACE STABLE
)
Neste caso, o referen
cial local permanece com sua orientação constante em relação ao espaço
inercial.
2.3.4.2.
REFERENCIAL
LOCALMENTE NIVELADO
(
LOCAL LEVEL
)
O referencial local, neste caso, é constituído por três eixos orientados segundo a regra da
mão direita, sendo que dois dos eixos estão sobre o plano tangente à superfície da Terra e o
terceiro eixo está alinhado com a vertical local, que é a perpendicular ao plano tangente à
superfície da
Terra
baixada a partir da posição do veículo.
O refererencial localmente nivelado pode ser
orientado, basicamente de duas maneiras:
- Sistema ENU (East North Up): Com a origem no ponto de referência do veículo na
superfície da Terra, seus eixos estão orientados segundo a direção do leste geográfico terrestre
(eixo x), do norte geográfico (eixo
y) e na direção da vertical local, apontando para cima (eixo z);
FIG. 2.
11
Referencial do veículo
o-x
b
-y
b
-z
b
x
b
z
b
y
b
o
C.M.
42
- Sistema NED (North East Down): Similar ao Sistema ENU, possui, no entanto, os eixos
orientados para o norte geográfico terrestre (eixo x), para o leste (eixo y) e na direção da vertical
lo
cal, apontando para baixo (eixo z).
Com relação à componente na vertical local de sua velocidade de rotação em relação ao
referencial da
Terra
, o referencial localmente nivelado pode ser configurado de diferentes formas:
-
Configuração geográfica;
-
Conf
iguração wander azimuth ;
e
- Configuração free azimuth .
2.3.4.2.1.
CONFIGURAÇÃO GEOGRÁF
ICA
Nesta configuração, o referencial mantém um eixo de coordenadas sempre apontado para o
Norte, outro sempre apontado para o Leste. O terceiro fica apontado na direção da
vertical local e
sentido para cima, para orientação do tipo ENU, ou para baixo, no caso de orientação do tipo
NED.
Direção Leste
Direção Norte
Direção da Vertical
Local
L
Latitude
Longitude
FIG. 2.12
Referencial Local na configuração geográfica e
orientação do tipo ENU
43
Chamando
-se de
N
a componente da velocidade angular do referencial local na direção
Norte, pode-se observar, através da
FIG.
2.13, que a componente na direção da vertical local,
com o referencial local segundo a orientação NED, será:
)
tan(
. L
ND
(2.24
)
Não é difícil de concluir que, no caso de o referencial local ter orientação do tipo ENU, ter-
se
-ia:
)
tan(
. L
NU
(2.25
)
Esta configuração facilita o cálculo da latitude e da longitude.
Um problema que ocorre com a utilização da configuração geográfica é a existência de
singularidades para navegação nas proximidades dos pólos, que exige uma variação de ângulo de
rumo ( heading ) excessivamente alta para manter o alinhamento com o Norte.
Na
FIG.
2.14, percebe-se que o veículo, ao cruzar o pólo Norte, se vê obrigado a m
udar
instantaneamente a sua referência do Norte de um ângulo de 180º.
Pólo
Veículo antes de
cruzar o Pólo Norte
Direção Norte
Veículo
depois de
cruzar o Pólo Norte
Direção Norte
FIG. 2.14
Navegação nas proximidades de um pólo
D
=
D
.tag(L)
y (leste)
x (norte)
z (down)
L
L
Eixo Polar
da Terra
Eixo Equatorial
da Terra
Latitude
Geodética
FIG. 2.13
Referencial Local na configuração geográfica e
orientação do tipo NED
44
No algoritmo de navegação apresentado neste trabalho será utilizada esta configuração, com
orientação NED.
Com configuração geográfica e orientação NED e a plataforma estacionária em relação à
Terra
, pode-se obter as componentes do vetor velocidade angular da
Terra
em relação ao
referencial inercial, que são as seguintes, conforme pode ser observado nas
FIG. 2.15
e 2.16
:
no hemisfério Norte:
)(.
0
)
cos(
.
L
sen
L
l
ie
(2.2
6)
e, no hemisfério Sul:
)(.
0
)
cos(
.
L
sen
L
l
ie
(2.27
)
onde
é a magnitude da velocidade de rotação da
Terra
em relação ao referencial inercial,
que é de
, aproximadamente,
15,04 graus por hora.
Pode-se observar que a
EQ. 2.24
se verifica tanto na EQ. 2.26 como na 2.27, como não
poderia deixar de ser.
FIG. 2.15 Componentes da Velocidade Angular da
Terra no
Referencial Local na configuração geográfica e orientação do tipo
NED, no hemisfério Norte
)(.)( L
sen
Z
ie
y (leste)
x (norte)
z (down)
L
Eixo Polar
da Terra
Eixo Equatorial
da Terra
Latitude
Geodética
Velocidade de
Rotação da Terra
)
cos(
.)( L
X
ie
L
45
.
2.3.4.2.2. CONFIGURAÇÃO FREE AZIMUTH
Na configuração Free Azimuth , os eixos situados no plano horizontal não são corrigidos de
modo a se alinharem com o Norte e com o Leste. Esta característica elimina a existência de
singularidades no cálculo da posição do corpo. O referencial com esta configuração apresenta
velocidade de rotação nula, em relação à vertical local, com referência ao espaço inercial; o que
significa que os eixos situados sobre o plano tangente à superfície da
Terra
na posição de
referência do corpo apresentam velocidade de rotação sobre a vertical, igual e oposta à projeção
da velocidade de rotação da
Terra
sobre a vertical local.
)(. L
sen
ZZ
x (leste)
y (norte)
z (up)
L
Eixo Polar
da Terra
Eixo Equatorial da
Terra
Latitude Geodética
Velocidade de
Rotação da Terra
FIG. 2.17
Referencial Local na configuração free-
azi
muth e
orientação do tipo ENU
FIG. 2.16 Componentes da Velocidade Angular da
Terra no
Referencial Local na configuração geográfica e ori
entação do tipo
NED, no hemisfério Sul
)
cos(
.)( L
X
ie
y (leste)
x (norte)
z (down)
L
Eixo Equatorial
da Terra
Latitude
Geodética
Velocidade de
Rotação
da
)(.)( L
sen
Z
ie
L
Eixo Polar
da Terra
46
Pode-se observar, através da
FI
G. 2.17, que a componente da velocidade angular do
referencial local em relação ao referencial da
Terra
na direção da vertical local,
Z
,com o
referencial local segundo a orientação ENU, será:
)(. L
sen
ZZ
(2.28
)
No caso
de o referencial local ter orientação do tipo NED, ter-
se
-
ia:
)(. L
sen
ZZ
(2.29
)
Desta maneira, tendo-se um veículo parado na superfície e associando-se ao mesmo um
referencial com a configuração free azimuth , este referencial, se observado
da
Terra
, girará
horizontalmente, com velocidade de rotação igual em magnitude e oposta em sentido à projeção
do vetor velocidade de rotação da
Terra
na vertical local.
2.3.4.2.3. CONFIGURAÇÃO WANDER AZIMUTH
Nesta configuração os eixos situados sobre o plano tangente à superfície da
Terra
na posição
de referência do veículo na superfície (plano horizontal local) não têm rotação em relação à
Terra
; o que equivale a dizer que esses eixos têm velocidade de rotação sobre a vertical, em
relação ao referencial inercial, igual à projeção do vetor velocidade de rotação da Terra na
vertical local.
Desta forma, tem-se, para o referencial local na configuração Wander Azimuth , tanto para
a orientação NED quanto para a ENU, o seguinte:
0
Z
(2.30
)
2.4.
NAVEG
AÇÃO INERCIAL
2.4.1.
FUNDAMENTOS
2.4.1.1.
PRINCÍPIO DA RELATIV
IDADE DE EINSTEIN
-
GALILEU
GUIZIOU, em 2004,
apresenta o Princípio da Relatividade de Einstein
-
Galileu.
47
Seja Ri um Referencial Inercial, baseado em três direções estelares. Sabe-se que 2 (dois)
giroscópios de 2 (dois) graus de liberdade, com seus eixos de rotação ortogonais, podem
materializar tal referencial.
Seja uma cabine em movimento dentro do campo de gravitação
w
, resultante do conjunto das
massas do universo (apenas as mais próximas são consideradas), que está submetida a um
conjunto de forças. Este conjunto de forças submete a cabine a uma aceleração absoluta
a.
Designa
-se como força específica ou aceleração estática ou aceleração não gravitacional o
vetor:
waf ,
onde:
a
é o vetor ac
elera
ção em relação ao referencial inercial
Ri
e
w é a aceleração decorrente do campo gravitacional.
FIG. 2.18
Cápsula auto
-
suficiente isolada do mundo exterior
Trajetória
R
i
Cápsula S
(
Vetor Velocidade Angular em
relação ao Ri
)
f
(
Vetor
força específica ou
aceleração não gravitacional)
w
(
Vetor
aceleração
gravitacional)
a
=
f
+
w
(
Vetor
aceleração)
C.M.
48
Questão proposta por Einstein:
Imagine
-
se um veículo totalmente isolado do mundo exterior, no que diz respeito a
:
-
telecomunicações;
-
emissões de todo tipo (eletromagnéticas, acústicas entre outras).
Não podendo, portanto, nada receber nem emitir. No entanto:
-
autônomo no que diz respeito à energia;
- com capacidade de transportar todo tipo de conhecimento (discos, livros,
CD-ROM e outros tipos de memória digital entre outros);
-
com a possibilidade de utilizar todos os meios de cálculos imagináveis.
O que se poderia, estando no interior do veículo, conhecer do movimento do mesmo?
Resposta:
Princípio da Relatividade de Einstein-Galileu: É possível medir, a bordo do veículo, dois
vetores: o vetor rotação instantânea
do veículo, em relação ao referencial inercial e o vetor
força específica
f ; e nada além disso .
Conseqüências deste prin
cípio:
a)
Este princípio mostra que, em missões espaciais e em vôos balísticos, sob ação
apenas do campo gravitacional, é impossível medir o que quer que seja deste campo.
Estando em órbita da
Terra
, ou de Marte ou até do Sol, nada permite se perceber a
difer
ença. Em todos os casos o veículo (ou o corpo em movimento) e os objetos dentro dele
estão em queda livre, implicando na sensação de flutuação, o que não significa
absolutamente em ausência de gravidade, que, antes, é a única agente.
b)
Como, então, evidencia
-
se g sobre a
Terra
?
Sobre a Terra, o contato da superfície de um corpo imóvel (o assoalho de uma
casa, por exemplo) sobre um objeto permite avaliar a gravidade, não através da medida do
peso deste objeto, mas da reação desta superfície sobre o objeto cujo peso se quer medir,
que tem sentido oposto. Uma balança mede, portanto, esta reação entre a superfície imóvel
e o objeto cujo peso se quer medir. A medição desta reação é impossível num vôo balístico.
Sensores foram concebidos de maneira a medirem esses vetores. O sensor que
mede a rotação instantânea de um corpo em relação ao Referencial Inercial chama-
se
giroscópios.
O que mede a força específica é denominado acelerômetro.
49
2.4.2.
SISTEMAS DE NAVEGAÇÃO INERCIAL
Conforme visto no item 2.4.1.1, as únicas informações que um veículo pode obter de forma
autônoma, ou seja, sem recorrer a nenhum auxílio externo, acerca de seu próprio movimento são
a velocidade angular em relação ao referencial inercial e a força específica. Uma vez que estas
grandezas são vetoriais e os sensores inerciais utilizados, quais sejam, os giroscópios e os
acelerômetros, via de regra, são capazes de realizar medidas em apenas um eixo, os Sistemas de
Navegação Inercial são, em geral, constituídos por 3 (três) giroscópios montados formando um
tr
iedro e 3 (três) acelerômetros montados da mesma forma, de sorte que, tendo-se três
componentes mutuamente ortogonais dos dois vetores desejados, possa-se, no espaço
tridimensional, obter os referidos vetores.
2.5. MECANIZAÇÕES TÍPICAS DE UM SISTEMA DE NA
VEGA
ÇÃO
INERCIAL
ALONZO, em 1994, entre outros, apresenta as mecanizações típicas de um Sistema de
Navegação Inercial.
Denomina
-se mecanizações o arranjo mecânico que une a plataforma, onde estão os sensores
inerciais, ao veículo.
Basicamente, há dois tipos
de
mecanizações
possíveis para os Sistemas de Navegação
Inercial: sistemas com plataforma estabilizada ( mechanized platform ou gimballed platform )
e sistemas com plataforma solidária ( strapdown ).
Nos sistemas com plataforma estabilizada, esta é suspensa de forma a poder ser girada sobre
três eixos ortogonais por um servomecanismo, de forma a
se
manter sempre alinhada com o
referencial local.
O sinal de saída dos giroscópios fornece a velocidade angular do referencial do corpo em
relação ao inercial. De posse da velocidade angular do referencial local em relação ao inercial,
dado este fornecido pelo algoritmo de navegação, obtém-se a velocidade angular do corpo em
relação ao referencial local e, assim, a atitude do veículo em relação ao referencial local. Esta
informação é enviada ao servomecanismo que mantém a plataforma alinhada com o referencial
local, como foi dito. A plataforma, portanto, não experimenta nenhuma rotação em relação ao
50
referencial local, apesar do veículo estar em movimento. Nessa situação, os acelerômetros, uma
vez que permanecem com seus eixos sempre alinhados com o referencial local, medem as
componentes da força específica diretamente neste referencial.
Assim
, os acelerômetros podem medir as forças específicas medidas diretamente
ao
algoritmo de navegação, de forma a se obter a velocidade e posição, via 2 (duas) sucessivas
integrações.
A
FIG. 2.1
9
ilustra o
Diagrama de Blocos
de obtenção da velocidade,
da posição e da atitude
do veículo em relação ao referencial local, ou de navega
ção, numa plataforma estabilizada.
Já nos sistemas do tipo plataforma solidária ( strapdown ), a plataforma é fixada diretamente
no veículo, acompanhando o referencial do corpo.
Neste tipo de plataforma, os acelerômetros fornecem as componentes da força específica no
referencial do corpo, havendo, portanto a necessidade de transformá-las em componentes no
referencial local, que é o de navegação.
Os giroscópios enviam as velocidades de rotação do referencial do corpo em relação ao
inercial a um algoritmo de atitude, que, de posse da informação da velocidade de rotação do
referencial local em relação ao inercial, proveniente do algoritmo de navegação, pode calcular os
parâmetros de atitude do corpo e, por conseguinte, a Matriz de Transformação de Coordenadas
do Referencial do Corpo para o Local. As forças específicas geradas pelos acelerômetros no
ALGORITMO DE
NAVEGAÇÃ
O
SERVO
-
MECANISMO
GIROSCÓPIOS
ACELERÔ
METROS
ATITUDE
VELOC
IDADE
E
POSIÇÃO
Rotação do Referencial Local
em relação ao referencial
inercial
Plataforma
E
stabilizada
FIG. 2.19
-
Diagrama de Blocos de obtenção da velocidade,
da
posição em relação ao r
eferencial da
Terra
, numa plataforma
estabilizada
51
referencial do corpo podem, então, ser levadas ao referencial local e, assim, usadas no algoritmo
de navegação, que fornece a velocidade e a posição
em relação ao referencial da
Terra.
Pode-se concluir, portanto, que é exigido um aparato computacional bem maior neste tipo de
sistema, já que a tarefa do servo-mecanismo da plataforma estabilizada é substituída por cálculos
de transformação de coordenadas. Desta maneira, não seria equivocado dizer que a plataforma
solidária é uma plataforma estabilizada virtualmente .
A FIG. 2.20 apresenta o diagrama de blocos de uma plataforma solidária.
2.6. ALGORITMOS
D
E
ALINHAMENTO
, DE NAVEGAÇÃO E DE
ATITUDE
Os algoritmos de alinhamento, de navegação e de atitude implementados neste trabalho
tomaram por base uma plataforma do tipo solidária, com referencial local ou de navegação
na
configuração geográfica e orientaçã
o
do tipo NED, no hemisfério Sul.
FIG. 2.20
Diagrama de Blocos
de obtenção da velocidade e da posição em
relação ao referencial da
Terra
, numa plataforma solidária
ALGORITMO DE
NAVEGAÇÃO
ALGORITMO DE
ATITUDE
GIROSCÓPIOS
ACELERÔMETROS
ATITUDE
VELOCIDADE
E
POSIÇÃO
Rotação do Referencial
Local em relação ao
referencial inercial
Plataforma
Solidária
( Strapdown )
TRANSFORMAÇÃO
DE COORDENADAS
MATRIZ DE
ROTAÇÃO
52
2.6.1.
ALGORITMO DE ALINHAM
ENTO
Inicialmente, necessita-se fornecer ao Sistema de Navegação Inercial os dados de posição e
orientação iniciais da plataforma. A informação da posição inicial pode ser obtida através de
recursos topográficos ou outro meio, como pelo GPS ( Global Positioning System ).
A informação da orientação pode ser fornecida pelo próprio Sistema de Navegação Inercial,
usando os seus giroscópios e acelerômetros e tomando como referência o vetor gravidade e o
vetor
rotação da
Terra
, que são
conhecidos, no que diz respeito a direção, sentido e magnitude.
O vetor gravidade permite conhecer a orientação da Plataforma com relação à vertical local.
Este procedimento, que será detalhado adiante, é chamado de nivelamento.
T
omando
-se por base o vetor rotação da Terra, realiza-se o chamado alinhamento, que será
abordado à frente, obtendo a orientação da Plataforma em relação ao Norte Verdadeiro.
O nivelamento e o alinhamento, designados para efeito de simplificação neste traba
lho
simplesmente como alinhamento, são realizados em duas etapas, que se constituirão nas duas
partes do algoritmo de alinhamento concebido neste trabalho: o Alinhamento Grosseiro e o
Alinhamento Fino.
No Alinhamento Grosseiro, usam-se as informações fornecidas tão somente pelos
giroscópios e acelerômetros.
No Alinhamento Fino, considera-se, além das informações fornecidas pelos giroscópios e
acelerômetros, às de outros instrumentos de medida, de forma a, através de uma filtragem ótima
dos dados obtidos das diversas fontes, obter-se uma maior precisão quanto à orientação da
plataforma.
Neste trabalho,
são
utilizados dois giroscópios de medição de deslocamento angular e um
girocompasso, que é o sensor que mede o deslocamento angular em relação ao Norte Verdad
eiro,
como instrumentos de medida auxiliares para a realização da filtragem ótima. Estes sensores não
possuem, no entanto, dinâmica para acompanhar os movimentos do corpo na navegação. Embora
a precisão destes sensores seja da mesma ordem dos giroscópios da plataforma, obtém-se, na
filtragem, uma acurácia melhor do que no caso do uso dos sensores isoladamente.
No Alinhamento Fino, que será detalhado mais adiante, o equacionamento da filtragem
concebido neste trabalho permite, além do refinamento das informações de orientação da
53
plataforma, a estimação, e posterior compensação, de um tipo de erro, chamado de bias dia a dia
,
que será detalhado mais adiante e que ocorre em cada um dos giroscópios e dos acelerômetros.
2.6.2.
ALGORITMO DE
NAVEGAÇÃO
O algoritmo de nav
egação consiste basicamente em duas integrações. Na primeira, integra
-
se
a
aceleração do veículo, em componentes em relação ao referencial local, obtendo-se as
componentes da velocidade neste referencial. As mesmas são usadas nas equações diferenciais
que
definem a atualização no tempo d
as
variáveis
de posição, quais sejam, latitude, longitude e
altura. Estas equações, uma vez integradas, fornecem os valores de tais variáveis.
2.6.3.
ALGORITMO DE
ATITUDE
Como o algoritmo de navegação necessita das componentes da força específica em relação
ao referencial local e os giroscópios fornecem-na em relação ao referencial do corpo, necessita-
se, a cada instante, conhecer a Matriz de Transformação de Coordenadas do referencial do corpo
para o referencial local (
l
b
C ). Por conseguinte, necessita-se resolver, para cada instante de tempo,
a equação diferencial de atualização temporal da referida matriz. Aos procedimentos que
envolvem esta atualização dá
-
se o nome de algoritmo de atitude.
Neste trabalho, os algoritmos de navegação e atitude foram implementados em conjunto.
Estes algoritmos foram baseados nos trabalhos de SAVAGE, em 1985, e TITTERTON e
WESTON, em 1997.
54
3.
ALGORITMO DE NAVEGAÇ
ÃO E ATITUDE
3.1.
NAVEGAÇÃO
3.1.1.
PRINCÍPIO DE FUNCION
AMENTO DE UM ACELERÔ
METRO
O princípio de funcionamento de um acelerômetro é apresentado, entre outros, por
ALONZO, 1994.
Um acelerômetro é, basicamente, um transdutor que relaciona o movimento de uma massa
em relação a uma câmara instrumentada, através de uma restrição elástica, viscosa ou
eletromagnética.
Todos os acelerômetros operam segundo o mesmo princípio, qual seja, o de medir o
deslocamento relativo de uma pequena massa, chamada massa de prova ou massa sísmica, restrita
no interior de uma câmara submetida a uma aceleração.
A restrição
imposta
ao movimento relativo da massa em relação à câmara do acelerômetro é
captada por um transdutor que retorna um sinal proporcional a esse movimento.
A aceleração inercial da massa de prova do acelerômetro é igual e oposta à aceleração
in
ercial do corpo onde o acelerômetro está fixado e que, em última análise, é a grandeza de
interesse.
De acordo com a Segunda Lei de Newton, a aceleração inercial da massa (
i
a
) é proporcional
à soma das forças aplicadas à massa (
res
F
), neste caso, a força gravitacional (
W
) e a força
elástica (
F
). Pode
-se, portanto, escrever:
WFFam
res
i
.
(3.1)
A saída de um acelerômetro é um sinal elétrico que é proporcional à for
ça específica (
m
F
).
A FIG.
3.1 apresenta um esquema do princípio de funcionamento de um acelerômetro.
FIG. 3.1
-
Acelerômetro
x
F
x
W
W
x
Massa de prova (
m)
55
Na
FIG.
3.1, F é a força elástica a que a massa de prova é submetida;
x
F é a co
mponente
da força F no eixo longitudinal do acelerômetro (suposto como x); W é a força de atração da
Terra
sobre a massa do acelerômetro e
x
W é a componente da força W no eixo longitudinal do
acelerômetro (suposto como x).
3.1.2.
OBTENÇÃO DA FORÇA ES
PECÍFICA
A segunda lei de Newton aplicada ao acelerômetro pode ser expressa como se segue.
i
xx
x
res
amWFF
x
)(
(3.
2)
onde
x
F é a componente da força elástica a que a massa de prova é submetida no eixo
longitudinal do acelerômetro (suposto como x
);
x
W é a componente da força de atração da Terra
sobre a massa de teste no eixo longitudinal do acelerômetro (suposto como x); m é a massa de
teste e
i
x
a )( é a componente, no eixo longitudinal do veículo, da aceleração do veículo no
referencial inercial.
Pode-
se, portanto, escrever o seguinte:
x
x
xx
x
i
wf
m
W
m
F
a )(
onde a quantidade
x
f é a componente em x de f , que é denominada de Força Específica, e
x
w é a componente em x (eixo longitudinal do acelerômetro) de w, que é denominada de
aceleração gravitacional
.
O acelerômetro fornece, portanto, a componente
x
f da força específica. Dispondo-se da
componente da aceleração gravitacional, em relação ao mesmo eixo,
x
w ,
pode
-se obter a a
componente da aceleração inercial da massa, e portanto do corpo, em relação a esse mesmo eixo,
x
i
a .
Procedendo analogamente em relação aos eixos y e z, pode-se obter, da soma das três
componentes, o vetor
i
a
, que é a aceleração do corpo na qual se está interessado.
56
3.1.3. OBTENÇÃO DA POSIÇÃO DO VEÍCULO EM RELAÇÃO AO
REFERENCIAL DA
TERRA
ALONZO, em 1994, e TITTERTON e WESTON, em 1997, apresentam as equações
envolvidas na obtenção da posição do veículo em relação ao referencial da Terra.
Seja a rotação da
Terra
em relação ao referencial inercial constante e dada por
ie
.
Seja a rotação do referencial local em relação ao referencial da
Terra
dada por
el
.
A rotação do referencial local em relação ao referencial inercial é, portanto, dada pela
segu
inte expressão:
l
el
l
ie
l
il
(3.
3)
onde o sobrescrito l indica apenas que as componentes dos vetores dizem respeito ao
referencial local.
A velocidade de um veículo em relação à superfície da Terra, que é, na verdade, o vetor
velocidade usual e
de interesse, pode ser expressa da seguinte maneira:
r
dt
rd
dt
rd
v
ie
ie
e
(3.
4)
onde:
e
dt
rd
é o vetor variação no tempo, em relação ao referencial da Terra, do vetor posição do
veículo, ou seja, é o vetor velocidade relativa à
Terra
;
i
dt
rd
é vetor variação no tempo, em relação ao referencial inercial, do vetor posição do
veículo, em relação ao centro da
Terra
, ou seja, é o vetor velocidade absoluta;
ie
é a velocidade de rotação da
Terra
em relaçã
o ao referencial inercial;
r é o vetor posição do veículo, em relação à origem do referencial da Terra.
Como
ie
é constante no tempo, diferenciando-
se a EQ. 3.
4
em relação ao tempo, tem
-
se:
i
ie
e
i
e
dt
rd
dt
rd
dt
vd
2
2
(3.
5)
57
Como, da EQ. 3.
4, rv
dt
rd
ie
e
i
, substituindo
-
se
na EQ. 3.
5
, tem
-
se:
rv
dt
rd
dt
vd
ie
e
ie
i
i
e
2
2
(3.
6)
As componentes de
e
v em relação ao referencial local são de maior interesse, uma vez que
definem componentes da velocidade nos planos horizontal e vertical em relação à superfície da
Terra
. Por conseguinte, seria mais conveniente se a equação diferencial para
e
v estivesse
expressa em relação ao referencial local.
Como:
e
ie
l
e
i
e
v
dt
vd
dt
vd
(3.
7)
o
nde:
i
e
dt
vd
é a variação no tempo, em relação ao referencial inercial, do vetor velocidade do
corpo em relação ao referencial da
Terra;
l
e
dt
vd
é a variação no tempo, em relação ao referencial local, do vetor velocidade do
corpo
em relação ao referencial da
Terra
.
Substi
tuindo
-
se
a EQ.
3.7 na
EQ. 3.6
, tem
-
se:
e
il
ie
e
ie
i
l
e
vrv
dt
rd
dt
vd
)(
2
2
(3.
8)
Como
wf
dt
rd
a
i
i
2
2
, da
EQ. 3.
8
tem
-
se:
)()( rwvf
dt
vd
ieie
e
ie
il
l
e
(3.
9)
Pode-se integrar duas vezes a equação, de modo a se obter, sucessivamente, a velocidade e a
posição do veículo em relação ao referencial da
Terra
, no referencial local.
Os dois últimos termos da equação são funções apenas da posição e são agrupados,
recebendo a designação de gravidade.
58
)( rwg
ieie
(3.1
0)
Desta forma, a
EQ. 3.9
pode ser escrita da seguinte forma:
gvf
dt
vd
e
ie
il
l
e
)(
(3.
1
1)
Como
elie
il
, p
ode
-
se ainda reescrever a EQ. 3.11
da seguinte maneira:
gvf
dt
vd
e
elie
l
e
)2(
(3.1
2)
Uma vez que se trabalha com as componentes dos vetores no referencial local e os
acelerômetros fornecem a força específica no referencial do corpo, faz-
se o seguinte:
gvfC
dt
vd
e
elie
b
l
b
l
e
)2(.
(3.1
3)
A
aceleração da gravidade é a aceleração que decorre da gravitação ou aceleração
gravitacional
e
da aceleração centrífuga.
Gravidade é definida como a força requerida para manter uma massa de teste com a
velocidade vertical constante
ou nula, em relação à Terra
.
É interessante notar que o vetor g é localmente perpendicular à superfície da Terra. Esse
fato parece resultar do provável processo de formação da
Terra
, como decorrente de um
resfriamento de uma massa fluida em rotação. Nesse processo, a superfície da
Terra
teria
assumido um formato equipotencial em relação à gravidade, de maneira a não haver tensões
horizontais na superfície. Conseqüentemente, a superfície da
Terra
teria ficado perpendicular ao
vetor
g em qualquer lugar do planeta. Assim, para fins de navegação, a superfície da Terra pode
ser represe
ntada por um elipsóide de revolução sobre o eixo de rotação da
Terra
.
A Gravitação é a força de atração entre os corpos, que
é
diretamente
proporcional às massas
dos mesmos e inversamente proporcional à distância entre eles. Ao se dividir a gravitação pela
massa do corpo de interesse, obtém-se a aceleração gravitacional deste corpo:
R
R
GM
R
R
R
GMm
mm
W
w
32
.
1
(3.14)
59
Pode-se, então, sistematizar o seguinte algoritmo para obtenção do vetor posição do veículo,
em relação ao referencial d
a
Terra
, no referencial local:
1)
Adicionar a
aceleração gravitacional
,
w,
à força específica;
2)
Remover a aceleração centrífuga, devida ao desalinhamento do veículo em
relação ao centro da
Terra
;
3)
Remover a aceleração de coriolis, devida ao movimento do veículo sobre a
superfície da
Terra
;
4)
Integrar duas vezes, incorporando as condições iniciais.
FIG. 3.3
Diagrama de blocos d
e obtenção da velocidade e da
posição em relação ao referencial da
Terra
w
Círculo de
latitude constante
Equador
)( rwg
ieie
g
R
-
ie
x (
ie
x
r)
ie
dt
._
dt
._
_).2(
elie
_)
(
ieie
3
(_)
.MG
(Provenient
e dos
acelerômetros)
f
i
a
0
e
v
e
v
0
r
r
(
centrífuga
)
(gravitacional,
w
)
(coriolis)
-
-
+
+
+
+
FIG. 3.2
aceleração gravitacional, w, e aceleração da gravidade, g
60
3.1.4.
EQUAÇÕES DE NAVEGAÇÃ
O
A expressão da aceleração, obtida no item anterior, equação (3.1
2)
, é a seguinte:
gvf
dt
vd
e
elie
l
e
)2(
Com o referencial local na configuração geográfica e orientação NED, os vetores das
equações acima, no hemisfério sul, podem ser expressos como se segue:
Tl
T
N
E
N
N
E
E
l
el
Tl
ie
T
EN
l
T
DEN
l
e
gg
hR
Lv
hR
v
hR
v
L
sen
L
hxxr
vvvv
00
)
tan(
.
)(.0)
cos(
.
(3.1
5)
Pode-
se substituir as expressões 3.1
5
na
EQ. 3.12
, obtendo
-
se as seguintes equações:
gv
hR
v
hR
vLfv
vv
hR
L
vv
hR
vLvL
sen
fv
vv
hR
v
hR
L
vL
sen
fv
E
E
N
N
EDD
EN
E
DE
E
DNEE
DN
N
E
N
ENN
22
2
.
1
.
1
).
cos(
..2
..
)
tan(
..
1
).
cos(
..2
).
(..2
..
1
.
)
tan(
).
(..2
(3.1
6)
Evidentemente
, o algoritmo de navegação pode ser concebido tanto resolvendo as equações
de navegação na forma
da EQ. 3.12
quanto na forma
da EQ. 3.16
.
3.2.
ATITUDE
3.2.1.
CONSIDERAÇÕES BÁSICA
S
Em
sistemas de navegação inercial dotados de plataformas do tipo solidária ou strapdown ,
faz
-se necessária a transformação das medidas das forças específicas, fornecidas pelos
acelerômetros e referenciadas em relação ao sistema de coordenadas do corpo, para o referencial
local, a fim de que possam ser usadas nas equações de navegação. Essa transformação é feita
através de um algoritmo computacional que consiste, basicamente, na integração da equação
61
diferencial de atualização no tempo da matriz de transformação de coordenadas do sistema do
corpo para o local. Assim, pode-se dizer que o algoritmo de atitude fornece, a cada instante, a
Matriz de Transformação de Coordenadas,
l
b
C
, permitindo, desta forma, obter-se tanto as
componentes da força específica no referencial local, como também os parâmetros de atitude do
veículo em relação ao mesmo referencial, que, para este trabalho, são os ângulos de Euler.
Para a resolução da equação de atualização temporal da atitude, como será visto adiante (
EQ.
3.33
), necessita-se da velocidade angular do referencial do corpo em relação ao inercial, em
componentes no referencial do corpo, que é fornecida pelos giroscópios, e da velocidade angular
do referencial local em relação ao inercial, em componentes no referencial local (passo 5.2 do
algoritmo de navegaçã
o, que será apresentado adiante).
As operações realizadas na transformação das coordenadas são apresentadas por IORIO, em
1995, e podem ser visualizadas
na
FIG. 3.4
.
No diagrama apresentado na
FIG. 3.
4,
tem
-
se que:
b
ib
é a velocidade angular do referencial do corpo em relação ao referencial inercial,
projetada no referencial do corpo. Essas componentes são fornecidas pelos
giroscópios
;
Transformação de
coordenadas
Rotina de
Integração da
Atitude
Extração dos
ângulos de Euler
Forças Específicas nas
coordenadas do corpo
(medidas dos
acelerômetros)
Forças Específicas nas
coordenadas locais
b
ib
(medidas dos
giroscópios
)
Atitude do corpo
( roll , pitch e heading )
l
b
C
FIG. 3.4
Diagrama das Opera
ções Realizadas para a
Transformação de Coordenadas
l
il
(a partir da res
olução das
equações de navegação)
62
l
il
é a velocidade angular do referencial local em relação ao referencial inercial, projetada
no referencial local. Essas componentes são obtidas a partir da resolução das equações de
navegação
, como será mostrado mais adiante, na apresentação do algoritmo de navegação (
Passo
5.2 do
item 3.3)
.
3.2.2.
EQUAÇÃO
DE ATUALIZAÇÃO DA MATRIZ DE TRANSFORMAÇÃO DE
COORDENADAS DO REFERENCIAL DO CORPO PARA O LOCAL
(EQUAÇÃO DE ATITUDE)
Como a plataforma inercial adotada neste trabalho é do tipo solidária ou strapdown , os
acelerômetros irão fornecer as componentes da força específica em relação ao referencial do
corpo. Desta forma, faz-se mister a obtenção da Matriz de Transformação de Coordenadas do
referencial do Corpo para o Local.
Inicialmente, será abordado o conceito de velocidade angular de um referencial em relação a
outro.
Partir
-
se
-á da equação (2.1), que apresenta a relação entre os vetores posição de um ponto P,
em relação a dois sistemas de coordenadas
,
e que será reescrita abaixo.
r
P
=
r
+ C.
s
P
(3.17
)
onde:
r
é o vetor das coordenadas da origem do referenci
al
x -y -z
no referencial
x-y-z;
s
P
é o vetor das coordenadas de P no referencial
x -y -z
; e
C é a Matriz de Transformação de Coordenadas do referencial x -y -z
para o referencial x-y-
z.
Derivando em relação ao tempo a
EQ. 3.17
tem
-
se:
(3.18
)
Como
s
P
= C
T
. s
P
(3.19
)
onde:
s
P
é o vetor das coordenadas de P no referencial
x-y-z
; e
r
P
= r
+ C.
s
P
.
.
.
63
C
T
, que é igual a C
-1
, é a Matriz de Transformação de Coordenadas do referencial x-y-z para
o referencial x -y -z .
substituindo
-se a EQ. 3.
19 na EQ. 3.19
, tem
-
se:
(3.20
)
Neste ponto, é útil derivar uma identidade que envolve a Matriz de Transformação de
Coordenadas C. Derivando em relação ao tempo ambos os lados de C.C
T
= I, tem
-
se:
(3.21
)
Portanto:
(3.22
)
Da
EQ. 3.22, pode-se concluir que
T
.C
C
é uma matriz anti-simétrica e, dessa forma, existe
um vetor
, chamado velocidade angular do referencial
x -y -z
em relação ao referencial
x-y-z
, tal
que:
(3.23
)
onde é o vetor
na forma de matriz anti
-
simétrica.
Pós-
multiplicando
a EQ. 3.23
por C tem
-
se:
(3.24
)
Passando
-se à notação utilizada neste trabalho, pode-se reescrever a EQ. 3.24, considerando
as componentes em relação ao referencial local, da seguinte forma:
l
b
l
lb
l
b
CC
).
(
(3.25
)
onde:
l
b
C
é a derivada em relação ao tempo da matriz de rotação do referencial do corpo para o
referencial local;
)(
l
lb
é matriz anti-simétrica correspondente ao vetor velocidade angular do referencial do
corpo em relação ao referencial local com componentes expressas em relação ao referencial local
l
lb
.
No entanto, como as medições dos giroscópios são em relação ao referencial do corpo, pode-
se reescrever
a EQ. 3.25
, pré
-
multiplicando por
1
l
b
C
, obtendo
-
se a seguinte expressão:
l
b
l
lb
l
b
l
b
l
b
CCCC
)..(
.
11
(3.26
)
r
P
=
r
+ C.C
T
.s
P
.
.
.
C.C
T
+ C.C
T
=
0
.
.
(C.C
T
)
T
= C.C
T
=
-
C.C
T
.
.
.
(
x)
.
( x).C
=
C
(
x)
=
C.C
T
.
64
Como
)(
)..(
1
b
lb
l
b
l
lb
l
b
CC
, tem
-
se:
)(.
1
b
lb
l
b
l
b
CC
(3.27
)
Pré
-
multiplicando
-
se
a
EQ. 3.27
por
l
b
C
, tem
-
se:
)
.(
b
lb
l
b
l
b
CC
(3.2
8)
A velocidade angular do referencial do corpo em relação ao referencial local é igual à
velocidade angular do referencial do corpo em relação ao referencial inercial menos a velocidade
angular do referencial local em relação ao referencial inercial. Em notação vetorial e
considerando
-
se as componentes em relação ao referencial do corpo, pode
-
se escrever:
b
ll
b
ib
b
lb
(3.2
9)
A expressão também é verdadeira com os vetores tomados na forma de matrizes anti-
simétricas, ou seja:
)()()(
b
ll
b
ib
b
lb
(3.30
)
Portanto, substituindo
-
se
a EQ. 3.30 na EQ. 3.28
, tem
-
se:
)(.)(.
b
il
l
b
b
ib
l
b
l
b
CCC
(3.31
)
Sabe
-s
e que:
l
b
l
il
l
b
b
il
CC
).
(.)(
1
(3.32
)
Onde
)(
l
il
é a forma anti-simétrica do vetor velocidade angular do referencial local em
relação ao referencial inercial, projetada no referencial local. Como anteriormente mencionado,
seu valor é obtido a partir das equações de navegação, como será visto mais adiante, quando da
apresentação do algoritmo de navegação
(
Passo 5.2 do
item 3.3).
Assim, substituindo
-
se
a EQ. 3.32 em 3.31
, tem
-
se:
l
b
l
il
l
b
l
b
b
ib
l
b
l
b
CCCCC
)..(
.)
.(
1
Obtém
-
se, portanto:
l
b
l
il
b
ib
l
b
l
b
CCC
).
()
.(
(3.33
)
A
EQ. 3.33 constitui-se, portanto, na equação de atualização no tempo da matriz de
transformação de coordenadas do referencial do corpo para o local, também chamada equação de
atitude.
65
3.2.3.
MODELO DO FORMATO DA
TERRA
A fim de gerar a posição de um corpo em relação à
Terra
, a partir das medidas fornecidas
pelos sensores inerciais, isto é, giroscópios e acelerômetros, faz-se necessário definir um modelo
para o formato da
Terra.
Para navegação com um grau mais elevado de exatidão, o modelo esférico não se mostra
suficientemente representativo.
Devido ao achatamento da Terra nos pólos, a Terra é geralmente modelada como um
elipsóide, que se aproxima bastante da verdadeira geometria.
Vários modelos para este elipsóide que simula o formato da Terra foram propostos, sendo
que o mais usual, e que foi adotado neste trabalho, o definido pelo World Geodetic System
Commitee, em 1984, o chamado WGS-84 system. A
FIG.
3.6 representa, de forma esquemática,
o elipsóide de referência para modelo do formato da
Terra
.
Elipsóide de
Referência
Superfície da
Terra
(fora de escala)
r
R
Eixo
Polar
FIG. 3.5
Elipsóide de Referência
66
De acordo com o referido modelo, os seguintes parâmetros são definidos:
- R,
comprimento do semi
-
eixo maior:
6378137,0 m;
-
r = R.(1
f),
comprimento do semi
-
eixo menor:
6356752,3142 m;
-
f = (R
r)/R,
acha
tamento do elipsóide:
1/298,257223563;
-
e = [f.(2
f)]
1/2
, excentricidade maior do elipsóide:
0,0818191908426;
- ,
velocidade de rotação da
Terra
:
7,292115 x 10
-5
rad/s
(15,041067º/hora).
Segundo o modelo da
Terra
de acordo com o elipsóide de referência do WGS-84, têm-se as
seguintes expressões para o raio de curvatura meridiano (
R
N
) e para o raio de curvatura
transverso
(R
E
):
2
3
22
2
)]
(.1[
)1
.(
L
sen
e
eR
R
N
(3.34
)
2
1
22
)]
(.1[ L
sen
e
R
R
E
(3.35
)
Usando-se os parâmetros acima, tem-se, para a velocidade angular do referencial local em
relação ao referencial da Terra, a chamada transporte rate , com componentes em relação ao
referencial local, a seguinte forma:
hR
Lv
hR
v
hR
v
E
E
N
N
E
E
l
el
)
tan(
.
(3.36
)
Para a variação no tempo da latitude e da longitude, as expressões são as que se seguem:
hR
v
L
N
N
(3.37)
hR
Lv
E
E
)
sec(
.
(3.38)
67
3.2.4.
MODELO DA ATRAÇÃO GR
AVITAC
IONAL DA
TERRA
Como os acelerômetros fornecem, conforme visto anteriormente, a força específica, para que
se possa obter o valor da aceleração, é necessário conhecer-se o valor da magnitude do vetor
gravidade, g. Assim, a exatidão do valor da aceleração depende, além de acelerômetros precisos,
do valor de g usado.
A gravidade varia com a latitude, devido à aceleração centrífuga, e com a altura, devido à
distância entre os centro de massas do corpo e da
Terra
.
Neste trabalho o vetor gravidade será considerado na direção da vertical local, ou seja
perpendicular ao elipsóide de referência, o que, apesar de, em função da não homogeneidade da
distribuição de massa da Terra e das diferenças entre a superfície da
Terra
a o elipsóide de
referência, não corresponder
com exatidão à realidade, fornece uma boa aproximação.
Vários modelos para a variação da gravidade com a latitude e com a altura têm sido
propostos na literatura. O modelo
é apresentado por
T
ITTERTON e WESTON, em 1997
:
2
0
2623
1
)]
2(.10.9,5)(.10.3024,51
.[780318
,9
R
h
L
sen
L
sen
g
(3.39
)
onde:
NE
RRR .
0
3.3. ALGORITMO DE NAVEGAÇ
ÃO
Com vistas à implementação computacional, segue uma proposta de
um
algoritmo de
navegação
para um Sistema de Navegação Inercial do tipo solidário, com referencial local ou de
navegação
na configuração geográfica e orientação do
tipo NED, e veículo no hemisfério Sul.
1º Passo
: Condições iniciais de posição e velocidade;
L,
, h, v
N
, v
E
e v
D
2º Passo
: Cálculo das condições iniciais de R
E
e
R
N
, usando a
EQ. 3.34 e a EQ. 3.35
;
68
3º Passo: Cálculo das componentes no referencial local da velocidade angular do
referencial local em relação ao referencial da
Terra, a transport rate , usando as expressões 3.36:
hR
Lv
hR
v
hR
v
N
E
N
N
E
E
l
el
)
tan(
.
4º Passo
:
Cálculo da Matriz de Rotação do Referencial do Corpo para o Local (
l
b
C
),
usando a transposta da matriz da
EQ. 2.1
4:
coscoscos
coscoscoscoscos
coscoscoscoscoscos
333231
232221
131211
sensen
sensensensensensensen
sensensensensensen
ccc
ccc
ccc
C
l
b
5º Passo
:
Loop
(início do Loop )
Passo 5.1: Cálculo das velocidades angulares do referencial da Terra em relação ao
referencial inercial e do referencial local em relação ao referencial da Terra, com componentes
em relação ao referencial local, ou seja,
l
ie
e
l
el
, respectivamen
te.
Passo 5.2: Cálculo da Matriz anti-simétrica correspondente ao vetor velocidade
angular do referencial local em relação ao referencial inercial:
)()()(
l
il
l
ie
l
il
Passo 5.3
: Cálculo da gravidade, usando a
EQ. 3.39
e expressão do vetor no re
ferencial
local:
2
0
2623
1
)]
2(.10.9,5)(.10.3024,51
.[780318
,9
R
h
L
sen
L
sen
g
e
g
g 0
0
Passo 5.4
: Leitura dos acelerômetros (no referencial do corpo):
z
y
x
b
f
f
f
f
69
Passo 5.5
: Cálculo das componentes da força específica no referencial local:
b
l
b
l
fCf .
Passo 5.6
: Integração da equação diferencial da velocidade, usando a
EQ
3.12:
gvfv
e
el
il
e
)2(
Passo 5.
7
:
Cálculo da altura:
dt
vthtth
tt
t
N
.)()(
Passo 5.8
: Cálculo da Latitude, a partir da
EQ. 3.37
:
dt
hR
v
tLttL
tt
t
N
N
.)()(
Passo 5.9
: Cálculo da Longitude, a partir da
EQ. 3.38
:
dt
hR
Lv
ttt
tt
t
E
E
.
)
sec(
.
)()(
Passo 5.10: Atualização das componentes da velocidade angular do referencial local
em relação ao da
Terra
, a transport rate , usando as expressões 3.36;
hR
Lv
hR
v
hR
v
N
E
N
N
E
E
l
el
)
tan(
.
Passo 5.11
: Leitura dos Gi
roscópios (no referencial do corpo):
x
b
ib
x
b
ib
x
b
ib
b
ib
)(
)(
)(
Passo 5.12: Integração da equação diferencial de atualização no tempo de
l
b
C , isto é,
da
EQ. 3.33
:
tt
t
l
b
l
il
b
ib
l
b
l
b
l
b
dt
CCtCttC
].
).
()
.(
[)()(
Passo 5.13
: Ortogonalização da Matriz
l
b
C
, usando o seguinte procedimento:
l
b
Tl
b
l
b
l
b
CCCIIC .])
.(
.[
5,0
70
(Fim do Loop ).
Nas integrações dos passos 5.6 e 5.12 foi usado o método Preditor-
Corretor,
conforme
DIEGUEZ, em 1992,
que será apresentado suscintamente na seqüência.
Seja a Equação Diferencial Ordinária
dx
dy
yxfy ),(
*
, onde )(xfy . A rotina para a
integração de y , a cada variação x da variável independente x, pelo Método Preditor-
Corretor,
usando Runge
-
Kutta de 4ª ordem é a seguinte:
F0= f
*
(x,y)
Para I de 1 até N
K1=
x.f
*
(x,y)
K2=
x.f
*
(x
+( x
)/2
,y
+(K1)/2
)
K3=
x.f
*
(x
+( x
)/2
,y
+(K2)/2
)
K4=
x.f
*
(x
+
x,y
+K3
)
y=y+(K1+2.(K2+K3)+K4)/6
x=x+
x
F(I)=f
*
(x,y)
Se I=1, então F1=F(1)
Se I=2, então F2=F(2)
Se I=3, então F3=F(3)
Se I > 3, então:
yP=y
+
x.[55.F3
-
59.F2+37.F1
-
9.F0]/24
(Preditor)
FP= f
*
(x,yP)
y=y+
x.[9.FP+ 19.F3
-
5.F2+F1]/24
(Corretor)
F0=F1; F1=F2; F2=F3; F3=f
*
(x,y)
Fim (Para)
3.4.
VALIDAÇÃO DO ALGORIT
MO DE NAVEGAÇÃO
3.4.1.
TRAJETÓRIA PROPOSTA
A trajetória proposta é a s
eguinte:
71
Conforme a
FIG. 3.7
, o veículo parte da cidade d
o Rio de Janeiro
, do nível do mar, ou seja:
Latitude: 22,83º Sul;
Longitude: 43,18
º Oeste;
Altura: 0 m.
O veículo parte com velocidade inicial nula e, durante 1 s, é submetido a uma aceleração de
100 m/s
2
. Em seguida, numa primeira etapa, se desloca em direção ao Norte verdadeiro, com
aceleração constante de 60 m/s
2
, durante 4 s. Numa segunda etapa, desloca-se, ainda em direção
ao Norte Verdadeiro, com velocidade constante durante 4 s. Em seguida, na terceira etapa, sofre
uma inclinação em pitch , sempre com uma velocidade constante, de forma que, após 6 s, o
veículo tenha descrito um arco de /3 rad (60º), com um raio de 1948,06 m. Segue-se a quarta
etapa, na qual o veículo prossegue com essa inclinação por mais 4 s, com velocidade constante.
Em seguida, na quinta etapa, o veículo sofre uma inclinação, de forma a que o ângulo pitch
retorne a zero, novamente descrevendo um arco de /3 rad (60º), com um raio de 1948,06 m,
desta feita no sentido contrário. Seguem-se mais 4 s com velocidade constante. É a sexta etapa.
Na sétima etapa, o veículo sofre uma inclinação em heading , descrevendo um arco de 2
rad
(360º), também com um raio de 1948,06 m, voltando, em seguida, o veículo a alinhar-
se
novament
e com o norte. A partir daí, o veículo permanece com velocidade constante até
completar 100 s de navegação.
Esta trajetória foi escolhida não baseada em alguma que seja característica de algum veículo
específico, mas tendo em vista a sua representatividade em termos de manobras por parte do
corpo móvel.
Outro fator determinante para esta escolha foi o fato de, a menos de uma altura inicial de
2000 m ao invés de 0 m, a mesma ter sido utilizada numa das simulações levadas a efeito por
IORIO, em 1995,
de sort
e que os resultados puderam ser comparados.
A trajetória proposta é ilustrada na FIG. 3.6.
72
3.4.2.
SIMULAÇÃO DAS MEDIDA
S DOS ACELERÔMETROS
A simulação das medidas dos acel
erômetros seguem o apresentado por IORIO, em 1995.
Far
-
se
-á, nas EQ. 3.16, as componentes da força específica anularem os demais termos de
suas respectivas equações, quando a velocidade for constante. Nos períodos em que houver
alguma componente de aceleração, a força específica correspondente deverá tanto anular as
demais componentes como também fornecer a aceleração necessária. Na terceira etapa, em que o
veículo realiza uma manobra ascendente, de forma a que seu ângulo pitch chegue a 60º (
/3
rad), sub
mete
-se o veículo a uma aceleração centrípeta tal que, em 6 segundos, faça o veículo
FIG. 3.6
Representação esquemática da trajetória do veículo
Norte
Leste
Down
1ª Etapa
2
ª Etapa
3
ª Etapa
4ª Etapa
5
ª Etapa
6
ª Etapa
7
ª Etapa
Veículo com Aceleração Constante de 60 m/s
2
, durante 4 s
Veículo com Velocidade Constante de 340 m/s, durante 4 s
Veículo
parte com velocidade inicial nula e é submetido,
durante 1 s, a uma aceleração de 100 m/s
2
V
eículo realiza manobra ascendente de pitch até 60º com Velocidade
Constante de 340 m/s, durante 6 s
Veículo com Velocidade Constante de 340 m/s, durante
4 s
Veículo realiza manobra descendente de pitch até 60º com Velocidade
Constante de 340 m/s, durante 6 s
Veículo realiza manobra de 360º em
heading com Velocidade
Constante de 340 m/s, durante
36 s
8
ª Etapa
Veículo com Velocidade Constante
de 340 m/s, durante 4 s
Veículo com Velocidade Constante
de 340 m/s, durante 36 s
0 s
5
s
9
s
15
s
19
s
25
s
29
s
65
s
100
s
Rio de
J
aneiro
1
s
73
descrever um arco de 60º, o que implica na subtração algébrica das componentes desta aceleração
nas expressões das forças específicas Norte e
down
, conforme descrito na
FI
G. 3.
7.
Raciocínio análogo é empregado nas manobras das quinta e sétima etapas.
Desta forma:
No primeiro segundo da trajetória, quando o veículo é submetido a uma aceleração de 100
m/s
2
, tem-
se:
gv
h
v
h
Rn
vLf
vv
h
Rn
L
vv
h
vLvL
sen
f
vv
h
Rn
v
h
Rn
L
vL
sen
f
ENED
ENDEDNE
DNEEN
22
2
.
Re
1
.
1
).
cos(
..2
..
)
tan(
..
Re
1
).
cos(
..2
).
(..2
100
..
1
.
)
tan(
).
(..2
Para
a primeira etapa da trajetória, na qual o veículo tem aceleração constante de 60 m/s
2
,
durante 4 s, tem
-
se:
gv
h
v
h
Rn
vLf
vv
h
Rn
L
vv
h
vLvL
sen
f
vv
h
Rn
v
h
Rn
L
vL
sen
f
ENED
ENDEDNE
DNEEN
22
2
.
Re
1
.
1
).
cos(
..2
..
)
tan(
..
Re
1
).
cos(
..2
).
(..2
60
..
1
.
)
tan(
).
(..2
Para a segunda etapa da trajetória, na qual o veículo tem velocidade constante, durante 4 s,
tem
-
se:
Norte
Down
Componente Norte:
-a
c
.sen
Componente
Down:
-a
c
.cos
Aceleração Centrípeta:
a
c
FIG. 3.7
Representação esquemática da aceleração centrípeta na manobra ascendente
da terceira etapa
74
gv
h
v
h
Rn
vLf
vv
h
Rn
L
vv
h
vLvL
sen
f
vv
h
Rn
v
h
Rn
L
vL
sen
f
ENED
ENDEDNE
DNEEN
22
2
.
Re
1
.
1
).
cos(
..2
..
)
tan(
..
Re
1
).
cos(
..2
).
(..2
..
1
.
)
tan(
).
(..2
Para a terceira etapa da trajetória, na qual o veículo realiza uma manobra ascendente até o
ângulo de pitch de 60º (
/3), durante 6 s, tem
-
se:
1
1
2
1
22
1
1
2
1
2
cos
..
Re
1
.
1
).
cos(
..2
..
)
tan(
..
Re
1
).
cos(
..2
).
(..2
...
1
.
)
tan(
).
(..2
R
v
gv
h
v
h
Rn
vLf
vv
h
Rn
L
vv
h
vLvL
sen
f
sen
R
v
vv
h
Rn
v
h
Rn
L
vL
sen
f
res
ENED
ENDEDNE
res
DNEEN
onde:
t
R
v
vvv
res
DN
res
.
1
1
1
22
1
Para um
t
max
= 6 s e um
max
=
/3, tem
-
se:
mR 06,19486.
3
340
1
Para a quarta etapa da trajetória, na qual o veículo tem velocidade constante, durante 4 s,
tem
-
se:
gv
h
v
h
Rn
vLf
vv
h
Rn
L
vv
h
vLvL
sen
f
vv
h
Rn
v
h
Rn
L
vL
sen
f
ENED
ENDEDNE
DNEEN
22
2
.
Re
1
.
1
).
cos(
..2
..
)
tan(
..
Re
1
).
cos(
..2
).
(..2
..
1
.
)
tan(
).
(..2
Para a quinta etapa da trajetória, na qual o veículo realiza uma manobra descendente, na qual
o ângulo de pitch passa de 60º (
/3) a 0º, em 6 s, tem
-
se:
75
2
2
2
2
22
2
2
2
2
2
cos
..
Re
1
.
1
).
cos(
..2
..
)
tan(
..
Re
1
).
cos(
..2
).
(..2
...
1
.
)
tan(
).
(..2
R
v
gv
h
v
h
Rn
vLf
vv
h
Rn
L
vv
h
vLvL
sen
f
sen
R
v
vv
h
Rn
v
h
Rn
L
vL
sen
f
res
ENED
ENDEDNE
res
DNEEN
onde:
t
R
v
vvv
res
DN
res
.
1
1
2
22
2
Para um
t
max
= 6 s e um
max
=
/3, tem
-
se:
mR 06,19486.
3
340
2
Para a sexta etapa da trajetória, na qual o veículo tem velocidade constante, durante 4 s, tem-
se:
gv
h
v
h
Rn
vLf
vv
h
Rn
L
vv
h
vLvL
sen
f
vv
h
Rn
v
h
Rn
L
vL
sen
f
ENED
ENDEDNE
DNEEN
22
2
.
Re
1
.
1
).
cos(
..2
..
)
tan(
..
Re
1
).
cos(
..2
).
(..2
..
1
.
)
tan(
).
(..2
Para a sétima etapa da trajetória, na qual o veículo realiza uma manobra circular, de forma a
que o ângulo heading vai de 0º a 360º (2
), em 36 s, tem
-
se:
gv
h
v
h
Rn
vLf
R
v
vv
h
Rn
L
vv
h
vLvL
sen
f
sen
R
v
vv
h
Rn
v
h
Rn
L
vL
sen
f
ENED
res
ENDEDNE
res
DNEEN
22
3
3
2
3
3
3
2
3
2
.
Re
1
.
1
).
cos(
..2
cos
...
)
tan(
..
Re
1
).
cos(
..2
).
(..2
...
1
.
)
tan(
).
(..2
onde:
76
t
R
v
vvv
res
DN
res
.
3
3
3
22
3
Para um
t
max
= 36 s e um
max
= 2
, tem
-
se:
mR
06
,
194836
.
2
340
3
Para a oitava etapa da trajetória, na qual o veículo tem velocidade constante, durante 36 s,
tem
-
se:
gv
h
v
h
Rn
vLf
vv
h
Rn
L
vv
h
vLvL
sen
f
vv
h
Rn
v
h
Rn
L
vL
sen
f
ENED
ENDEDNE
DNEEN
22
2
.
Re
1
.
1
).
cos(
..2
..
)
tan(
..
Re
1
).
cos(
..2
).
(..2
..
1
.
)
tan(
).
(..2
A simulação dos sinais de tais sensores é obtida multiplicando o vetor força específica no
Referencial Local pela Matriz de Rotação do Referencial Local para o Referencial do Corpo
(
b
l
C
), ou seja:
D
E
N
b
l
z
y
x
f
f
f
C
f
f
f
3.4.3.
SIMULAÇÃO DAS MEDIDA
S DOS GIROSCÓPIOS
A simulação das medidas dos giroscópios seguem o apresentado por IORIO, em 1995.
Tem
-
se que:
l
lb
l
el
l
ie
l
ib
No cálculo da velocidade angular do Corpo em relação ao Referencial Inercial (
l
ib
), que é a
grandeza que é medida pelos giroscópios, durante grande parte do percurso, o corpo não sofre
mudança de orientação, ou seja, a velocidade angular do referencial do corpo em relação ao local
(
l
lb
) é nula, e, portanto,
l
ib
é obtida pelas somas das velocidades angulares do Referencial
77
Local em relação ao Referencial da
Terra
, (
l
el
), e do Referencial da Terra para o Inercial, (
l
ie
).
As
expressões da mesma se encontram na
EQ. 3.15
. Apenas nas manobras que implicam em
movimentos do corpo em relação ao Referencial Local, ou seja, no movimento ascendente de até
o ângulo de pitch ( ) de 60º, no movimento descendente, com o mesmo ângulo variando de 60º
a 0º e no movimento em heading ( ) de 360º, a velocidade angular do corpo em relação ao
Referencial Local (
l
lb
) é diferente de zero. Em tais casos a mesma pode ser facilmente obtida,
uma vez que, em cada um dos referidos movimentos, só ocorre uma rotação simples em um dos
eixos. Assim, basta fazer-se a componente de
l
lb
correspondente ao eixo sobre o qual ocorre a
rotação igual à velocidade do veículo dividida pelo raio da manobra.
Assim:
Para as primeira, segunda, quarta, sexta e oitava etapas da trajetória, nas quais o Referencial
do Corpo permanece alinhado com o Referencial Local, tem
-
se:
0
0
0
tan
.
Re
)(.
0
)
cos(
.
0
0
0
)(.
0
)
cos(
.
h
Rn
v
h
Rn
v
h
v
L
sen
L
L
sen
L
E
N
E
D
l
el
E
l
el
N
l
el
l
ib
Para a terceira etapa da trajetória, na qual o veículo realiza uma manobra ascendente até o
ângulo de pitch de 60º (
/3), durante 6 s, tem
-
se:
0
0
)
tan(
.
Re
)(.
0
)
cos(
.
0
0
)(.
0
)
cos(
.
1
1
1
1
R
v
h
Rn
Lv
h
Rn
v
h
v
L
sen
L
R
v
L
sen
L
res
E
N
E
res
D
l
el
E
l
el
N
l
el
l
ib
onde:
22
1 DN
res
vvv
e
mR
06
,
1948
1
Para a quinta etapa da trajetória, na qual o veículo realiza uma manobra descendente, na qual
o ângulo de pitch passa de 60º (
/3) a 0º, em 6 s, tem
-
se:
78
0
0
)
tan(
.
Re
)(.
0
)
cos(
.
0
0
)(.
0
)
cos(
.
2
2
2
2
R
v
h
Rn
Lv
h
Rn
v
h
v
L
sen
L
R
v
L
sen
L
res
E
N
E
res
D
l
el
E
l
el
N
l
el
l
ib
onde:
22
2 DN
res
vvv
e
mR
06
,
1948
2
Para a sétima etapa da trajetória, na qual o veículo realiza uma manobra circular, de forma a
que o ângulo heading vai de 0º a 360º (2
), em 36 s, tem
-
se:
3
3
3
3
0
0
)
tan(
.
Re
)(.
0
)
cos(
.
0
0
)(.
0
)
cos(
.
R
v
h
Rn
Lv
h
Rn
v
h
v
L
sen
L
R
v
L
sen
L
res
E
N
E
res
D
l
el
E
l
el
N
l
el
l
ib
onde:
22
3 DN
res
vvv
e
mR 06,1948
3
3.4.4.
RESULTADOS DA SIMULA
ÇÃO
Os resultados da s
imulação estão apresentados da FIG. 3.9 à FIG. 3.21
.
Nas FIG. 3.9, 3.11 e 3.13, tem-se em vemelho, com marcadores em formato de
circunferência,
a trajetória calculada através da integração das equações de navegação. Os
mesmos parâmetros de posição também foram calculados de forma analítica, a cada iteração,
tomando
-
se por base as seguintes equações da cinemática:
tavv .
0
e
2
00
..
2
1
. tatvss ,
ond
e:
a
é a aceleração no período de uma iteração;
v
0
é a velocidade no início da iteração e v, ao final;
s
0
é a posição no início da iteração e s, ao final;
79
t
é o intervalo de tempo correspondente a uma iteração, ou seja, 0,01 s.
Os parâmetros calculados desta última maneira foram representados nos gráficos das
referidas figuras na cor verde, com marcadores na forma de triângulos.
Pode-se perceber, mormente através das FIG. 3.10, 3.12 e 3.14, que apresentam os erros,
baseados nas diferenças de valores entre os parâmetros calculados das duas maneiras e que as
diferenças dos valores são devidas aos erros na integração e, sobretudo ao fato de as leituras dos
sensores, sendo discretas no tempo, numa freqüência de 100 Hz, não traduzirem o aspecto
contínuo da evoluç
ão das forças específicas e das velocidades angulares.
0
10 20 30 40 50 60
70
80
90 100
0.398
0.3985
0.399
0.3995
0.4
0.4005
0.401
0.4015
0.402
0.4025
t
Latitude [rad]
Grafico da Latitude do veiculo x tempo
Lat calculada
Latitude real
FIG. 3.9 Latitude x tempo
80
0
10 20 30 40 50
60
70
80
90 100
-1.5
-1
-0.5
0
0.5
1
x 10
-5
t
Erro em Latitude [rad]
Grafico do Erro em Latitude do veiculo x tempo
Grandezas estatisticas do Erro em LATITUDE:
Valor Medio.............. : -3.1577e-006
Desvio Padrao........... : 5.8078e-006
Valor de Pico a Pico.. : 1.6904e-005
FIG. 3.10
Erro
s em
Latitude
0
10
20 30 40 50 60 70 80 90
100
0.7557
0.7558
0.7559
0.756
0.7561
0.7562
0.7563
0.7564
0.7565
t
Longitude [rad]
Grafico da Longitude do veiculo x tempo
Long calculada
Long real
FIG. 3.11
Longitude x tempo
81
0
10
20
30
40 50 60 70 80 90
100
-6
-4
-2
0
2
x 10
-5
t
Erro em Longitude [rad]
Grafico do Erro em Longitude do veiculo x tempo
Grandezas estatisticas do Erro em LONGITUDE:
Valor Medio.............. :
-7.9998e-006
Desvio Padrao........... : 1.6247e-005
Valor de Pico a Pico.. : 5.2599e-005
FIG. 3.12
Erros em
Longitude
0
10 20
30 40 50 60 70
80 90
100
-500
0
500
1000
1500
2000
2500
3000
3500
t
altura [m]
Grafico da altura do veiculo x tempo
Altura calculada
Altura real
FIG. 3.13
Altura x tempo
82
A
FIG. 3.15 apresenta um gráfico da longitude em função da latitude, onde fica evidenci
ada
a manobra correspondente à mudança do ângulo heading em 360º.
O ângulo roll não sofre alteração durante a trajetória, permanecendo sempre igual a zero,
no entanto, a FIG 3.16 mostra uma pequena variação do seu valor, durante o período
0
10 20 30
40
50 60 70 80
90
100
-1.5
-1
-0.5
0
0.5
t
Erro em altura [m]
Grafico do Erro em altura do veiculo x tempo
Grandezas estatisticas do Erro em ALTURA:
Valor Medio.............. : 0.17938
Desvio Padrao........... : 0.21196
Valor de Pico a Pico.. : 1.3861
0.398 0.3985 0.399 0.3995 0.4 0.4005 0.401 0.4015 0.402 0.4025
0.7557
0.7558
0.7559
0.756
0.7561
0.7562
0.7563
0.7564
0.7565
Latitude [rad]
Longitude [rad]
Grafico de Latitude x Longitude
FIG. 3.15
Longitude x Latitude
FIG. 3.14
Erros em Altura
83
correspondente à manobra em heading . Esta variação decorre, como foi explanado
anteriormente, sobretudo do fato de as leituras dos sensores serem discretas no tempo.
Na
FIG. 3.17, pode-se verificar as manobras ascendente e descendente do
veículo,
correspondentes às alterações do ângulo pitch , inicialmente de 0º a 60º (1,05 rad) e, depois, de
60º a 0º.
FIG. 3.16
Ângulo de roll x tempo
0
10 20 30 40 50 60
70 80
90
100
-0.5
0
0.5
1
1.5
2
2.5
3
3.5
x 10
-4
t
roll [rad]
Grafico de roll x tempo
0
10 20 30 40 50 60
70 80
90
100
0
0.2
0.4
0.6
0.8
1
1.2
1.4
t
pitch [rad]
Grafico de pitch x tempo
FIG. 3.17
Ângulo
pitch x tempo
84
A
FIG. 3.18 apresenta o gráfico da evolução do ângulo heading com o tempo. A única
manobra que implica na alteração do referido ângulo, correspondendo a uma variação de 0º a
360º, fica evidenciada na figura.
O valor máximo do módulo da velocidade é de 340 m/s pode ser facilmente obtido pelas
equações da cinemática, lembrando que os únicos períodos em que o veículo é submetido a
acelerações longitudinais são, inicialmente, por 1 s, a 100 m/s
2
e, logo em seguida, por 4 s, a 60
m/s
2
.
Portanto, empregando
-
se a equação
tavv .
0
,tem
-
se:
smv /
340
4.
60
)1.
100
0(
Na
FIG. 3.19 pode-se observar o aumento da velocidade norte no início da trajetória até
atingir o valor de 340 m/s; valor este que só é alterado nas manobras de variação de pitch e
heading , uma vez que surgem, respectivamente, componentes verticais e leste da velocidade,
como pode
-
se observar
na
FIG 3.20 e FIG. 3.21
.
0
10 20 30 40 50 60
70 80
90
100
-2
-1.5
-1
-0.5
0
0.5
1
1.5
2
t
heading [rad]
Grafico de gama x tempo
FIG. 3.18
Ângulo heading x tempo
85
0
10 20
30
40 50 60 70 80 90
100
-400
-300
-200
-100
0
100
200
300
400
t
velocidade norte [m/s]
Grafico da velocidade norte x tempo
FIG. 3.19
Velocidade Norte x tempo
0
10 20
30
40 50 60 70 80 90
100
-300
-250
-200
-150
-100
-50
0
50
t
velocidade vertical para baixo [m/s]
Grafico da velocidade vertical para baixo x tempo
FIG. 3.20
Velocidade Vertical para baixo x tempo
86
Da comparação dos resultados em latitude, longitude e altura obtidos via integração das
equações de navegação, que constitui o algoritmo de navegação propriamente dito, com
os
obtidos via equações da cinemática, pode-se perceber que o algoritmo está correto. O fato de os
gráficos da evolução dos ângulos de
roll,
pitch
e
heading
, bem como das velocidades norte,
vertical para baixo e leste mostrarem resultados coerentes com os esperados ratificam esta
conclusão. Os erros entre os valores esperados e os obtidos via algoritmo são creditados, como
dito anteriormente,
sobretudo
ao fato de as leituras dos sensores serem discretas, numa freqüência
de 100 Hz, o que não traduz fielmente as evoluções das componentes tanto da força específica
quanto da velocidade angular absolta do corpo, que são contínuas.
0
10 20
30
40 50 60 70 80 90
100
-400
-300
-200
-100
0
100
200
300
400
t
velocidade leste [m/s]
Grafico da velocidade leste x tempo
FIG. 3.21
Velocidade Leste x tempo
87
4.
TRATAMENTO DOS ERROS
4.1.
TIPOS DE ERROS
Tanto os giroscópios quanto os acelerômetros possuem erros nas suas medidas. Esses erros
sã
o, principalmente, os seguintes:
-
Sensibilidade: inclinação da reta de ajuste do fator de escala;
- Tendência ( bias ): erro sistemático em toda faixa de trabalho e que corresponde,
quando o parâmetro a ser medido é nulo, a medição de um valor diferente de zero. Corresponde,
portanto, a um desvio do zero da escala;
- Deriva ( drift ): refere-se à taxa na qual o erro de um sensor se acumula no tempo e
corresponde a um afastamento angular da reta ou curva que define a progressão da grandeza
medida no temp
o;
- Histerese: tendência dos componentes do sensor em manter seu estado de perturbação,
mesmo após o fim da excitação;
- Erros de acoplamento cruzado: sensibilidade a acelerações e rotações em direções
normais ao eixo de sensibilidade.
Estes erros, de uma forma geral, possuem componentes determinísticas e aleatórias. As
componentes determinísticas podem ser facilmente levantadas e corrigidas diretamente nas
medidas dos sensores. As componentes aleatórias, no entanto, demandam o auxílio de medida
fornecid
a por outro instrumento. De posse das duas medidas, procede-se uma estimação ótima,
por processos como a filtragem de Kalman.
4.2.
MODEL
AGEM
DOS ERROS DOS SENSO
RES
4.2.1.
MODEL
AGEM
DOS ERROS DO GIROSCÓ
PIO
Considerando-se, inicialmente, o sensor sem erro, sendo x o seu eixo de entrada e z o de
rotação e, tendo em vista, que o sinal de saída do mesmo é tensão elétrica, surge a necessidade de
um fator de escala, a que chamaremos k
gx
, expresso em unidade de velocidade angular sobre
unidade de tensão elétrica, de forma a
tornar verdadeira a equação que se segue:
88
x
=k
gx
.v
gx
,
onde
v
x
é a tensão elétrica de saída do giroscópio e
k
gx
é o fator de escala.
Levando
-
se em consideração os erros, pode
-
se empregar a seguinte equação
, apresentada por
TITTERTON e WESTON, em 1997, para expressar a velocidade angular fornecida pelo
giroscópio (
x
):
xzx
axz
z
gz
x
gx
fzzyyx
gx
x
nffBfBfBBMMS ......
).
1(
(4.
1)
onde:
S
gx
é o erro no fator de escala, que pode ser expresso como um polinômio em função de
x
,
de forma a representar as não linearidades
B
f
é o bias principal, insensível à gravidade;
B
gx
e
B
gz
são os bias secundários, sensíveis à gravidade;
B
axz
é o
bias
anisoelástico;
M
y
e
M
z
são os coeficientes de desalinhamento e
n
x
é a componente aleatória do erro.
Os coeficientes
S
gx
,
M
y
,
M
z
, B
gx
,
B
gz
e
B
axz
são, eminentemente, determinísticos e mensuráveis
em laboratórios e, portanto, seus efeitos podem ser compensados. Componentes aleatórias podem
ocorrer, dependendo do tipo de sensor utilizado, mas, por serem muito menores que as
componentes determinísticas, não carecem de ser estimadas, a não ser quando se faz necessária
uma exatidão muito grande nas medidas, como por exemplo, no caso de navegação não auxiliada
por longos períodos. O coeficiente B
f
, em contrapartida, possui uma componente aleatória
importante, que se traduz no chamado bias dia a dia, ou bias turn on-turn off , corres
pondente
a um desvio do zero, aleatório, que surge a cada vez que o sensor é ligado. Tem-se ainda o erro
n
x
, que é tratado como um ruído branco de média nula.
Neste trabalho, partiu-se do pressuposto de que as componentes determinísticas do erro
podem ser levantadas em laboratório e de que as componentes aleatórias dos coeficientes, à
exceção do bias dia a dia são inexistentes para o sensor utilizado ou, em existindo, podem ser
desprezadas para o tipo de navegação pretendida. Desta forma,
atentou
-
se
apenas para o valor
deste bias dia a dia, representado, neste trabalho, pela notação
bias
g
,
procurando
-se, através de
filtragem, estimá-
lo
. Assim, a equação de compensação do sinal do giroscópio utilizada neste
trabalho é a seguinte:
89
x
gx
xx
n
bias
(4.2)
Através da filtragem de Kalman, pode-se obter uma estimativa deste bias dia a dia e, então,
compensá
-
lo.
4.2.2.
MODEL
AGEM
DOS ERROS DO ACELERÔ
METRO
Da mesma forma que no giroscópio, no acelerômetro, o sinal de saída é também tensão
elétrica. Através de um fator de escala k
acx
, expresso em unidade de aceleração sobre unidade de
tensão elétrica, torna
-se verdadeira a equação que se segue:
f
x
= k
acx
.v
fx
,
onde
f
x
é o valor da força específica sem erro; v
fx
é a tensão elétrica de saída do giroscópio e
k
acx
é o fator de escala.
Levando
-
se em consideração os erros, pode
-
se empregar a seguinte equa
ção
, apresentada por
TITTERTON e WESTON, em 1997, para expressar a força espec
ífica
expressa pelo
acelerômetro (
x
f
):
xyxvfzzyyx
acx
x
nffBBfMfMfSf ....)1(
(4.
3)
onde:
S
acx
é o erro no fator de escala, que pode ser expresso como um polinômio em função de f
x
,
de forma incluir os efeitos não lineares.
B
f
é o bias principal;
B
v
é o coeficiente vibro
-
pendular;
M
y
e
M
z
são os coeficientes de co
-
relacionamentos cruzados e
n
x
é a componente aleatória do erro.
Os coeficientes S
x
, M
y
e M
z
são, como nos giroscópios, eminentemente determinísticos e
mensuráveis em laboratórios e, portanto, seus efeitos podem ser compensados. Eventuais
componentes aleatórias destes coeficientes, por serem pequenas, quando comparadas às
componentes determinísticas dos mesmos, podem ser desprezadas, senão em casos em que uma
extrema exatidão das leituras dos sensores é requerida, como no caso de navegação sem auxílio
por longos períodos. O coeficiente vibro-pendular (B
v
), para variações lentas na força específica,
pode também ser compensado para uma extensa faixa de medida. Da mesma forma que para o
90
giroscópio, o b
ias principal possui uma componente aleató
ria que não pode ser desprezada
.
Trata
-
se de um bias constante no tempo, criado sempre que o sensor é ligado: o bias dia a dia do
acelerômetro; representado neste trabalho pela notação
bias
ac
.
A componente aleatór
ia do erro
n
x
, corresponde a um ruído brando de média nula.
Tendo em vista que o bias dia a dia se constitui na componente aleatória mais importante da
equação de compensação, este trabalho considerou a seguinte equação de compensação do sinal
do acelerôm
etro
:
x
acx
xx
n
bias
ff
(4.4)
Mais uma vez há que se recorrer a uma medida fornecida por outro instrumento e,
subseqüente, estimação ótima do sinal, de maneira a minimizar este erro. Através da filtragem de
Kalman, empregada neste trabalho, obtém-
se
uma estimação para o bias dia a dia,
bias
acx
que é,
então, compensado.
4.3.
EQUAÇÕES DE PROPAGAÇÃO DE ERROS
As equações de erros são apresentadas por TITTERTON e WESTON, em 1997, e são
obtidas a partir da variação das equações correlatas na forma usual.
Inic
ialmente, para se obter a equação de propagação dos erros na orientação da plataforma,
far-
se
-
á algumas considerações preliminares.
Seja C a Matriz de Transformação de Coordenadas do referencial
o-x-y-
z
para o o
-x -y -z .
Como C é ortogonal, tem
-
se:
C.C
T
=
I
(4.
5)
Fazendo
-
se a variação da EQ. 4.5
, obtém
-
se o seguinte:
C.
C
T
+
C.C
T
= 0
(4.
6)
Portanto:
C.
C
T
=
C.C
T
(4.
7)
Da
EQ. 4.7 pode-se concluir que
C.C
T
é uma matriz anti-simétrica e pode ser representada
da seguinte forma:
C.C
T
= ( x)
(4.
8)
91
onde
(
x
)
é a forma anti-simétrica do vetor , que desempenha o papel de uma rotação,
chamada rotação virtual do referencial o-x-y-z para o o-x -y -z , com as componentes neste
último.
Portanto, tem
-
se:
C=
(
x
). C
(4.
9)
Designando o referencial o-x-y-z como o referencial local e o-x -y -z
como o do corpo, tem-
se que a matriz
C
representa a matriz
b
l
C
e, portanto,
Tl
b
CC .
Mas, pré
-
multiplicando a EQ. 4.
7
por
C
T
, chega
-se a:
C
T
=
C
T
.
C.C
T
(4.10
)
No en
tanto, substituin
do a EQ. 4.8 na EQ. 4.10
, obtém
-
se:
C
T
=
C
T
. (
x)
(4.11
)
D
e
onde pode
-se escrever o seguinte:
C
T
=
(
x
).C
T
.
(4.12
)
onde
(
x) é a forma anti-simétrica do vetor , que corresponde ao vetor
com as
componentes no referencial o-x-y-z, que é o referencial de interesse. Assim,
pode ser
identificado como o vetor dos erros em atitude da plataforma, ou seja, os ângulos de
desalinhamento, podendo
-
se, portanto escrever o seguinte:
l
b
l
b
CC
).
(
(4.13
)
E:
(4.14
)
Para se obter a equação de propagação dos erros de orientação, deve-se partir da equação de
atualização no tempo da Matriz de Transformação de Coordenadas referencial do corpo para o
local,
EQ. 3.33, uma vez que a mesma depende dos parâmetros de atitude. A referida equação
será reescrita a seguir:
l
b
l
il
b
ib
l
b
l
b
CCC
).
()
.(
(4.15
)
Fazendo a variação da equação acima, tem
-
se:
l
b
l
il
l
b
l
il
b
ib
l
b
b
ib
l
b
l
b
CCCCC
).
(
).
()(.)
.(
(4.16
)
92
Como
)( A
dt
d
dt
dA
, ou seja, as operações de variação e diferenciação no tempo são
comutativas, pode
-
se escrever:
l
b
l
il
l
b
l
il
b
ib
l
b
b
ib
l
b
l
b
CCCCC
dt
d
).
(
).
()(.)
.(
)(
(4.17
)
Substituindo
-
se
a EQ. 4.13
na EQ. 4.17
, tem
-
se:
]
).
(
).[
(
).
()(.)
].(
).
([]
).
([
l
b
l
il
l
b
l
il
b
ib
l
b
b
ib
l
b
l
b
CCCCC
dt
d
(4.18
)
Resolvendo a derivada no tempo do lado esquerdo da expressão, tem
-
se:
]
).
(
).[
(
).
()(.)
].(
).
([
).
(
).
(
l
b
l
il
l
b
l
il
b
ib
l
b
b
ib
l
b
l
b
l
b
CCCCCC
(4.19
)
Substituindo
-
se
a EQ. 4.15
na
EQ. 4.19
, obtém
-
se o seguinte:
]
).
(
).[
(
).
()(.)
].(
).
([
]
).
()
.(
).[
(
).
(
l
b
l
il
l
b
l
il
b
ib
l
b
b
ib
l
b
l
b
l
il
b
ib
l
b
l
b
CCCC
CCC
]
).
(
).[
(
).
()(.)
.().
(
).
).(
()
.().
(
).
(
l
b
l
il
l
b
l
il
b
ib
l
b
b
ib
l
b
l
b
l
il
b
ib
l
b
l
b
CCCC
CCC
(4.20
)
Pós-
multiplicando cada termo por
T
l
b
C
e trocando o sinal de todos os termos, tem
-
se:
T
l
b
b
ib
l
b
l
il
l
il
l
il
CC
).
(.)()
).(
()
).(
()(
(4.21
)
Em notação vetorial, pode
-se esc
rever a
EQ. 4.21
da seguinte forma:
b
ib
l
b
l
il
l
il
C.
(4.22
)
A
EQ. 4.22
é, portanto, a equação de propagação dos erros de orientação.
Para se obter a equação de propagação dos erros na velocidade, basta se calcular a variação
da equação de atualização no tempo da velocidade, que é a
EQ. 3.13
:
gvfCv
e
elie
bl
be
)2(.
(4.23
)
Calculando
-
se a variação, obtém
-
se:
gvvfCfCv
e
elie
e
elie
bl
b
bl
be
)2()2(..
(4.24
)
Como
l
b
l
b
CC
).
'(
, pode
-se escrever o seguinte:
gvvfCfCv
e
elie
e
elie
bl
b
b
l
be
)2()2(..
).
'(
(4.25
)
Como
lb
l
b
ffC .
e
'
).
(
).
'(
ll
ff
, tem
-
se:
93
gvvfCfv
e
elie
e
elie
bl
b
l
e
)2()2(.'
).
(
(4.26
)
Desprezando
-se os termos de Coriolis e considerando-se a gravidade conhecida, a EQ. 4.26
reduz
-se a:
bl
b
l
e
fCfv .'
).
(
(4.27
)
Finalmente, a equação de propagação de erros em posição pode ser expressa da seguinte
maneira:
e
vp
(4.28
)
A EQ. 4.22, a EQ. 4.27 e a EQ 4.28 constituem-se, respectivamente, nas equações de
propagação dos erros de orientação, de velocidade e de posição e que podem ser implementadas
na formulação do processo com vistas à filtragem de Kalman, de maneira a se poder estimar tais
erros.
Como será visto mais adiante, no alinhamento fino levado a efeito neste trabalho (item 5.5),
pelo fato se buscar estimar tão somente os erros de orientação, apenas a EQ. 4.22 fez parte da
equação do processo (EQ. 5.21).
94
5.
ALGORITMO DE
ALINHAMENTO
5.1.
DEFINIÇÃO
O Algoritmo de
Alinhamento
tem por finalidade, via de regra, gerar a orientação inicial;
informação esta que é requerida pelo algoritmo de navegação e atitude, cuja incerteza é
fortemente dependente da incerteza daquela informação.
Os erros na compensação dos bias dia a dia dos giroscópios e dos acelerômetros também são
bastante críticos no algoritmo de navegação. Por esta razão, o algoritmo de
alinhamen
to
proposto
neste trabalho
inclui a estimação de tais valores, de forma a minimizar estes erros.
5.2.
ALINHAMENTO
A determinação da orientação da plataforma em relação ao Norte verdadeiro denomina-
se
alinhamento.
5.3.
NIVELAMENTO
O nivelamento consiste na determinação da orientação da plataforma em relação à vertical
local.
O nivelamento será abordado em conjunto com o alinhamento, em duas etapas, o
Alinhamento Grosseiro e o Alinhamento Fino, que serão detalhadas na seqüência.
5.4.
ALINHAMENTO GROSSEIR
O
O procedime
nto usado neste trabalho para o alinhamento grosseiro
baseia
-
se na apresentação
feita por TITTERTON e WESTON, em 1997.
O alinhamento grosseiro se constitui na primeira das duas etapas com vistas à determinação
da orientação inicial da plataforma em relação ao referencial local. Nesta etapa, as leituras dos
giroscópios e acelerômetros da plataforma são usadas como forma de se obter a orientação
inicial.
95
O alinhamento abordado neste trabalho supõe que o corpo esteja estacionário em relação ao
referencial da
Terra
. É o chamado ground alignment ou alinhamento estacionário.
Para a realização do alinhamento, é necessário o conhecimento, através de GPS ou algum
outro meio, da latitude do corpo.
Uma vez que os vetores velocidade angular da
Terra
e gravidade são p
erfeitamente
conhecidos tanto no que diz respeito às suas magnitudes quanto às suas direções, pode-
se
facilmente obter as componentes destes vetores no referencial local.
Inicialmente, tome-se o vetor gravidade, que é medido pelos acelerômetros e, como um
exemplo simples, considere
-
se um acelerômetro dentro de um elevador.
A saída de um acelerômetro é um sinal elétrico que é proporcional não à aceleração do
corpo, mas à força específica (
m
F
f
).
A
FIG. 5.1 ilustra um acelerômetro num elevador, em 03 (três) situações distintas. Na
primeira o elevador está parado, ou seja, sua aceleração em relação ao espaço inercial (a
i
) é nula.
FIG. 5.1
Indicação de ac
elerômetro colocado em elevador em três situações
distintas
a
i
Indicação do
acelerômetro:
0
Indicação do
acelerômetro:
a
i
-
g
Indicação do
acelerômetro:
-
g
ELEVADOR PARADO
ELEVADO
R DESCENDO
COM ACELERAÇÃO
a
i
ELEVADOR EM QUEDA
LIVRE
+
Sentido positivo
tomado para
cima
a
i
=0
ga
i
96
Como o vetor aceleração é igual à soma do vetor gravidade (
g
) com o vetor força específica
( f
), tem
-
se:
gfgf0
Dessa forma, conclui-se que, no caso do elevador parado, o acelerômetro, que mede a força
específica, irá indicar
g
.
De forma análoga, é fácil entender que,
no caso de o elevador estar descendo com aceleração
igual à da gravidade, isto é, na situação de queda livre, o acelerômetro irá indicar 0 (zero) e, no
caso de o elevador estar descendo com uma aceleração
a
i
, o acelerômetro indicará
a
i
g.
Assim, como se pode observar, pela ilustração do elevador, a situação da plataforma no
alinhamento, pelo fato de ela estar estacionária em relação ao referencial da
Terra
, é ilustrada
pela primeira situação do elevador
Terra
e, assim, a força específica medida por ele é
g.
Adotando o Referencial Local com configuração geográfica e orientação NED, estando o
Sistema de Navegação Inercial estacionário, o vetor força específica tem as seguintes
componentes:
l
l
g
f 0
0
Tome
-se, agora, o outro vetor conhecido, o vetor velocidade angular da
Terra
em relação ao
referencial inercial,
.
Como foi visto no item 2.3.4.2.1,
EQ. 2.2
6 e EQ. 2.27, as componentes do vetor velocidade
angular da
Terra
em relação ao referencial inercial, com configuração geográfica e orientação
NED, que são as seguintes:
)(.
0
)
cos(
.
L
sen
L
l
ie
, no hemisfério Nor
te e
)(.
0
)
cos(
.
L
sen
L
l
ie
, no hemisfério Sul.
O vetor gravidade sensibiliza os acelerômetros, enquanto o vetor velocidade angular da
Terra
, os giroscópios. Entretanto, estes sensores, no caso da plataforma solidária ou strapdown ,
fornecem as componentes dos referidos vetores no referencial do corpo. Daí, a necessidade de se
recorrer às seguintes equações:
97
l
b
l
b
fCf .
(5.
1)
l
ie
b
l
b
ie
C .
(5.
2)
onde:
b
f
é a força específica com componentes no Referencial do co
rpo;
l
f
é a força específica com componentes no Referencial Local;
b
ie
é a velocidade angular do Referencial da Terra em relação do Referencial Inercial, com
componentes no Referencial do corpo;
l
ie
é a velocidade angular do Referencial da Terra em relação do Referencial Inercial, com
componentes no Referencial Local; e
b
l
C
é a Matriz de Transformação de Coordenadas do Referencial Local para o Referencial do
Corpo.
Uma vez que
se dispõe das leituras dos sensores, do valor da latitude e dos valores esperados
para as componentes da força específica e da velocidade angular no referencial local, pode-
se
obter os elementos da Matriz de Transformação de Coordenadas
b
l
C .
Como, no entanto, as leituras dos sensores possuem erros, pode-se falar em estimativas para
as componentes da força específica e para a velocidade angular da
Terra
em relação ao
referencial local e, portanto, na obtenção de uma estimativa de
b
l
C . Usando a notação ^ para
representar as estimativas, pode-se reescrever a
EQ. 5.1 e a EQ. 5.2
da seguinte forma:
l
b
l
b
fCf .
(5.
3)
l
ie
b
l
b
ie
C .
(5.
4)
Seja
333231
232221
131211
ccc
ccc
ccc
C
l
b
a Matriz de Transformação de Coordenadas do Referencial do
Corpo para o Referencial Local. Evidentemente, tem-se que
332313
322212
312111
ccc
ccc
ccc
C
b
l
é a Matriz de
Transformação de Coordenadas
do Referencial Local para o Referencial do Corpo.
98
Tomando, inicialmente a
EQ. 5.3
, pode
-se escrever o seguin
te:
gccc
ccc
ccc
f
f
f
z
y
x
0
0
332313
322212
312111
de onde, tem
-
se:
g
f
cgcf
g
f
cgcf
g
f
cgcf
z
z
y
y
x
x
)
.(
)
.(
)
.(
3333
3232
3131
(5.
5)
Finalmente, tomando
-
se a
EQ. 5.4
, a mesma pode ser escrita do seguinte modo:
)(.
0
)
cos(
.
332313
322212
312111
L
sen
L
ccc
ccc
ccc
z
y
x
de onde, tem
-
se:
)(..)
cos(
..
)(..)
cos(
..
)(..)
cos(
..
3313
3212
3111
L
sen
cLc
L
sen
cLc
L
sen
cLc
z
y
x
E, portanto:
)
tan(
)
cos(
.
)
tan(
)
cos(
.
)
tan(
)
cos(
.)
cos(
.
)(..
)
cos(
.
13
12
31
11
L
g
f
L
c
L
g
f
L
c
L
g
f
LL
L
sen
c
L
c
zz
yy
xxx
(5
.6)
Resta ainda obter-se a segunda linha da Matriz de Transformação de Coordenadas, que será
obtida pela condição de ortogonalidade.
Como foi visto no item 2.1.1, cada coluna da matriz C correspondem às componentes de
vetores unitários ortogonais entre si. Assim, designando por C
1
, C
2
e C
3
as primeira, segunda e
99
terceira linhas, respectivamente, da Matriz de Transformação de Coordenadas, pode-se entendê-
las como três vetores unitários dispostos como na
FIG. 5.
2.
Desta forma, em se tratando de vetores unitários, mutuamente ortogonais, os seguintes
produtos vetoriais, pela regra da mão direita, se verificam:
C
1
= C
2
x
C
3
(5.
11)
C
2
= C
3
x
C
1
(5.
12)
C
3
= C
1
x
C
2
(5.
13)
Conseqüentemente, através da EQ. 5.12, pode-se obter a segunda linha da Matriz de
Transformação de Coordenadas a partir dos elementos das duas outras linhas:
31123211
31133311
32133312
23
22
21
2
13
12
11
3132
3133
3233
13
12
11
33
32
31
2
..
..
..
0
0
0
cccc
cccc
cccc
c
c
c
C
c
c
c
cc
cc
cc
c
c
c
c
c
c
C
de onde, tem
-
se:
3112321123
3113331122
3213331221
..
..
..
ccccc
ccccc
ccccc
(5.
14)
x
z
C
2
C
1
C
3
y
o
FIG. 5.2
Representação das linhas da Matriz de Transformação
de Coordenadas como vetores
C
1
, C
2
e
C
3
100
Como, conforme se pode verificar na FIG. 2.3, os ângulos de Euler são tomados a partir do
referencial local, tem-se que a matriz expressa na EQ. 2.17 trata-se da Matriz de Transformação
de Coordenadas do Referencial Local para o do Corpo. Uma vez que, necessita-se, no momento,
de
l
b
C
, toma
-
se a transposta da matriz da EQ. 2.1
7,
obtendo
-
se:
cos
.
coscos
.
cos
...
coscos
.
cos
..
cos
.
cos
.
cos
..
cos
.
cos
..
cos
.
cos
333231
232221
131211
sensen
sensensensensensensen
sensensensensensen
ccc
ccc
ccc
C
l
b
Desta forma, ao final do alinhamento grosseiro, conhece-se os elementos de
l
b
C , ou antes,
estimativas para os mesmos e, portanto, pode-se obter as estimativas para os ângulos de roll ,
, pitch , , e heading , , que definem a orientação do corpo. Usando, mais uma vez ^ para
denotar estimativas, pode
-
se escrever:
cos
.
cos
cos
.
33
32
c
sen
c
tan
cos
33
32
sen
c
c
33
32
1
tan
c
c
(5
.15)
cos
.
cos
.
cos
21
11
sen
c
c
tan
cos
11
21
sen
c
c
11
21
1
tan
c
c
(5
.16)
cos
.
21
31
sen
c
sen
c
tan
cos
.
21
31
sen
sen
sen
c
c
.
tan
21
31
1
sen
c
c
(5
.17)
O algoritmo do alinhamento grosseiro, utilizado neste trabalho, é, pois, o seguinte:
1º Passo: Leitura da Latitude, L, do valor da magnitude da rotação da Terra
,
, e da
magnitude da aceleração da gravidade,
g;
2º Passo: Loop , no qual as leituras dos sensores foram tomadas 30 (trinta) vezes, de
forma a se poder chegar a uma melhor estimativa da orientação inicial;
(início do Loop )
Passo 2.1
: Leitura dos
acelerômetros;
Passo 2.2
: Leitura dos giroscópios;
Passo 2.3
: Armazenamento na memória das leituras dos sensores;
101
(Fim do Loop )
3º Passo
: Cálculo das médias das leituras de cada sensor, ou seja:
x
= média das leituras rea
lizadas pelo giroscópio alinhado com o eixo
x;
y
= média das leituras realizadas pelo giroscópio alinhado com o eixo
y;
z
= média das leituras realizadas pelo giroscópio alinhado com o eixo
z;
x
f
= média das leituras realizadas pelo acelerômetro alinhado com o eixo
x;
y
f
= média das leituras realizadas pelo acelerômetro alinhado com o eixo
y;
z
f
= média das leituras realizadas pelo acelerômetro al
inhado com o eixo
z;
4
º Passo
: Cálculo das estimativas dos elementos de
l
b
C
:
)
tan(
)
cos(
.
)
tan(
)
cos(
.
)
tan(
)
cos(
.
13
12
11
L
g
f
L
c
L
g
f
L
c
L
g
f
L
c
zz
yy
xx
g
f
c
g
f
c
g
f
c
z
y
x
33
32
31
3112321123
3113331122
3213331221
..
..
..
ccccc
ccccc
ccccc
5
º Passo
: Obtenção das estimativas de
l
b
C
e de
b
l
C :
333231
232221
131211
ccc
ccc
ccc
C
l
b
1
l
b
l
b
CC
102
6
º Passo
: Obtenção das estimativas dos ângulos de Euler:
33
32
1
tan
c
c
11
21
1
tan
c
c
.
tan
21
31
1
sen
c
c
5.5.
ALINHAMENTO FINO
Após se obter as estimativas dos ângulos de Euler, através do alinhamento grosseiro,
procede
-
se o alinhamento fino, com a finalidade de se estimar os erros das mesmas, de forma a se
poder compensá-los. Foi usado o auxílio de dois giroscópios medidores de deslocamento angular
e um girocompasso que fornece o ângulo de heading , com o objetivo de se obter as referidas
estimativas de erro ou desalinhamentos.
A filtragem de Kalman será utilizada na obtenção destes desalinhamentos.
No equacionamento do processo e das medidas, com vistas ao emprego da filtragem de
Kalman, decidiu
-
se incluir como estados os bias
dia a dia
dos acelerômetros e dos giroscópios, de
sorte que, em se obtendo êxito na estimativa destes parâmetros, se possa compensar este erro na
leitura dos sensores.
Desta forma, o vetor de
estados adotado neste trabalho tem a seguinte composição:
)13(
)13(
)13(
)19(
x
ac
x
g
x
x
acz
acy
acx
gz
gy
gx
bias
bias
bias
bias
bias
bias
bias
bias
x ,
onde:
103
é o vetor dos desalinhamentos, ou dos erros de orientação, em função dos ângulos de
Euler;
bias
g
é o vetor dos bias dia a dia dos giroscópios; e
bias
ac
é o vetor
dos bias dia a dia dos acelerômetros;
A equação
de propagação dos erros de orientação é a
EQ. 4.22
:
b
ib
l
b
l
il
l
il
C
No entanto, como o alinhamento, neste trabalho, é realizado com a plataforma estacionária
em relação ao referencial da Terra, pode-se considerar que a velocidade angular, em relação ao
referencial inercial, do referencial local,
l
il
, é igual à velocidade angular, em relação ao
referencial inercial, do referencial da
Terra,
l
ie
, e que o erro naqu
ela velocidade angular,
l
il
, é
nulo. Portanto, a equação de propagação de erros, neste caso, pode ser reescrita do seguinte
modo:
b
ib
l
b
l
ie
C
(5.18
)
onde:
b
ib
representa os erros nas medidas realizadas pelos giroscópios, composto pelo bias dia a
dia,
g
bias
e ruídos brancos,
g
w .
Como os bias dia a dia dos acelerômetros e dos giroscópios são constantes durante o
funcionamento dos sensores, as equações de atualização do
s bias são as seguintes:
0
g
as
ib
(5.19
)
0
ac
asib
(5.20
)
Desta forma, unindo a EQ 5.18, a EQ 5.19 e a EQ. 5.20
e usando a representação matricial, o
equacionamento da dinâmica do processo usado é o seguinte:
)16(
)13(
)13(
)33()33(
)33()33(
)33()33(
)19(
)13(
)13(
)13(
)33()33()33(
)33()33()33(
)33()33(
)33(
)19(
)13(
)13(
)13(
)(
)(
00
00
0
000
000
0
x
x
g
x
ac
xx
xx
l
xbx
x
x
ac
x
g
x
xxx
xxx
x
l
xb
x
l
ie
x
x
ac
x
g
x
tw
tw
C
bias
bias
C
as
ib
as
ib
(5.21
)
)(
l
ie
é o vetor velocidade angular da
Terra
em relação ao Referencial Inercial, com
componentes no Referencial Local, ou de Navegação, na forma anti
-
simétrica.
Ou seja:
104
)(.;0
);
cos(
.,
0
0
0
)(
lat
sen
lat
onde
DEN
NE
ND
ED
l
ie
)33( x
l
b
C é a Matriz de Transformação de Coordenadas do Referencial do Corpo (b) para o
local (
l
); e
)(
)(
)(
)(
tw
tw
tw
tw
acz
acy
acx
ac
e
)(
)(
)(
)(
tw
tw
tw
tw
gz
gy
gx
g
são os ruídos brancos associados aos acelerômetros e
aos giroscópios, respectivamente.
Na formulação da equação de saída, usar-
se
-á como medidas, além dos valores de
deslocamento angular em relação ao referencial local, fornecidos pelos 02 (dois) giroscópios e 1
(um) girocompasso, o fato de se conhecer os valores da força específica e velocidade angular e,
portanto, dispor
-
se do valor esperado para as medidas dos acelerômetros e giroscópios.
Tome
-
se inicialmente as medidas fornecidas pelos auxílios que fornecem os ângulos de Euler
do corpo ( , , ). Estes auxílios são dois giroscópios medidores de deslocamento angular e um
girocompasso que fornece o ângulo de heading , conforme a implementação de N
EBOT
, em
1997. Estes sensores, apesar de precisos, são inviáveis para utilização durante o movimento,
devido à dinâmica de funcionamento lenta. Por isso, sua utilização se restrin
ge ao alinhamento. O
erro aleatório destes sensores pode ser modelado na forma de ruído branco, assim o vetor v
aux
representa os erros destes auxílios e a equação de saída correspondente aos auxílios tem a forma
que se segue:
)(.
100
010
001
tvz
aux
aux
aux
aux
aux
(5.22
)
Pode-se, a partir do fato de se conhecer as componentes, no referencial local, da velocidade
angular da
Terra
em relação ao espaço inercial, tomando-se as leituras dos giroscópios como
medidas, escrever a seguinte equação de saída:
)(.
100
010
001
)(.
0
)
cos(
.
.. tv
bias
L
sen
L
CCz
gg
b
l
b
ib
l
ib
b
l
b
ib
g
(5.23
)
105
A equação de saída
5.23
é baseada nas medidas realizadas pelos giroscópios de um valor
perfeitamente conhecido. Desta forma, é de se esperar que os erros de medição sejam
decorrentes, unicamente, dos bias dia a dia e ruídos brancos associados a estes sensores, como
está expresso na equação acima.
Fica evidente que se está considerando, neste caso, o ruído de medição igual ao de processo,
ou seja,
)()( twtv
gg
. Esta observação é importante na compreensão da formulação da filtragem
de Kalman utilizada neste trabalho, qual seja, a formulação para ruídos de medida e de processo
correlacionados.
Analogamente, pode-se escrever, para os acelerômetros, com base no conhecimento do vetor
gravidade, a seguinte equação de saída:
)(
100
010
001
0
0
.. tv
bias
bias
bias
g
CffCfz
ac
acz
acy
acx
b
l
b
l
b
l
b
ac
(5.24
)
Aqui também tem
-
se que
)()( twtv
acac
.
Unindo a
EQ. 5.22, a EQ. 5.23 e a EQ. 5.24
, pode
-se escrever o seguinte:
)(
)(
)(
.
00
00
00
)33()33()33(
)33()33()33(
)33()33()33(
tv
tv
tv
bias
bias
I
I
I
z
z
z
ac
g
aux
ac
g
xxx
xxx
xxx
ac
g
aux
(5.25
)
Resumindo e usando a representação clássica para as equações de modelagem do processo e
de
saída, tem
-
se:
vNxHy
wGxFx
..
..
(5.26
)
onde:
)19( x
ac
g
bias
bias
x
;
)33()33()33(
)33()33()33(
)33()33(
)33(
000
000
0
xxx
xxx
x
l
xb
x
l
ie
C
F
;
)33()33(
)33()33(
)33()33(
00
00
0
xx
xx
l
xbx
C
G
)33()33()33(
)33()33()33(
)33()33()33(
00
00
00
xxx
xxx
xxx
I
I
I
H
;
)33()33()33(
)33()33()33(
)33()33()33(
00
00
00
xxx
xxx
xxx
I
I
I
N
106
A formulação empregada na estimação dos estados x
foi a filtragem de Kalman com ruídos
correlacionados, apresentada nas
EQ. 10.
74, EQ. 10.
75 e EQ. 10.76
, constantes do Apêndice 10.
1
deste trabalho.
O algoritmo da filtragem de Kalman com vistas ao alinhamento fino é o seguinte:
1º Passo: Leitura das condições iniciais do vetor de es
tados
x
e da matriz de covariância
do erro de estimação P. Leitura das matrizes de variância dos erros de medida (Q), de variância
de erros de processo (R) e de correlação de erros (S), da matriz H, que é invariante no tempo, e
das matrizes
l
b
C
e
b
l
C
iniciais;
2º Passo
: Loop , no qual as leituras dos sensores e dos auxílios por 150 segundos;
(início do Loop )
Passo 2.1
: Leitura dos acelerômetros;
Passo 2.2
: Leitura dos giroscópios;
Passo 2.3
: Leitura d
os auxílios;
Passo 2.4: Montagem das Matrizes F e G, que, por serem variantes no tempo, têm que
estar inseridas no Loop ;
Passo 2.5: Discretização do sistema. No caso, apenas a matriz F necessita ser
modificada. A expressão utilizada é a seguinte:
tFIF
xk
.
)99(
,
onde:
F
k
é a correspondente no caso discreto à matriz
F;
t é o período de amostragem, que, neste trabalho corresponde a 0,01 s, uma vez que
os sensores realizam suas leituras numa freqüência de 100 Hz.
Passo 2.6
: Montagem da
equação de saída;
Passo 2.7: Cálculo das estimativas dos estados, através da Filtragem de Kalman. Foi
usada a formulação para o caso de ruídos correlacionados;
Passo 2.8: De posse das estimativas dos desalinhamentos, correção dos valores dos
ângulos
de Euler;
Passo 2.9
: Atualização das matrizes
l
b
C
e
b
l
C
, com os novos ângulos
,
e ;
(Fim do Loop )
107
6.
SIMULAÇÕES
6.1.
OBJETIVOS DAS SIMULA
ÇÕES
A informação de posição e orientação do corpo produzida por um sistema de navegação
inercial é gerada com erros. Estes erros, que devem ser minimizados de forma a tornar aquela
informação tão exata quanto possível, têm origem tanto no erro dos valores medidos pelos
sensores (giroscópios e acelerômetros), como também nos erros dos valores de posição e
orientação iniciais da plataforma. Uma vez que os valores correspondentes à orientação inicia
l do
corpo são gerados, via de regra, pela própria plataforma, as simulações levadas a efeito neste
trabalho objetivaram, primeiramente, verificar o efeito, na navegação, dos erros nos valores da
orientação inicial, primeiramente, quando estes valores são obtidos apenas pelo procedimento de
alinhamento grosseiro e, em seguida, quando, além deste procedimento, realiza-se o alinhamento
fino.
Por ocasião do alinhamento fino, que se trata de uma filtragem de Kalman com ruídos de
medida e do modelo correlacionados, foram gerados, além dos valores de orientação inicial do
corpo, a estimação dos bias dia a dia para cada um dos sensores. Durante a simulação da
navegação, foi também verificado o efeito da utilização destes valores na correção da leitura dos
sensores.
6.2.
PARÂMETROS USADOS NA
S SIMULAÇÕES
Supondo
-se que os parâmetros determinísticos das equações de correção das leituras dos
sensores,
EQ. 4.1 e EQ. 4.3
, foram perfeitamente levantados, e que as componentes aleatórias dos
coeficientes das equações de compe
nsação dos sensores, à exceção do bias dia a dia, não existem,
ou, se existem, podem ser desprezadas para o tipo de navegação pretendida, este trabalho se ateve
apenas aos parâmetros que geram entradas aleatórias nas equações, quais sejam: os bias dia a di
a
dos acelerômetros e dos giroscópios e o ruído branco destes sensores. Foram usados os seguintes
valores para estes parâmetros:
-
acelerômetros:
bias dia a dia com média nula e desvio padrão de 1.10
-3
.g;
108
ruído branco com média nula e desvio padrão de 1.10
-3
.g, onde g é a aceleração da
gravidade, cujo valor é função da altura, conforme a equação (
3.2.4.1).
-
giroscópios:
bias dia a dia com média nula e desvio padrão de 1.10
-1
grau/hora
;
ruído branco com média nula e desvio padrão de 1.10
-1
grau/hora
.
- auxílios (medidores de deslocamento angular, usados durante o alinhamento fino,
conforme relatado no item 4.5:
ruído branco com média nula e desvio padrão de 1.10
-1
grau
.
As leituras dos sensores são realizadas a uma freqüência de 100 Hz.
Estes valores correspondem aos de sensores reais, disponíveis, com os quais se pretende
trabalhar numa eventual implementação real deste trabalho de simulação
.
6.3.
DESCRIÇÃO
As simulações realizadas constam, de uma forma geral, de um algoritmo de alinhamento que
fornece as estimativas dos valores de orientação da plataforma e dos bias dos acelerômetros e
giroscópios a um algoritmo de navegação.
No algoritmo de alinhamento, foram usadas 30 (trinta) medidas dos sensores no
levantamento da orientação da plataforma, no procedime
nto de alinhamento grosseiro. O valor 30
(trinta) foi utilizado de forma a se minimizar o erro decorrente do ruído branco. O erro decorrente
do bias dia a dia não poderia ser compensado apenas pelo cálculo da média das leituras, por
possuir um valor constante. Desta forma, não havia sentido em se realizar um número muito
grande de medidas. O número de 30 (trinta) leituras foi, assim, escolhido e mostrou-
se
satisfatório. No alinhamento fino, as medidas dos sensores e dos auxílios foram tomadas por um
período
de 150 segundos, à medida em que os estados, quais sejam, os desalinhamentos e os bias
dos sensores, eram estimados. O valor de 150 (cento e cinqüenta) segundos foi tomado de
maneira a se permitir que os valores das estimativas pudessem evoluir até o valor de
acomodação.
Duas simulações serão apresentadas. Inicialmente, de forma a se evitar a contaminação dos
erros oriundos das leituras dos sensores na navegação com os erros de orientação no instante
109
inicial, não foram incluídos erros de leitura dos sensor
es na navegação, ou seja, foi simulada uma
navegação usando
-
se sensores teoricamente perfeitos.
Em seguida, outra simulação foi realizada, incluindo-se aí os erros de leitura dos sensores.
Em ambas as simulações, o tempo de simulação da navegação foi de 720 segundos e a trajetória
utilizada foi a mesma descrita no item 3.4.1.
Na segunda simulação, três considerações foram feitas e os resultados foram gerados para
cada uma delas. A primeira delas é a que apresenta os resultados considerando-se que se dispõe
apenas dos valores de orientação do corpo móvel fornecidos pelo alinhamento grosseiro. Na
segunda hipótese, esses valores de orientação são corrigidos pelas estimativas dos
deslinhamentos obtidas pelo alinhamento fino. Finalmente, numa terceira hipótese, g
erou
-se os
resultados usando-se, além dos valores de orientação corrigidos através das estimativas de
desalinhamento provenientes do alinhamento fino, as leituras dos sensores compensadas através
dos valores das estimativas dos bias dia a dia.
110
7. ANÁLISE DOS RESULTADOS
Na simulação do alinhamento, os valores iniciais atribuídos aos ângulos de
roll
,
pitch
e
heading
foram nulos. No entanto, como os sensores são imperfeitos
imperfeições essas
simuladas, somando-se o bias e o ruído branco ao valor correto
esses ângulos são lidos
incorretamente. No alinhamento grosseiro, toma-se a média de 30 (trinta) leituras dos sensores,
de forma a se minimizar o efeito do ruído branco. Entretanto, devido ao erro decorrente dos bias
dia a dia, os valores dos ângulos, ao serem extraídos dos valores das leituras dos sensores com
erros, não são obviamente iguais aos valores atribuídos. A diferença entre os valores reais e seus
respectivos valores provenientes do alinhamento grosseiro produzem os chamados erros de
orientação ou
desalinhamentos iniciais da plataforma. O alinhamento fino é, então, implementado
de forma a se estimar o valor de tais desalinhamentos, que são, então, diminuídos dos valores de
orientação gerados no alinhamento grosseiro, de sorte que valores de orientação muito mais
próximos dos reais são obtidos. As FIG. 7.1 a 7.3 mostram a evolução, no alinhamento fino, da
estimativa desses desalinhamentos. Tem-se em azul, o desalinhamento, ou erro, depois de
realizado o alinhamento grosseiro. Pode-se observar que o alinhamento fino fornece uma
informação muito próxima do valor do desalinhamento produzido pelo grosseiro, permitindo que,
em seguida, este erro de orientação seja corrigido.
111
0
20 40 60
80
100 120 140 160
-0.16
-0.14
-0.12
-0.1
-0.08
-0.06
-0.04
-0.02
0
0.02
Estimativa do desalinhamento em X - roll
tempo [s]
droll [graus]
FIG. 7.1
Evolução da estimativa do
desalinhamento em roll, durante o alinhamento
fino
0
20
40 60
80
100
120
140
160
-0.14
-0.12
-0.1
-0.08
-0.06
-0.04
-0.02
0
Estimativa do desalinhamento em Y - pitch
tempo [s]
dpitch [graus]
FIG. 7.2
Evolução da estimativa do desalinhamento em pitch, durante o alinhamento
fino
112
O chamado bias dia a dia dos sensores é, como já foi dito, um desvio do zero da escala que
surge, diferente, sempre que o sensor é ligado. Durante o alinhamento fino, esses valores para
cada um dos 6 (seis) sensores empregados, quais sejam, 3 (três) acelerômetros e 3 (
três)
giroscópios, são estimados, de forma a permitir sua posterior compensação na leitura dos
sensores. Nas FIG. 7.4 e 7.5 são apresentadas, ilustrando as demais, as evoluções das duas
estimativas de bias: a do acelerômetro alinhado com o eixo x do corpo e a do giroscópio cujo
eixo de entrada é o eixo z do corpo. Em azul, tem
-
se o valor real, que é conhecido, uma vez que é
gerado pelo programa. E em verde, tem
-
se a evolução da estimativa.
0
20 40
60 80
100
120 140
160
-0.12
-0.11
-0.1
-0.09
-0.08
-0.07
-0.06
-0.05
-0.04
-0.03
-0.02
Estimativa do desalinhamento em Z - heading
tempo [s]
dheading [graus]
FIG. 7.3
Evolução da estimativa do desalinhamento em heading, durante o alinhamento
fino
113
0
20 40 60 80
100 120 140 160
-7
-6
-5
-4
-3
-2
-1
0
x 10
-3
Estimativa do bias do acel x
tempo [s]
biasx [g]
FIG. 7.4
Evolu
ção da estimativa do bias do acelerômetro alinhado com o eixo x do
corpo
0
20
40 60 80
100 120
140 160
0.06
0.08
0.1
0.12
0.14
0.16
0.18
0.2
Estimativa do bias do giro z
tempo [s]
bias em z [graus/h]
FIG. 7.5
Evolução da estimativa do bias do giroscópio com eixo de entrada alinhado
com o eixo z do corpo
114
Passando
-se à navegação e t
omando
-se, inicialmente, a simulação sem a inclusão de erros de
leitura pelos sensores, durante a navegação, foram geradas saídas para a orientação inicial
baseada apenas no alinhamento grosseiro e para a baseada no alinhamento fino. Como se supôs
sensores
ideais na navegação, não houve necessidade de se usar as estimações dos bias aleatórios
dos sensores, que foram considerados nulos. Nas FIG. 7.6 a 7.8, respectivamente, são
apresentados os gráficos da latitude, da longitude e da altura, em função do tempo. Na cor azul
,
com marcadores em formato de circunferência, tem-se a evolução destes parâmetros de posição,
considerando a hipótese de só se dispor dos dados de orientação inicial oriundos do alinhamento
grosseiro. Na cor vermelha, com marcadores em formato de triângulo apontado para cima,
os
resultados foram obtidos usando a orientação inicial a partir dos dados do alinhamento fino e, na
cor
preta, com marcadores em formato de triângulo apontado para baixo, partiu-se dos dados
corretos de orientação inicial. Pode-se perceber que os resultados, usando-se o alinhamento fino,
são bem mais próximos do ideal do que os usando apenas o grosseiro, como era esperado. A
melhora nos resultados com a utilização dos dados do alinhamento fino fica mais fácil de ser
visua
lizada através dos gráficos de erro, constantes das
FIG.
7.9 a 7.11, nos quais os valores
obtidos foram subtraídos dos esperados, usando-
se os valores corretos para a orientação inicial.
0
100 200 300 400 500 600 700 800
0.395
0.4
0.405
0.41
0.415
0.42
0.425
0.43
0.435
0.44
t
Latitude [rad]
Grafico da Latitude do veiculo x tempo
700
700.5
701
701.5
702
702.5
703
703.5
704
704.5
0.4337
0.4337
0.4338
0.4338
0.4339
0.434
0.434
0.434
0.4341
t
Latitude [rad]
Grafico da Latitude do veiculo x tempo
Fig. 7.6
Latitude x tempo
Legenda:
115
Fig. 7.
7
Longitude x tempo
Fig. 7.8
Altura x tempo
0
100 200 300 400 500 600 700 800
-500
0
500
1000
1500
2000
2500
3000
3500
t
altura [m]
Grafico da altura do veiculo x tempo
695
700 705
710 715
720
3120
3140
3160
3180
3200
3220
3240
3260
3280
3300
t
altura [m]
Grafico da altura do veiculo x tempo
0
100 200 300 400 500 600 700 800
0.7556
0.7558
0.756
0.7562
0.7564
t
Longitude [rad]
Grafico da Longitude do veiculo x tempo
660 670 680 690 700
710 720 730 740
0.7556
0.7556
0.7556
0.7556
0.7557
0.7557
0.7557
0.7557
0.7557
t
Longitude [rad]
Grafico da Longitude do veiculo x tempo
Legenda:
Legenda:
116
0
100 200 300
400
500
600
700 800
-0.5
0
0.5
1
1.5
2
2.5
3
3.5
x 10
-4
Erros em Latitude (Usando os Sensores ideais como referencia)
Alin grosseiro
Alin fino
Fig. 7.9
Erro em Latitude
0
100 200 300 400 500 600 700
800
-2
0
2
4
6
8
10
12
14
x 10
-5
Erros em Longitude (Usando os Sensores ideais como referencia)
Alin grosseiro
Alin fino
Fig. 7.10
Erro em Longitude
117
A simulação usando sensores perfeitos na navegação mostrou a coerência dos resultados
obtidos, validando a estimação dos desalinhamentos do alinhamento fino.
Em seguida, uma nova simulação foi realizada, desta feita, adicionando-se os bias dia a dia e
os ruídos brancos às leituras dos sensores, gerando assim erros na navegação. As
FIG. 7.12 a
7.14
apresentam, respectivamente, os gráficos da latitude, da longitude e da altura, tendo-se, em azul,
com marcadores em formato de circunferência, os resultados usando apenas os dados de
orientação inicial do alinhamento grosseiro, em vermelho, com marcadores em formato de
triângulo apontado para cima, usando os dados de orientação corrigidos pelas estimativas de
desali
nhamento provenientes do alinhamento fino e, em verde, com marcadores em formato de
losango,
foram usados, além das correções na orientação inicial através das estimativas de
desalinhamento obtidas no alinhamento fino, correções nas leituras dos sensores através das
estimações dos bias dia a dia dos mesmos. Pode-se perceber que, desta vez, eventualmente, os
resultados obtidos com os dados de orientação inicial do alinhamento grosseiro podem ser melhor
do que os obtidos com os dados do alinhamento fino, sem a compensação dos bias, como foi o
0
100 200 300 400 500 600 700
800
-160
-140
-120
-100
-80
-60
-40
-20
0
20
Erros em Altura (Usando os Sensores ideais como referencia)
Alin grosseiro
Alin fino
Fig. 7.11
Erro em Altura
118
caso, durante praticamente toda a trajetótia, dos dados de latitude e altura e, no final do percurso,
dos de longitude. No entanto, pode
-
se verificar que os resultados que utilizam, além dos dados do
alinhamento fino, as estimações dos bias produzem sempre os resultados mais próximos do ideal
para todos os parâmetros. A comparação dos resultados pode ser melhor avaliada através das
FIG. 7.15 a 7.17, que apresentam, respectivamente, os erros em latitude, em longitude e em
altura; erros estes que são obtidos subtraindo-se dos valores corretos, gerados simulando-
se
leitura sem erro por parte dos sensores, os obtidos em cada uma das três hipóteses consideradas.
0
100 200 300 400 500 600 700 800
0.395
0.4
0.405
0.41
0.415
0.42
0.425
0.43
0.435
0.44
t
Latitude [rad]
Grafico da Latitude do veiculo x tempo
701
701.2 701.4 701.6 701.8
702
702.2 702.4 702.6 702.8
0.4338
0.4338
0.4339
0.4339
0.434
0.434
t
Latitude [rad]
Grafico da Latitude do veiculo x tempo
FIG. 7.12 Latitude x tempo
Legenda:
0
100 200 300 400 500 600 700 800
0.7556
0.7557
0.7558
0.7559
0.756
0.7561
0.7562
0.7563
0.7564
0.7565
t
Longitude [rad]
Grafico da Longitude do veiculo x tempo
690
695
700
705
710
715
720
725
730
0.7556
0.7556
0.7557
0.7557
0.7557
0.7557
0.7557
t
Longitude [rad]
Grafico da Longitude do veiculo x tempo
FIG. 7.13
Longitude x tempo
Legenda:
119
Fig. 7.13
Longitude x tempo
FIG. 7.14
Altura x te
mpo
0
100 200 300 400
500
600 700 800
-1000
0
1000
2000
3000
4000
5000
6000
t
altura [m]
Grafico da altura do veiculo x tempo
0
100 200 300 400 500 600 700 800
-0.5
0
0.5
1
1.5
2
2.5
3
x 10
-4
Erros em Latitude
Alin grosseiro
Alin fino
Alin fino + bias dos giros e dos acelerometros estimados
FIG. 7.15
Erro em Latitude
Legenda:
120
0
100
200 300 400
500
600
700
800
-2
0
2
4
6
8
10
x 10
-5
Erros em Longitude (Usando os Sensores ideais como referencia)
FIG. 7.16
Erro em Longitude
0
100 200 300 400 500 600 700 800
-2000
-1500
-1000
-500
0
500
Erros em Altura (Usando os Sensores ideais como referencia)
FIG. 7.17
Erro em Altura
Legenda:
Legenda:
121
A ocorrência de erros menores para a hipótese de se partir da orientação inicial baseada no
alinhamento grosseiro do que para a baseada no alinhamento fino, sem a inclusão da estimação
dos bias, parece, à primeira vista, incoerente. No entanto, cabe lembrar aqui que a primeira
simulação realizada confirma que os dados de orientação inicial do corpo fornecidos pelo
alinhamento fino são, de fato, mais precisos que os fornecidos pelo alinhamento grosseiro, haja
vista
os melhores resultados obtidos na navegação, na primeira situação. Outrossim, pode-
se
verificar que os resultados obtidos, usando-se, além da correção na orientação inicial mediante as
estimativas dos deslinhamentos provenientes fino, as correções das leituras dos sensores
mediante as estimações dos bias, foram, dentre todas as hipóteses, os que mostraram ter os
menores erros, em todos os parâmetros. Portanto, pode-se concluir que a estimação dos bias dos
sensores implica, indubitavelmente, na melhoria dos resultados. Isto posto, fica claro que o que
ocorre é que, eventualmente, os erros na orientação podem se associar com os erros de leitura dos
sensores, provenientes dos bias dia a dia, compensando uns aos outros, de forma a que os
resultados, com a associação dos dois erros, venham a ser melhores do que os decorrentes apenas
dos erros de leitura dos sensores.
122
8.
CONCLUSÃO
Neste trabalho, foi concebido um algoritmo de alinhamento que consiste, basicamente, de um
alinhamento grosseiro, seguido de uma filtragem de Kalman, implementada com o uso de três
auxílios, quais sejam, dois giroscópios medidores de deslocamento angular e um girocompasso
que fornece o ângulo de heading , que permite estimar e, por conseguinte, compensar, além dos
erros na orientação in
icial do corpo, os bias dia a dia dos sensores.
Em seguida, um algoritmo de navegação foi implementado, de forma a permitir a verificação
dos resultados, primeiramente sem compensar os erros de orientação provenientes do
alinhamento grosseiro, depois compe
nsando
-os, através das estimativas dos desalinhamentos
obtidas na filtragem de Kalman e, finalmente, realizando-se, além da compensação dos erros de
orientação, a compensação dos bias dia a dia dos sensores.
A estimação dos bias dia a dia dos sensores, principal contribuição deste trabalho, mostrou-
se de extrema importância, haja vista que, como foi mostrado, através dos resultados da segunda
simulação, tão somente a estimação dos desalinhamentos do alinhamento grosseiro, via
alinhamento fino, não garante m
elhoria dos resultados para todos os parâmetros de posição.
Os resultados obtidos, mediante a compensação tanto dos erros de orientação inicial, quanto
dos bias dia a dia se mostraram bastante precisos e próximos do ideal, mesmo ao final de 720
segundos de
navegação.
Este trabalho abre uma série de perspectivas para novos trabalhos, dentre os quais pode-
se
citar:
a) tentativa de se implementar um alinhamento em movimento, ficando a sugestão de se
acrescentar, neste caso, ao equacionamento da filtragem de Kalman, a equação de erros de
velocidade e, eventualmente, utilizar
-
se velocímetros como auxílios;
b) utilização de outros tipos de auxílios no equacionamento da filtragem de Kalman;
c) implementação prática deste algoritmo, numa plataforma inercial;
123
9.
REFER
ÊNCIAS BIBLIOGRÁFICA
S
A
LONZO
, Kelly, Modern Inertial and Satellite Navigation Systems, 1994.
Disponível:
http://www.frc.ri.cmu.edu/~alonzo/pubs/pubs.html [
capturado em 27 nov. 2004]
CAMPOS, Vitor A. F., Aplicação do Filtro de Kalman e dos Filtros de Partículas à
Estimação de trajetórias em Navegação Inercial
, dissertação M. Sc., USP, 2004
CRANDALL, Stephen H., KARNOPP, Dean C., KURTZ, Edward F. Jr., PRIDMORE
-
BROWN,
David C., Dinamics of Mechanical and Eletromechanical Systems, McGraw-Hill, USA
,
1968, 256 p.
DIEGUEZ, J.P.P., Métodos Numéricos Computacionais para a Engenharia
Vol II, Editora
Interciência, 1992, 348 p.
DURÃO, Carlos R. C., Estudo e Implementação de um Algoritmo de Navegação Inercial,
dis
sertação M. Sc., UFRJ, 1992, 178 p.
EUROPEAN
ORGANIZATION FOR THE SAFETY OF AIR NAVI
GATION
e
INSTITUTE
OF GEODESY AND NAVIG
ATION,
WGS
-
84
Implemetion Manual, [on line]. 1998.
Disponível : http://www.wgs84.com/files/wgsman24.pdf [capturado em
maio de
2005]
F
ANG
, Cheng Jiang (Southeast University N
anjing),
SINS Error Model and Observability,
IEEE, 1996, 5 p.
FRANKLIN, G. F., POWELL, J. D., Digital Control of Dynamics Systems, Addison Wesley
Longman, Inc., EUA,
1997, 743 p.
GUIZIOU, Robert, Mecanique Spatiale, 2004.
Disponível:
http://artemmis.un
iv
-
mrs.fr/cybermeca/Formcont/mecaspa/index.htm
[capturado em 05 nov. 2004]
GRUZMAN, Maurício, Simulação de Giroscópio de Suspensão Cardâmica com Dois Graus
de Liberdade, dissertação M.Sc., IME, 2003. 237 p.
124
HAUG, Edward J., Computer Aided Kinematics and Dynamics of Mechanical Systems,
Allyn and Bacon, Massachusetts, 1989. 498 p.
HERMERLY, M. E., Filtro de Kalman: Teoria e Implementação
, CTA
IORIO, Cristiane de Oliveira, Filtragem de Kalman Aplicada a Navegadores Inerciais
,
dissertação M.Sc., IME, 1995. 111 p.
LABARRERE, M; KRIEF, J.P.; GIMONET, B; Le Filtrage et Ses Applications, Cepadues
Editions, Fran
ça, 405 p.
N
EBOT
, Eduardo (University of Sydney), Initial Calibration and Alignment of an Inertial
Navigation
, IEEE, 1997, 6 p.
P
ARK
, Heung Won, LEE, Jang Gyu e P
ARK,
Chan Gook, Covariance Analysis Strapdown
INS Considering Gyrocompass Characteristics
, IEEE, 1995, 9 p.
SHABANA, Ahmed A., Dynamics of Multibody Systems, John Wiley & Sons, EUA, pp.
214,
1988.
RAVEN,
Automatic Control Engeneering,
Mc Graw
Hill, USA
.
SAVAGE, Paul G., Introduction to Strapdown Inertial Navigation Systems, Strapdown
Associates, Inc, 1985, 538 p.
SAVAGE, Paul G., Strapdown Inertial Navigation
Lecture Notes, Strapdown Associates,
Inc, 1985, 538 p.
TITTERTON, David, WESTON, John, Strapdown Inertial Navigation Tecnology, IEE, UK,
1997, 452 p.
UM,
Tae Yoon,
LEE,
Jang Gyu, PARK, Seong-
Taek,
PARK e Chan Gook, Noise Covariance
Estimation for Systems With Bias States
, IEEE, 2000, 8 p.
125
10.
APÊNDICES
126
10.1.
APÊNDICE 1:
ESTIMAÇÃO ÓTIMA
Em geral, dispõe-se de duas formas de se obter o valor de uma grandeza em um determinado
instante: a) partindo-se de um valor desta grandeza no instante anterior e utilizando-se uma
equação que modele a evolução no tempo desta grandeza e b) med
indo
-se diretamente esta
grandeza. No entanto, tanto a modelagem quanto a medição possuem imprecisões, daí, qualquer
que seja a forma de se obter o valor da grandeza, falar-se em estimação. Na estimação ótima,
procura
-se, levando-se em consideração tanto a informação proveniente da modelagem quanto a
da medição, obter-se uma estimativa o mais próxima possível do valor real, minimizando um
critério ligado a esse afastamento.
Primeiramente será desenvolvida estimação por mínimos quadrados, para o caso estático ou
invariante no tempo, como base para o entendimento da estimação ótima. Em seguida, a idéia
será estendida para a estimação ótima de um processo variante no tempo, comumente conhecida
como Filtragem de Kalman .
Este apêndice foi baseado nos trabalhos
de
FRANKLIN e POWELL, em 1997, de
LABARRERE, KRIEF
e GIMONET e de
HERMERLY
.
10.1.1.
MÉTODO DOS MÍNIMOS Q
UADRADOS
O método dos mínimos quadrados foi descoberto em 1795 por um jovem matemático
chamado K. F. Gauss, que estudava o movimento dos planetas e dos cometas. Este método,
utilizado para determinação de parâmetros orbitais a partir de medidas associadas a ruídos, foi
objeto de um grande número de aperfeiçoamentos. No seu livro, Theoria Motus Corporum
Coelestium , publicado em 1809, Gauss elencou um certo número de idéias, à base de modernas
técnicas de estimação: ele se questionou quanto ao número mínimo de observações necessárias e
constatou que, em presença de erros de medida, é necessário um número maior de observações.
Na tentativa de obtenção dos parâmetros desejados, o método dos mínimos quadrados
forneceu uma solução que minimizava o afastamento entre a solução desejada, ou ideal, e a
solução obtida com estes parâmetros. O sucesso deste método reside essencialmente na forma
quadrática de seu critério de qualidade, que conduz a uma formulação simples: os parâmetros
desejados são, em geral, solução de um sistema de equações lineares. A adoção de um critério de
127
afastamento quadrático pode parecer arbitrária e limitada, mas o acréscimo da idéia de
coeficient
es de ponderação permite o aumento da abrangência do método para um grande número
de problemas. Ressalte-se, aqui, que o critério de afastamento quadrático representa a energia do
erro ou, usando a terminologia da estocástica, a variância deste. O método dos mínimos
quadrados clássico não pressupõe o conhecimento das características estatísticas do ruído de
medida. Os aperfeiçoamentos deste método, a partir do conhecimento das propriedades dos
ruídos, são devidos principalmente a Wiener e a Kalman.
O método dos mínimos quadrados será desenvolvido na sua forma global e na sua forma
recursiva. Seguir
-
se
-
á uma aplicação clássica que mostra a utilidade deste método.
10.1.1.1.
O CASO MONOVARIÁVEL
Consideremos, por exemplo, a determinação da aceleração da gravidade, a partir da
observação da queda de um corpo. A velocidade v está ligada ao tempo t pela seguinte relação
linear:
v = g.t
Na prática, não se tem acesso à verdadeira velocidade v, mas a uma medida v
m
que está
associada a um ruído de medida
b:
v
m
= v + b
Se as diferentes observações v
mi
da velocidade são representadas em um gráfico em função
do tempo, constata-se que os diferentes pontos não estão alinhados, muito embora, teoricamente,
as diferentes velocidades deveriam estar numa reta de coeficiente angular igual
a
g.
128
Como determinar a aceleração
g
? As diferentes medidas tomadas separadamente conduzem a
acelerações distintas. Pode-se, então, buscar uma reta v = g.t que leve em conta o conjunto das
observações e que minimize a distância vertical dos pontos referentes a cada observação a esta
reta, isto é, o critério de afastamento quadrático:
k
i
mi
i
vtg
1
2
).(C
C é uma função unimodal de g. O mínimo do critério é obtido, quando o gradiente de C em
relação a
g é nulo, ou seja:
0).(.2
1
k
i
mii
i
vtgt
g
C
donde, a melhor estimação para
g
é:
k
i
i
k
i
mi
t
vt
g
i
1
2
1
.
Generalizando, para o caso de k observações y
i
de uma expressão linear de parâmetro a e de
variável
x
i
, a melhor estimação de
a
, pelo critério do afastamento quadrático, é dada por:
0 1 2 3 4 5 6 7 8 9
10
0
10
20
30
40
50
60
70
80
90
100
Velocidade de corpo em queda livre x tempo - real em vermelho e medidas em azul
tempo em s
velocidade em m/s
FIG. 10.1
Medidas de velocidades e reta de coeficiente angular igual a g
129
k
i
i
k
i
ii
k
x
yx
a
1
2
1
.
(10.1)
O cálculo de
k
a
pode ser efetuado pela
EQ. 10.
1. Apesar de bastante simples, esta expressão
não é muito usual quando o número de observações k é muito grande. Neste caso,
k
a
aparece
como a relação entre duas somas que aumentam com o número de observações. Desta forma, é
preferível buscar
-se uma formulação recursiva.
Considere
-se os valores estimados
1k
a e
k
a , obtidos sucessivamente a partir de k-1 e k
observações.
Seja:
kkk
QPa .
, onde, comparando com
a EQ. 10.
1,
k
i
iik
k
i
i
K
yxQ
x
P
1
1
2
.
1
(10.2)
Pode-
se, portanto, escrever também:
111
.
kkk
QPa
D
a EQ. 10.
2
, tem
-
se:
kk
k
i
ii
k
i
iik
k
k
k
k
i
i
k
i
i
k
yxyxyxQ
x
P
xxx
P
...
11
1
11
2
1
2
1
1
2
1
2
(10.3)
A primeira equação da E
Q.
10.3
pode também ser escrita da seguinte forma:
1
2
1
..
kkkkk
PxPPP (10.4)
e, portanto:
11
...
kkkkkk
PxxPPP (10.5)
A partir da EQ. 10.
4
, pode
-
se exprimir uma expressão para
kk
xP . :
).1(
.
.).1(
2
1
1
2
11
kk
kk
kkkkkk
xP
xP
xPxPPP
(10.6)
130
Substituindo
a EQ. 10.
6 na EQ. 10.
5
, tem
-
se:
).1(
.
2
1
22
1
1
kk
kk
kk
xP
xP
PP
Assim, resumindo, as expressões obtidas são as seguintes:
kk
k
i
iik
yxyxQ ..
1
1
).1(
.
2
1
22
1
1
kk
kk
kk
xP
xP
PP
kkk
QPa .
Podemos, portanto, escrever:
kkk
kk
kk
kkk
k
i
ii
kk
kk
kk
yxQ
xP
xP
Pyxyx
xP
xP
Pa ..
).1(
.
...
).1(
.
1
2
1
22
1
1
1
1
2
1
22
1
1
Desenvolvendo a e
xpressão acima, tem
-
se:
).1(
..
...
).1(
.
.
2
1
32
1
11
2
1
22
1
11
kk
kkk
kkkk
kk
kk
kkk
xP
yxP
yxPQ
xP
xP
QPa
).1(
..
).1(
....
).1(
...
2
1
32
1
2
1
32
11
2
1
2
111
1
kk
kkk
kk
kkkkkk
kk
kkkk
kk
xP
yxP
xP
yxPyxP
xP
xPQP
aa
).1(
..
).1(
..
).1(
..
).1(
..
2
1
32
1
2
1
32
1
2
1
1
2
1
2
11
1
kk
kkk
kk
kkk
kk
kkk
kk
kkk
kk
xP
yxP
xP
yxP
xP
yxP
xP
xPa
aa
).(
).1(
.
1
2
1
1
1 kkk
kk
kk
kk
xay
xP
xP
aa
Chamando
).1(
.
2
1
1
kk
kk
xP
xP
de
k
K e observando, na
EQ. 10.
6, que
kkk
xPK . , tem-se as
seguinte
s fórmulas de recorrência:
).
.(
11 kkkkkk
xayKaa
).1(
.
2
1
1
kk
kk
k
xP
xP
K
e
(10.7)
11
..
kkkkk
PxKPP
A expressão recursiva que fornece o parâmetro estimado possui uma forma bastante
adaptada ao cálculo numérico. A estimativa no instante
k
é igu
al à estimativa no instante
k-1
mais
131
uma correção que depende da distância da reta estimada à nova observação y
k
. O
bservando
-
se
que
kkk
xPK .
, tem
-
se:
).
.(
.
11 kkkkkkk
xayxPaa
onde
kkk
xay .
1
representa o aporte de informação da o
bservação
y
k
;
).
.(
1 kkkk
xayx
é um
valor instantâneo do gradiente C, no instante k
;
P
k
é o peso associado a este novo valor. Ele
depende das estimações precedentes e decresce com o aumento de k. Quanto mais estimações se
fizer, melhores elas serão e menos associadas aos afastamentos entre a reta estimada e as
observações que, por fim, são essencialmente devidos aos ruídos das medidas. Será visto, na
interpretação estocástica dos mínimos quadrados, que
P
k
é uma medida do erro de estimação.
O algoritmo recursivo, dado pelas
EQ. 10.
7, supõe valores iniciais que podem ser calculados
a partir de k
0
observações pela formulação global, EQ. 10.
1.
Mas, caso se escolha um valor de
0
k
P suficientemente grande, a partir de um certo número de observações, a estimação é
independente da condição inicial
0
k
a , que, portanto, pode ser escolhida arbitrariamente e tomar,
em particular, o valor zero. De fato,
0
k
P é uma medida do erro de estimação verificado com a
s
k
0
primeiras observações. Em se inicializando de uma maneira qualquer, a incerteza sobre
0
k
a é
grande; da mesma forma que sobre
0
k
P . Escolhendo-
se
0
k
P grande, estar-
se
-á levando em conta,
sobret
udo, as medidas futuras; no entanto, um
0
k
P pequeno traduz que o algoritmo já está
bastante próximo do seu valor limite.
0
k
P é, portanto, um parâmetro que permite a regulagem da
convergência da estimação.
Estas propriedades são colocadas em evidência na resolução do exemplo da queda de um
corpo.
Os instantes de levantamento das medidas são aleatórios e distribuídos no período entre 0 e
10 segundos e, numa primeira etapa, a fim de analisar a convergência do algoritmo recursivo, o
ruído de medida é nulo. Neste caso, evidentemente, a inicialização da fórmula global aplicada ao
primeiro ponto de medida fornece imediatamente o valor exato da aceleração (
FIG. 10.
2). Se a
inicialização não é exata, a convergência a partir de um valor arbitrário
tomado igual a zero no
exemplo
, depende do valor inicial de
P
(
FIG. 10.
2
).
132
Em presença de ruído de medida, a evolução do parâmetro P, que não depende da medida
ruidosa, não é modificada (
FIG.
10.
3). Entretanto, a evolução da estimação depende do ruído,
sobretudo no início, para grandes valores de P (
FIG. 10.
4
).
0 1 2 3 4 5 6 7 8 9
10
0
1
2
3
4
5
6
7
8
9
10
Grafico das estimativas de g x tempo - sem ruido
tempo em s
g estimado
g correto
para P0=1000
para P0=1
para P0=0.1
para P0=0.01
para P0=0.001
0 1 2 3 4 5 6 7 8 9
10
0
0.05
0.1
Grafico de P x tempo - com ruido
tempo em s
P
para P0=1000
para P0=1
para P0=0.1
para P0=0.01
para P0=0.001
FIG. 10.2 Evolução da estimativa de g, na ausência de ruído de medida
FIG. 10.3 Evolução do parâmetro P, em presença de ruído de medida
133
10.1.1.2.
CASO MULTIVARIÁVEL
Esta formulação pode ser generalizada sem dificuldades para o caso multivariável, onde a
equação de medida
é dada por:
i
ni
niiii
bxaxaxay ......
221
onde
n
xxx ...,
21
representam n variáveis conhecidas;
n
aaa ...,
21
, os parâmetros
desconhecidos a estimar e
y, as observações associadas a um ruído b.
Em notação vetorial, escreve
-
se:
i
T
i
i
baxy .
O problema é encontrar-se, a partir de k observações y
i
, a melhor estimativa para o vetor a
,
pelo critério do afastamento quadrático
C:
k
i
k
T
i
i
axy
1
2
).(C
A solução é obtida escrevendo-
se que o gradiente de
C
, em relação a
a
, é nulo:
0 1 2 3 4 5 6 7 8 9
10
0
2
4
6
8
10
12
14
Grafico das estimativas de g x tempo - com ruido
tempo em s
g estimado
g correto
para P0=1000
para P0=1
para P0=0.1
para P0=0.01
para P0=0.001
FIG. 10.4
Evolução da estimativa de g, em presença de ruído de medida
134
0...
11
i
k
i
T
ik
k
i
T
ii
yxaxx
Se a matriz
k
i
T
ii
xx
1
.
não é singular, a solução é dada por:
i
k
i
T
i
k
i
T
iik
yxxxa ..
1
1
1
(10.8)
Se a matriz
k
i
T
ii
xx
1
. é singular, não há solução única. Esta eventualidade pode se produzir
se o número de observações for inferior ao número de parâmetros a determinar ou ainda se ao
menos duas variáveis do vetor
x
são linearmente dependentes.
De fato, se:
iiiii
bxaxay
221
..
, com
ii
xx
12
.
, tem
-
se:
iiii
bxaay
12
).
.(
e, portanto, não se pode deter
minar separadamente
1
a
e
2
a
, mas somente
2
.aa
i
.
Em notação matricial, considerando o vetor y
k
, cujos elementos são compostos pelas k
observações
y
i
e a matriz H
, cujas linhas são
T
i
x e que, à exceção do vetor dos ruídos b
, associa o
vetor das medidas com o vetor dos parâmetros que se quer estimas (a
k
), a estimativa pode ser
escrita da seguinte forma:
k
T
kK
T
k
k
yHHHa ...
1
(10.9)
Que é a solução da equação matricial aHy
k
k
. , usando o critério do afastamento
quadrático
C
, que fica definido da seguinte maneira:
).
.(
).( aHyaHy
k
k
T
k
k
C
Note
-se que esta solução é obtida, premultiplicando-se os dois membros da equação por
T
k
H
e resolvendo
-
se o sis
tema que possui, agora, o mesmo número de equações que o de incógnitas.
Como no caso monovariável, pode-se, a partir da
EQ. 10.
8, desenvolver uma formulação
recursiva, fazendo-
se:
135
kk
k
QPa .
, onde
k
i
i
i
k
k
i
T
ii
K
yxQ
xxP
1
1
1
.
.
k
k
kk
T
ii
kk
yxQQ
xxPP
.
.
1
1
1
1
Utilizando o lema da inversão matricial, cuja demonstração está no Apêndice 2 deste
trabalho, pode
-
se escrever:
1
1
111
...1.
k
T
kk
k
T
kk
kkk
PxxPxxPPP
Desta forma, tem
-
se as seguintes fórmulas de recorrência:
).
.(
1
1
k
T
k
kk
kk
axyKaa
11
..
k
T
k
kkk
PxKPP
e
(10.10)
1
11
)..1
.(
.
k
k
T
k
kkk
xPxHPK
No caso multivariável, pode
-
se fazer as mesmas considerações que no caso monovariável, no
que concerne a estrutura do estimador e sua inicialização. Pode-se notar, no entanto, que,
diferentemente da fórmula global, EQ. 10.8, as expressões da
EQ. 10.
10 não contêm inversão de
matriz, cabendo lembrar, aqui, que a expressão
)..1(
1
k
k
T
k
xPx é escalar.
O que se passará, então, no caso singular como foi relatado, para o qual não há solução
única? Na formulação global, a inversão da matriz fornece um índice de singularidade, ao passo
que as fórmulas recursivas da EQ. 10.10 conduzem, em qualquer caso, a uma solução que
depende das condições iniciais escolhidas e que pertence ao espaço solução.
10.1.1.3.
INTERPRETAÇÃO ESTOCÁ
STICA DOS MÍN
IMOS QUADRADOS
O método dos mínimos quadrados foi apresentado de forma determinística, sem se preocupar
com as propriedades estatísticas das variáveis ou com o(s) ruído(s). No caso de o ruído de
medição ser um ruído branco, gaussiano, de média nula, de va
riância
2
e independente das
variáveis
x, ou seja, 0][bE ,
IbbE
T
.].[
2
e
0].[
T
xbE
, pode-se caracterizar, no contexto
136
estatístico, a estimativa
k
a . Para isso, basta calcular-se a média e a variância do erro de
estimação
k
a
~
:
).
.(
.].[
~
1
k
k
T
kk
T
k
kk
yaHHHHaaa
bHHHa
kk
T
k
k
..].[
~
1
Como apenas os elementos do vetor
b
são aleatórios, a média, que é a própria expectância do
erro de estimação, pode ser calculada da seguinte maneira:
0][..].[]
~
[
1
bEHHHaE
kk
T
k
k
Assim, pode-
se concluir que, se o ruído de medição tem média nula, a estimativa
k
a
não tem
tendência a desviar
-
se, de forma que sua média é igual ao valor buscado.
Para o cálculo da variância do erro de estimação,
procede
-
se do seguinte modo:
]
~
.
~
[
~
T
kk
k
aaEP
11
).
.(
].
.[..).(
~
k
T
kk
T
k
T
kk
T
kk
XXXbbEXXXP
12
).(
~
k
T
kk
XXP
kk
PP .
~
2
Assim, a
matriz
P
k
, que, a cada iteração, intervém no cálculo, à exceção de um coeficiente,
é
a própria matriz de covariância do erro de estimação. Ela fornece a precisão da estimação.
Quando o número de observações tende a infinito, a variância do erro decresce sempre na direção
do zero, da mesma forma que P
k
. Na hipótese de um ruído de medição do tipo branco, de média
nula, os mínimos quadrados fornecem, portanto, uma estimativa sem tendência ao desvio e bem
consistente, ou seja, de variância tendente para o zero.
10.1.1.4.
MÍNIMOS QUADRADOS CO
M PONDERAÇÃO
Se todas as observações não têm a mesma precisão, uma vez que são afetados por ruídos
independentes, de magnitudes diferentes
i
, pode-se levá-los em conta na estimação através de
coeficientes de ponderação
i
, usando
-
se o seguinte critério:
k
i
T
i
ii
axy
1
22
).
.(
C
, com
i
i
1
137
A solução deste problema é obtida a partir do caso geral, substituindo-
se
i
y por
i
i
y
e
i
x por
i
i
x
, ou seja:
k
i
i
i
T
i
k
i
i
T
ii
k
yx
xx
a
1
1
1
2
.
.
Pode-
se, também, escrever
-
se na forma recursiva:
).
.(
11 k
T
k
kk
kk
axyKaa
11
..
k
T
k
kkk
PxKPP
e
(10.11)
1
1
2
1
)..
.(
.
k
k
T
k
k
k
kk
xPxxPK
Como pode-se observar, apenas o ganho K
k
, que corrige o estimador e a matriz de
covariância, é modificado.
10.1.1.5.
EXEMPLO DE APLICAÇÃO
Seja o seguinte sistema de equações:
2
7.4.3
6.3.2
Há mais equações do que incógnitas, não havendo, portanto, solução exata. No entanto,
pode
-
se buscar uma solução que minimize um critério de afastamento quadrático:
).
.(
).( aXyaXy
T
C
onde:
2
7
6
y
,
11
43
32
X
e
a .
Este é o problema clássico dos mínimos q
uadrados, cuja solução é dada por:
yXXXa
TT
..].[
1
138
De onde, tem
-
se:
66
,0
3
2
e
33
,2
3
7
Pode-se observar que a matriz X
T
X e sua inversa P são matrizes simétricas, positivas
definidas.
Neste caso simples, o cálculo recursivo não apresenta vantagem. Seria o caso, se o número
de equações fosse maior. Entretanto, a título de ilustração, utilizar-
se
-á a forma recursiva de
cálculo nas três equações.
Pode-se inicializar os parâmetros desejados como nulos e, a título de se escolher um valor
grande para a matriz P, to
mar
-
se
1000
0100
0
P .
Tem
-
se, para a primeira iteração, que corresponde à primeira equação:
3
2
1
x ,
6
1
y
,
231
,0
154
,0
1
2
1
k
k
e
822
,30118,46
118,46254,69
1
P
384,1
922,0
1
1
Para a segunda iteração:
4
3
2
x , 7
2
y ,
421
,1
196
,2
2
2
1
k
k
e
42
,904,13
04,1312,18
2
P
232,3
935,1
2
2
E, para a terceira iteração, tem
-
se:
1
1
3
x , 2
3
y ,
462
,1
056
,2
3
2
1
k
k
e
12
,459,5
59,565,7
3
P
20,2
49,0
3
3
Assim, após três iterações, obtém
-
se
0,49, no lugar de
0,66, e 2,20, ao invés de 2,33. Esta
diferença decorre da inicialização arbitrária que podem ser entendidas como decorrentes do
tratamento de medidas anteriores.
Três iterações, como foi visto, não são suficientes para dissipar o efeito da inicialização
utilizada.
Caso se tivesse escolhido
1000
0
01000
0
P
, obt
er
-se-
ia
uma melhor solução:
315
,2
64
,0
139
10.1.2.
O FILTRO DE KALMAN
O filtro de Kalman resolve de maneira elegante o problema da filtragem linear. Utilizando
-
se
a noção de estado, o filtro de Kalman se apresenta sob a forma de um sistema de equações
diferenciais ou recorrentes, o que facilita a resolução com o auxílio de uma calculadora ou de um
computador. Sua realização, bem adaptada que é ao tratamento numérico em linha, fornece não
apenas a estimação ótima, mas também a variância do erro de estimação.
O filtro de Kalman leva
a estimação ótima aos sistemas não estacionários, em presença de condições iniciais e entradas
determinísticas. É uma ferramenta básica no domínio aeroespacial, onde é particularmente
aplicada, seja para a determinação de órbitas,
seja para a navegação.
Após a apresentação do modelo matemático do sistema, as formulações básicas do filtro
discreto e contínuo serão estabelecidas. O filtro teoricamente estável pode divergir, quando da
implementação numérica. A fim de contornar este problema, diferentes caminhos serão
propostos.
10.1.2.1.
POSIÇÃO DO PROBLEMA
O problema da estimação do estado x
(t)
de um sistema dinâmico submetido a entradas
determinísticas e aleatórias, a partir de medidas z
(t)
associadas a ruídos, pode ser dividido,
basicamente
, em duas etapas. Entre uma medida e outra, o valor do vetor de estados é atualizado
através da equação de modelagem, ou equação dinâmica ou ainda planta, do sistema. É a
chamada atualização no tempo, ou estimação preditiva, ou simplesmente predição. Quand
o é feita
uma medida, esta informação é levada em consideração juntamente com a da estimação preditiva,
sendo, então, realizada a atualização por medida, ou estimação corrente ou ainda simplesmente
estimação.
No caso discreto, chamar-
se
-á de x
k/
a melhor estimativa de x
k
, uma vez realizadas as
observações
z
1
, z
2
... z
, com
k >
correspondente à predição;
k =
correspondente à estimação.
O objetivo final é obter-se uma estimativa ótima (estimação corrente), representada por )(
k
tx
no
caso contínuo e por
kk
x
/
no caso discreto, baseada numa estimativa obtida através da equação da
dinâmica do processo (estimação preditiva), representada por
)(
k
tx
no caso contínuo e por
1/ kk
x
140
no discreto, e em parâmetros de saída obtidos por um procedimento de medição, simbolizados
por
)(
k
ty
no caso contínuo e por
kk
y
/
no discreto.
O filtro clássico se aplica aos sistemas dinâmicos lineares, contínuos ou dis
cretos com ruídos
de medição brancos. No entanto, formulações foram desenvolvidas de forma a se poder eliminar
tais restrições. Neste trabalho, será apresentada tão somente a formulação para o caso de os
ruídos de medida e do modelo do processo não serem i
ndependentes, uma vez que tal formulação
foi utilizada na estimação dos desalinhamentos e dos bias dos acelerômetros e dos giroscópios.
10.1.2.2.
O MODELO MATEMÁTICO
A evolução do estado do sistema é descrita por um sistema de equações diferenciais:
)()()(
).
()( tvtutxtFtx
(10.12)
onde:
x
é o vetor dos estados, de dimensão n.
F(t)
é uma matriz função de
t
, de dimensão n x n.
u
é o vetor de entradas, função de
t, conhecido.
v
é um ruído branco gaussiano, de dimensão n, de média nula, isto é,
E[
v
(t)]=0
t e
covariância
E[
v
(t).
v
T
(t)]=Q(t).
(t
- )
, onde
Q(t)
é uma matriz definida não negativa.
O estado inicial é aleatório, de características estatísticas conhecidas, gaussiano, de média
E[
x
(t
0
)]=
m
0
, de covariância
E[(
x
(t
0
)-m
0
). (
x
(t
0
)-m
0
)
T
(t)]=
0
e independente do ruído
v.
O estado do sistema é observado através de m medidas
z(t)
, ligadas ao estado
x(t)
pela
equação:
)()(
).
()( twtxtHtz
(10.13)
onde
H(t)
é uma matriz função de t, de dimensão m x n e w, um ruído branco gaussiano, de
dimensão m, independente de v
(t)
e de x
(t
0
), de média nula e de covariância
E[
w
(t).
w
T
(t)]=R(t).
(t
- )
, onde
R(t)
é uma matriz definida positiva.
Este modelo é obtido seja através das leis físicas que regem o sistema, seja através da
aplicação de técnicas de identificação experimental.
O modelo discreto que é utilizado de forma mais corrente é obtido seja diretamente através
de uma modelagem discreta, seja pela discretização do modelo contínuo.
141
As equações tomam a seguinte forma:
kkk
k
k
vuxFx .
1
(10.14)
kkk
k
wxHz . (10.15)
onde
v
k
e
w
k
são ruídos pseudo
-
brancos gaussianos, de mé
dia nula, tais que:
jkwvE
RwwE
QvvE
kj
T
jk
kj
k
T
jk
kj
k
T
jk
,.0].[
.].[
.].[
onde
kj
é o delta de kronecker:
kj
=
O estado inicial x
0
é também uma variável gaussiana, independente dos ruídos u
k
e v
k
, de
média
m
0
e de matriz de covariância
0
.
10.1.2.3.
EXEMPLO DE PROBLEMA
DE FILTRAGEM
Consideremos o lançamento de um foguete. As equações de sua trajetória, obtidas pela
mecânica de vôo, podem ser escritas da seguinte forma:
)()()(
).
()( tvtutxtFtx
onde:
u
(t)
representa os comandos e
v
(t)
as perturbações, como o vento, por exemplo.
As condições iniciais são supostas conhecidas. Na
FIG. 10.
5 tem-se a representação da
trajetória esperada, no que diz respeito à altura, segundo a modelagem do processo, da trajetória
real, em altura, das medidas de altura do foguete realizadas, bem como as alturas reais
correspondentes a cada uma destas medidas.
1 k = j
0 k j
142
Com a ajuda de um radar, é possível medir a posição do foguete, que se liga ao vetor de
estados do sistema, através da
EQ. 10.16
:
)()(
).
()( twtxtHtz
(10.16)
onde
w
(t)
representa os ruídos de medida. Sabendo-se que apenas os comandos u
(t)
e as
observações
z
(t)
são acessíveis, deseja-se determinar os estados, ou seja, a posição exata do
foguete, a fim de corrigir sua trajetória:
Na ausência das perturbações v
(t)
, ou seja, no caso de a modelagem do processo ser perfei
ta,
a
EQ. 10.16
toma a seguinte forma:
)()(
).
()( tutxtFtx
(10.17)
A
EQ. 10.
17
é inteiramente determinística e conhecida. O vetor de estados x
(t)
do foguete
pode, desta forma, ser calculado, através da resolução da mesma.
H(t).
x
(t
) fornece uma medida
exata da posição, ao passo que
z
(t)
fornece uma medida com erro. Neste caso, a observação não é
útil na determinação do estado.
Em havendo perturbações v
(t)
, isso significa que o modelo não traduz exatamente o
processo, o que é geralmente o caso, pois, na prática, sempre há deficiências na modelagem,
decorrentes de simplificações ou de perturbações não previstas. Neste caso, os estados são função
destas perturbações e a observação contribui na determinação da trajetória do foguete, de sorte
que o valor das
contribuições depende da magnitude dos ruídos
v
(t)
e
w
(t)
.
A partir dos valores de u
(t)
e z
(t)
, o filtro deve fornecer uma estimação dos estados e de toda
a combinação linear dos mesmos, como, em particular, das medidas.
Legenda:
posição exata
posição medida
altura prevista pelo modelo
altura real
FIG. 10.5
Trajetórias ideal e real, em altura, do foguete
h
t
143
10.1.2.4.
A FORMULAÇÃO DO FILT
RO DE KALMAN
O Filtro de Kalman é uma generalização do método dos mínimos quadrados recursivo,
acrescentando a possibilidade de o modelo ser variante no tempo.
A estimativa da variância mínima é obtida através da média condicional de x
(t)
, dadas as
observações
z( ), que, no caso gaussiano, é igual à estimativa ótima linear. Busca-se, portanto, a
estimativa de x
(t)
, função do conjunto de observações: z(t) = { z( ), 0 <
< t }, representada por
))
(( tzx
, que minimiza o critério do afastamento quadrático:
)))]
(()(
.(
.
)))
(()(
[(
)( tzxtxAtzxtxEtP
T
Ou seja, busca-
se
E[x/z]
, que é a média condicional do vetor de estados x
(t)
, dadas as
observações
z( ), para a qual, partindo-
se da identidade
][]/[ xEzxEE
, pode
-
se escrever:
]/)
.(
.)
[()]
.(
.)
[(
zxxAxxExxAxxE
TT
ou:
]/[..]/[
)]
/..
[(
))
/(
.(
.
))
/((]/)
.(
.)
[(
zxEAzxEzxAxEzxExAzxExzxxAxxE
T
T
TT
(10.18)
Para minimizar o critério, basta minimizar
a EQ. 10.18
que é sempre positiva.
Este mínimo é
obtido por:
)]
(/)([)( tztxEtx
Esta estimativa não tem tendência ao desvio, ou seja:
][
)]
(/)([][ xEtztxEExE
A formulação do filtro de Kalman é portanto obtida simplesmente pelo cálculo da média
condicional.
Nota:
A estimativa é independente da matriz de ponderação A. Portanto, não é possível escolher-
se
pesos que permitam a estimação de certas variáveis de estado melhor que de outras.
Antes de est
abelecer
-se rigorosamente o filtro de Kalman, serão obtidas diferentes fórmulas,
através de considerações elementares, as quais apresentam a vantagem de permitirem uma melhor
compreensão. A que será apresentada na seqüência será útil na implemetação do fil
tro.
10.1.2.4.1. DETERMINAÇÃO INTUITI
VA DO FILTRO
144
A fim de se chegar à formulação definitiva do filtro, a estimação será considerada em dois
instantes diferentes, um antes e outro depois da medida z
k+1
. Suponha-se que o estado x
k
, no
instante
k, seja perfeitamente conhecido; assim, o estado no instante
k+1
é dado pela equação de
evolução:
kkk
k
k
vuxFx .
1
Uma vez que o ruído
v
k
é branco, de média nula, independente dos
x
i
precedentes, sua melhor
predição é nula, enquanto que a melhor
predição
do estado
x
k+1
é d
ada por:
kk
k
k
uxFx .
1
A variância do erro de predição é igual à deste ruído v
k
(ruído decorrente de imperfeição na
modelagem do processo):
k
T
kk
T
kkkkkk
QvvExxxxE ].[])
).(
[(
/11/11
Caso, no instante k, não se disponha do valor preciso do estado, mas tão somente u
ma
estimação
kk
x
/
, não havendo mais observações entre os instantes k e
k+1
, não é possível
melhorar esta estimação e a melhor predição (também chamada, como visto, de estimação
preditiva ou de atualização no tempo) é dada por:
kkk
k
kk
uxFx
//1
.
(10.19)
A predição da variância do erro de estimação
P
k+1/k
pode ser calculada da seguinte forma:
T
kkkkkkkk
T
kkkkkkkkkkkk
T
kkkkkk
kk
vxxFvxxFE
uxFvuxFuxFvuxFE
xxxxEP
))
.(
.))
.(
).().(.).().(
])
).(
[(
//
//
/11/11
/1
Como os ruídos de medida, v
k
, e os ruídos da modelagem do processo, w
k
, não são
correlacionados, de sorte que
kk
x
/
e
w
k
tampouco são correlacionados, o produto cruzado dos
termos é nulo e, portanto, pode-se escrever o seguinte:
T
kk
TT
kkkkkk
T
kk
TT
kkkkkk
kk
vvEFxxxxEF
vvFxxxxFEP
..)
).(
(.
)..)
).(
.(
//
//
/1
Como
kk
T
kkkkkk
PxxxxE
/
//
)
).(
(
e
k
T
kk
QvvE .
, tem
-
se, portanto:
k
T
kkkkkk
QFPFP ..
//1
145
A predição da variância do erro de estimação depende, portanto, da variância do erro no
instante
k, P
k/k
, que é atualizada no tempo até o instante
k+1
, e da variância do ruído do modelo
Q
k
.
A predição da observação pode ser obtida através dos valores da predição dos estados, ou
seja:
kk
k
kk
xHz
/1
1
/1
.
Uma vez efetuada a medida z
k+1
, no instante
k+1
, a diferença entre o valor medido z
k+1
e o
valor predito
kk
z
/1
fornece uma indicação do erro de estimação, a partir do qual pode-
se
melhorar a
estimação, caso se conheça a variância dos ruídos. Assim:
-
se o ruído de medida é nulo, a melhor estimativa de
H
k+1
.x
k+1
é dada pela medida de
z
k+1
;
- se a predição da variância do erro P
k+1/k
é nula, não há erro de predição e a melhor
estimativa de
1/1 kk
x é dada pela predição, sem levar em conta a observação z
k+1
. É o caso de
ruído
v
k
nulo e condições iniciais absolutamente precisas;
- se a variância do ruído e P
k+1/k
são diferentes de zero, efetuar-
se
-á uma correção
proporcional ao afast
amento entre o valor medido
1k
z
e o valor predito
kk
z
/1
:
)
.(
/11
1
/11/1
kkk
k
kkkk
zzKxx
onde o ganho
K
k+1
depende das variâncias dos ruídos.
No método dos mínimos quadrados com ponderação (item 10.1.1.4), foi visto que uma no
va
medida
z
k
se traduzia por uma correção da estimação; estimação esta que dependia da precedente
e da variância do ruído. Esta é a idéia de
kk
z
/1
, que é a medida prevista, baseada não numa
medição propriamente dita, mas no valor predito dos estados (
kk
x
/1
). Aplicando, portanto, as
fórmulas correspondentes a uma iteração, tem
-
se:
)
.(
/11
1
/11/1
kkk
k
kkkk
zzKxx (10.20)
kkkkkkkk
PHKPP
/111/11/1
..
e
(10.21)
1
11/111/11
)..
.(
.
k
T
kkkk
T
kkkk
RHPHHPK (10.22)
Dispõe
-se, assim, da estimativa e de sua variância no instante
k+1
, feitas as observações, ou
medidas,
z
1
... z
k+1
.
As
EQ. 10.
20 à 10.22
constituem as equações do filtro de Kalman discreto.
146
10.1.2.4.2.
O FILTRO DE KALMAN D
ISCRETO
Relembrando as equações do modelo:
kkk
k
k
vuxFx .
1
(10.23)
kk
k
k
wxHz . (10.24)
onde
v
k
, w
k
e
x
0
são variáveis gaussianas independentes, tais que:
kj
k
T
jkk
kj
k
T
jkk
RwwEwE
QvvEvE
xmxE
.].[0][
.].[0][
)
cov(
][
0
0
0
0
Os estados x
k
e a observação z
k
, que podem ser obtidas de forma linear, a partir dos ruídos v
,
w
e das condições iniciais gaussianas, são
também gaussianos.
)
.(
/11
1
/11/1
kkk
k
kkkk
zzKxx ,
com:
1
11/111/11
)..
.(
.
k
T
kkkk
T
kkkk
RHPHHPK
E a variância do erro de estimação:
kkkkkkkk
PHKPP
/111/11`/1
..
(10.25)
A
EQ. 10.25
pode também ser escrita sob a seguinte forma simétrica:
T
kkk
T
kkkkkkkk
RRKHKIPHKIP
11111/1111`/1
..).
.().
.(
(10.26)
A
EQ. 10.
26
fornece a variância do erro de estimação, quando a estimativa é linear, para
qualquer valor do ganho K
k+1
. Tem-se ainda que P
k+1/k+1
, que é uma matriz simétrica positiva
definida, aparece como a soma de duas matrizes simétricas positivas definidas. Esta pr
opriedade
pode ser interessante, por ocasião da implementação.
Finalmente, pode
-
se escrever as equações do filtro de Kalman discreto:
k
T
kkkkkk
kkkkkk
k
T
kkkk
T
kkkk
kkk
k
kk
kk
k
k
k
kkkk
QFPFP
PHKIP
RHPHHPK
uxFx
xHzKxx
..
).
.(
)..
.(
.
.
).
.(
/`/1
/1111`/1
1
11/111/11
//1
/1
1
1
1
/11/1
(10.27)
tendo como condições iniciais:
147
0
0
0/0
0
00/0
)(
)(
x
Cov
P
mxEx
Esta formulação do filtro evoca
uma série de observações:
- O filtro obtido é linear e pode ser esquematizado pela F
IG. 10.
6. Ele permite um cálculo
recursivo no tempo real de estimação, sem a necessidade de armazenamento das observações e
comandos passados.
- Pode-se distinguir, nas equações do filtro, antes de cada observação, a
predição
, segundo a
qual a evolução da estimativa é descrita através das mesmas equações do sistema:
kk
k
kk
kkk
k
kk
xHz
uxFx
/1
1
/1
//1
.
.
E, depois de cada observação, a
estimação
.
- Note-se que, na resolução do filtro de Kalman discreto, supõe-se a existência da inversa da
matriz
11/11
..
k
T
kkkk
RHPH . Esta inversa não tem que necessariamente existir no caso do
problema singular (R
k
= 0). A formulação obtida pode ser utilizada neste caso, contanto que a
invers
a seja substituída pela pseudo
-
inversa.
10.1.2.4.3.
O FILTRO DE KALMAN C
ONTÍNUO
Agora, as equações de evolução e de observação são ambas contínuas:
)()()(
).
()( tvtutxtFtx
(10.28)
)()(
).
()( twtxtHtz
(10.
29)
K
k+1
Retardo
H
k+1
F
k
u
k
+
+
1/1 kk
x
kk
x
/
kk
x
/1
kk
z
/1
1k
z
+
-
FIG. 10.6
Estrutura do filtro discreto
148
com:
0
).]
(
).
([
)]
(
).
([
)]
(
).
([
))
(
cov(
)]
([
)(
).
(
)]
(
).
([0
)]
([
)(
).
(
)]
(
).
([0
)]
([
00
0000
txtwEtxtvEtwtvE
txmtxE
ttRtwtwEtwE
ttQtvtvEtvE
TTT
T
T
Na hipótese de os ruídos brancos v
(t)
e w
(t)
serem ruídos gaussianos, a estrutura do filtro
ótimo pode ser obtida a partir do caso precedente discreto, passando
-
se ao limite. As equações do
filtro de Kalman contínuo podem ser obtidas mais simplesmente, substituindo-se a hipótese
gaussiana pela busca de um filtro linear sem tendência ao desvio, com variância mínima.
O filtro linear é da forma:
)(
).
()(
).
()(
).
()( tvtKtutBtxtAtx
(10.30)
E a estimativa é sem tendência ao desvio, ou seja:
)]
([
)]
([
)]
([
)]
([
txEtxE
txEtxE
Assim, tomando
-
se a média da EQ. 10.
28 e EQ. 10.30
, tem
-
se:
)(
)]
([
).
(
)]
([
).
(
).
()(
).
(
)]
([
).
( tutxEtFtxEtHtKtutBtxEtA
Donde, pode-
se escrever:
ItB
tHtKtFtA
)(
)(
).
()()(
E a expressão do filtro:
)]
(
).
()(
).[
()()(
).
()( txtHtztKtutxtFtx
(10.31)
onde a matriz de ganho
K(t)
deve minimizar a variância do erro de estimação xxx
~
. Este
erro,
x
~
, evolui segundo a equação diferencial
EQ. 10.32
, obtida a partir das
EQ.
10.28,
EQ.
10.29 e 10.31
:
)(
).
()()(
~
)).
(
).
()(()(
~
twtKtvtxtHtKtFtx
(10.32)
)(
~
tx é, portanto, a saída de um sistema linear que tem como entrada o ruído branco v
(t)
-
K(t).
w
(t)
, que tem como variância
Q+KRK
T
.
A variância
P
do erro
x
~
é a solução da equação diferencial:
TT
KRKQHKFPPHKFtP ..).
.().
.()(
(10.33)
149
A
EQ. 10.33
pode também ser escrita reagrupando
-
se todos os termos em K:
TTTTT
RHPKRRHPKQPHRHPFPPFtP )..(.
).
..(......)(
111
Pode-se mostrar que, para minimizar a variância do erro, basta minimizar
P
, o que equivale
a escolher:
)(
).
(
).
()(
1
tRtHtPtK
T
Assim, tem
-
se as seguintes equações do filtro:
)(
).
(
).
()(
)()(
).
(
).
(
).
(
).
()(
).
()(
).
()(
))
(
).
()(
).(
()()(
).
()(
1
1
tRtHtPtK
tQtPtHtRtHtPtFtPtPtFtP
txtHtztKtutxtFtx
T
TT
(10.34)
com as
seguintes condições iniciais:
00
00
)(
)(
tP
mtx
A equação de evolução de
P(t)
é uma equação de Ricatti que pode ser escrita da seguinte
forma:
)()(
).
(
).
()(
).
()(
).
()( tQtKtRtKtFtPtPtFtP
TT
A estrutura do filtro de Kalman contínuo pode ser representada conforme a
FIG. 10.
7. Pode-
se encontrar aí ainda a dinâmica do sistema corrigida pelo erro de estimação da medida:
10.1.2.4.4.
GENERALIZAÇÃO DO MOD
ELO DO PROCESSO
FIG. 10.7
Estrutura do filtro contínuo
K
(t)
H(t)
F(t)
u
k
+
+
)(tx
)(tx
)(tz
)(tz
+
-
+
150
As fórmulas precedentes obtidas são válidas para o sistema descrito no item 10.1.2.1, que
supõe, em particular, ruídos associados aos estados independentes dos ruídos associados às
observações, ou medidas, ruídos brancos associados às observações, e uma matriz de covariância
dos ruídos associados às observações positiva definida. Estas restrições podem ser retiradas,
modificando
-se as equações do filtro. O filtro de Kalman pode até mesmo ser modificado,
visando a utilização em sistemas não lineares ou em modelos mal definidos. Neste trabalho será
apresentado tão somente a formulação que permite o uso de ruídos de medidas e de modelagem
correlacionados, uma vez que tal formulação foi a empregada na estimação dos desalinhamentos
e bias dos acelerômetros e dos giroscópios.
Na generalização que será levada a efeito na seqüência, visando uma melhor facilidade de
compreensão, partir-
se
-á da associação de estimação linear com a idéia da projeção de um vetor
no plano que contém as observações.
10.1.2.4.4.1.
RUÍDOS CORRELACIONAD
OS
10.1.2.4.4.1.1.
ESTIMAÇÃO LINEAR COM
O UM PROBLEMA DE PRO
JEÇÃO
Um estimador linear
g(Y)
, que leva em consideração n observações
Y(n)
, tem a seguinte
forma:
)(....)2(.)1(.)(
21
nYYYYg
n
(10.35)
d
e
onde, fazendo
-
se
0
=-
1 e
Y(0)=X, tem
-
se:
n
i
n
i
ji
n
i
i
n
i
i
jYiYE
iYEiYXEYgXE
0 0
2
0
2
1
2
)]
(
).
([..
)(.)(.]
))
(
[(
e, portanto, a minimização de E
[(
X-g(Y
))
2
] requer apenas o cálculo de médias e covariâncias,
ou seja, primeiro e segundo momentos. Estimadores não lineares usualmente requerem o cálculo
de momentos de ordem s
uperior.
Consi
derando
-
se o caso particular da EQ. 10.35
para
n=1
, tem
-
se:
151
)]
1([.
)]
1(.[..2][
)]
1(.)1(...2[]
))
1(.
[(
]
))
(
[(
22
11
2
22
11
22
1
2
YEYXEXE
YYXXEYXEYgXE
(10.36)
O
1
que minimiza a expressão é obtido igualando-se a derivada da expressão em relação a
1
a zero:
0
)]
1([..2
)]
1(.[.2
0
)]
1([.
)]
1(.[..2][]
))
(
[(
2
1
22
11
2
1
2
1
YEYXE
YEYXEXEYgXE
0
)]
1([,
)]
1([
)]
1(.[
2
2
1
YE
YE
YXE
(10.37)
Seja:
1
0
1
, onde
10
22
0
22
0
.
)]
1(.[
)]
1([
],
[
YXE
eYEXE
(coeficiente de correlação).
A estimativa
X
é, portanto, dada por:
)1(.)(
1
0
YYgX
(10.38)
O erro de estimação é dado por:
10
0
1
0
)1(
.)1(.
~
YX
YXXXX
(10.39)
Pod
e-
se, assim, avaliar:
0....
)]
1([.
)]
1(.[
)1(.)1(.
)]
1(
).
[()]
1(.
~
[
1010
2
1
0
YEYXE
YXYXEYXXEYXE
(10.40)
Ou seja,
X
~
e
Y(1)
são não correlacionados.
Interpretação d
a EQ. 10.40
:
Sejam os vetores
v
0
e
v
1
.
Denotando
-
se por
1
v
u
o vetor unitário na direção
v
1
, a projeção
0
v
de
v
0
em
v
1
é dada por:
1
1
0
1
1
000
.
).
cos
.(
).
cos
.(
1
v
v
v
vuvv
v
(10.41)
152
Comparando
-
se
a EQ. 10.40 com a EQ. 10.41 e interpretando-se as variáveis aleatórias X e
Y(1)
como sendo vetores v
0
e v
1
, com normas
][
2
0
XE
e
)]
1([
2
1
YE
, e formando um
ângulo
=arcos
, conclui
-
se que a melhor estimativa de
X
pode ser entendida como a projeção de
X
em Y(1)
.
Teorema 1
:
Sejam
n
X
X
X
1
e
Y
Y
Y
1
variáveis aleatórias, com componentes possuindo média nula e
variância finita. Então, para
j=1,2...n
, há uma única variável aleatória
X
j
, tal que:
a)
)(YSX
j
, ou seja, uma estimativa de X que pertença ao espaço dos componentes de Y; e
b)
)()( YSXX
jj
, ou seja, interpretando )(
jj
XX
como um vetor, este é ortogonal ao
plano que contém os componentes de Y.
Sendo
n
X
X
X
1
a melhor estimativa baseada no critério do afastamento quadrático e
E[
Y.Y
T
] não singular, tem
-
se:
YYYEYXEX
TT
.].[
].
.[
1
(10.42)
Verificação: Como se está supondo estimadores lineares, a estimativa
X
pode ser escrita
como se segue:
YMX .
, onde
M
é uma matriz
n
x
.
Assim, devido a (b), para quaisquer
n
R
e
R
, tem
-
se:
0].[.].[
0
].
).
.
[(
.0]).
).(
.
.(
[
TT
TTTTT
YYEMYXE
YYMXEYYMXE
(10.43)
Sendo a última implicação devida ao fato de que
e
são genéricos. Logo, se (E[Y.Y
T
])
-1
existir, da EQ. 10.43
, tem
-
se:
1
].[
].
.[
TT
YYEYXEM
Comentários sobre o teorema:
a)
A estimativa
X
pode ser considerada a projeção de
X
em
S(Y)
;
153
b)
Para o caso particular de n=1 e =1, isto é,
X=X
1
e
Y=Y
1
, tem-
se
][
].[
2
1
11
YE
YXE
M
T
,
conforme
visto nas EQ 10.
37 e 10.38
;
c)
Caso
i
xi
mXE ][
e
i
yi
mYE ][
, então:
x
T
yy
T
yx
mmYmYEmYmXEX
1
])
).(
[(
].
)
).(
[(
10.1.2.4.4.1.2.
FORMA R
ECURSIVA PARA A ESTI
MAÇÃO LINEAR
A forma recursiva é importante, por exemplo, em aplicações em tempo real. De modo a se
determinar tal forma, considere-se k leituras do vetor
Y
Y
Y
1
, definido pelo teorema 1, tenham
sido efetuadas:
)1(
)1(
)1(
1
Y
Y
Y
,
)2(
)2(
)2(
1
Y
Y
Y
, ...,
)(
)(
)(
1
kY
kY
kY
.
Seja também:
Espaço dos componentes de Y
X
X
~
X
FIG. 10.8
Estimativa de uma
grandeza interpretada como uma projeção
da mesma
154
S(Y
k
), com Y
k
=[Y(1) Y(2) Y(k)] o subespaço linear gerado pelas observações até o instante
k. Conforme visto no item anterior, a melhor estimativa
n
X
X
X
1
de X, dado Y
k
é dada pela
projeção de
X
em
S(Y
k
).
Seja ago
ra o cenário mostrado na FIG. 10.
9.
Assim, qualquer
Z
em
S(Y
k
)
pode ser escrito, de modo único, da seguinte forma:
Z=Z
1
+Z
2
(10.44)
Com
)(
11 k
YSZ
e
Z
2
= combin
ação linear de ...,
,2,1,
~
1/
iY
kk
i
(10.45)
Tomando
-
se agora:
21
ZZXZ
k
i
(10.46)
Como
kkk
iii
XXX
~
, pode
-
se escrever:
kkkk
iiii
XZZXXX
~
~
21
, com
)(
11 k
YSZ
e
)()
~
(
12 ki
YSXZ
k
.
Pode-
se, adicionalmente, escr
ever:
111
~
kkk
iii
XXX
, com
)(
1
1
ki
YSX
k
e
)(
~
1
1
ki
YSX
k
.
E, como a decomposição é única, tem
-
se:
1
1
k
i
XZ (10.47)
S(Y
k-1
)
1
/
k
k
Y
1
/
~
k
k
Y
k
Y
FIG. 10.9
Subespaço das observações
155
No que se refere a
Z
2
, na EQ. 10.46
, observando
-
se a
FIG. 10.10
, pode
-
se verificar que:
kk
iii
XZXX
~
2
1
Z
1
Portanto,
Z
2
é a projeção de
X
i
em
)
~
(
1/ kk
YS .
Desta forma, pode
-
se escrever:
))
~
((
1/
1
kkiii
YS
em
X
de
projeção
XX
kk
(10.48)
Logo, da EQ. 10.42, da EQ. 10.48 e empilhando
k
i
X , i=1, 2, ..., n, no vetor
k
X , pode-
se
escrever:
X
i
S(Y
k
)
S(Y
k-1
)
1k
i
X
k
i
X
~
X
i
k
i
X
~
1k
i
X
2
Z
FIG. 10.10 Interpretação vetorial de Z
2
156
1/
1
1/1/1/1
~
.
])
~
.
~
[
])(
~
.[(
kk
T
kkkk
T
kkkk
YYYEYXEXX (10.49)
ou ainda:
)
.(
])
~
.
~
[
])(
~
.[(
1/
1
1/1/1/1 kkk
T
kkkk
T
kkkk
YYYYEYXEXX (10.50)
Assim,
a EQ. 10.50 corresponde a uma fórmula recursiva para a determinação da estimativa
X, caso
1/ kk
Y
dependa apenas de
1/ kk
X .
10.1.2.4.4.1.3.
FILTRO DE KALMAN
Seja o seguinte modelo dinâmico:
k
k
k
k
k
k
k
k
k
k
vNxHy
wGxFx
..
..
1
(10.51)
onde:
kj
k
T
jk
T
kk
kj
k
T
jk
kj
k
T
jkk
kj
k
T
jkk
QwxEvxE
nados
correlacio
ruídos
SvwE
QvvEvE
RwwEwE
.].[0].[
)(.].[
.].[0][
.].[0][
(10.52)
De modo a facilitar a obtenção das equações do Filtro de Kalman, convém atentar para a
FIG. 10.11
, relativa às projeções de
x
k
em
S(Y
k-1
)
e
S(Y
k
).
S(Y
k
-
1
)
1
/
k
k
x
1
/
~
k
k
x
k
x
FIG. 10.11
Projeções de
x
k
em S(
Y
k-1
) e S(
Y
k
)
S(Y
k
)
k
x
k
k
x
/
~
k
k
x
/
157
A projeção
1/ kk
Y de Y
k
em
S(Y
k-1
) é igual à projeção de
H.X
em
S(Y
k-1
), uma vez que
)(
1kk
YSW
. Portanto, pode
-
se escrever:
1/
1/
.
kk
kk
xHy (10.53)
Como
1/1/
~
kkkkk
yyy
, tem
-
se:
kkkkkkk
kk
vGxHxHvGxHy
1/1/
1/
~
..).(
~
(10.54)
Pode-
se perceber que:
)
~
()()(
1/
1
kk
kkkkk
yS
em
x
de
ojeção
Pr
YS
em
x
de
ojeção
Pr
YS
em
x
de
ojeção
Pr
(10.55)
ou seja:
1/
1
1/1/
1/
1//
~
.
])
~
.
~
[])(
~
.[(
kk
T
kkkk
T
kk
kkkkk
yyyEyxExx (10.56)
O problema agora se resume em calcular
]
~
.[
1/
T
kk
k
yxE
e
]
~
.
~
[
1/1/
T
kkkk
yyE .
1) Primeiramente, tome
-se o cálculo de
]
~
.[
1/
T
kk
k
yxE
:
Como:
kkk
kk
vFxHy
1/
1/
~
.
~
(10.57)
e, de acordo com a figura
,
]
~
.
~
[]
~
.
~
[]
~
.[]
~
).
~
[(
]
~
.[
1/1/1/1/1/1/1/1/1/1/
T
kkkk
T
kkkk
T
kkkk
T
kkkkkk
T
kkk
xxExxExxExxxExxE
,
tem
-
se:
T
kk
T
T
kkkk
T
T
kk
T
T
kkk
T
kk
k
HPHxxEGvxEHxxEyxE .
].
~
.
~
[].
.[
].
~
.[]
~
.[
1/
1/1/1/
1/
(10.
58)
onde:
]
~
.
~
[
1/1/
1/
T
kkkk
kk
xxEP
(10.59)
2) Tome
-
se agora o cálculo de
]
~
.
~
[
1/1/
T
kkkk
yyE :
Inicialmente, a partir das condições 10.52, que, dentre outros pontos, define que
kk
xv
,
conclui
-
se que
1/ kkk
xv
, ou seja,
0]
~
.[
1/
T
kkk
xvE .
Logo, da EQ. 10
.57
, tem
-
se:
T
k
T
kk
T
T
kk
T
T
kkkk
T
kkkkkk
T
kkkk
GQGHPH
GvvEGHxxEH
vGxHvGxHEyyE
....
].
.[.
].
~
.
~
[.
].
~
...
~
.[]
~
.
~
[
1/
1/1/
1/1/
1/1/
(10.60)
158
Pode-
se, então, substituir
a EQ. 10.
60 e EQ. 10.
58 na EQ. 10.56
, obtendo
-
se:
).
.(
)....
.(
.
1/
1
1/1/
1// kk
k
T
k
T
kk
T
kk
kkkk
xHyGQGHPHHPxx (10.61)
A
EQ. 10.
61
ainda não está na forma recursiva. A fim de obtê-la, deve-se relacionar
kk
x
/1
com
kk
x
/
. Para tanto, retomando a interpretação das estimativas como projeções de vetores no
plano das observações, pode
-
se escrever:
Projeção de
x
k+1
= (Projeção de F
k
.x
k
em S(Y
k
))+(Projeção de G
k
.w
k
em S(Y
k
))
Ou seja:
kk
k
kk
k
kk
wGxFx
///1
.
(10.62)
Resta calcular o valor de
kk
w
/
:
Como o ruído do modelo w
k
, neste caso, é correlacionado com o ruído de medida v
k
, ou seja,
k
T
kk
SvwE ].[ , tem-se que w
k
não é necessariamente ortogonal a
S(Y
k
). Conseqüentem
ente,
T
kk
T
k
T
kk
T
k
k
k
T
k
k
NSNvwEvNwEywE .
].
.[]).
.(
[].[
. Desta forma, como
w
k
é ortogonal a
S(Y
k-1
)
,
a melhor estimativa de
w
k
dado
Y
k
é igual à melhor estimativa dado
1/
~
kk
y
e pode
-
se escrever:
kk
w
/
= Projeção de
w
k
em
1/
~
kk
y (10.63)
Assim, conforme o teorema 1, tem
-
se:
1/
1
1/1/
1/
/
~
.
])
~
.
~
[
])(
~
.[(
kk
T
kkkk
T
kk
kkk
yyyEywEw
(10.64)
Utilizando
-
se
a EQ. 10.57
, pode
-
se calcular
]
~
.[
1/
T
kk
k
ywE :
T
k
T
k
T
T
kk
T
T
kkk
T
kkkk
T
kk
k
FS
FS
FvwEHxwE
vFxHwEywE
.
.0
].
.[
].
~
.[
])
~
.
.(
[]
~
.[
1/
1/
1/
(10.65)
Desta form
a, substituindo
-se a EQ. 10.65
e
a EQ. 10.60
na EQ. 10.
64
, obtém
-
se:
1/
1
1/
/
~
.......
kk
T
k
T
kk
T
k
kk
yGQGHPHFSw (10.66)
Substituindo
-
se, pois,
a EQ. 10.61
e
a EQ. 10.
66 na EQ. 10.62
, tem
-
se:
).
.(
.........
1/
1
1//1/1//1 kk
k
T
k
T
kk
T
kkkk
T
kkkkkkkk
xHyGQGHPHFwGHPFxFx
(10.67)
159
ou ainda:
).
.(
1/1//1 kk
k
k
kk
k
kk
xHyKxFx
(10.68)
onde:
1
1/
/
1/
.........
T
k
T
kk
T
k
kk
k
T
kkkk
GQGHPHFwGHPFK (10.69)
é definido como o ganho.
Resta determinar uma fórmula recursiva para a matriz de covariância do erro de estimação,
]
~
.
~
[
/1/1
/1
T
kkkk
kk
xxEP .
Subtraindo
-
se
a EQ.
10.68
de
k
k
k
k
k
wGxFx ..
1
e lembrando que
1/
.
kk
k
xHy
k
k
kkkkk
k
k
vNxHxHvNxH .
~
..)..(
1/1/
, obtém
-
se:
k
kk
k
k
kk
kk
kk
vNKwGxHKFx ...
~
..
~
1//1
(10.
70)
Desta forma, pode
-
se escrever:
T
k
T
kk
kk
T
k
T
k
T
kk
k
T
k
T
k
T
kk
kk
T
k
T
kk
k
T
kk
T
kkkk
kk
T
kkkk
GwvENK
KFvwEGKNvvENK
GwwEGHKFxxEHKFxxE
].
.[..
.
].
.[..
].
.[..
].
.[..
].
~
.
~
[..]
~
.
~
[
1/1//1/1
(10.71)
ou seja:
T
k
T
kkk
T
k
T
kkk
T
k
T
kkkk
T
kkk
T
kkkkkkkk
GSNKKFSG
KNQNKGRGHKFPHKFP
......
..........
/1/1
(10.73)
As equações do Filtro de Kalman, para o sistema modelado segundo a EQ. 10.51 e 10.52
,
considerando ruídos de medida e de modelagem correlacionados, que foi o caso do problema
tratado neste trabalho, são as seguintes:
).
.(
1/1//1 kk
k
k
kk
k
kk
xHyKxFx
(10.74)
1
1/
/
1/
.........
T
k
T
kk
T
k
kk
k
T
kkkk
GQGHPHFwGHPFK (10.
75)
T
k
T
kkk
T
k
T
kkk
T
k
T
kkkk
T
kkk
T
kkkkkkkk
GSNKKFSG
KNQNKGRGHKFPHKFP
......
..........
/1/1
(10.76)
160
10.1.2.4.4.1.4. FILTRO DE KALMAN PARA SISTEMAS COM ENTRA
DAS
DETERMINÍSTICAS
Seja agora o sist
ema:
k
k
k
k
k
k
k
wGuBxFx ...
1
(10.77)
kkkk
k
vNxHy ..
(10.
78)
com
u
k
representando uma entrada determinística.
Uma vez que a única diferença entre as
EQ. 10.
77 e 10.78 e as equações 10.51 foi a entrada
determinística
u
k
, pode-se afirmar que haverá alteração na equação da expectância, ou seja, do
valor esperado, mas não haverá alteração na covariância do erro de estimação, e, portanto, as
equações de
K
k
e
P
k+1/k
não se alteram.
Desta forma, as equações do Filtro de Kalman para um sistema modelado segundo as
EQ.
10.
77 e 10.78
são as seguintes:
).
.(
.
1/1//1 kk
k
k
kkk
k
kk
xHyKuBxFx
(10.79)
1
1/
/
1/
.........
T
k
T
kk
T
k
kk
k
T
kkkk
GQGHPHFwGHPFK (10.80)
T
k
T
kkk
T
k
T
kkk
T
k
T
kkkk
T
kkk
T
kkkkkkkk
GSNKKFSG
KNQNKGRGHKFPHKFP
......
..........
/1/1
(10.81)
10.2.
APÊNDICE 2:
LEMA DA INVERSÃO MA
TRICIAL
CDCBA
T
..
111
(10.82)
BCDCBCCBBA
TT
..)..
.(
.
1
(10.83)
De
fato, pósmultiplicando-
se
a EQ. 10.82
por A, tem
-
se:
CDCABA
T
....1
11
Posmultiplicando
-
se por
B
, obtém
-
se:
BCDCAAB
T
....
1
(10.84)
Posmultiplicando
-
se por
C
T
, chega
-se a:
]..
.[
.........
11 TTTTTT
CBCDDCACBCDCACACB
161
Posmultiplicando por [
D+CBC
T
]
-1
, tem
-
se:
11
..]..
.[
. DCACBCDCB
TTT
Posmultiplicando por
CB
, resulta:
BCCBCDCBBCDCA
TTT
..]..
.[
.....
11
(10.85)
Subtraindo
-
se
a EQ. 10.85
de
B
, obtém
-
se:
BCCBCDCBBBCDCAB
TTT
..]..
.[
.....
11
(10.86)
Substituindo
-se a EQ. 10.
84 na parte esquerda da EQ. 10.86
, chega
-
se, finalmente, a:
BCCBCDCBBA
TT
..]..
.[
.
1
162
11.
ANEXO
163
FLUXOGRAMA DOS ALGORITMOS DE
ALINHAMENTO
,
NAVEGAÇÃO E ATITUDE
Início
Valores Iniciais:
;
0
0
0
;;;;;;;;;
)
cos(
.
0
)
cos(
.
)(
)1(
)1(
)1(
)1(
)1()1()1(0
n
n
n
D
E
N
n
l
nnnEN
l
ie
v
v
v
v
hLeRRRg
l
l
i_aling=1
f
b
; (
ib
)
b
leitura
dos acel. e dos g
iros
Alinhamento Gross
eiro
:
tan
cos
.
tan
cos
.
tan
cos
.
13
12
11
g
f
c
g
f
c
g
f
c
zz
yy
xx
g
f
c
g
f
c
g
f
c
z
y
x
33
32
31
3112321123
3113331122
3213331221
..
..
..
ccccc
ccccc
ccccc
l
b
C
;.;;
21
31
1
11
21
1
33
32
1
sen
c
c
tg
c
c
tg
c
c
tg
i_aling= i_aling +1
i_aling > 30
Saídas:
-
dados estatísticos dos giros e
acelerômetros;
-
,,
1
NÃO
SIM
164
i_alinf=1
aux
,
aux
,
aux
,
f
b
; (
ib
)
b
leitura
dos acel.,
dos giros e dos auxílios
Alinhamento Fino:
wGxAx ..
)13(
)13(
)33()33(
)33()33(
)33()33(
)19(
)13(
)13(
)13(
)33()33()33(
)33()33()33(
)33(
)33(
)33(
)19(
)13(
)13(
)13(
)(
)(
00
00
0
000
000
0
x
g
x
ac
xx
xx
l
xbx
x
x
ac
x
g
x
xxx
xxx
x
x
l
b
x
l
ib
x
x
ac
x
g
x
tw
tw
C
bias
bias
C
as
ib
as
ib
vxHz .
)(
)(
)(
.
00
00
00
)33()33()33(
)33()33()33(
)33()33()33(
tv
tv
tv
bias
bias
I
I
I
z
z
z
ac
g
aux
ac
g
xxx
xxx
xxx
ac
g
aux
onde:
l
b
l
b
ac
l
ib
b
l
b
ib
g
aux
aux
aux
fCfzCzz .;.;
Filtragem de Kalman:
T
k
T
kkk
T
k
T
kkk
T
k
T
kkkk
T
kkk
T
kkkkkkkk
T
k
T
kk
T
k
kk
k
T
kkkk
kk
k
k
kk
k
kk
GSNKKFSG
KNQNKGRGHKFPHKFP
GQGHPHFwGHPFK
xHyKxFx
......
..........
.........
).
.(
/1/1
1
1/
/
1/
1/1//1
i_
alinf
= i_
alinf
+1
i_alinf > imax
SIM
NÃO
1
Saídas do Alinhamento Fino:
l
bnnn
n
C
)1(
)1()1()1(
;;
2
NÃO
165
dt
CCC
tt
t
l
b
b
il
b
ib
l
b
l
b
...
Fim
2
)(.
)1(
)1(
)1(
)1(
)1(
)1(
)1(
L
tg
R
v
R
v
R
v
n
n
n
n
n
n
e
E
n
N
e
E
l
n
el
f
b
; (
ib
)
b
leitura
dos acel. e dos giros
b
ib
l
b
l
ib
b
l
b
l
C
fCf
.
.
n=1
tt
t
e
el
il
e
dt
gvfv .)2(
l
el
l
ie
l
il
n
n
l
ie
n
e
E
n
N
e
E
D
E
N
l
el
n
e
n
n
n
nn
n
ne
nE
nnn
nn
N
nnnDnn
sen
tg
R
v
R
v
R
v
sen
exc
R
R
sen
exc
exc
R
R
R
h
sensen
g
longitude
t
hR
v
LL
latitude
t
hR
v
altura
tvhh
nn
n
)(.
0
)
cos(
.
;
)(.
)(.1
;
)(.1
)1
.(
;
1
).2(.
0000059
,0)(.
0053024
,01
.(
780318
,9
)(.
)
sec(
.
);
(.
);
(.
2
1
22
0
2
3
22
2
0
2
0
22
1
1
1
1
11
11
1
n= n +1
n > nmax
SIM
NÃO
Saída:
h, , L, , ,
finais
Livros Grátis
( http://www.livrosgratis.com.br )
Milhares de Livros para Download:
Baixar livros de Administração
Baixar livros de Agronomia
Baixar livros de Arquitetura
Baixar livros de Artes
Baixar livros de Astronomia
Baixar livros de Biologia Geral
Baixar livros de Ciência da Computação
Baixar livros de Ciência da Informação
Baixar livros de Ciência Política
Baixar livros de Ciências da Saúde
Baixar livros de Comunicação
Baixar livros do Conselho Nacional de Educação - CNE
Baixar livros de Defesa civil
Baixar livros de Direito
Baixar livros de Direitos humanos
Baixar livros de Economia
Baixar livros de Economia Doméstica
Baixar livros de Educação
Baixar livros de Educação - Trânsito
Baixar livros de Educação Física
Baixar livros de Engenharia Aeroespacial
Baixar livros de Farmácia
Baixar livros de Filosofia
Baixar livros de Física
Baixar livros de Geociências
Baixar livros de Geografia
Baixar livros de História
Baixar livros de Línguas
Baixar livros de Literatura
Baixar livros de Literatura de Cordel
Baixar livros de Literatura Infantil
Baixar livros de Matemática
Baixar livros de Medicina
Baixar livros de Medicina Veterinária
Baixar livros de Meio Ambiente
Baixar livros de Meteorologia
Baixar Monografias e TCC
Baixar livros Multidisciplinar
Baixar livros de Música
Baixar livros de Psicologia
Baixar livros de Química
Baixar livros de Saúde Coletiva
Baixar livros de Serviço Social
Baixar livros de Sociologia
Baixar livros de Teologia
Baixar livros de Trabalho
Baixar livros de Turismo
