基于运动控制技术的三关节机械手控制系统改造

唐辉 1 张华 1 陈志亮 1 金杰峰 2 张浩 2

摘要：本文介绍了一套基于运动控制技术的三关节机械手控制系统，用于对原有老式三关节机械手的改造。新系统采用运动控制器和交流伺服电机取代原有自整角机系统，实现三关节机械手的主从同步动作。新系统基于运动控制技术对主从操作的三关节手进行功能扩展，实现从手的单独自动控制，便于操作使用。

关键词：三关节机械手；交流伺服电机；运动控制；主从同步

1. 1. 引言

四0四公司某车间的三关节机械手使用时间已较长，原控制系统相对落后，电气故障率较高，不易维修，且配件采购困难。为避免三关节机械手故障引起停产而影响产能，急需采用新技术对现有三关节机械手控制系统进行升级改造。

1. 2. 三关节机械手结构

三关节机械手具有7个自由度，采用肩、肘、腕关节配置结构；机械手采用悬挂式安装、刚性传动方式，传动链完全由齿轮组成，其中腕、肘关节的运动为齿轮差动机构^【1】，如图1所示。

图 1 三关节机械手

三关节机械手由一支主手、一支从手、控制柜和操作盒组成。主手由驱动装置、传动轴、肩关节、肘关节、腕关节及手把组成，从手的组成结构与主手基本一致，只是将手把换成夹钳。

1. 3. 原有三关节机械手系统

原有三关节机械手系统采用自整角机实现主从随动。自整角机是一种对角位移或角速度的偏差有自整步能力的控制电机，可利用自整步特性将转角变为交流电压或由交流电压变为转角。两台或多台电机通过电路连接，使机械上互不相连的两根或多根转轴自动地保持相同的转角变化或同步旋转^【2】。

图 2 自整角机同步系统示意图

成对使用的自整角机，一个为发送机，一个为接收机，如上图所示。发送机与接收机的转子励磁绕组通以同一电源，定子绕组相互连接后，即可实现同步运行。当发送机转过一定角度时，接收机也会相应的转过相同的角度。

原有系统中，主手各关节安装自整角机发送机，从手各关节安装自整角机接收机，从而实现机械手的主从同步动作。

自整角机通常只作为传感器使用，即使是力矩式自整角机通常也只作为测位机用于带动指针类轻负载运行，并不适用于带动大负载运行。而原有系统中，自整角机作为驱动元件，带动机械手关节运动，应用不当，导致自整角机发热严重，不能长时间持续运行。

其次，自整角机不能单独运行，所以原有机械手无法实现从手预摆、动作复制等控制功能。

1. 4. 控制系统改造设计

      1. 系统架构

新控制系统基于运动控制和伺服驱动技术设计，对原系统进行升级改造，实现系统主从跟随动作，并满足各项功能的需要。

图 3 三关节机械手控制系统架构图

如上图所示，控制系统主要由运动控制器、操控界面、主手分系统和从手分系统组成。运动控制器根据操作界面的操控指令，分别控制主手分系统和从手分系统动作，实现系统功能。操控界面包括触摸屏和操作盒，用于系统的各项功能操作和控制。主手分系统主要由7个编码器和7个电磁制动器组成，用于采集主手各关节的运动信息，并在断电时固定各关节的位置。从手分系统主要由7套伺服驱动器、伺服电机及减速机组成，根据控制指令驱动从手各关节运动，完成相应动作。

1. 1. 2. 运动控制器

运动控制器是控制系统的核心，负责所有控制信号和控制指令的处理，按照系统要求控制各部件动作，并反馈运动状态，实现系统功能。

运动控制器是控制电机的运行方式的专用控制器，通常需要在复杂条件下，将预定的控制方案、规划指令转变成期望的机械运动，实现机械运动精确的位置控制、速度控制、加速度控制、转矩或力的控制。本系统采用西门子SIMOTION D运动控制器。SIMOTION D将运动控制、逻辑控制及工艺控制功能集成于一身，适用于具有许多运动部件的机器，为生产机械提供了完整的解决方案。系统具备定位、同步、电子凸轮及插补功能，可满足各类动作控制要求。该控制器久经国内外各类工程实践应用，其软硬件系统成熟稳定，安全可靠^【3】。

1. 1. 3. 操控界面

操控界面配置了触摸屏和操控盒，用于系统的各项操作、相关参数设置和系统状态的监测。

触摸屏组态了相应的界面用于系统相关参数设置、操作及监测，主要有系统运行监测、主从监测、参数设置、动作复制、输入状态、报警记录、系统设置等界面。

触摸屏的操作根据用户权限分级，主从、点动等普通操作不需要用户登录，而零点设定、力矩限制等参数设置，需要管理员登录才能操作。

系统设置操作盒，主要由各功能按钮及指示灯组成，用于系统的各项操作与指示。2对机械手的操控在同一操作盒上进行，分左、右两个区域分别操作2对机械手。操作盒布置如下图所示。

图 4 操作盒布局示意图

1. 1. 4. 主手分系统

主手为人工操作端，需要提供各关节运动的位置和速度参考信号。主手各关节驱动主要由编码器、电磁制动器组成。

主手各关节的运动带动传动轴上编码器的转动，运动控制器采集编码器的信号，解析出主手各关节的运动速度和位置，作为从手对应各关节的运动参考信号。主手去除了减速机，传动轴与编码器相连，操作阻力小，灵活性好。编码器为机械式多圈绝对值型，无需电池，稳定可靠、长寿命。编码器采用PROFINET通讯协议，且支持等时同步（IRT）模式，实时性好，可大大增强机械手的主从跟随性能。

主手各关节传动轴上安装电磁失电制动器，即制动器得电时松开，掉电时制动。系统上电后，运动控制器使制动器得电松开，当系统需要保持机械手的位置时，运动控制器将其断电，机械手保持当前位姿。系统掉电时，电磁抱闸也处于抱紧状态，可保持机械手位姿不变。

1. 1. 5. 从手分系统

从手为自动运动端，由电机驱动各轴关节运动，实现要求的动作。从手各关节驱动主要由伺服驱动器、伺服电机、减速机组成。运动控制器根据主从操作指令控制各伺服驱动器驱动相应伺服电机运动，实现从手的动作。

伺服电机自带反馈元件，由伺服驱动器实时采集其运动速度和位置，以便形成精确的速度和位置闭环控制。伺服电机还自带抱闸，在系统掉电或伺服电机去使能时抱紧，使从手各关节保持位姿不变。

本系统采用西门子SINAMICS S210系列伺服驱动器和1FK2系列伺服电机，配合西门子SIMOTION D运动控制器形成性能优化，易于使用的控制系统。伺服驱动器与运动控制器之间采用PROFINET网络通讯，实现控制指令的传达和数据的传输。

1. 5. 控制策略

      1. 主从跟随

从手和主手的主从跟随运动采用基于运动控制的电子齿轮同步技术实现。电子齿轮同步技术提供了使用“自动化控制”替代“刚性机械连接”的选项，具有高度柔性、便于维护的优点。电子齿轮同步运动功能（Gear）由同步对象（Synchronous Object）提供，主轴（Master）产生的运动参数（包括位置、速度、加速度）经过同步对象处理后，以指定的齿轮比线性传递给从轴（Slave），从而实现同步运动，与机械系统的齿轮功能相同。

图 5 电子齿轮同步功能示意图

系统中各关节以主手编码器作为主轴，从手对应各关节的驱动轴作为从轴，通过电子齿轮同步技术实现主从跟随动作。

1. 1. 2. 从手预摆

从手的上臂和下臂可以进行预摆操作，预摆通过伺服的点动实现。在预摆状态下，点击操作盒上的“上”、“上”、“下”、“下”按钮，可操作上臂或下臂的负向摆动和正向摆动，以便使其到达合适的位置。

系统基于运动控制的多轴插补技术，采用工业机器人运动控制算法，通过运动学正、逆解^【4】，控制从手在随动、预摆、点动等运动过程中，可保持末端夹钳姿态不变。

点击“回位”按钮，则从手各关节运动至主手各对应关节的位置，使主手和从手的位姿保持一致。

1. 1. 3. 动作记录与复制

图 6 动作记录与复制控制示意图

系统开发了动作记录与复制功能，即从手先在主从同步状态下，跟随主手进行动作到某个位置，同时控制器记录其运动轨迹；随后，从手可在非主从同步状态下，复制运动轨迹，执行相应动作至该位置。

系统基于高速数据采集算法，在主从随动时，高频采集从手各关节的位置信息，记录其运动轨迹，建立运动轨迹库，可记录不小于32个运动轨迹。开发三阶积分运动曲线拟合算法，根据记录的从手各关节位置信息进行运动曲线拟合，实时控制从手各关节的运动，实现32个运动轨迹的复制。

1. 6. 系统测试

控制系统改造后，对三关节机械手的主从操作、从手预摆、动作复制等关键功能进行测试，测试情况如下。

1. 1. 1. 主从操作测试

图 7 负载主从操作状态

如上图所示，三关节机械手夹持10kg负载情况下，主从操作时，从手动作与主手协调一致，主从随动响应时间均小于0.1s，各关节跟随位置差均小于0.5°，符合技术指标要求，相关测试数据见下表。

表 1 主从操作测试数据

测试项目	指标要求	实测值	测试结果
负载重量	≥10kg	10.1kg	符合要求
随动响应时间	≤0.1s	0.06s	符合要求
肩转跟随位置差	≤0.5°	0.13°	符合要求
肩摆跟随位置差	≤0.5°	0.15°	符合要求
肘转跟随位置差	≤0.5°	0.21°	符合要求
肘摆跟随位置差	≤0.5°	0.18°	符合要求
腕转跟随位置差	≤0.5°	0.20°	符合要求
腕摆跟随位置差	≤0.5°	0.15°	符合要求

1. 1. 2. 从手预摆测试

对三关节机械手从手进行预摆操作测试，上臂和下臂的正反向预摆功能均正常，且最大预摆角度均大于±90°，符合技术指标要求。相关测试数据见下表。

表 2 从手预摆测试数据

测试项目	指标要求	实测值	测试结果
上臂预摆	正向预摆正常	正常	符合要求
	最大预摆角度90°	95°	符合要求
	负向预摆正常	正常	符合要求
	最大预摆角度-90°	-95°	符合要求
下臂预摆	正向预摆正常	正常	符合要求
	最大预摆角度90°	96°	符合要求
	负向预摆正常	正常	符合要求
	最大预摆角度-90°	-96°	符合要求

1. 1. 3. 动作复制测试

经测试，在三关节机械手主从操作情况下，可记录32个动作轨迹，并均能独立复制执行，动作复制功能正常。

1. 7. 结论

经改造并通过测试，三关节机械手安装基于运动控制技术的新控制系统后，主从操作功能正常，且扩充了交叉换位、从手预摆、动作复制等功能，解决了故障频发、配件难买的问题，使该设备焕发了第二春，保证车间生产的正常进行。

参考文献：

[1] 张国伟,李斌,陈丽惠,等，核工业电随动主从机械手控制系统研制[J].仪器仪表学报.2010年8月第31卷第8期增刊，245-249页.

[2] 程明,微特电机及系统 第二版[M],中国电力出版社,2014.

[3] 王薇,吕其栋，张雪亮.深入浅出西门子运动控制器-SIMOTION实用手册[M].机械工业出版社.2013.

[4] John J．Craig．机器人学导论[M]．机械工业出版社.2006．