http://www.docin.com/p-1806017607.html 

点击(此处)折叠或打开

  1. Page 6
  2. 在二维平面内, 通过MPU的加速度 AX, AY, 以及姿态航向角Yaw(Z轴旋转角). 投射到OXY所在的绝对坐标系中. 


  1. 公式: 先扣除重力分量
  2. Ax = (AX - 16384*sin(Pitch/180*3.14))/16384
    Ay = (AY - 16384*sin(Roll/180*3.14))/16384

    重力加速度在一个方向上最大为 16384


       投射到绝对坐标系

  1. 绝对坐标系内X轴加速度 ax = Ax * cos(Yaw) + Ay * sin(Yaw)
  2. 绝对坐标系内Y轴加速度ay = Ay * cos(Yaw) + Ax * sin(Yaw)
  3. 绝对加速度可以考虑忽略一下精度,例如 div 100, 让加速度在静止时能为0. ????? 可取否?

 

    这种方式二次积分获得位置点,后面形成轨迹. 实际调试效果不佳.  勉强可用的情况是: 把 ax / ay 做个历史记录, 用算数平均值滤波算法得到加速度数据, 然后求位移. (初始化后先获取几百个数据, 计算平均值作为静态误差用于均值校准)


点击(此处)折叠或打开

  1. 实际的演算公式就是:
  2.  1. 开机静止, 初始化MPU, 先采集1000个绝对加速度, 去最后200个做均值得到 offset_ax, offset_ay
  3. 2. 开始运动.
  4. 加速度校准
  5. int x_a = ax - offset_ax;
  6. int y_a = ay - offset_ay;
  7. 速度积分: 例如积分时间为 0.2秒
  8. int x_v = (x_a * 0.2 + x_v + 0.5);
  9. int y_v = (y_a * 0.2 + y_v + 0.5); //因为y_a/y_v x_a/x_v 都是整数, 防止误差扩大 + 0.5 达到四舍五入效果
  10. 位移
  11. float y_s = (y_s + y_v * 0.2)
  12. float x_s = (x_s + x_v * 0.2)
实际效果还是有误差, 需要更精确地算法或者采用更精准的原件
              

                        
    


10-09 02:00
查看更多