标签：积分 Yaw MPU6050 integralX offset dt integralY gyroX 360
前几天研究学习了下MPU6050的姿态计算。Roll和Pitch角很容易得出，但是Yaw角就莫法直接得出。
因为重力加速度的Z轴，在相对地平面东西南北旋转时并无变化，因此只能靠Z轴的角速度来惯性累计估算旋转角度。
MPU6050仅靠Gryo角加速度来计算水平旋转时的Yaw偏航角度，关键是首先要去除零偏，然后是使用一阶互补积分滤波和卡尔曼滤波平滑数据跳动。
重点是要去除零偏，MPU6050的芯片即使静止放置不动，Gryo角加速度的数据也会不停偏移，必须要除掉这个偏移值。
而滤波算法的重点就是采样率，MPU6050初始化是什么采样率，取数据就得保证这个采样率，最好接上中断脚，用中断读取。
如果采样数据丢多了，角加速度根据算法累加出来的角度就会差很多。
要注意单片机读取问题，很多亲拿出来的数据漂的厉害，要么零偏没有去除，要么就是滤波算法采样有问题，读取数据的采样率不够，跟滤波算法带入的dt积分时间参数对不上。
我STC单片机跑得慢，主频只有30mHz，测试取一次数据要3-4ms左右，设置的分频是0x7，采样速率=1k/(1+7)=125Hz，间隔8ms读一次采样，加上发送的延迟，勉强够。
下面演示实际代码
首先拿到IMU的数据
/* IMU Data */
float gyroX, gyroY, gyroZ;
gyroX = (i2cData[8] << 8) | i2cData[9];
gyroY = (i2cData[10] << 8) | i2cData[11];
gyroZ = (i2cData[12] << 8) | i2cData[13];
gyroX /= 16.384f; // 我的MPU6050初始化的±2000LSB量程 (32768 / 2000) = 16.384f
gyroY /= 16.384f;
gyroZ /= 16.384f;
这里一开始让IMU静止不动，用1秒钟时间统计零偏，
我的采样率是125Hz，dt间隔时间是8ms，也就是统计125次采样做积分来确定零偏值：
btw:
上一篇写的卡尔曼滤波代码里面dt取值实际有问题
按10ms取一次数据，其实角速度数据已经有丢样，另外角速度没有去除零偏值，
所以积分出来的角度漂的很快，因重力加速度计算出来的角度权重比较大，所以最终结果只有卡尔曼滤波出来的角度比较准。
因上篇主要目的是演示卡尔曼滤波，就不回去改了。
static const float dt = 8.0 / 1000; // 间隔8ms
static const ZERO_OFFSET_COUN (1 / dt) // 1/8=125次每秒
static int g_GetZeroOffset = 0;
static float gyroX_offset = 0.0f, gyroY_offset = 0.0f, gyroZ_offset = 0.0f;
// 间隔8ms一次采样，统计125次，共1秒时间
if (g_GetZeroOffset++ < ZERO_OFFSET_COUN)
{
gyroX_offset += gyroX * dt; // 每次8%积分，累计加权125次
gyroY_offset += gyroY * dt;
gyroZ_offset += gyroZ * dt;
}
// 除去零偏
gyroX -= gyroX_offset;
gyroY -= gyroY_offset;
gyroZ -= gyroZ_offset;
然后就是靠惯性乘积去拿到XYZ三轴的旋转偏角
static float integralX = 0.0f, integralY = 0.0f, integralZ = 0.0f;
if (g_GetZeroOffset > ZERO_OFFSET_COUN) // 统计完零偏后开始累计偏角
{
integralX += gyroX * dt; // 每次8%权重累计偏转角度
integralY += gyroY * dt;
integralZ += gyroZ * dt;
// 360°一个循环
if (integralX > 360)
integralX -= 360;
if (integralX < -360)
integralX += 360;
if (integralY > 360)
integralY -= 360;
if (integralY < -360)
integralY += 360;
if (integralZ > 360)
integralZ -= 360;
if (integralZ < -360)
integralZ += 360;
}
这里integralZ就是Yaw角，物体相对地平面东南西北旋转的角度啦。
板子芯片正面朝天空摆放，逆时针转是正，顺时针转是负。
integralX和integralY分别对应Roll和Pitch角。
去除零偏后，测试板子静止摆放不动几分钟，积分的角度在零点几°范围来回摆。
静止摆10分钟的漂移也只有约1°内。几分钟的使用，按1°取值的精度足够用。
如果需要Yaw角长时间保持不飘，还是需要加磁力计。
只要Roll和Pitch角的数据，跟重力加速度计算出来的姿态角做个互补滤波或者卡尔曼滤波即可。
我看网上很多同志说漂移严重，应该很多都是没去除零偏，或者采样取值有问题，一般都是采样丢数据了。
来回旋转的精度没法精确测试，手拿板子旋转90°，再摆回去，来回转四五次，误差目测在1°内。
标签：积分,Yaw,MPU6050,integralX,offset,dt,integralY,gyroX,360 来源： https://www.cnblogs.com/qq70565912/p/15260148.html
本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享； 2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关； 3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关； 4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除； 5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。