pig881 发表于 2016-5-26 19:55:15

平衡车站起来但是很抖

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

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

http://player.youku.com/player.php/sid/XMTU4NTA2MjEwOA==/v.swf

GDHack 发表于 2016-5-26 23:15:51

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

pig881 发表于 2016-5-27 16:24:27

本帖最后由 pig881 于 2016-5-27 23:27 编辑

GDHack 发表于 2016-5-26 23:15 static/image/common/back.gif
贴算法，我帮你用数学方法分析


void Kalman_Filter(double angle_m,double gyro_m)
{
    angle+=(gyro_m-q_bias) * dtt;
    Pdot=Q_angle - P - P;
    Pdot=- P;
    Pdot=- P;
    Pdot=Q_gyro;
    P += Pdot * dtt;
    P += Pdot * dtt;
    P += Pdot * dtt;
    P += Pdot * dtt;
    angle_err = angle_m - angle;
    PCt_0 = C_0 * P;
    PCt_1 = C_0 * P;
    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;
    P -= K_0 * t_0;
    P -= K_0 * t_1;
    P -= K_1 * t_0;
    P -= 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 longnow = 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 发表于 2016-5-27 19:00:17

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

GDHack 发表于 2016-5-27 23:42:10

pig881 发表于 2016-5-27 16:24 static/image/common/back.gif
void Kalman_Filter(double angle_m,double gyro_m)
{
    angle+=(gyro_m-q_bias) * dtt;


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

pig881 发表于 2016-5-28 12:13:41

本帖最后由 pig881 于 2016-6-4 00:07 编辑

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

原野动力 发表于 2016-5-28 16:13:47

学习学习学习学习

GDHack 发表于 2016-5-28 23:29:05

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




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

darkorigin 发表于 2016-5-29 01:53:17

估计是滤波的参数问题
PID 和卡尔曼。
算法方面早还给老师了，所以帮不上啥。
页: [1]
查看完整版本: 平衡车站起来但是很抖