Prowadzenie quadrotora w kierunku celu

9

Pracuję nad quadrotorem. Znam jego pozycję - , do której chciałbym pójść - pozycję docelową , a następnie obliczam wektor - wektor jednostkowy, który zabierze mnie do mojego celu:abc

c = b - a
c = normalize(c)

Ponieważ quadrotor może poruszać się w dowolnym kierunku bez obrotu, próbowałem to zrobić

  1. obróć o kąt odchylenia robotówc
  2. podziel go na komponentyx,y
  3. przekaż je robotowi jako kąty przechylenia i nachylenia.

Problem polega na tym, że jeśli odchylenie wynosi 0 ° ± 5, to działa, ale jeśli odchylenie jest bliskie +90 lub -90, to się nie udaje i kieruje się w złym kierunku. Moje pytanie brzmi: czy brakuje mi czegoś oczywistego?

Hamza
źródło
1
Jak obliczasz kąt odchylenia? Z jakiego kontrolera korzystasz i jak wysyłasz dane?
DaemonMaker,
Interesujące pytanie @Hamza, witamy w Robotyka .
Mark Booth
@Hamza, jakiego języka i systemu używasz? Pracuję też nad quadkopterem z językiem programowania Atmega328 i Ada. Jeśli masz bloga o projekcie, udostępnij go.
Masz rację @ MarkBooth, miałem dwie zakładki otwarte i zamierzałem oznaczyć drugi post jako duplikat. Oznacziłem to przez pomyłkę i nie widziałem sposobu, aby go cofnąć. Biorąc pod uwagę, że zamknięcie go wymaga więcej niż jednego głosu, pomyślałem, że to nie będzie problem. Nie zdawałem sobie jednak sprawy, że opublikował komentarz w moim imieniu.
DaemonMaker
Nie ma problemu @DemonemonMaker te rzeczy się zdarzają. Zamknij, ponieważ zduplikowane głosy automatycznie publikują teraz komentarze, co moim zdaniem jest przydatną funkcją, ponieważ zachęca ludzi do przejrzenia drugiego pytania przed oddaniem własnego głosu.
Mark Booth

Odpowiedzi:

6

Ponownie wdrażam twoje rozwiązanie, otrzymuję to:

Kąt między wektorami

Po pierwsze, chcesz kąt między punktami i - nie konkretnie wektor jednostek. ABKąt między 2 punktami

( poprzez programowanie Fx ): θ=math.atan2(BxAx,ByAy)

Kąt odchylenia pojazdu

Następnie (i podejrzewam, że to jest twój problem), musisz odjąć kąt odchylenia pojazdu od obliczonej . ψθ

Heading vs Yaw

Jeśli używasz kompasu do „kąta odchylenia” pojazdu, może to być również twój błąd; kierunek i ziewanie nie są takie same . Kierunek kompasu wynosi zero wzdłuż dodatniej osi , zwiększając się, gdy obraca się w prawo :y

Róża wiatrów

Odchylenie wynosi zero wzdłuż dodatniej osi , zwiększając się, gdy obraca się w lewo :x

Wykres biegunowy

90-stopniowe nakładanie się tych pomiarów, w połączeniu z dodawaniem (zamiast odejmowaniem) odchylenia pojazdu od pożądanego odchylenia, może być powodem, dla którego rzeczy działały, gdy cel był w zakresie ± 5 ° i źle zachowywał się przy ± 90 °.

Konwersja na komponent X i Y

Stamtąd, można powiedzieć, że jest to wynik konwersji w jego i składników, przekazując je do robota jako kątów przechyłu i nachylenia. Dzięki powyższym poprawkom powinieneś uzyskać pożądany wynik w tym momencie. Jednak bezpośrednie mapowanie tych komponentów do kątów pochylenia może być problematyczne, ponieważ bierze się pod uwagę tylko różnicę położenia, a nie prędkość (naprawdę pęd) pojazdu.(θψ)xy

Kontrola PID

Możesz być najlepiej obsługiwany za pomocą pętli kontrolnych PID dla przechyłu i nachylenia pojazdu. Oznacza to, że gdy naprawisz kod i będziesz w stanie trafić swój cel, domyślam się, że zamiast tego zaczniesz go przesadzać - oscylując w tę iz powrotem. Prawidłowo dostrojony PID zapobiegnie temu, a jednocześnie pozwoli szybko zbliżyć się do celu.

Zamiast podłączając swoje i w rolce i boiska, uważają je za tych błędach wartości, rolki i pitch PID przyjąć jako wejście.xy

Ian
źródło
To trochę edycja, ponad 90% i całkowicie zmienia odpowiedź (z PID na ATAN2). Ale twoje krzykliwe umiejętności graficzne są bestią!
Spiked3
Nadal polecam PID (jest na dole), właśnie przepracowałem wstępną część pytania, aby upewnić się, że moje założenia były prawidłowe. Formuły graficzne są częścią formatowania lateksowego, co warto sprawdzić.
Ian
„kurs kompasu zaczyna się od dodatniej osi y i rośnie zgodnie z ruchem wskazówek zegara, podczas gdy odchylenie zaczyna się od dodatniej osi x i zwiększa się w kierunku przeciwnym” „konwersja komponentu x (θ − ψ) na roll i komponentu y na pitch” Nie rozumiem tego wcale - proszę o więcej wyjaśnień (czegoś mi brakuje).
Spiked3
W pierwotnym pytaniu wspomniano „przekazywanie [komponentów x i y] do robota jako kątów przechyłu i pochylenia”, co dla mnie wskazuje, że quadkopter porusza się na boki, zmieniając kąt przechyłu, oraz do przodu i do tyłu, zmieniając kąt pochylenia. Dodam trochę jasności.
Ian
schludny. Nigdy nie widziałem, żeby tak było. Widziałem i robię sam, po prostu odwróć cos / sin, aby uzyskać te same wyniki. Będę musiał przemyśleć jeszcze raz rzut / skok. Tak, spowodowałoby to ruch, ale nie jestem pewien, czy ma to coś wspólnego z tym, gdzie jesteś i dokąd zmierzasz, poza prędkością, z jaką tam dotrzesz. Dzięki.
Spiked3
5

Zakładam, że mówisz tutaj o wektorze 3D. Czy możesz tak po prostu uogólnić normalize()? Czy to takie powszechne (nigdy tego nie widziałem, więc jeśli tak, to wieść do mnie). W przeciwnym razie oczywiste problemy z zawijaniem kompasu dotyczą każdego ze składników X i Y. Dlaczego nie nazwać ich rzutem i / lub skokiem i / lub odchyleniem? (mieszanie nomenklatury 3D i 2D myli pytanie).

Moja normalizacja 2D wygląda mniej więcej tak;

int Pilot_QuickestTurnTo(int hdgNow, int hdgNew)
{
    hdgNow = Pilot_Hdg360(hdgNow);
    hdgNew = Pilot_Hdg360(hdgNew);
    if (hdgNow < hdgNew)
        hdgNow += 360;
    int left = hdgNow - hdgNew;
        return (left < 181 ? -left : 360 - left);
}

Jeśli to rzeczywiście quad, zakładam, że twoje komponenty X i Y to tak naprawdę YAW, wysokość ((X, Y) i Z). Będziesz musiał poradzić sobie YAW(X, Y)w 2D i po prostu upuścić lub zyskać wysokość dla Z (i znowu dlatego podejrzewam, że normalizacja jest czymś więcej niż masz).

Kolczasty 3
źródło