社区导航

 
快捷导航
  • 首页
  • 论坛
  • 查看新帖
  • 最新回复
  • 社区活动
  • 联系管理员
  • 消灭零回复
  • E金币兑换
  • 干货
搜索
查看: 1343|回复: 6

[问题讨论] BlueMicrosystem2使用MahonyAHRS融合算法解出来的数据不对

[复制链接]

4936

TA的帖子

9

TA的资源

版主

Rank: 6Rank: 6

发表于 2017-6-19 22:52:33 | 显示全部楼层 |阅读模式

因为ST官方提供的MotionFX没用明白
在网上找了一个传感器融合的代码,输入9轴数据输出四元数用MahonyAHRS替换原来的MotionFX_manager_run

将四元数按照原来的方式输出到手机,用BLUEMS查看骰子是一直转动的
不知道问题出在哪,官方是直接把寄存器值输入到MotionFX库
单位是LSB
我在调用 MahonyAHRSupdate函数时把对应的寄存器做了转换
加速度的单位是g
磁场的单位是gauss
陀螺仪的单位是dps
难道问题出在这?

  1. static void ComputeQuaternions(void)
  2. {
  3.   static SensorAxes_t quat_axes[SEND_N_QUATERNIONS];
  4.   static int32_t calibIndex =0;
  5.   static int32_t CounterFX  =0;
  6.   static int32_t CounterEC  =0;
  7.   SensorAxes_t ACC_Value;
  8.   SensorAxesRaw_t ACC_Value_Raw;
  9.   SensorAxes_t GYR_Value;
  10.   SensorAxes_t MAG_Value;

  11.   float ax,ay,az,gx,gy,gz,mx,my,mz;
  12.   /* Increment the Counter */
  13.   if(W2ST_CHECK_CONNECTION(W2ST_CONNECT_EC)) {
  14.     CounterEC++;
  15.   } else {
  16.     CounterFX++;
  17.   }

  18.   /* Read the Acc RAW values */
  19.   BSP_ACCELERO_Get_AxesRaw(TargetBoardFeatures.HandleAccSensor,&ACC_Value_Raw);

  20.   /* Read the Magneto values */
  21.   BSP_MAGNETO_Get_Axes(TargetBoardFeatures.HandleMagSensor,&MAG_Value);

  22.   /* Read the Gyro values */
  23.   BSP_GYRO_Get_Axes(TargetBoardFeatures.HandleGyroSensor,&GYR_Value);

  24. //  MotionFX_manager_run(ACC_Value_Raw,GYR_Value,MAG_Value);
  25.       
  26. /*
  27. LSM303AGR
  28. Linear acceleration sensitivity mg/LSB
  29. FS = ±2 g and in high-resolution mode -7% 0.98 +7%
  30. FS = ±4 g and in high-resolution mode -7% 1.95 +7%
  31. FS = ±8 g and in high-resolution mode -7% 3.9 +7%
  32. FS = ±16 g and in high-resolution mode -7% 11.72 +7%
  33. FS = ±2 g and in normal mode -7% 3.9 +7%
  34. FS = ±4 g and in normal mode -7% 7.82 +7%
  35. FS = ±8 g and in normal mode -7% 15.63 +7%
  36. FS = ±16 g and in normal mode -7% 46.9 +7%
  37. FS = ±2 g and in low-power mode -7% 15.63 +7%
  38. FS = ±4 g and in low-power mode -7% 31.26 +7%
  39. FS = ±8 g and in low-power mode -7% 62.52 +7%
  40. FS = ±16 g and in low-power mode -7% 187.58 +7%
  41. Magnetic sensitivity(2) mgauss/LSB
  42. -7% 1.5 +7%
  43.   
  44. LSM6DSM
  45. Linear acceleration sensitivity(2) mg/LSB
  46. FS = ±2 0.061
  47. FS = ±4 0.122
  48. FS = ±8 0.244
  49. FS = ±16 0.488
  50.   
  51. Angular rate sensitivity(2) mdps/LSB
  52. FS = ±125 4.375
  53. FS = ±245 8.75
  54. FS = ±500 17.50
  55. FS = ±1000 35
  56. FS = ±2000 70
  57. */
  58.   //+-2g = 0.061mg/LSB
  59.   ax = ACC_Value_Raw.AXIS_X * 0.061 / 1000.00;
  60.   ay = ACC_Value_Raw.AXIS_Y * 0.061 / 1000.00;
  61.   az = ACC_Value_Raw.AXIS_Z * 0.061 / 1000.00;
  62.   //1.5 mgauss/LSB
  63.   mx = MAG_Value.AXIS_X * 1.5 / 1000.00;
  64.   my = MAG_Value.AXIS_Y * 1.5 / 1000.00;
  65.   mz = MAG_Value.AXIS_Z * 1.5 / 1000.00;
  66.   //+-2000 = 70mdps/LSB
  67.   gx = GYR_Value.AXIS_X * 70.00 / 1000.00;
  68.   gy = GYR_Value.AXIS_Y * 70.00 / 1000.00;
  69.   gz = GYR_Value.AXIS_Z * 70.00 / 1000.00;
  70.   
  71.   MahonyAHRSupdate(gx,gy,gz,ax,ay,az,mx,my,mz);
  72.   int32_t QuaternionNumber = (CounterFX>SEND_N_QUATERNIONS) ? (SEND_N_QUATERNIONS-1) : (CounterFX-1);

  73.   /* Scaling quaternions data by a factor of 10000
  74.     (Scale factor to handle float during data transfer BT) */

  75.   /* Save the quaternions values */
  76.   if(q3 < 0){
  77.     quat_axes[QuaternionNumber].AXIS_X = (int32_t)(q0 * (-10000));
  78.     quat_axes[QuaternionNumber].AXIS_Y = (int32_t)(q1 * (-10000));
  79.     quat_axes[QuaternionNumber].AXIS_Z = (int32_t)(q2 * (-10000));
  80.   } else {
  81.     quat_axes[QuaternionNumber].AXIS_X = (int32_t)(q0 * 10000);
  82.     quat_axes[QuaternionNumber].AXIS_Y = (int32_t)(q1 * 10000);
  83.     quat_axes[QuaternionNumber].AXIS_Z = (int32_t)(q2 * 10000);
  84.   }
  85.    
  86.   /* Every QUAT_UPDATE_MUL_10MS*10 mSeconds Send Quaternions informations via bluetooth */
  87.   if(CounterFX==QUAT_UPDATE_MUL_10MS){
  88.     Quat_Update(quat_axes);
  89.     CounterFX=0;
  90.   }
  91. }
复制代码


  1. //=====================================================================================================
  2. // MahonyAHRS.c
  3. //=====================================================================================================
  4. //
  5. // Madgwick's implementation of Mayhony's AHRS algorithm.
  6. // See: [url]http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms[/url]
  7. //
  8. // Date                        Author                        Notes
  9. // 29/09/2011        SOH Madgwick    Initial release
  10. // 02/10/2011        SOH Madgwick        Optimised for reduced CPU load
  11. //
  12. //=====================================================================================================

  13. //---------------------------------------------------------------------------------------------------
  14. // Header files

  15. #include "MahonyAHRS.h"
  16. #include <math.h>

  17. //---------------------------------------------------------------------------------------------------
  18. // Definitions

  19. #define sampleFreq        1000.0f                        // sample frequency in Hz
  20. #define twoKpDef        (2.0f * 0.5f)        // 2 * proportional gain
  21. #define twoKiDef        (2.0f * 0.0f)        // 2 * integral gain

  22. //---------------------------------------------------------------------------------------------------
  23. // Variable definitions

  24. volatile float twoKp = twoKpDef;                                                                                        // 2 * proportional gain (Kp)
  25. volatile float twoKi = twoKiDef;                                                                                        // 2 * integral gain (Ki)
  26. volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;                                        // quaternion of sensor frame relative to auxiliary frame
  27. volatile float integralFBx = 0.0f,  integralFBy = 0.0f, integralFBz = 0.0f;        // integral error terms scaled by Ki

  28. //---------------------------------------------------------------------------------------------------
  29. // Function declarations

  30. float invSqrt(float x);

  31. //====================================================================================================
  32. // Functions

  33. //---------------------------------------------------------------------------------------------------
  34. // AHRS algorithm update

  35. void MahonyAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
  36.         float recipNorm;
  37.     float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;  
  38.         float hx, hy, bx, bz;
  39.         float halfvx, halfvy, halfvz, halfwx, halfwy, halfwz;
  40.         float halfex, halfey, halfez;
  41.         float qa, qb, qc;

  42.         // Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)
  43.         if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
  44.                 MahonyAHRSupdateIMU(gx, gy, gz, ax, ay, az);
  45.                 return;
  46.         }

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

  49.                 // Normalise accelerometer measurement
  50.                 recipNorm = invSqrt(ax * ax + ay * ay + az * az);
  51.                 ax *= recipNorm;
  52.                 ay *= recipNorm;
  53.                 az *= recipNorm;     

  54.                 // Normalise magnetometer measurement
  55.                 recipNorm = invSqrt(mx * mx + my * my + mz * mz);
  56.                 mx *= recipNorm;
  57.                 my *= recipNorm;
  58.                 mz *= recipNorm;   

  59.         // Auxiliary variables to avoid repeated arithmetic
  60.         q0q0 = q0 * q0;
  61.         q0q1 = q0 * q1;
  62.         q0q2 = q0 * q2;
  63.         q0q3 = q0 * q3;
  64.         q1q1 = q1 * q1;
  65.         q1q2 = q1 * q2;
  66.         q1q3 = q1 * q3;
  67.         q2q2 = q2 * q2;
  68.         q2q3 = q2 * q3;
  69.         q3q3 = q3 * q3;   

  70.         // Reference direction of Earth's magnetic field
  71.         hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2));
  72.         hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1));
  73.         bx = sqrt(hx * hx + hy * hy);
  74.         bz = 2.0f * (mx * (q1q3 - q0q2) + my * (q2q3 + q0q1) + mz * (0.5f - q1q1 - q2q2));

  75.                 // Estimated direction of gravity and magnetic field
  76.                 halfvx = q1q3 - q0q2;
  77.                 halfvy = q0q1 + q2q3;
  78.                 halfvz = q0q0 - 0.5f + q3q3;
  79.         halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2);
  80.         halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3);
  81.         halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2);  
  82.        
  83.                 // Error is sum of cross product between estimated direction and measured direction of field vectors
  84.                 halfex = (ay * halfvz - az * halfvy) + (my * halfwz - mz * halfwy);
  85.                 halfey = (az * halfvx - ax * halfvz) + (mz * halfwx - mx * halfwz);
  86.                 halfez = (ax * halfvy - ay * halfvx) + (mx * halfwy - my * halfwx);

  87.                 // Compute and apply integral feedback if enabled
  88.                 if(twoKi > 0.0f) {
  89.                         integralFBx += twoKi * halfex * (1.0f / sampleFreq);        // integral error scaled by Ki
  90.                         integralFBy += twoKi * halfey * (1.0f / sampleFreq);
  91.                         integralFBz += twoKi * halfez * (1.0f / sampleFreq);
  92.                         gx += integralFBx;        // apply integral feedback
  93.                         gy += integralFBy;
  94.                         gz += integralFBz;
  95.                 }
  96.                 else {
  97.                         integralFBx = 0.0f;        // prevent integral windup
  98.                         integralFBy = 0.0f;
  99.                         integralFBz = 0.0f;
  100.                 }

  101.                 // Apply proportional feedback
  102.                 gx += twoKp * halfex;
  103.                 gy += twoKp * halfey;
  104.                 gz += twoKp * halfez;
  105.         }
  106.        
  107.         // Integrate rate of change of quaternion
  108.         gx *= (0.5f * (1.0f / sampleFreq));                // pre-multiply common factors
  109.         gy *= (0.5f * (1.0f / sampleFreq));
  110.         gz *= (0.5f * (1.0f / sampleFreq));
  111.         qa = q0;
  112.         qb = q1;
  113.         qc = q2;
  114.         q0 += (-qb * gx - qc * gy - q3 * gz);
  115.         q1 += (qa * gx + qc * gz - q3 * gy);
  116.         q2 += (qa * gy - qb * gz + q3 * gx);
  117.         q3 += (qa * gz + qb * gy - qc * gx);
  118.        
  119.         // Normalise quaternion
  120.         recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
  121.         q0 *= recipNorm;
  122.         q1 *= recipNorm;
  123.         q2 *= recipNorm;
  124.         q3 *= recipNorm;
  125. }

  126. //---------------------------------------------------------------------------------------------------
  127. // IMU algorithm update

  128. void MahonyAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az) {
  129.         float recipNorm;
  130.         float halfvx, halfvy, halfvz;
  131.         float halfex, halfey, halfez;
  132.         float qa, qb, qc;

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

  135.                 // Normalise accelerometer measurement
  136.                 recipNorm = invSqrt(ax * ax + ay * ay + az * az);
  137.                 ax *= recipNorm;
  138.                 ay *= recipNorm;
  139.                 az *= recipNorm;        

  140.                 // Estimated direction of gravity and vector perpendicular to magnetic flux
  141.                 halfvx = q1 * q3 - q0 * q2;
  142.                 halfvy = q0 * q1 + q2 * q3;
  143.                 halfvz = q0 * q0 - 0.5f + q3 * q3;
  144.        
  145.                 // Error is sum of cross product between estimated and measured direction of gravity
  146.                 halfex = (ay * halfvz - az * halfvy);
  147.                 halfey = (az * halfvx - ax * halfvz);
  148.                 halfez = (ax * halfvy - ay * halfvx);

  149.                 // Compute and apply integral feedback if enabled
  150.                 if(twoKi > 0.0f) {
  151.                         integralFBx += twoKi * halfex * (1.0f / sampleFreq);        // integral error scaled by Ki
  152.                         integralFBy += twoKi * halfey * (1.0f / sampleFreq);
  153.                         integralFBz += twoKi * halfez * (1.0f / sampleFreq);
  154.                         gx += integralFBx;        // apply integral feedback
  155.                         gy += integralFBy;
  156.                         gz += integralFBz;
  157.                 }
  158.                 else {
  159.                         integralFBx = 0.0f;        // prevent integral windup
  160.                         integralFBy = 0.0f;
  161.                         integralFBz = 0.0f;
  162.                 }

  163.                 // Apply proportional feedback
  164.                 gx += twoKp * halfex;
  165.                 gy += twoKp * halfey;
  166.                 gz += twoKp * halfez;
  167.         }
  168.        
  169.         // Integrate rate of change of quaternion
  170.         gx *= (0.5f * (1.0f / sampleFreq));                // pre-multiply common factors
  171.         gy *= (0.5f * (1.0f / sampleFreq));
  172.         gz *= (0.5f * (1.0f / sampleFreq));
  173.         qa = q0;
  174.         qb = q1;
  175.         qc = q2;
  176.         q0 += (-qb * gx - qc * gy - q3 * gz);
  177.         q1 += (qa * gx + qc * gz - q3 * gy);
  178.         q2 += (qa * gy - qb * gz + q3 * gx);
  179.         q3 += (qa * gz + qb * gy - qc * gx);
  180.        
  181.         // Normalise quaternion
  182.         recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
  183.         q0 *= recipNorm;
  184.         q1 *= recipNorm;
  185.         q2 *= recipNorm;
  186.         q3 *= recipNorm;
  187. }

  188. //---------------------------------------------------------------------------------------------------
  189. // Fast inverse square-root
  190. // See: [url]http://en.wikipedia.org/wiki/Fast_inverse_square_root[/url]

  191. float invSqrt(float x) {
  192.         float halfx = 0.5f * x;
  193.         float y = x;
  194.         long i = *(long*)&y;
  195.         i = 0x5f3759df - (i>>1);
  196.         y = *(float*)&i;
  197.         y = y * (1.5f - (halfx * y * y));
  198.         return y;
  199. }

  200. //====================================================================================================
  201. // END OF CODE
  202. //====================================================================================================
复制代码



回复

使用道具 举报

4936

TA的帖子

9

TA的资源

版主

Rank: 6Rank: 6

 楼主| 发表于 2017-6-20 18:28:49 | 显示全部楼层


效果如图,感觉应该是陀螺仪输出的数据不对
ezgif-1-c469e6ceba.gif


回复

使用道具 举报

25

TA的帖子

0

TA的资源

一粒金砂(中级)

Rank: 2

发表于 2017-6-21 20:57:53 | 显示全部楼层
陀螺仪,磁力计没有做校准?MotionFX库里调用的Acc应该是用的寄存器原始数据,Gyro应该是mdps单位,而Mag应该是mGauss的单位。

点评

陀螺仪有一个轴静止时输出数据比较大 可能是这个原因 例程里掉用MahonyAHRSupdate函数的频率是100次每秒 MahonyAHRS算法里的sampleFreq改成100 陀螺仪使用DPS数据单位会出现轻微转动传感器骰子就高速转动的情况  详情 回复 发表于 2017-6-21 21:14


回复

使用道具 举报

4936

TA的帖子

9

TA的资源

版主

Rank: 6Rank: 6

 楼主| 发表于 2017-6-21 21:14:53 来自手机 | 显示全部楼层
yujie2510 发表于 2017-6-21 20:57
陀螺仪,磁力计没有做校准?MotionFX库里调用的Acc应该是用的寄存器原始数据,Gyro应该是mdps单位,而Mag应 ...

陀螺仪有一个轴静止时输出数据比较大
可能是这个原因
例程里掉用MahonyAHRSupdate函数的频率是100次每秒
MahonyAHRS算法里的sampleFreq改成100
陀螺仪使用DPS数据单位会出现轻微转动传感器骰子就高速转动的情况
适当的修改这些数值能变正常
但是找不出这几个数字之间的关系

还有一个问题是加速度和地磁在算法里不起作用
修改了KP和KI参数也没见到加速度补偿陀螺仪误差的效果


回复

使用道具 举报

25

TA的帖子

0

TA的资源

一粒金砂(中级)

Rank: 2

发表于 2017-6-25 20:00:20 | 显示全部楼层
在 osxMotionFX 的库里会用磁力计来补偿陀螺仪,如果你用的算法如果没有用到磁力计的数据的话,就需要保证所有的输入都是校准过后的数据。

点评

允许输入9轴数据,应该全通过加速度和磁力计补偿 可能是设置的问题,在实际测试时加速度和磁力计几乎看不到作用  详情 回复 发表于 2017-6-26 11:31


回复

使用道具 举报

4936

TA的帖子

9

TA的资源

版主

Rank: 6Rank: 6

 楼主| 发表于 2017-6-26 11:31:13 | 显示全部楼层
yujie2510 发表于 2017-6-25 20:00
在 osxMotionFX 的库里会用磁力计来补偿陀螺仪,如果你用的算法如果没有用到磁力计的数据的话,就需要保证 ...

允许输入9轴数据,应该全通过加速度和磁力计补偿
可能是设置的问题,在实际测试时加速度和磁力计几乎看不到作用


回复

使用道具 举报

29

TA的帖子

0

TA的资源

一粒金砂(中级)

Rank: 2

发表于 2017-6-28 08:01:15 | 显示全部楼层
嗯哼。是不对。不光是参数设置的问题。还有好多临界点的处理都没有。所以网上开源的的算法只有一个指导方向。实际应用还有许多需要处理的地方


回复

使用道具 举报

您需要登录后才可以回帖 登录 | 注册

本版积分规则

  • 论坛活动 E手掌握

    扫码关注
    EEWORLD 官方微信

  • EE福利  唾手可得

    扫码关注
    EE福利 唾手可得

小黑屋|手机版|Archiver|电子工程世界 ( 京ICP证 060456

GMT+8, 2018-4-21 17:36 , Processed in 0.199586 second(s), 19 queries , Redis On.

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