玩转树莓派

    返回首页    发表留言
本文作者:李德强
          第五节 自平衡算法
 
 

        接下来我们要对自平衡算法做一下详细描述。假设有这样一个情景(小时候我们都玩过的):如果有一根很高的木棍直立在你的手掌上,当木棍顶端向前倾斜时,想要让其恢复平衡状态,我们需要将手掌也向前移动,如下:

        我们来看以下三种情况:
        1、木棍顶端前倾的速度大于手掌前移的速度,木棍则继续前倾,直到倒下。
        2、木棍顶端前倾的等于大于手掌前移的速度,木棍则保持前倾状态。
        3、木棍顶端前倾的速度小于手掌前移的速度,木棍则上扬。
        因此我们需要使手掌的速度大于木棍顶端前倾的速度。但我们无法直接测量木棍前倾速度,而是通过MPU6050陀螺仪来测量其倾斜的角加速度进而计算出其倾斜角度。并根据倾斜角度通过PID反馈控制来修正电机正反转数,来实时调整小车的平衡。当然只调整电机速度是不行的,我们需要调整电机的加速度,通过加速度计算出速度:

void engine_move(s_engine *e)
{
        //时间片
        double timer = (double) ENG_TIMER / 100.0;
        //根据初速度、加速度算计当前速度
        e->vel = e->vel + e->acc * timer;
}

 

        关于PID控制的资料有很多,我们也不需要作过多的介绍,只对比例(P)、积分(I)、微分(D)各项做一点简单的说明:
        比例控制:比例控制器的输出与输入误差信号成比例关系。也就是线性关系。
        积分控制:如果在进入稳态后存在稳态误差,则称这个控制系统是有稳态误差的或简 称有差系统。为了消除稳态误差,在控制器中必须引入积分项。积分项对误差取决于时间的积分,随着时间的增加,积分项会增大。
        微分控制:在微分控制中,控制器的输出与输入误差信号的微分(即误差的变化率)成正比关系。

         一般来说PID控制分为位置式控制和增量式控制,本例所使用的是增量式控制,对PID式子化简,并从Timer的N到时N-1的计算如下(化简过程略):

Acc  = Kp * (et - et_1) + (Ki * et) + Kd * (et - 2 * et_1 + et_2)

 

下面来看一下引擎中自平衡原理:

 

        代码实现:

void engine_balance()
{
        s_engine *e = &engine;
        //启动MPU6050
        mpu6050_setup();
        //增量式PID输入
        double et = 0.0, et_1 = 0.0, et_2 = 0.0;
        //启动引擎
        while (true)
        {
                //取得角度
                mpu6050_value(&e->x, &e->y, &e->z, &e->ax, &e->ay, &e->az);
                //计算倾斜角
                double angle = e->y + e->dy;
                et_2 = et_1;
                et_1 = et;
                et = angle;
                //PID反馈控制
                e->acc = engine_pid(et, et_1, et_2);
                //显示输出
                printf("angle: %7.2f\tacc: %7.2f\tvel: %7.2f\tKp: %7.2f\tKi: %7.2f\tKd: %7.2f\n", angle, e->acc,e->vel, Kp, Ki, Kd);
                //通过电机加速度计算电机速度
                engine_move(e);
                //设定电机速度
                driver_set_speed(e->vel);
                //时间片
                usleep(ENG_TIMER * 1000);
        }
}

接下来需要确定PID中三个系数的值。你可以使用示波器,也可以自己编写图形化界面自己绘制波形。我们在这里只是简单的通过对小车倾斜做观察从而确定了系数取值(没有投入更多的精力使其完美的平衡)。这里我们给出小车的PID参考值为:

        Kp = 3989.00;
        Ki = 216.00;
        Kd = 32875.00;


        最后,非常感谢大家的支持!提供完整的程序代码(Linux 环境 gcc 5.3.1):

BalanceCar.tar.gz

    返回首页    返回顶部
#1楼  招聘  于 2017年03月31日12:53:19 发表
 
怎么改速度
#2楼  李德强  于 2017年03月31日22:28:09 发表
 
使用PWM信号,调整高电平时长即可。
  看不清?点击刷新

 

  Copyright © 2015-2023 问渠网 辽ICP备15013245号