用加速度和角速度计算位移

 

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

公式: 先扣除重力分量
Ax = (AX - 16384*sin(Pitch/180*3.14))/16384
Ay = (AY - 16384*sin(Roll/180*3.14))/16384
				
					重力加速度在一个方向上最大为 16384

       投射到绝对坐标系

绝对坐标系内X轴加速度 ax = Ax * cos(Yaw) + Ay * sin(Yaw)
绝对坐标系内Y轴加速度ay = Ay * cos(Yaw) + Ax * sin(Yaw)

 

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

实际的演算公式就是:
 1. 开机静止, 初始化MPU, 先采集1000个绝对加速度, 去最后200个做均值得到 offset_ax, offset_ay
2. 开始运动.
加速度校准
int x_a = ax - offset_ax;
int y_a = ay - offset_ay;
速度积分: 例如积分时间为 0.2秒
int x_v = (x_a * 0.2 + x_v + 0.5);
int y_v = (y_a * 0.2 + y_v + 0.5); //因为y_a/y_v  x_a/x_v 都是整数, 防止误差扩大 + 0.5 达到四舍五入效果
位移
float y_s = (y_s + y_v * 0.2)
float x_s = (x_s + x_v * 0.2)

实际效果还是有误差, 需要更精确地算法或者采用更精准的原件