基于FPGA的四足机器人控制系统

殷勇华，卞新高，汪 赟，朱灯林

（河海大学 机电工程学院，常州 213022）

0 引言

四足机器人是一个复杂的运动控制系统，其具有多关节、多驱动器、多传感器、多自由度的特点[1]。本实验室自行研制的JQRI00型四足步行机器人样机，由机架、腿、关节以及足四部分组成，如图1所示。电控柜、液压系统、电机以及作业装置等主要由机架负责承载；每条腿由三个自由度构成，分为上臂（含足部）、下臂、胯部三个部分，上臂与下臂、胯部与上臂、机架与胯部之间分别通过三个转动副连接，以便各腿能够实现在一定范围内的自由运动；通过液压缸驱动各关节的运动，每个液压缸都可与另外两个运动臂构成稳定的三角形结构以适应重载运行；每个液压缸的运动由电磁比例换向阀和液压缸内同轴安装的位移传感器构成闭环速度以及位移控制；腿与地面的接触状态识别则由足底安装的接触力传感器完成[2]。

1 控制系统功能需求

四足步行机器人的运动控制、运行状态以及故障的监测和诊断等都需要运动控制系统来完成。具体包括：

图1 JQRI00型四足步行机器人

1）运动数据的采集与处理功能

对四足步行机器人运动过程中各传感器输出的模拟量信号进行采样以及必要的处理，及时反馈给关节伺服控制器进行实时运动控制，同时将数据通过主控制器上传给监控计算机进行机器人位姿动态显示。

2）运动控制功能

自动行走时控制四足步行机器人按照操作人员发出的运动指令和预定的步态规划路径自动运行，在调试阶段完成由监控计算机人机界面发送的各自由度独立动作或部分连续动作命令规定的运动要求。

3）系统参数存储功能

有些诸如PI控制的比例系数与积分系数、液压缸运动位移上下限、比例阀阀芯正反向运动死区等参数常量需要存储在系统的FLASH中，以便机器人正常工作时，控制程序可以直接读取FLASH中相关参数并加载到相应的控制程序中。

4）监控管理功能

将机器人运动过程中检测到的实时数据如通讯状态、故障信号、运行状态等信息实时发送到监控计算机并进行存储。监控计算机需要根据系统要求设计相应的人机交互界面，并以数字和动画相结合的方式直观地反映出四足机器人的运行状态、故障信号等。通过该界面，能够方便地浏览当前以及历史数据。

2 控制系统的构成

目前的机器人控制系统结构主要有集中式、分布式、分层递阶式三种形式[3,4]。分布式系统结构由多个相对独立的子系统构成，每个子系统采用独立的微控制器来控制，彼此间通过总线实现数据的交换。分布式控制系统具有模块化，可靠性高、实时性强、扩展性好的优点[5,6]。

JQRI00型四足机器人是一个具有一定的负载能力和能适应复杂环境行走的机器人。其控制系统根据功能需求设计为三层结构，如图2所示。整个控制系统采用多层分布式结构，由监控层、控制层、设备层三部分组成[7]。控制层中各控制器之间通过专门设计的FPGA通信模块交换数据，其中主控制器将规划好的行走路线、步态位姿等信息细分解到各子控制器，控制层通过基于UDP协议的无线通信与监控计算机进行数据交换。设备层是控制系统的底层，负责执行具体的动作。

图2 控制系统结构

根据上述硬件功能配置，软件功能也相应的分为三个模块：

1）监控模块：对JQRI00型四足机器人行走过程中的运行状态进行实时监控。

2）主控制器模块：对JQRI00型四足机器人进行路径、步态和位姿规划，并把规划好的足底坐标转换成各个关节的控制量，同时将各子控制器采集的运动状态参数上传给监控机。

3）子控制器模块：对机器人各个关节的比例阀进行控制，驱动关节运动，同时采集运动过程中的状态参数并实时传递给主控制器。包括四个子控制器，每个子控制器独立控制一条腿。

三个功能模块之间相对独立，完成各自的任务并相互之间进行数据通信。监控模块、主控制器和子控制器使用不同的CPU。目前常用的控制器有单片机、DSP、ARM、PC等。由于行走机器人控制系统要求精度高、实时性强、灵活性高。本系统中主控制器采用32位ARM9内核的工控主板；子控制器采用德州仪器公司针对实时控制应用专门推出的C2000系列32位微控制器——TMS320F28027；而监控计算机主要实现数据监控，要求较好的人机交互性，这里选用自带无线网卡的普通笔记本电脑实现。

2.1 主控制器功能结构

主控制器是JQRI00型四足机器人控制系统中的核心部分。实际运行时，主控制器采用ARM9内核的嵌入式模块，在WINCE5.0操作系统下以VC++语言进行运动规划程序设计，完成机器人整体姿态分析、运动规划与诊断。主控制器连接有并行ISA总线、以太网接口、RS232接口和带光电隔离的输入输出模块。各部分主要完成以下功能：

1）并行ISA总线与通信模块的并行ISA 总线接口相连，实现主控制器对通信模块内各组双端口RAM 的直接读写[8]。

2）以太网接口连接无线路由器，与监控机定时通信，发送所有计算、测量的数值给监控机，并接收监控机发送的控制命令。

3）RS232接口连接倾角传感器，接收倾角传感器发送的空间3个角度的值。

4）带光电隔离的输入输出模块连接油泵，负责控制油泵的启动以及停止并为将来的后续扩展作预留。

5）FPGA通信模块利用FPGA的可编程逻辑设计功能，在FPGA内构建有四组双端口RAM模块以存储实时数据和控制指令，通信模块具有一个并行ISA总线接口以及四个高速串行SPI总线接口，SPI总线接口与子控制器相连接。

6）主控制器上还可以连接有USB接口，USB接口以及以太网接口模块负责完成主控制器的在线配置以及调试。

主控制器的结构如图3所示。

图3 主控制器的结构

2.2 子控制器功能结构

子控制器的主要功能是接收主控制器命令数据，伺服驱动各关节动作。获取机器人运行过程中液压缸位移量、运动方向、通信故障等信息并反馈至主控制器进行处理分析[2]，如图4所示。子控制器包含高速串行SPI总线、ADC模块、模拟量采集模块、PWM模块、伺服驱动模块以及差分驱动模块。各部分主要完成以下功能：

1）高速串行SPI总线经差分驱动模块与专用通信模块中的SPI接口相连，以便实现子控制器对双端口RAM模块的实时读写[9,10]。

2）模拟量采集模块分别连接位移传感器和力传感器，模拟量采集模块将位移传感器及力传感器反馈的电流信号转换为合适的电压信号后输入ADC模块，以实现对位移及力信号的采集与转换。实际中，位移传感器与各液压缸同轴安装；多个力传感器安装在腿的各关节处以及足底。

3）ADC模块连接模拟量采集模块，模拟量采集模块采集传感器传输的各种模拟量，将其传输给ADC 模块，ADC模块进行模数转换，将采集的信息转换为数字信号后传输给子控制器，子控制器根据ADC模块传过来的信息，再结合从通信模块传过来的控制指令，计算得到控制器输出量，控制器输出量经过PWM模块，得到模拟控制量，再经过伺服驱动模块，驱动液压缸进行动作。

4）伺服驱动模块包括PWM转DA电路与恒流斩波电路，通过控制PWM信号占空比的大小来实现控制通过比例阀线圈电流的大小，进而控制阀芯的运动方向与开度，最终实现对液压缸运动方向与速度的控制。

5）由于主控制器与位于四条腿上的子控制器有一定距离，采用差分驱动模块可以有效提高信号的传输距离并抑制噪声。

图4 子控制器的结构

2.3 FPGA通信

现场可编程门阵列（Field－Programmable Gate Array），简称FPGA，它是在PAL、GAL、CPLD等可编程器件的基础上进一步发展起来的，是为了在克服定制电路缺点的同时解决原有可编程器件门电路数量有限的不足而研发出来的一种半定制电路[11]。FPGA集成有内嵌乘法器、专用运算电路以及可灵活配置的片内RAM等资源，再加上其并行性、可灵活配置的位宽以及丰富的电平接口，都使其成为本课题中进行通信模块设计的最佳选择[12]。

如图5所示，通信模块包括并行ISA总线接口以及四个串行SPI总线接口，每个串行SPI总线接口各连接有双端口RAM模块，串行SPI总线接口与双端口RAM模块的一端相连，双端口RAM模块的另一端与并行ISA总线接口相连。

双端口RAM存储相应的状态信息和控制指令，且状态信息和控制指令数据在各组双端口RAM中分区配置；其一端与并行ISA总线接口相连，另一端分别与对应的高速串行SPI总线接口连接。并行ISA总线接口一端与主控制器的并行ISA总线相连，另一端与各组双端口RAM模块相连，以便实现主控制器对双端口RAM模块的实时读写。串行SPI总线接口一端与双端口RAM模块连接另一端与子控制器SPI总线连接，以便实现子控制器对双端口RAM模块的实时读写[13,14]。

图5 通信模块的结构

图6是通信模块的内部结构示意图。其包括：读寄存器、写寄存器、发送寄存器、接收寄存器、脉冲计数器、地址寄存器、读地址计数器、写地址计数器和读写控制模块等。

脉冲计数器分别连接发送寄存器、接收寄存器和读写控制模块，读写控制模块分别连接读地址计数器、地址寄存器、写地址计数器和双端口RAM模块，地址寄存器连接双端口RAM模块，接收寄存器连接写寄存器，写寄存器连接双端口RAM模块，双端口RAM模块连接读寄存器，读寄存器连接发送寄存器。

通信模块中各SPI模块均工作于从机模式，子控制器中的SPI模块工作于主机模式。当SPI模块检测到主机选择线（SS）下降沿的跳变信号时，启动SPI通信，首先将读、写地址计数器，地址寄存器以及SCK 脉冲计数器初始化；然后将当前读地址计数器的值赋给双端口RAM模块的地址寄存器，同时读写控制模块产生读信号以及使能信号，读取双端口RAM模块中储存的由精简ISA总线写入的命令数据的第一个字节赋给读寄存器并将其载入SPI发送寄存器；当接收到第一个SCK脉冲时：在脉冲的上升沿将发送寄存器的最高位移出并送上主机输入/从机输出线（MISO），在脉冲的下降沿接收主机输出/从机输入线（MOSI）上的信号并移入接收寄存器的最低位；接收到第二个脉冲时，重复第一个脉冲的过程，直到完成八个脉冲的动作后将脉冲计数器清零，同时将接收寄存器的值赋给写寄存器，并将写地址计数器的值赋给双端口RAM模块的地址寄存器，读写控制模块产生写信号以及使能信号，完成第一个接收数据的写入双端口RAM模块，最后，读、写地址计数器加1，至此，一个字节数据的完整通信就完成了，重复上述过程直到从机选择线（SS）信号被拉高，表明此次SPI通信结束，等待检测到下一个从机选择线（SS）下降沿信号开始下一次通信。通信模块控制逻辑流程如图7所示。

图6 通信模块的内部结构

图7 SPI模块控制逻辑

3 结束语

在四足机器人原理样机上进行的大量模拟试验表明，基于FPGA的四足机器人分布式控制系统，具有实时数据通信能力、能够进行有效路径规划和实时精确控制关节运动。主控制器负责机器人整体姿态分析、运动规划与诊断；子控制器负责每条腿运动参数采集和各自由度运动的速度、位置控制；通信模块在FPGA内构建多组双端口的RAM模块存储实时数据和控制指令，主控制器通过并行ISA总线直接读写所有各组双端口RAM模块，子控制器通过串行SPI总线直接读写对应一组双端口RAM模块，状态信息和控制指令数据在各组双端口RAM模块中分区配置，可靠地实现主控制器与各子控制器间毫秒级的高速数据交换。

[1]Matuszek C,Herbst E,Zettlemoyer L,et al.Learning to parse natural language commands to a robot control system[C]//Experimental Robotics.Springer International Publishing,2013:403-415.

[2]卞新高,朱灯林,王红路,等.基于CAN总线的四足机器人控制系统[J].2011.

[3]陈学东,孙翔,贾文川.多足步行机器人运动规划与控制[M].武汉:华中科技大学出版社,2006,6.

[4]陈德明.四足仿生机器人运动控制系统的设计与实现[D].西北工业大学,2007,4.

[5]王沫楠,孙立宁.多运动方式移动机器人控制系统设计[J].电机与控制学报,2005,9.

[6]樊昌信,曹丽娜.通信原理[M].北京:国防工业出版社2006,9.

[7]汪赟.基于FPGA的四足机器人控制系统设计[D].河海大学,2013,6.

[8]苏旭武,杨光友,程良明,等.一种基于ISA总线的FPGA在线配置方法[J].机械与电子,2005(5):47-49.

[9]王珏文,金伟信,蔡一兵,等.基于FPGA的SPI总线接口的实现[J].现代电子技术,2010,33(14):102-104.

[10]王松.基于FPGA的串行外围接口SPI设计与实现[J].微计算机信息,2010,32:117-119.

[11]邓强华.基于SOPC的数字光端机的设计[D].中南大学,2009.

[12]吴厚航.深入浅出玩转FPGA[M].北京:北京航空航天大学出版社,2010

[13]Nie Hua,Liu Kaihua,Sun Chunguang,Liu Zhigang .Research on serial port communication between DSP and FPGA[J].Electronic Measurement Technology,2006,29(6):112-114.

[14]Tanaka H,Ohnishi K,Nishi H,et al.Implementation of Bilateral Control System Based on Acceleration Control Using FPGA for Multi-DOF Haptic Endoscopic Surgery Robot[J].Industrial Electronics,IEEE Transactions on,2009,56(3):618-627.