关于四元数表示旋转组合的问题

huangrb

本人平生第一帖，献给阿莫了！
问题是这样的，陀螺仪传回的三个角分别是 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也就不适用了，继续连乘就不对了，一直不得解，后来看到一个坛友同志发的外国人的代码，代码如下

1. #include "IMU.h"
   
2. #include <math.h>
   
3. 
   
4. //----------------------------------------------------------------------------------------------------
   
5. // Definitions
   
6. 
   
7. #define Kp 2.0f                        // 比例增益支配率收敛到加速度计/磁强计
   
8. #define Ki 0.005f                // 积分增益支配率的陀螺仪偏见的衔接
   
9. #define halfT 0.5f                // 采样周期的一半
   
10. 
    
11. //---------------------------------------------------------------------------------------------------
    
12. // 变量定义
    
13. 
    
14. float q0 = 1, q1 = 0, q2 = 0, q3 = 0;        // 四元数的元素，代表估计方向
    
15. float exInt = 0, eyInt = 0, ezInt = 0;        // 按比例缩小积分误差
    
16. 
    
17. //====================================================================================================
    
18. // Function
    
19. //====================================================================================================
    
20. 
    
21. void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az) {
    
22.         float norm;
    
23.         float vx, vy, vz;
    
24.         float ex, ey, ez;         
    
25.        
    
26.         // 测量正常化
    
27.         norm = sqrt(ax*ax + ay*ay + az*az);      
    
28.         ax = ax / norm;
    
29.         ay = ay / norm;
    
30.         az = az / norm;      
    
31.        
    
32.         // 估计方向的重力
    
33.         vx = 2*(q1*q3 - q0*q2);
    
34.         vy = 2*(q0*q1 + q2*q3);
    
35.         vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;
    
36.        
    
37.         // 错误的领域和方向传感器测量参考方向之间的交叉乘积的总和
    
38.         ex = (ay*vz - az*vy);
    
39.         ey = (az*vx - ax*vz);
    
40.         ez = (ax*vy - ay*vx);
    
41.        
    
42.         // 积分误差比例积分增益
    
43.         exInt = exInt + ex*Ki;
    
44.         eyInt = eyInt + ey*Ki;
    
45.         ezInt = ezInt + ez*Ki;
    
46.        
    
47.         // 调整后的陀螺仪测量
    
48.         gx = gx + Kp*ex + exInt;
    
49.         gy = gy + Kp*ey + eyInt;
    
50.         gz = gz + Kp*ez + ezInt;
    
51.        
    
52.         // 整合四元数率和正常化
    
53.         q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
    
54.         q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
    
55.         q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
    
56.         q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;  
    
57.        
    
58.         // 正常化四元
    
59.         norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
    
60.         q0 = q0 / norm;
    
61.         q1 = q1 / norm;
    
62.         q2 = q2 / norm;
    
63.         q3 = q3 / norm;
    
64. }

复制代码

整合四元数和正常化那段，他的意思是用（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）显然外国的表示法是不符合事实的.
求救,到底要怎样才对?

huangrb

没人回复，我自己在用opengl模拟了一下，发现，外国人的是对的，连乘也是对的。

huangrb

感谢上帝，但是还是不明白原理

huangrb

外国人的不对，时间长了后，发现和连乘的不重叠了，测试是，先把飞机倾斜60度，这60度再不动，然后只绕飞机z轴，按一定的速度转，连乘的z轴一直不动，外国人的，z轴偏转了，外国的人能正常也一定是很大程度的依赖了他的滤波器修正了

john800422

關注一下~

残忆视觉

楼主有资料吗，还是不懂啊。分享一下吧