本帖最后由 275891381 于 2014-6-23 14:21 編輯 1. MPU6050陀螺儀 // 陀螺儀 float angleAx,gyroGy; MPU6050 accelgyro; int16_t ax, ay, az, gx, gy, gz; accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);//原始數(shù)據(jù)采集 angleAx=atan2(ax,az)*180/PI;//加速度計算角度 gyroGy=-gy/131.00;//陀螺儀角速度,,注意正負號與放置有關(guān) 2.濾波參數(shù)及函數(shù) //一階互補濾波 float K1 =0.05; // 對加速度計取值的權(quán)重 float dt=20*0.001;//注意:dt的取值為濾波器采樣時間 float angle1; void Yijielvbo(float angle_m, float gyro_m)//采集后計算的角度和角加速度 { angle1 = K1 * angle_m+ (1-K1) * (angle1 + gyro_m * dt); } //二階互補濾波 float K2 =0.2; // 對加速度計取值的權(quán)重 float x1,x2,y1; float dt=20*0.001;//注意:dt的取值為濾波器采樣時間 float angle2; 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; } //卡爾曼濾波參數(shù)與函數(shù) float dt=0.005;//注意: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; //角度數(shù)據(jù)置信度,角速度數(shù)據(jù)置信度 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 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; //最優(yōu)角度 q_bias += K_1 * angle_err; angle_dot = gyro_m-q_bias;//最優(yōu)角速度 } 3 三種濾波比較源代碼 // #include "Wire.h" #include "I2Cdev.h" #include "MPU6050.h" #include "Timer.h"http://時間操作系統(tǒng)頭文件 本程序用作timeChange時間采集并處理一次數(shù)據(jù) 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;//陀螺儀原始數(shù)據(jù) 3個加速度+3個角速度 //一階濾波 float K1 =0.05; // 對加速度計取值的權(quán)重 //float dt=20*0.001;//注意:dt的取值為濾波器采樣時間 float angle1;//一階濾波角度輸出 //二階濾波 float K2 =0.2; // 對加速度計取值的權(quán)重 float x1,x2,y1;//運算中間變量 //float dt=20*0.001;//注意:dt的取值為濾波器采樣時間 float angle2;//er階濾波角度輸出 //卡爾曼濾波參數(shù)與函數(shù) 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; //角度數(shù)據(jù)置信度,角速度數(shù)據(jù)置信度 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);//本語句執(zhí)行以后timeChange毫秒執(zhí)行回調(diào)函數(shù)getangle int tickEvent2=t.every(50, printout) ;//本語句執(zhí)行以后50毫秒執(zhí)行回調(diào)函數(shù)printout,串口輸出 } void loop() { t.update();//時間操作系統(tǒng)運行 } 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個數(shù)據(jù) 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; //最優(yōu)角度 q_bias += K_1 * angle_err; angle_dot = gyro_m-q_bias;//最優(yōu)角速度 } 4 數(shù)據(jù)繪圖 5.附件 mp6050+時間輪換頭文件+數(shù)據(jù)繪圖 6.部分使用幫助 mpu6050關(guān)于16384.0 131.0 兩個參數(shù)來歷 accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);//讀取原始6個數(shù)據(jù) 使勁晃陀螺儀看看最大數(shù)據(jù)時多大 就知道該除以多少換算成g和度/秒 我的為 +—32768 +—250所以判定 頭文件初始化的為最大小量程 +—2g +—250 ax/16384.0=x方向的加速度 單位g ,,加速度計算角度用上面的方法 gx/131.0=x軸向角速度 單位 度/秒 來歷: 截圖出來 6.定時器pid//非全代碼 #include "Timer.h" //pid參數(shù) float Setpoint; float Input,Output,errSum,lastErr; float kp, ki, kd; float timeChange=10;//pid采樣時間間隔毫秒 Timer t;//定時器時間 void setup(void) { t.every(timeChange, PIDCalc) ;//每10毫秒pid一次 } void PIDCalc () { float error = Setpoint - Input; errSum += (error * timeChange); float dErr = (error - lastErr) / timeChange; Output = kp * error +ki * errSum + kd * dErr; lastErr = error; } void loop(void) { t.update(); } |
|