专利名称：基于红外姿态检测的两轮直立式自平衡机器人及控制方法
技术领域：
本发明是一种基于红外传感器姿态检测的两轮直立式自平衡机器人及控 制方法，属于机器人控制领域。
背景技术：
两轮自平衡机器人是在伴随着当今对移动式机器人不断的深入研究的趋 势下出现的新型机器人，轮式机器人具有成本低、机构和控制算法简单、能 高速稳定的移动、动力利用率高，同时能够自学习，同过自强化学习来学会 之前不能完成的任务。这种机器人最大的特点就是能够自平衡，扩展了他适 应的环境变化的能力，同时，可以进行加速或者是减速以及实现原地回转和 任意半径转向，移动轨迹更为灵活易变，大大弥补了传统三轮或四轮转向不 足的缺点。
目前在美国的商品化两轮代步机器人Segway以内置的精密固态陀螺仪 (Solid-State Gyroscopes)来判断车身所处的姿势状态，透过精密且高 速的中央微处理器计算出适当的指令后，驱动马达来做到平衡的效果。但 是这种方法由于采用先进的固态陀螺仪，所以成本比较高，不能得到普及。 传统的两轮自平衡机器人的姿态检测系统多采用倾角仪，加速度计和陀 螺仪三者相结合的方式达到测量目的，这种方法成本较高，而且像陀螺仪， 倾角仪等器件容易受到温度的影响，产生零点漂移。
发明内容
本发明提出了一种基于红外传感器作为姿态检测的两轮自平衡机器人及 其控制方法，此机器人通过红外传感器检测与地面之间的距离来达到姿态检 测的目的，此种方法大大节约了传感器的成本，同时克服了零点漂移现象， 能够有效准确的对姿态进行检测。同时体积较小，甚至可以在普通的办公桌
桌面上工作。
本发明的主体思路是当机器人发生倾斜时，红外传感器检测的其与地 面的距离将发生变化，红外传感器将变化的信号传给主控制器，在主控制器
3中，将进行一系列的信号处理，之后，将处理好的信号转换成驱动电机的P丽 波信号，该信号传送到电机的控制电路板上，从而驱动电机，使电机朝着机 器人倾斜方向转动，从而达到控制机器人自平衡的目的。
本发明采取的具体技术方案如下包括主体支架、固定在主体支架下端 的电机支架、固定在电机支架上的两个电机、连接在两个电机上的轮子、主 控制电路、固定在主体支架上端的红外传感器支架和两个红外传感器。两个 红外传感器分别固定在红外传感器支架的左右两端，采集与地面之间的距离， 两个红外传感器的输出端分别与主控制电路的输入端相连，主控制电路的输 出端与电机相连。
上述机器人的自平衡控制方法包括以下步骤
1) 左右两个红外传感器分别检测与地面之间的距离，并将该距离以电压 信号的形式传送给主控制电路；设L1为左侧红外传感器检测到的与地面之间
的距离，设L2为右侧红外传感器检测到的与地面之间的距离，L0为机器人 处于平衡位置时红外传感器检测到的与地面之间的距离；
2) 主控制电路通过接收到的距离L1、 L2判断机器人的姿态信息，具体 为当L1〉L0且L2〈L0时，判断机器人为向右发生倾斜；
当LKLO且L2>L0时，判断机器人为向左发生倾斜；
3) 根据步骤2)得到的机器人姿态信息，主控制电路通过增量PID控制 算法，得出一组PWM波控制信号来控制电机，驱动电机进行转动，使机器人 重新达到平衡；具体为
当机器人向右倾斜时，电机向右转动，通过车轮带动机器人向右移动， 产生一段位移后，使机器人重新达到平衡；
当机器人向左倾斜时，电机向左转动，通过车轮带动机器人向左移动， 产生一段位移后，使机器人重新达到平衡。
本发明采用红外传感器作为姿态检测的信号输入，此种方法大大节约了传 感器的成本，同时克服了零点漂移现象，能够有效准确的对姿态进行检测。
图l自平衡机器人的机械结构2自平衡机器人的电气结构示意图 图3自平衡机器人电气结构框图 图4自平衡机器人动作示意图
图5自平衡机器人控制方法示意图 图6自平衡机器人控制方法原理图 图7整个控制过程的软件算法流程图 图8 (a)电机支架结构前视图 图8 (b)电机支架结构上视图 图8 (c)电机支架结构右视图 图9 (a)传感器支架前视图 图9 (b)传感器支架上视图 图9 (c)传感器支架右视图
具体实施例方式
下面结合附图对本发明作进一步说明
首先结合图l说明本发明的机械部分。本发明的机械结构比较简单，成 本低，容易实现。本机器人包括两个独立的轮子，机器人的轮子l采用的是 硬质的塑料，塑料外圈套用橡胶制成的轮胎，比起普通标准的轮子，这样不 仅能大大减少机器人的重量，同时大大减轻了电机5的负担，从而减低了电 机5的功率。进而可以减少了电机的成本。
电机支架3 (参见图8)采用的是铝质材料，电机5通过上下各两个固定 螺丝固定到电机支架3上。电机支架3通过左右个4个螺丝固定到机器人主 体支架2的下端，这样保证了即使机器人在剧烈运动时，机器人的驱动部分 仍然非常牢固。同时，电机支架3又与机器人电机5相连，构成机器人的底 盘及驱动执行机构。
机器人主体支架2的主要材料为优质的3毫米厚亚格力塑料板(塑料玻 璃板)。这样的材料大大减轻了机器人的重量，而且在机器人发生倾斜时，由 于采用较轻的塑料板替代传统的铝板，可以使机器人迅速的恢复的平衡位置， 给系统变得更加容易控制。同时，亚格力塑料板方便打孔，可以便于以后的扩展功能。
如图9所示，红外传感器支架4连接到机器人主体支架2的上端，同时,
红外传感器支架4的两端分别连接有一个红外传感器6，构成机器人的姿态检 测部分，为机器人提供姿态检测的信息。红外传感器支架4采用的是铝质的 支架，因为如果采用塑料玻璃板，由于支架较细，非常容易折断，因此采用 铝质支架。红外传感器支架4由一个固定螺丝固定到机器人主体支架2上。
下面结合图4说明本发明的电气部分。本发明的电气结构包括两个直流 无刷电机5、 DSP主控制电路板7、电源管理模块8和电池9。其中两个红外 反射式传感器6分别与DSP控制板7的A/D采样电路相连，A/D采样电路将 红外传感器的模拟信号转换成数字信号，进而便于在DSP控制器内进行处理。 在DSP控制器中将采样信号进行相应的处理后，由DSP输出电机控制信号 PWM波，电机5的驱动电路直接与DSP控制板7相连，接收由DSP发出的 PWM波控制信号。驱动电路将信号进行处理后进而直接控制电机5转动。
DSP控制板7的核心芯片采用的是TI公司生产TMS320F2812，该处理 器采用的是改进的哈佛总线结构，内部有数据总线和程序总线。时钟频率最 高可达到150MHZ,完全满足的自平衡机器人实时性的要求。其中 TMS320C28X系列DSP是目前控制领域最高性能的处理器，具有精度高，速 度快，集成度高等特点。
电机5是采用著名的集成舵机生产商FUTABA公司生产的S148型电机。 该电机为直流电机，驱动电路与电机集成，机身小巧(40.5X20.0X38.0)并且 性能高，采用5V直流供电，并且配合齿轮减速装置，最大转速为60rpm，扭 矩为3.40kg-cm，完全满足机器人所需要的动力性能要求。同时支持PWM波 控制，可以完成系统的基本要求。
红外传感器6采用的是SHARP公司生产的GP2Y0A02YK。该传感器采用 5V供电，为模拟量输出，探测范围为0cm-150cm，提供宽角度的测量范围， 响应时间为39ms，该传感器可以在极短的时间内采集到信号，为机器人提供 了极佳的动态性能。
电源管理模块8采用的是集成的DC-DC变压电路，他的主要作用是将电池(12v)转换成DSP控制板7、电机5、红外传感器6所需要的电压(5v)。 电源管理模块8所能承受的最大功率是2w，而机器人实际工作的最大功率不 足1.5w，所以不会存在烧毁器件的问题。
结合图4 6说明本发明的控制过程当机器人由于外界扰动发生倾斜时， 红外传感器6检测到的离地面10的距离信号将发生变化，红外传感器5将信 号传给DSP控制板7， DSP控制板7将采集到的信号进行相应的处理后，从 而得出具体的机器人倾斜的程度以及机器人倾斜的方向，进而通过处理，将 倾斜的程度以及倾斜的方向转化成用于电机转动控制的PWM波。左右电机5 的驱动电路接收来自DSP控制板7的PWM波控制信号，在经过信号调理后， 驱动电机进行转动。通过电机5的转动，带动与电机5直接相连的车轮1，使 机器人达到平衡。例如当机器人向右边倾斜时，机器人的重心随之偏离机 器人的轴线，电机5亦向右转动，通过车轮带动机器人向右移动，产生了一 段位移，使机器人的重心重新回到所在的轴线上，进而机器人重新达到平衡。
本发明的DSP控制算法采用的是增量式PID控制。具体方法如图5所示， 以小车车轮1与地面10的切点为原点，分别以水平和竖直方向为X,Y轴，建 立平面直角坐标系。当机器人发生倾斜时，机器人的重心如图位置所示，由 于机器人的倾斜，导致了红外传感器6测得与地面的距离L1、L2发生了变化， 通过对L1、 L2长度的变化，可以得知机器人姿态的信息。设L0为平衡位置 时红外传感器测得的与地面的距离，当L1〉L0且L2《0时，机器人向右发生 倾斜；当LKL0且L2〉L0时，机器人向左发生倾斜。由此，可以判断出机器 人倾斜的方向，已决定电机5的转动方向。以机器人向右发生倾斜为例，说 明通过测量L1、 L2的长度来决定机器人倾斜的程度。当机器人发生倾斜时， 倾斜程度越大，Ll的长度越长，而L2的长度越短，进而通过增量PID控制 算法得出一组PWM波控制信号来控制电机5，驱动电机5进行转动，使机器 人重新达到平衡，具体为
其中增量式PID控制算法可简单的表述如下
(" = Aw, (A) — Aw, (A: —— 1) A",、(l丄,(HI丄,(A:-l)-4l)+Al丄,("-丄ol + M""—化
72(|丄,(^-1)-丄0| + ^-2)-Z0|) (1=1,2) A"^)表示第k时刻的控制量，、表示比例系数，^表示积分系数，^表 示微分系数，lA("-Aj表示第k时刻与平衡位置的误差。
由此算法可以得出一组PWM波控制信号来控制电机，当倾斜程度越大
时，电机所产生的扭矩越大，从而克服机器人倾斜的力矩越大，因而可以通 过电机的转动使机器人的重心回到轴线上。
权利要求
1、一种基于红外姿态检测的两轮直立式自平衡机器人及控制方法，包括主体支架(2)、固定在主体支架(2)下端的电机支架(3)、固定在电机支架上的两个电机(5)、连接在两个电机(5)上的轮子(1)，其特征在于还包括主控制电路、固定在主体支架(2)上端的红外传感器支架(4)和两个红外传感器(6)，两个红外传感器(6)分别固定在红外传感器支架(4)的左右两端，采集与地面之间的距离，两个红外传感器的输出端分别与主控制电路的输入端相连，主控制电路的输出端与电机(5)相连。
2、 一种权利要求1中所述的一种基于红外姿态检测的两轮直立式自平衡机器 人及控制方法，其特征在于，该方法包括以下步骤1) 左右两个红外传感器分别检测与地面之间的距离，并将该距离以电压 信号的形式传送给主控制电路；设L1为左侧红外传感器检测到的与地面之间 的距离，设L2为右侧红外传感器检测到的与地面之间的距离，L0为机器人 处于平衡位置时红外传感器检测到的与地面之间的距离；2) 主控制电路通过接收到的距离L1、 L2判断机器人的姿态信息，具体 为当LPL0且L2《0时，判断机器人为向右发生倾斜；当LKLO且L2>L0时，判断机器人为向左发生倾斜；3) 根据步骤2)得到的机器人姿态信息，主控制电路通过增量PID控制 算法，得出一组PWM波控制信号来控制电机(5)，驱动电机(5)进行转动， 使机器人重新达到平衡；具体为当机器人向右倾斜时，电机(5)向右转动，通过车轮带动机器人向右移 动，产生一段位移后，使机器人重新达到平衡；当机器人向左倾斜时，电机(5)向左转动，通过车轮带动机器人向左移 动，产生一段位移后，使机器人重新达到平衡。
全文摘要
本发明是一种基于红外姿态检测的两轮直立式自平衡机器人及控制方法，属于机器人控制领域。本机器人主要包括两个彼此相互独立的轮子、两个直流电机、两个红外反射式传感器和机器人的主体骨架。同时，本机器人还包括信息处理系统，以及电源供电系统。本发明的方法是通过设置与机器人上部的红外传感器检测其与地面的距离，从而得出姿态信号，并根据得出的姿态信号进行信息处理，从而产生电机控制信号驱动电机，使机器人保持平衡。该方法简单易行，能够准确的掌握机器人的姿态信息，同时受环境影响较小，不容易产生零点漂移，并且成本低廉。
文档编号B25J19/00GK101537615SQ200910082949
公开日2009年9月23日 申请日期2009年4月24日 优先权日2009年4月24日
发明者航 刘, 武卫霞, 胡静敏, 阮晓钢 申请人:北京工业大学