【山东大学软件学院 21 级项目实训】IMU的数据处理和滤波以及姿态估计
·
首先是导入相应的port_imu_i2c.h文件
#include "port_imu_i2c.h"
-
宏定义和全局变量
#define IMU_THRESHOLD 1
#define IMU_THRESHOLD_CAL 2
#define IMU_MAX_ERROR 6
#define FILTER_SIZE 60
#define MPU_GYRO_CFG_REG 0X1B //陀螺仪配置寄存器
static float error_offset_start = 0;
static int imu_port_id=0;
static int imu_init_state[10]={0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
struct Vector3f gypo_data;
static float error_tmp;
-
rad_limit ()将弧度限制在 −𝜋 到 𝜋 之间
float rad_limit(float rad)
{
while(1){
if (rad < 0-PI)
rad = 2*PI + rad;
else if (rad >= PI)
rad = rad - 2*PI;
else
return rad;
}
}
-
KalmanFilter() 卡尔曼滤波器函数，用于滤波处理传感器数据
float KalmanFilter(float inData)
{
static float kalman = 0; //上次卡尔曼值(估计出的最优值)
static float p = 10;
float q = 0.001; //q:过程噪声
float r = 0.001; //r:测量噪声
float kg = 0; //kg:卡尔曼增益
p += q;
kg = p / ( p + r ); //计算卡尔曼增益
kalman = kalman + (kg * (inData - kalman)); //计算本次滤波估计值
p = (1 - kg) * p; //更新测量方差
// return kalman; //返回估计值
return inData;
}
-
初始化的三维向量
void InitVector3f(struct Vector3f *v)
{
v->x=0.000000001f;
v->y=0.000000001f;
v->z=0.0000000001f;
}
-
队列操作（初始化，判空，插入，判满等）
void initQueue(QueueF* queue)
{
queue->front = 0;
queue->rear = 0;
}
// 判空：front == rear
bool isEmpty(const QueueF* queue)
{
if (queue->front == queue->rear)
return true;
else
return false;
}
// 判满：(rear+1) % MAXSIZE == front
bool isFull(const QueueF* queue)
{
if (queue->rear == (queue->front+1) % MAXSIZE)
return true;
else
return false;
}
// 进队列
void addElem(QueueF* queue, float val)
{
queue->data[queue->front] = val;
queue->front = (queue->front + 1) % MAXSIZE;
}
// 出队列
// 获取队列长度：(rear - front) % MAXSIZE
int getLen(const QueueF* queue)
{
return (queue->front - queue->rear + MAXSIZE) % MAXSIZE;
}
-
构建Imu状态结构体 ImuStateZ 记录IMU状态信息
-
导入 ImuData 数据结构体（角度，加速度，角速度等）
typedef struct _ImuStateZ
{
uint8_t state;
uint8_t count;
int filter_count;
float error_offset;
QueueF data_z;
}ImuStateZ;
ImuStateZ imu_state_z;
//导入 ImuData 数据结构体（角度，加速度，角速度等）
ImuData imu_data;
//ACC传感器测量得到的加速度
struct Vector3f acc_data;
//校准后的姿态角数据
struct Vector3f euler_angle;
float invSqrt(float x) /*快速开平方求倒*/
{
return 1.0f/sqrt(x);
}
static float q0 = 1, q1 = 0, q2 = 0, q3 = 0; // 最优估计四元数
//互补滤波使用变量
static float rMat[3][3]={ {0.0f,0.0f,0.0f},{0.0f,0.0f,0.0f},{0.0f,0.0f,0.0f} };/*旋转矩阵*/
static float exInt = 0.0f;
static float eyInt = 0.0f;
static float ezInt = 0.0f; /*积分误差累计*/
/*计算旋转矩阵*/
void imuComputeRotationMatrix(void)
{
float q1q1 = q1 * q1;
float q2q2 = q2 * q2;
float q3q3 = q3 * q3;
float q0q1 = q0 * q1;
float q0q2 = q0 * q2;
float q0q3 = q0 * q3;
float q1q2 = q1 * q2;
float q1q3 = q1 * q3;
float q2q3 = q2 * q3;
rMat[0][0] = 1.0f - 2.0f * q2q2 - 2.0f * q3q3;
rMat[0][1] = 2.0f * (q1q2 + -q0q3);
rMat[0][2] = 2.0f * (q1q3 - -q0q2);
rMat[1][0] = 2.0f * (q1q2 - -q0q3);
rMat[1][1] = 1.0f - 2.0f * q1q1 - 2.0f * q3q3;
rMat[1][2] = 2.0f * (q2q3 + -q0q1);
rMat[2][0] = 2.0f * (q1q3 + -q0q2);
rMat[2][1] = 2.0f * (q2q3 - -q0q1);
rMat[2][2] = 1.0f - 2.0f * q1q1 - 2.0f * q2q2;
}
//角度弧度转换系数
static const float RAD_TO_DEG = 180.0f / PI;
static const float DEG_TO_RAD = PI / 180.0f;
// 这里的Kp,Ki是用于控制加速度计修正陀螺仪积分姿态的速度
static float Kp= 1.33f;//0.8f//10.0f // 2.0f
static float Ki= 0.051f;//0.008f // 0.002f
void imuUpdate(struct Vector3f acc, struct Vector3f gyro, struct Vector3f *state , float dt) /*数据融合 互补滤波*/
{
float normalise;
float ex, ey, ez;
float halfT = 0.5f * dt;
gyro.x = gyro.x * DEG_TO_RAD; /* 度转弧度 */
gyro.y = gyro.y * DEG_TO_RAD;
gyro.z = gyro.z * DEG_TO_RAD;
/* 加速度计输出有效时,利用加速度计补偿陀螺仪*/
if((acc.x != 0.0f) || (acc.y != 0.0f) || (acc.z != 0.0f))
{
/*单位化加速计测量值*/
normalise = invSqrt(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z);
acc.x *= normalise;
acc.y *= normalise;
acc.z *= normalise;
/*加速计读取的方向与重力加速计方向的差值，用向量叉乘计算*/
ex = (acc.y * rMat[2][2] - acc.z * rMat[2][1]);
ey = (acc.z * rMat[2][0] - acc.x * rMat[2][2]);
ez = (acc.x * rMat[2][1] - acc.y * rMat[2][0]);
/*误差累计，与积分常数相乘*/
exInt += Ki * ex * dt ;
eyInt += Ki * ey * dt ;
ezInt += Ki * ez * dt ;
/*用叉积误差来做PI修正陀螺零偏，即抵消陀螺读数中的偏移量*/
gyro.x += Kp * ex + exInt;
gyro.y += Kp * ey + eyInt;
gyro.z += Kp * ez + ezInt;
}
/* 一阶近似算法，四元数运动学方程的离散化形式和积分 */
float q0Last = q0;
float q1Last = q1;
float q2Last = q2;
float q3Last = q3;
q0 += (-q1Last * gyro.x - q2Last * gyro.y - q3Last * gyro.z) * halfT;
q1 += ( q0Last * gyro.x + q2Last * gyro.z - q3Last * gyro.y) * halfT;
q2 += ( q0Last * gyro.y - q1Last * gyro.z + q3Last * gyro.x) * halfT;
q3 += ( q0Last * gyro.z + q1Last * gyro.y - q2Last * gyro.x) * halfT;
/*单位化四元数*/
normalise = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
q0 *= normalise;
q1 *= normalise;
q2 *= normalise;
q3 *= normalise;
imuComputeRotationMatrix(); /*计算旋转矩阵*/
/*计算roll pitch yaw 欧拉角*/
state->y/*pitch*/ = -asinf(rMat[2][0]) * RAD_TO_DEG;
state->x/*roll*/ = atan2f(rMat[2][1], rMat[2][2]) * RAD_TO_DEG;
state->z/*yaw*/ = atan2f(rMat[1][0], rMat[0][0]) * RAD_TO_DEG;
}
// 插入排序
void insertionSort(float *arr, int size)
{
for (int i = 1; i < size; i++)
{
int j = i - 1;
float key = arr[i];
/* Move all elements greater than key to one position */
while (j >= 0 && key < arr[j])
{
arr[j + 1] = arr[j];
j = j - 1;
}
/* Find a correct position for key */
arr[j + 1] = key;
}
}
//获取计算IMU在z轴方向的误差
void get_z_error(ImuStateZ state_z, float *error)
{
if(state_z.filter_count > MAXSIZE){
int start = (state_z.data_z.front+(MAXSIZE - FILTER_SIZE)/2)% MAXSIZE;
float errors[MAXSIZE];
for(int i=0;i<FILTER_SIZE;i++){
errors[i] = state_z.data_z.data[(start+i)%MAXSIZE];
// error_t = error_t + state_z.data_z.data[(start+i)%MAXSIZE];
}
insertionSort(errors, FILTER_SIZE);
float error_t = 0.0;
for(int i=10;i<FILTER_SIZE-10;i++){
error_t = error_t + errors[i];
}
*error = error_t/(FILTER_SIZE-20);
}
}
static float last_seconds = 0.0;
static uint32_t ImuTime5ms = 0;
static uint32_t ImuTime10ms = 0;
static float error_offset = 0.0;
static void time_imu(void){
ImuTime5ms++;
ImuTime10ms++;
if(ImuTime5ms >= 5){
ImuTime5ms = 0;
error_tmp = myDMP.gryoz - error_offset;
if(fabs(myDMP.gryoz - error_offset) < IMU_THRESHOLD){
addElem(&imu_state_z.data_z, myDMP.gryoz);
imu_state_z.filter_count++;
get_z_error(imu_state_z, &error_offset);
}
else{
imu_state_z.filter_count=0;
}
}
if(ImuTime10ms >= 10){
ImuTime10ms = 0;
acc_data.x=myDMP.accx;
acc_data.y=myDMP.accy;
acc_data.z=myDMP.accz;
//陀螺传感器得到的角速度
gypo_data.x=myDMP.gryox;
gypo_data.y=myDMP.gryoy;
gypo_data.z=myDMP.gryoz;
imuUpdate(acc_data, gypo_data, &euler_angle , 0.01); /*数据融合 互补滤波*/
gypo_data.z=myDMP.gryoz - error_offset;
// gypo_data.z=gro_z_filter - error_offset;
if(fabs(gypo_data.z) < IMU_THRESHOLD_CAL){
gypo_data.z = 0;
}
// euler_angle.z = euler_angle.z + gypo_data.z*0.01;
float deta_t = seconds() - last_seconds;
imu_data.angle_z = imu_data.angle_z + gypo_data.z*DEG_TO_RAD*deta_t;
last_seconds = seconds();
}
}
void get_imu_data(){
while(1){
gyroscope(imu_port_id,DMPCH_GYPO_Z);
wait(0.01);
}
}
// 设置MPU6050寄存器
uint8_t MPU_Write_Byte(uint8_t reg,uint8_t data){
i2c_write(imu_port_id,DMP_IIC_Addr, OPTADDWIDTH_U8, reg, 1, &data);
return 0;
}
uint8_t MPU_Read_Byte(uint8_t reg)
{
uint8_t res;
i2c_read(imu_port_id,DMP_IIC_Addr, OPTADDWIDTH_U8, reg, 1, &res);
return res;
}
// 初始化IMU校准角度，进行误差校准。
void imu_adjust_angle_init(int port_id, int couters){
imu_port_id = port_id;
SetI2C_Clock(100);//提速读取陀螺
if(couters>200){
couters = 200;
}
else if(couters<40){
couters = 40;
}
gyroscope(imu_port_id,DMPCH_GYPO_Z);
wait(0.05);
float error_result = 0.0;
float error_results[200];
float error_tmp = 0.0;
int i = 0;
for(int j=0;j<10;j++){
gyroscope(imu_port_id,DMPCH_GYPO_Z);
wait(0.005);
}
while (true)
{
error_tmp = gyroscope(imu_port_id,DMPCH_GYPO_Z);
if(fabs(error_tmp)< IMU_MAX_ERROR){
if(i == 0){
error_results[i] = error_tmp;
error_result = error_tmp;
i=1;
}
else if(fabs(error_tmp - error_result) < IMU_THRESHOLD){
// error_result = (error_result + error_tmp) / 2;
error_results[i] = error_tmp;
error_result = (error_result+error_tmp)/2;
i++;
}
else{
i = 0;
}
}
if(i>couters){
break;
}
wait(0.001);
}
insertionSort(error_results, couters);
float error_t = 0.0;
int start = (int)(couters / 6);
for(int j=start;j<(couters-start);j++){
error_t = error_t + error_results[j];
}
error_offset = error_t/(couters-start*2);
error_offset_start = error_offset;
wait(0.5);
ThreadStart(get_imu_data);
setTimerHandle(time_imu, 1);// 1ms定时器
}
// 重置IMU数据
void imu_reset()
{
error_offset = error_offset_start;
imu_data.angle_z = 0;
}
// 获取IMU姿态角数据。
float imu_get_atitude(uint8_t port_id, ImuData * imu_data_tar)
{
if(imu_init_state[port_id]==0){
imu_adjust_angle_init(port_id, 50);
imu_init_state[port_id]=1;
}
if(port_id == imu_port_id){
imu_data_tar->angle_z = rad_limit(imu_data.angle_z);
imu_data_tar->angle_x = euler_angle.x*DEG_TO_RAD;
imu_data_tar->angle_y = euler_angle.y*DEG_TO_RAD;
imu_data_tar->gra_acc_x = myDMP.accx*9.8;
imu_data_tar->gra_acc_y = myDMP.accy*9.8;
imu_data_tar->gra_acc_z = myDMP.accz*9.8;
imu_data_tar->w_x = gypo_data.x*DEG_TO_RAD;
imu_data_tar->w_y = gypo_data.y*DEG_TO_RAD;
imu_data_tar->w_z = gypo_data.z*DEG_TO_RAD;
return error_offset;
}
else{
memset(imu_data_tar, 0, sizeof(ImuData));
return 0;
}
}
// 获取IMU角度信息
float imu_get_angle(uint8_t port_id)
{
if(imu_init_state[port_id]==0){
imu_adjust_angle_init(port_id, 50);
imu_init_state[port_id]=1;
}
return rad_limit(imu_data.angle_z)*RAD_TO_DEG;
}
// 获取IMU误差信息。
float imu_get_error(uint8_t port_id)
{
return error_tmp;
}
魔乐社区（Modelers.cn) 是一个中立、公益的人工智能社区，提供人工智能工具、模型、数据的托管、展示与应用协同服务，为人工智能开发及爱好者搭建开放的学习交流平台。社区通过理事会方式运作，由全产业链共同建设、共同运营、共同享有，推动国产AI生态繁荣发展。
更多推荐
5
0- 0
已为社区贡献1条内容
所有评论(0)