毛睿欣

摘 要：针对单臂机器人难以应对复杂的任务问题，以双臂协作进行书写任务为切入点，介绍了双臂机器人控制系统框架，并以TwinCAT软件的PLC为上层控制核心设计了双臂机器人实时控制系统，通过引入相对雅可比对机器人的动力学方程和阻抗方程进行了改进，为控制系统的实现提供了理论基础;并通过不同期望阻抗下的书写任务实验，验证所提出的控制系统设计方案的有效性。结果表明，所设计的控制系统能够满足实验任务的需求，相对阻抗控制行为与期望可保持高度的一致性。

关键词：双臂机器人;阻抗控制;可编程逻辑控制器;控制系统

中图分类号：TP242.3 文献标识码：A 文章编号：1001-5922（2021）12-0134-06

Study on Asymmetric PLC Task System of Dual-arm Robot Based On Impedance Control

Mao Ruixin

（Guangdong University of Technology， Guangzhou 510000， China）

Abstract：Single arm robot is difficult to deal with complex task. With the two-armed collaborative writing task as the entry point， the dual-arm robot control system framework is introduced， and taking the PLC of TwinCAT software as the upper control core to design real time control system for dual-arm robot. In addition， the relative Jacobian is introduced to improve the dynamics equation and impedance equation of the robot， which provides a theoretical basis for the realization of the control system. And The effectiveness of the proposed control system is verified by writing task experiments under different expected impedance. The results show that the designed control system can meet the requirements of experimental tasks， and the relative impedance control behavior can maintain a high degree of consistency with the expectation.

Key words：Dual-arm robot;Impedance control;PLC;Control system

0 引言

随着机器人技术的不断发展，单臂机器人在分拣、装配等领域的应用均获得了较高的认可度。然而，就如人类拥有一对手臂一样，单臂机器人往往难以应付复杂的任务。双臂机器人通常设计为具有相互作用的形式，并且在某些情况下，与其工作环境相互作用。因而，双臂机器人可为实现拟人化的直观任务提供平台，并能在执行某些工作时直接可取代人类进行独立作业[1-3]。其双手任务可分为对称或非对称。在非对称双手任务中，每个机械臂担当不同的角色，但每个机械臂都有所需完成的必要任务，例如交易刷卡、记录笔记、协同装配以及开启瓶盖等[4]。这些任务涉及大量复杂的技术，但双臂机器人所遇到的最大挑战是其任务阻抗控制的复杂性[5]。

以此为出发点，本文对双臂协作进行协同工作任务进行了研究，提出了一种在双臂系统上实现阻抗控制的方法，同时在控制系统的软件设计方面，通过TwinCAT软件实现PLC高实时性的总线控制系统，用以取代落后的运动控制卡+伺服驱动模式，来保障机器人控制算法的高速、实时运行。采用时间延迟估计技术，通过双臂的不精确动态信息估计机器人动力学使得控制得到了进一步简化。从控制角度来看，该算法的相对雅可比是通过组合每个臂的各个雅可比得出的，它创建了一个具有运动冗余的单臂链，从而有助于简化控制策略。另外，该方法将双臂作为单个机械手进行控制，更容易协调两个机械臂之间的相互作用。

1 双臂机器人控制系统架构概述

通常情况下机器人采用由上而下的循环控制模式，即运动控制卡控制上层运动规划，各种控制指令采用CAN或者485总线被发送到驱动装置，通过驱动装置来有效控制伺服电机的输出[6]。由此可见，在机器人由上而下的循环控制模式中，运动控制卡在机器人运动学以及运动轨迹规划等任务中发挥着至关重要的作用。

1.1 机器人控制系统软件架构

将德国倍福公司研发的TwinCAT软件作为双臂机器人上层控制的心脏，构建双臂机器人控制系统的软件架构。图1为基于TwinCAT PLC的控制系統软件架构，该架构在操作系统下构造了实时环境，使得其具有了PLC的功能。与传统的PLC相比，基于TwinCAT PLC的控制系统软件架构具有十分明显的优势。双臂机器人采用阻抗控制时必须对机器人的动力学数据、雅克比数据进行计算与存储，这使得TwinCAT软件的优势得到了充分体现。采用TwinCAT软件能够控制的最大电机轴数多达255个，即同时控制两台6轴工业机器人形成双臂系统绰绰有余。

在基于计算机的机器人控制系统中，控制时钟周期为毫秒级。一般而言，时钟周期与机器人运动控制优化空间之间成反比，其周期越长，运动控制优化空间越小;周期越短，运动控制优化空间越大。双臂机器人只有具有更大的运动控制优化空间，才能够获得良好的性能和更加平稳的运动效果。CAN总线控制周期为8～20 ms，基本能够满足基本插补位置模式，但这种控制方式没有机器人动力学反馈，且在宏观上对控制效果的影响并不明显[7]。而对于融入了机器人动力学的高级算法来说，周期比较长会导致速度、加速度等规划运动量的计算受到影响，同时外加控制系统的滞后性使得双臂机器人跟随运动指令的稳态误差会比较大，从而导致双臂机器人无法按照预先设定的轨迹运动。

1.2 阻抗控制中的高速总线

对于本文所涉及的双臂协作任务，需要通过阻抗控制来解决机器人的柔顺控制问题。阻抗控制下构建控制系统的微分方程，其微分方程所反映的是每一个周期控制指令的周期性变化。对于EtherCAT总线而言，其最高能够支持1 ms的控制周期，即和CAN总线相比能够控制的调整时间缩短了8倍，这对双臂机器人末端位置控制、机器人各关节速度与加速度的控制更加精准化。机器人受到阻抗单元控制时，其宏观体现在末端位置按照末端受力的一个收敛过程，更短的控制周期使得机器人的各项运动参数更加平滑[8]。

2 双臂机器人不对称任务

图2为双臂机器人执行不对称任务示意图，其中，机器人A为参考机器人;机器人B为目标机器人。坐标系定义如下：目标坐标系T 位于机器人B末端执行器;相对参考系R位于机器人A的末端执行器;机器人A的基坐标系为A;机器人B的基坐标系为B;世界坐标系为world。定义T相对于R的相对位置为向量xR∈RnR，nR表示任务空间的自由度。图2中显示了机器人A末端执行器的位姿xA，以及机器人B末端执行器的位姿xB;qA为机器人A的关节角度向量;qB为机器人B的关节角度向量。

2.1 相对雅可比的计算

在运动学领域中，相对速度向量x'R∈RnR是通过相对雅可比 JR∈RnR×nT与关节空间速度向量q'∈RnT=[ q'TA    q'TB ]T 的作用得出的。

x'R=JRq'

q'A∈RnA和q'B∈RnB分别表示机器人A和机器人B的关节空间速度，且nT=nA+nB，根据各自的物理含义可得：

x'R=x'R-x'A

将公式（2）代入公式（1）可得：

而                         是相对于惯性参考系的相对雅可比矩阵。

2.2 相对阻抗控制

任务空间中的相对加速度向量xR'' 可表示为xR''= JRq''+J'Rq'，式中：J'R为JR的导数;q''表示关节加速度。其逆运动学方程为：

式中，I∈RnT×nT为单位阵;v∈RnT为关节空间加速度向量。此时，设定v=0，可实现最小范数加速。

整个双臂系统的完整动力学为机器人A和机器人B的组合动力学，即

其中为关节力矩向量;M（q）为惯性矩阵，其为机器人 A与机器人B组合而成的对角矩阵;c（q，q' ）表示科里奥利力和离心力产生的组合扭矩;g（q）表示重力产生的组合扭矩; f （q，q' ）表示庫仑和粘性摩擦组合扭矩;τd表示由于干扰引起的扭矩;τe表示由于环境接触力引起的扭矩。

此种状态下，任务的物理系统目标动力学，可通过所需的阻抗方程实现[9]。具体可通过修改所需的阻抗方程使其能够适应双臂的两个末端执行器之间的相对运动。它们之间的内部阻抗以及末端执行器的外部阻抗之间的关系对应于等效单个机器人。在参考系R中，fR∈RnR表示两个末端执行器之间的相对接触力，从虚功原理中可得τe= JRT fR;MRd ， BRd ， KRd∈RnR×nR，它们分别表示所需的质量、阻尼和刚度矩阵;xRd ， x'Rd ， x''Rd∈RnR分别表示所需的位姿、速度和加速度向量。当在双臂系统中使用两个6自由度机器人时，不是管理12个组件来对两个臂执行阻抗控制，而是仅使用6个组件等效单个机械臂，因此更容易实现。此外，可根据末端执行器相对于任务空间之间的相对运动来指定期望的运动[10-11]。

2.3 时延估计和理想速度反馈

通常情况下，阻抗控制器中有两项关键技术，即时延估计及理想速度的反馈。时延估计是一种无需模型即可估算机器人动力学的技术。理想速度反馈源于自然导纳控制，用于消除不连续物质的非线性，如库仑摩擦、静摩擦力和惯性力等不确定性。前者负责连续非线性，而后者负责非连续非线性。机器人动力学方程可据此写为如下形式：

式中，M 为常对角矩阵;h（q ， q' ， q'' ）包含所有连续和不连续的非线性，为了对其进行计算，假设在给定的时间 t 和足够小的采样时间，可得

用（q ， q' ， q'' ）表示h（q ， q' ， q'' ）的估计值，可得（q ， q' ， q'' ）=h（q ， q' ， q'' ）（t-L），结合式（7）可得：

可得到操作空间动力学方程：

将v设为0，τ设为τu：

3 控制系统的实现

本文通过3个组实验验证所提出的相对阻抗控制在双臂系统上执行非对称任务，包括接触、非接触和过渡3种状态。据此确定给定的任务所需的轨迹和阻抗。

3.1 实验设置

双臂实时控制平台的配置如图3所示。

本文所采用的双臂机器人为三星公司制造的两个6自由度PUMA型工业机器人，Faraman-AC2作为参考机器人（左），Faraman-AT2作为目标机器人（右）。在参考机器人的末端执行器上安装有6维力/力矩传感器，在其上面固定丙烯酸板。通过该传感器测量动力学方程中的 fR，即相对于R的相对接触力。盘子上覆盖着一张白纸，使书写更加醒目。工具机器人的末端执行器上连接一个带有红色墨水的钢球。

控制器使用带有i7四核CPU的工控机，力/力矩传感器通过以太网与TwinCAT进行连接，采样周期设定为2 ms。

3.2 实验描述

以两个6自由度的工业机器人作为实验对象，通过两个机器人的协调运动来表示接触、非接触、过渡书写3种不对称任务，将工业机器人连接到参考机器人的末端执行器所在的板上写3个字母。

（1）相对位姿的期望轨迹。在xR-yR平面写入字母，设定zR轴方向位置超过书写板所在的位置，从而确保在写入的过程中始终保持接触。相对位置轨迹由27个直线段或者曲线段所组成，每一段由5阶多项式拟合而成，图4给出了期望轨迹与书写板位置随时间变化的关系。执行t =0～66 s整个任务，所需的姿态轨迹由与机器人左臂末端执行器来完成，其在R中的滚动俯仰-偏航角为[αR ， βR ， γR]T=[π ， 0 ， 0]T。因此，仅仅需要XR的期望值就可以满足要求，而不用机器人每一个末端执行器的期望值。

（2）期望的相对阻抗。为抵抗书写板刚性，在接触与非接触之间执行10次过渡，同时在zR约束方向设定高阻尼和低刚度的柔顺行为。为了验证沿着zR约束方向能够准确、平稳地实现期望的相对阻抗，对沿着zR约束方向的3种过阻尼阻抗状态进行测试，设定阻尼比ζZR=4，5，6。其余的自由度，当临界阻尼阻抗ζ=1时，必须采取底阻尼、高刚度的精准写入。对每一个过阻尼ζZR，設置对应的相对质量MRd，其阻尼矩阵和刚度矩阵分别为BRd和KRd。

3.3 控制器实现

为了实现期望的相对轨迹和相对阻抗行为，将式（12）中所提出的控制器模型用于实验。的选择对控制性能具有显著影响，原则上， 的最佳选择是M （ q ），即实际惯性矩阵。然而，由于M （ q ）难以准确估计并且计算有着实时性要求，因此 通常通过反复试验来调整。它的对角元素从稳定区域内的最小正值逐渐增加，而不使用系统参数。理想速度反馈增益被设置为没有发生振动的最高值。

值得注意的是，所提出的控制需要时延估计的关节加速度向量q''（t-L）。在实际实验中，编码器信号通常易被噪声干扰，且由于通过数值微分计算关节加速度，噪声的影响易被放大。通过降低 可以减弱噪声，而无需明确使用额外的低通滤波器。使用本文提出的相对阻抗方程（6），使得期望的任务等同于单个机器人的情况。

4 实验结果分析

4.1 相对阻抗组1#

期望的相对阻抗组1#，使用期望阻抗组1#参数进行双臂书写任务。xR， yR， zR轴的相对位置和姿态轨迹如图5所示。

除zR方向外，其余方向均实现了良好的跟踪精度。沿zR方向的位置误差因保持接触并获得所需的相对阻抗造成，为必要误差。期望的相对阻抗行为实现效果良好。

4.2 相对阻抗组2#和3#

为实现各种所需阻抗时验证所提出的控制器的性能，在两个附加条件下执行相同的书写任务。本文对所提出的阻尼比的变化相对于阻尼系数BRd和劲度系数KRd进行了修改，也可通过改变惯性系数MRd 将阻尼比改变为其期望值。

3个实验组的沿约束方向zR的相对位置响应如图6所示。观察到根据相对阻抗的响应变化，所需相对阻抗的阻尼比越小，建立时间越快，过冲越小。

在3个实验的书写任务期间沿着约束方向zR的相对阻抗误差范围如图7所示，2#和3#组的RMS误差分别为0.419和0.441。

由上述结果可知，所提出的控制器在不同的阻抗值下可以很好地实现所需的相对阻抗。

5 结语

本文基于TwinCAT PLC进行双臂机器人实时控制，实现了双臂机器人不对称PLC任务系统的阻抗控制，确保机器人控制算法的效率大大提升。采用时间延迟估计技术，通过双臂的不精确动态信息估计机器人动力学使得控制得到了进一步简化。

控制器可以将两个臂的关节速度映射到它们的末端执行器之间的相对运动。此外本文引入相对雅可比用来改进动力学和阻抗方程，以实现将双臂映射到一个机器人用以执行非对称双手书写任务。通过使用相对雅可比行列式，阻抗控制被简化为用于两个臂的单个控制器，使得两个末端执行器之间的相对运动表征执行期望任务。这使得在指定期望阻抗和期望轨迹时所提出的阻抗控制更简单且更直观。对于书写任务的实施，在不同的阻尼比下进行了3个组实验，相对刚度假设最大稳态误差为1.35%;时延估计的使用使得所提出的控制器更容易实现;对所提出的控制器进行了稳定性分析，结果表明误差最终是有界的。

参考文献

[1]李开霞， 丁 玲， 赵江海，等. 双臂机器人末端一体化关节控制与驱动单元设计[J]. 机电工程， 2017（9）：1 019-1 023.

[2]郭 鹏， 房灵申， 赵明扬，等. 基于扩展任务的双臂机器人位姿优化[J]. 组合机床与自动化加工技术， 2018， 527（1）：8-12.

[3]王美玲， 骆敏舟. 一种冗余自由度双臂机器人协调控制器设计[J]. 系统仿真学报， 2016， 28（8）：1 783-1 789.

[4]孟令威. 七自由度手臂运动控制与轨迹插值[J]. 机械设计与制造， 2017（05）：263-266.

[5]赵金刚， 戈新生. 动态规划求解空间双臂机器人非完整运动最优控制问题[J]. 力学季刊， 2016（2）：225-233.

[6]MORRIS，A. S，MADANI. Quadratic Optimal Control of a Two-flexible-link Robot Manipulator[J]. Quadratic Optimal Control of a Two-flexible-link Robot Manipulator， 1998， 1（16）： 97-108.

[7]PILTAN F，RASHIDIAN MR，SHAMSODINI M， et al. Effect of Rule Base on the Fuzzy-based Tuning Fuzzy Sliding Mode Controller： Applied to 2 Nd Order Nonlinear System[J]. International Journal of Advanced Science & Technology， 2012， 60（3）： 222-231.

[8]黄俊铭， 陈 浩. 深海海底车行星轮式行走机构建模分析[J]. 机械设计与制造， 2017（6）：5-9.

[9]刘 阔， 林剑峰， 马晓波， 等. 阻抗控制在挖掘机器人中的应用与研究[J]. 智能制造， 2010（5）：48-49.

[10]龚兰芳， 曹成涛， 许伦辉. 肢康复训练机器人协作控制与S型规划算法研究[J]. 中国工程机械学报，2019（06）： 482-487.