Gausst松鼠会

这个屌丝很懒，什么也没留下！

热门标签

MPU6050使用一阶互补和卡尔曼滤波算法平滑角度数据_mpu6050卡尔曼滤波

作者：Gausst松鼠会 | 2024-05-25 19:26:08

踩

mpu6050卡尔曼滤波

最近项目上想用MPU6050来自动探测物体的转向角度，花了2天时间学习如何拿陀螺仪的姿态角度，发现蛮难的，写点笔记。
下面是哔哩哔哩的一堆废话讲解，只想看代码本体的可以直接跳到最后。

应用场景是51单片机环境，有一块MPU6060，需要知道硬件板子水平摆放时，板子摆放的姿态和旋转的角度。编译环境只能用C语言。
首先单片机通过TTL串口接到MPU6050上拿到通信数据，水平旋转角度需要另外加地磁仪通过南北极磁性拿到。很遗憾设计硬件时没注意这茬，只用了一块MPU6050。不过呢可以用旋转时的角速度求出旋转幅度（这个下篇说）。但是拿到原始数据后，发现原始数据的跳动非常厉害，需要用带滤波的积分算法平滑过滤。

开始网上翻相关的算法，看到卡尔曼滤波和互补滤波算法，但是没有能跑起来的代码。但是很多纯写理论，我这种数学白痴完全看不懂。要么直接甩代码，云里雾里不明就里，根据理论推论很多代码甚至是错误的，各种计算方式，变量和函数名字也完全理解不了。
又翻了很久，在某代码里发现一句注释，找到国外TKJ Electronics的一系列代码，使用结合卡尔曼滤波和互补滤波算法的演示。终于搞了份可以用的代码。
TKJElectronics在github上有两份代码（ https://github.com/TKJElectronics），KalmanFilter和Example-Sketch-for-IMU-including-Kalman-filter，分别是卡尔曼滤波算法，和IMU融合卡尔曼滤波的样例。

代码演示了计算Roll，Pitch角和Yaw角并用卡尔曼过滤算法。
样例里面有四种芯片的演示，我用的是MPU6050，就直接看这个目录了，里面还有MPU6050+HMC5883L磁力计的样式，可惜手头板子当初没有想过算Yaw角旋转要磁力计，也就莫法实现读取Yaw角。加HMC5883L磁力计的那个样例，是读的磁力计的数据来算Yaw轴角度。
MPU6050的重力加速度出来的z轴数据基本不咋变化，仅依靠x和y轴数据肯定不准的，所以这里通过重力加速度算出来Yaw轴的角度无意义。

继续回来说代码。
下载回来的样例代码扩展名是.ino，搞不懂啥，改成.c，一样看，c语言万岁！
首先要先拿到陀螺仪的数据，角速度和重力加速度。
如何读取MPU6050的数据我略过。网上有很多现成的样例，直接拿来用。

/* IMU Data */

float accX, accY, accZ;

float gyroX, gyroY, gyroZ;

accX = ((i2cData[0] << 8) | i2cData[1]);

accY = ((i2cData[2] << 8) | i2cData[3]);

accZ = ((i2cData[4] << 8) | i2cData[5]);

//tempRaw = (i2cData[6] << 8) | i2cData[7];

gyroX = (i2cData[8] << 8) | i2cData[9];

gyroY = (i2cData[10] << 8) | i2cData[11];

gyroZ = (i2cData[12] << 8) | i2cData[13];

i2cData是MPU6050读到的14个字节的数据。
i2cData[6]和[7]是温度不用。
6个变量，样例里是double类型，我改float了，用不到那么高精度，且我是51单片机运算，计算资源极其有限。

acc打头的变量是重力加速度，gyro打头的变量是角速度。
这里角速度和重力加速度容易让人搞晕。
角速度反应的是芯片的运动加速度，静止时是个很小值，芯片运动的时候XYZ三轴值会对应速度变化。
重力加速度跟重力有关，反应芯片的摆放状态，垂直于地心引力。要获取芯片的摆放姿态，只需要重力加速度就够了。

芯片读出来的数据是16位，数据范围是2的16次方65536，10进制整数范围是-32768到32768。
这里要根据MPU6050初始化时，设置的角速度和重力加速度各自的量程换算下来，我设置的角速度是±2000deg/s，重力加速度是2g，分别使用32768除以2和2000就是各自换算单位。

TKJ Electronics样例里面double gyroXrate = gyroX / 131.0; 是用的250deg/s的量程，就是32768/250≈131
btw，2000deg/s是有多快？我感觉一般手机摇一摇这种，250deg/s应该就够吧？
我自己修改的代码中(32768/2)和(32768/2000)是使用的2000deg/s的量程计算单位

accX /= 16384.0f; // (32768 / 2) = 16384.0f

accY /= 16384.0f;

accZ /= 16384.0f;

gyroX /= 16.384f; // (32768 / 2000) = 16.384f

gyroY /= 16.384f;

gyroZ /= 16.384f;

下面是如何根据acc重力加速度计算芯片目前相对地面的两个偏转角度（roll和pitch角）。
上面说过，靠重力加速度无法计算yaw角，所以这里也略过yaw。

// Source: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf

// eq. 25 and eq. 26

// atan2 outputs the value of -π to π (radians) - see http://en.wikipedia.org/wiki/Atan2

// It is then converted from radians to degrees

#ifdef RESTRICT_PITCH // Eq. 25 and 26

float roll = atan2(accY, accZ) * RAD_TO_DEG;

float pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;

#else // Eq. 28 and 29

float roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;

float pitch = atan2(-accX, accZ) * RAD_TO_DEG;

#endif

这个计算的理论公式我自己没去推算（人笨...）。
国外有个学习网站详细描述了各种计算公式可以收藏参考，我看网上很多文章也就是照搬上面的图片和翻译。
数学达人收藏推荐：
Maths - Euler Angles - Martin Baker
喜欢刨根问底的理论基础的可以拿去学习。

回来继续说代码。

RAD_TO_DEG 是弧度到度的转换单位，定义如下，我也稍调整了下，把即时计算改成了静态值定义

//#define RAD_TO_DEG (360/PI/2) // 弧度转角度的转换率

//#define DEG_TO_RAD (2*PI/360) // 角度转弧度的转换率

#define RAD_TO_DEG 57.295779513082320876798154814105 // 弧度转角度的转换率

#define DEG_TO_RAD 0.01745329251994329576923690768489 // 角度转弧度的转换率

有个RESTRICT_PITCH宏定义，貌似用来约束atan输出的角度？不同的翻转计算方式不一样？
注释里有链接，里面是算法解释，全是矩阵方程，数学大牛可以看看。
欧拉角三维翻转有个方向死锁问题，不过一般应用，平衡小车，四轴飞行器都不会用到翻转90°，我这里应用场景也是不会发生垂直翻转场景，所以这里我是默认定义没去深究。

接下来就是卡尔曼滤波了，卡尔曼滤波一开始要先设置初始角度，原代码如下

kalmanX.setAngle(roll);

kalmanY.setAngle(pitch);

样例代码是cpp，搞了两个滤波对象对应x和y轴，分别初始化设置。
为了在单片机上跑，我给改成了c形式。

typedef struct

{

float Q_angle;

float Q_bias;

float R_measure;

float angle; // Reset the angle

float bias; // Reset bias

float P[2][2];

// Unbiased rate calculated from the rate and the calculated bias - you have to call getAngle to update the rate

float rate;

} KalmanFilter_t;

typedef struct

{

KalmanFilter_t *pKalmanX;

KalmanFilter_t *pKalmanY;

float gyroXangle, gyroYangle; // Angle calculate using the gyro only

float compAngleX, compAngleY; // Calculated angle using a complementary filter

float kalAngleX, kalAngleY; // Calculated angle using a Kalman filter

} KalmanFilterSys_t;

// Need set starting angle

static KalmanFilterSys_t *Get_Kalman_Filter(float roll, float pitch)

{

KalmanFilterSys_t *pSys = (KalmanFilterSys_t *)calloc(1, sizeof(KalmanFilterSys_t));

pSys->pKalmanX = (KalmanFilter_t *)calloc(1, sizeof(KalmanFilter_t));

pSys->pKalmanY = (KalmanFilter_t *)calloc(1, sizeof(KalmanFilter_t));

/* We will set the variables like so, these can also be tuned by the user */

pSys->pKalmanX->Q_angle = pSys->pKalmanY->Q_angle = 0.001f;

pSys->pKalmanX->Q_bias = pSys->pKalmanY->Q_bias = 0.003f;

pSys->pKalmanX->R_measure = pSys->pKalmanY->R_measure = 0.03f;

pSys->pKalmanX->angle = roll; // Reset the angle

pSys->pKalmanY->angle = pitch; // Reset bias

// Since we assume that the bias is 0 and we know the starting angle (use setAngle),

// the error covariance matrix is set like so -

// see: http://en.wikipedia.org/wiki/Kalman_filter#Example_application.2C_technical

pSys->pKalmanX->P[0][0] = 0.0f;

pSys->pKalmanX->P[0][1] = 0.0f;

pSys->pKalmanX->P[1][0] = 0.0f;

pSys->pKalmanX->P[1][1] = 0.0f;

pSys->pKalmanY->P[0][0] = 0.0f;

pSys->pKalmanY->P[0][1] = 0.0f;

pSys->pKalmanY->P[1][0] = 0.0f;

pSys->pKalmanY->P[1][1] = 0.0f;

pSys->gyroXangle = roll;

pSys->gyroYangle = pitch;

pSys->compAngleX = roll;

pSys->compAngleY = pitch;

return pSys;

};

显示详细信息

这里创建结构时，附带就把roll和pitch对应的angle初始化了。
结构创建好，直接设置数据计算即可，同样函数我给改成了c语言函数，同时设置roll和pitch。

float dt = 10.0 / 1000;

Kalman_Fileter_SetAngle(m_pKalmanFilter, roll, pitch, gyroX, gyroY, dt);

这里的dt直接影响一阶互补滤波的计算权重，我测试是10毫秒读一次数据，1秒作为拟合周期，dt就是10.0 / 1000毫秒，需要根据自身使用情况调整。TKJ Electronics的样例是用的1微妙，拟合周期也是1秒。
调用时dt取两次读取数据的实际间隔时间。我是用单片机中断读取数据，延迟时间很稳定，所以取固定值10ms。

注意自己的采样频率，如果是125Hz采样频率，dt间隔时间就是8ms，每1秒钟要读125次角度数据。
卡尔曼滤波的参数用的默认值，几个参数具体如何调整，数学大牛可以继续问看度娘，我也没去细看算法理论。
每次采样数据，经过Kalman_Fileter_SetAngle计算后，打印下各个数据结果看看

char text[260];

_snprintf_s(text, _TRUNCATE,

_T("roll(%.4f %.4f %.4f %.4f) pitch(%.4f %.4f %.4f %.4f)\n"),

roll,

m_pKalmanFilter->gyroXangle,

m_pKalmanFilter->compAngleX,

m_pKalmanFilter->kalAngleX,

pitch,

m_pKalmanFilter->gyroYangle,

m_pKalmanFilter->compAngleY,

m_pKalmanFilter->kalAngleY);

printf(text);

显示详细信息

这里打印的结果，
roll和pitch是原始的计算角度，会看到跳变会比较厉害，时大时小。
gyroXangle和gyroYangl经过dt乘积后，会看到数据基本抑制住跳变，但数据会有一点不断的累加漂移。
compAngleX和compAngleY是角速度与重力加速度算出的偏转角，进行一阶互补过滤后的角度，这个两个数据相对就比较稳定了。
kalAngleX和kalAngleY就是卡尔曼过滤算法后的角度数据了。这两个数据就是最稳定的。

这里的一阶互补过滤默认参数0.093和0.07权重占比，卡尔曼过滤也是用的默认参数，手动翻转电路板晃来晃去，目测计算的结果很精准，感觉不需要再调这个权重比例。
网上其他很多卡尔曼滤波代码拿来测试过，但是这个权重则是各种参数，算法看代码也是各种形式五花八门，实际跑出来的结果乱飘，真心不能直接抄…
可惜以前写的一个绘制频谱的软件代码搞丢了，不然可以实时显示对比下这些不同算法的计算结果曲线，对比看那个代码是准确的且平滑效果最佳。但我目测下，感觉就TKJ Electronics网站上这个样例算法最佳。

最后附上整个我自己整合修改后的卡尔曼角度滤波代码。原版代码是C++，我照顾51单片机里运行改成了C代码。

KalmanFilter.h

#pragma once

//#define RAD_TO_DEG (360/PI/2) // 弧度转角度的转换率

//#define DEG_TO_RAD (2*PI/360) // 角度转弧度的转换率

#define RAD_TO_DEG 57.295779513082320876798154814105 // 弧度转角度的转换率

#define DEG_TO_RAD 0.01745329251994329576923690768489 // 角度转弧度的转换率

// Comment out to restrict roll to ±90deg instead -

// please read: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf

#define RESTRICT_PITCH

typedef struct

{

float Q_angle;

float Q_bias;

float R_measure;

float angle; // Reset the angle

float bias; // Reset bias

float P[2][2];

// Unbiased rate calculated from the rate and the calculated bias - you have to call getAngle to update the rate

float rate;

} KalmanFilter_t;

typedef struct

{

KalmanFilter_t *pKalmanX;

KalmanFilter_t *pKalmanY;

float gyroXangle, gyroYangle; // Angle calculate using the gyro only

float compAngleX, compAngleY; // Calculated angle using a complementary filter

float kalAngleX, kalAngleY; // Calculated angle using a Kalman filter

} KalmanFilterSys_t;

// Need set starting angle

static KalmanFilterSys_t *Get_Kalman_Filter(float roll, float pitch)

{

KalmanFilterSys_t *pSys = (KalmanFilterSys_t *)calloc(1, sizeof(KalmanFilterSys_t));

pSys->pKalmanX = (KalmanFilter_t *)calloc(1, sizeof(KalmanFilter_t));

pSys->pKalmanY = (KalmanFilter_t *)calloc(1, sizeof(KalmanFilter_t));

/* We will set the variables like so, these can also be tuned by the user */

pSys->pKalmanX->Q_angle = pSys->pKalmanY->Q_angle = 0.001f;

pSys->pKalmanX->Q_bias = pSys->pKalmanY->Q_bias = 0.003f;

pSys->pKalmanX->R_measure = pSys->pKalmanY->R_measure = 0.03f;

pSys->pKalmanX->angle = roll; // Reset the angle

pSys->pKalmanY->angle = pitch; // Reset bias

// Since we assume that the bias is 0 and we know the starting angle (use setAngle),

// the error covariance matrix is set like so -

// see: http://en.wikipedia.org/wiki/Kalman_filter#Example_application.2C_technical

pSys->pKalmanX->P[0][0] = 0.0f;

pSys->pKalmanX->P[0][1] = 0.0f;

pSys->pKalmanX->P[1][0] = 0.0f;

pSys->pKalmanX->P[1][1] = 0.0f;

pSys->pKalmanY->P[0][0] = 0.0f;

pSys->pKalmanY->P[0][1] = 0.0f;

pSys->pKalmanY->P[1][0] = 0.0f;

pSys->pKalmanY->P[1][1] = 0.0f;

pSys->gyroXangle = roll;

pSys->gyroYangle = pitch;

pSys->compAngleX = roll;

pSys->compAngleY = pitch;

return pSys;

}

// Source: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf

//eq. 25 and eq. 26

// atan2 outputs the value of -π to π (radians) - see http://en.wikipedia.org/wiki/Atan2

// It is then converted from radians to degrees

static void Accel_To_Angle(float *p_roll, float *p_pitch, float accX, float accY, float accZ)

{

#ifdef RESTRICT_PITCH // Eq. 25 and 26

*p_pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;

*p_roll = atan2(accY, accZ) * RAD_TO_DEG;

#else // Eq. 28 and 29

*p_pitch = atan2(-accX, accZ) * RAD_TO_DEG;

*p_roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;

#endif

};

// The angle should be in degrees and the rate should be in degrees per second and the delta time in seconds

static float Kalman_Filter_GetAngle(KalmanFilter_t *pSys, float newAngle, float newRate, float dt)

{

// KasBot V2 - Kalman filter module - http://www.x-firm.com/?page_id=145

// Modified by Kristian Lauszus

// See my blog post for more information: http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it

// Discrete Kalman filter time update equations - Time Update ("Predict")

// Update xhat - Project the state ahead

/* Step 1 */

pSys->rate = newRate - pSys->bias;

pSys->angle += dt * pSys->rate;

// Update estimation error covariance - Project the error covariance ahead

/* Step 2 */

pSys->P[0][0] += dt * (dt*pSys->P[1][1] - pSys->P[0][1] - pSys->P[1][0] + pSys->Q_angle);

pSys->P[0][1] -= dt * pSys->P[1][1];

pSys->P[1][0] -= dt * pSys->P[1][1];

pSys->P[1][1] += pSys->Q_bias * dt;

// Discrete Kalman filter measurement update equations - Measurement Update ("Correct")

// Calculate Kalman gain - Compute the Kalman gain

/* Step 4 */

float S = pSys->P[0][0] + pSys->R_measure; // Estimate error

/* Step 5 */

float K[2]; // Kalman gain - This is a 2x1 vector

K[0] = pSys->P[0][0] / S;

K[1] = pSys->P[1][0] / S;

// Calculate angle and bias - Update estimate with measurement zk (newAngle)

/* Step 3 */

float y = newAngle - pSys->angle; // Angle difference

/* Step 6 */

pSys->angle += K[0] * y;

pSys->bias += K[1] * y;

// Calculate estimation error covariance - Update the error covariance

/* Step 7 */

float P00_temp = pSys->P[0][0];

float P01_temp = pSys->P[0][1];

pSys->P[0][0] -= K[0] * P00_temp;

pSys->P[0][1] -= K[0] * P01_temp;

pSys->P[1][0] -= K[1] * P00_temp;

pSys->P[1][1] -= K[1] * P01_temp;

return pSys->angle;

};

static void Kalman_Fileter_SetAngle(KalmanFilterSys_t *pSys, float roll, float pitch,

float gyroXrate, float gyroYrate, float dt)

{

#ifdef RESTRICT_PITCH

// This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees

if ((roll < -90 && pSys->kalAngleX > 90) || (roll > 90 && pSys->kalAngleX < -90)) {

pSys->pKalmanX->angle = roll;

pSys->compAngleX = roll;

pSys->kalAngleX = roll;

pSys->gyroXangle = roll;

}

else

pSys->kalAngleX = Kalman_Filter_GetAngle(pSys->pKalmanX, roll, gyroXrate, dt); // Calculate the angle using a Kalman filter

if (abs(pSys->kalAngleX) > 90)

gyroYrate = -gyroYrate; // Invert rate, so it fits the restriced accelerometer reading

pSys->kalAngleY = Kalman_Filter_GetAngle(pSys->pKalmanY, pitch, gyroYrate, dt);

#else

// This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees

if ((pitch < -90 && pSys->kalAngleY > 90) || (pitch > 90 && pSys->kalAngleY < -90)) {

pSys->pKalmanY->angle = pitch;

pSys->compAngleY = pitch;

pSys->kalAngleY = pitch;

pSys->gyroYangle = pitch;

}

else

pSys->kalAngleY = Kalman_Filter_GetAngle(pSys->pKalmanY, pitch, gyroYrate, dt); // Calculate the angle using a Kalman filter

if (abs(pSys->kalAngleY) > 90)

gyroXrate = -(gyroXrate); // Invert rate, so it fits the restriced accelerometer reading

pSys->kalAngleX = Kalman_Filter_GetAngle(pSys->pKalmanX, roll, gyroXrate, dt); // Calculate the angle using a Kalman filter

#endif

pSys->gyroXangle += gyroXrate * dt; // Calculate gyro angle without any filter

pSys->gyroYangle += gyroYrate * dt;

//pSys->gyroXangle += kalmanX.getRate() * dt; // Calculate gyro angle using the unbiased rate

//pSys->gyroYangle += kalmanY.getRate() * dt;

// Calculate the angle using a Complimentary filter

pSys->compAngleX = 0.93 * (pSys->compAngleX + gyroXrate * dt) + 0.07 * roll;

pSys->compAngleY = 0.93 * (pSys->compAngleY + gyroYrate * dt) + 0.07 * pitch;

// Reset the gyro angle when it has drifted too much

if (pSys->gyroXangle < -180 || pSys->gyroXangle > 180)

pSys->gyroXangle = pSys->kalAngleX;

if (pSys->gyroYangle < -180 || pSys->gyroYangle > 180)

pSys->gyroYangle = pSys->kalAngleY;

};

static void Delete_Kalman_Filter(KalmanFilterSys_t **ppSys)

{

free((*ppSys)->pKalmanX);

free((*ppSys));

*ppSys = 0;

};

显示详细信息

MPU6050使用一阶互补和卡尔曼滤波算法平滑角度数据 - 裤子多多 - 博客园 (cnblogs.com)

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/Gausst松鼠会/article/detail/623453