0.

Tytuł
1. Opis technologii i urządzeń współpracujących.
1.1. Cele i zakres projektu.
1.2. Komunikacja między urządzeniami.
1.3. Sterowanie quadrokopterem.
1.4. Obsługa silników.
1.5. Wizualizacja oraz kontrola za pośrednictwem Matlab'a.
1.6. Standardy komunikacji.
2. Opis przyjętych rozwiązań projektowych.
2.1. Komunikacja przy pomocy Matlaba za pośrednictwem USART i portu RS232.
2.2. Wizualizacja stanu rzeczywistego quadrokoptera przy pomocy panelu
operatorskiego.
2.3. Regulator położenia.
2.4. Modelowanie sterowania.
3. Założenia projektu.
3.1. Opis funkcji komunikacyjnych Matlaba oraz protokołu USART dla portu RS232.
3.2. Wizualizacja danych z czujników. Wprowadzenie do środowiska graficznego
Matlaba.
3.3. Regulator PID.
3.4. Teoretyczne rozważania sterowania quadrokopterem.
4. Dokumentacja użytkownika.
4.1. Obsługa urządzenie przy pomocy panelu operatorskiego.
4.2. Obsługa urządzenia korzystając z funkcji użytkowych.
4.3. Odczyt charakterystyk.
4.4. Dobór parametrów regulatora.
5. Dokumentacja serwisowa.
5.1. Instalacja pakietu Matlab.
5.2. Instalacja pakietu WinAvr.
5.3. Instalacja sterowników programatora.
5.4. Program testów komunikacji.
5.5. Program symulacji kinematyki quadrokoptera.
6. Literatura.
 1.Opis technologii i urządzeń współpracujących.

1.1. Cele i zakres projektu.

Celem projektu było zaprojektowanie systemu sterowania oraz wizualizacji układu
quadrokoptera sterowanego za pośrednictwem Matlaba.
W szczególności celem projektu jest:
– zaprojektowanie komunikacji pomiędzy Matlabem, a komputerem pokładowym,
– wizualizacja danych z czujników,
– zaprojektowanie regulatora położenia,
– wyznaczenie kinematyki quadrokoptera,
– zaprojektowanie panelu operatorskiego,
– zaprojektowanie funkcji użytkowych,
– zaprojektowanie programu testu pomiędzy urządzenianiami.

Zakres projektu obejmuje:

– zaprojektowanie funkcji komunikacji od strony mikroprocesora Atmega32 przy
użyciu protokołu USART,
– zaprojektowanie funkcji komunikacyjnych od strony Matlaba z wykorzystaniem
bibliotek programu Matlab,
– analiza danych odebranych z czujników,
– prezentacja liczbowa oraz graficzna otrzymanych danych, przy pomocy bibliotek
graficznych Matlaba,
– dobór nastaw regulatora PID,
– zaprojektowanie regulatora PID,
– ustalenie z jakich elementów będzie się składał panel operatorski,
– zastosowanie funkcji Matlaba przy programowniu panelu operatorskiego,
– zaprojektowanie funkcji do obsługi quadrokoptera,
– zaprojektowanie funkcji pomocy dla użytkownika poszczególnych funkcji,
– zaprojektowanie zadajników,
– zaprojektowanie fizycznego połączenia mikroprocesora i komputera poprzez port
RS232,
– zaprojektowanie funkcji przesyłania danych między urządzeniami,
– zaproponowanie sposobu symulacji zmiany wartości pobieranych z czujników.

Tematem projektu zespołowego jest autonomiczny quadrokopter. Składa się on

z czterech zasadniczych modułów: modułu komputera pokładowego, modułu sterownika
silników, modułu czujników oraz stacji operatorskiej na komputerze PC z oprogramowaniem
"MATLAB". Schemat urządzenia wraz z rozmieszczeniem silników znajduje się na rys. 1. [1]
 Rys.1 Wygląd quadrokoptera [1]

Rys.2 pokazuje gdzie został umiejscowiony przewód zasilający oraz akumulator litowo-
polimerowy służący do zasilania quadrokoptera, jego podzespołów elektronicznych oraz
silników. [1]

Rys.2 Przekrój quadrokoptera [1]

1.2. Komunikacja między urządzeniami. [1]

Moduł komputera pokładowego będzie miał za zadanie nadzorowanie pracy innych
urządzeń na pokładzie quadrokoptera połączonych ze sobą magistralą I2C. Jedynym
masterem magistrali jest komputer pokładowy, pozostałe urządzenia są urządzeniami slave.
Sterowanie quadrokoptera może odbywać się wyłącznie przy pomocy komputera
pokładowego, jednak w szczególnych przypadkach np. prowadzenie testów podzespołów,
komputer PC może przejąć część zadań komputera pokładowego. Do zadań komputera
pokładowego będzie należeć: akwizycja danych pomiarowych z modułu czujników,
sterowanie obrotami silników za pośrednictwem modułu komunikacyjnego silników oraz
komunikacja z komputerem PC. Ponadto moduł komputera posiada także odbiornik GPS,
który jest niezbędny do ustalenia pozycji quadrokoptera. Za pomocą zebranych danych
z modułu czujników będzie możliwa regulacja silnikami tak aby utrzymać stabilny lot oraz
wykonywanie bardziej zaawansowanych funkcji tj. przelot z punktu A do punktu
B o żądanych wcześniej współrzędnych geograficznych. Utrzymanie stabilnego lotu będzie
możliwe dzięki programowym regulatorom w procesorach komputera. Lot będzie możliwy
dzięki zmianie wartości żądanej nachylenia osi quadrokoptera względem ziemi. Zmiana
orientacji będzie możliwa dzięki regulacji obrotów dwóch spośród czterech silników. Zadania
do realizacji przez komputer pokładowy będą wysyłane z komputera PC. Komunikacja
z komputerem PC będzie możliwa dzięki modułowi bluetooth będącego częścią modułu
komputera pokładowego. Komputer wyposażony w podobny moduł będzie mógł nawiązać
połączenie z quadrokopterem przy pomocy komputera pokładowego.

1.3. Sterowanie quadrokopterem. [1]

Jednym z podstawowych problemów sterowania modelem latającym jest problem
lokalizacji, czyli wyznaczenia położenia w globalnym układzie odniesienia.
Najpopularniejszym sposobem precyzyjnego określenia lokalizacji jest połączenie dwóch
różnych czujników: akcelerometru oraz żyroskopu. Oba czujniki powinny pozwalać na
pomiar we wszystkich trzech osiach kartezjańskiego układu współrzędnych. Dane
pochodzące z tego typu czujników, poddane prostym obliczeniom matematycznym, mogą być
źródłem informacji o położeniu. Zaprojektowany w tym celu moduł czujników będzie
odpowiedzialny za akwizycję danych. Pomiar prędkości kątowej w trzech osiach odbywa się
przy pomocy dwóch żyroskopów (ponieważ dwa dwu-osiowe są tańsze niż jeden trój-
osiowy). Dodatkowo za pomocą programowych integratorów będzie możliwość odczytu
orientacji. Drugim zasadniczym czujnikiem umożliwiającym poprawne działanie
quadrokoptera jest akcelerometr. Przy pomocy akcelerometru będzie możliwość pomiaru
przyspieszenia a pośrednio dodatkowo prędkości i położenia. Dzięki współpracy obu
czujników można porównywać wyniki. Jeśli pomiary są skorelowane, można je uznać za
wiarygodne. W przeciwnym razie należy dokonać korekty kątów orientacji lub w skrajnym
przypadku całkowicie odrzucić któryś z pomiarów (traktując go jako błąd gruby). Moduł
czujników komunikuje się z nadrzędnym sterownikiem za pomocą wspólnej dla wszystkich
niezależnych modułów magistrali I2C. Moduł może na żądanie komputera pokładowego
dokonywać jednej z kilku metod przetwarzania danych pomiarowych, takich jak np.
uśrednianie czy też filtr medianowy. Moduł potrafi przesłać komputerowi pokładowemu jedną
z kilku możliwych ramek danych, zawierających potrzebne w danym momencie dane
pomiarowe, np. wyznaczone aktualne współrzędne czy też wektora przyśpieszeniem.

1.4. Obsługa silników. [1]

Moduł komunikacyjny silników i sterownik silnika za zadanie odizolować, magistrale
i2c bezpośrednio obsługującą sterowniki silników, od części zarządzanej przez komputer
pokładowy. Po stronie sterowników silników moduł pośredni sterowników silników pełni role
mastera natomiast od strony komputera pokładowego jest slavem. Logicznie, moduł pośredni,
kumuluje wszystkie parametry dostarczane przez sterowniki silników i zajmuje się ich
dystrybucją. W trybie dostępu rozszerzonego umożliwia połączenie bezpośrednie między
wybranym sterownikiem silnika oraz komputerem pokładowym, dzięki temu komenda
zostanie dostarczona tylko do określonego urządzenia. Zestaw rozkazów przekazywanych do
modułu pośredniego sterowników silników jest uzupełniony o jego diagnostykę oraz
wspominaną funkcje dostępu rozszerzonego. Struktura danych w ramkach przekazywanych
od strony komputera pokładowego nie jest kompatybilna z używaną od strony sterowników
silników. Po obu stronach parametry fizyczne transmisji są identyczne.

1.5. Wizualizacja oraz kontrola za pośrednictwem Matlab'a. [2]

Komputer PC przy pomocy modułu bluetooth będzie mógł nawiązać łączność
z komputerem pokładowym quadrokoptera i za jego pomocą sterować nim przy pomocy
programu "MATLAB". Sterowanie będzie odbywało się poprzez zmianę nastaw regulatora PI.
Ponadto wykorzystując program "MATLAB" będzie możliwa wizualizacja toru lotu
quadrokoptera oraz wszystkich jego parametrów. Będzie stworzony specjalny panel
operatorski w środowisku "MATLAB", panel będzie posiadał okna do wizualizacji położenia
qudrokoptera względem osi X, Y i Z na podstawie nawigacji GPS oraz nachylenia
quadrokoptera względem osi X, Y i Z na podstawie danych z żyroskopów oraz
akcelerometrów. Poza obrazem przedstawiającym helikopter, panel operatorski będzie
zawierał również aktualne odczyty z modułu czujników oraz modułu silników. W przypadku
wystąpienia awarii panel "MATLAB" wyświetli, które urządzenie w którym module przestało
działać. Będzie także możliwa regulacja nastaw regulatorów w komputerze pokładowym oraz
bezpośrednia kontrola silników. W trybie pracy ręcznej "MATLAB" będzie otrzymywał
z kontrolera informacje o zmianie prędkości obrotu silników, które następnie będą wysyłane
do komputera pokładowego. Za pośrednictwem "MATLAB" możliwe będzie awaryjne
wyłączenie quadrokoptera.

1.6. Standardy komunikacji. [1]

Komputer pokładowy łączy się z modułem czujników oraz modułem komunikacyjnym
silników poprzez magistrale I2C. Masterem magistrali jest komputer pokładowy natomiast
urządzenia slave to moduł czujników oraz moduł komunikacyjny silników. Magistrala będzie
pracować z częstotliwością 100 kHz. Magistrala UART będzie łączyć komputer pokładowy
z komputerem pokładowym. Warstwą fizyczną oraz łącza danych magistrali będą protokoły
związane ze standardem bluetooth natomiast warstwą aplikacji będzie protokół modbus.
Masterem magistrali będzie stacja operatorska na komputerze PC z programem MATLAB.
Transmisja będzie się odbywać z prędkością 4800 baud/s z 8 bitami ładunku, bitem
parzystości i jednym bitem stopu.

2.Opis przyjętych rozwiązań projektowych.

2.1. Komunikacja przy pomocy Matlaba za pośrednictwem USART

i portu RS232.
Komunikacja między urządzeniami odbywa się w sposób jednostronny. Komputer
pokładowy nie ma możliwości wysyłania komend, ani danych na własną rękę. Każde
działanie Komputera pokładowego związane z Matlabem jest wywołane przez Matlaba.
Komunikacja od strony Matlab'a opiera się o wbudowaną funkcję komunikacji z
portem RS232. Konfiguracja połączenia tym sposobem wymaga sprecyzowania wszystkich
parametrów komunikacji za pośrednictwem protokołu USART. Matlab posiada wbudowaną
kontrole sprecyzowanych parametrów komunikacyjnych i w wypadku błędnych ich
skonfigurowania wprowadza swoje poprawki. Aczkolwiek bez sprecyzowania parametrów
będą mogły wystąpić błędy komunikacyjne.

Protokół Matlaba opiera się na wysyłaniu znaków w standardzie ASCII. Korzystanie

z tego standardu oznacza przysyłanie 8 bitów danych. W przypadku realizacji projektu
quadrokoptera jest to korzystne i wygodne w obsłudze, ze względu na niewielkie wielkości
danych przesyłanych między urządzeniami.

2.2. Wizualizacja stanu rzeczywistego quadrokoptera przy

pomocy panelu operatorskiego.
W celu wizualizacji aktualnego stanu quadrokoptera niezbędny jest Matlab. Dzięki
wykorzystaniu graficznej wtyczki GUI Matlaba możliwe jest stworzenie stosunkowo
wygodnego i w pełnie użytecznego panelu dla operatora quadrokoptera. Do stworzenia panelu
wymagany jest tylko Matlab wraz z zainstalowanymi standardowymi wtyczkami.

Panel sterowniczy zawiera:

– aktualne położenie helikoptera
– orientacje helikoptera
– wartości nastaw regulatorów helikoptera
– prędkość obrotu silników
– prędkość liniowa środka helikoptera
– prędkość kątowa helikoptera
– tryb pracy helikoptera
– wizualizacje helikoptera z trzech rzutów oraz jego położenia w dwóch rzutach

2.3. Regulator położenia.

Zastosowanie regulatora jest konieczne w celu uzyskania stabilnej pozycji oraz
umożliwienia kontroli quadrokopterem. Stosowanie regulatora opartego na simulinku jest
możliwe jednak jak okazuje praktyka jest zbyt wolne w związku z czym dużo lepszym
rozwiązaniem jest zastosowanie regulatora numerycznego, który będzie regulował położenie
quadrokoptera opierając swoje wyniki na ostatnich próbkach i stosunkowo prostych
obliczeniach.

Regulacja będzie przeprowadzana na dwa sposoby. Pierwszy sposób opiera się na

regulacji położenia quadrokoptera w przestrzeni na podstawie danych z GPS'u oraz
żyroskopu. Ten sposób regulacji wykorzystuje cztery regulatory z których każdy reguluje
poziom na jakim znajduję się jeden z silników. Innym rodzajem regulacji jest regulacji przy
pomocy orientacji quadrokoptera czyli jego nachylenia wobec wszystkich trzech osi.
W związku z ilością mierzonych kątów regulacja ta przebiega za pośrednictwem trzech
regulatorów nachylenia wobec wszystkich osi. Zastosowanie wielu regulatorów jest
wynikiem tego, że regulacja jest przeprowadzana na rzeczywistym modelu, którego model
matematyczny jest skomplikowany. Wykorzystanie większej ilości regulatorów daję w efekcie
aproksymacje rzeczywistego modelu. W związku z tym, że rzeczywisty model jest
aproksymowany przez dużo mniej skomplikowany, regulatory układu należy dobierać w taki
sposób, aby regulacja przebiegała stosunkowo powoli by uniknąć rozregulowania układu.

2.4. Modelowanie sterowania.

Sterowanie odbywa się z wykorzystaniem regulatorów, w celu stabilizacji pozycji
quadrokoptera oraz bazując na równaniach kinematyki. Obliczenia są przekształcone do
postaci macierzowych.
Rys. 3 Kolejność wykonywanych operacji układu regulacji.

Obliczenia matematyczne pozwalają na aproksymacje obiektu do pewnego

uproszczonego modelu przy pomocy którego będzie się odbywać sterowanie. Aproksymacja
odbywa się poprzez zmierzenie charakterystyk śmigieł quadrokoptera, a następnie przy
pomocy jednej z metod parametryzacji sprowadzić charakterystyke siły ciągu quadrokoptera
do liniowej odpowiedzi.

3.Założenia projektu.

3.1. Opis funkcji komunikacyjnych Matlaba oraz protokołu USART

dla portu RS232.
Komunikacja między urządzeniami odbywa się w sposób jednostronny. Komputer
pokładowy nie ma możliwości wysyłania komend, ani danych na własną rękę. Każde
działanie Komputera pokładowego związane z Matlabem jest wywołane przez Matlaba.
Matlab posiada wbudowaną funkcję umożliwiającą komunikację przez port COM. Po
skonfigurowaniu komunikacji poprzez wybrany port Matlab wykonuje operacje zapisy
i odczytu danych w taki sposób jak to jest wykonywane na dowolnym pliku tekstowym.
Matlab wysyła tylko zmienne typu char znajdujące się w standardzie ASCII. W trakcie
konfiguracji portu transmisji należy ustawić prędkość oraz kontrole poprawności transmisji
przy pomocy następujących parametrów: Baud Rate, Data Bits, Stop Bits, Flow Control,
Terminator. Właściwa konfiguracja pomiędzy Matlabem, a urządzeniem podłączonym do
portu szeregowego jest potrzebna w celu poprawnej komunikacji między tymi urządzeniami.
Nadawanie sygnału odbywa się przy pomocy funkcji fprintf, zapisu do niestandardowego
wyjścia zdefiniowanego w argumencie funkcji. Odbiór danych może odbywać przy pomocy
funkcji fscanf w której należy określić typ pobieranej danej oraz miejsce z którego się ją
pobiera. Można tez zastosować funkcję fread gdzie należy podać strumień pobieranych
danych oraz ilość oczekiwanych bajtów do pobrania.

SerPIC = serial('COM1'); % ustalenie portu

set(SerPIC,'BaudRate', 9600, 'DataBits', 8, 'Parity', 'none','StopBits', 2,
'FlowControl', 'none', 'Terminator','LF');
fopen(SerPIC); % otwarcie szeregowego portu PIC
fprintf(SerPIC,'D'); % komenda rozpoczecia pomiaru

pobrany = fread(SerPIC,1);
fclose(SerPIC); % zamkniecie portu po skonczonej transmisji
delete(SerPIC);
clear SerPIC
Kod 1. Komunikacja stacji z komputerem pokładowym. [2]
Komunikacja zwrotna między mikroprocesorem, a Matlabem odbywa się przez protokól
transmisji USART dla komunikacji za pośrednictwem portu RS232. Wykorzystując
transmicje USART należy skonfigurować parametry komunikacji w ten sam sposób w jaki
zostały one sprecyzowane przy konfiguracji portu w Matlabie. W przypadku, gdy programy
nie miałyby tych samych ustawień komunikacja między tymi urządzeniami nie była by
możliwa. Koniguracja poszczególnych rejestrów mikroprocesora oraz szczegółowa instrukcja
 obsługi protokołu transmisji USART jest opisana w instrukcji mikroprocesora Atmega 32.

#define F_CPU 8000000

void InitUSART( unsigned long int baud )

{

UBRRH = (unsigned char)(((F_CPU/(16UL*baud))-1)>>8);

UBRRL = (unsigned char)((F_CPU/(16UL*baud))-1);
// Otwarcie kanalu odbioru i nadawania
UCSRB = (1<<RXEN)|(1<<TXEN);
// rozdzielczosc 8 bitów, 2 bity stopu
UCSRC = (1<<URSEL)|(1<<USBS)|(3<<UCSZ0);
}

unsigned char ReceiveUSART( void ) // funkcja odbioru bajtu z RS232

{
while ( !(UCSRA & (1<<RXC)) );
return UDR;
}

void TransmitUSART( unsigned char data ) // funkcja wysłania bajtu po RS232

{
while ( !( UCSRA & (1<<UDRE)) );
UDR = data;
}
Kod 2. Funkcje komunikacyjne mikroprocesor – Matlab [2]

3.2. Wizualizacja danych z czujników. Wprowadzenie do

środowiska graficznego Matlaba.
Wizualizacja aktualnego stanu pracy quadrokoptera oraz jego położenie w przestrzeni
jest na bieżąco prezentowane na panelu operatorskim. Panel operatorski działa tylko
w środowisku Matlaba i jest niezbędne by Matlab był uruchomiony w trakcie jego pracy.
Panel jest zrobiony z wykorzystaniem graficznego interfejsu Matlaba oraz funkcji
wizualizacji.

Po pobraniu danych z czujników lub zatwierdzeniu nowych nastaw panel aktualizuje

dane pojawiające się na wykresach oraz w ramkach danych. Ważnymi funkcjami panelu są
przyciski sterowania ręcznego w formie kursorów klawiaturowych, przyciski włączania trybu
ręcznego oraz wyłącznik awaryjny helikoptera.

Obsługa elementów panelu od strony programistycznej odbywa się przy pomocy tak
zwanych uchwytów (handles). Uchwyty są to parametry każdego obiektu w środowisku
panelu. Każdy obiekt na panelu posiada swoją indywidualną nazwę dzięki czemu
odwoływanie się do obiektów przy właściwym zastosowaniu nazewnictwa jest uproszczone.
Zmiany w obiektach przeprowadza się za pośrednictwem wcześniej wspomnianych
uchwytów. Chcąc zmienić parametr jakiegoś obiektu wystarczy go wywołać powołując się na
dany uchwyt.

set(handles.uitabela1,'data',tableData);
Kod 3 zmiana parametru obiektu
tableData = get(handles.uitable1,'data');
Kod 4 pobranie danych z parametru obiektu

Wykonanie akcji przy pomocy przycisku wymaga zadeklarowania specjalnej funkcji

obsługującej ten przycisk. Jest to wymagane ponieważ przyciski w środowisku Matlab mogą
pełnić wiele różnych funkcji. W wypadku panelu operatorskiego wymagana jest komunikacja
i reakcja z użytkownikiem w związku z tym należy tak zaprogramować przycisk, aby wysyłał
informacje zwrotną, gdy przycisk zostanie wciśnięty.

function slider_callback;

a = get(gcbo, 'Value');
Kod 6 wywołanie suwaka, oraz pobranie jego wartości.

Panel sterowniczy zawiera:

– aktualne położenie helikoptera
– orientacje helikoptera
– wartości nastaw regulatorów helikoptera
– prędkość obrotu silników
– prędkość liniowa środka helikoptera
– prędkość kątowa helikoptera
– tryb pracy helikoptera
– wizualizacje helikoptera z trzech rzutów oraz jego położenia w dwóch rzutach

Rys. 4 Panel sterowniczy wersja graficzna [2]

3.3. Regulator PID.

Zastosowanie regulatora jest konieczne w celu uzyskania stabilnej pozycji oraz
 uproszczonej kontroli quadrokoptera. Stosowanie regulatora opartego na simulinku jest
możliwe jednak jak pokazuje praktyka jest zbyt wolne w związku z czym dużo lepszym
rozwiązaniem jest zastosowanie regulatora numerycznego, który będzie regulował położenie
quadrokoptera opierając swoje obliczenia na ostatnich próbkach i stosunkowo prostych
obliczeniach.

Rys.5 postać regulatora zaprojektowanego w Simulinku.

Równanie regulatora PID ma następującą postać.

1 t 

(1)
t
u  t = k r [ e  t 
T i
∫
0
e  t  d
t T d d
e
dt
]

Dyskretna postać regulatora.

t T
d

(2)

u  t k = u  t k −1  K p [  1    e  t k 
T i  t

Numeryczna postać rówania:

start:
blad = pozycja_zadana - aktualna_pozycja
I = I + (blad*dt)
D = (blad - blad_wczesniejszy)/dt
wyj = (Kp*blad) + (Ki*I) + (Kd*D)
blad_wczesniejszy = blad
wait(dt)
goto start

Kod 7. Kod poglądowy regulatora w postaci numerycznej

W związku z tym, że regulator jest oparty na teoretycznych obliczeniach należy dla

bezpieczeństwa i komfortu pracy przyjąć dość wolny czas stabilizacji położenia. Dzięki temu
uzyskamy powolną ale skuteczną metodę stabilizacji położenia.
Rys. 6 sposób działania regulatora PI, w wersji numerycznej, dla przykładowych nastaw: Kp = 7,
Ki = 4. Regulacja do poziomu 40, a następnie 10.

3.4. Teoretyczne rozważania na temat sterowania

quadrokopterem.
Sterowanie helikoptera odbywa się bazując na nachyleniu helikoptera we wszystkich
trzech osiach.

a) Sterowanie z wykorzystaniem orientacji helikoptera odbywa się poprzez

wykorzystanie równań różnicowych opisujących ruch w przestrzeni trójwymiarowej. [5]

I ṗ =− I
q
r l T −T 

(3)
4 2

I q̇ =− I
p
r l T −T 

(4)
1 3

2
I ṙ = K  T T −T −T 

(5)
r 1 3 2 4

][ ]
T 1
(6)
[ ][ ] [
l 00 ṗ 0 −l 0 −l
= T 2
0 l 0 q̇ l 0 −l 0
00 l ṙ K r −K r K r − K r T 3
T 4

Jest to równanie wystarczające do sterowania helikopterem określają one zmianę kątów

nachylenia w zależności od siły ciągu silników.

Aby sterować helikopterem w przestrzeni należy zastosować przekształcenie Eulera.

Wykorzystując równania Eulera opisujące orientacje w przestrzeni, powyższe równanie
można przekształcić do następującej postaci oraz przyjmując równość:

T =T T T T

(7)
1 2 3 4

[ ] [ ] [ ] (8)
ẍ 0 0
m ÿ =R p Rq Rr 0  0
z̈ −T m
g

Gdzie Rp, Rq, Rr oznaczają macierze rotacji Eulera kątów p, q i r. [4]

[ ] (9)
1 0 0
R X , p = 0cos p−s in p
0 s
in p c
os p

[ ] (10)
co
s q 0 s
in q
R  Y , q = 0 1 0
−sin q 0 c
osq

2
I ṙ = K  T T −T −T 

(11)
r 1 3 2 4

Wymnożona macierz Rp Rq Rr =

[ (12) ]
c1 c 2 c1 s 2 s3 − s1 c3 s1 s3  c1 s 2 c3
R p Rq Rr = s1 c 2 s 1 s 2 s 3 c 1 c 3 s 1 s 2 c 3− c 1 s 3
− s2 c2 s3 c2 c3

b) Regulacja położenia odbywa się przy pomocy regulatora PID. Aby wyznaczyć
równania, wykonane zostały przekształcenia na podstawowych prawach fizyki. Zgodnie
z 2 prawem dynamiki Newtona.

(13)
F [ N ]= g [ ]
⋅  o
d
c
z
y
t w
a
r
t
o
ś
c
i [ k
g ]− w
a
r
t
o
ś
ć
s 2

Korzystając ze skonstruowanego modelu helikoptera wyznaczamy charakterystykę,

opisującą siłę ciągu w zależności od ilości obrotów silników. Logika podpowiada, że model
tej charakterystyki będzie obiektem pierwszego rzędu. Kolejnym krokiem modelowania
regulatora jest wyznaczenie równania opisującego otrzymaną charakterystykę. Jest to
możliwe poprzez zastosowaniu metody aproksymacji najmniejszych kwadratów. Korzystając
z tej aproksymacji wyznaczono część równania obiektu odpowiadającego wznoszeniu.
 W związku z metodą aproksymacji obiekt opisujący siłę ciągu opisuje równanie [7]:

(14)
F s  s =
s1

Bazując na równaniach ruchu ciała w przestrzeni oraz po przekształceniu transformatą

Laplace'a otrzymujemy następujące równanie opisujące wysokość śmigła nad położeniem
początkowym.[6]
F −m
g =m
a

(15)

d
v F −ma

(16)
=
dt m

F −mg

(17)
s 2 h=
m

(18)
h  s = 2
F m s
m
s

Uproszczony model MatLaba w simulinku wygląda następująco:

Rys 8. regulator położenia. Wysokości lotu quadrokoptera.

Równania dynamiki helikoptera opisane są następującymi wzorami.[6]

 ẋ 1
   x
x   1 u
(19)
= 01  0 1
ẋ 2 00 2

 
x1

(20)
y = 1 0 
x2
 Regulacja z zastosowaniem kąta nachylenia śmigieł. To tego rodzaju regulacji należy
zastosować równania ruchu obrotowego. Ten typ regulacji jest wskazany w związku ze
specyficznym sposobem sterowania czterema śmigłami. Dzięki zastosowaniu tego rodzaju
regulacji można uniknąć przekształceń związanych z zamianą wysokości lotu na nachylenie
do płaszczyzny. [7]
d 

(21)
T =J =F
l
d
t

(22)
  s = 2
F s
J
s

Korzystając z wyznaczonej charakterystyki siły grawitacji oraz z transformaty Laplace'a

otrzymano następujące równanie opisujące śmigła.

Rys. 9 regulator nachylenia.

Równania dynamiki śmigieł są zatem opisane następującymi równaniami [7]:

  xx  
0
  
ẋ 1 =
ẋ 2
01
00
1

2
1
J
u

(23)

 
y = 1 0  x 1
x2
(24)
 4.Dokumentacja użytkownika.

4.1. Obsługa urządzenie przy pomocy panelu operatorskiego.

Aby uruchomić panel sterowniczy należy otworzyć skrypt Matlaba o nazwie Panel.m.
Po uruchomienu tego pliku za pośrednictwem Matlaba, na ekranie pojawia się następujący
panel operatorski.

Rys. 10 Bloki panelu operatorskiego.

Panel operatorski składa się z kilku bloków do obsługi quadrokoptera:

1) Ręczna obsługa silników quadrokoptera. Ręczna zmiana jest możliwa tylko

podczas uruchomionego trybu ręcznego. Przesówanie suwaka do góry zwiększa moc.
Maksymalna moc możliwa do ustawienia na panelu wynosi 60% całkowitej mocy
silników.
2) Uruchamianie trybu ręcznego oraz jego wyłączanie. Umożliwia obsługe
quadrokoptera przy pomocy zmiany mocy silnikow.
3) Zmiany nastaw regulatorów. Regulacja parametrów odbywa się przez wpisanie
parametru regulatora, a następnie wciśnieciu enter.
4) Wyłącznik awaryjny. Wyłącznik awaryjny służy do całkowitego wyłączenia
silników. Wyłącznik awaryjny jest niezbędny, by uniknąć niebezpieczeństw
związanych z operowaniem na silnikach elektrycznych.
5) Pozycja quadrokoptera. Współrzędne quadrokoptera są reprezentowane jako
zmiana pozycji w stosunku do pozycji w której został uruchomiony.
 6) Orientacja quadrokoptera. Orientacja quadrokoptera jest reprezentowana jako
zmiana pozycji w stosunku do pozycji początkowej.
7) Procentowa moc silników.

4.2. Obsługa urządzenia korzystając z funkcji użytkowych.

Obsługa quadrokoptera jest również możliwa za pośrednictwem szeregu funkcji.
Wywołanie funkcji może przebiegać tylko z poziomu Matlaba. Tablica funkcji znajduje się
w załączniku. Wywołanie funkcji polega na wpisaniu urządzenia oraz nowe parametry
urządzenia. Przykładowe wywołanie funkcji zmiany mocy silników wygląda następująco.

Funkcja_4(2,[1 0 1 0 1 0 1 0])
Kod 5 Wywołanie funkcji zmieniającej moce wszystkich silników na 10%.

Wywoływanie funkcji w powyższy sposób jest nadrzędne do wszystkich innych

operacji wywolywanych poprzez Matlaba. To znaczy, że operacje wywoływane przy użyciu
funkcji mogą ustawić prace silników powyżej ograniczeniu 60% zdefiniowanym przy użyciu
panelu operatorskiego.

Stosowanie funkcji jest opisane w helpie funkcji. Wywołanie pomocy jest takie samo
jak wywołanie pomocy dla każdej innej funkcji Matlaba.

Help funkcja_4
Kod 6 Wywołanie pomocy funkcji obsługujących quadrokopter.

Po wywołaniu pomocy pojawi się następujący ekran pomocy.

%Funkcja 4 - Odczyt obrotow silnika -

%funkcja_4(nr_urz, data)
%zawartosc ramki PC -
%zawartosc ramki Quadro - 8 bajt -
% 2 bajty dla kazdego z czterech silnikow
% pierwszy bajt oznacza cyfre dziesietna
% drugi bajt oznacza cyfre jednosci
Kod 7 Pomoc funkcji quadrokoptera.

Każdy plik pomocy posiada informacje odnośnie zadania jakie ma wykonywać dana
funkcja. Sposób wywołania funkcji. Dane zwrotne otrzymywane z komputera pokładowego
quadrokoptera. Ramke wysyłaną do komputera pokładowego. Dane zwrotne oraz nadawcze
są przechowywane w wektorach o długości równej ilości bajtów przesyłanych między
urządzeniami.

4.3. Odczyt charakterystyk.

W związku z tym, że niebezpieczne jest uruchamianie funkcji quadrokoptera bez
wcześniejszego sprawdzenia w jaki sposób będzie się zachowywał quadrokopter. Zalecane
jest wcześniejsze uruchomienie symulacji wznoszenia oraz stabilizacji quadrokoptera.
Stworzone zostały w tym celu funkcje symulacyjne z opisanymi modelami matematycznymi
quadrokoptera wraz z regulatorem. W celu wywołania symulatora należy użyć funkcji
sym_reg. W argumentach funkcji należy wpisać poziom na jaki chcemy wznieść quadrokopter
oraz parametry regulatora. Opis funkcji symulacyjnej jest opisana w pomocy użytkownika.
Wywołanie pomocy jest takie samo jak wywoływanie pomocy funkcji we wcześniejszych
funkcjach.

Po wywołaniu funkcji na ekranie powinien pojawić się obiekt figure zawierający

wysokość wznoszenia się quadrokoptera oraz moc silników potrzebnych do tej operacji.
Przykładowa symulacja jest zaprezentowana w dziale 3.1 opisującym działanie regulatora
PID.

4.4. Dobór parametrów regulatora.

Dobór nastaw regulatorów jest bardzo istotny w związku z tym, że obiekt
quadrokoptera jest trzeciego rzędu. Wysokość rzędu obiektu sprawia, że jest on stosunkowo
trudny do regulacji. Bazując na teorii regulacji wiadomo, że obiekt posiadający części
całkujące dużo łatwiej ustabilizować korzystając z członu różniczkującego [8]. W związku z
tym zaleca się dobór nastaw regulatora rozpoczynając od ustawienia parametru wzmocnienia
oraz różniczkującego, natomiast człon całkujący ustawić na końcu. W związku z charakterem
obiektu najlepiej rozpocząć regulacje ustawiając dużą wartość wzmocnienia kilkukrotnie
większą od pozostałych członów i spróbować otrzymać oscylacje obiektu. Następnie dzięki
członowi różniczkującemu stopniowo go zwiększając przejść z układu przeregulowanego do
powoli stabilizującego się. Na koniec korzystając z członu cąłkującego przyśpieszyć proces
regulacji.
 5.Dokumentacja serwisowa.

5.1. Instalacja pakietu Matlab, WinAVR oraz sterowników

programatora.
Pakiet Matlab nie jest dostępny w wersji darmowej korzystanie z tego pakietu jest
możliwie po wykupieniu licencji. Na potrzeby ukończenia projektu, wykorzystany został
licencjonowany program Matlab znajdujący się w laboratorium Politechniki Wrocławskiej.

W celach programistycznych zastosowano środowisko programistyczne WinAVR.

Środowisko WinAVR jest wersją freeware'ową nie wymagającą rejestracji. Przy
programowaniu mikroprocesora zalecane jest stosowanie tego oprogramowania.

Instalacje sterowników pod systemem windows należy poprzedzić podłączeniem

programatora przez złącze USB. Po wykryciu urządzenia należy zainstalować sterownik
Windows_driver. Następnie wymagana jest instalacja biblioteki lib_usb. Programator jest
gotowy do użycia. Nie zaleca się instalacji biblioteki lib_usb w przypadku posiadania systemu
operacyjnego innego niż windows 2000 lub windows XP. Ponieważ spowoduje to
zatrzymanie działania wszystkich portów USB.

5.2. Program testów komunikacji.

Stacja pomiarowa zawierająca matlaba jest podłączona bezpośrednio przez port COM
do mikroprocesora Atmega32. Do wejścia analogowego mikroprocesora doprowadzony jest
zadajnik napięcia 5V. W Celu sprawdzenia poprawności danych najpierw wysyłany jest
sygnał z wartością napięcia, a po odczekaniu opóźnienia delay = 0.01s wysyłany jest sygnał z
połową wartości napięcia. Badanie ma na celu sprawdzenie dla jakich ustawień portu
komunikacja jest najefektywniejsza.

Badanie szybkości transmisji, przeprowadzono dla prędkości: 2400 b/s, 4800 b/s, 9600
b/s. Komunikacja za pośrednictwem RS-232 jest możliwa również przy prędkości 15600 b/s,
jednak środowisko MatLab'a nie obsługuję tej prędkości, w związku z czym ta prędkość
została pominięta.

Badanie szybkości transmisji:

Szybkość transmisji (4800 b/s),
Badanie parametrów otrzymanego obiektu
Rys. 11. Charakterystyka obiektu mierzonego na układzie Atmega32.

Badanie sygnałów zadawanych przez czujniki, ma na celu sprawdzenie czy Matlab

w poprawny sposób odbiera dane przychodzą przez port RS232. Wszystkie funkcje
wymagające informacji zwrotnej o położeniu helikoptera są generowane na podstawie wejścia
analogowego. Funkcja pobierająca wartość analogową (Kod 10) jest opisana w rozdziale 5.3.
natomiast funkcja opisująca komunikację (Kod 2) w rozdziale 3.1.

Wartości kątów są generowane następująco: minimalna wartość napięcia 0V odpowiada

-300 stopniom, napięcie 2,5V odpowiada 0 stopniom, maksymalne napięcie odpowiada +300
stopniom, wszystkie wartości pośrednie są generowane liniowo.

Położenie helikoptera generowane jest następująco. Położenie zmienia się w zakresie

0 – 10m w płaszczyźnie x oraz y. Wykorzystywane są zadajniki napięciowe generowane tak
samo jak kąty

Obroty silników generowane są na każdym z silników osobno na podstawie wejść

analogowych. Napięcie jest to nastepnie przetwarzane przez mikroprocesor na prędkość
obrotową silników.

Do badania obiektu oraz symulowania sygnałów wejściowych wykorzystywany jest

zadajnik napięciowy oparty o stabilizator LM317 oraz układ oparty na Atmega32.
Rys. 11 Schemat podłączenia komunikacji mikroprocesora Atmega32, układu MAX232 oraz portu
komunikacyjnego RS232. [9]

Rys. 12 Zadajnik napięciowy oparty na układzie LM317. [10]

Korzystając z powyższych układów oraz programu napisanego na mikroprocesorze

możliwe jest przetestowanie funkcji Matlaba.
 5.3. Program symulacji kinematyki quadrokoptera.
Wyznaczanie pozycji quadrokoptera jest możliwa dzięki znajomości jego równań
kinematyki. Korzystając z równania 6 po kilku przekształceniach można otrzymać postać:

[ ][
− ṗ (25)
l

]
Tx
q̇ =
Ty
l
ṙ −T x T y
K

Gdzie siły ciągu są sumowane na każdej z osi. Korzystając z takiego zabiegu możliwe
jest rozwiązanie równania przy znajomości oczekiwanych prędkości kątowych zważywszy, że
oczekujemy, aby quadrokopter stabilizował się powoli wystarczy prędkość mniejsza niż
5 [cm/s]. Należy zwrócić uwagę na występujące minusy w tym układzie równań. Oznacza to,
że silnik pracuje w przeciwnym kierunku. Wnika to z tego, że silniki na osi x pracują w
przeciwnym kierunku by zminimalizować wirowanie quadrokoptera dookoła środka swojej
osi obrotu.

dT = 0.01;
p = 0.05;
q = 0;
r = 0;
katy = [(p/l);(q/l);(r/(2*l))];
T2 = dT + katy(1)/2
T4 = -T2
T1 = dT + katy(2)/2
T3 = -T1
Kod 8. Przykładowe uruchomienie skryptu obliczającego siłę ciągu silników.

Po uruchomieniu skryptu pojawiają się wartości procentowe jakie powinny przyjąć

silniki by quadrokopter uzyskał oczekiwaną prędkość kątową. W powyższym skrypcie
występują taka zmienna jak dT. Zmienna ta określą różnice mocy mocy miedzy dwoma
silnikami znajdującymi się na tej samej osi. Wartość ta powinna być mała, aby quadrokopter
reagował na polecenia powoli. Dodatkowo można zauważyć, że ten sposób wyznaczania siły
ciągu umożliwia silnikom osiąganie mocy większych niż zerowych nawet, gdy nie mają
w danym momencie za zadanie sterowania swoją osią. Silniki te pracują ponieważ mają za
zadanie zredukować obracanie się quadrokoptera względem środka swoich osi. Regulacja
orientacji quadrokoptera wobec środka osi jest sterowana również niezależnie od tych
zabiegów ale użycie takiej metody zmniejsza niechciane rotacje w quadrokopterze.

Uzyskiwanie przez quadrokopter pewnej orientacji jest potrzebne ponieważ dzięki

właściwemu nachyleniu się, quadrokopter przemieszcza się w środowisku zewnętrznym.
Korzystając z równania 8 można wyznaczyć przyśpieszenie quadrokoptera w kierunku jednej
z osi. Wyznaczanie kątów nachylenia odbywa się przy stałej sumarycznej wartości sił ciągów
ponieważ, gdy silnik jednej z osi zaczyna pracować szybciej to silnik z przeciwnej strony tej
samej osi zwalnia obroty. Wyznaczanie tych kątów odbywa się dokładnie z równaniem 8.
 Należy pamiętać, że siła ciągu musi równoważyć siłę grawitacji zgodnie z równaniem 15.

Wyznaczenie charakterystyki siły ciągu jest istotna dla prawidłowej pracy całego
układu. Posiadając charakterystyki rzeczywistych silników możemy je aproksymować do
obiektu 1 rzędu (kod 9).

Motor = (motor1+motor2+motor3+motor4)/4;
a = [ones(length(Motor),1) Motor(:,1)];
Kod 9. Aproksymacja charakterystyki siły ciągu silników motor1-4 do liniowej charakterystyki
siły ciągu całego obiektu.

Komputer pokładowy w celach symulacyjnych generuje wartości na podstawie

zadajnika napięcia. Wartość pobierana przez mikroprocesor jest następnie przetwarzana na
wartość procentową. Przy pomocy kodu 10.

ADMUX=0x40; //konfiguracja rejestru

ADCSRA=0xe0; //konfiguracja rejestru
while(1){
pomiar = (ADC);
wynik = pomiar/256; //pomiar 8bitowy
}
Kod 10. Przykładowy kod do pobierania wartości analogowych.

W zwiazdku z tym, że komunikacja odbywa się przy pomocy kodowania ASCII,

konieczne jest wysyłanie informacji po bajcie. Co w przypadku wysyłanych danych oznacza
wysyłania każdej cyfry pojedyńczo. Funkcje wysyłające bajty za pośrednictwem protokołu
USART są opisane w rozdziale 3.1.

Schemat koncepcyjny funkcji komunikacji za pośrednictwem Matlaba jest opisany

w rozdziale 3.1. Przykładowa funkcja zadająca dane na mikroprocesor wygląda następująco.

function [] = funkcja_2(nr_urz, data)

%Funkcja 2 - Wyslanie nastaw PI -

%zawartosc ramki PC -
%zawartosc ramki Quadro - 4 bajt -
% 2 bajty P
% 2 bajty I

SerPIC = serial('COM1'); %<--change this appropriately

set(SerPIC,'BaudRate', 4800, 'DataBits', 8, 'Parity', 'none','StopBits', 2,
'FlowControl', 'none');
fopen(SerPIC); %--open the serial port to the PID

fprintf(SerPIC, '%s', nr_urz); %===adres urzadzenia

%****************************************************
fprintf(SerPIC, '%s', '2'); %===numer funkcji
%****************************************************
for i=1:1:size(data) %===dane wysylane na komputer pokladowy
fprintf(SerPIC, '%s', data(i));
if mod(i,4) == 0 %===przerwanie
a = 0;
end
end
%****************************************************
sum = ctrlsum(1,nr_urz,data); %===obliczenie sumy kontrolnej
for i=1:1:size(sum) %===suma kontrolna wysylane na komputer
pokladowy
fprintf(SerPIC, '%s', sum(i));
if mod(i,4) == 0 %===przerwanie
a = 0;
end
end
%****************************************************

fclose(SerPIC) %===koniec transmisji

delete(SerPIC)
clear SerPIC
Kod 11. Funkcja zmieniająca nastawy regulatora.
 6.Literatura.
[1] D. Chmielewski, K. Sambor, M. Kulik, Ł. Gąsiewicz, projekt zespołowy, „Model
autonomicznego quadrokoptera”, Politechnika Wrocławska, Wydział Elektroniki, lipiec 2010.
[2] D. Chmielewski, „Model autonomicznego quadrokoptera”, część 3. Stacja badawcza w
środowisku MATLAB, Politechnika Wrocławska, Wydział Elektroniki, lipiec 2010.
[3] W. Greblicki, „Podstawy automatyki”, Wrocław 2006.
[4] K. Tchoń, A. Mazur, R. Hossa, I. Dulęba, R. Muszyński, „Manipulatory i roboty mobilne”,
Akademicka Oficyna Wydawnicza PLJ.
[5] G. Hoffman, D. G. Rajnarayan, S. L. Waslander, „The Stanford testbed of autonomous
rotorcraft for multi agent control”, dostępne pod adresem:
http://hybrid.stanford.edu/~starmac/publications.htm , październik 2010.
[6] V. Sikiric, master of science thesis, „Control of Quadrocopter”, dostępne pod adresem:
http://www.nada.kth.se/utbildning/grukth/exjobb/rapportlistor/2008/rapporter08/sikiric_vedran_0
8027.pdf, październik 2010.
[7] B. Łysakowska, G. Mzyk, „Komputerowa symulacja automatycznej regulacji”, Wrocław
2005.
[8] J. Halaw, „Symulacja i komputerowe projektowanie dynamiki układów sterowania”,
Wrocław 2007.
[9] Dokumentacja mikroprocesora Atmega32, dostępna pod adresem internetowym
http://www.atmel.com/dyn/resources/prod_documents/doc2503.pdf.
[10] Dokumentacja układu LM317, dostępna pod adresem internetowym
http://www.national.com/ds/LM/LM117.pdf.