点击(此处)折叠或打开
- 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) 绝对加速度可以考虑忽略一下精度,例如 div 100, 让加速度在静止时能为0. ????? 可取否?
这种方式二次积分获得位置点,后面形成轨迹. 实际调试效果不佳. 勉强可用的情况是: 把 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)