双足机器人多电机协同控制系统设计

2020-01-03

计算机测量与控制 2019年12期

(湖北大学计算机与信息工程学院，武汉 430062)

0 引言

双足机器人是机器人研究领域最活跃的研究热点之一，运动控制系统作为双足机器人研究的核心内容，它直接决定了机器人性能的优劣[1-2]。它不仅控制机器人的启动、运行和停止，而且对控制速度和精度提出了严格的要求，同时要求对机器人中多个电机的控制要能达到协同的效果，从而使机器人完成难度较大、更加接近人的动作。因此在双足机器人的控制中，对多路步进电机进行协同控制是核心[3-4]。

要实现对步进电机的控制，一般采用单片机控制，但单片机的单时钟系统满足不了多个电机协同控制的需求，通常会运用多个单片机芯片控制，但是多个CPU同时控制就会面对资源浪费、相互干扰而稳定性差、系统设计困难等问题；采用专用芯片控制法，控制精确、协同性强，但是可移植性差；采用DSP控制方法设计复杂，技术要求较高，较少采用；FPGA的控制能力很强大，但是单纯采用FPGA芯片控制时，需要高性能的FPGA芯片，这种芯片价格昂贵，因此其应用范围受到一定限制[5-7]。对于双足机器人控制系统，其多路电机的控制精度与速度要求并不算很高，为了节省成本，需要设计出既能同时协同控制多路电机，而且控制精确、性价比高的控制方案[8-10]。

本文采用“ARM+FPGA”的双芯片异构控制方案。结合ARM控制能力强、成本低、操作简单的优点和FPGA多通道控制的优点，以ARM芯片为主控芯片，控制所有命令的发出与执行；FPGA用来做辅控芯片，在接到命令之后，发挥自己并行处理的能力去控制电机的运行状态，在ARM和PPGA的分工协作下完成对多个电机的协同控制。

1 多电机协同控制系统总体设计

双足机器人ARM+FPGA多电机协同控制系统包含有人机交互控制系统，实时控制系统，电机控制系统三大部分组成。人机交互系统在PC端完成，主要是接受人的命令，使用者按照自己的需要去给系统发送命令从而控制电机；实时控制系统就是负责指令的执行，这也是控制系统的关键；电机控制系统则是最后命令的实行者，可以完成对多台电机的同时控制。ARM+FPGA多电机协同控制系统的具体模块如图1所示。

图1 多电机协同控制系统的具体功能模块框图

1.1 上位机

在上位机上装有软件用于人机交互，人机交互界面采用 C#开发语言设计。上位机采用一般的PC机主要负责具体命令的设置，我们通过PC机给电机设置一系列的动作，让电机工作。同时PC机也需要把电机运行的数据，比如每个电机运行的位置，以及运行的时间显示出来。PC机还需要保存数据并与ARM芯片双向通信，把命令传给ARM去执行，并把ARM上的信息显示出来。PC机主要起人与系统的交互作用，它包括参数的设置，状态的显示还有通信。参数设置主要是对多台电机通道的选择以及电机的动作、频率和步数的设置；状态的显示包括命令发出的状况以及FPGA芯片传回命令的情况，还有机器运转的情况等等；通信模块的主要功能就是上位机与ARM系统的通信，主要负责PC机接受到的命令的输送，由于需要实时控制所以采用网口通信的方式，通过网线将PC和ARM控制器链接起来。

1.2 ARM控制器

ARM 控制器是整个控制系统的核心，主要用于电机驱动算法的实现，与上位机和FPGA 进行通信，以及控制输出PWM脉冲信号。ARM控制器采用STM32F103ZET6芯片，它包含JTAG下载模块、通信模块、I/O接口模块、与FPGA通信模块等。JTAG下载模块的功能是将在PC端软件上编译好的程序下载到ARM控制器里面，从而STM32F103芯片才能对系统进行控制；通信模块则是ARM控制器与上位机PC的联系通道；I/O 接口模块则是ARM系统对外界的控制，比如指示灯、红外线以及一些参数的设计等等，通过I/O口来实现对外接设备的控制；与FPGA的通信模块则与FPGA相连，将主控命令传送给STM32F103芯片。ARM控制器与PC端上位机以及FPGA进行双向通信，传递数据和控制命令。

1.3 FPGA控制器

在该电机控制系统中,FPGA控制器起上传下达的作用，它首先接收ARM的指令，通过输出脉冲指令，将指令传送到电机控制系统，由电机控制系统去具体执行控制电机运行，还接收电机传回来的数据，并根据返回信号控制脉冲输出。FPGA功能模块采用EP2C35F672C8N芯片，它包含有JTAG下载模块，I/O 接口模块，与ARM系统通信模块，还有与电机控制系统连接的脉冲输出模块。JTAG下载模块把PC端软件上编译好的程序下载到芯片，I/O接口模块与电机的控制系统相连，控制电机的运行，EP2C35F672C8N芯片需要对主频进行分频，通过I/O 接口输出多路频率来控制多路电机；它的通信模块与ARM系统相连接，负责接收ARM系统的命令并返回相应的状态。

ARM与FPGA之间是双向通信，ARM控制器传送上位机命令给FPGA，同时FPGA也要把运行状态信息传给ARM，主控ARM芯片总揽大局，实时控制整个运行过程，实时控制模块FPGA芯片则是命令的传送者，传给步进电机驱动器去执行。

1.4 步进电机驱动器与光栅传感

步进电机驱动器的作用是由于芯片产生的波形PWM信号都比较小，不足以驱动电机，因此还需要在控制电路后面加上电机控制系统，步进电机驱动器如图2所示。

图2 步进电机驱动器结构框图

步进电机驱动器包括信号产生源、步进控制器及功率放大器等。此系统接受FPGA控制器产生的脉冲信号，接着步进控制器把脉冲信号转换成环形脉冲，这样才能控制步进电机正转和反转。由于脉冲信号太弱而不能驱动电机，这时候就需要功率放大器把环型脉冲信号放大。光栅传感器把电机相关信息(速度、位置或扭矩)传给FPGA再传给ARM来与控制信号作对比，实现比较精确的闭环控制。

2 控制系统的ARM+FPGA设计实现

2.1 控制系统架构设计

根据任务要求，ARM控制芯片选用ST系列的STM32F103ZET6芯片，STM32F103使用的是高性能的ARM CortexTM-M3 32位的RISC内核，工作频率高达72 MHz，内置高速存储器，有144个端口，有2个12位的ADC和3个通用16位定时器以及1个PWM定时器，还有标准和先进的通信接口，以及USART接口和CAN接口。FPGA控制芯片使用Altera公司经济型的Cyclone系列的EP2C35F672C8N芯片，具有丰富的逻辑单元和I/O接口，价格低，可满足本控制系统的设计需求。在本电机控制系统中，两部分的主控芯片ARM和FPGA还需要相互的交换数据，来达到协同控制的目的。因此在ARM和FPGA之间的通信方式也是关键，此系统中我们选择FSMC(Flexible StaticMemory Controller，可变静态存储控制器)的通信方式，把FPGA当作ARM的一个SRAM来使用，针对两个控制芯片的特点，我们设计了如图3的通信电路，采用 SRAM 的方式连接。PB3～7是5个控制信号端口，8位地址线负责发送命令到FPGA来具体控制6个电机的运行，8个数据通信线路是ARM与FPGA的通信通道，wr=1时写数据，STM32给FPGA发命令，rd=1时读取6路电机各个电机运行的状态是否正常。

图3 STM32 与 FPGA 的通信连接图

ARM+FPGA控制系统中，FPGA把各台电机的运行状态信息传送给STM32，并最终在上位机人机交互操作界面上实时显示。在控制过程中，数据刷新速度特别快，FPGA要把数据准确地传送给STM32，则需要对当前状态值等数据给以锁存，用锁存信号lock对数据进行锁存，数据锁存以后，就可以传送数据了。如果rd_stm32是高电平，就把锁存的数据保存至寄存器，等候STM32进行读取。FPGA需要从STM32获取在上位机人机交互操作界面设定的各电机的运行参数与控制命令，在FPGA需要从STM32读取的数据时，通过FPGA端口的控制线与地址线，把数据保存至寄存器。

2.2 STM32软件设计

STM32软件所要完成的主要任务是将上位机传来的数据指令进行相应的处理与转化，然后发送给FPGA芯片，同时从FPGA芯片读取电机的实时运行状态，并且做相应的数据处理再返回到上位机显示出来，还需要接收光栅传感器发来的数据并与控制命令对比，通过模糊PID控制修正参数，再进行电机的闭环控制。

STM32控制程序的设计是通过模糊PID算法控制电机的运行，模糊PID算法主要是在STM32里面实现的，STM32的开发平台是Keil MDK，Keil是美国Keil Soft公司开发的微处理器软件开发平台，是目前ARM内核单片机开发的主流工具，Keil提供了包括C编译器、宏汇编、链接器、库管理和仿真调试器[6]，在Win10系统上运行。本文开发用的是2013年发布的最新版本Keil uVision5。

2.2.1 STM32主程序

STM32的电机控制程序功能是由主程序和中断程序配合协调共同完成，主程序包括系统初始化程序，主循环程序。软件首先必须对整个系统和设备初始化，包括时钟的初始化、GPIO口的初始化配置、定时器的配置、PWM波的配置、ADC的配置初始化以及中断变量的设置。在整个系统初始完成之后还需要进入不断的循环之中，这就包括对上位机传来的命令的接收执行，对设备传感器传来的转速，加速度进行计算比较并在程序中做相应的算法处理，以及设备故障检测、电压电流检测、工作状态检测等来不断判断机器是否正常运行以及给机器提供正常运行的指令。主要的程序流程图如图4所示。

图4 STM32主要程序流程图

2.2.2 STM32子程序

主程序是框架，而子程序则是系统的核心和主要部分，由他们来保证系统功能的正常实现，在子程序中，PWM波的产生是主线，通过对光耦传感器传来的速度，以及采集的电流进行算法分析，对PWM信号进行计算修正，再输出新的信号，从而完成对电流、转速和位置的三环控制，ADC程序中断为辅以完成电机速度位置的某些转换。主要程序包括位置检测程序、电流检测程序、模糊PID算法程序、UART通信程序、电流current换相中断函数等，其流程图如图5所示。

图5 STM32子程序流程图

2.2.3 STM32与FPGA的通信

STM32与FPGA的通信有两个方面：一方面是STM32把上位机的命令通过转化发送给FPGA；另一方面FPGA也需要把自己运行产生信号的信息返回给STM32，考虑到灵活性、及时性以及双向性，我们采用FSMC(Flexible Static Memory Controller)可变静态存储控制器的通信方式，它是STM32系列中内部集成的256KB以上的Flash，支持多种静态存储器类型，而且可以同步突发访问。

2.3 控制系统的FPGA设计

2.3.1 系统硬件设计

FPGA控制系统主要的任务是对50 MHz的频率进行分频，获得各通道的时钟频率；传达各步进电机的启动、停止、运行等命令；对各步进电机的运行状态锁定，并且将状态发送给上位机；接收STM32发来的指令信息并且转化处理之后传给电机控制系统。根据FPGA在系统的功能，FPGA的功能电路模块主要包括：电源模块、下载模块、与电机通信模块和与STM32的通讯模块。FPGA的主要功能模块电路如图6所示。

图6 FPGA功能模块电路图

2.3.2 基于IP核的FPGA设计

设计平台采用Quartus II，它是Altera公司的综合性CPLD/FPGA开发软件，该软件支持原理图、VHDL、Verilong等多种设计输入，而且自带有仿真器，可以完成从设计、仿真、适配到最后形成最终电路的完整过程而不需要其他的平台。此外，在QuartusII上面提供了免费的LPM调试模块，比如计数器、加法器、乘法器等，而且还有特制的有偿IP核供使用。本文基于系统提供的锁相环IP核模块进行设计，通过对PLL模块例化，并且把整个IP核模块和PLL模块添加到顶层电路图中去，整个顶层设计如图7所示。

图7 FPGA完整的顶层设计原理图

3 实验结果及分析

要检验控制系统最终达到的控制效果，需构建一个测试平台进行系统调试。首先进行散件电机的检验，再组建成真正的双足机器人，从观察机器人的运动状态，用示波器检测，以及上位机显示的机器人的运行数据三个方面来验证系统的真实可行性。通过观察实物电机和机器人的运动、分析示波器的波形以及上位机显示的运动数据三个方面来说明本套控制系统的可行性。

要对系统调试必须要对系统进行控制，这就必须要有人机交互界面，人机交互界面部分所要完成的任务主要有：与用户进行数据交换，人们根据需要把动作通过人机交互界面下达给控制系统，然后交给步进电机去执行。人机交互界面是由C#写成的，在windows 系统上都可使用，借用的平台是 Visualstudio 2013，利用.NET Framework2.0 库中I/O口和串口控件设计而成的，通过设置串口通信所需的参数，打开串口便可以进行数据传输，达到控制的目的。

上位机与系统板的通信采用改进串口协议，协议有帧头、数据长度、指令和参数构成，协议以0x55开头，表示有数据到达，接着是数据长度，用来校验数据，然后是指令，对串口的数据进行操作，包括要控制的电机号，程序下载的电机号以及动作等。在上位机页面设置了6路电机的操作模式来验证系统控制电机的同时性。多电机协同控制系统的重点是完成对多台电机的同步控制，所以应当对多台电机控制的同步性进行检测。

我们把 6 路电机用作双足机器人的腿上的 6 个关节，来验证双足机器人中多路电机协同控制的性能，每条腿用三个电机相当于人腿上的三个关节，即从上到下依次是髋关节、膝关节和足关节。人的腿上的一系列动作需要各个关节协同起来才能完成，机器人腿上的动作则需要各个电机协同工作才能完成。观察分析 6 路电机在前进动作完成期间的数据，数据如表1所示。