CN107199565A - 一种服务型机器人的运动控制系统 - Google Patents

Abstract

本发明属于服务型机器人和人工智能领域，特别是涉及一种服务型机器人的运动控制系统。一种服务型机器人的运动控制系统，包括数字信号处理器（DSP）、现场可编程门阵列（FPGA）、电源模块、外部存储模块、外部设备接口、局域网传输模块、串行或总线传输模块。DSP与FPGA之间通过外部存储器接口（EMIF）进行互联通信。DSP包括DSP核心模块、多路CAN总线接口、多路I2C、SPI和UART接口等DSP自带的外设接口。FPGA模块主要用于多路多种传感器的驱动、数据接收、数据处理、数据存储以及通信，EMIF总线接口用于与DSP进行通信，FPGA模块作为数字信号处理单元的一个协处理器存在。

Description

一种服务型机器人的运动控制系统

技术领域

本发明属于服务型机器人和人工智能领域，特别是涉及一种服务型机器人的运动控制系统。

背景技术

现有的机器人运动控制系统是一种基于PC或工控机，用于工业自动化生产的上位控制单元。其基于PC总线，利用DSP等实现多个伺服电机的多轴协调控制的一种电机运动控制系统。现有运动控制系统多基于PC等上位机，但在服务机器人领域，其上位机多为ARM等在移动设备上使用的主控，所以现在还未有基于服务型机器人的运动控制系统。现有服务型机器人的运动控制一般使用单独的STM32、DSP等主控进行电机控制、传感器驱动，而复杂算法方面的运算多使用CPU或ARM等处理器。甚至有的服务型机器人根本没有算法方面的运算。这些现状导致了如下问题的产生：第一，服务型机器人运动控制多种多样，实时性和一致性差别很大；第二，使用多种主控相互通信时会出现数据丢失、传输速度慢等问题，在整个服务型机器人上的表现就是工作不稳定，行为反应缓慢；第三，进行复杂算法应用时候成本高昂，功耗大，而效率并不高。

发明内容

本发明的目的在于克服现有技术中实时性差、数据容易丢失、传输速度慢、工作不稳定、功耗大、成本高等缺点，实现了服务型机器人运动控制的强实时性、高一致性，保证了数据不易丢失和高效的传输速度，而且系统成本低、功耗小。本发明大大提高了复杂算法运算的工作效率，并适用于服务型机器人各种类型的运动，包括步态行走、滑行、蹲起、拿握、摇摆、点头等各种机器人动作行为等。

为达到上述目的，本发明是通过下述技术方案实现的：

一种服务型机器人的运动控制系统，包含一个控制机器人运动的处理器，能够向上接受中央处理器的指令并反馈数据，并向下控制机器人各部件的工作。

所述的运动控制系统，该处理器与一个现场可编程门阵列（FPGA）连接，并通过现场可编程门阵列实现对各项子模块的控制与数据传输。

所述的运动控制系统，该处理器为DSP等具有实现数字信号处理技术的处理单元。

所述的运动控制系统，该DSP包括DSP核心模块、多路CAN总线接口、多路I2C、SPI和UART接口等DSP自带的外设接口。

所述的运动控制系统，该处理器与FPGA之间通过外部存储器接口（EMIF）进行互联通信。

所述的运动控制系统，FPGA模块主要用于多路多种传感器的驱动、数据接收、数据处理、数据存储以及通信， FPGA模块作为该处理器的一个协处理器存在。

所述的运动控制系统，其包含电源模块、外部存储模块、外部设备接口。

所述的运动控制系统，其包含局域网传输模块、串行或总线传输方式模块。

所述的运动控制系统，该局域网传输模块为以太网模块、或WIFI模块。

如权利要求8或9所述的运动控制系统，该串行或总线传输方式模块为USART模块、或I2C模块，或SPI模块，或CAN模块。

综上所述，本发明利用FPGA并行处理和IO兼容性强的特点，将电机控制、各个传感器驱动和通信等需要实时响应、速度和稳定性要求高的工作全都放在FPGA上进行，保证了服务型机器人运动控制的实时性强，一致性高；同时将传感器数据存储到内外部存储器中，保证了数据不易丢失，而外部存储器DDR3的高速读写性能、FPGA内部存储单元的高速读写，均保证了高效的传输速度。另外DSP与FPGA之间通过EMIF总线进行连接，使FPGA部分成为了DSP的一个拓展存储空间，DSP需要获取和发送数据时，只需要通过读写地址就可完成数据交互；DSP相对与CPU和ARM处理器来说成本低，功耗小。另外因为使用了EMIF总线与FPGA进行交互，使FPGA成为DSP的一个协处理器，大大提高了复杂算法运算的工作效率；由于本设计是针对运动控制方面，并未具体针对某个部位或动作，故包括机械手臂、腿部、腰部、头部等各个机器人运动部位均可适用，包括步态行走、滑行、蹲起、拿握、摇摆、点头等各种机器人动作行为等。

前面所述的为本申请的概述，因此必然有简化、概括和细节省略的情况；本领域的技术人员应该认识到，概述部分仅是对本申请的说明，而不应看作是对本申请的任何限定。本说明书中描述的装置和/或方法和/或其他主题的其他方面、特征和优点将会由于本说明书的阐述而变得清晰。概述部分是用来以一种简化的方式导入多个将在以下具体实施方式部分进一步描述的概念。本概述部分既非用于确定所要求保护主题的关键特征或必要特征，也非用来作为确定所要求保护主题的范围的辅助手段。

附图说明

通过下面说明书和所附的权利要求书并与附图结合，就会更加充分地清楚理解本申请的上述和其他特征。应当理解，这些附图仅是对本申请若干实施方式的描述，不应认为是对本申请范围的限定，通过附图，本申请内容将会得到更加明确和详细地说明。

图1是运动控制系统总体结构图。

图2是运动控制系统流程图。

图3是控制状态下运动控制系统软件流程图。

图4是非控制状态下运动控制系统软件流程图。

具体实施方式

在下面的具体实施方式部分中，结合作为说明书一部分的附图进行说明。在附图中，相同/类似的标记通常表示相同/类似的部件，除非说明书中另有说明。具体实施方式、附图和权利要求书中描述的用来举例说明的实施方式不应认为是对本申请的限定。在不偏离本申请表述的主题的精神或范围的情况下，可以采用本申请的其他实施方式，并且可以对本申请做出其他变化。应该很容易理解，可以对本说明书中一般性描述的、附图中图解说明的本申请的各个方面进行各种不同构成的配置、替换、组合，设计，而所有这些改变都显然在预料之中，并构成本申请的一部分。

参考图1，一种服务型机器人的运动控制系统，包括数字信号处理器（DSP）、现场可编程门阵列（FPGA）、电源模块、外部存储模块、外部设备接口、局域网传输模块、串行或总线传输方式模块。DSP与FPGA之间通过外部存储器接口（EMIF）进行互联通信。DSP包括DSP核心模块、多路CAN总线接口、多路I2C、SPI和UART接口等DSP自带的外设接口。FPGA模块主要用于多路多种传感器的驱动、数据接收、数据处理、数据存储以及通信，EMIF总线接口用于与DSP进行通信，FPGA模块作为数字信号处理单元的一个协处理器存在。

参考图2、图3和图4，该运动控制系统的整体工作流程从两个方面来说明，第一，从上位机发送控制指令状态下的运动控制系统来说明工作流程：

1、一个或多个上位机将控制指令通过通信模块，如串行或总线传输模块或局域网传输模块等下发给运动控制系统。

2、FPGA将接收到的控制指令解析后，存储至FPGA内部存储单元和外部存储模块，并发送中断告知DSP。

3、DSP算法模块获取中断后通过EMIF总线读取FPGA内部存储单元和外部存储模块中的控制指令。

4、DSP进行前期的算法运算。DSP模块中所带的多路CAN总线接口、多路I2C、SPI和UART接口等DSP自带的外设接口等用于调试或其他主控模块、驱动模块进行通信和拓展使用。

5、DSP通过EMIF总线往FPGA内部存储单元和外部存储模块中写入控制传感器指令，并发送中断告知FPGA。

6、FPGA获取中断后，读取存储单元和外部存储模块中的控制指令，判断为传感器控制指令。

7、FPGA通过外部设备接口配置外部传感器的寄存器以及进行设备初始化。

8、传感器正常工作后反馈采集的数据。

9、FPGA获取数据后，判断数据是否正确，如果正确则进行数据处理，包括数据格式处理，数据融合等，如果不正确，则重复第7步骤。

10、数据处理结束后, FPGA将数据存储至存储单元和外部存储模块，并发送中断告知DSP算法模块。

11、DSP算法模块通过解析通信协议，判断数据是否正确，如果错误，则重复第5步骤，如果正确，DSP进行下一步的算法运算。

12、算法运算后得到电机工作的控制参数，DSP将这些电机控制参数通过EMIF写入到FPGA内部存储单元和外部存储模块，并发送中断告知FPGA。

13、FPGA通过通信协议解析存储单元和外部存储模块中的控制指令，判断为电机控制参数，FPGA将电机控制参数解析为具体多个电机驱动信号，并下发给电机驱动板。

14、电机进行工作，并同时反馈电流、位置、速度环数据给FPGA。

15、FPGA解析闭环数据，判断运动是否完成。如果未完成，则将闭环数据写入存储单元和外部存储模块，并发送中断给DSP，DSP重复第10步骤。如果完成，则通过通信模块反馈给上位机。

16、上位机接收到反馈信息，确认运动到位，至此运动完成。

第二，从上位机没有发送控制指令，即静止或自动工作状态下的运运动控制系统工作流程来说明：

1、DSP进行前期的算法运算。DSP模块中所带的多路CAN总线接口、多路I2C、SPI和UART接口等DSP自带的外设接口等用于调试或其他主控模块、驱动模块进行通信和拓展使用。

2、DSP通过EMIF总线往FPGA内部存储单元和外部存储模块中写入控制传感器指令，并发送中断告知FPGA。

3、FPGA获取中断后，读取存储单元和外部存储模块中的控制指令，判断为传感器控制指令。

4、FPGA通过外部设备接口配置外部传感器的寄存器以及进行设备初始化。

5、传感器正常工作后反馈采集的数据。

6、FPGA获取数据后，判断数据是否正确，如果正确则进行数据处理，包括数据格式处理，数据融合等，如果不正确，则重复第4步骤。

7、数据处理结束后, FPGA将数据存储至存储单元和外部存储模块，并发送中断告知DSP算法模块。

8、DSP算法模块通过解析通信协议，判断数据是否正确，如果错误，则重复第2步骤，如果正确，DSP进行下一步的算法运算。

9、算法运算后得到电机工作的控制参数，DSP将这些电机控制参数通过EMIF写入到FPGA内部存储单元和外部存储模块，并发送中断告知FPGA。

10、FPGA通过通信协议解析存储单元和外部存储模块中的控制指令，判断为电机控制参数，FPGA将电机控制参数解析为具体多个电机驱动信号，并下发给电机驱动板。

11、电机进行工作，并同时反馈电流、位置、速度环数据给FPGA。

12、FPGA解析闭环数据，判断运动是否完成。如果未完成，则将闭环数据写入存储单元和外部存储模块，并发送中断给DSP，DSP重复第7步骤。如果完成，则通过通信模块反馈给上位机。

13、上位机接收到反馈信息，确认运动到位，至此运动完成。

实施例子：

1、通过使用运动控制系统实现机器人手部运动：

机器人手部分布有多种类型的传感器，包括检测温度的温度传感器、检测接触的触摸传感器、检测内外部力的力矩传感器、检测电机转动位置的磁编码器等，这些传感器分别与机器人手部的每个手指上。

机器人手部需要完成一个抓取工作，需要在时间T1内获取每个手指上的传感器的实时数据，然后进行抓取算法运算时间T2，并在时间T3内让各个关节电机工作。而时间T1、T2决定了机器人手部的灵敏度。

而时间T1等于各个传感器初始化时间Ta1、Ta2、Ta3…Tan的总和Ta加上各个传感器数据反馈时间Tb1、Tb2、Tb3…Tbn的总和Tb。通过使用运动控制系统中的外部设计接口与FPGA模块，使Ta=Ta1，Tb=Tb1，即只需要一个Ta1和Tb1的时间，就可以完成以前方案中n个Ta1加上n个Tb1时间的传感器工作时间。

数字信号处理器进行抓取算法运算的时间等于运动算法初始化时间Tc1加上获取传感器数据时间Tc2加上运动算法运算时间Tc3。通过使用运动控制系统中的FPGA与DSP模块，可以在传感器工作的同时完成运动算法的初始化，则Tc1=0，而通过EMIF总线通信的方式也使获取传感器数据时间Tc2减小。故抓取算法运算效率提高。

在以前的方案中各个关节电机工作时间T3等于关节数n乘以单个电机工作时间Td。通过使用FPGA模块与外部设备接口模块，使T3=Td。

在这种多传感器、多电机协同工作的机器人手部工作中，本设计在实际应用中的灵敏度和实时性远高于其他方案。

2、通过使用运动控制系统实现腿部运动：

要实现双足行走或站立等动作，各个电机需要在很短的时间内进行实时的工作，以保证零力矩点处于脚掌范围内。

这需要腿部的各个陀螺仪、加速度传感器等数据实时更新的同时关节电机进行调整工作。通过使用DSP算法模块、FPGA模块、外部设备接口模块加外部存储模块的架构，能实现在驱动传感器工作并获取数据的同时，进行平衡算法的运算，并驱动关节电机工作。而不需要等待传感器数据获取后进行算法计算，最后才驱动电机工作。这将更好的进行腿部运动，实现双足行走或站立。

综上所述，本发明利用FPGA并行处理和IO兼容性强的特点，将电机控制、各个传感器驱动和通信等需要实时响应、速度和稳定性要求高的工作全都放在FPGA上进行，保证了服务型机器人运动控制的实时性强，一致性高；同时将传感器数据存储到内外部存储器中，保证了数据不易丢失，而外部存储器DDR3的高速读写性能、FPGA内部存储单元的高速读写，均保证了高效的传输速度。另外DSP与FPGA之间通过EMIF总线进行连接，使FPGA部分成为了DSP的一个拓展存储空间，DSP需要获取和发送数据时，只需要通过读写地址就可完成数据交互；DSP相对与CPU和ARM处理器来说成本低，功耗小。另外因为使用了EMIF总线与FPGA进行交互，使FPGA成为DSP的一个协处理器，大大提高了复杂算法运算的工作效率；由于本设计是针对运动控制方面，并未具体针对某个部位或动作，故包括机械手臂、腿部、腰部、头部等各个机器人运动部位均可适用，包括步态行走、滑行、蹲起、拿握、摇摆、点头等各种机器人动作行为等。

从系统角度来说，硬件执行和软件执行之间区别不大；采用硬件或者软件通常(但并非总是，在一些情况下，在硬件和软件之间进行选择会很重要)是反映成本/效率权衡的设计选择。通过其实现本说明书中描述的方法和/或系统和/或其他技术的工具(例如硬件、软件、和/或固件)有许多，并且，根据方法和/或系统和/或其他技术所处环境不同，所选择的工具也可以不同。例如，如果实施者判定速度和准确性至关重要，则该实施者会选择以硬件和/或固件的工具为主；如果灵活性至关重要，则实施者可能选择以软件执行为主；实施者也可选择硬件、软件和/或固件的某些组合。

前述已通过框图、流程图和/或实施例子进行了详细描述，阐明了本申请装置和/或方法的不同实施方式。当这些框图、流程图和/或实施例包含一个或多个功能和/或操作时，本领域的技术人员会明白，这些框图、流程图和/或实施例中的各功能和/或操作可以通过各种硬件、软件、固件或实质上它们的任意组合而单独地和/或共同地实施。在一种实施方式中，本说明书中描述的主题的几个部分可通过特定用途集成电路(ASIC)、现场可编程门阵列(FPGA)、数字信号处理器(DSP)或其他集成形式实现。然而，本领域的技术人员会认识到，本说明书中描述的实施方式的一些方面能够全部或部分地在集成电路中以在一个或多个计算机上运行的一个或多个计算机程序的形式(例如，以在一个或多个计算机系统上运行的一个或多个计算机程序的形式)、以在一个或多个处理器上运行的一个或多个程序的形式(例如，以在一个或多个微处理器上运行的一个或多个程序的形式)、以固件的形式、或以实质上它们的任意组合的形式等效地实施，并且，根据本说明书中公开的内容，设计用于本申请的电路和/或写用于本申请的软件和/或固件的代码完全是在本领域技术人员的能力范围之内。另外，本领域的技术人员会认识到，无论用来实际进行分发的信号承载介质的类型是什么，本说明书中描述的主题的机制能够以各种形式作为程序产品分发，并且，本说明书中描述的主题的示例性实施方式均适用。例如，信号承载介质包括但不限于下列：可记录型介质，如软盘、硬盘、致密盘(CD)、数字视频光盘数字多功能盘(DVD)、数字磁带、计算机存储器等；传输型介质，如数字和/或模拟通讯介质(例如光缆、波导、有线通讯链路、无线通讯链路等)。

本领域的技术人员会认识到，以本说明书中说明的方式描述装置和/或方法，然后进行工程实践以将所描述的装置和/或方法集成到数据处理系统中，在本领域里是很常见的。也就是说，本说明书中描述的装置和/或方法中的至少一部分，可通过合理数量的实验集成到数据处理系统中。本领域技术人员会认识到，典型的数据处理系统通常包括下列中的一个或多个：系统单元壳体、视频显示设备、诸如易失性和非易失性存储器之类的存储器、诸如微处理器和数字信号处理器之类的处理器、诸如操作系统、驱动程序器、图形用户界面及应用程序之类的计算实体、一个或多个诸如触摸板或触摸屏之类的交互装置、和/或包括反馈回路和控制马达在内的控制系统(如检测位置和/或速度的反馈；用于移动和/或调节部件和/或大小的控制马达)。典型的数据处理系统可以采用任何合适的、能够通过商业方法得到的部件(如数据计算/通讯和/或网络计算/通讯系统中通常会有的那些部件)来实现。

本说明书中描述的主题内容有时会举例说明这样的不同部件，这些部件被包含于或连接于不同的其他部件。应当理解，所描述的这些结构仅仅是示例性的，实际上，也可以用许多其他结构来实现同一功能。从概念上来讲，用于实现同一功能的任何配置的部件都被有效地“关联”以获得所需功能。因此，本说明书中被组合在一起以实现特定功能的任何两个部件可视为相互“关联”以获得所需功能，而不论其结构或可能中间部件如何。同样地，如此关联的任何二个部件也可视为相互“可操作地连接”，亦即“可操作地耦合”以实现所需功能，能够被如此连接的任何两个部件也可视为相互“能够可操作地耦合”以实现所需功能。能够可操作地耦合的具体例子包括但不限于：物理上可耦合的和/或物理上互相作用的部件和/或可无线互相作用的和/或无线互相作用的部件和/或逻辑上互相作用和/或逻辑上可互相作用的部件。

对于本说明书中所用的基本上任何复数和/或单数术语，本领域的技术人员可以将复数解释为单数和/或将单数解释为复数，只要这样做从上下文和/或应用上看是合适的即可。为了清楚起见，在本说明书中可能将各种单数/复数组合明确地表述出来。

本领域技术人员应该明白，一般来说，本申请中，尤其是所附权利要求书(例如，所附权利要求书的正文)中所用的术语通常是“开放式”术语(例如，“包括”一词应该解释为“包括但不限于”，“具有”一词应该解释为“至少具有”，等等)。本领域技术人员还应该明白，如果意图是特定数目的所引入的权利要求限定，那么这种意图会在权利要求中明确地表述出来，如果没有这种表述，则不存在这种意图。例如，为了便于理解，下面所附的权利要求书中可使用引导性短语“至少一项”及“一项或多项”来引入权利要求限定。然而，不应将使用这种引导性短语解释成暗示通过术语“一”引入的权利要求限定，将包含这样引入的权利要求限定的任何特定权利要求限定为仅包含一项这种限定的申请，即使同一权利要求包含引导性短语“一项或多项”或“至少一项”以及诸如“一”之列的术语(例如，“一”通常应该解释成这意味着“至少一项”或“一项或多项”)；这同样适用于使用“所述”、“该”来引述权利要求限定的情况。此外，即使明确表述了特定数目的所引入的权利要求限定，本领域技术人员也应该认识到，这种表述应该解释成表示至少是所表述的数目(例如，只说“两项限定”，而没有其它修饰语，通常是指至少两项限定，亦即两项或更多项限定)。此外，在使用类似于“A、B或C等中的至少一个”的习惯性表述的情况下，通常其要表达的意思就是本领域技术人员会就该惯用表述所理解的那样(例如，“具有A、B或C中的至少一个的系统”将包括但不限于仅具有A的系统、仅具有B的系统、仅具有C的系统、具有A和B的系统、具有A和C的系统、具有B和C的系统和/或具有A、B和C的系统，等等)。本领域技术人员还会明白，实际上任何能够提供两个或更多选项的选言词和/或短语，无论是在说明书中、权利要求书中还是在附图中，都应该理解为其考虑了包括所述选项中的一个、所述选项中的任一个、或全部二个选项的所有可能性。例如，短语“A或B”应该理解成包括“A”或“B”或者“A和B”的可能性。

本申请中公开了本申请的多个方面和实施方式，本领域的技术人员会明白本申请的其它方面和实施方式。本申请中公开的多个方面和实施方式只是用于举例说明，并非是对本申请的限定，本申请的真正保护范围和精神应当以下面的权利要求书为准。

Claims (10)

1.一种服务型机器人的运动控制系统，包含一个控制机器人运动的处理器，能够向上接受中央处理器的指令并反馈数据，并向下控制机器人各部件的工作。

2.如权利要求1所述的运动控制系统，该处理器与一个现场可编程门阵列（FPGA）连接，并通过现场可编程门阵列实现对各项子模块的控制与数据传输。

3.如权利要求1或2所述的运动控制系统，该处理器为DSP等具有实现数字信号处理技术的处理单元。

4.如权利要求3所述的运动控制系统，该DSP包括DSP核心模块、多路CAN总线接口、多路I2C、SPI和UART接口等DSP自带的外设接口。

5.如权利要求1-4之一所述的运动控制系统，该处理器与FPGA之间通过外部存储器接口（EMIF）进行互联通信。

6.如权利要求2-5之一所述的运动控制系统，FPGA模块主要用于多路多种传感器的驱动、数据接收、数据处理、数据存储以及通信， FPGA模块作为该处理器的一个协处理器存在。

7.如权利要求1-6之一所述的运动控制系统，其包含电源模块、外部存储模块、外部设备接口。

8.如权利要求1-7之一所述的运动控制系统，其包含局域网传输模块、串行或总线传输方式模块。

9.如权利要求8所述的运动控制系统，该局域网传输模块为以太网模块、或WIFI模块。

10.如权利要求8或9所述的运动控制系统，该串行或总线传输方式模块为USART模块、或I2C模块，或SPI模块，或CAN模块。