stm32 MPU6050 姿态解算 Mahony互补滤波算法
文章目录
-
- 1，理论分析
-
- 1.1 MPU6050
- 1.2 Mahony算法原理
- 2，代码实现
-
- 1.1 MPU6050初始化及数据读取
- 1.2 Mahony算法c语言实现
- 1.3 将代码移植到你的工程
- 3，补充
项目地址： https://gitee.com/killerp/mpu6050_-mahony
1，理论分析
1.1 MPU6050
MPU6050是一个集成了陀螺仪和加速度计的传感器，它能输出在直角坐标系下的x，y，z轴的角速度和加速度数据。
陀螺仪输出的格式为：绕x轴的旋转角速度，绕y轴的角速度，绕z轴的角速度（分别称为roll角速度，pitch角速度和yaw角速度）。
加速度计输出的格式为：x轴的加速度，y轴的加速度，z轴的加速度。
另外还需要关注传感器的其他参数如：
- 陀螺仪的量程：eg.±2000dps
- 加速度计的量程：eg.2g
- adc转换精度为16bit
- 传感器采样率4-1000hz：eg.1000hz
我们从MPU6050那就得到了陀螺仪数据gx,gy,gz，加速度数据az,ay,az
螺仪转换精度2^16=65536,65536/{2000-(-2000)}=16.4,实际1°等于adc值16.4
采样率就是数据的更新率，也就是我们每次读取数据的频率。
1.2 Mahony算法原理
参考另一篇文章：基于Manony滤波算法的姿态解算
2，代码实现
1.1 MPU6050初始化及数据读取
该部分代码参考了正点原子的MPU6050例程；主要修改以下初始化代码
/*
* MPU6050模块:绕x轴为roll，绕y轴为pitch，绕z轴为yaw
*/
uint8_t MPU_Init(void)
{
uint8_t res;
IIC_Init(); //初始化IIC总线
MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X80); //复位MPU6050
//等待复位完成
delay_ms(100);
MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X00); //唤醒MPU6050
MPU_Set_Gyro_Fsr(3); //陀螺仪量程+-2000
MPU_Set_Accel_Fsr(0); //加速度计量程+-2g
MPU_Set_Rate(1000); //1khz采样率
MPU_Write_Byte(MPU_INT_EN_REG,0X00); //关闭中断
MPU_Write_Byte(MPU_USER_CTRL_REG,0X00); //关闭IIC主模式
MPU_Write_Byte(MPU_FIFO_EN_REG,0X00); //关闭FIFO
MPU_Write_Byte(MPU_INTBP_CFG_REG,0X80); //关闭INT
res=MPU_Read_Byte(MPU_DEVICE_ID_REG); //读取设备id，AD0引脚接地 故id应该为0x68
if(res==MPU_ADDR)
{
MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X01); //设置x轴为时钟
MPU_Write_Byte(MPU_PWR_MGMT2_REG,0X00); //开启陀螺仪加速度计
MPU_Set_Rate(1000);
}else return 1;
return 0;
}
重新编写一个读取mpu6050数据的函数。使我们读取的数据是经过平均滤波的数据。
使用6个FIFO队列对数据(gx,gy,gz,ax,ay,az)进行缓存，每次读取一次数据，就将数据入队，并计算队列的平均值，对原始数据进行平滑滤波。
#define Buf_SIZE 10 //队列长度，越大，平滑性越高
int16_t MPU6500_FIFO[6][Buf_SIZE]; //6个FIFO队列；0-2：陀螺仪数据；3-5：加速度计数据
int16_t lastAx,lastAy,lastAz,lastGx,lastGy,lastGz;
static uint8_t Wr_Index = 0; //当前FIFO的写入下标
//将val入队
static void MPU6500_NewVal(int16_t* buf,int16_t val) {
buf[Wr_Index] = val;
}
//计算FIFO中的平均值
static int16_t MPU6500_GetAvg(int16_t* buf)
{
int i;
int32_t sum = 0;
for(i=0;i<Buf_SIZE;i++)
sum += buf[i];
sum = sum / Buf_SIZE;
return (int16_t)sum;
}
//读取经过滤波的陀螺仪，加速度数据
void MPU6500_readGyro_Acc(int16_t *gyro,int16_t *acc)
{
static short buf[6]; //缓存原始数据：0-2：陀螺仪数据；3-5：加速度计数据
static int16_t gx,gy,gz;
static int16_t ax,ay,az;
//正点原子的库函数，读取传感器原始数据
MPU_Get_Gyroscope(&buf[0],&buf[1],&buf[2]);
MPU_Get_Accelerometer(&buf[3],&buf[4],&buf[5]);
//将原始数据入队
MPU6500_NewVal(&MPU6500_FIFO[0][0],buf[0]);
MPU6500_NewVal(&MPU6500_FIFO[1][0],buf[1]);
MPU6500_NewVal(&MPU6500_FIFO[2][0],buf[2]);
MPU6500_NewVal(&MPU6500_FIFO[3][0],buf[3]);
MPU6500_NewVal(&MPU6500_FIFO[4][0],buf[4]);
MPU6500_NewVal(&MPU6500_FIFO[5][0],buf[5]);
//更新FIFO入口指针
Wr_Index = (Wr_Index + 1) % Buf_SIZE;
//计算队列平均值
gx = MPU6500_GetAvg(&MPU6500_FIFO[4][0]);
gy = MPU6500_GetAvg(&MPU6500_FIFO[5][0]);
gz = MPU6500_GetAvg(&MPU6500_FIFO[6][0]);
//陀螺仪数据要减去偏移量
gyro[0] = gx - imu.Roll_offset; //gyro
gyro[1] = gy - imu.Pitch_offset;
gyro[2] = gz - imu.Yaw_offset;
ax = MPU6500_GetAvg(&MPU6500_FIFO[0][0]);
ay = MPU6500_GetAvg(&MPU6500_FIFO[1][0]);
az = MPU6500_GetAvg(&MPU6500_FIFO[2][0]);
acc[0] = ax; //acc
acc[1] = ay;
acc[2] = az;
}
1.2 Mahony算法c语言实现
首先将陀螺仪的数据转换成角度，这里封装成一个函数
//读取经过滤波的角速度，加速度
void get_IMU_Values(float *values)
{
int16_t gyro[3],acc[3];
MPU6500_readGyro_Acc(&gyro[0],&acc[0]);
for(int i=0;i<3;i++)
{
values[i]=((float) gyro[i])/16.4f; //这里结合理论分析思考
values[3+i]=(float) acc[i];
}
}
然后编写函数实现计算姿态角的功能，使用四元数计算姿态角的公式在理论分析中推导：
其中α为绕x轴旋转角即roll，β为绕y轴旋转角即pitch，γ为绕z轴旋转角即yaw。a,b,c,d即q0,q1,q2,q3.
//arcsin函数
float safe_asin(float v)
{
if (isnan(v)) {
return 0.0f;
}
if (v >= 1.0f) {
return PI/2;
}
if (v <= -1.0f) {
return -PI/2;
}
return asin(v);
}
//周期性的更新姿态角。
void get_angle(void )
{
static float q[4]; //四元数
float getValue[6]; //缓存读取的传感器数据
get_IMU_Values(getValue); //读取滤波后的传感器数据
//Mahony算法更新四元数
MahonyAHRSupdateIMU(getValue[0] * PI/180, getValue[1] * PI/180, getValue[2] * PI/180, getValue[3], getValue[4], getValue[5]);
//到此，用读取的数据更新了 q0,q1,q2,q3是定义在Mahony.c的全局变量，保存当前四元数的值
q[0] = q0;
q[1] = q1;
q[2] = q2;
q[3] = q3;
//更新全局变量IMU的属性
imu.ax = getValue[3];
imu.ay = getValue[4];
imu.az = getValue[5];
imu.Pitch_v = getValue[0];
imu.Roll_v = getValue[1];
imu.Yaw_v = getValue[2];
//通过四元数计算当前姿态角
imu.Roll = (atan2(2.0f*(q[0]*q[1] + q[2]*q[3]),1 - 2.0f*(q[1]*q[1] + q[2]*q[2])))* 180/PI;
imu.Pitch = -safe_asin(2.0f*(q[0]*q[2] - q[1]*q[3]))* 180/PI;
imu.Yaw = -atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2*q2 - 2 * q3 * q3 + 1)* 180/PI; // yaw
}
代码中MahonyAHRSupdateIMU()函数实现的就是四元数的更新算法。
逻辑上，首先用加速度计校准陀螺仪，方式是通过计算当前四元数姿态下的重力分量，与加速度计的重力分量作叉积，得到误差。
对误差作P(比例)和I(积分)运算后加到陀螺仪角速度上。最终由角速度计算新的四元数。
使用到的公式有：
四元数重力分量计算：
四元数旋转矩阵的转置表示：从地理坐标系转换到机体坐标系的旋转。重力向量设为[0,0,1]，与四元数旋转矩阵的转置矩阵相乘，表示机体坐标系下的重力分量。
所以由四元数得到的机体坐标系下的重力向量为：
由于加速度计测的是在机体坐标系下的重力向量，故将两个向量作叉积，即可得到他们之间的误差。
四元数更新方程：
代码中的 sampleFreq 即执行姿态解算的频率，这里用定时器，以500HZ的频率调用get_angle();
void MahonyAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az) {
float recipNorm;
float halfvx, halfvy, halfvz; //1/2 重力分量
float halfex, halfey, halfez; //1/2 重力误差
float qa, qb, qc;
//加速度数据有效时才进行校准
if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
//对加速度数据归一化
recipNorm = invSqrt(ax * ax + ay * ay + az * az);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;
// 由四元数计算重力分量
halfvx = q1 * q3 - q0 * q2;
halfvy = q0 * q1 + q2 * q3;
halfvz = q0 * q0 - 0.5f + q3 * q3;
// 将四元数重力分量 与 加速度计重力分量 作叉积 得到误差
halfex = (ay * halfvz - az * halfvy);
halfey = (az * halfvx - ax * halfvz);
halfez = (ax * halfvy - ay * halfvx);
// 使用积分？
if(twoKi > 0.0f) {
//对误差作积分
integralFBx += twoKi * halfex * (1.0f / sampleFreq); // integral error scaled by Ki
integralFBy += twoKi * halfey * (1.0f / sampleFreq);
integralFBz += twoKi * halfez * (1.0f / sampleFreq);
//反馈到角速度
gx += integralFBx; // apply integral feedback
gy += integralFBy;
gz += integralFBz;
}
else {
integralFBx = 0.0f; // prevent integral windup
integralFBy = 0.0f;
integralFBz = 0.0f;
}
// 对误差作比例运算并反馈
gx += twoKp * halfex;
gy += twoKp * halfey;
gz += twoKp * halfez;
}
// 计算1/2 dt
gx *= (0.5f * (1.0f / sampleFreq)); // pre-multiply common factors
gy *= (0.5f * (1.0f / sampleFreq));
gz *= (0.5f * (1.0f / sampleFreq));
qa = q0;
qb = q1;
qc = q2;
// 更新四元数
q0 += (-qb * gx - qc * gy - q3 * gz);
q1 += (qa * gx + qc * gz - q3 * gy);
q2 += (qa * gy - qb * gz + q3 * gx);
q3 += (qa * gz + qb * gy - qc * gx);
// 四元数归一化
recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
q0 *= recipNorm;
q1 *= recipNorm;
q2 *= recipNorm;
q3 *= recipNorm;
}
1.3 将代码移植到你的工程
项目地址：https://gitee.com/killerp/mpu6050_-mahony
若是使用stm32的软件iic，则需要在myiic.h中修改IO引脚。
若使用其他的芯片，则需要完成myiic.h中所有函数及延时函数的实现。
注意修改Mahony.h中的sampleFreq为定时器回调函数执行的频率。
3，补充
由于加速度计对水平方向的旋转无能为力，故用此程序得到的yaw角数据会一直漂移，无法得到校准；通常的解决方法是增加一个磁场传感器，来获得一个准确的水平方向角来校准陀螺仪的漂移。MPU6050支持扩展一个IIC接口到磁场传感器，可通过配置MPU6050的IIC MASTER 来读取磁场传感器的数据。
在Mahony中提供了包含磁场数据的融合函数：
void MahonyAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
