线束插植机械臂控制系统设计

，，，

(浙江工业大学 信息工程学院，杭州 310023)

0 引言

线束需要经过送线、裁线、端子压接、预装工艺以及总装工艺等一系列工序制作完成[1]，将压接好的线束装配到接插件中的过程成为插植。目前，国内外对于线束的送线、裁线、压接自动化技术研究较多，相关的自动化产品也都已经推向了市场。而线束装配[2]工艺较为特殊，再加上传统刚性机械臂一般以位置控制为主，很难实现对线束这类柔性材料的精确控制。因此，线束插植大多仍然依靠人工进行。另外，线束装配时机械臂末端与接插件存在接触力，需要对插植接触力进行控制。近年来，国内外学者纷纷对线束装配过程中的接触力进行了研究。2005年，Liu[3]等人通过对线束插孔过程中接触力的分析，调整机械手指的运动轨迹，实现了机器人的线插孔操作。2010年，Sun[4]等人采用摄像机初步检测两接插件的位置和姿态，并在接触过程中根据力传感器检测出的接触力大小，时时调整机械臂末端的姿态，成功实现了两个接插件的对接。本次设计利用柔性结构——SEA[5-7]串联弹性执行器，使得机械臂末端与环境相接触时具有较好的柔顺性。再结合闭环PD力控制方法，可以实现对插植过程中接触力的控制，避免出现由于接触力过小而导致线束未插紧，出现线束松动、脱落，或者接触力过大而造成接插件损坏、导线变形的状况。SEA串联弹性执行器是以电机为驱动源，在电机输出轴和负载之间加入弹簧等弹性结构而构成的柔性单元，具有低阻抗、体积小、能量密度高、力输出稳定以及对外部冲击载荷可以起到较好的缓存的特点[8]，已经广泛应用于仿生机械臂、康复机器人、柔顺机械臂等领域。2016年，张秀丽[9]等人设计了一款基于串联弹性执行器的柔顺机械臂-SoftArm II，在机械臂的关节处，电机通过串联弹性执行器与连杆相连，在采用被动柔顺装置SEA的基础上，柔顺机械臂外部采用位置PID控制方法，使得机械臂末端接触力处于一定的范围内。

本文首先针对线束插植工艺流程和要求，搭建了包含SEA串联弹性执行器柔性结构的直角坐标机械臂，并对其控制系统结构进行设计。然后，开发了专用的线束插植控制器，并对其坐标系示教、多线束自动插植编程、插植力控制功能以及相关的可视化界面进行了编程实现。最后，利用所研制的线束插植控制器，进行了手动插植实验，实现了机械臂的插植力控制。同时，通过在线编程，进行了线束自动插植实验，实现了多线束可编程插植。

1 线束插植机械臂控制系统设计

1.1 线束插植的工艺流程

线束插植的工艺流程如下：首先，对接插件所在坐标系进行标定。然后，根据接插件的规格，确定接插孔的坐标。最后，利用气爪抓取线束，进行线束插植。由于线束为柔性材料，且气爪在抓取线束时，由于抓取的位置与线束端子的距离不同，导致插植距离也不相同。为了防止出现第一次插植距离过短，线束未完全插入接插件中的情况。在第一次插植完成之后，打开气爪，松开线束，并令在插植方向的Y轴后退一定距离后，重新抓取线束，进行“二次插植”，以此提高插植的成功率，如图1所示。

图1 线束插植工艺流程图

由以上线束插植工艺流程图可知，本次设计的线束插植机械臂控制系统必须解决三个问题：

1)多线束的可编程插植的实现；

2)插植过程的插植力控制；

3)进行二次插植，提高插植成功率。

1.2 线束插植机械臂控制系统的方案设计

本次设计采用直角坐标机械臂为本体，在被动柔顺结构——SEA串联弹性执行器的基础上，结合闭环PD力控制方法，实现对线束的插植力控制。同时，开发专用的线束插植控制器，通过数控形式，实现多线束的可编程插植以及“二次插植”。整个控制系统的设计要求插植力可调，PD控制器的参数分别取：Kp=8.5，Kd=0.01，单根线束插植总时间控制在4 s左右。

线束插植机械臂控制系统的设计包括：机械结构设计和控制系统结构设计两个方面。在机械结构设计方面：线束插植机械臂的X、Z轴采用上银的KK60高精度型半封闭式滚珠丝杆线性模组，Y轴采用KK40型线性模组，采用L型角铁将其两两固定，构成XYZ三轴直角坐标机械臂。在线束插植方向上的Y轴的滑块下方，设计安装SEA串联弹性执行器。其由法兰盘、导向杆、U型固定支座、固定挡板、弹簧以及滑动支座构成。两根导向杆穿过通孔，插在U型固定支座和滑动支座内，滑动支座的前后各安装一组弹簧，分别用固定挡板进行固定，使得弹簧保持一定初始压缩量，滑动支座内部放置两个弹簧自复位线性位移传感器。整个线束插植机械臂放置于移动平台上，平台的另一侧设置有用于安放不同规格接插件的固定架。

在控制系统结构设计方面：机械臂每个轴所配的伺服驱动器以及IO模块作为EtherCAT从站，通过EtherCAT总线以线性拓扑结构[10]的方式，挂载在EtherCAT主站——线束插植控制器下。线束插植机械臂控制系统结构如图2所示。

图2 线束插植机械臂控制系统结构图

2 线束插植控制器设计

2.1 线束插植控制器总体设计

结合线束插植的工艺要求，其控制器的设计主要包括：坐标系示教、多线束插植可编程以及插植力控制功能设计。其中，所利用的CoDeSys自带的功能块如表1所示。

表1 所调用的功能块

2.1.1 坐标系示教功能设计

针对多线束插植可编程的要求，必须对接插件坐标系进行标定，进而确定接插孔的坐标。本次采用三点示教法进行坐标系示教。首先，移动机械臂至接插件原点(第一排最右端接插孔处)并记录该点，为A(Ox,Oy,Oz)；然后，将机械臂水平向左移动一段距离，记录下第二点坐标，为B(Xx,Xy,Xz)；最后，将机械臂竖直向上移动一段距离，记录下第三点坐标，为C(Yx,Yy,Yz)。

记三轴线束插植机械臂的基座标系为{A}，通过三点示教，计算接插件坐标系{B}。则接插件坐标系{B}相对于{A}，X轴方向的矢量：

(1)

Z轴方向的矢量为：

(2)

Y轴方向的矢量为：AYB=AZB×AXB。

分别进行归一化处理得：

(3)

2.1.2 多线束插植可编程功能设计

本次设计的线束插植控制器要求能够进行在线编写程序，通过对编写的指令进行解析，控制机械臂实现多线束的可编程插植。因此，多线束插植可编程功能设计包括：插植指令开发和指令解析。本次开发的所有插植指令如表2所示。

表2 线束插植控制指令

下面以movel指令的开发和解析为例：

movel直线运动控制指令对应的程序由定时器TON模块、输入位置模块PositionIn以及直线运动模块MC_MoveLinearAbsolute组成，如图3所示。

图3 直线运动控制程序

设计其有效指令形式为：Movel(3,4,5,6)，前三个参数分别对应程序中的X_Lin、Y_Lin、Z_Lin变量，最后一个参数对应速度Velocity变量。在在线编程界面中输入该指令，进行编译，控制器将对该指令进行解析，其解析流程如图4所示。

图4 指令解析程序流程图

首先进行语法检查，语法正确则对指令进行解码判断。然后根据解码结果，读取参数后调用相应的控制程序。其中，语法检查包括：检查指令字母、检查左右括号、检查逗号以及检查句末分号。首先检测输入的字符前5个是否为有效字母(设计规定控制指令有效字母数为5)，如果正确，则继续检测第6个字符是否为“(”。然后，检测字符“(”之后的字符是否为数字或者“，”。如果检测到数字或者“，”后，则继续检测是否出现字符“)”。最后，如果检测到字符“)”，则继续检测是否有字符“；”。若出现字符“；”，则语法正确；否则，语法错误。语法检测流程如图5所示。

图5 语法检测流程图

进行指令解码时，依次读取前5个字符的ASCII值，进行指令判断。如果检测到前5个字符的ASCII值对应的字符依次为m→o→v→e→l，则将n赋值为2。指令解码程序流程图如图6所示。指令与n对应关系如表3所示。

2.1.3 插植力控制功能设计

针对插植力可控的工艺要求，在设计被动柔顺机构——串联弹性执行器的基础上，采用闭环PD力控制方法，实现对插植力的控制。插植力控制程序由PD功能块和MC_ReadActualPosition读取电机实际位置模块构成。利用IO模块的12位精度ADC实时检测KS1-15弹簧自复位直线位移传感器的电压值，将检测出的电压V转化为实际接触力f实际，再与设定的期望接触力f期望作比较后，输出给 PD功能块。经过PD功能块，得出位置增量，再与MC_ReadActualPosition功能块读取的电机实时位置相加，作为

图6 指令解码程序流程图

名称n的值名称n的值movej1plant6movel2uplan7delay3state8opend4coord9close5其他10

输出信号输出给电机，从而控制电机运行的位置，使得弹簧的压缩量保持期望的值，实现对插植力的控制。闭环PD插植力控制原理如图7所示。

图7 插植力控制原理图

2.2 线束插植控制器界面设计

2.2.1 坐标系示教界面

坐标系示教界面包括单轴点动控制按键、系统状态显示、界面跳转功能键、回零按键、示教点及坐标系显示、示教功能键六个部分。利用控制器操作机械臂到三个示教点O、X、Y后记录其点的坐标，调用坐标系示教程序即可生成所示教的坐标系。坐标系示教界面如图8所示。

2.2.2 多线束插植编程界面

多线束插植编程界面包括5个部分：系统信息状态栏、程序编辑文本框、界面跳转功能键、程序文件处理键以及文件信息栏。通过编辑程序文本框可在线编写插植程序，当编译正确时点击运行按钮即可执行线束插植。多线束插植编程界面如图9所示。

图9 多线束插植编程界面

2.2.3 力控制插植界面

力控制插植界面包括：插植力设置、状态显示、单轴点动控制按键、界面跳转功能键、手动插植按钮以及机械臂速度位置信息显示6个部分，如图10所示。

图10 力控制插植界面图

在力控制插植界面，能够对期望插植力进行设置并实时显示实际的插植力，并且在插植结束之后，通过插植状态显示栏中的指示灯，能够反映当前接插件已经插植完成的接插孔状态。同时，设置有手动插植功能键，结合单轴点动控制按键，可以方便地进行手动插植调试。

3 实验结果与分析

3.1 手动插植力控制实验

通过ATI公司的Mini45 Net六维力/力矩传感器，多次测量接触力与弹簧形变量Δl以及Δl与位移传感器电压之间的关系，得出：k3=0.013 6 N/V。

设定PD控制器的参数分别为：Kp=8.5，Kd=0.01，设定期望插植力F=5N。本次插植实验所使用的线束，其前部是带有卡槽的铜端子，宽度2 mm。所使用的线束接插件共有三排接插孔，每一排共有10个接插孔。每个接插孔宽度约2.5 mm，高度约5 mm，孔间距不到0.5 mm。通过线束插植控制器的手动插植功能键，进行插植力控制实验，测试结果如图11所示。

图11 插植力控制实验结果图

由实验结果可知，运行至3.5 s左右，插植力逐渐增大至设定的期望插植力，由此可知该段曲线与线束插入接插孔内部的过程相对应。当实际插植力的大小到达5 N后，在5 N附近振荡，表明此时线束端子已经与接插孔完全卡牢即插植完成，实现了对插植力的控制。当实验运行至9.3 s时，实际插植力为4.519 2 N，此时实际插植力与期望插植力的偏差最大为-0.480 8 N，最大误差不到±10%。考虑自复位线性位移传感器自身的检测精度以及该应用场合的力控制精度要求不高，达到了预期的插植力控制要求。

3.2 自动插植以及监控测试

自动插植实验要求：对接插件第一排四个接插孔(从右往左排序)进行插植，并显示其插植状态。根据实际接插件的规格，记第一排最右端的接插孔对应坐标(0,0,0)，则所需插植的接插孔对应坐标为(9,0,0)。

图12 自动插植监控测试结果图

首先，操作线束插植运动控制器，对接插件的坐标系进行标定，进而确定每一个接插孔对应的坐标。然后，在多线束插植编程界面中，调用插植指令，编写“二次插植”程序。最后，编译无误后，点击运行按键，执行“二次插植”程序。自动插植监控测试结果如图12所示。

部分“二次插植”程序如图12(a)所示，通过线束插植控制器在线编写插植程序，能够实现多线束的可编程插植。由图12(b)监控结果可知，当所需插植的接插孔，插植完成时，其对应的插植状态指示灯被点亮，表明所设计的控制器能够实现对插植状态的实时监控。

从图12(c)中可知，系统完成二次插植时间较长，整个过程耗时21 s左右。其中，除去系统运行之前，监控示波器自身走过的时间3 s，以及设置的二次插植间隔时间7 s，整体二次插植时间大约10 s左右。考虑到本系统处于样机开发阶段，故设置整体运行速度和插植速度均较慢。而且为了观察插植力是否实现有效控制，在每一次插植完成之后，均进行了一定程度的延时。因此实际进行“二次插植”的时间为5～6 s，基本满足预期的设计要求。后期可以通过优化插植动作的时间分配和提升插植样机的运行速度，来降低线束插植时间，进一步提高插植效率。

4 结论

针对线束插植机械臂系统设计需要实现的目标：多线束可编程插植、插植力控制、“二次插植”，设计了一种基于SEA的线束插植机械臂控制系统。介绍了线束插植的工艺流程，提出了线束插植机械臂的设计方案。并且研制了专用的线束插植控制器，对控制器的坐标系示教、多线束自动插植编程、插植力控制功能以及对应的可视化人机界面进行了设计，实现了插植力的控制。并且，利用设计的控制器在线编写“二次插植”程序，实现了多线束的可编程插植。本次设计的线束插植机械臂控制系统能够应用于自动化线束装配领域，但还需进一步优化插植动作的时间分配，提高插植的效率。