Escolar Documentos
Profissional Documentos
Cultura Documentos
65/2014
CAMPINAS
2014
i
FICHA CATALOGRFICA ELABORADA PELA
BIBLIOTECA DA REA DE ENGENHARIA E ARQUITETURA - BAE - UNICAMP
iv
Agradecimentos
Ao meu orientador, Prof. Dr. Janito Vaqueiro Ferreira, pela pacincia durante a pesquisa e pela
oportunidade de realizar este trabalho.
Aos colegas do LMA (Laboratrio de Mobilidade Autnoma) pelas discusses e opinies que
ajudaram no desenvolvimento deste trabalho.
Aos membros das bancas de qualificao e defesa por disponibilizarem seus tempos e conheci-
mentos para a anlise e melhoria deste trabalho.
minha famlia e amigos pelo apoio que me permitiu realizar este trabalho e pela companhia
nos bons e maus momentos.
vii
Resumo
Este trabalho tem como objetivo desenvolver um controlador para um quadrirrotor cujos parme-
tros so desconhecidos. Para o controle deste sistema, normalmente so utilizados controladores
avanados que dependem do conhecimento de parmetros do sistema ou controladores simples que
necessitam de ajuste manual dos ganhos. O controlador desenvolvido possui baixa complexidade e
capaz de ajustar seus parmetros automaticamente para minimizar uma funo de custo durante o
voo, sem necessitar de conhecimento de nenhum parmetro do sistema. Entretanto, este controla-
dor necessita dos estados do sistema, que no esto disponveis diretamente. Portanto, uma anlise
da aplicao de mtodos de filtragem para estimao destes estados realizada, comparando-se
diversos possveis modelos estocsticos. Contudo, a filtragem necessita de sensores calibrados cor-
retamente, o que levou criao de novos algoritmos para calibrao dos sensores utilizados. Os
algoritmos desenvolvidos nestas trs reas representam passos na direo de criar quadrirrotores
que podem operar em ambientes diversificados de modo robusto.
ix
Abstract
This works objective is to develop a controller for a quadrotor with unknown parameters. For this
systems control, usually advanced controllers that require knowledge of the systems parameters
or simple controllers that require manual gain tunning are used. The controller developed has low
complexity and is able to adjust its parameters automatically to minimize a cost function during
flight, without requiring knowledge of any systems parameter. However, this controller requires
the systems states, which arent available directly. Hence an analysis of the use of filtering methods
to estimate these states is conducted, comparing many possible stochastic models. Nonetheless,
the filtering requires correctly calibrated sensors, which led to the creation of new algorithms for
calibrating the sensors used. The algorithms developed in these three areas represent steps in the
direction of creating quadrotors that can operate in diverse environments in a robust way.
xi
Lista de Figuras
1.1 Bambu-cptero. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2
1.2 Mquina voadora de Leonardo da Vinci. . . . . . . . . . . . . . . . . . . . . . . . 3
1.3 Helicptero de Bothezat. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4
1.4 Multirotor utilizado pela empresa omstudios. . . . . . . . . . . . . . . . . . . . . 5
1.5 Diagrama de blocos do quadrirrotor e pesquisa desenvolvida. . . . . . . . . . . . . 7
xiii
Lista de Figuras
xiv
Lista de Tabelas
xv
Lista de Smbolos
Sistemas de coordenadas
Espaos
Smbolos do filtro
Zk (i|j) Medio prevista utilizando o estado aumentado Xk (i|j), ver Eq. (5.11).
xvii
Lista de Smbolos
zi|j Leitura esperada com base na estimativa xi|j , ver Eq. (5.5).
Smbolos do controle
t Derivada da poltica (xt ; ) com relao as parmetros da poltica , ver Eq. (4.6).
xviii
Lista de Smbolos
Parmetros da modelagem
xix
Lista de Smbolos
sR
s Leitura ideal do sensor s em S, ver Eq. (3.3).
xx
Lista de Smbolos
Probabilidade
Quaternions e operaes
[q] Representao matricial do produto de quaternions pela direita, ver Eq. (A.4).
[q] Representao matricial do produto de quaternions pela esquerda, ver Eq. (A.3).
qB
A Quaternion de rotao da base A para a base B, ver pgina 121.
Smbolos da calibrao
s [i] Estimativa do valor esperado da leitura do sensor s no intervalo i, ver Eq. (6.2).
xxi
Lista de Smbolos
Ks,ij Elemento na posio (i, j) da matriz de ganhos do sensor s, ver Eq. (6.16).
RB
A Rotao da base A para a base B, ver Eq. (A.11).
xxii
Sumrio
Lista de Tabelas xv
1 Introduo 1
1.1 Histria do quadrirrotor . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2
1.2 Motivao da pesquisa . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5
1.3 Estrutura do trabalho e contribuies . . . . . . . . . . . . . . . . . . . . . . . . . 6
2 Reviso bibliogrfica 9
2.1 Controle de quadrirrotores . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10
2.2 Filtragem de estados . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12
2.3 Calibrao de sensores inerciais . . . . . . . . . . . . . . . . . . . . . . . . . . . 14
2.4 Modelagem e projeto de quadrirrotores . . . . . . . . . . . . . . . . . . . . . . . . 16
xxiii
Sumrio
5 Filtragem de estados 39
5.1 Anlise terica . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 40
5.1.1 Equacionamento geral dos filtros . . . . . . . . . . . . . . . . . . . . . . . 40
5.1.2 Equacionamento dos modelos . . . . . . . . . . . . . . . . . . . . . . . . 45
5.2 Simulaes . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51
5.2.1 Comparao entre modelos de atitude . . . . . . . . . . . . . . . . . . . . 52
5.2.2 Estudo da ampliao do rudo de processo no modelo cinemtico . . . . . 60
5.2.3 Adaptao automtica do rudo de processo no modelo cinemtico . . . . . 62
5.2.4 Estimao da velocidade angular . . . . . . . . . . . . . . . . . . . . . . . 66
5.2.5 Modelos de independncia para estimao da atitude e velocidade angular . 69
5.2.6 Estimao de atitude utilizando apenas magnetmetro . . . . . . . . . . . 72
5.2.7 Estimao da posio . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 74
5.2.8 Estimao completa do sistema partindo do equilbrio . . . . . . . . . . . 82
5.3 Concluso . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 84
xxiv
Sumrio
7 Concluso 113
7.1 Direes futuras para pesquisa . . . . . . . . . . . . . . . . . . . . . . . . . . . . 114
A Quaternions 117
A.1 Operaes entre quaternions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 118
A.2 Quaternion como uma rotao no R3 . . . . . . . . . . . . . . . . . . . . . . . . . 119
A.2.1 ngulo entre quaternions . . . . . . . . . . . . . . . . . . . . . . . . . . . 120
A.2.2 Transformao de matriz de rotao para quaternion . . . . . . . . . . . . 120
A.2.3 Transformao de quaternion para matriz de rotao . . . . . . . . . . . . 120
A.2.4 Evoluo no tempo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 121
A.3 Vantagens do quaternion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 122
xxv
Captulo 1
Introduo
UAVs (Unmanned Aerial Vehicles) so veculos que surgiram h cerca de um sculo e tive-
ram grande desenvolvimento nas ltimas dcadas com foco em guerras, sendo peas fundamentais
nas mesmas (C OOK, 2007), e pesquisa. Visando reduzir o tamanho destes veculos, e consequen-
temente seu custo e consumo de energia, a DARPA (Agncia de Projetos de Pesquisa Avanada de
Defesa dos Estados Unidos) criou em 2011 uma competio para desenvolver UAVs de pequeno
porte1 , na tentativa de ter resultados promissores como os obtidos nos desafios de carros autnomos
(S EETHARAMAN e outros, 2006).
O sucesso atual dos UAVs se deve ao desenvolvimento das diversas tecnologias utilizadas,
como viso computacional e algoritmos de controle e gerao de trajetria, e maturao dos siste-
mas. Satisfeitos com a qualidade dos veculos, o governo dos Estados Unidos passou a considerar
sua utilizao em territrio nacional para monitoramento de ambientes pblicos2.
Dentre os UAVs, o quadrirrotor bastante popular tanto pelas suas capacidades, como deco-
lagem e pouso vertical, quanto pela sua simplicidade de construo e manuteno, quando com-
parado a avies ou helicpteros. Estas caractersticas fizeram com que a comunidade cientfica e
empresarial desenvolvessem interesse no quadrirrotor, gerando inmeros projetos baseados neste
veculo.
1
www.uavforge.net
2
www.cdt.org/blogs/harley-geiger/2112drones-are-coming
1
Captulo 1. Introduo
Este captulo apresenta uma viso geral da histria do quadrirrotor na seo 1.1, seguida
na seo 1.2 pela motivao do trabalho aqui desenvolvido. A seo 1.3 finaliza apresentando a
estrutura do trabalho e as principais contribuies.
Em torno de 400 a.C., crianas chinesas brincavam com brinquedos voadores feitos de
bambu, que consistiam de uma hlice de madeira presa a um eixo enrolado com cordas que, ao
serem puxadas, tanto o eixo quanto a hlice levantavam voo. Atualmente estes brinquedos ainda
existem e se chamam taketombo, no Japo, ou bambu-cptero, sendo seu modelo moderno mos-
trado na figura 1.1. Em 1480 aproximadamente, Leonardo da Vinci projetou uma mquina que,
teoricamente, seria capaz de voar na vertical, recebendo o nome de parafuso areo em virtude de
seu desenho, apresentado na figura 1.2. Os modelos de mquinas que voavam na vertical posteri-
ormente desenvolvidos se assemelhavam mais ao bambu-cptero do que maquina de da Vinci.
Em 1783, Christian de Launoy criou um modelo com um par de rotores que giravam em
direes contrrias e utilizavam penas de peru como hlices. George Cayley se interessava muito
pelo brinquedo chins e desenvolveu vrios modelos utilizando penas, assim como Launoy, e els-
ticos como fonte de potncia. Ao fim do sculo XVIII, ele havia avanado para utilizar metal para
as lminas e molas para potncia. Alphonse Pnaud desenvolveu helicpteros de brinquedo com
3
Fonte: en.wikipedia.org/wiki/Bamboo-copter
2
Captulo 1. Introduo
rotores coaxiais em 1870, sendo um destes modelo dado aos Irmos Wright por seu pai.
No incio dos anos 1920, Ral Pateras Pescara demonstrou uma das primeiras aplicaes da
inclinao cclica, que permite que as hlices sejam inclinadas independentemente, permitindo que
o helicptero mova para frente e para trs sem uma propulso extra, alm de permitir movimen-
tos laterais. Alm disso, Pescara tambm provou o princpio da autorrotao, que permite que o
4
Fonte: en.wikipedia.org/wiki/Science_and_inventions_of_Leonardo_da_Vinci
3
Captulo 1. Introduo
Etienne Oehmichen pesquisou mquinas rotativas nos anos 1920 e, dentre seis projetos que
ele testou, um possua quatro rotores e oito propulsores movidos por um mesmo motor. Cinco dos
propulsores eram utilizados para estabilizar o helicptero lateralmente, um para direcionar e dois
para propulso. Tal modelo apresentava um nvel considervel de estabilidade e controlabilidade.
4
Captulo 1. Introduo
dstria. Como exemplo industrial tem-se o omcopter, mostrado na figura 1.4, que possui 8 motores
para ter grande estabilidade de filmagem com sua cmera, sendo capaz de produzir imagens com
altssima qualidade para a indstria de entretenimento. J como exemplo de pesquisa, temos a
Flying Machine Arena (L UPASHIN e outros, 2011), que desenvolve aplicaes como utilizar qua-
drirrotores para balancear um pndulo invertido (H EHN E DA NDREA, 2011), executar multi-flips
durante voo (L UPASHIN e outros, 2010) e interagir com bolinhas (M ULLER e outros, 2011).
Para quebrar esta barreira, este trabalho prope um conjunto de algoritmos com foco em
6
Fonte: www.omstudios.de
7
ardrone2.parrot.com
8
www.asctec.de/uav-applications/research/products/
5
Captulo 1. Introduo
facilidade de uso sem perda de performance em trs reas do lao de controle do sistema, como
mostrado na figura 1.5:
3. Calibrao de sensores inerciais: apesar destes sensores normalmente sarem de suas fbricas
calibrados, o transporte e condies ambientais diferentes podem alterar o comportamento
de sua leitura, fazendo com que novos algoritmos que sejam capazes de calibrar rapidamente
e de maneira fcil os sensores sejam necessrios.
Este trabalho est dividido da seguinte forma: este captulo situa o leitor quanto ao porqu da
realizao deste trabalho, enquanto o captulo 2 apresenta uma reviso bibliogrfica de trabalhos
relevantes encontrados na literatura. O captulo 3 apresenta a modelagem do sistema dinmico e
sensores utilizados e os captulos 4, 5 e 6 tratam dos problema de controle, filtragem e calibrao,
descritos na seo anterior. Por fim, o captulo 7 sumariza os resultados e concluses obtidos,
sugerindo trabalhos futuros a serem realizados.
As principais contribuies deste trabalho, apresentadas na figura 1.5 junto com seus blocos
afetados, podem ser divididas de acordo com os captulos da seguinte forma:
6
Captulo 1. Introduo
Sintonia
automtica
r u y
Controlador Quadrirrotor
x z
Filtro Sensores
7
Captulo 1. Introduo
8
Captulo 2
Reviso bibliogrfica
O controle do quadrirrotor pode ser feito atravs de diversas tcnicas, como mostrado na
seo 2.1. Estas tcnicas podem ser baseadas em modelos aproximados ou testes empricos, como
o caso do PID, ou podem ter desenvolvimentos tericos com prova de estabilidade do controlador.
Em ambos os casos, os mtodos apresentados podem contribuir para o entendimento de como o
controle realizado neste veculo.
Alm de revisar as tcnicas de controle, filtragem e calibrao, que so o foco deste trabalho,
importante analisar tambm os modelos de quadrirrotor existentes na literatura. Como o modelo
do quadrirrotor j foi demasiadamente abordado na literatura, a seo 2.4 faz uma coleta das me-
lhores descries destes modelos com seus variados nveis de complexidade. Alm disso, a seo
9
Captulo 2. Reviso bibliogrfica
O STARMAC (H OFFMANN e outros, 2004) utiliza LQR para controle da atitude, com base
em seu modelo linear do sistema, e um controle por modos deslizantes simples para a altitude,
considerando a gravidade como rudo. Posteriormente, um modelo mais complexo foi desenvol-
vido, mas o controle se tornou mais simples, com uso de PIDs em todos os estados (H OFFMANN
e outros, 2007).
Park e outros (2005) utiliza duas malhas de controle com base em PIDs. O PID interno
visa aproximar o comportamento da planta do comportamento desejado dado por uma planta de
referncia, fazendo papel de um controle adaptativo primitivo. Com base numa planta linear de
referncia, projeta um controlador PID externo. Apesar de ser extremamente simples, apresenta
10
Captulo 2. Reviso bibliogrfica
Castillo e outros (2004) utiliza um modelo linearizado para controle de posio, mas um con-
trolador no linear baseado em saturao. Este controlador visa aproximar o ngulo de rotao do
sistema de zero, levando a posio do sistema origem em seguida. Ao comparar com o LQR,
mostra que o controlador desenvolvido mais robusto a rudos e condies iniciais distantes da
origem. Uma mistura de linearizao por realimentao com controle por saturao desenvol-
vido por Salazar-Cruz (S ALAZAR -C RUZ e outros, 2005), onde o controlador obtido de forma
semelhante ao backstepping, ou seja, determinado construtivamente para provar a estabilidade do
sistema.
Voos (2009) realiza controle do sistema utilizando linearizao por realimentao, gerando
portanto um controle extremamente simples mas que depende dos parmetros do sistema. Tayebi
e McGilvray (2004) prova que o controle de atitude utilizando linearizao por realimentao
exponencialmente estvel enquanto um controle PD apenas assintoticamente estvel, alm de
fornecer uma lei de controle para o comportamento dinmico dos motores, apesar de necessitar de
diversos parmetros neste caso.
Guenard e outros (2005) prope uma lei de controle visando fazer com que o erro de ve-
locidade linear seja nulo atravs do uso de uma funo de saturao, evitando comportamentos
agressivos na posio. Esta lei garante a estabilidade exponencial da atitude e convergncia assin-
ttica da velocidade, sendo semelhante lei proposta por Tayebi e McGilvray (2004).
Devido presena do LQR e mtodos de ajustar seus custos presentes na literatura, este
trabalho descreve um controlador que visa otimizar uma funo de custo quadrtica durante voo.
Como um compromisso entre controladores complexos, como backstepping e modos deslizantes,
que dependem do conhecimento dos parmetros do sistema, e controladores mais simples, que so
ajustados manualmente, o algoritmo desenvolvido se baseia, de forma semelhante a controlado-
11
Captulo 2. Reviso bibliogrfica
res adaptativos, no ajuste automtico dos parmetros de um controlador simples, como os usados
normalmente na prtica. Isto permite adaptao para diferentes sistemas sem necessidade de con-
troladores complexos.
Sabatini (2006) apresenta um EKF baseado num modelo cinemtico para deteco de mo-
vimento do corpo humano, sendo utilizado como base para criao dos filtros cinemticos do ca-
ptulo 5. Para tal, ele utiliza a leitura do giroscpio como entrada do sistema e a rotao dada
por um quaternion. Alm disto, monitora a leitura do acelermetro e magnetmetro para avaliar se
diferem significativamente do valor nominal, ignorando-as caso isto ocorra. Entretanto, no apre-
senta nenhuma sugesto ou mtodo de escolha dos limiares de validao das leituras. Inclui ainda
um estimador de bias do acelermetro e magnetmetro no estado a ser estimado, prtica comum
em sistemas que devem funcionar durante muito tempo.
Ainda com foco na rea mdica, h o filtro desenvolvido por Madgwick e outros (2011),
que se baseia num processo de otimizao eficiente. Ele tambm utiliza a leitura do giroscpio
como entrada do modelo cinemtico, mas no utiliza derivados do filtro de Kalman para corrigir a
estimativa. Realizando um passo na direo que minimiza a diferena entre a estimativa do campo
e a leitura do sensor, o filtro constri uma outra estimativa do quaternion correto que, ao fazer
uma combinao convexa com a previso do modelo cinemtico, prov a estimativa final. Fazendo
uso de algumas simplificaes, o algoritmo passa a ter apenas um parmetro, cujo valor pode ser
selecionado por um mtodo apresentado no trabalho. Num relatrio tcnico (M ADGWICK, 2010),
o desenvolvimento do filtro e os passos para determinao deste parmetro so detalhados, alm da
introduo de um outro componente capaz de compensar o bias e drift do giroscpio.
12
Captulo 2. Reviso bibliogrfica
Gebre-Egziabher e outros (2000) utiliza um EKF para fazer a estimativa do quaternion de erro
entre a rotao real e a estimada. Ao fazer isto, assume que o erro pequeno e trabalha com apenas
o termo vetorial do quaternion, o qual submetido a um filtro passa-baixa guiado por um rudo.
Esta formulao requer que sejam escolhidas a constante do filtro e a covarincia do rudo, o que o
trabalho no discute. Ao utilizar esta formulao, o filtro dispensa o uso de um giroscpio na etapa
de previso, uma vez que, segundo os autores, um sensor custoso. No entanto, esta afirmao
no mais verdade atualmente, devido evoluo dos sensores embarcados na ltima dcada,
fazendo com que o filtro seja recomendado apenas em casos particulares onde no h giroscpios
ou eles no fornecem leituras corretas. Posteriormente, os autores utilizam um filtro complementar
para mesclar a estimao independente de giroscpios com o uso da integrao direta da leitura
do giroscpio (G EBRE -E GZIABHER E E LKAIM, 2008), novamente no discutindo a escolha dos
parmetros.
Elkaim e outros (2012) apresentam uma proposta de filtragem utilizando EKF mais focada
para veculos areos, representada na figura 2.1. O estado a ser estimado conta com a posio,
velocidade linear, atitude e bias do acelermetro e giroscpio, no utilizando magnetmetro. Ape-
sar de seus experimentos apresentarem bons resultados, a no linearidade da atitude do sistema
pode afetar a qualidade dos resultados quando acoplada ao GPS. O GPS possui nveis de erros
elevados e frequncia de amostragem baixa, quando comparado com os demais sensores, o que
pode fazer com que a correo na atitude seja elevada. Alm disso, a estimao dos bias aumenta
consideravelmente a complexidade do estado sem garantia de gerar melhores resultados, contanto
que a calibrao seja adequada. Nos resultados apresentados, fica claro que, exceto por correes
exageradas do filtro que so posteriormente revertidas, os bias so praticamente constantes.
13
Captulo 2. Reviso bibliogrfica
quanto filtros complementares tm sua base em sistemas lineares e as extenses para sistemas
no lineares no possuem as garantias originais. Por serem aproximaes, estes filtros devem ser
avaliados objetivamente quanto qualidade de suas estimaes. Neste trabalho, sero analisadas
extensamente as derivaes do filtro de Kalman, devido s suas simplicidades de uso, ajustes de
parmetros e utilizao abrangente na literatura.
A calibrao dos sensores pode ser feita de maneira online, durante seu funcionamento, ou
offline. Entre os mtodos online, existem abordagens (ROY E T HRUN ; S ABATINI, 1999; 2006)
que utilizam filtros para estimar os parmetros relevantes dos sensores ou algoritmos recursivos
(C RASSIDIS e outros; K APALDO ; P YLVANAINEN, 2005; 2005; 2008). Entretanto, se o intervalo
de tempo durante o qual os sensores so utilizados for pequeno, como no caso dos quadrirrotores
devido ao tempo de voo que a bateria capaz de prover, os parmetros permanecem praticamente
constantes, favorecendo o uso de tcnicas offline. Portanto, este trabalho tratar apenas de mtodos
offline de calibrao.
14
Captulo 2. Reviso bibliogrfica
Ojeda e outros (2000) mostra que alguns tipos de giroscpios possuem no linearidades sig-
nificativas. Para modelar estes erros, utiliza uma funo cbica do valor lido e quadrtica da tempe-
ratura para modelar o erro entre o valor lido e o correto, supondo que o rudo de leitura pequeno.
Utilizando uma base giratria de velocidade constante, ele capaz de computar os parmetros para
compensar o erro, fazendo com que a diferena ps-calibrao seja reduzida significativamente.
Em contrapartida, Aslan (A SLAN E S ARANLI, 2008) mostra que existem sensores, tanto aceler-
metro quanto giroscpio, que independem da temperatura e os giroscpios podem ter sua sada
extremamente linear, levantando questionamento da necessidade da calibrao proposta por Ojeda
em sensores mais novos.
O algoritmo TWOSTEP, criado por Alonso e Shuster (2002b), capaz de determinar bias
de um magnetmetro independentemente da atitude do sistema. Para isto, apresenta uma srie de
aproximaes justificadas para chegar a uma funo de custo que possui uma componente linear e
outra no linear. A partir da soluo da parte linear, ele otimiza a funo completa. Este algoritmo
foi estendido em (A LONSO E S HUSTER, 2002a) para incluir a matriz simtrica de ganhos do sensor.
Como o TWOSTEP depende de os dados estarem pr-coletados, analisando-os em batelada, Cras-
sidis e outros (2005) desenvolve um mtodo online baseado na soluo da parte linear da funo
de custo.
15
Captulo 2. Reviso bibliogrfica
Com a base girante inclinada, a gravidade atua ciclicamente num eixo conhecido do acelermetro,
permitindo estimar o perodo e a velocidade angular atuante, a qual aproximada por um modelo
linear. Sabendo em torno de qual eixo o sistema est girando, possvel utilizar a leitura do gi-
roscpio e a estimao a partir do acelermetro para gerar o ganho e bias daquele eixo. Apesar
de no ser capaz de tratar de desalinhamento entre os eixos, a simplicidade deste mtodo signifi-
cativa, fazendo com que seja uma base interessante para desenvolvimento de novos algoritmos de
calibrao de giroscpios.
Elkaim (2013) apresenta um algoritmo para calibrao do alinhamento entre dois sensores,
utilizando a leitura deles e o conhecimento do seu vetor unitrio na coordenada inercial. Renaudin
e outros (2010) utiliza mltiplos magnetmetros para gerar um algoritmo mais robusto a perturba-
es externas, permitindo uma melhor estimao da direo de movimento do sistema.
Pounds e outros (2004) visa construir um quadrirrotor de utilidade prtica, ao contrrio dos
sistemas utilizados normalmente que praticamente no possuem capacidade de carga. Dentre os
problemas presentes, encontra a questo de como gerar empuxo suficiente e gerenciar o compor-
tamento instvel. Atravs de anlises de aerodinmica, projeta uma nova hlice que seja capaz
16
Captulo 2. Reviso bibliogrfica
Neste trabalho, ser utilizado um modelo simplificado que no considera modelagem dos
motores ou hlices, mas que no se restringe a aproximaes lineares da dinmica do sistema. Este
modelo foi escolhido por ser prximo o mais prximo do comportamento do sistema sem requerer
levantamento de parmetros aerodinmicos.
17
Captulo 3
Este captulo apresentar a modelagem matemtica do quadrirrotor na seo 3.1 e dos senso-
res utilizados na seo 3.2. Estas modelagens sero utilizadas nos captulos seguintes para elaborar
os algoritmos de filtragem, calibrao e controle. A seo 3.3, define o sistema inercial fixo na
Terra, utilizado como referncia para os algoritmos de filtragem e calibrao.
Quadrirrotores, assim como a maioria dos veculos areos, so conhecidos por terem dinmi-
cas extremamente complicadas. Nesta dinmica, possvel incluir a deformao das hlices, cha-
mado blade flapping, equao dinmica dos motores, arrasto aerodinmico do veculo e das hlices
e muitos outros fatores (P OUNDS e outros; H OFFMANN e outros; B OUABDALLAH e outros; B OU -
ABDALLAH E S IEGWART, 2010; 2007; 2004; 2007). Apesar destes modelos serem extremamente
interessantes para projeto e simulao do sistema (P OUNDS e outros; P OUNDS E M AHONY, 2004;
2009), eles so usualmente impraticveis de serem utilizados no controle e filtragem do sistema,
levando ao uso de controladores lineares (P OUNDS E M AHONY, 2006) e sistemas inerciais que
no usam toda a dinmica do sistema (P OUNDS e outros, 2010). Por isso, decidiu-se utilizar neste
trabalho uma dinmica simplificada do veculo.
19
Captulo 3. Modelagem matemtica do quadrirrotor e dos sensores
F4 F1
u
B
z
y
x
F3 F2
I
z
u
u
y
x
Supondo que a resposta dos motores seja instantnea e as inrcias dos rotores sejam despre-
zveis, pode-se construir um modelo dinmico composto apenas pela posio p e velocidade v do
centro de gravidade do quadrirrotor num sistema de coordenadas inercial I, o quaternion unitrio
q que leva do sistema fixo ao quadrirrotor B a I e pela velocidade angular do sistema medida em
B. Os sistemas de coordenadas so apresentados na figura 3.1. O modelo dado por:
F f
p = v = R(q) +g+ , f N (0, f ) (3.1a)
m m
1
q = q (3.1b)
2
= J1 ( J + + ) , N (0, ), (3.1c)
Supondo que os rotores esto alinhados com o eixo zB e que no ocorre blade flapping, tem-
se que a fora F possui apenas componente em zB , denominada Fz . Sendo Fi a fora desenvolvida
por cada um dos rotores, cujas direes de suas rotaes so mostradas na figura 3.1, e assumindo
que os braos do quadrirrotor possuem o mesmo tamanho e so ortogonais entre si, tem-se que
20
Captulo 3. Modelagem matemtica do quadrirrotor e dos sensores
Apesar deste trabalho tratar como entradas a fora Fz e os torques , na realidade elas so
utilizadas para gerar as foras Fi que, ento, so utilizadas para computar a velocidade desejada
dos rotores. Como este mapeamento complexo, devido s no linearidades presentes nas hlices,
e no o foco deste trabalho, o trabalho de Pounds e outros (2004) recomendado para uma viso
detalhada do modelo.
Como possvel notar, mesmo com uma srie de hipteses simplificadoras, o modelo ainda
possui uma grande quantidade de parmetros. Estimar cada um dos parmetros precisamente
extremamente trabalhoso, o que motiva o uso de controladores e filtros que dependam do menor
nmero de parmetros possvel. Em todas as simulaes realizadas neste trabalho foi utilizado o
modelo dinmico completo da Eq. (3.1).
ss = fs (sR
s , s ), (3.3)
onde sR
s o valor correto da leitura medido num referencial S, S uma varivel aleatria e fs (, )
uma funo qualquer que faz o mapeamento para o valor lido ss . O referencial S deve ser
introduzido pois nem sempre ele estar alinhado com o referencial do corpo B. No entanto, se os
sensores estiverem rigidamente presos ao corpo do quadrirrotor, ento a matriz de rotao R de B
para S fixa.
Apesar desta funo fs (, ) variar para cada sensor, os sensores tratados neste trabalho so
21
Captulo 3. Modelagem matemtica do quadrirrotor e dos sensores
ss = Ks sR
s + bs + s , s N (0, s ) (3.4)
1
T
sR
s = Ks (ss bs ) + s , s N (0, K1 1
s s Ks ) (3.5)
Note que esta formulao assume que os parmetros do sensor no variam, mas esta hiptese
nem sempre verdadeira. Existem casos onde valor lido ss depende no linearmente do valor
de referncia sR
s e da temperatura (H AKYOUNG e outros; O JEDA e outros, 2001; 2000), mas em
outros casos o mesmo tipo de sensor no apresenta tais caractersticas (A SLAN E S ARANLI, 2008),
sendo portanto dependente da tecnologia de fabricao. O parmetro que mais apresenta variao
com o tempo o bias, que pode ser compensado dinamicamente durante a filtragem (M ADGWICK
e outros; S ABATINI, 2011; 2006). No entanto, se o tempo de operao do sistema for pequeno,
como o caso comum dos quadrirrotores, este parmetro no apresenta muita variao e pode ser
suposto constante.
Com a suposio de que o rudo do sistema gaussiano, a probabilidade de uma dada leitura
do sensor dada por:
32 12 1 T 1
p(ss ; s ) = (2) |s | exp Ks sR s Ks sR (3.6a)
s + bs ss s + bs ss
2
R
(3.6b)
s = s , Ks , bs , ss .
22
Captulo 3. Modelagem matemtica do quadrirrotor e dos sensores
3.2.1 Giroscpio
sR
= . (3.7)
3.2.2 Magnetmetro
1
sR
m = R(q) h, (3.8)
importante lembrar que, quando usado para aplicaes dentro de prdios, magnetmetros
podem sofrer fortes perturbaes devido s estruturas metlicas.
3.2.3 Acelermetro
1
sR
a = R(q) p + ( r) + r, (3.9)
23
Captulo 3. Modelagem matemtica do quadrirrotor e dos sensores
3.2.4 GPS
O GPS capaz de medir a posio do sistema no sistema inercial I. Alm disso, por serem
sistemas absolutos, sua matriz de ganho unitria e apresenta bias nulo, exceto em caso de falha
do sistema. Portanto, sua leitura sGP S dada por:
onde GP S o rudo do sensor. Ao contrrio dos casos anteriores, o rudo do GPS no gaussiano,
pois depende da escolha de satlites utilizados pelo sistema. No entanto, este trabalho aproxima
seu rudo por uma gaussiana de covarincia elevada, uma vez que os filtros utilizados dependem
deste comportamento do rudo.
Como o sistema inercial I arbitrrio, este trabalho utiliza o nico sistema de coordenadas
onde a gravidade g e o campo magntico h podem ser escritos como: .
h iT
g = 0 0 gz , gz < 0 (3.11a)
h iT
h = hx 0 hz , hx > 0 (3.11b)
24
Captulo 4
Este captulo apresentar o mtodo de controle utilizado neste trabalho e os resultados obti-
dos. Apesar do controlador utilizado ser simples, as tcnicas podem ser estendidas para controles
mais complexos, mas como visto na seo 2.1, normalmente controladores mais simples so utiliza-
dos na prtica. O principal problema no projeto de controladores encontrar constantes adequadas
para estabilizar o sistema e obter a performance desejada (E LKAIM e outros, 2012).
Este captulo est dividido da seguinte forma: a seo 4.1 apresenta toda a base terica por
traz do funcionamento do controlador, que aplicado na seo 4.2 em sistemas simulados com
25
Captulo 4. Controle por iterao aproximada de poltica
onde f (, ) uma funo desconhecida. Deseja-se determinar as entradas de controle ut tais que a
funo de custo
H
X
J(x0 , u0 , u1 , . . . , uH ) = C(xt , ut ) (4.2)
t=0
seja minimizada, onde C(, ) determinada pelo usurio para satisfazer seus requisitos e H o
horizonte de otimizao, utilizado para capturar a dinmica do sistema.
ut = (xt ; ), (4.3)
J(x0 ; )
k+1 = k , (4.4)
26
Captulo 4. Controle por iterao aproximada de poltica
onde J a mesma funo de custo definida anteriormente onde as entradas de controle so esco-
lhidas de acordo com a poltica e o tamanho do passo na direo do gradiente.
H
J(x0 ; ) X C(xt , ut )
= (4.5a)
t=0
H
X C(xt , ut ) xt C(xt , ut ) (xt ; ) xt C(xt , ut ) (xt ; )
= + + (4.5b)
t=0
xt ut x t u t
H
X xt
= (qt + rt Kt ) + rt t , (4.5c)
t=0
onde define-se:
t1
xt X xt (xt ; )
= , (4.7)
ut
t =0
H t1
! !
J(x0 ; ) X X xt
= (qt + rt Kt ) t + rt t . (4.8)
t=0 t =0
ut
27
Captulo 4. Controle por iterao aproximada de poltica
Se t = t + 1, tem-se que xt
ut
dado por:
xt +1 f (xt , ut )
= (4.9)
ut ut
e corresponde ao termo linearizado da funo em relao s entradas de controle. Assim, estes ter-
mos representam como entradas de controle passadas afetam estados futuros. A primeira hiptese
consiste em assumir que, para horizontes curtos, os termos para t > t + 1 so similares ao termo
de um passo.
Para muitos sistemas, cada estado afetado principalmente por uma entrada de controle, de-
finindo uma ortogonalidade das entradas. Como exemplo, Kolter (2010) cita um carro seguindo
uma trajetria: apesar da dinmica complexa do carro, sabe-se que a posio longitudinal princi-
palmente afetada pela acelerao, enquanto a posio lateral e o ngulo de direo afetada pela
rotao do volante. Esta simplicidade auxilia o aprendizado humano, uma vez que sistemas mais
acoplados demandam mais coordenao nos movimento, e, por isso, vrios sistemas construdos
por seres humanos possuem tal caracterstica. Assim, a segunda hiptese a de que o algoritmo
sabe para cada estado qual entrada de controle mais o afeta e se este efeito positivo ou negativo.
Esta hiptese responsvel por dar nome tcnica, uma vez que o usurio fornece ao algoritmo o
sinal da derivada de um estado para uma entrada de controle.
Sendo S a matriz de sinais das derivadas, possuindo apenas termos 1, 0 e 1, ela utilizada
para substituir u
xt
em na Eq. (4.8), chegando a:
t
H
J(x0 ; ) X
((qt + rt Kt ) St + rt t ) , (4.10)
t=0
onde:
t1
X
t = t . (4.11)
t =0
28
Captulo 4. Controle por iterao aproximada de poltica
Para determinar a matriz S para o quadrirrotor, seu modelo ser analisado prximo refern-
cia desejada, pois, apesar de ele no estar sempre nesta condio, isto evita conflitos na construo
da matriz. Para compreender porque isto aconteceria, considere o efeito que existe na posio y ao
aplicar um torque x . Supondo que o sistema possui um ngulo de direo prximo de 0, tem-se
que o torque ir afetar y de forma oposta da qual o faria caso a direo fosse prxima de 180 .
Portanto, ao considerar que o sistema est prximo de algum ponto bem comportado, algumas
simplificaes podem ser feitas. Alm disso, isto reduz tambm o acoplamento existente entre as
velocidades e atitudes. Nota-se que esta aproximao no restritiva uma vez que a referncia
pode ser computada por um gerador de trajetria, permitindo variaes na referncia para atingir o
objetivo alcanado.
Assumindo que as velocidades angulares so pequenas, tem-se que elas so afetadas princi-
palmente por seus respectivos torques. importante ressaltar que isto no necessariamente ver-
dade, pois uma inrcia muito reduzida em x, por exemplo, pode fazer com que um torque grande
em y altere significativamente a velocidade em x. Entretanto, esta uma aproximao razovel e
frequentemente utilizada por pilotos. Alm disso, um valor positivo dos torques gera valores po-
sitivos na velocidade, fazendo com que o sinal da derivada seja positivo. Portanto, tem-se que os
termos de S respectivos so dados por:
0 1 0 0
S = 0 0 1 0 (4.12)
0 0 0 1
onde as entradas foram ordenadas como u = [F, x , y , z ]T , conforme a notao da Eq. (3.1).
Considerando que o quaternion est prximo de sua referncia, o quaternion de erro pode
ser escrito como qe = [1, ex , ey , ez ]T . Cada termo ei principalmente afetado pela sua velocidade
angular, fazendo com que suas componentes da matriz S sejam os mesmos. O termo escalar prati-
camente no afetado por qualquer uma das entradas, fazendo com que seu termo seja nulo para
29
Captulo 4. Controle por iterao aproximada de poltica
A posio z do sistema principalmente afetada pela fora F, fato este que frequentemente
explorado no projeto de controladores. J para as posies x e y, assume-se um modelo linearizado
do sistema. Supondo que o quadrirrotor est pairado, uma rotao positiva em torno do eixo x faz
com que o sistema movimente-se na direo y. Em contrapartida, uma rotao positiva em y faz
o sistema se movimentar em x. Considerando que o torque i o responsvel por gerar rotaes no
eixo x, tem-se que a componente de S vale:
0 0 1 0
Sp = 0 1 0 0 (4.14)
1 0 0 0
Como a velocidade possui o mesmo comportamento descrito para a posio, tem-se que Sv = Sp .
importante destacar que existem duas hipteses com relao aos sinais utilizados:
1. Os termos da matriz Sp s esto corretos quando assume-se que Fz positivo. Caso contr-
rio, os termos referentes a x e y deveriam ser invertidos, uma vez que o sistema estaria se
movendo na direo reversa. O termo z permanece inalterado, uma vez que ele no depende
da direo de movimentao do sistema.
2. Assumiu-se, sem perda de generalidade, que o quaternion possui termo escalar positivo, re-
sultando na anlise realizada. Devido ao mapeamento duplo dos quaternions, seria possvel
estar numa situao onde este valor negativo, o que faria com que Sq tivesse que ser inver-
tida.
Como este trabalho supe que Fz 0, considerando a incapacidade do sistema de gerar em-
puxo negativo9, apenas zera-se esta entrada do sistema caso o valor desejado passe a ser negativo.
9
Existem quadrirrotores que podem alterar a inclinao de suas lminas, funcionando como helicpteros e sendo
30
Captulo 4. Controle por iterao aproximada de poltica
A princpio, as derivadas das funes de custo e poltica deveriam ser alteradas, uma vez que o
sistema encontra-se em modo de saturao, mas notou-se nas simulaes da seo 4.2 que isto no
necessrio, pois o sistema raramente se encontra em tais situaes.
Seja q o quaternion corrente do sistema e qdes o quaternion desejado para o sistema, pode-se
definir o erro como:
q = q qdes . (4.16)
Para os sinais de controle, considera-se que a massa do sistema conhecida, de forma que
apenas a fora tenha uma referncia diferente de zero. Caso a massa no fosse conhecida, o esforo
F poderia ser separado em F1 e F2 , de forma que F = F1 +F2 e F2 possuiria apenas uma constante
na componente zI , que no penalizada. Assim, evita-se que o sistema seja forado a manter um
capazes de gerar empuxo negativo, mas estes sistemas no sero tratados neste trabalho.
31
Captulo 4. Controle por iterao aproximada de poltica
erro vertical devido restrio do esforo de controle no permitir o ajuste devido massa.
Uma vez definidos os erros dos estados em relao aos valores desejados e dos esforos de
controle em relao ao necessrio ao necessrio para manter o quadrirrotor em voo, a funo de
custo dada simplesmente por:
Como deseja-se que a resposta de posio do sistema seja mais lenta, evitando oscilaes
caractersticas de sistemas subamortecidos, faz-se Qv = 10Qp e Qp = I3 , onde In a matriz iden-
tidade de tamanho n. Pelo mesmo motivo, define-se Q = 10I3 . J para o quaternion, novamente a
escolha mais delicada. Com base na aproximao de pequenos ngulos e o controlador utilizado,
que ser descrito na seo posterior, no faz sentido aplicar um custo componente escalar do
quaternion, uma vez que a informao est codificada nas demais componentes. Como o valor que
as componentes vetoriais alcanaro so usualmente menores do que as componentes de posio,
deve-se ter um custo maior do que Qp . Assim, Qq dado por:
" #
0 0
Qq = (4.18)
0 10I3
Outro fator que auxilia a evitar oscilaes o uso de um custo para os esforos de controle,
fazendo com que o sistema os aplique de maneira mais sbia. No entanto, um custo muito alto pode
fazer com que a resposta do sistema seja demasiadamente lenta, levando-o instabilidade. Assim,
escolheu-se Qu = 102 I4 .
Note que estes valores so arbitrrios e podem ser ajustados conforme o sistema opera caso
seja necessrio. Os valores descritos aqui foram ajustados para reduzir o erro mdio da simulao
do sistema sem aprendizado, descrita na seo 4.2.
A poltica utilizada pode ser separada na poltica de controle vertical e de controle da ro-
tao. Para o controle vertical, utiliza-se o mtodo descrito em (M ELLINGER E K UMAR, 2011),
32
Captulo 4. Controle por iterao aproximada de poltica
enquanto o controle da rotao dado pela lei de controle mais simples descrita em (TAYEBI E
M C G ILVRAY, 2004). A complexidade da lei restrita devido quantidade de parmetros a serem
aprendidos pelo controlador. Alm disso, o uso de termos integrativos nos controladores auxiliam
em manter a estabilidade e permitem que o sistema funcione fora da condio ideal. No entanto,
o uso destes termos em controladores preditivos como este no factvel, devido complexidade
do termo j simplificado pelo mtodo aqui descrito. Portanto, comum utilizar termos constantes
no controlador ao invs de integrativos, uma vez que os valores das constantes podem ser alterados
durante o passo da otimizao realizada.
Para corrigir a direo zB do sistema, deve-se computar a rotao desejada. O valor desejado
deste vetor dado pela norma da fora desejada, ou seja,
B Fdes
zdes = (4.21)
kFdes k
Conforme descrito em (M ELLINGER E K UMAR, 2011), para definir as demais componentes dese-
jadas define-se um vetor auxiliar xC
des com a direo desejada do sistema:
T
xC
des = [cos T , sin T , 0] , (4.22)
33
Captulo 4. Controle por iterao aproximada de poltica
B zBdes xC
des
ydes = B C
(4.23a)
kzdes xdes k
xBdes = ydes
B
zBdes . (4.23b)
A partir das coordenadas desejadas, pode-se obter o quaternion desejado qdes utilizando-se o m-
todo descrito na seo A.2.2.
= ~q K + (4.24)
Portanto, os parmetros da poltica de controle descrita pelas Eqs. (4.19) a (4.24) so dados
por:
= {Kp , Kv , F, , K , }. (4.25)
4.2 Simulaes
Nesta seo, o controlador descrito anteriormente ser utilizado para controlar o quadrirrotor
na presena de rudos. A comparao de seu desempenho com outros controladores encontrados
na literatura pode ser difcil, uma vez que os controladores podem ter sido ajustados com objetivos
34
Captulo 4. Controle por iterao aproximada de poltica
ou funes de custos diferentes. Portanto, utilizar-se- como base para comparao um controlador
sem aprendizado submetido s mesmas condies. Os parmetros deste controlador foram ajusta-
dos manualmente de forma a gerar um bom desempenho para diversas combinaes de inrcia do
quadrirrotor, visando uma competio justa entre o controlador esttico e o adaptativo.
4.2.1 Metodologia
A referncia para o controlador foi que o quadrirrotor deveria permanecer parado e sem
rotao no eixo z a partir desta mesma condio, ou seja, pT = 0 e T = 0. O tempo total de
simulao foi de 1 minuto, com frequncia de amostragem de 100Hz. Os parmetros do controlador
utilizados foram H = 4 e = 104 , onde o valor de deve ser pequeno para evitar variaes
grandes nos parmetros devido apenas aos rudos. Uma alternativa seria utilizar valores maiores
para e fazer uma mdia de uma sequncia de gradientes. No entanto, o sistema permaneceria
utilizando a mesma poltica durante muito tempo, o que motivou manter o controlador online.
Os rudos de torque e fora foram criados com a mesma escala, variando apenas a unidade, de
forma que apenas a fora ser descrita, onde cada componente gerada a partir de uma combinao
de rudo de onda quadrada e gaussiano. A intensidade da componente quadrada escolhida pelo
gerador U([5, 5]) e possui durao escolhida aleatoriamente pelo gerador U({10, 11, . . . , 100}),
fazendo com que a intensidade permanea no mximo 1 segundo. Terminado este perodo, novas
intensidades e duraes so amostradas at que a simulao termine. J a componente normal
gerada a partir de um rudo gaussiano de mdia zero e covarincia 10, ou seja, a partir do gerador
N (0, 10). Entretanto, foi realizada uma alterao nestes valores: como foi suposto que o rudo
de foras se devem a efeitos aerodinmicos e sabido que estes efeitos afetam menos as direes
horizontais do quadrirrotor, aplica-se apenas 10% do rudo nestas direes.
35
Captulo 4. Controle por iterao aproximada de poltica
Kp = 5I3
Kv = 10I3
F = [0, 0, 9, 81]T
(4.26)
= 100
K = 10I3
= [0, 0, 0]T .
A tabela 4.1 apresenta um resumo dos parmetros utilizados para gerar os resultados.
4.2.2 Resultados
A figura 4.1 apresenta os resultados da simulao, onde todas as variaes de inrcia foram
sobrepostas. As posies so dadas em metros e o direcionamento dado em graus.
36
Captulo 4. Controle por iterao aproximada de poltica
0.4
Esttico 0.4
Esttico
0.2 0.2
px [m]
py [m]
0 0
0.2 0.2
0.4 0.4
0 10 20 30 40 50 60 0 10 20 30 40 50 60
0.4
Com aprendizado 0.4
Com aprendizado
0.2 0.2
px [m]
py [m]
0 0
0.2 0.2
0.4 0.4
0 10 20 30 40 50 60 0 10 20 30 40 50 60
t [s] t [s]
(a) Erro na direo x (b) Erro na direo y
1
Esttico 15
Esttico
10
0.5
pz [m]
[ ]
5
0
0
0.5
5
1 10
0 10 20 30 40 50 60 0 10 20 30 40 50 60
1
Com aprendizado 15
Com aprendizado
10
0.5
pz [m]
[ ]
5
0
0
0.5
5
1 10
0 10 20 30 40 50 60 0 10 20 30 40 50 60
t [s] t [s]
(c) Erro na direo z (d) Erro de direo
Figura 4.1: Erros durante a simulao para o controlador esttico e com aprendizado. Cada curva
corresponde a uma combinao de inrcias.
4.2.3 Anlise
37
Captulo 4. Controle por iterao aproximada de poltica
durante alguns intervalos h um offset entre conjuntos de inrcias diferentes, o que no ocorre no
caso com aprendizado.
Um resultado inesperado foi que o aprendizado fez com que o comportamento de todos as va-
riaes de inrcias ficassem mais prximos entre si, apesar disto no estar especificado diretamente
no controlador. possvel que este comportamento represente um ponto de baixo custo da funo
utilizada, fazendo com que o controlador ajuste os parmetros para que todos os sistemas tenham
comportamentos prximos a este timo. Esta capacidade de ajuste do controlador talvez possa
ser explorada de maneira semelhante a controladores adaptativos numa malha interna, ajustando o
comportamento da planta para ser controlada por um controlador externo.
4.3 Concluso
Este captulo utilizou uma metodologia geral para aprendizado de parmetros de controla-
dores em sistemas dinmicos e testou seu desempenho para controle de um quadrirrotor sujeito a
rudos. Apesar de ser capaz de controlar adequadamente o sistema, foi percebida uma certa dificul-
dade no ajuste da funo de custo do controlador.
38
Captulo 5
Filtragem de estados
Este captulo apresentar as tcnicas de filtragem utilizadas neste trabalho e seus resultados.
Os filtros aqui utilizados so baseados no filtro de Kalman (K ALMAN, 1960), uma vez que os rudos
dos sensores so principalmente gaussianos. Para tratar de outros tipos de rudos, possvel utilizar
filtros H (S TEWART, 1996) ou filtros de partculas (A RULAMPALAM e outros, 2002). Alm disso,
assumir-se- que as matrizes de covarincia so constantes, pelos motivos discutidos na seo 3.2.
Para o caso de covarincias variantes, possvel usar filtros adaptativos para corrigi-las (H AN
e outros, 2009). Para mais detalhes das diversas tcnicas de filtragem Bayesiana disponveis, o
tutorial desenvolvido por Haug (2005) recomendado.
Este captulo est dividido da seguinte forma: a seo 5.1 apresenta o funcionamento geral
39
Captulo 5. Filtragem de estados
dos filtros utilizados e o equacionamento para o caso particular do quadrirrotor, a seo 5.2 traz os
resultados obtidos a partir da aplicao dos filtros e suas modificaes no quadrirrotor e a seo 5.3
apresenta um resumo dos resultados obtidos.
Esta seo apresenta o equacionamento dos filtros a serem utilizados juntamente com os
modelos aos quais so aplicados, onde estes modelos so baseados naqueles desenvolvidos no
captulo 3. A seo 5.1.1 detalha o equacionamento dos filtros EKF (Extended Kalman Filter) e
UKF (Unscented Kalman Filter), ambos alteraes do filtro de Kalman original (K ALMAN, 1960),
enquanto a seo 5.1.2 desenvolve os modelos elaborados anteriormente para serem usados junto
aos filtros.
importante notar que a ambas funes do sistema possuem mais termos ui do que nor-
malmente utilizados na literatura. Isso se deve ao fato de que u um sinal arbitrrio, incluindo
o sinal de controle. Como o sistema real pode ser contnuo e a computao no instantnea, a
discretizao permite que um dado intervalo seja influenciado por mais sinais do que o esperado.
Considerando que u apenas sinal de controle, a justificativa pode ser observada na figura 5.1.
Caso o tempo total de computao seja muito menor do que o perodo de amostragem, possvel
aproximar utilizando apenas uk . No entanto, se o tempo de computao for muito prximo do
perodo de amostragem, ento o intervalo de atuao de uk se torna desprezvel.
40
Captulo 5. Filtragem de estados
5.1.1.1 EKF
importante destacar que, ao contrrio do filtro de Kalman, que possui provas de otimali-
dade, o EKF no possui desempenho garantido, mas apresenta resultados bons caso o sistema seja
aproximadamente linear na regio de alta probabilidade da covarincia.
41
Captulo 5. Filtragem de estados
Para calcular a aproximao do estado futuro xk|k1 utilizando a estimativa xk1|k1, aplica-
se um passo semelhante previso do filtro de Kalman, com a diferena de que o modelo no linear
utilizado para propagar o modo da distribuio de probabilidade. Assim, sua previso dada por:
Uma vez previsto o estado futuro, pode-se calcular a leitura esperada zk|k1 e compar-la
com a leitura real zk . Sua diferena determina a novidade k presente na medida, sendo usada para
correo da estimativa do estado. O equacionamento para a etapa de correo :
importante ressaltar que, no filtro de Kalman para sistemas invariantes no tempo e que sa-
tisfazem algumas hipteses razoveis, a matriz de ganho Kk converge para um valor fixo. Por isso,
vrios produtos que utilizam o filtro de Kalman calculam-na num computador externo e utilizam
uma verso hard-coded no produto, evitando o reclculo repetido. Ao fazer isso, eles permitem
uma convergncia mais lenta da estimativa para realizar computaes mais rpidas. No entanto, a
convergncia pode no acontecer para sistemas variantes no tempo. Como o clculo das Jacobianas
das funes dependem da estimativa corrente, a propagao da covarincia Pk|k se comporta como
um sistema variante no tempo e, portanto, no possvel pr-computar a matriz Kk .
5.1.1.2 UKF
O UKF (Unscented Kalman Filter) (J ULIER e outros, 2000) foi criado no intuito de gerar um
filtro baseado no filtro de Kalman para sistemas no lineares e mais robusto do que o EKF. Frequen-
temente o EKF se torna muito confidente em sua estimativa, possuindo uma matriz de covarincia
42
Captulo 5. Filtragem de estados
menor do que a real (J ULIER E U HLMANN, 1997). Para resolver isto, usualmente os rudos tm
suas matrizes de covarincia amplificadas, visando evitar que a estimativa se torne inconsistente.
No entanto, aumentar a covarincia reduz a performance do sistema e torna a estimativa menos
eficiente. O UKF visa resolver estes problemas.
O UKF se baseia nos chamados pontos sigma Xi , que so amostras que representam a dis-
tribuio de probabilidade. Ao contrrio das partculas do filtro de partcula, os pontos sigma so
determinsticos dada uma distribuio de probabilidade. Estes pontos so uma combinao dos
estados e rudos atuantes no sistema da seguinte maneira:
Xi,x (k|k)
Xi (k|k) = Xi, (k|k) (5.6a)
Xi, (k + 1|k + 1)
Pk|k 0 0
k = 0 Qk 0 . (5.6b)
0 0 Rk+1
Pode-se considerar casos onde os pontos sigma so utilizados para fazer apenas a predio
do sistema, no possuindo os termos Xi, , ou para fazer apenas a correo, no possuindo portanto
Xi, . A formulao aqui apresentada a completa, mas produz os mesmos resultados de se gerar
pontos para a previso, colaps-los, gerar pontos para correo e colaps-los.
onde n a dimenso do estado aumentado, sendo portanto a soma dos tamanhos do vetor de estados,
do rudo e do rudo , (A)i a i-sima linha ou coluna da matriz A, dependendo da tcnica
utilizada para calcular a raiz10 e um parmetro a escolha do projetista, devendo ser escolhido de
10
Se a raiz de A de k for da forma k = AT A, ento as linhas de A devem ser utilizadas. Se k = AAT , as colunas
devem ser utilizadas. (J ULIER e outros, 2000)
43
Captulo 5. Filtragem de estados
forma a aumentar a performance do filtro11 (J ULIER e outros, 1995). Cada ponto possui tambm
um peso Wi associado, cujos valores so:
W0 = (5.8a)
n+
1
Wi = , i N2n . (5.8b)
2(n + )
Existem formulaes que utilizam um conjunto W i de pesos diferentes para a matriz de covarincia
(J ULIER E U HLMANN, 2004), visando reduzir a chance da estimativa se tornar inconsistente, mas
estas formulaes no sero utilizadas neste trabalho.
Uma vez calculados os pontos sigma, cada um deles utilizado para gerar uma previso do
estado futuro. Assim, o ponto sigma futuro pode ser calculado como:
Xi,x (k|k 1) = f (Xi,x (k 1|k 1), k, uk1, Xi,w (k 1|k 1)). (5.9)
Utilizando os pontos sigma da previso, calcula-se a medio esperada por cada um com:
11
Para a gaussiana, n + = 3 timo.
44
Captulo 5. Filtragem de estados
previses e as medidas:
2n
X
zk|k1 = Wi Zi (k|k 1) (5.12a)
i=0
2n
X T
Pzz (5.12b)
k|k1 = Wi Z(k|k 1) zk|k1 Z(k|k 1) zk|k1
i=0
2n
X T
Pxz (5.12c)
k|k1 = Wi Xi,x (k|k 1) xk|k1 Z(k|k 1) zk|k1 .
i=0
Calculadas estas distribuies, possvel aplicar o filtro de Kalman para corrigir a estimativa
do estado xk , alterando seu modo e covarincia:
1
Kk = Pxz zz
k|k1 Pk|k1 (5.13a)
k = zk zk|k1 (5.13b)
xk|k = xk|k1 + Kk k (5.13c)
Pk|k = Pk|k1 Kk Pzz T
k|k1 Kk (5.13d)
Para transformar este modelo contnuo no formato discreto descrito pela Eq. (5.1), deve-
se considerar o perodo de amostragem Ts . Suponha que u(t) e (t) so constantes no intervalo
t [(k 1)Ts , kTs ], pode-se calcular o valor de x(kTs ) a partir de x((k 1)Ts ) da seguinte
45
Captulo 5. Filtragem de estados
maneira:
Z kTs
x(kTs ) = f () = x((k 1)Ts ) + f (x( ), , u((k 1)Ts ), ((k 1)Ts ))d. (5.15)
(k1)Ts
x(kTs ) f() = x((k 1)Ts ) + Ts f (x((k 1)Ts ), kTs , u((k 1)Ts ), ((k 1)Ts )) (5.16)
pode ser utilizada, sendo computacionalmente mais barata. Estes so os dois modelos de discre-
tizao que sero utilizados para transformar o sistema contnuo em discreto na etapa de previso
dos filtros.
Alm disso, para o EKF, deve-se calcular as Jacobianas, usadas na Eq. (5.3). Para isto, monta-
se o modelo linearizado da Eq. (5.14) em torno de x((k 1)Ts ). A linearizao do modelo dada
por:
46
Captulo 5. Filtragem de estados
Novamente, este clculo possui custo computacional elevado e possvel usar as aproxima-
es de primeira ordem, sendo elas:
Fk = I + Ts Ak , Gk = Ts Bk . (5.20)
Para os sensores, modelos devidamente calibrados sero utilizados, de forma que na Eq. (3.4)
sero considerados K = I, b = 0 e a rotao R = I, onde I a matriz identidade.
1
q = q (5.21a)
2
= J1 ( J + + ) , N (0, Q ), (5.21b)
47
Captulo 5. Filtragem de estados
onde:
0 x y z
fq 1 x 0 z y
= (5.24a)
q 2 y z
0 x
z y x 0
q1 q2 q3
fq 1 q0 q3 q2
= (5.24b)
2 q3 q0
q1
q2 q1 q0
f h i
= J1 c(~x) c(~y) c(~z) , c(v) = v J + Jv (5.24c)
f
= J1 (5.24d)
Definindo estas equaes, as discretizaes descritas na seo anterior podem ser utilizadas
juntamente com um filtro para fazer o passo da predio.
Para utilizar os filtros com o modelo cinemtico, deve-se adicionar rudos de processo condi-
zentes. Usualmente, a prtica adicionar uma quantidade de rudos igual quantidade de estados
(T HRUN e outros, 2005), visando evitar que graus de liberdade sejam perdidos. No entanto, como
existe a restrio de que o quaternion q deve ser unitrio, o estado possui apenas trs graus de li-
berdade. Portanto, possvel adicionar apenas trs rudos sem gerar distribuies de probabilidade
incorretas. Alm disso, os rudos podem garantir que a norma de q permanea unitria se forem
48
Captulo 5. Filtragem de estados
1
q = ( + ) q, N (0, Q ). (5.26)
2
fq fq
onde q
e
so dadas pelas Eqs. (5.24a) e (5.24b), respectivamente.
Com base na Eq. (3.1a), supondo que a acelerao p do sistema conhecida, o modelo
discreto pode ser escrito como:
Ts2
pk = pk1 + Ts vk1 + p
2 (5.28)
vk = vk1 + Ts p,
Ts2
" # " #
1 Ts
Fk = , Gk = 2
. (5.29)
0 1 Ts
5.1.2.4 Giroscpio
A partir das Eqs. (3.4) e (3.7), tem-se que a leitura do giroscpio dada por:
zk = k + k , k N (0, R ), (5.30)
Hk = I3 , Lk = I3 . (5.31)
49
Captulo 5. Filtragem de estados
5.1.2.5 Magnetmetro
1
zm m
k = R(qk ) h + k , km N (0, Rm), (5.32)
onde R(qk ) a matriz de rotao associada ao quaternion qk , que pode ser obtida utilizando-se o
mtodo descrito na seo A.2.3.
Lmk = I3 , (5.33a)
h i
Hm k = H m
k,1 (qk ) H m
k,2 (qk ) H m
k,3 (q k ) H m
k,4 (qk ) , (5.33b)
2hx q0 2hz q2 2hx q1 + 2hz q3
Hm
k,1 (q) = 2hx q3 + 2hz q1 , Hm k,2 (q) = 2hx q2 + 2hz q0 , (5.33c)
5.1.2.6 Acelermetro
Supondo que r 0 e que a velocidade no chega a nveis onde seus termos associados so
significativos, a medio zak depende apenas do quaternion qk . Alm disso, quando o modelo com
apenas atitude utilizado, tem-se p = g.
50
Captulo 5. Filtragem de estados
Lak = I3 (5.35a)
za
Hak = k , (5.35b)
qk
za
onde qkk pode ser calculado da mesma maneira que Hm
k na Eq. (5.33b), substituindo-se hx = 0 e
hz = gz .
5.1.2.7 GPS
A leitura do GPS dada pela Eq. (3.10), reescrita aqui com a notao usada nos filtros:
zGP
k
S
= pk + kGP S , GP S N (0, RGP S ), (5.36)
HGP
k
S
= I3 . (5.37b)
5.2 Simulaes
Nesta seo, sero elaborados um conjunto de filtros para serem utilizados com o quadrirro-
tor. Para testar os filtros, os parmetros de simulao foram escolhidos com o objetivo de manter o
sistema em situaes extremas, apresentando altas velocidades e aceleraes. Com isso, o sistema
excitado em regies onde modelos linearizados no funcionam bem e filtros podem apresentar
respostas muito ruins. Ao ajustar os parmetros do filtro e estudar seu comportamento nessas con-
dies, sua robustez aumentada, assim como a certeza de que ele funcionar corretamente mesmo
em situaes fora do esperado.
Uma das partes mais cruciais em veculos areos a estimao correta da atitude. A atitude
normalmente utilizada por outros sistemas, como no caso de rotacionar a imagem de uma c-
mera para fazer associao com imagens previamente capturadas. Para isto, a seo 5.2.1 analisa
51
Captulo 5. Filtragem de estados
Como a ampliao dos rudos do sistema podem melhorar seu desempenho (T HRUN e ou-
tros, 2005), uma anlise do desempenho do modelo cinemtico para diferentes nveis de rudo
realizada na seo 5.2.2. Os resultados obtidos motivam a busca na seo 5.2.3 por um algoritmo
que reduza o erro de estimao da atitude nos casos em que ele elevado, mas sem prejudicar
significativamente os casos onde ele est prximo do ideal.
A seo 5.2.4 trata da estimao da velocidade angular, onde foi utilizado um algoritmo sim-
ples para previso e correo, que reforado pela adaptao da covarincia do rudo de processo.
Como esta estimao se baseia numa independncia da estimao da atitude, os desempenhos de
outros modelos de independncia entre estes estados so analisados na seo 5.2.5.
Uma vez analisado na seo 5.2.6 o problema de estimao da atitude e determinado como
utilizar apenas o magnetmetro para estimao da atitude, feita uma tentativa de estender a esti-
mao de atitude para incluir estimao da posio e velocidade na seo 5.2.7. Para isso, criado
um sistema onde a estimao da atitude considerado um sistema mestre e a estimao da veloci-
dade um escravo. Esta considerao permite que o filtro desenvolvido seja mais eficiente do que
o uso do estado completo.
Por fim, a seo 5.2.8 analisa o comportamento do sistema quando ele sai do ponto de equi-
lbrio e os nicos esforos atuantes so rudos gaussianos. Esta seo visa avaliar os filtros desen-
volvidos na seo 5.2.7, uma vez que as condies de teste eram extremas e, possivelmente, no
sero alcanadas durante o uso do sistema.
Devido complexidade e variedade dos modelos dinmicos dos sistemas, usualmente IMUs
(Inertial Measurement Units) e MARGs (Magnetic, Angular Rate, and Gravity) utilizam modelos
cinemticos da atitude para estimar a rotao atual do sistema. Alm disso, encontra-se frequen-
temente na literatura modelos cinemticos sendo usados para filtragem (T HRUN e outros, 2005).
No entanto, no foi encontrado nenhum trabalho que analisasse a qualidade desta aproximao,
sempre assumindo que as velocidades so reduzidas, o que de fato faz com que os dois modelos
52
Captulo 5. Filtragem de estados
Esta simulao tem como objetivo analisar a qualidade da estimao da atitude utilizando-se
os modelos cinemtico e dinmico do quadrirrotor. Deseja-se justificar o uso do modelo cinemtico
pela qualidade de sua estimao e velocidade, alm da ausncia de qualquer parmetro referente
ao sistema, utilizando apenas modelos dos sensores. Sero testados tambm o EKF e o UKF para
comparar seus resultados e decidir qual melhor para o caso de estimao de atitude. Vale ressaltar
que, apesar de n + = 3 ser timo para gaussianas utilizando o UKF, seu uso fez com que a matriz
de covarincia deixasse de ser positiva. Portanto os resultados aqui apresentados consideram = 0.
5.2.1.1 Metodologia
Para realizar as simulaes, assumiu-se que o tempo total de funcionamento do sistema foi
de 10 segundos, com a frequncia de amostragem de cada sensor sendo 100Hz. O rudo de pro-
cesso para o modelo dinmico possui covarincia Q = 0, 1I3, enquanto os sensores possuem
covarincias Ra = 104 I3 , Rm = 104 I3 e Q = R = 102 I3 .
Para testar a qualidade numa grande gama de sistemas, a matriz de inrcia foi dada por J =
diag(Jx , Jy , Jz ) com Jx , Jy {0, 1; 0, 2; . . . ; 1} e Jz = 1. A escolha de Jz fixo no altera a resposta
do sistema, uma vez que, considerando apenas a atitude, possvel trocar as coordenadas livremente
sem perda de generalidade. Alm disso, normalmente valores menores de torque so utilizados
quando a inrcia reduzida. Portanto, os torques aplicados e seus rudos so multiplicados pela
53
Captulo 5. Filtragem de estados
matriz de inrcia, visando coloc-los na escala correta, e manter a covarincia resultante do rudo
de processo independente da inrcia do sistema. Isto ocorre porque, segundo a Eq. (5.24d), a
covarincia do rudo ser multiplicada por J1 durante a filtragem, o que compensa a multiplicao
por J Aqui descrita. Para cada conjunto de inrcia, foram realizadas um total de 50 simulaes pelo
mtodo de Monte Carlo, supondo conhecimento do quaternion e velocidade inicial corretos.
A tabela 5.1 apresenta um resumo dos parmetros utilizados para gerar os resultados.
Para cada simulao i, tem-se um conjunto {qJx ,Jy ,i,j } de quaternions corretos e outro con-
junto {qFJx ,Jy ,i,j } de quaternions estimados pelo filtro F . Seja
1
eFJx ,Jy ,i,j = qJx ,Jy ,i qFJx ,Jy ,i (5.38)
o quaternion de erro entre seus quaternions corretos e estimados associados, ele descreve a rotao
necessria para alinhar o sistema de coordenadas estimado B ao sistema de coordenadas B correto.
Sendo 0 eFJx ,Jy ,i,j a componente escalar deste quaternion, que pode ser assumida positiva sem perda
54
Captulo 5. Filtragem de estados
Com base nestas rotaes, ser utilizado como medida de erro o ngulo mdio de rotao
requerido para uma dada combinao de inrcia, resultando na seguinte expresso:
N T fs
1 X X
EFJx ,Jy = JFx ,Jy ,i,j , (5.40)
N 3T4fs i=1 j= T fs +1
4
onde os primeiros 25% dos dados so ignorados para evitar grande influncia do regime transitrio
do filtro.
Com esta medida de erro, pode-se construir uma matriz EF onde cada elemento dado por
EFJx ,Jy ,
Jx , Jy {0, 1; 0, 2; . . . , 1}. No entanto, como notado anteriormente, o par Jx , Jy no
gera resultados diferentes do par Jy , Jx , exceto pelo fato de se utilizar amostragens para gerar os
EF +ET
resultados. Assim, ambos os resultados so vlidos para 2 pares e uma matriz EF = 2
F
construda, o que permite obter uma superfcie mais suave sem perder a corretude.
importante ressaltar que o quaternion existente no estado, tanto do modelo dinmico quanto
do modelo cinemtico, foi normalizado a cada vez que o estado era calculado. No entanto, a
variao da norma do quaternion to pequena que estes ajustes no prejudicam os resultados.
5.2.1.2 Resultados
A figura 5.2 mostra os resultados obtidos utilizando o EKF e o modelo dinmico de atitude,
onde a figura 5.2a utiliza as Jacobianas calculadas conforme a Eq. (5.19) e a figura 5.2b utiliza a
Eq. (5.20). Para o UKF utilizando o modelo dinmico, o resultado apresentado na figura 5.3. Em
todos os casos do modelo dinmico, foi utilizada a previso correta da Eq. (5.15), uma vez que a
verso aproximada apresentou resultados muito piores.
A figura 5.4 mostra os resultados do EKF utilizando o modelo cinemtico com e sem apro-
ximao das funes, enquanto a figura 5.5 mostra os resultados para o UKF utilizando ou no a
12
Caso ela seja negativa, pode-se utilizar o fato de que q e q representam a mesma rotao.
55
Captulo 5. Filtragem de estados
0.21 0.21
0.2 0.2
EEKF [ ]
0.19 0.19
EEKF [ ]
0.18 0.18
0.17 0.17
0.16 0.16
0.15 0.15
1 1 1 1
0.8 0.8 0.8 0.8
0.6 0.6 0.6 0.6
0.4 0.4 0.4 0.4
0.2 0.2 0.2 0.2
Jy 0 0
Jx Jy 0 0
Jx
(a) Jacobianas corretas (b) Jacobianas aproximadas
0.21
0.2
EUKF [ ]
0.19
0.18
0.17
0.16
0.15
1 1
0.8 0.8
0.6 0.6
0.4 0.4
0.2 0.2
Jy 0 0
Jx
Figura 5.3: Filtragem com UKF usando modelo dinmico.
Por fim, a figura 5.6 apresenta o tempo necessrio por iterao para cada tipo de filtro e
modelo simulados.
5.2.1.3 Anlise
A partir das figuras 5.2 a 5.5, nota-se que todos os resultados apresentam comportamentos
semelhantes para os casos onde Jx e Jy so prximos de Jz , que vale 1. Esta reduo do erro pode
ser associada reduo do acoplamento dinmico entre as velocidades conforme os valores das
inrcias se aproximam.
56
Captulo 5. Filtragem de estados
1.4 1.4
1.2 1.2
EEKF [ ]
EEKF [ ]
1 1
0.8 0.8
0.6 0.6
0.4 0.4
0.2 0.2
0 0 0 0
0.2 0.2 0.2 0.2
0.4 0.4 0.4 0.4
0.6 0.6 0.6 0.6
0.8 0.8 0.8 0.8
Jy
1 1
Jx Jy
1 1
Jx
(a) Jacobianas e previso corretas (b) Jacobianas e previso aproximadas
1.4 1.4
1.2 1.2
EUKF [ ]
EUKF [ ]
1 1
0.8 0.8
0.6 0.6
0.4 0.4
0.2 0.2
0 0 0 0
0.2 0.2 0.2 0.2
0.4 0.4 0.4 0.4
0.6 0.6 0.6 0.6
0.8 0.8 0.8 0.8
Jy
1 1
Jx Jy
1 1
Jx
(a) Previso correta (b) Previso aproximada
Esta reduo do erro com reduo das inrcias o oposto do comportamento apresentado
pelos filtros utilizando o modelo cinemtico. Neste caso, reduo das inrcias resulta em aumento
significativo do erro. Conforme descrito anteriormente, valores distintos para as inrcias geram o
acoplamento entre as velocidades angulares, que se torna mais forte com a reduo das inrcias.
57
Captulo 5. Filtragem de estados
1
2
1.2
2.2
1.4
2.4
1.6
2.6
1.8
2.8
3
2.2
Como o modelo no possui cincia deste acoplamento, ele no pode utiliz-lo para gerar melhores
estimativas. Alm disso, como a leitura do giroscpio utilizada como entrada, a entrada que
teoricamente era conhecida passa a ter um valor errado.
Para entender porque isto causa problemas, considere a figura 5.7. A linha da esquerda repre-
senta a velocidade correta do sistema e a distribuio de probabilidade de leituras do giroscpio
est em tambm representada esquerda. Suponha que a leitura resultante seja z . Quando este
valor lido utilizado como entrada do modelo cinemtico, o modelo descreve a distribuio de
probabilidade da entrada pela curva central. Utilizando estas duas curvas nota-se que z tem uma
dada probabilidade na distribuio centrada em , sendo esta a mesma probabilidade de na dis-
tribuio assumida pelo modelo, centrada em z . No entanto, uma outra leitura possui, segundo
o modelo cinemtico, a mesma probabilidade de ocorrer da velocidade real , apesar de ser muito
distante da velocidade real. Isto motiva a ampliao e criao de rudos como normalmente feito
ao se utilizar o modelo cinemtico (T HRUN e outros, 2005). No entanto, no existe um guia para
dizer o quanto aumentar, uma vez que gera um compromisso entre velocidade de resposta e ro-
bustez do sistema. Como a frequncia de oscilao das velocidades se torna mais alta conforme a
inrcia reduz e o modelo original no linear, a lentido de resposta do sistema pode fazer com que
a estimativa calculada pelo filtro esteja longe do valor real, fazendo com que o filtro perca muito de
sua eficincia ou mesmo divirja.
Um fato notvel que, no modelo dinmico, o uso das Jacobinas aproximadas no gera
alterao do erro medido pelo filtro, fazendo com que apenas o uso da previso aproximada gere
58
Captulo 5. Filtragem de estados
z
Figura 5.7: Visualizao do problema ao usar a sada do giroscpio como entrada. A curva da
esquerda representa a distribuio de probabilidade de uma leitura do giroscpio, centrada no va-
lor correto de velocidade . A linha tracejada representa uma leitura do giroscpio z utilizada
como entrada do modelo cinemtico e sua distribuio de probabilidade correspondente, segundo
o modelo. A linha preta representa uma leitura com mesma probabilidade de ocorrer que ,
utilizando a probabilidade baseada em z .
resultados ruins. O mesmo acontece para o modelo cinemtico, mas este tambm possui a mesma
resposta quando o modelo aproximado de previso usado. Como os modelos aproximados so
mais rpidos de serem calculados, conforme a figura 5.6, e no apresentam alteraes significativas
dos resultados, eles devem ser usados ao invs dos modelos reais. O EKF com o modelo cinemtico
aproximado leva em torno de 1ms para computar um passo de previso e correo. Tendo em
mente que o cdigo projetado foca na flexibilidade de testes, utilizando Python e com o filtro
independendo do modelo do sistema, a otimizao do cdigo em uma linguagem de nvel inferior
para um caso particular deve gerar aumento significativo de performance.
Ao analisar os grficos de erro do UKF e EKF, nota-se que eles no apresentaram diferenas
perceptveis. Apesar de o UKF ser teoricamente mais robusto, a aproximao linear para este
sistema razovel, permitindo que o EKF tenha desempenho considervel. Alm disso, ao analisar
o tempo do EKF e UKF para computar o modelo dinmico, claro que o UKF significativamente
mais lento, pois deve computar a integral do sistema para cada ponto sigma. No modelo cinemtico,
o uso do passo aproximado no gerou grandes benefcios para o UKF, uma vez que grande parte da
computao neste caso foi gasta na parte que independe do modelo, como clculo da raiz quadrada
da matriz de covarincia e clculo da sada esperada. Por estes motivos, o EKF, neste caso, se
apresenta como uma melhor alternativa.
59
Captulo 5. Filtragem de estados
Por fim, apesar do modelo cinemtico apresentar erros, em mdia, de cerca de 1, 4 contra
0, 2 do modelo dinmico, esta diferena desprezvel na maioria dos casos e estes erros so muito
menores do que ocorre na realidade. Alm disso, este modelo apresenta duas vantagens em relao
ao modelo dinmico.
Em primeiro lugar, tem-se que ele consideravelmente mais rpido de ser computado, sendo
esta diferena de aproximadamente uma ordem de grandeza. Esta diferena mais significativa
se for levado em conta que o algoritmo ser utilizado num sistema de tempo real para controle.
Apesar de possuir menos preciso, existe a possibilidade de ter uma malha de controle mais rpida
do que se o modelo dinmico fosse utilizado, possivelmente compensando os erros mais elevados.
Em seguida, tem-se que o sistema possui dinmicas que no foram introduzidas no modelo
dinmico, como discutido na seo 3.1. Para no permitir que filtro utilizando o modelo dinmico
se torne inconsistente, possvel aumentar o nvel de rudo considerado pelo sistema, reduzindo
sua performance. Como dito anteriormente, no existe um guia para escolha desse nvel de rudo,
fazendo com que o uso do modelo dinmico deva considerar o mximo de dinmica do sistema
possvel. No entanto, quanto mais fatores forem considerados no modelo, mais difcil se torna a
obteno de todos os parmetros e mais complexa se torna a etapa de previso do sistema. O mo-
delo cinemtico, em contrapartida, funciona exatamente da mesma maneira para qualquer sistema,
independente de quo complexa seja sua dinmica.
Na seo anterior, chegou-se concluso de que o modelo cinemtico de atitude mais reco-
mendvel para ser utilizado, devido sua robustez com relao a dinmicas no modeladas e menor
tempo de computao. No entanto, sabe-se tambm que uma prtica comum aumentar o nvel
de rudo de processo no modelo cinemtico, visando evitar que o filtro se torne inconsistente. Ser
analisado portanto o efeito gerado pelo aumento da covarincia de entrada do modelo cinemtico.
5.2.2.1 Metodologia
Para esta anlise, ser utilizado o mesmo mtodo descrito na seo 5.2.1.1. Utilizar-se- tam-
bm os mesmos valores aleatrios, evitando que um novo conjunto de valores leve a observaes
inconsistentes.
60
Captulo 5. Filtragem de estados
Como no apenas um filtro ser utilizado a partir desta seo, a notao do erro de rotao
simplificada para E .
5.2.2.2 Resultados
A figura 5.8 traz o erro ao utilizar o filtro com diversos ganhos da matriz de covarincia. O
erro calculado atravs da Eq. (5.40).
5.2.2.3 Anlise
Conforme o esperado, tem-se que um aumento do rudo gerou uma reduo do erro, forne-
cendo uma justificativa para esta prtica to comum. Ao passar de = 1 para = 10, o erro foi
reduzido quase pela metade nos piores casos, sem afetar significativamente a regio onde os erros
j eram reduzidos. No entanto, ao utilizar = 100, a superfcie de erro se torna mais plana, ainda
reduzindo o erro nos piores casos, mas gerando resultados piores nos demais.
Este aumento do erro ocorre porque, ao aumentar o rudo de processo, aumenta-se automa-
ticamente o peso das medies. No entanto, as medies tambm possuem rudos, sendo estes
completamente independentes da dinmica do sistema. Assim, o resultado corrobora com a hip-
tese formulada na anlise da seo anterior, que disse que o erro era menor nas regies onde as
inrcias so prximas devido ao comportamento da velocidade. Ao considerar a velocidade mais
imprecisa, h uma elevao do erro do sistema.
Quando = 1000, o filtro praticamente ignora a previso feita pelo sistema, fazendo com
que a superfcie de erro seja praticamente plana. Alm disso, o nvel de erro se eleva em todos
os casos, o que leva a crer que o uso apenas das medies do acelermetro e magnetmetro para
estimao do quaternion no recomendada.
61
Captulo 5. Filtragem de estados
1.4 1.4
1.2 1.2
1 1
E [ ]
E [ ]
0.8 0.8
0.6 0.6
0.4 0.4
0.2 0.2
0 0 0 0
0.2 0.2 0.2 0.2
0.4 0.4 0.4 0.4
0.6 0.6 0.6 0.6
0.8 0.8 0.8 0.8
Jy
1 1
Jx Jy
1 1
Jx
(a) = 1 (b) = 10
1.4 1.4
1.2 1.2
1 1
E [ ]
E [ ]
0.8 0.8
0.6 0.6
0.4 0.4
0.2 0.2
0 0 0 0
0.2 0.2 0.2 0.2
0.4 0.4 0.4 0.4
0.6 0.6 0.6 0.6
0.8 0.8 0.8 0.8
Jy
1 1
Jx Jy
1 1
Jx
(c) = 100 (d) = 1000
Ao analisar o efeito do aumento da covarincia do rudo de processo, nota-se que ele visa
nivelar a superfcie de erro. Apesar de isto beneficiar o caso geral, levanta a questo se seria
possvel desenvolver um algoritmo que ajustasse conforme a necessidade de cada sistema. No
caso onde as inrcias so prximas, o valor da covarincia deve permanecer o mesmo, mas deve
ser elevado para os casos onde as inrcias so menores. Alm disso, como baseado no modelo
cinemtico, esta adaptao no deve depender do conhecimento das inrcias do sistema.
62
Captulo 5. Filtragem de estados
Considere as estimativas qk|k e qk1|k1 obtidas pelo filtro. Supondo que a velocidade entre
eles foi constante e que o intervalo foi de Ts segundos, a Eq. (A.16) pode ser usada para estimar a
velocidade angular k1 como:
log qk|k q1
k1|k1
k1|k = 2 . (5.41)
Ts
Note que a suposio de velocidade constante foi a mesma feita na construo do modelo cinem-
tico ao utilizar a sada do giroscpio como entrada.
Seja zk1 a leitura do giroscpio utilizada para computar qk|k1 a partir de qk1|k1 e con-
siderando k1 como o valor real k1 da velocidade, possvel estimar o rudo de entrada k1 .
Utilizando a Eq. (5.30), chega-se a
Baseando-nos numa ideia semelhante apresentada por Han e outros (2009), possvel adap-
tar uma matriz de covarincia Q,k com objetivo de melhorar a resposta do sistema, de maneira
parecida com a matriz de covarincia Q descrita na seo 5.2.2.1. Esta estimativa permite que a
covarincia se adapte ao comportamento do sistema no momento, fornecendo estimativas mais pre-
cisas. Ela tambm deve ser eficiente de ser calculada, devido restrio do tempo de computao.
Utilizando um filtro passa-baixa para as estimativas, tem-se que, partindo de uma estimativa
inicial Q,0 , o algoritmo iterativo dado por:
T
Q,k = Q,k1 + (1 ) k1|k k1|k , (5.43)
5.2.3.2 Metodologia
Novamente, utiliza-se como base o mtodo descrito na seo 5.2.1.1, juntamente com os
mesmos valores aleatrios, evitando interpretaes errneas.
63
Captulo 5. Filtragem de estados
Para escolha do valor de , o intervalo [0, 8; 1, 0] ser analisado, uma vez que valores muito
baixos de devem gerar comportamento mais errtico do sistema. A cada , sero comparados
o erro mximo, mediano e mnimo para os casos Jx = Jy = 0, 1 e Jx = Jy = 1, utilizando a
Eq. (5.40) para clculo do erro. A condio inicial para estimativa do rudo de entrada dada por
Q,0 = 100R , onde R a covarincia do giroscpio e o ganho foi escolhido por tornar o erro
pequeno e uniforme na seo anterior.
5.2.3.3 Resultados
A figura 5.9 apresenta as medianas do erro obtido para as inrcias em seus extremos e
variando-se o parmetro . Alm disso, so mostrados tambm os mnimos e mximos do erro.
1 0.7
0.1 0.1
1.0 1.0
0.9 0.65
0.6
0.8
0.55
0.7
E [ ]
E [ ]
0.5
0.6
0.45
0.5
0.4
0.4
0.35
0.3 0.3
0.2 0.25
0.8 0.82 0.84 0.86 0.88 0.9 0.92 0.94 0.96 0.98 1
0.8 0.82 0.84 0.86 0.88 0.9
0.92 0.94 0.96 0.98 1
(a) [0, 8; 1, 0] (b) [0, 8; 0, 99]
Na figura 5.10, tem-se a superfcie de erro resultante para inrcias variantes utilizando =
0, 97. O tempo mdio de computao de cada iterao apresentado na figura 5.11 com o nome de
AEKF, juntamente com o tempo do EKF da figura 5.6a.
5.2.3.4 Anlise
64
Captulo 5. Filtragem de estados
0.55
0.5
E [ ]
0.45
0.4
0.35
0 0
0.2 0.2
0.4 0.4
0.6 0.6
0.8 0.8
Jy
1 1
Jx
2.9
Tempo (log10 [s])
2.95
3.05
EKF AEKF
Figura 5.11: Tempo mdio para computao de previso e correo. O prefixo A indica a adap-
tao da matriz de covarincia.
gerar erros maiores, provavelmente devido a passos muito grandes da adaptao. J para valores
elevados de , claro que existe um vale em torno do qual todos os erros se tornam menores.
Analisando o caso Jx = Jy = 0, 1, que apresenta piores respostas, o mnimo de suas curvas est
prximo de = 0, 97, onde este valor tambm gera erros reduzidos para o caso onde as inrcias
so iguais.
65
Captulo 5. Filtragem de estados
comportamento do sistema, ele foi capaz de perceber a regularidade das velocidades e se adaptar
de acordo, conforme desejado.
Com a atitude estimada, esta seo busca estimar a velocidade angular. As nicas duas pos-
sveis fontes de informao so a leitura do giroscpio e estimao atravs de quaternions conse-
cutivos, como feito na seo 5.2.3.
k = k1 + k1 ,
k1 N (0, Q ), (5.44)
Usando a mesma ideia da seo 5.2.3, a matriz de covarincia Q ser adaptada para que a
previso do filtro seja mais precisa. Para isto, ser utilizada uma estimativa de k1
dada por:
k1|k = k|k k1|k1, (5.45)
66
Captulo 5. Filtragem de estados
onde as estimativas so dadas pelo filtro. A partir de uma condio inicial ,0 , a adaptao pode
ser escrita como: T
Q,k = Q,k1 + (1 )k1|k
k1|k , (5.46)
5.2.4.2 Metodologia
O mtodo descrito na seo 5.2.1.1 ser utilizado como base e os valores utilizados para suas
simulaes. O resultado ser avaliado para uma condio inicial de covarincia Q,0 = I3 , uma vez
que o algoritmo adaptativo capaz de corrigir a matriz adequadamente. A condio inicial dada ao
filtro composta pelo valor correto de velocidade e uma matriz de covarincia igual identidade.
A medida de erro ser dada pela mdia da norma do erro entre a velocidade real e a estimada,
podendo ser escrita como:
N T fs
1 X X
EJx ,Jy = kJx ,Jy ,i,j Jx ,Jy ,i,j k, (5.47)
N 3T4fs i=1 j= T fs +1
4
onde Jx ,Jy ,i,j so as velocidade angulares corretas e Jx ,Jy ,i,j so suas estimativas. Utiliza-se a
matriz E que construda a partir dos elementos EJx ,Jy da maneira discutida na seo 5.2.1.1
para o erro de atitude.
5.2.4.3 Resultados
A figura 5.12 apresenta os erros de estimao para dois casos de inrcia ao variar o parmetro
, enquanto a figura 5.13 mostra a superfcie de erro ao variar a inrcia do sistema e manter =
0, 98. O tempo requerido pelo algoritmo mostrado na figura 5.14.
5.2.4.4 Anlise
Para o caso onde as inrcias valem 1, 0, evidente a partir da figura 5.12 que o valor = 0, 98
o timo. J para os valores de 0, 1, tem-se que este mesmo valor de o timo para a mediana e
o mximo e possui desempenho semelhante para o mnimo, sendo portanto o escolhido.
67
Captulo 5. Filtragem de estados
0.165
0.1
1.0
0.16
0.155
0.15
E [rad/s]
0.145
0.14
0.135
0.13
0.125
0.12
0.115
0.8 0.82 0.84 0.86 0.88 0.9 0.92 0.94 0.96 0.98 1
Figura 5.12: Mximo, mediana e mnimo do erro para Jx = Jy = 0, 1 e Jx = Jy = 1 variando-se
.
0.15
0.145
E [rad/s]
0.14
0.135
0.13
0.125
0.12
0 0
0.2 0.2
0.4 0.4
0.6 0.6
0.8 0.8
Jy
1 1
Jx
3.5
Tempo (log10 [s])
3.55
3.6
3.65
68
Captulo 5. Filtragem de estados
A superfcie de erro da figura 5.13 refora o fato de que inrcias menores gera mais aco-
plamento entre as velocidades, o que pode fazer com que variem mais rapidamente e sejam mais
difceis de estimar. O erro apresentado pequeno em relao s velocidades do sistema, fazendo
com que o filtro, apesar de extremamente simples, possa ser utilizado para estimar a velocidade an-
gular. Alm disso, seu tempo de computao da ordem de 300s, como mostrado na figura 5.14.
Este tempo de computao reduzido se deve ao fato do filtro ser linear e possuir equaes extrema-
mente simples de previso e sensoriamento.
As duas sees anteriores elaboraram filtros independentes para estimao da atitude e velo-
cidade angular. No entanto, sabe-se a partir do modelo dinmico que estes estados so fortemente
acoplados, levantando questionamentos sobre a validade desta aproximao. Portanto, esta seo
explora dois outros modelos de independncia e compara os resultados obtidos.
onde Pk1|k1 a covarincia da velocidade angular estimada por seu filtro e Q,k1 um termo
adicional de erro.
69
Captulo 5. Filtragem de estados
A cada passo da estimativa da atitude, um novo valor para Q,k computado de acordo com
a Eq. (5.43). Como Pk1|k1 definida pelo filtro da velocidade angular e no pode ser alterada
pelo filtro de atitude, deve-se fazer
para que a adaptao ocorra como esperado. Note que Q,k no necessariamente uma matriz
positiva definida, uma vez que a adaptao pode escolher utilizar uma covarincia menor do que a
real.
5.2.5.2 Metodologia
Sero utilizados os mesmos parmetros e mtodos de avaliao discutidos nas sees 5.2.1.1
e 5.2.4.2, alm de utilizar = 0, 97 e = 0, 98 no caso de adaptao das matrizes de covarincia.
5.2.5.3 Resultados
A figura 5.15 apresenta a superfcie de erro de atitude para o modelo de malha aberta. O
erro da velocidade angular o mesmo da seo anterior, uma vez que no h modificao na sua
estimao, e mostrado na figura 5.13.
70
Captulo 5. Filtragem de estados
0.6
0.55
EEKF [ ]
0.5
0.45
0.4
0.35
0 0
0.2 0.2
0.4 0.4
0.6 0.6
0.8 0.8
Jy
1 1
Jx
4.5 4.5
E [ ]
E [ ]
4 4
3.5 3.5
3 3
2.5 2.5
1 1 1 1
0.9 0.9 0.9 0.9
0.8 0.8 0.8 0.8
0.7 0.7 0.7 0.7
0.6 0.6 0.6 0.6
0.5 0.5 0.5 0.5
0.4 0.4 0.4 0.4
0.3 0.3 0.3 0.3
Jy 0.2
0.1 0.1
0.2
Jx Jy 0.2
0.1 0.1
0.2
Jx
(a) Q = 100I (b) Q = 1000I
5.2.5.4 Anlise
Comparando as figuras 5.15 e 5.10, fica claro que o modelo que utiliza a leitura direta do
giroscpio como entrada possui erros inferiores. Isto se d ao fato da adaptao da matriz de cova-
rincia ser totalmente controlada pelo algoritmo de estimao de atitude, o que no ocorre quando
a covarincia da estimativa da velocidade angular utilizada como parte do rudo de processo.
Tem-se portanto que, apesar da hiptese de independncia entre a velocidade angular e a atitude
ser forte, ela prov uma melhor estimativa.
Este resultado confirmado ao analisar as figuras 5.16 e 5.17. Conforme aumenta-se o rudo
da entrada da velocidade angular, chega-se a um ponto onde este valor no afeta mais significati-
71
Captulo 5. Filtragem de estados
1 0.24
0.8 0.22
E [ ]
E [ ]
0.6 0.2
0.4 0.18
0.2 0.16
0
0.1 0.1 0.1 0.1
0.2 0.2 0.2 0.2
0.3 0.3 0.3 0.3
0.4 0.4 0.4 0.4
0.5 0.5 0.5 0.5
0.6 0.6 0.6 0.6
0.7 0.7 0.7 0.7
0.8 0.8 0.8 0.8
Jy
0.9
1 1
0.9
Jx Jy
0.9
1 1
0.9
Jx
(a) Q = 100I (b) Q = 1000I
Figura 5.17: Erro de estimao da velocidade angular para o modelo de malha fechada.
vamente a estimativa da atitude, mas a superfcie de erro da velocidade angular passa a se tornar
mais plana. Apesar desta uniformizao do erro ser indicada para desenvolvimento de algoritmos
que independam da inrcia do sistema, o menor erro para Q = 1000I maior do que o maior
erro do modelo adaptativo apresentado na seo 5.2.4. Como o filtro acoplado no apresentou bons
resultados com adaptao da covarincia de sua entrada, uma vez que se tornava muito confiante
nas suas estimativas de velocidade angular devido s correes do acelermetro e magnetmetro,
ele no foi capaz de competir com um filtro mais simples mas capaz desta adaptao.
A estimao de atitude com base em acelermetro e magnetmetro funciona muito bem por
utilizar dois vetores no co-lineares, fazendo com que, ignorando-se o rudo, haja apenas uma
rotao do sistema de coordenadas que gere aquelas leituras. Entretanto, ao utilizar apenas um
sensor, tem-se uma infinidade de rotaes que fornecem tal leitura. Para visualizar o problema,
determine uma rotao que gere a leitura. Em seguida, rotacione em torno do vetor de referncia.
Note que a leitura no se alterou, mas h uma nova rotao. Entretanto, o magnetmetro possui
componentes no nulas em duas direes, ao contrrio do acelermetro, fazendo com que uma srie
de leituras limitem significativamente a quantidade de rotaes possveis para cada uma quando
leva-se em conta a velocidade angular do sistema, sendo esta obtida pelo giroscpio.
A anlise realizada nesta seo fornece um princpio para estimao da posio utilizando o
acelermetro, que ser explorada na seo seguinte.
72
Captulo 5. Filtragem de estados
5.2.6.1 Metodologia
So utilizados os mesmos mtodos das sees 5.2.2 e 5.2.3, mas as leituras do acelermetro
so desconsideradas. Para o algoritmo adaptativo, utiliza-se = 0, 97 e, para a covarincia esttica,
Q = R com {1, 10}, onde R a covarincia da leitura do giroscpio.
5.2.6.2 Resultados
2 2
1.8 1.8
1.6 1.6
E [ ]
E [ ]
1.4 1.4
1.2 1.2
1 1
0.8 0.8
0.6 0.6
0.4 0.4
0 0 0 0
0.2 0.2 0.2 0.2
0.4 0.4 0.4 0.4
0.6 0.6 0.6 0.6
0.8 0.8 0.8 0.8
Jy
1 1
Jx Jy
1 1
Jx
(a) = 1 (b) = 10
3.5
2.5
E [ ]
1.5
0.5
0 0
0.2 0.2
0.4 0.4
0.6 0.6
0.8 0.8
Jy
1 1
Jx
73
Captulo 5. Filtragem de estados
5.2.6.3 Anlise
Observando a figura 5.18, nota-se que o aumento da covarincia de entrada gera piores re-
sultados para todas as combinaes de inrcia, o que no ocorria na seo 5.2.3. Como analisado
anteriormente, o aumento da covarincia visa colocar mais peso nas leituras dos sensores. Entre-
tanto, foi discutido no incio desta seo o fato da leitura apenas do magnetmetro poder ser gerada
por uma infinidade de atitudes, fazendo com que a dinmica do sistema seja mais importante du-
rante a estimao do que quando havia dois sensores distintos para leitura.
O algoritmo adaptativo, cujo resultado apresentado na figura 5.19, apresenta erros mais
elevados do que apresentados com a covarincia esttica, mesmo ignorando a anomalia de elevado
erro. Novamente, isto ocorre devido a estimaes menos corretas da atitude, o que por sua vez gera
estimaes incorretas da velocidade de entrada e seu nvel de rudo.
Portanto, para estimao utilizando apenas o magnetmetro, deve-se utilizar uma covarincia
esttica e de mesmo valor do rudo do giroscpio, visando minimizar os erros de atitude. Esta con-
cluso ser utilizada como base para escolha dos parmetros correspondentes na seo seguinte.
Nas sees anteriores, ao utilizar apenas o modelo de atitude, foi desconsiderado o efeito da
acelerao do sistema. Se houver uma referncia externa para estimao da acelerao, ela pode ser
utilizada para compensar a acelerao na leitura do acelermetro, ficando apenas com a gravidade.
Caso esta referncia no exista, possvel utilizar a prpria leitura do acelermetro, mas
h um custo de aumentar o erro de estimao da atitude, como discutido na seo anterior. Esta
tcnica chama dead reckoning e utilizada por sistemas quando apenas leituras odomtricas esto
presentes.
Esta seo desenvolve um filtro que capaz de estimar a atitude e posio do sistema atravs
do acoplamento entre dois filtros. Alm dos sensores utilizados anteriormente, tambm utilizado
um GPS para correo da posio.
74
Captulo 5. Filtragem de estados
O GPS possui grandes erros e frequncia de amostragem menor do que os outros sensores
utilizados. Tais caractersticas fazem com que seu uso para correo de um modelo completo,
onde h acoplamento entre a posio do sistema e sua atitude, possa ser problemtica devido
aproximao linear do espao SO(4) dos quaternions. Alm disso, o modelo completo possui um
custo computacional muito alto devido dimenso do vetor de estados. Por estes motivos, foram
desenvolvidos dois filtros onde a informao flui em apenas uma direo entre subsistemas. A
figura 5.20 apresenta o modelo de Markov a ser utilizado, onde supe-se que todos os sensores
possuem a mesma frequncia de amostragem para facilitar o desenho.
zm
k
vk+1
zk1 qk aIk
pk+1 zGP
k+1
S
aBk zak
zm
k1
vk
zk2 qk1 aIk1
pk zGP
k
S
aBk1 zak1
Figura 5.20: Modelo de Markov para estimao conjunta da posio e atitude. Os ns em branco
so as variveis escondidas, os em cinza so observaes e os ns quadrados no permitem fluxo
de informao para seus ns superiores.
Este modelo pode ser visto como um sistema mestre-escravo, onde o sistema de atitude e ace-
lerao do sistema funciona de forma independente, sendo portanto o mestre. A partir dos estados
75
Captulo 5. Filtragem de estados
do mestre, uma funo determinstica computada para chegar acelerao no sistema inercial, a
qual fornecida de entrada para o clculo da posio e velocidade pelo sistema escravo. Fazendo
com que esta informao de acelerao s flua numa direo, os dois sistemas so desacoplados ao
considerar o problema de filtragem.
Neste modelo, tem-se uma parte que composta pelo filtro cinemtico utilizado anterior-
mente estendido para que haja uma estimao da acelerao do sistema. A evoluo da atitude do
sistema permanece a mesma descrita na seo 5.1.2.2, sendo independente da evoluo da estima-
tiva da acelerao aB .
Como no h nenhuma fonte externa para fornecer informaes sobre a acelerao, ela
modelada como um processo que apenas recebe um rudo aleatrio e possui comportamento igual
estimao online do bias do acelermetro (S ABATINI, 2006). Sua evoluo dada por:
a,B a,B
aBk = aBk1 + k1 , k1 N (0, Qa,B
) (5.50)
A leitura do acelermetro, dada pela Eq. (5.34) e supondo r = 0, pode ser reescrita como:
Com estas modificaes, tem-se um modelo para estimao da atitude qk|k e da acelerao
do sistema aBk|k . Pode-se utilizar estas estimaes para gerar uma estimao da acelerao aIk|k no
sistema inercial atravs da igualdade
aIk = R(qk )aBk (5.52)
Para calcular a covarincia da estimativa da acelerao aIk , deve-se utilizar expanso da srie
de Taylor da Eq. (5.52). Assim, a estimativa dada por:
Pa,I B T B
k|k = J(qk|k , ak|k )Pk|k Jk (qk|k , ak|k ), (5.53)
76
Captulo 5. Filtragem de estados
Uma vez calculada a acelerao no referencial inercial e sua covarincia, ela pode ser utili-
zada como a entrada p no modelo da Eq. (5.28). No entanto, necessrio chamar ateno para a
converso de escala feita. Os acelermetros usualmente medem acelerao em relao gravidade
e este modelo foi utilizado neste trabalho ao assumir que a gravidade vale g = [0, 0, 1]T . Como
a estimativa da posio dado em metros, devido leitura do GPS e por ser uma unidade do SI,
deve-se aplicar a converso devida. Seja g a escala da gravidade, tem-se:
= g aIk|k ,
p Ppk|k = 2g Pa,I (5.55)
k|k
5.2.7.2 Metodologia
Todos os parmetros relativos parte angular do filtro so semelhantes aos da seo 5.2.1.1 e
as mesmas variveis aleatrias so utilizadas, permitindo a comparao com as sees anteriores.
Alm disso, conforme discutido na seo 5.2.6, a leitura do giroscpio utilizada como entrada do
modelo cinemtico, com sua covarincia como covarincia do rudo de entrada.
Para cada uma das N simulaes, gerada uma matriz de covarincia Qp para gerar rudos
10
de entrada para p como Qp = I
2g 3
de forma que, no instante k, a acelerao dada por:
77
Captulo 5. Filtragem de estados
onde pk,1 constante por partes. A constante gravitacional, utilizada para converso de valores em
m/s2 para g dada por g = 9, 81.
Para a condio inicial, todos os estados tiveram seus valores zerados, exceto pelo quaternion,
que possui o valor correto. Como assume-se que o sistema comea parado e a posio computada
em relao sua posio inicial, a covarincia do filtro de posio teve sua condio inicial zerada.
Para o quaternion, foi utilizado um valor inicial para a covarincia de I4 , enquanto foi utilizado
104 I3 para a acelerao.
O GPS foi configurado com uma frequncia de amostragem de 10Hz, sendo 10 vezes menor
do que os demais sensores, e uma covarincia RGP S = 10I3.
A tabela 5.2 apresenta um resumo dos parmetros adicionais utilizados para simulao, sendo
estes utilizados em conjunto com os parmetros da tabela 5.1.
Para a estimao da atitude, a mesma Eq. (5.40) utilizada para o erro. J para a posio,
utilizada a mdia da norma dos erros, dada por:
N T fs
1 X X
EPJx ,Jy = kpi,j pJx ,Jy ,i,j k, (5.57)
N 3T4fs i=1 j= T fs +1
4
onde pi,j so as posies corretas obtidas pelo modelo de posio, que independe do modelo de
78
Captulo 5. Filtragem de estados
atitude e, portanto, das inrcias, e pJx ,Jy ,i,j so as estimativas da posio obtidas pelo filtro. A
matriz de erros EP construda a partir de EPJx ,Jy da mesma maneira que EF a partir de EFJx ,Jy no
fim da seo 5.2.1.1.
5.2.7.3 Resultados
Na figura 5.21, tem-se as superfcies de erro da atitude e posio ao variar a inrcia do sistema.
3 1.34
1.33
2.5
1.32
EP [m]
2 1.31
E [ ]
1.3
1.5
1.29
1 1.28
1.27
0.5
1.26
0 1.25
0 0 0 0
0.2 0.2 0.2 0.2
0.4 0.4 0.4 0.4
0.6 0.6 0.6 0.6
0.8 0.8 0.8 0.8
Jy
1 1
Jx Jy
1 1
Jx
(a) Atitude (b) Posio
A figura 5.22 mostra como os erros de atitude evoluem com o tempo para diferentes valores
de inrcias, enquanto a figura 5.23 faz o mesmo para a posio.
79
Captulo 5. Filtragem de estados
60 70
60
50
50
40
40
[ ]
[ ]
30
30
20
20
10
10
0 0
0 5 10 15 20 25 30 0 5 10 15 20 25 30
Figura 5.22: Evoluo do erro mximo, mediano e mnimo de atitude para = 1000.
5 5
4.5 4.5
4 4
3.5 3.5
3 3
P [m]
P [m]
2.5 2.5
2 2
1.5 1.5
1 1
0.5 0.5
0 0
0 5 10 15 20 25 30 0 5 10 15 20 25 30
Figura 5.23: Evoluo do erro mximo, mediano e mnimo de posio para = 1000.
5.2.7.4 Anlise
Observando a figura 5.21, nota-se que os resultados obtidos so bem satisfatrios, chegando a
baixos valores de erro de posio e atitude. Para valores de maiores do que 1000 no h benefcios
significativos na posio, uma vez que este valor j descarta grande parte da estimativa propagada
a partir da estimao da atitude e acelerao, baseando-se principalmente na leitura do GPS. J
para valores menores, o filtro apresenta melhores estimativas para o caso mediano at = 10,
a partir do qual se torna excessivamente confiante nas estimativas. No entanto, o pior caso do
erro da estimao aumenta consideravelmente para valores de abaixo de 1000, estabelecendo um
80
Captulo 5. Filtragem de estados
trade-off entre performance e robustez. Caso algum algoritmo adaptativo de estimao do ganho
da entrada fosse utilizado, talvez fosse possvel melhorar o desempenho, como foi feito com a
estimao da atitude na seo 5.2.3.
A partir da figura 5.22, fica claro que o sistema apresenta comportamento oscilatrio signifi-
cativo para valores pequenos das inrcias Jx e Jy , reduzindo a qualidade da estimativa. Entretanto,
mesmo o pior caso do erro de atitude aps o perodo transitrio est prximo de 10 graus, o que
um valor consideravelmente baixo. Esta diferena entre as inrcias no se repete na estimao
da posio, apresentada na figura 5.23, uma vez que o comportamento do sistema praticamente
todo descartado devido ao alto ganho do erro de entrada, como discutido anteriormente. Apesar de
se basear principalmente nas leituras do GPS, o erro obtido considerado pequeno para voos em
campo aberto.
Como a etapa de previso da posio ocorre a cada iterao, mas a correo ocorre apenas
quando h uma leitura do GPS, o que ocorre a cada 10 iteraes na simulao, espera-se que a
distribuio do tempo utilizado possua dois modos. A figura 5.24 confirma esta expectativa, com
o pico menor sendo cerca de 9 vezes menor do que o maior. A partir desta figura, claro que a
estimativa completa do filtro pode ser feita em menos de 1,5ms mesmo quando ocorre a correo,
o que somado com os 300s da estimao da velocidade angular, apresentada na seo 5.2.4,
fornece estimao completa dos estados em menos de 2ms. Como discutido anteriormente na
seo 5.2.1.3, este tempo ser alterado quando otimizaes das operaes forem feitas no sistema
final, possivelmente compensando o processador mais lento do sistema embarcado.
81
Captulo 5. Filtragem de estados
Os filtros desenvolvidos nas sees anteriores foram testados com velocidades, torques e
foras elevadas para avaliar sua robustez. Nesta seo, sero utilizados os mesmos filtros para
avaliar o comportamento do sistema a partir de seu equilbrio e utilizando apenas rudo gaussiano,
para avaliar os erros obtidos.
5.2.8.1 Metodologia
O mtodo aqui utilizado baseado naqueles descritos nas sees 5.2.1.1, 5.2.7.2 e 5.2.4.2,
realizando escolhas de parmetros e pequenas alteraes na gerao dos dados.
A condio inicial em todos os casos de velocidade angular nula e quaternion sem rotao.
O parmetro u1 da tabela 5.1 fixado em 0, fazendo com que os nicos torques aplicados sejam
rudos gaussianos. Fazendo o mesmo para a acelerao, o parmetro u1 da tabela 5.2 zerado.
Estas excitaes representam esforos constantes aplicados ao sistema, que podem ser gerados
pelo rotores ou por ventos. Assim, considerara-se aqui apenas os esforos causados por fontes
variantes, representadas pelos rudos gaussianos,
5.2.8.2 Resultados
5.2.8.3 Anlise
82
Captulo 5. Filtragem de estados
2.8 1.76
2.7
1.755
2.6
1.75
EP [m]
E [ ]
2.5
1.745
2.4
1.74
2.3
1.735
2.2
2.1 1.73
2 1.725
1 1 1 1
0.8 0.8 0.8 0.8
0.6 0.6 0.6 0.6
0.4 0.4 0.4 0.4
0.2 0.2 0.2 0.2
Jy 0 0
Jx Jy 0 0
Jx
(a) Erro mdio de atitude. (b) Erro mdio de posio.
0.047 1.26
0.046
E [rad/s]
1.24
0.045 EV [m/s]
0.044 1.22
0.043
1.2
0.042
0.041 1.18
0.04
0 0 1.16
1 1
0.2 0.2
0.8 0.8
0.4 0.4
0.6 0.6
0.6 0.6
0.4 0.4
0.8 0.8
0.2 0.2
Jy
1 1
Jx Jy 0 0
Jx
(c) Erro mdio de velocidade angular. (d) Erro mdio de velocidade linear.
J o erro mdio de atitude foi reduzido ligeiramente, alm de assumir um intervalo menor
de valores. Como a velocidade angular muito inferior aos casos tratados anteriormente, o filtro
capaz de estim-la facilmente. Como era de se esperar, os casos onde no h um forte acopla-
mento entre as velocidades angulares, o que evita que elas aumentem facilmente, apresentam erros
semelhantes de estimao de todos os estados
O nvel de erro obtido para a atitude e velocidade angular permite o uso do quadrirrotor em
ambiente abertos ou fechados, contanto que no haja grandes variaes do campo magntico ou
alguma outra forma de estimar a atitude, como uma cmera. O erro de posio factvel com am-
bientes abertos, mas muito grande para ambientes fechados. No entanto, o GPS foi desenvolvido
para ambientes abertos e sabido que ele possui erros elevados dentro de estruturas. Caso o sistema
deva ser utilizado em ambiente fechado, alguma outra forma de computao da posio deve ser
83
Captulo 5. Filtragem de estados
5.3 Concluso
A partir dos resultados apresentados na seo 5.2.1, pode-se concluir que o EKF utilizando
modelos aproximados para estimao da atitude do sistema prefervel em relao ao EKF usando
modelo exato e UKF. Apesar do modelo cinemtico apresentar erros maiores do que o modelo
dinmico, estes erros ainda so consideravelmente baixos. Como o modelo dinmico depende do
conhecimento da inrcia do sistema, cujo valor incorreto pode reduzir a performance do filtro, e do
rudo aplicado a ele, que pode ser variante e difcil de mensurar, uma vez que deve incluir efeitos de
vento e de dinmica presente no sistema mas no modelada, como arrasto aerodinmico, o modelo
cinemtico se apresenta coma uma boa alternativa. Alm disso, o tempo de computao do modelo
cinemtico consideravelmente menor, o que justifica seu uso nos diversos sistemas de IMU e
MARG comerciais existentes.
Na seo 5.2.2, observa-se que variar o nvel de rudo na entrada do modelo cinemtico nem
sempre bom, apesar de ser uma prtica comum (T HRUN e outros, 2005). Conforme o rudo
aumenta, nota-se que algumas partes da superfcie de erro se elevam enquanto outras abaixam,
at chegar um ponto onde a entrada praticamente desconsiderada na filtragem, o que gerou um
aumento do erro em toda a superfcie. Visando evitar que o erro aumente muito nas regies que
j era reduzido, mas diminua nas demais, foi desenvolvido um algoritmo de ajuste da matriz de
covarincia na seo 5.2.3. Este algoritmo possui apenas um parmetro, cuja escolha segue um
mtodo apresentado na seo, e muito eficiente. Os resultados mostraram uma grande reduo no
erro da estimao com um pequeno custo computacional extra, motivando o uso deste algoritmo
onde for possvel.
A estimao da velocidade angular dada por um filtro linear baseado em movimento brow-
niano, como discutido na seo 5.2.4. Os resultados mostram que o erro de estimao reduzido
para toda faixa de inrcias ao utilizar a mesma ideia de adaptao utilizada para a atitude na se-
o 5.2.3. Apesar de a atitude e a velocidade angular serem estimadas independentemente, a se-
o 5.2.5 mostra que esta hiptese plausvel e obtm bons resultados em relao a dois outros
modelos, principalmente pela sua capacidade de adaptao do rudo de covarincia de entrada.
Como os modelos descritos levam em conta apenas a atitude, eles s podem ser utilizados se
84
Captulo 5. Filtragem de estados
houver uma fonte externa para informar a acelerao, que compensada da leitura do acelermetro.
Caso contrrio, a estimao da atitude deve depender praticamente s do magnetmetro, uma vez
que o acelermetro utilizado na estimao da acelerao. A seo 5.2.6 mostrou que a ampliao
do rudo de entrada, que gerou melhores resultados na seo 5.2.2, produz piores resultados ao uti-
lizar apenas o magnetmetro, pois o comportamento dinmico do sistema se torna mais importante
para estimar a leitura esperada do sensor e, portanto, o ajuste realizado. Alm disso, a adaptao
da matriz de covarincia tambm fez com que a estimativa da atitude se tornasse pior, de forma
que recomenda-se o uso apenas da covarincia do giroscpio como fonte de rudo na entrada do
sistema.
Na seo 5.2.7 o modelo de atitude foi estendido para estimar simultaneamente a acelerao
do sistema, os quais so utilizados na estimao da posio e velocidade linear, auxiliados por um
GPS. Ao desacoplar o sistema de atitude e posio num formato mestre-escravo, foi obtido um
algoritmo eficiente para estimao da atitude, posio e velocidade linear. Este algoritmo apresenta
bons resultados tanto para a estimao da atitude quanto posio, dependendo fortemente da leitura
do GPS devido tentativa de minimizar o pior caso. A seo 5.2.8 finaliza estimando todos os
estados do veculo quando este no est submetido a situaes to extremas quanto as anteriores.
Por fim, observa-se que os erros de atitude e velocidade angular do sistema so pequenos o
suficiente para serem utilizados em ambientes abertos ou fechados, contanto que a leitura do mag-
netmetro permanea confivel ou algum outro sensor para observao da atitude seja utilizado. J
os erros de posio e velocidade linear s so factveis com ambientes abertos, devido sua dimen-
so. No entanto, o GPS no funciona bem em ambientes fechados, o que j limita a aplicao do
filtro nestas condies. Nada impede que o filtro seja utilizado com algum outro tipo de sensor que
funcione melhor em ambientes internos, como cmeras ou sensores laser.
Todos os algoritmos desenvolvidos se mostraram eficientes, com um tempo mximo para uma
iterao de 2ms. Como a arquitetura de software utilizada nos testes no otimizada para os casos
particulares dos sistemas, h um overhead computacional devido ao armazenamento de dados para
anlise da simulao e a linguagem utilizada foi Python, que rpida ao fazer contas grandes com
a biblioteca SciPy (J ONES e outros, 2001) mas lenta para fazer iteraes. Uma verso otimizada
para o modelo do sistema rodando num sistema embarcado e implementada em linguagem de nvel
inferior ser capaz de realizar as estimaes com grande folga de tempo.
Os algoritmos de filtragem de estados, como aqueles descritos neste captulo, geralmente as-
85
Captulo 5. Filtragem de estados
sumem que as leituras dos valores utilizados para previso e correo so dados pelos valores reais
somados a rudos. Entretanto, isto s ocorre se os sensores estiverem perfeitamente calibrados e
suas imperfeies forem compensadas. Portanto, uma calibrao de boa qualidade deve ser reali-
zada para evitar que o filtro utilize informaes incorretas, o que pode levar a instabilidades e falha
do sistema.
86
Captulo 6
Este captulo apresentar as tcnicas de calibrao utilizadas neste trabalho e seus resultados.
As tcnicas aqui desenvolvidas tem fundamento estatstico e visam construir mtodos que sejam
menos restritos e teoricamente mais corretos do que os encontradas na literatura, como as apresen-
tadas na seo 2.3. Para isso, necessrio trabalhar com o caso mais geral possvel e depender ao
mnimo de aproximaes. Alm disso, as ferramentas corretas para anlise dos resultados devem
ser utilizadas, evitando concluses errneas. A grande importncia da calibrao correta dos sen-
sores percebida na Eq. (3.5), que mostra como relacionar o valor obtido pelo sensor com o valor
correto de leitura.
A seo 6.2 trata da estimao da rotao entre o sistema de coordenadas S no qual as me-
dies so feitas e um sistema arbitrrio B definido pelo usurio. Ao invs de requisitar um ali-
nhamento correto entre os sistemas, importante permitir que as rotaes sejam arbitrrias. Os de-
87
Captulo 6. Calibrao de sensores inerciais e orientao
Por fim, a seo 6.3 apresenta uma concluso e resumo dos resultados obtidos, destacando
como os dois mtodos aqui descritos podem ser utilizados nos mesmos dados para reduzir o tempo
de calibrao.
Vale ressaltar que as nicas hipteses feitas pelo sistema de calibrao so:
2. possvel manter os sensores parados e acoplados ao sistema com o qual sero usados du-
rante alguns intervalos de tempo. Caso no seja possvel garantir que eles se mantenham
parados, mas pudermos garantir que a expectativa das perturbaes medidas em cada pe-
rodo so nulas, ento, pelo teorema do limite central, o rudo adicional pode ser considerado
gaussiano com mdia zero se o nmero de amostras for suficientemente grande.
A seo 6.1.1 traz a teoria dos algoritmos, descrevendo o mtodo de coleta de dados e os
mtodos de otimizao utilizados. A seo 6.1.2 descreve os algoritmos que utilizam as otimizaes
descritas anteriormente para gerar a calibrao. O mtodo utilizado para avaliar os algoritmos
13
www.nist.gov/dads/HTML/greedyalgo.html
88
Captulo 6. Calibrao de sensores inerciais e orientao
apresentado na seo 6.1.3 e os resultados na seo 6.1.4. Por fim, a seo 6.1.5 analisa os
resultados obtidos pelos mtodos de calibrao apresentados.
A seo 6.1.1.5 apresenta dois problemas encontrados durante a calibrao, o que faz com
que existam infinitas solues para o problema geral, descrevendo tambm suas possveis solues,
que so utilizadas nas calibraes feitas.
A coleta de dados de maneira apropriada essencial para uma calibragem correta do sistema.
A calibrao aqui apresentada composta de intervalos de tempo no qual o sistema encontra-se
parado e fixo ao corpo ao qual far uso dos sensores. Esta restrio visa evitar aceleraes diferentes
da gravidade e campos magnticos variantes. Aps capturar cada intervalo, um pr-processamento
de dados pode ser feito, reduzindo a quantidade de dados a ser processada pelo resto da calibrao
sem perder nenhuma informao
onde s [i] = Ks sR
s [i] + bs , conforme Eq. (3.4), s a covarincia do sensor, suposta constante
como justificado na seo 3.2, e s [i] a quantidade de amostras de leituras do sensor s coletadas
durante o intervalo i. A figura 6.1 mostra um exemplo dos conjuntos de amostragem para N = 4.
Como pode ser notado, em cada intervalo as leituras so constantes, exceto por um rudo.
89
Captulo 6. Calibrao de sensores inerciais e orientao
Com estes dados, possvel calcular a estimativa de menor varincia de s [i], dada por:
s [i]
1 X
s [i] = ss [i, j]. (6.2)
s [i] j=1
Esta estimativa tem exatamente o papel de substituir as s [i] leituras do intervalo sem perda
nenhuma de informao. Isto pode ser observado pelo fato de que s [i] tem exatamente a mesma
probabilidade, utilizando a Eq. (3.6), de todas as j amostras ocorrerem s[i, j], uma vez que
s
s [i] N s , . (6.3)
s [i]
90
Captulo 6. Calibrao de sensores inerciais e orientao
m [i]. No entanto, para cada intervalo i do sistema parado, pode-se utilizar uma rotao Ri e obter
sR
sR
a [i] = Ri g, sR
m [i] = Ri h, (6.5)
onde g e h so definidos pelas Eqs. (3.11a) e (3.11b), respectivamente. Note que, como no se
tem conhecimento das rotaes Ri e elas devem ser estimadas, no importa se Ri ou R1 i ser
utilizado, contanto que haja consistncia.
a = {a , Ka , ba , g} m = {m , Km , bm , h} (6.6)
e adicionando um conjunto {Ri } aos parmetros a serem estimados, pode-se definir uma fun-
o de custo como o negativo do logaritmo da funo de verossimilhana. Esta funo de custo,
excetuando-se as constantes, dada por:
N
X X T
s [i] Ks Ri sIs + bs s [i] 1 Ks Ri sIs + bs s [i] , (6.7)
J(a , m , {Ri }) = s
i=1 s{a,m}
91
Captulo 6. Calibrao de sensores inerciais e orientao
onde f () uma funo que extrai alguma caracterstica til da matriz de informao KTs 1
s Ks .
A funo de custo aproximada uma soma ponderada de quadrados de norma 2, cuja solu-
o possui forma fechada utilizando quaternions (K ARNEY, 2005). Para computar esta soluo, a
funo deve ser reescrita como:
Asi = f K1 I
K1 I
(6.10c)
s (s [i] bs ) + ss , s (s [i] bs ) ss
0 bx by bz
bx 0 az ay
f (a, b) = . (6.10d)
b
y az 0 ax
bz ay ax 0
Os autovalores da matriz Bi so dados por 0 i,0 i,1 i,2 i,3 . Tomando qi como o
autovetor associado ao autovalor i,0 , tem-se que a funo Ji (qi ) minimizada. Se a caracterstica
extrada pela funo f () for uma boa substituta da matriz de informao 1 i , espera-se que o
quaternion qi obtido seja prximo do quaternion qi que minimiza a funo Ji (Ri ).
Note que as computaes utilizadas para chegar ao quaternion qi so fceis de serem calcu-
ladas e produzem uma resposta fechada, no sendo sujeita a mnimos locais. Assim, esta soluo
qi pode ser utilizada como ponto inicial para otimizao da funo Ji (Ri ) para obter o quaternion
correto.
92
Captulo 6. Calibrao de sensores inerciais e orientao
Para isso, o mtodo de mnimos quadrados generalizado ser utilizado, sendo dado pela equa-
o:
y = X + , E[|X] = 0, Var[|X] = , (6.11)
A soluo deste problema dado pela soluo do problema de mnimos quadrados ponde-
rados, considerando como peso a matriz de informao 1 . Assim, a estimativa de dada
por:
1
= XT 1 X XT 1 y (6.12)
a
a [1] Ka R1~z I3 a [1]
0 0
a
" #
a [2] Ka R2~z I3 0 0
a [2]
g z
ya = .. , Xa = .. , a =
..
.. .. .. ..
, a = ,
. . . . . . . ba
a
a [N] Ka RN ~z I3 0 0 a [N ]
(6.13)
onde ~z o vetor unitrio na direo z.
93
Captulo 6. Calibrao de sensores inerciais e orientao
Quando no se sabe os valores corretos de g, h e das rotaes {Ri }, o que ocorre quando
eles tambm so estimados pelo algoritmo, encontra-se dois problemas na hora de determinar as
matrizes de ganho Ka e Km .
O primeiro decorre do fato de existir uma infinidade de valores tais que Ka Ri g constante
para todo Ri . Esta infinidade de solues pode afetar a otimizao dos parmetros, seja evitando
a convergncia ou tornando-a lenta. Para resolver este problema, estabeleceu-se que, durante a
calibrao de gz , seu valor ser alterado para que permanea sempre 1. Para isto, suponha que
o algoritmo se encontra na iterao k e estimativa Ka [k]. Aps resolver a Eq. (6.12) para o ace-
lermetro, tem-se uma estimativa g de g. Ao fazer Ka [k + 1] = |g|Ka [k], a escala determinada
por g mantida. Apesar de teoricamente o correto seria calcular Ka [k + 1] = gKa [k], o uso do
valor absoluto se mostrou mais robusto durante os testes. De maneira semelhante, fixa-se hx = 1 e
faz-se Km [k + 1] = |hx |Km [k] e hz [k + 1] = hz
hx
, novamente mantendo a escala e a direo de hx .
Um ponto importante de ser notado que isto define o sistema de coordenadas S utilizado:
o sistema onde a matriz Ka simtrica.
94
Captulo 6. Calibrao de sensores inerciais e orientao
Definindo
giS = Ri g, hSi = Ri h, (6.15)
95
Captulo 6. Calibrao de sensores inerciais e orientao
Com base na teoria descrita na seo anterior, pode-se desenvolver dois algoritmos para ca-
librao do magnetmetro e acelermetro, que seguem uma lgica simples, dada pelos seguintes
passos:
2. Gere uma primeira estimativa dos parmetros e calcule o custo J[0] associado utilizando a
Eq. (6.7). A estimao inicial dada por bias nulos e matrizes de ganho e rotaes como
identidade.
5. Se houve pouca alterao na estimativa dos campos, otimize os ganhos. Este condicional
foi criado porque foi percebido durante os testes que otimizar os ganhos a cada iterao no
96
Captulo 6. Calibrao de sensores inerciais e orientao
era vantajoso. Por isso, a otimizao dos ganhos s feita quando h uma alterao de
hz < 103 durante o passo anterior.
Vale a pena ressaltar que todos os passos do algoritmo so gulosos, ou seja, calculam a melhor
soluo para aquele instante. Esta abordagem nem sempre vantajosa, uma vez que pode apresentar
divergncias. Como h duas solues para o problema de estimao de rotao, comparar-se- dois
algoritmos baseados nelas:
1. O algoritmo A1 utiliza apenas a otimizao direta da Eq. (6.8b) para determinao da rotao.
2. O algoritmo A2 utiliza a soluo da Eq. (6.9) para gerar as estimativas qi [k]. Junto com o
passo 7, compara-se se J[k] > J[k 1], o que significa que houve um aumento da funo de
custo devido aproximao. Se isto ocorrer, a otimizao direta passa a ser utilizada nas ite-
raes futuras, passando a se comportar como A1. importante ressaltar que as estimativas
calculadas com mtodo aproximado so mantidas, mesmo quando elas aumentam o custo.
Notou-se nos testes que ignorar a iterao k e reverter para as estimativas da iterao k 1
era mais prejudicial do que manter as estimativas atuais.
Esta separao em dois mtodos visa avaliar a diferena entre um algoritmo que realiza um
passo timo a cada iterao, sempre calculando o mnimo, de um algoritmo minimizante, que
utiliza um modelo aproximado para dar um passo que minimize a funo, sem necessariamente
chegar ao timo.
6.1.3 Metodologia
Para realizar as simulaes, assume-se que um total N de 15 perodos foram coletados, cada
um com um total de [i] U({400, 401, . . . , 600}) amostras coletadas, sendo iguais a quanti-
dade de amostras para os sensores. A rotao de cada perodo foi gerada aleatoriamente conforme
descrito em (K UFFNER, 2004) e a condio de parada dos algoritmos foi fixada em 104 .
97
Captulo 6. Calibrao de sensores inerciais e orientao
Os campos foram gerados a partir de nmeros aleatrios nos intervalos [1, 5; 0, 5],
[0, 5; 1, 5] e [1, 5; 1, 5] para gz , hx e hz respectivamente. importante ressaltar que estes pa-
rmetros no tero seus valores recuperados, como explicado na seo 6.1.1.5.
O acelermetro e magnetmetro tem gerao de seus parmetros iguais. Cada termo bs,i do
bias possui um valor aleatrio em [1, 1], a matriz de ganho dada por Ks = I3 + Ks onde
Ks,ij U([0, 1; 0, 1]) e as covarincias so dadas por:
(
s,ij U([0, 5; 2]), se i = j
s = s (6.18)
s,ij U([0, 2; 0, 2]), caso contrrio,
Foram realizadas um total de 100 simulaes pelo mtodo de Monte Carlo, gerando novos
valores a cada execuo do algoritmo. A tabela 6.1 traz um resumo dos parmetros utilizados na
simulao.
98
Captulo 6. Calibrao de sensores inerciais e orientao
Alm disso, deve-se considerar o caso em que a calibrao funciona bem para os dados
coletados, pois est otimizando a funo de custo em cima deles, mas no capaz de aproximar
adequadamente outras regies. Este problema conhecido como overfitting e deve ser analisado.
Por isso, para cada simulao utilizada para a calibrao, realizou-se uma segunda simulao onde
novas rotaes so geradas aleatoriamente, mas os demais parmetros so os mesmos. Aps a
calibrao, a capacidade de reconstruo foi avaliada tanto para os dados de treinamento, quanto
para os dados de teste. Para estimar a rotao utilizando os dados de teste, foi utilizada a soluo
da Eq. (6.9) como primeira aproximao seguida pela otimizao da Eq. (6.8b).
6.1.4 Simulaes
0.2
0.5
0.2
0
log10
log10
0.4
0.6
0.5
0.8
1 1
1.2
1 0 1 2 3 4 5 6 7 8 9 10 1 0 1 2 3 4 5 6 7 8 9 10
log10 log10
(a) Acelermetro (b) Magnetmetro
Figura 6.2: Erro de reconstruo com dados de treinamento e variando a condio de parada.
6.1.5 Anlise
Os resultados para condio de parada variante so dados pelas figuras 6.2 e 6.3. Nota-se
claramente que, conforme a condio de parada diminui, o erro tanto de teste quanto de treinamento
99
Captulo 6. Calibrao de sensores inerciais e orientao
0.8
0.4
0.6
0.2
0.4
0
0.2
0.2
0
log10
log10
0.4 0.2
0.6 0.4
0.6
0.8
0.8
1
1
1.2
1.2
1.4
1 0 1 2 3 4 5 6 7 8 9 10 1 0 1 2 3 4 5 6 7 8 9 10
log10 log10
(a) Acelermetro (b) Magnetmetro
Figura 6.3: Erro de reconstruo com dados de teste e variando a condio de parada.
0.6
0.6
0.4 0.4
0.2 0.2
0 0
0.2 0.2
log10
log10
0.4 0.4
0.6 0.6
0.8 0.8
1 1
1.2 1.2
1.4 1.4
10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30
N N
(a) Acelermetro (b) Magnetmetro
Figura 6.4: Erro de reconstruo com dados de treinamento e variando o nmero de intervalos.
tambm diminui. No entanto, um valor de = 104 j suficiente, uma vez que no h alteraes
significativas ao utilizar variaes menores na condio de parada. Percebe-se tambm que os
algoritmos A1 e A2 possuem desempenhos semelhantes na reconstruo dos dados de treinamento
e teste, mostrando que ambos possuem capacidades semelhantes. Apesar de o erro de teste ser
maior do que o erro de treinamento, como era esperado, em 75% dos casos de teste tem-se que
< 101, indicando bom desempenho dos algoritmos para estimar os parmetros dos sensores.
Ao observar o efeito da quantidade de intervalos, presente nas figuras 6.4 e 6.5, tem-se que
o erro de reconstruo decresce conforme utiliza-se mais intervalos, o que tambm era esperado.
Nota-se que a melhora do desempenho nos dados de treinamento pequena, uma vez que a cali-
100
Captulo 6. Calibrao de sensores inerciais e orientao
0.6
0.4
0.5
0.2
0
0.2
log10
log10
0.4
0.5
0.6
0.8
1
1
1.2
1.5
1.4
10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30
N N
(a) Acelermetro (b) Magnetmetro
Figura 6.5: Erro de reconstruo com dados de teste e variando o nmero de intervalos.
2
1.5
Tempo (log10 [s])
Tempo (log10 [s])
1 1.5
0.5
1
0
0.5
0.5
1
1 0 1 2 3 4 5 6 7 8 9 10 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30
log10 N
(a) Variando condio de parada (b) Variando quantidade de intervalos
brao com poucas amostras j capaz de obter bons resultados, mas o erro de reconstruo dos
dados de teste decresce consideravelmente conforme o nmero de amostras aumenta. Alm disso,
a variao no erro entre simulaes tambm diminui, o que mostra que o algoritmo mais estvel
com um nmero superior de amostras. Um fato importante de ser destacado que A1 apresenta
grandes erros tanto de treinamento quanto de teste em alguns casos. Este comportamento pode ser
associado no linearidade da otimizao da rotao, uma vez que este o nico ponto que dife-
rencia A1 de A2 e pode fazer com que o sistema convirja para mnimos locais de baixa qualidade.
Com isso, tem-se que o uso da estimativa aproximada por A2 nas etapas iniciais, onde ela ainda
reduz a funo de custo, permite que o algoritmo encontre uma boa regio do espao de busca,
permitindo que posteriormente a otimizao correta atinja mnimos de melhor qualidade.
101
Captulo 6. Calibrao de sensores inerciais e orientao
1. A equao de custo, dada pela Eq. (6.7), utilizada para avaliar a condio de parada
uma soma dos erros de cada intervalo, fazendo com que um maior nmero de intervalos
tambm aumente o nmero de termos e, provavelmente, possua um valor mais elevado, alm
de apresentar maiores variaes entre iteraes do algoritmo, requerendo um maior nmero
de iteraes para alcanar .
Analisando os resultados, conclui-se que, apesar de haver de fato um aumento do tempo para
cada iterao do algoritmo, a quantidade de iteraes necessrias para convergncia significativa-
mente menor. Isto se deve ao fato de que a estimao dos parmetros gera resultados mais prximos
dos reais nas iteraes, devido quantidade maior de amostras, fazendo com que a calibrao se
torne mais rpida. Vale a pena ressaltar que, apesar de haver reduo do tempo, esta reduo no
significativa o suficiente para justificar por si s a coleta de mais dados.
102
Captulo 6. Calibrao de sensores inerciais e orientao
otimizao da rotao. Por este motivo, no necessrio utilizar algoritmos que faam uso da
esparsidade das matrizes s por no melhorar o tempo de computao significativamente. J em
A2, esta otimizao pode gerar tempos de computao menores. Entretanto, a figura 6.6b mostra
que, para todos os valores de N, 75% dos casos possuem tempo de computao menor do que
10 segundos, o que razovel e, portanto, a esparsidade das matrizes no foi explorada. Alm
disso, em todos os casos em que o tempo de A2 foi semelhante ao tempo de A1, o passo de ajuste
da rotao passou para a otimizao direta logo nas primeiras iteraes, quando a aproximao
gerou aumento do custo. Portanto, nestes casos, a otimizao das operaes matriciais tambm no
gerariam melhoras significativas.
Nesta seo, ser determinada a matriz de rotao R que transforma vetores de B para S.
Esta matriz importante para a calibrao pois, apesar das leituras serem feitas em S, elas pouco
interessam por depender da rotao em que os sensores so colocados. Apesar dos algoritmos de
filtragem poderem trabalhar diretamente no sistema de coordenadas S, os algoritmos de controle
trabalham num sistema de coordenadas arbitrrio e independente do posicionamento dos sensores.
103
Captulo 6. Calibrao de sensores inerciais e orientao
Para realizar a calibrao, assume-se que, na condio normal de equilbrio, B difere de I por
uma rotao no eixo z apenas. Esta restrio no prejudique o uso do algoritmo, pois usualmente
sabe-se como chegar ao sistema B desejado a partir do sistema B considerado pelo algoritmo.
Ao invs de descrever como os dados so coletados, mais fcil descrever como eles so
decompostos, fazendo com que a coleta deva seguir s restries impostas. No fim da seo um
resumo da metodologia na ordem lgica de coleta apresentado.
104
Captulo 6. Calibrao de sensores inerciais e orientao
seguinte maneira:
i S
qI = qSB i qBB i qBI qII , (6.20)
onde:
qII um quaternion formado pela rotao em torno do eixo z de um ngulo I arbitrrio. Esta
rotao representa a construo de um referencial I arbitrrio, uma vez que a direo do
campo magntico, dada pela Eq. (3.11b), pode no ser conhecido. Portanto, I um referen-
cial paralelo ao plano normal Terra.
i B
qI tambm formado por uma rotao em torno do eixo z de um ngulo Ii ( pi2 , pi2 ). Esta
rotao permite que as medies possuam rotaes entre si, uma vez que pode ser impratic-
vel requerer que o sistema no possua rotaes indesejadas ao mov-lo. Para adicionar esta
liberdade, necessrio que o usurio informe se Ii negativo, positivo ou nulo.
i B
qB formado por uma rotao em torno do eixo x ou y do sistema de coordenadas B local de um
ngulo Bi ( pi2 , pi2 ). Esta a etapa que de fato define os eixos do sistema de coordenadas
B. Novamente, o usurio deve informar se o ngulo negativo, positivo ou nulo, devendo
informar ainda se a rotao foi realizada em torno do eixo x ou y local.
qSB a matriz de rotao R arbitrria, sendo portanto independente da rotao atual do sistema.
Com base nestes quaternions definidos anteriormente, a seguinte metodologia para coleta dos
dados sugerida:
1. Posicione o rob numa superfcie paralela superfcie terrestre. Determine a rotao atual e
faa marcas no cho, indicando a posio do sistema.
2. Utilize algum apoio para rotacionar o sistema no eixo x ou y local. Ao posicionar o rob e
seu apoio no cho, veja se ele est rotacionado para direita ou esquerda da posio original.
105
Captulo 6. Calibrao de sensores inerciais e orientao
~xI I i
~yI I
~zS ~yS
i
B
~xS R
~xB ~xB
4. Opcionalmente, gere rotaes apenas no eixo z, fazendo com que os quaternions qBB sejam
nulos.
Esta metodologia extremamente simples e pode ser utilizada numa grande variedade de
sistemas. Alm disso, o passo 1 captura uma posio onde as rotaes i qBI e i qBB so nulas, o que
ajuda na convergncia do algoritmo.
Para determinar cada um dos quaternions, mtodos de otimizao de uma funo de custo
so utilizados. No entanto, o mapeamento duplo deve ser considerado durante esta otimizao,
evitando que ela convirja para valores incorretos. Para isso, seja q um quaternion qualquer, seu
primeiro termo no nulo, comeando a partir do escalar, deve ser positivo. Ao invs de aplicar
esta restrio otimizao, a soluo ser determinada sem restries e, caso o primeiro termo seja
negativo, a soluo inteira ser negada, simplificando os algoritmos utilizados.
N
X
i B I
2
i S 1
J qSB , {i qBB }, {i qBI }, qII = S i B
q0 qB qB qI qI qI
, (6.21)
i=1
onde q0 o quaternion que no possui rotaes e i qSI so as estimativas obtidas a partir de leitura
106
Captulo 6. Calibrao de sensores inerciais e orientao
dado por:
1
= XT X XT y. (6.23)
onde as matrizes Mi dependem do quaternion estimado naquele instante. Alm disso, para estima-
o de cada quaternion, supe-se que os demais so conhecidos.
Os valores Mi da Eq. (6.24) podem ser obtidos diretamente a partir da funo de custo,
presente na Eq. (6.21), utilizando a notao de multiplicao matricial, descrita na seo anterior.
107
Captulo 6. Calibrao de sensores inerciais e orientao
e para qII elas so dadas por:
h
ih 1 i
Mi = qSB i qBB i qBI i qSI , (6.26)
onde as notaes de produtos de quaternions como matrizes so dadas pelas Eqs. (A.3) e (A.4).
Para estimar i qBB e i qBI , deve-se levar em considerao que eles dependem apenas de suas
prprias leituras, sendo portanto independentes para cada i. Para utilizar a mesma formulao
anterior, faz-se Mi = 0 para i [2, 3, . . . , N], utilizando somente M1 na estimao. Alm disso, se
os passos descritos na seo 6.2.1.1 forem utilizados para coleta dos dados, ento estes quaternions
no sero ajustados para a amostra do passo 1, por eles j serem conhecidos. Portanto, para i qBB
tem-se:
h 1 i
M1 = qSB i qBI qII i qSI (6.27)
e, para i qBI , tem-se
h I i
i S 1
qSB i B
(6.28)
M1 = qB qI qI .
6.2.2 Metodologia
108
Captulo 6. Calibrao de sensores inerciais e orientao
Considera-se que foram feitas M medies em torno de cada eixo. Seus quaternions i qBB so
dados por rotaes aleatrias no intervalo ( 2 , 2 ) em torno de seus eixos respectivos. Supe-se
tambm a existncia da rotao coletada no passo 1 descrito na seo 6.2.1.1, que possui rotaes
nulas nos quaternions i qBI e i qBB .
Cada medio de rotao realizada afetada por um rudo uniforme. Para gerar cada rudo,
gera-se um quaternion aleatrio como descrito anteriormente e seu ngulo multiplicado por um
fator , que representa a incerteza, utilizando a decomposio polar.
Foram realizadas um total de 100 simulaes pelo mtodo de Monte Carlo, gerando novos
valores a cada execuo do algoritmo. A tabela 6.2 traz um resumo dos parmetros utilizados na
simulao.
Uma vez determinadas as estimativas de cada quaternion, o valor de R estimado qSB com-
parado com o correto qSB atravs da rotao entre eles, dada pela Eq. (A.8).
6.2.3 Simulaes
6.2.4 Anlise
Em todos os casos, as estimativas que possuem medies com rotaes em torno apenas do
eixo z apresentaram resultados melhores. Apesar disso j ser esperado, uma vez que elas possuem
3M +1 amostras contra 2M +1 das demais, elas obtiveram esta melhora no s devido quantidade
de amostras.
109
Captulo 6. Calibrao de sensores inerciais e orientao
2 2
1.5 1.5
1 1
0.5 0.5
log10 [ ]
log10 [ ]
0 0
0.5 0.5
1 1
1.5 1.5
2 2
2.5 2.5
0.05 0.01 0.005 0.001 0.05 0.01 0.005 0.001
(a) M = 2 e rotaes em x e y (b) M = 2 e rotaes em x, y e z
2 2
1.5 1.5
1 1
0.5 0.5
log10 [ ]
log10 [ ]
0 0
0.5 0.5
1 1
1.5 1.5
2 2
2.5 2.5
0.05 0.01 0.005 0.001 0.05 0.01 0.005 0.001
(c) M = 5 e rotaes em x e y (d) M = 5 e rotaes em x, y e z
2 2
1.5 1.5
1 1
0.5 0.5
log10 [ ]
log10 [ ]
0 0
0.5 0.5
1 1
1.5 1.5
2 2
2.5 2.5
0.05 0.01 0.005 0.001 0.05 0.01 0.005 0.001
(e) M = 10 e rotaes em x e y (f) M = 10 e rotaes em x, y e z
Figura 6.8: Erro na estimativa de R para variadas quantidades de amostra e nveis de erro.
Ao comparar as figuras 6.8b e 6.8c, nota-se que ambas possuem desempenhos semelhantes,
apesar da primeira ter 7 amostras e a segunda 11. Esta melhora de performance pode ser associada
110
Captulo 6. Calibrao de sensores inerciais e orientao
ao menor nmero de graus de liberdade na estimao da rotao em torno de z, uma vez que sabe-se
que qBB so nulos, o que faz com que as estimativas sejam mais robustas ao rudo.
Na figura 6.8b, nota-se tambm que a mediana se encontra prxima expectativa do erro das
medies, mostrando que apenas 2 medies so insuficientes para fazer com que a estimao de
R tenha erros menores do que as estimativas originais. Aumentando o nmero de amostras, tem-se
uma melhora lenta da qualidade de estimao, o que se deve grande liberdade de movimentao
a cada medio.
Como observado, a grande fonte de erro do algoritmo se deve ao nvel de liberdade dado s
medidas. Caso seja possvel garantir que uma dada rotao em x e uma em z tenham o mesmo
qBI , o que pode ocorrer se o sistema se encontra num balano, deve-se fazer uso desta informao
durante os passos do algoritmo, alterando facilmente as matrizes Mi para incluir ambas medidas.
Entretanto, este trabalho foca em fornecer o mximo de liberdade para o usurio e evitar que fossem
feitas hipteses muito restritivas, que no podem ser satisfeitas facilmente.
6.3 Concluso
Com base nos resultados da seo 6.1.4 e anlise da seo 6.1.5, pode-se concluir que o
algoritmo proposto para calibrao capaz de obter boas estimativas dos parmetros para aceler-
metros e magnetmetros. Conclumos tambm que no necessrio o uso de uma condio de
parada muito restrita, uma vez que a qualidade da estimao permanece a mesma. Alm disso,
nota-se que o aumento do nmero de intervalos amostrados melhora significativamente a capaci-
dade de generalizao das estimativas dos parmetros, resultando em erros menores nos dados de
teste.
Deve-se ressaltar a importncia de se utilizar dados de teste para verificar a calibrao: apesar
de uma maior quantidade de perodos amostrados no melhorar muito o erro para os dados utiliza-
dos na calibrao, ela influencia muito na qualidade final da calibrao. Esta melhora s poder ser
111
Captulo 6. Calibrao de sensores inerciais e orientao
Os resultados apresentados na seo 6.2.3 e analisados na seo 6.2.4 mostram que a calibra-
o da rotao R entre B e I escalam bem com o nvel de rudo e quantidade de amostras, apesar
do algoritmo descrito impor pouqussimas restries ao usurio. Alm disso, o algoritmo proposto
pode ser utilizado com qualquer metodologia de estimao de atitude em 3 dimenses.
Destaca-se o fato de que os dois algoritmos podem ser utilizados com os mesmos dados. Uma
vez feita a calibrao dos parmetros, como mostrado ao longo da seo 6.1, estima-se tambm as
matrizes de rotao Ri da leitura e seus quaternions qi associados. Comparando as equaes dos
sensores, dadas pelas Eqs. (3.9) e (3.8), com a Eq. (6.5), tem-se que as estimativas qi podem ser
utilizadas na calibrao da rotao ao fazer i qSI = q1
i . Portanto, ao invs de realizar uma coleta
de dados para a calibrao dos sensores e outra para calibrao da rotao R, possvel realizar
apenas uma coleta, possivelmente aumentando a quantidade de amostras para cada algoritmo.
112
Captulo 7
Concluso
113
Captulo 7. Concluso
pacidade de gerar grandes erros esporadicamente, apesar de apresentar grandes vantagens no caso
geral. Para ambientes fechados, outros tipos de sensores, como cmeras e sensores laser, devem ser
investigados.
Por fim, a filtragem espera que as leituras dos sensores sejam apenas verses ruidosas do
valor real. Isto requer uma boa calibrao dos sensores e levou ao desenvolvimento do captulo 6.
A calibrao do acelermetro e magnetmetro apresentou erros pequenos de reconstruo do sinal
original, com medianas menores do que 0.1 desvios padres. Discutiu-se ainda a importncia de
se utilizar dados de teste para avaliar a calibrao, prtica pouco comum nesta comunidade. Como
a calibrao destes sensores define um sistema de coordenadas arbitrrio, possivelmente diferente
do sistema de coordenadas desejado para especificar trajetrias e waypoints, foi desenvolvido em
seguida um algoritmo para determinar a rotao entre estes dois sistemas de coordenadas, cujo
desempenho tambm satisfatrio.
Assim, o controle do quadrirrotor, foco principal desta pesquisa, mostrou ser necessrio pes-
quisa em duas reas relacionadas, filtragem de estados e calibrao de sensores, nas quais foram re-
alizadas anlises e desenvolvidos algoritmos para que o sistema como um todo possa corretamente.
Estes resultados so expressivos e inovadores, tendo gerado por ora trs publicaes finalizadas
(M IRANDA E F ERREIRA ; M IRANDA E F ERREIRA ; M IRANDA E F ERREIRA, 2013a; 2013b; 2014)
e outras em desenvolvimento, alm de fornecer ideias para pesquisas posteriores. importante
destacar que, apesar do foco deste trabalho ser quadrirrotores, os avanos alcanados podem ser
explorados em outras reas, como veculos terrestres, aquticos ou outros tipos de veculos areos,
devido similaridade dos sistemas e generalidade dos algoritmos.
Como o desenvolvimento do controle levou ao estudo de filtragem, que por sua vez incentivou
a pesquisa em calibrao, as pesquisas futuras aqui descritas seguem o caminho contrrio, para que
desenvolvimentos numa rea possam ter seus subsistemas funcionando adequadamente.
114
Captulo 7. Concluso
caracterizao do bias drift destes sensores deve ser realizada, visando verificar se este efeito
significativo o suficiente para incluir no algoritmo de calibrao e para fornecer dados para que o
algoritmo de filtragem seja capaz de compensar esta variao.
O filtro de estados apresentado na seo 5.2.8 supunha que o sistema estava constantemente
sofrendo aceleraes elevadas, o que gerava o pior desempenho na estimao da atitude. Um algo-
ritmo adaptativo, semelhante aos apresentados para ajustar a covarincia de entrada, pode permitir
que o sistema automaticamente se ajuste s diversas condies, possivelmente obtendo erros me-
nores quando o sistema estiver de fato quase parado.
Outra extenso do filtro a adio de estimadores de parmetros dos sensores, como bias,
ou do quadrirrotor, como inrcia. Enquanto os primeiros podem ser utilizados para melhorar a
qualidade da estimativa dos estados, os segundos podem ser explorados pelo controlador para se ter
uma lei de controle com melhores garantias de estabilidade. Os filtros devem tambm ser testados
com leituras obtidas a partir da calibrao imperfeita dos sensores, verificando a robustez a valores
incorretos mas prximos dos verdadeiros.
Por fim, o controlador apresentado no captulo 4 supe que o sistema est parado. Para que
o controlador siga trajetrias arbitrrias, a matriz de aproximao do sinal da derivada deve ser
115
Captulo 7. Concluso
modificada para acompanhar a referncia. Alm disso, o quadrirrotor pode no ser capaz de de-
terminar as velocidade angulares para os motores com base apenas nas foras e torques desejados,
como o caso quando os parmetros das hlices so desconhecidos. Portanto, o controlador apre-
sentado deve ser modificado para fornecer velocidades de rotao diretamente, podendo ser usado
em qualquer quadrirrotor.
Tem-se ainda que a robustez do controlador ao utilizar estimativas dos estados, dadas por
filtros, deve ser analisada. Uma possibilidade de melhoria seria o uso de outros controladores
que utilizam mais informao do sistema ou que possuem mais garantias de estabilidade como
base para o controle por aprendizado descrito neste trabalho. Esta frente tambm pode facilitar a
portabilidade do algoritmo a sistemas com modelos diferentes.
116
Apndice A
Quaternions
Este apndice descreve o que so quaternions e suas operaes, sendo um resumo de sua
teoria utilizada neste trabalho. Para mais detalhes, recomenda-se a leitura de (C HOU, 1992) e
(G RAF, 2008).
O quaternion uma extenso matemtica dos nmeros complexos para quatro dimenses,
podendo ser escritos como:
" # q0
q0 q1
q= =
= q0 + q1 i + q2 j + q3 k, (A.1)
~q q
2
q3
A seo A.1 descreve as operaes que podem ser realizadas com quaternions, sendo base
para sua utilizao como rotao, descrita na seo A.2. A seo A.3 fornece uma breve descrio
das vantagens computacionais de se utilizar quaternions para representar rotaes.
117
Captulo A. Quaternions
Assim como o espao C dos nmeros complexos tem sua base escolhida de forma que i2 =
1, o espao H escolhido de forma que i2 = j 2 = k 2 = ijk = 1. Com esta definio do
espao, o produto entre dois quaternions, denotado neste trabalho, pode ser escrito como:
q p =(q0 p0 q1 p1 q2 p2 q3 p3 ) (A.2a)
+ (q0 p1 + q1 p0 + q2 p3 q3 p2 )i
+ (q0 p2 q1 p3 + q2 p0 + q3 p1 )j
+ (q0 p3 + q1 p2 q2 p1 + q3 p0 )k
" #
q0 p0 ~q ~p
= , (A.2b)
q0 ~p + q0~q + ~q ~p
q3 q2 q1 q0
118
Captulo A. Quaternions
Caso kqk = 1, ou seja, o quaternion unitrio, ele pode representar uma rotao de um
ngulo = 2 em torno da direo unitria v no R3 . Neste trabalho, todos os quaternions so
assumidos unitrios, pois so utilizados apenas para representar rotaes. Como uma rotao =
+2
2
ao redor de v equivalente a uma rotao = 2
na mesma direo, tem-se que
cos + + ~v sin + = cos ~v sin , (A.7)
2 2 2 2
A seo A.2.1 apresenta o mtodo usado para calcular o ngulo entre dois quaternions, sendo
usada como uma medida de erro. As sees A.2.2 e A.2.3 descrevem como o quaternion se re-
laciona com a matriz de rotao no espao e como utiliz-lo para rotacionar vetores. Por fim, a
seo A.2.4 descreve o modelo dinmico do quaternion.
119
Captulo A. Quaternions
Dados dois quaternions q e p, o quaternion que representa a distncia entre eles dado por
d = q p1 . Utilizando a decomposio polar, dada pela Eq. (A.6), o ngulo de rotao entre eles
pode ser calculado como:
q,p = 2 arccos |d0 |, (A.8)
uma vez que um ngulo negativo pode ser representado por um ngulo positivo e uma inverso da
direo.
que pode no satisfazer as restries de ortonormalidade devido a problemas numricos, seu qua-
ternion equivalente pode ser obtido a partir do mtodo descrito em (BAR -I TZHACK, 2000). Seja a
matriz K definida como:
Rxx + Ryy + Rzz Ryz Rzy Rzx Rxz Rxy Ryx
1 R yz Rzy Rxx R yy Rzz R xy + Ryx R xz + Rzx
K= ,
3
Rzx Rxz Rxy + Ryx Rxx + Ryy Rzz Ryz + Rzy
Rxy Ryx Rxz + Rzx Ryz + Rzy Rxx Ryy + Rzz
(A.10)
o quaternion que representa a rotao dado pelo autovetor associado ao maior autovalor de K.
Alm disso, este autovalor 1 se e somente se R for uma matriz de rotao ortonormal.
Sejam as bases xA = [1, 0, 0]T , yA = [0, 1, 0]T e zA = [0, 0, 1]T de um sistema de coordena-
das A e as bases xB , yB e zB de outro sistema de coordenadas B. Note que esta escolha de A no
gera perda de generalidade, uma vez que a rotao entre A e B pode ser descrita por uma rotao
entre A e A , seguida de uma rotao entre A e B, onde A possui as bases escolhidas. A matriz de
120
Captulo A. Quaternions
rotao RB
A de A para B pode ser escrita como:
h i
RB
A = B
x y B
z B , (A.11)
Portanto, para definir a matriz de rotao a partir de um quaternion, necessrio apenas descrever
como um quaternion qB A utilizado para representar um vetor v na base B.
A
A 1
vA = qA B
(A.13a)
B v q B
B 1
1
A B
= qB qA v qAA
qAB (A.13b)
A B 1
A 1
= qAB q B
A v qA qB (A.13c)
1
= qA B A
qA B
(A.13d)
B qA v B qA
implica que qA
B qA no realiza nenhuma rotao no vetor, ou seja, qB qA = 1. Pela definio
B A B
B 1
do elemento recproco, tem-se portanto que qA .
B = qA
1 A
qB B
A (t) = (t) qA (t) (A.14a)
2
1 B 1
= B B
q (t) (t) qA (t) qB
A (t) (A.14b)
2 A
1
= qB (t) B (t), (A.14c)
2 A
121
Captulo A. Quaternions
A [k]Ts
d[k] = ev = e 2 , (A.15)
onde a velocidade angular A [k] suposta constante no intervalo t [kTs , (k + 1)Ts ). Atravs
da definio do logaritmo de quaternions, dada pela Eq. (A.5), pode-se obter a velocidade angular
como: 1
ln d[k] ln qB
A [k + 1] qB
A [k]
A [k] = 2 =2 . (A.16)
Ts Ts
2. Velocidade e tamanho: apesar de quaternions requerem mais operaes para se fazer rotaes
do a representao direta por matrizes de rotao, eles contm menos termos, sendo mais
compactos. Como a rotao poder ser descrita atravs de multiplicaes e somas, ela
computacionalmente vantajosa em relao aos ngulos de Euler, que requerem operaes
trigonomtricas.
122
Captulo A. Quaternions
4. Medio de erro simplificada: a medida de erro entre rotaes utilizada neste trabalho dada
pelo ngulo entre duas orientaes, que pode ser calculada facilmente atravs de quaternions,
como descrito na seo A.2.1. Esta medida mais complicada para o caso de matrizes de
rotao, enquanto o uso direto dos ngulos de Euler podem gerar erros grandes para pequenas
rotaes.
5. Dinmica: como descrito na seo A.2.4, a dinmica do quaternion linear com a velocidade
angular, o que permite tirar vantagem de toda a teoria de sistemas lineares, em particular da
garantia de previso tima dada pelo filtro de Kalman.
123
Referncias Bibliogrficas
BAR-ITZHACK, I.Y. New method for extracting the quaternion from a rotation matrix. Journal
of guidance, control, and dynamics, v. 23, n. 6, 2000.
BESNARD, L. Control of a quadrotor vehicle using sliding mode disturbance observer. American
Control Conference, 2007. ACC07, pp. 52305235, 2007.
BESNARD, L.; SHTESSEL, Y.B. e LANDRUM, B. Quadrotor vehicle control via sliding mode
controller driven by sliding mode disturbance observer. Journal of the Franklin Institute, v.
349, n. 2, 658684, maro 2012.
125
Referncias Bibliogrficas
BRISSET, P.; DROUIN, A. e GORRAZ, M. The paparazzi solution. MAV2006, Sandestin, Flo-
rida, 2006.
CASTILLO, P.; DZUL, A. e LOZANO, R. Real-time stabilization and tracking of a four-rotor mini
rotorcraft. Control Systems Technology, IEEE Transactions on, v. 12, n. 4, 510516, 2004.
CHOU, J.C. Quaternion kinematic and dynamic differential equations. Robotics and Automation,
IEEE Transactions on, v. 8, n. 1, 5364, 1992.
COOK, K.L.B. The silent force multiplier: The history and role of uavs in warfare. In Aerospace
Conference, 2007 IEEE, pp. 17. March 2007.
CRASSIDIS, J.L.; MARKLEY, F.L. e LIGHTSEY, E.G. Global Positioning System Integer Ambi-
guity Resolution Without Attitude Knowledge. Journal of Guidance Control and Dynamics,
v. 22, n. 2, 212218, maro 1999.
DEVAUD, J.; NAJKO, S.; NAHEDIC, P.L.; MAUSSIRE, C.; ZANTE, E. e MARZAT, J. Full
design of a low-cost quadrotor UAV by student team. In Proceedings of the International
Conference on System Engineering and Technology, ICSET 2012. 2012.
ELKAIM, G.H. Misalignment Calibration Using Body Frame Measurements. Relatrio tcnico,
2013.
126
Referncias Bibliogrficas
GUENARD, N.; HAMEL, T. e MOREAU, V. Dynamic modeling and intuitive control strategy for
an X4-flyer. In Control and Automation, 2005. ICCA05. International Conference on, pp.
141146, vol. 1. 2005.
HAN, J.; SONG, Q. e HE, Y. Adaptive unscented Kalman filter and its applications in nonlinear
control. Chapter of Applications of Kalman Filter, . . . , , n. April, 125, 2009.
HAUG, A.J. A tutorial on Bayesian estimation and tracking techniques applicable to nonlinear and
non-Gaussian processes. MITRE Corporation, McLean, 2005.
HOFFMANN, G.; RAJNARAYAN, D.G.; WASLANDER, S.L.; DOSTAL, D.; JANG, J.S. e TOM-
LIN, C.J. The Stanford testbed of autonomous rotorcraft for multi agent control (STARMAC).
In Digital Avionics Systems Conference, 2004. DASC 04. The 23rd. Ieee, 2004. ISBN 0-7803-
8539-X.
HOFFMANN, G.M.; HUANG, H.; WASLANDER, S.L. e TOMLIM, C.J. Quadrotor helicopter
flight dynamics and control: Theory and experiment. In Proc. of the AIAA Guidance, Naviga-
tion, and Control Conference, August, pp. 120. 2007.
127
Referncias Bibliogrficas
HOL, J. Sensor Fusion and Calibration using Inertial Sensors, Vision, Ultra-Wideband and
GPS. 2011. p. 1165. Tese (Doutorado).
JONES, E.; OLIPHANT, T.; PETERSON, P. e outros. SciPy: Open source scientific tools for
Python. 2001.
JULIER, S.J. e UHLMANN, J.K. New extension of the Kalman filter to nonlinear systems. In
AeroSense97, pp. 182193. 1997.
JULIER, S.J. e UHLMANN, J.K. Unscented Filtering and Nonlinear Estimation. Proceedings of
the IEEE, v. 92, n. 3, 401422, maro 2004.
JULIER, S.J.; UHLMANN, J.K. e DURRANT-WHYTE, H.F. A new approach for filtering non-
linear systems. In American Control Conference, 1995. Proceedings of the, v. 3, pp. 1628
1632, vol. 3. American Autom Control Council, 1995. ISBN 0-7803-2445-5.
JULIER, S.J.; UHLMANN, J.K. e DURRANT-WHYTE, H.F. A new method for the nonlinear
transformation of means and covariances in filters and estimators. Automatic Control, IEEE
Transactions on, v. 45, n. 3, 477482, 2000.
KALMAN, R.E. A new approach to linear filtering and prediction problems. Journal of basic
Engineering, v. 82, n. 1, 3545, 1960.
KARNEY, C.F.F. Quaternions in molecular modeling. Journal of Molecular Graphics and Mo-
delling, v. 25, n. 5, 595604, junho 2005.
KOLTER, J. Z. Learning and control with inaccurate models. 2010. p. 1183. Tese (Doutorado).
KUFFNER, J.J. Effective sampling and distance metrics for 3D rigid body path planning. In Ro-
botics and Automation, 2004. Proceedings. ICRA04. 2004 IEEE International Conference
on, April, pp. 39933998, vol. 4. 2004. ISBN 0780382323.
LUPASHIN, S.; SCHOLLIG, A.; HEHN, M. e DANDREA, R. The flying machine arena as of
2010. In Robotics and Automation (ICRA), 2011 IEEE International Conference on, pp.
29702971. IEEE, 2011.
128
Referncias Bibliogrficas
MADGWICK, S.O.H. An efficient orientation filter for inertial and inertial/magnetic sensor arrays.
Relatrio tcnico, 2010.
URL: http://sharenet-wii-motion-trac.googlecode.com
MARINS, J.L.; YUN, X.; BACHMANN, E.B.; MCGHEE, R.B. e ZYDA, M.J. An extended
Kalman filter for quaternion-based orientation estimation using MARG sensors. In Intelligent
Robots and Systems, 2001. Proceedings. 2001 IEEE/RSJ International Conference on, v. 4,
pp. 20032011, vol. 4. Ieee, 2001. ISBN 0-7803-6612-3.
MCGEE, L.A. e SCHMIDT, S.F. Discovery of the Kalman Filter as a Practical Tool for Aerospace
and Industry. Relatrio Tcnico November, 1985.
MELLINGER, D. e KUMAR, V. Minimum snap trajectory generation and control for quadrotors.
In Robotics and Automation (ICRA), 2011 IEEE International Conference on, pp. 2520
2525. 2011.
MIRANDA, C.S. e FERREIRA, J.V. Greedy calibration of the rotation between sensor and body
frame without external references. In Proceedings of the ABCM International Congress of
Mechanical Engineering. ABCM, 2013a.
MIRANDA, C.S. e FERREIRA, J.V. Greedy statistically correct simultaneous calibration of mag-
netometers and accelerometers. In Proceedings of the ABCM International Congress of Me-
chanical Engineering. ABCM, 2013b.
MIRANDA, C.S. e FERREIRA, J.V. Sensor and body frames rotation calibration through attitude
restriction. In Proceedings of the IFAC World Congress (to appear). IFAC, 2014.
129
Referncias Bibliogrficas
PARK, S.; WON, D.H.; KANG, M.S.; KIM, T.J.; LEE, H.G. e KWON, S.J. RIC (robust internal-
loop compensator) based flight control of a quad-rotor type UAV. In Intelligent Robots and
Systems, 2005.(IROS 2005). 2005 IEEE/RSJ International Conference on, pp. 35423547.
Ieee, 2005. ISBN 0-7803-8912-3.
POUNDS, P.; MAHONY, R. e CORKE, P. Modelling and control of a large quadrotor robot.
Control Engineering Practice, v. 18, n. 7, 691699, julho 2010.
POUNDS, P.; MAHONY, R.; GRESHAM, J.; CORKE, P. e ROBERTS, J. Towards dynamically-
favourable quad-rotor aerial robots. In Proceedings of the 2004 Australasian Conference on
Robotics & Automation. 2004.
RENAUDIN, V.; AFZAL, M.H. e LACHAPELLE, G. New method for magnetometers based
orientation estimation. In Position Location and Navigation Symposium (PLANS), 2010 IE-
EE/ION, D, pp. 348356. Ieee, maio 2010. ISBN 978-1-4244-5036-7.
130
Referncias Bibliogrficas
ROY, N. e THRUN, S. Online self-calibration for mobile robots. In Robotics and Automation,
1999. Proceedings. 1999 IEEE International Conference on, v. 3, pp. 22922297, vol. 3.
IEEE, 1999.
SABATINI, A.M. Quaternion-based extended Kalman filter for determining orientation by inertial
and magnetic sensing. Biomedical Engineering, IEEE Transactions on, v. 53, n. 7, 1346
1356, julho 2006.
SALAZAR-CRUZ, S.; PALOMINO, A. e LOZANO, R. Trajectory tracking for a four rotor mini-
aircraft. In Decision and Control, 2005 and 2005 European Control Conference. CDC-
ECC05. 44th IEEE Conference on, pp. 25052510. Ieee, 2005. ISBN 0-7803-9567-0.
SEETHARAMAN, G.; LAKHOTIA, A. e BLASCH, E.P. Unmanned vehicles come of age: The
darpa grand challenge. Computer, v. 39, n. 12, 2629, December 2006.
THRUN, S.; BURGARD, W. e FOX, D. Probabilistic Robotics. Intelligent Robotics and Auto-
nomous Agents Series. Mit Press, 2005. ISBN 9780262201629.
131