Je travaille sur un quadrirotor. Je connais sa position - , où j'aimerais aller - la position cible b , et à partir de là je calcule un vecteur c - un vecteur unitaire qui m'amènera à ma cible:$a$$b$$c$

c = b - a
c = normalize(c)

Puisqu'un quadrotor peut se déplacer dans n'importe quelle direction sans rotation, ce que j'ai essayé de faire est

1. faire pivoter par l'angle de lacet du robot$c$
2. le diviser en ses composants $x, y$
3. passez-les au robot sous forme d'angles de roulis et de tangage.

Le problème est que si le lacet est de 0 ° ± 5, cela fonctionne, mais si le lacet est proche de +90 ou -90, il échoue et se dirige dans de mauvaises directions. Ma question est-ce que je manque quelque chose d'évident ici?

En réimplémentant votre solution, j'obtiens ceci:

Angle entre les vecteurs

Tout d'abord, vous voulez l'angle entre les points $A$ et $B$ - pas spécifiquement le vecteur unitaire.

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

Angle de lacet du véhicule

$\psi$$\theta$

Heading vs Yaw

$y$

$x$

Le chevauchement de 90 degrés entre ces mesures, combiné à l'ajout (au lieu de soustraire) le lacet du véhicule du lacet souhaité, peut être la raison pour laquelle les choses fonctionnaient lorsque votre cible était à ± 5 ° et se comportait mal à ± 90 °.

Conversion en composantes X et Y

$(\theta-\psi)$$x$$y$

Contrôle PID

Vous pouvez être mieux servi en utilisant des boucles de contrôle PID pour le roulis et le tangage du véhicule. Autrement dit, une fois que vous avez corrigé votre code et que vous êtes en mesure d'atteindre votre cible, je suppose que vous commencerez à le dépasser à la place - oscillant d'avant en arrière. Un PID correctement réglé empêchera cela de se produire tout en vous permettant d'approcher rapidement la cible.

$x$$y$

Je suppose que vous parlez d'un vecteur 3D ici. Pouvez-vous simplement généraliser normalize()comme ça? Est-ce si courant (je ne l'ai jamais vu alors si c'est le cas, alors nouvelles pour moi). Sinon, des problèmes évidents d'enveloppe de boussole s'appliquent à chacun des composants X et Y. Pourquoi ne pas les appeler roulis et / ou tangage et / ou lacet? (mélanger la nomenclature 3D et 2D confond la question).

Ma normalisation 2D ressemble à ceci;

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);
}

S'il s'agit bien d'un quad, je suppose que vos composants X et Y sont vraiment YAW, Altitude ((X, Y) & Z). Vous devrez gérer le YAW(X, Y)2D, et simplement baisser ou gagner de l'altitude pour Z (et encore une fois, c'est pourquoi je soupçonne que normaliser est plus que ce que vous avez).