平衡车站起来但是很抖

pig881

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

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

GDHack

贴算法，我帮你用数学方法分析

pig881

GDHack 发表于 2016-5-26 23:15
贴算法，我帮你用数学方法分析

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;  

  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;
  
     c1 = 0;//clean
     c2 = 0;
   lastTime = now;

  
}

wetnt

这个效果已经很不错了！收藏

GDHack

pig881 发表于 2016-5-27 16:24
void Kalman_Filter(double angle_m,double gyro_m)
{
    angle+=(gyro_m-q_bias) * dtt;

只贴这两个函数恐怕不够。如果不方便把全部代码贴出来的话，建议你先整理一下算法，用公式或者伪代码表示也可以。

pig881

现在已经可以很稳的平衡了

原野动力

学习学习学习学习

GDHack

pig881 发表于 2016-5-28 12:13
哦，我以为你只需要看算法，所以我就贴了这两个，全部代码如下：

OK，先研究一下，找到问题第一时间回复你。

darkorigin

估计是滤波的参数问题
PID 和卡尔曼。
算法方面早还给老师了，所以帮不上啥。