当前位置: 代码迷 >> 综合 >> 对于互补滤波的解释
  详细解决方案

对于互补滤波的解释

热度:43   发布时间:2024-01-17 05:29:45.0

一、互补滤波原理:

陀螺仪动态响应特性良好,但计算姿态时,会产生累积误差。数字罗盘和加速度传感器测量姿态没有累积误差,但动态响应较差。因此,它们在频域上特性互补,可以采用互补滤波器融合这三种传感器的数据,提高测量精度和系统的动态性能。

二、实际程序操作过程

主要讲一下互补滤波程序

float ComplementaryFilter(float angle_m, float gyro_m)
{gyro_m -= 0.55;angleyy = K1 * angle_m + (1-K1) * (angleylast + gyro_m * dtt);angleylast = angleyy;return angleyy;
}

上面的程序中有两个传入参数,一个是MPU6050硬件算出来的角度,但是它会受加速度的影响,也就是,将mpu6050来回左右晃的时候,这个角度会有较大波动,理论上是保持不变的,这个角度的特点是变化比较迅速,但不是很准确。第二个参数是传回来的加速度,我们通过积分,可以得到短时间内的角度,这个值比较短时间内比较准,但是会有累计误差。所以我们综合两个值的优势,取权重,一般取K1=0.1,也就是说短时间的积分值占主要作用。综合两者的优势,我们发现效果特别明显,来回晃动,角度至基本保持不变·!

下面是完整的一套程序

#include"MPU6050.h"float gyro_x,gyro_y,gyro_z,acc_x,acc_y,acc_z;
float angleyy,angleylast;
float dtt = 0.005;
float K1 = 0.005;void Delay(long t)
{    volatile long i=t,j;while(i--)    {j=1;            //加大循环,就加大这个while(j--);}
}void I2C_Init(void)
{gpio_init  (SCL_PIN, GPO, 1);    //初始化gpiogpio_init  (SDA_PIN, GPO, 1);    //初始化gpioport_pull (SCL_PIN,ENABLE);port_pull (SDA_PIN,ENABLE);
}
//**************************************
//I2C起始信号
//**************************************
void I2C_Start(void)
{SDADIR_OUT();SCLDIR_OUT();SDA1();                    //拉高数据线SCL1();                    //拉高时钟线Delay(1);                 //延时SDA0();                    //产生下降沿Delay(1);                 //延时SCL0();                    //拉低时钟线
}
//**************************************
//I2C停止信号
//**************************************
void I2C_Stop(void)
{SDADIR_OUT();SCLDIR_OUT();SDA0();                    //拉低数据线SCL1();                    //拉高时钟线Delay(1);                 //延时SDA1();                    //产生上升沿Delay(1);                 //延时
}
//**************************************
//I2C发送应答信号
//入口参数:ack (0:ACK 1:NAK)
//**************************************
void I2C_SendACK(unsigned char ack_dat)
{SCL0();SDADIR_OUT();SCLDIR_OUT();if(ack_dat) SDA0();else    SDA1();SCL1();                    //拉高时钟线Delay(1);                 //延时SCL0();                    //拉低时钟线Delay(1);                 //延时
}
//**************************************
//I2C接收应答信号
//**************************************
uint8_t I2C_RecvACK(void)
{uint8_t cry;SCL0();SDADIR_IN();SCLDIR_OUT();SCL1();Delay(1);if(GET_SDA) cry=1;else   cry=0;SCL0();Delay(1);return(cry);}
//**************************************
//向I2C总线发送一个字节数据
//**************************************
void I2C_Senduint8_t(uint8_t dat)
{uint8_t i;SDADIR_OUT();SCLDIR_OUT();for (i=0; i<8; i++)         //8位计数器{if(dat&0x80)SDA1();               //送数据口elseSDA0();dat <<= 1;              //移出数据的最高位SCL1();                //拉高时钟线Delay(1);                 //延时SCL0();                //拉低时钟线Delay(1);                 //延时}while(I2C_RecvACK());
}
//**************************************
//从I2C总线接收一个字节数据
//**************************************
uint8_t I2C_Recvuint8_t(void)
{uint8_t i,cy;uint8_t dat = 0;SDADIR_OUT();SDA1();                    //使能内部上拉,准备读取数据,SDADIR_IN();for (i=0; i<8; i++)         //8位计数器{dat <<= 1;SCL1();                //拉高时钟线Delay(1);if(GET_SDA) cy = 1;else    cy = 0;                  //延时dat |= cy;             //读数据SCL0();                //拉低时钟线Delay(1);                 //延时}return dat;
}uint8 read_ch(uint8 ack_x)
{uint8 i;uint8 c;c=0;SCL0();Delay(1);SDADIR_OUT();SDA1();             //置数据线为输入方式SDADIR_IN();for(i=0;i<8;i++){Delay(1);SCL0();         //置时钟线为低,准备接收数据位Delay(1);SCL1();         //置时钟线为高,使数据线上数据有效Delay(1);c<<=1;if(GET_SDA) c+=1;   //读数据位,将接收的数据存c}SDADIR_OUT();SCL0();Delay(1);I2C_SendACK(ack_x);return c;
}//**************************************
//向I2C设备写入一个字节数据
//**************************************
void Single_WriteI2C(uint8_t REG_Address,uint8_t REG_data)
{I2C_Start();                  //起始信号I2C_Senduint8_t(SlaveAddress);   //发送设备地址+写信号I2C_Senduint8_t(REG_Address);    //内部寄存器地址,I2C_Senduint8_t(REG_data);       //内部寄存器数据,I2C_Stop();                   //发送停止信号
}
//**************************************
//从I2C设备读取一个字节数据
//**************************************
uint8_t Single_ReadI2C(uint8_t REG_Address)
{uint8_t REG_data;I2C_Start();                   //起始信号I2C_Senduint8_t(SlaveAddress);    //发送设备地址+写信号I2C_Senduint8_t(REG_Address);     //发送存储单元地址,从0开始I2C_Start();                   //起始信号I2C_Senduint8_t(SlaveAddress+1);  //发送设备地址+读信号REG_data=I2C_Recvuint8_t();       //读出寄存器数据I2C_SendACK(0);                //接收应答信号I2C_Stop();                    //停止信号return REG_data;
}
//**************************************
//初始化MPU6050
//**************************************
void InitMPU6050(void)
{I2C_Init();//SCLIO |= 0x02;//SDAIO |= 0x08;Single_WriteI2C(PWR_MGMT_1, 0x00);	//解除休眠状态Single_WriteI2C(SMPLRT_DIV, 0x07);Single_WriteI2C(CONFIG, 0x06);Single_WriteI2C(GYRO_CONFIG, 0x18);Single_WriteI2C(ACCEL_CONFIG, 0x10);
}
//**************************************
//合成数据
//**************************************
void I2C_Read_regs(uint8 dev_add, uint8 reg, uint8 *dat_add, uint8 num)
{I2C_Start();I2C_Senduint8_t(dev_add | 0x00);  //发送器件地址加写位I2C_Senduint8_t( reg );   				//发送从机寄存器地址I2C_Start();I2C_Senduint8_t(dev_add | 0x01);  //发送器件地址加读位while(--num){*dat_add = read_ch(1); //读取数据dat_add++;}*dat_add = read_ch(0); //读取数据I2C_Stop();
}void Get_AccData(void)
{uint8 dat[6];I2C_Read_regs(SlaveAddress, ACCEL_XOUT_H, dat, 6);acc_x = (int16)(((uint16)dat[0]<<8 | dat[1]))*9.8/4096.0;acc_y = (int16)(((uint16)dat[2]<<8 | dat[3]))*9.8/4096.0;acc_z = (int16)(((uint16)dat[4]<<8 | dat[5]))*9.8/4096.0;
}void Get_Gyro(void)
{uint8 dat[6];I2C_Read_regs(SlaveAddress, GYRO_XOUT_H, dat, 6);gyro_z = (int16)(((uint16)dat[0]<<8 | dat[1]))/16.4;gyro_y = (int16)(((uint16)dat[2]<<8 | dat[3]))/16.4;gyro_z = (int16)(((uint16)dat[4]<<8 | dat[5]))/16.4;
}float ComplementaryFilter(float angle_m, float gyro_m)
{gyro_m -= 0.55;angleyy = K1 * angle_m + (1-K1) * (angleylast + gyro_m * dtt);angleylast = angleyy;return angleyy;
}头文件
#ifndef _MPU6050_H_
#define _MPU6050_H_#include "KEA_gpio.h"//定义引脚
#define SCL_PIN PTH4
#define SDA_PIN PTH3//#define SCL  PTXn_T(SCL_PIN,OUT) //IIC时钟引脚定义  
//#define SDA  PTXn_T(SDA_PIN,OUT) //IIC数据引脚定义//#define SCLIO PTXn_T(SCL_PIN,DDR)
//#define SDAIO PTXn_T(SDA_PIN,DDR)#define GET_SDA             gpio_get (SDA_PIN)
#define SDA0()          gpio_set (SDA_PIN, 0)	//SDA = 0
#define SDA1()          gpio_set (SDA_PIN, 1)	//SDA = 1
#define SCL0()          gpio_set (SCL_PIN, 0)	//SCL = 0
#define SCL1()          gpio_set (SCL_PIN, 1)	//SCL = 1
#define SDADIR_OUT()    gpio_ddr (SDA_PIN, GPO)    //输出方向   SDAIO = 1
#define SDADIR_IN()     gpio_ddr (SDA_PIN, GPI)    //输入方向   SDAIO = 0
#define SCLDIR_OUT()    gpio_ddr(SCL_PIN,GPO)
#define SCLDIR_IN()     gpio_ddr(SCL_PIN,GPI)//测试样例,看 main_test 函数//****************************************
// 定义MPU6050内部地址
//****************************************
#define	SMPLRT_DIV		0x19	//陀螺仪采样率,典型值:0x07(125Hz)
#define	CONFIG		    0x1A	//低通滤波频率,典型值:0x06(5Hz)
#define	GYRO_CONFIG		0x1B	//陀螺仪自检及测量范围,典型值:0x18(不自检,2000deg/s)
#define	ACCEL_CONFIG	0x1C	//加速计自检、测量范围及高通滤波频率,典型值:0x01(不自检,2G,5Hz)
#define	ACCEL_XOUT_H	0x3B
#define	ACCEL_XOUT_L	0x3C
#define	ACCEL_YOUT_H	0x3D
#define	ACCEL_YOUT_L	0x3E
#define	ACCEL_ZOUT_H	0x3F
#define	ACCEL_ZOUT_L	0x40#define	GYRO_XOUT_H		0x43
#define	GYRO_XOUT_L		0x44
#define	GYRO_YOUT_H		0x45
#define	GYRO_YOUT_L		0x46
#define	GYRO_ZOUT_H		0x47
#define	GYRO_ZOUT_L		0x48
#define	PWR_MGMT_1		0x6B	//电源管理,典型值:0x00(正常启用)
#define	WHO_AM_I			0x75	//IIC地址寄存器(默认数值0x68,只读)
#define	SlaveAddress	    0xD0	//IIC写入时的地址字节数据,+1为读取//****************************************
//函数声明
//****************************************
//MPU6050操作函数extern float gyro_x,gyro_y,gyro_z,acc_x,acc_y,acc_z;void  InitMPU6050(void);													//初始化MPU6050
void  I2C_Init(void);
void  I2C_Start(void);
void  I2C_Stop(void);
void  Get_Gyro(void);
void  Get_AccData(void);
void  I2C_Senduint8_t(uint8_t data);
uint8_t  I2C_Recvuint8_t(void);
uint8_t  Single_ReadI2C(uint8_t REG_Address);						//读取I2C数据
void  Single_WriteI2C(uint8_t REG_Address,uint8_t REG_data);	//向I2C写入数据
void I2C_Read_regs(uint8 dev_add, uint8 reg, uint8 *dat_add, uint8 num);
//int GetData(uint8_t REG_Address);
float ComplementaryFiltering(float angle_m, float gyro_m);#endif                                                                                                                                                                                                                                                     
  相关解决方案