CN111775145A - 一种串并联机器人的控制系统 - Google Patents

Abstract

本发明公开了一种串并联机器人控制系统，由上位机、嵌入式PC控制器、伺服驱动器、伺服电机、电主轴模块、误差补偿模块部件组成。上位机处理控制文件，提取点位信息，按照控制文件指定方式插值得到运动轨迹上的中间点位，并通过运动学逆解程序生成位置表；将位置表传输到嵌入式PC控制器。嵌入式PC控制器运行TwinCAT平台开发程序，将位置表转化成运动控制信号，控制伺服驱动器，继而控制伺服电机，完成对串并联机器人控制。本发明系统可控制串并联机器人进行点动或使末端沿复杂轨迹运动，并具有电主轴安全联动、运动误差补偿的功能；充分发挥了PC计算能力强、应用程序灵活的特点，可方便地进行拓展开发，完善串并联机器人的应用。

Description

一种串并联机器人的控制系统

技术领域

本发明涉及一种控制系统，尤其是涉及一种串并联机器人的控制系统

背景技术

为了提高对生产环境的适应性，满足灵活多变的市场需求，近年来全球制造业都在探索研发新型制造系统与装备。串并联机器人是其中的代表，与传统机器人相比，具有刚度重量比大、响应速度快、末端姿态能力良好、环境适应性强等优点。然而串并联机器人末端的笛卡尔空间运动是其关节空间运动的复杂非线性映射，控制较为困难，限制了串并联机器人的应用。

目前机器人的控制系统可以大致分为四种类型，即封闭式结构、PC嵌入NC结构、NC嵌入PC结构、软件型开放式结构。其中封闭式结构占据目前制造业的主流，它需要使用专用工具进行开发且受制于系统供应商；PC嵌入NC结构是在控制器内部嵌入PC，用于处理一些非实时任务，它具有一定的开放性，但控制系统的核心仍然是封闭的。NC嵌入PC结构是指使用PC加上运动控制卡进行控制，开放性较强；软件型开放式结构以PC为基础，在计算机操作系统下运行软件化的NC，实现了控制方案的软件化，是机器人控制系统发展的趋势。由倍福自动化有限公司所研发的TwinCAT平台，可以方便地安装在PC上，支持EtherCAT协议的TwinCAT平台可将一台PC转变成一个具有多轴控制功能的实时控制器。

专利号为201810166162.0的一种含有动平台附加约束的五自由度精密串并联机器人，公开提到了一种串并联机器人。它的特点是：从机器人的固定基座到末端执行器的运动链由两部分串联形成，即三个移动副支链组成的三自由度并联部分与两个旋转副组成的二自由度串联部分。与传统的机器人相比，串并联机器人运动链形式新颖，结构复杂，因而缺少相应的控制系统。

发明内容

针对串并联机器人控制系统的空白，本发明提出了一种串并联机器人的控制系统。这一控制系统采用上下位机、软件型开放式结构，克服了传统控制系统计算能力不足、开发不便的特点。并在如4所述的串并联机器人上试运行，实现了机器人的末端轨迹控制，控制效果良好。

为了实现上述目的，本发明采用下述技术方案：

一种串并联机器人的控制系统，包括上位机、嵌入式PC控制器、I/O模块、伺服驱动器、伺服电机、电主轴模块、误差补偿模块、附属部件；其中上位机处理控制文件，提取点位信息，再按照控制文件指定的方式插值得到运动轨迹上的中间点位，并通过运动学逆解模块生成位置表；上位机将位置表通过EtherCAT总线传输到嵌入式PC控制器；嵌入式PC控制器上运行基于TwinCAT开发的程序模块，该程序模块将位置表转成控制信号，通过IO模块控制伺服驱动器，继而控制伺服电机和电主轴模块，使得串并联机器人做出指定的运动；误差补偿模块将输出信号处理成计算机可识别的信号，通过I/O模块输入嵌入式PC控制器，再经嵌入式PC控制器上传至上位机；通过上位机的补偿量计算程序处理得到补偿量后，传输回嵌入式PC控制器中，进行误差补偿。

优选地，上位机运行windows操作系统的微型计算机，在上位机上运行的程序有通信模块、解释模块、插值模块、逆解模块、补偿量计算模块。

优选地，嵌入式PC控制器采用倍福工控PC，运行基于TwinCAT平台开发的程序，嵌入式PC控制器带有触控屏，用于与外界进行信息交互。

优选地，I/O模块由若干端子模块与串口模块组成；嵌入式PC控制器通过I/O模块对伺服驱动器进行反馈控制，并从I/O模块读取电主轴模块的运行信号和误差补偿模块传回的测量值。

优选地，电主轴模块由电主轴、电主轴驱动器、冷却系统、换刀系统、电主轴模块附属部件组成；当电主轴运行时，电主轴驱动器才会向嵌入式PC控制器发送允许伺服轴运动的信号。

优选地，误差补偿模块包含误差传感器和信号调理电路；误差传感器用于测量机器人末端的实际位姿，信号调理电路将模拟信号转化为方便处理的数字信号。

优选地，上位机与嵌入式PC控制器协同完成串并联机器人的控制任务，上位机完成串并联机器人控制所需的复杂数据处理工作，将所需要的机器人末端运动换算成各伺服轴的位置表；嵌入式PC控制器实现运动控制，根据上位机发来的伺服轴的位置表，控制伺服驱动器运动；上位机的数据计算与嵌入式PC控制器的运动控制是异步进行的；上位机将计算得到的伺服轴位置表发送到嵌入式PC控制器后，由嵌入式PC控制器将位置表处理成伺服驱动器的控制指令，存储于堆栈中，并依照“先进先出”的队列模式，依次向各个伺服驱动器发送这些指令，实现串并联机器人的多轴联动，使得其末端沿指定轨迹运行。

优选地，通信模块用于和其它计算机或制造系统交换信息，取得串并联机器人的控制文件，并将其存储或发送给解释模块；控制文件中包含了机器人的末端的轨迹类型、确定轨迹的有限个目标点的位姿、轨迹容许弓高误差信息；控制文件的格式兼容通用数控程序的格式，对串并联机器人进行控制。

优选地，解释模块从控制文件中提取出轨迹类型、目标点位姿、轨迹容许弓高误差信息，并提交给插值模块。

优选地，插值模块根据控制文件中的信息，生成中间点位姿表；中间点位姿表记录了串并联机器人末端即将经过的每个中间点的位姿与姿态；中间点的个数由插值模块根据控制文件中的信息自动计算，使得轨迹的弓高误差不超过设定的范围。

优选地，逆解模块对于插值模块传来的中间点位姿表中的每一个中间点，调用一次逆解程序；逆解程序的输入量为中间点的位置坐标与姿态，输出量为该中间点的各关节值，即为了让串并联机器人的末端达到该中间点，机器人的各个关节，包含移动或转到关节，应具有的值；逆解模块将关节值写入位置表，发送给嵌入式PC控制器。

优选地，补偿量计算模块从嵌入式PC控制器获取来自误差补偿模块的位置数据，并调用逆解程序计算当前串并联机器人的各关节实际位置，与当前点位的各关节理论位置对比后计算出补偿量，再将补偿量写入嵌入式PC控制器的程序中的补偿表内，使得机器人末端精度得以改善。

优选地，上位机采用普通PC，上位机主要处理复杂的数据操作，包括与其他系统的通信、控制文件的解读、点位的插值、点位运动学逆解、补偿量的计算，这些操作均由软件完成。硬件方面上位机与嵌入式PC控制器通过EtherCAT总线相连。

优选地，控制文件按照通用的数控程序格式给出。上位机从通信模块中获得控制文件后，利用文件解释模块取得所要求的机器人轨迹类型、目标点数据、电主轴状态等信息。

优选地，点位插值由上位机程序中的插值模块完成。获得目标点的数据和轨迹类型后，插值模块将目标点的数据解离成目标点的姿态和目标点的空间坐标，分别进行姿态插值和空间坐标插值。

优选地，姿态插值是指给定几个目标点的姿态，也就是机器人末端的姿态，求出由这些目标点确定的轨迹上的中间点的姿态。具体的做法是将描述这些点姿态的欧拉角转化成四元数，再利用四元数球面线性插值公式对点的四元数进行插值，得到中间点的四元数，然后把中间点的四元数换算回欧拉角。通过这样的姿态插值可以确保机器人末端姿态的平滑过渡。

优选地，空间坐标插值是指给定几个目标点的空间坐标，也就是机器人末端的空间位置，求出由这些点确定的轨迹上的中间点的空间位置。空间坐标插值的形式由控制文件中的轨迹类型确定，支持直线插值、圆弧插值、B样条插值等形式。插值得到的中间点必定在控制文件所要求的轨迹上，中间点的数目根据控制文件中给定的许用弓高误差确定，由插补模块自动计算。

优选地，姿态插值与空间坐标插值生成的中间点的数目是一致的，将以上步骤得到的每个中间点的空间坐标和姿态对应组合，得到中间点位姿表。

优选地，直线插值为空间直线插值；圆弧插值为空间三点圆弧插值；B样条插值包含4类，均为空间B样条插值，它们分别是：给定样条控制点的准均匀B样条插值、给定样条样条型值点的准均匀B样条插值、给定样条上的点的准均匀B样条插值、给定样条上的点的均匀B样条插值。插值的方式可以灵活地修改或扩充。

优选地，直线插值、圆弧插值、B样条插值均为等距离插值，即对于插值得到的一系列中间点，相邻的两个中间点间的距离相等。称这一距离为步长。对于圆弧和样条曲线，步长由控制文件中给出的许用弓高误差和目标点的坐标计算，其中圆弧的步长由圆的半径与弓高关系公式确定；样条曲线的步长计算则是先根据目标点计算其最小曲率半径，然后在其曲率半径最小处通过把样条曲线近似成圆弧，利用圆的半径与弓高关系公式确定步长。

优选地，中间点位姿表，是机器人末端轨迹与姿态的列表，不能直接用于机器人的控制，需要换算出对应的机器人各关节变量的值。这一工作由逆解模块完成。逆解模块根据串并联机器人的运动学模型和已知的位姿数据建立方程组，并通过数值迭代的方式从方程组中求解关节变量。每一个中间点对应一组关节变量，将所有关节变量组合成位置表，发送到嵌入式PC控制器。

优选地，嵌入式PC控制器是倍福工控PC，在嵌入式PC控制器上运行基于TwinCAT开发的程序模块。嵌入式PC控制器对伺服驱动器进行反馈控制。同时运行一个人机交互界面，实时显示机器人的运行状态与报警状态等信息，完成机器人上下使能、运动程序选择、步进、暂停等简单操作。嵌入式PC控制器还接收来自电主轴模块的信号，用于机器人的安全联动控制。

优选地，采用安全联动控制，是指只有当电主轴运行时，各伺服轴才能运动。

优选地，嵌入式PC控制器上的程序模块，包含三个子模块：TwinCAT C++、TwinCATPLC、TwinCAT NC。其中TwinCAT C++将上位机发来的位置表读入内存，TwinCAT PLC将内存中的位置数据换算成伺服电机的运动值，逐行写入TwinCAT NC中的FIFO堆栈，以“先入先出”的方式通过TwinCATNC模块产生相应的伺服驱动器控制信号。在TwinCAT PLC中调节FIFO堆栈写入速度倍率，可以实现机器人运行速度的控制。

优选地，误差补偿模块，由误差补偿传感器与信号调理子模块组成。误差补偿传感器可以是倾角传感器、视觉传感器或其它传感器，用于测量机器人的末端实际位姿。信号调理子模块将传感器的输出处理成便于计算机识别的信号，通过I/O模块输入嵌入式PC控制器，再经嵌入式PC控制器上传至上位机。通过上位机的补偿量计算程序处理得到补偿量后，传输回嵌入式PC控制器中，添加到TwinCAT PLC模块中的补偿量表中，提交给TwinCAT NC模块进行误差补偿。

与现有技术相比，本发明具有如下的突出的实质性特点和显著的技术进步：

1.本发明串并联机器人有多个伺服轴，且机器人的末端位置与这些伺服轴的位置间关系复杂，运动互相耦合；其控制过程往往涉及到复杂的数学问题，如非线性方程组求解，传统控制系统不擅长处理这类控制任务；本发明在高性能的上位机上完成主要的数据处理工作，上位机是运行windows系统的普通PC，软件开发限制小，可以方便地调用第三方库来开发处理复杂数学问题的软件模块；嵌入式PC控制器只需要接收由上位机处理完毕的机器人各关节的位置值，并完成运动控制；本发明控制系统对嵌入式PC控制器的要求不高，可以降低硬件成本；

2.本发明在嵌入式PC控制器中利用FIFO堆栈与“先进先出”的队列模式来存储未执行的运动数据进行多轴联动，数据计算与运动控制可以异步进行，使得控制系统兼具强大的数据处理能力与强大的实时运动控制能力；通过触控屏可以观察运动指令的执行状态，也可以直接对控制器发送暂停、步进、点动等指令；

3.控制系统兼容G代码等通用的数控代码，具有较强的通用性；上位机软件开发灵活，扩展性好，串并联机器人系统可借助上位机方便地接入集成制造系统；该系统具有良好的末端轨迹控制能力，可以控制串并联机器人沿复杂的曲线运动，这些运动可以通过参数化的指令快速调用；可通过误差补偿模块有效减少串并联机器人的定位误差；具有安全联动功能，防止生产事故。

说明书附图

图1为本发明串并联机器人的控制系统的结构示意图。

图2为图1所示结构的框图。

图3为串并联机器人的控制系统的末端运动曲线拟合示意图。

具体实施方式：

串并联机器人的控制系统实际布置如图1所示：

标号1的设备是上位机。

标号2的设备是配电柜，在2内部装有嵌入式PC控制器、I/O模块、机器人各轴伺服驱动器、电源，在2的上部装有一块触控屏(标号3)，与2内部的嵌入式PC控制器相连。

标号4的设备是如专利号：201810166162.0所述的串并联机器人作为本系统的控制对象；

标号5、6、7的设备是伺服电机，用于驱动机器人的并联部分；标号9、11的设备是伺服电机，用于驱动机器人的串联部分。

标号8的设备是配电柜，其内部装有电主轴驱动器、电源，以及必要的附属部件，用于控制电主轴的冷却机、松紧刀机构。设备8与电主轴(标号10)共同组成了电主轴模块。

控制系统的结构框图如图2所示：

标号1是上位机，在其上运行的程序有：9-通信模块、11-解释模块、12-插值模块、13-逆解模块、14-补偿量计算模块、10-存储模块。

标号2是嵌入式PC控制器，其上运行的程序有15-TwinCAT C++模块、16-TwinCATPLC模块、17-TwinCATNC模块。

标号3是I/O模块；标号4是电主轴模块；标号5是触控屏；标号6是误差补偿模块；标号7是各轴伺服驱动器；标号8是各轴伺服电机。标号18至27为各模块间传递的数据文件。

下面将结合示意图对本发明的具体实施方式进行更详细的描述。

实施例一

在本实施例中，参见图2，一种串并联机器人的控制系统，包括上位机1、嵌入式PC控制器2、I/O模块3、伺服驱动器7、伺服电机8、电主轴模块4、误差补偿模块6、附属部件；其中上位机1处理控制文件，提取点位信息，再按照控制文件指定的方式插值得到运动轨迹上的中间点位，并通过运动学逆解模块生成位置表；上位机1将位置表通过EtherCAT总线传输到嵌入式PC控制器2；嵌入式PC控制器2上运行基于TwinCAT开发的程序模块，该程序模块将位置表转成控制信号，通过IO模块3控制伺服驱动器7，继而控制伺服电机8和电主轴模块4，使得串并联机器人做出指定的运动；误差补偿模块6将输出信号处理成计算机可识别的信号，通过I/O模块3输入嵌入式PC控制器2，再经嵌入式PC控制器2上传至上位机1；通过上位机1的补偿量计算程序处理得到补偿量后，传输回嵌入式PC控制器2中，进行误差补偿。

实施例二

本实施例与实施例一基本相同，特别之处在于：

在本实施例中，参见图2，串并联机器人的控制系统的上位机1是运行windows操作系统的微型计算机，在上位机1上运行的程序有通信模块9、解释模块11、插值模块12、逆解模块13、补偿量计算模块14。

嵌入式PC控制器2采用倍福工控PC，运行基于TwinCAT平台开发的程序，嵌入式PC控制器2带有触控屏5，用于与外界进行信息交互。

I/O模块3由若干端子模块与串口模块组成；嵌入式PC控制器2通过I/O模块3对伺服驱动器7进行反馈控制，并从I/O模块3读取电主轴模块4的运行信号和误差补偿模块6传回的测量值。

电主轴模块4由电主轴、电主轴驱动器、冷却系统、换刀系统、电主轴模块附属部件组成；当电主轴运行时，电主轴驱动器才会向嵌入式PC控制器2发送允许伺服轴运动的信号。

误差补偿模块6包含误差传感器和信号调理电路；误差传感器用于测量机器人末端的实际位姿，信号调理电路将模拟信号转化为方便处理的数字信号。

上位机1与嵌入式PC控制器2协同完成串并联机器人的控制任务，上位机1完成串并联机器人控制所需的复杂数据处理工作，将所需要的机器人末端运动换算成各伺服轴的位置表；嵌入式PC控制器2实现运动控制，根据上位机1发来的伺服轴的位置表，控制伺服驱动器运动；上位机1的数据计算与嵌入式PC控制器2的运动控制是异步进行的；上位机1将计算得到的伺服轴位置表发送到嵌入式PC控制器2后，由嵌入式PC控制器2将位置表处理成伺服驱动器7的控制指令，存储于堆栈中，并依照“先进先出”的队列模式，依次向各个伺服驱动器7发送这些指令，实现串并联机器人的多轴联动，使得其末端沿指定轨迹运行。

通信模块9用于和其它计算机或制造系统交换信息，取得串并联机器人的控制文件，并将其存储或发送给解释模块；控制文件中包含了机器人的末端的轨迹类型、确定轨迹的有限个目标点的位姿、轨迹容许弓高误差信息；控制文件的格式兼容通用数控程序的格式，对串并联机器人进行控制。

解释模块11从控制文件中提取出轨迹类型、目标点位姿、轨迹容许弓高误差信息，并提交给插值模块12。

插值模块12根据控制文件中的信息，生成中间点位姿表；中间点位姿表记录了串并联机器人末端即将经过的每个中间点的位姿与姿态；中间点的个数由插值模块12根据控制文件中的信息自动计算，使得轨迹的弓高误差不超过设定的范围。

逆解模块13对于插值模块12传来的中间点位姿表中的每一个中间点，调用一次逆解程序；逆解程序的输入量为中间点的位置坐标与姿态，输出量为该中间点的各关节值，即为了让串并联机器人的末端达到该中间点，机器人的各个关节，包含移动或转到关节，应具有的值；逆解模块13将关节值写入位置表，发送给嵌入式PC控制器2。

补偿量计算模块14从嵌入式PC控制器2获取来自误差补偿模块6的位置数据，并调用逆解程序计算当前串并联机器人的各关节实际位置，与当前点位的各关节理论位置对比后计算出补偿量，再将补偿量写入嵌入式PC控制器2的程序中的补偿表内，使得机器人末端精度得以改善。

实施例三

本实施例与前述实施例基本相同，特别之处在于：

在本实施例中，控制机器人的末端沿指定的圆弧运动。

1.编写控制文件，控制文件中包含以下信息：轨迹类型为圆、圆上三个点的笛卡尔坐标与对应的姿态、许用弓高误差。

2.见图2，上位机的通信模块9接受到该控制文件18，将其发送给存储模块10和解释模块11，存储模块11将文件储存。

3.解释模块从文件中读取出轨迹类型、目标点的位姿、轨迹容许弓高误差，并将这些数据19发送给插值模块12。

4.插值模块首先将三个点的笛卡尔坐标和采用欧拉角表示的姿态分离，根据三个点的笛卡尔坐标计算圆心坐标和半径。再根据半径和许用弓高误差计算出进给步长。由半径和进给步长可以确定中间点个数。中间点是指定圆弧上的点，中间点越多，机器人轨迹的弓高误差越小，根据中间点的个数将圆弧的圆心角等分后，根据半径和圆心坐标依次计算各个中间点的笛卡尔坐标。姿态方面，计算相邻的每2个给定点之间中间点的个数，并将两点的欧拉角转化成四元数后使用四元数球面线性插值算法分两段得到中间点的四元数，再将其转化为欧拉角，得到中间点的姿态，采用欧拉角表示。最后将中间点的笛卡尔坐标和姿态一一结合，得到中间点位姿表20，发送给逆解模块13。

5.逆解模块13接受到中间点的位姿表之后，对表中的每一个中间点调用逆解程序。逆解程序通过迭代的方式求解该中间点的位姿与串并联机器人各关节的值组成的非线性方程组，将解得的关节值写入位置表21，发送给嵌入式PC控制器2。逆解模块可以根据不同的机器人的结构做出修改。

6.嵌入式PC控制器2运行着基于基于TwinCAT开发的程序模块，其中的程序模块15TwinCAT C++把位置表21中的关节值按格式处理逐行读入内存；程序模块16TwinCAT PLC读取这些这些关节值，处理后送入FIFO堆栈，由17TwinCATNC按照“先进先出”原则，控制各轴伺服电机运动到指定位置，机器人的末端即按照指定的圆弧运动。同时，TwinCAT PLC接受来自电主轴模块的运转信号，如果这一信号中断或异常，则各轴伺服电机的位置不能改变，借此可以实现电主轴的安全联动功能，防止发生意外。

7.当需要提高定位精度时，开启误差补偿模块6。误差补偿模块测量当前的末端实际位姿27，将测量值与理论值写入文件22，通过嵌入式PC控制器传回上位机，在上位机中经误差补偿程序处理后得到每个关节伺服电机的补偿值。将补偿值23传回嵌入式PC控制器，写入TwinCAT NC内的运动补偿表中，使得末端精度得以提高。

实施例四

本实施例与前述实施例基本相同，特别之处在于：

在本实施例中，控制机器人的末端沿光滑曲线运动，本系统通过B样条曲线拟合的方式来实现末端沿光滑曲线运动，具体的做法是：

1、编写控制文件，控制文件中包含以下信息：光滑曲线上若干点的笛卡尔坐标与姿态值、许用步长误差波动、轨迹容许弓高误差。

2、见图2，上位机的通信模块9接受到该控制文件18，将其发送给存储模块10和解释模块11，存储模块11将文件储存。

3、解释模块从文件中读取出给定点的位姿、轨迹容许弓高误差、步长波动容许值，并将这些数据19发送给插值模块12。

4、插值模块首先将所有给定点的笛卡尔坐标和采用欧拉角表示的姿态分离，把给定点的笛卡尔坐标视作B样条曲线的型值点，并通过这些点反求出B样条曲线的控制点。借此可以得到一条通过所有给定点的B样条曲线。B样条曲线是参数曲线，容易计算该曲线的最小曲率半径，并利用最小曲率半径和给定的轨迹容许弓高误差来算出进给步长。

5、从第一个给定点开始，以该点为圆心，步长为半径作圆，联立B样条曲线的参数方程和圆的方程，利用数值迭代的方式可以求出下一点对应的参数值，迭代的收敛由许用步长误差波动确定，再根据B样条曲线的参数方程求出第二个点的笛卡尔坐标。再对第二个点重复以上过程，求出第三个点的坐标。以此类推，可以求出所有中间点的笛卡尔坐标。

6、姿态方面，计算相邻的每两个给定点之间中间点的个数，并将相邻两个给定点的欧拉角转化成四元数后，使用四元数球面线性插值算法得到中间点的四元数，再将其转化为欧拉角，得到中间点的采用欧拉角表示的姿态。最后将中间点的笛卡尔坐标和姿态一一结合，得到中间点位姿表20，发送给逆解模块13。

7、后续步骤与实施方式1一致，即对中间点位姿表20逐个求运动学逆解，得到位置表21，送入嵌入式PC控制器2执行，即可使机器人末端沿计算出的B样条曲线运动。若给定的光滑曲线上的点足够多，机器人的末端轨迹就能充分贴近光滑曲线。

8、为更好说明，可见图3所示，对正弦曲线的拟合过程：

标号B的较粗曲线是正弦曲线，即期望的机器人末端的运行轨迹。在其上取若干个点作为B样条曲线的型值点，即拟合得到的B样条曲线需要经过的点。这里取了6个点，标号分别为1至6，用小三角形标出。根据这给定的6个点的笛卡尔坐标可以计算出B样条曲线的6个控制点，标号为a至f。根据准均匀B样条曲线的性质，点1与点a重叠；点6与点f重叠，根据控制点的笛卡尔坐标可以计算出B样条曲线的参数方程，并按照本实施方式第4、5步所述方法求出所有中间点Pn的笛卡尔坐标，所有中间点用“+”号标出，再按照本实施方式6所述的方式计算出每个中间点的姿态，生成位姿表后发送给嵌入式PC控制器执行。

图3所示的过程是极度简化的情况，原曲线上只选取了6个给定点，因此机器人末端运行轨迹(见曲线A)和原曲线(曲线B)有一定误差。实际运行过程中，原曲线上的给定点可以取远多于6个，从而使拟合得到的曲线良好贴合原曲线。

综上所述，本发明公开了一种串并联机器人控制系统，由上位机、嵌入式PC控制器、伺服驱动器、伺服电机、电主轴模块、误差补偿模块部件组成。上位机处理控制文件，提取点位信息，按照控制文件指定方式插值得到运动轨迹上的中间点位，并通过运动学逆解程序生成位置表；将位置表传输到嵌入式PC控制器。嵌入式PC控制器运行TwinCAT平台开发程序，将位置表转化成运动控制信号，控制伺服驱动器，继而控制伺服电机，完成对串并联机器人控制。本发明系统可控制串并联机器人进行点动或使末端沿复杂轨迹运动，并具有电主轴安全联动、运动误差补偿的功能；充分发挥了PC计算能力强、应用程序灵活的特点，可方便地进行拓展开发，完善串并联机器人的应用。

上面对本发明实施例结合附图进行了说明，但本发明不限于上述实施例，还可以根据本发明的发明创造的目的做出多种变化，凡依据本发明技术方案的精神实质和原理下做的改变、修饰、替代、组合或简化，均应为等效的置换方式，只要符合本发明的发明目的，只要不背离本发明串并联机器人的控制系统的技术原理和发明构思，都属于本发明的保护范围。

Claims (12)

1.一种串并联机器人的控制系统，其特征在于：包括上位机(1)、嵌入式PC控制器(2)、I/O模块(3)、伺服驱动器(7)、伺服电机(8)、电主轴模块(4)、误差补偿模块(6)、附属部件；其中上位机(1)处理控制文件，提取点位信息，再按照控制文件指定的方式插值得到运动轨迹上的中间点位，并通过运动学逆解模块生成位置表；上位机(1)将位置表通过EtherCAT总线传输到嵌入式PC控制器(2)；嵌入式PC控制器(2)上运行基于TwinCAT开发的程序模块，该程序模块将位置表转成控制信号，通过IO模块(3)控制伺服驱动器(7)，继而控制伺服电机(8)和电主轴模块(4)，使得串并联机器人做出指定的运动；误差补偿模块(6)将输出信号处理成计算机可识别的信号，通过I/O模块(3)输入嵌入式PC控制器(2)，再经嵌入式PC控制器(2)上传至上位机(1)；通过上位机(1)的补偿量计算程序处理得到补偿量后，传输回嵌入式PC控制器(2)中，进行误差补偿。

2.如权利要求1所述串并联机器人的控制系统，其特征在于：上位机(1)是运行windows操作系统的微型计算机，在上位机(1)上运行的程序有通信模块(9)、解释模块(11)、插值模块(12)、逆解模块(13)、补偿量计算模块(14)。

3.如权利要求1所述串并联机器人的控制系统，其特征在于：嵌入式PC控制器(2)采用倍福工控PC，运行基于TwinCAT平台开发的程序，嵌入式PC控制器(2)带有触控屏(5)，用于与外界进行信息交互。

4.如权利要求1所述串并联机器人的控制系统，其特征在于：I/O模块(3)由若干端子模块与串口模块组成；嵌入式PC控制器(2)通过I/O模块(3)对伺服驱动器(7)进行反馈控制，并从I/O模块(3)读取电主轴模块(4)的运行信号和误差补偿模块(6)传回的测量值。

5.如权利要求1所述串并联机器人的控制系统，其特征在于：电主轴模块(4)由电主轴、电主轴驱动器、冷却系统、换刀系统、电主轴模块附属部件组成；当电主轴运行时，电主轴驱动器才会向嵌入式PC控制器(2)发送允许伺服轴运动的信号。

6.如权利要求1所述串并联机器人的控制系统，其特征在于：误差补偿模块(6)包含误差传感器和信号调理电路；误差传感器用于测量机器人末端的实际位姿，信号调理电路将模拟信号转化为方便处理的数字信号。

7.如权利要求1所述串并联机器人的控制系统，其特征在于：上位机(1)与嵌入式PC控制器(2)协同完成串并联机器人的控制任务，上位机(1)完成串并联机器人控制所需的复杂数据处理工作，将所需要的机器人末端运动换算成各伺服轴的位置表；嵌入式PC控制器(2)实现运动控制，根据上位机(1)发来的伺服轴的位置表，控制伺服驱动器运动；上位机(1)的数据计算与嵌入式PC控制器(2)的运动控制是异步进行的；上位机(1)将计算得到的伺服轴位置表发送到嵌入式PC控制器(2)后，由嵌入式PC控制器(2)将位置表处理成伺服驱动器(7)的控制指令，存储于堆栈中，并依照“先进先出”的队列模式，依次向各个伺服驱动器(7)发送这些指令，实现串并联机器人的多轴联动，使得其末端沿指定轨迹运行。

8.如权利要求2所述串并联机器人的控制系统，其特征在于：通信模块(9)用于和其它计算机或制造系统交换信息，取得串并联机器人的控制文件，并将其存储或发送给解释模块；控制文件中包含了机器人的末端的轨迹类型、确定轨迹的有限个目标点的位姿、轨迹容许弓高误差信息；控制文件的格式兼容通用数控程序的格式，对串并联机器人进行控制。

9.如权利要求2所述串并联机器人的控制系统，其特征在于：解释模块(11)从控制文件中提取出轨迹类型、目标点位姿、轨迹容许弓高误差信息，并提交给插值模块(12)。

10.如权利要求2所述串并联机器人的控制系统，其特征在于：插值模块(12)根据控制文件中的信息，生成中间点位姿表；中间点位姿表记录了串并联机器人末端即将经过的每个中间点的位姿与姿态；中间点的个数由插值模块(12)根据控制文件中的信息自动计算，使得轨迹的弓高误差不超过设定的范围。

11.如权利要求2所述串并联机器人的控制系统，其特征在于：逆解模块(13)对于插值模块(12)传来的中间点位姿表中的每一个中间点，调用一次逆解程序；逆解程序的输入量为中间点的位置坐标与姿态，输出量为该中间点的各关节值，即为了让串并联机器人的末端达到该中间点，机器人的各个关节，包含移动或转到关节，应具有的值；逆解模块(13)将关节值写入位置表，发送给嵌入式PC控制器(2)。

12.如权利要求2所述串并联机器人的控制系统，其特征在于：补偿量计算模块(14)从嵌入式PC控制器(2)获取来自误差补偿模块(6)的位置数据，并调用逆解程序计算当前串并联机器人的各关节实际位置，与当前点位的各关节理论位置对比后计算出补偿量，再将补偿量写入嵌入式PC控制器(2)的程序中的补偿表内，使得机器人末端精度得以改善。

Images