SZYBKI ALGORYTM ESTYMACJI PRĘDKOŚCI WZNOSZENIA CZTEROWIRNIKOWEGO MIKROWIROPŁATA Z WYKORZYSTANIEM CZUJNIKA PRZYSPIESZENIA M

MODELOWANIE INŻYNIERSKIE ISSN 1896-771X 44, s. 125-130, Gliwice 2012

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: marcin.kmiecik@pwr.wroc.pl, krzysztof.sibilski@pwr.wroc.pl

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.

2. PRZYJĘTE OZNACZENIA

Niech I = {e_x, e_y, e_z} 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

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 R_a^b oznacza orientację kątową układu b wyrażoną w a. W rzeczywistości kąt θ opisany jest w układzie B¹ wyznaczonym jako B¹ = R_I¹(ψ)I, a kąt φ w układzie B² = R₁²(θ)B¹. 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 F^B 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]:

g mR F

a dt mR

mR dv

F _I^B _{I t}^B _I^B

I B I

B     . (1)

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 Rys.13. Układ współrzędnych związany z korpusem

czterowirnikowego mikrowiropłata

ciężkości, a jego osie pomiarowe są równoległe do wersorów {e1, e2, e3}, pomiar akcelerometrem a_m można wyrazić wzorem [2]:

) (a g R

a a a

a _I^B _I

z y x

m  





















 , (2)

gdzie a_I 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):

m g F m R

g mR a F

B B t

I B I B t

m   

 . (3)

Stąd, w świetle przyjętych założeń, a_z = F_t^B / 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ć:

) (v g mR

F_t^B  _I^B  , gdzie [4]: (4)



















































cos cos sin

sin cos

cos sin cos

sin sin

sin 0

cos

B

RI . (5)

Ponadto oznaczono składowe wektora prędkości wyrażone w układzie współrzędnych korpusu B jako:





















 w v u v

R_I^B . (6)

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:



 cos cos g w

a_z   . (7)

Gdy (R_I^B1 ≈ I₃) prędkość wznoszenia w wyrażona w układzie B₁ 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:

 



^

 a g dt

w _z coscos . (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

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]:

2 2 1 1 0

2 2 1 1 0

) (

) ) (

(   





















 

 z z

z z

z X

z z Y

H (9)

Na rys. 2 przedstawiono porównanie wyników pomiarów prędkości wznoszenia dla trzech różnych wartości częstotliwości granicznej f_g. Dla f_g = 0,1Hz widoczny jest wyraźnie wpływ

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 f_g = 0,7Hz. Stąd jako optymalną wybrano f_g = 0,7Hz, która jest najmniejszą częstotliwością, dla której nie występuje składowa stała pomiaru.

Rys.14. Dobór częstotliwości granicznej filtracji f_g

3.5. Implementacja programistyczna filtra

W dziedzinie czasu transmitancja (9) może być zapisana jako:

0

2 2 1 1 2 2 1 1 0











  _  _  _  _

 ^{k k k k k}

k

y y

x x

y x , (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 dt_k:

.

) cos cos ( _,









k k k

k k k k

z k

a w

dt g

a

a  

(11)

Dla filtra górnoprzepustowego β₀ + β₁ + β₂ = 0 oraz β₀ = β₂. Wtedy (10) przybiera postać:

0

2 2 1 1 1

0( )







  _  _  _

 ^{k k k k}

k

y y

a

y a . (12)

Wartości α_i, β_i dobrane na podstawie [6] dla częstotliwości granicznej f_g = 0,7Hz wynoszą odpowiednio β₀ = 1, α₀ = 1, α₁ = 0,96937, α₂ = -1,9689.

4. PODSUMOWANIE I WNIOSKI

Omawiany algorytm zaimplementowano w sterowniku mikrośmigłowca. Na rys. 3

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

Rys.15. Porównanie wyników estymacji prędkości wznoszenia za pomocą sonaru i czujnika przyspieszenia z wykorzystaniem omawianego algorytmu

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.