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

Transkrypt

1 MODELOWANE NŻYNERSKE SSN X 44, s , Gliwice 202 SZYBK ALGORYTM ESTYMACJ PRĘDKOŚC WZNOSZENA CZTEROWRNKOWEGO MKROWROPŁATA Z WYKORZYSTANEM CZUJNKA PRZYSPESZENA MARCN KMECK, KRZYSZTOF SBLSK nstytut nżynierii Lotniczej, Procesowej i Maszyn Energetycznych, Politechnia Wrocławsa marcin.mieci@pwr.wroc.pl, rzysztof.sibilsi@pwr.wroc.pl Streszczenie. W artyule rozważono problem poprawy jaości estymacji prędości wznoszenia bezzałogowego statu powietrznego pionowego startu i lądowania lasy miro za pomocą czujnia ultradźwięowego i jednosti do pomiarów inercyjnych (ang. nertial Measurement Unit, MU). Praca dotyczy nisich pułapów, w zaresie od 0 6m, a wiec bezpośrednio dotya problemu autonomicznego startu i lądowania. Do prezentacji wyniów użyto czterowirniowego mirowiropłata (ang. quadrocopter). Zastosowano sterowanie za pomocą regulatora proporcjonalno-całująco-różniczującego PD. Wynii poparto badaniami teoretycznymi oraz analizą wyniów rzeczywistych omawiany algorytm został zaimplementowany w sterowniu mirośmigłowca.. WSTĘP Stabilizacja pułapu bezzałogowego statu powietrznego jest podstawową westią w drodze do zapewnienia bezpieczeństwa jego działania w trybie autonomicznym. W przypadu statu pionowego startu i lądowania pozwala na stabilny zawis i systematyczne wyonywanie dalszych powierzonych mu zadań oraz autonomiczny start i lądowanie. Jednocześnie zapewnienie stabilnego zawisu bezzałogowego statu powietrznego jest westią trudną, gdyż z puntu widzenia teorii sterowania proces ten jest silnie nieliniowy. Dodatowym czynniiem zwięszającym złożoność zagadnienia jest fat, że w więszości pratycznych rozwiązań, ze względu na nisie oszty, stabilizacja pułapu wspomnianej lasy statów powietrznych doonywana jest przy pomocy czujniów ultradźwięowych. Te z olei zapewniają stosunowo nisą rozdzielczość pomiaru (o. cm), częstotliwość próbowania na poziomie 20Hz oraz mają zasięg działania ograniczony z góry do o.. 7m i z dołu do o. 0,35m. W związu z tym niemożliwe jest doładne oreślenie pułapu, a tym bardziej z uwagi na małą rozdzielczość prędości wznoszenia. Problem trudności wyznaczenia prędości wznoszenia wydaje się być luczowy, przez wzgląd na ogromne znaczenie doładności pochodnej uchybu sterowania w procesie regulacji z zastosowaniem regulatora PD. Rozwiązaniem wydaje się więc być wyznaczanie prędości wznoszenia za pomocą jednosti inercyjnej MU.

2 26 M. KMECK, K. SBLSK 2. PRZYJĘTE OZNACZENA Niech = {e x, e y, e z } oznacza inercjalny uład współrzędnych tai, że oś X wsazuje północ, oś Y wschód, a oś Z jest równoległa do wetora grawitacji. Niech B = {e, e 2, e 3 } będzie uładem współrzędnych powiązanym ze statiem powietrznym, ja poazano Rys.3. Uład współrzędnych związany z orpusem czterowirniowego mirowiropłata na rys.. Kąty φ, θ, ψ oznaczają olejno przechylenie, pochylenie i odchylenie statu powietrznego. φ oraz θ zostały zaznaczone na rysunu jedynie w celach poglądowych. Niech R a b oznacza orientację ątową uładu b wyrażoną w a. W rzeczywistości ąt θ opisany jest w uładzie B wyznaczonym jao B = R (ψ), a ąt φ w uładzie B 2 = R 2 (θ)b. Założono, że {φ, θ, ψ} są znane. 3. POMAR PRĘDKOŚC WZNOSZENA NA PODSTAWE JEDNOSTK MU 3.. Opis dynamii ruchu Druga zasada dynamii Newtona opisuje relację między przyłożoną siłą a przyspieszeniem oraz zachowana jest jedynie dla uładów inercjalnych. W onteście tej pracy uwzględniane siły to siła ciągu śmigieł i siła przyciągania ziemsiego. Z puntu widzenia dalszych obliczeń wygodnie jest wyrażać je w uładzie B. Niech F B oznacza całowitą przyłożoną siłę wyrażoną w B, m masę obietu, F B t siłę ciągu wyrażoną w B, R B orientację uładu B wyrażoną w, g stałą grawitacji a v oreśla całowitą prędość statu powietrznego wyrażoną w. Wtedy, dla wolnozmiennego R B, druga zasada dynamii Newtona dla ruchu postępowego przyjmuje postać []: B B dv B B B F mr mr a Ft mr g. () dt 3.2. nterpretacja pomiaru doonywanego acelerometrem Z uwagi na swoją budowę acelerometr mierzy przyspieszenie bezwzględne uładu. Załadając, że jest sztywno umocowany do ramy obietu i umieszczony w jego środu

3 SZYBK ALGORYTM ESTYMACJ PRĘDKOŚC WZNOSZENA CZTEROWRNKOWEGO 27 ciężości, a jego osie pomiarowe są równoległe do wersorów {e, e 2, e 3 }, pomiar acelerometrem a m można wyrazić wzorem [2]: ax B am a y R ( a g), (2) a z gdzie a oznacza całowite przyspieszenie w puncie pomiarowym, czyli mierzona jest różnica między przyspieszeniem uładu a przyspieszeniem ziemsim. Równanie to należy następnie przedstawić w uładzie B. Ponadto doonano podstawienia za a zgodnie z (): B B B Ft mr g B Ft am R g. (3) m m Stąd, w świetle przyjętych założeń, a z = F t B / m [, 3] Estymacja prędości wznoszenia Kąt odchylenia ψ nie wpływa na pomiar prędości wznoszenia, założono więc ψ=0. Można zatem zapisać: B B F mr ( v g), gdzie [4]: (4) t cos 0 sin B R sin sin cos sin cos. (5) cos sin sin cos cos Ponadto oznaczono sładowe wetora prędości wyrażone w uładzie współrzędnych orpusu B jao: u R B v v. (6) w Z puntu widzenia estymacji prędości wznoszenia istotna jest jedynie sładowa prędości w, w przybliżeniu równoległa do wetora grawitacji. Uwzględniając (2), (3), (4), (5) i (6) otrzymuje się uproszczony model pomiaru prędości wznoszenia: w g cos cos. (7) a z Gdy (R B 3 ) prędość wznoszenia w wyrażona w uładzie B jest równa prędości wznoszenia w uładzie inercjalnym. Słuszność wszystich powyższych uproszczeń zostanie potwierdzona esperymentalnie w dalszej części opracowania. Ostatecznie: w az g cos cos dt. (8) 3.4. Projet filtra Pomiar doonywany czujniiem przyspieszenia obarczony jest błędem o wielu źródłach. Do głównych należą: nieliniowość, niemożliwość doładnej alibracji czujnia (doładnego pomiaru wartości przyspieszenia ziemsiego na zadanej wysoości), zmiany temperatury, ograniczona prędość odpowiedzi (inercyjność), ograniczona doładność przetwornia

4 28 M. KMECK, K. SBLSK analogowo-cyfrowego, czy wreszcie błędy numeryczne związane z obliczeniami doonywanymi z wyorzystaniem uładów cyfrowych. Wszystie te elementy sprawiają, że wyorzystanie w pratyce wzoru (8) bez filtracji jest niemożliwe, ponieważ w stosunowo rótim czasie amplituda sucesywnie całowanego błędu pomiarowego przerosłaby istotną informację. Ponieważ jedna błąd pomiarowy ma charater wolnozmienny, w przeciwieństwie do sygnału istotnego ze względu na estymację prędości wznoszenia, możliwe jest oddzielenie jednego od drugiego za pomocą filtra górnoprzepustowego. Projetowanie filtrów jest osobną dziedziną naui i w tym artyule zaprezentowany zostanie jedynie zarys tej tematyi wraz z propozycją implementacji. Głównym zadaniem projetowym jest wybór rodzaju zastosowanego filtra dysretnego i częstotliwości granicznej. W przypadu uładów sterowania na szczególną uwagę zasługuje taże odpowiedź fazowa filtra. W związu z ograniczonymi możliwościami obliczeniowymi platformy autopilota, decydująca o ocenie przydatności filtra jest taże złożoność obliczeniowa algorytmu programistycznego. Esperymentalnie wyazano, że wystarczającym, ze względu na doładność i bardzo prostym w implementacji, algorytmem jest filtr dysretny Butterwortha drugiego rzędu o transmitancji dysretnej w postaci ogólnej [5]: 2 Y( z) 0 z z 2 H ( z) (9) 2 X ( z) z z Na rys. 2 przedstawiono porównanie wyniów pomiarów prędości wznoszenia dla trzech różnych wartości częstotliwości granicznej f g. Dla f g = 0,Hz widoczny jest wyraźnie wpływ 0 2 Rys.4. Dobór częstotliwości granicznej filtracji f g nieidealnej alibracji czujnia (co jest nieuninione) łatwo zauważyć, że sładowa stała nie została całowicie wyeliminowana. Dla f g = Hz widoczny jest przede wszystim wzrost przesunięcia fazowego względem f g = 0,7Hz. Stąd jao optymalną wybrano f g = 0,7Hz, tóra jest najmniejszą częstotliwością, dla tórej nie występuje sładowa stała pomiaru.

5 SZYBK ALGORYTM ESTYMACJ PRĘDKOŚC WZNOSZENA CZTEROWRNKOWEGO mplementacja programistyczna filtra W dziedzinie czasu transmitancja (9) może być zapisana jao: 0x x 2x2 y 2 y2 y, (0) gdzie x to olejne wartości wejściowe, a y wyjściowe. Wartości x, to całowany dysretnie odczyt acelerometru pomniejszony o przyspieszenie ziemsie w (8,). Najprostszy scenariusz całowania dysretnego to reguła prostoątów. Niech a oznacza powierzchnię olejnego prostoąta wyliczonego jao iloczyn (7) i czasu próbowania dt : a ( az, g cos cos ) dt () w a. Dla filtra górnoprzepustowego β 0 + β + β 2 = 0 oraz β 0 = β 2. Wtedy (0) przybiera postać: 0 ( a a ) y 2 y2 y. (2) Wartości α i, β i dobrane na podstawie [6] dla częstotliwości granicznej f g = 0,7Hz wynoszą odpowiednio β 0 =, α 0 =, α = 0,96937, α 2 = -, PODSUMOWANE WNOSK Omawiany algorytm zaimplementowano w sterowniu mirośmigłowca. Na rys. 3 Rys.5. Porównanie wyniów estymacji prędości wznoszenia za pomocą sonaru i czujnia przyspieszenia z wyorzystaniem omawianego algorytmu przedstawiono porównanie jaości estymacji prędości wznoszenia z wyorzystaniem omawianego algorytmu w zestawieniu z pomiarem ultradźwięowym. Widoczna jest wyraźna

6 30 M. KMECK, K. SBLSK poprawa jaości pomiaru. Bardzo istotną zaletą jest taże zwięszenie częstotliwości pomiaru w. W górnej części wyresu zaprezentowano pomiar pułapu z zastosowaniem sonaru po uprzednim wygładzeniu charaterystyi przez uśrednianie (filtracja dolnoprzepustowa) dzięi opisywanemu procesowi filtracji w procesie stabilizacji pułapu, możliwa jest znaczna poprawa jaości sterowania. Postać dysretna filtra (2) ma wiele zalet pratycznych względem (0) i (). Przede wszystim implementacja filtra wymaga jedynie 4 operacji mnożenia i 3 dodawania a więc złożoność obliczeniowa operacji jest nisa (stąd nazwa szybi algorytm w tytule artyułu). Poza tym uniano jawnego całowania co jest nie do przecenienia z programistycznego puntu widzenia, ponieważ w długiej perspetywie zmienna reprezentująca w mogłaby ulec przepełnieniu. LTERATURA. Stevens B.L., Lewis F.L.: Aircraft control and simulation. New Yor: Wiley-nterscience, Mahony R. et al.: Nonlinear complementary filters on the special orthogonal group. EEE Transactions on Automatic Control 2008, 53(5), p Beard R.W.: Quadrotor dynamics and control. Brigham Young University, Sibilsi K.: Modelowanie i symulacja dynamii ruchu obietow latajacych. Warszawa: Ofic. Wyd. MH, Selesnic. et al.: Generalized Digital Butterworth Filter Design. EEE Transactions on Signal Processing 998, Vol. 46, No Fisher T.: MKFilter Design Tool, A FAST ALGORTHM FOR QUADROTOR RATE OF CLMB ESTMATON WTH ACCELEROMETER MEASUREMENTS Summary. Authors of this wor 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 baced 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 feedbac loop of a PD controller.