四元数与欧拉角的转换

【环境】

DJI OSDK 3.9提供了 TOPIC_QUATERNION 遥测数据接口，支持获取飞行器的四元数数据，高达200HZ。

我们知道旋转矩阵、欧拉角、四元数主要用于表示坐标系中的旋转关系，虽然四元数可以帮助我们减少计算量和计算时存储占用的空间，但有些时候我们又需要比较直观的旋转角，比如roll /yaw /pitch，此时我们就需要将四元数转换成欧拉角，在OSDK-3.9的 flight_control sample 中就提供有该转换函数：

Telemetry::Vector3f
toEulerAngle(void* quaternionData)
{
 Telemetry::Vector3f ans;
 Telemetry::Quaternion* quaternion = (Telemetry::Quaternion*)quaternionData;

double q2sqr = quaternion->q2 * quaternion->q2;
 double t0 = -2.0 * (q2sqr + quaternion->q3 * quaternion->q3) + 1.0;
 double t1 =
 +2.0 * (quaternion->q1 * quaternion->q2 + quaternion->q0 * quaternion->q3);
 double t2 =
 -2.0 * (quaternion->q1 * quaternion->q3 - quaternion->q0 * quaternion->q2);
 double t3 =
 +2.0 * (quaternion->q2 * quaternion->q3 + quaternion->q0 * quaternion->q1);
 double t4 = -2.0 * (quaternion->q1 * quaternion->q1 + q2sqr) + 1.0;

t2 = (t2 > 1.0) ? 1.0 : t2;
 t2 = (t2 < -1.0) ? -1.0 : t2;

ans.x = asin(t2);
 ans.y = atan2(t3, t4);
 ans.z = atan2(t1, t0);

return ans;
}

此篇文章分享自 Bruce，如果您有什么疑问或者建议欢迎回复讨论~ 欢迎点赞哦！