Implementarea unui sistem

de control pentru un robot tip

quadcopter
Studeni:
Daniel Gheorghi
Ionu Vntu

Coordonatori:
Conf. Dr. Ing. Letiia Mirea
.L. Dr. Ing. Ctlin Brescu

Sesiunea de Comunicri tiinifice Studen eti 2015

Facultatea de Automatic i Calculatoare
Universitatea Tehnic Gheorghe Asachi Ia i

Implementarea unui sistem de control

pentru un robot tip quadcopter
Introducere
Sistemul fizic
Funcionare
Modele matematice
Ecuaiile mecanicii clasice
Formalismul Denavit-Hartenberg
Metoda Euler-Lagrange

Implementare

Simulare MATLAB
Algoritm de control
FreeRTOS i restricii de timp real
Interfaa grafic

Concluzii

Introducere
Sistemul fizic
Funcionare
Modele matematice
Implementare
Concluzii

Drone

Principii de control similare

Introducere
Sistemul fizic
Funcionare
Modele matematice
Implementare
Concluzii

Aplicaii:
Cartografiere
Supraveghere i
fotografie aerian
Militare

Introducere
Sistemul fizic
Funcionare
Modele matematice
Implementare
Concluzii

Senzor distan
Sonar HC-SR-04

Arduino
driver wireless - USB

Cortex M-4
Arhitectur ARM
32 bii
72 MHz
256 KB Flash
48 KB RAM
Senzori MEMS 3D
(accelerometru, giroscop,
busol)
Programator integrat
Port USB utilizator
Porturi USART, SPI

Comunicaie
Wireless
2.4GHz
5 linii
256 canale

Propulsie (max 6.7N/motor)

Motor outrunner BLDC Roxxy 2827/ 34 (max 12A)
Elice 12x4.5
ESC SP-BLC-22T (max 50A)
Acumulator LiPo Turnigy 5000mAh (20-30C)

Introducere
Sistemul fizic
Funcionare
Modele matematice
Implementare
Concluzii
Aeronava
Quadcopter
Lungime bra 22.5cm
Greutate 1.5kg

Introducere
Sistemul fizic
Funcionare
Modele matematice
Implementare
Concluzii

Axele de rotaie
Roll (Ruliu)
Pitch (Tangaj)
Yaw (Giraie)

Hover (planare)

Condiia de stabilitate la punct fix:

- Compensarea forei gravitaionale prin fora de
propulsie
- Anularea cuplurilor de rotaie ale elicelor

Introducere
Sistemul fizic
Funcionare
Modele matematice
Implementare
Concluzii

Ridicare Coborre

Introducere
Sistemul fizic
Funcionare
Modele matematice
Implementare
Concluzii

Deplasare nainte Deplasare napoi

Introducere
Sistemul fizic
Funcionare
Modele matematice
Implementare
Concluzii

Deplasare spre stnga Deplasare spre dreapta

Introducere
Sistemul fizic
Funcionare
Modele matematice
Implementare
Concluzii

Rotire spre stnga Rotire spre dreapta

Introducere
Sistemul fizic
Funcionare
Modele matematice
Implementare
Concluzii

Model bazat pe ecuaiile mecanicii clasice

Alegerea sistemelor de coordonate:

- reper inerial
- reperul ataat quadcopter-ului
- configuraie + (plus)

Unghiurile Roll, Pitch, Yaw

Introducere
Sistemul fizic
Funcionare
Modele matematice
Implementare
Concluzii

Model bazat pe ecuaiile mecanicii clasice

Ecuaia de stare a vitezei liniare - n sistem de

referin inerial

m= + +

m masa quadcopert-ului
g - acceleraia gtavitaional
R matricea de rotaie de la sistemul
de referin al robotului la cel inerial
TB propulsia motoarelor pe axele X, Y
i Z
FD fore de frecare liniare

Introducere
Sistemul fizic
Funcionare
Modele matematice
Implementare
Concluzii

Model bazat pe ecuaiile mecanicii clasice

Ecuaia de stare a vitezei unghiulare - in sistemul de referin al robotului

= =

vectorul vitezelor unghiulare

I matricea de inerie a quadcoper-ului
cuplul total

Introducere
Sistemul fizic
Funcionare
Modele matematice
Implementare
Concluzii

Model bazat pe ecuaiile mecanicii clasice

Relaia dintre viteza unghiular exprimat n sistemul de referin al quadcopter-ului i

derivatele unghiurilor Roll, Pitch, Yaw

- vitezele unghiulare pe axele X, Y, Z

unghiul Phi
-unghiul Theta
-unghiul Psi

Introducere
Sistemul fizic
Funcionare
Modele matematice
Implementare
Concluzii

Modelul cinematic prin formalismul

Denavit - Hartenberg

- Se poate determina modelul cinematic

direct utiliznd doar 4 parametri pentru
a descrie relaia dintre dou repere
consecutive
- Necesit ca fiecare articulaie din
lanul cinematic s aib un singur grad
de libertate
Q: Are aeronava tip quadcopter
articulaii?

A: DA! Chiar 3 art. de rotaie!

Introducere
Sistemul fizic
Funcionare
Modele matematice
Implementare
Concluzii

Modelul cinematic prin formalismul

Denavit - Hartenberg

Similare d.p.d.v. D-H

De ce?
Se consider c lungimea
segmentelor dintre cele 3
articulaii cu un singur grad
de libertate este 0.

Introducere
Sistemul fizic
Funcionare
Modele matematice
Implementare
Concluzii

Modelul cinematic prin formalismul

Denavit - Hartenberg

Se
obin parametrii D-H
di
0
0
0

ai
0
0
0

i
Pitch(q1)
Roll(q2)
Yaw(q3)

i
-90
90
0

i matricea de rotaie conform 0An = 0A11A2...(n-1)An (orientarea asiului fa de sistemul de

coordonate ataat, paralel cu reperul inerial fix al Pmntului)

cos(x) =
C(x)
sin(X) =

Introducere
Sistemul fizic
Funcionare
Modele matematice
Implementare
Concluzii

Modelul dinamic prin metoda

Euler - Lagrange

Abordare energetic a dinamicii unui sistem prin funcia

introdus de Lagrange:
K(q,q) en. cinetic
P(q) en. potenial

Un sistem n micare liber (gravitaional) va parcurge

traiectoria cu variaie minim a energiei
problem
de optimizare cu soluia:
= cupluri neconservative

(0, n micare liber)

cte o relaie pentru fiecare articulaie

Introducere
Sistemul fizic
Funcionare
Modele matematice
Implementare
Concluzii

Modelul dinamic prin metoda

Euler - Lagrange

Forma final a modelului dinamic:

M = matricea de mas 3x3 ce modeleaz
ineriile
C = matricea ce modeleaza forele Coriolis i
centrifugale
G = matricea cuplurilor gravitaionale (nul
pentru modelarea dinamicii unghiulare a unei
aeronave perfect simetric fa de axele de
rotaie)
Modelarea s-a realizat considernd
aeronava quadcopter ca avnd puncte
de mas n centrul de greutate i la
captul celor 4 brae.

Introducere
Sistemul fizic
Funcionare
Modele matematice
Implementare
Concluzii

Simulare

Sistem de ecuaii diferentiale implementat in script s-function

x1=

=
= + +
=
=

x2=

vitezele liniare

x3=
Yaw

vectorul unghiurilor Roll,Pitch i

x4=

acceleraiile unghiulare

Introducere
Sistemul fizic
Funcionare
Modele matematice
Implementare
Concluzii

Simulare

Introducere
Sistemul fizic
Funcionare
Modele matematice
Implementare
Concluzii

Simulare

Introducere
Sistemul fizic
Funcionare
Modele matematice
Implementare
Concluzii

Simulare

Introducere
Sistemul fizic
Funcionare
Modele matematice
Implementare
Concluzii

Simulare

Rezultatul reglrii cu un controller PID la

referin de 10 grade pe axa X(unghi
Phi)

Rezultatul reglrii cu un controller PID la

referin de 10 grade pe axa X(unghi
Phi) n prezena perturbaiilor adugate
in ecuaiile modelului

Introducere
Sistemul fizic
Funcionare
Modele matematice
Implementare
Concluzii

Layerele aplicaiei

Senzori
MEMS
FreeRT
OS

NRF24L
01
(wireles
s)

NRF24L
01
(wireles
s)

Arduin
o

ESC
(invertoare)

Matla
b

Elemente de execuie
(motoare)

GUI

Atitudinea
aeronavei

Introducere
Sistemul fizic
Funcionare
Modele matematice
Implementare
Concluzii

FreeRTOS

Task1 (T = 0.5s)
Procesare date primite de la utilizator
Modificare variabile de stare
Modificare de funcionare (iniializare, oprire, reglare unghi etc.)

Task2 (T = 0.01s)

Citire senzori (accelerometru, giroscop, busol electronic, sonar)

Procesare orientare quadcopter prin filtru complementar
Citire buffere de recepie ale modulului wireless
Scriere n bufferele de transmisie ale modulului wireless
CPU Load: 60%

Task3 (T = 0.01s)
Generare comand pentru motoare
CPU Load: 0.8% pentru un controller PID clasic

Introducere
Sistemul fizic
Funcionare
Modele matematice
Implementare
Concluzii

Vizualizare
orientare i
referin

Interfaa Matlab

Atitudine
a
aeronave
i

Date
senzori

nregistrare
date pentru
identificare
sistem

Pupitru
comand

Introducere
Sistemul fizic
Funcionare
Modele matematice
Implementare
Concluzii

Elemente de siguran

Limitare comand
Fiindc sistemul are frecri neglijabile, poate fi uor
destabilizat
Astfel, comanda este limitat la valori care s nu permit o
dinamic foarte rapid, cel puin pentru perioada de testare

Conexiune
Conexiunea dintre interfaa utilizator i aeronav este
verificat periodic
Detecie problem conexiune
oprire motoare!

Buton interfa
Butonul STOP oprete imediat semnalele de comand pentru

Introducere
Sistemul fizic
Funcionare
Modele matematice
Implementare
Concluzii

Troubleshoot

Modul wireless NRF24L01

Datasheet: CE 150us pentru transmisie
Realitate: CE minim 500us pentru transmisie

Comunicaie serial Arduino Matlab

Fr o limitare strict a fluxului de date, canalul se blocheaza des
Nu se poate respecta Postulatul Valvano dect n sistemul ncorporat

Perturbaii
Sistemul este uor de perturbat, chiar i de curenii de aer provocai de
elice ntr-un mediu restrns

Bateria
Scade drastic puterea motoarelor chiar daca nu este complet descrcat.

Introducere
Sistemul fizic
Funcionare
Modele matematice
Implementare
Concluzii

S-au realizat

Softul necesar interfarii cu procesul (citire date,

transmisie), controlului manual, implementrii unei legi
de reglare convenionale sau neconvenionale i stocrii
de date n vederea identificrii.
Modele matematice ale procesului obinute analitic.
Terenul pentru o modelare experimental (identificare).
Controler al unghiului pe o ax de rotaie.
Observaie: cadrul fizic al procesului (asiul, motoarele i ESC-urile) a fost preluat de la un proiect
anterior al altei echipe

Introducere
Sistemul fizic
Funcionare
Modele matematice
Implementare
Concluzii

Se propune

Realizarea controlului pe 3 axe (2 de rotaie i una de

translaie)
Optimizarea layerelor software.
mbuntirea modelului matematic obinut din ecuaiile
mecanicii clasice.
Completarea modelului dinamic Euler-Lagrange cu
dinamica pe axele liniare.