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

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

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

时间： 2024-07-30 04:36:56

欧拉角与四元数转换——编程的相关文章

3D数学基础:四元数与欧拉角之间的转换

在3D图形学中,最常用的旋转表示方法便是四元数和欧拉角,比起矩阵来具有节省存储空间和方便插值的优点.本文主要归纳了两种表达方式的转换,计算公式采用3D笛卡尔坐标系: 图1 3D Cartesian coordinate System (from wikipedia) 定义分别为绕Z轴.Y轴.X轴的旋转角度,如果用Tait-Bryan angle表示,分别为Yaw.Pitch.Roll. 图2 Tait-Bryan angles (from wikipedia) 一.四元数的定义 通过旋转轴和绕该

旋转矩阵、欧拉角、四元数理论及其转换关系

博客转载自:http://blog.csdn.net/lql0716/article/details/72597719 1. 概述 旋转矩阵.欧拉角.四元数主要用于表示坐标系中的旋转关系,它们之间的转换关系可以减小一些算法的复杂度. 本文主要介绍了旋转矩阵.欧拉角.四元数的基本理论及其之间的转换关系. 2.原理 2.1 旋转矩阵 对于两个三维点 p1(x1,y1,z1),p2(x2,y2,z2),由点 p1 经过旋转矩阵 R 旋转到 p2,则有 注:旋转矩阵为正交矩阵RRT=E 任意旋转矩阵: 

飞控姿态解算中，欧拉角与四元数之间的转换

//欧拉角转四元数 void cjx_from_euler(float roll, float pitch, float yaw) { float cr2 = cosf(roll*0.5f); float cp2 = cosf(pitch*0.5f); float cy2 = cosf(yaw*0.5f); float sr2 = sinf(roll*0.5f); float sp2 = sinf(pitch*0.5f); float sy2 = sinf(yaw*0.5f); q1 = cr2

【3D计算机图形学】变换矩阵、欧拉角、四元数

[3D计算机图形学]变换矩阵.欧拉角.四元数 旋转矩阵.欧拉角.四元数主要用于:向量的旋转.坐标系之间的转换.角位移计算.方位的平滑插值计算.   一.变换矩阵: 首先要区分旋转矩阵和变换矩阵: 旋转矩阵:向量绕某一个轴旋转,用3x3的矩阵表示. 变换矩阵:向量的移动.旋转.缩放,用4x4的矩阵表示. 这里额外补充一个知识,就是三维坐标变换是用4x4矩阵(采用齐次坐标)而不是3x3矩阵的原因是:统一平移和缩放(本来是向量加法来描述)为矩阵乘法的形式来计算.所以旋转矩阵也扩展为4x4矩阵,这样一来

三维空间旋转（欧拉角、四元数、旋转矩阵）

姿态角(欧拉角) 姿态角即RPY(roll, pitch,yaw)又叫欧拉角,是由三个角组成的. 俯仰角(pitch) 翻滚角(roll) 偏航角(yaw) 其中最直观的就是其绕刚体自身的X.Y.Z三个轴分别旋转的角度,这就是欧拉角(Euler Angle)表示方法. 需要注意的是,欧拉角的表示方式里,yaw.pitch.roll的顺序对旋转的结果是有影响的. 给定一组欧拉角角度值,比如yaw=45度,pitch=30度,roll=60度,按照yaw-pitch-roll的顺序旋转和按照yaw-

Unity复杂的旋转-欧拉角和四元数

一.欧拉角欧拉角最容易表示,用三个变量X,Y,Z可以直观的表示绕着某个轴的旋转角度. 在Unity里就是Transform组件的Rotation里的X Y Z三个变量代表了欧拉角 二.四元数四元数相比于欧拉角就比较复杂了,由四个变量组成(在Unity中称为X,Y,Z,W),但是这些变量的值不代表旋转角度,所以可能给你一个向量(0.7,0,0,0.7)你并不知道实际旋转的角度,当然四元数的详细解释需要数学方面的深入研究,有兴趣的可以自行查找有关资料 因为在Unity没有可视化界面可以调整四元数(因

欧拉角转四元数

二.欧拉角  欧拉角指的是:以世界坐标系为参考坐标系(一定记住是世界坐标系),使用x,y,z三个值来分别表示绕(世界的)x轴.y轴.z轴 旋转的角度量值.其取值是在[0, 360]间.一般用roll, pitch, yaw来表示这些分量的旋转值.因为是以世界坐标系为参考坐标系,因此每一次的旋转都不会影响到后续的旋转转轴.即:它无法表示任意轴的旋转. 一直以为  x轴是pitch   y是yaw ,z 是roll的,今天受到教训了,搞了很久 原来,x轴对应的roll,y轴对应的是pitch  ,z

Matrix4x4矩阵变换、欧拉角转四元数、角度转弧度

Matrix4x4 1 // 重置矩阵 2 void MatrixIdentity(float m[4][4]) 3 { 4 m[0][0] = 1; m[0][1] = 0; m[0][2] = 0; m[0][3] = 0; 5 m[1][0] = 0; m[1][1] = 1; m[1][2] = 0; m[1][3] = 0; 6 m[2][0] = 0; m[2][1] = 0; m[2][2] = 1; m[2][3] = 0; 7 m[3][0] = 0; m[3][1] = 0;

欧拉角与四元数（计算，编程）

欧拉角 四元数计算公式 四元数的基本数学方程为 : q = cos (a/2) + i(x * sin(a/2)) + j(y * sin(a/2)) + k(z * sin(a/2)) 其中a表示旋转角度,(x,y,z)表示旋转轴 下面是如何把具体的四元数与旋转轴和旋转角度对应起来. 1.指出旋转轴和旋转角度,如何转化为四元素. 假定旋转轴是:RAxis = Z轴,换算成三维空间单位向量就是RAxis = [0 0 1],旋转60度 那么转化成四元数就是 q.w=cos(60°/2) = 0.