當前位置：首頁 >

MPU6050开发 -- 卡尔曼滤波

發布時間：2025/3/15 44 豆豆

生活随笔收集整理的這篇文章主要介紹了 MPU6050开发 -- 卡尔曼滤波小編覺得挺不錯的,現在分享給大家,幫大家做個參考.

如需轉載請注明出處：https://blog.csdn.net/qq_29350001/article/details/78687974

上一篇文章有講到卡爾曼濾波了，現在需要將其添加到我們之前的C52測試程序中。

STM32 相關工程，下載：STM32F10x 卡爾曼濾波

一、再看一下卡爾曼濾波程序

#include<math.h> #include "stm32f10x.h" #include "Kalman_Filter.h" //卡爾曼濾波參數與函數 float dt=0.001;//注意：dt的取值為kalman濾波器采樣時間 float angle, angle_dot;//角度和角速度 float P[2][2] = {{ 1, 0 }, { 0, 1 }}; float Pdot[4] ={ 0,0,0,0}; float Q_angle=0.001, Q_gyro=0.005; //角度數據置信度,角速度數據置信度 float R_angle=0.5 ,C_0 = 1; float q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1; //卡爾曼濾波 float Kalman_Filter(float angle_m, float gyro_m)//angleAx 和 gyroGy { angle+=(gyro_m-q_bias) * dt; angle_err = angle_m - angle; Pdot[0]=Q_angle - P[0][1] - P[1][0]; Pdot[1]=- P[1][1]; Pdot[2]=- P[1][1]; Pdot[3]=Q_gyro; P[0][0] += Pdot[0] * dt; P[0][1] += Pdot[1] * dt; P[1][0] += Pdot[2] * dt; P[1][1] += Pdot[3] * dt; PCt_0 = C_0 * P[0][0]; PCt_1 = C_0 * P[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 * P[0][1]; P[0][0] -= K_0 * t_0; P[0][1] -= K_0 * t_1; P[1][0] -= K_1 * t_0; P[1][1] -= K_1 * t_1; angle += K_0 * angle_err; //最優角度 q_bias += K_1 * angle_err; angle_dot = gyro_m-q_bias;//最優角速度 return angle； }

或者：

#include "Wire.h" #include "I2Cdev.h" #include "MPU6050.h" #include "Timer.h"//時間操作系統頭文件本程序用作timeChange時間采集并處理一次數據 Timer t;//時間類 float timeChange=20;//濾波法采樣時間間隔毫秒 float dt=timeChange*0.001;//注意：dt的取值為濾波器采樣時間 // 陀螺儀 float angleAx,gyroGy;//計算后的角度（與x軸夾角）和角速度 MPU6050 accelgyro;//陀螺儀類 int16_t ax, ay, az, gx, gy, gz;//陀螺儀原始數據 3個加速度+3個角速度 //一階濾波 float K1 =0.05; // 對加速度計取值的權重 //float dt=20*0.001;//注意：dt的取值為濾波器采樣時間 float angle1;//一階濾波角度輸出 //二階濾波 float K2 =0.2; // 對加速度計取值的權重 float x1,x2,y1;//運算中間變量 //float dt=20*0.001;//注意：dt的取值為濾波器采樣時間 float angle2;//er階濾波角度輸出 //卡爾曼濾波參數與函數 float angle, angle_dot;//角度和角速度 float angle_0, angle_dot_0;//采集來的角度和角速度 //float dt=20*0.001;//注意：dt的取值為kalman濾波器采樣時間 //一下為運算中間變量 float P[2][2] = {{ 1, 0 }, { 0, 1 }}; float Pdot[4] ={ 0,0,0,0}; float Q_angle=0.001, Q_gyro=0.005; //角度數據置信度,角速度數據置信度 float R_angle=0.5 ,C_0 = 1; float q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1; void setup() { Wire.begin();//初始化 Serial.begin(9600);//初始化 accelgyro.initialize();//初始化 int tickEvent1=t.every(timeChange, getangle);//本語句執行以后timeChange毫秒執行回調函數getangle int tickEvent2=t.every(50, printout) ;//本語句執行以后50毫秒執行回調函數printout，串口輸出 } void loop() { t.update();//時間操作系統運行 } void printout() { Serial.print(angleAx);Serial.print(','); Serial.print(angle1);Serial.print(','); Serial.print(angle2);Serial.print(','); // Serial.print(gx/131.00);Serial.print(','); Serial.println(angle);//Serial.print(','); // Serial.println(Output); } void getangle() { accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);//讀取原始6個數據 angleAx=atan2(ax,az)*180/PI;//計算與x軸夾角 gyroGy=-gy/131.00;//計算角速度 Yijielvbo(angleAx,gyroGy);//一階濾波 Erjielvbo(angleAx,gyroGy);//二階濾波 Kalman_Filter(angleAx,gyroGy); //卡爾曼濾波 } void Yijielvbo(float angle_m, float gyro_m) { angle1 = K1 * angle_m+ (1-K1) * (angle1 + gyro_m * dt); } void Erjielvbo(float angle_m,float gyro_m) { x1=(angle_m-angle2)*(1-K2)*(1-K2); y1=y1+x1*dt; x2=y1+2*(1-K2)*(angle_m-angle2)+gyro_m; angle2=angle2+ x2*dt; } void Kalman_Filter(double angle_m,double gyro_m) { angle+=(gyro_m-q_bias) * dt; angle_err = angle_m - angle; Pdot[0]=Q_angle - P[0][1] - P[1][0]; Pdot[1]=- P[1][1]; Pdot[2]=- P[1][1]; Pdot[3]=Q_gyro; P[0][0] += Pdot[0] * dt; P[0][1] += Pdot[1] * dt; P[1][0] += Pdot[2] * dt; P[1][1] += Pdot[3] * dt; PCt_0 = C_0 * P[0][0]; PCt_1 = C_0 * P[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 * P[0][1]; P[0][0] -= K_0 * t_0; P[0][1] -= K_0 * t_1; P[1][0] -= K_1 * t_0; P[1][1] -= K_1 * t_1; angle += K_0 * angle_err; //最優角度 q_bias += K_1 * angle_err; angle_dot = gyro_m-q_bias;//最優角速度 }

二、解析

卡爾曼濾波函數有兩個參數，它的實參定義為?angleAx，gyroGy;//計算后的角度（與x軸夾角）和角速度?

那么如何計算angleAx，gyroGy？

（1）angleAx 計算

angleAx=atan2(ax,az)*180/PI; //計算與x軸夾角 ??PI 為 3.14

ax、az是什么？

MPU6050初始化設置范圍為2g時，靈敏度 16384 LSB/g

ax =?ACCEL_XOUT_H /?16384

az =?ACCEL_ZOUT_H /?16384

因此可以這樣寫：

angleAx=atan2(（ACCEL_XOUT_H /?16384）,（ACCEL_ZOUT_H /?16384）)*180/3.14;?

（2）gyroGy 計算

gyroGy=-gy/131.00; //計算角速度 ?

注意，131.00 是當陀螺儀量程為± 250 °/s時的靈敏度 131?LSB/°/s。

MPU6050初始化設置范圍為± 2000 °/s時，靈敏度為 16.4?LSB/°/s。

gy是什么？

gy =?GYRO_YOUT_H

因此可以這樣寫：

gyroGy=-GYRO_YOUT_H/16.4;

（3）卡爾曼濾波函數

而這樣的一個 Kalman_Filter(angleAx, gyroGy); ? 卡爾曼濾波，每次卡只能得到一個方向的角度。

如此說來，我們再獲取其他兩個角度即可。

具體上面這三個與姿態角（Euler角）yaw pitch roll 對應關系，我還沒搞清楚...

但大體上卡爾曼濾波和C52測試程序就是這樣子結合的。只待進一步驗證了...

三、編寫程序

參看：MPU6050 卡爾曼濾波

//**************************************** // Update to MPU6050 by shinetop // MCU: STC89C52 // 2012.3.1 // 功能: 顯示加速度計和陀螺儀的10位原始數據 //**************************************** // 使用單片機STC89C52 // 晶振：11.0592M // 顯示：串口 // 編譯環境 Keil uVision2 //**************************************** #include <REG52.H> #include <math.h> //Keil library #include <stdio.h> //Keil library #include <INTRINS.H> typedef unsigned char uchar; typedef unsigned short ushort; typedef unsigned int uint; //**************************************** // 定義51單片機端口 //**************************************** sbit SCL=P1^5; //IIC時鐘引腳定義 sbit SDA=P1^4; //IIC數據引腳定義 //**************************************** // 定義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 TEMP_OUT_H 0x41 #define TEMP_OUT_L 0x42 #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為讀取 //************************************************************************************************** //定義類型及變量 //************************************************************************************************** uchar dis[6]; //顯示數字(-511至512)的字符數組 int dis_data; //變量 //******角度參數************ float Gyro_y; //Y軸陀螺儀數據暫存 float Angle_gy; //由角速度計算的傾斜角度 float Accel_x; //X軸加速度值暫存 float Angle_ax; //由加速度計算的傾斜角度 float Angle; //小車最終傾斜角度 uchar value; //角度正負極性標記 //************************************************************************************************** //函數聲明 //************************************************************************************************** void Delay5us(); void delay(unsigned int k); //延時 void lcd_printf(uchar *s,int temp_data); //********************************MPU6050操作函數*************************************************** void InitMPU6050(); //初始化MPU6050 void I2C_Start(); void I2C_Stop(); void I2C_SendACK(bit ack); bit I2C_RecvACK(); void I2C_SendByte(uchar dat); uchar I2C_RecvByte(); void I2C_ReadPage(); void I2C_WritePage(); void display_ACCEL_x(); void display_ACCEL_y(); void display_ACCEL_z(); uchar Single_ReadI2C(uchar REG_Address); //讀取I2C數據 void Single_WriteI2C(uchar REG_Address,uchar REG_data); //向I2C寫入數據 //******************************************************************************** //整數轉字符串 //******************************************************************************** void lcd_printf(uchar *s,int temp_data) { if(temp_data<0) { temp_data=-temp_data; *s='-'; } else *s=' '; *++s =temp_data/10000+0x30; temp_data=temp_data%10000; //取余運算 *++s =temp_data/1000+0x30; temp_data=temp_data%1000; //取余運算 *++s =temp_data/100+0x30; temp_data=temp_data%100; //取余運算 *++s =temp_data/10+0x30; temp_data=temp_data%10; //取余運算 *++s =temp_data+0x30; } //****************************************************************************************************** //串口初始化 //******************************************************************************************************* void init_uart() { TMOD=0x21; TH1=0xfd; //實現波特率9600（系統時鐘11.0592MHZ） TL1=0xfd; SCON=0x50; PS=1; //串口中斷設為高優先級別 TR0=1; //啟動定時器 TR1=1; ET0=1; //打開定時器0中斷 ES=1; EA=1; } //************************************************************************************************* //串口發送函數 //************************************************************************************************* void SeriPushSend(uchar send_data) { SBUF=send_data; while(!TI);TI=0; } //************************************************************************************************* //************************************延時********************************************************* //************************************************************************************************* void delay(unsigned int k) { unsigned int i,j; for(i=0;i<k;i++) { for(j=0;j<121;j++); } } //************************************************************************************************ //延時5微秒(STC90C52RC@12M) //不同的工作環境,需要調整此函數 //注意當改用1T的MCU時,請調整此延時函數 //************************************************************************************************ void Delay5us() { _nop_();_nop_();_nop_();_nop_(); _nop_();_nop_();_nop_();_nop_(); _nop_();_nop_();_nop_();_nop_(); _nop_();_nop_();_nop_();_nop_(); _nop_();_nop_();_nop_();_nop_(); _nop_();_nop_();_nop_();_nop_(); } //************************************************************************************************* //I2C起始信號 //************************************************************************************************* void I2C_Start() { SDA = 1; //拉高數據線 SCL = 1; //拉高時鐘線 Delay5us(); //延時 SDA = 0; //產生下降沿 Delay5us(); //延時 SCL = 0; //拉低時鐘線 } //************************************************************************************************* //I2C停止信號 //************************************************************************************************* void I2C_Stop() { SDA = 0; //拉低數據線 SCL = 1; //拉高時鐘線 Delay5us(); //延時 SDA = 1; //產生上升沿 Delay5us(); //延時 } //************************************************************************************************** //I2C發送應答信號 //入口參數:ack (0:ACK 1:NAK) //************************************************************************************************** void I2C_SendACK(bit ack) { SDA = ack; //寫應答信號 SCL = 1; //拉高時鐘線 Delay5us(); //延時 SCL = 0; //拉低時鐘線 Delay5us(); //延時 } //**************************************************************************************************** //I2C接收應答信號 //**************************************************************************************************** bit I2C_RecvACK() { SCL = 1; //拉高時鐘線 Delay5us(); //延時 CY = SDA; //讀應答信號 SCL = 0; //拉低時鐘線 Delay5us(); //延時 return CY; } //***************************************************************************************************** //向I2C總線發送一個字節數據 //***************************************************************************************************** void I2C_SendByte(uchar dat) { uchar i; for (i=0; i<8; i++) //8位計數器 { dat <<= 1; //移出數據的最高位 SDA = CY; //送數據口 SCL = 1; //拉高時鐘線 Delay5us(); //延時 SCL = 0; //拉低時鐘線 Delay5us(); //延時 } I2C_RecvACK(); } //***************************************************************************************************** //從I2C總線接收一個字節數據 //****************************************************************************************************** uchar I2C_RecvByte() { uchar i; uchar dat = 0; SDA = 1; //使能內部上拉,準備讀取數據, for (i=0; i<8; i++) //8位計數器 { dat <<= 1; SCL = 1; //拉高時鐘線 Delay5us(); //延時 dat |= SDA; //讀數據 SCL = 0; //拉低時鐘線 Delay5us(); //延時 } return dat; } //***************************************************************************************************** //向I2C設備寫入一個字節數據 //***************************************************************************************************** void Single_WriteI2C(uchar REG_Address,uchar REG_data) { I2C_Start(); //起始信號 I2C_SendByte(SlaveAddress); //發送設備地址+寫信號 I2C_SendByte(REG_Address); //內部寄存器地址， I2C_SendByte(REG_data); //內部寄存器數據， I2C_Stop(); //發送停止信號 } //******************************************************************************************************* //從I2C設備讀取一個字節數據 //******************************************************************************************************* uchar Single_ReadI2C(uchar REG_Address) { uchar REG_data; I2C_Start(); //起始信號 I2C_SendByte(SlaveAddress); //發送設備地址+寫信號 I2C_SendByte(REG_Address); //發送存儲單元地址，從0開始 I2C_Start(); //起始信號 I2C_SendByte(SlaveAddress+1); //發送設備地址+讀信號 REG_data=I2C_RecvByte(); //讀出寄存器數據 I2C_SendACK(1); //接收應答信號 I2C_Stop(); //停止信號 return REG_data; } //****************************************************************************************************** //初始化MPU6050 //****************************************************************************************************** void InitMPU6050() { Single_WriteI2C(PWR_MGMT_1, 0x00); //解除休眠狀態 Single_WriteI2C(SMPLRT_DIV, 0x07); Single_WriteI2C(CONFIG, 0x06); Single_WriteI2C(GYRO_CONFIG, 0x18); Single_WriteI2C(ACCEL_CONFIG, 0x01); } //****************************************************************************************************** //合成數據 //****************************************************************************************************** int GetData(uchar REG_Address) { uchar H,L; H=Single_ReadI2C(REG_Address); L=Single_ReadI2C(REG_Address+1); return ((H<<8)+L); //合成數據 } //****************************************************************************************************** //超級終端（串口調試助手）上顯示10位數據 //****************************************************************************************************** void Display10BitData(int value) { uchar i; // value/=64; //轉換為10位數據 lcd_printf(dis, value); //轉換數據顯示 for(i=0;i<6;i++) { SeriPushSend(dis[i]); } // DisplayListChar(x,y,dis,4); //啟始列，行，顯示數組，顯示長度 } //********************************************************* // 卡爾曼濾波 //********************************************************* //Kalman濾波，20MHz的處理時間約0.77ms； float 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; //輸出值(后驗估計)的微分=角速度 return Gyro_y; } //********************************************************* // 傾角計算（卡爾曼融合） //********************************************************* void Angle_Calcu(void) { //------加速度-------------------------- //范圍為2g時，換算關系：16384 LSB/g //角度較小時，x=sinx得到角度（弧度）, deg = rad*180/3.14 //因為x>=sinx,故乘以1.3適當放大 Accel_x = GetData(ACCEL_XOUT_H); //讀取X軸加速度 Angle_ax = (Accel_x - 1100) /16384; //去除零點偏移,計算得到角度（弧度） Angle_ax = Angle_ax*1.4*180/3.14; //弧度轉換為度, //Display10BitData(Angle_ax,2,1); //-------角速度------------------------- //范圍為2000deg/s時，換算關系：16.4 LSB/(deg/s) Gyro_y = GetData(GYRO_YOUT_H); //靜止時角速度Y軸輸出為-30左右 Gyro_y = -(Gyro_y + 30)/16.4; //去除零點偏移，計算角速度值,負號為方向處理 //Angle_gy = Angle_gy + Gyro_y*0.01; //角速度積分得到傾斜角度. //Display10BitData(Gyro_y,8,1); //-------卡爾曼濾波融合----------------------- //Kalman_Filter(Angle_ax,Gyro_y); //卡爾曼濾波計算傾角 Display10BitData(Kalman_Filter(Angle_ax,Gyro_y)); /*//-------互補濾波----------------------- //補償原理是取當前傾角和加速度獲得傾角差值進行放大，然后與 //陀螺儀角速度疊加后再積分，從而使傾角最跟蹤為加速度獲得的角度 //0.5為放大倍數，可調節補償度；0.01為系統周期10ms Angle = Angle + (((Angle_ax-Angle)*0.5 + Gyro_y)*0.01);*/ } //******************************************************************************************************* //主程序 //******************************************************************************************************* void main() { delay(500); //上電延時 init_uart(); InitMPU6050(); //初始化MPU6050 delay(150); while(1) { Angle_Calcu(); SeriPushSend(0x0d); SeriPushSend(0x0a);//換行，回車 delay(2000); } }

我覺的這次代碼問題不大了啊，為什么獲取的值為 -00001 還是不對。

繼續思考！！

四、可能出現錯誤：

Rebuild target 'Target 1' assembling STARTUP.A51... compiling MPU6050_C52.c... linking... *** WARNING L16: UNCALLED SEGMENT, IGNORED FOR OVERLAY PROCESSSEGMENT: ?PR?_DISPLAY10BITDATA?MPU6050_C52 *** WARNING L16: UNCALLED SEGMENT, IGNORED FOR OVERLAY PROCESSSEGMENT: ?PR?_KALMAN_FILTER?MPU6050_C52 *** WARNING L16: UNCALLED SEGMENT, IGNORED FOR OVERLAY PROCESSSEGMENT: ?PR?ANGLE_CALCULATE?MPU6050_C52 *** ERROR L107: ADDRESS SPACE OVERFLOWSPACE: DATA SEGMENT: ?DT?MPU6050_C52LENGTH: 0071H *** ERROR L105: PUBLIC REFERS TO IGNORED SEGMENTSYMBOL: GYRO_YSEGMENT: ?DT?MPU6050_C52 *** ERROR L105: PUBLIC REFERS TO IGNORED SEGMENTSYMBOL: ANGLESEGMENT: ?DT?MPU6050_C52 *** ERROR L105: PUBLIC REFERS TO IGNORED SEGMENTSYMBOL: DISSEGMENT: ?DT?MPU6050_C52 *** ERROR L105: PUBLIC REFERS TO IGNORED SEGMENTSYMBOL: DIS_DATASEGMENT: ?DT?MPU6050_C52 *** ERROR L105: PUBLIC REFERS TO IGNORED SEGMENTSYMBOL: ACCEL_XSEGMENT: ?DT?MPU6050_C52 *** ERROR L105: PUBLIC REFERS TO IGNORED SEGMENTSYMBOL: ANGLE_GYSEGMENT: ?DT?MPU6050_C52 *** ERROR L105: PUBLIC REFERS TO IGNORED SEGMENTSYMBOL: ANGLE_AXSEGMENT: ?DT?MPU6050_C52 Program Size: data=135.1 xdata=0 code=2873 Target not created

說明data空間已經不夠用，原因是你可能有好多函數，而函數內部的局部變量又沒有定義其空間。

這種情況下，系統會將變量分配到你在 Otions for Target 對話框里的設置的空間。

如果你在下圖所示中的 Memory Model 里設置成 Small:variables in DATA,則DATA空間很快便用完，導致data空間不夠用。

解決的辦法有兩種：

一是通過更改Memory Model設置，可以設置成pdata或xdata，以便有足夠大的空間。

但這又帶來新的問題，程序運行速度減慢，而且code代碼也會加大，因為如果一個局部變量被存放在了xdata空間，匯編語言訪問xdata空間的代碼大小要比訪問data空間的代碼大，變量一旦很多，程序的代碼也會逐漸增大；

二是根據自己的要求設置變量的空間。

所以這涉及到代碼優化的問題，遇到具體問題時，在運行速度和代碼大小之間取得適合自己的情況。