借助MPU6050上手卡尔曼滤波_mpu6050卡尔曼滤波

作者：你好赵伟 | 2024-04-24 13:01:02

踩

mpu6050卡尔曼滤波

一、MPU6050原理介绍

它是一个6轴姿态传感器，测量芯片X、Y、Z轴的角速度和加速度，通过数据融合进一步得到姿态角，其中数据融合可以用互补滤波或者卡尔曼滤波，它还内置了加速度计和陀螺仪

对于加速度计而言，它的测量原理可以这么理解：芯片内部有三个弹簧测力计，通过牛顿第二定律F = M * a，如果知道了三个轴的弹簧所受到的弹力F，可以预先使三个弹簧的质量为单位质量，那么就可以方便推出三个轴的加速度，再依次合成两个轴的加速度得到加速度的矢量三角形，从而推出相应的角度。当然要借助ADC转换，也就是每个弹簧测力计连接一个电位器，当弹簧测力计位置发生改变时，那么就会输出一个电压，根据这个电压来量化三个轴受到的力，从而得到三个轴所受到的加速度。但是需要注意的是，加速度计具有静态稳定、动态不稳定的特性，原因是如果弹簧测力计所受到的力不是来源于芯片静止状态下，由于自身偏置的角度和重力所带来的，而是由于芯片自身的加速运动带来的，就会导致解算出来的角度是错误的。比如在某一时刻，X轴和Z轴方向的弹簧测力计测出来了加速度，但是可能是因为芯片放置的角度是倾斜的并且此时是静止，也可能是因为它此时是水平放置的但是沿着X轴有一个加速度的运动状态

对于陀螺仪而言，MPU6050测出来的是角速度反而不是角度，并且也是借助ADC转换，通过电压来量化角速度。可以用角速度积分的方式得到角度，通过这种方式得到的角度不会因为芯片运动而产生很大的偏差，相反，如果芯片静止，理想角速度值是0dps，但是在外界噪音的干扰下，可能角速度值是0.1dps，积分一秒之后，得到的角度是0.1度，1分钟之后是6度，还能忍受，一小时之后是360度，转了一圈。这就是零点漂移现象，零点说的测量出来的角速度是理想值0dps附近跳变。也就是陀螺仪具有动态稳定、静态不稳定的特性

结合陀螺仪和加速度计的特性，可以发现它们是互相补充。加速度计得到的值，可以视为估计值或者观测值，而陀螺仪得到的值，可以视为测量值，那么就可以采用互补滤波的方式将估计值和测量值进行数据融合。

但是yaw轴方向的角度，在静态的时候无法用加速度计进行纠正，因为偏航角的平面与重力相互垂直，也就是说，重力在偏航角平面的投影为0。在俯仰角（Pitch）和翻滚角（Roll）不变的情况下，无论偏航角是多少，加速度计测出来的值是一样的，这就导致静态的时候Yaw轴方向的估计值或者观测值无法得到，也就无法去修正测量值了

二、MPU6050参数

ADC采集传感器的量化范围

陀螺仪和加速度计的满量程选择

可配置的数字低通滤波器

可配置的时钟和采样分频

从机地址的设置

MPU6050作为从机通信时的引脚

MPU6050作为主机通信时的引脚，一般用来进行拓展，比如直接访问外接的磁力计和气压计

中断输出引脚，即信号触发INT引脚产生电平跳变

数字运动处理器DMP，是芯片内部自带的一个姿态结算的硬件算法

三、姿态解算原理

这部分先不写，留到之后再补充笔记，目前先快进到卡尔曼滤波获取最优的角度估计值

学习资料参考如下：

https://zhuanlan.zhihu.com/p/195683958

https://mp.weixin.qq.com/mp/appmsgalbum?__biz=MzU2OTk0Njc0Mw==&action=getalbum&album_id=1324080014694072321&scene=173&from_msgid=2247484874&from_itemidx=1&count=3&nolastread=1#wechat_redirect

四、利用卡尔曼滤波进行姿态解算

参考：3、利用卡尔曼滤波进行姿态解算_哔哩哔哩_bilibili

下面的代码中的各种计算，是对公式简化后进行计算，并不能算是卡尔曼滤波的通用模板

有关公式如下：

代码如下（不能算作通用代码，是根据陀螺仪这个模型建立的，所以不具有通用性）

创建卡尔曼滤波器实体时，记得对函数指针赋值：


#define OBSERVE_STATE_NUM 2			 
#define dT                1.f 			//B矩阵用到，但是计算的时候dT会手动计算出来
#define Q_MATRIX_ELEMENT  0.0025f		
#define R_MATRIX_ELEMENT  0.3f
 
#define Kalman_Filter_DeInit 		Kal_Filter.Init_Flag == DeInit
 
 
typedef enum Filter_Init_e
{
	Init = 0,
	DeInit,
	
}Filter_Init_e;
		
 
typedef struct Measure_Data
{
	
	float pitch;
	float roll;
	
	float rate_pitch;
	float rate_roll;
		
}Measure_Data;
 
 
typedef struct Prior_Hat
{
	float pitch;
	float roll;
	
	float Prior_Hat_Err_Cov[OBSERVE_STATE_NUM][OBSERVE_STATE_NUM];
	
}Prior_Hat;
 
 
typedef struct System_Input
{
	
	float rate_pitch;
	float rate_roll;
	
}System_Input;
 
typedef struct Posterior_Hat
{
	float pitch;
	float roll;
	
	float Posterior_Hat_Err_Cov[OBSERVE_STATE_NUM][OBSERVE_STATE_NUM];
	
}Posterior_Hat;
 
 
typedef struct Matrix
{
	uint8_t State_Tran_A[OBSERVE_STATE_NUM][OBSERVE_STATE_NUM];
	
	uint8_t Ctrl_B[OBSERVE_STATE_NUM][OBSERVE_STATE_NUM];
	
	uint8_t Observed_H[OBSERVE_STATE_NUM][OBSERVE_STATE_NUM];
	
	uint8_t Unit_Matrix_I[OBSERVE_STATE_NUM][OBSERVE_STATE_NUM];
	
	float Process_Noise_Cov_Q[OBSERVE_STATE_NUM][OBSERVE_STATE_NUM];
 
	float Measure_Noise_Cov_R[OBSERVE_STATE_NUM][OBSERVE_STATE_NUM];
	
	float Kalman_Gain[OBSERVE_STATE_NUM][OBSERVE_STATE_NUM];
	
}Matrix;
 
 
typedef struct Time_Cnt
{
	uint32_t Time_Last;
	
	uint32_t Time_This;
	
	uint32_t Time_dT;
	
}Time_Cnt;
 
 
typedef struct Kal_Filter_Pass_Data
{
	float pitch;
	float roll;
	
}Kal_Filter_Pass_Data;
 
typedef struct Kalman_Filter
{
	Measure_Data   				Meas_Data; 
	Prior_Hat 					Pri_Hat;
	System_Input				Sys_Input;
	Posterior_Hat   			Post_Hat;
	Matrix						All_Matrix;
	Time_Cnt        			Time;
	Filter_Init_e   			Init_Flag; 
	Kal_Filter_Pass_Data        KalPass_Data;
	
	
	
	void				(*Set_Initial)( struct Kalman_Filter *Kal_Filter );
	void				(*Data_Update)( struct Kalman_Filter *Kal_Filter );
	void				(*Predict)( struct Kalman_Filter *Kal_Filter );
	void				(*Correct)( struct Kalman_Filter *Kal_Filter );
	void                (*KalPass_Data_Update)( struct Kalman_Filter *Kal_Filter );
	
}Kalman_Filter;
 
 
 
 
 
 
 
void Set_Matrix( Matrix *All_Matrix );	
void Set_Posterior_Hat_Initial( Posterior_Hat *Post_Hat );
void Set_Initial_Step( struct Kalman_Filter *Kal_Filter );
 
 
void Meas_Data_Updata( Measure_Data  *Meas_Data );
void Prior_Hat_Update( Kalman_Filter *Kal_Filter );
void Posterior_Hat_Update( Kalman_Filter *Kal_Filter );
void System_Input_Update( Kalman_Filter *Kal_Filter );
void Kalman_Gain_Update( Kalman_Filter *Kal_Filter );
void Data_Update_Step( struct Kalman_Filter *Kal_Filter );
 
 
void Predict_Step( struct Kalman_Filter *Kal_Filter );
 
 
void Correct_Step( struct Kalman_Filter *Kal_Filter );
 
 
void KalPass_Data_Update( Kalman_Filter *Kal_Filter );
 
 
 
 
Kalman_Filter Kal_Filter = 
{
	.Init_Flag   = DeInit, //Ò»¿ªÊ¼ÉèÖÃÎªÎ´³õÊ¼»¯
	
	
 
	.Set_Initial = Set_Initial_Step,
	
	.Data_Update = Data_Update_Step,
	
	.Predict     = Predict_Step,
	
	.Correct     = Correct_Step,
	
	.KalPass_Data_Update = KalPass_Data_Update,
};
 
 
 
 
 
 
 
//--------------------------初始条件设置开始--------------------------//
void Set_Matrix( Matrix *All_Matrix )
{
	//A矩阵
	All_Matrix->State_Tran_A[0][0] = 1;
	All_Matrix->State_Tran_A[0][1] = 0;
	All_Matrix->State_Tran_A[1][0] = 0;
	All_Matrix->State_Tran_A[1][1] = 1;
	
	//B矩阵
	All_Matrix->Ctrl_B[0][0] = dT;
	All_Matrix->Ctrl_B[0][1] = 0;
	All_Matrix->Ctrl_B[1][0] = 0;
	All_Matrix->Ctrl_B[1][1] = dT;
	
	//H矩阵
	All_Matrix->Observed_H[0][0] = 1;
	All_Matrix->Observed_H[0][1] = 0;
	All_Matrix->Observed_H[1][0] = 0;
	All_Matrix->Observed_H[1][1] = 1;
	
	//单位阵I
	All_Matrix->Unit_Matrix_I[0][0] = 1;
	All_Matrix->Unit_Matrix_I[0][1] = 0;
	All_Matrix->Unit_Matrix_I[1][0] = 0;
	All_Matrix->Unit_Matrix_I[1][1] = 1;
	
	//过程噪音的协方差矩阵Q
	All_Matrix->Process_Noise_Cov_Q[0][0] = Q_MATRIX_ELEMENT;
	All_Matrix->Process_Noise_Cov_Q[0][1] = 0;
	All_Matrix->Process_Noise_Cov_Q[1][0] = 0;
	All_Matrix->Process_Noise_Cov_Q[1][1] = Q_MATRIX_ELEMENT;
	
	//测量噪音的协方差矩阵R
	All_Matrix->Measure_Noise_Cov_R[0][0] = R_MATRIX_ELEMENT;
	All_Matrix->Measure_Noise_Cov_R[0][1] = 0;
	All_Matrix->Measure_Noise_Cov_R[1][0] = 0;
	All_Matrix->Measure_Noise_Cov_R[1][1] = R_MATRIX_ELEMENT;
}
 
void Set_Posterior_Hat_Initial( Posterior_Hat *Post_Hat )
{
	Post_Hat->pitch = 0.f;
	Post_Hat->roll  = 0.f;
	
	Post_Hat->Posterior_Hat_Err_Cov[0][0] = 1.f;
	Post_Hat->Posterior_Hat_Err_Cov[0][1] = 0.f;
	Post_Hat->Posterior_Hat_Err_Cov[1][0] = 0.f;
	Post_Hat->Posterior_Hat_Err_Cov[1][1] = 1.f;
}
 
void Set_Initial_Step( Kalman_Filter *Kal_Filter )
{	
	Set_Matrix( &( Kal_Filter->All_Matrix ) );
	
	Set_Posterior_Hat_Initial( &( Kal_Filter->Post_Hat ) );
}
 
//----------------------初始条件设置结束----------------------------------------//
 
 
 
 
 
 
 
 
//----------------------陀螺仪数据更新开始---------------------------//
void Meas_Data_Updata( Measure_Data  *Meas_Data )
{
	
}
void System_Input_Update( Kalman_Filter *Kal_Filter )
{
	Imu_Meas_Data   *Meas_Data  = &( Kal_Filter->Meas_Data );
	System_Input	*Sys_Input	= &( Kal_Filter->Sys_Input );
	
	Sys_Input->rate_roll  = Meas_Data->rate_roll;
	
	Sys_Input->rate_pitch = Meas_Data->rate_pitch;
}
 
void Data_Update_Step( Kalman_Filter *Kal_Filter )
{
	Imu_Data_Updata( &( Kal_Filter->Meas_Data ) );
	
	System_Input_Update( Kal_Filter );
 
}
//----------------------陀螺仪数据更新结束----------------------------//
 
 
 
 
 
 
 
 
//-------------------------------预测工作开始-----------------------------------//
void Prior_Hat_Update( Kalman_Filter *Kal_Filter )
{
	Prior_Hat 			*Pri_Hat	= &( Kal_Filter->Pri_Hat );
	System_Input		*Sys_Input	= &( Kal_Filter->Sys_Input );
	Posterior_Hat       *Post_Hat  	= &( Kal_Filter->Post_Hat );
	Matrix				*All_Matrix = &( Kal_Filter->All_Matrix );
	Time_Cnt            *Time       = &( Kal_Filter->Time );
	
 
//这一次的先验估计值和先验误差的协方差矩阵都会用到上一次的后验估计值和后验误差的协方差	
	
	
	//获取时间间隔
	Time->Time_This = HAL_GetTick();
	
	Time->Time_dT = Time->Time_This - Time->Time_Last;
	
	Time->Time_Last = Time->Time_This;
 
	
    //先验估计值roll
	Pri_Hat->roll  = Post_Hat->roll + Time->Time_dT * Sys_Input->rate_roll * 0.001f;
	
	//先验估计值pitch
	Pri_Hat->pitch = Post_Hat->pitch + Time->Time_dT * Sys_Input->rate_pitch * 0.001f;
	
	//先验误差的协方差
	Pri_Hat->Prior_Hat_Err_Cov[0][0] = Post_Hat->Posterior_Hat_Err_Cov[0][0] + All_Matrix->Process_Noise_Cov_Q[0][0];
	
	Pri_Hat->Prior_Hat_Err_Cov[1][1] = Post_Hat->Posterior_Hat_Err_Cov[1][1] + All_Matrix->Process_Noise_Cov_Q[1][1];
	
}
 
void Predict_Step( Kalman_Filter *Kal_Filter )
{
	Prior_Hat_Update( Kal_Filter );
 
}
 
//--------------------------------预测工作结束--------------------------------------//
 
 
 
 
 
 
 
 
 
 
//--------------------------------校正工作开始--------------------------------------//
void Kalman_Gain_Update( Kalman_Filter *Kal_Filter )
{
	Prior_Hat 			*Pri_Hat	= &( Kal_Filter->Pri_Hat );
	Matrix				*All_Matrix = &( Kal_Filter->All_Matrix );
	
	All_Matrix->Kalman_Gain[0][0] = ( Pri_Hat->Prior_Hat_Err_Cov[0][0] + 
                                      All_Matrix->Measure_Noise_Cov_R[0][0] ) *
									  Pri_Hat->Prior_Hat_Err_Cov[0][0];
	
	All_Matrix->Kalman_Gain[1][1] = ( Pri_Hat->Prior_Hat_Err_Cov[1][1] + 
                                      All_Matrix->Measure_Noise_Cov_R[1][1] ) *
									  Pri_Hat->Prior_Hat_Err_Cov[1][1];
	
}
void Posterior_Hat_Update( Kalman_Filter *Kal_Filter )
{
	Prior_Hat 			*Pri_Hat	= &( Kal_Filter->Pri_Hat );
	Imu_Meas_Data       *Meas_Data  = &( Kal_Filter->Meas_Data );
	Posterior_Hat       *Post_Hat  	= &( Kal_Filter->Post_Hat );
	Matrix				*All_Matrix = &( Kal_Filter->All_Matrix );
	
//这一次的后验估计值和后验误差的协方差会用到这一次的先验估计值和先验误差的协方差
	
	//后验估计值roll
	Post_Hat->roll = Pri_Hat->roll + 
					 All_Matrix->Kalman_Gain[0][0] * ( Meas_Data->roll - Pri_Hat->roll );
	
	//后验估计值pitch
	Post_Hat->pitch = Pri_Hat->pitch + 
				      All_Matrix->Kalman_Gain[1][1]*(Meas_Data->pitch - Pri_Hat->pitch);
	
	//后验误差的协方差矩阵
	Post_Hat->Posterior_Hat_Err_Cov[0][0] = ( 1 - All_Matrix->Kalman_Gain[0][0] ) *                 
                                              Pri_Hat->Prior_Hat_Err_Cov[0][0];
				
	Post_Hat->Posterior_Hat_Err_Cov[1][1] = ( 1 - All_Matrix->Kalman_Gain[1][1] ) * 
                                              Pri_Hat->Prior_Hat_Err_Cov[1][1];
		
}
 
 
 
 
 
 
 
 
void Correct_Step( Kalman_Filter *Kal_Filter )
{
	Kalman_Gain_Update( Kal_Filter );
	
	Posterior_Hat_Update( Kal_Filter );
}
 
//--------------------------------修正工作结束------------------------------------//
 
 
 
void Start_Kalman_Filter(void)
{
	
	if( Kalman_Filter_DeInit )
		Kal_Filter.Set_Initial( &Kal_Filter );
	
 
	Kal_Filter.Data_Update( &Kal_Filter );
	
	
	Kal_Filter.Predict( &Kal_Filter );
	
	
	Kal_Filter.Correct( &Kal_Filter );
	
	
	Kal_Filter.KalPass_Data_Update( &Kal_Filter );
	
}

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/你好赵伟/article/detail/479396