Professional Documents
Culture Documents
convencional
Engenharia Aeroespacial
Jri
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 filtro 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 fim, com base nos mdulos anteriores, foi construdo um mdulo final 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.
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 final 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
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 finais . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 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 finais . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 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 Draganflyer 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, RN ED . . . . . . . . . . . . . . . . . . . . 63
45 Trajectria do objecto ao longo da simulao no referencial NED, RN ED . . . . . . . 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 definem a dinmica do ALIV . . . . . . . . . . 98
66 Equaes em diagrama de blocos que definem os sensores do ALIV . . . . . . . . . . 98
67 Parmetros adoptados para a resoluo da simulao . . . . . . . . . . . . . . . . . 99
68 Configurao adoptada para a gerao do executvel, Generic Real-Time Target . . 99
69 Hardware Implementation do Configuration Parameters . . . . . . . . . . . . . . 100
70 Configurao adoptada para a gerao do executvel, Real-Time Windows Target . 100
71 offsets do ficheiro ufo.xml . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 101
72 Animao da primeira hlice no FLIGHTGEAR atravs do ficheiro 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 definidos 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 configuraes 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 fins de investigao cientifica, entre muitas outras situaes [2].
Ao longo do tempo surgiram inmeros UAVs, construdos e desenvolvidos de acordo com o fim
a que se propunham. De entre as vrias configuraes, 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.
9
Figura 1: Quadrirotor convencional [4]
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.
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 .
Devido dificuldade 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].
Na Austrlia foi desenvolvido e testado outro Quadrirotor, o X-4 Flyer, Figura 5 [10, 11]. Um
dos testes referiu-se ao estudo da influncia do peso das unidades de medidas inerciais. Para isso,
foi construda uma nova unidade muito mais leve que as comerciais existentes, e verificou-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 fio ao Quadrirotor, mas revelaram ser insuficientes 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.
De entre as modificaes 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 Wifi 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 Draganflyer. 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 especificas 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 Vehicle1 . 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
simplificar o problema, tornando-se posteriormente num ptimo ponto de partida para o projecto
final 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 fivel para uma futura implementao.
Analisando o equipamento do ALIV, verifica-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.
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.
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.
15
segundo a direco e1 , 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
e2 , 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 e2
+ - = = Deslocamento positivo segundo e1
+ + - - Rotao anti-horria
h + + + + Deslocamento negativo segundo e3
Tabela 1: Diferencial de rotao aplicado aos rotores para obteno dos movimentos desejados
Figura 9: ALIV
Um Quadrirotor convencional caracterizado por ter os quatro rotores fixos, 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.
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 e2 para a esquerda ou para a direita, esta pode ser obtida
atravs da rotao do conjunto motor mais hlice segundo o seu eixo transversal e1 (Figura 10).
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 e1 , 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 e2 (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 e1 do conjunto motor mais hlice entre o mesmo par de rotores, Figura
12.
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.
Em termos de sensores, existem trs acelermetros para cada um dos eixos ortogonais, (e1 , e2 , e3 ),
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.
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 fixo, 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
ez e e3 quando estabilizados a apontar para o centro da Terra [17].
O Quadrirotor possui seis graus de liberdade definidos por doze estados. Seis dos doze estados
controlam a atitude do sistema, isto , os ngulos de Euler ( ) e as velocidades angulares no
referencial ABC, RABC , (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, RN ED .
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 :
SE = S S S (1)
o que leva a2 :
2
A deduo pormenorizada das equaes e matrizes apresentadas nesta seco, podem ser consultadas ao longo do
Apndice A.
20
cos cos sin cos sin
SE = cos sin sin cos . sin sin sin sin + cos cos cos sin (2)
cos sin cos + sin sin sin sin cos sin cos cos cos
O modelo matemtico baseado nas equaes do movimento newtoniano e o referencial ABC, RABC ,
considerado o referencial de trabalho porque as unidades sensoriais identificam 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, RABC .
d
FT = [mVT ]B + [mVT ]B (3)
dt
d
MT = [I]B + [I]B (4)
dt
em que VT 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, RABC , atravs de uma transformao de Euler:
h iT h iT
Fg = mSE 0 0 g =m sin cos sin cos cos g (5)
B
e que a fora total exercida no Quadrirotor, FT , constituda pela soma da fora gravtica, Fg , e
da fora de propulso, Fp , gerada pela rotao de cada um dos rotores, a expanso da Equao 3
conduz a:
U 1
Fpx sin QW RV
V = Fpy + g cos sin RU P W (6)
m
Fpz cos cos P V QU
W
Analisando agora a Equao 4 e considerando a massa e inrcia do Quadrirotor constantes, com
Iyz = Ixy = Ixz = 0 a matriz de inrcia torna-se numa matriz principal e a expanso da Equao 4
conduz a:
I P
11 (I33 I22 ) RQ
MT = I22 Q + (I11 I33 ) QP (7)
(I22 I11 ) P Q
I33 R
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, RABC ) dada por r, ento o vector
posio r do Quadrirotor referente ao referencial inercial, RN ED , pode ser escrito como:
21
r =X i +Y j +Z k (8)
r = X i +Y j +Zk (9)
. . .
Sendo assim as componentes da velocidade absoluta em termos do referencial inercial ( X Y Z ) podem
X
U
T
Y = SE V (10)
W
Z
possvel obter a velocidade angular em termos dos ngulos de Euler, no referencial inercial
RN ED , com base nas velocidades de rotao ( P Q R ) do referencial local RABC . Para isso basta
realizar a seguinte transformao matemtica:
P
=T Q (11)
R
Em que T igual a:
1 tan sin tan cos
T = 0 cos sin (12)
0 sec sin sec cos
3.2.2 Quaternies
22
e a matriz de rotao global Sq :
q02 + q12 q22 q32 2(q1 q2 + q0 q3 ) 2(q1 q3 q0 q2 )
Sq = 2(q1 q2 q0 q3 ) q02 q12 + q22 q32 2(q2 q3 + q0 q1 ) (14)
2(q1 q3 + q0 q2 ) 2(q2 q3 q0 q1 ) q02 q12 q22 + q32
.
q0
P q1 q0 q3 q2 .
q1
Q = q2 q3 q0 q1 (16)
q.
2
R q3 q2 q1 q0 .
q3
e verificando que a matriz que lhe caracterstica ortogonal e quando invertida as equaes so
bi-lineares em termos de qi e velocidades angulares, podemos obter a derivada dos quaternies como
sendo [16]:
.
q0 0 P Q R q0 q0
.
q1 1 P 0 R Q q1 1 q1
. = = q (17)
q
2 2 Q R 0 P
q2
2 q2
.
q3 R Q P 0 q3 q3
com base nesta ltima equao e na nova matriz de rotao global Sq , 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.
U 1
Fpx 2(q1 q3 q0 q2 ) QW RV
V = Fpy + g 2(q2 q3 + q0 q1 ) RU P W (18)
m
Fpz q02 q12 q22 + q32 P V QU
W
23
.
P Mx P P
. 1
Q = My Q I Q (19)
. I
R Mz R R
.
q0 q0
.
q1
. = 1 q q1 (20)
q
2 2 q2
.
q3 q3
X U
Y = SqT V (21)
W
Z
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.
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
Pout [W] 28
Pin [W] 56
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:
2000 2
2000rpm = rad/s = Kv
60
= 0
m |V | V0
(22)
= K (V V ) = 2000 (V 9) = 20002
m v 0 60 (V 9) |V | > V0
O funcionamento do motor foi simulado usando o SIMULINK e os blocos existentes para o efeito
so apresentados na Figura 17.
O actuador foi construdo com base na tenso aplicada ao motor tendo o mesmo uma constante
de tempo de sistema de primeira ordem de ts = 20 ms e uma sensibilidade Kv .
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 coeficiente 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.
Com base nesta informao, possvel construir um modelo matemtico caracterstico deste
conjunto atravs das seguintes expresses [18]:
4r4 Ct 2
Fp = m (23)
2
P 4r5 Cp 2
MT = = m (24)
m 3
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:
rC
KK = Cp = 0.0140 m , Mp = KK Fp (27)
t
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.
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
definido:
mm - Massa do motor
27
Para o momento de inrcia Iyy obtm-se:
1
mm x2m + zm
2 + m l2
Iy1 = Iy2 = 12 m l
I = 1 0.060 15 103 2 + 32 103 2 +0.060 0.602 = 0.0216 kg m2
y1 12
(30)
1
mm x2m + zm
2
Iy3 = Iy4 = 12
I = 1 0.060 15 103 2 + 32 103 2 = 6.245 106 kg m2
y3 12
1
mm x2m + zm
2 + m l2
Iz1 = Iz2 = 12 m l
I = 1 0.060 15 103 2 + 32 103 2 +0.060 0.642 = 0.0216 kg m2
z1 12
(32)
1
mm x2m + zm
2 + m l2
Iz3 = Iz4 = 12 m c
I = 1 0.060 15 103 2 + 32 103 2 +0.060 0.602 =0.0246 kg m2
z3 12
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 eficcia
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.
0
Um acelermetro num determinado ponto P mede a componente de acelerao ap dada por [16]:
28
0
ap = ap g (35)
FABC + SE mg .
ap = + ABC r + ABC (ABC r) (36)
m
Esta equao aplicada na modelao dos trs acelermetros, um para cada eixo sendo ap =[apx apy apz ]
faltando adicionar o rudo e os parmetros de ordem elctrica que limitam a gama de trabalho do
acelermetro. Neste caso foram usados os parmetros definidos no datasheet do acelermetro do
ALIV, cedido pelo Eng. Severino Raposo (Tabela 5).
Nmero de bits 10
Resoluo 0.0383g
ymax ymin
4y = (37)
2N
em que ymax corresponde gama de leitura mxima e N ao nmero de bits.
3.5.2 Magnetmetro
Dmag 0
em que o vector (Nmag , Emag , Dmag ) corresponde projeco do vector unitrio inicialmente a
apontar para norte, e S matriz de rotao global definida 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:
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, Rxyz . Para isso, para alm da utilizao da matriz S que define a
rotao entre o referencial inercial, RN ED , e o mvel, RABC , necessrio uma nova matriz Scam que
define a passagem entre este ltimo e o referencial de imagem, Rxyz . A matriz S neste caso igual
matriz SE apresentada na Seco 3.1 e demonstrada no Apndice A. Quanto matriz Scam 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, RN ED , para o da imagem, Rxyz , pode ser dada por:
ST = Scam S (39)
em que a nova atitude pode ser retirada da matriz ST atravs das seguintes equaes:
= atan2 (ST (2, 3) , ST (3, 3))
T
ST (1,3)
T = arcsin det(S = arcsin (ST (1, 3)) (40)
T)
= atan2 (S (1, 2) , S (1, 1))
T T T
Pz ZN ED Zcam
30
" #
h i h i STT 0 h i
QTP 1 = QT 1 = Q 1 M (42)
T T 1
em que Q corresponde matriz com as coordenadas dos pontos de referncia no referencial inercial
RN ED , e QP matriz com a projeco desses mesmos pontos no referencial da imagem, Rxyz .
x
QP = y = SQ T (43)
z
em que juntando a projeco fica:
xi x/z
yi = y/z (44)
1 1
Como foi dito anteriormente, a projeco dos pontos vem no referencial da imagem, Rxyz , 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 QP calculada, foram aplicadas as
seguintes equaes:
x = 1 round (x w)
p w i
(45)
y = 1 round (y h)
p h i
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 eficientes 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
(46)
y = Cx + Du
No entanto para que tal seja possvel, necessrio que as equaes sejam lineares ou linearizadas.
X0 = [U V W P Q R ex ey ez ] = [0 0 0 0 0 0 0 0 h 0 0 0] (47)
f f
x = f (X0 , U0 ) + |X=X0 x + |U =U0 u (48)
X U
Considerando que o estado X definido pela soma de uma perturbao x com a sua posio de
referncia X0 , X = x + X0 e da mesma forma U = u + U0 , a expresso anterior fica:
f f
x = f (X0 , U0 ) + (X X0 ) + (U U0 ) (49)
X U
Resolvendo agora f (X0 , U0 ) para cada uma das equaes do movimento, em que U0 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 (X0 , U0 ) = 0 (50)
f f
x= (X X0 ) + (U U0 ) (51)
X U
Por comparao com a Equao 46 deduz-se a matriz da dinmica:
f f1 f1
1 ...
X1 X2 X12
f2 f2
f X
A= = 1 X2
(52)
.. .. ..
X . . .
..
f12 f12
.
X1 X12 X=X0 ;U =U0
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 = [U1 U2 U3 U4 ]:
f f1 f1
1 ...
U1 U2 U4
f2 f2
f U ... ...
B= = 1 U2
(53)
.. .. ..
U . . .
..
f12 f12
.
U1 U4 X=X0 ;U =U0
Para o clculo da matriz B, assumido que as foras segundo x e y so nulas, Fx ' Fy ' 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:
4
X
Fz = Fi = F1 + F2 + F3 + F4 (54)
i=1
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, KK a constante caracterstica da
modelao definida na Seco 3.3, que relaciona o momento segundo e3 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 u0 que minimiza
o valor de J. No caso usual de uma referncia nula e de um intervalo de tempo semi-infinito, 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:
1
J= (xT Qx + uT Ru)dt (56)
2 0
u0 = Kx (57)
AT P + P A P BR1 B T P + Q = 0 (58)
em ordem a uma matriz P constante, simtrica, semidefinida e positiva, a partir da qual obtida a
matriz de realimentao [17]:
K = R1 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 ts . 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), influenciando no s
a estabilidade como a rapidez do sistema. O mtodo de Bryson sugere uma definio 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]:
1 1
Qii = Rii = (60)
x2i,max u2i,max
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 filtro 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
(61)
y = Cx + Du + v
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 QK = E wwT e RK = E vv T , ficando assim estabelecidos os pressupostos
y = E {Cx + v} = C x (62)
Utilizando a dinmica do sistema da Equao 61, a derivada do erro da dinmica dada por:
.
e = (A LC) x
x e + Gw Lv (63)
ye = C x
e+v (64)
35
O filtro 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 filtro 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 filtro 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 QK e
RK e o perodo de amostragem ts . O mesmo comando devolve um sistema em espao de estados,
KEST, constitudo pelo filtro 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 filtro de Kalman com base nas sadas
provenientes dos sensores a bordo do ALIV. Sendo assim, toda a filosofia adoptada na construo dos
diferentes mdulos de simulao baseou-se no mesmo princpio, como ilustra a Figura 19.
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 filtro de
Kalman e pela realimentao dos estados estimados, para obteno do factor correctivo ideal a aplicar
a cada um dos rotores. Por fim 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 ficheiro5 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 ficheiro 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 finalizar 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.
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 e1 , e2 , e3 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 =(apx , apy , apz , Nmag , Emag , Dmag ).
Atravs nica e exclusivamente destas sadas verificou-se que a translao no observvel, isto
, o filtro 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 filtro 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 e3 .
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
(66)
y = Cx + Du
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=(u1 , u2 , u3 , u4 ).
Considerando a realimentao deste sistema temos que a matriz em anel fechado correspondente
dada por:
38
Af = (A BK) (67)
.
x = (A BK) 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:
em que Cj uma matriz com dimenses [4 6] e Dj 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=(u1 , u2 , u3 , u4 ), caso da matriz Dj . 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 yj = U , V , W , R .
No entanto, como para este caso as sadas provenientes do joystick de acordo com os movimentos
pretendidos so as sadas definidas na Equao 70, de acordo com a Equao 69 estamos interessados
em resolver o seguinte sistema:
.
U
.
V h i
. = Cj (A BK)1 Bu + Dj u = Cj A1 B + Dj u (71)
W f
.
R
. .
u1 U U
. .
u2 h i1
= Cj A1 B + Dj V. = J V. (72)
u f W W
3 . .
u4 R R
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=(u1 , u2 , u3 , u4 ), e yj =
. . .
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.
40
isto x=(u, v, w, p, q, r, x, y, z, , , ), e u=(u1 , u2 , u3 , u4 ). Devido adio das sadas
referentes projeco dos pontos, a matriz C ter dimenses [14 12] e a matriz D [14 4],
y =(apx , apy , apz , x1 , y1 , x2 , y2 , x3 , y3 , x4 , y4 , Nmag , Emag , Dmag ). 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 define o filtro de Kalman (Seco 4), foram atribudas as matrizes
de ponderao Q e R, e as matrizes de covarincia QK e RK que melhor satisfizessem o sistema6 .
Como o filtro 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 filosofia utilizada no desenvolvimento dos mdulos de simulao, tanto o filtro
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 definida no ficheiro
inicial antes de lanar a simulao. A Figura 21 ilustra o raciocnio utilizado na construo deste
mdulo.
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 final de observao mais uma vez definida no ficheiro 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.
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-file 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 definir os parmetros de resoluo do
sistema e proceder posteriormente compilao do executvel (Apndice D).
Um dos problemas do ficheiro 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 definida inicialmente,
o sistema novamente compilado, e por fim 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.
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 ficheiro 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 flexvel.
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 definir 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 definio 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 definir 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 definio do processo de seleco do mdulo de simulao.
Nesta figura 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.
44
Fazendo uso das tcnicas tradicionais de visualizao dos grficos dos estados ao longo do tempo,
verificou-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 grafismos
do FLIGHTGEAR, foi utilizada a ferramenta Aerospace Toolbox [20] contida no MATLAB7 .
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.
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.
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 fim, so discutidos os resultados finais
deste trabalho com base no mdulo integral na Seco 6.3.3.
Como pode ser observado na figura, este primeiro sistema simplesmente constitudo por uma
funo que define 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:
4
X
Fpropi = Kf i2 = 1.1617 105 i2 = Fpropz = Fpropi = 4.6468 105 i2 (73)
i=1
46
0 r r
. Fpropz gm 9.81 1.2
w=0 S 0 = 0 0 = = = 503.3246 rad/s
m 4.6468 105 4.6468 105
g
(74)
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 definir as matrizes A e B que definem 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 ts .
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 ficheiro 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 X0 =[u, v, w, p, q, r, ex , ey , ez , , , ] = [0 0 0
0 0 0 0 0 h 0 0 0], as matrizes A e B calculadas foram (em unidades SI):
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
.. .. .. .. .. .. .. .. .. .. .. ..
. . . . . . . . . . . .
fdinamica
1 0 0 0 0 0 0 0 0 0 0 0
n
A= = (75)
g = 9.81
X
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
2Kf 0
0 0 0 0
m = 0.0097
0 0 0 0
0.0097 0.0097 0.0097 0.0097
2Kf 0 Ll
I(1,1) = 0.1427
0 0 0.1427 0.1427
fdinamica
B= =
U 0.1731 0.1731 0 0
2Kf 0 Lc
0.0018
0.0018 0.0018 0.0018
I(2,2) = 0.1731
0 0 0 0
.. .. .. ..
2Kf KK0 = 0.0018
. . . .
I(3,3)
(76)
Estas matrizes esto de acordo com as frmulas da Seco 4.1 confirmando 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:
umax = vmax = wmax = 0.2 m/s pmax = qmax = rmax = 0.063 rad/s
xmax = ymax = zmax = 0.4 m max = max = max = 0.049 rad (77)
u
1max = u2max = u3max = u4max = 3.16 rad/s
Qii = [25 25 25 250 250 250 6.25 6.25 6.25 418 418 418] Rii = [0.1 0.1 0.1 0.1] (78)
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.
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 X0 . 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 definidos 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.
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).
Atravs do comando kalmd do MATLAB possvel obter a construo de um estimador com base
no filtro 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 QK e RK , e o
perodo de amostragem ts . 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
(79)
y = Cx + Du + v
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 definidas como matrizes Jacobianas das equaes
definidas pelos sensores em ordem a cada um dos estados e entradas dos motores intervenientes na
simulao, respectivamente. O resultado foi:
50
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
fsensores n
(80)
C= = 0 0 0 1 0 0 0 0 2 g = 19.62
X
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
0 0 0.0087 0.0087
2Kf 0
= 0.0097
m
0.0071 0.0071 0 0
fsensores 0.0097 0.0097 0.0097 0.0097
f 0 Ll
D= = 2KI(1,1) ddy = 0.0071
U 0 0 0 0
.. .. .. ..
. . . .
2Kf 0 Lc ddz = 0.0087
0 0 0 0 I(2,2)
(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, QK e RK pode ser finalmente calculado o estimador:
G = eye(12, 6);
H = 0 eye(9, 6);
(82)
dd = 1 104
Q
Kii = [dd dd dd dd dd dd] RKii = [dd dd dd dd dd dd dd dd dd],
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 verificar se o comportamento atravs
da utilizao da estimativa dos estados era semelhante.
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 filtro 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 confirmar o modelo. Isto , como foi explicado
na Seco 4.3, que com o passar do tempo o erro x
e = 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 grficos 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 X0 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 e, ve, w,
e pe, qe, re, ,
e e e e apresentam um pico na altura dessa
mudana da posio de referncia.
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 figura anterior, e comparando o resultado com o obtido na Figura 27,
apesar de no final 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 dificuldade 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 figuras referentes a essa situao no vo ser apresentadas.
Estabilizao numa posio bastante diferente da inicialmente conhecida por parte do estimador
De modo a finalizar esta seco, ainda apresentada uma simulao para demonstrar as capaci-
dades do Quadrirotor e a fiabilidade 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 definida por:
Xi = [u v w p q r x y z q0 q1 q2 q3 ]
X0 = [u v w p q r x y z ]
X0 = [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 grficos anteriores possvel observar a rapidez com que o Quadrirotor conseguiu
estabilizar numa posio final 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 finais.
Aps a validao independente dos dois mdulos anteriores, apresentado os resultados do mdulo
integrado, responsvel pela simulao final adoptada para o controlo verstil e integral do MALIV8 .
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 filosofia 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, exemplificativa da filosofia adoptada na construo deste mdulo, o
estimador composto pelo filtro 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 magnetmetro9 .
Figura 39: Controlador em diagrama de blocos adoptado para o mdulo com joystick
A nova matriz de ganho K e o filtro 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 ficheiro controlo.m inserido no Apndice C
58
Com desta vez:
Qii = 1 102 [364 364 364 364 364 364] Rii = [10 10 10 10]
pmax = qmax = rmax = 0.52 rad/s u1max = u2max = u3max = u4max = 0.32 rad/s
max = max = max = 0.52 rad
(85)
QKii = [dd dd dd dd dd dd] RKii = [dd dd dd dd dd dd]
H = 0 eye(6, 6);
G = eye(6, 6)
De modo a ilustrar a validade do modelo desenvolvido, optou-se por definir uma trajectria e
verificar se se conseguia efectu-la com o controlo do joystick. Isto , procurou ser verificado a
estabilidade e versatilidade do MALIV neste mdulo de simulao uma vez j se ter confirmado que
tanto a dinmica como o estimador faziam o Quadrirotor reagir como o esperado.
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:
59
6. Voltar posio inicial com x = y = 0
A Figura 40 procura ilustrar o resultado da posio do MALIV em termos de xyz ao longo do tempo
durante a simulao:
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.
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 identificar e
verificar 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.
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
+ 9 = 11.4032 V
Kv
onde Kv corresponde ao factor de converso entre tenso e velocidade e 9 ao fim 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.
62
um UAV deste tipo poder vir a ter.
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 magnetmetro10 .
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
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
fsensores 0 0 0.3577 0 0.1018 0 2.7072 0
C= =
X
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:
0 0 0.0052 0.0052
0.0043 0.0043 0 0
fsensores 0.0105 0.0090 0.0097 0.0097
D= = (87)
U 0 0 0 0
.. .. .. ..
. . . .
0 0 0 0
A partir daqui, tanto a matriz de ganho K como o filtro 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]);
Com11 :
Qii = [ 25 25 25 250 250 250 6.25/10 6.25/10 6.25/10 418 418 418] Rii = [0.1 0.1 0.1 0.1]
H = 0 eye(14, 6);
G = eye(12, 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 xmax = ymax = zmax 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 definida 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 definida no ficheiro 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 :
Ao contrrio do mdulo com joystick, em que existe a liberdade por parte do utilizador em efectuar
qualquer trajectria, neste mdulo a altitude fixa e definida no ficheiro 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 filtro de Kalman procure anular o erro entre a posio dos pontos
de referncia e a sua posio actual. Desta forma, possvel verificar 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. h i
Para efectuar esta simulao, o movimento dos pontos com coordenadas Qx Qy no refer-
encial fixo foi associado a trs botes do joystick, com trs funes dependentes do tempo:
" # " #
Qx Qx
= + Tt (89)
Qy Qy
65
Figura 45: Trajectria do objecto ao longo da simulao no referencial NED, RN ED
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 verificar se o MALIV consegue acompanh-lo.
Em termos da posio segundo os trs eixos inerciais o resultado foi o seguinte, Figura 46.
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 grficos referentes aos motores
e s velocidades para esta situao, uma vez essa teoria j ter sido amplamente discutida. Aqui,
o importante era confirmar se o MALIV conseguiria manter uma altitude estabilizada e vigiar um
objecto animado, algo que j foi comprovado.
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 justificaes adiantadas nas seces anteriores, quando se justificaram 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.
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.
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 solidificado para os fins a que se prope, estando assim renas as condies para a elaborao
do mdulo final: o 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 final 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 final sero j apresentados os resultados para os diferentes tipos de misses
consideradas.
Uma vez se tratar do mdulo final, 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 exemplificar as
capacidades do Quadrirotor desenvolvido em realizar objectivos destinados a um UAV deste tipo.
Misso n1: A primeira misso serve para verificar 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 final no ficheiro inicial,
inicio.m, e depois de correr o ficheiro 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.
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
definida inicialmente (5 m). Como pode ser observado atravs da Figura 51, desta vez, e como foi
justificado 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 final desejada
antes de se estabilizar. O objectivo desta misso foi cumprido, e conseguiu-se verificar a validade do
70
simulador construdo tendo em conta a permuta entre os dois controladores.
Misso n2: Esta misso consiste na verificao 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 RN ED
ao longo da simulao, e o throttle do joystick para a seleco do controlador envolvido, ao longo
do tempo.
71
Como pode ser observado nas figuras 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 fim de se conseguir estabilizar, o mesmo possua a posio final 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.
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.
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.
5. Perseguio do objecto aps paragem, segundo uma trajectria rectilnea e de forma autnoma
7. Perseguio do objecto aps paragem, segundo uma nova trajectria rectilnea e de forma
autnoma
74
A Figura 59 representa a trajectria efectuada pelo objecto em termos de xy no referencial RN ED .
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 fim 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 verificar 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.
O que se verifica 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 final 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 final 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 final 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.
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 filosofia adoptadas possuem. Sendo que, houve a preocupao de acabar as presentes discusses
com situaes que se poderiam reflectir 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 justificasse.
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 verificaram-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 simplificaes com o intuito de conseguir construir um
sistema mais adequado s necessidades, a soluo final demonstrou ser bastante satisfatria, dado
que vrios objectivos foram sendo ultrapassados com a finalidade de aproximar cada vez mais a
simulao de um ambiente real. Com isto, no s se conseguiram apresentar diferentes mdulos de
simulao, como no fim 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.
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
fim, 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] Office of the Secretary of Defence Unmanned Aircraft Systems Roadmap: 2005-2030, 2008
[6] Tarek Hamel, Robert Mahony, Rogelio Lozano, & James Ostrowski. Dynamic Modelling and
Configuration 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-flyer. Em ICCA, Budapeste, Hungria, 2005
[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 Hoffmann, 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
82
Apndice A
Nesta seco sero apresentados com detalhe todos os passos envolvidos na obteno das equaes
que definem a dinmica do problema.
1 0 0 cos 0 sin cos sin 0
S = 0 cos sin S = 0 1 S = sin
0 cos 0
S = S S S
cos cos sin cos sin
SE = cos sin sin cos sin sin sin sin + cos cos cos sin
cos sin cos + sin sin sin sin cos sin cos cos cos
Dinmica
FT = d [mVT ]B + [mVT ]B
dt
MT = d [I]B + [I]B
dt
Dado que a massa constante, = [P Q R] e V T = [U V W ] ento:
h iT h iT QW RV
P Q R U V W = RU P W
P V QU
U
QW RV
FT = m V + m RU P W
P V QU
W
h iT sin
SE 0 0 g = cos sin g
cos cos B
U 1
Fpx sin QW RV
V = Fpy + g cos sin RU P W
m
Fpz cos cos P V QU
W
83
F
U = mpx g sin QW + RV
Fpy
V = m + g sin cos RU + P W
F
W = mpz + g cos cos P V + QU
I11 0 0
Para a equao dos momentos, I uma matriz principal, I = 0 I22 0
0 0 I33
h i
[I] = (I33 I22 ) RQ (I11 I33 ) QP (I22 I11 ) P Q
I 11 P (I33 I22 ) RQ
MT = I22 Q + (I11 I33 ) QP
(I22 I11 ) P Q
I33 R
P =M x
I11
(I33 I22 )
I11 RQ
M (I11 I33 )
Q= I y I22 QP
22
R= M z
I33
(I22 I11 )
I33 PQ
r = X i +Y j +Z k
r = X i +Y j +Zk
X U
T
Y = SE V
W
Z
X=cos cos V + (cos sin sin cos sin ) V +(cos sin cos +sin sin )W
Y =sin cos U +(sin sin sin +cos cos )V +(sin sin cos sin cos )W
Z= sin U +cos sin V + cos cos W
84
Cinemtica, para os ngulos de Euler:
1 tan sin tan cos P
T = 0 cos sin = = T Q
0 sec sin sec cos R
= P + Q tan sin + R tan cos
= Q cos R sin
= Q sec sin + R sec cos
Partindo dos ngulos de Euler iniciais so obtidos os quaternies para o inicio da simulao:
q0 = (cos (/2) cos (/2) cos (/2) + sin (/2) sin (/2) sin (/2))
q1 = (sin (/2) cos (/2) cos (/2) cos (/2) sin (/2) sin (/2))
q2 = (cos (/2) sin (/2) cos (/2) + sin (/2) cos (/2) sin (/2))
q3 = (cos (/2) cos (/2) sin (/2) sin (/2) sin (/2) cos (/2))
Da equao da cinemtica:
. . . .
P = 2(q0 q1 + q3 q2 q2 q3 q1 q0 )
. . . .
Q = 2(q3 q1 + q0 q2 + q1 q3 q2 q0 )
R = 2(q q. q q. + q q. q q. )
2 1 1 2 0 3 3 0
.
q0
P q1 q0 q3 q2 .
q1
Q = 2 q2 q3 q0 q1
q.
2
R q3 q2 q1 q0
.
q3
Do facto da matriz ser ortogonal e quando invertida as equaes serem bi-lineares em termos de
qi e velocidades angulares:
. 1 . 1
q0 = (P q1 + Qq2 + Rq3 ) q1 = (P q0 Rq2 + Qq3 )
2 2
. 1 . 1
q2 = (Qq0 + Rq1 P q3 ) q3 = (Rq0 Qq1 + P q2 )
2 2
85
Apndice B
Acelermetro
0
Um acelermetro num determinado ponto P mede a componente de acelerao ap dada por:
0
ap = ap 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:
VP = Vcg + r
d (Vcg ) d ( r)
aP = +
dt dt
Esta ltima equao pode ser expressa em termos do referencial do corpo ABC, RABC , 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:
FABC + SE mg .
ap = + ABC r + ABC (ABC r)
m
Cmara embarcada
Seguindo o mesmo princpio para o clculo da matriz S, a matriz Scam clculada atravs das
seguintes matrizes:
1 0 0 cos 0 sin cos sin 0
S camx = 0 cos sin Scamy = 0 1 0 Scamz = sin cos 0
0 sin cos sin 0 cos 0 0 1
Atendendo s equaes:
Px XN ED Xcam
T
Py = YN ED + S Ycam
Pz ZN ED Zcam
86
= atan2 (ST (2, 3) , ST (3, 3))
ST (1,3)
= arcsin det(ST ) = arcsin (ST (1, 3))
= atan2 (ST (1, 2) , ST (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,
Rxyz :
0 0 0 1/f 1 0 0 0 1 0 0 0
0 1 0 0 0 1 0 0 0 1 0 0
Mper =
0 0 1
; Ttr = ; Ttrcp =
0
0
0 1 0
0 0 1 0
0 0 0 1 Px Py Pz 1 f 1 106 0 0 1
1 0 0 0 cos 0 sin 0 cos sin 0 0
0 cos sin 0 0 1 0 0 sin cos 0 0
S =
S =
S =
0 sin cos 0
sin 0 cos 0
0
0 1 0
0 0 0 1 0 0 0 1 0 0 0 1
o que finalmente leva a que a projeco dos pontos seja dada por:
QP = QM
em que Q corresponde matriz com as coordenadas dos pontos de referncia no referencial fixo,
RN ED :
2 1 0 1
2 0 0 1
Q=
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
%R u n S c r i p t que i n i c i a l i z a a s p r i n c i p a i s c o n s t a n t e s u t i l i z a d a s no s i s t e m a
% Parmetros 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 ]
% ro d e n s i d a d e do a r [ g /m^ 3 ]
% ts amostragem [ s ]
% fr frequncia
% P a r m e t r o s e s t r u t u r a i s , ALIV
% m massa [ kg ]
% mm Massa motor [ Kg ]
% xm Comprimento do motor s e g u n d o x [m]
% ym Comprimento do motor s e g u n d o y [m]
% zm Comprimento do motor s e g u n d o z [m]
% Ixx Momento de i n r c i a t o t a l s e g u n d o x [ Kg .m^ 2 ]
% Iyy Momento de i n r c i a t o t a l s e g u n d o y [ Kg .m^ 2 ]
% Izz Momento de i n r c i a t o t a l s e g u n d o z [ Kg .m^ 2 ]
% I Matriz s i m t r i c a i n e r c i a l
% Lc D i s t n c i a do CG ao R o t o r ( c o m p r i m e n t o ) [m]
% Ll D i s t n c i a do CG ao R o t o r ( l a r g u r a ) [m]
% P a r m e t r o s e s t r u t u r a i s motor+h l i c e , ALIV
% Cp C o e f i c i e n t e de p r e s s o
% Ct C o e f i c i e n t e de p r o p u l s o
% r _ h e l i c e R a i o do H l i c e [m]
% Vmax V o l t a g e m mxima do motor [ V o l t s ]
% RPM_max r o t a o mxima do motor [ rpm ]
% rad_max r o t a o mxima do motor [ r a d / s ]
% Kf F=Kf (w^2) r e l a o e n t r e F o r a e r o t a o a p l i c a d a
% Km M=Km (w^3) r e l a o e n t r e momento e r o t a o a p l i c a d a
% KK r e l a o e n t r e Kf e Km
% Kv relo entre tenso e rotao aplicada
% Vmin Tenso minima ou z o n a m o r t a [ V o l t s ]
% TVOLT V o l t s a p l i c a d o s a c a d a motor t a l que dw = 0 [ V o l t s ]
% r o t a c a o 0 r o t a 1 2 0 o a p i c a d a a c a d a motor t a l que dw = 0 [ r a d / s ]
% Parmetros i n i c i a i s
% Xini Condio i n i c i a l dos e s t a d o s
% Xini2 C o n d i o i n i c i a l d o s e s t a d o s no e s t i m a d o r
% Parmetros dos s e n s o r e s
% ddx D i s t n c i a do a c e l e r m e t r o em r e l a o ao CG , s e g u n d o x [m]
% ddy D i s t n c i a do a c e l e r m e t r o em r e l a o ao CG , s e g u n d o y [m]
% ddz D i s t n c i a do a c e l e r m e t r o em r e l a o ao CG , s e g u n d o z [m]
% cam P o s i o da cmara , r e f e r e n c i a l ABC : [ x , y , z , p h i , t e t a , p s i ]
% n_cp Distncia focal i n i c i a l
% L D i s t n c i a e n t r e l i n h a s [m]
% LN D i s t n c i a e n t r e l i n h a s [m]
% f F o c a l da cmara
% pt_1_2 Ponto de r e f e r n c i a , l a d o 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 a d o e s q u e r d o
% noise R u d o a s s o c i a d o s m e d i e s do m a g n e t m e t r o
% n nmero de b i t s
% ymin V a l o r de s a d a mximo p a r a o m a g n e t m e t r o
% ymax V a l o r de s a d a mnimo p a r a o m a g n e t m e t r o
% r e s o l u c a o Gama de r e s o l u o
% V e c t o r e s com o s e s t a d o s i n i c i a i s
% X_0J V e c t o r com o s e s t a d o s i n i c i a i s ( c a s o do J o y s t i c k )
% X_0 V e c t o r com o s e s t a d o s i n i c i a i s ( c a s o da cmara )
% XU_0 V e c t o r com a r o t a c a o 0 e o s e s t a d o s i n i c i a i s de r e f e r n c i a
% U_0 V e c t o r com a r o t a c a o 0
% Y_0 V e c t o r com a s s a d a s d o s t r s s e n s o r e s
% Y_0J V e c t o r com a s s a d a s do a c e l e r m e t r o e m a g n e t m e t r o
%
clear all
g l o b a l w_motor0 Kf KK m g I L l Lc fr r o t a c a o 0 ddx ddy ddz cam L LN f
syms TVOLT
%
%P a r m e t r o s F s i c o s
g = 9.81; %A c e l e r a o g r a v i t i c a [m/ s ^ 2 ]
ro = 1.225; %D e n s i d a d e do a r [ Kg/m^ 3 ]
ts = 0.02; %Amostragem [ ms ]
f r = 16; %F r e q u n c i a [ Hz ]
88
%P a r m e t r o s e s t r u t u r a i s , ALIV
m = 1.2; %Massa t o t a l [ Kg ]
mm = 0 . 0 6 0 ; %Massa motor [ Kg ]
xm = 0 . 0 1 5 ; %Comprimento do motor s e g u n d o x [m]
ym = 0 . 0 1 5 ; %Comprimento do motor s e g u n d o y [m]
zm = 0 . 0 3 2 ; %Comprimento do motor s e g u n d o z [m]
Lc = 0 . 6 4 ; %D i s t n c i a do CG ao R o t o r ( c o m p r i m e n t o )
Ll = 0.6; %D i s t n c i a do CG ao R o t o r ( l a r g u r a )
I y 3 = ( 1 / 1 2 ) mm ( xm^2 + zm ^ 2 ) ; I y 4 = I y 3 ;
I y 1 = I y 3 + (mm ( L l ^ 2 ) ) ; I y 2 = I y 1 ;
Iyy = Iy1 + Iy2 + Iy3 + Iy4 ; %Momento de inercia total segundo y
%
%P a r m e t r o s e s t r u t u r a i s motor+h l i c e , ALIV
Cp = 0 . 0 3 7 4 ; %C o e f i c i e n t e de P r e s s o
Ct = 0 . 1 0 4 7 ; %C o e f i c i e n t e de p r e p u l s o
r_helice = 0.2458/2; %R a i o do H l i c e [m] = d i a m e t r o 9
Vmax = 1 2 ; %V o l t a g e m mxima do motor
RPM_max = 6 0 0 0 ; %r o t a o mxima do motor [ rpm ]
%V a l o r c o n s t a n t e que r e l a c i o n a R o t a o e a F o r a a p l i c a d a ao motor
Kf = d o u b l e ( 4 r o ( r _ h e l i c e ^4) Ct / ( p i ^ 2 ) ) ; %F=Kf (w^2)
%R e l a c i o n a Kf e Km
KK = d o u b l e ( r _ h e l i c e Cp / ( Ct p i ) ) ;
rad_max = d o u b l e (RPM_max2 p i / 6 0 ) ; %RPM mximo do motor em r a d / s
Kv = d o u b l e ( 2 0 0 0 2 p i / 6 0 ) ; %r e l a o e n t r e t e n s o e v e l o c i d a d e
Vmin = d o u b l e ( Vmax rad_max /Kv ) ; %t e n s a o minima ou z o n a m o r t a
%V e l o c i d a d e de r o t a o a p l i c a d o s a c a d a motor t a l que dw = 0
r o t a c a o 0 = d o u b l e ( Kv (TVOLTVmin ) ) ; %V o l t s para rad / s
w_motor0 = [ r o t a c a o 0 r o t a c a o 0 r o t a c a o 0 r o t a c a o 0 ] ;
%A p r o x i m a o i n i c i a l : R e l a o t e n s o / r o t a o n o s m o t o r e s
wtoN = 2 Kf r o t a c a o 0 ; %M o t o r e s idnticos
%
%P a r m e t r o s i n i c i a i s
X i n i = [ 0 0 0 0 0 0 0 0 5 1 0 0 0 ] ; %C o n d i e s iniciais para X
Xini2 = [0 0 0 0 0 0 0 0 0 0 0 0 ] ; %C o n d i e s iniciais para estimador
%E s t a d o s i n i c i a i s p a r a a p o s i o de referncia
U0 = 0 ; %V e l o c i d a d e L i n e a r s e g u n d o X , NED
V0 = 0 ; %V e l o c i d a d e L i n e a r s e g u n d o Y , NED
W0 = 0 ; %V e l o c i d a d e L i n e a r s e g u n d o Z , NED
P0 = 0 ; %V e l o c i d a d e n g u l a r s e g u n d o Ex , ABC
Q0 = 0 ; %V e l o c i d a d e n g u l a r s e g u n d o Ey , ABC
R0 = 0 ; %V e l o c i d a d e n g u l a r s e g u n d o Ez , ABC
X0_NED = 0 ; %P o s i o s e g u n d o X , NED
Y0_NED = 0 ; %P o s i o s e g u n d o Y , NED
Z0_NED = 7; %P o s i o s e g u n d o Z , NED
phi_0 = 0 ; % n g u l o de R o l a m e n t o
teta_0 = 0 ; % n g u l o de P i c a d a
ppsi_0 = 0 ; % n g u l o de G u i n a d a
q0_0 = 1 ; %Q u a t e r n i o i n i c i a l q0
q1_0 = 0 ; %Q u a t e r n i o i n i c i a l q1
q2_0 = 0 ; %Q u a t e r n i o i n i c i a l q2
q3_0 = 0 ; %Q u a t e r n i o i n i c i a l q3
save ( constantes ) ; %G u a r d a o v a l o r d a s c o n s t a n t e s
89
%
%P a r m e t r o s d o s s e n s o r e s
n o i s e = 1 e 3;
n = 10; %nmero de b i t s
ymin = 2g ; %V a l o r de s a d a mximo
ymax = 2 g ; %V a l o r de s a d a mnimo
r e s o l u c a o = ( ymax ymin ) / ( 2 ^ n ) ; %Gama de r e s o l u o
%A c e l e r m e t r o
ddx = 0 ; %D i s t n c i a em r e l a o ao CG s e g u n d o x
ddy = 0 . 0 0 5 ; %D i s t n c i a em r e l a o ao CG s e g u n d o y
ddz = 0 . 0 3 ; %D i s t n c i a em r e l a o ao CG s e g u n d o z
%Cmara e m b a r c a d a
w=800/2;
hh =600/2;
deg = p i / 1 8 0 ;
n_cp = 1 ;
cam = [ . 0 1 0 0 . 0 1 0 90 deg 0 ] ; %P o s i o da cmara , ABC
L = 2; %d i s t n c i a e n t r e a s l i n h a s e s q e dir
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 ] ; %p o n t o 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 ] ; %p o n t o s 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 ] ; %p o n t o de r e f e r n c i a , e s q u e r d o
tx = 1 . 5 ;
ty = 1 . 5 ;
%
%V e c t o r e s com o s e s t a d o s i n i c i a i s , u s a d o s p a r a a c o n s t r u o d a s m a t r i z e s n u m r i c a s
%V e c t o r com o s e s t a d o s i n i c i a i s ( c a s o do J o y s t i c k )
X_0J = [ P0 Q0 R0 phi_0 t e t a _ 0 p p s i _ 0 ] ;
%V e c t o r com o s e s t a d o s i n i c i a i s ( c a s o da cmara )
X_0 = [ U0 V0 W0 P0 Q0 R0 X0_NED Y0_NED Z0_NED phi_0 t e t a _ 0 p p s i _ 0 ] ;
%V e c t o r com o s e s t a d o s i n i c i a i s m a i s a r o t a o de c a d a 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 t e t a _ 0 p p s i _ 0 ] ;
%
%C o n s t r u o d a s m a t r i z e s n u m r i c a s , m a t r i z e s de ganho e de k a l m a n p a r a ambos o s c a s o s , mdulo com
%j o y s t i c k e mdulo de v i s u a l i z a o
%M a t r i z e s n u m r i c a s do s i s t e m a p a r a p o s i o r e f e r n c i a
[ A_num, B_num, C_num , D_num ] = n u m e r i c o (XU_0 ) ;
%M a t r i z e s de ganho e de Kalman p a r a c o n s 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 ) ;
n u m e r i c o .m
% P a r m e t r o s de e n t r a d a :
% XU_0 V e c t o r com a r o t a c a o 0 e o s e s t a d o s i n i c i a i s de referncia
% P a r m e t r o de s a d a :
% A_num M a t r i z A n u m r i c a p a r a e s p a o de e s t a d o s
% B_num M a t r i z B n u m r i c a p a r a e s p a o de e s t a d o s
% C_num M a t r i z C n u m r i c a p a r a e s p a o de e s t a d o s
% D_num M a t r i z D n u m r i c a p a r a e s p a o de e s t a d o s
% L i n e a r i z a o numrica das m a t r i z e s A e B
% J Matriz Jacobiana
% XU_ 0 V e c t o r com a r o t a c a o 0 e o s e s t a d o s i n i c i a i s de referncia
% dxdt_0 E s t a d o s i n i c i a i s p a r a v a l o r e s de r e f e r n c i a
% dd Perturbao
% X_1 V e c t o r com p e r t u r b a o numa dada r e f e r n c i a
90
% dxdt_1 V e c t o r com s t a d o s a p s p e r t u r b a o num v a l o r de r e f e r n c i a
% L i n e a r i z a o numrica das m a t r i z e s C e D
% Z matriz Jacobiana
% dxdt_0 S a d a s i n i c i a i s p a r a v a l o r e s de r e f e r n c i a
% X_1 V e c t o r com p e r t u r b a o numa dada r e f e r n c i a
% dxdt_1 V e c t o r com s a d a s a p s p e r t u r b a o num v a l o r de r e f e r n c i a
%
%P a r m e t r o s i n i c i a i s , de r e f e r n c i a
%V a l o r e s de r e f e r n c i a
U0 = XU_0 ( 5 ) ; %V e l o c i d a d e L i n e a r s e g u n d o X , NED
V0 = XU_0 ( 6 ) ; %V e l o c i d a d e L i n e a r s e g u n d o Y , NED
W0 = XU_0 ( 7 ) ; %V e l o c i d a d e L i n e a r s e g u n d o Z , NED
P0 = XU_0 ( 8 ) ; %V e l o c i d a d e n g u l a r s e g u n d o Ex , ABC
Q0 = XU_0 ( 9 ) ; %V e l o c i d a d e n g u l a r s e g u n d o Ey , ABC
R0 = XU_0 ( 1 0 ) ; %V e l o c i d a d e n g u l a r s e g u n d o Ez , ABC
X0_NED = XU_0 ( 1 1 ) ; %P o s i o s e g u n d o X , NED
Y0_NED = XU_0 ( 1 2 ) ; %P o s i o s e g u n d o Y , NED
Z0_NED = XU_0 ( 1 3 ) ; %P o s i o s e g u n d o Z , NED
phi_0 = XU_0 ( 1 4 ) ; % n g u l o de R o l a m e n t o
t e t a _ 0 = XU_0 ( 1 5 ) ; % n g u l o de P i c a d a
p p s i _ 0 = XU_0 ( 1 6 ) ; % n g u l o de G u i n a d a
%
%L i n e a r i z a o n u m r i c a d a s m a t r i z e s A e B
%V e c t o r com o e s t a d o 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 = m o d e l o _ s i s t e m a (X_0 ) ; %E s t a d o s i n i c i a i s
dd=1e 5; J = [ ] ; %P e r t u r b a o
f o r k =1:1:17 %C o n s t r u o da m a t r i z A e B
X_1=X_0 ; X_1( k)=X_1( k)+dd ; %E s t a d o + p e r t u r b a o
dxdt_1=m o d e l o _ s i s t e m a (X_1 ) ; %D e r i v a d a r e s u l t a n t e
J =[ J ( dxdt_1dxdt_0 ) / dd ] ; %C o n s t r u o d a s c o l u n a s
end %da m a t r i z a u x i l i a r
%l i n e a r i z a c a o com o s n g u l o s de e u l e r
f o r k =1:3
X_1=X_0 ; e u l _ 1=z e r o s ( 3 , 1 ) ; e u l _ 1 ( k)=dd ; %P e r t u r b a o q u a t e r n i e s
q_1=e u l e r 2 q u a 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 ; %S u b s t i t u i o n o s e s t a d o s
dxdt_1=m o d e l o _ s i s t e m a (X_1 ) ; %D e r i v a d a r e s u l t a n t e
J =[ J ( dxdt_1dxdt_0 ) / dd ] ; %A d i o d a s n o v a s c o l u n a s
end
%E q u a e s de E u l e r
Euler_num ( 1 : 3 , k ) = (T [ X_0 ( 8 ) X_0 ( 9 ) X_0 ( 1 0 ) ] / dd ) ;
end
%C o n s t r u o d a s m a t r i z e s n u m r i c a s A e B
A_num ( 1 : 9 , 1 : 9 ) = J ( 1 : 9 , 5 : 1 3 ) ; %U t i l i z a o d a s c o l u n a s de J
A_num ( 1 : 9 , 1 0 : 1 2 ) = J ( 1 : 9 , 1 8 : 2 0 ) ;
A_num ( 1 0 : 1 2 , 1 : 1 2 ) = z e r o s ( 3 , 1 2 ) ;
A_num ( 1 0 : 1 2 , 1 : 1 2 ) = Euler_num ( 1 : 3 , 5 : 1 6 ) ; %M a t r i z A n u m r i c a
B_num ( 1 : 9 , 1 : 4 ) = J ( 1 : 9 , 1 : 4 ) ;
B_num ( 1 0 : 1 2 , 1 : 4 ) = z e r o s ( 3 , 4 ) ; %M a t r i z B n u m r i c a
%
%L i n e a r i z a o n u m r i c a d a s m a 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 t e t a _ 0 p p s i _ 0 ] ;
dxdt_0 = s e n s o r (X_0 ) ;
91
f o r k =1:1:16 %C o n s t r u o da m a t r i z C e D
X_1=X_0 ; X_1( k)=X_1( k)+dd ; %E s t a d o + p e r t u r b a o
dxdt_1=s e n s o r (X_1 ) ; %D e r i v a d a r e s u l t a n t e
Z=[Z ( dxdt_1 dxdt_0 ) / dd ] ; %C o n s t r u o d a s c o l u n a s
end %da m a t r i z a u x i l i a r
%C o n s t r u o d a s m a t r i z e s n u m r i c a s C e D
C_num ( 1 : 1 4 , 1 : 1 2 ) = Z ( 1 : 1 4 , 5 : 1 6 ) ; %M a t r i z C n u m r i c a
D_num ( 1 : 1 4 , 1 : 4 ) = Z ( 1 : 1 4 , 1 : 4 ) ; %M a t r i z D n u m r i c a
m o d e l o _ s i s t e m a .m
f u n c t i o n [ X_ponto ] = m o d e l o _ s i s t e m a ( IN )
% A p l i c a a d i n m i c a e a c i n e m a t i c a a o s e s t a d o s a c t u a i s do Q u a d r i r o t o r . S i m u l a a d i n m i c a ,
% e s e r v e p a r a c o n s t r u i r a s m a t r i z e s A e B n u m r i c a s a t r a v s da f u n o n u m e r i c o .m
% P a r m e t r o s de e n t r a d a :
% IN V e c t o r com a r o t a c a o 0 e o s e s t a d o s a c t u a i s
% P a r m e t r o de s a d a :
% X_ponto V e c t o r com v a r i a o de c a d a um d o s e s t a d o s a s e r i n t e g r a d o
% Matriz Transformao
% S M a t r i z de t r a n s f o r m a o , q u a t e r n i e s
% F o r a s e Momentos
% F F o r a e x e r c i d a p o r c a d a motor [ N ]
% Fx Fora segundo x [N]
% Fy Fora segundo y [N]
% Fz Fora segundo z [N]
% Mx Momento s e g u n d o x [Nm]
% My Momento s e g u n d o y [Nm]
% Mz Momento s e g u n d o z [Nm]
% E q u a e s da d i n m i c a
% V l i n e a r V e c t o r com a v a r i a o d a s v e l o c i d a d e s l i n e a r e s
% V r o t a c a o V e c t o r com a v a r i a o d a s v e l o c i d a d e s a n g u l a r e s
% E q u a e s da C i n e m t i c a
% P o s i c a o V e c t o r com a v a r i a o da p o s i o NED
% T M a t r i z de r o t a o
% dq V e c t o r com a v a r i a o d o s q u a t e r n i e s
%
g l o b a l Kf KK m g I Lc L l
%
%E s t a d o s a c t u a i s do Q u a d r i r o t o r
w_motor = [ IN ( 1 ) IN ( 2 ) IN ( 3 ) IN ( 4 ) ] ; %r o t a o d o s m o t o r e s , r a d / s
U = IN ( 5 ) ; %V e l o c i d a d e L i n e a r s e g u n d o X , NED
V = IN ( 6 ) ; %V e l o c i d a d e L i n e a r s e g u n d o Y , NED
W = IN ( 7 ) ; %V e l o c i d a d e L i n e a r s e g u n d o Z , NED
P = IN ( 8 ) ; %V e l o c i d a d e n g u l a r s e g u n d o Ex , ABC
Q = IN ( 9 ) ; %V e l o c i d a d e n g u l a r s e g u n d o Ey , ABC
R = IN ( 1 0 ) ; %V e l o c i d a d e n g u l a r s e g u n d o Ez , ABC
X_NED = IN ( 1 1 ) ; %P o s i o s e g u n d o x , NED
Y_NED = IN ( 1 2 ) ; %P o s i o s e g u n d o y , NED
Z_NED = IN ( 1 3 ) ; %P o s i o s e g u n d o z , NED
q0 = IN ( 1 4 ) ; %Q u a t e r n i o q0
q1 = IN ( 1 5 ) ; %Q u a t e r n i o q1
q2 = IN ( 1 6 ) ; %Q u a t e r n i o q2
q3 = IN ( 1 7 ) ; %Q u a t e r n i o q3
%
%M a t r i z T r a n s f o r m a o
[ S ] = [ q0 ^2 + q1 ^2 q2 ^2 q3 ^2 2 ( q1 q2 + q0 q3 ) 2 ( q1 q3 q0 q2 ) ;
2 ( q1 q2 q0 q3 ) q0 ^2 q1 ^2 + q2 ^2 q3 ^2 2 ( q2 q3 + q0 q1 ) ;
2 ( q1 q3 + q0 q2 ) 2 ( q2 q3 q0 q1 ) q0 ^2 q1 ^2 q2 ^2 + q3 ^2 ] ;
%
%F o r a s e Momentos
for i =1:1:4
F ( i ) = Kf ( w_motor ( i ) ^ 2 ) ;
end
Fx = 0; %F o r a s e g u n d o x d e s p r e z a v e l
Fy = 0; %F o r a s e g u n d o y d e s p r e z a v e l
Fz = (F(3)+F(1)+F(4)+F ( 2 ) ) ; %F o r a s e g u n d o z , n e g a t i v a
Mx = ( F(1) F ( 2 ) ) L l ; %Momento s e g u n d o x
My = ( F(3) F ( 4 ) ) Lc ; %Momento s e g u n d o y
Mz = KK(F(3)+F(1) F(4)+F ( 2 ) ) ; %Momento s e g u n d o z
%
%E q u a e s da D i n m i c a
92
%Segunda l e i de Newton
V l i n e a r = [ Fx Fy Fz ] /m + S [ 0 0 g ] ( [ QWRV RUPW PVQU ] ) ;
%A p l i c a o da L e i d o s momentos
V r o t a c a o = i n v ( 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 ) ] ;
%
%E q u a e s da C i n e m t i c a
%C i n e m t i c a p a r a a p o s i o
P o s i c a o = S [ U V W] ;
[T] = [ 0 P Q R ;
P 0 R Q ;
Q R 0 P ;
R Q P 0];
%C i n e m t i c a p a r a a r o t a o
lambda = 1 [ q0 q1 q2 q3 ] [ q0 q1 q2 q3 ] ;
[ dq ] = ( 1/2)T [ q0 q1 q2 q3 ] + lambda [ q0 q1 q2 q3 ] ;
%
%V e c t o r de s a d a com o s E s t a d o s
X_ponto = [ V l i n e a r V r o t a c a o P o s i c a o dq ] ;
s e n s o r .m
f u n c t i o n [ Y _ s e n s o r e s ] = s e n s o r (X)
% Funo que s i m u l a o s s e n s o r e s , t r s a c e l e r m e t r o s , magnetometro , e s e n s o r de p o s i o b a s e a d o
% numa cmara e m b a r c a d a no Q u a d r i r o t o r
% P a r m e t r o s de e n t r a d a :
% X V e c t o r com a r o t a c a o 0 e o s e s t a d o s a c t u a i s
% P a r m e t r o de s a d a :
% Y _ s e n s o r e s V e c t o r com a s s a d a s d o s S e n s o r e s
% Matriz Transformao
% S M a t r i z de t r a n s f o r m a o , E u l e r
% F o r a s e Momentos
% F F o r a e x e r c i d a p o r c a d a motor [ N ]
% Fx Fora segundo x [N]
% Fy Fora segundo y [N]
% Fz Fora segundo z [N]
% Mx Momento s e g u n d o x [Nm]
% My Momento s e g u n d o y [Nm]
% Mz Momento s e g u n d o z [Nm]
% Sensor Acelermetro
% A_sensor V e c t o r com a s a c e l e r a e s t o t a i s [m/ s ^ 2 ]
% S e n s o r Cmara e m b a r c a d a
% pts M a t r i z com o s p o n t o s de r e f e r n c i a
% NED P o s i o a c t u a l do Q u a d r i r o t o r [ X Y Z p h i t e t a p s i ]
% P o s _ s e n s o r V e c t o r com a p r o j e c o d o s p o n t o s de r e f e r n c i a na imagem
% S e n s o r Magnetmetro
% Mag_sensor V e c t o r com a o r i e n t a o do m a g n e t m e t r o
%
%E s t a d o s a c t u a i s do Q u a d r i r o t o r
w_motor = [ X ( 1 ) X ( 2 ) X ( 3 ) X ( 4 ) ] ; %r o t a o d o s m o t o r e s , r a d / s
U = X(5); %V e l o c i d a d e L i n e a r s e g u n d o X , NED
V = X(6); %V e l o c i d a d e L i n e a r s e g u n d o Y , NED
W = X(7); %V e l o c i d a d e L i n e a r s e g u n d o Z , NED
P = X(8); %V e l o c i d a d e n g u l a r s e g u n d o Ex , ABC
Q = X(9); %V e l o c i d a d e n g u l a r s e g u n d o Ey , ABC
R = X(10); %V e l o c i d a d e n g u l a r s e g u n d o Ez , ABC
X_NED = X ( 1 1 ) ; %P o s i o s e g u n d o x , NED
Y_NED = X ( 1 2 ) ; %P o s i o s e g u n d o y , NED
Z_NED = X ( 1 3 ) ; %P o s i o s e g u n d o z , NED
phi = X(14); % n g u l o de r o l a m e n t o
teta = X(15); % n g u l o de p i c a d a
ppsi = X(16); % n g u l o de g u i n a d a
%
%M a t r i z T r a n s f o r m a o
93
( s i n ( p p s i ) s i n ( t e t a ) s i n ( p h i ))+( cos ( p p s i ) cos ( p h i ) ) s i n ( phi ) cos ( t e t a ) ;
( cos ( p p s i ) s i n ( t e t a ) cos ( p h i ))+( s i n ( p p s i ) s i n ( p h i ) ) ...
( c o s ( p h i ) s i n ( t e t a ) s i n ( p p s i )) ( s i n ( p h i ) c o s ( p p s i ) ) cos ( phi ) cos ( t e t a ) ];
%
%F o r a s e Momentos
for i =1:1:4
F ( i ) = Kf ( w_motor ( i ) ^ 2 ) ;
end
Fx = 0; %F o r a s e g u n d o x d e s p r e z a v e l
Fy = 0; %F o r a s e g u n d o y d e s p r e z a v e l
Fz = (F(3)+F(1)+F(4)+F ( 2 ) ) ; %F o r a s e g u n d o z , n e g a t i v a
Mx = ( F(1) F ( 2 ) ) L l ; %Momento s e g u n d o x
My = ( F(3) F ( 4 ) ) Lc ; %Momento s e g u n d o y
Mz = KK(F(3)+F(1) F(4)+F ( 2 ) ) ; %Momento s e g u n d o z
%
%S e n s o r A c e l e r m e t r o
dU = V l i n e a r ( 1 ) ; %A c e l e r a o linear s e g u n d o x , ABC
dV = V l i n e a r ( 2 ) ; %A c e l e r a o linear s e g u n d o y , ABC
dW = V l i n e a r ( 3 ) ; %A c e l e r a o linear s e g u n d o z , ABC
V r o t a c a o = i n v ( 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 = V r o t a c a o ( 1 ) ; %A c e l e r a o a n g u l a r s e g u n d o x , ABC
dQ = V r o t a c a o ( 2 ) ; %A c e l e r a o a n g u l a r s e g u n d o y , ABC
dR = V r o t a c a o ( 3 ) ; %A c e l e r a o a n g u l a r s e g u n d o z , ABC
%
%S e n s o r Cmara e m b a r c a d a
%P r o j e c o d o s p o n t o s de r e f e r n c i a na imagem
img_pts = p r o j _ p t s (NED, [ cam ; f ] , p t s ) ;
img_pts ( : , 2 ) = img_pts ( : , 2 ) ; %C o r r e c o de e i x o
x1 = img_pts ( 1 , 1 ) ; x2 = img_pts ( 2 , 1 ) ; x3 = img_pts ( 3 , 1 ) ; x4 = img_pts ( 4 , 1 ) ;
y1 = img_pts ( 1 , 2 ) ; y2 = img_pts ( 2 , 2 ) ; y3 = img_pts ( 3 , 2 ) ; y4 = img_pts ( 4 , 2 ) ;
P o s _ s e n s o r = [ x1 y1 x2 y2 x3 y3 x4 y4 ] ; %P o s i o x y d o s p o n t o s de r e f e r n c i a na
%imagem
%
%S e n s o r M a g n e t m e t r o s
Mag_sensor = S [ 1 0 0 ] ; %V e c t o r com o r i e n t a o do m a g n e t m e t r o
%
%V e c t o r com a s s a d a s d o s S e n s o r e s
Y _ s e n s o r e s = [ A_sensor P o s _ s e n s o r Mag_sensor ] ;
p r o j _ p t s .m
f u n c t i o n y = p r o j _ p t s (NED, cam , p t s )
% P a r m e t r o s de e n t r a d a :
% NED P o s i o a c t u a l do Q u a d r i r o t o r [ X Y Z p h i t e t a p s i ]
% cam P o s 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
% pts M a t r i z com o s p o n t o s de r e f e r n c i a
% P a r m e t r o de s a d a :
% y V e c t o r com a p o s i o x e y da p r o j e c o d o s p o n t o s
% M a t r i z e s de t r a n s f o r m a o e n t r e RNED e Rabc
% tff0 M a t r i z de t r a n s f o r m a o p a r a p h i e n t r e RNED e RABC
% ttt0 M a t r i z de t r a n s f o r m a o p a r a t e t a e n t r e RNED e RABC
94
% tyy0 M a t r i z de t r a n s f o r m a o p a r a p s i e n t r e RNED e RABC
% S M a t r i z de t r a n s f o r m a o e n t r e RNED e RABC
% t f f c a m M a t r i z de t r a n s f o r m a o p a r a p h i e n t r e RABC e Rxyz
% t t t c a m M a t r i z de t r a n s f o r m a o p a r a t e t a e n t r e RABC e Rxyz
% t y y c a m M a t r i z de t r a n s f o r m a o p a r a p s i e n t r e RABC e Rxyz
% Scam M a t r i z de t r a n s f o r m a o e n t r e RABC e Rxyz
% ST M a t r i z de t r a n s f o r m a o e n t r e RNED e Rxyz
% P o s 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 P o s 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 n g u l o de r o l a m e n t o [ r a d ]
% teta n g u l o de p i c a d a [ r a d ]
% psi n g u l o de g u i n a d a [ r a d ]
% M a t r i z de t r a n s f o r m a o p a r a a p r o j e c o d o s p o n t o s no r e f e r e n c i a l Rxyz
% M M a t r i z de p r o j e c o d o s p o n t o s e n t 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 ) ; t e t a _ 0 = NED ( 5 ) ; p s i _ 0 = NED ( 6 ) ;
%
%M a t r i z e s de t r a n s f o r m a o
%
%P o s 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
%
%M a t r i z de t r a n s f o r m a o p a r a a p r o j e c o d o s p o n t o s no r e f e r e n c i a l Rxyz
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 = [ c o s ( p s i ) s i n ( p s i ) 0 0 ; s i n ( p s i ) c o s ( p s i ) 0 0 ; 0 0 1 0 ; 0 0 0 1 ] ;
B_teta = [ c o s ( 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 c o s ( t e t a ) 0 ; 0 0 0 1 ] ;
B_phi = [ 1 0 0 0 ; 0 c o s ( p h i ) s i n ( p h i ) 0 ; 0 s i n ( p h i ) c o s ( p h i ) 0 ; 0 0 0 1 ] ;
B_tetacam = [ c o s ( t e t a c a m ) 0 s i n ( t e t a c a m ) 0 ; 0 1 0 0 ; s i n ( t e t a c a m ) 0 c o s ( t e t a c a m ) 0 ; 0 0 0 1 ] ;
T_trcp = [ 1 0 0 0 ; 0 1 0 0 ; 0 0 1 0 ; x_cp 0 0 1 ] ;
%M a t r i z de p r o j e c o d o s p o n t o s e n t r e RNED e Rxyz
M_total = T_tr B_psi B_teta B_phi B_tetacam T_trcp M_per ;
%
%V e c t o r de s a d a com a p o s i o x e y da p r o j e c o d o s p o n t o s
p t s ( : , 1 ) = p t s ( : , 1 ) ; %C o r r e c o da p o s i o x
pts_p = p t s M_total ; %P r o j e c o d o s p o n t o s
len_pp = l e n g t h ( pts_p ( : , 1 ) ) ;
f o r i = 1 : len_pp , pts_p ( i , : ) = pts_p ( i , : ) / pts_p ( i , 4 ) ;
end
pts_p ( : , 1 : 3 ) = pts_p ( : , 1 : 3 ) f ; %A p l i c a o da u n i d a d e f o c a l c o r r e c t a
y = pts_p ( 1 : len_pp , 2 : 3 ) ; %p o s i o x e y da p r o j e c o d o s p o n t o s
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 d a s m a t r i z e s n u m r i c a s c a l c u l a o s g a n h o s p a r a o LQR e a s m a t r i z e s d i s c r e t a s
% a s e r e m u s a d a s no f i l t r o de k a l m a n p a r a ambas a s s i t u a e s , com J o y s t i c k ( 2 s e n s o r e s ) , com
95
% cmara i n t e g r a d a ( Voo autnomo )
% P a r m e t r o de e n t r a d a :
% A_num M a t r i z A n u m r i c a para espao de estados
% B_num M a t r i z B n u m r i c a para espao de estados
% C_num M a t r i z C n u m r i c a para espao de estados
% D_num M a t r i z D n u m r i c a para espao de estados
% P a r m e t r o de s a d a :
% K Ganho d i s c r e t o LQR ( com a cmara e m b a r c a d a )
% KJ Ganho d i s c r e t o LQR ( P a r a o c a s o do J o y s t i c k )
% A_kd M a t r i z A d i s c r e t a p a r a F . Kalman em e s p a o e s t a d o s ( cmara )
% B_kd M a t r i z B d i s c r e t a p a r a F . Kalman em e s p a o e s t a d o s ( cmara )
% C_kd M a t r i z C d i s c r e t a p a r a F . Kalman em e s p a o e s t a d o s ( cmara )
% D_kd M a t r i z D d i s c r e t a p a r a F . Kalman em e s p a o e s t a d o s ( cmara )
% MM M a t r i z de i n t e r f a c e com J o y s t i c k
% A_kdJ M a t r i z A d i s c r e t a p a r a F . Kalman em e s p a o e s t a d o s ( J o y s t i c k )
% B_kdJ M a t r i z B d i s c r e t a p a r a F . Kalman em e s p a o e s t a d o s ( J o y s t i c k )
% C_kdJ M a t r i z C d i s c r e t a p a r a F . Kalman em e s p a o e s t a d o s ( J o y s t i c k )
% D_kdJ M a t r i z D d i s c r e t a p a r a F . Kalman em e s p a o e s t a d o s ( J o y s t i c k )
% S i t u a o autnoma
% Q M a t r i z de p o n d e r a o Q p a r a a p l i c a o do c o n t r o l o moderno
% R M a t r i z de p o n d e r a o R p a r a a p l i c a o do c o n t r o l o moderno
% K M a t r i z ganho , LQR d i s c r e t o
% G M a t r i z de c o v a r i n c i a
% Qc M a t r i z de c o v a r i n c i a
% Rc M a t r i z de c o v a r i n c i a
% H M a t r i z de c o v a r i n c i a
% sys2 S i s t e m a em e s p a o de e s t a d o s
% KEST S i s t e m a em e s p a o de e s t a d o s p a r a o e s t i m a d o r
% S i t u a o c o n t r o l a d a a t r a v s do j o y s t i c k
% A Nova m a t r i z A , 6 e s t a d o s , [ p q r p h i t e t a p s i ]
% B Nova m a t r i z B , 6 e s t a d o s , [ p q r p h i t e t a p s i ]
% C Nova m a t r i z C , 6 s a d a s , [ A _ s e n s o r Mag_sensor ]
% D Nova m a t r i z D, 6 s a d a s , [ A _ s e n s o r Mag_sensor ]
% QJ M a t r i z de p o n d e r a o Q p a r a a p l i c a o do c o n t r o l o moderno
% RJ M a t r i z de p o n d e r a o R p a r a a p l i c a o do c o n t r o l o moderno
% KJ M a t r i z ganho , LQR d i s c r e t o
% C1 M a t r i z p a r a s a d a s [ u v w r ] p a r a c o n s t r u o da m a t r i z MM
% D1 M a t r i z p a r a s a d a s [ u v w r ] p a r a c o n s t r u o da m a t r i z MM
% MM M a t r i z de i n t e r f a c e com J o y s t i c k
% GJ M a t r i z de c o v a r i n c i a
% QcJ M a t r i z de c o v a r i n c i a
% RcJ M a t r i z de c o v a r i n c i a
% HJ M a t r i z de c o v a r i n c i a
% s y s 2 J S i s t e m a em e s p a o de e s t a d o s
% KESTJ S i s t e m a em e s p a o de e s t a d o s p a r a o e s t i m a d o r
%
global fr
t_s = 1/ f r ;
%
%S i t u a o autnoma
%M a t r i z de p o n d e r a o Q p a r a a p l i c a o do c o n t r o l o moderno LQR
Q = diag ([25 ,25 ,25 ,250 ,250 ,250 ,6.25/10 ,6.25/10 ,6.25/10 ,418 ,418 ,418]);
%M a t r i z de p o n d e r a o R p a r a a p l i c a o do c o n t r o l o moderno LQR
R = ( 1 e 1) d i a g ( [ 1 , 1 , 1 , 1 ] ) ;
%F i l t r o de Kalman
dd =(1e 4);
G = eye ( 1 2 , 6 ) ;
Qc = d i a g ( [ dd dd dd dd dd dd ] ) ;
Rc = d i a g ( [ dd dd dd dd dd dd dd dd dd dd dd dd dd dd ] ) ;
H = 0 e y e ( 1 4 , 6 ) ;
s y s 2 = s s (A_num , [ B_num G ] , C_num , [ D_num H ] ) ; %Passagem e s p a o e s t a d o s
[ KEST ] = kalmd ( s y s 2 , Qc , Rc , t_s ) ; %F i l t r o de Kalman
[ A_kd , B_kd , C_kd , D_kd ] = s s d a t a (KEST ) ; %O b t e n o d a s m a t r i z e s
%
%S i t u a o c o n t r o l a d a a t r a v s do j o y s t i c k
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 m a t 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 m a t r i z D, 6 s a d a s
%M a t r i z de p o n d e r a o Q p a r a a p l i c a o do c o n t r o l o moderno LQR
QJ = ( 1 e 2) d i a g ( [ 3 6 4 , 3 6 4 , 3 6 4 , 3 6 4 , 3 6 4 , 3 6 4 ] ) ;
%M a t r i z de p o n d e r a o R p a r a a p l i c a o do c o n t r o l o moderno LQR
RJ = ( 1 e0 ) d i a g ( [ 1 0 , 1 0 , 1 0 , 1 0 ] ) ;
%F i l t r o de Kalman
ddJ =(1e 6);
GJ = e y e ( 6 , 6 ) ;
QcJ = d i a g ( [ ddJ ddJ ddJ ddJ ddJ ddJ ] ) ;
RcJ = d i a g ( [ ddJ ddJ ddJ ddJ ddJ ddJ ] ) ;
HJ = 0 e y e ( 6 , 6 ) ;
s y s 2 J = s s (A , [ B GJ ] , C , [ D HJ ] ) ; %Passagem e s p a o e s t a d o s
[ KESTJ ] = kalmd ( s y s 2 J , QcJ , RcJ , t_s ) ; %F i l t r o de Kalman
[ A_kdJ , B_kdJ , C_kdJ , D_kdJ ] = s s d a t a ( KESTJ ) ; %O b t e n o d a s m a 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 ) ; %M a t r i z p a r a a s 4 s a d a s [ du dv dw d r ]
D1 ( 1 : 3 , 1 : 4 ) = B_num ( 1 : 3 , 1 : 4 ) ;
D1 ( 4 , 1 : 4 ) = B_num ( 6 , 1 : 4 ) ; %M a t r i z p a r a a s 4 s a d a s [ du dv dw d r ]
MM = (( 1) C1 i n v (ABKJ ) B + D1 ) ; %I n t e r f a c e j o y s t i c k keyboard
97
Apndice D
Diagrama de blocos
At este ponto, os ficheiros usados para a construo das matrizes iniciais, nomeadamente o ficheiro
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 figuras
que se seguem exemplificam o resultado destas mesmas transformaes.
A dinmica do sistema foi construda atravs de blocos que representassem as variaes dos
estados, Figura 65.
Cada um destes blocos foi construdo com base na modelao adiantada na Seco 3.2.2 e
apresentada no ficheiro 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 ficheiro 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 filtro de Kalman consoante os sensores utilizados.
98
Criao do executvel
Figura 68: Configurao adoptada para a gerao do executvel, Generic Real-Time Target
Depois, com o Build do mesmo indicador (Figura 68), procede-se construo do ficheiro
executvel que conter a resoluo em tempo real do sistema desenvolvido em ambiente SIMULINK.
99
Real-Time Windows Target
e seleccionar desta vez Real-Time Windows Target no Real-Time Workshop, Figura 70.
Figura 70: Configurao adoptada para a gerao do executvel, Real-Time Windows Target
Por fim, 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
Tipicamente, o FLIGHTGEAR trabalha com ficheiros 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 definir 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 ficheiro ufo.xml e das
definies pr-estabelecidas no FLIGHTGEAR. A Figura 72 apresenta como exemplo as alteraes
necessrias ao ficheiro ufo.xml para a obteno da rotao da hlice nmero um. Como pode ser
constatado na mesma figura, 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 ficheiro ufo.xml
A partir deste ponto, necessrio que este modelo assuma e reaja de acordo com a dinmica
desenvolvida em ambiente SIMULINK.
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.
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 definidos 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 definidos 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 N ED (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.
Seguindo o mesmo raciocnio, e atribuindo a altitude inicial do clculo fica totalmente configurado
o bloco que envia os dados da dinmica do ALIV para o FLIGHTGEAR sem que este salte da sua
configurao inicial para a posio de clculo, Figura 75.
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