CN103817695A - 一种机器人柔性关节的控制方法及驱动装置 - Google Patents

Abstract

本发明公开了一种机器人柔性关节的控制方法及驱动装置。其中方法包括以下步骤：1）采集弹簧两端的位置测量信号；2）对位置测量信号进行预处理；3）将预处理得到的信号输入至上位机，上位机进行规划，并向各柔性关节输入目标信号；4）计算得到速度指令信号5）将速度指令信号输入至对应的伺服驱动器，控制柔性关节运动；6）比对现有信号与目标信号，若相等，停止运行；若不相等，重复步骤1)～5)直至现有信号与目标信号相等。本发明将采集的位置信号直接在功能扩展卡上进行处理，避免了传送至上位机进行处理时产生的不确定的通讯延时，机器人柔性关节的控制效果好；通过力或力矩控制柔性关节，能够有效防止意外事故的发生，机器人安全性高。

Description

一种机器人柔性关节的控制方法及驱动装置

技术领域

本发明涉及机器人技术领域，具体涉及一种机器人柔性关节的控制方法及驱动装置。

背景技术

随着机器人技术的发展，人们对机器人与环境交互时的灵活性和安全性提出了更高的要求。对于机器人的运动控制而言，精确地控制关节驱动力是提升机器人灵活性和安全性的重要方法。在此背景下，串联弹性驱动装置应运而生，成为学界和产业界的研究热点。

与串联弹性驱动装置同时诞生的是串联弹性驱动装置的力/力矩伺服技术。如图1所示，是一个典型的串联弹性驱动装置运动控制和伺服驱动系统。在这个系统中，伺服电机和伺服驱动器组成一个独立的电机伺服子系统，能够完成电流环、速度环、位置环的控制任务；两个绝对式编码器、弹簧、编码器数据采集及其变送模块组成一个独立的力/力矩传感子系统，能够得到电机实际作用于负载的力/力矩，并发送给控制器。

系统工作时，由绝对式编码器采集弹簧两端的位置信息，通过编码器数据采集及其变送模块经由数据总线发送至上位机，计算机生成电流、速度或位置指令，经由数据总线控制伺服驱动器工作，伺服驱动器驱动伺服电机运转，伺服电机通过减速器压缩或拉伸弹簧，进而带动负载运动。

在该串联弹性驱动装置的力/力矩伺服结构中，力/力矩传感与伺服控制是分离的，运算任务被集中于作为运动控制器的计算机且由一个运动控制器来控制多个串联弹性驱动装置。这种伺服结构具有以下不足之处：

1).不确定的信号传输延时导致控制性能差

力/力矩传感子系统采集的信号经由数据总线到达控制器，控制器生成的控制指令同样经由数据总线到达电机伺服子系统，这两个过程都存在传输延时。一方面，在较高频率下（控制频率1KHz，采样频率5KHz），传输延时会变得不可忽视；另一方面，由于多个串联弹性驱动装置同时工作存在总线冲突，这使得传输延时时间变得难以估计。这种不确定的信号传输延时，严重影响了系统的控制性能。

2).系统可扩展性差

数据总线的信道带宽存在上限。总线能够挂载的节点数理论上不超过带宽与单节点数据量的比值（带宽/单节点数据量）。而单节点数据量又随着控制频率的提升成比例增加。因此存在关系如下：节点数上限正比于带宽与控制频率的比值（节点数上限∝带宽/控制频率）。以CAN总线为例，系统数据总线采用CAN总线，工作在1KHz控制频率，5KHz采样频率时，实验测得总线上至多只能挂载两个串联弹性驱动装置节点。这样的现实使得该伺服结构实现多节点控制存在困难。

3).上层软件架构复杂

串联弹性驱动装置的力/力矩伺服控制，在多关节机器人的控制中属于比较底层的运动控制。算法虽然简单，但对实时性要求较高。如果以该伺服结构为基础架构多关节机器人的运动控制软件，实时性要求高的底层关节控制和实时性要求相对较低的上层规划糅合在一起，会增加软件架构的困难。

发明内容

本发明提出了一种机器人柔性关节的控制方法及驱动装置，解决了现有技术因为信号传输延时导致控制性能差的问题。

一种机器人柔性关节的控制方法，每个柔性关节均包括设在负载和电机减速器之间的弹簧，该弹簧两端均设有绝对式编码器，包括以下步骤：

1）通过绝对式编码器采集弹簧两端的位置测量信号，即负载的位置测量信号

和电机减速器的位置测量信号

2）先分别对负载的位置测量信号

和电机减速器的位置测量信号

进行滤波处理，得到负载的位置信号θ₁和电机减速器的位置信号θ₂，然后对负载的位置信号θ₁和电机减速器的位置信号θ₂进行差分以及微分处理，得到弹簧的状态信号（θ₁-θ₂）、负载的速度信号dθ₁/dt、减速器输出轴的速度信号dθ₂/dt和柔性关节的角度信号；

3）将预处理得到的弹簧状态信号、柔性关节的角度信号输入至上位机，上位机进行多关节机器人运动规划，并向各柔性关节输入目标弹簧状态量Δθ_d；

4）各柔性关节对预处理得到的各种信号进行计算得到速度指令信号Y，计算公式为：

$\begin{array}{c}Y=R*[P*({\mathrm{\Δ\θ}}_d-({\mathrm{\θ}}_1-{\mathrm{\θ}}_2))+D*(\frac{d{\mathrm{\Δ\θ}}_d}{\mathrm{dt}}-({\mathrm{d\θ}}_1/\mathrm{dt}-{\mathrm{d\θ}}_2/\mathrm{dt}))+\mathrm{\η}*\\ ({\mathrm{d\θ}}_1/\mathrm{dt})]\end{array}$

其中，R为速度转换比，R=减速器减速比*电机编码器线数/绝对式编码器线数，P为比例系数，D为微分系数，η为衰减系数，η范围通常取（0,1）；

5）将速度指令信号Y输入至对应的伺服驱动器，控制柔性关节运动；

6）比对柔性关节的弹簧的状态信号（θ₁-θ₂）与目标弹簧状态量Δθ_d是否相等，若相等，电机停止运行；若不相等，保持电机运动状态，并重复步骤1)～5)直至弹簧的状态信号（θ₁-θ₂）与目标弹簧状态量Δθ_d相等。

各柔性关节均具有下位机，所述步骤2）、步骤4）和步骤6）均在各关节的下位机上进行。所述步骤6）中的电机指的是柔性关节内的电机。

步骤2）中，所述滤波在下位机中进行，通过数字滤波器，使采集到的信号更逼近真值。

步骤4）中，P和D的取值与构建柔性关节的弹簧强度，希望达到的控制性能目标有关，控制理论中通常不对PD的取值进行约束，只有在特定控制任务中，其取值才有意义。

在对弹簧模型辨识后，目标弹簧状态量实际与目标力或力矩等价。例如，假设弹簧是线性理想弹簧，根据胡克定律可知，弹簧的形变量信号乘以弹簧的弹性系数可以得到力的信号，同时，通过弹簧模型也可以得到力矩的信号。

具体控制时，下位机控制弹簧的状态信号（θ₁-θ₂），使之趋近并等于目标弹簧状态量Δθ_d。通过对弹簧的状态信号（θ₁-θ₂）的控制，结合弹簧模型，进行柔性关节输出力或者力矩的控制。

例如，假设弹簧模型为τ＝K（θ₁-θ₂-Δθ₀），其中，τ为柔性关节的输出力矩，K为刚度系数，Δθ₀为弹簧自然状态，（θ₁-θ₂-Δθ₀）为弹簧的形变量信号。要控制柔性关节的输出力矩为τ_d。那么上位机可以设定下位机通过对柔性关节的控制使得（θ₁-θ₂）等于Δθ_d，则此时柔性关节的输出力矩 $\mathrm{\τ}=K({\mathrm{\θ}}_1-{\mathrm{\θ}}_2-{\mathrm{\Δ\θ}}_0)=K*({\mathrm{\Δ\θ}}_d-$ ${\mathrm{\Δ\θ}}_0)=K*\frac{{\mathrm{\τ}}_d}{K}{\mathrm{\τ}}_d,$ 从而到达了力矩控制的目的。

负载的位置信号θ₁或电机减速器的位置信号θ₂均可以作为柔性关节的角信号，作为优选，所述电机减速器的位置信号θ₂为柔性关节的角度信号。

因为驱动柔性关节的电机通过电机减速器输出，采用电机减速器的位置信号θ₂作为柔性关节的角信号能够更真实、准确的反应柔性关节的状态。

步骤5）中，速度指令信号先通过信号转换得到可被伺服驱动器识别的信号后，再输入至伺服驱动器中。因为步骤4）中生成的速度指令信号不能被伺服驱动器自己读取识别，所以需要进行信号转换。

步骤1）中绝对式编码器的采样频率为f1，步骤5）中发送速度指令信号的频率为f2，f1与f2的比值取值范围为2：1～10：1。

比值偏大或者偏小均对控制产生影响，如比值过小时，柔性关节抖动较为厉害。

为了使控制更稳定，作为优选，f1与f2的比值取值范围为4：1～6：1。

一种机器人柔性关节的驱动装置，包括若干柔性关节和上位机，其中，每个柔性关节包括两个用于采集位置信息的绝对式编码器以及驱动柔性关节运动的伺服驱动器，所述上位机用于对各柔性关节进行运动规划，每个柔性关节和上位机之间还连接有功能扩展卡，所述功能扩展卡包括：

信号转换模块，将绝对式编码器输出的差分SSI制式信号转换为电平SSI制式信号；

SSI通讯模块，将信号转换模块得到的电平SSI制式信号转换为数字信号；

MCU，预处理来自SSI通讯模块的数字信号，并根据上位机的柔性关节规划和预处理后的信号进行计算得到速度指令信号；

通信模块，将速度指令信号转换为能被伺服驱动器识别的控制信号，并发送至伺服驱动器，控制柔性关节的运动。

所述功能扩展卡还包括I/O端口、JTAG端口以及存储有控制程序的片外FLASH。

I/O端口用于接收来自外部的扩展功能信号；JTAG端口用于修改和增减MCU上存储的扩展功能以及外部程序的下载。

所述功能扩展卡还包括用于在所述功能扩展卡、上位机以及柔性关节之间进行的通讯端口，该通讯端口为CAN通讯端口或者RS232端口。

功能扩展卡通过RS232或CAN通讯组成运动控制网络。

本发明的一种机器人柔性关节的控制方法及驱动装置，通过将采集的位置信号直接在功能扩展卡上进行处理，避免了传送至上位机进行处理时产生的不确定的通讯延时，机器人柔性关节的控制效果好；通过力和力矩控制柔性关节，能够有效防止意外事故的发生，机器人的安全性高。

附图说明

图1是典型的串联弹性驱动装置运动控制和伺服驱动系统；

图2是功能扩展卡的结构示意图；图3是本发明机器人柔性关节的驱动装置所组成的控制系统图；

图4是功能扩展卡应用于RS232网络控制模式的示意图；

图5是功能扩展卡应用于CAN分布式网络控制模式示意图；

图6是功能扩展卡应用于独立工作模式示意图。

具体实施方式

如图2、3所示，本发明实施例中机器人柔性关节的驱动装置，包括若干柔性关节和上位机，每个柔性关节均包括设在负载和电机减速器之间的弹簧，该弹簧两端均设有绝对式编码器，还包括驱动柔性关节运动的伺服驱动器，上位机用于对各柔性关节进行运动规划，其每个柔性关节和上位机之间还连接有功能扩展卡，功能扩展卡包括：

信号转换模块，将绝对式编码器输出的差分SSI制式信号转换为电平SSI制式信号；

SSI通讯模块，将信号转换模块得到的电平SSI制式信号转换为数字信号；

MCU，预处理来自SSI通讯模块的数字信号，并根据上位机的柔性关节规划和预处理后的信号进行计算得到速度指令信号；

通信模块，将速度指令信号转换为能被伺服驱动器识别的控制信号，并发送至伺服驱动器，控制柔性关节的运动。

功能扩展卡还包括I/O端口（图中未画出）、JTAG端口、存储有控制程序的片外FLASH（图中未画出）以及复位电路。

I/O端口用于接收来自外部的扩展功能信号；JTAG端口用于修改和增减MCU上存储的扩展功能以及外部程序的下载；复位电路用于在串联弹性驱动装置出错时进行复位。

功能扩展卡还包括用于在功能扩展卡、上位机以及柔性关节之间进行的通讯端口，该通讯端口为CAN通讯端口或者RS232端口。

本实施中，上位机可以是计算机、PLC或其他运动控制器，主要用来协调各柔性关节的运动，例如下达一个目标力矩指令使柔性关节输出特定的力矩至负载。

利用本发明的装置进行柔性关节的控制，控制方法包括以下步骤：

1）每个柔性关节通过绝对式编码器采集弹簧两端的位置测量信号并将采集的位置测量信号输送至对应的功能扩展卡中，其中，绝对式编码器采集的位置测量信号有两个，一个是负载的位置测量信号

另一个是电机减速器的位置测量信号

2）负载的位置测量信号

和电机减速器的位置测量信号

这两个信号均为差分SSI制式的信号，对这两个信号分别进行如下操作：

第一步，进行信号转换处理，具体步骤为，先通过信号转换模块将信号由差分SSI制式转换为电平SSI制式，再通过SSI通讯模块将信号由电平SSI制式转换为数字信号的形式；

第二步，进行滤波处理，具体步骤为：MCU对SSI通讯模块输出的数字信号进行滤波处理。

对负载的位置测量信号

和电机减速器的位置测量信号

分别进行如上操作后，得到负载的位置信号θ₁和电机减速器的位置信号θ₂，然后MCU对负载的位置信号θ₁和电机减速器的位置信号θ₂进行差分以及微分处理，得到弹簧的状态信号（θ₁-θ₂）、负载的速度信号dθ₁/dt、减速器的速度信号dθ₂/dt和柔性关节的角信号。本实施例把电机减速器的位置信号θ₂作为柔性关节的角度信号。本实施例MCU采用了FIR滤波器中的Equiripple滤波器（等波纹滤波器）进行滤波处理。

3）各功能扩展卡将各自的弹簧状态信号和柔性关节的角度信号输入至上位机，上位机进行多关节机器人运动规划，并向各柔性关节的功能扩展卡输入目标弹簧状态量Δθ_d。

4）MCU对步骤2）中得到的信号进行计算，得到速度指令信号Y，计算公式为：

$\begin{array}{c}Y=R*[P*({\mathrm{\Δ\θ}}_d-({\mathrm{\θ}}_1-{\mathrm{\θ}}_2))+D*(\frac{d{\mathrm{\Δ\θ}}_d}{\mathrm{dt}}-({\mathrm{d\θ}}_1/\mathrm{dt}-{\mathrm{d\θ}}_2/\mathrm{dt}))+\mathrm{\η}*\\ ({\mathrm{d\θ}}_1/\mathrm{dt})]\end{array}$

其中，R为速度转换比，R=减速器减速比*电机编码器线数/绝对式编码器线数，例如在一台实验样机中，机器人柔性关节的减速器减速比为120倍，电机编码器线数为4000，绝对式编码器线数为262144，那么R=120*4000/262144=1.831；P为比例系数，取P=-30；D为微分系数，取D=0.2；η为衰减系数，η范围通常取（0,1），取η=0.985。

5）将速度指令信号输入至通信模块，速度指令信号先通过信号转换得到可被伺服驱动器识别的信号后，再发送至对应的伺服驱动器，控制柔性关节运动。

其中，发送速度指令信号的频率为f2，绝对式编码器的采样频率为f1与发送速度指令信号的频率之比f1：f2=5：1。

6）比对柔性关节的弹簧的状态信号（θ₁-θ₂）与目标弹簧状态量Δθ_d是否相等，若相等，电机停止运行；若不相等，保持电机运动状态，并重复步骤1)～5)直至弹簧的状态信号（θ₁-θ₂）与目标弹簧状态量Δθ_d相等。

为了满足多种应用场合的需求，本发明提出的驱动装置设计了多种操作模式，同时开放了编程端口以适应未来新增的需求。

功能扩展卡的操作模式有：

1）RS232网络控制模式

2）CAN分布式网络控制模式

3）独立运行控制模式

RS232网络控制模式：

如图4所示，在RS232工作模式下，功能扩展卡通过RS232通讯和上位机相连，由上位机发送运动控制命令到功能扩展卡，功能扩展卡经过命令解析、执行进而控制伺服驱动器工作，该模式的特点如下：

（1）功能扩展卡通过RS232与上位机相连，上位机发送运动命令到功能扩展卡。功能扩展卡解析命令后执行，控制伺服驱动器工作，并把命令执行结果和其他必要信息反馈给上位机；

（2）每个RS232接口只能挂载一个功能扩展卡，如果有多个RS232口则可挂载多个功能扩展卡；

（3）功能扩展卡内置的控制算法、软件滤波的相关参数可以通过RS232配置。

CAN分布式网络控制模式：

如图5所示，在CAN分布式网络控制模式下，一条CAN线能挂载多个功能扩展卡。节点的数目与应用时设定的控制周期和请求的反馈信息量有关。在分布式系统中，主机能够给网络中的各节点发送控制命令。这使功能扩展卡的安装位置变得灵活，同时也方便了布线，增加了系统可靠性。该模式的特点如下：

（1）一条CAN线可挂载多个节点；

（2）功能扩展卡的安装位置灵活，通信线抗干扰能力强，布线方便；

（3）功能扩展卡内置的控制算法、软件滤波的相关参数可以通过CAN配置。

（4）系统状态可以通过CAN通讯反馈给上位机，实现上位机对系统的监控。

独立运行控制模式：

如图6所示，在独立工作模式下，机器人柔性关节的控制不需要上位机参与。功能扩展卡根据本地传感器获得的信息决定所要运行的控制程序。运动程序事先编写并储存于功能扩展卡的FLASH中。最典型的例子为各柔性关节的零力矩或特定力矩控制。该工作模式的特点如下：

（1）机器人柔性关节的控制不需要上位机参与。功能扩展卡根据本地传感器获得的信息决定所要运行的控制程序。运动程序事先编写并储存于功能扩展卡的FLASH中；

（2）I/O接口可以用来触发/终止运动程序的运行；

（3）独立运行模式的程序通过JTAG编程端口下载到功能扩展卡；

（4）对于具有特定运动的装置，独立工作模式具有结构简单、低成本等特点。

Claims (10)

1.一种机器人柔性关节的控制方法，每个柔性关节均包括设在负载和电机减速器之间的弹簧，该弹簧两端均设有绝对式编码器，其特征在于，包括以下步骤：

1）通过绝对式编码器采集弹簧两端的位置测量信号，即负载的位置测量信号

和电机减速器的位置测量信号

2）先分别对负载的位置测量信号

和电机减速器的位置测量信号

进行滤波处理，得到负载的位置信号θ₁和电机减速器的位置信号θ₂，然后对负载的位置信号θ₁和电机减速器的位置信号θ₂进行差分以及微分处理，得到弹簧的状态信号（θ₁-θ₂）、负载的速度信号dθ₁/dt、减速器输出轴的速度信号dθ₂/dt和柔性关节的角度信号；

3）将步骤2）得到的弹簧状态信号、柔性关节的角度信号输入至上位机，上位机进行多关节机器人运动规划，并向各柔性关节输入目标弹簧状态量Δθ_d；

4）各柔性关节对预处理得到的各种信号进行计算得到速度指令信号Y，计算公式为：

$\begin{array}{c}Y=R*[P*({\mathrm{\Δ\θ}}_d-({\mathrm{\θ}}_1-{\mathrm{\θ}}_2))+D*(\frac{d{\mathrm{\Δ\θ}}_d}{\mathrm{dt}}-({\mathrm{d\θ}}_1/\mathrm{dt}-{\mathrm{d\θ}}_2/\mathrm{dt}))+\mathrm{\η}*\\ ({\mathrm{d\θ}}_1/\mathrm{dt})]\end{array}$

其中，R为速度转换比，R=减速器减速比*电机编码器线数/绝对式编码器线数，P为比例系数，D为微分系数，η为衰减系数；

5）将速度指令信号Y输入至对应的伺服驱动器，控制柔性关节运动；

6）比对柔性关节的弹簧的状态信号（θ₁-θ₂）与目标弹簧状态量Δθ_d是否相等，若相等，电机停止运行；若不相等，保持电机运动状态，并重复步骤1)～5)直至弹簧的状态信号（θ₁-θ₂）与目标弹簧状态量Δθ_d相等。

2.根据权利要求1所述的机器人柔性关节的控制方法，其特征在于，通过对弹簧的状态信号（θ₁-θ₂）的控制，结合弹簧模型，进行柔性关节输出力或者力矩的控制。

3.根据权利要求1所述的机器人柔性关节的控制方法，其特征在于，将负载的位置信号θ₁或电机减速器的位置信号θ₂作为柔性关节的角度信号。

4.根据权利要求3所述的机器人柔性关节的控制方法，其特征在于，所述电机减速器的位置信号θ₂为柔性关节的角度信号。

5.根据权利要求1所述的机器人柔性关节的控制方法，其特征在于，步骤5）中，速度指令信号先通过信号转换得到可被伺服驱动器识别的信号后，再输入至伺服驱动器中。

6.根据权利要求1所述的机器人柔性关节的控制方法，其特征在于，步骤1）中绝对式编码器的采样频率为f1，步骤5）中发送速度指令信号的频率为f2，f1与f2的比值取值范围为2：1～10：1。

7.根据权利要求6所述的机器人柔性关节的控制方法，其特征在于，f1与f2的比值取值范围为4：1～6：1。

8.一种机器人柔性关节的驱动装置，包括若干柔性关节和上位机，其中，每个柔性关节包括两个用于采集位置信息的绝对式编码器以及驱动柔性关节运动的伺服驱动器，所述上位机用于对各柔性关节进行运动规划，其特征在于，每个柔性关节和上位机之间还连接有功能扩展卡，所述功能扩展卡包括：

信号转换模块，将绝对式编码器输出的差分SSI制式信号转换为电平SSI制式信号；

SSI通讯模块，将信号转换模块得到的电平SSI制式信号转换为数字信号；

MCU，预处理来自SSI通讯模块的数字信号，并根据上位机的柔性关节规划和预处理后的信号进行计算得到速度指令信号；

通信模块，将速度指令信号转换为能被伺服驱动器识别的控制信号，并发送至伺服驱动器，控制柔性关节的运动。

9.根据权利要求8所述的机器人柔性关节的驱动装置，其特征在于，所述功能扩展卡还包括I/O端口、JTAG端口以及存储有控制程序的片外FLASH。

10.根据权利要求9所述的机器人柔性关节的驱动装置，其特征在于，所述功能扩展卡还包括用于在所述功能扩展卡、上位机以及柔性关节之间进行的通讯端口，该通讯端口为CAN通讯端口或者RS232端口。

Images