三部曲之一，单维度PID调整，上视频
冷静一点后，觉得四轴飞行器一步到位不太现实，不如分三步曲去做计划：
1. 用PID控制单维度（如角度），达到较好的自动化效果
2. 自平衡小车（实际上就是1加上轮子）
3. 四轴飞行器
第一步昨天做通了，在PID参数上有些体会，先上视频，效果是让陀螺仪保持45度水平角
http://v.youku.com/v_show/id_XNTY1MjA0MDY0.html
再上代码
#include <I2Cdev.h>
#include <MPU6050.h>
#include <Wire.h>//添加必须的库文件
//====一下三个定义了陀螺仪的偏差===========
#define Gx_offset 0.2
#define Gy_offset 0.81
#define Gz_offset -0.88
//====================
MPU6050 accelgyro;
int16_t ax,ay,az;
int16_t gx,gy,gz;//存储原始数据
double aax,aay,aaz,ggx,ggy,ggz;//存储量化后的数据
double tx,ty;
double Ax,Ay,Az;//单位 g(9.8m/s^2)
double Gx,Gy,Gz;//单位 °/s
static double vari_estima_X= 0.1;
static double vari_estima_Y= 0.1;
static double vari_metron=0.0001;
static double KG_X = 0.5;
static double KG_Y = 0.5;
static double metron_X =0;
static double metron_Y =0;
static double estima_X = 0.2;
static double estima_Y = 0.2;
static int time_span_micro =50;
static float time_span =0.05;
double now_error, last_error, error_diff, error_int;
double fusion_angle =0;
double Angel_accX,Angel_accY,Angel_accZ;//存储加速度计算出的角度
long LastTime,NowTime,TimeSpan;//用来对角速度积分的
void setup()//MPU6050的设置都采用了默认值，请参看库文件
{
Wire.begin();
Serial.begin(38400);
Serial.println("Initializing I2C device.....");
accelgyro.initialize();
Serial.println("Testing device connections...");
Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful":"MPU6050 connection failure");
}
void loop()
{
accelgyro.getMotion6(&ax,&ay,&az,&gx,&gy,&gz);//获取三个轴的加速度和角速度
//Serial.print(ax/16384.00);
//Serial.print(gx/131.00);
//======一下三行是对加速度进行量化，得出单位为g的加速度值
Ax=ax/16384.00;
Ay=ay/16384.00;
Az=az/16384.00;
//==========以下三行是用加速度计算三个轴和水平面坐标系之间的夹角
//请参考：http://www.geek-workshop.com/forum.php?mod=viewthread&tid=2328&page=1#pid27876
//个人觉得原帖中case0算的不对，应该是z/sqrt（x*x+y*y），估计是江南楼主写错了
Angel_accX=atan(Ax/sqrt(Az*Az+Ay*Ay))*180/3.14 -Gx_offset; //offset
Angel_accY=atan(Ay/sqrt(Ax*Ax+Az*Az))*180/3.14 -Gy_offset; //offset
Angel_accZ=atan(Az/sqrt(Ax*Ax+Ay*Ay))*180/3.14;
//==========以下三行是对角速度做量化==========
// ggx=gx/131.00;
// ggy=gy/131.00;
// ggz=gz/131.00;
//===============以下是对角度进行积分处理================
NowTime=millis();//获取当前程序运行的毫秒数
TimeSpan=NowTime-LastTime;//积分时间这样算不是很严谨
//下面三行就是通过对角速度积分实现各个轴的角度测量，当然假设各轴的起始角度都是0
Gx=Gx+(ggx-Gx_offset)*TimeSpan/1000;
Gy=Gy+(ggy-Gy_offset)*TimeSpan/1000;
Gz=Gz+(ggz-Gz_offset)*TimeSpan/1000;
automatic();
LastTime=NowTime;
//analogWrite(6, 0);
//analogWrite(5, 80); //conter clockwise
//KG_X = KG_cal(vari_estima_X, vari_metron) ;
//KG_Y = KG_cal(vari_estima_Y, vari_metron) ;
KG_X =0.1;
KG_Y =0.1;
estima_X = estima_cal(estima_X, Angel_accX, KG_X);
estima_Y = estima_cal(estima_Y, Angel_accY, KG_Y);
//vari_estima_X = vari_estima_cal(KG_X, vari_estima_X);
//vari_estima_Y = vari_estima_cal(KG_Y, vari_estima_Y);
//==============================
// Serial.print(Angel_accX);
// Serial.print(",");
// Serial.println(Angel_accY);
// Serial.print(estima_X);
// Serial.print(",");
// Serial.println(estima_Y);
// Serial.print(Ax);
// Serial.print(",");
// Serial.println(Ay);
//delay(time_span_micro);//这个用来控制采样速度
}
//void updateKalman (double my_metron_X,double my_metron_Y){
//KG_X = KG_cal(vari_estima_X, vari_metron);
//KG_Y = KG_cal(vari_estima_Y, vari_metron);
//estima_X = estima_cal(estima_X, my_metron_X, KG_X);
//estima_Y = estima_cal(estima_Y, my_metron_Y, KG_Y);
//vari_estima_X = vari_estima_cal(KG_X, vari_estima_X);
//vari_estima_Y = vari_estima_cal(KG_Y, vari_estima_Y);
//}
//------------------------------------------------------------------
double KG_cal(double vari_estima, double vari_metron){
double temp = vari_estima*vari_estima / (vari_estima*vari_estima+ vari_metron*vari_metron);
double result =sqrt(temp);
return result;
}
//------------------------------------------------------------------
double estima_cal (double estima, double metron, double KG){
double result1 = estima + KG*(metron - estima);
return result1;
}
//------------------------------------------------------------------
double vari_estima_cal (double KG, double vari_estima){
double result2 = vari_estima * sqrt(1-KG);
return result2;
}
double PID_control(double error, double error_diff, double error_int ){
double P=2;
double I=1;
double D=1;
double command = P*error + I*error_int + D*error_diff;
return command;
}
void automatic(){
//Serial.print("now_error=");
//Serial.println(now_error);
//Serial.print("error_diff=");
//Serial.println(error_diff);
double command = PID_control(now_error, error_diff, error_int);
//Serial.print("command=");
//Serial.println(command);
int regu_pwm = int (command);
//Serial.println(regu_pwm);
if(regu_pwm >= 250){
regu_pwm=250;
analogWrite(6, 0);
analogWrite(5, regu_pwm); //conter clockwise
Serial.println("clock");
}else if (regu_pwm > 0 & regu_pwm<250){
analogWrite(6, 0);
analogWrite(5, regu_pwm); //conter clockwise
Serial.println("clock");
}else if (regu_pwm <= 0 & regu_pwm > -250){
analogWrite(5, 0);
analogWrite(6, regu_pwm); // clockwise
Serial.println("CONTER CLOCK");
}else if (regu_pwm <=-250){
regu_pwm=250;
analogWrite(5, 0);
analogWrite(6, regu_pwm); // clockwise
Serial.println("CONTER CLOCK");
}
delay (time_span_micro);
//fusion_angle = update_Estimat(fusion_angle, estima_Y, Gy );
autoCorrection();
Serial.print("estima_Y=");
Serial.println(estima_Y);
//uodate_error(time_span, 45, Angel_accY);
uodate_error(time_span, 45, estima_Y);
//uodate_error(time_span, 45, fusion_angle);
}
void uodate_error(double time_span, double consigne, double mesure){
now_error = consigne - mesure;
error_diff = (now_error - last_error)/time_span;
error_int = error_int + now_error*time_span;
last_error =now_error ;
}
double update_Estimat(double estimate, double metron_1, double metron_2){
double temp = 0.4* estimate + 0.3*metron_1 + 0.3*metron_2;
return temp;
}
void autoCorrection (){
if ( Angel_accX > -0.2 & Angel_accX < 0.2 ){
Gx=0;
}
}
接下来是体会，有这么几点：
1。P参数越大，回复速度越快，但同时越容易调整过头出现震荡，因此适合同时调大D参数，预计惯性累加量来及时减小电机转动输出（接近要到达位置时）。故曰：大P配大D，小P配小D
2 。 卡尔曼滤波函数 updateKalman 其实没有完全调通，主要是卡尔曼增益一下子就趋近于0了，导致角度响应变得非常慢。不过，如果一直将卡尔曼增益固定在0.1 （见estima_cal函数），反而波形变得很平滑而且响应速度很好。
3。 自己做的融合算法简直就是狗P （见fusion_angle），暂时不报以希望。网上说通过三轴角速度得到的角度容易受运动影响，我保持一个角度拿着mpu6050，突然超某方向运动（但维持角度不变），在串口输出上看到影响不是那么大，可能是因为已经滤过波的吧。
4。 PID函数仍然不那么精确， 理论上说在平衡位置（这里设的是45度）时，PID控制的输出量为零，但我这里还有个十几，当然十几的pwm是带不动电机的，所以视频上面表现的还可以。
5。 autoCorrection函数是用来修正陀螺仪误差的，每次三轴加速度得到的角度为零时，也会将陀螺仪积分的角度归零，可囧的是，如果一直保持非水平位置的话，好像都没有修正的机会
希望各位为我做指点。
感谢分享{:soso_e179:} 我看很多人說互補濾波也不錯，樓主可以試試看~。 請問command最後怎麼跟控制值融合的啊??
是哪一行?啊?
:victory: 留爪待学··· TTTTTTT33 发表于 2013-6-2 13:09 static/image/common/back.gif
請問command最後怎麼跟控制值融合的啊??
是哪一行?啊?
double command = PID_control(now_error, error_diff, error_int);
int regu_pwm = int (command);
然后防饱和处理,如果大于pwm范围255就用极值, 这里用的250,
if.. else if ... else if ....
分正转和反转写入analog pin
analogWrite(6, 0);172.
analogWrite(5, regu_pwm); //conter clockwise camion 发表于 2013-6-2 13:17 static/image/common/back.gif
double command = PID_control(now_error, error_diff, error_int);
int regu_pwm = int (command);
可以用這個設定範圍constrain(X, outMin, outMax); 我用互補濾波時發現，如果採樣速度太慢會衝過頭，但採樣速度變快時會對抖動的影響加大，不知道該怎麼辦......:'( 靠，搞清楚为什么融合算法失效了
原因是： 原帖代码中三轴加速度的XY 和 陀螺仪的XY 是反过来的
现在上正确的融合算法，比较了一下，两种角度测法结果差别不是特别大，最多可能差十几度吧，融合后介于两者之间#include <I2Cdev.h>
#include <MPU6050.h>
#include <Wire.h>//添加必须的库文件
//====一下三个定义了陀螺仪的偏差===========
#define Gx_offset 0.2
#define Gy_offset 0.81
#define Gz_offset -0.88
//====================
MPU6050 accelgyro;
int16_t ax,ay,az;
int16_t gx,gy,gz;//存储原始数据
double aax,aay,aaz,ggx,ggy,ggz;//存储量化后的数据
double tx,ty;
double Ax,Ay,Az;//单位 g(9.8m/s^2)
double Gx,Gy,Gz;//单位 °/s
static double vari_estima_X= 0.1;
static double vari_estima_Y= 0.1;
static double vari_metron=0.0001;
static double KG_X = 0.5;
static double KG_Y = 0.5;
static double metron_X =0;
static double metron_Y =0;
static double estima_X = 0.2;
static double estima_Y = 0.2;
static int time_span_micro =50;
static float time_span =0.05;
double now_error, last_error, error_diff, error_int;
double fusion_angle =0;
double Angel_accX,Angel_accY,Angel_accZ;//存储加速度计算出的角度
long LastTime,NowTime,TimeSpan;//用来对角速度积分的
void setup()//MPU6050的设置都采用了默认值，请参看库文件
{
Wire.begin();
Serial.begin(38400);
Serial.println("Initializing I2C device.....");
accelgyro.initialize();
Serial.println("Testing device connections...");
Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful":"MPU6050 connection failure");
}
void loop()
{
accelgyro.getMotion6(&ax,&ay,&az,&gx,&gy,&gz);//获取三个轴的加速度和角速度
//Serial.print(ax/16384.00);
//Serial.print(gx/131.00);
//======一下三行是对加速度进行量化，得出单位为g的加速度值
Ax=ax/16384.00;
Ay=ay/16384.00;
Az=az/16384.00;
//==========以下三行是用加速度计算三个轴和水平面坐标系之间的夹角
//请参考：http://www.geek-workshop.com/forum.php?mod=viewthread&tid=2328&page=1#pid27876
//个人觉得原帖中case0算的不对，应该是z/sqrt（x*x+y*y），估计是江南楼主写错了
Angel_accX=atan(Ax/sqrt(Az*Az+Ay*Ay))*180/3.14 -Gx_offset; //offset
Angel_accY=atan(Ay/sqrt(Ax*Ax+Az*Az))*180/3.14 -Gy_offset; //offset
Angel_accZ=atan(Az/sqrt(Ax*Ax+Ay*Ay))*180/3.14;
//==========以下三行是对角速度做量化==========
ggx=gx/131.00;
ggy=gy/131.00;
ggz=gz/131.00;
//===============以下是对角度进行积分处理================
NowTime=millis();//获取当前程序运行的毫秒数
//下面三行就是通过对角速度积分实现各个轴的角度测量，当然假设各轴的起始角度都是0
TimeSpan=NowTime-LastTime;//积分时间这样算不是很严谨
Gx=Gx+(ggx-Gx_offset)*time_span_micro/1000;
Gy=Gy+(ggy-Gy_offset)*time_span_micro/1000;
Gz=Gz+(ggz-Gz_offset)*time_span_micro/1000;
Serial.print("Gy=");
Serial.println(Gx);
automatic();
LastTime=NowTime;
//KG_X = KG_cal(vari_estima_X, vari_metron) ;
//KG_Y = KG_cal(vari_estima_Y, vari_metron) ;
KG_X =0.1;
KG_Y =0.1;
estima_X = estima_cal(estima_X, Angel_accX, KG_X);
estima_Y = estima_cal(estima_Y, Angel_accY, KG_Y);
//vari_estima_X = vari_estima_cal(KG_X, vari_estima_X);
//vari_estima_Y = vari_estima_cal(KG_Y, vari_estima_Y);
//==============================
// Serial.print(Angel_accX);
// Serial.print(",");
// Serial.println(Angel_accY);
// Serial.print(estima_X);
// Serial.print(",");
// Serial.println(estima_Y);
// Serial.print(Gx);
// Serial.print(",");
//delay(time_span_micro);//这个用来控制采样速度
}
void updateKalman (double my_metron_X,double my_metron_Y){
KG_X = KG_cal(vari_estima_X, vari_metron);
KG_Y = KG_cal(vari_estima_Y, vari_metron);
estima_X = estima_cal(estima_X, my_metron_X, KG_X);
estima_Y = estima_cal(estima_Y, my_metron_Y, KG_Y);
vari_estima_X = vari_estima_cal(KG_X, vari_estima_X);
vari_estima_Y = vari_estima_cal(KG_Y, vari_estima_Y);
}
//------------------------------------------------------------------
double KG_cal(double vari_estima, double vari_metron){
double temp = vari_estima*vari_estima / (vari_estima*vari_estima+ vari_metron*vari_metron);
double result =sqrt(temp);
return result;
}
//------------------------------------------------------------------
double estima_cal (double estima, double metron, double KG){
double result1 = estima + KG*(metron - estima);
return result1;
}
//------------------------------------------------------------------
double vari_estima_cal (double KG, double vari_estima){
double result2 = vari_estima * sqrt(1-KG);
return result2;
}
double PID_control(double error, double error_diff, double error_int ){
double P=1.8;
double I=1;
double D=1;
double command = P*error + I*error_int + D*error_diff;
return command;
}
void automatic(){
//Serial.print("now_error=");
//Serial.println(now_error);
//Serial.print("error_diff=");
//Serial.println(error_diff);
double command = PID_control(now_error, error_diff, error_int);
//Serial.print("command=");
//Serial.println(command);
int regu_pwm = int (command);
//Serial.println(regu_pwm);
//if(regu_pwm >= 250){
// regu_pwm=250;
// analogWrite(6, 0);
// analogWrite(5, regu_pwm); //conter clockwise
// //Serial.println("clock");
//
//}else if (regu_pwm > 0 & regu_pwm<250){
// analogWrite(6, 0);
// analogWrite(5, regu_pwm); //conter clockwise
// //Serial.println("clock");
//
//}else if (regu_pwm <= 0 & regu_pwm > -250){
// analogWrite(5, 0);
// analogWrite(6, regu_pwm); // clockwise
// //Serial.println("CONTER CLOCK");
//}else if (regu_pwm <=-250){
// regu_pwm=250;
// analogWrite(5, 0);
// analogWrite(6, regu_pwm); // clockwise
// //Serial.println("CONTER CLOCK");
//}
delay (time_span_micro);
fusion_angle = update_Estimat(fusion_angle, estima_Y, Gx );
autoCorrection();
//Serial.print("estima_Y=");
//Serial.println(estima_Y);
//Serial.print("fusion_angle=");
//Serial.println(fusion_angle);
//uodate_error(time_span, 45, Angel_accY);
//uodate_error(time_span, 0, estima_Y);
uodate_error(time_span, 45, fusion_angle);
}
void uodate_error(double time_span, double consigne, double mesure){
now_error = consigne - mesure;
error_diff = (now_error - last_error)/time_span;
error_int = error_int + now_error*time_span;
last_error =now_error ;
}
double update_Estimat(double estimate, double metron_1, double metron_2){
double temp = 0.4* estimate + 0.3*metron_1 + 0.3*metron_2;
return temp;
}
void autoCorrection (){
Serial.print("Angel_accY=");
Serial.println(Angel_accY);
if ( Angel_accY > -0.5 & Angel_accY < 0.5 ){
Serial.println("00000000000000");
Gx=0;
}
if ( Angel_accX > -0.5 & Angel_accX < 0.5 ){
Serial.println("00000000000000");
Gy=0;
}
} 楼主分享的真不错 感谢分享 希望继续分享 看了好多天的MPU6050这东西的算法 还是算不出X和Y轴方向不受重力影响的加速度现在想研究角度 通过时时的陀螺仪角度 利用空间几何算算法把 X和Y轴运动时的加速度重力分量去掉 希望得到楼主指导。有没有更简捷的方法得到我想要的数据 mark！！！！！！！！！！！！！！！！！！！！！！
页:
[1]