平衡车的抖动…………

guolei123 · 发表于 2014-1-8 17:51:41

用的互补滤波算法，没有使用P、I、D等控制，现在车子会出现特别激烈的抖动，求解啊，不知道为啥…………第一次做平衡车…………

pig881 · 发表于 2016-5-27 23:22:22

woshiwangbin 发表于 2014-1-15 15:34
这位大哥汝甚屌！拜求赐教！！

为何这么屌。。。

我的平衡车用 Arduino UNO 做的，MPU6050,  PID 卡尔曼滤波，可是站起来后很抖，

我调遍了参数也没用，怎么破？

void Kalman_Filter(double angle_m,double gyro_m)
{
angle+=(gyro_m-q_bias) * dtt;
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] * dtt;
P[0][1] += Pdot[1] * dtt;
P[1][0] += Pdot[2] * dtt;
P[1][1] += Pdot[3] * dtt;
angle_err = angle_m - angle;
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;//也许应该用last_angle-angle
}

void pwm_calculate()
{
unsigned long  now = millis();    // 当前时间(ms)
int Time = now - lastTime;

  int range_error;

  //Serial.print("  R:");Serial.print(c2);
  //Serial.print("  L:");Serial.print(c1);
  range+=(c1+c2)*0.5;
  range*=0.9;
  range_error=c1-c2;
  range_error_all+=range_error;

  wheel_speed=range-last_wheel;
  last_wheel=range;

  pwm=angle*k1+angle_dot*k2+range*k3+wheel_speed*k4; //use PID tho calculate the pwm

  if(pwm>255)                            //Maximum Minimum Limitations
pwm=255;
  if(pwm<-255)
pwm=-255;
//Serial.print(pwm);Serial.print("\t");
//  Serial.print(c1);Serial.print("\t");
//  Serial.print(c2);Serial.print("\t\n");
/* if(turn_flag==0)
{
   pwm_R=pwm+range_error_all;
   pwm_L=pwm-range_error_all;
}
else
{
      pwm_R=pwm-turn_flag*68;  //Direction PID control
      pwm_L=pwm+turn_flag*68;
      range_error_all=0;    //clean
}
*/
   c1 = 0;//clean
   c2 = 0;
lastTime = now;
//Serial.print(Time);Serial.print("\n");

}