两轮机器人自平衡控制系统设计

商世龙 刘 通 韩礼辰 李博宇

沈阳航空航天大学 辽宁 沈阳 110136

1 研究背景与意义

2011年发生在日本福岛的核电站泄漏事故,造成16万日本民众被迫搬离家园,12.5公里的地区被划为禁区,在这种危险恶劣的环境下,只有依靠移动机器人来进行探测和侦察。此外,随着移动机器人应用领域的不断扩大,面临的环境和任务也越来越复杂,机器人要经常工作于一些空间狭小、转角较多的工作场合。如何在复杂的环境下灵活快捷的执行各种任务是当下研究的热点。

2 两轮机器人整体方案设计

2.1 两轮机器人的基本平衡理论 两轮机器人在自然状态下是不稳定结构,车轮支持力方向与车身方向存在角度θ偏差,为对两轮拟器人受力分析,现将系统简化为可左右移动平台上倒立摆模型。简化模型如图2.1所示,若对系统增加额外的受力,使得恢复力与位移方向相反才行,控制倒立摆底部的车轮,使得它做加速运动,这样两轮机器人自身会受到额外的惯性力,改力与车轮的加速度方向相反,大小成正比,这样倒立摆受到的恢复力为：

式中,由于θ很小,所以进行了线性化。假设负反馈是车轮加速度α与偏角θ成正比,比例为k1,如果比例k1＞g,那么恢复力的方向便于位移方向相反了。

图2.1 两轮机器人简化模型

此外,为了使得倒立摆能够尽快在垂直位置稳定下来,还需要增加阻尼力。因此需要另外增加控制阻尼力,增加的阻尼力与偏角的速度成正比,方向相反。因此式(2.1)可变为：

按照上面的控制方法,可把倒立摆模型变为单摆模型,能够稳定在垂直位置上,因此,可得控制车轮加速度的控制算法：

式中,θ1为倾角,θ2为角速度,k1,k2为比例系数。只要保证k1＞g,k2＞g条件下,可以使得系统像单摆一样维持在直立状态。

2.2 两轮机器人硬件总体方案设计 两轮机器人自平衡控制系统硬件框图如图2.2所示。

图2.2 两轮机器人自平衡控制系统硬件框图

2.3 两轮机器人软件总体方案设计 两轮机器人直立控制器采用PD控制,与角度成比例的控制量是称为比例控制；与角速度成比例的控制量称为微分控制(角速度是角度的微分)其中微分参数相当于阻尼力,可以有效抑制车模震荡。

两轮机器人速度控制器采用PI控制,为了保证系统稳定,可以将该系统近似为一个积分环节。将原来的微分环节和这个积分环节合并,形成－一个比例控制环节。这样可以保持系统控制传递函数不变,避免了微分计算。

3 两轮机器人硬件电路设计

3.1 两轮机器人核心控制模块设计 核心控制模块由STM32F103RCT6系统板和外围电路组成。为实现两轮机器人自平衡,需要采集和传递各种信号,其设计方案为：将单片机与传感器模块连接,通过陀螺仪采集信号,传送给单片机获得两轮机器人的倾斜角度,加速度传感器可以获得小车的加速度,将单片机与电机内部的编码器连接可获得平均速度；单片机与驱动模块以PWM信号传递,用来控制电机工作,使两轮机器人保持直立状态。

3.2 两轮机器人姿态检测模块设计 姿态检测模块采用MPU6050传感器和外围电路构成,其电路原理图如图3.1所示。

图3.1 MPU6050电路原理图

3.3 两轮机器人驱动模块设计 本方案采用的驱动芯片为TB6612芯片,TB6612芯片为双驱动,即可以驱动两个电机,其相对于传统的L298N效率上提高很多且相比L298N的热耗性和外围二极管续流电路,它无需外加散热片,外围电路简单,只需外接电源滤波电容就可以直接驱动电机,利于减小系统尺寸。对于PWM信号,它支持高达100KHz的频率有较大优势。

3.4 两轮机器人电机及编码器设计 两轮机器人的驱动电机采用直流电机,其电机内部自带编码器,其上集成了上拉电阻,可直接输出方波,编码器可获得两轮机器人的平均速度,以PWM信号形式传递给单片机实时控制姿态。

4 两轮机器人软件设计

4.1 系统主程序 系统进入工作状态时主函数进行单片机的初始化,初始化的工作包括两部分,一部分是对于单片机各个应用到的模块进行初始化。第二部分是应用于程序初始化,即对于两轮机器人控制程序中应用得到的变量值进行初始化,初始化工作结束后便等待姿态检测子程序获取姿态信息。系统在主程序初始化之后,每5ms中断一次,就可以读取MPU6050这一姿态控制模块采集的姿态数据,与此同时完成电机速度检测,在姿态数据读取完成之后,为了解决陀螺仪漂移问题,采取将加速度计与陀螺仪信号融合的方法。

4.2 两轮机器人平衡控制子程序 MPU6050获得两轮机器人的加速度和倾角之后,使用DMP滤波及角度融合。以车身倾角为输入量,通过控制两个电机的正反转保持车身平衡。调用PD算法对车体进行控制。其平衡控制算法为：

上式中balance是车身平衡的电机控制变量,Bias为滤波后的倾角偏差值,Gyro为小车当前的角速度。经过PD算法处理后的PWM波可以用来控制电机的输出。

4.3 两轮机器人速度控制子程序 本系统以编码器所测得脉冲数作为速度反馈环节。形成了速度的闭环控制。速度控制的函数参数为两轮机器人速度的偏差,本文在电机控制上采用了PI算法,其速度算法公式为：

上式中Velocity为速度的电机控制量,Encoder为上一次目标速度与实际速度的偏差,Encoder_Integral为速度偏差的累积。获得速度的电机控制量之后,再将(4.1)式与(4.2)相加,获得最终的PWM值,然后将其送到PWM寄存器。

4.4 转向控制子程序 两轮机器人转向环使用P(比例)控制器或者P(比例)D(微分)控制器,一般的控制系统单纯的P控制或者PI控制就可以了,转向环就是这种“一般的控制系统”,对响应要求不高,所以只使用P控制即可。

相比于直立环和速度环,转向环是最不重要的,如果缺少了直立环和速度环,车身无法长时间保付直立。转向环的作用是使两轮机器人行驶过程中,跟随给定的z轴角速度,具体来说,若设定的z轴目标角速度为零,那么应该两轮机器人应该走直线。

5 总结与展望

本文主要研究了基于单片机的两轮自平衡机器人控制系统。通过硬件与软件来调整车身的姿态,实现两轮机器人动态平衡和直线行走。

本次设计只是简单地建立了自平衡机器人的模型,在未来还有以下方面可以继续研究和提高：

(1)使用速度更快的控制器,如DSP,ARM等,提高系统控制速度。

(2)采用高精度的陀螺仪及加速度计,提高系统姿态检测精度。