Estoy trabajando en un quadrotor. Sé su posición, , a dónde me gustaría ir, la posición de destino b , y a partir de eso calculo un vector c , un vector unitario que me llevará a mi objetivo:$a$$b$$c$

c = b - a
c = normalize(c)

Como un quadrotor puede moverse en cualquier dirección sin rotación, lo que he intentado hacer es

1. gire por el ángulo de guiñada de los robots$c$
2. dividirlo en sus componentes $x, y$
3. páselos al robot como ángulos de balanceo y cabeceo.

El problema es que si la guiñada es 0 ° ± 5, entonces esto funciona, pero si la guiñada está cerca de +90 o -90 falla y se dirige hacia direcciones equivocadas. Mi pregunta es ¿me estoy perdiendo algo obvio aquí?

Al volver a implementar su solución, obtengo esto:

Ángulo entre vectores

Primero, quieres el ángulo entre los puntos $A$ y$B$ , no específicamente el vector unitario.

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

Ángulo de guiñada del vehículo

$\psi$$\theta$

Encabezado vs Yaw

$y$

$x$

La superposición de 90 grados entre estas mediciones, combinada con la suma (en lugar de restar) la guiñada del vehículo de la guiñada deseada, puede ser la razón por la cual las cosas funcionaron cuando su objetivo estaba dentro de ± 5 ° y se comportó mal a ± 90 °.

Conversión a Componente X e Y

$(\theta-\psi)$$x$$y$ componentes , pasándolos al robot como los ángulos de balanceo y cabeceo. Con las correcciones anteriores, debería obtener el resultado deseado en este punto. Sin embargo, mapear directamente estos componentes a los ángulos de inclinación puede ser problemático ya que solo está considerando la diferencia de posición y no la velocidad (realmente, el impulso) del vehículo.

Control PID

Puede ser mejor servido usando bucles de control PID para el balanceo y el cabeceo del vehículo. Es decir, una vez que arregle su código y pueda alcanzar su objetivo, supongo que comenzará a sobrepasarlo, oscilando de un lado a otro. Un PID sintonizado correctamente evitará que eso suceda mientras le permite acercarse rápidamente al objetivo.

$x$$y$

Asumiré que estás hablando de un vector 3D aquí. ¿Puedes generalizar normalize()así? ¿Es tan común (nunca lo he visto, así que si es así, entonces noticias para mí). De lo contrario, los problemas obvios de ajuste de la brújula se aplican a cada uno de los componentes X e Y. ¿Por qué no llamarlos rodar y / o lanzar y / o guiñar? (mezclar nomenclatura 3D y 2D confunde la pregunta).

Mi normalización 2D se parece a esto;

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

Si de hecho es un quad, supongo que sus componentes X e Y son realmente YAW, Altitud ((X, Y) y Z). Tendrá que manejar el YAW(X, Y)en 2D, y simplemente dejar caer o ganar altitud para Z (y de nuevo por eso sospecho que normalizar es más de lo que tiene).