CN111687846B - 一种四足机器人分布式高实时性控制系统及方法 - Google Patents

Abstract

本公开提供了一种四足机器人分布式高实时性控制系统及方法，属于机器人控制技术领域，包括至少一个控制终端，每条行走单腿包括至少一个控制主站，每个运动关节包括至少一个控制从站；控制终端运行多足控制算法并将控制指令传递给控制主站，控制主站根据接收到的控制指令，运行单腿控制算法并将控制指令传递给控制从站；控制从站根据控制主站的控制指令对电机进行运动控制，提取编码器脉冲数返回给控制主站以确定关节电机的偏转角度，控制主站将获取到的数据实时回传给控制终端以实现控制参数的实时更新；本公开有效的避免了因一个节点负载过重而影响系统整体运行速度的情况，分布式的架构也具有极强的灵活性和弹性伸缩能力。

Description

一种四足机器人分布式高实时性控制系统及方法

技术领域

本公开涉及机器人控制技术领域，特别涉及一种四足机器人分布式高实时性控制系统及方法。

背景技术

本部分的陈述仅仅是提供了与本公开相关的背景技术，并不必然构成现有技术。

实现腿足式机器人控制系统的高实时性要求，始终是机器人研究领域的一个热点和难题。目前，大多数腿足式机器人控制系统都采用CAN总线或RS485总线，尽管它们具有传输距离远、成本低、稳定性强等诸多优势，但仍在实时性方面表现地不尽人意。

本公开发明人发现，CAN总线最大传输速率仅达到1Mbps，在以前的工程实验中，采用集中式架构和CAN总线进行一条机械单腿两个自由度的运动控制，带宽占用比就可达到35％，由此可推测采用集中式CAN通信控制一条三自由度的单腿，就可达到近50％的带宽占用比，这极可能造成通信拥堵或出现错误帧的情况，这样既无法提供足够的安全保证，也无法实现实时性的要求。

发明内容

为了解决现有技术的不足，本公开提供了一种四足机器人分布式高实时性控制系统及方法，在使用EtherCAT通信的基础上，采用分布式架构，有效的避免了因一个节点负载过重而影响系统整体运行速度的情况；同时采用分布式架构的方式可以根据不同需要来进行资源信息配置，可以随时添加新的网络节点来实现其他的任务要求，具有极强的灵活性和弹性伸缩能力。

为了实现上述目的，本公开采用如下技术方案：

本公开第一方面提供了一种四足机器人分布式高实时性控制系统。

一种四足机器人分布式高实时性控制系统，包括至少一个控制终端，每条行走单腿包括至少一个控制主站，每个运动关节包括至少一个控制从站；

控制终端运行多足控制算法并将控制指令传递给控制主站，控制主站根据接收到的控制指令，运行单腿控制算法并将控制指令传递给控制从站；

控制从站根据控制主站的控制指令对电机进行运动控制，提取编码器脉冲数返回给控制主站以确定关节电机的偏转角度，控制主站将获取到的数据实时回传给控制终端以实现控制参数的实时更新。

作为可能的一些实现方式，所述控制主站通过EtherCAT总线与控制从站通信，控制终端通过CAN总线与控制主站通信。

作为可能的一些实现方式，所述控制从站为每个运动关节的驱动器，编码器设置在各个关节电机处，通过信号线与驱动器连接。

作为可能的一些实现方式，所述多足控制算法，具体为：在一个控制周期内，根据机器人的当前状态，控制机器人做出相应的动作，并获取奖赏值作为反馈，以整个控制过程的奖赏值总和达到最大值为目标进行神经网络参数的实时更新。

作为进一步的限定，控制终端根据编码器以及惯性测量单元的数据，获取机器人最新的状态，并计算奖赏值。

作为进一步的限定，对于每条行走单腿，控制终端将数据帧发送给控制主站，数据经过控制主站处理后发送到第一从站，并依次向后传递，直到最后的从站将信息返回，由控制主站打包处理传回控制终端。

作为进一步的限定，控制主站通过单腿虚拟力柔顺算法计算出电机力矩，驱动器控制电机转动，实现机器人的运动。

作为更进一步的限定，所述控制主站根据每条行走单腿的足端期望位置、足端当前位置与足端速度，得到电机力矩。

作为更进一步的限定，所述电机力矩为雅克比矩阵的转置与足端虚拟力的乘积，所述足端虚拟力为期望位置与实际位置的差值和刚度的第一乘积与阻尼和足端速度的第二乘积的加和。

作为进一步的限定，所述机器人当前状态至少包括各个行走单腿的关节角、机身的俯仰角和横滚角以及俯仰角和横滚角的角速度；所述动作至少包括期望转动角度；所述奖赏值由俯仰角和横滚角及它们的角速度共同判定，当机器人朝着目标方向且平衡性良好时，机器人会获得较高的奖赏值。

本公开第二方面提供了一种四足机器人分布式高实时性控制方法。

一种四足机器人分布式高实时性控制方法，包括以下步骤：

控制终端接收控制指令，根据机器人当前状态及奖赏值采取动作；

控制终端通过正运动学运算与足端轨迹规划，得到足端期望位置、足端当前位置与足端速度，并将其发送到每个行走单腿的控制主站中；

控制主站根据单条行走单腿虚拟力柔顺算法计算出各关节电机力矩，并发送到每个运动关节对应的驱动器从站中，驱动器控制电机转动，实现机器人的运动；

控制终端根据每个运动关节对应的编码器与惯性测量单元采集的参数，获取最新的状态，并计算奖赏值，更新神经网络参数，直到达到最佳运动效果。

本公开第三方面提供了一种四足机器人，包括本公开第一方面所述的四足机器人分布式高实时性控制系统。

本公开第四方面提供了一种四足机器人，利用本公开第二方面所述的四足机器人分布式高实时性控制方法进行运动控制。

与现有技术相比，本公开的有益效果是：

1、本公开所述的系统及方法，主站和从站之间采用EtherCAT协议进行通信，相对于其他传统的现场总线系统，EtherCAT具有传输速率极强的优越性，它可以提供纳秒级精度的同步，将EtherCAT协议应用于腿足式机器人控制系统中，可以有效地缩短控制周期频率，达到腿足机器人运动的高实时性要求；而且EtherCAT在网络拓扑方面也几乎没有任何要求，可以随意实现线型、星型、树型等不同拓扑方式；EtherCAT还具有低成本、易实现的优势，主站设备既不需要高性能的CPU，也不用其他昂贵的协处理器，仅需要一个以太网口，就可实现EtherCAT的高速通信。

2、本公开所述的系统及方法，在使用EtherCAT通信的基础上采用分布式架构，解决了腿足机器人运动过程中的实时性控制问题，可以有效避免因一个节点负载过重而影响系统整体运行速度的情况；分布式架构不像集中式机构需要性能极强的处理器进行统一的数据处理与调配，仅需要在不同节点配置性能平均的处理器就能很好地完成计算、存储等复杂任务，在低成本等基础上获得了较高的性能，性价比高；同时，分布式架构还具有极强的灵活性和弹性伸缩能力，可以根据不同需要来进行资源信息配置，也可以随时添加新的网络节点来实现其他的任务要求。

3、本公开所述的系统及方法，控制终端运行多足控制算法，控制主站根据接收到的控制指令，运行单条行走单腿的虚拟力控制算法并将控制指令传递给控制从站，在进行机器人整体控制的同时，实现了单条行走单腿的精准控制，同时通过对神经网络参数的实时更新，极大的提高了机器人的运动控制效果。

附图说明

图1为本公开实施例1提供的电动四足机器人控制系统拓扑图。

图2为本公开实施例1提供的电动四足机器人的单腿控制系统框图。

图3为本公开实施例1提供的电动四足机器人控制学习算法简易流程图。

图4为本公开实施例2提供的电动四足机器人控制流程图。

具体实施方式

应该指出，以下详细说明都是示例性的，旨在对本公开提供进一步的说明。除非另有指明，本文使用的所有技术和科学术语具有与本公开所属技术领域的普通技术人员通常理解的相同含义。

需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本公开的示例性实施方式。如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合。

在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。

实施例1：

本公开实施例1提供了一种四足机器人分布式高实时性控制系统，如图1所示，包括1个工控机(控制终端)，4个STM32模块主站(控制主站)，12个驱动器从站(控制从站)，每个主站与其对应的三个从站采用线型拓扑连接，每个单腿的控制系统框图如图2所示。

本实施例的四足机器人分布式高实时性控制系统采用EtherCAT协议，工控机运行四足控制算法并将信息通过CAN总线传递给STM32模块主站。

4个STM32模块作为EtherCAT主站运行单腿控制算法并将信息传递给驱动器从站，12个驱动器作为EtherCAT从站通过EtherCAT总线从主站接收相应的控制指令，并根据主站的控制指令对电机进行运动控制，同时提取编码器脉冲数返回给主站以确定关节电机的偏转角度，并作为中间节点响应其他驱动器传回主站的信息；编码器通过信号线与驱动器相连，固定在各个关节电机处。

本实施例所述的控制系统采用EtherCAT协议，有一个工控机，4个STM32主站，12个驱动器从站，每个主站与其3个从站采用线型拓扑连接，构建简单，易于管理。EtherCAT的优越性能保证了四足机器人运动的高实时性要求。

工控机采用德国控创公司的嵌入式四核工控机，工控机上运行QNX6.5实时操作系统，其核心非常小巧，而且运行速度极快，这是实现四足机器人操作系统高实时性需求的巨大优势。工控机将数据帧通过CAN总线发送给STM32模块，数据经过STM32处理后，通过EtherCAT总线发送到第一从站，并依次向后传递，直到最后的从站将信息返回，由STM32打包处理传回工控机。

STM32模块包括四足机器人每条单腿对应的一个EtherCAT主站模块，共4个STM32模块，STM32模块作为EtherCAT主站通过CAN总线从工控机接收信息，将信息处理后通过EtherCAT总线发送到从站中，并将从站返回的信息传回工控机。

STM32模块配备两路CAN通道和一个百兆以太网接口，模块上运行虚拟力单腿控制算法，通过EtherCAT的高速通信，能够实现快速实时的单腿运动控制。

驱动器包括每条单腿上的横摆髋关节驱动器、俯仰髋关节驱动器和俯仰膝关节驱动器，共4条单腿12个驱动器。驱动器作为从站通过EtherCAT总线接收主站指令，并控制电机进行运动，作为中间节点响应其他驱动器信息的回传。

具体的，所述驱动器作为从站，通过EtherCAT总线接收主站发送的信息，从信息中提取出此关节电机的控制指令，并转化为电机的控制参数，同时向主站返回编码器采集的脉冲数以计算电机关节角，并作为响应将其他驱动器从站的信息回传到主站。

驱动器选型采用ELMO公司的驱动器，支持EtherCAT协议，最小供电电压仅需11V，四足机器人的总供电电源48v直流电源完全能满足其需求。而且此驱动器最大输出电流可达160A，能够为电动四足的关节电机提供充分的力矩控制，实现电动四足机器人运动方式的多样性和爆发力。

编码器通过信号线与驱动器相连，并固定在相应的关节电机上，编码器采用雷尼绍的增量式编码器，电机转动时带动编码器进行旋转，编码器会返回对应的脉冲数，以此来计算关节电机转动角度。

具体的，编码器用于测量关节电机的角偏移量，根据编码器采集的脉冲信号和减速比，通过计算就能得到关节电机偏转角度。

四足控制算法采用策略梯度强化学习方法，具体为：

在一个控制周期内，策略π根据机器人当前状态S，指挥机器人做出相应的动作A并获取奖赏值R作为反馈，控制流程如图3所示。

强化学习的目标就是找到最优的策略π，使整个控制过程的奖赏值总和达到最大值。

策略梯度强化学习算法的目标函数如下：

J(θ)＝E(r₁+γr₂+γ²r₃+...|π_θ) (1)

其中，r_t为t时刻的反馈奖赏值；γ是折扣率，0＜γ≤1；π_θ(s，a)＝P(s|a，θ)，即参数为θ的策略π在状态s的情况下，做出动作a的概率为P。

对目标函数求导可得到策略π的优化方向：

其中，机器人在状态s采取动作a的动作状态函数为：

根据策略的优化方向，参数θ_t可以更新为θ_t+1

其中，α为学习效率。

虚拟力控制算法是基于虚拟模型和关节力控制的柔顺算法，将单腿虚拟成刚度-阻尼系统，可计算足端虚拟力为：

F＝K₁×ΔP+K₂×V (5)

其中，K₁为刚度，K₂为阻尼，ΔP是期望位置与实际位置之差，V是足端速度。

由此可计算出电机目标力矩为：

τ＝J^TF (6)

其中，J^T为雅克比矩阵的转置。

实施例2：

本公开实施例2提供了一种四足机器人分布式高实时性控制方法，控制流程图如图4所示，包括以下步骤：

上位机向工控机发送指令，工控机中的策略π根据当前状态S_i及奖赏值R_i采取动作A_i；

其中，所述上位机通过路由器的无线网络，根据TCP/IP协议与工控机进行数据传输，上位机可以根据四足机器人不同的运动步态发送相应的指令，控制机器人进行原地踏步、对角小跑、疾跑等不同运动；

经过正运动学运算与足端轨迹规划，工控机计算出足端期望位置P_d，足端当前位置P_i与足端速度V，并将其发送到STM32模块中；

STM32根据单腿虚拟力柔顺算法计算出电机力矩τ，驱动器控制电机转动，实现四足机器人的运动；

工控机再根据编码器与IMU(Inertial Measurement Unit，惯性测量单元)采集的参数，获取最新的状态S_i，并计算奖赏值R_i，不断训练强化学习神经网络，更新神经网络参数θ，直到取得较好的运动效果。

其中，状态S_i包括四足机器人12个腿部关节角，机身的俯仰角和横滚角，以及俯仰角和横滚角的角速度，共16个参数；动作A_i包括12个电机的期望转动角度；奖赏值R_i由俯仰角和横滚角及它们的角速度共同判定，当机器人朝着目标方向且平衡性良好时，机器人会获得较高的奖赏值。

实施例3：

本公开实施例3提供了一种四足机器人，包括本公开实施例1所述的四足机器人分布式高实时性控制系统。

实施例4：

本公开实施例4提供了一种四足机器人，利用本公开实施例2所述的四足机器人分布式高实时性控制方法进行运动控制。

本领域内的技术人员应明白，本公开的实施例可提供为方法、系统、或计算机程序产品。因此，本公开可采用硬件实施例、软件实施例、或结合软件和硬件方面的实施例的形式。而且，本公开可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器和光学存储器等)上实施的计算机程序产品的形式。

本公开是参照根据本公开实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。

这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，所述的程序可存储于一计算机可读取存储介质中，该程序在执行时，可包括如上述各方法的实施例的流程。其中，所述的存储介质可为磁碟、光盘、只读存储记忆体(Read-Only Memory，ROM)或随机存储记忆体(RandomAccessMemory，RAM)等。

以上所述仅为本公开的优选实施例而已，并不用于限制本公开，对于本领域的技术人员来说，本公开可以有各种更改和变化。凡在本公开的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本公开的保护范围之内。

Claims (11)

1.一种四足机器人分布式高实时性控制系统，其特征在于，包括至少一个控制终端，每条行走单腿包括至少一个控制主站，每个运动关节包括至少一个控制从站；

控制终端运行多足控制算法并将控制指令传递给控制主站，控制主站根据接收到的控制指令，运行单腿控制算法并将控制指令传递给控制从站；

所述多足控制算法采用策略梯度强化学习方法，策略梯度强化学习算法的目标函数为：

J(θ)＝E(r₁+γr₂+γ²r₃+...|π_θ) (1)

其中，r_t为t时刻的反馈奖赏值；γ是折扣率，0＜γ≤1；π_θ(s，a)＝P(s|a，θ)，即参数为θ的策略π在状态s的情况下，做出动作a的概率为P；

对目标函数求导可得到策略π的优化方向：

控制从站根据控制主站的控制指令对电机进行运动控制，提取编码器脉冲数返回给控制主站以确定关节电机的偏转角度，控制主站将获取到的数据实时回传给控制终端以实现控制参数的实时更新，控制终端根据每个运动关节对应的编码器与惯性测量单元采集的参数，获取机器人最新的状态，并计算奖赏值作为反馈，所述四足机器人最新的状态至少包括各个行走单腿的关节角、机身的俯仰角和横滚角以及俯仰角和横滚角的角速度；所述动作至少包括期望转动角度；所述奖赏值由俯仰角和横滚角及它们的角速度共同判定。

2.如权利要求1所述的四足机器人分布式高实时性控制系统，其特征在于，所述控制主站通过EtherCAT总线与控制从站通信，控制终端通过CAN总线与控制主站通信。

3.如权利要求1所述的四足机器人分布式高实时性控制系统，其特征在于，所述控制从站为每个运动关节的驱动器，编码器设置在各个关节电机处，通过信号线与驱动器连接。

4.如权利要求1所述的四足机器人分布式高实时性控制系统，其特征在于，所述多足控制算法，具体为：在一个控制周期内，根据机器人的当前状态，控制机器人做出相应的动作，并获取奖赏值作为反馈，以整个控制过程的奖赏值总和达到最大值为目标进行神经网络参数的实时更新。

5.如权利要求4所述的四足机器人分布式高实时性控制系统，对于每条行走单腿，控制终端将数据帧发送给控制主站，数据经过控制主站处理后发送到第一从站，并依次向后传递，直到最后的从站将信息返回，由控制主站打包处理传回控制终端。

6.如权利要求4所述的四足机器人分布式高实时性控制系统，其特征在于，控制主站通过单腿虚拟力柔顺算法计算出电机力矩，驱动器控制电机转动，实现机器人的运动。

7.如权利要求6所述的四足机器人分布式高实时性控制系统，其特征在于，所述控制主站根据每条行走单腿的足端期望位置、足端当前位置与足端速度，得到电机力矩。

8.如权利要求7所述的四足机器人分布式高实时性控制系统，其特征在于，所述电机力矩为雅克比矩阵的转置与足端虚拟力的乘积，所述足端虚拟力为期望位置与实际位置的差值和刚度的第一乘积与阻尼和足端速度的第二乘积的加和。

9.如权利要求1-8任一所述的一种四足机器人分布式高实时性控制系统的控制方法，其特征在于，包括以下步骤：

控制终端接收控制指令，根据机器人当前状态及奖赏值采取动作；

控制终端通过正运动学运算与足端轨迹规划，得到足端期望位置、足端当前位置与足端速度，并将其发送到每个行走单腿的控制主站中；

控制主站根据单条行走单腿虚拟力柔顺算法计算出各关节电机力矩，并发送到每个运动关节对应的驱动器从站中，驱动器控制电机转动，实现机器人的运动；

控制终端根据每个运动关节对应的编码器与惯性测量单元采集的参数，获取最新的状态，并计算奖赏值，更新神经网络参数。

10.一种四足机器人，其特征在于，包括权利要求1-8任一项所述的四足机器人分布式高实时性控制系统。

11.如权利要求10所述的四足机器人，其特征在于，利用权利要求9所述的四足机器人分布式高实时性控制方法进行运动控制。

Images