CN101554726B

CN101554726B - 一种柔性两轮自平衡机器人系统及其运动控制方法

Info

Publication number: CN101554726B
Application number: CN2009100842598A
Authority: CN
Inventors: 阮晓钢; 李欣源; 赵建伟; 韩红桂; 王启源; 于乃功; 任红格; 蔡建羡
Original assignee: Beijing University of Technology
Current assignee: Beijing University of Technology
Priority date: 2009-05-15
Filing date: 2009-05-15
Publication date: 2011-01-19
Anticipated expiration: 2029-05-15
Also published as: US20100292840A1; US8321053B2; WO2010130179A1; CN101554726A

Abstract

一种柔性两轮自平衡机器人系统及其运动控制方法，包括主控制器(1)、运动控制器(2)、辅助控制器(3)、伺服驱动器(4)、传感器、输入输出设备、直流电机(5)和电源系统；躯干顶板(11)上设有摄像头(12)，躯干(6)和底盘(8)均装有倾角仪、陀螺仪，躯干(6)和底盘(8)之间以装有双臂扭簧(25)和圆柱弹簧(26)的柔性关节(7)连接；主控制器(1)与运动控制器(2)、辅助控制器(3)、输入输出设备连接。主控制器(1)接收的用户指令为即时指令或预存指令；通过主控制器(1)运行运动行为决策算法，运动控制器(2)运行运动平衡控制算法，伺服驱动器(4)对电机(5)的转矩控制，实现机器人平面行走和遥控。

Description

一种柔性两轮自平衡机器人系统及其运动控制方法

技术领域

本发明涉及一种两轮移动式机器人系统及其运动控制方法，为工科院校的控制理论课程和机器人设计课程提供一种实验研究平台。

背景技术

两轮自平衡机器人，又称移动式倒立摆，它的行走机构为轴心相对、平行连接于机体两侧的轮子。该种机器人兼有移动式机器人的离散时间动作决策问题和倒立摆的连续时间姿态控制问题。因而，它可成为机器人学和控制科学领域交叉的综合研究对象和理想示教设备。

现有的两轮自平衡机器人的机身整体为刚性构造，例如专利ZL200510094939.X中所述机器人。从模仿人类身体来设计机器人的角度来说，刚性机体不能体现出人类躯体，尤其是腰部结构所具有的前后俯仰运动中的柔性(弹性)。然而，完全真实仿照人体，利用人工肌肉和人工关节软组织设计机器人难度大、成本高、控制复杂、不易维护，作为仅针对柔性机体机器人的平衡控制和运动控制问题的研究和示教设备并不实用。

针对上述问题，专利ZL200720103279.1设计了一种柔性两轮直立式机器人本体。该机器人引入一段圆柱弹簧连接实现柔性机身，这种设计虽然简单易行，但是存在一些不足。首先，单纯的圆柱弹簧连接方式使该机器人柔性段具有无限自由度，其中绝大部分不可控制，例如左右晃动自由度，这对于两轮直立式机器人姿态控制的研究并没有显著意义，反而带来很多不必要的麻烦。其次，在柔性关节高度不变的情况下，圆柱弹簧材料直径和圈数变化范围十分有限，直接制约柔性关节刚度值的取值范围，使机器人作为示教设备在操作性和参数调节范围上略显不足。此外，专利ZL200720103279.1仅涉及该类机器人的机械本体结构，并没有给出机器人适用的电气系统和姿态平衡控制方法。

发明内容

本发明的目的在于提出一种机身具有柔性俯仰关节的两轮自平衡机器人系统及其运动控制方法。该机器人系统是一种开放式智能机器人研究开发平台，为控制科学和机器人学的研究和教学提供实验对象；运动控制方法实现该机器人的基本功能，为用户开发、使用该机器人提供参考。

本发明是采用以下技术手段实现的：

一种柔性两轮自平衡机器人系统，包括电气系统和机械本体；其中，所述电气系统包括主控制器1、运动控制器2、辅助控制器3、电机伺服驱动器4、传感器、输入输出设备、直流电机5和电源系统；所述机械本体包括躯干6、柔性关节7、底盘8、保护支架9和双轮10；

躯干6为层式框架，顶板11上固定有至少一个摄像头12，顶板11和底板13之间由隔板14分隔成若干层，躯干安装倾角仪15、陀螺仪16和超声波传感器17；所述躯干底板13和底盘8之间以柔性关节7连接；底盘8为箱式结构，在其内部装载倾角仪18、陀螺仪19，直流电机5分别固定在底盘8两侧内壁上，电机5的轴一端与轮子10的轴联接，另一端接编码器20；底盘8底面安装有可拆卸保护支架9，支架9脚轮21；双轮10的轴心位于一条直线上；

主控制器1为嵌入式计算机系统；运动控制器2选用数字信号处理器系统；辅助控制器3选用单片机、DSP嵌入式系统；

传感器包括：检测机器人躯干6和底盘8俯仰倾角变化的倾角仪15、18，倾角速度变化的陀螺仪16、19，检测电机5转角变化的编码器20，检测图像信息的摄像头12，以及检测障碍物距离信息的超声波传感器17；

柔性关节7的顶端和底端为上支承圆盘22和下支承圆盘23，剖面为“凸”字形，圆盘中心的通孔用于穿过电线；上、下支承圆盘通过有转动轴(24)的二连杆连接，转动轴24向外的延伸部分，各固定双臂扭簧25，扭簧25的上臂固定在上支承圆盘22上，扭簧的下臂固定在下支承圆盘23上；在上支承圆盘22和下支承圆盘23之间压紧固定一段圆柱弹簧26，其两端分别套在上、下支撑圆盘22、23的凸台外；

电气系统的主控制器1与运动控制器2、辅助控制器3、输入输出设备连接；运动控制器2与倾角仪15、18、陀螺仪16、19、编码器20、伺服驱动器4相连接；辅助控制器3与超声波传感器17和遥控接收器27连接；摄像头12与主控制器1；电源系统连接各电气设备供电。

前述的电源系统包括：充电电池28、转换电压的电源继电器板29、监测电池电压的电源监控板30。

前述的直流电机5选用直流力矩电机、带减速器的直流电机或直流无刷电机。

前述的输入输出设备包括：键盘31、鼠标32、遥控器33、液晶显示屏34、显示器35、麦克风36、扬声器37。

一种柔性两轮自平衡机器人系统的运动控制方法，包括：主控制器1接收来自输入设备的用户操作指令；主控制器1监测各传感器反馈信息；主控制器1定时参照用户操作指令和传感器反馈信息，通过运动行为决策算法计算机器人的直行和偏航速度控制命令，下达给运动控制器2执行；在一个运动控制周期内，运动控制器2读取机器人躯干6和底盘8的倾角仪15、18与陀螺仪16、19的反馈信号，与零位值比较得出倾角和倾角速度的误差信号；运动控制器2读取电机编码器20的反馈信号，计算机器人直行和偏航速度，与主控制器给定的控制命令对比得出误差信号；根据误差信号，运动控制器2按预定的运动平衡控制算法计算电机的控制量，发送给伺服驱动器4执行；伺服驱动器4控制电机5运动，电机5带动双轮10使机器人维持机身平衡以及按指定方式运动，

主控制器1接收的用户指令，可以是即时指令，如遥控器键入，也可以是预存指令；

主控制器1通过与运动控制器2通信间接获取倾角仪15、18、陀螺仪16、19和编码器20的反馈信息，通过与辅助控制器3通信间接获取超声波传感器17反馈信息和遥控器33输入的指令；

所述主控制器1的运动行为决策算法为：动作发生器参考用户命令或超声波测距信息计算出期望直行和偏航速度控制命令，决策器根据机器人姿态平衡的状况判断是否执行期望控制命令；若姿态平衡状况良好，则期望控制命令即为实际控制命令；若姿态平衡状况不佳，则实际控制命令为零，即先调整机器人平衡；

运动控制器2通过模数A/D转换多次采集倾角仪15、18和陀螺仪16、19 的输出信号，并经过均值滤波等信号处理算法形成一个控制周期计算所用的反馈信号；

所述运动控制器2的运动平衡控制算法为：直行速度控制器K₁根据姿态误差信号和直线运动速度误差计算出直线速度控制量u₁，偏航速度控制器K₂根据偏航速度误差计算出的偏航速度控制量u₂，[u₁ u₂]^T左乘耦合矩阵D计算出电机控制转矩[τ₁ τ₂]^T；耦合矩阵D为二阶常数矩阵

[\begin{matrix} a & a \\ a & - a \end{matrix}] .

伺服驱动器4通过电机电枢电流反馈闭环，实现电机5的转矩控制。

前述的预存指令包括；程序存储的动作顺序表、动作指令、任务式指令。

前述的动作发生器的设计方法包括查表、动态规划、专家系统等算法。

前述的决策器的设计方法包括查表、模糊逻辑、专家系统等算法。

前述的直线运动速度控制器和偏航速度控制器的设计方法包括：闭环极点配置、线性二次型最优控制、鲁棒控制、模糊控制、自适应控制等各种适合算法；

本发明与现有技术相比，具有以下优点：

第一，本发明作为一种智能机器人，兼有移动式机器人的离散时间动作决策问题和倒立摆的连续时间姿态控制问题，因此可作为机器人学和控制科学领域交叉的综合研究对象，满足多学科研教的需要。

第二，本发明在保留两轮自平衡机器人的原有优点基础上，具有新的应用价值，是一种与工程实际问题联系紧密，实用价值较高的研究、示教设备。在一些实际应用中，如火箭发射，存在重心在支点之上的控制问题，而这类对象并不是限定在直线或圆形轨道上运动的，因此轨道式倒立摆对这类问题的模拟具有局限性。两轮自平衡机器人(移动式倒立摆)则更接近实际控制问题。本发明具有双轮行走机构，延续了两轮自平衡机器人上述优势。此外，在实际应用中，某些仿人两轮机器人的腰部关节以及两轮电动车的驾驶员腰部都可看作柔性关节而抽象为本发明所述结构，因此本发明可作为研究这类特定系统控制问题的理想对象。

第三，本发明改进了柔性关节的机械结构，使得它在应用中更加合理、实用。首先，引入单自由度旋转铰，将机器人柔性关节平衡的控制目标限定在俯仰自由度，从而保证系统的可控性。另外，在柔性关节高度不变的情况下，圆柱弹簧材料直径和圈数可调范围十分有限，制约了柔性关节刚度值的调节范围，而扭簧横卧的安装方式使得弹簧材料直径的选择余地大大增加，引入对称扭簧则能扩大柔性关节的刚度调节范围，使得机器人作为研究对象时具有更丰富的特性。

第四，本发明中的柔性关节中弹簧的动态特性是非线性的，而弹簧材料的阻尼、疲劳以及关节的摩擦带来系统参数的不确定性，这使得系统的非线性和不确定性增加，更加适合非线性控制、鲁棒控制和智能控制的研究。

第五，本发明的电气系统采用层式结构，各级控制器各司其职，使得机器人的处理性能更加强劲，能够有效降低应对复杂控制算法时的计算负担，这十分符合该机器人作为多用途研究平台的定位。此外，分层结构使得硬件系统具有良好的扩展能力。

第六，本发明的电气系统既有PC系统，也包括嵌入式系统，而且在其PC系统中能直接开发各嵌入式系统的应用软件。这样，用户在连接鼠标、键盘、显示器等I/O设备后，在机器人系统上直接开发多种硬件系统的程序，既方便了开发，又能够实践多种硬件体系的软件开发过程，使得机器人适用于多门课程的教学实践环节。

第七，本发明的运动控制方法能够实现柔性两轮自平衡机器人的姿态平衡控制和运动速度控制，使得用户能在此基础上进一步开发其他高级控制功能。

第八，本发明配备遥控系统和避障系统，使得机器人具有遥控操作和自主避障运行的双重模式。

第九，本发明配备视觉传感系统和语音处理系统，使得机器人可扩展视觉识别、语音识别功能。

附图说明

图1柔性两轮自平衡机器人的机械结构主视图；

图2柔性两轮自平衡机器人的电子元件布局示意图；

图3柔性两轮自平衡机器人的机械结构侧视图；

图4柔性两轮自平衡机器人底盘的俯视图；

图5柔性两轮自平衡机器人的柔性关节结构主视图；

图6柔性两轮自平衡机器人的柔性关节结构侧视图；

图7柔性两轮自平衡机器人的柔性关节俯视图；

图8柔性两轮自平衡机器人的电气系统连接示意图；

图9柔性两轮自平衡机器人的电气系统原理图；

图10柔性两轮自平衡机器人的运动控制系统框图；

图11主控制器监控软件串口接收消息处理函数流程图；

图12主控制器监控软件定时器消息处理函数流程图；

图13运动控制器主程序流程图；

图14运动控制器定时中断控制程序流程图；

图15运动控制器SCI接收中断程序流程图；

图16柔性两轮自平衡机器人的速度控制仿真曲线；

图17柔性两轮自平衡机器人速度控制中姿态角变化仿真曲线；

图18辅助控制器超声波测距控制函数流程图；

图19辅助控制器遥控指令检测控制函数流程图；

图20辅助控制器主程序流程图。

具体实施方式

下面结合附图，介绍本实施例。

一、柔性两轮自平衡机器人的硬件

1.电气系统选型

主控制器1选用研祥嵌入式计算机(EPC)EC5-1717CLDNA。该系统是一款采用Intel915GM芯片组的高性能单板电脑，板载1.6GHz Pentium M处理器，256MB D DR II 533内存，集成显卡、声卡，提供各种接口。EPC的硬盘存储器38为东芝抗震硬盘MK4036GAC，存储容量40GB，工作中可抗最大2.0G振动及200G瞬时冲击。

运动控制器2选用飓风公司MSK2812系统板。该系统的处理器采用TI公司TMS320F2812DSP，系统为5V直流供电。

MSK2812的仿真器39选用飓风公司的XDS510USB，USB2.0接口。

辅助控制器3选用凌阳科技公司的SPCE061A系统板，系统为5V直流供电，处理器为16位u′nSPTM微控制器，板载麦克风36，可实现语音识别功能。

伺服驱动器4选用Copley Motion公司的AJC55-18。AJC55-18可用于位置，转速和扭矩控制，外部控制器可以通过模拟方式(±10V，扭矩，速度，位置)，PWM(扭矩，速度)，以及极性PWM方式(扭矩，速度)对目标电机进行伺服控制。AJC55-18可通过串口与PC系统通信，设定工作模式和控制器参数。

液晶显示屏17选用台湾友达光电公司的B084SN0，3.3v供电，分辨率800*600。接口包括LVDS端口，背光灯电源线。液晶显示屏的背光升压板(40)选用三菱公司的T511024.04，5V直流供电。

扬声器37选用一对功率为2W的无源扬声器单元。

倾角仪15、18选用Crossbow公司的CXTA-01。它主要采用高稳定性的硅微机械电容倾角传感器，以模拟信号方式输出倾斜角度。CXTA-01的测量范围为±75°，分辨率为0.05°，带宽为125Hz，8-30V直流供电，输出电压0-5V，零位电压2.5V。

陀螺仪16、19选用Silicon Sensing Systems Japan公司的CRS03。CRS03角速度传感器基于MEMS技术制造，在剧烈冲击和震动条件下仍能保持卓越的性能，温漂小并具有良好的重复性，主要参数为：量程±100°/s，直流5V供电，零位电压2.5V，测量分辨率20mV/°/s，带宽为10Hz。

选用四个Parallax公司的PING)))超声波传感器17组成的阵列作为避障传感器。PING)))为5V直流供电，具有一个超声波发射模块和一个接受模块，通过测量超声波脉冲遇到物体反射后的回波传播时间来测量距离。PING)))的测量范围2cm-3m，分辨率＜1cm。

机器人视觉传感器选用USB接口CMOS摄像头12，130万像素。

遥控系统包括遥控器33和遥控接收器27，分别为3V和5V直流供电，遥控距离20米。遥控器33有12个按键，当有键按下时，遥控接收器27的4个IO口将输出不同的电平组合。

机器人的驱动电机5选用Maxon公司的直流电机RE40套件，电机为24V供电，额定功率150W，配备26∶1的行星齿轮减速器，最大输出转矩7.5Nm，电机后端配有增量式光电编码器20，精度为1000线。

机器人的电源为24V 9Ah镍氢充电电池28。29为电源继电器板，OP-DC01上的电压转换模块将24V直流输入转为5V直流输出，用于相关设备供电。定制直流24V输入的ATX电源适配器41，输出ATX/AT接口电源，为主控制器1、硬盘存储器38、背光升压板40供电。电源监控板30OP-DY01上的单片机在检测到电压低于22.5V时，控制蜂鸣器报警。

2.机械结构与电气元件布局

本实施例总重量20kg，高度700mm，宽度400mm，长度150mm，轮子直径250mm。机器人的机械结构和电器元件布局如下：

如图1、2、3所示，躯干部分6为铝合金框架，分为上、中、下三层。躯干6的上层为前后敞口的中空屉框，内部固定主控制器1，顶板11之上通过中空的圆柱形支架42固定摄像头12。躯干6的中层由平行底面的三块亚格力隔板14隔为四小层，其中，最上一层安装硬盘存储器38、辅助控制器3和遥控接收器27，第二层安装ATX电源适配器41，第三层安装电源继电器板29和电源监控板30，第四层安装背光升压板40。躯干的中层的四周装有亚格力面板，用以保护内部电子元件以及防止中间隔板14脱落，其中前面板43上固定着液晶显示屏34和扬声器37。躯干的下层为前后敞口的中空屉框，内部装载倾角仪15和陀螺仪16。底层框架前端固定一个面朝前的带状支架44，用于安装四个超声波传感器17。

躯干底板13和底盘8顶端面之间以柔性俯仰关节7连接。柔性关节7的结构如图5、6、7所示：顶端和底端为上、下支承圆盘22、23，剖面为“凸”字形，圆盘中心开通孔用于电线穿过。在上支撑圆盘凸台面上，以沉头螺钉45固定上支座46。上支座46的方形底板中央开通孔，与支撑圆盘通孔孔径相同，装配时两孔同心。上支座46的底板两侧有竖起的圆顶角剑头形支座壁，其上开有同心的通孔。在下支撑圆盘23的相同位置以相同方式安装下支座47，下支座47与上支座46不同的是其两个侧壁均为双层，令上支座46侧壁可插入间隙紧密配合，另外，下支座壁47的通孔直径略小。上支座46两侧壁上的通孔中分别插入铜套48后与下支座47的侧壁通孔直径相同，上、下支座46、47装配时两侧壁通孔同心，在其中分别插入转动轴24，并以挡板49固定，使得上支承圆盘22和下支承圆盘23能够以转动轴24为铰接点转动。两个转动轴24外侧各穿过一个双臂扭簧25，扭簧25的上臂固定在上支承圆盘22的卡槽中，扭簧的下臂末端的圆勾通过螺栓50固定在下支座47的侧壁上。在上、下支座46、47装配时，上、下支撑圆盘22、23之间压紧固定一段圆柱弹簧26，其两端分别套在上、下支撑圆盘22、23 的凸台外。

如图2、4所示，机器人的底盘部分8为铝合金制箱式结构。在底盘左右两侧的中心位置各装有一组轴系，主要包括转轴51、轴承52、轴承座53等。在底盘8内部中间位置，左右轮电机5前后错开固定在齿轮传动箱54外，电机的轴通过传动齿轮55与轴系的转轴51联接。使用螺栓56将轴系的转轴53与轴辖57连接，两者之间夹持固定轮子10。在底盘8内，运动控制器2和仿真器39安装在前侧，电机伺服控制器4安装在后侧，倾角仪18、陀螺仪19和电池28安装在底部。底盘8底面有两个可拆卸保护支架9，支架末端的脚轮21在机器人直立时悬空，倾倒时脚轮21触地起到支撑保护作用。

3.电气系统连接

如图8所示，电气系统各部分的连接方法如下：

EC5-1717CLDNA与硬盘存储器38连接组成主控制器1；在EC5-1717CLDNA面板接口FP1的1、2脚之间串入触点开关58作为它的启动按钮。ATX电源适配器41连接EC5-1717CLDNA为其供电。

EC5-1717CLDNA的四个RS232串口COM1、COM2、COM 3、COM4分别与MSK2812、SPCE061A、两个ACJ55-18的串口连接。此外，EC5-1717CLDNA通过USB接口连接DSP的仿真器39XDS510USB，仿真器39再与DSP的JTAG接口连接。

EC5-1717CLDNA的USB1接口与摄像头12连接，SPK接口与扬声器37连接，LVDS接口与液晶显示屏34连接。背光升压板40的一端连接液晶显示屏34的背光灯电源线，另一端的1、2、3、4线与ATX电源适配器41的直流5V输出连接，5、6线与ATX电源适配器41的GND相连。

另外，在机器人停止运动时，EC5-1717CLDNA可以连接键盘31、鼠标32和显示器35，进行程序编写与调试。

MSK2812板由电源继电器板29的+5V输出供电，它的J7接口的38，37，36，35脚，即A/D转换输入通道，分别与两个倾角仪(CXTA01)15、18和两个陀螺仪(CRS03)16、19的模拟信号输出端连接；两个陀螺仪16、19分别由MSK2812的J7接口的1和7脚，即+5V输出供电；倾角仪15、18则由电源继电器板的24V输出直接供电。

MSK2812与两个伺服控制器(AJC55-18)4间的连接包括控制信号线和编码器反馈信号线。控制信号包括电机使能信号、电机转动方向信号和PWM转速控制量信号。其中，MSK2812的J5接口的3、7脚分别与控制左、右电机的AJC55-18的J5接口的3脚连接，作为AJC55-18的使能信号线；MSK2812的J5接口的5、1脚分别与控制左、右电机的AJC55-18的J5接口的6脚连接，作为电机5转动方向选择信号线；MSK2812的J7接口17、18脚为PWM输出，分别与控制左、右电机5的(AJC55-18)4的J5接口的20脚连接，作为转速控制量信号线。左、右电机编码器20的反馈信号经AJC55-18缓存后连接至MSK2812，具体接线为左、右电机AJC55-18的J5接口的10、11脚，分别接MSK2812的J7接口的27、28脚和J6接口的13、14脚。

两个AJC55-18的J3接口的3、4脚为电源输入端，分别接电源继电器板29的+24V和GND；J2接口的3、4脚为控制电压的输出端，分别与电机的+/-输入端连接，其中3脚与电机+输入端之间串接一个电机开关59；J4接口的4、6分别为+5V和GND，分别与编码器20排线的2、3线连接，J4接口的1、8、2、9、3、10脚为编码器A通道、B通道和零位信号的共模输入端，分别接编码器排线的5、6、7、8、9、10线。

SPCE061A由电源继电器板29的+5V输出供电，它的J1接口的2～5针，分别连接超声波传感器17，1#～4#的Signal端；J2接口2～5针，分别连接遥控接收器27的输出D0～D3。

两组24V4.5Ah的镍氢电池组并联组成24V9Ah电池28。电池28经一个双刀双掷的船型开关60分别与电源继电器板29和充电器插口61连接。当船型开关60从“关”拨到“开”时，电池给开始供电，拨到“充电”时，外接直流充电器62为电池充电。

电源继电器板29的+/-输入端连接电池组28的+/-端，提供24V和5V两种直流输出，分别连接各对应设备的供电端。

电源监控板30的Vd、GND和Vin分别接电源继电器板29的+5V、GND和+24V。

4.电气系统的工作原理

本实施例机器人的主要功能是在保持机身俯仰姿态平衡的前提下，能够进行行走、转弯等运动，这种运动可以是机器人按照行为决策规则自主智能行为，也可以是按照用户即时下达的操作指令运动。由此，机器人电气系统的工作原理如图9所示：机器人的主控制器1经串口从运动控制器2和辅助控制器3读取各个传感器反馈信息和遥控操作指令并在液晶屏34上显示，然后按照运动行为决策算法计算出机器人的运动控制命令，并通过串口下达给运动控制器2；运动控制器2由模数转换模块得到倾角仪15、18、陀螺仪16、19的反馈信号，经过伺服驱动器4中继读取编码器20反馈信号，然后，综合接收到得控制命令和反馈信号，按预定的运动平衡控制算法计算出电机的转矩控制量，发送对应的PWM信号给伺服驱动器4执行；伺服驱动器4控制电机5运动，电机5带动轮子10使机器人维持机身平衡以及按主控制器1控制命令运动。

二、机器人系统的运动控制方法

本实施例给出柔性两轮自平衡机器人自主避障和遥控操作两种运动模式控制系统的软件实现，其他如机器人智能行为决策、语音操作等控制功能可以参考本实施例软件，添加相应模块进行设计。

图10为控制系统的框图，功能为实现机器人的两种运动模式，一为自主避障运动模式，二为遥控操作运动模式。其中，运动模式通过遥控器按键选择；机器人的运动以动作集合描述，包含：前进、后退、左转弯、右转弯、静止和左、右原地旋转；通过控制机器人的直行速度V_x和偏航速度ω_α来完成动作，控制命令如下表所示：

前进

后退

左转弯

右转弯

原地左转

原地右转

静止

V_x ^c

v

-v

v/2

0

ω_α ^c

0

-2v/D

2v/D

-2v/D

2v/D

0

表中v的值取0.1m/s，D为两轮轮心距，取0.4m。

整个控制系统由四部分软件实现，分别运行在主控制器1、运动控制器2、辅助控制器3和伺服驱动控制器4中。

1.主控制器1中的监控程序

主控制器1中的监控程序RobotControl.exe为Windows系统应用程序，采用 Visual C++软件开发。该程序可实时监测遥控命令(遥控输入键值R^c)与机器人运行状态(直线速度V_x、旋转角速度ω_α、底盘和躯干的姿态角θ₁和θ₂、姿态角速度

和

超声波传感器测距信息D_S1-D_S4)，并实现机器人运动行为决策。程序的核心部分为串口通信和运动行为决策。

串口通信的内容为主控制器1向运动控制器2下达控制命令，以及运动控制器2和辅助控制器3向主控制器1上传机器人状态信息。通信方式为异步串行通信，通信协议为：串口传输波特率19200，每帧8位数据位，无奇偶校验，1个停止位。主控制器1下达给运动控制器2的控制命令数据包的格式为：<F0h，F0h，V，数据，W，数据，数据包长度值，FFh，FFh>。其中，数据包长度值为整型变量，字符“V”代表机器人直行速度控制命令V_x ^c，字符“W”代表机器人偏航速度控制命令ω_α ^c，“数据”为他们对应的浮点型变量值。

主控制器1接收的状态信息数据包的格式为：<F0h，F0h，数据代码，数据，数据代码，数据，......，数据包长度值，FFh，FFh>。其中，数据包长度值为整型变量，数据代码与其对应的数据名称如下表所示：

表中除R^c以外其他数据类型均为浮点型，R^c为遥控指令代码，代码的值及其对应遥控器按键和所定义功能如下表所示：

代码

00h

01h

02h

03h

04h

05h

06h

07h

08h

09h

0Ah

0Bh

按键

1

2

3

4

5

6

7

8

9

10

11

12

功能

预留

自主避障模式

遥控操作模式

前进

后退

左转弯

右转弯

原地左转

原地右转

静止

预留

本例采用开源的串口类CSerial实现串口操作，相关代码和说明可在 http://codegure.earthweb.com/network/serialpot.shtml下载。

图10中标出机器人运动行为决策控制的框图，其具体算法为：运动发生器查询当前运行模式参数值(缺省为遥控操作模式)，若为自主避障运行模式，则查询最新的超声波测距值，依照预置的行为规则表选择动作，若为遥控操作模式，则程序查询遥控输入指令直接从运动集合中选择相应的动作(缺省动作为静止)；随后将选定动作对应的期望直行和偏航速度控制命令V_x ^d、ω_α ^d输出到决策器；决策器读取最近10次接收的底盘和躯干的姿态角θ₁和θ₂、姿态角速度

和计算它们的均方值Δ₁、Δ₂、Δ₃、Δ₄，若((Δ₁＞25)∧(Δ₂＞25))∨((Δ₃＞2500)∧(Δ₄＞2500))为真，则认为机器人姿态不稳定，决策器输出控制命令为

V_{x}^{c} = ω_{α}^{c} = 0,

即令机器人保持静止，若((Δ₁＞25)∧(Δ₂＞25))∨((Δ₃＞2500)∧(Δ₄＞2500))为假，决策器输出的控制命令为

V_{x}^{c} = V_{x}^{d},

ω_{α}^{c} = ω_{α}^{d} .

其中，机器人自主避障运行模式的行为规则与超声波传感器17反馈的障碍物距离信息直接相关，具体为下表所示：

编号	S1	S2	S3	S4	动作	编号	S1	S2	S3	S4	动作
												1	1	1	1	1	原地左转	9	0	1	1	1	向左转弯
2	1	1	1	0	向右转弯	10	0	1	1	0	向左转弯
												3	1	1	0	1	向右转弯	11	0	1	0	1	向左转弯
4	1	1	0	0	向右转弯	12	0	1	0	0	向右转弯
												5	1	0	1	1	向左转弯	13	0	0	1	1	向左转弯
6	1	0	1	0	向右转弯	14	0	0	1	0	向左转弯
												7	1	0	0	1	向左转弯	15	0	0	0	1	向左转弯
8	1	0	0	0	向右转弯	16	0	0	0	0	前进

表中S1-S4代表机器人身前从左至右的四个超声波传感器17，当S1所测距离值D_S1小于1米时，S1＝1，否则S1＝0，S2-S4同理赋值。

监控程序的流程为：程序启动后，主线程初始化串口COM1、COM2(主要设定波特率19200、8位数据位、无奇偶校验、1位停止位、缓冲区512字节等)，创建并启动它们各自的串口监测线程，设置1000ms定时器Timer，最后在无限循环中处理消息队列中消息。当COM1或COM2收到一个字节时，串口监测线程将该字节以消息方式传递给主线程，主线程响应该消息后运行对应的消息处理函数OnCommunication1或OnCommunication2(程序流程如图11所示)，函数中将收到的字节读入缓冲区，判断此字节是否为数据包结尾，若否，退出消息处理函数，若是，则校验该数据包长度值是否正确，若不正确，则放弃该数据包，清空缓冲区，退出消息处理函数，若正确，则按串口通信协议解析该数据包，取出状态数据保存至存储区，更新视窗中对应的显示信息，清空缓冲区，退出消息处理函数。Timer计时满1000ms向主线程发送消息，主线程响应该消息后运行消息处理函数OnTimer(程序流程如图12所示)，函数中读取存储区用户指令和机器人的状态信息，根据运动行为决策算法计算出控制命令V_x ^c、ω_α ^c；如果控制命令与上一次定时程序计算结果不同，则通过串口COM1向运动控制器(2)发送新的控制命令后退出函数，若相同，则直接退出函数。

2.运动控制器2中的控制软件

运动控制器2的程序采用TI公司的CCS软件开发，并固化在存储单元中。程序以SCI接收中断的方式接收主控制器1控制命令，以25ms定时中断的方式实现机器人姿态平衡和运动的实时控制，并在每次定时中断后向主控制器1发送状态数据。其中，定时中断的优先级高于SCI接收中断优先级。本实施例给出主程序和中断程序的算法流程，涉及到DSP的资源配置、使用，串口的接收、发送的操作等的具体实现，可直接参考TI公司为TMS320F2812 DSP提供的例程。

主程序流程如图13所示：首先进行必要的初始化，完成程序使用的变量的初始化和配置DSP各寄存器状态，主要配置控制中所需的IO端口和A/D通道、通用定时器T1-T4(设置并开启T1为中断周期计时器，T2、T4为编码器的计数器，T3为PWM信号输出的比较计时器)、SCI(设定波特率19200、8位数据位、无奇偶校验、1位停止位、使能收发FIFO缓冲等)。然后，开启PWM输出，向伺服驱动器发送使能信号；接着，使能SCI接收端口RX、T1定时中断、SCI接收中断；最后，进行无限循环等待中断到来，在每次循环中，判断定时中断程序运行标志变量Flag是否为1，若否则无操作，若是则将θ₁、

θ₂、

V_x、ω_α的数值按照串口通信协议经串口发送至主控制器1，随后将Flag置为0。

当SCI的接收FIFO缓存收到一个字节时产生中断，CPU响应该中断后，保存当前程序现场，转入SCI接收中断程序。如图14，SCI接收中断程序的流程为：首先，关闭SCI接收中断；第二步，从接收FIFO缓存中读取一个字节；第三步，判断该字节是否为数据包末尾，若否，存储该字节，返回第二步读取下一个字节，若是，则判断该数据包长度是否正确，若不正确，则放弃该数据包，执行下一步，若正确，则解析该数据包数据，取出V_x ^c和ω_α ^c的数值并存储；第四步，清空接收FIFO缓存；最后，打开SCI中断，退出中断程序。

每当T1计时满25ms时产生中断，DSP响应该中断，保存当前程序现场，转入定时中断程序。如图15所示，定时中断程序的流程为：

Step1：关闭定时器T1中断。

Step2：获取编码器信息。每个编码器输出信号为两组正交编码序列，DSP中的正交编码脉冲电路对这两组信号的上升沿和下降沿均进行计数，因此产生的时钟频率是每组输入序列的四倍，所以读取通用定时器T2、T4的计数器数值后，需除以四才能得到一个周期内编码器输出的脉冲数n_l，n_r。

Step3：计算机器人的累积直线位移、累积偏航角度、直线速度和偏航角速度。由编码器信息可知一周期内每个轮子转过的角度

由于控制周期T很短(25ms)，可近似计算轮子转速根据机器人运动学方程，其直线速度V_x＝(ω_l+ω_r)·R_w/2，转弯速度ω_α＝2(ω_l-ω_r)·R_w/D，其中

为左轮转速，

为右轮转速，D为两轮轮距，R_w为轮子半径。由于控制周期T很短，因此将V_x与T相乘累加到上个控制周期的累积直线位移得到当前累积直线位移，即x(t)＝x(t-T)+V_xT，用同样的方法计算累积偏航角度，即α(t)＝α(t-T)+ω_αT。

Step4：按照底盘的倾角仪18、陀螺仪19，躯干的倾角仪15、陀螺仪16的顺序，对它们输出的模拟电压信号分别进行A/D转换。为避免采样过程中偶然因素的影响，每个信号均连续采样10次，进行A/D转换，去掉其中最大值和最小值后求均值赋给对应的变量：

Step5：计算底盘和躯干的倾斜角度和倾斜角速度。通过公式：

θ_{i} = (U_{θ_{i}} - U 0_{θ_{i}}) K_{θ}, i = 1,2,

{\overset{\cdot}{θ}}_{i} = (U_{{\overset{\cdot}{θ}}_{i}} - {U 0}_{{\overset{\cdot}{θ}}_{i}}) K_{\overset{\cdot}{θ}}, i = 1,2,

计算出对应机器人底盘和躯干的倾斜角度和倾斜角速度。其中，U0_x为设定的零位电压值，为2.5V；K_θ＝8，

K_{\overset{\cdot}{θ}} = 9

为比例系数，是预先通过固定机器人双轮，摆动机身，将编码器测得的底盘转角、转速分别除以

和获得。

Step6：以控制命令V_x ^c、ω_α ^c为参考输入，以检测到的x、V_x、α、ω_α、θ₁、

θ₂、为反馈量，通过运动平衡控制算法计算出两个电机控制转矩对应PWM的占空比和转动方向。其中，转矩绝对值范围[0，5](Nm)，对应占空比为0～100％，大于等于零时为正转，小于零时为反转。

Step7：刷新输出至伺服驱动器的PWM占空比和电机转动方向。

Step8：设置计时器T1，重新开始25ms计时，设置T2、T4，重新开始计数。

Step9：开T1中断，退出中断程序。

Step6中提到的运动平衡控制算法为：如图10所示，直行速度控制器K₁根据姿态零位误差和直行速度误差计算出直线速度控制量u₁，偏航速度控制器K₂根据偏航速度误差计算出的偏航速度控制量u₂，[u₁ u₂]^T左乘耦合矩阵D计算出电机控制转矩[τ₁ τ₂]^T；直行速度控制器和偏航速度控制器的设计选用线性二次型最优控制算法：

首先，将机器人的线性状态方程分解为直行运动和偏航运动两个单输入子系统。根据机器人的机械系统的特点和参数，建立其数学模型，在直立平衡点线性化获得线性状态空间方程：

\overset{\cdot}{X} = AX + Bu

其中，

X = {[\begin{matrix} x & α & θ_{b_{1}} & θ_{b_{2}} & \overset{\cdot}{x} & \overset{\cdot}{α} & {\overset{\cdot}{θ}}_{b_{1}} & {\overset{\cdot}{θ}}_{b_{2}} \end{matrix}]}^{T},

u＝[τ_l τ_r]^T，

A = [\begin{matrix} 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 \\ 0 & 0 & 113.8 & - 135.9 & 0 & 0 & 1.4 & - 1.4 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & - 123.58 & 137.77 & 0 & 0 & - 14.2 & 14.2 \\ 0 & 0 & 100.83 & - 102.07 & 0 & 0 & 10.9 & 10.9 \end{matrix}],

B = [\begin{matrix} 0 & 0 \\ 0 & 0 \\ 0 & 0 \\ 0 & 0 \\ 1.5726 & 1.5726 \\ 3.3755 & - 3.2755 \\ - 9.1867 & - 9.1867 \\ 2.4738 & 2.4738 \end{matrix}] .

可得直行运动子系统的状态方程：

{\overset{\cdot}{X}}_{1} = A_{1} X_{1} + B_{1} u_{1}

其中，

X_{1} = {[\begin{matrix} x & θ_{b_{1}} & θ_{b_{2}} & \overset{\cdot}{x} & {\overset{\cdot}{θ}}_{b_{1}} & {\overset{\cdot}{θ}}_{b_{2}} \end{matrix}]}^{T},

A_{1} = [\begin{matrix} 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 \\ 0 & 113.8 & - 135.9 & 0 & 1.4 & - 1.4 \\ 0 & - 1.2358 & 1.3777 & 0 & - 14.2 & 14.2 \\ 0 & 1.0083 & - 1.0207 & 0 & 10.9 & 10.9 \end{matrix}],

B_{1} = [\begin{matrix} 0 \\ 0 \\ 0 \\ 1.5726 \\ - 9.1867 \\ 2.4738 \end{matrix}]

可得偏航运动子系统的状态方程：

{\overset{\cdot}{X}}_{2} = A_{2} X_{2} + B_{2} u_{2}

其中，

X_{2} = {[\begin{matrix} α & \overset{\cdot}{α} \end{matrix}]}^{T},

A_{2} = [\begin{matrix} 0 & 1 \\ 0 & 0 \end{matrix}],

B_{2} = [\begin{matrix} 0 \\ 3.3755 \end{matrix}] .

u₁、u₂与u的关系为：

u = D \cdot [\begin{matrix} u_{1} \\ u_{2} \end{matrix}] = [\begin{matrix} 0.5 & 0.5 \\ 0.5 & - 0.5 \end{matrix}] [\begin{matrix} u_{1} \\ u_{2} \end{matrix}] .

其次，通过求得直行运动子系统与偏航运动子系统的能控性矩阵均满秩，证明两者完全能控，系统各状态值又均可测量获得，因此以V_x ^c、ω_α ^c分别为两个子系统参考输入，状态X₁、X₂为反馈量，构造如图10中标出的运动平衡控制系统，并采用线性二次型最优控制方法设计两个子系统的状态反馈控制器：

定义两个子系统的性能指标：

其中Q₁＝diag[1 100 100 100 10 10]、Q₂＝diag[1 1]为状态变量的加权矩阵，R₁＝R₂＝1为控制量的加权系数。通过Matlab程序的函数K＝lqr(A，B，Q，R)可求得反馈控制律u₁ ^*＝-K₁X₁，u₂ ^*＝-K₂X₂，分别使得性能指标极小。

K₁＝[-2.2361 -137.7072 35.3820 -23.769 -14.9711 -8.8326]

K₂＝[2.2361 22.3755]

机器人跟踪脉宽直线、旋转速度信号的曲线如图16所示。图17中曲线为调速过程中机器人底盘、躯干部分倾角的变化。

3.辅助控制3中的软件

辅助控制器3的软件在凌阳公司的μ’nSP^TM集成开发环境中编写，通过串口下载并固化到它的存储器中。软件的功能是控制超声波传感器17测距，检测遥控输入指令，并将结果发送给主控制器1。其工作流程如图20所示：程序开始时先进行必要的初始化，如完成程序中各个变量的初始化，设置串口、IO端口和定时器TimerA参数等；然后开始无限循环，每次循环中按照超声波传感器的编号顺序，循环调用GetDistance函数，控制超声波传感器测量与前方障碍物的距离并存储，待4个传感器均测量完毕，将所有测量结果按照通信协议通过串口发送给组织级EPC。在两个超声波传感器工作的间歇，调用GetRemoteKey函数读取遥控接收器的信息，根据返回结果更新遥控器指令代码，并按照通信协议经串口发至组织级EPC。

如图18所示，GetRemoteKey函数的流程为：设置IOA低八位为输入，将IOA低八位输入值存储至Key_temp变量，延时10ms，将IOA低八位输入值与Key_temp比较，若相同，则说明遥控按键确实触发，返回Key_temp值，若不同，则说明遥控按键没有触发，返回Null。

如图19所示，GetDistance函数的流程为：设置IOA高八位为输出，向第i号超声波传感器对应的IO引脚IOA.i发送一个5us脉冲；设置IOA为高八位为输入，等待IOA.i有上升沿到来，启动计时器TimerA；当IOA.i下降沿到来，停止计时，读取计时数据赋给变量SonarTime，按照公式Distance＝(SonarTime*0.01050917)/2计算出测距结果并返回。

4.伺服驱动器4的软件

在AJC55-18配套PC端软件COPLEY MOTION2中，通过输入直流电机(5)参数能够自动计算伺服程序的比例积分(PI)调节参数，简单设置控制系统结构后即可自动生成伺服控制程序，并通过串口下载固化至AJC55-18的存储单元中。该伺服程序控制周期1ms，以DSP输出的PWM信号和转向信号分别为电机参考转矩和参考转向，取电枢电流为负反馈，实现电机转矩PI伺服控制。

三、机器人系统的使用

使用本实施例的机器人时，可按如下步骤操作：

1.确认机器人的电气系统连接正确，为机器人连接鼠标32。

2.将主电源的船型开关60拨至“开”。

3.按下触点开关58启动主控制器1EC5-1717CLDNA。

4.运行RobotControl.exe。

5.断开鼠标32，将机器人扶稳至直立位置。

6.打开电机开关59，机器人开始平衡控制。

7.待机器人在直立位置上稳定后，不再扶持。

8.通过遥控器33按钮选择机器人运行模式或动作指令操作机器人运动。

9.停机时，通过遥控器33令机器人静止，扶稳机器人后依次关闭电机开关59、RobotControl.exe、EC5-1717CLDNA、主电源船型开关60。

Claims

1.一种柔性两轮自平衡机器人系统，包括电气系统和机械本体；其中，所述电气系统包括主控制器(1)、运动控制器(2)、辅助控制器(3)、电机伺服驱动器(4)、传感器、输入输出设备、直流电机(5)和电源系统；所述机械本体包括躯干(6)、柔性关节(7)、底盘(8)、保护支架(9)和双轮(10)；其特征在于：

所述躯干(6)为层式框架，顶板(11)上固定有至少一个摄像头(12)，顶板(11)和底板(13)之间由隔板(14)分隔成若干层，躯干安装第一倾角仪(15)、第一陀螺仪(16)和超声波传感器(17)；

所述躯干底板(13)和底盘(8)之间以柔性关节(7)连接；

所述底盘(8)为箱式结构，在其内部装载第二倾角仪(18)、第二陀螺仪(19)，两个直流电机(5)分别固定在底盘(8)左右两侧内壁上，直流电机(5)的轴一端与双轮(10)的轴联接，另一端接编码器(20)；底盘(8)底面安装有可拆卸保护支架(9)，支架(9)末端有悬空脚轮(21)；双轮(10)的轴心位于一条直线上；

所述柔性关节(7)的顶端和底端为上支承圆盘(22)和下支承圆盘(23)，剖面为“凸”字形，上支撑圆盘和下支撑圆盘的中心通孔用于穿过电线；在上支撑圆盘凸台面上，以沉头螺丝(45)固定上支座(46)，上支座(46)的方形底板中央开通孔，与支撑圆盘通孔孔径相同，装配时两孔同心，上支座(46)的底板两侧有竖起的圆顶角剑头形支座壁，其上开有同心的通孔，在下支撑圆盘(23)的相同位置以相同方式固定下支座(47)，下支座(47)与上支座(46)不同的是其两个侧壁均为双层，令上支座(46)侧壁可插入间隙紧密配合，另外，下支座(47)的壁通孔直径略小，上支座(46)两侧壁上的通孔中分别插入铜套(48)后与下支座(47)的侧壁通孔直径相同，上支座(46)和下支座(47)装配时两侧壁通孔同心，在其中分别插入转动轴(24)，并以挡板(49)固定，使得上支承圆盘(22)和下支承圆盘(23)能够以转动轴(24)为铰接点转动；转动轴(24)向外的延伸部分，各固定一个双臂扭簧(25)，扭簧(25)的上臂固定在上支承圆盘(22)上，扭簧(25)的下臂固定在下支承圆盘(23)上；在上支承圆盘(22)和下支承圆盘(23)之间压紧固定一段圆柱弹簧(26)，其两端分别套在上支承圆盘(22)和下支承圆盘(23)的凸台外；

所述主控制器(1)为嵌入式计算机系统；

所述运动控制器(2)选用数字信号处理器系统；

所述辅助控制器(3)选用单片机或数字信号处理器系统；

所述传感器包括：检测机器人躯干(6)和底盘(8)俯仰倾角变化的第一倾角仪(15)、第二倾角仪(18)，检测机器人躯干(6)和底盘(8)俯仰倾角速度变化的第一陀螺仪(16)、第二陀螺仪(19)，检测直流电机(5)转角变化的编码器(20)，检测图像信息的摄像头(12)，以及检测障碍物距离信息的超声波传感器(17)；

所述电气系统的连接关系为：主控制器(1)与运动控制器(2)、辅助控制器(3)、输入输出设备连接；运动控制器(2)与第一倾角仪(15)、第二倾角仪(18)、第一陀螺仪(16)、第二陀螺仪(19)、编码器(20)、伺服驱动器(4)相连接；辅助控制器(3)与超声波传感器(17)和遥控接收器(27)连接；摄像头(12)与主控制器(1)；伺服驱动器(4)与直流电机(5)连接；电源系统连接各电气设备供电。

2.根据权利要求1所述的一种柔性两轮自平衡机器人系统，其特征在于：所述电源系统包括：充电电池(28)、转换电压的电源继电器板(29)、监测电池电压的电源监控板(30)。

3.根据权利要求1所述的一种柔性两轮自平衡机器人系统，其特征在于：所述直流电机(5)选用直流力矩电机或直流无刷电机。

4.根据权利要求1所述的一种柔性两轮自平衡机器人系统，其特征在于：所述的输入输出设备包括：键盘(31)、鼠标(32)、遥控器(33)、显示器(35)、麦克风(36)、扬声器(37)。

5.一种柔性两轮自平衡机器人系统的运动控制方法，包括：主控制器(1)接收来自输入设备的用户操作指令；主控制器(1)监测各传感器反馈信息；主控制器(1)定时参照用户操作指令和传感器反馈信息，通过运动行为决策算法计算机器人的直行和偏航速度控制命令，下达给运动控制器(2)执行；在一个运动控制周期内，运动控制器(2)读取机器人躯干(6)和底盘(8)的第一倾角仪(15)、第二倾角仪(18)、第一陀螺仪(16)、第二陀螺仪(19)的反馈信号，与零位值比较得出倾角和倾角速度的误差信号；运动控制器(2)读取电机编码器(20)的反馈信号，计算机器人直行和偏航速度，与主控制器给定的控制命令对比得出误差信号；根据误差信号，运动控制器(2)按预定的运动平衡控制算法计算电机的控制量，发送给伺服驱动器(4)执行；伺服驱动器(4)控制直流电机(5)运动，直流电机(5)带动双轮(10)使机器人维持机身平衡以及按指定方式运动，其特征在于：

所述主控制器(1)接收的用户指令，是即时指令或预存指令；

主控制器(1)通过与运动控制器(2)通信间接获取第一倾角仪(15)、第二倾角仪(18)、第一陀螺仪(16)、第二陀螺仪(19)和编码器(20)的反馈信息，通过与辅助控制器(3)通信间接获取超声波传感器(17)反馈信息和遥控器(33)输入的指令；

所述主控制器(1)的运动行为决策算法为：动作发生器参考用户命令或超声波测距信息计算出期望直行和偏航速度控制命令，决策器根据机器人姿态平衡的状况判断是否执行期望控制命令；若姿态平衡状况良好，则期望控制命令即为实际控制命令；若姿态平衡状况不佳，则实际控制命令为零，即先调整机器人平衡；

运动控制器(2)通过模数(A/D)转换多次采集倾角仪(15、18)和陀螺仪(16、19)的输出信号，并经过均值滤波信号处理算法形成一个控制周期计算所用的反馈信号；

所述运动控制器(2)的运动平衡控制算法为：直行速度控制器K₁根据姿态误差信号和直线运动速度误差计算出直线速度控制量u₁；偏航速度控制器K₂根据偏航速度误差计算出的偏航速度控制量u₂；[u₁ u₂]^T左乘耦合矩阵D计算出电机控制转矩[τ₁ τ₂]^T；耦合矩阵D为二阶常数矩阵

伺服驱动器(4)通过电机电枢电流反馈闭环，实现电机(5)的转矩控制。

6.根据权利要求5所述的柔性两轮自平衡机器人系统的运动控制方法，其特征在于：所述的预存指令包括：程序存储的动作顺序表、动作指令、任务式指令。

7.根据权利要求5所述的柔性两轮自平衡机器人系统的运动控制方法，其特征在于：所述动作发生器的设计方法包括查表、动态规划、专家系统算法。

8.根据权利要求5所述的柔性两轮自平衡机器人系统的运动控制方法，其特征在于：所述决策器的设计方法包括查表、模糊逻辑、专家系统算法。

9.根据权利要求5所述的柔性两轮自平衡机器人系统的运动控制方法，其特征在于：所述直线运动速度控制器和偏航速度控制器的设计方法包括闭环极点配置、线性二次型最优控制、鲁棒控制、模糊控制、自适应控制算法。