szybki algorytm estymacji prędkości wznoszenia czterowirnikowego

szybki algorytm estymacji prędkości wznoszenia czterowirnikowego

MODELOWANIE INŻYNIERSKIE
44, s. 125-130, Gliwice 2012
ISSN 1896-771X
SZYBKI ALGORYTM ESTYMACJI PRĘDKOŚCI WZNOSZENIA
CZTEROWIRNIKOWEGO MIKROWIROPŁATA
Z WYKORZYSTANIEM CZUJNIKA PRZYSPIESZENIA
MARCIN KMIECIK, KRZYSZTOF SIBILSKI
Instytut Inżynierii Lotniczej, Procesowej i Maszyn Energetycznych, Politechnika Wrocławska
e-mail: [email protected], [email protected]
Streszczenie. W artykule rozważono problem poprawy jakości estymacji
prędkości wznoszenia bezzałogowego statku powietrznego pionowego startu
i lądowania klasy mikro za pomocą czujnika ultradźwiękowego i jednostki
do pomiarów inercyjnych (ang. Inertial Measurement Unit, IMU). Praca dotyczy
niskich pułapów, w zakresie od 0 − 6m, a wiec bezpośrednio dotyka problemu
autonomicznego startu i lądowania. Do prezentacji wyników użyto
czterowirnikowego mikrowiropłata (ang. quadrocopter). Zastosowano sterowanie
za pomocą regulatora proporcjonalno-całkująco-różniczkującego PID. Wyniki
poparto badaniami teoretycznymi oraz analizą wyników rzeczywistych –
omawiany algorytm został zaimplementowany w sterowniku mikrośmigłowca.
1. WSTĘP
Stabilizacja pułapu bezzałogowego statku powietrznego jest podstawową kwestią w drodze
do zapewnienia bezpieczeństwa jego działania w trybie autonomicznym. W przypadku statku
pionowego startu i lądowania pozwala na stabilny zawis i systematyczne wykonywanie
dalszych powierzonych mu zadań oraz autonomiczny start i lądowanie. Jednocześnie
zapewnienie stabilnego zawisu bezzałogowego statku powietrznego jest kwestią trudną, gdyż
z punktu widzenia teorii sterowania proces ten jest silnie nieliniowy. Dodatkowym
czynnikiem zwiększającym złożoność zagadnienia jest fakt, że w większości praktycznych
rozwiązań, ze względu na niskie koszty, stabilizacja pułapu wspomnianej klasy statków
powietrznych dokonywana jest przy pomocy czujników ultradźwiękowych. Te z kolei
zapewniają stosunkowo niską rozdzielczość pomiaru (ok. 1cm), częstotliwość próbkowania
na poziomie 20Hz oraz mają zasięg działania ograniczony z góry do – ok.. 7m – i z dołu –
do ok. 0,35m. W związku z tym niemożliwe jest dokładne określenie pułapu, a tym bardziej –
z uwagi na małą rozdzielczość – prędkości wznoszenia. Problem trudności wyznaczenia
prędkości wznoszenia wydaje się być kluczowy, przez wzgląd na ogromne znaczenie
dokładności pochodnej uchybu sterowania w procesie regulacji z zastosowaniem regulatora
PID. Rozwiązaniem wydaje się więc być wyznaczanie prędkości wznoszenia za pomocą
jednostki inercyjnej IMU.
126
M. KMIECIK, K. SIBILSKI
2. PRZYJĘTE OZNACZENIA
Niech I = {ex, ey, ez} oznacza inercjalny układ współrzędnych taki, że oś X wskazuje
północ, oś Y wschód, a oś Z jest równoległa do wektora grawitacji. Niech B = {e1, e2, e3}
będzie układem współrzędnych powiązanym ze statkiem powietrznym, jak pokazano
Rys.13. Układ współrzędnych związany z korpusem
czterowirnikowego mikrowiropłata
na rys. 1. Kąty φ, θ, ψ oznaczają kolejno przechylenie, pochylenie i odchylenie statku
powietrznego. φ oraz θ zostały zaznaczone na rysunku jedynie w celach poglądowych. Niech
Rab oznacza orientację kątową układu b wyrażoną w a. W rzeczywistości kąt θ opisany jest w
układzie B1 wyznaczonym jako B1 = RI1(ψ)I, a kąt φ w układzie B2 = R12(θ)B1. Założono, że
{φ, θ, ψ} są znane.
3. POMIAR PRĘDKOŚCI WZNOSZENIA NA PODSTAWIE JEDNOSTKI IMU
3.1. Opis dynamiki ruchu
Druga zasada dynamiki Newtona opisuje relację między przyłożoną siłą a przyspieszeniem
oraz zachowana jest jedynie dla układów inercjalnych. W kontekście tej pracy uwzględniane
siły to siła ciągu śmigieł i siła przyciągania ziemskiego. Z punktu widzenia dalszych obliczeń
wygodnie jest wyrażać je w układzie B. Niech FB oznacza całkowitą przyłożoną siłę
wyrażoną w B, m masę obiektu, FtB siłę ciągu wyrażoną w B, RIB orientację układu B
wyrażoną w I, g stałą grawitacji a v określa całkowitą prędkość statku powietrznego wyrażoną
w I. Wtedy, dla wolnozmiennego RIB, druga zasada dynamiki Newtona dla ruchu
postępowego przyjmuje postać [1]:
dv
(1)
F B  mRIB
 mRIB a I  Ft B  mRIB g .
dt I
3.2. Interpretacja pomiaru dokonywanego akcelerometrem
Z uwagi na swoją budowę akcelerometr mierzy przyspieszenie bezwzględne układu I.
Zakładając, że jest sztywno umocowany do ramy obiektu i umieszczony w jego środku
SZYBKI ALGORYTM ESTYMACJI PRĘDKOŚCI WZNOSZENIA CZTEROWIRNIKOWEGO… 127
ciężkości, a jego osie pomiarowe są równoległe do wersorów {e1, e2, e3}, pomiar
akcelerometrem am można wyrazić wzorem [2]:
a x 
am   a y   RIB (a I  g ) ,
(2)
 a z 
gdzie aI oznacza całkowite przyspieszenie w punkcie pomiarowym, czyli mierzona jest
różnica między przyspieszeniem układu a przyspieszeniem ziemskim. Równanie to należy
następnie przedstawić w układzie B. Ponadto dokonano podstawienia za aI zgodnie z (1):
Ft B  mRIB g
Ft B
B
am 
 RI g 
.
(3)
m
m
Stąd, w świetle przyjętych założeń, az = FtB / m [1, 3].
3.3. Estymacja prędkości wznoszenia
Kąt odchylenia ψ nie wpływa na pomiar prędkości wznoszenia, założono więc ψ=0.
Można zatem zapisać:
Ft B  mR IB (v  g ) , gdzie [4]:
(4)
 cos 
R   sin  sin 
cos  sin 
B
I
 sin  
cos  sin  cos   .
 sin  cos  cos  
0
(5)
Ponadto oznaczono składowe wektora prędkości wyrażone w układzie współrzędnych
korpusu B jako:
u 
B
RI v   v  .
(6)
 w
Z punktu widzenia estymacji prędkości wznoszenia istotna jest jedynie składowa prędkości
w, w przybliżeniu równoległa do wektora grawitacji. Uwzględniając (2), (3), (4), (5) i (6)
otrzymuje się uproszczony model pomiaru prędkości wznoszenia:
az  w  g cos cos .
(7)
Gdy (RIB1 ≈ I3) prędkość wznoszenia w wyrażona w układzie B1 jest równa prędkości
wznoszenia w układzie inercjalnym. Słuszność wszystkich powyższych uproszczeń zostanie
potwierdzona eksperymentalnie w dalszej części opracowania. Ostatecznie:
w   az  g cos  cos dt .
(8)
3.4. Projekt filtra
Pomiar dokonywany czujnikiem przyspieszenia obarczony jest błędem o wielu źródłach.
Do głównych należą: nieliniowość, niemożliwość dokładnej kalibracji czujnika (dokładnego
pomiaru wartości przyspieszenia ziemskiego na zadanej wysokości), zmiany temperatury,
ograniczona prędkość odpowiedzi (inercyjność), ograniczona dokładność przetwornika
128
M. KMIECIK, K. SIBILSKI
analogowo-cyfrowego, czy wreszcie błędy numeryczne związane z obliczeniami
dokonywanymi z wykorzystaniem układów cyfrowych. Wszystkie te elementy sprawiają,
że wykorzystanie w praktyce wzoru (8) bez filtracji jest niemożliwe, ponieważ w stosunkowo
krótkim czasie amplituda sukcesywnie całkowanego błędu pomiarowego przerosłaby istotną
informację. Ponieważ jednak błąd pomiarowy ma charakter wolnozmienny,
w przeciwieństwie do sygnału istotnego ze względu na estymację prędkości wznoszenia,
możliwe jest oddzielenie jednego od drugiego za pomocą filtra górnoprzepustowego.
Projektowanie filtrów jest osobną dziedziną nauki i w tym artykule zaprezentowany zostanie
jedynie zarys tej tematyki wraz z propozycją implementacji. Głównym zadaniem
projektowym jest wybór rodzaju zastosowanego filtra dyskretnego i częstotliwości granicznej.
W przypadku układów sterowania na szczególną uwagę zasługuje także odpowiedź fazowa
filtra. W związku z ograniczonymi możliwościami obliczeniowymi platformy autopilota,
decydująca o ocenie przydatności filtra jest także złożoność obliczeniowa algorytmu
programistycznego. Eksperymentalnie wykazano, że wystarczającym, ze względu
na dokładność i bardzo prostym w implementacji, algorytmem jest filtr dyskretny
Butterwortha drugiego rzędu o transmitancji dyskretnej w postaci ogólnej [5]:
Y ( z )  0  z 11  z 2  2
(9)
H ( z) 

X ( z )  0  z 11  z 2 2
Na rys. 2 przedstawiono porównanie wyników pomiarów prędkości wznoszenia dla trzech
różnych wartości częstotliwości granicznej fg. Dla fg = 0,1Hz widoczny jest wyraźnie wpływ
Rys.14. Dobór częstotliwości granicznej filtracji fg
nieidealnej kalibracji czujnika (co jest nieuniknione) – łatwo zauważyć, że składowa stała nie
została całkowicie wyeliminowana. Dla fg = 1Hz widoczny jest przede wszystkim wzrost
przesunięcia fazowego względem fg = 0,7Hz. Stąd jako optymalną wybrano fg = 0,7Hz, która
jest najmniejszą częstotliwością, dla której nie występuje składowa stała pomiaru.
SZYBKI ALGORYTM ESTYMACJI PRĘDKOŚCI WZNOSZENIA CZTEROWIRNIKOWEGO…
3.5. Implementacja programistyczna filtra
W dziedzinie czasu transmitancja (9) może być zapisana jako:
 x  1 xk 1   2 xk 2  1 yk 1   2 yk 2
,
yk  0 k
0
129
(10)
gdzie x to kolejne wartości wejściowe, a y – wyjściowe. Wartości x, to całkowany dyskretnie
odczyt akcelerometru pomniejszony o przyspieszenie ziemskie wk (8,11). Najprostszy
scenariusz całkowania dyskretnego to reguła prostokątów. Niech ak oznacza powierzchnię
kolejnego prostokąta wyliczonego jako iloczyn (7) i czasu próbkowania dtk:
ak  (a z ,k  g cos  k cos k )dtk
(11)
w  a.
k

k
k
Dla filtra górnoprzepustowego β0 + β1 + β2 = 0 oraz β0 = β2. Wtedy (10) przybiera postać:
 (a  ak 1 )  1 y k 1   2 yk 2
.
(12)
yk  0 k
0
Wartości αi, βi dobrane na podstawie [6] dla częstotliwości granicznej fg = 0,7Hz wynoszą
odpowiednio β0 = 1, α0 = 1, α1 = 0,96937, α2 = -1,9689.
4. PODSUMOWANIE I WNIOSKI
Omawiany algorytm zaimplementowano w sterowniku mikrośmigłowca. Na rys. 3
Rys.15. Porównanie wyników estymacji prędkości wznoszenia za pomocą
sonaru i czujnika przyspieszenia z wykorzystaniem omawianego algorytmu
przedstawiono porównanie jakości estymacji prędkości wznoszenia z wykorzystaniem
omawianego algorytmu w zestawieniu z pomiarem ultradźwiękowym. Widoczna jest wyraźna
130
M. KMIECIK, K. SIBILSKI
poprawa jakości pomiaru. Bardzo istotną zaletą jest także zwiększenie częstotliwości pomiaru
w. W górnej części wykresu zaprezentowano pomiar pułapu z zastosowaniem sonaru po
uprzednim „wygładzeniu” charakterystyki przez uśrednianie (filtracja dolnoprzepustowa) –
dzięki opisywanemu procesowi filtracji w procesie stabilizacji pułapu, możliwa jest znaczna
poprawa jakości sterowania.
Postać dyskretna filtra (12) ma wiele zalet praktycznych względem (10) i (11). Przede
wszystkim implementacja filtra wymaga jedynie 4 operacji mnożenia i 3 dodawania – a więc
złożoność obliczeniowa operacji jest niska (stąd nazwa „szybki algorytm” w tytule artykułu).
Poza tym unikano jawnego całkowania – co jest nie do przecenienia z programistycznego
punktu widzenia, ponieważ w długiej perspektywie zmienna reprezentująca wk mogłaby ulec
przepełnieniu.
LITERATURA
1. Stevens B.L., Lewis F.L.: Aircraft control and simulation. New York: Wiley-Interscience,
2003.
2. Mahony R. et al.: Nonlinear complementary filters on the special orthogonal group. IEEE
Transactions on Automatic Control 2008, 53(5), p. 1203-1218.
3. Beard R.W.: Quadrotor dynamics and control. Brigham Young University, 2006.
4. Sibilski K.: Modelowanie i symulacja dynamiki ruchu obiektow latajacych. Warszawa:
Ofic. Wyd. MH, 2004.
5. Selesnick I. et al.: Generalized Digital Butterworth Filter Design. IEEE Transactions on
Signal Processing 1998, Vol. 46, No. 6.
6. Fisher T.: MKFilter Design Tool, http://www-users.cs.york.ac.uk/~fisher/ .
A FAST ALGORITHM FOR QUADROTOR RATE OF CLIMB
ESTIMATION WITH ACCELEROMETER MEASUREMENTS
Summary. Authors of this work present a fast algorithm for rate of climb
detection of an quadrotor aircraft basing on accelerometer measurements. A
second order discrete Butterworth filter is proposed. Additionally a possibility to
further lessen computational complexity is shown in case of described project
assumptions. A choice for optimal filter's cut-off frequency is backed up by filter
characteristics’ comparison.
Suitability of the above solution is confirmed via rate of climb accelerometer
measurements collected in-flight, i.e. in presence of significant disturbances, in
comparison with rate of climb estimated with an ultrasonic sensor. A major
improvement of altitude stabilization algorithm’s precision is shown after
accelerometer measurements have been incorporated in the feedback loop of a
PID controller.