关于四元数表示旋转组合的问题
本人平生第一帖，献给阿莫了！问题是这样的，陀螺仪传回的三个角分别是 a，b，c，那应该怎么用一个四元数来表示这次旋转呢？
假设原始姿态q0=｛1，0，0，0｝，q1=｛cos（a/2），sin（a/2），0，0｝表示绕x轴转角，
q2=｛cos（b/2），0，sin(b/2),0｝表示绕y轴转角，q3= ｛cos（c/2）,0,0,sin(c/2)｝表示绕z轴转角，
我开始想用三个四元数连乘，来表示组合，即q0 = q0*q1*q2*q3，
但是我后来又想，q0*q1后，y轴在世界坐标系里已经不是（0，1，0）了，那q2也就不适用了，继续连乘就不对了，一直不得解，后来看到一个坛友同志发的外国人的代码，代码如下#include "IMU.h"
#include <math.h>
//----------------------------------------------------------------------------------------------------
// Definitions
#define Kp 2.0f // 比例增益支配率收敛到加速度计/磁强计
#define Ki 0.005f // 积分增益支配率的陀螺仪偏见的衔接
#define halfT 0.5f // 采样周期的一半
//---------------------------------------------------------------------------------------------------
// 变量定义
float q0 = 1, q1 = 0, q2 = 0, q3 = 0; // 四元数的元素，代表估计方向
float exInt = 0, eyInt = 0, ezInt = 0; // 按比例缩小积分误差
//====================================================================================================
// Function
//====================================================================================================
void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az) {
float norm;
float vx, vy, vz;
float ex, ey, ez;
// 测量正常化
norm = sqrt(ax*ax + ay*ay + az*az);
ax = ax / norm;
ay = ay / norm;
az = az / norm;
// 估计方向的重力
vx = 2*(q1*q3 - q0*q2);
vy = 2*(q0*q1 + q2*q3);
vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;
// 错误的领域和方向传感器测量参考方向之间的交叉乘积的总和
ex = (ay*vz - az*vy);
ey = (az*vx - ax*vz);
ez = (ax*vy - ay*vx);
// 积分误差比例积分增益
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;
}整合四元数和正常化那段，他的意思是用（1，sin(a/2)，sin(b/2)，sin(c/2)）表示一次叁轴旋转。但是我们用a=60,b=0,c=0，即只绕x轴转了60度来验证一下。
以外国人的意思，这次旋转的四元数应该是（1，1/2，0，0），而根据四元数表示旋转的资料说明，应该是（cos30,sin30,0,0）=（根号3/2,1/2,0,0）显然外国的表示法是不符合事实的.
求救,到底要怎样才对? 没人回复，我自己在用opengl模拟了一下，发现，外国人的是对的，连乘也是对的。 感谢上帝，但是还是不明白原理 外国人的不对，时间长了后，发现和连乘的不重叠了，测试是，先把飞机倾斜60度，这60度再不动，然后只绕飞机z轴，按一定的速度转，连乘的z轴一直不动，外国人的，z轴偏转了，外国的人能正常也一定是很大程度的依赖了他的滤波器修正了 關注一下~ 楼主有资料吗，还是不懂啊。分享一下吧{:lol:}
页:
[1]