欧拉角与四元数转换——编程

Posted 2020-08-25

三、四元数到欧拉角的转换

arctan和arcsin的结果是 ，这并不能覆盖所有朝向(对于 角 的取值范围已经满足)，因此需要用atan2来代替arctan。

四、在其他坐标系下使用
在其他坐标系下，需根据坐标轴的定义，调整一下以上公式。如在Direct3D中，笛卡尔坐标系的X轴变为Z轴，Y轴变为X轴，Z轴变为Y轴（无需考虑方向）。

2.2.3 四元数计算出欧拉角的程序设计
float q0 = 1, q1 = 0, q2 = 0, q3 = 0; //定义四元素
void IMU_Update(void)
{
float norm;
float gx = MPU6500_Gyro.X * GYRO_GR,gy = MPU6500_Gyro.Y * GYRO_GR,gz = MPU6500_Gyro.Z * GYRO_GR;//角度之间的单位转换
float ax = Acc_Avg.X,ay = Acc_Avg.Y,az = Acc_Avg.Z;
float q0q0 = q0 * q0;
float q0q1 = q0 * q1;
float q0q2 = q0 * q2;
float q1q1 = q1 * q1;
float q1q3 = q1 * q3;
float q2q2 = q2 * q2;
float q2q3 = q2 * q3;
float q3q3 = q3 * q3;
float vx, vy, vz;
float ex, ey, ez;
float q0_yawq0_yaw = q0_yaw * q0_yaw;
float q1_yawq1_yaw = q1_yaw * q1_yaw;
float q2_yawq2_yaw = q2_yaw * q2_yaw;
float q3_yawq3_yaw = q3_yaw * q3_yaw;
float q1_yawq2_yaw = q1_yaw * q2_yaw;
float q0_yawq3_yaw = q0_yaw * q3_yaw;

//**************************Yaw轴计算******************************

//Yaw轴四元素的微分方程
q0_yaw = q0_yaw + (-q1_yaw * gx - q2_yaw * gy - q3_yaw * gz) * SAMPLE_HALF_T;
q1_yaw = q1_yaw + (q0_yaw * gx + q2_yaw * gz - q3_yaw * gy) * SAMPLE_HALF_T;
q2_yaw = q2_yaw + (q0_yaw * gy - q1_yaw * gz + q3_yaw * gx) * SAMPLE_HALF_T;
q3_yaw = q3_yaw + (q0_yaw * gz + q1_yaw * gy - q2_yaw * gx) * SAMPLE_HALF_T;
//规范化Yaw轴四元数
norm = sqrt(q0_yawq0_yaw + q1_yawq1_yaw + q2_yawq2_yaw + q3_yawq3_yaw);
q0_yaw = q0_yaw / norm;
q1_yaw = q1_yaw / norm;
q2_yaw = q2_yaw / norm;
q3_yaw = q3_yaw / norm;
if(ax * ay * az == 0)
return ;
//规范化加速度计值
norm = sqrt(ax * ax + ay * ay + az * az); 
ax = ax / norm;
ay = ay / norm;
az = az / norm;
//估计重力方向和流量/变迁
vx = 2 * (q1q3 - q0q2);
vy = 2 * (q0q1 + q2q3);
vz = q0q0 - q1q1 - q2q2 + q3q3 ;
//向量外积再相减得到差分就是误差
ex = (ay * vz - az * vy) ; 
ey = (az * vx - ax * vz) ;
ez = (ax * vy - ay * vx) ;
//对误差进行PI计算
ex_int = ex_int + ex * IMU_KI;
ey_int = ey_int + ey * IMU_KI;
ez_int = ez_int + ez * IMU_KI;

//校正陀螺仪
gx = gx + IMU_KP * ex + ex_int;
gy = gy + IMU_KP * ey + ey_int;
gz = gz + IMU_KP * ez + ez_int;

//四元素的微分方程
q0 = q0 + (-q1 * gx - q2 * gy - q3 * gz) * SAMPLE_HALF_T;
q1 = q1 + (q0 * gx + q2 * gz - q3 * gy) * SAMPLE_HALF_T;
q2 = q2 + (q0 * gy - q1 * gz + q3 * gx) * SAMPLE_HALF_T;
q3 = q3 + (q0 * gz + q1 * gy - q2 * gx) * SAMPLE_HALF_T;

//规范化Pitch、Roll轴四元数
norm = sqrt(q0q0 + q1q1 + q2q2 + q3q3);
q0 = q0 / norm;
q1 = q1 / norm;
q2 = q2 / norm;
q3 = q3 / norm;

//求解欧拉角
Angle.X = atan2(2 * q2q3 + 2 * q0q1, -2 * q1q1 - 2 * q2q2 + 1) * 57.3f;
Angle.Y = asin(-2 * q1q3 + 2 * q0q2) * 57.3f;
Angle.Z = atan2(2 * q1_yawq2_yaw + 2 * q0_yawq3_yaw, -2 * q2_yawq2_yaw - 2 * q3_yawq3_yaw + 1) * 57.3f;
}
程序说明:该程序由开源四轴BlackHole1提供。
2.2.4 硬件加速引擎（DMP）理论分析
MPU-6050整合了3轴陀螺仪、3轴加速器，数字运动处理(DMP: Digital Motion Processor)硬件加速引擎，该DMP功能的最强大功能就是它使用的是MPU6050整合的内部加速引擎，避开上文的利用MPU6050原始数据，通过各种算法得到稳定的四元数，而我们利用官方的DMP驱动程序直接读取出四元数的数值，省下大量的工作量，在利用四元数计算公式计算出稳定，有效的欧拉角。但是因为没有采用地磁补偿，航向角（Yaw）会随时间产生误差。
计算出欧拉角的部分程序如下：
q0 = quat[0] / q30; //q30格式转换为浮点数
q1 = quat[1] / q30;
q2 = quat[2] / q30;
q3 = quat[3] / q30; 
//计算得到俯仰角/横滚角/航向角
*pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch
*roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
*yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3; //yaw