08-两轮平衡小车

平衡原理

小时候我们玩过一个游戏, 拿一根筷子或者一根笔放在手心上, 我们想方设法不让筷子倒下.
大家还记得是如何做到的吗? 如下图所示, 大圆表示的是轮子, 小圆表示的棍子末端, 为了方便大家理解, 我们假设木板始终是水平的, 并且棍子的一端固定在木板上面.
现在我们看到棍子已经有倾斜的趋势了, 棍子与竖直方向的夹角是θ, 那大家还记得大明湖畔的受力分析吗?

在该图中, m表示质量, g表示重力加速度. 我们沿着棍子的垂直的方向分解棍子的重力,也就是棍子末端倒下时受到的力, 我们可以得出棍子倒下去的力为: mgsin(θ)
现在我们只需要找到一个力,能够抵消掉mgsin(θ)即可将棍子扭正. 让我们来思考一下, 如何才能不让棍子倒下呢?

答案是我们需要一个与mg*sin(θ)反向的作用力, 在当前情况下,我们只需要让轮子向右边加速运动, 棍子在反向作用力的情况下就可以得到一个水平方向上的力: -ma (因为a表示的是轮子向右运动的加速度用a表示,所以反作用力就是-ma)

我们再向棍子的垂线方向上分解这个反向作用力:-ma, 我们可以得出垂直棍子方向上的分力为: -macos(θ), 结合物理知识我们可以很容易得出在棍子锤子方向上,棍子所受到的合力F = mgsin(θ) - macos(θ).

在当前的案例中, m质量,g是重力加速度, θ是变化量, 通过我们合力公式F = mgsin(θ) - macos(θ), 我们可以得出只要acos(θ) > g*sin(θ) 我们就可以得到一个反向的作用力让棍子立起来.

这个分析结论与我们小时候在手心上玩竹竿或者筷子的原理是一样的, 当棍子向右边倒下的时候, 我们控制电机向右加速, 当棍子向左边倒下的时候, 我们控制电机向左边加速.
当然,仅仅这样是不够的, 因为我们需要让棍子稳定的直立起来, 如果仅仅是考虑加速度, 很容易就导致棍子在直立位置附近来回震荡, 为了减少震荡, 尽快稳定到平衡位置,我们需要考虑增加阻尼力, 阻尼力与角速度成正比, 方向相反. 加上阻尼力之后, 回复力为: F = mgsin(θ) - macos(θ) - mkw . 其中w表示角速度, k表示系数

直立环

示例代码

int balance_pid(float pitch, float balance_angel, float current){
    //float balance_KP = 440 ;   // 300 -0.5
    //float balance_KD = 0;
    static float angle_integral;
    float error = (pitch - balance_angel);
    if(angle_integral > 100){
        angle_integral = 0;
    }
    int balance_pwm = g_balanceKP * error + g_balanceKI * angle_integral + g_balanceKD * current;
    angle_integral += error;
    return balance_pwm;
}

速度环

示例代码

int velocity_pid(int target,int left,int right){
    static float vel_pwm,encoder_least,encoder,encoder_integral;
    encoder_least = 0 - (left + right);
    encoder *= 0.84;
    encoder += encoder_least*0.16;
    encoder_integral += encoder;
    if(encoder_integral > 15000) encoder_integral = 15000;
    if(encoder_integral < -15000) encoder_integral = -15000;
    vel_pwm = (-g_velocityKP) * encoder + (-g_velocityKP/200) * encoder_integral;
    // 当pitch值过大的时候， 应该让小车停止转动
    if(sPitch > -70 || sPitch > 70 ){
        encoder_integral = 0;
    }
    return vel_pwm;
}