Concepção de Um Pequeno Sensor Inercial 3D PDF

Universidade de Aveiro Departamento de Electrnica, Telecomunicaes e
Informtica
2009
Francisco Lus Magno Concepo de um Pequeno Sensor Inercial 3D

Morgado
Universidade de Aveiro Departamento de Electrnica, Telecomunicaes e

Informtica
2009
Francisco Lus Magno Concepo de um Pequeno Sensor Inercial 3D

Morgado
Dissertao apresentada Universidade de Aveiro para cumprimento dos

requisitos necessrios obteno do grau de Mestre em Eng. Electrnica e
Telecomunicaes, realizada sob a orientao cientfica do Dr. Telmo Reis
Cunha, Professor Auxiliar Convidado do Departamento de Electrnica e
Telecomunicaes da Universidade de Aveiro
o jri / the jury

presidente / president
Prof. Dr. Jos Carlos Esteves Duarte Pedro

Professor Catedrtico da Universidade de Aveiro
vogais / examiners commitee
Prof. Dr. Telmo Reis Cunha

Professor Auxiliar Convidado da Universidade de Aveiro ( orientador )
Prof. Dr. Jaime dos Santos Cardoso

Professor Auxiliar Convidado da Faculdade de Engenharia da Universidade do Porto
Agradecimentos /
acknowledgements
Agradeo ao meu orientador por me ter dado a oportunidade de realizar este

projecto que faz uso de conceitos e tecnologias de extrema actualidade,
interesse e aplicabilidade. Agradeo ao Departamento de Electrnica e
Telecomunicaes da Universidade de Aveiro pelos recursos disponibilizados
e agradeo, finalmente, minha famlia e namorada por todo o apoio.
palavras-chave
Unidade inercial de medida, MEMS, acelermetro, giroscpio
resumo
Com este trabalho pretende-se desenvolver um IMU (Inertial Measurement

Unit), fazendo uso de acelermetros e giroscpios MEMS (Micro-machined
Electro-Mechanical Systems) de baixo custo. Esta tecnologia emergente abre
novas portas no mundo dos sistemas de navegao inerciais dada a sua
simplicidade mecnica. Este IMU permite transferir as medidas para uma
unidade de processamento/armazenamento e, por conseguinte, a sua
integrao em sistemas de navegao mais complexos que faam uso de
multiplas unidades de medida: IMU, GPS - Global Positioning System,
odmetro, magnetmetro.
keywords
Inertial measurement unit, MEMS, accelerometer, gyroscope
abstract
This work aims to develop an IMU (Inertial Measurement Unit), making use of
low cost MEMS (Micro-machined Electro-Mechanical Systems)
accelerometers and gyroscopes . This emerging technology opens new doors
in the world of inertial navigation systems due to its mechanical simplicity. This
allows transfer IMU measures to a processing/storage unit, hence its
integration into more complex navigation systems that make use of multiple
units of measure: IMU, GPS - Global Positioning System, odometer,
magnetometers.
Contedo
1
Introduo
Enquadramento
2.1
Sistema de Navegao Inercial (INS)
. . . . . . . . . . . . . . . . . . . . .
2.2
Giroscpio e Acelermetro
. . . . . . . . . . . . . . . . . . . . . . . . . . .
2.2.1
Giroscpio MEMS
. . . . . . . . . . . . . . . . . . . . . . . . . . .
2.2.2
Acelermetro MEMS . . . . . . . . . . . . . . . . . . . . . . . . . .
10
Concepo do IMU
13
3.1
Escolha dos sensores
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
3.2
Projecto do esquemtico
3.3
Projecto do PCB
3.4
Produto nal
3.5
Software do PIC - uxograma
14
. . . . . . . . . . . . . . . . . . . . . . . . . . . .
15
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
19
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
20
. . . . . . . . . . . . . . . . . . . . . . . . .
21
Algoritmo de processamento dos dados
23
4.1
. . . . . . . . . . . . . . . . . . . . . . . . . . . . .
24
4.2
Clculo da Orientao
4.1.1
Teoria
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
24
4.1.2
Prtica . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
26
Clculo da Posio
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
27
4.2.1
Teoria
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
27
4.2.2
Prtica . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
28
Resultados
29
Concluses
32
Apndice A
33
Apndice B
38
Lista de Figuras
1
Plataforma giro estabilizada (adaptado de [2])
. . . . . . . . . . . . . . . .
Modelo conceptual de um giroscpio mecnico (adaptado de [2]) . . . . . .
Modelo de um giroscpio MEMS, que faz uso do efeito de coriolis. . . . . .
Modelo conceptual do acelermetro (adaptada de [2]).

Equao da dinmica:
M y M x = Ky C y
. . . . . . . . . . . . . . .
11
Acelermetro de tecnologia MEMS. . . . . . . . . . . . . . . . . . . . . . .
11
Diagrama do caminho do processamento do sinal
. . . . . . . . . . . . . .
13
Fotograas dos sensores inerciais usados na concepo do IMU. . . . . . . .
14
Circuito de acondicionamento dos sinais vindos do giroscpio, acelermetro

e sensor de temperatura
. . . . . . . . . . . . . . . . . . . . . . . . . . . .
Disposio espacial dos sensores.
(Ax, Ay, Az)
(Gx, Gy, Gz)
16
representam
vectores dos eixos de sensibilidade dos acelermetros e os giroscpios, respectivamente, orientados em trs direces ortogonais
(Xl , Yl , Zl ).
. . . .
17
10
Andar de amplicao dos acelermetros horizontais.
. . . . . . . . . . . .
17
11
Circuito de recepo dos sinais aps o acondicionamento e de transmisso

de dados via RS-232
12
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
18
No lado esquerdo, a azul: pistas da PCB de topo;

No lado esquerdo, a vermelho: pistas da PCB por baixo.
. . . . . . . . . .
19
13
Disposio dos componentes na PCB
. . . . . . . . . . . . . . . . . . . . .
20
14
Fotograa do IMU. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
21
15
Fluxograma do algoritmo do microprocessador.
. . . . . . . . . . . . . . .
22
16
Algoritmo aplicado ao INS
(adaptado de [2]) . . . . . . . . . . .
23
17
Referencial local e referencial global.
. . . . . . . . . . . . . . . . . . . . . . . .
24
strapdown
(Xl , Yl , Zl ): referencial local

(Xg , Yg , Zg ) :referencial global .
18
Valor medido do acelermeto Az (horizontal) com o IMU em repouso,

temperatura ambiente (20C). . . . . . . . . . . . . . . . . . . . . . . . . .
19
Random walk que advm da dupla integrao das aceleraes que provm
do acelermetro Ax.
20
29
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
30
Medidas obtidas do giroscpio Gx quando em repouso, temparatura constante de 20C.
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
30
21
Inuncia gerada por variaes da temperatura nos acelermetros. . . . . .
31
22
Inuncia gerada por variaes da temperatura nos giroscpios.
31
. . . . . .
Lista de Tabelas
1
Caractersticas dos sensores. . . . . . . . . . . . . . . . . . . . . . . . . . .
15
Mdia relativa a medidas do giroscpio e acelermetro quando em repouso.
29
Desvio padro relativo a medidas do giroscpio e acelermetro quando em

repouso.
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
29
Introduo
Este projecto consiste, essencialmente, em trs componentes: a concepo e construo

do IMU (Inertial Measurement Unit); o desenvolvimento de um algoritmo para integrao
dos dados de seis sensores inerciais (trs acelermetros e trs giroscpios), para determinao dos deslocamentos linear e angular; e nalmente a anlise da qualidade das medidas
obtidas.
A primeira componente essencialmente de desenvolvimento de electrnica. Os acelermetros e giroscpios que devero compor o IMU dispem tipicamente de um sinal em
tenso que varia proporcionalmente com a acelerao linear ou velocidade angular respectivamente. Ser, portanto, necessrio implementar ADCs sincronizadas para converter essa
informao no formato digital. A informao em digital dever ser, ento, manipulada por
um micro-processador (PIC18 - Microchip), que simultaneamente a enviar por uma porta
srie RS232.
Um sensor adicional de temperatura ser tambm includo, com o intuito
de avaliar a sensibilidade dos sensores s variaes da temperatura. Todo o IMU dever

assentar num algoritmo responsvel por gerar uma base de tempo que sincronizar todas
as medidas. Ser feita uma caracterizao estatstica dos erros produzidos pelos sensores,
que ser fundamental para os processamentos posteriores, e para a possibilidade futura de
integrao dos dados deste IMU com outros sensores dedicados localizao (receptores
GPS, odmetros, etc.). Finalmente, e mediante vrios testes que sero executados, ser
analisada de uma forma quanticada a qualidade das solues de navegao (posio e
orientao) que este IMU apresenta.
Enquadramento
Os sistemas de navegao baseados em sensores de
dead-reckoning (i.e., cuja informao de
navegao se reporta apenas a variaes relativas s posies anteriores, e no a posio

absoluta actual) tm sido utilizados desde h vrias dcadas para o posicionamento de
sistemas que se deslocam sobre a superfcie da Terra (automveis, navios, etc.), em gua
(submarinos), no ar (avies, msseis) e tambm no espao (satlites). Destes sensores, os
que se baseam em medir aceleraes relativamente a um referencial inercial tm ocupado
um lugar predominante - so os elementos de base aos Sistemas de Navegao Inerciais
(INS - Inertial Navigation Systems).
Com o desenvolver da tecnologia, vrios tipos de
sensores inerciais foram sendo desenvolvidos, levando existncia de duas classes distintas
de sensores - os de elevada preciso (high-grade) e os de menor preciso (low-grade). Naturalmente, os primeiros so extremamente mais caros que os segundos, e so de natureza
conceptual muito diferente.
Nas duas ltimas dcadas tem havido uma forte evoluo
em reas de investigao que se dedicam localizao robusta de preciso de viaturas

que se deslocam sobre a superfcie terrestre, com especial nfase nos automveis.
Com
o aparecimento da tecnologia GPS (Global Positioning System), o posicionamento absoluto de preciso tornou-se muito acessvel para muitas aplicaes.
No entanto, quando
se considera um automvel a deslocar-se numa zona urbana, ou em zonas arborizadas, o

bloqueio dos sinais GPS provenientes dos satlites torna-se um srio problema que conduz
incapacidade de determinao de posies (ou de posies com a desejada exactido) em
vrios locais do trajecto. Para aumentar a robustez destes sistemas de navegao, vrias
equipas de investigao tm-se dedicado integrao de receptores GPS com sensores de
dead-reckoning
(que no dependem de sinais provenientes do exterior da viatura), sendo
frequentemente utilizados sensores inerciais de mdia e elevada preciso (ver, por exemplo,
[4]-[7]). Muitos destes sistemas integrados foram aplicaes directas de sistemas integrados desenvolvidos para aeronaves, onde as exigncias de preciso so muito elevadas. No
entanto, para sistemas terrestres, este rigor posicional muitas vezes menos exigente, o
que possibilita a utilizao de equipamento mais barato (mas menos preciso). No caso em
concreto dos automveis, e dada a disponibilidade dos sinais GPS, so pouco frequentes as
situaes onde o posicionamento nal depende apenas dos sensores inerciais durante um
largo perodo de tempo. Mais realista considerar-se que a informao dos sensores inerciais servem para suavizar os erros associados s posies GPS ou para calcular, durante
curtos intervalos de tempo, a posio da viatura quando os sinais GPS so bloqueados.
Assim, torna-se interessante testar o comportamento de um sistema de navegao inte-
grado que considere apenas sensores de muito baixo custo (sensores inerciais MEMS),
averiguando-se, em situaes reais, quais os nveis de preciso e robustez que se atingem
com este sistema. Neste sentido, o sistema inercial desenvolvido neste trabalho tem como
objectivo ser inserido num sistema de navegao para uma viatura, cujo desenvolvimento
dos restantes elementos considerado nos trabalhos da dissertao de Mestrado Integrado
em Engenharia Electrnica e de Telecomunicaes, da Universidade de Aveiro, intitulada
"Desenvolvimento e anlise de desempenho de um sistema de navegao integrado [8].
2.1 Sistema de Navegao Inercial (INS)

Este tipo de sistemas so caracterizados por determinarem a posio processando continuamente medidas que so captadas vindas de sensores inerciais, nomeadamente acelermetros
e giroscpios, dispostos de forma ortogonal. Integrando a informao proveniente dos acelermetros determina-se a velocidade e, por sua vez, integrando a velocidade determina-se
o deslocamento.
A grande vantagem dos INSs advm do facto de no necessitarem de informao proveniente de referncias externas. Isto tornou a navegao inercial na soluo mais comum
para veculos areos e martimos. Mais recentemente, devido aos avanos tecnolgicos, o
leque de aplicaes destes sistemas tem vindo a aumentar e os custos a diminuir, tornando
possvel, por exemplo, captao da dinmica do humano.
Tipicamente os INSs inserem-se em uma de duas categorias: nos sistemas de plataforma
strapdown ).
giro-estabilizada ou nos sistemas xos (
Nos sistemas de plataforma giro-estabilizada os sensores inerciais esto montados numa

plataforma que procura manter a mesma orientao independentemente de toda a dinmica
a que est sujeita.
Isto consegue-se custa de uma estrutura de trs arcos articulados
(gimbals) e de trs motores que permitem plataforma total liberdade de orientao Figura 1. Quando a plataforma sujeita a um deslocamento angular, os giroscpios quanticam esse movimento fazendo com que os motores contrariem esse deslocamento. Desta
forma garante-se que integrando as medidas dos acelermetros se obtm deslocamentos
lineares relativos ao referencial global.
Nos princpios da dcada de setenta do sculo passado, a industria da navegao inercial
gimbals ), que cara-
propunha o desao de eliminar toda a estrutura de eixos articulados (
cterizava os sistemas inerciais da poca, xando os acelermetros e os giroscpios. Desta

forma, processava-se toda a informao sensorial com algoritmos mais poderosos transferindo, assim, toda a complexidade mecnica para os algoritmos matemticos. Substituam-
Figura 1: Plataforma giro estabilizada (adaptado de [2])
se os eixos mecnicos articulados por eixos matemticos articulados, proporcionando

assim o aparecimento dos sistemas inerciais xos (
strapdown ).
Este tipo de sistemas usa sensores que, ao contrrio das plataformas giro-estabilizadas,
esto xados ao corpo cuja dinmica se pretende descrever, portanto as medidas que
daqui so retiradas dizem respeito ao referencial local do corpo. Para que as aceleraes
provenientes dos trs acelermetros sejam integradas correctamente estas medidas tero
de ser previamente projectadas no referencial global, e isso consegue-se custa de trs
giroscpios dispostos ortogonalmente que permitem uma contnua estimativa da orientao
do referencial local relativamente ao referencial global.
articulados (
Visto isto, eliminam-se os eixos
gimbals ), e por conseguinte desaparece a necessidade de calibrao, tornando
estes sistemas mais veis. Por outro lado, esta xao possibilita que se construam os
sistemas em dimenses mais reduzidas e a custo mais baixo. Para alm destes benefcios,
estes sistemas trazem o inconveniente de serem computacionalmente mais exigentes.
2.2 Giroscpio e Acelermetro

Nesta seco pretende-se fazer uma breve descrio dos sensores aqui usados referindo
alguns aspectos morfolgicos e erros mais relevantes que lhes esto associados. Uma descrio mais detalhada pode ser encontrada em [1, 2].
Figura 2: Modelo conceptual de um giroscpio mecnico (adaptado de [2])
2.2.1
Giroscpio MEMS
Giroscpios so sensores com a funo de quanticar variaes angulares relativamente a

um determinado eixo com elevada preciso e ignorar todos os outros tipos de dinmica
(lineares e angulares) - Figura 2. A grandeza dos sinais gerados por estes dispositivos vem
tipicamente expressa em /s e pode atingir muitas centenas de graus por segundo. H, no
entanto, um compromisso entre o alcance das medidas e a preciso.
O princpio que est por trs dos giroscpios MEMS (Micro-machined Electro-Mechanical
Systems) o efeito de Coriolis que estabelece que uma massa
velocidade linear
movendo-se com uma
v num referencial que roda com uma velocidade angular
sofre o efeito
de uma fora
FC = 2 M ( v)
(1)
O modelo mais simples consiste numa massa de prova que levada a vibrar ao longo
de um eixo X que quando sofre uma rotao em torno de Z induz, segundo o efeito de
coriolis, uma vibrao no eixo Y, perpendicular a X e Z - gura 3.
Estes sensores so constitudos por um nmero reduzido de componentes mecnicos, e
por conseguinte so relativamente baratos de construir.
As principais vantagens deste tipo de giroscpios relativamente a outros, nomeadamente os mecnicos, so:
tamanho/peso reduzido
Figura 3: Modelo de um giroscpio MEMS, que faz uso do efeito de coriolis.
baixo consumo energtico
estrutura slida
tempo de start-up reduzido
custo nanceiro reduzido
grau de abilidade elevado
no requer manuteno
resistncia a ambientes hostis
A grande desvantagem destes giroscpios prende-se com o facto de actualmente ainda no

serem to precisos quanto os giroscpios pticos [1, 2].
A desvantagem dos giroscpios
pticos serem vrias ordens de grandeza mais caros.

As fontes de erro com maior relevo nos giroscpios MEMS so:
Erro Sistemtico: possvel medir o erro sistemtico do sensor integrando as medidas sucessivamente quando no se lhe imposto qualquer rotao e se verica
uma diferena entre o valor esperado e o valor real calculado em
determinado o seu valor
,
/h.
Uma vez
este pode ser eliminado subtraindo esse valor s medidas
extradas do sensor. Caso esta compensao no se verique, os deslocamentos angulares calculados sero afectados de um deslocamento proporcional a
,
que cresce
linearmente em ordem ao tempo
(t) = t
(2)
Rudo Branco: As principais causas de rudo branco nos giroscpios MEMS so perturbaes termo-mecnicas que geram variaes de frequncia muito superior taxa
de amostragem. Este rudo caracterizado por um conjunto de variveis aleatrias
de mdia nula em que cada uma destas variveis est igualmente distribuda e caracterizada por uma varincia nita
2.
Desta forma o rudo introduz
random walk
de mdia nula nas medidas, aps integrao, com um desvio padro
(t) =
t t
(3)
Para uma explicao um pouco mais detalhada deve referir-se a [1] e [2].
Rudo de Baixa Frequncia (
Fliker Noise ):
iker noise
responsvel por gerar
variaes que so inversamente proporcionais frequncia (1/f ). Este rudo perde

relevncia para as altas frequncias, tornando-se desprezvel quando comparado com
o rudo branco.
Efeito de Variaes na Temperatura: A relao entre a temperatura e a variao

no sinal altamente no linear. A forma mais comum de eliminar este erro implementando um sensor de temperatura, cujas medidas so inseridas num modelo
matemtico de compensao dos parmetros dos sensores sensveis s variaes da
temperatura. Alguns sistemas inerciais contm internamente sensores de temperatura e por conseguinte fazem uma correco automtica dos erros provenientes desta
fonte.
Erros de Calibrao: Os erros de calibrao surgem devido ao desalinhamento dos

sensores durante a fase de montagem do IMU e aos erros intrnsecos ao acondicionamento dos sinais. A natureza destes erros muito idntica dos erros sistemticos
e reectem-se em desvios quando integrados.
2.2.2
Acelermetro MEMS
Os acelermetros tm por objectivo medir aceleraes com alta preciso numa direco
muito bem denida.
Estes sinais so tipicamente expressos em m/s
10
ou popularmente
Figura 4: Modelo conceptual do acelermetro (adaptada de [2]).

Equao da dinmica:
M y M x = Ky C y
Figura 5: Acelermetro de tecnologia MEMS.
referida constante de acelerao gravtica terrestre
g.
O princpio que est por trs dos
acelermetros MEMS consiste numa massa suspensa por um brao de natureza idntica
ao de uma mola e de um amortecedor em srie.
Tal como indica a gura 4, fazendo a
leitura do deslocamento da massa possvel calcular o valor da acelerao pela segunda

lei de Newton
F = ma
(4)
H ainda outra categoria de acelermetros MEMS cujo princpio o de medir as variaes da frequncia de uma onda acstica de superfcie presente num brao exvel com
uma massa suspensa - gura 5.
Estes dispositivos usufruem das vantagens da tecnologia MEMS e por isso partilham
11
das mesmas qualidades previamente mencionadas relativamente aos giroscpios.

Tal como os giroscpios a grande desvantagem dos acelermetros MEMS ainda no
serem to precisos quanto os acelermetros que usam as tcnicas tradicionais.
Por partilharem a mesma tecnologia com os giroscpios MEMS acima referidos, os acelermetros sofrem essencialmente dos mesmos erros, no entanto importa destacar algumas
diferenas:
Erro Sistemtico: Quando o acelermetro est sujeito a condies tais, que o valor do
sinal seja previsvel, nomeadamente quando est em repouso, possvel determinar
o erro sistemtico
pela diferena entre o valor real e o esperado que provoca um
erro na posio calculada (integrando duplamente a acelerao) que cresce de forma

quadrtica
s (t) =
t2
2
(5)
Rudo Branco: Tal como os giroscpios MEMS os acelermetros sofrem de perturbaes termo-mecnicas geradoras de desvios no sinal. Neste caso, devido dupla
integrao do sinal, o rudo branco gera
random walk
de segunda ordem de mdia
nula e desvio padro
r
s t3/2
12
t
3
(6)
Concepo do IMU
Para a construo do IMU foram tidos em conta essencialmente os seguintes factores:

ortogonalidade dos seis sensores inerciais; a reduo do nmero de fontes de erro; e a sua
estabilidade e robustez mecnica.
Nesta seco ser explicado com mais detalhe quais
os sensores usados e a forma como foram implementados, tendo em conta os factores

previamente mencionados.
No diagrama da gura 6 ilustram-se os andares principais por onde os sinais provenientes dos sensores devem passar at se obter a posio e a orientao do IMU. Este
diagrama servir como guia durante a fase da construo. As aceleraes e as velocidades
angulares passam pelo andar de amplicao, por forma a ajustar os sinais s caractersticas das entradas dos andar seguintes.
Este circuito de amplicao constituido por
seis amplicadores concebidos para maximizar a excurso de sinal de cada um dos sinais
enviados pelos sensores. Dado que o micro-controlador usado contm apenas um conversor
analgico-digital (ADC), com um tempo de converso obviamente no nulo, seria impossvel guardar quaisquer seis medidas que dissessem respeito ao mesmo instante de tempo.
Visto isto, implementou-se um andar de sincronizao com o objectivo de memorizar,
durante um determinado tempo, as medidas, provenientes do andar de amplicao, permitindo assim ADC converter os seis sinais relativos ao mesmo instante de tempo. Por
outro lado, foi implementado um sensor de temperatura com o intuito de ser feita uma
anlise dos erros com origem nas variaes da temperatura. Depois de toda a informao
ser processada, as medidas so enviadas, j em formato digital, para o sistema de armazenamento de dados do sistema de navegao integrado, ou para um computador (para
possibilitar vrios testes), que far a integrao de todos os dados.
Figura 6: Diagrama do caminho do processamento do sinal
13
3.1 Escolha dos sensores

Os critrios com maior relevncia usados para a determinao dos modelos dos acelermetros e dos giroscpios a usar foram:
o preo, rudo, a sensibilidade s variaes de
temperatura, o alcance e o tamanho.

Para o acelermetro, o alcance refere-se aos valores mximo e mnimo para os quais
o acelermetro ainda tem sensibilidade.
gs
Este parmetro vem tipicamente expresso em
(mltiplos da acelerao da gravidade). Este valor deve ser escolhido de forma a no
exceder em demasia as especicaes exigidas pela aplicao para que haja um mximo
aproveitamento da excurso do sinal. Neste caso, para um implementao onde se consideram condies de circulao normais para uma viatura, espera-se que uma variao at
0.3g seja suciente.
Quanto ao rudo e sensibilidade devido a variaes de temperatura,
devem ser to baixos quanto possvel - vm expressos em
mV
e em
%/ C , respectivamente.
No caso do giroscpio, a mesma ideia aplica-se para o alcance, mas desta vez as unidades
vm expressas em
/s.
Para esta aplicao no ser necessrio um intervalo de valores
para o alcance superior a
70 /s.
Assim sendo, foram escolhidos o acelermetro ADXL103 (Analog Devices) e o giroscpio MEV-50A-R (Murata), por apresentarem um custo reduzido e por corresponderem s
exigncias acima mencionadas - gura .
Figura 7: Fotograas dos sensores inerciais usados na concepo do IMU.
Quanto ao sensor de temperatura, foi usado um LM75 (digital) - erro

exigir qualquer tipo de calibrao.
1 C -
por no
Apesar de ser um pouco mais caro do que outros
sensores, tais como a PTC ou o termopar, este apresenta uma razo preo/qualidade
melhor.
Na tabela 1 esto descritas as caractersticas acima mencionadas para cada sensor.
14
Sensor
Preo
Alcance
Rudo
Sens. Temp.
Tamanho
ADXL103
16 euro
14 euro
1, 7g
70 /s
6mV rms
7mV rms
0, 3%/ C
4%/ C
552
10 10 3
MEV-50A-R
Tabela 1: Caractersticas dos sensores.
3.2 Projecto do esquemtico

No circuito da gura 8, esto projectados os sensores responsveis por fazer a aquisio
dos sinais de entrada (andares de
input
do IMU vistos no diagrama anterior) relativos
dinmica (aceleraes e rotaes) e temperatura . Esto ainda implementados o andar

de amplicao e as respectivas referncias ajustveis, que permitiro calibrar o
oset
dos
sensores inerciais. O amplicador do giroscpio ter um ganho unitrio, j o amplicador

do acelermetro est projectado para um ganho de 8,3.
sinal que ca acessvel ADC est compreendida entre
[3, 3](m/s )
- gura 10.
Desta forma, a excurso de
[0.3, 0.3](g) ou aproximadamente
Este modelo aplica-se aos trs acelermetros com a pequena
diferena de que no caso do sensor vertical, o valor de referncia para a amplicao no

a 2,5 V mas sim a 1,5 V, para que o efeito da gravidade seja eliminado e portanto haja
um melhor aproveitamento da excurso. Para que esta compensao por
sentido, o algoritmo de integrao ter de desfazer est compensao por
hardware faa
software antes
de proceder integrao. Estes ganhos foram pensados por forma a maximizar a gama de
utilizao do IMU. Este foi pensado para um carro que se desloca dentro de uma cidade,
com variaes de dinmica adequadas s condies normais de circulao.
Este circuito dever ser implementado trs vezes ( excepo do sensor de temperatura)
e cada um deles ser disposto de forma ortogonal relativamente aos outros, tal como se
pode ver na gura 9.
Depois de acondicionados, os sinais so transferidos para o andar de sincronizao
sample and hold
SMP04), e seguidamente para o micro-controlador. Depois de conver-
tidas, as medidas so enviadas via RS-232 (MAX232) para um computador responsvel

pelo processamento do algoritmo de integrao - gura 11.
15
Figura 8: Circuito de acondicionamento dos sinais vindos do giroscpio, acelermetro e

sensor de temperatura
16
Figura 9:
Disposio espacial dos sensores.
(Ax, Ay, Az)
(Gx, Gy, Gz)
representam
vectores dos eixos de sensibilidade dos acelermetros e os giroscpios, respectivamente,

orientados em trs direces ortogonais
(Xl , Yl , Zl ).
Figura 10: Andar de amplicao dos acelermetros horizontais.
17
Figura 11: Circuito de recepo dos sinais aps o acondicionamento e de transmisso de

dados via RS-232
18
3.3 Projecto do PCB

A organizao dos componentes foi feita no sentido de aproximar os sensores inerciais o
mais possvel, para minimizar as fontes de erro associadas ao afastamento dos eixos de
sensibilidade dos sensores. O ideal seria colocar os seis sensores coincidentes num ponto
com os eixos de sensibilidade dispostos ortogonalmente.
Colocando o acelermetro e o
giroscpio o mais prximos de um dos cantos da PCB (gura 13) torna possvel montar
as trs PCBs com base na congurao da gura 9.
A PCB foi concebida de forma a se poder, posteriormente, editar algumas ligaes
com
jumpers, permitindo, assim, personalizar a PCB funo respectiva.
implementaram-se trs PCBs diferentes pelo preo de uma.
Figura 12: No lado esquerdo, a azul: pistas da PCB de topo;

No lado esquerdo, a vermelho: pistas da PCB por baixo.
19
Desta maneira
Figura 13: Disposio dos componentes na PCB
3.4 Produto nal

O resultado nal uma estrutura mvel com base em madeira que suporta toda a electrnica envolvida para a concepo do IMU - gura 14.
20
Figura 14: Fotograa do IMU.
3.5 Software do PIC - uxograma

As funes principais do microprocessador so: fazer a recolha dos dados provenientes
dos sensores inerciais atravs da ADC (6 canais) que lhe est incorporada, e das medidas
de temperatura atravs do protocolo I2C. Depois de captados, os valores dos sensores, o
microprocessador deve agrupar as medidas numa trama predenida e envi-las pela porta
srie. Este procedimento repetido a uma cadncia de 100Hz. No incio de cada turno
as medidas inerciais dos seis canais so memorizadas por
sample & hold's, permitindo
ADC converter os dados, do mesmo instante, de cada sensor, sequencialmente - gura 15.
Todo o procedimento funcionar com base num algortmo temporal com a qualidade
de se poder sincronizar a partir de um sinal externo, oriundo de um GPS (1PPS - 1 pulso
por segundo).
Este sinal de sincronismo utilizado para sincronizar todos os sensores
utilizados no sistema de navegao para que, ao integrar os dados de todos, em conjunto,

estes se reportem aos mesmos instantes de tempo efectivos. No micro-controlador, o sinal
de sincronismo produz uma interrupo e a rotina que atende essa interrupo efectua o
acerto do relgio interno no PIC com o valor absoluto do tempo que recebido via porta
21
srie RS232.
Figura 15: Fluxograma do algoritmo do microprocessador.
22
Algoritmo de processamento dos dados
O algoritmo adoptado para o processamento da informao em sistemas
strapdown
tem
essencialmente quatro fases como se pode ver na gura 16. Na primeira determina-se a
orientao do dispositivo fazendo uso dos sinais captados dos giroscpios, na segunda fase
usa-se a orientao previamente calculada para projectar as medidas vindas dos acelermetros no referencial global. De seguida faz-se a compensao das medidas para eliminar
a inuncia da gravidade e nalmente aplica-se a dupla integrao por forma a obter a
posio relativamente s condies iniciais estabelecidas.
Figura 16: Algoritmo aplicado ao INS
strapdown
(adaptado de [2])
Nesta seco a letra 'l' ser usada em ndice de vectores que se reram ao referencial
local. O mesmo se aplica letra 'g', mas neste caso os vectores referir-se-o ao referencial
global, tal como indica a gura 17.
23
Figura 17: Referencial local e referencial global.
(Xl , Yl , Zl ): referencial local

(Xg , Yg , Zg ) :referencial global
4.1 Clculo da Orientao

4.1.1
Teoria
Para representar a orientao do IMU sero usados os cossenos directores, isto , a atitude
do referencial local ser descrita pela matriz de projeco
respondem aos versores directores dos eixos do referencial
para projectar um dado vector
C , 3 3, cujas colunas corlocal, Xl , Yl , Zl . Desta forma,
vl ,denido relativamente ao referencial local, no referencial
global basta calcular
vg = C v l
(7)
Para que a atitude seja bem determinada a matriz
tem de ser actualizada com a
mesma frequncia que so amostradas as velocidades angulares provenientes dos giroscpios
l (t) = (lx (t) , ly (t) , lz (t))T .
Sendo
C (t)
24
a matriz orientao no instante
pode-
se exprimir a variao de
pela denio de derivada
(t) = lim C (t + t) C (t)

C
t0
t
em que
(8)
C (t + t)
C (t), com a
corresponde ao perodo de amostragem. Depois, possvel exprimir
como sendo a multiplicao da matriz de projeco da iterao anterior,

matriz rotao
A (t)
C (t + t) = C (t) A (t)
(9)
em que
A (t) = I +
(10)
=
0

0
de notar que
pequenos.
A (t) = I +
s se verica para ngulos de rotao,
(11)
muito
Fazendo as substituies devidas:
1 Um
dado vector v quando multiplicado por uma matriz rotao altera a sua direco mantendo a
norma. As trs matrizes responsveis por rodar um vector no espao 3-D so as seguintes:
1
0
0
Rx = 0 cos () sin ()
0 sin () cos ()
cos () 0 sin ()
0
1
0
Ry =
sin () 0 cos ()
cos () sin () 0
Rz = sin () cos () 0
0
0
1
Uma rotao custa de ngulos positivos signica rodar em torno do eixo das abcissas aplicando a regrada-mo-direita com o polegar a apontar no sentido positivo do eixo. Desta forma, qualquer rotao no
espao pode ser representada pela multiplicao das trs matrizes previamente descritas
vR = Rx Ry Rz v
sendo vR o vector v aps a rotao. No entanto, a ordem pela qual as matrizes so multiplicadas deve ser
respeitada, isto , rotaes no espao tridimensional no so comutativas. Por outro lado, para ngulos de
rotao muito pequenos o resultado de sin () , sin () e sin () aproxima-se de , e respectivamente
e os cossenos destes ngulos tendero para 1. Feitas as substituies, as multiplicao entre as matrizes
25
= lim C (t + t) C (t)
C
t0
t
C (t) A (t) C (t)
= lim
t0
t
C (t) (I + ) C (t)
= lim
t0
t
= C (t) lim
t0 t
(12)
(13)
(14)
(15)
Como a taxa a que so captadas as medidas sucientemente elevada, a aproximao de

pequenos ngulos pode ser tida em considerao
= (t)
t0 t
(16)
0
lz (t) ly (t)
(t) = lz (t)
0
lx (t)
ly (t) lz (t)
0
(17)
lim
onde
Finalmente, substituindo, est-se perante a equao diferencial
(t) = C (t) (t)

C
com a seguinte soluo
C (t) = C (0) exp
(18)

(t) dt
(19)
0
em que
C (0)
4.1.2
Prtica
a orientao inicial do dispositivo.
Na prtica, as velocidades angulares so captadas a uma dada frequncia de amostragem

e por isso estes sinais no so contnuos, mas sim discretos. Visto isto, a soluo aqui apresentada assume como constantes as velocidades angulares durante o perodo de amostragem
rotao passam a ser comutativas e de valor
1
R = Rx Ry Rz
26
t, at que novas medidas sejam adquiridas.
Para o perodo
(2.13) a seguinte
t+t
C (t + t) = C (t) exp
[t, t + t], a soluo da equao

(t) dt
(20)
onde
t+t
(t) dt = B
(21)
t
com
0
lz t ly t
B = lz t
0
lx t
ly t lx t
0
(22)
Aplicando a expanso em srie de Taylor da funo exponencial obtm-se

B2 B3 B4
+
+
+ ...
C (t + t) = C (t) I + B +
2!
3!
4!

2
2
2 2
B
B
=
C (t) I + B +
+ ...
2!
3!
4!

1
2 4
2 4
= C (t) I + 1
+
... B +
+
. . . B2
3!
5!
2!
4!
6!

1 cos () 2
sin ()
B+
B
= C (t) I +
2
em que
l = (lx , ly , lz )T . A equao
projeco C todas as iteraes.
= |l t|
actualizar a matriz
(23)
(24)
(25)
(26)
(2.20) ser, ento, usada para
4.2 Clculo da Posio

4.2.1
Teoria
Para que as aceleraes provenientes dos acelermetros
al (t) = (alx (t) , aly (t) , alz (t))T
se-
jam correctamente integradas, estas devem ser projectadas no referencial global da seguinte
maneira:
ag (t) = C (t) al (t)
27
(27)
Depois, com as medidas j referenciadas aos eixos globais, aplica-se a dupla integrao e
faz-se a compensao da acelerao da gravidade
vg (t) = vg (0) +
(ag (t) gg ) dt
(28)
vg (t) dt
(29)
pg (t) = pg (0) +
0
onde
vg (0)
pg (0)
so a velocidade e a posio iniciais (condies iniciais) e
gg
acelerao devido gravidade.
4.2.2
Prtica
Assumindo, ento, que os valores das aceleraes so constantes durante o perodo
t,
integrao feita da seguinte maneira:
vg (t + t) = vg (t) + t (ag (t + t) gg )
(30)
pg (t + t) = pg (t) + t vg (t + t)
(31)
28
Mdia
Eixo de sensibilidade
Giroscpios ( /s)
2
Acelermetros (m/s )
6.225 10
4.930 104
7.948 10
6.790 104
4.904 104
9.069 104
Tabela 2: Mdia relativa a medidas do giroscpio e acelermetro quando em repouso.

Desv. Padro
Eixo de sensibilidade
Giroscpios ( /s)
2
Acelermetros (m/s )
Tabela 3:
1.424 10
3.169 106
1.900 10
2.642 105
2.438 105
1.006 104
Desvio padro relativo a medidas do giroscpio e acelermetro quando em
repouso.
Resultados
Aqui mostram-se alguns resultados aps a implementao do IMU: efectuou-se uma anlise
estatstica com os sensores em repouso e outra quando sujeitos a variaes de temperatura.
Nas seguintes tabelas (2 e 3) encontram-se os resultados destas medies.
, tambm, possvel vericar a importncia de um sensor de temperatura de maneira
a poder fazer-se uma compensao dos desvios acumulados por variaes da temperatura.
Figura 18: Valor medido do acelermeto Az (horizontal) com o IMU em repouso, temperatura ambiente (20C).
29
Figura 19: Random walk que advm da dupla integrao das aceleraes que provm do
acelermetro Ax.
30
Figura 20: Medidas obtidas do giroscpio Gx quando em repouso, temparatura constante

de 20C.
Figura 21: Inuncia gerada por variaes da temperatura nos acelermetros.
Figura 22: Inuncia gerada por variaes da temperatura nos giroscpios.
Para fazer a anlise da inuncia da temperatura nos sinais dos sensores, introduziu-
31
se o IMU dentro de um volume com um tamanho sucientemente grande para o cobrir.

Posteriormente aqueceu-se o ar dentro do volume com um ferro de soldar, conseguindo-se
uma variao uniforme da temperatura do ar em torno dos sensores, sem variaes bruscas.
Concluses
Hoje em dia, a tecnologia MEMS ainda no permite criar sistemas inerciais que mantenham um erro de posio inferior a 1 metro, ao m de 1 minuto, quando em repouso. Foi
visto que este erro gerado, mesmo quando o IMU est parado, deve-se ao desvio que a
integrao do rudo dos sensores inerciais proporciona. Durante este projecto registou-se,
tambm, que outra forte componente do erro se deve imperfeita ortogonalidade dos sensores entre si, consequncia de toda a electrnica envolvida ter sido montada manualmente.
Por outro lado, apesar de um IMU no ser capaz de determinar a posio garantindo um
erro constante, este pode ser uma boa soluo quando complementado com outros sistemas
de posicionamento, tais como o GPS. Deste modo, o desvio gerado pelo IMU corrige-se
periodicamente custa das medies absolutas do GPS e as zonas mortas compreendidas
entre sucessivas actualizaes do GPS passam a ser conhecidas com a contribuio do
IMU. Esta cooperao extremamente interessante, pois permite complementar um GPS,
reduzindo signicativamente o erro, tornando-o comparvel a GPSs de maior qualidade e
custo. Este IMU, pode ser uma mais valia em experincias futuras que envolvam outros
dispositivos de navegao e outros algoritmos.
32
Apndice A
Este programa responsvel pelo recepo, armazenamento e integrao das medidas

extraidas dos sensores.
# # # # #
# NOTES #
# # # # #
#
____________
_PROTOCOL_
#
#
___________ ____________________________ ___________________ ______
| EA | BB | 0 0 | 0 B| Ax | Wbz| Ay | Wbx| Az | Wby | Temp | MsByte |
header
measures
time
|
| chksum |
| L s B y t e |CHKSUM|
#
#
#
#
#!C : / P r o g r a m a s / P e r l / b i n / p e r l . e x e
use
strict ;
use
Win32 : : S e r i a l P o r t ;
use
Math : : M a t r i x ;
use
Time : : H i R e s
use
POSIX qw ( c e i l
qw ( g e t t i m e o f d a y
tv_interval ) ;
floor );
constants
use
constant
PI
=> 4
use
constant
X_OFFSET_LIN
=>
use
constant
Y_OFFSET_LIN
=>
constant
Z_OFFSET_LIN
=>
128;
use
constant
X_OFFSET_ANG
=>
128;
use
constant
Y_OFFSET_ANG
=>
129;
use
constant
Z_OFFSET_ANG
=>
128;
my
1);
128;
use
atan2 ( 1 ,
128;
variables
$out ;
my
$port ;
my
@matched ;
my
$var ;
my
$checksum ;
my
$chk_aux ;
my
$ a=c h r ( 0 x e a ) ;
my
$b=c h r ( 0 xbb ) ;
my
$ c=c h r ( 0 x 0 0 ) ;
my
$d=c h r ( 0 x0b ) ;
my
$t_start ;
my
$t0 ;
my
$t1 ;
my
@filter ;
my
$count_filter
0;
my $B
= new
Math : : M a t r i x ( [ 0 , 0 , 0 ] ,
[0 ,0 ,0] ,
[0 ,0 ,0]);
my $C
= new
Math : : M a t r i x ( [ 1 , 0 , 0 ] ,
[0 ,1 ,0] ,
[0 ,0 ,1]);
my
= new
Math : : M a t r i x ( [ 0 , 0 , 0 ] ) ;
$ang_velocity
my
$lin_acceleration
= new
Math : : M a t r i x ( [ 0 , 0 , 0 ] ) ;
my
$lin_accel_proj
= new
Math : : M a t r i x ( [ 0 , 0 , 0 ] ) ;
my
$lin_vel_proj
= new
Math : : M a t r i x ( [ 0 , 0 , 0 ] ) ;
my
$lin_pos_proj
= new
Math : : M a t r i x ( [ 0 , 0 , 0 ] ) ;
my
$temperature ;
my
$time ;
# My way
variables
my
$pos_1
= new
my
$pos
= new
Math : : M a t r i x ( [ 0 , 0 , 0 ] ) ;
my
$vel_1
= new
Math : : M a t r i x ( [ 0 , 0 , 0 ] ) ;
my
$vel
= new
Math : : M a t r i x ( [ 0 , 0 , 0 ] ) ;
# Raw
Math : : M a t r i x ( [ 0 , 0 , 0 ] ) ;
Measures
my
$raw_lin_acc
= new
Math : : M a t r i x ( [ 0 , 0 , 0 ] ) ;
my
$r aw_ an g_ ve l = new
Math : : M a t r i x ( [ 0 , 0 , 0 ] ) ;
my
$raw_temperature ;
my
$raw_time ;
#
my
Calibration
Variables
@offset_ang_sum
my
@offset_lin_sum
(0 ,0 ,0);
(0 ,0 ,0);
33
my
@offset_lin_calibrated
(512 ,512 ,512);
my
@offset_ang_calibrated
(512 ,512 ,512);
###################################################################
# MAIN
# START TIME COUNTING
$t_start
time ;
# COMMUNICATION
$port
= new
( rs232 )
Win32 : : S e r i a l P o r t
( "COM3 " ) ;
$ p o r t >b a u d r a t e ( 1 1 5 2 0 0 ) ;
$ p o r t >d a t a b i t s ( 8 ) ;
$ p o r t > s t o p b i t s ( 1 ) ;
$ p o r t >p a r i t y ( " n o n e " ) ;
while
(1){
$out
$ p o r t >i n p u t ;
i f ( $out
ne
' '){
# INPUT MEASURES # # # # # # # # # # # # # # # #
$var
$var . $out ;
i f ( $ v a r =~/( $ a $ b $ c $ d ) ( . { 1 } ) ( . { 1 } ) ( . { 1 } ) ( . { 1 } ) ( . { 1 } ) ( . { 1 } )
(.{1})(.{1})(.{1})(.{1})(.{1})(.{1})
(.{1})(.{1})(.{1})(.{1})(.{1})(.{1})(.
*)/ o)
$ c h e c k s u m =(0 x e a +0xbb+0x0b+o r d ( $ 2 )+ o r d ( $ 3 )+ o r d ( $ 4 )+
o r d ( $ 5 )+ o r d ( $ 6 )+ o r d ( $ 7 )+ o r d ( $ 8 )+ o r d ( $ 9 )+
o r d ( $ 1 0 )+ o r d ( $ 1 1 )+ o r d ( $ 1 2 )+ o r d ( $ 1 3 )+ o r d ( $ 1 4 )+
o r d ( $ 1 5 )+ o r d ( $ 1 6 )+ o r d ( $ 1 7 )+ o r d ( $ 1 8 ) ) & ( 0 xFF ) ;
$chk_aux =
#
$19 ;
CHECKSUM TRUE ?
i f ( $ c h e c k s u m ==
print
$var
$20 ;
@matched =
#
print
o r d ( $chk_aux ) ) {
"CHKSUM TRUE ! " ;
( $1 ,
$2 , $3 , $4 , $5 , $6 , $7 , $8 , $9 , $ 1 0 , $ 1 1 , $ 1 2 , $ 1 3 ,
s p r i n t f ("% x " ,
o r d ( $matched [ 1 1 ] )
<<
$14 ,
$15 , $16 , $17 , $18 ,
$19 ) ;
8);
# TREAT MEASURES # # # # # # # # # # # # # # #
( $raw_ang_vel ,
$raw_lin_acc ,
( $ang_velocity ,
#
#
#
#
#
#
$raw_temperature ,
$lin_acceleration ,
$raw_time )
$temperature ,
$time )
print
$ a n g _ v e l o c i t y >p r i n t ( " Ang_vel : " ) ;
s p r i n t f ( "TIME :
%i " , $ t i m e ) ;
>p r i n t ( " l i n _ a c e
$lin_acceleration
print
$temperature ;
print
$time ;
print
values
:");
=pod
foreach
( @matched ) {
print
i s o 2 h e x ( $_ ) ;
}
print
"\ n " ;
=c u t
PROCESS INPUT # # # # # # # # # # # # # # # # # #
=pod
=
0;
0;
$ang_velocity
>[0][0]
>[0][1]
>[0][2]
0;
$ang_velocity
>[0][1]
+=
$ang_velocity
$ang_velocity
#=c u t
#
=pod
$lin_acceleration
0.06;
>[0][0]
=
0;
$lin_acceleration
>[0][1]
>[0][2]
0;
$lin_acceleration
>[0][0]
+=
$lin_acceleration
0;
=c u t
#
if (
#
= GetRawMeasures ( \ @matched ) ;
G e t M e a s u r e s ( \ @matched ) ;
>[0][1] = 1 $lin_acceleration >[0][1]*0.7/0.3;

$ l i n _ a c c e l e r a t i o n >p r i n t ( " " ) ;
print
$lin_acceleration >[0][0] ,"\ t " , $lin_acceleration >[0][1] ,"\ t " , $lin_acceleration >[0][2] ,"\n ";
print
$lin_acceleration >[0][0] ,"\ t " , $lin_acceleration >[0][1] ,"\ t " , $lin_acceleration >[0][2] ,"\n ";
$ r a w _ l i n _ a c c >p r i n t ( " " ) ;
$raw_ang_vel >p r i n t ( " " ) ;
$lin_acceleration
( time
print
$t_start )
" DONE
Calculate
!";
1;
> $ARGV [ 0 ] ) {
#AFTER warm
up
time
Mtx B
$B = CalcMtxB ( $ a n g _ v e l o c i t y ) ;
Calculate
Mtx C
34
$C = CalcMtxC ( $ a n g _ v e l o c i t y ,
#
$lin_acceleration
Project
$C ) ;
Acceleration
$lin_accel_proj
#
$B ,
>p r i n t ( " a c c : \ n " ) ;
ProjAccel ( $lin_acceleration ,
$ l i n _ a c c e l _ p r o j >p r i n t ( " l
$C ) ;
p:");
=pod
###############################################################
# # # # #
# MY WAY
#
Integrates
$vel_1
#
Acceleration
= MyIntAcc ( $ l i n _ a c c e l _ p r o j ,
Integrates
$pos_1 =
$vel ) ;
Velocity
MyIntVel ( $ l i n _ a c c e l _ p r o j ,
$vel
$pos
= $pos_1 ;
$vel ,
$pos ) ;
$vel_1 ;
# # # # # # # # # # #
=c u t
###############################################################
# # # # # # #
# ARTICLE WAY:
#
Euler
Integrates
method
Acceleration
$lin_vel_proj
Int egAccel eratio n ( $lin_vel_proj ,
$ l i n _ v e l _ p r o j >p r i n t ( " v e l
Integrates
$lin_accel_proj ) ;
proj ");
Velocity
$ l i n _ p o s _ p r o j=I n t e g V e l o c i t y ( $ l i n _ v e l _ p r o j , $ l i n _ p o s _ p r o j ) ;
###############################################################
}
else {
$raw_ang_vel
$raw_lin_acc
$raw_lin_acc
$raw_lin_acc
$offset_ang_sum [ 0 ]
+= $raw_ang_vel
>[0][0]/10.0;
+= $raw_ang_vel
>[0][1]/10.0;
+=
$offset_lin_sum [ 0 ]
+=
+=
+=
if
(++ $ c o u n t _ f i l t e r
>
>[0][2]/10.0;
>[0][0]/10.0;
>[0][1]/10.0;
>[0][2]/10.0;
9){
print
p r i n t f ( " % . 1 f \ t %.1 f \ t %.1 f \ t %.1 f \ t %.1 f \ t %.1 f \ n " ,
" @ f i l t e r \n " ;
$offset_ang_sum [ 0 ] ,
$offset_lin_sum [ 0 ] ,
$offset_lin_sum [ 2 ] ) ;
( $offset_ang_sum [ 0 ] ,
( $offset_lin_sum [ 0 ] ,
@offset_ang_sum
(0 ,
0,
0);
@offset_lin_sum
(0 ,
0,
0);
$count_filter
0;
}
# # # # # # # # # # #
# $ p o s >p r i n t ( "My
pos :
$time
$ l i n _ v e l _ p r o j >p r i n t ( " v e l :
$ l i n _ v e l _ p r o j >p r i n t ( " " ) ;
");
$time
");
$ l i n _ p o s _ p r o j >p r i n t ( " " ) ;

#
$ l i n _ a c c e l _ p r o j >p r i n t ( " " ) ;
# $B>p r i n t
( "B : \ n " ) ;
# $C>p r i n t ( " \ n " ) ;

# $ r a w _ l i n _ a c c >p r i n t ( " " ) ;
=pod
( undef ,
$t1 )
print
" elapsed :
$t0
$t1 ;
gettimeofday ;
",
( $t1
$t0 ) ,
"\ n " ;
=c u t
if
( ( time
$ t _ s t a r t ) >= $ARGV [ 1 ] ) {
exit ;
}
}
else {
print
"CHECKSUM ERROR ! " ;
35
$offset_ang_sum [ 2 ] ) ;
$offset_lin_sum [ 2 ] ) ;
print
print
#
sprintf
sprintf
print
("%2 x \ n "
("%2 x \ n "
sprintf
, $checksum ) ;
, $checksum )
("%d \ n "
,"
, $chk_aux ) ,
",
o r d ( $chk_aux ) ;
"\ n " ;
}
}
}
}
#p r i n t
$var ;
###################################################################
# FUNCTIONS
##################################
##
Integrates
sub
Acceleration
MyIntAcc
(MY WAY)
my
$l_a
= $_ [ 0 ] ;
my
$l_v
= $_ [ 1 ] ;
my
$grav
= new
Math : : M a t r i x ( [ 0 . 0 ,
1.0 ,
0.0]);
( ( ( $ l_ a >s u b t r a c t ( $ g r a v ))> m u l t i p l y _ s c a l a r ( 0 . 0 1 0 ) ) > add ( $ l _ v ) ) ;
return
}
##################################
##
Integrates
sub
Acceleration
MyIntVel
(MY WAY)
my
$l_a
= $_ [ 0 ] ;
my
$l_v
= $_ [ 1 ] ;
my
$l_p
= $_ [ 2 ] ;
my
$grav
= new
Math : : M a t r i x ( [ 0 . 0 ,
1.0 ,
0.0]);
( ( ( ( $l _ a >s u b t r a c t ( $ g r a v ))> m u l t i p l y _ s c a l a r ( 0 . 0 1 0
return
* 0 . 0 1 0 * 0 . 5 ) ) > add ( $l_v>m u l t i p l y _ s c a l a r ( 0 . 0 1 0 ) ) ) > add ( $ l _ p ) ) ;
}
##################################
##
Integrates
sub
Acceleration
IntegAcceleration
my
$l_v_p
= $_ [ 0 ] ;
my
$l_a_p
= $_ [ 1 ] ;
my
$grav
= new
Math : : M a t r i x ( [ 0 . 0 ,
$l_a_p>p r i n t ( " l i n
ace
prj :
9.8 ,
0.0]);
");
#($l_a_p>s u b t r a c t ( $ g r a v ))> p r i n t ( " l i n
ace
proj :
");
$l_a_p>p r i n t ( " a c c : " ) ;
# Vg ( t+ t )
return
= Vg ( t )
+ t
* ( Ag ( t+ t )
( ( $l_a_p>add ( $ g r a v )
Gg )
)> m u l t i p l y _ s c a l a r ( 0 . 0 1 0 ) ) > add ( $l_v_p )
);
}
##################################
##
Integrates
sub
Velocity
IntegVelocity
my
$l_v_p
my
$l_p_p
return
= $_ [ 0 ] ;
= $_ [ 1 ] ;
( $l_v_p>m u l t i p l y _ s c a l a r ( 0 . 0 1 0 ) ) > add ( $l_p_p )
);
}
##################################
## Get RAW M e a s u r e s
sub
GetRawMeasures
from
@matched
No
units
convertions
are
done !
my @m = @{$_ [ 0 ] } ;
my $a_v = new
my
$l_a
my
= new
$temp =
my
$ti
<<
<<
8
o r d ( $m [ 2 ] ) ,
o r d ( $m [ 8 ] ) ,
( o r d ( $m [ 1 1 ] )
( o r d ( $m [ 5 ] )
<<
o r d ( $m [ 1 2 ] ) ,
<<
o r d ( $m [ 6 ] ) ,
8)
o r d ( $m [ 1 7 ] ) ;
( o r d ( $m [ 3 ] )
( o r d ( $m [ 9 ] )
<<
<<
)
o r d ( $m [ 1 3 ] ) ;
return
Math : : M a t r i x ( [ ( o r d ( $m [ 7 ] )
Math : : M a t r i x [ ( o r d ( $m [ 1 ] )
( o r d ( $m [ 1 4 ] )
( $a_v ,
$ l_ a ,
<<
24)
$temp ,
( o r d ( $m [ 1 5 ] )
<<
16)
( o r d ( $m [ 1 6 ] )
<<
$ti );
}
##################################
## Get
sub
Measures
GetMeasures
from
@matched
my @m = @{$_ [ 0 ] } ;
# my
= @{$_ [ 1 ] } ;
# my
= @{$_ [ 2 ] } ;
my $a_v =
new
Math : : M a t r i x ( [ ( ( ( o r d ( $m [ 7 ] )
( ( ( o r d ( $m [ 1 1 ] )
o r d ( $m [ 1 2 ] ) )
# my
new
<<
<<
$offset_ang_calibrated
[0])
*100/( $offset_ang_calibrated
[1])
[1]) ,
[2])
[2])]);
Math : : M a t r i x ( [
( o r d ( $m [ 3 ] )
o r d ( $m [ 8 ] ) )
$l_a
)+
( ( ( o r d ( $m [ 3 ] )
o r d ( $m [ 4 ] ) )
<<
( o r d ( $m [ 1 ] )
$offset_lin_calibrated [0])
*(2.5/8.3)/(
*(2.5/8.3)/(
$offset_lin_calibrated [0]) ,
$offset_lin_calibrated [1]) ,
36
[0]) ,
o r d ( $m
( o r d ( $m [ 5 ] )
my
new
$l_a
<<
( ( ( o r d ( $m [ 9 ] )
<<
$temp =
my
$ti
print
print
$offset_lin_calibrated [ 2 ] ) ] ) ;
8 ) + o r d ( $m [ 2 ] ) ) $ o f f s e t _ l i n _ c a l i b r a t e d [ 0 ] ) * ( 2 . 5 / 5 ) / ( $ o f f s e t _ l i n _ c a l i b r a t e d
$offset_lin_calibrated [1])*(2.5/5)/( $offset_lin_calibrated [1]) ,
o r d ( $m [ 1 0 ] ) ) $ o f f s e t _ l i n _ c a l i b r a t e d [ 2 ] ) * ( 2 . 5 / 5 ) / ( $ o f f s e t _ l i n _ c a l i b r a t e d [ 2 ] ) ] ) ;
Math : : M a t r i x ( [ ( ( ( o r d ( $m [ 1 ] )
( ( ( o r d ( $m [ 5 ] )
my
*(2.5/8.3)/(
return
<<
[0]) ,
o r d ( $m [ 6 ] ) )
o r d ( $m [ 1 3 ] ) ;
( o r d ( $m [ 1 4 ] )
<<
24)
( o r d ( $m [ 1 5 ] )
s p r i n t f ("% x " ,
( o r d ( $m [ 1 1 ] )
<<
8))
s p r i n t f ("% x " ,
( o r d ( $m [ 1 0 ] )
<<
8)
( $a_v ,
$ l_ a ,
$temp ,
+
+
<<
16)
( o r d ( $m [ 1 6 ] )
<<
8)
o r d ( $m [ 1 7 ] ) ;
( o r d ( $m [ 1 0 ] ) ) ;
( o r d ( $m [ 1 1 ] ) )
);
$ti );
}
##################################
## Mtx B
sub
CalcMtxB
my $B_aux = new
Math : : M a t r i x ( [ 0 , 0 , 0 ] ,
[0 ,0 ,0] ,
[0 ,0 ,0]);
my $a_v = $_ [ 0 ] ;
#
line
$B_aux > [ 0 ] [ 0 ]
$B_aux > [ 0 ] [ 1 ]
$B_aux > [ 0 ] [ 2 ]
line
$B_aux > [ 1 ] [ 0 ]
$B_aux > [ 1 ] [ 1 ]
$B_aux > [ 1 ] [ 2 ]
#l i n e
0;
$a_v > [ 0 ] [ 2 ] * 0 . 0 1 0 ;
$a_v > [ 0 ] [ 1 ] * 0 . 0 1 0 ;
$a_v > [ 0 ] [ 2 ]
*0.010;
0;
$a_v
>[0][0]
*0.010;
$B_aux > [ 2 ] [ 0 ]
$B_aux > [ 2 ] [ 1 ]
$B_aux > [ 2 ] [ 2 ]
return
$a_v > [ 0 ] [ 1 ] * 0 . 0 1 0 ;
$a_v > [ 0 ] [ 0 ] * 0 . 0 1 0 ;
0;
$B_aux ;
} ##################################
## Mtx C
sub
CalcMtxC
my $a_v
= $_ [ 0 ] ;
my
$b
= $_ [ 1 ] ;
my
$c
= $_ [ 2 ] ;
my
$c_next
my
$identity
= new
my
$sigma
my
$sigma_rad
i f ( $sigma
= new
!=
Math : : M a t r i x ( [ 0 , 0 , 0 ] , [ 0 , 0 , 0 ] , [ 0 , 0 , 0 ] ) ;
Math : : M a t r i x ( [ 1 , 0 , 0 ] , [ 0 , 1 , 0 ] , [ 0 , 0 , 1 ] ) ;
( $a_v>m u l t i p l y _ s c a l a r ( 0 . 0 1 0 ) ) > a b s o l u t e ;
$sigma
* PI / 1 8 0 ;
0){
>add ( $b>m u l t i p l y _ s c a l a r ( s i n ( $ s i g m a _ r a d ) / $ s i g m a _ r a d ) ) ;
( $b>m u l t i p l y ( $b))> m u l t i p l y _ s c a l a r ((1 c o s ( $ s i g m a _ r a d ) ) / ( $ s i g m a _ r a d * $ s i g m a _ r a d ) )
$c_next
$identity
$c_next
$ c _ n e x t >add (
$c_next
$ c >m u l t i p l y ( $ c _ n e x t ) ;
}
else {
$identity
>add ( $b )
)>add (
$c_next
$c_next
$ c >m u l t i p l y ( $ c _ n e x t ) ;
( $b>m u l t i p l y ( $b))> m u l t i p l y _ s c a l a r ( 0 . 5 )
}
return
$c_next ;
}
##################################
## Ag ( t )
sub
my
$l_a
my
$c
{
= $_ [ 0 ] ;
= $_ [ 1 ] ;
# Hardware
$ l_ a
compensation
>[0][1]
#c o n v e r t
$l_a
* Ab ( t )
= C( t )
ProjAccel
+=
g' s
to
canceling
1;
m/ s ^2
= $ l _ a >m u l t i p l y _ s c a l a r ( 9 . 8 ) ;
#$ l _ a >p r i n t ( " l
return
a :");
( $ c >m u l t i p l y ( $l _ a >t r a n s p o s e ())) > t r a n s p o s e ( )
}
##################################
##
sub
string
to
hex
iso2hex ($)
my
for
$hex
(my
my
=
$i
' ';
=
$ordno
$hex
.=
0; $i
=
<
ord
l e n g t h ( $_ [ 0 ] ) ; $ i ++)
s u b s t r ( $_ [ 0 ] ,
s p r i n t f ("[%02 x ] " ,
$i ,
1);
$ordno ) ;
37
);
);
);
$hex
=~
s/
$ //;;
$hex ;
}
Apndice B
Programa do microcontrolador PIC18F458.

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
//
MAIN . C
#i n c l u d e
" main . h "
//# i n c l u d e
<h t c . h>
//
DEFINES
//
GLOBAL VARIABLES
// i n t
rec
0;
// u n s i g n e d
char
ola
// u n s i g n e d
char
a;
0;
// u n s i g n e d
char
aux ;
// u n s i g n e d
char
buf ;
// u n s i g n e d
char
ack
// u n s i g n e d
char
ack2
0;
// u n s i g n e d
char
conv_result
// u n s i g n e d
long
out ;
// u n s i g n e d
long
time ;
// u n s i g n e d
char
str [10];
0;
= 0 x00 ;
unsigned
char
adc_buffer_full
unsigned
char
c h a n n e l =0;
unsigned
char
str [10];
unsigned
char
measures [ 6 ] [ 2 ] ;
// u n s i g n e d
unsigned
long
char
1;
measures_long [ 6 ] ;
measures_char [ 7 ] ;
// u n s i g n e d
long
// u n s i g n e d
long
filtered_measures [ 6 ] ;
// u n s i g n e d
char
window =
// u n s i g n e d
char
unsigned
char
w i n d o w _ m e a s u r e s [ 6 ] [ WINDOW_SIZE ] ;
0;
0;
delta_10ms
0;
// u n s i g n e d
long
measures_average [ 6 ] ;
// u n s i g n e d
long
n =
0;
unsigned
long
time
unsigned
long
time_aux =
unsigned
char
unsigned
char
time_byte [ 4 ] ;
// u n s i g n e d
unsigned
char
long
char
nono
// eeprom
0;
0;
0;
aux ;
ack
1;
= 0 x35 ;
unsigned
char
var1 [ 2 5 6 ] ;
/////////////////////////////////
////
MAIN/ / / / / / / / / / / / / / / / / / / / / / / /
/////////////////////////////////
void
main ( v o i d )
{
/
GIE =
1;
PEIE =
SSPIE =
1;
1;
*/
//
Configurations
disableInterrupts ();
initUsart ();
confTIMER0 ( ) ;
//
confTIMER1 ( ) ;
//
InitI2cMaster ( ) ;
//
InitI2cSlave ();
enableInterrupts ( ) ;
ENABLE_TIMER0_INT
//
//////////////
TIMER . h
38
time
= 0 x7778797A ;
while (1){
//
unsigned
long
to
bytes
( i
*8);
f o r ( i = 0 ; i < 4 ; i ++){
time_aux =
t i m e >>
time_byte [ i ]
( unsigned
c h a r ) time_aux ;
putch ( time_byte [ i ] ) ;
}
putch ( ' \ n ' ) ;
*/
///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
//
I2C
//
/////////
while (1)
//
MAIN WHILE
{
/
//
PART RELATED TO I2C
SSPCON1bits . SSPEN =
0;
SSPCON1bits . SSPEN =
1;
( temperature )
DelayMs ( 1 ) ;
INTCONbits . GIE =
1;
P I E 1 b i t s . SSPIE =
1;
INTCON2bits . RBPU =
0;
T R I S B b i t s . TRISB1 =
0;
0;
0;
PORTBbits . RB1 =
0;
PORTBbits . RB2 =
0;
PORTBbits . RB3 =
0;
/ / PORTB PULLUPS ON
DelayMs ( 5 ) ;
InitI2cMaster ( ) ;
while (1){
StartI2c ( ) ;
//
//
RepStartI2c ( ) ;
//
putch ( ' S ' ) ;
//
Begin
//
transmission
Begin
//
Slave
w h i l e ( SSPCON2bits . ACKSTAT ) ;
putch ( 'A ' ) ;
Address
//
Check
for
Acknoledge
from
slave
0: sent ;
1 : not
tab
WaitIdleI2c ( ) ;
//
Wait
SSPCON2bits . RCEN= 1 ;
//
( repeat )
DelayUs ( 2 5 ) ;
W r i t e I 2 c ( 0 x91 ) ;
//
transmission
nl
//
here
while
enables
Lines
arent
idle
reception
DelayMs ( 2 ) ;
DelayUs ( 1 0 0 ) ;
//
putch ( 'T ' ) ;

l t o a ( SSPBUF ,
str );
putstr ( str ) ;
nl
//
//
Get
the
byte
Print
the
string
received
and
convert
it
to
string
SSPBUF = 0 x 0 0 ;
WaitIdleI2c ( ) ;
//
//
Wait
SSPCON2bits .ACKDT =
0;
SSPCON2bits .ACKEN =
1;
here
while
Lines
arent
idle
DelayMs ( 2 ) ;
RepStartI2c ( ) ;
//
//
l t o a ( SSPBUF ,
str );
//
putstr ( str ) ;
nl
Begin
//
//
transmission
Get
the
byte
Print
the
string
( repeat )
received
and
39
convert
it
to
string
sent
//
//
l t o a ( SSPBUF ,
str );
//
putstr ( str ) ;
nl
enables
//
//
//
DelayMs ( 2 ) ;
//
reception
Get
the
byte
Print
the
string
//
enables
received
and
convert
it
to
string
reception
DelayMs ( 1 0 0 ) ;
StopI2c ( ) ;
}
*/
///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
//
while ( 1 ) ;
//
PART RELATED TO ADC
i f ( adc_buffer_full ){
//
Save
//
ADC
value
measures_char [ channel ]
= ADRESH ;
measures [ channel ] [ 0 ]
= ADRESH ;
= ADRESL ;
/ / ADC b e c o m e s
Show
me
i f ( channel
channel
//
the
==
bits
YES
then
conversion
empty
adc_buffer_full
//
0;
next
channel
5){
0;
SaveCurrentTemperature ( ) ;
i f ( delta_10ms ) {
// ShowMeasures ( ) ;
nl
SendMeasures ( ) ;
delta_10ms
0;
// e l s e {
//
n++;
//}
}
e l s e { c h a n n e l ++;}
i f ( channel
==
0){
//
start
if
SAMPLE/HOLD
SampleAndHold ( 1 0 ) ;
}
//
Start
conversion
ADC( c h a n n e l ) ;
}
/ /REST OF THE PROGRAM
}
}
//
//
(SPENDING TIME
!)
MAIN WHILE END

MAIN END
///////////////////////////////////////
////
INT
//////////////////////////////
///////////////////////////////////////
void
e n a b l e I n t e r r u p t s ( void )
{ INTCONbits . GIE = 1 ; }
void
d i s a b l e I n t e r r u p t s ( void )
{ INTCONbits . GIE = 0 ; }
//
INTERRUPT SERVICE ROUTINE
//
//
High
#pragma
priority
code
interrupt
vector
InterruptVectorHigh
= 0 x08
void
InterruptVectorHigh
( void )
40
_asm
goto
InterruptHandlerHigh
/ / jump
to
interrupt
routine
_endasm
}
//
//
High
priority
interrupt
#pragma
code
#pragma
interrupt
routine
void
()
{
INTCONbits . GIE =
0;
//
TIMER
if
( INTCONbits . TMR0IF ) {
delta_10ms
// c h e c k
for
TMR0
overflow
1;
t i m e ++;
TMR0L=TIMER0_VALUE_LOW ;
TMR0L=TIMER0_VALUE_HIGH ;
INTCONbits . TMR0IF =
0;
// c l e a r
interrupt
flag
/ / ADC
i f ( P I R 1 b i t s . ADIF ) {
//
= ADRESH ;
//
= ADRESL ;
adc_buffer_full
P I R 1 b i t s . ADIF =
1;
0;
}
/
//
I2C
i f ( P I R 1 b i t s . SSPIF ) {
//
b t o a ( SSPSTAT ,
//
putstr ( str ) ;
str );
putch ( ' \ n ' ) ;
P I R 1 b i t s . SSPIF =
0;
*/
INTCONbits . GIE =
1;
}
//
void
ltoa ( long
int
value ,
char
* dest )
{
long
int
char
r e s t o =0 ,
i f ( value
val
i f ( val
v a l=v a l u e ;
<
i =0 ,
j =0 ,
aux = 0 ;
0)
==
0){
val ;
d e s t [ 0 ] = 0 x30 ;
dest [1]=0;
return ;
}
f o r ( i =0;
val
>
0;
i ++)
{
resto
val
10;
val
val
10;
dest [ i ]
resto
i f ( value
<
0)
+ 0 x30 ;
{
d e s t [ i ++] =
'
';
}
dest [ i
]=0;
f o r ( j =0;
j <i ;
j ++,
{
aux =
dest [ j ] ;
dest [ j ]
dest [ i ]
= aux ;
dest [ i ] ;
41
}
}
void
inttoa ( int
value ,
char
* dest )
{
int
v a l=v a l u e ;
char
r e s t o =0 ,
i =0 ,
j =0 ,
aux = 0 ;
i f ( ! value )
{
dest [0]= '0 ';
dest [1]= '\0 ';
return
}
i f ( value
val
<
f o r ( i =0;
0)
val ;
val
>
0;
i ++)
{
resto
val
10;
val
val
10;
dest [ i ]
resto
i f ( value
<
0)
+ 0 x30 ;
{
d e s t [ i ++] =
'
';
}
dest [ i
]=0;
f o r ( j =0;
j <i ;
j ++,
{
aux =
dest [ j ] ;
dest [ j ]
dest [ i ]
= aux ;
dest [ i ] ;
}
}
/
void
Sample ( v o i d ) {
TRISD = 0 x 0 0 ;
PORTD = 0 x 0 0 ;
}
void
Hold ( v o i d ) {
TRISD = 0 x 0 0 ;
PORTD = 0xFF ;
}
*/
void
SampleAndHold ( u n s i g n e d
unsigned
int
int
sample_time ) {
i =0;
TRISD = 0 x 0 0 ;
PORTD = 0 x 0 0 ;
w h i l e ( sample_time >
i ) { i ++;}
TRISD = 0 x 0 0 ;
PORTD = 0xFF ;
}
void
SendMeasures ( v o i d ){
char
//
check_sum =
header
0;
bytes
p u t c h ( 0 xEA ) ;
//
Our
p u t c h ( 0 xBB ) ;
//
IMU
stuff
putch ( 0 x00 ) ;
//
Message
type
p u t c h ( 0 x0B ) ;
//
Payload
length
*
//
data
bytes
//
bits
conversion
putch ( measures_char [ 0 ] ) ;
*/
//
10
bits
conversions
putch ( measures [ 0 ] [ 0 ] ) ;
42
//
temperature
dummy
putch ( 0 x00 ) ;
//
unsigned
long
to
bytes
( i
*8);
f o r ( i = 0 ; i < 4 ; i ++){
time_aux =
t i m e >>
t i m e _ b y t e [3 i ]
( unsigned
c h a r ) time_aux ;
}
//
time
bytes
( big
endien )
putch ( time_byte [ 0 ] ) ;
check_sum =
0xEA +
0xBB +
0 x0B +
/
bits
conversion
measures_char [ 0 ] +
*/
//
10
bits
conversion
measures [ 0 ] [ 0 ] +
measures [ 0 ] [ 1 ] +
measures [ 1 ] [ 0 ] +
measures [ 1 ] [ 1 ] +
measures [ 2 ] [ 0 ] +
measures [ 2 ] [ 1 ] +
measures [ 3 ] [ 0 ] +
measures [ 3 ] [ 1 ] +
measures [ 4 ] [ 0 ] +
measures [ 4 ] [ 1 ] +
measures [ 5 ] [ 0 ] +
measures [ 5 ] [ 1 ] +
time_byte [ 0 ] +
time_byte [ 1 ] +
time_byte [ 2 ] +
time_byte [ 3 ] ;
// c h e c k
sum
byte
43
p u t c h ( check_sum ) ;
}
/
unsigned
long
DoubleByteToLong ( u n s i g n e d
unsigned
long
unsigned
long
high_aux ;
unsigned
long
low_aux ;
low_aux =
( unsigned
( unsigned
high_aux
h i g h _ a u x <<
high_aux
return
high ,
unsigned
l o n g ) low ;
high_aux
aux =
char
aux = 0 ;
long ) high ;
8;
low_aux ;
aux ;
*/
void
ShowMeasures ( v o i d ) {
char
for
i ;
( i = 0 ; i < 6 ; i ++){
l t o a ( measures_char [ i ] ,
putstr ( str ) ;
str );
tab
}
}
/
void
ShowMeasuresAverage ( v o i d ) {
char
for
i ;
( i = 0 ; i < 6 ; i ++){
l t o a ( measures_average [ i ] ,
putstr ( str ) ;
str );
tab
}
}
*/
/*
void
FilterMeasures ( void ){
i f ( window < WINDOW_SIZE) {

w i n d o w _ m e a s u r e s [ 0 ] [ window ]
filtered_measures [ 0 ]
+= w i n d o w _ m e a s u r e s [ 0 ] [ window ] ;
window++;
}
else {
+=
window_measures [ 0 ] [ j ] ;
+= w i n d o w _ m e a s u r e s [ 1 ] [ j ] ;
window_measures [ 0 ] [ j ]
j ++;
if ( j
>
9){ j
0;}
44
char
low ) {
*/
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
/ / ADC. C
#i n c l u d e
void
"ADC. h "
ADC( u n s i g n e d
unsigned
i
//
char
char
channel ){
i ;
0;
A/D RESULT JUSTIFICATION
RIGHT_JUSTIFIED
/ / PORT CONFIGURATION CONTROL
ALL_ANALOGIC
//
CONVERSION CLOCK FREQUENCY
FREQ_OSC_32
//
For
XTAL =
2 0MHz
/ / CHANNEL
i f ( channel
==
0){
CHANNEL_0 }
//
pin2
( RA0 )
else
i f ( channel
==
1){
CHANNEL_1 }
//
pin3
( RA1 )
else
i f ( channel
==
2){
CHANNEL_2 }
//
pin4
( RA2 )
else
i f ( channel
==
3){
CHANNEL_3 }
//
pin5
( RA3 )
else
i f ( channel
==
4){
CHANNEL_4 }
//
pin7
( RA5 )
else
i f ( channel
==
5){
CHANNEL_5 }
//
pin8
( RE0 )
else
i f ( channel
==
6){
CHANNEL_6 }
//
pin9
( RE1 )
else
i f ( channel
==
7){
CHANNEL_7 }
//
p i n 1 0 ( RE2 )
/ / ADC SWITCH ON/OFF

ADC_ON
//
INTERRUPTS
INTERRUPTS_ON
//
DELAY AQUISITION TIME
13 us
INSTRUCTION_64
//
CONVERSION SWITCH
. . .
Start
conversion
CONVERSION_ON
//
. . .
interrupt
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
//
I2C . C
#i n c l u d e
//
" I2C . h "
INIT MASTER
void
I n i t I 2 c M a s t e r ( void ){
T R I S C b i t s . TRISC3
1;
1;
SSPCON1
= 0 x38 ;
SSPCON2 = 0 x 0 0 ;
//
SSPSTAT = 0 x 8 0 ;
SSPSTATbits . SMP =
1;
SSPSTATbits . CKE =
1;
SSPADD
//
SSPADD
49;
//
= 0 x0C ;
2 0M/ ( 4
//
* SSPADD+1)
P I R 1 b i t s . PSPIF
0;
//
Parallel
P I R 2 b i t s . BCLIF
0;
//
Bus
P I E 1 b i t s . SSPIE
INTCONbits . PEIE
1 0 0K <=> SSPADD =
49
4 0 0 kHz
1;
=
//
1;
Slave
collision
Synchronous
//
Port
Serial
Peripheral
RESERVED on
Detection
Port
Interrupt
PIC16F873 / 8 7 6 ;
MUST BE CLEARED
cleared
Interrupt
Enable
Bit
Bit
enabled
enabled
}
//
INIT
void
SLAVE
I n i t I 2 c S l a v e ( void ){
1;
1;
SSPCON1
= 0 x36 ;
SSPCON2 = 0 x 0 0 ;
SSPSTAT = 0 x 8 0 ;
SSPADD
2;
P I R 1 b i t s . PSPIF
0;
//
Parallel
P I R 2 b i t s . BCLIF
0;
//
Bus
Slave
collision
Port
RESERVED on
Detection
cleared
45
PIC16F873 / 8 7 6 ;
MUST BE CLEARED
SSPCON2bits . GCEN
P I E 1 b i t s . SSPIE
=
=
INTCONbits . PEIE
1;
1;
=
//
//
1;
General
Call
Synchronous
//
Enabled
Serial
Peripheral
Port
Interrupt
Interrupt
Enable
Bit
}
//
START
void
S t a r t I 2 c ( void ){
WaitIdleI2c ( ) ;
SSPCON2bits . SEN= 1 ;
//
automatically
cleared
by
hardware
}
//
REPEATED START
void
RepStartI2c ( void ){
WaitIdleI2c ( ) ;
SSPCON2bits . RSEN= 1 ;
//
automatically
cleared
by
hardware
}
//
STOP
void
StopI2c ( void ){
WaitIdleI2c ( ) ;
SSPCON2bits . PEN= 1 ;
//
automatically
cleared
by
hardware
}
/ / READ SLAVE
unsigned
char
unsigned
ReadSlaveI2c ( void ){
char
i2cReadData ;
WaitIdleI2c ( ) ;
//
RCEN= 1 ;
//
S h o w R e g i s t e r ( SSPBUF ) ;
//
ShowI2cRegisters ( ) ;
//
WaitIdleI2c ( ) ;
//
delay (1000);
i2cReadData
//
= SSPBUF ;
WaitIdleI2c ( ) ;
/ / ACKEN =
/
nl
1;
//
send
acknowledge
*
if
ack
{
ACKDT= 0 ;
}
else
{
ACKDT= 1 ;
}
ACKEN= 1 ;
//
send
acknowledge
sequence
*/
return (
i2cReadData
);
}
/ / READ MASTER
unsigned
char
unsigned
ReadMasterI2c ( void ){
char
i2cReadData ;
WaitIdleI2c ( ) ;
//
S h o w R e g i s t e r ( SSPBUF ) ;
//
ShowI2cRegisters ( ) ;
//
WaitIdleI2c ( ) ;
nl
delay (1000);
i2cReadData
= SSPBUF ;
WaitIdleI2c ( ) ;
SSPCON2bits .ACKEN =
/
1;
//
send
acknowledge
*
if
ack
{
ACKDT= 0 ;
}
else
{
ACKDT= 1 ;
}
ACKEN= 1 ;
//
send
acknowledge
sequence
*/
return (
i2cReadData
);
}
//
WRITE
unsigned
char
WriteI2c ( unsigned
char
i2cWriteData ){
46
Bit
enabled
enabled
WaitIdleI2c ( ) ;
SSPBUF =
i2cWriteData ;
//
delay ( 3 5 ) ;
//
//
return
ACKSTAT
return
Delay
must
);
be
//
higher
function
than
31
returns
to
allow
'1 '
if
ACKNOWLEDGE
transmission
reception
is
acknowledged
0;
}
//
WAIT FOR IDLE
void
WaitIdleI2c (){
while
((
SSPCON2 & 0 x1F
SSPSTATbits .R_W )
{};
//
}
void
ShowI2cRegisters ( void ){
unsigned
char
string [ 9 ] ;
putch ( ' S ' ) ;

b t o a ( SSPSTAT ,
string );
putstr ( string ) ;
tab
putch ( 'C ' ) ;

b t o a ( SSPCON1 ,
string );
putstr ( string ) ;
tab
putch ( 'C ' ) ; putch ( ' 2 ' ) ;

b t o a ( SSPCON2 ,
string );
putstr ( string ) ;
}
void
ShowRegister ( unsigned
unsigned
char
btoa ( reg ,
char
reg ){
bits [ 9 ] ;
bits );
putstr ( bits ) ;
}
void
btoa ( unsigned
char
val ,
unsigned
str [0]
( val
& 0 x 8 0 ) >>
7)
+ 0 x30 ;
str [1]
( val
& 0 x 4 0 ) >>
6)
+ 0 x30 ;
str [2]
( val
& 0 x 2 0 ) >>
5)
+ 0 x30 ;
str [3]
( val
& 0 x 1 0 ) >>
4)
+ 0 x30 ;
str [4]
( val
& 0 x 0 8 ) >>
3)
+ 0 x30 ;
str [5]
( val
& 0 x 0 4 ) >>
2)
+ 0 x30 ;
str [6]
( val
& 0 x 0 2 ) >>
1)
+ 0 x30 ;
str [7]
( val
str [8]
0;
& 0 x01 )
char
* s t r ){
+ 0 x30 ;
}
//
DELAY ALPHA
void
delay ( unsigned
unsigned
for
int
int
count ){
i ;
( i = 0 ; i <c o u n t ;++ i ) { }
}
//
INTERRUPT SERVICE ROUTINE
/ / MASTER TRANSMISSION ROUTINE

/
void
I 2 c I n t e r r u p t S e r v i c e R o u t i n e ( void ){
static
if
char
( state
s t a t e =0;
==
0){
s t a t e =1;
W r i t e I 2 c ( 0 x02 ) ;
}
if
(( state
==
1)
&&
(ACKSTAT ==
0)){
s t a t e =2;
W r i t e I 2 c ( 0 x05 ) ;
}
i f (( state
==
2)
&&
(ACKSTAT ==
0)){
s t a t e =0;
StopI2c ( ) ;
}
}
*/
//
SLAVE RECEPTION ROUTINE
void
I 2 c I n t e r r u p t S e r v i c e R o u t i n e ( void ){
static
char
unsigned
i f (( state
s t a t e =0;
char
==
buffy ;
0)){
state
buffy
= SSPBUF ;
1;
ShowRegister ( buffy ) ;
nl
47
wait
for
idle
and
not
writing
else {
putch ( ' F ' ) ;
}
i f ( state
==
1){
state
0;
buffy
= SSPBUF ;
ShowRegister ( buffy ) ;
nl
}
else {
putch ( ' F ' ) ;
}
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
//
DELAY . C
*
* Delay f u n c t i o n s
* See delay . h f o r d e t a i l s
*
* Make s u r e t h i s c o d e i s c o m p i l e d
*/
#i n c l u d e
with
full
optimization ! ! !
" d e l a y . h"
void
DelayMs ( u n s i g n e d
char
cnt )
{
#i f
XTAL_FREQ <= 2MHZ
do
DelayUs ( 9 9 6 ) ;
w h i l e ( c n t ) ;
#e n d i f
#i f
XTAL_FREQ >
unsigned
do
do
char
2MHZ
i ;
4;
DelayUs ( 2 5 0 ) ;
w h i l e ( i ) ;
}
}
w h i l e ( c n t ) ;
#e n d i f
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
//
USART . C
*
* USART MODULE
*
* f i l e s : usart . c ,
*
*/
#i n c l u d e
void
usart . h
" u s a r t . h"
initUsart ()
{
T R I S C b i t s . TRISC7 =
1;
//
RX
( input )
T R I S C b i t s . TRISC6 =
0;
//
TX
( output )
SPBRG =
10;
/ / baud
TXSTAbits . SYNC =
21 >57600
0;
RCSTAbits . SPEN =
1;
TXSTAbits .BRGH =
1;
RCSTAbits . CREN =
1;
INTCONbits . PEIE =
0;
P I E 1 b i t s . RCIE =
0;
P I E 1 b i t s . TXIE =
0;
TXSTAbits . TXEN =
10 >115200;
//
Periferal
interrupt
enable
bit
1;
}
void
pu ts tr ( char
* str )
{
while ((
putch (
* s t r ) > 0)
* ( s t r ++)
);
48
( inturrupes
p/
usart )
}
void
putch ( unsigned
char
byte )
{
w h i l e ( ! P I R 1 b i t s . TXIF )
continue ;
TXREG =
byte ;
}
unsigned
char
getch ()
{
w h i l e ( ! P I R 1 b i t s . RCIF )
continue ;
return
RCREG;
}
void
SerString_rom ( s t a t i c
char
rom
* str )
//
send
string
via
RS232
{
while ((
putch (
* s t r ) > 0)
* ( s t r ++)
);
}
void
putch_uchar
unsigned
char
value
{
unsigned
char
if
char
i ;
numero [ 1 0 ] ;
( value
==
0)
putch ( ' 0 ' ) ;

return ;
}
i =0;
while
( value )
// putch
'0 '
numero [ i ++] =
value
( value
'0 '
value
10)
);
( v a l u e %10);
10;
}
while ( i
p u t c h ( numero [ i ] ) ;
}
void
putch_ushortlong (
unsigned
short
long
value )
{
unsigned
char
if
char
i ;
numero [ 1 0 ] ;
( value
==
0)
putch ( ' 0 ' ) ;

return ;
}
i =0;
while
( value )
numero [ i ++] =
value
value
'0 '
/
( v a l u e %10);
10;
}
while ( i
}
unsigned
char
getch_number ( )
{
// i n s i r a
um
unsigned
char
numero
v a l u e =0 ,
tmp = 0 ;
putch ( ' \ n ' ) ;

putch ( ' \ r ' ) ;
putch ( ' : ' ) ;
tmp =
getch ( ) ;
while
( tmp
!=
13)
{
if
if
( tmp ==
( tmp >=
8)
'0 ')
&&
value
( tmp
<= '9 ')
value
value
*10
value /10;
putch ( ' \ r ' ) ;

putch ( ' : ' ) ;
putch_uchar ( v a l u e ) ;
putch ( '
');
putch ( '
');
putch ( '
');
49
( tmp ' 0 ' ) ;
tmp =
getch ( ) ;
}
putch ( ' \ r ' ) ;
putch ( ' : ' ) ;
putch_uchar ( v a l u e ) ;
putch ( '
');
putch ( '
');
putch ( '
return
');
value ;
}
void
putch_ulong (
unsigned
long
value
{
unsigned
char
if
char
i ;
numero [ 1 0 ] ;
( value
==
0)
putch ( ' 0 ' ) ;

return ;
}
i =0;
while
( value )
numero [ i ++] =
value
value
'0 '
/
( v a l u e %10);
10;
}
while ( i
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
//
TIMER0 . C
*
* TIMER0
*
* files :
*
*/
MODULE
TIMER0 . c ,
TIMER0 . h
#i f n d e f
__TIMER0_C__
#d e f i n e
__TIMER0_C__
#i n c l u d e
"TIMER0 . h "
//
Global
//
Functions
void
Variables
confTIMER0 ( v o i d )
{
//
OPTION REGISTER CONFIGURATION
T0CONbits . T0CS = OPERATION_TYPE ;

T0CONbits . T0SE = LOW_TO_HIGH;
T0CONbits . PSA =
1;
T0CONbits . T08BIT =
switch
//
0;
PRESCALER :
//
8 bits
1= n o t
1;
assigned ;
16 b i t s
0= a s s i g n e d
= 0
(TIMER0_PRESCALER)
{
case
2:
T0CONbits . T0PS2 = 0 ;
break ;
case
4:
break ;
case
8:
break ;
case
16:
break ;
case
32:
break ;
case
64:
break ;
case
128:
break ;
case
256:
break ;
break ;
default :
}
TMR0L=TIMER0_VALUE_LOW ;
//230;
//
100
TMR0H=TIMER0_VALUE_HIGH ;
//
INTCONbits . T0IE = 0 ; / /
TIMER0
interrupt
enabled
}
#e n d i f
50
Referncias
[1] Telmo Reis Cunha. High Precision Navigation Integrating Satellite Information - GPS
- and Inertial System Data. Tese de Doutoramento, Faculdade de Engenharia da Universidade do Porto, 2002
[2] Oliver J. Woodman. An Introduction to inertial navigation. University of Cambridge,
2007
[3] A. D. King. Inertial Navigation - Forty Years of Evolution. 1998
[4] Chen, W. and P.A. Cross (1990) Integration of GPS and Inertial System for Precise Surveying Applications. Department of Surveying, University of Newcastle upon
Tyne, pp. 375 - 395.
[5] Nayak, R.A. (2000) Reliable and Continuous Urban Navigation Using Multiple GPS
Antennas and a Low Cost IMU. Master Thesis, Department of Geomatics Engineering,
University of Calgary, Alberta.
[6] Shin, E.H. (2001) Accuracy Improvement of Low Cost INS/GPS for Land Applications. Master Thesis, Department of Geomatics Engineering, University of Calgary,
Alberta.
[7] Schwarz, K.P., H.E. Mantell, N. EI-Sheimy, R. Li, M. A. Chapman and D. Cosandier
1993: VISAT - A Mobile Highway Survey System of High Accuracy. Proceedings of
the VNIS'93, pp.476-481. Stevens
[8] Magno Morgado, G., "Desenvolvimento e anlise de desempenho de um sistema de
navegao integrado", dissertao de Mestrado Integrado em Engenharia Electrnica
e Telecomunicaes, Universidade de Aveiro, Nov. 2008.
51

Concepção de Um Pequeno Sensor Inercial 3D PDF

Caricato da

Informazioni sul documento

Titolo originale

Copyright

Formati disponibili

Condividi questo documento

Condividi o incorpora il documento

Opzioni di condivisione

Hai trovato utile questo documento?

Questo contenuto è inappropriato?

Copyright:

Formati disponibili

Concepção de Um Pequeno Sensor Inercial 3D PDF

Caricato da

Copyright:

Formati disponibili

Universidade de Aveiro Departamento de Electrnica, Telecomunicaes e

Francisco Lus Magno Concepo de um Pequeno Sensor Inercial 3D

Universidade de Aveiro Departamento de Electrnica, Telecomunicaes e

Francisco Lus Magno Concepo de um Pequeno Sensor Inercial 3D

Dissertao apresentada Universidade de Aveiro para cumprimento dos

o jri / the jury

Prof. Dr. Jos Carlos Esteves Duarte Pedro

vogais / examiners commitee

Prof. Dr. Telmo Reis Cunha

Prof. Dr. Jaime dos Santos Cardoso

Agradeo ao meu orientador por me ter dado a oportunidade de realizar este

Unidade inercial de medida, MEMS, acelermetro, giroscpio

Com este trabalho pretende-se desenvolver um IMU (Inertial Measurement

Inertial measurement unit, MEMS, accelerometer, gyroscope

Sistema de Navegao Inercial (INS)

Escolha dos sensores

Software do PIC - uxograma

Algoritmo de processamento dos dados

Plataforma giro estabilizada (adaptado de [2])

Modelo conceptual de um giroscpio mecnico (adaptado de [2]) . . . . . .

Modelo de um giroscpio MEMS, que faz uso do efeito de coriolis. . . . . .

Modelo conceptual do acelermetro (adaptada de [2]).

Acelermetro de tecnologia MEMS. . . . . . . . . . . . . . . . . . . . . . .

Diagrama do caminho do processamento do sinal

Fotograas dos sensores inerciais usados na concepo do IMU. . . . . . . .

Circuito de acondicionamento dos sinais vindos do giroscpio, acelermetro

Disposio espacial dos sensores.

(Ax, Ay, Az)

(Gx, Gy, Gz)

Andar de amplicao dos acelermetros horizontais.

Circuito de recepo dos sinais aps o acondicionamento e de transmisso

No lado esquerdo, a azul: pistas da PCB de topo;

Disposio dos componentes na PCB

Fluxograma do algoritmo do microprocessador.

Algoritmo aplicado ao INS

Referencial local e referencial global.

(Xl , Yl , Zl ): referencial local

Valor medido do acelermeto Az (horizontal) com o IMU em repouso,

Medidas obtidas do giroscpio Gx quando em repouso, temparatura constante de 20C.

Inuncia gerada por variaes da temperatura nos acelermetros. . . . . .

Inuncia gerada por variaes da temperatura nos giroscpios.

Caractersticas dos sensores. . . . . . . . . . . . . . . . . . . . . . . . . . .

Mdia relativa a medidas do giroscpio e acelermetro quando em repouso.

Desvio padro relativo a medidas do giroscpio e acelermetro quando em

Este projecto consiste, essencialmente, em trs componentes: a concepo e construo

Um sensor adicional de temperatura ser tambm includo, com o intuito

de avaliar a sensibilidade dos sensores s variaes da temperatura. Todo o IMU dever

Os sistemas de navegao baseados em sensores de

dead-reckoning (i.e., cuja informao de

navegao se reporta apenas a variaes relativas s posies anteriores, e no a posio

Com o desenvolver da tecnologia, vrios tipos de

Nas duas ltimas dcadas tem havido uma forte evoluo

em reas de investigao que se dedicam localizao robusta de preciso de viaturas

se considera um automvel a deslocar-se numa zona urbana, ou em zonas arborizadas, o

(que no dependem de sinais provenientes do exterior da viatura), sendo

2.1 Sistema de Navegao Inercial (INS)

giro-estabilizada ou nos sistemas xos (

Nos sistemas de plataforma giro-estabilizada os sensores inerciais esto montados numa

Isto consegue-se custa de uma estrutura de trs arcos articulados

gimbals ), que cara-

propunha o desao de eliminar toda a estrutura de eixos articulados (

cterizava os sistemas inerciais da poca, xando os acelermetros e os giroscpios. Desta

Figura 1: Plataforma giro estabilizada (adaptado de [2])

Software do PIC - uxograma

Fotograas dos sensores inerciais usados na concepo do IMU. . . . . . . .

Andar de amplicao dos acelermetros horizontais.

Inuncia gerada por variaes da temperatura nos acelermetros. . . . . .

Inuncia gerada por variaes da temperatura nos giroscpios.

giro-estabilizada ou nos sistemas xos (

propunha o desao de eliminar toda a estrutura de eixos articulados (

cterizava os sistemas inerciais da poca, xando os acelermetros e os giroscpios. Desta

se os eixos mecnicos articulados por eixos matemticos articulados, proporcionando

Giroscpios so sensores com a funo de quanticar variaes angulares relativamente a

custo nanceiro reduzido

grau de abilidade elevado

Tal como indica a gura 4, fazendo a

Este circuito de amplicao constituido por

0.3g seja suciente.

Figura 7: Fotograas dos sensores inerciais usados na concepo do IMU.

sensores inerciais. O amplicador do giroscpio ter um ganho unitrio, j o amplicador

diferena de que no caso do sensor vertical, o valor de referncia para a amplicao no