基于S3C44BOX和SM5004的足球机器人运动控制系统
2008-07-09
作者：田 琦，张国良，陈林鹏，刘延飞
摘 要： Robocup中型组的足球机器人" title="足球机器人">足球机器人控制系统" title="控制系统">控制系统设计。描述了以S3C44BOX为主控芯片，SM5004为驱动芯片的底层运动控制" title="运动控制">运动控制模块的硬件结构，通过分析四轮全向机器人的运动学模型，使用模糊自适应PID控制算法完成对机器人的闭环控制。实践证明，以这种控制器作为ROBOCUP中型组机器人的底层运动控制系统" title="运动控制系统">运动控制系统，其速度、位置及控制精度等指标均达到了设计要求。
关键词： Robocup中型组 运动控制 ARM 四轮全向机器人 PID
1 足球机器人简述
机器人足球是人工智能领域与机器人领域的基础研究课题，是一个极富挑战性的高技术密集型项目。机器人足球比赛的设想首先是由加拿大不列颠哥伦比亚大学的教授Alan Mackworth在1992年的论文《On Seeing Robots》中提出的。如今，国际机器人足球联合会（FIRA）和机器人世界杯赛联合会（Robocup）已组织了各种形式的机器人足球竞赛，有力地促进了这一活动的发展。在竞赛足球机器人中主要有以下关键技术：图像采集与处理、路径规划、决策、运动控制、多传感器信息融合、多智能体协作等。
机器人足球由四部分组成：机器人小车系统、视觉系统、通信系统和计算机决策系统。机器人小车作为比赛的具体执行者，其性能优劣直接影响到策略水平的发挥。足球机器人底层控制系统的研究与开发是新型足球机器人小车设计任务中的重要组成部分。小车的性能主要由车体性能与车载嵌入式控制系统——足球机器人底层控制系统决定。当小车车体具有良好的运动性能后，小车的性能就由车载嵌入式系统决定。因此构建一个快速、安全、可靠的实时嵌入式系统是整个系统的关键。
2 东风—Ⅱ型机器人
东风—Ⅱ型机器人是由第二炮兵工程学院独立设计完成的Robocup中型组足球机器人，其视觉子系统采用了基于全景视觉的图像采集与处理，利用图像信息、惯性仪表与超声波传感器的信息融合为机器人自定位，上位机利用机器人的自定位以及球场上本方、对方和球的位置信息做出路径规划，并对下位机发出指令。下位机根据接收到的指令把机器人的速度分解到四个轮子上，并由运动控制芯片完成反馈控制，其结构如图1所示。
3 机器人的运动控制系统
本机器人的硬件系统由PC机和32位嵌入式微处理器、4轴运动控制芯片、驱动芯片、存储芯片以及接口芯片等组成。其中PC机作为上位机，主要完成图像处理、控制决策等任务；而机器人小车的运动控制主要由嵌入式系统和运动控制卡完成。系统框图如图2。
3.1 SM5004的主要功能
SM5004为深圳斯迈迪公司研制的DSP运动控制专用芯片，内嵌强大的软件插补和监控模块，能同时控制四个脉冲型伺服电机驱动器或步进电机驱动器，SM5004还有伺服电机反馈输入（编码器信号、在位信号和报警信号）、加减速驱动、软硬件限位、自动原点搜索、同步动作、输入信号滤波等功能。
在本系统中，SM5004完成的主要功能如下：
(1)接收CPU的命令和数据，并进行相应的驱动操作。SM5004根据接收到的命令，输出相应的脉冲串，控制四个伺服电机并分别进行定速控制。
(2)反馈控制。SM5004每个轴都有2个32位位置计数器。一个是逻辑位置计数器，用于计算实际输出的脉冲数，以此记录驱动所处的逻辑位置；另一个是实际位置计数器，用于计算从外部编码器来的脉冲，以此记录驱动实际位置。每轴还有两个32位比较寄存器，用于比较逻辑位置计数器和实际位置计数器，并根据差值关系实现软件限位，从而达到闭环控制。
3.2 ARM微控器S3C44BOX
S3C44BOX是由SAMSUNG公司推出的16/32位RISC处理器，是一款高性价比和高性能的微控制器。S3C44BOX使用ARM7TDMI内核，采用0.25μm CMOS工艺制造，它提供了丰富的片上外设，主要包括：8KB Cache和内部SRAM，带自动握手的2通道UART，4通道DMA，带PWM功能的5通道定时器和一个内部定时器，I/O端口，通过片内DLL倍频器可实现最大66MHz的工作频率。S3C44BOX与SM5004的连接如图3所示。
3.3 PWM控制信号输出模块
SM5004有四路PWM输出，可同时控制四个伺服电机进行运动，但从芯片输出的PWM信号不足以直接驱动电机转动，需要使用驱动器将其转换为可用的驱动信号。驱动电路中H桥电路选取了允许大电流大电压的功率场效应管IRF4905和IRF3205，并使功率场效应管完全工作于饱和区，有效防止了因电路发热而浪费能源，设计中也注意消除了共态导通问题；输入端采用光电隔离消除电路对控制端的影响。实验证明，该驱动电路设计完全满足要求，可应用于中型机器人比赛。驱动电路如图4所示。
图4 驱动电路
3.4 测速反馈单元
为了提高反馈信号的精度，对光电码盘输出的光电脉冲进行四倍频，将四倍频后的脉冲信号计数，在转速较高的时刻可以满足精度要求，即使在转速较低、脉冲很少时，四倍频后的测速脉冲依然可以获得较高的分辨率。该电路除了四倍频信号外还具有电机鉴相功能。当周期电机转速方向改变时，两个脉冲周期Encoder A与Encoder B的相位发生变化，在第二个触发器的输入与输出端将出现不同的电平，此时同或门会输出低电平，使CLEAR置零，16位计数器复位，开始新的转动方向转速计数。逻辑原理图如图5所示。
图5 测速反馈单元逻辑原理
4 四轮全向机器人的运动控制
东风—Ⅱ型机器人采用了四轮全向移动的运动方式，具有全向运动能力的系统使机器人可以向任意方向做直线运动，而之前不需要做旋转运动，并且这种轮系可满足一边做直线运动一边旋转的要求，达到终状态所需要的任意姿态角。全向轮系的应用将使足球机器人具有运动快速灵活、控球稳定、进攻性强及易于控制等优点，使机器人在赛场上更具竞争力。
4.1 全向轮
本机器人采用的全向轮在大轮的周围均匀分布着小轮子，大轮由电机驱动，小轮可自由转动。这种全方位轮可有效避免普通轮子不能侧滑所带来的非完整性约束，使机器人具有平面运动的全部三个自由度，机动性增强。基于以上分析，选择使用这种全向轮。
4.2 运动学分析
在建立机器人的运动模型前，先做以下假设：
(1)小车是在一个理想的平面上运动，地面的不规则可以忽略。
(2)小车是一个刚体，形变可以忽略。
(3)轮子和地面之间满足纯滚动的条件，没有相对滑动。
全方位移动机器人由四个全向轮作为驱动轮，且为间隔90°均匀分布，其简化运动学模型如图6所示。其中xw-yw为绝对坐标系；xm-ym为固连在机器人车体上的相对坐标系，其坐标原点与机器人中心重合；θ为xw与xm的夹角；δ为轮子与ym的夹角；L为机器人中心到轮子中心的距离；vi为第i个轮子的沿驱动方向的速度。
可求出运动学方程如下：
因为轮子为对称分布，常数δ为45°，故得到全向移动机器人的运动模型：
v=PS
其中
为轮子的速度，
为机器人整体期望速度。
P为转换矩阵。
这样，就可以将机器人整体期望速度，解算为分别到四个轮子的速度，把数据传送到控制器中，就可完成对机器人的控制。
4.3 控制算法
传统直流电机调速系统一般采用PID算法，PID控制器设计完成后，参数就固定不变，无法根据实际负载的变化而调整。为了能够提高系统的动态性能及轨迹精度，本文提出了一种模糊自适应控制算法，从而使速度响应具有超调小、响应速度快、适应性强及受对象参数变化影响小等性能。系统取得了较好的动静态性能。
模糊自适应PID控制是模糊控制" title="模糊控制">模糊控制与传统PID控制的结合, 它的设计思想是运用模糊数学的基本理论和方法，把规则的条件、操作用模糊集表示，并把这些模糊控制规则以及有关信息(如评价指标、初始PID参数等)作为知识存入知识库中，然后根据控制系统的实际响应情况，运用模糊推理，即可自动实现对PID参数的最佳调整。这就是模糊自适应PID控制。
自适应模糊PID控制器以误差e和误差变化ec作为输入，找出PID三个参数与e和ec之间的模糊关系，在运行中通过不断检测e和ec，根据模糊控制规则对三个参数进行在线修改，以满足不同e和ec时对控制参数的不同要求。其结构如图7所示。
模糊控制设计的核心是总结工程设计人员的技术知识和实际操作经验，建立合适的模糊规则表，得到针对kp、ki、kd三个参数分别整定的模糊控制表。根据kp、ki、kd三个参数分别的作用，可制定模糊控制规则。以kp为例，列规则如表1，ki、kd可类似推出。
其中，将e、ec、kp、ki、kd分别定义了七个模糊子集，{NB，NM，NS，O，PS，PM，PB}，子集中元素分别代表负大、负中、负小、零、正小、正中、正大。设e、ec和kp、ki、kd均服从正态分布，因此可得出各模糊子集的隶属度。根据各模糊子集的隶属度赋值表和各参数模糊控制模型，应用模糊合成推理设计PID参数的模糊矩阵表，查出修正参数代入下式计算：
在线运行过程中，控制系统通过对模糊逻辑规则的结果处理、查表和运算，完成对PID参数的在线自校正。其工作流程图如图8所示。
机器人运动控制系统是整个Robocup机器人系统的执行机构，在场上的表现直接影响了整个足球机器人系统。机器人小车的性能优劣对整个系统起着举足轻重的作用。本文基于合理的理论假设，分析了机器人的运动模型，介绍了基于ARM+FPGA控制器的足球机器人运动控制的实现。实验证明，以这种控制器作为Robocup中型组机器人的底层运动控制系统，速度、位置及控制精度等指标均达到了设计要求。与以DSP为主控芯片的控制系统相比，减低了设计成本。这种机器人运动控制的实现方法对全方位移动机器人有一定的普适性。
参考文献
[1] 李岩，荣盘祥.基于S3C44BOX嵌入式μCLinux系统原理及应用.北京：清华大学出版社，2005.
[2] 张翮，熊蓉，褚健，等.一种全方位移动机器人的运动分析与控制实现.浙江大学学报(工学版)，2004，38(12).
[3] 刘金琨.先进PID控制MATLAB仿真.北京：电子工业出版社，2003.