wordpress快速扒站,网站设计与制作前景,网站建设设计规划书,wordpress 显示链接替换【STM32 HAL库】MPU6050姿态解算 卡尔曼滤波 前言MPU6050寄存器代码详解mpu6050.cmpu6050.h 使用说明 前言
本篇文章基于卡尔曼滤波的原理详解与公式推导#xff0c;来详细的解释下如何使用卡尔曼滤波来解算MPU6050的姿态 参考资料#xff1a;Github_mpu6050
MPU6050寄存器… 【STM32 HAL库】MPU6050姿态解算 卡尔曼滤波 前言MPU6050寄存器代码详解mpu6050.cmpu6050.h 使用说明 前言
本篇文章基于卡尔曼滤波的原理详解与公式推导来详细的解释下如何使用卡尔曼滤波来解算MPU6050的姿态 参考资料Github_mpu6050
MPU6050寄存器
我们简单介绍下MPU6050驱动里设计到的寄存器详情见MPU-60X0寄存器手册 Who Am I 寄存器 为数不多的默认值不为0的寄存器其默认值为0x68也即MPU6050的设备地址通过读取该寄存器的值来判断识别到的设备是否是MPU6050 Power Management 1 寄存器 通过该寄存器配置电源模式、时钟源选择、设备复位和睡眠模式等功能。 Sample Rate Divider 寄存器 采样速率分频器通过该寄存器设置分频系数配置 MPU6050 数据输出速率。 CONFIGURATION 寄存器 有GYROSCOPE CONFIGURATION 和 ACCELEROMETER CONFIGURATION 寄存器用来配置陀螺仪、加速度计参数 详情见MPU-60X0寄存器手册
MEASUREMENT 寄存器 用来存放加速度计、温度计、陀螺仪测量到的原始数据 详情见MPU-60X0寄存器手册
代码详解
mpu6050.c
/** mpu6050.c** Created on: Nov 13, 2019* Author: Bulanov Konstantin** Contact information* -------------------** e-mail : leech001gmail.com*//** |---------------------------------------------------------------------------------* | Copyright (C) Bulanov Konstantin,2021* |* | This program is free software: you can redistribute it and/or modify* | it under the terms of the GNU General Public License as published by* | the Free Software Foundation, either version 3 of the License, or* | any later version.* |* | This program is distributed in the hope that it will be useful,* | but WITHOUT ANY WARRANTY; without even the implied warranty of* | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the* | GNU General Public License for more details.* |* | You should have received a copy of the GNU General Public License* | along with this program. If not, see http://www.gnu.org/licenses/.* |* | Kalman filter algorithm used from https://github.com/TKJElectronics/KalmanFilter* |---------------------------------------------------------------------------------*/#include math.h
#include mpu6050.h#define RAD_TO_DEG 57.295779513082320876798154814105 //用以弧度转角度角度 弧度 × (180/π)其中 180/π 的值约为 57.2958#define WHO_AM_I_REG 0x75 // MPU6050 的 WHO_AM_I 寄存器地址。通过读取该寄存器的值如果返回 0x68则表示该 I2C 设备是 MPU6050 传感器。
#define PWR_MGMT_1_REG 0x6B // MPU6050 的 Power Management 1 寄存器地址 (电源管理 1 寄存器)用于配置电源模式、时钟源选择、设备复位和睡眠模式等功能。
#define SMPLRT_DIV_REG 0x19 // MPU6050 的 Sample Rate Divider 寄存器地址 (采样速率分频器)通过该寄存器设置分频系数配置 MPU6050 数据输出速率。
#define ACCEL_CONFIG_REG 0x1C // MPU6050 的 ACCEL_CONFIG 寄存器地址 (加速度计配置寄存器)用于设置加速度计的量程范围例如 ±2g、±4g 等。
#define ACCEL_XOUT_H_REG 0x3B // MPU6050 的加速度计 X 轴高位数据寄存器地址用于读取 X 轴加速度高字节数据。
#define TEMP_OUT_H_REG 0x41 // MPU6050 的温度传感器高位数据寄存器地址用于读取温度的高字节数据。
#define GYRO_CONFIG_REG 0x1B // MPU6050 的 GYRO_CONFIG 寄存器地址 (陀螺仪配置寄存器)用于设置陀螺仪的量程范围例如 ±250°/s、±500°/s 等。
#define GYRO_XOUT_H_REG 0x43 // MPU6050 的陀螺仪 X 轴高位数据寄存器地址用于读取 X 轴陀螺仪角速度的高字节数据。// Setup MPU6050
#define MPU6050_ADDR 0xD0 // MPU6050 的补位后的 8 位地址MPU6050的 7 位地址为 110 100x (0x68 或 0x69)最低位 x 取决于 AD0 引脚状态。I2C 通信时左移一位并在最低位加 0写或 1读形成 8 位地址。以写为例MPU6050 地址为 1101 0000 (0xD0) 表示写操作
const uint16_t i2c_timeout 100; // I2C 通信超时时间单位为毫秒。
const double Accel_Z_corrector 14418.0;uint32_t timer;Kalman_t KalmanX {.Q_angle 0.001f,.Q_bias 0.003f,.R_measure 0.03f};Kalman_t KalmanY {.Q_angle 0.001f,.Q_bias 0.003f,.R_measure 0.03f,
};uint8_t MPU6050_Init(I2C_HandleTypeDef *I2Cx)
{uint8_t check; // 存储从 WHO_AM_I 寄存器读取到的设备 ID实际上是该寄存器的值0x68该寄存器默认值为 0x68 (104)也即 MPU6050 的地址uint8_t Data;HAL_I2C_Mem_Read(I2Cx, MPU6050_ADDR, WHO_AM_I_REG, 1, check, 1, i2c_timeout); // 通过 I2Cx 访问 MPU6050 的 WHO_AM_I 寄存器读取 1 字节数据到 check超时时间为 i2c_timeout。if (check 104) // 检验 check 的值 是否为 0x68{// 设置 Power Management 1 寄存器 (电源管理1寄存器)将其第7位 置1 (1000 0000) 进行设备复位防止数据残留Data 0x80;HAL_I2C_Mem_Write(I2Cx, MPU6050_ADDR, PWR_MGMT_1_REG, 1, Data, 1, i2c_timeout);// 设置 Power Management 1 寄存器 (电源管理1寄存器)将其所有位置0 (0000 0000) 进行唤醒Data 0;HAL_I2C_Mem_Write(I2Cx, MPU6050_ADDR, PWR_MGMT_1_REG, 1, Data, 1, i2c_timeout);// 设置 Sample Rate Divider 寄存器 (采样速率分频器)采样速率 1kHz / ( 0X04 1) 200HzData 0x04;HAL_I2C_Mem_Write(I2Cx, MPU6050_ADDR, SMPLRT_DIV_REG, 1, Data, 1, i2c_timeout);//设置 ACCEL_CONFIG 寄存器 (加速度计配置 Accelerometer Configuration )// 关闭X Y Z轴的自检功能配置量程为(-2g,2g) XA_ST0,YA_ST0,ZA_ST0, FS_SEL0 - - 2gData 0x00;HAL_I2C_Mem_Write(I2Cx, MPU6050_ADDR, ACCEL_CONFIG_REG, 1, Data, 1, i2c_timeout);//设置 GYRO_CONFIG 寄存器 (陀螺仪配置 Gyroscopic Configuration )// 关闭X Y Z轴的自检功能并配置量程为 (-250 °/s,250 °/s) XG_ST0, YG_ST0, ZG_ST0 , FS_SEL0 - ±250 °/sData 0x00;HAL_I2C_Mem_Write(I2Cx, MPU6050_ADDR, GYRO_CONFIG_REG, 1, Data, 1, i2c_timeout);return 0; //返回0表示 MPU6050 正常}return 1; //返回1表示 MPU6050 异常
}void MPU6050_Read_Accel(I2C_HandleTypeDef *I2Cx, MPU6050_t *DataStruct)
{uint8_t Rec_Data[6]; //存储读取到的 6 字节加速度计原始数据// 通过 I2Cx 访问 MPU6050 的 Accelerometer Measurements 寄存器组中的 ACCEL_XOUT_H_REG 寄存器(加速度计X轴加速度高字节 寄存器)读取 6 字节数据( X轴加速度的高字节 --- Z轴加速度的低字节 )到 Rec_Data 超时时间为 i2c_timeout。HAL_I2C_Mem_Read(I2Cx, MPU6050_ADDR, ACCEL_XOUT_H_REG, 1, Rec_Data, 6, i2c_timeout);//将读取到的存储在 Rec_Data[6] 中的原始数据 分离转换为 X、Y、Z轴加速度的 16 位有符号整数DataStruct-Accel_X_RAW (int16_t)(Rec_Data[0] 8 | Rec_Data[1]);DataStruct-Accel_Y_RAW (int16_t)(Rec_Data[2] 8 | Rec_Data[3]);DataStruct-Accel_Z_RAW (int16_t)(Rec_Data[4] 8 | Rec_Data[5]);/*** 将原始值转换为以 g 为单位的加速度值。根据 FS_SEL 配置来进行换算这里 FS_SEL 0对应量程为 ±2g因此需要将原始值除以 16384.0来得到加速度值。 根据 Accelerometer Measurements 加速度计测量寄存器组中的 AFS_SEL 配置中的描述可知量程对应的除数更多细节请参考 ACCEL_CONFIG 与 Accelerometer Measurements寄存器的配置。 ****/// 将原始值转换为 g 为单位的加速度DataStruct-Ax DataStruct-Accel_X_RAW / 16384.0; // X 轴加速度DataStruct-Ay DataStruct-Accel_Y_RAW / 16384.0; // Y 轴加速度DataStruct-Az DataStruct-Accel_Z_RAW / Accel_Z_corrector; //Z 轴加速度 (由于Z轴加速度计的值会受重力、传感器零飘、传感器精度与校准的影响导致在水平放置时Accel_Z_RAW 不为 2g 量程对应的理论值 16384而是实际值。修正参数就是根据实际情况测出的用来矫正零飘和其他影响的校正因子
}void MPU6050_Read_Gyro(I2C_HandleTypeDef *I2Cx, MPU6050_t *DataStruct)
{uint8_t Rec_Data[6]; //存储读取到的 6 字节陀螺仪计原始数据// 通过 I2Cx 访问 MPU6050 的 Gyroscope Measurements 寄存器组中的 GYRO_XOUT_H_REG 寄存器(陀螺仪 X 轴角速度高字节 寄存器)读取 6 字节数据( X 轴角速度的高字节 --- Z 轴角速度的低字节 )到 Rec_Data 超时时间为 i2c_timeout。HAL_I2C_Mem_Read(I2Cx, MPU6050_ADDR, GYRO_XOUT_H_REG, 1, Rec_Data, 6, i2c_timeout);//将读取到的存储在 Rec_Data[6] 中的原始数据 分离转换为 X、Y、Z轴角速度的 16 位有符号整数DataStruct-Gyro_X_RAW (int16_t)(Rec_Data[0] 8 | Rec_Data[1]);DataStruct-Gyro_Y_RAW (int16_t)(Rec_Data[2] 8 | Rec_Data[3]);DataStruct-Gyro_Z_RAW (int16_t)(Rec_Data[4] 8 | Rec_Data[5]);/*** 将原始值转换为度每秒dpsdegrees per second。根据 FS_SEL 的配置选择适当的比例系数。这里假设 FS_SEL 0对应量程为 ±250 °/s因此需要将原始值除以 131.0 来得到角速度dps。根据 Gyroscope Measurements 角速度计测量寄存器组中的 FS_SEL 配置中的描述可知量程对应的除数更多细节请参考 GYRO_CONFIG 与 Gyroscope Measurements寄存器的配置。 ****/// 将原始值转换为 dps 为单位的加速度DataStruct-Gx DataStruct-Gyro_X_RAW / 131.0; // X 轴角速度DataStruct-Gy DataStruct-Gyro_Y_RAW / 131.0; // Y 轴角速度DataStruct-Gz DataStruct-Gyro_Z_RAW / 131.0; // Z 轴角速度
}void MPU6050_Read_Temp(I2C_HandleTypeDef *I2Cx, MPU6050_t *DataStruct)
{uint8_t Rec_Data[2]; // 用于存储从温度寄存器读取的 2 字节数据int16_t temp; // 用于存储拼接后的 16 位原始温度数据// 从 Temperature Measurement 温度测量寄存器组 中的 TEMP_OUT_H 寄存器开始读取 2 字节数据高字节和低字节HAL_I2C_Mem_Read(I2Cx, MPU6050_ADDR, TEMP_OUT_H_REG, 1, Rec_Data, 2, i2c_timeout);// 将读取到的高字节和低字节拼接为 16 位的有符号整数temp (int16_t)(Rec_Data[0] 8 | Rec_Data[1]);// 将原始温度值转换为摄氏温度 (详情见 Temperature Measurement// 转换公式温度值(°C) (原始值 / 340.0) 36.53 DataStruct-Temperature (float)((int16_t)temp / (float)340.0 (float)36.53);
}/*** brief 读取 MPU6050 所有传感器数据并使用卡尔曼滤波器计算角度* * 该函数通过 I2C 接口读取 MPU6050 传感器的加速度计、陀螺仪和温度的数据* 并使用卡尔曼滤波器对预测角度与测量角度(加速度计和陀螺仪数据)进行融合以计算俯仰角pitch和滚转角roll。** param I2Cx I2C 句柄用于通过 I2C 接口与 MPU6050 通信* param DataStruct 指向 MPU6050_t 结构体的指针存储读取到的原始传感器数据以及处理后的数据如加速度、角速度、角度等*/
void MPU6050_Read_All(I2C_HandleTypeDef *I2Cx, MPU6050_t *DataStruct)
{uint8_t Rec_Data[14]; // 用于存储从 MPU6050 读取的 14 字节数据int16_t temp; // 临时变量用于存储原始温度数据//1.通过 I2C 接口从 MPU6050 读取 14 字节加速度、温度、角速度数据HAL_I2C_Mem_Read(I2Cx, MPU6050_ADDR, ACCEL_XOUT_H_REG, 1, Rec_Data, 14, i2c_timeout);//2.解析数据// 解析加速度计数据DataStruct-Accel_X_RAW (int16_t)(Rec_Data[0] 8 | Rec_Data[1]);DataStruct-Accel_Y_RAW (int16_t)(Rec_Data[2] 8 | Rec_Data[3]);DataStruct-Accel_Z_RAW (int16_t)(Rec_Data[4] 8 | Rec_Data[5]);// 解析温度数据temp (int16_t)(Rec_Data[6] 8 | Rec_Data[7]);// 解析陀螺仪数据DataStruct-Gyro_X_RAW (int16_t)(Rec_Data[8] 8 | Rec_Data[9]);DataStruct-Gyro_Y_RAW (int16_t)(Rec_Data[10] 8 | Rec_Data[11]);DataStruct-Gyro_Z_RAW (int16_t)(Rec_Data[12] 8 | Rec_Data[13]);//3.转换解析后的数据 原始数据 -- 实际意义数据// 将原始加速度计数据 --- 以 g 为单位的加速度值DataStruct-Ax DataStruct-Accel_X_RAW / 16384.0;DataStruct-Ay DataStruct-Accel_Y_RAW / 16384.0;DataStruct-Az DataStruct-Accel_Z_RAW / Accel_Z_corrector;// 将原始温度数据 --- 摄氏温度DataStruct-Temperature (float)((int16_t)temp / (float)340.0 (float)36.53);// 将原始陀螺仪数据 --- 角速度(以 度每秒 (dps) 为单位DataStruct-Gx DataStruct-Gyro_X_RAW / 131.0;DataStruct-Gy DataStruct-Gyro_Y_RAW / 131.0;DataStruct-Gz DataStruct-Gyro_Z_RAW / 131.0;//4.卡尔曼滤波// 计算时间增量 dt单位为秒double dt (double)(HAL_GetTick() - timer) / 1000; // 获取时间差毫秒转换为秒timer HAL_GetTick(); // 更新计时器// 计算滚转角 rolldouble roll; // 用于存储计算得到的滚转角X 轴double roll_sqrt sqrt(DataStruct-Accel_X_RAW * DataStruct-Accel_X_RAW DataStruct-Accel_Z_RAW * DataStruct-Accel_Z_RAW);if (roll_sqrt ! 0.0){roll atan(DataStruct-Accel_Y_RAW / roll_sqrt) * RAD_TO_DEG; // 先计算出弧度制 roll 再弧度转换为角度值}else{roll 0.0;}// 计算俯仰角 pitchdouble pitch atan2(-DataStruct-Accel_X_RAW, DataStruct-Accel_Z_RAW) * RAD_TO_DEG;// 如果俯仰角度变化过快(超过90度)防止角度跳变if ((pitch -90 DataStruct-KalmanAngleY 90) || (pitch 90 DataStruct-KalmanAngleY -90)){KalmanY.angle pitch; DataStruct-KalmanAngleY pitch;}else{// 卡尔曼滤波器更新俯仰角度 YDataStruct-KalmanAngleY Kalman_getAngle(KalmanY, pitch, DataStruct-Gy, dt);}// 如果俯仰角绝对值超过 90 度则反转 X 轴的陀螺仪角速度防止符号错误if (fabs(DataStruct-KalmanAngleY) 90)DataStruct-Gx -DataStruct-Gx;// 卡尔曼滤波器更新滚转角度 XDataStruct-KalmanAngleX Kalman_getAngle(KalmanX, roll, DataStruct-Gx, dt);
}/*** brief 使用卡尔曼滤波融合角度预测值与角度测量值得到最优角度值* * 卡尔曼滤波通过结合系统的预测值和测量值来修正角度和偏置的估计减少噪声对结果的影响。* * param Kalman 卡尔曼滤波器结构体指针包含预测角度、预测偏置、角度协方差、偏置协方差、噪声协方差及协方差矩阵* param newAngle 角度测量值读取加速度计的三轴加速度分量再计算反正切得到角度测量值* param newRate 角速度“实际“值他只是可以看作角速度实际值实际上是角速度测量值。只不过因陀螺仪精度问题而有过程噪声。可以理解为匀变速直线运动的状态方程中必须有 速度v 的参与这个 v 实际上也是测量值* param dt 时间间隔两个传感器数据采样之间的时间差秒* return double 滤波后的最优角度值*/double Kalman_getAngle(Kalman_t *Kalman, double newAngle, double newRate, double dt)
{
/*---------------------预测阶段--------------------------*/// 1. 预测角度// 角速度 陀螺仪角速度 - 陀螺仪偏置值 (得到无偏角速度)double rate newRate - Kalman-bias;// 预测角度 前一时刻角速 时间间隔*角速度Kalman-angle dt * rate;// 2. 预测协方差矩阵Kalman-P[0][0] dt * (dt * Kalman-P[1][1] - Kalman-P[0][1] - Kalman-P[1][0] Kalman-Q_angle); // 预测角度协方差Kalman-P[0][1] - dt * Kalman-P[1][1]; // 预测角度和偏置的协方差Kalman-P[1][0] - dt * Kalman-P[1][1]; // 预测偏置和角度的协方差Kalman-P[1][1] Kalman-Q_bias * dt; // 预测偏置协方差/*---------------------更新阶段--------------------------*/// 3. 更新卡尔曼增益// 总误差协方差 预测协方差 测量噪声协方差double S Kalman-P[0][0] Kalman-R_measure;// 卡尔曼增益 Kdouble K[2]; K[0] Kalman-P[0][0] / S; // 角度的卡尔曼增益K[1] Kalman-P[1][0] / S; // 偏置的卡尔曼增益// 4. 更新角度和偏置// 测量残差 测量值 - 预测值double y newAngle - Kalman-angle;// 根据卡尔曼增益更新角度和偏置的估计值修正预测阶段的误差Kalman-angle K[0] * y; // 更新角度估计。Kalman-bias K[1] * y; // 更新偏置估计// 5. 更新协方差矩阵 Pdouble P00_temp Kalman-P[0][0];double P01_temp Kalman-P[0][1];Kalman-P[0][0] - K[0] * P00_temp; // 更新角度协方差Kalman-P[0][1] - K[0] * P01_temp; // 更新角度和偏置的协方差Kalman-P[1][0] - K[1] * P00_temp; // 更新偏置和角度的协方差Kalman-P[1][1] - K[1] * P01_temp; // 更新偏置协方差// 6. 返回滤波后的最优角度值return Kalman-angle;
};mpu6050.h
/** mpu6050.h** Created on: Nov 13, 2019* Author: Bulanov Konstantin* 本头文件定义了用于操作 MPU6050 传感器的结构和函数包括加速度计、陀螺仪、温度传感器的读取函数* 以及用于角度计算的卡尔曼滤波算法。*/#ifndef INC_GY521_H_
#define INC_GY521_H_#endif /* INC_GY521_H_ */#include stdint.h
#include i2c.h/* * MPU6050 数据结构体* 该结构体保存从 MPU6050 传感器读取的原始加速度和原始陀螺仪数据* 以及经过处理后的加速度、角速度和温度数据*/
typedef struct
{int16_t Accel_X_RAW; // X 轴加速度原始数据int16_t Accel_Y_RAW; // Y 轴加速度原始数据int16_t Accel_Z_RAW; // Z 轴加速度原始数据double Ax; // X 轴加速度值gdouble Ay; // Y 轴加速度值gdouble Az; // Z 轴加速度值gint16_t Gyro_X_RAW; // X 轴陀螺仪原始数据int16_t Gyro_Y_RAW; // Y 轴陀螺仪原始数据int16_t Gyro_Z_RAW; // Z 轴陀螺仪原始数据double Gx; // X 轴角速度值°/sdouble Gy; // Y 轴角速度值°/sdouble Gz; // Z 轴角速度值°/sfloat Temperature; // 传感器的温度°Cdouble KalmanAngleX; // X 轴的卡尔曼滤波计算角度double KalmanAngleY; // Y 轴的卡尔曼滤波计算角度
} MPU6050_t;/* * 卡尔曼滤波器结构体* 用于根据 MPU6050 数据平滑地估算出角度滤除噪声并提供更稳定的输出*/
typedef struct
{double Q_angle; // 角度过程噪声协方差double Q_bias; // 偏差过程噪声协方差double R_measure; // 测量噪声协方差double angle; // 当前估计角度double bias; // 当前估计偏差double P[2][2]; // 误差协方差矩阵
} Kalman_t;// 初始化 MPU6050 传感器配置 MPU6050 参数返回 0 表示成功1 表示失败
uint8_t MPU6050_Init(I2C_HandleTypeDef *I2Cx); // 读取 MPU6050 加速度计数据更新到 mpu6050 结构体中
void MPU6050_Read_Accel(I2C_HandleTypeDef *I2Cx, MPU6050_t *DataStruct); // 读取 MPU6050 陀螺仪数据更新到 mpu6050 结构体中
void MPU6050_Read_Gyro(I2C_HandleTypeDef *I2Cx, MPU6050_t *DataStruct); // 读取 MPU6050 温度数据更新到 mpu6050 结构体中
void MPU6050_Read_Temp(I2C_HandleTypeDef *I2Cx, MPU6050_t *DataStruct); // 读取 MPU6050 的加速度计、陀螺仪和温度数据并更新到 mpu6050 结构体中
void MPU6050_Read_All(I2C_HandleTypeDef *I2Cx, MPU6050_t *DataStruct); // 使用卡尔曼滤波器计算角度根据新测得的角度和角速度更新滤波器返回平滑的角度值
double Kalman_getAngle(Kalman_t *Kalman, double newAngle, double newRate, double dt);使用说明
STEP1复制mpu6050的.c .h到你的工程文件夹中并add文件与编译路径
STEP2Includes
#include mpu6050.hSTEP3声明私有变量PV
/ * USER CODE BEGIN PV * /
MPU6050_t MPU6050;
/ * USER CODE END PV * /STEP4初始化MPU6050
void setup(void){while (MPU6050_Init(hi2c1) 1);
}STEP5调用函数读取并解析数据
void loop(void){MPU6050_Read_All(hi2c1, MPU6050);HAL_Delay (100);
}经过卡尔曼滤波后得到的数据(最优估计)为MPU6050.KalmanAngleX和MPU6050.KalmanAngleY 可以打印输出
