码迷,mamicode.com
首页 > 其他好文 > 详细

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

时间:2016-12-27 14:05:07      阅读:782      评论:0      收藏:0      [点我收藏+]

标签:编程   update   读取   数据   dmp   加速度计   ack   设计   oid   

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

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

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

标签:编程   update   读取   数据   dmp   加速度计   ack   设计   oid   

原文地址:http://www.cnblogs.com/ningxiwumeng/p/6225506.html

(0)
(0)
   
举报
评论 一句话评论(0
登录后才能评论!
© 2014 mamicode.com 版权所有  联系我们:gaon5@hotmail.com
迷上了代码!