【uFun试用体验】使用陀螺仪姿态解算

新手平衡小车的卡尔曼滤波算法总结.rar

1.设置好陀螺仪后可以获取陀螺仪的三轴加速度数据，设置好量程，坐标轴既可以进行姿态解算。

Accel_X = (i2c_buff[0] << 8|i2c_buff[1])* -1;
Accel_Y = (i2c_buff[2] << 8|i2c_buff[3] )* -1;
Accel_Z = ( i2c_buff[4] << 8|i2c_buff[5]);

复制代码

上诉的-1为调整坐标轴方向。
2.将得到数据除以灵敏度得到各方向的加速度值，读取的数据最好进行滤波，如滑动平均滤波。

Accel.X = (float)Accel_Xtempout / 8192.0; //835.918f; ////8192 = 32768 /4 835.918
Accel.Y = (float)Accel_Ytempout / 8192.0; //835.918f;
Accel.Z = (float)Accel_Ztempout / 8192.0; //835.918f;

复制代码

8192 = 32768 /4 。其中32768是方向为正，量程为±4g时的最大值。正确的算法是（读到的加速度值） / (量程最大值（注意最大值）) = （你需要的加速度值）/ 4。所以运算一下得到你需要的单位为g的加速度值。
3.得到三轴的加速度数据后即可进行姿态解算，由于缺少三轴陀螺仪数据，瞬间运动时姿态数据无法修正，所以并没有使用四元素算法解算，直接根据几何知识得出三个姿态角。
公式及原理参考知乎博主的文章

https://zhuanlan.zhihu.com/p/20082486?columnSlug=devymex

复制代码

得到roll角

float fNorm = sqrt(read_acc[0] * read_acc[0] + read_acc[1] * read_acc[1] + read_acc[2] * read_acc[2]);
float fNormXZ = sqrt(read_acc[0] * read_acc[0] + read_acc[2] * read_acc[2]);
float fCos = fNormXZ / fNorm;
float roll= acosf(fCos) * 57.3;
if read_acc[1] > 0) {
roll = -roll
}

复制代码

得到pitch