elabora˘c~ao de projeto para constru˘c~ao de quadricoptero ...³rio_final_ic_aep... · escola...
TRANSCRIPT
Escola Politecnica da Universidadede Sao Paulo
Elaboracao de Projeto para Construcao deQuadricoptero Autonomo Eficiente
Associacao dos EngenheirosPolitecnicos
Relatorio Final
Autor:Henrique KokronRodrigues
Orientador:Prof.Dr. Roberto Spinola
Barbosa
Novembro 2014
Contents
1 Introducao 2
2 Objetivos 3
3 Metodologia 3
4 Resultados 3
4.1 Elaboracao do Modelo Matematico . . . . . . . . . . . . . . . . . . . . . 3
4.1.1 Analise Cinematica . . . . . . . . . . . . . . . . . . . . . . . . . . 3
4.1.2 Analise Dinamica . . . . . . . . . . . . . . . . . . . . . . . . . . . 5
4.1.3 Espaco de Estados . . . . . . . . . . . . . . . . . . . . . . . . . . 9
4.1.4 Linearizacao . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
4.2 Verificacao do Modelo Matematico . . . . . . . . . . . . . . . . . . . . . 13
4.2.1 Degrau em V1 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14
4.2.2 Degrau em V2 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19
4.2.3 Degrau em todas as voltagens . . . . . . . . . . . . . . . . . . . . 21
4.2.4 Degrau positivo em V1 e V3 e negativo em V2 e V4 . . . . . . . . 23
4.2.5 Analise . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25
4.3 Estabilizacao do Quadricoptero . . . . . . . . . . . . . . . . . . . . . . . 25
4.3.1 Tecnica de Controle . . . . . . . . . . . . . . . . . . . . . . . . . 25
4.3.2 Simulacao Numerica . . . . . . . . . . . . . . . . . . . . . . . . . 27
5 Analise e Conclusoes 30
6 Bibliografia 31
A Programacao no Scilab 32
1
1 Introducao
Quadricopteros sao aeronaves propulsionadas por quatro rotores, que geram empuxo
atraves do movimento de helices em um fluido (ar). A configuracao mais comum e
em formato de cruz grega, com um rotor em cada ponta. Para manter estabilidade, o
ideal e que dois rotores girem em sentido horario e os outros dois no sentido contrario. A
movimentacao da aeronave se da a partir da variacao da velocidade de cada rotor, sendo
possıvel rodar o veıculo ao redor de tres eixos de referencia e acelera-lo na direcao comum
aos eixos dos rotores.
Os quadricopteros comecaram a ser desenvolvidos no inıcio do seculo passado, em
conjunto com o surgimento de outros veıculos aereos, como helicopteros e avioes. A
principal motivacao para a construcao de helicopteros e quadricopteros era a capacidade
destes de decolar e pousar verticalmente, alem de conseguirem pairar durante o voo, ou
seja, a maior versatilidade e manobrabilidade quando comparados a avioes, que precisam
de velocidade para manter o empuxo sobre as asas que geram sua sustentacao. O de-
senvolvimento de helicopteros acabou prevalecendo sobre o de quadricopteros devido a
maior simplicidade mecanica e construtiva em modelos nos quais os rotores sao alimen-
tados por motores a combustao, o que facilitou a criacao de modelos de maior porte que
mantivessem a eficiencia e funcionalidade desejadas. No entanto, nas ultimas decadas, os
quadricopteros eletricos de pequeno porte tornaram-se bastante populares nos ambientes
academicos. Por serem muito ageis, versateis, dinamicamente estaveis e terem construcao
simples, esses veıculos foram amplamente adotados como objeto de estudo nas areas de
dinamica e controle, geralmente focados na maximizacao da capacidade de manobra e
na automatizacao desses veıculos, respectivamente. Mas a maioria dos trabalhos procura
unir essas duas areas, tentando, por exemplo, aumentar as capacidades de manobra de
um quadricoptero autonomo, ou desenvolver o controle para multiplos veıculos voando
simultaneamente.
O desenvolvimento de veıculos autonomos esta relacionado com o avanco das tecnolo-
gias digitais, mais especificamente, de microcontroladores. Esses dispositivos sao “pe-
quenos computadores”, compostos por um microprocessador e perifericos em um unico
circuito eletrico integrado. Sao preferencialmente usados para controlar sistemas embar-
cados, com tarefas predefinidas. Repetindo o programa continuamente, o sistema pode
funcionar por um longo perıodo de tempo sem intervencoes adicionais do programador,
ou seja, de maneira autonoma. Com o desenvolvimento de seus componentes, os micro-
controladores tornaram-se menores e mais baratos, em geral, mais acessıveis, ampliando
as possibilidades de automatizacao de sistemas.
Atualmente, as possıveis aplicacoes para quadricopteros sao bastante vastas. Dada
2
a sua agilidade e capacidade de realizar manobra, poderiam ser um meio de transporte
(tanto de carga quanto de pessoas) bastante versatil e veıculos de resgate ou de recon-
hecimento rapidos e eficientes, ainda mais quando autonomos. Hoje em dia a aplicacao
pratica deste tipo de prototipo se restringe a realizacao de filmagens, geralmente guiado
por controle remoto. Para que as possıveis aplicacoes tornem-se reais, e preciso superar
barreiras tecnicas como a dificuldade de construir modelos de grande porte, gerada pela
complexa relacao entre peso e potencia, e a baixa autonomia. Diante do exposto, conclui-
se a importancia de estudos a respeito da eficiencia desses veıculos.
2 Objetivos
Esse trabalho tem como objetivo projetar um quadricoptero autonomo eficiente. O
projeto consiste em modelar matematicamente o veıculo e realizar simulacoes computa-
cionais. Tambem faz parte do projeto, o desenvolvimento dos algoritmos de controle para
estabilidade da planta, e a simulacao da planta controlada. Devera ser avaliada ainda, a
influencia de cada etapa do projeto no desempenho e na eficiencia do quadricoptero.
3 Metodologia
A realizacao do projeto envolvera, em um primeiro momento, o estudo aprofundado dos
conceitos de dinamica e controle. A partir do que foi estudado, sera feita a primeira
modelagem matematica do quadricoptero, a fim de entender-se quais sao os principais
parametros (i.e, peso, tamanho, formato) a serem considerados no projeto. Esse modelo
devera ser verificado atraves de um programa de simulacao numerica, o Scilab. Os resul-
tados serao analisados e avaliados de forma indicar se o modelo e adequado. Em seguida,
sera elaborado o controle para estabilizacao do veıculo, e serao realizadas simulacoes para
comprovar seu funcionamento.
4 Resultados
4.1 Elaboracao do Modelo Matematico
4.1.1 Analise Cinematica
Para melhor retratar o movimento de um quadricoptero no espaco, serao adotados dois
sistemas de coordenadas, um fixo ao corpo (com origem no baricentro), denominado Base
B, e um referencial absoluto chamado de Base A, vide a figura abaixo.
3
x
y
z
F1
F2
F3
F4
Ω1
Ω2
Ω3
Ω4
Z
X
Y
Figura 1: Modelo generico de um quadricoptero.
Os vetores posicao e velocidade do corpo no referencial absoluto sao dados por
−→XA = (X, Y, Z) e
−→XA = (X, Y , Z)
e no referencial solidario ao corpo, por
−→xB = (x, y, z) e −→xB = (x, y, z)
Para descrever o movimento rotacional, foi definido o vetor velocidade angular no refer-
encial fixo ao corpo, assim como um vetor θ contendo os angulos de Euler, ψ, θ, φ.
−→ωB = (ω1, ω2, ω3) e θ = (ψ, θ, φ)
As velocidades ω1, ω2, ω3 sao comumente chamadas de roll, pitch e yaw rate, respectiva-
mente, enquanto os angulos ψ, θ, φ sao conhecidos por heading, attitude e bank angle, ou
mesmo, angulo de arfagem, guinada e rolagem.
A relacao entre os dois referencias pode ser descrita atraves dos angulos de Euler. A
NASA estipula a transformacao 3-2-1 como a padrao para aeronaves e para essa sequencia
a matriz de transformacao e
[R] =
cψcθ sψcθ −sθ−sψcφ+ cψsθsφ cψcφ+ sψsθsφ cθsφ
sψsφ+ cψsθcφ −cψsφ+ sψsθcφ cθcφ
Sendo que a transformacao e realizada da seguinte maneiraxy
z
= [R]
XYZ
Vale ressaltar que, adotando essa convencao, o eixo vertical da base B e da base A tem
sentidos opostos quando alinhados. A relacao entre a derivada dos angulos de Euler e a
4
velocidade e angular e
[ω] =
−sθ 0 1
cθsφ cφ 0
cθcφ −sφ 0
[θ]
4.1.2 Analise Dinamica
Modelo do Veıculo Em um primeiro momento, sera considerado como o sistema a
ser avaliado o quadricoptero completo, com motores e helices, porem simplificando sua
acao a forcas e momentos invariaveis. As forcas que serao consideradas para a analise
sao as gravitacionais e aerodinamicas. A forca gerada pela acao da gravidade e vertical
e negativa em relacao ao referencial inercial, e o empuxo gerado por cada propulsor
Fi e vertical e negativo no referencial do corpo. Sera contabilizada ainda uma forca
FD decorrente do arrasto aerodinamico sobre o quadricoptero. A representacao vetorial
dessas forcas segue abaixo.
−→Fi = (0, 0,−Fi)B
−→FD = (FD1, FD2, FD3)A
Aplicando o Teorema do Movimento do Baricentro, chega-se a seguinte relacao.XYZ
=
0
0
−g
+ [R]T
0
0−
∑41 Fi
M
+1
M
FD1
FD2
FD3
(1)
onde M e a massa total do veıculo e g a aceleracao da gravidade.
Os momentos atuantes no sistema sao decorrentes da acao dos propulsores, sendo
que uma parcela e gerada pela resistencia aerodinamica a rotacao das helices e outra
pela direcao e distancia da forca de empuxo em relacao ao baricentro do veıculo. Os
momentos resistentes serao denotados por MRi, e os momentos gerados pelas forcas serao
Mx e My.Adotando D como a distancia no plano xy entre o baricentro e o centro das
helices (onde sao aplicadas as forcas), pode-se representar essas forcas pelos vetores
−→Mx = (d(F2 − F4), 0, 0)B
−→My = (0, d(F1 − F3), 0)B
−−→MRi = (0, 0,−1iMRi)B
Vale ressalatar que, como ha duas helices que giram em um sentido e duas no sentido
contrario, ocorre mudanca no sinal do momento resistivo. Aplicando o Teorema da quan-
tidade de momento angular no referencial fixo ao corpo, a partir do baricentro, obtem-se
5
a equacao matricial ω1
ω1
ω3
=
Mx
JxMy
Jy∑41MRi
Jz
+
(Jy−Jz)ω2ω3
Jx(Jz−Jx)ω1ω3
Jx(Jx−Jy)ω1ω2
Jx
(2)
onde Jx, Jy e Jz sao os momentos de inercia do conjunto.
O movimento relativo entre as helices e a estrutura do quadricoptero causa um mo-
mento giroscopico, que pode ser adicionado ao equacionamento. Ele atua apenas nos
eixos x e y, e depende da velocidade em torno deles, assim como da rotacao dos motores
e helices e do momento de inercia vertical do conjunto motriz. Sera chamada de Ωi a
velocidade de cada motor, seguindo a orientacao apresentada na figura 1, e Jmz a inercia
da helice e do rotor. Denotado por MG, o torque giroscopico em relacao ao referencial
movel e MGx
MGy
MGz
=
−Jmzω2 (−Ω1 + Ω2 − Ω3 + Ω4)
Jmzω1 (−Ω1 + Ω2 − Ω3 + Ω4)
0
(3)
Adicionando a equacao (3) a (2), e fazendo ΩR = (−Ω1 + Ω2 − Ω3 + Ω4), encontra-seJxω1
Jyω1
Jzω3
=
Mx
My∑41MRi
+
(Jy − Jz)ω2ω3
(Jz − Jx)ω1ω3
(Jx − Jy)ω1ω2
+
−Jmzω2ΩR
+Jmzω1ΩR
0
(4)
Incluindo o Motor com Helice Para incluir os motores DC e as helices no modelo,
ira primeiro se considerar os efeitos aerodinamicos gerados por cada helice. Pela teoria
de helice, sabe-se que o torque e a potencia podem ser calculados segundo as equacoes
TH = CT4ρ r4
H
π2Ω2H (5)
PH = CP4ρ r5
H
π3Ω3H (6)
onde CT e CP sao os coeficientes de empuxo e potencia, respectivamente; ρ e a densidade
do ar; r e o raio da helice; e ΩH sua velocidade de rotacao. O momento resistivo pode
ser obtido atraves da relacao
PH = MHΩH (7)
Que fornece
MH = Cp4ρ r5
H
π3Ω2H (8)
Tanto CP quanto CT sao valores que variam conforme a velocidade axial das helices.
6
Contudo, sera adotado que o quadricoptero nao assume velocidade vertical elevada nas
condicoes de operacao analisadas, tornando seus valores constantes. Dessa maneira,
simplifica-se (5) e (8) para
TH = LΩ2H (9)
MH = DΩ2H (10)
onde L e uma constante de empuxo (lift) e D de arrasto (drag).
A partir de (9) e (10), as forcas e os momentos atuantes no sistema sao reescritas
como segue−→Fi = (0, 0,−LΩ2
i )B−−→MRi = (0, 0,−1iDΩ2
i )B
−→Mx = (dL(Ω2
2 − Ω24), 0, 0)B
−→My = (0, dL(Ω2
1 − Ω23), 0)B
Substituindo as relacoes acima em (2) e (4),XYZ
=
0
0
−g
+ [R]T
0
0LM
(Ω21 + Ω2
2 + Ω23 + Ω2
4)
+1
M
FD1
FD2
FD3
(11)
Jxω1
Jyω1
Jzω3
=
dL(Ω22 − Ω2
4)
dL(Ω21 − Ω2
3)
D(−Ω21 + Ω2
2 − Ω23 + Ω2
4)
+
(Jy − Jz)ω2ω3
(Jz − Jx)ω1ω3
(Jx − Jy)ω1ω2
+
−Jmzω2ΩR
+Jmzω1ΩR
0
(12)
E importante expressar o problema em funcao da velocidade dos motores (igual a
velocidade das helices), visto que tambem e possıvel modela-lo e obter equacoes para des-
crever seu funcionamento. Considerou-se que os quatro motores sao de corrente contınua,
nao-escovados e exatamente iguais. O circuito equivalente esta representado na figura 2,
na qual V0 e a tensao de entrada, R a resistencia interna, L a indutancia, em a queda de
tensao gerada pelo motor e τ o torque gerado.
R L
V0
em τ
Figura 2: Circuito equivalente de um motor DC sem escovas.
7
A equacao diferencial para o circuito e
Ldi
dt+Ri+ em = V0 (13)
e as relacoes eletromecanicas para o torque τ e a tensao em sao como segue
τ = Kt i (14)
em = Ke Ωm (15)
onde Kt e Ke sao constantes de torque e de tensao, nessa ordem, e Ωm e a velocidade
do motor. Vale ressaltar que, apesar de terem unidades distintas ([Km] = V s/rad e
[Kt] = Nm/A), elas tem o mesmo valor numerico. Isso decorre da relacao entre potencia
mecanica e potencia eletrica no elemento do circuito.
Para a grande maioria dos motores eletricos usados em quadricopteros, a indutancia
interna e muito baixa, e pode ser desprezada. Substituindo (15) em (13), e tomando
L ≈ 0, encontra-se
i =V0
R− Ke Ωm
R(16)
Analisa-se, entao, a dninamica do motor. Os esforcos presentes no conjunto rotor e helice
estao ilustrados abaixo.
ΩDΩ2τ
Figura 3: Momentos atuantes sobre o motor e a helice.
O TMA para o conjunto resulta em
JmzΩm = τ −DΩ2m (17)
Das equacoes (14), (16) e (17) obtem-se a equacao diferencial que descreve a aceleracao
dos motores, aspresentada logo abaixo.
Ωm =
(−KtKe
JmzR
)Ωm +
(Kt
JmzR
)V0 −
(D
Jmz
)Ω2m (18)
8
4.1.3 Espaco de Estados
As equacoes diferencias (11), (12) e (18), apresentadas na secao anterior, sao capazes de
descrever completamente o problema proposto. O sistema tem dez graus de liberdade,
seis deles resultantes do movimento livre do corpo no espaco e os outros quatro da rotacao
de cada motor. Ademais, as equacoes obtidas indicam que ele e de decima sexta ordem.
As equacoes sao altamente nao lineares, o que dificulta suas resolucoes, porem e possıvel
resolve-las atraves de simulacao numerica.
O metodo dos Espacos de Estado e uma forma pratica e compacta de lidar com sitemas
nao lineares, como o tratado nesse trabalho. Mantendo as equacoes no domınio do tempo,
podemos representar as entradas e saıdas do sistema como vetores, expressando a relacao
entre elas como matrizes.
Definir-se-a o vetor contendo as variaveis de estado por
X = [X, Y, Z, ψ, θ, φ, X, Y , Z, ω1, ω2, ω3,Ω1,Ω2,Ω3,Ω4]T
e a equacao matricial que descreve o sistema e
X = A‘X +B‘u‘ +NL+D (19)
As matrizes N e L e surgem devidos as nao linearidades, e a matriz D contempla os
efeitos aerodinamicos. Elas sao apresentadas a seguir.
A‘ =
0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 sφcθ
cφcθ
0 0 0 0
0 0 0 0 0 0 0 0 0 0 cφ −sφ 0 0 0 0
0 0 0 0 0 0 0 0 0 1 sφsθcθ
cφsθcθ
0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 −KtKe
RJmz0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 −KtKe
RJmz0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 −KtKe
RJmz0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 −KtKe
RJmz
9
B‘ =
0 0 0 0 0
0 0 0 0 0
0 0 0 0 0
0 0 0 0 0
0 0 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 0 0 0
0 0 0 0 0
0 0 0 0 0
0 KtRJmz
0 0 0
0 0 KtRJmz
0 0
0 0 0 KtRJmz
0
0 0 0 0 KtRJmz
u‘ = [ g V1 V2 V3 V4 ]T
10
N =
0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0
− LMR31 − L
MR31 − L
MR31 − L
MR31 0 0 0 0 0
− LMR32 − L
MR32 − L
MR32 − L
MR32 0 0 0 0 0
− LMR33 − L
MR33 − L
MR33 − L
MR33 0 0 0 0 0
0 dLJx
0 −dLJx
−Jmz
Jx0 Jy−Jz
Jx0 0
dLJy
0 dLJy
0 0 Jmz
Jy0 Jz−Jx
Jy0
− DJmz
DJmz
− DJmz
DJmz
0 0 0 0 Jx−JyJx
Onde R31, R32 e R33 sao os elementos da matriz [R] de transformacao de base.
L = [ Ω21 Ω2
2 Ω23 Ω2
4 ω2ΩR ω1ΩR ω2ω3 ω1ω3 ω1ω2 ]T
D = [ 0 0 0 0 0 0 FD1 FD2 FD3 0 0 0 0 0 0 0 ]T
4.1.4 Linearizacao
Um dos principais modos de operacao de um drone e pairando. Os quadricopteros sao
muito utlizados para fazer filmagens, e a estabilidade nesse momento e de extrema im-
portancia. Para essa situacao, pode-se assumir que os deslocamentos e velocidades angu-
lares sao pequenos, de forma que a expansao por series de Taylor em torno desse ponto
nos fornece uma aproximacao razoavel para o problema. A grande vantagem dessa abor-
dagem e a maior simplicidade no controle de um sistema linear em relacao ao nao linear.
A expansao por Serie de Taylor de primeira ordem para funcoes de multiplas variaveis
e
f(x, y) ≈ f(x0, y0) +∂f
∂x
∣∣∣∣x0
(x− x0) +∂f
∂y
∣∣∣∣y0
(y − y0)
Aplicando isso as equacoes diferenciais referente a cada variavel de estado, e desprezando
os efeitos de arrasto na movimentaca do quadricopero, pode-se obter a equacao matricial
˙X = AX +Bu (20)
11
sendo X = X −X0, e u = u − u0. Tanto X quanto u representam as condicoes iniciais
do sistema. Para a situacao de hovering, elas sao
X0 = [0, 0, Z0, 0, π, 0, 0, 0, 0, 0, 0, 0,Ω0,Ω0,Ω0,Ω0]T
u0 = [V0, V0, V0, V0]T
para que seja mantido o equilıbrio vertical,
Ω0 =
√Mg
4L(21)
A tensao para que Ωm = Ω0, pode ser determinada por
V0 = KeΩ0 +DR
Kt
Ω20 (22)
Para facilitar a notacao matricial, sera denotado
CL =
(KtKe
RJmz+
2D
JmzΩ0
)As matrizes A e B sao escritas em funcao desses valores, conforme e apresentado subse-
quentemente.
A =
0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0
0 0 0 0 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 0 0 0 0 0 0 0 0 1 0 0 0 0 0
0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0
0 0 0 04LΩ2
0
M0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 04LΩ2
0
M0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 2LΩ0
M2LΩ0
M2LΩ0
M2LΩ0
M
0 0 0 0 0 0 0 0 0 0 0 0 0 dLΩ0
Jx0 −dLΩ0
Jx
0 0 0 0 0 0 0 0 0 0 0 0 dLΩ0
Jy0 −dLΩ0
Jy0
0 0 0 0 0 0 0 0 0 0 0 0 −2DΩ0
Jz2DΩ0
Jz−2DΩ0
Jz2DΩ0
Jz
0 0 0 0 0 0 0 0 0 0 0 0 −CL 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 −CL 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 −CL 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 −CL
12
B =
0 0 0 0
0 0 0 0
0 0 0 0
0 0 0 0
0 0 0 0
0 0 0 0
0 0 0 0
0 0 0 0
0 0 0 0
0 0 0 0
0 0 0 0
0 0 0 0Kt
RJmz0 0 0
0 Kt
RJmz0 0
0 0 Kt
RJmz0
0 0 0 Kt
RJmz
4.2 Verificacao do Modelo Matematico
Para verificar o modelo matematico, as equacoes encontradas na secao anterior foram
implementadas no Scilab, um programa open source de simulacao numerica. A analise
consiste em submeter as entradas a variacoes de valor, e observar a reacao das saıdas.
Foram escolhidos casos em que se pode prever facilmente a resposta do sistema, a fim de
se comparar os resultados obtidos com aquilo que era esperado. Posteriormente, foi feita
uma rapida comparacao entre os resultados do modelo linear e do nao linear, visando
comprovar a hipotese de que o modelo linear e uma boa aproximacao para quando o
quadrirrotor esta pairando.
Os dados utilizados para a simulcao sao para um quadricoptero generico com tamanho,
massa e momentos de inercia consistentes, cujos dados encontram-se na tabela 1. Ele e
equipado com quatro motores eletricos tipo outrunner, modelo BC2836/9 e helices de
9x6 polegadas, e os dados para estes componentes estao listados nas tabela 3 e 2.
13
Constante Unidade Valor
M [kg] 1.0
Jx [kgm2] 8.1x10−3
Jy [kgm2] 8.1x10−3
Jz [kgm2] 14.2x10−3
d [m] 0.20
Tabela 1: Contantes do Quadricoptero
Constante Valor
Ct 0.13
Cp 0.06
Tabela 2: Contantes da Helice
Constante Unidade Valor
Ri [Ω] 0.107
Ke [V srad−1] 0.0108
Kt [NmA−1] 0.0108
Jmz [kgm2] 1.15x10−4
Tabela 3: Contantes do motor
Outras constantes utilizadas foram a aceleracao da gravidade g = 9.81[m/s2] e a
densidade do ar ρ = 1.2[kg/m3]. Com esse valores obtivemos os valores das constantes de
empuxo L = 1.46x10−5 [Ns2/rad2] e de arrasto D = 3.8x10−7 [Nms2/rad2]. A rotacao
de cada motor na situacao de equilıbrio e Ω0 = 409.85 rad/s (3913 rpm), e a tensao para
manter essa rotacao V0 = 5.059V .
4.2.1 Degrau em V1
Ao se aplicar um degrau de 0.5V na tensao fornecida ao motor 1, foi observada a seguinte
resposta no tempo.
14
Figura 4: Variacao na velocidade do motor 1 no tempo para um degrau de 0.5V .
Essa mudanca deve gerar um desbalanceamento dos momemtos de arrasto das helices,
assim como um momento sobre o eixo y, causando mudancas no angulo de arfagem (ψ)
e de guinada (θ).
Figura 5: Variacao do angulo ψ no tempo para um degrau de 0.5V .
15
Figura 6: Variacao do angulo θ no tempo para um degrau de 0.5V .
Figura 7: Velocidade angular em torno do eixo y pelo tempo para um degrau de 0.5V .
Como o angulo de guinada deixa de ser nulo, surge uma componente da forca de
sustentacao na direcao X.
16
Figura 8: Posicao X no tempo para um degrau de 0.5V .
E importante ressaltar que a simulacao so e valida para os limites abaixo
π
2< θ <
3π
2e − π
2< φ <
π
2
visto que passando desses pontos o quadricoptero asssume uma posicao em que as helices
estao orientadas para baixo, tornando impossıvel o equilıbrio do veıculo. Em casos de
manobras mais complexas (i.e. loopings), e necessario utilizar o sistema de quaternos,
que evita descontinuidades. Assim, e possıvel assumir uma posicao de nao-equilıbrio
momentaneamente, e depois retornar a orientacao correta.
Alem disso, a linerarizacao so garante bons resultados para pequenas variacoes nos
angulos. Na Figura 9, comparamos os valores obtidos para o angulo φ no modelo nao
linear com o linear.
17
Figura 9: Angulo θ para o modelo linear (preto) e nao linear (vermelho) no tempo paraum degrau de 0.5V no motor 1.
Por mais que o angulo φ se mantenha similiar para valores grandes, uma das principais
limitacoes do modelo linear e o fato de nao levar em conta a perda de sustentacao com a
mudanca dos angulos de rolagem e guinada. A diferenca brusca pode ser vista no grafico
a seguir.
Figura 10: Altura no tempo para um degrau de 0.5V no motor 1 no modelo linear (preto)e no nao linear (vermelho).
As maiores variacoes ocorrem a partir do instante t = 0.3 s . Olhando para o grafico
18
de θ, consegue-se definir melhor um limite para a aproximacao feita de
−π6< φ <
π
6e
5π
8< θ <
7π
6
4.2.2 Degrau em V2
Ao se aplicar um degrau de 0.5V na tensao fornecida ao motor 2, a resposta foi igual
aquela do motor 1, de acordo com o esperado. Nesse caso, as mudancas devem gerar
um desbalanceamento dos momemtos de arrasto das helices de modulo igual, porem com
diferente sentido daquele observado no primeiro caso, gearndo um angulo de arfagem
contrario.
Figura 11: Variacao do angulo ψ no tempo para um degrau de 0.5V .
19
Figura 12: Variacao do angulo φ no tempo para um degrau de 0.5V .
Figura 13: Velocidade angular em torno do eixo x pelo tempo para um degrau de 0.5V .
Como o angulo de rolagem deixa de ser nulo, surge uma componente da forca de
sustentacao na direcao Y .
20
Figura 14: Posicao X no tempo para um degrau de 0.5V .
4.2.3 Degrau em todas as voltagens
Esse caso ilustra a o movimento de aceleracao vertical do quadricoptero. Os resultados
apresentados ja fazem a comparacao com o caso nao linear, visto que a resposta e muito
simples. Para os graficos abaixo, foi aplicada uma tensao de 1V em cada motor.
Figura 15: Altura no tempo para um degrau de 1V em todos motores no modelo linear(preto) e no nao linear (vermelho).
21
Figura 16: Velocidade em Z no tempo para um degrau de 1V em todos motores no modelolinear (preto) e no nao linear (vermelho).
Figura 17: Velocidade do motor 1 no tempo para um degrau de 1V no modelo linear(preto) e no nao linear (vermelho).
Observa-se que, para o degrau de 1V, o modelo linear fornece resultados muito sim-
ilares aos do nao linear. Para um degrau de maior intensidade, as diferencas sao mais
significativas, como mostram os graficos abaixo.
22
Figura 18: Altura no tempo para um degrau de 5V em todos motores no modelo linear(preto) e no nao linear (vermelho).
Figura 19: Velocidade do motor 1 no tempo para um degrau de 5V no modelo linear(preto) e no nao linear (vermelho).
4.2.4 Degrau positivo em V1 e V3 e negativo em V2 e V4
Essa configuracao ilustra os comandos para o veıculo girar em torno do proprio eixo
vertical, mantendo a altura constate.
23
Figura 20: Variacao do angulo ψ no tempo.
Figura 21: Velocidade do motor 1 (preto) e do motor 2 (azul) ao longo do tempo.
24
4.2.5 Analise
Analisando qualitativamente e quantitativamente as simulacoes expostas nessa secao,
pode-se perceber que o modelo matematico desenvolvido representa bem o sistema fısico
real, umas vez que os resultados numericos sao condizentes com o esperado. De forma
comparativa, foram definidos tambem os limites para a aproximacao obtida atraves da
linearizacao do sistema de equacoes. Percebeu-se que ela funciona bem para angulos de
rolagem e guinada de ate 30 graus, por curtos perıodos de tempo, como e comum para a
aproximacao por serie de Taylor de primeira ordem.
4.3 Estabilizacao do Quadricoptero
O controle realizado para establizar o veıculo quadrirrotor foi pensado levando em con-
sideracao o sistema linear apresentado anteriormente. O controle linear e muito mais
simples, menos custoso e, como o problema linear e uma boa aproximacao da realidade
para agulos pequenos, ele funciona bem para o manter o veıculo pairando.
Considerando que os sensores geralmente encontrados nos quadricopteros sao acelero-
metros e girometros, e possıvel monitorar o estado do veıculo em todos os instantes.
O acelerometro fornece as acelerecoes absolutas nas direcoes do referencial inercial, en-
quanto o girometro indica as velocidades angulares no referencial fixo ao corpo, ou seja,
os sensores nos fornecem X, Y , Z e ω1, ω2, ω3. Atravez da integracao dupla dos dados
do acelerometro, somada a integracao simples das medidas do girometro, obtem-se a
posicao completa do quadricoptero. E comum encontrar modelos com bussulas, sistemas
de geoposicionamento, barometros e radares. Esses componentes proporcionam dados
adicionais, que podem ser usados para aumentar a precisao do quadricoptero, ou para
registrar o percurso do veıculo.
Os drones tem movimentacao espacial completa, ou seja, com seis graus de liberdade,
e possuem apenas quatro motores, caracterizando um sistema sub-atuado. Tendo isso em
vista, as variaveis escolhidas para serem controladas foram os angulos θ, φ e ψ, e a altura
Z. Para que o quadricoptero esteja pairando em equilıbrio, todas essas variaveis devem
ter valor nulo, exceto θ, que deve ser π (devido a orientacao da base movel).
4.3.1 Tecnica de Controle
A tecnica de controle implementada foi o controle proporcional-derivativo. A acao desse
tipo de controlador e dada por
u(t) = Kpe(t) +Kdde(t)
dt
25
onde u(t) e o sinal do controlador, e(t) e o sinal de erro da variavel medida, Kp o ganho
proporcional e Kd o ganho derivativo. O sinal do controlador age sobre os atuadores, que
por sua vez atuam sobre a planta. Um esquema generico e apresentando a seguir.
EntradasControlador
-+
e(t) u(t)Sistema
Saidas
Figura 22: Esquema de controle.
Para as variaveis Z e ψ, foi adotado Kp = 0, de forma que o controle na realidade e
proporcional as velocidades Z e ω3 (para a aproximacao feita φ = −ω3). Isso ocorre pois
nao ha sentido fixar uma altura ou um angulo ψ para que o veıculo seja pilotavel.
Para definir o sinal de erro, primeiro e necessario especificar os valores de referencia,
ou seja, as entradas. Para que o quadricoptero esteja parelelo ao solo, em equlıbrio, os
valores desejados para as variaveis controladas (relativas a condicao incial) sao
θd = 0 ˙θd = 0 φd = 0 ˙φd = 0 ˙ω3d = 0 ˙Zd = 0
Consequentemente, os sinais de erro ficam
eθ = θ − θd = θ eθ = ˙θ − ˙θd = ˙θ eψ = ˙ψ − ˙ψd = ˙ψ
eφ = φ− φd = φ eφ = ˙φ− ˙φd = ˙φ eZ = ˙Z − ˙Zd = ˙Z
Ao se analisar a matriz dinamica do sistema (matriz A apresentada em 4.1.4), conclui-
se que os motores 2 e 4 atuam sobre o angulo φ, os motores 1 e 3 atuam sobre o angulo θ
e todos os motores atuam sobre a velocidade vertical e sobre ω3. Logo, pode-se controlar
o quadricopotero com o sinal do controlador atuando sobre as tensoes de entrada nos
motores de acordo com as expressoes abaixo.
V1 = V0 −Kp1eZ +Kp2eω3 −Kp3eθ −Kd3eθ
V2 = V0 −Kp1eZ −Kp2eω3 −Kp4eφ −Kd4eφ
V3 = V0 −Kp1eZ +Kp2eω3 +Kp3eθ +Kd3eθ
V4 = V0 −Kp1eZ −Kp2eω3 +Kp4eφ +Kd4eφ
26
4.3.2 Simulacao Numerica
Para testar o controle PD desenvolvido na secao anterior, foi feito um programa no Scilab.
Foram usados dados iguais aos da secao 4.2., incluindo ainda valores para os ganhos de
controle. Esses valores foram ajustados manualmente partindo de estimativas iniciais,
ate se obter resultados adequados. As constantes utilizadas nas simulacoes que serao
apresentadas foram Kp1 = 1.0, Kp2 = 0.5, Kp3 = Kp4 = 0.4 e Kd3 = Kd4 = 0.15.
Os graficos a seguir ilustram o veıculo assumindo a posicao de equilıbrio a partir de
uma situacao inicial bastante instavel. No insntante t = 0, o quadricoptero esta inclinado
com θ = π/10; rad e φ = π/10 rad, em queda com velocidade Z = −1 m/s e girando em
torno do proprio eixo a uma taxa ω3 = π/6 rad/s.
Figura 23: Velocidade em Z ao longo do tempo.
27
Figura 24: Velocidade ω3 ao longo do tempo.
Figura 25: Angulo θ ao longo do tempo.
28
Figura 26: Angulo φ ao longo do tempo.
Figura 27: Velocidade dos motores 1 (preto), 2(azul), 3(vermelho) e 4(verde) ao longo dotempo.
Partindo destes graficos, e imediata a conclusao de que a planta esta bem contro-
lada.Todas as variaveis se anulam apos t = 2.5 s, apresentando baixa oscilacao a partir
desse instante. A figura 27 e bastante interessante pois mostra a atuacao de cada motor
separadamente. Os pares de motores 1 e 3 atuam em uma faixa mais alta que o par 2 e
4, gerando uma diferenca que anula a velocidade ω3, e entre os motores de cada par ha
uma variacao que corrige os angulos de guinada e rolagem, respectivamente.
29
5 Analise e Conclusoes
Neste trabalho, foi desenvolvido um conjunto de equacoes diferenciais que copoem um
modelo matematico para representar um quadricoptero. Essas equacoes descrevem o
comportamento do veıculo no espaco, assim como a acao dos motores. O sistema obtido e
nao linear e, visando simplificar o controle, ele foi linearizado para um ponto de equilıbrio.
O ponto escolhido foi para o quadricoptero paralelo ao chao, com a forca de sustentacao
igual ao proprio peso, ou seja, pairando. Em seguida, implementou-se o controle para
que o veıculo se mantenha estavel ao redor dessa posicao. O tipo de controle escolhido foi
o proporcional derivativo, dada a sua simplicidade e eficiencia. O sistema e sub-atuado,
pois tem apenas 4 motores para seis graus de liberdade. As variaveis escolhidas para
serem controladas foram os angulos de guinada e rolagem, a velocidade de rotacao em
torno do proprio eixo e a velocidade vertical.
Com a utilizacao de simulacao numerica, provou-se que o modelo matematico do
quadricoptero e fiel ao problema real. Os resultados para a simulacao do sistema em malha
aberta foram condizentes com o que era esperado do sistema real. Outro importante passo
foi a determinacao da validade da aproximacao para o sistema linear, indicando valores
para os angulos de guinada e rolagem dentro dos quais essa aproximacao e razoavel. A
simulacao em malha fechada comprovou que o metodo de controle utilizado e eficiente.
Todas as variaveis em questao foram estabilizadas em um perıodo de tempo curto, atraves
da variacao do sinal de entrada em funcao dos seus erros, mantendo o funcionamento dos
motores em uma faixa aceitavel de velocidade.
Desenvolvimentos Futuros O proximo passo seria transferir os densenvolvimentos
teoricos para um prototipo real. Para isso, seria preciso se aprofundar nos estudos de
sensores utilizados, baterias, motores e dos componentes embarcados, ou seja, um estudo
do hardware envolvido. De forma colaborativa, esse estudo pode auxiliar a desenvolver
um controle mais completo, ou seja, com mais informacoes sobre as quais se basear, au-
mentando sua precisao. Tambem e possıvel aumentar a precisao do controle investindo no
processamento de dados. Uma frente interessante para ser explorada e o estudo empırico
dos dados de consumo energetico. Avaliando o desempenho de varias helices, motores e
baterias, encontraria-se um conjunto de componentes para otimizar o tempo de voo, ou a
capacidade de carga. Outro caminho que pode ser explorado e o desenvolvimento de um
controle nao-linear para o sistema real, possibilitando a realizacao do controle em casos
mais radicais de desequilıbrio, como em um looping ou manobras muito bruscas de troca
de direcao. Como foi exposto na introducao, o estudo de drones tem hoje uma variedade
muito grande de opcoes de aprofundamento, sendo que este trabalho forma uma boa base
de partida.
30
6 Bibliografia
1. BARUH, H. Analytical Dynamics. Rutgers University: WCB/McGraw-Hill, 1999.
718p.
2. OGATA, K. Enegenharia de Controle Moderno. Quarta Edicao. Sao Paulo: Pearson
Education do Brasil, 2003. 788p.
3. GREITZER, E.M.; SPAKOVSKY, Z. S.; WAITZ, I. A. 16.Unified: Thermodynam-
ics and Propulsion. Notas de aula. Massachussets Institute of Technology. Disponıvel em:
http://web.mit.edu/16.unified/www/FALL/thermodynamics/notes/notes.html. Data
de acesso: 22/06/2014.
4. ARGENTIM, L. M.; REZENDE, W. C.; SANTOS, P. E.; AGUIAR, R. A. PID,
LQR and LQR-PID on a Quadcopter Platform. In: INFORMATICS, ELECTRONICS &
VISION (ICIEV), 17 - 18 maio, 2013, Dhaka. Disponıvel em: http://ieeexplore.ieee.
org/xpl/articleDetails.jsp?tp=&arnumber=6572698&url=http%3A%2F%2Fieeexplore.
ieee.org%2Fxpls%2Fabs_all.jsp%3Farnumber%3D6572698. Acesso em: 18/07/2014.
5. BRESCIANNO, T. Modelling, Identification and Control of a Quadcopter Helicopter.
2008. 180f. Dissertacao (Mestrado em Engenharia Eletronica). Department of Automatic
Control, Lund University, Lund.
31
A Programacao no Scilab
Seguem os codigos para a simulacao realizada no Scilab do sistema linearizado.
clc
clear
//simulac~ao do movimento de um quadricoptero no espaco - LINEARIZADO
g = 9.81; // m/s
//Definic~ao dos parametros gerais do quadricoptero
M = 1.0; // kg
Jx = 0.0081; //
Jy = 0.0081; //
Jz = 0.0142; // kg*m
L= 1.46*10^(-5);
D= 3.8*10^(-7);
d = 0.20; // m
//Dados do motor
R=0.107;
Ke=0.0108;
Kt=Ke;
Jtp=1.15*10^(-4);
// parametros inicias para y
Xzero = 0;
Yzero = 0;
Zzero = 5; // m
psi0 = 0;
teta0 =%pi;
fi0 = 0; // rad
Xponto0 = 0;
Yponto0 = 0;
Zponto0 = 0; // m/s
W1zero = 0;
W2zero = 0;
W3zero = 0; // rad/s
Omega_0 = sqrt(M*g/(4*L));
disp(Omega_0);
Omega_1 = Omega_0;
Omega_2 = Omega_0;
32
Omega_3 = Omega_0;
Omega_4 = Omega_0;
// parametros temporais (s)
t0 = 0;
Dt = 0.0001;
tf = 5;
t = [t0:Dt:tf];
// matriz y0
X0 = [Xzero;Yzero;Zzero;psi0;teta0;fi0;Xponto0;Yponto0;Zponto0;W1zero;W2zero;W3zero;Omega_0;Omega_0;Omega_0;Omega_0];
X0_barra = [0;0;0;0;0;0; 0; 0; -1; 0; 0; 0; 0; 0; 0; 0] //Valores podem ser alterados conforme a condic~ao incial
//tensao para omega0
Vo= Ke*Omega_0 + D*(Omega_0^2)*R/Kt;
disp(Vo);
disp("---------")
V1=Vo;
V2=Vo;
V3=Vo;
V4=Vo;
//definic~ao dos limites para a tens~ao aplicada
Vmax = 14.4;
Vmin=0.1;
function dx = f(t,x)
//CONTROLE DA PLANTA
//valores de ref \\valores barra
zponto_ref = 0;
psipnt_ref=0;
teta_ref= 0;
tetapnt_ref=0;
phi_ref=0;
phipnt_ref=0;
//erros
e_zponto = x(9) - zponto_ref;
e_psipnt = -x(12) - psipnt_ref;
33
e_teta = x(5) - teta_ref;
e_tetapnt = x(11) - tetapnt_ref;
e_phi= x(6) - phi_ref;
e_phipnt = x(10) - phipnt_ref;
//ganhos
Kp_zponto = 1;
Kp_psipnt = .5;
Kp_teta = .4;
Kd_teta = .15;
Kp_phi = .4;
Kd_phi = .15;
////valor das voltagens
V1 = Vo -Kp_zponto*e_zponto - Kp_psipnt*e_psipnt - Kp_teta*e_teta - Kd_teta*e_tetapnt; if V1>Vmax then V1=Vmax; end if V1<Vmin then V1=Vmin; end
disp(V1);
V2 = Vo -Kp_zponto*e_zponto + Kp_psipnt*e_psipnt - Kp_phi*e_phi - Kd_phi*e_phipnt; if V2>Vmax then V2=Vmax; end if V2<Vmin then V2=Vmin; end
disp(V2);
V3 = Vo -Kp_zponto*e_zponto - Kp_psipnt*e_psipnt + Kp_teta*e_teta + Kd_teta*e_tetapnt; if V3>Vmax then V3=Vmax; end if V3<Vmin then V3=Vmin; end
disp(V3);
V4 = Vo -Kp_zponto*e_zponto + Kp_psipnt*e_psipnt + Kp_phi*e_phi + Kd_phi*e_phipnt; if V4>Vmax then V4=Vmax; end if V4<Vmin then V4=Vmin; end
disp(V4);
disp("----------")
//Matriz dinamica do sistema linearizado
A75 = (4*L*Omega_0^2)/M; A86 = (4*L*Omega_0^2)/M
A913 = (2*L*Omega_0)/M; A914 = A913; A915 = A913; A916 = A913;
A1014 = (2*d*L*Omega_0)/Jx;
A1016=(-2*d*L*Omega_0)/Jx;
A1113 = (2*d*L*Omega_0)/Jy;
A1115=(-2*d*L*Omega_0)/Jy;
A1213 = (-2*D*Omega_0)/Jz;
A1214=(2*D*Omega_0)/Jz;
A1215=(-2*D*Omega_0)/Jz;
A1216=(2*D*Omega_0)/Jz;
A1313 = -(Ke*Kt/R + 2*D*Omega_0)/Jtp; A1414 = A1313; A1515=A1313; A1616 = A1313;
34
A = [0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0
0 0 0 0 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 0 0 0 0 0 0 0 0 1 0 0 0 0 0
0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0
0 0 0 0 A75 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 A86 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 A913 A914 A915 A916
0 0 0 0 0 0 0 0 0 0 0 0 0 A1014 0 A1016
0 0 0 0 0 0 0 0 0 0 0 0 A1113 0 A1115 0
0 0 0 0 0 0 0 0 0 0 0 0 A1213 A1214 A1215 A1216
0 0 0 0 0 0 0 0 0 0 0 0 A1313 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 A1414 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 A1515 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 A1616]
//Matriz de proporcionalidade da entrada
Kv = Kt/(R*Jtp);
B = [0 0 0 0
0 0 0 0
0 0 0 0
0 0 0 0
0 0 0 0
0 0 0 0
0 0 0 0
0 0 0 0
0 0 0 0
0 0 0 0
0 0 0 0
0 0 0 0
Kv 0 0 0
0 Kv 0 0
0 0 Kv 0
0 0 0 Kv]
35
// Vetor das entradas - tens~oes nos motores
u = [V1-Vo;V2-Vo;V3-Vo;V4-Vo]
//Equac~ao matricial
dx = A*x + B*u ;
endfunction
x= ode ("adams ", X0_barra, t0, t, f);
36
Seguem os codigos para a simulacao realizada no Scilab do sistema nao-linear.
clc
clear
//simulac~ao do movimento de um quadricoptero no espaco
g = 9.81; // m/s
//Definic~ao dos parametros gerais do quadricoptero
M = 1.0; // kg
Jx = 0.0081; //
Jy = 0.0081; //
Jz = 0.0142; // kg*m
L= 1.46*10^(-5);
D= 3.8*10^(-7);
d = 0.20; // m
//Dados do motor
R=0.107;
Ke=0.0108;
Kt=Ke;
Jmz=1.15*10^(-4);
// parametros inicias para y
Xzero = 0;
Yzero = 0;
Zzero = 0; // m
psi0 = 0;
teta0 =%pi;
fi0 = 0; // rads
Xponto0 = 0;
Yponto0 = 0;
Zponto0 = 0; // m/s
W1zero = 0;
W2zero = 0;
W3zero = 0; // rad/s
37
Omega_0 = sqrt(M*g/(4*L));
disp(Omega_0);
Omega_1 = Omega_0;
Omega_2 = Omega_0;
Omega_3 = Omega_0;
Omega_4 = Omega_0;
// parametros temporais (s)
t0 = 0;
Dt = 0.0001;
tf = .5;
t = [t0:Dt:tf];
// matriz X0
X0 = [Xzero;Yzero;Zzero;psi0;teta0;fi0;Xponto0;Yponto0;Zponto0;W1zero;W2zero;W3zero;Omega_0;Omega_0;Omega_0;Omega_0];
//tensao para omega0
Vo= Ke*Omega_0 + D*(Omega_0^2)*R/Kt;
disp(Vo);
disp("---------")
V1=Vo;
V2=Vo;
V3=Vo;
V4=Vo;
Vmax = 14.4;
Vmin=0.1;
function dx=f(t,x)
//senos e cossenos
S4 = sin(x(4));
S5 = sin(x(5));
S6 = sin(x(6));
C4 = cos(x(4));
C5 = cos(x(5));
C6 = cos(x(6));
38
//matriz A
A411 = S6/C5;
A412 = C6/C5;
A511 = C6;
A512 = -S6;
A611 = S5*S6/C5;
A612 = S5*C6/C5;
A1313 = -Kt*Ke/R/Jmz;
A1414 = -Kt*Ke/R/Jmz;
A1515 = -Kt*Ke/R/Jmz;
A1616 = -Kt*Ke/R/Jmz;
A = [0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 A411 A412 0 0 0 0
0 0 0 0 0 0 0 0 0 0 A511 A512 0 0 0 0
0 0 0 0 0 0 0 0 0 1 A611 A612 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 A1313 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 A1414 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 A1515 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 A1616]
//matriz N
N71=-L/M*(S4*S6+C4*S5*C6);N72=N71;N73=N71;N74=N71;
N81=-L/M*(-C4*S6+S4*S5*C6);N82=N81;N83=N81;N84=N81;
N91=-L/M*(C5*C6);N92=N91;N93=N91;N94=N91;
N102=d*L/Jx; N104=-d*L/Jx;
N105=-Jmz/Jx
N107=(Jy-Jz)/Jx;
N111=d*L/Jy; N113=-d*L/Jy;
39
N116=Jmz/Jy;
N118=(Jz-Jx)/Jy;
N121=-D/Jz;N122=D/Jz;N123=N121;N124=N122;
N129=(Jx-Jy)/Jz;
N131=-D/Jmz;
N142=-D/Jmz;
N153=-D/Jmz;
N164=-D/Jmz;
N = [0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0
N71 N72 N73 N74 0 0 0 0 0
N81 N82 N83 N84 0 0 0 0 0
N91 N92 N93 N94 0 0 0 0 0
0 N102 0 N104 N105 0 N107 0 0
N111 0 N113 0 0 N116 0 N118 0
N121 N122 N123 N124 0 0 0 0 N129
N131 0 0 0 0 0 0 0 0
0 N142 0 0 0 0 0 0 0
0 0 N153 0 0 0 0 0 0
0 0 0 N164 0 0 0 0 0]
//matriz L
L1= x(13)^2;
L2= x(14)^2;
L3= x(15)^2;
L4= x(16)^2;
L5=(-x(13)+x(14)-x(15)+x(16))*x(11);
L6=(-x(13)+x(14)-x(15)+x(16))*x(10);
L7=x(11)*x(12);
L8=x(10)*x(12);
L9=x(10)*x(11);
40
L = [L1;L2;L3;L4;L5;L6;L7;L8;L9]
//matriz u
u = [g;V1+.5;V2;V3;V4]
//matrix B =
Kv=Kt/Jmz/R;
B = [0 0 0 0 0
0 0 0 0 0
0 0 0 0 0
0 0 0 0 0
0 0 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 0 0 0
0 0 0 0 0
0 0 0 0 0
0 Kv 0 0 0
0 0 Kv 0 0
0 0 0 Kv 0
0 0 0 0 Kv]
//equacao matricial
dx = A*x + B*u + N*L;
endfunction
x = ode ("adams ", X0, t0, t, f);
41