76行代码如何完成一个双轮平衡小车?

xobo

xobo

发表于 2017-01-11 20:59:52
内容来源: 网络

Ruff Lite

Ruff Lite 是 Ruff 团队针对 MCU(MicroController Unit,微控制器)推出的 Ruff OS
,具有高实时性,占用内存小等特点。目前官方支持的开发板为TI TM4C1294-LaunchPad ,Ruff Lite支持的硬件接口包括:GPIO、UART、I2C、ADC、PWM、QEI。

原理简介

两轮自动平衡车是一个典型的自动控制系统,由执行元件(直流电机),传感模块(陀螺仪和编码器)和主控平台控制系统(Ruff 开发板)组成。直流电机控制两轮正反转,陀螺仪检测车身姿态,编码器检测电机转速,这两组传感器数据反馈给控制系统,经由 PID 控制算法计算,给出控制直流电机的控制量,通过这一闭环过程,从而形成负反馈,保证车身平衡

物件清单

主控平台

Ruff Lite 开发版 (型号 TM4C1294-V1 )

传感器及执行元件

  • 陀螺仪模块 (型号 GY-521)

  • 直流电机驱动模块 (型号 TB6612FNG )

  • 编码器模块(随直流电机一体)(型号 MG513-30 )

MG513-30 是该直流电机的型号,由电机驱动模块进行驱动,这里我们用它自带的编码器模块

其它

  • 机械元件

  • 12V 锂电池

  • 电压转换模块(12V-5V)

直流电机 12V 供电,开发板 5V 供电

开发步骤

1. 初始化 APP,选择 tm4c1294-v1 开发板(对应 TI TM4C1294-LaunchPad)
$ rap init --board tm4c1294-v1
2. 添加陀螺仪驱动,id 为 gyro,型号选择 GY-521,其余参数默认
$ rap device add gyro (GY-521)
3. 添加电机驱动,id 为 motor,型号选择 TB6612FNG
$ rap device add motor (TB6612FNG)
4. 添加编码器驱动,id 为 encoder,型号选择 MG513-30,其余参数默认
$ rap device add encoder (MG513-30)
5. 编写控制算法

(见下文)

6. 调试

(见下文)

7. 扫描开发板
$ rap scan
7. 部署应用
$ rap deploy

控制算法

PID(比例-积分-微分)控制算法是工程上最常用的自动控制算法,参数 P 实现基本控制作用,参数 D 避免系统震荡,参数I用来消除系统静差,本平衡小车系统由 PID 算法进行控制,从而保持平衡

平衡车内部有两个反馈环,一个是由陀螺仪反馈姿态倾角和角速度构成的 直立环,一个是由编码器反馈直流电机转速构成的 速度环,由此构成一个串级 PID 控制系统(见下图),速度环控制的输出作为直立环控制的输入,直立环由 PD 控制(比例-微分控制)系统构成,保证小车的基本平衡(参数 P 的作用)和避免震荡(参数 D 的作用),速度环由 PI 控制(比例-积分控制)系统,消除姿态倾角的静差(参数I的作用)

具体 PID 算法的原理及推导过程请参考自控控制专业书籍,这里只对算法作用和各个参数的意义进行简要说明

控制程序

src/index.js

$.ready(function(error) {
    if (error) {
        console.log('error', error);
        return;
    }

    var gyro = $('#gyro'); // 陀螺仪传感器(GY-521)
    var enc = $('#encoder'); // 编码器传感器(MG513-30)
    var motor = $('#motor'); // 电机驱动控制器(TB6612FNG)

    // 直立环PD控制
    var getBalancePwm = function (actualAngle, actualGyro) {
        var targetAngle = 0.8; // 小车静止平衡时的姿态角度
        var kP = 80; // 直立环比例(P)控制参数
        var kD = 2; // 直立环微分(D)控制参数

        // 直立环控制分量
        var balancePwm = kP * (actualAngle - targetAngle) + kD * actualGyro;
        return balancePwm / 1000;
    };

    // 速度环PI控制
    var encoder = 0;
    var sumEncoder = 0;
    var getVelocityPwm = function (actualLEncoder, actualREncoder) {
        var targetVelocity = 0; // 小车静止平衡时的电机输出转速
        var kP = 3; // 速度环比例(P)控制参数
        var kI = kP / 200; // 速度环积分(I)控制参数

        // FIR二阶低通滤波
        encoder = 0.2 * (actualLEncoder + actualREncoder - targetVelocity) + 0.8 * encoder;
        sumEncoder += encoder;

        // 积分限幅
        if (sumEncoder >= 3000) {
            sumEncoder = 3000;
        }
        if (sumEncoder <= -3000) {
            sumEncoder = -3000;
        }

        // 速度换控制分量
        var velocityPwm = kP * encoder + kI * sumEncoder;
        return (velocityPwm / 1000);
    };

    var cycle = 20; // 采样/控制周期均为20ms,即1s采样/控制50次
    var gyroAcquire, encAcquire, balanceControl;

    var angleX = 0;
    var gyroY = 0;
    var rpm = 0;

    // 每隔20ms,获取陀螺仪沿X轴的姿态倾角和角速度
    gyroAcquire = setInterval(function() {
        gyro.getFusedMotionX(cycle, function (error, _angleX, _gyroY) {
            angleX = _angleX;
            gyroY = _gyroY;
        });
    }, cycle);

    // 每隔20ms,获取编码器的速度值
    encAcquire = setInterval(function() {
        enc.getRpm(function (error, _rpm) {
            rpm = _rpm;
        });
    }, cycle);

    // 每隔20ms,利用反馈值计算控制量,控制电机正反转
    balanceControl = setInterval(function() {
        var balancePwm = getBalancePwm(angleX, gyroY);
        var velocityPwm = getVelocityPwm(rpm, rpm);
        var pwmDuty = balancePwm - velocityPwm;

        if (pwmDuty >= 0) {
            if (pwmDuty >= 1) {
                pwmDuty = 1;
            }
            // 控制车身前进(电机A正转B反转,A与B相差180度安装)
            motor.forwardRotateA(pwmDuty);
            motor.backwardRotateB(pwmDuty);
        } else {
            if (pwmDuty <= -1) {
                pwmDuty = -1;
            }
            // 控制车身后退(电机A反转B正转,A与B相差180度安装)
            motor.backwardRotateA(-pwmDuty);
            motor.forwardRotateB(-pwmDuty);
        }

        // 若倾角超过30度,停止整个控制系统运行
        if (angleX >= 30 || angleX <= -30) {
            // 停止陀螺仪采样
            clearInterval(gyroAcquire);
            // 停止编码器采样
            clearInterval(encAcquire);
            // 停止电机控制逻辑
            clearInterval(balanceControl);
            // 停止电机A/B转动
            motor.stopRotateA();
            motor.stopRotateB();
        }
    }, cycle);
});

调试

目标角度调试

装好整个机械元件后,要进行目标角度调试,即 targetAngle 变量,具体方法,将控制 motor 前后转动的代码全部注释掉,然后在 balanceControl 这个函数中,打印 angleX,得到小车趋于平衡静止时的角度,应该大约在正负3度以内。

算法参数调试

首先确定参数的极性。

先屏蔽掉外反馈环 PI 控制,保持 kP 和 kD 参数不变,看是否小车有平衡的趋势,及车轮是否会向倾倒的一侧转动,若是,则 kP 和 kD 参数为正数不需要改变,若否,则 kP 和 kI 参数需要改为负数。

之后屏蔽掉内反馈环 PD 控制,保持 kI 和 kD 参数不变,用手去转动连接编码器的那个车轮,看是否是此PI控制是正反馈,即给车轮一个小的转动,车轮是否会一直加速到最大速度,若是,则 kP 和 kD 参数为正数不需要改变,若否,则 kP 和 kI 参数需要改为负数(上述程序中只需要改 kP 即可,kI 为 kP/200)。

然后确定参数的数值。一般情况下,整个机械元件装稳定后,PD 算法参数(kP 和 kD)和 PI 算法中的参数(kP 和 kI)应该不需要变动就可以直接运行在你的平衡小车上。

FAQ

Ruff MPU版(ruff-mbd-v1)可以作为主控平台么?

不能,因为底层的 OpenWRT(基于 Linux)不是实时操作系统,系统启动后会运行很多进程,Ruff 进程不一定时刻占有 CPU,因此不能稳定地每隔一个控制周期(这里是 20ms)获得传感器数据,不满足控制系统的实时性要求。而 MCU 版 Ruff 的底层操作系统是 Nuttx RTOS,能够保证实时操作。

控制系统中控制周期是多少?

控制周期为 20ms,即1秒内控制系统控制 50 次。每个控制周期需要做的内容包括 1) 获取陀螺仪和编码器两个传感器的数据,2) 传入直立环和速度环算法中进行计算得到控制量,3) 将控制量作用于直流电机上。

用Ruff MCU开发板开发平衡车与用其它开发板(如 stm32)进行裸板开发,有什么相同点与不同点?

相同点是都满足实时性控制的要求(如本案例的 20ms 控制周期)。

不同点主要体现在 开发效率可移植性两个方面,若用其它 MCU 开发板进行裸板开发,要面对 硬件接口协议(I2C接口陀螺仪,QEI接口编码器,PWM 和 GPIO 接口的直流电机控制器),外设模块协议(比如给陀螺仪发送什么命令获取到加速度值和角速度值)和 硬件定时器中断(通过配置寄存器设置20ms定时器中断)等其它问题,且代码不具备可移植性和复用性,但在整个开发过程中若用 Ruff 开发,可直面业务逻辑,即自动控制算法,而不用关心硬件模块的任何细节,你面对的只有外设模块的 API,并且由于没有任何硬件平台的逻辑,程序本身具备可复用性。

内容来源:https://segmentfault.com/a/1190000008080464

用户评论
开源开发学习小组列表