2 votos

¿Cómo obtener la velocidad angular a partir de la diferencia entre el cuaternión de orientación y el tiempo?

Estoy usando la biblioteca Bullet Physic para programar alguna función, donde tengo la diferencia entre la orientación del giroscopio dada en cuaterniones y la orientación de mi objeto, y el tiempo entre cada cuadro en milisegundos. Todo lo que quiero es establecer la orientación de mi giroscopio a la orientación de mi objeto en el espacio 3D. Pero todo lo que puedo hacer es establecer la velocidad angular a mi objeto. Tengo la diferencia de orientación y el tiempo, es por eso que necesito vector de velocidad angular [Wx,Wy,Wz] de eso.

Después de leer esto: angularVelocidadArtículo1 y esto: angularVelocidadArtículo2

Hice algo así:

btQuaternion diffQuater = gyroQuater - boxQuater;
btQuaternion diffConjQuater;

diffConjQuater.setX(-(diffQuater.x()));
diffConjQuater.setY(-(diffQuater.y()));
diffConjQuater.setZ(-(diffQuater.z()));

////////////////
//W(t) = 2 * dq(t) /dt * conj(q(t))

btQuaternion velQuater;

velQuater = ((diffQuater * 2) / d_time) * diffConjQuater;

Pero no funciona como espero, es decir, está escrito, la parte vectorial del cuaternión resultante debe ser el vector de la velocidad angular, y la parte escalar debe ser 0, pero mi resultado no es así.

vector de velocidad angular representado como cuaternión con parte escalar cero, es decir W (t ) = (0, W x (t ), W y (t ), W z (t ))

¿Alguna idea?

3voto

Mike Puntos 1113

Esta pregunta podría ir mejor en el sitio de GameDev SE Sin embargo, a primera vista, esta fórmula:

velQuater = ((diffQuater * 2) / d_time) * diffConjQuater;

no representa lo que hay en el comentario anterior. La fórmula que buscas es $\omega(t) = 2q'(t)\bar{q}(t)$ (donde como es típico he escrito $q'(t) = \frac{dq(t)}{dt}$ ), pero lo que está escrito en ese código es $\omega(t) = 2q'(t)\bar{q'}(t)$ En otras palabras, no estás multiplicando por el conjugado de tu cuaternión de orientación original (como debería ser) sino por el conjugado de (la aproximación de) la derivada. La línea que quieres debería ser algo así como

velQuater = ((diffQuater * 2) / d_time) * conjBoxQuater;

pero tenga en cuenta que todo esto se basa en otra suposición: que gyroQuater y boxQuater representan la orientación del el mismo objeto en dos puntos cercanos en el tiempo. Por su descripción, parece que este no es el caso, y si no es así, tendrá que ser más explícito sobre el comportamiento que busca.

0voto

mhmd Puntos 11

La forma en que estoy haciendo esto mediante el uso de código siguiente. Su ineficiente sin embargo parece estar trabajando.

static Vector3T toAngularVelocity(const QuaternionT& start, const QuaternionT& end, RealT delta_sec)
{
    RealT p_s, r_s, y_s;
    toEulerianAngle(start, p_s, r_s, y_s);

    RealT p_e, r_e, y_e;
    toEulerianAngle(end, p_e, r_e, y_e);

    RealT p_rate = (p_e - p_s) / delta_sec;
    RealT r_rate = (r_e - r_s) / delta_sec;
    RealT y_rate = (y_e - y_s) / delta_sec;

    //TODO: optimize below
    //Sec 1.3, https://ocw.mit.edu/courses/mechanical-engineering/2-154-maneuvering-and-control-of-surface-and-underwater-vehicles-13-49-fall-2004/lecture-notes/lec1.pdf
    RealT wx = r_rate       + 0                             - y_rate * sinf(p_e);
    RealT wy = 0            + p_rate * cosf(r_e)            + y_rate * sinf(r_e) * cosf(p_e);
    RealT wz = 0            - p_rate * sinf(r_e)            + y_rate * cosf(r_e) * cosf(p_e);

    return Vector3T(wx, wy, wz);
}

static void toEulerianAngle(const QuaternionT& q
    , RealT& pitch, RealT& roll, RealT& yaw)
{
    RealT ysqr = q.y() * q.y();

    // roll (x-axis rotation)
    RealT t0 = +2.0f * (q.w() * q.x() + q.y() * q.z());
    RealT t1 = +1.0f - 2.0f * (q.x() * q.x() + ysqr);
    roll = std::atan2f(t0, t1);

    // pitch (y-axis rotation)
    RealT t2 = +2.0f * (q.w() * q.y() - q.z() * q.x());
    t2 = ((t2 > 1.0f) ? 1.0f : t2);
    t2 = ((t2 < -1.0f) ? -1.0f : t2);
    pitch = std::asinf(t2);

    // yaw (z-axis rotation)
    RealT t3 = +2.0f * (q.w() * q.z() + q.x() * q.y());
    RealT t4 = +1.0f - 2.0f * (ysqr + q.z() * q.z());  
    yaw = std::atan2f(t3, t4);
}

i-Ciencias.com

I-Ciencias es una comunidad de estudiantes y amantes de la ciencia en la que puedes resolver tus problemas y dudas.
Puedes consultar las preguntas de otros usuarios, hacer tus propias preguntas o resolver las de los demás.

Powered by:

X