gy521 读取数据 + stm32f103zet6 + 姿态融合 + 卡尔曼滤波

提示：文章写完后，目录可以自动生成，如何生成可参考右边的帮助文档

---

前言

一、引脚接法

VCC接电源正极

GND接电源负极

SCL和SDA接单片机IO口（依据程序的定义接口接，IIC通信）

XDA和XCL接外围传感器（一般不接直接悬空）

AD0 接地/VCC 由MPU硬件地址决定

INIT外部中断（悬空）

#define MPU_ADDR 0X6

二、MPU6050陀螺仪，加速度计数据获取

1.初始化mpu6050

uint8_t MPU_Init( void )
{

	uint8_t res;
	MPU_IIC_Init();
	/* MPU_PWR_MGMT1_REG：电源管理寄存器 */
	MPU_Write_Byte( MPU_PWR_MGMT1_REG, 0X80 );//复位MPU6050
    //	printf("读寄存器值：%02X\n",MPU_Read_Byte( MPU_PWR_MGMT1_REG ));
	delay_ms(100);
	MPU_Write_Byte( MPU_PWR_MGMT1_REG, 0X00 );//唤醒MPU6050
	
	/* 设置陀螺仪量程：±2000dps
	 设置加速度计量程：±2g
	     设置采样频率：20Hz（低通滤波器频率100Hz）   */
	MPU_Set_Gyro_Fsr( 3 );
	MPU_Set_Accel_Fsr( 0 );
	MPU_Set_Rate( 20 );
	
	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 ); //读取MPU6050ID
	printf("ID:%X\r\n",res);
	/* 确认ID */
	if( res==MPU_ADDR )
	{
		MPU_Write_Byte( MPU_PWR_MGMT1_REG, 0X01 );//以PLL X轴作为时钟参考
		MPU_Write_Byte( MPU_PWR_MGMT2_REG, 0X00 );//使能陀螺仪和加速度计
		MPU_Set_Rate( 50 );
		return 0;
	}
	else
		return 1;
	
}

2.读取数据

//获取陀螺仪数据
uint8_t MPU_Get_Gyroscope( short *gx, short *gy, short *gz )
{
	uint8_t buf[6],res;
	
	/* MPU_GYRO_XOUTH_REG：x轴高八位寄存器
    三个轴的寄存器地址是连续的，先高位，后低位	*/
	if( (res=MPU_Read_Continue( MPU_ADDR, MPU_GYRO_XOUTH_REG, 6, buf ))==0 )
	{
		*gx = ((uint16_t)buf[0]<<8)|buf[1];
		*gy = ((uint16_t)buf[2]<<8)|buf[3];
		*gz = ((uint16_t)buf[4]<<8)|buf[5];
	}
	
	return res;
}
//获取加速度计数据
uint8_t MPU_Get_Accelerometer( short *ax, short *ay, short *az )
{
	uint8_t buf[6],res;
	
	/* MPU_ACCEL_XOUTH_REG：x轴高八位寄存器
    三个轴的寄存器地址是连续的，先高位，后低位	*/
	if( (res=MPU_Read_Continue( MPU_ADDR, MPU_ACCEL_XOUTH_REG, 6, buf ))==0 )
	{
		*ax = ((uint16_t)buf[0]<<8)|buf[1];
		*ay = ((uint16_t)buf[2]<<8)|buf[3];
		*az = ((uint16_t)buf[4]<<8)|buf[5];
	}
	
	return res;
}

3.数据融合（卡尔曼）

1. 计算值 mpu6050对原始数据角度计算中关于16384 与 16.4两个参数来历

void IMU_GetData(void)
{
	MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz);
	MPU_Get_Accelerometer(&accx,&accy,&accz);
}

void IMU_Update(void)
{
	IMU_GetData();

	//accx,accy,accz,gx,gy,gz静止时输出分别为：384,-330,16474,-4,19,-1 ，计算时减去
	Accx = (float)(accx - 384 )/ 16384;  
	Accy = (float)(accy + 330 )/ 16384;
	Accz = (float)(accz - 16474)/16384;
	
	Gyrox= (float)(gyrox + 4)/16.4;
	Gyroy= (float)(gyroy - 19)/16.4;
	Gyroz= (float)(gyroz + 1)/16.4;
	//对加速度进行卡尔曼滤波
	Accx = KalmanFilter(0,Accx,0.0049,7);
	Accy = KalmanFilter(1,Accy,0.0049,7);
	Accz = KalmanFilter(2,Accz,0.0049,7);
	//对角速度进行卡尔曼滤波
	Gyrox=KalmanFilter(3,Gyrox,0.0049,7);
	Gyroy=KalmanFilter(4,Gyroy,0.0049,7);
	Gyroz=KalmanFilter(5,Gyroz,0.0049,7);
	//mpu6050_send_data(pitch,roll,0);
	//计算角度
	Angle_Calculate();
	//对结果卡尔曼滤波
	pitch = KalmanFilter(6,pitch,0.01,3);
	roll = KalmanFilter(7,roll,0.02,3);
	send_send_data(pitch-offset_pitch,roll-offset_roll,0,0,0,0);
	//0ffset    pitch     roll
	
} 

计算角度 Angle_Calculate(),参考一下代码自己做修改

//******角度参数************
float Gyro_y;        //Y轴陀螺仪数据暂存
float Angle_gy;      //由角速度计算的倾斜角度
float Accel_x;	     //X轴加速度值暂存
float Angle_ax;      //由加速度计算的倾斜角度
float Angle;         //最终测量角度

/*************卡尔曼滤波*********************************/
void Kalman_Filter(float Accel,float Gyro)		
{
		
	static const float Q_angle=0.001;  
	static const float Q_gyro=0.003;
	static const float R_angle=0.5;
	static const float dt=0.01;	                  //dt为kalman滤波器采样时间;
	static const char  C_0 = 1;
	static float Q_bias, Angle_err;
	static float PCt_0, PCt_1, E;
	static float K_0, K_1, t_0, t_1;
	static float Pdot[4] ={0,0,0,0};
	static float PP[2][2] = { { 1, 0 },{ 0, 1 } };
	
	Angle+=(Gyro - Q_bias) * dt; //先验估计

	Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; // Pk-先验估计误差协方差的微分

	Pdot[1]= -PP[1][1];
	Pdot[2]= -PP[1][1];
	Pdot[3]=Q_gyro;
	
	PP[0][0] += Pdot[0] * dt;   // Pk-先验估计误差协方差微分的积分
	PP[0][1] += Pdot[1] * dt;   // =先验估计误差协方差
	PP[1][0] += Pdot[2] * dt;
	PP[1][1] += Pdot[3] * dt;
		
	Angle_err = Accel - Angle;	//zk-先验估计
	
	PCt_0 = C_0 * PP[0][0];
	PCt_1 = C_0 * PP[1][0];
	
	E = R_angle + C_0 * PCt_0;
	
	K_0 = PCt_0 / E;
	K_1 = PCt_1 / E;
	
	t_0 = PCt_0;
	t_1 = C_0 * PP[0][1];

	PP[0][0] -= K_0 * t_0;		 //后验估计误差协方差
	PP[0][1] -= K_0 * t_1;
	PP[1][0] -= K_1 * t_0;
	PP[1][1] -= K_1 * t_1;
		
	Angle	+= K_0 * Angle_err;	 //后验估计
	Q_bias	+= K_1 * Angle_err;	 //后验估计
	Gyro_y   = Gyro - Q_bias;	 //输出值(后验估计)的微分=角速度
}
void Angle_Calculate(void)
{
	static	uint8_t DataBuffer[2];	//数据缓存
/****************************加速度****************************************/
	
	Accel_x  = accX;	  //读取X轴加速度
	Angle_ax = (Accel_x +220) /16384;   //去除零点偏移,计算得到角度（弧度）
	Angle_ax = Angle_ax*1.2*180/3.14;     //弧度转换为度,

/****************************角速度****************************************/
	Gyro_y =  gyroY;	      //静止时角速度Y轴输出为-18左右
	Gyro_y = (Gyro_y + 18)/16.4;         //去除零点偏移，计算角速度值 
//		Angle_gy = Angle_gy + Gyro_y*0.01;  //角速度积分得到倾斜角度.	

/***************************卡尔曼滤波+角度融合*************************************/
	Kalman_Filter(Angle_ax,Gyro_y);       //卡尔曼滤波计算倾角

/*******************************互补滤波******************************************/

/*补偿原理是取当前倾角和加速度获
	得倾角差值进行放大，然后与陀螺
	仪角速度叠加后再积分，从而使倾
	角最跟踪为加速度获得的角度0.5
	为放大倍数，可调节补偿度;
	0.01为系统周期10ms	
*/	
//	Angle = Angle + (((Angle_ax-Angle)*0.5 + Gyro_y)*0.01);*/
															  		
}

旋转角度定义：pitch 为绕x轴旋转，roll为绕y轴旋转

4.卡尔曼滤波

上述3对结果进行卡尔曼滤波参考一下一维滤波代码修改
参考卡尔曼滤波算法及C语言实现_源代码

/*-------------------------------------------------------------------------------------------------------------*/
/*        
        Q:过程噪声，Q增大，动态响应变快，收敛稳定性变坏
        R:测量噪声，R增大，动态响应变慢，收敛稳定性变好        
*/

static        double x_last[9]={0};
static        double p_last[9]={0};
double KalmanFilter(int chnel,const double ResrcData,double ProcessNiose_Q,double MeasureNoise_R)
{
        double R = MeasureNoise_R;
        double Q = ProcessNiose_Q;

        

        double x_mid = x_last[chnel];
        double x_now;

        

        double p_mid ;
        double p_now;
        double kg;        

        x_mid=x_last[chnel]; //x_last[chnel]=x(k-1|k-1),x_mid=x(k|k-1)
        p_mid=p_last[chnel]+Q; //p_mid=p(k|k-1),p_last[chnel]=p(k-1|k-1),Q=噪声
        kg=p_mid/(p_mid+R); //kg为kalman filter，R为噪声
        x_now=x_mid+kg*(ResrcData-x_mid);//估计出的最优值
                
        p_now=(1-kg)*p_mid;//最优值对应的covariance        

        p_last[chnel] = p_now; //更新covariance值
        x_last[chnel] = x_now; //更新系统状态值

        return x_now;                
}

/*-------------------------------------------------------------------------------------------------------------*/

结果截图

过程比较复杂但还是有效，最终结果在电机不动时在0.1度以内，在电机转动时在0.3度（看图估计的）以内

1.实测效果图（已减去偏移量）

1，电机不动（注意左侧刻度）series2为最终pitch series4为最终roll

2,电机转动，在3中对结果滤波前和滤波后对比
pitch:
series1 : 滤波前
series2 : 滤波后

roll:
series3 : 滤波前
series4 : 滤波后

实测显示时延较大，动态响应比不滤波前延后10个数据点，采样一次时10ms，即滞后了100ms，可能是我滤波参数没调好，在卡尔曼滤波的收敛性和响应性之间没有平衡好。
【尬了】
刚测了一下，发现最原始数据和最终数据的延时特别大，看来还得再调调！！！
series1为Gyrox
series2为pitch

接着开始Pid

---