还有大侠做姿态融合吗？帮忙看看9250数据融合的一个问题

jssd

硬件为mpu9250，算法为mahonyUpdate，9轴数据融合后，姿态挺稳，但有一个问题，
        当偏航角转到0时再陆续变小的话，偏航角会瞬间变为-300度，然后再继续变小，直到-360度时再瞬变为+330度（差不多这个角度，但没到360）。
       反之，当角度增加到-300度时也会瞬变为0度。也就是说，imu转一圈会出现两次偏航角瞬变，其他角度正常。
       请问是什么原因？
       代码和图片明天贴上来。先行谢过！

1. //=============================================================================================
   
2. // MahonyAHRS.c
   
3. //=============================================================================================
   
4. //
   
5. // Madgwick's implementation of Mayhony's AHRS algorithm.
   
6. // See: http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/
   
7. //
   
8. // From the x-io website "Open-source resources available on this website are
   
9. // provided under the GNU General Public Licence unless an alternative licence
   
10. // is provided in source."
    
11. //
    
12. // Date                        Author                        Notes
    
13. // 29/09/2011        SOH Madgwick    Initial release
    
14. // 02/10/2011        SOH Madgwick        Optimised for reduced CPU load
    
15. //
    
16. // Algorithm paper:
    
17. // http://ieeexplore.ieee.org/xpl/login.jsp?tp=&arnumber=4608934&url=http%3A%2F%2Fieeexplore.ieee.org%2Fstamp%2Fstamp.jsp%3Ftp%3D%26arnumber%3D4608934
    
18. //
    
19. //=============================================================================================
    
20. 
    
21. //-------------------------------------------------------------------------------------------
    
22. // Header files
    
23. 
    
24. #include "MahonyAHRS.h"
    
25. #include <math.h>
    
26. 
    
27. //-------------------------------------------------------------------------------------------
    
28. // Definitions
    
29. 
    
30. float twoKi;                // 2 * integral gain (Ki)
    
31. float q0, q1, q2, q3;        // quaternion of sensor frame relative to auxiliary frame
    
32. float integralFBx, integralFBy, integralFBz;  // integral error terms scaled by Ki
    
33. float invSampleFreq;
    
34. float roll, pitch, yaw;
    
35. char anglesComputed;
    
36. 
    
37. //#define twoKpDef        (5.0f * 0.5f)        // 2 * proportional gain
    
38. //#define twoKiDef        (0.5f * 1.0f)        // 2 * integral gain
    
39. 
    
40. #define twoKpDef        (10.0f * 0.5f)        // 2 * proportional gain
    
41. #define twoKiDef        (0.5f * 1.0f)        // 2 * integral gain
    
42. void Mahony_Init(float sampleFrequency)
    
43. {
    
44.         twoKi = twoKiDef;        // 2 * integral gain (Ki)
    
45.         q0 = 1.0f;
    
46.         q1 = 0.0f;
    
47.         q2 = 0.0f;
    
48.         q3 = 0.0f;
    
49.         integralFBx = 0.0f;
    
50.         integralFBy = 0.0f;
    
51.         integralFBz = 0.0f;
    
52.         anglesComputed = 0;
    
53.         invSampleFreq = 1.0f / sampleFrequency;
    
54. }
    
55. float Mahony_invSqrt(float x)
    
56. {
    
57.         float halfx = 0.5f * x;
    
58.         float y = x;
    
59.         long i = *(long*)&y;
    
60.         i = 0x5f3759df - (i>>1);
    
61.         y = *(float*)&i;
    
62.         y = y * (1.5f - (halfx * y * y));
    
63.         y = y * (1.5f - (halfx * y * y));
    
64.         return y;
    
65. }
    
66. 
    
67. void Mahony_update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz)
    
68. {
    
69.         float recipNorm;
    
70.         float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
    
71.         float hx, hy, bx, bz;
    
72.         float halfvx, halfvy, halfvz, halfwx, halfwy, halfwz;
    
73.         float halfex, halfey, halfez;
    
74.         float qa, qb, qc;
    
75. 
    
76.         // Compute feedback only if accelerometer measurement valid
    
77.         // (avoids NaN in accelerometer normalisation)
    
78.         if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
    
79. 
    
80.                 // Normalise accelerometer measurement
    
81.                 recipNorm = Mahony_invSqrt(ax * ax + ay * ay + az * az);
    
82.                 ax *= recipNorm;
    
83.                 ay *= recipNorm;
    
84.                 az *= recipNorm;
    
85. 
    
86.                 // Normalise magnetometer measurement
    
87.                 recipNorm = Mahony_invSqrt(mx * mx + my * my + mz * mz);
    
88.                 mx *= recipNorm;
    
89.                 my *= recipNorm;
    
90.                 mz *= recipNorm;
    
91. 
    
92.                 // Auxiliary variables to avoid repeated arithmetic
    
93.                 q0q0 = q0 * q0;
    
94.                 q0q1 = q0 * q1;
    
95.                 q0q2 = q0 * q2;
    
96.                 q0q3 = q0 * q3;
    
97.                 q1q1 = q1 * q1;
    
98.                 q1q2 = q1 * q2;
    
99.                 q1q3 = q1 * q3;
    
100.                 q2q2 = q2 * q2;
     
101.                 q2q3 = q2 * q3;
     
102.                 q3q3 = q3 * q3;
     
103. 
     
104.                 // Reference direction of Earth's magnetic field
     
105.                 hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2));
     
106.                 hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1));
     
107.                 bx = sqrtf(hx * hx + hy * hy);
     
108.                 bz = 2.0f * (mx * (q1q3 - q0q2) + my * (q2q3 + q0q1) + mz * (0.5f - q1q1 - q2q2));
     
109. 
     
110.                 // Estimated direction of gravity and magnetic field
     
111.                 halfvx = q1q3 - q0q2;
     
112.                 halfvy = q0q1 + q2q3;
     
113.                 halfvz = q0q0 - 0.5f + q3q3;
     
114.                 halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2);
     
115.                 halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3);
     
116.                 halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2);
     
117. 
     
118.                 // Error is sum of cross product between estimated direction
     
119.                 // and measured direction of field vectors
     
120.                 halfex = (ay * halfvz - az * halfvy) + (my * halfwz - mz * halfwy);
     
121.                 halfey = (az * halfvx - ax * halfvz) + (mz * halfwx - mx * halfwz);
     
122.                 halfez = (ax * halfvy - ay * halfvx) + (mx * halfwy - my * halfwx);
     
123. 
     
124.                 // Compute and apply integral feedback if enabled
     
125.                 if(twoKi > 0.0f) {
     
126.                         // integral error scaled by Ki
     
127.                         integralFBx += twoKi * halfex * invSampleFreq;
     
128.                         integralFBy += twoKi * halfey * invSampleFreq;
     
129.                         integralFBz += twoKi * halfez * invSampleFreq;
     
130.                         gx += integralFBx;        // apply integral feedback
     
131.                         gy += integralFBy;
     
132.                         gz += integralFBz;
     
133.                 } else {
     
134.                         integralFBx = 0.0f;        // prevent integral windup
     
135.                         integralFBy = 0.0f;
     
136.                         integralFBz = 0.0f;
     
137.                 }
     
138. 
     
139.                 // Apply proportional feedback
     
140.                 gx += twoKpDef * halfex;
     
141.                 gy += twoKpDef * halfey;
     
142.                 gz += twoKpDef * halfez;
     
143.         }
     
144. 
     
145.         // Integrate rate of change of quaternion
     
146.         gx *= (0.5f * invSampleFreq);                // pre-multiply common factors
     
147.         gy *= (0.5f * invSampleFreq);
     
148.         gz *= (0.5f * invSampleFreq);
     
149.         qa = q0;
     
150.         qb = q1;
     
151.         qc = q2;
     
152.         q0 += (-qb * gx - qc * gy - q3 * gz);
     
153.         q1 += (qa * gx + qc * gz - q3 * gy);
     
154.         q2 += (qa * gy - qb * gz + q3 * gx);
     
155.         q3 += (qa * gz + qb * gy - qc * gx);
     
156. 
     
157.         // Normalise quaternion
     
158.         recipNorm = Mahony_invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
     
159.         q0 *= recipNorm;
     
160.         q1 *= recipNorm;
     
161.         q2 *= recipNorm;
     
162.         q3 *= recipNorm;
     
163.         anglesComputed = 0;
     
164. }
     
165. void Mahony_computeAngles()
     
166. {
     
167.         roll = atan2f(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2);
     
168.         pitch = asinf(-2.0f * (q1*q3 - q0*q2));
     
169.         yaw = atan2f(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3);
     
170.         anglesComputed = 1;
     
171. }
     
172. float getRoll() {
     
173.         if (!anglesComputed) Mahony_computeAngles();
     
174.         return roll * 57.29578f;
     
175. }
     
176. float getPitch() {
     
177.         if (!anglesComputed) Mahony_computeAngles();
     
178.         return pitch * 57.29578f;
     
179. }
     
180. float getYaw() {
     
181.         if (!anglesComputed) Mahony_computeAngles();
     
182.         return yaw * 57.29578f + 180.0f;
     
183. }
     
184. 
     
185. static float acc_x,acc_y,acc_z;
     
186. static float groy_x,groy_y,groy_z;
     
187. static float mag_x,mag_y,mag_z;
     
188. 
     
189. void Mahony_send_ANO(uint8_t fun,uint8_t* p,int len)
     
190. {
     
191.         uint8_t buf[256];
     
192.         int L=0;
     
193.         uint8_t ver = 0;
     
194.        
     
195.         buf[L] = 0xAA; ver += buf[L++];
     
196.         buf[L] = 0x05; ver += buf[L++];
     
197.         buf[L] = 0xAF; ver += buf[L++];
     
198.         buf[L] = fun;  ver += buf[L++];
     
199.         buf[L] = len;  ver += buf[L++];
     
200.         for(int i=0;i<len;i++){
     
201.                 buf[L] = p[i]; ver += buf[L++];
     
202.         }
     
203.         buf[L++] = ver;
     
204. 
     
205.         ESB_SendBuff(buf,L);
     
206. }
     
207. void Mahony_send_ANO_STATUS(void)
     
208. {
     
209.         uint8_t buf[256];
     
210.         uint8_t L=0;
     
211.        
     
212.         int16_t roll  = (int16_t)(getRoll()*100);
     
213.         int16_t pitch = (int16_t)(getPitch()*100);
     
214.         int16_t yaw   = (int16_t)(getYaw()*100);
     
215.        
     
216.        
     
217.         buf[L++] = (uint8_t)(roll>>8);
     
218.         buf[L++] = (uint8_t)(roll>>0);
     
219.         buf[L++] = (uint8_t)(pitch>>8);
     
220.         buf[L++] = (uint8_t)(pitch>>0);
     
221.         buf[L++] = (uint8_t)(yaw>>8);
     
222.         buf[L++] = (uint8_t)(yaw>>0);
     
223.         buf[L++] = 0;
     
224.         buf[L++] = 0;
     
225.         buf[L++] = 0;
     
226.         buf[L++] = 0;
     
227.         buf[L++] = 0;
     
228.         buf[L++] = 0;
     
229.         buf[L++] = 0;
     
230. 
     
231.         Mahony_send_ANO(0x01,buf,L);
     
232. }
     
233. 
     
234. void Mahony_send_ANO_SENSER(int16_t gx, int16_t gy, int16_t gz, int16_t ax, int16_t ay, int16_t az, int16_t mx, int16_t my, int16_t mz)
     
235. {
     
236.         uint8_t buf[256];
     
237.         uint8_t L=0;
     
238.        
     
239.         buf[L++] = (uint8_t)(ax>>8);
     
240.         buf[L++] = (uint8_t)(ax>>0);
     
241.         buf[L++] = (uint8_t)(ay>>8);
     
242.         buf[L++] = (uint8_t)(ay>>0);
     
243.         buf[L++] = (uint8_t)(az>>8);
     
244.         buf[L++] = (uint8_t)(az>>0);
     
245.        
     
246.         buf[L++] = (uint8_t)(gx>>8);
     
247.         buf[L++] = (uint8_t)(gx>>0);
     
248.         buf[L++] = (uint8_t)(gy>>8);
     
249.         buf[L++] = (uint8_t)(gy>>0);
     
250.         buf[L++] = (uint8_t)(gz>>8);
     
251.         buf[L++] = (uint8_t)(gz>>0);
     
252.        
     
253.         buf[L++] = (uint8_t)(mx>>8);
     
254.         buf[L++] = (uint8_t)(mx>>0);
     
255.         buf[L++] = (uint8_t)(my>>8);
     
256.         buf[L++] = (uint8_t)(my>>0);
     
257.         buf[L++] = (uint8_t)(mz>>8);
     
258.         buf[L++] = (uint8_t)(mz>>0);
     
259. 
     
260.         Mahony_send_ANO(0x02,buf,L);
     
261. }
     
262. 
     
263. void Mahony_process(int16_t gx, int16_t gy, int16_t gz, int16_t ax, int16_t ay, int16_t az, int16_t mx, int16_t my, int16_t mz)
     
264. {
     
265.         // Convert gyroscope degrees/sec to radians/sec
     
266.         gx = gx+9; gy = gy+13; gz = gz+66;
     
267. //        ax = ax+40; ay = ay-20; az = az+20;
     
268.         mx = mx-200; my = my+150; mz = mz+150;
     
269.        
     
270.         groy_x = gx*3.1415926/16.4f/180.0f;
     
271.         groy_y = gy*3.1415926/16.4f/180.0f;
     
272.         groy_z = gz*3.1415926/16.4f/180.0f;
     
273. 
     
274. //        groy_x = gx*0.030517f*0.0174533f;
     
275. //        groy_y = gy*0.030517f*0.0174533f;
     
276. //        groy_z = gz*0.030517f*0.0174533f;
     
277.        
     
278.         acc_x = ax/2048.0f;
     
279.         acc_y = ay/2048.0f;
     
280.         acc_z = az/2048.0f;
     
281.         mag_x = my/1.0f;
     
282.         mag_y = mx/1.0f;
     
283.         mag_z = -mz/1.0f;
     
284.        
     
285. //        mag_x = 0;
     
286. //        mag_y = 0;
     
287. //        mag_z = 0;
     
288.        
     
289.         Mahony_update(groy_x,groy_y,groy_z,acc_x,acc_y,acc_z,mag_x,mag_y,mag_z);
     
290.        
     
291.         Mahony_send_ANO_STATUS();
     
292.         Mahony_send_ANO_SENSER(gx,gy,gz,ax,ay,az,mx,my,mz);
     
293. }
     
294. 
     
295. 
     
296. //============================================================================================
     
297. // END OF CODE
     
298. //============================================================================================
     
299. 
     
300. 
     

复制代码

jssd

搞定了，算法没问题，问题出在182行
return yaw * 57.29578f + 180.0f;改为 return yaw * 57.29578f;

heyangfengyue

jssd 发表于 2021-1-21 10:30
搞定了，算法没问题，问题出在182行
return yaw * 57.29578f + 180.0f;改为 return yaw * 57.29578f; ...

流弊极了！

rei1984

问下lz用的上位机是哪款软件

dxm123

jssd 发表于 2021-1-21 10:30
搞定了，算法没问题，问题出在182行
return yaw * 57.29578f + 180.0f;改为 return yaw * 57.29578f; ...
(引用自2楼)

完整的代码，可以分享吗？