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:$a$$b$$c$

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ów$c$
2. podziel go na komponenty$x, 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?

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. $A$$B$

( poprzez programowanie Fx ): $\theta = math.atan2(B_{x}-A_{x}, B_{y}-A_{y})$

Kąt odchylenia pojazdu

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

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$

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

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.$(\theta-\psi)$$x$$y$

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.$x$$y$

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).