Regulator PID, algorytm, quadcopter

[Puchaczov]

Witajcie, mam kilka pytań odnośnie regulatorów PID. Chciałbym wykorzystać je do sterowania silników quadcoptera ale zanim do etapu pisania algorytmu na uC przystąpię, chciałem naskrobać sobie coś wysokopoziomowego żeby zobaczyć wykresy jakie to generuje. Oglądając wykresy z działania takich regulatorów z sieci zacząłem się zastanawiać czy mój algorytm jest poprawny. Może mógłby ktoś rzucić okiem, dodatkowo podrzucę wykresiki. Kod jest w C#, zwrocStr(...) generuje stringa którego należy przekleić do scilaba żeby wypluło ładny wykresik (jeżeli komuś będzie się chciało 🙂).

dane zadania: oś quadcoptera przechylona jest pod kątem 5 stopni do podłoża (+5stopni to przechył lewej końcówki osi w góre, -5stopni to przechył prawej osi w górę). Oczywiście quad powinien mieć każdą oś regulowaną osobno (tak mi się wydaje) ale to jest nie istotne bo to narazie tylko przykład. W realnej implementacji -5 stopni będzie oznaczało +2.5 i -2.5stopna przechyłu końcówek osi

Założenie: regulowaną wielkością jest sygnał PWM którymi będzie sterowany silnik quada, wchodzącymi wartościami jest bieżący kąt nachylenia osi oraz zadany kąt nachylenia osi

   class Program
   {
       private float calkUchyb = 0.0f;
       private float poprzUchyb = 0.0f;
       public string zwrocStr(ref List<float> list)
       {
           StringBuilder str = new StringBuilder();
           str.Append(String.Format("plot([1:{0}],", list.Count.ToString()));
           str.Append("[");
           for (int i = 0; i < list.Count - 1; ++i)
           {
               var item = list[i].ToString();
               str.Append(item);
               str.Append(",");
           }
           str.Append(list[list.Count - 1]);
           str.Append("])");
           return str.ToString();
       }
       public float PID(float biezacyKat, float oczekiwanyKat, ref float wspP, ref float wspI, ref float wspD, ref float stopien)
       {
           float uchyb = biezacyKat - oczekiwanyKat;
           float p = (uchyb * stopien) * wspP;
           float i = (calkUchyb * stopien) * wspI;
           float d = ((uchyb - poprzUchyb) * stopien) * wspD;
           poprzUchyb = uchyb;
           return p + i + d;
       }
       static void Main(string[] args)
       {
           float zadane_nachylenie = 50;
           float biezace_nachylenie = 5;
           float wspP = 0.7f;
           float wspI = 0.0f;
           float wspD = 0;
           float stopien = (2.0f - 1.0f) / 90.0f;
           Program p = new Program();
           List<float> list = new List<float>();
           for(int i = 0; i < 50; ++i)
           {
               float odpowiedz = p.PID(biezace_nachylenie, zadane_nachylenie, ref wspP, ref wspI, ref wspD, ref stopien);
               float noweStopnie = odpowiedz / stopien; //odzyskuje stopnie
               list.Add(biezace_nachylenie);
               biezace_nachylenie = zadane_nachylenie + noweStopnie;
           }
           string str = p.zwrocStr(ref list);
           Console.WriteLine(str);
           Console.ReadKey();
       }
   }

oto jaki otrzymałem wykres:

jednak podam nadmierny (>1) parametr wspP (1.7), wykres zaczyna kiksować i wygląda tak:

podejrzewam jakąś wadę ukrytą w moim algorytmie, prawdopodobnie czegoś nie doczytałem albo niedostatecznie zrozumiałem, może ktoś byłby tak uprzejmy i rzucił okiem 🙂

Dziękuję i jako nowo przybyły witam wszystkich:)

[slawko_k]

Człon I masz raczej źle zrobiony bo calkUchyb nie modyfikujesz a powinieneś za każdym razem go aktualizować chyba ze jest tylko ja nie widzę. Do tego wg mnie powinieneś to podzielic przez czas co jaki tą całkę mierzysz.

Nie rozumię linijki

biezace_nachylenie = zadane_nachylenie + noweStopnie;

Nachylenie odczytujesz z czujników a zadajesz prawdopodobnie 0 czyli poziom. w jakim celu to jakoś ze sobą wiążesz.

Do koptera podpowiem ze potrzebujesz raczej 2 regulatorów do każdej osi.

Jeden dla prędkości kątowej z żyroskopu i na tym spokojnie można już latać i tak wielu "kopterowców" lata temu zaczynało swoją przygodę z tym a drugi dla odchylenia od poziomu który to musisz wyliczyć na podstawie sygnału z żyroskopu i akcelerometru.

[Puchaczov]

masz rację, zapomniałem przecałkować. Tak wygląda poprawiona już funkcja:

       public float PID(float biezacyKat, float oczekiwanyKat, ref float wspP, ref float wspI, ref float wspD, ref float stopien)
       {
           float uchyb = biezacyKat - oczekiwanyKat;
           calkUchyb += uchyb;
           float p = (uchyb * stopien) * wspP;
           float i = (calkUchyb * stopien) * wspI;
           float d = ((uchyb - poprzUchyb) * stopien) * wspD;
           poprzUchyb = uchyb;
           return p + i + d;
       }

z tym czasem to specjalnie go nie umieściłem uznając, że czas t = 1

ta linijka

biezace_nachylenie = zadane_nachylenie + noweStopnie;

jest przyznam dla mnie problematyczna bo nie do końca wiem jak zinterpretować zwracane dane przez funkcję PID. Potraktowałem to jako potrzebe 'przyrostu' wartości i dostosowałem ją do tego co mi się wydawało, że będzie prawidłowe.

Obrazując to liczbowo, pierwszy obrót pętli sprawi, że funkcja PID(5.0, 50.0, 0.7, 0.0, 0.0, 0.0111111114) zwróci ~-0.45. Żeby sobie z tego odzyskać stopnie dzielę to przez zmienną stopnie i wychodzi, że zmienna noweStopnie ma wartość -40.4999962~. Ta wartość to jakiś wartość 'jak daleko jestem od celu' z tego co zrozumiałem. Z tego co zauważyłem to wartość ta jest ujemna gdy wartość nachylenie ma wzrastać, dodatnia gdy będzie spadać. Dzięki temu mam coraz mniejszy uchyb aż w końcu osiąga on wartości bliskie zeru.

Nachylenie odczytujesz z czujników a zadajesz prawdopodobnie 0 czyli poziom. w jakim celu to jakoś ze sobą wiążesz.

chcę sterować poziomem nachylenia quadro do podłoża. Wymyśliłem sobie, że powinienem mieć 'lokalny poziom nachylenia' tzn uznaję, że quadro nie jest przechylone (0 stopni) podczas gdy jest przechylone o jakiś dowolny kąt (n). Dzięki temu wyrównuje lot do tego lokalnego kąta co powinno (według mnie) ułatwić mi sterowanie.

Jeżeli chodzi o czujniki, to mam już dane z magnetometru, akcelerometru i żyroskopu. Dane z acc i gyro są połączone filtrem dopełnień (mam zamiar go trochę przystosować do własnych potrzeb później). Reasumując Roll, Pitch, Yaw już jest. Kupiłem ostatnio również barometr ale nie zdołałem go jeszcze oprogramować. Zbliżam się coraz bardziej do kupna silników i wydrukowania ramy

EDIT: zmieniłem troszeczke sposób liczenia:

   class Program
   {
       private float calkUchyb = 0.0f;
       private float poprzUchyb = 0.0f;
       public string zwrocStr(ref List<float> list)
       {
           StringBuilder str = new StringBuilder();
           str.Append(String.Format("plot([1:{0}],", list.Count.ToString()));
           str.Append("[");
           for (int i = 0; i < list.Count - 1; ++i)
           {
               var item = list[i].ToString();
               str.Append(item);
               str.Append(",");
           }
           str.Append(list[list.Count - 1]);
           str.Append("])");
           return str.ToString();
       }
       public float PID(float biezacyKat, float oczekiwanyKat, ref float wspP, ref float wspI, ref float wspD, ref float stopien)
       {
           float uchyb = oczekiwanyKat - biezacyKat;
           calkUchyb += uchyb;
           float p = (uchyb * stopien) * wspP;
           float i = (calkUchyb * stopien) * wspI;
           float d = ((uchyb - poprzUchyb) * stopien) * wspD;
           poprzUchyb = uchyb;
           return p + i + d;
       }
       static void Main(string[] args)
       {
           float biezace_nachylenie = 5;
           float wspP = 0.9f;
           float wspI = 0.0f;
           float wspD = 0.01f;
           float stopien = (2.0f - 1.0f) / 90.0f;
           float[] zadane_nachylenia = { 50, 25, 27 };
           Program p = new Program();
           List<float> list = new List<float>();
           foreach(var zadane_nachylenie in zadane_nachylenia)
           {
               int f = 0;
               int x = 0;
               while((!(biezace_nachylenie > zadane_nachylenie - 0.05 && biezace_nachylenie < zadane_nachylenie + 0.05) || f < 10) && x < 100)
               {
                   float odpowiedz = p.PID(biezace_nachylenie, zadane_nachylenie, ref wspP, ref wspI, ref wspD, ref stopien);
                   float noweStopnie = odpowiedz / stopien;
                   list.Add(biezace_nachylenie);
                   biezace_nachylenie = biezace_nachylenie + noweStopnie;
                   f += 1;
                   x += 1;
               }
           }
           string str = p.zwrocStr(ref list);
           Console.WriteLine(str);
           Console.ReadKey();
       }
   }

zmiana kąta ( z 5 -> 50 -> 25 -> 27) nachylenia ma taką charakterystykę:

P = 0.9, D = 0.01, I = 0;

15 -> 20 -> 21 -> 22 -> 22.5 -> 23 -> -15 -> -4:

nadal mam wątpliwości czy to działa w 100% poprawnie ponieważ przy daniu zbyt dużego parametru P, dostaję oscylacje które z każdą iteracją są coraz większe i większe, czyż nie powinny być ~stałe?

[mactro]

Oscylacje jak najbardziej mogą się zwiększać. Oznacza to, że źle dobrałeś nastawy regulatora, i sterowany obiekt nie jest już stabilny. Stałe oscylacje też nie są dobre, powinny one gasnąć. Do tłumienia oscylacji używa się członu D. Ogólnie to temat jest bardzo szeroki - na kierunkach typu automatyka i robotyka są całe przedmioty poświęcone tego typu zagadnieniom.

[Polecacz 101]

Produkcja i montaż PCB - wybierz sprawdzone PCBWay!
   • Darmowe płytki dla studentów i projektów non-profit
   • Tylko 5$ za 10 prototypów PCB w 24 godziny
   • Usługa projektowania PCB na zlecenie
   • Montaż PCB od 30$ + bezpłatna dostawa i szablony
   • Darmowe narzędzie do podglądu plików Gerber
Zobacz również » Film z fabryki PCBWay