现在有一个需求，手上只有一个陀螺仪，如何计算欧拉角，不考虑积分误差，无需使用其他传感器融合，数学没学好，不知怎么操作

陀螺仪直接积分出来的是欧拉角吗

万能的坛友啊，知道的告知一下

可以参考很多四轴的开源代码

1. /*
   
2. 1主函数在最后面
   
3. 2作者：黑市，黑视，智涅
   
4. 3四元数更新用的是一阶算法，还有二阶三阶甚至全阶，阶数越高精度越好，不过没多大必要。
   
5.         一阶二阶这些简化算法就是用简单的值取代了一些三角函数而已
   
6. 4详细书籍可以看《捷联式惯性导航原理》，袁信著。我以前看了这本书PDF版好久了，估计一半还没能吃透。
   
7. 5关于利用加速度计来修正姿态，大家貌似都是做飞机的，飞机上的加速度计的情况跟我做的东西差别太大，
   
8. 应该不能直接引用我那一套，大家还是引用网上例如权重法来进行修正吧！用罗盘修正姿态就更不用说了~
   
9. */
   
10. #define EulerAngle_Type float        //定义类型
    
11. #define Quaternion_Type float
    
12. #define Acc_Type int
    
13. #define Gyro_Type int
    
14. #define Euler_Martix_Type float
    
15. 
    
16. struct EulerAngle                //欧拉角结构体
    
17. {
    
18.         EulerAngle_Type Roll, Pitch, Yaw;
    
19. }
    
20. struct Quaternion                //四元数结构体
    
21. {
    
22.         Quaternion_Type q0, q1, q2, q3;
    
23. }
    
24. struct Acc                //加速度值结构体
    
25. {
    
26.         Acc_Type x, y, z;
    
27. }
    
28. struct Gyro                //陀螺仪值结构体
    
29. {
    
30.         Gyro_Type x, y, z;
    
31. }
    
32. struct Euler_Martix                //欧拉（姿态）矩阵结构体
    
33. {
    
34.         Euler_Martix_Type T11,T12,T13,        T21,T22,T23,        T31,T32,T33;
    
35. }
    
36. 
    
37. Quaternion Normalize(Quaternion e)                //四元数归一化
    
38. {
    
39.         Quaternion_Type s = (Quaternion_Type)Math.Sqrt(e.q0 * e.q0 + e.q1 * e.q1 + e.q2 * e.q2 + e.q3 * e.q3);
    
40.         e.q0 /= s;
    
41.         e.q1 /= s;
    
42.         e.q2 /= s;
    
43.         e.q3 /= s;
    
44.         return e;
    
45. }
    
46. 
    
47. Quaternion Multiply_L1(Acc lacc)                //一阶算法
    
48. {
    
49.         Quaternion Q_result;
    
50.         Q_result.q0 = BQ.q0 - BQ.q1 * lacc.x / 2 - BQ.q2 * lacc.y / 2 - BQ.q3 * lacc.z / 2;
    
51.         Q_result.q1 = BQ.q1 + BQ.q0 * lacc.x / 2 + BQ.q2 * lacc.z / 2 - BQ.q3 * lacc.y / 2;
    
52.         Q_result.q2 = BQ.q2 + BQ.q0 * lacc.y / 2 - BQ.q1 * lacc.z / 2 + BQ.q3 * lacc.x / 2;
    
53.         Q_result.q3 = BQ.q3 + BQ.q0 * lacc.z / 2 + BQ.q1 * lacc.y / 2 - BQ.q2 * lacc.x / 2;
    
54.         Q_result = Quaternion_Normalize(Q_result);
    
55.         return Q_result;
    
56. }
    
57. 
    
58. Euler_Martix Q_to_EM(Quaternion e)                //把四元数变换成欧拉角（姿态）矩阵T
    
59. {
    
60.         Euler_Martix result;
    
61.         Euler_Martix_Type q00,q01,q02,q03,q11,q12,q13,q22,q23,q33;
    
62.         q00=e.q0*e.q0;
    
63.         q01=e.q0*e.q1;
    
64.         q02=e.q0*e.q2;
    
65.         q03=e.q0*e.q3;
    
66.         q11=e.q1*e.q1;
    
67.         q12=e.q1*e.q2;
    
68.         q13=e.q1*e.q3;
    
69.         q22=e.q2*e.q2;
    
70.         q23=e.q2*e.q3;
    
71.         q33=e.q3*e.q3;
    
72.         result.T11=q00+q11-q22-q33;
    
73.         result.T12=2*(q12+q03);
    
74.         result.T13=2*(q13-q02);
    
75.         result.T21=2*(q12-q03);
    
76.         result.T22=q22-q33+q00-q11;
    
77.         result.T23=2*(q23+q01);
    
78.         result.T31=2*(q13+q02);
    
79.         result.T32=2*(q23-q01);
    
80.         result.T33=q33-q22-q11+q00;
    
81.         return result;
    
82. }
    
83. 
    
84. Quaternion Ea_to_Qu(EulerAngle ea)                //把欧拉角变换成四元数      后来不用这个方法了，用矩阵那个了
    
85. {
    
86.         Quaternion result;
    
87. 
    
88.         Quaternion_Type CosY = Math.Cos(ea.Yaw * .5);
    
89.         Quaternion_Type SinY = Math.Sin(ea.Yaw * .5);       
    
90.         Quaternion_Type CosP = Math.Cos(ea.Pitch * .5f);
    
91.         Quaternion_Type SinP = Math.Sin(ea.Pitch * .5);
    
92.         Quaternion_Type CosR = Math.Cos(ea.Roll * .5f);
    
93.         Quaternion_Type SinR = Math.Sin(ea.Roll * .5f);
    
94.        
    
95.         result.q0 = CosY * CosP * CosR + SinY * SinP * SinR;
    
96.         result.q1 = CosY * CosP * SinR - SinY * SinP * CosR;
    
97.         result.q2 = CosY * SinP * CosR + SinY * CosP * SinR;
    
98.         result.q3 = SinY * CosP * CosR - CosY * SinP * SinR;
    
99.         return result;
    
100. }
     
101. 
     
102. Acc        coordinate_body_to_inertia(Euler_Martix EM,Acc lacc)                //将体坐标加速度变换到惯性坐标
     
103. {
     
104. //做飞机不需要，省略
     
105. }
     
106. 
     
107. EulerAngle EM_to_EU(Euler_Martix lem)        //从姿态矩阵中提取姿态角
     
108. {
     
109.         EulerAngle result;
     
110.         result.Yaw = Math.Atan2(lem.T12, lem.T11);
     
111.         result.Pitch = -Math.Asin(lem.T13);
     
112.         result.Roll = Math.Atan2(lem.T23, lem.T33);
     
113.         return result;
     
114. }
     
115. 
     
116. void main()
     
117. {
     
118.         Quaternion BQ;//定义姿态四元素
     
119.         Euler_Martix BEM;//定义欧拉矩阵
     
120.         EulerAngle BEA;//定义欧拉角
     
121.        
     
122.         BQ.q0=1;//初始化四元数
     
123.         BQ.q1=BQ.q2=BQ.q3=0;//初始化四元数
     
124. 
     
125.         char gx,gy,gz;//定义陀螺仪三个轴，用来装值
     
126.        
     
127.         while（1）//要不断更新，所以弄个循环，你要知道你的更新速率，才能转化下面的一些参数。
     
128.                         //根据转动的不可交互性，更新速率越快越好，看书！
     
129.         {
     
130.                 gx=25;
     
131.                 gy=14;
     
132.                 gz=4;
     
133.                 //以上为读取陀螺仪的三个值，我随便赋值作为例子哈
     
134.                
     
135.                 EulerAngle lea;//定义欧拉小转角，每次更新的小转角，下面就用到
     
136.                
     
137.                 lea.Yaw = ((float)gx / 62200 * Math.PI);
     
138.                 lea.Pitch = ((float)gy / 62200 * Math.PI);
     
139.                 lea.Roll = ((float)gz/ 62200 * Math.PI);
     
140.                 //把陀螺仪的三个值转化为角度值（根据自己的采样率、精度等参数转化），不懂请请自行查资料搞懂
     
141.                
     
142.                 BQ = Multiply_L1(BQ, lea);//更新姿态四元素，由旧四元数和小转角更新得到新四元数
     
143.                
     
144.                 BEM = Q_to_EM(BQ);//将更新完的姿态四元素转成欧拉矩阵
     
145.                 BEA = EM_to_EU(BEM);//从欧拉矩阵中提取欧拉角
     
146.         }
     
147. }
     
148. 
     

复制代码

以前收集的，不知道适合不。

无薪税绵 发表于 2018-4-9 12:47
以前收集的，不知道适合不。

陀螺仪的数据是角速度，我是按照update imu的那个算法处理的，只不过没有加加速度和磁力计进行矫正