请问得到四元数后怎么计算YAW，ROLL，PITCH

               

请问得到四元数后怎么计算YAW，ROLL，PITCH？  [复制链接]

cw628 24 主题 0 好友 139 积分 注册会员 莫元 127 发消息	电梯直达 1楼   发表于 2012-7-5 21:32:54  | 只看该作者  | 倒序浏览 我用这个程序得到了四元数，怎么进一步计算姿态YAW，ROLL，PITCH？ //---------------------------------------------------------------------------------------------------- // Header files #include "AHRS.h" #include <math.h> //---------------------------------------------------------------------------------------------------- // Definitions #define Kp 2.0f                        // 比例增益支配收敛率accelerometer/magnetometer #define Ki 0.005f                // 积分增益执政速率陀螺仪的衔接gyroscopeases #define halfT 0.5f                // 采样周期的一半 //--------------------------------------------------------------------------------------------------- // Variable definitions float q0 = 1, q1 = 0, q2 = 0, q3 = 0;        // 四元数的元素，代表估计 方向 float exInt = 0, eyInt = 0, ezInt = 0;        // 按比例缩小积分误差 //==================================================================================================== // Function //==================================================================================================== void AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {         float norm;         float hx, hy, hz, bx, bz;         float vx, vy, vz, wx, wy, wz;         float ex, ey, ez;         // 辅助变量，以减少重复操作数         float q0q0 = q0*q0;         float q0q1 = q0*q1;         float q0q2 = q0*q2;         float q0q3 = q0*q3;         float q1q1 = q1*q1;         float q1q2 = q1*q2;         float q1q3 = q1*q3;         float q2q2 = q2*q2;            float q2q3 = q2*q3;         float q3q3 = q3*q3;                            // 测量正常化         norm = sqrt(ax*ax + ay*ay + az*az);                ax = ax / norm;         ay = ay / norm;         az = az / norm;         norm = sqrt(mx*mx + my*my + mz*mz);                   mx = mx / norm;         my = my / norm;         mz = mz / norm;                           // 计算参考磁通方向         hx = 2*mx*(0.5 - q2q2 - q3q3) + 2*my*(q1q2 - q0q3) + 2*mz*(q1q3 + q0q2);         hy = 2*mx*(q1q2 + q0q3) + 2*my*(0.5 - q1q1 - q3q3) + 2*mz*(q2q3 - q0q1);         hz = 2*mx*(q1q3 - q0q2) + 2*my*(q2q3 + q0q1) + 2*mz*(0.5 - q1q1 - q2q2);                  bx = sqrt((hx*hx) + (hy*hy));         bz = hz;                          //估计方向的重力和磁通（V和W）         vx = 2*(q1q3 - q0q2);         vy = 2*(q0q1 + q2q3);         vz = q0q0 - q1q1 - q2q2 + q3q3;         wx = 2*bx*(0.5 - q2q2 - q3q3) + 2*bz*(q1q3 - q0q2);         wy = 2*bx*(q1q2 - q0q3) + 2*bz*(q0q1 + q2q3);         wz = 2*bx*(q0q2 + q1q3) + 2*bz*(0.5 - q1q1 - q2q2);                    // 错误是跨产品的总和之间的参考方向的领域和方向测量传感器         ex = (ay*vz - az*vy) + (my*wz - mz*wy);         ey = (az*vx - ax*vz) + (mz*wx - mx*wz);         ez = (ax*vy - ay*vx) + (mx*wy - my*wx);                  // 积分误差比例积分增益         exInt = exInt + ex*Ki;         eyInt = eyInt + ey*Ki;         ezInt = ezInt + ez*Ki;                  // 调整后的陀螺仪测量         gx = gx + Kp*ex + exInt;         gy = gy + Kp*ey + eyInt;         gz = gz + Kp*ez + ezInt;                  // 整合四元数率和正常化         q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;         q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;         q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;         q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;                    // 正常化四元         norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);         q0 = q0 / norm;         q1 = q1 / norm;         q2 = q2 / norm;         q3 = q3 / norm; } //==================================================================================================== // END OF CODE //====================================================================================================
	收藏13 顶、支持0 踩、反对0
	回复 举报

js200300953 7 主题 1 好友 331 积分 中级会员 莫元 315 发消息	2楼   发表于 2012-7-6 08:13:22  | 只看该作者 这里有讲 本帖子中包含更多资源 您需要 登录 才可以下载或查看，没有帐号？注册
	回复 举报

js200300953 7 主题 1 好友 331 积分 中级会员 莫元 315 发消息	3楼   发表于 2012-7-6 10:59:20  | 只看该作者 这段话很奇怪：         // 正常化四元         norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);         q0 = q0 / norm;         q1 = q1 / norm;         q2 = q2 / norm;         q3 = q3 / norm; 复制代码 既然精度都是float，为什么用sqrt？用sqrtf啊。 除法一般比乘法慢很多，应该把除法移到第一句。 单位化向量有“超级快速”的算法（刚刚才发现的），见平方根倒数速算法。
	博客地址
	回复 举报

cw628 24 主题 0 好友 139 积分 注册会员 莫元 127 发消息	4楼   发表于 2012-7-6 23:41:10  | 只看该作者 js200300953 发表于 2012-7-6 10:59  这段话很奇怪： 既然精度都是float，为什么用sqrt？用sqrtf啊。 除法一般比乘法慢很多，应该把除法移 ... 四元数和卡尔曼滤波是什么关系？一直没有搞懂～  我用的卡尔曼是这样的：以Y轴为例，先用加速度计计算出倾角（atan2（x，z）），然后用卡尔曼滤波融合陀螺仪得到最终姿态，但是在Y轴指向地面时，X和Z的分量都是0，加速度计计算出倾角在90度附近时出问题，小于30度精度还比较不错，是否利用4元数可以计算任意角度的倾角，但是为何程序的入参里面会有陀螺仪的数据，不是加速度计和磁力计就可以计算出静态角度，然后和陀螺仪积分计算出姿态吗？不解～～
	回复 举报

js200300953 7 主题 1 好友 331 积分 中级会员 莫元 315 发消息	5楼   发表于 2012-7-7 08:41:03  | 只看该作者 建议你翻一翻旧帖，这些问题都曾经讨论过。
	回复 举报

cw628 24 主题 0 好友 139 积分 注册会员 莫元 127 发消息	6楼   发表于 2012-7-7 21:39:50  | 只看该作者 js200300953 发表于 2012-7-7 08:41  建议你翻一翻旧帖，这些问题都曾经讨论过。 我翻过论坛好多姿态相关的帖子了，没有看到说这个的呢？
	回复 举报

fsswl 0 主题 0 好友 21 积分 新手上路 莫元 21 发消息	7楼   发表于 2012-7-10 13:11:01  | 只看该作者 cw628 发表于 2012-7-7 21:39  我翻过论坛好多姿态相关的帖子了，没有看到说这个的呢？ 以我目前的理解,传感器是理想的话,通过四元数与欧拉角之间的相互变换,可以得到理想的三个姿态角,这是一个解耦过程,然后可以进一步地进行控制,但遗憾的是传感器都不可能是理想的,而卡尔曼滤波算法就是一个让我们找到相对更理想的数据进行处理 不对的地方请批评指正,谢谢
	回复 举报

济南电子爱好者 8 主题 0 好友 76 积分 注册会员 莫元 71 发消息

8楼   发表于 2012-10-7 11:45:29  | 只看该作者