搜索
bottom↓
回复: 2

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

[复制链接]
(7108953)

出0入4汤圆

发表于 2021-1-21 00:22:05 来自手机 | 显示全部楼层 |阅读模式
本帖最后由 jssd 于 2021-1-21 09:29 编辑

硬件为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. // Header files

  22. #include "MahonyAHRS.h"
  23. #include <math.h>

  24. //-------------------------------------------------------------------------------------------
  25. // Definitions

  26. float twoKi;                // 2 * integral gain (Ki)
  27. float q0, q1, q2, q3;        // quaternion of sensor frame relative to auxiliary frame
  28. float integralFBx, integralFBy, integralFBz;  // integral error terms scaled by Ki
  29. float invSampleFreq;
  30. float roll, pitch, yaw;
  31. char anglesComputed;

  32. //#define twoKpDef        (5.0f * 0.5f)        // 2 * proportional gain
  33. //#define twoKiDef        (0.5f * 1.0f)        // 2 * integral gain

  34. #define twoKpDef        (10.0f * 0.5f)        // 2 * proportional gain
  35. #define twoKiDef        (0.5f * 1.0f)        // 2 * integral gain
  36. void Mahony_Init(float sampleFrequency)
  37. {
  38.         twoKi = twoKiDef;        // 2 * integral gain (Ki)
  39.         q0 = 1.0f;
  40.         q1 = 0.0f;
  41.         q2 = 0.0f;
  42.         q3 = 0.0f;
  43.         integralFBx = 0.0f;
  44.         integralFBy = 0.0f;
  45.         integralFBz = 0.0f;
  46.         anglesComputed = 0;
  47.         invSampleFreq = 1.0f / sampleFrequency;
  48. }
  49. float Mahony_invSqrt(float x)
  50. {
  51.         float halfx = 0.5f * x;
  52.         float y = x;
  53.         long i = *(long*)&y;
  54.         i = 0x5f3759df - (i>>1);
  55.         y = *(float*)&i;
  56.         y = y * (1.5f - (halfx * y * y));
  57.         y = y * (1.5f - (halfx * y * y));
  58.         return y;
  59. }

  60. void Mahony_update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz)
  61. {
  62.         float recipNorm;
  63.         float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
  64.         float hx, hy, bx, bz;
  65.         float halfvx, halfvy, halfvz, halfwx, halfwy, halfwz;
  66.         float halfex, halfey, halfez;
  67.         float qa, qb, qc;

  68.         // Compute feedback only if accelerometer measurement valid
  69.         // (avoids NaN in accelerometer normalisation)
  70.         if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {

  71.                 // Normalise accelerometer measurement
  72.                 recipNorm = Mahony_invSqrt(ax * ax + ay * ay + az * az);
  73.                 ax *= recipNorm;
  74.                 ay *= recipNorm;
  75.                 az *= recipNorm;

  76.                 // Normalise magnetometer measurement
  77.                 recipNorm = Mahony_invSqrt(mx * mx + my * my + mz * mz);
  78.                 mx *= recipNorm;
  79.                 my *= recipNorm;
  80.                 mz *= recipNorm;

  81.                 // Auxiliary variables to avoid repeated arithmetic
  82.                 q0q0 = q0 * q0;
  83.                 q0q1 = q0 * q1;
  84.                 q0q2 = q0 * q2;
  85.                 q0q3 = q0 * q3;
  86.                 q1q1 = q1 * q1;
  87.                 q1q2 = q1 * q2;
  88.                 q1q3 = q1 * q3;
  89.                 q2q2 = q2 * q2;
  90.                 q2q3 = q2 * q3;
  91.                 q3q3 = q3 * q3;

  92.                 // Reference direction of Earth's magnetic field
  93.                 hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2));
  94.                 hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1));
  95.                 bx = sqrtf(hx * hx + hy * hy);
  96.                 bz = 2.0f * (mx * (q1q3 - q0q2) + my * (q2q3 + q0q1) + mz * (0.5f - q1q1 - q2q2));

  97.                 // Estimated direction of gravity and magnetic field
  98.                 halfvx = q1q3 - q0q2;
  99.                 halfvy = q0q1 + q2q3;
  100.                 halfvz = q0q0 - 0.5f + q3q3;
  101.                 halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2);
  102.                 halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3);
  103.                 halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2);

  104.                 // Error is sum of cross product between estimated direction
  105.                 // and measured direction of field vectors
  106.                 halfex = (ay * halfvz - az * halfvy) + (my * halfwz - mz * halfwy);
  107.                 halfey = (az * halfvx - ax * halfvz) + (mz * halfwx - mx * halfwz);
  108.                 halfez = (ax * halfvy - ay * halfvx) + (mx * halfwy - my * halfwx);

  109.                 // Compute and apply integral feedback if enabled
  110.                 if(twoKi > 0.0f) {
  111.                         // integral error scaled by Ki
  112.                         integralFBx += twoKi * halfex * invSampleFreq;
  113.                         integralFBy += twoKi * halfey * invSampleFreq;
  114.                         integralFBz += twoKi * halfez * invSampleFreq;
  115.                         gx += integralFBx;        // apply integral feedback
  116.                         gy += integralFBy;
  117.                         gz += integralFBz;
  118.                 } else {
  119.                         integralFBx = 0.0f;        // prevent integral windup
  120.                         integralFBy = 0.0f;
  121.                         integralFBz = 0.0f;
  122.                 }

  123.                 // Apply proportional feedback
  124.                 gx += twoKpDef * halfex;
  125.                 gy += twoKpDef * halfey;
  126.                 gz += twoKpDef * halfez;
  127.         }

  128.         // Integrate rate of change of quaternion
  129.         gx *= (0.5f * invSampleFreq);                // pre-multiply common factors
  130.         gy *= (0.5f * invSampleFreq);
  131.         gz *= (0.5f * invSampleFreq);
  132.         qa = q0;
  133.         qb = q1;
  134.         qc = q2;
  135.         q0 += (-qb * gx - qc * gy - q3 * gz);
  136.         q1 += (qa * gx + qc * gz - q3 * gy);
  137.         q2 += (qa * gy - qb * gz + q3 * gx);
  138.         q3 += (qa * gz + qb * gy - qc * gx);

  139.         // Normalise quaternion
  140.         recipNorm = Mahony_invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
  141.         q0 *= recipNorm;
  142.         q1 *= recipNorm;
  143.         q2 *= recipNorm;
  144.         q3 *= recipNorm;
  145.         anglesComputed = 0;
  146. }
  147. void Mahony_computeAngles()
  148. {
  149.         roll = atan2f(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2);
  150.         pitch = asinf(-2.0f * (q1*q3 - q0*q2));
  151.         yaw = atan2f(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3);
  152.         anglesComputed = 1;
  153. }
  154. float getRoll() {
  155.         if (!anglesComputed) Mahony_computeAngles();
  156.         return roll * 57.29578f;
  157. }
  158. float getPitch() {
  159.         if (!anglesComputed) Mahony_computeAngles();
  160.         return pitch * 57.29578f;
  161. }
  162. float getYaw() {
  163.         if (!anglesComputed) Mahony_computeAngles();
  164.         return yaw * 57.29578f + 180.0f;
  165. }

  166. static float acc_x,acc_y,acc_z;
  167. static float groy_x,groy_y,groy_z;
  168. static float mag_x,mag_y,mag_z;

  169. void Mahony_send_ANO(uint8_t fun,uint8_t* p,int len)
  170. {
  171.         uint8_t buf[256];
  172.         int L=0;
  173.         uint8_t ver = 0;
  174.        
  175.         buf[L] = 0xAA; ver += buf[L++];
  176.         buf[L] = 0x05; ver += buf[L++];
  177.         buf[L] = 0xAF; ver += buf[L++];
  178.         buf[L] = fun;  ver += buf[L++];
  179.         buf[L] = len;  ver += buf[L++];
  180.         for(int i=0;i<len;i++){
  181.                 buf[L] = p[i]; ver += buf[L++];
  182.         }
  183.         buf[L++] = ver;

  184.         ESB_SendBuff(buf,L);
  185. }
  186. void Mahony_send_ANO_STATUS(void)
  187. {
  188.         uint8_t buf[256];
  189.         uint8_t L=0;
  190.        
  191.         int16_t roll  = (int16_t)(getRoll()*100);
  192.         int16_t pitch = (int16_t)(getPitch()*100);
  193.         int16_t yaw   = (int16_t)(getYaw()*100);
  194.        
  195.        
  196.         buf[L++] = (uint8_t)(roll>>8);
  197.         buf[L++] = (uint8_t)(roll>>0);
  198.         buf[L++] = (uint8_t)(pitch>>8);
  199.         buf[L++] = (uint8_t)(pitch>>0);
  200.         buf[L++] = (uint8_t)(yaw>>8);
  201.         buf[L++] = (uint8_t)(yaw>>0);
  202.         buf[L++] = 0;
  203.         buf[L++] = 0;
  204.         buf[L++] = 0;
  205.         buf[L++] = 0;
  206.         buf[L++] = 0;
  207.         buf[L++] = 0;
  208.         buf[L++] = 0;

  209.         Mahony_send_ANO(0x01,buf,L);
  210. }

  211. 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)
  212. {
  213.         uint8_t buf[256];
  214.         uint8_t L=0;
  215.        
  216.         buf[L++] = (uint8_t)(ax>>8);
  217.         buf[L++] = (uint8_t)(ax>>0);
  218.         buf[L++] = (uint8_t)(ay>>8);
  219.         buf[L++] = (uint8_t)(ay>>0);
  220.         buf[L++] = (uint8_t)(az>>8);
  221.         buf[L++] = (uint8_t)(az>>0);
  222.        
  223.         buf[L++] = (uint8_t)(gx>>8);
  224.         buf[L++] = (uint8_t)(gx>>0);
  225.         buf[L++] = (uint8_t)(gy>>8);
  226.         buf[L++] = (uint8_t)(gy>>0);
  227.         buf[L++] = (uint8_t)(gz>>8);
  228.         buf[L++] = (uint8_t)(gz>>0);
  229.        
  230.         buf[L++] = (uint8_t)(mx>>8);
  231.         buf[L++] = (uint8_t)(mx>>0);
  232.         buf[L++] = (uint8_t)(my>>8);
  233.         buf[L++] = (uint8_t)(my>>0);
  234.         buf[L++] = (uint8_t)(mz>>8);
  235.         buf[L++] = (uint8_t)(mz>>0);

  236.         Mahony_send_ANO(0x02,buf,L);
  237. }

  238. 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)
  239. {
  240.         // Convert gyroscope degrees/sec to radians/sec
  241.         gx = gx+9; gy = gy+13; gz = gz+66;
  242. //        ax = ax+40; ay = ay-20; az = az+20;
  243.         mx = mx-200; my = my+150; mz = mz+150;
  244.        
  245.         groy_x = gx*3.1415926/16.4f/180.0f;
  246.         groy_y = gy*3.1415926/16.4f/180.0f;
  247.         groy_z = gz*3.1415926/16.4f/180.0f;

  248. //        groy_x = gx*0.030517f*0.0174533f;
  249. //        groy_y = gy*0.030517f*0.0174533f;
  250. //        groy_z = gz*0.030517f*0.0174533f;
  251.        
  252.         acc_x = ax/2048.0f;
  253.         acc_y = ay/2048.0f;
  254.         acc_z = az/2048.0f;
  255.         mag_x = my/1.0f;
  256.         mag_y = mx/1.0f;
  257.         mag_z = -mz/1.0f;
  258.        
  259. //        mag_x = 0;
  260. //        mag_y = 0;
  261. //        mag_z = 0;
  262.        
  263.         Mahony_update(groy_x,groy_y,groy_z,acc_x,acc_y,acc_z,mag_x,mag_y,mag_z);
  264.        
  265.         Mahony_send_ANO_STATUS();
  266.         Mahony_send_ANO_SENSER(gx,gy,gz,ax,ay,az,mx,my,mz);
  267. }


  268. //============================================================================================
  269. // END OF CODE
  270. //============================================================================================


复制代码


本帖子中包含更多资源

您需要 登录 才可以下载或查看,没有帐号?注册

x
(7072449)

出0入4汤圆

 楼主| 发表于 2021-1-21 10:30:29 | 显示全部楼层
搞定了,算法没问题,问题出在182行
return yaw * 57.29578f + 180.0f;改为 return yaw * 57.29578f;
(4613164)

出0入0汤圆

发表于 2021-2-18 21:38:34 | 显示全部楼层
jssd 发表于 2021-1-21 10:30
搞定了,算法没问题,问题出在182行
return yaw * 57.29578f + 180.0f;改为 return yaw * 57.29578f; ...

流弊极了!
回帖提示: 反政府言论将被立即封锁ID 在按“提交”前,请自问一下:我这样表达会给举报吗,会给自己惹麻烦吗? 另外:尽量不要使用Mark、顶等没有意义的回复。不得大量使用大字体和彩色字。
您需要登录后才可以回帖 登录 | 注册

本版积分规则

手机版|Archiver|amobbs.com 阿莫电子论坛 ( 公安交互式论坛备案:44190002001997 粤ICP备09047143号 )

GMT+8, 2021-4-13 07:04

© Since 2004 www.amobbs.com, 原www.ourdev.cn, 原www.ouravr.com

快速回复 返回顶部 返回列表