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

2017-01-13 10:47:59来源:segmentfault作者:Ruff人点击


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,并且由于没有任何硬件平台的逻辑,程序本身具备可复用性。

最新文章

123

最新摄影

微信扫一扫

第七城市微信公众平台