Controlo e Simulao de um Quadrirotor

convencional
Srgio Eduardo Aurlio Pereira da Costa
Dissertao para obteno do Grau de Mestre em
Engenharia Aeroespacial
Jri
Presidente: Prof. Fernando Jos Parracho Lau (DEM)
Orientao: Prof. Jos Ral Carreira Azinheira (DEM)
Vogais: Prof. Alexandra Bento Moutinho (DEM)
Setembro de 2008
1
RESUMO
A presente tese teve como objectivo propor um modelo capaz de controlar e simular um UAV de
asas rotativas. Esse UAV caracterizado por quatro rotores, responsveis pela gerao do movimento,
integrados numa estrutura cruzada que embarca, no seu centro, o conjunto do material necessrio
sua automatizao: este tipo de UAV denomina-se Quadrirotor.
Partindo de um Quadrirotor existente, o ALIV - Autonomous Locomotion Individual Vehicle,
procurou-se construir em ambiente MATLAB/SIMULINK um modelo adaptado, MALIV - My Au-
tonomous Locomotion Individual Vehicle, capaz de pr prova as reais capacidades de um UAV deste
gnero. Para isso, foram utilizados, no s os sensores a bordo do ALIV, como foram implementados
dois mdulos diferentes para melhorar o seu controlo integrado. Um deles faz uso de um joystick, e
o segundo de uma cmara embarcada para o controlo da posio do Quadrirotor.
Atravs das sadas provenientes dos sensores modelados, foi possvel realizar um ltro de Kalman
para a observaao dos estados globais. A essa estimativa foi aplicado um controlador ptimo, o LQR
Regulador Linear Quadrtico, para consequentemente se fechar o anel atravs da realimentao
para cada um dos motores. A este tipo de controlador, que usufrui do princpio da separao d-se o
nome de: LQG Regulador Linear Gaussiano.
Por m, com base nos mdulos anteriores, foi construdo um mdulo nal em tempo real para
simular a reaco do MALIV em misses destinadas a UAVs deste gnero: este mdulo integra
a estabilizao a uma dada altitude controlado pelo joystick, como o seguimento de um objecto
animado com velocidades moderadas.
Palavras-chave: Quadrirotor; Filtro de Kalman; Controlo ptimo; Regulador Linear Gaussiano;
Simulao em Tempo Real.
2
ABSTRACT
The present thesis proposes a model to simulate and control a rotary wing UAV. This UAV is
characterized by four rotors that are responsible for the whole movement and are placed in a cross
structure that includes, in its center, all the necessary equipment for its automatization. Such a UAV
is usually called Quadrotor.
Based on an existing Quadrotor, named ALIV - Autonomous Locomotion Individual vehicle, a
model was built in MATLAB/SIMULINK, named MALIV - My ALIV, able to test the real capacities
of a UAV like this one.
Through the modelling of its real sensors, it was possible to design a Kalman Filter to estimate
the Quadrotor state. Using this estimated state, a Linear Quadratic Regulator (LQR) was applied, to
obtain the feedback for each one of the rotors. This type of control design, based on the separation
principle, is often called LQG - Linear Quadratic Gaussian.
Finally, based on the above modules, a nal real-time SIMULNIK model was built, which simulates
the ALIV reaction for missions targetted for this type of UAVs: this integrated model, either performs
the stabilization at a certain altitude, under joystick control, or visually tracks a slowly moving object.
Keywords: Quadrotor; Kalman Filter; Optimal Control; LQG - Linear Quadratic Gaussian; Real-
Time Simulation.
3
Contedo
1 Introduo 9
1.1 UAVs - Introduo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
1.2 Quadrirotores convencionais . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
1.3 Histria dos Quadrirotores . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10
1.3.1 O princpio da Histria dos Quadrirotores . . . . . . . . . . . . . . . . . . . . 10
1.3.2 Os Quadrirotores presentemente . . . . . . . . . . . . . . . . . . . . . . . . 11
1.4 Objectivos . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13
1.5 Organizao/Estrutura da tese . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14
2 Plataforma 15
2.1 Plataforma Quadrirotor convencional . . . . . . . . . . . . . . . . . . . . . . . . . . 15
2.2 Plataforma ALIV . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16
2.2.1 Funcionamento geral do ALIV . . . . . . . . . . . . . . . . . . . . . . . . . . 17
2.2.2 Equipamento a bordo do ALIV . . . . . . . . . . . . . . . . . . . . . . . . . 18
2.3 Plataforma considerada no estudo . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19
3 Modelao matemtica do problema 20
3.1 Modelao da dinmica . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20
3.2 Modelao da cinemtica . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21
3.2.1 ngulos de Euler . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21
3.2.2 Quaternies . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22
3.3 Modelao do conjunto motor mais hlice . . . . . . . . . . . . . . . . . . . . . . . 24
3.4 Peso e momentos de inrcia . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26
3.5 Sensores . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28
3.5.1 Acelermetros . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28
3.5.2 Magnetmetro . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29
3.5.3 Cmara embarcada . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30
4 Controlador 32
4.1 Linearizao das equaes . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32
4.2 LQR (Regulador Quadrtico Linear) . . . . . . . . . . . . . . . . . . . . . . . . . . 34
4.3 Estimador . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34
5 Implementao em SIMULINK 37
5.1 Mdulos nais . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38
5.1.1 Mdulo com Joystick . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38
5.1.2 Mdulo de visualizao . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 40
5.1.3 Mdulo integrado . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41
5.2 Tempo real . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 42
5.2.1 Criao de um executvel atravs do Real-Time Workshop . . . . . . . . . . 42
5.2.2 Real-Time Windows Target . . . . . . . . . . . . . . . . . . . . . . . . . . . 43
5.3 Visualizao 3D dos resultados . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44
4
6 Apresentao e discusso dos resultados 46
6.1 Sistema em anel fechado ideal . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46
6.1.1 Clculo da matriz de ganho K . . . . . . . . . . . . . . . . . . . . . . . . . 47
6.1.2 Apresentao dos resultados para o anel fechado ideal . . . . . . . . . . . . . 48
6.2 Sistema com sensor de posio ideal . . . . . . . . . . . . . . . . . . . . . . . . . . 50
6.2.1 Construo do estimador . . . . . . . . . . . . . . . . . . . . . . . . . . . . 50
6.2.2 Apresentao dos resultados para o sensor de posio ideal . . . . . . . . . . 52
6.3 Mdulos nais . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57
6.3.1 Mdulo com joystick . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 58
6.3.2 Mdulo de visualizao . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63
6.3.3 Mdulo integrado . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 69
7 Concluses 79
7.1 Trabalhos futuros . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 79
5
Lista de Figuras
1 Quadrirotor convencional [4] . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10
2 Primeiro Quadrirotor, Brguet-Richet Quad-Rotor Helicopter, 1907 [5] . . . . . . . . 10
3 Quadrirotr de Etienne Oemichen, 1922 [3] . . . . . . . . . . . . . . . . . . . . . . . 10
4 Draganyer da RC Toys [9] . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
5 X-4 Flyer II [11] . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12
6 Quadrirotor, projecto STARMAC II[12] . . . . . . . . . . . . . . . . . . . . . . . . . 12
7 ALIV - Nomenclatura . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15
8 Relao de rotao para os movimentos do Quadrirotor . . . . . . . . . . . . . . . . 15
9 ALIV . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16
10 ALIV - Translao lateral . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17
11 ALIV - Translao frontal . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18
12 ALIV - Movimento de guinada . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18
13 Emissor . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19
14 Receptor . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19
15 Referenciais: NED e ABC . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20
16 Funcionamento genrico de um motor elctrico de corrente contnua . . . . . . . . . 24
17 Actuador . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25
18 Hlice 9x4,5 APCE . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26
19 Filosofa dos mdulos durante a fase de simulao . . . . . . . . . . . . . . . . . . . 37
20 Ilustrao do mdulo com joystick durante a fase de simulao . . . . . . . . . . . . 40
21 Ilustrao do mdulo de visualizao durante a simulao . . . . . . . . . . . . . . . 41
22 Ilustrao do mdulo integrado durante a simulao . . . . . . . . . . . . . . . . . . 42
23 Implementao do joystick em tempo real . . . . . . . . . . . . . . . . . . . . . . . 44
24 O ALIV real e virtual . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45
25 Simulao do ALIV no FLIGHTGEAR . . . . . . . . . . . . . . . . . . . . . . . . . 45
26 Sistema em anel fechado ideal . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46
27 Posio para deslocao a 5 m de altitude e 3 m segundo x e y (anel fechado ideal) . 48
28 Atitude para deslocao a 5 m de altitude e 3 m segundo x e y (anel fechado ideal) . 48
29 Rotao nos motores na deslocao a 5 m de altitude e 3 m segundo x e y (anel
fechado ideal) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49
30 Sistema com sensor de posio ideal . . . . . . . . . . . . . . . . . . . . . . . . . . 50
31 Estimador em diagrama de blocos para o caso do sistema com sensor de posio ideal 51
32 Erro entre os estados reais e estimados, sensor de posio ideal . . . . . . . . . . . . 53
33 Posio para deslocao a 5 m de altitude e 3 m segundo x e y, sensor de posio ideal 54
34 Atitude para deslocao a 5 m de altitude e 3 m segundo x e y, sensor de posio ideal 54
35 Erro entre a posio estimada e real, sensor de posio ideal . . . . . . . . . . . . . 55
36 Erro entre a atitude estimada e real, sensor de posio ideal . . . . . . . . . . . . . . 56
37 Posio e atitude para uma situao limite, sensor de posio ideal . . . . . . . . . . 56
38 Diagrama de blocos do mdulo integrado . . . . . . . . . . . . . . . . . . . . . . . . 57
39 Controlador em diagrama de blocos adoptado para o mdulo com joystick . . . . . . 58
40 Posiao xyz ao longo da segunda trajectria . . . . . . . . . . . . . . . . . . . . . . 60
6
41 Atitude ao longo da segunda trajectria . . . . . . . . . . . . . . . . . . . . . . . . 60
42 Voltagem aplicada aos motores ao longo da segunda trajectria . . . . . . . . . . . . 61
43 Velocidades lineares ao longo da segunda trajectria . . . . . . . . . . . . . . . . . . 62
44 Pontos de referncia no referencial NED, R
NED
. . . . . . . . . . . . . . . . . . . . 63
45 Trajectria do objecto ao longo da simulao no referencial NED, R
NED
. . . . . . . 66
46 Posiao de vigilncia xyz ao longo da trajectria . . . . . . . . . . . . . . . . . . . . 66
47 Atitude ao longo da trajectria de vigilncia . . . . . . . . . . . . . . . . . . . . . . 67
48 Posiao de vigilncia xyz ao longo da trajectria, segunda trajectria . . . . . . . . . 67
49 Atitude ao longo da segunda trajectria de vigilncia . . . . . . . . . . . . . . . . . . 68
50 Velocidades lineares ao longo da segunda trajectria . . . . . . . . . . . . . . . . . . 68
51 Posio do MALIV ao longo do tempo, Misso n1 . . . . . . . . . . . . . . . . . . 70
52 Selector dos mdulos ao longo da primeira misso . . . . . . . . . . . . . . . . . . . 70
53 Posio do MALIV ao longo do tempo, Misso n2 . . . . . . . . . . . . . . . . . . 71
54 Selector dos mdulos ao longo da segunda misso . . . . . . . . . . . . . . . . . . . 71
55 Erro associado entre as sadas estimas e as sadas reais para a coordenada y dos pontos
de referncia . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 72
56 Velocidades lineares do MALIV ao longo do tempo, Misso n2 . . . . . . . . . . . . 72
57 Atitude do MALIV ao longo do tempo, Misso n2 . . . . . . . . . . . . . . . . . . 73
58 Alterao dos pontos de referncia em diagrama de blocos . . . . . . . . . . . . . . 74
59 Trajectria do objecto a ser vigiado segundo xy . . . . . . . . . . . . . . . . . . . . 75
60 Trajectria do objecto a ser vigiado segundo xy ao longo do tempo . . . . . . . . . . 75
61 Selector dos mdulos ao longo da terceira misso . . . . . . . . . . . . . . . . . . . 76
62 Posio do MALIV ao longo do tempo, Misso n3 . . . . . . . . . . . . . . . . . . 76
63 Atitude do MALIV ao longo do tempo, Misso n3 . . . . . . . . . . . . . . . . . . 77
64 Velocidades lineares do MALIV ao longo do tempo, Misso n3 . . . . . . . . . . . . 77
65 Equaes em diagrama de blocos que denem a dinmica do ALIV . . . . . . . . . . 98
66 Equaes em diagrama de blocos que denem os sensores do ALIV . . . . . . . . . . 98
67 Parmetros adoptados para a resoluo da simulao . . . . . . . . . . . . . . . . . 99
68 Congurao adoptada para a gerao do executvel, Generic Real-Time Target . . 99
69 Hardware Implementation do Conguration Parameters . . . . . . . . . . . . . . 100
70 Congurao adoptada para a gerao do executvel, Real-Time Windows Target . 100
71 osets do cheiro ufo.xml . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 101
72 Animao da primeira hlice no FLIGHTGEAR atravs do cheiro ufo.xml . . . . . . 102
73 Blocos para interface entre o Simulink e o FLIGHTGEAR . . . . . . . . . . . . . . . 102
74 Flat Earth to LLA utilizado . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 103
75 Modelo completo utilizado na interface entre o Simulink e o FLIGHTGEAR . . . . . 103
7
Lista de Tabelas
1 Diferencial de rotao aplicado aos rotores para obteno dos movimentos desejados 16
2 Caractersticas do motor AXI 2212-12 . . . . . . . . . . . . . . . . . . . . . . . . . 25
3 Caractersticas da hlice 9 4, 5APCE . . . . . . . . . . . . . . . . . . . . . . . . . 26
4 Peso dos componentes a bordo do ALIV . . . . . . . . . . . . . . . . . . . . . . . . 27
5 Limitaes dos Acelermetros utilizados . . . . . . . . . . . . . . . . . . . . . . . . 29
8
1 Introduo
1.1 UAVs - Introduo
Os UAVs (Unmanned Aerial Vehicle) so denidos de acordo com o Departamento de Defesa
dos Estados Unidos, como veculos areos com propulso prpria e que no possuem uma cabine de
pilotagem. Fazem em geral uso de foras aerodinmicas para provocarem sustentao, e podem ser
voados autonomamente ou por controlo remoto [1].
Os UAVs tornaram-se num objecto de estudo e desenvolvimento depois de terem sido provadas
as suas capacidades para enfrentarem situaes em que por alguma razo o objectivo da misso no
fosse a mais aconselhada para uma aeronave de pilotagem convencional. Os UAVs tm actualmente
variadssimas conguraes e capacidades, de acordo com as misses a que se propem. Entre essas
misses, destacam-se situaes de guerra, onde os UAVs podem desempenhar um papel importante
quer em termos de reconhecimento e vigilncia, quer em termos de bombardeamento. Mas no foram
s para situaes de guerra que os UAVs se tornaram importantes, actualmente existem inmeros
UAVs que desempenham funes preponderantes na observao de fenmenos meteorolgicos, onde
no possvel a presena humana, em zonas de vigilncia prolongadas em que no se afectado pelo
cansao inerente dos pilotos e onde situaes de voo estabilizado so conseguidas, em laboratrios e
universidades onde so usados para ns de investigao cientica, entre muitas outras situaes [2].
Ao longo do tempo surgiram inmeros UAVs, construdos e desenvolvidos de acordo com o m
a que se propunham. De entre as vrias conguraes, destacam-se os UAVs de asa rotativa, por
serem os pioneiros no tipo de UAV utilizado para o desenvolvimento desta tese, a qual se cingir ao
controlo e simulao de um Quadrirotor.
1.2 Quadrirotores convencionais
O UAV de asa rotativa utilizado o chamado Quadrirotor, ou Quadrotor. O Quadrirotor con-
vencional um UAV de dimenses reduzidas constitudo por quatro rotores xos. Os rotores so
instalados em cada um dos quatro cantos de uma estrutura cruzada, que embarca no seu centro,
o conjunto de equipamento necessrio para a sua automatizao, Figura 1. De modo a anular a
resistncia associada rotao das ps das hlices, cada um dos pares de rotores opostos, gira num
sentido diferente. por isso considerado de fcil construo. Outra vantagem o facto de, por
possuir quatro rotores, os dimetros das hlices poderem ser menores do que as de um helicptero
convencional. Sendo assim, cada um deles necessita de menor energia cintica, e ainda possibilita ao
UAV alcanar locais de menor acessibilidade. No entanto, os Quadrirotores apresentam uma dinmica
algo difcil de se conseguir estabilizar, que se poder tornar numa diculdade acrescida, principal-
mente se o mesmo for constitudo por equipamentos de auxlio de baixo custo. Actualmente, os
Quadrirotores possuem diversos sensores e sistemas de controlo para obter a sua estabilizao, e a
grande vantagem a simplicidade de construo, e o facto de se poder control-lo integralmente,
variando apenas a velocidade de rotao dos rotores [3].
9
Figura 1: Quadrirotor convencional [4]
1.3 Histria dos Quadrirotores
1.3.1 O princpio da Histria dos Quadrirotores
Os Quadrirotores surgiram no princpio dos anos 1900 e vrios exemplares foram criados nos vinte
anos seguintes. Inicialmente, a ideia seria transportar um piloto que controlasse os quatro rotores. O
primeiro Quadrirotor da histria foi criado em 1907 por Charles Richet e pelos irmos Breguet com
o nome Brguet-Richet Quad-Rotor Helicopter (Figura 2).O Quadrirotor pesava cerca de 578 kg, e
possua um mecanismo bsico de ligao dos controlos do piloto aos rotores. Consta-se que, no
conseguio levantar mais que 1.5 m, no conseguindo obter a estabilidade necessria sua pilotagem
[5]. Apesar dos resultados insatisfatrios, Charles Richet e os irmos Breguet introduziram a teoria
dos pares de rotores a rodar em sentidos opostos, ainda hoje utilizada.
Figura 2: Primeiro Quadrirotor, Brguet-Richet Quad-Rotor Helicopter, 1907 [5]
Outro Quadrirotor na histria dos UAVs de asa rotativa, foi criado em 1922 por Etienne Oemichen.
Este Quadrirotor possua para alm dos quatro rotores, oito propulsores, estando tudo ligado ao mesmo
motor (Figura 3). Para a altura em que se concretizou, este Quadrirotor revelou um considervel grau
de liberdade e de controlabilidade, realizando mais de mil ensaios durante a dcada de 1920 .
Figura 3: Quadrirotr de Etienne Oemichen, 1922 [3]
Devido diculdade para se atingir a estabilidade de uma aeronave deste tipo, bem como da
carga fsica exercida por o piloto durante a fase de pilotagem, os Quadrirotores acabaram por perder
10
o interesse, ressurgindo sobre a forma de UAVs de asas rotativas entre as dcadas de 80 e 90. A sua
simplicidade de construo, o facto de todos os rotores contribuirem para a fora propulsiva, e ainda
a carga til descolagem serviram como principais atributos[3].
1.3.2 Os Quadrirotores presentemente
Hoje em dia, existem no s diversos Quadrirotores comercializados, como vrios laboratrios
de investigao utilizam-nos para realizarem experincias para diferentes ns. Os objectivos destas
investigaes passam primordialmente por testar micro elementos para controlo de aeronaves no
tripuladas, bem como a obteno de novos processos de estabilizao e controlo dos Quadrirotores
[6, 7, 8]. Neste captulo, sero apresentadas algumas dessas investigaes, tendo como base os
Quadrirotores.
O Draganyer (Figura 4) um Quadrirotor comercializado pela RC Toys [9]. O Draganyer
controlado atravs de um transmissor de radio controlo (R/C) e a sua estrutura composta por tubos
de bra de carbono que lhe conferem um peso reduzido. Da electrnica existente a bordo do Dra-
ganyer, destaca-se o receptor dos comandos do piloto, trs giroscpios piezoelctricos, e um micro
controlador para efectuar clculos de controlo e enviar a tenso necessria para os motores. Actual-
mente, o Draganyer j possui quatro sensores infravermelhos, que possibilitam que este se estabilize
sozinho quando opera no exterior. Muitas universidades e investigadores utilizam este modelo como
base das suas investigaes.
Figura 4: Draganyer da RC Toys [9]
Na Austrlia foi desenvolvido e testado outro Quadrirotor, o X-4 Flyer, Figura 5 [10, 11]. Um
dos testes referiu-se ao estudo da inuncia do peso das unidades de medidas inerciais. Para isso,
foi construda uma nova unidade muito mais leve que as comerciais existentes, e vericou-se que
melhores condies de estabilidade eram conseguidas. Foi introduzida uma linearizao do modelo
da dinmica em torno de um ponto de equilbrio, da qual surgiram as funes de transferncia que
relacionam as entradas do piloto com os ngulos de Euler. O X-4 Flyer construdo pesa cerca de 2 kg
e tem um comprimento de 70 cm tendo as hlices um dimetro de 28 cm. Os testes a este modelo
realizaram-se com uma bateria ligada por um o ao Quadrirotor, mas revelaram ser insucientes para
conferir propulso necessria ao Quadrirotor para este se tornar controlvel. Com base nisto, os tra-
balhos passaram por construir uma nova estrutura, capaz de realizar maior sustentao, ser mais leve,
embarcar as fontes de energia, um sistema wireless e uma cmara. Este novo design caracterizado
por possuir os rotores invertidos (Figura 5), e simulaes realizadas, indicaram que podia ser fcil
11
conduzi-lo atravs de um controlo remoto ou de um piloto automtico.
Figura 5: X-4 Flyer II [11]
Um grupo de investigadores da universidade de Stanford, Stanford Testbed of Autonomous Rotor-
craft for Multi-Agent (STARMAC) [12], est a realizar um projecto que visa demonstrar capacidades
de vigilncia e controlo em ambiente real por parte de um conjunto de Quadrirotores (Figura 6). O
projecto prope-se a demonstrar o controlo de vrios Quadrirotores em simultneo, que voaro com
total autonomia seguindo uma trajectria fornecida. Inicialmente, e numa primeira fase, os objectivos
passaram pelo domnio dos conceitos de voo. Nesta fase do projecto foi utilizado o modelo comercial
DraganFlyer IV, no entanto este revelou ter uma dinmica muito instvel e muito rpida que o tornava
extremamente difcil de pilotar. Assim foi criado com sucesso um controlador a bordo, que atravs
do amortecimento tornasse a pilotagem mais facilitada. Quanto ao seu estado era denido por trs
giroscpios, trs acelermetros e trs magnetometros mais um GPS para o controlo da posio. De
modo a alcanarem os objectivos propostos, realizaram ainda novas conguraes tanto a nvel da
electrnica de bordo como da estrutura utilizada. No nal desta primeira fase, apesar dos primeiros
objectivos terem sido obtidos, vericaram-se algumas limitaes do Quadrirotor que levaram para a
segunda fase do projecto, a alterao aos elementos do Quadrirotor original.
Figura 6: Quadrirotor, projecto STARMAC II[12]
De entre as modicaes ocorridas, destacam-se: a instalao de novos rotores para se obter maior
fora de sustentao e com isso possibilitarem a instalao de baterias mais poderosas e de outros
sistemas electrnicos. O aumento da resistncia de elementos estruturais, e a troca de elementos de
comunicao via bluetooth por Wi que se traduziu numa melhoria da estimao da posio via GPS.
Para alem do STARMAC II (Figura 6) experimental, esto em construo outros cinco Quadrirotores,
que com base nas investigaes anteriores, pretendem comear a realizar misses em ambiente real.
Um grupo de investigadores da Pensilvnia [13] desenvolveu um Quadrirotor usando um modelo
comercial, o HMX-4, semelhante ao Draganyer. Neste trabalho so usados dois computadores, um
12
a bordo e outro na estao de trabalho, para combinarem informao dos sensores inerciais a bordo
e da cmara no solo que controla o Quadrirotor atravs de marcas especicas colocadas na base
da estrutura do mesmo. Devido s limitaes de peso do HMX-4, nem GPS nem acelermetros
adicionais puderam ser colocados. O computador da estao de trabalho processa as imagens da
cmara posicionada no solo e calcula as entradas dos motores. O computador de bordo estabiliza o
Quadrirotor usando a informao dos giroscpios e recebendo as entradas para os motores provenientes
do computador da estao de trabalho. Depois deste trabalho, desenvolveram desta vez um mtodo
com duas cmaras, uma a bordo e outra no solo, para estimar a altura e a atitude do Quadrirotor
[14]. Este processo demonstrou ter erros menores quer na posio quer na atitude.
Uma equipa do Instituto Federal de Tecnologia Suia (EPFL) [15] trabalhou no controlo da razo
angular e na altitude de um Quadrirotor. Antes da implementao real, foram realizadas inmeras
simulaes em ambiente MATLAB. Usaram a dinmica do modelo do Quadrirotor para testar dois
controladores, o PID (proportional, integral and derivative) e o ptimo (LQ: Linear Quadratic). A
funo do controlador era estabilizar a altitude enquanto compensava os erros iniciais nos ngulos
de rolamento, picada e guinada. O LQ obteve um resultado pior do que o PID, devido ao facto da
dinmica do actuador no estar includa na anlise usada para determinar a matriz de ganho [15].
1.4 Objectivos
A presente tese tem como objectivo principal obter o controlo e a simulao de um Quadrirotor
convencional atravs do MATLAB/SIMULINK, para uma futura implementao num UAV deste tipo,
de modo a melhorar a estabilidade e a operacionalidade do mesmo.
Para esta tese foi utilizado como modelo de referncia um novo conceito de Quadrirotor, o ALIV -
Autonomous Locomotion Individual Vehicle
1
. O ALIV um Quadrirotor inovador que para alm das
caractersticas gerais de um UAV deste tipo, possui ainda graus de liberdade extra como consequncia
da rotao de dois dos seus rotores. Este tipo de capacidade, leva a que seja possvel no s deslocar
o ALIV horizontalmente, como por outro lado, seja possvel estabiliz-lo em posies diferentes das
habituais. Actualmente o ALIV pilotado atravs de dois joysticks, um que basicamente efectua
o controlo de potncia e as deslocaes horizontais devido rotao prpria dos dois rotores, e
outro que controla a atitude do ALIV e a consequente deslocao horizontal atravs do diferencial de
rotao entre os pares de rotores, situao convencional. Desta forma extremamente complicado
conseguir estabilizar e controlar o ALIV de acordo com as misses pretendidas. Por isso, foi proposta
a simulao e o controlo do ALIV em termos convencionais, isto , desprezando a liberdade adicional
de rotao dos dois rotores. Esta situao traz o inconveniente de no fazer uso das propriedades
adicionais deste Quadrirotor e de todos os benefcios que da possam surgir, por outro lado, vem
simplicar o problema, tornando-se posteriormente num ptimo ponto de partida para o projecto
nal em que tais caractersticas sejam consideradas. Com base nisto, foi desenvolvido um modelo
matemtico representativo da situao convencional, e criado um sistema de controlo adequado s
suas exigncias operacionais, tornando-o mais verstil e de melhor pilotagem. Para isso foram tidas
1
O ALIV insere-se num projecto liderado pelo Eng. Severino Raposo e tem uma patente pendente relativa ao
seu sistema e processo: WO 2008/054234 (PCT/PT2006/000026) System and Process of vector propulsion with
independent control of three translation and three rotation axis. Ao Eng. Severino Raposo presto os meus mais
sinceros agradecimentos pela disponibilidade prestada no esclarecimento de qualquer dvida referente ao assunto, e
facultao de todos os dados presentes nesta tese a que a ele se referem.
13
em considerao todas as caractersticas do ALIV de modo a aproximar o mais possvel a simulao
da realidade, e tornar o modelo mais vel para uma futura implementao.
Analisando o equipamento do ALIV, verica-se que este no possui qualquer sensor de posio,
fazendo com que o mesmo no possa funcionar de uma forma autnoma. objectivo desta tese
apresentar um modelo em que apenas um joystick seja o responsvel pelo controlo da posio, e
posteriormente, um sensor de posio baseado numa cmara incorporada que permita ao ALIV tornar-
se completamente autnomo. Em qualquer destas situaes, fazendo uso tambm dos outros sensores
a bordo do ALIV, os acelermetros e o magnetmetro, foi possvel controlar e estabilizar o Quadrirotor
com maior facilidade, conseguindo com isto, demonstrar as reais capacidades que um UAV deste tipo
pode apresentar em diversas misses.
1.5 Organizao/Estrutura da tese
A presente tese, aps a respectiva introduo em que dado a conhecer o estado da arte e um
pouco da histria dos Quadrirotores convencionais ao longo do captulo 1, prossegue no captulo 2 com
a explicao da plataforma do Quadrirotor convencional e a descrio do ALIV, o Quadrirotor utilizado
como referncia para todo o trabalho. O captulo contm uma breve introduo s caractersticas
adicionais que o mesmo oferece em comparao com um Quadrirotor convencional, e as principais
diferenas que da advm na obteno dos movimentos transversais. Ainda no mesmo captulo,
explicado ao pormenor o conceito da plataforma convencional de um Quadrirotor. Utilizando o ALIV
como exemplo, demonstrado como os seis graus de liberdade podem ser obtidos atravs nica e
exclusivamente do diferencial de rotao dos pares de rotores. O captulo termina com a descrio
das caractersticas principais do equipamento a bordo do ALIV que sero utilizadas futuramente na
modelao matemtica do problema.
O captulo 3 apresenta o modelo matemtico referente dinmica e cinemtica com base na
descrio da plataforma de um Quadrirotor convencional ilustrada no captulo 2, bem como de algum
equipamento a bordo do ALIV. O mesmo captulo inclui nomeadamente, o clculo dos momentos de
inrcia, a estimativa do peso, e a modelao matemtica dos actuadores com base no conjunto motor
mais hlice, atendendo s caractersticas dos componentes utilizados e anteriormente apresentados
no captulo 2. O mesmo captulo termina com a modelao dos sensores utilizados para o controlo
da posio e da atitude do ALIV. Todas estas variveis revelam ser de grande importncia no s em
termos das equaes da dinmica como da aproximao entre o modelo real e o simulado.
O captulo 4 apresenta o controlador abordado para a resoluo de um problema deste tipo, justi-
cando as aproximaes tomadas que simplicam e justicam o uso de tal controlador, o controlador
ptimo LQR - Regulador Linear Quadrtico.
O captulo 5 apresenta o trabalho realizado em ambiente MATLAB/SIMULINK. Neste captulo,
apresentada a losofa dos mdulos de simulao implementados para o cumprimento dos objectivos
a que se prope a presente tese, bem como a adaptao dos mesmos para simulaes em tempo real
e na demonstrao virtual em 3D dos resultados, utilizando o simulador de voo, FLIGHTGEAR.
Por m, no captulo 6, so apresentados e discutidos os resultados obtidos ao longo do trabalho
desenvolvido. Estes resultados englobam no s a fase inicial de validao da dinmica e do estimador
implementado, bem como da funcionalidade dos mdulos fnais.
No captulo 7, so apresentadas as concluses e as possibilidades de trabalho futuro.
14
2 Plataforma
2.1 Plataforma Quadrirotor convencional
Como foi visto na introduo, os movimentos associados a um Quadrirotor convencional esto
puramente associados variao de rotao de cada um dos rotores. A cada rotor, est associada
uma fora de sustentao, um binrio e uma fora de resistncia em torno do seu eixo de rotao. Em
consequncia, regra geral colocar os rotores opostos, um e dois, a rodar num determinado sentido,
e os rotores trs e quatro a rodar no sentido contrrio (Figura 7). Desta forma, possvel compensar
os binrios resistentes envolvidos, forando a acelerao angular do Quadrirotor (perpendicular ao
plano da Figura 7) a zero. Assim, comprovado que no necessrio a existncia de um rotor de
cauda como nos helicpteros convencionais para obter a estabilizao da guinada, contribuindo todos
os rotores neste caso para a fora de sustentao.
Figura 7: ALIV - Nomenclatura
No entanto, se a ideia for introduzir alguma guinada no UAV, necessrio aplicar um diferencial
de rotao entre os pares de rotores, mantendo-se a fora de sustentao total constante (situao (a)
da Figura 8). Todos os outros movimentos do Quadrirotor processam-se de uma forma semelhante.
Figura 8: Relao de rotao para os movimentos do Quadrirotor
Assim, se se quiser introduzir um movimento de picada, e a consequente deslocao do Quadrirotor
15
segundo a direco e
1
, necessrio aplicar um diferencial na rotao do par de rotores um e dois,
mantendo a fora de sustentao do par constante (situao (b) da Figura 8), enquanto se se quiser
aplicar um movimento de rolamento, e a consequente deslocao do Quadrirotor segundo a direco
e
2
, o diferencial deve ser aplicado ao par de rotores trs e quatro, mantendo mais uma vez constante
a fora de sustentao criada por esse par (situao (c) da Figura 8) [3].
A Tabela 1 resume a teoria atrs avanada.
Motores 1 2 3 4
= = + - Deslocamento positivo segundo e
2
+ - = = Deslocamento positivo segundo e
1
+ + - - Rotao anti-horria
h + + + + Deslocamento negativo segundo e
3
Tabela 1: Diferencial de rotao aplicado aos rotores para obteno dos movimentos desejados
O controlo e o movimento do Quadrirotor est simplesmente associado velocidade de rotao de
cada um dos rotores, devido ao binrio resistivo que actua nas ps do rotor na direco oposta sua
rotao. este o princpio bsico que rege a dinmica deste sistema. Construir o modelo matemtico
para o tipo de movimento caracterstico de um Quadrirotor, assim o primeiro passo a ser realizado
na anlise e obteno do sistema de controlo necessrio.
2.2 Plataforma ALIV
O ALIV (Figura 9) um Quadrirotor inovador em relao aos modelos de Quadrirotores existentes.
No entanto, de uma forma geral, o ALIV parecido aos chamados Quadrirotores convencionais at
aqui referidos. Este constitudo por quatro rotores nas extremidades de uma estrutura cruzada,
e embarca todo o equipamento a bordo no centro da sua estrutura, o mais centrado e alinhado
possvel com o seu centro de gravidade. Sendo assim, o ALIV continua a usufruir das qualidades e
caractersticas inerentes a um UAV deste tipo. Entre estas podemos destacar a facilidade de construo
comparativamente a UAVs genricos de asa xa, e do mtodo tpico de funcionamento operacional
em que todos os seus rotores contribuem para a fora propulsiva, ao contrrio do sucedido em UAVs
de asa rotativa tpicos como o so os helicpteros convencionais.
Figura 9: ALIV
Um Quadrirotor convencional caracterizado por ter os quatro rotores xos, isto , todos os movi-
mentos de translao so criados atravs do diferencial de rotao dos pares de rotores existentes
(Seco 2.1). Quanto ao ALIV, para alm destas capacidades, a possibilidade de rotao do eixo de
16
apoio de um dos seus pares de rotores possibilita tambm algumas destas deslocaes transversais.
Este tipo de conceito torna de facto o ALIV inovador em relao aos outros modelos existentes,
fazendo-o conseguir realizar manobras de maior complexidade ou atingir posies estacionrias difer-
entes das convencionais.
Nesta seco ser dado a conhecer com algum pormenor o ALIV, o Quadrirotor adoptado como
referncia para todo o trabalho desenvolvido. por isso importante fazer uma descrio do seu modo
de funcionamento, tanto em termos de graus de liberdade (Seco 2.2.1) como na descrio de uma
forma sucinta e adequada do seu equipamento a bordo (Seco 2.2.2) que sero relevantes na fase
de construo da dinmica do problema e do prprio controlador.
2.2.1 Funcionamento geral do ALIV
Actualmente, o ALIV pilotado atravs de dois joysticks que tentam realizar e controlar todo o
tipo de movimentos de translao ou rotao. O presente captulo serve para ilustrar e demonstrar
de uma forma idealizada esse tipo de movimentos.
De uma forma geral, um dos joysticks responsvel pelos movimentos de translao, guinada e
controlo de potncia, e o outro pelos movimentos de rotao. Em relao ao primeiro joystick, ao
boto thrust est associado o controlo de potncia integral, isto , um controlo idntico de potncia
a cada um dos rotores, controlo da altitude. portanto necessrio um constante ajuste de potncia
transmitida aos motores para que estes mantenham o Quadrirotor a uma dada altitude e igualem a
fora gravtica causada pelo peso do corpo quando este efectua uma deslocao transversal.
Em termos da translao lateral segundo e
2
para a esquerda ou para a direita, esta pode ser obtida
atravs da rotao do conjunto motor mais hlice segundo o seu eixo transversal e
1
(Figura 10).
Figura 10: ALIV - Translao lateral
Este tipo de movimento obtido com a deslocao do primeiro joystick para a esquerda ou
para a direita e constitui a parte inovadora em termos de movimentos associados aos Quadrirotores
convencionais.
Para uma translao frontal segundo e
1
, para a frente ou para trs, o mtodo em tudo semel-
hante. O controlo efectuado deslocando mais uma vez o primeiro joystick mas desta vez segundo
o outro eixo. O resultado consiste na rotao do conjunto motor mais hlice segundo o seu eixo
transversal e
2
(Figura 11).
17
Figura 11: ALIV - Translao frontal
Atravs do primeiro joystick ainda possvel obter um controlo direccional atravs da rotao
do ALIV em torno do seu eixo vertical, movimento de guinada. Para isso, roda-se o joystick para
a esquerda ou para a direita consoante a guinada pretendida, o que equivale a uma rotao oposta
segundo o eixo transversal e
1
do conjunto motor mais hlice entre o mesmo par de rotores, Figura
12.
Figura 12: ALIV - Movimento de guinada
Ao segundo joystick esto associados os movimentos de rotao. Estes movimentos so causados
pelo diferencial de rotao entre o mesmo par de rotores como explicado na Seco 2.1 que caracteriza
o tipo de funcionamento de um Quadrirotor convencional. A este tipo de movimentos, de uma forma
idealizada, no necessrio aplicar um ajuste na potncia uma vez que a fora vertical gerada pelo
conjunto dos quatro rotores, fora propulsiva, se mantm constante.
2.2.2 Equipamento a bordo do ALIV
De modo a cumprir os requisitos de pilotagem e futuramente de capacidade de voo autnomo,
o ALIV, semelhana de outros UAVs, possui a bordo diferentes tipos de equipamento mecnico e
electrnico. Nesta seco pretende-se identicar algum desse equipamento mais relevante bem como
a sua interligao.
Analisando o ALIV numa ptica mais estrutural e mecnica, o mesmo construdo sobre uma
estrutura de tubos cilndricos em alumnio com aproximadamente 6 mm de dimetro que servem de
apoio a todo o material embarcado. Tal como tem vindo a ser descrito, o mesmo possui quatro
conjuntos motor mais hlice responsveis pela gerao da sua dinmica e controlados atravs de
quatro servos GWS Pico. Actualmente o motor utilizado o GWS 350 e as hlices so idnticas ao
modelo X-UFO com aproximadamente 19 cm de dimetro. Estes conjuntos so alimentados atravs
de uma bateria de ltio de 11.1 V e 1.7 Ah. O ALIV possui ainda uma bateria auxiliar para alimentao
do microcontrolador e dos servos, de NiMh 12 V , 0.8 Ah. Do ponto de vista electrnico, o ALIV
composto por um emissor e placa de comando actual (Figura 13, cedida pelo Eng. Severino Raposo)
18
responsvel pela emisso do sinal de controlo via rdio. Atravs do controlo por rdio FM com
transceiver possvel controlar e ler os sensores internos do ALIV a uma amostragem de cerca de 8
amostras por segundo com possibilidade de extenso para 16 amostras por segundo.
Figura 13: Emissor
Em termos de sensores, existem trs acelermetros para cada um dos eixos ortogonais, (e
1
, e
2
, e
3
),
que so os LIS3L06AL com capacidade leitura de
+
/2 g, extensvel at
+
/6 g da acelerao global num
dado ponto, e os sensores de rotao, os Axis Magneto-Inductive Sensor Driver que funcionam como
giroscpios para leitura da atitude. As leituras efectuadas por ambos os sensores so processadas num
microprocessador. Existem ainda sensores da corrente total e da tenso s baterias. Para a recepo
dos dados emitidos pelo emissor, utilizado um receptor, ilustrado na Figura 14, cedida pelo Eng.
Severino Raposo. Este receptor constitudo por quatro sadas em PWM de 10bits para os quatro
motores DC, cada uma com 10 A a 20 A de corrente mxima e 15 V mximo na sada.
O sistema tem 3 LEDs de medio de potncia de sinal de rdio e 3 LEDs para a potncia da
bateria, um canal livre ADC, um porto digital I/O de 8 bits e sete linhas I/O adicionais controlveis
atravs dos dois joysticks.
Figura 14: Receptor
2.3 Plataforma considerada no estudo
O Quadrirotor um UAV caracterizado pela sua simplicidade de funcionamento mas em contra-
partida por instabilidades que o levam a ser de difcil pilotagem. O ALIV, partindo do mesmo princpio,
apresenta diculdades acrescidas pela adio de graus de liberdade suplementares. Foi principalmente
por esta razo, que se optou por considerar inicialmente as caractersticas do ALIV actuando como
se de um Quadrirotor convencional se tratasse, acabando por desprezar os graus de liberdade suple-
mentares. Assim, como referido nos objectivos, adoptando esta simplicao o trabalho tornou-se
mais acessvel, no deixando de ser um bom ponto de partida para a futuros trabalhos.
19
3 Modelao matemtica do problema
3.1 Modelao da dinmica
Para a elaborao da dinmica foram assumidos dois referenciais. O primeiro referencial um
referencial considerado inercial e xo, ligado superfcie da Terra com os dois primeiros eixos tan-
gentes ao meridiano e ao paralelo, referencial NED (North-East-Down). O segundo referencial um
referencial mvel, ligado ao corpo rgido do Quadrirotor com origem no centro de gravidade, ser
denominado ABC (Aircraft Body Centered), Figura 15. Ambos os referenciais possuem as direces
e
z
e e
3
quando estabilizados a apontar para o centro da Terra [17].
Referencial NED (North-East-Down), R
NED
= [ e
x
e
y
e
z
]
Referencial ABC (Aircraft Body Centered), R
ABC
= [ e
1
e
2
e
3
]
Figura 15: Referenciais: NED e ABC
O Quadrirotor possui seis graus de liberdade denidos por doze estados. Seis dos doze estados
controlam a atitude do sistema, isto , os ngulos de Euler ( ) e as velocidades angulares no
referencial ABC, R
ABC
, (P Q R) . Os outros seis estados controlam tanto a posio do centro de
gravidade do Quadrirotor (X Y Z) , como a sua velocidade linear (U V W) em termos do referencial
inercial NED, R
NED
.
Para se proceder derivao dos estados entre estes referenciais, torna-se assim necessrio obter
a matriz de rotao entre os dois sistemas de coordenadas. A matriz de rotao global de NED para
ABC resulta da sequncia de trs rotaes, isto :
S
E
= S

(1)
o que leva a
2
:
2
A deduo pormenorizada das equaes e matrizes apresentadas nesta seco, podem ser consultadas ao longo do
Apndice A.
20
S
E
=
_

_
cos cos sincos sin
cos sin sincos . sin sin sin sin+cos cos cos sin
cos sin cos +sin sin sin sin cos sincos cos cos
_

_
(2)
O modelo matemtico baseado nas equaes do movimento newtoniano e o referencial ABC, R
ABC
,
considerado o referencial de trabalho porque as unidades sensoriais identicam as componentes neste
mesmo referencial. Para o movimento do corpo rgido foi considerada a segunda lei de Newton que
permite obter as equaes da dinmica do Quadrirotor expressas no referencial local ABC, R
ABC
.
F
T
=
d
dt
[mV
T
]
B
+ [mV
T
]
B
(3)
M
T
=
d
dt
[I]
B
+ [I]
B
(4)
em que V
T
o vector com as velocidades lineares (U V W) e o vector com as velocidades
ngulares (P Q R) . Considerando que a fora por aco da gravidade uma constante no refer-
encial inercial e pode ser expressa no referencial local, R
ABC
, atravs de uma transformao de Euler:
F
g
= mS
E
_
0 0 g
_
T
= m
_
sin cos sin cos cos
_
T
B
g (5)
e que a fora total exercida no Quadrirotor, F
T
, constituda pela soma da fora gravtica, F
g
, e
da fora de propulso, F
p
, gerada pela rotao de cada um dos rotores, a expanso da Equao 3
conduz a:
_

W
_

_
=
1
m
_

_
F
px
F
py
F
pz
_

_
+g
_

_
sin
cos sin
cos cos
_

_
QW RV
RU PW
PV QU
_

_
(6)
Analisando agora a Equao 4 e considerando a massa e inrcia do Quadrirotor constantes, com
I
yz
= I
xy
= I
xz
= 0 a matriz de inrcia torna-se numa matriz principal e a expanso da Equao 4
conduz a:
M
T
=
_

_
I
11

P
I
22

Q
I
33

R
_

_
+
_

_
(I
33
I
22
) RQ
(I
11
I
33
) QP
(I
22
I
11
) PQ
_

_
(7)
onde M
T
corresponde ao vector dos momentos aplicados ao Quadrirotor, M
T
= ( M
x
M
y
M
z
)
3.2 Modelao da cinemtica
3.2.1 ngulos de Euler
Com base na cinemtica do problema, e assumindo que a distncia do centro do referencial NED
ao centro de gravidade do Quadrirotor (origem do referencial, R
ABC
) dada por r, ento o vector
posio

r do Quadrirotor referente ao referencial inercial, R
NED
, pode ser escrito como:
21

r = X

i +Y

j +Z

k (8)
o que leva a que a velocidade absoluta seja:

r =

X

i +

Y

j +

Z

k (9)
Sendo assim as componentes da velocidade absoluta em termos do referencial inercial (
.
X
.
Y
.
Z ) podem
ser obtidas atravs das velocidades no referencial local ( U V W ) atravs da forma:
_

Z
_

_
= S
T
E
_

_
U
V
W
_

_
(10)
possvel obter a velocidade angular em termos dos ngulos de Euler, no referencial inercial
R
NED
, com base nas velocidades de rotao ( P Q R) do referencial local R
ABC
. Para isso basta
realizar a seguinte transformao matemtica:
_

_
= T
_

_
P
Q
R
_

_
(11)
Em que T igual a:
T =
_

_
1 tan sin tan cos
0 cos sin
0 sec sin sec cos
_

_
(12)
3.2.2 Quaternies
Para alm do mtodo tradicional de representao da orientao correspondente rotao de um
sistema de coordenadas de uma aeronave com base nos ngulos de Euler, existem outros mtodos que
usam quatro ou mais variveis com o mesmo propsito. O principal objectivo destes sistemas eliminar
as singularidades existentes na representao da rotao em termos de ngulos de Euler associadas s
funes trigonomtricas, bem como incrementar o tempo de processamento computacional durante
a fase dos clculos de navegao. De entre os modelos alternativos existentes e desenvolvidos at
ao momento, o mtodo dos Quaternies, usando quatro variveis, aquele que actualmente mais
utilizado [16].
Para a utilizao deste mtodo so denidas quatro variveis, os quaternies sendo:
_

_
q
0
= cos

2
q
1
= cos sin

2
q
2
= cos sin

2
q
3
= cos sin

2
(13)
22
e a matriz de rotao global S
q
:
S
q
=
_

_
q
2
0
+q
2
1
q
2
2
q
2
3
2(q
1
q
2
+q
0
q
3
) 2(q
1
q
3
q
0
q
2
)
2(q
1
q
2
q
0
q
3
) q
2
0
q
2
1
+q
2
2
q
2
3
2(q
2
q
3
+q
0
q
1
)
2(q
1
q
3
+q
0
q
2
) 2(q
2
q
3
q
0
q
1
) q
2
0
q
2
1
q
2
2
+q
2
3
_

_
(14)
No entanto os parmetros q
i
podem ser representados em termos de ngulos de Euler:
_

_
q
0
= (cos (

/2) cos (

/2) cos (

/2) + sin(

/2) sin(

/2) sin(

/2))
q
1
= (sin(

/2) cos (

/2) cos (

/2) cos (

/2) sin(

/2) sin(

/2))
q
2
= (cos (

/2) sin(

/2) cos (

/2) + sin(

/2) cos (

/2) sin(

/2))
q
3
= (cos (

/2) cos (

/2) sin(

/2) sin(

/2) sin(

/2) cos (

/2))
(15)
em que a escolha do sinal arbitrria mas dever ser consistente.
ainda possvel obter as equaes da cinemtica equivalentes em termos de Quaternies. De
entre um dos vrios mtodos, destaca-se o de Whittaker [16] em que partindo das equaes das
velocidades angulares em termos de quaternies:
_

_
P
Q
R
_

_
=
_

_
q
1
q
0
q
3
q
2
q
2
q
3
q
0
q
1
q
3
q
2
q
1
q
0
_

_
_

_
.
q
0
.
q
1
.
q
2
.
q
3
_

_
(16)
e vericando que a matriz que lhe caracterstica ortogonal e quando invertida as equaes so
bi-lineares em termos de q
i
e velocidades angulares, podemos obter a derivada dos quaternies como
sendo [16]:
_

_
.
q
0
.
q
1
.
q
2
.
q
3
_

_
=
1
2
_

_
0 P Q R
P 0 R Q
Q R 0 P
R Q P 0
_

_
_

_
q
0
q
1
q
2
q
3
_

_
=
1
2

q
_

_
q
0
q
1
q
2
q
3
_

_
(17)
com base nesta ltima equao e na nova matriz de rotao global S
q
, que partindo dos valores
iniciais dos ngulos de Euler, os quaternies so obtidos (Equao 15) e os clculos durante a fase
de navegao so iniciados. As Equaes 18, 19, 20 e 21 constituem assim o modelo completo da
dinmica a ser usado no clculo da trajectria e da atitude de uma aeronave com base no sistema de
coordenadas NED.
_

W
_

_
=
1
m
_

_
F
px
F
py
F
pz
_

_
+g
_

_
2(q
1
q
3
q
0
q
2
)
2(q
2
q
3
+q
0
q
1
)
q
2
0
q
2
1
q
2
2
+q
2
3
_

_
QW RV
RU PW
PV QU
_

_
(18)
23
_

_
.
P
.
Q
.
R
_

_
=
_

_
M
x
M
y
M
z
_

_
1
I

_

_
P
Q
R
_

_
I
_

_
P
Q
R
_

_
(19)
_

_
.
q
0
.
q
1
.
q
2
.
q
3
_

_
=
1
2

q
_

_
q
0
q
1
q
2
q
3
_

_
(20)
_

Z
_

_
= S
T
q
_

_
U
V
W
_

_
(21)
Dado que estas equaes no so lineares, estas tero de ser primeiramente linearizadas e usadas
em espao de estados para se proceder construo do sistema de controlo moderno (Seco 4.1)
para as futuras simulaes.
3.3 Modelao do conjunto motor mais hlice
A nica fora propulsiva existente no Quadrirotor gerada pela rotao das hlices. por isso
de esperar que a escolha do conjunto motor mais hlice seja de extrema importncia. De uma forma
geral, a ecincia das ps est associada ao motor que a propulsiona, e a ecincia desse conjunto est
associada estrutura que se pretende operar [18]. Isto revela a diculdade em projectar um conjunto
constitudo por um motor mais uma hlice ideal para este tipo de UAV, dada a sua importncia
acrescida relativamente atribuio dos graus de liberdade do mesmo. Partindo deste pressuposto,
para a modelao deste conjunto foram assumidos modelos de referncia mais ou menos comuns para
um UAV com estas caractersticas. Numa situao deste tipo, estamos interessados em calcular a
fora e os momentos gerados por um dado conjunto motor mais hlice atravs da tenso aplicada a
cada um dos motores e poder relacion-la se possvel com a rotao envolvida no conjunto. Como
tal foi assumido o andamento caracterstico de um motor em termos de rotao sofrida por tenso
aplicada. Este andamento engloba uma zona morta em que a velocidade de rotao nula, e uma
rotao limite (saturao) que no permite que a velocidade exceda a rotao mxima, Figura 16.
Figura 16: Funcionamento genrico de um motor elctrico de corrente contnua
24
Um problema que ocorre com os motores elctricos que apesar de serem idnticos, no se
comportam exactamente da mesma forma. Isto faz com que se tivesse de realizar ensaios experimentais
a cada um dos quatro conjuntos motor mais hlice, e atravs por exemplo de um sistema massa mola
obter uma equao que permitisse relacionar a tenso aplicada com a fora gerada. Esta situao
acabou por no se realizar em relao ao conjunto utilizado no ALIV, e como tal, teve de se criar
um modelo matemtico, que apesar das suas limitaes, pudesse de certa forma modelar com algum
realismo o conjunto.
Para isso foi assumido o motor, AXI 2212-12 (Figura 16). Este motor comum para utilizaes
semelhantes s pretendidas e os valores padro so apresentados na Tabela 2:
RPM/V 2000
Gama entrada [V] 9-12
RPM mximo 6000
Thrust [g] 350
P
out
[W] 28
P
in
[W] 56
Tabela 2: Caractersticas do motor AXI 2212-12
Com base nos dados da Tabela 2 e na curva avanada na Figura 16, foi possvel caracterizar a
gama de funcionamento do motor e a relao existente entre a tenso aplicada e a rotao gerada,
com a constante do motor:
2000rpm =
2000 2
60
rad/s = K
v
_
_
_

m
= 0 |V | V
0

m
= K
v
(V V
0
) = 2000 (V 9) =
20002
60
(V 9) |V | > V
0
(22)
O funcionamento do motor foi simulado usando o SIMULINK e os blocos existentes para o efeito
so apresentados na Figura 17.
Figura 17: Actuador
O actuador foi construdo com base na tenso aplicada ao motor tendo o mesmo uma constante
de tempo de sistema de primeira ordem de t
s
= 20 ms e uma sensibilidade K
v
.
Para a modelao da hlice foi utilizada, segundo o mesmo princpio do motor, a hlice 9x4,5
APCE, Figura 18. possvel modelar uma hlice atravs de trs dos seus parmetros caractersticos,
C
t
, C
p
, e r [18]. Os primeiros dois so o coeciente de impulso (thrust) e de potncia (power )
e so obtidos experimentalmente podendo ser consultados com base em tabelas caractersticas da
hlice consoante o valor das velocidades envolvidas, e o ltimo o comprimento da p da hlice. Os
valores caractersticos desta hlice podem ser consultados na Tabela 3, onde C
t
e C
p
equivalem aos
25
valores experimentais correspondentes a gamas muito baixas, dadas as velocidades associadas aos
movimentos do ALIV.
Raio da hlice [m] 0.1229
Coeciente de impulso (thrust) C
t
0.1047
Coeciente de potncia (power ) C
p
0.0374
Tabela 3: Caractersticas da hlice 9 4, 5APCE
Com base nesta informao, possvel construir um modelo matemtico caracterstico deste
conjunto atravs das seguintes expresses [18]:
F
p
=
4r
4
C
t

2

2
m
(23)
M
T
=
P

m
=
4r
5
C
p

3

2
m
(24)
Figura 18: Hlice 9x4,5 APCE
Daqui se retira que tanto a fora como os momentos gerados variam apenas com a rotao
aplicada ao conjunto, podendo as equaes anteriores serem escritas como:
F
p
= K
f

2
m
, K
f
=
4r
4
C
t

2
=
4 1.2 (0.2458/2)
4
0.1047

2
= 1.1617 10
5
kg m/rad
2
(25)
M
T
= K
m

2
m
, K
m
=
4r
5
C
p

3
=
4 1.2 (0.2458/2)
5
0.0374

3
= 1.6234 10
7
kg m
2
/rad
2
(26)
K
K
=
r C
p
C
t
= 0.0140 m, M
p
= K
K
F
p
(27)
onde K
f
, K
m
e K
K
so constantes caractersticas da modelao para a hlice e o motor adoptados.
3.4 Peso e momentos de inrcia
Um aspecto fulcral na performance de um Quadrirotor, bem como de qualquer UAV, reside no
peso da estrutura e do conjunto de equipamento embarcado. Um Quadrirotor caracteriza-se por
transportar a maior parte do seu equipamento numa seco central o mais perto possvel do centro
de gravidade desejado. necessrio por isso, que o equipamento seja no s leve como de pequenas
dimenses. Isto faz com que o preo desse equipamento aumente. O espao tambm no ilimitado,
26
e por isso a relao entre equipamento mais relevante e custos envolvidos algo inevitvel. Se por um
lado queremos mais sensores para ter maior instrumentao, por outro, devido ao espao, os mesmos
tm de ser mais pequenos e por isso mais caros. Este o compromisso assumido num projecto deste
tipo. Para a elaborao deste trabalho foi assumido o peso total do ALIV. Este peso engloba todo
o material envolvido na descrio do equipamento a bordo do ALIV (Seco 2.2.2), resultando num
peso total de 1.2 kg como demonstrado com base nos pesos apresentados na Tabela 4.
Bateria de Ltio 160
Circuito Receptor de Comunicaes 50
4 Servos GWS Pico 40
4 Motores DC GWS 350 260
Bateria Auxiliar 90
Cabos Elctricos 50
4 Rolamentos 40
Interruptores ON/OFF 20
Estrutura de Alumnio 490
Total [g] 1200
Tabela 4: Peso dos componentes a bordo do ALIV
Em relao aos momentos de inrcia, foram desprezados os efeitos causados no s pelo equipa-
mento como pela prpria estrutura em si, e a matriz inercial foi considerada diagonal principal. Este
tipo de aproximaes uma tcnica comum em qualquer tipo de abordagem referenciada neste tra-
balho, dada a estrutura particular em forma de cruz de um Quadrirotor. Como tal foram assumidos
para o clculo dos momentos de inrcia, os motores e a sua localizao dentro da estrutura. Foi
denido:
m
m
- Massa do motor
x
m
- Comprimento do motor ao longo do eixo x
y
m
- Comprimento do motor ao longo do eixo y
z
m
- Comprimento do motor ao longo do eixo z
l
l
- Distncia do centro de gravidade ao centro dos motores 1 e 2
l
c
- Distncia do centro de gravidade ao centro dos motores 3 e 4
Para o momento de inrcia I
xx
obtm-se:
_

_
I
x
1
= I
x
2
=
1
12
m
m
_
y
2
m
+z
2
m
_
I
x
1
=
1
12
0.060
_
15 10
3
_
2
+
_
32 10
3
_
2

=6.245 10
6
kg m
2
I
x
3
= I
x
4
=
1
12
m
m
_
y
2
m
+z
2
m
_
+m
m
l
2
c
I
x
3
=
1
12
0.060
_
15 10
3
_
2
+
_
32 10
3
_
2

+0.060 0.64
2
= 0.0246 kg m
2
(28)
I
xx
= 2 I
x
1
+ 2 I
x
3
=2
_
6.245 10
6
_
+ 2 0.0246 =0.0492 kg m
2
(29)
27
Para o momento de inrcia I
yy
obtm-se:
_

_
I
y
1
= I
y
2
=
1
12
m
m
_
x
2
m
+z
2
m
_
+m
m
l
2
l
I
y
1
=
1
12
0.060
__
15 10
3
_
2
+
_
32 10
3
_
2
_
+0.060 0.60
2
= 0.0216 kg m
2
I
y
3
= I
y
4
=
1
12
m
m

x
2
m
+z
2
m

I
y
3
=
1
12
0.060
_
15 10
3
_
2
+
_
32 10
3
_
2

= 6.245 10
6
kg m
2
(30)
I
yy
= 2 I
y
1
+ 2 I
y
3
= 2 0.0216 + 2
_
6.245 10
6
_
= 0.0432 kg m
2
(31)
Para o momento de inrcia I
zz
obtm-se:
_

_
I
z
1
= I
z
2
=
1
12
m
m
_
x
2
m
+z
2
m
_
+m
m
l
2
l
I
z
1
=
1
12
0.060
_
15 10
3
_
2
+
_
32 10
3
_
2

+0.060 0.64
2
= 0.0216 kg m
2
I
z
3
= I
z
4
=
1
12
m
m
_
x
2
m
+z
2
m
_
+m
m
l
2
c
I
z
3
=
1
12
0.060
_
15 10
3
_
2
+
_
32 10
3
_
2

+0.060 0.60
2
=0.0246 kg m
2
(32)
I
zz
= 2 I
z
1
+ 2 I
z
3
= 2 0.0216 + 2 0.0246 = 0.0924 kg m
2
(33)
O que resulta na matriz inercial:
I =

0.0492 0 0
0 0.0432 0
0 0 0.0924

kg m
2
(34)
3.5 Sensores
Com base no que tem vindo a ser descrito, torna-se evidente que para o aumento do realismo
da simulao e uma futura implementao para testes em ambientes reais, de extrema importncia
fazer uso do equipamento a bordo do ALIV. O ALIV tem a bordo um conjunto de trs acelermetros
para medio da acelerao global num dado ponto e um magnetmetro (Seco 2.2.2) para o clculo
da atitude, onde atravs dos quais, possvel calcular a estimativa dos estados envolvidos no controlo
da atitude. Para que estes possam ser simulados com realismo, necessrio realizar uma modelao
matemtica dos mesmos, e com isso observar quantos estados estamos em condies de recuperar.
3.5.1 Acelermetros
Um acelermetro um sensor que mede a acelerao absoluta no seu referencial. Essa acelerao
medida no ponto em que o acelermetro se encontra, por isso de modo a aumentar a sua eccia
de extrema importncia aproxim-lo o mais possvel do centro de gravidade do corpo. No caso do
ALIV, o acelermetro encontra-se 5 mm para a frente e 30 mm para cima do seu centro de gravidade.
Um acelermetro num determinado ponto P mede a componente de acelerao a

p
dada por [16]:
28
a

p
= a
p
g (35)
onde a
p
e g so respectivamente a acelerao absoluta e a fora gravtica no ponto P. Assumindo que
r, o vector que liga o ponto de medio do acelermetro ao centro de gravidade e que o Quadrirotor
tem velocidade angular, , em relao ao referencial inercial R
NED
, ento a acelerao absoluta para
um dado ponto P pode ser escrita como
3
[16]:
a
p
=
F
ABC
+S
E
mg
m
+
.

ABC
r +
ABC
(
ABC
r) (36)
Esta equao aplicada na modelao dos trs acelermetros, um para cada eixo sendo a
p
=[a
px
a
py
a
pz]
faltando adicionar o rudo e os parmetros de ordem elctrica que limitam a gama de trabalho do
acelermetro. Neste caso foram usados os parmetros denidos no datasheet do acelermetro do
ALIV, cedido pelo Eng. Severino Raposo (Tabela 5).
Gama de leitura
+
/2g
Nmero de bits 10
Resoluo 0.0383g
Tabela 5: Limitaes dos Acelermetros utilizados
A resoluo foi calculada com base na seguinte denio:
y =
y
max
y
min
2
N
(37)
em que y
max
corresponde gama de leitura mxima e N ao nmero de bits.
3.5.2 Magnetmetro
Os magnetmetros so aparelhos que em termos aeronuticos so teis no controlo da atitude das
aeronaves. Geralmente so dispositivos que medem a magnitude e direco do campo magntico,
esttico ou alternado, baseando-se nas propriedades de saturao de ligas metlicas. Devido sua
complexidade, optou-se por fazer a implementao de um modelo matemtico mais simples de modo
a puder dar continuidade ao problema.
Assim, foi admitido que o magnetmetro funciona com base na direco e sentido indicados por
uma agulha inicialmente calibrada a apontar para o norte magntico, magnetmetro tri-axial. O
magnetmetro tem assim como sadas a orientao instantnea da agulha em termos do referencial
inercial consoante a atitude do Quadrirotor. Este princpio pode ser modelado atravs da seguinte
equao matemtica:
_

_
N
mag
E
mag
D
mag
_

_
= S
E
_

_
1
0
0
_

_
(38)
em que o vector (N
mag
, E
mag
, D
mag) corresponde projeco do vector unitrio inicialmente a
apontar para norte, e S matriz de rotao global denida na Equao 2.
3
Por motivo de interesse e de espao, a deduo da Equao 36 foi deixada para consulta no Apndice B
29
3.5.3 Cmara embarcada
De todos os sensores modelados, este o nico que no est actualmente presente no ALIV. Foi
concebido de forma a permitir um controlo da posio com base na visualizao de determinados
pontos de referncia atravs de uma cmara embarcada. Isto permite, construir simulaes mais
abrangentes, tornando o ALIV mais autnomo e verstil.
Para a modelao da cmara embarcada foram considerados trs referenciais:
Referencial NED (North-East-Down), R
NED
= [ e
x
e
y
e
z
]
Referencial ABC (Aircraft Body Centered), R
ABC
= [ e
1
e
2
e
3
]
Referencial da imagem xyz, R
xyz
= [ x y z ]
Com base na posio dos pontos de referncia no referencial NED, e da posio do Quadrirotor em
relao ao mesmo, atravs das caractersticas da cmara modelada possvel projectar os pontos de
referncia no referencial da imagem, R
xyz
. Para isso, para alm da utilizao da matriz S que dene a
rotao entre o referencial inercial, R
NED
, e o mvel, R
ABC
, necessrio uma nova matriz S
cam
que
dene a passagem entre este ltimo e o referencial de imagem, R
xyz
. A matriz S neste caso igual
matriz S
E
apresentada na Seco 3.1 e demonstrada no Apndice A. Quanto matriz S
cam
baseada
no mesmo princpio, e como tal tambm igual matriz S, so que desta vez, ( ) correspon-
dem orientao da cmara em relao ao referencial mvel, isto , (
cam

cam

cam
). Assim,
a matriz total de passagem do referencial inercial, R
NED
, para o da imagem, R
xyz
, pode ser dada por:
S
T
= S
cam
S (39)
em que a nova atitude pode ser retirada da matriz S
T
atravs das seguintes equaes:
_

T
= atan2 (S
T
(2, 3) , S
T
(3, 3))

T
= arcsin
_
S
T
(1,3)
det(S
T
)
_
= arcsin(S
T
(1, 3))

T
= atan2 (S
T
(1, 2) , S
T
(1, 1))
(40)
e a posio da cmara em relao ao referencial inercial, R
NED
, atravs da matriz S como sendo:
_

_
P
x
P
y
P
z
_

_
=
_

_
X
NED
Y
NED
Z
NED
_

_
+S
T
_

_
X
cam
Y
cam
Z
cam
_

_
(41)
em que ( X
NED
Y
NED
Z
NED
) igual ao vector com a posio actual do Quadrirotor no refer-
encial inercial, R
NED
, e ( X
cam
Y
cam
Z
cam
) a posio da cmara em relao ao referencial mvel,
R
ABC
. Partindo do conjunto da nova posio, Equao 41 e da atitude, Equao 40, juntamente
com as coordenadas de um qualquer ponto de referncia no referencial xo e do comprimento focal
f que caracteriza a cmara, possvel obter a sua projeco no referencial da imagem, R
xyz
atravs
da matriz M denida pela multiplicao das matrizes que caracterizam a rotao, S, e translao,
T, dos pontos de referncia.
4
4
Por motivos de interesse e de espao, a deduo completa das equaes referentes modelao da cmara foi
deixada para consulta ao longo do Apndice B
30
_
Q
T
P
1
_
=
_
Q
T
1
_
_
S
T
T
0
T
T
1
_
=
_
Q 1
_
M (42)
em que Q corresponde matriz com as coordenadas dos pontos de referncia no referencial inercial
R
NED
, e Q
P
matriz com a projeco desses mesmos pontos no referencial da imagem, R
xyz
.
Q
P
=
_

_
x
y
z
_

_
= SQT (43)
em que juntando a projeco ca:
_

_
x
i
y
i
1
_

_
=
_

_
x
/z
y
/z
1
_

_
(44)
Como foi dito anteriormente, a projeco dos pontos vem no referencial da imagem, R
xyz
, em ter-
mos de ( x, y ), pelo que, de modo a simular mais correctamente a utilizao de uma cmara verdica,
procedeu-se pixelizao para uma resoluo tipicamente VGA de h =640 por w =380 pixeis. Aos
valores de sada em termos de ( x, y ) correspondentes matriz Q
P
calculada, foram aplicadas as
seguintes equaes:
_
_
_
x
p
=
1
w
round(x
i
w)
y
p
=
1
h
round(y
i
h)
(45)
31
4 Controlador
O objectivo deste trabalho conseguir estabilizar o Quadrirotor numa dada posio de referncia.
Essa posio de referncia consiste numa primeira fase, numa situao de voo pairado a uma dada
altitude. Para isso necessrio considerar um sistema de controlo de modo a permitir que as instabil-
idades do movimento sejam compensadas. O controlador escolhido, foi o controlador moderno LQR
(Linear Quadratic Regulator ). Este controlador dos mais ecientes e mais usados em tcnicas de con-
trolo, particularmente devido sua fcil implementao e robustez nas tcnicas de controlo MIMO
(Multi Input Multi Output) como o caso deste sistema. Para aplicao do LQR necessrio que as
equaes do movimento do Quadrirotor se apresentem sob a forma de espao de estados linear, isto :
_
_
_
.
x = Ax +Bu
y = Cx +Du
(46)
No entanto para que tal seja possvel, necessrio que as equaes sejam lineares ou linearizadas.
4.1 Linearizao das equaes
possvel observar que as equaes obtidas na Seco 3.2.2 referentes modelao da dinmica
do sistema, deram origem a equaes no lineares. Como foi referido anteriormente, para aplicao
do sistema de controlo adoptado, as equaes tm de ser lineares. Isto quer dizer que se tem de
proceder linearizao das equaes anteriormente obtidas em torno de uma posio de referncia.
O estado considerado para a linearizao do Quadrirotor inclui as velocidades linear e angular no
referencial ABC, R
ABC
,
_
U V W
_
T
e
_
P Q R
_
T
respectivamente, a posio no referencial
xo NED, R
NED
,
_
e
x
e
y
e
z
_
T
e os ngulos de Euler
_

_
T
, resultando num estado de
12 variveis. A entrada a tenso fornecida aos quatro motores, U =
_
U
1
U
2
U
3
U
4
_
T
.
Para uma situao de voo pairado a uma altitude h, considerando os ngulos de Euler para
controlo, temos como referncia:
X
0
= [U V W P Q R e
x
e
y
e
z
] = [0 0 0 0 0 0 0 0 h 0 0 0] (47)
E usando a expanso em termos de sries de Taylor de primeira ordem, temos aproximadamente
que a derivada em ordem ao tempo das perturbaes dos estados igual a:

x = f (X
0
, U
0
) +
f
X
|
X=X
0
x +
f
U
|
U=U
0
u (48)
Considerando que o estado X denido pela soma de uma perturbao x com a sua posio de
referncia X
0
, X = x +X
0
e da mesma forma U = u +U
0
, a expresso anterior ca:

x = f (X
0
, U
0
) +
f
X
(X X
0
) +
f
U
(U U
0
) (49)
Resolvendo agora f (X
0
, U
0
) para cada uma das equaes do movimento, em que U
0
tal que o
somatrio exercido em cada um dos rotores leva a
.
W = 0 (posio de referncia a uma dada altitude
com acelerao vertical nula), obtm-se uma situao de equilibrio:
32
f (X
0
, U
0
) = 0 (50)
isto quer dizer que da Equao 48 inicial resulta apenas:

x =
f
X
(X X
0
) +
f
U
(U U
0
) (51)
Por comparao com a Equao 46 deduz-se a matriz da dinmica:
A =
f
X
=
_

_
f
1
X
1
f
1
X
2
. . .
f
1
X
12
f
2
X
1
f
2
X
2

.
.
.
.
.
.
.
.
.
f
12
X
1
.
.
.
f
12
X
12
_

_
X=X
0
;U=U
0
(52)
isto quer dizer que a matriz A corresponde matriz Jacobiana em que cada uma das funes f
corresponde a uma equao do movimento (equaes de estado) que podem ser consultadas no
Apndice A e X o vector dos estados do Quadrirotor, assumindo os ngulos de Euler para controlo
ao invs dos quaternies. Analogamente, a matriz de entrada B corresponde matriz Jacobiana mas
em termos dos parmetros de entrada U correspondentes a cada um dos motores, U = [U
1
U
2
U
3
U
4
]:
B =
f
U
=
_

_
f
1
U
1
f
1
U
2
. . .
f
1
U
4
f
2
U
1
f
2
U
2
. . . . . .
.
.
.
.
.
.
.
.
.
f
12
U
1
.
.
.
f
12
U
4
_

_
X=X
0
;U=U
0
(53)
Para o clculo da matriz B, assumido que as foras segundo x e y so nulas, F
x
F
y
0
quando comparadas com a fora segundo z criada pela totalidade dos rotores, e considerado que
todos eles se comportam de igual maneira e que por isso todos contribuem na mesma proporo para
a fora propulsiva:
F
z
=
4

i=1
F
i
= F
1
+F
2
+F
3
+F
4
(54)
Em relao aos momentos temos que:
_

_
M
x
= (F
3
F
4
) L
M
y
= (F
2
F
1
) L
M
z
= (F
1
+F
2
F
3
F
4
) K
K
(55)
em que L corresponde distncia entre o CG (assumido como sendo exactamente igual ao ponto
central da estrutura) e o eixo de rotao de cada um dos rotores, K
K
a constante caracterstica da
modelao denida na Seco 3.3, que relaciona o momento segundo e
3
com as foras aplicadas.
33
4.2 LQR (Regulador Quadrtico Linear)
Nesta seco ser dado a conhecer de uma forma breve os princpios tericos do controlador adop-
tado, LQR. O LQR consiste na minimizao de uma funo de custo J que pode assumir variadssimas
formas, correspondendo a situaes e objectivos diferentes, cuja optimizao em conjuno com a
restrio do sistema (expressa pelas equaes da dinmica), conduz a uma soluo u
0
que minimiza
o valor de J. No caso usual de uma referncia nula e de um intervalo de tempo semi-innito, corre-
spondente ao problema do regulador quadrtico linear, onde no caso estacionrio a funo de custo
tem matrizes de ponderao (matrizes Q e R) constantes:
J =
1
2

0
(x
T
Qx +u
T
Ru)dt (56)
e conduz a uma soluo ptima linear da forma:
u
0
= Kx (57)
A minimizao da funo de custo com a restrio da dinmica do sistema resulta na resoluo
de uma equao algbrica de Riccati (no caso estacionrio):
A
T
P +PAPBR
1
B
T
P +Q = 0 (58)
em ordem a uma matriz P constante, simtrica, semidenida e positiva, a partir da qual obtida a
matriz de realimentao [17]:
K = R
1
B
T
P (59)
A resoluo do problema do regulador quadrtico linear pode ser obtida atravs do comando lqrd
do MATLAB dadas as quatro matrizes: A, B, Q, R e o perodo de amostragem t
s
. A matriz de
realimentao K obtida ento colocada no anel de realimentao de forma a fechar o anel e a
estabilizar o sistema. A matriz Q e a matriz R so denominadas matrizes de ponderao dos estados
e entradas respectivamente, sendo os seus valores arbitrrios (Q 0; R > 0), inuenciando no s
a estabilidade como a rapidez do sistema. O mtodo de Bryson sugere uma denio para Q e R,
colocando-as como matrizes diagonais onde cada um dos termos o quadrado do inverso do mximo
esperado para cada varivel durante a manobra [17]:
Q
ii
=
1
x
2
i,max
R
ii
=
1
u
2
i,max
(60)
4.3 Estimador
Num controlo realstico nem sempre os estados no esto todos disponveis para ser realizada a
realimentao. Em vez disso, so as sadas provenientes dos sensores que so acessveis. Usando
controlo moderno, possvel consoante o nmero e o tipo de medies retiradas da dinmica do
sistema, estimar os estados parciais ou totais do problema. funo do estimador saber calcular o
valor dos estados atravs de informao parcial recebida dos sensores. Aps a estimao dos estados,
34
so esses mesmos estados estimados que so usados na realimentao do sistema.
Para o projecto de um Quadrirotor estamos interessados em controlar doze estados. Trs ve-
locidades lineares, trs velocidades angulares, a posio e a atitude do UAV em termos de angulos
de Euler, com base na rotao de cada um dos quatro rotores envolvidos. S assim conseguiremos
uma capacidade de voo autnomo por parte de um Quadrirotor. O ALIV s tem disponveis os trs
acelermetros e o magnetmetro, e no possvel obter uma estimativa dos doze estados pretendidos,
em vez disso, o estimador apenas consegue observar seis dos mesmos estados, as trs velocidades an-
gulares, (P Q R) , e a atitude do ALIV, ( ) . Desta forma, impossvel conseguir automatizar
o ALIV porque a translao no observvel.
Uma vez que se faz uso da tcnica de controlo moderno LQR (Seco 4.2) em que a realimentao
feita usando todos os estados que caracterizam o sistema, necessrio que o estimador construdo
saiba estimar a totalidade dos estados para uma futura realimentao. Esta tcnica denomina-se por
LQG, linear quadratic gaussian [16].
O ltro de Kalman um estimador usado para navegao ou outras aplicaes que requerem a
reconstruo dos estados atravs de medies do rudo, baseado num tratamento probabilstico do
processo e medio do rudo [16]. Com base na teoria amplamente explicada na referncia adoptada
como base, e partindo desta vez de uma dinmica estocstica de um sistema em espao de estados
caracterizada por:
_
_
_
.
x = Ax +Bu +Gw
y = Cx +Du +v
(61)
em que w(t) um processo de rudo desconhecido que actua no sentido de perturbar a aeronave,
como por exemplo o vento, e v(t) uma medio de rudo desconhecida geralmente associada ao rudo
dos sensores. Uma vez que a dinmica do sistema (Equao 61) constituda por a caracterizao
de rudos associados aeronave, o estado x(t) agora um estado probabilstico, assim como o so as
sadas y(t), os seus valores so obtidos assim, partindo de valores expectveis para as variveis w(t)
e v(t) [16]. Assumindo que w(t) e v(t) so processo de rudo branco, caracterizados em termos das
matrizes espectrais Q
K
= E
_
ww
T
_
e R
K
= E
_
vv
T
_
, cando assim estabelecidos os pressupostos
para a construo de um estimador na forma da Equao 34 para o sistema estocstico em espao
de estados referido na Equao 61, em que agora a estimativa da sada dada por:
y = E {Cx +v} = C x (62)
Utilizando a dinmica do sistema da Equao 61, a derivada do erro da dinmica dada por:
.
x = (ALC) x +Gw Lv (63)
em que o erro da sada denido como y = y y igual a:
y = C x +v (64)
e o erro da covarincia dado por:
P(t) = E
_
x x
T
_
(65)
35
O ltro de Kalman procura assim calcular a matriz de ganho ptimo L que minimize o erro da
covarincia P. O processo de obteno da matriz L para o caso do ltro de Kalman semelhante ao
problema demonstrado na teoria referente ao controlo moderno LQR (Seco 4.2) na Equao 57.
Pelo que, e mais uma vez por o MATLAB a calcular semelhana da matriz de realimentao do
ganho K, a teoria envolvida no vai ser novamente ilustrada, podendo ser consultada na referncia
adoptada [16].
O ltro de Kalman assim obtido com base no comando kalmd do MATLAB, que tem como
parmetros de entrada o sistema em espao de estados a ser observado, as matrizes espectrais Q
K
e
R
K
e o perodo de amostragem t
s
. O mesmo comando devolve um sistema em espao de estados,
KEST, constitudo pelo ltro de Kalman e que tem como sadas no s a estimativa dos estados e
das prprias sadas.
36
5 Implementao em SIMULINK
Como foi explicitado na Seco 4.3, de modo a tornar a simulao mais realstica, recorreu-se a
uma realimentao da estimativa dos estados obtidos atravs do ltro de Kalman com base nas sadas
provenientes dos sensores a bordo do ALIV. Sendo assim, toda a losoa adoptada na construo dos
diferentes mdulos de simulao baseou-se no mesmo princpio, como ilustra a Figura 19.
Figura 19: Filosofa dos mdulos durante a fase de simulao
Em termos gerais existem trs blocos principais, o primeiro bloco representa a dinmica de um
Quadrirotor convencional e constitudo pelas treze equaes deduzidas na Seco 3.2.2. A estas
treze equaes esto associadas treze variveis, que aps integrao e passagem de quaternies a
Euler constituem os doze estados de um Quadrirotor convencional para o controlo. O segundo bloco
representa os sensores embarcados, neste bloco simulado o comportamento dos sensores com base
nas equaes deduzidas na Seco 3.5, obtendo-se como sadas as leituras efectuadas pelos sensores
consoante o mdulo a ser simulado. Em ambos os blocos so recebidos como variveis de entrada
os estados actuais do Quadrirotor e a rotao de cada um dos rotores para que os clculos possam
ser efectuados consecutivamente. O terceiro bloco principal consiste na modelao do controlador de
acordo com o mdulo a ser simulado, Seco 4.3. Este bloco responsvel pela criao do ltro de
Kalman e pela realimentao dos estados estimados, para obteno do factor correctivo ideal a aplicar
a cada um dos rotores. Por m existe ainda um bloco que representa os actuadores. Neste bloco,
como foi explicado na Seco 3.3, so aplicados os parmetros do modelo dos motores adoptado,
de modo a simular de uma forma mais realstica o funcionamento dos mesmos. Todos estes blocos
so iniciados atravs de um cheiro
5
responsvel pela construo dos valores iniciais dos estados, das
matrizes e dos ganhos envolvidos de acordo com a simulao e a situao pretendida.
Uma vez que o ALIV no possui qualquer tipo de sensor para o controlo da posio, foram
assumidas duas alternativas de modo a poder controlar integralmente os dozes estados do ALIV.
A primeira das situaes consistiu em utilizar apenas um joystick para o controlo da posio e os
sensores a bordo para o controlo da atitude: mdulo com joystick (Seco 5.1.1).
A segunda hiptese consistiu na modelao de uma cmara a bordo do ALIV, de modo a torn-lo
completamente autnomo atravs da visualizao e dos sensores anteriormente considerados: mdulo
de visualizao (Seco 5.1.2).
5
O Apndice C apresenta entre outros, o cheiro inicio.m, responsvel pela criao dos parmetros iniciais envolvidos
no comeo da simulao, e das matrizes de ganho ideais para o tipo de simulao pretendida.
37
Para nalizar foi ainda criado um mdulo que engloba os dois anteriores: mdulo integrado
(Seco 5.1.3). Neste mdulo conduzido o Quadrirotor at uma posio em que possa observar os
pontos de referncia ou alvo, atravs do mdulo com joystick, e a partir da passe a voo autnomo
atravs do mdulo de visualizao.
5.1 Mdulos nais
5.1.1 Mdulo com Joystick
Como j foi referido anteriormente na Seco 3.5, o ALIV possui actualmente dois tipos de sensores,
trs acelermetros que medem a acelerao global num dado ponto, e um magnetmetro responsvel
primordialmente pelo controlo da guinada. Com base na modelao matemtica desenvolvida na
Seco 3.5 atravs destes dois conjuntos de sensores temos seis variveis de sada. Trs das variveis
representam a acelerao total num dado ponto para cada um dos eixos e
1
, e
2
, e
3
respectivamente, e
as outras trs variveis a componente do vector unitrio de acordo com a atitude do ALIV inicialmente
calibrado a apontar para norte, y =(a
px
, a
py
, a
pz
, N
mag
, E
mag
, D
mag).
Atravs nica e exclusivamente destas sadas vericou-se que a translao no observvel, isto
, o ltro de Kalman no consegue observar os doze estados para o controlo integral do Quadrirotor.
Uma vez no ter qualquer informao que possa ser restituda para estimar a posio, as velocidades
lineares do mesmo tambm no so conseguidas. Como tal, dos dozes estados iniciais, com base nos
sensores a bordo do ALIV o ltro de Kalman apenas consegue estimar seis deles, as trs velocidades
angulares e os ngulos de Euler, x=(p, q, r, , , ). Assim surgiu como hiptese a criao de um
mdulo que controlasse a posio atravs do joystick de modo a tornar o mesmo mais verstil. A ideia
foi atribuir a cada um dos graus de liberdade do joystick, um dos graus de liberdade do Quadrirotor,
controlados atravs da atribuio das aceleraes lineares e da acelerao angular segundo e
3
.
Para fazer variar a posio do Quadrirotor, de acordo com a Seco 2.1, necessrio fazer variar
a rotao entre cada par de rotores. Assim, possvel fazer o Quadrirotor deslocar-se ao longo
dos trs eixos inerciais pela introduo de velocidades lineares. Devido s grandes instabilidades
inerentes ao movimento e estabilizao de um Quadrirotor convencional, ao movimento do joystick
no basta estarem associadas as velocidades lineares, necessrio que essas mesmas velocidades
conduzam estabilizao da atitude do Quadrirotor de modo a torn-lo controlvel. Sendo assim,
estamos interessados em obter uma matriz, que para uma dada velocidade linear introduzida atravs
do joystick, no s desloque o Quadrirotor sobre uma determinada direco, como ao mesmo tempo
o faa posteriormente estabilizar.
Para o clculo dessa matriz foi considerado mais uma vez o sistema em espao de estados original:
_
_
_
.
x = Ax +Bu
y = Cx +Du
(66)
em que a matriz da dinmica A tem dimenses [6 6] correspondente aos seis estados x = (p, q, r,
, ), e a matriz de entrada B [6 4] correspondente s quatro entradas dos motors u=(u
1
, u
2
, u
3
, u
4
).
Considerando a realimentao deste sistema temos que a matriz em anel fechado correspondente
dada por:
38
A
f
= (ABK) (67)
o que conduz equao:
.
x = (ABK) x +Bu (68)
Como estamos interessados que a uma perturbao inicial causada pela variao da rotao dos
motores, o ALIV reaja com uma deslocao transversal e a consequente estabilizao, estamos inter-
essados em resolver:
0 = (ABK) x +Bu (ABK) x = Bu x = (ABK)
1
(Bu) = A
1
f
Bu (69)
De modo a conseguirmos relacionar as sadas do joystick em termos de aceleraes lineares e acel-
erao angular segundo e
3
, y
j
=

.
U,
.
V ,
.
W,
.
R

, com as entradas aos motores u=(u

1
, u
2
, u
3
, u
4
),
pretendemos resolver o seguinte sistema:
y
j
=
_

_
.
U
.
V
.
W
.
R
_

_
= C
j
x +D
j
u (70)
em que C
j
uma matriz com dimenses [4 6] e D
j
uma matriz com dimenses [4 4]. Ambas
obtidas mais uma vez com base no Jacobiano, desta vez entre as equaes provenientes dos sensores
com os estados x=(p, q, r, , , ), caso da matriz C
j
, e entre as equaes provenientes dos sensores
com as entradas u=(u
1
, u
2
, u
3
, u
4
), caso da matriz D
j
. No fundo estas matrizes so obtidas com
base no conjunto de linhas e colunas pretendidas das matrizes originais A e B do sistema da Equao
66 para se obter as sadas pretendidas y
j
=

.
U,
.
V ,
.
W,
.
R

.
No entanto, como para este caso as sadas provenientes do joystick de acordo com os movimentos
pretendidos so as sadas denidas na Equao 70, de acordo com a Equao 69 estamos interessados
em resolver o seguinte sistema:
_

_
.
U
.
V
.
W
.
R
_

_
= C
j
(ABK)
1
Bu +D
j
u =
_
C
j
A
1
f
B +D
j
_
u (71)
_

_
u
1
u
2
u
3
u
4
_

_
=
_
C
j
A
1
f
B +D
j
_
1
_

_
.
U
.
V
.
W
.
R
_

_
= J
_

_
.
U
.
V
.
W
.
R
_

_
(72)
em que a matriz J a matriz que relaciona o movimento do joystick com a deslocao transver-
sal do Quadrirotor em termos dos parnetris de entrada aos motores u=(u
1
, u
2
, u
3
, u
4
), e y
j
=

.
U,
.
V ,
.
W,
.
R

as sadas provenientes do joystick. Os valores da Equao 72 sero ento adiciona-

39
dos aos valores provenientes da realimentao dos seis estados. Isto faz com que sejam atribudos ao
ALIV velocidades lineares que permitam a sua deslocao transversal e a consequente estabilizao.
Com base nisto, a Figura 20 demonstra o raciocnio utilizado na construo deste mdulo durante a
fase de simulao.
Figura 20: Ilustrao do mdulo com joystick durante a fase de simulao
5.1.2 Mdulo de visualizao
Um dos problemas no controlo de um Quadrirotor conseguir a sua estabilizao, e o principal
objectivo devido s suas diculdades em termos de pilotagem, que o mesmo consiga ser o mais
autnomo possvel. Foi com base nestes dois pressupostos que a utilizao do modelo desenvolvido
para a cmara embarcada (Seco 3.5.3) revelou ser de extrema importncia.
Na soluo desenvolvida anteriormente, devido inexistncia de um sensor que conseguisse con-
trolar a posio, a mesma foi conseguida atravs de um joystick. Embora a simulao resultasse,
no o mais ideal para os objectivos a que se prope um Quadrirotor. Ao Quadrirotor desenvolvido
nestes moldes exigido uma posio de permanncia a dada altitude, quer seja para a observao
de um elemento estacionrio quer seja para um objecto com movimento inferior s capacidades de
movimentao do Quadrirotor. Se tivermos um controlo da posio, esse objectivo conseguido
sem que o utilizador tenha de fazer um constante ajuste atravs do joystick, tornando o Quadrirotor
totalmente autnomo e muito mais ecaz.
Com o modelo da cmara desenvolvido na Seco 3.5.3, para alm das seis sadas j consider-
adas para o caso dos acelermetros e do magnetmetro, temos mais oito sadas correspondentes s
projeces no referencial R
xyz
dos quatro pontos alvo. Quatro projeces segundo x, e mais quatro
segundo y. Utilizando o total das catorze sadas provenientes dos sensores, o ltro de Kalman est
agora em situao de conseguir observar os doze estados globais. assim possvel obter um controlo
tanto da atitude como da posio do ALIV com base nas estimativas calculadas.
Para o desenvolvimento deste mdulo, ao contrrio do mdulo anterior, foram utilizadas as
matrizes Jacobiana para a totalidade dos estados. Desta vez a matriz da dinmica A para o sis-
tema em espao de estados comuns ter dimenses [12 12] e a matriz de entrada B [12 4],
40
isto x=(u, v, w, p, q, r, x, y, z, , , ), e u=(u
1
, u
2
, u
3
, u
4
). Devido adio das sadas
referentes projeco dos pontos, a matriz C ter dimenses [14 12] e a matriz D [14 4],
y =(a
px
, a
py
, a
pz
, x
1
, y
1
, x
2
, y
2
, x
3
, y
3
, x
4
, y
4
, N
mag
, E
mag
, D
mag). Seguindo os moldes habitu-
ais para a obteno da realimentao atravs da matriz de ganho K do LQR e do sistema caracterstico
em sistema espao de estados que dene o ltro de Kalman (Seco 4), foram atribudas as matrizes
de ponderao Q e R, e as matrizes de covarincia Q
K
e R
K
que melhor satiszessem o sistema
6
.
Como o ltro de Kalman procura minimizar o erro ao longo do tempo entre o estado real e o
estimado, projeco dos pontos pode ser atribudo um movimento caracterstico ao longo do tempo.
Isto faz com que o ALIV reaja de forma a minimizar o erro existente entre a projeco dos pontos e
a sua posio, atribuindo-lhe capacidades autnomas na observao, por exemplo, de um objecto em
movimento.
De acordo com a losoa utilizada no desenvolvimento dos mdulos de simulao, tanto o ltro
de Kalman como o controlador so constantes e construdos de acordo com a posio inicial de
referncia. A altitude de observao bem como qualquer outro estado portanto denida no cheiro
inicial antes de lanar a simulao. A Figura 21 ilustra o raciocnio utilizado na construo deste
mdulo.
Figura 21: Ilustrao do mdulo de visualizao durante a simulao
5.1.3 Mdulo integrado
Um inconveniente do primeiro mdulo de simulao a diculdade de estabilizar o ALIV numa
dada posio, bem como a necessidade da permanncia do utilizador no controlo do mesmo atravs
do joystick. Um inconveniente do segundo mdulo de simulao a necessidade do ALIV ter de estar
a uma dada altitude de modo a que possa observar os pontos ou o objecto de referncia. Deste
modo, foi construdo um mdulo integrado, em que inicialmente conduzido o Quadrirotor atravs
do mdulo com joystick at uma posio em que consiga observar os pontos de referncia, e a partir
da passa a voo autnomo atravs do mdulo de visualizao.
Dado que a dinmica e os sensores se mantm inalterados para cada um dos mdulos, excepo
6
A construo do controlador pode ser analisada no cheiro controlo.m inserido no Apndice C.
41
da integrao da cmara embarcada, a diferena reside no bloco do controlador, responsvel pela
estimativa dos estados e pela realimentao das entradas dos motores. Como estamos a lidar com
duas situaes distintas, em que as sadas dos sensores e os estados a serem estimados so de
dimenses diferentes, necessrio ento utilizar dois controladores. Um deles o construdo no
mdulo com joystick, o outro, o construdo no mdulo de visualizao.
A altitude nal de observao mais uma vez denida no cheiro inicial, e a permuta entre os
dois mdulos (controladores) efectuada atravs do throttle do joystick. Na primeira fase da simu-
lao, necessrio elevar o Quadrirotor e posicion-lo de uma forma mais ou menos estabilizada numa
posio em que possa visualizar os pontos alvo e se situe na proximidade da altitude de observao.
A partir da, e com a alterao do throttle do joystick, utilizando agora o controlador desenvolvido
no mdulo de visualizao, o mesmo estabiliza na posio escolhida como referncia. A partir deste
momento possvel o ALIV comportar-se de uma forma completamente autnoma. A Figura 22
ilustra o raciocnio utilizado na construo deste mdulo durante a simulao.
Figura 22: Ilustrao do mdulo integrado durante a simulao
5.2 Tempo real
Como tem vindo a ser descrito ao longo deste trabalho, objectivo do mesmo aproximar o mais
possvel as simulaes obtidas com a realidade, tendo em vista a futura implementao do estudo em
ambiente real. Assim, de modo a tornar a reaco do ALIV ajustada em relao ao envio dos dados
por parte dos sensores e consequente correco de tenso aplicada a cada um dos rotores, foi criado
um modelo em tempo real atravs do MATLAB e SIMULINK, usando as ferramentas Real-Time
Workshop e Real-Time Windows Target.
5.2.1 Criao de um executvel atravs do Real-Time Workshop
O Real-Time Workshop o primeiro passo para a construo de um sistema em tempo real. O Real-
Time Workshop uma extenso das capacidades do Simulink e do MATLAB que automaticamente
gera, agrega e compila um determinado executvel atravs de um sistema em diagrama de blocos.
Este executvel tem a capacidade de produzir posteriormente um MAT-le contendo os resultados da
42
execuo do modelo em tempo real. Neste captulo, por no ser relevante, no ser debatido o modo
de funcionamento e os princpios do Real-Time Workshop, podendo ser consultada essa informao
na documentao que acompanha o MATLAB. No entanto, ao longo do Apndice D, sero ilustrados
os aspectos mais importantes que levaram construo do executvel.
Uma das limitaes existentes actualmente no processo de criao de um executvel a partir de um
sistema em SIMULINK, que a totalidade do sistema seja constitudo por diagrama de blocos. Isto
faz, com as MATLAB Functions utilizadas at ao momento tenham de ser convertidas para diagrama
de blocos (Apndice D).
Depois destas converses, o sistema desenvolvido em ambiente Simulink est em condies de
ser utilizado pelo Real-Time Workshop. Para tal, necessrio denir os parmetros de resoluo do
sistema e proceder posteriormente compilao do executvel (Apndice D).
Um dos problemas do cheiro criado de ter apenas a resoluo do sistema para uma situao
pontual. Para se verem os resultados de outra simulao, a mesma teria de ser denida inicialmente,
o sistema novamente compilado, e por m o executvel novamente lanado. Para a simulao do
ALIV, e da forma como ele foi desenvolvido, estamos interessados em poder alterar constantemente
no s as situaes iniciais, bem como efectuar a permuta dos controladores, de modo a observar
instantaneamente e em tempo real as alteraes provocados no ALIV. Para isso ser possvel, foi
utilizado posteriormente criao do executvel, o Real-Time Windows Target.
5.2.2 Real-Time Windows Target
O Real-Time Windows Target uma soluo para observar e testar os sistemas em tempo real.
um ambiente em que o computador que corre o cheiro em tempo real e o computador onde
se visualizam os resultados o mesmo. O Real-Time Windows Target realiza a interaco entre o
executvel e o SIMULINK, permitindo ao utilizador usar o modelo desenvolvido em SIMULINK para
visualizar os resultados, usando os mesmos blocos que em modo normal. O Real-Time Windows
Target permite ainda alterar as constantes envolvidas durante a simulao, da mesma forma que se
faz no modo normal, tornando assim a resoluo do sistema em tempo real muito mais exvel.
semelhana do que aconteceu em relao ao Real-Time Workshop, pela falta de relevncia em
relao ao trabalho desenvolvido no ser descrito o modo de funcionamento do Real-Time Windows
Target, que pode ser consultado vastamente por toda a documentao que acompanha o MATLAB.
No entanto, o processo utilizado neste trabalho para se obter a interaco do Real-Time Windows
Target com o modelo desenvolvido pode ser acompanhado em pormenor ao longo do Apndice D.
Para se proceder interaco entre o modelo em ambiente SIMULINK e o executvel, necessrio
fazer uma nova compilao do sistema, desta vez usando o Real-Time Windows Target. Depois,
necessrio usar o modo externo do SIMULINK para conectar o sistema ao executvel. Desta forma,
quando se iniciar a simulao, a mesma decorre em tempo real, podendo os dados ser observados
e alterados exactamente da mesma maneira que se faz quando se utilizam modelos que correm no
modo normal.
Todo o controlo da simulao desenvolvida caracterizada por determinados movimentos do
joystick, quer seja para denir o mdulo de simulao, quer seja para controlar a posio do ALIV
quando estamos a operar no mdulo do joystick. Atravs da interaco com executveis a correr em
tempo real, necessrio fazer a interaco em termos de hardware correspondente ao joystick de modo
43
a que este possa operar em tempo real. Sendo assim, os blocos tradicionais que procuram simular
no s os eixos como os botes do joystick tiveram de ser substitudos por blocos caractersticos do
Real-Time Windows Target para o efeito.
A denio dos eixos foi atribuda a blocos de analog in e os botes a digital in. Dentro
de cada um destes blocos, foi atribudo o canal correspondente ao eixo ou ao boto pretendido
respectivamente. Com este efeito, foi possvel denir o joystick em tempo real, processo fundamental
para a visualizao e controlo dos movimentos do ALIV, caso contrrio a simulao no faria sentido.
A Figura 23 ilustra os blocos usados na denio do processo de seleco do mdulo de simulao.
Figura 23: Implementao do joystick em tempo real
Nesta gura pode ser visualizado o bloco responsvel pela permuta dos controladores, e ainda, o
princpio adiantado na formulao do problema quando o ALIV opera no mdulo com joystick, Seco
5.1.1.
Para alm dos blocos de analog input caractersticos do throttle e da atribuio dos movimen-
tos caractersticos do ALIV, rolamento, picada e altitude, existem ainda os digital input (botes do
joystick), responsveis no modelo desenvolvido, pela atribuio de movimento transversal aos pon-
tos de referncia, representando uma simulao de uma situao de vigilncia, a um objecto com
velocidades moderadas.
5.3 Visualizao 3D dos resultados
Aps o desenvolvimento da plataforma capaz de simular o comportamento do ALIV, e de acordo
com o princpio de funcionamento do mesmo, cou evidente que o principal objecto de estudo
controlar a atitude e a posio do mesmo face s diversas situaes pretendidas. Isto implica observar
o resultado obtido para os seis estados (X, Y, Z, , , ) em relao ao referencial inercial R
NED
.
44
Fazendo uso das tcnicas tradicionais de visualizao dos grcos dos estados ao longo do tempo,
vericou-se que os mesmos no eram muito descritivos da simulao ocorrida. Por isso, de modo
a se poder observar concretamente a reaco do ALIV a determinadas simulaes e objectivos, foi
utilizado o simulador de voo Opensource FLIGHTGEAR[19].
De modo a incorporar o FLIGHTGEAR com a plataforma de simulao desenvolvida em SIMULINK,
e com isso, poder visualizar o comportamento do ALIV em ambiente 3D fazendo uso dos grasmos
do FLIGHTGEAR, foi utilizada a ferramenta Aerospace Toolbox [20] contida no MATLAB
7
.
Uma vez pretendermos seguir a trajectria do ALIV e ver a sua reaco ao longo da simulao,
foi criado um modelo virtual desenvolvido em SOLIDWORKS [21] para incorporao no simulador. A
Figura 24 ilustra o modelo construdo em comparao com o modelo real.
Figura 24: O ALIV real e virtual
Para a construo deste modelo foi tido como objectivo principal obter uma ideia clara da es-
trutura do ALIV no fazendo nfase aos detalhes da electrnica existentes a bordo, sendo que os
mesmos so desnecessrios para a compreenso da visualizao da posio e da atitude do ALIV.
Ao longo do Apndice E, podem ainda ser consultadas em pormenor as transformaes necessrias
para que o modelo inicialmente desenvolvido em SOLIDWORKS possa ser adoptado e correctamente
animado pelo FLIGHTGEAR de acordo com a simulao que est a decorrer. A Figura 25 ilustra
o FLIGHTGEAR quando o mesmo iniciado com o UFO e durante uma fase da simulao. Nesta
imagem pode ser observado que o modelo 3D assumido o do ALIV e que este est alinhado de
acordo com a posio inicial.
Figura 25: Simulao do ALIV no FLIGHTGEAR
7
O Apndice E contm a descrio do processo que tornou possvel a interface entre o SIMULINK e o simulador de
voo Opensource FLIGHTGEAR
45
6 Apresentao e discusso dos resultados
Nesta seco so apresentados os resultados obtidos no decorrer deste trabalho. De uma forma
cronolgica, os aspectos e os objectivos ultrapassados, so expostos como ponto de partida para as
futuras adaptaes e consequentes aproximaes realidade.
As discusses passam pelos aspectos mais relevantes no funcionamento de um Quadrirotor con-
vencional desde a sua dinmica ao tipo de misses a que se prope, isto , voo pairado e situaes
de vigilncia. Inicialmente, e como no poderia deixar de ser, validado o modelo matemtico de-
senvolvido para a dinmica do Quadrirotor, Seco 6.1. Depois, so utilizados os sensores para a
realimentao dos motores atravs dos estados estimados, primeiro com um sensor de posio ideal
para validaao do modelo na Seco 6.2, e depois com o mdulo do joystick na Seco 6.3.1, e por
ltimo com o mdulo de visualizao na Seco 6.3.2. Por m, so discutidos os resultados nais
deste trabalho com base no mdulo integral na Seco 6.3.3.
6.1 Sistema em anel fechado ideal
Para a realizao deste trabalho, e uma vez estar a ser utilizado um modelo de certa forma
desconhecido, o primeiro objectivo a ser cumprido era sem dvida a validao do modelo matemtico
e de toda a teoria a ele inerente. Sendo assim, de acordo com o modelo matemtico desenvolvido na
Seco 3, foi criada uma funo que representasse a dinmica do Quadrirotor, e com base no controlo
moderno LQR (Seco 4.2), procedeu-se construo da matriz de ganho K para a realimentao do
sistema. O resultado foi a construo de um sistema em anel fechado caracterstico do funcionamento
de um Quadrirotor convencional controlado (Figura 26).
Figura 26: Sistema em anel fechado ideal
Como pode ser observado na gura, este primeiro sistema simplesmente constitudo por uma
funo que dene a dinmica de um Quadrirotor convencional apresentada ao longo da Seco 3, e por
uma matriz de ganho K (controlo ptimo LQR), de modo a se conseguir a posio de estabilizao
pretendida. Inicialmente, estes motores possuem a rotao
i
necessria para que seja compensado
o peso e seja igualada a fora segundo a vertical causada pela gravidade, isto , de acordo com a
Seco 3.3:
F
prop
i
= K
f

2
i
= 1.1617 10
5

2
i
=F
prop
z
=
4

i=1
F
prop
i
= 4.6468 10
5

2
i
(73)
46
.
w = 0
F
prop
z
m
S
_

_
0
0
g
_

_
= 0
0
=
_
gm
4.6468 10
5
=
_
9.81 1.2
4.6468 10
5
= 503.3246 rad/s
(74)
6.1.1 Clculo da matriz de ganho K
Como foi apresentado na Seco 4 a matriz de ganho K pode ser obtida com base no comando
lqrd do MATLAB. Para isso, apenas necessrio denir as matrizes A e B que denem a dinmica
do sistema, as matrizes de ponderao Q e R que melhor se ajustam aos estados envolvidos durante
a simulao, e o perodo de amostragem t
s
.
De acordo de novo com a mesma seco, as matrizes A e B podem ser assumidas como matrizes
Jacobianas das diferentes equaes envolvidas em ordem a cada uma dos estados ou entradas. Por isso
foi criado um cheiro denominado numerico.m (Apndice C) que realiza o clculo tanto da matriz A
como da matriz B para o caso da posio de referncia escolhida. Assumindo que se pretende a estabi-
lizao do Quadrirotor a uma dada altitude, isto X
0
=[u, v, w, p, q, r, e
x
, e
y
, e
z
, , , ] = [0 0 0
0 0 0 0 0 h 0 0 0], as matrizes A e B calculadas foram (em unidades SI):
A =
f
dinamica
X
=
_

_
0 0 0 0 0 0 0 0 0 0 9.81 0
0 0 0 0 0 0 0 0 0 9.81 0 0
0 0 0 0 0 0 0 0 0 0 0 0
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
1 0 0 0 0 0 0 0 0 0 0 0
0 1 0 0 0 0 0 0 0 0 0 0
0 0 1 0 0 0 0 0 0 0 0 0
0 0 0 1 0 0 0 0 0 0 0 0
0 0 0 0 1 0 0 0 0 0 0 0
0 0 0 0 0 1 0 0 0 0 0 0
_

_
g = 9.81
(75)
B =
f
dinamica
U
=
_

_
0 0 0 0
0 0 0 0
0.0097 0.0097 0.0097 0.0097
0 0 0.1427 0.1427
0.1731 0.1731 0 0
0.0018 0.0018 0.0018 0.0018
0 0 0 0
.
.
.
.
.
.
.
.
.
.
.
.
_

_
2K
f

0
m
= 0.0097
2K
f

0
Ll
I(1,1)
= 0.1427
2K
f

0
Lc
I(2,2)
= 0.1731
2K
f
KK
0
I(3,3)
= 0.0018
(76)
Estas matrizes esto de acordo com as frmulas da Seco 4.1 conrmando assim a validade do
procedimento elaborado. Quanto s matrizes de ponderao Q e R foram assumidas de acordo com
47
o princpio de Bryson (Seco 4.2), o resultado foi o seguinte:
_

_
u
max
= v
max
= w
max
= 0.2 m/s p
max
= q
max
= r
max
= 0.063 rad/s
x
max
= y
max
= z
max
= 0.4 m
max
=
max
=
max
= 0.049 rad
u
1
max
= u
2
max
= u
3
max
= u
4
max
= 3.16 rad/s
(77)
Q
ii
= [25 25 25 250 250 250 6.25 6.25 6.25 418 418 418] R
ii
= [0.1 0.1 0.1 0.1] (78)
6.1.2 Apresentao dos resultados para o anel fechado ideal
O objectivo dos resultados apresentados nesta seco de demonstrar que a dinmica assumida
para o caso de um Quadrirotor convencional est de acordo com a teoria avanada na Seco 3.
Controlo da posio transversal segundo xy aps estabilizao a uma dada altitude
De modo a no prolongar muito estes resultados preliminares, foi realizada uma trajectria que
envolvesse os trs eixos inerciais. Foi escolhida assim uma simulao que passasse pela estabilizao
a uma dada altitude (5 m) e pela deslocao transversal para x = 3m e depois para y = 3m atravs
da alterao do vector de referncia X
0
. O resultado o apresentado na Figura 27.
Figura 27: Posio para deslocao a 5 m de altitude e 3 m segundo x e y (anel fechado ideal)
Figura 28: Atitude para deslocao a 5 m de altitude e 3 m segundo x e y (anel fechado ideal)
48
Para alm da posio vertical, tambm a posio transversal pretendida foi conseguida com
sucesso, mantendo-se o Quadrirotor estabilizado. A Figura 28 representa a variao da atitude.
A azul pode ser observado o movimento de picada quando se pretende obter a deslocao segundo
x, e a vermelho o movimento de rolamento quando o deslocamento para ser efectuado segundo
y. Ambos os resultados esto de acordo com os referenciais denidos na Figura 7, sendo os valores
bastante parecidos pela quase simetria do Quadrirotor adoptado.
Figura 29: Rotao nos motores na deslocao a 5 m de altitude e 3 m segundo x e y (anel fechado
ideal)
Em termos de motores, na primeira fase estamos a actuar no conjunto dos quatro rotores, de
modo a que o Quadrirotor suba ou desa. Na Figura 29 pode ser observado que existe uma fase
inicial em que existe um aumento repentino de todos os motores de modo a aumentar a altitude do
Quadrirotor, seguida de uma fase de diminuio at que os mesmo atingam a velocidade caracterstica
que compense o peso corpo. Na segunda fase da simulao, estamos a actuar sequencialmente nos
dois pares de rotores.
Primeiramente no par de rotores 3&4, aumentando o rotor 3 e diminuindo o rotor 4 na mesma
ordem de grandeza de forma a manter constante a fora vertical, e posteriormente no par de rotores
1&2, aumentando o 1 e diminuindo o 2 na mesma ordem de grandeza pelas razes j adiantadas,
Figura 29.
Mais uma vez os resultados esto de acordo com o modelo desenvolvido para a dinmica do
Quadrirotor convencional, pelo que, apesar da simplicidade do sistema desenvolvido at aqui, este
49
revela ser de extrema importncia para a continuao da implementao de modelos mais complexos.
6.2 Sistema com sensor de posio ideal
Aps a vericao da dinmica, os objectivos passam pela utilizao do ltro de Kalman para
a realimentao das entradas dos motores com base nos valores estimados dos estados, conferindo
uma maior realidade ao sistema. Para isso, numa primeira fase, foi assumido um sensor de posio
ideal dado o ALIV no possuir qualquer sensor capaz de observar a posio. A Figura 30 representa
o modelo desenvolvido durante esta fase do projecto.
Figura 30: Sistema com sensor de posio ideal
Como pode ser constatado em relao ao modelo inicial, desta vez j esto includos os actuadores
(Seco 3.3), e os blocos responsveis pela construo da estimativa dos estados, os sensores (Seco
3.5) e o estimador (Seco 4.3).
6.2.1 Construo do estimador
Atravs do comando kalmd do MATLAB possvel obter a construo de um estimador com base
no ltro de Kalman em sistema espao de estados (Seco 4.3). Para isso, necessrio fornecer ao
comando o sistema em espao de estados a ser observvel, as matrizes espectrais Q
K
e R
K
, e o
perodo de amostragem t
s
. Desta vez, ao contrrio do que se sucedeu para o caso do clculo da
matriz de ganho K (Seco 4.2) o sistema inicial em sistema espao de estados formado por:
_
_
_
.
x = Ax +Bu +Gw
y = Cx +Du +v
(79)
no entanto a teoria enumerada para o clculo das matrizes numricas A e B (Seco 6.1) aplica-se
na ntegra ao clculo das matriz C e D, desta vez, utilizando as equaes fornecidas pelos modelos
matemticos desenvolvidos para os sensores. Assim, mais uma vez as matrizes C de dimenso
[12 12] e D de [12 4] podem tambm ser denidas como matrizes Jacobianas das equaes
denidas pelos sensores em ordem a cada um dos estados e entradas dos motores intervenientes na
simulao, respectivamente. O resultado foi:
50
C =
f
sensores
X
=
_

_
0 0 0 0 0 0 19.62 0
0 0 0 0 0 19.62 0 0
0 0 0 0 0 0 0 0
0 0 1 0 0 0 0 0
0 0 0 1 0 0 0 0
0 0 0 0 1 0 0 0
0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 1
0 0 0 0 0 0 1 0
_

_
2 g = 19.62
(80)
D =
f
sensores
U
=
_

_
0 0 0.0087 0.0087
0.0071 0.0071 0 0
0.0097 0.0097 0.0097 0.0097
0 0 0 0
.
.
.
.
.
.
.
.
.
.
.
.
0 0 0 0
_

_
2K
f

0
m
= 0.0097
2K
f

0
Ll
I(1,1)
ddy = 0.0071
2K
f

0
Lc
I(2,2)
ddz = 0.0087
(81)
As matrizes A e B so idnticas dado que a dinmica no se alterou. Atravs da atribuio de
valores razoveis s matrizes G, H, Q
K
e R
K
pode ser nalmente calculado o estimador:
_

_
G = eye(12, 6); H = 0 eye(9, 6);
Q
K
ii
= [dd dd dd dd dd dd] R
K
ii
= [dd dd dd dd dd dd dd dd dd], dd = 1 10
4
(82)
sys = ss(A, [B G], C, [D H]); [KEST, L, P, M] = kalmd(sys, Q
K
, R
K
, t
s
);
Em que dd corresponde a uma perturbao associada ao rudo. Depois de se ter a construo do
estimador, o mesmo foi inserido no diagrama de blocos apresentado na Figura 30. A Figura 31 ilustra
o interior desse bloco. O sistema em espao de estados para o caso do estimador o resultado do
KEST como explicado na Seco 4.3.
Figura 31: Estimador em diagrama de blocos para o caso do sistema com sensor de posio ideal
51
6.2.2 Apresentao dos resultados para o sensor de posio ideal
Para se obter a comprovao dos resultados alcanados, foi decidido realizar uma simulao idntica
desenvolvida para a situao de anel fechado de modo a poder vericar se o comportamento atravs
da utilizao da estimativa dos estados era semelhante.
Erro associado aos estados
Como foi referido ao longo da Seco 4.3, desta vez estamos a realizar a realimentao dos
motores atravs das estimativas dos estados obtidos com base nas sadas disponveis dos sensores
e das capacidades do ltro de Kalman em recuperar com base nisso os estados pretendidos. Como
tal, objectivo deste primeiro modelo desenvolvido, conseguir que as estimativas dos estados sejam
aproximadamente iguais aos estados reais de forma a conrmar o modelo. Isto , como foi explicado
na Seco 4.3, que com o passar do tempo o erro x = x x seja nulo, e que durante esse tempo
o Quadrirotor esteja em condies de conseguir atingir os objectivos propostos sem que para isso se
perda o controlo ou se atinjam valores inconvenientes de operao. Para a trajectria at ento vinda
a ser adoptada como referncia, os resultados obtidos relativamente ao erro associado entre o estado
estimado e o real foram os seguintes, Figura 32.
52
Figura 32: Erro entre os estados reais e estimados, sensor de posio ideal
Atravs destes grcos possvel observar que o modelo tem o comportamento pretendido, dado
que em qualquer um dos estados o erro associado tende para zero aps ter ocorrido uma mudana
na referncia. Dado que x = X X
0
de esperar que sempre que se alterar a posio de referncia
o erro inicie-se nesse mesmo valor e tenda depois para zero. Por isso que no momento da alterao
da posio de referncia transversal, o erro passa para o valor da posio desejada, neste caso 3 m.
Pela mesma razo, os restantes erros u, v, w, p, q, r,

,

e

apresentam um pico na altura dessa
mudana da posio de referncia.
Controlo da posio transversal segundo xy aps estabilizao a uma dada altitude
Depois da observao do andamento em termos do erro associado entre os estados reais e os
estimados, procede-se ilustrao dos estados globais de forma a conrmar os resultados previstos
anteriormente. adiantados atravs simplesmente da observao dos erros.
Para o caso da posio global o resultado foi o seguinte (Figura 33).
53
Figura 33: Posio para deslocao a 5 m de altitude e 3 m segundo x e y, sensor de posio ideal
Como pode ser observado na gura anterior, e comparando o resultado com o obtido na Figura 27,
apesar de no nal a posio ser idntica e estar estabilizada, at alcan-la, o Quadrirotor reagiu de
forma diferente. Esta situao de todo normal e aceitvel, dado que desta vez estamos a realizar uma
realimentao atravs da estimativa dos estados e no num anel fechado ideal, no sendo os valores
atingidos um inconveniente modulao at aqui realizada. Esta situao podia ser contornada com
a diminuio da ponderao dos estados referentes posio na matriz Q, no entanto, essa hiptese
revelou uma maior diculdade por parte do Quadrirotor em se estabilizar.
Em relao atitude o resultado foi o seguinte, Figura 34.
Figura 34: Atitude para deslocao a 5 m de altitude e 3 m segundo x e y, sensor de posio ideal
O que importa aqui destacar comparando os resultados com a situao em anel fechado repre-
sentado na Figura 28, que desta vez, apesar da posio de referncia ser mesma distncia, o
Quadrirotor precisar de uma atitude mais acentuada para conseguir realizar a mesma trajectria. En-
quanto que na situao anterior atingia uma picada e um rolamento na ordem dos 6, desta vez, para
54
atingir a mesma distncia precisa aproximadamente do dobro, 12, muito provavelmente associado ao
erro inical existente na atitude, Figura 32. Isto faz estarmos em condies de assumir que o Quadriro-
tor mais limitativo na sua gama de operatividade do que o esperado inicialmente. Apesar disso, o
objectivo de alcanar uma determinada posio estabilizada conseguido e est em consonncia com
os resultados obtidos na situao ideal em anel fechado. O mesmo acontece para o comportamento
dos motores intervenientes ao longo da simulao, pelo que, por motivos de interesse e economia de
espao, as guras referentes a essa situao no vo ser apresentadas.
Estabilizao numa posio bastante diferente da inicialmente conhecida por parte do estimador
De modo a nalizar esta seco, ainda apresentada uma simulao para demonstrar as capaci-
dades do Quadrirotor e a abilidade do modelo desenvolvido at ao momento. Para isso, foi colocado
o Quadrirotor numa posio diferente da situao inicial, em que todos os estados globais so nulos,
e pedido uma nova posio de referncia. Neste tipo de situao o estimador no tem qualquer
informao sobre a posio de partida, logo de esperar erros iniciais algo acentuados. Adoptando
uma condio inicial denida por:
X
i
= [u v w p q r x y z q
0
q
1
q
2
q
3
]
X
i
= [0 0.2 0 0 0 0 2 3 2 1 0.2 0 0.15] (83)
e pretendendo uma condio nal estabilizada:
X
0
= [u v w p q r x y z ]
X
0
= [0 0 0 0 0 0 0 2 5 0 0 0] (84)
o resultado entre a posio e a atitude estimada e real est representado na Figura 35 e na Figura
36.
Figura 35: Erro entre a posio estimada e real, sensor de posio ideal
55
Figura 36: Erro entre a atitude estimada e real, sensor de posio ideal
Como se pode constatar o estimador conseguiu anular o erro inicial, pelo que normal que tenha
conseguido atingir a posio pretendida, Figura 37.
Figura 37: Posio e atitude para uma situao limite, sensor de posio ideal
56
Atravs dos grcos anteriores possvel observar a rapidez com que o Quadrirotor conseguiu
estabilizar numa posio nal bastante diferente da inicialmente conhecida, isto revela ser de extrema
importncia na comprovao do modelo e da teoria adiantada nos resultados anteriores, estando assim
reunidas as condies para o ponto de partida dos mdulos nais.
6.3 Mdulos nais
At esta altura, conseguiram ser vericados os princpios fundamentais pelos quais se regem os
mdulos desenvolvidos. Isto , tanto a dinmica como o estimador revelaram estar de acordo com
o esperado, possibilitando assim a implementao dos mdulos nais caractersticos do MALIV - My
Autonomous Locomotion Individual Vehicle, que consiste num modelo adaptado do ALIV original em
que apenas feito, uso das propriedades de um Quadrirotor convencional. Com base nos resultados
obtidos nas duas ultimas seces, estamos assim em condies de aplicar as alteraes necessrias
para a implementao dos mdulos nais.
A partir desta altura, os resultados obtidos so apresentados com base na simulao em tempo
real, usufruindo das capacidades do MATLAB e do SIMULINK para o efeito, Seco 5.2. O modelo
apresentado na Figura 38 corresponde ao modelo nal, um mdulo integrado, onde possvel realizar
a permuta entre os dois mdulos inicialmente concebidos: o mdulo com joystick e o mdulo de
visualizao, Seco 5.1.
Figura 38: Diagrama de blocos do mdulo integrado
Aps a validao independente dos dois mdulos anteriores, apresentado os resultados do mdulo
integrado, responsvel pela simulao nal adoptada para o controlo verstil e integral do MALIV
8
.
8
Nesta seco, semelhana do que se vai passar nas seguintes, ser dado nfase aos resultados obtidos e no ao
modo como o mdulo foi construdo, para isso que foi criada a Seco 5.1 de explicao da losoa adoptada em
cada um dos mdulos principais.
57
6.3.1 Mdulo com joystick
O primeiro mdulo a ser testado o mdulo com joystick. Este mdulo caracteriza-se por fazer
uso das sadas disponveis nos sensores existentes a bordo do ALIV original e pela utilizao de um
joystick para o controlo da sua posio, Seco 5.1.1.
Com base na Figura 20, exemplicativa da losoa adoptada na construo deste mdulo, o
estimador composto pelo ltro de Kalman semelhana do que aconteceu para o caso do sensor
de posio ideal, Seco 6.2, mas desta vez, apenas esto disponveis as sadas provenientes do
acelermetro e do magnetmetro, Figura 39. Isto faz com que para a construo do novo LQG, tanto
as novas matrizes A[6 6], e B [6 4], sejam agora formadas pelo conjunto de linhas e colunas
das matrizes originais referentes s seis variveis de estado [p q r ], e as matrizes C [6 6] e
D [6 4] formadas pelo conjunto de linhas e colunas das matrizes originais referentes s sadas do
acelermetro e do magnetmetro
9
.
Figura 39: Controlador em diagrama de blocos adoptado para o mdulo com joystick
A nova matriz de ganho K e o ltro de Kalman, so calculados com base nos mesmos comandos
originais e da descrio anterior:
_
_
_
K = lqrd(A, B, Q, R, t
s
)
sys = ss(A, [B G], C, [D H]);
9
A construo destas matrizes pode ser acompanhada no cheiro controlo.m inserido no Apndice C
58
Com desta vez:
_

_
Q
ii
= 1 10
2
[364 364 364 364 364 364] R
ii
= [10 10 10 10]
p
max
= q
max
= r
max
= 0.52 rad/s u
1
max
= u
2
max
= u
3
max
= u
4
max
= 0.32 rad/s

max
=
max
=
max
= 0.52 rad
Q
K
ii
= [dd dd dd dd dd dd] R
K
ii
= [dd dd dd dd dd dd]
G = eye(6, 6) H = 0 eye(6, 6);
(85)
Depois da realimentao, exercido o controlo da posio atravs do joystick como explicado ao
longo da Seco 5.1.1 e ilustrado na Figura 20.
Uma vez o joystick s possuir trs graus de liberdade, teve de se proceder escolha de trs dos
quatro estados de controlo
_
.
U
.
V
.
W
.
R
_
. A escolha recaiu na eliminao do ltimo estado. Sendo
assim, ao movimento de rolamento do joystick est associado a deslocao do MALIV ao longo do
eixo x, e ao movimento de picada a deslocao segundo o eixo y. Como constatado, esta conveno
no est de acordo com o princpio de funcionamento do Quadrirotor adoptado, mas revelou ser uma
maneira mais ecaz no controlo da posio do MALIV atravs do joystick. Assim quando o joystick
se desloca ao longo de um dos seus eixos o movimento do MALIV processa-se ao longo do mesmo
eixo (Apndice D).
Apresentao dos resultados
De modo a ilustrar a validade do modelo desenvolvido, optou-se por denir uma trajectria e
vericar se se conseguia efectu-la com o controlo do joystick. Isto , procurou ser vericado a
estabilidade e versatilidade do MALIV neste mdulo de simulao uma vez j se ter conrmado que
tanto a dinmica como o estimador faziam o Quadrirotor reagir como o esperado.
Controlo da posio transversal segundo xy aps estabilizao a uma dada altitude
Esta simulao passou por realizar pequenos ajustes no movimento do joystick, na totalidade dos
seus trs canais, para conseguir observar a alterao da posio vertical e horizontal ao longo do
tempo, isto , adoptando novas posies de referncia. Para sintetizar os resultados, procedeu-se
elaborao de uma trajectria ao longo dos trs eixos inerciais. Depois da estabilizao vertical, foram
realizadas deslocaes transversais para ver como o MALIV reagia.
Procedeu-se ento simulao da seguinte trajectria:
1. Elevao do MALIV at uma altitude estabilizada na ordem dos 12 m de altitude
2. Manter a posio de altitude durante aproximadamente 30 s
3. Deslocar o MALIV para a posio de y = 12 m
4. Permanecer em y = 12 m e deslocar o MALIV para x = 12 m
5. Manter essa posio durante aproximadamente 25 s
59
6. Voltar posio inicial com x = y = 0
7. Descer at ao solo com velocidades moderadas
A Figura 40 procura ilustrar o resultado da posio do MALIV em termos de xyz ao longo do tempo
durante a simulao:
Figura 40: Posiao xyz ao longo da segunda trajectria
Um dos inconvenientes deste mdulo de simulao, como j foi referido ao longo deste trabalho,
consiste na necessidade do constante ajuste por parte do utilizador da posio de referncia pretendida.
Isto faz com que neste mdulo no sejam de esperar posies de referncia claramente estabilizadas,
no entanto, e como pode ser constatado na Figura 40, consegue-se realizar as trajectrias pretendidas
sem que os valores oscilem muito.
A Figura 41 ilustra os ajustes referidos de modo a possibilitar a permanncia ao longo das posies
de referncia pretendidas.
Figura 41: Atitude ao longo da segunda trajectria
60
Aqui podem ser constatados os movimentos de rolamento e picada necessrios para a deslocao
transversal semelhana do que tem vindo a ser demonstrado, mas desta vez associados a movimentos
do joystick. Foram alcanados aproximadamente valores de 10

para se obter uma trajectria de mais

ou menos 10 m de distncia. Este valor at inferior ao caso apresentado para o sensor de posio
ideal, em que o Quadrirotor precisava de mais de 10 de rotao para atingir os 3 m de distncia.
Isto deve-se ao facto, de anteriormente trabalharmos directamente na posio de referncia, e no
alterarmos gradualmente a velocidade transversal como o caso deste mdulo. Neste aspecto este
modelo revela ser menos limitativo. Em relao aos motores, para a obteno dos movimentos
transversais e como tem vindo a ser referido temos de actuar nos pares de rotores. Na Figura 42
podem ser observados em detalhe os diferenciais existentes entre o mesmo par de rotores para que
as posies transversais sejam obtidas. Optou-se por este sistema de visualizao uma vez a rotao
necessria para as deslocaes verticais ser consideravelmente maior. Assim, consegue-se identicar e
vericar o correcto funcionamento dos mesmos. Isto vem provar a extrema susceptibilidade do MALIV
em adoptar posies estabilizadas, j que um diferencial reduzido no mesmo par de rotores provoca
deslocaes horizontais algo considerveis.
Figura 42: Voltagem aplicada aos motores ao longo da segunda trajectria
61
Como pode ser observado, por volta dos 27.5 s existe um aumento de voltagem no motor n2 e uma
diminuio no motor n1 na mesma ordem de grandeza (Figura 42) assim obtido um deslocamento
negativo segundo x, Figura 40. O mesmo acontece por volta dos 9 s onde desta vez para se obter
uma deslocao positiva segundo y (Figura 40) actuou-se no par de rotores n3 e n4, aumentando a
tenso do motor n3 e diminuindo a do motor n4 (Figura 42). Isto foi modelado atravs da utilizao
de um bloco de derivada associado ao movimento do joystick. Aps esta fase, existe a provocao
de uma acelerao idntica mas de sentido contrrio para que o MALIV se consiga estabilizar, que
ocorre mais ou menos nos 11.4 V , Figura 42, que equivale aos:
503.3246
K
v
+ 9 = 11.4032 V
onde K
v
corresponde ao factor de converso entre tenso e velocidade e 9 ao m da zona morta
do motor adoptado, Seco 3.3. Em termos de altitude no se procedeu da mesma maneira por
inaptido dos resultados com esse sistema, sendo assim, a acelerao vertical sempre anulada com
movimentos de guinada por parte do joystick, contrrios aos anteriormente realizados.
Em relao s velocidades, e devido aos movimentos transversais, as velocidades transversais U
e V apresentam valores na ordem dos 3 m/s, enquanto que a velocidade vertical W ronda os 6 m/s,
Figura 43.
Figura 43: Velocidades lineares ao longo da segunda trajectria
de notar ainda a diminuio da velocidade vertical medida que o MALIV se aproxima do
solo, conseguindo assim com este mdulo efectuar uma descida mais ou menos controlada e com
velocidades verticais convenientes. Todas as velocidades envolvidas durante a simulao, provaram
ser de grandezas bastante aceitveis para o tipo de UAV e de movimentos alcanados.
Este mdulo apesar de possibilitar uma versatilidade ao MALIV de acordo com os sensores nele
embarcados, revelou ser pouco funcional, dado ser necessria alguma prtica para conseguir operar o
MALIV em condies. Todos os valores retirados podem ser alterados consoante a rapidez de resposta
que pretendemos, por isso, o critrio de seleco para este tipo de movimentos, est relacionado com
aquilo que se pensa ser uma situao considerada normal e ajustada ao tipo de funcionalidade que
62
um UAV deste tipo poder vir a ter.
6.3.2 Mdulo de visualizao
O principal problema do mdulo anterior a necessidade do utilizador permanecer em constante
alerta de modo a conseguir obter o controlo posicional do MALIV. De forma a corrigir essa situao foi
criado o mdulo de visualizao que tem como base o modelo da cmara desenvolvida e apresentada
ao longo da Seco 3.5.3. Este modelo consiste numa cmara embarcada no MALIV que atravs
da visualizao, de quatro pontos de referncia consegue a estabilizao numa posio NED. Desta
forma, o MALIV passa a ser autnomo, no havendo interaco entre o utilizador e a misso a que
o MALIV se destina. Essa misso passa pela escolha da altitude de observao, e depois por misses
de vigilncia associadas a objectos com velocidades moderadas.
O importante neste mdulo vericar o modelo da cmara desenvolvido, e saber se em conjunto
com as sadas dos sensores j vericadas, o ltro de Kalman est em condies de recuperar a
totalidade dos doze estados globais. Basicamente a ideia substituir o sensor de posio ideal j
vericado (Seco 6.2), pelo modelo da cmara desenvolvido. Dado que se optou pela utilizao
de pontos de referncia, a visualizao dos mesmos no referencial da imagem, R
xyz
, corresponde a
um par de coordenadas (x, y), desprezando o eixo vertical. Isto equivale para os quatro pontos de
referncia, a oito sadas provenientes da cmara, quatro pares (x, y), Figura 44.
Figura 44: Pontos de referncia no referencial NED, R
NED
Sendo assim, mais uma vez necessrio criar as novas matrizes C e D provenientes do Jacobiano
das equaes dos sensores, enquanto que as matrizes A e B permanecem inalteradas pela dinmica
ser a mesma. de realar, que as primeiras e ltimas trs linhas, em conjunto com as primeiras e
ltimas trs colunas, correspondem s matrizes C e D j calculadas no mdulo com joystick, em que
apenas era utilizado o acelermetro e o magnetmetro
10
.
Considerando os quatro pontos de referncia, a nova matriz C [14 6] obtida foi:
10
Tanto a construo destas matrizes como de todos os outros parmetros envolvidos durante esta simulao podem
ser consultados ao longo do cdigo desenvolvido e apresentado em anexo no Apndice C.
63
C =
f
sensores
X
=
_

_
0 0 0 0 0 0 19.62 0
0 0 0 0 0 19.62 0 0
0 0 0 0 0 0.0001 0.0001 0
0 0 0 0.3577 0.0512 2.5547 0.1023 0.7153
0 0 0.3577 0 0.1028 0.1028 2.7093 0.3577
0 0 0 0.3577 0 2.5036 0 0.7153
0 0 0.3577 0 0.1018 0 2.7072 0
0 0 0 0.3577 0 2.5036 0 0.7153
0 0 0.3577 0 0.1028 0 2.7093 0
0 0 0 0.3577 0.0512 0 0.1023 0.7153
0 0 0.3577 0 0.1028 0 2.7093 0.3577
0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 1
0 0 0 0 0 0 1 0
_

_
(86)
A matriz D apresenta resultados diferentes em comparao com o caso do sensor de posio ideal,
porque a ainda no tinha sido adoptada a posio correcta do acelermetro na estrutura do MALIV.
No entanto o raciocnio o mesmo. A matriz D [14 4] obtida foi:
D =
f
sensores
U
=
_

_
0 0 0.0052 0.0052
0.0043 0.0043 0 0
0.0105 0.0090 0.0097 0.0097
0 0 0 0
.
.
.
.
.
.
.
.
.
.
.
.
0 0 0 0
_

_
(87)
A partir daqui, tanto a matriz de ganho K como o ltro de Kalman em espao de estados foram
calculados com base nos comandos j explicitados:
_
_
_
K = lqrd(A, B, Q, R, t
s
)
sys = ss(A, [B G], C, [D H]);
Com
11
:
_

_
Q
ii
= [ 25 25 25 250 250 250 6.25/10 6.25/10 6.25/10 418 418 418] R
ii
= [0.1 0.1 0.1 0.1]
R
K
ii
= [dd dd dd dd dd dd dd dd dd dd dd dd dd dd] Q
K
ii
= [dd dd dd dd dd dd]
G = eye(12, 6) H = 0 eye(14, 6);
(88)
11
Os valores mximos esperados para os estados envolvidos durante a simulao, para obteno das matrizes Q e R
so idnticos aos apresentados na Equao 77, excepo de x
max
= y
max
= z
max
agora considerados igual a 1.26 m
64
Apresentao dos resultados
Mais uma vez estes resultados so obtidos com a simulao em tempo real e com o mdulo
integrado, fazendo uso desta vez do controlador construdo para o caso do mdulo de visualizao
ao contrrio do que foi utilizado no mdulo de joystick, Figura 39. O objectivo destes resultados
passa por demonstrar as capacidades de vigilncia do MALIV a um objecto animado com velocidades
moderadas, a uma altitude denida inicialmente.
Vigilncia dum objecto ao longo dos eixos transversais xy aps estabilizao a uma dada altitude
Para que se possa testar este mdulo necessrio comearmos a simulao numa posio em que
o MALIV esteja em condies de observar os pontos de referncia, assumindo os pontos de referncia
mencionados na Figura 44, condio necessria que o MALIV esteja a uma dada altitude e perto da
posio (x, y) = (0, 0). A altitude escolhida para observao denida no cheiro inicio.m (Apndice
C). De modo a conseguirmos comparar os resultados obtidos com este mdulo, com resultados
anteriormente obtidos, procedeu-se deslocao do MALIV numa trajectria semelhante. Isto :
1. Estabilizao altitude denida no cheiro inicio.m, 7 m
2. Deslocao transversal e positiva segundo o eixo y
3. Estabilizao na nova posio
4. Deslocao transversal e negativa segundo o eixo x
5. Estabilizao na nova posio
6. Deslocao transversal oposta, recuperao da posio inicial a 7 m de altitude
Ao contrrio do mdulo com joystick, em que existe a liberdade por parte do utilizador em efectuar
qualquer trajectria, neste mdulo a altitude xa e denida no cheiro inicial, e a posio transversal
controlada com a deslocao dos pontos de referncia. Isto acontece porque o modelo foi construdo
de forma que o MALIV com base no ltro de Kalman procure anular o erro entre a posio dos pontos
de referncia e a sua posio actual. Desta forma, possvel vericar as capacidades de vigilncia do
MALIV a um objecto animado com velocidades transversais, desde que para tal, o mesmo consiga
obter informao sobre elementos caractersticos desse objecto a ser vigiado, no presente caso, os
pontos de referncia.
Para efectuar esta simulao, o movimento dos pontos com coordenadas
_
Q
x
Q
y
_
no refer-
encial xo foi associado a trs botes do joystick, com trs funes dependentes do tempo:
_
Q
x
Q
y
_
=
_
Q
x
Q
y
_
+Tt (89)
onde T corresponde a um vector associado ao boto pressionado do joystick. Assim, ao boto n1
est associado uma deslocao negativa dos pontos de referncia sobre o eixo dos x, T = [1 0], ao
boto n2 uma deslocao positiva dos pontos de referncia sobre o eixo dos y, T = [0 1], e ao boto
nmero trs uma deslocao do objecto segundo x e y, T = [1 1], Figura 45.
65
Figura 45: Trajectria do objecto ao longo da simulao no referencial NED, R
NED
Isto quer dizer que para se efectuar a trajectria pretendida e consequentemente analisar a esta-
bilidade do MALIV, necessrio simular o movimento de um objecto com essa trajectria atravs dos
botes 1 a 3 do joystick, e vericar se o MALIV consegue acompanh-lo.
Em termos da posio segundo os trs eixos inerciais o resultado foi o seguinte, Figura 46.
Figura 46: Posiao de vigilncia xyz ao longo da trajectria
Como pode ser observado, o MALIV consegue seguir o objecto animado com velocidade de
1.5 m/s, e desta vez, ao contrrio do que sucedia para o mdulo com joystick, possvel o MALIV
permanecer na mesma posio, estando esta totalmente estabilizada e no sendo necessria qualquer
interveno por parte do utilizador. A escolha da velocidade foi arbitrria, no entanto procurou-se
ilustrar os resultados com uma velocidade em que o MALIV se comportasse dentro da normalidade.
O MALIV consegue tambm acompanhar objectos com velocidades a rondar os 2.5 m/s, mas desta
forma, atinge valores de atitude e tenso aos motores bastante elevados.
A atitude a esperada e est de acordo com a teoria at agora desenvolvida, isto , aps uma
rotao inicial, existe uma rotao oposta para se obter a estabilizao na posio escolhida. picada
est associada a deslocao segundo o eixo x, e ao rolamento a deslocao segundo y. No entanto,
pode ser observado desta vez um acoplamento entre o rolamento e a guinada, Figura 47. Apesar
disso, o MALIV consegue a obteno da posio e orientao desejadas.
66
Figura 47: Atitude ao longo da trajectria de vigilncia
Em termos de limites so atingidos desta vez mais ou menos 10 de amplitude mas s em termos
da guinada, em rolamento e picada so atingidos apenas 5 de amplitude para a deslocao contnua.
Ao contrrio das outras discusses, no vo ser apresentados os grcos referentes aos motores
e s velocidades para esta situao, uma vez essa teoria j ter sido amplamente discutida. Aqui,
o importante era conrmar se o MALIV conseguiria manter uma altitude estabilizada e vigiar um
objecto animado, algo que j foi comprovado.
Vigilncia de um objecto em constante movimento
Para alm da simulao anterior, foi ainda elaborada uma simulao que se prendia pela vigilncia
de um objecto animado continuamente de uma velocidade de 1.5 m/s ao longo de x e y. A diferena
desta simulao consiste no prolongamento da trajectria ao longo dos eixos, de forma a poder
observar como se comporta o MALIV em termos de atitude e velocidades. Esta simulao serve para
ilustrar a base de muitas justicaes adiantadas nas seces anteriores, quando se justicaram no s
o andamento da tenso aos motores e a atitude do Quadrirotor quando este efectuava deslocamentos
transversais. A trajectria foi a representada na Figura 48.
Figura 48: Posiao de vigilncia xyz ao longo da trajectria, segunda trajectria
67
Atravs da Figura 48 podemos observar um tipo de trajectria diferente das habituais, desta vez
houve uma deslocao permanente do objecto durante mais ou menos 30 s segundo os dois eixos xy.
A atitude equivalente a esta trajectria foi, Figura 49.
Figura 49: Atitude ao longo da segunda trajectria de vigilncia
Para alm do acoplamento j referenciado entre a guinada e o rolamento, na Figura 49 podemos
ainda constatar que a anulao da rotao inicial efectuada no momento da concluso da deslocao
sobre os eixos, isto faz com que at a o MALIV esteja animado com uma determinada velocidade
que o permite continuar a deslocar segundo os mesmos, Figura 50.
Figura 50: Velocidades lineares ao longo da segunda trajectria
Isto quer dizer que, quando o objecto a ser observado parou, o MALIV efectuou uma rotao
contrria de modo anular a sua velocidade e com isso conseguir parar na posio devida.
Apesar das limitaes inerentes visualizao dos pontos de referncia numa situao inicial que
fez com que cada uma destas simulaes se inicie j a uma dada altitude, este mdulo revela estar
68
bastante solidicado para os ns a que se prope, estando assim renas as condies para a elaborao
do mdulo nal: o mdulo integrado.
6.3.3 Mdulo integrado
At agora tm vindo a ser apresentadas as discusses dos resultados de uma forma progressiva,
obtidas ao mesmo tempo que diferentes objectivos foram ultrapassados e a proximidade com um
ambiente real se tornava mais evidente. O mdulo integrado corresponde ao mdulo nal adoptado
para a simulao do MALIV, Figura 38. Este mdulo composto pelos mdulos com joystick e
mdulo de visualizao. Isto , este mdulo representa a totalidade de toda a formulao terica
desenvolvida at ao momento, desde a dinmica, os sensores e os diferentes controladores adoptados.
Foi por isso considerado essencial, fazer uma abordagem deste tipo para a seco das discusses.
Este mdulo funciona assim, como a concluso de todos os outros resultados obtidos, em que tudo
se enquadra para que este mdulo corresponda de facto a uma situao possvel de ambiente real.
Tal como foi debatido ao longo da Seco 5.1 em que se d nfase teoria por de trs de cada
um dos mdulos principais, os dois mdulos anteriores tm fragilidades que provocava a inaptido dos
mesmos para o controlo verstil e automatizado do MALIV em ambiente real, objectivo da presente
tese. Se por um lado ao controlarmos o MALIV atravs do Joystick, temos uma maior liberdade
em termos posicionais caracterizada pela no automatizao do mesmo e necessidade constante de
interveno por parte do utilizador, por outro, utilizando a cmara modelada para controlo posicional,
estamos limitados aptido da mesma em observar os pontos de referncia, ou seja, as simulaes
j se iniciam por cima dos mesmos.
Como foi explicado na Seco 6.3.3, a ideia por de trs deste mdulo consiste na conduo do
MALIV atravs do mdulo com joystick at uma posio em que o mesmo consiga observar os pontos
de referncia, e partir da, use o controlador desenvolvido no mdulo de visualizao para se estabilizar
e vigiar o objecto se o mesmo for animado de velocidades transversais moderadas. Essa transio
efectuada usando o throttle do joystick para permuta entre os mdulos.
Como o facto de qualquer um dos controladores j ter sido apresentado e discutido nas seces
anteriores, nesta discusso nal sero j apresentados os resultados para os diferentes tipos de misses
consideradas.
Apresentao dos resultados
Uma vez se tratar do mdulo nal, e dado que o objectivo consiste na elaborao de situaes
plausveis de serem consideradas ajustadas a um ambiente real, ao contrrio das outras simulaes,
nesta ltima seco so consideradas trs tipos de misses distintas que procuram exemplicar as
capacidades do Quadrirotor desenvolvido em realizar objectivos destinados a um UAV deste tipo.
Misso n1: A primeira misso serve para vericar se a permuta entre os mdulos funciona.
elevado o MALIV atravs do mdulo com joystick, e aps o mesmo se situar numa altitude
que consiga visualizar os pontos de referncia, procede-se permuta dos controladores.
Para esta primeira misso seleccionada a altura desejada de observao nal no cheiro inicial,
inicio.m, e depois de correr o cheiro lanada a simulao. A Figura 51 ilustra a posio inercial
69
do MALIV ao longo da presente misso, e a Figura 52 o andamento do throttle do joystick para se
visualizar o momento da transio entre os mdulos.
Figura 51: Posio do MALIV ao longo do tempo, Misso n1
Figura 52: Selector dos mdulos ao longo da primeira misso
Na Figura 52, visvel passagem do segundo 12, a alterao do valor do throttle (por ac-
cionamento manual) para valores inferiores a zero, fazendo com que no selector ilustrado na Figura
23 o mesmo cause a realimentao dos motores atravs do controlador construdo para o mdulo
de visualizao. A partir deste momento o MALIV desloca-se no sentido de se estabilizar altitude
denida inicialmente (5 m). Como pode ser observado atravs da Figura 51, desta vez, e como foi
justicado no sensor de posio ideal (Seco 6.2) j no h a oscilao to evidente da posio
vertical, dado que agora o MALIV foi conduzido at perto dela atravs do joystick. Desta maneira,
o salto entre a posio actual e a de referncia muito menor, e por isso, muito menor o erro
entre as mesmas fazendo com que o MALIV no se afaste consideravelmente da posio nal desejada
antes de se estabilizar. O objectivo desta misso foi cumprido, e conseguiu-se vericar a validade do
70
simulador construdo tendo em conta a permuta entre os dois controladores.
Misso n2: Esta misso consiste na vericao de uma situao em ambiente real em que h
o objectivo de vigiar um objecto situado inicialmente numa posio afastada.
Ao contrrio da misso anterior em que apenas foi demonstrada a aptido do mdulo integrado
em realizar a permuta dos dois mdulos discutidos nas seces anteriores, nesta misso os pontos de
referncia que pretendem simular um objecto, esto distantes do ponto de partida do MALIV cerca
de 5 m de distncia segundo o eixo y. Para isso necessrio adicionar 5 m coordenada y dos pontos
representados na Figura 44.
A Figura 53 e a Figura 54, representam o andamento da posio do MALIV no referencial R
NED
ao longo da simulao, e o throttle do joystick para a seleco do controlador envolvido, ao longo
do tempo.
Figura 53: Posio do MALIV ao longo do tempo, Misso n2
Figura 54: Selector dos mdulos ao longo da segunda misso
71
Como pode ser observado nas guras anteriores, no momento da primeira transio, aproximada-
mente 25 s, o MALIV encontra-se na posio (x, y, z) = (0, 6, 6) pelo que se seguir uma correco
da mesma, para que ao m de se conseguir estabilizar, o mesmo possua a posio nal pretendida,
isto (x, y) = (0, 5) com 5 m de altitude. A Figura 55 ilustra o erro associado entre as sadas estimas
e as sadas reais no momento da permuta entre os mdulos para o caso de cada uma das coordenadas
y dos pontos de referncia.
Figura 55: Erro associado entre as sadas estimas e as sadas reais para a coordenada y dos pontos
de referncia
O pico mais elevado corresponde ao ponto 2, e est de acordo com o esperado, uma vez esse ponto
corresponder ao vrtice isolado do tringulo formado pelo conjunto dos quatro pontos de referncia,
Figura 44. A Figura 56 apresenta as velocidades lineares envolvidas, e obviamente, que tambm nessa
zona, a velocidade segundo y, V , a mais considervel.
Figura 56: Velocidades lineares do MALIV ao longo do tempo, Misso n2
A segunda parte da simulao consistiu na atribuio do movimento ao objecto (entre os 45 s e os
72
80 s aproximadamente), algo j debatido na seco do mdulo de visualizao, Seco 6.3.2. Como
tal, apenas se deixa como referncia, o andamento ajustado das velocidades, Figura 56, e a atitude
envolvida, Figura 57, mais uma vez em consonncia com as duas deslocaes segundo x efectuadas,
Figura 53.
A misso termina com uma nova permuta entre os mdulos, perto dos 105 s, Figura 54, de modo
a que se consiga aterrar o MALIV de uma forma suave na posio de partida, longe do local em que
se situaria o objecto a ser vigiado, Figura 53.
Figura 57: Atitude do MALIV ao longo do tempo, Misso n2
O sucesso desta misso levou a que fossem testados alguns dos limites do MALIV, no s na
primeira abordagem em relao ao objecto, mas tambm no tipo de trajectrias que o mesmo consegue
realizar.
Misso n3: Esta ltima misso consiste na tentativa de demonstrar as capacidades do MALIV
na vigilncia de trajectrias mais complexas, bem com na perseguio de um objecto inicialmente
animado com velocidades moderadas.
Nesta ltima misso as capacidades do MALIV so levadas ao extremo, e o objectivo que aps
conduzi-lo de encontro a um objecto que j esteja a efectuar uma trajectria, o MALIV consiga seguir
o objecto.
Para que tal simulao seja possvel, desta vez atribudo ao boto n1 do joystick, uma funo
trigonomtrica que simule uma trajectria circular. Essa trajectria circular adoptada pelos pontos
de referncia enquanto se estiver a pressionar o boto n1, quando o boto pressionado for o n3,
atribudo um movimento linear segundo x semelhana do que aconteceu na misso n2. Da
forma como a equao est construda, quando se iniciar a trajectria circular, o objecto estar sobre
a posio (x, y) = (0, 10) e efectuar uma translao contrria ao sentido dos ponteiros do relgio,
Figura 58.
73
Figura 58: Alterao dos pontos de referncia em diagrama de blocos
A ideia elevar o MALIV at uma dada altura, prxima da altitude escolhida inicialmente, e
faze-lo posteriormente interceptar a trajectria do objecto, para que a partir da, com a permuta dos
controladores, averiguar as capacidades do MALIV em efectuar a mesma trajectria circular de 10 m
de dimetro, com base no sistema de visualizao.
A misso consistiu em:
1. Elevao do MALIV para perto da altitude de vigilncia
2. Conduo do MALIV at um ponto de passagem da trajectria do objecto
3. Permuta dos controladores
4. Perseguio do objecto segundo uma trajectria circular de forma autnoma
5. Perseguio do objecto aps paragem, segundo uma trajectria rectilnea e de forma autnoma
6. Perseguio do objecto segundo uma nova trajectria circular e de forma autnoma
7. Perseguio do objecto aps paragem, segundo uma nova trajectria rectilnea e de forma
autnoma
8. Permuta dos controladores
9. Aterragem suave por parte do MALIV
74
A Figura 59 representa a trajectria efectuada pelo objecto em termos de xy no referencial R
NED
.
Figura 59: Trajectria do objecto a ser vigiado segundo xy
A Figura 60 representa a posio xy no referencial R
NED
ao longo do tempo por parte do objecto.
Figura 60: Trajectria do objecto a ser vigiado segundo xy ao longo do tempo
A ideia passa por conduzir o MALIV at perto da posio inicial do objecto, e depois atribuir
o movimento ao objecto atravs do boto n1 do joystick. Por m realiza-se a transio entre os
mdulos com o throttle para ver se o MALIV consegue acompanhar o objecto, Figura 61.
75
Figura 61: Selector dos mdulos ao longo da terceira misso
Por comparao entre a Figura 60 e a Figura 61, possvel vericar que a transio entre os
mdulos, que ocorre perto dos 12 s, posterior atribuio do movimento do objecto que acontece
perto dos 9 s .
Na altura em que decorre a transio dos mdulos, perto dos 12 s, a posio do objecto aproxi-
madamente de (x, y) = (2, 7), Figura 60, e a posio do MALIV nessa altura de aproximadamente
(x, y) = (1, 6), Figura 62.
Figura 62: Posio do MALIV ao longo do tempo, Misso n3
O que se verica que apesar de haver um erro inicial mximo em termos de posio x na
ordem dos 3 m, o MALIV consegue recuperar praticamente a totalidade do erro e percorrer a mesma
trajectria que o objecto a ser vigiado, ao ponto de no nal da primeira trajectria circular, por volta
dos 40 s, apresentar praticamente a mesma posio que o objecto, isto , (x, y) = (10, 2). Quanto
mais tempo passar ou quanto menor seja a velocidade do objecto, menor o erro.
76
Posteriormente primeira trajectria, h uma zona de trajectria rectilnea seguida de uma tra-
jectria circular, Figura 59. Nestas condies o MALIV comporta-se como o esperado e consegue
acompanhar o objecto. Na fase nal da primeira trajectria rectilnea (aproximadamente 65 s), o
objecto est na posio (x, y) = (5, 0), Figura 60, da mesma forma que tambm o MALIV se situa
na mesma posio, Figura 62. Isto revela o melhor comportamento do MALIV em trajectrias rec-
tilneas. No nal da segunda trajectria circular, mais uma vez h o acompanhamento total por parte
do MALIV, sendo que passagem do segundo 100, tanto o MALIV como o objecto se encontram
bastante prximos do ponto de coordenadas (x, y) = (4, 4). A trajectria do objecto termina no ponto
de coordenadas (x, y) = (2, 4), Figura 59, e foi a partir dos 130 s que se efectuou nova mudana nos
mdulos, Figura 61, para realizar a aterragem manual atravs do joystick, Figura 62.
Figura 63: Atitude do MALIV ao longo do tempo, Misso n3
Figura 64: Velocidades lineares do MALIV ao longo do tempo, Misso n3
77
Em relao atitude existe um pico em termos de picada perto da primeira permuta dos mdulos,
15 s, dado que a como j foi mencionado, o erro entre a posio x era o maior e rondava os 3 m.
Segue-se uma zona de rotaes mais ou menos equilibradas correspondentes trajectria circular, em
que se ronda os 4, e rotaes bem menos acentuadas quando o MALIV j se encontra numa posio
estabilizada, 50 s, Figura 63 .
Em termos de velocidades (Figura 64) o resultado est ajustado aos movimentos efectuados pelo
MALIV. Para as trajectrias circulares estamos na presena de velocidades segundo x e y, enquanto
que na mudana da posio transversal x, s a velocidade U revela ser considervel, Figura 64)
Estas simulaes servem no s para testar as diferentes possibilidades em termos de misses
exigidas a um UAV como o MALIV, mas tambm, para demonstrar a robustez que o tipo de controlador
e losoa adoptadas possuem. Sendo que, houve a preocupao de acabar as presentes discusses
com situaes que se poderiam reectir numa situao real, no deixando descorar aquilo que foi desde
sempre o objectivo principal desta tese, conseguir controlar e simular um Quadrirotor convencional,
aproximando-o mais possvel de uma situao real em que o mesmo possa vir a ser substitudo
verdadeiramente pelo ALIV.
78
7 Concluses
A presente tese teve como objectivo principal, estudar um UAV de asas rotativas composto por
quatro rotores capazes de por si s controlar os seis graus de liberdade que caracterizam o movimento
segundo os trs eixos inerciais, o Quadrirotor. Foi proposto que, a partir de dados facultados de um
Quadrirotor existente, o ALIV, se conseguisse simular situaes em ambiente real para as quais o
princpio de funcionamento do mesmo se justicasse.
O primeiro objectivo a ser cumprido, foi a modelao de uma dinmica capaz de reproduzir os
efeitos esperados pela variao da rotao de cada um dos quatro rotores, base fundamental na
caracterizao de um Quadrirotor. Com isto, atravs de dados estruturais do ALIV, conseguiu-se
validar as capacidades do mesmo, em obter deslocaes transversais, funcionando como se de um
Quadrirotor convencional se tratasse.
Partindo do equipamento a bordo do ALIV, foram modelados os sensores responsveis pelo controlo
da atitude do mesmo. De modo a tornar o ALIV mais funcional e verstil, houve a necessidade de
modelar um controlo posicional. Desta forma, foram criadas duas solues, uma atravs do joystick,
e outra atravs de uma cmara embarcada. Nos dois mdulos construdos vericaram-se limitaes
prejudiciais ao funcionamento do ALIV em ambiente real. Se por um lado no primeiro havia a
necessidade da presena constante do utilizador e uma grande sensibilidade em termos de estabilidade
posicional, no segundo havia a necessidade de o mesmo partir numa posio em que j observasse
os pontos de referncia. Com base nisto, foi construdo um mdulo integrado capaz de utilizar o
mdulo com joystick numa fase inicial at visualizao dos pontos de referncia, e o mdulo de
visualizao, para a vigilncia de um objecto animado com velocidades moderadas. Desta forma, foi
possvel apresentar simulaes plausveis de caracterizar misses a que um UAV deste gnero possa
estar sujeito.
Tendo em conta que um dos objectivos proposto pela presente tese era aproximar o mais possvel
o sistema desenvolvido com a realidade para futura implementao no ALIV, os modelos foram adap-
tados para uma simulao em tempo real. Foi ainda criado um mdulo de interface com o simulador
FLIGHTGEAR, onde mais facilmente possvel observar a movimentao do ALIV ao longo do tempo.
Com isto, apesar de os dados no puderem ser transpostos para o papel, revelaram ser uma mais valia
na observao 3D da atitude do mesmo, ao invs da visualizao temporal de cada um dos estados
envolvidos na simulao.
Apesar de terem sido realizadas algumas simplicaes com o intuito de conseguir construir um
sistema mais adequado s necessidades, a soluo nal demonstrou ser bastante satisfatria, dado
que vrios objectivos foram sendo ultrapassados com a nalidade de aproximar cada vez mais a
simulao de um ambiente real. Com isto, no s se conseguiram apresentar diferentes mdulos de
simulao, como no m se conseguiu integr-los de forma a tornar a simulao do MALIV mais verstil,
apresentando alguns exemplos do comportamento do mesmo, face a diversas misses propostas.
7.1 Trabalhos futuros
Como tem vindo a ser referido, a presente tese consiste na implementao de um sistema capaz
de no futuro ser implementado no ALIV de modo a que se possa por em prtica todos os princpios
exemplicados virtualmente ao longo desta tese.
79
Desta forma, em termos de continuidade do trabalho desenvolvido, esto um conjunto de processos
que podero aproximar ainda mais estas simulaes do ambiente real exigido pelo ALIV. Entre as
quais, destacam-se processos correctivos e de melhorias modelao desenvolvida. Para isso poder
proceder-se adopo de outros componentes do ALIV como sejam o caso do sistema motor mais
hlice, modelados atravs de ensaios experimentais sobre os mesmos para que a modelao fosse mais
exacta, e ainda a implementao dos graus de liberdade suplementares do ALIV, ganhando com isso
novas possibilidades de simulao, tanto de estabilizao como de deslocaes transversais. E por
m, a ligao entre o sistema desenvolvido e o modelo real atravs da troca de informao entre
os equipamentos embarcados e uma estao de trabalho. Com a construo de uma plataforma de
testes, a integridade da estrutura do ALIV pode ser facilmente salvaguardada, comeando depois a
realizao de pequenos testes entre a estao de trabalho e o ALIV que validem as simulaes at
ento desenvolvidas.
80
Referncias
[1] Oce of the Secretary of Defence Unmanned Aircraft Systems Roadmap: 2005-2030, 2008
[2] http://en.wikipedia.org/wiki/Unmanned_aerial_vehicle, Fevereiro 2008
[3] http://en.wikipedia.org/wiki/Quadrotor, Fevereiro 2008
[4] http://forum.heli4.com/index.php?showtopic=4856, Fevereiro 2008
[5] J.Gordon Leishman. The Brguet-Richet Quadrotor Helicopter of 1907, 2002
[6] Tarek Hamel, Robert Mahony, Rogelio Lozano, & James Ostrowski. Dynamic Modelling and
Conguration Stabilization for an X4-Flyer. Em IFAC 15th Triennial World Congress, Barcelona,
Espanha, 2002
[7] Tarek Hamel, Robert Mahony, & Abdelhamid Chriette. Visual servo trajectory tracking for a four
rotor VTOL aerial vehicle. Em Proceedings of the IEEE International Conference on Robotics
and Automation, 2002
[8] N. Guenard, T. Hamel, V. Moreau. Dynamic modeling and intuitive control strategy for an
X4-yer. Em ICCA, Budapeste, Hungria, 2005
[9] http://www.rctoys.com/draganyer5.php, Maro 2008
[10] P. Pounds, R. Mahony, P. Hynes, & J. Roberts. Design of a Four-Rotor Aerial Robot. Em
Proceedings of the 2002 Australasian Conference on Robotics and Automation, 2002
[11] Paul Pounds, Robert Mahony, Joel Gresham, Peter Corke, & Jonathan Roberts. Towards
Dynamically-Favourable Quad-Rotor Aerial Robots. Em Proceedings of the 2004 Australasian
Conference on Robotics and Automation, 2004
[12] Gabe Homann, Dev Gorur Rajnarayan, Steven L. Waslander, David Dostal, Jung Soon Jang,
& Claire J. Tomlin. The Stanford Testbed of Autonomous Rotorcraft for Multi Agent Control
(STARMAC). Em Proceedings of the 23rd Digital Avionics Systems Conference, Stanford, 2004
[13] E. Altug, J. P. Ostrowski, & R. Mahony. Control of a Quadrotor Helicopter Using Visual Feed-
back. Em Proceedings of the IEEE International Conference on Robotics and Automation, 2002
[14] E. Altug, J. P. Ostrowski, & C. J. Taylor. Quadrotor Control Using Dual Camera Visual Feedback.
Em Proceedings of the IEEE International Conference on Robotics and Automation, 2003
[15] Samir Bouabdallah, Andre Noth, & Roland Siegwan. PID vs. LQ Control Techniques Applied
to an Indoor Micro Quadrotor. Em Proceedings of 2004 IEEE/RSJ International Conference On
Intelligent Robots and Systems, 2004
[16] Brian L. Stevens, Frank L. Lewis. Aircraft Control and Simulation, 2nd Edition, New York: John
Wiley & Sons, Inc., 2003
[17] J. R. Azinheira. Controlo de Voo. Folhas da cadeira de Controlo de Voo, Instituto Superior
Tcnico, Lisboa 2007
81
[18] Barnes W. McCormick . Aerodynamics, Aeronautics, and Flight Mechanics, 2nd Edition, New
York: John Wiley & Sons, Inc., 1995
[19] http://www.FLIGHTGEAR.org, Julho 2008
[20] http://www.mathworks.com/products/aerotb/, Julho 2008
[21] http://www.SOLIDWORKS.com, Julho 2008
[22] http://www.BLENDER.org, Julho 2008
82
Apndice A
Nesta seco sero apresentados com detalhe todos os passos envolvidos na obteno das equaes
que denem a dinmica do problema.
Deduo da matriz de rotao global:
S

=
_

_
1 0 0
0 cos sin
0 sin cos
_

_
S

=
_

_
cos 0 sin
0 1 0
sin 0 cos
_

_
S

=
_

_
cos sin 0
sin cos 0
0 0 1
_

_
S = S

S
E
=
_

_
cos cos sincos sin
cos sin sincos sin sin sin sin+cos cos cos sin
cos sin cos +sin sin sin sin cos sincos cos cos
_

_
Dinmica
F
T
=
d
dt
[mV
T
]
B
+ [mV
T
]
B
M
T
=
d
dt
[I]
B
+ [I]
B
Dado que a massa constante, = [P Q R] e V
T
= [U V W] ento:
_
P Q R
_
T

_
U V W
_
T
=
_

_
QW RV
RU PW
PV QU
_

_
F
T
= m
_

W
_

_
+m
_

_
QW RV
RU PW
PV QU
_

_
S
E
_
0 0 g
_
T
=
_

_
sin
cos sin
cos cos
_

_
B
g
_

W
_

_
=
1
m
_

_
F
px
F
py
F
pz
_

_
+g
_

_
sin
cos sin
cos cos
_

_
QW RV
RU PW
PV QU
_

_
83

U =
F
px
m
g sin QW +RV

V =
F
py
m
+g sincos RU +PW

W =
F
pz
m
+g cos cos PV +QU
Para a equao dos momentos, I uma matriz principal, I =
_

_
I
11
0 0
0 I
22
0
0 0 I
33
_

_
[I] =
_
(I
33
I
22
) RQ (I
11
I
33
) QP (I
22
I
11
) PQ
_
M
T
=
_

_
I
11

P
I
22

Q
I
33

R
_

_
+
_

_
(I
33
I
22
) RQ
(I
11
I
33
) QP
(I
22
I
11
) PQ
_

P =
M
x
I
11

(I
33
I
22
)
I
11
RQ

Q =
M
y
I
22

(I
11
I
33
)
I
22
QP

R =
M
z
I
33

(I
22
I
11
)
I
33
PQ
Cinemtica, para a posio:

r = X

i +Y

j +Z

r =

X

i +

Y

j +

Z

k
_

Z
_

_
= S
T
E
_

_
U
V
W
_

X=cos cos V + (cos sin sincos sin) V +(cos sincos +sin sin)W

Y =sin cos U+(sin sin sin+cos cos )V +(sin sin cos sincos )W

Z=sinU+cos sinV +cos cos W

84
Cinemtica, para os ngulos de Euler:
T =
_

_
1 tan sin tan cos
0 cos sin
0 sec sin sec cos
_

_
=
_

_
= T
_

_
P
Q
R
_

= P +Qtan sin +Rtan cos

= Qcos Rsin

= Qsec sin +Rsec cos

Cinemtica, para quaternies:
Partindo dos ngulos de Euler iniciais so obtidos os quaternies para o inicio da simulao:
q
0
= (cos (

/2) cos (

/2) cos (

/2) + sin(

/2) sin(

/2) sin(

/2))
q
1
= (sin(

/2) cos (

/2) cos (

/2) cos (

/2) sin(

/2) sin(

/2))
q
2
= (cos (

/2) sin(

/2) cos (

/2) + sin(

/2) cos (

/2) sin(

/2))
q
3
= (cos (

/2) cos (

/2) sin(

/2) sin(

/2) sin(

/2) cos (

/2))
Da equao da cinemtica:
_

_
P = 2(q
0
.
q
1
+q
3
.
q
2
q
2
.
q
3
q
1
.
q
0
)
Q = 2(q
3
.
q
1
+q
0
.
q
2
+q
1
.
q
3
q
2
.
q
0
)
R = 2(q
2
.
q
1
q
1
.
q
2
+q
0
.
q
3
q
3
.
q
0
)
_

_
P
Q
R
_

_
= 2
_

_
q
1
q
0
q
3
q
2
q
2
q
3
q
0
q
1
q
3
q
2
q
1
q
0
_

_
_

_
.
q
0
.
q
1
.
q
2
.
q
3
_

_
Do facto da matriz ser ortogonal e quando invertida as equaes serem bi-lineares em termos de
q
i
e velocidades angulares:
.
q
0
=
1
2
(Pq
1
+Qq
2
+Rq
3
)
.
q
1
=
1
2
(Pq
0
Rq
2
+Qq
3
)
.
q
2
=
1
2
(Qq
0
+Rq
1
Pq
3
)
.
q
3
=
1
2
(Rq
0
Qq
1
+Pq
2
)
85
Apndice B
Acelermetro
Um acelermetro num determinado ponto P mede a componente de acelerao a

p
dada por:
a

p
= a
p
g (90)
Sendo r o vector que liga o ponto de medio do acelermetro ao centro de gravidade. A veloci-
dade instantnea no ponto P para um espao inercial dada por:
V
P
= V
cg
+ r
e a acelerao inercial obtida diferenciando esta equao em relao ao referencial inercial:
a
P
=
d (V
cg
)
dt
+
d ( r)
dt
Esta ltima equao pode ser expressa em termos do referencial do corpo ABC, R
ABC
, para se de-
terminar a sada do acelermetro. Ao primeiro termo esto associadas as foras envolvidas, enquanto
que ao segundo termo se pode usar o teorema de Coriolis para diferenciar ( r). O resultado da
soma destes dois termos ser a estimativa da acelerao global e pode ser escrito como:
a
p
=
F
ABC
+S
E
mg
m
+
.

ABC
r +
ABC
(
ABC
r)
Cmara embarcada
Seguindo o mesmo princpio para o clculo da matriz S, a matriz S
cam
clculada atravs das
seguintes matrizes:
S
cam
x
=
_

_
1 0 0
0 cos sin
0 sin cos
_

_
S
cam
y
=
_

_
cos 0 sin
0 1 0
sin 0 cos
_

_
S
cam
z
=
_

_
cos sin 0
sin cos 0
0 0 1
_

_
S
cam
= S
cam
z
S
cam
y
S
cam
x
Atendendo s equaes:
_

_
P
x
P
y
P
z
_

_
=
_

_
X
NED
Y
NED
Z
NED
_

_
+S
T
_

_
X
cam
Y
cam
Z
cam
_

_
86
_

_
= atan2 (S
T
(2, 3) , S
T
(3, 3))
= arcsin
_
S
T
(1,3)
det(S
T
)
_
= arcsin(S
T
(1, 3))
= atan2 (S
T
(1, 2) , S
T
(1, 1))
A matriz M pode ser calculada pela multiplicao das matrizes responsveis pela adio de rotao
e translaco aos pontos de referncia de modo a se obter a sua projeco no referencial da imagem,
R
xyz
:
M
per
=
_

_
0 0 0
1
/f
0 1 0 0
0 0 1 0
0 0 0 1
_

_
; T
tr
=
_

_
1 0 0 0
0 1 0 0
0 0 1 0
P
x
P
y
P
z
1
_

_
; T
trcp
=
_

_
1 0 0 0
0 1 0 0
0 0 1 0
f 1 10
6
0 0 1
_

_
S

=
_

_
1 0 0 0
0 cos sin 0
0 sin cos 0
0 0 0 1
_

_
S

=
_

_
cos 0 sin 0
0 1 0 0
sin 0 cos 0
0 0 0 1
_

_
S

=
_

_
cos sin 0 0
sin cos 0 0
0 0 1 0
0 0 0 1
_

_
M = T
tr
S

T
trcp
M
per
o que nalmente leva a que a projeco dos pontos seja dada por:
Q
P
= QM
em que Q corresponde matriz com as coordenadas dos pontos de referncia no referencial xo,
R
NED
:
Q =
_

_
2 1 0 1
2 0 0 1
2 0 0 1
2 1 0 1
_

_
em que a 3 coluna corresponde ao eixo z e a 4 coluna projeco segundo a unidade focal que
iniciada a 1.
87
Apndice C

i n i c i o .m

%RunScr i pt que i n i c i a l i z a as p r i n c i p a i s c ons t a nt e s u t i l i z a d a s no s i s t e ma

% Par met r os F s i c o s :
% g a c e l e r a o g r a v t i c a [m/ s ^2]
% r o de ns i dade do ar [ g/m^3]
% t s amostragem [ s ]
% f r f r e q u n c i a
% Par met r os e s t r u t u r a i s , ALIV
% m massa [ kg ]
% mm Massa motor [ Kg ]
% xm Compri mento do motor segundo x [m]
% ym Compri mento do motor segundo y [m]
% zm Compri mento do motor segundo z [m]
% I x x Momento de i n r c i a t o t a l segundo x [ Kg .m^2]
% I y y Momento de i n r c i a t o t a l segundo y [ Kg .m^2]
% I z z Momento de i n r c i a t o t a l segundo z [ Kg .m^2]
% I Mat r i z s i m t r i c a i n e r c i a l
% Lc Di s t nc i a do CG ao Rotor ( compri mento ) [m]
% Ll Di s t nc i a do CG ao Rotor ( l a r g u r a ) [m]
% Par met r os e s t r u t u r a i s motor+h l i c e , ALIV
% Cp Co e f i c i e n t e de pr e s s o
% Ct Co e f i c i e n t e de pr opul s o
% r _he l i c e Rai o do H l i c e [m]
% Vmax Vol tagem mxima do motor [ Vol t s ]
% RPM_max r ot a o mxima do motor [ rpm]
% rad_max r ot a o mxima do motor [ r ad / s ]
% Kf F=Kf (w^2) r e l a o e nt r e For a e r ot a o a p l i c a d a
% Km M=Km(w^3) r e l a o e nt r e momento e r ot a o a p l i c a d a
% KK r e l a o e nt r e Kf e Km
% Kv r e l o e nt r e t ens o e r ot a o a p l i c a d a
% Vmin Tenso mi ni ma ou zona morta [ Vol t s ]
% TVOLT Vol t s a p l i c a d o s a cada motor t a l que dw = 0 [ Vol t s ]
% r ot acao0 r ot a120 o api cada a cada motor t a l que dw = 0 [ r ad / s ]
% Par met r os i n i c i a i s
% Xi ni Condi o i n i c i a l dos e s t ados
% Xi ni 2 Condi o i n i c i a l dos e s t ados no e s t i mador
% Par met r os dos s e n s o r e s
% ddx Di s t nc i a do ac e l e r me t r o em r e l a o ao CG, segundo x [m]
% ddy Di s t nc i a do ac e l e r me t r o em r e l a o ao CG, segundo y [m]
% ddz Di s t nc i a do ac e l e r me t r o em r e l a o ao CG, segundo z [m]
% cam Pos i o da cmara , r e f e r e n c i a l ABC: [ x , y , z , phi , t et a , p s i ]
% n_cp Di s t nc i a f o c a l i n i c i a l
% L Di s t nc i a e nt r e l i n h a s [m]
% LN Di s t nc i a e nt r e l i n h a s [m]
% f Focal da cmara
% pt_1_2 Ponto de r e f e r n c i a , l ado d i r e i t o
% pt_2_1 Ponto de r e f e r n c i a c e n t r a l
% pt_2_2 Ponto de r e f e r n c i a c e n t r a l
% pt_3_2 Ponto de r e f e r n c i a , l ado es quer do
% no i s e Ru do a s s oc i a do s medi es do magnetmetro
% n nmero de b i t s
% ymi n Val or de s a da mximo par a o magnetmetro
% ymax Val or de s a da m ni mo par a o magnetmetro
% r e s o l u c a o Gama de r e s o l u o
% Vec t or es com os e s t ados i n i c i a i s
% X_0J Vect or com os e s t ados i n i c i a i s ( cas o do J o y s t i c k )
% X_0 Vect or com os e s t ados i n i c i a i s ( cas o da cmara )
% XU_0 Vect or com a r ot acao0 e os e s t ados i n i c i a i s de r e f e r n c i a
% U_0 Vect or com a r ot acao0
% Y_0 Vect or com as s a d a s dos t r s s e n s o r e s
% Y_0J Vect or com as s a d a s do ac e l e r me t r o e magnetmetro
%
c l e a r a l l
g l o b a l w_motor0 Kf KK m g I Ll Lc f r r ot acao0 ddx ddy ddz cam L LN f
syms TVOLT
%
%Par met r os F s i c o s
g = 9 . 8 1 ; %Ac e l e r a o g r a v i t i c a [m/ s ^2]
r o = 1. 225; %Densi dade do ar [ Kg/m^3]
t s = 0 . 0 2 ; %Amostragem [ ms ]
f r = 16; %Fr e qunc i a [ Hz ]
%
88
%Par met r os e s t r u t u r a i s , ALIV
m = 1 . 2 ; %Massa t o t a l [ Kg ]
mm = 0. 060; %Massa motor [ Kg ]
xm = 0. 015; %Compri mento do motor segundo x [m]
ym = 0. 015; %Compri mento do motor segundo y [m]
zm = 0. 032; %Compri mento do motor segundo z [m]
Lc = 0 . 6 4 ; %Di s t nc i a do CG ao Rotor ( compri mento )
Ll = 0 . 6 ; %Di s t nc i a do CG ao Rotor ( l a r g u r a )
I x 1 = (1/12) mm ( ( ym^2) + (zm^2) ) ; I x 2 = I x 1 ;
I x 3 = I x 1 + (mm( Lc ^2) ) ; I x 4 = I x 3 ;
I x x = I x 1 + I x 2 + I x 3 + I x 4 ; %Momento de i n e r c i a t o t a l segundo x
I y 3 = (1/12) mm (xm^2 + zm^2) ; I y 4 = I y 3 ;
I y 1 = I y 3 + (mm( Ll ^2) ) ; I y 2 = I y 1 ;
I y y = I y 1 + I y 2 + I y 3 + I y 4 ; %Momento de i n e r c i a t o t a l segundo y
I z 1 = (1/12) mm (xm^2 + zm^2) + mm( Ll ^2) ; I z 2 = I z 1 ;
I z 3 = (1/12) mm (xm^2 + zm^2) + mm( Lc ^2) ; I z 4 = I z 3 ;
I z z = I z 1 + I z 2 + I z 3 + I z 4 ; %Momento de i n e r c i a t o t a l segundo z
[ I ] = [ I x x 0 0 ; %Mat r i z s i me t r i c a i n e r c i a l
0 I y y 0 ;
0 0 I z z ] ;
%
%Par met r os e s t r u t u r a i s motor+h l i c e , ALIV
Cp = 0. 0374; %Co e f i c i e n t e de Pr es s o
Ct = 0. 1047; %Co e f i c i e n t e de pr e pul s o
r _he l i c e = 0. 2458/2; %Rai o do H l i c e [m] = di amet r o 9
Vmax = 12; %Vol tagem mxima do motor
RPM_max = 6000; %r ot a o mxima do motor [ rpm]
%Val or c ons t ant e que r e l a c i o n a Rotao e a For a a p l i c a d a ao motor
Kf = doubl e (4 r o ( r _he l i c e ^4)Ct /( pi ^2) ) ; %F=Kf (w^2)
%Val or c ons t ant e que r e l a c i o n a Rotao e o Momento a pl i c a d o ao motor
Km = doubl e (4 r o ( r _he l i c e ^5)Cp/( pi ^3) ) ; %M=Km(w^3)
%Re l ac i ona Kf e Km
KK = doubl e ( r _he l i c e Cp/( Ct pi ) ) ;
rad_max = doubl e (RPM_max2 pi /60) ; %RPM mximo do motor em r ad / s
Kv = doubl e (20002 pi /60) ; %r e l a o e nt r e t ens o e v e l o c i d a d e
Vmin = doubl e (Vmax rad_max/Kv ) ; %t ens ao mi ni ma ou zona morta
%Vol t s a p l i c a d o s a cada motor t a l que dw = 0 ( as s umi dos t odos i g u a i s )
TVOLT = s o l v e ( (TVOLT9)^2 mg /( Kf ( Kv^2)4) ,TVOLT) ;
TVOLT = doubl e (TVOLT( 1 ) ) ;
%Ve l oc i dade de r ot a o a p l i c a d o s a cada motor t a l que dw = 0
r ot acao0 = doubl e ( Kv(TVOLTVmin ) ) ; %Vol t s par a r ad / s
w_motor0 = [ r ot acao0 r ot acao0 r ot acao0 r ot acao0 ] ;
%Aproxi mao i n i c i a l : Rel ao t ens o / r ot a o nos mot or es
wtoN = 2Kf r ot acao0 ; %Motores i d n t i c o s
%
%Par met r os i n i c i a i s
Xi ni = [ 0 0 0 0 0 0 0 0 5 1 0 0 0 ] ; %Condi es i n i c i a i s par a X
Xi ni 2 = [ 0 0 0 0 0 0 0 0 0 0 0 0 ] ; %Condi es i n i c i a i s par a e s t i mador
%Es t ados i n i c i a i s par a a pos i o de r e f e r n c i a
U0 = 0; %Ve l oc i dade Li ne a r segundo X, NED
V0 = 0; %Ve l oc i dade Li ne a r segundo Y, NED
W0 = 0; %Ve l oc i dade Li ne a r segundo Z, NED
P0 = 0; %Ve l oc i dade ngul ar segundo Ex , ABC
Q0 = 0; %Ve l oc i dade ngul ar segundo Ey , ABC
R0 = 0; %Ve l oc i dade ngul ar segundo Ez , ABC
X0_NED = 0; %Pos i o segundo X, NED
Y0_NED = 0; %Pos i o segundo Y, NED
Z0_NED = 7; %Pos i o segundo Z, NED
phi _0 = 0; %ngul o de Rol amento
teta_0 = 0; %ngul o de Pi cada
ppsi _0 = 0; %ngul o de Gui nada
q0_0 = 1; %Quat er ni o i n i c i a l q0
q1_0 = 0; %Quat er ni o i n i c i a l q1
q2_0 = 0; %Quat er ni o i n i c i a l q2
q3_0 = 0; %Quat er ni o i n i c i a l q3
s ave ( c ons t ant e s ) ; %Guarda o v a l o r das c ons t a nt e s
89
%
%Par met r os dos s e n s o r e s
no i s e = 1e 3;
n = 10; %nmero de b i t s
ymi n = 2g ; %Val or de s a da mximo
ymax = 2g ; %Val or de s a da m ni mo
r e s o l u c a o = ( ymax ymi n )/(2^n ) ; %Gama de r e s o l u o
%Acel er met r o
ddx = 0; %Di s t nc i a em r e l a o ao CG segundo x
ddy = 0. 005; %Di s t nc i a em r e l a o ao CG segundo y
ddz = 0 . 0 3 ; %Di s t nc i a em r e l a o ao CG segundo z
%Cmara embarcada
w=800/2;
hh=600/2;
deg = pi /180;
n_cp = 1;
cam = [ . 0 1 0 0. 01 0 90deg 0 ] ; %Pos i o da cmara , ABC
L = 2; %d i s t n c i a e nt r e as l i n h a s esq e d i r
LN = 2; %d i s t n c i a
f = 2 . 5 ; %f o c a l da cmara
pt_1_2 = [ LN L/2 0 ] ; %ponto de r e f e r n c i a , d i r e i t o
pt_2_1 = [LN 0 0 ] ; pt_2_2 = [ LN 0 0 ] ; %pont os de r e f e r n c i a c e n t r a i s
pt_3_2 = [ LN L/2 0 ] ; %ponto de r e f e r n c i a , es quer do
t x = 1 . 5 ;
t y = 1 . 5 ;
%
%Vec t or es com os e s t ados i n i c i a i s , us ados par a a c ons t r u o das ma t r i z e s numr i cas
%Vect or com os e s t ados i n i c i a i s ( cas o do J o y s t i c k )
X_0J = [ P0 Q0 R0 phi _0 teta_0 ppsi _0 ] ;
%Vect or com os e s t ados i n i c i a i s ( cas o da cmara )
X_0 = [ U0 V0 W0 P0 Q0 R0 X0_NED Y0_NED Z0_NED phi _0 teta_0 ppsi _0 ] ;
%Vect or com os e s t ados i n i c i a i s mai s a r ot a o de cada motor i n i c i a l
XU_0 = [ w_motor0 U0 V0 W0 P0 Q0 R0 X0_NED Y0_NED Z0_NED phi _0 teta_0 ppsi _0 ] ;
%Vect or com a r ot a o i n i c i a l , r ad / s
U_0 = w_motor0 ;
%Vect or com as s a d a s dos s e n s o r e s c or r e s ponde nt e s aos e s t ados i n i c i a i s
Y_0= s e ns or (XU_0) ;
Y_0J = z e r os ( 1 , 6 ) ;
Y_0J ( 1 : 3 ) = Y_0( 1 : 3 ) ; %Cons t r uo do v e c t or de s a da
Y_0J ( 4 : 6 ) = Y_0( 12: 14) %par a o cas o do j o y s t i c k , 6 s a d a s
%
%Cons t r uo das ma t r i z e s numr i cas , ma t r i z e s de ganho e de kal man par a ambos os cas os , mdul o com
%j o y s t i c k e mdul o de v i s u a l i z a o
%Mat r i z e s numr i cas do s i s t e ma par a pos i o r e f e r n c i a
[ A_num, B_num, C_num, D_num] = numer i co (XU_0) ;
%Mat r i z e s de ganho e de Kalman par a c ons t r u o do c o n t r o l a d o r
[ K, KJ , A_kd, B_kd, C_kd , D_kd, MM, A_kdJ , B_kdJ , C_kdJ , D_kdJ ] = c o n t r o l o (A_num, B_num, . . .
C_num, D_num) ;

numer i co .m

f u n c t i o n [ A_num, B_num, C_num, D_num] = numer i co (XU_0)

% Funo que r e a l i z a as oper a e s numr i cas par a obt eno das ma t r i z e s
% n e c e s s r i a s par a a u t i l i z a o no LQR e do F i l t r o de Kalman usando
% es pao de e s t ados
% Par met r os de ent r ada :
% XU_0 Vect or com a r ot acao0 e os e s t ados i n i c i a i s de r e f e r n c i a
% Parmetro de s a da :
% A_num Mat r i z A numr i ca par a es pao de e s t ados
% B_num Mat r i z B numr i ca par a es pao de e s t ados
% C_num Mat r i z C numr i ca par a es pao de e s t ados
% D_num Mat r i z D numr i ca par a es pao de e s t ados
% Li n e a r i z a o numr i ca das ma t r i z e s A e B
% J Mat r i z Jacobi ana
% XU_ 0 Vect or com a r ot acao0 e os e s t ados i n i c i a i s de r e f e r n c i a
% dxdt_0 Es t ados i n i c i a i s par a v a l o r e s de r e f e r n c i a
% dd Per t ur bao
% X_1 Vect or com pe r t ur ba o numa dada r e f e r n c i a
90
% dxdt_1 Vect or com s t ados aps pe r t ur ba o num v a l o r de r e f e r n c i a
% Li n e a r i z a o numr i ca das ma t r i z e s C e D
% Z mat r i z Jacobi ana
% dxdt_0 Sa das i n i c i a i s par a v a l o r e s de r e f e r n c i a
% X_1 Vect or com pe r t ur ba o numa dada r e f e r n c i a
% dxdt_1 Vect or com s a d a s aps pe r t ur ba o num v a l o r de r e f e r n c i a
%
%Par met r os i n i c i a i s , de r e f e r n c i a
w_motor0 = [ XU_0( 1) XU_0( 2) XU_0( 3) XU_0( 4 ) ] ; %Rotao em cada um dos mot or es
%Val or e s de r e f e r n c i a
U0 = XU_0( 5 ) ; %Ve l oc i dade Li ne a r segundo X, NED
V0 = XU_0( 6 ) ; %Ve l oc i dade Li ne a r segundo Y, NED
W0 = XU_0( 7 ) ; %Ve l oc i dade Li ne a r segundo Z, NED
P0 = XU_0( 8 ) ; %Ve l oc i dade ngul ar segundo Ex , ABC
Q0 = XU_0( 9 ) ; %Ve l oc i dade ngul ar segundo Ey , ABC
R0 = XU_0( 1 0 ) ; %Ve l oc i dade ngul ar segundo Ez , ABC
X0_NED = XU_0( 1 1 ) ; %Pos i o segundo X, NED
Y0_NED = XU_0( 1 2 ) ; %Pos i o segundo Y, NED
Z0_NED = XU_0( 1 3 ) ; %Pos i o segundo Z, NED
phi _0 = XU_0( 1 4 ) ; %ngul o de Rol amento
teta_0 = XU_0( 1 5 ) ; %ngul o de Pi cada
ppsi _0 = XU_0( 1 6 ) ; %ngul o de Gui nada
quat = e ul e r 2qua t ( [ phi _0 teta_0 ppsi _0 ] ) ; %Passagem par a qu a t e r ni e s
q0_0 = quat ( 1 ) ; %Quat er ni o i n i c i a l q0
q1_0 = quat ( 2 ) ; %Quat er ni o i n i c i a l q1
q2_0 = quat ( 3 ) ; %Quat er ni o i n i c i a l q2
q3_0 = quat ( 4 ) ; %Quat er ni o i n i c i a l q3
%
%Li n e a r i z a o numr i ca das ma t r i z e s A e B
%Vect or com o es t ado i n i c i a l
X_0 = [ w_motor0 U0 V0 W0 P0 Q0 R0 X0_NED Y0_NED Z0_NED q0_0 q1_0 q2_0 q3_0 ] ;
dxdt_0 = model o_si stema (X_0) ; %Es t ados i n i c i a i s
dd=1e 5; J =[ ] ; %Per t ur bao
f o r k =1: 1: 17 %Cons t r uo da mat r i z A e B
X_1=X_0; X_1( k)=X_1( k)+dd ; %Estado + pe r t ur ba o
dxdt_1=model o_si stema (X_1) ; %Der i vada r e s u l t a n t e
J=[ J ( dxdt_1dxdt_0 )/ dd ] ; %Cons t r uo das c ol unas
end %da mat r i z a u x i l i a r
%l i n e a r i z a c a o com os ngul os de e u l e r
f o r k=1:3
X_1=X_0; eul _1=z e r os ( 3 , 1 ) ; eul _1 ( k)=dd ; %Per t ur bao q u a t e r n i e s
q_1=e ul e r 2qua t ( eul _1 ) ; %e q u i v a l e n t e s a e u l e r
X_1(4+9+(1:4))=q_1 ; %Su b s t i t u i o nos e s t ados
dxdt_1=model o_si stema (X_1) ; %Der i vada r e s u l t a n t e
J=[ J ( dxdt_1dxdt_0 )/ dd ] ; %Adi o das novas c ol unas
end
f o r k =1: 1: 16 %Su b s t i t u i o da equao r e f e r e n t e aos
%q u a t e r n i e s
X_0 = [ w_motor0 U0 V0 W0 P0 Q0 R0 X0_NED Y0_NED Z0_NED phi _0 teta_0 ppsi _0 ] ;
X_0( k ) = X_0( k)+ dd ;
[ T] = [ 1 t an (X_0( 15) ) s i n (X_0( 14) ) t an (X_0( 15) ) cos (X_0( 14) ) ;
0 cos (X_0( 14) ) s i n (X_0( 14) ) ;
0 (1/ cos (X_0( 15) ) ) s i n (X_0( 14) ) (1/ cos (X_0( 15) ) ) cos (X_0( 1 4 ) ) ] ;
%Equaes de Eul e r
Euler_num ( 1 : 3 , k ) = (T[ X_0( 8) X_0( 9) X_0( 10) ] / dd ) ;
end
%Cons t r uo das ma t r i z e s numr i cas A e B
A_num( 1 : 9 , 1 : 9 ) = J ( 1 : 9 , 5 : 1 3 ) ; %Ut i l i z a o das c ol unas de J
A_num( 1 : 9 , 1 0 : 1 2 ) = J ( 1 : 9 , 1 8 : 2 0 ) ;
A_num( 10: 12 , 1: 12) = z e r os ( 3 , 1 2 ) ;
A_num( 10: 12 , 1: 12) = Euler_num ( 1 : 3 , 5 : 1 6 ) ; %Mat r i z A numr i ca
B_num( 1 : 9 , 1 : 4 ) = J ( 1 : 9 , 1 : 4 ) ;
B_num( 1 0 : 1 2 , 1 : 4 ) = z e r os ( 3 , 4 ) ; %Mat r i z B numr i ca
%
%Li n e a r i z a o numr i ca das ma t r i z e s C e D
Z=[ ] ;
X_0 = [ w_motor0 U0 V0 W0 P0 Q0 R0 X0_NED Y0_NED Z0_NED phi _0 teta_0 ppsi _0 ] ;
dxdt_0 = s e ns or (X_0) ;
91
f o r k =1: 1: 16 %Cons t r uo da mat r i z C e D
X_1=X_0; X_1( k)=X_1( k)+dd ; %Estado + pe r t ur ba o
dxdt_1=s e ns or (X_1) ; %Der i vada r e s u l t a n t e
Z=[Z ( dxdt_1dxdt_0 ) / dd ] ; %Cons t r uo das c ol unas
end %da mat r i z a u x i l i a r
%Cons t r uo das ma t r i z e s numr i cas C e D
C_num( 1 : 1 4 , 1 : 1 2 ) = Z( 1 : 1 4 , 5 : 1 6 ) ; %Mat r i z C numr i ca
D_num( 1 : 1 4 , 1 : 4 ) = Z( 1 : 1 4 , 1 : 4 ) ; %Mat r i z D numr i ca

model o_si stema .m

f u n c t i o n [ X_ponto ] = model o_si stema ( IN)

% Apl i c a a di nmi ca e a c i ne mat i c a aos e s t ados a c t u a i s do Quadr i r ot or . Si mul a a di nmi ca ,
% e s e r v e par a c o n s t r u i r as ma t r i z e s A e B numr i cas a t r a v s da f uno numer i co .m
% Par met r os de ent r ada :
% IN Vect or com a r ot acao0 e os e s t ados a c t u a i s
% Parmetro de s a da :
% X_ponto Vect or com v a r i a o de cada um dos e s t ados a s e r i nt e g r a do
% Mat r i z Tr ansf or mao
% S Mat r i z de t r ans f or mao , q u a t e r n i e s
% For as e Momentos
% F For a e x e r c i d a por cada motor [ N]
% Fx For a segundo x [ N]
% Fy For a segundo y [ N]
% Fz For a segundo z [ N]
% Mx Momento segundo x [Nm]
% My Momento segundo y [Nm]
% Mz Momento segundo z [Nm]
% Equaes da di nmi ca
% Vl i n e a r Vect or com a v a r i a o das v e l o c i d a d e s l i n e a r e s
% Vr ot acao Vect or com a v a r i a o das v e l o c i d a d e s a ng ul a r e s
% Equaes da Ci nemt i ca
% Pos i cao Vect or com a v a r i a o da pos i o NED
% T Mat r i z de r ot a o
% dq Vect or com a v a r i a o dos q ua t e r n i e s
%
g l o b a l Kf KK m g I Lc Ll
%
%Es t ados a c t u a i s do Quadr i r ot or
w_motor = [ IN ( 1) IN ( 2) IN ( 3) IN ( 4 ) ] ; %r ot a o dos motores , r ad / s
U = IN ( 5 ) ; %Ve l oc i dade Li ne a r segundo X, NED
V = IN ( 6 ) ; %Ve l oc i dade Li ne a r segundo Y, NED
W = IN ( 7 ) ; %Ve l oc i dade Li ne a r segundo Z, NED
P = IN ( 8 ) ; %Ve l oc i dade ngul ar segundo Ex , ABC
Q = IN ( 9 ) ; %Ve l oc i dade ngul ar segundo Ey , ABC
R = IN ( 1 0 ) ; %Ve l oc i dade ngul ar segundo Ez , ABC
X_NED = IN ( 1 1 ) ; %Pos i o segundo x , NED
Y_NED = IN ( 1 2 ) ; %Pos i o segundo y , NED
Z_NED = IN ( 1 3 ) ; %Pos i o segundo z , NED
q0 = IN ( 1 4 ) ; %Quat er ni o q0
q1 = IN ( 1 5 ) ; %Quat er ni o q1
q2 = IN ( 1 6 ) ; %Quat er ni o q2
q3 = IN ( 1 7 ) ; %Quat er ni o q3
%
%Mat r i z Tr ansf or mao
[ S ] = [ q0^2 + q1^2 q2^2 q3^2 2( q1q2 + q0q3 ) 2( q1q3 q0q2 ) ;
2( q1q2 q0q3 ) q0^2 q1^2 + q2^2 q3^2 2( q2q3 + q0q1 ) ;
2( q1q3 + q0q2 ) 2( q2q3 q0q1 ) q0^2 q1^2 q2^2 + q3^2 ] ;
%
%For as e Momentos
f o r i =1: 1: 4
F( i ) = Kf ( w_motor ( i ) ^2) ;
end
Fx = 0; %For a segundo x d e s p r e z a v e l
Fy = 0; %For a segundo y d e s p r e z a v e l
Fz = (F(3)+F(1)+F(4)+F ( 2 ) ) ; %For a segundo z , ne ga t i v a
Mx = (F(1)F( 2) ) Ll ; %Momento segundo x
My = (F(3)F( 4) ) Lc ; %Momento segundo y
Mz = KK(F(3)+F(1)F(4)+F ( 2 ) ) ; %Momento segundo z
%
%Equaes da Di nmi ca
92
%Segunda l e i de Newton
Vl i n e a r = [ Fx Fy Fz ] /m + S[ 0 0 g ] ( [ QWRV RUPW PVQU] ) ;
%Apl i c a o da Le i dos momentos
Vr ot acao = i nv ( I ) ( [ Mx My Mz ] ) + [ ( I (2 ,2) I ( 3 , 3) ) RQ/ I ( 1 , 1) . . .
( I (3 ,3) I ( 1 , 1) ) RP/ I ( 2 , 2) ( I (1 ,1) I ( 2 , 2) ) PQ/ I ( 3 , 3 ) ] ;
%
%Equaes da Ci nemt i ca
%Ci nemt i ca par a a pos i o
Pos i cao = S [ U V W] ;
[ T] = [ 0 P Q R ;
P 0 R Q ;
Q R 0 P ;
R Q P 0 ] ;
%Ci nemt i ca par a a r ot a o
l ambda = 1 [ q0 q1 q2 q3 ] [ q0 q1 q2 q3 ] ;
[ dq ] = ( 1/2)T[ q0 q1 q2 q3 ] + l ambda [ q0 q1 q2 q3 ] ;
%
%Vect or de s a da com os Es t ados
X_ponto = [ Vl i ne ar Vrotacao Posi cao dq ] ;

s e ns or .m

f u n c t i o n [ Y_sensor es ] = s e ns or (X)
% Funo que s i mul a os s e ns or e s , t r s ac e l e r me t r os , magnetometro , e s e ns or de pos i o baseado
% numa cmara embarcada no Quadr i r ot or
% Par met r os de ent r ada :
% X Vect or com a r ot acao0 e os e s t ados a c t u a i s
% Parmetro de s a da :
% Y_sensor es Vect or com as s a d a s dos Se ns or e s
% Mat r i z Tr ansf or mao
% S Mat r i z de t r ans f or mao , Eul e r
% For as e Momentos
% F For a e x e r c i d a por cada motor [ N]
% Fx For a segundo x [ N]
% Fy For a segundo y [ N]
% Fz For a segundo z [ N]
% Mx Momento segundo x [Nm]
% My Momento segundo y [Nm]
% Mz Momento segundo z [Nm]
% Sens or Acel er met r o
% A_sensor Vect or com as a c e l e r a e s t o t a i s [m/ s ^2]
% Sens or Cmara embarcada
% pt s Mat r i z com os pont os de r e f e r n c i a
% NED Pos i o a c t u a l do Quadr i r ot or [ X Y Z phi t e t a p s i ]
% Pos_sensor Vect or com a pr oj e c o dos pont os de r e f e r n c i a na imagem
% Sens or Magnetmetro
% Mag_sensor Vect or com a o r i e nt a o do magnetmetro
%
g l o b a l g Kf KK Ll Lc m I ddx ddy ddz cam L LN f
%
%Es t ados a c t u a i s do Quadr i r ot or
w_motor = [ X( 1) X( 2) X( 3) X( 4 ) ] ; %r ot a o dos motores , r ad / s
U = X( 5 ) ; %Ve l oc i dade Li ne a r segundo X, NED
V = X( 6 ) ; %Ve l oc i dade Li ne a r segundo Y, NED
W = X( 7 ) ; %Ve l oc i dade Li ne a r segundo Z, NED
P = X( 8 ) ; %Ve l oc i dade ngul ar segundo Ex , ABC
Q = X( 9 ) ; %Ve l oc i dade ngul ar segundo Ey , ABC
R = X( 1 0 ) ; %Ve l oc i dade ngul ar segundo Ez , ABC
X_NED = X( 1 1 ) ; %Pos i o segundo x , NED
Y_NED = X( 1 2 ) ; %Pos i o segundo y , NED
Z_NED = X( 1 3 ) ; %Pos i o segundo z , NED
phi = X( 1 4 ) ; %ngul o de r ol ament o
t e t a = X( 1 5 ) ; %ngul o de pi cada
pps i = X( 1 6 ) ; %ngul o de gui nada
%
%Mat r i z Tr ansf or mao
[ S ] = [ cos ( t e t a ) cos ( pps i ) cos ( t e t a ) s i n ( pps i ) . . .
s i n ( t e t a ) ;
( s i n ( phi ) s i n ( t e t a ) cos ( pps i )) ( cos ( phi ) s i n ( pps i ) ) . . .
93
( s i n ( pps i ) s i n ( t e t a ) s i n ( phi ))+( cos ( pps i ) cos ( phi ) ) s i n ( phi ) cos ( t e t a ) ;
( cos ( pps i ) s i n ( t e t a ) cos ( phi ))+( s i n ( pps i ) s i n ( phi ) ) . . .
( cos ( phi ) s i n ( t e t a ) s i n ( pps i )) ( s i n ( phi ) cos ( pps i ) ) cos ( phi ) cos ( t e t a ) ] ;
%
%For as e Momentos
f o r i =1: 1: 4
F( i ) = Kf ( w_motor ( i ) ^2) ;
end
Fx = 0; %For a segundo x d e s p r e z a v e l
Fy = 0; %For a segundo y d e s p r e z a v e l
Fz = (F(3)+F(1)+F(4)+F ( 2 ) ) ; %For a segundo z , ne ga t i v a
Mx = (F(1)F( 2) ) Ll ; %Momento segundo x
My = (F(3)F( 4) ) Lc ; %Momento segundo y
Mz = KK(F(3)+F(1)F(4)+F ( 2 ) ) ; %Momento segundo z
%
%Sens or Acel er met r o
Vl i n e a r = [ Fx Fy Fz ] /m + S[ 0 0 g ] ( [ QWRV RUPW PVQU] ) ;
dU = Vl i n e a r ( 1 ) ; %Ac e l e r a o l i n e a r segundo x , ABC
dV = Vl i n e a r ( 2 ) ; %Ac e l e r a o l i n e a r segundo y , ABC
dW = Vl i n e a r ( 3 ) ; %Ac e l e r a o l i n e a r segundo z , ABC
Vr ot acao = i nv ( I ) ( [ Mx My Mz ] ) + [ ( I (2 ,2) I ( 3 , 3) ) RQ/ I ( 1 , 1) . . .
( I (3 ,3) I ( 1 , 1) ) RP/ I ( 2 , 2) ( I (1 ,1) I ( 2 , 2) ) PQ/ I ( 3 , 3 ) ] ;
dP = Vr ot acao ( 1 ) ; %Ac e l e r a o angul ar segundo x , ABC
dQ = Vr ot acao ( 2 ) ; %Ac e l e r a o angul ar segundo y , ABC
dR = Vr ot acao ( 3 ) ; %Ac e l e r a o angul ar segundo z , ABC
A_sensor = [ dU dV dW] + [ ( dQddz dRddy ) (dRddx dPddz ) ( dPddy dQddx ) ] + . . .
[ (Q(Pddy Qddx ) ) (R(Qddz Pddy ) ) (P(Rddx Pddz ) ) ] . . .
[ ( R(Rddx Pddz ) ) (P(Pddy Qddx ) ) (Q(Qddz Pddy ) ) ] + S[ 0 0 g ] ;
%
%Sens or Cmara embarcada
pt_1_2 = [LN L/2 0 ] ; %ponto de r e f e r n c i a , d i r e i t o
pt_2_1 = [ LN 0 0 ] ; pt_2_2=[LN 0 0 ] ; %pont os de r e f e r n c i a c e n t r a i s
pt_3_2 = [LN L/2 0 ] ; %ponto de r e f e r n c i a , es quer do
pt s =[pt_1_2 1; pt_2_1 1; pt_2_2 1; pt_3_2 1 ] ;
NED = [X_NED Y_NED Z_NED phi t e t a pps i ] ; %Pos i o do Quadr i r ot or
%Pr oj ec o dos pont os de r e f e r n c i a na imagem
i mg_pts = pr oj _pt s (NED, [ cam; f ] , pt s ) ;
i mg_pts ( : , 2 ) = i mg_pts ( : , 2 ) ; %Cor r eco de e i x o
x1 = i mg_pts ( 1 , 1 ) ; x2 = i mg_pts ( 2 , 1 ) ; x3 = i mg_pts ( 3 , 1 ) ; x4 = i mg_pts ( 4 , 1 ) ;
y1 = i mg_pts ( 1 , 2 ) ; y2 = i mg_pts ( 2 , 2 ) ; y3 = i mg_pts ( 3 , 2 ) ; y4 = i mg_pts ( 4 , 2 ) ;
Pos_sensor = [ x1 y1 x2 y2 x3 y3 x4 y4 ] ; %Pos i o xy dos pont os de r e f e r n c i a na
%imagem
%
%Sens or Magnetmetros
Mag_sensor = S[ 1 0 0 ] ; %Vect or com o r i e nt a o do magnetmetro
%
%Vect or com as s a d a s dos Se ns or e s
Y_sensor es = [ A_sensor Pos_sensor Mag_sensor ] ;

pr oj _pt s .m

f u n c t i o n y = pr oj _pt s (NED, cam, pt s )

% Funo que c a l c u l a a pr oj e c o dos pont os de r e f e r n c i a no r e f e r e n c i a l de imagem , Rxyz , com bas e
% na pos i o a c t u a l do Quadr i r ot or e das c a r a c t e r s t i c a s da cmara
% Par met r os de ent r ada :
% NED Pos i o a c t u a l do Quadr i r ot or [ X Y Z phi t e t a p s i ]
% cam Pos i o e a t i t u d e da cmara em r e l a o ao r e f e r e n c i a l ABC
% pt s Mat r i z com os pont os de r e f e r n c i a
% Parmetro de s a da :
% y Vect or com a pos i o x e y da pr oj e c o dos pont os
% Mat r i z e s de t r ans f or ma o e nt r e RNED e Rabc
% t f f 0 Mat r i z de t r ans f or ma o par a phi e nt r e RNED e RABC
% t t t 0 Mat r i z de t r ans f or ma o par a t e t a e nt r e RNED e RABC
94
% t yy0 Mat r i z de t r ans f or ma o par a p s i e nt r e RNED e RABC
% S Mat r i z de t r ans f or ma o e nt r e RNED e RABC
% t f f cam Mat r i z de t r ans f or ma o par a phi e nt r e RABC e Rxyz
% t t t cam Mat r i z de t r ans f or ma o par a t e t a e nt r e RABC e Rxyz
% tyycam Mat r i z de t r ans f or ma o par a p s i e nt r e RABC e Rxyz
% Scam Mat r i z de t r ans f or ma o e nt r e RABC e Rxyz
% ST Mat r i z de t r ans f or ma o e nt r e RNED e Rxyz
% Pos i o e a t i t u d e g l o b a i s da cmara em r e l a o ao r e f e r e n c i a l RNED
% P Pos i o da cmara em r e l a o ao r e f e r e n c i a l RNED [m]
% phi ngul o de r ol ament o [ r ad ]
% t e t a ngul o de pi cada [ r ad ]
% p s i ngul o de gui nada [ r ad ]
% Mat r i z de t r ans f or ma o par a a pr oj e c o dos pont os no r e f e r e n c i a l Rxyz
% M Mat r i z de pr oj e c o dos pont os e nt r e RNED e Rxyz
%
x_cam = cam( 1 ) ; y_cam = cam( 2 ) ; z_cam = cam( 3 ) ; phi_cam = cam( 4 ) ; teta_cam = cam( 5 ) ;
psi _cam = cam( 6 ) ;
f=cam( 7 ) ;
X_0 = NED( 1 ) ; Y_0 = NED( 2 ) ; Z_0 = NED( 3 ) ; phi _0 = NED( 4 ) ; teta_0 = NED( 5 ) ; psi _0 = NED( 6 ) ;
%
%Mat r i z e s de t r ans f or ma o
t f f 0 = [ 1 0 0 ; 0 cos ( phi _0 ) s i n ( phi _0 ) ; 0 s i n ( phi _0 ) cos ( phi _0 ) ] ;
t t t 0 = [ cos ( teta_0 ) 0 s i n ( teta_0 ) ; 0 1 0 ; s i n ( teta_0 ) 0 cos ( teta_0 ) ] ;
t yy0 = [ cos ( psi _0 ) s i n ( psi _0 ) 0 ; s i n ( psi _0 ) cos ( psi _0 ) 0 ; 0 0 1 ] ;
S = t yy0 t t t 0 t f f 0 ; %Mat r i z de t r ans f or ma o e nt r e RNED e RABC
t f f cam = [ 1 0 0 ; 0 cos ( phi_cam) s i n ( phi_cam) ; 0 s i n ( phi_cam) cos ( phi_cam ) ] ;
t t t cam = [ cos ( teta_cam) 0 s i n ( teta_cam ) ; 0 1 0 ; s i n ( teta_cam) 0 cos ( teta_cam ) ] ;
tyycam = [ cos ( psi _cam) s i n ( psi _cam) 0 ; s i n ( psi _cam) cos ( psi _cam) 0 ; 0 0 1 ] ;
Scam = tyycam t t t cam t f f cam ; %Mat r i z de t r ans f or ma o e nt r e RABC e Rxyz
ST = ScamS ; %Mat r i z de t r ans f or ma o e nt r e RNED e Rxyz
%
%Pos i o e a t i t u d e g l o b a i s da cmara em r e l a o ao r e f e r e n c i a l RNED
P = [ X_0; Y_0; Z_0] + S [ x_cam; y_cam; z_cam ] ; %Pos i o da cmara em r e l a o ao r e f e r e n c i a l RNED [m]
phi = at an2 (ST( 2 , 3) , ST( 3 , 3 ) ) ; %ngul o de r ol ament o [ r ad ]
t e t a = a s i n (ST( 1 , 3) / det (ST) ) ; %ngul o de pi cada [ r ad ]
p s i = at an2 (ST( 1 , 2) , ST( 1 , 1 ) ) ; %ngul o de gui nada [ r ad ]
i f t e t a ==0; t e t a=1e 6; end ;
%
%Mat r i z de t r ans f or ma o par a a pr oj e c o dos pont os no r e f e r e n c i a l Rxyz
n_cp=1; %uni dade f o c a l admi t i da como 1
x_prp = P( 1 ) ; y_prp = P( 2 ) ; z_prp = P( 3 ) ;
x_cp=n_cp1e 6; tetacam=0;
M_per = [ 0 0 0 1/n_cp ; 0 1 0 0 ; 0 0 1 0 ; 0 0 0 1 ] ;
T_tr = [ 1 0 0 0; 0 1 0 0; 0 0 1 0; x_prp y_prp z_prp 1 ] ;
B_psi = [ cos ( p s i ) s i n ( p s i ) 0 0 ; s i n ( p s i ) cos ( p s i ) 0 0 ; 0 0 1 0 ; 0 0 0 1 ] ;
B_teta = [ cos ( t e t a ) 0 s i n ( t e t a ) 0 ; 0 1 0 0 ; s i n ( t e t a ) 0 cos ( t e t a ) 0 ; 0 0 0 1 ] ;
B_phi = [ 1 0 0 0 ; 0 cos ( phi ) s i n ( phi ) 0 ; 0 s i n ( phi ) cos ( phi ) 0 ; 0 0 0 1 ] ;
B_tetacam = [ cos ( tetacam ) 0 s i n ( tetacam ) 0 ; 0 1 0 0 ; s i n ( tetacam ) 0 cos ( tetacam ) 0 ; 0 0 0 1 ] ;
T_trcp = [ 1 0 0 0 ; 0 1 0 0 ; 0 0 1 0 ; x_cp 0 0 1 ] ;
%Mat r i z de pr oj e c o dos pont os e nt r e RNED e Rxyz
M_total = T_trB_psi B_tetaB_phi B_tetacamT_trcpM_per ;
%
%Vect or de s a da com a pos i o x e y da pr oj e c o dos pont os
pt s ( : , 1 ) = pt s ( : , 1 ) ; %Cor r eco da pos i o x
pts_p = pt s M_total ; %Pr oj ec o dos pont os
l en_pp = l e ngt h ( pts_p ( : , 1 ) ) ;
f o r i = 1: l en_pp , pts_p ( i , : ) = pts_p ( i , : ) / pts_p ( i , 4 ) ;
end
pts_p ( : , 1 : 3 ) = pts_p ( : , 1 : 3 ) f ; %Apl i c a o da uni dade f o c a l c o r r e c t a
y = pts_p ( 1 : l en_pp , 2 : 3 ) ; %pos i o x e y da pr oj e c o dos pont os

c o n t r o l o .m

f u n c t i o n [ K, KJ , A_kd, B_kd, C_kd , D_kd, MM, A_kdJ , B_kdJ , C_kdJ , D_kdJ ] = c o n t r o l o (A_num, . . .
B_num, C_num, D_num)
% Funo que a t r a v s das ma t r i z e s numr i cas c a l c u l a os ganhos par a o LQR e as ma t r i z e s d i s c r e t a s
% a serem us adas no f i l t r o de kal man par a ambas as s i t ua e s , com J o y s t i c k (2 s e n s o r e s ) , com
95
% cmara i nt e g r a da ( Voo autnomo )
% Parmetro de ent r ada :
% A_num Mat r i z A numr i ca par a es pao de e s t ados
% B_num Mat r i z B numr i ca par a es pao de e s t ados
% C_num Mat r i z C numr i ca par a es pao de e s t ados
% D_num Mat r i z D numr i ca par a es pao de e s t ados
% Parmetro de s a da :
% K Ganho d i s c r e t o LQR ( com a cmara embarcada )
% KJ Ganho d i s c r e t o LQR ( Para o cas o do J o y s t i c k )
% A_kd Mat r i z A d i s c r e t a par a F. Kalman em es pao e s t ados ( cmara )
% B_kd Mat r i z B d i s c r e t a par a F. Kalman em es pao e s t ados ( cmara )
% C_kd Mat r i z C d i s c r e t a par a F. Kalman em es pao e s t ados ( cmara )
% D_kd Mat r i z D d i s c r e t a par a F. Kalman em es pao e s t ados ( cmara )
% MM Mat r i z de i n t e r f a c e com J o y s t i c k
% A_kdJ Mat r i z A d i s c r e t a par a F. Kalman em es pao e s t ados ( J o y s t i c k )
% B_kdJ Mat r i z B d i s c r e t a par a F. Kalman em es pao e s t ados ( J o y s t i c k )
% C_kdJ Mat r i z C d i s c r e t a par a F. Kalman em es pao e s t ados ( J o y s t i c k )
% D_kdJ Mat r i z D d i s c r e t a par a F. Kalman em es pao e s t ados ( J o y s t i c k )
% Si t ua o autnoma
% Q Mat r i z de ponder ao Q par a a p l i c a o do c o n t r o l o moderno
% R Mat r i z de ponder ao R par a a p l i c a o do c o n t r o l o moderno
% K Mat r i z ganho , LQR d i s c r e t o
% G Mat r i z de c o v a r i n c i a
% Qc Mat r i z de c o v a r i n c i a
% Rc Mat r i z de c o v a r i n c i a
% H Mat r i z de c o v a r i n c i a
% s ys 2 Si s t ema em es pao de e s t ados
% KEST Si s t ema em es pao de e s t ados par a o e s t i mador
% Si t ua o c ont r ol a da a t r a v s do j o y s t i c k
% A Nova mat r i z A, 6 es t ados , [ p q r phi t e t a p s i ]
% B Nova mat r i z B, 6 es t ados , [ p q r phi t e t a p s i ]
% C Nova mat r i z C, 6 s a das , [ A_sensor Mag_sensor ]
% D Nova mat r i z D, 6 s a das , [ A_sensor Mag_sensor ]
% QJ Mat r i z de ponder ao Q par a a p l i c a o do c o n t r o l o moderno
% RJ Mat r i z de ponder ao R par a a p l i c a o do c o n t r o l o moderno
% KJ Mat r i z ganho , LQR d i s c r e t o
% C1 Mat r i z par a s a d a s [ u v w r ] par a c ons t r u o da mat r i z MM
% D1 Mat r i z par a s a d a s [ u v w r ] par a c ons t r u o da mat r i z MM
% MM Mat r i z de i n t e r f a c e com J o y s t i c k
% GJ Mat r i z de c o v a r i n c i a
% QcJ Mat r i z de c o v a r i n c i a
% RcJ Mat r i z de c o v a r i n c i a
% HJ Mat r i z de c o v a r i n c i a
% s ys 2J Si s t ema em es pao de e s t ados
% KESTJ Si s t ema em es pao de e s t ados par a o e s t i mador
%
g l o b a l f r
t_s = 1/ f r ;
%
%Si t ua o autnoma
%Mat r i z de ponder ao Q par a a p l i c a o do c o n t r o l o moderno LQR
Q = di ag ( [ 25 , 25 , 25 , 250 , 250 , 250 , 6. 25/10 , 6. 25/10 , 6. 25/10 , 418 , 418 , 418] ) ;
%Mat r i z de ponder ao R par a a p l i c a o do c o n t r o l o moderno LQR
R = (1 e 1) di ag ( [ 1 , 1 , 1 , 1 ] ) ;
K = l q r d (A_num, B_num, Q, R, t_s ) ; %LQR do s i s t ema , Mat r i z K
%F i l t r o de Kalman
dd=(1e 4);
G = eye ( 1 2 , 6 ) ;
Qc = di ag ( [ dd dd dd dd dd dd ] ) ;
Rc = di ag ( [ dd dd dd dd dd dd dd dd dd dd dd dd dd dd ] ) ;
H = 0 eye ( 1 4 , 6 ) ;
s ys 2 = s s (A_num, [ B_num G] , C_num, [ D_num H] ) ; %Passagem es pao e s t ados
[ KEST] = kal md ( sys2 , Qc , Rc , t_s ) ; %F i l t r o de Kalman
[ A_kd, B_kd, C_kd , D_kd] = s s dat a (KEST) ; %Obteno das ma t r i z e s
%
%Si t ua o c ont r ol a da a t r a v s do j o y s t i c k
A( 1 : 3 , 1 : 3 ) = A_num( 4 : 6 , 4 : 6 ) ;
A( 1 : 3 , 4 : 6 ) = A_num( 4 : 6 , 1 0 : 1 2 ) ;
A( 4 : 6 , 1 : 3 ) = A_num( 1 0 : 1 2 , 4 : 6 ) ;
A( 4 : 6 , 4 : 6 ) = A_num( 1 0 : 1 2 , 1 0 : 1 2 ) ; %Nova mat r i z A, 6 e s t ados
B( 1 : 3 , 1 : 4 ) = B_num( 4 : 6 , 1 : 4 ) ;
B( 4 : 6 , 1 : 4 ) = B_num( 1 0 : 1 2 , 1 : 4 ) ; %Nova mat r i z B, 6 e s t ados
96
C( 1 : 3 , 1 : 3 ) = C_num( 1 : 3 , 4 : 6 ) ;
C( 1 : 3 , 4 : 6 ) = C_num( 1 : 3 , 1 0 : 1 2 ) ;
C( 4 : 6 , 1 : 3 ) = C_num( 1 2 : 1 4 , 4 : 6 ) ;
C( 4 : 6 , 4 : 6 ) = C_num( 1 2 : 1 4 , 1 0 : 1 2 ) ; %Nova mat r i z C, 6 s a d a s
D( 1 : 3 , 1 : 4 ) = D_num( 1 : 3 , 1 : 4 ) ;
D( 4 : 6 , 1 : 4 ) = D_num( 1 2 : 1 4 , 1 : 4 ) ; %Nova mat r i z D, 6 s a d a s
%Mat r i z de ponder ao Q par a a p l i c a o do c o n t r o l o moderno LQR
QJ = (1 e 2) di ag ( [ 364 , 364 , 364 , 364 , 364 , 364] ) ;
%Mat r i z de ponder ao R par a a p l i c a o do c o n t r o l o moderno LQR
RJ = (1 e0 ) di ag ( [ 1 0 , 1 0 , 1 0 , 1 0 ] ) ;
KJ = l q r d (A, B, QJ , RJ , t_s ) ; %LQR do s i s t ema , Mat r i z K
%F i l t r o de Kalman
ddJ=(1e 6);
GJ = eye ( 6 , 6 ) ;
QcJ = di ag ( [ ddJ ddJ ddJ ddJ ddJ ddJ ] ) ;
RcJ = di ag ( [ ddJ ddJ ddJ ddJ ddJ ddJ ] ) ;
HJ = 0 eye ( 6 , 6 ) ;
s ys 2J = s s (A, [ B GJ ] , C, [ D HJ ] ) ; %Passagem es pao e s t ados
[ KESTJ ] = kal md ( sys2J , QcJ , RcJ , t_s ) ; %F i l t r o de Kalman
[ A_kdJ , B_kdJ , C_kdJ , D_kdJ ] = s s dat a (KESTJ ) ; %Obteno das ma t r i z e s
C1 ( 1 : 3 , 1 : 3 ) = A_num( 1 : 3 , 4 : 6 ) ;
C1 ( 1 : 3 , 4 : 6 ) = A_num( 1 : 3 , 1 0 : 1 2 ) ;
C1 ( 4 , 1 : 3 ) = A_num( 4 , 4 : 6 ) ;
C1 ( 4 , 4 : 6 ) = A_num( 4 , 1 0 : 1 2 ) ; %Mat r i z par a as 4 s a d a s [ du dv dw dr ]
D1 ( 1 : 3 , 1 : 4 ) = B_num( 1 : 3 , 1 : 4 ) ;
D1( 4 , 1 : 4 ) = B_num( 6 , 1 : 4 ) ; %Mat r i z par a as 4 s a d a s [ du dv dw dr ]
MM = (( 1)C1 i nv (ABKJ)B + D1 ) ; %I n t e r f a c e j o y s t i c k keyboar d
97
Apndice D
Diagrama de blocos
At este ponto, os cheiros usados para a construo das matrizes iniciais, nomeadamente o cheiro
dinamica.m e sensor.m (Apndice C) eram, durante a fase de simulao, utilizados posteriormente
como MATLAB Functions em ambiente SIMULINK. Com a necessidade de construo de um ambiente
em tempo real, estas mesmas funes tiveram de ser transformadas em diagramas de blocos. As guras
que se seguem exemplicam o resultado destas mesmas transformaes.
A dinmica do sistema foi construda atravs de blocos que representassem as variaes dos
estados, Figura 65.
Figura 65: Equaes em diagrama de blocos que denem a dinmica do ALIV
Cada um destes blocos foi construdo com base na modelao adiantada na Seco 3.2.2 e
apresentada no cheiro dinamica.m. Este bloco recebe os estados actuais e a rotao dos motores, e
calcula a variao dos estados globais a serem posteriormente integrados.
Em relao ao cheiro sensor.m, cada um dos sensores do sistema foi desenvolvido num bloco
principal, Figura 66. Este bloco recebe as variveis de entrada dos sensores, e produz as sadas que
sero fornecidas ao ltro de Kalman consoante os sensores utilizados.
Figura 66: Equaes em diagrama de blocos que denem os sensores do ALIV
98
Criao do executvel
Para a criao do executvel necessrio denir os parmetros de resoluo do sistema atravs do
Solver no Conguration Parameters do SIMULINK, Figura 67.
Figura 67: Parmetros adoptados para a resoluo da simulao
Depois necessrio denir a congurao adoptada para a gerao do executvel a partir do
sistema desenvolvido. Para esta situao, foi escolhido o Generic Real-Time Target no Real-Time
Workshop, mais uma vez do Conguration Parameters do SIMULINK, Figura 68.
Figura 68: Congurao adoptada para a gerao do executvel, Generic Real-Time Target
Depois, com o Build do mesmo indicador (Figura 68), procede-se construo do cheiro
executvel que conter a resoluo em tempo real do sistema desenvolvido em ambiente SIMULINK.
99
Real-Time Windows Target
Para realizar a interaco entre o executvel e o modelo desenvolvido em SIMULINK, necessrio
denir no Hardware Implementation do Conguration Parameters as seguinte opes, Figura 69.
Figura 69: Hardware Implementation do Conguration Parameters
e seleccionar desta vez Real-Time Windows Target no Real-Time Workshop, Figura 70.
Figura 70: Congurao adoptada para a gerao do executvel, Real-Time Windows Target
Por m, aps a compilao, para concluir a interaco do Real-Time Windows Target com
o SIMULINK, necessrio correr o modelo em External mode atravs do menu Simulation, e
estabelecer a ligao do mesmo com o modelo criado em tempo real atravs do Connect to Target
do mesmo menu.
100
Apndice E
FLIGHTGEAR
O FLIGHTGEAR um simulador de voo Opensource desenvolvido por elementos independentes
que tem como principal objectivo desenvolver uma plataforma acessvel a todos os interessados atravs
de servios gratuitos sem violao dos direitos de programao. Devido ao facto de ser Opensource,
o FLIGHTGEAR possibilita que cada utilizador desenvolva e altere as conguraes iniciais ou at
mesmo pores de cdigo para o simulador reagir conforme as suas necessidades.
Incorporao do ALIV no FLIGHTGEAR
O FLIGHTGEAR tem a capacidade de conseguir descodicar inmeros cheiros de modelao 3D,
principalmente todos aqueles caractersticos de programas de modelao 3D Opensource. O SOLID-
WORKS no entra nessa categoria, como tal, teve de ser utilizado um add-in do SOLIDWORKS
propicio para este tipo de transformaes, o Dxf Converter for SOLIDWORKS.
Com isto, aps a construo e gravao do modelo no formato apropriado, foi preciso denir o
FLIGHTGEAR para visualiza-lo durante a simulao. Para isso, fez-se uso do modelo do FLIGHT-
GEAR ideal para situaes deste gnero, o UFO. Assim, para que o modelo 3D do ALIV seja o
adoptado durante a simulao, basta iniciar o FLIGHTGEAR com o UFO e associar o cheiro de
congurao inicial do mesmo, ufo.xml, ao novo modelo pretendido.
Aps a incluso do modelo, necessrio corrigir a sua posio e orientao atravs de osets
adicionados ao mesmo cheiro de congurao inicial do UFO. A Figura 71 ilustra as alteraes
efectuadas ao cheiro ufo.xml de modo a que o FLIGHTGEAR abra e alinhe correctamente o modelo
3D desenvolvido do ALIV sempre que se iniciar o simulador.
Figura 71: osets do cheiro ufo.xml
Tipicamente, o FLIGHTGEAR trabalha com cheiros de modelao 3D AC3D, por isso foi utilizado
ainda o programa de modelao 3D Opensource BLENDER[22] para fazer a converso de DXF para
AC3D. Desta forma, foi possvel denir no modelo desenvolvido do ALIV diferentes objectos, entre os
quais, cada um dos rotores, de forma a que os mesmos pudessem ser animados consoante a velocidade
angular atruibuda. Este objectivo conseguido fazendo mais uma vez uso do cheiro ufo.xml e das
denies pr-estabelecidas no FLIGHTGEAR. A Figura 72 apresenta como exemplo as alteraes
necessrias ao cheiro ufo.xml para a obteno da rotao da hlice nmero um. Como pode ser
constatado na mesma gura, o movimento da hlice dado em rotaes por minuto, e est associado
ao motor equivalente.
101
Figura 72: Animao da primeira hlice no FLIGHTGEAR atravs do cheiro ufo.xml
A partir deste ponto, necessrio que este modelo assuma e reaja de acordo com a dinmica
desenvolvida em ambiente SIMULINK.
Interface entre o SIMULINK e o FLIGHTGEAR, Aerospace Toolbox
A Aerospace Toolbox formada por inmeros blocos que permitem ao utilizador desenvolver e
aperfeioar os seus modelos aeronuticos, fazendo uso desses blocos nos mais variadssimos campos,
como a aerodinmica, a propulso e at mesmo na construo de dinmicas completas de aeronaves.
No entanto, como j foi referido, para o caso da presente tese, a importncia desta Toolbox est
associada a outra virtude, a possibilidade da visualizao 3D do movimento da aeronave atravs do
simulador de voo FLIGHTGEAR. Para fazer uso desta propriedade, necessrio adicionar os blocos
provenientes da Aerospace Toolbox para que se possa realizar a interface entre a dinmica desenvolvida
em SIMULINK e o simulador, Figura 73.
Figura 73: Blocos para interface entre o Simulink e o FLIGHTGEAR
O primeiro bloco o Pack net_fdm Packet for FLIGHTGEAR, que cria atravs das entradas
da posio e da orientao um pacote compatvel com uma determinada verso do FLIGHTGEAR.
O segundo bloco o Send net_fdm Packet to FLIGHTGEAR, que transmite o pacote elaborado
anteriormente para um FLIGHTGEAR que esteja a correr no mesmo computador ou num computador
em rede. Ambos os blocos so denidos para serem transmitidos mesma frequncia de simulao.
O terceiro bloco o Generate Run Script, e responsvel por criar os parmetros de iniciao para
o FLIGHTGEAR. Aqui podem ser denidos entre outros, o tipo de aeronave utilizada (no caso da
presente tese o ALIV) e a posio de partida.
Embora sejam estes os blocos principais que possibilitam a gerao da imagem no FLIGHTGEAR
atravs do SIMULINK, necessrio proceder algumas adaptaes extra para que se mantenha uma
102
continuidade entre o inicio dos clculos e a posio inicial adoptada no FLIGHTGEAR. Nomeadamente
converso de coordenadas NED (adoptadas na simulao) para LLA (adoptadas pelo FLIGHT-
GEAR), atravs do bloco Flat Earth to LLA. Para isso, necessrio indicar a posio e orientao
inicial do clculo de acordo com a posio inicial escolhida no bloco Generate Run Script, Figura 74.
Figura 74: Flat Earth to LLA utilizado
Seguindo o mesmo raciocnio, e atribuindo a altitude inicial do clculo ca totalmente congurado
o bloco que envia os dados da dinmica do ALIV para o FLIGHTGEAR sem que este salte da sua
congurao inicial para a posio de clculo, Figura 75.
Figura 75: Modelo completo utilizado na interface entre o Simulink e o FLIGHTGEAR
Na imagem anterior possvel observar como foi indicado anteriormente, a passagem dos valores
referentes rotao dos motores para o FLIGHTGEAR, de modo a que depois, estes possam ser
associados ao modelo 3D das hlices durante a simulao.
103